基于栅格地图的Dijkstra算法路径规划

用MATLAB 实现，基于栅格地图的Dijkstra算法路径规划。

function [] = Dijkstraguihua（）

clear all;
close all;
disp（‘Dijkstra Path Planning start!!‘）;

p.start=[4.527.5];  
p.goal=[26.52.5]; 
p.XYMAX=30;    

obstacle=GetBoundary（p）;%初始化障碍物

path=Dijkstra（obstaclep）;%30x2

figure（1）

if length（obstacle）>=1
    plot（obstacle（:1）obstacle（:2）‘Or‘）;
    hold on;
end
plot（p.start（1）p.start（2）‘*b‘）;hold on;
plot（p.goal（1）p.goal（2）‘*r‘）;hold on;
if length（path）>=1
    %plot（path（:1）path（:2）‘b‘‘linewidth‘2）;
    hold on;
end
j=size（path1）;%30
mindistance=0;
for i=j:-1:2
    s=distance（path（i1）path（i2）path（i-11）path（i-12））;
    mindistance=s+mindistance;
end
axis（[0 p.XYMAX 0 p.XYMAX]）
set（gca‘xtick‘0:1:p.XYMAX‘ytick‘0:1:p.XYMAX‘GridLinestyle‘‘-‘...
    ‘xgrid‘‘on‘‘ygrid‘‘on‘）;
grid on;
disp（‘Shortest Length is‘）
disp（ mindistance）

end



function path=Dijkstra（obstaclep）

path=[];

open=[p.start（1） p.start（2） 0 p.start（1） p.start（2）];
close=[];

next=MotionModel（）;

findFlag=false;

while ~findFlag
    initNopen=length（open（:1））;

    if initNopen==0 
        disp（‘No path to goal!!‘）; 
        return; 
    end
    for io=1:initNopen
        if isSamePosi（open（io1:2）p.goal）
            disp（‘Find Goal!!‘）;
            close=[open;close];
            findFlag=true;
            break;
        end
        
        for in=1:length（next（:1））
            m=[open（io1）+next（in1） open（io2）+next（in2） open（io3）+next（in3）];

            if isObstacle（mobstacle） 
                continue;
            end

            [flag targetInd]=FindList（mopenclose）;
            
            if flag==1
                if m（3）                    open（targetInd3）=m（3）;
                    open（targetInd4）=open（io1）;
                    open（targetInd5）=open（io2）;
                end
            elseif flag==2 
                if m（3）                    close（targetInd4）=open（io1）;
                    close（targetInd5）=open（io2）;
                    open=[open; close（targetInd:）];
                    close（targetInd:）=[];
                end
            else 
                open=[open;[m open（io1） open（io2）]];
            end
        end
    end
    if findFlag==false
        close=[close; open（1:initNopen:）];
        open（1:initNopen:）=[];
    end

    %animation（openclosepobstacle）;

end

path=GetPath（closep.startio）;

end


function result=isSamePosi（ab）

result=false;
if a（1）==b（1） && a（2）==b（2）
    result=true;
end
end

function boundary=GetBoundary（~）

boundary=[];
for i1=0:30
   boundary=[boundary;[0.5 i1+0.5]];
   boundary=[boundary;[28.5 i1+0.5]];
   boundary=[boundary;[29.5 i1+0.5]];
end
for i2=0:30
    %boundary=[boundary;[i2+0.5 0.5]];
    boundary=[boundary;[i2+0.5 29.5]];
end
for i3=0:10
    boundary=[boundary;[i3+8.5 24.5]];
    boundary=[boundary;[i3+8.5 25.5]];
end
for i4=0:1
  

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件       63140  2014-09-24 22:49  Dijkstra\09-24-2014 10-48-51 _jpg
     文件        1515  2017-05-08 11:41  Dijkstra\DijkstraPlan.m
     文件       10664  2017-05-08 14:20  Dijkstra\DijkstraSample.m
     文件        5949  2017-06-01 12:06  Dijkstra\Dijkstraguihua.m
     文件        4604  2017-05-09 16:42  Dijkstra\Dijkstrasampleyichu.m
     文件         156  2017-05-09 16:48  Dijkstra\distance.m
     目录           0  2018-01-19 09:30  Dijkstra\