一种车载捷联惯导、里程计及高程计组合导航方法

摘要

本发明公开了一种车载捷联惯导、里程计及高程计组合导航方法，该方法包括如下步骤：捷联惯导初始对准；捷联惯导进行惯性解算，里程计和高程计也开始工作；计算平均速度观测量和高度观测量；通过卡尔曼滤波得到捷联惯导误差、里程计刻度系数误差、捷联惯导安装误差、杆臂误差及高程计误差；对捷联惯导误差、里程计刻度系数误差、捷联惯导安装误差、杆臂误差及高程计误差进行校正；校正后的捷联惯导姿态信息、速度信息及位置信息作为导航信息进行输出；返回惯性解算步骤。本发明使导航系统不依赖外部设备、不受外部信号干扰，能为“停车就打”提供高精度的位置和方位基准信息，该组合导航方法还具有快速、简便、工程实用性强等突出优点。

权利要求

1.一种车载捷联惯导、里程计及高程计组合导航方法，其特征在于：该方法包括如下步骤，步骤1，在载车静止时，捷联惯导进行初始对准，并在初始对准完成后捷联惯导进入组合导航状态；

步骤2，捷联惯导进行惯性解算，得到惯导输出位置、速度、姿态及高度，计算杆臂速度，同时通过里程计得到里程计输出速度以及通过高程计得到高程计输出高度；

步骤3，在卡尔曼滤波周期内对惯导输出速度、计算的杆臂速度及里程计输出速度分别进行积分运算，得到惯导里程增量、杆臂里程增量及里程计里程增量，将惯导里程增量、杆臂里程增量及里程计里程增量分别除以卡尔曼滤波周期，得到惯导平均速度、杆臂平均速度及里程计平均速度，然后计算平均速度观测量；通过惯导输出高度和高程计输出高度得到高度观测量；

步骤4，将所述平均速度观测量和所述高度观测量输入至卡尔曼滤波器中，从而得到捷联惯导误差估计值、杆臂误差估计值、捷联惯导安装误差估计值、里程计刻度系数误差估计值及高程计误差估计值；

步骤5，利用上述捷联惯导误差估计值对捷联惯导误差进行校正，利用里程计刻度系数误差估计值对里程计刻度系数进行校正，利用捷联惯导安装误差对捷联惯导安装矩阵进行修正，利用所述杆臂误差估计值对杆臂进行校正，利用所述高程计误差估计值对高程计误差进行校正；校正后的捷联惯导速度信息、位置信息及姿态信息作为导航信息进行输出；然后返回步骤2。

2.根据权利要求1所述的车载捷联惯导、里程计及高程计组合导航方法，其特征在于：步骤2中，利用捷联惯导姿态矩阵和杆臂长度估计值计算杆臂速度：

其中， 表示计算的杆臂速度， 表示捷联惯导计算的姿态矩阵， 表示惯导载体坐标系b相对地球的速度在捷联惯导载体坐标系b上的投影， 表示计算的杆臂长度。

3.根据权利要求2所述的车载捷联惯导、里程计及高程计组合导航方法，其特征在于：步骤2中，利用里程计输出脉冲个数计算里程计输出速度；在载车坐标系m下矢量形式的里程计输出速度为：其中， 表示在载车坐标系m下矢量形式的里程计输出速度， 表示里程计刻度系数，Ni表示里程计输出脉冲数，Ts表示采样周期；

通过如下方式将载车坐标系m下的里程计输出速度转换到导航坐标系n下的里程计输出速度：其中， 表示捷联惯导安装矩阵。

4.根据权利要求3所述的车载捷联惯导、里程计及高程计组合导航方法，其特征在于：步骤3中，平均速度观测量通过下式计算：

其中，z1表示平均速度观测量，T表示卡尔曼滤波周期， 表示捷联惯导输出的速度。

5.根据权利要求4所述的车载捷联惯导、里程计及高程计组合导航方法，其特征在于：步骤3中，通过如下方式得到高度观测量：

其中，z2表示高度观测量， 表示惯导输出高度， 表示高程计输出高度。

6.根据权利要求5所述的车载捷联惯导、里程计及高程计组合导航方法，其特征在于：步骤4中，所述卡尔曼滤波器中使用如下状态方程：

其中， 表示捷联惯导平台失准角变化率， 表示捷联惯导速度误差变化率，表示捷联惯导位置误差变化率， 表示捷联惯导陀螺常值漂移变化率， 表示捷联惯导加速度计常值漂移变化率， 表示里程计刻度系数误差变化率， 表示捷联惯导俯仰安装误差变化率， 表示捷联惯导方位安装误差变化率， 表示杆臂误差变化率， 表示高程计误差变化率， 表示导航坐标系n相对惯性系角速度在导航坐标系n下的投影，εb表示陀螺常值漂移， 表示陀螺随机噪声，fb表示惯导比力， 表示地球自转角速度在导航坐标系n上的投影， 表示导航坐标系n相对地球转动角速度在导航坐标系n上的投影，δVn为惯导输出速度误差，Vn表示惯导输出速度， 表示加速度计常值漂移， 表示加速度计随机噪声，δP表示惯导位置误差，RM表示地球子午圈曲率半径，h为捷联惯导所在高度，L为捷联惯导所在维度，VN为北向速度，VE为东向速度，RN为地球卯酉圈半径；wb表示白噪声。

说明书

一种车载捷联惯导、里程计及高程计组合导航方法

技术领域

[0001] 本发明涉及车载导航技术领域，更为具体来说，本发明为一种车载捷联惯导、里程计及高程计组合导航方法。

背景技术

[0002] 目前，车载导弹在任何地点任何时间往往需要满足能实现停车即打的能力，这就要求车载导航系统必须提供高精度连续的位置信息和姿态信息。传统车载导航系统为实现高精度的定位和精确的姿态，使用的是捷联惯导和卫星导航组合导航的方式。但是，卫星导航易受电子干扰的影响，特别在军事应用上的影响更为明显。

[0003] 因此，如何减弱甚至消除电子干扰对车载导航的影响、提高车载导航精度和准确度，成为了本领域技术人员亟待解决的技术问题和始终研究的重点。

发明内容

[0004] 为解决常规车载导航系统易受电子干扰的影响的问题，本发明创新地提出了一种车载捷联惯导、里程计及高程计组合导航方法，其具有不依赖外部设备、不受外部信号干扰的特点，为车载导弹提供自主的定位、定向信息。

[0005] 为实现上述的技术目的，本发明公开了一种车载捷联惯导、里程计及高程计组合导航方法，该方法包括如下步骤，

[0006] 步骤1，在载车静止时，捷联惯导进行初始对准，并在初始对准完成后捷联惯导进入组合导航状态；

[0007] 步骤2，捷联惯导进行惯性解算，得到惯导输出位置、速度、姿态及高度，计算杆臂速度，同时通过里程计得到里程计输出速度以及通过高程计得到高程计输出高度；

[0008] 步骤3，在卡尔曼滤波周期内对惯导输出速度、计算的杆臂速度及里程计输出速度分别进行积分运算，得到惯导里程增量、杆臂里程增量及里程计里程增量，将惯导里程增量、杆臂里程增量及里程计里程增量分别除以卡尔曼滤波周期，得到惯导平均速度、杆臂平均速度及里程计平均速度，然后计算平均速度观测量；通过惯导输出高度和高程计输出高度得到高度观测量；

[0009] 步骤4，将所述平均速度观测量和所述高度观测量输入至卡尔曼滤波器中，从而得到捷联惯导误差估计值、杆臂误差估计值、捷联惯导安装误差估计值、里程计刻度系数误差估计值及高程计误差估计值；

[0010] 步骤5，利用上述捷联惯导误差估计值对捷联惯导误差进行校正，利用里程计刻度系数误差估计值对里程计刻度系数进行校正，利用所述捷联惯导安装误差对捷联惯导安装矩阵进行修正，利用所述杆臂误差估计值对杆臂进行校正，利用所述高程计误差估计值对高程计误差进行校正；校正后的捷联惯导速度信息、位置信息及姿态信息作为导航信息进行输出；然后返回步骤2。

[0011] 本发明可直接应用于具有捷联惯导系统、里程计、气压高程计的车载导航系统，本发明能够使车载导航系统不受外部信号干扰，能够完全自主地为车载导弹提供可靠的导航信息，是一种较理想的车载组合导航方案。

[0012] 进一步地，步骤2中，利用捷联惯导姿态矩阵和杆臂长度估计值计算杆臂速度：

[0013]

[0014] 其中， 表示计算的杆臂速度， 表示捷联惯导计算的姿态矩阵， 表示惯导载体坐标系b相对地球的速度在捷联惯导载体坐标系b上的投影， 表示计算的杆臂长度。

[0015] 进一步地，步骤2中，利用里程计输出脉冲个数计算里程计输出速度；在载车坐标系m下矢量形式的里程计输出速度为：

[0016]

[0017] 其中， 表示在载车坐标系m下矢量形式的里程计输出速度， 表示里程计刻度系数，Ni表示里程计输出脉冲数，Ts表示采样周期；

[0018] 通过如下方式将载车坐标系m下的里程计输出速度转换到导航坐标系n下的里程计输出速度：

[0019]

[0020]

[0021] 其中， 表示捷联惯导安装矩阵。

[0022] 进一步地，步骤3中，平均速度观测量通过下式计算：

[0023]

[0024] 其中，表示平均速度观测量，T表示卡尔曼滤波周期， 表示捷联惯导输出的速度。

[0025] 采用捷联惯导和里程计平均速度量测量，可以一定程度上消除载车振动及量化误差对组合导航的影响。

[0026] 进一步地，步骤3中，通过如下方式得到高度观测量：

[0027]

[0028] 其中，z2表示高度观测量， 表示惯导输出高度， 表示高程计输出高度。

[0029] 进一步地，步骤4中，所述卡尔曼滤波器中使用如下状态方程：

[0030]

[0031]

[0032]

[0033] 其中， 表示捷联惯导平台失准角变化率， 表示捷联惯导速度误差变化率，表示捷联惯导位置误差变化率， 表示捷联惯导陀螺常值漂移变化率， 表示捷联惯导加速度计常值漂移变化率， 表示里程计刻度系数误差变化率， 表示捷联惯导俯仰安装误差变化率， 表示捷联惯导方位安装误差变化率， 表示杆臂误差变化率，表示高程计误差变化率， 表示导航坐标系n相对惯性系角速度在导航坐标系n下的投影，εb表示陀螺常值漂移， 表示陀螺随机噪声，fb表示惯导比力， 表示地球自转角速度在导航坐标系n上的投影， 表示导航坐标系n相对地球转动角速度在导航坐标系n上的投影，δVn为惯导输出速度误差，Vn表示惯导输出速度，▽b表示加速度计常值漂移， 表示加速度计随机噪声，δP表示惯导位置误差，RM表示地球子午圈曲率半径，h为捷联惯导所在高度，L为捷联惯导所在维度，VN为北向速度，VE为东向速度，RN为地球卯酉圈半径。

[0034] 进一步地，步骤4中，所述的卡尔曼滤波器使用如下的观测方程：

[0035]

[0036] z2＝δh-δhb

[0037] 其中，μ＝[δα 0 δβ]T为捷联惯导安装误差，δh为捷联惯导高度误差，δhb为气压高程计误差。

[0038] 基于上述改进的技术方案，可以更为准确的估计出捷联惯导等的各项误差，以便对其进行校正。

[0039] 进一步地，步骤5中，通过下式对各项误差进行校正：

[0040]

[0041]

[0042]

[0043]

[0044]

[0045] k＝1-δk；

[0046]

[0047]

[0048] 基于上述改进的技术方案，本发明车载捷联惯导/里程计/高程计组合导航方法可估计出陀螺常值漂移、加速度计常值零偏、捷联惯导安装误差、里程计刻度系数误差、杆臂误差等，有效地减小水平和高程位置误差发散速度。同时，能估计出捷联惯导的姿态误差、速度误差、位置误差，因此，修正后的捷联惯导姿态、速度和位置可以作为组合导航的导航结果。

[0049] 进一步地，所述杆臂设置于捷联惯导惯性测量单元的敏感中心与里程计速度测量点之间；在导航坐标系n下，按照如下方式确定捷联惯导惯性测量单元的敏感中心速度、里程计输出速度、杆臂速度之间的关系：

[0050]

[0051] 其中， 表示捷联惯导惯性测量单元的敏感中心速度， 表示里程计输出速度， 表示杆臂速度。

[0052] 本发明的有益效果为：本发明可直接应用于具有捷联惯导系统、里程计、气压高程计的车载导航系统，这三种传感器工作都不依赖于外部设备，从而本发明能够使车载导航系统不受外部信号干扰，能够完全自主地为车载导弹提供可靠的导航信息，是一种较理想的车载导航方案。

[0053] 该组合导航方法不依赖外部设备，不受外部信号干扰，能够独立自主的为车载导弹提供较高精度的位置信息，同时该组合导航方法能保证方位误差不发散，能为“停车就打”提供高精度的方位基准信息。另外，该组合导航方法还具有快速、简便、工程实用性强等突出优点，可为车载定位定向系统提供自主可靠的组合导航方法。

附图说明

[0054] 图1为车载捷联惯导、里程计及高程计组合导航方法流程示意图。

[0055] 图2为车载捷联惯导、里程计及高程计组合导航方法的实施框图。

具体实施方式

[0056] 下面结合说明书附图对本发明的车载捷联惯导、里程计及高程计组合导航方法进行详细的解释和说明。

[0057] 如图1和图2所示，本发明具体公开了一种车载捷联惯导、里程计及高程计组合导航方法，该组合导航方法的核心改进点在于增加了杆臂速度模型、高程计误差模型以及卡尔曼滤波器中平均速度观测量的设计；里程计辅助捷联惯导水平位置误差发散速度较小，高度误差发散速度较快，引入高程计后，使得高度误差发散变小；而对于姿态信息的输出，本发明利用捷联惯导本身校正后的姿态信息。

[0058] 具体来说，该方法包括如下步骤。

[0059] 步骤1，在载车静止时，捷联惯导进行初始对准，以获得惯性导航的位置、速度、姿态初始值。并且在初始对准完成后捷联惯导进入组合导航状态，初始对准后，载车可以启动。

[0060] 步骤2，捷联惯导进行惯性解算，得到惯导输出位置、速度、姿态及高度，计算杆臂速度，同时通过里程计得到里程计输出速度以及通过高程计得到高程计输出高度；具体如下。

[0061] 本实施例中，车载里程计采用霍尔传感器，该霍尔传感器加装于载车传动轴上，并以脉冲形式输出非转向轮轴中心点的前进速度，利用里程计输出脉冲个数计算里程计输出速度；本实施例的载车坐标系m为右前上坐标系；考虑载车正常行驶时车辆无侧滑、无打滑且无跳跃的情况下，在载车坐标系m下矢量形式的里程计输出速度为：

[0062]

[0063] 其中，上述 表示在载车坐标系m下矢量形式的里程计输出速度， 表示里程计刻度系数，Ni表示里程计输出脉冲数，Ts表示采样周期。

[0064] 由于组合导航需要输出导航坐标系(n系)的位置，所以本发明将里程计在载车坐标系下的输出转换到导航坐标系，设捷联惯导载体坐标系为b系，考虑到b系与m系不重合，则b系和n系下里程计的理想输出分别为：

[0065]

[0066]

[0067] 其中， 表示捷联惯导姿态矩阵， 表示捷联惯导安装矩阵； 由俯仰安装角α、横滚安装角γ及方位安装角β组成，通过计算得到里程计输出与横滚安装角γ无关，因此，里程计输出也与横滚安装误差角无关。考虑到里程计刻度系数误差δk、捷联惯导俯仰安装误差δα、捷联惯导方位安装误差δβ及捷联惯导的平台失准角 则里程计实际计算的速度可写成：

[0068]

[0069] 其中， μ＝[δα 0 δβ]T，而×表示矢量叉乘符号。

[0070] 本实施例中，杆臂长度为捷联惯导惯性测量单元的敏感中心与里程计速度测量点之间的距离，当载车有角运动时，捷联惯导惯性测量单元(IMU)与里程计速度测量点之间的速度有差异，在组合导航时，由杆臂引起的速度差异与组合导航系统的误差传播无关，如不消除就会影响组合导航精度；在导航坐标系n下，按照如下方式确定捷联惯导惯性测量单元的敏感中心速度、里程计输出速度、杆臂速度之间的关系：

[0071]

[0072]

[0073]

[0074] 其中， 表示捷联惯导惯性测量单元的敏感中心速度， 表示里程计输出速度， 表示杆臂速度， 表示惯导载体坐标系b相对地球的速度在捷联惯导载体坐标系b上的投影，lb表示杆臂长度，δlb表示杆臂误差估计值。

[0075] 步骤3，在卡尔曼滤波周期内对惯导输出速度、计算的杆臂速度及里程计输出速度分别进行积分运算，得到惯导里程增量、杆臂里程增量及里程计里程增量，将惯导里程增量、杆臂里程增量及里程计里程增量分别除以卡尔曼滤波周期，得到惯导平均速度、杆臂平均速度及里程计平均速度，然后计算平均速度观测量；通过惯导输出高度和高程计输出高度得到高度观测量。

[0076] 需要说明的是，里程计本质属于速度传感器，因此量测量应为速度量测量。由于捷联惯导与里程计非理想刚性连接，载车机动时，惯导安装处存在振动，该振动引起捷联惯导速度与里程计测量速度存在误差，且里程计采样周期一般很短，造成里程计输出的瞬时速度存在很大的量化误差，为消除振动及量化误差对组合导航的影响，采用滤波周期的平均速度作为观测量，即对观测量在一段时间进行积分，然后除以滤波周期，得到平均观测量，具体通过如下方式得到平均速度观测量：

[0077]

[0078]

[0079] 其中，z1为平均速度观测量，T为卡尔曼滤波周期， 为捷联惯导安装处理想速度，δVn为捷联惯导速度误差。

[0080] 考虑到实际卡尔曼滤波周期比较短，在滤波周期内子惯导速度误差、失准角、里程计刻度系数误差、捷联惯导安装误差、杆臂误差近似为常值，将式(5)、式(8)与式(16)代入式(15)，可得观测方程如下：

[0081]

[0082] 本发明中，由气压高程计和捷联惯导输出的高度测量量，通过下式得到高度观测量：

[0083]

[0084] 其中，z2表示高度观测量， 表示惯导输出高度， 表示高程计输出高度。

[0085] 考虑到捷联惯导与气压高程计的高度误差，可以得到高度观测方程：

[0086] z2＝δh-δhb

[0087] 其中，δh为捷联惯导输出高度误差，δhb为高程计误差。

[0088] 步骤4，将平均速度观测量和高度观测量输入至卡尔曼滤波器中，到达卡尔曼滤波周期进行滤波解算，执行卡尔曼滤波算法(其具体算法为本领域的常规技术手段)，从而得到捷联惯导误差估计值、杆臂误差估计值、捷联惯导安装误差估计值、里程计刻度系数误差估计值及高程计误差估计值。

[0089] 步骤5，对捷联惯导误差、里程计刻度系数误差、捷联惯导安装误差、杆臂误差及高程计误差进行校正；具体地，利用上述捷联惯导误差估计值对捷联惯导误差进行校正，利用里程计刻度系数误差估计值对里程计刻度系数进行校正，利用所述捷联惯导安装误差对捷联惯导安装矩阵进行修正，利用所述杆臂误差估计值对杆臂进行校正，利用所述高程计误差估计值对高程计误差进行校正；校正后的捷联惯导速度信息、位置信息及姿态信息作为导航信息进行输出；然后返回步骤2。上述步骤2、3、4、5不断迭代进行，从而不断输出组合导航信息。

[0090] 具体实施时，对于上述式(4)，忽略二阶小量，则可得到里程计测速模型：

[0091]

[0092]

[0093]

[0094] μ＝[δα 0 δβ]T

[0095] 其中， 表示利用里程计实际计算的速度， 表示在导航坐标系n下矢量形式的里程计输出速度， 表示在捷联惯导在载体坐标系b下矢量形式的里程计输出速度， 表示捷联惯导姿态矩阵， 表示捷联惯导安装矩阵，δα表示捷联惯导俯仰安装误差估计值，δβ表示捷联惯导方位安装误差估计值，δk表示里程计刻度系数误差估计值， 表示捷联惯导的平台失准角。

[0096] 具体实施时，对于上述式(8)，忽略二阶小量，得到如下杆臂速度模型：

[0097]

[0098] 其中， 表示实际计算的杆臂速度， 表示惯导载体坐标系b相对地球的速度在捷联惯导载体坐标系b上的投影，lb表示杆臂长度，δlb表示杆臂误差估计值。

[0099] 气压高程计利用气压表测量载车周围环境的大气压力，利用标准大气模型确定高度。因此，气压高程计不需要外部设备信息，其不受外部信号干扰具有完全自主的特点，本发明捷联惯导/里程计/高程计组合后仍具有完全自主的特点，引入高程计后可以抑制高度通道的发散趋势。由于载车实际所处的实际基准海平面大气参数与标准海平面的大气参数存在差异以及大气压力测量误差造成气压高程计测量存在误差，本发明将气压高程计误差δhb建模为一阶马尔可夫过程：

[0100]

[0101] 其中， 表示修正后的高程计误差，δhb表示标准高程计误差，τb表示相关时间，wb表示白噪声。

[0102] 具体实施时，本发明捷联惯导/里程计/高程计组合导航系统状态使用22维：

[0103]

[0104] 本发明设计的状态方程包括捷联惯导误差方程、惯性器件误差模型、里程计刻度系数误差模型、捷联惯导安装误差模型、杆臂误差模型、高程计误差模型，卡尔曼滤波器中使用如下状态方程：

[0105]

[0106]

[0107]

[0108] 其中， 表示捷联惯导平台失准角变化率， 表示捷联惯导速度误差变化率，表示捷联惯导位置误差变化率， 表示捷联惯导陀螺常值漂移变化率， 表示捷联惯导加速度计常值漂移变化率， 表示里程计刻度系数误差变化率， 表示捷联惯导俯仰安装误差变化率， 表示捷联惯导方位安装误差变化率， 表示杆臂误差变化率，表示高程计误差变化率， 表示导航坐标系n相对惯性系角速度在导航坐标系n下的投影，εb表示陀螺常值漂移， 表示陀螺随机噪声，fb表示惯导比力， 表示地球自转角速度在导航坐标系n上的投影， 表示导航坐标系n相对地球转动角速度在导航坐标系n上的投影，δVn为惯导输出速度误差，Vn表示惯导输出速度，▽b表示加速度计常值漂移， 表示加速度计随机噪声，δP表示惯导位置误差，RM表示地球子午圈曲率半径，h为捷联惯导所在高度，L为捷联惯导所在维度，VN为北向速度，VE为东向速度，RN为地球卯酉圈半径。

[0109] 从上式可以看出，里程计刻度系数误差、捷联惯导俯仰安装误差、捷联惯导方位安装误差和杆臂距离误差都建模为随机常值。

[0110] 将本发明中的状态方程、量测方程、观测方程离散化(具体离散处理过程为本领域技术人员的常规技术手段)，可得到

[0111]

[0112] 其中，wk,vk均为离散化的零均值白噪声，其协方差阵分别为Qk,Rk。由于状态方程和观测方程都是线性方程，因此可以采用标准卡尔曼滤波算法进行组合导航。

[0113] 采用标准卡尔曼滤波后得到各项误差估计值，根据各误差模型，通过下式对各项误差进行校正。

[0114]

[0115]

[0116]

[0117]

[0118]

[0119] k＝1-δk

[0120]

[0121]

[0122] 在本说明书的描述中，参考术语“本实施例”、“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

[0123] 以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明实质内容上所作的任何修改、等同替换和简单改进等，均应包含在本发明的保护范围之内。