北斗/GPS無(wú)人飛行器動(dòng)態(tài)導(dǎo)航中改進(jìn)自適應(yīng)算法*

徐　偉，李　智，，王勇軍

（1.桂林電子科技大學(xué) 電子工程與自動(dòng)化學(xué)院，廣西 桂林541004；2.桂林航天工業(yè)學(xué)院無(wú)人機(jī)遙測(cè)重點(diǎn)實(shí)驗(yàn)室，廣西 桂林541004）

北斗/GPS無(wú)人飛行器動(dòng)態(tài)導(dǎo)航中改進(jìn)自適應(yīng)算法*

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

（1.桂林電子科技大學(xué) 電子工程與自動(dòng)化學(xué)院，廣西 桂林541004；2.桂林航天工業(yè)學(xué)院無(wú)人機(jī)遙測(cè)重點(diǎn)實(shí)驗(yàn)室，廣西 桂林541004）

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

北斗；GPS；組合導(dǎo)航；無(wú)人飛行器；自適應(yīng)算法

0　引言

北斗是中國(guó)正在實(shí)施的自主發(fā)展、獨(dú)立運(yùn)行的全球衛(wèi)星導(dǎo)航系統(tǒng)，與目前應(yīng)用最成熟的GPS或其他類(lèi)型導(dǎo)航設(shè)備可構(gòu)成組合導(dǎo)航系統(tǒng)，為工作在復(fù)雜環(huán)境中的飛行器提供可靠導(dǎo)航信息，而飛行器機(jī)動(dòng)性未知，組合導(dǎo)航系統(tǒng)運(yùn)動(dòng)模型以及噪聲統(tǒng)計(jì)特性存在誤差。為了處理各種誤差，文獻(xiàn)[1]直接對(duì) GPS接收機(jī)的輸出結(jié)果進(jìn)行動(dòng)態(tài)濾波；文獻(xiàn)[2]設(shè)計(jì)一種容錯(cuò)型聯(lián)邦強(qiáng)跟蹤濾波器并應(yīng)用于COMPASS、GPS、GLONASS組合導(dǎo)航系統(tǒng)中；文獻(xiàn)[3-4]對(duì)GPS、GLONASS、GALILEO組合導(dǎo)航系統(tǒng)提出自適應(yīng)聯(lián)邦Kalman濾波以及雙重自適應(yīng)算法。

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

1　北斗/GPS組合導(dǎo)航系統(tǒng)模型的建立

1.1聯(lián)邦濾波器設(shè)計(jì)

視北斗、GPS為兩個(gè)相互獨(dú)立的聯(lián)邦成員，并分別設(shè)計(jì)自適應(yīng)子濾波器，采用容錯(cuò)性最好的無(wú)反饋重置聯(lián)邦濾波器對(duì)各子濾波器進(jìn)行數(shù)據(jù)融合，見(jiàn)圖1。若北斗、GPS子濾波器局部狀態(tài)估計(jì)和相應(yīng)估計(jì)協(xié)方差陣分別為(，P11)，(，P22)，且各局部估計(jì)互不相關(guān)，即 Pij=0 (i≠j)，則全局最優(yōu)估計(jì)為：

圖1　北斗/GPS組合導(dǎo)航系統(tǒng)聯(lián)邦濾波器

北斗、GPS子濾波器數(shù)學(xué)模型的建立方法相同，不妨以GPS為例。子濾波器建立的狀態(tài)方程有12個(gè)狀態(tài)變量，觀(guān)測(cè)方程有由接收機(jī)輸出的3個(gè)觀(guān)測(cè)量，且每組狀態(tài)變量和對(duì)應(yīng)觀(guān)測(cè)量相互獨(dú)立?？煞謩e對(duì)e，n，u(東北天坐標(biāo)系)三個(gè)軸向的狀態(tài)變量以及對(duì)應(yīng)觀(guān)測(cè)量單獨(dú)濾波，以e軸模型建立進(jìn)行討論。

1.2北斗/GPS組合導(dǎo)航系統(tǒng)狀態(tài)方程建立

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

狀態(tài)方程為：

式中，X=[xeveaeεe]T，xe、ve、ae分別為東向位置、速度和加速度；εe為e軸方向上的總誤差，等效一階馬爾可夫過(guò)程；Uk-1為加速度“當(dāng)前”均值；Wk-1為系統(tǒng)噪聲；狀態(tài)轉(zhuǎn)移矩陣為 Φk，k-1，系統(tǒng)噪聲協(xié)方差陣為 Qk-1。將加速度一步預(yù)測(cè)作為“當(dāng)前”加速度均值，則狀態(tài)一步預(yù)測(cè)方程為：

式中：

1.3北斗/GPS組合導(dǎo)航系統(tǒng)觀(guān)測(cè)方程建立

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

觀(guān)測(cè)方程為：

式中，觀(guān)測(cè)值 Zk=E，量測(cè)噪聲 V=ωZe，量測(cè)矩陣 H= [1001]。

Rk為觀(guān)測(cè)噪聲協(xié)方差R的離散化形式R=。

2　算法描述

2.1簡(jiǎn)化Sage-Husa算法

在線(xiàn)同時(shí)估計(jì)噪聲統(tǒng)計(jì)特性會(huì)使維數(shù)較高的組合導(dǎo)航系統(tǒng)實(shí)時(shí)性變差，認(rèn)為R對(duì)濾波影響最為明顯，視Q 為定值[6]。

設(shè)線(xiàn)性離散時(shí)間系統(tǒng)為：

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

2.2改進(jìn)自適應(yīng)算法

因飛行器的飛行環(huán)境復(fù)雜，且其運(yùn)動(dòng)規(guī)律未知，系統(tǒng)干擾存在不穩(wěn)定性，僅僅實(shí)時(shí)估計(jì)量測(cè)噪聲不能明顯獲得高精度導(dǎo)航信息，以及實(shí)時(shí)并準(zhǔn)確估計(jì)出系統(tǒng)干擾。

可以通過(guò)CS模型中加速度協(xié)方差估計(jì)出系統(tǒng)干擾，但是在CS模型中，當(dāng)飛行器弱機(jī)動(dòng)時(shí)，其當(dāng)前加速度較小，與較大的加速度上下限amax差值偏大，加速度協(xié)方差偏大，導(dǎo)致Q偏大?？梢?jiàn)，加速度上下限不能自適應(yīng)飛行器的機(jī)動(dòng)特性，濾波器跟蹤能力較差。因此，本文對(duì)加速度協(xié)方差的計(jì)算進(jìn)行改進(jìn)。利用速度估計(jì)、速度濾波預(yù)測(cè)之間的差值改進(jìn)了加速度協(xié)方差的計(jì)算表達(dá)式：

由式(9)可知，當(dāng)飛行器弱機(jī)動(dòng)或無(wú)機(jī)動(dòng)時(shí)，其速度濾波 vk、預(yù)測(cè)估計(jì) vk，k-1之差較小，加速度協(xié)方差較小；當(dāng)飛行器強(qiáng)機(jī)動(dòng)時(shí)，其速度濾波 vk、預(yù)測(cè)估計(jì) vk，k-1之差值較大，加速度協(xié)方差較大[9]。因此，不管飛行器機(jī)動(dòng)性如何，濾波器都能保持較好跟蹤。

在進(jìn)行濾波過(guò)程中，不僅能實(shí)時(shí)估計(jì)R，而且根據(jù)飛行器的機(jī)動(dòng)特性計(jì)算出加速度方差，從而較準(zhǔn)確地估計(jì)出Q。

3　實(shí)驗(yàn)過(guò)程以及算法仿真

3.1實(shí)驗(yàn)過(guò)程

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

3.2算法仿真分析

實(shí)驗(yàn)仿真曲線(xiàn)、數(shù)據(jù)均來(lái)自MATLAB。

實(shí)驗(yàn)1：改進(jìn)自適應(yīng)算法仿真

根據(jù)建立的北斗/GPS組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型，利用標(biāo)準(zhǔn)Kalman濾波、簡(jiǎn)化Sage-Husa算法和改進(jìn)自適應(yīng)算法對(duì)該模型經(jīng)度方向誤差進(jìn)行濾波估計(jì)，濾波效果如圖2和圖3所示。同時(shí)統(tǒng)計(jì)了兩種算法下的經(jīng)緯度、速度均方根和均值誤差，如表1所示。對(duì)每種方法進(jìn)行M=100次蒙特卡羅仿真，用均方根誤差衡量濾波精度，每一時(shí)刻的均方誤差為(dik為真值與觀(guān)測(cè)值的偏差)：

表1　兩種算法的濾波結(jié)果

從圖2、圖3的仿真曲線(xiàn)可知，在相同時(shí)刻，改進(jìn)自適應(yīng)算法得到的位置濾波效果顯然要優(yōu)于另外兩種算法，因?yàn)轱w行器采集數(shù)據(jù)過(guò)程中，其運(yùn)動(dòng)規(guī)律和噪聲未知。標(biāo)準(zhǔn)Kalman、簡(jiǎn)化 Sage-Husa算法均未同時(shí)估計(jì) Q和R，也沒(méi)有考慮加速度上下限對(duì)狀態(tài)估計(jì)精度的影響，改進(jìn)自適應(yīng)算法不僅估計(jì)R，而且根據(jù)飛行器的機(jī)動(dòng)特性自適應(yīng)調(diào)整Q，跟蹤精度提升。表1的統(tǒng)計(jì)結(jié)果也說(shuō)明了這一點(diǎn)。

圖2　三種濾波方法的經(jīng)度均方根誤差

圖3　三種濾波方法的經(jīng)度方向誤差

實(shí)驗(yàn)2：北斗/GPS組合導(dǎo)航系統(tǒng)計(jì)算仿真

利用無(wú)反饋重置聯(lián)邦濾波器對(duì)北斗、GPS信息進(jìn)行融合，子濾波器均采用改進(jìn)自適應(yīng)算法。將北斗/GPS組合導(dǎo)航系統(tǒng)的導(dǎo)航信息與實(shí)際飛行數(shù)據(jù)作差，并分別與北斗、GPS子系統(tǒng)的仿真結(jié)果進(jìn)行對(duì)比，仿真結(jié)果見(jiàn)圖4～圖6，相關(guān)統(tǒng)計(jì)量見(jiàn)表2。

圖4　GPS系統(tǒng)東向誤差

從圖4～圖6可以看出，GPS、北斗系統(tǒng)的位置誤差分別在 1 m、2 m以?xún)?nèi)，而北斗/GPS組合導(dǎo)航系統(tǒng)的位置誤差在1.2 m以?xún)?nèi)，其誤差處于GPS和北斗系統(tǒng)之間。表2的誤差統(tǒng)計(jì)結(jié)果表明北斗/GPS組合導(dǎo)航系統(tǒng)的位置誤差僅次于GPS。這是由于北斗、GPS同屬于衛(wèi)星導(dǎo)航系統(tǒng)，系統(tǒng)性能、誤差特性相似，優(yōu)劣相當(dāng)，因此同種類(lèi)型導(dǎo)航系統(tǒng)之間的信息融合效果不及于不同類(lèi)型導(dǎo)航系統(tǒng)之間取長(zhǎng)補(bǔ)短帶來(lái)的濾波效果。然而采用無(wú)反饋重置聯(lián)邦濾波器的北斗/GPS組合導(dǎo)航系統(tǒng)，容錯(cuò)性能最好，且計(jì)算量少，滿(mǎn)足導(dǎo)航系統(tǒng)實(shí)時(shí)性高的要求。

圖5　北斗系統(tǒng)東向誤差

圖6　北斗/GPS組合導(dǎo)航系統(tǒng)誤差

表2　組合導(dǎo)航系統(tǒng)誤差統(tǒng)計(jì)

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

本文提出一種改進(jìn)自適應(yīng)算法，根據(jù)飛行器的運(yùn)動(dòng)狀態(tài)實(shí)時(shí)估計(jì)Q，并估計(jì)出R，同時(shí)融合北斗、GPS系統(tǒng)信息，通過(guò)與標(biāo)準(zhǔn)Kalman、簡(jiǎn)化Sage-Husa算法的仿真比較，驗(yàn)證改進(jìn)自適應(yīng)算法能明顯提高組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。將改進(jìn)自適應(yīng)算法應(yīng)用于北斗/GPS組合導(dǎo)航系統(tǒng)中，其定位精度雖與GPS相當(dāng)，但是提高了系統(tǒng)容錯(cuò)性。隨著中國(guó)北斗的不斷發(fā)展，與其他衛(wèi)星導(dǎo)航系統(tǒng)以及不同類(lèi)型導(dǎo)航設(shè)備(如捷聯(lián)慣導(dǎo))之間的組合擁有更大的應(yīng)用潛力。

[1]房建成，萬(wàn)德鈞，吳秋平，等.GPS動(dòng)態(tài)濾波的新方法[J].中國(guó)慣性技術(shù)學(xué)報(bào)，1997，5(2)：4-10.

[2]張輝，吳鵬，張國(guó)柱，等.基于容錯(cuò)性聯(lián)邦強(qiáng)跟蹤濾波的多星座組合導(dǎo)航算法研究[J].全球定位系統(tǒng)，2012，37 (4)：12-18.

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

[4]孫永榮，吳玲，趙偉，等.多星座組合導(dǎo)航自適應(yīng)聯(lián)合卡爾曼濾波算法研究[J].宇航學(xué)報(bào)，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]蘭義華，任浩征.一種基于“當(dāng)前”模型的改進(jìn)卡爾曼濾波算法[J].山東大學(xué)學(xué)報(bào)(工學(xué)版)，2012，42(5)：12-17.

[7]周宏仁，敬忠良，王培德.機(jī)動(dòng)目標(biāo)跟蹤[M].北京：國(guó)防工業(yè)出版社，1994.

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

[9]沈云峰，朱海.簡(jiǎn)化的 Sage-Husa自適應(yīng)濾波算法在組合導(dǎo)航中的應(yīng)用與仿真[J].青島大學(xué)學(xué)報(bào)，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

國(guó)家自然科學(xué)基金(61361006)；廣西自動(dòng)檢測(cè)技術(shù)與儀器重點(diǎn)實(shí)驗(yàn)室基金項(xiàng)目(YQ14203)；廣西高校無(wú)人機(jī)遙測(cè)重點(diǎn)實(shí)驗(yàn)室(WRJ2015ZR02)

2015-07-15)

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

李智(1965-)，男，博士，教授，博士生導(dǎo)師，主要研究方向：自動(dòng)測(cè)試總線(xiàn)與系統(tǒng)、現(xiàn)代測(cè)試?yán)碚撆c技術(shù)。

中文引用格式：徐偉，李智，王勇軍.北斗/GPS無(wú)人飛行器動(dòng)態(tài)導(dǎo)航中改進(jìn)自適應(yīng)算法[J].電子技術(shù)應(yīng)用，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.