成利梅，崔晉龍

(1.太原科技大學(xué)，山西 太原 030024；2.國網(wǎng)山西送變電工程有限公司,山西 太原 033000)

0 引言

四旋翼飛行器因其高度的靈活性和機動性得到廣泛研究和應(yīng)用[1,2]，而位置和姿態(tài)的準確測量是四旋翼實現(xiàn)自主飛行的重要基礎(chǔ)。

慣性導(dǎo)航系統(tǒng)(INS)通過慣性測量單元(IMU)的測量能以較高的頻率解算得到飛行器的位置和姿態(tài)，但IMU在長期測量中存在明顯的漂移誤差，測量精度較低[3]。GPS測量精度較高，沒有誤差積累和漂移現(xiàn)象，但其更新速率較慢[4]。因此，將慣性導(dǎo)航系統(tǒng)和GPS進行組合來獲取飛行器的位置和姿態(tài)可以實現(xiàn)優(yōu)勢互補。

卡爾曼濾波(EKF)[5，6]、無跡卡爾曼(UKF)濾波[7，8]和其他非線性優(yōu)化方法均在組合導(dǎo)航中具有良好性能。其中，擴展卡爾曼濾波是實踐中最實用的方法。然而在傳統(tǒng)的擴展卡爾曼濾波中選取四元數(shù)描述姿態(tài)，沒有解決四元數(shù)的歸一化問題[9]。近年來，直接采用旋轉(zhuǎn)矩陣表示姿態(tài)的方法避免了四元數(shù)表示姿態(tài)的過參數(shù)和歸一化問題。

采用間接法的卡爾曼濾波以誤差量作為狀態(tài)量，但IMU誤差量是由真實值和估計值直接求差獲得，并沒有考慮兩個向量是在不同的坐標系下表示的。針對IMU誤差坐標系的一致性問題，基于誤差一致性表示的幾何EKF(GEKF)[10，11]被提出，通過幾何變換將誤差表示在同一坐標系下。最近，常路賓研究了基于SE(3)的擴展卡爾曼濾波姿態(tài)估計算法[12]，該研究采用旋轉(zhuǎn)矩陣描述姿態(tài)，并將陀螺儀偏差描述在同一坐標系，但該研究僅考慮了姿態(tài)估計。本文將基于誤差一致性的位置估計加入狀態(tài)估計，實現(xiàn)了飛行器的位姿估計。

1 系統(tǒng)模型

對于四旋翼飛行器而言，位置和姿態(tài)信息為控制器提供反饋信息，位置和姿態(tài)的測量精度將影響控制器的控制效果。慣性導(dǎo)航系統(tǒng)通過慣性傳感器測量機體的加速度和角速度，然后通過解算獲得機體的位置、姿態(tài)信息。GPS通過測量可得到機體的位置信息。

1.1 慣性傳感器數(shù)學(xué)模型

在慣性導(dǎo)航系統(tǒng)中，常用加速度計測量機體的加速度，用陀螺儀測量機體的角速度，加速度計和陀螺儀的數(shù)學(xué)模型[13]滿足：

(1)

其中:am和ωm分別為加速度計和陀螺儀的測量輸出；ab和ωb分別為真實的加速度和角速度；wa和wg分別為加速度計和陀螺儀的零均值高斯白噪聲；εa和εg分別為加速度計和陀螺儀的常值漂移，其滿足如下方程:

(2)

其中:ζa和ζg分別為加速度計和陀螺儀的隨機游走，為零均值高斯白噪聲。

1.2 位置姿態(tài)解算

為了描述四旋翼的位置和姿態(tài)，需要先建立兩個坐標系——參考坐標系和載體坐標系。四旋翼的位置可由其質(zhì)心在參考坐標系下的向量描述，姿態(tài)可由參考坐標系和載體坐標系之間的旋轉(zhuǎn)矩陣描述。若旋轉(zhuǎn)矩陣R∈SO(3)表示載體坐標系到參考坐標系的旋轉(zhuǎn)矩陣，則其對應(yīng)的指數(shù)形式[14]為：

R=eφ×.

(3)

其中:φ=θα為李群R∈SO(3)所對應(yīng)的李代數(shù)，θ為旋轉(zhuǎn)角度，a為旋轉(zhuǎn)軸對應(yīng)的單位向量。

同時，旋轉(zhuǎn)矩陣的微分滿足：

(4)

其中：ωb為載體的旋轉(zhuǎn)角速度。

由于加速度計測量得到的是機體在載體坐標系下的加速度，因此需要將加速度通過旋轉(zhuǎn)矩陣轉(zhuǎn)換到參考坐標系，機體在參考坐標系下的位置和速度滿足:

(5)

1.3 GPS測量模型

GPS系統(tǒng)能夠輸出機體在參考坐標系下的位置，其測量模型滿足:

Z=p+V.

(6)

其中：Z為GPS的輸出；V為測量噪聲，為零均值高斯白噪聲。

由于慣性傳感器的輸出存在漂移，因此基于慣性傳感器解算獲得的機體導(dǎo)航信息誤差會隨著時間累積，不適合于長時間的定位。GPS輸出精度較高，但數(shù)據(jù)更新速率較慢。本設(shè)計通過卡爾曼濾波對兩個系統(tǒng)的輸出信息進行融合,以得到更為準確的位置姿態(tài)信息。

2 INS/GPS幾何擴展卡爾曼濾波算法

2.1 狀態(tài)變量與狀態(tài)方程

本系統(tǒng)對機體的位置、速度、姿態(tài)以及加速度計和陀螺儀的漂移進行估計，定義狀態(tài)量X=(pTvTRTεaTεgT)T。本文先對狀態(tài)量的誤差進行估計，用估計得出的誤差量進一步修正狀態(tài)量。

(7)

(8)

將式(8)表示成指數(shù)形式可得:

(9)

將式(9)展開，可得旋轉(zhuǎn)矩陣誤差的一階近似值為:

(10)

其中：I為單位矩陣。

基于式(10)定義旋轉(zhuǎn)矩陣誤差的向量形式如下:

(11)

當估計載體坐標系趨于載體坐標系時，dφ則趨于0。

(12)

結(jié)合式(7)、式(11)、式(12)定義如下狀態(tài)誤差矢量:

dX=(dpTdvTdφTdεaTdεgT)T.

(13)

將式(13)中的每一項求導(dǎo)可得:

(14)

結(jié)合式(4)和式(5)可得狀態(tài)量的估計值滿足:

(15)

將式(2)、式(4)、式(5)、式(15)代入式(14)可得:

(16)

(17)

(18)

(19)

(20)

將式(16)～式(20)整理可得基于誤差矢量的狀態(tài)方程為:

(21)

式(21)中:

(22)

(23)

(24)

采用前向歐拉法對狀態(tài)方程(21)進行離散可得離散化之后的狀態(tài)方程，k時刻的狀態(tài)方程為:

dXk+1=FdXk+Gwk.

(25)

式(25)中:

F=I+FcΔt.

(26)

G=GcΔt.

(27)

其中：Δt為采樣時間間隔。

2.2 量測方程

我們選取GPS輸出的位置作為系統(tǒng)的觀測量，結(jié)合式(6)可得系統(tǒng)的測量方程為:

Zk=HXk+Vk.

(28)

其中：Zk為k時刻的位置測量值;Xk為k時刻的狀態(tài)量；H=(I3×303×12);Vk為測量噪聲。

2.3 信息融合機制

綜上，式(25)和式(28)構(gòu)成了卡爾曼濾波器的狀態(tài)方程與量測方程。

Xk+1|k=

(29)

(30)

Kk+1=Pk+1|kHT[HPk+1|kHT+Sk+1]-1.

(31)

其中：Sk+1為測量噪聲Vk+1的協(xié)方差矩陣。

此時狀態(tài)誤差的更新為:

(32)

誤差協(xié)方差的更新為:

Pk+1=[I-Kk+1Hk+1]Pk+1|k.

(33)

結(jié)合式(13)、式(29)和式(32)可得:

(34)

在實際系統(tǒng)中，陀螺儀和加速度計的采樣速率為100 Hz，而GPS系統(tǒng)的采樣速率約為1 Hz，兩者速率不匹配。在本文中，當GPS測量更新時，融合兩者數(shù)據(jù)更新狀態(tài)量，其他時刻采用狀態(tài)的預(yù)測值作為系統(tǒng)的輸出。濾波算法的流程如下所示:

if IMU采樣到第k次數(shù)據(jù) then

end

if GPS測量更新 then

Kk+1=Pk+1|kHT[HPk+1|kHT+Sk+1]-1

Pk+1=[I-Kk+1Hk+1]Pk+1|k

else

Pk+1=Pk+1|k

end

3 仿真驗證

為了驗證所設(shè)計算法的有效性，我們在MATLAB環(huán)境下進行仿真驗證[15]。整體的仿真流程如圖1所示。軌跡發(fā)生器生成對應(yīng)位置和姿態(tài)軌跡的理想加速度和角速度數(shù)據(jù)，模擬慣性傳感器模塊按照式(1)對理想加速度和角速度加噪聲后輸出到所設(shè)計的濾波器，濾波器最后輸出狀態(tài)量的估計值。

圖1 仿真系統(tǒng)流程圖

陀螺儀和加速度計的性能參數(shù)如表1所示。

表1 陀螺儀與加速度計性能參數(shù)

由表1可知，系統(tǒng)的過程噪聲協(xié)方差矩陣Qc=diag(100 100 100 0.01 0.01 0.01 0.04 0.04 0.04 0.000 1 0.000 1 0.000 1)。假設(shè)測量噪聲協(xié)方差矩陣S=diag(0.0120.0120.012)。

仿真過程中，采樣周期Δt=0.01 s，仿真時間t=100 s。

圖2和圖3分別為系統(tǒng)在所設(shè)計算法作用下的位置估計誤差和速度估計誤差。由圖2和圖3可知，三個方向的位置以及速度估計誤差均穩(wěn)定在零的附近。圖4為姿態(tài)角估計誤差，由歐拉角(Φ,Θ,Ψ)誤差表示。從圖4中可得，歐拉角誤差在±5°以內(nèi)。綜上可得，所設(shè)計的算法可較好地實現(xiàn)四旋翼位置和姿態(tài)的估計。

圖2 位置估計誤差 圖3 速度估計誤差 圖4 姿態(tài)角估計誤差

4 結(jié)束語

本文對四旋翼飛行器的位姿估計算法進行了研究，采用幾何擴展卡爾曼濾波算法實現(xiàn)了慣性導(dǎo)航系統(tǒng)和GPS系統(tǒng)的信息融合。通過將旋轉(zhuǎn)矩陣誤差向量和經(jīng)一致性表示的慣性傳感器漂移誤差作為狀態(tài)誤差矢量，推導(dǎo)出了基于一致性表示的擴展卡爾曼濾波。仿真結(jié)果表明所提出的算法能較好地實現(xiàn)四旋翼飛行器的位姿估計。