唐 軍， 王子夢(mèng)， 羅瑞智

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

0 引 言

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

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

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

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

1.1 坐標(biāo)系建立

機(jī)體坐標(biāo)系obxbybzb與機(jī)體固連，如圖1所示。通常以地心作為坐標(biāo)原點(diǎn)oe，以北、東、地方向?yàn)榇蟮刈鴺?biāo)系的正方向。規(guī)定機(jī)體旋轉(zhuǎn)的正方向?yàn)橛沂致菪较颉C(jī)體繞obxb軸旋轉(zhuǎn)的角度即滾轉(zhuǎn)角φ(roll)，繞obyb軸旋轉(zhuǎn)的角度即俯仰角θ(pitch)，繞obzb軸旋轉(zhuǎn)的角度即偏航角ψ(yaw)。

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

圖1 ROV機(jī)體坐標(biāo)系

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

1.2 姿態(tài)表示

(1)

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

(2)

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

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

(3)

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

(4)

2 傳感器測(cè)量模型

2.1 陀螺儀測(cè)量模型

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

ω=ωm-ωb-ωn

(5)

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

2.2 加速度計(jì)測(cè)量模型

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

a=am-ab-an

(6)

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

2.3 磁力計(jì)測(cè)量模型

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

(7)

3 MEKF姿態(tài)估計(jì)

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

圖3 MEKF算法原理框圖

3.1 狀態(tài)變量

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

(8)

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

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

(9)

進(jìn)一步離散化，得到遞推表達(dá)式

(10)

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

由誤差狀態(tài)連續(xù)時(shí)間運(yùn)動(dòng)學(xué)方程

(11)

離散化得到遞推表達(dá)式

(12)

其中

(13)

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

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

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

(14)

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

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

(15)

(16)

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

以加速度計(jì)和磁力計(jì)作為觀測(cè)信息，得到觀測(cè)方程

(17)

分別對(duì)加速度計(jì)和磁力計(jì)觀測(cè)方程求偏導(dǎo)，得到雅可比矩陣Hx

(18)

由鏈?zhǔn)角髮?dǎo)法則，求得觀測(cè)方程關(guān)于誤差狀態(tài)變量δX的雅可比矩陣H

(19)

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

(20)

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

ΔZ=Z-=

(21)

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

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

(22)

式中V為測(cè)量噪聲方差陣。

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

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

(23)

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

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

誤差狀態(tài)融合進(jìn)標(biāo)稱狀態(tài)后，必須重置處理

(24)

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

4 實(shí)驗(yàn)與仿真

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

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

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

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

表1 各算法在動(dòng)態(tài)實(shí)驗(yàn)下姿態(tài)估計(jì)誤差分析統(tǒng)計(jì)表

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

5 結(jié) 論

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