一种机器人避障中的最短路径规划方法

摘要

本发明涉及一种机器人避障中的最短路径规划方法，其包括以下步骤：采用卷积运算获得概率栅格地图；对原始的A*算法进行改进，得到改进后的A*算法的代价函数；在概率栅格地图的基础上，采用改进后的A*算法进行从起点到终点的最短路径搜索。本发明对原有栅格地图进行改进得到概率栅格地图，并在概率栅格地图的基础上，采用改进的A*算法进行最短路径搜索，采用本发明规划得到的路径能够使机器人避开障碍物一定距离，使机器人更加安全可靠地运动。

说明书

技术领域

本发明属于机器人避障技术领域，具体涉及一种机器人避障中的最短路径 规划方法。

背景技术

机器人避障路径规划是指在给定的环境障碍条件下，选择一条从起始点到 目标点的路径，使机器人可以安全、无碰撞地通过所有的障碍。这种自主地躲 避障碍物并完成作业任务的方法，是机器人研究和应用中的一个重要内容。目 前，为了能够快速地找到一条从起始点到目标点的最优或最短路径，研究者们 已经开发出很多不同的算法，例如A*算法、Dijkstra算法、遗传算法、粒子群 算法和人工势场法等。

与A*算法相比，Dijkstra算法、遗传算法和粒子群算法都存在数据复杂、 计算量大等缺点。A*算法是在Dijkstra算法基础上加了约束条件的最好优先算 法，在搜索中加入了与问题有关的启发性信息，指导搜索朝最有希望的方向进 行，用于搜索状态空间的最短路径，这比无方向性的Dijkstra算法理论上更加 迅速。然而，采用A*算法规划出的最优路径往往与障碍物紧挨，与机器人之间 缺少缓冲地带。人们更希望略微增加路径长度而使机器人避开周围障碍物，使 机器人更加安全可靠地运动。

人工势场法是为数不多的考虑了安全问题的机器人避障路径规划方法。采 用人工势场法规划出来的路径一般是比较平滑且安全的，但这种方法存在局部 最优，即容易出现局部收敛的问题；而且当两个障碍物位置比较接近时，根据 人工势场法规则，它们之间的通道是不能通过的，因而此时利用人工势场法进 行路径规划要么由于障碍物过近导致规划失败，要么就要沿障碍物外围绕远， 导致规划出来的路径过长。此外，采用人工势场法规划出来的路径多为不规则 曲线，不符合机器人的运动习惯。

发明内容

为了解决现有技术存在的上述问题，本发明提供了一种机器人避障中的最 短路径规划方法。

为实现上述目的，本发明采取以下技术方案：一种机器人避障中的最短路 径规划方法，其包括以下步骤：

采用卷积运算获得概率栅格地图；

对原始的A*算法进行改进，得到改进后的A*算法的代价函数为：

f(n)＝k₁*g(n)+k₂*h(n)+k₃*k(n)，

式中，g(n)表示从起点S到节点n之间的实际代价，代表了搜索广度的优先 趋势；h(n)表示从节点n到目标点D之间的最佳路径的估计代价，包含了搜 索中的启发信息；k(n)表示概率栅格地图中栅格对应的概率值；k₁为代价 g(n)对应的权重，k₂为代价h(n)对应的权重，k₃为概率值k(n)对应的 权重，其中0＜k_i＜1(i＝1,2,3)；

在概率栅格地图的基础上，采用改进后的A*算法进行从起点到终点的最短 路径搜索。

进一步地，所述采用卷积运算获得概率栅格地图的过程为：

(1)利用二维数组定义栅格地图，其包括：

自定义栅格地图中的栅格类型为：FREE、OBSTACLE、START、END和 PROBABILITY；FREE代表表示完全没有障碍物，OBSTACLE表示确定有障碍 物，START表示起点，END表示终点；PROBABILITY从高到低表示障碍物存 在的可能性大小，PROBABILITY的值越大表示障碍物存在的可能性越大；

自定义二维数组的数据类型如下：

其中，s_x和s_y表示某个栅格在地图中的坐标，s_g和s_h表示A*算法规 划中的两个代价距离，s_style表示栅格类型；structAStarNode*s_parent定义了 一个相同类型的指针；ints_is_in_closetable和ints_is_in_opentable表示某一个 栅格在搜索过程中是否已经遍历过，如果已经遍历过，则s_is_in_closetable＝1， s_is_in_opentable＝0；否则，s_is_in_closetable＝0，s_is_in_opentable＝1；

根据自定义的二维数组的数据类型，将二维数组表示为：

AStarNodemap_maze[ROW][COLUMN]；

(2)对栅格地图做卷积运算，得到概率栅格地图，其具体过程为：

对于栅格地图中非边缘部分进行栅格概率化，其具体过程为：

选择一个行数和列数均为m且m为奇数的权矩阵作为卷积核，卷积核表示 为：

$R=\left(\begin{array}{cccc}{R}_{11}& R_{12}& \mathrm{...}& R_{1m}\\ R_{21}& R_{22}& \mathrm{...}& R_{2m}\\ .& & & \\ .& & & \\ .& & & \\ R_{m1}& R_{m2}& \mathrm{...}& R_{mm}\end{array}\right),$

在原始栅格地图中对应选择一个行数和列数均为m且m为奇数的矩阵G：

$G=\left(\begin{array}{cccc}{G}_{11}& G_{12}& \mathrm{...}& G_{1m}\\ G_{21}& G_{22}& \mathrm{...}& G_{2m}\\ .& & & \\ .& & & \\ .& & & \\ G_{m1}& G_{m2}& \mathrm{...}& G_{mm}\end{array}\right),$

则概率栅格地图的中心栅格的概率值为：

$R_{\frac{m+1}{2}\frac{m+1}{2}}^{*}=R_{11}{G}_{11}+R_{12}{G}_{12}+...+R_{\frac{m+1}{2}\frac{m+1}{2}}{G}_{\frac{m+1}{2}\frac{m+1}{2}}+...+R_{mm}{G}_{mm};$

对于栅格地图中边缘部分进行栅格概率化，其具体过程为：

首先，对原有栅格地图做膨胀处理，即在原有栅格地图的四周向外扩充 圈；

其次，采用与栅格地图中非边缘部分栅格概率化相同的方法得到原始栅格 地图的边界部分中各栅格的概率值。

进一步地，所述采用改进后的A*算法进行从起点到终点的最短路径搜索的 过程为：

在概率栅格地图上规定上、下、左、右、左上、左下、右上和右下八个方 向的搜索位置，对于每个方向的搜索位置均使用改进后的A*算法的代价函数计 算代价值，将最小代价值所对应的位置定为下一时刻的位置；

对于一个m*m的概率栅格地图，其中心栅格z周围有8个相邻的栅格；将 这8个相邻的栅格分为两类，一类是水平或垂直栅格，一类是对角栅格；在中 心栅格z周围的8个相邻的栅格中寻找一个与中心栅格之间距离值最小的栅格 b，将中心栅格z标注为已走过的栅格，画出从中心栅格z到下一个栅格b之间 的路径，然后以栅格b作为父节点，重复上述过程；

进行下一次栅格搜索时，首先判断当前栅格的父节点到当前栅格的距离与 上次栅格的父节点到上次栅格的距离之和是否小于上次栅格的父节点到当前栅 格的距离，如果是，则将上次栅格的父节点到上次栅格到当前栅格作为最短路 径，否则需要重新规划最短路径；当前栅格的父节点即为上次栅格。

由于采用以上技术方案，本发明的有益效果为：本发明对原有栅格地图进 行改进得到概率栅格地图，并在概率栅格地图的基础上，采用改进的A*算法进 行最短路径搜索，采用本发明规划得到的路径能够使机器人避开障碍物一定距 离，使机器人更加安全可靠地运动。

附图说明

图1是本发明一实施例中提供的机器人避障中的最短路径规划方法的流程 图；

图2是栅格地图中卷积核的示意图；

图3是起点S、节点n、目标点D以及障碍物之间的相对位置关系示意图；

图4是中心栅格z与其周围栅格之间的相对位置关系示意图；

图5是中心栅格z与障碍物之间的相对位置关系示意图；

图6是采用原始A*算法和改进后的A*算法规划得到的机器人路径示意图； 中，图(a)是采用原始A*算法规划得到的机器人路径示意图；图(b)是采用 改进后的A*算法规划得到的机器人路径示意图。

具体实施方式

下面结合附图和实施例对本发明的技术方案进行详细的说明。

如图1所示，本发明提供了一种机器人避障中的最短路径规划方法，其包 括以下步骤：

1)采用卷积运算获得概率栅格地图，其具体过程为：

(1)利用二维数组定义栅格地图，其包括：

自定义栅格地图中的栅格类型为：FREE、OBSTACLE、START、END和 PROBABILITY。其中，FREE代表表示完全没有障碍物，OBSTACLE表示确定 有障碍物，START表示起点，END表示终点；PROBABILITY从高到低表示障 碍物存在的可能性大小，PROBABILITY的值越大表示障碍物存在的可能性越 大。

自定义二维数组的数据类型如下：

其中，s_x和s_y表示某个栅格在地图中的坐标，s_g和s_h表示A*算法规 划中的两个代价距离，s_style表示栅格类型。structAStarNode*s_parent定义了 一个相同类型的指针，从而可以定义一个链表来表示所要存储的节点，方便计 算机内存管理。ints_is_in_closetable和ints_is_in_opentable表示某一个栅格在 搜索过程中是否已经遍历过，如果已经遍历过，则s_is_in_closetable＝1， s_is_in_opentable＝0；否则，s_is_in_closetable＝0，s_is_in_opentable＝1。

根据自定义的二维数组的数据类型，将二维数组表示为：

AStarNodemap_maze[ROW][COLUMN]。

(2)对栅格地图做卷积运算，得到概率栅格地图，其具体过程为：

对于栅格地图中非边缘部分进行栅格概率化，其具体过程为：

选择一个行数和列数均为m且m为奇数的权矩阵作为卷积核，卷积核表示 为：

$R=\left(\begin{array}{cccc}{R}_{11}& R_{12}& \mathrm{...}& R_{1m}\\ R_{21}& R_{22}& \mathrm{...}& R_{2m}\\ .& & & \\ .& & & \\ .& & & \\ R_{m1}& R_{m2}& \mathrm{...}& R_{mm}\end{array}\right),$

在原始栅格地图中对应选择一个行数和列数均为m且m为奇数的矩阵G：

$G=\left(\begin{array}{cccc}{G}_{11}& G_{12}& \mathrm{...}& G_{1m}\\ G_{21}& G_{22}& \mathrm{...}& G_{2m}\\ .& & & \\ .& & & \\ .& & & \\ G_{m1}& G_{m2}& \mathrm{...}& G_{mm}\end{array}\right),$

则概率栅格地图的中心栅格的概率值为：

$R_{\frac{m+1}{2}\frac{m+1}{2}}^{*}=R_{11}{G}_{11}+R_{12}{G}_{12}+...+R_{\frac{m+1}{2}\frac{m+1}{2}}{G}_{\frac{m+1}{2}\frac{m+1}{2}}+...+R_{mm}{G}_{mm}.$

例如，如图2所示，选择一个行数和列数均为m＝3的权矩阵作为卷积核， 卷积核表示为：

$R=\left(\begin{array}{ccc}{R}_1& R_2& R_3\\ R_4& R_5& R_6\\ R_7& R_8& R_9\end{array}\right),$

在原始栅格地图中对应选择一个行数和列数均为m＝3的矩阵G：

$G=\left(\begin{array}{ccc}{G}_1& G_2& G_3\\ G_4& G_5& G_6\\ G_7& G_8& G_9\end{array}\right),$

则概率栅格地图的中心栅格的概率值为：

$R_5^{*}=R_1{G}_1+R_2{G}_2+R_3{G}_3+R_4{G}_4+R_5{G}_5+R_6{G}_6+R_7{G}_7+R_8{G}_8+R_9{G}_9.$

对于栅格地图中边缘部分进行栅格概率化，其具体过程为：

首先，对原有栅格地图做膨胀处理，即在原有栅格地图的四周各向外扩充 圈。例如，原有栅格地图的行数为90，列数为100，即90X100，则扩充 后的栅格地图的行数为92，列数为102，即92X102。

其次，采用与栅格地图中非边缘部分栅格概率化相同的方法得到原始栅格 地图的边界部分中各栅格的概率值。

通过上述对初始栅格地图的卷积运算，栅格地图中各个栅格的值由原来的 FREE或OBSTACLE两个值变为PROBABILITY(包括FREE和OBSTACLE)， 概率栅格地图中各栅格的值不仅能够表示某个栅格有没有障碍物，还能表示该 栅格存在障碍物的可能性，栅格的值越大，表示该栅格存在障碍物的概率越大。

2)对原始的A*算法进行改进，得到改进后的A*算法的代价函数为：

f(n)＝k₁*g(n)+k₂*h(n)+k₃*k(n)，

式中，如图3所示，g(n)表示从起点S到节点n之间的实际代价，代表 了搜索广度的优先趋势。h(n)表示从节点n到目标点D之间的最佳路径的估 计代价，包含了搜索中的启发信息。k(n)表示概率栅格地图中栅格对应的概 率值。k₁为代价g(n)对应的权重，k₂为代价h(n)对应的权重，k₃为概率 值k(n)对应的权重，其中0＜k_i＜1(i＝1,2,3)。

同时调节k₁、k₂和k₃三个权重可以更加有效的调节规划出的机器人路径。 例如，可以根据需要控制所规划出的路径是否是距离最短，是否与障碍物保持 一定距离，使机器人更安全等。其中g(n)和h(n)两个代价值主要决定所规 划的路径是否最短，而k(n)主要决定所规划的机器人路径是否与障碍物保持 一定距离。因此，若想得到最短路径，调节k₁和k₂的值，使k₁和k₂均略大于k₃； 若想得到离障碍物较远的路径，调节k₁和k₂的值，使k₁和k₂均略小于k₃。

3)在概率栅格地图的基础上，采用改进后的A*算法进行从起点到终点的 最短路径搜索。

改进后的A*算法是基于栅格地图的搜索算法，将栅格地图转变成概率栅格 地图之后，每个栅格的种类便包含起点、终点、完全无障碍区域、确定障碍物 区域和疑似障碍物区域。

在概率栅格地图上规定上、下、左、右、左上、左下、右上和右下八个方 向的搜索位置，对于每个方向的搜索位置均使用改进后的A*算法的代价函数计 算代价值，将最小代价值所对应的位置定为下一时刻的位置。

搜索过程中，对于一个m*m的概率栅格地图，其中心栅格z周围有8个相 邻的栅格。将这8个相邻的栅格分为两类，一类是水平或垂直栅格，一类是对 角栅格。在中心栅格z周围的8个相邻的栅格中寻找一个与中心栅格之间距离 值最小的栅格b，将中心栅格z标注为已走过的栅格，画出从中心栅格z到下一 个栅格b之间的路径，然后以栅格b作为父节点，重复上述过程。

进行下一次栅格搜索时，首先判断当前栅格的父节点(即上次栅格)到当 前栅格的距离与上次栅格的父节点到上次栅格的距离之和是否小于上次栅格的 父节点到当前栅格的距离，如果是，则将上次栅格的父节点到上次栅格到当前 栅格作为最短路径，否则需要重新规划最短路径。

下面对以上描述进行举例说明。

如图4所示，搜索过程中，在栅格z的周围有8个相邻的栅格，8个相邻的 栅格分为两大类，其中一类为水平或垂直栅格，当前栅格到与之水平或竖直的 栅格的距离为10；另一类为对角栅格，当前栅格到其对角栅格的距离为14。在 栅格z周围的8个相邻的栅格中寻找一个与之距离值最短的栅格，例如栅格1。 然后，把栅格z标注为已走过的栅格，画出从栅格z到下一个栅格1之间的路 径，然后以栅格1为父节点，重复这一过程。

如图5所示，从栅格z开始搜索，黑色部分为障碍物，在第一次搜索过程 中栅格1的代价值最小，但第二次搜索时栅格1的右侧为障碍物，不能穿越， 只能从栅格1向上或者向下运动，例如选择向下运动到栅格8，此时从栅格z 经过栅格1到达栅格8的代价值为10+10＝20。而直接从栅格z到栅格8的代价 值为14。显然，14<20，因此，从栅格z到栅格1再到栅格8的路径不是最短 路径，需要重新规划最短路径。

原始的A*算法虽然能够保证搜索到全局最优路径，这也正是机器人领域中 路径规划时寻找最短路径所想要的结果，但是考虑到机器人在运动过程中要避 障，而且机器人本身车体有一定宽度，当机器人沿着最短路径行走时往往面临 着与障碍物相碰的情况。此外，当机器人通过狭窄通道时，人们往往希望机器 人能够沿着通道中间的道路行走，但是实际规划的路径可能紧紧沿着某一侧的 墙运动，这往往会带来不可预知的危险。

如图6所示，其中，图a为采用原始A*算法规划得到的机器人路径示意图， 从图a中可以看到规划出的路径一直沿着障碍物。图b为采用改进后的A*算法 规划得到的机器人路径示意图，从图b中可以看出采用改进后的A*算法规划得 到的机器人路径避免了路径一直沿着障碍物这个问题，给机器人运动过程中留 出了一定的安全距离，使机器人运动更安全。

本发明不局限于上述最佳实施方式，本领域技术人员在本发明的启示下都 可得出其他各种形式的产品，但不论在其形状或结构上作任何变化，凡是具有 与本申请相同或相近似的技术方案，均落在本发明的保护范围之内。