基于改进领航者法的多AUV队形控制方法研究

李 良，李一平，张岳星，曾俊宝，徐高朋

（1.中国科学院沈阳自动化研究所 机器人学国家重点实验室，辽宁 沈阳 110016；2.中国科学院机器人与智能制造创新研究院，辽宁 沈阳 110169；3.辽宁省水下机器人重点实验室，辽宁 沈阳 110169；4.中国科学院大学，北京 100049）

0 引言

自主水下机器人（AUV）是当前人类探索海洋的有效工具之一，可用于绘制海底地图、研究水生动植物生活模式、观测海洋现象、安装海底管道和国防应用等[1]。随着水下任务的日益复杂，需要通过多AUV协作的方式来完成单个AUV无法完成的水下任务。在一些任务中，多AUV还需要按照固定的队形编队运动[2]。

近年来许多学者对多 AUV编队控制进行了研究，比较成熟的编队控制方法有以下几种：基于虚拟结构法[3]、基于行为法[4]、基于跟随领航者法[5-7]、基于人工势场法[8-9]、基于路径跟随法[10-12]和基于信息一致性法[13-14]等。文献[15]对多 AUV编队方法进行了综述，其中跟随领航者法最易于理解，工程应用最为广泛。文献[16]在跟随领航者法的基础上，基于跟随者的状态反馈制定领航者的运动策略，解决了跟随者出现异常时队形可能保持的问题。文献[6]使用文献[16]中提出的方法，在国内较早的基于水声通信对编队控制方法展开外场试验验证，同时分析了实际应用中水声通信存在的问题和难点，并对此提出了宝贵的解决意见。随后也有研究人员提出了多种编队控制方法，但大多处于理论分析和仿真实验验证阶段，没有在实际中进行应用。

本文在水声通信的基础上，对传统的跟随领航者法进行改进，提出了一种基于改进跟随领航者法的编队控制方法。通过仿真实验验证该方法的可行性并确定关键控制参数，然后在湖上对该方法进行试验验证，对编队控制结果进行定量描述和详细分析，证明该编队控制方法的实用性和有效性。

1 编队控制方法

传统的跟随领航者法是指多 AUV在三维水下环境编队运动时，领航者按照给定的航行路线和速度运动，在此期间通过水声通信周期性广播自身位置和速度信息，跟随者根据接收到的领航者信息自主调整航行路线和速度，与领航者保持期望的队形。

本文研究3台AUV的编队控制，将其划分为1台领航者和2台跟随者。在跟随领航者法的基础上进行改进，跟随者通过跟踪虚拟目标点实现编队，将对跟随者的航向和速度控制简化成单一的速度控制，并采用多级调控方法对跟随者的速度进行精准调控。建立一套队形评价标准，对编队控制的快速性、准确性和稳定性进行定量评价。

1.1 改进的跟随领航者法

对传统的跟随领航者法改进，改进方法中AUV之间通信数据量变小，更适应通信受限情况下的水声环境。同时在通信效果不稳定时，AUV仍能按照预设轨迹运动，抗扰动能力强，改进方法分为以下2步。

第1步，简化控制变量。给定AUV的运动深度和航向，AUV在不同深度定深运动，提高水声通信成功率。AUV沿预设航线运动，将对跟随者的航向和速度控制简化为只对速度控制。

第2步，精确调控速度。跟随者基于与领航者之间的距离误差反馈，调整速度以形成和保持队形。根据距离误差的大小，设置多级调控模式，提高队形形成的快速性和队形保持的稳定性。

某一时刻领航者和跟随者的位置关系如图1所示，其中OA(xa,ya)代表领航者的真实位置和发送给跟随者的位置，OB(xB,yB)和OC(xC,yC)代表2个跟随者的位置，O1和O2为跟随者的虚拟目标点，ΔOAO1O2为期望的编队队形。对其中一台跟随者B的速度控制方法进行数学说明。将整个平面划分成区域Ⅰ和Ⅱ2部分，分别表示跟随者在领航者的后方和前方2种情况。

图1 领航者和跟随者位置关系Fig.1 Position relation between leader and followers

式中：1α为射线与aγ之间的夹角，根据1α的大小，确定跟随者所处的区域。

式中：λ代表跟随者所处区域，λ=1表示处于区域Ⅰ，λ=2表示处于区域Ⅱ。令

式中：L为跟随者与领航者之间的期望距离；e为跟随者与领航者之间的距离误差。

根据公式（2）–（3），提出 4种跟随者 B相对与虚拟目标点O1的位置状态：正常（N），激进（A），落后（L）和脱离（B）。根据位置状态，制定跟随者速度控制策略，如表1所示。

表1 跟随者速度控制策略Tab.1 Speed control strategy of the followers

表中R≥0，表示允许的距离误差；vb和va为跟随者和领航者的速度；k1和k2为比例系数且k2＞k1＞ 0；vmin和vmax表示设定的AUV最小和最大航行速度。跟随者在正常状态下，缓慢调整速度，维持队形。在激进或者落后状态下，快速调整速度，形成队形。在脱离状态下，需要以最小速度航行等待领航者，由脱离状态变成激进状态后快速形成队形。

1.2 队形评价指标

为定量评价编队控制效果，从队形形成的快速性、准确性和队形保持的稳定性3个方面定义队形评价指标。

定义1：队形误差。

式中：ei为其中 2台 AUV的距离误差；Di为 2台AUV之间的欧氏距离；Li为2台AUV的期望距离；E为队形误差，其大小可以代表编队控制的精度。

定义2：队形形成时间。

式中：t'为首次形成队形的时间，其大小可以描述编队控制的快速性。

定义3：队形保持时间。

设AUV误差采样周期为Terror，误差采样点集合为E={E1,…En}，若∃Ej，j=1,…n，∀ei，都有ei≤R，i=1,2,3，则称在Ej采样点时刻保持了队形。

式中：Nhold为Ej的总数；Ttotal为队形保持时间，其大小可以反映编队控制的稳定性。

2 仿真验证

对编队控制方法进行仿真验证，同时确定控制方法中参数k1和k2取值。在MATLAB上进行仿真实验，3台AUV分别从坐标点(0,15)、(0,0)和(0,-15)（领航者位于中间）沿X轴方向运动约500 m，领航者的期望速度va=1.2 m/s，通信周期T=5s，AUV之间期望距离L1=L2=L3=30 m，整体为正三角形队形。通信成功率C=80%，其中C的定义为

式中：Nr为跟随者接收到领航者信息的总包数；Ns代表领航者发送信息的总包数；C越大，代表通信效果越好。

利用遗传算法对参数k1和k2进行优化，过程如下。

Step1：编码。给定k1和k2的取值范围均为（0，1]，采用二进制编码方式，k1和k2分别对应 10条染色体的前5位和后5位。

Step2：初始化种群。随机产生4个10位二进制个体。

Step3：解码。对种群个体解码，得到4组k1和k2的值。

计算每个个体适应度值。

Step5：选择。采用最佳保留选择算子，即将适应度最高的个体完整的复制到下一代群体中，其余个体按照轮盘赌选择方法执行选择操作。

Step6：交叉。采用随机配对方法，对每组个体进行单点交叉，交叉概率Pc=0.5。

Step7：变异。采用均匀变异方法，变异概率Pm=0.005。

Step8：进化终止条件。迭代次数N=50时停止，否则执行Step3。

优化后的结果如图2所示，随着迭代次数的增加，最优适应度越来越大。将最后一步迭代得到的最优个体解码得到参数k1=0.04，k2=0.18。当取上述参数时，3台AUV的运动轨迹如图3所示，编队效果很好。

图2 最优适应度值的迭代曲线Fig.2 Iteration curves of the optimal fitness value

图3 仿真运动轨迹Fig.3 Trajectory of AUVs in the simulation test

3 外场试验

为验证本文编队控制方法的实用性，使用 3台“探索100”AUV[17]进行了水下编队试验，如图4所示。编队试验中期望队形为正三角形，即L1=L2=L3。领航者保持在队伍的前方，跟随者在后方两侧保持跟随，试验方案如下：3台AUV定深航行，预定航线为3条间距约15 m的由西向东平行直线，航行距离约500 m。试验中，取R=5 m，vmin=0.5 m/s，vmax=1.9 m/s，Terror=1s。一共进行了多次编队试验，每次试验结果差别不大，由于本文篇幅有限，任取其中一次实验数据具体分析，如图5–9。

图4 试验中的AUVFig.4 AUVs in the field test

图5 航行轨迹Fig.5 Trajectory of AUVs

图5显示了多AUV的航行轨迹，可以看出正三角形队形逐渐形成并趋于稳定。图6为多AUV航行速度变化，可以看出跟随者1和跟随者2不断调整自身速度以保持期望队形。图7为多AUV航向信息，航向角基本与预设值相同，证明改进跟随领航者法中简化控制变量是简单有效的。图8为深度变化图，在垂直面上3台AUV分别在不同深度定深运动，且深度控制稳定，有利于减小对水平面上航向和速度控制的扰动。图9为队形误差图，可以看出三角形每条边的误差和总误差不断收敛，队形形成速度快，控制精度高，期望队形不断趋于稳定。

图6 航行速度信息Fig.6 Speed of AUVs

图7 航向角信息Fig.7 Heading of AUVs

图8 深度信息Fig.8 Depth of AUVs

图9 队形误差Fig.9 Formation errors

4 结束语

在水声通信的基础上，对跟随领航者编队控制方法进行改进，在MATLAB上对该方法进行仿真验证并确定关键控制参数，最后在湖上进行外场试验验证。试验结果表明：该方法在队形形成的快速性、维持队形的稳定性和队形控制的精确性等方面表现得很好。

AUV导航精度对编队效果影响很大，本文中采用电子罗盘+多普勒计程仪的低成本组合导航方法，短距离内导航误差较小。未来将对AUV导航方式进行升级，拟采用惯导+多普勒计程仪的高精度导航方法，提高编队精度。

本文试验环境中水声通信效果较好，下一步计划在水声通信效果较差甚至无法通信的环境中，对本文提出的方法进行改进，实现编队控制，最大限度的解决通信问题对编队控制的影响。