姿態(tài)解算與外力加速度同步估計(jì)算法

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

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

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

中圖分類號(hào)：TP273+.5 文獻(xiàn)標(biāo)志碼：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)用中，如機(jī)器人、汽車產(chǎn)業(yè)和可穿戴設(shè)備等領(lǐng)域，常要求運(yùn)動(dòng)著的機(jī)體能夠確定其自身當(dāng)前的姿態(tài)，以及其外力加速度數(shù)據(jù)，來估算出機(jī)體的位置信息。目前常采用的方法是，用慣性測(cè)量傳感器測(cè)量機(jī)體的慣性數(shù)據(jù)，通過算法解算出當(dāng)前載體的姿態(tài)，再根據(jù)姿態(tài)信息解算得出外力加速度。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 算法融合及實(shí)驗(yàn)結(jié)果分析

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

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

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

4.2 實(shí)驗(yàn)結(jié)果分析

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

通過三種方式驗(yàn)證算法可行性和準(zhǔn)確度。

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

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

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

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

5 結(jié)語

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

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

參考文獻(xiàn)：

[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ī)器人步態(tài)學(xué)習(xí)與控制[J]. 計(jì)算機(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]. 計(jì)算機(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]閻世梁，王銀玲，張華.基于改進(jìn)互補(bǔ)濾波器的低成本微小飛行器姿態(tài)估計(jì)方法[J]. 計(jì)算機(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í)穑?吳美平， 張開東， 等. 基于精密單點(diǎn)定位的航空重力儀運(yùn)動(dòng)加速度估計(jì)[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]李海濤. 加速度計(jì)/磁強(qiáng)計(jì)捷聯(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.