袁 亮，楚仕彬

（新疆大學(xué)機(jī)械工程學(xué)院，烏魯木齊 830047）

基于卡爾曼濾波的無人機(jī)姿態(tài)測量研究*

袁 亮，楚仕彬

（新疆大學(xué)機(jī)械工程學(xué)院，烏魯木齊 830047）

文章針對無人機(jī)姿態(tài)測量中受制于計算機(jī)存儲和處理每個參數(shù)字節(jié)的限制，使得計算精度也同樣受到一定限制，舍入誤差不可避免的存在于系統(tǒng)之中，對于卡爾曼濾波狀態(tài)估計的誤差可以由測量更新過程來修正，然而對誤差協(xié)方差矩陣P卻沒有相應(yīng)的修正，隨著迭代次數(shù)不斷增加，將導(dǎo)致P矩陣失真，打破其對角對稱性。為提高無人機(jī)姿態(tài)測量的準(zhǔn)確性，由此設(shè)計由陀螺儀、加速度計和磁強(qiáng)計組成的姿態(tài)測量系統(tǒng)，采用UD分解擴(kuò)展卡爾曼濾波方法，實(shí)現(xiàn)對四元數(shù)的估計，并通過實(shí)驗(yàn)進(jìn)行分析驗(yàn)證，實(shí)驗(yàn)結(jié)果表明該UD分解擴(kuò)展卡爾曼濾波方法能夠在低動態(tài)環(huán)境下準(zhǔn)確解算出姿態(tài)角度，并能有效克服舍入誤差的影響。

卡爾曼濾波；無人機(jī)；姿態(tài)測量

0 引言

在實(shí)際應(yīng)用卡爾曼濾波的時候，我們需要存儲大量的參數(shù)，并且需要進(jìn)行大量的矩陣運(yùn)算，在小型四旋翼無人機(jī)的控制中，主控制器的規(guī)格一般較低，并不能實(shí)現(xiàn)高精度的計算，只能采用精度較低（單精度32位）的算法來執(zhí)行卡爾曼濾波。這使得舍入誤差不可避免的存在于系統(tǒng)之中，隨著迭代次數(shù)的增加，誤差協(xié)方差矩陣P會逐漸失真，從而導(dǎo)致卡爾曼增益矩陣K失真。小的增益會使?fàn)顟B(tài)估計反應(yīng)遲緩，大的增益會產(chǎn)生不穩(wěn)定、動蕩的狀態(tài)估計。極端的協(xié)方差矩陣失真也能夠?qū)е萝浖到y(tǒng)的崩潰。因此在實(shí)際應(yīng)用卡爾曼濾波的過程中，要盡可能的減小誤差協(xié)方差矩陣計算的誤差，特別是P必須保持正定。

為了消除舍入誤差對濾波結(jié)果的影響，提高濾波精度，在實(shí)際應(yīng)用中可以使用高精度算法（64位）來執(zhí)行卡爾曼濾波，文獻(xiàn)［1］中提出采用約瑟夫式的協(xié)方差更新，可以有效的提高計算精度，文獻(xiàn)［15］提出采用平方根式的卡爾曼濾波，可以有效的提高計算精度，并且能夠保持誤差協(xié)方差的對稱性和正定性。

1 系統(tǒng)組成及姿態(tài)解算原理

本文所采用的姿態(tài)測量系統(tǒng)主要由三軸加速度計、三軸陀螺儀和三軸磁強(qiáng)計組成。主控制器讀取姿態(tài)測量傳感器的數(shù)據(jù)，通過卡爾曼濾波對傳感器測量數(shù)據(jù)進(jìn)行融合處理，得到當(dāng)前最優(yōu)四元數(shù)，從而計算出無人機(jī)的姿態(tài)角度。

2 采用UD分解算法

受到處理器有效字長的限制，卡爾曼濾波在實(shí)際濾波過程中存在舍入誤差積累，隨著卡爾曼濾波運(yùn)行次數(shù)不斷增加，矩陣的失真就越大，從而出現(xiàn)誤差協(xié)方差矩陣P不能保持正定性，濾波出現(xiàn)不穩(wěn)定的情況［2］。

對P（n×n維）矩陣進(jìn)行UD分解，即P=UDUT，其中U（n×n維）為單位上三角矩陣，且對角線元素為1，D（n×n維）為對角陣，P為對陣正定矩陣。并且產(chǎn)生唯一UD分解步驟如下：

（1）對于第n列，有

（2）對于其它列，j=n-1，n-1，...，1，有

3 UD分解的擴(kuò)展卡爾曼濾波算法

本卡爾曼濾波將四元數(shù)作為狀態(tài)估計量，并且四元數(shù)更新采用三階畢卡算法，與此同時將加速度計和磁強(qiáng)計的測量值作為觀測量。濾波原理如圖1所示。

圖1 卡爾曼濾波原理

3.1 卡爾曼濾波狀態(tài)方程

目前求解四元數(shù)微分方程主要有兩種方法：一種是四階龍格庫塔法，另一種就是本文所使用的三階畢卡逼近法。其預(yù)測方程為：

表1 畢卡各階表達(dá)式

3.2 卡爾曼濾波觀測方程

將加速度計和磁強(qiáng)計傳感器測量的值作為觀測量Z（t）。

3.3 采用UD分解算法的擴(kuò)展卡爾曼濾波流程

根據(jù)卡爾曼濾波的原理，基于UD分解的擴(kuò)展卡爾曼濾波流程如下：

UD分解算法的實(shí)現(xiàn)步驟。

上述過程即為采用UD分解算法的卡爾曼濾波的實(shí)現(xiàn)步驟，濾波器每次執(zhí)行都按此過程循環(huán)。

（6）經(jīng)過卡爾曼濾波之后，就能得到最優(yōu)估計四元數(shù)，再通過式（3）便可得到姿態(tài)角度。

4 實(shí)驗(yàn)

4.1 實(shí)驗(yàn)準(zhǔn)備及參數(shù)設(shè)定

為驗(yàn)證本文的算法，使用“第七實(shí)驗(yàn)室”的開發(fā)板captain，該開發(fā)板集成STM32405RG（主控制芯片）、MPU6050（三軸陀螺儀和三軸加速度計）、HMC5883（磁強(qiáng)計），同時該開發(fā)板具有豐富的通訊接口，能夠?qū)⒆藨B(tài)數(shù)據(jù)通過UART上傳至上位機(jī)進(jìn)行分析。構(gòu)建驗(yàn)證系統(tǒng)如表2所示。

表2 系統(tǒng)試驗(yàn)參數(shù)

表3 卡爾曼濾波參數(shù)初始化

4.2 軟件實(shí)現(xiàn)

本軟件開發(fā)平臺為Keil uVision4，姿態(tài)測量過程分為系統(tǒng)初始化、姿態(tài)初始化、姿態(tài)更新三個過程，如圖2所示。

圖2 姿態(tài)測量過程

根據(jù)參考坐標(biāo)系定義，判斷航向的象限如表4所示。

表4 航向象限的判斷

4.3 實(shí)驗(yàn)結(jié)果

本實(shí)驗(yàn)采用一個能夠?qū)崿F(xiàn)勻速轉(zhuǎn)動和加速轉(zhuǎn)動的轉(zhuǎn)臺，動態(tài)測量姿態(tài)和傳感器的數(shù)據(jù)，本實(shí)驗(yàn)數(shù)據(jù)的采集頻率為100Hz，采集次數(shù)600次以上，為保證無人機(jī)飛行過程中能夠穩(wěn)定、可靠的飛行，姿態(tài)變化不超過±60°，本實(shí)驗(yàn)均是在此姿態(tài)范圍內(nèi)進(jìn)行實(shí)驗(yàn)。圖3和圖4為靜止時刻俯仰測量對比圖，從圖中我們可以看出，采用UD分解算法的擴(kuò)展卡爾曼濾波測量值更為穩(wěn)定和精確，能夠有效的抑制舍入誤差對系統(tǒng)穩(wěn)定性的影響；圖5為在勻速轉(zhuǎn)動的情況下的對比圖，從圖中可以看出，在步進(jìn)電機(jī)轉(zhuǎn)動勻速改變姿態(tài)的過程中，采用UD分解算法的擴(kuò)展卡爾曼濾波測量值更為穩(wěn)定，在局部抖動的情況下，依然能夠消除抖動，準(zhǔn)確的測量姿態(tài)；圖6為加速運(yùn)動情況下的對比圖，從圖中可以看出，在加速運(yùn)動的過程中，采用UD分解算法的擴(kuò)展卡爾曼濾波能夠較為準(zhǔn)確的跟蹤姿態(tài)，不會因加速運(yùn)動出現(xiàn)劇烈的角度變化，未采用UD分解的算法雖然也能大體跟蹤姿態(tài)，但是其抖動較為明顯，不適宜在無人機(jī)上使用。

圖3 靜止時刻俯仰測量對比圖

圖4 靜止時刻局部放大圖

圖5 勻速運(yùn)動情況下的測量值

圖6 加速運(yùn)動情況下的測量值

5 結(jié)論

本文研究基于UD分解算法的擴(kuò)展卡爾曼濾波在四旋翼無人機(jī)姿態(tài)測量中的應(yīng)用，通過實(shí)驗(yàn)，發(fā)現(xiàn)采用UD分解算法的擴(kuò)展卡爾曼濾波能夠更為穩(wěn)定的測量姿態(tài)，能夠有效抑制舍入誤差帶來的影響，防止P矩陣發(fā)散，提高姿態(tài)測量精度。

［1］李濤，練軍想，曹聚亮，等.GNSS與慣性及多傳感器組合導(dǎo)航系統(tǒng)原理［M］.北京：國防工業(yè)出版社，2011.

［2］馮鵬，鄒世開.卡爾曼濾波器UD分解濾波算法的改進(jìn)［J］.遙測遙控.2005，26（1）：10-14.［3］張秋陽.無人機(jī)姿態(tài)測算及其誤差補(bǔ)償研究［D］.長沙：中南大學(xué)，2011.

［4］陳祖貴.UDU分解濾波算法及其應(yīng)用［J］.中國空間科學(xué)技術(shù)，1989（6）：39-48.

［5］隋樹林，袁健，張文霞，等.基于UD-EKF自主光學(xué)導(dǎo)航方法仿真［J］.系統(tǒng)仿真學(xué)報.2007，19（3）：482-485.

［6］A M Sabatini.Quaternion-based extended kal-man filter for determining orientation by inertial andmagnetic sensing［J］. 2006，53（7）：1346-1356.

［7］李建文，郝順義，黃國榮.基于UD分解的改進(jìn)型強(qiáng)跟蹤濾波器 ［J］.系統(tǒng)工程與電子技術(shù).2009，32（8）：1953-1957.

［8］E R Bachmann，IDuman，et.al.Orientation Trac-king for Humans and Robots Using Inertial Sensors［R］.Robotics and Automation.1999：187-194.

［9］H J Luinge，P H Veltink.Measuring orien-tation of human body segments usingminia-ture gyroscopes and accelerometers［J］.Medical and Biological Engineering and Computing.2006，43（2）：273-282.

［10］王宏偉，馬廣富.基于正交最小二乘和UD分解的模糊建模方法［J］.控制與決策，2003，18（6）：758-763.

［11］XUE Liang，QIN Wei.et.al.Application of Quaternion Based Extended Kalman Filter for MAV Attitude Estimation Using MEMS Sensors［J］.Nanotechnology and Preci-sion Engineering.7（2），2009.163-167.

［12］G.J.Bierman.Measurement updating using the U-D factorization［J］.Automatic.1976，12：375-382.

［13］數(shù)學(xué)手冊編寫組.數(shù)學(xué)手冊［M］.1版.北京：高等教育出版社，1979.

［14］王學(xué)斌，徐建宏，張章.卡爾曼濾波參數(shù)分析與應(yīng)用方法研究［J］.計算機(jī)應(yīng)用與軟件，2012，29（6）：212-215.

［15］Arasaratnam，Haykin.Square-Root Quadra-ture Kalman Filtering［J］.signal processing IEEE.2008，56（6）：5859-2593.

（編輯 李秀敏）

Research on UAV Attitude Calculation Based on Kalman Filter

YUAN Liang，CHU Shi-bin
（Mechanical Engineering Academy，Xinjiang University，Urumqi830000，China）

Subject to the limitations of computer storage and processing parameters of each byte，which makes the precision of calculation is also subject to certain restrictions，rounding errors inevitably exist in the system，the errors of Kalman filter state estimation can be corrected by the update process ofmeasured，but the error covariancematrix P can notbe corrected，along w ith the iteration number increased，the covariance matrix P w ill become distortion.It is diagonal symmetry w ill broken.In order to improve the attitudemeasurement accuracy of UAV，thus design the attitudemeasurement system composed of gyroscope accelerometer andmagnetometer，using themethod of UD decomposition EKF to data fusion to estimate Quaternion and verified by the experiment.The experimental results show that the UD decomposition EKFmethod can accurately calculate the attitude angle in low dynamic environment，and can effectively overcome the influence of rounding errors.

kalman filter；unmanned aerial vehicle；attitude estimate

TH165；TG659

A

1001-2265（2015）07-0110-04 DOI：10.13462/j.cnki.mmtamt.2015.07.030

2014-09-26；

2014-10-27

國家自然科學(xué)基金（61262059，31460248）；新疆優(yōu)秀青年科技創(chuàng)新人才培養(yǎng)項(xiàng)目（2013721016）；新疆自治區(qū)科技支疆項(xiàng)目（201591102）

袁亮（1972-），男，烏魯木齊人，新疆大學(xué)副教授，博士，研究方向?yàn)闄C(jī)器人控制技術(shù)，計算機(jī)圖像處理，（E-mail）ylhap@163.com。