一種基于慣性/視覺信息融合的無人機自主著陸導航算法

劉 暢，楊良軍,劉 峰，王 翌，徐 策，劉崇亮

(1.北京自動化控制設備研究所，北京 100074;2. 海軍駐昆明地區軍事代表處，昆明 650000)

一種基于慣性/視覺信息融合的無人機自主著陸導航算法

劉 暢1，楊良軍2,劉 峰1，王 翌1，徐 策1，劉崇亮1

(1.北京自動化控制設備研究所，北京 100074;2. 海軍駐昆明地區軍事代表處，昆明 650000)

無人機自主著陸過程中需要實時獲得高精度的導航信息，對自主性、實時性的要求較高?，F有的導航方式都存在各自的不足，且在室內等新型環境中不能使用。針對這一問題，提出了一種視覺/慣性組合導航算法。首先建立了世界坐標系下慣性導航的數學模型，隨后通過Kalman濾波實現位置、姿態匹配，其中位置匹配完成速度誤差、加表零偏的估計；姿態匹配完成安裝誤差角、陀螺漂移的估計，并利用估計得到的安裝誤差角和視覺導航系統輸出的姿態信息對慣導姿態進行修正。仿真結果表明，該算法具有一定的工程應用價值。

無人機；自主著陸；視覺/慣性組合導航；位置、姿態匹配

0 引言

無人機(Unmanned Aerial Vehicle,UAV)是一種無人駕駛、可重復使用的航空器的簡稱[1]，因其具有成本低、機動性能好、使用方便、無人員傷亡風險等眾多優點，目前發展迅速，已經獲得了廣泛應用。自主飛行、執行任務是無人機新的發展方向，而自主著陸是其中的一個重要部分。

現有的著陸導航方式主要包括儀表著陸系統(ILS)[2]、微波著陸系統(MLS)[3-4]、DGPS/捷聯慣導組合導航系統[5]，它們都有各自的不足：ILS軌道單一，下滑道固定，波束覆蓋區很小，不能為飛機提供正常曲線進場和分段進場的信息，而且易受到在相近頻率工作的無線電臺、機場附近高層建筑甚至周圍農作物的干擾，導致航向信標的失真；MLS 在性能上雖優于 ILS，但其設備昂貴，不利于推廣，且這些設備機動性能較差，臨時架設困難，不利于緊急行動；DGPS/捷聯慣導組合導航系統存在衛星信號容易受干擾、室內無法定位等問題。此外，這三種導航系統都依賴外界信息的輸入，容易受到干擾。因此研究出一種新的完全自主的、可靠的導航方式成為了解決無人機自主著陸問題的關鍵。

計算機視覺是用計算機實現人的視覺功能——對客觀世界的三維場景的感知、識別和理解[6]。視覺導航系統[7-8]具有眾多優點，一方面其結構簡單、功耗低、信息量大，提升了無人機導航過程中的抗干擾能力和自主化程度；另一方面視覺導航精度較高，姿態精度可達角分級，定位可達到優于D‰的精度(D為無人機與著陸目標的相對距離)。因此利用計算機視覺信息來輔助完成或獨立完成無人機的自主著陸任務已成為新的發展趨勢。然而一些情況也會導致視覺導航系統無法正常工作，如當無人直升機降至一定高度時，攝像頭視野范圍內可能只包含一部分著陸目標；或著陸目標被建筑物等障礙物遮擋等情況。此外，視覺導航系統工作時需要對獲取的圖像進行處理，花費較長時間。故視覺導航系統存在可靠性、實時性等問題。慣性導航技術是唯一同時具有自主、實時、連續、隱蔽、不受干擾，無時間、地點、環境限制的運動信息感知技術，但存在誤差隨時間累積增大的問題。

本文針對無人機著陸導航問題，結合視覺導航和慣性導航的優缺點，對視覺/慣性組合導航算法[9-11]進行了研究，首先給出了慣性導航在世界坐標系下的誤差模型；之后采用姿態、位置雙匹配通過Kalman濾波對慣性導航產生的誤差進行估計、修正;最后通過仿真驗證了算法的有效性。利用視覺導航來輔助修正慣性導航，可以在保證導航精度的同時，滿足系統對可靠性、實時性的要求。

1 視覺/慣性組合導航算法概述

無人機在著陸過程中，需要相對于著陸目標的相對位置姿態信息。而慣性導航系統的輸出通常是無人機的絕對位置姿態信息，因此本文建立了世界坐標系下慣性導航的模型。在世界坐標系下進行慣性導航可以避免絕對導航信息和相對導航信息之間的互相轉換，簡化了導航過程中的計算。

視覺導航系統在著陸過程中，可以鎖定著陸目標，通過對拍攝的包含著陸目標的圖片進行特征提取處理，即可對無人機的相對位姿信息進行解算。本文采用Kalman濾波分別完成位置匹配和姿態匹配，其中位置濾波器將慣導導航解算得到的相對位置與視覺導航估計出的相對位置進行Kalman濾波計算，估計誤差速度、水平姿態失調角、加速度零偏；姿態濾波器將慣導慣性系姿態陣和視覺導航系統慣性系姿態陣之間的誤差量作為量測量進行濾波，估計視覺導航系統和慣導之間的安裝誤差、陀螺漂移，并利用估計出的安裝誤差和視覺導航系統輸出的姿態計算慣導系統的航向角。視覺/慣性組合導航算法的主要流程如圖1所示。

圖1 慣性/視覺組合導航算法原理圖Fig.1 Scheme of INS/Vision integrated navigation algorithm

2 視覺/慣性組合導航算法

2.1 坐標系定義

機體坐標系ObXbYbZb：取常用的右前上坐標系。

世界坐標系OwXwYwZw：由于攝像機坐標系并不能給出空間點的具體位置信息，且攝像機在空間中可以安放在任何位置，所以需要一個基準坐標系來描述空間點和攝像機的位置，一般稱這個基準坐標系為世界坐標系。本文中世界坐標系與著陸目標固連，三軸分別指向著陸目標的前上右方向。

相機坐標系OcXcYcZc：坐標系原點Oc取無人機質心，OcXc軸指向相機正前方，OcYc指向相機正上方，OcZc指向相機右側。

地球坐標系OeXeYeZe：原點位于地心，OeXe軸穿越本初子午線與赤道的交點，OeZe軸穿越地球北極點，OeYe軸穿越東經90°子午線與赤道的交點。該坐標系與地球固連。

地理坐標系OnXnYnZn：坐標系原點取無人機所處位置對應地表處，三軸分別指向所在地的北天東方向。

2.2 世界坐標系下的慣性導航誤差方程

由于本文采用在世界坐標系下進行慣性導航解算，因此推導了其誤差方程，為后文組合導航算法奠定基礎：

1)速度誤差方程

(1)

2)位置誤差方程

(2)

3)姿態誤差方程

(3)

2.3 慣性/光學測量組合導航數學模型

2.3.1 位置匹配

1)系統狀態方程

(4)

A22=-2A11

2)量測向量和量測矩陣

位置匹配Kalman濾波器的量測量以視覺導航系統為基準的慣導位置誤差，由此得到位置匹配量測量和量測矩陣的計算公式為：

Zloc=PI-PC

(5)

(6)

式中，PI表示慣導系統導航解算得到的相對位置參數，PC表示視覺導航系統估計出的相對位置參數。

2.3.2 姿態匹配

本文采用視覺導航系統、慣導相對于慣性系的姿態作為量測信息，可有效減小系統狀態方程的復雜度，從而使計算更加快速、準確。

1)慣導慣性系姿態解算

設初始時刻慣導慣性系姿態四元數為[1,0,0,0]，利用陀螺敏感到的相對慣性空間的角增量，即可計算慣導慣性姿態四元數：

(7)

(8)

式中，Δφx、Δφy、Δφz為陀螺輸出的角度增量。

2)視覺導航系統慣性系姿態解算

視覺導航系統慣性系姿態矩陣可由下面公式計算得到

(9)

(10)

(11)

(12)

3)系統狀態方程

姿態匹配的系統狀態向量為Xatt=[δa,δθ,ε]，其中，δa為3個子慣導慣性系姿態誤差角δαx、δαy、δαz，δθ為3個安裝角誤差δθx、δθy、δθz，ε為3個陀螺漂移εx、εy、εz，則系統狀態方程可寫為

(13)

4)量測向量與量測矩陣

(14)

(15)

(16)

(17)

式中，δα?為3個慣性失調角構成的反對稱矩陣。

(18)

(19)

將式(19)展開并忽略二次項得

(20)

A+δα?A+Bδθ?C-Aδθ?=I

即

A=I+Aδθ?-Bδθ?C-δα?A

(21)

取矩陣的上三角元素得作為觀測量則可得到姿態匹配的量測向量和量測矩陣：

(22)

(23)

3 仿真及分析

假設無人機機體坐標系和世界坐標系重合，速度為0。視覺導航系統和慣導的三軸安裝誤差分別為dqx=0.6°、dqy=0.3°、dqz=-1°；三軸陀螺常值漂移分別為ex=2(°)/h、ey=3(°)/h、ez=5(°)/h；無人機和著陸目標的相對位置分別為1m、3m、1m；暫不考慮視覺導航系統輸出的相對姿態的誤差。

在對陀螺漂移等參數進行估計時，需要無人機進行一定的機動將參數分離出來。假定無人機的運動順序如下：1) 靜止10s；2) 以2(°)/s的速度滾轉5s；3) 以-2(°)/s的速度轉回原位；4) 保持靜止；5) 從50s開始以5(°)/s的速度轉動航向4s；6) 以-5(°)/s的速度將航向轉回原位；7) 保持靜止。利用生成的軌跡進行仿真得到安裝誤差、陀螺漂移及速度誤差估計情況如圖2～圖4所示。

由圖2～圖4可以看出，視覺導航系統和慣導之間的安裝誤差、陀螺漂移以及慣導速度誤差都得到了很好的估計。在對安裝誤差的估計中，無人機姿態變化帶來量測矩陣的變化，從而使得無人機旋轉所繞軸之外的2個安裝誤差角可觀而迅速收斂。從圖中還可以看出：無人機滾轉后繞Y軸、Z軸的2個安裝誤差角迅速收斂；而在無人機航向角變化后，繞X軸的安裝誤差也得到了較為準確的估計。

圖2 安裝誤差估計結果Fig.2 Estimated results of installation error

圖3 陀螺漂移估計結果Fig.3 Estimated results of gyroscope drift

圖4 速度誤差估計結果Fig.4 Estimated results of velocity error

在上述仿真基礎上，加入視覺導航姿態噪聲和相對位置噪聲，其他條件保持不變。

設加入的3個相對位置噪聲為0.1m，仿真得到的速度誤差估計結果如圖5所示。

圖5 加入噪聲后速度誤差估計結果Fig.5 Estimated results of velocity error with noise

由圖5可以看出，相對位置噪聲對速度誤差估計結果的影響較小。

設加入的3個姿態角噪聲為0.10°。進行12次仿真計算，得到的安裝誤差角和陀螺漂移估計結果如圖6及圖7所示。

估計誤差如表1所示。

從圖6、圖7和表1可以看出，加入視覺導航姿態噪聲以后，對安裝誤差以及陀螺漂移的估計帶來了一定的影響：繞x軸安裝誤差估計誤差為0.040°(1σ)，繞y軸安裝誤差估計誤差為0.036°(1σ)，繞z軸安裝誤差估計誤差為0031°(1σ)；x陀螺漂移估計誤差為0.063°(1σ)，y陀螺漂移估計誤差為0.084°(1σ)，z陀螺漂移估計誤差為0.077°(1σ)。

圖6 加入噪聲后安裝誤差估計結果Fig.6 Estimated results of installation error with noise

圖7 加入噪聲后陀螺漂移估計結果

4 結論

本文針對無人機著陸過程對自主性、可靠性以及導航精度的要求，設計了一種視覺/慣性組合導航算法。首先給出了慣性導航在世界坐標系下的誤差模型，隨后采用Kalman濾波分別完成位置和姿態匹配，利用精度較高的視覺導航系統對慣導進行了修正，抑制了慣性導航誤差隨著時間的發散。最后通過仿真驗證了算法的可行性，對無人機著陸導航具有一定的工程應用價值。

[1] 周樹春.基于GPS的無人機自動著陸控制系統設計與實現[D].西安：西北工業大學，2007.

[2] 佚名. 儀表著陸系統[J].航空世界, 2013(4):80.

[3] 張強. 著陸系統發展趨勢研究[J].黑龍江科技信息, 2015(27):87-88.

[4] 靳致文.微波著陸系統精度技術研究[J].現代導航,2010,10(5):1-5.

[5] Berdjag D, Pomorski D.DGPS/INS data fusion for land navigation [J].Journal of Donghua University,2004,21(3):88-93．

[6] 管敘軍,王新龍. 視覺導航技術發展綜述[J].航空兵器,2014(5):3-8.

[7] Fraundorfer F, Scaramuzza D. Visual odometry:Part Ⅱ: Matching,robustness, optimization, and applications[J].IEEE Robotics &Automation Magazine,2012, 19(2):78-90.

[8] Conte G, Doherty P. A visual navigation system for UAS based on geo-referenced imagery[C]// Conference on Unmanned Aerial Vehicle in Geomatics, Zurich,Switzerland, 2011.

[9] Kelly J, Sukhatme G S. Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration [J].The International Journal of Robotics Research,2011, 30(1): 56-79.

[10] Mirzaei F M, Roumeliotis S I. A Kalman filter-based algorithm for IMU-camera calibration: Observability analysis and performance evaluation [J].IEEE Transactions on Robotics, 2008, 24(5): 1143-1156.

[11] 蔡鳴, 孫秀霞, 徐嵩,等. 視覺技術輔助的無人機自主著陸組合導航研究[J].應用光學, 2015, 36(3):343-350.

Navigation Algorithm Based on Inertial/Vision Information Fusion of UAV Autonomous Landing

LIU Chang1, YANG Liang-jun2, LIU Feng1, WANG Yi1, XU Ce1, LIU Chong-liang1

(1. Beijing Institute of Automatic Control Equipment, Beijing 100074, China;2. Military Representative Office of Navy in Kunming Area, Kunming 650000, China)

High-precision navigation information which needs to be real-time and autonomic is essential during UAV autonomous landing. The existing navigation modes have their respective weaknesses and can’t be used indoors. An INSVision integrated navigation algorithm is raised to solve this problem.Mathematical model of inertial navigation in the world coordinate system is built firstly, then position matching and attitude matching by Kalman filtering are presented. Velocity error and accelerometer zero offset are estimated by position matching, installation error angle and gyroscope drift are estimated by attitude matching. The attitude information provided by INS can be corrected by the installation error angle and the attitude information provided by vision navigation system. The simulation results show that this algorithm is feasible.

UAV; Autonomous landing; INS/Vision integrated navigation; Position/Attitude matching

10.19306/j.cnki.2095-8110.2016.06.002

2015-12-11；

2016-03-07。

劉暢(1991 - )，男，碩士，主要從事導航制導與控制方面的研究。E-mail: 839010787@qq.com

V279

A

2095-8110(2016)06-0006-06