多旋翼無人機自適應DUKF姿態估計算法

蔡安江，劉凱峰，劉立博，翟彥昭

(西安建筑科技大學機電工程學院，陜西 西安 710055)

1 引言

多旋翼無人機的姿態估計是多旋翼無人機導航及控制的基礎，姿態估計的精度極大地影響著無人機導航和控制等方面的精度[1-2]。現有研究人員對于如何使用存在噪聲干擾的陀螺儀、加速度計和磁力計數據獲得精確的姿態角的問題進行了大量的研究[3-6]。在靜止或穩定飛行的狀態下，使用加速度計可以很方便地估計出無人機的橫滾角和俯仰角，但無人機處于加速或減速狀態時，由于外部加速度的存在，會極大地影響橫滾角和俯仰角估計的精度，甚至會使測量的橫滾角和俯仰角完全無法使用。同理，當環境中存在較大的電磁干擾時，磁力計測得的數據會嚴重失真，在估計航向角時，也會出現較大誤差。針對這個問題，文獻[7]提出一種自適應濾波方法，該方法基于擴展卡爾曼濾波算法進行無人機姿態姿態解算，在卡爾曼濾波的校正階段，以測量的加速度的模與重力加速度絕對值的差值為基數乘以單位矩陣作為加速度計的噪聲協方差，以測量的磁場的模與當地地磁矢量絕對值的差值為基數乘以單位矩陣作為磁力計的噪聲協方差，當存在較大的外部加速度或干擾磁場時，加速度計或磁力計的噪聲協方差會增加，在卡爾曼濾波的解算中會減小對加速度計或磁力計數據的權重，外部干擾越大，加速度計或磁力計數據的權重會越小，在一定程度上可以減小外部干擾。但這種方法使加速度計或磁力計的每個分量都具有相同的噪聲方差，而實際情況中一般不會出現每個分量都具有相同的干擾，這樣構造的噪聲協方差會與實際情況不相符，嚴重時可能導致濾波發散。

針對這些問題，提出一種以殘差矢量為基礎構造相應的傳感器噪聲協方差陣的自適應策略，這樣構造的傳感器的噪聲方差完全與外部干擾分量相對應，不會造成構造的協方差與實際情況不相符的情況。同時，結合一種精度高且實時性好的DUKF算法[8-9]，設計了一種使用DUKF的多旋翼無人機自適應姿態估計算法。最后，通過提取靜止狀態PIXHAWK飛控中的慣性傳感器和磁力計數據，進行相關的仿真實驗，驗證了所提算法的有效性。

2 姿態估計系統模型

2.1 姿態估計系統的狀態方程

則可以建立連續時間的狀態方程為[10]：

為了方便迭代計算，將連續時間的狀態方程轉換為離散時間狀態方程：

式中：T—解算周期；I—四階單位矩陣。為了描述方便，后續內容中符號將用q代替。

2.2 姿態估計系統的量測方程

使用三軸加速度計可測得重力在機體坐標系下的三個分量，使用三軸磁力計可測得地磁場在機體坐標系下的三個分量。重力場在地球坐標系的三個分量為［0，0，|g|］，其中g表示重力加速度，現假定當地磁場在地球坐標系的三個分量為［bx，by，bz］，則有基于三軸加速度計和三軸磁力計數據的量測方程為：

式中：O—三維零矩陣；—用四元數表示的地球坐標系到機體坐標系的姿態轉移矩陣。

3 自適應DUKF姿態估計算法

3.1 自適應構造傳感器噪聲協方差陣

卡爾曼濾波過程中，殘差表示理論估計值與實際測量值之差，正常情況下這個差值是比較小的，但是當存在較大的外部干擾時，這個差值就會變得比較大，并且這個值與外部干擾的大小是成正比的。所以提出根據卡爾曼濾波過程中的殘差來構造加速度計和磁力計的測量噪聲協方差的自適應策略。

在加速度計校正過程中，假設加速度殘差為：

其中εk1(1)、εk1(2)和εk1(3)就分別表示x軸、y軸和z軸外部加速度干擾的程度，此時構造加速度計噪聲協方差為：

式中：ka—常數，表示設置的權重因子。

則當某軸存在外部加速度時，相應軸的殘差分量可以表示其外部加速度大小，外部加速度較大時，該軸的量測噪聲方差值也較大，卡爾曼濾波器在估計過程中對該軸加速度計測量值的信任度較低，使用該軸加速度數據對姿態角估計的校正程度也就越低，在很大程度上就丟棄了該存在干擾的數據；反之，當殘差值較小時，表示存在的外部加速度干擾較小，此時構造的測量噪聲方差值也較小，卡爾曼濾波器在估計過程中對該軸加速度計的測量值的信任度較高，該軸加速度數據對姿態角估計的校正程度也就越高，使正確的加速度值能更好的校正姿態估計值。

同理，在磁力計校正過程中，假設地磁場測量殘差為：

式中：εk2(1)、εk2(2)和εk2(3)—x軸、y軸和z軸外部電磁場干擾的程度，此時構造磁力計噪聲協方差為：

式中：km—常數，表示設置的權重因子。

則總的噪聲協方差陣為：

3.2 自適應DUKF姿態估計算法

根據前面的系統狀態方程和量測方程，以姿態四元數為狀態向量的非線性系統為：

式中：qk—k時刻系統的四元數狀態向量；zk—k時刻三軸加速度計數據和三軸磁力計數據組成的量測向量；Ak-1—式(5)中的狀態轉移矩陣；wk，vk—互不相關的零均值高斯白噪聲過程，其方差分別為：

則自適應DUKF姿態估計算法流程為：

(1)系統初始化

設置初始狀態四元數為q0=[ 10 0 0]T，初始狀態誤差協方差陣為：

(2)計算系統先驗估計

①讀取三軸陀螺儀數據，獲得wx,wy和wz，將其帶入式(4)中計算得到Ω。

②計算離散時間狀態轉移矩陣

③時間更新

由于系統狀態方程具有線性特征，系統先驗狀態估計值及其誤差協方差陣的計算與線性卡爾曼濾波相同，則k-1時刻系統先驗狀態估計值為：

k-1時刻系統先驗狀態估計誤差協方差為：

④Sigma點選取

根據先驗狀態估計值及其誤差協方差選擇一組加權Sigma點，選取的Sigma點為：

式中：a∈R—調節參數，控制Sigma點在周圍的分布；i—矩陣均方根的第i列。

(3)量測更新

①計算變換后的Sigma點

經過非線性量測方程h(·)變換后的Sigma點為：

②讀取加速度計和磁力計數據

③計算殘差εk，并使用自適應的方法構造總的噪聲協方差陣Rk

加速計噪聲協方差為：

式中：ka—加速度計噪聲的自適應系數；

εk(i)—矢量εk的第i個元素。

磁力計噪聲協方差為：

總噪聲協方差為：

④計算量測預測值及其協方差

計算先驗狀態估計值與量測預測值間的互協方差

⑤確定卡爾曼增益矩陣

(4)進入下一個迭代周期，重復步驟2-3的過程，算法流程，如圖1所示。

圖1 算法結構框圖Fig1 Algorithm Block Diagram

4仿真實驗及結果分析

通過提取水平靜止狀態的PIXHAWK飛控120s的陀螺儀、加速度計和磁力計的數據進行相關仿真實驗。其中，為了驗證所提自適應算法在存在外部加速度的抗干擾能力，在加速度數據中按照一定時間間隔加入固定的常值偏移以模擬存在的外部加速度干擾，使用未加自適應環節的DUKF算法和所提算法(A-DUKF)進行仿真實驗對比，仿真結果，如圖2所示。并對各算法的姿態估計誤差均方根值(RMS)進行計算，計算結果，如表1所示。為了驗證所提自適應算法在存在外部磁場干擾時的抗干擾能力，在磁力計數據中按照一定時間間隔加入固定的常值偏移以模擬外部環境中存在的磁異常，使用未加自適應環節的DUKF算法和所提算法(ADUKF)進行仿真實驗對比，仿真結果如圖3所示，并對各算法的姿態估計誤差均方根值(RMS)進行計算，計算結果，如表2所示。

圖2 存在外部加速度干擾的姿態角對比Fig.2 Attitude Angle Comparison with External Acceleration Interference

表1 存在外部加速度干擾的姿態角估計誤差(°)Tab.1 Attitude Angle Estimation Error with External Acceleration Interference(°)

圖3 存在外部磁場干擾的姿態角對比Fig.3 Attitude Angle Comparison of External Magnetic Field Interference

表2 存在外部磁場干擾的姿態角估計誤差(°)Tab.2 Attitude Angle Estimation Error with External Magnetic Field Interference(°)

由圖2(a)、圖2(b)、圖2(c)和表1可以看出，由于外部加速度的干擾，DUKF算法的橫滾角、俯仰角和航向角估計均存在較大偏差，其橫滾角、俯仰角和航向角估計的誤差均方根值分別為0.8147°、1.0256°和0.3823°；而所提的自適應姿態估計算法(ADUKF)則具有較高的姿態估計精度，其橫滾角、俯仰角和航向角的姿態估計誤差均方根值分別為0.0570°、0.1533°和0.0435°。由圖3(a)、圖3(b)、圖3(c)和表2可以看出，由于外部電磁場的干擾，DUKF算法估計的橫滾角、俯仰角和航向角均存在很大的誤差，其橫滾角、俯仰角和航向角的姿態估計誤差均方根值分別為0.9475°、0.9804°和3.9198°；而所提自適應姿態估計算法(A-DUKF)則能較精確的估計這三個姿態角，其橫滾角、俯仰角和航向角的姿態估計誤差均方根值分別為0.0168°、0.0195°和0.2051°。

5 結論

為了提高存在外部加速度或外部電磁場干擾時多旋翼無人機的姿態估計精度，使用根據殘差構造加速度計和磁力計噪聲協方差的自適應策略，并結合一種計算精度高且計算量較小的衍生無跡卡爾曼濾波(DUKF)算法，由此提出一種多旋翼無人機自適應DUKF姿態估計算法。利用從水平靜止狀態的PIXHAWK飛控中提取的傳感器數據，將所提算法(A-DUKF)與未使用自適應策略的DUKF算法進行仿真實驗對比，結果表明，與DUKF算法相比，所提算法(A-DUKF)在存在外部加速度干擾的情況下，橫滾角估計誤差均方根值由0.8147°減小為0.0570°，俯仰角估計誤差均方根值由1.0256°減小為0.1533°，航向角姿態估計誤差均方根值由0.3823°減小為0.0435°；在存在外部磁場干擾的情況下，橫滾角估計誤差均方根值由0.9475°減小為0.0168°，俯仰角估計誤差均方根值由0.9804°減小為0.0195°，航向角姿態估計誤差均方根值由3.9198°減小為0.2051°。證明了所提算法在抗干擾能力方面的有效性，對工程實踐具有一定的參考意義。