匣亟互亘司篓釜篓鐾銎兰兰鳖兰竺坠
文章编号：１６７１—４５９８（２０１１）１０—２５０１—０３中围分类号：Ｖ２４９．３文献标识码：Ａ基于捷联惯导／里程计的车载高精度定位定向方法研究
杨波１，王跃钢１，彭辉煌２
（１．第二炮兵工程学院自动控制系，陕西西安７１００２５；２．第二炮兵驻８６０２厂军事代表室。湖北孝感４３２１００）
摘要：研究了一种基于捷联惯导系统和里程计进行车载高精度定位定向的方法；采用里程计与捷联惯导中的陀螺仪构成航位推算系统，建立了航位推算系统的误差模型ｌ为了增强航位推算姿态误差的估计效果。将载车姿态与位置信息一起作为量测。构建组合定位定向的量测方程ｌ采用Ｓａｇｅ－Ｈｕｓａ自适应滤波设计捷联惯导／里程计组合定位定向算法，以增强算法对外界环境和载车机动的鲁棒性；仿真结果表明．基于捷联惯导／里程计的车载定位定向方法能够达到士２０．４ｍ（３ｄ）的定位精度，航向精度达到土３．１’（３０）．水平姿态精度达到士０．６’（３ｄ）。关键词：捷联惯导Ｉ里程计，定位定向ｌ自适应滤波ＲｅｓｅａｒｃｈｏｎＰｒｅｃｉｓｅＰｏｓｉｔｉｏｎａｎｄＯｒｉｅｎｔａｔｉｏｎＤｅｔｅｒｍｉｎａｔｉｏｎｆｏｒ
ＶｅｈｉｃｌｅＢａｓｅｄｏｎＳＩＮＳ／Ｏｄｏｍｅｔｅｒ
ＹａｎｇＢ０１ＷａｎｇＹｕｅｇａｎ９１ＰｅｎｇＨｕｉｈｕａｎ９２（１．ＤｅｐａｒｔｍｅｎｔｏｆＡｕｔｏｍａｔｉｏｎ，ＳｅｃｏｎｄＡｒｔｉｌｌｅｒｙＥｎｇｉｎｅｅｒｉｎｇＣｏｌｌｅｇｅ，Ｘｉ’ａｎ７１００２５，Ｃｈｉｎａ；２．ＭｉｌｉｔａｒｙＤｅｐｕｔｙＣＩｆｆｉｃｅｏｆＳｅｃｏｎｄＡｒｔｉｌｌｅｒｙｉｎＰｌａｎｔ８６０２，Ｘｉａｏｇａｎ４３２１００，Ｃｈｉｎａ）
Ａｂｓｔｒａｃｔ：Ａｍｅｔｈｏｄｏｆｐｒｅｃｉｓｅｐｏｓｉｔｉｏｎａｎｄｏｒｉｅｎｔａｔｉｏｎｄｅｔｅｒｍｉｎａｔｉｏｎｆｏｒｖｅｈｉｃｌｅｂａｓｅｄｓｔｒａｐｄｏｗｎｉｎｅｒｔｉａｌｎａｖｉｇａｔｉｏｎｓｙｓｔｅｍ（ＳＩＮＳ）
ａｎｄｏｄｏｍｅｔｅｒｗａｓｒｅｓｅａｒｃｈｅｄｉｎｔｈｉｓｐａｐｅｒ．ＴｈｅｏｄｏｍｅｔｅｒａｎｄｇｙｒｏｓｃｏｐｅｓｉｎＳＩＮＳｗｅｒｅａｄｏｐｔｅｄｔＯｃｏｎｓｔｒｕｃｔｔｈｅｄｅａｄｒｅｃｋｏｎｉｎｇｓｙｓｔｅｍ，ａｎｄ
ｍｏｄｅｌｓｏｆｔｈｅｄｅａｄｒｅｃｋｏｎｉｎｇｓｙｓｔｅｍｗｅｒｅｂｕｉｌｔ．ＩｎｏｒｄｅｒｔＯｅｎｈａｎｃｅｔｈｅｅｓｔｉｍａｔｅｅｆｆｅｃｔｏｆａｔｔｉｔｕｄｅｉｎｄｅａｄｒｅｃｋｏｎｉｎｇ。ｔｈｅａｔｔｉｔｕｄｅａｎｄｐｏｓｉｔｉｏｎｏｆｔｈｅｖｅｈｉｃｌｅｗｅｒｅｃｈｏｚｅｎｔｈｅｏｂｓｅｒｖａｔｉｏｎｓ，ａｎｄｔｈｕｓｔｈｅｏｂｓｅｒｖａｔｉｏｎｅｑｕａｔｉｏｎｏｆｉｎｔｅｇｒａｔｅｄｐｏｓｉｔｉｏｎａｎｄｏｒｉｅｎｔａｔｉｏｎｄｅｔｅｒｍｉ—ｎａｔｉｏｎｗｅｒｅｂｕｉｌｔ．Ｓａｇｅ－Ｈｕｓａｓｅｌｆ－ａｄａｐｔｉｖｅｆｉｌｔｅｒｉｎｇｗａｓａｄｏｐｔｅｄｉｎｔｈｅｄｅｓｉｇｎｏｆＳＩＮＳ／ｏｄｏｍｅｔｅｒｉｎｔｅｇｒａｔｅｄｐｏｓｉｔｉｏｎａｎｄｏｒｉｅｎｔａｔｉｏｎｄｅｔｅｒｍｉ—ｎａｔｉｏｎａｌｇｏｒｉｔｈｍ，ｗｈｉｃｈｅｎｈａｎｃｅｔｈｅｒｏｂｕｓｔｎｅｓｓｔｏｏｕｔｓｉｄｅｅｎｖｉｒｏｎｍｅｎｔａｎｄｍａｎｅｕｖｅｒｏｆｔｈｅｖｅｈｉｃｌｅ．Ｓｉｍｕｌａｔｉｏｎｒｅｓｕｌｔｓｓｈｏｗｅｄｔｈａｔ，ｐｏ—
ｓｉｔｉｏｎａｃｃｕｒａｃｙｏｆｔｈｉｓｐｏｓｉｔｉｏｎａｎｄｏｒｉｅｎｔａｔｉｏｎｄｅｔｅｒｍｉｎａｔｉｏｎｍｅｔｈｏｄｆｏｒｖｅｈｉｃｌｅｂａｓｅｄＳＩＮＳ／ｏｄｏｍｅｔｅｒｒｅａｃｈｅｄ士２０．４ｍ（３０）．ｈｅａｄｉｎｇ
ｃｕｒａｃｙｒｅａｃｈｅｄ士３．１’（３口），ａｎｄｈｏｒｉｚｏｎｔａｌａｔｔｉｔｕｄｅａｃｃｕｒａｃｙｒｅａｃｈｅｄ士０．６’（３ｄ）．Ｋｅｙｗｏｒｄｓ：ｓｔｒａｐｄｏｗｎｉｎｅｒｔｉａｌｎａｖｉｇａｔｉｏｎｓｙｓｔｅｍ（ＳＩＮＳ）；ｏｄｏｍｅｔｅｒ；ｐｏｓｉｔｉｏｎａｎｄｏｒｉｅｎｔａｔｉｏｎｄｅｔｅｒｍｉｎａｔｉｏｎ；ｓｅｌｆ—ａｄａｐｔｉｖｅｆｉｌｔｅｒ
Ｏ引言
现代战争中，为了增强车载武器系统的快速机动性，要求诸如发射车、指挥车、装甲车等特殊军用车辆具备精确、快速的定位定向能力，以期实现。停车就打”，甚至“边走边打”；同时，还要求车载定位定向系统在战场恶劣环境下具备较强的独立自主性和抗干扰性。目前，国内外广泛研究采用了以卫星／航位推算或惯导／卫星组合导航为主要模式的车载定位定向技术Ｄ－３３。但是，由于卫星导航信号容易被遮挡或干扰，在战场环境下不能绝对依赖，而一旦卫星导航失效，仅靠航位推算或惯导本身无法实现较高精度导航定位，因此上述两种模式的独立自主性和抗干扰能力较差。针对自主性和抗干扰问题，采用惯导与里程计进行组合导航是一个有效的解决办法。文献［４］将捷联惯导与里程计输出的速度之差作为组合导航量测。有效减缓了定位误差的发散
收稿日期：２０１１—０３—１８ｌ傣回日期：２０１１—０５—１０。基金项目：中国博士后科学基金资助（２００９０４６１４７１）．作者简介：杨渡（１９８０一）。男．江苏省滨海县人，讲师，博士后，主
要从事惯性导航与组合导航等方向的研究．
速度，但随着时间推移定位误差仍然明显发散，而且未考虑里程计刻度系数误差的影响。事实上，里程计刻度系数误差对航位推算精度的影响比较大［５］，必须对其建模并进行估计补偿。文献［６］以捷联惯导与航位推算的位置之差作为量测没计组合导航算法，由于直接将捷联惯导姿态矩阵作为航位推算的姿态矩阵，必将导致航位推算误差会随着惯导姿态误差的发散而较快增大，从而也影响了最终的组合导航精度。为此，本文利用里程计输出与捷联惯导中陀螺仪输出独立进行姿态解算，在一定程度上减缓航位推算姿态误差的发散程度；对里程计刻度系数误差进行建模并列人系统状态，引入载车姿态信息作为组合导航量测之一，以提高姿态误差的估计补
偿效果；采用Ｓａｇｅ－Ｈｕｓａ自适应滤波设计捷联惯导／里程计组合定位定向算法，以增强系统对外界环境和载车机动等的鲁棒性。
ｌ基于捷联惯导／里程计的车载定位定向原理
在车载定位定向系统中，采用东一北一天地理坐标系作为导航坐标系。捷联惯导与里程计均捷联安装在载车上。其中里程计安装在载车的前轮轴上。因为后轮作为驱动轮，容易出现打滑或滑行而引起较大误差；而前轮是从动轮，出现上述现象的情况相对较少［７】．捷联惯导经过导航解算输出载车的姿态、位
万方数据·２５０２·计算机测量与控制第１９卷置和速度信息，里程计采用路程增量输出。首先，利用里程计与捷联惯导中的陀螺仪构成航位推算系统，经过解算获得载车的姿态、位置和速度信息；然后，将捷联惯导输出的姿态、位置与航位推箅系统输出的对应信息相减作为量测，送入组合定位定向滤波器（采用Ｓａｇｅ－Ｈｕｓａ自适应滤波）进行滤波；经过滤波计算，获得系统状态（即捷联惯导与航位推算系统误差）的最优估计值；最后，利用所获得的系统误差估计值实时对捷联惯导和航位推算系统进行误差校正，并将校正后的捷联惯导系统输出作为整个车载定位定向系统的输出。因此，基于捷联惯导／里程计的车载定位定向原理方案如图１所示。广—————Ｔ姿奈、位置、速度定付定目信息输『｛：捷联惯导ＩＩ——坛习姿态｜ｆ、，骨
误并按ｌＦＳ｛ｌｇｅ＿删ｕｓａｆＩ辽成滤波器
ｉ统潢差仍汁值
图１基于捷联惯导／里程计的车载定位定向原理框图
２航位推算系统的误差模型本文采用间接法滤波，即选取系统误差作为组合定位定向滤波器的状态，为此需要对捷联惯导和航位推算系统进行误差分析与建模。捷联惯导的误差方程在很多文献中已有研究，这里就不作赘述了，文中只给出航位推算系统的误差方程。２．１航位推算系统的误差源及其模型在航位推算系统中，除了初始误差，里程计和陀螺仪误差是最主要的误差源。其中，里程计的误差主要为刻度系数误差，通常可考虑为随机常值，即艿Ｒ一０（１）
陀螺仪误差经标定补偿后，主要还剩下一些无法标定的随机漂移，主要有随机常值漂移和自噪声。因此，陀螺仪的误差可表示为：ｅｉ一“＋％（ｉ＝工，Ｙ，ｚ）（２）
其中，ｉ＝。，Ｙ，ｚ表示分别沿载车坐标系的ｚ、Ｙ、ｚ轴；％
为ｉ轴上陀螺仪的白噪声；“为ｉ轴上陀螺仪的随机常值误差，其满足ｅｈ一０（ｉ＝ｚ，Ｙ，ｚ）（３）
２．２航位推算系统的误差方程在航位推算系统中，如果以捷联惯导姿态矩阵作为航位推算的姿态矩阵，则会导致航位推箅误差将随着捷联惯导姿态误差的发散而迅速增大。而捷联惯导姿态误差发散的重要原因之一是，惯导速度和位置误差随时间快速发散而引起位置速率嘁的计算存在较大误差。
由于里程计测量误差不随时间增长。而随路程增长，因此航位推算的速度和位置误差其发散速度要明显低于惯导。此时，如果利用航位推算获得的速度、位置信息并结合陀螺输出来进行航位推算姿态解算，可在一定程度上减缓姿态误差的发散速度，进而有效提高整个航位推算的精度，而具体解算方法则与捷联惯导姿态解箅相似。因此，只需以航位推算系统的输出信息及其误差来替代捷联惯导姿态误差方程中的对应物理量即可，则航位推算系统的姿态误差方程可表示为：妒Ｄ＝Ｍ１９Ｄ＋Ｍ２铲Ｄ＋眠Ⅸ＋Ｍ４岛＋Ｍ５ｗｇ（４）其中，伽＝［‰，，Ｉ肼，≯ＤＵ］７，ＳＰ。＝［乩ｏ，毅。，赫Ｄ］７，Ｅｂ＝ｋ，％，ｅ“］７，毗＝［％，％，％］７。这里，虹，庐ＤⅣ，‰为航位推算沿东、北、天向的姿态误差角；３／－Ｄ，毅Ｄ，虢。为航位推算的纬、经、高度误差。类似于捷联惯导位置误差方程的推导方法，可获得航位推算系统的位置误差方程为：护Ｄ—Ｍ６伽＋Ｍ７铲Ｄ＋Ｍ８ＳＫ（５）上述矩阵Ｍ。，Ｍ２…Ｍ８的具体表达式可参见文献［７］。３捷联惯导／里程计组合定位定向算法为了设计获得捷联惯导／里程计组合定位定向算法，需要分别建立组合定位定向的状态方程和量测方程，并在此基础上采用适当的滤波算法进行滤波计算。３．１组合定位定向的状态方程由于采用间接法滤波，因此选取系统误差作为组合定位定向滤波器的状态。捷联惯导／里程计组合定位定向的系统误差主要包括：捷联惯导的数学平台姿态误差角托，“，如，速度误差加ｚ，知。，跏ｕ，位置误差乩，毅，虢，陀螺常值漂移￡“，￡钾，‰，加速度计常值误差ｖ。，ｖ妇，ｖ。；航位推算的姿态误差角簪Ｄ￡，‰。声ＤＵ，位置误差孔ｏ，毅ｏ，脯ｏ。里程计刻度系数误差涨。即捷联惯导／里程计组合定位定向的系统状态ｘ为：Ｘ一［九．“，丸，舳￡，枷～，劬ｕ，乩，毅，虢，ｅｋ，￡ｂ，‰，ｖｋ，ｖ如，ｖｈ，声ＤＥ，≯Ｗ，≯聊，乩Ｄ．双Ｄ，胁Ｄ，涨］丁（６）于是，根据捷联惯导和航位推算系统的误差方程，并结合系统状态ｘ，可列写出捷联惯导／里程计组合定位定向的状态方程为：文＝ＦＸ＋ＧＷ（７）其中，Ｆ为系统状态矩阵，Ｇ为系统噪声驱动阵，ｗ为系统白噪声。这里，ｗ＝［％，％，％，％，％，％］丁’其中％，％，％为陀螺自噪声，ｔ‰，％，％为加速度计自噪声。３．２组合定位定向的量测方程由于航位推算精度受到姿态矩阵精度的影响，为此除了位置信息再引人姿态信息作为组合定位定向的量测，这样能够有效提高航位推算姿态误差的可观测性和估计效果，从而提高航位推算的姿态精度和定位精度。因此，将捷联惯导的姿态、位置输出与航位推算系统的对应输出相减作为量测ｚ，即Ｚ＝［豳一如，良一如，ｙｓ一扬，Ｌｓ—Ｌｏ，２ｓ—ｂ，ｈｓ—ｈｏ］７（８）这里，如，如，Ｙｓ分别为捷联惯导输出的载车航向、俯仰和滚动角；如，如，ｙＤ分别为航位推算输出的航向、俯仰和滚动角；Ｌｓ，Ａｓ，ｈｓ为捷联惯导输出的纬、经、高度；ＬＤ，ＡＤ，ｈＤ为航位推算输出的纬、经、高度。设捷联惯导的航向、俯仰和滚动角误差分别为劫，留，村，航位推算的航向、俯仰和滚动角误差分别为却Ｄ，帮。，毋。，则根据式（８）可得：Ｚ＝［劬一劬Ｄ，田一田Ｄ，渺一毋Ｄ，乩’＿乩Ｄ，毅一擞Ｄ，赫一蕊Ｄｒ（９）