环境感知与规划专题（一）——A*算法入门

前言

A算法作为路径规划算法中应用最广泛实用的算法，由于A算法的算法原理较为简单，网络上能够找到各种各样的学习资料，本文仅对其实现过程进行总结，同时，附上matlab仿真源码及结果图供参考学习。

算法实现过程

A*算法作为一种启发式的寻路算法被广泛应用于解决游戏中以及机器人、无人车、无人机的路径规划问题。该算法的步骤为：

1 .把起始节点添加到openList；

2 . 重复如下步骤：

a) 寻找openList当中F值最低的节点，将其称为当前节点；

b) 将该节点从openList中删除，并加入closeList当中；

c) 对于当前节点相邻的8个节点:
* 如果它不可通过或者已在closeList当中，忽略它，反之如下；
* 如果它不在openList当中，将其添加进去。把当前节点作为它的父节点。同时更新它的F,G和H值。
* 如果它已经在openList当中，通过判断沿当前节点到它的路径的G值是否更小，若更小，则将当前节点作为它的父节点，同时更新它的F和G值。否则，不做任何操作。

d) 当目标节点已添加到closeList时，路径已被找到；或者没有找到目标节点，此时openList已为空。这时候，路径不存在。以上两种情况结束循环。

3 .路径回归。从目标节点开始，沿着每一个节点的父节点移动至起始节点，形成的路径即为所求路径。

仿真代码

	clear,clc
	startPosition = [1 5];
	goalPosition = [8 8];
	findGoal = false;
	map = [0 0 0 0 0 0 0 0 0 0;
      0 0 0 1 1 1 0 1 0 0;
      0 0 0 0 0 0 1 0 1 0;
      0 0 0 0 0 1 0 0 1 0;
      0 0 0 0 1 0 0 1 0 0;
      0 0 0 0 0 0 1 0 1 1;
      0 0 1 0 1 0 0 0 0 0;
      0 1 0 0 0 1 0 0 0 0;
      1 0 0 0 0 0 1 0 0 0;
      0 0 0 0 0 0 0 0 0 0;];
	[mapRow, mapCol] = size(map);
	closeList = struct('row', 0, 'col', 0, 'g', 0, 'h', 0, 'fartherNodeRow', 0, 'fartherNodeCol', 0);
	closeListLength = 0;
	openList = struct('row', 0, 'col', 0, 'g', 0, 'h', 0, 'fartherNodeRow', 0, 'fartherNodeCol', 0);
	openListLength = 0;
	direction = [0, -1; -1, -1; -1, 0; -1, 1;