Astar 均匀方向扩展子节点（30°）

最新推荐文章于 2025-05-29 22:55:08 发布

Ruo_su.

最新推荐文章于 2025-05-29 22:55:08 发布

阅读量195

点赞数

文章标签：算法

本文链接：https://blog.youkuaiyun.com/qq_44752711/article/details/128049458

版权

该博客介绍了一段MATLAB代码，用于实现A*寻路算法，从起点到终点规划路径。代码首先定义了地图，设置障碍物和起终点，然后通过不断扩展节点并比较代价函数来寻找最优路径。最终，绘制出找到的路径。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

clear;clc;close all;
figure(1);
ImpRgb=imread("map.png");
Imp=rgb2gray(ImpRgb);
imshow(Imp);
hold on;
rows=size(Imp,1);
cols=size(Imp,2);
Map=ones(rows,cols);

%空闲值设为1  障碍物设为2  起始点和终点分别为 3 4
startpose=[250,100];
goalpose=[800,900];
plot(startpose(2),startpose(1),'r*');
plot(goalpose(2),goalpose(1),'r*');
hold on;
startInd=sub2ind([rows,cols],startpose(1),startpose(2));
goalInd=sub2ind([rows,cols],goalpose(1),goalpose(2));

Map(startpose(1),startpose(2))=3;
Map(goalpose(1),goalpose(2))=4;


%--障碍物
for i=228:645
    for j=310:588
        Map(j,i)=2;
    end
end

for k=175:785
    for l=1025:1270
        Map(k,l)=2;
    end
end


%初始化
parentNode=startpose;
closeList=[startInd,0];
openList=struct;
childNodes=testGetChildnode(Map,closeList,parentNode);
for i=1:length(childNodes)
    openList(i).node=childNodes(i).node;
    openList(i).row=childNodes(i).row;
    openList(i).col=childNodes(i).col;
    openList(i).g=sqrt(2);
    openList(i).h=norm([goalpose(1),goalpose(2)]-[childNodes(i).row,childNodes(i).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(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).row,openList(idx_min).col];

while true
    
    childNodes=testGetChildnode(Map,closeList,parentNode);
    %判断这些子节点是否在openlist中  --若在，则比较更新  --不在，追加到openlist中
        for i=1:length(childNodes)
        [in_flag,idx]=ismember(childNodes(i).node,[openList.node]);
        
        %计算代价函数
        g=sqrt(2)+openList(idx_min).g;
        h=norm([goalpose(1),goalpose(2)]-[childNodes(i).row,childNodes(i).col]);
        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).row=childNodes(i).row;
               openList(idx).col=childNodes(i).col;
               path{childNodes(i).node,2}=[path{childNodes(i).parentInd,2};[childNodes(i).row,childNodes(i).col]];
           end
       else    %不在的话追加到openlist中
           openList(end+1).node=childNodes(i).node;
           openList(end).g=g;
           openList(end).h=h;
           openList(end).f=f;
           openList(end).row=childNodes(i).row;
           openList(end).col=childNodes(i).col;
           path{childNodes(i).node,2}=[path{childNodes(i).parentInd,2};[childNodes(i).row,childNodes(i).col]];
       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).row,openList(idx_min).col];
    if (norm([parentNode(1),parentNode(2)]-[goalpose(1),goalpose(2)])<sqrt(2)/2)
        path{goalInd,2}=[path{openList(idx_min).node,2};[goalpose(1),goalpose(2)]];
        
        break;
    end
end

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

function childNodes=testGetChildnode(Map,closeList,parentNode)

%每隔30°选取一个扩展点 --12个
%--排除超过边界之外的、位于障碍物的、位于closelist中的
%[rows,cols]=size(Map)
%将parentnode 整数化
rows=960;
cols=1536;
node1=round(parentNode(1));
node2=round(parentNode(2));
parentNode_ind=sub2ind([rows,cols],node1,node2);
childNodes=struct;
closeList=closeList(:,1);
r=2;

%--1
i=0;
childNode=[parentNode(1),parentNode(2)-r];
if ~(childNode(1)<0.5 || childNode(1)>rows||childNode(2)<0.5 || childNode(2)>cols)  %边界内
        node1=round(childNode(1));
        node2=round(childNode(2)); %子节点所在栅格  --不是障碍物
    if Map(node1,node2)~=2  
        childNode_idx=sub2ind([rows,cols],node1,node2);
        if ~ismember(childNode_idx,closeList)
            i=1+i;
            childNodes(i).node=childNode_idx;
            childNodes(i).row=childNode(1);
            childNodes(i).col=childNode(2);
            childNodes(i).parentInd=parentNode_ind;
        end
    end
end

%--2
childNode=[parentNode(1),parentNode(2)+r];
if ~(childNode(1)<0.5 || childNode(1)>rows||childNode(2)<0.5 || childNode(2)>cols)
        node1=round(childNode(1));
        node2=round(childNode(2));
    if Map(node1,node2)~=2
        childNode_idx=sub2ind([rows,cols],node1,node2);
        if ~ismember(childNode_idx,closeList)
            i=i+1;
            childNodes(i).node=childNode_idx;
            childNodes(i).row=childNode(1);
            childNodes(i).col=childNode(2);
            childNodes(i).parentInd=parentNode_ind;
        end
    end
end

%--3
childNode=[parentNode(1)-r,parentNode(2)];
if ~(childNode(1)<0.5 || childNode(1)>rows||childNode(2)<0.5 || childNode(2)>cols)
        node1=round(childNode(1));
        node2=round(childNode(2));
    if Map(node1,node2)~=2
        childNode_idx=sub2ind([rows,cols],node1,node2);
        if ~ismember(childNode_idx,closeList)
            i=i+1;
            childNodes(i).node=childNode_idx;
            childNodes(i).row=childNode(1);
            childNodes(i).col=childNode(2);
            childNodes(i).parentInd=parentNode_ind;
        end
    end
end

%--4
childNode=[parentNode(1)+r,parentNode(2)];
if ~(childNode(1)<0.5 || childNode(1)>rows||childNode(2)<0.5 || childNode(2)>cols)
        node1=round(childNode(1));
        node2=round(childNode(2));
    if Map(node1,node2)~=2
        childNode_idx=sub2ind([rows,cols],node1,node2);
        if ~ismember(childNode_idx,closeList)
            i=i+1;
            childNodes(i).node=childNode_idx;
            childNodes(i).row=childNode(1);
            childNodes(i).col=childNode(2);
            childNodes(i).parentInd=parentNode_ind;
        end
    end
end

%--5
childNode=[parentNode(1)+r*sin(pi/6),parentNode(2)-r*cos(pi/6)];
if ~(childNode(1)<0.5 || childNode(1)>rows||childNode(2)<0.5 || childNode(2)>cols)
        node1=round(childNode(1));
        node2=round(childNode(2));
    if Map(node1,node2)~=2
        childNode_idx=sub2ind([rows,cols],node1,node2);
        if ~ismember(childNode_idx,closeList)
            i=i+1;
            childNodes(i).node=childNode_idx;
            childNodes(i).row=childNode(1);
            childNodes(i).col=childNode(2);
            childNodes(i).parentInd=parentNode_ind;
        end
    end
end

%--6
childNode=[parentNode(1)-r*sin(pi/6),parentNode(2)-r*cos(pi/6)];
if ~(childNode(1)<0.5 || childNode(1)>rows||childNode(2)<0.5 || childNode(2)>cols)
        node1=round(childNode(1));
        node2=round(childNode(2));
    if Map(node1,node2)~=2
        childNode_idx=sub2ind([rows,cols],node1,node2);
        if ~ismember(childNode_idx,closeList)
            i=i+1;
            childNodes(i).node=childNode_idx;
            childNodes(i).row=childNode(1);
            childNodes(i).col=childNode(2);
            childNodes(i).parentInd=parentNode_ind;
        end
    end
end

%--7
childNode=[parentNode(1)+r*sin(pi/6),parentNode(2)+r*cos(pi/6)];
if ~(childNode(1)<0.5 || childNode(1)>rows||childNode(2)<0.5 || childNode(2)>cols)
    node1=round(childNode(1));
    node2=round(childNode(2));
    if Map(node1,node2)~=2
        childNode_idx=sub2ind([rows,cols],node1,node2);
        if ~ismember(childNode_idx,closeList)
            i=i+1;
            childNodes(i).node=childNode_idx;
            childNodes(i).row=childNode(1);
            childNodes(i).col=childNode(2);
            childNodes(i).parentInd=parentNode_ind;
        end
    end
end

%--8
childNode=[parentNode(1)-r*sin(pi/6),parentNode(2)+r*cos(pi/6)];
if ~(childNode(1)<0.5 || childNode(1)>rows||childNode(2)<0.5 || childNode(2)>cols)
        node1=round(childNode(1));
        node2=round(childNode(2));
    if Map(node1,node2)~=2
        childNode_idx=sub2ind([rows,cols],node1,node2);
        if ~ismember(childNode_idx,closeList)
            i=i+1;
            childNodes(i).node=childNode_idx;
            childNodes(i).row=childNode(1);
            childNodes(i).col=childNode(2);
            childNodes(i).parentInd=parentNode_ind;
        end
    end
end

%--9
childNode=[parentNode(1)+r*sin(pi/3),parentNode(2)-r*cos(pi/3)];
if ~(childNode(1)<0.5 || childNode(1)>rows||childNode(2)<0.5 || childNode(2)>cols)
        node1=round(childNode(1));
        node2=round(childNode(2));
    if Map(node1,node2)~=2
        childNode_idx=sub2ind([rows,cols],node1,node2);
        if ~ismember(childNode_idx,closeList)
            i=i+1;
            childNodes(i).node=childNode_idx;
            childNodes(i).row=childNode(1);
            childNodes(i).col=childNode(2);
            childNodes(i).parentInd=parentNode_ind;
        end
    end
end

%--10
childNode=[parentNode(1)-r*sin(pi/3),parentNode(2)-r*cos(pi/3)];
if ~(childNode(1)<0.5 || childNode(1)>rows||childNode(2)<0.5 || childNode(2)>cols)
        node1=round(childNode(1));
        node2=round(childNode(2));
    if Map(node1,node2)~=2
        childNode_idx=sub2ind([rows,cols],node1,node2);
        if ~ismember(childNode_idx,closeList)
            i=i+1;
            childNodes(i).node=childNode_idx;
            childNodes(i).row=childNode(1);
            childNodes(i).col=childNode(2);
            childNodes(i).parentInd=parentNode_ind;
        end
    end
end

%--11
childNode=[parentNode(1)+r*sin(pi/3),parentNode(2)+r*cos(pi/3)];
if ~(childNode(1)<0.5 || childNode(1)>rows||childNode(2)<0.5 || childNode(2)>cols)
        node1=round(childNode(1));
        node2=round(childNode(2));
    if Map(node1,node2)~=2
        childNode_idx=sub2ind([rows,cols],node1,node2);
        if ~ismember(childNode_idx,closeList)
            i=i+1;
            childNodes(i).node=childNode_idx;
            childNodes(i).row=childNode(1);
            childNodes(i).col=childNode(2);
            childNodes(i).parentInd=parentNode_ind;
        end
    end
end

%--12
childNode=[parentNode(1)-r*sin(pi/3),parentNode(2)+r*cos(pi/3)];
if ~(childNode(1)<0.5 || childNode(1)>rows||childNode(2)<0.5 || childNode(2)>cols)
        node1=round(childNode(1));
        node2=round(childNode(2));
    if Map(node1,node2)~=2
        childNode_idx=sub2ind([rows,cols],node1,node2);
        if ~ismember(childNode_idx,closeList)
            i=i+1;
            childNodes(i).node=childNode_idx;
            childNodes(i).row=childNode(1);
            childNodes(i).col=childNode(2);
            childNodes(i).parentInd=parentNode_ind;
        end
    end
end

end

结果