孫長庫,黃 璐,王 鵬,郭肖亭

(天津大學(xué)精密測試技術(shù)及儀器國家重點實驗室,天津 300072)

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

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

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

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

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

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

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

1.1 雙IMU測量

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

(1)

(2)

(3)

1.2 視覺測量

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

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

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

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

(4)

(5)

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

(6)

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

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

(7)

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

(8)

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

(9)

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

(10)

(11)

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

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

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

2.1 標(biāo)定矩陣

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

(12)

組合移項得:

(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所對應(yīng)的特征向量即最優(yōu)四元數(shù)qopt。

Kqopt=λmaxqopt

(19)

3 實驗與結(jié)果分析

搭建如圖3所示的運動平臺雙IMU與視覺組合測量系統(tǒng)。

圖3 實驗裝置

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

圖5 不同測量方法的輸出頻率對比

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

3.1 標(biāo)定實驗

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

圖6 轉(zhuǎn)臺運動時IMUb各軸輸出

標(biāo)定矩陣時,轉(zhuǎn)臺靜止于原點,令運動平臺隨機擺動。IMUb與IMUn的輸出如圖7所示。

圖7 搖擺臺任意運動時IMU各軸輸出

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

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

3.2 單軸運動實驗

令轉(zhuǎn)臺繞轉(zhuǎn)臺坐標(biāo)系的x軸以1°/s的速度勻速往返轉(zhuǎn)動,最大角度為60°,從t=0時刻起開始測量?？柭鼮V波器的初始化參數(shù)為:

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

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

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

表2 t=1.74 s時各項輸出

表3 t=1.75 s時各項輸出

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

圖8 S值變化

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

(20)

圖9 單軸轉(zhuǎn)動測量結(jié)果

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

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

圖10 測量過程中相機采集的部分圖片

3.3 多軸運動實驗

令轉(zhuǎn)臺以2°/s的速度繞x、z軸交錯往返轉(zhuǎn)動,具體運動軌跡為: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°。測量誤差計算方式與3.2相同,測量結(jié)果如圖11所示。

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

(21)

圖11 多軸轉(zhuǎn)動測量結(jié)果

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

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

4 結(jié)論

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