AHRS姿態估計的迭代擴展卡爾曼濾波方法

史宜巧，趙 輝

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

1 引言

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

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

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

2 四元數姿態估計的非線性問題模型

設載體自身坐標系（B系）為右－前－上坐標系，參考系（N系）為東－北－天局部導航坐標系（ENU）。姿態四元數定義為：

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

μ—N系中的轉軸矢量；

ψ—從系通過轉軸轉到系所經過的角度。

因此可以得到從B系變換到N系的姿態變換矩陣為：

在ENU坐標系下可得上式姿態變換矩陣對應的歐拉角為：

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

根據式（2）給出的變換關系，下面推導四元數姿態估計的非線性狀態空間模型。

2.1 狀態方程

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

根據各個狀態量隨著時間的動態變化過程，下面來導出狀態方程，四元數隨時間變化的動態方程為：

式中：=ω?βω；ω—陀螺儀讀數；

βω—陀螺儀偏差；

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

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

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

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

上式中狀態轉移矩陣A與過程噪聲vk分別為：

其中，

假設過程噪聲均值為0，則過程噪聲協方差矩陣為：

對角上的協方差矩陣分別為：

2.2 觀測模型

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

g—當地重力加速度；

根據式（13）可進一步構建觀測模型為：

3 基于迭代擴展卡爾曼濾波的姿態估計算法

在上一節建立狀態－空間模型的基礎上，現在可以直接利用IEKF算法對姿態四元數及各個傳感器的偏差進行估計，IEKF算法在目標跟蹤領域已經比較成熟，其原理和推導過程可見文獻[11?12]。在此僅給出算法關鍵步驟。

（1）時間更新

（2）觀測更新

當迭代至設定次數M或狀態值穩定時，令

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

式中：g—重力加速度；

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

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

對于觀測方程中的磁傳感器測量模型，當地地磁參考矢量一般需要通過全球地磁模型計算獲得，為避免對于外部數據的依賴，這里中采用構造磁參考量的方法，即在式（13）中用近似值bN替代bN。

當前估計得到的磁傳感器零偏誤差。

根據上述過程可以得到基于IEKF 的姿態估計算法步驟如下：

（2）根據上一時刻狀態估計值和基于式（11）和式（12）計算出的過程噪聲協方差矩陣，利用（15）計算當前狀態預測值和相應的協方差矩陣Pk|k?1。

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

（4）對當前狀態估計值中的四元數部分進行歸一化。回到步驟（2）或算法結束。

4 結果與分析

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

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

圖1 IMU與ABB機器人固聯轉動軌跡與姿態Fig.1 Trajectory and Orientation of ABB Robot Borne IMUs

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

圖2 MPU9150原始數據（轉動速度0.3m/s）Fig.2 Raw Data from MPU9150（Spinning Sspeed 0.3m/s）

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

圖3 這里算法姿態估計vs.參考姿態Fig.3 Proposed vs.Referenced

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

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

圖4 Madgwick算法姿態估計vs.參考姿態Fig.4 Orientation Estimates：Madgwick vs.Referenced

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

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

式中：T—測量時間；

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

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

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

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

5 總結

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