基于自適應(yīng)強(qiáng)跟蹤濾波的捷聯(lián)慣導(dǎo)/里程計(jì)組合導(dǎo)航方法

李士心，黃鳳榮，邱時(shí)前，范超男

（1. 天津職業(yè)技術(shù)師范大學(xué) 電子工程學(xué)院，天津 300222；2. 河北工業(yè)大學(xué) 機(jī)械工程學(xué)院，天津 300401；3. 北京鐳航世紀(jì)科技有限公司，北京 100081）

車(chē)載定位定向系統(tǒng)是為陸基武器平臺(tái)提供陣地坐標(biāo)和方位指向的基準(zhǔn)設(shè)備，一般采用捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）與里程計(jì)（OD）組合模式進(jìn)行不依賴(lài)外界信息的自主導(dǎo)航，以避免戰(zhàn)時(shí)受外界干擾。對(duì)于大型輪式車(chē)而言，由于輪胎與地面接觸面積大，會(huì)導(dǎo)致里程計(jì)標(biāo)度由于輪胎胎壓變化、路況變化等原因發(fā)生較大變化，無(wú)法滿(mǎn)足車(chē)載定位定向系統(tǒng)給里程計(jì)分配的誤差要求，從而直接影響系統(tǒng)定位精度。

非線(xiàn)性系統(tǒng)狀態(tài)估計(jì)在組合導(dǎo)航、初始對(duì)準(zhǔn)等領(lǐng)域具有廣泛的應(yīng)用[1-5]，發(fā)展了擴(kuò)展卡爾曼濾波（EKF）、無(wú)跡卡爾曼濾波（Unsented Kalman Filter, UKF）、中心差分卡爾曼濾波（Central Difference Kalman Filter），帶漸消因子的強(qiáng)跟蹤濾波（STF）、粒子濾波（Particle Filter, PF）、容積卡爾曼濾波（CKF）等多種非線(xiàn)性濾波方法[6-8]。其中，STF提出漸消因子的概念，有效解決了 EKF在模型不確定時(shí)的濾波問(wèn)題；CKF采用球面徑向容積準(zhǔn)則進(jìn)行相應(yīng)的數(shù)值積分運(yùn)算[9]，不僅克服了EKF和UKF在強(qiáng)非線(xiàn)性系統(tǒng)中的應(yīng)用局限性，而且濾波精度高于CDKF和PF。

為解決里程計(jì)標(biāo)度誤差在大型輪式車(chē)行駛過(guò)程會(huì)發(fā)生變化導(dǎo)致定位精度變差的問(wèn)題，本文以 CKF為算法框架，引入STF漸消因子的基本理論，提出了SINS/OD組合導(dǎo)航的自適應(yīng)ST-CKF算法，并通過(guò)仿真分析和跑車(chē)試驗(yàn)驗(yàn)證方法的正確性。

1 強(qiáng)跟蹤濾波器

假設(shè)有如下形式的非線(xiàn)性離散系統(tǒng)：

其中：fk和hk+1分別為狀態(tài)函數(shù)和量測(cè)函數(shù)；與分別為系統(tǒng)狀態(tài)向量和觀測(cè)向量；系統(tǒng)過(guò)程噪聲wk和量測(cè)噪聲vk互不相關(guān)且

STF在EKF的基礎(chǔ)上引入漸消因子，實(shí)時(shí)調(diào)整預(yù)測(cè)誤差協(xié)方差陣，使濾波殘差序列保持相互正交，從而保持對(duì)系統(tǒng)狀態(tài)的跟蹤。STF的遞推公式為：

其中，λk+1為漸消因子，In×n為n階單位陣，

采用次優(yōu)算法計(jì)算漸消因子：

其中，tr[·]為矩陣求跡的算子，為未引入漸消因子的狀態(tài)預(yù)測(cè)協(xié)方差陣。

為實(shí)際輸出殘差序列的協(xié)方差陣，通常是未知的，由下式進(jìn)行估算：

其中，為遺忘因子。

式(13)和式(14)利用了測(cè)量函數(shù)的泰勒級(jí)數(shù)展開(kāi)式計(jì)算漸消因子，存在求解雅克比矩陣的問(wèn)題。根據(jù)文獻(xiàn)[4]，為避免測(cè)量函數(shù)的雅克比矩陣數(shù)值計(jì)算，假設(shè)引入漸消因子之前的狀態(tài)預(yù)測(cè)誤差協(xié)方差陣為新息協(xié)方差陣為互協(xié)方差陣為則式(13)(14)分別有如下的等價(jià)表達(dá)式：

2 容積卡爾曼濾波器

根據(jù)文獻(xiàn)[6]，CKF算法步驟如下：

1）時(shí)間更新

假設(shè)k時(shí)刻的狀態(tài)xk的狀態(tài)誤差協(xié)方差為Pk，對(duì)做Cholesky分解，有：

選擇容積點(diǎn)為：

系統(tǒng)狀態(tài)方程傳播后容積點(diǎn)為：

其中，m=2n，且有是點(diǎn)集[1]的第i列。

計(jì)算狀態(tài)預(yù)測(cè)值：

計(jì)算誤差協(xié)方差陣：

2）量測(cè)更新

對(duì)Pk+1|k做Cholesky分解：

計(jì)算容積點(diǎn)：

經(jīng)量測(cè)方程傳播后的容積點(diǎn)為：

計(jì)算觀測(cè)預(yù)測(cè)值：

計(jì)算量測(cè)誤差協(xié)方差陣：

計(jì)算一步預(yù)測(cè)互相關(guān)協(xié)方差陣：

計(jì)算濾波增益：

狀態(tài)估計(jì)值：

求取狀態(tài)誤差協(xié)方差估計(jì)值：

3 自適應(yīng)強(qiáng)跟蹤容積卡爾曼濾波器

STF算法在工程上有以下三點(diǎn)缺陷：1）STF算法基于EKF算法框架，將非線(xiàn)性函數(shù)截?cái)嘟凭€(xiàn)性化，對(duì)于強(qiáng)非線(xiàn)性系統(tǒng)，計(jì)算間隔要設(shè)的非常小以減小截?cái)嗾`差；2）需要用數(shù)值計(jì)算方法計(jì)算非線(xiàn)性函數(shù)的雅可比矩陣，運(yùn)算量比較大；3）對(duì)于某些不可微非線(xiàn)性函數(shù)或時(shí)刻點(diǎn)，無(wú)法計(jì)算雅可比矩陣。

因此，STF數(shù)值計(jì)算穩(wěn)定性不佳，甚至對(duì)于強(qiáng)非線(xiàn)性系統(tǒng)，容易濾波發(fā)散。STF的缺陷都是基于EKF算法框架下的，尋求一種可以代替EKF的算法框架，既能保持STF漸消因子強(qiáng)迫正交的理論優(yōu)勢(shì)，又能避免計(jì)算雅可比矩陣的繁瑣，還能進(jìn)一步提高非線(xiàn)性函數(shù)線(xiàn)性化的計(jì)算精度，具有比較強(qiáng)的現(xiàn)實(shí)意義。CKF算法過(guò)程中不需要計(jì)算導(dǎo)數(shù)，且具有泰勒展開(kāi)的三階精度，故基于CKF算法框架設(shè)計(jì)強(qiáng)跟蹤C(jī)KF （ST-CKF）算法，能避免STF的工程應(yīng)用缺陷，融合STF魯棒性強(qiáng)和CKF計(jì)算精度高的優(yōu)點(diǎn)，實(shí)現(xiàn)工程應(yīng)用的目的。

ST-CKF的算法步驟如下：

1）時(shí)間更新

由式(18)～(22)計(jì)算沒(méi)有引入漸消因子的狀態(tài)預(yù)測(cè)協(xié)方差陣進(jìn)行Cholesky分解：

2）計(jì)算漸消因子

由式(23)～(28)計(jì)算沒(méi)有引入漸消因子的量測(cè)

誤差協(xié)方差陣和一步預(yù)測(cè)互相關(guān)協(xié)方差陣

然后由式(32)～(34)、式(11)～(17)計(jì)算漸消因子λk+1。

3）量測(cè)更新

根據(jù)加入漸消因子λk+1的狀態(tài)xk+1的統(tǒng)計(jì)特性由式(24)求取相應(yīng)加入漸消因子的容積點(diǎn)以此重復(fù)式(25)～(31)的流程求得狀態(tài)估計(jì)和Pk+1。

4 SINS/OD組合導(dǎo)航卡爾曼濾波模型

本文選取SINS的姿態(tài)角誤差φE、φN、φU，水平速度誤差δvE、δvN，經(jīng)緯度誤差δλS、δLS，陀螺常值漂移εx、εy、εz，加速度計(jì)零偏?x、?y、?z，里程計(jì)推算經(jīng)緯度誤差δλD、δLD，里程計(jì)標(biāo)度因數(shù)誤差δKD，里程計(jì)方位安裝偏角φα作為狀態(tài)變量，建立17維的SINS/OD組合導(dǎo)航狀態(tài)方程；選取 SINS解算經(jīng)緯度與里程計(jì)推算經(jīng)緯度之差作為量測(cè)構(gòu)建量測(cè)方程。誤差模型的建立與推導(dǎo)過(guò)程參看文獻(xiàn)[10]。

SINS/OD組合導(dǎo)航的狀態(tài)向量為：

式中：W為3只陀螺漂移和3只加速度計(jì)零偏測(cè)量噪聲，F(xiàn)為系統(tǒng)狀態(tài)矩陣，見(jiàn)式(37)。的第i行第j列元素，vD為里程計(jì)輸出速度。

量測(cè)方程為：

其中，V為里程計(jì)推算緯度和經(jīng)度的量測(cè)噪聲，

5 仿真分析和試驗(yàn)驗(yàn)證

分別采用EKF、STF、ST-CKF進(jìn)行SINS/OD組合導(dǎo)航的仿真分析，濾波性能的好壞可以通過(guò) SINS/OD組合導(dǎo)航的定位精度、里程計(jì)安裝偏角和標(biāo)度誤差的跟蹤性能來(lái)衡量。

假設(shè)：3個(gè)陀螺的常值漂移均為0.01 (°)/h，隨機(jī)白噪聲標(biāo)準(zhǔn)差為0.01 (°)/h；3個(gè)加速度計(jì)的零偏均為0.05 mg，隨機(jī)白噪聲標(biāo)準(zhǔn)差為0.05 mg；初始水平姿態(tài)角誤差 0.01°，初始航向誤差 0.1°，初始水平速度誤差和初始經(jīng)緯度誤差均為0；載車(chē)初始位置為北緯40°，東經(jīng) 118°，初始時(shí)刻方位角為 45°，俯仰角為 0°，橫滾角為0°，以20 m/s的速度勻速行駛，行駛時(shí)間為 900 s；里程計(jì)標(biāo)度誤差設(shè)為0.01，方位安裝偏角設(shè)為 0.05°?？紤]大型輪式車(chē)的實(shí)際應(yīng)用背景進(jìn)行以下兩種情況仿真分析：

1）假設(shè)里程計(jì)標(biāo)度誤差δKD受路況變化影響發(fā)生了突變，在300s時(shí)由0.01階躍到0.02；

2）假設(shè)里程計(jì)標(biāo)度誤差δKD受胎壓變化影響發(fā)生了漸變，在300s時(shí)開(kāi)始到600s結(jié)束，由0.01斜坡變化到0.02。

以上兩種情況分別采用EKF、STF、ST-CKF對(duì)δKD進(jìn)行估計(jì)，里程計(jì)標(biāo)度誤差階躍變化和斜坡變化的跟蹤曲線(xiàn)如圖1和圖2所示。

圖1 EKF、STF和ST-CKF算法對(duì)δDK 階躍變化的跟蹤曲線(xiàn)Fig.1 Step change tracking curve ofδDK using EKF, STF and ST-CKF algorithms

圖2 EKF、STF和ST-CKF算法對(duì) δKD線(xiàn)斜坡變化的跟蹤曲Fig.2 Ramp change tracking curve ofδDK using EKF, STF and ST-CKF algorithms

由圖1～2中曲線(xiàn)不難看出，EKF在運(yùn)行一段時(shí)間后可以有效地估計(jì)出δKD的常值量，然而在隨后誤差發(fā)生幅度較大的突變或漸變時(shí)，EKF失去了跟蹤能力。因此對(duì)于 SINS/OD組合導(dǎo)航系統(tǒng)而言，EKF可以在δKD為常值或變化不大的情況下使用，作為一種在線(xiàn)自主標(biāo)定方法是有效的，而STF和ST-CKF無(wú)論對(duì)誤差的突變還是漸變都具有很強(qiáng)的跟蹤能力。

為驗(yàn)證ST-CKF的工程實(shí)用性，進(jìn)行了大型輪式車(chē)的跑車(chē)試驗(yàn)。試驗(yàn)采用的激光陀螺定位定向系統(tǒng)慣性元件精度為：陀螺零偏穩(wěn)定性＜0.015 (°)/h，陀螺零偏重復(fù)性＜0.005 (°)/h，加速度計(jì)零偏穩(wěn)定性和重復(fù)性＜0.05 mg。試驗(yàn)設(shè)計(jì)的行車(chē)路線(xiàn)如圖3所示（僅示意變化量），行車(chē)?yán)锍?20 km，包括去程和回程共2個(gè)航程。試驗(yàn)前靜止對(duì)準(zhǔn)3 min，試驗(yàn)過(guò)程中以GPS位置信息作為參考基準(zhǔn)，GPS定位精度＜2 m。分別采用EKF、STF和ST-CKF對(duì)跑車(chē)過(guò)程中錄取的激光陀螺定位定向系統(tǒng)和里程計(jì)原始數(shù)據(jù)進(jìn)行離線(xiàn)仿真。

圖3 行車(chē)軌跡Fig.3 Trajectory of vehicle

圖4 第1航程EKF算法的位置誤差曲線(xiàn)Fig.4 Position error curve of test-1 using EKF algorithm

圖5 第2航程EKF算法的位置誤差曲線(xiàn)Fig.5 Position error curve of test-2 using EKF algorithm

兩個(gè)航程的EKF位置誤差試驗(yàn)結(jié)果如圖4和圖5所示，STF和ST-CKF位置誤差試驗(yàn)結(jié)果如圖6和圖7所示，圖中毛刺為GPS跳數(shù)所致。由圖4和圖5可以看出，由于EKF無(wú)法實(shí)時(shí)跟蹤里程計(jì)標(biāo)度的變化，去程定位誤差最大達(dá)到了 1617 m，回程最大誤差為619 m，定位誤差＞7‰D（其中，D為里程），根本沒(méi)有發(fā)揮出慣性元件的使用精度。由圖6和圖7可以看出，STF和ST-CKF均達(dá)到了＜1.5‰D的定位精度，對(duì)比可以看出ST-CKF對(duì)STF的改進(jìn)，去程定位誤差最大由282 m提升到128 m，回程定位誤差最大由180 m提升到111 m，使得SINS/OD組合導(dǎo)航系統(tǒng)的定位精度由1.3‰D提高到0.6‰D，ST-CKF比STF更進(jìn)一步消除了里程計(jì)誤差的影響，達(dá)到了慣性元件的理論精度。

圖6 第1航程STF和ST-CKF算法的位置誤差曲線(xiàn)Fig.6 Position error curve of test-1 using STF and ST-CKF algorithms

圖7 第2航程STF和ST-CKF算法的位置誤差曲線(xiàn)Fig.7 Position error curve of test-2 using STF and ST-CKF algorithms

去程和回程的里程計(jì)標(biāo)度誤差和方位安裝偏角ST-CKF估計(jì)曲線(xiàn)如圖8和圖9所示。由圖8可以看出，里程計(jì)標(biāo)度誤差在試驗(yàn)過(guò)程中近似斜坡緩慢變化了大約 0.015；由圖9可以看出，里程計(jì)標(biāo)度誤差在190 km處由于路況（高速轉(zhuǎn)入市內(nèi)公路）改變而發(fā)生了明顯突變。里程計(jì)標(biāo)度的實(shí)際變化包括了仿真分析的兩種情況，說(shuō)明了仿真分析的合理性。由圖8分析，如果不能對(duì)里程計(jì)誤差進(jìn)行實(shí)時(shí)估計(jì)補(bǔ)償?shù)脑?huà)，會(huì)造成約220 000×0.015÷2=1650 m的位置誤差，與EKF去程定位誤差最大1617 m相符，這也反映了里程計(jì)標(biāo)度誤差自適應(yīng)補(bǔ)償?shù)谋匾浴?/p>

圖8 第1航程ST-CKF算法里程計(jì)誤差估計(jì)曲線(xiàn)Fig.8 OD’s error estimated curve of test-1 using ST-CKF algorithm

圖9 第2航程ST-CKF算法里程計(jì)誤差估計(jì)曲線(xiàn)Fig.9 OD’s error estimated curve of test-2 using ST-CKF algorithm

6 結(jié) 論

本文針對(duì)EKF在SINS/OD組合導(dǎo)航誤差模型中對(duì)里程計(jì)誤差模型描述不準(zhǔn)確時(shí)難以實(shí)時(shí)跟蹤誤差變化以及STF在工程應(yīng)用上的局限性，用CKF替代EKF的算法框架，引入STF漸消因子的基本理論，提出了SINS/OD組合導(dǎo)航的自適應(yīng)ST-CKF算法，并進(jìn)行了仿真分析和跑車(chē)試驗(yàn)。仿真證明STF和ST-CKF具有實(shí)時(shí)跟蹤里程計(jì)誤差變化的能力，驗(yàn)證了算法有效性。跑車(chē)試驗(yàn)結(jié)果表明，基于自適應(yīng)STF的組合導(dǎo)航方法能有效估計(jì)速度輔助傳感器的時(shí)變誤差，所提出的ST-CKF算法進(jìn)一步提高了誤差估計(jì)精度，有效解決了里程計(jì)標(biāo)度誤差在大型輪式車(chē)行駛過(guò)程發(fā)生變化導(dǎo)致定位精度變差的問(wèn)題。另外，本文方法不僅適用于陸用定位定向?qū)Ш?，還可以推廣到SINS/DVL組合的航海導(dǎo)航領(lǐng)域[11-12]，具有良好的應(yīng)用前景。

參考文獻(xiàn)（References）：

[1] Zhang L, Yang C, Chen Q W, et al. Robust H-infinity CKF/KF hybrid filtering method for SINS alignment[J].IET Science, Measurement & Technology, 2016, 10(8):916-925

[2] Cui B B, Chen X Y, Xu Y, et al. Performance analysis of improved iterated cubature Kalman filter and its application to GNSS/INS[J]. ISA Transactions, 2017, 66: 460-468.

[3] Zhou W D, Hou J X, Liu L, et al. Design and simulation of the integrated navigation system based on extended Kalman filter[J]. Open Physics, 2017, 15(1): 182-187.

[4] 王小旭, 趙琳, 薛紅香. 強(qiáng)跟蹤C(jī)DKF及其在組合導(dǎo)航中的應(yīng)用[J]. 控制與決策, 2010, 25(12): 1837-1842.

Wang X X, Zhao L, Xue H X. Strong tracking CDKF and application for integrated navigation[J]. Control and Decision, 2010, 25(12): 1837-1842.

[5] 趙小明, 趙帥, 郭永剛, 等. 基于強(qiáng)跟蹤濾波的車(chē)載行進(jìn)間對(duì)準(zhǔn)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2015, 23(2): 141-144.Zhao X M, Zhao S, Guo Y G, et al. In-motion alignment based on strong tracking filter[J]. Journal of Chinese Inertial Technology, 2015, 23(2): 141-144.

[6] Arasaratnam I, Haykin S. Cubature Kalman filters[J].IEEE Transactions on Automatic Control, 2009, 54(6):1254-2169.

[7] Pasha S A, Hoang D T, Vo B N. Nonlinear baysian filtering using the unscented linear fractional transformation model[J]. IEEE Transactions on Signal Processing,2013, 58(2): 477-489.

[8] Gustafsson F. Particle filter theory and practice with positioning applications[J]. IEEE Aerospace and Electronic Systems Magazine, 2015, 25(7): 53-82.

[9] 劉萬(wàn)利, 張秋昭. 基于 Cubature卡爾曼濾波的強(qiáng)跟蹤濾波算法[J]. 系統(tǒng)仿真學(xué)報(bào), 2014, 26(5): 1102-1107

Liu W L, Zhang Q Z. New strong tracking filter based on cubature Kalman filter[J]. Journal of System Simulation,2014, 26(5): 1102-1107

[10] 趙洪松, 繆玲娟, 沈軍. 捷聯(lián)慣導(dǎo)/里程計(jì)高精度組合導(dǎo)航算法[J]. 兵工學(xué)報(bào), 2014, 35(4): 433-440.

Zhao H S, Miao L J, Shen J. High accuracy algorithm for SINS/Odometer integrated navigation system[J]. Acta Armamentarii, 2014, 35(4): 433-440.

[11] Gao W, Li J C. Adaptive Kalman filtering for the integrated SINS/DVL system[J]. Journal of Computational Information Systems. 2013, 16(9): 6443-6450.

[12] Wang L, Cheng X H, Li S X, et al. Adaptive interacting multiple model filter for AUV integrated navigation[J]. Journal of Chinese Inertial Technology, 2016, 24(4): 511-515.