CN113721622B - 一种机器人路径规划方法 - Google Patents

Abstract

本发明公开了一种机器人路径规划方法，属于路径规划技术领域。它包括获取环境空间信息，根据环境空间信息生成栅格地图；其中，栅格地图中包括黑格和白格，黑格表示有障碍物，白格表示无障碍物；生成采样点集X，并利用PRM算法对采样点集X的采样点进行双向搜索生成路径集F；然后根据路径集F得到最优折线路径；之后对最优折线路径进行平滑处理得到最优规划路径。针对现有技术中利用PRM算法进行路径规划时搜索效率低的问题，本发明提供一种机器人路径规划方法，通过实现路径的双向搜索，从而提高了PRM进行路径搜索的效率，并且提高了搜索的安全性。

Description

一种机器人路径规划方法

技术领域

本发明属于路径规划技术领域，更具体地说，涉及一种机器人路径规划方法。

背景技术

路径规划是移动机器人如何在多约束条件下从起始位置安全到达目标位置的关键技术，根据对环境信息的把握程度可把路径规划划分为基于先验完全信息的全局路径规划和基于传感器信息的局部路径规划。其中，全局路径规划需要掌握所有的环境信息，根据环境地图的所有信息进行路径规划；局部路径规划只需要由传感器实时采集环境信息，了解环境地图信息，然后确定出所在地图的位置及其局部的障碍物分布情况，从而可以选出从当前结点到某一子目标结点的最优路径。

近年来，移动机器人的发展技术有着飞跃式的进步，关于机器人路径规划技术俨然成为了机器人未来发展的重要研究领域。针对机器人的路径规划，现有技术也提出了一些解决方案，例如发明创造名称为：一种基于改进双向RRT机器人路径规划方法(申请日：2020年12月30日；申请号：202011609932.8)，该方案公开了一种基于改进双向RRT机器人路径规划方法，针对RRT算法的不足，提出了一种中心圆采样策略，降低了双向RRT算法采样随机性；融入了障碍物膨胀策略，使得规划的路径和障碍物保持一定的间距，更加符合实际机器人运行路径；引入了目标偏向策略，提升算法的搜索效率；最后将规划的路径进行样条插值，使得规划的路径更加的平稳与光滑；该方案提出的CC_BRRT算法，一定程度上减少了采样的盲目性，缩短了路径长度，减少了采样节点的数量，提升了路径的光滑性。

现如今，机器人的路径规划技术广泛运用在工业，服务业，娱乐等各个方面，机器人在执行各项任务时都基于路径规划的方法选择上，有效而又可行的路径规划技术可以使得机器人更加安全而可靠的完成人类所需要的任务。传统的路径规划算法有Dijkstra算法、A*算法、人工势场法等，随着移动机器人工作空间复杂度的提升，逐渐涌现出一系列的智能仿生算法，如遗传算法、粒子群算法、人工蜂群算法等。

此外，现有技术中路径规划算法还包括随机路标图算法(PRM)，该算法通过在构形空间中进行采样、对采样点进行碰撞检测、测试相邻采样点是否能够连接来表示路径图的连通性，解决了难以在高维空间中构造有效路径图的问题，路径规划时计算量小且实时性较好。但是在面对狭窄通道时，搜索效率会明显下降，易陷入局部最优。因此，如何提高在利用PRM进行路径规划时的搜索效率，是现有技术亟需解决的问题。

发明内容

1.要解决的问题

针对现有技术中利用PRM算法进行路径规划时搜索效率低的问题，本发明提供一种机器人路径规划方法，通过实现路径的双向搜索，从而提高了PRM进行路径搜索的效率，并且提高了搜索的安全性。

2.技术方案

为了解决上述问题，本发明所采用的技术方案如下：

本发明的一种机器人路径规划方法，包括获取环境空间信息，根据环境空间信息生成栅格地图；其中，栅格地图中包括黑格和白格，黑格表示有障碍物，白格表示无障碍物；而后生成采样点集X，并利用PRM算法对采样点集X的采样点进行双向搜索生成路径集F；然后根据路径集F得到最优折线路径；之后对最优折线路径进行平滑处理得到最优规划路径。

更进一步地，生成采样点集X的具体过程为：先创建采样点集X，该采样点集X初始为空；在栅格地图上进行随机取点，并判断所取的采样点是否在黑格上，若所取的采样点不在黑格上，则将该采样点加入至采样点集X。

更进一步地，生成路径集F的具体过程为：

(3-1)先创建路径集F，该路径集F初始为空；

(3-2)设定初始点和目标点，初始点为正向原点，目标点为反向原点；

(3-3)以正向原点为起始点，向靠近反向原点的任意方向搜索采样点，将正向原点与搜索的采样点连接得到正向连线，然后判断正向连线是否与黑格接触，若正向连线不与黑格接触则将该正向连线加入至路径集F中；

(3-4)以反向原点为起始点，向靠近正向原点的任意方向搜索采样点，将反向原点与搜索的采样点连接得到反向连线，然后判断反向连线是否与黑格接触，若反向连线不与黑格接触则将该反向连线加入至路径集F中；

(3-5)遍历路径集F中新增加的正向连线对应的采样点，将正向连线对应的采样点依次作为新的正向原点并重复步骤(3-3)，然后遍历路径集F中新增加的反向连线对应的采样点，将反向连线对应的采样点依次作为新的正向原点并重复步骤(3-4)；

(3-6)当路径集F中有新增加的正向连线或反向连线时，则重复步骤(3-5)，直至新增的正向连线与新增的反向连线对应有相同的采样点。

更进一步地，根据路径集F得到最优折线路径图的具体过程为：利用启发式搜索算法在路径集F中寻找最优折线路径。

更进一步地，根据路径集F得到最优折线路径图的具体过程为：若路径集F为空，则没有最优折线路径。

更进一步地，利用启发式搜索算法在路径集F中寻找最优折线路径的具体过程为：令正向连线与反向连线所对应的相同采样点为重复点；选取初始点至重复点之间连续的正向连线以及目标点至该重复点之间连续的反向连线；然后将选取的正向连线和反向连线相连得到若干条折线路径；之后将若干条折线路径进行长度比较得到最短的折线路径，该条最短的折线路径为最优折线路径。

更进一步地，采用三次样条差值的方式对最优折线路径进行平滑处理。

更进一步地，采用三次样条差值的方式对最优折线路径进行平滑处理的具体步骤如下：依次取最优折线路径上两个相邻采样点，对两个采样点区间进行分割得到若干个小区间；然后对每个小区间进行样条插值得到三次多样式函数；之后对三次多样式函数进行解析得到插值点，再根据插值点绘制得到最优规划路径。

更进一步地，利用linspace函数对两个采样点区间进行分割。

3.有益效果

相比于现有技术，本发明的有益效果为：

本发明的一种机器人路径规划方法，通过采用PRM算法进行双向搜索生成最优折线路径，大大节省了路径生成时间，大大提升了算法的收敛速度，进而提高了搜索的效率。进一步通过对最优折线路径进行平滑处理，实现了对规划路径的优化，提高了机器人工作的安全性。

附图说明

图1为本发明方法的流程示意图；

图2为实施例1的路径搜索示意图一；

图3为实施例1的路径搜索示意图二；

图4为实施例1的最优折线路径示意图；

图5为实施例1的最优规划路径示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例；而且，各个实施例之间不是相对独立的，根据需要可以相互组合，从而达到更优的效果。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为进一步了解本发明的内容，结合附图和实施例对本发明作详细描述。

实施例1

结合图1所示，本发明的一种机器人路径规划方法，通过采用PRM算法进行双向搜索生成最优折线路径，大大提升了算法的收敛速度，进而提高了搜索的效率。进一步通过对最优折线路径进行平滑处理，实现了对规划路径的优化，提高了机器人工作的安全性。本发明的方法步骤具体如下：

(1)生成栅格地图

首先获取环境空间信息，具体地，机器人可通过激光雷达、摄像头传感器或者超声波传感器对周围环境空间信息进行获取，本实施例中机器人通过激光雷达获取环境空间信息。进一步地，根据环境空间信息生成栅格地图，值得说明的是，机器人根据环境空间信息生成的栅格地图包括若干个尺寸相同的格子，具体地，栅格地图中包括黑格和白格，黑格表示有障碍物，白格表示无障碍物。

(2)生成采样点集X

根据栅格地图生成采样点集X，具体地，先创建采样点集X，该采样点集X初始为空；在栅格地图上进行随机取点，具体地，在栅格地图上进行均匀采样随机取点，并判断所取的采样点是否在黑格上，即判断所取的采样点是否位于障碍物上，若所取的采样点不在黑格上，则将该采样点加入至采样点集X。

(3)生成路径集F

利用PRM算法对采样点集X的采样点进行双向搜索生成路径集F，值得说明的是，通过双向搜索可以大大提高PRM的收敛速度，进而大大提高了路径的搜索效率。进一步地，生成路径集F的具体过程为：

(3-1)先创建路径集F，该路径集F初始为空；需要说明的是，创建路径集F的目的是为了集合所有可行路径，进而可以得到最优的路径。

(3-2)设定初始点和目标点，初始点为正向原点，目标点为反向原点；值得说明的是，根据机器人的工作需求设定初始点和目标点，例如结合图2所示，本示例中的初始点s设定在栅格地图的左上方，而目标点g设定在栅格地图的右上方。

(3-3)以正向原点为起始点，向靠近反向原点的任意方向搜索采样点，例如结合图2所示，以s为起始点，向靠近反向原点g的任意方向搜索靠近正向原点的采样点，本示例中向靠近反向原点g的任意方向搜索到a₁，a₂，a₃，a₄四个采样点；将正向原点与搜索的采样点连接得到正向连线，然后判断正向连线是否与黑格接触，即判断正向连线路径是否穿过障碍物；若正向连线不与黑格接触则将该正向连线加入至路径集F中；本示例中得到<s,a₁>，<s,a₂>，<s，a₃>，<s，a₄>四条正向连线，且该示例的四条正向连线均未与黑格接触，即将该四条正向连线加入至路径集F中。

(3-4)以反向原点为起始点，向靠近正向原点的任意方向搜索采样点，例如结合图2所示，以g为起始点，向靠近正向原点s的任意方向搜索采样点，本示例中向靠近正向原点s的任意方向搜索到b₁，b₂，b₃，b₄四个采样点；将反向原点与搜索的采样点连接得到反向连线，然后判断反向连线是否与黑格接触，即判断反向连线路径是否穿过障碍物；若反向连线不与黑格接触则将该反向连线加入至路径集F中；本示例中得到<g，b₁>，〈g，b₂>，〈g，b₃>，〈g，b₄>四条反向连线，且该示例的四条反向连线均未与黑格接触，即将该四条反向连线加入至路径集F中。

(3-5)遍历路径集F中新增加的正向连线对应的采样点，将正向连线对应的采样点依次作为新的正向原点并重复步骤(3-3)，本示例结合图3所示，新增加的正向连线对应的采样点分别是a₁，a₂，a₃，a₄四个采样点，即a₁，a₂，a₃，a₄四个采样点依次作为新的正向原点，即作为新的起始点重复步骤(3-3)，得到五条新的正向连线〈a₁，a₁₁>，〈a₂，a₂₁>，<a₂，a₂₂>，<a₄，a₄₁>、<a₄，a₄₂>。

然后遍历路径集F中新增加的反向连线对应的采样点，将反向连线对应的采样点依次作为新的正向原点并重复步骤(3-4)；本示例结合图3所示，新增加的反向连线对应的采样点分别是b₁，b₂，b₃，b₄四个采样点，即b₁，b₂，b₃，b₄四个采样点依次作为新的反向原点，即作为新的起始点重复步骤(3-4)，得到四条新的反向连线<b₁，b₁₁>，<b₁，b₁₂>，<b₂，b₂₁>，<b₂，b₂₂>。

(3-6)当路径集F中有新增加的正向连线或反向连线时，则重复步骤(3-5)，直至新增的正向连线与新增的反向连线对应有相同的采样点。例如图3所示，以a₁₁为起始点执行步骤(3-3)得到正向连线<a₁₁，b₁₁>，以b₁₁为起始点执行步骤(3-4)得到反向连线<b₁₁，a₁₁>，即新增的正向连线与新增的反向连线对应的采样点相同。

通过上述步骤即可生成路径集F，需要说明的是，本发明通过双向搜索大大提高了路径集F的生成效率，进而提高了路径的搜索效率。

(4)生成最优折线路径

根据路径集F得到最优折线路径，具体地，利用启发式搜索算法在路径集F中寻找最优折线路径。具体步骤如下：

(4-1)令正向连线与反向连线所对应的相同采样点为重复点；结合图3所示，本示例中a₁₁，b₁₁为重复点。

(4-2)选取初始点至重复点之间连续的正向连线以及目标点至重复点之间连续的反向连线；结合图3所示，本示例中选取<s，a₁>、<a₁，a₁₁>、<g，b₁>、<b₁，b₁₁>以及<a₁₁，b₁₁>。

(4-3)将选取的正向连线和反向相连得到若干条折线路径；将若干条折线路径进行长度比较得到最短的折线路径，该条最短的折线路径为最优折线路径。本实施例如图3所示，将选取的正向连线和反向连线得到一条折线路径，该条折线路径即为最优折线路径<s，a₁，a₁₁，b₁₁，b₁，g>，最后的最优折线路径如图4所示。需要说明的是，若路径集F为空，则没有路径可寻，即没有最优折线路径。

(5)生成最优规划路径

对最优折线路径进行平滑处理得到最优规划路径，最优规划路径如图5所示。值得说明的是，最优折线路径的拐点过多，过多的拐点使得机器人不得不随时调整自身的姿态以来满足路径规划的要求，但是过多的姿态调整显然加长了行驶的时间，行驶性能也会因此下降。本发明通过对最优折线路径进行平滑处理，使得路径成为一条平滑曲线，并且大大减少了拐点，进而提高了机器人的移动效率，进一步保证了机器人的移动安全性。

进一步地，本发明采用三次样条差值的方式对最优折线路径进行平滑处理。具体地，在每一个小区间上(x_i-1，x_i)(i＝1，2，...，n)内构建三次多样式函数：

f_i(x)＝a_i+b_i(x-x_i)+c_i(x-x_i)²+d_i(x-x_i)³ (1)

f(x)，f′(x)，f″(x)在[x₁，x_n]上连续。满足：

f(x₀)＝y₀，......，f(x_n+1)＝y_n+1 (2)

f_{_}(x_i)＝f₊(x_i)＝y_i，i＝1，2，3，...，n (3)

f_{_}′(x_i)＝f₊′(x_i)＝y_i，i＝1，2，3，...，n (4)

f_{_}″(x_i)＝f₊″(x_i)＝y_i，i＝1，2，3，...，n (5)

其中，a_i，b_i，c_i，d_i为待定系数；因[x₁，x_n]上有n+1个小区间，每个区间上面对应的是4个待定系数，所以f(x)共有4n+4个待定系数，为了确定f(x)的三次函数多样式，则要求对应4n+4个插值条件，以此解出方程组的解。由式(2)可知具有n+2个条件；由式(3)可知f(x)在插值处连续，具有n个条件；式(4)和式(5)代表f(x)的一阶导数和二阶导数在插值点处连续，共具有2n个插值条件。因此，由以上的公式可以得到4n+2个插值条件。为了解出三次样条插值函数，则还需要两个边界条件。

进一步地，为得到本发明中方程组的解，本发明使用的是边界条件中的非扭结边界条件：第一个小区间的三次多样式函数和第二个小区间的三次多样式函数的三阶导数相等；最后一个小区间的三次多样式函数和倒数第二个小区间的三次多样式函数的三阶倒数相等，即：

f₀″′(x₀)＝f₁″′(x₁)，f_n-2″′(x_n-2)＝f_n-1″′(x_n-1)(6)

通过上述公式即可解出方程组的解，从而实现对最优折线路径的平滑处理。

值得说明的是，通过下述步骤可以实现对优折线路径的平滑处理：

(5-1)依次取最优折线路径上两个相邻采样点，对两个采样点区间进行分割得到若干个小区间；具体地，先存储最优折线路径各个拐点，该拐点即采样点，本示例通过txt文件进行拐点的存储。之后利用linspace函数生成线性间距向量，产生两个采样点例如x₁，x₂之间的N点行矢量，即为在两采样点区间中分割若干等分，本示例将分割点设置为100。

(5-2)对每个小区间进行样条插值得到三次多样式函数；具体地，样条插值通过对函数方程的解析得到一个三次多样式y＝f(x)，该多样式对于离散数据测量断点(x，y)会以逼近作用。由于在(x_i，y_i)与(x_i+1，y_i+1)之间只能确定一条直线，但是对于三次多样式而言，便不仅仅局限于一条，故为了得到确切的三次多样式的表达式，本发明中添加的边界条件为非扭结边界条件。具体地，用spline函数三次样条插值计算出由向量x与y确定的一元函数y＝f(x)在插值点xx处的值，之后由xx值得到yy的值，以y矩阵的每一列与x配对，得出插值点函数值yy矩阵。

(5-3)对三次多样式函数进行解析得到插值点，再根据插值点绘制得到最优规划路径。具体地，设置返回值，返回由向量x与y确定的分段样条多项式的系数矩阵，之后绘画得到三次样条插值后的函数曲线图。

由上述路径优化过程可知，平滑处理后的路径的转折点数量更少，路径更加平滑，从而可以减少移动机器人导航时的转弯次数，使得移动机器人在工作环境中行驶更加灵活，降低移动机器人运行时间以及能耗的损失，提高工作效率。

本发明的一种机器人路径规划方法，先通过PRM算法采用正反方向交替搜索的方式使得算法在路径寻优过程中大大节省了时间，提升了算法的收敛速度，之后再融合三次样条插值的方法提升了路径的光滑度，确保机器人可以更加安全的工作。仿真实验结果表明，路径质量更加优化，寻路效率更高，能够有效的解决机器人路径规划任务。

在上文中结合具体的示例性实施例详细描述了本发明。但是，应当理解，可在不脱离由所附权利要求限定的本发明的范围的情况下进行各种修改和变型。详细的描述和附图应仅被认为是说明性的，而不是限制性的，如果存在任何这样的修改和变型，那么它们都将落入在此描述的本发明的范围内。此外，背景技术旨在为了说明本技术的研发现状和意义，并不旨在限制本发明或本申请和本发明的应用领域。

Claims (5)

1.一种机器人路径规划方法，其特征在于，包括

获取环境空间信息，根据环境空间信息生成栅格地图；其中，栅格地图中包括黑格和白格，黑格表示有障碍物，白格表示无障碍物；

生成采样点集X，并利用PRM算法对采样点集X的采样点进行双向搜索生成路径集F；

根据路径集F得到最优折线路径；

对最优折线路径进行平滑处理得到最优规划路径；

生成采样点集X的具体过程为：先创建采样点集X，该采样点集X初始为空；在栅格地图上进行随机取点，并判断所取的采样点是否在黑格上，若所取的采样点不在黑格上，则将该采样点加入至采样点集X；

生成路径集F的具体过程为：

3-1、先创建路径集F，该路径集F初始为空；

3-2、设定初始点和目标点，初始点为正向原点，目标点为反向原点；

3-3、以正向原点为起始点，向靠近反向原点的任意方向搜索采样点，将正向原点与搜索的采样点连接得到正向连线，然后判断正向连线是否与黑格接触，若正向连线不与黑格接触则将该正向连线加入至路径集F中；

3-4、以反向原点为起始点，向靠近正向原点的任意方向搜索采样点，将反向原点与搜索的采样点连接得到反向连线，然后判断反向连线是否与黑格接触，若反向连线不与黑格接触则将该反向连线加入至路径集F中；

3-5、遍历路径集F中新增加的正向连线对应的采样点，将正向连线对应的采样点依次作为新的正向原点并重复步骤3-3，然后遍历路径集F中新增加的反向连线对应的采样点，将反向连线对应的采样点依次作为新的正向原点并重复步骤3-4；

3-6、当路径集F中有新增加的正向连线或反向连线时，则重复步骤3-5，直至新增的正向连线与新增的反向连线对应有相同的采样点；

根据路径集F得到最优折线路径图的具体过程为：利用启发式搜索算法在路径集F中寻找最优折线路径；

利用启发式搜索算法在路径集F中寻找最优折线路径的具体过程为：

令正向连线与反向连线所对应的相同采样点为重复点；

选取初始点至重复点之间连续的正向连线以及目标点至该重复点之间连续的反向连线；

将选取的正向连线和反向连线相连得到若干条折线路径；

将若干条折线路径进行长度比较得到最短的折线路径，该条最短的折线路径为最优折线路径。

2.根据权利要求1所述的一种机器人路径规划方法，其特征在于，根据路径集F得到最优折线路径图的具体过程为：若路径集F为空，则没有最优折线路径。

3.根据权利要求1或2所述的一种机器人路径规划方法，其特征在于，采用三次样条差值的方式对最优折线路径进行平滑处理。

4.根据权利要求3所述的一种机器人路径规划方法，其特征在于，采用三次样条差值的方式对最优折线路径进行平滑处理的具体步骤如下：

依次取最优折线路径上两个相邻采样点，对两个采样点区间进行分割得到若干个小区间；

对每个小区间进行样条插值得到三次多样式函数；

对三次多样式函数进行解析得到插值点，再根据插值点绘制得到最优规划路径。

5.根据权利要求4所述的一种机器人路径规划方法，其特征在于，利用linspace函数对两个采样点区间进行分割。