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é)方差陣分別為

　　北斗、GPS子濾波器數(shù)學(xué)模型的建立方法相同，不妨以GPS為例。子濾波器建立的狀態(tài)方程有12個(gè)狀態(tài)變量，觀測(cè)方程有由接收機(jī)輸出的3個(gè)觀測(cè)量，且每組狀態(tài)變量和對(duì)應(yīng)觀測(cè)量相互獨(dú)立。可分別對(duì)e，n，u(東北天坐標(biāo)系)三個(gè)軸向的狀態(tài)變量以及對(duì)應(yīng)觀測(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=[xe  ve  ae  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)觀測(cè)方程建立

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

　　觀測(cè)方程為：

　　2 算法描述

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

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

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

　　式(6)、式(7)中Xk是n維狀態(tài)向量，Zk是m維觀測(cè)序列，?椎k，k-1是維狀態(tài)轉(zhuǎn)移矩陣，Hk是m×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)前加速度k較小，與較大的加速度上下限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ú)線數(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)仿真曲線、數(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為真值與觀測(cè)值的偏差)：

　　從圖2、圖3的仿真曲線可知，在相同時(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)。

　　實(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～圖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ì)算量少，滿足導(dǎo)航系統(tǒng)實(shí)時(shí)性高的要求。

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)用潛力。

參考文獻(xiàn)

　　[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