基于自適應(yīng)容積信息濾波的無人機(jī)相對導(dǎo)航方法

蘇炳志，何權(quán)榮，曹 晞，裴文龍，張少華

（1.中國直升機(jī)設(shè)計研究所，天津 300300；2.94657部隊，九江 332000；3.陸軍裝備部航空軍代局駐天津地區(qū)航空軍代室，天津 300384）

無人機(jī)采用編隊方式可以協(xié)同完成多樣、復(fù)雜的任務(wù)，在搜索救援、合作目標(biāo)跟蹤、空中表演等領(lǐng)域得到廣泛應(yīng)用[1]。準(zhǔn)確可靠的相對位置、速度和姿態(tài)信息是安全地保持無人機(jī)隊形和避免編隊內(nèi)碰撞的前提，因此無人機(jī)間的相對導(dǎo)航對于編隊飛行是至關(guān)重要的。最常見的相對導(dǎo)航方法是在每架無人機(jī)上配備全球衛(wèi)星導(dǎo)航系統(tǒng)（Global Navigation Satellite System,GNSS）和慣性導(dǎo)航系統(tǒng)（Inertial Navigation System,INS）傳感器，使用相對GNSS測量來修正相對INS[2,3]。盡管基于GNSS/INS的相對導(dǎo)航方法在美國F-18飛機(jī)編隊飛行項目中得到應(yīng)用[4]，但是該方法在衛(wèi)星導(dǎo)航信號受到干擾或丟失情況下性能急劇下降。因此，學(xué)者們發(fā)展了一種基于視覺傳感器和信標(biāo)點的視覺輔助慣性導(dǎo)航以提高相對導(dǎo)航系統(tǒng)的自主性和減少對外界的依賴性[5]。

INS/Vision相對導(dǎo)航精度與用于狀態(tài)估計的非線性濾波算法息息相關(guān)。在過去很長一段時間，擴(kuò)展卡爾曼濾波（Extended Kalman Filtering,EKF）一直是相對導(dǎo)航狀態(tài)估計的重要方法[6]。但是EKF只適用于非線性方程能用一階泰勒級數(shù)進(jìn)行近似的弱非線性情況，并且其在狀態(tài)遞推估計過程需要求解非線性系統(tǒng)的雅克比矩陣[7]。為了克服EKF的限制，學(xué)者們采用高斯加權(quán)積分法解決系統(tǒng)非線性問題，提出了一系列確定性采樣非線性高斯濾波。Julier和Uhlmann采用無跡變換生成一系列Sigma點來捕獲高斯分布的均值和方差，提出了具有三階泰勒級數(shù)逼近精度的無跡卡爾曼濾波（Unscented Kalman Filtering,UKF）[8]。Arasaratnam首次以公式形式詳盡地推導(dǎo)了一種基于三階球面-徑向容積準(zhǔn)則的確定性采樣非線性高斯濾波——容積卡爾曼濾波（Cubature Kalman Filtering,CKF）[9]。與UKF相比，CKF各容積點的權(quán)重相同并且全為正數(shù)，因此其計算形式更簡單、數(shù)值穩(wěn)定性更好。

飛行過程中由于無人機(jī)間相對姿態(tài)和位置變化將導(dǎo)致部分信標(biāo)點被機(jī)體/機(jī)翼遮擋，此時可見信標(biāo)點數(shù)發(fā)生變化，上述EKF等卡爾曼類濾波難以處理這種情況下的狀態(tài)估計問題；將每個信標(biāo)點當(dāng)成單獨的信息源，從多傳感器信息融合的角度出發(fā)對多個信標(biāo)點提供的無人機(jī)間的相對視線矢量信息進(jìn)行融合，王小剛采用擴(kuò)展信息濾波（Extended Information Filtering,EIF）實現(xiàn)了INS/Vision相對導(dǎo)航狀態(tài)估計[10]。文獻(xiàn)[11]和[12]分別將UKF和CKF與信息濾波相結(jié)合發(fā)展了無跡信息濾波（Unscented Information Filtering,UIF）和容積信息濾波（Cubature Information Filtering,CIF）。

無論是卡爾曼濾波還是信息濾波，均需要知道量測噪聲先驗統(tǒng)計特性來設(shè)置濾波器的量測噪聲協(xié)方差，并且在整個編隊飛行過程中量測噪聲統(tǒng)計特性不變，系統(tǒng)才能獲得最優(yōu)狀態(tài)估計。當(dāng)視覺導(dǎo)航系統(tǒng)處于實際運(yùn)行環(huán)境中，由于各種不確定因素的影響，噪聲的統(tǒng)計特性變得復(fù)雜，并具有時變特性[13]。為了解決上述問題，Sarkka和Nummenmaa將變分貝葉斯（Variational Bayesian,VB）理論和卡爾曼濾波相結(jié)合提出了變分貝葉斯自適應(yīng)卡爾曼濾波[14]，在線估計和修正濾波器的量測噪聲協(xié)方差矩陣，降低了狀態(tài)估計誤差，提高了系統(tǒng)精度；之后將變分貝葉斯理論推廣到非線性系統(tǒng)中提出了變分貝葉斯自適應(yīng)CKF[15]。

為了解決基于視覺的無人機(jī)相對導(dǎo)航系統(tǒng)可見信標(biāo)點數(shù)隨時間變化和視覺量測噪聲統(tǒng)計特性未知且時變問題，本文在前人研究成果的基礎(chǔ)上對CIF進(jìn)行改進(jìn)，在CIF中融入變分貝葉斯理論，將量測噪聲協(xié)方差建模為逆威舍特分布，提出了基于逆威舍特分布的變分貝葉斯自適應(yīng)容積信息濾波（Variational Bayesian Adaptive Cubature Information Filtering,VBACIF）。建立基于視覺輔助的相對導(dǎo)航系統(tǒng)模型，設(shè)計基于VBACIF的無人機(jī)編隊相對導(dǎo)航濾波器，參考多傳感器信息融合原理將每個信標(biāo)點當(dāng)作一個單獨信息源，采用變分貝葉斯理論和三階球面-徑向容積準(zhǔn)則對每個信標(biāo)點的相對視線量測噪聲協(xié)方差陣和無人機(jī)間的相對導(dǎo)航狀態(tài)進(jìn)行聯(lián)合估計。最后，通過蒙特卡洛打靶仿真對比分析驗證了本文算法的有效性和優(yōu)越性。

1 相對導(dǎo)航系統(tǒng)模型

對于長機(jī)-僚機(jī)編隊模式，僚機(jī)需要在編隊機(jī)動前后以長機(jī)為參考進(jìn)行相對位置保持。用于提供相對信息的相對導(dǎo)航系統(tǒng)對于編隊飛行控制是至關(guān)重要的。在僚機(jī)本體系下，以長機(jī)和僚機(jī)之間的相對運(yùn)動關(guān)系建立雙機(jī)間相對導(dǎo)航運(yùn)動學(xué)模型；基于光學(xué)傳感器和一系列信標(biāo)點的視覺導(dǎo)航系統(tǒng)輔助估計相對狀態(tài)。

1.1 相對導(dǎo)航運(yùn)動學(xué)模型

長機(jī)和僚機(jī)之間的相對導(dǎo)航運(yùn)動學(xué)模型由相對質(zhì)心運(yùn)動方程和相對姿態(tài)運(yùn)動方程共同構(gòu)成。

1.1.1 相對質(zhì)心運(yùn)動方程

1.1.2 相對姿態(tài)運(yùn)動方程

用于表示姿態(tài)的四元數(shù)由歐拉旋轉(zhuǎn)軸e和旋轉(zhuǎn)角θ表示為：

1.2 慣性和視覺傳感器測量模型

慣性導(dǎo)航系統(tǒng)的加速度計和陀螺儀測量模型分別為：

圖1 視覺傳感器量測Fig.1 Measurement of visual sensor

2 變分貝葉斯自適應(yīng)容積信息濾波

考慮如下所示的離散系統(tǒng)：

2.1 容積信息濾波

2.1.1 預(yù)測

2.1.2 更新

2.2 變分貝葉斯自適應(yīng)容積信息濾波

在變分貝葉斯容積信息濾波中，將系統(tǒng)狀態(tài)和量測噪聲協(xié)方差矩陣當(dāng)作隨機(jī)變量，采用變分變?nèi)~斯近似理論和容積準(zhǔn)則進(jìn)行聯(lián)合估計；VBACIF通過在線跟蹤量測噪聲統(tǒng)計特性并自適應(yīng)調(diào)節(jié)濾波器的量測噪聲協(xié)方差陣以適應(yīng)系統(tǒng)中量測噪聲未知或時變等情況。

2.2.1 預(yù)測

系統(tǒng)狀態(tài)kx和量測噪聲協(xié)方差是相互獨立的，根據(jù)先驗知識，認(rèn)為它們分別服從高斯分布和逆威舍特分布[15]。因此1k-時刻系統(tǒng)狀態(tài)和第i個信息源的量測噪聲協(xié)方差可表示成高斯-逆威舍特分布形式：

2.2.2 更新

聯(lián)合先驗分布和預(yù)測分布均是高斯-逆威舍特分布形式，那么聯(lián)合后驗分布的函數(shù)形式將保持不變（共軛性），即：

3 基于VBACIF的相對導(dǎo)航濾波器

本節(jié)設(shè)計基于VBACIF的相對導(dǎo)航濾波器。在濾波器設(shè)計中，直接使用具有模為1約束的姿態(tài)四元數(shù)作為狀態(tài)參數(shù)進(jìn)行濾波，將導(dǎo)致狀態(tài)協(xié)方差陣奇異；因此，采用無約束三維矢量——羅德里格斯誤差矢量來表示姿態(tài)四元數(shù)誤差，羅德里格斯誤差矢量δ s與姿態(tài)四元數(shù)偏差δq有如下的關(guān)系：

如圖2所示將每個信標(biāo)點當(dāng)作獨立的信息源，采用VBACIF算法融合視覺量測估計得到狀態(tài)如式(60)所示：

圖2 視覺量測信息融合架構(gòu)Fig.2 The fusion architecture of visual measurement information

相對位置和速度可以直接從式經(jīng)量測修正后的狀態(tài)向量中獲得，相對姿態(tài)更新按下式計算：

4 仿真分析

為了驗證VBACIF算法的有效性和優(yōu)越性，開展數(shù)值仿真驗證，將VBACIF與EIF和CIF進(jìn)行比較。

4.1 仿真初始條件

加速度計初始漂移、隨機(jī)游走和噪聲分別為0.2mg、0.002mg/s1/2和0.02mg·s1/2，陀螺儀初始漂移、隨機(jī)游走和噪聲分別為0.1°/h、0.06°/h3/2和0.01°/h1/2。信標(biāo)點在僚機(jī)上的布局如表1所示，第7個信標(biāo)點在0≤t≤10s和90s≤t≤100s時間段內(nèi)被機(jī)體/機(jī)翼遮擋不可見，其他信標(biāo)點在仿真時間段內(nèi)均可見。參照文獻(xiàn)[3]的飛行軌跡開展編隊飛行相對導(dǎo)航仿真。

表1 信標(biāo)位置列表Tab.1 List of beacon locations

視覺量測噪聲服從高斯分布N(vi,k;0,Ri,vis)，并且量測噪聲是時變的，即方差Ri,vis變化如式(64)所示：

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

將以三維相對位置、速度和姿態(tài)誤差來描述相對導(dǎo)航濾波器的估計精度，其中相對位置和速度誤差可直接由相對導(dǎo)航濾波器估計的值與標(biāo)稱值作差值得到，而三維相對姿態(tài)誤差δ?、δφ和δγ計算如下：

圖3 VBACIF的相對位置估計誤差Fig.3 Relative position estimated error using the VBACIF

圖4 VBACIF的相對速度估計誤差Fig.4 Relative velocity estimated error using the VBACIF

圖5 VBACIF的相對姿態(tài)估計誤差Fig.5 Relative attitude estimated error using the VBACIF

圖7 VBACIF的R7,k(1,1)估計曲線Fig.7 Estimation curve of R7,k(1,1)using the VBACIF

如圖6-7所示，VBACIF能夠精確估計和快速跟蹤視覺量測噪聲統(tǒng)計特性的變化；此外，在VBACIF的算法中，不同信標(biāo)點的噪聲統(tǒng)計特性估計是相互獨立，這使得部分信標(biāo)點不可見的情況下其他信標(biāo)點（表1中第1~6個信標(biāo)點）噪聲統(tǒng)計特性估計能正常進(jìn)行，而第7個信標(biāo)點的量測噪聲協(xié)方差能夠在可見時間段10s~90s內(nèi)進(jìn)行估計。

圖6 VBACIF的R1,k(1,1)估計曲線Fig.6 Estimation curve of R1,k(1,1)using the VBACIF

圖8-10是基于EIF、CIF和VBACIF的相對導(dǎo)航100次蒙特卡洛打靶仿真三維相對位置、速度和姿態(tài)均方根誤差曲線的比較結(jié)果，表2是相對位置、速度和姿態(tài)平均均方根誤差的匯總。打靶仿真結(jié)果表明，EIF的估計精度最差并且估計曲線初始階段存在振蕩現(xiàn)象，這是由于基于一階泰勒展開的EIF具有較大截斷誤差并且缺乏協(xié)方差自適應(yīng)估計及調(diào)節(jié)能力；VBACIF的估計精度比CIF高，且估計穩(wěn)定性不受量測噪聲統(tǒng)計特性變化的影響，這是由于VBACIF在進(jìn)行狀態(tài)估計的同時對量測噪聲統(tǒng)計特性進(jìn)行了估計，并用其在線自適應(yīng)調(diào)節(jié)濾波器的量測噪聲協(xié)方差矩陣。VBACIF算法的相對位置、速度和姿態(tài)精度較CIF算法分別提高了24.85%、9.41%和45.52%。

圖8 三種濾波算法的相對位置估計精度比較Fig.8 Comparison of relative position estimated accuracy for three filtering algorithms

圖9 三種濾波算法的相對速度估計精度比較Fig.9 Comparison of relative velocity estimated accuracy for three filtering algorithms

圖10 三種濾波算法的相對姿態(tài)估計精度比較Fig.10 Comparison of relative attitude estimated accuracy for three filtering algorithms

表2 三種濾波算法的相對導(dǎo)航精度對比Tab.2 Comparison of relative navigation accuracy of three algorithms

5 結(jié)論

本文針對無人機(jī)編隊基于視覺輔助的相對導(dǎo)航系統(tǒng)信標(biāo)點數(shù)時變和視覺量測噪聲協(xié)方差不確定問題，在信息濾波框架下提出了一種包含量測噪聲自適應(yīng)估計的相對導(dǎo)航方法，主要工作總結(jié)如下：

1)單次仿真結(jié)果表明，在量測噪聲統(tǒng)計特性先驗信息不足且時變和可見信標(biāo)點數(shù)變化的情況下，基于VBACIF的視覺輔助相對導(dǎo)航濾波器能夠?qū)Ω餍艠?biāo)點相對視線矢量量測噪聲協(xié)方差進(jìn)行估計并實時跟蹤其變化情況，同時能夠精確地給出無人機(jī)之間的相對位置、速度和姿態(tài)信息，具有良好的自適應(yīng)能力。

2)不同濾波算法蒙特卡洛打靶仿真的位置、速度和姿態(tài)估計均方根誤差比較結(jié)果表明，基于VBACIF的相對導(dǎo)航精度高于基于EIF和CIF的，驗證了本文濾波算法VBACIF的有效性和優(yōu)越性。

3)基于VBACIF算法提出的包含量測噪聲協(xié)方差自適應(yīng)估計的相對導(dǎo)航方法在有人/無人機(jī)空中加油、航天器交會對接和衛(wèi)星編隊等航空航天多飛行器系統(tǒng)中同樣具有潛在應(yīng)用前景。