蔡 鳴，孫秀霞，徐 嵩，劉 希，劉 日

（空軍工程大學(xué) 航空航天工程學(xué)院，陜西 西安710038）

引言

自主著陸是影響無人機(jī)能否安全回收從而重復(fù)使用的重要因素之一［1］。在著陸過程中，導(dǎo)航系統(tǒng)發(fā)揮著至關(guān)重要的作用，但目前廣泛使用的慣性導(dǎo)航系統(tǒng)（inertial navigation system，INS）會(huì)產(chǎn)生隨時(shí)間積累的誤差，全球定位系統(tǒng)（global position system，GPS）在戰(zhàn)時(shí)的實(shí)用性受到限制，因此須將不同導(dǎo)航系統(tǒng)有機(jī)結(jié)合構(gòu)成組合導(dǎo)航系統(tǒng)，揚(yáng)長(zhǎng)避短［2］。隨著計(jì)算機(jī)視覺技術(shù)的飛速發(fā)展，其在無人機(jī)領(lǐng)域內(nèi)的應(yīng)用與日俱增，如運(yùn)動(dòng)目標(biāo)檢測(cè)［3］與空中加油的錐套跟蹤［4］，與此同時(shí)，基于視覺（vision）的導(dǎo)航方法逐漸成為該領(lǐng)域內(nèi)的研究熱點(diǎn)。由于視覺導(dǎo)航方法不需要接受外界信號(hào)，以之取代GPS與INS構(gòu)成組合導(dǎo)航系統(tǒng)，可以大大增加導(dǎo)航系統(tǒng)的自主性［5－6］，目前主要應(yīng)用于自主著陸［7］、長(zhǎng)僚機(jī)配合［8］等相對(duì)導(dǎo)航中。

文獻(xiàn)［9］研究了無人直升機(jī)自主著艦中基于視覺圖像處理的運(yùn)動(dòng)狀態(tài)估計(jì)問題，利用色標(biāo)在圖像中的線、點(diǎn)等幾何信息作為求解依據(jù)；文獻(xiàn)［10］設(shè)計(jì)了一種僅依靠視覺傳感器數(shù)據(jù)的無人機(jī)俯仰角與高度估計(jì)方法，并利用自適應(yīng)卡爾曼濾波提高高度參數(shù)的估計(jì)精度。但以上文獻(xiàn)中都用到了人工預(yù)置的輔助著陸圖像，在缺少無人著陸場(chǎng)景信息的情況下適應(yīng)性受到限制。

射影幾何理論中，多視圖幾何作為一個(gè)重要理論被廣泛研究，旨在利用同一特征在多個(gè)投影圖像之間的對(duì)應(yīng)關(guān)系解決視覺問題。在利用圖像處理技術(shù)成功實(shí)現(xiàn)各幅圖像之間特征匹配的前提下，多視圖幾何與輔助特征形狀無關(guān)［11］。本文基于單目攝像機(jī)兩個(gè)時(shí)刻所拍攝地面圖像之間的雙視圖幾何關(guān)系，設(shè)計(jì)了Vision／INS組合導(dǎo)航方法，有效地減少了對(duì)輔助著陸特征的要求，在能夠任意實(shí)現(xiàn)前后時(shí)刻圖像特征匹配的場(chǎng)景中均可適用。

Vision／INS組合導(dǎo)航系統(tǒng)中，常用的濾波算法包括擴(kuò)展卡爾曼濾波（extended kalman filter，EKF）和無跡卡爾曼濾波（unscented kalman filter，UKF），但分別由于近似誤差較大與計(jì)算效率低等原因?qū)е聻V波性能降低甚至失效。作為一種UKF方法的改進(jìn)形式，本文采用平方根無跡卡爾曼濾波（SR－UKF），有效地避免以上缺點(diǎn)，為組合導(dǎo)航提供了更高效、準(zhǔn)確的濾波技術(shù)。

1 Vision／INS信息融合方案

本文所設(shè)計(jì)的Vision／INS信息融合方案如圖1所示。

圖1 Vision／INS組合導(dǎo)航方案Fig.1 Integrated navigation scheme for Vision／INS

2 濾波模型

2.1 坐標(biāo)系選取

在本文中選取東－北－天坐標(biāo)系為導(dǎo)航坐標(biāo)系，機(jī)體坐標(biāo)系為前右下方向。為簡(jiǎn)便解算過程，假設(shè)攝像機(jī)坐標(biāo)原點(diǎn)與機(jī)體坐標(biāo)系原點(diǎn)重合。對(duì)于攝像機(jī)而言，采用針孔模型描述投影關(guān)系，如圖2所示。

圖2 攝像機(jī)針孔模型Fig.2 Pinhole camera model

圖2中Ocxcyczc為攝像機(jī)坐標(biāo)系，Ouxuyu為圖像像素坐標(biāo)系，［u0v0］為光心Oc在圖像平面上的投影，稱為主點(diǎn)，Pn＝［xn，yn，zn，1］T為導(dǎo)航坐標(biāo)系中某點(diǎn)的齊次坐標(biāo)，其圖像平面上的對(duì)應(yīng)投影點(diǎn)的齊次坐標(biāo)為Pu＝［u，v，1］T。兩者之間的射影關(guān)系可以表示為

式中：s為比例因子；Kcam為攝像機(jī)內(nèi)參數(shù)矩陣；［R T］為攝像機(jī)的外參數(shù)矩陣；R和T＝－R?C分別為攝像機(jī)坐標(biāo)系到導(dǎo)航的旋轉(zhuǎn)矩陣和平移向量；?C為光心Oc的非齊次坐標(biāo)。

2.2 過程方程

將INS誤差與慣性元器件漂移作為濾波器的狀態(tài)量，表示為X＝［ΔPosTΔVTΔΨTΔT

εT］，其中ΔP∈R3、ΔV∈R3和ΔΨ∈SO（3）分別表示位置誤差、速度誤差和姿態(tài)誤差，ε∈R3和Δ∈R3分別表示陀螺漂移和加計(jì)零偏。由文獻(xiàn)［12］可知，連續(xù)系統(tǒng)的過程方程為

其中，

式中：［fn×］表示由fn＝fb構(gòu)成的反對(duì)稱矩陣；為機(jī)體系到導(dǎo)航系的方向余弦陣；fb為比力；eV和eΨ表示速度和姿態(tài)角的測(cè)量高斯白噪聲?？紤]離散模型，對(duì)（2）式進(jìn)行一階近似離散化可得：

其中，

2.3 基于雙視圖幾何的量測(cè)方程

選取感興趣特征在圖像坐標(biāo)系上投影點(diǎn)的像素坐標(biāo)作為濾波器的觀測(cè)量，基于雙視圖幾何關(guān)系構(gòu)建量測(cè)方程。

若tk－1時(shí)刻的攝像機(jī)系相對(duì)于導(dǎo)航系的旋轉(zhuǎn)矩陣和平移向量分別為 Rtk－1＝I3×3、Ttk－1＝03×1（即將其視為導(dǎo)航系）、誘導(dǎo)平面π上某一世界特征點(diǎn)Pn在圖像平面上投影點(diǎn)齊次坐標(biāo)，tk時(shí)刻攝像機(jī)系相對(duì)于導(dǎo)航系的旋轉(zhuǎn)矩陣和平移向量分別為Rtk，tk－1和Ttk，tk－1，則tk時(shí)刻圖像平面中對(duì)應(yīng)投影點(diǎn)的齊次坐標(biāo)可以表示為

其中 Htk，tk－1稱為單應(yīng)矩陣，且

式中：n為誘導(dǎo)平面π的單位法向量；d為誘導(dǎo)平面到tk－1時(shí)刻攝像機(jī)坐標(biāo)系原點(diǎn)的距離［13］。

從定義中可以看出，雙視圖幾何中所有量都定義在前一時(shí)刻的攝像機(jī)坐標(biāo)系下，不適用于導(dǎo)航問題。若將（6）式表達(dá)的雙視圖間的單應(yīng)矩陣約束作為量測(cè)方程的基礎(chǔ)，應(yīng)用到Vision／INS組合導(dǎo)航系統(tǒng)中，必須將在導(dǎo)航坐標(biāo)系中描述的參數(shù)轉(zhuǎn)換到tk－1時(shí)刻的攝像機(jī)坐標(biāo)系下，實(shí)行一次坐標(biāo)變換。

若tk－1時(shí)刻某個(gè)時(shí)變參數(shù)在導(dǎo)航系中的非齊次坐標(biāo)為，則通過坐標(biāo)變換將其轉(zhuǎn)換到tk－1時(shí)刻的攝像機(jī)系（用表示）后，有

式中：Rtk－1＝Eulr2DCM（Ψtk－1）為tk－1時(shí)刻導(dǎo)航系到攝像機(jī)系的方向余弦陣（Eulr2DCM表示將歐拉角轉(zhuǎn)換為方向余弦陣的函數(shù)）；為tk－1時(shí)刻攝像機(jī)光心在導(dǎo)航系中的非齊次坐標(biāo)，當(dāng)機(jī)體系與攝像機(jī)系重合時(shí)，有＝Postk－1。

為了使單應(yīng)矩陣適用于組合導(dǎo)航，需要對(duì)以下矢量進(jìn)行代換。

1）在tk－1時(shí)刻攝像機(jī)系中攝像機(jī)的平移向量的

由于平移向量與光心位置的關(guān)系為

其中

則

首先將導(dǎo)航系原點(diǎn)坐標(biāo)On＝［0 0 0］T變換到tk－1時(shí)刻的攝像機(jī)系中，有

由于特征點(diǎn)所在平面為導(dǎo)航系坐標(biāo)系的Onxnyn平面，所以誘導(dǎo)平面的法向量在導(dǎo)航系中的坐標(biāo)為nn＝［0 0 1］T，則其在tk－1時(shí)刻的攝像機(jī)系中的坐標(biāo)為

經(jīng)過上面的坐標(biāo)變換，將（11）式和（13）式帶入（7）式中，單應(yīng)矩陣可改寫為

將其與不同時(shí)刻的導(dǎo)航參數(shù)通過以下形式相聯(lián)系：

所以（7）式最終可表示為

綜合所得，由雙視圖幾何中單應(yīng)矩陣關(guān)系決定的量測(cè)方程為

式中：函數(shù)H（·）表示由（16）式和（17）式構(gòu)成的非線性運(yùn)算；Vk∈R2為圖像量測(cè)的高斯白噪聲。

3 基于SR－UKF的算法實(shí)現(xiàn)

傳統(tǒng)UKF算法中，在利用前一周期的狀態(tài)估計(jì)值與誤差方差矩陣獲取Sigma點(diǎn)集時(shí)，需要對(duì)誤差方差陣進(jìn)行開方，若方差矩陣的階數(shù)過高，采樣過程會(huì)增加計(jì)算機(jī)的計(jì)算負(fù)擔(dān)，降低運(yùn)行效率。另外，由于計(jì)算機(jī)在數(shù)值運(yùn)算過程中存在舍入操作，可能使誤差方程矩陣非正定，導(dǎo)致開方運(yùn)算失敗以致濾波失效。因此，Rudolph［14］提出了SRUKF，在不改變運(yùn)算復(fù)雜度的前提下提高了UKF的運(yùn)算效率，并且避免了誤差方差矩陣出現(xiàn)非正定的情況。

SR－UKF的主要思想是利用誤差方差陣的Cholesky分解矩陣代替其自身參加濾波算法的迭代更新，避免每次Sigma點(diǎn)采樣時(shí)的矩陣開方。在濾波算法的迭代更新過程中，主要用到的3個(gè)數(shù)學(xué)工具分別為矩陣的QR分解、Cholesky因子更新以及有效最小二乘法。應(yīng)用于本文的濾波器模型，具體算法流程如圖3所示。

圖3 組合導(dǎo)航濾波算法流程圖Fig.3 Flow chart of integrated navigation filtering algorithm

1）初始化

令初始時(shí)刻的INS參數(shù)誤差與協(xié)方差矩陣的Cholesky分解因子為

同時(shí)，定義SR－UKF中所用的權(quán)值式中：λ＝α2（n＋k）－n；α決定點(diǎn)集中的采樣點(diǎn)到均值的距離，通常賦予較小正值（10－4≤α≤1）；k≥0用于保證方差陣的半正定性；β≥0用于包含狀態(tài)量分布的高階成分。

2）Sigma點(diǎn)計(jì)算與時(shí)間更新

Sigma點(diǎn)計(jì)算與時(shí)間更新公式為

其中（Sk－1）i為矩陣Sk－1的第i列，即：

式中：矩陣Q表示系統(tǒng)過程噪聲Wk的方差陣；qr（·）返回矩陣QR分解所得R陣中的上三角矩陣；cholupdate（·）表示Cholesky一階更新。因此有：

其中R表示量測(cè)噪聲Vk的方差陣。因此有

3）量測(cè)更新

量測(cè)更新公式為

4 仿真結(jié)果與分析

在Matlab仿真環(huán)境中，首先利用軌跡發(fā)生器生成無人機(jī)著陸運(yùn)動(dòng)軌跡，如圖4所示。

圖4 無人機(jī)著陸軌跡與跑道特征點(diǎn)標(biāo)識(shí)Fig.4 UAV landing trajectory and track feature points identification

無人機(jī)從偏離跑道中心線45°方向進(jìn)入對(duì)準(zhǔn)階段，其初始位置為Pos（t0）＝［－51.65 505 42.63］Tm，初始速度為V（t0）＝［7 －7 0］Tm／s，經(jīng)過9s后對(duì)準(zhǔn)跑道中心線，并繼續(xù)勻速飛行5s準(zhǔn)備進(jìn)入下滑階段。圖4中起點(diǎn)A與終點(diǎn)B之間的軌跡為無人機(jī)的著陸軌跡，地面兩條平行線為跑道邊緣，周圍對(duì)稱分布9×11個(gè)地面特征點(diǎn)?；谥戃壽E，可以計(jì)算理想的導(dǎo)航參數(shù)變化曲線，如圖5所示。

圖5 無人機(jī)著陸理想導(dǎo)航參數(shù)Fig.5 UAV landing ideal navigation parameters

將加計(jì)零偏和陀螺漂移視為常值偏差，其設(shè)定如表1所示。

表1 慣性測(cè)量單元常值漂移Table 1 Constant drift of inertial measurement unit

將加計(jì)零偏、陀螺漂移和圖4所示的理想加速度與角速度（歐拉角的微分）相加，從而模擬加速度計(jì)與陀螺在實(shí)際工作中的輸出值利用捷聯(lián)慣導(dǎo)算法解算得到INS位置、速度、姿態(tài)角誤差，分別如圖5（a）、5（b）、5（c）中的虛線所示。

在Vision／INS組合導(dǎo)航仿真實(shí)驗(yàn)中，設(shè)定濾波器狀態(tài)初始值為

其中，

初始狀態(tài)的協(xié)方差陣為

令W 與V均為高斯白噪聲，其方差陣分別為

選擇INS解算周期為0.1s，SR－UKF的濾波周期為1s，即INS每解算10次進(jìn)行1次校正；進(jìn)行100次蒙特卡洛仿真后，得到組合導(dǎo)航的參數(shù)誤差曲線如圖6中實(shí)線所示。

圖6 組合導(dǎo)航與慣導(dǎo)誤差對(duì)比Fig.6 Navigation and inertial navigation error comparison

從誤差曲線對(duì)比圖中可以看出：

1）本文設(shè)計(jì)的基于雙視圖幾何的Vision／INS組合導(dǎo)航系統(tǒng)輸出的經(jīng)過校正的位置和速度參數(shù)的誤差明顯減小，精度有了很大程度的提高，其誤差最大值與在同一時(shí)刻INS誤差的比較列寫于表2。

表2 位置與速度誤差比較Table 2 Position and velocity error comparison

2）導(dǎo)航參數(shù)誤差保持在一定的平穩(wěn)水平，體現(xiàn)了本文所設(shè)計(jì)的INS／Vision組合導(dǎo)航可以有效地抑制INS誤差隨時(shí)間積累導(dǎo)致的發(fā)散。

3）對(duì)于姿態(tài)角而言，滾轉(zhuǎn)角與俯仰角誤差精度提高較為明顯，而偏航角誤差與INS基本保持在同一水平。

將理想著陸軌跡、INS單獨(dú)工作時(shí)的導(dǎo)航軌跡、本文的組合導(dǎo)航軌跡比較列于圖7中。從圖7可以看出，與INS相比，組合導(dǎo)航獲得的軌跡與真實(shí)軌跡十分接近。

最后，為了驗(yàn)證本文中所使用的雙視圖幾何約束，選取第34s這一時(shí)刻，將利用特征點(diǎn)世界坐標(biāo)與理想軌跡計(jì)算所得的理想圖像點(diǎn)，以及利用第33s的圖像點(diǎn)與雙視圖幾何濾波器所估計(jì)的觀測(cè)圖像點(diǎn)在圖8中進(jìn)行了比較。從圖8中可以看出，利用雙視圖幾何濾波器計(jì)算得到的圖像點(diǎn)與真實(shí)圖像點(diǎn)相差不多，體現(xiàn)了算法的正確性。

圖7 3種著陸軌跡的對(duì)比Fig.7 Contrast of three landing trajectories

圖8 真實(shí)圖像點(diǎn)與雙視圖幾何濾波器估計(jì)圖像點(diǎn)對(duì)比Fig.8 Contrast of true image points and two－view geometry filter estimated image points

5 結(jié)論

本文以無人機(jī)自主著陸為工程背景，設(shè)計(jì)了一種Vision／INS組合導(dǎo)航方案。針對(duì)地面特征須人工預(yù)置的條件約束，基于雙視圖幾何設(shè)計(jì)了濾波器，并利用SR－UKF估計(jì)INS誤差，有效地實(shí)現(xiàn)視覺信息與慣性信息的融合。與INS相比，組合導(dǎo)航系統(tǒng)的參數(shù)精度顯著提高，且具有良好的自主性。

［1］ Zhang Jianhong，Zhang Ping.Autonomous precise landing control law for UAV［J］.Journal of System Simulation，2009，21（3）：743－748.張建宏，張平.無人機(jī)自主精確著陸控制律設(shè)計(jì)及仿真研究［J］.系統(tǒng)仿真學(xué)報(bào)，2009，21（3）：743－748.

［2］ Liu Jianye，Zeng Qinghua，Zhao Wei，et al.Navigation system theory and application［M］.Xi'an：Northwestern Polytechnical University Press，2010.劉建業(yè)，曾慶化，趙偉，等.導(dǎo)航系統(tǒng)理論與應(yīng)用［M］.西安：西北工業(yè)大學(xué)出版社，2010.

［3］ Dong Jing，F(xiàn)u Dan，Yang Xia.Real－time moving object detection and tracking by using UAV videos［J］.Journal of Applied Optics，2013，34（2）：255－259.董晶，傅丹，楊夏.無人機(jī)視頻運(yùn)動(dòng)目標(biāo)實(shí)時(shí)檢測(cè)及跟蹤［J］.應(yīng)用光學(xué)，2013，34（2）：255－259.

［4］ Wang Xufeng，Dong Xinmin，Kong Xingwei，et al.MS－KF fusion algorithm for drogue tracking［J］.Journal of Applied Optics，2013，34（6）：951－956.王旭峰，董新民，孔星煒，等.MS－KF融合算法用于錐套跟蹤［J］.應(yīng)用光學(xué)，2013，34（6）：951－956.

［5］ Roumeliotis S，Johnson A，Montgomery J.Augmenting inertial navigation with image－based motion estimation［J］.IEEE International Conference on Robotics and Automation，2002，4：4326－4333.

［6］ Li Jian，Li Xiaomin，Qian Kechang，et al.Motion state estimation for micro UAV using inertial sensor and stereo camera pairs［J］.Acta Aeronautica et Astronautica Sinica，2011，32（12）：2310－2317.李建，李小民，錢克昌，等.基于雙目視覺和慣性器件的微小型無人機(jī)運(yùn)動(dòng)狀態(tài)估計(jì)方法［J］.航空學(xué)報(bào)，2011，32（12）：2310－2317.

［7］ Gao Yangjun，Sun Xiuxia，Liu Yukun，et al.A hybrid iterative quasi－sliding mode control strategy for autolanding of UAV［J］.Flight Dynamics，2013，31（6）：521－525.高楊軍，孫秀霞，劉宇坤，等.無人機(jī)自主著陸的雙環(huán)混合迭代滑?？刂疲跩］.飛行力學(xué)，2013，31（6）：521－525.

［8］ Wang Xiaogang，Guo Jifeng，Cui Naigang.A relative navigation method based on INS／Vision for UAV［J］.Journal of Harbin Institute of Technology，2010，42（7）：1029－1032.王小剛，郭繼峰，崔乃剛.INS／Vision相對(duì)導(dǎo)航系統(tǒng)在無人機(jī)上的應(yīng)用［J］.哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2010，42（7）：1029－1032.

［9］ Qiu Liwei，Song Zishan，Shen Weiqun.Computer vision scheme used for the automate landing of unmanned helicopter on ship deck［J］.Acta Aeronautica et Astronautica Sinica，2003，24（4）：331－335.邱力為，宋子善，沈?yàn)槿?用于無人直升機(jī)著艦控制的計(jì)算機(jī)視覺技術(shù)研究［J］.航空學(xué)報(bào)，2003，24（4）：331－335.

［10］Pan Xiang，Ma Deqiang，Wu Yijun，et al.Estimating pitch attitude and altitude of unmanned aerial vehicle vision－based landing［J］.Journal of Zhejiang U－niversity：Engineering Science，2009，43（4）：1029－1032.潘翔，馬德強(qiáng)，吳貽軍，等.基于視覺著陸的無人機(jī)俯仰角與高度估計(jì)［J］.浙江大學(xué)學(xué)報(bào)：工學(xué)版，2009，43（4）：1029－1032.

［11］Hartley R.Multiple view geometry in computer vision［M］.London：Cambridge University Press，2003.

［12］Indelman V，Gurfil P，Rivlin E.Real－time mosaic－aided aerial navigation：II.sensor fusion［C］／／AIAA modeling and simulation technologies conference，2009.Reston，VA：AIAA，2009.

［13］Wu Fuchao.Mathematical technique of computer vision［M］.Beijing：Science Press，2008：89－91.吳福朝.計(jì)算機(jī)視覺中的數(shù)學(xué)方法［M］.北京：科學(xué)出版社，2008：88－91.

［14］Der Merwe R A，Wan E A.The square－root unscented kalman filter for state and parameter estimation［C］／／Proceedings of IEEE international conference on acoustics，speech and signal，Salt Lake City，Utah，USA，2001.USA：IEEE，2001：3461－3464.