文章目录

• HW2 — A* and JPS
• • 源代码在RVIZ中出现的异常
  • 项目解析
  • • A* 和 Dijkstra 算法步骤
    • • 数据结构
      • 算法流程
    • 不同启发式函数对A*运行效率的影响
    • • 控制变量
      • Euclidean
      • Manhattan
      • Diagonal
    • Tie Breaker
    • • 性能对比
    • JPS规划算法
    • • Look Ahead Rule
      • 算法步骤
      • JPS VS A*

HW2 – A* and JPS

深蓝学院移动机器人 第二章 学习笔记和报告
Github源码链接

源代码在RVIZ中出现的异常

源码中坐标 id 应改为 "world"，原来的id\world会导致RVIZ中坐标id匹配不上从而导致通信失败，点云不呈现。

项目解析

A* 和 Dijkstra 算法步骤

数据结构

用 strcut GridNode 存放地图节点，节点 id = 0(open list), 1(close list)

用std::multimap来创建 open list

算法流程

While (open list非空)

• 弹出open list 中 f(n)最小的节点作为当前点

• 判断当前点是否是终点

  • True：结束算法
  • FALSE：继续

• 扩展当前点的邻居节点至neighborPterSets，将邻居节点与当前点的距离更新至便代价数组edgeCostSets

• 遍历neighborPtrSets中的节点m，判断他们的$g(m)$是否需要更新，是否可以加入open list

  • 已更新至 close list (id = 1)：放弃扩展

  • no expand(id = -1 && $g(n)=inf$)

    • 更新$g(m)=g(n)+Cnm$并放入 open list ，同时更新 id = 0
    • 更新$f(n)=g(n)+h(n)$

  • expanded，即已在open list(id = 0)

    • 若g(m)>g(n)+Cnmg(m)>g(n)+C_{nm}g(m)>g(n)+Cnm​，则g(m)=g(n)+Cnmg(m)=g(n)+C_{nm}g(m)=g(n)+Cnm​，并更新$f(n)=g(n)+h(n)$

Finally 路径点回溯

不同启发式函数对A*运行效率的影响

控制变量

1、控制相同的地图，所以要先实现点云的保存和读取

参考：

1、如何读取和保存点云

2、设置点云保存路径

void Savepcd()
{// 保存.pcdpcl::PCDWriter writer;if (!cloudMap.empty()){string filename = "/home/sudaxia/ROSTEMPLATE/catkin_ws/src/grid_path_searcher/src/Binary.pcd";writer.write(filename, cloudMap, true);cout << "Point cloud saved：" << cloudMap.points.size() << endl;}else{PCL_ERROR("\a->保存点云为空！\n");}
}void loadpcd()
{// 读取.pcdpcl::PCDReader pcdReader;string file_name = "/home/sudaxia/ROSTEMPLATE/catkin_ws/src/grid_path_searcher/src/Binary.pcd";pcdReader.read(file_name, cloudMap);cout << "Loaded point cloud：" << cloudMap.points.size() << endl;
}

2、为了在控制相同终点的同时继承原来的代码，在./src/rviz_plugins/src/goal_tool.cpp中可以看到3D Nav goal的目标点确定是通过话题通信进行发布的，所以我们需要写一个发布节点在相同的主题下发布固定的目标点

目前只是手动在地图上点一个固定点，发布暂时不知道怎么弄，等后续C++和ROS功力加深之后再继续搞这个问题 – 2023.2.28

Euclidean

Manhattan

Diagonal

Tie Breaker

很多时候在搜索时地图会存在对称性，二者皆可，所以其实没有必要对它们二者都进行拓展，选择一个即可

有一流行方法如下：
h=h×(1.0+p)p<minimum cost of one stepexpected maximum path costh = h \times (1.0+p) \\ p < \frac{\text{minimum cost of one step}}{\text{expected maximum path cost}} h=h×(1.0+p)p<expected maximum path costminimum cost of one step​
这套算法满足的前提是，实际工况下h≈(0.3∼0.5)h∗h \approx (0.3 \sim 0.5)h^*h≈(0.3∼0.5)h∗，所以$h=h\times (1.0+p)<h\ast$依旧满足

性能对比

Tie breaker

Normal

虽然 Tie breaker 可以有效提升前端路径规划的效率，但是由于最终我们还是需要生成机器人可以运行的轨迹，所以基于后期轨迹优化的需要，选用合适的Tie breaker是十分重要的。

JPS规划算法

JPS是为了彻底消灭系统中存在对称路径的这样一种方法。JPS与A* 唯一的区别就在于扩展节点的方式不一样，JPS采用了更加高效的方法来搜索需要加进OpenList的点，然后在OpenList中弹出最小值。JPS完整地保留了A*的框架。

Look Ahead Rule

算法步骤

While (open list非空)

• 弹出open list 中 f(n)最小的节点作为当前点

• 判断当前点是否是终点

  • True：结束算法
  • FALSE：继续

• 按照 look ahead rules 扩展当前点的下一个跳跃点至neighborPterSets，将跳跃点与当前点的距离更新至便代价数组edgeCostSets

• 遍历neighborPtrSets中的节点m，判断他们的$g(m)$是否需要更新，是否可以加入open list

  • 已更新至 close list (id = 1)：放弃扩展

  • no expand(id != 1)

    • 更新$g(m)=g(n)+Cnm$并放入 open list ，同时更新 id = 0

    • 更新$f(n)=g(n)+h(n)$

    • expanded，即已在open list(id = 0)

      • 若g(m)>g(n)+Cnmg(m)>g(n)+C_{nm}g(m)>g(n)+Cnm​，则g(m)=g(n)+Cnmg(m)=g(n)+C_{nm}g(m)=g(n)+Cnm​，并更新$f(n)=g(n)+h(n)$

Finally 路径点回溯

JPS VS A*