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

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

引用著錄：孫蕊，程琦，張羽成，等.一種基于AR運動模型CKF算法的無人機導航[J].測繪工程，2017，26(12)：41-44，49.

DOI:10.19349/j.cnki.issn1006-7949.2017.12.008

一種基于AR運動模型CKF算法的無人機導航

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

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

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

無人機；導航；濾波算法；自回歸模型；GPS

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

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

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

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

1 AR運動模型的構建

傳統(tǒng)的離散時間差分模型可以描繪物體運動，但它們一旦建立就不會變化，不能自適應地隨著過程噪聲和測量噪聲強度去調整，自然會在一定程度上導致性能降低。針對該問題，本文用AR模型來描述無人機的運動狀態(tài)。AR 模型的原理是：通過對一組已有的歷史數(shù)據(jù)進行線性組合，給出關心時刻的預測信息[15],即根據(jù)p個歷史數(shù)據(jù)信息，推出第p時刻前面或后面的數(shù)據(jù)。其本質與插值法類似，兩者都是增加有效數(shù)據(jù)，不同的是AR模型是由p點遞推，而插值是由兩點(或少數(shù)幾點)去推導多點，所以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)中，一般動態(tài)模型是離散時間運動模型，形式如下：

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

(2)

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

k時刻觀測方程如下：

zk=Hxk+vk.

(3)

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

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

(4)

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

(5)

Ek+1=φEkxEk+aEk.

(6)

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

1)獲得一組采樣點及其對應權值。

n.

(7)

每個粒子對應權值：

(8)

式中：n是維度。

2)計算容積點集的一步預測，i=1，2

(9)

3)計算系統(tǒng)狀態(tài)量的一步預測及協(xié)方差矩陣，一步預測由容積點集的預測加權求和得到。

(10)

(11)

4)根據(jù)一步預測值，對P(k+1|k)做Cholesky分解，產生新的容積點集。

(12)

(13)

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

(14)

6)由5)得到容積點集的預測觀測值，通過加權求和得到系統(tǒng)預測值、k+1時刻量測誤差協(xié)方差陣及k+1時刻一步預測互相關協(xié)方差陣。

(15)

(16)

(17)

7)計算Kalman增益矩陣。

(18)

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

(19)

(20)

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

3 實驗和分析

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

圖1 飛行軌跡

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

(21)

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

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

圖2 各個方向濾波結果對比

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

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

2)在整個仿真運動過程中，無人機均是以變化的速度及變化的加速度飛行，這表明CKF-AR算法對無人機的運動類型沒有限制，在無人機導航定位中可以適應無人機多種類型的飛行行為。

4 結束語

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

[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導航中的抗差自適應Kalman濾波算法[J]. 測繪科學, 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動態(tài)定位自適應卡爾曼濾波算法研究[J]. 測繪工程, 2012, 21(4):29-32.

[9] 楊元喜, 唐穎哲, 李慶田,等. 用于GIS道路信息修測的動態(tài)GPS自適應濾波試驗[J]. 測繪科學, 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模型無人自主空中加油預測制導策略[J]. 兵工自動化, 2016, 35(10):55-59.

[責任編輯：王文福]

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

國家自然科學基金(41704022)；江蘇省自然科學基金(BK20170780)；中央高?；究蒲袠I(yè)務費專項資金(NJ20160015； NS2017043)

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