PRM概率路线图全称 Probabilistic Roadmap,是一种路径规划算法,利用随机撒点的方式将空间抽样并将问题转为图搜索,利用A*或Dijkstra算法找到起始结束节点的最短路径。

可以想到撒点数越密,得到的路径越接近最优路径,不过运算时间也越长。

算法原理如下:

1. 首先确定地图与起始结束点位置,对地图随机撒点,将起始结束节点加入随机点中,并剔除撒到障碍物上的点。

2. 建立所有节点的邻接矩阵无向图,图的边为两两随机点的距离,如果两两随机点直线距离通过障碍物,则设置该边为无穷大。

3. 利用第2步建好的图,用最短路径搜索算法找出邻接矩阵中通过起始与结束节点中最短的边,通常用A*算法,我这里用了Dijkstra算法,A*以后有机会再介绍。

下面的实现用到了很早之前写的dijkstra算法,注意如果撒点过少,可能无法构造出从起始到结束的一条完整路径,大概会在最后一个循环中死循环。

matlab代码如下:

main.m:

clear all;
close all;
clc; num = ; %随机撒点的数量
img = imread('map.png'); %空间地图
imshow(img);
hold on; [h,w]=size(img);
p=ginput(); %选取起始与结束位置 rnd = floor([rand(num,)*h,rand(num,)*w])+; %空间随机撒点
rnd2 = p; %将起始与结束节点加入随机节点一并计算
for i=:num %只保留随机节点中不在障碍物上的节点
if(img(rnd(i,),rnd(i,))==)
rnd2=[rnd2;double(rnd(i,:))];
end
end
plot(rnd2(:,),rnd2(:,),'.'); A = zeros(length(rnd2)); %构造无向图邻接矩阵
for i=:length(rnd2)-
for j=i+:length(rnd2)
p1 = rnd2(i,:);
p2 = rnd2(j,:); isobs = check_obs(img,p1,p2); %判断两个随机点连线是否通过障碍物
if isobs == %连线间没有障碍物则记录距离
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 = ; %起始节点索引
dstind = ; %结束节点索引 %%对矩阵A使用dijkstra最短路径算法,这里没有使用A*
m = length(A);
S=inf(,m); %从开始的源点到每一个节点的距离
S(srcind)=; %源点到自己的距离为0
parent=zeros(,m); %存储每个节点的前驱,在松弛过程中赋值
parent(srcind)=; %源点的前趋是自己 visit=zeros(,m); %标记某个节点是否访问过了
index=srcind; %从index节点开始搜索 %判断是否对所有节点都找的最短路径了。可能会有源点没有路径到目标节点的情况,那就无限循环了
while sum(visit)~=m %没有出队列操作,不过通过visit来等价的表示了
visit(index)=; %标记第index节点为已入列的节点
[S parent]=relax(S,parent,A,visit,index,m); %松弛,如果两个节点间有更短的距离,则用更短的距离
index=extract_min(S,visit,index,m); %使用已访问的最小的节点作为下一次搜索的开始节点
end plot(p(:,),p(:,),'r.')
ind = dstind;
while ind~=
line([rnd2(ind,) rnd2(parent(ind),)],[rnd2(ind,) rnd2(parent(ind),)],'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()-p2(),p1()-p2());
sin_dir = sin(direct);
cos_dir = cos(direct);
for r=:d
p = floor(p2 + r*[sin_dir cos_dir]); y = p();
x = p();
if y>= && y<=h && x>= && x<=w
if img(y,x) ==
isobs = ;
return;
end
end
end
isobs = ;
end

relax.m:

%边缘松弛,使用更短的距离作为节点的值
function [S pa]=relax(S,pa,A,visit,index,m)
i=index;
for j=:m
if A(i,j)~=inf && visit(j)~= %搜索没有标记过的节点
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=:m
if visit(j)~=
if S(j)<Mi
Mi=S(j);
index=j;
end
end
end
end

结果如下:

原图:

算法结果:

matlab练习程序(概率路线图PRM)的更多相关文章

  1. matlab练习程序(SUSAN检测)

    matlab练习程序(SUSAN检测) SUSAN算子既可以检测角点也可以检测边缘,不过角点似乎比不过harris,边缘似乎比不过Canny.不过思想还是有点意思的. 主要思想就是:首先做一个和原图像 ...

  2. (转)matlab练习程序(HOG方向梯度直方图)

    matlab练习程序(HOG方向梯度直方图)http://www.cnblogs.com/tiandsp/archive/2013/05/24/3097503.html HOG(Histogram o ...

  3. matlab示例程序--Motion-Based Multiple Object Tracking--卡尔曼多目标跟踪程序--解读

    静止背景下的卡尔曼多目标跟踪 最近学习了一下多目标跟踪,看了看MathWorks的关于Motion-Based Multiple Object Tracking的Documention. 官网链接:h ...

  4. matlab练习程序(模拟退火SA)

    模拟退火首先从某个初始候选解开始,当温度大于0时执行循环. 在循环中,通过随机扰动产生一个新的解,然后求得新解和原解之间的能量差,如果差小于0,则采用新解作为当前解. 如果差大于0,则采用一个当前温度 ...

  5. matlab练习程序(快速搜索随机树RRT)

    RRT快速搜索随机树英文全称Rapid-exploration Random Tree,和PRM类似,也是一种路径规划算法. 和PRM类似,算法也需要随机撒点,不过不同的是,该算法不是全局随机撒点,而 ...

  6. matlab练习程序(透视投影,把lena贴到billboard上)

    本练习程序是受到了这个老外博文的启发,感觉挺有意思,就尝试了一下.他用的是opencv,我这里用的是matlab. 过去写过透视投影,当时是用来做倾斜校正的,这次同样用到了透视投影,不过更有意思,是将 ...

  7. matlab练习程序(多圆交点)

    最近总是对计算几何方面的程序比较感兴趣. 多圆求交点,要先对圆两两求交点. 有交点的圆分为相切圆和相交圆. 相切圆求法: 1.根据两圆心求直线 2.求公共弦直线方程 3.求两直线交点即两圆切点. 相交 ...

  8. matlab练习程序(矩形变换为单连通形状)

    变换使用的模板必须是单连通的,而且模板中心必须在模板内,如果在模板中打个结或是月牙形,这里的程序就处理不了了. 虽然非单连通模板也有办法处理,不过不是这里要讨论的. 这里用到的方法和矩形变换为圆那片文 ...

  9. matlab练习程序(渲染三原色)

    这里我用的空间是x向右为正,y向下为正,z向屏幕里面为正.相当于标准右手系绕x轴旋转了180度. 将三个点光源放在 r = [0.3,0,0.5];g = [0.3,-0.5*cos(pi/6),-0 ...

随机推荐

  1. ubuntu 虚拟机添加多个站点

    我们安装好lamp环境,然后开始操作,比如一个站点叫test.ubuntu1.com,一个叫test.ubuntu2.com 1.修改hosts文件,路径/etc/hosts sudo vim /et ...

  2. 吴裕雄--天生自然Numpy库学习笔记:NumPy 广播(Broadcast)

    广播(Broadcast)是 numpy 对不同形状(shape)的数组进行数值计算的方式, 对数组的算术运算通常在相应的元素上进行. 如果两个数组 a 和 b 形状相同,即满足 a.shape == ...

  3. Cisco TrustSec(理解)

    1.Cisco TrustSec的限制当指定了无效的设备ID时,受保护的访问凭据(Protected access credential,PAC)设置将失败并保持挂起状态. 即使在清除PAC并配置正确 ...

  4. JavaScript中关于隐式转换的一些总结

    JavaScript运算符中的隐式转换规律:一.递增递减运算符(前置.后置)1.如果包含的是有效数字字符串或者是有效浮点数字符串,则会将字符串转换(Number())为数值,再进行加减操作,返回值的类 ...

  5. rem在三星s5部分机型情况下 设置的字体大小与手机实际字体大小不一致问题

    rem在三星s5部分机型情况下 设置的字体大小与手机实际字体大小不一致问题 判断是特殊机型,做特殊处理. var u=navigator.userAgent; if($(window).width() ...

  6. redis 基础 Redis 数据类型

    String(字符串) Hash(哈希) List(列表) Set(集合) zset(sorted set:有序集合)

  7. win10 设置jdk环境变量

    win10环境变量较为特殊 只需要设置JAVA_HOME和PATH,设置后可以通过java -version 和 javac -version 验证查看jdk得版本号和编译版本号 具体如下图所示:

  8. Vue系列(六)之常用指令v-model

    v-model 基本使用 修饰符 .trim .number .lazy 前面讲到的插值,其实都是单向绑定,数据变-->视图变.有些元素可以与用户交互,比如input,select等,那么我们希 ...

  9. hadoop3.1.1高可用集群web端口9870

  10. 八 Spring的IOC的XML和注解的区别及其整合开发

    xml和注解的区别 xml和注解整合开发 注解:取消扫描配置开启注解配置 扫描:<context:component-scan base-package="" />  ...