郭肖亭, 孫長庫,2, 王 鵬,2

(1. 天津大學(xué)精密測試技術(shù)及儀器國家重點實驗室, 天津 300072; 2. 中航工業(yè)洛陽電光設(shè)備研究所光電控制技術(shù)重點實驗室, 河南 洛陽 471009)

0 引 言

姿態(tài)確定問題是頭盔姿態(tài)確定問題[1]、頭盔瞄準(zhǔn)定位[2]、無人機(jī)飛行控制[3]、增強(qiáng)現(xiàn)實[4]等領(lǐng)域的關(guān)鍵性問題之一,在工程實踐和科學(xué)研究中有重要的理論和實踐意義。單一姿態(tài)測量方法在精度、速度等方面因自身技術(shù)的限制,難以滿足測量需求,多傳感器融合姿態(tài)測量技術(shù)[5]應(yīng)用而生。將視覺和慣性融合進(jìn)行姿態(tài)測量,可以發(fā)揮視覺方法重復(fù)性和穩(wěn)定性好[6-7]以及慣性方法更新頻率高、不受環(huán)境光干擾的特點[8],實現(xiàn)各傳感器的優(yōu)勢互補(bǔ)。

針對視覺、慣性融合視覺和慣性融合測量的相關(guān)問題,國內(nèi)外學(xué)者進(jìn)行了大量的研究和探索[9-13]。在實際應(yīng)用中,融合姿態(tài)測量的系統(tǒng)模型通常是非線性或/和非高斯,因此通常使用擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)[14]、無跡卡爾曼濾波(unscented Kalman filter, UKF)[15]、容積卡爾曼濾波(cubature Kalman filter, CKF)[16-18]等適合于計算機(jī)迭代處理的非線性濾波算法進(jìn)行多傳感器融合。EKF是高斯白噪聲假設(shè)下典型的非線性濾波方法,但EKF存在理論上的局限性,其一階線性截斷誤差使得濾波精度低甚至導(dǎo)致濾波發(fā)散,尤其在系統(tǒng)非線性較強(qiáng)、存在未知干擾或狀態(tài)突變等情況下。UKF不對系統(tǒng)模型進(jìn)行線性化處理, 將對非線性系統(tǒng)的函數(shù)近似轉(zhuǎn)換成對其后驗概率密度的近似,實現(xiàn)簡單,濾波效果優(yōu)于EKF。CKF采用一組等權(quán)值的容積點集來計算后驗概率密度,相比EKF和UKF,具有更優(yōu)的非線性逼近性能、數(shù)值精度。

上述非線性濾波算法都屬于高斯濾波器,即假設(shè)狀態(tài)的后驗分布和預(yù)測分布近似為高斯分布。而在實際應(yīng)用中,往往存在著模型不確定性或/和噪聲統(tǒng)計特性不完全已知,這些不確定因素會造成傳統(tǒng)的非線性卡爾曼濾波算法失去最優(yōu)性,使得估計精度降低甚至濾波發(fā)散。而基于魯棒控制理論的H∞濾波算法[19]與卡爾曼濾波器采用最小方差估計準(zhǔn)則不同,該算法使用極大極小準(zhǔn)則,不需要知道環(huán)境噪聲的統(tǒng)計特性等先驗信息,通過使噪聲能量和估計誤差能量的最大比值小于某一個正數(shù),確保外界干擾對狀態(tài)估計結(jié)果的影響最小,因此該算法對系統(tǒng)模型誤差和外界干擾具有更強(qiáng)的魯棒性。將H∞魯棒濾波算法和CKF相結(jié)合,形成魯棒CKF濾波策略,可以克服系統(tǒng)非線性、非高斯特性以及突變型不良測量對濾波帶來的影響,提高測量系統(tǒng)的濾波精度以及魯棒性。針對計算誤差、舍入誤差、模型失準(zhǔn)等原因造成協(xié)方差矩陣失去正定性而往往出現(xiàn)濾波中斷的問題,使用矩陣對角化變換代替標(biāo)準(zhǔn)CKF算法中的Cholesky分解[20],提高濾波穩(wěn)定性。姿態(tài)參數(shù)采用計算量小、可全姿態(tài)工作且不存在奇異性的四元數(shù),針對其在非線性濾波中易出現(xiàn)協(xié)方差矩陣奇異性的問題,使用乘性誤差四元數(shù)三維矢量進(jìn)行預(yù)測值和方差的計算,以保持濾波算法的穩(wěn)定性。

搭建了慣性和視覺多傳感器融合姿態(tài)測量系統(tǒng),針對系統(tǒng)中各傳感器坐標(biāo)系以及輔助坐標(biāo)系不一致的問題,通過將各傳感器測量值轉(zhuǎn)換到目標(biāo)坐標(biāo)系中完成測量系統(tǒng)坐標(biāo)系的統(tǒng)一,將提出的基于矩陣對角化變換的魯棒四元數(shù)容積卡爾曼濾波(quaternion cubature Kalman filter,QCKF)算法在實驗平臺上進(jìn)行驗證,并與標(biāo)準(zhǔn)CKF進(jìn)行對比,驗證本文算法的有效性。

1 視覺和慣性融合姿態(tài)測量原理

融合姿態(tài)測量原理如圖1所示。慣性測量單元(inertial measurement unit,IMU)和目標(biāo)物體固定連接,攝像機(jī)正對與目標(biāo)物體固定連接的立體靶標(biāo)。在目標(biāo)物體運(yùn)動過程中,IMU以初始姿態(tài)為基準(zhǔn),測量其相對空間慣性坐標(biāo)系姿態(tài);攝像機(jī)拍攝立體靶標(biāo)運(yùn)動圖像,通過對特征點圖像坐標(biāo)分析計算,完成視覺姿態(tài)測量。

圖1 姿態(tài)測量系統(tǒng)結(jié)構(gòu)示意圖Fig.1 Schematic diagram of fusion measurement system

慣性測量和視覺測量在不同的坐標(biāo)系中進(jìn)行并且各自的測量姿態(tài)與目標(biāo)物體運(yùn)動姿態(tài)存在差異,因此對測量系統(tǒng)坐標(biāo)系進(jìn)行統(tǒng)一。定義ObXbYbZb、OcXcYcZc、OtXtYtZt、OnXnYnZn分別表示運(yùn)動物體坐標(biāo)系(b系)、攝像機(jī)坐標(biāo)系(c系)、靶標(biāo)坐標(biāo)系(t系)以及IMU坐標(biāo)系(n系),如圖2所示。測量中使用低精度的微機(jī)電系統(tǒng)(micro-electro-mechanical system,MEMS)-IMU,其無法感測地球自轉(zhuǎn)角速率,因此可選取和地球固定連接的坐標(biāo)系為近似空間慣性坐標(biāo)系OiXiYiZi,本文中選取c系。坐標(biāo)系統(tǒng)一的目標(biāo)可表述為將慣性測量和視覺測量均轉(zhuǎn)換到目標(biāo)物體在c系中的旋轉(zhuǎn)姿態(tài)。

(1)

(2)

圖2 測量系統(tǒng)坐標(biāo)系統(tǒng)一Fig.2 Coordinate system normalization

2 視覺和慣性融合姿態(tài)測量算法

2.1 視覺姿態(tài)測量

視覺姿態(tài)測量方法是使用攝像機(jī)拍攝運(yùn)動物體上特征點,根據(jù)特征點在攝像機(jī)中成像坐標(biāo)的變化,結(jié)合已知特征點在被測物體上的布局坐標(biāo),實現(xiàn)對被測物體姿態(tài)的測量[6-7]。本文視覺測量結(jié)構(gòu)采用由外向內(nèi)的方法,即特征點布置在運(yùn)動體上,而攝像機(jī)固定在環(huán)境中并朝向被測物,在室內(nèi)近距離高精度定姿尤其是頭部跟蹤應(yīng)用場景中,此種測量結(jié)構(gòu)與由內(nèi)向外的視覺測量結(jié)構(gòu)相比更實用。立體靶標(biāo)由4個非共面紅外LEDs組成,為視覺測量的特征點,各個特征點之間的相對空間幾何關(guān)系已知。使用紅外特征點是為了避免環(huán)境中雜散光源在圖像處理過程中進(jìn)行特征識別時帶來干擾。使用4個非共面特征點可以保證在較大的運(yùn)動范圍內(nèi)仍可以正確識別各個特征點。

在較大的運(yùn)動范圍內(nèi)使用較少的特征點進(jìn)行視覺姿態(tài)測量,比例正交投影迭代變換(pose from orthography and scaling with iterations,POSIT)是比較經(jīng)典的非線性迭代算法;而POSIT算法的測量精度與測量范圍有關(guān),小范圍內(nèi)可以達(dá)到較高的測量精度,隨測量范圍增加精度會相應(yīng)地降低。針對此問題本文使用基于約束校準(zhǔn)的姿態(tài)測量算法[6]。標(biāo)準(zhǔn)POSIT算法以初始位置為基準(zhǔn)進(jìn)行相對姿態(tài)測量,如圖3中A方案所示;而約束校準(zhǔn)算法采用圖3中B方案,先標(biāo)定后測量。首先對全空間進(jìn)行標(biāo)定,建立基準(zhǔn)位置,空間基矢量,基準(zhǔn)姿態(tài)的一一對應(yīng)關(guān)系;測量階段尋找距離待測量位置最近的基準(zhǔn)點,則待測位置姿態(tài)可表示為基準(zhǔn)姿態(tài)與待測位置相對基準(zhǔn)位置姿態(tài)的代數(shù)和為

θx=θbase+θbase-x

(3)

式中,θx,θbase,θbase-x分別表示待測量位置姿態(tài)、選定基準(zhǔn)位置姿態(tài)以及待測位置相對基準(zhǔn)位置姿態(tài)。

圖3 POSIT算法和約束校準(zhǔn)算法對比Fig.3 Contrast of POSIT and restrain calibration algorithm

2.2 陀螺儀模型

慣性姿態(tài)測量借助IMU中三自由度陀螺儀完成。陀螺儀感測目標(biāo)物體運(yùn)動,輸出角速度,通過姿態(tài)解算算法實現(xiàn)慣性姿態(tài)測量。陀螺儀輸出中存在許多誤差項,需要對其進(jìn)行標(biāo)定補(bǔ)償。陀螺儀輸出可建模為

(4)

零位偏差的標(biāo)定可采用每次測量之前先標(biāo)定的方法[21]。傳感器上電后先保持陀螺儀水平靜止測量幾組數(shù)據(jù),對每組數(shù)據(jù)計算漂移角度,然后時間和漂移角度進(jìn)行高階多項式曲線擬合獲取擬合系數(shù),將每組數(shù)據(jù)擬合系數(shù)取均值得到標(biāo)定擬合系數(shù)并保存。測量過程中根據(jù)標(biāo)定擬合系數(shù)和采樣時間估計漂移角度,從漂移角度反算得到偏差值,陀螺儀輸出減去反算偏差對原始輸出中的零位偏差進(jìn)行補(bǔ)償。具體過程如圖4所示。而對于與時間有關(guān)的隨機(jī)漂移的補(bǔ)償,可將其包含在濾波狀態(tài)量中,在每次濾波循環(huán)中對其不斷更新,從而進(jìn)一步對陀螺儀輸出量進(jìn)行補(bǔ)償。

圖4 陀螺儀零位偏差標(biāo)定及補(bǔ)償Fig.4 Calibration and compensation of gyroscope bias

2.3 矩陣對角化變換

標(biāo)準(zhǔn)CKF算法中使用Cholesky分解進(jìn)行協(xié)方差矩陣的分解,而Cholesky分解要求待分解矩陣具有正定性。當(dāng)協(xié)方差矩陣失去正定性時可導(dǎo)致濾波立即中斷,對算法穩(wěn)定性提出了極大的挑戰(zhàn)。濾波中斷多發(fā)生在濾波進(jìn)行過多次之后由于模型失配、計算誤差等造成的協(xié)方差矩陣非正定。而基于矩陣對角化變換的方法對協(xié)方差矩陣進(jìn)行分解,不需要矩陣具有正定性。

定理1設(shè)A為n階實對稱矩陣,則必有n階正交矩陣V,使得VTAV=V-1AV=D,即A=VTDV,其中D是以A的n個特征值為對角元素的對角陣。

而在卡爾曼濾波過程中協(xié)方差矩陣P是實對稱矩陣,由定理1立即可得P=VTDV,其中D是以P的n個特征值為對角元素的對角陣,V為n個特征值對應(yīng)的兩兩正交的特征向量構(gòu)成的正交矩陣陣,令

(5)

2.4 基于矩陣對角化變換的魯棒四元數(shù)容積卡爾曼濾波

不同于卡爾曼濾波基于使估計誤差的方差最小化的思想,H∞濾波是基于極大極小魯棒控制準(zhǔn)則的濾波算法,即最小化可能存在最大誤差,具體指的是在對濾波初始狀態(tài)、系統(tǒng)噪聲、觀測噪聲等先驗信息未知的條件下獲取狀態(tài)量的最佳估計值,定義代價函數(shù)

(6)

引入約束條件γ,有

(7)

式(7)可改寫為

(8)

(9)

將H∞濾波應(yīng)用到CKF中對H∞濾波方法中狀態(tài)估計協(xié)方差矩陣的遞推方程進(jìn)行轉(zhuǎn)換

Pk+1=Pk+1|k-

(10)

如果對任意時刻γ取滿足條件pk>0的最小值,則可以得到最優(yōu)H∞濾波。當(dāng)γ→∞時,H∞濾波器退化為卡爾曼濾波器。當(dāng)取最小值時濾波器的魯棒性最好,但估計方差不一定最小;當(dāng)γ→∞時則可以得到最小方差估計,但魯棒性較差。因此通過適當(dāng)選擇的值,調(diào)節(jié)系統(tǒng)在測量精度和系統(tǒng)魯棒性之間的平衡,以滿足測量需要。

CKF采用一組等權(quán)值的容積點集來計算后驗概率密度[14],對于模型精確系統(tǒng)的狀態(tài)估計精度可達(dá)到3階,但是當(dāng)存在較大系統(tǒng)模型誤差或系統(tǒng)狀態(tài)存在突變的情況,仍會產(chǎn)生較大的估計誤差。在實際應(yīng)用中,單一的CKF算法有時難以滿足測量需求,將QCKF和H∞魯棒濾波相結(jié)合,在CKF算法的框架中,將H∞濾波的魯棒特性加入到協(xié)方差矩陣更新計算中,并使用矩陣對角化變換代替標(biāo)準(zhǔn)CKF中的Cholesky分解,增加濾波算法的計算穩(wěn)定性。具體過程如下。

步驟1計算容積點

(11)

(12)

四元數(shù)和陀螺儀漂移部分容積點相應(yīng)為

(13)

式中,i=1,2,…,n。

步驟2計算經(jīng)過狀態(tài)方程傳遞后的容積點

(14)

(15)

步驟3計算k+1時刻的狀態(tài)預(yù)測值

(16)

式中

(17)

(18)

(19)

步驟4計算k+1時刻狀態(tài)誤差協(xié)方差矩陣

(20)

式中

步驟5計算更新后的狀態(tài)容積點

(21)

(22)

步驟6計算經(jīng)過測量方程的容積點

(23)

(24)

式中,i=1,2,…,n。

步驟7計算k+1時刻的測量預(yù)測值

(25)

(26)

(27)

步驟8估計k+1時刻的測量誤差協(xié)方差和一步預(yù)測互協(xié)方差矩陣

(28)

(29)

步驟9計算k+1時刻的濾波增益矩陣

(30)

步驟10計算k+1時刻的狀態(tài)估計值

(31)

(32)

(33)

(34)

(35)

步驟11使用式(10)進(jìn)行k+1時刻的狀態(tài)誤差協(xié)方差矩陣更新。

該方法將CKF和H∞濾波結(jié)合,發(fā)揮CKF算法的高精度和H∞濾波的魯棒性的特點,使用矩陣對角化變換改善數(shù)值計算的穩(wěn)定性。視覺和慣性融合測量系統(tǒng),在保持與慣性高頻率輸出的同時,在視覺和慣性數(shù)據(jù)重合時刻進(jìn)行濾波,用視覺數(shù)據(jù)抑制融合姿態(tài)隨時間漂移,獲取高頻率、穩(wěn)定、高精度的融合測量輸出。

3 實驗驗證及結(jié)果分析

視覺和慣性融合姿態(tài)測量系統(tǒng)裝置如圖5所示。實驗中轉(zhuǎn)臺為目標(biāo)運(yùn)動物體,IMU、立體靶標(biāo)通過螺絲安裝在轉(zhuǎn)臺上,攝像機(jī)朝向靶標(biāo)放置,計算機(jī)控制轉(zhuǎn)臺運(yùn)動、操作傳感器進(jìn)行測量并對測量數(shù)據(jù)進(jìn)行分析計算。其中實驗中使用的三維立體靶標(biāo)結(jié)構(gòu)如圖6所示,各特征點之間的空間幾何關(guān)系在圖中標(biāo)示,外圍3個LEDs在同一平面內(nèi)為等邊三角形3個頂點,中心LED位于三角形中心且距離平面25 mm。

圖5 測量系統(tǒng)實驗平臺Fig.5 Measurement system experimental platform

圖6 立體靶標(biāo)Fig.6 Three-dimensional target

表1 測量系統(tǒng)中固定旋轉(zhuǎn)矩陣標(biāo)定結(jié)果

在測量過程中,轉(zhuǎn)臺根據(jù)設(shè)計路徑進(jìn)行運(yùn)動,固定在轉(zhuǎn)臺上的IMU和立體靶標(biāo)隨轉(zhuǎn)臺一起轉(zhuǎn)動,運(yùn)動的同時進(jìn)行IMU測量數(shù)據(jù)采集以及攝像機(jī)拍攝立體靶標(biāo)在不同姿態(tài)下的圖像如圖7所示;對拍攝圖像進(jìn)行處理提取特征點,結(jié)合各個特征點的幾何空間坐標(biāo)關(guān)系,使用約束校準(zhǔn)方法求解視覺姿態(tài)測量值。視覺姿態(tài)測量值和IMU測量數(shù)據(jù)結(jié)合,使用不同的融合算法,完成融合姿態(tài)量的求解。其中慣性數(shù)據(jù)采集周期為0.01 s,視覺數(shù)據(jù)采集周期為1.8 s。

圖7 不同姿態(tài)拍攝圖像特征點提取Fig.7 Processed images of target from different attitudes

為了對基于矩陣對角化變換的魯棒CKF方法的有效性進(jìn)行驗證,在不同的運(yùn)動形式下將其與標(biāo)準(zhǔn)CKF、純慣性姿態(tài)測量以及視覺姿態(tài)測量進(jìn)行對比。主要進(jìn)行了轉(zhuǎn)臺繞不同的旋轉(zhuǎn)軸的單軸運(yùn)動,如圖8～圖10所示,以及轉(zhuǎn)臺俯仰軸和方位軸交替運(yùn)動的兩軸運(yùn)動,兩軸交替運(yùn)動的具體運(yùn)動方式為

(1)Z=-36°,X=-24°～ +24°;

(2)Z=-33°,X=+24°～-24°;

?

(25)Z=+36°,X=-24°～ +24°。

其中,X指代俯仰軸,Z指代方位軸。方位角階梯式上升從負(fù)的最大值運(yùn)動到正的最大值,在方位角每增加3°的運(yùn)動過程中,俯仰角往返運(yùn)動具體如圖11所示。

對轉(zhuǎn)臺單、雙軸運(yùn)動形式下不同算法的姿態(tài)角、姿態(tài)角誤差輸出圖形(見圖8～圖12)以及對應(yīng)的均方根誤差(root-mean-square error,RMSE)數(shù)據(jù)表2、表3進(jìn)行分析可知,將慣性和視覺進(jìn)行融合求解姿態(tài),可抑制純慣性測量角度漂移現(xiàn)象,保持視覺測量的穩(wěn)定性,使融合姿態(tài)更新速率與慣性保持一致;同時改進(jìn)的魯棒CKF與標(biāo)準(zhǔn)CKF相比準(zhǔn)確度較好。

圖8 俯仰角-36°～36°運(yùn)動姿態(tài)曲線及對應(yīng)誤差Fig.8 -36°～36°pitch axis attitude curve and its error curve

圖9 橫滾軸-33°～33°運(yùn)動姿態(tài)曲線以及對應(yīng)誤差Fig.9 -33°～33°roll axis attitude curve and its error curve

圖10 方位角-48°～48°運(yùn)動姿態(tài)曲線以及對應(yīng)誤差Fig.10 -48°～48°yaw axis attitude curve and its error curve

不同算法RMSE/(°)俯仰角橫滾角方位角視覺0.07580.04390.0943CKF0.06870.06120.0955魯棒CKF0.05230.06530.0873

表3 兩軸運(yùn)動不同算法的姿態(tài)角RMSE

對圖8～圖11姿態(tài)角誤差輸出圖形進(jìn)一步分析可知,視覺姿態(tài)誤差角隨時間變化平穩(wěn),沒有與時間相關(guān)的趨勢項;但視覺姿態(tài)中會存在尖峰噪聲,而尖峰噪聲的出現(xiàn)是隨機(jī)的,沒有明顯的規(guī)律。

魯棒CKF保持與慣性同頻率的高頻輸出且能抑制慣性隨時間漂移帶來的誤差而跟隨穩(wěn)定的視覺姿態(tài)。標(biāo)準(zhǔn)CKF也具有高頻輸出、抑制漂移的特點,但其輸出噪聲較大無論是視覺的尖峰噪聲或慣性的抖動噪聲均在標(biāo)準(zhǔn)CKF融合結(jié)果中有比較明顯的體現(xiàn),魯棒CKF在弱化融合結(jié)果跟隨測量噪聲和系統(tǒng)噪聲變化方面優(yōu)于CKF。單軸運(yùn)動,每個軸的運(yùn)動時間均不到60 s,其效果表明短時間內(nèi)魯棒CKF數(shù)據(jù)融合算法的有效性;而兩軸交替運(yùn)動,運(yùn)動時間達(dá)到760多秒且運(yùn)動形式復(fù)雜,其效果表明長時間魯棒CKF數(shù)據(jù)融合算法的有效性。

為直觀說明數(shù)據(jù)融合后相比純視覺姿態(tài)高頻輸出的特點,以圖8俯仰角單軸運(yùn)動為例給出融合后數(shù)據(jù)更新速率與視覺、慣性更新速率的對比,如圖13所示。由圖13可知,融合姿態(tài)可抑制慣性姿態(tài)角度漂移,能跟隨穩(wěn)定的視覺姿態(tài),保持慣性數(shù)據(jù)高頻輸出的特點。

圖11 俯仰軸和方位軸交替運(yùn)動姿態(tài)角誤差曲線Fig.11 Pitch axis and yaw axis alternating motion attitude angle error curves

圖12 俯仰軸和方位軸交替運(yùn)動姿態(tài)角曲線Fig.12 Pitch axis and yaw axis alternating motion attitude angle curves

圖13 融合前后數(shù)據(jù)更新頻率對比Fig.13 Contrast of update frequency before and after data fusion

4 結(jié) 論

基于視覺和慣性融合姿態(tài)測量的目的,搭建測量系統(tǒng)平臺,對測量涉及的坐標(biāo)系之間的關(guān)系進(jìn)行了梳理和標(biāo)定,目的是將不同的傳感器測量值統(tǒng)一到相同的坐標(biāo)系中,為進(jìn)一步的融合計算做準(zhǔn)備。運(yùn)動物體在單軸短時間運(yùn)動以及雙軸長時間運(yùn)動的不同運(yùn)動形式中,使用視覺法、慣性法、CKF、矩陣對角化魯棒CKF法對姿態(tài)角進(jìn)行計算并對姿態(tài)誤差角進(jìn)行對比。實驗結(jié)果表明,魯棒CKF算法有效地利用視覺測量穩(wěn)定性好的特性來抑制慣性測量誤差隨時間無約束發(fā)散并結(jié)合慣性測量更新頻率高的特點,極大地提高融合姿態(tài)的輸出頻率。改進(jìn)的魯棒CKF在抑制視覺測量的尖峰噪聲、慣性測量的抖動噪聲均明顯優(yōu)于CKF濾波算法。使用矩陣對角化變換代替標(biāo)準(zhǔn)CKF算法中的Cholesky分解,可以避免在狀態(tài)誤差協(xié)方差矩陣非正定時濾波中斷的情況,增加融合濾波算法數(shù)值計算的穩(wěn)定性。

[1] GROVES P D. Principles of GNSS, inertial, and multisensor integrated navigation systems[M]. London:Artech House, 2008.

[2] FOXLIN E. Head-tracking relative to a moving vehicle or simulator platform using differential inertial sensors[C]∥Proc.of the SPIE-International Society for Optical Engineering,2002:133-144.

[3] CARRILLO L R G, LPEZ A E D, LOZANO R, et al. Combining stereo vision and inertial navigation system for a quad-rotor UAV[J].Journal of Intelligent & Robotic Systems,2012,65(1): 373-387.

[4] CHAI L, HOFF W A, VINCENT T. 3-D motion and structure estimation using inertial sensors and computer vision for augmented reality[J]. Presence: Teleoperators and Virtual Environments, 2002, 11(5):474-492.

[5] GEBRE-EGZIABHER D, HAYWARD R C, POWELL J D. Design of multi-sensor attitude determination systems[J]. IEEE Trans.on Aerospace & Electronic Systems,2004,40(2):627-649.

[6] SUN C, SUN P, WANG P. An improvement of pose measurement method using global control points calibration[J]. Plos One, 2015, 10(7): e0133905.

[7] WANG P, XIAO X, ZHANG Z, et al. Study on the position and orientation measurement method with monocular vision system[J]. Chinese Optics Letters, 2010, 8(1): 55-58.

[8] GROVES P D. Navigation using inertial sensors[J]. IEEE Aerospace & Electronic Systems Magazine, 2015, 30(2):42-69.

[9] ENAYATI N. A quaternion-based unscented Kalman filter for robust optical/inertial motion tracking in computer-assisted surgery[J]. IEEE Trans.on Instrumentation & Measurement, 2015, 64(8): 2291-2301.

[10] ZHOU S, FEI F, ZHANG G, et al. 2D human gesture tracking and recognition by the fusion of MEMS inertial and vision sensors[J]. Sensors Journal IEEE, 2014, 14(4):1160-1170.

[11] YANG Z, SHEN S. Monocular visual-inertial state estimation with online initialization and camera-IMU extrinsic calibration[J]. IEEE Trans.on Automation Science & Engineering, 2017,14(1): 39-51.

[12] LIU Y, XIONG R, WANG Y, et al. Stereo visual-inertial odometry with multiple Kalman filters ensemble[J]. IEEE Trans.on Industrial Electronics, 2016, 63(10): 6205-6216.

[13] ARAAR O, AOUF N, VITANOV I. Vision based autonomous landing of multirotor UAV on moving platform[J]. Journal of Intelligent & Robotic Systems, 2016, 85:1-16.

[14] 趙琳,張勝宗,李亮,等.基于自適應(yīng)EKF的BDS/GPS精密單點定位方法[J].系統(tǒng)工程與電子技術(shù),2016,38(9):2142-2148.

ZHAO L, ZHANG S Z, LI L, et al. BDS/GPS integrated precise point positioning based on adaptive extended Kalamn filter[J]. Systems Engineering and Electronics,2016,38(9):2142-2148.

[15] 喬相偉,周衛(wèi)東,吉宇人.用四元數(shù)狀態(tài)切換無跡卡爾曼濾波器估計的飛行器姿態(tài)[J].控制理論與應(yīng)用,2012,29(1):97-103.

QIAO X W, ZHOU W D, JI Y R. Aircraft attitude estimation based on quaternion state-switching unscented Kalman filter[J]. Control Theory & Applications, 2012, 29(1): 97-103.

[16] 黃湘遠(yuǎn),湯霞清,武萌.基于降維CKF和平滑的SINS/OD動基座對準(zhǔn)[J].系統(tǒng)工程與電子技術(shù),2016,38(9):2135-2141.

HUANG X Y, TANG X Q, WU M. Research on moving base initial alignment of SINS/OD with reduced dimension CKF smoo-ther[J]. Systems Engineering and Electronics, 2016, 38(9):2135-2141.

[17] ARASARATNAM I, HAYKIN S. Cubature kalman filters[J]. IEEE Trans.on Automatic Control, 2009, 54(6): 1254-1269.

[18] 張秋昭, 張書畢, 鄭南山,等. GPS/INS組合系統(tǒng)的多重漸消魯棒容積卡爾曼濾波[J]. 中國礦業(yè)大學(xué)學(xué)報, 2014, 43(1):162-168.

ZHANG Q Z, ZHANG S B, ZHENG N S, et al. Multiple fading robust cubature Kalman filter for DPS/INS integrated navigation[J]. Journal of China University of Mining and Technology, 2014, 43(1):162-168.

[19] LIM J. A tutorial-game theory-based extended H infinity filtering approach to nonlinear problems in signal processing[J]. Digital Signal Processing, 2014, 34(1): 1-15.

[20] 趙利強(qiáng), 陳坤云, 王建林, 等. 基于矩陣對角化變換的高階容積卡爾曼濾波[J]. 控制與決策, 2016, 31(6): 1080-1086.

ZHAO L Q, CHEN K Y, WANG J L, et al. High-degree cubature Kalman filter based on diagonalization of matrix[J]. Control and Decision,2016, 31(6): 1080-1086.

[21] ZHANG Y, YANG X, XING X, et al. The standing calibration method of MEMS gyro bias for autonomous pedestrian navigation system[J].Journal of Navigation,2016,70(3):607-617.