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

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

關(guān)鍵詞：姿態(tài)解算；四元數(shù)；擴展卡爾曼濾波；梯度下降法；外力加速度

中圖分類號：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 引言

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

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

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

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

1 四元數(shù)介紹及基本運算

四元數(shù)應(yīng)用在姿態(tài)解算中不僅計算量小還可以消除萬向節(jié)死鎖問題，并具有能夠全角度解析姿態(tài)角等優(yōu)點， 因此采用四元數(shù)作為姿態(tài)解算的數(shù)學(xué)推導(dǎo)演算依據(jù)。

1.1 四元數(shù)表示形式

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

2 擴展卡爾曼濾波姿態(tài)解算

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

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

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

3 梯度下降法姿態(tài)解算

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

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

4 算法融合及實驗結(jié)果分析

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

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

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

4.2 實驗結(jié)果分析

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

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

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

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

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

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

5 結(jié)語

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

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

參考文獻：

[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]周浩，浦劍濤，梁嵐珍，等. 基于體感的仿人機器人步態(tài)學(xué)習(xí)與控制[J]. 計算機應(yīng)用，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]成怡，金海林，修春波，等.四軸飛行器組合導(dǎo)航非線性濾波算法[J]. 計算機應(yīng)用，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]閻世梁，王銀玲，張華.基于改進互補濾波器的低成本微小飛行器姿態(tài)估計方法[J]. 計算機應(yīng)用，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]張?zhí)穑?吳美平， 張開東， 等. 基于精密單點定位的航空重力儀運動加速度估計[J]. 導(dǎo)航與控制， 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]李海濤. 加速度計/磁強計捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)解算方法研究[D].太原：中北大學(xué)，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.