0 引　言

随着海上航运的蓬勃发展，行驶在港口附近水域与航道上的船舶更加密集，发生的船舶失控险情逐年增多^[1]，如2020年唐山港出现CAPE型散货船主机失控故障^[2]，2023年日照港某大型货船进港时出现全船失电引发搁浅事故等，这些险情对港口通航效率和通航安全造成了重大威胁。各大港口为提高通航过程中对大型船舶故障的应急处置能力开展了各种探索，其中在货船进港过程中配备多艘拖轮以带缆状态进行伴航，是一种应对紧急险情的有效手段^{[3 – 4]}。

拖轮海上作业强度大，提高伴航过程中的自主性，能够降低驾驶员工作强度、提高伴航安全性和效率。随着智能船舶技术的快速发展，智能拖轮也取得了一定的成果^[5]，拖轮自主伴航成为可能。多拖轮带缆自主伴航问题可以看作存在合作目标的编队航行及避障控制问题。近年来，随着智能船舶、导航与规划、无人船舶运动控制等技术的发展，多无人船舶自主编队避障航行成为了研究热点，解决这一问题通常需要设计轨迹规划算法与轨迹跟踪运动控制算法。研究者们提出了以领航者跟随者法^{[6 – 7]}、人工势场法^{[8 – 9]}\动态窗口法^[10]、反步法控制^[11]、滑动模态控制^[12]、模型预测控制^[13]等为主的编队避障控制方法。其中，模型预测控制（Model Predictive Control，MPC）作为一种能够同时考虑船舶动力学特性、执行器约束、最优化目标与多安全性约束的控制方法，在单船运动控制与多船编队控制领域均得到了广泛研究。柳晨光等^[14]基于三自由度非线性状态空间模型的近似线性化设计了欠驱动船舶的轨迹跟踪控制器。陈国权等^[15]基于模型预测控制方法，考虑避碰规则与环境干扰，设计实现了一种水面无人艇自主航行模型预测控制器。贾金恒等^[16]针对无人帆船动力学特点设计了“帆-舵”联合MPC控制器，提升了无人帆船轨迹跟踪的收敛速度。Wen 等^[17]提出一种基于分布式模型预测控制的编队避障导航控制器，通过在优化目标中引入一致性项来确保编队内部的协同，同时通过构造时滞观测器来估计外部干扰，从而实现了多无人艇的协同编队避障。

然而，多拖轮带缆伴航航行任务下，需要考虑带缆状态对拖轮运动的限制，使得多拖轮带缆自主伴航控制问题增加了多种运动学安全性约束，导致传统的自主编队控制方法无法适用。此外，目前的研究成果主要以船体纵向力、横向力及转艏力矩组成的广义控制力作为控制量，而拖轮通过控制双全回转推进器的推力和回转角度实现航行，在研究拖轮带缆自主伴航时，MPC方法与双全回转推进器推进模式的结合问题也有待深入研究。

因此，本文针对多拖轮在带缆状态下对进出港大型船舶进行自主伴航的控制问题，考虑由拖缆引起的多运动学安全性约束与拖轮双全回转推进模式，提出一种分层模型预测控制器，基于该分层模型预测控制方法设计多拖轮自主带缆伴航控制策略，并通过数值仿真验证算法的有效性。

1 拖轮动力学模型

作为一种典型的水面船舶，拖轮的动力学模型可以描述为^[13]：

$\left\{ {\begin{array}{*{20}{l}} {{\boldsymbol{\dot \eta }} = {\boldsymbol{R}}({\boldsymbol{\eta }}){\boldsymbol{\nu }}}，\\ {{\boldsymbol{M\dot \nu }} + {\boldsymbol{C}}({\boldsymbol{\nu }}){\boldsymbol{\nu }} + {\boldsymbol{D}}({\boldsymbol{\nu }}){\boldsymbol{\nu }} = {\boldsymbol{\tau }}} 。\end{array}} \right.$

(1)

式中：${\mathbf{\eta }} = {[x,y,\psi ]^{\mathrm{T}}}$为拖轮在地球坐标系中的位置状态量，$x$、$y$、$\psi$分别为位置坐标与艏向角；${\boldsymbol{\nu }} = {[u,v,r]^{\rm T}}$为拖轮在船体坐标系中的速度状态量，$u$、$v$、$r$分别为纵向速度、横向速度与艏向角速度；$R(\psi )$为地球坐标系速度与船体坐标系速度之间的转换矩阵，其具体形式为

${\boldsymbol{R}}({\boldsymbol{\eta }}) = {\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)

式中：${\boldsymbol{M}}$为惯性质量矩阵；${\boldsymbol{C}}$为科里奥利向心力矩阵；$D$为水动力阻尼矩阵；${\mathbf{\tau }} = {[{\tau _u},{\tau _v},{\tau _r}]^{\mathrm{T}}}$为广义控制力向量，其中，${\tau _u}$${\tau _v}$${\tau _r}$分别为纵向力、横向力与转艏力矩。惯性质量矩阵、科里奥利向心力矩阵与水动力阻尼矩阵的具体形式为^[18]：

${\boldsymbol{M}} = \left[ {\begin{array}{*{20}{c}} {{m_t} - {X_{\dot u}}}&0&0 \\ 0&{{m_t} - {Y_{\dot v}}}&0 \\ 0&0&{{I_z} - {N_{\dot r}}} \end{array}} \right] ，$

(3)

${\boldsymbol{C}} = \left[ {\begin{array}{*{20}{c}} 0&0&{ - {m_t}v + {Y_{\dot v}}v} \\ 0&0&{{m_t}u - {X_{\dot u}}u} \\ 0&0&0 \end{array}} \right] ，$

(4)

${\boldsymbol{D}} = \left[ {\begin{array}{*{20}{c}} { - {X_{uu}}\left| u \right|} &{ - {X_{vv}}v} &{ - {X_{vr}}r - {X_{rr}}r} \\ 0&{ - {Y_v} - {Y_{\left| v \right|v}}v}&{ - {Y_r} - {Y_{\left| v \right|r}}\left| v \right| - {Y_{\left| r \right|r}}\left| r \right|} \\ 0&{ - {N_v} - {N_{vvr}}vr}&{ - {N_r} - {N_{\left| r \right|r}}\left| r \right| - {N_{vrr}}vr} \end{array}} \right]。$

(5)

式中：${m_t}$为拖轮质量；${X_{\dot u}}$、${Y_{\dot v}}$、${N_{\dot r}}$为附加质量；${X_{uu}}$、${X_{vv}}$、${X_{vr}}$、${X_{rr}}$、${Y_v}$、${Y_r}$；${N_v}$、${N_r}$、${Y_{\left| v \right|v}}$、${Y_{\left| v \right|r}}$、${Y_{\left| r \right|r}}$；${N_{vvr}}$、${N_{\left| r \right|r}}$、${N_{vrr}}$均为水动力项。

2 任务场景

多拖轮在带缆状态下自主伴随大型船舶开展进出港航行任务时，需要考虑如下约束：

约束1　最佳应急预备状态。多拖轮带缆伴航过程中，拖轮应尽可能位于设定的编队位置处，从而确保在需要应急作业时能够最快进入作业状态，将这一最佳状态定义为最佳应急预备状态。

约束2　内部避碰。伴航过程中，拖轮应始终避免与大型船舶及编队中其他拖轮碰撞。

约束3　向内避障。伴航过程中，拖轮应规避外部障碍物。由于存在拖缆连接，拖轮在避障时必须向编队内部避障，避免拖缆与障碍物碰撞。

约束4　拖缆松弛。伴航过程中，拖缆不应紧绷施力^[4]，即拖轮与大型船舶系缆点之间的距离应小于设定的最大缆长。

约束5　单侧限位。伴航过程中，由于拖轮与大型船舶之间存在拖缆连接，拖轮位置必须始终处在大型船舶的单侧，从而避免拖缆与大型船舶间碰撞。

其中，约束2为编队避障任务中常见的约束，而约束3～约束5为引入带缆状态影响的运动学安全约束。因此，带缆伴航任务是指拖轮编队在满足上述运动学安全约束条件的条件下以最佳应急预备状态对大型船舶完成伴航的任务，本文选取4艘拖轮典型伴航模式（见图1）开展研究，4艘拖轮分布于大型船舶左右舷侧。同时，本文做出如下假设：1）假设拖缆刚性，即在紧绷前拖缆上无力的作用。2）编队中拖轮可精确获取各成员航行信息，且可获得大型船舶短暂时域内的运动预测信息。

3 控制器设计

为完成多拖轮自主带缆伴航任务，本文提出一种分层模型预测控制架构（见图2），该架构由顶层的轨迹规划器与底层的轨迹跟踪运动控制器组成。顶层轨迹规划器考虑拖轮运动学安全约束与最优化目标，基于多约束模型预测控制（Multi-Constraints Model Predictive Control, MCMPC）实现多拖轮的协同轨迹规划。底层架构考虑双全回转推进推力分配特性，设计轨迹跟踪运动控制器（Azimuth Thruster-based MPC, ATMPC）实现拖轮的轨迹跟踪控制。

如图2所示，顶层控制器规划各拖轮的参考轨迹序列，各拖轮底层控制器完成对参考轨迹序列的跟踪。${\mathbf{\eta }}_t^i(t)$、${\mathbf{\nu }}_t^i(t)$分别为各拖轮当前时刻的位置及速度。$i = 1,\,2,\,...,\,n$为拖轮编队中的拖轮编号，$\mathbf{\eta}_{\mathrm{ship}}(t)$、$\mathbf{\nu}_{\mathrm{ship}}(t)$分别为大型船舶的运动规划或运动预测序列，${{\mathbf{\eta }}_o}$、${L_o}$为外界局部静态障碍物信息。${\mathbf{\eta }}_{tr}^i(t)$为顶层控制器规划的参考轨迹序列，${M_u}$为顶层控制器的预测时域，${T_u}$为顶层控制器的控制周期，${N_p}$、${N_c}$分别为底层控制器的预测时域与控制时域，${T_l}$为底层控制器的控制周期，${\mathbf{\eta }}_{trh}^i(t)$为${\mathbf{\eta }}_{tr}^i(t)$经过3次Hermite插值后的参考轨迹序列。在分层MPC控制器控制架构下，顶层轨迹规划器使用相对较长的预测视野完成轨迹规划，而底层运动控制器使用较短的预测视野完成轨迹跟踪，并通过3次Hermite插值实现顶层规划的轨迹与底层的控制周期匹配，如图3所示。

3.1 MCMPC轨迹规划器设计 3.1.1 MCMPC模型预测方程

本文提出一种基于多约束模型预测控制的多拖轮协同轨迹规划器，用于生成满足带缆伴航任务要求的最优安全轨迹。对于第$i$条拖轮，以${{\boldsymbol{X}}^{ui}} = [{x^{ui}},{y^{ui}}, {\psi ^{ui}}, {u^{ui}},{v^{ui}},{r^{ui}}]^{\rm T}$作为状态变量，${{\boldsymbol{U}}^{ui}} = {[\Delta {u^{ui}},\Delta {v^{ui}},\Delta {r^{ui}}]^{\mathrm{T}}}$作为控制变量，基于前向欧拉法对式(1)中的运动学方程进行离散化，得到预测模型：

$\left\{\begin{split} &{{\boldsymbol{X}}^{ui}}(k + m|k) = {{\boldsymbol{X}}^{ui}}(k + m - 1|k) +\\&\qquad\qquad\quad\;\;\; ({{\boldsymbol{A}}^{ui}}(k + m - 1|k){T_u}){{\boldsymbol{X}}^{ui}}(k + m - 1|k) + \\&\qquad\qquad\quad\;\;\; {{\boldsymbol{B}}^{ui}}{{\boldsymbol{U}}^{ui}}(k + m - 1|k)，\\ &{{\boldsymbol{Y}}^{ui}}(k + m|k) = {{\boldsymbol{C}}^{ui}}{{\boldsymbol{X}}^{ui}}(k + m|k) 。\end{split} \right.$

(6)

其中，

$\boldsymbol{A}^{ui}=\left[\begin{array}{*{20}{l}}0_{3\times3} & \boldsymbol{R}(\psi^{ui}) \\ 0_{3\times3} & 0_{3\times3}\end{array}\right]，\qquad\qquad\qquad\quad$

(7)

${{\boldsymbol{B}}^{ui}} = \left[ {\begin{array}{*{20}{c}} {{{{0}}_{3 \times 3}}} \\ {{{\boldsymbol{I}}_{3 \times 3}}} \end{array}} \right] {{\boldsymbol{C}}^{ui}} = {\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{I}}_{3 \times 3}}} \\ {{{{0}}_{3 \times 3}}} \end{array}} \right]^{\mathrm{T}}}。$

(8)

式中：$(·)(k+m|k)$表示在k时刻对k+m时刻的预测值，简记为${(\cdot)}_{m}(k)$，同时将k时刻的完整预测序列记为$(\tilde{\cdot})(k)$。

3.1.2 MCMPC滚动优化问题构建

根据是否检测到局部障碍物，MCMPC滚动优化问题可描述为：

$\begin{gathered}\min\text{ }J^{ui}=J_q^{ui}+J_{oT}^{ui}+J_{ic}^{ui}，\mathrm{s.t}\mathrm{.\text{ C}}_U^{ui},\mathrm{\text{C}}_p^{ui}\mathrm{,\text{C}}_{oc}^{ui}\end{gathered}，$

(9)

$\begin{gathered}\min\text{ }J^{ui}=J_q^{ui}+J_{oT}^{ui}+J_{oo}^{ui}，\mathrm{s.t.\text{ C}}_U^{ui}\mathrm{,\text{C}}_p^{ui}\mathrm{,\text{C}}_{oc}^{ui},\text{C}_{op}^{ui}\end{gathered}。$

(10)

式中：$J_q^{ui}$、$J_{oT}^{ui}$、$J_{ic}^{ui}$、$J_{oo}^{ui}$为目标函数；$\mathit{\text{C}}_U^{ui}$、$\mathit{\text{C}}_p^{ui}$、$\mathit{\text{C}}_{oc}^{ui}$、$\mathit{\text{C}}_{op}^{ui}$为约束。基于第2节中的任务要求，对于拖轮$i$，设计MCMPC问题的最优化目标函数。

1）最优作业位置保持目标函数$J_q^{ui}$：

$J_q^{ui} = \sum\limits_{m = 1}^{{M_u}} {{\boldsymbol{e}}{{_{\eta m}^{ui}}^{\mathrm{T}}}(k){{\boldsymbol{Q}}^u}{\boldsymbol{e}}_{\eta m}^{ui}(k)}，$

(11)

${\boldsymbol{e}}_{\eta m}^{ui}(k) = {\boldsymbol{\eta }}_m^{ui}(k) - {\boldsymbol{\eta }}_{rm}^{ui}(k)。$

(12)

式中：${\boldsymbol{e}}_{\eta m}^{ui}(k)$为拖轮预测位置${{\boldsymbol{\eta }}^{ui}}$与最佳作业位置${\boldsymbol{\eta }}_r^{ui}$之间的误差；${\boldsymbol{\eta }}_r^{ui}$由设置的队形及大型船舶位置$\boldsymbol{\eta}\mathrm{_{ship}}$计算；${{\boldsymbol{Q}}^u}$为设定的权重矩阵。

2）拖轮内避碰目标函数$J_{oT}^{ui}$：

$J_{oT}^{ui}=w_{oT}\sum\limits_{j\in tug_b^i}^{ }\sum\limits_{m=1}^{M_u}\frac{1}{(d_{jm}^i(k)-D_t)^2}。$

(13)

式中：$tug_b^i$为拖轮内避碰集。设定编队中位于大型船舶后方的拖轮为较低优先级，对位于前方的同侧拖轮进行避碰。$d_{jm}^i(k)$为对应拖轮之间的距离，${D_t} = 2{L_i}$为拖轮内避碰半径，${w_{oT}}$为设定的权重。

3）最优预备作业缆长目标函数$J_{ic}^{ui}$：

$J_{ic}^{ui} = {w_{ic}}\sum\limits_{m = 1}^{{M_u}} {\frac{1}{{{{(d_{bm}^i(k))}^2}}}} 。$

(14)

式中：$d_{bm}^i(k)$为拖轮$i$与对应系缆点间的距离；${w_{ic}}$为设定权重。

4）外避障目标函数$J_{oo}^{ui}$：

$J_{oo}^{ui} = {w_{oo}}\sum\limits_{j = 1}^{{N_o}} {\sum\limits_{m = 1}^{{M_u}} {\frac{1}{{{{(d_{ojm}^i(k) - {D_{oj}})}^2}}}} }。$

(15)

式中：${N_o}$为拖轮局部感知到的与拖轮位于大型船舶同侧的障碍数量；$d_{ojm}^i(k)$为拖轮与障碍物之间的距离；${D_{oj}} = {L_{oj}} + {L_i}$，${L_{oj}}$为设定的障碍物的半径；${w_{oo}}$为设定的权重。

相应的，对于，设计MCMPC问题的约束为：

1）速度约束与速度增量约束$C_U^{ui}$：

$\left\{ {\begin{array}{*{20}{l}} {{\boldsymbol{\nu }}_{\min }^u \leqslant {\boldsymbol{\nu }}_m^{ui}(k) \leqslant {\boldsymbol{\nu }}_{\max }^u} ，\\ {{\boldsymbol{U}}_{\min }^u \leqslant {\boldsymbol{U}}_{m - 1}^{ui}(k) \leqslant {\boldsymbol{U}}_{\max }^u} 。\end{array}} \right.$

(16)

2）单侧限位半空间安全约束$C_p^{ui}$：

$\left\{ \begin{array}{*{20}{l}}\text{ if} \quad\boldsymbol{h}_{pm}^{ui}\boldsymbol{p}_{shipm}(k) + b_{pm}^{ui} < 0，\boldsymbol{h}_{pm}^{ui}\boldsymbol{p}_m^{ui}(k) + b_{pm}^{ui} > 0，\\ \text{ else}\quad\boldsymbol{h}_{pm}^{ui}\boldsymbol{p}_m^{ui}(k)+b_{pm}^{ui} < 0，\end{array}\right.$

(17)

${\boldsymbol{h}}_{pm}^{ui} = [ - \sin ({\psi _{shipm}}(k)),\cos ({\psi _{shipm}}(k))] ，$

(18)

$b_{pm}^{ui} = - d_{pm}^{ui}(k)\sin (\alpha _{pm}^{ui}(k) - {\psi _{shipm}}(k)) ，$

(19)

$\alpha _{pm}^{ui}(k) = {\text{atan2(}}y_{pm}^{ui}(k),x_{pm}^{ui}{\text{(k))}} ，$

(20)

$d_{pm}^{ui}(k) = \sqrt {x_{pm}^{ui}{{(k)}^2} + y_{pm}^{ui}{{(k)}^2}} 。$

(21)

式中：${P_{pm}}(x_{pm}^{ui},y_{pm}^{ui})$由设定的相对距离${D_p}$与$\boldsymbol{\eta}\mathrm{_{ship}}$确定。${{\boldsymbol{p}}_{shipm}}(k)$、${\boldsymbol{p}}_m^{ui}(k)$分别为伴航目标与拖轮的位置坐标。通过该约束可以确保拖轮始终位于大型船舶的单侧，如图4所示。

3）拖缆松弛安全约束$C_{oc}^{ui}$：

$d_{bm}^i(k) \leqslant {R_{oc}} 。$

(22)

式中：${R_{oc}}$为设定的最大缆长。

4）向内避障环形约束与半空间约束$C_{op}^{ui}$：

向内避障环形约束

$d_{bm}^i(k) < {d_{obj}}(k) 。$

(23)

式中：${d_{obj}}(k)$为障碍物与对应系缆点间的距离。

向内避障半空间约束

$\left\{ {\begin{array}{*{20}{l}} {{\text{ if}\quad}{\boldsymbol{h}}_{bpm}^{ui}{{\boldsymbol{p}}_{oj}} + b_{bpm}^{ui} < 0，{\text{ }}{\boldsymbol{h}}_{pm}^{ui}{\boldsymbol{p}}_m^{ui}(k) + b_{bpm}^{ui} > 0} ，\\ {{\text{else }\quad}{\boldsymbol{h}}_{bpm}^{ui}{\boldsymbol{p}}_m^{ui}(k) + b_{bpm}^{ui} < 0} 。\end{array}} \right.$

(24)

式中：${P_{bm}}(x_{bpm}^{ui},y_{bpm}^{ui})$为系缆点与障碍物中心连线与障碍物边界的交点，${{\boldsymbol{p}}_{oj}}(k)$为障碍物的坐标（见图5）。${\boldsymbol{h}}_{bpm}^{ui}$为障碍物边界过${P_{bm}}$的切向量，$b_{bpm}^{ui}$计算式为：

$b_{bpm}^{ui} = - {\boldsymbol{h}}_{bpm}^{ui}{{\boldsymbol{P}}_{bm}}^{\rm{T}}。$

(25)

对于每个有效障碍，式(23)与式(24)联立构成$C_{op}^{ui}$，通过该约束可以确保拖轮始终向内避障。

基于上述目标函数与约束构造优化问题，求解可得最优参考轨迹${\mathbf{\eta }}_{tr}^i(t)$。

3.1.3 前置预优化设计

MCMPC优化问题需要一个可行的初始解猜测保证求解器的正确求解，本文提出一种基于前置预优化的方法获得初始解猜测。设定如图6所示的临近可行队形，基于该队形与$\mathbf{\tilde{\eta}}_{\mathrm{ship}}(k)$可计算得到预优化的参考轨迹序列${\mathbf{\tilde \eta }}_f^{ui}(k)$，从而构造前置预优化问题为：

$\begin{gathered}\min\text{ }J=J_f^{ui}=\sum\limits_{m=1}^{M_u}\boldsymbol{e}_{\eta fm}^{ui\rm{T}}(k)\boldsymbol{Q}_f^u\boldsymbol{e}_{\eta fm}^{ui}(k)，\\ \mathrm{s.t}.\text{ C}_U^{ui}。\\ \end{gathered}$

(26)

式中：${\boldsymbol{e}}_{\eta fm}^{ui}(k)$为通过模型预测的拖轮位置$\eta _m^{ui}(k)$与预优化的参考轨迹序列${{\eta }}_{fm}^{ui}(k)$之间的误差。以零控制量作为预优化问题的初始猜测求解预优化问题，再将预优化问题的最优解${{\tilde {U}}_f^{{ui}^ * }}(k - 1)$作为MCMPC的可行初始解猜测。由图6可知，预优化是最大化可行性的一种近似求解，因此可以较大程度上保证MCMPC的初始可行性。

3.2 ATMPC轨迹跟踪控制器设计

本文设计考虑双全回转推进器推进模式特性的模型预测控制器ATMPC，用于控制各拖轮跟随顶层规划器规划出的安全轨迹。拖轮配备的全回转推进器推力与回转角度均可实时变化，且均存在限幅约束与增量约束，从而导致推力分配矩阵存在非线性特征。对于拖轮$i$，可表示为：

${\boldsymbol\tau}^{li}={\boldsymbol\varGamma}^{li}\boldsymbol{f}^{li}，$

(27)

${\boldsymbol{\varGamma}}^{li} = \left[ \begin{array}{*{20}{l}} & \cos(\beta_l^{li}) & \cos(\beta_r^{li}) \\ & \sin(\beta_l^{li}) & \sin(\beta_r^{li}) \\ & l_{xl}\sin(\beta_l^{li}) - l_{yl}\cos(\beta_l^{li}) & l_{xr}\sin(\beta_r^{li}) - l_{yr}\cos(\beta_r^{li})\end{array} \right]。$

(28)

式中：${{\boldsymbol{f}}^{li}} = {[f_l^{li},f_r^{li}]^{\rm{T}}}$为双全回转推进器的推力；${{\boldsymbol{\beta }}^{li}} = [\beta _l^{li}, \beta _r^{li}]^{\rm T}$为双全回转推进器的回转角度；${{\boldsymbol{\tau }}^{li}}$为三自由度的合力与力矩。对式(1)中的拖轮动力学模型进行简化，忽略高阶耦合水动力项，结合式(27)中的推力分配矩阵建立状态空间方程。再基于前向欧拉法离散化可得预测模型为：

$\left\{\begin{split} &{{\boldsymbol{X}}^{li}}(k + m|k) = {{{\bar {\boldsymbol A}}}^{li}}(k + m - 1|k){{\boldsymbol{X}}^{li}}(k + m - 1|k) +\\&\qquad\qquad\;\;\;\; {{{{\bar {\boldsymbol B}}}}^{li}}{{\boldsymbol{U}}^{li}}(k + m - 1|k)，\\& {{\boldsymbol{Y}}^{li}}(k + m|k) = {{\boldsymbol{C}}^{li}}{{\boldsymbol{X}}^{li}}(k + m|k){\text{ }}。\end{split} \right.$

(29)

$\overline{\boldsymbol{A}}^{li} = \left[ \begin{array}{*{20}{l}}(\boldsymbol{I}_{6\times6}+\boldsymbol{A}^{li}T_l)\boldsymbol{P}_{\xi}+\boldsymbol{B}^{li}\boldsymbol{\varGamma}^{li}\boldsymbol{P}_fT_l \\ \boldsymbol{P}_{\delta}\end{array} \right],\overline{\boldsymbol{B}}^{li} = \left[ \begin{array}{*{20}{c}}0_{6\times4} \\ \boldsymbol{I}_{4\times4}\end{array} \right]。$

(30)

$\left.A^{li}=\left[ \begin{array} {cc}0_{3\times3} & \boldsymbol{R}(\psi^{ui}) \\ 0_{3\times3} & \boldsymbol{M}^{-1}(\boldsymbol{C}+\boldsymbol{D}^{^{\prime}}) \end{array}\right.\right],\boldsymbol{B}^{li}=\left[\begin{array}{*{20}{c}}0_{3\times3} \\ \boldsymbol{M}^{-1}\end{array}\right]。$

(31)

$\left\{\begin{split} &{{\boldsymbol{P}}_\xi } = [{{\boldsymbol{I}}_{6 \times 6}}, {{{0}}_{6 \times 4}}] ,\;\; {{\boldsymbol{P}}_\delta } = [{{{0}}_{4 \times 6}},{{\boldsymbol{I}}_{4 \times 4}}] ,\\& {{\boldsymbol{P}}_f} = [{{{0}}_{2 \times 6}},{{\boldsymbol{I}}_{2 \times 2}},{{{0}}_{2 \times 2}}] ,\;\; {{\boldsymbol{C}}^{li}} = \left[ {{{\boldsymbol{I}}_{3 \times 3}},{{{0}}_{3 \times 10}}} \right]。\end{split}\right.$

(32)

式中：${T_l}$为控制周期；${{\boldsymbol{D}}^{'}}$为式(5)忽略高阶水动力项的形式；${{{\bar {\boldsymbol A}}}^{li}}$为拖轮的动力学状态转移矩阵；${{{\bar {\boldsymbol B}}}^{li}}$为拖轮的动力学输入矩阵；${{\boldsymbol{C}}^{li}}$为输出矩阵；${{\boldsymbol{P}}_\xi }$，${{\boldsymbol{P}}_\delta }$，${{\boldsymbol{P}}_f}$为状态量选择矩阵；${{\boldsymbol{X}}^{li}} = [{x^{li}},{y^{li}},{\psi ^{li}},{u^{li}},{v^{li}},{r^{li}},f_l^{li},f_r^{li}, \beta _l^{li}, \beta _r^{li}]^{\rm{T}}$为状态变量；${{\boldsymbol{U}}^{li}} = {[\Delta f_l^{li},\Delta f_r^{li},\Delta \beta _l^{li},\Delta \beta _r^{li}]^{\rm{T}}}$为控制变量。ATMPC的轨迹跟踪问题可描述为：

$\begin{split} & \min\text{ }J^{li}=\sum\limits_{m=1}^{N_p}\boldsymbol{e}_{\eta m}^{li\rm{T}}(k)\boldsymbol{Q}^l\boldsymbol{e}_{\eta m}^{li}(k)+ \\ &\qquad\quad\; \; \; \; \text{ }\sum\limits_{m=1}^{N_p}\boldsymbol{U}_{m-1}^{li\rm{T}}(k)\boldsymbol{R}_l\boldsymbol{U}_{m-1}^{li}(k)，\\ & s.t.\text{ }\boldsymbol{\delta}_{\min}^l\text{ \lt }\boldsymbol{\delta}_m^{li}(k) < \boldsymbol{\delta}_{\max}^l，\\ & \quad\; \; \text{ }\boldsymbol{U}_{\min}^l\text{ \lt }\boldsymbol{U}_m^{li}(k) < \boldsymbol{U}_{\max}^l\text{，}m=1,2,...,N_p。\end{split}$

(33)

式中：${\boldsymbol{e}}_{\eta m}^{li}(k) = {\boldsymbol{Y}}_{\eta m}^{li}(k) - {\boldsymbol{\eta }}_{trh}^i(k)$；${\boldsymbol{\eta }}_{trh}^i(k)$为${\boldsymbol{\eta }}_{trh}^i(t)$以${T_l}$为周期离散得到的序列；${\boldsymbol{\eta }}_{trh}^i(t)$由${\boldsymbol{\eta }}_{tr}^i(t)$以${T_l}$为间隔基于分段3次Hermite插值得到。实时求解式(33)，将最优控制量序列的首个分量作为当前时刻的控制输入控制拖轮完成轨迹跟踪。

4 仿真试验验证 4.1 仿真对象与仿真参数设置

选择1∶18缩比的拖轮自航模为仿真对象，船长为2 m，船宽为0.58 m，型深为0.27 m，排水量为140 kg，吃水深度为0.215 m。根据经验公式对式(3)～式(5)中的参数进行估算，得到完整的动力学模型^[18]。控制器参数中的${M_u}$、${Q^u}$、${T_u}$、${D_p}$、$L$、${R_{oc}}$、${w_{oo}}$、${w_{ic}}$、${w_{oT}}$、${Q^l}$、${R^l}$、${N_p}$、${N_c}$、${T_l}$分别为20、diag (1000，1000，1000) 、0.5、2、2、15、1000、100、10、diag (1，1，1)、diag (10000，10000，10000)7、3、0.2。拖轮的推进器坐标为(−0.84 m，−0.16 m)、(−0.84 m，0.16 m)，推进器推力与增量约束分别为[−150 N，200 N]、[−30 N，30 N]，推进器回转角度及增量约束分别为[−60°，60°]、[−12°，12°]。目标大型货船按照设定的轨迹运动，拖轮编队的最佳参考队形与前置预优化队形的相对位置如表1所示。

4.2 仿真试验结果

图7为各拖轮的运动轨迹。可知，拖轮编队在所设计控制器作用下实现了带缆伴航任务，且伴航过程中各拖轮可实现向内避障，避障后重新回到相对最优的预备作业位置。

图8为带缆伴航过程中拖轮的位置与最优参考队形之间的误差变化曲线，从中可以进一步说明拖轮在随目标大型船舶转向及向内避障后可以快速地重新收敛回到最佳应急预备队形并保持稳定的队形状态。

图9与图10为带缆伴航过程中拖轮推进器的推力与回转角度变化曲线，可以看出推进器推力与角度的饱和约束和增量约束均得到满足，表明本文提出的方法可以在考虑拖轮双全回转推进器推进模式及推进器推力、回转角度约束的情况下完成多拖轮带缆伴航控制。

图11为多拖轮带缆伴航过程中拖缆松弛约束的变化曲线。可知，在目标大型船舶转向较为剧烈处（例如，170 s与741 s左右时）与向内避障前后位置（例如，440 s、540 s、813 s时），安全性约束均可以得到满足，同时在整个伴航过程中，拖轮编队始终向内避障，保证在拖轮编队能够完成与目标大型船舶及外部障碍物避碰的同时，还能通过向内避障保证拖缆不与外界障碍物碰撞。进一步表明本文提出的控制器可以有效地完成多拖轮带缆伴航控制任务。

图12为应用不考虑多运动学安全性约束的传统MPC轨迹规划器结合ATMPC轨迹跟踪控制器完成带缆伴航任务的结果。可知，传统方法无法满足带缆伴航要求中的向内避障安全性约束，虽然拖轮成功避障，但遭遇队形内嵌式障碍物时会向编队外侧避障，从而导致拖缆与障碍物发生碰撞的危险。

图13为应用传统方法完成带缆伴航过程中的拖缆松弛约束变化曲线。除了向外避障的情况（例如，610 s、773 s与800 s时）以外，在伴航目标大型船舶转向较为剧烈的时刻（例如，171 s与752 s时）与向内避障后重新收敛回到最佳应急预备队形的过程中（例如，125 s时），也会出现违反拖缆松弛安全性约束的情况，导致拖缆紧绷对大型船舶错误施力的结果，无法安全实现带缆伴航任务，而采用本文提出的控制策略可以避免违反上述运动学安全约束。

5 结　语

本文针对海上多拖轮带缆伴航问题设计了一种分层MPC控制器，将带缆伴航控制器设计为顶层的轨迹规划器与底层的轨迹跟踪运动控制器。在顶层轨迹规划器中，首先对带缆伴航问题进行定义和描述，分析由带缆状态引入的运动学安全性约束，之后通过考虑这些安全性约束设计基于MCMPC的轨迹规划器规划安全的拖轮伴航轨迹，并提出前置预优化机制，解决多安全性约束下的初始解猜测问题。在底层轨迹跟踪控制器中，考虑拖轮双全回转推进器推进特性设计了基于ATMPC方法的轨迹跟踪控制器跟踪顶层规划出的安全轨迹，从而完成多拖轮带缆自主伴航任务。最后，通过仿真试验验证了本文提出控制器的有效性。仿真试验结果表明，相较于传统方法，本文提出的控制器可以在考虑拖轮双全回转推进器推进模式的情况下保证拖轮在伴航过程中满足拖缆引入的运动学安全性约束，从而在保证拖轮带缆伴航系统安全性的同时使得拖轮处于最佳应急预备状态，研究结果为拖轮自主伴航的应用提供了理论参考。