運(yùn)動(dòng)平臺(tái)雙IMU與視覺(jué)組合姿態(tài)測(cè)量算法*

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

(天津大學(xué)精密測(cè)試技術(shù)及儀器國(guó)家重點(diǎn)實(shí)驗(yàn)室,天津 300072)

姿態(tài)測(cè)量在商業(yè)與軍事領(lǐng)域都具有重要作用。常見(jiàn)的姿態(tài)測(cè)量方法包括慣性測(cè)量單元IMU(Inertial Measurement Unit)測(cè)量[1-2]、視覺(jué)測(cè)量[3-4]以及磁強(qiáng)計(jì)測(cè)量[5]。IMU測(cè)量速度快,短期精度高,但其誤差會(huì)隨著時(shí)間發(fā)散,對(duì)于低精度MEMS IMU來(lái)說(shuō)誤差發(fā)散更為明顯,一般需要其他測(cè)量方法進(jìn)行不斷校正。視覺(jué)測(cè)量精度較高,穩(wěn)定性較好,但其采樣頻率受相機(jī)拍攝幀率限制,測(cè)量速度較慢,且測(cè)量的范圍受標(biāo)記點(diǎn)的限制,當(dāng)標(biāo)記點(diǎn)被遮擋時(shí)無(wú)法實(shí)現(xiàn)測(cè)量。磁強(qiáng)計(jì)測(cè)量精度高,但容易受到周?chē)艌?chǎng)變化影響。

以上幾種測(cè)量方法各有特點(diǎn),憑借單一的測(cè)量方法實(shí)現(xiàn)快速準(zhǔn)確的姿態(tài)測(cè)量難度較大,采用高精度器件來(lái)提升精度的話成本會(huì)大大提高,因此出現(xiàn)了許多組合姿態(tài)測(cè)量[6-8]方法,例如IMU與視覺(jué)組合測(cè)量,IMU與磁強(qiáng)計(jì)組合測(cè)量等。Hugo[9]研究了一種慣性與視覺(jué)組合頭部跟蹤系統(tǒng)。該方法利用視覺(jué)測(cè)量對(duì)慣性測(cè)量校準(zhǔn),在視覺(jué)測(cè)量周期內(nèi)慣性獨(dú)立進(jìn)行測(cè)量,在視覺(jué)測(cè)量輸出結(jié)果時(shí)將兩者測(cè)量結(jié)果做差,將差值作為下一個(gè)周期內(nèi)的慣性測(cè)量的補(bǔ)償量。其融合方法較為簡(jiǎn)單直接,其結(jié)果依賴于視覺(jué)測(cè)量的精度。王康友[10]對(duì)室內(nèi)近距離的慣性和視覺(jué)組合的定姿技術(shù)進(jìn)行了研究。將相機(jī)與IMU固定連接,利用多速率卡爾曼濾波器解決了視覺(jué)測(cè)量與慣性測(cè)量周期的不匹配問(wèn)題。相較于直接做差補(bǔ)償,使用卡爾曼濾波器對(duì)測(cè)量值進(jìn)行融合[11]更充分利用了視覺(jué)和慣性測(cè)量結(jié)果中的信息。

現(xiàn)有的IMU與視覺(jué)組合測(cè)量系統(tǒng)中,大多為測(cè)量物體相對(duì)于慣性坐標(biāo)系的姿態(tài),例如測(cè)量物體相對(duì)于地面的姿態(tài)。而在實(shí)際應(yīng)用中,有時(shí)需將物體放置在特定平臺(tái)上,測(cè)量物體相對(duì)于平臺(tái)的姿態(tài),如機(jī)艙中飛行員頭部相對(duì)于機(jī)艙的姿態(tài)[12]。當(dāng)平臺(tái)處于運(yùn)動(dòng)狀態(tài)時(shí),固定在物體上的IMU的輸出包含了平臺(tái)的運(yùn)動(dòng)信息,會(huì)對(duì)測(cè)量結(jié)果造成干擾。對(duì)此,本文將另一IMU固定在運(yùn)動(dòng)平臺(tái)上,組成雙IMU慣性測(cè)量系統(tǒng),通過(guò)兩運(yùn)動(dòng)坐標(biāo)系之間的歐拉角微分方程計(jì)算物體相對(duì)于運(yùn)動(dòng)平臺(tái)的姿態(tài)。研究了雙IMU與視覺(jué)組合姿態(tài)測(cè)量算法,針對(duì)IMU與視覺(jué)測(cè)量結(jié)果的融合問(wèn)題,提出了一種多速率自適應(yīng)拓展卡爾曼濾波算法來(lái)進(jìn)行融合,將IMU的高速率與視覺(jué)的穩(wěn)定性結(jié)合起來(lái),實(shí)現(xiàn)了物體相對(duì)于運(yùn)動(dòng)平臺(tái)快速準(zhǔn)確的姿態(tài)測(cè)量。

1 雙IMU與視覺(jué)組合姿態(tài)測(cè)量原理

雙IMU與視覺(jué)組合姿態(tài)測(cè)量系統(tǒng)如圖1所示。慣性測(cè)量部分,將一IMU固定在轉(zhuǎn)臺(tái)(被測(cè)目標(biāo))上,測(cè)量轉(zhuǎn)臺(tái)的運(yùn)動(dòng)角速度,設(shè)為IMUb;另一IMU通過(guò)連接桿固定在運(yùn)動(dòng)平臺(tái)上,測(cè)量運(yùn)動(dòng)平臺(tái)的運(yùn)動(dòng)角速度,設(shè)為IMUn。視覺(jué)測(cè)量部分,將一帶有非共面標(biāo)記點(diǎn)的立體靶標(biāo)固定在轉(zhuǎn)臺(tái)上,相機(jī)通過(guò)連接桿固定在運(yùn)動(dòng)平臺(tái)上。轉(zhuǎn)臺(tái)基座固定在運(yùn)動(dòng)平臺(tái)上,設(shè)轉(zhuǎn)臺(tái)與運(yùn)動(dòng)平臺(tái)的初始姿態(tài)為零,測(cè)量時(shí)轉(zhuǎn)臺(tái)所轉(zhuǎn)動(dòng)的姿態(tài)歐拉角即為被測(cè)目標(biāo)。

圖1 雙IMU與視覺(jué)組合姿態(tài)測(cè)量系統(tǒng)

設(shè)IMUb坐標(biāo)系為ob-xbybzb,IMUn坐標(biāo)系為on-xnynzn,方向與IMU各敏感軸相同。目標(biāo)坐標(biāo)系為oz-xzyzzz,方向與轉(zhuǎn)臺(tái)各運(yùn)動(dòng)軸相同。平臺(tái)(參考)坐標(biāo)系為op-xpypzp,與轉(zhuǎn)臺(tái)位于原點(diǎn)時(shí)的目標(biāo)坐標(biāo)系相同。相機(jī)坐標(biāo)系為ov-xvyvzv,z軸與相機(jī)光軸重合。慣性坐標(biāo)系為oi-xiyizi,在空間中固定不動(dòng)。

1.1 雙IMU測(cè)量

IMU的角速度原始輸出受自身制造工藝、外界溫度以及噪聲等因素影響,存在一定的誤差[13-14]。在讀取IMU輸出后,減去陀螺儀零偏并采用指數(shù)加權(quán)平均法進(jìn)行平滑濾波,減小噪聲引起的波動(dòng)。

(1)

(2)

(3)

1.2 視覺(jué)測(cè)量

視覺(jué)測(cè)量技術(shù)根據(jù)所使用相機(jī)個(gè)數(shù)分為單目測(cè)量與多目測(cè)量,其中單目測(cè)量系統(tǒng)結(jié)構(gòu)較簡(jiǎn)單且視場(chǎng)較大,因此本文采用單目測(cè)量方法。利用靶標(biāo)上的4個(gè)非共面紅外LED點(diǎn)作為特征點(diǎn)。在得到圖像中各特征點(diǎn)的圖像坐標(biāo)以及對(duì)應(yīng)的空間坐標(biāo)后,采用較為成熟的POSIT[15]算法進(jìn)行計(jì)算求解姿態(tài)。

1.3 多速率自適應(yīng)拓展卡爾曼濾波算法

得到雙IMU與視覺(jué)測(cè)量的結(jié)果后,采用多速率自適應(yīng)拓展卡爾曼濾波(MAEKF)算法進(jìn)行融合。與傳統(tǒng)的EKF相比,使用衰減記憶[16]自適應(yīng)算法對(duì)濾波器是否發(fā)散進(jìn)行了判斷,若發(fā)散則增大融合過(guò)程中視覺(jué)測(cè)量的權(quán)重,使濾波器更好地克服發(fā)散問(wèn)題,獲得更好的融合效果。

設(shè)系統(tǒng)的狀態(tài)模型方程和測(cè)量模型方程為:

(4)

(5)

測(cè)量時(shí)IMU與視覺(jué)更新頻率不同,視覺(jué)測(cè)量頻率較低,IMU則頻率較高。將IMU測(cè)量頻率設(shè)為視覺(jué)測(cè)量頻率的整數(shù)倍,采用多速率濾波器融合不同頻率的數(shù)據(jù)。當(dāng)視覺(jué)測(cè)量無(wú)輸出時(shí),只進(jìn)行慣性計(jì)算以及卡爾曼濾波估計(jì)誤差方差陣P的迭代,輸出值為慣性測(cè)量值,當(dāng)視覺(jué)輸出數(shù)據(jù)時(shí)再進(jìn)行數(shù)據(jù)融合,輸出值為融合測(cè)量值。多速率濾波器的輸出如式(6)所示:

(6)

普通的卡爾曼濾波器在濾波時(shí)對(duì)于濾波收斂問(wèn)題考慮較少,當(dāng)濾波器參數(shù)以及初始值選擇(R、Q、P0)不合適時(shí),有時(shí)會(huì)導(dǎo)致濾波發(fā)散,使濾波偏離真值。引入評(píng)判濾波發(fā)散的標(biāo)準(zhǔn)可以為改善濾波器性能提供依據(jù)。將衰減記憶算法與卡爾曼濾波結(jié)合,在預(yù)測(cè)誤差方差陣中引入遺忘因子S。引入的遺忘因子S相較于傳統(tǒng)的加權(quán)衰減記憶濾波,其取值由殘差序列與其協(xié)方差陣決定,并引入濾波發(fā)散判斷機(jī)制,對(duì)卡爾曼濾波的發(fā)散起到更好的收斂效果。

當(dāng)視覺(jué)測(cè)量輸出結(jié)果時(shí),殘差序列:

(7)

為白噪聲序列,其協(xié)方差陣為:

(8)

將殘差序列平方和與協(xié)方差陣的跡的比較作為濾波發(fā)散判據(jù):

(9)

γ≥1,為發(fā)散系數(shù)。γ的大小決定了發(fā)散判斷的嚴(yán)格程度,當(dāng)γ=1時(shí)為嚴(yán)格發(fā)散判斷。若式(9)不成立則系統(tǒng)收斂,S=1,此時(shí)式(5)為普通的卡爾曼濾波方程,若式(9)成立則系統(tǒng)發(fā)散,需要對(duì)遺忘因子S進(jìn)行估計(jì)。利用嚴(yán)格發(fā)散判據(jù)進(jìn)行估計(jì),此時(shí):

(10)

(11)

整個(gè)卡爾曼濾波系統(tǒng)可分為三大部分:增益計(jì)算部分、測(cè)量更新部分與衰減記憶遺忘因子計(jì)算部分。增益部分中,根據(jù)IMU的角速度輸出值以及前一刻的姿態(tài)計(jì)算卡爾曼濾波器的增益矩陣、預(yù)測(cè)誤差方差陣與估計(jì)誤差方差陣。測(cè)量更新部分中,通過(guò)判斷IMU與視覺(jué)測(cè)量的計(jì)算時(shí)間是否相等來(lái)決定姿態(tài)更新輸出為慣性測(cè)量值還是融合測(cè)量值。遺忘因子部分中,根據(jù)殘差序列與其協(xié)方差陣比較來(lái)判斷濾波器是否發(fā)散,從而更新S值。系統(tǒng)的流程如圖2所示。

圖2 多速率自適應(yīng)拓展卡爾曼濾波流程圖

2 慣性測(cè)量坐標(biāo)系之間的標(biāo)定

2.1 標(biāo)定矩陣

IMUb與轉(zhuǎn)臺(tái)之間剛性連接,采用基于TRIAD[17]算法的正交雙矢量標(biāo)定法進(jìn)行標(biāo)定。正交化雙矢量標(biāo)定法在矢量選擇上使用兩個(gè)相互正交的矢量,與TRIAD算法相比少了一步叉積使矢量正交的步驟。

(12)

組合移項(xiàng)得:

(13)

2.2 標(biāo)定矩陣

(14)

(15)

ai為矢量的權(quán)重,設(shè)ai=1/n,得

(16)

g(q)=qTKq

(17)

(18)

式中:Z=[B23-B32,B31-B13,B12-B21]T,S=B+BT,s=trB。K的最大特征值λmax所對(duì)應(yīng)的特征向量即最優(yōu)四元數(shù)qopt。

Kqopt=λmaxqopt

(19)

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

搭建如圖3所示的運(yùn)動(dòng)平臺(tái)雙IMU與視覺(jué)組合測(cè)量系統(tǒng)。

圖3 實(shí)驗(yàn)裝置

圖4 測(cè)量靶標(biāo)的結(jié)構(gòu)

圖5 不同測(cè)量方法的輸出頻率對(duì)比

圖5中組合測(cè)量的輸出由兩種顏色的點(diǎn)組成,其中綠點(diǎn)為此刻組合測(cè)量的姿態(tài)輸出值由慣性測(cè)量部分決定,黑點(diǎn)為此刻時(shí)組合測(cè)量的姿態(tài)輸出值由濾波融合的結(jié)果決定,黑點(diǎn)與視覺(jué)測(cè)量輸出時(shí)間相同,即當(dāng)視覺(jué)存在輸出時(shí)組合測(cè)量的輸出為濾波融合值。

3.1 標(biāo)定實(shí)驗(yàn)

在測(cè)量開(kāi)始前需要進(jìn)行坐標(biāo)系標(biāo)定。標(biāo)定矩陣時(shí),運(yùn)動(dòng)平臺(tái)靜止,轉(zhuǎn)臺(tái)分別繞x軸與z軸以1 °/s的速度勻速運(yùn)動(dòng)30 s。此時(shí)IMUb各軸的輸出如圖6 所示。

圖6 轉(zhuǎn)臺(tái)運(yùn)動(dòng)時(shí)IMUb各軸輸出

標(biāo)定矩陣時(shí),轉(zhuǎn)臺(tái)靜止于原點(diǎn),令運(yùn)動(dòng)平臺(tái)隨機(jī)擺動(dòng)。IMUb與IMUn的輸出如圖7所示。

圖7 搖擺臺(tái)任意運(yùn)動(dòng)時(shí)IMU各軸輸出

各標(biāo)定矩陣計(jì)算結(jié)果如表1所示。

表1 標(biāo)定矩陣計(jì)算結(jié)果

3.2 單軸運(yùn)動(dòng)實(shí)驗(yàn)

令轉(zhuǎn)臺(tái)繞轉(zhuǎn)臺(tái)坐標(biāo)系的x軸以1°/s的速度勻速往返轉(zhuǎn)動(dòng),最大角度為60°,從t=0時(shí)刻起開(kāi)始測(cè)量。卡爾曼濾波器的初始化參數(shù)為:

以t從1.74 s到1.75 s時(shí)多速率自適應(yīng)拓展卡爾曼濾波的的更新為例,分析濾波過(guò)程中各個(gè)變量的更新情況。

t=1.74 s時(shí),視覺(jué)無(wú)輸出,慣性部分獨(dú)立進(jìn)行測(cè)量,增益計(jì)算部分計(jì)算卡爾曼濾波器的增益矩陣K、預(yù)測(cè)誤差方差陣Pk,k-1與估計(jì)誤差方差陣Pk,此時(shí)測(cè)量更新部分的輸出為慣性測(cè)量值,衰減記憶遺忘因子計(jì)算部分無(wú)更新。各項(xiàng)結(jié)果如表2所示。

t=1.75 s時(shí),增益計(jì)算部分與t=1.74 s時(shí)相同,視覺(jué)有輸出,測(cè)量更新部分輸出融合測(cè)量值。遺忘因子部分根據(jù)濾波是否發(fā)散來(lái)更新S值,t=1.75 s時(shí)式(9)成立,根據(jù)式(11)更新S,各項(xiàng)結(jié)果如表3所示。

表2 t=1.74 s時(shí)各項(xiàng)輸出

表3 t=1.75 s時(shí)各項(xiàng)輸出

t=1.76 s時(shí),視覺(jué)無(wú)輸出,與t=1.74 s時(shí)的計(jì)算相同,慣性部分獨(dú)立進(jìn)行測(cè)量。以此類(lèi)推,直到t=2.0 s 時(shí)視覺(jué)再次有輸出,與t=1.75 s時(shí)的計(jì)算相同。整個(gè)測(cè)量過(guò)程中S值的變化如圖8所示,當(dāng)濾波發(fā)散即式(9)成立時(shí),根據(jù)式(11)更新S,其余時(shí)刻S=1。

圖8 S值變化

得到慣性、視覺(jué)單獨(dú)進(jìn)行測(cè)量的測(cè)量值以及組合測(cè)量值,以轉(zhuǎn)臺(tái)自身輸出的角度作為真值對(duì)測(cè)量值進(jìn)行誤差計(jì)算。如式(20)所示:

(20)

圖9 單軸轉(zhuǎn)動(dòng)測(cè)量結(jié)果

圖9中藍(lán)線為慣性測(cè)量結(jié)果,紅線為視覺(jué)測(cè)量結(jié)果,綠線為組合測(cè)量結(jié)果。由測(cè)量結(jié)果得知:慣性測(cè)量的姿態(tài)角誤差隨著時(shí)間發(fā)散,120 s后誤差達(dá)到3°以上,組合測(cè)量則很好的控制了濾波的發(fā)散,姿態(tài)角的輸出與視覺(jué)測(cè)量接近,并且更新頻率與慣性測(cè)量相同,保持較高頻率輸出。

當(dāng)轉(zhuǎn)臺(tái)運(yùn)動(dòng)到57°到60°和-50°到-60°的范圍時(shí),靶標(biāo)在相機(jī)的視場(chǎng)之外,此時(shí)視覺(jué)無(wú)法進(jìn)行測(cè)量,無(wú)姿態(tài)輸出,圖10(c)即部分特征點(diǎn)在相機(jī)視場(chǎng)外時(shí)所采集的圖像。此時(shí)慣性部分獨(dú)立進(jìn)行測(cè)量,組合測(cè)量的輸出由慣性部分決定,圖9(a)中紅圈部分的測(cè)量結(jié)果放大后如圖9(b)所示。由圖可知,在視覺(jué)測(cè)量失效的短時(shí)間內(nèi)組合測(cè)量能較好地跟蹤轉(zhuǎn)臺(tái)的姿態(tài)變化,誤差發(fā)散在可接受范圍,可以認(rèn)為系統(tǒng)正常工作。因此,組合測(cè)量與單獨(dú)的視覺(jué)測(cè)量相比,不僅提高了測(cè)量速度,還擴(kuò)大了測(cè)量范圍。

圖10 測(cè)量過(guò)程中相機(jī)采集的部分圖片

3.3 多軸運(yùn)動(dòng)實(shí)驗(yàn)

令轉(zhuǎn)臺(tái)以2°/s的速度繞x、z軸交錯(cuò)往返轉(zhuǎn)動(dòng),具體運(yùn)動(dòng)軌跡為:x軸正向旋轉(zhuǎn)30°→z軸正向旋轉(zhuǎn)30°→x軸負(fù)向旋轉(zhuǎn)60°→z軸負(fù)向旋轉(zhuǎn)60°→x軸正向旋轉(zhuǎn)30°→z軸正向旋轉(zhuǎn)30°。測(cè)量誤差計(jì)算方式與3.2相同,測(cè)量結(jié)果如圖11所示。

測(cè)量結(jié)果誤差的均方根誤差(RMSE)的計(jì)算如式(21)所示,均方根誤差與誤差絕對(duì)值的最大值如表4所示。

(21)

圖11 多軸轉(zhuǎn)動(dòng)測(cè)量結(jié)果

IMU測(cè)量視覺(jué)測(cè)量組合測(cè)量RMSE/(°)俯仰角1.405 40.326 50.327 7方位角1.257 20.212 30.220 2誤差絕對(duì)值俯仰角2.689 70.704 70.724 8的最大值/(°)方位角2.076 30.472 60.502 5

由圖11和表4得知:在多軸運(yùn)動(dòng)的情況下的測(cè)量結(jié)果與單軸運(yùn)動(dòng)類(lèi)似,組合測(cè)量結(jié)果的準(zhǔn)確度優(yōu)于IMU測(cè)量,與視覺(jué)測(cè)量接近,測(cè)量頻率與IMU測(cè)量相同,遠(yuǎn)高于視覺(jué)測(cè)量。在整個(gè)運(yùn)動(dòng)過(guò)程中組合測(cè)量誤差的姿態(tài)角均方差在0.35°以內(nèi),證明了系統(tǒng)在測(cè)量運(yùn)動(dòng)復(fù)雜的被測(cè)目標(biāo)時(shí)也能保持一定的穩(wěn)定性與精度。

4 結(jié)論

本文研究了運(yùn)動(dòng)平臺(tái)上雙IMU與視覺(jué)組合姿態(tài)測(cè)量算法。利用固定在平臺(tái)上的IMUn補(bǔ)償平臺(tái)運(yùn)動(dòng)對(duì)姿態(tài)測(cè)量的影響,通過(guò)歐拉角微分方程計(jì)算相對(duì)姿態(tài),并采用正交雙矢量標(biāo)定法與q-method標(biāo)定慣性測(cè)量中的轉(zhuǎn)換矩陣。視覺(jué)測(cè)量采用POSIT算法計(jì)算姿態(tài)。提出了一種多速率自適應(yīng)拓展卡爾曼濾波算法,用于融合IMU與視覺(jué)的測(cè)量結(jié)果。搭建了雙IMU與視覺(jué)組合姿態(tài)測(cè)量系統(tǒng)并進(jìn)行了實(shí)驗(yàn)論證。實(shí)驗(yàn)證明,組合姿態(tài)測(cè)量算法實(shí)現(xiàn)了快速高精度的姿態(tài)測(cè)量。相較于慣性測(cè)量,組合測(cè)量抑制了誤差發(fā)散,可保持長(zhǎng)時(shí)間穩(wěn)定工作;相較于視覺(jué)測(cè)量,組合測(cè)量提高了計(jì)算速度并增大了測(cè)量范圍。在多軸運(yùn)動(dòng)的情況下,組合測(cè)量的姿態(tài)角均方差在0.35°以內(nèi)。