0 引　言

多自主水下航行器（Autonomous Underuater Vehicle, AUV）系统在作业效率、经济性方面相较单AUV系统具有很大优势，是近年来AUV装备的研究热点之一。目前已经有多种AUV系统编队控制方法实际应用，如基于领航者-跟随者的编队控制^{[1 − 3]}、基于虚拟结构的编队控制^[4]、基于势函数的编队控制^[5]、基于一致性的编队控制^{[4 − 6]}、基于行为的编队控制^[7]、基于路径跟随的编队控制^{[7 − 9]}等。其中，基于领航者-跟随者的编队控制方法由于自身简洁实用的特点，被广泛应用。

基于领航者-跟随者的编队控制中，领航AUV按期望路径航行，跟随AUV根据设定的相对领航AUV的位置关系，执行点跟随航行即可实现编队控制。领航AUV切换航向时，跟随AUV跟踪的目标点位置快速变化，位于切换航向内侧的跟随者需减速，位于外侧的需加速，以适应切换点处航行距离差异导致的队形保持问题。该方法的关键是获取领航者的位置和航向信息及在路径切换出的队形保持。

相较于通过水声通信设备获取领航AUV的目标和航向信息，本文聚焦于AUV编队的队形保持问题，提出基于B样条曲线对规划航路切换点处做局部B样条拟合优化，平滑领航AUV在路径切换时的理想航向角过渡，一定程度改善多AUV编队的队形保持难度。在此基础上，通过数值仿真开展基于领航跟随编队控制方法在不同AUV平台上的控制效果研究。为突出对比，本文选用500 kg的“海翔500”AUV与 5000 kg的“NPS”AUV进行对比。AUV对边采用3节点编队航行，对比分析了基于领航跟随编队控制方法下的2种AUV的队形距离偏差、队形角度偏差。

1 坐标系定义及模型建立 1.1 AUV模型建立

大地坐标系为$ E-\xi \eta \zeta $、随体坐标系为$ b-xyz $，两坐标系原点重合，$ E-\xi \eta \zeta $坐标系依次绕z-y-x轴转动$ \psi 、\theta 、\varphi $后与$ b-xyz $坐标轴重合。AUV的位置及姿态用$ \mathit{\eta }= {\left[n,e,d,\varphi ,\theta ,\psi \right]}^{{\mathrm{T}}} $表示，运动状态用$ \mathit{v}={\left[u,v,w,p,q,r\right]}^{{\mathrm{T}}} $表示。$ n、e、d $为在$ E-\xi \eta \zeta $坐标系下沿北、东、向下指向的位置，$ \varphi、\theta、\psi $为相应的欧拉角（滚转、俯仰和偏航）；$ u、v、w、p、q、r $为AUV在随体坐标系$ b-xyz $下的广义速度。

AUV的运动学方程可表示为：

$ \dot{\mathit{\eta }}=\mathit{J}\left(\mathit{\eta }\right)\mathit{v} 。$

(1)

式中：$ \mathit{J}\left(\mathit{\eta }\right)=\left[\begin{array}{cc}{\boldsymbol{R}}_{b}^{n}\left(\mathrm{\varTheta }\right)& {0}_{3\times 3}\\ {0}_{3\times 3}& {\boldsymbol{T}}_{\mathrm{\Theta }}\left(\mathrm{\varTheta }\right)\end{array}\right] $，

$ \begin{aligned} {\boldsymbol{R}}_{b}^{n}\left(\mathrm{\varTheta }\right)=&{\mathit{R}}_{z,\psi }{\mathit{R}}_{y,\theta }{\mathit{R}}_{x,\varphi } = \\ &\left[\begin{array}{ccc}\cos\psi \cos\theta & -\sin\psi \cos\varphi +\cos\psi \sin\theta \sin\varphi \\ \sin\psi \cos\theta & \cos\psi \cos\varphi +\sin\psi \sin\theta \sin\varphi \\ -\sin\theta & \cos\theta \sin\varphi \end{array}\right. \\& \left.\begin{array}{ccc} \sin\psi \sin\varphi +\cos\psi \sin\theta \cos\varphi \\ -\cos\psi \sin\varphi +\sin\psi \sin\theta \cos\varphi \\ \cos\theta \cos\varphi \end{array}\right],\end{aligned}$

$ \begin{aligned} &{\mathit{T}}_{\mathrm{\varTheta }}\left(\mathrm{\varTheta }\right)=\left[\begin{array}{ccc}1& \sin\varphi \tan\theta & \cos\varphi \tan\theta \\ 0& \cos\varphi & -\sin\varphi \\ 0& \sin\varphi /\cos\theta & \cos\varphi /\cos\theta \end{array}\right], \\ &\theta \ne \pm 9{0}^{{\mathrm{o}}}，{\mathit{T}}_{\mathrm{\varTheta }}^{-1}\left(\mathrm{\varTheta }\right) = \left[ \begin{array}{ccc}1& 0& -\sin\theta \\ 0& \cos\varphi & \cos\theta \sin\varphi \\ 0& -\sin\varphi & \cos\theta \cos\varphi \end{array} \right],\theta \ne \pm 9{0}^{{\mathrm{o}}} 。\end{aligned}$

动力学方程可表示为：

$ \boldsymbol{M}\dot{v}+\boldsymbol{C}\left(v\right)v+\boldsymbol{D}\left(v\right)v+\mathit{g}\left(\eta \right)=\mathit{\tau } 。$

(2)

式中：$ \boldsymbol{M} $为系统的惯性矩阵（包含附加水质量）；$ \boldsymbol{C}\left(v\right) $为科里奥利向心力矩阵(包含附加水质量)；$ \boldsymbol{D}\left(v\right) $为阻尼矩阵；$ \mathit{g}\left(\eta \right) $为重力、浮力的力和力矩向量；$ \mathit{\tau } $为控制输入向量。

“海翔500”AUV为中国船舶科学研究中心研发的重490 kg、长3.7 m的中小型AUV。“NPS”AUV为美国海军研究生院研发的重 5450 kg、长5.3 m的中大型AUV。2型AUV均采用尾部双推进器、扁平体结构，在操纵特性上类似，以该2型平台开展领航跟随编队控制方法在不同重量AUV下编队队形控制效果比较研究较为合理。“海翔500”AUV和“NPS”AUV的数学模型详见文献[10，11]，其外形如图1所示。

1.2 领航跟随编队控制算法

基于领航者-跟随者的编队控制中，一般领航AUV执行路径跟随任务，路径跟随任务一般通过组成路径的路径点序列发送给领航者AUV。跟随者AUV通过接收领航者AUV的位置和航向，并根据队形设定计算跟踪位置点，进行点跟综，从而实现编队控制。一般将AUV编队控制限定在水平面内，路径由路径点序列P表示：

$ \mathit{P}=\left[{P}_{1},{P}_{2},\cdots {P}_{i},{P}_{i+1},{P}_{i+2},\cdots {P}_{n}\right],i=\mathrm{1,2},...,n-2。$

跟随者AUV位置记为$ {P}_{f} $，其与领航者AUV的相对位置用$ \left(\rho ,\theta \right) $表示，领航者的航向用$ {\phi }_{a} $表示。则跟随者的目标跟踪点位置$ {p}_{f}^{d} $为：

$ {p}_{f}^{d}={p}_{a}+{R}^{bo}\left({\phi }_{a}\right)\left[\begin{array}{c}\rho {\cos}\left(\theta \right)\\ \rho {\sin}\left(\theta \right)\end{array}\right]。$

式中：$ {R}^{bo}\left({\phi }_{a}\right)=\left[\begin{array}{cc}{\cos}{\phi }_{a}& -{\sin}{\phi }_{a}\\ {\sin}{\phi }_{a}& {\cos}{\phi }_{a}\end{array}\right] $为领航AUV机体坐标系到大地坐标系的转换矩阵。

领航者AUV采用改进视线法进行路径跟随任务时，在转换点前领航AUV的理想航向角为$ {\phi }_{a}^{d}= {\rm{arctan}}\left(\dfrac{{{\mathit{y}}}_{i+1}-\mathit{y}_{i}}{\mathit{x}_{i+1}-\mathit{x}_{i}}\right)+{\rm{arctan}}\left(\dfrac{{-e}_\mathit{n}}{\Delta }\right) $。转换点后，领航AUV的理想航向角为$ {\phi }_{a}^{d}={\rm{arctan}}\left(\dfrac{\mathit{y}_{i+2}-\mathit{y}_{i+1}}{\mathit{x}_{i+2}-\mathit{x}_{i+1}}\right)+{\rm{arctan}}\left(\dfrac{{-e}_{n}}{\Delta }\right) $。其中，$ {e}_{n} $为领航者位置距离设计路径的法向距离，$ \Delta $一般取3～5倍AUV的长度。由于切换航向，路径切换点处的跟踪AUV的理想跟踪点位置发生跳变，简化路径偏差为0，可推导得到跳变距离：

$ \left|{\widehat{p}}_{f}^{d}-{p}_{f}^{d}\right|={\rho }_{k}\sqrt{2-2\rm{cos}{\alpha }_{i+1}}。$

式中：$ {p}_{f}^{d} $、$ {\widehat{p}}_{f}^{d} $为路径切换前后跟随者的目标跟踪点；$ {\rho }_{k} $为队形设计时跟随者与领航者的固定偏移距离；$ {\alpha }_{i+1} $为切换前后路径的夹角。可知切换路径的夹角较小时，跟踪AUV的理想跟踪点位置变化距离较小，跟随者所需调整的航行距离较小，较易保持编队队形。在路径夹角较大时，除了AUV机动性不足的航向超调外，跟随AUV的理想跟踪点位置变化较大也是影响AUV编队队形保持的重要影响因素。

2 数值仿真 2.1 路径跟随仿真比较

考虑AUV在经济航速2 kn下完成给定路径的编队航向任务。2型AUV的航向控制均采用比例微分控制，深度和纵倾采用滑膜分步控制，最大舵角限制30°。基于改进视线法进行路径跟随。航行路径点序列设置为（单位m）：

$ \begin{aligned} &{P}_{1}=\left(\mathrm{0,0}\right),{P}_{2}=\left(\mathrm{1450,0}\right),{P}_{3}=\left(\mathrm{1450,1300}\right),\quad\quad\ \ \\ &{P}_{4}=\left(\mathrm{500,1300}\right),{P}_{5}=\left(\mathrm{500,500}\right), \end{aligned}$

$ \begin{aligned}&{P}_{6}=\left(\mathrm{0,500}\right),{P}_{7}=\left(-\mathrm{500,1800}\right),{P}_{8}=\left(\mathrm{1000,1800}\right),\\ &{P}_{9}=\left(\mathrm{2000,1800}\right),{P}_{10}=\left(\mathrm{2500,180}\right) 。\end{aligned} $

仿真时间 6000 s，航行距离约6.2 km，仿真可得2型AUV在路径跟随任务中的航行轨迹如图2所示，其航向及法向偏差距离时间历程图如图3所示。

从仿真结果可知2型AUV均具备较好的路径跟随能力，稳定航行段的法向偏差距离控制在1 m以内。切换点处设定了路径切换距离20 m，计算法向距离的参照路径发生变化。实际的最大法向偏差距离从图2局部放大图可以看出在10 m左右。其中“NPS”AUV在设定参数下航向调整的超调量更大，调整时间较长，但从路径偏差距离上比较，两者在设定参数下的路径跟随性能比较接近。

2.2 基于领航跟随控制的“海翔500”AUV编队仿真

设定2个跟随AUV与领航者AUV的相对位置分别为($ {\rho }_{1} $,$ {\theta }_{1} $)$ =\left(\mathrm{100,13}{5}^\circ\right) $、($ {\rho }_{2} $,$ {\theta }_{2} $)$ =\left(100,-13{5}^{\circ}\right) $，编队航行速度2 kn，仿真时间 6000 s，跟随AUV与领航AUV距离偏差大于10 m时跟随者调整速度以保持队形，得到领航AUV与跟随AUV的航行轨迹如图4所示。跟随AUV与领航AUV距离偏差及角度偏差时间历程如图5所示。

由图4可知，在切换点处AUV编队队形的保持较差，但在内外侧AUV的航速调整的基础上很快能恢复编队队形。由图5可知在切换点转弯内侧的AUV最大距离偏差13 m，而转弯外侧的AUV与领航AUV的相对距离偏差最大达到35 m附近。在稳定段，“HX500”AUV编队中跟随AUV与领航AUV的距离偏差保持在8 m左右（小于判断距离10 m），角度偏差保持在5°以内。

2.3 基于领航跟随控制的NPS AUV编队仿真

设定2个跟随AUV与领航者AUV的相对位置分别为($ {\rho }_{1} $,$ {\theta }_{1} $)$ =\left(\mathrm{100,13}{5}^{\circ}\right) $、($ {\rho }_{2} $,$ {\theta }_{2} $)$ =\left(100,-13{5}^{\circ}\right) $，编队航行速度2 kn，仿真时间 6000 s，跟随AUV与领航AUV距离偏差大于10 m时跟随者调整速度以保持队形，得到领航AUV与跟随AUV的航行轨迹如图6所示。跟随AUV与领航AUV距离偏差及角度偏差时间历程如图7所示。

由图6可知，在切换点处AUV编队队形的保持较差，可以通过内外侧AUV的航速调整可恢复队形。由图7可知，转弯外侧的AUV与领航AUV的相对距离偏差最大达到50 m。在稳定段，“NPS”AUV编队中跟随AUV与领航AUV的距离偏差保持在8 m左右（小于10 m），角度偏差保持在5°以内。

2.4 不同AUV编队的队形保持效果对比

通过上述基于领航跟随的编队控制仿真，“海翔500”AUV编队和“NPS”AUV编队虽然个体平台在重量、尺寸上具有较大差异，但在相同的控制策略下均能较好实现地实现AUV路径跟随和编队航行任务。在路径跟随任务上，“NPS”AUV由于惯性大等缘故，航向调整超调大、时间较长，但从路径偏差距离上比较2型AUV路径跟随性能比较接近。

编队航行任务上，“HX500”与“NPS” AUV编队中1号跟随者、2号跟随者与领航者的距离偏差、角度偏差时间历程对比如图8和图9所示。可知在2型AUV在路径跟随任务上性能接近，但在基于领航跟随编队控制实现编队航行时，更大重量和体积的“NPS”AUV编队的队形保持效果明显比“HX500” AUV编队的队形保持效果差。

3 结　语

本文基于领航跟随编队控制方法，采用500 kg的“海翔500”AUV与 5000 kg的“NPS”AUV开展AUV编队航行仿真研究，对领航跟随编队控制方法在不同AUV平台上的控制效果进了研究，对比分析了基于领航跟随编队控制方法下的2型AUV在队形距离偏差、队形角度偏差。结论如下：

1）500 kg的“海翔500”AUV与 5000 kg的“NPS”AUV在路径跟随上的航行能力接近，且基于领航跟随的编队控制方法对500 kg与5000 kg级别的AUV编队均具有较好的控制效果，稳定航行段跟随者与领航者的距离偏差保持在10 m以内，角度偏差保持在5°以内。

2）基于领航跟随的编队控制方法在切换点处，由于队形相对位置的约束，对平台的加减速性能有一定要求，重量尺寸更大的NPS AUV在直角拐弯处跟随者与领航者的距离偏差较“海翔500”AUV增大50%左右。