CN111307114A - 基于运动参考单元的水面舰船水平姿态测量方法 - Google Patents

Abstract

本发明公开了基于运动参考单元的水面舰船水平姿态测量方法，属于船舶姿态测量技术领域。具体步骤包括：给定初始导航参数，捷联惯导***初始对准得到初始四元数；***采样周期设置并实时采集三个轴上陀螺和加速度计的输出信号；利用陀螺输出信息得到计算四元数，进而得到载体坐标系与地理坐标系的关系矩阵，判断运载体运动状态；然后，利用和载体坐标系与地理坐标系之间的转移矩阵，得到失准角；获得失准角计算值后，对计算四元数进行更新和修正。即使***有运动加速度时，依然保持失准角的最优计算，保证***在不同运动状态下均具有较高的姿态测量精度，有效的提高了***姿态测量精度。

Description

基于运动参考单元的水面舰船水平姿态测量方法

技术领域

本发明属于船舶姿态测量技术领域，具体涉及基于运动参考单元的水面舰船水平姿态测量方法。

背景技术

本发明是基于运动参考单元的水面舰船水平姿态测量方法，利用加速度计和陀螺仪输出进行运载体运动状态判断，根据不同的运动状态采用不同的方法计算失准角。然后利用失准角实时修正计算四元数得到准确的水平姿态。在载体处于低机动状态时，加速度计可以准确的测量重力加速度，可以直接利用加速度计和陀螺输出信息计算失准角获得较高的姿态测量精度。当载体存在运动加速度时，加速度计输出包含两部分：重力加速度和载体运动加速度，且不能将两种加速度直接分离开来，导致无法以重力加速度为参考进行姿态解算。即可以认为运动加速度为干扰加速度，使得***姿态误差增大。因此，有必要对如何有效的避免运动加速度的干扰，使得载体有运动加速度时，依然具有较高的姿态测量精度进行研究。

发明内容

本发明的目的在于提供提高***姿态测量精度的基于运动参考单元的水面舰船水平姿态测量方法。

本发明的目的通过以下技术方案来实现：

基于运动参考单元的水面舰船水平姿态测量方法，包括以下步骤：

步骤1、给定初始导航参数(t＝0时刻)：运动参考单元初始对准得到初始四元数Q(0)；

步骤2、***设置采样周期h，实时采集三个轴上陀螺和加速度计的输出信号

与f^b＝[f_x ^b f_y ^b f_z ^b]^T；

姿态解算周期H，姿态解算周期循环标志k＝1,2…，有H＝t_k-t_k-1。姿态解算周期等于***采样周期，即H＝h。

步骤3、利用陀螺输出信息得到计算四元数

进而得到载体坐标系b与地理坐标系n的关系矩阵

其中

步骤4、判断运载体运动状态规则如下：

若

且

则判断运载体处于低机动状态；

若

或

则运载体处于高机动状态。

其中运载体合角速度为

合加速度为

n取

步骤5、当运载体处于低机动状态时，利用加速度计输出信息f^b(k-1)和载体坐标系b与地理坐标系n之间的转移矩阵，得到水平失准角φ^b(k-1)。

其中“×”代表矢量间叉乘运算，C₃(k-1)代表

的第三行行向量。

步骤6、当运载体处于高机动状态时，设计合理的高通滤波器将载体机动加速度从加速度计输出信息中提取出来，再加以补偿。即：

其中高通滤波器H(z)其传递函数为：

其中τ为时间常数参数，τ取0.03s。

然后，利用

和载体坐标系b与地理坐标系n之间的转移矩阵，得到失准角φ^b(k-1)。

步骤7、获得失准角计算值φ^b(k-1)后，对计算四元数

进行更新和修正如下：

其中

α是误差角修正系数，取α＝0.01；Δθ(k-1)是经过失准角修正后的角增量且有Δθ(k-1)＝|Δθ(k-1)|。

至此就完成了基于运动参考单元的水面舰船水平姿态更新和修正。

本发明的有益效果在于：

本发明综合利用运动参考单元输出的加速度和角速度信息，进行载体运动状态检测，根据检测结果采用不同的失准角计算方法。即使***有运动加速度时，依然保持失准角的最优计算，保证***在不同运动状态下均具有较高的姿态测量精度，有效的提高了***姿态测量精度。

附图说明

图1为本发明用于捷联惯导***的带杆臂补偿的升沉测量方法流程图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步说明：

下面结合附图1举例对本发明做更详细地描述：捷联惯导***的带杆臂补偿的升沉测量方法，包括如下步骤：

步骤1、给定初始导航参数(t＝0时刻)：运动参考单元初始对准得到初始四元数Q(0)；

步骤2、***设置采样周期h，实时采集三个轴上陀螺和加速度计的输出信号

与f^b＝[f_x ^b f_y ^b f_z ^b]^T；姿态解算周期H，姿态解算周期循环标志k＝1,2…，有H＝t_k-t_k-1。姿态解算周期等于***采样周期，即H＝h。

步骤3、利用陀螺输出信息得到计算四元数

进而得到载体坐标系b与地理坐标系n的关系矩阵

其中

步骤4、判断运载体运动状态规则如下：

若

且

则判断运载体处于低机动状态；

若

或

则运载体处于高机动状态。

其中运载体合角速度为

合加速度为

n取

步骤5、当运载体处于低机动状态时，利用加速度计输出信息f^b(k-1)和载体坐标系b与地理坐标系_n之间的转移矩阵，得到水平失准角φ^b(k-1)。

其中“×”代表矢量间叉乘运算，C₃(k-1)代表

的第三行行向量。

步骤6、当运载体处于高机动状态时，设计合理的高通滤波器将载体机动加速度从加速度计输出信息中提取出来，再加以补偿。即：

其中高通滤波器H(z)其传递函数为：

其中τ为时间常数参数，τ取0.03s。

然后，利用

和载体坐标系b与地理坐标系n之间的转移矩阵，得到失准角φ^b(k-1)。

步骤7、获得失准角计算值φ^b(k-1)后，对计算四元数

进行更新和修正如下：

其中

α是误差角修正系数，取α＝0.01；Δθ(k-1)是经过失准角修正后的角增量且有Δθ(k-1)＝|Δθ(k-1)|。

至此就完成了基于运动参考单元的水面舰船水平姿态更新和修正。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.基于运动参考单元的水面舰船水平姿态测量方法，其特征在于，包括以下步骤：

步骤1、给定初始导航参数(t＝0时刻)：运动参考单元初始对准得到初始四元数Q(0)；

步骤2、***设置采样周期h，实时采集三个轴上陀螺和加速度计的输出信号

与f^b＝[f_x ^b f_y ^b f_z ^b]^T，姿态解算周期H，姿态解算周期循环标志k＝1,2…，有H＝t_k-t_k-1；

步骤3、利用陀螺输出信息得到计算四元数

进而得到载体坐标系b与地理坐标系n的关系矩阵

步骤4、判断运载体运动状态规则如下：

若

且

则判断运载体处于低机动状态；

若

或

则运载体处于高机动状态；

步骤5、当运载体处于低机动状态时，利用加速度计输出信息f^b(k-1)和载体坐标系b与地理坐标系n之间的转移矩阵，得到水平失准角φ^b(k-1)；

步骤6、当运载体处于高机动状态时，设计合理的高通滤波器将载体机动加速度从加速度计输出信息中提取出来，再加以补偿，即

然后，利用

和载体坐标系b与地理坐标系n之间的转移矩阵，得到失准角φ^b(k-1)；

步骤7、获得失准角计算值φ^b(k-1)后，对计算四元数

进行更新和修正如下：

2.根据权利要求1所述的基于运动参考单元的水面舰船水平姿态测量方法，其特征在于，步骤2中姿态解算周期等于***采样周期，即H＝h。

3.根据权利要求1所述的基于运动参考单元的水面舰船水平姿态测量方法，其特征在于，步骤3所述的四元数

具体为：

4.根据权利要求1所述的基于运动参考单元的水面舰船水平姿态测量方法，其特征在于，步骤4中运载体合角速度为

合加速度为

n取

5.根据权利要求1所述的基于运动参考单元的水面舰船水平姿态测量方法，其特征在于，步骤5所述的水平失准角φ^b(k-1)具体为：

其中“×”代表矢量间叉乘运算，C₃(k-1)代表

的第三行行向量。

6.根据权利要求1所述的基于运动参考单元的水面舰船水平姿态测量方法，其特征在于，步骤6中高通滤波器H(z)其传递函数为：

其中τ为时间常数参数，τ取0.03s；

失准角φ^b(k-1)具体为：

7.根据权利要求1所述的基于运动参考单元的水面舰船水平姿态测量方法，其特征在于，步骤7中

α是误差角修正系数，取α＝0.01；Δθ(k-1)是经过失准角修正后的角增量且有Δθ(k-1)＝|Δθ(k-1)|。

Images