学习笔记：A*算法

为争。

已于 2022-10-26 21:22:49 修改

阅读量1.3k

点赞数 1

分类专栏：路径规划与轨迹跟踪文章标签：学习

于 2022-10-25 10:51:01 首次发布

本文链接：https://blog.csdn.net/ohhjack/article/details/127507919

版权

路径规划与轨迹跟踪专栏收录该内容

9 篇文章 12 订阅

订阅专栏

一、算法简介

1、A*算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜多问题的有效算法。

2、广泛应用于室内机器人路径搜索、游戏动画路径搜索等。

二、算法思想

1、A*算法结合了贪心算法（深度优先）和Dijkstra算法（广度优先），是一种启发式搜索算法。

2、路径优劣评价公式：f(n)=g(n)+h(n)。

3、f(n)是从初始状态经由状态n到目标状态的代价估计

g(n)是在状态空间中从初始状态到最佳状态n的实际代价

h(n)是从状态n到目标状态的最佳路径的估计代价

4、使用两个状态表，分别称为openList和closeList表。openList表由待考察的节点组成，closeList表由已经考察过的节点组成。

假如A点定为目标节点，他周围共有8个节点，以右边节点为例，g（n）为A到达右边节点的代价，即为1，h（n）是右边的节点到目标节点的估计代价，估计因为不知道哪些地方会有障碍物，就根据当前节点到目标节点的横向加纵向之和，就作为估计节点，即h（n）为3，f(n)=g(n)+h(n)=1+3=4

三、算法初始化

1、将地图栅格化，把每一个正方形格子的中央称为节点，确定栅格属性，即每一个格子有两种状态：可走和不可走(体现障碍物)。

2、定义两个列表集合：openList和closeList。openList表由待考察的节点组成， closeList表由已经考察过的节点组成。类似Dijkstra算法的U集合和S集合。

3、确定起始节点和目标节点。

1、初始时，定义A为父节点，节点A离自身距离为0，路径完全确定，移入closeList中。

2、父节点A周围共有8个节点，定义为子节点。将子节点放入openList中，成为待考察对象。

3、若某个节点既未在openList，也没在closeList中，则表明还未搜索到该节点。

4、路径优劣判断依据是移动代价，单步移动代价采取Manhattan计算方式，即把横向和纵向移动一个节点的代价定义为10。斜向移动代价参考等腰三角形计算斜边的方式，距离为14。

四、算法过程

以I点为例，从A到I为斜角，g(n)=14，I到T的横纵相加h(n)=30+10=40，此时f(n)=54。以此类推，可以计算出周围所有八个节点的f(n)，最小的为F点。

所以，现在openList = {B,C,D,E,G,H,l}, closeList = {A,F}。

1、从openList中选择f值最小的（方格)节点l，从openList里取出，放到closeList 中，并把它作为新的父节点。

2、检查所有与它相邻的子节点，忽略障碍物不可走节点、忽略已经存在于closeList的节点；如果方格不在openList中，则把它们加入到openList中。

3、如果某个相邻的节点已经在openList 中，则检查这条路径是否更优，也就是说经由当前节点(我们选中的节点)到达那个节点是否具有更小的G值。如果没有，不做任何操作。例如第一二部，从A走向F后，F再往下走，可以走的为CDHI，此时需要检测F点到CDHI的距离是否小于A点到CDHI的距离，如果小于可以走，如果大于不可以走。

4、依次类推，不断重复。一旦搜索到目标节点T，完成路径搜索，结束算法。

% A*算法
% 作者：Ally
% 日期：2021/1/9
clc
clear
close all

%% 画地图

% 栅格地图的行数、列数定义
m = 5;
n = 7;
start_node = [2, 3];
target_node = [6, 3];
obs = [4,2; 4,3; 4,4];

for i = 1:m
    plot([0,n], [i, i], 'k');
    hold on
end
    
for j = 1:n
     plot([j, j], [0, m], 'k');
end

axis equal
xlim([0, n]);
ylim([0, m]);   

% 绘制障碍物、起止点颜色块
fill([start_node(1)-1, start_node(1), start_node(1), start_node(1)-1],...
    [start_node(2)-1, start_node(2)-1 , start_node(2), start_node(2)], 'g');

fill([target_node(1)-1, target_node(1), target_node(1), target_node(1)-1],...
    [target_node(2)-1, target_node(2)-1 , target_node(2), target_node(2)], 'r');

for i = 1:size(obs,1)
    temp = obs(i,:);
    fill([temp(1)-1, temp(1), temp(1), temp(1)-1],...
        [temp(2)-1, temp(2)-1 , temp(2), temp(2)], 'b');
end

%% 预处理

% 初始化closeList
closeList = start_node;
closeList_path = {start_node,start_node};
closeList_cost = 0;
child_nodes = child_nodes_cal(start_node,  m, n, obs, closeList);  

% 初始化openList
openList = child_nodes;
for i = 1:size(openList,1)
    openList_path{i,1} = openList(i,:);
    openList_path{i,2} = [start_node;openList(i,:)];
end

for i = 1:size(openList, 1)
    g = norm(start_node - openList(i,1:2));
    h = abs(target_node(1) - openList(i,1)) + abs(target_node(2) - openList(i,2));
    f = g + h;
    openList_cost(i,:) = [g, h, f];
end

%% 开始搜索
% 从openList开始搜索移动代价最小的节点
[~, min_idx] = min(openList_cost(:,3));
parent_node = openList(min_idx,:);


%% 进入循环
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
   
    
    % 从openList移除移动代价最小的节点到 closeList
    closeList(end+1,: ) =  openList(min_idx,:);
    closeList_cost(end+1,1) =   openList_cost(min_idx,3);
    closeList_path(end+1,:) = openList_path(min_idx,:);
    openList(min_idx,:) = [];
    openList_cost(min_idx,:) = [];
    openList_path(min_idx,:) = [];
 
    % 重新搜索：从openList搜索移动代价最小的节点
    [~, min_idx] = min(openList_cost(:,3));
    parent_node = openList(min_idx,:);
    
    % 判断是否搜索到终点
    if parent_node == target_node
        closeList(end+1,: ) =  openList(min_idx,:);
        closeList_cost(end+1,1) =   openList_cost(min_idx,1);
        closeList_path(end+1,:) = openList_path(min_idx,:);
        flag = 0;
    end
end
    

%% 画路径
path_opt = closeList_path{end,2};
path_opt(:,1) = path_opt(:,1)-0.5;
path_opt(:,2) = path_opt(:,2)-0.5;
scatter(path_opt(:,1), path_opt(:,2), 'k');
plot(path_opt(:,1), path_opt(:,2), 'k');



function child_nodes = child_nodes_cal(parent_node, m, n, obs, closeList)

child_nodes = [];
field = [1,1; n,1; n,m; 1,m];

% 第1个子节点
child_node = [parent_node(1)-1, parent_node(2)+1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

% 第2个子节点
child_node = [parent_node(1), parent_node(2)+1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

% 第3个子节点
child_node = [parent_node(1)+1, parent_node(2)+1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

% 第4个子节点
child_node = [parent_node(1)-1, parent_node(2)];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

% 第5个子节点
child_node = [parent_node(1)+1, parent_node(2)];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

% 第6个子节点
child_node = [parent_node(1)-1, parent_node(2)-1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

% 第7个子节点
child_node = [parent_node(1), parent_node(2)-1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

% 第8个子节点
child_node = [parent_node(1)+1, parent_node(2)-1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

%% 排除已经存在于closeList的节点
delete_idx = [];
for i = 1:size(child_nodes, 1)
    if ismember(child_nodes(i,:), closeList , 'rows')
        delete_idx(end+1,:) = i;
    end
end
child_nodes(delete_idx, :) = [];

学习自B站：小黎的Ally

视频链接：路径规划与轨迹跟踪系列算法学习_第4讲_A*算法_哔哩哔哩_bilibili