史宜巧，趙 輝

（1.江蘇電子信息職業(yè)學(xué)院智能制造學(xué)院，江蘇 淮安 223003；2.河北工程大學(xué)信息與電氣工程學(xué)院，河北 邯鄲 056038）

1 引言

基于微機電系統(tǒng)（micro?electro mechanical system，MEMS）的慣性測量單元（inertial measurement unit，IMU）和磁羅盤組成的航姿參考系統(tǒng)[1（]Attitude and Heading Reference System，AHRS）通過實時測量到的角速率，加速度，以及磁航向，將短期精度較高（角速率）的姿態(tài)計算和長期精度較高（加速度，磁場）的姿態(tài)計算結(jié)果進行融合，從而實現(xiàn)對載體姿態(tài)的估計，在消費級導(dǎo)航，服務(wù)型運動感知等低成本要求的領(lǐng)域已經(jīng)有了廣泛的應(yīng)用[2]。目前常用的AHRS實時姿態(tài)估計方法可大致分為互補濾波[3]，QUEST型方法[4?5]以及濾波器估計方法[6?8]。其中濾波器方法可以在姿態(tài)估計的同時對系統(tǒng)誤差參數(shù)進行實時估計，且更易于融合各類元件信息以取得更好的整體性能。因此得到了廣泛的研究。這里研究即為濾波方法。

在姿態(tài)估計問題中應(yīng)用濾波估計方法需要解決兩個問題，一是觀測模型通常是非線性的；二是姿態(tài)四元數(shù)在估計中不能夠自然歸一化。對此，通常的做法是將過程與觀測模型線性化，以四元數(shù)等姿態(tài)表征參數(shù)表示的估計誤差和傳感器偏差估計誤差作為狀態(tài)量，將加速度計和磁傳感器等外部信息作為觀測量。將問題轉(zhuǎn)化為線性濾波對狀態(tài)量進行求解。例如，乘性擴展卡爾曼濾波[9?10（]Multiplicative Extended Kalman Filter，MEKF）將誤差四元數(shù)作為狀態(tài)量，利用一階近似避免了觀測信息引入的非線性，且隱式地包含了歸一化條件。由于截斷誤差的影響，基于誤差估計的方法估計精度不高，尤其在姿態(tài)變化劇烈條件下可能產(chǎn)生較大的誤差。

針對上述問題，這里將姿態(tài)變量與傳感器偏差參數(shù)直接作為待求解狀態(tài)，提出一種基于非線性濾波的四元數(shù)姿態(tài)估計算法。根據(jù)四元數(shù)姿態(tài)表示原理和傳感器測量輸出模型建立了AHRS 系統(tǒng)四元數(shù)姿態(tài)估計的直接形式非線性狀態(tài)空間模型，采用構(gòu)造偽觀測量方法解決四元數(shù)歸一化問題，在此基礎(chǔ)上推導(dǎo)出了觀測函數(shù)的雅克比矩陣，采用迭代擴展卡爾曼濾波（iter‐ated EKF，IEKF）方法進行濾波求解，實現(xiàn)了對姿態(tài)的實時估計。此外，利用閾值法處理載體機動時產(chǎn)生的運動加速度。通過實際傳感器測量與ABB 機器人同步姿態(tài)參考數(shù)據(jù)集對這里算法進行了驗證，并與基于非線性濾波、互補濾波等的姿態(tài)估計方法以及商用姿態(tài)測量單元的結(jié)果進行了對比。結(jié)果表明，相比現(xiàn)有常用方法，這里算法在姿態(tài)估計準(zhǔn)確度與精度方面具有較好的性能。

2 四元數(shù)姿態(tài)估計的非線性問題模型

設(shè)載體自身坐標(biāo)系（B系）為右－前－上坐標(biāo)系，參考系（N系）為東－北－天局部導(dǎo)航坐標(biāo)系（ENU）。姿態(tài)四元數(shù)定義為：

式中：ψ=|ψ|，且ψ=ψμ；

μ—N系中的轉(zhuǎn)軸矢量；

ψ—從系通過轉(zhuǎn)軸轉(zhuǎn)到系所經(jīng)過的角度。

因此可以得到從B系變換到N系的姿態(tài)變換矩陣為：

在ENU坐標(biāo)系下可得上式姿態(tài)變換矩陣對應(yīng)的歐拉角為：

式中：Rij—矩陣(q)相應(yīng)位置的元素。

根據(jù)式（2）給出的變換關(guān)系，下面推導(dǎo)四元數(shù)姿態(tài)估計的非線性狀態(tài)空間模型。

2.1 狀態(tài)方程

考慮到待求解的量和影響姿態(tài)估計精度的因素，將四元數(shù)，陀螺儀偏差βω，加速度計偏差βa以及磁傳感器偏差βm作為狀態(tài)量，即：

根據(jù)各個狀態(tài)量隨著時間的動態(tài)變化過程，下面來導(dǎo)出狀態(tài)方程，四元數(shù)隨時間變化的動態(tài)方程為：

式中：=ω?βω；ω—陀螺儀讀數(shù)；

βω—陀螺儀偏差；

υω~N(0,Σω)—陀螺儀測量噪聲，矩陣：

各個傳感器偏差視為一階Gauss?Markov過程，即：

式中：系數(shù)矩陣Αi,i=ω,a,m—對角矩陣，對角元素分別為三軸分量對應(yīng)的Gauss?Markov過程系數(shù)；ηi~N(0,)為相應(yīng)的過程噪聲。

對式（5）和式（7）進行離散化，可得離散狀態(tài)方程為：

上式中狀態(tài)轉(zhuǎn)移矩陣A與過程噪聲vk分別為：

其中，

假設(shè)過程噪聲均值為0，則過程噪聲協(xié)方差矩陣為：

對角上的協(xié)方差矩陣分別為：

2.2 觀測模型

以靜止條件下加速度計和磁傳感器的輸出ak，mk為觀測值。各個傳感器的輸出模型可寫為參考系中的自然矢量到體坐標(biāo)系的變換加上測量誤差。

g—當(dāng)?shù)刂亓铀俣龋?/p>

根據(jù)式（13）可進一步構(gòu)建觀測模型為：

3 基于迭代擴展卡爾曼濾波的姿態(tài)估計算法

在上一節(jié)建立狀態(tài)－空間模型的基礎(chǔ)上，現(xiàn)在可以直接利用IEKF算法對姿態(tài)四元數(shù)及各個傳感器的偏差進行估計，IEKF算法在目標(biāo)跟蹤領(lǐng)域已經(jīng)比較成熟，其原理和推導(dǎo)過程可見文獻[11?12]。在此僅給出算法關(guān)鍵步驟。

（1）時間更新

（2）觀測更新

當(dāng)?shù)猎O(shè)定次數(shù)M或狀態(tài)值穩(wěn)定時，令

在觀測更新過程中，由于載體本身機動，會產(chǎn)生較大幅值的運動加速度，這一方面會降低測得加速度中包含的姿態(tài)信息量，使得姿態(tài)估計誤差增大，另一方面會與式（13）給出的觀測模型是不相符的，可能導(dǎo)致濾波器發(fā)散。這里采用對觀測值進行閾值判別的方法來改善，即采用如下判別準(zhǔn)則來確定是否進行下一次迭代：

式中：g—重力加速度；

ai,i=x,y,z—加速度計的測量值。

若滿足上式，則認為當(dāng)前的加速度觀測值可以用于觀測更新，否則不進行觀測更新。

對于觀測方程中的磁傳感器測量模型，當(dāng)?shù)氐卮艆⒖际噶恳话阈枰ㄟ^全球地磁模型計算獲得，為避免對于外部數(shù)據(jù)的依賴，這里中采用構(gòu)造磁參考量的方法，即在式（13）中用近似值bN替代bN。

當(dāng)前估計得到的磁傳感器零偏誤差。

根據(jù)上述過程可以得到基于IEKF 的姿態(tài)估計算法步驟如下：

（2）根據(jù)上一時刻狀態(tài)估計值和基于式（11）和式（12）計算出的過程噪聲協(xié)方差矩陣，利用（15）計算當(dāng)前狀態(tài)預(yù)測值和相應(yīng)的協(xié)方差矩陣Pk|k?1。

（3）若當(dāng)前加速度計的觀測值滿足式（17），則利用前述的IEKF觀測更新步驟計算，Pk|（k計算過程中利用式（18）構(gòu)造磁參考量）；若不滿足，否則回到步驟（2）。

（4）對當(dāng)前狀態(tài)估計值中的四元數(shù)部分進行歸一化?；氐讲襟E（2）或算法結(jié)束。

4 結(jié)果與分析

本小節(jié)基于一個開放的姿態(tài)測量數(shù)據(jù)集[13?15]來驗證這里方法的有效性，同時給出常用的互補濾波姿態(tài)估計方法對該數(shù)據(jù)集的處理結(jié)果和誤差情況作為性能對比。該數(shù)據(jù)集中包括幾種常用低成本慣性測量單元原始數(shù)據(jù)、商用級姿態(tài)傳感器姿態(tài)輸出結(jié)果以及ABB公司工業(yè)機器人給出的姿態(tài)四元數(shù)。各個傳感器輸出數(shù)據(jù)均是以ABB機器人為平臺，在與之固聯(lián)的條件下同步測量得到。因此，工業(yè)機器人輸出的姿態(tài)四元數(shù)可以作為真實姿態(tài)參考。此外，采用同樣與ABB機器人固聯(lián)同步測量的兩型商用高精度姿態(tài)單元Xsens MTi?30和PNI SENTRAL M&M給出的姿態(tài)估計結(jié)果作為對比。

這里方法和其他幾種姿態(tài)估計算法的輸入均采用數(shù)據(jù)集中包含的Inversense 公司MPU9150 型IMU 測量數(shù)據(jù)，除集成有MEMS加速度計和陀螺儀外，該型IMU還包括了一個磁羅盤（三軸磁強計）。測量數(shù)據(jù)包括IMU與機器人固聯(lián)轉(zhuǎn)動時MPU9150傳感器測得的三軸加速度、三軸角速度與三軸磁場數(shù)據(jù)。IMU與ABB機器人固聯(lián)轉(zhuǎn)動的軌跡與姿態(tài)，如圖1所示。機器人轉(zhuǎn)動速度為0.3m/s。

圖1 IMU與ABB機器人固聯(lián)轉(zhuǎn)動軌跡與姿態(tài)Fig.1 Trajectory and Orientation of ABB Robot Borne IMUs

MPU9150測得的三軸加速度，三軸角速度，三軸磁場等原始數(shù)據(jù)，如圖2所示。圖中顯然可見轉(zhuǎn)動過程中的數(shù)次機動在傳感器測量結(jié)果導(dǎo)致了不同程度的瞬態(tài)變化。基于該組數(shù)據(jù)，采用這里方法與Madgwick互補濾波算法進行對姿態(tài)四元數(shù)進行估計求解。

圖2 MPU9150原始數(shù)據(jù)（轉(zhuǎn)動速度0.3m/s）Fig.2 Raw Data from MPU9150（Spinning Sspeed 0.3m/s）

為了直觀地比較姿態(tài)估計準(zhǔn)確性，利用式將各個算法估計及機器人給出的參考四元數(shù)轉(zhuǎn)換為歐拉角。這里算法與Madgwick互補濾波算法得到的歐拉角，MTi?30 給出的歐拉角以及PNI SENTRAL M&M給出的歐拉角與機器人運動過程中的參考姿態(tài)的對比情況，如圖3~圖6所示。（圖中：roll—橫滾角；pitch—俯仰角；yaw—偏航角，下文同）

圖3 這里算法姿態(tài)估計vs.參考姿態(tài)Fig.3 Proposed vs.Referenced

圖6 PNI SENTRAL M&M傳感器姿態(tài)估計vs.參考姿態(tài)Fig.6 Orientation Estimates：PNI SENTRAL M&M vs.Referenced

從圖中可見，相比各個算法及姿態(tài)測量單元給出的結(jié)果，根據(jù)這里算法計算出的歐拉角與機器人得到的歐拉角整體上最為接近，且在機動情況下的姿態(tài)跟蹤響應(yīng)要優(yōu)于其他方法給出的結(jié)果，同時可見兩型姿態(tài)傳感器在機動時均出現(xiàn)了較大的姿態(tài)誤差。

圖4 Madgwick算法姿態(tài)估計vs.參考姿態(tài)Fig.4 Orientation Estimates：Madgwick vs.Referenced

圖5 XSENS MTi?30傳感器姿態(tài)估計vs.參考姿態(tài)Fig.5 Orientation Estimates：MTi?30 vs.Referenced

根據(jù)上述兩種算法估計結(jié)果和兩型姿態(tài)傳感器輸入結(jié)果，通過下式進行計算時間平均的均方根誤差（RMSE）。

式中：T—測量時間；

γkk時刻的（任一）歐拉角的參考值和估計值。

進一步地，依據(jù)式（19）給出了各個方法所得歐拉角的（時間）均方根誤差，如圖7所示。圖中顯然可見這里算法的誤差最小，這表明這里算法的姿態(tài)估計結(jié)果與ABB機器人給出參考姿態(tài)在時間均方意義上最為接近；圍繞均方根誤差值的標(biāo)準(zhǔn)差，如圖8所示。可見這里方法估計誤差在整體上的波動最小。

圖7 各個方法所得歐拉角的均方根誤差Fig.7 Root Mean Square Error of Euler Angle by Each Method

圖8 各個方法所得歐拉角誤差圍繞均方根誤差的標(biāo)準(zhǔn)差Fig.8 Std.of Euler Angle Centered by RMSE for Each Method

5 總結(jié)

這里針對低成本AHRS系統(tǒng)姿態(tài)估計準(zhǔn)確性不足問題提出一種基于非線性濾波求解的姿態(tài)估計方法。通過構(gòu)建基于直接形式姿態(tài)估計的非線性狀態(tài)空間模型采用迭代擴展卡爾曼濾波方法實現(xiàn)了對姿態(tài)四元數(shù)與傳感器偏差的實時估計。通過實測數(shù)據(jù)對這里算法進行了驗證。結(jié)果表明，這里算法在姿態(tài)估計準(zhǔn)確性與估計精度上優(yōu)于現(xiàn)有常用姿態(tài)估計方法。下一步需要針對如何從算法優(yōu)化和模型改進上進一步提高基于直接形式非線性姿態(tài)估計方法的計算效率等方面展開研究。