車(chē)載慣導(dǎo)系統(tǒng)動(dòng)基座高精度自主式對(duì)準(zhǔn)方法

楊 波，王躍鋼，郭志斌，單 斌

（第二炮兵工程大學(xué) 自動(dòng)控制系，西安 710025）

車(chē)載慣導(dǎo)系統(tǒng)動(dòng)基座高精度自主式對(duì)準(zhǔn)方法

楊 波，王躍鋼，郭志斌，單 斌

（第二炮兵工程大學(xué) 自動(dòng)控制系，西安 710025）

研究了一種利用里程計(jì)與車(chē)輛運(yùn)動(dòng)約束條件來(lái)輔助車(chē)載慣導(dǎo)系統(tǒng)進(jìn)行動(dòng)基座高精度自主式對(duì)準(zhǔn)的方法。利用里程計(jì)輸出與慣導(dǎo)姿態(tài)輸出進(jìn)行航位推算，將航位推算獲得的速度信息與慣導(dǎo)輸出的對(duì)應(yīng)信息相減作為量測(cè)之一；利用車(chē)輛運(yùn)動(dòng)約束條件，將慣導(dǎo)速度輸出沿車(chē)體橫向、垂向的投影作為量測(cè)之二；選取慣導(dǎo)系統(tǒng)誤差與里程計(jì)誤差作為系統(tǒng)狀態(tài)，采用卡爾曼濾波設(shè)計(jì)動(dòng)基座高精度對(duì)準(zhǔn)算法。仿真結(jié)果表明，在載車(chē)行駛條件下，該方法的東向、北向失準(zhǔn)角估計(jì)精度分別達(dá)到0.21′、0.25′，天向失準(zhǔn)角的估計(jì)精度達(dá)到2.16′。

慣導(dǎo)系統(tǒng)；航位推算；車(chē)輛運(yùn)動(dòng)約束；動(dòng)基座對(duì)準(zhǔn)；卡爾曼濾波

現(xiàn)代戰(zhàn)爭(zhēng)要求陸地戰(zhàn)車(chē)、導(dǎo)彈發(fā)射車(chē)等特殊軍用車(chē)輛能夠在作戰(zhàn)區(qū)域內(nèi)快速反應(yīng)、機(jī)動(dòng)作戰(zhàn)，并能夠?yàn)檐?chē)載武器隨時(shí)自主地提供精確的水平與方位基準(zhǔn)，以期實(shí)現(xiàn)停車(chē)就打甚至邊走邊打[1]。這就要求車(chē)載慣導(dǎo)系統(tǒng)能夠在車(chē)輛運(yùn)動(dòng)條件下實(shí)現(xiàn)高精度自主式對(duì)準(zhǔn)。

傳統(tǒng)的車(chē)載慣導(dǎo)動(dòng)基座對(duì)準(zhǔn)通常需要輔助設(shè)備提供速度等信息，以此作為對(duì)準(zhǔn)濾波的觀測(cè)量。如采用GPS 提供的速度、位置信息輔助慣導(dǎo)實(shí)現(xiàn)行進(jìn)間快速高精度對(duì)準(zhǔn)[2-4]，但是 GPS衛(wèi)星信號(hào)易受遮擋或被干擾，且戰(zhàn)時(shí)不可用，因此這種方法抗干擾性和自主性較差。為了提高自主性和抗干擾性，文獻(xiàn)[5]研究了利用里程計(jì)提供的速度信息輔助捷聯(lián)慣導(dǎo)進(jìn)行動(dòng)基座對(duì)準(zhǔn)，但是需要借助特定的載車(chē)行進(jìn)方式，這必然降低了快速機(jī)動(dòng)性。文獻(xiàn)[6]利用里程計(jì)與陀螺儀進(jìn)行航位推算，將航位推算獲得的姿態(tài)、速度與捷聯(lián)慣導(dǎo)對(duì)應(yīng)輸出相減作為量測(cè)設(shè)計(jì)動(dòng)基座對(duì)準(zhǔn)算法，不需要載車(chē)作特殊機(jī)動(dòng)或停車(chē)，但對(duì)準(zhǔn)精度還有待進(jìn)一步提高。文獻(xiàn)[7]提出了基于動(dòng)態(tài)零速條件的動(dòng)基座自對(duì)準(zhǔn)方法，利用車(chē)體橫向和垂向速度為零的特點(diǎn)構(gòu)造對(duì)準(zhǔn)濾波器，不依賴任何輔助信息，且具有較強(qiáng)的機(jī)動(dòng)性，但是該方法的對(duì)準(zhǔn)精度有限。

為了有效提高對(duì)準(zhǔn)精度，并確保對(duì)準(zhǔn)方法的自主性與機(jī)動(dòng)性，本文提出利用里程計(jì)與車(chē)輛運(yùn)動(dòng)約束條件共同輔助車(chē)載慣導(dǎo)系統(tǒng)進(jìn)行動(dòng)基座對(duì)準(zhǔn)，研究了對(duì)準(zhǔn)方案與對(duì)準(zhǔn)濾波算法，并通過(guò)仿真驗(yàn)證了所提方法的正確性。

1 慣導(dǎo)系統(tǒng)動(dòng)基座高精度自主式對(duì)準(zhǔn)方案

以捷聯(lián)慣導(dǎo)系統(tǒng)為對(duì)準(zhǔn)研究對(duì)象，選取東北天地理坐標(biāo)系作為導(dǎo)航系。載車(chē)出發(fā)前首先對(duì)車(chē)載捷聯(lián)慣導(dǎo)系統(tǒng)加電并進(jìn)行粗對(duì)準(zhǔn)，快速解算獲得捷聯(lián)姿態(tài)矩陣的粗略值，粗對(duì)準(zhǔn)的精度較低，一般在角分級(jí)至1°左右。粗對(duì)準(zhǔn)結(jié)束后，載車(chē)即出發(fā)，同時(shí)開(kāi)始進(jìn)行動(dòng)基座對(duì)準(zhǔn)。在動(dòng)基座對(duì)準(zhǔn)過(guò)程中，利用里程計(jì)的路程增量輸出與捷聯(lián)慣導(dǎo)輸出的姿態(tài)矩陣進(jìn)行航位推算速度更新，獲得載車(chē)的實(shí)時(shí)速度信息；將航位推算獲得的速度信息與慣導(dǎo)輸出的對(duì)應(yīng)信息相減作為量測(cè)之一，將慣導(dǎo)速度輸出沿車(chē)體橫向、垂向的投影作為量測(cè)之二，送入卡爾曼濾波器中進(jìn)行對(duì)準(zhǔn)濾波；經(jīng)過(guò)濾波計(jì)算獲得數(shù)學(xué)平臺(tái)失準(zhǔn)角的估計(jì)值，利用該估計(jì)值對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)的姿態(tài)矩陣進(jìn)行誤差修正，從而完成車(chē)載慣導(dǎo)系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)。因此，慣導(dǎo)系統(tǒng)動(dòng)基座高精度自主式對(duì)準(zhǔn)的原理方案如圖1所示。

圖1 慣導(dǎo)系統(tǒng)動(dòng)基座高精度自主式對(duì)準(zhǔn)的原理框圖Fig.1 Accurate independent alignment schematic for inertial navigation system on moving base

2 航位推算速度更新算法

在航位推算過(guò)程中，本文只需進(jìn)行載車(chē)的速度解算。利用車(chē)載捷聯(lián)慣導(dǎo)實(shí)時(shí)獲得的載車(chē)姿態(tài)矩陣將里程計(jì)測(cè)得的載車(chē)速度轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下進(jìn)行速度更新計(jì)算，從而實(shí)時(shí)獲得載車(chē)在導(dǎo)航系下的速度。

設(shè)航位推算的速度更新周期為T(mén)D，里程計(jì)在每個(gè)更新周期內(nèi)測(cè)得的路程增量為ΔS(i)，則ti時(shí)刻載車(chē)速度在車(chē)體坐標(biāo)系（b系）下的投影可表示為

3 航位推算的速度誤差方程

除了慣導(dǎo)姿態(tài)誤差，里程計(jì)刻度系數(shù)誤差是航位推算最主要的誤差源。不同路面條件、環(huán)境溫度和輪胎狀態(tài)將引起里程計(jì)刻度系數(shù)的變化，從而造成里程計(jì)測(cè)量誤差[8]，通常可將里程計(jì)刻度系數(shù)誤差考慮為隨機(jī)常值[9]，即

從而，將式(8)展開(kāi)可得航位推算的速度誤差方程如下：

4 動(dòng)基座高精度對(duì)準(zhǔn)濾波算法

為了在動(dòng)基座環(huán)境下實(shí)現(xiàn)高精度自主式對(duì)準(zhǔn)，需要利用航位推算結(jié)果與車(chē)輛運(yùn)動(dòng)約束條件共同輔助車(chē)載慣導(dǎo)系統(tǒng)進(jìn)行對(duì)準(zhǔn)。選取捷聯(lián)慣導(dǎo)與航位推算的誤差作為系統(tǒng)狀態(tài)，將航位推算獲得的速度信息與捷聯(lián)慣導(dǎo)輸出的對(duì)應(yīng)信息相減作為量測(cè)之一；利用車(chē)輛運(yùn)動(dòng)約束條件，將捷聯(lián)慣導(dǎo)速度輸出沿車(chē)體橫向、垂向的投影作為量測(cè)之二；采用卡爾曼濾波設(shè)計(jì)對(duì)準(zhǔn)濾波算法。

4.1 對(duì)準(zhǔn)濾波的狀態(tài)方程

選取慣導(dǎo)系統(tǒng)與航位推算的誤差作為對(duì)準(zhǔn)濾波的系統(tǒng)狀態(tài)，包括捷聯(lián)慣導(dǎo)數(shù)學(xué)平臺(tái)失準(zhǔn)角φE、φN、φU，速度誤差δVE、δVN、δVU，位置誤差δL、δλ、δh，陀螺常值漂移εbx、εby、εbz，加速度計(jì)常值誤差▽bx、▽by、▽bz；里程計(jì)刻度系數(shù)誤差δKD，即對(duì)準(zhǔn)濾波的系統(tǒng)狀態(tài)X為

式中根據(jù)式(9)～(11)可知，航位推算的速度誤差與捷聯(lián)慣導(dǎo)數(shù)學(xué)平臺(tái)失準(zhǔn)角、里程計(jì)刻度系數(shù)誤差之間存在線性關(guān)系，則不再將其列入系統(tǒng)狀態(tài)。

根據(jù)捷聯(lián)慣導(dǎo)和航位推算的誤差模型，可列寫(xiě)出對(duì)準(zhǔn)濾波的狀態(tài)方程如下：

式中，F(xiàn)為系統(tǒng)狀態(tài)矩陣，G為系統(tǒng)噪聲驅(qū)動(dòng)陣，W為系統(tǒng)白噪聲，W=[wgx,wgy,wgz,wax,way,waz]T，其中wgx、wgy、wgz為陀螺白噪聲，wax、way、waz為加速度計(jì)白噪聲。

4.2 對(duì)準(zhǔn)濾波的量測(cè)方程

將航位推算獲得的速度信息與捷聯(lián)慣導(dǎo)輸出的對(duì)應(yīng)信息相減作為對(duì)準(zhǔn)濾波的量測(cè)之一Z1，即

由于捷聯(lián)慣導(dǎo)與航位推算獲得的載車(chē)速度信息中均含誤差，則根據(jù)式(14)有

再將式(9)～(11)代入到式(15)中可得

對(duì)于在地面上行駛的車(chē)輛，滿足如下運(yùn)動(dòng)約束條件：如果車(chē)輛不發(fā)生側(cè)滑或跳躍，其行駛速度在車(chē)體橫向、垂向上的分量應(yīng)該為零[10]。基于這一約束條件，可以將捷聯(lián)慣導(dǎo)速度輸出沿車(chē)體坐標(biāo)系x、z軸上的投影作為對(duì)準(zhǔn)濾波的量測(cè)之二Z2，即

然而，由于捷聯(lián)慣導(dǎo)速度誤差的存在，導(dǎo)致其實(shí)際速度輸出沿車(chē)體坐標(biāo)系x、z軸上的投影并不為零。設(shè)捷聯(lián)慣導(dǎo)速度誤差在車(chē)體坐標(biāo)系x、z軸上的投影分別為則根據(jù)式(17)可知：

將式(22)展開(kāi)，并取第一、三行可得：

再將式(21)與(23)進(jìn)行比較可得：

從而將量測(cè)Z1、Z2一起作為對(duì)準(zhǔn)濾波的量測(cè)，并根據(jù)式(16)、(18)和(24)可將對(duì)準(zhǔn)濾波的量測(cè)方程表達(dá)為如下形式：

獲得對(duì)準(zhǔn)濾波的狀態(tài)方程和量測(cè)方程以后，就可以利用卡爾曼濾波算法進(jìn)行濾波計(jì)算了。通過(guò)濾波計(jì)算，可以遞推計(jì)算出系統(tǒng)狀態(tài)X(包含捷聯(lián)慣導(dǎo)數(shù)學(xué)平臺(tái)失準(zhǔn)角)的最優(yōu)估計(jì)值，利用數(shù)學(xué)平臺(tái)失準(zhǔn)角的估計(jì)值對(duì)捷聯(lián)慣導(dǎo)姿態(tài)矩陣作修正，從而完成車(chē)載慣導(dǎo)運(yùn)動(dòng)基座對(duì)準(zhǔn)。

5 仿真與結(jié)論

設(shè)捷聯(lián)慣導(dǎo)中陀螺常值漂移為0.02 (°)/h，隨機(jī)游走為；加速度計(jì)常值誤差為10-4g，隨機(jī)游走為里程計(jì)刻度系數(shù)誤差為0.5%；捷聯(lián)慣導(dǎo)粗對(duì)準(zhǔn)誤差為1°，初始速度誤差為0 m/s，初始位置誤差為15 m；對(duì)準(zhǔn)時(shí)間為600 s，對(duì)準(zhǔn)過(guò)程中載車(chē)處于正常行駛運(yùn)動(dòng)狀態(tài)。基于上述仿真條件，對(duì)文中研究的動(dòng)基座高精度自主式對(duì)準(zhǔn)方法進(jìn)行計(jì)算機(jī)仿真驗(yàn)證，仿真結(jié)果如圖2～4所示。

圖2 捷聯(lián)慣導(dǎo)東向失準(zhǔn)角的估計(jì)誤差Fig.2 Estimation error of SINS east misalignment angle

圖3 捷聯(lián)慣導(dǎo)北向失準(zhǔn)角的估計(jì)誤差Fig.3 Estimation error of SINS north misalignment angle

圖4 捷聯(lián)慣導(dǎo)天向失準(zhǔn)角的估計(jì)誤差Fig.4 Estimation error of SINS upside misalignment angle

根據(jù)仿真結(jié)果可以看出，盡管之前粗對(duì)準(zhǔn)的誤差較大（1°），但是該方法仍然取得了較高的對(duì)準(zhǔn)精度：經(jīng)過(guò)600 s的動(dòng)基座對(duì)準(zhǔn)，捷聯(lián)慣導(dǎo)東、北、天向失準(zhǔn)角的估計(jì)精度分別達(dá)到0.21′、0.25′和2.16′，而且三個(gè)失準(zhǔn)角的估計(jì)誤差收斂效果均很顯著。其中，東向、北向失準(zhǔn)角的估計(jì)速度較快，在仿真第90 s時(shí)濾波結(jié)果就已達(dá)到穩(wěn)態(tài)，而天向失準(zhǔn)角直到仿真第400 s左右時(shí)才達(dá)到穩(wěn)態(tài)，這是由于天向失準(zhǔn)角的可觀測(cè)性較差所引起的。此外，對(duì)準(zhǔn)過(guò)程中不需要載車(chē)作特定的輔助機(jī)動(dòng)，也不需要中途停車(chē)，載車(chē)正常行駛即可。

綜上所述，本文充分考慮陸地車(chē)輛的運(yùn)動(dòng)特點(diǎn)，提出利用里程計(jì)與車(chē)輛運(yùn)動(dòng)約束條件來(lái)輔助車(chē)載慣導(dǎo)系統(tǒng)進(jìn)行動(dòng)基座高精度自主式對(duì)準(zhǔn)，研究了里程計(jì)航位推算的速度更新算法與速度誤差方程，利用航位推算獲得的速度信息、慣導(dǎo)速度輸出沿車(chē)體橫向與垂向的投影構(gòu)造量測(cè)，從而研究獲得了動(dòng)基座高精度對(duì)準(zhǔn)濾波算法。該對(duì)準(zhǔn)方法不僅精度高，而且自主性強(qiáng)，機(jī)動(dòng)性好，不需要借助任何外界信息，也不需要載車(chē)作特殊輔助機(jī)動(dòng)或中途停車(chē)，非常有利于戰(zhàn)場(chǎng)環(huán)境下車(chē)載武器系統(tǒng)的快速反應(yīng)、機(jī)動(dòng)作戰(zhàn)，因此具有良好的軍事應(yīng)用前景。

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

[1] 付強(qiáng)文, 秦永元, 李四海, 等. 車(chē)輛運(yùn)動(dòng)學(xué)約束輔助的慣性導(dǎo)航算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2012, 20(6): 640-643. Fu Qiang-wen, Qin Yong-yuan, Li Si-hai, et al. Inertial navigation algorithm aided by motion constraints of vehicle[J]. Journal of Chinese Inertial Technology, 2012, 20(6): 640-643.

[2] Ali J, Ushap M. A consistent and robust Kalman filter design for in-motion alignment of inertial navigation system [J]. Measurement, 2009, 42(4): 557-582.

[3] 馬建萍. GPS輔助捷聯(lián)慣導(dǎo)系統(tǒng)動(dòng)基座初始對(duì)準(zhǔn)新方[J]. 傳感技術(shù)學(xué)報(bào), 2010, 23(11): 1656-1661. Ma Jian-ping. A new GPS aided initial alignment algorithm for SINS on moving base[J]. Chinese Journal of Sensors and Actuators, 2010, 23(11): 1656-1661.

[4] Woonseon H, Han Kyungjun, Lee Chulsoo, Paik Boksu. Three stage in flight alignment with covariance shaping adaptive filter for the strapdown inertial navigation system (SDINS)[C]//AIAA Guidance, Navigation, and Control Conference. Toronto, Ontario, Canada, 2010.

[5] Miao Ling-juan, Shen Jun, Liu Wei. Initial alignment technique for SINS of vehicles in the moving state[J]. Journal of Beijing Institute of Technology, 2002, 11(3): 234-239.

[6] 楊波, 彭培林, 王躍鋼, 等. 里程計(jì)輔助捷聯(lián)慣導(dǎo)運(yùn)動(dòng)基座對(duì)準(zhǔn)方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2013, 21(3): 298-301. Yang Bo, Peng Pei-lin, Wang Yue-gang, et al. Alignment method of strapdown inertial navigation system aided by odometer on moving base[J]. Journal of Chinese Inertial Technology, 2013, 21(3): 298-301.

[7] 王躍鋼, 雷堰龍, 楊波, 等. 基于動(dòng)態(tài)零速條件的動(dòng)基座自對(duì)準(zhǔn)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2012, 20(6): 658-662. Wang Yue-gang, Lei Yan-long, Yang Bo, et al. Selfalignment on moving base under dynamic zero velocity conditions[J]. Journal of Chinese Inertial Technology, 2012, 20(6): 658-662.

[8] Georgy J, Noureldin A, Korenberg M, Bayoumi M. Low-cost three-dimensional navigation solution for RISS/ GPS integration using mixture particle filter[J]. IEEE Transactions on Vehicular Technology, 2010, 59(2): 599-615.

[9] 趙洪松, 繆玲娟, 沈軍. 捷聯(lián)慣導(dǎo)/里程計(jì)高精度組合導(dǎo)航算法[J]. 兵工學(xué)報(bào), 2014, 35(4): 433-440. Zhao Hong-song, Miao Ling-juan, Shen Jun. High accuracy algorithm for SINS/Odometer integrated navigation system[J]. Acta Armamentarii, 2014, 35(4): 433-440.

[10] Dissanayake G, Sukkarieh S, Nebot E, et al. The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications[J]. IEEE Transactions on Robotics and Automation, 2001, 17(5): 731-747.

Accurate independent alignment method of inertial navigation system for vehicles on moving base

YANG Bo, WANG Yue-gang, GUO Zhi-bin, SHAN Bin
（Department of Automation, The Second Artillery Engineering University, Xi’an 710025, China）

An accurate independent alignment method of inertial navigation system (INS) aided by odometer and vehicle motion constraints on moving base was studied. The outputs of odometer and INS attitude were adopted to achieve dead reckoning, and the velocity outputs of dead reckoning and INS were chosen as the one observation. Based on vehicle motion constraints, the projections of INS velocity outputs in the vehicle body’s transverse and vertical directions were chosen as the other observation. The errors of INS and odometer were taken as system states, and a Kalman filter was adopted to design the accurate alignment algorithm on moving base. Simulation results show that, under the moving-base condition, the estimation accuracies of east misalignment angle and north misalignment angle reach 0.21′ and 0.25′, respectively, and the estimation accuracy of upside misalignment angle reaches 2.16′.

inertial navigation system; dead reckoning; vehicle motion constraints; alignment on moving base; Kalman filter

V249.3

：A

2015-05-19；

：2015-09-22

第二炮兵工程大學(xué)基金（2013QNJJ021）

楊波（1980—），男，講師，博士后，從事慣性導(dǎo)航與組合導(dǎo)航研究。E-mail：yangbo8093@sina.com

1005-6734(2015)05-0580-05

10.13695/j.cnki.12-1222/o3.2015.05.005