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

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

微小型飛行器多傳感器融合容積姿態(tài)估計(jì)*

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

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

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

非線性濾波；容積卡爾曼濾波；歐拉角；多傳感器；姿態(tài)估計(jì)；微小型飛行器

0 引言

姿態(tài)參數(shù)是微小型飛行器MAVs實(shí)現(xiàn)穩(wěn)定飛行的關(guān)鍵參數(shù)之一[1-2]。MAVs載荷較輕，不適合搭載光纖/激光姿態(tài)測(cè)量系統(tǒng)，通常搭載微機(jī)電系統(tǒng)MEMS陀螺儀進(jìn)行姿態(tài)測(cè)量。MEMS陀螺儀動(dòng)態(tài)性能良好，但是精度低、噪聲大，且其測(cè)量誤差會(huì)隨著時(shí)間積累，不適合長(zhǎng)時(shí)間載體姿態(tài)確定。而測(cè)量重力場(chǎng)分量的加速度計(jì)與測(cè)量地磁場(chǎng)分量的磁強(qiáng)計(jì)則具有良好的靜態(tài)性能，不存在誤差積累問題，但是易受載體機(jī)動(dòng)、外部磁場(chǎng)影響，從而導(dǎo)致動(dòng)態(tài)性能下降[3]。將陀螺儀、加速度計(jì)以及磁強(qiáng)計(jì)的數(shù)據(jù)進(jìn)行融合處理，則能夠發(fā)揮加速度計(jì)與磁強(qiáng)計(jì)的靜態(tài)優(yōu)點(diǎn)，對(duì)陀螺儀動(dòng)態(tài)誤差進(jìn)行補(bǔ)償，從而有效提高姿態(tài)測(cè)量的精度。

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

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

針對(duì)UKF存在的濾波穩(wěn)定性較差、耗時(shí)較長(zhǎng)以及參數(shù)設(shè)置困難等問題，本文在文獻(xiàn)[12]的基礎(chǔ)上，將CKF引入到基于歐拉角描述的姿態(tài)估計(jì)中，將陀螺儀、加速度計(jì)以及磁強(qiáng)計(jì)的數(shù)據(jù)融合處理，在保證相同濾波精度的前提下，提高了濾波穩(wěn)定性，降低了估計(jì)耗時(shí)。

1 系統(tǒng)建模

1.1 多傳感器測(cè)量結(jié)構(gòu)

多傳感器測(cè)量結(jié)構(gòu)由陀螺儀、加速度計(jì)和磁強(qiáng)計(jì)組成，如圖1所示。陀螺儀輸出的角速率信息可以通過積分獲得載體姿態(tài)，具有很好的動(dòng)態(tài)性能和短時(shí)精度，但是精度較低，姿態(tài)誤差隨時(shí)間積累很快；加速度計(jì)則可以通過感知重力加速度在其測(cè)量軸上分量的大小來確定載體的姿態(tài)角，具有很好的長(zhǎng)期穩(wěn)定性，但是受載體機(jī)動(dòng)加速度影響嚴(yán)重；磁強(qiáng)計(jì)則能測(cè)得地磁場(chǎng)在載體坐標(biāo)系的投影。采用CKF對(duì)上述數(shù)據(jù)進(jìn)行融合，可以獲得靜態(tài)漂移和動(dòng)態(tài)特性均較好的姿態(tài)估計(jì)[18-19]。

圖1 多傳感器測(cè)量結(jié)構(gòu)Fig.1 Multi-sensor measurement structure

1.2 狀態(tài)方程

常見的姿態(tài)運(yùn)動(dòng)學(xué)模型主要有歐拉角、四元數(shù)以及羅德里格斯參數(shù)等。其中歐拉角法具有簡(jiǎn)便直觀、物理含義明確的優(yōu)點(diǎn)，且不存在冗余參數(shù)，雖然在俯仰角為90°時(shí)存在奇異，但對(duì)MAVs而言，俯仰角基本不會(huì)到達(dá)90°，因此本文采用歐拉角法對(duì)姿態(tài)運(yùn)動(dòng)學(xué)進(jìn)行描述來建立狀態(tài)方程。取狀態(tài)向量為

(1)

式中：φ，θ，γ分別為偏航角、俯仰角、滾動(dòng)角。

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

(2)

考慮常值誤差和測(cè)量噪聲，式(2)變?yōu)?/p>

(3)

式中：v1，v2，v3為陀螺輸出數(shù)據(jù)中的測(cè)量噪聲。

又

(4)

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

(5)

1.3 量測(cè)方程

基于雙向量法建立量測(cè)方程[20]。首先，通過重力向量求解滾轉(zhuǎn)角φ 和俯仰角θ 。通常認(rèn)為在導(dǎo)航系下，重力向量是不變的，可以表示為gn=(0，0，g)T，其中g(shù)為當(dāng)?shù)刂亓铀俣?。記載體坐標(biāo)系下的重力向量為gb=(gbx，gby，gbz)T，則有

(7)

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

(8)

然后，基于磁強(qiáng)計(jì)的輸出得到偏航角ψk為

(9)

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

綜合式(8)，(9)根據(jù)加速度計(jì)與磁強(qiáng)計(jì)的輸出，可得量測(cè)方程為

(10)

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

2 容積卡爾曼濾波方法

假設(shè)一個(gè)非線性系統(tǒng)：

(11)

式中：xk∈Rn為k 時(shí)刻系統(tǒng)的狀態(tài)向量；zk∈Rm為k時(shí)刻系統(tǒng)的觀測(cè)向量；wk為均值為0、協(xié)方差為Qk的n 維隨機(jī)過程噪聲；vk為均值為0、協(xié)方差為Rk的m 維隨機(jī)量測(cè)噪聲，且wk，vk互不相關(guān)。CKF通過三階容積積分原理，計(jì)算函數(shù)的標(biāo)準(zhǔn)加權(quán)高斯積分[15]。標(biāo)準(zhǔn)的CKF步驟如下：

(1) 初始化

(12)

(2) 時(shí)間更新

1) 設(shè)k-1時(shí)刻協(xié)方差矩陣Pk-1|k-1正定，對(duì)其進(jìn)行因式分解得到Sk-1|k-1，即

(13)

2) 容積點(diǎn)估計(jì)

(14)

3) 容積點(diǎn)傳播

(15)

4) 求解狀態(tài)一步預(yù)測(cè)值

(16)

5) 計(jì)算預(yù)測(cè)誤差協(xié)方差矩陣

(17)

(3) 量測(cè)更新

1) 預(yù)測(cè)誤差協(xié)方差矩陣分解

(18)

2) 容積點(diǎn)估計(jì)

(19)

3) 容積點(diǎn)傳播

(20)

4) 計(jì)算量測(cè)預(yù)測(cè)值

(21)

(22)

(23)

(4) 狀態(tài)更新

1) 求解Kalman增益

(24)

(25)

3 仿真校驗(yàn)

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

從估計(jì)精度、濾波穩(wěn)定性以及運(yùn)算耗時(shí)3個(gè)方面對(duì)仿真結(jié)果進(jìn)行分析。

圖2，3和表1，2反映了不同濾波方法的估計(jì)精度?？梢钥闯?，CKF的估計(jì)精度與UKF估計(jì)精度基本一致，均優(yōu)于EKF，以估計(jì)誤差MonteCarlo仿真絕對(duì)均值進(jìn)行比較，在滾轉(zhuǎn)角、俯仰角和偏航角3個(gè)維度上相比于EKF分別提高約15.8%，16.8%，19.8%，以估計(jì)誤差均方差MonteCarlo仿真均值比較，則分別提高29.7%，28.7%，30.7%。

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

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

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

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

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

圖4 100次Monte Carlo仿真NES指標(biāo)對(duì)比Fig.4 Comparison of 100 Monte Carlo simulations with NES index

算法EKFUKFCKFNES均值(MNES)12.10654.37794.3818

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

NES 指標(biāo)反映濾波的穩(wěn)定性和一致性，當(dāng) NES 指標(biāo)的數(shù)值越接近估計(jì)狀態(tài)量的維數(shù)(本文中狀態(tài)量維數(shù)為 3)，說明該濾波算法的穩(wěn)定性和一致性越好。仿真結(jié)果顯示，CKF與UKF的穩(wěn)定性相一致，均明顯優(yōu)于EKF。

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

4 結(jié)論

(1) 估計(jì)精度方面，CKF與UKF相當(dāng)，均優(yōu)于EKF，以估計(jì)誤差為指標(biāo)進(jìn)行比較，CKF在滾轉(zhuǎn)角、俯仰角和偏航角3個(gè)維度上相比EKF分別提高了15.8%，16.8%，19.8%，以誤差均方差做指標(biāo)進(jìn)行比較，則分別提高了29.7%，28.7%，30.7%；

(2) 濾波穩(wěn)定性與一致性方面，以NES為指標(biāo)進(jìn)行比較，CKF與UKF相當(dāng)，均顯著優(yōu)于EKF；

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

[1] 周婧，高印寒，劉長(zhǎng)英，等.基于自適應(yīng)算法的單目視覺系統(tǒng)的姿態(tài)解算[J].光學(xué)精密工程，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ī)姿態(tài)估計(jì)[J].光學(xué)精密工程，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ī)微型姿態(tài)航向系統(tǒng)數(shù)據(jù)處理[J].中南大學(xué)學(xué)報(bào)：自然科學(xué)版，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] 張欣，白越，趙常均，等.多旋翼姿態(tài)解算中的改進(jìn)自適應(yīng)擴(kuò)展Kalman算法[J].光學(xué)精密工程，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] 宋宇，翁新武，郭昕剛.基于四元數(shù)EKF算法的小型無人機(jī)姿態(tài)估計(jì)[J].吉林大學(xué)學(xué)報(bào):理學(xué)版，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ìn)容積卡爾曼濾波的奇異避免姿態(tài)估計(jì)[J].航空學(xué)報(bào)，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，F(xiàn)OSS 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] 賈瑞才.重力/地磁輔助的歐拉角無跡卡爾曼濾波姿態(tài)估計(jì)[J].光學(xué)精密工程，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].系統(tǒng)工程與電子技術(shù)，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] 胡振濤，曹志偉，李松，等.基于容積卡爾曼濾波的異質(zhì)多傳感器融合算法[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ìn)的迭代容積卡爾曼濾波姿態(tài)估計(jì)[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào)，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] 康軼非，宋永端，宋宇，等.平方根容積卡爾曼濾波在移動(dòng)機(jī)器人SLAM中的應(yīng)用[J].機(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] 王松，田波，戰(zhàn)榆莉，等.基于修正EKF的微小型飛行器姿態(tài)估計(jì)[J].高技術(shù)通訊，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] 賈瑞才.基于四元數(shù)EKF的低成本MEMS姿態(tài)估計(jì)算法[J].傳感技術(shù)學(xué)報(bào)，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，F(xiàn)U 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-)，男，湖北黃石人。教授，博士，主要研究方向?yàn)樾畔⑷诤希繕?biāo)定位與跟蹤。

通信地址：430033 湖北省武漢市解放大道717號(hào)147號(hào)信箱 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