一種基于AR運(yùn)動(dòng)模型CKF算法的無(wú)人機(jī)導(dǎo)航

孫 蕊，程 琦，張羽成，王冠宇

(南京航空航天大學(xué) 民航學(xué)院，江蘇 南京 211106)

引用著錄：孫蕊，程琦，張羽成，等.一種基于AR運(yùn)動(dòng)模型CKF算法的無(wú)人機(jī)導(dǎo)航[J].測(cè)繪工程，2017，26(12)：41-44，49.

DOI:10.19349/j.cnki.issn1006-7949.2017.12.008

一種基于AR運(yùn)動(dòng)模型CKF算法的無(wú)人機(jī)導(dǎo)航

孫 蕊，程 琦，張羽成，王冠宇

(南京航空航天大學(xué) 民航學(xué)院，江蘇 南京 211106)

針對(duì)無(wú)人機(jī)組合導(dǎo)航濾波算法中傳統(tǒng)的運(yùn)動(dòng)模型單一固定，靈活性差，無(wú)法精確描述無(wú)人機(jī)復(fù)雜多變的運(yùn)動(dòng)狀態(tài)，甚至還會(huì)導(dǎo)致濾波發(fā)散等問題，提出了一種基于AR運(yùn)動(dòng)模型的CKF算法，并應(yīng)用于無(wú)人機(jī)導(dǎo)航定位中。通過(guò)滑動(dòng)窗的方法，構(gòu)建實(shí)時(shí)動(dòng)態(tài)更新AR模型估計(jì)物體運(yùn)動(dòng)狀態(tài)，并且結(jié)合CKF進(jìn)行濾波，從而有效提高無(wú)人機(jī)導(dǎo)航性能。仿真實(shí)驗(yàn)結(jié)果表明，該算法能夠有效提高無(wú)人機(jī)導(dǎo)航定位精度，優(yōu)于其余幾種基于傳統(tǒng)運(yùn)動(dòng)模型的濾波算法。

無(wú)人機(jī)；導(dǎo)航；濾波算法；自回歸模型；GPS

精確導(dǎo)航定位技術(shù)是無(wú)人機(jī)實(shí)現(xiàn)安全飛行的基礎(chǔ)。全球定位系統(tǒng)(Global Positioning System, GPS)和慣性導(dǎo)航系統(tǒng)(Inertial Navigation System, INS)可以實(shí)時(shí)獲取無(wú)人機(jī)的位置和姿態(tài)信息，目前在無(wú)人機(jī)導(dǎo)航中被廣泛應(yīng)用。GPS/INS通過(guò)濾波算法進(jìn)行組合，可以有效提高無(wú)人機(jī)導(dǎo)航性能。濾波算法可以對(duì)GPS與INS數(shù)據(jù)進(jìn)行估計(jì)、修正、融合，對(duì)兩個(gè)系統(tǒng)進(jìn)行取長(zhǎng)補(bǔ)短，來(lái)解決GPS信號(hào)失鎖引起的無(wú)法定位問題和INS長(zhǎng)時(shí)間積累誤差引起的漂移問題，因此在無(wú)人機(jī)組合導(dǎo)航中具有非常重要的作用。目前，無(wú)人機(jī)導(dǎo)航大多采用非線性濾波。早期的擴(kuò)展卡爾曼濾波 (Extended Kalman Filter, EKF)通過(guò)對(duì)非線性函數(shù)進(jìn)行Taylor級(jí)數(shù)展開，從而實(shí)現(xiàn)線性化解算。但是其精度只停留在一階，尤其在無(wú)人機(jī)高動(dòng)態(tài)系統(tǒng)的導(dǎo)航中誤差較大[1-2]。無(wú)跡卡爾曼濾波(Unscented Kalman filter, UKF)通過(guò)經(jīng)無(wú)跡變換后的采樣點(diǎn)集來(lái)逼近非線性函數(shù)概率分布，因此不需要計(jì)算非線性系統(tǒng)的雅可比矩陣，相比EKF具有更好的非線性估計(jì)性能[3]。粒子濾波(Particle filter，PF)是一種基于蒙特卡洛仿真的濾波，它通過(guò)粒子分布來(lái)逼近系統(tǒng)的后驗(yàn)概率，理論上只要粒子數(shù)目足夠多，就可以實(shí)現(xiàn)任意估計(jì)精度。然而PF隨著粒子數(shù)目增多而導(dǎo)致計(jì)算量增大，因此難以滿足無(wú)人機(jī)實(shí)際導(dǎo)航應(yīng)用中的實(shí)時(shí)性需求[4]。

為了更好地滿足無(wú)人機(jī)的導(dǎo)航需求，有必要研究精度更高、更易工程實(shí)現(xiàn)的方法。容積卡爾曼濾波(Cubature Kalman filter , CKF)是近年來(lái)提出的一種新型非線性高斯濾波方法。CKF具有嚴(yán)格的數(shù)學(xué)證明，通過(guò)三階容積法則的數(shù)值積分方法來(lái)近似高斯加權(quán)積分，充分利用了容積積分近似計(jì)算多維函數(shù)積分，具有效率高的特點(diǎn)。經(jīng)證明其對(duì)隨機(jī)變量非線性變換后概率分布具有良好的逼近精度。相比于EKF、UKF和PF等非線性濾波算法，CKF算法的計(jì)算結(jié)果對(duì)系統(tǒng)狀態(tài)的估計(jì)更加精確，更能有效逼近非線性系統(tǒng)的真實(shí)狀態(tài)，輸出結(jié)果也更穩(wěn)定，而且實(shí)現(xiàn)簡(jiǎn)單，能夠達(dá)到三階精度[5]。

在濾波算法中，無(wú)人機(jī)的運(yùn)動(dòng)模型通過(guò)對(duì)短時(shí)間內(nèi)其所遵循的規(guī)律來(lái)預(yù)測(cè)下一個(gè)時(shí)刻的狀態(tài)，對(duì)實(shí)現(xiàn)高性能的估計(jì)至關(guān)重要。實(shí)際情況中運(yùn)動(dòng)載體一般難以確保規(guī)則運(yùn)動(dòng), 因而構(gòu)造精確的函數(shù)模型十分困難[6]。如果其不能精準(zhǔn)地表征物體實(shí)際運(yùn)動(dòng)狀態(tài)，就會(huì)導(dǎo)致濾波器估計(jì)精度下降甚至發(fā)散[7-9]。關(guān)于運(yùn)動(dòng)模型的構(gòu)建，國(guó)內(nèi)外學(xué)者都做了相應(yīng)的研究，并且應(yīng)用到多個(gè)領(lǐng)域。傳統(tǒng)運(yùn)動(dòng)模型的研究包括：勻速(Constant Velocity, CV)模型、勻加速(Constant Acceleration , CA)模型、恒定轉(zhuǎn)向角的勻速(Constant Turn Rate and Velocity, CTRV)模型、恒定轉(zhuǎn)向角的勻加速(Constant Turn Rate and Acceleration, CTRA)模型、恒定曲率和加速度(Constant Curvature and Acceleration, CCA)模型等[10-11]。然而這些傳統(tǒng)模型單一固定，靈活性差，無(wú)法精確地描述出無(wú)人機(jī)復(fù)雜多變的運(yùn)動(dòng)狀態(tài)。為了克服這一缺點(diǎn)，近幾年來(lái),基于實(shí)時(shí)動(dòng)態(tài)的運(yùn)動(dòng)特征來(lái)構(gòu)建運(yùn)動(dòng)模型的研究逐漸興起。Zhou 和 Li針對(duì)全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System, GNSS)導(dǎo)航中傳統(tǒng)運(yùn)動(dòng)模型只考慮前一時(shí)刻狀態(tài)信息的問題，提出基于滑動(dòng)窗口和多項(xiàng)式擬合的實(shí)時(shí)動(dòng)態(tài)運(yùn)動(dòng)模型，該方法可以從多個(gè)歷史時(shí)段中得到更多信息，從而提高了導(dǎo)航系統(tǒng)的準(zhǔn)確性和可靠性[12]。Lan等提出了一種多模態(tài)運(yùn)動(dòng)模型并行自適應(yīng)估計(jì)算法，該算法在描述和處理復(fù)雜的運(yùn)動(dòng)過(guò)程時(shí)具有較強(qiáng)的靈活性[13]。Jin等提出了基于最小均方根誤差優(yōu)化的自回歸運(yùn)動(dòng)狀態(tài)預(yù)測(cè)模型，該模型比基于傳統(tǒng)運(yùn)動(dòng)模型的卡爾曼濾波方法得到的精度更高[14]。在地面車輛的位置和運(yùn)動(dòng)狀態(tài)估計(jì)中，這些新型的運(yùn)動(dòng)模型確實(shí)在一定程度上展現(xiàn)了他們的優(yōu)勢(shì)。但是，傳統(tǒng)的車輛運(yùn)動(dòng)模型都是基于二維平面，而無(wú)人機(jī)的運(yùn)動(dòng)是在三維空間中，含有更多的變量、同時(shí)環(huán)境也更為復(fù)雜?，F(xiàn)有的運(yùn)動(dòng)模型依然無(wú)法精確的描述無(wú)人機(jī)的運(yùn)動(dòng)狀況。

針對(duì)以上問題，本文提出了一種基于自回歸(Auto Regressive, AR)運(yùn)動(dòng)模型的CKF算法并且運(yùn)用于無(wú)人機(jī)導(dǎo)航中。文章通過(guò)滑動(dòng)窗的方法，構(gòu)建實(shí)時(shí)動(dòng)態(tài)更新的AR運(yùn)動(dòng)模型，對(duì)無(wú)人機(jī)的運(yùn)動(dòng)狀態(tài)進(jìn)行預(yù)測(cè)，從而有效提高無(wú)人機(jī)的導(dǎo)航性能。最后，通過(guò)在仿真實(shí)驗(yàn)中和其余幾種基于傳統(tǒng)運(yùn)動(dòng)模型的CKF算法進(jìn)行比較，證明了設(shè)計(jì)算法的有效性。

1 AR運(yùn)動(dòng)模型的構(gòu)建

傳統(tǒng)的離散時(shí)間差分模型可以描繪物體運(yùn)動(dòng)，但它們一旦建立就不會(huì)變化，不能自適應(yīng)地隨著過(guò)程噪聲和測(cè)量噪聲強(qiáng)度去調(diào)整，自然會(huì)在一定程度上導(dǎo)致性能降低。針對(duì)該問題，本文用AR模型來(lái)描述無(wú)人機(jī)的運(yùn)動(dòng)狀態(tài)。AR 模型的原理是：通過(guò)對(duì)一組已有的歷史數(shù)據(jù)進(jìn)行線性組合，給出關(guān)心時(shí)刻的預(yù)測(cè)信息[15],即根據(jù)p個(gè)歷史數(shù)據(jù)信息，推出第p時(shí)刻前面或后面的數(shù)據(jù)。其本質(zhì)與插值法類似，兩者都是增加有效數(shù)據(jù)，不同的是AR模型是由p點(diǎn)遞推，而插值是由兩點(diǎn)(或少數(shù)幾點(diǎn))去推導(dǎo)多點(diǎn)，所以AR模型要比插值方法效果更好。

歷史數(shù)據(jù)Xt,白噪聲序列表示為at,回歸系數(shù)用φj(j=1,2,..p)表示，則可得到AR模型：

Xi+1=φ1Xt+φ2Xt-1+φ3Xt-2+…+φpXt-p+1+at.

(1)

2 基于AR模型的CKF算法(CKF-AR)

在非線性系統(tǒng)中，一般動(dòng)態(tài)模型是離散時(shí)間運(yùn)動(dòng)模型，形式如下：

xk+1=Fk+1|kxk+wk.

(2)

式中：xk表示k時(shí)刻的狀態(tài)向量；Fk+1|k是狀態(tài)轉(zhuǎn)移矩陣；過(guò)程噪聲wk為零均值高斯隨機(jī)過(guò)程；其協(xié)方差矩陣為Qk。

k時(shí)刻觀測(cè)方程如下：

zk=Hxk+vk.

(3)

式中：zk是系統(tǒng)的量測(cè)值；H為測(cè)量矩陣；測(cè)量噪聲vk是零均值高斯隨機(jī)過(guò)程，獨(dú)立于wk,其協(xié)方差矩陣為Rk。

在本文算法中，選取系統(tǒng)的狀態(tài)向量為xk=[Ek,Nk,Uk,VEk,VNk,VUk,θEk,θNk,θUk]T。其中Ek,Nk,Uk分別是無(wú)人機(jī)在當(dāng)?shù)刈鴺?biāo)系中的東向、北向和高度坐標(biāo)；VEk,VNk,VUk分別是無(wú)人機(jī)在東向、北向和垂直方向上的速度；θEk,θNk,θUk為無(wú)人機(jī)的姿態(tài)角。在構(gòu)建CKF-AR算法時(shí)，需要解決一個(gè)問題。即在傳統(tǒng)的濾波算法的狀態(tài)轉(zhuǎn)移方程中第k時(shí)刻的狀態(tài)僅與上一時(shí)刻狀態(tài)有關(guān)，而在AR模型中，第k時(shí)刻的狀態(tài)與前p個(gè)時(shí)刻狀態(tài)有關(guān)。針對(duì)該問題，本文算法創(chuàng)新之處在于先由AR模型預(yù)測(cè)出xk，然后構(gòu)造出如下關(guān)系式：

(4)

xEk=[EkEk-1Ek-2…Ek-p+1]T.

(5)

Ek+1=φEkxEk+aEk.

(6)

式中：φEk是Ek的回歸系數(shù)；xEk是前p個(gè)歷史數(shù)據(jù)；aEk是k時(shí)刻Ek的白噪聲，這樣就得到了k+1時(shí)刻的預(yù)測(cè)值。Nk,Uk,VEk,VNk,VUk,θEk,θNk,θUk的計(jì)算過(guò)程類似，這里不再贅述。CKF-AR算法步驟如下：

1)獲得一組采樣點(diǎn)及其對(duì)應(yīng)權(quán)值。

n.

(7)

每個(gè)粒子對(duì)應(yīng)權(quán)值：

(8)

式中：n是維度。

2)計(jì)算容積點(diǎn)集的一步預(yù)測(cè)，i=1，2

(9)

3)計(jì)算系統(tǒng)狀態(tài)量的一步預(yù)測(cè)及協(xié)方差矩陣，一步預(yù)測(cè)由容積點(diǎn)集的預(yù)測(cè)加權(quán)求和得到。

(10)

(11)

4)根據(jù)一步預(yù)測(cè)值，對(duì)P(k+1|k)做Cholesky分解，產(chǎn)生新的容積點(diǎn)集。

(12)

(13)

5)將由4)預(yù)測(cè)的容積點(diǎn)集代入觀測(cè)方程，得到預(yù)測(cè)的觀測(cè)量，i=1，2…2n。

(14)

6)由5)得到容積點(diǎn)集的預(yù)測(cè)觀測(cè)值，通過(guò)加權(quán)求和得到系統(tǒng)預(yù)測(cè)值、k+1時(shí)刻量測(cè)誤差協(xié)方差陣及k+1時(shí)刻一步預(yù)測(cè)互相關(guān)協(xié)方差陣。

(15)

(16)

(17)

7)計(jì)算Kalman增益矩陣。

(18)

8)最后，計(jì)算系統(tǒng)的狀態(tài)更新和協(xié)方差更新。

(19)

(20)

令k為k+ 1，并返回步驟1。

3 實(shí)驗(yàn)和分析

本實(shí)驗(yàn)利用Matlab隨機(jī)模擬一段不規(guī)則無(wú)人機(jī)飛行軌跡，如圖1所示。在整個(gè)期間,無(wú)人機(jī)均以變化的速度飛行。采樣頻率為10 Hz，共采樣1 200個(gè)點(diǎn)，對(duì)應(yīng)的東、北、高方向上的坐標(biāo)為(E，N，U)、連接這些樣本點(diǎn)的曲線作為參考軌跡。同時(shí)利用仿真軟件生成3組1 200個(gè)滿足均值為0，方差為1的正態(tài)分布散點(diǎn)w1、w2、w3作為誤差。對(duì)應(yīng)地用(E(i)+w1(i),N(i)+w2(i),U(i)+w3(i))作為GPS的觀測(cè)數(shù)據(jù)。實(shí)驗(yàn)設(shè)定觀測(cè)噪聲和過(guò)程噪聲都是均值為0、方差為1的高斯白噪聲。

圖1 飛行軌跡

本次仿真主要觀測(cè)信息為無(wú)人機(jī)的位置坐標(biāo)，比較本文算法與基于CV模型的CKF算法(CKF-CV)和基于CA模型的CKF算法(CKF-CA)在東、北、高方向上的濾波效果，比較整個(gè)飛行過(guò)程中定位精度；然后分別計(jì)算上述濾波模型及觀測(cè)數(shù)據(jù)的均方根誤差(Root Mean Square Error, RMSE)，通過(guò)均方根誤差的大小來(lái)評(píng)價(jià)上述各方案的表現(xiàn)。均方根誤差對(duì)一組測(cè)量中的特大或特小誤差反映非常敏感，所以，均方根誤差能夠很好地反映出測(cè)量的精密度。RMSE計(jì)算公式如下：

(21)

式中：Xgx(i)為被評(píng)價(jià)的值，即濾波后的值或觀測(cè)值；xr(i)為真值，n為觀測(cè)次數(shù)

圖2顯示了CKF-AR、CKF-CA和CKF-CV算法在東、北、高方向上的濾波結(jié)果，表1計(jì)算了各算法的均方根誤差。

圖2 各個(gè)方向?yàn)V波結(jié)果對(duì)比

根據(jù)圖2和表1的結(jié)果,得出結(jié)論如下：

1)在整個(gè)仿真運(yùn)動(dòng)過(guò)程中，飛行軌跡有直線也有曲線，飛行過(guò)程隨機(jī)性和不規(guī)則性極強(qiáng)。CKF-AR算法具有非常明顯的效果，定位精度提高了55.4%，而對(duì)應(yīng)的CKF-CA及CKF-CV分別提高了25.9%和14.0%。這表明本文所設(shè)計(jì)的算法可以充分描述無(wú)人機(jī)不規(guī)則運(yùn)動(dòng)的特征，大大提高了定位精度。

2)在整個(gè)仿真運(yùn)動(dòng)過(guò)程中，無(wú)人機(jī)均是以變化的速度及變化的加速度飛行，這表明CKF-AR算法對(duì)無(wú)人機(jī)的運(yùn)動(dòng)類型沒有限制，在無(wú)人機(jī)導(dǎo)航定位中可以適應(yīng)無(wú)人機(jī)多種類型的飛行行為。

4 結(jié)束語(yǔ)

本文構(gòu)建了一種基于AR運(yùn)動(dòng)模型的CKF自適應(yīng)濾波算法并運(yùn)用于無(wú)人機(jī)導(dǎo)航，并且在和基于傳統(tǒng)的運(yùn)動(dòng)模型濾波算法比較中，獲得了較好的估計(jì)精度。實(shí)驗(yàn)證明，不管無(wú)人機(jī)是在直線還是曲線的運(yùn)動(dòng)情況下，CKF-AR算法都具有比較好的定位精度，并且在運(yùn)動(dòng)模型的構(gòu)建中，不需要速度、加速度等一系列其他的運(yùn)動(dòng)特征，可以實(shí)現(xiàn)實(shí)時(shí)快速運(yùn)動(dòng)模型的構(gòu)建。在今后的研究中，將在多個(gè)場(chǎng)景中采集真實(shí)數(shù)據(jù)，從而進(jìn)一步對(duì)所設(shè)計(jì)的算法進(jìn)行驗(yàn)證。

[1] LIU G, LI Q, SHI W, et al. Application of dynamic Kalman filtering in state estimation of navigation test[J]. Chinese Journal of Scientific Instrument, 2009, 30(2):396-400.

[2] WANG J. Research of algorithm for mobile node localization in mine based on Kalman filtering[J]. Journal of Electronic Measurement & Instrument, 2013.

[3] Hu, G., Gao, S., & Zhong, Y. A derivative UKF for tightly coupled INS/GPS integrated navigation.[J]. Isa Transactions, 2015: 135-144.

[4] Carmi A, Oshman Y. Fast Particle Filtering for Attitude and Angular-Rate Estimation from Vector Observations[J]. Journal of Guidance Control & Dynamics, 2015, 32(1):70-78.

[5] JIA B, XIN M, CHENG Y. High-degree cubature Kalman filter[J]. Automatica, 2013, 49(2):510-518.

[6] 高為廣,張雙成,王飛,等. GPS導(dǎo)航中的抗差自適應(yīng)Kalman濾波算法[J]. 測(cè)繪科學(xué), 2005, 30(2):98-100.

[7] JWO D, WANG S. Adaptive fuzzy strong tracking extended Kalman filtering for GPS navigation[J]. IEEE Sensors Journal, 2007, 7(5): 778-789.

[8] 李勇軍, 左娟. GPS動(dòng)態(tài)定位自適應(yīng)卡爾曼濾波算法研究[J]. 測(cè)繪工程, 2012, 21(4):29-32.

[9] 楊元喜, 唐穎哲, 李慶田,等. 用于GIS道路信息修測(cè)的動(dòng)態(tài)GPS自適應(yīng)濾波試驗(yàn)[J]. 測(cè)繪科學(xué), 2003, 28(4):9-11.

[10] BAR-SHALOM Y, Li X, KIRUBARAJAN T. Estimation with applications to tracking and navigation: theory algorithms and software[M]. Wiley: Hoboken, 2001, 53 (6): 993-999.

[11] SCHUBERT R, RICHTER E, WANIELIK G. Comparison and evaluation of advanced motion models for vehicle tracking[C]// International Conference on Information Fusion. IEEE, 2008:1-6.

[12] ZHOU Z, LI B. GNSS windowing navigation with adaptively constructed dynamic model[J]. GPS Solutions, 2015, 19(1):37-48.

[13] LAN J, LI X R. Equivalent-model augmentation for variable-structure multiple-model estimation[J]. IEEE Transactions on Aerospace and Electronic Systems, 2013, 49(4): 2615-2630.

[14] JIN B, GUO J, He D, et al. Adaptive Kalman filtering based on optimal autoregressive predictive model[J]. GPS Solutions, 2017, 21(2):1-11.

[15] 齊泉林, 宋科璞, 朱雪耀. 基于卡爾曼濾波的AR模型無(wú)人自主空中加油預(yù)測(cè)制導(dǎo)策略[J]. 兵工自動(dòng)化, 2016, 35(10):55-59.

[責(zé)任編輯：王文福]

ACKF-ARalgorithmbasedUAVnavigation

SUN Rui, CHENG Qi, ZHANG Yucheng,WANG Guanyu

(College of Civil Aviation,Nanjing University of Aeronautics and Astronautics,Nanjing211106, China)

Filtering algorithms have been widely used in GPS/INS integration based UAV navigation. The motion model, as the key component in the filter prediction, is critical for the algorithm performance. However, the traditional motion model only uses a fixed motion mode for the prediction, which is not able to describe the UAV motion accurately. In this paper, an AR motion model based CKF algorithm for the UAV state estimation is proposed. By constructing the real-time dynamic updating AR model with sliding window, the adaptive filtering is designed for the UAV states estimation. The simulation results show that the positioning estimations from the designed CKF-AR based filtering algorithm are superior than the other traditional motion model based filtering results.

UAV; navigation;filtering algorithm;AR model;GPS

P237

A

1006-7949(2017)12-0041-04

2017-04-20

國(guó)家自然科學(xué)基金(41704022)；江蘇省自然科學(xué)基金(BK20170780)；中央高?；究蒲袠I(yè)務(wù)費(fèi)專項(xiàng)資金(NJ20160015； NS2017043)

孫 蕊(1984-)，女，博士.