A*算法结合了贪心算法(深度优先)和Dijkstra算法(广 度优先),是一种启发式搜索算法
贪心算法存在可能搜索不到目标结点的情况,非最优路径
Dijkstra算法是最优路径
深度优先:以当前搜索到的最小路径作为目标路径继续往下搜索
广度优先:对搜索空间内的每一个点进行搜索
路径优劣评价公式为: f(n)=g(n)+h(n)。
f(n) 是从初始状态经由状态n到目标状态的代价估计,
g(n) 是在状态空间中从初始状态到状态n的实际代价,
h(n) 是从状态n到目标状态的最佳路径的估计代价。
使用了两个状态表,分别称为openList表和closeList表。(类似于Dijkstra算法中的U和S)
openList表由待考察的节点组成,
closeList表由已经考察 过的节点组成。
预处理
1.地图栅格化
2.定义两个列表集合
3.更新集合直至找到目标节点
从openList中选择 f值最小的 ( 方格 ) 节点I,从 openList 里取出,放到 closeList 中,并把它作为新的父节点 。检查所有与它相邻的子节点,忽略障碍物不可走节点、 忽略已经存在于closeList的节点;如果方格不在openList 中,则把它们加入到 openList中。
4.完成路径搜索,结束算法
%% 进入循环
flag = 1;
while flag
% 找出父节点的忽略closeList的子节点
child_nodes = child_nodes_cal(parent_node, m, n, obs, closeList);
% 判断这些子节点是否在openList中,若在,则比较更新;没在则追加到openList中
for i = 1:size(child_nodes,1)
child_node = child_nodes(i,:);
[in_flag,openList_idx] = ismember(child_node, openList, 'rows');
g = openList_cost(min_idx, 1) + norm(parent_node - child_node);
h = abs(child_node(1) - target_node(1)) + abs(child_node(2) - target_node(2));
f = g+h;
if in_flag % 若在,比较更新g和f
if g < openList_cost(openList_idx,1)
openList_cost(openList_idx, 1) = g;
openList_cost(openList_idx, 3) = f;
openList_path{openList_idx,2} = [openList_path{min_idx,2}; child_node];
end
else % 若不在,追加到openList
openList(end+1,:) = child_node;
openList_cost(end+1, :) = [g, h, f];
openList_path{end+1, 1} = child_node;
openList_path{end, 2} = [openList_path{min_idx,2}; child_node];
end
end