一种惯性导航系统的预积分算法的制作方法

本发明属于惯性导航技术领域，具体涉及一种捷联惯性导航系统的预积分算法。

背景技术：

在水下惯性导航系统中，载体惯性数据输出频率较高，约为200hz。但水下组合导航中常用到的声学传感器数据更新频率较低，约10s更新一次。在多传感器数据融合优化过程中，常使用求系统最大后验概率的方法对载体进行导航定位，而若常规惯性因子以惯性量测的速率添加进数据融合过程中，会使数据融合计算负荷增大，实时性变差。

针对该问题，普遍方式为在惯性数据中每隔几拍采一次有效数据，采用抽取的数据按较低的频率进行解算，此种方式可以明显提高运算效率，降低运算复杂度，但是该种方式没有将全部数据充分利用，容易损失导航精度。在水下需要进行长时间导航的环境中，往往对导航精度要求较高，因此需要一种既可以降低数据融合算法中的运算复杂度，充分利用惯性量测数据的计算方法。

技术实现要素：

本发明解决的技术问题：提出一种惯性导航系统的预积分算法，在水下组合导航方法中，实现在不丢失惯性数据的基础上降低惯性解算频率，以提高算法的实时性。

本发明的技术方案：

一种惯性导航系统的预积分算法，包括以下步骤：

步骤1：初始化定义；

步骤2：计算预积分结果；

步骤3：计算等效旋转矢量；

步骤4：计算连续两个时刻之间机体系的旋转关系；

步骤5：惯性状态解算。

步骤1所述的初始化定义，具体为初始化运动载体的位置信息速度信息和姿态信息其中：式中△px、△py、△pz是x、y、z三个方向的位置变化量，△vx、△vy、△vz是x、y、z三个方向的速度变化量，是ti时刻到ti 1时刻的姿态变化量，ti是载体运动初始时刻，i是单位矩阵。

步骤2所述的计算预积分结果，具体为：当有新的量测数据在tj 1时刻加入系统中时，计算更新位置变化量更新速度变化量更新姿态变化量计算预积分△xi→j 1，其中tj 1＝tj △t，tj是运动载体预积分计算时刻，△t是新的量测数据到来的时间间隔。

所述的计算更新位置变化量计算公式为：

计算更新速度变化量计算公式为：

式中fj是新增惯性量测；

计算更新姿态变化量计算公式为：式中为tj时刻到tj 1时刻的姿态信息；

计算预积分△xi→j 1，计算公式为：

步骤3所述的计算等效旋转矢量具体为：在速度更新周期t＝tj 1-tj较短的情况下，等效旋转矢量φ计算如下：

其中，式中，是导航系相对于机体系的角速度，是惯性系相对于机体系的角速度，是ti时刻运动载体的姿态矩阵，是导航系相对于惯性系的角速度。

步骤4所述的计算连续两个时刻之间机体系的旋转关系，计算公式如下：

步骤5所述的惯性状态解算，包括导航系统的位置速度和姿态的解算，计算公式分别为：

为ti时刻到tj时刻的姿态旋转矩阵，为ti时刻导航系统的位置，为ti时刻机体系的姿态矩阵，为ti时刻的速度，为ti时刻所在位置的重力加速度，为ti时刻运动载体的角速度。

本次预积分过程结束，在下一次预积分开始时，将参考坐标系选为此时的机体坐标系，并进行下列初始化：令进入下一次预积分过程。

本发明的有益效果：该惯性导航系统的预积分算法，在水下组合导航方法中，可实现在不丢失惯性数据的前提下降低惯性解算频率，降低运算时间，提高算法的实时性。

具体实施方式

以下对本发明实施例做进一步详述：

一种惯性导航系统的预积分算法，包括以下步骤：

步骤1：初始化定义；

步骤2：计算预积分结果；

步骤3：计算等效旋转矢量；

步骤4：计算连续两个时刻之间机体系的旋转关系；

步骤5：惯性状态解算。

步骤1所述的初始化定义，具体为初始化运动载体的位置信息速度信息和姿态信息其中：式中△px、△py、△pz是x、y、z三个方向的位置变化量，△vx、△vy、△vz是x、y、z三个方向的速度变化量，是ti时刻到ti 1时刻的姿态变化量，ti是载体运动初始时刻，i是单位矩阵。

步骤2所述的计算预积分结果，具体为：当有新的量测数据在tj 1时刻加入系统中时，计算更新位置变化量更新速度变化量更新姿态变化量计算预积分△xi→j 1，其中tj 1＝tj △t，tj是运动载体预积分计算时刻，△t是新的量测数据到来的时间间隔。

所述的计算更新位置变化量计算公式为：

计算更新速度变化量计算公式为：

式中fj是新增惯性量测；

计算更新姿态变化量计算公式为：式中为tj时刻到tj 1时刻的姿态信息；

计算预积分△xi→j 1，计算公式为：

步骤3所述的计算等效旋转矢量具体为：在速度更新周期t＝tj 1-tj较短的情况下，等效旋转矢量φ计算如下：

其中，式中，是导航系相对于机体系的角速度，是惯性系相对于机体系的角速度，是ti时刻运动载体的姿态矩阵，是导航系相对于惯性系的角速度。

步骤4所述的计算连续两个时刻之间机体系的旋转关系，计算公式如下：

步骤5所述的惯性状态解算，包括导航系统的位置速度和姿态的解算，计算公式分别为：

为ti时刻到tj时刻的姿态旋转矩阵，为ti时刻导航系统的位置，为ti时刻机体系的姿态矩阵，为ti时刻的速度，为ti时刻所在位置的重力加速度，为ti时刻运动载体的角速度。

预积分过程结束，在下一次预积分开始时，将参考坐标系选为此时的机体坐标系，并进行下列初始化：令进入下一次预积分过程。

等价惯性解算方法在预积分步长为50步时，误差比传统二子样算法要大，虽然等价惯性解算在约7小时的仿真数据中最终有较大的误差，但是等价算法的误差和传统算法保持在同一个数量级，说明了等价算法是传统算法的有效近似。同时，在仿真时间上对两种方法进行比较，传统解算方法用时253.780516s，等价解算方法用时185.32451s，因此可以看出等价解算方法在计算效率上更胜一筹。在实际应用过程中，应该权衡计算精度与计算时间之间的关系，在精度要求较高，数据量很大，严重影响计算的实时性的情况下可以采用等价惯性解算的方法进行计算。

在三维仿真中，比较几种不同步长下的预积分算法时间，如下表所示：

从表中可以看出，在基于预积分的惯性解算算法中，随积分步长的增加，运算时间逐渐缩短。预积分的时间越长惯性解算算法的精度越低，但是算法的实时性也会越好。因此在实际应用中在保证预积分时间较小的前提下，合理设置预积分的步长避免解算误差过大。

上面所述的实施例仅仅是对本发明的优选实施方式之一进行描述，并非对本发明的构思和范围进行限定。在不脱离本发明构思的前提下，对本发明的技术方案做出的各种变形和改进，均应落入到本发明的保护范围，本发明请求保护的技术内容，已经全部记录在权利要求书中。

技术特征：

1.一种惯性导航系统的预积分算法，其特征在于：包括以下步骤：

步骤1：初始化定义；

步骤2：计算预积分结果；

步骤3：计算等效旋转矢量；

步骤4：计算连续两个时刻之间机体系的旋转关系；

步骤5：惯性状态解算。

2.根据权利要求1所述的一种惯性导航系统的预积分算法，其特征在于：步骤1所述的初始化定义，具体为初始化运动载体的位置信息速度信息和姿态信息其中：式中δpx、δpy、δpz是x、y、z三个方向的位置变化量，δvx、δvy、δvz是x、y、z三个方向的速度变化量，是ti时刻到ti 1时刻的姿态变化量，ti是载体运动初始时刻，i是单位矩阵。

3.根据权利要求1所述的一种惯性导航系统的预积分算法，其特征在于：步骤2所述的计算预积分结果，具体为：当有新的量测数据在tj 1时刻加入系统中时，计算更新位置变化量更新速度变化量更新姿态变化量计算预积分δxi→j 1，其中tj 1＝tj δt，tj是运动载体预积分计算时刻，δt是新的量测数据到来的时间间隔。

4.根据权利要求3所述的计算一种惯性导航系统的预积分算法，其特征在于：所述的计算更新位置变化量计算公式为：

计算更新速度变化量计算公式为：

式中fj是新增惯性量测；

计算更新姿态变化量计算公式为：式中为tj时刻到tj 1时刻的姿态信息；

计算预积分δxi→j 1，计算公式为：

5.根据权利要求1所述的一种惯性导航系统的预积分算法，其特征在于：步骤3所述的计算等效旋转矢量具体为：在速度更新周期t＝tj 1-tj较短的情况下，等效旋转矢量φ计算如下：

其中，式中，是导航系相对于机体系的角速度，是惯性系相对于机体系的角速度，是ti时刻运动载体的姿态矩阵，是导航系相对于惯性系的角速度。

6.根据权利要求1所述的一种惯性导航系统的预积分算法，其特征在于：步骤4所述的计算连续两个时刻之间机体系的旋转关系，计算公式如下：

7.根据权利要求1所述的一种惯性导航系统的预积分算法，其特征在于：步骤5所述的惯性状态解算，包括导航系统的位置速度和姿态的解算，计算公式分别为：

为ti时刻到tj时刻的姿态旋转矩阵，为ti时刻导航系统的位置，为ti时刻机体系的姿态矩阵，为ti时刻的速度，为ti时刻所在位置的重力加速度，为ti时刻运动载体的角速度；

本次预积分过程结束后，在下一次预积分开始时，将参考坐标系选为此时的机体坐标系，并进行下列初始化：令进入下一次预积分过程。

8.根据权利要求7所述的一种惯性导航系统的预积分算法，其特征在于：ti时刻的速度计算公式为：

ti时刻机体系的姿态矩阵计算公式为：

技术总结
本发明公开了一种惯性导航系统的预积分算法，包括以下步骤：步骤1：初始化定义；步骤2：计算预积分结果；步骤3：计算等效旋转矢量；步骤4：计算连续两个时刻之间机体系的旋转关系；步骤5：惯性状态解算，该惯性导航系统的预积分算法，在水下组合导航方法中，可实现在不丢失惯性数据的前提下降低惯性解算频率，降低运算时间，提高算法的实时性。

技术研发人员：张一博;谢阳光;王国庆
受保护的技术使用者：中国航空工业集团公司西安飞行自动控制研究所
技术研发日：2019.11.21
技术公布日：2020.06.09