A*（matlab）

最新推荐文章于 2023-10-27 17:38:59 发布

Ruo_su.

最新推荐文章于 2023-10-27 17:38:59 发布

阅读量192

点赞数

文章标签： matlab 开发语言算法

本文链接：https://blog.csdn.net/qq_44752711/article/details/128067113

版权

clear;clc;close all;
figure(1);
ImpRgb=imread("map.png");
Imp=rgb2gray(ImpRgb);  %白色为255，黑色为0
imshow(Imp);
hold on;
rows=size(Imp,1);  %y
cols=size(Imp,2);  %x
%plot(220, 20,'r*');
%Imp(220,20);

%空闲值设为1  障碍物设为2  起始点和终点分别为 3 4
startpose=[10,1];
goalpose=[460,620];
plot(startpose(1,2), startpose(1,1),'ro','MarkerSize',5,'MarkerFaceColor','r');
plot(goalpose(1,2),goalpose(1,1),'go','MarkerSize',5,'MarkerFaceColor','g');
hold on;

%startInd = (startpose(1) - 1)*cols + startpose(2);
startInd = sub2index(startpose,cols);
index2sub(startInd, cols)
goalInd = sub2index(goalpose,cols);
%goalInd =  (goalpose(1) - 1)*cols + goalpose(2);


%初始化
parentNode=startInd;
closeList=[startInd,0];  %索引与f值
openList=struct;
%传入为线性索引、返回也为线性索引
childNodes=getChildnode(Imp,closeList,parentNode);

for i=1:length(childNodes)
    cur_sub = index2sub(childNodes(i),cols);
    openList(i).parentSub = startpose;
    openList(i).sub = cur_sub;
    openList(i).node=childNodes(i); %线性索引
    openList(i).g= norm(cur_sub - startpose,2);
    openList(i).h= norm(cur_sub - goalpose,2);
    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(childNodes)
%     path{childNodes(i).node,2}=[[startpose(1),startpose(2)];[childNodes(i).row,childNodes(i).col]];
% end

[~,idx_min]=min([openList.f]);
parentNode=openList(idx_min).node;

while true
    
    childNodes=getChildnode(Imp,closeList,parentNode);
    %判断这些子节点是否在openlist中  --若在，则比较更新  --不在，追加到openlist中
        for i=1:length(childNodes)
            [in_flag,idx]=ismember(childNodes(i),[openList.node]);

            %计算代价函数
            cur_sub = index2sub(childNodes(i),cols);
            parentSub = index2sub(parentNode,cols);
            g = norm(cur_sub - parentSub,2) +openList(idx_min).g;
            h = norm(cur_sub - goalpose,2);
            f=g+h;

           if in_flag  %如果在openlist中  比较f 更新
               if f < openList(idx).f
                   openList(idx).f = f;
                   openList(idx).h = h;
                   openList(idx).g = g;
                   openList(idx).parentSub = parentSub;
               end
           else    %不在的话追加到openlist中
               openList(end+1).node=childNodes(i);
               openList(end).g = g;
               openList(end).h = h;
               openList(end).f = f;
               openList(end).parentSub = parentSub;
               openList(end).sub = cur_sub;
           end
       end
    closeList(end+1,:) = [openList(idx_min).node,openList(idx_min).f];
    openList(idx_min) = [];
    
    %重新搜索
    [~,idx_min] = min([openList.f]);
    parentNode = openList(idx_min).node;
    
    cur_node = index2sub(parentNode,cols);
    dis = norm(cur_node - goalpose,2)
    if (dis < 5 )
        %path{goalInd,2}=[path{openList(idx_min).node,2};[goalpose(1),goalpose(2)]];
        fprintf('find goal!\n');
        break;
    end
end

% path_target=path{goalInd,2};
% plot(path_target(:,2),path_target(:,1),'b-');

function index = sub2index(pt,cols)
    index = (pt(1) - 1)*cols + pt(2);
end

function sub = index2sub(index,cols)
    row = floor(index/cols)+1;
    col = mod(index,cols);
    sub = [row,col];
end

function childNodes = getChildNode(map,closeList, parentNode)
% 选取父节点周边8个节点作为备选子节点，线性化坐标
% 排除超过边界之外的、位于障碍区的、位于closeList中的

step = 5;
[rows, cols] = size(map);
parentSub = index2sub(parentNode,cols);
row_parentNode = parentSub(1);
col_parentNode = parentSub(2);
childNodes = [];
closeList = closeList(:,1);
% 第1个子节点
childNode = [row_parentNode, col_parentNode+step];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
        childNode(2) < 1 || childNode(2) > cols)
    if map(childNode(1), childNode(2)) ~= 0
        childNode_LineIdx = sub2index(childNode,cols);
        if ~ismember(childNode_LineIdx, closeList)
            childNodes(end+1) = childNode_LineIdx;
        end
    end
end

% 第2个子节点
childNode = [row_parentNode-step, col_parentNode+step];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
        childNode(2) < 1 || childNode(2) > cols)
    if map(childNode(1), childNode(2)) ~= 0
       childNode_LineIdx = sub2index(childNode,cols);
        if ~ismember(childNode_LineIdx, closeList)
            childNodes(end+1) = childNode_LineIdx;
        end
    end
end


% 第3个子节点
childNode = [row_parentNode-step, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
        childNode(2) < 1 || childNode(2) > cols)
    if map(childNode(1), childNode(2)) ~= 0
        childNode_LineIdx = sub2index(childNode,cols);
        if ~ismember(childNode_LineIdx, closeList)
            childNodes(end+1) = childNode_LineIdx;
        end
    end
end


% 第4个子节点
childNode = [row_parentNode-step, col_parentNode-step];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
        childNode(2) < 1 || childNode(2) > cols)
    if map(childNode(1), childNode(2)) ~= 0
        childNode_LineIdx = sub2index(childNode,cols);
        if ~ismember(childNode_LineIdx, closeList)
            childNodes(end+1) = childNode_LineIdx;
        end
    end
end


% 第5个子节点
childNode = [row_parentNode, col_parentNode-step];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
        childNode(2) < 1 || childNode(2) > cols)
    if map(childNode(1), childNode(2)) ~= 0
       childNode_LineIdx = sub2index(childNode,cols);
        if ~ismember(childNode_LineIdx, closeList)
            childNodes(end+1) = childNode_LineIdx;
        end
    end
end


% 第6个子节点
childNode = [row_parentNode+step, col_parentNode-step];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
        childNode(2) < 1 || childNode(2) > cols)
    if map(childNode(1), childNode(2)) ~= 0
        childNode_LineIdx = sub2index(childNode,cols);
        if ~ismember(childNode_LineIdx, closeList)
            childNodes(end+1) = childNode_LineIdx;
        end
    end
end


% 第7个子节点
childNode = [row_parentNode+step, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
        childNode(2) < 1 || childNode(2) > cols)
    if map(childNode(1), childNode(2)) ~= 0
       childNode_LineIdx = sub2index(childNode,cols);
        if ~ismember(childNode_LineIdx, closeList)
            childNodes(end+1) = childNode_LineIdx;
        end
    end
end


% 第8个子节点
childNode = [row_parentNode+step, col_parentNode+step];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
        childNode(2) < 1 || childNode(2) > cols)
    if map(childNode(1), childNode(2)) ~= 0
        childNode_LineIdx = sub2index(childNode,cols);
        if ~ismember(childNode_LineIdx, closeList)
            childNodes(end+1) = childNode_LineIdx;
        end
    end
end
end

function index = sub2index(pt,cols)
    index = (pt(1) - 1)*cols + pt(2);
end

function sub = index2sub(index,cols)
    row = floor(index/cols)+1;
    col = mod(index,cols);
    sub = [row,col];
end