視覺(jué)信息位姿估計(jì)在火箭起飛漂移量中的應(yīng)用

高 燕，夏 霏

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

0 引言

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

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

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

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

1 視覺(jué)信息位姿估計(jì)原理

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

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

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

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

(1)

式中，fx,fy為橫向和縱向等效焦距，表示實(shí)際光學(xué)焦距與像元的橫縱長(zhǎng)度之比。

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

(2)

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

(3)

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

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

，

(4)

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

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

圖2 相機(jī)成像過(guò)程示意圖Fig.2 Schematic diagram of camera imaging process

2 相機(jī)標(biāo)定

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

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

若引入投影矩陣：

(5)

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

(6)

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

(7)

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

m=(ATA)-1ATU。

(8)

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

(9)

3 多相機(jī)正交迭代算法

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

(10)

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

(11)

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

控制點(diǎn)pi在多攝像機(jī)系統(tǒng)坐標(biāo)系的坐標(biāo)qi=Rpi+t；

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

(12)

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

(13)

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

tk=t(Rk)=

(14)

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

(15)

(16)

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

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

Rk+1=UVT。

(17)

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

4 測(cè)量結(jié)果計(jì)算與分析

4.1 仿真實(shí)驗(yàn)

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

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

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

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

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

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

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

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

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

4.2 真實(shí)實(shí)驗(yàn)

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

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

5 結(jié)束語(yǔ)

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