|
1.在Planner_core代码中的全局规划的初始化中，先由use_dijkstra参数判断使用dijkstra算法还是A*算法； 2.当使用A*算法时，实例化AStarExpansion，使其设置好搜索的网格大小和地图障碍物，代价值等内容； 3.然后调用calculatePotentials计算出从起始位置到目标位置的路径，得到查找路径过程的数组； 4.在getPlanFromPotential函数中调用getPath函数将查找路径过程的数组转换为从起始位置到目标位置的每个经过的网格的坐标数据，然后再转换为对应的每个pose的数组 calculatePotentials的实现方法如下 1.清空队列（这里的队列应该相当于A*里的openlist） 2.计算出起始位置在网格中的索引 3.将起始位置如队列 4.填充网格的势场值（potential，应该相当于A*算法中F=G+H中的G值）为POT_HIGH（1.0e10），并将起始位置的势场值设为0 5.计算目标位置的索引 6.循环计算得到路径： （1）将堆顶数据（即openlist中的最小值，根据排序的第一个元素）赋值给top,用于判断是否找到目标地点 （2）将堆顶数据移到队尾，重新构造排列堆，将队尾数据移除 （3）判断是否找到目标地点，找到返回true （4）调用add函数计算当前网格邻近四个方向的势场值： 1.如果邻近网格势场值小于POT_HIGH（该网格即已添加到openlist中），返回，不进行下一步计算 2.如果代价值大于障碍物致命值同时不是未知值，返回 3.计算还没添加进openlist的邻近网格的代价值，返回上一节点加上每移动一格的代价值，如果上一节点势场值小于0，先计算当前节点周围及上一节点的最小值，再返回上一节点加上每移动一格的代价值 4.计算该邻近节点的坐标值，并计算该邻近节点到目标点的距离 5.将该邻近节点（节点的F值和坐标索引）加入队尾，然后加入该邻近节点重新构造堆 |
|
|
