一种车载惯组与里程计行进间对准方法及系统与流程

1.本发明涉及惯性导航技术领域，尤其涉及一种车载惯组与里程计行进 间对准方法及系统。


背景技术：

2.目前，惯导系统在航空、航天、航海以及陆地导航等领域中的有着非 常广泛的应用。尤其在军事领域中，惯导系统更是发挥着巨大的作用。车 载惯导是陆基武器系统中重要的组成部分用于引导武器系统，提供导航信 息。惯导系统在进入导航之前必须完成初始对准，为之后的惯性测量提供 初始姿态，因此，初始对准的精度是影响导航精度的关键因素。车载惯导 的初始对准问题一直以来都是惯导领域研究的热点。近年来，国内车载惯 导普遍采用静态初始对准方式，即在载车启动之前，利用罗经对准、多位 置对准以及旋转调制对准等方法完成静基座的初始对准，该种方式需要车 辆静止一段时间才能出发，为提高武器系统的机动能力和战时反应能力， 可采用行进间对准方式，无需停车等待，可有效提高车辆反应速度，同时 保证设备的自主能力。


技术实现要素：

3.本发明要解决的技术问题在于，针对现有技术的上述缺陷，提供一种 车载惯组与里程计行进间对准方法及系统，旨在解决现有技术中的行进间 对准方法耗费时间长、对准精度低的问题。
4.为了解决上述技术问题，本发明所采用的技术方案如下：
5.第一方面，本发明提供一种车载惯组与里程计行进间对准方法,其中， 所述方法包括：
6.根据惯导和里程计信息，获取惯性系统下的姿态转换矩阵，并根据所 述姿态转换矩阵完成行进间粗对准，得到粗略姿态角信息；
7.建立误差模型，并根据所述误差模型，完成行进间精对准，得到精确 姿态角信息；
8.根据行进间对准得到的精确初始姿态角信息可继续进行导航，实时获 取车辆的姿态角和位置信息。
9.在一种实现方式中，所述根据所述姿态转换矩阵完成行进间粗对准， 得到粗略姿态角信息，包括：
10.对所述姿态矩阵进行分解，并获取所述惯导系统中的重力偏斜；
11.根据所述重力偏斜，对所述重力偏斜进行补偿；
12.对所述姿态矩阵求解，完成行进间粗对准。
13.在一种实现方式中，所述姿态矩阵分解为：其中，n表示导航坐标系，b为捷联惯导坐标系，n0表示初始时刻导航坐标 系，i
b0
表示初始时刻捷联
惯组惯性坐标系，t表示当前时刻，初始时刻为0， i0表示初始时刻惯性坐标系，为初始时刻导航坐标系与导航坐标系的转 换矩阵，为初始时刻捷联惯组惯性坐标系与捷联惯导坐标系的转换矩 阵；为初始时刻捷联惯组惯性坐标系与初始时刻捷联惯组惯性坐标系的 转换矩阵；为初始时刻惯性坐标系与初始时刻导航坐标系的转换矩阵。
14.在一种实现方式中，所述建立误差模型，并根据所述误差模型，完成 行进间精对准，得到精确姿态角信息，包括：建立行进间精对准误差模型， 包含姿态误差、速度误差、位置误差、陀螺漂移误差、加速度计偏置误差、 里程计刻度系数误差、俯仰安装误差角、方位安装误差角和外杆臂误差。
15.在一种实现方式中，所述建立误差模型，并根据所述误差模型，完成 行进间精对准，得到精确姿态角信息，包括：
16.将捷联惯导解算的速度与使用里程计进行航位推算解算的速度作差， 得到差值；
17.将所述差值作为所述观测量。
18.在一种实现方式中，所述根据所述误差模型，完成行进间精对准，得 到精确姿态角信息，包括：
19.获取sage
‑
husa自适应滤波算法，并根据所述sage
‑
husa自适应滤波 算法进行递推计算，得到所述精确姿态角。
20.在一种实现方式中，所述方法还包括：
21.在进行计算时，进行逆向回溯，将不同阶段的解算结果作为下一阶段 进行解算的初始值。
22.第二方面，本实施例还提供一种车载惯组行进间对准算法框架，其中， 所述系统包括：
23.粗对准模块，用于根据惯导和里程计信息，获取惯性系统下的姿态转 换矩阵，并根据所述姿态转换矩阵完成行进间粗对准，得到粗略姿态角信 息；
24.精对准模块，用于建立误差模型，并根据所述误差模型，完成行进间 精对准，得到精确姿态角信息；
25.导航解算模块，用于根据行进间对准得到的精确初始姿态角信息可继 续进行导航，实时获取车辆的姿态角和位置信息。
26.第三方面，本发明实施例还提供一种终端设备，其中，所述终端设备 包括存储器、处理器及存储在所述存储器中并可在所述处理器上运行的车 载惯组行进间对准程序以及导航程序，所述处理器执行所述车载惯组行进 间对准程序时，实现上述方案中任一项所述的车载惯组与里程计行进间对 准方法的步骤。
27.有益效果：与现有技术相比，本发明提供了一种车载惯组与里程计行 进间对准方法，本发明首先在惯导系统中，获取惯性系统下的姿态转换矩 阵，并根据所述姿态转换矩阵完成行进间粗对准，得到粗略姿态角信息。 然后建立误差模型，并根据所述误差模型，完成行进间精对准，得到所述 精确姿态角。最后根据所述精确姿态角，确定姿态角、方位角和位置信息。 本发明可以实现无准备时间行进间对准，并且对准精度与静基座对准精度 相同，有效提高了行进间对准精度和使用性能。
附图说明
28.图1为本发明实施例提供的车载惯组与里程计行进间对准方法的具体 实施方式的流程图。
29.图2为本发明实施例提供的车载惯组与里程计行进间对准方法中载车 行进时惯性系下不同时刻重力矢量示意图。
30.图3为本发明实施例提供的车载惯组与里程计行进间对准方法中中行 进间对准逆向回溯示意图。
31.图4是本发明实施例提供的车载惯组行进间对准系统的原理框图。
32.图5是本发明实施例提供的终端设备的内部结构原理框图。
具体实施方式
33.为使本发明的目的、技术方案及效果更加清楚、明确，以下参照附图 并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施 例仅用以解释本发明，并不用于限定本发明。
34.经研究发现，惯导系统在进入导航之前必须完成初始对准，初始对准 为之后的惯性测量提供基础，初始对准的精度是影响导航精度的关键因素。 因而，车载惯导的初始对准问题一直以来都是惯导领域研究的热点。近年 来，国内车载惯导普遍采用静态初始对准方式，即在载车启动之前，利用 罗经对准、多位置对准以及旋转调制对准等方法完成静基座的初始对准。
35.目前，车载惯导行进间初始对准方案主要分为两种：传递对准和引入 外部辅助信息的对准。传递对准是利用高精度主惯导信息做参考，利用子 惯导与主惯导的信息差，估算出两者之间的失准角，从而实现对子惯导的 对准。传递对准对主惯导精度提出了很高的要求，不仅增加了成本，并且 传递对准的误差补偿技术也是难点所在；外部辅助信息的对准主要采用惯 导/gps/里程计组合方案。gps系统可以实时给出较高精度的位置与速度信 息，但由于gps信号容易被侦查和干扰，系统的自主性和隐蔽性会降低。 惯导系统可以输出完备的导航信息，是一种全自主的导航系统，但是导航 误差随时间发散，因而纯惯导系统不能满足车载武器系统对导航精度的要 求。里程计作为速度观测装置，可以提供精确的速度信息，也可以提取出 航向信息。它具有误差发散缓慢、输出精度高以及成本低廉等优点，因此， 里程计辅助下的惯导行进间对准方案显得更为实际。
36.基于里程计辅助下的惯导要完成行进间对准，必须建立相应的误差传 递模型，利用滤波器或者观测器对状态变量进行估计。在控制理论中，当 把系统考虑成随机系统时，采用卡尔曼滤波器估计状态；当把系统考虑为 确定性系统时，采用状态观测器估计状态。利用卡尔曼滤波器对状态进行 估计是目前大部分学者青睐的方案。但是由于系统误差模型的不完善以及 噪声统计特性未知，导致对准速度较慢，器件误差估计效果差以及系统对 噪声敏感等问题。当系统模型或噪声特性变化较大，甚至会导致滤波结果 发散；对于第二种方案，状态的估计采用状态观测器。经典控制理论由于 缺乏对系统模型特性的应用，导致控制精度不高、收敛速度慢、系统鲁棒 性不强等问题。可见，但是，这些方法耗费时间长、对准精度低并且无法 消除和补偿行进过程中的干扰与误差，从而导致对准精度较低。
37.为了解决上述问题，本发明提供一种车载惯组与里程计行进间对准方 法，具体实
施时，本实施例首先根据惯导和里程计信息，获取惯性系统下 的姿态转换矩阵，并根据所述姿态转换矩阵完成行进间粗对准，得到粗略 姿态角信息。然后建立误差模型，并根据所述误差模型，完成行进间精对 准，得到精确姿态角信息，最后根据行进间对准得到的精确初始姿态角信 息可继续进行导航，实时获取车辆的姿态角和位置信息。
38.采用行进间对准方式无需等待停车等待，可有效提高车辆的反应速度。 同时为保证定位定向设备的自主性能，可在无卫星辅助的情况下实现快速 定向。目前对于行进间对准的需求为：1)无需停车等待，即在车辆行驶过 程中上电进行对准，提高对准的灵活性；2)缩短时间。
39.示例性方法
40.本实施例的方法可应用于终端设备中，所述终端设备为电脑等智能化 产品。具体实施时，如图1中所示，本实施例的车载惯组与里程计行进间 对准方法包括如下步骤：
41.步骤s100、根据惯导和里程计信息，获取惯性系统下的姿态转换矩阵， 并根据所述姿态转换矩阵完成行进间粗对准，得到粗略姿态角信息。
42.具体实施时，本实施例的使用工况为：在车辆行驶过程对定位定向设 备上电，利用里程计提供的速度信息辅助惯导在车辆行驶15min后完成初 始对准，提高系统的机动能力和战时生存能力。具体地，行进状态下由于 存在线速度及线加速度不同时刻重力矢量关系如图2所示。由于载车行进 状态下捷联惯导的角速度远远大于地球自转角速度，传统的解析粗对准算 法在行进状态下对准精度无法满足误差模型线性化要求的小偏差角条件。 因此本实施例基于惯性系思想设计了能够将载体角运动与线运动有效隔离 的对准算法，仅仅根据重力矢量在惯性系下的变化进行对准，在晃动或行 进状态下仍能实现较高的对准精度。
43.本实施例对所述姿态矩阵进行分解，并获取所述惯导系统中的重力偏 斜；然后根据所述重力偏斜，对所述重力偏斜进行补偿；最后对所述姿态 矩阵求解，完成行进间粗对准。具体地，本实施例将姿态转换矩阵分解为： 其中，其中，n表示导航坐标系，b为捷联惯导 坐标系，n0表示初始时刻导航坐标系，i
b0
表示初始时刻捷联惯组惯性坐标 系，t表示当前时刻，初始时刻为0，i0表示初始时刻惯性坐标系，为 初始时刻导航坐标系与导航坐标系的转换矩阵，为初始时刻捷联惯组 惯性坐标系与捷联惯导坐标系的转换矩阵；为初始时刻捷联惯组惯性坐 标系与初始时刻捷联惯组惯性坐标系的转换矩阵；为初始时刻惯性坐 标系与初始时刻导航坐标系的转换矩阵。
44.惯性系下由牛顿第二定律有：
[0045][0046]
其中，也就是产生了重力偏斜，通过行进间对准过程的惯性 定位数据计算进而补偿重力偏斜的影响有利于提高粗对准精度。然 后，递推求解即可确
定完成行进间粗对准。
[0047]
在本实施例中，由于行进间对准过程中，只能在初始时刻装订了一次 位置信息，后续过程将再也无法获得准确位置信息，而此时姿态和方位信 息、里程计准确的刻度系数等参数均未准确获得，直接采用存储的参数进 行行进间对准过程中的位置解算将出现较大误差。为解决该问题，同时为 了降低初始对准时间，本项目采用逆向回溯技术，借助于数据存储的方法， 在行进间对准的不同阶段重复利用相同的数据，采用逆向导航算法，实现 不同阶段解算结果与后一阶段初值之间的衔接，具体如图3中所示。
[0048]
步骤s200、建立误差模型，并根据所述误差模型，完成行进间精对准， 得到精确姿态角信息。
[0049]
具体实施时，本实施例首先建立行进间精对准的误差模型：
[0050][0051][0052][0053][0054][0055][0056][0057][0058][0059][0060][0061]
其中：r
m
为捷联 惯导所在位置处子午圈曲率半径；为东北天方向失准角，为导航坐标 下东北天方向速度误差，为导航坐标系相对地球坐标系的旋转角速度在 导航坐标系下的表示。为地球自转角速率在导航坐标系下的表示；λ和l 分别表示捷联惯导所处位置的经度和纬度；为陀螺零位偏移在捷联惯导 坐标系下的表示；f为加速度计输出值在导航坐标系下的表示，v为捷联惯 导相对于地球表面的速度在导航坐标系下的标识。
[0062]
令则系统方程可以改写 为：
[0063][0064]
然后选择捷联惯导解算的速度与使用里程计进行航位推算解算的速度 之差作为观测量，可以得到：
[0065][0066]
其中：其中：
[0067]
z＝hx+w
v
。
[0068]
接着，本实施例获取sage
‑
husa自适应滤波算法，并根据所述sage
‑
husa自适应滤波算法对所述观测量进行递推计算，得到所述精确姿态角信息。 具体地，本实施例对系统状态方程和观测方程离散化。采用sage
‑
husa自 适应滤波方法进行递推计算，获得准确的姿态转移矩阵(即第二姿态转 移矩阵)，具体如下：
[0069]
将系统状态空间模型离散化后得到：
[0070][0071]
其中，
[0072]
其中r
k
是量测噪声方差阵，采用r
k
的自适应估计方法：
[0073][0074]
步骤s300、根据行进间对准得到的精确初始姿态角信息可继续进行导 航，实时获取车辆的姿态角和位置信息。
[0075]
在本实施例中，对准方案在车辆行进过程中不限制行驶路线、不限制 行进距离也不限制行进状态，不依赖卫星，只需依靠惯导和里程计进行行 进间对准，实现的对准精度与静基座对准精度相当。该技术的应用将有效 拓展定位定向设备的使用便利性。该技术与晃动基座对准技术类似，分为 行进间粗对准技术、行进间精对准技术，同时，为了保证对准完成后惯性 精度，还需实现行进间对准逆向回溯过程。由于行进间对准过程中，只能 在初始时刻装订了一次位置信息，后续过程将再也无法获得准确位置信息， 而此时姿态和方位
信息、里程计准确的刻度系数等参数均未准确获得，直 接采用存储的参数进行行进间对准过程中的位置解算将出现较大误差。为 解决该问题，同时为了降低初始对准时间，本项目采用逆向回溯技术，借 助于数据存储的方法，在行进间对准的不同阶段重复利用相同的数据，采 用逆向导航算法，实现不同阶段解算结果与后一阶段初值之间的衔接。具 体如图3中所示，在进行计算时，将不同阶段的解算结果作为下一阶段进 行解算的初始值。图3中，在行进间对准的开始时刻(t0)，起始时刻装订 完位置后，120s后完成行进间粗对准，获得粗略的姿态转移矩阵，之后进 行精对准，约300秒后完成整个精对准过程，获得准确的姿态转移矩阵， 通过逆向导航算法回溯至精对准初始时刻进行组合定位解算，在 900s(15min)前完成整个行进间对准过程，此后设备便可以持续准确的姿态 角、方位角和位置信息。
[0076]
综上，本实施例首先在惯导系统中，获取惯性系统下的姿态转换矩阵， 并根据所述姿态转换矩阵完成行进间粗对准，得到粗略姿态信息。然后建 立误差模型，并根据所述误差模型，完成行进间精对准，得到所述精确姿 态角。最后根据所述精确姿态角，确定姿态角、方位角和位置信息。本实 施例可以实现无准备时间行进间对准，并且对准精度与静基座对准精度相 同，有效提高了对准精度，并且本实施例无需停车等待，提高对准的灵活 性；还缩短了对准时间，将行进间对准的时间由30min缩短至15min。
[0077]
示例性装置
[0078]
如图4中所示，本实施例还提供一种车载惯组行进间对准系统，该装 系统包括：粗对准模块10、精对准模块20以及导航解算模块30。具体地， 所述粗对准模块10，用于根据惯导和里程计信息，获取惯性系统下的姿态 转换矩阵，并根据所述姿态转换矩阵完成行进间粗对准，得到粗略姿态角 信息。所述精对准模块20，用于建立误差模型，并根据所述误差模型，完 成行进间精对准，得到精确姿态角信息。所述导航解算模块30，用于根据 行进间对准得到的精确初始姿态角信息可继续进行导航，实时获取车辆的 姿态角和位置信息。
[0079]
具体实施时，本实施例的使用工况为：在车辆行驶过程对定位定向设 备上电，利用里程计提供的速度信息辅助惯导在车辆行驶15min后完成初 始对准，提高系统的机动能力和战时生存能力。具体地，行进状态下由于 存在线速度及线加速度不同时刻重力矢量关系如图2所示。由于载车行进 状态下捷联惯导的角速度远远大于地球自转角速度，传统的解析粗对准算 法在行进状态下对准精度无法满足误差模型线性化要求的小偏差角条件。 因此本实施例基于惯性系思想设计了能够将载体角运动与线运动有效隔离 的对准算法，仅仅根据重力矢量在惯性系下的变化进行对准，在晃动或行 进状态下仍能实现较高的对准精度。
[0080]
在一种实现方式中，所述粗对准模块10包括：
[0081]
分解单元，用于对所述姿态矩阵进行分解，并获取所述惯导系统中的 重力偏斜；
[0082]
解算单元，用于根据所述重力偏斜，对所述重力偏斜进行补偿；
[0083]
粗对准单元，用于对所述姿态矩阵求解，完成行进间粗对准。
[0084]
具体地，本实施例中的所述姿态矩阵分解为：
[0085]
其中，n表示导航坐标系，b为捷联惯导坐标系， n0表示初始时刻导航坐标系，i
b0
表示初始时刻捷联惯组惯性坐标系，t表示 当前时刻，初始时刻为0，i0表示初始时刻惯性坐标系，为初始时刻导 航坐标系与导航坐标系的转换矩
阵，为初始时刻捷联惯组惯性坐标系 与捷联惯导坐标系的转换矩阵；为初始时刻捷联惯组惯性坐标系与初始 时刻捷联惯组惯性坐标系的转换矩阵；为初始时刻惯性坐标系与初始 时刻导航坐标系的转换矩阵。
[0086]
惯性系下由牛顿第二定律有：
[0087][0088]
其中，也就是产生了重力偏斜，通过行进间对准过程的惯性 定位数据计算进而补偿重力偏斜的影响有利于提高粗对准精度。然 后，递推求解即可确定完成行进间粗对准。
[0089]
在一种实现方式中，所述精对准模块20包括：
[0090]
推算单元，用于将捷联惯导解算的速度与使用里程计进行航位推算解 算的速度作差，得到差值；
[0091]
观测量确定单元，用于将所述差值作为所述观测量；
[0092]
精对准单元，用于获取sage
‑
husa自适应滤波算法，并根据所述 sage
‑
husa自适应滤波算法进行递推计算，得到所述精确姿态角。
[0093]
基于上述实施例，本发明还提供了一种终端设备，其原理框图可以如 图5所示。该终端设备包括通过系统总线连接的处理器、存储器、网络接 口、显示屏、温度传感器。其中，该终端设备的处理器用于提供计算和控 制能力。该终端设备的存储器包括非易失性存储介质、内存储器。该非易 失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储 介质中的操作系统和计算机程序的运行提供环境。该终端设备的网络接口 用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实 现一种车载惯组与里程计行进间对准方法。该终端设备的显示屏可以是液 晶显示屏或者电子墨水显示屏，该终端设备的温度传感器是预先在终端设 备内部设置，用于检测内部设备的运行温度。
[0094]
本领域技术人员可以理解，图5中示出的原理框图，仅仅是与本发明 方案相关的部分结构的框图，并不构成对本发明方案所应用于其上的终端 设备的限定，具体的终端设备以包括比图中所示更多或更少的部件，或者 组合某些部件，或者具有不同的部件布置。
[0095]
在一个实施例中，提供了一种终端设备，终端设备包括存储器、处理 器及存储在存储器中并可在处理器上运行的车载惯组行进间对准程序，处 理器执行车载惯组行进间对准程序时，实现如下操作指令：
[0096]
根据惯导和里程计信息，获取惯性系统下的姿态转换矩阵，并根据所 述姿态转换矩阵完成行进间粗对准，得到粗略姿态角信息；
[0097]
建立误差模型，并根据所述误差模型，完成行进间精对准，得到精确 姿态角信息；
[0098]
根据行进间对准得到的精确初始姿态角信息可继续进行导航，实时获 取车辆的姿态角和位置信息。
[0099]
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流 程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序 可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时， 可包括如上述各方法的实施例的流程。
其中，本发明所提供的各实施例中 所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易 失性和/或易失性存储器。非易失性存储器可包括只读存储器(rom)、可 编程rom(prom)、电可编程rom(eprom)、电可擦除可编程rom(eeprom) 或闪存。易失性存储器可包括随机存取存储器(ram)或者外部高速缓冲存 储器。作为说明而非局限，ram以多种形式可得，诸如静态ram(sram)、 动态ram(dram)、同步dram(sdram)、双数据率sdram(ddrsdram)、 增强型sdram(esdram)、同步链路(synchlink)dram(sldram)、存储 器总线(rambus)直接ram(rdram)、直接存储器总线动态ram(drdram)、 以及存储器总线动态ram(rdram)等。
[0100]
综上，本发明公开了一种车载惯组与里程计行进间对准方法及系统， 所述方法包括：根据惯导和里程计信息，获取惯性系统下的姿态转换矩阵， 并根据所述姿态转换矩阵完成行进间粗对准，得到粗略姿态角信息；建立 误差模型，并根据所述误差模型，完成行进间精对准，得到精确姿态角信 息，根据行进间对准得到的精确初始姿态角信息可继续进行导航，实时获 取车辆的姿态角和位置信息。本发明可以实现无准备时间行进间对准，并 且对准精度与静基座对准精度相同，有效提高了对准精度，并且本发明无 需停车等待，提高对准的灵活性，还缩短了对准时间。
[0101]
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对 其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通 技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修 改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不 使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。