無人機(jī)視覺輔助著陸中的姿態(tài)和位置估算

李 飛， 唐大全， 沈 寧， 尹洪偉， 唐 波

(1.海軍航空工程學(xué)院，山東煙臺(tái) 264001; 2.中國人民解放軍92514部隊(duì)，山東煙臺(tái) 264001)

0 引言

據(jù)資料統(tǒng)計(jì)，無人機(jī)回收階段的故障數(shù)占整個(gè)任務(wù)執(zhí)行階段故障數(shù)的80%，因此無人機(jī)著陸導(dǎo)航技術(shù)受到廣泛關(guān)注。由計(jì)算機(jī)視覺發(fā)展起來的視覺輔助導(dǎo)航方法，具有精度高、不受電子干擾等特點(diǎn)，因此在無人機(jī)的著陸導(dǎo)航中受到越來越多的關(guān)注［1］。視覺輔助導(dǎo)航方法主要是通過安裝在飛機(jī)上的攝像機(jī)，獲得著陸點(diǎn)附近的圖像，使用計(jì)算機(jī)視覺算法，估計(jì)飛機(jī)的飛行狀態(tài)和相對于著陸點(diǎn)的位置和方位，結(jié)合其他機(jī)載傳感器的信息，實(shí)現(xiàn)無人機(jī)的自主著陸控制。

目前在利用視覺進(jìn)行著陸的研究中，研究最多的是提取跑道邊緣線和地平線等線特征來估算飛機(jī)的姿態(tài)和位置［2－6］。然而在戰(zhàn)時(shí)，公路等簡易機(jī)場都可以作為無人機(jī)的著陸場，這時(shí)候，地平線的提取易受建筑物、高山等地形的影響，跑道邊緣線的提取容易受河流、公路等具有線特征地物的影響。

為了克服這些缺點(diǎn)，本文尋找易辨識(shí)的特征。特殊標(biāo)識(shí)的點(diǎn)在圖像中較容易辨識(shí)，這些點(diǎn)在夜晚可以是布置在跑道附近的燈光點(diǎn)，白天可以布置特殊形狀的物體或者提取跑道附近特殊圖案的角點(diǎn);然后尋求地面上的點(diǎn)和圖像上的點(diǎn)之間的對應(yīng)關(guān)系，結(jié)合計(jì)算機(jī)視覺算法，提取出無人機(jī)著陸需要的相對跑道位置信息和姿態(tài)信息，同時(shí)結(jié)合最小中值法提高了精度。

1 坐標(biāo)系及坐標(biāo)變換

1.1 方程坐標(biāo)系介紹

在本文的研究中用到跑道坐標(biāo)系、機(jī)體坐標(biāo)系、攝像機(jī)坐標(biāo)系，3個(gè)坐標(biāo)系之間的關(guān)系如圖1所示。

各坐標(biāo)系的定義如下。跑道坐標(biāo)系P:ZP軸沿跑道中心線指向進(jìn)場方向;XP軸在跑道平面內(nèi)，與ZP垂直且指向右;YP軸垂直跑道平面且向下。

圖1 幾種坐標(biāo)系之間的關(guān)系Fig.1 The relation among the coordinate systems

攝像機(jī)坐標(biāo)系C:攝像機(jī)固聯(lián)在飛機(jī)上，坐標(biāo)原點(diǎn)為光學(xué)中心;ZC為光軸方向;XC垂直于飛機(jī)對稱面指向飛機(jī)右側(cè);YC由笛卡兒右手系決定。為了論述方便，假設(shè)攝像機(jī)系和機(jī)體系是重合的。

1.2 坐標(biāo)系變換

如何將跑道坐標(biāo)系中的特征點(diǎn)投影到成像平面，是研究的基礎(chǔ)，這個(gè)投影過程分為以下兩步。

1)跑道坐標(biāo)系向攝像機(jī)坐標(biāo)系的變換，公式為

其中:旋轉(zhuǎn)矩陣R由3個(gè)姿態(tài)角組成;平移矩陣D由飛機(jī)相對跑道的位置構(gòu)成。

2)攝像機(jī)坐標(biāo)系向圖像坐標(biāo)系變換近似為小孔成像。圖像坐標(biāo)系的原點(diǎn)O1為光軸與圖像平面的交點(diǎn);f為焦距;(x，y)為攝像機(jī)系中參考點(diǎn)在圖像上的投影坐標(biāo)。則有投影公式為

2 飛機(jī)姿態(tài)位置解算

下面將分兩步解算飛機(jī)的姿態(tài)和位置:第一步求出特征點(diǎn)在攝像機(jī)系中的坐標(biāo)［7］;第二步根據(jù)攝像機(jī)坐標(biāo)及其對應(yīng)的跑道坐標(biāo)系坐標(biāo)，解算出飛機(jī)的姿態(tài)和位置［8］。

2.1 解算特征點(diǎn)在攝像機(jī)坐標(biāo)系中的坐標(biāo)

假設(shè)已知3個(gè)特征點(diǎn)在跑道系和圖像系上的坐標(biāo)。

圖2 3個(gè)特征點(diǎn)的幾何圖形Fig.2 The geometry figure of 3 feature points

3個(gè)特征點(diǎn)在攝像機(jī)系中坐標(biāo)。dij(i，j=1，2，3)為這3個(gè)特征點(diǎn)在跑道系中的對應(yīng)距離，根據(jù)圖2中的對應(yīng)關(guān)系，有并且參考式(2)(為了表述方便，這里用Xi代替，等式便可寫成

對于N=3個(gè)點(diǎn)，有

對于此多項(xiàng)式方程，最多有4個(gè)解。如果得到唯一解，根據(jù)X1是正數(shù)，可以將其唯一確定，然后根據(jù)式(2)便可確定其他兩個(gè)坐標(biāo)值。

為了得到唯一解需要再多增加幾個(gè)點(diǎn)。對于N個(gè)點(diǎn)，能夠得到(N(N－1))/2個(gè)fij(Xi，Xj)=0，進(jìn)而得到((N－1)(N－2))/2個(gè)四次方程g(X)=0。

對于N=4，得到3個(gè)諸如式(5)的等式，寫成矩陣形式

令 t5=(t0，t1，t2，t3，t4)=(1，X，X2，X3，X4)T，則方程(6)可看作齊次線性方程組。對該齊次線性方程組運(yùn)用奇異值分解的方法［9］便可求得唯一解。

同樣對于任意N≥5個(gè)點(diǎn)，都可以獲得如式(6)形式的齊次線性方程組，再利用奇異值分解的方法即可求得唯一解。

2.2 解算飛機(jī)姿態(tài)位置

由于飛機(jī)的姿態(tài)和位置構(gòu)成了旋轉(zhuǎn)矩陣R和平移向量D，因此可以通過求出R和D得出飛機(jī)的姿態(tài)和位置。首先利用正交化算法［8］。假設(shè)跑道上3個(gè)點(diǎn)P1，P2，P3，在攝像機(jī)坐標(biāo)系對應(yīng)坐標(biāo)為 C1，C2，C3。記為

顯然，Up=(rp1，rp2，rp3)和 Uc=(rc1，rc2，rc3)是正交陣，于是，其中

在實(shí)際成像過程中，往往存在成像噪聲，一些點(diǎn)的誤差可能大于兩三個(gè)像素，甚至?xí)霈F(xiàn)匹配誤差，如果直接利用這些匹配點(diǎn)將產(chǎn)生較大的誤差，魯棒性會(huì)很差。根據(jù)文獻(xiàn)［10］，將最小中值法用于計(jì)算旋轉(zhuǎn)矩陣和平移向量可以大大提高算法的魯棒性。

下面以4點(diǎn)算法為例，對最小中值法進(jìn)行闡述。

1)從圖像平面上的點(diǎn)(1，2，…，n)中隨機(jī)選出4個(gè)點(diǎn)，例如，J=(1，2，3，4)。

2)利用4點(diǎn)定位算法求得這4個(gè)點(diǎn)在攝像機(jī)系中的坐標(biāo)，并由圖像匹配給出對應(yīng)的跑道系坐標(biāo)。

3)重復(fù)1)～2)，得出第k種隨機(jī)點(diǎn)樣本組合的跑道系坐標(biāo)和攝像機(jī)系坐標(biāo)。假設(shè)共取m個(gè)樣本組合。

4)取出3)中m種樣本組合的第k種。4個(gè)點(diǎn)對為(P1，P2，P3，P4)，(C1，C2，C3，C4)，在這 4 個(gè)點(diǎn)中，任取3個(gè)點(diǎn)進(jìn)行組合，共有4種組合，如第1種點(diǎn)對的組合為(P1，P2，P3) 和(C1，C2，C3)，若(P1，P2，P3) 和(C1，C2，C3)中的一組3個(gè)點(diǎn)共線或接近共線，則舍棄不用，否則對(P1，P2，P3)和(C1，C2，C3)使用正交化算法求出旋轉(zhuǎn)矩陣和平移向量，4種組合解算完畢求出沒有舍棄的組合的平均值RJ和DJ。

5)計(jì)算殘差平方為

6)計(jì)算中值為

3 卡爾曼濾波方法的應(yīng)用

已知系統(tǒng)狀態(tài)為:X(k)=［Xb，Yb，Zb，Vx，Vy，Vz，ψ，?，γ，r，q，p］T，(Xb，Yb，Zb)為飛機(jī)在跑道坐標(biāo)系中的位置;(Vx，Vy，Vz)為飛機(jī)在橫向、垂直、縱向的速度;(ψ，?，γ)為飛機(jī)的航向角、俯仰角、滾轉(zhuǎn)角;(r，q，p)為橫滾角速度、俯仰角速度、偏航角速度。濾波測量量由第2節(jié)求得的飛機(jī)姿態(tài)和位置給出:Z(k)=［Xb，Yb，Zb，ψ，θ，γ］T。飛機(jī)的位置從某一時(shí)刻到下一時(shí)刻的系統(tǒng)轉(zhuǎn)移關(guān)系為

飛機(jī)的姿態(tài)角與上一時(shí)刻姿態(tài)角和轉(zhuǎn)動(dòng)角速度的關(guān)系為

由式(12)和式(13)可以得出系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣。

在得到了上述條件和飛機(jī)的初始時(shí)刻參數(shù)后，應(yīng)用卡爾曼濾波方法［11］，就可得到每個(gè)時(shí)刻飛機(jī)的姿態(tài)和位置。

4 仿真與分析

參考國際民航組織精密進(jìn)場著陸Ⅲ級(jí)精度要求，提出無人機(jī)著陸導(dǎo)航系統(tǒng)導(dǎo)航精度要求:到達(dá)著陸點(diǎn)時(shí)，垂直精度高于1.97 m;橫向精度高于13.45 m。由于跑道長1000 m，所以對于縱向的位置偏差并沒有做要求。俯仰角精度高于0.5°，滾轉(zhuǎn)角精度高于0.5°，航向角精度高度高于0.5°。

4.1 仿真條件

構(gòu)建20個(gè)隨機(jī)點(diǎn)，這些點(diǎn)在沿跑道橫向服從均值為0，標(biāo)準(zhǔn)方差為100的正態(tài)分布，在沿跑道縱向0～1000 m內(nèi)服從均勻隨機(jī)分布，并且假設(shè)在點(diǎn)匹配過程中存在兩對錯(cuò)誤匹配點(diǎn)。

飛機(jī)初始下滑高度為60 m，距離跑道起點(diǎn)縱向距離為1000 m。飛機(jī)下滑角為－3°，攝像機(jī)光軸與飛機(jī)縱軸夾角3°，且在飛機(jī)縱軸下方，下滑速度為40 m/s。系統(tǒng)噪聲方差陣為對角陣，前6 個(gè)對角元素為［2，2，2，0.25，0.25，0.25］，后6 個(gè)對角元素均為(0.5π/180)2。測量噪聲方差陣為對角陣，對角元素為［5，5，5，(0.5π/180)2，(0.5π/180)2，(0.5π/180)2］。

4.2 仿真結(jié)果分析

基于上述仿真條件，使用MatlabR2009a軟件進(jìn)行仿真，取N為4，即針對4點(diǎn)算法情況進(jìn)行仿真，著陸導(dǎo)航信息誤差圖見圖3和圖4。

圖3 位置估計(jì)誤差Fig.3 The position estimation error

圖4 姿態(tài)估計(jì)誤差Fig.4 The attitude estimation error

由圖3可知，對著陸安全性要求最高的橫向位置誤差基本能夠控制在5 m以內(nèi)，尤其是當(dāng)距離機(jī)場越近時(shí)，誤差越小，20 s后，距離機(jī)場縱向距離為201 m，橫向誤差基本在2 m以內(nèi);高度誤差基本控制在1.5 m以內(nèi)，20 s后，距離地面高度在18 m，高度誤差控制在0.5 m以內(nèi);縱向誤差維持在10 m以內(nèi)。由圖4可知，俯仰角的誤差精度基本控制在0.5°內(nèi)，尤其進(jìn)入10 s后，精度達(dá)到 0.2°以內(nèi)，進(jìn)入 20 s后，精度達(dá)到 0.1°以內(nèi);偏航角在仿真開始時(shí)誤差較大，但隨著距離機(jī)場越近時(shí)，誤差逐漸減小，進(jìn)入20 s后，精度達(dá)到0.2°;滾轉(zhuǎn)角始終保持在0.5°的精度范圍。綜上所述，本文所提算法滿足無人機(jī)著陸要求。

最小中值法對于提高算法的魯棒性至關(guān)重要，文章對最小中值法和常用的最小二乘法的效果進(jìn)行了對比。兩種方法的方差對比結(jié)果如表1和表2所示。由表1和表2可知，最小中值法解算出的導(dǎo)航信息的精度要遠(yuǎn)大于最小二乘法，究其原因，主要是噪聲和錯(cuò)誤匹配點(diǎn)的影響，最小二乘法假設(shè)噪聲是高斯分布的，這是合理的，因?yàn)辄c(diǎn)的誤差往往在一兩個(gè)像素之內(nèi)，但是有一些點(diǎn)可能沒有很好地提取或者存在錯(cuò)誤匹配，其誤差要遠(yuǎn)大于兩三個(gè)像素，這些點(diǎn)將嚴(yán)重影響導(dǎo)航信息估計(jì)的精度;而最小中值法能夠排除誤差較大的那些匹配點(diǎn)，由于僅考慮殘差的中值，理論上即使有接近50%的錯(cuò)誤數(shù)據(jù)，最小中值法都能給出精度很高的導(dǎo)航信息估計(jì)。

表1 位置誤差方差對比Table 1 The contrast of position error variance

表2 姿態(tài)誤差方差對比Table 2 The contrast of attitude error variance

此外，文章還對N為5的情況進(jìn)行了實(shí)驗(yàn)仿真，即5點(diǎn)算法的情況。通過分析，使用4點(diǎn)算法解算一幀圖像平均耗時(shí)52 ms，而使用5點(diǎn)算法解算一幀圖像平均耗時(shí)320 ms，對于實(shí)時(shí)性要求很高的無人機(jī)著陸，5點(diǎn)算法不可取。

5 結(jié)束語

針對無人機(jī)在公路等簡易機(jī)場著陸的情況，分析了線特征作為著陸輔助特征的缺點(diǎn)，并選取點(diǎn)特征進(jìn)行輔助降落，運(yùn)用N點(diǎn)算法和正交化算法求出了飛機(jī)的姿態(tài)和位置，考慮到噪聲的影響，引入最小中值法和卡爾曼濾波法提高了算法的精度。通過仿真驗(yàn)證了文章所提算法解算出的位置和姿態(tài)均能滿足著陸導(dǎo)航要求，并且通過對比分析了最小中值法對提高魯棒性的作用，最后通過實(shí)驗(yàn)，發(fā)現(xiàn)5點(diǎn)算法的實(shí)時(shí)性無法滿足著陸要求。

在公路等簡易機(jī)場上，基于輔助特征點(diǎn)的視覺著陸在本文中得到了探索，為無人機(jī)視覺著陸的研究提供了一種新的思路。

［1］ 陳海.無人機(jī)自主控制綜述及自主著陸控制系統(tǒng)設(shè)計(jì)［D］.西安:西北工業(yè)大學(xué)，2007.

［2］ PAN Xiang，MA Deqiang.Vision-based approach angle and height estimation for UAV landing［C］//IEEE Congress on Image and Signal Processing，2008:801-805.

［3］ LIU Tianshu，F(xiàn)LEMING G.Videogrammetric determination of aircraft position and attitude for vision-based autonomous landing［C］//The 44th AIAA Aerospace Sciences Meeting and Exhibit，2006，1437:1-35.

［4］ 朱憲偉，李由.機(jī)載視覺自主著陸過程中的跑道提取方法［J］.國防科技大學(xué)學(xué)報(bào)，2009，31(2):20-24.

［5］ TRISIRIPISAL P，PARKS M R.Stereo analysis for visionbased guidance and control of aircraft landing［C］//44th AIAA Aerospance Sciences Meeting and Exhibit.Reno，Nevada，2006，1438.

［6］ 陳龍勝，陳謀，姜長生.基于視覺信息的無人機(jī)自主著陸過程姿態(tài)和位置估計(jì)［J］.電光與控制，2009，16(5):47-51.

［7］ 馬頌德，張正友.計(jì)算機(jī)視覺［M］.北京:科學(xué)出版社，1998:53-56，159-162.

［8］ QUAN Long，LAN Zhongdan.Linear N-point camera pose determination［J］.IEEE Transactions On Pattern Analysis And Machine Intelligence，1999，21(8):774-780.

［9］ 陳祖明，周家勝.矩陣輪引論［M］.北京:北京航空航天大學(xué)出版社，1998:192-195.

［10］ GHYZEL P A.Vision-based navigation for autonomous landing of UAV［D］.Naval Postgraduate School，2000.

［11］ 付夢印.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用［M］.北京:科學(xué)出版社，2010.