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

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

1 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

同理，在磁力計(jì)校正過(guò)程中，假設(shè)地磁場(chǎng)測(cè)量殘差為：

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

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

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

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

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

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

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

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

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

(2)計(jì)算系統(tǒng)先驗(yàn)估計(jì)

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

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

③時(shí)間更新

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

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

④Sigma點(diǎn)選取

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

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

(3)量測(cè)更新

①計(jì)算變換后的Sigma點(diǎn)

經(jīng)過(guò)非線性量測(cè)方程h(·)變換后的Sigma點(diǎn)為：

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

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

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

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

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

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

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

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

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

⑤確定卡爾曼增益矩陣

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

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

4仿真實(shí)驗(yàn)及結(jié)果分析

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

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

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

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

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

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

5 結(jié)論

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