RRT路径规划算法

快速扩展随机树(RRT)算法，是近十几年应用比较广泛的一种运动规划算法。

它的大致原理为：原始的RRT算法通过一个初始点作为根节点，通过随机采样，增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中通过回溯的方式，找到这条从初始点到目标点的路径。

RRT总体是一种基于概率采样的搜索方法，通过状态空间的随机采样点，把搜索导向空白区域，从而生成一条从起始点到目标点的规划路径，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，当然它也有自身的缺点，这个做完后续的实验便可以体会到。

总体步骤

1.建立树，根节点为起始点，指定目标点

2.读取地图数据

3.在地图中随机得到一个点，记作p_rand

4.遍历当前的整个树，找到距离随机点最近的点，记作p_near

5.由p_near向随机点p_rand扩展一个步长的距离，记步长的记录为Delta，扩展后的点记作p_new

6.检测此p_new点是否在障碍物上，如果在障碍物上，则跳出本次循环，从第3步重新开始

7.将新产生的点p_new插入到整个树中

8.当p_new点距离目标点小于某个范围时，退出搜索，然后将整个路径画出来

实现

在实现上述步骤的时候，会有一些细节上的处理过程，上述的步骤只是大致的步骤描述。

先建立树以及地图：

%设置起始点
init_x = 10;
init_y = 10;

%设置目标点
end_x = 450;
end_y = 450;

%建立树，根节点为起始点
Tree.v(1).x = init_x;
Tree.v(1).y = init_y;
Tree.v(1).xFather = init_x;  %根节点的父节点仍是它本身
Tree.v(1).yFather = init_y;
Tree.v(1).dis = 0; %从父节点到该节点的距离

%读取地图
figure(1);
map = im2bw(imread('map1.bmp'));  %读取图片并二值化作为地图
imshow(map);
mapX = size(map,1);%地图x轴长度
mapY = size(map,2);%地图y轴长度
hold on;
plot(init_x, init_y, 'ro', 'MarkerSize',5, 'MarkerFaceColor','r');
plot(end_x, end_y, 'go', 'MarkerSize',5, 'MarkerFaceColor','b');% 绘制起点和目标点

注意上述建立树的过程中，结构体中保存的是当前点的x，y坐标，以及其父节点的坐标xFather 与 yFather ，这个是为了后续回溯路线时方便，当我们找到最后一个点时，从最后一个点一直向上回溯其父节点，然后将其保存，必然可以回溯到起始点，然后就可以将整个路径保存下来。

在地图中随机得到一个点，但是这个点不能在障碍物上，并且需要注意此随机点不能超过整个地图的距离：

%Step 1: 在地图中随机采样一个点p_rand 但是p_rand不能是在障碍物上
%当是原点坐标时，重新随机
while (p_rand(1) == 1 && p_rand(2) == 1) || (map(p_rand(1),p_rand(2)) == 0)
    p_rand(1) = round(rand() * mapX); % rand()生成的是0~1均匀分布的随机数，乘以800再向上取整，数便为[1,800]间的整数
    p_rand(2) = round(rand() * mapY);
    if p_rand(1) < 1
       p_rand(1) = 1;
    else if p_rand(1) > mapX
       p_rand(1) = mapX;
        end
    end
    if p_rand(2) < 1
       p_rand(2) = 1;
    else if p_rand(2) > mapY
       p_rand(2) = mapY;
       end
    end          
end

%是在障碍物上时就一直随机
while map(p_rand(1),p_rand(2)) == 0
    p_rand(1) = round(rand() * mapX); % rand()生成的是0~1均匀分布的随机数，乘以800再向上取整，数便为[1,800]间的整数
    p_rand(2) = round(rand() * mapY);  
    if p_rand(1) < 1
       p_rand(1) = 1;
    else if p_rand(1) > mapX
       p_rand(1) = mapX;
        end
    end
    if p_rand(2) < 1
       p_rand(2) = 1;
    else if p_rand(2) > mapY
       p_rand(2) = mapY;
       end
    end         
end

遍历树，从树中找到最近邻近点p_near：

p_near=[];
%Step 2: 遍历树，从树中找到最近邻近点p_near 
min_distance = 1000;
index = 1;  %index先指向根节点
for i = 1:count
    distance = sqrt( ( Tree.v(i).x - p_rand(1) )^2 + ( Tree.v(i).y - p_rand(2) )^2 );
    if distance < min_distance
        min_distance = distance;
        index = i;
    end
end
p_near(1) = Tree.v(index).x; %保存距离随机点距离最近的点
p_near(2) = Tree.v(index).y;

由距离随机点最近的点向随机点已Delta的补偿扩展得到p_new节点，并且检测p_new节点是否在障碍物上：

p_new=[];
%Step 3: 由距离随机点最近的点向随机点已Delta的补偿扩展得到p_new节点
Delta = 10;  %步长设置为10
p_new(1) = p_near(1) + round( ( p_rand(1) - p_near(1) ) * Delta/min_distance );  %求出扩展一个步长后的坐标
p_new(2) = p_near(2) + round( ( p_rand(2) - p_near(2) ) * Delta/min_distance );
%防止坐标计算到负数造成错误
if p_new(1) < 1
    p_new(1) = 1;
else if p_new(1) >= mapX
    p_new(1) = mapX - 1;     
    end
end
if p_new(2) < 1
    p_new(2) = 1;
else if p_new(2) >= mapY
    p_new(2) = mapY;           
    end
end

%检查p_new节点是否触碰到障碍物  因为是二值化图像，白色为1，黑色为0 黑色即为障碍物
%注意p_new(1)代表的是列x，p_new(2)是行y，因此判断的时候需要写map(p_new(2),p_new(1)) 
%而不是map(p_new(1),p_new(2))    
if map(p_new(2),p_new(1)) ~= 1  
   continue;  %舍弃本次采样点，跳出本次循环
end
count = count + 1;  %采样点可用时，继续后面循环

将新点插入到整个树中：

%Step 4: 将p_new插入树   
Tree.v(count).x = p_new(1);         
Tree.v(count).y = p_new(2); 
Tree.v(count).xFather = p_near(1);    
Tree.v(count).yFather = p_near(2);
Tree.v(count).dist = min_distance; 

检查是否到达目标点附近：

new_distance = sqrt( ( p_new(1) - end_x )^2 + ( p_new(2) - end_y )^2 );  %计算新点相对终点的距离，小于一定阈值则判定到达终点
if new_distance <= ArriveDis
    plot(p_new(1), p_new(2), 'bo', 'MarkerSize',2, 'MarkerFaceColor','b'); % 绘制x_new
    line( [p_new(1) p_near(1)], [p_new(2) p_near(2)], 'Marker','.','LineStyle','-'); %连接x_near和x_new
    line( [end_x p_new(1)], [end_y p_new(2)], 'Marker','.','LineStyle','-'); %连接x_Target和x_new
    break;
end

将各个路径画出来，并且在while循环中增加一个延时，这样可以让算法搜索路径的过程更好的显示出来：

%Step 6:将x_near和x_new之间的路径画出来
plot(p_new(1), p_new(2), 'bo', 'MarkerSize',2, 'MarkerFaceColor','b'); % 绘制x_new
line( [p_new(1) p_near(1)], [p_new(2) p_near(2)], 'Marker','.','LineStyle','-'); %连接x_near和x_new
hold on;     %要多次在同一张图上绘制线段，所以使用plot后需要接上hold on命令

pause(0.01); %暂停时间

之后从终点开始逆序寻找各个父节点就可以将整个路径画出来：

%一直检测到坐标点是起始点坐标
while path(path_count,1) ~= init_x || path(path_count,2) ~= init_y
    %从终点逆序一直寻找当前点的父节点，就可以最终找到所有节点的编号
    i2=1;
    while ( Result_LIST(i2,1) ~= path(path_count,1) || Result_LIST(i2,2) ~= path(path_count,2) )
        i2 = i2 + 1;
    end
    new_n_index = i2;   
        
    path_count = path_count + 1;
    %将此节点的父节点保存下来
    path(path_count,1) = Result_LIST(new_n_index,3);
    path(path_count,2) = Result_LIST(new_n_index,4);
    n_index = new_n_index;
end

上述的代码展示出了关键步骤的代码，不过RRT算法也有其缺点，一个是其找到的路径只是可行路径，并不是最优路径，另外的，当地图很复杂的时候，RRT算法就容易陷入死循环而出不去，就像这样：