基于自適用擴展卡爾曼濾波算法的無人機姿態估計

柏 植，許海峰，郭 凱，尹 珠,2

(1.宿州學院 機械與電子工程學院，安徽 宿州 234000；2.中國科學技術大學，安徽 合肥 230000)

近年來，無人機的應用在災區救援、情報收集、視頻拍攝和農業治理等方面的應用越來越廣泛[1]。實現無人機安全、穩定的飛行的前提就是高精度的導航系統和姿態解算系統，微機電系統(MEMS)的出現使得無人機可以完成姿態解算，但是MEMS慣性器件具有以下問題：(1)陀螺儀測量值存在漂移，計算時陀螺儀測量誤差值會不斷累積，誤差會越來越大；(2)加速度計區分不了內力和外力加速度，飛行器抖動時候會出現較大的誤差，動態性能較差；(3)磁力計易受環境中磁場的干擾，因此其誤差無法預測且難以消除，所以要想獲取準確的姿態信息就需要使用濾波融合算法處理MEMS測量值來獲取盡可能準確的姿態信息[2]。

描述無人機的姿態角的方法常用的有：歐拉角法、四元數法、等效旋轉矩陣法和方向余弦算法等[3]。其中歐拉角法描述姿態信息簡單，沒有太多的參數，但是當角度接近90°時，會有奇異值，不能實現全姿態解算，且計算量較大；方向余弦法可以實現全姿態解算，但是其參數太多，計算量大且復雜；等效旋轉矩陣法可以消除計算誤差，但是參數過多，計算復雜；四元數法只需要求解四個方程，計算量小，該法具有簡單、易于實現的特點。綜上所述，本文采用四元數法來描述姿態角信息[4]。

本文針對傳統的EKF算法融合MEMS傳感器的測量值時，解算到姿態角穩定性低和易受環境影響的缺點，提出基于自適應EKF算法的姿態估計，用四元數描述姿態角，并將四元數作為EKF的狀態量，將加速計測量值和磁力計解算出的偏航角作為EKF的觀測量，并設計了基于新息的自適應調整器，自適應的在線修正過程噪聲矩陣Q和觀測噪聲矩陣R，從而提高了姿態解算的魯棒性。

1 無人機姿態運動模型

1.1 坐標系之間的轉換

為了更好的描述姿態角，本文采用歐拉角描述姿態角，使用四元數進行姿態角的轉換，本文建立了兩個坐標系：機體坐標系(b系)和導航坐標系(n系)[5]。本文定義的機體坐標系0-XbYbZb，該坐標系和機體固定在一起，設無人機的質心為坐標系原點，Xb軸方向指向其右翼方向，Yb軸方向指向其機頭方向，Zb軸方向為垂直無人機機體向上。定義的導航坐標系0-XnYnZn，該坐標系同樣設無人機的質心為坐標原點，Xn軸、Yn軸和Zn軸的方向分別指向東-北-天三個方向。b系可以看成是n系分別旋轉φ(yaw)、θ(pitch)、ψ(roll)三個角度得到[6]。三次旋轉的姿態旋轉矩陣如式(1)所示。

(1)

1.2 四元數描述姿態角

本文定義四元數的表達形式為：

q=[q0q1q2q3]T=[q0e]T

(2)

式中：q0為標量部分，e為矢量部分。使用四元數表示的姿態旋轉矩陣如式(3)所示。

(3)

(4)

式中：wx、wy、wz為陀螺儀測量的三軸角速度值，由于式(4)是連續的，對其使用畢卡逼近法離散化處理[7]，可得：

q(k+1)=ρ(k)q(k)

(5)

式(5)中：

(6)

得到更新后的四元數矩陣，根據四元數和歐拉角之間的轉換關系，便可以計算得到四元數表示的歐拉角，如式(7)、式(8)、式(9)所示。

θ=arcsin[2(q2q3+q1q0)]

(7)

(8)

(9)

在使用磁力計進行偏航角測量的時候，由于無人機的狀態不一定是水平的狀態，所以要借助加速度計解算出的姿態角對磁力計的測量值進行補償[8]，單單使用加速度計解算的俯仰角和橫滾角的估計值為：

(10)

(11)

式中：ax、ay、az為MEMS測量得到的三軸加速度。

所以測量得到磁場的水平分量的估計值表達式為：

(12)

式中：mx、my、mz為MEMS器件測量的三軸磁場強度。綜上可得偏航角的估計值為：

(13)

由于陀螺儀的靜態誤差比較大、加速度計的動態誤差比較大和磁力計易受環境影響的特點，所以就需要使用相應的濾波融合算法對多個MEMS傳感器的測量數據進行融合，本文就提出了AEKF算法對多個MEMS傳感器的測量數據進行處理，得到準確穩定的數據。

2 擴展卡爾曼濾波

卡爾曼濾波是應用比較廣泛的線性濾波器，而無人機姿態解算是非線性系統，所以對該非線性系統使用泰勒公式展開，對其一階線性截斷，并舍去其高階無窮小項，這就是EKF[9]。該算法具有簡單、計算量較小和參數較少的優點，所以本文使用EKF算法。

對系統進行建模，即系統的狀態方程和觀測方程為：

(14)

上式中，Wk和Vk分別為k時刻系統的過程噪聲和觀測噪聲，它們的方差分別是Qk和Rk，且都為零均值高斯白噪聲矩陣。

系統k+1時刻狀態量估計值可以通過得到k時刻狀態量通過式(15)算得。即

(15)

(16)

對上式中h(x(k))中的四元數四個狀態量分別求偏導，可得Jacobi矩陣H(k)為：

(17)

在本文的模型中，系統的狀態量為X(k)=[q0(k)q1(k)q2(k)q3(k)Δgx(k)Δgy(k)Δgz(k)]T，其中q0(k)，q1(k)，q2(k)，q3(k)是k時刻的四元數矩陣，Δgx(k)、Δgy(k)、Δgz(k)為陀螺儀k時刻的測量誤差。本文EKF算法包含狀態預測、協方差預測、增益矩陣計算、狀態更新和協方差更新五個步驟，對應公式如(18)-式(22)所示。

(18)

(19)

(20)

(21)

(22)

綜合上述的算法便可以得到解算后的四元數矩陣，然后使用式(7)、式(8)、式(9)便可以得到無人機此時的姿態角。

在使用EKF算法時，狀態噪聲的協方差矩陣和觀測噪聲的協方差矩陣都很難獲得準確的數值并且自始至終都是常數，但是在實際的工程當中，狀態噪聲和觀測噪聲不會是恒定不變的，所以EKF的數據融合效果就會受到較大的影響，為了優化濾波器的數據融合效果，提出了基于新息的自適用算法，自適應的在線修正觀測噪聲矩陣，可以有效降低由于傳感器高斯白噪聲帶來的誤差，使得濾波器收斂的更加迅速，最終的濾波值更加準確。

3 擴展卡爾曼濾波的優化

由于EKF算法在實際運行過程中，狀態噪聲矩陣和觀測噪聲矩陣是固定不變的，這樣濾波器就很難一直保持在最佳狀態。因此，為了優化濾波效果，本文使用基于新息的自適應算法實時修正濾波器的噪聲矩陣，增加濾波器的魯棒性[10]。

令式(21)中Δ(k+1)=Z(k+1)-h(x(k+1))，該值被稱為殘差或者新息，它表示在k+1時刻測量值和估計值之間的差值，為了讓系統的觀測噪聲和狀態噪聲可以自適應的動態調整，所以自適應調整噪聲矩陣的算法為[5]：

(23)

(24)

Q(k+1)=K(k+1)C(k+1)KT(k+1)

(25)

上式中，M為所選取的平滑窗口，其值的選取根據經驗選取，根據多次仿真經驗，M取3時，濾波效果最佳，C(k+1)為Δ(k+1)的協方差矩陣，綜上，基于AEKF算法的流程如圖1所示，下面作簡要描述。

圖1 系統結構圖

4 仿真與分析

為了驗證本文提出的AEKF算法相對傳統的EKF的優越性，本文采用高性能處理器STM32、三軸陀螺儀MPU6050、三軸加速度計MPU6050和三軸磁力計HMC5883L來采集原始數據，采樣的時間為10秒，采樣頻率為100Hz，單片機將采樣到的原始數據傳送到筆記本，在PC端的MATLAB平臺分別使用EKF算法和AEKF算法對采集的原始數據進行處理，最后使用MATLAB將處理結果描繪出來，并對兩種算法的仿真結果進行分析與比較。

仿真時，將傳感器采集平臺放在桌面上保持固定的角度并靜止，采集原始數據傳送給PC機MATLAB后分別使用兩種算法進行仿真，仿真結果圖如下所示。圖2為常規EKF算法的四元數仿真圖，圖3為常規EKF算法的角度解算圖，圖4為AEKF的四元數仿真圖，圖5為AEKF算法的角度結算圖。

圖2 EKF算法四元數仿真圖

圖3 EKF算法解算的角度仿真圖

圖4 AEKF算法四元數仿真圖

圖5 AEKF算法解算的角度仿真圖

分析圖2和圖4可以發現，兩種算法的四元數收斂都非常迅速，且比較穩定，沒有出現較大的波動，說明了本文提出的算法的有效性。分析圖3和圖5可以發現，傳統的EKF算法在pitch、roll、yaw三種姿態角解算中的收斂時間分別為0.23s、0.25s、0.3s，收斂的角度更集中在-4.2°～-2.9°、12°～16°、68.5°～70.5°之間，而AEKF算法在pitch、roll、yaw三種姿態角解算中的收斂時間分別為0.13s、0.17s、0.09s，收斂的角度更集中在-3.8°～-2.8°、13°～15°、69°～70°之間，通過對比可以發現，本文提出的算法可以讓姿態角在更短時間內完成收斂，并且在收斂的過程中波動性更小，穩定性較好，具有較高的魯棒性。EKF算法和AEKF算法姿態角均值和均方差對比結果如表1所示，從表1中也可以發現，AEKF算法相比EKF算法，姿態角的均方差更小，穩定性更好，靜態性能更優。

表1 EKF算法和AEKF算法姿態角均值和均方差對比

5 結論

針對傳統EKF算法融合多個MEMS傳感器數據結果不穩定易發散的問題，提出了基于AEKF的四元數姿態估計算法。首先介紹了如何使用四元數建立飛行器的姿態運動模型，然后以四元數為狀態量，以三軸加速度和偏航角測量值為觀測量，使用EKF算法對其進行融合，最后，為了使EKF工作在最佳濾波狀態，使用自適應算法在線修正狀態噪聲和觀測噪聲。通過仿真平臺對兩種算法進行仿真運算并繪圖對比，仿真結果表明：AEKF算法可以使姿態角收斂的更快，并且解算的姿態角更穩定，魯棒性更優，有效的降低了由于MEMS噪聲對姿態解算的影響。