SLAM从入门到精通（a*搜路算法）
source link: https://blog.csdn.net/feixiaoxing/article/details/133875544
Go to the source link to view the article. You can view the picture content, updated content and better typesetting reading experience. If the link is broken, please click the button below to view the snapshot at that time.
【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】
目前机器人常用的搜路算法主要有这么几种，迪杰斯特拉算法、a*算法、贪心算法。每一种算法都有自己的场景和优势，可以灵活选择。但一般来说，客户的场景不算很复杂的话，搜路算法越简单越好，只要能达到最终的目标即可。对于特别复杂的场景，建议也不要通过底层算法的变更来解决业务的问题，这反而是得不偿失的。所以说，这三种算法，如果没有特别原因的话，最好都实现一下，这样方便fae的同学现场部署和实施。搜路算法本身只是一个拓扑算法，它帮助我们分析了目的地本身是否可达，但是机器人能不能过去，这就两说了。
下面，我们就看下a*算法是怎么实现的。
1、a*算法的核心
a*算法的核心其实就是F=G+H。其中F是总代价，G是起始点到当前点的代价，H是当前点到目标点的代价。两者加在一起，就是每次选择新插入点的标准。
2、a*算法的流程
a*算法的伪代码流程一般是这样的，
1）将开始点设置为p；
2）p点插入到封闭集当中；
3）搜寻p的所有邻接点，如果邻接点没有在开放集或者封闭集之中，则计算该点的F值，设置该邻接点的parent为p，将临界点插入到开放集当中；
4）判断开放集是否为空，如果为空，则搜路失败，否则继续；
5）从开放集挑出F数值最小的点，作为寻路的下一步起始点；
6）判断该点是否是终点，如果是，结束查找，否则继续；
7）跳转到3继续执行。
3、a*算法的注意事项
整个a*算法还是不算太复杂的。需要注意的地方只有一处，那就是3）中如果发现邻接点已经在开放集中，那需要重新计算它的G值。一旦发现当前G值更小，则需要同步更新parent、G值和F值。
4、测试代码
算法的整个过程参考了一本ROS参考书上的python代码。大家可以实际下载下来查看一下效果。代码是用python编写，需要安装matplotlib库。
代码中motion_model表示了当前点周围8个点的行驶代价；node_verify则是判断当前点是否在障碍物上；astar_plan是所有算法真正的入口；而map_generation则构建了一个基本的搜寻场景。
5、运行效果图
运行效果如下所示，供大家参考。直接用python3 astar.py运行即可，
Recommend
About Joyk
Aggregate valuable and interesting links.
Joyk means Joy of geeK