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

结果如下：

原图：

算法结果：