CN114371711A - 一种机器人编队避障路径规划方法 - Google Patents

Abstract

本发明提供的一种机器人编队避障路径规划方法，所述路径规划方法包括：获取机器人参数信息、路径约束信息、任务环境信息和算法参数信息；获取实际环境中的障碍物信息，并构建真实环境栅格图，将地图均匀分成等比例大小的栅格；根据所述真实环境栅格图获得机器人编队路径规划构建空间；采用VSF‑A*的队形可变的编队路径规划算法在所述机器人编队路径规划构建空间中进行搜索，获得可行的机器人编队路径。根据实例任务需要，一种考虑队形可变的机器人编队路径规划方法具有路径长度短、避障能力强的优点。

Description

一种机器人编队避障路径规划方法

技术领域

本发明涉及机器人领域，尤其涉及一种机器人编队避障路径规划方法。

背景技术

随着科技迅速发展，机器人被应用到多种领域，如探索、侦察、搜救。但由于真实环境的复杂性和单机器人性能的局限性，仅靠单机器人难以完成复杂任务且效率较低，因而需要多机器人编队通过相互协作配合完成复杂任务。为使机器人编队能快速到达目标点执行任务，需要规划出一条无避碰且使得特定性能指标最优的可行路径。

针对多机器人编队路径规划问题，现有技术中通常要求机器人编队保持队形不变的条件下，环境存在机器人编队的可行路径。然而，障碍较多的环境难以满足上述要求，机器人编队将无法前往指定目标，同时队形保持常常需要绕行较远的路径以实现避撞，导致编队执行任务的效率降低。

发明内容

鉴于上述问题，提出了本发明以便提供克服上述问题或者至少部分地解决上述问题的一种机器人编队避障路径规划方法。

根据本发明的一个方面，提供了一种机器人编队避障路径规划方法，所述路径规划方法包括：

获取机器人参数信息、路径约束信息、任务环境信息和算法参数信息；

获取实际环境中的障碍物信息，并构建真实环境栅格图，将地图均匀分成等比例大小的栅格；

根据所述真实环境栅格图获得机器人编队路径规划构建空间；

采用基于VSF-A*算法的队形可变的编队路径规划算法在所述机器人编队路径规划构建空间中进行搜索，获得可行的机器人编队路径。

可选的，所述机器人参数信息包括机器人的尺寸，所述路径约束信息包括机器人编队的起点位置和目标点位置，所述任务环境信息包括地图尺寸、障碍物数量和障碍物位置。

可选的，所述根据所述真实环境栅格图构建机器人编队路径规划具体包括：

对所述真实环境栅格图中的障碍区进行一次膨胀获得单个机器人的避障环境地图；

对所述真实环境栅格图中的障碍区进行二次膨胀获得机器人编队的避障环境地图；

根据所述单个机器人的避障环境地图和所述机器人编队的避障环境地图获得机器人编队路径规划构型空间。

可选的，所述路径规划方法还包括：

获取编队路径规划路径长度指标和队形保持指标；

建立下述多目标规划指标：

minω₁J₁+ω₂J₂ (1)

路径长度指标J₁，编队路径由一系列离散的路径点构成，编队路径为Path＝{P_i,i＝1,2,..,N}，其中P_i为第i个路径点，N为路径点的总数量，路径长度指标为：

其中，L_i为第i段路径的长度，线段P_iP_i+1的长度|P_iP_i+1|；

队形保持指标J₂表示为：

J₂＝L_C/L (3)

其中，L为路径总长度，L_C为纵向队形运动的路径长度，路径长度为路径穿越障碍膨胀区域的长度。

可选的，所述采用基于VSF-A*的队形可变的编队路径规划算法在所述机器人编队路径规划构建空间中进行搜索，获得可行的机器人编队路径具体包括：

初始化算法中的OPEN表和CLOSE表，并将规划的起点插入OPEN表，CLOSE表为空；

判断所述OPEN表是否为空，若OPEN表为空，则结束搜索；否则，更新当前节点；

从当前的所述OPEN表中取出代价值最小的节点作为新的当前节点，将当前节点从所述OPEN表中删除，并放入所述CLOSE表；

判断当前节点是否为目标节点，如果当前节点为目标节点，结束节点扩展循环，创建目标节点，父节点设置为当前节点，并将所述目标节点压入CLOSE表；

否则，节点扩展与存储；以当前节点为中心进行节点扩展，获得当前节点的子节点；并判断所述子节点的可行性，计算所有可行子节点的代价值，并将所述可行子节点存入OPEN表中，同时更新当前节点；

反溯最终规划航迹；根据目标节点和CLOSE表中的已扩展节点，利用节点间的扩展关系，从目标节点向上反溯直至起始节点，得到从起始点到目标点的路径，所述路径为满足编队机器人变形的最短路径。

可选的，所述节点代价值计算的具体步骤包括：

考虑路径长度最小化和编队保持的需求，构建统一化的代价函数，如式(4)所示；

每个节点的代价值f包括累计长度代价g、估计长度代价h、穿越软障碍代价e；

f＝ω₁(g+h)+ω₂e (4)

其中g为从起点到当前点的路径长度，h为当前点到目标点的估计长度，e为编队路径规划通过软障碍区域的路径长度，ω₁为路径长度的权重系数，ω₂为经过软障碍区域的路径长度权重系数。

本发明提供的一种机器人编队避障路径规划方法，所述路径规划方法包括：获取机器人参数信息、路径约束信息、任务环境信息和算法参数信息；获取实际环境中的障碍物信息，并构建真实环境栅格图，将地图均匀分成等比例大小的栅格；根据所述真实环境栅格图获得机器人编队路径规划构建空间；采用基于VSF-A*的队形可变的编队路径规划算法在所述机器人编队路径规划构建空间中进行搜索，获得可行的机器人编队路径。根据实例任务需要，一种考虑队形可变的机器人编队路径规划方法具有路径长度短、避障能力强的优点。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1为本发明实施例提供的一种机器人编队避障路径规划方法流程图；

图2为本发明实施例所提供的机器人编队路径规划问题示意图；

图3为本发明实施例所提供的机器人编队路径规划环境栅格图；

图4为本发明实施例所提供的机器人编队障碍一次膨胀地图；

图5为本发明实施例所提供的机器人编队障碍二次膨胀地图；

图6为本发明实施例所提供的机器人编队路径规划结果图。

具体实施方式

下面将参照附图更详细地描述本发明公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。

本发明的说明书实施例和权利要求书及附图中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元。

下面结合附图和实施例，对本发明的技术方案做进一步的详细描述。

如图1所示，本发明公开的一种考虑队形可变的机器人编队路径规划方法，针对机器人编队路径规划问题，在A*路径规划的框架上，引入穿越软障碍路径代价指标，构建包含路径长度和软障碍违背度的统一化代价函数，以生成一条能够规避障碍且长度较小的编队路径，实现机器人编队快速抵达目标点。规划出的结果与传统队形不变情况下的结果对比如图2所示。

本发明公开的一种考虑队形可变的机器人编队路径规划方法，包括如下步骤：

步骤一：获取机器人参数信息、路径约束信息、任务环境信息和算法参数信息。机器人参数信息包括机器人的尺寸。路径约束信息包括机器人编队的起点位置和目标点位置。任务环境信息包括地图尺寸、障碍数量和位置。

步骤二：基于真实环境中的障碍信息，构建真实环境栅格图，其中将地图均匀分成等比例大小的栅格。对真实环境栅格图中的障碍区进行一次膨胀获得考虑单个机器人避障的环境地图，对真实环境栅格图中的障碍区进行二次膨胀获得考虑机器人编队避障的环境地图，获得机器人编队路径规划的构型空间。

步骤三：构建编队路径规划中需考虑的路径长度指标和队形保持指标。

针对编队路径规划问题，为了使得编队在障碍环境下仍能具有较高的通过性，应尽可能地减小纵向队形的路径长度，同时考虑到尽可能快地抵达目标点，应减小编队的路径总长度。建立下述多目标规划指标：

minω₁J₁+ω₂J₂ (1)

路径长度指标J₁：编队路径由一系列离散的路径点构成，记为Path＝{P_i,i＝1,2,..,N}，其中P_i为第i个路径点，N为路径点的总数量，路径长度指标为：

其中，L_i为第i段路径的长度，线段P_iP_i+1的长度|P_iP_i+1|。

队形保持指标J₂：编队任务过程中，应尽可能保证编队队形，即尽可能增大采用期望队形运动的路径长度比例，或者尽可能减小采用纵向队形实现障碍规避的路径长度比例，因此队形保持指标可表示为：

J₂＝L_C/L (3)

其中，L为路径总长度，L_C为路径穿越障碍二次膨胀区域的长度。

步骤四：利用提出的基于VSF-A*的队形可变的编队路径规划算法，在构建的机器人编队路径规划构型空间中进行搜索，获得可行的机器人编队路径，流程如下：

步骤1)：初始化算法中的OPEN表和CLOSE表，同时将规划的起点插入OPEN表，CLOSE表为空；

步骤2)：判断OPEN表是否为空，若OPEN表为空，则结束搜索；若OPEN表不为空，则执行步骤3)。

步骤3)：更新当前节点。从当前的OPEN表中取出代价值最小的节点作为新的当前节点，将当前节点从OPEN表中删除，并放入CLOSE表。

节点代价值计算的具体实现如下：

为了实现队形可变编队的路径规划，综合考虑路径长度最小化和编队保持的需求，构建统一化的代价函数，如式(4)所示。每个节点的代价值f包括累计长度代价g、估计长度代价h、穿越软障碍代价e。

f＝ω₁(g+h)+ω₂e (4)

其中g为从起点到当前点的路径长度，h为当前点到目标点的估计长度，e为编队路径规划通过软障碍区域的路径长度，ω₁为路径长度的权重系数，ω₂为经过软障碍区域的路径长度权重系数。

步骤4)：判断当前节点是否为目标节点。若当前节点为目标节点，则结束节点扩展循环，转而执行步骤6)；否则执行步骤5)。

步骤5)：节点扩展与存储。以当前节点为中心进行节点扩展，获得当前节点的子节点。判断子节点的可行性，计算所有可行子节点的代价值，并将可行子节点存入OPEN表中。然后执行步骤3)。

步骤6)：创建目标节点，其父节点设置为当前节点，并将目标节点压入CLOSE表。

步骤7)：反溯最终规划航迹。根据目标节点和CLOSE表中的已扩展节点，利用节点间的扩展关系，从目标节点向上反溯直至起始节点，获得从起始点到目标点的路径，路径为满足编队机器人变形的最短路径。

实施例1：

本实施例所提供的一种考虑队形可变的机器人编队路径规划方法具体实现步骤如下：

步骤一：输入机器人编队参数信息、路径约束信息、任务环境信息和算法参数信息。

设定机器人大小为0.4m*0.4m，初始位置为(4，20),初始速度为(0m/s,0m/s)^T，目标位置为(198，80),地图大小为200*100，障碍数量15个。

步骤二：根据上述具体实例的参数输入，建立针对该机器人编队路径规划问题的环境地图。如图3所示，环境地图规模为200*100，分别率为0.1m，包含15个障碍，小大都等于一个栅格(0.1m*0.1m),其位置分别为(70，100)、(40，32)、(51，22)、(124，21)、(95，66)、(71，34)、(167，17)、(118，6)、(110，95)、(184，44)、(58，17)、(152，45)、(151，75)、(77，9)、(114，57)。

如图4所示，对真实环境地图中的障碍进行一次膨胀获得新的环境地图，其中黑色区域为真实环境中的障碍区域，深灰色区域表示根据单个机器人尺寸(0.4m)膨胀得到的硬障碍区，在该区域内单个机器人可以通行，编队机器人保持编队不变无法通行，白色区域记为可通行区。

如图5所示，对真实环境地图中的障碍进行二次膨胀获得新的环境地图，其中浅灰色区域表示根据三角编队尺寸(1.2m)膨胀得到的软障碍区，在该区域内编队机器人保持三角编队无法通过，但是若采用纵向队形是可以依次通行，白色区域记为可通行区。

为保证机器人的运行安全，路径规划时需进行障碍检测，以保证规划的路径不会穿过不可通行区。

步骤三：构建编队路径规划中需考虑的障碍约束条件、路径长度指标和队形保持指标。

minω₁J₁+ω₂J₂

J₂＝L_C/L

本实施例中，取ω₁＝1，ω₂＝5。

步骤四：利用本发明提出的VSF-A*算法，在构建的构型空间中进行搜索，获得可行的机器人编队路径，VSF-A*算法流程如下：

步骤1)：初始化算法中的OPEN表和CLOSE表，同时将规划的起点插入OPEN表，此时CLOSE表为空；

步骤2)：判断OPEN表是否为空，若OPEN表为空，则结束搜索；若OPEN表为空，则执行步骤4)。

步骤3)：更新当前节点。从当前的OPEN表中取出代价值最小的节点作为新的当前节点，将当前节点从OPEN表中删除，并放入CLOSE表。

节点代价值计算的具体实现如下：

f＝ω₁(g+h)+ω₂e

其中g表示从起点到当前点的路径长度，h表示当前点到目标点的估计长度，e表示编队路径规划通过软障碍区域的路径长度，ω₁表示路径长度的权重系数，ω₂表示经过软障碍区域的路径长度权重系数，本实施例中ω₁＝1，ω₂＝10。

步骤4)：判断当前节点能否满足收敛条件。若当前节点能够在满足所有所述的条件下能到达目标节点，则结束节点扩展循环，转而执行步骤6)；否则执行步骤5)。

步骤5)：节点扩展与存储。以当前节点为中心进行节点扩展，获得当前节点的子节点。判扩展子节点的可行性，计算所有可行节点的代价值，并将从可行节点存入OPEN表中。然后执行步骤3)。

步骤6)：创建目标节点，其父节点设置为当前节点，并将目标节点压入CLOSE表。

步骤7)：反溯最终规划航迹。根据目标节点和CLOSE表中的已扩展节点，利用节点间的扩展关系，从目标节点向上反溯直至起始节点，得到从起始点到目标点的路径，该路径即为满足编队机器人变形的最短路径。

针对上述具体实例，利用本实施例所述的一种考虑队形可变的机器人编队路径规划方法得到的路径结果如图6中下方的路径所示。

采用队形不可变的机器人编队传统路径规划得到的路径结果如图6中上方的路径所示。尽管传统方法得到的路径结果能够规避障碍并引导机器人编导到达目标位置，但是其规划出的路径需要绕行实现障碍规避，而本专利提出的路径规划方法能够通过队形变化更好地实现障碍规避，并能够以更小的路径长度抵达目标点。

根据前述的机器人编队路径生成仿真结果与分析可见，本实施例所述的一种考虑队形可变的机器人编队路径规划方法能够为机器人编队提供满足实际复杂约束的可行路径，且路径结果与传统方法相比，具有路径长度短，避障能力强的优点，因此本发明具有工程实用性，能够实现预期的发明目的。

有益效果：能够根据机器人编队抵达给定目标点的任务需求，利用队形可变的编队运动灵活性，生成一条满足要求的机器人编队可行路径，具有路径长度短、避障能力强的优点。

以上的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种机器人编队避障路径规划方法，其特征在于，所述路径规划方法包括：

获取机器人参数信息、路径约束信息、任务环境信息和算法参数信息；

获取实际环境中的障碍物信息，并构建真实环境栅格图，将地图均匀分成等比例大小的栅格；

根据所述真实环境栅格图获得机器人编队路径规划构建空间；

采用基于VSF-A*算法的队形可变的编队路径规划算法在所述机器人编队路径规划构建空间中进行搜索，获得可行的机器人编队路径。

2.根据权利要求1所述的一种机器人编队避障路径规划方法，其特征在于，所述机器人参数信息包括机器人的尺寸，所述路径约束信息包括机器人编队的起点位置和目标点位置，所述任务环境信息包括地图尺寸、障碍物数量和障碍物位置。

3.根据权利要求1所述的一种机器人编队避障路径规划方法，其特征在于，所述根据所述真实环境栅格图构建机器人编队路径规划具体包括：

对所述真实环境栅格图中的障碍区进行一次膨胀获得单个机器人的避障环境地图；

对所述真实环境栅格图中的障碍区进行二次膨胀获得机器人编队的避障环境地图；

根据所述单个机器人的避障环境地图和所述机器人编队的避障环境地图获得机器人编队路径规划构型空间。

4.根据权利要求1所述的一种机器人编队避障路径规划方法，其特征在于，所述路径规划方法还包括：

获取编队路径规划路径长度指标和队形保持指标；

建立下述多目标规划指标：

min ω₁J₁+ω₂J₂ (1)

路径长度指标J₁，编队路径由一系列离散的路径点构成，编队路径为Path＝{P_i,i＝1,2,..,N}，其中P_i为第i个路径点，N为路径点的总数量，路径长度指标为：

其中，L_i为第i段路径的长度，线段P_iP_i+1的长度|P_iP_i+1|；

队形保持指标J₂表示为：

J₂＝L_C/L (3)

其中，L为路径总长度，L_C为纵向队形运动的路径长度，路径长度为路径穿越障碍膨胀区域的长度。

5.根据权利要求1所述的一种机器人编队避障路径规划方法，其特征在于，所述采用基于VSF-A*的队形可变的编队路径规划算法在所述机器人编队路径规划构建空间中进行搜索，获得可行的机器人编队路径具体包括：

初始化算法中的OPEN表和CLOSE表，并将规划的起点插入OPEN表，CLOSE表为空；

判断所述OPEN表是否为空，若OPEN表为空，则结束搜索；否则，更新当前节点；

从当前的所述OPEN表中取出代价值最小的节点作为新的当前节点，将当前节点从所述OPEN表中删除，并放入所述CLOSE表；

判断当前节点是否为目标节点，如果当前节点为目标节点，结束节点扩展循环，创建目标节点，父节点设置为当前节点，并将所述目标节点压入CLOSE表；

否则，节点扩展与存储；以当前节点为中心进行节点扩展，获得当前节点的子节点；并判断所述子节点的可行性，计算所有可行子节点的代价值，并将所述可行子节点存入OPEN表中，同时更新当前节点；

反溯最终规划航迹；根据目标节点和CLOSE表中的已扩展节点，利用节点间的扩展关系，从目标节点向上反溯直至起始节点，得到从起始点到目标点的路径，所述路径为满足编队机器人变形的最短路径。

6.根据权利要求1所述的一种机器人编队避障路径规划方法，其特征在于，所述节点代价值计算的具体步骤包括：

考虑路径长度最小化和编队保持的需求，构建统一化的代价函数，如式(4)所示；

每个节点的代价值f包括累计长度代价g、估计长度代价h、穿越软障碍代价e；

f＝ω₁(g+h)+ω₂e (4)

其中g为从起点到当前点的路径长度，h为当前点到目标点的估计长度，e为编队路径规划通过软障碍区域的路径长度，ω₁为路径长度的权重系数，ω₂为经过软障碍区域的路径长度权重系数。

Images