车载捷联惯导系统定位测姿算法研究

第１５卷第ｌ期２００７年２月

中国惯性技术学报

ＪｏｕｍａｌｏｆＣｈｉｎｅｓｅＩｎｅｒｔｉａｌＴｃｃｈｎｏｌｏｇｙ

Ｖｂｌ．１５Ｎｏ．１

Ｆｅｂ．２００７

文章编号：１００５－６７３４（２００７）０１一００２４－０４

车载捷联惯导系统定位测姿算法研究

陈允芳１，叶泽田２，钟若飞３

（１．山东科技大学地球信息科学与工程学院，青岛２６６５１０；２．中国测绘科学研究院，北京１０００３９；

３．首都师范大学，北京１０００３７）

摘要：ＧＰｓ／ＩＮｓ组合精确测定平台的位置和姿态是移动测图系统中的重要模块。对陀螺仪和加速度计所测角速度和比力进行两次积分得载体姿态、速度和位置即ｓＩＮｓ力学机械编排。目前该过程大多在地理坐标系进行。

这里详细推导了地球坐标系中完整的解算过程，以四元数姿态矩阵更新及重力计算为核心，由ＩＭｕ原始观测值解算出了载体位置、速度和姿态等参数，可快速高效与ＧＰｓ输出的位置速度信息进行组合滤波处理，可据此编程进行工程应用数据处理。

关键词：捷联惯导系统；姿态矩阵；坐标转换；力学编排；四元数

中图分类号：ｕ６６６．１文献标识码：Ａ

ＰｏｓｉｔｉｏｎｉｎｇａｎｄｏｒｉｅｎｔａｔｉｏｎｃｏｍｐｕｔａｔｉｏｎｏｎＶｅｈｉｃｌｅ－ｂｏｒｎｅ

ＳＩＮＳａｎｄｄｉｓｃｕｓｓｏｆｃａｌｃｕｌａｔｉｏｎｅｒｒｏｒ

ｃＨＥＮＹｕｎ．‰９１，ＹＥｚｅ－ｔｉａｎ２，ｚＨＯＮＧＲｕｏ．ｆｅｉ３

（１．Ｇｅｏ·ｉｎｆｏ衄ａｔｉｏｎＳｃｉｅｎｃｅ＆ＥｎｇｉｎｅｅｒｉｎｇＣｏｌｌｅｇｅ，ＳｈａｎｄｏｎｇＵｎｉｖｅｒＳｉ哆ｏｆＳｃｉｅｎｃｅａＩｌｄＴｂｃｈｎｏｌｏｇｙ，Ｑｉｎｇｄａｏ

２６６５１０，Ｃｈｉｎａ；２．ＳｕｒｖｅｙｉｎｇａＴｌｄＭａｐｐｉｎｇＳｃｉｅｎｃｅＲｅｓｅａｒｃｈＩｎＳｔｉｔｕｔｅｏｆＣｈｉｎａ，Ｂｅｉｊｉｎｇ１０００３９，Ｃｈｉｎａ；３．Ｃ印ｉｔａｌ

ＮｏｍｌａｌＵｎｉｖｅｒＳｉ劬Ｂｅｉｊｉｎｇ１０００３７，Ｃｈｉｎａ）

Ａｂｓｔｒａｃｔ：ＧＰＳａｎｄＩＮＳｉｎｔｅｇｒａｔｅｄｔｏａｃｃｕｒａｔｅｌｙｄｅｔｅｍｉｎｉｎｇｐｏｓｉｔｉｏｎａＩｌｄａｔｔｉｔｕｄｅｏｆｎａｔＩ‘ｏｏｆｉｓＶｉｔａｌｍｏｄｕｌｅｉｎｍｏｂｉｌｅｍａｐｐｉｎｇＳｙｓｔｅｍ．Ｓｐｅｃｉｎｃｆｏｒｃｃ行ｏｍｓｐｅｅｄｏｍｅｔｅｒ蚰ｄ舭ｇｌｅｒａｔｅ矗ｏｍ留ｒｏａｒｅｉｎｔｅ铲ａｔｅｄｔｗｉｃｅｒｅｓｐｅｃｔｉＶｅｌｙｔｏａｃｈｉｅｖｅａｎ沁ｄｅ，ｖｅｌｏｃ时ａＩｌｄｐｏｓｉｔｉｏｎｎ锄ｅｌｙＳＩＮＳｍｅｃｈａＩｌｉｚａｔｉｏｎ．Ｃｕｒｒｅｎｔｌｙｔｈｉｓｔｏｏｋｐｌａｃｅｄｉｎｇｅｏｇｒ印ｈｉｃｃｏｏｒｄｉｎａｔｅ，ｗｈｉＩｅｈｅｒｅｄｅｍｏｎｓｔｒａｔｅｄｉｎｄｅｔａｉｌｍｅｗｈｏｌｅｍｅｃｈａＪｌｉｚａｔｉｏｎｉｎｅａｎｌｌ－ｃｅｎｔｃｌｒｃｄｅａｒｔｈ－ｆｉｘｅｄｃｏｏｒｄｉｎａｔｅ，ｍｏｓｔｌｙｑｕａｔｅｍｉｏｎａ钍ｉｔｕｄｅｍａｔｒｉｘｕｐｄａｔｉｎｇ锄ｄｇｒａｖｉｔ）ｒｃａＩｃｕｌａｔｉｏｎ．Ｕｌｔｉｍａｔｅｌｙｖｅｈｉｃｌｅｎａｖｉｇａｔｉｏｎｐａｒ锄ｅｔｅｒｓｓｕｃｈａＳａｔｔｉｔｕｄｅ，ｖｅＩｏｃｉｔｙ锄ｄｐｏｓｉｔｉｏｎｗｅｒｃｇａｈｅｄ丘ｏｍＩＭＵｏｒｉｇｉｎ“０ｂｓｅｒｖａｔｉｏｎｓ．Ｍａｔｈｅｍａｔｉｃｓｐｌａｔｆｏ眦ｉｓｆｏｍｌｃｄｉｎＳｒＮＳｔｏｃａｒｒｙｏｕｔｓｕⅣｅｙｉｎｇａＪｌｄｃａｌｃｕｌａｔｉｎｇｐｒｅｃｉｓｅｌｙｔｈｅｎａｖｉｇａｔｉｏｎｍｏＶｅｍｅｎｔ

ｐａｒ锄ｃｔｅｒＳ．Ｔｈｅｒｅｓｕｌｔｓａｒｃｐｒｏｎｅｔｏｉｎｔｅ黟ａｔｅｗｉｔｌｌｓｉｍｉｌａｒｐａｍｍｅｔｅｒｓ疔ｏｍＧＰＳｔｏｆｉｌｔｅｒｐｒｏｃｅｓｓｉｎｇ．Ｐｒｏ可锄ｍｉｎｇｈｅｒｃｂｙｃ锄ｐｍｃｅｓｓｄａｔａｉｎｅｎｇｉｎｅｅｒｉｎｇａｐｐｌｉｃａｔｉｏｎ

Ｋｅｙｗｏｒｄｓ：ＳＩＮＳ；ａｔｔｉｔｕｄｅｍａｔｒｉｘ；ｃｏｏｒｄｉｎａｔｅｔｒａｎｓｆｏｍａｔｉｏｎ；ｍｅｃｈａｎｉｚａｔｉｏｎ；ｑｕａｔｅｍｉｏｎ

随着惯性技术与卫星导航定位技术的发展，由ＧＰＳ／ＩＮｓ不同程度组合而成的定位定姿传感器已成为移动测图系统中确定载体轨迹和平台姿态的重要工具，其中ＧＰｓ多用于定位而ＩＮＳ则用于测姿。捷联惯导系统（ｓＩＮｓ）是将惯性仪表直接固联在载体上而无须采用机械陀螺稳定平台，通过导航计算机中相应程序建立“数学的”陀螺稳定平台，即计算机处理测量值得到载体位置、速度和运动方向估计值以实现导航平台功能。尽管ｓＩＮＳ于２０世纪５０年代即在美国获得专利，但因受限于惯性设备和计算机技术的发展而一直未能实用。近年来，电子和高速计算机技术的发展使得捷联技术得以实现和充分发展，这是惯性技术在近２０年内发展的一个重要的标志。

ＩＮＳ的核心部件是惯性测量单元（ＩＭＵ），按照其陀螺仪和加速计等元件的精度，可将惯导分为不同等级：战略级（＜０．０００ｌ（ｏ）／｝ｌ，ｌ岖）、导航级（０：０００ｌ～０．０１５）（ｏ）／Ｉｌ，５～１００嵋）、低成本（（１～１０（。冲，（０．１～１）ｎ培）。考虑到惯性设备出口管制政策及需求与成本等问题，民用ＩＮｓ精度范围一般为低成本级别。

收稿日期：２００６—０６—１６；修回日期：２００６—１２—２２

基金项目：国家８６３基金课题（２００６ＡＡｌ２２３２４）；教育部三维信息获取与应用重点实验室开放基金

作者简介：陈允芳（１９７７一），女，博士生，讲师，主要从事移动测图与组合导航。

２５陈允芳等：车载捷联惯导系统定位测姿算法研究第１期１惯性导航初始对准

因ＩＮｓ系统属航位推算系统（ＤＲ，ＤｅａｄＲｅｃｋｏｎｉｎｇ），它是依赖于前一时刻导航参数估计值的递推更新运算，故位置、速度和姿态等初始导航参数等信息是准确进行解算的基础。惯导系统初始化包括给定初始速度和位置、惯导平台初始对准、陀螺仪测漂定标等。重点是确定ＩＮｓ的初始姿态矩阵，水平对准可由加速度计的量测输出获得水平方位，即俯仰角和横滚角来实现，但因低成本ＩＭｕ的陀螺仪精度不足以感受地球自转角速率，无法获得航向信息，只能引入外部航向信息，如利用ＧＰＳ所测速度进行匹配对准，或引入其它方位测量方法，如磁航向或双天线ＧＰｓ定向系统等来获得初始航向信息。为得到较高精度初始姿态数据，本研究中水平对准由倾角仪提供横滚角和俯仰角来完成，方位对准采用ｖＥｃＴＯＲ航向测量系统实现，其２ｍ基线方位精度可达Ｏ．１０（ＲＭＳ），足以满足方位对准的精度要求。由此可获取三个初始姿态角。可利用卡尔曼滤波对平台误差角进行估计出并校正系统，使载体坐标系与导航坐标系对准ｆ５１。对低成本ＩＮＳ，一般需要进行动基座对准”１。

２地球坐标系力学编排

本研究所用ＩＭｕ的陀螺仪和加速计分辨率分别为０．０００６（。）／ｓ和Ｏ．１５ｍ毋属低成本精度级别ＩＮＳ。它包括三个ＦＯＧｓ陀螺（测量角速率）和三个ｉＭＥＭＳ加速度计（测量比力），将陀螺仪和加速度计输出信息进行捷联姿态解算，即可求得运动载体不断变化的姿态角及速度、位移增量。

２．１导航坐标系及导航方程

因陀螺仪敏感的是相对于惯性空间的运动，而要测量的是载体相对于某个参照系的运动，故必须进行坐标变换。此外，加速度计所测加速度输出信号中往往包含重力加速度分量和有害加速度分量，必须对重力加速度分量进行修正，对有害加速度分量进行补偿，然后经过积分运算得到运载体的速度和所在的地理位置。

惯性导航的基础是精确定义的一系列右手参考坐标系，较常用的是惯性坐标系（ｊ系）、地球坐标系（ｅ系）、地理坐标系（ｎ系）、载体坐标系（ｂ系）及计算坐标系（ｋ系）等，详细定义参见文献【３】。如图１所示，其中ｅ和ｎ系都三轴

定义为东北天方向，ｂ系三轴定义为右前天方向。

下面重点考虑ｋ系的合理选取问题。工程应用中，常选取ｅ、ｎ坐标系中的任一系作为ｋ。若选取ｎ系，计算时可直接输出地理坐标（五，仍办）和运行姿态，且重力计算模型简单，但因ｎ系本身随载体运动而旋转，需要附加大量计算工作来补偿由此产生的影响，且须不断进行ｅ系和ｎ系间的坐标转换，即将ｎ系的ＩＮＳ导航结果转换到ｅ系中，或将ｅ系中的ＧＰＳ定位测速结果转换到ｎ系中，此频繁转换势必影响计算精度；在ｅ系计算时，ｅ系本身不随载体运动变化，计算过程及公式简单，可直接获三鼓二豫妄孕地理坐标系

图ｌ各坐标系示意图

Ｆ培１ＣｏｏｒｄｉｎａｔｅｓＳｋｅｔｃｈＭａｐ

得载体在地心直角坐标系（如ｗＧＳ８４）中的三维直角坐标（置ｙ，ｚ），直接用于修正ＩＮｓ，适合与ＧＰｓ数据进行组合处理，且省去了反复的坐标变换，程序更简洁和模块化，导航计算精度也较ｎ系稍优，但正常重力计算模型和姿态角的提取计算较复杂。考虑到要用ＧＰｓ，ＩＮＳ进行组合导航，而ＧＰＳ定位是在ｅ系进行，故本文选ｅ系作为计算坐标系【１】【３１。

导航方程可从任何坐标系导出，不同坐标系导航方程会有不同形式。选ｅ系后，根据牛顿第二定律和科里奥里定律，载体相对于ｂ系的速度变化可由惯性轴系的速度变化来表示，载体相对于地球的加速度由三部分加速度组成哺１：

壬Ｋｌ。＝茂，ｔ《×ｙ。＋ｇ。

Ⅲ

，６为加速度计所得比力加速度，∞：×ｙ。为科里奥里加速度，ｇ。是ｅ系表示的正常重力。因ＧＰｓ观测量、引力常量都是ｅ系的值，由上式可推得ｅ系表示的ＳＩＮＳ导航方程“¨”：

『，。¨ｙ。］

．Ｙ‘＝ｌ矿。｝＝ｌ群，。一２球ｙ。＋窖。Ｉ

Ｉ农ｌⅨ（蹴一球）

其中，，。＝《厂６即转化至ｅ系后的比力值。上述方程又称为ＩＮＳ力学（或机械）编排，它描述了待求导航信息与各观测量间的数学关系，其中比力和角速度是动态系统输入的观测值，位置、速度及姿态信息是状态变量。注意，上标（如ｂ）表示向量各分量所对应的坐标系，下标ｉｂ则代表ｂ系相对于ｉ系的旋转，