基于反冪法和擴展卡爾曼濾波的姿態估計算法

張 雄，黃衛華，陳勁峰

(武漢科技大學 信息科學與工程學院，湖北 武漢 430081)

0 引 言

低成本的微機電系統(micro-electro-mechanical systems，MEMS)慣性傳感器廣泛用于無人機的姿態測量，準確的姿態估計是無人機實現各種復雜任務的重要前提[1]。然而，單一的MEMS傳感器難以獲取準確的姿態信息[2]，多傳感器的優勢互補，已成為獲得無人機高精度姿態信息的主要方式。

擴展卡爾曼濾波(extended Kalman filtering，EKF)是實現多傳感器數據融合的有效方法之一。但常規EKF的測量值維數過高，且測量值線性化的過程中會引入截斷誤差，從而導致濾波器算法穩定性下降和收斂速度慢[3]。為了降低測量值的維數，避免高階項截斷誤差，文獻[4]采用四元數估計算法(quaternion estimation，QUEST)、文獻[5]采用Davenport的四元數法將計算的四元數作為EKF的測量值；為了提高了算法的穩定性和收斂速度，文[6]提出一種基于最優四元數法構造Davenport矩陣K原理的改進算法，對EKF測量矩陣進行改造。但是，由于測量值都需要先計算Davenport矩陣K的準確特征值，然后根據特征值函數計算特征向量(四元數)，當特征值的準確性下降時，會導致測量值的精度降低[7]，最終引起EKF的精度降低。

綜上所述，本文設計了一種基于反冪法和EKF的姿態估計算法。基于反冪法(inverse power method，IPM)，在已知近似特征值時，直接計算出準確的特征向量[8]，有效降低EKF精度對特征值的準確性的依賴程度，并減小測量值的維數。首先通過觀測矢量和參考矢量構造Davenport矩陣K，然后根據Davenport矩陣K的近似特征值，采用反冪法計算矩陣K特征向量，并將其作為EKF的測量值，最后將本文所設計的算法用于自行搭建的無人機平臺上，實現了無人機的穩定飛行，滿足了系統的高精度及實時性要求。

1 基于反冪法和EKF的姿態估計算法的算法結構

隨著MEMS技術的發展，在姿態測量場合用慣性測量單元對姿態進行精確實時測量變得非常普遍。本文以加速度計、陀螺儀、磁力計組成姿態測量系統，對無人機進行實時姿態測量。基于反冪法和EKF的濾波系統結構如圖1所示。

圖1 基于反冪法和EKF的組合濾波系統結構

首先通過加速度計、磁力計的參考矢量和觀測矢量構造Davenport矩陣K。其次根據Davenport矩陣K的近似特征值，使用帶原點位移的反冪法計算Davenport矩陣K的特征向量(四元數)。反冪法的初始向量為上一次EKF的估計值，反冪法的終止條件為相鄰兩次迭代特征向量的距離到達規定的閾值或者到達最大迭代次數。最后由四元數的微分方程得到狀態量qω，將反冪法求解的姿態四元數qIPM作為EKF的測量值，根據EKF濾波算法更新估計四元數qest，并計算姿態角。

2 基于反冪法和EKF的姿態解算算法

2.1 基于反冪法的確定性算法

2.1.1 近似特征值求解

(1)

(2)

式中：x,y,z分別為坐標系3個軸上的分量。

當加速度計和磁力計旋轉后的參考矢量和載體矢量分別相等時，可得載體的最優姿態四元數。考慮到不同傳感器精度不一樣，為了降低精度較差傳感器的影響，本文定義損失函數f(q) 為

(3)

其中，ai為傳感器的權重，i=1,2，且

(4)

其中，σi為第i個傳感器的標準差，由傳感器3個軸標準差的平均值代替。由于標準差σi越大精度越差，ai使得精度越差的傳感器所占的權重越小。

(5)

其中，四元數q=[q0,q1,q2,q3]T，D1、D2、D3是關于四元數q的矩陣。

將式(5)代入式(3)中，令

(6)

則式(3)變為

(7)

D1THxT+D2THyT+D3THzT-q=0

(8)

其中

(9)

將式(8)中D1、D2、D3的四元數q提取出來，則Davenport矩陣K為

(K-I)q=0

(10)

其中，Davenport矩陣K是式(9)中Hx,Hy,Hz各分量的線性組合，其具體形式為

(11)

考慮到加速度計和磁力計輸出存在噪聲、溫漂等不確定性因素，導致系數矩陣K-I可能存在秩虧問題，從而使得式(10)無法求解。為了增強算法的魯棒性，在四元數q后面增加一個誤差因子ε1q，其中誤差因子系數ε1數量級為10-5，略大于單精度浮點數的精度。式(10)可以表示為

Kq=(1+ε1)q

(12)

其中，1+ε1是Davenport矩陣K的近似特征值，四元數q是特征值1+ε1對應的特征向量。

2.1.2 反冪法求解

求解特征向量的經典方法是首先使用牛頓法或解析法求出Davenport矩陣K特征多項式的準確特征值，然后再根據特征值函數求特征向量。上述方法沒有考慮特征值準確性對四元數精度的影響。為了降低四元數對特征值精度的敏感度，本文根據Davenport矩陣K的近似特征值，使用帶原點位移的反冪法，通過迭代序列直接求解Davenport矩陣K的特征向量q。 反冪法公式為

(13)

其中，α為近似特征值。

為了減少計算量，對系數矩陣K-αI進行列主元LU分解，將其分解為下三角矩陣L和上三角矩陣U

K-αI=LU

(14)

LU分解使得式(13)在迭代過程中每一步只需要求解兩個三角方程組即可求得四元數q(k+1)

(15)

其中，q(k)的極大值規格化是為了防止四元數溢出。

當采樣間隔較小時，當前反冪法計算的四元數q(k+1)與上一次EKF的估計值qest,t-1的夾角較小。由于冪法初始向量與所求特征向量的夾角較小時，算法收斂速度較快，即使在終止條件閾值較大時仍可以得到誤差較小的結果。故第一次運行時反冪法的初始向量設為q(0)=[1,0,0,0]T, 之后的初始向量設為上一次EKF的估計值qest,t-1。 上述設置既考慮了歷史信息，加快了收斂速度，也避免了初始向量選取的盲目性。

反冪法的終止條件為到達最大迭代次數N。為了避免固定的迭代次數使Davenport矩陣K隨機變化和初始向量的不同選擇導致計算精度波動較大，以及為了能夠度量特征向量的精度。反冪法的另一個終止條件設為前后兩次迭代所求特征向量之間的距離d到達閾值ε2[10]

(16)

其中，ε2<10-4。

為了滿足四元數的范數條件，迭代終止后四元數需要歸一化。

2.1.3 算法步驟

經過上述推導，本文獲得了加速度計和磁力計融合后的計算四元數q(k+1)，其計算簡單且對特征值精度不敏感。基于反冪法的確定性算法的步驟為：

步驟2 按式(9)計算Davenport矩陣K的基本元素Hx,Hy,Hz，按式(11)確定Davenport矩陣K；

步驟3 代入四元數誤差因子系數ε1，獲得反冪法近似特征值α；代入初始向量q(0)、終止條件中的閾值ε2、最大迭代次數N；

步驟4 對方程系數矩陣K-αI進行列主元LU分解，獲得下三角矩陣L和上三角矩陣U，置迭代次數k=0；

步驟5 解式(15)中的三角形方程組，獲得四元數q(k+1)；

步驟6 若相鄰兩次迭代四元數之間的距離d<ε2，則輸出歸一化的四元數q(k+1)，停算；否則，轉步驟7；

步驟7 若k小于最大迭代次數N，置k=k+1，轉步驟5；否則停算。

2.2 擴展卡爾曼濾波器設計

2.2.1 濾波觀測方程

選取反冪法根據近似特征值迭代出的四元數q(k+1)作為測量值qmeasure,t。 考慮到反冪法每次求解的四元數q(k+1)，其正負都是Davenport矩陣K的解。正負四元數都表示相同的姿態信息，即四元數具有雙值性。其作為測量值與EKF的預測值進行融合時，存在四元數的正負符號選取問題。只有測量值與預測值方向基本相同時，才可以得到準確的估計值。EKF的預測值通過陀螺儀的積分得到，積分具有連續性，一旦積分初值(上一次EKF估計值)給定，那么預測值將連續變化。預測值與上一次EKF估計值qest,t-1的方向基本相同，不存在正負號選取問題。故只需要保證測量值qmeasure,t與上一次EKF估計值qest,t-1方向基本相同，就可以得到準確的估計值。余弦相似性常被用來度量兩單位向量的接近程度。如果求解的四元數q(k+1)與上一次EKF的估計值qest,t-1內積大于0，那么兩者夾角小于90°，方向基本相同。反之，方向基本相反。綜上測量值qmeasure,t的正負符號選取算法為

(17)

測量值qmeasure,t的觀測噪聲vt主要來源于加速度計和磁力計的輸出，假定其服從高斯分布。在t時刻的觀測方程可以表示為

zt=qmeasure,t+vt

(18)

由于式(13)也可以看作是關于四元數qmeasure,t的更新公式

qmeasure,t=(K-αI)qmeasure,t-1

(19)

所以觀測噪聲的協方差矩陣Σνt可近似為

Σvt=JΣacc,magJT

(20)

其中，雅可比矩陣J為

(21)

其中，ax,ay,az分別為加速度計的三軸數據；mx,my,mz分別為磁力計的三軸數據； Σacc,mag為加速度計和磁力計噪聲構成的協方差矩陣

Σacc,mag=diag[Σacc,Σmag]

(22)

其中，Σacc,Σmag分別是加速度計和磁力計經過歸一化后的協方差矩陣。

2.2.2 濾波狀態方程

選取四元數作為狀態變量，根據四元數微分方程可知

(23)

其中，Ωb為

(24)

式中：ωx,ωy,ωz分別是陀螺儀的三軸輸出。設采樣時間為Ts。 為了計算方便，采用二階龍格庫塔法求解式(23)。假定陀螺儀噪聲t服從高斯分布。在t時刻的狀態方程為

(25)

其中，Δθ2為

Δθ2=(ω(x,t-1)Ts)2+(ω(y,t-1)Ts)2+(ω(z,t-1)Ts)2

(26)

(27)

其中，陀螺儀的方差Σgyro為

(28)

其中，σωx,σωy,σωz分別為各軸角速度的標準差； Ξt為式(25)中狀態量qt關于陀螺儀三軸輸出ωx,ωy,ωz的雅克比矩陣

(29)

根據以上建立的狀態方程(25)和觀測方程(18)，首先，利用EKF算法的遞推方程對狀態向量和協方差矩陣進行預測，并計算濾波增益。然后，根據測量值qmeasure,t更新得到估計四元數qest,t，并更新協方差矩陣，為下一次循環做準備。最后，對估計四元數qest,t進行歸一化處理，并計算姿態角。

3 實驗與結果分析

3.1 實驗測試平臺

本文搭建了四旋翼無人機平臺，用于驗證算法的實際運行效果。微處理器選用STM32F405；慣性傳感器為MPU6050，包含三軸加速度計和三軸陀螺儀；磁力計為IST8310，無人機系統實物如圖2所示。

圖2 無人機系統

采集無人機靜止時加速度計、磁力計、陀螺儀傳感器的數據。以矩陣的形式存放在MATLAB中,使用協方差函數Cov分別計算3個傳感器的協方差矩陣。協方差矩陣對角線上的元素即為傳感器3個軸的方差。本文傳感器的標準差見表1。

表1 傳感器標準差

由于加速度計和磁力計的單位不同，且參與運算的載體矢量均為歸一化的矢量。故加速度和磁力計的標準差還要分別除以當地重力加速度和磁場總強度進行歸一化。已知武科大青山校區的重力為9.7936 m/s2,磁場強度為49863.1 nT，則協方差矩陣Σacc,Σmag分別為

(30)

由式(30)和式(4)可知，加速度計和磁力計的權重ai分別為0.7507和0.2493。

3.2 實驗結果分析

3.2.1 實時性實驗

影響EKF算法實時性的主要因素是測量值的解算速度。本節比較QUEST算法、快速線性姿態估計算法[11](fast linear attitude estimator，FLAE)，以及本文提出的基于反冪法的確定性算法的解算速度。采集了一組四旋翼動態運動時的傳感器原始數據，使用MATLAB仿真，比較3種算法的時間消耗。為了保證3種算法精度相當，仿真參數設置見表2。IPM算法的近似特征值為1+10-6，其中特征向量距離誤差限3.7×10-5等價于另外兩種算法的特征值誤差限9.8186×10-6。

表2 仿真參數設置

實驗結果如圖3和表3所示。由圖3可知IPM算法與FLAE算法(牛頓解)濾波時間相近，兩者明顯比QUEST算法快。這是因為QUEST算法求解Davenport矩陣K的過程中需要計算伴隨矩陣和行列式，而IPM算法和FLAE算法不需要，所以顯著減少了時間消耗。

圖3 時間消耗比較

算法平均時間(10-5s)最大時間/s最小時間(10-5s)QUEST5.39960.00404.3761FLAE2.70650.00712.1151IPM2.91660.00441.8963

由表3可知，IPM算法的平均時間比經典的QUEST算法的平均時間快45.98%，僅比FLAE算法的平均時間多7.76%。但IPM算法的最小時間比FLAE算法的最小時間還少10.34%，最大時間比FLAE最大時間少38.02%。綜上，IPM算法是一種快速的算法，能夠為無人機提供實時的姿態測量值。

3.2.2 靜態實驗

進行靜態實驗時，將無人機水平放置，靜止不動。去除傳感器零偏后，使用梯度下降擴展卡爾曼濾波算法[12](gradient descent-extended kalman filtering，GD-EKF)與本文的EKF算法同時進行姿態解算。考慮到無人機的實時性，兩種算法的最大迭代次數均設為3次。本文EKF算法的地磁參考矢量根據第12代國際地磁參考場模型得到，GD-EKF算法的偏航角由傾角補償得到。實驗結果如圖4所示。

圖4 靜態測試

由圖4可知，本文EKF算法與GD-EKF算法的俯仰角波動范圍均為±0.1°，本文EKF算法的橫滾角在±0.2°，GD-EKF算法的橫滾角變化范圍超過0.2°。GD-EKF算法的偏航角有明顯的毛刺和突變分量，可能會導致無人機的震蕩。GD-EKF算法3個角的波動幅度均大于本文EKF算法。綜上，本文EKF算法在靜態環境下具有較高精度，且穩定性比GD-EKF算法較好。

3.2.3 繞軸實驗

定義繞X軸旋轉對應俯仰角，繞Y軸旋轉對應橫滾角，繞Z軸旋轉對應偏航角。首先將四旋翼實驗平臺繞X軸順時針旋轉40°，接著逆時針旋轉80°，再順時針旋轉40°回到0°。然后將實驗平臺繞Y軸重復以上步驟。最后將平臺繞Z軸順時針旋轉2圈，回到0°后，再逆時針旋轉2圈。實驗結果如圖5所示。

圖5 繞軸測試

通過對比可知，本文EKF算法與GD-EKF算法計算的橫滾角和俯仰角基本一致，在繞軸過程中GD-EKF算法對橫滾角和俯仰角的跟隨略滯后本文的EKF算法。當四旋翼不水平時，GD-EKF算法計算的偏航角誤差較大，而本文EKF算法通過調整傳感器權重降低了偏航角誤差。當四旋翼水平時，兩種算法計算的偏航角一致。綜上，本文的EKF算法姿態跟蹤性能優于梯度下降擴展卡爾曼濾波算法，能滿足無人機的姿態測量需要。

3.2.4 自由飛行實驗

移植商業飛控的姿態解算算法到本文搭建的平臺上，使用商業算法計算的姿態角作為參考角，將傳感器原始數據和參考角數據發送到上位機。實驗結果如圖6、圖7所示。

圖6 本文算法估計值與參考值

圖7 本文算法估計值與參考值誤差

由圖6可知，本文的EKF算法可以很好跟蹤參考值。由圖7可知，俯仰角和橫滾角的最大誤差均小于2°，僅偏航角在±180°切換時出現了較大誤差，之后很快回到正常值。偏航角出現較大誤差的原因是傳感器原始數據與參考角數據是在不同控制周期發送的，數據傳輸存在延遲。本文算法計算的俯仰角的均方根誤差為0.396°，橫滾角均方根誤差為0.3558°。綜上，本文算法具有較高的精度，能夠滿足四旋翼的實際飛行需要。

4 結束語

本文設計了一種基于反冪法和EKF的姿態解算算法。反冪法基于近似特征值，利用迭代序列直接求解Davenport矩陣K的特征向量(四元數)，降低了測量值對特征值精度的敏感度；此外，將反冪法的計算四元數作為EKF的測量值，降低了測量值維數并有效避免了測量值線性化過程中產生的誤差。仿真實驗和無人機自主飛行實驗結果表明，本文所設計算法能夠滿足無人機的姿態解算任務，具有較高的精度，算法簡單且易于實現。本文下一步的工作是解決運動加速度和磁場干擾對算法精度的影響，以及大范圍運動時磁參考矢量的修正問題。