[发明专利]基于连通图和迭代搜索的导航路径规划方法

说明书

本发明公开基于连通图和迭代搜索的导航路径规划方法，该导航路径规划方法在机器人预先行走构建出的区域连通图内，按照合理的路径代价搜索出最接近实际起点的路径优化起点和最接近实际终点的路径优化终点，再在迭代搜索的方式的基础上利用启发路径代价和匹配的搜索优先级，完成路径优化起点和路径优化终点之间的导航路径规划。有效减少扩展节点的搜索量，提高区域连通图内的路径节点的搜索速度，提高机器人在区域连通图内的通过性，规划出的路径在地图上的覆盖面增大。

技术领域

本发明涉及路径规划的技术领域，涉及一种基于连通图和迭代搜索的导航路径规划方法。

背景技术

传统的导航路径规划算法(例如Dijkstra、A*、D*)，对栅格地图的精度和面积依赖比较大，随着栅格的读取精确程度增加或所要表征的地图面积增大，搜索成本呈现指数形态增长，且传统的图搜索算法搜索出的路径对机器人通过性考虑较少。

发明内容

为了解决上述技术问题，本发明技术方案在机器人预先行走构建出的区域连通图内，按照合理的路径代价搜索出最接近实际起点的路径优化起点和最接近实际终点的路径优化终点，再按照迭代搜索的方式完成路径优化起点和路径优化终点之间的导航路径规划。具体技术方案如下：

基于连通图和迭代搜索的导航路径规划方法，包括：在预先构建的区域连通图内，利用节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值，搜索出与预先设置的导航起点相匹配的路径优化起点以及与预先设置的导航终点相匹配的路径优化终点；其中，每个节点都位于所属的一条运动轨迹线段上，运动轨迹线段是机器人预先移动产生的；在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点是所述路径优化终点时，根据所述区域连通图内的节点对应的父节点信息，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导航路径；再将当前搜索出的优化导航路径的两端分别连接上所述导航起点和所述导航终点，完成导航路径的规划；在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点不是所述路径优化终点时，则将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，再将当前扩展节点的邻域节点中的未扩展节点设置为新的待扩展节点，直到当前搜索出的启发路径代价和最小的待扩展节点是所述路径优化终点；其中，在所述区域连通图内的任意节点所耗费的启发路径代价和等于从所述路径优化起点到该节点的实际移动代价与从该节点到所述路径优化终点的预测路径代价的和值。与现有技术相比，本技术方案可以有效减少扩展节点的搜索量，提高区域连通图内的路径节点的搜索速度，提高机器人在区域连通图内的通过性，规划出的路径在地图上的覆盖面增大。也在连通图这一替代性的导航地图上发挥启发式搜索算法的路径规划优势。

进一步地，所述利用节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值，搜索出与预先设置的导航起点相匹配的路径优化起点以及与预先设置的导航终点相匹配的路径优化终点的方法包括：若存在待判断节点与所述导航起点的连线不经过障碍物栅格点，则确定该待判断节点是直通所述导航起点的节点；然后对每个直通所述导航起点的节点在其所属的运动轨迹线段上的居中程度值换算出的对应的代价值进行数值排序；再将代价值最小的直通所述导航起点的节点设置为所述路径优化起点；若存在待判断节点与所述导航终点的连线不经过障碍物栅格点，则确定该待判断节点是直通所述导航终点的节点；然后对每个直通所述导航终点的节点在其所属的运动轨迹线段上的居中程度值换算出的对应的代价值进行数值排序；再将代价值最小的直通所述导航终点的节点设置为所述路径优化终点；其中，所述待判断节点属于预先构建的区域连通图内的节点；每个所述待判断节点都位于各自所属的一条运动轨迹线段上。

该技术方案在区域连通图内，利用每个节点在其所属的运动轨迹线段上的居中程度值（表示向运动轨迹线段的中点靠拢的程度、或表示为偏离运动轨迹线段的端点的程度），搜索出直通机器人的导航点且产生代价值最小的节点，作为与导航起点的距离相匹配的路径优化起点、以及与导航终点的距离相匹配的路径优化终点，开启在区域连通图内规划出以路径优化起点为起点、以路径优化终点为终点的导航路径。