李 旦,劉 明,胡華峰,羅 偉
(湖北航天技術(shù)研究院總體設(shè)計所,武漢 430040)
大角度斜置激光慣組與里程計組合導航方法
李 旦,劉 明,胡華峰,羅 偉
(湖北航天技術(shù)研究院總體設(shè)計所,武漢 430040)
針對激光慣組斜置條件下慣組坐標系相對里程計坐標系是大角度的情況,建立了兩坐標系之間的轉(zhuǎn)換關(guān)系,推導了里程計輸出轉(zhuǎn)換的安裝誤差補償模型,在補償模型基礎(chǔ)上給出了安裝誤差計算公式。以慣組位移與里程計位移之差作為量測值,建立了狀態(tài)和量測方程,運用Kalman濾波技術(shù)實現(xiàn)慣組/里程計組合導航,最后通過跑車試驗對提出的慣性/里程計組合導航方法進行了驗證,結(jié)果表明了該方法的有效性。
里程計;大角度;安裝誤差;組合導航
為了滿足航天飛行器無準備陣地、無依托發(fā)射的要求,必須具備能夠?qū)崿F(xiàn)自主定位定向的能力。慣性導航系統(tǒng)具有自主性及封閉性,能實時輸出載體的位置、速度及姿態(tài)等信息,但是它難以克服導航誤差隨時間累積的缺點。里程計是一種自主的距離信息測量傳感器,它與慣性導航系統(tǒng)組合之后能夠起到抑制慣性誤差發(fā)散的作用,因此采用慣性/里程計組合方案能夠建立高精度自主定位定向?qū)Ш较到y(tǒng)。
激光慣組精度高、穩(wěn)定性好,是實現(xiàn)高精度自主定位定向的重要組成部分。由于激光陀螺存在死區(qū),激光慣組通常工作在機械抖動偏頻模式下,在此模式下激光陀螺噪聲較大,難以實現(xiàn)高精度定向。因此將激光慣組斜置,使慣組的3個正交軸與水平面保持一個較大的角度,這樣慣組繞天向軸旋轉(zhuǎn)時激光陀螺可以工作在速率偏頻模式下,此模式下激光慣組避開了激光陀螺死區(qū),同時激光陀螺噪聲小、定向精度高。
里程計測量車輛行進方向的路程增量,通過建立車體坐標系(右前上)將里程計一維輸出轉(zhuǎn)換為三維輸出,再通過車體系到慣組坐標系的轉(zhuǎn)換以及慣組坐標系到導航系的轉(zhuǎn)換得到里程計在導航系下的投影輸出。通常情況下車體系到慣組坐標系是小角度,可以按照小量三角函數(shù)近似的方式進行轉(zhuǎn)換。當激光慣組工作在斜置狀態(tài)下時,3個正交軸與水平面的夾角都是大角度,車體系到慣組坐標系之間的角度關(guān)系不能再以小量的形式進行三角近似,需建立坐標系之間的安裝偏角關(guān)系進行補償。
里程計通過輸入軸與車輛變速齒輪連接,以脈沖形式輸出采樣間隔內(nèi)路程增量;定義車輛行駛過程中速度坐標系為m系,m系X軸朝右、Y軸向前,Z軸與X、Y軸構(gòu)成右手直角坐標系。里程計脈沖輸出在m系內(nèi)的表達式為
(1)
(2)
(3)
式中,δαψ表示航向安裝偏角誤差值,δαθ表示俯仰安裝偏角誤差值,δKod表示刻度系數(shù)誤差值。將式(3)中三角函數(shù)一階泰勒展開,合并后忽略誤差量的二階及以上小量,得到:
sinαψcosαθ+δαψcosαψcosαθ-
δαθsinαψsinαθ
(4)
cosαψcosαθ-δαψsinαψcosαθ-
δαθcosαψsinαθ
(5)
(6)
將式(4)~式(6)代入式(3)有
(7)
(8)
得到里程計輸出計算值與真實值之間的關(guān)系為
(9)
當慣組與里程計相對安裝偏角較大時,需要事先標定安裝偏角。由式(2)得到
(10)
(11)
(12)
從而得到中間變量
(13)
式中,Δαφ是計算安裝偏角的中間量,進一步得到安裝偏角計算公式為:
(14)
利用式(14)得到俯仰安裝偏角和航向安裝偏角后,再根據(jù)式(2)可將里程計輸出脈沖數(shù)轉(zhuǎn)換為路程增量。需要說明的是上述計算安裝偏角的方法同樣適用于慣組安裝偏角是小角度的情況。
(15)
(16)
(17)
采用實際跑車試驗數(shù)據(jù)對上述方法進行驗證,試驗用激光慣組陀螺精度為0.003(°)/h,加速度計精度為5.0×10-5g,里程計安裝于車輛變速轉(zhuǎn)軸上,同步采集慣組數(shù)據(jù)和里程計脈沖數(shù)。根據(jù)第2節(jié)的方法標定慣組與里程計安裝偏角,出發(fā)之前車輛靜止慣組完成初始對準,之后車輛機動2h,取前30min數(shù)據(jù)計算出航向安裝偏角αψ和俯仰安裝偏角αθ,再以計算得到的安裝偏角補償里程計輸出,與慣組數(shù)據(jù)進行組合導航解算,得到組合導航輸出參數(shù)。跑車過程中同步采集差分GPS數(shù)據(jù)作為位置誤差比較參考基準。
為了分析第2節(jié)所述方法計算的準確性,以里程計輸出與慣性導航的姿態(tài)作航位推算得到導航位置輸出,將航位推算結(jié)果與GPS結(jié)果比較,如圖1所示。由于跑車路線為從南往北接近直線,因此近似認為經(jīng)度方向誤差為車輛橫向誤差,緯度誤差為車輛縱向誤差。由車輛橫向誤差與高度誤差的數(shù)值可以看出,橫向誤差不超過40m,天向誤差不超過20m,因此航向安裝偏角αψ和俯仰安裝偏角αθ估計值是較為準確的。里程計航位推算縱向方向誤差約為220m,主要依據(jù)里程計刻度系數(shù)誤差引起的線性漂移誤差在里程計航位推算過程中是逐漸累積的,不可消除,必須通過組合導航濾波估計對刻度系數(shù)誤差作修正后才能減少誤差積累。
圖1 里程計航位推算位置誤差Fig.1 Position error of odometer dead-reckoning
圖2為慣性/里程計組合導航位置誤差結(jié)果,從圖2可知組合導航修正了里程計刻度系數(shù)誤差,在緯度方向誤差較小,最大不超過20m,而經(jīng)度方向及高度方向的組合誤差與里程計航位推算的誤差相當。這一方面說明組合導航對里程計刻度系數(shù)誤差的修正作用,另一方面說明安裝偏角計算值與實際情況相符,從而在橫向與垂向航位推算與組合導航效果一致,因為慣性/里程計組合導航主要修正縱向誤差。
圖2 慣性/里程計組合導航位置誤差Fig.2 Position error of IMU/OD integrated navigation
慣性/里程計組合是實現(xiàn)自主定位定向的重要方法,針對慣組安裝方式存在大角度的情況,建立了大角度安裝誤差補償數(shù)學模型,它同樣適用于小角度的情況。在此基礎(chǔ)上給出了安裝偏角計算公式,該方法合理可行、易操作,通過跑車試驗對方法進行驗證,里程計航位推算橫向位置誤差與高度位置誤差表明安裝偏角計算方法的正確性,慣性/里程計組合導航位置誤差表明Kalman濾波對里程計刻度系數(shù)誤差的修正作用,通過濾波修正后提高了組合導航的精度。
[1]BevlyDM,ParkinsonB.CascadedKalmanfiltersforaccurateestimationofmultiplebiases,dead-reckoningnavigationandfullstatefeedbackcontrolofgroundvehicles[J].IEEETransactiononControlSystemTechnology, 2007,15(2):199-208.
[2] 嚴恭敏,秦永元,楊波. 車載航位推算系統(tǒng)誤差補償技術(shù)研究[J].西北工業(yè)大學學報, 2006,24(1):26-31.
[3]RogersRM.Improvedheadingusingdualspeedsensorsforangularrateandodometryinlandnavigation[C]//IEEEPositionLocationandNavigationSymposium, 1998:177-184.
[4] 秦永元,張洪鉞,汪叔華. 卡爾曼濾波與組合導航原理[M].西安:西北工業(yè)大學出版社, 1998.
[5] 郭振西,繆玲娟,沈軍. 里程計組合的捷聯(lián)慣導系統(tǒng)運動基座對準研究[J].北京理工大學學報, 2005,25(1):67-70.
[6] 李增科,高井祥,王堅,等. 基于位置修正的井下車輛INS/Odometer組合導航系統(tǒng)[J].煤炭學報, 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 - ),男,工程師,主要從事慣性導航及組合導航方面的研究。E-mail:329364231@qq.com
V249.3
A
2095-8110(2016)06-0017-04