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

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

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

0 引言

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

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

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

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

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

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

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

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

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

1.2 坐標(biāo)系變換

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

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

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

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

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

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

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

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

圖2 3個特征點的幾何圖形Fig.2 The geometry figure of 3 feature points

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

對于N=3個點，有

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

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

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

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

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

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

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

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

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

下面以4點算法為例，對最小中值法進行闡述。

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

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

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

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

5)計算殘差平方為

6)計算中值為

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

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

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

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

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

4 仿真與分析

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

4.1 仿真條件

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

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

4.2 仿真結(jié)果分析

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

圖3 位置估計誤差Fig.3 The position estimation error

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

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

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

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

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

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

5 結(jié)束語

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

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

［1］ 陳海.無人機自主控制綜述及自主著陸控制系統(tǒng)設(shè)計［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］.國防科技大學(xué)學(xué)報，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］ 陳龍勝，陳謀，姜長生.基于視覺信息的無人機自主著陸過程姿態(tài)和位置估計［J］.電光與控制，2009，16(5):47-51.

［7］ 馬頌德，張正友.計算機視覺［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.