人工势场和虚拟结构相结合的多水下机器人编队控制

潘无为， 姜大鹏， 庞永杰， 李岳明， 张强

(哈尔滨工程大学 水下机器人技术重点实验室， 黑龙江 哈尔滨 150001)

人工势场和虚拟结构相结合的多水下机器人编队控制

潘无为， 姜大鹏， 庞永杰， 李岳明， 张强

(哈尔滨工程大学 水下机器人技术重点实验室， 黑龙江 哈尔滨 150001)

传统的多水下机器人(AUV)编队算法，例如领航跟随法、虚拟结构法，对编队形成过程中AUV间的避碰问题和编队行进过程中的避障问题，没有进行有效解决。人工势场法可利用势函数进行避碰、避障，但队形组织能力略显不足。针对上述问题，提出了一种人工势场和虚拟结构相结合的多AUV编队控制算法。将系统分为3个部分：编队参考点、虚拟结构质点和AUV. 以编队参考点为中心形成期望的虚拟结构以组织队形；虚拟结构质点以期望的虚拟结构为运动目标，并在运动的过程中，通过人工势场斥函数，实现避碰和避障；AUV对虚拟结构质点进行目标跟踪，从而渐进形成AUV的队形。通过编队路径跟踪、编队队形变换和编队避障等一系列仿真实验，对算法的可靠性和灵活性进行充分验证。实验结果表明：在随机的初始位置条件下，多AUV系统可以快速无碰撞地形成队形；在编队行进过程中进行灵活的队形变换，并对障碍物有效避碰。

兵器科学与技术； 多水下机器人； 编队控制； 人工势场； 虚拟结构

0 引言

多水下机器人(AUV)系统，是指由多个AUV组成的系统，通过协作完成诸如区域搜索、地形扫描、环境数据采集等任务。编队问题作为多AUV系统的重要方面，日渐引起了众多学者的关注。目前采用较多的编队算法包括：领航跟随法[1]、虚拟结构法[2-3]、人工势场法[4-5]和基于行为法[6-7]。

领航跟随法采用链式的拓扑结构，编队具有一个全局领航者和若干子领航者，跟随者以一定的位置偏移跟踪领航者，从而形成队形。例如，Cui等[1]提出了参考轨迹和虚拟AUV的概念，采用反步法使AUV的运动状态收敛至虚拟AUV的状态，从而形成队形。虚拟结构法要求机器人以刚体上的相应点的位置和姿态信息作为各自的跟踪目标形成队形。例如Ren[2]采用信息一致性算法，提出了一种分布式虚拟结构的多智能体编队算法；赵宁宁等[3]在Serret-Frenet坐标系下采用AUV曲线路径跟踪算法，得到了多AUV编队路径跟踪的算法，实际上是以曲线参考点形成虚拟结构，AUV在跟踪虚拟结构的过程中形成编队。领航跟随法和虚拟结构法的缺点在于，算法虽然对编队的保持问题进行了研究，但对于编队的形成问题一般未做研究，例如在编队形成和编队变换过程中AUV之间的避碰问题。另外，算法针对编队行进过程中的避障问题，研究也很不充分。

人工势场法，通过设置势场函数，AUV在势场的吸引力、排斥力作用下，得到相应的队形。AUV之间的吸引力用于队形的自组织，目标点对AUV的吸引力可使AUV驶向目标，排斥力则用于AUV之间的避碰和躲避障碍物。人工势场法的缺点在于，队形的变化需要对势场函数的重新配置，因而不够灵活。例如，Leonard等[4]采用虚拟智能体和人工势场相结合的方法进行多智能体的仿真，需要通过调整虚拟智能体的位置以改变势场形状，进而控制智能体的队形；Cifuentes等[5]将人工势场布置成“沟槽”的形状，机器人运动到“沟槽”中形成队形，不同队形的变化需要不同形式的“沟槽”。由此可见采用人工势场进行队形的布置实际上并不简便。另外，人工势场法需要考虑如何将势场力转化为面向具体机器人模型可用的控制量。

针对以上不足，本文提出了一种人工势场和虚拟结构相结合的多AUV编队控制算法，并做了以下工作：

1)面向多AUV的实际应用，将系统分解为3个部分：编队参考点、虚拟结构质点和AUV. 编队参考点负责队形组织；虚拟结构质点负责避碰和避障，并作为AUV的跟踪目标；AUV则是编队的最终实体。

2)设计人工势场函数，实现避碰和避障。采用向障碍物边界投影的方法，通过投影点设置人工势场。从而将避障和避碰方法归于统一。

3)采用AUV目标跟踪算法，跟踪虚拟结构质点，使得多AUV系统最终收敛于期望的队形。

4)通过多AUV路径跟踪，队形变换和编队避障等一系列仿真实验，验证了该算法的可靠性和稳定性。

1 问题描述

1.1 坐标系

为了描述问题，共建立了3个坐标系(见图1)，分别是大地坐标系OnNE({n})、随体坐标系Obxy({b})和目标坐标系Otxtyt({t})。 {n}系平面为AUV所在区域地球表面的切平面，原点On为切点，OnN轴指向北，OnE轴指向东。认为{n}系为惯性坐标系，AUV在{n}系平面内运动。{b}系原点Ob位于AUV的重心，Obx轴与艇体纵向轴线重合，指向艇艏，Oby轴为Obx轴顺时针旋转90°所得。{t}系原点Ot位于目标点处，Otxt轴指向与目标点速度向量相同，Otyt轴为Otxt轴顺时针旋转90°所得。

图1 {n}、{b}和{t}坐标系Fig.1 Reference frames {n}, {b} and {t}

1.2AUV模型

对于在水平面运动的AUV，其运动学和动力学模型[8]可表示为

(1)

2 多AUV编队系统设计

如图2所示：黑色圆点表示编队参考点，周围的浅色圆点表示期望的虚拟结构；深色圆点表示虚拟结构质点。编队参考点作为编队构成的中心，在其周围形成期望的虚拟结构队形，作为虚拟结构质点的期望位置。在初始时刻，多AUV在一定的范围内随机分布，虚拟结构质点与AUV的初始位置相同，并向期望的虚拟结构运动。在运动过程中，以虚拟结构质点为中心，设置人工势场，从而实现避碰和避障。AUV以虚拟结构质点作为跟踪目标，在目标跟踪算法的控制下，最终实现自身的队形。

图2 多AUV编队系统示意图Fig.2 Schematic diagram of multi-AUV formation system

对于第i个AUV，其运动学层面上的控制目标为，通过给定AUV的期望艏向和期望航速，使AUV的位置收敛至期望的编队位置，使多AUV系统形成稳定的编队队形，并在此过程中实现避碰和避障，保持必要的安全距离。

2.1 虚拟结构的渐进生成

虚拟结构质点模型为

(2)

式中：qi∈R2，pi∈R2分别为第i个质点在{n}系下的位置和速度；ui∈R2为第i个质点的控制输入，

ui=c1(qri-qi)+c2(pri-pi)+fi+fi,k,

(3)

认为质点i处于邻近其他质点产生的势场中，则第i个质点所具有的总势能可表示为

(4)

式中：φ(x)为势能函数，‖x‖为向量模值。即人工势场能为质点相对距离的函数。由于只考虑将人工势场用于避碰，而非组织队形，则只需要势场具有斥力。构造势函数(见图3)为

(5)

式中：k为势场强度系数，其选取应考虑AUV的惯性大小和执行机构的控制输出大小；k选取的过小，则不利于安全避碰；k选取的过大，则可能超出执行机构的输出范围，没有实际意义；R为势场作用范围。

图3 势函数Fig.3 Potential function

为了避免人工势场势函数对编队队形的干扰，应令最终队形中质点之间的距离大于势函数的作用范围，即

‖qj-qi‖≥R∀i,j∈N且j≠i，

(6)

从而在最终队形的某个邻域内，(3)式右侧的人工势场力项可以消去。(2)式、(3)式构成一个线性2阶系统：

(7)

由Hurwite判据可知，应令c1>0，c2>0使系统稳定。此时，系统的自然频率和相对阻尼系数分别为

(8)

虚拟结构质点作为AUV的跟踪目标，应避免剧烈的运动，以减少AUV的频繁机动。选择c1、c2使此2阶系统成为一个过阻尼系统，可令c1较小，c2较大。由于初始时刻AUV的位置为一定范围内的随机分布，个别AUV的位置可能远离编队参考点，c1较小可以避免虚拟结构质点过快向编队参考点运动，使AUV速度饱和；c2较大则可以令AUV进行速度匹配，便于队形的稳定形成。

对于队形形成过程中的人工势场作用，可以认为是对该2阶系统的干扰。人工势场负梯度项的存在，令质点向着人工势场能减小的方向运动，从而避免碰撞。

2.2 基于虚拟结构的队形变换

相比于人工势场法，虚拟结构法更利于组织队形。设编队参考点在{n}系下的位置和速度表示为qr∈R2，pr∈R2. 则有如下关系成立：

(9)

2.3 编队避障

对于编队行进过程中的避障问题，同样采用人工势场的方法解决。将障碍物以一个圆形区域包围，作为AUV编队的避障区域。当编队靠近障碍物时，会在圆上产生一个投影点，以投影点为基础产生人工势场，保证编队与障碍物具有一定的安全距离。如图4所示，Ok为第k个障碍物包围圆圆心的位置，Rk为包围圆的半径。则有投影点的坐标qi,k为

(10)

令避障函数的人工势场和2.1节具有相同的形式，表示为

(11)

图4 避障示意图Fig.4 Schematic diagram of obstacle avoidance

2.4AUV目标跟踪算法

在实际应用中，AUV的控制系统多采用解耦的形式，分为规划部分和运动控制部分[9]。规划部分根据作业任务的要求，提出期望的路径点，从而给出AUV的期望艏向和期望速度。运动控制器则根据期望的艏向和速度，分别控制舵角和螺旋桨转速。本文在运动层面给出AUV的目标跟踪算法，而AUV的动力学控制采用文献[10]中智能AUV的控制算法。

qa=q-qt.

(12)

选取Lyapunov函数

(13)

对V求导可得

(14)

选取期望的相对速度

(15)

则有

(16)

式中：Ua,max>0为最大的相对跟踪速度；Δpa>0保证当目标跟踪误差为0时，分母不为0. 可知在期望的相对速度下，跟踪误差将收敛至0.

图5 AUV目标跟踪Fig.5 AUV target tracking

根据向量加法，AUV的期望速度向量为

pd=pt+pa.

(17)

在坐标系{t}中跟踪的纵向误差s和横向误差e可表示为

(18)

式中：χt表示质点i的艏向，即速度向量的方向，如图1所示。

采用等方位角算法进行目标跟踪，AUV的期望航向角χd、目标点航向角χt和等方位航向角χr，三者的关系可表示为

χd=χt+χr.

(19)

等方位航向角可表示为

χr=arctan(-e/Δe),

(20)

式中：Δe>0表示前视距离。

由AUV的艏向角ψ、航向角χ和漂角β三者的关系(见图1)，可得AUV的期望艏向角可表示为

ψd=χd-β,

(21)

式中：β=arcsin(v/U)；U为AUV的合速度，即速度向量的模值。

由(15)式、(18)式推导，AUV的期望速度Ud可表示为

(22)

式中：Δs>0为控制跟踪汇合的参数。可知当AUV落后于质点时Ud>Ut，AUV超前于质点时Ud

3 仿真实验

[11]，选取AUV的模型系数如下：m11=25.80，m22=33.80，m33=2.76，m23=m32=6.20，d11=27.0，d22=17.0，d33=0.5，d23=0.2，d32=0.5. 控制参数c1=0.005，c2=0.500，k=4，Δe=10，Δs=10. AUV之间的避碰距离为10 m，实际应用中可根据AUV的主尺度和编队间距调整。AUV的初始位置在300 m×300 m的矩形区域内随机产生，艏向在[-π/2,π/2]的范围内随机产生。编队参考点的初始位置为(150 m,150 m)。

仿真实验1：3个AUV进行编队协同路径跟踪的仿真实验。编队参考点以1 m/s的速度沿OnN轴正方向运动，之后做圆形轨迹运动。AUV以编队参考点为圆心，30 m为半径的圆上分布，将圆3等分。AUV编队效果见图6，其中◇表示AUV的位置，其上的箭头表示AUV的艏向，*表示编队参考点的位置，实线表示AUV的运动轨迹，加粗虚线表示编队参考点的运动轨迹。图7为虚拟结构质点收敛至期望编队位置的误差曲线，以及AUV对虚拟结构质点进行目标跟踪的误差曲线。图8为AUV之间的相对距离曲线。

图6 AUV编队路径跟踪 Fig.6 AUV formation path following

图7 AUV编队误差Fig.7 AUV formation error

图8 AUV间距Fig.8 Distance between AUVs

由图6～图8可知，多AUV系统在随机分布的初始位置下，虚拟结构位置偏差和AUV目标跟踪误差，逐渐收敛至0，形成期望的编队队形，表明编队形成过程中算法的有效性。多AUV编队跟随参考点沿期望的编队轨迹运动，并在编队行进和转向的过程中均保持了良好的队形。表明算法对于多AUV直线编队路径跟踪和曲线编队路径跟踪，均具有良好的适用性。

多AUV系统的初始位置不利于队形的形成，在队形形成的过程中AUV发生交汇，位置在(260 m,170 m)附近，见图6的局部放大图。期望编队位置在左侧的AUV，初始位置在初始化区域的右侧；而期望编队位置在右侧的AUV，初始位置与之相反。在AUV驶向编队期望位置的过程中二者发生交汇，因而改变了原轨迹，并行行驶一段距离后，前后交错避免了碰撞，继而驶向期望位置。由图8可知， 80～150 s的时间段内AUV之间的距离较小，发生交汇，左右两侧的AUV在较长的时间段内保持了10 m左右的距离，此时人工势场势函数已经产生作用，避免了AUV之间的碰撞。之后AUV逐渐收敛至期望的队形，AUV的间距稳定在52 m.

分析图7中的AUV目标跟踪误差可知，在初始时刻目标跟踪误差为0，因为AUV和虚拟结构质点具有相同的初始位置。误差随之变大，原因在于AUV的初始艏向是随机分布的，质点开始运动后，AUV进行跟踪需要调整艏向，由于回转半径的限制，使得AUV落后于质点。当AUV艏向调整完毕，跟踪误差迅速收敛至0. 图7局部放大图对应AUV发生交汇的过程，由于势函数的干扰，导致质点偏离原来轨迹，从而使得跟踪误差变大。

图9 AUV螺旋运动Fig.9 AUV helical motion

仿真实验2：12个AUV进行螺旋扩张运动的仿真实验。期望的队形为圆环队形，圆的半径为80 m，并以0.1 m/s的速度扩张，绕参考点旋转的相对线速度为0.3 m/s. 如图9所示，AUV由初始的随机位置收敛至圆环上，并均匀分布。多AUV编队跟随参考点沿OnN轴正方向运动的同时，绕参考点匀速转动，从而形成了螺旋运动的运动形式。

仿真实验3：10个AUV进行队形变换的仿真实验。AUV之间的期望距离为50 m，第1个队形为矩形队形，第2个队形为三角队形。仿真结果如图10，AUV群体由初始时刻的随机位置，逐渐收敛至矩形队形，当队形稳定后，逐步变换为三角队形。在队形变化过程中，AUV较有秩序地进行，避免了个体之间的碰撞。最终，在新队形下达到了稳定。

图10 AUV队形变换Fig.10 AUV formation changing

由仿真实验2、实验3可知，提出的编队控制算法可以灵活实现编队的队形控制，进行复杂的队形变化。并在编队变化的过程中，保证AUV之间的安全距离。

仿真实验4：10个AUV进行避碰的仿真实验。障碍物势函数的作用距离为15 m. 图11(a)中，障碍物的位置为(800 m,150 m)，半径为60 m；图11(b)中，两个障碍物的位置为(800 m,50 m)，(800 m,250 m)，半径为40 m. 由图11可知，AUV编队在行进过程中，遇到编队路径上的障碍物。AUV个体在障碍物边界上的投影点产生人工势场，对AUV的运动造成影响。AUV在势场力的作用下，改变原来的运动轨迹，沿障碍物的边界划过障碍物。在成功完成避障机动之后，多AUV系统又恢复了原期望的队形。

图11 AUV队形避碰Fig.11 AUV formation with obstacle avoidance

图12 AUV间距和AUV与障碍物距离曲线Fig.12 Distance between AUVs and distance between AUV and obstacle

图12为在避碰过程中，AUV与障碍物边界的距离，以及AUV之间的相互距离。由图12可见，在500～700 s的时间段内，AUV编队进行避障机动。图12(a)为AUV与单个障碍物边界的距离，分析数据可知，最小值为13 m. 此时，AUV处于避碰势函数的作用之下，与障碍物保持了一定的安全距离。图12(b)为AUV躲避单个障碍物时，AUV之间的相互距离。由于障碍物的干扰，AUV编队队形被破坏，处于中间位置处的AUV向两侧运动，“挤压”两侧的AUV，使得AUV之间的距离变小。但是由于避碰函数的作用，AUV之间的距离始终大于0，从而避免了AUV之间的碰撞。当完成避障机动后，AUV恢复编队队形，AUV之间的距离恢复到一个固定值。图12(c)为AUV分别与两个障碍物边界的距离，图12(d)为AUV穿过两个障碍物间隙时，AUV之间的距离。比较图12(b)、图12(d)两图可知，AUV穿过两个障碍物间隙，队形的破坏较小。原因在于处于编队中央的AUV，并不具有碰撞障碍物的危险，前方的AUV通过间隙后，只有后方两侧的AUV受到了障碍物的影响，向内侧运动，使得AUV间距变小。这一分析结果可与图11中AUV编队轨迹相印证。

4 结论

本文通过人工势场和虚拟结构相结合，提出了一种新型多AUV编队控制算法。采用虚拟结构法进行编队队形的组织，并利用人工势场势函数，实现了AUV之间的避碰和编队的避障。进行了编队协同路径跟踪仿真实验，编队变换仿真实验和编队避障仿真实验。仿真实验表明，在随机分布的初始条件下，算法不仅可以有效地组织队形，灵活地进行编队变换，而且在编队形成和编队行进的过程中，有效地避碰和避障，保障AUV之间、AUV和障碍物之间具有足够的安全距离。

参考文献(References)

[1] Cui R, Ge S S, How B V E, et al. Leader-follower formation control of underactuated autonomous underwater vehicles[J]. Ocean Engineering, 2010, 37(17/18):1491-1502．

[2] Ren W. Decentralization of virtual structures in formation control of multiple vehicle systems via, consensus strategies[J]. European Journal of Control, 2008, 14(2):93-103.

[3] 赵宁宁, 徐德民, 高剑, 等. 基于Serret-Frenet坐标系的多AUV编队路径跟踪控制[J]. 鱼雷技术, 2015, 23(1): 35-39. ZHAO Ning-ning, XU De-min, GAO Jian, et al. Formation path following control of multiple AUVs based on Serret-Frenet coordinate system[J]. Torpedo Technology, 2015, 23(1): 35-39. (in Chinese)

[4] Leonard N E, Fiorelli E. Virtual leaders, artificial potentials and coordinated control of groups[C]∥IEEE Conference on Decision and Control.Orlando, America：IEEE Control Systems Society, 2001: 2963-2973.

[5] Cifuentes S, Girón-Sierra J M, Jiménez J. Virtual fields and behaviour blending for the coordinated navigation of robot teams: Some experimental results.[J]. Expert Systems with Applications, 2015, 42(10):4778-4796.

[6] Djapic V. Unifying behavior based control design and hybrid stability theory for AUV application[D]. Riverside: University of California, 2009: 72-105.

[7] Jiang D P, Pang Y J, Qin Z B. Coordinated control of multiple autonomous underwater vehicle system[C]∥Proceedings of 2010 8th World Congress on Intelligent Control and Automation. Jinan, China:IEEE Robotics and Automation Society, IEEE Control System Society, 2010: 4901-4906．

[8] Fossen T I. Handbook of marine craft hydrodynamics and motion control, first edition[M]. United Kingdom: John Wiley & Sons Ltd, 2011.

[9] 甘永. 水下机器人运动控制系统体系结构的研究[D].哈尔滨: 哈尔滨工程大学, 2007: 10-18. GAN Yong. Research on motion control system architecture of underwater vehicle[D]. Harbin: Harbin Engineering University, 2007: 10-18. (in Chinese)

[10] 李岳明, 庞永杰, 万磊. 水下机器人自适应S面控制[J]. 上海交通大学学报, 2012, 46(2): 195-200. LI Yue-ming, PANG Yong-jie, WAN Lei. Adaptive S plane control for autonomous underwater vehicle[J]. Journal of Shanghai Jiao Tong University, 2012, 46(2): 195-200. (in Chinese)

[11] Do K D, Pan J. Global robust adaptive path following of underactuated ships[J]. Autonomous, 2006, 42(10): 1713-1722.

A Multi-AUV Formation Algorithm Combining Artificial Potential Field and Virtual Structure

PAN Wu-wei， JIANG Da-peng， PANG Yong-jie， LI Yue-ming， ZHANG Qiang

(Science and Technology on Underwater Vehicle Laboratory，Harbin Engineering University, Harbin 150001, Heilongjiang， China)

Collision avoidance in the forming process of formation and obstacle avoidance on the formation proceeding are not solved using traditional multi-AUV formation algorithms, such as leader-follower and virtual structure. Artificial potential field algorithm can be used to solve the above problems, while its ability of formation organization is not satisfied. A multi-AUV formation control algorithm combing artificial potential field and virtual structure is proposed to overcome the problems. The system is divided into three parts: formation reference point, virtual structure particle and AUV. The desired virtual structure, which is the moving target of virtual structure particles, is organized to surround the formation reference point. Repulsive artificial potential is used to avoid collision and obstacle while the particles move. The AUVs track the particles to form a certain formation asymptotically. The formation path following, formation changing and obstacle avoidance are simulated to verify the availability and flexibility of the proposed algorithm. The results indicate that the multi-AUV system can form a formation without collision from random initial positions, and the fleet can change formation flexibly and avoid obstacles effectively while proceeding.

ordnance science and technology; multi-AUV；formation control；artificial potential field；virtual structure

2016-05-18

国家自然科学基金项目(51209051、51509057、51309066)； 中央高校基本科研项目(HEUCFD1514)； 海洋工程国家重点实验室开放课题项目(1415)

潘无为(1988—)， 男， 博士研究生， E-mail： panwuwei@hrbeu.edu.cn

姜大鹏(1981—)， 男， 副教授， 硕士生导师， E-mail： jdp103@hrbeu.edu.cn

TP273+.5

A

1000-1093(2017)02-0326-09

10.3969/j.issn.1000-1093.2017.02.017