CN107588769A - 一种车载捷联惯导、里程计及高程计组合导航方法 - Google Patents
一种车载捷联惯导、里程计及高程计组合导航方法 Download PDFInfo
- Publication number
- CN107588769A CN107588769A CN201710965097.3A CN201710965097A CN107588769A CN 107588769 A CN107588769 A CN 107588769A CN 201710965097 A CN201710965097 A CN 201710965097A CN 107588769 A CN107588769 A CN 107588769A
- Authority
- CN
- China
- Prior art keywords
- mrow
- inertial navigation
- mover
- mtd
- odometer
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Abstract
本发明公开了一种车载捷联惯导、里程计及高程计组合导航方法，该方法包括如下步骤：捷联惯导初始对准；捷联惯导进行惯性解算，里程计和高程计也开始工作；计算平均速度观测量和高度观测量；通过卡尔曼滤波得到捷联惯导误差、里程计刻度系数误差、捷联惯导安装误差、杆臂误差及高程计误差；对捷联惯导误差、里程计刻度系数误差、捷联惯导安装误差、杆臂误差及高程计误差进行校正；校正后的捷联惯导姿态信息、速度信息及位置信息作为导航信息进行输出；返回惯性解算步骤。本发明使导航系统不依赖外部设备、不受外部信号干扰，能为“停车就打”提供高精度的位置和方位基准信息，该组合导航方法还具有快速、简便、工程实用性强等突出优点。
Description
技术领域
本发明涉及车载导航技术领域，更为具体来说，本发明为一种车载捷联惯导、里程计及高程计组合导航方法。
背景技术
目前，车载导弹在任何地点任何时间往往需要满足能实现停车即打的能力，这就要求车载导航系统必须提供高精度连续的位置信息和姿态信息。传统车载导航系统为实现高精度的定位和精确的姿态，使用的是捷联惯导和卫星导航组合导航的方式。但是，卫星导航易受电子干扰的影响，特别在军事应用上的影响更为明显。
因此，如何减弱甚至消除电子干扰对车载导航的影响、提高车载导航精度和准确度，成为了本领域技术人员亟待解决的技术问题和始终研究的重点。
发明内容
为解决常规车载导航系统易受电子干扰的影响的问题，本发明创新地提出了一种车载捷联惯导、里程计及高程计组合导航方法，其具有不依赖外部设备、不受外部信号干扰的特点，为车载导弹提供自主的定位、定向信息。
为实现上述的技术目的，本发明公开了一种车载捷联惯导、里程计及高程计组合导航方法，该方法包括如下步骤，
步骤1，在载车静止时，捷联惯导进行初始对准，并在初始对准完成后捷联惯导进入组合导航状态；
步骤2，捷联惯导进行惯性解算，得到惯导输出位置、速度、姿态及高度，计算杆臂速度，同时通过里程计得到里程计输出速度以及通过高程计得到高程计输出高度；
步骤3，在卡尔曼滤波周期内对惯导输出速度、计算的杆臂速度及里程计输出速度分别进行积分运算，得到惯导里程增量、杆臂里程增量及里程计里程增量，将惯导里程增量、杆臂里程增量及里程计里程增量分别除以卡尔曼滤波周期，得到惯导平均速度、杆臂平均速度及里程计平均速度，然后计算平均速度观测量；通过惯导输出高度和高程计输出高度得到高度观测量；
步骤4，将所述平均速度观测量和所述高度观测量输入至卡尔曼滤波器中，从而得到捷联惯导误差估计值、杆臂误差估计值、捷联惯导安装误差估计值、里程计刻度系数误差估计值及高程计误差估计值；
步骤5，利用上述捷联惯导误差估计值对捷联惯导误差进行校正，利用里程计刻度系数误差估计值对里程计刻度系数进行校正，利用所述捷联惯导安装误差对捷联惯导安装矩阵进行修正，利用所述杆臂误差估计值对杆臂进行校正，利用所述高程计误差估计值对高程计误差进行校正；校正后的捷联惯导速度信息、位置信息及姿态信息作为导航信息进行输出；然后返回步骤2。
本发明可直接应用于具有捷联惯导系统、里程计、气压高程计的车载导航系统，本发明能够使车载导航系统不受外部信号干扰，能够完全自主地为车载导弹提供可靠的导航信息，是一种较理想的车载组合导航方案。
进一步地，步骤2中，利用捷联惯导姿态矩阵和杆臂长度估计值计算杆臂速度：
其中，表示计算的杆臂速度，表示捷联惯导计算的姿态矩阵，表示惯导载体坐标系b相对地球的速度在捷联惯导载体坐标系b上的投影，表示计算的杆臂长度。
进一步地，步骤2中，利用里程计输出脉冲个数计算里程计输出速度；在载车坐标系m下矢量形式的里程计输出速度为：
其中，表示在载车坐标系m下矢量形式的里程计输出速度，表示里程计刻度系数，Ni表示里程计输出脉冲数，Ts表示采样周期；
通过如下方式将载车坐标系m下的里程计输出速度转换到导航坐标系n下的里程计输出速度：
其中，表示捷联惯导安装矩阵。
进一步地，步骤3中，平均速度观测量通过下式计算：
其中，表示平均速度观测量，T表示卡尔曼滤波周期，表示捷联惯导输出的速度。
采用捷联惯导和里程计平均速度量测量，可以一定程度上消除载车振动及量化误差对组合导航的影响。
进一步地，步骤3中，通过如下方式得到高度观测量：
其中，z2表示高度观测量，表示惯导输出高度，表示高程计输出高度。
进一步地，步骤4中，所述卡尔曼滤波器中使用如下状态方程：
其中，表示捷联惯导平台失准角变化率，表示捷联惯导速度误差变化率，表示捷联惯导位置误差变化率，表示捷联惯导陀螺常值漂移变化率，表示捷联惯导加速度计常值漂移变化率，表示里程计刻度系数误差变化率，表示捷联惯导俯仰安装误差变化率，表示捷联惯导方位安装误差变化率，表示杆臂误差变化率，表示高程计误差变化率，表示导航坐标系n相对惯性系角速度在导航坐标系n下的投影，εb表示陀螺常值漂移，表示陀螺随机噪声，fb表示惯导比力，表示地球自转角速度在导航坐标系n上的投影，表示导航坐标系n相对地球转动角速度在导航坐标系n上的投影，δVn为惯导输出速度误差，Vn表示惯导输出速度，▽b表示加速度计常值漂移，表示加速度计随机噪声，δP表示惯导位置误差，RM表示地球子午圈曲率半径，h为捷联惯导所在高度，L为捷联惯导所在维度，VN为北向速度，VE为东向速度，RN为地球卯酉圈半径。
进一步地，步骤4中，所述的卡尔曼滤波器使用如下的观测方程：
z2＝δh-δhb
其中，μ＝[δα 0 δβ]T为捷联惯导安装误差，δh为捷联惯导高度误差，δhb为气压高程计误差。
基于上述改进的技术方案，可以更为准确的估计出捷联惯导等的各项误差，以便对其进行校正。
进一步地，步骤5中，通过下式对各项误差进行校正：
k＝1-δk；
基于上述改进的技术方案，本发明车载捷联惯导/里程计/高程计组合导航方法可估计出陀螺常值漂移、加速度计常值零偏、捷联惯导安装误差、里程计刻度系数误差、杆臂误差等，有效地减小水平和高程位置误差发散速度。同时，能估计出捷联惯导的姿态误差、速度误差、位置误差，因此，修正后的捷联惯导姿态、速度和位置可以作为组合导航的导航结果。
进一步地，所述杆臂设置于捷联惯导惯性测量单元的敏感中心与里程计速度测量点之间；在导航坐标系n下，按照如下方式确定捷联惯导惯性测量单元的敏感中心速度、里程计输出速度、杆臂速度之间的关系：
其中，表示捷联惯导惯性测量单元的敏感中心速度，表示里程计输出速度，表示杆臂速度。
本发明的有益效果为：本发明可直接应用于具有捷联惯导系统、里程计、气压高程计的车载导航系统，这三种传感器工作都不依赖于外部设备，从而本发明能够使车载导航系统不受外部信号干扰，能够完全自主地为车载导弹提供可靠的导航信息，是一种较理想的车载导航方案。
该组合导航方法不依赖外部设备，不受外部信号干扰，能够独立自主的为车载导弹提供较高精度的位置信息，同时该组合导航方法能保证方位误差不发散，能为“停车就打”提供高精度的方位基准信息。另外，该组合导航方法还具有快速、简便、工程实用性强等突出优点，可为车载定位定向系统提供自主可靠的组合导航方法。
附图说明
图1为车载捷联惯导、里程计及高程计组合导航方法流程示意图。
图2为车载捷联惯导、里程计及高程计组合导航方法的实施框图。
具体实施方式
下面结合说明书附图对本发明的车载捷联惯导、里程计及高程计组合导航方法进行详细的解释和说明。
如图1和图2所示，本发明具体公开了一种车载捷联惯导、里程计及高程计组合导航方法，该组合导航方法的核心改进点在于增加了杆臂速度模型、高程计误差模型以及卡尔曼滤波器中平均速度观测量的设计；里程计辅助捷联惯导水平位置误差发散速度较小，高度误差发散速度较快，引入高程计后，使得高度误差发散变小；而对于姿态信息的输出，本发明利用捷联惯导本身校正后的姿态信息。
具体来说，该方法包括如下步骤。
步骤1，在载车静止时，捷联惯导进行初始对准，以获得惯性导航的位置、速度、姿态初始值。并且在初始对准完成后捷联惯导进入组合导航状态，初始对准后，载车可以启动。
步骤2，捷联惯导进行惯性解算，得到惯导输出位置、速度、姿态及高度，计算杆臂速度，同时通过里程计得到里程计输出速度以及通过高程计得到高程计输出高度；具体如下。
本实施例中，车载里程计采用霍尔传感器，该霍尔传感器加装于载车传动轴上，并以脉冲形式输出非转向轮轴中心点的前进速度，利用里程计输出脉冲个数计算里程计输出速度；本实施例的载车坐标系m为右前上坐标系；考虑载车正常行驶时车辆无侧滑、无打滑且无跳跃的情况下，在载车坐标系m下矢量形式的里程计输出速度为：
其中，上述表示在载车坐标系m下矢量形式的里程计输出速度，表示里程计刻度系数，Ni表示里程计输出脉冲数，Ts表示采样周期。
由于组合导航需要输出导航坐标系(n系)的位置，所以本发明将里程计在载车坐标系下的输出转换到导航坐标系，设捷联惯导载体坐标系为b系，考虑到b系与m系不重合，则b系和n系下里程计的理想输出分别为：
其中，表示捷联惯导姿态矩阵，表示捷联惯导安装矩阵；由俯仰安装角α、横滚安装角γ及方位安装角β组成，通过计算得到里程计输出与横滚安装角γ无关，因此，里程计输出也与横滚安装误差角无关。考虑到里程计刻度系数误差δk、捷联惯导俯仰安装误差δα、捷联惯导方位安装误差δβ及捷联惯导的平台失准角则里程计实际计算的速度可写成：
其中，μ＝[δα 0 δβ]T，而×表示矢量叉乘符号。
本实施例中，杆臂长度为捷联惯导惯性测量单元的敏感中心与里程计速度测量点之间的距离，当载车有角运动时，捷联惯导惯性测量单元(IMU)与里程计速度测量点之间的速度有差异，在组合导航时，由杆臂引起的速度差异与组合导航系统的误差传播无关，如不消除就会影响组合导航精度；在导航坐标系n下，按照如下方式确定捷联惯导惯性测量单元的敏感中心速度、里程计输出速度、杆臂速度之间的关系：
其中，表示捷联惯导惯性测量单元的敏感中心速度，表示里程计输出速度，表示杆臂速度，表示惯导载体坐标系b相对地球的速度在捷联惯导载体坐标系b上的投影，lb表示杆臂长度，δlb表示杆臂误差估计值。
步骤3，在卡尔曼滤波周期内对惯导输出速度、计算的杆臂速度及里程计输出速度分别进行积分运算，得到惯导里程增量、杆臂里程增量及里程计里程增量，将惯导里程增量、杆臂里程增量及里程计里程增量分别除以卡尔曼滤波周期，得到惯导平均速度、杆臂平均速度及里程计平均速度，然后计算平均速度观测量；通过惯导输出高度和高程计输出高度得到高度观测量。
需要说明的是，里程计本质属于速度传感器，因此量测量应为速度量测量。由于捷联惯导与里程计非理想刚性连接，载车机动时，惯导安装处存在振动，该振动引起捷联惯导速度与里程计测量速度存在误差，且里程计采样周期一般很短，造成里程计输出的瞬时速度存在很大的量化误差，为消除振动及量化误差对组合导航的影响，采用滤波周期的平均速度作为观测量，即对观测量在一段时间进行积分，然后除以滤波周期，得到平均观测量，具体通过如下方式得到平均速度观测量：
其中，z1为平均速度观测量，T为卡尔曼滤波周期，为捷联惯导安装处理想速度，δVn为捷联惯导速度误差。
考虑到实际卡尔曼滤波周期比较短，在滤波周期内子惯导速度误差、失准角、里程计刻度系数误差、捷联惯导安装误差、杆臂误差近似为常值，将式(5)、式(8)与式(16)代入式(15)，可得观测方程如下：
本发明中，由气压高程计和捷联惯导输出的高度测量量，通过下式得到高度观测量：
其中，z2表示高度观测量，表示惯导输出高度，表示高程计输出高度。
考虑到捷联惯导与气压高程计的高度误差，可以得到高度观测方程：
z2＝δh-δhb
其中，δh为捷联惯导输出高度误差，δhb为高程计误差。
步骤4，将平均速度观测量和高度观测量输入至卡尔曼滤波器中，到达卡尔曼滤波周期进行滤波解算，执行卡尔曼滤波算法(其具体算法为本领域的常规技术手段)，从而得到捷联惯导误差估计值、杆臂误差估计值、捷联惯导安装误差估计值、里程计刻度系数误差估计值及高程计误差估计值。
步骤5，对捷联惯导误差、里程计刻度系数误差、捷联惯导安装误差、杆臂误差及高程计误差进行校正；具体地，利用上述捷联惯导误差估计值对捷联惯导误差进行校正，利用里程计刻度系数误差估计值对里程计刻度系数进行校正，利用所述捷联惯导安装误差对捷联惯导安装矩阵进行修正，利用所述杆臂误差估计值对杆臂进行校正，利用所述高程计误差估计值对高程计误差进行校正；校正后的捷联惯导速度信息、位置信息及姿态信息作为导航信息进行输出；然后返回步骤2。上述步骤2、3、4、5不断迭代进行，从而不断输出组合导航信息。
具体实施时，对于上述式(4)，忽略二阶小量，则可得到里程计测速模型：
μ＝[δα 0 δβ]T
其中，表示利用里程计实际计算的速度，表示在导航坐标系n下矢量形式的里程计输出速度，表示在捷联惯导在载体坐标系b下矢量形式的里程计输出速度，表示捷联惯导姿态矩阵，表示捷联惯导安装矩阵，δα表示捷联惯导俯仰安装误差估计值，δβ表示捷联惯导方位安装误差估计值，δk表示里程计刻度系数误差估计值，表示捷联惯导的平台失准角。
具体实施时，对于上述式(8)，忽略二阶小量，得到如下杆臂速度模型：
其中，表示实际计算的杆臂速度，表示惯导载体坐标系b相对地球的速度在捷联惯导载体坐标系b上的投影，lb表示杆臂长度，δlb表示杆臂误差估计值。
气压高程计利用气压表测量载车周围环境的大气压力，利用标准大气模型确定高度。因此，气压高程计不需要外部设备信息，其不受外部信号干扰具有完全自主的特点，本发明捷联惯导/里程计/高程计组合后仍具有完全自主的特点，引入高程计后可以抑制高度通道的发散趋势。由于载车实际所处的实际基准海平面大气参数与标准海平面的大气参数存在差异以及大气压力测量误差造成气压高程计测量存在误差，本发明将气压高程计误差δhb建模为一阶马尔可夫过程：
其中，表示修正后的高程计误差，δhb表示标准高程计误差，τb表示相关时间，wb表示白噪声。
具体实施时，本发明捷联惯导/里程计/高程计组合导航系统状态使用22维：
本发明设计的状态方程包括捷联惯导误差方程、惯性器件误差模型、里程计刻度系数误差模型、捷联惯导安装误差模型、杆臂误差模型、高程计误差模型，卡尔曼滤波器中使用如下状态方程：
其中，表示捷联惯导平台失准角变化率，表示捷联惯导速度误差变化率，表示捷联惯导位置误差变化率，表示捷联惯导陀螺常值漂移变化率，表示捷联惯导加速度计常值漂移变化率，表示里程计刻度系数误差变化率，表示捷联惯导俯仰安装误差变化率，表示捷联惯导方位安装误差变化率，表示杆臂误差变化率，表示高程计误差变化率，表示导航坐标系n相对惯性系角速度在导航坐标系n下的投影，εb表示陀螺常值漂移，表示陀螺随机噪声，fb表示惯导比力，表示地球自转角速度在导航坐标系n上的投影，表示导航坐标系n相对地球转动角速度在导航坐标系n上的投影，δVn为惯导输出速度误差，Vn表示惯导输出速度，▽b表示加速度计常值漂移，表示加速度计随机噪声，δP表示惯导位置误差，RM表示地球子午圈曲率半径，h为捷联惯导所在高度，L为捷联惯导所在维度，VN为北向速度，VE为东向速度，RN为地球卯酉圈半径。
从上式可以看出，里程计刻度系数误差、捷联惯导俯仰安装误差、捷联惯导方位安装误差和杆臂距离误差都建模为随机常值。
将本发明中的状态方程、量测方程、观测方程离散化(具体离散处理过程为本领域技术人员的常规技术手段)，可得到
其中，wk,vk均为离散化的零均值白噪声，其协方差阵分别为Qk,Rk。由于状态方程和观测方程都是线性方程，因此可以采用标准卡尔曼滤波算法进行组合导航。
采用标准卡尔曼滤波后得到各项误差估计值，根据各误差模型，通过下式对各项误差进行校正。
k＝1-δk
在本说明书的描述中，参考术语“本实施例”、“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明实质内容上所作的任何修改、等同替换和简单改进等，均应包含在本发明的保护范围之内。
Claims (8)
1.一种车载捷联惯导、里程计及高程计组合导航方法，其特征在于：该方法包括如下步骤，
步骤1，在载车静止时，捷联惯导进行初始对准，并在初始对准完成后捷联惯导进入组合导航状态；
步骤2，捷联惯导进行惯性解算，得到惯导输出位置、速度、姿态及高度，计算杆臂速度，同时通过里程计得到里程计输出速度以及通过高程计得到高程计输出高度；
步骤3，在卡尔曼滤波周期内对惯导输出速度、计算的杆臂速度及里程计输出速度分别进行积分运算，得到惯导里程增量、杆臂里程增量及里程计里程增量，将惯导里程增量、杆臂里程增量及里程计里程增量分别除以卡尔曼滤波周期，得到惯导平均速度、杆臂平均速度及里程计平均速度，然后计算平均速度观测量；通过惯导输出高度和高程计输出高度得到高度观测量；
步骤4，将所述平均速度观测量和所述高度观测量输入至卡尔曼滤波器中，从而得到捷联惯导误差估计值、杆臂误差估计值、捷联惯导安装误差估计值、里程计刻度系数误差估计值及高程计误差估计值；
步骤5，利用上述捷联惯导误差估计值对捷联惯导误差进行校正，利用里程计刻度系数误差估计值对里程计刻度系数进行校正，利用所述捷联惯导安装误差对捷联惯导安装矩阵进行修正，利用所述杆臂误差估计值对杆臂进行校正，利用所述高程计误差估计值对高程计误差进行校正；校正后的捷联惯导速度信息、位置信息及姿态信息作为导航信息进行输出；然后返回步骤2。
2.根据权利要求1所述的车载捷联惯导、里程计及高程计组合导航方法，其特征在于：
步骤2中，利用捷联惯导姿态矩阵和杆臂长度估计值计算杆臂速度：
<mrow>
<msubsup>
<mover>
<mi>v</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>l</mi>
<mi>a</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<msubsup>
<mover>
<mi>C</mi>
<mo>^</mo>
</mover>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>e</mi>
<mi>b</mi>
</mrow>
<mi>b</mi>
</msubsup>
<mo>&times;</mo>
<msup>
<mover>
<mi>l</mi>
<mo>^</mo>
</mover>
<mi>b</mi>
</msup>
<mo>;</mo>
</mrow>
其中，表示计算的杆臂速度，表示捷联惯导计算的姿态矩阵，表示惯导载体坐标系b相对地球的速度在捷联惯导载体坐标系b上的投影，表示计算的杆臂长度。
3.根据权利要求2所述的车载捷联惯导、里程计及高程计组合导航方法，其特征在于：
步骤2中，利用里程计输出脉冲个数计算里程计输出速度；在载车坐标系m下矢量形式的里程计输出速度为：
<mrow>
<msubsup>
<mover>
<mi>v</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>o</mi>
<mi>d</mi>
</mrow>
<mi>m</mi>
</msubsup>
<mo>=</mo>
<mover>
<mi>k</mi>
<mo>^</mo>
</mover>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mfrac>
<msub>
<mi>N</mi>
<mi>i</mi>
</msub>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
</mfrac>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
</mrow>
其中，表示在载车坐标系m下矢量形式的里程计输出速度，表示里程计刻度系数，Ni表示里程计输出脉冲数，Ts表示采样周期；
通过如下方式将载车坐标系m下的里程计输出速度转换到导航坐标系n下的里程计输出速度：
<mrow>
<msubsup>
<mover>
<mi>v</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>o</mi>
<mi>d</mi>
</mrow>
<mi>b</mi>
</msubsup>
<mo>=</mo>
<msubsup>
<mover>
<mi>C</mi>
<mo>^</mo>
</mover>
<mi>m</mi>
<mi>b</mi>
</msubsup>
<msubsup>
<mover>
<mi>v</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>o</mi>
<mi>d</mi>
</mrow>
<mi>m</mi>
</msubsup>
<mo>;</mo>
</mrow>
<mrow>
<msubsup>
<mover>
<mi>v</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>o</mi>
<mi>d</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<msubsup>
<mover>
<mi>C</mi>
<mo>^</mo>
</mover>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msubsup>
<mover>
<mi>C</mi>
<mo>^</mo>
</mover>
<mi>m</mi>
<mi>b</mi>
</msubsup>
<msubsup>
<mover>
<mi>v</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>o</mi>
<mi>d</mi>
</mrow>
<mi>m</mi>
</msubsup>
<mo>;</mo>
</mrow>
其中，表示捷联惯导安装矩阵。
4.根据权利要求3所述的车载捷联惯导、里程计及高程计组合导航方法，其特征在于：
步骤3中，平均速度观测量通过下式计算：
<mrow>
<msub>
<mi>z</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mi>T</mi>
</mfrac>
<mo>&Integral;</mo>
<msubsup>
<mover>
<mi>v</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mi>d</mi>
<mi>t</mi>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<mi>T</mi>
</mfrac>
<mo>&Integral;</mo>
<msubsup>
<mover>
<mi>v</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>o</mi>
<mi>d</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mi>d</mi>
<mi>t</mi>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<mi>T</mi>
</mfrac>
<mo>&Integral;</mo>
<msubsup>
<mover>
<mi>v</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>l</mi>
<mi>a</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mi>d</mi>
<mi>t</mi>
</mrow>
其中，z1表示平均速度观测量，T表示卡尔曼滤波周期，表示捷联惯导输出的速度。
5.根据权利要求4所述的车载捷联惯导、里程计及高程计组合导航方法，其特征在于：
步骤3中，通过如下方式得到高度观测量：
<mrow>
<msub>
<mi>z</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>h</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mover>
<mi>h</mi>
<mo>^</mo>
</mover>
<mi>b</mi>
</msub>
<mo>;</mo>
</mrow>
其中，z2表示高度观测量，表示惯导输出高度，表示高程计输出高度。
6.根据权利要求5所述的车载捷联惯导、里程计及高程计组合导航方法，其特征在于：
步骤4中，所述卡尔曼滤波器中使用如下状态方程：
<mrow>
<msub>
<mi>M</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mn>1</mn>
<mo>/</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>R</mi>
<mi>M</mi>
</msub>
<mo>+</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>sec</mi>
<mi> </mi>
<mi>L</mi>
<mo>/</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>R</mi>
<mi>M</mi>
</msub>
<mo>+</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
<mrow>
<msub>
<mi>M</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>V</mi>
<mi>N</mi>
</msub>
<mo>/</mo>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>R</mi>
<mi>M</mi>
</msub>
<mo>+</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>V</mi>
<mi>E</mi>
</msub>
<mi>tan</mi>
<mi> </mi>
<mi>L</mi>
<mi> </mi>
<mi>sec</mi>
<mi> </mi>
<mi>L</mi>
<mo>/</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>R</mi>
<mi>N</mi>
</msub>
<mo>+</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>V</mi>
<mi>E</mi>
</msub>
<mi>sec</mi>
<mi> </mi>
<mi>L</mi>
<mo>/</mo>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>R</mi>
<mi>N</mi>
</msub>
<mo>+</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中，表示捷联惯导平台失准角变化率，表示捷联惯导速度误差变化率，表示捷联惯导位置误差变化率，表示捷联惯导陀螺常值漂移变化率，表示捷联惯导加速度计常值漂移变化率，表示里程计刻度系数误差变化率，表示捷联惯导俯仰安装误差变化率，表示捷联惯导方位安装误差变化率，表示杆臂误差变化率，表示高程计误差变化率，表示导航坐标系n相对惯性系角速度在导航坐标系n下的投影，εb表示陀螺常值漂移，表示陀螺随机噪声，fb表示惯导比力，表示地球自转角速度在导航坐标系n上的投影，表示导航坐标系n相对地球转动角速度在导航坐标系n上的投影，δVn为惯导输出速度误差，Vn表示惯导输出速度，▽b表示加速度计常值漂移，表示加速度计随机噪声，δP表示惯导位置误差，RM表示地球子午圈曲率半径，h为捷联惯导所在高度，L为捷联惯导所在维度，VN为北向速度，VE为东向速度，RN为地球卯酉圈半径。
7.根据权利要求6所述的车载捷联惯导、里程计及高程计组合导航方法，其特征在于：
步骤5中，通过下式对各项误差进行校正：
<mrow>
<msup>
<mi>V</mi>
<mi>n</mi>
</msup>
<mo>=</mo>
<msup>
<mover>
<mi>V</mi>
<mo>^</mo>
</mover>
<mi>n</mi>
</msup>
<mo>-</mo>
<msup>
<mi>&delta;V</mi>
<mi>n</mi>
</msup>
<mo>;</mo>
</mrow>
<mrow>
<mi>P</mi>
<mo>=</mo>
<mover>
<mi>P</mi>
<mo>^</mo>
</mover>
<mo>+</mo>
<mi>&delta;</mi>
<mi>P</mi>
<mo>;</mo>
</mrow>
<mrow>
<msup>
<mi>&epsiv;</mi>
<mi>b</mi>
</msup>
<mo>=</mo>
<msup>
<mover>
<mi>&epsiv;</mi>
<mo>^</mo>
</mover>
<mi>b</mi>
</msup>
<mo>+</mo>
<msup>
<mi>&delta;&epsiv;</mi>
<mi>b</mi>
</msup>
<mo>;</mo>
</mrow>
<mrow>
<msup>
<mo>&dtri;</mo>
<mi>b</mi>
</msup>
<mo>=</mo>
<msup>
<mover>
<mo>&dtri;</mo>
<mo>^</mo>
</mover>
<mi>b</mi>
</msup>
<mo>+</mo>
<mi>&delta;</mi>
<msup>
<mo>&dtri;</mo>
<mi>b</mi>
</msup>
<mo>;</mo>
</mrow>
k＝1-δk；
<mrow>
<msubsup>
<mi>C</mi>
<mi>m</mi>
<mi>b</mi>
</msubsup>
<mo>=</mo>
<mrow>
<mo>(</mo>
<mi>I</mi>
<mo>+</mo>
<mi>&mu;</mi>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
<msubsup>
<mover>
<mi>C</mi>
<mo>^</mo>
</mover>
<mi>m</mi>
<mi>b</mi>
</msubsup>
<mo>;</mo>
</mrow>
<mrow>
<msup>
<mi>l</mi>
<mi>b</mi>
</msup>
<mo>=</mo>
<msup>
<mover>
<mi>l</mi>
<mo>^</mo>
</mover>
<mi>b</mi>
</msup>
<mo>-</mo>
<msup>
<mi>&delta;l</mi>
<mi>b</mi>
</msup>
<mo>.</mo>
</mrow>
8.根据权利要求7所述的车载捷联惯导、里程计及高程计组合导航方法，其特征在于：所述杆臂设置于捷联惯导惯性测量单元的敏感中心与里程计速度测量点之间；在导航坐标系n下，按照如下方式确定捷联惯导惯性测量单元的敏感中心速度、里程计输出速度、杆臂速度之间的关系：
<mrow>
<msubsup>
<mi>v</mi>
<mrow>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<msubsup>
<mi>v</mi>
<mrow>
<mi>o</mi>
<mi>d</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>v</mi>
<mrow>
<mi>l</mi>
<mi>a</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
其中，表示捷联惯导惯性测量单元的敏感中心速度，表示里程计输出速度，表示杆臂速度。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710965097.3A CN107588769B (zh) | 2017-10-17 | 2017-10-17 | 一种车载捷联惯导、里程计及高程计组合导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710965097.3A CN107588769B (zh) | 2017-10-17 | 2017-10-17 | 一种车载捷联惯导、里程计及高程计组合导航方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107588769A true CN107588769A (zh) | 2018-01-16 |
| CN107588769B CN107588769B (zh) | 2020-01-03 |
Family
ID=61053361
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710965097.3A Active CN107588769B (zh) | 2017-10-17 | 2017-10-17 | 一种车载捷联惯导、里程计及高程计组合导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107588769B (zh) |
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109540130A (zh) * | 2018-10-25 | 2019-03-29 | 北京航空航天大学 | 一种连采机惯性导航定位定向方法 |
| CN109579833A (zh) * | 2018-12-04 | 2019-04-05 | 上海航天控制技术研究所 | 一种对返回式运载火箭的垂直着陆阶段的组合导航方法 |
| CN110221333A (zh) * | 2019-04-11 | 2019-09-10 | 同济大学 | 一种车载ins/od组合导航系统的量测误差补偿方法 |
| CN111060096A (zh) * | 2019-12-27 | 2020-04-24 | 武汉迈普时空导航科技有限公司 | 一种mems-imu模块组合里程计的数据处理方法及系统 |
| CN111323050A (zh) * | 2020-03-19 | 2020-06-23 | 哈尔滨工程大学 | 一种捷联惯导和多普勒组合系统标定方法 |
| CN111380516A (zh) * | 2020-02-27 | 2020-07-07 | 上海交通大学 | 基于里程计测量信息的惯导/里程计车辆组合导航方法及系统 |
| CN111811537A (zh) * | 2020-07-02 | 2020-10-23 | 重庆青年职业技术学院 | 一种捷联惯性导航的误差补偿方法及导航系统 |
| CN112254725A (zh) * | 2020-10-19 | 2021-01-22 | 北京航天发射技术研究所 | 一种基于天线转塔的高精度实时测姿装置及方法 |
| CN113503882A (zh) * | 2021-06-03 | 2021-10-15 | 北京自动化控制设备研究所 | 一种车载惯性/地磁组合导航方法及装置 |
| CN113834499A (zh) * | 2021-08-26 | 2021-12-24 | 北京航天发射技术研究所 | 一种车载惯组与里程计行进间对准方法及系统 |
| CN113884102A (zh) * | 2020-07-04 | 2022-01-04 | 华为技术有限公司 | 传感器安装偏差角的标定方法、组合定位系统和车辆 |
| CN114322998A (zh) * | 2021-12-02 | 2022-04-12 | 河北汉光重工有限责任公司 | 一种基于杆臂估计的sins_od组合导航校正方法 |
| CN114383609A (zh) * | 2021-12-22 | 2022-04-22 | 中国煤炭科工集团太原研究院有限公司 | 基于捷联惯导和里程计的组合导航矿井定位方法和装置 |
| CN115615430A (zh) * | 2022-12-21 | 2023-01-17 | 中国船舶集团有限公司第七〇七研究所 | 基于捷联惯导的定位数据修正方法及系统 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2843902B2 (ja) * | 1996-03-04 | 1999-01-06 | 防衛庁技術研究本部長 | 車両用慣性航法装置 |
| CN102997892A (zh) * | 2011-09-15 | 2013-03-27 | 北京自动化控制设备研究所 | 基于惯性/里程计/气压高度陆用导航系统高度组合方法 |
| CN103217157A (zh) * | 2012-01-18 | 2013-07-24 | 北京自动化控制设备研究所 | 一种惯导/里程计自主组合导航方法 |
| CN105318876A (zh) * | 2014-07-09 | 2016-02-10 | 北京自动化控制设备研究所 | 一种惯性里程计组合高精度姿态测量方法 |
| CN105371840A (zh) * | 2015-10-30 | 2016-03-02 | 北京自动化控制设备研究所 | 一种惯性/视觉里程计/激光雷达的组合导航方法 |
| CN105509738A (zh) * | 2015-12-07 | 2016-04-20 | 西北工业大学 | 基于惯导/多普勒雷达组合的车载定位定向方法 |
| CN106595705A (zh) * | 2016-11-22 | 2017-04-26 | 北京航天自动控制研究所 | 一种基于gps的飞行中惯性初始基准偏差估计方法 |
-
2017
- 2017-10-17 CN CN201710965097.3A patent/CN107588769B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2843902B2 (ja) * | 1996-03-04 | 1999-01-06 | 防衛庁技術研究本部長 | 車両用慣性航法装置 |
| CN102997892A (zh) * | 2011-09-15 | 2013-03-27 | 北京自动化控制设备研究所 | 基于惯性/里程计/气压高度陆用导航系统高度组合方法 |
| CN103217157A (zh) * | 2012-01-18 | 2013-07-24 | 北京自动化控制设备研究所 | 一种惯导/里程计自主组合导航方法 |
| CN105318876A (zh) * | 2014-07-09 | 2016-02-10 | 北京自动化控制设备研究所 | 一种惯性里程计组合高精度姿态测量方法 |
| CN105371840A (zh) * | 2015-10-30 | 2016-03-02 | 北京自动化控制设备研究所 | 一种惯性/视觉里程计/激光雷达的组合导航方法 |
| CN105509738A (zh) * | 2015-12-07 | 2016-04-20 | 西北工业大学 | 基于惯导/多普勒雷达组合的车载定位定向方法 |
| CN106595705A (zh) * | 2016-11-22 | 2017-04-26 | 北京航天自动控制研究所 | 一种基于gps的飞行中惯性初始基准偏差估计方法 |
Non-Patent Citations (2)
| Title |
|---|
| FEI GUO 等: "Research of SINS/DVL/OD Integrated Navigation System based on Observability Analysis", 《PROCEEDINGS OF THE 35TH CHINESE CONTROL CONFERENCE》 * |
| 陈文学 等: "基于激光测距法提高车载激光捷联惯导/里程仪组合导航系统导航精度的初步研究", 《光学与光电技术》 * |
Cited By (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109540130A (zh) * | 2018-10-25 | 2019-03-29 | 北京航空航天大学 | 一种连采机惯性导航定位定向方法 |
| CN109579833A (zh) * | 2018-12-04 | 2019-04-05 | 上海航天控制技术研究所 | 一种对返回式运载火箭的垂直着陆阶段的组合导航方法 |
| CN110221333B (zh) * | 2019-04-11 | 2023-02-10 | 同济大学 | 一种车载ins/od组合导航系统的量测误差补偿方法 |
| CN110221333A (zh) * | 2019-04-11 | 2019-09-10 | 同济大学 | 一种车载ins/od组合导航系统的量测误差补偿方法 |
| CN111060096A (zh) * | 2019-12-27 | 2020-04-24 | 武汉迈普时空导航科技有限公司 | 一种mems-imu模块组合里程计的数据处理方法及系统 |
| CN111380516A (zh) * | 2020-02-27 | 2020-07-07 | 上海交通大学 | 基于里程计测量信息的惯导/里程计车辆组合导航方法及系统 |
| CN111380516B (zh) * | 2020-02-27 | 2022-04-08 | 上海交通大学 | 基于里程计测量信息的惯导/里程计车辆组合导航方法及系统 |
| CN111323050A (zh) * | 2020-03-19 | 2020-06-23 | 哈尔滨工程大学 | 一种捷联惯导和多普勒组合系统标定方法 |
| CN111811537A (zh) * | 2020-07-02 | 2020-10-23 | 重庆青年职业技术学院 | 一种捷联惯性导航的误差补偿方法及导航系统 |
| CN111811537B (zh) * | 2020-07-02 | 2023-09-08 | 重庆青年职业技术学院 | 一种捷联惯性导航的误差补偿方法及导航系统 |
| CN113884102A (zh) * | 2020-07-04 | 2022-01-04 | 华为技术有限公司 | 传感器安装偏差角的标定方法、组合定位系统和车辆 |
| CN112254725A (zh) * | 2020-10-19 | 2021-01-22 | 北京航天发射技术研究所 | 一种基于天线转塔的高精度实时测姿装置及方法 |
| CN113503882A (zh) * | 2021-06-03 | 2021-10-15 | 北京自动化控制设备研究所 | 一种车载惯性/地磁组合导航方法及装置 |
| CN113503882B (zh) * | 2021-06-03 | 2023-09-12 | 北京自动化控制设备研究所 | 一种车载惯性/地磁组合导航方法及装置 |
| CN113834499A (zh) * | 2021-08-26 | 2021-12-24 | 北京航天发射技术研究所 | 一种车载惯组与里程计行进间对准方法及系统 |
| CN114322998A (zh) * | 2021-12-02 | 2022-04-12 | 河北汉光重工有限责任公司 | 一种基于杆臂估计的sins_od组合导航校正方法 |
| CN114383609A (zh) * | 2021-12-22 | 2022-04-22 | 中国煤炭科工集团太原研究院有限公司 | 基于捷联惯导和里程计的组合导航矿井定位方法和装置 |
| CN115615430A (zh) * | 2022-12-21 | 2023-01-17 | 中国船舶集团有限公司第七〇七研究所 | 基于捷联惯导的定位数据修正方法及系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107588769B (zh) | 2020-01-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107588769A (zh) | 一种车载捷联惯导、里程计及高程计组合导航方法 | |
| US6459990B1 (en) | Self-contained positioning method and system thereof for water and land vehicles | |
| Fu et al. | High-accuracy SINS/LDV integration for long-distance land navigation | |
| US8229606B2 (en) | Systems and methods for estimating position, attitude, and/or heading of a vehicle | |
| JP5328252B2 (ja) | ナビゲーションシステムの位置検出装置および位置検出方法 | |
| CN100593689C (zh) | 基于捷联惯性导航系统的姿态估计和融合的方法 | |
| JP7073052B2 (ja) | ビークルの角度位置を測定するシステムおよび方法 | |
| CN103245359B (zh) | 一种惯性导航系统中惯性传感器固定误差实时标定方法 | |
| CN104515527B (zh) | 一种无gps信号环境下的抗粗差组合导航方法 | |
| KR20170109646A (ko) | 단일 안테나에 기초한 ｇｎｓｓ―ｉｎｓ 차량 자세 결정 방법 | |
| CN107121141A (zh) | 一种适用于定位导航授时微系统的数据融合方法 | |
| CN104764467B (zh) | 空天飞行器惯性传感器误差在线自适应标定方法 | |
| CN111156994A (zh) | 一种基于mems惯性组件的ins/dr&gnss松组合导航方法 | |
| CN1932444B (zh) | 适用于高速旋转体的姿态测量方法 | |
| CN101571394A (zh) | 基于旋转机构的光纤捷联惯性导航系统初始姿态确定方法 | |
| CN104697526A (zh) | 用于农业机械的捷联惯导系统以及控制方法 | |
| CN101963512A (zh) | 船用旋转式光纤陀螺捷联惯导系统初始对准方法 | |
| CN112432642B (zh) | 一种重力灯塔与惯性导航融合定位方法及系统 | |
| CN108088443A (zh) | 一种定位定向设备速度补偿方法 | |
| CN102116634A (zh) | 一种着陆深空天体探测器的降维自主导航方法 | |
| CN107677292B (zh) | 基于重力场模型的垂线偏差补偿方法 | |
| CN103792561A (zh) | 一种基于gnss通道差分的紧组合降维滤波方法 | |
| CN105928515A (zh) | 一种无人机导航系统 | |
| CN105737842A (zh) | 基于旋转调制和虚拟里程仪的车载自主导航方法 | |
| CN113340298A (zh) | 一种惯导和双天线gnss外参标定方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |