ros navigation 解析之A*路径规划

        最近在看ros的导航规划算法,后面打算一一解析一边,先从global planner的A*算法开始吧。

        ros里面的全局规划用的是global_planner而不是navfn包,修改这种选择也比较简单,只需要将move_base参数的

global_planner替换为global_planner/GlobalPlanner即可。

全局参数设置

       然后全局规划函数在global_planner/planner_core.cpp中,我们先去看 GlobalPlanner::initialize函数的一些参数设置

ros navigation 解析之A*路径规划_第1张图片使用Quadratic算法还是Potential算法,我们选择Potential算法

ros navigation 解析之A*路径规划_第2张图片选择A*算法还是Dijkstra算法,我们用A*算法

ros navigation 解析之A*路径规划_第3张图片选择GridPath合适GradientPath,我们用GridPath即栅格地图

初始化里面主要就是设置参数。

     然后我们进入到最重要的路径规划部分

函数是在GlobalPlanner::makePlan函数中,最核心的计算cost的函数就是

A*函数

由于我们选用的A*算法,所以该函数去到astar.cpp里面去查看

ros navigation 解析之A*路径规划_第4张图片

核心函数就在add函数里面,是计算每一个当前点的周围可用点到起始点的potential即cost

ros navigation 解析之A*路径规划_第5张图片

该函数的核心就在calculatePotential里面,根据之前的参数选择,该函数应该是使用的PotentialCalculateor,

所以在potential_calculator.h里面ros navigation 解析之A*路径规划_第6张图片

该函数是计算起始点到当前点的potential,选取当前点周围prev_potential最小的值然后加上该点的cost值。

上面的流程就将起始点可到终点的相关点的cost计算完毕,下面就选择cost最小的路径即可。

继续返回GlobalPlanner::makePlan函数中,

计算路径

 GlobalPlanner::getPlanFromPotential函数中,核心的函数就是

计算路径核心

根据之前初始化的选择,我们使用girdPath,在grid_path.cpp中

ros navigation 解析之A*路径规划_第7张图片

至此,A*算法就将全局路径计算完毕

你可能感兴趣的:(机器人,ros,算法)