matlab绘制搜索树,matlab练习程序（快速搜索随机树RRT）
RRT快速搜索随机树英文全称Rapid-exploration Random Tree，和PRM类似，也是一种路径规划算法。
和PRM类似，算法也需要随机撒点，不过不同的是，该算法不是全局随机撒点，而是一次撒一个点，然后判断当前搜索树与随机点距离，然后找到搜索树距离随机点最近的节点，向该随机点方向扩展。这里随机点有一定的概率是终点，所以搜索树最终是能够到达终点的。
算法流程如下：
1. 首先确定地图与起始结束点位置，设置搜索树，这里定义了一个随机点列表和一个随机点索引前驱列表代表搜索树。
2. 随机撒一个点，该点有可能是最终点，也有可能是全局中的一个随机点，设为nextp。
3. 找到搜索树中距离nextp最近的节点，从该节点向nextp方向扩展step距离，生成新的路径。
4. 判断新生成的路径是否通过障碍物或者该路径已经被搜索过，如果都没有则该路径加入到搜索树中，否则重新生成随机点。
5. 不断循环直到搜索树最终节点距离终点小于一定阈值，搜索结束，根据前驱列表画出搜索路径。
matlab代码如下：
main.m：
clear all;
close all;
clc;
img = imread('map.png'); %空间地图
imshow(img);
hold on;
[h,w]=size(img);
p=ginput(); %选取起始与结束位置
plot(p(:,1),p(:,2),'r.');
pc = p(1,:); %随机节点列表
step = 20; %随机扩展步长
parent = 1; %所有节点前驱，初始节点前驱为自己
while norm(pc(end,:)-p(2,:))>step %搜索到距离结束节点一定距离停止
if rand()<0.3 %按30%概率随机搜索，70%概率朝着结束位置搜索
nextp = [rand()*h rand()*w];
else
nextp = p(2,:);
end
diff = repmat(nextp,length(pc(:,1)),1)-pc; %计算节点树与待搜索节点距离
[~,ind] = min(sqrt(diff(:,1).^2+diff(:,2).^2)); %找到距离带搜索节点最小的节点树节点
direct = atan2(nextp(1)-pc(ind,1),nextp(2)-pc(ind,2));
sin_dir = sin(direct);
cos_dir = cos(direct);
newp = pc(ind,:) + step*[sin_dir cos_dir]; %向着待搜索节点方向扩展节点树
isobs = check_obs(img,newp,pc(ind,:)); %判断该路径是否有障碍物
if isobs==1 %有障碍物重新搜索
continue;
end
diff = repmat(newp,length(pc(:,1)),1)-pc; %判断该路径是否已搜索过，如果已搜索过，则重新搜索
if min(sqrt(diff(:,1).^2+diff(:,2).^2))
continue;
end
pc=[pc;newp]; %将新节点加入节点树
parent = [parent;ind]; %设置新节点的前驱
line([pc(ind,1) pc(parent(ind),1)],[pc(ind,2) pc(parent(ind),2)]);
end
line([pc(ind,1) p(2,1)],[pc(ind,2) p(2,2)],'color','r');
ind = length(pc);
while ind~=1
ind = parent(ind); %不断搜索当前节点的父节点
line([pc(ind,1) pc(parent(ind),1)],[pc(ind,2) pc(parent(ind),2)],'color','r');
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
结果如下：
原图：
算法结果：
标签：p2,img,搜索,matlab,RRT,节点,dir,随机
来源： https://www.cnblogs.com/tiandsp/p/12252727.html