0 引　言

水面无人艇（Unmanned Surface Vessel, USV）是一种经济性好、机动性高、体型小的水上航行器，其可用于民用领域，如地理勘探，海上气象预报等，也可用于军用领域，如军事侦察，追踪目标，打击敌舰等军事任务。复杂多变的战场环境对军用无人艇的性能提出了更高的要求。然而军用无人艇在作战时其武器载荷会发生突变，导致其自主路径跟踪精度降低，从而可能引起无人艇自身平台危险或者任务失败的风险。因此，对其路径跟踪过程中载荷突变的工况进行研究，以提高无人艇自主路径跟踪进度非常有必要。

目前水面无人艇的路径跟踪主要难点在于：1）水面无人艇本身是欠驱动系统，一般不存在横向推力作用，从而无法像无人机或者水下潜航器一样进行直接横移控制；2）水面无人艇在路径跟踪过程中由于外界风、浪、流的干扰以及艇载载荷变化等影响，可能引起吃水、重量、水动力系数等参数的变化，从而导致控制器精度降低或失效；3）实际控制过程中转舵存在响应时间，所以控制精度会有所下降。

自抗扰控制（Active Disturbance Rejection Control，ADRC）是20世纪80年代由韩京清教授基于PID控制器的局限性提出的一种不依赖于模型且对非线性模型具有很好控制效果的控制器，由3个部分组成，即跟踪微分器TD（Tracking Differentiator），扩张状态观测器ESO（Extended State Observer），以及非线性状态误差反馈控制律NLSEF（Nonlinear State Error Feedback Law）。由于其参数繁多，难以整定，韩京清教授又提出了其线性形式，即线性自抗扰控制（Linear ADRC，LADRC）。随后，高志强教授又将带宽概念应用至LADRC中，设计线性扩张状态观测器（Linear Extended State Observer, LESO）和线性状态误差反馈控制律（Linear State Error Feedback, LSEF），从而克服了传统ADRC具有的参数多且整定困难的问题，使得LADRC需整定参数减少到了4个，使LADRC拥有了良好的鲁棒性能和动态性能，并且在多个领域得到了应用推广。

近年来，许多学者将ADRC控制器运用到水面无人艇跟踪控制中，王常顺等^[1]引入一种基于混沌局部搜索策略的双种群遗传算法对自抗扰控制器参数进行在线优化。Junxiang Hu等^[2]将萤火虫算法（Firefly Algorism, FA）引入自抗扰控制算法中，实现了NLSEF参数的整定，在考虑风浪干扰时，FA-ADRC的响应速度和抗干扰能力显著大于ADRC，有很好的稳定性和鲁棒性。董惠玲^[3]首先针对USV路径跟踪设计了一种基于结合自适应模糊控制的自抗扰控制器，然后又针对USV与UAV协同控制中存在的快速变换的大角度航向角控制问题设计了一种具有模糊自适应调参功能的滑模ADRC控制器，并通过仿真实验验证了所设计控制器具有良好鲁棒性。Lamraoui等^[4]针对传统ADRC中的扩张状态观测器在处理快变扰动时受噪声测量而受到的限制做了改进，提出了GESO和HESO，并与传统ADRC控制器进行了比较，验证了该观测器在扰动估计质量和准确的无噪声状态估计方面的优良性能，然而其所考虑的干扰函数是近似多项式形式，这与实际不符。

王晓惠等^[5]在ADRC控制器中引入遗传变异结合粒子群优化策略（GA-PSO），对ADRC控制器初值进行初始化以及全局参数寻优，最后通过仿真和实验验证了其控制性能优于传统控制器的性能。杨忠凯等^[6]将基于可变船长比的视线导引算法（Light of sight, LOS）与ADRC算法结合，并通过实船实验验证该算法可行性。胡俊祥等^[7]使用LADRC到无人艇航向控制中，并与自抗扰控制进行对比，验证了其具有相似的动静态性能和更简单的调参过程。

尽管以上研究成果针对ADRC难调参问题提出了多种解决方案，但对突变载荷可能产生的跟踪轨迹严重超调和抖振现象仍没有很好的解决。因此，本文提出了一种基于正切Sigmoid函数跟踪微分器的LADRC算法的路径跟踪控制算法，基于神经网络常用的激励函数Sigmoid函数，提出一种形式更简单、调参容易的正切Sigmoid函数的跟踪微分器（Tangent Sigmoid Tracking Differentiator, TSTD），设计的TSTD跟踪微分器能够减缓速度以及首向角的振动变化。由于ADRC将内外部干扰当成总干扰去除掉，而LADRC的调参难度又远小于ADRC，故对于载荷突变具有很好的鲁棒性和稳定性。最后，使用设计方法与PID控制器进行了对比分析，验证了其良好的鲁棒性和抗干扰能力。

1 USV动力学模型

在路径跟踪过程中，无人艇需要发射任务载荷以执行特定任务。然而，无人艇在海上执行火力发射任务时，如图1所示，发射武器载荷会使得无人艇的负载发生突变，从而影响无人艇姿态和水动力学参数，而复杂的海面环境又会对无人艇产生较大干扰。因此，首先必须建立六自由度无人艇模型，武器载荷的激变力模型以及干扰模型等。

首先，建立惯性坐标系以及船体船体坐标系如图2所示。其中，速度变量$ \boldsymbol{\nu} {\text{ = }}{[u,v,w,p,q,r]^{\rm T}} $，分别为船体坐标系内的纵荡速度、横荡速度、垂荡速度，横摇角速度，纵摇角速度，以及首摇角速度；位置变量$ \boldsymbol{\eta} {\text{ = }}{[x,y,z,\phi ,\theta ,\psi ]^{\rm T}} $，分别表示惯性坐标系下的x、y、z坐标以及横摇角、纵摇角、首向角大小。

1.1 无人艇建模

由文献[8]可知，考虑海风、海浪、海流干扰的六自由度无人艇的运动方程为：

$ \begin{gathered} \boldsymbol{M\dot \nu + C(\nu {\text{)}}{\nu _{{r}}} + G\eta }+ {\boldsymbol{g}_0} = {\boldsymbol{\tau} _1} + {\boldsymbol{\tau} _{{\text{wind}}}} + {\boldsymbol{\tau} _{{\text{wave}}}} + \\ {\boldsymbol{\tau} _{{\text{damp}}}} + {\boldsymbol{\tau} _{{{cf}}}} + {N_1}，\\ \end{gathered} $

(1)

$ \boldsymbol{\dot \eta _1} = {\boldsymbol{J}_1}({\boldsymbol{\eta} _1})\boldsymbol{\nu}，$

(2)

式中：$\boldsymbol{M}$为惯性矩阵，即刚体质量矩阵${\boldsymbol{M}_{{\text{RB}}}}$和附加质量矩阵${\boldsymbol{M}_{\text{A}}}$ 的总和，其表示无人艇运动产生的水动力学效应，下标${\text{RB}}$和${\text{A}}$分别为刚体和附加；$\boldsymbol{C(\nu )}$为Coriolis向心力矩阵，即标准Coriolis刚体矩阵与附加质量矩阵之和；$\boldsymbol G$为恢复系数矩阵；$ \boldsymbol{\nu }$为线速度和角速度；${\boldsymbol{\nu} _{\text{r}}}$为相对流线速度和角速度，下标$ {{r}} $表示相对；${\boldsymbol{g}_0}$为负载重力（力矩）向量；${\boldsymbol{\tau} _{{\text{damp}}}}$为阻尼力和力矩；$ {\boldsymbol{\tau} _{{\text{cf}}}} $为非线性横流阻尼力和力矩，下标$ {{cf}} $为横流；${\boldsymbol{\tau} _{{\text{wind}}}}$为海风干扰阻力和力矩；${{\boldsymbol{J}_1}({\boldsymbol{\eta} _1})}$为惯性坐标系和固定坐标系的转换矩阵；${N_1}$为激变力和激变力矩。

由文献[8]可以得到：

$ \begin{split} & \boldsymbol{M} =\boldsymbol{M}_{\text{RB}}+\boldsymbol{M}_{\text{A}}=\boldsymbol{H}^{\text{T}}\boldsymbol{M}_{\text{RB}}^{\text{CG}}\boldsymbol{H}+\boldsymbol{M}_{\text{A}}=\\ &\left[\begin{array}{cc}I& 0\\ S({r}_{\text{g}})& I\end{array}\right]\left[\begin{array}{cc}(m+{m}_{{p}})I& 0\\ 0& {I}_{\text{g}}\end{array}\right]\left[\begin{array}{cc}I& S{({r}_{\text{g}})}^{\text{T}}\\ 0& I\end{array}\right]+\\ &-{\rm diag}[{X}_{\dot{{{u}}}},{Y}_{\dot{{{v}}}},{Z}_{\dot{{{w}}}},{K}_{\dot{{{p}}}},{M}_{\dot{{{q}}}},{N}_{\dot{{{r}}}}]=\\ &(m+{m}_{{p}})\left[\begin{array}{cc}I& S{({r}_{\text{g}})}^{\rm T}\\ S({r}_{\text{g}})& S({r}_{\text{g}})S{({r}_{\text{g}})}^{\rm T}\end{array}\right]+\\ &\left[\begin{array}{cc}0& 0\\ 0& {I}_{\text{g}}\end{array}\right] - {\rm diag}[{X}_{\dot{{{u}}}},{Y}_{\dot{{{v}}}},{Z}_{\dot{{{w}}}},{K}_{\dot{{{p}}}},{M}_{\dot{{{q}}}},{N}_{\dot{{{r}}}}]。\end{split} $

(3)

式中：${m_{{p}}}$为负载质量，下标${{p}}$为负载；$S( \cdot )$ 为反对称矩阵；$I$为单位阵；${I_{{g}}}$ 为转动惯量矩阵；${r_{{g}}}$为船体坐标系下的船体及负载的重心坐标，下标${{g}}$表示重心，且${r_{{g}}} = {[{x_g},{y_g},{z_g}]^{\rm T}}$；$H( \cdot )$为把中心从重心移到原点的坐标变换矩阵；$ {X_{{{\dot {{u}}}}}}、{Y_{{{\dot{{ v}}}}}}、{Z_{{{\dot {{w}}}}}}、{K_{{{\dot {{p}}}}}}、{M_{{{\dot {{q}}}}}}、{N_{{{\dot {{r}}}}}} $为水动力导数，公式由挪威科技大学Fossen教授提出，详见表1。

表1中，${C_{{d}}}$为阻力系数，下标${{d}}$为阻力；${x_{{{Li}}}}$为船长方向第${{i}}$段分段坐标($ i = 1,2, \ldots ,{n_f} $)，${n_{{f}}}$表示分段数，大小见表2。

$ \begin{aligned} \boldsymbol{C}(\boldsymbol{\nu })=& \boldsymbol{C}_{\text{RB}}({\boldsymbol{\nu} }_{2})+\boldsymbol{C}_{\text{A}}({\boldsymbol{\nu} }_{\text{r,1}},{\boldsymbol{\nu}}_{\text{r,2}})=\\ & \boldsymbol{H}^{\text{T}}\boldsymbol{C}_{\text{RB}}^{\text{CG}}({\boldsymbol{\nu} }_{2})\boldsymbol{H}+\boldsymbol{C}_{\text{A}}({\boldsymbol{\nu} }_{\text{r,1}},{\boldsymbol{\nu} }_{\text{r,2}})=\\ & (m+{m}_{{p}})\cdot\\ & \left[ \begin{array}{cc}S({\nu }_{2})& S({\nu }_{\text{2}})S{({r}_{g})}^{\rm T}\\ S({r}_{g})S({r}_{g})& S({r}_{g})S({r}_{g})S{({r}_{g})}^{\rm T} - S({I}_{g}{\nu }_{2})/(m+{m}_{{p}})\end{array} \right]+\\ & \left[\begin{array}{cc}0& -S(0.1\cdot m{v}_{1})\\ -S(0.1\cdot m{v}_{1})& -S(1.5\cdot m{v}_{2})\end{array}\right]，\end{aligned} $

(4)

$ \begin{aligned} \boldsymbol{G} =&\boldsymbol{H}^{\text{T}}\boldsymbol{G}_{\text{CF}}\boldsymbol{H}=\\ &\left[\begin{array}{cc}I& 0\\ S({r}_{g})& I\end{array}\right]\text{diag}[0,0,\rho g(2{A}_{{\text{ω}}{\_}\text{pont})},\\ &\rho g\nabla G{M}_{\text{T}},\rho g\nabla G{M}_{\text{L}},0]\left[\begin{array}{cc}I& S{({r}_{{g}})}^{\text{T}}\\ 0& I\end{array}\right]。\end{aligned} $

(5)

$ {\boldsymbol{\nu} _{{r}}} = \boldsymbol{\nu} - {\left[ {\begin{array}{*{20}{c}} {{u_{\text{c}}}}&{{v_{\text{c}}}}&0&0&0&0 \end{array}} \right]^{\text{T}}}，$

(6)

$ {\boldsymbol{g}_0} = [{\boldsymbol{f}_{{p}}};{\boldsymbol{M}_{{p}}}] ，$

(7)

$ \begin{aligned} {\boldsymbol{\tau} _1} =& \left[ {\begin{array}{*{20}{l}} {{\boldsymbol{\tau} _{{\text{linear}}}}} \\ {{\boldsymbol{\tau} _{{\text{torque}}}}} \end{array}} \right] = \left[ \begin{gathered} {\boldsymbol{T}_1} + {\boldsymbol{T}_2} \\ {\boldsymbol{r}_1} \times {\boldsymbol{T}_1} + {\boldsymbol{r}_2} \times {\boldsymbol{T}_2} \\ \end{gathered} \right] = \\ &\left[ {\begin{array}{*{20}{c}} \boldsymbol{I}&\boldsymbol{I} \\ {\boldsymbol{S}({\boldsymbol{r}_1})}&{\boldsymbol{S}({\boldsymbol{r}_2})} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{\boldsymbol{T}_1}} \\ {{\boldsymbol{T}_2}} \end{array}} \right]，\\ \end{aligned} $

(8)

其中，$ \begin{gathered} {T_{{p}}} = {[{t_{{p}}},0,0]^{\text{T}}} , {t_{{p}}} = \left\{ \begin{gathered} {K_{{\text{pos}}}} \cdot {n_{{p}}} \cdot |{n_{{p}}}|,{n_{{p}}} > 0 \\ {K_{{\text{neg}}}} \cdot {n_{{p}}} \cdot |{n_{{p}}}|,{n_{{p}}} < 0 \\ \end{gathered} \right.{{p = 1,2}} \\ \end{gathered} $。

因为${\boldsymbol{\tau} _1}$为控制量，故有：

$ {\boldsymbol{\tau} _1} = \left[ {{\tau _{{X}}};0;0;0;0;{\tau _{{N}}}} \right]。$

(9)

式中：下标${{p}}$为螺旋桨编号（${{p = 1,2}}$）；${t_{{p}}}$为推力大小；${T_{{p}}}$ 为推力矢量；${n_{{p}}}$为螺旋桨转速；${K_{{\text{pos}}}}$和${K_{{\text{neg}}}}$分别为螺旋桨正转和反转推力系数，下标${\text{pos}}$和${\text{neg}}$分别表示正转和反转；${\tau _{{X}}}$为螺旋桨总推力，下标${{X}}$为${{X}}$轴方向；${\tau _{\text{N}}}$为螺旋桨总扭矩，下标${{N}}$为首摇方向；$ {\boldsymbol{f}_{\text{p}}} $和$ {\boldsymbol{M}_{\text{p}}} $分别为负载重力和力矩，下标${{p}}$为负载。

$ {\boldsymbol{N}_1} = \left[ {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} {{F_{{x}}}} \\ {{F_{{y}}}} \\ {{F_{{z}}}} \end{array}} \\ { - {F_{{y}}}({z_{\text{g}}} - {z_{{j}}}) + {F_{{z}}}({y_{\text{g}}} - {y_{{j}}})} \\ { - {F_{{x}}}({z_{\text{g}}} - {z_{{j}}}) + {F_{{z}}}({x_{\text{g}}} - {x_{{j}}})} \\ { - {F_{{x}}}({y_{\text{g}}} - {y_{{j}}}) + {F_{{y}}}({x_{\text{g}}} - {x_{{j}}})} \end{array}} \right]。$

(10)

式中：${N_1}$为激变力和激变力矩；${F_{{x}}}$、${F_{{y}}}$、${F_{{z}}}$ 分别表示${{{x}}_{{b}}}$、${{{y}}_{{b}}}$、${{{z}}_{{b}}}$方向的激变力大小；坐标$ {r_{{j}}} = \left( {{x_{{j}}},{y_{{j}}},{z_{{j}}}} \right)' $为激变力作用位置在船体上的船体坐标系中的坐标。

$ \begin{aligned} {\boldsymbol{\tau} _{{\text{damp}}}} = &{\boldsymbol{h}_{{r}}} \cdot {\boldsymbol{\nu} _{{r}}} = [{X_{{u}}},{Y_{{v}}},{Z_{{w}}},{K_{{p}}},{M_{{q}}},{N_{{r}}}(1 + \\ &10 \cdot |{\boldsymbol{\nu} _{\text{r}}}(6)|){]^{\text{T}}} \cdot {\boldsymbol{\nu} _{{r}}} = \\ &\left( {\left[ {\begin{array}{*{20}{l}} {24.4\displaystyle\frac{g}{{{U_{{\text{max}}}}}}} \\ 0 \\ {2 \cdot 0.3 \cdot {\omega _3}{M_{33}}} \\ {2 \cdot 0.2 \cdot {\omega _4}{M_{44}}} \\ {2 \cdot 0.4 \cdot {\omega _5}{M_{55}}} \\ {\displaystyle\frac{{{M_{66}}}}{{{T_{{\text{yaw}}}}}}} \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} 0 \\ 0 \end{array}} \\ 0 \end{array}} \\ 0 \\ 0 \\ {\displaystyle\frac{{{M_{66}}}}{{{T_{{\text{yaw}}}}}}10 \cdot abs(r)} \end{array}} \right]} \right) \cdot {\nu _{{r}}}。\\ \end{aligned} $

(11)

式中：$ {X_{{u}}}、{Y_{{v}}}、{Z_{{w}}}、{K_{{p}}}、{M_{{q}}}、{N_{{r}}} $均为水动力导数，下标分别为纵荡、横荡、垂荡、横摇、纵摇、首摇，详见表1。$g$为重力加速度；${U_{{\text{max}}}}$为无人艇最大速度；${\omega _{{i}}}{{(i = 3,4,5)}}$表示自然频率；${M_{{{ii}}}}{{(i = 3,4,5,6)}}$表质量矩阵的第$i$行第$i$列元素$(i = 3,4,5,6)$，${\text{kg}}$；${T_{{\text{yaw}}}}$表示首摇时间常数^[8]，下标${\text{yaw}}$为首摇。

$ {\boldsymbol{\tau} _{{\text{cf}}}} = \left[ {0;Y{h_{{\text{cf}}}};Z{h_{{\text{cf}}}};0;M{h_{{\text{cf}}}};N{h_{{\text{cf}}}}} \right]。$

(12)

式中，$ Y{h_{{\text{cf}}}}、Z{h_{{\text{cf}}}}、M{h_{{\text{cf}}}}、N{h_{{\text{cf}}}} $为使用切片理论^[9]得到的水动力导数，详见表1，其余变量及其表达式见表2。

因此，可以得到六自由度速度变量的微分方程：

$ \dot{\boldsymbol{\nu}} = {\boldsymbol{M}^{ - 1}}({\boldsymbol{\tau} _1} + {\boldsymbol{\tau} _{{\text{damp}}}} + {\boldsymbol{\tau} _{{\text{cf}}}} + {\boldsymbol{\tau} _{{\text{wind}}}} + {\boldsymbol{\tau} _{{\text{wave}}}} - {\boldsymbol{N}_0}) ，$

(13)

$ {\boldsymbol{N}_0} = \boldsymbol{C}(\boldsymbol{\nu} {\text{)}}{\boldsymbol{\nu} _{\text{r}}} + \boldsymbol{G\eta} + {\boldsymbol{g}_0} - {\boldsymbol{N}_1} 。$

(14)

假设1　认为无人艇武器载荷质量远小于无人艇总重量，即考虑${m_{\text{p}}} \approx 0$。

则式(13)可以写成：

$ \dot{\boldsymbol{\nu}} = {\boldsymbol{A}_0}\boldsymbol{\nu} + {\boldsymbol{B}_0}\boldsymbol{\tau} + \boldsymbol{\omega}，$

(15)

式中：$\tau $为控制力和力矩；$\omega $为不包含$ \nu $和$\tau $的其他项的总和。

1.2 激变力设置

参考潜射导弹的作用规律，由于无人艇发射导弹时不涉及“水锤”效应和高压燃气造成的负压区激变力，故无人艇受到的导弹激变力仅有导弹作用反力，根据文献[10]，可将无人艇受到的竖直方向突变激变力${F_{{j}}}$ 考虑为激变力作用半径$r$和作用时间$t$的函数，如下式：

$ {F_{{j}}} = [0;0;\iint\limits_{{{\text{A}}_{\text{1}}}} {cj(r,t) \cdot {\rm d}s}] 。$

(16)

式中：下标${{j}}$为激变力；$cj(r,t)$为$r$和时间$t$的冲击力函数；${{{A}}_{\text{1}}}$为其作用区间。若考虑该导弹发射方向为斜发，则有：

$ \begin{aligned} {F_{\text{j}}} =& [\sin (\alpha )\iint\limits_{{{\text{A}}_{\text{1}}}} {cj(r,t) \cdot {\rm d}s};\sin (\beta )\iint\limits_{{{\text{A}}_1}} {cj(r,t) \cdot {\rm d}s}; \\ &\sin (\gamma )\iint\limits_{{{\text{A}}_{\text{1}}}} {cj(r,t) \cdot {\rm d}s} ]。\end{aligned} $

(17)

式中：$\alpha $、$\beta $、$\gamma $为导弹发射方向在固定坐标系下的角度，rad。

参考文献[11]，可得到导弹对潜艇的作用反力的作用力曲线以及作用反力的大致范围。考虑在相似（缩尺比$\alpha = 30$）实船上发射一枚作用反力最大值为$3 \times {10^6}\ {\text{N}}$的潜射导弹，设无人艇受到的作用反力为${F_{{\text{max}}}}$，实船航行区域水密度与无人艇航行水域近似，故作如下假设：

假设2　实船和船模受到的导弹作用反力也满足傅汝德比较定律。

则有计算式：

$ {F_{\max }} = 3 \times {10^6}\ {\text{N}}/{30^3} = 111.11\ {\text{N}}。$

(18)

考虑发射角度为：$ \alpha = \beta = - \gamma = \text π /4 $，则可以得到发射反力的近似作用曲线，其${{{x}}_{{n}}}$轴分量作用力曲线如图3所示。

1.3 风、浪、流干扰模型

一般地，海风干扰视为定常干扰。根据文献[12]，式(1)已考虑海流干扰力和力矩。相对流速${v_r}$的表达式见式(6)，其中下标$c$表示海流，${u_{\text{c}}}$和${v_{\text{c}}}$表示船体坐标系下的海流速度。由文献[13]，其风浪流干扰可用式(19)和式(20)表示。

$ \begin{aligned} {T_{{{wu}}}} = &{T_{{{wv}}}} = 0.08\left(\sin (0.2t) + \cos \left(0.2t + \frac{\text π}{4}\right) + \right.\\ &\left.\sin \left(0.2t + \frac{\text π}{6}\right)\right) ，\end{aligned} $

(19)

$ \begin{aligned} {T_{{{wr}}}} = & 0.1\left(\sin (0.2t) + \cos \left(0.2t + \frac{\text π }{4}\right) +\right. \\ & \left.\sin \left(0.2t + \frac{\text π }{6}\right)\right) 。\end{aligned} $

(20)

式中：$ {T_{{{wu}}}} $、$ {T_{{{wv}}}} $、$ {T_{{{wr}}}} $分别为船体坐标系中的$x$、$y$、$\psi $方向的干扰力和力矩。

2 路径跟踪控制设计

路径跟踪控制分为制导和控制两部分，本文采用的引导律是积分视距法（Integral Light of Sight，ILOS），采用的控制方法是改进的线性自抗扰方法（Linear Active Disturbance Rejection Control，LADRC）。

2.1 ILOS引导律

视距法的原理是根据期待路径点和船舶之间的位置关系求算出期待的航向角${\chi_d}$，下标${{d}}$表示期待值。

如图4所示，假设当前期待路径点为$P_{{i}}^{{n}} = ({x_{{i}}},{y_{{i}}})$（惯性坐标系下），下一期待路径点为$P_{{i}}^{{{n + 1}}} = ({x_{{{i + 1}}}},{y_{{{i + 1}}}})$，且路径点总数为${N_{{\text{path}}}}$，则两路径点连线与${{X}}$轴夹角为：

$ {{\text π} _{\text{p}}} = \arctan (({x_{{\text{i + 1}}}} - {x_{\text{i}}})/({y_{{\text{i + 1}}}} - {y_{\text{i}}}))，$

(21)

可知，无人艇在$t$时刻的位置为$(x(t),y(t))$。则USV和实时路径位置间的路径跟踪垂向误差以及切向误差分别为：

$ {y_{{e}}} = - (x(t) - {x_{\text{i}}})\cos ({{\text π} _{{p}}}) + (y(t) - {y_{\text{i}}})\sin ({{\text π} _{{p}}}) ，$

(22)

$ {x_{{e}}} = - (x(t) - {x_{\text{i}}})\sin ({{\text π} _{{p}}}) + (y(t) - {y_{\text{i}}})\cos ({{\text π} _{{p}}})。$

(23)

式中：${{\text π} _{{p}}}$为$P_{{i}}^{{n}}P_{{i}}^{{{n + 1}}}$连线与惯性坐标系${x_{{n}}}$轴的夹角，${\text{rad}}$。设${R_{{\text{switch}}}}$为切换半径，$ {\text{m}} $。

可以得到两路径点间的距离为：

$ d = \sqrt {{{({x_{{{i + 1}}}} - {x_{{i}}})}^2} + {{({y_{{{i + 1}}}} - {y_{{i}}})}^2}}。$

(24)

由此可得，期待路径点的切换条件为：① $d - {x_{{e}}} < {R_{{\text{switch}}}}$；② $i < {N_{{\text{path}}}}$。

传统LOS法得到的USV实时指令航向角为：

$ {\chi_{{d}}} = {{\text π} _{{p}}} - \arctan ({y_{\text{e}}}/\Delta )。$

(25)

其中：$\Delta $为前视距离，${\text{m}}$。如图4所示，${y_{{e}}}$为横向误差，${\text{m}}$。

ILOS引导律采用Lekkas and Fossen^[15]在2014年提出如下形式：

$ {\chi_{\text{d}}} = {{\text π} _{\text{p}}} - \arctan ({K_{{p}}}{y_{\text{e}}} + {K_{i}}{y_{{\text{int}}}}) 。$

(26)

式中：${K_{{p}}} = 1/\Delta $，${K_{{i}}} = \kappa {K_{{p}}}$，$\kappa $>0；${y_{{\rm int} }}$表示式(24)的积分值。

$ {\dot y_{{\rm int} }} = U{y_{\text{e}}}/\sqrt {{\Delta ^2} + {{({y_{\text{e}}} + \kappa {y_{{\text{int}}}})}^2}}。$

(27)

式中：U为水平速度和，其表达式见(28)。

由文献[16]知，如果期待航向角如式(26)所示，且积分项的时间导数如式(27)所示，则${y_{\text{e}}} = 0$是全局$\kappa $-指数渐进稳定以及局部渐近稳定的。

$ U = \sqrt {{u^2} + {v^2}} 。$

(28)

2.2 改进LADRC算法设计

韩京清教授提出的ADRC的核心结构由跟踪微分器TD、状态扩张观测器ESO、非线性状态误差反馈控制律NLSEF3部分组成。高志强^[17]提出线性自抗扰控制器（LADRC），将传统ADRC中的ESO和NLSEF引入带宽概念，从而改为了线性化的LESO和LSEF，使得参数整定过程极大简化。然而，LADRC容易引起系统的超调且无法过滤给定的信号。

本文所使用的控制器是基于LADRC控制器改进而来，其中，针对航向角控制设计的控制器是二阶LADRC控制器，其结构如图3所示，其中的转速$n$满足饱和函数：$ - 101.737\ {\text{r/min}} \leqslant n \leqslant 103.931\ {\text{r/min}} $。

2.2.1 航向控制器

由式(2)得：

$ \dot{\boldsymbol{\eta}} = \boldsymbol{J}(\boldsymbol{\eta} )\boldsymbol{\nu}，$

(29)

其中，$ \boldsymbol{J}(\boldsymbol{\eta} ) = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{J}_{{R}}}}&0 \\ 0&{{\boldsymbol{J}_T}} \end{array}} \right] $。

$ {\boldsymbol{J}_R} = \left[ {\begin{array}{*{20}{c}} {{\rm c} \psi {\rm c}\theta }&{ - {\rm s}\psi {\rm c}\phi +{\rm c}\psi {\rm s}\theta {\rm s}\phi }&{{\rm s}\psi {\rm s}\phi + {\rm c}\psi {\rm c}\phi {\rm s}\theta } \\ {{\rm s}\psi {\rm c}\theta }&{{\rm c}\psi {\rm c}\phi + {\rm s}\phi {\rm s}\theta {\rm s}\psi }&{ - {\rm c}\psi {\rm s}\phi + {\rm s}\theta {\rm s}\psi {\rm c}\phi } \\ { - {\rm s}\theta }&{{\rm c}\theta {\rm s}\phi }&{{\rm c}\theta {\rm c}\phi } \end{array}} \right] ，$

(30)

$ {\boldsymbol{J}_T} = \left[ {\begin{array}{*{20}{c}} 1&{{\rm s}\phi {\rm s}\theta /{\rm c}\theta }&{{\rm c}\phi {\rm s}\theta /{\rm c}\theta } \\ 0&{{\rm c}\phi }&{ - {\rm s}\phi } \\ 0&{{\rm s}\phi /{\rm c}\theta }&{{\rm c}\phi /{\rm c}\theta } \end{array}} \right] ，$

(31)

其中：${\rm s}\phi $、${\rm s}\psi $、${\rm s}\theta $中的${\rm s}$为正弦函数$\sin ( \cdot )$；${\rm c}\phi $、${\rm c}\psi $、${\rm c}\theta $中的${\rm c}$为余弦函数$\cos ( \cdot )$。根据式(15)可得，$D = [0,0,0,0,0,1]$。在图4中，有：

$ \chi = \psi + {\beta _c} ，$

(32)

$ {\beta _{\text{c}}} = 1/\sin (v(t)/U(t))。$

(33)

式中：${\beta _{\text{c}}}$为偏航角，一般认为其变化缓慢，则有：

$ \begin{aligned} \ddot \chi = &(\dot \psi + {{\dot \beta }_{\text{c}}})' = (\dot \psi )' = (\boldsymbol{D\dot \eta })' = (\boldsymbol{DJ}(\boldsymbol{\eta} )\boldsymbol{\nu} )' = \\ &\boldsymbol{D}\boldsymbol{\dot v} + F(\theta ,\phi ,q,t) 。\end{aligned} $

(34)

这里的$F( \cdot )$表示非线性函数。代入式(15)，可得：

$ \begin{aligned} \ddot \chi = &\boldsymbol{D\dot \nu} + F(\theta ,\phi ,q,t) = \boldsymbol{D}({\boldsymbol{A}_0}\boldsymbol{\nu} + {\boldsymbol{B}_0}\boldsymbol{\tau} + \boldsymbol{\omega} ) + \\ &F(\theta ,\phi ,q,t) 。\end{aligned} $

(35)

则可得2阶系统：

$ \begin{gathered} \ddot \chi (t) = {b_0}{\tau _N} + \\ \underbrace {\boldsymbol{D}({\boldsymbol{A}_0}\boldsymbol{\nu} + {\boldsymbol{B}_0}\boldsymbol{\tau} + \boldsymbol{\omega} ) + F(\theta ,\phi ,q,t) - {b_0}{\tau _N}}_f 。\\ \end{gathered} $

(36)

这里的$f$指的是内外干扰的总和，$ {b_0} $为控制器参数。

定义该系统的状态为：${z_1} = \chi $，${z_2} = \dot \chi $，和${z_3} = {f_2}(t)$；则系统可以写为：

$ \left\{ \begin{gathered} {{\dot z}_1} = {z_2} ，\\ {{\dot z}_2} = {z_3} + {b_0}{\tau _N}，\\ {{\dot z}_3} = \dot f(t) = h(t)。\\ \end{gathered} \right. $

(37)

则可得三阶LESO如式(38):

$ \left\{ \begin{gathered} {{\dot {\hat z}}_1} = {{\hat z}_2} + {\beta _1}(\chi (t) - {{\hat z}_1})，\\ {{\dot {\hat z}}_2} = {{\hat z}_3} + {b_{02}}{\tau _N}(t) + {\beta _2}(\chi (t) - {{\hat z}_1})，\\ {{\dot {\hat z}}_3} = {\beta _3}(\chi (t) - {{\hat z}_1})。\\ \end{gathered} \right. $

(38)

取该LESO增益系数为${\beta _1} = 3{\omega _o}$，${\beta _2} = 3{\omega _o}^2$以及$ {\beta _3} = \omega _o^3 $。同样地，因此，如果选择合适的带宽${\omega _o}$，则可使得$ {\hat z_1} \to \chi (t)、{\hat z_2} \to \dot \chi (t)、{\hat z_3} \to f(t) $。

正切Sigmoid函数使用文献[18]中的函数，稳定性证明过程也已在该文中给出。

其中，正切Sigmoid函数如式(39)所示，TSTD如式(40)所示。

$ {\text{tansig}}(x) = 2/(1 + {e^{( - 2x)}}) - 1 ，$

(39)

$ \left\{ \begin{gathered} {{\dot x}_1}(t) = {x_2}(t)，\\ T{S_1} = {\text{tansig}}[\beta ({x_1}(t) - {\chi _d}(t))]，\\ T{S_2} = {\text{tansig}}[{x_2}(t)/k]，\\ {{\dot x}_2}(t) = {k^2}\left\{ { - {l_1}|{x_1}(t) - {\chi _d}(t){|^p}\cdot T{S_1}\left. { - {l_2}T{S_2}} \right\}} \right.，\\ re{f_1}(t) = {x_1}(t) 。\\ \end{gathered} \right. $

(40)

则可得系统控制律为：

$ {\tau _N}(t) = - ({k_{\text{p}}}({z_1}(t) - {\text{re}}{{\text{f}}_1}(t)) + {k_{\text{d}}}{z_2}(t) + {z_3}(t))/{b_0}。$

(41)

式中：${{re}}{{{f}}_{\text{1}}}{{(t)}}$为参考输出值；$ {k_{{p}}} $和$ {k_{{d}}} $为控制器系数，上述系数取值详见表3。

3 仿真实验结果与分析

针对上述设计的控制器，使用挪威科技大学的双体水面无人艇“Otter”的水动力参数并在Matlab软件中搭建其数学模型并进行仿真实验。针对本文工况，即载荷突变是局部工况，只用考虑其直线路径跟踪工况即可。

3.1 仿真设置

激变力曲线见图2，其激变力设置见表4。

其中，下标${{j}}$均为激变力；PID控制器的参数见表5。

3.2 不考虑干扰的路径跟踪

考虑到ILOS制导律使用的参考路径是路径散点集合，为了更直观比较控制算法对跟踪精度的影响，使无人艇从惯性坐标系原点沿着${{X}}$轴进行直线路径跟踪，其初始首向朝向${{X}}$轴正向，且初速度设置为0，其发射载荷时间设置在无人艇水平合速度稳定后，其具体设置参数可见表4，则可考虑其路径点集合为：

$ [{x_{{d}}}] = [0,100,200,300,400,500,600,700,800,900,1000]，$

$ [y_d] = [0,0,0,0,0,0,0,0,0,0,0]，$

首先考虑无风、浪、流干扰的情况，在Simulink中对该工况进行仿真，并与PID控制器进行对比，其参数可见表5。可得到图5所示的仿真结果。在0～20 s之间，由图5(a)可知，由于无人艇模型考虑海流流速，故其控制输出的期待航向角在小范围内波动，而其中PID控制器的ILOS制导算法输出了一个较大的期待航向角，这是因为PID算法相较于ILADRC算法响应更慢。

在20～30 s间，由图5(c)知，在激变力产生时，ILADRC算法输出的回转力矩大小要远大于PID算法，说明ILADRC控制器对于激变作用反力的干扰更灵敏，且在激变作用反力作用结束之后，在30～70 s间，由图5(b)可知，ILADRC的收敛的时间比PID控制器短30 s左右，其收敛速度提高了约50%。而在响应幅值上，由图5(b)的数据可知，PID控制器的最大误差响应幅值是0.587 m，而ILADRC控制器的最大误差响应幅值是0.274 m，在响应幅值上，ILADRC控制器使得其减小了约53.41%。

3.3 考虑风、浪、流干扰时的路径跟踪运动

考虑存在风、浪、流干扰的情况，可得到图6所示的响应结果。在0～20 s间，由图6(a)和图6(c)知，ILADRC产生了较大的回转力矩，而PID控制器输出的回转力矩一直很小，因而在接近20 s时，由图6(b)可知，PID控制器产生了一个较大的横向误差，此误差与20 s时的激变力反作用力产生的误差叠加在一起，由图6(b)可知，PID控制器的最大误差响应幅值达到0.65 m，增大了约10.22%，而ILADRC的最大误差响应幅值只有0.277 m，只提高了1.09%。在30 s以后，由于风、浪、流的影响，PID控制器会产生较大的横向超调，由图6(b)知，在只有风、浪、流干扰时，PID控制器的最大误差响应幅值可达到约0.118 m，而ILADRC控制器的最大误差响应幅值约为0.03 m，因此，在考虑风、浪、流干扰的情况下，ILADRC的具有良好的抗干扰性能。

4 结　语

本文针对无人艇进行路径跟踪时发生突变载荷的情况进行了数学建模，并对无人艇进行了六自由度动力方程建模。考虑无人艇路径跟踪过程中发生的突变载荷会使得无人艇的运动状态发生变化，本文采用了一种基于TSTD-LADRC的控制方法进行了无人艇的航向角控制，通过Simulink对所建立“Otter”无人艇模型和控制器进行了搭建并进行了运动仿真，并与PID控制器进行了比较。结果表明，本文设计的航向角控制器具有较好的鲁棒性和稳定性，同时，与传统PID控制器本文设计的控制器针对激变力变化更为灵敏且路径跟踪的精度更高。