基于改进人工势场法的多水下机器人编队避障方法及系统

1.本发明涉及水下机器人控制领域，尤其涉及一种基于改进人工势场法的多水下机器人编队避障方法及系统。


背景技术：

2.近年来，陆上资源的日益匮乏激发了人们对海洋资源的探索和开发；水下机器人因其在海洋环境监测、海洋资源勘测、海洋科学研究中的重要作用，成为海洋开发过程中的主要工具，水下机器人技术也得到了学术界的广发关注。由于水下环境的复杂多变，单个水下机器人在执行任务过程中会受到各种约束，因此多水下机器人协作成为重要的工作方式，而多水下机器人协作的基础正是编队控制；避障是指当水下机器人传感器上检测到周围出现障碍物时，能够以最快的时间和最小的成本完成躲避障碍的动作，是完成水下机器人作业的重要工作之一。
3.针对多水下机器人编队及避障过程中的控制问题，国内外学者给出了一系列的控制方法。其中多水下机器人群组的编队控制方法主要包括领航跟随者法、虚拟结构法、基于行为法以及分布式一致性算法等；分布式一致性算法采用分布式网络进行信息交互，具有灵活性高和鲁棒性强等特点。水下机器人的避障方法主要包括人工势场法、模型预测法、最优控制法等。人工势场法因其原理简单，反应迅速，易于实现和扩展被广泛采用。
4.发明人发现，传统的人工势场法主要存在局部极小值的问题，即当水下机器人所受目标区域引力、障碍物斥力及邻近水下机器人合力为零时将致使水下机器人群组出现避障失败的情况。


技术实现要素：

5.针对现有技术存在的不足，本发明的目的是提供一种基于改进人工势场法的多水下机器人编队避障方法及系统，能够避免传统人工势场法避障存在的局部极小值问题，并且可以使水下机器人群组应对水下静止障碍物；考虑时变海流的影响，同时也解决了编队过程中队形的稳定性问题。
6.为了实现上述目的，本发明是通过如下的技术方案来实现：
7.第一方面，本发明的实施例提供了一种基于改进人工势场法的多水下机器人编队避障方法，包括：
8.初始化水下机器人群组的状态信息；
9.水下机器人群组基于相邻水下机器人势场以及分布式一致性算法进行编队运动；
10.判断水下机器人群组是否形成指定编队队形，若水下机器人群组形成指定编队队形，基于目标区域势场做朝向目标区域方向的运动，同时躲避运动过程中的障碍物；
11.判断水下机器人群组是否处于障碍物势场中，若水下机器人群组处于障碍物势场中则判断其是否陷入局部最优陷阱；若水下机器人群组陷入局部最优陷阱，则基于障碍函数的辅助势场使水下机器人群组逃离局部最优陷阱；
12.水下机器人群组在障碍物势场及目标区域势场的联合作用下做避障运动，到达目标区域时完成任务。
13.作为进一步的实现方式，若水下机器人群组未形成指定编队队形，则水下机器人群组继续进行基于邻居水下机器人势场以及分布式一致性算法的编队运动。
14.作为进一步的实现方式，若水下机器人群组未处于障碍物势场中，则判断水下机器人群组是否到达目标区域所在区域。
15.作为进一步的实现方式，如果已经到达目标区域则水下机器人群组完成任务，否则水下机器人群组继续基于目标区域势场做朝向目标区域方向的运动。
16.作为进一步的实现方式，若水下机器人群组处于障碍物势场中但并未陷入局部最优陷阱，则水下机器人群组在障碍物势场以及目标区域势场的联合作用下做避障运动。
17.作为进一步的实现方式，在每个水下机器人基于相邻水下机器人势场以及分布式一致性算法的进行编队运动之前，先获取相邻水下机器人以及目标区域的位置信息；所有水下机器人只与编队中的部分机器人进行信息交互。
18.作为进一步的实现方式，假设水下机器人群组是在水下同一深度进行编队，即编队完成后队形中每个机器人节点有相同的z轴信息，并在三维水下空间中追踪目标区域；并假设每一深度有不同速度的时变海流，以此模拟复杂海流的干扰。
19.第二方面，本发明实施例还提供了一种基于改进人工势场法的多水下机器人编队避障系统，包括：
20.状态信息初始化模块，被配置为：初始化水下机器人群组的状态信息；
21.编队运动模块，被配置为：水下机器人群组基于邻居水下机器人势场以及分布式一致性算法进行编队运动；
22.指定编队队形判断模块，被配置为：判断水下机器人群组是否形成指定编队队形，若水下机器人群组形成指定编队队形，基于目标区域势场做朝向目标区域方向的运动，同时躲避运动过程中的障碍物；
23.障碍物势场及局部最优陷阱判断模块，被配置为：判断水下机器人群组是否处于障碍物势场中，若水下机器人群组处于障碍物势场中则判断其是否陷入局部最优陷阱；若水下机器人群组陷入局部最优陷阱，则基于障碍函数的辅助势场使水下机器人群组逃离局部最优陷阱；
24.联合避障运动模块，被配置为：水下机器人群组在障碍物势场及目标区域势场的联合作用下做避障运动，到达目标区域时完成任务。
25.第三方面，本发明实施例还提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现所述的基于改进人工势场法的多水下机器人编队避障方法。
26.第四方面，本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现所述的基于改进人工势场法的多水下机器人编队避障方法。
27.上述本发明的实施例的有益效果如下：
28.(1)本发明的一个或多个实施方式采用的基于分布式一致性算法的协同控制方法，在执行过程中不存在中心控制节点，避免了集中式控制水下机器人群组与控制中心连
接的网络拓扑形式，各水下机器人地位平等，因此可适用于多种不同的编队队形的通信拓扑结构。
29.(2)本发明的一个或多个实施方式分布式的控制架构使每个水下机器人编队过程中只需获得其邻居相关的信息，从而提高了编队的整体计算能力，同时使编队的灵活性和鲁棒性大幅提高。
30.(3)本发明的一个或多个实施方式在避障中采用的改进的人工势场方法，既结合了传统人工势场法的原理简单、易于实现和操作的特点，同时解决了机器人群组易陷入局部最优解的问题，进而消除了水下机器人群组臂章失败的弊端。
31.(4)本发明的一个或多个实施方式将分布式一致性算法与改进后的人工势场法相结合，实现了机器人群组在向目标区域移动过程中稳定编队与安全避障。
附图说明
32.构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
33.图1是本发明根据一个或多个实施方式的流程图；
34.图2是本发明根据一个或多个实施方式的水下机器人及障碍物的势场及影响范围图；
35.图3是本发明根据一个或多个实施方式的水下机器人所受合力图；
36.图4是本发明根据一个或多个实施方式的多水下机器人编队拓扑结构图。
具体实施方式
37.应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。
38.需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
39.实施例一：
40.本实施例提供了一种基于改进人工势场法的多水下机器人编队避障方法，改进后的人工势场法为：在传统人工势场法的基础上增加额外作用力，该作用力可以在水下机器人群组陷入局部最优陷阱时起作用，进而避免了算法陷入局部最优而致使避障失败的情况。
41.如图1所示，本实施例包括以下步骤：
42.步骤1：初始化水下机器人群组的状态信息；
43.步骤2：水下机器人群组基于邻居水下机器人势场以及分布式一致性算法进行编队运动；
44.步骤3：判断水下机器人群组是否形成指定编队队形，如果队形已形成，就执行步骤4，否则返回继续执行步骤2；
45.步骤4：水下机器人群组基于目标区域势场做朝向目标区域方向的运动，同时躲避运动过程中出现的障碍物；
46.步骤5：判断水下机器人群组是否处于障碍物势场中，如果处于障碍物势场，就执行步骤6，否则执行步骤9；
47.步骤6：判断水下机器人群组是否陷入局部最优陷阱，如果陷入局部最优陷阱，就执行步骤7，否则执行步骤8；
48.步骤7：基于障碍函数的辅助势场发挥作用，帮助水下机器人群组逃离局部最优陷阱；
49.步骤8：水下机器人群组在障碍物势场以及目标区域势场的联合作用下做避障运动；
50.步骤9：判断水下机器人群组是否到达目标区域所在区域，如果已经到达目标区域说明水下机器人群组已完成任务，整个过程结束，否则继续执行步骤4。
51.具体的，水下机器人群组协同编队控制技术具体可以应用于各类军事和民用领域以执行各种复杂的任务。由于水下地形以及各种鱼群等因素的影响，水下机器人群组的避障是一项非常重要的工作，考虑到要面对的复杂海流的影响，保持稳定的编队队形也是必不可少的工作。
52.本实施例将装有导航定位装置的水下机器人群组放入水下相同深度且队形任意，每个水下机器人可通过通信组网获取其邻居机器人的位姿信息和速度信息；建立水下机器人编队的拓扑通信结构g＝(v,e,a)；其中g为各水下机器人节点组成的加权无向图，v＝{v1,v2,...,v
n
}是水下机器人各节点的集合，e＝{e
ij
＝(v
i
,v
j
)}∈v
×
v是加权图的边集，a＝[a
ij
]是图g的加权邻接矩阵，且a
ij
≥0为权重(表示节点i和j可以相互传递信息)。
[0053]
进一步的，所述步骤1中的初始化机器人群组的状态信息是指群组中各水下机器人可以获取当前时刻自身的位置信息和速度信息。假设水下机器人群组是在水下同一深度进行编队，即编队完成后队形中每个机器人节点有相同的z轴信息，并在三维水下空间中追踪目标区域；并假设每一深度有不同速度的时变海流，以此模拟复杂海流的干扰。
[0054]
定义水下机器人节点v
i
在时刻t的自身位置信息x
i
＝[x
i
(t),y
i
(t),z
i
(t)]，下文为了表示方便也用q
i
表示，速度信息为v
i
(t)＝[v
xi
(t),v
yi
(t),v
zi
(t)]。在此基础上，设计单个水下机器人的动力学模型为二阶系统，其数学描述为：
[0055][0056]
其中，u
i
(t)为水下机器人节点v
i
在时刻t的输入控制律。
[0057]
机器人群组初始化状态信息后通过获取及邻居机器人的位姿信息基于分布式一致性算法快速完成编队，并在群组编队完成以后获取目标区域的位置信息，在目标区域势场的作用下向目标区域移动；该群组能在有限的时间内形成指定的编队队形并保持队形参与避障，在避障完成之后能够很快恢复队形。
[0058]
进一步的，所述步骤2中，在每个水下机器人基于相邻水下机器人势场以及分布式一致性算法的编队运动之前，要先获取相邻水下机器人以及目标区域的位置信息；所有水下机器人只与编队中的部分机器人进行信息交互，而不是获得全局编队信息，这种分布式的方法有利于减轻通信网络的带宽消耗。
[0059]
水下机器人节点在分布式一致性算法下的编队运动要求每个水下机器人节点能够获取其邻居节点的位置信息，由于编队队形最初形成过程中假定不存在障碍物干扰，所以只需要考虑相邻两个水下机器人势场受力机器运动的情况。
[0060]
每一深度的时变海流设计为v0(t)，假设海水的阻力与海水的流速是呈正相关的且相关系数为k0，表示为：
[0061]
f0(t)＝k0v0(t)
[0062]
如图2所示，本实施例中对水下机器人节点v
i
的势场设计如下：
[0063][0064]
其中，k1、k2分别表示v
i
斥力势场参数和引力势场参数；d(q
i
,q
j
)表示节点v
i
与v
j
间的距离范式，且d(q
i
,q
j
)＝||q
i
‑
q
j
||；r(i,j)与r(i,j)分别表示节点v
i
斥力势场的最大作用半径和引力势场的最小作用半径。
[0065]
由节点v
i
势场产生的势场力为：
[0066][0067]
则节点v
i
所受水下机器人群组的合力为：
[0068][0069]
取此过程中水下机器人i所受合外力为：
[0070]
f
totali
＝f
ii
(q
i
)+f0(t)+f
att
(q
i
)
[0071]
在上述水下机器人间势场力以及目标区域合力作用下，基于分布式一致性算法，水下机器人群组开始在向目标区域运动的过程中完成编队。在该过程中的控制输入设计为：
[0072][0073]
其中，γ0、γ1分别是位置和速度控制相关的系数，d
*
∈(r(i,j),r(i,j))表示机器人i与机器人j之间的期望距离，以表示编队信息。通过引入积分项来抵消运动过程中所受合外力的干扰。
[0074]
进一步的，所述步骤4中，水下机器人群组基于目标区域势场做朝向目标区域方向的运动，该过程是通过目标区域的引力势场作用，本实施例的目标区域的引力势场设计为：
[0075][0076]
其中，k
att
表示目标区域引力场的引力系数；d(q
i
,p
x
(q
i
))表示节点v
i
与目标区域的距离范式，且d(q
i
,p
x
(q
i
))＝||q
i
‑
p
x
(q
i
)||；d
t*
表示设定的目标区域距离；x(q
t
)为目标区域的集合，p
x
(q
i
)表示第i个机器人节点在目标区域的投影。
[0077]
引力为：
[0078][0079]
进一步的，所述步骤5中要判断水下机器人群组是否处于障碍物势场中，即带有传感器的水下机器人检测其与障碍物之间的距离；再根据所设障碍物势场的影响范围判断该距离是否小于势场的作用范围，如果小于则说明水下机器人群组已经检测到障碍物并开始在障碍物斥力的作用下做避障运动。
[0080]
如图2所示，本实施例的障碍物i斥力势场为：
[0081][0082]
其中，k
rep
表示目标区域引力场的引力系数；d(q
i
,q
oi
)表示节点v
i
与第i个障碍物的距离范式，且d(q
i
,q
oi
)＝||q
i
‑
q
oi
||；q
*
表示设定的第i个障碍物作用范围。
[0083]
障碍物i的斥力为：
[0084][0085]
假设整个路径中的障碍物个数为m，节点v
i
所受的障碍物的合力为：
[0086][0087]
进一步的，步骤6中判断水下机器人群组是否陷入局部最优陷阱是相对于群组中每一个水下机器人而言的，当群组陷入局部最优陷阱时，水下机器人群组所受合力为零，如图3所示，局部最优陷阱的表示为：
[0088]
f
total
＝f0(t)+f
att
(q
i
)+f
rep
(q
i
)+f
ii
(q
i
)＝0
[0089]
进一步的，步骤7中引入避障函数是指改进的人工势场方法将障碍函数加入其中；障碍函数是一个连续函数，其中点的值随着点到达优化问题的可行区域边界而增加到无穷大，其作用是阻止函数进入不可行域。
[0090]
本实施例中障碍函数的主要作用是当水下机器人群组靠近障碍物的距离达到了预设距离的最小值时起作用，而假设在该值算法陷入局部最优解，避免了局部最小陷阱而导致群组避障失败的现象。
[0091]
进一步的，本实施例采用的障碍函数为对数函数的形式，即：
[0092][0093]
对数函数的屏蔽功能为当x<b时，g(x,b)被定义为
‑
log(b
‑
x)，当x
→
b时对数函数趋向于无穷大，此处的b表示设置的状态边界(当x的值接近b，解释为状态靠近设定的边界时，函数的值趋于无穷大，作用是阻止x越过边界)，应于水下机器人陷入局部最优陷阱时，给其一个与运行方向垂直的额外作用力使其摆脱局部最优的影响。
[0094]
进一步的，步骤8中水下机器人群组在障碍物势场以及目标区域势场的联合作用下做避障运动，考虑到海流因素、目标区域势场、障碍物势场、以及内部势场的联合作用，该种情况下的避障控制输入设计为：
[0095][0096]
其中，k表示避障函数调节系数，通过修改其值可以改变避障函数作用强度进而改变算法的性能。
[0097]
进一步的，步骤9中判断水下机器人群组是否到达目标区域所在区域，可以通过比较机器人群组的位置信息与目标区域的位置信息差值来获得，当差值到零时说明水下机器人群组编队及其避障工作已完成。
[0098]
实施例二：
[0099]
本实施例提供了一种基于改进人工势场法的多水下机器人编队避障系统，包括：
[0100]
状态信息初始化模块，被配置为：初始化水下机器人群组的状态信息；
[0101]
编队运动模块，被配置为：水下机器人群组基于邻居水下机器人势场以及分布式一致性算法进行编队运动；
[0102]
指定编队队形判断模块，被配置为：判断水下机器人群组是否形成指定编队队形，若水下机器人群组形成指定编队队形，基于目标区域势场做朝向目标区域方向的运动，同时躲避运动过程中的障碍物；
[0103]
障碍物势场及局部最优陷阱判断模块，被配置为：判断水下机器人群组是否处于障碍物势场中，若水下机器人群组处于障碍物势场中则判断其是否陷入局部最优陷阱；若水下机器人群组陷入局部最优陷阱，则基于障碍函数的辅助势场使水下机器人群组逃离局部最优陷阱；
[0104]
联合避障运动模块，被配置为：水下机器人群组在障碍物势场及目标区域势场的联合作用下做避障运动，到达目标区域时完成任务。
[0105]
实施例三：
[0106]
本实施例提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现实施例一所述的基于改进人工势场法的多水下机器人编队避障方法。
[0107]
实施例四：
[0108]
本实施例提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现实施例一所述的基于改进人工势场法的多水下机器人编队避障方法。
[0109]
以上实施例二
‑
四中涉及的各步骤与方法实施例一相对应，具体实施方式可参见实施例一的相关说明部分。术语“计算机可读存储介质”应该理解为包括一个或多个指令集
的单个介质或多个介质；还应当被理解为包括任何介质，所述任何介质能够存储、编码或承载用于由处理器执行的指令集并使处理器执行本发明中的任一方法。
[0110]
本领域技术人员应该明白，上述本发明的各模块或各步骤可以用通用的计算机装置来实现，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。本发明不限制于任何特定的硬件和软件的结合。
[0111]
以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。