李士心,黃鳳榮,邱時(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)證方法的正確性。
假設(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á)式:
根據(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ì)值:
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。
本文選取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è)噪聲,
分別采用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
本文針對(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.