CN116414139A - 基于A-Star算法的移动机器人复杂路径规划方法 - Google Patents

Abstract

本发明公开了一种基于A‑Star算法的移动机器人复杂路径规划方法，包括步骤：获取环境地图并栅格化，在环境地图上标记障碍物、移动机器人的起点A和终点B；同时以A和B为起始节点、以对向的当前节点为目标节点，展开双向搜索，搜索的时候，仅保留部分子节点，且采用核心衰减系数作为动态权重计算启发函数，并对初步路径进行曲线平滑，得到路径轨迹。本发明引入动态权重、启发函数不唯一、并引入角度对当前节点到目标节点方向进行约束，能减少运算量、提高搜索效率、优化规划路径。

Description

基于A-Star算法的移动机器人复杂路径规划方法

技术领域

本发明涉及机器人路径规划领域，尤其涉及一种基于A-Star算法的移动机器人复杂路径规划方法。

背景技术

随着智慧城市的不断建设，智能设备不断普及，越来越多的领域开始应用移动机器人。比如食品物流运输，无人机和自动驾驶等。移动机器人的技术核心在于路径规划，该算法可以保证机器人高效率地，安全地完成作业。辅助机器人从起点开始避开道路中障碍物并快速地到达目的地。路径规划主要分为全局路径规划和局部路径规划，全局路径规划用于静态路面环境，在配合激光雷达生成的已知静态路面环境下进行路径规划，从而找到最优路径。局部路径规划多是用于存在动态障碍的路面环境下的局部避障控制，能够在短距离内进行路径的再优化，即动态路径规划。传统算法有A*算法、D*算法、Dijkstra等算法，智能轨迹规划算法有蚁群算法、粒子群算法、神经网络等算法。智能寻路算法会多次进行迭代来寻找最优路径，这个过程，伴随有大量的随机运动，导致寻优效率底下，相比之下，传统路径规划算法具有良好的适用性和可拓展性，以A*算法为代表。

A*算法也称为A-Star算法、A星算法，是一种求解最短路径最有效的直接搜索方法，也是许多其他问题的常用启发式算法，其原理为，每一步巡径时，以当前所述位置的节点为当前节点，向周围拓展子节点，与当前节点相邻的8个节点，都为子节点。然后计算每个子节点的代价评价函数f(n)，将f(n)最小的子节点作为最优节点，在一步拓展中，将该最优节点，作为下一步拓展的父节点，也就是下一步的当前节点。

发明内容

本发明的目的就在于提供一种解决上述所述的搜索效率低、拐点不平滑、拓展冗余点多等问题，能优化巡径、巡径效率高、路径更合理、更符合移动机器人的物理运动性的，基于A-Star算法的移动机器人复杂路径规划方法。

为了实现上述目的，本发明采用的技术方案是这样的：一种基于A-Star算法的移动机器人复杂路径规划方法，包括以下步骤：

（1）获取环境地图并栅格化为c1行c2列的栅格图，在栅格图上标记障碍物、移动机器人的起点A和终点B；

（2）同时以A和B为起始节点、以对向的当前节点为目标节点，展开双向搜索，其中，A1出发为正向，B1出发为反向，包括步骤（21）-（23）；

（21）分别为正向和反向的当前节点拓展子节点；

第n步时，正向和反向的当前节点An和Bn，为前一步拓展得到的最优节点，且当n=1时，A1=A，B1=B；

（22）作An和Bn的连线；

（23）从子节点中选择待选点；

对正向当前节点，找到其被连线穿过的子节点，保留该子节点和与它距离最近的2~4个子节点作为待选点，其余删除；对反向当前节点，找到其被连线穿过的子节点，保留该子节点和与它距离最近的2~4个子节点作为待选点，其余删除；

（24）为正向搜索和反向搜索拓展最优节点，二者拓展方式相同，其中正向搜索的拓展方式包括步骤（a1）~（a4）；

（a1）设置第一代价评价函数f₁(n)和第二代价评价函数f₂(n)；

式中，n为当前节点的步数，g(n)为起始节点到正向当前节点的实际代价，h(n)为正向当前节点到反向当前节点的实际代价，p(n)为正向当前节点到终点的实际代价，a(g(n))为当前节点的核心衰减系数；

其中，σ为常量且/>

，e为自然常数，c为栅格图行列平均值，/>

；

（a2）对An，计算每个待选点的f₁(n)；

（a3）选f₁(n)值最小的待选点，分别与Bn和B连线，并计算两条连线的夹角θ；

（a4）若θ＜90°，则f₁(n)值最小的待选点为最优节点，否则作An和B的连线，并按步骤（23）重新选择待选点，并计算每个待选点的f₂(n)，将f₂(n)值最小的待选点为最优节点；

（a5）正向的起始节点经每一步正向的当前节点后与最优节点的连线，构成正向的规划路径；

（3）每拓展一步最优节点，判断一次正向搜索和反向搜索是否相遇，若未相遇，重复步骤（2），否者从相遇处开始，向起点和终点回溯，绘制出初步路径；

（4）对初步路径进行曲线平滑，得到路径轨迹。

作为优选：步骤（a2）中，g（n）、p（n）分别采用下式计算；

式中，（x₁,y₁）、（x₂,y₂）、（x₃,y₃）分别为正向当前节点、反向当前节点、终点的坐标。

作为优选：步骤（3）中，满足下列任一情况，则判断正向搜索和反向搜索相遇；

情况一：正向和反向的两个最优节点相遇；

情况二：正向的最优节点与反向的规划路径相遇；

情况三：反向的最优节点与正向的规划路径相遇。

作为优选：所述曲线平滑为采用贝塞尔曲线平滑。

与现有技术相比，本发明的优点在于：

（1）本发明在A-Star算法的启发函数中引入核心衰减系数a(g(n))作为改进后的动态权重，权重随着当前点与目标点距离而动态变化，距离越近权重越大，搜索更快。同时，启发函数不唯一，而是引入了角度判断来选择启发函数，本发明中，θ＜90°时采用第一代价评价函数f₁(n)作为启发函数，θ≥90°时第二代价评价函数f₂(n)作为启发函数。θ的目的是加入当前节点到目标节点的约束，保持规划路线的大致方向，从而进一步提高A-Star算法的搜索效率。

（2）本发明在进行进行路径搜寻中，对当前节点的子节点进行了优化，在不被障碍物阻挡的条件下，优化掉部分子节点，仅保留少部分节点作为待选点，能节约资源的占用，提高算法的运行效率。待选点的选择，我们先默认An和Bn的连线来选择待选点，同时加入了角度判定。若该连线选出的f₁(n)值最小的待选点，得到的夹角θ≥90°了，就要重新使用An和B的连线来选择待选点，从而保证保留下来的子节点相对准确。

（3）本发明还引入了动态双向搜索，同时以终点和起点出发向对向巡径，配合动态权重的启发函数以及搜索优化，增大了前期的搜寻范围，减少了算法遍历点数，提高了算法搜索效率。

（4）对初步路径进行曲线平滑：通过Bezier曲线对已经规划出的最优路径进行平滑处理，使得拐角处的拐角处平滑化，更符合机器人运动特性。A-Star算法的改进后在减少耗时跟路径寻优上都有较好效果。

附图说明

图1为本发明流程图；

图2为给当前节点拓展子节点的示意图；

图3为按本发明步骤（22）（23）选择待选点的示意图；

图4是实施例2在20×20的栅格图，障碍物占比0.2的路面环境得到的路径仿真图；

图5为实施例2在60×60的栅格图，障碍物占比0.2的路面环境得到的路径仿真图；

图6为实施例2在60×60的栅格图，障碍物占比0.3的路面环境得到的路径仿真图；

图7为在60×60的栅格图，障碍物占比0.3的一种路面环境中采用现有A-Star路径规划得到的路径仿真图；

图8为对图7中地图采用本发明方法得到的路径仿真图；

图9为60×60的栅格图，障碍物占比0.3的另一种路面环境中采用现有A-Star路径规划得到的路径仿真图；

图10为对图9中地图采用本发明方法得到的路径仿真图；

图11为本发明基于A-Star算法的算法流程图。

具体实施方式

下面将结合附图对本发明作进一步说明。

实施例1：参见图1、图2、图3和图11，一种基于A-Star算法的移动机器人复杂路径规划方法，包括以下步骤：

（1）获取环境地图并栅格化为c1行c2列的栅格图，在栅格图上标记障碍物、移动机器人的起点A和终点B；

（2）同时以A和B为起始节点、以对向的当前节点为目标节点，展开双向搜索，其中，A1出发为正向，B1出发为反向，包括步骤（21）-（23）；

（21）分别为正向和反向的当前节点拓展子节点；

第n步时，正向和反向的当前节点An和Bn，为前一步拓展得到的最优节点，且当n=1时，A1=A，B1=B；

（22）作An和Bn的连线；

（23）从子节点中选择待选点；

对正向当前节点，找到其被连线穿过的子节点，保留该子节点和与它距离最近的2~4个子节点作为待选点，其余删除；对反向当前节点，找到其被连线穿过的子节点，保留该子节点和与它距离最近的2~4个子节点作为待选点，其余删除；

（24）为正向搜索和反向搜索拓展最优节点，二者拓展方式相同，其中正向搜索的拓展方式包括步骤（a1）~（a4）；

（a1）设置第一代价评价函数f₁(n)和第二代价评价函数f₂(n)；

式中，n为当前节点的步数，g(n)为起始节点到正向当前节点的实际代价，h(n)为正向当前节点到反向当前节点的实际代价，p(n)为正向当前节点到终点的实际代价，a(g(n))为当前节点的核心衰减系数；

其中，σ为常量且/>

，e为自然常数，c为栅格图行列平均值，/>

；

（a2）对An，计算每个待选点的f₁(n)；

（a3）选f₁(n)值最小的待选点，分别与Bn和B连线，并计算两条连线的夹角θ；

（a4）若θ＜90°，则f₁(n)值最小的待选点为最优节点，否则作An和B的连线，并按步骤（23）重新选择待选点，并计算每个待选点的f₂(n)，将f₂(n)值最小的待选点为最优节点；

（a5）正向的起始节点经每一步正向的当前节点后与最优节点的连线，构成正向的规划路径；

（3）每拓展一步最优节点，判断一次正向搜索和反向搜索是否相遇，若未相遇，重复步骤（2），否者从相遇处开始，向起点和终点回溯，绘制出初步路径；

（4）对初步路径进行曲线平滑，得到路径轨迹。

步骤（a2）中，g（n）、p（n）分别采用下式计算；

式中，（x₁,y₁）、（x₂,y₂）、（x₃,y₃）分别为正向当前节点、反向当前节点、终点的坐标。

步骤（3）中，满足下列任一情况，则判断正向搜索和反向搜索相遇；

情况一：正向和反向的两个最优节点相遇；

情况二：正向的最优节点与反向的规划路径相遇；

情况三：反向的最优节点与正向的规划路径相遇。

所述曲线平滑为采用贝塞尔曲线平滑。

关于子节点和待选点：如图2所示，子节点为与当前节点相邻的8个节点，正向的当前节点An对应8个子节点，分别标记为1-8，反向的当前节点Bn对应8个子节点，也分别标记为1-8。假设本实施例中，待选点为与子节点距离最近的2个子节点，按步骤（22）连接An和Bn，得到一条连线，对An来说，标记3的子节点被连线穿过，所以保留标记3、2、4三个子节点作为待选点；对Bn来说，标记7的子节点被连线穿过，所以保留标记7、6、8三个子节点作为待选点。当然，若待选点为与子节点距离最近的3个或4个子节点，我们根据距离排序，取距离最近的前3个或前4个子节点即可。

关于夹角θ，参见图3，本发明就是判断图3中的夹角θ是否超过90，若超过，则需要按步骤（a4）重新选择待选点。

本发明的算法基于A-Star算法，A-Star算法的预处理包括：

S1-1：将地图栅格化，每个正方形格子的中央成为节点

S1-2：确定起始节点和目标节点；

S1-3：设定setOpen列表和setClosed列表，setOpen列表中存放已拓展但未访问过的节点，setClosed列表中存放已访问过的节点；

S1-4：初始时，定义起点为父节点，存入setClosed列表中；

S1-5：父节点周围共8个节点，定义为子节点，存入setOpen列表中。

而本发明是在A-Star算法基础上的改进，所以其理论框架基于A-Star算法。在进行节点拓展时，基于本发明方法，其拓展的方式参见图11，具体如下：

S2-1：将地图栅格化，每个正方形格子的中央成为节点；

S2-2：确定起始节点和目标节点，本发明是双向巡径，所以同时以A和B为起始节点、以对向的当前节点为目标节点；

S2-3：设定setOpen列表和setClosed列表，setOpen列表中用于存放已拓展但未访问过的节点，setClosed列表中用于存放已访问过的节点，开始时setOpen列表为起始节点、setClosed列表为空；

S2-4：双向搜索时，两边拓展的方法是一样的，我们以单边搜索为例，初始第一步时，将起始节点作为父节点，存入setClosed列表中，并从setOpen列表中删除该起始节点，然后通过本发明方法，拓扑得到最优节点，再将最优节点放入setClosed列表，更新其方向和代价信息，并作为新的父节点，同时将其从setOpen列表中删去；

在正向搜索和反向搜索都完成一次，拓展出最优节点，也就是下一步的父节点后，我们需要判断一次双向巡径是否相遇，若相遇则搜索结束，回溯得到回溯，若未相遇，则进行下一次搜索，具体下下面步骤S2-5。

S2-5：以第n步为例，在第n步时，正向搜索和反向搜索都分别进行第n次拓展，两边搜索方法相同，我们还是以一个单边为例。首先按步骤（22）做An和Bn的连线，按步骤（23）选择待选点，按步骤（24）拓展待选点，将待选点存入setOpen列表中，从待选点中得到最优节点，并将最优节点存入setClosed列表，更新其方向和代价信息，并作为新的父节点，同时将其从setOpen列表中删去。

实施例2：参见图1到图6，为了说明本发明效果，我们将不同的路面环境，按大小的栅格化为尺寸不同的栅格图，其中，在20×20的栅格图、障碍物占比0.2的路面环境中采用本发明方法进行复杂路径规划仿真，得到图5。在60×60的栅格图、障碍物占比0.2的路面环境中采用本发明方法进行复杂路径规划仿真，得到图6。在60×60的栅格图、障碍物占比0.3的路面环境中采用本发明方法进行复杂路径规划仿真，得到图6。

从图4，图5，图6可以看到，随着栅格图模拟的地图范围的增大，障碍物的存在形式更加多变，路线相比小范围地图更复杂，同时障碍物占比的增加，同样会增大路径的寻优难度，因此，保障前期的路径寻优很重要。

实施例3：参见图1到图10，为了说明本发明的效果，我们采用下表中6种路径规划方法对同一张40x40栅格的复杂路面环境进行路径规划，得到下表1：

表1 多种算路径规划方法仿真对比

从表1可以看出，传统A-Star算法在加上权值系数的优化后在时间上有了一定的减少，但是所规划得到的最优路径的概率大大降低，而采用动态权值优化在时间上相对普通权值系数运行时间有所增加，但是寻优率得到了更好的改善，取得的路径相对更好，并且时间上比传统A-Star算法更快，与权值A-Star效率相差无几。加入搜索向优化后，让算法在无死路的情况下避免走回头路，大大减少冗余点的遍历。最后加入双向搜索，让算法运行效率进一步提升，搜索效率相比传统A-Star降低大约40％，步长相比普通的权值优化更好，极大减少了遍历点数，加快算法搜索效率，在复杂空间内具有良好的效果，既保证了运行效率又保证了路径的相对优秀，在更为开阔的环境下，带来的效果。

对比实验：

我们采用方法一和方法二，对两幅相同的地图进行了模拟对比实验。方法一为使用固定权值系数的双向A-Star路径规划算法，方法二为本发明方法。得到图7、图8、图9、图10，从图中可以看出，采用本发明方法所得到的路径在路径更优，遍历点数少，转折点相对更少。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于A-Star算法的移动机器人复杂路径规划方法，其特征在于，包括以下步骤：

（1）获取环境地图并栅格化为c1行c2列的栅格图，在栅格图上标记障碍物、移动机器人的起点A和终点B；

（2）同时以A和B为起始节点、以对向的当前节点为目标节点，展开双向搜索，其中，A1出发为正向，B1出发为反向，包括步骤（21）-（23）；

（21）分别为正向和反向的当前节点拓展子节点；

第n步时，正向和反向的当前节点An和Bn，为前一步拓展得到的最优节点，且当n=1时，A1=A，B1=B；

（22）作An和Bn的连线；

（23）从子节点中选择待选点；

对正向当前节点，找到其被连线穿过的子节点，保留该子节点和与它距离最近的2~4个子节点作为待选点，其余删除；对反向当前节点，找到其被连线穿过的子节点，保留该子节点和与它距离最近的2~4个子节点作为待选点，其余删除；

（24）为正向搜索和反向搜索拓展最优节点，二者拓展方式相同，其中正向搜索的拓展方式包括步骤（a1）~（a4）；

（a1）设置第一代价评价函数f₁(n)和第二代价评价函数f₂(n)；

式中，n为当前节点的步数，g(n)为起始节点到正向当前节点的实际代价，h(n)为正向当前节点到反向当前节点的实际代价，p(n)为正向当前节点到终点的实际代价，a(g(n))为当前节点的核心衰减系数；

其中，σ为常量且/>

，e为自然常数，c为栅格图行列平均值，/>

；

（a2）对An，计算每个待选点的f₁(n)；

（a3）选f₁(n)值最小的待选点，分别与Bn和B连线，并计算两条连线的夹角θ；

（a4）若θ＜90°，则f₁(n)值最小的待选点为最优节点，否则作An和B的连线，并按步骤（23）重新选择待选点，并计算每个待选点的f₂(n)，将f₂(n)值最小的待选点为最优节点；

（a5）正向的起始节点经每一步正向的当前节点后与最优节点的连线，构成正向的规划路径；

（3）每拓展一步最优节点，判断一次正向搜索和反向搜索是否相遇，若未相遇，重复步骤（2），否者从相遇处开始，向起点和终点回溯，绘制出初步路径；

（4）对初步路径进行曲线平滑，得到路径轨迹。

2.根据权利要求1所述的基于A-Star算法的移动机器人复杂路径规划方法，其特征在于，步骤（a2）中，g（n）、p（n）分别采用下式计算；

式中，（x₁,y₁）、（x₂,y₂）、（x₃,y₃）分别为正向当前节点、反向当前节点、终点的坐标。

3.根据权利要求1所述的基于A-Star算法的移动机器人复杂路径规划方法，其特征在于，步骤（3）中，满足下列任一情况，则判断正向搜索和反向搜索相遇；

情况一：正向和反向的两个最优节点相遇；

情况二：正向的最优节点与反向的规划路径相遇；

情况三：反向的最优节点与正向的规划路径相遇。

4.根据权利要求1所述的基于A-Star算法的移动机器人复杂路径规划方法，其特征在于，所述曲线平滑为采用贝塞尔曲线平滑。

Images