多旋翼無人機自適應(yīng)DUKF姿態(tài)估計算法

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

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

1 引言

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

針對這些問題，提出一種以殘差矢量為基礎(chǔ)構(gòu)造相應(yīng)的傳感器噪聲協(xié)方差陣的自適應(yīng)策略，這樣構(gòu)造的傳感器的噪聲方差完全與外部干擾分量相對應(yīng)，不會造成構(gòu)造的協(xié)方差與實際情況不相符的情況。同時，結(jié)合一種精度高且實時性好的DUKF算法[8-9]，設(shè)計了一種使用DUKF的多旋翼無人機自適應(yīng)姿態(tài)估計算法。最后，通過提取靜止?fàn)顟B(tài)PIXHAWK飛控中的慣性傳感器和磁力計數(shù)據(jù)，進(jìn)行相關(guān)的仿真實驗，驗證了所提算法的有效性。

2 姿態(tài)估計系統(tǒng)模型

2.1 姿態(tài)估計系統(tǒng)的狀態(tài)方程

則可以建立連續(xù)時間的狀態(tài)方程為[10]：

為了方便迭代計算，將連續(xù)時間的狀態(tài)方程轉(zhuǎn)換為離散時間狀態(tài)方程：

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

2.2 姿態(tài)估計系統(tǒng)的量測方程

使用三軸加速度計可測得重力在機體坐標(biāo)系下的三個分量，使用三軸磁力計可測得地磁場在機體坐標(biāo)系下的三個分量。重力場在地球坐標(biāo)系的三個分量為［0，0，|g|］，其中g(shù)表示重力加速度，現(xiàn)假定當(dāng)?shù)卮艌鲈诘厍蜃鴺?biāo)系的三個分量為［bx，by，bz］，則有基于三軸加速度計和三軸磁力計數(shù)據(jù)的量測方程為：

式中：O—三維零矩陣；—用四元數(shù)表示的地球坐標(biāo)系到機體坐標(biāo)系的姿態(tài)轉(zhuǎn)移矩陣。

3 自適應(yīng)DUKF姿態(tài)估計算法

3.1 自適應(yīng)構(gòu)造傳感器噪聲協(xié)方差陣

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

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

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

式中：ka—常數(shù)，表示設(shè)置的權(quán)重因子。

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

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

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

式中：km—常數(shù)，表示設(shè)置的權(quán)重因子。

則總的噪聲協(xié)方差陣為：

3.2 自適應(yīng)DUKF姿態(tài)估計算法

根據(jù)前面的系統(tǒng)狀態(tài)方程和量測方程，以姿態(tài)四元數(shù)為狀態(tài)向量的非線性系統(tǒng)為：

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

則自適應(yīng)DUKF姿態(tài)估計算法流程為：

(1)系統(tǒng)初始化

設(shè)置初始狀態(tài)四元數(shù)為q0=[ 10 0 0]T，初始狀態(tài)誤差協(xié)方差陣為：

(2)計算系統(tǒng)先驗估計

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

②計算離散時間狀態(tài)轉(zhuǎn)移矩陣

③時間更新

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

k-1時刻系統(tǒng)先驗狀態(tài)估計誤差協(xié)方差為：

④Sigma點選取

根據(jù)先驗狀態(tài)估計值及其誤差協(xié)方差選擇一組加權(quán)Sigma點，選取的Sigma點為：

式中：a∈R—調(diào)節(jié)參數(shù)，控制Sigma點在周圍的分布；i—矩陣均方根的第i列。

(3)量測更新

①計算變換后的Sigma點

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

②讀取加速度計和磁力計數(shù)據(jù)

③計算殘差εk，并使用自適應(yīng)的方法構(gòu)造總的噪聲協(xié)方差陣Rk

加速計噪聲協(xié)方差為：

式中：ka—加速度計噪聲的自適應(yīng)系數(shù)；

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

磁力計噪聲協(xié)方差為：

總噪聲協(xié)方差為：

④計算量測預(yù)測值及其協(xié)方差

計算先驗狀態(tài)估計值與量測預(yù)測值間的互協(xié)方差

⑤確定卡爾曼增益矩陣

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

圖1 算法結(jié)構(gòu)框圖Fig1 Algorithm Block Diagram

4仿真實驗及結(jié)果分析

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

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

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

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

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

5 結(jié)論

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