李士心，黃鳳榮，邱時前，范超男

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

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

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

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

1 強跟蹤濾波器

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

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

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

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

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

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

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

其中，為遺忘因子。

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

2 容積卡爾曼濾波器

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

1）時間更新

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

選擇容積點為：

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

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

計算狀態(tài)預(yù)測值：

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

2）量測更新

對Pk+1|k做Cholesky分解：

計算容積點：

經(jīng)量測方程傳播后的容積點為：

計算觀測預(yù)測值：

計算量測誤差協(xié)方差陣：

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

計算濾波增益：

狀態(tài)估計值：

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

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

STF算法在工程上有以下三點缺陷：1）STF算法基于EKF算法框架，將非線性函數(shù)截斷近似線性化，對于強非線性系統(tǒng)，計算間隔要設(shè)的非常小以減小截斷誤差；2）需要用數(shù)值計算方法計算非線性函數(shù)的雅可比矩陣，運算量比較大；3）對于某些不可微非線性函數(shù)或時刻點，無法計算雅可比矩陣。

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

ST-CKF的算法步驟如下：

1）時間更新

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

2）計算漸消因子

由式(23)～(28)計算沒有引入漸消因子的量測

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

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

3）量測更新

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

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

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

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

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

量測方程為：

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

5 仿真分析和試驗驗證

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

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

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

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

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

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

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

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

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

圖3 行車軌跡Fig.3 Trajectory of vehicle

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

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

兩個航程的EKF位置誤差試驗結(jié)果如圖4和圖5所示，STF和ST-CKF位置誤差試驗結(jié)果如圖6和圖7所示，圖中毛刺為GPS跳數(shù)所致。由圖4和圖5可以看出，由于EKF無法實時跟蹤里程計標(biāo)度的變化，去程定位誤差最大達(dá)到了 1617 m，回程最大誤差為619 m，定位誤差＞7‰D（其中，D為里程），根本沒有發(fā)揮出慣性元件的使用精度。由圖6和圖7可以看出，STF和ST-CKF均達(dá)到了＜1.5‰D的定位精度，對比可以看出ST-CKF對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)一步消除了里程計誤差的影響，達(dá)到了慣性元件的理論精度。

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

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

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

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

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

6 結(jié) 論

本文針對EKF在SINS/OD組合導(dǎo)航誤差模型中對里程計誤差模型描述不準(zhǔn)確時難以實時跟蹤誤差變化以及STF在工程應(yīng)用上的局限性，用CKF替代EKF的算法框架，引入STF漸消因子的基本理論，提出了SINS/OD組合導(dǎo)航的自適應(yīng)ST-CKF算法，并進(jìn)行了仿真分析和跑車試驗。仿真證明STF和ST-CKF具有實時跟蹤里程計誤差變化的能力，驗證了算法有效性。跑車試驗結(jié)果表明，基于自適應(yīng)STF的組合導(dǎo)航方法能有效估計速度輔助傳感器的時變誤差，所提出的ST-CKF算法進(jìn)一步提高了誤差估計精度，有效解決了里程計標(biāo)度誤差在大型輪式車行駛過程發(fā)生變化導(dǎo)致定位精度變差的問題。另外，本文方法不僅適用于陸用定位定向?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] 王小旭, 趙琳, 薛紅香. 強跟蹤CDKF及其在組合導(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] 趙小明, 趙帥, 郭永剛, 等. 基于強跟蹤濾波的車載行進(jìn)間對準(zhǔn)[J]. 中國慣性技術(shù)學(xué)報, 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] 劉萬利, 張秋昭. 基于 Cubature卡爾曼濾波的強跟蹤濾波算法[J]. 系統(tǒng)仿真學(xué)報, 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)/里程計高精度組合導(dǎo)航算法[J]. 兵工學(xué)報, 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.