CN115639827A - 一种结构化环境下机器人路径规划方法和系统 - Google Patents

Abstract

本申请提供了一种结构化环境下机器人路径规划方法和系统。该机器人路径规划方法包括：在机器人运动的环境地图中，结合机器人实时航向角增加起点至运动终点的方向引导约束，对八方向A星算法的搜索节点进行精简；基于搜索节点精简后的八方向A星算法，根据所述环境地图的方向角启发函数和所述机器人的当前航向角，对所述机器人的路径进行规划。籍此，通过增加机器人运动时的方向引导约束，对搜索节点进行精简，有效提高了结构空间环境中，机器人路径的搜索效率；利用方向角启发函数，有效减少了机器人在结构化空间换进观众的转角度数，从而得到真正的最优路径，保证机器人可以快速行驶，最终减少机器人起点至终点的运行时间。

Description

一种结构化环境下机器人路径规划方法和系统

技术领域

本申请涉及机器人技术领域，特别涉及一种结构化环境下机器人路径规划方法和系统。

背景技术

随着机器人技术的快速发展，机器人的路径规划是机器人的智能运动的关键环节。

常规的路径规划算法只关注了规划效率的问题，以保证路径最短的前提下，规划时间更短为最优原则。按照常规路径规划算法的最优判断依据，所规划的路径中往往存在障碍物穿越、折点多等问题。在结构空间环境中，尤其是类似仓储空间，货物布局非常有规律，采用常规的路径规划算法很容易造成机器人从起点至终点路径规划时存在多段相同距离路线，从而使机器人陷入局部最优或者随机选择路径，所规划的路径不适合机器人行进。

因而，亟需提供一种针对上述现有技术不足的技术方案。

发明内容

本申请的目的在于提供一种结构化环境下机器人路径规划方法和系统，以解决或缓解上述现有技术中存在的问题。

为了实现上述目的，本申请提供如下技术方案：

本申请提供一种结构化环境下机器人路径规划方法,包括：在机器人运动的环境地图中，结合机器人实时航向角增加起点至运动终点的方向引导约束，对八方向A星算法的搜索节点进行精简；基于搜索节点精简后的八方向A星算法，根据所述环境地图的方向角启发函数和所述机器人的当前航向角，对所述机器人的路径进行规划。

优选的，还包括：根据所述机器人的航向角，构建所述环境地图的方向角启发函数；所述方向角启发函数为：

其中，

表示第

个搜索节点的转向损失函数；

表示所述机器人的航向角；

表 示第

个搜索节点与终点形成夹角。

优选的，所述基于搜索节点精简后的八方向A星算法，根据所述环境地图的方向角启发函数和所述机器人的当前航向角，对所述机器人的路径进行规划包括：基于搜索节点精简后的八方向A星算法，根据所述环境地图的方向角启发函数和所述机器人的当前航向角，规划所述机器人的初始路径；基于预设的折点判定策略，提取所述初始路径中的转折点；基于提取的转折点，对所述初始路径进行回溯计算，生成所述机器人的优化路径。

优选的，所述基于搜索节点精简后的八方向A星算法，根据所述环境地图的方向角启发函数和所述机器人的当前航向角，规划所述机器人的初始路径，具体为：根据所述机器人的路径规划中的父节点，搜索所述机器人的路径规划中的子节点坐标变化值，以对所述机器人的路径规划进行不可行路径子节点判定，得到开放节点集合；其中，所述开放节点集合中包含的多个子节点均为所述机器人的可行路径；基于搜索节点精简后的八方向A星算法的曼哈顿距离，求所述开放节点集合中当前父节点的最小距离子节点为下一路径节点，以生成所述机器人的初始路径。

优选的，所述基于提取的折点，对所述初始路径进行回溯计算，生成所述机器人的 优化路径，包括：基于提取的折点，生成所述机器人的一次规划路径；确定与所述一次规划 路径的终点相关联且不存在障碍物的最远搜索路径对应的第

个折点；确定所述一次规划 路径前向连接中与所述第

个折点相关联且不存在障碍物的最远搜索路径对应的第

个折 点，依次循环，直至到达所述一次规划路径的起点，得到所述机器人的优化路径；其中，

均为正整数。

优选的，所述预设的折点判定策略为：

其中，

、

、

表示所述初始路径中相邻的三个节点在所 述环境地图中的坐标。

本申请实施例还提供一种结构化环境下机器人路径规划系统，包括：节点精简单元，配置为在机器人运动的环境地图中，结合机器人实时航向角增加起点至运动终点的方向引导约束，对八方向A星算法的搜索节点进行精简；路径规划单元，配置为基于搜索节点精简后的八方向A星算法，根据所述环境地图的方向角启发函数和所述机器人的当前航向角，对所述机器人的路径进行规划。

有益效果：

本申请提供的结构化环境下机器人路径规划方法，首先，在机器人运动的环境地图中，结合机器人实时航向角增加起点至运动终点的方向引导约束，对八方向A星算法的搜索节点进行精简；然后，基于搜索节点精简后的八方向A星算法，根据所述环境地图的方向角启发函数和所述机器人的当前航向角，对所述机器人的路径进行规划。籍此，通过增加机器人运动时的方向引导约束，对搜索节点进行精简，有效提高了结构空间环境中，机器人路径的搜索效率；利用方向角启发函数，有效减少了机器人在结构化空间换进观众的转角度数，从而得到真正的最优路径，保证机器人可以快速行驶，最终减少机器人起点至终点的运行时间。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。其中：

图1为根据本申请的一些实施例提供的一种结构化环境下机器人路径规划方法的流程示意图；

图2为根据本申请的一些实施例提供的方向角启发函数的原理示意图；

图3为机器人在传统的八方向A星算法的栅格规划示意图；

图4为本申请的增加方向角启发函数的八方向A星算法的栅格规划示意图；

图5为机器人在结构化环境下传统的A星算法的路径规划示意图；

图6为机器人在结构化环境下增加搜索邻域的多方向A星算法的路径规划示意图；

图7为机器人在结构化环境下本申请的八方向A星算法的路径规划示意图；

图8为根据本申请的一些实施例提供的对机器人的初始路径进行优化的流程示意图；

图9为根据本申请的一些实施例提供的机器人的基于优先折点判据的路径优化原理示意图；

图10为根据本申请的一些实施例提供的一种结构化环境下机器人路径规划系统的结构示意图。

具体实施方式

下面将参考附图并结合实施例来详细说明本申请。各个示例通过本申请的解释的方式提供而非限制本申请。实际上，本领域的技术人员将清楚，在不脱离本申请的范围或精神的情况下，可在本申请中进行修改和变型。例如，示为或描述为一个实施例的一部分的特征可用于另一个实施例，以产生又一个实施例。因此，所期望的是，本申请包含归入所附权利要求及其等同物的范围内的此类修改和变型。

A星(A*)算法是一种在图形平面上，有多个节点的路径，求出最低通过成本的算法，A星算法作为一种启发式搜索算法，常被用于游戏中的NPC移动计算，通过对A星算法的改进，将其应用在机器人的路径规划中。传统的A星算法，虽然对搜索路径长度具有一定的优化作用，但同时也增加了路径搜索的遍历栅格数量，降低了算法的路径搜索效率。

在结构空间环境中，尤其类似仓储空间，货物布局非常有规律，这很容易造成机器人从起点至终点路径规划时存在多段相同距离路线，从而使机器人陷入局部最优或者随机选择路径。机器人路径的规划，也只是考虑了路径长度，而没有考虑空间概念，因而，在规划的路径中出现了紧贴障碍物顶点、穿越相连障碍物对角的现象，所规划的路径不适合机器人行进。

基于此，申请人提出了一种结构化环境下机器人路径规划方法，将机器人的方向角启发函数和终点方向的方向引导约束引入A星算法中，不但有效提高了机器人路径上节点搜索效率，而且减少了机器人在结构化空间换进观众的转角度数，从而得到真正的最优路径，在机器人行走过程中减少用于障碍物检测和避障的时间，保证机器人可以快速行驶，最终减少机器人起点至终点的运行时间。

如图1所示，该结构化环境下机器人路径规划方法中包括：

步骤S101、在机器人运动的环境地图中，结合机器人实时航向角增加起点至运动终点的方向引导约束，对八方向A星算法的搜索节点进行精简。

在本申请中，机器人路径规划基于已有的环境地图进行，可以采用栅格地图，通过对机器人运动现场的设计图进行转换或者现场测量绘制完成。

在传统的A星搜索算法中，一般采用欧式距离作为代价函数，该函数为理想无障碍物下的路径距离，该函数得到的路径距离明显小于真实路径，容易使机器人的路径算法陷入局部最优；还有采用曼哈顿距离作为代价函数，但是曼哈顿距离只有在四向搜索情况下可行，在结构化空间环境下，该函数得到的路径距离远远大于真实路径，启发效果不好。

在图2中，出发节点

至目标节点

在

轴和

轴的绝对距离分别为：

在栅格地图中，机器人在横向、纵向移动一个栅格的距离为1，斜线移动一个栅格 的距离为

，因此，机器人运动起点至终点的实际路径的长度

为：

……（1）

通过该距离模型，可以有效的解决欧几里得距离计算量大，曼哈顿距离非最短启发效果非最佳的问题。除了距离信息外，搜索遍历栅格的数量通过增加方向引导约束来增加启发函数信息量，正常情况下，八方向A星算法从出发节点开始，需要依次搜索其紧邻的8个方向，如图1所示。但是在室内结构化空间环境下，障碍物布局规整，因而搜索类似5、6、7三个方向节点对机器人路径搜索意义不大。通过起点至终点的引导向量（方向引导约束）的增加，可以进一步精简八方向搜索节点数量为5节点甚至更少。

由于在结构化空间环境下，机器人搜索或移动方向都是朝着目标节点的，因而，所有当前节点相连的八个节点进行远端节点（背离机器人运动方向）搜索的意义不大，浪费时间，因而，在对八方向A星算法的搜索节点进行精简时，结合实时航向角增加起点至运动终点的方向引导约束，连接当前节点和目标节点，与连线相交节点，以及相交节点两侧的各两个节点，共5个栅格为搜索栅格。如图2所示。籍此，通过机器人运动终点的方向引导约束，可以有效降低路径搜索的遍历栅格数量，提高路径搜索效率。

步骤S102、基于搜索节点精简后的八方向A星算法，根据环境地图的方向角启发函数和机器人的当前航向角，对机器人的路径进行规划。

具体的，根据机器人的航向角，构建环境地图的方向角启发函数。如图2所示，在八方 向A星搜索算法中，搜索节点1方向角

，则可知搜索节点1方向角

，搜索节点3方向 角

，……，依次类推，搜索节点8方向角

。本申请中，机器人的航向角为

，则 栅格地图的方向角启发函数为：

…………………………（2）

其中，

表示第

个搜索节点的转向损失函数；

表示第

个搜索节点与终点形 成的夹角。

结合机器人运动起点至终点的方向引导约束

，以及，栅格地图的方向角启发 函数

，将传统的八方向A星算法优化为：

其中，

为第

个搜索节点距机器人运动终点的距离启发函数，具体可按照公 式（1）进行计算；

为机器人的运动起点至第

个搜索节点的实际代价函数，

为机器人 的运动起点经由第

个搜索节点至运动终点的估价函数。在此，优化后的算法中，函数

与函数

与A星算法中的函数保持一致。

如图2所示，机器人运动的出发节点

至目标节点

存在相同长度的路 径

和

，如果未增加方向角启发函数

，则算法容易随机选择等距路径，造成机器人的 行驶效率降低。因此，结合机器人的当前航向角朝向，通过增加方向角启发函数可以快速选 择路径

作为规划路径。对机器人而言，次路径需要旋转的角度更小，更易于快速行进。

如图3、图4所示，同样完成图中的栅格地图路径规划，对于遍历栅格数量，本申请增加了方向角启发函数后的八方向A星算法（图4）比传统的八方向A星算法（图3）相比，明显减少了遍历栅格数；在规划路径长度同样最优的前提下，规划时间缩短了45.38%。对比数据如表1所示，表1如下：

传统的A星算法在进行路径规划时，只能搜索父节点上、下、左、右四个邻域方向节点作为下一步的可行路径，规划路径长度采用曼哈顿距离或者切比雪夫距离计算。由于只能四向搜索，因此，最终规划路径中折点含量多，所规划的路径也不是最短路径，如图5所示；在增加了搜索邻域，使路径折点数和长度进行优化后的多方向A星算法，虽然较传统的A星算法有了改进，但该方法并不是转为机器人路径规划设计，在路径规划中，只考虑了路径长度，并未考虑空间概念，因而，在规划的路径中出现了紧贴障碍物顶点和穿越障碍物对角的现象，如图6所示。

此外，在机器人完成的初步规划路径中，往往存在着路径折点不够平滑且不是最优的情况。传统的路径平滑算法采用遍历节点进行平滑处理，但是这种方法需要从起点或者终点逐个回溯所有路径节点以判断路径是否更短，且未与障碍物碰撞，未碰撞则舍弃中间节点，若碰撞则保留上一节点。但这种方法的缺点是需要遍历初步规划路径上的所有节点，极大的浪费算力，降低整体运行速度。

基于此，本申请中，基于优先折点判据的回溯方法，初步筛选部分节点，经过初步筛选后的节点再通过Floyd算法连接，对机器人的初步规划路径进行优化，生成机器人的优化路径，如图8所示，包括：

步骤S801、基于搜索节点精简后的八方向A星算法，根据环境地图的方向角启发函数和机器人的当前航向角，规划机器人的初始路径。具体的，根据机器人的路径规划中的父节点，搜索机器人的路径规划中的子节点坐标变化值，得到开放节点集合；基于搜索节点精简后的八方向A星算法的曼哈顿距离，求开放节点结合中当前父节点的最小距离子节点为下一路径节点，以生成机器人的初始路径。

对于多方向A星算法在路径规划中出现的问题（如图6所示），本申请，根据父节点 可以搜索的子节点坐标变化值，将八个搜索方向定位仪[0 1;0 -1;-1 0;1 0;-1 1; -1 - 1; 1 1; 1 -1]，对应父节点[上；下；左；右；左上；右上；左下；右下]八个方向的子节点。为 了避免出现规划路径与障碍物顶点相交，当子节点出现在[左上；右上；左下；右下]时，增加 判断该子节点相邻父节点两侧是否为障碍物栅格。例如，当出现左上方子节点时，则判断该 节点的右侧和下侧有无障碍物栅格，有障碍物栅格则该节点不是可行路径，不能放入开放 节点中。如果该节点右侧和下侧均无障碍物节点，则该子节点为可行路径，放入开放节点 中。下一步在开放节点中，通过A星算法

的曼哈顿距离求父节点的最小距离子节点作为 下一路径节点。

据此对增加搜索邻域的多方向A星算法进行优化，得到本申请的八方向A星算法，试验结果如图7所示，由于增加了不可行路径子节点判定，造成了算法执行效率的降低和路径长度的增加，但是该路径才是避免机器人与障碍物碰撞的最优路径，可以在机器人行走过程中减少用于障碍物检测和避障的时间，保证机器人可以快速行驶，最终减少机器人起点至终点的运行时间。

在同一环境下，传统A星算法、增加搜索邻域的多方向A星算法与本申请的八方向A星算法，机器人的路径规划长度与执行时间如表2所示，表2如下：

步骤S802、基于预设的折点判定策略，提取初始路径中的转折点。

其中，预设的折点判定策略为：

其中，

、

、

表示初始路径中相邻的三个节点在环境地 图中的坐标。初始路径中

点坐标为

点坐标为

点坐 标为

。其中，

为当前节点，

为上一节点，

为下一节点。

在机器人的初始路径规划完成后，机器人在栅格地图中所规划路径相邻共线节点 间欧式坐标距离差相等，即

点、

点、

点满足折点判定策略。籍此，有效降低了 运算维度，可以有效加快路径折点的判定。如果路径上的三个折点坐标满足折点判定策略， 则连续的三个坐标点在同一直线上，不存在折点，依次类推，搜索出所有节点。

步骤S803、基于提取的转折点，对初始路劲改进行回溯计算，生成机器人的优化路径。

基于预设的折点判定策略，可以将初始路径中的所有普通节点用

表示，初 始路径中的所有转向节点用

表示，将机器人运动的起点定义为普通节点，机器人运 动的重点定义为折点，然后将所有节点存进同一数组（比如：折点集合file_list）中，在路 径的回溯计算中，优先连接折点。

在对初始路径进行平滑计算时，首先，基于提取的折点，生成机器人的一次规划路 径；然后，确定与一次规划路径的终点相关联且不存在障碍物的最远搜索路径对应的第

个 折点；最后，确定一次规划路径前向连接中与第

个折点相关联且不存在障碍物的最远搜索 路径对应的第

个折点,并依次循环，直至达到一次规划路径的起点，得到机器人的优化路 径。其中，

均为正整数。

也就是说，首先获得初始路径的折点集合file_list；然后，通过回溯算法，从终点开始连接路径中的第二个折点，判断该线段是否与障碍物有交点，如果没有交点，则终点连接路径中的第三个折点，继续判断是否与障碍物有交点，知道该线段遇到障碍物；最后，依次回溯第三折点（假设第三折点与终点连接的线段遇到障碍物）与第二折点之间的剩余节点，最终找到没有与障碍物相交的线段，确定该线段首尾节点，舍弃回溯过程中的其它中间节点。

如图9所示，点A与点B为可以直线连接的相近折点，点A与点B之间的距离定义为

，依次类推，点A与点C、点A与点D连接时，线段与障碍物之间不存在交点，因此初始路 径可以变更为线段AD。如果出现路径与障碍物相交时，如

，则该路径长度

趋向 于无穷大。据此，有：

则可以判定B点为路径中的多余节点，在路径中去掉B节点。

若：

则判定C点为路径中的多余节点，在路径中去掉C节点。

当线段与障碍物相交时，

趋向于无穷大，则：

若点G的坐标为

，那么从点G回溯点D与点G之间的初始路径节点，可以表示 为：

、

、

、

。

其中，

为点G与点F之间的路径，

为点G与点E之间的路径，当点G与点A连接时， 如果

，则删除该节点，如果

，则删除该节点，如果

，则删 除该节点。

若：

则保留点E，舍弃点D。此时，机器人的初始路径由点A→B→C→D→G,优化为由点A经过点E至点G（即A→E→G），籍此，完成点A至点G的路径优化，得到机器人由点A至点G的优化路径。

以图9中的路径为例，机器人的初始路径的节点数组（即折点集合file_list）设为int a[16][3]，其中，节点标志位中“1”代表此处为折点，“0”代表为非折点，则机器人的一次规划路径的节点序列如表3所示，表3如下：

由表3可以看出，本申请的八方向A星算法在没有进行二次路径平滑优化时，路径中包含有6个折点，如表4所示，机器人行进路径明显不是最短，而且需要进行多次转弯操作，增加了机器人的行进用时，表4如下：

在执行完本申请的基于优先折点判据的路径优化后，最终路径平滑由原来遍历16个点，优化为只遍历3个点，如表5所示，大大减少了遍历节点数，提高了算法运行效率，表5如下：

本申请中，当平滑处理连接折点时，如果与新的平滑路径与障碍物碰撞，则对两个折点件的节点进行回溯，籍此，显著缩短普通路径平滑优化的时间复杂度，不比再像传统算法一样，遍历初始路径中的全部节点，而是仅需要遍历其中少数节点。在出现与障碍物相交的路径后，再以该折点为起点向前回溯原路径节点，极大的降低了平滑路径的判断工作量，进而从根本上缩短了对初始路径进行平滑的处理时间。

利用本申请的折点判定策略，可以有效的减少A星算法规划路径的非关键节点，从而提高全局路径规划效率，构建全局的最优路径，避免局部最优。因此，本申请的机器人路径规划方法所规划的路径更短更平滑，曲率变化连续，可以使机器人控制更加容易，减少其转向操作，保证规划路径满足机器人全局最优，对于机器人的全局实时动态路径规划具有良好的稳定性和实时性。

如图10所示，本申请实施例还提供一种结构化环境下机器人路径规划系统，该系统包括：

节点精简单元，配置为在机器人运动的环境地图中，结合机器人实时航向角增加起点至运动终点的方向引导约束，对八方向A星算法的搜索节点进行精简；

路径规划单元，配置为基于搜索节点精简后的八方向A星算法，根据环境地图的方向角启发函数和机器人的当前航向角，对机器人的路径进行规划。

本申请所提供的结构化环境下机器人路径规划系统能够实现上述结构化环境下机器人路径规划方法任一实施例的步骤、流程，并达到相同的技术效果，在此不再一一赘述。

以上所述仅为本申请的优选实施例，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (7)

1.一种结构化环境下机器人路径规划方法，其特征在于，包括：

在机器人运动的环境地图中，结合机器人实时航向角增加起点至运动终点的方向引导约束，对八方向A星算法的搜索节点进行精简；

基于搜索节点精简后的八方向A星算法，根据所述环境地图的方向角启发函数和所述机器人的当前航向角，对所述机器人的路径进行规划。

2.根据权利要求1所述的结构化环境下机器人路径规划方法，其特征在于，还包括：根据所述机器人的航向角，构建所述环境地图的方向角启发函数；

所述方向角启发函数为：

其中，

表示第

个搜索节点的转向损失函数；

表示所述机器人的航向角；

表示第

个搜索节点与终点形成夹角。

3.根据权利要求1所述的结构化环境下机器人路径规划方法，其特征在于，所述基于搜索节点精简后的八方向A星算法，根据所述环境地图的方向角启发函数和所述机器人的当前航向角，对所述机器人的路径进行规划包括：

基于搜索节点精简后的八方向A星算法，根据所述环境地图的方向角启发函数和所述机器人的当前航向角，规划所述机器人的初始路径；

基于预设的折点判定策略，提取所述初始路径中的转折点；

基于提取的转折点，对所述初始路径进行回溯计算，生成所述机器人的优化路径。

4.根据权利要求3所述的结构化环境下机器人路径规划方法，其特征在于，所述基于搜索节点精简后的八方向A星算法，根据所述环境地图的方向角启发函数和所述机器人的当前航向角，规划所述机器人的初始路径，具体为：

根据所述机器人的路径规划中的父节点，搜索所述机器人的路径规划中的子节点坐标变化值，以对所述机器人的路径规划进行不可行路径子节点判定，得到开放节点集合；其中，所述开放节点集合中包含的多个子节点均为所述机器人的可行路径；

基于搜索节点精简后的八方向A星算法的曼哈顿距离，求所述开放节点集合中当前父节点的最小距离子节点为下一路径节点，以生成所述机器人的初始路径。

5.根据权利要求3所述的结构化环境下机器人路径规划方法，其特征在于，所述基于提取的折点，对所述初始路径进行回溯计算，生成所述机器人的优化路径，包括：

基于提取的折点，生成所述机器人的一次规划路径；

确定与所述一次规划路径的终点相关联且不存在障碍物的最远搜索路径对应的第

个折点；

确定所述一次规划路径前向连接中与所述第

个折点相关联且不存在障碍物的最远搜索路径对应的第

个折点，依次循环，直至到达所述一次规划路径的起点，得到所述机器人的优化路径；其中，

均为正整数。

6.根据权利要求3所述的结构化环境下机器人路径规划方法，其特征在于，所述预设的折点判定策略为：

其中，

、

、

表示所述初始路径中相邻的三个节点在所述环境地图中的坐标。

7.一种结构化环境下机器人路径规划系统，其特征在于，包括：

节点精简单元，配置为在机器人运动的环境地图中，结合机器人实时航向角增加起点至运动终点的方向引导约束，对八方向A星算法的搜索节点进行精简；

路径规划单元，配置为基于搜索节点精简后的八方向A星算法，根据所述环境地图的方向角启发函数和所述机器人的当前航向角，对所述机器人的路径进行规划。

Images