基于視覺/慣導(dǎo)的無人機(jī)組合導(dǎo)航算法研究

黃劍雄，劉小雄，章衛(wèi)國，高鵬程

(西北工業(yè)大學(xué) 自動化學(xué)院, 西安 710072)

0 引言

傳統(tǒng)的無人機(jī)通常采用全球定位系統(tǒng)(GPS,global position system)與慣性測量單元(IMU,inertial measurement unit)進(jìn)行融合[1]，為無人機(jī)提供所需的姿態(tài)、速度以及位置等導(dǎo)航信息。

當(dāng)GPS受遮擋或完全無信號時(shí)，這將直接影響到使用GPS與IMU的傳統(tǒng)組合導(dǎo)航算法，嚴(yán)重的情況下，甚至?xí)蛊渫耆?，從而引發(fā)無人機(jī)墜機(jī)的事故。

針對上述問題，許多學(xué)者將研究重點(diǎn)轉(zhuǎn)向了同時(shí)定位與地圖構(gòu)建(SLAM，simultaneous localization and mapping)的研究，并提出了許多優(yōu)秀的開源算法。其中利用視覺傳感器的SLAM算法，主要分為基于特征點(diǎn)法的SLAM算法，如PTAM[2]、ORB_SLAM[3]，與基于直接法或半直接法的SLAM算法，如DSO[4]、SVO[5]等算法。其中，ORB_SLAM是基于特征點(diǎn)法的SLAM算法中性能非常優(yōu)越的算法，并且于2017年推出了同時(shí)支持單目、雙目、RGBD的ORB_SLAM2[6]。

但由于無人機(jī)的機(jī)載處理器性能較弱，如在NVIDIA JETSON TX2平臺上，ORB_SLAM2經(jīng)過GPU加速等優(yōu)化后，通常也只能輸出10～20 Hz的位姿信息。這對于無人機(jī)的導(dǎo)航而言，確實(shí)有些捉襟見肘。因此，利用誤差狀態(tài)卡爾曼濾波(ErKF，error-state kalmanfilter)松耦合方法[7]對視覺位姿與慣導(dǎo)信息進(jìn)行融合從而提升導(dǎo)航信息的輸出頻率是非常有效的手段之一。

在傳統(tǒng)的使用擴(kuò)展卡爾曼濾波(EKF,extended kalmanfilter)對GPS信息與慣性信息進(jìn)行融合的流程中，建模的準(zhǔn)確與否直接影響到EKF的精確性。為了解決系統(tǒng)建模不準(zhǔn)確的問題，自適應(yīng)漸消卡爾曼濾波(AFKF,adaptive fading kalman filter)應(yīng)運(yùn)而生。AFKF主要通過實(shí)時(shí)的計(jì)算遺忘因子，不斷調(diào)整系統(tǒng)的協(xié)方差矩陣，從而削弱歷史數(shù)據(jù)的權(quán)重，即限制了模型誤差累積。國內(nèi)外學(xué)者在傳統(tǒng)AFKF方面做了許多研究工作，其中，AFKF直接對協(xié)方差陣進(jìn)行加權(quán)漸消處理的合理性已得到了完備的證明[8]；學(xué)者也提出了非常合理的最佳遺忘因子的求取算法[9]。

基于上述文獻(xiàn)的研究成果，本文提出了使用AFKF作為視覺位姿與慣導(dǎo)信息的融合算法，進(jìn)一步提升濾波結(jié)果的精度。在此基礎(chǔ)上，本文還討論了一種由于視覺SLAM解算過程而引入的視覺位姿信息相對慣性信息滯后的問題，并提出了一種延時(shí)補(bǔ)償方法以解決這類滯后問題，進(jìn)一步改善導(dǎo)航解算精度。

1 ORB_SLAM2簡介及其簡化工作

ORB_SLAM2是當(dāng)前基于特征點(diǎn)法的主流SLAM算法之一，前端是一個(gè)輕量級的定位模型，后端主要采用BA優(yōu)化方法，可以實(shí)現(xiàn)未知環(huán)境中的定位。ORB_SLAM2在原ORB_SLAM的基礎(chǔ)上，新增了對雙目以及RGB-D攝像頭的支持，并可以實(shí)現(xiàn)地圖重用、回環(huán)檢測以及重定位等功能。

ORB_SLAM2一共包含3個(gè)主要線程以及一個(gè)次要線程，分別是Tracking線程、Local Mapping線程、Loop Closure線程及全局BA線程，其中全局BA線程僅發(fā)生在回環(huán)檢測成功后才執(zhí)行。

本文為了保證實(shí)時(shí)性要求，對ORB_SLAM2框架進(jìn)行了一定程度的簡化，簡化包括去除閉環(huán)檢測以及回環(huán)修正的線程；同時(shí)，為了與慣導(dǎo)進(jìn)行融合，增加了Scheduler線程，Scheduler中主要完成IMU數(shù)據(jù)的采集以及基于EKF框架的視覺慣導(dǎo)融合。如圖1所示。

圖1 簡化ORB_SLAM2流程框圖

本文暫且把修改后的框架稱為簡化ORB_SLAM2，簡化ORB_SLAM2共包含3個(gè)線程，分別是Tracking線程、Local Mapping線程、Scheduler線程。

在Scheduler線程中，本文將ORB_SLAM2與IMU視為相互獨(dú)立的傳感器，按設(shè)定頻率進(jìn)行數(shù)據(jù)采集與處理，并基于EKF框架進(jìn)行融合，得到融合后的無人機(jī)位置、速度以及姿態(tài)信息，作為無人機(jī)的導(dǎo)航信息。

2 基于EKF的視覺慣導(dǎo)組合導(dǎo)航算法

2.1 EKF預(yù)測更新

假設(shè)系統(tǒng)狀態(tài)為[q,v,p,εr,▽r]T，其中q表示無人機(jī)姿態(tài)，v表示無人機(jī)速度，p表示無人機(jī)位置，εr表示角速度bias，▽r表示加速度bias。

將陀螺儀誤差與加速度計(jì)誤差建模為一階馬爾科夫隨機(jī)過程，可以將陀螺儀誤差模型建模如下：

(1)

加計(jì)誤差模型建模如下：

(2)

根據(jù)運(yùn)動學(xué)關(guān)系，式(1)以及式(2)，可得如下運(yùn)動學(xué)狀態(tài)方程：

(3)

其中：

(4)

其中：ωm表示陀螺儀量測值，am表示加速度計(jì)量測值。

上述運(yùn)動狀態(tài)方程中，含有噪聲項(xiàng)，這是我們無法在實(shí)際測量中直接去除的，因此我們需要將這些噪聲項(xiàng)通過EKF估計(jì)出來之后再進(jìn)行消除。我們首先將上述運(yùn)動方程中的噪聲項(xiàng)去除，然后對噪聲項(xiàng)進(jìn)行建模，得到我們最終的誤差狀態(tài)方程：

(5)

由式(1)和式(2)描述的IMU誤差模型可知，如果我們將噪聲項(xiàng)消除，則IMU誤差應(yīng)該恒為0，則其倒數(shù)也為0，因此可以得到如上消除噪聲項(xiàng)后的運(yùn)動方程，我們對每一個(gè)狀態(tài)量求取上述兩組運(yùn)動方程的誤差，則可以得到我們最終的狀態(tài)量為[δθ,δv,δp,εr,▽r]T的誤差狀態(tài)方程如下：

(6)

對各狀態(tài)量求偏導(dǎo)，可得如下預(yù)測模型的狀態(tài)空間方程：

(7)

其具體表達(dá)形式為：

(8)

因?yàn)闄C(jī)載處理器只能以離散化的計(jì)算形式對導(dǎo)航數(shù)據(jù)進(jìn)行處理，因此需要將式(7)進(jìn)行離散化處理。具體離散化流程可參考文獻(xiàn)[10],本文采用泰勒展開的方式，并且僅取二階項(xiàng)，如式(9)所示：

(9)

其中：Φk+1,k=Φ(tk+1,tk)，F(xiàn)k=F(tk)，T=tk+1-tk。根據(jù)系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣Φ以及噪聲矩陣G，可以求得系統(tǒng)的噪聲協(xié)方差矩陣Qk的離散化形式，如式(10)：

(10)

其中：Qc為系統(tǒng)噪聲的協(xié)方差矩陣，利用式(9)，式(10)求得的狀態(tài)轉(zhuǎn)移矩陣與離散過程噪聲協(xié)方差矩陣，則可計(jì)算k+1時(shí)刻的狀態(tài)協(xié)方差矩陣：

(11)

以及完成狀態(tài)量的預(yù)測過程：

(12)

2.2 EKF量測更新

量測更新采用視覺SLAM解算的位姿信息，由于視覺輸出位姿信息是離散形式的，因此本文的量測方程可以直接離散化形式表示為：

Zk+1=Hk+1Xk+1+Vk+1

(13)

其中：

(14)

其中：[Xn,Yn,Zn]T為轉(zhuǎn)換到本文所定義的導(dǎo)航坐標(biāo)系下的軌跡對應(yīng)的三維坐標(biāo)值。

根據(jù)視覺SLAM的特性，量測噪聲Vk+1可以表示為：

(15)

其中：np∈R3×1為位置量測值所含白噪聲，nθ∈R3×1為姿態(tài)量測值所含白噪聲。

不難知道：

(16)

則有：

(17)

(18)

同理可得：

(19)

根據(jù)上述量測更新模型，可以計(jì)算k+1時(shí)刻的增益矩陣：

(20)

進(jìn)一步可以更新k+1時(shí)刻的狀態(tài)協(xié)方差矩陣：

Pk+1=(I-Kk+1Hk+1)Pk+1,k(I-Kk+1Hk+1)T+Kk+1RKk+1

(21)

并完成對系統(tǒng)狀態(tài)量的更新：

(22)

3 自適應(yīng)漸消卡爾曼濾波

由于在建立EKF的系統(tǒng)模型的過程中，通常會對其在合理的范圍內(nèi)進(jìn)行近似或簡化，以提高解算效率，但這些簡化把不可避免的會使EKF的估計(jì)誤差不斷增大，甚至嚴(yán)重影響濾波效果。因此，本文采用了自適應(yīng)卡爾曼濾波的方式來進(jìn)一步提高視覺慣導(dǎo)融合的精度。

3.1 基本原理

為了減小在建模時(shí)由于模型近似或簡化帶來的估計(jì)誤差，可以在時(shí)間更新中求取當(dāng)前時(shí)刻狀態(tài)預(yù)測協(xié)方差矩陣時(shí)，乘上相應(yīng)的遺忘因子[12]，即：

(23)

其余更新流程與式(12)，式(20)～(22)一致，通過分析可知，當(dāng)估計(jì)誤差增大時(shí)，會導(dǎo)致遺忘因子λk+1增大，從而狀態(tài)預(yù)測協(xié)方差矩陣Pk+1,k也會相應(yīng)增大，Pk+1,k增大會使增益Kk+1增大，因此在計(jì)算經(jīng)過量測更新得到的當(dāng)前狀態(tài)預(yù)測值時(shí)，會更多的信任量測信息，從而抑制歷史狀態(tài)信息帶來的影響，即可減小由于建模時(shí)模型近似或簡化帶來的估計(jì)誤差。從λk+1的物理意義可知，通常有：

λk+1≥1

(24)

3.2 遺忘因子的求取方式

由于通過求取最優(yōu)的遺忘因子，應(yīng)當(dāng)使自適應(yīng)漸消卡爾曼濾波成為最優(yōu)濾波器，因此遺忘因子在求取過程中，首先應(yīng)該滿足最優(yōu)濾波器的性質(zhì)。由文獻(xiàn)[9]可知，最終可將最優(yōu)濾波器的性質(zhì)轉(zhuǎn)化為如下等價(jià)條件：

(25)

其中：C0k為殘差的協(xié)方差，如下：

(26)

其中：zk為殘差，是一個(gè)白噪聲序列：

zk=Zk-HkXk

(27)

通過聯(lián)立式(25)與式(20)，可以將式(25)展開得到：

(28)

假設(shè)Qk，Rk，P0均正定，且Hk滿秩，則將式(28)化簡并將式(23)代入，可展開得到：

λkMk=Nk

(29)

其中：

(30)

(31)

其中：C0k由實(shí)際數(shù)據(jù)計(jì)算得到，而并非式(26)所示的理想化表達(dá)式得到，則最佳遺忘因子計(jì)算方法如下：

λk=max{1,tr(Nk)/tr(Mk)}

(32)

4 視覺信息滯后的補(bǔ)償方法

4.1 問題描述

通過AFKF的方法，能夠得到精確的無人機(jī)姿態(tài)、速度以及位置信息。但由于實(shí)時(shí)的視覺SLAM求解位姿的過程根據(jù)具體運(yùn)算平臺以及算法的不同，往往需要30～200 ms甚至更長的運(yùn)算時(shí)間，這將會導(dǎo)致視覺位姿信息滯后于慣導(dǎo)信息。

圖2 延時(shí)示意圖

如圖2所示，由于視覺SLAM求解位姿需要一定的處理時(shí)間，通常，在t1時(shí)刻獲得的圖像信息，要在t2時(shí)刻才能求解出相應(yīng)的SLAM位姿結(jié)果，并傳遞到組合導(dǎo)航流程與慣性信息融合，t2時(shí)刻獲得的圖像信息同理，要在t3時(shí)刻才能夠解算出相應(yīng)的位姿結(jié)果，這不可避免的導(dǎo)致了視覺位姿滯后于慣導(dǎo)信息，即在t3時(shí)刻對視覺位姿以及慣性信息進(jìn)行融合時(shí)，視覺位姿缺失了t2時(shí)刻至t3時(shí)刻的運(yùn)動信息。因此，我們需要對視覺位姿進(jìn)行補(bǔ)償，以減緩甚至消除這類延時(shí)對系統(tǒng)帶來的影響。

通常在簡化ORB_SLAM2實(shí)時(shí)運(yùn)行時(shí)：

t2-t1≠t3-t2

(33)

也可以理解為，通常連續(xù)兩組位姿之間的慣導(dǎo)數(shù)據(jù)的數(shù)量并不相同，即：

m≠n

(34)

如圖2所示，假設(shè)IMU采樣周期為T，則有：

(35)

本文著重針對圖2中，t2至t3時(shí)刻的延時(shí)處理做詳細(xì)說明，其余時(shí)刻同理。

4.2 補(bǔ)償姿態(tài)延時(shí)

由于視覺位姿的姿態(tài)信息包含了從初始參考幀一直到t2時(shí)刻的姿態(tài)信息，因此，只需要補(bǔ)償t3時(shí)刻相對于t2時(shí)刻的旋轉(zhuǎn)量即可。可令t2時(shí)刻為參考時(shí)刻，即t2時(shí)刻對應(yīng)的旋轉(zhuǎn)四元數(shù)為單位四元數(shù)[1,0,0,0]T。

Φt2 + T=ωt2 + TbT= [Δθx,Δθy,Δθz]T|t2 + T

(36)

t2+T時(shí)刻的四元數(shù)增量為：

qt2 + T(t) =

(37)

進(jìn)而可得到，t2+T時(shí)刻對應(yīng)的旋轉(zhuǎn)四元數(shù)為：

Q(t2+T)=Q(t2)?qt2+T(t)

(38)

經(jīng)過k次迭代后，可以得到表示由t2時(shí)刻到t3時(shí)刻，機(jī)體坐標(biāo)系旋轉(zhuǎn)量的四元數(shù)Q(t3)。

(39)

進(jìn)而通過旋轉(zhuǎn)矩陣與歐拉角間的轉(zhuǎn)換關(guān)系[11]，可求出t3時(shí)刻對應(yīng)的真實(shí)姿態(tài)角，從而完成姿態(tài)延時(shí)的補(bǔ)償。

4.3 補(bǔ)償位置延時(shí)

由于視覺位姿的姿態(tài)信息包含了從初始參考幀一直到t2時(shí)刻的位置信息，因此，只需要補(bǔ)償t3時(shí)刻相對于t2時(shí)刻的平移量即可，也即是，認(rèn)為t2時(shí)刻為參考時(shí)刻，即t2時(shí)刻對應(yīng)的相對位置為：

pt2=[0,0,0]T

(40)

其對應(yīng)的初始速度可以由組合導(dǎo)航算法在t2時(shí)刻輸出的速度量得到，則t2時(shí)刻對應(yīng)的速度為：

vt2= [vx,vy,vz]T|t2

(41)

假設(shè)t2+T時(shí)刻IMU采集到的機(jī)體坐標(biāo)系下的原始加速度值為：

at2 + T= [ax,ay,az]T|t2 + T

(42)

則通過運(yùn)動學(xué)公式，可以得到t2+T時(shí)刻對應(yīng)的速度為：

vt2+T=vt2+at2+TT

(43)

同理，通過運(yùn)動學(xué)公式，可以得到t2+T對應(yīng)的相對位置為：

(44)

經(jīng)過k次迭代，最終可以得到B_坐標(biāo)系相對于B坐標(biāo)系在導(dǎo)航坐標(biāo)系下的相對位移：

pt3= [px,py,pz]T|t3

(45)

由于在時(shí)刻t3傳入組合導(dǎo)航系統(tǒng)的視覺位置量測信息，表示的是B坐標(biāo)系在導(dǎo)航坐標(biāo)系下的位置信息Pt2，則t3時(shí)刻對應(yīng)的真實(shí)位置信息應(yīng)該為：

Pt3=Pt2+pt3

(46)

5 實(shí)驗(yàn)結(jié)果分析

本文將采用EuRoC數(shù)據(jù)集中的V2_02_medium序列來對本文算法進(jìn)行驗(yàn)證，首先是基于該序列驗(yàn)證本文的AFKF算法，由此分析AFKF的相對于傳統(tǒng)EKF在視覺慣導(dǎo)組合導(dǎo)航中的優(yōu)越性；其次，本文將基于簡化ORB_SLAM2從V2_02_medium序列解算得到的視覺位姿信息，人工模擬200 ms的延時(shí)，并采用本文的延時(shí)補(bǔ)償方法對延時(shí)進(jìn)行處理，并分析本文算法的有效性。

5.1 AFKF仿真結(jié)果

本文首先基于簡化ORB_SLAM2對V2_02_medium圖像序列進(jìn)行視覺位姿解算，得到相應(yīng)的視覺位姿，并在數(shù)學(xué)仿真平臺上完成視覺位姿與V2_02_medium序列中相應(yīng)的慣導(dǎo)數(shù)據(jù)進(jìn)行坐標(biāo)系統(tǒng)一，最終對得到的數(shù)據(jù)分別通過傳統(tǒng)EKF算法與AFKF算法進(jìn)行導(dǎo)航解算，并作相應(yīng)比較，初始化數(shù)據(jù)如下所示：

P=diag([(0.5*pi/180)2,(0.5*pi/180)2,

(0.5*pi/180)2，(0.5)2，(0.5)2，(0.5)2，

(0.4)2，(0.4)2，(0.4)2，(0.5*pi/180/3600)2,

(0.5*pi/180/3600)2,(0.5*pi/180/3600)2，

(5E-4)2,(5E-4)2,(5E-4)2])

(47)

q=diag([(2*pi/180)2,(2*pi/180)2,

(2*pi/180)2,(5E-3)2,(5E-3)2,

(5E-3)2，(5E-2)2,(5E-2)2,

(5E-2)2，(0.5*pi/180)2,

(0.5*pi/180)2,(0.5*pi/180)2，

(5E-3)2,(5E-3)2,(5E-3)2])

(48)

R=diag([(0.5*pi/180)2,(0.5*pi/180)2,

(0.5*pi/180)2,(0.5)2,(0.5)2,(0.5)2])

(49)

通過仿真可以得到姿態(tài)曲線，以及與姿態(tài)真值之間姿態(tài)誤差曲線，如圖3和圖4所示。

圖3 EKF、AFKF姿態(tài)曲線

圖4 EKF、AFKF與真值的姿態(tài)誤差曲線

如圖3和圖4所示，傳統(tǒng)EKF方法與AFKF方法，均能較好地跟蹤姿態(tài)真值，其誤差在5°以內(nèi)，并且，AFKF相對于傳統(tǒng)EKF而言，在一定程度上可以降低姿態(tài)誤差，以獲得更高精度的姿態(tài)解算值。

速度曲線，以及與速度真值之間的速度誤差曲線分別如圖5和圖6所示。相對于傳統(tǒng)EKF而言，通過AFKF可以有效降低與真值之間的速度誤差，并誤差在0.2 m/s以內(nèi)，相對于傳統(tǒng)EKF算法，性能提升了約30%左右。

圖5 EKF、AFKF速度曲線

圖6 EKF、AFKF與真值的速度誤差曲線

位置曲線，以及與位置真值之間的位置誤差曲線分別如圖7～圖8所示。通過AFKF方法，可以使通過組合導(dǎo)航解算得到的位置、速度以及姿態(tài)信息的精度進(jìn)一步提高，從而提高濾波器性能。

圖7 EKF、AFKF位置曲線

圖8 EKF、AFKF與真值的位置誤差曲線

5.2 延時(shí)補(bǔ)償仿真結(jié)果

本小節(jié)在前一小節(jié)的基礎(chǔ)上，對得到的視覺位姿人工添加了相對于V2_02_medium序列中慣導(dǎo)信息的200 ms的滯后，并在數(shù)學(xué)仿真平臺上分別采用了未加延時(shí)補(bǔ)償?shù)腁FKF算法與添加了延時(shí)補(bǔ)償?shù)腁FKF進(jìn)行了數(shù)據(jù)融合，本小節(jié)的仿真初始化數(shù)據(jù)與式(47)～(49)一致。

圖9，10分別為姿態(tài)曲線，以及未采用延時(shí)補(bǔ)償方法和采用延時(shí)補(bǔ)償方法時(shí)，通過AFKF得到的姿態(tài)與真值之間的姿態(tài)誤差曲線。

如圖9和圖10所示，采用本文的延時(shí)補(bǔ)償方法后，不僅顯著地降低了姿態(tài)誤差，使跟蹤性能得到了非常大的提升，并且始終將姿態(tài)誤差維持在5°以內(nèi)，基本與未加入200 ms人工延時(shí)的估計(jì)精度一致。

圖9 采用與未采用延時(shí)補(bǔ)償?shù)腁FKF姿態(tài)曲線

圖10 采用與未采用延時(shí)補(bǔ)償?shù)腁FKF姿態(tài)與真值的姿態(tài)誤差曲線

圖11～12分別為速度曲線，以及未采用延時(shí)補(bǔ)償方法和采用延時(shí)補(bǔ)償方法時(shí)，通過AFKF得到的姿態(tài)與真值之間的速度誤差曲線。

如圖11和圖12所示，通過本文的延時(shí)補(bǔ)償方法，速度誤差顯著降低，有效緩解了人工延時(shí)帶來的誤差，相對于未采用延時(shí)補(bǔ)償?shù)那闆r，對真值的跟蹤更加精確。

圖11 采用與未采用延時(shí)補(bǔ)償?shù)腁FKF速度曲線

圖12 采用與未采用延時(shí)補(bǔ)償?shù)腁FKF速度與真值的速度誤差曲線

圖13和圖14分別為位置曲線，以及未采用延時(shí)補(bǔ)償方法和采用延時(shí)補(bǔ)償方法時(shí)，通過AFKF得到的姿態(tài)與真值之間的位置誤差曲線。

如圖13和圖14所示，采用本文的延時(shí)補(bǔ)償方法后，有效地降低了由人工延時(shí)帶來的位置誤差，并且基本維持了未加入人工延時(shí)時(shí)的位置解算精度，誤差維持在0.4 m內(nèi)。

圖13 采用與未采用延時(shí)補(bǔ)償?shù)腁FKF位置曲線

圖14 采用與未采用延時(shí)補(bǔ)償?shù)腁FKF位置與真值的位置誤差曲線

6 結(jié)束語

通過實(shí)驗(yàn)證明，本文的算法通過引入自適應(yīng)算法與延時(shí)補(bǔ)償算法，有效提高組合導(dǎo)航系統(tǒng)的解算精度，提高濾波器性能，能夠在保證視覺位姿解算精度的前提下，得到200 Hz的導(dǎo)航解算結(jié)果，非常適合無人機(jī)等需要高頻率導(dǎo)航信息，且硬件算力有限的平臺[13-14]。