基于动态窗口模型预测的空气动力艇路径规划与跟踪控制

Path Planning and Tracking Control of Aerodynamic Craft Based on Dynamic Window Model Prediction

摘要

为提高空气动力滑行艇路径跟踪效果，提出一种应用于空气动力滑行艇的动态窗口模型预测控制算法，以实现路径规划与跟踪控制，提高其路径跟踪精度.首先，采用传统动态窗口算法对空气动力滑行艇进行局部路径规划与路径跟踪避障仿真分析，结果表明传统动态窗口算法在空气动力滑行艇路径跟踪控制时，横向跟踪误差与艏摇角速度均不收敛，控制效果较差.其次，提出一种动态窗口模型预测控制算法，使用该算法与传统动态窗口算法进行对比.最后，通过路径跟踪仿真验证，改进后的算法在路径跟踪时横向跟踪误差更小，验证了本文所提的应用于空气动力滑行艇路径规划与路径跟踪控制的动态窗口模型预测控制算法的有效性.

Abstract

To improve the path tracking effect of the aerodynamic planing craft， a dynamic window model prediction control algorithm applied to the aerodynamic planing craft was proposed to realize path planning and tracking control and improve its path tracking accuracy. Firstly， the traditional dynamic window algorithm was adopted to conduct simulation analysis of local path planning and obstacle avoidance during path tracking for the aerodynamic planing craft. The results show that during the path tracking control of the aerodynamic planing craft by the traditional dynamic window algorithm， both the lateral tracking error and the yaw angular velocity do not converge， and the control effect is poor. Secondly， a dynamic window model prediction control algorithm was proposed， and this algorithm was compared with the traditional dynamic window algorithm. Finally， through path tracking simulation verification， the improved algorithm has a smaller lateral tracking error during path tracking， which verifies the effectiveness of the dynamic window model prediction control algorithm proposed for the path planning and path tracking control of the aerodynamic planing craft.

Graphical abstract

关键词

空气动力滑行艇 / 动态窗口 / 模型预测控制 / 路径规划 / 路径跟踪

Key words

aerodynamic planing craft / dynamic window / model prediction control / path planning / path tracking

空气动力艇是一种设计用于在水面或湿地区域运行的载运工具，它利用空气动力学原理来推进，与传统的螺旋桨或喷水推进器不同.这种独特的推进方式赋予了空气动力艇在多种环境条件下的运行能力，尤其是在湿地、沼泽、浅水等复杂地形中，传统船舶在上述地形中无法运行，因此对欠驱动空气动力艇的研究具有重要意义.无人船路径规划是一个重要的研究方向，它为无人船完成各种航行任务提供了基础和前提.因此，研究高效、准确的路径规划算法对于提高无人船的智能化水平^［1］具有重要意义.

许多学者对无人船舶的路径规划与避碰展开了研究.Kim等^［2］针对使用动态窗口算法（dynamic window approach，DWA）时，船舶可能在避障后偏离路径的问题，提出了改进的DWA算法，并通过改进前后算法的仿真比较，证明改进后的算法可以较好地跟踪路径.Kim等^［3］对水面无人艇基于国际海上避碰规则（COLREGs）的自动避碰算法展开研究，提出利用最近会遇距离（distance to closest point of approach，DCPA）和最近会遇时间（time of closest point of approach，TCPA）预测危险情况，对DWA算法改进后提出CCDWA算法，通过与DWA算法的仿真比较，验证了CCDWA算法符合COLREGs标准.Liu等^［4］提出了一种USV编队路径规划算法，基于快速前进算法提出了一种有约束的快速前进算法，提高了算法的实时性，增强了算法处理动态避碰的能力.Yu等^［5］基于自动识别系统，提出一种A*-VVGO算法将速度变化整合到路径规划中，实现了能效、快速性、经济性的多目标优化.Liu等^［6］基于蚁群算法和聚类算法构建了一种适用于复杂海洋环境的自动避障算法，并通过仿真证明算法能根据不同的障碍物分布找到合适的搜索范围.Chen等^［7］提出了基于网格图的改进蚁群优化-人工势场（ant colony optimization-artificial potential field，ACO-APF）算法，并通过仿真验证了该算法在USV局部和全局路径规划中的有效性.MahmoudZadeh等^［8］提出了一种将b样条数据框架与粒子群优化求解引擎结合的不间断路径规划系统，通过仿真验证了该算法在路径规划不间断海洋采样任务中的适用性与鲁棒性.Zhang等^［9］提出一种USV智能目标搜索路径规划算法，可根据任务目标特点切换不同的局部路径规划模式，采用改进的A*算法进行避障，通过仿真证明了该算法可以提高USV目标识别效率，降低避障时的转弯成本.Wei等^［10］设计了基于自适应LOS和改进PID的USV轨迹跟踪算法，并通过实验验证了该算法能够对1米级的水中障碍物进行避障.谭智坤等^［11］提出了融合改进动态窗口法与速度障碍法的局部路径规划算法，通过数值仿真证明该算法在规划路径长度航行时间与平均速度等方面均优于传统算法，并且具有更强的动态避障能力.何志强等^［12］提出了一种基于区域人工势场的目标跟踪策略，用于USV目标跟踪任务.张扬等^［13］针对传统人工势场法的局部最小和路径不平滑问题，通过动态调整斥力系数、引入逃逸力，并结合国际避碰规则设计转向力，使改进算法在静态与动态障碍物环境中均能引导无人艇安全避障，且路径更平滑.张宗鑫等^［14］针对含未知干扰的非对称欠驱动无人艇轨迹跟踪问题，设计未知系统动态估计器并结合改进滑模控制，相比传统算法具有更强的工程适用性.翁昱等^［15］针对鱼雷状小型无人艇在狭窄水域作业时的精度保持难题，提出基于智能预测控制的轨迹跟踪算法，通过构建非线性状态空间模型并结合改进粒子群算法优化控制序列，仿真与湖试结果表明该算法超调小、抗干扰性强，对小型USV的工程应用具有参考价值.顾潮宏等^［16］提出基于视线法的优化人工势场法，通过修正引力与斥力函数，有效降低多障碍环境下的航迹偏差与艏向角变化，解决了传统算法中转向过大与局部最小值问题.

学者对无人船路径规划与避障控制算法已经作了许多分析，但对于空气动力滑行艇局部路径规划与路径跟踪控制方面的研究较少.本文的研究对象为一种欠驱动空气动力滑行艇，滑行艇通常用于高速航行，其设计目标是最小化水面阻力，从而实现快速且高效的运动.这种高速行驶的特性要求路径规划算法具备更高的实时性和精度，以确保安全和效率.针对滑行艇的动力学特性，本文提出一种新的应用于空气动力滑行艇的动态窗口模型预测控制算法，以实现空气动力滑行艇的局部路径规划与路径跟踪避障控制.

1 动态窗口算法

动态窗口算法是常用的局部路径规划算法，通过对参考横向偏差代价、艏向角偏差代价、障碍物距离代价等进行计算，对所有预测轨迹进行评价，最后选取代价最小的轨迹对应的速度组作为参考速度对移动体进行控制.

图1为动态窗口算法示意图.图1a中包括固定窗口以及动态窗口中的离散采样点.其中障碍物映射区域是通过预测轨迹探测得知的.图1b中为移动体在各个采样的速度组下的预测轨迹以及对碰撞障碍物轨迹的删除操作.其中对于移动体的轨迹预测计算通过移动体的运动学方程，如式（1）所示：

\left.\begin{array}{l}x(t+\mathrm{1})=x(t)+u(t)\cdot \mathrm{c}\mathrm{o}\mathrm{s}(\psi (t))\cdot \mathrm{\Delta }t,\\ y(t+\mathrm{1})=y(t)+u(t)\cdot \mathrm{s}\mathrm{i}\mathrm{n}(\psi (t))\cdot \mathrm{\Delta }t,\\ \psi (t+\mathrm{1})=\psi (t)+r(t)\cdot \mathrm{\Delta }t.\end{array}\right\}(1)

其中：x，y，\psi分别为移动体在平面内的横、纵坐标与航向角；u为移动体坐标系下的纵荡速度；r为移动体艏摇角速度；\mathrm{\Delta }t为离散后相邻时刻的时间增量.

为分析动态窗口算法对空气动力滑行艇局部路径规划与路径跟踪避障时的性能，进行了局部路径规划与路径跟踪的仿真分析.该工况下，考虑到滑行艇舵力受到螺旋桨转速的影响，将纵荡速度控制与航向角控制解耦.使用动态窗口算法对滑行艇进行局部路径规划，得出滑行艇的最优艏摇角速度.使用PID算法得出舵板转角.全局路径规划产生的路径设置如式（2）所示：

\left.\begin{array}{c}{Y}_{\mathrm{N}}=\mathrm{5}\times (\mathrm{1}-\mathrm{c}\mathrm{o}\mathrm{s}(\mathrm{2}\mathrm{\pi }t)),\\ X_{\mathrm{E}}=\mathrm{30}\times \mathrm{2}\mathrm{\pi }t.\end{array}\right\}(2)

其中：X_{\mathrm{E}}为东向位移；Y_{\mathrm{N}}为北向位移；t为时间.

图2为基于动态窗口算法的局部路径规划仿真结果.由图2a～图2c可知，曲线路径跟踪误差振荡发散.由图2d可以看出，滑行艇艏摇角速度也振荡发散.

由仿真分析可以得出，使用传统动态窗口算法对滑行艇进行局部路径规划与路径跟踪控制时，由于算法的预测路径是通过运动学方程进行计算，没有考虑无人船的运动特性，横向控制效果较差.

2 空气动力滑行艇模型

空气动力滑行艇是一种高速水面船舶，主动力来自空气螺旋桨与空气舵板的空气动力.其具有纵荡、横荡、升沉、横摇、纵摇、艏摇6个自由度.为建立滑行艇数学模型，分别对滑行艇所受的水动压力、黏性阻力、静浮力、重力与空气桨、舵力进行数学建模，最后采用MMG分离型模型将所有合外力进行叠加，建立滑行艇动力学模型.

本文采用的坐标系为地球固定坐标系（EFCS）、船体固定坐标系（BFCS）和质心坐标系（CMCS）.坐标系EFCS的x，y，z轴不随船旋转，其原点是船舶质心起点.坐标系BFCS的x_b，y_b，z_b轴与船体固定，轴x_b与龙骨重合，轴y_b垂直于船体对称平面，原点在船尾与龙骨的交点.坐标系CMCS的x_cg，y_cg，z_cg轴与船体固定，原点是船的质心，轴x_cg平行于龙骨.图3所示为船舶坐标系与惯性坐标系.

图中：u为纵荡速度，v为横荡速度，w为垂荡速度，\dot{\phi }为横摇角速度，\dot{\theta }为纵摇角速度，\dot{\psi }为艏摇角速度.x为地球固定坐标系中纵向坐标，y为横向坐标，z为垂向坐标，\phi为横摇角，\theta为纵摇角，\psi为艏摇角.地球固定坐标系与质心固定坐标系的转换关系如式（3）所示.

滑行艇6自由度动力学模型如式（3）所示：

\left.\begin{array}{l}m(\dot{u}+qw-rw)=X，\\ m(\dot{v}+ru-pw)=Y，\\ m(\dot{w}+pv-qu)=Z，\\ I_x\dot{p}+(I_z-I_y)qr=K，\\ I_y\dot{q}+(I_x-I_z)rp=M，\\ I_z\dot{r}+(I_y-I_x)pq=N.\end{array}\right\}(3)

其中：m为滑行艇质量；p为横摇角速度；q为纵摇角速度；r为艏摇角速度；\dot{p}，\dot{q}，\dot{r}分别为对应各个角速度对时间的导数；\dot{u}，\dot{v}，\dot{w}分别对应各个速度对时间的导数；I_x，I_y，I_z 分别为滑行艇绕x，y，z轴的转动惯量；X，Y，Z分别为滑行艇所受纵向、横向、垂向的合力；K，M，N分别为滑行艇所受横摇、纵摇、艏摇力矩.X，Y和Z如式（4）所示.

\left.\begin{array}{l}X=X_{\mathrm{i}\mathrm{m}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{t}}+X_{\mathrm{f}\mathrm{l}\mathrm{o}\mathrm{a}\mathrm{t}}+X_{\mathrm{g}\mathrm{r}\mathrm{a}\mathrm{v}\mathrm{i}\mathrm{t}\mathrm{y}}+X_{\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{c}\mathrm{o}\mathrm{u}\mathrm{s}}+X_P+X_R，\\ Y=Y_{\mathrm{i}\mathrm{m}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{t}}+Y_{\mathrm{f}\mathrm{l}\mathrm{o}\mathrm{a}\mathrm{t}}+Y_{\mathrm{g}\mathrm{r}\mathrm{a}\mathrm{v}\mathrm{i}\mathrm{t}\mathrm{y}}+Y_{\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{c}\mathrm{o}\mathrm{u}\mathrm{s}}+Y_P+Y_R，\\ Z=Z_{\mathrm{i}\mathrm{m}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{t}}+Z_{\mathrm{f}\mathrm{l}\mathrm{o}\mathrm{a}\mathrm{t}}+Z_{\mathrm{g}\mathrm{r}\mathrm{a}\mathrm{v}\mathrm{i}\mathrm{t}\mathrm{y}}+Z_{\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{c}\mathrm{o}\mathrm{u}\mathrm{s}}+Z_P+Z_R，\\ K=K_{\mathrm{i}\mathrm{m}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{t}}+K_{\mathrm{f}\mathrm{l}\mathrm{o}\mathrm{a}\mathrm{t}}+K_{\mathrm{g}\mathrm{r}\mathrm{a}\mathrm{v}\mathrm{i}\mathrm{t}\mathrm{y}}+K_{\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{c}\mathrm{o}\mathrm{u}\mathrm{s}}+K_P+K_R，\\ M=M_{\mathrm{i}\mathrm{m}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{t}}+M_{\mathrm{f}\mathrm{l}\mathrm{o}\mathrm{a}\mathrm{t}}+M_{\mathrm{g}\mathrm{r}\mathrm{a}\mathrm{v}\mathrm{i}\mathrm{t}\mathrm{y}}+M_{\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{c}\mathrm{o}\mathrm{u}\mathrm{s}}+M_P+M_R，\\ N=N_{\mathrm{i}\mathrm{m}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{t}}+N_{\mathrm{f}\mathrm{l}\mathrm{o}\mathrm{a}\mathrm{t}}+N_{\mathrm{g}\mathrm{r}\mathrm{a}\mathrm{v}\mathrm{i}\mathrm{t}\mathrm{y}}+N_{\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{c}\mathrm{o}\mathrm{u}\mathrm{s}}+N_P+N_R.\end{array}\right\}（4）

其中：下角标\mathrm{i}\mathrm{m}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{t}代表来自水的动压力；\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{c}\mathrm{o}\mathrm{u}\mathrm{s}代表水的黏性阻力；\mathrm{f}\mathrm{l}\mathrm{o}\mathrm{a}\mathrm{t}代表静浮力；\mathrm{g}\mathrm{r}\mathrm{a}\mathrm{v}\mathrm{i}\mathrm{t}\mathrm{y}代表重力；P与R分别表示螺旋桨力与舵力.

使用2D+t理论计算动压力，得到平底空气动力滑行艇的质心处总冲击力计算如式（5）所示：

\left.\begin{array}{l}{X}_{\mathrm{i}\mathrm{m}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{t}}={\int }_{\mathrm{0}}^{L_{\mathrm{k}}}{U}_{\mathrm{s}}{f}_x\mathrm{d}x，\\ Y_{\mathrm{i}\mathrm{m}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{t}}={\int }_{\mathrm{0}}^{L_{\mathrm{k}}}{U}_{\mathrm{s}}{f}_y\mathrm{d}x，\\ Z_{\mathrm{i}\mathrm{m}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{t}}={\int }_{\mathrm{0}}^{L_{\mathrm{k}}}{U}_{\mathrm{s}}{f}_z\mathrm{d}x，\\ K_{\mathrm{i}\mathrm{m}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{t}}={\int }_{\mathrm{0}}^{L_{\mathrm{k}}}{U}_{\mathrm{s}}{m}_{x\mathrm{c}\mathrm{g}}\mathrm{d}x，\\ M_{\mathrm{i}\mathrm{m}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{t}}=-{\int }_{\mathrm{0}}^{L_{\mathrm{k}}}{U}_{\mathrm{s}}(x_{\mathrm{b}}-X_{\mathrm{c}\mathrm{g}})f_z\mathrm{d}x，\\ N_{\mathrm{i}\mathrm{m}\mathrm{p}\mathrm{a}\mathrm{c}\mathrm{t}}={\int }_{\mathrm{0}}^{L_{\mathrm{k}}}{U}_{\mathrm{s}}(x_{\mathrm{b}}-X_{\mathrm{c}\mathrm{g}})f_y\mathrm{d}x.\end{array}\right\}(5)

其中：f_x为滑行艇截面在纵向受到的水冲击力；f_y 为截面在横向受到的水冲击力；f_z为截面在垂向受到的水冲击力；m_xcg为截面在纵向受到的水冲击力矩.X_cg为质心在船体固定坐标系中的水平坐标；x_{\mathrm{b}}为截面在船体固定坐标系中的纵向坐标；L_{\mathrm{k}}为滑行艇的船长；U_{\mathrm{s}}为滑行系数，其与滑行艇航行的Froude数有关，体现了滑行艇从航行初始的不稳定的排水、半排水状态，到稳定滑行状态的转变过程.

平底空气动力滑行艇质心处所受的静浮力与力矩计算如式（6）所示：

\left.\begin{array}{l}{X}_{\mathrm{f}\mathrm{l}\mathrm{o}\mathrm{a}\mathrm{t}}=\mathrm{s}\mathrm{i}\mathrm{n}\theta {\int }_{\mathrm{0}}^{L_{\mathrm{k}}}{f}_{z,\mathrm{f}}\mathrm{d}x，\\ Y_{\mathrm{f}\mathrm{l}\mathrm{o}\mathrm{a}\mathrm{t}}=-\mathrm{c}\mathrm{o}\mathrm{s}\theta \mathrm{s}\mathrm{i}\mathrm{n}\phi {\int }_{\mathrm{0}}^{L_{\mathrm{k}}}{f}_{z,\mathrm{f}}\mathrm{d}x，\\ Z_{\mathrm{f}\mathrm{l}\mathrm{o}\mathrm{a}\mathrm{t}}=-\mathrm{c}\mathrm{o}\mathrm{s}\theta \mathrm{c}\mathrm{o}\mathrm{s}\phi {\int }_{\mathrm{0}}^{L_{\mathrm{k}}}{f}_{z,\mathrm{f}}\mathrm{d}x，\\ K_{\mathrm{f}\mathrm{l}\mathrm{o}\mathrm{a}\mathrm{t}}=-{\int }_{\mathrm{0}}^{L_{\mathrm{k}}}{m}_{\mathrm{g},\mathrm{f}}\mathrm{d}x，\\ M_{\mathrm{f}\mathrm{l}\mathrm{o}\mathrm{a}\mathrm{t}}={\int }_{\mathrm{0}}^{L_{\mathrm{k}}}{f}_{z,\mathrm{f}}(x_{\mathrm{b}}-X_{\mathrm{c}\mathrm{g}})\mathrm{d}x，\\ N_{\mathrm{f}\mathrm{l}\mathrm{o}\mathrm{a}\mathrm{t}}=\mathrm{0}.\end{array}\right\}(6)

其中：m_g，f是截面内的浮力；f_z，f是截面内的浮力矩.

滑行艇质心处所受总的重力与力矩计算如式（7）所示：

\left.\begin{array}{l}{X}_{\mathrm{g}\mathrm{r}\mathrm{a}\mathrm{v}\mathrm{i}\mathrm{t}\mathrm{y}}=-mg\mathrm{s}\mathrm{i}\mathrm{n}\theta ,\\ Y_{\mathrm{g}\mathrm{r}\mathrm{a}\mathrm{v}\mathrm{i}\mathrm{t}\mathrm{y}}=-mg\mathrm{c}\mathrm{o}\mathrm{s}\theta \mathrm{s}\mathrm{i}\mathrm{n}\phi ,\\ Z_{\mathrm{g}\mathrm{r}\mathrm{a}\mathrm{v}\mathrm{i}\mathrm{t}\mathrm{y}}=-mg\mathrm{c}\mathrm{o}\mathrm{s}\theta \mathrm{c}\mathrm{o}\mathrm{s}\phi ,\\ K_{\mathrm{g}\mathrm{r}\mathrm{a}\mathrm{v}\mathrm{i}\mathrm{t}\mathrm{y}}=\mathrm{0},\\ M_{\mathrm{g}\mathrm{r}\mathrm{a}\mathrm{v}\mathrm{i}\mathrm{t}\mathrm{y}}=\mathrm{0},\\ N_{\mathrm{g}\mathrm{r}\mathrm{a}\mathrm{v}\mathrm{i}\mathrm{t}\mathrm{y}}=\mathrm{0}.\end{array}\right\}(7)

使用半经验半理论的回归公式法来获取滑行艇的黏性水动力导数.纵向、横向和艏摇黏性水动力与船舶的水平运动速度有关.船的水平面运动分为常速域小漂角运动和低速域大漂角运动.滑行艇一般工作在常速域小漂角的情况下，采用Kijima等^［17］提出的模型，得到垂荡、横摇和纵摇的黏性水动力表达式如（8）所示：

\left.\begin{array}{l}{Z}_{\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{c}\mathrm{o}\mathrm{u}\mathrm{s}}=-Z_{w}w-Z_{q}q，\\ K_{\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{c}\mathrm{o}\mathrm{u}\mathrm{s}}=Y_{\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{c}\mathrm{o}\mathrm{u}\mathrm{s}}{z}_{\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{c}\mathrm{o}\mathrm{u}\mathrm{s}}+\mathrm{2}{K}_{\mathrm{p}}p+K_{|p|p}|p|p，\\ M_{\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{c}\mathrm{o}\mathrm{u}\mathrm{s}}=-M_{w}w-M_{q}q.\end{array}\right\}(8)

其中：z_viscous为船舶横向黏性水动力等效作用点与船舶质心的垂向距离；Kₚ是横摇运动的线性黏性阻尼系数；Z_w是垂向力对垂荡速度w的一阶线性水动力导数，也称垂荡线性阻尼系数；Z_q 是垂向力对纵摇角速度q的一阶线性水动力导数；K_p 是横摇力矩对横摇角速度p的一阶线性水动力导数，即线性横摇阻尼系数；下标|p|p表示船舶横摇运动的非线性黏性平方阻尼项；K_|p|p 是非线性横摇阻尼系数；M_w 是纵摇力矩对垂荡速度w的一阶线性水动力导数；M_q 是纵摇线性阻尼系数.

通过实验测试螺旋桨转速与舵角对船舶受力的关系.实验利用拉压力传感器测量缆绳拉力变化，在静水实验池中固定船体，分别记录不同转速和舵角下的静、动态力数据，如图4所示.

在对舵板力和力矩进行实验分析的基础上，得出了舵板纵向力与螺旋桨转速和舵角之间的关系，以及横向力与螺旋桨转速和舵角之间的关系.图5为纵向力实验数据曲面，图6为横向力实验数据曲面.

通过拟合横向力与纵向力，得到纵向与横向上的桨舵力与螺旋桨转速n和舵板转角\delta之间的经验公式（9）：

\left.\begin{array}{l}{R}_y=f_{Ry}\left(n,\delta \right)=c\cdot \left(a\cdot n^{\mathrm{3}}+b\cdot n\right)\cdot \mathrm{s}\mathrm{i}\mathrm{n}\delta \mathrm{c}\mathrm{o}\mathrm{s}\delta ,\\ R_x=f_{Rx}\left(n,\delta \right)=d\cdot \left(a\cdot n^{\mathrm{3}}+b\cdot n\right)\cdot \mathrm{c}\mathrm{o}\mathrm{s}\delta \mathrm{c}\mathrm{o}\mathrm{s}\delta .\end{array}\right\}(9)

其中：a，b，c，d均为针对特定船-桨-舵系统的试验拟合经验系数；R_x 为船体受到的纵荡阻力；R_y 为船体受到的横荡阻力；f_Rx，f_Ry 为船体受到的纵向摩擦阻力与横向摩擦阻力；n是螺旋桨转速；\delta是舵板转角，向右取正.

设(x_P,y_P,z_P)为螺旋桨在质心坐标系中的坐标，力作用线平行于船舶甲板，位于滑行艇的对称平面.

滑行艇的螺旋桨推力和扭矩可以通过式（10）获得：

X_P=R_x;Y_P=\mathrm{0};Z_P=\mathrm{0};K_P=\mathrm{0};M_P=-R_x{z}_P;N_P=\mathrm{0}.(10)

其中，z_P 为螺旋桨等效作用点在质心坐标系中的垂向坐标.

设(x_{\mathrm{r}},y_{\mathrm{r}},z_{\mathrm{r}})为舵板等效力点在质心坐标系中的坐标，力平行于船舶甲板，穿过舵板等效力点.作用在规划船上的舵板力和力矩可以通过公式（11）获得：

\begin{array}{l}{X}_R=\mathrm{0};Y_R=R_y;Z_R=\mathrm{0};K_R=-R_y{z}_{\mathrm{r}};\\ M_R=R_x{z}_{\mathrm{r}};N_R=R_y{x}_{\mathrm{r}}.\end{array}(11)

其中：z_r为舵板等效力点在质心坐标系中的垂向坐标；x_r为舵板等效力点在质心坐标系中的纵向坐标.

3 动态窗口模型预测控制算法

3.1 模型预测控制算法

模型预测控制（model predictive control，MPC）是工业过程控制中一种有效的过程控制算法.其基本组成要素可以概括为预测模型、滚动优化和反馈校正.图7为MPC原理图，其中：k表示当前采样时刻；h表示预测时域的长度.

3.2 动态窗口模型预测控制算法

对于空气动力滑行艇，螺旋桨和方向舵产生推力和转向力矩.对于滑行艇的运动控制，速度控制和轨迹控制是耦合的.滑行艇转弯的3个自由度速度是：纵荡速度u，艏摇角速度r和横荡速度v.船舶在操舵过程中，仅使用纵荡速度和艏摇角速度无法明确预测船舶的未来轨迹，也无法将横荡速度设定为速度控制目标.因此，可以将模型预测控制中预测模型引入，并进行动力学模型和运动学模型的联合预测.在此背景下，本文提出了一种用于空气动力滑行艇的动态窗口模型预测控制（dynamic window approach_model predictive control，DWA_MPC）算法.

图8所示为DWA_MPC算法流程.将螺旋桨转速Prop和舵角Rudd设置为一组状态（Prop，Rudd），并将执行器限制和增量限制组合起来以设置动态窗口.在该空间中对多组执行器状态进行采样，并使用预测模型预测特定时间段内的运动轨迹.然后，通过目标点距离和障碍物距离等评估方法计算这些轨迹的成本函数，并选择与最佳轨迹对应的执行器状态作为控制变量，以驱动船舶航行.

螺旋桨转速分辨率取10 r/min，舵角分辨率为0.1°.将给定采样时间间隔内的控制量和控制量增量作为限制量，并按照上述分辨率进行离散化处理.在离散控制增量的条件下，使用龙格-库塔法离散模型进行状态预测.计算损失函数，并将损失函数的最小控制增量选为最优控制量.

代价函数如式（12）所示：

\begin{array}{c}G\left(\mathrm{P}\mathrm{r}\mathrm{o}\mathrm{p},\mathrm{R}\mathrm{u}\mathrm{d}\mathrm{d}\right)=\alpha \cdot \mathrm{H}\mathrm{e}\mathrm{a}\mathrm{d}\mathrm{i}\mathrm{n}\mathrm{g}\left(\mathrm{P}\mathrm{r}\mathrm{o}\mathrm{p},\mathrm{R}\mathrm{u}\mathrm{d}\mathrm{d}\right)+\\ \beta \cdot \mathrm{D}\mathrm{i}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}\left(\mathrm{P}\mathrm{r}\mathrm{o}\mathrm{p},\mathrm{R}\mathrm{u}\mathrm{d}\mathrm{d}\right)+\gamma \cdot \mathrm{V}\mathrm{e}\mathrm{l}\mathrm{o}\mathrm{c}\mathrm{i}\mathrm{t}\mathrm{y}\left(\mathrm{P}\mathrm{r}\mathrm{o}\mathrm{p},\mathrm{R}\mathrm{u}\mathrm{d}\mathrm{d}\right).\end{array}(12)

其中：Heading（Prop，Rudd）为航向角代价函数；Distance（Prop，Rudd）为障碍物代价函数；Velocity（Prop，Rudd）为速度代价函数；\alpha ,\beta ,\gamma分别为航向角代价函数、障碍物代价函数、速度代价函数的权重系数.

3.3 动态窗口模型预测控制算法仿真与分析

为了验证算法的有效性，通过3种工况下的局部路径规划与路径跟踪避障仿真进行分析.分别为路径跟踪工况、路径跟踪时动态障碍物避障工况和路径跟踪时动态与静态障碍物避障工况.

1) 路径跟踪.参考路径公式如式（13）所示：

\left.\begin{array}{l}{X}_{\mathrm{r}\mathrm{e}\mathrm{f}}=\mathrm{5}\times \left(\mathrm{1}-\mathrm{c}\mathrm{o}\mathrm{s}(\mathrm{2}\mathrm{\pi }s)\right)，\\ Y_{\mathrm{r}\mathrm{e}\mathrm{f}}=\mathrm{30}\times \mathrm{2}\mathrm{\pi }s.\end{array}\right\}(13)

其中：s为从0到1的参数方程中的参数；X_ref与Y_ref分别为X轴方向与Y轴方向的参考路径坐标值.

图9为DWA_MPC路径跟踪仿真结果.图9a为航迹图，图9b为曲线路径跟踪误差，图9c为仿真中舵角控制指令曲线，图9d为仿真中滑行艇的艏摇角速度曲线.由图9b可知，DWA_MPC曲线路径跟踪误差小于0.026 m，优于传统动态窗口算法.由图9d可以看出，滑行艇艏摇角速度小于0.9°/s，优于传统动态窗口算法.

2) 路径跟踪时动态障碍物避障.设置动态障碍物阻挡参考路径，研究DWA_MPC算法在路径跟踪时的动态避障能力.参考路径公式如式（13）所示.

图10为路径跟踪时动态障碍物避障工况下的仿真结果.图10a中动态障碍物运动特性为从位置（0，40）到位置（50，50）做匀速直线运动，持续时间为200 s，平均速度为0.17 m/s.障碍物完全脱离对跟随路径的遮挡位置在（15.5，43.1），对应时间为62 s.图11中横向误差与图9为同一量级，是由于DWA_MPC算法没有采用横向绕行通过，而是选择减速等待通过，从图10b中可以看出在37~60 s之间螺旋桨转速降低至1 000 r/min，航速也从0.90 m/s降至0.55 m/s.螺旋桨转速也属于该算法模型中需要计算的一部分，算法具体会作出哪种避碰决策，取决于在算法中设置的损失函数的各项权重参数的调整.

图11为路径跟踪与动态障碍物的横向跟踪误差.由图10c和图11可以看出，在DWA_MPC算法控制下，滑行艇的艏摇角速度与航行轨迹的曲线路径跟踪误差均未发散.

图12显示了滑行艇在不同时刻的关键位置.图12中细线簇为DWA_MPC算法计算的预测触须簇.实心圆圈表示移动与静态障碍物.空心圆圈标记线为参考路径.粗黑实线表示滑行艇的实际轨迹，其变化端点表示滑行艇在每一时刻的位置.细线簇与加粗实线之间的部分表示通过DWA_MPC算法从预测触须簇中选择的最佳轨迹，其对应的执行器状态是此时的最佳控制量.由图12a~图12i可知，移动的障碍物与滑行艇之间没有发生碰撞，滑行艇从动态障碍物移动轨迹的后方避开障碍物.避开动态障碍物之后，完成路径跟踪控制任务到达参考路径终点.

3) 路径跟踪时动态与静态障碍物避障.设置动态障碍物阻挡参考路径，并且有静态障碍物位于参考路径上.研究DWA_MPC算法在路径跟踪时的动态避障能力.

参考路径公式如式（14）所示：

\left.\begin{array}{l}{X}_{\mathrm{r}\mathrm{e}\mathrm{f}}=\mathrm{100}\cdot \mathrm{s}\mathrm{i}\mathrm{n}(\mathrm{0.5}\cdot \mathrm{\pi }\cdot s)，\\ Y_{\mathrm{r}\mathrm{e}\mathrm{f}}=\mathrm{50}\cdot \left(\mathrm{1}-\mathrm{c}\mathrm{o}\mathrm{s}(\mathrm{0.5}\cdot \mathrm{\pi }\cdot s)\right).\end{array}\right\}(14)

静态障碍物位置为（60，10），半径为4 m.动态障碍物位置从（40，0）匀速运动到（50，50），半径为10 m.

图13为路径跟踪时动态与静态障碍物避障工况下的仿真结果.图13a显示了航迹图，其中粗黑实线表示实际轨迹，实心圆圈表示不同时刻障碍物的位置，其余细线表示DWA_MPC算法每次获得的最佳轨迹预测.图13b显示了模拟中的航速和螺旋桨转速，虚线表示螺旋桨转速，实线表示速度.图13c显示了模拟中的艏摇角速度和舵角，虚线表示舵角，实线表示艏摇角速度.

图14显示了滑行艇在不同时刻的关键位置，图14中细线簇为DWA_MPC算法计算的预测触须簇.实心圆圈表示移动与静态障碍物.空心圆圈标记的线为参考路径.粗黑实线表示滑行艇的实际轨迹.黑色实线的变化端点表示滑行艇在每一时刻的位置.细线簇与加粗实线之间的部分表示通过DWA_MPC算法从预测触须簇中选择的最佳轨迹，其对应的执行器状态是此时的最佳控制量.由图14a~图14h可知，移动的障碍物和滑行艇之间没有发生碰撞，滑行艇从动态障碍物移动轨迹的后方避开障碍物，并从左侧避开静态障碍物.避开所有障碍物之后，滑行艇完成路径跟踪控制任务，到达参考路径终点.

4 结 论

本文结合滑行艇运动控制特点，提出了一种动态窗口模型预测控制算法.在路径跟踪、路径跟踪时动态障碍物避障、路径跟踪时动态与静态障碍物避障3种工况下，使用提出的动态窗口模型预测控制算法对空气动力滑行艇进行控制仿真分析.仿真实验结果证明了该算法相较传统的动态窗口算法路径跟踪横向误差与艏摇角速度峰值更小，并且能够对静态与动态障碍物进行避障.通过路径跟踪仿真，改进后的动态窗口模型预测控制在路径跟踪时横向跟踪误差更小，证明了本文提出的基于动态窗口模型预测控制算法的空气动力滑行艇路径规划与路径跟踪控制算法的有效性.

参考文献

[1] 俞建成， 孙朝阳， 张艾群. 海洋机器人环境能源收集利用技术现状［J］. 机器人， 2018， 40（1）： 89-101.

[2] Yu Jian-cheng， Sun Chao-yang， Zhang Ai-qun. The present status of environmental energy harvesting and utilization technology of marine robots［J］. Robot， 2018， 40（1）： 89-101.

[3] Kim H G， Yun S J， Choi Y H， et al. Improved dynamic window approach with path-following for unmanned surface vehicle［J］. Journal of Embedded Systems and Applications， 2017， 12（5）： 295-301.

[4] Kim H G， Yun S J， Choi Y H， et al. Collision avoidance algorithm based on COLREGs for unmanned surface vehicle［J］. Journal of Marine Science and Engineering， 2021， 9（8）： 863.

[5] Liu Y C， Bucknall R. Path planning algorithm for unmanned surface vehicle formations in a practical maritime environment［J］. Ocean Engineering， 2015， 97： 126-144.

[6] Yu K， Liang X F， Li M Z， et al. USV path planning method with velocity variation and global optimisation based on AIS service platform［J］. Ocean Engineering， 2021， 236： 109560.

[7] Liu X Y， Li Y， Zhang J， et al. Self-adaptive dynamic obstacle avoidance and path planning for USV under complex maritime environment［J］. IEEE Access， 2019， 7： 114945-114954.

[8] Chen Y L， Bai G Q， Zhan Y， et al. Path planning and obstacle avoiding of the USV based on improved ACO-APF hybrid algorithm with adaptive early-warning［J］. IEEE Access， 2021， 9： 40728-40742.

[9] MahmoudZadeh S， Abbasi A， Yazdani A， et al. Uninterrupted path planning system for multi-USV sampling mission in a cluttered ocean environment［J］. Ocean Engineering， 2022， 254： 111328.

[10] Zhang J， Zhang F， Liu Z Y， et al. Efficient path planning method of USV for intelligent target search［J］. Journal of Geovisualization and Spatial Analysis， 2019， 3（2）： 13.

[11] Wei G， Xiao C S， Zhu M， et al. A cruise USV system design and path planning test［C］//2022 International Conference on High Performance Big Data and Intelligent Systems （HDIS）. Tianjin， 2022： 271-276.

[12] 谭智坤， 张隆辉， 刘正锋， 等. 融合改进动态窗口法与速度障碍法的无人船局部路径规划［J］. 船舶力学， 2023， 27（3）： 311-322.

[13] Tan Zhi-kun， Zhang Long-hui， Liu Zheng-feng， et al. Local path planning for USVs based on the fusion algorithm of improved dynamic window approach and velocity obstacle algorithm［J］. Journal of Ship Mechanics， 2023， 27（3）： 311-322.

[14] 何志强， 专祥涛， 梁杰， 等. 基于区域人工势场的无人船目标跟踪策略研究［J］. 中国造船， 2023， 64（1）： 236-245.

[15] He Zhi-qiang， Xiang-tao Zhuan， Liang Jie， et al. Research on target tracking strategy of unmanned surface vessel based on regional artificial potential field［J］. Shipbuilding of China， 2023， 64（1）： 236-245.

[16] 张扬， 彭鹏菲， 曹杰. 基于改进APF算法的水面无人艇局部路径规划［J］. 兵器装备工程学报， 2023， 44（9）： 42-48.

[17] Zhang Yang， Peng Peng-fei， Cao Jie. Local path planning for USV based on improved APF algorithm［J］. Journal of Ordnance Equipment Engineering， 2023， 44（9）： 42-48.

[18] 张宗鑫， 杨春曦， 杨健全， 等. 含未知干扰的非对称欠驱动无人艇轨迹跟踪［J］. 控制工程， 2023， 30（12）： 2185-2191.

[19] Zhang Zong-xin， Yang Chun-xi， Yang Jian-quan， et al. Asymmetric underactuated unmanned surface vehicle trajectory tracking with unknown disturbance［J］. Control Engineering of China， 2023， 30（12）： 2185-2191.

[20] 翁昱， 曾庆军， 李维， 等. 基于智能预测控制的鱼雷状小型无人艇轨迹跟踪研究［J］. 中国舰船研究， 2024， 19（1）： 158-168.

[21] Weng Yu， Zeng Qing-jun， Li Wei， et al. Trajectory tracking of small torpedo-type unmanned surface vessel based on intelligent predictive control［J］. Chinese Journal of Ship Research， 2024， 19（1）： 158-168.

[22] 顾潮宏， 冯友兵， 仲伟波， 等. 基于优化人工势场法的无人水面艇路径规划［J］. 舰船科学技术， 2025， 47（5）： 126-131.

[23] Gu Chao-hong， Feng You-bing， Zhong Wei-bo， et al. Path planning of unmanned surface vehicles based on optimized artificial potential field method［J］. Ship Science and Technology， 2025， 47（5）： 126-131.

[24] Kijima K， Nakiri Y. Approximate expression for hydrodynamic derivatives of ship manoeuvring motion taking into account of the effect of stern shape［J］. Transactions of the West-Japan Society of Naval Architects， 1999（98）： 67-77.