0 引　言

在计划航线上航行并对于航行时遇到的航行障碍物采取合理的避让措施是水面无人艇（unmanned surface vehicle，USV）的基本功能之一。海上避障避碰是实现USV自主航行的关键问题。

针对USV自主航行过程中的避碰问题国内外专家开展大量的研究，形成了以速度障碍法^[1-2]（velocity obstacles，VO）、人工势场法^[3-4]（artificial potential field，APF）、遗传算法^[5-6]（genetic algorithm，GA）、动态窗口法^[7-8]（dynamic window，DW）和模型预测控制^[9-12]（model predictive control，MPC）等为主的智能避碰决策算法。其中遗传算法、速度障碍法和人工势场法在进行航路规划时通常不考虑船舶的动力学特性，导致求解得到的避碰决策方案与实际船舶运动存在较大的差距；动态窗口法虽然能够提供多种避让方案，但存在不适用于动态环境的缺点。相比于遗传算法、速度障碍法、人工势场法和动态窗口法，MPC从船舶运动模型着手，充分考虑船舶运动控制中的软硬性约束条件，将避碰决策行为与控制策略结合起来，因此能够较好满足USV的实际自主航行设计要求。

在使用MPC算法解决船舶智能避碰和自主航行问题的研究中，Johansen等^[9]通过离散船舶航向偏转控制序列的方式，结合MPC算法并考虑部分《国际海上避碰规则》（international regulations for preventing collisions at sea，COLREGs）实现了ASV的自主避碰决策。Eriksen等^[11-12]利用MPC与非线性规划模型结合，将动态船舶、静态障碍物和ASV的航速大小作为模型约束实现了ASV在期望航迹上的自主航行。现有研究中均没有考虑船舶安全领域和风流对ASV在自主航行过程中的影响。

为了解决需要同时满足USV自身动力特性约束、风流影响、静态障碍物和动态障碍物的安全领域约束和避碰规则约束的USV自主航行问题，本文提出一种基于MPC的USV自主航行模型。根据存在外界环境干扰的船舶动力学模型构造MPC的预测模型，使得算法的解更符合USV的动力学特性；通过USV自主航行过程中对于期望航迹的航迹偏移量、对于期望速度大小的航速变化量、碰撞危险度以及对于避碰规则中的Rule-14和Rule-15符合程度构造MPC的评价函数；考虑自主航行过程中所遇到的航行障碍物（主要包括静态障碍物和动态障碍物）的安全领域和USV执行机构的动力输出构造该模型的约束条件。为了验证算法的可行性，本文设定期望航迹和航行过程中的期望速度，并随机设置静态障碍物和动态障碍物展开算法准确性验证。仿真结果表明，该模型能够实现复杂水域内USV在设定的期望航迹上自主航行，并在能够对所遭遇的一些静态障碍物或动态障碍物进行自主合理避碰。

1 USV动力学模型

本文研究对象为USV，按照常规水面舰船的设计要求，在动力学模型上进行一定简化处理，即只考虑船舶纵荡、横荡和首摇3个自由度上的动力学响应。参考Fossen^[13]提出的船舶状态空间模型，USV的动力学模型如下式：

$ \left\{ {\begin{aligned} &{\dot \eta =R(\psi )\upsilon }，\\ &{M\dot \upsilon + C(\upsilon )\upsilon + D(\upsilon )\upsilon = \tau + \omega } 。\end{aligned}} \right. $

(1)

其中： $ \eta = {[x,y,\psi ]^{\rm T}} $ 为USV在大地坐标系上的位置和航向； $ \upsilon = {[u,v,r]^{\rm T}} $ 为USV在附体坐标系上的线速度和转首速度。坐标系如图1所示^[14]。

$ {\boldsymbol{R}}(\psi ) $ 为从附体坐标系到大地坐标系的转换矩阵，计算如下式：

$ {\boldsymbol{R}}(\psi ) = \left( {\begin{array}{*{20}{c}} {\cos (\psi )}&{ - \sin (\psi )}&0 \\ {\sin (\psi )}&{\cos (\psi )}&0 \\ 0&0&1 \end{array}} \right)，$

(2)

式中： $ M $ 为计算船舶刚体质量和附加质量的矩阵，计算式为

$ {\boldsymbol{M}}{\text{ = }}{{\boldsymbol{M}}_{{\text{RB}}}} + {{\boldsymbol{M}}_{\text{A}}}。$

(3)

式（3）中的 $ {M_{{{RB}}}} $ 为船舶刚体质量矩阵，计算如下式：

$ {{\boldsymbol{M}}_{{{RB}}}} = \left( {\begin{array}{*{20}{c}} m&0&0 \\ 0&m&0 \\ 0&0&{{I_z}} \end{array}} \right) 。$

(4)

式中： $ m $ 为船舶的质量； $ {I_z} $ 为船舶绕Z轴的转动惯量。

$ {{\boldsymbol{M}}_{{A}}} $ 为船舶附加质量矩阵，本文做了模型简化处理，令 $ {{\boldsymbol{M}}_{{A}}} = 0 $ 。

$ {\boldsymbol{C}}(\upsilon ) $ 是科里奥利力和向心力矩阵，计算如下式：

$ {\boldsymbol{C}}(\upsilon ){\text{ = }}C{(\upsilon )_{{{RB}}}} + C{(\upsilon )_{{A}}}，$

(5)

由于 $ {{\boldsymbol{M}}_{{A}}} = 0 $ 可得 $ C{(\upsilon )_{{A}}} = 0 $ 。 $ C{(\upsilon )_{{{RB}}}} $ 的计算如下式：

$ C{(\upsilon )_{{{RB}}}} = \left( {\begin{array}{*{20}{c}} 0&0&{ - mv} \\ 0&0&{mu} \\ {mv}&{ - mu}&0 \end{array}} \right)，$

(6)

$ D(\upsilon ) $ 表示系统的阻尼效应矩阵，计算如下式：

$ D(\upsilon )\upsilon = {D_{{L}}}\upsilon + {D_{NL}}(\upsilon )\upsilon。$

(7)

式中：

$ \left\{ {\begin{aligned} &{{{\boldsymbol{D}}_{{L}}} = \left( {\begin{array}{*{20}{c}} {{X_u}}&0&0 \\ 0&{{Y_v}}&{{Y_r}} \\ 0&{{N_v}}&{{N_r}} \end{array}} \right)} ，\\ &{{{\boldsymbol{D}}_{{{NL}}}}(\upsilon )\upsilon = - \left( {\begin{array}{*{20}{c}} {{X_{\left| u \right|u}}\left| u \right|u + {X_{uuu}}{u^3}} \\ {{Y_{\left| v \right|v}}\left| v \right|v + {Y_{vvv}}{v^3}} \\ {{N_{\left| r \right|r}}\left| r \right|r + {N_{vvv}}{v^3}} \end{array}} \right)} 。\end{aligned}} \right. $

(8)

$ \tau = {[{\tau _{{X}}},{\tau _{{Y}}},{\tau _{{N}}}]^{\rm T}} $ 分别表示船舶作用在X，Y轴上的推力和在Z轴上的转船力矩。

$ \tau = \left( {\begin{array}{*{20}{c}} {{\tau _{{X}}}} \\ {{\tau _{{Y}}}} \\ {{\tau _{{N}}}} \end{array}} \right) = \left( {\begin{array}{*{20}{c}} {{F_{{X}}}} \\ {{F_{{Y}}}} \\ {l{F_{{Y}}}} \end{array}} \right)。$

(9)

式中， $ l $ 为作用在 $ {F_{{Y}}} $ 的有效力臂。

$ \omega $ 为受外界环境因素影响的总和，为了简化模型，忽略波浪对船舶运动的影响，着重考虑风、流对USV运动的影响。风流对船舶的动力学响应建模如下式：

$ \begin{split} \omega =\;& {\tau _{{w}}} + {\tau _{{{cu}}}} ，\\ {\tau _{{w}}}=\;&\left[ {\begin{array}{*{20}{c}} {{X_{{W}}}} \\ {{Y_{{W}}}} \\ {{N_{{W}}}} \end{array}} \right] = \left[ \begin{aligned} &{\dfrac{1}{2}{\rho _{{W}}}{V_{{W}}}{C_{{{WX}}}}({\theta _{{W}}}){A_{{{FW}}}}} \\ & {\dfrac{1}{2}{\rho _{{W}}}{V_{{W}}}{C_{{{WY}}}}({\theta _{{W}}}){A_{{{LW}}}}} \\ & {\dfrac{1}{2}{\rho _{{W}}}{V_{{W}}}{C_{{{WN}}}}({\theta _{{W}}}){A_{{{LW}}}}L} \end{aligned} \right] ，\\ {\tau _{{{cu}}}}=\;&\left[ \begin{array}{*{20}{c}} {{X_{{{cu}}}}} \\ {{Y_{{{cu}}}}} \\ {{N_{{{cu}}}}} \end{array} \right] = \left[ \begin{aligned} {\dfrac{1}{2}{\rho _{{{cu}}}}{V_{{{cu}}}}{C_{{{cuX}}}}(\theta ){A_{{F}}}} \\ {\dfrac{1}{2}{\rho _{{{cu}}}}{V_{{{cu}}}}{C_{{{cuY}}}}(\theta ){A_{{L}}}} \\ {\dfrac{1}{2}{\rho _{{{cu}}}}{V_{{{cu}}}}{C_{{{cuN}}}}(\theta ){A_{{F}}}} \end{aligned} \right] 。\end{split} $

(10)

式中： $ {\tau _{{w}}} $ 为风载荷； $ {\tau _{{{cu}}}} $ 为流载荷； $\,{\rho _{{W}}}$ 为空气密度； $ {V_{{W}}} $ 为相对风速； $ {\theta _{{W}}} $ 为相对风向角； $ {A_{{{FW}}}} $ 为船舶水线以上正投影面积； $ {A_{{{LW}}}} $ 为船舶水线以上侧投影面积；L为船舶长度; $ {C_{{{WX}}}}({\theta _{{W}}}) $ 和 $ {C_{{{WY}}}}({\theta _{{W}}}) $ 分别为X，Y轴上的风压力系数； $ {C_{{{WN}}}}({\theta _{{W}}}) $ 是绕 $ {{Z}} $ 轴的风压力矩系数； $\,{\rho _{{{cu}}}}$ 为流体密度； $ {V_{{{cu}}}} $ 为流速； $ \theta $ 为流向角； $ {A_{{F}}} $ 为船舶水下部分正投影面积； $ {A_{{L}}} $ 为船舶水下部分侧投影面积； $ {C_{{{cuX}}}}(\theta ) $ 和 $ {C_{{{cuY}}}}(\theta ) $ 分别为X，Y轴上的流载荷系数； $ {C_{{{cuN}}}}(\theta ) $ 是绕Z轴的流载荷转矩系数^[15]。

2 算法基本原理

基于MPC的USV自主航行算法的基本原理包括预测模型、滚动优化和反馈校正^[16]。

1）预测模型是指在预测时域内根据USV动力学模型预测USV的运动状态；

2）滚动优化是指利用评价函数和相关约束选择最优的控制输入 $ \tau $ ；

3）反馈校正是指USV在控制时域内执行控制输入序列后输出新的运动状态，利用新的运动状态重新执行预测模型和滚动优化得到新的最优的控制输入。

算法基本流程如图2所示^[17]。

2.1 动态碍航物运动轨迹预测

USV在自主航行过程中遭遇的动态障碍物一般为动态船舶。针对动态船舶的未来轨迹进行准确预测非常困难。为了简化问题，假定目标船舶以当前时刻的运动状态在未来做一个直线运动^[18]。在 $ t $ 时刻目标船舶的位置预测方程如下：

$ \left\{ \begin{aligned} &\left( \begin{array}{*{20}{c}} {{x_{{T}}}(t)} \\ {{y_{{T}}}(t)} \end{array} \right) = \left( \begin{array}{*{20}{c}} {{x_{{T}}}({t_0})} \\ {{y_{{T}}}({t_0})} \end{array} \right) + \Delta tA\left( \begin{array}{*{20}{c}} {{u_{{T}}}({t_0})} \\ {{v_{{T}}}({t_0})} \end{array} \right) ，\\ & \Delta t =t - {t_0} ，\\ & A = \left( \begin{array}{*{20}{c}} {\cos ({\psi _{{T}}})}&{ - \sin ({\psi _{{T}}})} \\ {\sin ({\psi _{{T}}})}&{\cos ({\psi _{{T}}})} \end{array} \right) 。\end{aligned} \right. $

(11)

式中： ${[{x_{{T}}}(t), {y_{{T}}}(t)]^{\rm T}}$ 为目标船舶在 $ t $ 时刻的位置； $[{x_{{T}}}({t_0}), {y_{{T}}}({t_0})]^{\rm T}$ 为目标船舶在 $ {t_0} $ 时刻的位置； $ {\psi _{{T}}} $ 为目标船舶在 $ {t_0} $ 时刻的航向； $ {[{u_{{T}}}({t_0}),{v_{{T}}}({t_0})]^{\rm T}} $ 为目标船舶在 $ {t_0} $ 时刻的纵荡速度和横荡速度。

2.2 静态障碍物建模

将USV自主航行过程中遇到的静态障碍物进行简单处理，将障碍物所在的最小外接圆进行适当缩放，并将该圆所在的外界边界作为USV的安全边界^[19]，如图3所示。

静态碍航物模型如下：

$ \left\{ {\begin{aligned} &{{{\left( {{x_{\text{s}}} - {x_{\text{s}}}_0} \right)}^2} + {{\left( {{y_{\text{s}}} - {y_{\text{s}}}_0} \right)}^2} = {r_{\text{s}}}^2} ，\\ &{{r_{\text{s}}}=kr} 。\end{aligned}} \right. $

(12)

式中： $ \left( {{x_{\text{s}}}_0,{y_{\text{s}}}_0} \right) $ 为静态障碍物的位置坐标； $ {r_{\text{s}}} $ 为安全边界的半径； $ r $ 为障碍物外接圆的半径； $ k $ 为 $ r $ 的放大系数。

3 基于MPC的USV自主航行模型

本文设计的USV自主航行模型如下：

$ \begin{split} &{\min \;\;C(\tau ) = {K_{{p}}}{C_{{p}}}(\tau ) + {K_{{U}}}{C_{{U}}}(\tau ) + {K_{{{COL}}}}{C_{{{COL}}}}(\tau )}，\\ &{\rm{s.t}}\left\{ \begin{aligned} &{{h_s}(N(\tau ),{p_s},Np,{r_s}) \leqslant 0}，\\ &{{h_T}(N(\tau ),{p_T},Np,{r_T}) \leqslant 0}，\\ &{{h_\tau }(\tau ) \leqslant 0}，\\ & {{g_\tau }(\tau ) = 0} 。\end{aligned} \right. \end{split} $

模型中评价函数 $ C(\tau ) $ 主要由航迹偏移量 $ {C_{{p}}}(\tau ) $ 、航速变化量 $ {C_{{U}}}(\tau ) $ 和避碰规则的执行成本函数 $ {C_{{{COL}}}}(\tau ) $ 三部分组成。其中 $ {K_{{p}}} $ ， $ {K_{{U}}} $ 和 $ {K_{{{COL}}}} $ 分别为其权重参数。权重参数根据实际情况进行设定。

基于MPC的USV自主航行的约束条件主要由外部环境约束 $ {h_s}(N(\tau ),{p_s},Np,{r_s}) $ 和 $ {h_T}(N(\tau ),{p_T},Np,{r_T}) $ 与USV执行机构本身的约束 $ {h_\tau }(\tau ) $ 和 $ {g_\tau }(\tau ) $ 两大部分组成。其中约束条件 $ {h_s}(N(\tau ),{p_s},Np,{r_s}) \leqslant 0 $ 确保USV在自主航行过程中不会与静态障碍物发生碰撞； $ {h_T}(N(\tau ),{p_T},Np,{r_T}) \leqslant 0 $ 确保USV在自主航行过程中不会与动态障碍物发生碰撞； $ {h_\tau }(\tau ) \leqslant 0 $ 以及 $ {g_\tau }(\tau ) = 0 $ 确保模型计算得到的动力分配 $ \tau $ 是满足USV执行机构的内部机械特性约束。

3.1 航迹偏移量

航迹偏移量 $ {C_{\text{p}}}(\tau ) $ 主要是用于描述USV在预测时域中的预测轨迹与期望轨迹之间的偏差值。 $ {C_{\text{p}}}(\tau ) $ 越小说明USV在预测时域中的预测轨迹更接近期望轨迹。

将USV的期望航迹按一定距离间隔离散化为期望轨迹序列 $ p{d_{1:all}} = [p{d_1},p{d_2}, \cdots ,p{d_{all}}] $ 。其中：

$ p{d_q} = {[{x_q},{y_q}]^{\rm T}},\;\;q \in [1,all]，$

$ {t_0} $ 时刻USV在预测时域（ $ Np $ ）中的期望轨迹序列为：

$ p{d_{{t_0} + 1:Np}} = [p{d_{{t_0} + 1}},p{d_{{t_0} + 2}}, \cdots ,p{d_{{t_0} + Np}}]，$

以 $ \tau $ 为控制器输入后在预测时域中的预测轨迹序列为：

$ {p_{{t_0} + 1:Np}}(\tau ) = [{p_{{t_0} + 1}}(\tau ),{p_{{t_0} + 2}}(\tau ), \cdots ,{p_{{t_0} + Np}}(\tau )] ，$

其中：

$ {p_{{t_0} + k}}(\tau ) = \left( {\begin{array}{*{20}{c}} {{N_{{t_0} + k,1}}(\tau )} \\ {{N_{{t_0} + k,2}}(\tau )} \end{array}} \right) \text{，} k \in [1,Np]。$

$ {\boldsymbol{N}}(\tau ) $ 是一个 $ k \times 6 $ 的矩阵，表示的是预测时域内USV的运动状态空间矩阵。

USV在预测时域中的航迹偏移量计算如下式：

$ {C_{\text{p}}}(\tau ) = \sum\limits_{k = 1}^{Np} {\dfrac{{\left\| {{p_{{t_0} + k}}(\tau ) - p{d_{{t_0} + k}}} \right\|_2^2}}{{\left\| {p{d_{{t_0}}} - p{d_{{t_0} + k}}} \right\|_2^2}}} 。$

(13)

3.2 航速变化量

航速变化量 $ {C_{\text{U}}}(\tau ) $ 主要是USV在自主航行过程中航速大小与期望航速大小之间的偏差值。 $ {C_{\text{U}}}(\tau ) $ 越小说明USV在自主航行过程中航速大小越接近期望航速。

USV在期望航迹上行驶时，如果设定期望航速大小为 $ {U_{\text{d}}} $ ，则USV在预测时域中的航速变化量 $ {C_{\text{U}}}(\tau ) $ 计算如下式:

$ {C_{\text{U}}}(\tau ) = \sum\limits_{k = 1}^{Np} {\dfrac{{\left| {{U_{{t_0} + k}}(\tau ) - {U_{\text{d}}}} \right|}}{{{U_{\text{d}}}}}} 。$

(14)

式中：

$ \begin{split} {U_{{t_0} + k}}(\tau ) =\;& \sqrt {u_{{t_0} + k}^2(\tau ) + v_{{t_0} + k}^2(\tau )} ，\\ {u_{{t_0} + k}}(\tau ) =\;& {N_{{t_0} + k,4}}(\tau ) ，\\ v_{{t_0} + k}^{}(\tau ) = \;& {N_{{t_0} + k,5}}(\tau ) 。\end{split} $

3.3 避碰规则的成本函数

本文设计的自主航行USV遵守COLREGs中第7、第8、第14与第15条规则。设计违反规则的成本函数如下式：

$ {C_{{\text{COL}}}}(\tau ) = - \sum\limits_{k = 1}^{Np} {\mu R\dfrac{{V{p_k}(\tau )}}{{\left\| {{p_{{t_0} + k}} - {p_{{t_0}}}} \right\|_2^{}}}}，$

(15)

式中： $\,\mu$ 是一个布尔变量，当两船形成对遇或右交叉局面时 $\,\mu = 1$ ，此时本船为让路船舶并向右转向避让他船，否则 $\,\mu = 0$ ； $ R $ 是船舶碰撞危险度，采用最近会遇距离（DCPA）与最近会遇时间（TCPA）相结合的方式来计算船舶间的碰撞危险度。计算模型如下：

$ R = {\alpha _1}{R_{{{DCPA}}}} + {\alpha _2}{R_{{{TCPA}}}} 。$

(16)

式中： $ {R_{{\text{DCPA}}}} $ 为DCPA的隶属度函数； $ {R_{{{TCPA}}}} $ 为TCPA的隶属度函数； $ {\alpha _1} $ 和 $ {\alpha _2} $ 为 $ {R_{{{DCPA}}}} $ 和 $ {R_{{{TCPA}}}} $ 的权重。

$ {R_{{{DCPA}}}} $ 和 $ {R_{{{TCPA}}}} $ 的计算模型如下：

$ \begin{split} &{{R_{{{DCPA}}}} = \left\{ {\begin{array}{*{20}{c}} 1&{0 \leqslant \left| D \right| < {d_1}} ，\\ {{{\left( {\dfrac{{{d_2} - D}}{{{d_2} - {d_1}}}} \right)}^2}}，&{{d_1} \leqslant \left| D \right| < {d_2}}，\\ 0，&{\left| D \right| > {d_2}}，\end{array}} \right.} \\ &{{R_{{{TCPA}}}} = \left\{ {\begin{array}{*{20}{c}} 1&{0 \leqslant \left| T \right| < {t_1}}，\\ {{{\left( {\dfrac{{{t_2} - T}}{{{t_2} - {t_1}}}} \right)}^2}}，&{{t_1} \leqslant \left| T \right| < {t_2}}，\\ 0，&{\left| T \right| > {t_2}} ，\end{array}} \right.} \\ &{{t_1} = \left\{ {\begin{array}{*{20}{c}} {\dfrac{{\sqrt {d_{_1}^2 - {D^2}} }}{V}}，&{D \leqslant {d_1}} ，\\ 0，&{D > {d_1}} ，\end{array}} \right.} \\ &{{t_2} = \left\{ {\begin{array}{*{20}{c}} {\dfrac{{\sqrt {d_{_2}^2 - {D^2}} }}{V}}，&{D \leqslant {d_2}} ，\\ 0，&{D > {d_2}}。\end{array}} \right.} \end{split} $

(17)

式中： $ D $ 为本船与目标船之间的DCPA； $ T $ 为本船与目标船之间的TCPA； $ {d_1} $ 为船舶的安全会遇距离； $ {d_2} $ 为船舶的绝对安全会遇距离； $ V $ 为目标船的相对运动速度。

$ V{p_k}(\tau ) $ 是指USV在 $ {t_0} + k $ 时刻的位置在 $ {t_0} $ 时刻的附体坐标系上的投影。其含义如图4 所示。

当USV往右转向时 $ V{p_k}(\tau ) > 0 $ ，当USV往左转弯时 $ V{p_k}(\tau ) < 0 $ 。

式（15）表明在 $ R > 0 $ 且 $ \mu = 1 $ 的情况下，USV执行向右转向的避让措施所得到的 $ {C_{{{COL}}}}(\tau ) $ 更小，确保USV遵循规则并有效降低成本。

3.4 约束条件

模型的约束主要包括USV外部环境约束和内部执行机构约束两部分。

1）外部环境约束主要是指USV在自主航行过程中除按照预先设定的航线航行外，还需避让航行障碍物（静态的障碍物与动态的障碍物），其中动态障碍物为在航的目标船。

假设在 $ {t_0} $ 时刻有 $ n $ 个静态障碍物，它们的位置坐标分别为：

$ {p_s} = [{p_{{s_1}}},{p_{{s_2}}}, \cdots {p_{{s_n}}}]。$

式中： $ {p_{{s_i}}} = ({x_i},{y_i}) $ ， $ i \in [1,n] $ ，静态障碍物安全领域半径分别为 $ {r_s} = [{r_{{s_1}}},{r_{{s_2}}}, \cdots {r_{{s_n}}}] $ 。

在预测时域（ $ Np $ ）中，防止USV与静态障碍物发生碰撞的约束为：

$ {h_s}(N(\tau ),{p_s},Np,{r_s}) \leqslant 0。$

(18)

式中：

$ \begin{split} & {{h_s}(N(\tau ),{p_s},Np,{r_s}) = \left[ {\begin{array}{*{20}{c}} {{h_{{s_1}}}(N(\tau ),{p_{{s_1}}},Np,{r_{{s_1}}})} \\ {{h_{{s_2}}}(N(\tau ),{p_{{s_2}}},Np,{r_{{s_2}}})} \\ {{h_{{s_3}}}(N(\tau ),{p_{{s_3}}},Np,{r_{{s_3}}})} \\ \vdots \\ {{h_{{s_n}}}(N(\tau ),{p_{{s_n}}},Np,{r_{{s_n}}})} \end{array}} \right]}，\\ &{{h_{{s_i}}}(N(\tau ),{P_{{s_i}}},Np,{r_s}) = \left[ {\begin{array}{*{20}{c}} {{h_{{s_{i1}}}}({N_{{t_0} + 1}}(\tau ),{P_{{s_i}}},Np,{r_{{s_i}}})} \\ {{h_{{s_{i2}}}}({N_{{t_0} + 2}}(\tau ),{P_{{s_i}}},Np,{r_{{s_i}}})} \\ {{h_{{s_{i3}}}}({N_{{t_0} + 3}}(\tau ),{P_{{s_i}}},Np,{r_{{s_i}}})} \\ \vdots \\ {{h_{{s_{iNp}}}}({N_{{t_0} + Np}}(\tau ),{P_{{s_i}}},Np,{r_{{s_i}}})} \end{array}} \right]}，\\ &{{h_{{s_{ik}}}}({N_{{t_0} + k}}(\tau ),{P_{{s_i}}},Np,{r_{{s_i}}}) = {r_{{s_i}}}^2 - \left\| {\left( {\begin{array}{*{20}{c}} {{N_{{t_0} + k,1}}(\tau )} \\ {{N_{{t_0} + k,2}}(\tau )} \end{array}} \right) - {P_{{s_i}}}} \right\|_2^2} 。\end{split} $

其中： $ k \in [1,Np] $ ， $ i \in [1,n] $ 。

对于动态船舶，本文目标船的船舶领域简化为圆形。设在 $ {t_0} $ 时刻有 $ m $ 个动态目标船，它们的位置坐标分别为：

$ {p_T} = [{p_{{T_1}}},{p_{{T_2}}}, \cdots {p_{{T_m}}}]，$

$ {p_{{T_j}}} = ({x_j},{y_j}) \text{，} j \in [1,m]，$

对应的船舶领域半径为：

$ {r_T} = [{r_{{T_1}}},{r_{{T_2}}}, \cdots {r_{{T_m}}}]，$

在预测时域（ $ Np $ ）中，防止USV与动态障碍物发上碰撞的约束为：

$ {h_T}(N(\tau ),{p_T},Np,{r_T}) \leqslant 0。$

(19)

式中：

$ \;{{h_T}(N(\tau ),{p_T},Np,{r_T}) = \left[ {\begin{array}{*{20}{c}} {{h_{{T_1}}}(N(\tau ),{p_{{T_1}}},Np,{r_{{T_1}}})} \\ {{h_{{T_2}}}(N(\tau ),{p_{{T_2}}},Np,{r_{{T_2}}})} \\ {{h_{{T_3}}}(N(\tau ),{p_{{T_3}}},Np,{r_{{T_3}}})} \\ \vdots \\ {{h_{{T_m}}}(N(\tau ),{p_{{T_m}}},Np,{r_{{T_m}}})} \end{array}} \right]}，$

$ \begin{split} & {{h_{{T_j}}}(N(\tau ),{P_{{T_j}}},Np,{r_T}) = \left[ {\begin{array}{*{20}{c}} {{h_{{T_{j1}}}}({N_{{t_0} + 1}}(\tau ),{P_{{T_{j1}}}},Np,{r_{{T_j}}})} \\ {{h_{{T_{j2}}}}({N_{{t_0} + 2}}(\tau ),{P_{{T_{j2}}}},Np,{r_{{T_j}}})} \\ {{h_{{T_{j3}}}}({N_{{t_0} + 3}}(\tau ),{P_{{T_{j3}}}},Np,{r_{{T_j}}})} \\ \vdots \\ {{h_{{T_{jNp}}}}({N_{{t_0} + Np}}(\tau ),{P_{{T_{jNp}}}},Np,{r_{{T_j}}})} \end{array}} \right]}，\\ &\;{{h_{{T_{jk}}}}({N_{{t_0} + k}}(\tau ),{P_{{T_{jk}}}},Np,{r_{{T_j}}}) = {r_{{T_j}}}^2 - \left\| {\left( {\begin{array}{*{20}{c}} {{N_{{t_0} + k,1}}(\tau )} \\ {{N_{{t_0} + k,2}}(\tau )} \end{array}} \right) - {P_{{T_{jk}}}}} \right\|_2^2} 。\end{split} $

其中： $ k \in [1,Np] $ ， $ j \in [1,m] $ 。

2）内部约束主要是指USV执行机构的动力特性。为简化研究，只考虑执行机构所提供的 $ \tau $ 的大小范围作为其内部约束条件。

在预测时域（ $ Np $ ）中，USV执行机构内部约束为：

$ \begin{gathered} {h_\tau }(\tau ) \leqslant 0，\\ {g_\tau }(\tau ) = 0 。\\ \end{gathered} $

(20)

式中：

$ \begin{split} &{{h_\tau }(\tau ) = \left[ {\begin{array}{*{20}{c}} {{h_{\tau 1}}(\tau )} \\ {{h_{\tau 2}}(\tau )} \\ {{h_{\tau 3}}(\tau )} \\ \vdots \\ {{h_{\tau Np}}(\tau )} \end{array}} \right]}，\\ &{{h_{\tau k}}(\tau ) = \left[ {\begin{array}{*{20}{c}} {{\tau _k} - {\tau _{\max }}} \\ {{\tau _{\min }} - {\tau _k}} \end{array}} \right]} ，\\ &{{g_\tau }(\tau ) = \left[ {\begin{array}{*{20}{c}} {{g_{\tau 1}}(\tau )} \\ {{g_{\tau 2}}(\tau )} \\ \vdots \\ {{g_{\tau Np}}(\tau )} \end{array}} \right]} ，\\ &{{g_{\tau k}}(\tau ) = {\tau _3} - l{\tau _2}} 。\end{split} $

其中， $ k \in [1,Np] $ 。

4 仿真实验

采用的USV基础数据来自挪威科技大学的Autosea项目，相关模型参数详见参考文献[18]。

设置风速 ${U_{\text{W}}} = 4\;{\rm{m/s}}$ ，风向角 $ {\psi _{\text{W}}} = 30^\circ $ ；流速为 ${V_{{\text{cu}}}} = 1\;{\rm{m/s}}$ ，流向角为 $ \theta = 0^\circ $ 。

设置USV在 $ t = {t_1} $ 时刻的运动状态为：

$ {N_{{t_0}}} = {[0,0,0,8,0,0]^{\rm T}}。$

此时 ${\tau _{{t_1}}} = {[9\;040,0,0]^{\rm T}}$ ；设置期望航速大小为 $8\;{\rm{m/s}}$ ；设置预测时域为 $Np = 24\;{\rm{s}}$ 。

在期望航迹上设置3个静态障碍物，静态障碍物信息如表1所示。

设置2个动态船舶作为动态障碍物，动态障碍物信息如表2所示。

表2中目标船TS1运动状态为：

$ {N_{TS1}}={[800,900,270,9,0,0]^{\rm T}}，$

目标船TS2运动状态为：

$ {N_{TS2}}={[10\;208,3536,180,8,0,0]^{\rm T}}。$

USV自身能够提供的 $ \tau $ 的大小范围信息如表3所示。

USV中转船力臂 $l = 4\;{\rm{m}}$ ，即满足 $ {\tau _3} = 4{\tau _2} $ 的约束条件。

仿真结果如图5所示。

其中从 $ {t_1} $ 时刻到 $ {t_3} $ 时刻USV与TS1距离，从 $ {t_8} $ 时刻到 $ {t_{11}} $ 时刻USV与TS2距离分别如图6和图7所示。

自主航行过程中USV航行速度大小如图8所示。

自主航行过程中USV的航向如图9所示。

根据图5和图6可以看出：针对静态障碍物A，B和C，USV能够进行有效避让；针对动态障碍物的目标船TS1与目标船TS2，USV均从右侧安全避让，符合COLREGS中的第Rule-14和Rule-15条规定。从图7可以看出，当不需要对障碍物进行避让时，USV航速稳定在 $8\;{\rm{m/s }}$ 左右，符合设定的期望航速。

5 结　语

本文提出一种基于MPC的USV自主航行模型充分考虑了船舶动力学特性、航行经济性、规则适用性以及执行机构的动力输出约束，所形成的控制器策略更加贴近实际USV的航行特性。通过仿真实验实现了USV在期望航迹上的自主航行，同时也验证了航行过程中合理避让动/静态障碍物的能力。但在USV动力控制方面仅对 $ \tau $ 的最大值和最小值作为约束，尚未以USV的螺旋桨转速和舵角直接控制输入作为模型的约束条件，未来将进一步对该问题进行优化。