北斗/GPS無人飛行器動態導航中改進自適應算法*

徐　偉，李　智，，王勇軍

（1.桂林電子科技大學 電子工程與自動化學院，廣西 桂林541004；2.桂林航天工業學院無人機遙測重點實驗室，廣西 桂林541004）

北斗/GPS無人飛行器動態導航中改進自適應算法*

徐偉1，李智1，2，王勇軍2

（1.桂林電子科技大學 電子工程與自動化學院，廣西 桂林541004；2.桂林航天工業學院無人機遙測重點實驗室，廣西 桂林541004）

北斗與GPS或其他類型導航設備可構成組合導航系統，為無人飛行器提供高精度、高可靠導航信息。在處理動態導航數據采用“當前”統計模型(CS模型)并利用標準Kalman濾波方法時，系統動態噪聲和觀測噪聲未知且時變，而且加速度上下限值不能自適應于未知運動規律的無人飛行器當前加速度，導致導航精度降低。為此，提出一種適應于飛行器運動狀態的改進自適應算法，并應用于北斗/GPS動態導航系統中。實驗仿真驗證了該算法能有效提高組合導航系統的精度和可靠性，并能更好地自適應于無人飛行器的機動特性。

北斗；GPS；組合導航；無人飛行器；自適應算法

0　引言

北斗是中國正在實施的自主發展、獨立運行的全球衛星導航系統，與目前應用最成熟的GPS或其他類型導航設備可構成組合導航系統，為工作在復雜環境中的飛行器提供可靠導航信息，而飛行器機動性未知，組合導航系統運動模型以及噪聲統計特性存在誤差。為了處理各種誤差，文獻[1]直接對 GPS接收機的輸出結果進行動態濾波；文獻[2]設計一種容錯型聯邦強跟蹤濾波器并應用于COMPASS、GPS、GLONASS組合導航系統中；文獻[3-4]對GPS、GLONASS、GALILEO組合導航系統提出自適應聯邦Kalman濾波以及雙重自適應算法。

標準 Kalman濾波是一種在正確的運動模型和噪聲統計特性下對導航系統動態數據進行實時濾波的有效方法，但在實際應用中受限。Sage-Husa算法利用噪聲估計器對未知且時變的噪聲統計特性進行估計[5]，而北斗/ GPS組合導航系統維數較高，計算量大，一旦Q(過程噪聲協方差陣)和R(量測噪聲協方差陣)分別失去半正定性和正定性會導致濾波發散。簡化Sage-Husa算法刪去q(過程噪聲均值)和 r(量測噪聲均值)的計算，認為 Q穩定，并改進R估計表達式[6]。而實際情況下的Q會變化，CS模型中加速度上下限取值過大或過小，均導致系統噪聲方差與實際不符，跟蹤精度低。為此，本文提出一種適應飛行器運動狀態的改進自適應算法，它實時估計R，且利用速度濾波、預測估計間的差值改進加速度協方差計算表達式，實現Q自適應于飛行器的機動特性，并提高跟蹤能力。通過實驗仿真與標準 Kalman、簡化Sage-Husa算法進行了比較和分析。

1　北斗/GPS組合導航系統模型的建立

1.1聯邦濾波器設計

視北斗、GPS為兩個相互獨立的聯邦成員，并分別設計自適應子濾波器，采用容錯性最好的無反饋重置聯邦濾波器對各子濾波器進行數據融合，見圖1。若北斗、GPS子濾波器局部狀態估計和相應估計協方差陣分別為(，P11)，(，P22)，且各局部估計互不相關，即 Pij=0 (i≠j)，則全局最優估計為：

圖1　北斗/GPS組合導航系統聯邦濾波器

北斗、GPS子濾波器數學模型的建立方法相同，不妨以GPS為例。子濾波器建立的狀態方程有12個狀態變量，觀測方程有由接收機輸出的3個觀測量，且每組狀態變量和對應觀測量相互獨立?？煞謩e對e，n，u(東北天坐標系)三個軸向的狀態變量以及對應觀測量單獨濾波，以e軸模型建立進行討論。

1.2北斗/GPS組合導航系統狀態方程建立

由于飛行器是在三維空間的運動，考慮飛行器的位置、速度、加速度以及GPS在e軸方向上的總誤差，采用CS模型[7]描述飛行器的運動。

狀態方程為：

式中，X=[xeveaeεe]T，xe、ve、ae分別為東向位置、速度和加速度；εe為e軸方向上的總誤差，等效一階馬爾可夫過程；Uk-1為加速度“當前”均值；Wk-1為系統噪聲；狀態轉移矩陣為 Φk，k-1，系統噪聲協方差陣為 Qk-1。將加速度一步預測作為“當前”加速度均值，則狀態一步預測方程為：

式中：

1.3北斗/GPS組合導航系統觀測方程建立

系統的觀測量包括接收機輸出的飛行器東向、北向和天向坐標分量 xe、xn、xu，各個方向的總誤差 εe、εn、εu以及量測噪聲(觀測方程中僅列出東向觀測量)[1]，實驗中經緯高度量測值單位為 m，并用量測值疊加高斯白噪聲。

觀測方程為：

式中，觀測值 Zk=E，量測噪聲 V=ωZe，量測矩陣 H= [1001]。

Rk為觀測噪聲協方差R的離散化形式R=。

2　算法描述

2.1簡化Sage-Husa算法

在線同時估計噪聲統計特性會使維數較高的組合導航系統實時性變差，認為R對濾波影響最為明顯，視Q 為定值[6]。

設線性離散時間系統為：

式(6)、式(7)中Xk是n維狀態向量，Zk是m維觀測序列，Φk，k-1是維狀態轉移矩陣，Hk是 m×n維觀測矩陣[8]。Wk-1和Vk是相互獨立的正態白噪聲序列。算法描述如下：

2.2改進自適應算法

因飛行器的飛行環境復雜，且其運動規律未知，系統干擾存在不穩定性，僅僅實時估計量測噪聲不能明顯獲得高精度導航信息，以及實時并準確估計出系統干擾。

可以通過CS模型中加速度協方差估計出系統干擾，但是在CS模型中，當飛行器弱機動時，其當前加速度較小，與較大的加速度上下限amax差值偏大，加速度協方差偏大，導致Q偏大?？梢?，加速度上下限不能自適應飛行器的機動特性，濾波器跟蹤能力較差。因此，本文對加速度協方差的計算進行改進。利用速度估計、速度濾波預測之間的差值改進了加速度協方差的計算表達式：

由式(9)可知，當飛行器弱機動或無機動時，其速度濾波 vk、預測估計 vk，k-1之差較小，加速度協方差較??；當飛行器強機動時，其速度濾波 vk、預測估計 vk，k-1之差值較大，加速度協方差較大[9]。因此，不管飛行器機動性如何，濾波器都能保持較好跟蹤。

在進行濾波過程中，不僅能實時估計R，而且根據飛行器的機動特性計算出加速度方差，從而較準確地估計出Q。

3　實驗過程以及算法仿真

3.1實驗過程

實驗所用的接收機為國內和芯星通公司的北斗/ GPS雙模模塊，將其安置在無人飛行器上，通過無線數傳模塊與上位機CDT軟件進行通信。讓無人飛行器保持一定高度在操場實時采集并存儲數據，采集頻率1 Hz，實驗時間350 s。飛行器的初始位置為緯度25.28°，經度110.33°，初始速度為 1 m/s，初始狀態協方差 P0=diag (402，1.02，0.12，402)，初始加速度為0。濾波參數如下：

3.2算法仿真分析

實驗仿真曲線、數據均來自MATLAB。

實驗1：改進自適應算法仿真

根據建立的北斗/GPS組合導航系統數學模型，利用標準Kalman濾波、簡化Sage-Husa算法和改進自適應算法對該模型經度方向誤差進行濾波估計，濾波效果如圖2和圖3所示。同時統計了兩種算法下的經緯度、速度均方根和均值誤差，如表1所示。對每種方法進行M=100次蒙特卡羅仿真，用均方根誤差衡量濾波精度，每一時刻的均方誤差為(dik為真值與觀測值的偏差)：

表1　兩種算法的濾波結果

從圖2、圖3的仿真曲線可知，在相同時刻，改進自適應算法得到的位置濾波效果顯然要優于另外兩種算法，因為飛行器采集數據過程中，其運動規律和噪聲未知。標準Kalman、簡化 Sage-Husa算法均未同時估計 Q和R，也沒有考慮加速度上下限對狀態估計精度的影響，改進自適應算法不僅估計R，而且根據飛行器的機動特性自適應調整Q，跟蹤精度提升。表1的統計結果也說明了這一點。

圖2　三種濾波方法的經度均方根誤差

圖3　三種濾波方法的經度方向誤差

實驗2：北斗/GPS組合導航系統計算仿真

利用無反饋重置聯邦濾波器對北斗、GPS信息進行融合，子濾波器均采用改進自適應算法。將北斗/GPS組合導航系統的導航信息與實際飛行數據作差，并分別與北斗、GPS子系統的仿真結果進行對比，仿真結果見圖4～圖6，相關統計量見表2。

圖4　GPS系統東向誤差

從圖4～圖6可以看出，GPS、北斗系統的位置誤差分別在 1 m、2 m以內，而北斗/GPS組合導航系統的位置誤差在1.2 m以內，其誤差處于GPS和北斗系統之間。表2的誤差統計結果表明北斗/GPS組合導航系統的位置誤差僅次于GPS。這是由于北斗、GPS同屬于衛星導航系統，系統性能、誤差特性相似，優劣相當，因此同種類型導航系統之間的信息融合效果不及于不同類型導航系統之間取長補短帶來的濾波效果。然而采用無反饋重置聯邦濾波器的北斗/GPS組合導航系統，容錯性能最好，且計算量少，滿足導航系統實時性高的要求。

圖5　北斗系統東向誤差

圖6　北斗/GPS組合導航系統誤差

表2　組合導航系統誤差統計

4　結束語

本文提出一種改進自適應算法，根據飛行器的運動狀態實時估計Q，并估計出R，同時融合北斗、GPS系統信息，通過與標準Kalman、簡化Sage-Husa算法的仿真比較，驗證改進自適應算法能明顯提高組合導航系統的導航精度。將改進自適應算法應用于北斗/GPS組合導航系統中，其定位精度雖與GPS相當，但是提高了系統容錯性。隨著中國北斗的不斷發展，與其他衛星導航系統以及不同類型導航設備(如捷聯慣導)之間的組合擁有更大的應用潛力。

[1]房建成，萬德鈞，吳秋平，等.GPS動態濾波的新方法[J].中國慣性技術學報，1997，5(2)：4-10.

[2]張輝，吳鵬，張國柱，等.基于容錯性聯邦強跟蹤濾波的多星座組合導航算法研究[J].全球定位系統，2012，37 (4)：12-18.

[3]吳玲，孫永榮，陳傳德.多星座組合導航自適應信息融合濾波算法[J].航天控制，2010，28(6)：38-4.

[4]孫永榮，吳玲，趙偉，等.多星座組合導航自適應聯合卡爾曼濾波算法研究[J].宇航學報，2009，30(5)：1879-1884.

[5]SAGE A P，HUSA G W.Adaptive filtering with unknown prior statistics[C].Proceedings of Joint Automatic Control Conference，1969：760-769.

[6]蘭義華，任浩征.一種基于“當前”模型的改進卡爾曼濾波算法[J].山東大學學報(工學版)，2012，42(5)：12-17.

[7]周宏仁，敬忠良，王培德.機動目標跟蹤[M].北京：國防工業出版社，1994.

[8]KALMAN R E.A new approach to linear filtering and prediction problems[J].ASME J Basic Eng，1960，82：35.

[9]沈云峰，朱海.簡化的 Sage-Husa自適應濾波算法在組合導航中的應用與仿真[J].青島大學學報，2001，16 (1)：44-47.

An improved adaptive algorithm of BD/GPS dynamic navigation for UAV

Xu Wei1，Li Zhi1，2，Wang Yongjun2
(1.School of Electronic Engineering and Automation，Guilin University of Electronic Technology，Guilin 541004，China；2.Key Laboratory of unmanned aerial vehicle telemetry，Guilin Institute of Aerospace，Guilin 541004，China)

Compass can constitute integrated navigation system which provides high-precision,highly reliable information for unmanned aerial vehicle(UAV)with GPS or other types of navigation equipments.When using current statistical model(CS model)and standard Kalman filter to deal with dynamic navigation data,dynamic noise and observation noise was unknown and time-varying, and limits of acceleration couldn't be adaptive to the current acceleration of UAV.All of these has resulted in low navigation accuracy.To solve the problem,the paper presents an improved adaptive algorithm which adapts to UAV's motion,and applies to Compass/GPS dynamic navigation system.The simulation shows that it can effectively improve the accuracy and reliability,and adapt to the maneuvering characteristics of UAV better.

Compass；GPS；integrated navigation；unmanned aerial vehicle；adaptive algorithm

V448

A

10.16157/j.issn.0258-7998.2015.10.015

國家自然科學基金(61361006)；廣西自動檢測技術與儀器重點實驗室基金項目(YQ14203)；廣西高校無人機遙測重點實驗室(WRJ2015ZR02)

2015-07-15)

徐偉(1989-)，男，碩士研究生，主要研究方向：組合導航、數據融合技術。

李智(1965-)，男，博士，教授，博士生導師，主要研究方向：自動測試總線與系統、現代測試理論與技術。

中文引用格式：徐偉，李智，王勇軍.北斗/GPS無人飛行器動態導航中改進自適應算法[J].電子技術應用，2015，41 (10)：58-61.

英文引用格式：Xu Wei，Li Zhi，Wang Yongjun.An improved adaptive algorithm of BD/GPS dynamic navigation for UAV[J]. Application of Electronic Technique，2015，41(10)：58-61.