視覺信息位姿估計在火箭起飛漂移量中的應用

高 燕，夏 霏

(太原衛(wèi)星發(fā)射中心，山西 太原 030027)

0 引言

在火箭點火起飛至離開發(fā)射塔架前，火箭垂直向上飛行，火箭在該階段由于控制的延遲和超調(diào)等原因出現(xiàn)擺動，表現(xiàn)為橫向漂移，漂移量就是指火箭垂直起飛過程中相對基準彈道的偏移，主要用于評定火箭的飛行控制性能。目前火箭起飛漂移量主要依靠高速電視攝像機交匯測量實現(xiàn)，該方法對圖像判讀要求較高，并且測量精度偏低。

位姿估計是視覺測量方法中的基礎(chǔ)問題，常用在空間目標交匯對接、飛行器導航與目標定位等方面。李少飛[1]提出基于原始點云對物體進行6D位姿估計的深度學習方法，僅依靠三維點云信息對散亂場景中的物體進行位姿估計。蔣雪源等[2]提出采用動態(tài)遍歷與預聚類相結(jié)合的視覺定位方法，能夠有效提高位姿估計的精度。許允喜等[3]提出了擴展正交迭代算法，同時利用點和直線段的特征，定義誤差函數(shù)，迭代求解其最小值，使得位姿估計誤差更低、抗噪性更強、算法更穩(wěn)定。洪洋等[4]利用矩形的幾何特性及空間點、線的共面特征，通過獲取一對正交消隱點，確立無窮遠點在像平面的投影關(guān)系，完成姿態(tài)估計。郭軍等[5]引入三維重構(gòu)信息，克服了正交迭代算法的位姿模糊，提高了位姿估計的收斂速度和魯棒性。李鑫等[6]利用直接最小二乘法實現(xiàn)了高精度的相機位姿估計。凌寒羽等[7]在高精度快速位姿估計算法中，加入改進的自適應加權(quán)物方殘差目標迭代算法，提高計算精度和穩(wěn)健性。席志紅等[8]提出將不確定性描述矩陣加入到位姿求解等式中，使得求解精度更高，穩(wěn)定性更強。

多目位姿估計是當前采用較多的方法，其基本原理是利用多臺相機對目標進行拍攝，最終獲取目標坐標系和相機坐標系之間的位姿轉(zhuǎn)換關(guān)系，其求解過程稱為透視n點投影問題(Perspective-n-Point，PnP)。

結(jié)合火箭起飛漂移量應用實際，提出基于視覺信息的起飛漂移量測量計算方法，利用單幅圖像對測量設(shè)備進行準確標定，并使用正交迭代算法求解箭體的位置和姿態(tài)，試驗結(jié)果表明該方法有較高的測量精度。

1 視覺信息位姿估計原理

通常使用中心透視投影模型表示相機的成像過程,如圖1所示。該模型為一線性模型，假設(shè)物體表面的反射光或發(fā)射光都經(jīng)過一個“針孔”點而投影在像平面上，此投影中心或針孔稱為光心，滿足物點、像點和光心三點共線，并據(jù)此建立中心投影的基本方程——共線方程[9]。

圖1 中心透視成像模型Fig.1 Central perspective imaging model

圖1中，Ow-XwYwZw為世界坐標系;Oc-XcYcZc為攝像機坐標系;Oc為光心。平面O0-uv和平面O1-xy均為成像平面，其中O0-uv以像素為單位建立坐標系，平面O1-xy以物理單位建立坐標系，原點O1為相機光軸與成像平面的交點，其在平面O0-uv的坐標為[u0,v0]，像素在2個方向的物理尺寸為[dx,dy]。

設(shè)像點P的像素坐標為[u,v]，圖像坐標為[x,y]，物點P在世界坐標系中的坐標為[Xw,Yw,Zw]，在攝像機坐標系中的坐標為[Xc,Yc,Zc]，則根據(jù)共線關(guān)系，存在：

(1)

式中，fx,fy為橫向和縱向等效焦距，表示實際光學焦距與像元的橫縱長度之比。

設(shè)世界坐標系到攝像機坐標系的坐標轉(zhuǎn)換矩陣和平移向量為{R ,t}，則：

(2)

結(jié)合上兩式，得到：

(3)

式中，K為內(nèi)參矩陣，反映相機內(nèi)部的固有特性，與等效焦距和主點相關(guān)。{R,t}為外參矩陣，與相機在世界坐標系中的位置和姿態(tài)相關(guān)，反映了相機的外部位姿。

在實際的成像系統(tǒng)中，物像之間理想的線性投影關(guān)系并不能實現(xiàn)，由于各種因素的影響，成像投影點與投影幾何中心存在偏差，稱之為光學畸變誤差，其存在會影響最終結(jié)果的正確性，因此需要建立像差模型對該誤差進行修正。相機成像偏差并非只受單一因素影響，是由多種類型畸變綜合作用的結(jié)果，一般包括徑向畸變、切向畸變和薄棱鏡畸變[10]。在忽略各種畸變的高階項后，理論像點和實際像點可表示為：

，

(4)

式中，k1，p1，p2，s1，s2為相差系數(shù)，已知相差系數(shù)可以對圖像進行有效修正，從而保證測量結(jié)果的正確性。由于相差系數(shù)只與相機本身的成像性質(zhì)有關(guān)，所以相差系數(shù)可以認為是相機內(nèi)部參數(shù)的一部分。

綜上所述，相機的成像過程如圖2所示，某點在世界坐標系中的坐標經(jīng)過剛體坐標變換，得到該點在相機中的坐標，經(jīng)過透視投影，得到理想圖像坐標，相差修正后得到真實圖像坐標，最后經(jīng)過處理得到該點的像素坐標。簡單的說，已知物點P對應像點的像素坐標[u，v]，及相機的內(nèi)參系數(shù)fx，fy，u0，v0，外參矩陣R，t，相差系數(shù)k1，p1，p2，s1，s2，可以求得該點在世界坐標系中的坐標。

圖2 相機成像過程示意圖Fig.2 Schematic diagram of camera imaging process

2 相機標定

由上一節(jié)可知，利用視覺信息進行物體的位姿估計，需要先知道相機的內(nèi)外參數(shù)及相差系數(shù)，相機參數(shù)的獲取通過相機標定實現(xiàn)，其標定的準確程度將會直接影響最終測量精度。

鑒于三維坐標維度信息充足、測量方便和求解效率高的優(yōu)點，采用三維標定參照物的方式進行標定測量。在測量空間中布置一系列非共面靶標，要盡可能充滿整個視場，在線性運算基礎(chǔ)上利用歸一化的求解方式進行多點迭代運算，通過單張圖像實現(xiàn)內(nèi)外參數(shù)的求解。即利用已知靶標的圖像坐標及對應的世界坐標估計相機內(nèi)外參數(shù)的初值，與鏡頭畸變參數(shù)一起進行非線性優(yōu)化搜索，估計出所有參數(shù)的最優(yōu)解[11]。

若引入投影矩陣：

(5)

則式(3)可以描述為關(guān)于像點的顯示表達：

(6)

可將方程組(6)寫成矩陣形式：

(7)

式中，m中未知量的個數(shù)是11，已知標定點的二維像素坐標和三維世界坐標，要求解式(7)，那么標定點的數(shù)量只要保證N≥6即可。由于方程的個數(shù)大于未知量的個數(shù)，因此該方程組為超定方程組，利用最小二乘法進行求解，其結(jié)果為：

m=(ATA)-1ATU。

(8)

上述過程中沒有考慮圖像畸變和噪聲的影響，只能獲得粗略的內(nèi)外參數(shù)結(jié)果。為了提高精度，以式(8)的計算結(jié)果為初值，采用LM(Levenberg-Marquardt)算法進行迭代優(yōu)化，構(gòu)造目標函數(shù)為：

(9)

3 多相機正交迭代算法

在計算過程中需要建立火箭的箭體坐標系Om-XmYmZm來反映箭體上所有點的信息，原點Om位于箭體尾部中心，Ym軸與箭體縱軸重合指向火箭頭部，Xm軸在箭體縱對稱面內(nèi)，垂直于Ym軸，在火箭平飛時指向上方，Zm軸與Xm、Ym構(gòu)成右手坐標系?；鸺陲w行過程中，其上各點坐標在箭體坐標系中的位置不變，可以用來描述空間點對箭體的位置關(guān)系和火箭姿態(tài)。通過箭體坐標系到世界坐標系的轉(zhuǎn)換矩陣和平移向量{Rmw,tmw}，可以將箭體上各點在箭體坐標系下的位置轉(zhuǎn)換為世界坐標系下的位置：

(10)

將式(10)帶入式(3)可以得到：

(11)

{Rmw,tmw}反映了火箭在世界坐標系中的位姿。由式(11)可知，在已知箭體上多個目標點的箭體坐標(Xm ,Ym,Zm)、相機拍攝的對應目標點的圖像坐標(u,v)及相機的內(nèi)外參數(shù)K,Rwc,twc的條件下，可以對{Rmw,tmw}進行求解。

控制點pi在多攝像機系統(tǒng)坐標系的坐標qi=Rpi+t；

物方殘差矢量在多攝像機系統(tǒng)坐標系下表示為：

(12)

多攝像機系統(tǒng)的位姿估計問題可以轉(zhuǎn)化為最小化下式的物方殘差平方和：

(13)

步驟1：對于第k次迭代，給定R的值，則t存在最優(yōu)解：

tk=t(Rk)=

(14)

步驟3：根據(jù)誤差函數(shù)則有：

(15)

(16)

可以得到：Rk+1=argmin(R)tr(RTM(Rk))。

對M(Rk)進行奇異值分解SVD，記為M(Rk)=UDTT，其中D=diag(σ1,σ2,...,σn)，得到式(16)問題的最優(yōu)解為：

Rk+1=UVT。

(17)

步驟4：判斷迭代矩陣的收斂性，設(shè)定一個閾值，當目標函數(shù)小于這個閾值，或者目標函數(shù)的相對變化量小到一定程度，或者達到了預設(shè)的迭代次數(shù)上限，迭代停止，輸出位姿參數(shù)R和t，否則返回步驟1重新計算。

4 測量結(jié)果計算與分析

4.1 仿真實驗

仿真實驗在Matlab環(huán)境下運行，依據(jù)透視投影原理將物體投影到攝像機平面，由于攝像機誤差和特征點提取誤差的存在，為模擬真實性，在圖像特征點中加入噪聲。

本實驗中，攝像機內(nèi)部參數(shù)為：等效焦距為(2 200，2 200)，主點為(1 024 pixel，1 024 pixel)，像面大小為(2 048 pixel×2 048 pixel)，取3個攝像機組成多攝像機系統(tǒng)。定義目標物尺寸為[-10,10]×[-10,10]×[-10,10]的立方體，在其表面均勻分布6個點作為標志點，3臺相機獲取的總標識點數(shù)為12個，3臺相機布站在與參考點坐標系原點的距離為100，且光軸指向原點的位置，目標的標志點投影到歸一化圖像平面產(chǎn)生圖像點，在圖像點中加入均值為零，標準方差為1 pixel的高斯噪聲，隨機仿真500次，統(tǒng)計誤差均值。旋轉(zhuǎn)矩陣的誤差計算公式為ER=arccos((tr(RTRg)-1)/2)，平移量的誤差計算公式為Et=‖t-tg‖/‖tg‖·100。

算法隨著噪聲、點數(shù)、攝像機個數(shù)增加的位姿估計誤差如圖4所示。從圖4中可以看出，隨著加入的噪聲級別、點數(shù)和相機個數(shù)的不斷增加，旋轉(zhuǎn)矩陣和平移量的誤差變化逐漸減小，可以看出算法有效且精度較高。

(a) 旋轉(zhuǎn)矩陣誤差隨噪聲級變化圖

(b) 旋轉(zhuǎn)矩陣誤差隨點數(shù)變化圖

(c) 旋轉(zhuǎn)矩陣誤差隨相機數(shù)變化圖

(d) 平移量誤差隨噪聲級變化圖

(e) 平移量誤差隨點數(shù)變化圖

(f) 平移量誤差隨相機數(shù)變化圖

4.2 真實實驗

將上述方法應用在實際情況中，其解算過程為：① 對高速電視測量系統(tǒng)進行標定，方法為在設(shè)備視場內(nèi)選取已知點作為標志點，測量標志點的大地坐標，每個視場內(nèi)至少需要6個標志點，且標志點最好均勻分布在整個視場。對拍攝的單幅圖像中的標志點進行判讀，結(jié)合測站和發(fā)射點的大地坐標，對設(shè)備內(nèi)外參數(shù)進行精確標校，得到設(shè)備相機的等效焦距，光心在測量坐標系中的位置，相機坐標系到發(fā)射坐標系轉(zhuǎn)換矩陣等參數(shù)；② 將發(fā)射坐標系作為這個多攝像機系統(tǒng)的攝像坐標系，以火箭中心為原點建立箭體坐標系，依據(jù)火箭外形尺寸計算火箭上所有測量點在箭體坐標系中的坐標；③ 對各相機每個圖像中的測量點進行判讀，并用判讀結(jié)果進行逐點多目正交迭代解算，獲得各測量時刻箭體坐標系到攝像坐標系的轉(zhuǎn)換矩陣，從而解算出各測量點的漂移量參數(shù)。

將起飛前火箭中心的漂移量位置參數(shù)與實測值進行比對，統(tǒng)計其在各方向上的總誤差均在厘米級，在一定程度上可以反映火箭起飛漂移量的測量精度。

5 結(jié)束語

利用攝像機視覺測量的結(jié)果，將單幅圖像中多個已知空間位置的特征點進行標定，從而獲得多臺攝像機的內(nèi)外參數(shù)，在此基礎(chǔ)上，對多攝像機拍攝的全部圖像序列統(tǒng)一表達，再把圖像上特征點的物方殘差作為目標函數(shù)，最后使用多目正交迭代算法求解各時刻箭體的位置和姿態(tài)，得到箭體各位置的漂移量。仿真和實際測量結(jié)果都表明，該方法是有效、精確的。但在解算過程中發(fā)現(xiàn)，由于火箭起飛過程中存在運動模糊現(xiàn)象，以及火箭起飛時會引起相機抖動，都給解算結(jié)果帶來誤差。基于此，下一步可以進一步分析誤差模型，尋求適應性更強的解法。