A* 算法

翊阡

于 2025-02-26 11:36:25 发布

阅读量192

点赞数 2

文章标签：算法 matlab

本文链接：https://blog.csdn.net/qq_47721783/article/details/145852981

版权

启发式搜索是利用启发函数对搜索过程进行指导，从而实现高效的搜索，是一种智能搜索，典型的如A*算法。

增量搜索是对以前的搜索结果信息进行的再利用以提高搜索效率，从而大大减少搜索范围，节约搜索时间，典型如D*\LOA*\D*Lite

A*算法是一种静态路网中求解最短路径最有效的直接搜索方法，广泛应用于室内机器人路径搜索，游戏动画路径搜索等

路径优劣评价公式为fn=gn+hn.fn是从初始状态下经由状态N到目标状态的代价估计，gn是在状态空间中从初始状态到状态n的实际代价，hn是从状态n到目标状态的最佳路径的估计代价。

寻找邻近子节点

function childNodes = getChildNode1(field,closeList, parentNode )

[rows,cols] = size(field);
[row_parentNode,col_parentNode] = ind2sub([rows,cols],parentNode);
childNodes = [];
closeList = closeList(:,1);
 %第一个子节点
 childNode =[row_parentNode,col_parentNode+1];
 if ~(childNode(1) < 1 || childNode(1) > rows || ...
         childNode(2) < 1 || childNode(2) > cols)
     if field(childNode(1),childNode(2)) ~= 2
         childNode_LineIdx = sub2ind([rows,cols], childNode(1),childNode(2));
         if ~ismember(childNode_LineIdx,closeList)
             childNodes(end+1) = childNode_LineIdx;
         end
     end
 end

 %2
 childNode =[row_parentNode-1,col_parentNode+1];
 if ~(childNode(1) < 1 || childNode(1) > rows || ...
         childNode(2) < 1 || childNode(2) > cols)
     if field(childNode(1),childNode(2)) ~= 2
         childNode_LineIdx = sub2ind([rows,cols], childNode(1),childNode(2));
         if ~ismember(childNode_LineIdx,closeList)
             childNodes(end+1) = childNode_LineIdx;
         end
     end
 end
 %3
 childNode =[row_parentNode-1,col_parentNode];
 if ~(childNode(1) < 1 || childNode(1) > rows || ...
         childNode(2) < 1 || childNode(2) > cols)
     if field(childNode(1),childNode(2)) ~= 2
         childNode_LineIdx = sub2ind([rows,cols], childNode(1),childNode(2));
         if ~ismember(childNode_LineIdx,closeList)
             childNodes(end+1) = childNode_LineIdx;
         end
     end
 end
 %4
 childNode =[row_parentNode-1,col_parentNode-1];
 if ~(childNode(1) < 1 || childNode(1) > rows || ...
         childNode(2) < 1 || childNode(2) > cols)
     if field(childNode(1),childNode(2)) ~= 2
         childNode_LineIdx = sub2ind([rows,cols], childNode(1),childNode(2));
         if ~ismember(childNode_LineIdx,closeList)
             childNodes(end+1) = childNode_LineIdx;
         end
     end
 end
 %5
 childNode =[row_parentNode,col_parentNode-1];
 if ~(childNode(1) < 1 || childNode(1) > rows || ...
         childNode(2) < 1 || childNode(2) > cols)
     if field(childNode(1),childNode(2)) ~= 2
         childNode_LineIdx = sub2ind([rows,cols], childNode(1),childNode(2));
         if ~ismember(childNode_LineIdx,closeList)
             childNodes(end+1) = childNode_LineIdx;
         end
     end
 end
 %6
 childNode =[row_parentNode+1,col_parentNode-1];
 if ~(childNode(1) < 1 || childNode(1) > rows || ...
         childNode(2) < 1 || childNode(2) > cols)
     if field(childNode(1),childNode(2)) ~= 2
         childNode_LineIdx = sub2ind([rows,cols], childNode(1),childNode(2));
         if ~ismember(childNode_LineIdx,closeList)
             childNodes(end+1) = childNode_LineIdx;
         end
     end
 end
 %7
 childNode =[row_parentNode+1,col_parentNode];
 if ~(childNode(1) < 1 || childNode(1) > rows || ...
         childNode(2) < 1 || childNode(2) > cols)
     if field(childNode(1),childNode(2)) ~= 2
         childNode_LineIdx = sub2ind([rows,cols], childNode(1),childNode(2));
         if ~ismember(childNode_LineIdx,closeList)
             childNodes(end+1) = childNode_LineIdx;
         end
     end
 end
 %8
 childNode =[row_parentNode+1,col_parentNode+1];
 if ~(childNode(1) < 1 || childNode(1) > rows || ...
         childNode(2) < 1 || childNode(2) > cols)
     if field(childNode(1),childNode(2)) ~= 2
         childNode_LineIdx = sub2ind([rows,cols], childNode(1),childNode(2));
         if ~ismember(childNode_LineIdx,closeList)
             childNodes(end+1) = childNode_LineIdx;
         end
     end
 end




end

Astar算法

%基于栅格地图的机器人路径规划算法

clc
clear
close all

%% 构建栅格地图场景
%  栅格界面大小；行数和列数
rows = 10;
cols = 20;
 %定义栅格地图全域， 并初始化空白区域
 [field,cmap] = defColorMap(rows,cols);


%起始点和目标点
starPos = 2;
goalPos = rows*cols-2;
field(starPos) = 4;
field(goalPos) = 5;

%% 预处理
%初始化closeList
parentNode = starPos;
closeList = [starPos,0];
%初始化 openList
openList = struct;
childNodes = getChildNode1(field,closeList,parentNode);
for i = 1:length(childNodes)
    [row_starPos,col_startPos] = ind2sub([rows,cols],starPos);
    [row_goalPos,col_goalPos] = ind2sub([rows,cols],goalPos);
    [row,col ] = ind2sub([rows,cols],childNodes(i));

    %存入openList 结构体
    openList(i).node = childNodes(i);
    openList(i).g = norm([row_starPos,col_startPos]- [row,col]);
    openList(i).h = abs(row_goalPos-row) + abs(col_goalPos- col);
    openList(i).f = openList(i).g + openList(i).h;
end

%初始化 path 
for i= 1:rows*cols
    path{i,1} = i;
end

for i = 1:length(openList)
    node = openList(i).node;
    path{node,2} = [starPos,node];
end

%% 开始搜索
%从openlist开始搜索移动代价最小的节点
[~,idx_min] = min([openList.f]);
parentNode = openList(idx_min).node;

%进入循环
while true
    %找出父节点的8个子节点
    childNodes = getChildNode1(field,closeList,parentNode);

    %判断这些子节点是否在open list中，若在则比较更新；没在则追加到open list中
    for i = 1:length(childNodes)
        %需要判断的子节点

        childNode = childNodes(i);
        [in_flag,idx] = ismember(childNode,[openList.node]);
        %计算代价函数
        [row_parentNode,col_paretNode] = ind2sub([rows,cols],parentNode);
        [row_childNode,col_childNode] = ind2sub([rows,cols],childNode);
        [row_goalPos,col_goalPos] = ind2sub([rows,cols],goalPos);
        g = openList(idx_min).g + norm( [row_parentNode,col_paretNode] - ...
           [row_childNode,col_childNode] );
        h = abs(row_goalPos - row_childNode) + abs(col_goalPos - col_childNode);
        f= g + h;

        if in_flag   %若在，比较更新g和f
            if f< openList(idx).f
                openList(idx).g = g;
                openList(idx).h = h;
                openList(idx).f = f;
                path{childNode,2} = [path{parentNode,2},childNode];
            end
        else    %若不在，追加到openlist
            openList(end+1).node = childNode;
            openList(end).g = g;
            openList(end).h = h;
            openList(end).f = f;
            path{childNode,2} = [path{parentNode,2},childNode];
        end
    end
    %从oplist中移出移动代价最小的节点到closeList
    closeList(end+1,:) = [openList(idx_min).node,openList(idx_min).f];
    openList(idx_min) = [];
     
     
    %在openlist搜索移动代价最小的节点作为父节点
    [~,idx_min] = min([openList.f]);
    parentNode = openList(idx_min).node;

     % 判断是否搜索到终点
     if parentNode == goalPos
         closeList(end+1,:) = [openList(idx_min).node,openList(idx_min).f];
         break
     end
end

%% 画路径

%找出目标最优路径
path_target = path{goalPos,2};
field(path_target(2:end-1)) = 6;

%画栅格图
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;