基于乘性擴展卡爾曼濾波的水下機器人姿態(tài)估計*

唐 軍， 王子夢， 羅瑞智

(江西理工大學 機電工程學院，江西 贛州 341000)

0 引 言

隨著低成本慣性傳感器的出現(xiàn)，特別是基于微機電系統(tǒng)(micro-electro-mechanical system,MEMS)的慣性傳感器[1]的出現(xiàn)，姿態(tài)估計[2]技術(shù)的應(yīng)用領(lǐng)域大大拓寬，廣泛應(yīng)用于航天器、機器人等領(lǐng)域。對于纜控水下機器人(remotely operated vehicle,ROV)[3]來說，實時精準的姿態(tài)信息是其控制和決策的基礎(chǔ)。ROV航姿參考系統(tǒng)(attitude and heading reference system,AHRS)由陀螺儀、加速度計以及磁力計組合而成。其中，陀螺儀動態(tài)響應(yīng)快，測量精度高，但存在積分漂移問題；加速度計雖然不會產(chǎn)生積分誤差，但運行時數(shù)據(jù)噪聲特別大；磁力計極其容易受到外界金屬及電磁設(shè)備的干擾和影響。因此，使用姿態(tài)估計算法融合各傳感器的測量數(shù)據(jù)，實現(xiàn)優(yōu)勢互補，具有重要的研究意義。

常見的姿態(tài)估計算法有三種：梯度下降(gradient descent,GD)法[4]、顯性互補濾波(explicit complementary filtering,ECF)[5]以及卡爾曼濾波。擴展卡爾曼濾波(extended Kalman filtering,EKF)是目前最常用的非線性姿態(tài)估計算法。根據(jù)狀態(tài)向量中使用的姿態(tài)表示形式不同以及觀測量的形式不同，EKF的實現(xiàn)方式[6]有乘性擴展卡爾曼濾波(multiplicative EKF,MEKF)和加性擴展卡爾曼濾波(additive EKF,AEKF)。其中，MEKF[7]被視為姿態(tài)估計的標準算法，廣泛應(yīng)用于航空航天[8]、機器人[9]等領(lǐng)域，但在水下機器人領(lǐng)域的應(yīng)用較少。

因此，本文以水下機器人CHASING M2 ROV為研究對象，用四元數(shù)表示ROV姿態(tài)角度，建立MEMS傳感器測量模型，設(shè)計了基于MEKF的姿態(tài)估計算法。為驗證其可行性，采集ROV水下動態(tài)實驗數(shù)據(jù)進行融合，與AHRS獲得的參考值相比較，并在仿真環(huán)境下，將其與ECF,GD兩種算法分析對比。

1 四元數(shù)姿態(tài)表示

1.1 坐標系建立

機體坐標系obxbybzb與機體固連，如圖1所示。通常以地心作為坐標原點oe，以北、東、地方向為大地坐標系的正方向。規(guī)定機體旋轉(zhuǎn)的正方向為右手螺旋方向。機體繞obxb軸旋轉(zhuǎn)的角度即滾轉(zhuǎn)角φ(roll)，繞obyb軸旋轉(zhuǎn)的角度即俯仰角θ(pitch)，繞obzb軸旋轉(zhuǎn)的角度即偏航角ψ(yaw)。

選取的大地坐標系oexeyeze與機體坐標系obxbybzb的關(guān)系如圖2所示。

圖1 ROV機體坐標系

圖2 機體坐標系與大地坐標系的關(guān)系

1.2 姿態(tài)表示

(1)

四元數(shù)一般表示為

(2)

式中q0∈為q∈4的標量部分，qv=[q1q2q3]T∈3為向量部分。

從機體坐標系到大地坐標系的旋轉(zhuǎn)矩陣可以用四元數(shù)表示[10]為

(3)

最后，聯(lián)立式(1)、式(3)可得出四元數(shù)與歐拉角之間的轉(zhuǎn)換關(guān)系為

(4)

2 傳感器測量模型

2.1 陀螺儀測量模型

MEMS陀螺儀固連于機體內(nèi)，軸向與機體坐標系一致，測量的是三個機體軸方向的角速度。測量模型表示為

ω=ωm-ωb-ωn

(5)

式中ω∈3為角速度的真值；ωm∈3為角速度測量值；ωb∈3為漂移誤差；ωn∈3為陀螺儀測量噪聲向量，可以看成高斯白噪聲向量。

2.2 加速度計測量模型

MEMS加速度計固連于機體內(nèi)，其軸向與機體坐標系一致，測量的是三個機體軸向的比力分量。測量模型表示為

a=am-ab-an

(6)

式中a∈3為比力的真值；am∈3為加速度計測量值；ab∈3為漂移誤差；an∈3為加速度計測量噪聲向量，為高斯白噪聲向量。

2.3 磁力計測量模型

記磁場矢量為em∈3。磁力計固連于機體內(nèi)，其軸向于機體坐標系一致。它測量的是沿著不同方向軸的磁場矢量，記為bmm∈3。建立測量模型為

(7)

3 MEKF姿態(tài)估計

MEKF將真實狀態(tài)分為標稱狀態(tài)和誤差狀態(tài)兩部分。對兩種狀態(tài)分別進行估計，最后將結(jié)果進行疊加，以獲取真實估計。其原理如圖3所示。

圖3 MEKF算法原理框圖

3.1 狀態(tài)變量

定義真實狀態(tài)變量Xt,標稱狀態(tài)變量X,誤差狀態(tài)變量δX為

(8)

式中q，δq,δθ分別為四元數(shù)、誤差四元數(shù)以及誤差旋轉(zhuǎn)向量，ωb,δωb分別為陀螺儀偏差和偏差誤差，qt為真實狀態(tài)四元數(shù)，ωt為真實偏差。

3.2 標稱狀態(tài)方程模型

(9)

進一步離散化，得到遞推表達式

(10)

3.3 誤差狀態(tài)方程模型

由誤差狀態(tài)連續(xù)時間運動學方程

(11)

離散化得到遞推表達式

(12)

其中

(13)

式中θi,ωi為高斯隨機脈沖噪聲。式(13)為旋轉(zhuǎn)向量轉(zhuǎn)換為旋轉(zhuǎn)矩陣的轉(zhuǎn)換公式，即羅德里格旋轉(zhuǎn)公式[7]。其中，[u]×為旋轉(zhuǎn)向量的反對稱矩陣。

3.4 誤差狀態(tài)預(yù)測

由式(8)建立誤差狀態(tài)預(yù)測方程

(14)

式中P為誤差協(xié)方差矩陣；Fx和Fi為誤差狀態(tài)變量和噪聲變量的雅可比矩陣；Qi為噪聲變量的協(xié)方差矩陣

另外，相關(guān)的調(diào)查數(shù)據(jù)顯示，中國物流并購標的企業(yè)所在地主要集中在東部地區(qū)，其中以發(fā)達地區(qū)、沿海省份為主。這些地區(qū)或城市憑借著經(jīng)濟優(yōu)勢或地域優(yōu)勢成為近10年物流并購的熱點地區(qū)，而且這些地區(qū)并購發(fā)生量占全國總并購數(shù)目的68%。

(15)

(16)

3.5 誤差狀態(tài)觀測

以加速度計和磁力計作為觀測信息，得到觀測方程

(17)

分別對加速度計和磁力計觀測方程求偏導，得到雅可比矩陣Hx

(18)

由鏈式求導法則，求得觀測方程關(guān)于誤差狀態(tài)變量δX的雅可比矩陣H

(19)

式中XδX為真實狀態(tài)關(guān)于誤差狀態(tài)的雅可比矩陣，Qδθ為真實狀態(tài)四元數(shù)關(guān)于誤差旋轉(zhuǎn)矢量的雅可比矩陣

(20)

由式(17)，結(jié)合傳感器實際測量值可得觀測信息ΔZ

ΔZ=Z-=

(21)

3.6 誤差狀態(tài)校正

對誤差狀態(tài)預(yù)測后，需要進行補償修正。由式(14)、式(19)、式(21)可得校正方程

(22)

式中V為測量噪聲方差陣。

3.7 誤差狀態(tài)與標稱狀態(tài)融合

誤差狀態(tài)更新后，將其與標稱狀態(tài)融合，標稱狀態(tài)也隨之更新

(23)

根據(jù)式(4)、式(23)即可由實時更新的四元數(shù)轉(zhuǎn)換得到姿態(tài)信息。

3.8 誤差狀態(tài)重置

誤差狀態(tài)融合進標稱狀態(tài)后，必須重置處理

(24)

式中G=I6×6，為6階單位陣。

4 實驗與仿真

為了驗證所設(shè)計的MEKF算法的性能，將ROV放入水中進行動態(tài)實驗，完成了滾轉(zhuǎn)、俯仰、偏航等動作，獲取傳感器數(shù)據(jù)后，在MATLAB中分別實現(xiàn)ECF,GD,MEKF算法仿真。如圖4所示。

圖4 ECF,GD,MEKF姿態(tài)估計及絕對誤差

由圖4(a)～(c)可以看出，三種算法均對AHRS參考值具有較好的逼近效果。其中，GD算法的收斂性能不足，估計數(shù)值在參考值附近振蕩現(xiàn)象較為嚴重，而且動態(tài)性能較差。這是沒有選取合適的迭代步長導致的。ECF算法有較強的收斂性和動態(tài)性，但存在準確性不足的問題。

經(jīng)過統(tǒng)計分析圖4(d)的數(shù)據(jù)，得到動態(tài)實驗下各角度的絕對誤差均值Mean與均方根誤差(root mean square error,RMSE)對比結(jié)果，如表1所示。

表1 各算法在動態(tài)實驗下姿態(tài)估計誤差分析統(tǒng)計表

由表1可知，從滾轉(zhuǎn)角來看，本文MEKF算法的誤差均值分別較ECF,GD減少了39.14 %和42.10%，而RMSE略小于ECF,GD。從俯仰角來看，本文MEKF算法效果尤為明顯，誤差均值較ECF,GD分別減少了60.46 %和77.20 %，RMSE分別減少了61.14 %和65.58 %。從偏航角來看，本文MEKF算法效果略優(yōu)于ECF。

5 結(jié) 論

針對ROV姿態(tài)估計問題，本文設(shè)計了基于MEKF的姿態(tài)估計算法。該算法將真實狀態(tài)分為標稱狀態(tài)和誤差狀態(tài)，通過離散化狀態(tài)運動學方程，得到狀態(tài)遞推表達式，采用擴展卡爾曼濾波框架估計誤差狀態(tài)，并與標稱狀態(tài)融合，得到真實狀態(tài)。為驗證其可行性，結(jié)合動態(tài)實驗數(shù)據(jù)進行仿真。結(jié)果表明：該算法估計效果最接近參考值，且性能較優(yōu)。對實際應(yīng)用于ROV姿態(tài)估計系統(tǒng)中具有參考意義。