运动规划之A*搜索算法-1 介绍 （A* Search）

0、 Planning problem

0.1 Given

• Map
• Starting location
• Goal location
• Cost

0.2 Goal

• Find minimum cost path

1、 A* - Artificial Intelligence for Robotics

how a A* search works in a grid?

1.1 heuristic function

h函数表示从任意点（x,y）到目标G的最小估计成本。

g function

h函数表示从起始点S移动到(x,y)的最小估计成本。

f-value

f-value表示从起始点S到目标G的成本，f = g + h;
f的最小值即我们要的最短规划路径。

open list

• 首先把起始位置点加入到一个称为“open List”的列表，

• 在寻路的过程中，不断把待测试的点加入open List中，同时，为了效率考虑，通常这个列表是个已经排序的列表

• open list :

起始点[0,0]，其(g,f)值为（0，9）
点[4,1]，其(g,f)值为（5，9）	点[4,2]，其(g,f)值为（6，9）
	点[3,2]，其(g,f)值为（7，11）	点[4,3]，其(g,f)值为（7，9）
	点[3,3]，其(g,f)值为（8，11）	点[4,3]，其(g,f)值为（8，9）

closed list

• 计算过程中，将f最小的节点不断的加入close list中
• close list 即为最短路径

2、 A* Pseudocode

To implement A* search, you will implement the following psuedocode, step by step in a sequence of exercises:

Search( grid, initial_point, goal_point ) :

1. Initialize an empty list of open nodes. // initialize open list

2. Initialize a starting node with the following:

   1.  x and y values given by initial_point.
   2.  g = 0, where g is the cost for each move. 
   3.  h given by the heuristic function (a function of the current coordinates and the goal).
   

3. Add the new node to the list of open nodes. //update open list

4. while the list of open nodes is nonempty:

    1. Sort the open list by f-value    //f = g + h, 遍历open list ,找到f值最小的node 
    2. Pop the optimal cell (called the current cell). 	//将f最小的node 更新到cell中，即close list
    3. Mark the cell's coordinates in the grid as part of the path.
    4. if the current cell is the goal cell:
     		return the grid.			//返回A*算法找到的最小path
      	else, expand the search to the current node's neighbors. This includes the following steps:
    		- Check each neighbor cell in the grid to ensure that the cell is empty: it hasn't been closed and is not an obstacle.
    		- If the cell is empty, compute the cost (g value) and the heuristic, and add to the list of open nodes.
    		- Mark the cell as closed.
   

5. If you exit the while loop because the list of open nodes is empty, you have run out of new nodes to explore and haven’t found a path.

reference：A* Pathfinding for Beginners