基于卡爾曼濾波的小型無人機(jī)姿態(tài)估計算法研究

聶 鵬，李佩華，李正強(qiáng)，鄭 旺，宋 平

(沈陽航空航天大學(xué) 機(jī)電工程學(xué)院，沈陽 110136)

姿態(tài)測量系統(tǒng)是無人機(jī)導(dǎo)航及姿態(tài)控制系統(tǒng)的重要組成部分，其精度直接影響到無人機(jī)各控制系統(tǒng)的精度。由于小型無人機(jī)帶負(fù)載的能力有限，要求姿態(tài)測量系統(tǒng)的體積質(zhì)量小、功耗低。隨著MEMS(Micro-Electro-Mechanical Systems)技術(shù)和計算機(jī)技術(shù)的發(fā)展，小型無人機(jī)姿態(tài)的測量普遍采用捷聯(lián)慣性測量單元(Inertial Measurement Unit,IMU)，其主要由低成本MEMS陀螺儀、加速度計和磁強(qiáng)計組成[1-2]。由于陀螺儀存在漂移，在姿態(tài)估計過程中存在累計誤差，加速度計會受到無人機(jī)飛行過程中機(jī)體振動的影響，同時磁強(qiáng)計是一種磁阻傳感器，容易受到外部磁場的干擾。單獨(dú)使用無法滿足控制系統(tǒng)的精度要求。本文采用加速度計及磁強(qiáng)計輸出作為觀測量，修正陀螺姿態(tài)解算的誤差，利用擴(kuò)展卡爾曼濾波器，對不同類型傳感器的數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，得到較高精度的姿態(tài)信息。

1 坐標(biāo)系定義及姿態(tài)矩陣

求取姿態(tài)角之前要定義載體坐標(biāo)系和導(dǎo)航坐標(biāo)系。本文的導(dǎo)航坐標(biāo)系定義為東北天坐標(biāo)系，原點選在無人機(jī)重心；載體坐標(biāo)系原點選在無人機(jī)重心，xb沿機(jī)體橫軸指向右，yb沿機(jī)體縱軸指向前，zb沿機(jī)體豎軸指向上，滿足右手定則。θ，γ，Ψ分別是無人機(jī)的俯仰角，橫滾角和航向角，則有載體坐標(biāo)系到導(dǎo)航坐標(biāo)系轉(zhuǎn)換的姿態(tài)矩陣為

(1)

(2)

在得到隨時間變化的姿態(tài)四元數(shù)后，通過公式(3)、(4)、(5)即可得到3個姿態(tài)角:

(3)

(4)

(5)

式中Tij由式(2)中姿態(tài)矩陣與四元數(shù)關(guān)系得出。

2 卡爾曼濾波姿態(tài)算法的設(shè)計

(6)

(7)

求解式(6)的微分方程即是對四元數(shù)q進(jìn)行更新。此處采用四階龍格庫塔法直接求解四元數(shù)微分方程：

(8)

2.1 利用加速度計和磁強(qiáng)計計算測量四元數(shù)

MEMS陀螺儀動態(tài)響應(yīng)特性良好，但運(yùn)行時會發(fā)生溫度漂移；同時積分運(yùn)算會產(chǎn)生累積誤差。加速度計和磁強(qiáng)計傳感器測量姿態(tài)沒有累積誤差，但動態(tài)響應(yīng)較差。因此，它們特性互補(bǔ)，可以采用算法融合這三種傳感器的數(shù)據(jù)，利用無時間累積誤差的重力場和磁場來修正陀螺給出的姿態(tài)角，提高測量精度。在融合之前通過重力場和磁場計算出最優(yōu)姿態(tài)四元數(shù)，作為測量四元數(shù)。這個計算過程可以將其認(rèn)為是最小平方誤差準(zhǔn)則的基礎(chǔ)算法[5]。誤差函數(shù)Sk定義為:

(9)

(10)

將計算測量四元數(shù)看成最小化問題，即通過使得加速度計和磁強(qiáng)計測量的誤差函數(shù)Sk最小化來得到最優(yōu)化的旋轉(zhuǎn)四元數(shù)。梯度下降法作為一種基本的最優(yōu)化算法，有工作量少，存儲變量較少以及對初始點要求不高等優(yōu)點[6]。式(9)描述了在初始點為q0和一個可變步長μ情況下，基于梯度下降法計算一個n次迭代的姿態(tài)四元數(shù)。式(11)表示計算誤差函數(shù)，它由目標(biāo)函數(shù)和其雅可比矩陣來定義。

(11)

(12)

(13)

誤差函數(shù)Sk>1e-3將作為停止準(zhǔn)則，避免無限迭代。實驗表明，如果選擇較小的值Sk<1e-3，相當(dāng)于依靠傳感器測量精度；同時選擇較大的值Sk>1e-3，相當(dāng)于假設(shè)測量是有噪聲的。從IMU的測量噪聲的結(jié)果中，本文選擇Sk≤0.5。傳感器融合另外一個問題就是選擇初始四元數(shù)值，實驗中發(fā)現(xiàn)，一個任意的初始條件會產(chǎn)生冗余的姿態(tài)信號。因此在系統(tǒng)開始運(yùn)行后靜止幾秒鐘，利用重力場和磁場通過算法得到初始四元數(shù)值。

2.2 擴(kuò)展卡爾曼濾波

本文采用文獻(xiàn)[7-8]中Kalman濾波器?？紤]如下系統(tǒng)：

xk+1=Axk+Bwk

yk=Cxk+vk

(14)

將四元數(shù)的4個參數(shù)作為狀態(tài)變量:

X=[q0q1q2q3]。

由式(7)得:

(15)

式(13)為建立的Kalman濾波的狀態(tài)方程。wk=[δxδyδz]近似為系統(tǒng)噪聲，其協(xié)方差矩陣Qk為的非負(fù)定常值對角陣。接著建立觀測方程，量測失量和狀態(tài)變量相同。但是，量測四元數(shù)由方程(11)(12)得出。量測矩陣C=I4,其中I4是4×4單位矩陣。

設(shè)采樣周期為T，將狀態(tài)方程和觀測方程離散化，同時略去二階以上項得到:

X(k)=A(k,k-1)X(k-1)+B(k-1)W(k-1)

(16)

Y(k)=C(k)X(k)+V(k).

(17)

式中

A(k,k-1)=

V(k)為量測噪聲，其協(xié)方差陣R(k)為正定常數(shù)對角陣。通過上面的分析，根據(jù)離散型擴(kuò)展卡爾曼濾波的遞推方程，建立卡爾曼濾波的時間傳播方程。

狀態(tài)一步預(yù)測：

(18)

狀態(tài)估計：

(19)

濾波增益：

K(k)=P(k,k-1)HT(k)[H(k)P(k,k-1)HT(k)+Rk]-1

(20)

一步預(yù)測均方誤差：

P(k,k-1)=Φ(k,k-1)P(k-1)ΦT(k,k-1)+Γ(k-1)Q(k-1)ΓT(k-1)

(21)

估計均方誤差：

P(k)=[I-K(k)H(k)]P(k,k-1)

(22)

姿態(tài)估計的流程圖如圖1所示。

圖1 姿態(tài)估計框圖

3 濾波器測試結(jié)果

本文采用的慣性測量單元包括Invensense公司的MPU6050(三軸陀螺儀和三軸加速度)和霍尼韋爾公司的HMC5883L(三軸磁強(qiáng)計)和導(dǎo)航計算機(jī)。測量單元安裝在單軸轉(zhuǎn)臺上，計算機(jī)串口波特率為38 400 bps，數(shù)據(jù)長度為10 000點。本文進(jìn)行了多組實驗，通過轉(zhuǎn)臺勻速轉(zhuǎn)動，采集測量單元所測數(shù)據(jù)并進(jìn)入姿態(tài)解算程序，得出當(dāng)前姿態(tài)測量單元姿態(tài)角。在進(jìn)行轉(zhuǎn)臺實驗時，單軸轉(zhuǎn)臺保持水平，慣性測量單元的橫滾角和俯仰角保持為零，只能利用偏航角進(jìn)行比較，在這里選取轉(zhuǎn)臺在5°/s和80°/s速率下的數(shù)據(jù)來評價濾波器。

圖2的結(jié)果是使轉(zhuǎn)臺處于靜止?fàn)顟B(tài)，為采用本文所設(shè)計算法，只利用MEMS陀螺儀輸出的數(shù)據(jù)解算出的3個姿態(tài)角，可以看出姿態(tài)角是發(fā)散的。圖5、圖6中可見，采用本文濾波器解算姿態(tài)，航向角動態(tài)誤差小于3°，有效補(bǔ)償了陀螺漂移引起的姿態(tài)誤差。

圖2 陀螺儀姿態(tài)解算

圖3 5°/s時濾波后姿態(tài)角輸出

圖4 80°/s時濾波后姿態(tài)角輸出

圖5 5°/s時航向角誤差

圖6 80°/s時航向角誤差

4 結(jié)論

本文提出了一種基于擴(kuò)展卡爾曼濾波技術(shù)的姿態(tài)融合算法，利用加速度計和磁強(qiáng)計的數(shù)據(jù)有效的補(bǔ)償了陀螺儀漂移引起的姿態(tài)誤差，很好的抑制了MEMS陀螺的漂移，提高了MEMS慣性器件在姿態(tài)測量應(yīng)用中的測量精度。實驗結(jié)果表明：本文所設(shè)計的算法具有較好的穩(wěn)定性和姿態(tài)估計精度，為下一步的姿態(tài)測量系統(tǒng)的研發(fā)奠定了理論基礎(chǔ)。

參考文獻(xiàn)(References)：

[1] 吳杰,閆建國.基于修正的卡爾曼濾波的姿態(tài)估計算法研究[J].計算機(jī)仿真,2012,29(2):54-57.

[2] 劉興川,張盛,李麗哲，等.基于四元數(shù)的MARG傳感器姿態(tài)測量算法[J].清華大學(xué)學(xué)報(自然科學(xué)版),2012,52(5):627-631.

[3] 薛亮,姜澄宇,常洪龍，等.基于狀態(tài)約束的MIMU/磁強(qiáng)計組合姿態(tài)估計濾波算法[J].中國慣性技術(shù)學(xué)報,2009,17(3):338-343.

[4] Eugenio Denti,Roberto Galatolo,Francesco Schettini.An AHRS based on a kalman filter for the integration of inertial,magnetometric and GPS data[C].27th Internation Congress of the Aeronautical Sciences,2010.

[5] Cordova Alarcon J R,Rodriguez Cortes H,Vicente Vivas E.Extended kalman filter tuning in attitude estimation from inertial and magnetic field measurements[C].Electrical Engineering,Computing Science and Automatic Control,CCE,2009 6thInternational Conference on,2009.

[6] Sebastian O H.Madgwick,Andrew J L.Harrison,Ravi Vaidyanathan.Estimation of IMU and MARG orientation using a gradient descent algorithm[C].IEEE International Conference on Rehabilitation Robotics,2011.

[7] 黃旭,王常虹,伊國興，等.利用磁強(qiáng)計及微機(jī)械加速度計和陀螺的姿態(tài)估計擴(kuò)展卡爾曼濾波器[J].中國慣性技術(shù)學(xué)報,2005,13(2):27-34.

[8] 楊淑潔,曾慶雙,伊國興.低成本無人機(jī)姿態(tài)測量系統(tǒng)研究[J].傳感器與微系統(tǒng),2012,31(2):15-22.