学习笔记：A*算法matlab实现

石磊 · 发表于 2024-7-15 18:49

A*算法是一种很常用很经典的路径搜索算法。与Dijkstra算法对比，Dijkstra算法计算一个点到其他点的最短路径，而A*算法存眷点到点的最短路径。Dijkstra算法的广度优先算法，A*算法是启发式算法，类似于深度优先算法。关于A*算法其他介绍本文不再赘述，重点存眷A*算法的伪代码和matlab实现。
A*算法伪代码：

* 初始化open_set和close_set；
* 将起点插手open_set中，并设置cost为0（优先级最高）；
* 如果open_set不为空，则从open_set中拔取cost最小的节点n：
* 如果节点n为终点，则：
* 从终点开始逐步追踪parent节点，一直达到起点；
* 返回找到的成果路径，算法结束；
* 如果节点n不是终点，则：
* 将节点n从open_set中删除，并插手close_set中；
* 遍历节点n所有的邻近节点：
* 如果邻近节点m在close_set中，则：
* 跳过，拔取下一个邻近节点
* 如果邻近节点m也不在open_set中，则：
* 设置节点m的parent为节点n
* 计算节点m的cost
* 将节点m插手open_set中
* 如果邻近节点m在open_set中，则：
* 若节点m的cost小于本来的cost：
* 更新节点m的cost
* 设置节点m的parent为节点n

复制代码

A*算法matlab实现：

function [path] = AStar(map, p_start, p_goal)
[~, mapWidth] = size(map);
p_start_index = getIndex(p_start, mapWidth);
p_goal_index = getIndex(p_goal, mapWidth);
openList = [p_start_index; 0]; % 为了拔取cost最小的节点，open_set中也存放了节点的代价
closeList = [];
% costTable存放了节点id、到起点的欧式距离、到终点的曼哈顿距离、节点的估计代价
costTable = [p_start_index; 0; Mdistance(p_start, p_goal); Mdistance(p_start, p_goal)];
edges = int32.empty(0, 2);
while ~isempty(openList)
[~, min_cost_index] = min(openList(2, :));
p_current_index = openList(1, min_cost_index); % 拔取cost最小的节点作为当前节点
openList(:, min_cost_index) = [];
closeList = [closeList, p_current_index];
p_current = index2node(p_current_index, mapWidth);
if (isequal(p_current, p_goal)) % 如果当前节点就是终点，回溯得到起点到终点之间的路径
path = findPath(edges, p_start_index, p_goal_index, mapWidth);
return;
end
% 遍历当前节点的邻居节点
for i = -1 : 1
for j = -1 : 1
p_neig = p_current + int32([i, j]);
p_neig_index = getIndex(p_neig, mapWidth);
% 如果邻居节点不成行或者已经在close_set中，跳过
if ~isNodeFree(map, p_neig) || ~isempty(find(closeList == p_neig_index, 1))
continue;
end
% 计算邻居节点的估计代价
[EdOfPNeig, MdOfPNeig, costOfPNeig] = cost(p_neig, p_current, p_current_index, costTable, p_goal);
if isempty(find(openList(1, :) == p_neig_index, 1))
openList = [openList, [p_neig_index; costOfPNeig]];
costTable = [costTable, [p_neig_index; EdOfPNeig; MdOfPNeig; costOfPNeig]];
edges = [edges; [p_neig_index, p_current_index]];
else
[~, column] = find(costTable(1, :) == p_neig_index);
if costOfPNeig < costTable(4, column)
% 若邻居节点已经在open_set中且新cost小于原cost，则更新邻居节点的估计代价和父节点
[row, ~] = find(edges(:, 1) == p_neig_index);
edges(row, :) = [];
edges = [edges; [p_neig_index, p_current_index]];
costTable(:, column) = [];
costTable = [costTable, [p_neig_index; EdOfPNeig; MdOfPNeig; costOfPNeig]];
[~, col] = find(openList(1, :) == p_neig_index);
openList(:, col) = [];
openList = [openList, [p_neig_index; costOfPNeig]];
end
end
end
end
end
end

复制代码

以上就是A*算法的matlab实现，下面对此中的一些自定义函数进行补充：

% 获取节点在地图中的id
function [index] = getIndex(p, mapWidth)
index = p(2) + mapWidth * (p(1) - 1);
index = int32(index);
end
% 将节点id转换成节点坐标
function coordinate = index2node(index, mapWidth)
y = int32(mod(index, mapWidth));
if y == 0
y = mapWidth;
end
x = int32((index-y)/mapWidth + 1);
coordinate = int32([x, y]);
end
% 计算两个节点之间的欧式距离
function [Ed] = Edistance(p1, p2)
p1_p2 = int32(p1) - int32(p2);
if p1_p2(1) == 0 && p1_p2(2) == 0
Ed = 0;
elseif p1_p2(1) == 0 || p1_p2(2) == 0
Ed = 10;
else
Ed = 14;
end
Ed = int32(Ed);
end
% 计算两个节点之间的曼哈顿距离
function [Md] = Mdistance(p1,p2)
p1_p2 = int32(p1) - int32(p2);
Md = abs(p1_p2(1)) + abs(p1_p2(2));
Md = Md * 10;
Md = int32(Md);
end
% 判断节点是否为可行节点
function [isFree] = isNodeFree(map, p)
[mapLength, mapWidth] = size(map);
if p(1) < 1 || p(2) < 1 || p(1) > mapLength || p(2) > mapWidth
isFree = 0;
else
isFree = ~map(p(1), p(2));
end
end
% 计算邻居节点到起点的欧式距离、到终点的曼哈顿距离、节点的估计代价
function [di2start, Md2goal, co] = cost(p_neig, p_current, p_current_index, costTable, p_goal)
[~, column] = find(costTable(1, :) == p_current_index);
di2start = int32(costTable(2, column)) + Edistance(p_neig, p_current);
Md2goal = Mdistance(p_neig, p_goal);
co = int32(di2start + Md2goal);
end
% 通过回溯得到该算法实现找到的最优路径
function [path] = findPath(edges, p_start_index, p_goal_index, mapWidth)
[row, ~] = find(edges(:, 1) == p_goal_index);
index_son = edges(row, 1);
index_father = edges(row, 2);
path = index2node(index_son, mapWidth);
while index_father ~= p_start_index
[row, ~] = find(edges(:, 1) == index_father);
index_son = index_father;
path = [path; index2node(index_son, mapWidth)];
index_father = edges(row, 2);
end
path = [path; index2node(p_start_index, mapWidth)];
path = flipud(path);
end

复制代码

至此，我们完成了A*算法的matlab实现。通过读取png图像作为二维地图，并对障碍物进行膨胀操作，执行A*算法得到如下的效果：

通过上图可以看出，A*算法能够搜索出一条可行路径，但是存在一些问题，例如：

存在多余的拐点；
搜索出来的路径不够平滑；
没有考虑实际的运动学约束。

针对这些问题，可以通过去除多余拐点，曲线插值/拟合，考虑机器人实际的运动学模型等方式来解决。

上图为针对上述问题进行优化后的最终效果，可以看到优化后的路径对比于原始路径有不少的提升，但仍存在可以改良的处所，后续将运用更多方式针对路径优化进行探究。

		自动登录	找回密码
密码			立即注册

学习笔记：A*算法matlab实现

本帖子中包含更多资源