捷联惯导_里程计组合导航方法_张小跃

Integrated navigation method for SINS and odometer
Zhang Xiaoyue Yang Gongliu Zhang Chunxi
（ School of Instrumentation Science and Optoelectronics Engineering，Beijing University of Aeronautics and Astronautics，Beijing 100191 ，China）
选取里程计标度误差kd偏航安装角误差和俯仰安装角误差并假设里程计载体坐标系下输出误差向量为vbd则里程计实际输出在载体坐标系下向量表示为vbdvbdvbdsincoscoscossin??????????kd1kdpd4与一般是小角度或者经过事先标校后剩余残差为小角度这里假设均为0由此造成的误差归结到与中因此有sincoscoscossin??????????0????????105sincoscoscossin??????????1????????6将式5代入式3可得vbd0????????10kdpd7将式6代入式4可得vbdvbd1????????kd1kdpd8将式8展开并忽略二阶小量结合式7可得里程计输出误差如下
（ 7）
（ 2） 式中， δρ 为 位 置 速 率 误 差； δΩ 为 地 球 转 速 误 差； δg 为重力误差； δf 为加速度计输出比力误 差； ε 为陀螺输出角速率误差．
b n b n n
δα ψ v + δ v = 1 K D（ 1 + δ K D ） P D δα θ
要： 针对车载自主导航需求， 基于卡尔曼滤波器， 实现捷联惯导与里程计量测信 息的组合导航． 推导了里程计误差模型， 结合捷联惯组误差模型与捷联系统误差模型， 建立了 捷联惯导 / 里程计自主组合导航系统误差状态模型 ． 建立了捷联惯导 / 里程计组合导航量测模 型， 阐述了估计误差修正方法． 采用仿真计算对此方法进行了验证 ， 仿真结果表明： 组合导航过 程中， 初始姿态误差能得到有效估计， 姿态误差和位置误差均能控制在一定精度范围内， 应用 此组合导航方法相对于传统的航位推算方法能得到更高的导航精度 ， 能有效实现自主高精度 定位定向． 词： 捷联惯导； 里程计； 卡尔曼滤波； 组合导航 中图分类号： V 249． 3 关 键 文献标识码： A 5965 （ 2013 ） 07092205 文 章 编 号： 1001-
现代战争中， 随着侦察技术的发展和各种精 确打击武器的出现， 为了提高陆基导弹武器系统 的生存能力和快速反应能力， 实现无依托阵地快 ． 速机动发射十分重要 为了提高武器系统的机动 性， 隐藏发射点避免受敌攻击， 提高生存力， 发射 车必须具有快速的自动定位定向能力， 甚至在行 ， 进中定位定向的能力， 以期实现“停车就打 ” 甚 ［1 ］ “边走边打” ， 至 车载惯性导航系统是实现发射 车定位定向的重要设施．
里程计输出与惯导系统输出进行组合， 可以 采用位置量组合的方式或者速率量组合的方式 ， 这里采用速率量组合的方式
［1 ］
． 里程计误差与 里程计横滚安装偏差角 α γ 无关， 即里程计在载体 坐标系下输出速度 可以表示如下： sin α ψ cos α θ sin α ψ cos α θ b v D = cos α ψ cos α θ v D = cos α ψ cos α θ K D P D sin α θ sin α θ （ 3） v 为里程计所测速度在载体坐标系 （ 车体 式中， 坐标系） 下的投影向量理想值； α ψ 为里程计航向 安装偏差角； α θ 为里程计俯仰安装偏差角； v D 为 里程计输出速度量； K D 为里程计标度； P D 为里程 计输出脉冲． 选取里程计标度误差 δK D 、 偏航安装角误差 δα ψ 和俯仰安装角误差 δα θ ， 并假设里程计载体坐 标系下输出误差向量为 δv D ， 则里程计实际输出 在载体坐标系下向量表示为 b b ～ vb D = v D + δv D = sin（ α ψ + δα ψ ） cos（ α θ + δα θ ） cos（ α + δα ） cos（ α + δα ） ψ ψ θ θ sin（ α θ + δα θ ） K （ 1 + δK ） P D D D （ 4） α ψ 与 α θ 一般是小角度或者经过事先标校后 这里假设均为 0 ， 由此造成的 剩余残差为小角度， 误差归结到 δα ψ 与 δα θ 中， 因此有 sin α ψ cos α θ 0 cos α cos α = 1 ψ θ sin α 0
Abstract： In order to complete independent navigation for land vehicle，integration of strapdown inertial navigation system （ SINS） and odometer was realized based on Kalman filter． Odometer error model was proposed ，which was associated with inertial measurement unit（ IMU） error model and INS error model， and error model of integration system was established． SINS / odometer integration navigation observation model was established，the compensation method of error estimation was described． The integrated navigation method was validated in a simulation，the simulation results indicate that： in the process of integrated navigation，initial and attitude error and position error retained in a certain range， navigation error of attitude could be estimated， precision was improved compared with the traditional deadreckoning algorithm，and independent high accuracy position and attitude determination was obtained． Key words： strapdown inertial navigation system； odometer； Kalman filter； integrated navigation
0803 ； 网络出版时间： 20130702 10 ： 03 收稿日期： 2012网络出版地址： www． cnki． net / kcms / detail /11． 2625． V． 20130702． 1003． 005． html zhangxiaoyue@ buaa． edu． cn． 作者简介： 张小跃（ 1982－ ） ， 男， 湖南益阳人， 讲师，
n 姿态误差为 φ， 位置误差为 δθ， 高度 误差为 δv ， 误差为 δh， 定义如下：
～ v n = v n + δv n ～ Cn I － （ φ ×） ］ Cn b =［ b
～n n C e =［ I － （ δθ ×） ］ Ce ～ h = h + δh
b D b D
（ 8）
将式（ 8 ） 展开并忽略二阶小量， 结合式 （ 7 ） ， 可得里程计输出误差如下：
924
北 京 航 空 航 天 大 学 学 报
2013 年
δα ψ δα ψ δv = δK D K D P D = δK D vD δα θ δα θ
2013 年 7 月 第 39 卷 第 7 期
北京航空航天大学学报 Journal of Beijing University of Aeronautics and Astronautics
July 2013 Vol． 39 No. 7
捷联惯导 / 里程计组合导航方法
张小跃
摘
杨功流
张春熹
（ 北京航空航天大学 仪器科学与光电工程学院，北京 100191 ）
选取惯性导航系统 / 里程计组合来实现发射 车行进过程中定位定向， 发射车从一个已知坐标 点出发， 行进途中， 惯导系统 / 里程计组合导航， 达 、 到任意发射点后即可立即得知自身位置 姿态和 方位， 这种全自主的高精度导航定位定向方式 ， 途 中不需要任何外部信息、 不受外部干扰， 在现代战 争环境中具有非常明显的优势． 当前针对惯导与 里程计组合的研究主要有航位推算方法、 基于航 位推算过程误差模型的组合导航以及考虑部分里
·n = － （ δρ n + 2 δΩn ） × v n － （ ρ n + 2 Ωn ） × δv n n n b n δv + f × φ + C b δf + δg b · = δρ n + Ωn × δθ + φ × （ Ωn + ρ n ） － C n φ bε · n n δθ = δρ － ρ × δθ · n δh = δv z n e
（ 1）
～ v n 为导航计算所得速度； v n 为不包含误差 式中， 的真实速度； 上标 n 表示定义在导航坐标系下； ～ n Cn b 为计算所得姿态捷联矩阵 ； C b 为无误差的捷 ～n 联矩阵真实值； I 为单位矩阵； C e 为计算所得从 地球坐标系到导航坐标系的转换矩阵 ； C 为从地 ～ 球坐标系到导航坐标系的转换矩阵真实值； h 为 计算所得高度值； h 为高度真实值． 系统误差模型 方程如下：
（ 5） = δα ψ 1 δα θ （ 6）
sin（ α ψ + δα ψ ） cos（ α θ + δα θ ） cos（ α + δα ） cos（ α + δα ） ψ ψ θ θ sin（ α θ + δα θ ） 将式（ 5 ） 代入式（ 3 ） 可得 0 b vD = 1 KD PD 0 将式（ 6 ） 代入式（ 4 ） 可得
·
输出速度信息转换到导航坐标系下与惯性导航解 算输出速度进行比较， 完成组合导航过程， 观测量 如下所示： ～ ～b v n － Cn Δv = ～ b v D 式中 ～ ～b b b Cn I － （ φ ×） ］ Cn （ 14 ） b v D =［ b（ v D + δ v D ） 将式 （ 14 ） 右边展开， 略去二阶小量， 并与式 （ 1 ） 中第 1 式代入式（ 13 ） 可得 n n n b Δv = δ v + （ φ × ） v － C b δ v D 假设： C11 C = C21 C31
θ b b D
［1－4 ］
1
组合状态误差模型
组合导航误差状态量主要包含了导航参数误
惯性器件误差状态、 里程计误差状态， 下 差状态、 面就各部分误差状态模型进行介绍 ． 1． 1 惯导系统误差模型 5 －6］中 针对惯性导航系统误差模型， 文献［
［5－6 ］
已有详细阐述， 这里不再详细推导， 直接参考、 引 用如下． 惯导系统误差状态量用向量表示如下： 速度
第7 期
张小跃等： 捷联惯导 / 里程计组合导航方法
［1－4 ］
923
程计误差的惯导 / 里程计组合导航
， 实际应用
1． 2
里程计误差模型
最为广泛的为传统的航位推算方法 ． 本文提出了一种捷联惯导 / 里程计的组合导 航方法， 推导了完整的里程计误差模型， 并基于此 组合导航量测模 建立了组合导航误差状态模型、 型和估计误差修正方法， 最后采用仿真计算验证 并与传统的航位推算方法 了这种组合导航方法， 进行了比较．
b D
（ 9）
2
组合量测模型
组合导航过程中， 惯性导航系统引入里程计
ቤተ መጻሕፍቲ ባይዱ
1． 3
状态误差模型
车载应用条件下， 载体动态相对较小， 这里假 设惯性器件（ 陀螺、 加速度计 ） 输出误差可以简化 表示成零偏误差与白噪声之和， 零偏误差为随机 B gy ， B gz ， B ax ， B ay ， B az ． 以 B gx 分别表示为 B gx ， 常值， 为例满足： B gx = 0