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; 0, 1; 1, 1; 1, 0; 1, -1;];
openList(1).row = startPosition(1);
openList(1).col = startPosition(2);
openListLength = openListLength + 1;
count = 0;
%找到目标或者开启列表已空时退出循环
while (openListLength > 0 || findGoal == true)
count = count + 1;
%比较f值，更新当前格
f = openList(1).g + openList(1).h;
nodePosition = 1;
for i = 1:openListLength
if f > openList(i).g + openList(i).h
f = openList(i).g + openList(i).h;
nodePosition = i;
end
end
%将当前格加入关闭列表，并去掉开启列表中对应的节点
closeListLength = closeListLength + 1;
closeList(closeListLength) = openList(nodePosition);
openList(nodePosition) = [];
openListLength = openListLength - 1;
% openListLength = 0;
if closeList(closeListLength).row == goalPosition(1) && closeList(closeListLength).col == goalPosition(2)
findGoal = true;
break;
end
%检查相邻格并更新g、h值以及对应的父节点
for i = 1: 8
newPosition = [closeList(closeListLength).row, closeList(closeListLength).col] + direction(i, :);
%碰撞检测以及相邻点有效性检测
if all(newPosition > 0) && newPosition(1) <= mapRow && newPosition(2) <= mapCol && map(newPosition(1), newPosition(2)) ~= 1
if i == 2
if map(newPosition(1) + 1, newPosition(2)) == 1 || map(newPosition(1), newPosition(2) + 1) == 1
continue;
end
end
if i == 4
if map(newPosition(1), newPosition(2) - 1) == 1 || map(newPosition(1) + 1, newPosition(2)) == 1
continue;
end
end
if i == 6
if map(newPosition(1), newPosition(2) - 1) == 1 || map(newPosition(1) - 1, newPosition(2)) == 1
continue;
end
end
if i == 8
if map(newPosition(1) - 1, newPosition(2)) == 1 || map(newPosition(1), newPosition(2) + 1) == 1
continue;
end
end
flagCloseList = false;
%关闭列表检测，若已在关闭列表则忽略
for j = 1 : closeListLength
if closeList(j).row == newPosition(1) && closeList(j).col == newPosition(2)
flagCloseList = true;
break;
end
end
if flagCloseList
continue;
end
%若通过检测且不在开启列表中，则将其加入开启列表，并更新父节点
flagOpenList = false;
openListPosition = 0;
for j = 1 : openListLength
if openList(j).row == newPosition(1) && openList(j).col == newPosition(2)
flagOpenList = true;
openListPosition = j;
break;
end
end
if flagOpenList == false
openListLength = openListLength + 1;
openList(openListLength).row = newPosition(1);
openList(openListLength).col = newPosition(2);
if i == 2 || i == 4 || i == 6 || i == 8
openList(openListLength).g = closeList(closeListLength).g + 1.4;
else
openList(openListLength).g = closeList(closeListLength).g + 1;
end
openList(openListLength).h = abs(goalPosition(1) - openList(openListLength).row) + abs(goalPosition(2) - openList(openListLength).col);
openList(openListLength).fartherNodeRow = closeList(closeListLength).row;
openList(openListLength).fartherNodeCol = closeList(closeListLength).col;
else
%若通过检测且已在开启列表中，根据相邻格在路径上的新G值判断是否需要更新相邻格的父节点并重新计算G与H值
if openList(openListPosition).g > (closeList(closeListLength).g + 1)
openList(openListPosition).g = closeList(closeListLength).g + 1;
openList(openListPosition).fartherNodeRow = closeList(closeListLength).row;
openList(openListPosition).fartherNodeCol = closeList(closeListLength).col;
end
end
end
end
end
%起点终点着色
map(startPosition(1), startPosition(2)) = 128;
map(goalPosition(1), goalPosition(2)) = 70;
%路径回归
pathList = struct('row', 0, 'col', 0, 'g', 0, 'h', 0, 'fartherNodeRow', 0, 'fartherNodeCol', 0);
pathListLength = 0;
pathPosition = 0;
i = closeListLength;
while pathPosition ~= 1
for j = 1 : closeListLength
if(closeList(i).fartherNodeRow == closeList(j).row && closeList(i).fartherNodeCol == closeList(j).col)
pathListLength = pathListLength + 1;
pathList(pathListLength) = closeList(j);
pathPosition = j;
i = j;
break;
end
end
end
pathListLength = pathListLength + 1;
for i = 1 : pathListLength - 1
pathList(pathListLength - i + 1) = pathList(pathListLength - i);
end
pathList(1) = closeList(closeListLength);
%地图着色
for i = 1 : mapRow
for j = 1 : mapCol
if map(i,j) == 1
map(i,j) = 250;
end
if map(i,j) == 0
map(i,j) = 0;
end
end
end
%关闭列表着色
for i = 2:closeListLength - 1
map(closeList(i).row, closeList(i).col) = 80;
imagesc(map);
pause(0.5);
end
%最终路径着色
for i = 1 : pathListLength
map(pathList(pathListLength - i + 1).row, pathList(pathListLength - i + 1).col) = 160;
imagesc(map);
pause(0.5);
end
仿真结果
如图1所示，绿色方格为起始节点，蓝色方格为目标节点，黄色方格为障碍物。
如图2所示，淡蓝色方格为算法迭代过程中，所有被添加至closeList中的节点，绿色为算法最终生成的最优路径。
作者简介： 一个被Coding耽误的无人机算法工程师，控制、导航略懂一二，热衷技术，喜欢乒乓、音乐、电影，欢迎交流。 知乎：@遥远的乌托邦 GitHub: https://github.com/DistantUtopia 微信公众号：@遥远的乌托邦
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明环境感知与规划专题（一）——A*算法入门！
