寧凱文，張小躍，張春熹

(北京航空航天大學(xué)儀器科學(xué)與光電工程學(xué)院，北京 100191)

0　引言

慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)是利用陀螺儀、加速度計(jì)等慣性敏感元件及初始位置來(lái)確定載體的位置、速度和姿態(tài)，它不依賴(lài)于任何外部信息，但是其導(dǎo)航誤差會(huì)隨著導(dǎo)航時(shí)間而迅速累積，因此很難長(zhǎng)時(shí)間單獨(dú)使用[1]。全球定位系統(tǒng)(Global Positioning System，GPS)能夠提供全球性、全天候的導(dǎo)航定位授時(shí)服務(wù)，并且精度高、誤差不隨時(shí)間累積，但是GPS信號(hào)容易受到干擾，且導(dǎo)航結(jié)果的數(shù)據(jù)更新率低[2]。因此，INS和GPS具有良好的互補(bǔ)特性，INS/GPS組合導(dǎo)航系統(tǒng)現(xiàn)已成為機(jī)載導(dǎo)航的一種主要模式。

針對(duì)實(shí)際工程應(yīng)用中有色噪聲模型系數(shù)難以精確獲取的問(wèn)題，本文提出了一種利用濾波殘差對(duì)有色噪聲進(jìn)行建模的方法。將濾波殘差作為有色噪聲的樣本觀測(cè)值，使用濾波所得的多個(gè)歷元的殘差序列對(duì)有色噪聲進(jìn)行擬合與修正，建立有色噪聲自回歸模型，在獲得相關(guān)系數(shù)的基礎(chǔ)上進(jìn)行濾波處理，從而減小有色噪聲對(duì)濾波結(jié)果的影響。最后，采用轉(zhuǎn)臺(tái)試驗(yàn)驗(yàn)證了本文提出的方法，并與未考慮有色噪聲的傳統(tǒng)的Kalman濾波方法進(jìn)行了比較。

1　有色噪聲對(duì)組合導(dǎo)航濾波估值的影響

Kalman濾波是一種離散線性遞推的最優(yōu)估計(jì)算法，其動(dòng)態(tài)模型為：

(1)

其中，Xk、Xk-1分別為k、k-1 時(shí)刻的狀態(tài)矢量；Φk,k-1為狀態(tài)轉(zhuǎn)移矩陣；Zk為量測(cè)矢量；Hk為量測(cè)陣；wk-1為狀態(tài)噪聲，vk為量測(cè)噪聲矩陣。

離散化的Kalman濾波狀態(tài)估計(jì)方程為：

(2)

因此，可以計(jì)算出有色噪聲的綜合影響函數(shù)值，并用以修正狀態(tài)參數(shù)估值。這里，分別計(jì)算有色狀態(tài)噪聲和有色量測(cè)噪聲對(duì)狀態(tài)參數(shù)估值的影響。

設(shè)有色狀態(tài)噪聲為1階AR模型，則狀態(tài)噪聲wk的表達(dá)式為：

wk=Ωk,k-1wk-1+εk-1

(3)

Ωk,k-1為相關(guān)系數(shù)矩陣，此時(shí)的狀態(tài)參數(shù)預(yù)報(bào)值為：

(4)

根據(jù)最小二乘法解得狀態(tài)參數(shù)估計(jì)值為[7-8]：

(5)

有色狀態(tài)噪聲對(duì)狀態(tài)參數(shù)估計(jì)值的影響函數(shù)為：

(6)

同理，設(shè)有色量測(cè)噪聲為1階AR模型，則量測(cè)噪聲vk的表達(dá)式為：

vk=γk,k-1vk-1+δk-1

(7)

γk,k-1為相關(guān)系數(shù)矩陣，有色量測(cè)噪聲對(duì)狀態(tài)參數(shù)的影響函數(shù)為：

(8)

綜上所述，有色噪聲對(duì)導(dǎo)航濾波估值的影響函數(shù)為：

(9)

因此，當(dāng)存在有色噪聲時(shí)，會(huì)使參數(shù)估值嚴(yán)重偏離理論真值。在實(shí)際導(dǎo)航應(yīng)用中有必要考慮有色噪聲對(duì)組合導(dǎo)航的影響，針對(duì)有色噪聲建立模型以減小其對(duì)參數(shù)估值的影響。

2　INS/GPS組合系統(tǒng)與有色噪聲模型建立

2.1　機(jī)載INS/GPS組合導(dǎo)航模型

本文將陀螺和加速度計(jì)的標(biāo)度誤差和非正交誤差放入系統(tǒng)狀態(tài)誤差模型得到33維的誤差狀態(tài)向量，然后結(jié)合典型機(jī)載運(yùn)動(dòng)軌跡分析了標(biāo)度誤差和非正交誤差的可觀測(cè)性，最終得到優(yōu)化后的機(jī)載INS/GPS組合導(dǎo)航模型。

組合系統(tǒng)狀態(tài)誤差模型可以表示成如下形式：

(10)

式中，X(t)為33維的系統(tǒng)誤差狀態(tài)向量：

φUaBxaByaBzgBxgBygBzgSFx

gSFygSFzgMAxygMAxzgMAyxgMAyz

gMAzxgMAzyaSFxaSFyaSFzaMAxy

aMAxzaMAyxaMAyzaMAzxaMAzy]T

本文將飛機(jī)的典型機(jī)動(dòng)過(guò)程分為3種:直線飛行、上升飛行和轉(zhuǎn)彎飛行，飛機(jī)飛行階段如表1所示。

表1　飛機(jī)飛行軌跡

結(jié)合上述典型飛行軌跡，本文采用數(shù)學(xué)仿真的方法，根據(jù)誤差狀態(tài)量的協(xié)方差來(lái)分析陀螺與加速度計(jì)的標(biāo)度誤差、非正交誤差的可觀測(cè)性[9]。分析結(jié)果為誤差狀態(tài)向量中的gSFz、gMAxy、gMAzx、gMAzy、aSFx、aMAxz、aMAyx、aMAyz、aMAzy不可觀測(cè)。刪除不可觀測(cè)的誤差狀態(tài)量，組合系統(tǒng)誤差狀態(tài)向量由33維降至24維，優(yōu)化后的系統(tǒng)誤差狀態(tài)向量為：

φUaBxaByaBzgBxgBygBz

gSFygSFzgMAxygMAzxaSFxaSFyaMAxzaMAyxaMAyz]T

W(t)為系統(tǒng)噪聲：

W(t)=[01×3aWxaWyaWzgWxgWygWz01×15]T

其中，aWx、aWy、aWz為加速度計(jì)輸出噪聲，gWx、gWy、gWz為陀螺輸出噪聲。

G(t)為系統(tǒng)噪聲驅(qū)動(dòng)陣：

09×309×3]

該系統(tǒng)采用INS解算的位置和速度與GPS測(cè)量到的位置和速度之差作為量測(cè)信息，組合系統(tǒng)的量測(cè)方程表示為：

(11)

Hv=[03×3I303×18]，v為量測(cè)噪聲[10]。

2.2　基于濾波殘差建模的有色噪聲處理方法

根據(jù)上述分析可知，有色噪聲對(duì)組合導(dǎo)航估值的影響不能忽略，且濾波器預(yù)報(bào)值之間的相關(guān)性會(huì)在殘差中得到充分體現(xiàn)。因此，可以將狀態(tài)殘差和量測(cè)殘差分別作為有色狀態(tài)噪聲和有色量測(cè)噪聲的樣本觀測(cè)值，建立相應(yīng)的有色噪聲函數(shù)模型[11-12]，并在濾波過(guò)程中自適應(yīng)地對(duì)有色噪聲進(jìn)行補(bǔ)償，從而控制其對(duì)狀態(tài)參數(shù)估值的影響。

根據(jù)系統(tǒng)誤差方程可得系統(tǒng)狀態(tài)殘差向量和量測(cè)殘差向量,分別為：

(12)

設(shè)k時(shí)刻的狀態(tài)殘差vXk為：

vXk=α1vXk-1+α2vXk-2+…+αmvXk-m+ek

(13)

將式(13)寫(xiě)成矩陣的形式為：

e=Bα-vXk

(14)

式中，

其中，vXk為由狀態(tài)殘差構(gòu)成的偽觀測(cè)向量，e為誤差向量，α為有色狀態(tài)噪聲系數(shù)矩陣[13-14]。

將式(14)兩邊轉(zhuǎn)置可得：

(15)

(16)

(17)

同理，可得量測(cè)誤差向量的矩陣的形式為：

f=Cβ-vk

(18)

其中，

其中，vk為由殘差序列構(gòu)成的偽觀測(cè)向量，C

為誤差向量，β為有色狀態(tài)噪聲模型系數(shù)矩陣，所以有色量測(cè)噪聲的預(yù)報(bào)值為：

(19)

因此，改進(jìn)后的狀態(tài)參數(shù)預(yù)報(bào)值和量測(cè)值分別為：

(20)

由上述可知，可以得到有色噪聲模型修正后的狀態(tài)參數(shù)估值為[15]：

(21)

式(21)給出了顧及有色噪聲的狀態(tài)參數(shù)估值，本節(jié)建立了有色噪聲自回歸模型，通過(guò)濾波所得的多個(gè)歷元的殘差序列獲取模型參數(shù)，得出有色噪聲的預(yù)報(bào)值并分別將其補(bǔ)償?shù)浇M合模型中，從而減小有色噪聲對(duì)組合導(dǎo)航系統(tǒng)的影響。

3　實(shí)驗(yàn)驗(yàn)證與分析

本文采用轉(zhuǎn)臺(tái)試驗(yàn)驗(yàn)證上述提出的顧及有色噪聲的機(jī)載INS/GPS組合導(dǎo)航方法。轉(zhuǎn)臺(tái)試驗(yàn)設(shè)備包括INS/GPS組合導(dǎo)航系統(tǒng)、轉(zhuǎn)臺(tái)、雙路可跟蹤直流電源(0V～30V，0A～3A)、數(shù)據(jù)采集計(jì)算機(jī)等,INS/GPS組合導(dǎo)航系統(tǒng)由光纖捷聯(lián)慣組和GPS接收機(jī)構(gòu)成。主要實(shí)驗(yàn)設(shè)備的安裝情況如圖1和圖2所示。

整個(gè)試驗(yàn)時(shí)間為700s，前500s系統(tǒng)進(jìn)行INS/GPS組合導(dǎo)航，后200s系統(tǒng)進(jìn)行純慣導(dǎo)解算，轉(zhuǎn)臺(tái)的轉(zhuǎn)動(dòng)情況與表1飛機(jī)飛行軌跡一致。在組合導(dǎo)航過(guò)程中，可以通過(guò)比較慣性傳感器漂移的殘差大小直接得出有色噪聲擬合與修正的效果。另外，純慣導(dǎo)階段的導(dǎo)航精度嚴(yán)重依賴(lài)于組合導(dǎo)航階段慣性傳感器誤差的補(bǔ)償精度[16]。因此，通過(guò)比較純慣導(dǎo)階段的導(dǎo)航精度也可以間接評(píng)估組合導(dǎo)航階段對(duì)有色噪聲的修正情況。

為了更加直觀地觀察有色噪聲補(bǔ)償前后殘差的變化情況，以陀螺為例，本文選取了組合導(dǎo)航過(guò)程中50s的殘差估計(jì)情況，圖3～圖5分別為有色噪聲補(bǔ)償前后組合導(dǎo)航階段陀螺漂移的殘差估計(jì)量。圖6～圖8分別給出了后300s本文提出的有色噪聲處理方法與未進(jìn)行有色噪聲處理的位置誤差比較。

表2為組合導(dǎo)航過(guò)程中有色噪聲補(bǔ)償前后陀螺殘差的均方根值。從結(jié)果可以看出，本文提出的顧及有色噪聲的組合方法與未進(jìn)行有色噪聲處理的方法相比，慣性傳感器漂移的殘差減小了約1/2，有效控制了有色噪聲對(duì)系統(tǒng)的影響。

表2　陀螺有色噪聲補(bǔ)償前后殘差的RMS比較

700s時(shí)，傳統(tǒng)Kalman濾波處理后的位置誤差分別為592.2m、444.8m和176.6m，本文提出的顧及有色噪聲的組合方法的位置誤差分別為507.81m、393.65m和155.6m。系統(tǒng)的導(dǎo)航精度分別提高了14.3%、11.5%和11.9%。因此，本文提出的基于殘差序列處理有色噪聲的組合導(dǎo)航方法能得到更高的導(dǎo)航精度，驗(yàn)證了該方法的有效性。

4　結(jié)論

針對(duì)機(jī)載組合導(dǎo)航的應(yīng)用，本文提出了一種利用殘差序列對(duì)系統(tǒng)有色噪聲和量測(cè)有色噪聲進(jìn)行擬合與修正的方法。首先分析了狀態(tài)有色噪聲和量測(cè)有色噪聲對(duì)組合導(dǎo)航濾波解的影響，在此基礎(chǔ)上，將殘差作為有色噪聲的樣本觀測(cè)值，利用殘差序列對(duì)有色噪聲進(jìn)行建模，然后將其應(yīng)用到機(jī)載組合導(dǎo)航系統(tǒng)當(dāng)中，得到有色噪聲模型修正后的狀態(tài)參數(shù)估值。最后設(shè)計(jì)了轉(zhuǎn)臺(tái)試驗(yàn)，驗(yàn)證結(jié)果表明本文提出的方法解決了傳統(tǒng)Kalman濾波無(wú)法處理有色噪聲的問(wèn)題，能有效減弱有色噪聲對(duì)導(dǎo)航結(jié)果的影響，并且當(dāng)GPS失效時(shí)，本文提出的方法能得到更高的導(dǎo)航精度。

[1]秦永元. 慣性導(dǎo)航(第二版)[M]. 北京:科學(xué)出版社, 2014.

QIN Yong-yuan. Inertial navigation(2rd) [M]. Beijing: Science Press, 2014.

[2]黃欣, 許建新, 孔雪博,等. 基于嵌入式平臺(tái)的MEMS/GPS組合導(dǎo)航系統(tǒng)實(shí)現(xiàn)研究[J]. 導(dǎo)航與控制, 2017,16(1):11-16+43.

HUANG Xin, XU Jian-xin, KONG Xue-bo, et al. Physical implementation of MEMS/GPS integrated navigation based on the embedded platform [J]. Navigation and Control, 2017,16(1):11-16+43.

[3]Liang Y Q, Jia Y M. A nonlinear quaternion-based fault-tolerant SINS/GNSS integrated navigation method for autonomous UAVs [J]. Aerospace Science & Technology, 2015, 40:191-199.

[4]牛強(qiáng)軍, 張潮, 蘇登輝. SINS/GPS 組合導(dǎo)航卡爾曼濾波算法研究[J]. 兵工自動(dòng)化, 2015, 34(5):38-41.

NIU Qiang-jun, ZHANG Chao, SU Deng-hui. Research on SINS/GPS integrated navigation Kalman filter algorithm [J].Ordnance Industry Automation, 2015, 34(5): 38-41.

[5]周小剛, 劉潔瑜, 汪立新, 等.視覺(jué)/慣性導(dǎo)航抗有色噪聲濾波算法設(shè)計(jì)與仿真[J]. 現(xiàn)代防御技術(shù), 2015, 43(2): 199-202+209.

ZHOU Xiao-gang, LIU Jie-yu, WANG Li-xin, et al. Design of anti-colored noise filter for vision/INS integrated navigation system and simulation experiment [J]. Modern Defence Technology, 2015, 43(2): 199-202+209.

[6]賴(lài)際舟, 柳敏, 李志敏, 等. 基于有色噪聲自回歸建模的慣性/衛(wèi)星交互多模型濾波導(dǎo)航算法[J]. 導(dǎo)航定位與授時(shí), 2015, 2(6): 19-24.

LAI Ji-zhou, LIU Min, LI Zhi-min, et al. Interacting multiple model filter algorithm of the inertial/GPS integrated system based on the colored noise regression modeling[J]. Navigation Positioning & Timing, 2015, 2(6): 19-24.

[7]楊元喜, 崔先強(qiáng). 動(dòng)態(tài)定位有色噪聲影響函數(shù)——以一階AR模型為例[J]. 測(cè)繪學(xué)報(bào), 2003, 32(1):6-10.

YANG Yuan-xi, CUI Xian-qiang. Influence functions of colored noises on kinematic positioning—— taking the AR model of first class as an example [J]. Acta Geodaetica et Cartographica Sinica, 2003, 32(1):6-10.

[8]崔先強(qiáng), 楊元喜, 張曉東. 導(dǎo)航解算中的有色噪聲及其協(xié)方差矩陣自適應(yīng)擬合[J]. 空間科學(xué)學(xué)報(bào), 2012, 32(4):592-597.

CUI Xian-qiang, YANG Yuan-xi, ZHANG Xiao-dong. Adaptive fitting of colored noises and corresponding covariance matrices in navigation [J]. Chinese Journal of Space Science, 2012, 32(4):592-597.

[9] Cho S Y, Kim B D, Cho Y S, et.al. Observability analysis of the INS/GPS navigation system on the measurements in land vehicle applications[C]. International Conference on Control, Seoul, Korea, 2007:17-20.

[10]Zhang X Y, Shi H T, Pan J Y, et al. Integrated navigation method based on inertial navigation system and Lidar[J]. Otical Engineering, 2016,55(4): 044102.

[11]崔先強(qiáng), 楊元喜, 高為廣. 多種有色噪聲自適應(yīng)濾波算法的比較[J]. 武漢大學(xué)學(xué)報(bào)(信息科學(xué)版), 2006, 31(8): 731-735.

CUI Xian-qiang, YANG Yuan-xi, GAO Wei-guang. Comparison of adaptive filter arithmetics in controlling influence of colored noises [J]. Geomatics and Information Science of Wuhan University, 2006, 31(8): 731-735.

[12]Chang G. On Kalman filter for linear system with colored measurement noise [J]. Journal of Geodesy, 2014, 88(12):1163-1170.

[13]Yang Y X, Cui X Q. Adaptively robust filter with multi adaptive factors [J]. Survey Review, 2008, 40(309): 260-270.

[14]吳富梅. 組合導(dǎo)航系統(tǒng)誤差分析與補(bǔ)償理論及方法研究[D]. 解放軍信息工程大學(xué), 2007.

WU Fu-mei. Research on theory and method of error analysis and compensation of integrated navigation system [D]. PLA Information Engineering University, 2007.

[15]Yang Y X, Gao W G. A new learning statistic for adaptive filter based on predicted residuals[J]. Progress in Natural Science, 2006, 16(8):833-837.

[16]韓松來(lái). GPS和捷聯(lián)慣導(dǎo)組合導(dǎo)航新方法及系統(tǒng)誤差補(bǔ)償方案研究[D]. 國(guó)防科技大學(xué), 2010.

HAN Song-lai. Novel GPS/SINS integrated architechture and systematic error compensation methods[D]. National University of Defense Technology, 2010.