基于線特征的無人機自主著陸慣性/視覺導航方法

李 洪，王大元，明 麗，童 櫟

（1.北京自動化控制設備研究所，北京 100074；2.陸軍裝備部航空軍事代表局駐北京地區航空軍事代表室，北京 100012）

衛星拒止條件下無人機高精度自主導航技術是決定無人機能否順利完成戰略任務的關鍵，其中著陸階段更是飛行全過程中的研究難點之一[1]。根據無人機著陸剖面可知，無人機著陸過程的各階段對姿態和位置的測量均具有嚴格的要求[2]。

目前常用的無人機著陸導航方式有慣性導航（Inertial Navigation System, INS），全球定位系統（Global Positioning System, GPS）[3]等。但是慣性導航誤差會隨時間累積而發散，GPS戰時易受干擾，兩種導航方法均具有明顯缺點。視覺導航是基于計算機視覺技術提出，通過視覺傳感器采集無人機著陸過程中的實時圖像，并計算無人機與機場跑道之間的相對幾何關系，從而求取二者之間的相對姿態與位置的導航方式。如何實時準確地獲取無人機相對著陸場站的位姿信息是視覺導航的關鍵技術與主要瓶頸[4,5]。

視覺導航不依賴于地面設備完全自主，在測量精度方面也具有明顯優勢。近年來基于機場跑道點特征的無人機視覺著陸技術迅速發展，但是點特征本身對環境依賴性很大，在季節交替、天氣變化和跑道磨損時極易發生誤匹配現象，而線特征作為更高層次的特征具有較高的光照不變性，在圖像中位置也更為精確；國防科技大學張小苗等[6]利用機場跑道平行信息求解消隱點[7]與無人機位姿之間的關系，由單幅圖像解算無人機相對位姿。周郎明等[8]在無人機降落前期利用完整的結構化線特征配置解算出無人機的六自由度位姿參數。在著陸末段中，無人機越過跑道起始線后存在視覺測量盲區[9]，無法得到準確的位姿解算結果。相比之下，慣性導航方式雖然誤差會隨時間累積而發散，但是不受光照、視線等因素影響且具有完全自主性[10,11]，與視覺導航具有很好的互補性[12,13]。因此，本文針對無人機視覺著陸導航過程中點特征魯棒性差、著陸末段存在視覺測量盲區等問題，開展基于線特征的無人機自主著陸慣性/視覺導航方法研究。

本文提出了一種基于機場跑道三條等距平行線（包括機場跑道左右邊線以及中心線）的慣性/視覺相對位姿測量模型。通過三條等距平行線確定像平面的消隱點坐標與消隱線方程，隨后將其代入視覺測量模型實時估計相機投影轉換矩陣，并以此求解無人機相對機場跑道的位姿以及垂向和側向距離。在此基礎上利用擴展卡爾曼濾波方法（Extend Kalman Filter, EKF）實現慣性導航信息與視覺測量信息的融合，解決無人機著陸末段越過跑道起始線后視覺測量存在盲區以及純慣性誤差隨時間發散等問題。最終通過飛行試驗對本文所提方法的有效性進行驗證。

1 基礎理論

1.1 坐標系定義

如圖1所示，建立機場坐標系、視覺坐標系、世界坐標系、攝像機坐標系、圖像坐標系。

圖1 坐標系示意圖Fig.1 Coordinate system diagram

其中機場坐標系（a系）：以跑道著陸端起始線與跑道中心線的交點為原點ao；ax軸沿跑道中心線，前向為正；ya軸垂直于跑道平面，向上為正；za軸與跑道起始線重合，右向為正；oaxayaza構成右手坐標系；機場坐標系下某點的坐標用(xa,ya,za)表示。

視覺坐標系（v系）：著陸視覺導航系統坐標系，簡稱視覺坐標系；以光學系統的像方主點為原點ov；xv軸平行于光軸，前向為正；yv軸平行于成像平面坐標系的橫軸，向上為正；zv軸與xv軸和yv軸構成右手坐標系，右向為正。

世界坐標系（w系）：以跑道著陸端瞄準點起始線與跑道中心線的交點為原點ow；xw軸與跑道起始線重合，右向為正；yw軸垂直于跑道平面，向下為正；zw軸沿跑道中心線，前向為正；owxwywzw構成右手坐標系；世界坐標系下某點的坐標用（xw,yw,zw）表示。

攝像機坐標系（c系）：以光學系統的像方主點為原點oc；當正對光學系統觀察時，xc軸平行于成像平面坐標系的水平軸，左向為正；yc軸平行于成像平面坐標系的垂直軸，向下為正；zc軸指向觀察者，并與xc軸和yc軸構成右手坐標系。

圖像坐標系（i系）：在攝像機光敏面所在的平面內建立圖像坐標系，它是一個二維平面坐標系，以圖像左上角為原點，沿圖像水平方向向右為圖像坐標系的xi軸，沿圖像垂直方向向下為圖像坐標系的yi軸，圖像坐標系的單位是像素。

1.2 無窮遠元素及Plücker表示法

在物空間中，兩條平行線永不相交，在歐式空間基礎上通過引入無窮遠元素構建射影空間，平面內一組平行線相交于無窮遠處唯一點P∞i，稱之為消隱點(Vanishing Point)。如圖2所示。該點在像平面上的位置只跟攝像機的姿態有關而與攝像機的位置無關。

圖2 一組平行線相交于消隱點示意圖Fig.2 Schematic diagram of a set of parallel lines intersecting at the vanishing point

消隱點代表對應平行線的方向，不平行直線的無窮遠點不同，平面上所有的無窮遠點構成一條直線，即消隱線（Vanishing Line）。消隱線是空間內一組平行面在無窮遠處的唯一交線。在像空間圖像坐標系下直線方程可描述為：

因此，直線可用三維向量表示：

而在物空間世界坐標系中設兩點A、B的三維坐標分別為(3×1矩陣)，那么它們的齊次坐標為：過這兩點的直線可用一個4×4的反對稱齊次矩陣L表示，該矩陣稱之為Plücker矩陣。

另外，直線L可用其方向向量與矩m來表示，稱之為Plücker坐標，記為：

其中，是直線的方向向量，矩m(可表征ABCΔ 的面積或者O到直線L的距離)是該直線和原點確定平面的法向量，即如圖3所示。

圖3 直線的Plücker坐標含義Fig.3 Plücker coordinate meaning of a straight line

由此可得Plücker矩陣與Plücker坐標間的關系為：

在攝像機映射T的作用下，用Plücker矩陣定義的直線L表示圖像坐標系下對應直線的像l:

式(6)中，K為相機內參矩陣：

為世界坐標系到攝像機坐標系的姿態轉移矩陣，為攝像機坐標系原點在世界坐標系中的位置向量。s2只是直線中各參數的共有系數，因此式(6)可簡化為：

2 視覺測量位姿解算

2.1 消隱點及消隱線成像方程

由圖2可知，在無人機著陸過程中，視覺著陸系統對跑道線特征進行提取，其中跑道左右邊線及中心線為一組平行線，可用來進行無窮遠處消隱點坐標和消隱線方程的計算，設物空間內跑道上三條等距平行線為L0w、L1w、L2w，其在像平面內成像為l0i、l1i、l2i，則像空間內消隱點坐標可由以下關系求解：

消隱線方程為：

假設物空間中一點A的四維齊次坐標那么過點A且方向為(三維單位列向量)，可表示為：

當參數λ由0變化到∞時，A點由有限點變化到無窮遠點，該點在世界坐標系下的坐標為：

根據圖像共軛方程獲得消隱點與無人機姿態轉移矩陣的關系為：

進一步整理方程得：

假設像空間消隱直線l∞i上某點為x，其在物空間的反向投影為一條方向為的直線。由點x在直線上，可得：

利用與平面的法向量nπ正交可得：

利用物空間中方向為的直線投影為像空間的點x，得：

對式(17)進行轉置變換得：

由式(16)(18)可得：

根據式(15)(19)可得像空間消隱線方程：

2.2 無人機位姿解算

聯立式(9)(10)(14)(20)得方程組如下：

式(21)中，為姿態轉移矩陣：

其中，

由式(21)(24)解得姿態轉移矩陣：

由此解得三個姿態角：

由式(8)利用線方程計算相對位置方法：

其中：

即：

同理，由直線L2確定：

由式(30)(31)求解α0、α2：

最后解得無人機在世界坐標系下的垂向、側向位置ty、tx：

3 慣性/視覺融合方法

由慣性導航系統可獲得無人機瞬時姿態、速度、位置及原始慣性傳感器信息，組合導航EKF狀態方程：

其中，F(t)為t時刻狀態轉移矩陣，為t時刻系統隨機噪聲向量，系統狀態向量如式(35)所示：

其中：

濾波狀態量分別為北天東速度誤差(單位：m/s)、維度誤差(單位：rad)、高度誤差(單位：m)、經度誤差(單位：rad)、北天東向失準角誤差(單位：rad)、載體系XYZ方向陀螺漂移(單位：rad/s)、載體系XYZ方向加速度計零位(單位： m/s2)。

觀測方程定義如下：

組合導航系統的觀測量為機場坐標系慣性導航輸出的垂向側向位置與視覺著陸系統導航結果的差值，如式(38)：

綜上，給出了無人機著落末段越過起始線后，視覺存在盲區時，利用跑道左右邊線和中線三條等距平行線的慣性/視覺導航方法。

4 試驗驗證

本試驗選用某型號固定翼無人機為試驗載機，試驗地點為中國西北部某民用機場。試驗判定基準為差分衛星后處理獲取的垂向、側向位置信息。試驗結果如圖4、圖5所示。

圖4 垂向、側向相對位移Fig.4 Vertical and lateral relative displacement

圖5 垂向、側向誤差Fig.5 Vertical and lateral error

圖4為差分衛星基準、著陸視覺導航、慣性/視覺組合導航三種導航方式結果與機場相對垂向、側向位置測量值曲線，三種導航方式結果分別對應圖中紅色、藍色、黑色曲線。圖5為以差分衛星為基準，視覺導航與慣性/視覺組合導航兩種導航方式的垂向、側向誤差曲線。兩幅圖像橫坐標均為無人機與機場跑道起始線間的距離，在距離跑道826 m時開始應用本文算法。由圖5可知，在本文提出的算法進行無人機實時位姿解算，無人機垂向定位精度可達10 m，側向定位精度可達5 m。在圖5中可以看出，在視覺信息發生跳變時，慣性/視覺融合方法可以有效抑制視覺跳變，試驗結果驗證了本文提出算法的有效性。

5 結 論

本文針對固定翼無人機著陸場景下傳統點特征提取受光照、跑道磨損等外界因素干擾，影響定位精度的問題，提出了一種基于線特征的無人機位姿解算方法。引入無窮遠元素，通過跑道內三條等距平行線的方程計算圖像空間內無窮遠處的消隱點坐標及消隱線方程，從而解算出實時的無人機位姿，并基于EKF算法將視覺位姿與慣性導航輸出結果進行融合，經機載試驗驗證，本文提出的算法可以使無人機垂向定位精度達10 m、側向定位精度達5 m，解決了無人機著陸過程中提取點特征受外界因素干擾、視覺信息易發生跳變、慣性導航誤差隨時間發散等問題。