手把手教用matlab做無人駕駛（二）-路徑規劃A*算法

整個程序下載地址如下：https://download.csdn.net/download/caokaifa/10641075

對於路徑規劃算法-A*算法在matlab中仿真，首先我們在matlab中構建地圖：

先給出matlab主函數程序：

%   editor:  Robert.Cao
%   2018.9.1
clc
clear all
close all
disp('A Star Path Planing start!!')
p.start=[1,1];  %起始點
p.goal=[10,3];  %目標點
p.XYMAX=11; 
obstacle=GetBoundary(p);%得到邊界數據
nObstacle=20;
obstacle=GetObstacle(nObstacle,obstacle,p);
figure(1)
if length(obstacle)>=1
    plot(obstacle(:,1),obstacle(:,2),'om');hold on;
end
plot(p.start(1),p.start(2),'*r');hold on;
plot(p.goal(1),p.goal(2),'*b');hold on;

 註釋：代碼可能直接複製放到matlab中運行可能會出錯，代碼本身是沒錯的，可能複製上去格式不對，我運行了一下因爲空格的原因，所有你一條一條複製到matlab中肯定不會出錯。

現在解釋一下上面代碼：

p.start=[1,1];  %起始點就是代表機器人的起始點座標

p.goal=[10,3];  %機器人目標點座標

p.XYMAX=11; %代表我們要畫一個地圖的長和寬

obstacle=GetBoundary(p);%這裏調用了function 函數GetBoundary，這個函數就是通過我們設置 p.XYMAX=11函數來取得地圖的邊界

nObstacle=20;%這個函數就是我們在地圖中隨機加入二十個障礙物

obstacle=GetObstacle(nObstacle,obstacle,p);%這個obstacle包含了障礙物和地圖邊界數據
if length(obstacle)>=1
    plot(obstacle(:,1),obstacle(:,2),'om');hold on;
end
plot(p.start(1),p.start(2),'*r');hold on;
plot(p.goal(1),p.goal(2),'*b');hold on; %這段程序沒什麼解釋的，就是畫圖，包含了機器人座標的起始和終點位置。

下面，我們給出GetBoundary 函數與GetObstacle函數：

function ：GetBoundary

function boundary=GetBoundary(p)
boundary=[];

for i1=0:(p.XYMAX+1)
    boundary=[boundary;[0 i1]];
end
for i2=0:(p.XYMAX+1)
    boundary=[boundary;[i2 0]];
end
for i3=0:(p.XYMAX+1)
    boundary=[boundary;[p.XYMAX+1 i3]];
end
for i4=0:(p.XYMAX+1)
    boundary=[boundary;[i4 p.XYMAX+1]];
end
boundary=[boundary;[11 11]];
boundary=[boundary;[9 1]];
boundary=[boundary;[10 2]];
boundary=[boundary;[11 3]];
boundary=[boundary;[10 1]];
boundary=[boundary;[11 2]];
boundary=[boundary;[11 1]];

end

function ：GetObstacle

function obstacle=GetObstacle(nob,obstacle,p)

ob=round(rand([nob,2])*p.XYMAX);

removeInd=[];
for io=1:length(ob(:,1))
   if(isSamePosi(ob(io,:),p.start) || isSamePosi(ob(io,:),p.goal))
        removeInd=[removeInd;io];
    end  
end
ob(removeInd,:)=[];

obstacle=[obstacle;ob];

現在看看matlab中的效果：

圖中可以看出邊界點，障礙物點，機器人起始座標，終點座標，下一篇將在這個地圖中做A*算法。