矩陣對(duì)角化變換魯棒QCKF在視覺(jué)和慣性融合姿態(tài)測(cè)量中的應(yīng)用

郭肖亭, 孫長(zhǎng)庫(kù),2, 王 鵬,2

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

0 引 言

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

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

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

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

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

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

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

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

(1)

(2)

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

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

2.1 視覺(jué)姿態(tài)測(cè)量

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

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

θx=θbase+θbase-x

(3)

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

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

2.2 陀螺儀模型

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

(4)

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

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

2.3 矩陣對(duì)角化變換

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

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

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

(5)

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

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

(6)

引入約束條件γ,有

(7)

式(7)可改寫為

(8)

(9)

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

Pk+1=Pk+1|k-

(10)

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

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

步驟1計(jì)算容積點(diǎn)

(11)

(12)

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

(13)

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

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

(14)

(15)

步驟3計(jì)算k+1時(shí)刻的狀態(tài)預(yù)測(cè)值

(16)

式中

(17)

(18)

(19)

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

(20)

式中

步驟5計(jì)算更新后的狀態(tài)容積點(diǎn)

(21)

(22)

步驟6計(jì)算經(jīng)過(guò)測(cè)量方程的容積點(diǎn)

(23)

(24)

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

步驟7計(jì)算k+1時(shí)刻的測(cè)量預(yù)測(cè)值

(25)

(26)

(27)

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

(28)

(29)

步驟9計(jì)算k+1時(shí)刻的濾波增益矩陣

(30)

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

(31)

(32)

(33)

(34)

(35)

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

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

3 實(shí)驗(yàn)驗(yàn)證及結(jié)果分析

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

圖5 測(cè)量系統(tǒng)實(shí)驗(yàn)平臺(tái)Fig.5 Measurement system experimental platform

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

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

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

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

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

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

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

?

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié) 論

基于視覺(jué)和慣性融合姿態(tài)測(cè)量的目的,搭建測(cè)量系統(tǒng)平臺(tái),對(duì)測(cè)量涉及的坐標(biāo)系之間的關(guān)系進(jìn)行了梳理和標(biāo)定,目的是將不同的傳感器測(cè)量值統(tǒng)一到相同的坐標(biāo)系中,為進(jìn)一步的融合計(jì)算做準(zhǔn)備。運(yùn)動(dòng)物體在單軸短時(shí)間運(yùn)動(dòng)以及雙軸長(zhǎng)時(shí)間運(yùn)動(dòng)的不同運(yùn)動(dòng)形式中,使用視覺(jué)法、慣性法、CKF、矩陣對(duì)角化魯棒CKF法對(duì)姿態(tài)角進(jìn)行計(jì)算并對(duì)姿態(tài)誤差角進(jìn)行對(duì)比。實(shí)驗(yàn)結(jié)果表明,魯棒CKF算法有效地利用視覺(jué)測(cè)量穩(wěn)定性好的特性來(lái)抑制慣性測(cè)量誤差隨時(shí)間無(wú)約束發(fā)散并結(jié)合慣性測(cè)量更新頻率高的特點(diǎn),極大地提高融合姿態(tài)的輸出頻率。改進(jìn)的魯棒CKF在抑制視覺(jué)測(cè)量的尖峰噪聲、慣性測(cè)量的抖動(dòng)噪聲均明顯優(yōu)于CKF濾波算法。使用矩陣對(duì)角化變換代替標(biāo)準(zhǔn)CKF算法中的Cholesky分解,可以避免在狀態(tài)誤差協(xié)方差矩陣非正定時(shí)濾波中斷的情況,增加融合濾波算法數(shù)值計(jì)算的穩(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精密單點(diǎn)定位方法[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)切換無(wú)跡卡爾曼濾波器估計(jì)的飛行器姿態(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動(dòng)基座對(duì)準(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]. 中國(guó)礦業(yè)大學(xué)學(xué)報(bào), 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), 陳坤云, 王建林, 等. 基于矩陣對(duì)角化變換的高階容積卡爾曼濾波[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.