标签：function amp while step 练习 UNC 生成 color check
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))<sqrt(step) 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
结果如下：
原图：
算法结果：
标签：function amp while step 练习 UNC 生成 color check
原文地址：https://www.cnblogs.com/tiandsp/p/12252727.html
