随机波浪下基于粒子滤波算法的船舶航向控制

王化明，岳彩宇，陈 林，王恒家，娄贞光

(1.浙江海洋大学船舶与海运学院，浙江舟山 316022；2.浙江海洋大学东海科学技术学院，浙江舟山 316000)

船舶航向控制是船舶运动控制的主要内容之一。在实际船舶航行中，船舶拥有良好的航向控制性能，能够保证航行的安全、降低能源损耗。随着控制理论的发展，将控制算法[1-4]应用在船舶控制当中，实现对船舶航向的自动控制。在低海况下，控制算法可以精确地控制船舶航向，但是船舶在高海况下航行时，船舶的控制性能会迅速下降，这是由于高海况下船舶以非线性运动为主，且随机波浪对船舶运动状态的影响很大以及控制算法与船舶状态变量的交互影响所造成的。通过对船舶状态变量进行滤波处理，得到其真实状态值，从而可以更有效地控制船舶的航向。因此滤波算法在对船舶航向控制性能提升方面具有重要意义。

目前，利用整体型船舶运动模型研究船舶在水面运动，主要是仅考虑在静水条件下，未考虑环境扰动对船舶运动的影响，如WANG Xuegang,et al[5]基于整体型船舶模型在静水条件下利用支持向量机对船舶运动黑箱建模。实际生活中，环境扰动对船舶运动影响非常大，如风、水流、波浪等，其中波浪影响特别明显，因此需要考虑波浪的影响，建立一种船舶扰动模型来准确地描述船舶的运动情况。在平稳随机过程的波浪作用下，船舶扰动呈现的是一种非平稳非随机过程，属于一种非常复杂的非线性运动，船舶每一时刻运动变量都会在真实状态附近上下波动，因此通过滤波算法估计得到船舶运动变量的真实状态很有必要。传统是通过卡尔曼滤波进行数据处理，如郭鹏飞等[6]利用卡尔曼滤波算法对目标的状态进行估计。但是该算法要求系统是线性系统。随着非线性估计理论的发展，出现了适用于非线性系统的滤波算法[7-8]，其中粒子滤波算法[9-11]非常适合解决非线性系统的状态估计问题。

本文基于船舶非线性运动数学模型，考虑随机波浪的影响，建立一种船舶扰动模型更准确的来描述船舶的水面运动。利用粒子滤波算法对船舶运动变量进行状态估计，再根据比例-积分-微分(proportion integral derivative,PID)控制算法得到船舶命令舵角，对船舶的航向进行调整，使得船舶在随机波浪下的航向更稳定。

1 船舶扰动数学模型

一般采用船舶三自度整体型运动数学模型研究船舶的水面运动。考虑波浪对船舶水面运动影响，在三自由船舶运动数学模型基础上，引入波浪项。由于波浪对船舶的作用较为复杂，本文将有向浪简化为随机波浪，仅研究在随机波浪下船舶的水面运动情况。船舶的扰动数学模型为：

船舶的姿态方程及舵机表达式为：

其中，m 表示船的重量，X，Y，N 分别表示船舶纵向运动方向、横向运动方向、艏摇运动方向的力和力矩；u，v，r，δ，ψ 分别表示船舶纵向运动速度、横向运动速度、艏摇角速度、舵角、航向角；Tr为时间常数，δr为命令舵角；Xw，Yw，Nw分别表示波浪对船舶纵向运动、横向运动、艏摇运动的主干扰力和力矩。随机波浪下船舶受到的主抗力和力矩的计算公式[12]，随机波浪的频率是根据能量均分法得到的。

对式(1)转换可以得到船舶的状态方程，即：

对上式离散化处理，经整理后可以表示为：

船舶在实际航向中，可通过姿态传感器的测量数据得到船舶的各种状态信息，如纵向速度横向速度、艏摇角速度的值，由于仪器本身的因素使得测量的数据中存在中高斯噪声。船舶的观测方程可表示为：

其中，m1(t)，m2(t)，m3(t)为高斯白噪声。

2 粒子滤波算法

粒子滤波(particle filter,PF)的理论来源于贝叶斯估计，通过空间中一组带有权值的粒子来表示贝叶斯估计中的状态量的后验概率密度函数。它解决了贝叶斯估计中由于积分存在导致后验概率密度函数难以计算问题，适用于任何能状态空间模型表示的非线性系统。

对于非线性系统，通常可以由状态方程和观测方程表示[13-14]：

其中，xt-1表示系统状态量；nt表示系统噪声；yt表示观察量；lt表示观察噪声；系统噪声nt和观测噪声lt相互独立。根据贝叶斯估计原理[15]，可求得状态变量x0：k的后验概率密度p(x0：t|y1：t)。根据蒙特卡罗仿真原理，任意函数g(·)的数学期望表示为：

因此，式(8)可以近似写为：

然而由于后验概率分布函数不能直接得到，也使得通过一组粒子去近似后验概率密度难以实施。贝叶斯重要性采样定理给出了这个问题的解决方法，即先从一个已知的、易于采样的分布函数q(x0：t|y1：t)中采样，通过对由分布函数采样得到的粒子进行加权来近似p(x0：t|y1：t)。

对式(8)进行变换可以得到：

通过式(11)，利用带有权值的一组离散粒子可以实现对系统状态的估计。令u=g1(·)，v=g2(·)，r=g3(·)，利用粒子滤波对船舶运动变量的状态进行估计。

为了实现对后验分布的递推估计，采用序贯重要性采样方法，对权值更新公式进一步处理，获得粒子权值迭代计算公式。可将分布函数改写为：q(x0：t|y1：t)=q(xt|x0：t-1|y1：t)q(x0：t-1|y1：t)可以得到：

由于粒子滤波对系统状态估计逼近最优估计[16]，分布概率密度函数只与xk-1和zk有关，则：

在序贯重要性采样方法中，最常见的分布函数为：

将式(15)带入到式(14)可得：

为了防止粒子匮乏，对粒子进行重采样[17]，重采样主要思想是对粒子和其相应的权值表示的概率密度函数重新进行采样。通过提高权值较大粒子的数目和降低权值较小粒子的数目。经过均匀重采样后，每一个粒子的权重都会变为，即：

权重的计算公式可简化为：

在对权重进行归一化处理后，通过式(11)对系统状态进行估计。

3 PID 航向控制算法

增量式PID 控制算法适合对离散的数据处理，因此将其应用于船舶的航向控制。船舶指定航向与实际航向的偏差表达式为：

其中，ψd(t)为t 时刻船舶的指定航向值，ψ(t)为t 时刻船舶的航向实际值。

命令舵角表达式为：

4 算法流程

(1)整体粒子滤波及船舶航向控制流程如图1所示。假设状态初始概率分布结合t 时刻的值u，v，r 可以得到3 组采样，每组含有N 个粒子，分别表示t 时刻u，v，r 的先验分布。

图1 粒子滤波及船舶航向控制流程Fig.1 Particle filtration affects the ship's course control process

(2)将所得到每个粒子的值代入到系统状态方程和观测方程后计算得到t 时刻每个粒子的观测值，将u，v，r 代入到船舶扰动方程得到t 时刻各运动变量的值作为真实观测值，根据式(17)计算每个粒子的权重，并对粒子的权重归一化处理。

p(x0)为高斯分布。

(5)采用PID 航向控制算法，由ψt，δt计算得到船舶t 时刻的命令舵角。通过四阶龙格库塔法计算得到t+1 时刻的船舶的各速度值、航向角和舵角，即ut+1，vt+1，rt+1，ψt+1，δt+1，作为t+1 时刻的真实观测值。

(6)重复步骤(2)～(5)。利用步骤(3)更新的3 组新粒子群与t+1 时刻的观测值，计算t+1 时刻的权重及估计新的各速度值。

5 仿真实验与分析

以“Mariner”号为对象，基于船舶扰动运动数学模型，利用MATLAB 软件仿真船舶的航向运动。船长L为171.8 m，船宽B 为23.17 m，吃水D 为8.23 m。扰动模型中各流体动力导数的值取自PMM[18]试验。船的质量无量纲m′为7.98×10-3，转动惯量无量纲I′z为3.92×10-4，船舶重心的纵向坐标无量纲x′G为-2.3×10-2。波浪谱采用P-M 波谱。

利用粒子滤波算法对船舶的纵向速度、横向速度、艏摇角速度进行优化。每组粒子数设为200，粒子初始概率密度p(x0)采用标准正态分布的概率密度。船舶纵向速度u 初值为7.717 5 m·s-1，横向速度v 为初值0 m·s-1，艏摇角速度r 初值0 rad·s-1，舵角δ初值为0°，航向角度ψ 初值为0°，指定航向角ψd为15°。Tr取0.25。机械舵角受实际限制[19]，即|δ|≤35°。

式(17)指的是在给定的状态量的条件下，每一种输出值出现的可能性。从观测方程可得对于1 个确定的输入值，输出值仅受到观测噪声的影响，而船舶各运动速度是通过传感器采集得到，对于传感器中的噪声一般认为是高斯噪声，因此每一个粒子权重的计算公式可以表示为：

船舶运动总时间为3 200 s，采样间隔为1 s，采用临界比例法多次测试得到1 组参数值，即Kp=3，KI=0.000 1，KD=0.5。在2 级海况(有义波高为0.5 m)下仿真船舶未滤波处理以及在经过粒子滤波处理后的船舶直航运动(航向保持)与转向运动(航向跟踪)。

5.1 直航运动

图2 和图3 表示的是在相同的航向控制器控制下，船舶在波浪作用下做直航运动时，经滤波处理和无滤波处理条件下的航向角与舵角变化曲线。从图2 可以看出经粒子滤波后船舶的航向变化很小，在±2°以内，大幅度提升了船舶航向控制的精度。由图3 可看出经粒子滤波处理后，舵角的幅度控制在±5°，降低了操舵的幅度，减少了舵机的磨损，降低了能源的消耗。对于无滤波时，舵角的幅值较大，一方面是船体受波浪扰动影响；另一方面跟PID 控制参数取值有关，虽然采用临界比例法确定了一组控制参数，但是本质上还是通过经验确定的，存在着误差，且在波浪的影响下，又进一步扩大了误差，进而影响到操舵的幅值。

图2 波浪条件下航向角变化Fig.2 Course angle variation in sea condition

图3 波浪条件下舵角变化Fig.3 Change of rudder angle in sea condition

5.2 转向运动

图4 和图5 表示的是在相同的航向控制器控制下，船舶在波浪作用下做转向运动时，经滤波处理和无滤波处理条件下的航向角与舵角变化曲线。船舶在1 621 s 时指定航向角由更改为。从图4 可以看出在1 621 s之前，船舶做直航运动，之后在航向控制算法控制下船舶开始转向，最后将船舶航向角平稳调整到且保持不变，实现了对航向的跟踪。图5 在1 621 s 舵角达到最大舵角，以此快速调整船舶航向到新的指定航向角。对于船舶整个转向运动，在相同的控制器控制下，经粒子滤波处理后，船舶的航向角更接近指定航向角，且变化的范围更小，说明了滤波提升了控制算法对船舶航向控制的精度。

图4 波浪条件下航向角变化Fig.4 Course Angle variation in sea condition

图5 波浪条件下舵角变化Fig.5 Change of rudder Angle in sea condition

5.3 Z 型运动

图6 和图7 表示船舶做Z 型运动时，经滤波处理和无滤波处理条件下的航向角与舵角变化曲线。船舶在运动刚开始时保持直航运动，然后在1 621 s 将指令航向角调整到+10°，在2 929 s 时刻将指令航向角调整到-10°，以此让船舶做Z 型运动。从图中可以看出经粒子滤波处理后船舶航向角虽然在波浪影响下出现波动，但是波动范围很小，波动范围在±2°以内，舵角变化范围在±5°以内，与未滤波时的舵角相比较，降低了操舵幅度，减少能源损耗。

图6 波浪条件下航向角变化Fig.6 Course Angle variation in sea condition

图7 波浪条件下舵角变化Fig.7 Change of rudder Angle in sea condition

研究船舶航向控制性主要考虑航向角的变化及舵角的摆动情况。采用均方根统计方法对舵角和航向角的误差进行统计[5]，即：

结果表明在船舶不同类型的运动下，经粒子滤波处理后，航向角和舵角的均方根误差都在一定程度得到降低，船舶的航向更加稳定，舵角摆舵幅度得到降低，如表1 所示。

表1 航向角和舵角的均方根误差Tab.1 Mean square root error of heading angle and rudder angle

6 结论

本文基于整体型船舶运动模型，考虑波浪对船舶运动状态的影响，建立三自由度船舶扰动模型，采用PID 航向控制算法仿真船舶在波浪条件下的直航运动与转向运动，并利用粒子滤波算法对船舶运动状态进行估计。可以得到以下结论：

a.航向控制算法根据船舶运动状态对船舶进行控制，波浪对船舶运动状态影响较大，直接影响到船舶的航向控制算法对船舶航向控制精度。

b.利用粒子滤波算法可以估计得到波浪环境下船舶的运动状态，逼近船舶真实的运动状态。

c.经过粒子滤波估计得到船舶的运动状态，代入到PID 航向控制算法中，船舶的摆舵幅值和航向角偏差都较小，船舶在波浪中航行时，其航向更加稳定。

d.利用粒子滤波算法对船舶状态变量进行估计，采用相同的航向控制器控制船舶做直航运动、转向运动及Z 型运动，船舶航向控制性能都得到了显著提升。