CN109425339A - 一种基于惯性技术的考虑杆臂效应的舰船升沉误差补偿方法 - Google Patents

Abstract

本发明公开一种基于惯性技术的考虑杆臂效应的舰船升沉误差补偿方法，属于船舶导航技术领域。本发明首先建立舰船研究对象建立载体与地理坐标坐标系，采集传感器数据；然后计算舰船的横摇γ纵摇θ和解算加速度；再通过积分运算、数字滤波和再次积分得到补偿前的升沉H；其次计算误差升沉；最后用未补偿的升沉Hins减去误差升沉Hi得到补偿后的升沉Hout。在惯性导航系统的基础上，建立升沉解算误差补偿模型，不仅仅利用捷联惯导姿态角输出信息，而且利用了惯性测量元件安装的杆臂信息。该方法大大降低了惯性测量元件安装的局限性，从而提高舰船升沉精度，进而提高惯性导航系统的导航精度。

Description

一种基于惯性技术的考虑杆臂效应的舰船升沉误差补偿方法

技术领域

本发明涉及一种基于惯性技术的考虑杆臂效应的舰船升沉误差补偿方法，属于船舶导航技术领域。

背景技术

随着惯性导航系统(Inertial Navigation System，INS)和惯性测量技术的迅速发展，惯性导航系统在舰船导航中的应用也逐渐深入，且对其导航的精确性和功能性要求也不断提升。惯导系统所能提供的速度、位置、姿态信息已经不能完全满足使用者的需求，特别是当海况恶劣且舰船的航行状态不稳定时，舰船瞬时运动信息的准确提取对舰载武器的对准、舰载飞行器升降控制有十分重要的意义。

舰船在海上航行过程中，会受到海浪和海风等复杂海洋环境因素的激励，会产生6自由度的摇荡运动，其中包括6个角运动即纵摇、横摇和艏摇，以及3个线运动即横荡、纵荡和垂荡(即升沉)。目前舰升沉通常由舰船上的惯性测量组件实时监测获得。惯性测量组件核心部件由一个体积很小的捷联惯性测量单元构成，包括3个加速度计、3个光纤陀螺仪和一个实时处理计算单元。3个加速度计主要用于监测运动物体在3个坐标轴上的轴向线性加速度，舰船升沉解算通常用高通数字滤波器滤除导航系统中舒勒振荡和加速度计偏值测量误差的影响，再经过线性加速度二次积分得到垂向位置偏移，进而获得准确的升沉运动信息。而大多数升沉模型通常把船舶的质心与惯性器件看成一体，把惯性测量组件解算出的升沉当作是船舶质心的升沉。而实际应用中，质心通常无法与取得一致，两者间的存在杆臂。传统意义上杆臂效应是指由于惯性组件安装位置与载体摇摆中心不重合带来的离心加速度和切向加速度所引起的加速度计测量误差。而升沉解算需要的加速度信息来自加速度计，这间接地导致惯性测量算出的升沉与船舶质心处的升沉不同。若要解算船舶质心处升沉，则必须惯性测量组件解算出的升沉进行补偿归算。

发明内容

本发明的目的是为了解决上述问题，提供一种基于惯性技术的考虑杆臂效应的舰船升沉误差补偿方法。

本发明的目的是通过以下步骤实现的：

一种基于惯性技术的考虑杆臂效应的舰船升沉误差补偿方法，其特征在于，包含以下步骤：

步骤一以船舶质心为原点，建立舰船载体坐标系b系和当地地理坐标系t系；载体坐标系中，x轴指向船首，y轴指向右弦，z轴垂直于x、y轴构成平面向天；惯性导航初始对准后(粗对准与精对准)，利用四元数法计算并实时更新三个姿态角即横摇γ纵摇θ航向和捷联矩阵其中是舰船载体坐标系b系与地理坐标系t系的关系矩阵；

步骤二采集加速度计原始数据，解算加速度计敏感的载体坐标系下的三个方向的加速度 f^b，由步骤一的捷联矩阵和载体坐标系加速度f^b计算地理坐标系下的加速度信息f^t；变换公式如下：

步骤三对步骤二中的加速度f^t信息一次积分，得到速度v_in，其中积分时间为惯导解算采样时间0.01s；公式为：

其中t-t0＝0.01s，v₀为上一时刻的速度值；

设计并运用巴特沃斯高通数字滤波器，滤除舒勒周期对升沉运动的干扰，提取线运动升沉信息；其中巴特沃斯高通数字滤波器为转移函数差分方程为：

其中Fb[0]、Fb[1]、Fb[2]、Fb[3]；Fa[0]、Fa[1]、Fa[2]、Fa[3]为滤波器系数；滤波输出得到速度信息v_filter，对v_filter再次积分得到补偿前的升沉H_ins；公式如下：

其中t-t0＝0.01s，H₀为上一时刻补偿前的升沉；

步骤四测量惯性测量组件安装位置相对于船舶质心位置的杆臂长度L1、L2、L3；其中 L1是x轴的分量，L2是y轴分量，L3是z轴分量；其补偿部分为由杆臂引起的误差升沉；该部分为升沉的主要误差，其数学模型为：

H_i＝Z-L₃＝L₁ sinθ-L₂ sinγcosθ+L₃(cosγcosθ-1) (4)

式中，Z为地理坐标系下的惯性测量组件安装处与船舶质心处(坐标系原点)的动态垂向位移差；其公式为：

Z＝L₁ sinθ-L₂ sinγcosθ+L₃ cosγcosθ (5)

式中L3为载体坐标系下z轴的杆臂；

步骤五由补偿前的升沉Hins和误差升沉Hi得到补偿后的升沉Hout；公式为：

H_out＝H_ins-H_i (6)

舰船的升沉补偿完成。

本发明的优点在于：

(1)减小舰船的升沉误差；

(2)惯性测量组件无需安装在船舶质心处，只需要提供惯性测量原件距质心的杆臂与所处位置的横纵摇信息，便可以求出船舶整体的升沉，降低导航设备安装局限性。

附图说明

图1是本发明的步骤流程图；

图2是本发明的原理图；

图3是本发明利用Matlab仿真输入的升沉真实值；

图4是本发明利用Matlab仿真得到的升沉补偿前后对比图(第一组仿真实验)；

图5是本发明利用Matlab仿真得到的升沉补偿前后对比图(第二组仿真实验)；

图6是本发明利用Matlab仿真得到的升沉补偿前后对比图(第三组仿真实验)。

具体实施方式

下面结合附图对本发明做进一步描述。

步骤一：以船舶质心为原点，建立舰船载体坐标系b系和当地地理坐标系t系。载体坐标系中，x轴指向船首，y轴指向右弦，z轴垂直于x、y轴构成平面向天。惯性导航初始对准后(粗对准与精对准)，利用四元数法计算并实时更新三个姿态角即横摇γ纵摇θ航向和捷联矩阵其中是舰船载体坐标系b系与地理坐标系t系的关系矩阵。

步骤二：采集加速度计原始数据，解算加速度计敏感的载体坐标系下的三个方向的加速度f^b，由步骤一的捷联矩阵和载体坐标系加速度f^b计算地理坐标系下的加速度信息f^t。变换公式如下

步骤三：对步骤二中的加速度f^t信息一次积分，得到速度v_in，其中积分时间为惯导解算采样时间0.01s。公式为

其中t-t0＝0.01s，v₀为上一时刻的速度值。

设计并运用巴特沃斯高通数字滤波器，滤除舒勒周期对升沉运动的干扰，提取线运动升沉信息。其中巴特沃斯高通数字滤波器为转移函数差分方程为：

其中Fb[0]、Fb[1]、Fb[2]、Fb[3]；Fa[0]、Fa[1]、Fa[2]、Fa[3]为滤波器系数。滤波输出得到速度信息v_filter，对v_filter再次积分得到补偿前的升沉H_ins。公式如下

其中t-t0＝0.01s，H₀为上一时刻补偿前的升沉。

步骤四：测量惯性测量组件安装位置相对于船舶质心位置的杆臂长度L1、L2、L3。其中 L1是x轴的分量，L2是y轴分量，L3是z轴分量。其补偿部分为由杆臂引起的误差升沉。该部分为升沉的主要误差，其数学模型为：

H_i＝Z-L₃＝L₁ sinθ-L₂ sinγcosθ+L₃ (cosγcosθ-1) (4)

式中，Z为地理坐标系下的惯性测量组件安装处与船舶质心处(坐标系原点)的动态垂向位移差。其公式为

Z＝L₁ sinθ-L₂ sinγcosθ+L₃ cosγcosθ (5)

式中L3为载体坐标系下z轴的杆臂。

步骤五：由补偿前的升沉Hins和误差升沉Hi得到补偿后的升沉Hout。公式为

H_out＝H_ins-H_i (6)

舰船的升沉补偿完成。

对本发明的有益效果说明如下：

MATLAB仿真

(1)在以下的仿真条件下，对该方法进行仿真实验：

捷联导航系统基本参数：

载体初始位置：北纬45.796度，东经126.6705度；

载体姿态角：

γ＝10度*sin(2*pi*k*hn/T_p)，其中T_p为纵摇周期10秒，k等于1800000。

θ＝5度*sin(2*pi*k*hn/T_r)，其中T_r为横摇周期10秒，k等于1800000。

φ＝0度；其中γ，θ，φ分别表示舰船的横摇角纵摇角和航向角；

载体姿态误差角：γ＝0度，θ＝0度，φ＝0度；其中γ，θ，φ分别表示舰船的横摇角纵摇角和航向角；

赤道半径：Re＝6378393米

地球自转角速度(弧度/秒)：

地球表面重力加速度：g0＝9.78049；

陀螺仪常值漂移：0度/小时；

加速度计零偏：0米/秒平方

采样频率hn＝0.01s

仿真时间t＝5小时。

升沉解算参数：

三阶巴特沃斯差分方程系数如下：

Fb[0]＝0.999476538272341；Fb[1]＝-2.998429614817024；

Fb[2]＝2.998429614817024；Fb[3]＝-0.999476538272341；

Fa[0]＝1.000000000000000；Fa[1]＝-2.998952802460766；

Fa[2]＝2.997906153161101；Fa[3]＝-0.998953350556863；

升沉真实值为1*sin(2*pi*k*hn/T_heave)米，其中T_heave为10秒，k等于1800000

载体坐标系下的惯性测量元件杆臂长度L1＝2.1米、L2＝0.6米、L3＝3.1米。

上述发明方法未补偿的升沉值Hins幅值为1.26米、补偿后升沉值Hout幅值为0.961米。补偿后的升沉误差为3.9厘米，而惯性测量组件升沉精度为5厘米，补偿效果良好。

(2)改变载体姿态角，对比仿真1，考察对不同姿态角的升沉补偿效果。在以下的仿真条件下，对该方法进行仿真实验：

捷联导航系统基本参数：

载体初始位置：北纬45.796度，东经126.6705度；

载体姿态角：

γ＝15度*sin(2*pi*k*hn/T_p)，其中T_p为纵摇周期10秒，k等于1800000。

θ＝10度*sin(2*pi*k*hn/T_r)，其中T_r为横摇周期10秒，k等于1800000。

φ＝0度；其中γ，θ，φ分别表示舰船的横摇角纵摇角和航向角；

载体姿态误差角：γ＝0度，θ＝0度，φ＝0度；其中γ，θ，φ分别表示舰船的横摇角纵摇角和航向角；赤道半径：Re＝6378393米；

地球自转角速度(弧度/秒)；

地球表面重力加速度：g0＝9.78049；

陀螺仪常值漂移：0度/小时；

加速度计零偏：0米/秒平方；

采样频率hn＝0.01s；

仿真时间t＝5小时；

上述发明方法未补偿升沉值Hins幅值为1.67米、补偿后升沉值Hout幅值为0.968米。补偿后的误差3.2厘米，而惯性测量组件升沉精度为5厘米，补偿效果良好。

升沉解算参数：

三阶巴特沃斯差分方程系数如下：

Fb[0]＝0.999476538272341；Fb[1]＝-2.998429614817024；

Fb[2]＝2.998429614817024；Fb[3]＝-0.999476538272341；

Fa[0]＝1.000000000000000；Fa[1]＝-2.998952802460766；

Fa[2]＝2.997906153161101；Fa[3]＝-0.998953350556863；

升沉真实值为1*sin(2*pi*k*hn/T_heave)米，其中T_heave为升沉周期10秒，k等于1800000

载体坐标系下的惯性测量元件杆臂长度L1＝2.1米、L2＝0.6米、L3＝3.1米。

发明所述方法得到补偿前升沉Hins幅值为米、补偿后升沉值Hout为0.977米。补偿后的升沉误差2.3厘米，而惯性测量组件升沉精度为5厘米，补偿效果良好。

(3)改变杆臂参数，对比仿真1，考察对不同杆臂对升沉补偿效果的影响。在以下的仿真条件下，对该方法进行仿真实验：

捷联导航系统基本参数：

载体初始位置：北纬45.796度，东经126.6705度；

载体姿态角：

γ＝10度*sin(2*pi*k*hn/T_p)，其中T_p为10秒，k等于1800000。

θ＝5度*sin(2*pi*k*hn/T_r)，其中T_r为10秒，k等于1800000。

φ＝0度；其中γ，θ，φ分别表示舰船的横摇角纵摇角和航向角；

载体姿态误差角：γ＝0度，θ＝0度，φ＝0度；其中γ，θ，φ分别表示舰船的横摇角纵摇角和航向角；

赤道半径：Re＝6378393米

地球自转角速度(弧度/秒)：

地球表面重力加速度：g0＝9.78049；

陀螺仪常值漂移：0度/小时；

加速度计零偏：0米/秒平方

采样频率hn＝0.01s

仿真时间t＝5小时。

升沉解算参数：

三阶巴特沃斯差分方程系数如下：

Fb[0]＝0.999476538272341；Fb[1]＝-2.998429614817024；

Fb[2]＝2.998429614817024；Fb[3]＝-0.999476538272341；

Fa[0]＝1.000000000000000；Fa[1]＝-2.998952802460766；

Fa[2]＝2.997906153161101；Fa[3]＝-0.998953350556863；

升沉真实值幅值为1*sin(2*pi*k*hn/T_heave)米，其中T_heave为升沉周期10秒，k 等于1800000，

载体坐标系下的杆臂长度L1＝1.1米、L2＝0.3米、L3＝1.5米。

发明所述方法未补偿的升沉值Hins幅值为1.09米、补偿后升沉值Hout幅值为0.98米。补偿后的误差2.0厘米，而惯性测量组件升沉精度为5厘米，补偿效果良好。

Claims (1)

1.一种基于惯性技术的考虑杆臂效应的舰船升沉误差补偿方法，其特征在于，包含以下步骤：

步骤一 以船舶质心为原点，建立舰船载体坐标系b系和当地地理坐标系t系；载体坐标系中，x轴指向船首，y轴指向右弦，z轴垂直于x、y轴构成平面向天；惯性导航初始对准后(粗对准与精对准)，利用四元数法计算并实时更新三个姿态角即横摇γ纵摇θ航向和捷联矩阵其中是舰船载体坐标系b系与地理坐标系t系的关系矩阵；

步骤二 采集加速度计原始数据，解算加速度计敏感的载体坐标系下的三个方向的加速度f^b，由步骤一的捷联矩阵和载体坐标系加速度f^b计算地理坐标系下的加速度信息f^t；变换公式如下：

步骤三 对步骤二中的加速度f^t信息一次积分，得到速度v_in，其中积分时间为惯导解算采样时间0.01s；公式为：

其中t-t0＝0.01s，v₀为上一时刻的速度值；

设计并运用巴特沃斯高通数字滤波器，滤除舒勒周期对升沉运动的干扰，提取线运动升沉信息；其中巴特沃斯高通数字滤波器为转移函数差分方程为：

其中Fb[0]、Fb[1]、Fb[2]、Fb[3]；Fa[0]、Fa[1]、Fa[2]、Fa[3]为滤波器系数；滤波输出得到速度信息v_filter，对v_filter再次积分得到补偿前的升沉H_ins；公式如下：

其中t-t0＝0.01s，H₀为上一时刻补偿前的升沉；

步骤四测量惯性测量组件安装位置相对于船舶质心位置的杆臂长度L1、L2、L3；其中L1是x轴的分量，L2是y轴分量，L3是z轴分量；其补偿部分为由杆臂引起的误差升沉；该部分为升沉的主要误差，其数学模型为：

H_i＝Z-L₃＝L₁sinθ-L₂sinγcosθ+L₃(cosγcosθ-1) (4)

式中，Z为地理坐标系下的惯性测量组件安装处与船舶质心处(坐标系原点)的动态垂向位移差；其公式为：

Z＝L₁sinθ-L₂sinγcosθ+L₃cosγcosθ (5)

式中L3为载体坐标系下z轴的杆臂；

步骤五由补偿前的升沉Hins和误差升沉Hi得到补偿后的升沉Hout；公式为：

H_out＝H_ins-H_i (6)

舰船的升沉补偿完成。