姿態解算與外力加速度同步估計算法

孟唐宇 浦劍濤 方建軍 梁嵐珍

摘要：針對慣性導航應用中，姿態解算與外力加速度估計互相干擾的問題，提出一種基于四元數和擴展卡爾曼濾波器的姿態解算與外力加速度同步估計算法。首先，利用估計的外力加速度修正傳感器加速度數據得到準確的反向重力加速度，再結合地磁場向量通過梯度下降算法解算得到旋轉四元數的測量值；其次，構建擴展卡爾曼濾波模型，對旋轉四元數和外力加速度進行更新，得到旋轉四元數的預測值和外力加速度的預測值；最后，用旋轉四元數的測量值和測量得到的加速度數據對預測值通過擴展卡爾曼濾波的方法進行校正，最終得到準確的旋轉四元數和參考坐標系下三軸方向上的外力加速度。實驗表明，通過擴展卡爾曼濾波同時對姿態和外力加速度估計的方法，能夠迅速收斂并準確得機體姿態信息以及外力加速度信息，歐拉角誤差為±1.95°，加速度誤差為±0.12m/s2，并且該算法能有效抑制外力加速度對姿態解算的影響，準確估計外力加速度。

關鍵詞：姿態解算；四元數；擴展卡爾曼濾波；梯度下降法；外力加速度

中圖分類號：TP273+.5 文獻標志碼：A

Abstract：Aiming at the problem of mutual interference between attitude algorithm and external force acceleration estimation in inertial navigation system， a new method based on quaternion and extended Kalman filter was proposed. Firstly， the acceleration data of the sensor was corrected by using the estimated external force acceleration data to obtain the accurate reverse gravity acceleration， combined with geomagnetic field vector and calculated by the gradient descent algorithm， the rotate quaternions were obtained. Secondly， the extended Kalman filter model was constructed to update the rotate quaternions and external force acceleration， the prediction value of rotate quaternions and the external force were obtained. Finally， the measured values of rotate quaternions and the acceleration data were corrected by Kalman filtering method， the accurate rotate quaternions and the external force acceleration of the three axis directions in reference coordinate system were obtained. The experimental results show that the method for the synchronization estimation of attitude and external force acceleration by extended Calman filter can quickly converge and accurately get the information of the attitude and the external force acceleration， its Euler angle error is ±1.95° and acceleration error is ±0.12m/s2. The method can effectively restrain the influence of the external force acceleration on the attitude algorithm， and accurately estimate the external force.

Key words：attitude algorithm； quaternion； extended Kalman filter； gradient descent algorithm； external force acceleration

0 引言

在慣性導航應用中，如機器人、汽車產業和可穿戴設備等領域，常要求運動著的機體能夠確定其自身當前的姿態，以及其外力加速度數據，來估算出機體的位置信息。目前常采用的方法是，用慣性測量傳感器測量機體的慣性數據，通過算法解算出當前載體的姿態，再根據姿態信息解算得出外力加速度。

當機體加速移動時，外力加速度的存在會嚴重影響姿態解算的結果，當外力加速度越大時，求解的姿態角與實際值偏差越大。姿態解算算法主要有歐拉角法[1]、方向余弦法[2-3]和四元數法[4]等。歐拉角法是采用對反向重力向量的反三角函數求解的方法，雖然計算量小但存在萬向節死鎖問題；方向余弦法是通過旋轉矩陣對載體坐標系進行轉換，計算過程中要對旋轉矩陣中的9個數據和傳感器數據進行計算，所以計算量通常較大；四元數法相較于方向余弦法，采用四維數據進行計算，計算量大幅下降。在算法的運用中Wang等[5]運用四階龍格庫塔法和旋轉矩陣的方法進行計算，并利用每次的計算結果來校正傳感器的誤差參數。這樣雖然減小了計算量但是忽略了外力加速度對姿態的影響，實驗結果可能會受到外力加速度的影響。Madgwick[6]運用梯度下降法解算載體姿態，這樣計算在靜態時準確度較高，但在動態時會產生姿態角的大幅抖動，并且過度依賴加速度計數據，使得外力加速度對計算姿態的影響更加明顯。在算法的運用中如成怡等[7]利用非線性濾波，閻世梁等[8]運用互補濾波， Bachmann等[9]卡爾曼濾波，Madgwick等[10]運用梯度下降法， Marins等[11]利用擴展卡爾曼濾波算法。以上算法中為了消除外力加速度對姿態的影響，均將加速計傳感器采集到的數據進行歸一化，再進行姿態解算，這樣可以減小外力加速度對反向重力向量的影響，但仍然沒有從根本上消除外力加速度的影響，反向重力向量估計依然是不準確的。本文在姿態解算過程中，利用擴展卡爾曼濾波器模型，對姿態與外力加速度同步進行估計，不僅在姿態解算時消除了外力加速度對姿態解算精度的影響，同時可以得到外力加速度信息。

在外力加速度估計算法運用中張甜等[12]利用全球定位系統（Global Positioning System， GPS）基站方式進行加速度估計，如果在室內、地下室等GPS信號弱的環境下無法測量出機體的運動加速度。李海濤[13]利用電子羅盤修正加速度計信號的方法對機體外力加速度進行估計，然而電子羅盤信號容易受到外界電磁干擾，所得外力加速度會受到一定影響，因此姿態解算也會產生一定誤差。

本研究采用基于四元數和擴展卡爾曼濾波器對機體姿態和外力加速度同時估計。利用外力加速度的估計值對采集到的加速度數據進行修正，得到修正后的反向重力加速度再結合磁場向量，采用梯度下降法估計出四元數測量值與加速度測量值；在此基礎上，用擴展卡爾曼濾波器對預測四元數和預測外力加速度值同時進行修正，從而得到當前狀態下機體準確的旋轉四元數和參考坐標系下的外力加速度。

1 四元數介紹及基本運算

四元數應用在姿態解算中不僅計算量小還可以消除萬向節死鎖問題，并具有能夠全角度解析姿態角等優點， 因此采用四元數作為姿態解算的數學推導演算依據。

1.1 四元數表示形式

對三角函數的±180°反解，得到姿態歐拉角的全角度解析，所得四元數為當前載體坐標系旋轉到參考坐標系的四元數。從而有效解決歐拉角法產生的萬向節死鎖問題，同時減小運算量，并且提高運算速度。

2 擴展卡爾曼濾波姿態解算

為了預測非線性四元數和外力加速度值，引入擴展卡爾曼濾波器[11]。擴展卡爾曼濾波器能夠預測出當前載體坐標系旋轉到參考坐標系的旋轉四元數和外力加速度，并通過測量值進行修正從而得到當前狀態下準確的旋轉四元數和外力加速度。這樣計算得出的四元數不受外力加速度影響，計算出的姿態角在機體非勻速運動時波動小、外力加速度信息準確，同時分解出的外力加速度可用于慣性導航的研究。

擴展卡爾曼濾波姿態解算基本流程如圖1所示。首先，利用陀螺儀數據更新旋轉四元數，并且更新外力加速度數據；其次，估計預測協方差Pk以及擴展卡爾曼濾波增益K。再次，根據更新得到的旋轉四元數計算載體坐標系下的反向重力向量；最后，根據測量值得到狀態變量的最優估計并且更新協方差。其中xk表示狀態變量，k表示當前狀態，k-1表示上一時刻狀態；Pk-1表示上一時刻協方差的值。

當前所得四元數，即為載體轉向參考坐標系的旋轉四元數，所得加速度為載體坐標系中三軸方向的加速度。通過四元數相乘的方法，把所得外力加速度轉換成參考坐標系中三軸加速度，為實現慣性定位提供有效的加速度信息。

3 梯度下降法姿態解算

實際應用表明，僅把加速計采集到的數據作為反向重力向量，這樣數據中隱含的外力加速度會影響旋轉四元數測量值求解的準確度。為解決這一問題，先利用外力加速度的估計值修正加速計數據以得到準確的反向重力向量，再根據修正后的反向重力向量與磁場向量解算出測量四元數，從而減少外力加速度對旋轉四元數的影響。

3.1 利用修正后的反向重力向量解算橫滾角和俯仰角

4 算法融合及實驗結果分析

4.1 梯度下降法和擴展卡爾曼濾波融合

融合算法基本流程如圖2所示。

梯度下降法循環計算2～3次，得到的四元數和加速計測值作為擴展卡爾曼濾波的測量值。融合算法結構如圖3所示，其中Mag代表電子羅盤傳感器所測量的磁場向量，Acc代表加速度傳感器所測量的加速度向量，Gyr代表陀螺儀傳感器所測量的角速度向量，acc代表計算得出的參考坐標系下機體外力加速度向量。

4.2 實驗結果分析

實驗MCU選用MEGA2560，傳感器選用MPU6050、HMC5883L實驗平臺，MEGA2560以應答方式通過串口把九軸數據傳送給Matlab進行實驗仿真。傳感器坐標系即為機體坐標系。

通過三種方式驗證算法可行性和準確度。

1）機體處于水平位置。機體姿態模型如圖4所示。所建模型和實物姿態一致。

機體處于水平位置未加擴展卡爾曼濾波與加上擴展卡爾曼濾波歐拉角對比曲線如圖5所示，Pitch、Roll、Yaw角度在加入擴展卡爾曼濾波前后均穩定在0°，波動幅度±1.95°，但加入擴展卡爾曼濾波算法后角度波動幅度比未加入擴展卡爾曼濾波算法波動幅度有所減小。歐拉角波動是因為慣性傳感器數據存在噪聲引起，經過算法過濾后波動幅度減少了10%。在靜止狀態下兩種算法解算出的歐拉角差別不大主要是應為沒有外力加速度干擾。

機體向Y正方向以0.6m/s2勻加速運動，加入擴展卡爾曼濾波算法和未加入擴展卡爾曼濾波算法歐拉角對比曲線如圖8所示，當機體做勻加速運動時，歐拉角曲線與機體靜止時相比，加入擴展卡爾曼濾波算法的Roll有所減小，但范圍在±0.5°，而沒有加入擴展卡爾曼濾波算法的Roll減小了3°。外力加速度對加入擴展卡爾曼濾波算法姿態解算的結果影響很小而對未加入擴展卡爾曼濾波算法姿態解算影響較大。因機體處于水平位置，只在Y軸有一恒定加速度，Pitch和Yaw角度均穩定在0°，Yaw會隨運動方向的誤差有1°的變化波動。

3）機體在自由運動時，加入擴展卡爾曼濾波算法和未加入擴展卡爾曼濾波算法歐拉角對比曲線如圖10，當機體處于自由運動時，加入擴展卡爾曼濾波算法與未加入擴展卡爾曼濾波算法姿態角曲線Pitch、Roll、Yaw角曲線趨勢基本相同，但未加入濾波器的算法解算出的歐拉角均偏大，說明機體在自由運動時所產生的外力加速度對未加入濾波器算法的姿態解算產生較大影響，誤差為±4.3°，而對加入濾波器算法的姿態解算影響較小，誤差為±1.94°。

5 結語

本文在姿態解算與外力加速度同步估計中，基于四元數和擴展卡爾曼濾波器。通過對外力加速度的估計，修正反向重力向量，從而解算出旋轉四元數的測量值，并利用卡爾曼濾波器對旋轉四元數與外力加速度進行更新。這樣減小了外力速度對姿態的影響，并且準確計算出姿態四元數和外力加速度。最后通過傳感器采集數據解算當前傳感器的姿態和加速度，驗證了該方法的可靠性。在Matlab中姿態更新時間間隔在15ms左右，且采用應答方式獲取傳感器信息這會產生一定時間損耗。將算法移植到嵌入式系統中進行運算，這樣不僅最大限度地降低數據在傳輸過程中產生的不必要的時間損耗，而且運算速度將大幅提升。現該算法已在室內定位小車項目中應用。

目前的研究比較集中于姿態的解算，尚未將外力加速度用于慣性導航中，這將是后續工作中要研究的一個重點工作。

參考文獻：

[1]SUSHKOV O. Robot localisation using a distributed multimodal Kalman filter， and friends[D]. Sydney： University of New South Wales， 2006.

[2]PREMERLANI W， BIZARD P. Direction cosine matrix IMU： theory[EB/OL]. [2015-04-10]. http：//gentlenav.googlecode.com/files/DCMDraft2.pdf

[3]周浩，浦劍濤，梁嵐珍，等. 基于體感的仿人機器人步態學習與控制[J]. 計算機應用，2015，35（3）：787-791.（ZHOU H， PU J T， LIANG L Z， et al. Gait learning and control of humanoid robot based on Kinect[J]. Journal of Computer Applications， 2015， 35（3）：787-791.）

[4]KUIPERS J B. Quaternions and Rotation Sequences[M]. Princeton： Princeton University Press， 1999： 127-142.

[5]WANG Y， LI N， CHEN X， et al. Design and implementation of an AHRS based on MEMS sensors and complementary filtering[J]. Advances in Mechanical Engineering， 2014， 6（6）： 1-11.

[6]MADGWICK S O H. An efficient orientation filter for inertial and inertial/magnetic sensor arrays[R/OL]. [2015-04-30]. http：//www.xio.co.uklres/doc/madgwickinternaIJeport.pdf.

[7]成怡，金海林，修春波，等.四軸飛行器組合導航非線性濾波算法[J]. 計算機應用，2014，34（S1）：341-344.（CHENG Y， JIN H L， XIU C B， et al. Nonlinear filter algorithm for quadrotor integrated navigation [J]. Journal of Computer Applications， 2014， 34（S1）：341-344.）

[8]閻世梁，王銀玲，張華.基于改進互補濾波器的低成本微小飛行器姿態估計方法[J]. 計算機應用，2013，33（7）：2078-2082.（YAN S L， WANG Y L， ZHANG H. Improved complementary filter for attitude estimation of micro air vehicles using lowcost inertial measurement units [J]. Journal of Computer Applications，2013， 33（7）：2078-2082.）

[9]BACHMANN E R， DUMAN I， USTA U Y， et al. Orientation tracking for humans and robots using inertial sensors[C]// Proceedings of the 1999 IEEE International Symposium on Computational Intelligence in Robotics and Automation. Piscataway， NJ： IEEE， 1999： 187-194.

[10]MADGWICK S O H， HARRISON A J L， VAIDYANATHAN R. Estimation of IMU and MARG orientation using a gradient descent algorithm[C]// Proceedings of the 2011 IEEE International Conference on Rehabilitation Robotics. Piscataway， NJ： IEEE， 2011： 1-7.

[11]MARINS J L， YUN X， BACHMANN E R， et al. An extended Kalman filter for quaternionbased orientation estimation using MARG sensors[C]// Proceedings of the 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE， 2001， 4： 2003-2011.

[12]張甜， 吳美平， 張開東， 等. 基于精密單點定位的航空重力儀運動加速度估計[J]. 導航與控制， 2014， 13（4）：26-31.（ZHANG T， WU M P， ZHANG K D， et al. Airborne gravitometer kinematic acceleration determination based on precise point positioning[J].Navigation and Control， 2014， 13（4）： 26-31.）

[13]李海濤. 加速度計/磁強計捷聯慣導系統姿態解算方法研究[D].太原：中北大學，2007.（LI H T. Study on attitude algorithms of accelerometers/magnetometers strapdown inertial navigation system[D].Taiyuan： North University of China， 2007.）

[14]HENDERSON D M. Euler angles， quaternions， and transformation matrices[R]. Washington， DC：NASA JSC， 1977.