A* 算法的MATLAB代码详解（包教包会）

时间  2020-12-03
标签 node web 算法 数组 函数 学习 字体 spa .net code 栏目 MATLAB 繁體版
原文   原文链接

A* 算法的MATLAB代码详解（包教包会）node

本文中代码参考A*算法的Matlab实现，并对其作出了进一步修改，对A* 算法的matlab代码进行详细注释，修改绘图显示结果，帮助刚接触的小白同窗能更好的学习。
这里只介绍A* 算法的代码部分，对于A* 算法的理论基础后面有时间会单独写一篇博客来介绍。


web

目录

主程序：

function main()
clear;
clc;
tic%计时开始
open1=[];
disp('A Star Path Planing start!!')
obstacle=[];
map.XYMAX=20; %%表明咱们要画一个地图的长和宽
map.start=[1,20];%起始点 注意必须在地图范围内
map.goal=[20,5]; %目标点 注意必须在地图范围内
obstacle=GetBoundary(map);%获得边界数据
%obstacle=[obstacle;1,6;1,7;18,1;4,19;5,19;5,18;6,18;18,12;19,12;19,13;19,11;11,6;7,1;8,1;8,2;2,12;2,13;2,14;2,15;3,13;11,5;12,5;12,4;12,3;13,3;14,3;17,8;18,8;18,7;18,6;17,6;16,6;4,13;5,13;11,20;11,19;12,19;9,14;10,14;10,13;11,12;10,12;15,18;16,18;16,17;18,16;17,15;14,10;14,9;14,11;14,12;15,10;13,10;3,2;3,3;6,7;6,8;5,8;7,7;7,6;];
nObstacle=50;%在地图中随机加入XX个障碍物
obstacle=GetObstacle(nObstacle,obstacle,map);%障碍物和边界坐标
FillPlot(obstacle,'k',1);%画出障碍点
hold on;%保持图形不关闭
path=AStar(obstacle,map);%A*算法
if length(path)>=1
    plot(path(:,1),path(:,2),'-c','LineWidth',5);hold on;
end
toc%计时结束
function path=AStar(obstacle,map)%算法的主程序
%用于存储路径
goal=[map.goal,1,1,1,1];
path=[];
open=[];%建立OpenList
close=[];%建立CloseList
findFlag=false;%用于判断while循环是否结束
%================1.将起始点放在Openlist中======================
%open变量每一行节点坐标，代价值F=G+H,代价值G,父节点坐标]
open =[map.start(1),map.start(2),0+Manhattan_cost(map.start,map.goal),0,map.start(1),map.start(2)];
next=MotionModel();%更新状态--下一步的八个点
openlist=[];
%=======================2.重复如下过程==============================
while ~findFlag
%--------------------首先判断是否达到目标点，或无路径-----
    if isempty(open(:,1))
        disp('No path to goal!!');
        return;
    end    
    %判断目标点是否出如今open列表中
    [isopenFlag,Id]=isopen(map.goal,open);
    if isopenFlag
        disp('Find Goal!!');
        close = [open(Id,:);close];
        findFlag=true;
        break;
    end
    %按照Openlist中的第三列（代价函数F）进行排序，查找F值最小的节点
    [~,I] = sort(open(:,3)); %对OpenList中第三列排序
    open=open(I,:);%open中第一行节点是F值最小的  
    %将F值最小的节点(即open中第一行节点)，放到close第一行(close是不断积压的)，做为当前节点
    close = [open(1,:);close];
    current = open(1,:);
    open1=[open;open1];
    open(1,:)=[];%由于已经从open中移除了，因此第一行须要为空   
    %对当前节点周围的8个相邻节点进行处理
    for in=1:length(next(:,1)) 
        %得到相邻节点的坐标,代价值F先等于0,代价值G先等于0  ,后面两个值是其父节点的坐标值，暂定为零(由于暂时还没法判断其父节点坐标是多少)
        m=[current(1,1)+next(in,1),current(1,2)+next(in,2),0,0,0,0]; 
        m(4)=current(1,4)+next(in,3); % m(4)相邻节点G值
        m(3)=m(4)+Manhattan_cost(m(1:2),map.goal);%m(3)相邻节点F值   
        openlist=[openlist;m(:,1:2)];
        %>>若是它不可达，忽略它，处理下一个相邻节点
        if isObstacle(m,obstacle)
            continue;
        end 
        [flag,targetInd]=FindList(m,open,close);
        %>>若是它在Closelist中，忽略此相邻节点
        if flag==1 
            continue;
        %>>若是它不在Openlist中，加入Openlist,并把当前节点设置为它的父节点
        elseif flag==2 
            m(5:6)=[current(1,1),current(1,2)];%将当前节点做为其父节点
            open = [open;m];%将此相邻节点加放openlist中
        %>>剩下的状况就是它在Openlist中，检查由当前节点到相邻节点是否更好，若是更好则将当前节点设置为其父节点，并更新F,G值；不然不操做    
        else
            %由当前节点到达相邻节点更好(targetInd是此相邻节点在open中的行号 此行的第3列是代价函数F值)
            if m(3) < open(targetInd,3)
                %更好，则将此相邻节点的父节点设置为当前节点，不然不做处理
                m(5:6)=[current(1,1),current(1,2)];%将当前节点做为其父节点
                open(targetInd,:) = m;%将此相邻节点在Openlist中的数据更新
            end
        end        
        %下面的end是判断八个相邻节点的for循环的end
    end      
end 
    PlotGrid(map);%绘制地图
    FillPlot(openlist,'r',0.3);%画出搜索过的节点，透明度为0.3，颜色为红色
end
closed=close(:,1:6);%屡次一举的行为，可是没有又不行
path=GetPath(close,map.start,map.goal);
end

调用到的其余函数以下：算法

1.PlotGrid

function PlotGrid(map)%绘制网格,传入参数为地图大小、起点和目标点等信息
for i = 1:map.XYMAX+3%在地图周围有一圈围墙，因此是地图大小+3
   line([-0.5,map.XYMAX+1.5],[i-1.5,i-1.5]);%划线，重点是line函数的参数用法
end
for j = 1:map.XYMAX+3
   line([j-1.5,j-1.5],[-0.5,map.XYMAX+1.5]);
end
plot(map.start(1),map.start(2),'og','MarkerSize',13,'LineWidth',3);%画出起点圈圈
hold on;
plot(map.goal(1),map.goal(2),'ob','MarkerSize',13,'LineWidth',3);%画出目标点圈圈
axis([-1.5,map.XYMAX+2.5,-1.5,map.XYMAX+2.5]);%设置坐标轴
axis equal;
end

2. MotionModel

function next = MotionModel()
%调用这个函数目的是找到当前点周围八个点的坐标和移动到该点的代价值
next = [-1,1,14;...%表明当前位置左上角的栅格，移动到改栅格的代价值是14（对角线距离）
    0,1,10;...
    1,1,14;...
    -1,0,10;...
    1,0,10;...
    -1,-1,14;...
    0,-1,10;...
    1,-1,14];
end

3.Manhattan_cost

function cost =Manhattan_cost(m,goal)
%计算启发函数代价值 ，这里采用曼哈顿算法
cost=10*abs(m(1)-goal(1))+10*abs(m(2)-goal(2));
end

4.isopen

function [isopenFlag,Id] = isopen(goal,open)
%判断目标点否在open列表中，在open中，isopenFlag = 1,不在open中，isopenFlag = 0 .并反回索引号
isopenFlag = 0;
Id = 0;%初始化
if  isempty(open)%若是open列表为空，则不在open列表中
    isopenFlag = 0;
else %open列表不为空时
    for i = 1:length(open(:,1))%列表中有多少个坐标就循环多少次
       if isequal(goal(1:2),open(i,1:2))%在Openlist中
            isopenFlag = 1;
            Id = i;
            return;
       end 
    end
end
end

5.isObstacle

function flag=isObstacle(m,obstacle)
%判断节点m是否为障碍点，若是是就返为1，不是就返回0
for i=1:length(obstacle(:,1))%对全部的障碍物挨个判断
    if isequal(obstacle(i,:),m(1:2))
        flag=true;
        return;
    end
end
flag=false;
end

6.GetPath

function path=GetPath(close,start,goal)%传入参数为close列表、起点和目标点
ind=1;
ik=-1;
path=[];
while 1%这部分将close中的行数据进行清洗，目的是在全部的close坐标中找出一条最短路径
    path=[path; close(ind,1:2)];%把close里面的路径坐标提取出来
    if isequal(close(ind,1:2),start) %判断跟起点是否相同  
        break;
    end
    for io=1:length(close(:,1))%这个循环的目的是找出相同的坐标点
        if isequal(close(io,1:2),close(ind,5:6))
            ik=ik+1;
            ind=io;
            break;
        end
    end       
end
next1=MotionModel();%当前点的八邻域
lujing=[close(1,5:6)];%取目标点前一个依次往回搜
balinyu=[];
jiaoji=[];%这些变量都是起到存放值的做用而已 
current=[close(1,5:6),close(1,3:4),close(1,5:6)];
ha=length(close(:,1));
while 1
for in=1:length(next1(:,1)) %此处和以前的正序搜索同样的
        m=[current(1,1)+next1(in,1) , current(1,2)+next1(in,2) , 0 , 0 , 0 ,0]; 
        m(4)=current(1,4)+next1(in,3); % m(4)  相邻节点G值
        m(3)=m(4)+Manhattan_cost(m(1:2),start);% m(3)  相邻节点F值
        balinyu=[balinyu;m];%找出当前点的八邻域坐标，下一步是找在close里的
end
jiao=ismember(close(:,1:2),balinyu(:,1:2),'rows');%找出八邻域与close列表的的交集
for ppp=1:length(close(:,1))
    if jiao(ppp,1)==1
       jiaoji=[close(ppp,:);jiaoji];%堆叠
    end
end
       jiaoji=sortrows(jiaoji,4); %对OpenList中第三列排序
        lujing=[jiaoji(1,1:2);lujing];%第一行节点是F值最小的  
        current =jiaoji(1,1:6);
        balinyu=[];
        jiaoji=[];   
    if ha==1%搜索次数没了
        break;
    else
        ha=ha-1;
    end
end
lujing=unique(lujing,'rows');%删除重复坐标
lujing=[lujing;goal];%加上目标点才是完整的
for pp=1:length(lujing(:,1))
    x = lujing(pp,1);
    y = lujing(pp,2);
    A(pp,1)=x;
    A(pp,2)=y;%这部分是将路径坐标拿出来另外存放
end
    plot( A(:,1), A(:,2),'b','linewidth',4)%绘制路线
    B=length(close(:,1))-1;
    node=B*8;%close中每有一个元素，就说明在open里面有8个元素
    disp(['搜索的节点数为：',num2str(node)]);
    k=0;%用于存放路径长度的变量
    for i=1:length(lujing(:,1))-1
    b=10*sqrt(((lujing(i+1,1)-lujing(i,1))^2)+((lujing(i+1,2)-lujing(i,2))^2));%简单的两点间距离公式
    k=k+b;%路径长度值的逐个累加
    end
    disp(['路径长度为',num2str(k)]);
    axis tight;
end

7.GetObstacle

function obstacle=GetObstacle(nob,obstacle,map)
%生成障碍点的坐标
ob=round(rand([nob,2])*map.XYMAX);%随机生产障碍点
removeInd=[];

for io=1:length(ob(:,1))%遍历ob数组，检查哪些坐标与start和goal重合，并将其索引存在removeInd中
   if(isequal(ob(io,:),map.start) || isequal(ob(io,:),map.goal))
        removeInd=[removeInd;io];
   end
end
ob(removeInd,:)=[];%删除重复的节点
obstacle=[obstacle;ob];%将ob障碍点加入到obstacle中
end

8.GetBoundary

function boundary=GetBoundary(map)
%得到地图的边界的坐标
boundary=[];
for i1=0:(map.XYMAX+1)
    boundary=[boundary;[0 i1]];
end
for i2=0:(map.XYMAX+1)
    boundary=[boundary;[i2 0]];
end
for i3=0:(map.XYMAX+1)
    boundary=[boundary;[map.XYMAX+1 i3]];
end
for i4=0:(map.XYMAX+1)
    boundary=[boundary;[i4 map.XYMAX+1]];
end
end

9.FindList

function [flag,targetInd]=FindList(m,open,close)
%{
函数功能：
若是相邻节点(m存储其信息)  已经在Closelist中，则flag = 1  targetInd = 其所在close的行数，用来定位
若是相邻节点(m存储其信息)    不在Openlist 中，则flag = 2  targetInd = []
若是相邻节点(m存储其信息)  已经在Openlist 中，则flag = 3  targetInd = 其所在open的行数，用来定位
%}
%若是openlist为空，则必定不在openlist中
if  isempty(open)
    flag=2;
    targetInd=[];    
else  %open不为空时，须要检查是否在openlist中
    %遍历openlist，检查是否在openlist中
    for io=1:length(open(:,1))
        if isequal(m(1:2),open(io,1:2))%在Openlist中
            flag=3;
            targetInd=io;
            return;
        else  %不在Openlist中
            flag=2;
            targetInd=[];
        end
    end
end
%若是能到这一步，说明：  必定不在Openlist中    那么须要判断是否在closelist中
%遍历Closelist（注意closelist不可能为空）
for ic = 1:length(close(:,1))
    if isequal(m(1:2),close(ic,1:2))%在Closelist中
        flag=1;
        targetInd=ic;
        return;%在Closelist中直接return
    end
end
end

10.FillPlot

function FillPlot(coord,color,tp)%传入参数为填充点坐标、填充颜色和透明度
for i = 1:length(coord(:,1))%求出第一列的长度，有多长就循环填充多少次
    x = coord(i,1);%将第i个障碍物的坐标赋给x、y而后对他们填充
    y = coord(i,2);    
    X = [x-0.5,x+0.5,x+0.5,x-0.5];%填充的大小
    Y = [y-0.5,y-0.5,y+0.5,y+0.5];
    fill(X,Y,color,'EdgeColor','none','FaceAlpha',tp);%填充障碍物坐标所在栅格
    set(gca,'FontSize',40,'Fontname', 'Times New Roman');%设置字体以及字号
    set(gca,'XTick',0:5:20);
    set(gca,'YTick',0:5:20);%设置坐标轴刻度
    hold on;
end
axis tight;
 end

运行结果图：

输出提示以下
红色深浅不一是表明该栅格被搜索的次数
结果图2
结果图3
参考资料：
路径规划A算法matlab代码注释
A算法的Matlab实现





数组

相关文章
相关标签/搜索
每日一句
    每一个你不满意的现在，都有一个你没有努力的曾经。
本站公众号
   欢迎关注本站公众号,获取更多信息
联系我们 最近搜索 最新文章 沪ICP备13005482号-10 MyBatis教程 SQL 教程 MySQL教程 Java 教程 Thymeleaf 教程 Hibernate教程 Spring教程 Redis教程