微小型飛行器多傳感器融合容積姿態估計*

石章松，吳中紅，劉健，傅冰

(海軍工程大學 電子工程學院，湖北 武漢 430033)

微小型飛行器多傳感器融合容積姿態估計*

石章松，吳中紅，劉健，傅冰

(海軍工程大學 電子工程學院，湖北 武漢 430033)

針對微機電系統(MEMs)陀螺儀精度低、噪聲大且誤差隨時間累積的問題，擴展卡爾曼濾波(EKF)線性化誤差的問題和無跡卡爾曼濾波(UKF)時間耗費大的問題，提出了一種歐拉角容積卡爾曼濾波(CKF)姿態估計方法。建立了歐拉角姿態運動學模型，以姿態角為狀態量、加速度計和磁強計輸出解算得到的姿態角為觀測量，采用容積數值積分理論來計算非線性函數的均值與方差，實現了多傳感器輔助的微小型飛行器(MAVs) CKF姿態估計方法。仿真結果表明：估計精度方面，CKF與UKF相當，優于EKF；濾波穩定性方面，CKF與UKF相當，顯著優于EKF；時間耗費方面，CKF優于UKF。

非線性濾波；容積卡爾曼濾波；歐拉角；多傳感器；姿態估計；微小型飛行器

0 引言

姿態參數是微小型飛行器MAVs實現穩定飛行的關鍵參數之一[1-2]。MAVs載荷較輕，不適合搭載光纖/激光姿態測量系統，通常搭載微機電系統MEMS陀螺儀進行姿態測量。MEMS陀螺儀動態性能良好，但是精度低、噪聲大，且其測量誤差會隨著時間積累，不適合長時間載體姿態確定。而測量重力場分量的加速度計與測量地磁場分量的磁強計則具有良好的靜態性能，不存在誤差積累問題，但是易受載體機動、外部磁場影響，從而導致動態性能下降[3]。將陀螺儀、加速度計以及磁強計的數據進行融合處理，則能夠發揮加速度計與磁強計的靜態優點，對陀螺儀動態誤差進行補償，從而有效提高姿態測量的精度。

文獻[4-5]分別開展了將重力場與地磁場數據輔助MEMS器件進行姿態估計的研究，應用擴展卡爾曼濾波EKF[6-7]實現了對姿態參數的估計。通過對非線性系統的一階線性化近似，EKF能夠較好地處理一般的非線性系統，但是由于忽略了系統的部分非線性特性，當系統的非線性特性較為突出、初始誤差較大時，EKF存在估計效果急劇下降和濾波收斂速度緩慢的問題，不能達到可靠的估計效果[8]。而無跡卡爾曼濾波UKF[9-10]采用無跡變換進行隨機變量傳播，以獲取更為準確的非線性函數概率分布，能夠更好地應對非線性特征明顯的非線性系統估計問題[11]，文獻[12]將UKF方法引入到姿態估計中，克服了EKF在姿態估計中的線性化誤差問題，提高了姿態估計的精度。

但是UKF需要對3個可調參數進行適當選擇才能達到良好的濾波相關，且需要2n+1個采樣點，收斂時間相比于EKF有一定的增加[12]，在工程實現上存在限制[13]。而容積卡爾曼濾波CKF[14]使用一組等權值的容積點集來計算非線性變換后的隨機變量的均值和協方差，具有更優的非線性逼近性能(三階矩)、數值精度以及濾波穩定性，同時具有實現簡單、運算時間短的特點，目前廣泛應用于數據融合[15]、姿態估計[16]以及移動機器人位姿估計[17]問題中。

針對UKF存在的濾波穩定性較差、耗時較長以及參數設置困難等問題，本文在文獻[12]的基礎上，將CKF引入到基于歐拉角描述的姿態估計中，將陀螺儀、加速度計以及磁強計的數據融合處理，在保證相同濾波精度的前提下，提高了濾波穩定性，降低了估計耗時。

1 系統建模

1.1 多傳感器測量結構

多傳感器測量結構由陀螺儀、加速度計和磁強計組成，如圖1所示。陀螺儀輸出的角速率信息可以通過積分獲得載體姿態，具有很好的動態性能和短時精度，但是精度較低，姿態誤差隨時間積累很快；加速度計則可以通過感知重力加速度在其測量軸上分量的大小來確定載體的姿態角，具有很好的長期穩定性，但是受載體機動加速度影響嚴重；磁強計則能測得地磁場在載體坐標系的投影。采用CKF對上述數據進行融合，可以獲得靜態漂移和動態特性均較好的姿態估計[18-19]。

圖1 多傳感器測量結構Fig.1 Multi-sensor measurement structure

1.2 狀態方程

常見的姿態運動學模型主要有歐拉角、四元數以及羅德里格斯參數等。其中歐拉角法具有簡便直觀、物理含義明確的優點，且不存在冗余參數，雖然在俯仰角為90°時存在奇異，但對MAVs而言，俯仰角基本不會到達90°，因此本文采用歐拉角法對姿態運動學進行描述來建立狀態方程。取狀態向量為

(1)

式中：φ，θ，γ分別為偏航角、俯仰角、滾動角。

設陀螺儀輸出的角速度為(ωb，x，ωb，y，ωb，z)T，則有

(2)

考慮常值誤差和測量噪聲，式(2)變為

(3)

式中：v1，v2，v3為陀螺輸出數據中的測量噪聲。

又

(4)

將式(3)帶入式(4)，可得狀態方程為

(5)

1.3 量測方程

基于雙向量法建立量測方程[20]。首先，通過重力向量求解滾轉角φ 和俯仰角θ 。通常認為在導航系下，重力向量是不變的，可以表示為gn=(0，0，g)T，其中g為當地重力加速度。記載體坐標系下的重力向量為gb=(gbx，gby，gbz)T，則有

(7)

則由式(6)可得滾轉角φk、俯仰角θk為

(8)

然后，基于磁強計的輸出得到偏航角ψk為

(9)

式中：Hb=(Hbx，Hby，Hbz)T為磁強計的輸出值。

綜合式(8)，(9)根據加速度計與磁強計的輸出，可得量測方程為

(10)

式中：wk為量測噪聲，其均值為0且方差為Rk。

2 容積卡爾曼濾波方法

假設一個非線性系統：

(11)

式中：xk∈Rn為k 時刻系統的狀態向量；zk∈Rm為k時刻系統的觀測向量；wk為均值為0、協方差為Qk的n 維隨機過程噪聲；vk為均值為0、協方差為Rk的m 維隨機量測噪聲，且wk，vk互不相關。CKF通過三階容積積分原理，計算函數的標準加權高斯積分[15]。標準的CKF步驟如下：

(1) 初始化

(12)

(2) 時間更新

1) 設k-1時刻協方差矩陣Pk-1|k-1正定，對其進行因式分解得到Sk-1|k-1，即

(13)

2) 容積點估計

(14)

3) 容積點傳播

(15)

4) 求解狀態一步預測值

(16)

5) 計算預測誤差協方差矩陣

(17)

(3) 量測更新

1) 預測誤差協方差矩陣分解

(18)

2) 容積點估計

(19)

3) 容積點傳播

(20)

4) 計算量測預測值

(21)

(22)

(23)

(4) 狀態更新

1) 求解Kalman增益

(24)

(25)

3 仿真校驗

通過計算機仿真對文中方法的有效性進行驗證。仿真參數：狀態初始值為x0=(0，0，0)T，MAV的角速度為ω=(0.05，0.05，0.05)Trad/s，陀螺儀采樣間隔為0.1s，初始估計的誤差協方差矩陣為P0=diag(1，1，1)，過程噪聲協方差矩陣Qk=diag(0.0012，0.0012，0.0012)，量測噪聲協方差矩陣為Rk=diag(0.0052，0.0052，0.0052)，100次MonteCarlo仿真的均值結果如圖2～4以及表1～4所示。

從估計精度、濾波穩定性以及運算耗時3個方面對仿真結果進行分析。

圖2，3和表1，2反映了不同濾波方法的估計精度。可以看出，CKF的估計精度與UKF估計精度基本一致，均優于EKF，以估計誤差MonteCarlo仿真絕對均值進行比較，在滾轉角、俯仰角和偏航角3個維度上相比于EKF分別提高約15.8%，16.8%，19.8%，以估計誤差均方差MonteCarlo仿真均值比較，則分別提高29.7%，28.7%，30.7%。

圖2 100次Monte Carlo仿真估計誤差均值比較Fig.2 Comparison of estimation mean error of 100 Monte Carlo simulations with different filters

圖3 100次Monte Carlo仿真估計誤差均方差比較Fig.3 Comparison of mean square error of 100 Monte Carlo simulations with different filters

圖4、表3反映了不同濾波方法的濾波穩定性。采用歸一化方差(normalizederrorsquared，NES)指標進行比較：

表1 歐拉角的估計誤差絕對均值Table 1 Absolute mean value of the estimation error of Euler angle mrad

表2 歐拉角的估計均方差均值Table 2 Mean value of the estimation mean square error of Euler angle mrad

圖4 100次Monte Carlo仿真NES指標對比Fig.4 Comparison of 100 Monte Carlo simulations with NES index

算法EKFUKFCKFNES均值(MNES)12.10654.37794.3818

表4 100次Monte Carlo仿真耗時Table 4 Time cost of 100 Monte Carlo simulations s

NES 指標反映濾波的穩定性和一致性，當 NES 指標的數值越接近估計狀態量的維數(本文中狀態量維數為 3)，說明該濾波算法的穩定性和一致性越好。仿真結果顯示，CKF與UKF的穩定性相一致，均明顯優于EKF。

表4顯示了不同濾波方法的運算耗時。EKF的運算效率最優，CKF次之，UKF最差，其中CKF為EKF耗時的1.474倍，UKF為EKF的1.730倍，但CKF單次運算周期為0.096 s，支持150 Hz以上的姿態角更新，滿足MAVs姿態估計實時性要求。

4 結論

(1) 估計精度方面，CKF與UKF相當，均優于EKF，以估計誤差為指標進行比較，CKF在滾轉角、俯仰角和偏航角3個維度上相比EKF分別提高了15.8%，16.8%，19.8%，以誤差均方差做指標進行比較，則分別提高了29.7%，28.7%，30.7%；

(2) 濾波穩定性與一致性方面，以NES為指標進行比較，CKF與UKF相當，均顯著優于EKF；

(3)時間耗費方面，CKF劣于EKF，但優于UKF，其中CKF為EKF耗時的1.474倍，為UKF耗時的0.816倍，盡管劣于EKF，但單次解算周期僅為0.096 s，支持150 Hz以上的姿態角更新，滿足MAVs姿態估計實時性要求。

[1] 周婧，高印寒，劉長英，等.基于自適應算法的單目視覺系統的姿態解算[J].光學精密工程，2012，20(12)：2796-2803. ZHOU Jing，GAO Yin-han，LIU Chang-ying，et al.Attitude Calculation of Single Camera Visual System Based on Adaptive Algorithm[J].Optics and Precision Engineering，2012，20(12)：2796-2803.

[2] 王琳，何昕，魏仲慧.采用多站圖像直線特征的飛機姿態估計[J].光學精密工程，2013，21(7)：1831-1839. WANG Lin，HE Xin，WEI Zhong-hui.Attitude Estimation of Aircrafts Using Line Features on Multi-Camera Images[J].Optics and Precision Engineering，2013，21(7)：1831-1839.

[3] 宋英麟，鮮斌，茹濱超，等.無人機微型姿態航向系統數據處理[J].中南大學學報：自然科學版，2013，44(Sl2)：89-93. SONG Ying-lin，XIAN Bin，RU Bin-chao，et al.Micro Attitude and Heading Reference System for Unmanned Aerial Vehicles[J].Journal of Central South University：Nature Science ed,2013，44(Sl2)：89-93.

[4] 張欣，白越，趙常均，等.多旋翼姿態解算中的改進自適應擴展Kalman算法[J].光學精密工程，2014，22(12)：3384-3390. ZHANG Xin，BAI Yue，ZHAO Chang-jun，et al.Improved Adaptive Extended Kalman Algorithm for Attitude Estimation of Multi-Rotor UAV[J].Optics and Precision Engineering，2014，22(12)：3384-3390.

[5] 宋宇，翁新武，郭昕剛.基于四元數EKF算法的小型無人機姿態估計[J].吉林大學學報:理學版，2015，53(3)：511-518. SONG Yu，WENG Xin-wu，GUO Xin-gang.Small UAV Attitude Estimation Based on the Algorithm of Quaternion Extended Kalman Filter[J].Journal of Jilin University:Science Edition ed，2015，53(3)：511-518.

[6] ASADIAN A，KERMANI M R，PATEL R V.A Novel Force Modeling Scheme for Needle Insertion Using Multiple Kalman Filters [J].IEEE Trans.Instrum.Meas.，2012，61(2)：429-438.

[7] SAHA M，GHOSH R，GOSWAMI B.Robustness and Sensitivity Metrics for Tuning the Extended Kalman Filter [J].IEEE Trans.Instrum.Meas.，2014，63(4)：964-971.[8] 魏喜慶，宋申民.基于改進容積卡爾曼濾波的奇異避免姿態估計[J].航空學報，2013，34(3)：610-619. WEI Xi-qing，SONG Shen-min.Improved Cubature Kalman filter Based Attitude Estimation Avoiding Singularity[J].Acta Aeronautica et Astronautica Sinica，2013，34(3)：610-619.

[9] KANDEPU R，FOSS B，IMSLAND L.Applying the Unscented Kalman Filter for Nonlinear State Estimation [J].J.Process Control，2008，18(7-8)：753-768.

[10] DINI D H，MANDIC D P，JULIER S J.A Widely Linear Complex Unscented Kalman Filter [J].IEEE Signal Process.Lett.，2011，18(11)：623-626.

[11] GUSTAFSSON F，HENDEBY G.Some Relations between Extended and Unscented Kalman Filters [J].IEEE Trans.Signal Process，2012，60(2)：545-555.

[12] 賈瑞才.重力/地磁輔助的歐拉角無跡卡爾曼濾波姿態估計[J].光學精密工程，2014，22(12)：3280-3286. JIA Rui-cai.Attitude Estimation Base on Gravity/ Magnetic Assisted Euler Angle UKF[J].Optics and Precision Engineering，2014，22(12)：3280-3286.

[13] 劉華，吳文，王世元.基于平方根CKF的多傳感器序貫式融合跟蹤算法[J].系統工程與電子技術，2015，37(7)：1494-1498. LIU Hua，WU Wen，WANG Shi-yuan.Multi-Sensor Sequential Fusion Tracking Algorithm Based on Square-Root Cubature Kalman Filter[J].Systems Engineering and Electronics，2015，37(7)：1494-1498.

[14] ARASARATNAM I，HAYKIN S.Cubature Kalman Filters [J].IEEE Trans.on Autoynatic Control，2009，54(6)：1254-1269.

[15] 胡振濤，曹志偉，李松，等.基于容積卡爾曼濾波的異質多傳感器融合算法[J].光電子·激光，2014，25(4)：697-703. HU Zhen-tao，CAO Zhi-wei，LI Song，et al.Hererogeneous Multi-Sensor Fusion Algorithm Based on Cubature Kalman Filter[J].Journal of Optoelectronics·Laser，2014，25(4)：697-703.

[16] 錢華明，黃蔚，葛磊.基于改進的迭代容積卡爾曼濾波姿態估計[J].哈爾濱工業大學學報，2014，46(6)：116-122. QIAN Hua-ming，HUANG Wei，GE Lei.Attitude Estimation Based on Improved Iterated Cubature Kalman Filter [J].Journal of Harbin Institute of Technology，2014，46(6)：116-122.

[17] 康軼非，宋永端，宋宇，等.平方根容積卡爾曼濾波在移動機器人SLAM中的應用[J].機器人，2013，35(2)：186-193. KANG Yi-fei，SONG Yong-duan，SONG Yu，et al.Square-Root Cubature Kalman Filter and Its Application to SLAM of an Mobile Robot[J].Robot，2013，35(2)：186-193.

[18] 王松，田波，戰榆莉，等.基于修正EKF的微小型飛行器姿態估計[J].高技術通訊，2011，21(6)：612-618.

WANG Song，TIAN Bo，ZHAN Yu-li，et al.Flight Attitude Estimation for MAVs Based on Amended EKF[J].Chinese High Technology Letters，2011，21(6)：612-618.

[19] 賈瑞才.基于四元數EKF的低成本MEMS姿態估計算法[J].傳感技術學報，2014，27(1)：90-95. JIA Rui-cai.Attitude Estimation Algorithm for Low Cost MEMS Based on Quaternion EKF[J].Chinese Journal of Sensors and Actuators，2014，27(1)：90-95.

[20] JONGHEE B，YOUDAN K.Nonlinear Estimation for Spacecraft Attitude Using Decentralized Unscented Information Filter[C]∥International Conference on Control Automation and Systems.Korea：2010：1562-1566.

Cubature Attitude Estimation for Micro Aerial Vehicles Based on Multi-Sensor Fusion

SHI Zhang-song，WU Zhong-hong，LIU Jian，FU Bing

(Naval University of Engineering，College of Electronic and Engineering，Hubei Wuhan 430033，China)

For micro electromechanical system (MEMS), the gyroscope has the problem of low accuracy, high noise and time quickly accumulated error; while extended kalman filter (EKF) has the problem of linearization error; and unscented kalman filter (UKF) has the problem of high time cost. Therefore, a cubature attitude estimation method for micro aerial vehicle is proposed. An attitude kinematics model based on Euler angles is established. Taking attitude angle as the filter state and outputs of accelerometer and magnetometer as measurement, the cubature numerical integration theory is used to calculate the mean and variance of the nonlinear function. On this basis, cubature Kalman filter (CKF) attitude estimation for micro aerial vehicle with multi-sensor assisted is realized. Simulation results show that the estimation accuracy of CKF is equivalent to that of UKF and better than EKF; the stability of CKF is equivalent to UKF and better than EKF; and the time consumption of CKF is better than UKF.

nonlinear filtering; cubature Kalman filter；Euler angles；multi-sensor；attitude estimation；micro aerial vehicles

2016-03-15；

2016-08-10

有

石章松(1975-)，男，湖北黃石人。教授，博士，主要研究方向為信息融合，目標定位與跟蹤。

通信地址：430033 湖北省武漢市解放大道717號147號信箱 E-mail：yizhousan@163.com

10.3969/j.issn.1009-086x.2017.03.006

TP273;TP212.9;TP391.9

A

1009-086X(2017)-03-0034-06