基于反冪法和擴展卡爾曼濾波的姿態(tài)估計算法

張 雄，黃衛(wèi)華，陳勁峰

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

0 引 言

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

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

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

1 基于反冪法和EKF的姿態(tài)估計算法的算法結(jié)構(gòu)

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

圖1 基于反冪法和EKF的組合濾波系統(tǒng)結(jié)構(gòu)

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

2 基于反冪法和EKF的姿態(tài)解算算法

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

2.1.1 近似特征值求解

(1)

(2)

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

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

(3)

其中，ai為傳感器的權(quán)重，i=1,2，且

(4)

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

(5)

其中，四元數(shù)q=[q0,q1,q2,q3]T，D1、D2、D3是關(guān)于四元數(shù)q的矩陣。

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

(6)

則式(3)變?yōu)?/p>

(7)

D1THxT+D2THyT+D3THzT-q=0

(8)

其中

(9)

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

(K-I)q=0

(10)

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

(11)

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

Kq=(1+ε1)q

(12)

其中，1+ε1是Davenport矩陣K的近似特征值，四元數(shù)q是特征值1+ε1對應(yīng)的特征向量。

2.1.2 反冪法求解

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

(13)

其中，α為近似特征值。

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

K-αI=LU

(14)

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

(15)

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

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

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

(16)

其中，ε2<10-4。

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

2.1.3 算法步驟

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

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

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

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

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

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

步驟7 若k小于最大迭代次數(shù)N，置k=k+1，轉(zhuǎn)步驟5；否則停算。

2.2 擴展卡爾曼濾波器設(shè)計

2.2.1 濾波觀測方程

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

(17)

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

zt=qmeasure,t+vt

(18)

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

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

(19)

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

Σvt=JΣacc,magJT

(20)

其中，雅可比矩陣J為

(21)

其中，ax,ay,az分別為加速度計的三軸數(shù)據(jù)；mx,my,mz分別為磁力計的三軸數(shù)據(jù)； Σacc,mag為加速度計和磁力計噪聲構(gòu)成的協(xié)方差矩陣

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

(22)

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

2.2.2 濾波狀態(tài)方程

選取四元數(shù)作為狀態(tài)變量，根據(jù)四元數(shù)微分方程可知

(23)

其中，Ωb為

(24)

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

(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)中狀態(tài)量qt關(guān)于陀螺儀三軸輸出ωx,ωy,ωz的雅克比矩陣

(29)

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

3 實驗與結(jié)果分析

3.1 實驗測試平臺

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

圖2 無人機系統(tǒng)

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

表1 傳感器標準差

由于加速度計和磁力計的單位不同，且參與運算的載體矢量均為歸一化的矢量。故加速度和磁力計的標準差還要分別除以當?shù)刂亓铀俣群痛艌隹倧姸冗M行歸一化。已知武科大青山校區(qū)的重力為9.7936 m/s2,磁場強度為49863.1 nT，則協(xié)方差矩陣Σacc,Σmag分別為

(30)

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

3.2 實驗結(jié)果分析

3.2.1 實時性實驗

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

表2 仿真參數(shù)設(shè)置

實驗結(jié)果如圖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算法的平均時間比經(jīng)典的QUEST算法的平均時間快45.98%，僅比FLAE算法的平均時間多7.76%。但IPM算法的最小時間比FLAE算法的最小時間還少10.34%，最大時間比FLAE最大時間少38.02%。綜上，IPM算法是一種快速的算法，能夠為無人機提供實時的姿態(tài)測量值。

3.2.2 靜態(tài)實驗

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

圖4 靜態(tài)測試

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

3.2.3 繞軸實驗

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

圖5 繞軸測試

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

3.2.4 自由飛行實驗

移植商業(yè)飛控的姿態(tài)解算算法到本文搭建的平臺上，使用商業(yè)算法計算的姿態(tài)角作為參考角，將傳感器原始數(shù)據(jù)和參考角數(shù)據(jù)發(fā)送到上位機。實驗結(jié)果如圖6、圖7所示。

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

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

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

4 結(jié)束語

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