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

（1.北京自動(dòng)化控制設(shè)備研究所，北京 100074；2.陸軍裝備部航空軍事代表局駐北京地區(qū)航空軍事代表室，北京 100012）

衛(wèi)星拒止條件下無(wú)人機(jī)高精度自主導(dǎo)航技術(shù)是決定無(wú)人機(jī)能否順利完成戰(zhàn)略任務(wù)的關(guān)鍵，其中著陸階段更是飛行全過(guò)程中的研究難點(diǎn)之一[1]。根據(jù)無(wú)人機(jī)著陸剖面可知，無(wú)人機(jī)著陸過(guò)程的各階段對(duì)姿態(tài)和位置的測(cè)量均具有嚴(yán)格的要求[2]。

目前常用的無(wú)人機(jī)著陸導(dǎo)航方式有慣性導(dǎo)航（Inertial Navigation System, INS），全球定位系統(tǒng)（Global Positioning System, GPS）[3]等。但是慣性導(dǎo)航誤差會(huì)隨時(shí)間累積而發(fā)散，GPS戰(zhàn)時(shí)易受干擾，兩種導(dǎo)航方法均具有明顯缺點(diǎn)。視覺(jué)導(dǎo)航是基于計(jì)算機(jī)視覺(jué)技術(shù)提出，通過(guò)視覺(jué)傳感器采集無(wú)人機(jī)著陸過(guò)程中的實(shí)時(shí)圖像，并計(jì)算無(wú)人機(jī)與機(jī)場(chǎng)跑道之間的相對(duì)幾何關(guān)系，從而求取二者之間的相對(duì)姿態(tài)與位置的導(dǎo)航方式。如何實(shí)時(shí)準(zhǔn)確地獲取無(wú)人機(jī)相對(duì)著陸場(chǎng)站的位姿信息是視覺(jué)導(dǎo)航的關(guān)鍵技術(shù)與主要瓶頸[4,5]。

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

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

1 基礎(chǔ)理論

1.1 坐標(biāo)系定義

如圖1所示，建立機(jī)場(chǎng)坐標(biāo)系、視覺(jué)坐標(biāo)系、世界坐標(biāo)系、攝像機(jī)坐標(biāo)系、圖像坐標(biāo)系。

圖1 坐標(biāo)系示意圖Fig.1 Coordinate system diagram

其中機(jī)場(chǎng)坐標(biāo)系（a系）：以跑道著陸端起始線(xiàn)與跑道中心線(xiàn)的交點(diǎn)為原點(diǎn)ao；ax軸沿跑道中心線(xiàn)，前向?yàn)檎?；ya軸垂直于跑道平面，向上為正；za軸與跑道起始線(xiàn)重合，右向?yàn)檎?；oaxayaza構(gòu)成右手坐標(biāo)系；機(jī)場(chǎng)坐標(biāo)系下某點(diǎn)的坐標(biāo)用(xa,ya,za)表示。

視覺(jué)坐標(biāo)系（v系）：著陸視覺(jué)導(dǎo)航系統(tǒng)坐標(biāo)系，簡(jiǎn)稱(chēng)視覺(jué)坐標(biāo)系；以光學(xué)系統(tǒng)的像方主點(diǎn)為原點(diǎn)ov；xv軸平行于光軸，前向?yàn)檎?；yv軸平行于成像平面坐標(biāo)系的橫軸，向上為正；zv軸與xv軸和yv軸構(gòu)成右手坐標(biāo)系，右向?yàn)檎?/p>

世界坐標(biāo)系（w系）：以跑道著陸端瞄準(zhǔn)點(diǎn)起始線(xiàn)與跑道中心線(xiàn)的交點(diǎn)為原點(diǎn)ow；xw軸與跑道起始線(xiàn)重合，右向?yàn)檎?；yw軸垂直于跑道平面，向下為正；zw軸沿跑道中心線(xiàn)，前向?yàn)檎?；owxwywzw構(gòu)成右手坐標(biāo)系；世界坐標(biāo)系下某點(diǎn)的坐標(biāo)用（xw,yw,zw）表示。

攝像機(jī)坐標(biāo)系（c系）：以光學(xué)系統(tǒng)的像方主點(diǎn)為原點(diǎn)oc；當(dāng)正對(duì)光學(xué)系統(tǒng)觀(guān)察時(shí)，xc軸平行于成像平面坐標(biāo)系的水平軸，左向?yàn)檎粂c軸平行于成像平面坐標(biāo)系的垂直軸，向下為正；zc軸指向觀(guān)察者，并與xc軸和yc軸構(gòu)成右手坐標(biāo)系。

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

1.2 無(wú)窮遠(yuǎn)元素及Plücker表示法

在物空間中，兩條平行線(xiàn)永不相交，在歐式空間基礎(chǔ)上通過(guò)引入無(wú)窮遠(yuǎn)元素構(gòu)建射影空間，平面內(nèi)一組平行線(xiàn)相交于無(wú)窮遠(yuǎn)處唯一點(diǎn)P∞i，稱(chēng)之為消隱點(diǎn)(Vanishing Point)。如圖2所示。該點(diǎn)在像平面上的位置只跟攝像機(jī)的姿態(tài)有關(guān)而與攝像機(jī)的位置無(wú)關(guān)。

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

消隱點(diǎn)代表對(duì)應(yīng)平行線(xiàn)的方向，不平行直線(xiàn)的無(wú)窮遠(yuǎn)點(diǎn)不同，平面上所有的無(wú)窮遠(yuǎn)點(diǎn)構(gòu)成一條直線(xiàn)，即消隱線(xiàn)（Vanishing Line）。消隱線(xiàn)是空間內(nèi)一組平行面在無(wú)窮遠(yuǎn)處的唯一交線(xiàn)。在像空間圖像坐標(biāo)系下直線(xiàn)方程可描述為：

因此，直線(xiàn)可用三維向量表示：

而在物空間世界坐標(biāo)系中設(shè)兩點(diǎn)A、B的三維坐標(biāo)分別為(3×1矩陣)，那么它們的齊次坐標(biāo)為：過(guò)這兩點(diǎn)的直線(xiàn)可用一個(gè)4×4的反對(duì)稱(chēng)齊次矩陣L表示，該矩陣稱(chēng)之為Plücker矩陣。

另外，直線(xiàn)L可用其方向向量與矩m來(lái)表示，稱(chēng)之為Plücker坐標(biāo)，記為：

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

圖3 直線(xiàn)的Plücker坐標(biāo)含義Fig.3 Plücker coordinate meaning of a straight line

由此可得Plücker矩陣與Plücker坐標(biāo)間的關(guān)系為：

在攝像機(jī)映射T的作用下，用Plücker矩陣定義的直線(xiàn)L表示圖像坐標(biāo)系下對(duì)應(yīng)直線(xiàn)的像l:

式(6)中，K為相機(jī)內(nèi)參矩陣：

為世界坐標(biāo)系到攝像機(jī)坐標(biāo)系的姿態(tài)轉(zhuǎn)移矩陣，為攝像機(jī)坐標(biāo)系原點(diǎn)在世界坐標(biāo)系中的位置向量。s2只是直線(xiàn)中各參數(shù)的共有系數(shù)，因此式(6)可簡(jiǎn)化為：

2 視覺(jué)測(cè)量位姿解算

2.1 消隱點(diǎn)及消隱線(xiàn)成像方程

由圖2可知，在無(wú)人機(jī)著陸過(guò)程中，視覺(jué)著陸系統(tǒng)對(duì)跑道線(xiàn)特征進(jìn)行提取，其中跑道左右邊線(xiàn)及中心線(xiàn)為一組平行線(xiàn)，可用來(lái)進(jìn)行無(wú)窮遠(yuǎn)處消隱點(diǎn)坐標(biāo)和消隱線(xiàn)方程的計(jì)算，設(shè)物空間內(nèi)跑道上三條等距平行線(xiàn)為L(zhǎng)0w、L1w、L2w，其在像平面內(nèi)成像為l0i、l1i、l2i，則像空間內(nèi)消隱點(diǎn)坐標(biāo)可由以下關(guān)系求解：

消隱線(xiàn)方程為：

假設(shè)物空間中一點(diǎn)A的四維齊次坐標(biāo)那么過(guò)點(diǎn)A且方向?yàn)?三維單位列向量)，可表示為：

當(dāng)參數(shù)λ由0變化到∞時(shí)，A點(diǎn)由有限點(diǎn)變化到無(wú)窮遠(yuǎn)點(diǎn)，該點(diǎn)在世界坐標(biāo)系下的坐標(biāo)為：

根據(jù)圖像共軛方程獲得消隱點(diǎn)與無(wú)人機(jī)姿態(tài)轉(zhuǎn)移矩陣的關(guān)系為：

進(jìn)一步整理方程得：

假設(shè)像空間消隱直線(xiàn)l∞i上某點(diǎn)為x，其在物空間的反向投影為一條方向?yàn)榈闹本€(xiàn)。由點(diǎn)x在直線(xiàn)上，可得：

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

利用物空間中方向?yàn)榈闹本€(xiàn)投影為像空間的點(diǎn)x，得：

對(duì)式(17)進(jìn)行轉(zhuǎn)置變換得：

由式(16)(18)可得：

根據(jù)式(15)(19)可得像空間消隱線(xiàn)方程：

2.2 無(wú)人機(jī)位姿解算

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

式(21)中，為姿態(tài)轉(zhuǎn)移矩陣：

其中，

由式(21)(24)解得姿態(tài)轉(zhuǎn)移矩陣：

由此解得三個(gè)姿態(tài)角：

由式(8)利用線(xiàn)方程計(jì)算相對(duì)位置方法：

其中：

即：

同理，由直線(xiàn)L2確定：

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

最后解得無(wú)人機(jī)在世界坐標(biāo)系下的垂向、側(cè)向位置ty、tx：

3 慣性/視覺(jué)融合方法

由慣性導(dǎo)航系統(tǒng)可獲得無(wú)人機(jī)瞬時(shí)姿態(tài)、速度、位置及原始慣性傳感器信息，組合導(dǎo)航EKF狀態(tài)方程：

其中，F(xiàn)(t)為t時(shí)刻狀態(tài)轉(zhuǎn)移矩陣，為t時(shí)刻系統(tǒng)隨機(jī)噪聲向量，系統(tǒng)狀態(tài)向量如式(35)所示：

其中：

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

觀(guān)測(cè)方程定義如下：

組合導(dǎo)航系統(tǒng)的觀(guān)測(cè)量為機(jī)場(chǎng)坐標(biāo)系慣性導(dǎo)航輸出的垂向側(cè)向位置與視覺(jué)著陸系統(tǒng)導(dǎo)航結(jié)果的差值，如式(38)：

綜上，給出了無(wú)人機(jī)著落末段越過(guò)起始線(xiàn)后，視覺(jué)存在盲區(qū)時(shí)，利用跑道左右邊線(xiàn)和中線(xiàn)三條等距平行線(xiàn)的慣性/視覺(jué)導(dǎo)航方法。

4 試驗(yàn)驗(yàn)證

本試驗(yàn)選用某型號(hào)固定翼無(wú)人機(jī)為試驗(yàn)載機(jī)，試驗(yàn)地點(diǎn)為中國(guó)西北部某民用機(jī)場(chǎng)。試驗(yàn)判定基準(zhǔn)為差分衛(wèi)星后處理獲取的垂向、側(cè)向位置信息。試驗(yàn)結(jié)果如圖4、圖5所示。

圖4 垂向、側(cè)向相對(duì)位移Fig.4 Vertical and lateral relative displacement

圖5 垂向、側(cè)向誤差Fig.5 Vertical and lateral error

圖4為差分衛(wèi)星基準(zhǔn)、著陸視覺(jué)導(dǎo)航、慣性/視覺(jué)組合導(dǎo)航三種導(dǎo)航方式結(jié)果與機(jī)場(chǎng)相對(duì)垂向、側(cè)向位置測(cè)量值曲線(xiàn)，三種導(dǎo)航方式結(jié)果分別對(duì)應(yīng)圖中紅色、藍(lán)色、黑色曲線(xiàn)。圖5為以差分衛(wèi)星為基準(zhǔn)，視覺(jué)導(dǎo)航與慣性/視覺(jué)組合導(dǎo)航兩種導(dǎo)航方式的垂向、側(cè)向誤差曲線(xiàn)。兩幅圖像橫坐標(biāo)均為無(wú)人機(jī)與機(jī)場(chǎng)跑道起始線(xiàn)間的距離，在距離跑道826 m時(shí)開(kāi)始應(yīng)用本文算法。由圖5可知，在本文提出的算法進(jìn)行無(wú)人機(jī)實(shí)時(shí)位姿解算，無(wú)人機(jī)垂向定位精度可達(dá)10 m，側(cè)向定位精度可達(dá)5 m。在圖5中可以看出，在視覺(jué)信息發(fā)生跳變時(shí)，慣性/視覺(jué)融合方法可以有效抑制視覺(jué)跳變，試驗(yàn)結(jié)果驗(yàn)證了本文提出算法的有效性。

5 結(jié) 論

本文針對(duì)固定翼無(wú)人機(jī)著陸場(chǎng)景下傳統(tǒng)點(diǎn)特征提取受光照、跑道磨損等外界因素干擾，影響定位精度的問(wèn)題，提出了一種基于線(xiàn)特征的無(wú)人機(jī)位姿解算方法。引入無(wú)窮遠(yuǎn)元素，通過(guò)跑道內(nèi)三條等距平行線(xiàn)的方程計(jì)算圖像空間內(nèi)無(wú)窮遠(yuǎn)處的消隱點(diǎn)坐標(biāo)及消隱線(xiàn)方程，從而解算出實(shí)時(shí)的無(wú)人機(jī)位姿，并基于EKF算法將視覺(jué)位姿與慣性導(dǎo)航輸出結(jié)果進(jìn)行融合，經(jīng)機(jī)載試驗(yàn)驗(yàn)證，本文提出的算法可以使無(wú)人機(jī)垂向定位精度達(dá)10 m、側(cè)向定位精度達(dá)5 m，解決了無(wú)人機(jī)著陸過(guò)程中提取點(diǎn)特征受外界因素干擾、視覺(jué)信息易發(fā)生跳變、慣性導(dǎo)航誤差隨時(shí)間發(fā)散等問(wèn)題。