動基座視覺雙慣性姿態(tài)測量數(shù)據(jù)融合方法研究

徐丹旸， 宋 瀟

(河南科技大學(xué)，河南 洛陽 471000)

0 引言

基于視覺和慣性數(shù)據(jù)融合的姿態(tài)確定方法已成功應(yīng)用于無人地面車輛(UGV)、頭部運動跟蹤、姿態(tài)跟蹤等領(lǐng)域[1-2]，其設(shè)計均考慮了單傳感器的特點。視覺傳感器沒有累積誤差，測量穩(wěn)定性高，但存在視線和環(huán)境光干擾問題[3]；慣性傳感器具有重量輕、適合快速運動跟蹤的特點，但由于存在嚴(yán)重的漂移問題而缺乏長期穩(wěn)定性[4]。因此，通常采用視覺和慣性傳感器融合技術(shù)來克服單一傳感器固有的缺點。

目前的視覺/慣性組合系統(tǒng)多為相機(jī)與單慣性傳感器的組合[5-8]。當(dāng)目標(biāo)物在一個運動的基座上做相對運動時，其相對于運動基座的運動與基座本身的運動耦合在一起，無法通過單一陀螺儀有效地分離開來，所以，單慣性傳感器無法滿足動基座上相對姿態(tài)測量的要求[9]。本文介紹并搭建了基于視覺和雙陀螺儀的姿態(tài)測量系統(tǒng)。在該系統(tǒng)中，慣性測量由兩個陀螺儀共同完成：其中一個陀螺儀與運動基座固連，作為輔陀螺儀，用以測量運動基座的角速度；另外一個陀螺儀與目標(biāo)物固連，作為主陀螺儀，用以測量運動基座和主陀螺儀的耦合角速度。通過對兩個陀螺儀的角速度進(jìn)行差分計算，可以從主陀螺儀數(shù)據(jù)中剔除運動基座的干擾角速度。在融合視覺測量與慣性測量之前，各個器件的輸出系統(tǒng)統(tǒng)一變換到參考系下進(jìn)行，涉及多個坐標(biāo)系的轉(zhuǎn)換。為此，本文給出了系統(tǒng)中坐標(biāo)系歸一化方案。

卡爾曼濾波是傳感器融合的常用方法[10-13]，但其僅適用于線性系統(tǒng)，而在視覺與慣性融合中，系統(tǒng)方程式是非線性的。為了解決非線性問題，文獻(xiàn)[14]提出了一種基于采樣點的卡爾曼濾波，即容積卡爾曼濾波。利用一組確定性采樣點來捕獲系統(tǒng)的相關(guān)統(tǒng)計參量，從而避免對非線性系統(tǒng)方程的線性化，提高了濾波算法的穩(wěn)定性。針對所搭建系統(tǒng)，本文設(shè)計了一種容積卡爾曼濾波，有效地實現(xiàn)了對姿態(tài)角的估計。

本文搭建了基于視覺/雙陀螺儀傳感器的動基座姿態(tài)測量系統(tǒng)，建立了測量系統(tǒng)的坐標(biāo)系，并提出全局坐標(biāo)系歸一化方案，實現(xiàn)了不同坐標(biāo)系下傳感器數(shù)據(jù)的統(tǒng)一。利用容積卡爾曼濾波對視覺和慣性結(jié)果進(jìn)行融合，實現(xiàn)了高精度、大范圍快速、穩(wěn)定的姿態(tài)測量，最后將該算法在實驗平臺上進(jìn)行測試，驗證其有效性。

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

圖1所示為基于視覺/雙陀螺儀傳感器的運動基座姿態(tài)測量系統(tǒng)。

轉(zhuǎn)臺模擬被測目標(biāo)物，其上固定連接有主陀螺儀和視覺靶標(biāo)，當(dāng)轉(zhuǎn)臺轉(zhuǎn)動時，主陀螺儀和視覺靶標(biāo)做同步跟隨運動。相機(jī)拍攝立體靶標(biāo)圖像，利用姿態(tài)解算算法，通過圖像中特征點坐標(biāo)完成視覺姿態(tài)解算。當(dāng)基座運動時，主陀螺儀輸出的角速度中包括轉(zhuǎn)臺轉(zhuǎn)動角速度和基座運動角速度。而輔陀螺儀僅感知到基座的運動并輸出相應(yīng)的角速度。對主陀螺儀和輔陀螺儀輸出角速度進(jìn)行差分計算，便可將運動基座的運動信息從主陀螺儀輸出角速度中剔除，得到轉(zhuǎn)臺相對于運動基座的轉(zhuǎn)動角速度。慣性和視覺測量在不同的坐標(biāo)系中進(jìn)行，必須將不同的測量統(tǒng)一到相同的參考坐標(biāo)系下。

圖1 姿態(tài)測量系統(tǒng)Fig.1 Attitude measurement system

1) 定義目標(biāo)坐標(biāo)系為b系，表示為ObXbYbZb。目標(biāo)坐標(biāo)系與轉(zhuǎn)臺固連，隨著轉(zhuǎn)臺的轉(zhuǎn)動而做同步轉(zhuǎn)動。

2) 定義相機(jī)坐標(biāo)系為c系，表示為OcXcYcZc。原點Oc為相機(jī)鏡頭的光學(xué)中心，OcZc軸與鏡頭光軸方向一致。

3) 定義視覺靶標(biāo)坐標(biāo)系為t系，表示為OtXtYtZt。本文中,靶標(biāo)坐標(biāo)系即為對靶標(biāo)LED點坐標(biāo)測量的雙目測量系統(tǒng)的坐標(biāo)系。

4) 定義主陀螺儀坐標(biāo)系為m系，表示為OmXmYmZm。

5) 定義輔陀螺儀坐標(biāo)系為s系，表示為OsXsYsZs。

6) 定義參考坐標(biāo)系為r系，表示為OrXrYrZr。令參考坐標(biāo)系與轉(zhuǎn)臺位于零位時的轉(zhuǎn)臺坐標(biāo)系重合。與目標(biāo)坐標(biāo)系不同的是，參考坐標(biāo)系與運動基座固連，不隨轉(zhuǎn)臺的轉(zhuǎn)動而運動。

(1)

(2)

2 全局坐標(biāo)系歸一化

2.1 標(biāo)定

標(biāo)定中，主陀螺儀和視覺靶標(biāo)相對位置關(guān)系保持不變。在適合的距離，正對于視覺靶標(biāo)安裝一個高分辨率相機(jī)。在標(biāo)定過程中，相機(jī)保持靜止，主陀螺儀和靶標(biāo)一起做多個自由度的運動。在這個過程中，相機(jī)不斷拍攝靶標(biāo)圖像，標(biāo)定主陀螺儀輸出角速度[15]。圖2所示為靶標(biāo)系與主陀螺儀系標(biāo)定。

圖2 靶標(biāo)系與主陀螺儀系標(biāo)定Fig.2 Calibration of target coordinate and master gyro coordinate

(3)

(4)

(5)

(6)

由四元數(shù)相乘的性質(zhì)，可得

(7)

(8)

其中：

(9)

(10)

qw為四元數(shù)q的標(biāo)量部分;qv為四元數(shù)q的矢量部分;R,L分別表示兩種計算方式。結(jié)合式(6)、式(7)和式(8)，可得

(11)

(12)

式中:矩陣A為

(13)

W=diag(w1,w2,…,wK),是為了防止視覺誤差導(dǎo)致計算不準(zhǔn)確而引入的權(quán)重矩陣,權(quán)重系數(shù)可以由估計誤差來調(diào)整，若誤差越大，則該相鄰兩幀圖像的數(shù)據(jù)權(quán)重越小

(14)

2.2 標(biāo)定

(15)

由式(15)可以構(gòu)造矩陣方程

(16)

(17)

2.3 標(biāo)定

輔陀螺儀坐標(biāo)系到相機(jī)坐標(biāo)系的標(biāo)定，與主陀螺儀坐標(biāo)系到靶標(biāo)坐標(biāo)系的標(biāo)定類似。標(biāo)定過程中，輔陀螺儀和相機(jī)位置關(guān)系始終保持不變。在適合的距離正對相機(jī)固定放置一個高精度靶標(biāo)。在標(biāo)定過程中，靶標(biāo)始終保持靜止，輔陀螺儀和相機(jī)一起做多自由度運動。整個過程中，相機(jī)不斷拍攝靶標(biāo)圖像，輔陀螺儀輸出角速度值。

(18)

2.4 旋轉(zhuǎn)矩陣最優(yōu)正交化

由于旋轉(zhuǎn)矩陣具有正交性，所以在標(biāo)定和測量中，旋轉(zhuǎn)矩陣需要進(jìn)行最優(yōu)正交化。假設(shè)存在旋轉(zhuǎn)矩陣C，則對其最優(yōu)化的迭代算法為

(19)

(20)

成立時，完成迭代，把滿足式(20)的C(p+1)作為C的標(biāo)定結(jié)果。式中:fp與fp+1分別代表兩次相鄰迭代；ε為設(shè)定的最優(yōu)化閾值。

3 基于容積卡爾曼濾波的數(shù)據(jù)融合

在本系統(tǒng)中，狀態(tài)向量xk除了包括待估計的3個姿態(tài)角，還包括對2個陀螺儀的零偏。相對于歐拉角，四元數(shù)具有不存在萬向鎖的優(yōu)點，故狀態(tài)中的角度可以用四元數(shù)q來替代表示。由于四元數(shù)中參數(shù)冗余，用矢量部分qv來代替四元數(shù)進(jìn)行求解。取狀態(tài)向量xk=[qvk,bmk,bsk]T，是9×1維向量。具體濾波步驟如下。

1)計算容積點。

(21)

(22)

四元數(shù)和陀螺儀漂移部分容積點為

(23)

(24)

(25)

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

(26)

(27)

3) 計算k+1時刻的一步狀態(tài)預(yù)測值。因四元數(shù)求均值不能直接相加，可求得

(28)

式中：

(29)

(30)

(31)

(32)

(33)

4) 計算狀態(tài)誤差協(xié)方差矩陣

(34)

式中，Qk為預(yù)先設(shè)定的系統(tǒng)過程噪聲。

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

(35)

(36)

容積點為

(37)

(38)

(39)

其中，i=1,2,…,n。

6) 預(yù)測測量值為

(40)

(41)

(42)

7) 計算信息

(43)

8) 計算卡爾曼濾波增益為

(44)

式中:

(45)

(46)

(47)

(48)

(49)

其中，Rk為預(yù)先設(shè)定的觀測噪聲。

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

(50)

(51)

狀態(tài)中零偏和四元數(shù)部分更新為

(52)

(53)

(54)

10) 狀態(tài)誤差協(xié)方差矩陣更新為

(55)

4 實驗測試及結(jié)果分析

動基座視覺雙慣性姿態(tài)測量系統(tǒng)裝置如圖3所示。相機(jī)型號為Basler acA2040-120 μm， Sensonor STIM 300 MEMS IMU中的陀螺儀作為系統(tǒng)中的陀螺儀。相機(jī)和陀螺儀參數(shù)見表1。

圖3 測量系統(tǒng)實驗平臺Fig.3 Experimental platform of measurement system

表1 陀螺儀和相機(jī)參數(shù)

為了驗證測量的精度，利用一個高精度二維轉(zhuǎn)臺作為目標(biāo)運動物體，主陀螺儀、立體靶標(biāo)通過螺絲安裝在轉(zhuǎn)臺上，轉(zhuǎn)臺固定在搖擺臺上。相機(jī)和輔陀螺儀通過光學(xué)支撐棒固定在搖擺臺上。當(dāng)轉(zhuǎn)臺位于零位時，靶標(biāo)正對相機(jī)。實驗中使用的三維立體靶標(biāo)內(nèi)嵌4個非共面紅外LED點，波長為940 nm。通過對特征點的幾何特征等的篩選，可以排除背景中的干擾，實現(xiàn)對特征點的定位提取。

為了驗證本系統(tǒng)濾波算法的有效性，利用搭建的系統(tǒng)進(jìn)行角度測量實驗，該系統(tǒng)中轉(zhuǎn)臺用以模擬頭部運動姿態(tài)。對于實際的頭部運動，俯仰運動的范圍相對較小，而方位運動的范圍相對較大。所以在實驗中，轉(zhuǎn)臺從零位開始，俯仰角以15°為間隔，從-60°運動到60°。在每個間隔中，方位軸先從-100°運動到100°，再從100°運動到-100°，以20°為間隔往返運動。

轉(zhuǎn)臺實際轉(zhuǎn)動角度和視覺測量結(jié)果見圖4。

圖4 轉(zhuǎn)臺實際轉(zhuǎn)角和視覺測量Fig.4 Rotation angle and the vision measurement

由于本系統(tǒng)中的轉(zhuǎn)臺僅可以繞兩個軸進(jìn)行轉(zhuǎn)動，所以僅研究俯仰角度和方位角度的估計效果?？梢园l(fā)現(xiàn)，視覺測量的范圍有限，當(dāng)轉(zhuǎn)動角度大于一定程度時，相機(jī)就無法拍攝到4個特征點，視覺信息缺失。在實際應(yīng)用中，視線的模糊、環(huán)境的干擾等也會導(dǎo)致視覺中斷。除此之外，視覺測量的頻率較低。由于圖像處理、傳感器數(shù)據(jù)讀取等原因，視覺測量的頻率一般遠(yuǎn)低于慣性測量。僅依靠視覺測量，無法滿足實時測量的需求。

轉(zhuǎn)臺轉(zhuǎn)角的慣性測量結(jié)果見圖5。

圖5 轉(zhuǎn)臺轉(zhuǎn)角的慣性測量Fig.5 Inertial measurement of rotation angle

從圖5可知，僅利用陀螺儀去估計角度是不可行的，會在較短時間內(nèi)產(chǎn)生無法接受的誤差。陀螺儀輸出的角速度中包含著隨機(jī)噪聲和變化緩慢的零偏等誤差項。這些誤差項隨著時間不斷累積，會導(dǎo)致測量誤差越來越大，最后結(jié)果發(fā)散。所以，必須借助額外的信息對角偏移以及零偏進(jìn)行估計和補(bǔ)償。但是慣性測量具有不受視場等限制，測量速度快等優(yōu)點。

由上述分析可知，視覺和慣性是高度互補(bǔ)的兩種測量手段。將視覺測量和慣性測量進(jìn)行數(shù)據(jù)融合，不僅能解決視場限制的問題，還能提高測量穩(wěn)定性和測量速度。本系統(tǒng)中，視覺輸出的頻率為50 Hz，陀螺儀輸出頻率為1000 Hz，將二者通過容積卡爾曼濾波進(jìn)行多頻率數(shù)據(jù)融合，可以達(dá)到與陀螺儀輸出相同的測量速度。

角度測量的融合結(jié)果和融合誤差如圖6所示。

圖6 融合結(jié)果和融合誤差Fig.6 Fusion results and errors

兩臺陀螺儀的零偏初值均設(shè)為零，在測量初始時，誤差存在一個峰值，這是由于在濾波對于零偏的估計還沒有收斂的情況下，視覺的突然消失導(dǎo)致了陀螺儀角速度中的零偏無法被充分補(bǔ)償，產(chǎn)生較快的誤差累積。但幾十秒后，零偏估計逐漸接近真值，即使在視覺不存在的時間段中，基于純慣性的角度估計誤差增加也比較緩慢。這說明，算法能夠有效地利用視覺對兩臺陀螺儀的零偏進(jìn)行較為準(zhǔn)確的估計。整體來看，測量的絕對誤差在絕大多數(shù)時間內(nèi)低于0.1°，精度較高。表2中列出了測量角度的均方根誤差。

表2 單軸運動不同算法的姿態(tài)角均方根誤差

由于初始時刻陀螺儀零偏不準(zhǔn)導(dǎo)致的誤差，使得整體融合的精度下降，但是即使在這種情況下，融合結(jié)果的均方根誤差仍小于0.1°，而在零偏估計收斂后，融合的精度得到了大幅度的提高。由于實驗中視覺的測量范圍較小，且存在很多純慣性測量的區(qū)間，所以方位角的融合誤差大于純視覺誤差，但俯仰角在零偏穩(wěn)定后的精度高于視覺精度。這個結(jié)果證明了融合視覺和慣性不僅能擴(kuò)大測量范圍、提高測量速度和穩(wěn)定性，還可以在一定程度上提高測量精度。

5 結(jié)論

為解決運動基座上的姿態(tài)測量問題，本文研究了一種基于視覺和雙陀螺儀的姿態(tài)測量系統(tǒng)，并對測量系統(tǒng)中的坐標(biāo)系進(jìn)行了定義，提出了坐標(biāo)系全局歸一化的方法，并利用容積卡爾曼濾波對視覺和慣性測量數(shù)據(jù)進(jìn)行融合，實現(xiàn)了高精度、大范圍、快速、穩(wěn)定的姿態(tài)測量。實驗證明，數(shù)據(jù)融合能夠?qū)崿F(xiàn)視覺與慣性兩種傳感器的優(yōu)勢互補(bǔ)，并有效地對慣性角度誤差累積和兩臺陀螺儀零偏進(jìn)行校正補(bǔ)償，在俯仰角范圍為±60°、方位角范圍為±100°的大范圍動基座測量實驗中，兩個姿態(tài)角融合結(jié)果的均方根誤差均不超過0.1°。