李 旦，劉 明，胡華峰，羅 偉

(湖北航天技術(shù)研究院總體設(shè)計(jì)所，武漢 430040)

大角度斜置激光慣組與里程計(jì)組合導(dǎo)航方法

李 旦，劉 明，胡華峰，羅 偉

(湖北航天技術(shù)研究院總體設(shè)計(jì)所，武漢 430040)

針對(duì)激光慣組斜置條件下慣組坐標(biāo)系相對(duì)里程計(jì)坐標(biāo)系是大角度的情況，建立了兩坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，推導(dǎo)了里程計(jì)輸出轉(zhuǎn)換的安裝誤差補(bǔ)償模型，在補(bǔ)償模型基礎(chǔ)上給出了安裝誤差計(jì)算公式。以慣組位移與里程計(jì)位移之差作為量測(cè)值，建立了狀態(tài)和量測(cè)方程，運(yùn)用Kalman濾波技術(shù)實(shí)現(xiàn)慣組/里程計(jì)組合導(dǎo)航，最后通過(guò)跑車試驗(yàn)對(duì)提出的慣性/里程計(jì)組合導(dǎo)航方法進(jìn)行了驗(yàn)證，結(jié)果表明了該方法的有效性。

里程計(jì)；大角度；安裝誤差；組合導(dǎo)航

0 引言

為了滿足航天飛行器無(wú)準(zhǔn)備陣地、無(wú)依托發(fā)射的要求，必須具備能夠?qū)崿F(xiàn)自主定位定向的能力。慣性導(dǎo)航系統(tǒng)具有自主性及封閉性，能實(shí)時(shí)輸出載體的位置、速度及姿態(tài)等信息，但是它難以克服導(dǎo)航誤差隨時(shí)間累積的缺點(diǎn)。里程計(jì)是一種自主的距離信息測(cè)量傳感器，它與慣性導(dǎo)航系統(tǒng)組合之后能夠起到抑制慣性誤差發(fā)散的作用，因此采用慣性/里程計(jì)組合方案能夠建立高精度自主定位定向?qū)Ш较到y(tǒng)。

激光慣組精度高、穩(wěn)定性好，是實(shí)現(xiàn)高精度自主定位定向的重要組成部分。由于激光陀螺存在死區(qū)，激光慣組通常工作在機(jī)械抖動(dòng)偏頻模式下，在此模式下激光陀螺噪聲較大，難以實(shí)現(xiàn)高精度定向。因此將激光慣組斜置，使慣組的3個(gè)正交軸與水平面保持一個(gè)較大的角度，這樣慣組繞天向軸旋轉(zhuǎn)時(shí)激光陀螺可以工作在速率偏頻模式下，此模式下激光慣組避開(kāi)了激光陀螺死區(qū)，同時(shí)激光陀螺噪聲小、定向精度高。

里程計(jì)測(cè)量車輛行進(jìn)方向的路程增量，通過(guò)建立車體坐標(biāo)系(右前上)將里程計(jì)一維輸出轉(zhuǎn)換為三維輸出，再通過(guò)車體系到慣組坐標(biāo)系的轉(zhuǎn)換以及慣組坐標(biāo)系到導(dǎo)航系的轉(zhuǎn)換得到里程計(jì)在導(dǎo)航系下的投影輸出。通常情況下車體系到慣組坐標(biāo)系是小角度，可以按照小量三角函數(shù)近似的方式進(jìn)行轉(zhuǎn)換。當(dāng)激光慣組工作在斜置狀態(tài)下時(shí)，3個(gè)正交軸與水平面的夾角都是大角度，車體系到慣組坐標(biāo)系之間的角度關(guān)系不能再以小量的形式進(jìn)行三角近似，需建立坐標(biāo)系之間的安裝偏角關(guān)系進(jìn)行補(bǔ)償。

1 大角度條件下里程計(jì)輸出模型

里程計(jì)通過(guò)輸入軸與車輛變速齒輪連接，以脈沖形式輸出采樣間隔內(nèi)路程增量；定義車輛行駛過(guò)程中速度坐標(biāo)系為m系，m系X軸朝右、Y軸向前，Z軸與X、Y軸構(gòu)成右手直角坐標(biāo)系。里程計(jì)脈沖輸出在m系內(nèi)的表達(dá)式為

(1)

(2)

(3)

式中，δαψ表示航向安裝偏角誤差值，δαθ表示俯仰安裝偏角誤差值，δKod表示刻度系數(shù)誤差值。將式(3)中三角函數(shù)一階泰勒展開(kāi)，合并后忽略誤差量的二階及以上小量，得到：

sinαψcosαθ+δαψcosαψcosαθ-

δαθsinαψsinαθ

(4)

cosαψcosαθ-δαψsinαψcosαθ-

δαθcosαψsinαθ

(5)

(6)

將式(4)～式(6)代入式(3)有

(7)

(8)

得到里程計(jì)輸出計(jì)算值與真實(shí)值之間的關(guān)系為

(9)

2 慣組相對(duì)里程計(jì)安裝偏角標(biāo)定方法

當(dāng)慣組與里程計(jì)相對(duì)安裝偏角較大時(shí)，需要事先標(biāo)定安裝偏角。由式(2)得到

(10)

(11)

(12)

從而得到中間變量

(13)

式中,Δαφ是計(jì)算安裝偏角的中間量，進(jìn)一步得到安裝偏角計(jì)算公式為：

(14)

利用式(14)得到俯仰安裝偏角和航向安裝偏角后，再根據(jù)式(2)可將里程計(jì)輸出脈沖數(shù)轉(zhuǎn)換為路程增量。需要說(shuō)明的是上述計(jì)算安裝偏角的方法同樣適用于慣組安裝偏角是小角度的情況。

3 慣性/里程計(jì)組合導(dǎo)航

(15)

(16)

(17)

4 方法驗(yàn)證與分析

采用實(shí)際跑車試驗(yàn)數(shù)據(jù)對(duì)上述方法進(jìn)行驗(yàn)證，試驗(yàn)用激光慣組陀螺精度為0.003(°)/h，加速度計(jì)精度為5.0×10-5g，里程計(jì)安裝于車輛變速轉(zhuǎn)軸上，同步采集慣組數(shù)據(jù)和里程計(jì)脈沖數(shù)。根據(jù)第2節(jié)的方法標(biāo)定慣組與里程計(jì)安裝偏角，出發(fā)之前車輛靜止慣組完成初始對(duì)準(zhǔn)，之后車輛機(jī)動(dòng)2h，取前30min數(shù)據(jù)計(jì)算出航向安裝偏角αψ和俯仰安裝偏角αθ，再以計(jì)算得到的安裝偏角補(bǔ)償里程計(jì)輸出，與慣組數(shù)據(jù)進(jìn)行組合導(dǎo)航解算，得到組合導(dǎo)航輸出參數(shù)。跑車過(guò)程中同步采集差分GPS數(shù)據(jù)作為位置誤差比較參考基準(zhǔn)。

為了分析第2節(jié)所述方法計(jì)算的準(zhǔn)確性，以里程計(jì)輸出與慣性導(dǎo)航的姿態(tài)作航位推算得到導(dǎo)航位置輸出，將航位推算結(jié)果與GPS結(jié)果比較，如圖1所示。由于跑車路線為從南往北接近直線，因此近似認(rèn)為經(jīng)度方向誤差為車輛橫向誤差，緯度誤差為車輛縱向誤差。由車輛橫向誤差與高度誤差的數(shù)值可以看出，橫向誤差不超過(guò)40m，天向誤差不超過(guò)20m，因此航向安裝偏角αψ和俯仰安裝偏角αθ估計(jì)值是較為準(zhǔn)確的。里程計(jì)航位推算縱向方向誤差約為220m，主要依據(jù)里程計(jì)刻度系數(shù)誤差引起的線性漂移誤差在里程計(jì)航位推算過(guò)程中是逐漸累積的，不可消除，必須通過(guò)組合導(dǎo)航濾波估計(jì)對(duì)刻度系數(shù)誤差作修正后才能減少誤差積累。

圖1 里程計(jì)航位推算位置誤差Fig.1 Position error of odometer dead-reckoning

圖2為慣性/里程計(jì)組合導(dǎo)航位置誤差結(jié)果，從圖2可知組合導(dǎo)航修正了里程計(jì)刻度系數(shù)誤差，在緯度方向誤差較小，最大不超過(guò)20m，而經(jīng)度方向及高度方向的組合誤差與里程計(jì)航位推算的誤差相當(dāng)。這一方面說(shuō)明組合導(dǎo)航對(duì)里程計(jì)刻度系數(shù)誤差的修正作用，另一方面說(shuō)明安裝偏角計(jì)算值與實(shí)際情況相符，從而在橫向與垂向航位推算與組合導(dǎo)航效果一致，因?yàn)閼T性/里程計(jì)組合導(dǎo)航主要修正縱向誤差。

圖2 慣性/里程計(jì)組合導(dǎo)航位置誤差Fig.2 Position error of IMU/OD integrated navigation

5 結(jié)論

慣性/里程計(jì)組合是實(shí)現(xiàn)自主定位定向的重要方法，針對(duì)慣組安裝方式存在大角度的情況，建立了大角度安裝誤差補(bǔ)償數(shù)學(xué)模型，它同樣適用于小角度的情況。在此基礎(chǔ)上給出了安裝偏角計(jì)算公式，該方法合理可行、易操作，通過(guò)跑車試驗(yàn)對(duì)方法進(jìn)行驗(yàn)證，里程計(jì)航位推算橫向位置誤差與高度位置誤差表明安裝偏角計(jì)算方法的正確性，慣性/里程計(jì)組合導(dǎo)航位置誤差表明Kalman濾波對(duì)里程計(jì)刻度系數(shù)誤差的修正作用，通過(guò)濾波修正后提高了組合導(dǎo)航的精度。

[1]BevlyDM,ParkinsonB.CascadedKalmanfiltersforaccurateestimationofmultiplebiases,dead-reckoningnavigationandfullstatefeedbackcontrolofgroundvehicles[J].IEEETransactiononControlSystemTechnology, 2007,15(2):199-208.

[2] 嚴(yán)恭敏,秦永元,楊波. 車載航位推算系統(tǒng)誤差補(bǔ)償技術(shù)研究[J].西北工業(yè)大學(xué)學(xué)報(bào), 2006,24(1):26-31.

[3]RogersRM.Improvedheadingusingdualspeedsensorsforangularrateandodometryinlandnavigation[C]//IEEEPositionLocationandNavigationSymposium, 1998:177-184.

[4] 秦永元,張洪鉞,汪叔華. 卡爾曼濾波與組合導(dǎo)航原理[M].西安:西北工業(yè)大學(xué)出版社, 1998.

[5] 郭振西,繆玲娟,沈軍. 里程計(jì)組合的捷聯(lián)慣導(dǎo)系統(tǒng)運(yùn)動(dòng)基座對(duì)準(zhǔn)研究[J].北京理工大學(xué)學(xué)報(bào), 2005,25(1):67-70.

[6] 李增科，高井祥，王堅(jiān)，等. 基于位置修正的井下車輛INS/Odometer組合導(dǎo)航系統(tǒng)[J].煤炭學(xué)報(bào), 2013, 38(11): 2077-2083.

[7]IlyasM,YangY,QianQS,etal.Low-costIMU/Odometer/GPSintegratednavigationaidedwithtwoantennaeheadingmeasurementforlandvehicleapplication[C]// 25thChineseControlandDecisionConference(CCDC), 2013: 4521-4526.

[8]ZhuJ,GaoJ,LiK,etal.Mapbuildingforindoortrackedautonomousmobilerobotwithlaserrangefinderandelectroniccompass[C]//ICCSIT2011, 2011: 245-254.

[9]SeoJ，LeeHK,LeeJG，etal.LeverarmcompensationforGPS/INS/Odometerintegratedsystem[J].InternationalJournalofControl,AutomationandSystems, 2006, 4(2):247-254.

[10]HemerlyEM,SchadVR.ImplementationofaGPS/INS/Odometernavigationsystems[C]//ABCMSymposiumSeriesinMechatronics, 2008, 3:519-524.

[11]IlyasM,ZhangR,QianQS,etal.PerformanceanalysisofmediumaccuracyFOG-basedIMUintegratedwithopticalodometerforlandvehiclenavigation[J].AppliedMechanicsandMaterials, 2012, 245:334-339.

[12]XuC,GaoJ,WangJ.RobustKalmanfilteringofGNSS/INSintegratedsystem[J].JournalofChinaUniversityofMining&Technology, 2010, 39(5):773-778.

Research on Integrated Navigation of LIMU and Odometer with Large Angle

LI Dan, LIU Ming, HU Hua-feng, LUO Wei

(The General Design Institute of Hubei Aerospace Technology Academy, Wuhan 430040, China)

The relationship between IMU coordinate system and odometer coordinate system is established when large angel is posed due to their installation strategy. Mathematical model is chosen to acquire navigation parameters of odometer from its pulse, and the formula of large angle is put forward. Process equation and measure equation are acquired based on displacements of IMU and odometer by Kalman-filter, field test is carried out to validate the method and the results turn out to be effectual.

Odometer; Large angle; Installation error; Integrated navigation

10.19306/j.cnki.2095-8110.2016.06.004

2015-06-10；

2015-08-29。

李旦(1985 - ),男,工程師,主要從事慣性導(dǎo)航及組合導(dǎo)航方面的研究。E-mail:329364231@qq.com

V249.3

A

2095-8110(2016)06-0017-04