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

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

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

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

DOI:10.19349/j.cnki.issn1006-7949.2017.12.008

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

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

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

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

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

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

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

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

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

1 AR運動模型的構建

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

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

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

(1)

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

在非線性系統中，一般動態模型是離散時間運動模型，形式如下：

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

(2)

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

k時刻觀測方程如下：

zk=Hxk+vk.

(3)

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

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

(4)

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

(5)

Ek+1=φEkxEk+aEk.

(6)

式中：φEk是Ek的回歸系數；xEk是前p個歷史數據；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)計算系統狀態量的一步預測及協方差矩陣，一步預測由容積點集的預測加權求和得到。

(10)

(11)

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

(12)

(13)

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

(14)

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

(15)

(16)

(17)

7)計算Kalman增益矩陣。

(18)

8)最后，計算系統的狀態更新和協方差更新。

(19)

(20)

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

3 實驗和分析

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

圖1 飛行軌跡

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

(21)

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

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

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

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

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

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

4 結束語

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

[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動態定位自適應卡爾曼濾波算法研究[J]. 測繪工程, 2012, 21(4):29-32.

[9] 楊元喜, 唐穎哲, 李慶田,等. 用于GIS道路信息修測的動態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)；中央高校基本科研業務費專項資金(NJ20160015； NS2017043)

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