基于虚拟势场的群体智能机器人路径规划方法及装置

技术领域

本申请涉及路径规划技术领域，特别涉及一种基于虚拟势场的群体智能机器人路径规划方法及装置。

背景技术

相关技术已有的群体智能领域路径规划算法，其仍存在许多不足。例如，蚁群算法和人工蜂群算法寻优能力对参数的依赖较高，易陷入局部最优，且需要较长时间才能发挥正反馈的作用，收敛速度较慢。而粒子群优化算法存在精度低，在粒子速度较快的情况下还存在难以收敛的问题。总而言之，目前群体智能机器人的路径规划算法很难同时满足高鲁棒性、去中心化、易于拓展、适应性强的要求，也难以实现对机器人效率要求较高的对抗性博弈任务。

发明内容

本申请提供一种基于虚拟势场的群体智能机器人路径规划方法及装置，在传统虚拟力场的优点基础上，能够进一步提高机器人的运动精度、灵敏度、反应速度，且具有良好的可拓展性，高效地完成更加复杂的对抗性博弈任务。

本申请第一方面实施例提供一种基于虚拟势场的群体智能机器人路径规划方法，包括以下步骤：根据预测的围捕对象的下一时刻的预测位置和预测速度计算围捕机器人与所述围捕对象间的第一吸引力；根据所述围捕机器人间的距离信息计算所述围捕机器人间的第一排斥力，根据所述围捕机器人与障碍物间的距离信息计算所述围捕机器人的第二排斥力；根据所述围捕机器人间的距离信息和位置信息计算所述围捕机器人的松散力；根据所述围捕对象、所述围捕机器人和所述障碍物的位置计算所述围捕机器人对所述围捕对象的第三排斥力和所述障碍物对所述围捕对象的第四排斥力，以及所述围捕对象自身的第五排斥力，并在所述围捕对象静止或所述围捕机器人与所述围捕对象间的距离小于预设值时，计算所述围捕机器人的环绕力；将所述第一吸引力、所述第一排斥力、所述第二排斥力、所述松散力和所述环绕力进行叠加后，计算所述围捕机器人的运动速度，将所述第三排斥力、所述第四排斥力和所述第五排斥力进行叠加后计算所述围捕对象的运动速度，并实时根据所述围捕机器人的运动速度和所述围捕对象的运动速度控制所述围捕机器人和所述围捕对象运动。

可选地，在本申请的一个实施例中，在计算围捕机器人与所述围捕对象间的第一吸引力之前，还包括：利用卡尔曼滤波器预测所述围捕对象的下一时刻预测位置；将所述下一时刻预测位置和所述围捕对象的下一时刻实际位置进行比较根据比较结果更新所述卡尔曼滤波器的状态矩阵和协方差矩阵，并利用更新后的卡尔曼滤波器预测所述围捕对象的下一时刻的预测位置和预测速度。

可选地，在本申请的一个实施例中，所述围捕机器人与所述围捕对象间的第一吸引力F

其中，k

可选地，在本申请的一个实施例中，所述围捕机器人间的第一排斥力F

其中，r

可选地，在本申请的一个实施例中，所述围捕机器人的环绕力F

其中，k

可选地，在本申请的一个实施例中，所述围捕机器人的松散力F

τ＝(-sinθ

其中，θ

可选地，在本申请的一个实施例中，在所述围捕对象视野范围内所述围捕机器人的数目小于等于2时，所述围捕机器人对所述围捕对象的第三排斥力F

其中，k

在所述围捕对象视野范围内所述围捕机器人的数目大于2时，所述围捕机器人对所述围捕对象的第三排斥力F

其中，R

可选地，在本申请的一个实施例中，所述围捕对象自身的第五排斥力F

其中，k

可选地，在本申请的一个实施例中，在计算出所述围捕机器人的运动速度和所述围捕对象的运动速度之后，还包括：利用增量式PD算法对所述所述围捕机器人的运动速度和所述围捕对象的运动速度进行修正。

本申请第二方面实施例提供一种基于虚拟势场的群体智能机器人路径规划装置，包括：第一计算模块，用于根据预测的围捕对象的下一时刻的预测位置和预测速度计算围捕机器人与所述围捕对象间的第一吸引力；第二计算模块，用于根据所述围捕机器人间的距离信息计算所述围捕机器人间的第一排斥力，根据所述围捕机器人与障碍物间的距离信息计算所述围捕机器人的第二排斥力；第三计算模块，用于根据所述围捕机器人间的距离信息和位置信息计算所述围捕机器人的松散力；第四计算模块，用于根据所述围捕对象、所述围捕机器人和所述障碍物的位置计算所述围捕机器人对所述围捕对象的第三排斥力和所述障碍物对所述围捕对象的第四排斥力，以及所述围捕对象自身的第五排斥力，并在所述围捕对象静止或所述围捕机器人与所述围捕对象间的距离小于预设值时，计算所述围捕机器人的环绕力；规划模块，用于将所述第一吸引力、所述第一排斥力、所述第二排斥力、所述松散力和所述环绕力进行叠加后，计算所述围捕机器人的运动速度，将所述第三排斥力、所述第四排斥力和所述第五排斥力进行叠加后计算所述围捕对象的运动速度，并实时根据所述围捕机器人的运动速度和所述围捕对象的运动速度控制所述围捕机器人和所述围捕对象运动。

本申请实施例的基于虚拟势场的群体智能机器人路径规划方法及装置，可以使群体智能机器人在无领导者的情况下快速、准确、高鲁棒性地进行运动，在实现智能避障的同时作为有机的整体高效、稳定完成对抗性博弈任务(如对目标机器人的包围、攻击)。拓展性强、对硬件要求低，只需将相同的独立程序写入机器人，每个机器人仅需收集局部信息，即可加入团队共同完成任务。

本申请附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本申请的实践了解到。

附图说明

本申请上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1为根据本申请实施例提供的一种基于虚拟势场的群体智能机器人路径规划方法的流程图；

图2为根据本申请实施例提供的一种不同情况下围捕机器人围猎效率比较示意图；

图3为根据本申请实施例提供的增加速度修正项对机器人围猎效率的影响对比示意图；

图4为根据本申请实施例提供的smooth函数示意图；

图5为根据本申请实施例提供的松散力作用示意图；

图6为根据本申请实施例提供的应用改进势力场模型时围捕对象突围示意图；

图7为根据本申请实施例提供的新引入的额外斥力场示意图；

图8为根据本申请实施例的基于虚拟势场的群体智能机器人路径规划装置的示例图。

具体实施方式

下面详细描述本申请的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本申请，而不能理解为对本申请的限制。

图1为根据本申请实施例提供的一种基于虚拟势场的群体智能机器人路径规划方法的流程图。

如图1所示，该基于虚拟势场的群体智能机器人路径规划方法包括以下步骤：

在步骤S101中，根据预测的围捕对象的下一时刻的预测位置和预测速度计算围捕机器人与围捕对象间的第一吸引力。

可选地，在本申请的一个实施例中，在计算围捕机器人与围捕对象间的第一吸引力之前，还包括：利用卡尔曼滤波器预测围捕对象的下一时刻预测位置；将下一时刻预测位置和围捕对象的下一时刻实际位置进行比较根据比较结果更新卡尔曼滤波器的状态矩阵和协方差矩阵，并利用更新后的卡尔曼滤波器预测围捕对象的下一时刻的预测位置和预测速度。

与常规无人驾驶任务不同，围捕机器人在执行围捕任务时并不到达目标点，而是形成以围捕对象(猎物)为圆心、半径约为R

卡尔曼滤波器分为预测和更新两个阶段，在预测阶段，可以用当前围捕对象位置信息转换得到围捕对象下一状态预计位置。根据需求，设k时刻的状态矩阵为围捕对象位置矢量为x

其中F

围捕对象位置改变后，通过将观测到的实际位置与预计位置进行比较，引入卡尔曼增益K，更新状态矩阵与协方差矩阵，用于下一次预测，具体公式如下：

P

利用该模型可以得到围捕对象下一节点的预计位置，从而估计围捕对象此时的运动策略。

在(1)围捕对象运动速度未知、(2)借助卡尔曼滤波法预测围捕对象速度、(3)围捕对象速度完全已知三种情况下，分别进行了100次仿真测试并取平均时间，为了确保围捕机器人能够追上围捕对象，设定围捕机器人始终以最大速度运动。三种情况下围猎的平均时间如图2所示。由此可见，该围捕对象速度预测的方法能够显著提升围猎效率，且卡尔曼滤波法对围捕对象策略的估计与实际情况吻合较好。

在围猎任务中，围捕机器人并不直接到达目标点，其最终任务是在围捕对象周围保持距离并形成一个稳定的半径为R

而在对抗性博弈任务中，由于围捕对象的运动，包围圈的圆心也随之不断移动，导致包围过程中围捕机器人的运动更加复杂，难以对围捕对象形成稳定的包围。因此，传统的仅基于位置的势函数并不能直接适用，需要考虑到围捕机器人与围捕对象的运动速度修改势场。此时围捕机器人将利用上述的卡尔曼滤波法得到围捕对象当前的预测相对速度v

其中，k

当|r

在相同的情况下进行200次仿真测试后，在围捕过程中增加以上速度修正项可以明显提升围捕效率，如图3所示。可以预期当猎物的速度更加复杂时，速度修正项将发挥更加显著的作用。

在步骤S102中，根据围捕机器人间的距离信息计算围捕机器人间的第一排斥力，根据围捕机器人与障碍物间的距离信息计算围捕机器人的第二排斥力。

不同于模拟环境的理想模型，实际的围捕机器人具有不可忽略的空间体积，因此为了规避围捕机器人间的相互碰撞，对围捕机器人相互之间需要设定一安全半径R

该虚拟力在围捕机器人与其他对象相距较远时将不考虑排斥力，避免由于自身斥力场影响区域过大而影响其他对象的正常运动，而在距离进入一定安全半径内时则随距离的逐渐接近而产生逐渐增大的斥力。

由于围捕机器人可利用视觉远距离识别障碍物，因此将障碍物纳入该虚拟力场中，并采用相同形式的排斥力模型，适当调整比例系数的大小，以r

经实验验证，该算法能较好地避免围捕机器人与周围环境的碰撞，在考虑障碍物的基础上实现较好的路径规划。

对于虚拟力场这一算法，其显著的问题便是可能出现的局部最优解，即在一点处围捕机器人所受合势场力为零，从而陷入该处无法移动。对对抗性博弈任务而言，围捕对象的运动导致势场的变化更加频繁，局部最优解的位置更易发生变化，这种不确定性将导致围捕机器人更可能暂时陷入新出现局部最优解中，从而导致围捕效率下降。为了避免在追捕过程中出现斥力过大导致围捕机器人无法接近围捕对象的情况，修改了斥力模型，使得斥力随着与诶不对象之间的距离r

该模型在此前基础上添加了

在步骤S103中，根据围捕机器人间的距离信息和位置信息计算围捕机器人的松散力。

由于围猎任务要求围捕机器人全方位环绕围捕对象，因此在围捕过程中其应更分散地进行运动，避免提前集群的情况出现。对此，围捕机器人引入了松散偏好规则，即围捕机器人会倾向于向松散空间方向移动，表现为松散力这一作用力，在其作用下机器人更易向机器人较少的空间进行移动，从而在围猎中出现包抄等高级策略。

由于围捕对象的运动，实际上基于围捕对象所建立的参考系是运动的，而静态项目设计的模型本身连续性欠佳，在参考系开始移动的情况下，这一问题带来的影响被放大，使输出的松散力极不稳定。为解决这一问题，引入一光滑函数smooth对输出的作用力进行修正，该函数利用作为过渡的三角函数，将间断点进行平滑连接，以保证动态围猎下包围圈的稳定形成，以begin表示间断点的下限，end表示间断点的上限，对其中的任意输入量x，如图4所示，该函数的具体映射关系如下：

相关技术中对松散空间给出了严谨的数学定义，但经过实际测试发现在其松散偏好规则下最终难以实现最高效率的围捕，因此对其松散偏好规则进行了一定修改。如图5所示，围捕机器人将获取其视野内最近两个己方机器人的位置r

若视野中没有己方机器人，则松散力为零，松散偏好规则不发挥作用。

若视野中有一个己方机器人，则：

θ

若视野中有两个及以上数量的机器人，则获取最近的两个机器人的位置，若这两个己方机器人都在目标的同一侧，则松散力指向另一侧，使机器人倾向于向较空旷的空间移动；若这两个己方机器人在目标的异侧，则有：

该力根据最近两个己方机器人与该机器人的距离，选择与该机器人距离较远的一侧作为松散空间，倾向于向此侧移动。

松散力主要使得机器人分散，实现包抄、包围等高级策略，因此其应当主要在群体较为接近目标时发挥作用，因此对其进行如下修正，使其在距离目标较远时对机器人的运动影响较小，而距离目标较近时发挥主要作用：

τ＝(-sinθ

其中，θ

经过仿真和实际测试，调整之后机器人在包围目标过程中的运动不连续性频率明显降低，能够更加高效地实现对猎物的围捕，且更加容易形成规整、稳定的包围圈。

在步骤S104中，根据围捕对象、围捕机器人和障碍物的位置计算围捕机器人对围捕对象的第三排斥力和障碍物对围捕对象的第四排斥力，以及围捕对象自身的第五排斥力，并在围捕对象静止或围捕机器人与围捕对象间的距离小于预设值时，计算围捕机器人的环绕力。

对于围猎对象，为了实现对围捕机器人的反围捕逃逸，以尽力延长被围捕时间，需要开发新的逃逸算法。与围捕机器人的“搜索——追逐——包围”的流程对应，围捕对象本身也存在“规避——逃逸——突围”的对应阶段，其与周围围捕机器人数量及所处位置都密切相关。因此，考虑到算法对各阶段的实用性及简洁度，围捕机器人依旧采用了虚拟势场法进行设计。

当围捕对象视野中猎人的数量小于或等于2时，其不可能将围捕对象包围，但由于随时可能出现新的围捕机器人使包围圈能够形成，围捕对象仍应尽力远离视野中的围捕机器人。由于此阶段猎人的运动仅需简单的排斥力引导即可，因此围捕机器人采用了相对科学且简单的库仑力模型来计算围捕机器人对围捕对象的虚拟力。以r

此外，对障碍物而言，猎物也需要对其进行规避，因而可以采用相同的策略，得到障碍物对猎物的排斥力F

当围捕机器人的数量大于2后，为增强围捕对象的对抗性及延长被围捕时间，需要对突围时的势场进行合理设计。当围捕对象处于以围捕机器人为顶点的多边形外时，仍采取远离围捕机器人的策略即可；但当围捕对象进入多边形内部后，此时若仍采用简单的排斥力模型，会在该多边形内形成一局部最优解，导致围捕对象被困在多边形中。该模型下，围捕机器人只需完成包抄动作，围捕对象即无法脱困，存在明显的改进空间。因此对于较近的两个围捕机器人，修改其势力模型为电偶极子模型，其表达式如下：

该势场模型的作用效果可使猎物尽力远离距离其最远的猎人，同时可以尝试对抗最近的两个猎人，从此处突围，如图6所示。

为了避免围捕对象突围后最近的两个围捕机器人仍然存在吸引力而使围捕对象做出错误决策，在原本势力场的基础上再叠加一个斥力场：

该势力场与r

在各围捕机器人都充分接近后，为实现彻底包围，围捕机器人需要确认围捕对象停止后进行包围环绕，即在v

其中

在步骤S105中，将第一吸引力、第一排斥力、第二排斥力、松散力和环绕力进行叠加后，计算围捕机器人的运动速度，将第三排斥力、第四排斥力和第五排斥力进行叠加后计算围捕对象的运动速度，并实时根据围捕机器人的运动速度和围捕对象的运动速度控制围捕机器人和围捕对象运动。

可选地，在本申请的一个实施例中，在计算出围捕机器人的运动速度和围捕对象的运动速度之后，还包括：利用增量式PD算法对围捕机器人的运动速度和围捕对象的运动速度进行修正。

Raspblock视觉全向车搭载树莓派(Raspberry Pi 4B)作为主板，既可以连接摄像机、激光雷达进行目标识别与定位，又可以调控麦克纳姆轮的电机输出，实现运动的实时控制。

围捕机器人在单次运动中，通过摄像机和避障模块获取外部信息，计算得到拟分子间作用力F

由F

最后，为提高围捕机器人运动的鲁棒性，采用增量式PD算法对围捕机器人的最终输出速度v进行修正，记围捕机器人上一时刻的运动速度为v

v＝m×v′+n×(v

该速度将控制围捕机器人的运动。

而对于围捕对象的运动，当围捕机器人包围圈已基本形成时，围捕对象停止逃逸行为，输出速度v

而对预计速度v

v

结合二者，猎物的预计速度v

同样，猎物的最终输出速度也采用增量式PD算法进行修正。

经多次实验验证，机器人在该算法下能快速、均匀、稳定地实现围捕，而猎物也同样能尽可能逃离围捕、延长存活时间。

根据本申请实施例提出的基于虚拟势场的群体智能机器人路径规划方法，对环境的适应性更好，在有障碍物的情况下能够将障碍物纳入到虚拟力场中，实现智能高效的路径规划；优化了虚拟力场，增加了光滑函数，使得机器人的运动更加平滑；能够实现更加复杂的对抗性博弈任务，更加贴合实际应用情景，在面对群体成员退出或加入等突发状况时，群体能自发调整路径、完成运动任务。能实际应用于无人机、无人车等载体实现去中心化的群体智能，且能通过简单复制实现机器人规模的拓展。

其次参照附图描述根据本申请实施例提出的基于虚拟势场的群体智能机器人路径规划装置。

图8为根据本申请实施例的基于虚拟势场的群体智能机器人路径规划装置的示例图。

如图8所示，该基于虚拟势场的群体智能机器人路径规划装置10包括：第一计算模块100、第二计算模块200、第三计算模块300、第四计算模块400和规划模块500。

其中，第一计算模块100，用于根据预测的围捕对象的下一时刻的预测位置和预测速度计算围捕机器人与围捕对象间的第一吸引力。第二计算模块200，用于根据围捕机器人间的距离信息计算围捕机器人间的第一排斥力，根据围捕机器人与障碍物间的距离信息计算围捕机器人的第二排斥力。第三计算模块300，用于根据围捕机器人间的距离信息和位置信息计算围捕机器人的松散力。第四计算模块400，用于根据围捕对象、围捕机器人和障碍物的位置计算围捕机器人对围捕对象的第三排斥力和障碍物对围捕对象的第四排斥力，以及围捕对象自身的第五排斥力，并在围捕对象静止或围捕机器人与围捕对象间的距离小于预设值时，计算围捕机器人的环绕力。规划模块500，用于将第一吸引力、第一排斥力、第二排斥力、松散力和环绕力进行叠加后，计算围捕机器人的运动速度，将第三排斥力、第四排斥力和第五排斥力进行叠加后计算围捕对象的运动速度，并实时根据围捕机器人的运动速度和围捕对象的运动速度控制围捕机器人和围捕对象运动。

需要说明的是，前述对基于虚拟势场的群体智能机器人路径规划方法实施例的解释说明也适用于该实施例的基于虚拟势场的群体智能机器人路径规划装置，此处不再赘述。

根据本申请实施例提出的基于虚拟势场的群体智能机器人路径规划装置，对环境的适应性更好，在有障碍物的情况下能够将障碍物纳入到虚拟力场中，实现智能高效的路径规划；优化了虚拟力场，增加了光滑函数，使得机器人的运动更加平滑；能够实现更加复杂的对抗性博弈任务，更加贴合实际应用情景，在面对群体成员退出或加入等突发状况时，群体能自发调整路径、完成运动任务。能实际应用于无人机、无人车等载体实现去中心化的群体智能，且能通过简单复制实现机器人规模的拓展。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或N个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中，“N个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更N个用于实现定制逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本申请的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本申请的实施例所属技术领域的技术人员所理解。

应当理解，本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，N个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。如，如果用硬件来实现和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。