嚴(yán)丹，鄧志紅，張雁鵬

(1.北京理工大學(xué) 自動(dòng)化學(xué)院,北京 100081；2.中國廣核集團(tuán) 蘇州熱工研究院,江蘇 蘇州 215004)

0 引言

基于微慣性測量單元(MIMU)或微慣性與磁組合測量單元(MIMMU)的姿態(tài)解算系統(tǒng)，具有自主性強(qiáng)、受外界環(huán)境影響小、體積小、質(zhì)量輕、性價(jià)比高的優(yōu)點(diǎn)，因此被廣泛應(yīng)用于無人機(jī)、機(jī)器人、人體動(dòng)作捕捉等領(lǐng)域。

MIMU由三軸陀螺儀和三軸加速度計(jì)組成，可用于測量載體的角速率和加速度。MIMMU相比于MIMU增加了三軸磁力計(jì)，可用于測量外界磁感應(yīng)強(qiáng)度。利用角速率積分可以獲得載體姿態(tài)，但由于微陀螺儀隨機(jī)漂移較嚴(yán)重，在積分過程中解算誤差會(huì)隨時(shí)間累積，長時(shí)間工作很可能造成發(fā)散。當(dāng)載體處于靜止或勻速運(yùn)動(dòng)時(shí)，加速度計(jì)只測得載體坐標(biāo)系下重力加速度分量，可用于校正橫滾角和俯仰角，但當(dāng)載體加速運(yùn)動(dòng)時(shí)會(huì)影響校正效果。當(dāng)載體附近無鐵磁干擾時(shí)，磁力計(jì)只測得載體坐標(biāo)系下地球磁場分量，可用于校正偏航角，但當(dāng)磁力計(jì)受外界鐵磁干擾較嚴(yán)重時(shí)會(huì)影響校正效果。由于利用后二者解算姿態(tài)角只基于一個(gè)解算周期內(nèi)的加速度計(jì)數(shù)據(jù)和磁數(shù)據(jù)，解算誤差不隨時(shí)間積累，因此使用MIMU或MIMMU數(shù)據(jù)進(jìn)行信息融合，共同解算載體姿態(tài)，一方面可以有效地提高解算精度，另一方面可以使系統(tǒng)長時(shí)間工作而誤差不發(fā)散。

基于MIMU或MIMMU解算載體姿態(tài)的信息融合算法可分為三大類：Kalman濾波(KF)法[1-9]、互補(bǔ)濾波法[10-12]和最優(yōu)化補(bǔ)償方法[13-17]。KF法通過建立狀態(tài)方程和觀測方程，實(shí)現(xiàn)多傳感器數(shù)據(jù)融合。Luinge等[1]提出了一種基于IMU數(shù)據(jù)解算人體骨骼姿態(tài)的線性KF算法。該算法以姿態(tài)角估計(jì)誤差和陀螺儀補(bǔ)償誤差作為狀態(tài)量，通過建立誤差模型推導(dǎo)出Kalman濾波器的狀態(tài)方程和觀測方程。Roetenberg等在文獻(xiàn)[1]算法基礎(chǔ)上增加了磁擾動(dòng)誤差作為狀態(tài)量，實(shí)現(xiàn)了基于IMMU數(shù)據(jù)的信息融合[2]，相對(duì)于文獻(xiàn)[1]可以有效預(yù)防航向漂移和外界磁干擾。但上述算法存在計(jì)算量較大的問題，Sabatelli等[3]提出一種兩級(jí)擴(kuò)展Kalman濾波(EKF)算法，相比于使用Kalman濾波器同時(shí)融合加速度計(jì)和磁力計(jì)數(shù)據(jù)，該算法的計(jì)算量更小。針對(duì)載體運(yùn)動(dòng)加速度或外界鐵磁干擾過大、影響系統(tǒng)姿態(tài)解算問題，近年來一部分文獻(xiàn)使用自適應(yīng)Kalman濾波器來增加系統(tǒng)的魯棒性。例如：Sabatini[4]設(shè)計(jì)了一種門限式自適應(yīng)因子，當(dāng)運(yùn)動(dòng)加速度或者磁擾動(dòng)大于某個(gè)閾值時(shí)，不使用該時(shí)刻的相應(yīng)數(shù)據(jù)修正狀態(tài)量。Ren等[5]提出一種改進(jìn)的自適應(yīng)因子，隨運(yùn)動(dòng)加速度或磁擾動(dòng)的增大線性加大觀測噪聲方差陣中對(duì)應(yīng)元素的值，從而實(shí)現(xiàn)逐漸減小Kalman濾波器對(duì)加速度計(jì)或磁力計(jì)數(shù)據(jù)的置信度。Tong等[6]基于傳感器誤差模型建立乘性擴(kuò)展Kalman濾波器(MEKF)，并提出了使用隱馬爾可夫模型對(duì)當(dāng)前加速度以及磁干擾狀態(tài)進(jìn)行判斷，以作為自適應(yīng)調(diào)整MEKF中觀測噪聲方差陣的依據(jù)。另一部分文獻(xiàn)使用較新的KF算法，例如無跡Kalman濾波器[7-8]、協(xié)方差膨脹的乘法擴(kuò)展Kalman濾波器(CI-MEKF)[9]。該類方法雖然解算效果較好，但計(jì)算量相比于后兩類算法較大。

根據(jù)文獻(xiàn)[10]的理論推導(dǎo)，互補(bǔ)濾波法可通過比例積分(PI)控制器對(duì)陀螺儀所估計(jì)姿態(tài)進(jìn)行高通濾波，對(duì)加速度計(jì)或磁力計(jì)所估計(jì)姿態(tài)進(jìn)行低通濾波。該類算法計(jì)算量小，但解算效果受參數(shù)影響較大。

最優(yōu)化補(bǔ)償算法是通過構(gòu)建并優(yōu)化損失函數(shù)進(jìn)行信息融合的方法，其計(jì)算量介于前兩類之間，且可以對(duì)非高斯噪聲所引入的誤差進(jìn)行校正。Bachmann等[14]提出一種使用加速度計(jì)和磁力計(jì)數(shù)據(jù)，對(duì)基于角速率更新的先驗(yàn)四元數(shù)進(jìn)行補(bǔ)償?shù)乃惴?。它通過上一時(shí)刻后驗(yàn)四元數(shù)獲得當(dāng)前加速度計(jì)和磁力計(jì)數(shù)據(jù)的估計(jì)值，將測量值與估計(jì)值的均方誤差看作代價(jià)函數(shù)，以姿態(tài)四元數(shù)作為自變量，利用高斯牛頓法對(duì)該代價(jià)函數(shù)進(jìn)行最優(yōu)化，以獲得后驗(yàn)四元數(shù)。Madgwick等在文獻(xiàn)[15-16]中提出了一種基于梯度下降的濾波方法。相比于文獻(xiàn)[14]，在代價(jià)函數(shù)不變的前提下，該算法改變了最優(yōu)化方法，有效降低了計(jì)算復(fù)雜度。此外，它利用誤差積分反饋進(jìn)行陀螺儀漂移補(bǔ)償，利用慣性坐標(biāo)系下磁感應(yīng)強(qiáng)度在y軸沒有分量的特點(diǎn)進(jìn)行磁補(bǔ)償。該算法解算精度與KF算法類似且計(jì)算量更小。

綜上所述，基于最優(yōu)化補(bǔ)償?shù)奈T性與磁組合姿態(tài)測量方法，相比于KF法計(jì)算量更小，相比于互補(bǔ)濾波法魯棒性更強(qiáng)。但目前的最優(yōu)化補(bǔ)償算法未考慮外界鐵磁物質(zhì)對(duì)于磁力計(jì)的干擾，當(dāng)鐵磁干擾較大時(shí)該算法無法正常工作。另外，該算法使用同一個(gè)濾波器對(duì)MIMMU數(shù)據(jù)進(jìn)行融合，加速度計(jì)數(shù)據(jù)會(huì)對(duì)偏航角的解算造成不利影響，磁力計(jì)數(shù)據(jù)會(huì)對(duì)橫滾角和俯仰角的解算造成不利影響。因此，本文提出一種基于梯度下降法的兩級(jí)姿態(tài)更新算法：在第1級(jí)濾波中通過陀螺儀和加速度計(jì)數(shù)據(jù)更新俯仰角和橫滾角，在第2級(jí)濾波中通過陀螺儀和磁力計(jì)數(shù)據(jù)更新偏航角，以提高系統(tǒng)解算精度。針對(duì)磁力計(jì)易受外界鐵磁干擾問題，該算法在第2級(jí)濾波中對(duì)鐵磁干擾進(jìn)行實(shí)時(shí)估計(jì)并補(bǔ)償，使算法具有一定抗鐵磁干擾能力。最后對(duì)所提方法進(jìn)行了仿真和實(shí)測實(shí)驗(yàn)驗(yàn)證。

1 傳感器建模

1.1 坐標(biāo)系建立

取地心慣性坐標(biāo)系Oixiyizi(簡稱i系)為慣性坐標(biāo)系，其原點(diǎn)Oi位于地球地心，xi軸指向春分點(diǎn)，zi軸沿地球自轉(zhuǎn)軸，yi軸與xi、zi軸構(gòu)成右手直角坐標(biāo)系。取載體坐標(biāo)系Obxbybzb(簡稱b系)與傳感器模塊固連，其原點(diǎn)Ob位于傳感器模塊重心，xb軸沿傳感器模塊橫軸指向右側(cè)，zb軸沿傳感器模塊豎軸指向上方，yb軸與xb、zb軸構(gòu)成右手直角坐標(biāo)系。設(shè)初始時(shí)刻的載體坐標(biāo)系與導(dǎo)航坐標(biāo)系Onxnynzn(簡稱n系)重合。

1.2 姿態(tài)表示與測定

目前，常用的姿態(tài)更新方法有四元數(shù)法[1-10,12-17]、歐拉角法[11]、旋轉(zhuǎn)矢量法[18-19]等。其中，歐拉角法存在萬向節(jié)死鎖問題，無法適用于全姿態(tài)解算。旋轉(zhuǎn)矢量法采用多子樣算法對(duì)不可交換誤差進(jìn)行有效補(bǔ)償，計(jì)算量相比于四元數(shù)法較大，適用于角機(jī)動(dòng)頻繁或存在嚴(yán)重角振動(dòng)的運(yùn)載體的姿態(tài)更新。由于機(jī)器人、無人機(jī)、人體動(dòng)作捕捉等一般不存在嚴(yán)重角振動(dòng)，因此本文選用四元數(shù)法進(jìn)行載體姿態(tài)更新。

1.2.1 四元數(shù)姿態(tài)更新

四元數(shù)可用于表示剛體旋轉(zhuǎn)。設(shè)剛體相對(duì)于n系做定點(diǎn)運(yùn)動(dòng)，采用四元數(shù)表示剛體的定點(diǎn)運(yùn)動(dòng)：

(1)

式中：Q為從n系到b系的旋轉(zhuǎn)四元數(shù)；un為旋轉(zhuǎn)軸和旋轉(zhuǎn)方向；α為轉(zhuǎn)過的角度。通過四元數(shù)表示可以將b系看作由n系經(jīng)過無中間過程的一次性等效旋轉(zhuǎn)形成的。

利用四元數(shù)表示旋轉(zhuǎn)矩陣：

(2)

四元數(shù)微分方程：

(3)

由于陀螺儀測量需要一定時(shí)間，所測得角速率是離散數(shù)據(jù)，需要對(duì)(3)式進(jìn)行基于1階近似的離散化處理，可得

(4)

式中：I4為4階單位矩陣；T為陀螺儀采樣時(shí)間；ωk為k時(shí)刻陀螺儀量測值；Qk為k時(shí)刻n系到b系的旋轉(zhuǎn)四元數(shù)；Z(·)為ωk的反對(duì)稱矩陣，定義為

(5)

ωk,x、ωk,y、ωk,z分別為向量ωk的3個(gè)元素。

1.2.2 歐拉角

載體的空間姿態(tài)可以看作是導(dǎo)航坐標(biāo)系Onxnynzn依次繞航向軸、俯仰軸、橫滾軸旋轉(zhuǎn)ψ、θ、γ后的復(fù)合結(jié)果。

當(dāng)旋轉(zhuǎn)順序設(shè)定為z-x-y時(shí)，從n系到b系的旋轉(zhuǎn)矩陣為

(6)

1.3 傳感器模型

本文所使用的傳感器MIMMU由微陀螺儀、微加速度計(jì)及微磁力計(jì)組成。設(shè)t時(shí)刻微陀螺儀所測數(shù)據(jù)為yg,t，其中包含b系相對(duì)于i系的角速度信息、陀螺零偏、測量噪聲；設(shè)t時(shí)刻微加速度計(jì)所測數(shù)據(jù)為ya,t，其中包含載體運(yùn)動(dòng)加速度、b系下所測得的重力加速度、系統(tǒng)固有偏置、測量噪聲；設(shè)t時(shí)刻微磁力計(jì)所測數(shù)據(jù)為ym,t，其中包含b系下所測得的地球磁場、外界鐵磁干擾、系統(tǒng)固有偏置、測量噪聲。在不考慮傳感器非正交誤差、靈敏度誤差前提下，其具體量測模型如下：

(7)

為了簡化計(jì)算，本文將ds,t和dh,t合并為t時(shí)刻磁力計(jì)所受鐵磁干擾矢量dt，原磁力計(jì)量測模型可簡化為

(8)

2 數(shù)據(jù)融合算法設(shè)計(jì)

圖1 本文提出的姿態(tài)解算算法框圖Fig.1 Block diagram of the proposed attitude determination method

由于Kg、Ka、Km、bg、ba、bm屬于傳感器系統(tǒng)誤差，可通過數(shù)據(jù)預(yù)處理校正。假設(shè)當(dāng)前傳感器已經(jīng)過校正，設(shè)Kg、Ka、Km均為3階單位矩陣以及bg、ba、bm均為零向量，可得

yg,t=ωt+vg,t,

(9)

(10)

(11)

若初始時(shí)刻傳感器水平放置，即初始姿態(tài)的z軸(即zn軸)與重力加速度方向重合，則加速度計(jì)所測x軸與y軸數(shù)據(jù)均為0，即g=[0 0gz]T。隨后，當(dāng)傳感器旋轉(zhuǎn)到某一姿態(tài)并保持靜止或勻速運(yùn)動(dòng)時(shí)，可通過(6)式、(10)式求得加速度計(jì)量測值ya：

(12)

此時(shí)傳感器的俯仰角θ和橫滾角γ為

θ=arcsinya,y,

(13)

(14)

但無法通過加速度計(jì)數(shù)據(jù)算出偏航角ψ.

一方面，由于磁力計(jì)相比于加速度計(jì)更容易受到外界環(huán)境干擾(如電子設(shè)備等)，其數(shù)據(jù)可信度相對(duì)較小。另一方面，無論傳感器初始姿態(tài)如何，磁力計(jì)所測數(shù)據(jù)都與偏航角ψ有關(guān)，可以利用磁力計(jì)數(shù)據(jù)修正載體偏航角。因此本文采用兩級(jí)姿態(tài)更新法，在第1級(jí)更新中將陀螺儀與加速度計(jì)數(shù)據(jù)融合，對(duì)載體的俯仰角和橫滾角進(jìn)行更新；在第2級(jí)更新中將陀螺儀與磁力計(jì)數(shù)據(jù)融合，對(duì)載體的偏航角進(jìn)行更新。

2.1 第1級(jí)姿態(tài)更新

由于本文所提算法主要應(yīng)用于人體運(yùn)動(dòng)捕捉、小型機(jī)器人、無人機(jī)等設(shè)備姿態(tài)解算，載體運(yùn)動(dòng)加速度相對(duì)于重力加速度較小，因此忽略運(yùn)動(dòng)加速度部分，加速度計(jì)量測模型可簡化為

(15)

t時(shí)刻加速度的實(shí)際測量值與估計(jì)值之間誤差可以表示為

(16)

由此可知，當(dāng)估計(jì)四元數(shù)q越接近t時(shí)刻由n系到b系旋轉(zhuǎn)四元數(shù)的真實(shí)值qt時(shí)，ε1(q)越接近[0 0 0]T。利用ε1(·)的歐幾里得范數(shù)作為代價(jià)函數(shù)：

F1(q)=ε1(q)Tε1(q),

(17)

將t-1時(shí)刻由n系到b系的后驗(yàn)估計(jì)四元數(shù)作為估計(jì)四元數(shù)的初始值，利用梯度下降法對(duì)F1(q)進(jìn)行最優(yōu)化，沿?fù)p失函數(shù)的負(fù)梯度方向更新變量，有助于減小損失函數(shù)：

(18)

設(shè)q=[q0q1q2q3]T、g=[gxgygz]T，由(2)式、(16)式、(17)式可得

(19)

(20)

利用陀螺儀數(shù)據(jù)更新旋轉(zhuǎn)四元數(shù)的1階離散公式為

(21)

(22)

式中：μ1為置信于a,t的權(quán)重。

(23)

式中：ρ1=μ1β1，為可調(diào)參數(shù)。

2.2 第2級(jí)姿態(tài)更新

由于地球地表磁場強(qiáng)度在25～65 μT范圍內(nèi)，比較微弱，鐵磁干擾數(shù)值常大于地球地表磁場，因此無法忽略鐵磁干擾部分，本文選擇對(duì)鐵磁干擾dt進(jìn)行估計(jì)。

由(11)式得到t時(shí)刻磁力計(jì)的實(shí)際測量值與估計(jì)值之間誤差為

(24)

由(11)式和(24)式可知，當(dāng)估計(jì)四元數(shù)q越接近t時(shí)刻旋轉(zhuǎn)四元數(shù)的真實(shí)值qt，且估計(jì)鐵磁干擾d越接近t時(shí)刻鐵磁干擾的真實(shí)值dt時(shí)，ε2(q,d)越接近[0 0 0]T。

令x=[qd]T，利用ε2(·)的歐幾里得范數(shù)作為代價(jià)函數(shù):

F2(x)=ε2(x)Tε2(x)，

(25)

利用梯度下降法對(duì)F2(x)進(jìn)行最優(yōu)化：

(26)

(27)

式中：Λ(x)為第2級(jí)代價(jià)函數(shù)F2(x)關(guān)于估計(jì)四元數(shù)q求偏導(dǎo)后轉(zhuǎn)置的結(jié)果，可用于優(yōu)化估計(jì)四元數(shù)。由于該公式太過復(fù)雜，在此省略展示。

(28)

式中：μ2為置信于m,t的權(quán)重，其數(shù)值較?。沪?=μ2β2，為可調(diào)參數(shù)。

2.3 整體更新

根據(jù)磁力計(jì)量測模型(11)式，可得t時(shí)刻后驗(yàn)估計(jì)鐵磁干擾為

(29)

3 實(shí)驗(yàn)驗(yàn)證

3.1 解算誤差表示

令真實(shí)姿態(tài)四元數(shù)為qtrue，通過算法解算出的姿態(tài)四元數(shù)為q，q相對(duì)于qtrue的誤差可通過Δq表示為

(30)

根據(jù)四元數(shù)的輻角公式(1)式，可得解算誤差角Δθ為

(31)

式中：scalar(·)為提取四元數(shù)內(nèi)標(biāo)量的函數(shù)。本文在后續(xù)的實(shí)驗(yàn)中通過Δθ衡量解算誤差。

3.2 仿真實(shí)驗(yàn)

通過軌跡發(fā)生器可自定義載體運(yùn)動(dòng)情況，并產(chǎn)生模擬陀螺儀、加速度計(jì)、磁力計(jì)數(shù)據(jù)以及真實(shí)姿態(tài)數(shù)據(jù)。

設(shè)計(jì)載體運(yùn)動(dòng)方式和外部環(huán)境的步驟如下：

1)載體靜止20 s；

2)載體繞xn軸以3π rad/s角速率旋轉(zhuǎn)40 s，沿xn、yn、zn軸分別施加10 μT的鐵磁干擾；

3)載體靜止20 s，外界鐵磁干擾保持不變；

4)載體繞xn軸以3π rad/s角速率旋轉(zhuǎn)40 s，沿xn、yn、zn軸分別施加30 μT的鐵磁干擾；

5)載體靜止20 s，外界鐵磁干擾保持不變；

6)載體繞xn軸以3π rad/s角速率旋轉(zhuǎn)40 s，沿xn、yn、zn軸分別施加50 μT的鐵磁干擾；

7)載體靜止20 s，外界鐵磁干擾保持不變。

采樣周期設(shè)為100 Hz，陀螺儀漂移設(shè)置為0.001°/s，加速度計(jì)漂移設(shè)置為8×10-6g/s，磁力計(jì)漂移設(shè)置為0.05 μT.地球局部參數(shù)均根據(jù)北京進(jìn)行設(shè)置，重力加速度設(shè)為[0 m/s2，0 m/s2，-9.801 47 m/s2]，地球表面磁場設(shè)為[27.77 μT，-3.35 μT，46.97 μT].地球自轉(zhuǎn)角速率設(shè)置為0.000 072 921 rad/s，地球近似半徑設(shè)置為6 378 137 m.

利用上述數(shù)據(jù)對(duì)本文提出算法進(jìn)行仿真測試。將本文提出算法的實(shí)驗(yàn)結(jié)果與角速率積分法、互補(bǔ)濾波法、傳統(tǒng)優(yōu)化法以及兩級(jí)姿態(tài)更新法的運(yùn)行結(jié)果進(jìn)行對(duì)比。其中，角速率積分法僅利用微陀螺儀數(shù)據(jù)，通過離散化四元數(shù)微分方程更新姿態(tài)四元數(shù)。本文用作對(duì)比的互補(bǔ)濾波法由左國玉等[12]提出，同時(shí)對(duì)MIMMU數(shù)據(jù)進(jìn)行融合。其余3種算法都是基于梯度下降法優(yōu)化姿態(tài)四元數(shù)，具體來說：傳統(tǒng)優(yōu)化法由Madgwick等[16]提出，它使用一個(gè)濾波器同時(shí)對(duì)MIMMU數(shù)據(jù)進(jìn)行融合，且在建模過程中未考慮外界鐵磁干擾；兩級(jí)姿態(tài)更新法是根據(jù)本文提出算法去除鐵磁補(bǔ)償部分獲得的，它與前者區(qū)別在于采用了兩級(jí)濾波器，先對(duì)陀螺儀與加速度計(jì)數(shù)據(jù)進(jìn)行融合，更新載體的俯仰角和橫滾角，再對(duì)陀螺儀與磁力計(jì)數(shù)據(jù)進(jìn)行融合，更新載體的偏航角；本文提出算法相比于兩級(jí)姿態(tài)更新法，在第2級(jí)濾波建模中考慮了鐵磁干擾，增加了鐵磁干擾補(bǔ)償。

在本文提出算法中令ρ1=0.000 026 7，ρ2=0.000 030 7.各算法解算誤差角Δθ的階段均值與總體均值如表1所示，表中MeanΔθ為Δθ均值。各算法針對(duì)該數(shù)據(jù)的解算誤差角Δθ隨時(shí)間變化情況如圖2所示。

表1 各算法MeanΔθ統(tǒng)計(jì)表Tab.1 MeanΔθ of each algorithm (°)

圖2 各算法解算誤差對(duì)比圖(針對(duì)仿真數(shù)據(jù))Fig.2 Comparison among the attitude determination errors of various algorithms (for simulated data)

根據(jù)仿真設(shè)定，實(shí)驗(yàn)載體在20～60 s、60～120 s、120～200 s階段所受外界鐵磁干擾逐漸增加，通過該仿真數(shù)據(jù)可對(duì)各個(gè)算法抗鐵磁干擾能力和姿態(tài)解算精度進(jìn)行測試。由表1可以看出，互補(bǔ)濾波法、傳統(tǒng)優(yōu)化法、兩級(jí)姿態(tài)更新法以及本文提出算法的各階段MeanΔθ始終低于角速率積分法，表明使用MIMMU數(shù)據(jù)解算載體姿態(tài)有利于提高解算精度。另外，本文所提算法各階段的MeanΔθ相近，沒有呈現(xiàn)遞增狀況，且都小于3°；而其余3種融合解算算法各階段的MeanΔθ都呈現(xiàn)出明顯遞增，且均大于3°，表明本文提出算法相比于其他對(duì)比算法可以有效估計(jì)并補(bǔ)償外界鐵磁干擾，使系統(tǒng)具有一定的抗鐵磁干擾能力且解算精度更高。

根據(jù)仿真設(shè)定，載體在60～80 s、120～140 s、180～200 s處于靜止?fàn)顟B(tài)，且所受鐵磁干擾遞增。由圖2可以看出，在這些時(shí)間段，角速率積分法解算誤差不變，傳統(tǒng)優(yōu)化法、兩級(jí)姿態(tài)更新法以及本文提出算法的解算誤差隨時(shí)間減少，而互補(bǔ)濾波法解算誤差在60～80 s區(qū)間隨時(shí)間減少、在120～140 s和180～200 s區(qū)間隨時(shí)間增加。表明基于最優(yōu)化的補(bǔ)償算法相比于互補(bǔ)濾波法魯棒性更強(qiáng)。通過對(duì)比傳統(tǒng)優(yōu)化法和兩級(jí)姿態(tài)更新法的解算結(jié)果可以看出，兩級(jí)姿態(tài)更新相比于單濾波器更有利于提高解算精度，當(dāng)載體處于靜止時(shí)其補(bǔ)償解算誤差的速度也更快。通過對(duì)比兩級(jí)姿態(tài)更新法和本文提出算法的解算結(jié)果可以看出，本文所設(shè)計(jì)的鐵磁干擾補(bǔ)償算法效果明顯，可以有效提高系統(tǒng)的魯棒性，使之適用于各種鐵磁干擾環(huán)境。

3.3 實(shí)測實(shí)驗(yàn)

使用荷蘭Xsens公司生產(chǎn)的MTi-G-710作為傳感器，其內(nèi)部包含三軸微陀螺儀、三軸微加速度計(jì)、三軸微磁力計(jì)，相關(guān)參數(shù)如表2所示。MTi-G-710所配套的軟件內(nèi)置一套基于KF的姿態(tài)解算算法，當(dāng)傳感器處于均勻磁場中時(shí)，該算法解算結(jié)果的誤差均方根(RMS)如表3所示。本文將該算法輸出的姿態(tài)四元數(shù)看作真實(shí)姿態(tài)四元數(shù)，進(jìn)行后續(xù)實(shí)驗(yàn)。

通過手持MIMMU模塊，分別在鐵磁干擾不同的3個(gè)地點(diǎn)進(jìn)行自由晃動(dòng)、獲得3組數(shù)據(jù)，陀螺儀、加速度計(jì)、磁力計(jì)的采樣周期都設(shè)置為100 Hz.由于MTi-G-710中磁力計(jì)的輸出數(shù)據(jù)是單位化后的結(jié)果，即當(dāng)外界無鐵磁干擾時(shí)，每一時(shí)刻磁力計(jì)數(shù)據(jù)的范數(shù)都為1，因此外界鐵磁干擾越大，每一時(shí)刻磁力計(jì)數(shù)據(jù)的范數(shù)越偏離1.根據(jù)(32)式量化該組數(shù)據(jù)所受鐵磁干擾的大?。?/p>

表2 MTi-G-710內(nèi)置傳感器相關(guān)參數(shù)表Tab.2 Specifications of MTi-G-710

表3 MTi-G-710配套算法解算精度表Tab.3 Attitude determination performance (°)

(32)

式中：n為該組數(shù)據(jù)總時(shí)刻數(shù)。

利用(32)式對(duì)各數(shù)據(jù)受鐵磁干擾大小進(jìn)行計(jì)算，結(jié)果如表4所示。由表4可知，數(shù)據(jù)1所受鐵磁干擾整體最小，數(shù)據(jù)2次之，數(shù)據(jù)3所受鐵磁干擾最大。

利用上述3組數(shù)據(jù)進(jìn)行算法檢驗(yàn)，將本文提出算法與角速率積分法、互補(bǔ)濾波法、傳統(tǒng)優(yōu)化法以及兩級(jí)姿態(tài)更新法進(jìn)行對(duì)比。

由于上述實(shí)驗(yàn)算法都需要進(jìn)行參數(shù)設(shè)置，利用數(shù)據(jù)1對(duì)各算法進(jìn)行調(diào)參，其中在本文提出算法中令ρ1=0.000 026 7，ρ2=0.000 030 7.各算法解算誤差角Δθ的均值如表5所示。將互補(bǔ)濾波法與角速率積分法的MeanΔθ進(jìn)行對(duì)比，通過表5可以看出：隨著外界鐵磁干擾的增大，該算法校正陀螺儀數(shù)據(jù)的效果逐漸變差，甚至在數(shù)據(jù)3時(shí)其解算誤差超過單純角速率積分法。由此表明互補(bǔ)濾波法的解算效果受參數(shù)影響大、魯棒性較弱。而傳統(tǒng)優(yōu)化法、兩級(jí)姿態(tài)更新法以及本文提出算法對(duì)于上述3組數(shù)據(jù)，它們的解算誤差始終低于角速率積分法以及互補(bǔ)濾波法，表明最優(yōu)化補(bǔ)償法相對(duì)于互補(bǔ)濾波法解算效果更好，且魯棒性較強(qiáng)，在不同鐵磁干擾環(huán)境下都能對(duì)陀螺儀數(shù)據(jù)進(jìn)行一定程度的有效校正。根據(jù)表5，對(duì)比傳統(tǒng)優(yōu)化法和兩級(jí)姿態(tài)更新法的MeanΔθ可以看出，通過兩級(jí)姿態(tài)更新可以有效提高姿態(tài)解算精度；對(duì)比兩級(jí)姿態(tài)更新法與本文提出算法的MeanΔθ可以看出，增加鐵磁干擾補(bǔ)償有利于進(jìn)一步提高算法解算精度，并使其可以很好地適應(yīng)不同鐵磁干擾環(huán)境。

表5 各算法MeanΔθ統(tǒng)計(jì)表Tab.5 MeanΔθ of each algorithm (°)

以數(shù)據(jù)1為例進(jìn)行具體分析。數(shù)據(jù)1的角速率、加速度、磁力計(jì)數(shù)據(jù)范數(shù)隨時(shí)間變化情況如圖3所示，各算法針對(duì)數(shù)據(jù)1的解算誤差角Δθ隨時(shí)間變化情況如圖4所示。通過對(duì)比圖3、圖4可以看出：0～25 s，傳感器處于靜止?fàn)顟B(tài)，此時(shí)角速率積分法、互補(bǔ)濾波法、傳統(tǒng)優(yōu)化法的解算誤差都隨時(shí)間累積，而兩級(jí)姿態(tài)更新法和本文提出算法的解算誤差先增大、后減小，表明兩級(jí)姿態(tài)更新相比于單濾波器融合法可以更有效地補(bǔ)償傳感器靜置時(shí)陀螺儀漂移帶來的解算誤差；40～50 s，陀螺儀發(fā)生較大漂移，各算法解算誤差都出現(xiàn)不同程度的陡增；50～70 s，根據(jù)角速率積分法可以看出在這一時(shí)段陀螺儀漂移情況較平穩(wěn)，本文提出算法相比于其他對(duì)比算法能更快地通過加速度計(jì)和磁力計(jì)數(shù)據(jù)降低解算誤差；70～85 s，傳感器恢復(fù)靜止?fàn)顟B(tài)，前3種方法解算誤差都隨時(shí)間增加，兩級(jí)姿態(tài)更新法雖能在一定程度上補(bǔ)償此前解算誤差，但補(bǔ)償速率較慢，本文提出算法由于加入了鐵磁補(bǔ)償，可以有效估計(jì)外界鐵磁干擾，以較快速率將解算誤差降低到較低狀態(tài)，這一特點(diǎn)使本算法更適用于長時(shí)間正常工作。

圖3 陀螺儀、加速度計(jì)、磁力計(jì)數(shù)據(jù)范數(shù)隨時(shí)間變化(針對(duì)數(shù)據(jù)1)Fig.3 Magnitudes of gyroscope data,accelerometer data and magnetometer data (for Data 1)

圖4 各算法解算誤差對(duì)比(針對(duì)數(shù)據(jù)1)Fig.4 Comparison among the attitude determination errors of various algorithms (for Data 1)

各算法針對(duì)數(shù)據(jù)2、數(shù)據(jù)3的解算誤差角Δθ隨時(shí)間變化情況分別如圖5、圖6所示。對(duì)比圖4、圖5、圖6可以看出，外界鐵磁干擾的增大會(huì)對(duì)互補(bǔ)濾波法、傳統(tǒng)優(yōu)化法、兩級(jí)姿態(tài)更新法的解算精度有較大影響，此時(shí)本文所提鐵磁補(bǔ)償法發(fā)揮作用，使系統(tǒng)在較強(qiáng)鐵磁干擾環(huán)境下仍能正常工作。

圖5 各算法解算誤差對(duì)比(針對(duì)數(shù)據(jù)2)Fig.5 Comparison among the attitude determination errors of various algorithms (for Data 2)

圖6 各算法解算誤差對(duì)比(針對(duì)數(shù)據(jù)3)Fig.6 Comparison among the attitude determination errors of various algorithms (for Data 3)

4 結(jié)論

本文使用MIMMU作為傳感器，提出了一種抗鐵磁干擾的兩級(jí)姿態(tài)更新姿態(tài)解算方法：它基于姿態(tài)四元數(shù)更新，可用于全姿態(tài)解算；第1級(jí)通過陀螺儀和加速度數(shù)據(jù)更新俯仰角和偏航角，第2級(jí)在考慮鐵磁干擾的情況下通過磁力計(jì)數(shù)據(jù)修正偏航角。

仿真和實(shí)測實(shí)驗(yàn)結(jié)果表明：兩級(jí)姿態(tài)更新相比于單濾波器方法可以有效提高解算精度，而本文提出的利用梯度下降法估計(jì)并補(bǔ)償鐵磁干擾方法，可以使算法具有一定抗鐵磁干擾能力，解決了在外界存在鐵磁干擾的環(huán)境下低精度器件中高精度姿態(tài)獲取的難題。本文方法可廣泛應(yīng)用于人體運(yùn)動(dòng)捕捉、無人機(jī)及機(jī)器人等領(lǐng)域，具有廣闊的應(yīng)用前景。