CN112327890A - 一种基于whca*算法的水下多机器人路径规划 - Google Patents

Abstract

本申请公开了一种基于WHCA*算法的水下多机器人路径规划，其特征在于：包括以下步骤：1)通过多个水下机器人携带的摄像头获取环境信息；2)通过步骤1)获取的环境信息，建立水下三维地图信息，确认水下机器人可以通行的区域；3)建立多个水下机器人之间的通讯，获得各个水下机器人的位置信息，并将位置信息发送至各个水下机器人；4)WHCA*计划阶段：使用WHCA*算法对各个水下机器人的移动路径进行规划；5)每个水下机器人依次从步骤4)中做好的移动路径信息中进行搜索，搜索其从当前位置到其目标位置的路径信息。本发明确定的方法构建一种水下多智能体路径规划和时间规划方法，避免多智能体在移动过程中发生碰撞。

Description

一种基于WHCA*算法的水下多机器人路径规划

技术领域

本发明涉及一种基于WHCA*算法的水下多机器人路径规划，属于机器人控制领域。

背景技术

现有技术是基于反应式避障的水下多智能体协作系统。首先通过摄像头获取环境信息和其他机器人位置信息。给每个机器人分配从当前位置行走到指定位置的任务，如果在行走过程当中有障碍物或者其他机器人阻挡并发生碰撞，机器人执行向左或者向右侧移或者后退躲避开障碍物，如果机器人数目比较多的话可能会出现相互避障的情况，影响整体避障效率。

发明内容

本发明提供一种基于WHCA*算法的水下多机器人路径规划，构建一种水下多智能体路径规划和时间规划方法，避免多智能体在移动过程中发生碰撞。

本发明采取的技术方案是，一种基于WHCA*算法的水下多机器人路径规划，包括以下步骤：

3)通过多个水下机器人携带的摄像头获取环境信息；

4)通过步骤1)获取的环境信息，建立水下三维地图信息，确认水下机器人可以通行的区域；

3)建立多个水下机器人之间的通讯，获得各个水下机器人的位置信息，并将位置信息发送至各个水下机器人；

4)WHCA*计划阶段：使用WHCA*算法对各个水下机器人的移动路径进行规划；

5)每个水下机器人依次从步骤4)中做好的移动路径信息中进行搜索，搜索其从当前位置到其目标位置的路径信息；

6)WHCA*执行阶段：各个水下机器人根据搜索到的路径信息执行相应路径；

7)重复步骤4)至步骤6)的过程，直至各个水下机器人到达其目标位置。

优化的，上述基于WHCA*算法的水下多机器人路径规划，每个机器人依次在步骤2)建立的水下三维地图信息中搜索从当前位置到其目标位置的路径信息。

优化的，上述基于WHCA*算法的水下多机器人路径规划，步骤1)中，多个水下机器人使用三维扫描的方法获取水下环境信息后，将水下环境信息发送至服务器，服务器汇总各个多个水下机器人收集的环境信息并根据环境信息建立水下三维地图信息。

优化的，上述基于WHCA*算法的水下多机器人路径规划，步骤4)中，使用WHCA*算法对各个水下机器人的移动进行路径规划包括：使用WHCA*算法在水下机器人不得相互碰撞的约束下，为所有机器人寻找到从开始位置到最终位置的路径。

优化的，上述基于WHCA*算法的水下多机器人路径规划，在步骤4)和步骤5)中，WHCA* 算法只会对下次规划阶段行动计划作出保留，其过程为：WHCA*算法首先将保留表进行初始化操作，然后计算指定长度路径并存储在每个机器人汇总的保留表当中，在水下机器人移动时，根据保留表中动作移动指定距离；步骤4)和步骤5)中，生成的路径信息保证在下次行动阶段过程中是无冲突的，并且计划路径只在行动阶段是没有冲突的，行动阶段数据始终小于等于计划阶段。

优化的，上述基于WHCA*算法的水下多机器人路径规划，在执行阶段完成以后，擦除保留表当中的数据，并且从机器人的当前位置开始执行一个新的计划阶段；WHCA*算法首先会将保留表进行初始化操作，然后计算直至长度为W的路径并存储在每个机器人汇总的保留表当中，在移动阶段，机器人根据保留表中动作移动指定距离。

优化的，上述基于WHCA*算法的水下多机器人路径规划，在步骤4)中，使用WHCA*算法对各个水下机器人的移动路径进行规划的具体步骤包括：将WHCA*算法分为规划阶段和行动阶段；

7-1)在规划阶段，系统首先清空保留表，内的数据信息，然后随机选择一个机器人先进入规划阶段；

7-2)随机选取的第一个机器人通过全局路径搜索后找到一条由起始位置到最终位置的可行路径，将长度为W的路径的时间和空间信息存储在保留表当中；

7-3)随机选取的第一个机器人执行完成规划阶段后；系统随后再随机选择一个机器人执行规划阶段，随机选取的第二个机器人通过全局路径搜索后找到一条由起始位置到最终位置的可行路径，将长度为W的路径的时间和空间信息存储在保留表当中；

7-4)随机选取的第二个机器人执行完规划阶段后，从剩余机器人当中再随机选择一个机器人；随机选取的第三个机器人通过全局路径搜索后找到一条由起始位置到最终位置的可行路径，将长度为W的路径的时间和空间信息存储在保留表当中；

7-5)随机选取的第三个机器人执行完规划阶段后，从剩余机器人当中再随机选择一个机器人并重复步骤7-2)至步骤7-4)，直至遍历全部机器人；

7-6)所有机器人进入行动阶段，根据保留表当中存储的前W步路径进行移动；移动完成后重新进入规划阶段重复上述过程，直至所有机器人到达系统设置的最终位置。

优化的，上述基于WHCA*算法的水下多机器人路径规划，在步骤6)中，随机选取的第二个机器人的长度为W的路径中不能够与随机选取的第一个机器人存在同一时间位于同一位置的情况，如果在W长度的路径内存在碰撞的可能性，则随机选取的第二个机器人通过全局路径重新搜索一条由起始位置到最终位置的可行路径；如果不存在其他路径，则随机选取的第二个机器人会在可能发生碰撞位置的前一个位置停止不动，等待随机选取的第一个机器人通过后再继续执行保留表中的路径；

在步骤6)中，随机选取的第三个机器人的长度为W的路径中不能够与随机选取的第一个机器人或随机选取的第二个机器人存在同一时间位于同一位置的情况，即不能发生碰撞，如果在W长度的路径内存在碰撞的可能性，则随机选取的第三个机器人通过全局路径重新搜索一条由起始位置到最终位置的可行路径；如果不存在其他路径，则随机选取的第三个机器人会在可能发生碰撞位置的前一个位置停止不动，等待其他机器人通过后再继续执行保留表中的路径。

优化的，上述基于WHCA*算法的水下多机器人路径规划，在步骤4)中使用WHCA*算法对各个水下机器人的移动路径进行规划时，使用Q-learning方法进行强化学习，Q-learning 强化学习方法通过状态、动作、奖励三个要素来不断对水下机器人的动作进行优化，其过程包括：

建立虚拟仿真环境，在水下障碍物的位置标注出黑色方块表示无法通行，系统给出机器人的初始位置以及机器人最终需要到达的位置；

机器人根据当前状态执行向四个方向移动或者原地不动的动作；机器人每移动一步或原地不同则给予机器人奖励值为-1，若机器人发生碰撞则给予奖励值为-10，当机器人到达指定位置时给予奖励值+1000；

对于单一机器人训练机器人从初始位置到最终位置的得分最高，对于多机器人训练，评价指标为所有机器人得分总和；

在水下机器人移动过程中，为每个状态所对应的动作设定一个效用值，在不断迭代训练过程中，机器人对于每一个状态下尽量选择效用值较大的动作作为当前状态下要执行的动作，帮助机器人避免碰撞快速到达最终位置。

优化的，上述基于WHCA*算法的水下多机器人路径规划，在步骤5)中，通过机器人对当前状态判断进行选择最优路径，具体包括：

机器人通过全局路径搜索时，最优路径为机器人从起始位置到最终位置的最短路径，在执行WHCA*算法时，规划阶段仅将最优路径的前W部分存储到保留表当中；在Q-learning 当中得分最高的路径为最优路径；

如果机器人将要发生碰撞，即在上一个规划阶段加行动阶段的周期内未检测到碰撞，则在新周期的规划阶段，会随机从一个机器人开始，进行路径规划并将其前W步路径的时间信息和空间信息存储到保留表当中，则另一个机器人在进行规划阶段时候会使用A*全局搜索重新进行路径搜索，避免与保留表中存在时间和空间上路径信息重合的问题，即避免发生碰撞；

如果未搜索到其他路径，则会在发生碰撞前一个位置保持不动，等另一个机器人通过后再继续执行保留表中的路径规划信息，避免发生碰撞。

本次发明的技术方案分为三个阶段完成：第一阶段为：获取并分析环境信息；第二阶段为：使用WHCA*算法进行时间和路径规划；第三阶段为：根据计划执行相应路径并解决冲突问题。

第一阶段：水下机器人通过摄像头获取水下环境信息。机器人通过获取的环境信息建立水下三维地图信息，以确认机器人可以通行的区域。机器人通过通讯系统获取其他机器人位置信息。

第二阶段：WHCA*算法分为计划阶段和移动阶段两部分。首先WHCA*算法会进行计划阶段，每个机器人依次搜索从当前位置到其目标位置的路径信息。WHCA*算法只会对下次规划阶段行动计划作出保留。因此，生成的路径信息保证在下次行动阶段过程中是无冲突的。在移动阶段，每个机器人根据计划阶段保留路径进行行动。由于计划路径只在行动阶段是没有冲突的，所以行动阶段数据始终小于等于计划阶段。在执行阶段完成以后将会擦除保留表当中的数据，并且从机器人的当前位置开始执行一个新的计划阶段。WHCA*算法首先会将保留表进行初始化操作。然后计算直至长度为W的路径并存储在每个机器人汇总的保留表当中，然后移动阶段机器人根据保留表中动作移动指定距离。

本发明技术方案带来的有益效果为：能够避免多智能体在移动过程中的碰撞问题，相对于现有方法能能够减少时间成本。

具体实施方式

下面结合具体实施例进一步阐述本发明的技术特点。

本发明为一种基于WHCA*算法的水下多机器人路径规划，包括以下步骤：

通过多个水下机器人携带的摄像头获取环境信息；

通过步骤1)获取的环境信息，建立水下三维地图信息，确认水下机器人可以通行的区域；

3)建立多个水下机器人之间的通讯，获得各个水下机器人的位置信息，并将位置信息发送至各个水下机器人；

4)WHCA*计划阶段：使用WHCA*算法对各个水下机器人的移动路径进行规划；

5)每个水下机器人依次从步骤4)中做好的移动路径信息中进行搜索，搜索其从当前位置到其目标位置的路径信息；

6)WHCA*执行阶段：各个水下机器人根据搜索到的路径信息执行相应路径；

7)重复步骤4)至步骤6)的过程，直至各个水下机器人到达其目标位置。

每个机器人依次在步骤2)建立的水下三维地图信息中搜索从当前位置到其目标位置的路径信息。

步骤1)中，多个水下机器人使用三维扫描的方法获取水下环境信息后，将水下环境信息发送至服务器，服务器汇总各个多个水下机器人收集的环境信息并根据环境信息建立水下三维地图信息。

步骤4)中，使用WHCA*算法对各个水下机器人的移动进行路径规划包括：使用WHCA* 算法在水下机器人不得相互碰撞的约束下，为所有机器人寻找到从开始位置到最终位置的路径。

在步骤4)和步骤5)中，WHCA*算法只会对下次规划阶段行动计划作出保留，其过程为：WHCA*算法首先将保留表进行初始化操作，然后计算指定长度路径并存储在每个机器人汇总的保留表当中，在水下机器人移动时，根据保留表中动作移动指定距离；步骤4)和步骤5)中，生成的路径信息保证在下次行动阶段过程中是无冲突的，并且计划路径只在行动阶段是没有冲突的，行动阶段数据始终小于等于计划阶段。

在执行阶段完成以后，擦除保留表当中的数据，并且从机器人的当前位置开始执行一个新的计划阶段；WHCA*算法首先会将保留表进行初始化操作，然后计算直至长度为W的路径并存储在每个机器人汇总的保留表当中，在移动阶段，机器人根据保留表中动作移动指定距离。

下面通过举例说明WHCA*对各个水下机器人进行路径规划的具体过程和算法。

例如，有3个水下机器人在指定区域内移动，系统获取每个机器人的当前位置信息并给出最终需要达到的位置信息。每个机器人均需要在绕过障碍物并且避免发生碰撞的前提下达到系统设置的最终位置。系统采取WHCA*算法对机器人行动路线进行规划，WHCA*算法分为两个阶段：规划阶段和行动阶段。在规划阶段，系统会先清空保留表(保留表主要用于存储规划阶段的规划路径以及作为行动阶段的参考)内的数据信息，然后随机选择一个机器人先进入规划阶段。每次先进入规划阶段的机器人时随机的，主要为了避免因为固定顺序而导致的某些机器人每次必须进行绕路的行为。此处以随机选择1号机器人为例进行具体操作说明，1号机器人通过全局路径搜索后找到一条由起始位置到最终位置的可行路径，将长度为W的路径的时间和空间信息存储在保留表当中。(此处保留路径应该小于等于路径长度，一般采用固定值，例如路径总长度的四分之一)当1号机器人执行完成规划阶段后，系统随机选择一个机器人执行规划阶段，此处以随机选择2号机器人为例进行说明，2号机器人通过全局路径搜索后找到一条由起始位置到最终位置的可行路径，将长度为 W的路径的时间和空间信息存储在保留表当中。此处需要注意的是，在长度为W的路径中不能够与1号机器人存在同一时间位于同一位置的情况，即不能发生碰撞，如果在W长度的路径内存在碰撞的可能性，则2号机器人通过全局路径重新搜索一条由起始位置到最终位置的可行路径。如果不存在其他路径，则2号机器人会在可能发生碰撞位置的前一个位置停止不动，等待1号机器人通过后再继续执行保留表中的路径。当2号机器人执行完规划阶段后，从剩余机器人当中随机选择一个机器人。此处以随机选择3号机器人为例进行说明，3号机器人通过全局路径搜索后找到一条由起始位置到最终位置的可行路径，将长度为 W的路径的时间和空间信息存储在保留表当中。此处需要注意的是，在长度为W的路径中不能够与1号机器人或2号机器人存在同一时间位于同一位置的情况，即不能发生碰撞，如果在W长度的路径内存在碰撞的可能性，则3号机器人通过全局路径重新搜索一条由起始位置到最终位置的可行路径。如果不存在其他路径，则3号机器人会在可能发生碰撞位置的前一个位置停止不动，等待其他机器人通过后再继续执行保留表中的路径。当3号机器人执行完规划阶段后，在此时所有机器人均已经执行完成了规划阶段，所有机器人进入行动阶段，根据保留表当中存储的前W步路径进行移动。移动完成后重新进入规划阶段重复上述过程，直至所有机器人到达系统设置的最终位置。

WHCA*算法的编程实例如下：

使用Q-learning强化学习方法训练模型的具体过程为：

Q-learning强化学习方法通过状态、动作、奖励三个要素来不断对水下机器人的动作进行优化。建立虚拟仿真环境，在水下障碍物的位置标注出黑色方块表示无法通行，系统给出机器人的初始位置以及机器人最终需要到达的位置。机器人会根据当前状态执行向四个方向移动或者原地不动的动作。机器人每移动一步或原地不同则给予机器人奖励值为-1，若机器人发生碰撞则给予奖励值为-10，当机器人到达指定位置时给予奖励值+1000。对于单一机器人训练机器人从初始位置到最终位置的得分最高。对于多机器人训练，评价指标为所有机器人得分总和。Q-learning当中Q表示动作效用函数，用于评价在特定状态下采取某个动作的优劣。在水下机器人移动过程中，每个状态所对应的动作都有一个效用值，在不断迭代训练过程中，使得机器人对于每一个状态下尽量选择效用值较大的动作作为当前状态下要执行的动作，帮助机器人避免碰撞快速到达最终位置。

通过机器人对当前状态判断进行选择最优路径，状态判断的具体过程和如何选择最优路径，请具体说明。如果将要发生碰撞，保留一个机器人路径信息，其他发生冲突机器人使用A*搜索替代路径。

机器人通过全局路径搜索时，最优路径为机器人从起始位置到最终位置的最短路径，在执行WHCA*算法时，规划阶段仅将最优路径的前W部分存储到保留表当中。在Q-learning 当中得分最高的路径为最优路径。如果机器人将要发生碰撞(该情况可能发生于在W+1步将会发生碰撞的条件)即在上一个周期(规划阶段+行动阶段)内未检测到碰撞。此时，在新周期的规划阶段，会随机从一个机器人开始，进行路径规划并将其前W步路径的时间信息和空间信息存储到保留表当中，则另一个机器人在进行规划阶段时候会使用A*全局搜索重新进行路径搜索，避免与保留表中存在时间和空间上路径信息重合的问题，即避免发生碰撞。如果未搜索到其他路径，则会在发生碰撞前一个位置保持不动，等另一个机器人通过后再继续执行保留表中的路径规划信息，避免发生碰撞。。

当然，上述说明并非是对本发明的限制，本发明也并不限于上述举例，本技术领域的普通技术人员，在本发明的实质范围内，作出的变化、改型、添加或替换，都应属于本发明的保护范围。

Claims (10)

1.一种基于WHCA*算法的水下多机器人路径规划，其特征在于：包括以下步骤：

1)通过多个水下机器人携带的摄像头获取环境信息；

2)通过步骤1)获取的环境信息，建立水下三维地图信息，确认水下机器人可以通行的区域；

3)建立多个水下机器人之间的通讯，获得各个水下机器人的位置信息，并将位置信息发送至各个水下机器人；

4)WHCA*计划阶段：使用WHCA*算法对各个水下机器人的移动路径进行规划；

5)每个水下机器人依次从步骤4)中做好的移动路径信息中进行搜索，搜索其从当前位置到其目标位置的路径信息；

6)WHCA*执行阶段：各个水下机器人根据搜索到的路径信息执行相应路径；

7)重复步骤4)至步骤6)的过程，直至各个水下机器人到达其目标位置。

2.根据权利要求1所述的基于WHCA*算法的水下多机器人路径规划，其特征在于：每个机器人依次在步骤2)建立的水下三维地图信息中搜索从当前位置到其目标位置的路径信息。

3.根据权利要求1所述的基于WHCA*算法的水下多机器人路径规划，其特征在于：步骤1)中，多个水下机器人使用三维扫描的方法获取水下环境信息后，将水下环境信息发送至服务器，服务器汇总各个多个水下机器人收集的环境信息并根据环境信息建立水下三维地图信息。

4.根据权利要求1所述的基于WHCA*算法的水下多机器人路径规划，其特征在于：步骤4)中，使用WHCA*算法对各个水下机器人的移动进行路径规划包括：使用WHCA*算法在水下机器人不得相互碰撞的约束下，为所有机器人寻找到从开始位置到最终位置的路径。

5.根据权利要求1所述的基于WHCA*算法的水下多机器人路径规划，其特征在于：在步骤4)和步骤5)中，WHCA*算法只会对下次规划阶段行动计划作出保留，其过程为：WHCA*算法首先将保留表进行初始化操作，然后计算指定长度路径并存储在每个机器人汇总的保留表当中，在水下机器人移动时，根据保留表中动作移动指定距离；步骤4)和步骤5)中，生成的路径信息保证在下次行动阶段过程中是无冲突的，并且计划路径只在行动阶段是没有冲突的，行动阶段数据始终小于等于计划阶段。

6.根据权利要求1所述的基于WHCA*算法的水下多机器人路径规划，其特征在于：在执行阶段完成以后，擦除保留表当中的数据，并且从机器人的当前位置开始执行一个新的计划阶段；WHCA*算法首先会将保留表进行初始化操作，然后计算直至长度为W的路径并存储在每个机器人汇总的保留表当中，在移动阶段，机器人根据保留表中动作移动指定距离。

7.根据权利要求1所述的基于WHCA*算法的水下多机器人路径规划，其特征在于：在步骤4)中，使用WHCA*算法对各个水下机器人的移动路径进行规划的具体步骤包括：将WHCA*算法分为规划阶段和行动阶段；

7-1)在规划阶段，系统首先清空保留表，内的数据信息，然后随机选择一个机器人先进入规划阶段；

7-2)随机选取的第一个机器人通过全局路径搜索后找到一条由起始位置到最终位置的可行路径，将长度为W的路径的时间和空间信息存储在保留表当中；

7-3)随机选取的第一个机器人执行完成规划阶段后；系统随后再随机选择一个机器人执行规划阶段，随机选取的第二个机器人通过全局路径搜索后找到一条由起始位置到最终位置的可行路径，将长度为W的路径的时间和空间信息存储在保留表当中；

7-4)随机选取的第二个机器人执行完规划阶段后，从剩余机器人当中再随机选择一个机器人；随机选取的第三个机器人通过全局路径搜索后找到一条由起始位置到最终位置的可行路径，将长度为W的路径的时间和空间信息存储在保留表当中；

7-5)随机选取的第三个机器人执行完规划阶段后，从剩余机器人当中再随机选择一个机器人并重复步骤7-2)至步骤7-4)，直至遍历全部机器人；

7-6)所有机器人进入行动阶段，根据保留表当中存储的前W步路径进行移动；移动完成后重新进入规划阶段重复上述过程，直至所有机器人到达系统设置的最终位置。

8.根据权利要求7所述的基于WHCA*算法的水下多机器人路径规划，其特征在于：在步骤6)中，随机选取的第二个机器人的长度为W的路径中不能够与随机选取的第一个机器人存在同一时间位于同一位置的情况，如果在W长度的路径内存在碰撞的可能性，则随机选取的第二个机器人通过全局路径重新搜索一条由起始位置到最终位置的可行路径；如果不存在其他路径，则随机选取的第二个机器人会在可能发生碰撞位置的前一个位置停止不动，等待随机选取的第一个机器人通过后再继续执行保留表中的路径；

在步骤6)中，随机选取的第三个机器人的长度为W的路径中不能够与随机选取的第一个机器人或随机选取的第二个机器人存在同一时间位于同一位置的情况，即不能发生碰撞，如果在W长度的路径内存在碰撞的可能性，则随机选取的第三个机器人通过全局路径重新搜索一条由起始位置到最终位置的可行路径；如果不存在其他路径，则随机选取的第三个机器人会在可能发生碰撞位置的前一个位置停止不动，等待其他机器人通过后再继续执行保留表中的路径。

9.根据权利要求1所述的基于WHCA*算法的水下多机器人路径规划，其特征在于：在步骤4)中使用WHCA*算法对各个水下机器人的移动路径进行规划时，使用Q-learning方法进行强化学习，Q-learning强化学习方法通过状态、动作、奖励三个要素来不断对水下机器人的动作进行优化，其过程包括：

建立虚拟仿真环境，在水下障碍物的位置标注出黑色方块表示无法通行，系统给出机器人的初始位置以及机器人最终需要到达的位置；

机器人根据当前状态执行向四个方向移动或者原地不动的动作；机器人每移动一步或原地不同则给予机器人奖励值为-1，若机器人发生碰撞则给予奖励值为-10，当机器人到达指定位置时给予奖励值+1000；

对于单一机器人训练机器人从初始位置到最终位置的得分最高，对于多机器人训练，评价指标为所有机器人得分总和；

在水下机器人移动过程中，为每个状态所对应的动作设定一个效用值，在不断迭代训练过程中，机器人对于每一个状态下尽量选择效用值较大的动作作为当前状态下要执行的动作，帮助机器人避免碰撞快速到达最终位置。

10.根据权利要求9所述的基于WHCA*算法的水下多机器人路径规划，其特征在于：在步骤5)中，通过机器人对当前状态判断进行选择最优路径，具体包括：

机器人通过全局路径搜索时，最优路径为机器人从起始位置到最终位置的最短路径，在执行WHCA*算法时，规划阶段仅将最优路径的前W部分存储到保留表当中；在Q-learning当中得分最高的路径为最优路径；

如果机器人将要发生碰撞，即在上一个规划阶段加行动阶段的周期内未检测到碰撞，则在新周期的规划阶段，会随机从一个机器人开始，进行路径规划并将其前W步路径的时间信息和空间信息存储到保留表当中，则另一个机器人在进行规划阶段时候会使用A*全局搜索重新进行路径搜索，避免与保留表中存在时间和空间上路径信息重合的问题，即避免发生碰撞；

如果未搜索到其他路径，则会在发生碰撞前一个位置保持不动，等另一个机器人通过后再继续执行保留表中的路径规划信息，避免发生碰撞。