无人水下航行器(unmanned underwater vehicle, UUV)在军事侦察与监视、反潜与巡逻、海洋测绘、海洋资源勘探等领域都发挥着重要作用，是当前海洋工程领域的研究热点。当任务复杂时，单个UUV难以胜任，这就需要多个UUV之间的合作和协调来完成任务。应用较多的编队策略主要有3种：领航-跟随法、基于行为法和虚拟结构法。Encarnacao等^[1-2]最早把领航-跟随法应用到欠驱动船舶的编队控制问题。Yoo等^[3]利用反步法设计了船舶编队控制器，但未考虑模型参数的不确定性和外界干扰。Balch等^[4]首次提出了基于行为的编队控制方法, 文献[5]提出一种基于零空间的行为控制方法用于编队控制。Tan^[6]首次提出了虚拟结构的概念，并应用在移动机器人的编队控制上^[7]。为进一步提高编队的鲁棒性，Desai等^[8]基于无源法设计了采用双向通信结构的同步路径跟踪控制。Wang等^[9]实现了单个UUV的路径跟踪，同时利用路径参数进行一致性算法设计实现了多UUV的协同运动。但是上述研究未同时考虑海流干扰和参数不确定性的影响。Peng等^[10]采用神经网络和反步法研究了多UUV编队控制问题，UUV模型不确定性和有界未知海流的扰动得到了补偿。齐小伟等^[11]实现了含有模型不确定性与未知海浪干扰项的多无人艇协同编队控制，并引入一阶滤波器代替反步计算中的微分项，显著减少了计算量。

考虑外界环境扰动，并引入二阶滤波器，提出了一种基于领航者的多UUV编队协调方法。领航者采用滤波反步法进行设计，在反步法中加入了二阶滤波器，可以避免繁琐的求导过程，简化计算量；同时为提高跟踪精度，又引入了滤波跟踪误差补偿回路。

1 领航者路径跟踪控制器的设计 1.1 欠驱动UUV水平面模型

假设存在外界干扰，针对欠驱动UUV, 其水平面模型^[12]的方程表示形式为

$ \left\{ \begin{array}{l} \dot u = \frac{{{m_{22}}}}{{{m_{11}}}}vr - \frac{{{d_{11}}}}{{{m_{11}}}}u + \frac{1}{{{m_{11}}}}\left( {{\tau _u} + {\tau _{uE}}} \right)\\ \dot v = - \frac{{{m_{11}}}}{{{m_{22}}}}ur - \frac{{{d_{22}}}}{{{m_{22}}}}v + \frac{{{\tau _{vE}}}}{{{m_{22}}}}\\ \dot r = \frac{{{m_{11}} - {m_{22}}}}{{{m_{33}}}}uv - \frac{{{d_{33}}}}{{{m_{33}}}}r + \frac{1}{{{m_{33}}}}\left( {{\tau _r} + {\tau _{rE}}} \right)\\ \dot x = u\cos \psi - v\sin \psi \\ \dot y = u\sin \psi + v\cos \psi \\ \dot \psi = r \end{array} \right. $

式中：${m_{11}} = m - {X_{\dot u}}$,${m_{22}} = m - {Y_{\dot v}}$, ${m_{33}} = {I_z} - {N_{\dot r}}$,${d_{11}} = {X_u} + {X_{u\left| u \right|}}\left| u \right|$,${d_{22}} = {Y_v} + {Y_{v\left| v \right|}}\left| v \right| $ ,${d_{33}} = {N_r} + {N_{r\left| r \right|}}\left| r \right| $,$m = 40$,${X_{\dot u}} = - 1.42$,${X_u} = 0.1$,${X_{u\left| u \right|}} = 8.2$,$ {Y_{\dot v}} = - 38.4$,$ {Y_v} = 10$,$ {Y_{v\left| v \right|}} = 200$,$ {N_{\dot r}} = - 8.9$,$ {N_r} = 5$,${N_{r\left| r \right|}} = 15 $, $ {I_z} = 8.0$。

1.2 欠驱动UUV路径跟踪误差方程的建立

首先进行领航者路径跟踪控制器的设计，令期望路径上虚拟向导x_i=η_0i(θ_i)+R($φ$_0i(θ_i))l_i的虚拟期望位置向量^[13]为

$ {\mathit{\boldsymbol{P}}_d} = {\left[ {{x_d}\left( s \right),{y_d}\left( s \right)} \right]^{\rm{T}}} $

(1)

式中s为期望路径的弧长。

UUV的实际位置向量为

$ \mathit{\boldsymbol{P}} = {\left[ {x,y} \right]^{\rm{T}}} $

(2)

E=[x_e, y_e]^T为位置误差向量，具体可以写为

$ \left\{ \begin{array}{l} {x_e} = x - {x_d}\\ {y_e} = y - {y_d} \end{array} \right. $

(3)

UUV的期望艏向角ψ_d完全由期望的路径得到

$ {\psi _d} = \arctan \left( {\frac{{{{y'}_d}\left( \varepsilon \right)}}{{{{x'}_d}\left( s \right)}}} \right) $

(4)

北东坐标系下的跟踪误差可以表示为

$ \mathit{\boldsymbol{E}} = {\mathit{\boldsymbol{R}}^{\rm{T}}}\left( \psi \right){\mathit{\boldsymbol{P}}_e} $

(5)

其中

$ {\mathit{\boldsymbol{P}}_e} = \mathit{\boldsymbol{P}} - {\mathit{\boldsymbol{P}}_d},{\psi _e} = \psi - {\psi _F} $

$ {\mathit{\boldsymbol{R}}^{\rm{T}}}\left( \psi \right) = \left[ {\begin{array}{*{20}{c}} {\cos \psi }&{\sin \psi }\\ { - \sin \psi }&{\cos \psi } \end{array}} \right] $

对式(5)求导，得路径跟踪误差方程为

$ \left\{ \begin{array}{l} {{\dot x}_e} = r{y_e} + u - {u_F}\cos {\psi _e}\\ {{\dot y}_e} = - r{x_e} + {u_F}\sin {\psi _e} + v \end{array} \right. $

(6)

同时有：

$ {{\dot \psi }_e} = r - {r_F} $

(7)

式中：u_F是虚拟目标点的速度，r_F是虚拟目标的角速度，ψ是UUV的艏向角，UUV的纵向速度为u，横向速度为v，艏摇角速度为r。

1.3 二阶滤波器设计

定义如式(8)所示的二阶滤波器，对输入信号β进行逼近：

$ \left\{ \begin{array}{l} {{\dot x}_1} = {x_2}\\ {{\dot x}_2} = - 2\zeta {w_n}{x_2} - {w_n}\left( {{x_2} - \mathit{\boldsymbol{\beta }}} \right) \end{array} \right. $

(8)

式中：x₁为控制输出量，0 < ζ≤1为阻尼比，w_n>0为自然频率。

令x₁=β^o，则β^o和${\mathit{\boldsymbol{\dot \beta }}^o}$均为经过滤波后的连续有界输出信号值，将输入信号β到输出信号β^o的转变过程表示成传递函数的形式：

$ G\left( s \right) = \frac{{{\mathit{\boldsymbol{\beta }}^o}\left( s \right)}}{{\mathit{\boldsymbol{\beta }}\left( s \right)}} = \frac{{w_n^2}}{{{s^2} + 2\zeta {w_n}s + w_n^2}} $

(9)

由式(9)可知，如果输入信号β的带宽低于G(s)，则误差信号|β-β^o|将会很小，假设输入信号β的带宽为已知，那么，只需要选取足够大且满足系统要求的自然角频率w_n，就能够获得输出信号β^o和其导数${\mathit{\boldsymbol{\dot \beta }}^o}$，且保证了逼近误差|β-β^o|的值很小。但是，自然角频率w_n选取过大^[14]会使系统更易受高频噪声的影响，因此需要选择一个较为合理的值，从而满足系统性能的需求。二阶滤波器的结构框图如图 1所示，从图中可以看出，输出信号β^o和其导数${\mathit{\boldsymbol{\dot \beta }}^o}$都是通过积分过程得到的，这种方法可弥补由非微分过程得到信号的不足，即可以将噪声对闭环控制系统的影响降到最低。

1.4 路径跟踪控制器设计

本文基于Lyapunov理论和反步法控制理论，同时引入1.3节设计的二阶滤波器，设计了领航者路径跟踪控制器。

构造Lyapunov函数为

$ {V_{xy}} = \frac{1}{2}x_e^2 + \frac{1}{2}y_e^2 $

(10)

对式(10)求导，并将(6)式代入可得

$ {{\dot V}_{xy}} = {x_e}\left( {u - {u_F}\cos {\psi _e}} \right) + {y_e}\left( {v + {u_F}\sin {\psi _e}} \right) $

(11)

纵向速度u和艏向角跟踪误差ψ_e的期望虚拟控制律可以分别设计为

$ {u_c} = - {k_1}{x_e} + {u_F}\cos {\psi _e} $

(12)

$ {\psi _c} = - \arcsin \left( {\frac{{{k_2}{x_e}}}{{\sqrt {1 + {{\left( {{k_2}{x_e}} \right)}^2}} }}} \right) $

(13)

式中k₁、k₂都是常数。

将式(12)、(13)代入式(11)得

$ {{\dot V}_{{\rm{xy}}}} = - {k_1}x_e^2 - {k_2}{u_F}\frac{1}{{\sqrt {1 + {{\left( {{k_2}{x_e}} \right)}^2}} }}x_e^2 + {x_e}y $

(14)

此处，令理想虚拟滤波控制输入向量为

$ {\mathit{\boldsymbol{\beta }}_c} = {\left[ {{x_{\rm{c}}},{y_c},{\psi _c},{u_c},{r_c}} \right]^{\rm{T}}} $

(15)

理想信号β_c^o的导数为

$ \mathit{\boldsymbol{\dot \beta }}_{\rm{c}}^o = {\left[ {\dot x_{ec}^o,\dot y_{ec}^o,\dot \psi _c^o,\dot u_c^o,\dot r_c^o} \right]^{\rm{T}}} $

(16)

利用滤波输出信号β_c^o和其导数值$\mathit{\boldsymbol{\dot \beta }}_{\rm{c}}^o$分别代替虚拟控制信号β_c以及导数值${{\mathit{\boldsymbol{\dot \beta }}}_{\rm{c}}}$，则位置滤波跟踪误差信号为

$ \left[ {\begin{array}{*{20}{c}} {{{\tilde x}_e}}\\ {{{\tilde y}_e}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{x_e} - x_{ec}^o}\\ {{y_e} - y_{ec}^o} \end{array}} \right] $

(17)

对式(17)两边求导，并代入式(14)整理得到

$ \left[ {\begin{array}{*{20}{c}} {{{\dot x}_e}}\\ {{{\dot y}_e}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{{\dot x}_{ec}}}\\ {{{\dot y}_{ec}}} \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} {{{\dot {\tilde x}}_e}}\\ {{{\dot {\tilde y}}_e}} \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} {\dot x_{ec}^o - {{\dot x}_{ec}}}\\ {\dot y_{ec}^o - {{\dot y}_{ec}}} \end{array}} \right] $

(18)

期望位置信号[x_ec, y_ec]^T选为

$ \left[ {\begin{array}{*{20}{c}} {{{\dot x}_{ec}}}\\ {{{\dot y}_{ec}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} { - {k_x}{{\tilde x}_e} + \dot x_{ec}^o}\\ { - {k_y}{{\tilde y}_e} + \dot y_{ec}^o} \end{array}} \right] $

(19)

整理可得

$ \begin{array}{*{20}{c}} {\left[ {\begin{array}{*{20}{c}} {{{\dot {\tilde x}}_e}}\\ {{{\dot {\tilde y}}_e}} \end{array}} \right] = \left[ \begin{array}{l} r{{\tilde y}_e} - {k_x}{{\tilde x}_e} + \dot x_{ec}^o - {{\dot x}_{ec}}\\ - r{{\tilde x}_e} - {k_y}{{\tilde y}_e} + \dot y_{ec}^o - {{\dot y}_{ec}} \end{array} \right] + }\\ {\left[ {\begin{array}{*{20}{c}} 1&{\frac{{\cos \psi _c^o\left( {\cos \tilde \psi - 1} \right){u_F} - \sin \psi _c^o\sin \tilde \psi {u_F}}}{{\tilde \psi }}}\\ 0&{\frac{{\sin \psi _c^o\left( {\cos \tilde \psi - 1} \right){u_F} + \cos \psi _c^o\sin \tilde \psi {u_F}}}{{\tilde \psi }}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {\tilde u}\\ {\tilde \psi } \end{array}} \right]} \end{array} $

(20)

为消除航向角误差，对${\tilde \psi }$求导，得到

$ \begin{array}{*{20}{c}} {\dot {\tilde \psi} = r - {r_F} - \dot \psi _c^o = }\\ {{r_e} + \left( {r_c^o - {r_c}} \right) + \tilde r - {r_F} - \dot \psi _c^o} \end{array} $

(21)

式中角速度跟踪误差定义为$\tilde r = r-r_c^o$。

设计虚拟角速度控制信号r_c为

$ {r_c} = {r_F} + \dot \psi _c^o - {k_\psi }\tilde \psi - {w_\psi } $

(22)

将式(22)代入式(21)得到

$ \dot {\tilde \psi} = - {k_\psi }\tilde \psi + \left( {r_c^o - {r_c}} \right) + \tilde r - {w_\psi } $

(23)

式中：k_ψ为正常数，w_ψ为待设计补偿项。

UUV平面路径跟踪控制器的设计为

$ \left\{ \begin{array}{l} {\tau _u} = {m_{11}}\left( { - {k_u}\tilde u - {k_{iu}} + \dot u_c^o - {\omega _u}} \right) - \\ \;\;\;\;\;\;{m_{22}}vr + {d_{11}}u - {\tau _{uE}}\\ {\tau _r} = {m_{33}}\left( { - {k_r}\tilde r - {k_{ir}}\tilde r + \dot u_c^o - {\omega _r}} \right) - \\ \;\;\;\;\;\left( {{m_{11}} - {m_{22}}} \right)uv + {d_{33}}r - {\tau _{rE}} \end{array} \right. $

(24)

式中：w_u和w_r分别为纵向控制输入和艏向力矩控制输入的反馈补偿项，k_iu和k_ir为积分项的增益, 引入积分项是为保证控制系统对噪声具鲁棒性。

将式(24)代入UUV数学模型中，可以得到纵向速度u和艏向角速度r的误差导数为

$ \left\{ \begin{array}{l} \dot {\tilde u} = - {k_u}\tilde u - {k_{iu}}\tilde u - {w_u}\\ \dot {\tilde r} = - {k_r}\tilde v - {k_{ir}}\tilde r - {w_r} \end{array} \right. $

(25)

1.5 滤波误差补偿回路设计

针对位置和艏向角进行滤波误差补偿设计，以保证跟踪精度。

令滤波信号位置和艏向角补偿误差为

$ \left[ {\begin{array}{*{20}{c}} {{v_x}}\\ {{v_y}}\\ {{v_\psi }} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{{\tilde x}_e} - {\zeta _x}}\\ {{{\tilde y}_e} - {\zeta _y}}\\ {\tilde \psi - {\zeta _\psi }} \end{array}} \right] $

(26)

其中，结合式(21)构造位置滤波补偿信号ζ_x、ζ_y和ζ_ψ如下

$ \begin{array}{*{20}{c}} {\left[ {\begin{array}{*{20}{c}} {{{\dot \zeta }_x}}\\ {{{\dot \zeta }_y}} \end{array}} \right] = \left[ \begin{array}{l} r{\zeta _y} - {k_x}{\zeta _x} + \dot x_{ec}^o - {{\dot x}_{ec}}\\ - r{\zeta _x} - {k_y}{\zeta _y} + \dot y_{ec}^o - {{\dot y}_{ec}} \end{array} \right] + }\\ {\left[ {\begin{array}{*{20}{c}} 1&{\frac{{\cos \psi _c^o\left( {\cos \tilde \psi - 1} \right){u_F} - \sin \psi _c^o\sin \tilde \psi {u_F}}}{{\tilde \psi }}}\\ 0&{\frac{{\sin \psi _c^o\left( {\cos \tilde \psi - 1} \right){u_F} + \cos \psi _c^o\sin \tilde \psi {u_F}}}{{\tilde \psi }}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{\zeta _{\rm{u}}}}\\ {{\zeta _\psi }} \end{array}} \right]} \end{array} $

(27)

$ {{\dot \zeta }_\psi } = - {k_\psi }{\zeta _\psi } + r_c^o - {r_c} + {\zeta _r} $

(28)

其中，初始条件为ζ_x(0)=0、ζ_y(0)=0和ζ_ψ(0)=0，ζ_r=0。

又由于ζ_u=ζ_r=0，令速度误差补偿信号为

$ \left\{ \begin{array}{l} {v_u} = \tilde u - {\zeta _u} = \tilde u\\ {v_r} = \tilde r - {\zeta _r} = \tilde r \end{array} \right. $

(29)

结合式(26)构造Lyapunov能量函数为

$ {V_1} = \frac{1}{2}v_x^2 + \frac{1}{2}v_y^2 + \frac{1}{2}v_\psi ^2 $

(30)

对式(30)求导，并将式(20)、(23)、(27)和(28)代入得

$ \begin{array}{*{20}{c}} {{{\dot V}_1} = - {k_x}v_x^2 - {k_y}v_y^2 - {k_\psi }v_\psi ^2 + {v_r}{v_\psi } - {w_\psi }{v_\psi } + {v_x}{v_u} + }\\ {\left[ {\frac{{\left( {\cos \tilde \psi - 1} \right)\cos \psi _c^o - \sin \tilde \psi \sin \psi _c^o}}{{\tilde \psi }} \cdot } \right.}\\ {\left. {\frac{{\left( {\cos \tilde \psi - 1} \right)\sin \psi _c^o + \sin \tilde \psi \cos \psi _c^o}}{{\tilde \psi }}} \right]\left[ {\begin{array}{*{20}{c}} {{v_x}}\\ {{v_y}} \end{array}} \right]{v_\psi }} \end{array} $

图 2给出了本文所设计控制器的系统框图，从图中可以看出，由期望位置和姿态构造的虚拟控制律加到二阶滤波控制器后，省去了求导过程，且达到了滤波输出的目的，同时为消除由滤波引起的误差，设计了滤波补偿回路，进而提高了系统的稳定性和跟踪性能。

2 编队控制器设计 2.1 坐标变换

为简化控制器的设计，定义三个误差变量e_x、e_y、e_ψ，分别为领航者与跟随UUV在x轴、y轴方向距离误差以及领航者与跟随UUV的航向角偏差，则位置误差和航向角误差在北东坐标系下可表示为

$ \left[ {\begin{array}{*{20}{c}} {{e_x}}\\ {{e_y}}\\ {{e_z}} \end{array}} \right] = - \left[ {\begin{array}{*{20}{c}} {\cos \psi }&{\sin \psi }&0\\ { - \sin \psi }&{\cos \psi }&0\\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{x_x} - x}\\ {{y_x} - y}\\ {{\psi _x} - \psi } \end{array}} \right] $

(31)

式中：[x_x, y_x, ψ_x]^T是领航者UUV在北东坐标系下的位置与姿态向量，[x, y, ψ]^T是跟随UUV在北东坐标系下的实际位置和姿态向量。

对式(31)两边求导并结合UUV水平面方程可得

$ \left\{ \begin{array}{l} {{\dot e}_x} = - {u_x} + u\cos {e_\psi } + v\sin {e_\psi } + {e_y}{r_x}\\ {{\dot e}_y} = - {v_x} + u\sin {e_\psi } + v\cos {e_\psi } + {e_x}{r_x}\\ {{\dot e}_\psi } = {r_x} - r \end{array} \right. $

(32)

综上所述，UUV水平面编队模型可以用方程形式表示为

$ \left\{ \begin{array}{l} {{\dot e}_x} = - {u_x} + u\cos {e_\psi } + v\sin {e_\psi } + {e_y}{r_x}\\ {{\dot e}_y} = - {v_x} + u\sin {e_\psi } + v\cos {e_\psi } + {e_x}{r_x}\\ {{\dot e}_\psi } = {r_x} - r\\ {m_{11}}\dot u - {m_{22}}vr + {d_{11}} = {\tau _u} + {\tau _{uE}}\\ {m_{22}}\dot v + {m_{11}}ur + {d_{22}}v = {\tau _{vE}}\\ {m_{33}}\dot r + \left( {{m_{22}} - {m_{11}}} \right)uv + {d_{33}}r = {\tau _r} + {\tau _{rE}} \end{array} \right. $

(33)

由上述描述可知，编队控制的目的可转换为各个UUV跟踪上领航者，基于此目的可知设计目标为：

1) 设计虚拟控制量u_α和v_α使跟随UUV与领航UUV的横向距离误差e_x→0和纵向距离误差e_y→0，即

$ \mathop {\lim }\limits_{t \to \infty } \left\| {{e_x}} \right\| = {l_x},\mathop {\lim }\limits_{t \to \infty } \left\| {{e_y}} \right\| = {l_y} $

(34)

式中l_x和l_y为跟随UUV与领航UUV的横向距离误差和纵向距离误差。

2) 跟随UUV在实际控制输入τ_u和τ_v的作用下，使得跟随UUV的实际纵向速度和实际横向速度分别满足u→u_α和v→v_α，即

$ \mathop {\lim }\limits_{t \to \infty } \left\| {u - {u_\alpha }} \right\| = 0,\mathop {\lim }\limits_{t \to \infty } \left\| {v - {v_\alpha }} \right\| = 0 $

(35)

2.2 控制器设计

为实现编队目标，分别从镇定跟踪误差和跟踪虚拟速度两部分来设计控制器以达到多UUV编队目的。镇定跟踪误差部分采用反步法，对e_x和e_y的设计转换为对变量z₁、z₂进行反步法控制器的设计，设计的过程中引入了横向速度虚拟控制量u_α和纵向速度虚拟控制量v_α，通过引入虚拟控制量可以使得UUV跟踪误差得到镇定。镇定虚拟速度误差部分采用了滑模控制法，分别从镇定纵向速度误差变量和横向速度误差变量出发，设计积分型滑模面，并对之求导，通过选取合适的纵向推力滑模控制率和转向力矩滑模控制率，以实现对速度跟踪误差的镇定。

2.2.1 跟踪误差控制器设计

为了简化设计，此处引入误差坐标变换，坐标变换为

$ \left\{ \begin{array}{l} {z_1} = {e_x}\cos {e_\psi } - {e_y}\sin {e_\psi }\\ {z_2} = {e_x}\sin {e_\psi } + {e_y}\cos {e_\psi } \end{array} \right. $

(36)

根据坐标变换，使$\mathop {\lim {\mathit{e}_\mathit{x}}}\limits_{t \to \infty } = 0$，$\mathop {\lim {\mathit{e}_\mathit{y}}}\limits_{t \to \infty } = 0$成立可以转换为使$\mathop {\lim {\mathit{z}_{\rm{1}}}}\limits_{t \to \infty } = 0$，$\mathop {\lim {\mathit{z}_{\rm{2}}}}\limits_{t \to \infty } = 0$成立，即镇定误差变量e_x和e_y可以转换为镇定变量z₁、z₂，所以接下来对z₁、z₂进行研究。

对式子(36)的左右两边进行求导得到

$ \left\{ \begin{array}{l} {{\dot z}_1} = u - {u_x}\cos {e_\psi } + {v_x}\sin {e_\psi } + {z_2}r\\ {{\dot z}_2} = v - {u_x}\sin {e_\psi } + {v_x}\cos {e_\psi } - {z_1}r \end{array} \right. $

(37)

令变量z₁和z₂的Lyapunov函数V₂为

$ {V_2} = \frac{1}{2}z_1^2 + \frac{1}{2}z_2^2 $

(38)

对V₂求导并将式(37)代入可得

$ \begin{array}{*{20}{c}} {{{\dot V}_2} = {z_1}\left( {u - {u_x}\cos {e_\psi } + {v_x}\sin {e_\psi } + {z_2}r} \right) + }\\ {{z_2}\left( {v - {u_x}\sin {e_\psi } - {v_x}\cos {e_\psi } - {z_1}r} \right)} \end{array} $

此处定义两个虚拟控制量分别为u_α和v_α，其中u_α为纵向速度u的虚拟控制量，v_α为横向速度v的虚拟控制量，设计虚拟控制量分别为

$ {u_\alpha } = \frac{{ - {\alpha _1}{z_1}}}{{\sqrt {z_1^2 + z_2^2 + L} }} + {u_x}\cos {e_\psi } - {v_x}\sin {e_\psi } $

(39)

$ {v_\alpha } = \frac{{ - {\alpha _2}{z_2}}}{{\sqrt {z_1^2 + z_2^2 + L} }} + {u_x}\sin {e_\psi } + {v_x}\cos {e_\psi } $

(40)

其中, a₁=0, a₂=0, L>0是要设计的参数。则将式(39)、(40)代入式(38)，并求导得

$ \begin{array}{*{20}{c}} {{{\ddot V}_2} = \frac{{2a_3^2\left( {z_1^2 + z_2^2} \right)}}{{z_1^2 + z_2^2 + L}} - \frac{{a_3^2\left( {z_1^2 + z_2^2} \right)}}{{{{\left( {z_1^2 + z_2^2 + L} \right)}^2}}} \le }\\ {2a_3^2 - a_3^2 = a_3^2} \end{array} $

由上式可以知${{\ddot V}_2}$是有界的，所以$\mathop {\lim {\mathit{z}_{\rm{1}}}}\limits_{t \to \infty } = 0$，$\mathop {\lim {\mathit{z}_{\rm{2}}}}\limits_{t \to \infty } = 0$成立，即$\mathop {\lim {\mathit{e}_\mathit{x}}}\limits_{t \to \infty } = 0$，$\mathop {\lim {\mathit{e}_{\rm{y}}}}\limits_{t \to \infty } = 0$，误差跟踪得到镇定。

2.2.2 速度滑模控制器设计

本节利用滑模控制法^[15-18]来设计纵向推力控制律和侧向运动控制律，即使得式(37)成立，具体设计如下：

1) 纵向推力滑模控制律设计

令UUV的纵向速度跟踪误差变量为

$ {u_e} = u - {u_\alpha } $

(41)

误差变量的滑模面为

$ {S_1} = {\lambda _1}\int_0^1 {{u_e}\left( \tau \right){\rm{d}}\tau } + {u_e},{\lambda _1} > 0 $

(42)

式中λ₁为正的常数，对式(42)求导得

$ {{\dot S}_1} = \frac{1}{{{m_{11}}}}\left( {{\lambda _1}{m_{11}}{u_e} + {m_{22}}vr - {d_{11}}u - {m_{11}}{{\dot u}^a} + {\tau _u} + {\tau _{uE}}} \right) $

为使${{\dot S}_1} = 0$，此处选取等效控制律为

$ {\tau _{u - eq}}\left( t \right) = - {\lambda _1}{{\hat m}_{11}}{u_e} - {{\hat m}_{22}}vr + {{\hat d}_{11}}u + {{\hat m}_{11}}{{\dot u}^a} $

(43)

式中：${{\hat m}_{11}}$、${{\hat m}_{22}}$和${{\hat d}_{11}}$分别为对不确定参数m₁₁、m₂₂和d₁₁的估计值，同时假设不确定参数的估计误差是存在上界的，即满足：

$ \left| {{m_{ii}} - {{\hat m}_{ii}}} \right| \le {M_{ii}},\left| {{d_{ii}} - {{\hat d}_{ii}}} \right| \le {D_{ii}},i = 1,2,3 $

(44)

选取Lyapunov函数V₃为

$ {V_3} = \frac{1}{2}{m_{11}}S_1^2 $

(45)

求V₃对时间t的导数可得：${{\dot V}_3} = {S_1}({\lambda _1}{m_{11}}{u_e} + {m_{22}}vr -{d_{11}}u -{m_{11}}{{\dot u}^a} + {\tau _u} + {\tau _{uE}})$设计纵向推力控制律为

$ {\tau _u} = {\tau _{u,{\rm{eq}}}} + {\tau _{u,{\rm{swit}}}} $

(46)

式中：τ_u,eq为等效控制律, τ_u,swit为切换控制律，并且选取切换控制律为

$ {\tau _{u,{\rm{swit}}}}\left( t \right) = - {F_1}{\mathop{\rm sgn}} \left( {{S_1}} \right) $

(47)

式中F₁为待设计的参数。

这样纵向推力滑模控制律变为

$ \begin{array}{*{20}{c}} {{\tau _u} = {\tau _{u,{\rm{eq}}}} + {\tau _{u,{\rm{swit}}}} = - {\lambda _1}{{\hat m}_{11}}{u_e} - {{\hat m}_{22}}vr + }\\ {{{\hat d}_{11}}u + {{\hat m}_{11}}{{\dot u}^a} - {F_1}{\mathop{\rm sgn}} \left( {{S_1}} \right)} \end{array} $

(48)

将τ_u代入式(47)得

$ \begin{array}{*{20}{c}} {{{\dot V}_3} = {S_1}\left( {{\lambda _1}{u_e}\left( {{m_{11}} - {{\hat m}_{11}}} \right) + \left( {{m_{22}} - {{\hat m}_{22}}} \right)vr + } \right.}\\ {\left. {\left( {{{\hat d}_{11}} - {d_{11}}} \right)u + \left( {{{\hat m}_{11}} - {m_{11}}} \right){{\dot u}^a} + {\tau _{uE}}} \right) - {F_1}\left| {{S_1}} \right|} \end{array} $

此处选取参数F₁为

$ \begin{array}{*{20}{c}} {{F_1} = {\lambda _1}{M_{11}}\left| {{u_e}} \right| + {M_{22}}\left| {vr} \right| + {D_{11}}\left| u \right| + }\\ {{M_{11}}\left| {{{\dot u}^a}} \right| + {\alpha _1} + {\tau _{uE,\max }}} \end{array} $

(49)

其中α₁>0，将F₁代入式中可得

$ \begin{array}{*{20}{c}} {{V_3} \le {S_1}\left( {{\lambda _1}{u_e}{M_{11}} + {M_{22}}vr + {D_{11}}u + {M_{11}}{{\dot u}^a} + {\tau _{uE}}} \right) - }\\ {\left( {{\lambda _1}{M_{11}}\left| {{u_e}} \right| + {M_{22}}\left| {vr} \right| + {D_{11}}\left| u \right| + {M_{11}}\left| {{{\dot u}^a}} \right| + } \right.}\\ {\left. {{\tau _{uE,\max }} + {\alpha _1}} \right)\left| {{S_1}} \right| \le - {\alpha _1}\left| {{S_1}} \right|} \end{array} $

由式(49)可知, 当且仅当S₁=0时，等号成立，等式(42)在式(48)以及式(49)的作用下，满足从任意位置状态点的滑模到达条件，即系统将渐进稳定于S₁=0处，也渐进稳定于u_e=0处，纵向速度误差镇定，纵向速度可以跟踪上虚拟纵向速度。

2) 侧向运动滑模控制律设计

定义UUV侧向速度跟踪误差变量为

$ {{\rm{v}}_e} = v - {v_\alpha } $

(50)

滑模面S₂设计如下

$ {S_2} = {{\dot v}_e} + {\lambda _2}{v_e} $

(51)

式中λ₂为正常数。对S₂求导可得

$ {{\dot S}_2} = \ddot v - {{\ddot v}^a} + {\lambda _2}\left( {\dot v - {{\dot v}^a}} \right) $

(52)

$ \begin{array}{*{20}{c}} {\ddot v = }\\ {\frac{{\left( { - {d_{22}}{m_{33}}\dot v - {m_{11}}{m_{33}}\dot ur - {m_{11}}u{\tau _r} + {m_{33}}{\tau _{vE}} - {m_{11}}u{\tau _{rE}}} \right)}}{{{m_{22}}{m_{33}}}} + }\\ {{m_{11}}{d_{33}}ur + \frac{{{m_{11}}\left( {{m_{22}} - {m_{11}}} \right){u^2}v}}{{{m_{22}}{m_{33}}}}} \end{array} $

(53)

对式(40)两次求导可得

$ \begin{array}{*{20}{c}} {{{\ddot v}_\alpha } = Z\dot r + {{\dot b}_2}}\\ {{b_2} = \left( {{a_1}{u_x}{{\left( {\sqrt {z_1^2 + z_2^2 + L} } \right)}^{ - 1}} + {{\dot u}_x} - {v_x}{r_x}} \right)\sin {e_\psi } + }\\ {\left( {{a_1}{v_x}{{\left( {\sqrt {z_1^2 + z_2^2 + L} } \right)}^{ - 1}} + {{\dot v}_x} - {u_x}{r_x}} \right)\cos {e_\psi } - }\\ {\left( {{{\left( {\sqrt {z_1^2 + z_2^2 + L} } \right)}^{ - 1}}{a_1}v + {{\left( {\sqrt {z_1^2 + z_2^2 + L} } \right)}^{ - 4}} \cdot } \right.}\\ {\left. {{a_2}{z_2}\left( {{a_1}z_1^2 + {a_2}z_2^2} \right)} \right)} \end{array} $

(54)

$ \begin{array}{*{20}{c}} {Z = - {u_\alpha } + \frac{{{z_1}\left( {{a_2} - {a_1}} \right)}}{{\sqrt {z_1^2 + z_2^2 + L} }}}\\ {{b_1} = \left( {{a_1}{u_x}{{\left( {\sqrt {z_1^2 + z_2^2 + L} } \right)}^{ - 1}} + {{\dot u}_x} - {v_x}{r_x}} \right)\cos {e_\psi } - }\\ {\left( {{a_1}{v_x}{{\left( {\sqrt {z_1^2 + z_2^2 + L} } \right)}^{ - 1}} + {{\dot v}_x} - {u_x}{r_x}} \right)\sin {e_\psi } - }\\ {\left( {{{\left( {\sqrt {z_1^2 + z_2^2 + L} } \right)}^{ - 1}}{a_1}u + {{\left( {\sqrt {z_1^2 + z_2^2 + L} } \right)}^{ - 4}} \cdot } \right.}\\ {\left. {{a_1}{z_1}\left( {{a_1}z_1^2 + {a_2}z_2^2} \right)} \right)}\\ {\dot r = {\tau _r} - {d_{33}} - \frac{{\left( {{m_{22}} - {m_{11}}} \right)uv}}{{{m_{33}}}}} \end{array} $

(55)

将式(39)、(40)代入式(51)可得

$ {{\dot S}_2} = \frac{{{n_1}{\tau _r} - {f_3} + {m_{33}}{\tau _{uE}} + {n_1}{\tau _{rE}}}}{{{m_{22}}{m_{33}}}} $

(56)

其中，

$ {n_1} = - \left( {{m_{22}}Z + {m_{11}}u} \right) $

(57)

选取f₃为

$ \begin{array}{*{20}{c}} {{f_3} = {m_{22}}Z\left( {\left( {{m_{11}} - {m_{22}}} \right)uv - {d_{33}}r} \right) - }\\ {{m_{22}}{m_{33}}\left( {r\dot Z + {{\dot f}_2} + {\lambda _2}\left( {\dot v - {{\dot v}^a}} \right)} \right) + {d_{22}}{m_{33}}\dot v + }\\ {{m_{11}}{m_{33}}\dot ur - {m_{11}}{d_{33}}ur - {m_{11}}\left( {{m_{22}} - {m_{11}}} \right){u^2}v} \end{array} $

(58)

为使${{\dot S}_2} = 0$, 可选取等效控制律为

$ {\tau _{r,eq}} = \frac{{{{\hat f}_3}}}{{{{\hat b}_1}}} $

(59)

其中${{\mathit{\hat b}}_1}$是b₁参数的估计值，${{\mathit{\hat f}}_3}$是参数f₃的估计值。同样假设参数b₁和参数f₃的估计值与参数本身之间的误差是有界的，即

$ \left| {{b_1} - {{\hat b}_1}} \right| \le E,\left| {{f_3} - {{\hat f}_3}} \right| \le H $

(60)

定义Lyapunov函数为V₄:

$ {V_4} = \frac{1}{2}{m_{22}}{m_{33}}S_2^2 $

(61)

则对之求导可得

$ \begin{array}{*{20}{c}} {{{\dot V}_4} = {m_{22}}{m_{33}}{S_2}{{\dot S}_2} = }\\ {{m_{22}}{m_{33}}{S_2}\frac{{{n_1}{\tau _r} - {f_3} + {m_{33}}{\tau _{vE}} + {n_1}{\tau _{rE}}}}{{{m_{22}}{m_{33}}}} = }\\ {{S_2}\left( {{n_1}{\tau _r} - {f_3} + {m_{33}}{\tau _{vE}} + {n_1}{\tau _{rE}}} \right)} \end{array} $

(62)

同理转向力矩的滑模控制率可设计为

$ {\tau _r} = {\tau _{r,eq}} + {\tau _{r,{\rm{swit}}}} $

(63)

切换控制律选为

$ {\tau _{r,{\rm{swit}}}}\left( t \right) = - \frac{{{F_2}{\mathop{\rm sgn}} \left( {{S_2}} \right)}}{{{{\hat b}_1}}} $

(64)

其中F₂为待设计的参数。得

$ {\tau _r} = {\tau _{r,{\rm{eq}}}} + {\tau _{r,{\rm{swit}}}} = \frac{{{f_3} - {F_2}{\mathop{\rm sgn}} \left( {{S_2}} \right)}}{{{{\hat b}_1}}} $

(65)

将式(65)代入式(62)可得

$ \begin{array}{*{20}{c}} {{{\hat V}_4} = {S_2}\left( {{n_1}\frac{{{{\hat f}_3} - {F_2}{\mathop{\rm sgn}} \left( {{S_2}} \right)}}{{{{\hat n}_1}}} - {f_3} + {m_{33}}{\tau _{vE}} + {n_1}{\tau _{rE}}} \right) = }\\ {{S_2}\left( {\left( {\frac{{{n_1}}}{{{{\hat n}_1}}} - 1} \right){{\hat f}_3} - \frac{{{n_1}{F_2}{\mathop{\rm sgn}} \left( {{S_2}} \right)}}{{{{\hat n}_1}}} + {{\hat f}_3} - {f_3} + } \right.}\\ {\left. {{n_1}{\tau _{rE}} + {m_{33}}{\tau _{vE}} - {n_1}\frac{{{F_2}{\mathop{\rm sgn}} \left( {{S_2}} \right)}}{{{{\hat n}_1}}}} \right)} \end{array} $

(66)

控制参数F₂满足等式：

$ \begin{array}{*{20}{c}} {{F_2} = B\left( {H + {\alpha _2} + {n_1}{\tau _{rE,\max }} + {M_{3i}}{\tau _{vE,\max }}} \right) + }\\ {\left( {B - 1} \right)\left| {{{\hat f}_3}} \right|} \end{array} $

(67)

其中α₂>0，B为$b{\hat b^{ - 1}}$的上界，即$b{\hat b^{ - 1}}$≤B，代入上式可得

$ {{\dot V}_4} \le - {\alpha _2}\left| {{S_2}} \right| $

(68)

由式(68)可知，当且仅当S₂=0时等号成立，式(52)在式(65)、(67)的作用下，满足从任意位置状态点的滑模到达条件，即系统将渐进稳定于S₂=0处，也渐进稳定于v_e=0处，所以横向速度误差镇定，横向速度可以跟踪上虚拟纵向速度。

由以上讨论可知对于由多个UUV组成的多UUV群体，在领航者满足式(24)，跟随UUV满足式(48)、(65)的情况下，多UUV可以以一定的队形编队前进。

3 仿真实验结果与分析

针对本文所设计的基于滤波反步法的领航者路径跟踪控制器以及所设计的编队控制器进行仿真验证，假设做编队任务的有三个UUV，领航UUV与跟随UUV模型相同，初始条件为

$ {\left[ {{x_1}\left( 0 \right),{y_1}\left( 0 \right),{\psi _1}\left( 0 \right)} \right]^{\rm{T}}} = {\left[ {100,0.3,{{9.0}^ \circ }} \right]^{\rm{T}}}, $

$ {\left[ {{x_2}\left( 0 \right),{y_2}\left( 0 \right),{\psi _2}\left( 0 \right)} \right]^{\rm{T}}} = {\left[ {85, - 27,{{9.0}^ \circ }} \right]^{\rm{T}}}, $

$ {\left[ {{x_3}\left( 0 \right),{y_3}\left( 0 \right),{\psi _3}\left( 0 \right)} \right]^{\rm{T}}} = {\left[ {135, - 13.5,{{171}^ \circ }} \right]^{\rm{T}}} $

控制器参数为a₁=5, a₂=1, L=0.1, λ₁=1, λ₂=1。M_ii和D_ii的大小分别为相应参数的5%，l_x=15，l_y=27。加入的扰动为高斯白噪声。α₁=1×10⁴，α₂=1×10⁴，领航者期望速度为0.63 m/s。仿真结果如图 3~6所示。选取领航者的期望路径为圆曲线，相关控制参数为为k_x=1.5，k_y=1.8，k_u=20，k_ψ=15，k_r=20，k_iu=10，k_ir=10，p₂₁=10，p₂₂=10，滤波器参数为ζ=0.9，w_n=20。

图 3为UUV编队航迹图，设计的路径跟踪路线是圆曲线，编队队形为三角形，从图 3可以看出，各UUV的初始分布不具规律性，当领航者开始运动时，领航者随之运动并很快跟踪上领航者并形成期望的编队队形，然后保持此队形完成航行任务。图 4为各UUV的艏向角曲线，初始时各UUV的艏向角不同，经过一定时间航行后，各个UUV的艏向角相等并一直保持，符合所走的圆形轨迹，达到了期望的编队运动效果。图 5是各UUV的横向速度和纵向速度曲线，从图中可以看出起始时刻的速度震荡较明显，一段时间之后所有UUV的期望速度收敛到一致，图中的速度变化有一些小的毛刺，但是波动较小，说明所设计的控制器具有良好的鲁棒性。图 6为每个UUV的横坐标和纵坐标曲线，仿真结果表明和设计的圆曲线路径一致。

4 结论

1) 利用滤波反步法设计路径跟踪控制器，同时引入二阶滤波器，简化求导的计算量。

2) 引入滤波跟踪误差补偿回路，提高了路径跟踪精度。

3) 利用反步法设计位置跟踪误差，推导出UUV纵向速度虚拟控制律和横向速度虚拟控制律，该虚拟控制律使位置跟踪误差收敛到零。

4) 采用滑模控制方法对速度跟踪误差进行设计，对纵向速度误差设计积分滑模面，推导出纵向推力。同理可将UUV横向速度与横向虚拟速度误差作为对象进行镇定，设计转向力矩滑模控制律使得横向速度误差镇定。所设计的控制器考虑到了外界环境扰动对UUV的影响，并对扰动进行了抑制，增强了整个系统的鲁棒性。最后的仿真结果证明了它的有效性，并且对外界扰动具有良好的鲁棒性。