徐丹旸， 宋 瀟

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

0 引言

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

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

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

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

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

圖1所示為基于視覺(jué)/雙陀螺儀傳感器的運(yùn)動(dòng)基座姿態(tài)測(cè)量系統(tǒng)。

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

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

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

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

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

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

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

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

(1)

(2)

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

2.1 標(biāo)定

標(biāo)定中，主陀螺儀和視覺(jué)靶標(biāo)相對(duì)位置關(guān)系保持不變。在適合的距離，正對(duì)于視覺(jué)靶標(biāo)安裝一個(gè)高分辨率相機(jī)。在標(biāo)定過(guò)程中，相機(jī)保持靜止，主陀螺儀和靶標(biāo)一起做多個(gè)自由度的運(yùn)動(dòng)。在這個(gè)過(guò)程中，相機(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分別表示兩種計(jì)算方式。結(jié)合式(6)、式(7)和式(8)，可得

(11)

(12)

式中:矩陣A為

(13)

W=diag(w1,w2,…,wK),是為了防止視覺(jué)誤差導(dǎo)致計(jì)算不準(zhǔn)確而引入的權(quán)重矩陣,權(quán)重系數(shù)可以由估計(jì)誤差來(lái)調(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)定過(guò)程中，輔陀螺儀和相機(jī)位置關(guān)系始終保持不變。在適合的距離正對(duì)相機(jī)固定放置一個(gè)高精度靶標(biāo)。在標(biāo)定過(guò)程中，靶標(biāo)始終保持靜止，輔陀螺儀和相機(jī)一起做多自由度運(yùn)動(dòng)。整個(gè)過(guò)程中，相機(jī)不斷拍攝靶標(biāo)圖像，輔陀螺儀輸出角速度值。

(18)

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

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

(19)

(20)

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

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

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

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

(21)

(22)

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

(23)

(24)

(25)

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

(26)

(27)

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

(28)

式中：

(29)

(30)

(31)

(32)

(33)

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

(34)

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

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

(35)

(36)

容積點(diǎn)為

(37)

(38)

(39)

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

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

(40)

(41)

(42)

7) 計(jì)算信息

(43)

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

(44)

式中:

(45)

(46)

(47)

(48)

(49)

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

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

(50)

(51)

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

(52)

(53)

(54)

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

(55)

4 實(shí)驗(yàn)測(cè)試及結(jié)果分析

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

圖3 測(cè)量系統(tǒng)實(shí)驗(yàn)平臺(tái)Fig.3 Experimental platform of measurement system

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

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

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

轉(zhuǎn)臺(tái)實(shí)際轉(zhuǎn)動(dòng)角度和視覺(jué)測(cè)量結(jié)果見圖4。

圖4 轉(zhuǎn)臺(tái)實(shí)際轉(zhuǎn)角和視覺(jué)測(cè)量Fig.4 Rotation angle and the vision measurement

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

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

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

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

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

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

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

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

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

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

5 結(jié)論

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