运动规划相关基础算法——基于搜索的路径规划算法之A*

1. A* 概述

A* 是一种启发式算法，是由Dijkstra发展而来的。一般基于grid(栅格)地图或者Voronoi(诺维图)进行机器人、无人车路径规划的基础算法。

2. BFS(广度优先搜索算法)

BFS维护的是一个队列的容器，遵循先进先出的原则。

3. Dijkstra算法

Dijkstra是运筹学中进行最短路径查找的经典算法。Dijktra算法维护的数据结构与BFS相同，都是队列。不同的是Dijkstra的数据结构类型是具有一定规则的优先级队列。
代价函数：f(n) = g(n);
Dijkstra搜索策略：扩展/访问累计代价最小的节点g(n)。

• g(n):从开始状态到节点n的累积代价的当前最佳代价

优点：在具有可行解的路径搜索空间，总能找到路径最短的最优解；
缺点：对目标点没有启发信息，属于盲目搜索，搜索效率低。

4. A* 算法的流程步骤

A* 算法本质上相当于Dijkstra算法加上启发式信息；
A* 算法维护的数据结构也是一个队列，遵循先进先出的原则。

4.1 代价函数

f(n) = g(n) + h(n)
g(n): 从起始点start到当前点n的代价值；
h(n): 从当前点n到目标点goal的估计代价值函数；在路径规划中h(n)函数一般选择曼哈顿距离或者欧式距离。

4.2 伪代码

input：map、start_point、goal_point
output:path

open_list[start_point];
close_list[];

while(true){
	if(open_list is empty) break;
	Remove the node “n” with the lowest f(n) from the open_list[];
	Add the ‘n’ to close_list[];
	Mark node “n” as expanded;
	if(n == goal_point) break;
	For m { //m是n未搜索过的相邻的节点
		if(g(m) == inf){
			g(m) = g(n) + distance(m, n);
			f(m) = g(m) + h(m);
		}
		
		if(g(m) > g(n) + distance(m, n)){
			g(m) = g(n) + distance(m, n);
			f(m) = g(m) + h(m);
		}
	}
	Find full open_list[];
}

Find Path from open_list;

4.3 回溯找路径

以上图为例，假设目标点为（9， 9），起始点为（4， 5）,在open_list[],中分别存放，当前节点的坐标，以及其父节点坐标，按图示的方法，从目标点回溯找到起始点，即找到一条路径。