rapidly exploring random tree（快速搜索随机树）路径规划

直接上程序

%% 流程初始化
clear all; close all;
pic_num=1;%制作gif所需参数
x_I=1; y_I=1;           % 设置初始点
x_G=600; y_G=600;       % 设置目标点
goal(1)=x_G;
goal(2)=y_G;
Thr=50;                 %设置目标点阈值 当到这个范围内时则认为已到达目标点
Delta= 30;              % 设置扩展步长，扩展结点允许的最大距离
%% 建树初始化
T.v(1).x = x_I;         % T是我们要做的树，v是节点，这里先把起始点加入到T里面来
T.v(1).y = y_I; 
T.v(1).xPrev = x_I;     % 起始节点的父节点仍然是其本身
T.v(1).yPrev = y_I;
T.v(1).dist=0;          %从父节点到该节点的距离，这里可取欧氏距离
T.v(1).indPrev = 0;     %父节点的索引
%% 开始构建树——作业部分
ImpRgb=imread('zhangai.png');  
Imp=im2bw(ImpRgb,0.1);
xL=size(Imp,1);%地图x轴长度
yL=size(Imp,2);%地图y轴长度
for k1=xL:yL
    Imp(k1,:)=1;
end
Imp1=double(Imp);
colormap([0,0,0;1,1,1]);  % 创建颜色
pcolor(0.5:size(Imp,1)-1 + 0.5, 0.5:size(Imp,2)-1 + 0.5, Imp1); % 赋予栅格颜色
shading flat;set(gca,'ydir','reverse');
hold on
plot(x_I, y_I, 'ro', 'MarkerSize',10, 'MarkerFaceColor','r');hold on
plot(x_G, y_G, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');% 绘制起点和目标点
count=1;
for iter = 1:3000
    %Step 1: 在地图中随机采样一个点x_rand
    x_rand=Sample(Imp,goal);
    %Step 2: 遍历树，从树中找到最近邻近点x_near 
    [x_near,index]= Near(x_rand,T);
    plot(x_near(1), x_near(2), 'go', 'MarkerSize',2);
    %Step 3: 扩展得到x_new节点
    x_new=Steer(x_rand,x_near,Delta);%关于求角度的部分存疑，待后续查验
    if x_new(1)<=0||x_new(2)<=0||x_new(1)>=625||x_new(2)>=625
        continue;
    end
    %x_new
    %检查节点是否是collision-free
    flag = collisionChecking(x_near,x_new,Imp1);
    if  flag== false %如果有障碍物则跳出
        continue;
    end
    count=count+1;
    
    %Step 4: 将x_new插入树T 
    %提示：新节点x_new的父节点是x_near
    T.v(count).x = x_new(1);        
    T.v(count).y = x_new(2); 
    T.v(count).xPrev = x_near(1);     % 起始节点的父节点仍然是其本身
    T.v(count).yPrev = x_near(2);
    T.v(count).dist = Distance(x_new,x_near);          %从父节点到该节点的距离，这里可取欧氏距离
    T.v(count).indPrev = index;     %父节点的索引
    %Step 5:检查是否到达目标点附近 
    %提示：注意使用目标点阈值Thr，若当前节点和终点的欧式距离小于Thr，则跳出当前for循环
    if Distance(x_new,goal) < Thr
        break;
    end
   %Step 6:将x_near和x_new之间的路径画出来
   %提示 1：使用plot绘制，因为要多次在同一张图上绘制线段，所以每次使用plot后需要接上hold on命令
   %提示 2：在判断终点条件弹出for循环前，记得把x_near和x_new之间的路径画出来
%    plot([x_near(1),x_near(2)],[x_new(1),x_new(2)]);
%    hold on
  line([x_near(1),x_new(1)],[x_near(2),x_new(2)]);
   pause(0.1); %暂停0.1s，使得RRT扩展过程容易观察
   
    %======================================================================
    %将结果显示为GIF的部分
    F = getframe(gcf);
    I = frame2im(F);
    [I,map2] = rgb2ind(I,256);
    if pic_num == 1
        imwrite(I,map2,'RRT.gif','gif', 'Loopcount',inf,'DelayTime',0.1);
    else
        imwrite(I,map2,'RRT.gif','gif','WriteMode','append','DelayTime',0.1);
    end
    pic_num = pic_num + 1;
end
%% 路径已经找到，反向查询
if iter < 2000
    path.pos(1).x = x_G; path.pos(1).y = y_G;
    path.pos(2).x = T.v(end).x; path.pos(2).y = T.v(end).y;
    pathIndex = T.v(end).indPrev; % 终点加入路径
    j=0;
    while 1
        path.pos(j+3).x = T.v(pathIndex).x;
        path.pos(j+3).y = T.v(pathIndex).y;
        pathIndex = T.v(pathIndex).indPrev;
        if pathIndex == 1
            break
        end
        j=j+1;
    end  % 沿终点回溯到起点
    path.pos(end+1).x = x_I; path.pos(end).y = y_I; % 起点加入路径
    for j = 2:length(path.pos)
        plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'b', 'Linewidth', 3);
    end
    F = getframe(gcf);
    I = frame2im(F);
    [I,map2] = rgb2ind(I,256);
    if pic_num == 1
        imwrite(I,map2,'RRT.gif','gif', 'Loopcount',inf,'DelayTime',0.2);
    else
        imwrite(I,map2,'RRT.gif','gif','WriteMode','append','DelayTime',0.2);
    end
    pic_num = pic_num + 1;
else
    disp('Error, no path found!');
end
hold off;

function X_rand=Sample(map,goal)
if unifrnd(0,1)<0.5
   X_rand(1)= unifrnd(0,1)* size(map,1);   
   X_rand(2)= unifrnd(0,1)* size(map,2);  
else
   X_rand=goal; %这一步很重要，并不是完全随机，而是时刻要以目标为导向
end

function [X_near,index]=Near(X_rand,T)
v_code = [T.v(1).x,T.v(1).y];
min_distance = sqrt(norm(X_rand - v_code ));
%min_distance=sqrt((X_rand(1)-T.v(1).x)^2+(X_rand(2)-T.v(1).y)^2);
for T_iter=1:size(T.v,2) %第2个维度表示生成树上的节点
    temp_code = [T.v(T_iter).x,T.v(T_iter).y];
    temp_distance=sqrt(norm(X_rand - temp_code));
    %temp_distance=sqrt(((1).x)^2+(X_rand(2)-T.v(T_iter).y)^2);
    if temp_distance<=min_distance
        min_distance=temp_distance;
        X_near(1)=T.v(T_iter).x;
        X_near(2)=T.v(T_iter).y;
        index=T_iter;
    end
end

function X_new=Steer(X_rand,X_near,StepSize)
%atan2(a,b)是4象限反正切，它的取值不仅取决于正切值a/b，还取决于点 (b, a) 落入哪个象限
% a是纵坐标y,b是横坐标x。
theta = atan2(X_rand(2)-X_near(2),X_rand(1)-X_near(1));  % direction to extend sample to produce new node
X_new = X_near(1:2) + StepSize * [cos(theta)  sin(theta)];

function feasible=collisionChecking(~,goalPose,map)%冲突检查：判断起始点到终点之间是否有障碍物

feasible=true;%可行的，可执行的
% dir=atan2(goalPose(1)-startPose(1),goalPose(2)-startPose(2));%目标点和起始点之间的角度
x = floor(goalPose(1))+1;
y = floor(goalPose(2))+1;
if map(y,x)==0 % 矩阵的行表示x,矩阵的列表示y。
    feasible=false;
end

% for r=0:0.5:sqrt(sum((startPose-goalPose).^2))%sqrt(sum((startPose-goalPose).^2)):两点间的距离
%     posCheck = startPose + r.*[sin(dir) cos(dir)];%以0.5的间隔得到中间的点
%     if ~(feasiblePoint(ceil(posCheck),map) && feasiblePoint(floor(posCheck),map) && ...
%             feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],map) && feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],map))
%         feasible=false;break;
%     end
%     if ~feasiblePoint([floor(goalPose(1)),ceil(goalPose(2))],map), feasible=false; end

end

所用的地图如图1所示，黑色块部分是障碍，图是截的别的博主的，所以在程序中有一部分需要图像处理，二值化，将蓝线和红线擦除。

 最后的仿真结果，因为求解的过程中涉及随机，所以每次运行求解出的路径不同。本文放了两个作为参考。

 

 图1 路径规划结果图（1）

---

 图2 路径规划结果图（2）

 结论：应用传统的RRT方法做路径规划，相比于人工势场法是比较好的，但是存在运算时间长，迭代次数多的问题，另外，每次求的并不是最优路径，而是找到一条路径能从起点通向终点，同时，因为求解的随机性，每次得到的路径都是不一样的，如果终点被障碍包围，只有一条狭长的甬道通向终点，那么RRT方法就不是可取的了，因为在这种情况下几乎求不出解来，需要做改进。