matlab練習程序（概率路線圖PRM）

PRM概率路線圖全稱 Probabilistic Roadmap，是一種路徑規划算法，利用隨機撒點的方式將空間抽樣並將問題轉為圖搜索，利用A*或Dijkstra算法找到起始結束節點的最短路徑。

可以想到撒點數越密，得到的路徑越接近最優路徑，不過運算時間也越長。

算法原理如下：

1. 首先確定地圖與起始結束點位置，對地圖隨機撒點，將起始結束節點加入隨機點中，並剔除撒到障礙物上的點。

2. 建立所有節點的鄰接矩陣無向圖，圖的邊為兩兩隨機點的距離，如果兩兩隨機點直線距離通過障礙物，則設置該邊為無窮大。

3. 利用第2步建好的圖，用最短路徑搜索算法找出鄰接矩陣中通過起始與結束節點中最短的邊，通常用A*算法，我這里用了Dijkstra算法，A*以后有機會再介紹。

下面的實現用到了很早之前寫的dijkstra算法，注意如果撒點過少，可能無法構造出從起始到結束的一條完整路徑，大概會在最后一個循環中死循環。

matlab代碼如下：

main.m：

clear all;
close all;
clc;

num = 200;                      %隨機撒點的數量
img = imread('map.png');        %空間地圖
imshow(img);
hold on;

[h,w]=size(img);
p=ginput();                 %選取起始與結束位置

rnd = floor([rand(num,1)*h,rand(num,1)*w])+1;      %空間隨機撒點
rnd2 = p;                   %將起始與結束節點加入隨機節點一並計算     
for i=1:num                 %只保留隨機節點中不在障礙物上的節點
    if(img(rnd(i,2),rnd(i,1))==255)
        rnd2=[rnd2;double(rnd(i,:))];
    end
end
plot(rnd2(:,1),rnd2(:,2),'.');

A = zeros(length(rnd2));                %構造無向圖鄰接矩陣
for i=1:length(rnd2)-1
    for j=i+1:length(rnd2)
        p1 = rnd2(i,:);
        p2 = rnd2(j,:);

        isobs = check_obs(img,p1,p2);   %判斷兩個隨機點連線是否通過障礙物
        if isobs == 0                   %連線間沒有障礙物則記錄距離
            A(i,j) = norm(p1-p2);
            A(j,i) = norm(p1-p2);
        else                            %連線中有障礙物則距離為inf
            A(i,j) = inf;
            A(j,i) = inf;
        end
    end

end

srcind = 1;     %起始節點索引
dstind = 2;     %結束節點索引

%%對矩陣A使用dijkstra最短路徑算法，這里沒有使用A*
m = length(A);
S=inf(1,m);                 %從開始的源點到每一個節點的距離
S(srcind)=0;                %源點到自己的距離為0
parent=zeros(1,m);          %存儲每個節點的前驅，在松弛過程中賦值
parent(srcind)=1;           %源點的前趨是自己

visit=zeros(1,m);           %標記某個節點是否訪問過了
index=srcind;               %從index節點開始搜索

%判斷是否對所有節點都找的最短路徑了。可能會有源點沒有路徑到目標節點的情況，那就無限循環了
while sum(visit)~=m         %沒有出隊列操作，不過通過visit來等價的表示了    
    visit(index)=1;                             %標記第index節點為已入列的節點
    [S parent]=relax(S,parent,A,visit,index,m);    %松弛，如果兩個節點間有更短的距離，則用更短的距離
    index=extract_min(S,visit,index,m);         %使用已訪問的最小的節點作為下一次搜索的開始節點   
end

plot(p(:,1),p(:,2),'r.')
ind = dstind;
while ind~=1
    line([rnd2(ind,1) rnd2(parent(ind),1)],[rnd2(ind,2) rnd2(parent(ind),2)],'color','r');
    ind = parent(ind);      %不斷搜索當前節點的父節點
end

check_obs.m：

function isobs = check_obs(img,p1,p2)  
[h w]=size(img);
d = norm(p1-p2);
direct = atan2(p1(1)-p2(1),p1(2)-p2(2));
sin_dir = sin(direct);
cos_dir = cos(direct);
for r=0:d
    p = floor(p2 + r*[sin_dir cos_dir]);
    
    y = p(2);
    x = p(1);
    if y>=1 && y<=h && x>=1 && x<=w      
        if img(y,x) ==0
            isobs = 1;
            return;
        end
    end
end
isobs = 0;
end

relax.m：

%邊緣松弛，使用更短的距離作為節點的值
function [S pa]=relax(S,pa,A,visit,index,m)  
   i=index;
   for j=1:m
        if A(i,j)~=inf && visit(j)~=1   %搜索沒有標記過的節點
            if S(j)>S(i)+A(i,j)         %將較小的值賦給正在搜尋的節點
                S(j)=S(i)+A(i,j);
                pa(j)=i;
            end
        end        
    end    
end

extract_min.m：

%提取隊列中尚未標記的最小的值的序號
function index=extract_min(S,visit,index,m)
    Mi=inf;
    for j=1:m
        if visit(j)~=1
           if S(j)<Mi
                Mi=S(j);
                index=j;
           end
        end
    end    
end

結果如下：

原圖：

算法結果：