基于幾何擴展卡爾曼濾波的四旋翼位姿估計算法*

成利梅，崔晉龍

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

0 引言

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

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

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

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

1 系統模型

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

1.1 慣性傳感器數學模型

在慣性導航系統中，常用加速度計測量機體的加速度，用陀螺儀測量機體的角速度，加速度計和陀螺儀的數學模型[13]滿足：

(1)

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

(2)

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

1.2 位置姿態解算

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

R=eφ×.

(3)

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

同時，旋轉矩陣的微分滿足：

(4)

其中：ωb為載體的旋轉角速度。

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

(5)

1.3 GPS測量模型

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

Z=p+V.

(6)

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

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

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

2.1 狀態變量與狀態方程

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

(7)

(8)

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

(9)

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

(10)

其中：I為單位矩陣。

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

(11)

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

(12)

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

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

(13)

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

(14)

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

(15)

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

(16)

(17)

(18)

(19)

(20)

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

(21)

式(21)中:

(22)

(23)

(24)

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

dXk+1=FdXk+Gwk.

(25)

式(25)中:

F=I+FcΔt.

(26)

G=GcΔt.

(27)

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

2.2 量測方程

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

Zk=HXk+Vk.

(28)

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

2.3 信息融合機制

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

Xk+1|k=

(29)

(30)

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

(31)

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

此時狀態誤差的更新為:

(32)

誤差協方差的更新為:

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

(33)

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

(34)

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

if IMU采樣到第k次數據 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 仿真驗證

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

圖1 仿真系統流程圖

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

表1 陀螺儀與加速度計性能參數

由表1可知，系統的過程噪聲協方差矩陣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)。假設測量噪聲協方差矩陣S=diag(0.0120.0120.012)。

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

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

圖2 位置估計誤差 圖3 速度估計誤差 圖4 姿態角估計誤差

4 結束語

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