基于惯性参考系的动基座初始对准与定位导航

基于惯性参考系的动基座初始对准与定位导航 第33卷??第3期2011年3月 文章编号:1001??506X(2011)03??0618??04 系统工程与电子技术 SystemsEngineeringandElectronicsVol.33??No.3March2011 严恭敏,翁??浚,白??亮,秦永元 (西北工业大学自动化学院,陕西西安710072) ????摘??要:以惯性坐标系作为捷联惯导解算参考基准并综合外 测速度辅助信息,可有效隔离载体角运动和线运动干扰的影响,实现捷 联惯导系统(strapdowninertialnavigationsystem,SINS)动基座初始对准。 但现有算法的对准精度受载体位移影响,通过位移补偿降低重力矢量 在惯性系投影的偏斜误差,同时给出了基于惯性参考基准的SINS定位 方法,提高SINS动基座初始对准精度,并且具备对准过程中进行实时定 位导航的能力。车载实测数据分析表明,动基座方位对准精度达到 0.032??,相对定位精度约为行程的0.15%。 关键词:捷联惯导系统;动基座初始对准;惯性参考系;车载试验 中图分类号:V249.3????????文献标志 码:A????????DOI:10.3969/j.issn.1001??506X.2011.03.30 Initialin??movementalignmentandpositiondetermination basedoninertialreferenceframe YANGong??min,WENGJun,BAILiang,QINYong??yuan ——————————————————————————————————————————————— (DepartmentofAutomaticControl,NorthwesternPolytechnicalUniversity,Xi an710072,China) ????Abstract:Insomestrapdowninertialnavigationsystem(SINS)applications,bytakinginertialframeas computingreferenceandwiththeaidingofanexternalvelocitymeasuringequipment,theangularandlinearmovingdisturbancecanbeeasilyeliminatedtorealizeanin??movementinitialattitudeandheadingalignment.However,thealignmentaccuracyissubjecttothedisplacementofSINS.ThedisplacementofSINSiscompen??satedtodiminishthegravitydeflexionerrorthatexpressesininertialframe.Moreover,basedontheinertialref??erenceframe,anewSINSpositiondeterminationmethodisdeduced.Theinitialalignmentaccuracyisimprovedandareal??timepositiondeterminationabilityisreachedbyusingtheproposedmethod.Finally,somevehiculartestsarecarriedoutandtheresultsshowthatthein??movementheadingalignmentaccuracyreaches0.032??,andthepositionalaccuracyisabout0.15%oftherunningdistance. Keywords:strapdowninertialnavigationsystem(SINS);in??movementinitialalignment;inertialreferenceframe;vehiculartest 0??引??言 ????捷联惯导系统(strapdowninertialnavigationsystem,SINS)在进 ——————————————————————————————————————————————— 入导航任务之前必须先进行初始对准,建立起精确的平台初始指向,即姿态矩阵。基于小失准角线性误差模型的初始对准方法研究已经比较成熟,无论是在静基座、晃动干扰基座还是在运动基座环境下,但其先决条件是必须事先能够获得姿态阵粗略值。随着对惯性技术应用要求的不断提高,为了在不具备姿态阵粗略值情况下实现初始对准,学者们提出了大方位失准角(水平失准角依然为小角度)和大失准角等方法,研究重点主要集中在惯导非线性误差方程建模和非线性滤波两方面。纵观SINS初始对准方法,其核心问题是获取精确姿态阵,而通常认为速度和 收稿日期:2010??05??25;修回日期:2010??11??30。 [1??3] 位置初值容易获得。例如在进行SINS/全球定位系统 (globalpositionsystem,GPS)运动中初始对准时一般就不存在定位的困惑。但是,在有些应用领域,为了降低GPS人为干扰和无线电信号遮蔽等影响以增强导航系统的自主性,也常采用测速设备辅助(如里程仪和多普勒测速仪)的动基座初始对准方案。然而这些测速设备是相对于载体坐标系进行测量的,致使在姿态阵对准之前无法给出相对于导航系的定位结果。文献[4]利用数据存储和逆向导航算法,解决了姿态阵对准之后的精确定位问题,可是对准过程中依然不能提供实时的定位信息。 文献[5??6]介绍了一种基于惯性坐标系参考基准的初始对准思路,但未给出算法实现细节;文献[7??8]对该类算法作了详细推导,有效地——————————————————————————————————————————————— 隔离载体角晃动干扰的影响;文 (??)男,,博士,E??163. !619??!?? 献[9]在文献[7]的基础上将算法推广到动基座中去,利用外测速度补偿了载体运动的影响,达到了姿态阵对准的目 的,但未能解决对准过程中实时定位问题,并且运动位移对姿态阵对准精度有影响。在文献[9]的基础上,本文作了以下改进:通过运动位移补偿从而削弱了由此引起的惯性系重力偏斜误差,给出了基于惯性参考基准的SINS定位方法。因此,无需任何初始姿态信息、数据存储和复杂的非线性建模与滤波计算,本文除实现动基座姿态阵初始对准外,同时具备对准过程中实时定位导航的能力。 2??姿态阵对准 动基座下的时变捷联姿态矩阵Cnb(t)可拆写成如下4个矩阵链乘形式 n00 Cnb(t)=Cn0(t)Ci0(t)Cib0Cb(t) n n i ib0 (7) 式中,Cnn0(t)与载体实时移动的经度 t和纬度Lt有关;Ci0(t) ——————————————————————————————————————————————— 由载体初始经度 纬度L0以及对准时间t确定;利用陀螺0、输出的角运动信息对Cb(t)进行实时跟踪。求解上述3个矩阵相对比较容易,可参见文献[9]。这里指出的是,当载体移动经度变化量! t= t- 0和纬度变化量!Lt=Lt-L0均是小角度时,近似有 C(t)?I-n n0 ib0 1??相关坐标系与矢量运算介绍 基于惯性参考系的动基座初始对准中涉及到较多的坐标系,包括:地心惯性坐标系(i系)、地球坐标系(e系)、导航坐标系(n系)、捷联惯导坐标系(b系)、初始时刻惯性坐标系(i0系)、初始时刻地球坐标系(e0系)、初始时刻导航坐标系(n0系)、初始时刻捷联惯导惯性坐标系(ib0系)等,具体定义可参见文献[9]。 为了方便后文推导,先简要介绍一些有关的矢量运算关系。 假设v(??)是一时变三维矢量,它在参考坐标系r和动坐标系m的投影分别表示为vr(??)和vm(??),且设坐标系m r 至r的坐标变换矩阵为Cm(??),则在??时刻有如下坐标变换 -! tsinL0 0-!Lt ! tcosL0 !Lt0 ——————————————————————————————————————————————— ! tsinL0-! tcosL0 (8) 在式(7)中求解两惯性坐标系之间旋转变化关系的常值矩阵Ci0是初始对准的关键,下面详细介绍它的计算过程。b0 在如下惯导比力方程中 b vn(t)=Cnb(t)fsf(t)-nnnn[??in(t)+??ie(t)]?v(t)+g ! i (9) 若将 d[Cib0(t)vb0(t)]v(t)== dt n!ib0ib0Cib0(t)[vib0(t)+??nib0(t)?v(t)] !n ni 关系 vr(??)=Crm(??)vm(??) ????考虑到矩阵微分公式 C(??)=C(??)[??(??)?] 与叉乘运算的坐标变换公式 m ——————————————————————————————————————————————— Crm(??)[??)?vm(??)]=rm(??! (1)(2) rmrmmrm 代入,并且方程两边同时左乘Cinb0(t),可得 !ib0 b0ib0b0nb v(t)+??nib0(t)?v(t)=Cn(t){Cb(t)fsf(t)- ii [??in(t)+??ie(t)]?v(t)+g} (3) b0b0 考虑到??ni(t)=-??in(t),式(10)移项化简整理得到b0 nnnn (10) ii [C(??)??(??)]?[C(??)v(??)] 若将式(1)两边同时微分,可得 v(??)=C(??)v(??)+C(??)v(??)= m Crm(??)v(??)+Crm(??)[??rm(??)?vm(??)]= !m !r ——————————————————————————————————————————————— r mmrmrm m !ib0 0b0 v(t)-Cb(t)fsf(t)+(Cib00??ie)?v(t)= ib0 biii r m !m ! rm m Ci0Cn(t)Cn(t)g ib0i0n0n (11) 0n 利用式(8)和gn=[00-g]T,可将式(11)右端的Cnn(t)g C(??)v(??)+??(??)?[C(??)v(??)] 再对式(4)两边从0~t时间段内积分,得到 r ——————————————————————————————————————————————— m !m 部分改写成 (4) Cn0(t)gn?gn-g![! tcosL0 n n r rmrm m !Lt0]T(12) n ##?? ! t vr(??)d??= t0 r rm # t 易知,经度或纬度变化角度乘以地球半径Re后可近似为东 ——————————————————————————————————————————————— 向或北向的距离变化量,记pE0(t)=Re! tcosL0和pN(t) r Cm(??)vm(??)d??+ ! (??)?[C(??)v(??)]d?? m rm m =Re!Lt。一般车载导航系统在高度方向上的距离变化不 (5) 大,因此在式(12)中添加高度分量pU0(t)后,可近似得 Cn0(t)g=g-g-n n n n n 假设矢量初始v(0)=v(0)=0,则有Crm(t)vm(t),因此式(5)可整理成 t r # t0 vr(??)d??=vr(t)= ——————————————————————————————————————————————— ! n ![pE0(t)Re n pN0(t) T n 0] T ? ##?? t0r Crm(??)vm(??)d??=Crm(t)vm(t)-r rm ! n ![pE0(t)Re pN0(t)pU0(t)] n ??gn-!pn0(t) Re (13) ——————————————————————————————————————————————— ib0 ib0i0 i0n0 (??)?[C(??)v(??)]d??rm m (6) 再将式(13)代入式(11),并应用关系式p(t)?CC(t)pn0(t)(参见后文式(17)),整理可得 b00ib0 vib0(t)-Cb(t)fbsf(t)+(Ci0??ie)?v(t)+ 式(6)也就是分部积分公式的简单变形。若坐标系m相对r不动,则有??rm(??)=0,即式(6)右边第二项为0,即可以将!ib0 ii iiin !pb0(t)=Cib00Cn(t)g00 (14) ??!??!620 系统工程与电子技术第33卷?? n ! 式(14)左边第3项为载体移动和地球自转引起的哥氏力补偿项;第4项相当于载体移动造成的重力偏斜补偿项,虽然这两项一般为小——————————————————————————————————————————————— 量,但是本文正是通过对它们的补偿进一步提高对准精度。 定义 pb0(t)= i # t Cib00(??)pib0(??)d??= # 0n0 t Cb0(??)Cb)vib0(??)d??=ib0(?? n # t0 n Cb0(??)vb(??)d??=pn0(t) n0 n 它表示载体在初始时刻导航坐标系n0下的位移矢量。在 t # 0ib0ib0 ——————————————————————————————————————————————— p?? vb0(??)d??= i # t0ib0 C(t)v(t)d?? ib0 b b 式(16)右边第2项中,有??n0ib0(??)=-??ie为常值矢量,并且还可将Ci0(??)近似为t0时刻的常值矩阵,所以式(16)近b0似为 ??pn0(t)?Ci0(t)pib0(t)+??ie0?[Cib00(t0)p#b0(t)]??b0载体以常值速度vib0行驶,则在t时刻有 00ib0 |??ie?Cib0(t0)p(t)]|%# p#(t)= 式中,v(t)表示测速设备(如里程仪或多普勒测速仪等)提供 的载体系速度。为了避免外测速度微分和提高抗干扰能力,对式(14)实施两次积分,并令所有积分初值均为0,可得 V(t)=CU(t) 式中 V(t)=p(t)-ib0 ——————————————————————————————————————————————— ib0 ib0 ib0i0 i0 b #p(t)= #p t0t0 (??)d??(??)d?? nnni (17) # ib0 式(17)中,可对右边第2项即修正项的量级作出估计:假设 n n ib02 ??tiev2 (15) ib0i0 现取v ib0 ——————————————————————————————————————————————— =10m/s和t=1000s,则修正项最大约为360m, 可见高精度定位时对该修正项进行补偿是非常必要的。根据式 (17)可得经度、纬度和高度实时定位结果为 N0E0,Lt=L0+ t= 0+ RN+h0(RM+h0)cosL0 ht=h0+pU0(t) n n n ##C t0 t ?? ib0 b (??1)f(??1)d??1d??+(C i!p??b0(t)Re??0 n Cin(??1)gd??1d?? 00 b sf??)?i0ie ——————————————————————————————————————————————— ib0p#(t)+ (18) Ui(t)= 在式(15)中,可利用两个不同对准时刻t1和t2下的两组计算值,即Vib0(t1)、Ui(t1)和Vib0(t2)、Ui(t2),通过构造矩阵方程 ## 有了实时定位之后,可进一步准确计算式(7)中与定位相关 n0ib0 的矩阵Cn(t),因而认n0(t)。由于式(17)的p(t)来自于p 为式(18 ) 的定位是基于惯性参考系基准转换获得的,本文称为基于惯性参考基准的定位导航方法。至此,便完成了姿态阵对准和定位导航。 求解Ci0b0,算法比较简单,可参见文献[9]。但是,由于Vb0(t)中第3项包含C未知,因此需采取迭代的方法实现,即不妨先忽略Vib0(t)中的Ciib00项,求得Ciib00的粗略值,之后再次利用完整的Vib0(t)表达式,精确求解Cib00。 i ib0 i0 ii 4??试验验证与讨论 ——————————————————————————————————————————————— 3??定位导航 在分部积分式(6)中,若分别以矩阵Cib0(t)和矢量pb0(t)代替,得到 n0 i ##?? n0 t0 t Ci0(??)pib0(??)d??=Cib00(t)pib0(t)-b0 n0 n0ib0 ! n (??)?[Cib00(??)pib0(??)]d?? n (16) 式中等号左边积分即为 ????此外,还进行了一次时间4000s行程46km的试验,最大行车速度达25m/s,图1为方位和定位误差结果,其中定位误差中的毛刺是载车通过桥和隧道等情形时GPS丢 星造成的,由于纯惯导和航位推算高度通道均是发散的,因而图中未给出高度误差。由图1可以看出,方位误差收敛后精度约为0.05??;——————————————————————————————————————————————— 定位误差在25min之前增长缓慢, !621??!?? 基本随距离呈线性发散趋势,符合航位推算误差增长的规律,但30min之后定位误差明显增大了。由于地球自转引起C(??)随时间缓慢变化,而在式(17)中将C(??)近似为常值Cib00(t0),给位移矢量pn0(t)计算带来了一定的误差。当时间比较短时地球转过角度很小,所以Cib00(??)变化很小,可近似为常值,对定位误差影响也很小;而当时间比较长时地球可转过几度甚至十几度的角度,这时Cib00(??)变化较大,近似为常值时误差增大 ,从而导致定位误差越来越大。因此,本文定位结果不适合于长时间情况,而对于20min~30min以内的动基座实时定位导航应用场合具备较高的精度。 nn nn0 ib0 n0ib0 效隔离载体角运动和线运动干扰的影响。在文献[9]的基础上,本文进行了以下改进:通过运动位移补偿从而削弱了由此引起的惯性系重力偏斜误差,给出了基于惯性基准的惯导系统定位方法。因此,无需任何初始姿态信息、数据存储和复杂的非线性建模与滤波计算,本文除实现动基座姿态阵初始对准外,同时具备对准过程中实时定位导航的能力。最后,利用实测数据验证了所提方法具有较高的方位对准和——————————————————————————————————————————————— 定位导航精度。 参考文献: [1]DmitriyevSP,StepanovOA,ShepelSV.Nonlinearfiltering methodsapplicationinINSalignment[J].IEEETrans.onAerospaceandElectronicSystems,1997,33(1):260??271.[2]WuYX,HuXP,WuMP,etal.Strapdowninertialnavigation usingdualquaternionalgebra:erroralalysis[J].IEEETrans.onAerospaceandElectronicSystems,2006,42(1):259??266.[3]JulierSJ,UhlmannJK.Unscentedfilteringandnonlinearesti?? mation[J].Proc.oftheIEEEAerospaceandElectronicSys??tems,2004,92(3):401??422. [4]严恭敏,严卫生,徐德民.逆向导航算法及其在捷联罗经动基座 初始对准中的应用[C]&程代展,李川.第27届中国控制会议 论文集(第5册).北京:北京航空航天大学出版 社,2008:724??729.(YanGM,YanWS,XuDM.OnreversenavigationalgorithmanditsapplicationtoSINSgyro??compassin??movementalignment[C]&ChengDZ,LiC.Proc.ofthe27thChinese 图1??长时间的动基座对准方位误差与定位误差 ControlConference,2008:724??729.) ——————————————————————————————————————————————— [5]GaiffeT,CottreauY,FaussotN,etal.Highlycompactfiberoptic gyrocompassforapplicationsatdepthsupto3000meters[C]&Proc.oftheInternationalSymposiumonUnderwaterTechnology,2000:155??160. [6]NapolitanoF,GaiffeT,CottreauY.PHINS:thefirsthighper?? formancesinertialnavigationsystembasedonfiberopticgyroscopes[C]&Proc.of9thSaintPetersburgInternationalConferenceonIntegratedNavigationSystems,2002:296??304.[7]秦永元,严恭敏,顾冬晴,等.摇摆基座上基 于信息的捷联惯导粗 对准研究[J].西北工业大学学 报,2005,23(5):681??684.(QinYY,YanGM,GuDQ,etal.AcleverwayofSINScoarsealignmentdespiterockingship[J].JournalofNorthwesternPolytechnicalUniversity,2005,23(5):681??684.) [8]练军想,吴文启,吴美平,等.车载SINS行进间初始对准方法[J]. 中国惯性技术学 报,2007,15(2):155??159.(LianJX,WuWQ,WuMP,etal.SINSinitialalignmentalgorithmformarchingvehi??cles[J].JournalofChineseInertialTechnology,2007,15(2):155??159.) [9]严恭敏,秦永元,卫育新,等.一种适用于SINS动基座初始对准 的新算法[J].系统工程与电子技 术,2009,31(3):634??637.(YanGM,QinYY,WeiYX,etal.Newinitialalignmental——————————————————————————————————————————————— gorithmforSINSonmovingbase[J].SystemsEngineering:) 为了消除长时间对准对定位导航精度的影响,现提出一种处理思路如下:直接由式(11)两次积分并且类似于式(16),可推导出如下两个严格意义的等式p(t)-ib0 ##C t0 ?? ib0 b (??1)f(??1)d??1d??+(CCib00 ni b sf ib0i0 ??)?p#(t)= ib0 i0ie ## t0 ib0i0 ?? n ——————————————————————————————————————————————— Cn0(??1)gd??1d??n i i (19) p(t)=Cib0(t)pb0(t)+ # t0 i n ??in(??)?[Cib0(??)pib0(??)]d?? i0 n n (20) 虽然式(19)左边的C和右边的C(??1)未知,但是在对准过程中的某tk时刻,Cib00可使用前一时刻的计算值,并且Cn0(??1)中的定位信息可通过式(20)获得,而计算p(tk)时,式(20)右边的??in(??)和Cib0(??)均可使用tk-1时刻的速度和定位值计算。依此形成递推算法,能较好解决动基座条件下同时完成姿态阵对准和实时定位的问题。关于该递推算法的具体实现和效果验证还需较多篇幅,将另行文作详细介绍。 n n ——————————————————————————————————————————————— n i 5??结??论 在惯导系统动基座初始对准中,若以惯性坐标系作为 ———————————————————————————————————————————————