CN110702110A - 一种基于无迹卡尔曼滤波的舰船升沉运动测量方法 - Google Patents

Abstract

本发明涉及一种基于无迹卡尔曼滤波的舰船升沉运动测量方法，(1)实时采集捷联惯性导航系统的输出数据得到载体坐标系下陀螺仪、加速度；(2)计算实时姿态矩阵，得到地理坐标系下舰船升沉运动加速度信息；(3)通过对加速度数据进行快速傅里叶变换得到升沉加速度的幅度谱和相位谱，进而计算得到升沉位移的幅度谱和相位谱；(4)对舰船升沉位移频域峰值检测来确定舰船升沉运动的主要正弦周期分量组成；(5)根据舰船升沉加速度测量值与舰船升沉运动状态量关系的解析模型，基于UKF进行升沉运动滤波解算；(6)实时输出舰船升沉运动的速度和位移信息。本发明的优点在于：在无外部观测信息输入的情况下实现舰船升沉运动信息的实时精确测量。

Description

一种基于无迹卡尔曼滤波的舰船升沉运动测量方法

技术领域

本发明属于船舶运动测量技术领域，特别涉及一种基于无迹卡尔曼滤波的舰船升沉运动测量方法。

背景技术

许多海上作业，如航行补给、舰载机的起降、海底测绘和海上起重机工作等，都需要对由海浪、海风和洋流等复杂海洋环境因素引起的舰船升沉运动进行补偿，这就需要实时、精确的舰船升沉运动信息。捷联惯性导航技术是一项发展较为成熟、精度较高、稳定性优良的自主导航技术。因此本方法采用捷联惯性导航系统测量舰船的升沉速度和位移信息，由于惯性导航系统的误差累积和高度通道发散特性，不能在长时间内持续获得高精度的速度、位移信息，故需要对惯性导航系统测量数据进行处理。

卡尔曼滤波器广泛应用于最优估计领域中。经典的卡尔曼滤波针对线性高斯系统对目标状态做出最优估计，在一定条件下得到准确地估计状态，EKF针对非线性高斯系统，对其非线性部分通过一阶泰勒展开扩展使其模型线性化，并且仅在特定范围的初始条件下收敛，而无迹卡尔曼滤波(UKF)通过UT(Unscented Transformation)的非线性变换方法直接进行非线性函数的状态及其方差阵传播，避免了非线性函数线性化近似过程中复杂的Jacobian矩阵的求解，适用于强非线性系统，它改善了状态估计的准确性和收敛性。

发明内容

本发明要解决的技术问题是提供一种实时、高精度、较低成本的基于无迹卡尔曼滤波的舰船升沉运动测量方法。

为解决上述技术问题，本发明的技术方案为：一种基于无迹卡尔曼滤波的舰船升沉运动测量方法，其创新点在于：所述舰船升沉运动测量方法包括如下步骤：

步骤1、实时采集安装在舰船内的惯性测量单元的各轴陀螺仪和加速度计的输出数据；

步骤2、计算得到载体坐标系b与地理坐标系n的姿态矩阵，得到地理坐标系下舰船升沉运动加速度；

步骤3、对舰船升沉加速度信息做FFT，得到升沉加速度的幅度谱和相位谱,进而计算得到升沉位移的幅度谱和相位谱；

步骤4、在升沉位移频域内进行峰值检测，确定升沉运动模型参数及每个周期分量运动模型所对应的频率和相位；其中，舰船升沉位移模型为

式(1)中j＝1,2,...,N_m，z(t)为舰船升沉位移、A_j,ω_j和分别是每一个正弦周期分量的振幅、特征频率和相位；

步骤5、根据舰船升沉运动加速度测量值y_k与舰船升沉运动状态量x_k关系的解析模型，基于无迹卡尔曼滤波进行升沉运动滤波解算；

步骤6、实时输出舰船升沉运动的速度和位移信息z(t)。

进一步地，所述步骤2具体为：根据步骤1得到的陀螺仪角增量输出Δθ_k和加速度计比力输出

舰船升沉加速度计算的迭代形式为：

式中I为三维单位矩阵；φ^b为姿态矩阵水平失准角；(·)×为反对称矩阵；gⁿ为舰船所处位置重力加速度；C₃为姿态矩阵

的第三行向量；

为舰船加速度；

为

的天向分量，即舰船升沉加速度。

进一步地，所述步骤3具体为：每隔时间间隔T对舰船升沉加速度信息做FFT，升沉位移的幅度谱A(ω)和相位谱由加速度的幅度谱和相位谱求得：

进一步地，所述步骤4具体为：每隔时间间隔T在升沉位移频域内进行峰值检测，其中频域峰值检测的频率范围为0.05～1Hz，舰船升沉运动周期分量的增加或减少都需要重新初始化式(1)的舰船升沉运动模型，系统需要根据海况变化进行调整。

进一步地，所述步骤5具体为：舰船升沉运动加速度测量值y_k与舰船升沉运动状态量x_k关系的解析模型为：

式(2)中

bz,k为舰船升沉加速度零偏、为过程噪声，V_k为量测噪声，且式(2)为无迹卡尔曼滤波的状态方程和量测方程，分别对状态预测和量测预测做UT变换，基于无迹卡尔曼滤波进行升沉运动滤波解算。

本发明的优点在于：本发明基于无迹卡尔曼滤波的舰船升沉运动测量方法，在不增加新的传感器，不需要外部信息的情况下，通过实时采集安装在舰船内的惯性测量单元的各轴陀螺仪和加速度计的输出数据，计算得到载体坐标系与地理坐标系的姿态矩阵，得到地理坐标系下舰船升沉运动加速度，通过对舰船升沉加速度信息做FFT和对舰船升沉位移信息做频域峰值检测获得舰船升沉运动的各次谐波信息，得到舰船升沉运动模型，结合惯性导航系统测得的舰船升沉运动加速度和舰船升沉运动状态量的关系，得到舰船升沉运动的观测模型，基于UKF对升沉运动滤波解算获得舰船升沉位移。海浪模拟运动平台实验结果表明，运用本发明方法可以得到高精度无延时的舰船升沉位移。

附图说明

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

图1为本发明实施例提供的舰船升沉测量方法的步骤流程图。

图2为舰船升沉运动滤波标准差图。

图3为舰船升沉位移测量曲线图。

图4为本发明实施例提供的舰船升沉测量系统测量原理流程图。

具体实施方式

下面的实施例可以使本专业的技术人员更全面地理解本发明，但并不因此将本发明限制在所述的实施例范围之中。

实施例

本实施例基于无迹卡尔曼滤波的舰船升沉运动测量方法，如图1所示，包括以下步骤：

步骤1：实时采集安装在舰船内的惯性测量单元的各轴陀螺仪和加速度计载体坐标系下的输出数据Δθ_k和

较佳地，陀螺仪输出角增量Δθ_k需采用多子样算法补偿后输出。

步骤2：由于不使用高精度的惯性导航系统，陀螺仪、加速度计零偏影响会导致姿态矩阵在短时间内发散，其可通过实时计算姿态矩阵的水平失准角进行补偿，进而计算得到载体坐标系b与地理做变性n的姿态矩阵

其可通过迭代的方式得到地理坐标系下舰船升沉运动加速度y_k；舰船升沉加速度计算的迭代形式为：

式中I为三维单位矩阵；φ^b为姿态矩阵水平失准角；(·)×为反对称矩阵；gⁿ为舰船所处位置重力加速度；C₃为姿态矩阵

的第三行向量；

为舰船加速度；为

的天向分量，即舰船升沉加速度；较佳地，舰船加速度计算根据比力方程需考虑哥氏加速度、离心加速度的影响：

式中

为舰船加速度；

为姿态矩阵，

为加速度计比力输出，

为哥氏加速度补偿项，为地球自转速度，vⁿ为舰船航行速度，gⁿ为舰船所处位置重力加速度。

一般情况，由于舰船航行速度不高，可以忽略式中右端的地球自转速度和哥氏加速度补偿项；简化为：

步骤3：每隔时间间隔T对舰船升沉加速度信息做FFT，得到升沉加速度的幅度谱

和相位谱

升沉位移的幅度谱A(ω)和相位谱

求解形式为：

较佳地，对舰船升沉加速度信息做FFT，需要具体根据舰船升沉运动周期选择合适的窗口长度和采样时间，要保证可以检测到升沉运动波形的最高频率并满足升沉运动的频率分辨率要求，采样时间应大于20秒。

步骤4：每隔时间间隔T在升沉位移频域内进行峰值检测，其中频域峰值检测的频率范围为0.05～1Hz，确定升沉运动模型参数N_m及每个周期分量运动模型所对应的频率ω_j和相位

舰船升沉运动周期分量的增加或减少都需要重新初始化式(1)的舰船升沉运动模型，系统需要根据海况变化进行调整；其中舰船升沉位移模型为

式(1)中j＝1,2,...,N_m，z(t)为舰船升沉位移、A_j,ω_j和

分别是每一个正弦周期分量的振幅、特征频率和相位。

步骤5：根据舰船升沉运动加速度测量值y_k与舰船升沉运动状态量x_k关系的解析模型，基于UKF进行升沉运动滤波解算；其中，解析模型为：

式(2)中

b_z,k为舰船升沉加速度零偏、W_k-1为过程噪声，V_k为量测噪声；式(2)为UKF的状态方程和量测方程，分别对两个方程做UT变换，基于UKF进行升沉运动滤波解算

步骤6：实时输出舰船升沉运动的速度和位移信息：

利用某型号MEMS惯性导航系统在海浪模拟运动平台上实验，海浪模拟运动平台上放置激光测距仪，测得台面到实验室房顶的相对运动，经过转换得到实时升沉位移，其中MEMS惯性导航系统中陀螺常值漂移约0.75°/s，加速度计的逐次启动常值约15mg(g＝9.8m/s²)。记录下惯性导航系统测量得到的升沉加速度数据和激光测距仪测量得到的升沉位移数据，作为事后处理和分析。为了对比升沉运动处理算法的精度，这里同时使用扩展卡尔曼滤波(EKF)的方法处理数据，把激光测距仪测量得到的升沉位移作为参考基准。

图2给出了实验数据在0～54秒，时间段的升沉位移卡尔曼滤波方差收敛图，其中曲线a为EKF方法升沉位移标准差收敛曲线，升沉位移滤波收敛时间大约为11s，曲线b为本发明UKF方法升沉位移标准差收敛曲线，升沉位移滤波收敛时间大约为4.8s。

图3给出了实验数据在0～54秒，时间段的升沉位移滤波结果图，其中曲线c为激光测距仪测量的升沉位移参考值，升沉幅度约0.1m，周期为10s左右，曲线e为采用本发明实施例中提供的舰船升沉测量方法的结果，它与激光测距仪测量的升沉位移参考值吻合得比较好，精度较高；图中还给出了如曲线e所示的使用EKF方法的处理结果。很明显本发明方法具有更高的精度。

同样以激光测距仪测量的升沉位移参考基准，表1给出了EKF方法升沉位移测量结果和激光测距仪测量的升沉位移结果的误差的RMS(Root Mean Square，平方根)，以及本发明实施例方法的结果和激光测距仪测量的升沉位移结果的误差的RMS的精度统计结果。由表1可见，本发明基于UKF的舰船升沉测量方法相比于EKF方法具有更快的收敛时间和更高的收敛精度。本发明方法的实时升沉测量误差约为最大升沉幅值的5％。

表1升沉精度统计

综上，本发明涉及一种基于UKF的舰船升沉运动测量方法，在不增加新的传感器，不需要外部信息的情况下，实时采集惯性测量单元的输出数据，计算实时姿态信息，得到地理坐标系下舰船升沉运动加速度信息，通过对舰船升沉加速度信息做FFT和对舰船升沉位移做频域峰值检测获得舰船升沉运动的各次谐波信息，得到舰船升沉运动模型，结合惯性导航系统测得的舰船升沉运动加速度和舰船升沉运动状态量的关系，得到舰船升沉运动的观测模型，基于UKF对升沉运动滤波解算，实现对升沉信息的实时精确测量。

图4为本发明实施例提供的舰船升沉测量系统测量原理流程图，包括如下步骤：(1)实时采集安装在舰船内的惯性测量单元的各轴陀螺仪和加速度计的输出数据；(2)计算得到载体坐标系b与地理做变性n的姿态矩阵

得到地理坐标系下舰船升沉运动加速度y_k；(3)每隔时间间隔T对舰船升沉加速度信息做FFT，得到升沉加速度的幅度谱

和相位谱

进而计算得到升沉位移的幅度谱A(ω)和相位谱

(4)每隔时间间隔T在升沉位移频域内进行峰值检测，其中频域峰值检测的频率范围为0.05～1Hz，确定升沉运动模型参数N_m及每个周期分量运动模型所对应的频率ω_j和相位得到舰船升沉运动模型；(5)根据舰船升沉运动加速度测量值y_k与舰船升沉运动状态量x_k关系的解析模型，基于UKF进行升沉运动滤波解算；(6)实时输出舰船升沉运动的位移信息。

以上显示和描述了本发明的基本原理和主要特征以及本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (5)

1.一种基于无迹卡尔曼滤波的舰船升沉运动测量方法，其特征在于：所述舰船升沉运动测量方法包括如下步骤：

步骤1、实时采集安装在舰船内的惯性测量单元的各轴陀螺仪和加速度计的输出数据；

步骤2、计算得到载体坐标系b与地理坐标系n的姿态矩阵，得到地理坐标系下舰船升沉运动加速度；

步骤3、对舰船升沉加速度信息做FFT，得到升沉加速度的幅度谱和相位谱,进而计算得到升沉位移的幅度谱和相位谱；

步骤4、在升沉位移频域内进行峰值检测，确定升沉运动模型参数及每个周期分量运动模型所对应的频率和相位；其中，舰船升沉位移模型为

式(1)中j＝1,2,...,N_m，z(t)为舰船升沉位移、A_j,ω_j和分别是每一个正弦周期分量的振幅、特征频率和相位；

步骤5、根据舰船升沉运动加速度测量值y_k与舰船升沉运动状态量x_k关系的解析模型，基于无迹卡尔曼滤波进行升沉运动滤波解算；

步骤6、实时输出舰船升沉运动的速度和位移信息z(t)。

2.根据权利要求1所述的基于无迹卡尔曼滤波的舰船升沉运动测量方法，其特征在于：所述步骤2具体为：根据步骤1得到的陀螺仪角增量输出Δθ_k和加速度计比力输出

舰船升沉加速度计算的迭代形式为：

式中I为三维单位矩阵；φ^b为姿态矩阵水平失准角；(·)×为反对称矩阵；gⁿ为舰船所处位置重力加速度；C₃为姿态矩阵

的第三行向量；为舰船加速度；

为

的天向分量，即舰船升沉加速度。

3.根据权利要求1所述的基于无迹卡尔曼滤波的舰船升沉运动测量方法，其特征在于：所述步骤3具体为：每隔时间间隔T对舰船升沉加速度信息做FFT，升沉位移的幅度谱A(ω)和相位谱

由加速度的幅度谱

和相位谱

求得：

4.根据权利要求1所述的基于无迹卡尔曼滤波的舰船升沉运动测量方法，其特征在于：所述步骤4具体为：每隔时间间隔T在升沉位移频域内进行峰值检测，其中频域峰值检测的频率范围为0.05～1Hz，舰船升沉运动周期分量的增加或减少都需要重新初始化式(1)的舰船升沉运动模型，系统需要根据海况变化进行调整。

5.根据权利要求1所述的基于无迹卡尔曼滤波的舰船升沉运动测量方法，其特征在于：所述步骤5具体为：舰船升沉运动加速度测量值y_k与舰船升沉运动状态量x_k关系的解析模型为：

式(2)中

b_z,k为舰船升沉加速度零偏、为过程噪声，V_k为量测噪声，且式(2)为无迹卡尔曼滤波的状态方程和量测方程，分别对状态预测和量测预测做UT变换，基于无迹卡尔曼滤波进行升沉运动滤波解算。

Images