CN113447018A - 一种水下惯性导航***的姿态实时估计方法 - Google Patents

Abstract

本发明涉及水下运动载体姿态实时测量领域，且公开了一种水下惯性导航***的姿态实时估计方法，其包括对IMU中的陀螺传感器的信息进行处理，得到导航坐标系下的角速度，其步骤为：1.1、将三轴陀螺的原始测量值，扣除零偏后得到体坐标系下的三轴角速度值，三轴陀螺的原始测量值记为G_measure，零偏记为g_bias，体坐标系下的角速度值记为G_b，则有：G_b＝G_measure–g_bias，G_b、G_measure、g_bias都是三维列矢量，三个分量分别对应X轴、Y轴、Z轴。本发明相比与卡尔曼滤波器，能够明显减少运算量，并且引入经典的闭环负反馈环节，与基于权重的校正方法相比，计算结果精度更高，收敛性更好。

Description

一种水下惯性导航***的姿态实时估计方法

技术领域

本发明属于水下运动载体姿态实时测量领域，具体为一种水下惯性导航***的姿态实时估计方法。

背景技术

水下运动载体需要提供高精度而且实时的姿态估计。高精度测量是高精度控制的前提，实时性则是对应了控制周期的需要。

差分GPS接收机虽然可以提供误差不随时间发散的姿态信息，但水下大部分时间无法接收到GPS信号，无法实现实时性测量。

要提供实时的姿态信息，传统的方法就是基于惯性测量单元的捷联惯性导航算法。由于陀螺解算具有发散特性，为了抑制误差的增长，通常利用GPS对惯性导航的误差进行校正，其特点是误差不随时间发散。

利用外部不随时间发散的绝对测量信息抑制惯性导航***误差的方法主要是采用卡尔曼滤波器法与基于权重的校正方法。

卡尔曼滤波器通过建立状态方程与观测方程对状态方程中的状态量进行估计，得到滤波后的位置、速度、姿态。然而卡尔曼滤波器的运算量偏大，运算量与滤波器的维数平方成正比，而且非白噪声条件下的应用以及参数设置的不合理往往会导致滤波器不稳定，因此设计一种水下惯性导航***的姿态实时估计方法来解决这种问题很有必要。

发明内容

本发明的目的在于：为解决上述背景技术中提出的问题，本发明提供了一种水下惯性导航***的姿态实时估计方法。

为实现上述目的，本发明提供如下技术方案：一种水下惯性导航***的姿态实时估计方法，包括：

(一)对IMU中的陀螺传感器的信息进行处理，得到导航坐标系下的角速度，其步骤为：

1.1、将三轴陀螺的原始测量值，扣除零偏后得到体坐标系下的三轴角速度值，三轴陀螺的原始测量值记为G_measure，零偏记为g_bias，体坐标系下的角速度值记为G_b，则有：G_b＝G_measure–g_bias，G_b、G_measure、g_bias都是三维列矢量，三个分量分别对应X轴、Y轴、Z轴；

1.2、通过姿态转换矩阵得到导航坐标系下的角速度值，采用传统成熟方法利用陀螺信息得到姿态转换矩阵

导航坐标系下的角速度记为G_r，则有：

是三维列矢量，且三个分量分别对应X轴、Y轴、Z轴；

(二)基于陀螺角输入速度的进行姿态预测，其公式如下：ZT(n)＝ZT(n-1)+ω(n-1)×T：并使用数字积分器实现对P2支路以及和支路的积分，其步骤为：

2.1、计算预测的姿态：

是当前时刻姿态预测输出，等效于前面公式的ZT(n)，经过上式积分前的

等效于上述公式的ZT(n-1)；

2.2、计算当前时刻姿态误差err_ZT(n)：

ZT_ref(n)表示当前时刻的外部姿态参考输入，

表示当前时刻的姿态预测，来自上面公式中积分后的

2.3、根据需要的带宽BD确定支路的系数P1，P2，其中P1＝1.47BD，P2＝1.787BD²；

2.4、根据姿态误差计算P1支路输出path1，path1＝err_ZT(n)×P1；

2.5、根据姿态误差计算P2支路输出path21，path21＝path21+err_ZT(n)×P2×T；

2.6、通过测量得到的导航坐标系的角速度支路：

角速度X轴：

角速度Y轴：

角速度Z轴：

2.7、计算姿态输出：

与卡尔曼滤波器相比，本发明能够明显减少运算量，并且引入经典的闭环负反馈环节，与基于权重的校正方法相比，计算结果精度更高，收敛性更好，且特点为运行较为平缓，俯仰轴、横滚轴无较大姿态角度变换，易实现，易推广，能够快速准确的对弹体姿态信息进行实时估计。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：

图1为本发明的原理步骤示意图；

图2为本发明的姿态预测模型示意图；

图3为本发明运用的数字积分器原理示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例；基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-3，本具体实施方式提供的一种水下惯性导航***的姿态实时估计方法，以导航坐标系为例进行说明，一共需要三个方位支路，分别实现对俯仰(X轴)、偏航(Y轴)、横滚(Z轴)的参考姿态进行跟踪，然后分别得到俯仰、偏航、横滚的本地姿态估计，以任意一个轴向的姿态估计为例进行说明，其步骤为包括：(一)对IMU中的陀螺传感器的信息进行处理，得到导航坐标系下的角速度，其步骤为：1.1、将三轴陀螺的原始测量值，扣除零偏后得到体坐标系下的三轴角速度值，三轴陀螺的原始测量值记为G_measure，零偏记为g_bias，体坐标系下的角速度值记为G_b，则有：G_b＝G_measure–g_bias，G_b、G_measure、g_bias都是三维列矢量，三个分量分别对应X轴、Y轴、Z轴；1.2、通过姿态转换矩阵得到导航坐标系下的角速度值，采用传统成熟方法利用陀螺信息得到姿态转换矩阵

导航坐标系下的角速度记为G_r，则有：

是三维列矢量，且三个分量分别对应X轴、Y轴、Z轴；(二)基于陀螺角输入速度的进行姿态预测，其公式如下：ZT(n)＝ZT(n-1)+ω(n-1)×T：并使用数字积分器实现对P2支路以及和支路的积分，其步骤为：2.1、计算预测的姿态：

是当前时刻姿态预测输出，等效于前面公式的ZT(n)，经过上式积分前的

等效于上述公式的ZT(n-1)；2.2、计算当前时刻姿态误差err_ZT(n)：

ZT_ref(n)表示当前时刻的外部姿态参考输入，

表示当前时刻的姿态预测，来自上面公式中积分后的

2.3、根据需要的带宽BD确定支路的系数P1，P2，其中P1＝1.47BD，P2＝1.787BD²；2.4、根据姿态误差计算P1支路输出path1，path1＝err_ZT(n)×P1；2.5、根据姿态误差计算P2支路输出path21，path21＝path21+err_ZT(n)×P2×T；2.6、通过测量得到的导航坐标系的角速度支路：

角速度X轴：

角速度Y轴：

角速度Z轴：

2.7、计算姿态输出：

与卡尔曼滤波器相比，本发明能够明显减少运算量，并且引入经典的闭环负反馈环节，与基于权重的校正方法相比，计算结果精度更高，收敛性更好，且特点为运行较为平缓，俯仰轴、横滚轴无较大姿态角度变换，易实现，易推广，能够快速准确的对弹体姿态信息进行实时估计。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (1)

1.一种水下惯性导航***的姿态实时估计方法，其特征在于，所述方法包括：

(一)对IMU中的陀螺传感器的信息进行处理，得到导航坐标系下的角速度，其步骤为：

1.1、将三轴陀螺的原始测量值，扣除零偏后得到体坐标系下的三轴角速度值，三轴陀螺的原始测量值记为G_measure，零偏记为g_bias，体坐标系下的角速度值记为G_b，则有：G_b＝G_measure-g_bias，G_b、G_measure、g_bias都是三维列矢量，三个分量分别对应X轴、Y轴、Z轴；

1.2、通过姿态转换矩阵得到导航坐标系下的角速度值，采用传统成熟方法利用陀螺信息得到姿态转换矩阵

导航坐标系下的角速度记为G_r，则有：

是三维列矢量，且三个分量分别对应X轴、Y轴、Z轴；

(二)基于陀螺角输入速度的进行姿态预测，其公式如下：ZT(n)＝ZT(n-1)+ω(n-1)×T：并使用数字积分器实现对P2支路以及和支路的积分，其步骤为：

2.1、计算预测的姿态：

是当前时刻姿态预测输出，等效于前面公式的ZT(n)，经过上式积分前的

等效于上述公式的ZT(n-1)；

2.2、计算当前时刻姿态误差err_ZT(n)：

ZT_ref(n)表示当前时刻的外部姿态参考输入，

表示当前时刻的姿态预测，来自上面公式中积分后的

2.3、根据需要的带宽BD确定支路的系数P1，P2，其中P1＝1.47BD，P2＝1.787BD²；

2.4、根据姿态误差计算P1支路输出path1，path1＝err_ZT(n)×P1；

2.5、根据姿态误差计算P2支路输出path21，path21＝path21+err_ZT(n)×P2×T：

2.6、通过测量得到的导航坐标系的角速度支路：

角速度X轴：

角速度Y轴：

角速度Z轴：

2.7、计算姿态输出：

Images