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

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

一種基于慣性/視覺信息融合的無人機(jī)自主著陸導(dǎo)航算法

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

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

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

無人機(jī)；自主著陸；視覺/慣性組合導(dǎo)航；位置、姿態(tài)匹配

0 引言

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

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

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

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

1 視覺/慣性組合導(dǎo)航算法概述

無人機(jī)在著陸過程中，需要相對(duì)于著陸目標(biāo)的相對(duì)位置姿態(tài)信息。而慣性導(dǎo)航系統(tǒng)的輸出通常是無人機(jī)的絕對(duì)位置姿態(tài)信息，因此本文建立了世界坐標(biāo)系下慣性導(dǎo)航的模型。在世界坐標(biāo)系下進(jìn)行慣性導(dǎo)航可以避免絕對(duì)導(dǎo)航信息和相對(duì)導(dǎo)航信息之間的互相轉(zhuǎn)換，簡化了導(dǎo)航過程中的計(jì)算。

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

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

2 視覺/慣性組合導(dǎo)航算法

2.1 坐標(biāo)系定義

機(jī)體坐標(biāo)系ObXbYbZb：取常用的右前上坐標(biāo)系。

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

相機(jī)坐標(biāo)系OcXcYcZc：坐標(biāo)系原點(diǎn)Oc取無人機(jī)質(zhì)心，OcXc軸指向相機(jī)正前方，OcYc指向相機(jī)正上方，OcZc指向相機(jī)右側(cè)。

地球坐標(biāo)系OeXeYeZe：原點(diǎn)位于地心，OeXe軸穿越本初子午線與赤道的交點(diǎn)，OeZe軸穿越地球北極點(diǎn)，OeYe軸穿越東經(jīng)90°子午線與赤道的交點(diǎn)。該坐標(biāo)系與地球固連。

地理坐標(biāo)系OnXnYnZn：坐標(biāo)系原點(diǎn)取無人機(jī)所處位置對(duì)應(yīng)地表處，三軸分別指向所在地的北天東方向。

2.2 世界坐標(biāo)系下的慣性導(dǎo)航誤差方程

由于本文采用在世界坐標(biāo)系下進(jìn)行慣性導(dǎo)航解算，因此推導(dǎo)了其誤差方程，為后文組合導(dǎo)航算法奠定基礎(chǔ)：

1)速度誤差方程

(1)

2)位置誤差方程

(2)

3)姿態(tài)誤差方程

(3)

2.3 慣性/光學(xué)測量組合導(dǎo)航數(shù)學(xué)模型

2.3.1 位置匹配

1)系統(tǒng)狀態(tài)方程

(4)

A22=-2A11

2)量測向量和量測矩陣

位置匹配Kalman濾波器的量測量以視覺導(dǎo)航系統(tǒng)為基準(zhǔn)的慣導(dǎo)位置誤差，由此得到位置匹配量測量和量測矩陣的計(jì)算公式為：

Zloc=PI-PC

(5)

(6)

式中，PI表示慣導(dǎo)系統(tǒng)導(dǎo)航解算得到的相對(duì)位置參數(shù)，PC表示視覺導(dǎo)航系統(tǒng)估計(jì)出的相對(duì)位置參數(shù)。

2.3.2 姿態(tài)匹配

本文采用視覺導(dǎo)航系統(tǒng)、慣導(dǎo)相對(duì)于慣性系的姿態(tài)作為量測信息，可有效減小系統(tǒng)狀態(tài)方程的復(fù)雜度，從而使計(jì)算更加快速、準(zhǔn)確。

1)慣導(dǎo)慣性系姿態(tài)解算

設(shè)初始時(shí)刻慣導(dǎo)慣性系姿態(tài)四元數(shù)為[1,0,0,0]，利用陀螺敏感到的相對(duì)慣性空間的角增量，即可計(jì)算慣導(dǎo)慣性姿態(tài)四元數(shù)：

(7)

(8)

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

2)視覺導(dǎo)航系統(tǒng)慣性系姿態(tài)解算

視覺導(dǎo)航系統(tǒng)慣性系姿態(tài)矩陣可由下面公式計(jì)算得到

(9)

(10)

(11)

(12)

3)系統(tǒng)狀態(tài)方程

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

(13)

4)量測向量與量測矩陣

(14)

(15)

(16)

(17)

式中，δα?為3個(gè)慣性失調(diào)角構(gòu)成的反對(duì)稱矩陣。

(18)

(19)

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

(20)

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

即

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

(21)

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

(22)

(23)

3 仿真及分析

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

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

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

圖2 安裝誤差估計(jì)結(jié)果Fig.2 Estimated results of installation error

圖3 陀螺漂移估計(jì)結(jié)果Fig.3 Estimated results of gyroscope drift

圖4 速度誤差估計(jì)結(jié)果Fig.4 Estimated results of velocity error

在上述仿真基礎(chǔ)上，加入視覺導(dǎo)航姿態(tài)噪聲和相對(duì)位置噪聲，其他條件保持不變。

設(shè)加入的3個(gè)相對(duì)位置噪聲為0.1m，仿真得到的速度誤差估計(jì)結(jié)果如圖5所示。

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

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

設(shè)加入的3個(gè)姿態(tài)角噪聲為0.10°。進(jìn)行12次仿真計(jì)算，得到的安裝誤差角和陀螺漂移估計(jì)結(jié)果如圖6及圖7所示。

估計(jì)誤差如表1所示。

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

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

圖7 加入噪聲后陀螺漂移估計(jì)結(jié)果

4 結(jié)論

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

[1] 周樹春.基于GPS的無人機(jī)自動(dòng)著陸控制系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)[D].西安：西北工業(yè)大學(xué)，2007.

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

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

[4] 靳致文.微波著陸系統(tǒng)精度技術(shù)研究[J].現(xiàn)代導(dǎo)航,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] 管敘軍,王新龍. 視覺導(dǎo)航技術(shù)發(fā)展綜述[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] 蔡鳴, 孫秀霞, 徐嵩,等. 視覺技術(shù)輔助的無人機(jī)自主著陸組合導(dǎo)航研究[J].應(yīng)用光學(xué), 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 - )，男，碩士，主要從事導(dǎo)航制導(dǎo)與控制方面的研究。E-mail: 839010787@qq.com

V279

A

2095-8110(2016)06-0006-06