移动机器人运动规划 --- 基于图搜索的A*算法_基于搜索的运动规划-CSDN博客

移动机器人运动规划 --- 基于图搜索的A*算法

A*算法
- A*算法伪代码
- A* 算法步骤示例
- A*算法分析
- - - 启发函数设计
  - A*应用的更好方式

A*算法

A算法与Dijkstra算法的框架是完全一样的，**A算法就是有启发性的Dijkstra算法**

代价函数：g(n) 表示的是从开始节点到当前n节点的代价累加

启发函数：h(n) 表示当前节点到目标节点估计所花的代价

优先级队列：维护的是代价函数+启发函数的节点从小到大排序 f(n)=g(n)+h(n)

每次弹出的节点就是最小的f(n)值的节点。

A*算法伪代码

维护一个优先级队列，存储所有被扩展的节点，且节点按f()值的大小自动按从小到大排列。

-所以节点的启发值h(n)是为被定义的，是不知道的，到具体的节点再计算

-优先级队列首先为空，以起始节点Xs进行初始化

-起始节点g(Xs)=0，并且初始化其它节点的代价为无穷大

-循环：
	1、如果队列是空的，返回false，跳出循环
	2、**弹出优先级队列中f(n)最小的节点n** [唯一与djikstra不同的地方]
	3、标记节点n为被扩展节点
	4、如果节点n为目标节点，返回true,跳出循环
	5、找到n节点周围可以扩展的所以节点（没被扩展过）m
		6、进行判断 如果g(m)为无穷大（说明其它节点也没发现过m）,
			7、则计算 真正的g(m)=g(n)+Cnm，然后将m节点加入到优先级队列中
		8、进行判断 如果g(m)不为无穷大，有值了(说明其它节点发现过m，m已经在优先级队列中)
			9、再次进行判断 如果之前发现m时计算的g(m)比g(n)+Cnm大的话
				10、更新g(m)=g(n)+Cnm。
	11、重复循环至步骤1

-结束循环

A* 算法步骤示例

下面是一个A算法的演示图，每个边有个预先设置的代价g，每个节点有提前估计好的启发f

以这个图将A 算法运行的步骤进行一个示例：

1、首先初始化队列，将起始节点放入优先级队列中
在这里插入图片描述
2、弹出起始节点
可扩展的节点仅有a节点，计算f(a)=g(a)+h(a)=1+5=6

3、将扩展的节点放入优先级队列

4、弹出f最小节点，扩展周围节点
弹出a节点，a节点周围可以扩展的节点为 b\d\e 。并且根据g与h值计算f值
在这里插入图片描述
5、根据f值大小，压入队列中
d节点的f(d)=6最小，它在最下方。

6、弹出f值最小节点，扩展周围节点
弹出d节点，可扩展节点为G节点，计算f(G)=6

7、将最新扩展的节点加入优先级队列中，并进行排序
G最小，排到最底下
在这里插入图片描述
8、弹出f值最小节点
弹出的节点是目标节点G，算法结束