慣性測量單元姿態融合的動態分析*

謝 敏，趙來定，王召文

(1.南京郵電大學 通信與信息工程學院，江蘇 南京210003；2.南京郵電大學 通信與網絡技術國家地方聯合工程研究中心，江蘇 南京210003)

0 引言

微機電系統內部集成了動態傳感器、數字信號處理模塊、串口通信等模塊，是信息技術和機械工程的結合[1]。目前，微機電傳感器具有成本低、體積小、功耗低、可靠性高等優勢，廣泛用于高智能化的行業中。因此，慣性測量單元(Inertial Measurement Unit，IMU)常采用微機電傳感器。

慣性測量單元應用廣泛，遠至國家軍事防御，近至日常智能設備。慣性測量單元可以測量裝置或載體自身的加速度、角度等狀態數據，一般通過加速度計等傳感器進行測量。其中，陀螺儀用于測量設備自身的旋轉運動。加速度計用于測量裝置或載體的受力情況[2]。磁力計用于確定設備所處的方位。

陀螺儀易受溫度、摩擦等因素的影響，存在誤差，即零點漂移。加速度計的實時性較差，不能快速、準確做出響應，存在測量噪聲。在短時間內，加速度計數據可信度較差；相反，陀螺儀的加速度數據在短時間內可信度較強，而長時間工作后，會因漂移產生測量誤差。磁力計只針對水平情況下方位角的測量，且易受周圍環境影響，可信度單一。因此，需要有效結合傳感器，提高數據可信度。

現有的慣性器件處理算法，利用互補濾波算法修正陀螺儀的值，橢圓擬合解算得到運動載體的航向角[3]；Mahony互補濾波方法可以提高解算精度[4]；在捷聯慣導的初始對準方面有維納濾波、卡爾曼濾波[5]，在最優化算法方面有梯度下降法[6]、Newton法等。其中，梯度下降法可以有效抑制角度噪聲。互補濾波可以利用系數結合多組數據。卡爾曼濾波可以提高角度和角速度精度，同時抑制零點漂移。

實驗利用動態三軸搖擺臺測試慣性測量單元的動態特性。實驗證明，將梯度下降算法、互補濾波算法、卡爾曼濾波算法結合后，對傳感器數據進行處理，可以有效提高慣性測量單元的數據精度，使其具有良好的連續性與穩定性。

1 姿態角計算

1.1 幾何模型下的姿態角

一般地，加速度計可以測到一個動載體平臺相對于地球的大地坐標系(N系)的運動狀態。假設平臺(聯動慣性測量單元)做自由落體運動，則三軸加速度值均為零。若將加速度計測得的加速度值看作矢量R，在載體坐標系(B系)中分解成三個方向矢量R x、R y、R z，即R在xyz軸上的投影，利用三軸矢量可以得到姿態角中的俯仰角和橫搖角[7]，其幾何關系如圖1所示。

圖1 加速度計幾何模型

根據公式：

求得矢量R在yoz平面上的投影R yz[3]，R x和R yz存在正切關系，得橫搖角?，同理可得俯仰角θ。

姿態角中的航向角Ψ，在裝置水平放置時，可由磁力計單獨獲得[4]。磁力計是利用地球的南北極的微弱磁場作為判斷依據。地磁場也可看作一個矢量H地磁，在某一地點，這個矢量可以被分解為三個相互垂直的矢量，其中H x、H y與水平面相平行，H z與水平面垂直[8]。若保持磁力計xoy面和水平面平行，則磁力計的三軸與這三個矢量對應，如圖2所示。

圖2 磁力計分量示意圖

水平方向的H x和H y，矢量和指向磁北。航向角Ψ是當前方向和磁北的夾角。裝置水平可得航向角Ψ：

經過推導，可以得到一組姿態角。由三軸加速度計數據可以得到橫搖角、俯仰角，由磁力計水平分量可以得到航向角，但是，該組姿態角直接由慣性器件得到，受周圍環境影響較大，穩定性較差。

1.2 基于梯度下降法的姿態角

四元數是一種姿態角的表示方式。四元數q0、q1、q2、q3，其中q0作為實部，其余作為虛部[5]，并可由三軸角度表示。

依照四元數乘法可以得到一個代表三軸旋轉角度的四元數q。

捷聯慣導的初始對準有兩個常用坐標系[9]：一個是導航坐標系(N系)，導航坐標系是導航基準的坐標系；另一個是載體坐標系(B系)，是關于載體的坐標系。

姿態矩陣生成四元數，將導航坐標系轉換到載體坐標系。假設空間矢量在載體系和導航系中的投影分別為r′和r，四元數在兩個坐標系內轉換可以通過p′=qpq-1實現。

梯度下降法定義為標量場中某一點上的梯度指向標量場增長最快的方向，梯度的長度是最大變化率[6]，并沿著最大變化率所在的負方向尋找最優解。

通過梯度下降法將姿態矩陣存在的誤差減至最小。將誤差定義成一個關于q的函數向量f(q)，偏微分處理得到最速下降方向，擴展到三軸得到雅克比矩陣J(q)。

下面引入梯度下降法的公式，代入四元數。

將加速度計數據ax、ay、az與歸一化的重力加速度矢量[0 0 1]之間的差值作為誤差函數fg(q)，微分得雅克比矩陣J(q)。

已知誤差函數及其雅克比矩陣，可以利用梯度公式求出誤差函數的梯度▽fg(q)：

將該梯度代入梯度下降的公式，其中步長μt與實際運動時的角速度和采樣時間有關。這樣就獲得了姿態四元數q▽，t。

1.3 四元數微分方程

姿態四元數q是時間的變量，自變量為t時刻角度θt，由陀螺儀積分得到，因變量為四元數q t。u為三軸矢量i、j、k歸一化后的矢量，w t為角速度矢量[10]。

對q t求導得到另一組姿態四元數q w，t：

2 互補濾波和卡爾曼濾波

通過加速度計和磁力計數據所得姿態角，僅反映出慣性測量單元當前狀態，受到環境噪聲影響大，數據容易出現野值。雖然基于梯度下降法的數據和基于微分方程的數據在一定程度上可以抑制測量噪聲，但是梯度下降法具有“碗底”效應，步長過大時不能快速收斂。因此，需要將梯度下降法數據和微分方程數據相結合[11]。

2.1 微積分的互補濾波

將 姿態 四 元 數qΔ，t[12-13]與 姿 態 四 元 數q w，t互 補結合，得到q t：

基于四元數的微分方程適合用于物體高速運動狀態，梯度下降法適合用于低速運動。因此，高速運動下α取值要小一些，低速運動下α要大一些。從收斂速度的角度來看，當梯度下降的收斂速度等于微分方程的收斂速度時，可得最優解。設四元數微分方程的收斂速度為β。當α近似為0時適合對動態物體的測量。

式(20)中q▽，t-1相比于μt▽fg(q)所占比重較小，可忽略不計，q▽，t得到近似表達，并求得q t。 根據姿態四元數q t和姿態矩陣得到姿態角?t、θt和Ψt。

2.2 基于PI的姿態角互補濾波

對于梯度下降法，經過仿真后發現，若梯度下降的步長μ過大，則尋至最優解時間較長，就像“碗底”一樣。步長過大會越過最優值，在最優值附近持續振蕩，如圖3所示；步長太小，到達“碗底”的時間就會變長。

圖3 梯度下降法“碗底”示意圖

因為大步長會引起振蕩，使數據產生毛刺，所以在實驗過程中采用的步長較小。

磁力計和加速度計所測得角度沒有累積誤差，但動態響應較差[14]。利用互補濾波器融合三種傳感器的數據，提高測量精度和系統的動態性能。互補濾波法需要用到加速度計數據ax、ay、az，陀螺儀數據wx、wy、wz，磁力計 數據mx、my、mz。 互補濾波器 公式如下，其中C0(s)為加速度計和磁力計觀測到的姿態矩陣，Cw(s)為陀螺儀測量數據計算得到的姿態矩陣。

互補濾波器傳遞函數：

姿態矩陣：

通過與梯度下降法相同的方法得到重力分量Vx、Vy、Vz及ax、ay、az，誤差ex、ey、ez可以表示為兩者叉乘：

角速度更新四元數，并轉化為姿態角：

2.3 姿態角之間的互補

基于幾何的姿態角?、θ、Ψ，基于梯度下降法的姿 態 角?t、θt、Ψt和 基 于 互 補 濾 波 的 姿 態 角?n、θn、Ψn，這三組不同的姿態角進行姿態角之間的互補，其中濾波系數k是根據加速度計數據ax、ay、az所得[11]：

當載體處于低速運動時，梯度下降法通過小步長迭代能夠快速收斂；當載體處于高速運動時，PI互補濾波跟蹤性能更好。幾何的姿態角作為參照標準，可以防止低速所帶來的震動以及超速運動。

2.4 卡爾曼濾波

梯度下降法和PI互補濾波解決了加速度計和陀螺儀的部分噪聲和收斂問題，但陀螺儀因為重力存在零點漂移，漂移角度經過較長時間的累計，會使測量出現偏差。

卡爾曼濾波是一種基于無偏最小方差遞推準則的濾波算法[16]，它既適用于平穩隨機過程，也適用于非平穩過程。其核心是通過建立狀態模型和測量模型，利用上一狀態的估計值和這一狀態的測量值得到狀態轉移方程，并更新狀態模型。相比于需要知曉所有狀態的維納濾波，卡爾曼濾波更適合于傳感器輸出濾波處理。

卡爾曼濾波第一個公式給出狀態方程、觀測方程：

其中X k為k時刻的系統狀態矩陣；A為狀態轉移矩陣；u k為k時刻的角速度矢量；B為輸入加權矩陣；w k為系統噪聲，為零均值白噪聲過程。以t時刻俯仰角θt，陀螺儀輸出wt，零漂Biast，采樣周期Δt為例，對狀態更新方程進行二維矩陣描述。

卡爾曼濾波的第二個公式計算的是系統的協方差矩陣：

其中需要給出兩個值Qaccl、Qgyro，分別為加速度計所得到的角度噪聲、陀螺儀的角速度噪聲。Q為矩陣[θtBiast]T的協方差矩陣：

設矩陣P k-1=[a b；c d]，A已知，根據方程可得：

第三個公式通過協方差矩陣計算卡爾曼增益：

其中P k和H已知，R為觀測噪聲矩陣。在慣性測量單元系統中有兩種狀態變化量：角度θt和零漂Biast，分別對應各自的卡爾曼增益，設為K k=[k0k1]。

得到[k0k1]后，通過第四個公式對狀態矩陣X k進行修正。

寫成矩陣形式如式(49)所示，其中Z k=θaccl，是加速度計所得到的角度值，作為觀測輸入。

零點漂移值可以對陀螺儀角速度修正：

最后一個公式對協方差矩陣進行更新：

至此，即為卡爾曼濾波的五大方程[17]。輸入參數為加速度計所得角度值和陀螺儀的角速度值，以及角度、角速度噪聲值，輸出為修正后的角度和角速度。

3 算法總結

上文中算法的數據處理流程可以分為3個部分：(1)根據幾何模型計算出姿態角，即從器件寄存器直接計算得到；(2)利用加速度計和陀螺儀數據分別通過梯度下降法和PI互補濾波法得出姿態角，兩組姿態角再進行互補計算；(3)將第二步得到的姿態角和角速度通過卡爾曼濾波修正，得到最終三軸的角度和角速度。卡爾曼濾波流程圖如圖4所示。

圖4 卡爾曼算法流程圖

4 實驗結果與分析

為了驗證該姿態融合算法，本次實驗采用了STM32F405RGT6作為芯片平臺，ADIS16460作為六軸傳感器模塊，HMC5883_L作為磁力計模塊進行實驗，通過串口將數據反饋給上位機。其中，對傳感器初始化做出了改進：通過對ADIS16460的DEC_RATE將原先的采樣率從512 S/s提升至2 048 S/s，增強了慣導輸出的連續性。ADIS16460的FLTR_CTRL寄存器可以提供對數字低通濾波器的控制。此濾波器包含兩個級聯均值濾波器，它們提供Bartlett窗口、FIR濾波器響應。為了保證數據輸出的實時性，將其濾波器抽頭數從64降至16。原先數據輸出是通過外部中斷結束后輸出，現改進為2 ms的計時器中斷，一方面是為了數據的連續性，另一方面固定的間隔時間數據可以保證數據的實時性。

4.1 靜態實驗分析

實驗裝置分為兩套：第一套是原始算法，由慣性器件直接輸出數據的慣性測量單元；第二套是經過姿態融合濾波算法處理的慣性測量單元。將兩套實驗裝置靜置于桌面，以俯仰軸的輸出數據為例，進行對零點漂移的驗證。

從圖5中虛線可以看出，未經過融合濾波算法的俯仰角在靜止時總是朝著負角度方向呈現增加趨勢，最終會偏離真實值。從圖中實線可以看出，經過融合濾波算法的俯仰角在靜止時的角度總是圍繞著一個數值上下小幅度變化，不會出現偏移。因此，姿態融合濾波算法能夠有效地抑制角度偏移。

圖5 靜止俯仰角度

4.2 動態實驗驗證

將慣性測量單元放置在搖擺臺的重心位置，并設置搖擺臺運動方式為正弦運動，其幅度為20°，運動周期為8 s。共進行3次試驗，3次試驗分別包括橫搖、俯仰和方位。實驗結果如圖6～圖10所示。

圖10 動態方位角度

在動態實驗中，未進行過濾波算法修正的橫搖、俯仰和方位可以總結出以下三個問題：

(1)從圖6動態俯仰角度可以看出，原始算法數據存在著滯后的問題，在前四分之一周期內延時了約1 000個采樣點，并隨時間逐漸增多；

圖6 動態俯仰角度

(2)原始算法下俯仰角度區間會隨時間出現角度偏移，不能始終保持在正負20°之間；

(3)從圖9動態橫搖局部角度可以看出，原始數據在角度變化過程中呈現不連續的階梯狀、斷崖式的變化。

圖7 動態俯仰局部角度

圖8 動態橫搖角度

圖9 動態橫搖局部角度

經過姿態融合濾波算法的修正后，解決了俯仰軸的滯后問題，不會出現相位偏移現象；減小了零點漂移帶來的時間偏移，俯仰角度總是保持在正負20°之間；平滑了輸出角度，使其在局部呈現為一條光滑曲線。因此，經過姿態融合濾波算法的慣導裝置在動態特性的追蹤和穩定性上優于原始程序。

5 結論

本文介紹了一種基于梯度下降法、互補濾波法和卡爾曼濾波的姿態融合算法。首先，從原理上分析了梯度下降法和四元數微分法的優勢；其次，介紹了互補濾波法和卡爾曼濾波算法對姿態數據進行結合的過程；最后，通過動態分析實驗驗證了姿態融合算法的可行性。

實驗結果證明，基于梯度下降法、互補濾波法和卡爾曼濾波的姿態融合算法能夠成功地抑制加速度計的測量噪聲，并減小陀螺儀的零點漂移所帶來的影響。相比于傳統算法輸出數據的滯后、不連續，基于姿態融合算法的數據實時性強，角度曲線連續且平滑，能夠有效、準確地描述運動中物體的姿態。同時，在硬件上調整了濾波器的參數，提高了數據輸出速率。因此，基于該姿態融合算法的慣性測量單元具有良好的穩定性與可靠性，在工程應用中有一定的參考價值。