Measurement method of ship's heave motion information based on IMU and UKF algorithm
-
摘要:
为获取实时、精确的船舶升沉运动信息，根据船舶升沉运动模型和频谱分析方法，建立描述惯性测量单元（IMU）的加速度测量信息与船舶升沉运动状态量关系的解析模型。基于无迹卡尔曼滤波（UKF）算法非线性滤波的特点，进行升沉运动滤波解算。通过仿真分析证明了所提方法在船舶升沉运动测量中的有效性。利用三自由度平台升沉运动测量实验验证，结果表明，同一模型下，相比于扩展卡尔曼滤波（EKF）算法的解算结果，所提方法具有更快的收敛速度和更高的测量精度；对船舶升沉位移的估计精度达到最大升沉幅值的5%，可以得到精确、无时延的船舶升沉运动信息。
-
关键词:
- 升沉测量 /
- 惯性测量单元(IMU) /
- 无迹卡尔曼滤波(UKF) /
- 频谱分析 /
- 平台实验
Abstract:To obtain accurate estimation of ship heave motion information in real time, according to the ship heave kinematics model and spectral analysis method, an analytical model describing the relationship between the acceleration measurement information of the Inertial Measurement Unit (IMU) and the ship heave motion state quantity was established. Based on the characteristics of Unscented Kalman Filter (UKF) nonlinear filtering algorithm, the heave motion filtering solution is performed. Simulation analysis proves the effectiveness of the proposed algorithm in ship heave motion measurement. The experimental verification of the three-degree-of-freedom platform heave motion measurement is performed. The experimental results show that, compared with the solution of the Extended Kalman Filter (EKF) method, the algorithm in this paper has faster convergence speed and higher measurement accuracy; the estimation accuracy of the ship's heave displacement reaches 5% of the maximum heave amplitude, and accurate and no-delay ship heave motion information can be obtained.
-
许多海上作业，如航行补给、舰载机的起降、海底测绘和海上起重机工作等，都需要对由海浪、海风和洋流等复杂海洋环境因素引起的船舶升沉运动进行补偿，这就需要实时、精确的船舶升沉运动信息[1-4]。
卡尔曼滤波器(KF)广泛应用于最优估计领域中。经典的卡尔曼滤波针对线性高斯系统对目标状态做出最优估计，在一定条件下得到准确的估计状态[5]。扩展卡尔曼滤波(EKF)针对非线性高斯系统，对其非线性部分通过一阶泰勒展开使其模型线性化，并且仅在特定范围的初始条件下收敛[5]，而无迹卡尔曼滤波(UKF)通过UT(Unscented Transformation)的非线性变换方法直接进行非线性函数的状态及其方差阵传播，避免了非线性函数线性化近似过程中复杂的Jacobian矩阵的求解，适用于强非线性系统，改善了状态估计的准确性和收敛性[6-7]。利用惯性测量单元(IMU)可以测量船舶的升沉加速度，通过积分计算得到升沉速度和升沉位移。由于惯性器件误差和惯导系统高度通道不稳定等原因，加速度积分误差会随时间不断增长。故使用惯性传感器测量船舶升沉运动信息通常需要外部传感器的辅助或者通过滤波估计的方式实现。Godhavn[8]提出使用GPS实时动态差分技术(RTK)辅助加速度计进行升沉运动测量，得到了较高的升沉运动精度，但该方法需要基站辅助，而在海洋上很难获得高精度的参考信号。升沉运动测量中，由于所需要的升沉信号频率与船舶主动航行运动、积分漂移的频率不同，可以对积分信号进行滤波或估计来得到有效的升沉运动信息。通过设计高通滤波器的方法进行船舶升沉运动测量，往往需要解决滤波器相位超前问题。Hu和Tao[9]提出通过设计一对级联高通和全通IIR滤波器以解决相位误差。黄卫权等[2]提出基于带限傅里叶线性组合(BMFLC)算法对船舶升沉运动频率实时估计来调整滤波器结构。严恭敏等[10]通过先设计低通滤波器，再采用互补的方法将其转换为高通滤波器，改善了高频段的相频特性，但在阻通过渡带的幅频特性不理想。袁书明等[11]提出了自适应延时校正算法来解决高通滤波器相位超前问题。相比于通过设计高通滤波器来进行船舶升沉测量，在时域内设计的卡尔曼滤波器提供了另一种解决方案，该方法不存在相位问题且可以实时进行船舶升沉测量。Küchler等[12]采用EKF对船舶升沉位移进行滤波估计，但由于忽略了船舶横摇和纵摇角对船舶升沉加速度计算的影响，并且相比于非线性滤波存在收敛慢和线性化误差的缺点，会造成测量误差。
针对上述问题，本文提出了基于IMU器件与UKF算法测量船舶升沉运动信息的方法。首先，分析了船舶升沉运动模型，根据姿态更新算法和比力方程计算得到船舶升沉加速度；其次，通过对升沉加速度进行频谱分析和频域峰值检测，得到升沉运动测量所需的频率信息；然后，结合升沉运动模型，建立船舶升沉运动测量的状态方程和量测方程，并基于UKF理论进行滤波解算；最后，通过仿真分析和平台实验对本文方法进行了验证。
1. 船舶升沉运动测量原理
1.1 船舶升沉运动模型
船舶的升沉运动从短时间来看，就是船舶沿地理坐标系天向轴方向所做的周期运动。根据信号叠加原理，可以通过多个振幅不同、频率不同、初始相位不同的余弦波的叠加得到[13-14]。将其表示为关于时间t的函数：
(1) 式中：Aj、ωj和φj分别为第j个余弦波分量的幅值、特征频率和初始相位；Nm为拟合海浪运动主要频率分量的余弦波个数。
船舶升沉运动测量依赖于对式(1)中各参数的准确估计。船舶升沉速度和升沉加速度可以分别通过升沉位移的一阶导和二阶导得到。
升沉速度表示为
(2) 升沉加速度表示为
(3) 由式(1)和式(3)可以得到，船舶升沉位移各余弦分量幅值与升沉加速度各余弦分量幅值的关系为
(4) 式中：A(ωj)为船舶升沉位移各余弦分量幅值；
同样可以得到，船舶升沉位移和升沉速度的各余弦分量初始相位与升沉加速度各余弦分量初始相位之间的关系为
(5) 
(6) 式中：φ(ωj)、
1.2 基于IMU的船舶升沉信息测量
船舶升沉运动测量如图 1所示。以船舶中心为船体坐标系的原点，yb轴沿船体纵轴指向船体前向，xb轴与yb轴垂直沿船体横轴指向船体右舷，zb轴与前两轴垂直沿船体竖直向上。取船体坐标系oxbybzb为载体坐标系(b系)、地理坐标系oxnynzn为导航坐标系(n系)，这里取“东北天”坐标系，原点与船体坐标系原点重合。本文假设IMU安装在船体中心位置，所包含的三轴加速度计和三轴陀螺仪可以分别测量船舶在船体坐标系下沿3个坐标轴运动的加速度信息和绕3个坐标轴旋转的角速度信息，通过对陀螺仪和加速度计信息处理得到船舶的升沉信息。
船舶升沉运动信息测量依赖于准确的水平姿态测量，考虑平台失准角误差，捷联惯导姿态矩阵更新表达式为
(7) 式中：Cb(k)n为k时刻的姿态矩阵; Cb(k-1)n为k-1时刻的姿态矩阵；I为单位矩阵；Δθk为k-1时刻到k时刻陀螺仪的等效旋转矢量输出；ϕb为平台失准角在载体坐标系下的投影；(·)×为反对称矩阵。
船舶处于低加速度运动状态时，可以利用加速度计测量进行水平姿态对准得到初始姿态矩阵Cb(0)n。另外，平台水平失准角在载体坐标系的投影可以表示为[15]
(8) 式中：fsfb为载体坐标系下的加速度计测量的比力信息；C为姿态矩阵Cbn的第3行向量。
捷联惯导系统的比力方程为
(9) 式中：vn为载体在地理坐标系下的速度；gn为重力加速度在地理坐标系下的投影；(2ωien+ωenn)×vn为地球自转和哥氏补偿项，可忽略不计。
式(9)简化为
(10) 船舶升沉加速度可以表示为
(11) 式中：
对带有误差的升沉加速度信号az二次积分得到船舶的升沉位移估算，由于加速度信号存在误差，该积分值随着时间的推移会迅速地偏离船舶升沉位移的真实值。如图 2所示，利用IMU采集波浪模拟平台升沉运动信息，通过计算得到导航坐标系下升沉加速度，经二次积分运算得到升沉位移曲线。从图 2中可以看出，由于未经滤波处理，捷联解算得到的船舶升沉位移估算误差迅速增大。
为了抑制噪声并消除加速度零偏对船舶升沉运动信息测量的影响，通过对IMU输出信息进行捷联解算和滤波处理得到拟合升沉运动的主要余弦分量，从而得到船舶升沉位移信息。
船舶升沉运动估计算法流程如图 3所示。首先，对IMU进行标定并做相应的误差补偿[16]，根据式(7)和式(8)对陀螺仪和加速度计数据处理得到系统的实时姿态矩阵，通过对载体坐标系下加速度信息进行坐标变换得到导航坐标系下的船舶升沉加速度az。然后，选择合适的窗口长度对升沉加速度信号az作快速傅里叶变换(FFT)，保证可以检测到升沉运动波形主要余弦分量的最高频率，由此可以得到升沉加速度信号的幅度谱
通过仿真生成船舶升沉位移信息，并对其进行FFT。图 4为船舶升沉运动的幅度谱，峰值检测算法可以检测出幅度谱中的局部最大值。可以看出，波形中有2个主要的局部峰值，即Nm=2。峰值检测算法输出每个余弦波的幅值Aj和特征频率ωj，根据各波形的特征频率ωj和FFT得到的相频谱得到对应的相位φj。
为了得到准确的船舶升沉运动模型，必须去掉特征频率为零和特征频率接近的余弦波分量，同时为了应对海浪时刻的变化，必须在固定的时间间隔重新进行峰值检测。
2. 基于UKF的船舶升沉运动测量
2.1 船舶升沉运动状态空间模型
式(1)为船舶升沉运动模型，组成升沉运动的每个频率分量的余弦波都可以写成微分方程(12)的解：
(12) 初始时刻为zj(t0)、
由于船舶升沉运动模型中的频率信息需要通过固定时间间隔的FFT得到，即组成船舶升沉运动周期分量的增加或减少都需要重新初始化系统，系统模型需要根据海况变化进行调整。因为FFT得到的余弦波特征频率信息是在一段时间范围内的均值，只能被用来初始化系统，所以将特征频率ωj作为系统变量进行估计，建模为随机常值模型。
(13) 建立系统的状态向量为
(14) 式中：Xj为第j个频率分量的系统状态量，其组成分量为该特征频率余弦波的位移、速度和特征频率，初值由FFT得到的参数计算得到。
根据式(4)和式(10)，得到系统的状态空间模型为
(15) 式中：Yj对应第j个频率分量余弦波的升沉加速度。
2.2 船舶升沉运动测量的状态方程
根据IMU输出计算得到的升沉加速度为各分量余弦波加速度的和，同时考虑加速度测量的零偏和噪声，得到
(16) 式中：bz为升沉加速度零偏，通常由加速度计零偏造成；ζz为升沉加速度测量噪声。
为了消除升沉加速度零偏对升沉运动信息测量的影响，需要将零偏bz作为系统变量进行准确的估计。结合式(14)得到最终的系统状态向量为
(17) 式中：
(18) 经离散化处理，得到系统的状态方程为
(19) 式中：
(20) 其中：T为状态更新时间间隔；wk为系统的过程噪声，系统协方差矩阵为Qk=E[wk wkT], 由各频率组成部分的系统变量的动力学模型确定。
2.3 船舶升沉运动测量的观测方程
由式(15)得到系统的观测方程为
(21) 式中：az, k为k时刻船舶在地理坐标系下的升沉加速度。
根据式(15)和式(16)，式(21)经离散化处理，系统的观测方程为
(22) 式中：
(23) 量测噪声方差矩阵为Rk=ζz, k2，由升沉加速度测量噪声确定。
2.4 无迹卡尔曼滤波
2.4.1 无迹卡尔曼滤波模型
(24) 式中：Xk∈Rn为k时刻系统的状态向量；Yk∈Rm为k时刻系统的观测向量；f(·)为实的n维向量函数；h(·)为实的m维向量函数；f(·)和h(·)对其自变量而言都是非线性的；wk∈Rn为系统的过程噪声，常设为协方差Qk≥0的零均值高斯白噪声；vk∈Rm为系统的量测噪声，常设为方差Rk>0的零均值高斯白噪声。
2.4.2 无迹卡尔曼滤波器设计
船舶升沉运动模型属于非线性模型，拟合船舶升沉运动的主要余弦分量是时刻变化的，而FFT得到的各余弦运动的频率、幅值和相位都不具有实时性，直接使用这些参数进行拟合是无意义的。UKF适用于非线性系统，相比于EKF，避免了非线性方程线性化过程产生的误差，对模型变量估计的快速性和准确性是拟合船舶升沉运动的重要前提。
对加速度计测量得到的升沉运动加速度数据作FFT，计算得到滤波器的状态初始值X0，设定过程噪声协方差Qk、量测噪声方差Rk和初始协方差矩阵P0。式(19)和式(22)分别为系统的状态方程和量测方程，UKF推导过程如下[17]：
1) 给定状态估计
(25) 式中：n为状态向量的维数；α用于调节sigma点在均值
2) 时间更新。
(26) 式中：
(27) 其中：β为先验知识常数，高斯分布时最优值取2。
(28) 式中：κ为比例因子，状态估计时通常设为0；
3) 量测更新。
(29) 式中：
3. 仿真验证
基于IMU和UKF算法测量船舶升沉运动信息，通过滤波估计可以得到组成升沉运动的主要余弦分量，叠加后得到准确的升沉速度和升沉位移信息。为了验证本文方法的有效性，利用仿真数据来进行验证。仿真条件如下：船舶升沉运动拟合频段为0.05~0.2 Hz，模型参数Nm=4，升沉加速度由升沉位移真值的二阶导数与加速度零偏和噪声叠加而成，数据采样频率为50 Hz，升沉加速度零偏为0.05 m/s2。图 5为升沉加速度仿真实验数据。初始滤波参数如下：各余弦分量的升沉位移滤波标准差为0.2 m, 升沉速度滤波标准差为0.2 m/s, 特征频率滤波标准差为0.3 rad/s, 加速度零偏滤波标准差为0.5 m/s2。特征频率滤波噪声方差为4×10-6(rad/s)2，升沉加速度零偏滤波噪声方差为1×10-8(m/s2)2，升沉加速度滤波噪声方差为4×10-8(m/s2)2。升沉位移初值为[0 m 0 m 0 m 0 m]T, 升沉速度初值为[0.03 m/s 0.06 m/s 0.04 m/s 0.05 m/s]T, 特征频率初值为[0.31 rad/s 0.42 rad/s 0.54 rad/s 0.6 rad/s]T，加速度零偏初值为0 m/s2。
图 6~图 8分别为上述仿真条件下的船舶升沉加速度零偏估计、升沉速度和升沉位移的滤波结果。
图 6 升沉加速度零偏估计仿真结果Figure 6. Simulation results of zero bias estimation of heave acceleration通过图 6的仿真结果可以看出，升沉加速度零偏估计在滤波开始后8 s左右收敛到0.05 m/s2附近，与仿真条件给出的真值吻合。从图 7和图 8的仿真结果可以看出，升沉速度与升沉位移滤波估计值与真值吻合。同样可以看出，相比于升沉位移估计，升沉速度估计具有更高的精度，这是由于升沉速度为加速度信号的一次积分，而升沉位移是加速度信号的两次积分。因此，升沉加速度误差对位移的影响大于对速度的影响。仿真结果表明，升沉速度、位移误差保持在可接受的范围内，证明了本文方法的可行性。
4. 平台实验
为了验证本文方法对船舶升沉运动速度、位移的估计效果，通过设计相关的实验进行验证。采用实验室现有的三自由度运动平台、Crossbow公司的IMU440惯性测量单元和激光测距仪搭建实验环境。同时，在相同升沉运动测量模型和初始条件下与EKF算法结果相比较。如图 9所示，三自由度运动平台用来模拟船舶升沉运动，IMU440惯性测量单元和激光测距仪安装在三自由度运动平台的中心位置，其中惯性测量单元用作升沉运动信息测量，激光测距仪测量安装平台与地面固定平台之间的相对运动，经过转换得到升沉运动的速度和位移基准。
IMU采集、计算得到平台升沉加速度信息如图 10所示。
根据升沉运动的频率分辨率要求，窗口长度取20~30 s数据长度，对升沉加速度数据作FFT，通过峰值检测算法得到船舶升沉运动模型参数，计算得到系统的滤波初值X0=[-0.01 m 0.01 m/s 0.61 rad/s 0 m/s2]T，根据实验调整确定系统状态滤波初始协方差矩阵P0=diag([0.4 m 0.5 m/s 0.5 rad/s 0.8 m/s2]T)2。系统的特征频率噪声方差通常由升沉运动的实际工况确定，而由于加速度计零偏是造成天向加速度偏移的主要原因，其噪声特性可以先根据IMU440惯性测量单元的加速度计性能参数给出，再根据实际的滤波结果做出调整，设定系统的过程噪声协方差矩阵和量测噪声方差: Qk=diag([0 m 0 m/s 2×10-3 rad/s 2×10-4 m/s2]T)2, Rk=9×10-8(m/s2)2。
图 11和图 12为升沉运动滤波结果。图中：蓝色虚线为本文方法滤波结果，红色实线为相同模型下EKF算法滤波结果。滤波估计状态标准差曲线显示，基于UKF的方法具有更快的收敛速度和更高的测量精度。
由于在实际应用中，需要在固定时间间隔对海浪的主要频率分量进行检测，当模型中新增或减少频率分量时，需要对系统重新初始化，在面临高动态的海浪运动变化时，为得到高精度的升沉运动信息，往往需要滤波器具有快速收敛的特性。
图 13和图 14给出了升沉运动测量实验的速度和位移曲线。图中：黑色实线为激光测距仪测量得到的升沉速度、位移参考值，其中升沉位移幅值最大约0.1 m，蓝色虚线为采用UKF算法得到的结果，红色点划线为采用EKF算法得到的结果。
图 15给出了船舶升沉位移估计误差曲线。在滤波稳定后，本文方法升沉位移测量误差小于0.5 cm，即小于最大升沉幅值的5%，能够得到高精度、无时延的升沉运动信息。
5. 结论
1) 根据姿态更新算法和实时计算平台水平失准角得到船舶水平姿态信息，进而根据比力方程计算得到升沉加速度信息。通过对升沉加速度作FFT并结合升沉运动学模型得到系统滤波初值。
2) 算法实现了准确估计升沉加速度零偏和组成升沉运动的各余弦分量，进而通过叠加得到船舶升沉速度和升沉位移信息。
3) 仿真分析和平台实验结果表明，滤波估计的快速收敛和无时延特性适合船舶升沉运动组成频率分量变化、系统重新初始化的情况；对船舶升沉位移的估计精度达到最大升沉幅值的5%。对满足船舶海上作业具有一定的工程借鉴价值。
-
图 6 升沉加速度零偏估计仿真结果
Figure 6. Simulation results of zero bias estimation of heave acceleration
-
[1] RICHTER M, SCHNEIDER K, WALSER D, et al. Real-time heave motion estimation using adaptive filtering techniques[J]. IFAC Proceedings Volumes, 2014, 47(3): 10119-10125. doi: 10.3182/20140824-6-ZA-1003.00111 [2] 黄卫权, 李智超, 卢曼曼. 基于BMFLC算法的船舶升沉测量方法[J]. 系统工程与电子术, 2017, 39(12): 159-164.HUANG W Q, LI Z C, LU M M. Method of measurement for ship heave motion based on BMFLC algorithm[J]. Systems Engineering and Electronics, 2017, 39(12): 159-164(in Chinese). [3] 孙伟, 孙枫. 基于惯导解算的舰船升沉测量技术[J]. 仪器仪表学报, 2012, 33(1): 162-172.SUN W, SUN F. Measurement technology of ship heave movement based on SINS resolving[J]. Chinese Journal of Scientific Instrument, 2012, 33(1): 162-172(in Chinese). [4] 刘星伯. 基于捷联航姿系统的舰船瞬时线运动信息测量研究[D]. 哈尔滨: 哈尔滨工程大学, 2009.LIU X B. Study on the measuring of the instaneous movements of the ships based on SINS[D]. Harbin: Harbin Engineering University, 2009(in Chinese). [5] 秦永元. 卡尔曼滤波与组合导航原理[M]. 3版. 西安: 西北工业大学出版社, 2015: 199-220.QIN Y Y. Kalman filtering and integrated navigation principle[M]. 3rd ed. Xi'an: Northwestern Polytechnical University Press, 2015: 199-220(in Chinese). [6] JULIER S J, UHLMANN J K, DURRANT-WHYTE H F. A new approach for filtering nonlinear systems[C]//American Control Conference, 1995: 1628-1632. [7] JULIER S J. Unscented filtering and nonlinear estimation[J]. Proceedings of the IEEE, 2004, 92(3): 401-422. doi: 10.1109/JPROC.2003.823141 [8] GODHAVN J M. High quality heave measurements based on GPS RTK and accelerometer technology[C]//OCEANS 2000 MTS/IEEE Conference and Exhibition. Piscataway: IEEE Press, 2000: 309-314. [9] HU Y P, TAO L M. Real-time zero phase filtering for heave measurement[C]//The 11th IEEE International Conference on Electronic Measurement & Instruments. Piscataway: IEEE Press, 2013: 321-326. [10] 严恭敏, 苏幸君, 翁浚, 等. 基于惯导和无时延滤波器的船舶升沉测量[J]. 导航定位学报, 2016, 4(2): 91-93.YAN G M, SU X J, WENG J, et al. Measurement of ship's heave motion based on INS and zero-phase-delay digital filter[J]. Journal of Navigation and Positioning, 2016, 4(2): 91-93(in Chinese). [11] 袁书明, 程建华, 马斌. 基于自适应频率估计的船舶瞬时线运动测量方法[J]. 中国惯性技术学报, 2016, 24(5): 565-570.YUAN S M, CHENG J H, MA B. Measurement method for ship instantaneous linear movement based on adaptive frequency estimation[J]. Journal of Chinese Inertial Technology, 2016, 24(5): 565-570(in Chinese). [12] KVCHLER S, EBERHARTER J K, LANGER K, et al. Heave motion estimation of a vessel using acceleration measurements[J]. IFAC Proceedings Volumes, 2011, 44(1): 14742-14747. doi: 10.3182/20110828-6-IT-1002.01935 [13] 刘婉婷. 船舶升沉运动预报研究[D]. 大连: 大连海事大学, 2016.LIU W T. Study on heave motion prediction of ships[D]. Dalian: Dalian Maritime University, 2016(in Chinese). [14] 李智超. 基于惯导系统的舰船升沉测量技术研究[D]. 哈尔滨: 哈尔滨工程大学, 2018.LI Z C. Research on ship heave measurement technology based on inertial navigation system[D]. Harbin: Harbin Engineering University, 2018(in Chinese). [15] 严恭敏, 翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019: 222-224.YAN G M, WENG J. Strapdown inertial navigation algorithm and integrated navigation principle[M]. Xi'an: Northwestern Polytechnical University Press, 2019: 222-224(in Chinese). [16] 刘萍. 基于MEMS的高精度姿态检测系统设计[D]. 哈尔滨: 哈尔滨工程大学, 2016: 11-21.LIU P. Design of high precision attitude detection system based on MEMS[D]. Harbin: Harbin Engineering University, 2016: 11-21(in Chinese). [17] 袁赣南, 张涛. 四元数UKF超紧密组合导航滤波方法[J]. 北京航空航天大学学报, 2010, 36(7): 762-766. https://bhxb.buaa.edu.cn/CN/abstract/abstract8434.shtmlYUAN G N, ZHANG T. Quaternion unscented Kalman filtering for ultra-tight integration[J]. Journal of Beijing University of Aeronautics and Astronautics, 2010, 36(7): 762-766(in Chinese). https://bhxb.buaa.edu.cn/CN/abstract/abstract8434.shtml -
下载:
下载:
点击查看大图
计量
- 文章访问数: 1204
- HTML全文浏览量: 218
- PDF下载量: 121
- 被引次数: 0
