[发明专利]一种时间优化的平滑A*算法

说明书

本发明公开了一种时间优化的平滑A*算法，包括如下步骤：步骤S1：对A*算法的代价估计函数进行加权处理；步骤S2：对A*算法的OPEN列表使用最小二叉堆进行优化；步骤S3：对步骤S1、S2优化后的路径进行3次B样条优化处理，得到最终的经3次B样条优化处理之后的路径；本发明采用时间优化的平滑A*算法，通过对A*算法的代价估计函数进行加权处理以及对A*算法的OPEN列表使用最小二叉堆进行优化，减少了路径规划算法运算时间，优化了整体路径平滑度，解决了现有的A*算法在实际的工程运用中存在折线多、转折次数多、累计转角大等问题，提高了机器人的工作效率。

技术领域

本发明涉及一种路径规划A*算法，尤其涉及一种时间优化的平滑A*算法，具体来说是涉及一种融合最小二叉堆、系数加权和3次B样条优化来减少路径规划算法运算时间和对整体路径平滑度进行优化的平滑A*算法。

背景技术

A*算法是广泛用于寻路和图遍历的计算机算法。由于其具有较好的性能和准确性，经常被使用在最短路径求解中，是一种启发式搜索算法，其成本估计函数为：f(n)＝g(n)+h(n)；式中：f(n)为从起始节点经由当前节点n到目标节点的估计成本，g(n)为初始节点到当前节点n的实际成本，h(n)为从当前节点到目标节点n的最佳路径的估计成本。

A*算法的算法思想在于：A*算法典型的实现方法是使用优先级队列来重复执行选择要扩展的最小估计成本节点。此优先级队列称为开集，在算法的每个步骤中，从开集中移除具有最低f(n)值的节点，其相邻节点的f(n)和g(n)值被相应地更新，并且这些相邻节点被添加到队列中。算法继续进行，直到目标节点具有比队列中的任何节点更低的值(或者直到队列为空)，而该目标节点的值就是最短路径的长度。

在运行此算法之后，目标节点将指向其前导节点。依此类推，直到某个节点的前导是开始节点，就可以得到最短路径序列。

A*算法采用了一个启发函数h(n)，使算法利用启发信息朝着某个确定的方向搜索，所以其访问的节点比Dijkstra算法少得多，能快速地导向目标结点，访问到目标节点后算法停止。对于起始点和目标点都确定的情况，A*能利用启发函数快速找到目标点，现有的A*算法生成的路径图如图1所示，图1中的白色折线连接起来所经过的黑点为算法计算出的CLOSE节点，其他黑色部分均为障碍物。

目前，A*算法的运行过程主要是按照移入、排序、移出的步骤来进行的，其中所耗费的主要代价在于遍历状态表来找出open表中f(n)最小的顶点。当移动机器人所处的面积比较大的时候，出行路线很长，open表中的数据量将很大，当反复查询这张open表的时候会导致算法运行所需的时间代价更高。因此open表的数据结构决定了算法效率的高低，优化算法中状态的查找速度可以有效提升算法的效率。

同时，A*算法在实际的工程运用中存在折线多、转折次数多、累计转角大等问题，给机器人运行造成较大的麻烦，例如，转角多时，必须准确计算出机器人和障碍物间距，否则就会发生碰撞；当累计角度大、转角多时，机器人的工作效率也会下降。

本发明选择A*算法作为最短路径规划的基础算法，其代价估计函数f(n)＝g(n)+h(n)是影响算法运行效率的重要因素。当从初始点向目标点搜索时，算法权衡g(n)、h(n)这两个值，每次循环时，检查最小节点。搜索最短路径的关键在于启发函数h(n)的选取。h(n)越接近实际值，搜索效率越高。

理论上，如果h(n)是0，则只有g(n)起作用，此时A*演变为Dijkstra算法。而Dijkstra算法是用于在有向图或无向图中的节点之间找到最短路径的算法。常见的方法是将单个节点固定为源节点，并找到从源节点到图中所有其他节点的最短路径，形成最短路径树；Dijkstra算法的算法思想在于：开始节点被称为源节点，某个节点的距离是指从初始节点到该节点的距离。Dijkstra算法将分配一些初始距离值，并逐步更新节点的距离值。