CN111473796B - 基于sps算法的机器人路径规划方法 - Google Patents

Abstract

本发明请求保护一种基于LSPS算法的机器人路径规划方法。机器人路径规划问题常利用网格法来对环境进行建模，然而这使得现实中的环境与建模环境严重不匹配，使得路径精度急剧下降。大致可分为三步，第一步，生成n条随机路径，第二步，对穿过障碍物的路径进行该障碍物的有限周围点生成，第三步，本文采用了改进的A*算法，对穿过障碍物的路径进行修复，并选取适应性最好的路径。对于该算法，本文在不同环境中进行了测试，并与其它算法进行了对比，结果发现，穿过障碍物的路径能够很快的被修复，在各指标上皆较优于所对比的算法。并且该算法鲁棒性较强，能够适应各种复杂环境。

Description

基于SPS算法的机器人路径规划方法

技术领域

本发明属于人工智能与机器人自动控制领域，尤其涉及机器人路径规划方法。

背景技术

在进行机器人作业的时候需要对机器人行走路径进行合理规划，使其能够在规定时间内完成指定任务。

但如今在进行路径规划地图建模时，通常会将障碍物网格化，这就可能使得地图上的障碍物与现实中的障碍物严重不符合，使得某些障碍物之间有一定宽度的间隙也无法通过；其次，随着地图环境愈来愈复杂，由于许多启发式算法是通过随机点来生成一系列可行路径，因此最终得到最优路径所耗的时间也陡然增加。

SPS算法为创新算法现目前并未运用在任何领域，但该算法属于机器人路径规划问题范畴，因此可以运用在机器人路径规划以及车载导航系统等方面。首先现有技术难以使得机器人获得周围环境的准确信息，再者由于现有算法多数用网格化建模，因此会导致机器人难以通过障碍物与障碍物之间的缝隙或者无法从其边缘走过。本发明主要打破了网格化建模的方式，该算法能够适应于各类由不规则障碍物组成的环境。

发明内容

本发明旨在解决以上现有技术的问题。提出了一种基于SPS算法的机器人路径规划方法。本发明的技术方案如下：

一种基于SPS算法的机器人路径规划方法，其包括以下步骤：

步骤1、机器人生成由起点到终点的n条不重复随机路径；

步骤2、当某条随机路径与一系列障碍物发生冲突时，利用SPS算法对每个障碍物进行有限周围点生成，生成靠近与路径相冲突的障碍物的点，并且该点不能在任何障碍物内；

步骤3、对穿过障碍物的路径利用改进A*算法进行路径修复；改变了当前处理点的子代的定义，原A*算法是将周围8点作为其子代并从中筛选，而改进A*算法是通过之前生成的有限周围点集合作为其子代，并从中筛选；

步骤4、计算每条路径的适应度值，筛选出适应度最小的路径，机器人按照该路径进行规划。

进一步的，所述步骤1机器人生成由起点到终点的n条不重复随机路径，具体包括：

通过指定路径节点数量，以及路径条数，随机选取中间点，该中间点表示随机的初始路径除开起点与终点剩余的点，在起点与终点的连线为矩形对角线的区域内。

进一步的，所述步骤2当某条随机路径与障碍物发生冲突时，对每个障碍物进行有限周围点生成，具体包括：

通过获得随机路径与障碍物的交点ip^s与ip^t，其中ip^s为离起点s较近的与障碍物的交点，ip^t为离终点较近的与障碍物的交点，将ip^s存入Temp Set中，Temp Set是定义的集合，用来存储被判断有意义的点，有意义是指该点不在任何障碍物内，并且靠近与路径冲突的障碍物，并在ip^s周围生成以宽度为w的正方形领域8个点，8个点分别为正方形的四个顶点，加上正方形四条边的中点，并判断8个点是否不在障碍物内，是否靠近障碍物，条件是同时满足，8个点是逐一判断，如果满足条件便存入Temp Set中，如果不满足则舍去，紧接着将刚处理的点存入SPS Set中并从Temp Set中移除，SPS Set是定义的集合来存储已经处理过的点，之后处理下一个点，直到满足停止条件为止，停止条件为已经围绕障碍物生成了一周，或者终点已经包含在当前处理点以w为宽度的正方形区域内。

进一步的，判断是否靠近障碍物准则为：ip^s点以w为宽度的正方形领域是否与该线段冲突障碍物相交，若相交则表示靠近障碍物，若不相交则表示远离并舍去。

进一步的，所述停止条件为：ip^t在当前处理点的以w为宽度的正方形8领域以内，或者Temp Set为空。

进一步的，所述步骤3改进的A*算法具体包括：通过获取SPS冲突障碍物周围生成有限点集合并固定起始点来进行寻路；首先定义Open List与Closed List，前者是用来存储搜寻过的点，后者是用来存储已经处理过的点，每次寻找Open List中f值最小的点作为当前处理点，选取后从Open list中删除，并添加至Closed List里，并搜索SPS集合中与当前处理点连线不与障碍物冲突的点为child，计算g值，h值，f值；g值代表着当前处理点到起点的累计距离，h值代表当前处理点到终点的直线距离，f值为g值与h值相加，其中中间点坐标为(x_e，y_e),起点坐标为(x_cn，y_cn),终点坐标为(x_en，y_en)。f(e)＝g(e)+h(e)；；

如果child没有在Closed list里，并且没有在Open list中，添加至Open list里，若在Open list中，则判断其g值是否小于Open list中该点的g值，若小于替换，如果大于，舍去；如果在Closed List中直接舍去。直到当前处理点为终点则停止循环，输出路径，改进主要是改变了搜索点的方式，最初是通过将当前处理点的正方形区域8个点作为子代，并从中筛选，而改进的A*是从SPS集合中筛选子代。

进一步的，所述步骤4的适应度函数为：

fitness＝0.8|Lengt|+0.2|smoot|

其中|Lengt|表示该路径的长度，|smoot|表示该路径的平滑度。

本发明的优点及有益效果如下：

目前的主流算法为GA，PSO等启发式算法。该类算法首先通过网格化地图来对机器人周围环境进行判断，该方式，是通过将地图网格化后，当障碍物处在某一网格范围内，便标记该网格为障碍物。当网格间距取得较小时会导致建模环境与真实环境大大降低，这便会导致算法的性能有较大的局限性；当网格间距取得较小时会导致生成环境所耗时间会大大增加。

为了使得机器人在能够更适应真实环境，因此打破传统网格化建模方式是非常有必要的。该发明提供了另外一种解决思路，通过生成随机路径，并修复与障碍物冲突的路径，最后筛选出适应度函数最小的路径，来获得全局最优解，即节省了时间又提高了路径的精度。

本发明的核心创新点为提出了SPS算法以及对A*算法进行了改进。

通过获得随机路径与障碍物的交点ip^s与ip^t，其中ip^s为离起点s较近的与障碍物的交点，ip^t为离终点较近的与障碍物的交点，将ip^s存入Temp Set中，该Temp Set是定义的集合，用来存储被判断有意义的点，其中有意义是指该点不在任何障碍物内，并且靠近与路径冲突的障碍物。并在ip^s周围生成以宽度为w的正方形领域8个点分别为正方形的四个顶点，加上正方形四条边的中点并逐一判断8个点是否不在障碍物内，是否靠近障碍物，如果满足条件便存入Temp Set中，如果不满足则舍去，紧接着将刚处理的点存入SPS Set中并从Temp Set中移除，其中该SPS Set是定义的集合来存储已经处理过的点，之后处理下一个点，直到满足停止条件为止。该停止条件为已经围绕障碍物生成了一周，或者终点已经包含在当前处理点以w为宽度的正方形区域内。

相较于原A*算法，本发明改变了当前处理点的子代的定义，原A*算法是将周围8点作为其子代并从中筛选，而改进A*算法是通过之前生成的有限周围点集合作为其子代，并从中筛选，加快了A*算法的收敛速度。

附图说明

图1是随机路径生成示意图；

图2SPS算法示意图；

图3在tunnel_doglet环境下的实验对比图；

图4在var density环境下的实验对比图；

图5SPS算法流程图；

图6改进A*算法流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是：

本发明提供了一种基于SPS算法的机器人路径规划方法，可以通过输入真实障碍物环境，起始点来得到当前环境下最适合机器人的移动路径。该算法有很好的鲁棒性能够适应各种不同的环境并相较于现主流算法有更好的效果。

该算法流程可以被简要总结如下：

Step 1.生成有起点到终点的n条不重复路径

Step 2.在与障碍物冲突的周围生成SPS

Step 3.对穿过障碍物的路径利用改进A*算法进行路径修复

Step 4.计算每条路径的适应度值，筛选出适应度最小的路径

对于Step 1.通过指定路径节点数量，以及路径条数，随机选取中间点，其在起点与终点的连线为矩形对角线的区域内，如图1所示。

对于Step 2.当随机路径与一些列障碍物发生冲突时，对每个障碍物进行有限周围点生成。通过获得与障碍物交点ip^s与ip^t，其中ip^s为离起点s较近的与障碍物的交点，ip^t为离终点较近的与障碍物的交点，将ip^s存入Temp Set中，并在ip^s周围生成以宽度为w的正方形领域8个点，并判断8个点是否不在障碍物内，是否靠近障碍物，如果满足条件便存入Temp Set中，如果不满足则舍去，紧接着将刚处理的点存入SPS Set中并从Temp Set中移除，之后处理下一个点，直到满足停止条件为止。如图2所示。

对于Step 3.改进的A*算法，该算法通过获取SPS集合并固定起始点来进行寻路。首先需要定义Open List与Closed List，前者是用来存储搜寻过的点，后者是用来存储已经处理过的点。每次寻找Open List中f值最小的点作为当前处理点，选取后从Open list中删除，并添加至Closed List里，并搜索SPS集合中与当前处理点连线不与障碍物冲突的点为child，计算其g值，h值，f值。如果child没有在Closed List里，并且没有在Open List中，添加至Open List里，若果在Open List中，则判断其g值是否小于Open List中该点的g值，若果小于替换，如果大于，舍去。如果在Closed List中直接舍去。直到当前处理点为终点则停止循环，输出路径。

对于Step 4.通过建立适应度函数来计算每一条路径的适应度值，从而来判断筛选出最优路径。

本发明具有很好的鲁棒性，能够适应各种环境，并且相较现主流算法有更好的性能，更适合机器人在规定时间内完成指定任务。

同时克服了传统算法需要对环境进行网格化建模，而导致障碍物与障碍物之间间隙无法穿过，实际环境与建模环境严重不符等问题。

图3，图4为三种算法在两个不同复杂度的环境下得到的结果。可以发现不管障碍物环境如何复杂，障碍物多么密集，本算法能找到很好的路径来供机器人行走并且在性能上远优于现在主流算法。

通过生成不重复的n条随机路径，并在与路径冲突的一系列障碍物周围利用SPS算法生成有限周围点，之后利用改进的A*算法来对不可行路径进行修复，最后利用建立的适应度函数模型对每条路径进行适应性评估，最后筛选出适应度值最低的，作为该算法的最优解。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (6)

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

步骤1、机器人生成由起点到终点的n条不重复随机路径；

步骤2、当某条随机路径与一系列障碍物发生冲突时，利用SPS算法对每个障碍物进行有限周围点生成，生成靠近与路径相冲突的障碍物的点，并且该点不能在任何障碍物内；

步骤3、对穿过障碍物的路径利用改进A*算法进行路径修复；改变了当前处理点的子代的定义，原A*算法是将周围8点作为其子代并从中筛选，而改进A*算法是通过之前生成的有限周围点集合作为其子代，并从中筛选；

步骤4、计算每条路径的适应度值，筛选出适应度最小的路径，机器人按照该路径进行规划；

所述步骤3改进的A*算法具体包括：通过获取SPS冲突障碍物周围生成有限点集合并固定起始点来进行寻路；首先定义Open List与Closed List，前者是用来存储搜寻过的点，后者是用来存储已经处理过的点，每次寻找Open List中f值最小的点作为当前处理点，选取后从Open List中删除，并添加至Closed List里，并搜索SPS集合中与当前处理点连线不与障碍物冲突的点为child，计算g值，h值，f值；g值代表着当前处理点到起点的累计距离，h值代表当前处理点到终点的直线距离，f值为g值与h值相加，其中中间点坐标为(x_e，y_e),起点坐标为(x_cn，y_cn),终点坐标为(x_en，y_en)；

f(e)＝g(e)+h(e)；

如果child没有在Closed List里，并且没有在Open List中，添加至Open List里，若在Open List中，则判断其g值是否小于Open List中该点的g值，若小于替换，如果大于，舍去；如果在Closed List中直接舍去；直到当前处理点为终点则停止循环，输出路径，改进主要是改变了搜索点的方式，最初是通过将当前处理点的正方形区域8个点作为子代，并从中筛选，而改进的A*是从SPS集合中筛选子代。

2.根据权利要求1所述的一种基于SPS算法的机器人路径规划方法，其特征在于，所述步骤1机器人生成由起点到终点的n条不重复随机路径，具体包括：

通过指定路径节点数量，以及路径条数，随机选取中间点，该中间点表示随机的初始路径除开起点与终点剩余的点，在起点与终点的连线为矩形对角线的区域内。

3.根据权利要求1所述的一种基于SPS算法的机器人路径规划方法，其特征在于，所述步骤2当某条随机路径与障碍物发生冲突时，对每个障碍物进行有限周围点生成，具体包括：

通过获得随机路径与障碍物的交点ip^s与ip^t，其中ip^s为离起点s较近的与障碍物的交点，ip^t为离终点较近的与障碍物的交点，将ip^s存入Temp Set中，Temp Set是定义的集合，用来存储被判断有意义的点，有意义是指该点不在任何障碍物内，并且靠近与路径冲突的障碍物，并在ip^s周围生成以宽度为w的正方形领域8个点，8个点分别为正方形的四个顶点，加上正方形四条边的中点，并判断8个点是否不在障碍物内，是否靠近障碍物，条件是同时满足，8个点是逐一判断，如果满足条件便存入Temp Set中，如果不满足则舍去，紧接着将刚处理的点存入SPS Set中并从Temp Set中移除，SPS Set是定义的集合来存储已经处理过的点，之后处理下一个点，直到满足停止条件为止，停止条件为已经围绕障碍物生成了一周，或者终点已经包含在当前处理点以w为宽度的正方形区域内。

4.根据权利要求3所述的一种基于SPS算法的机器人路径规划方法，其特征在于，判断是否靠近障碍物准则为：ip^s点以w为宽度的正方形领域是否与冲突障碍物相交，若相交则表示靠近障碍物，若不相交则表示远离并舍去。

5.根据权利要求3所述的一种基于SPS算法的机器人路径规划方法，其特征在于，所述停止条件为：ip^t在当前处理点的以w为宽度的正方形8领域以内，或者Temp Set为空。

6.根据权利要求1-5之一所述的一种基于SPS算法的机器人路径规划方法，其特征在于，所述步骤4计算每条路径的适应度值，公式为：

fitness＝0.8|Lengt|+0.2|smoot|

其中|Lengt|表示该路径的长度，|smoot|表示该路径的平滑度。