基于matlab的RRT算法实现

一.基于采样的路径规划

        1.内容来源：深蓝学院课程

二. RRT( Rapidly-exploring Random Tree)

1.RRT的基本思想：

RRT的基本思想是通过随机抽样来增长一棵树，树的每一个节点代表一个可能的机器人位置。每次迭代，RRT都会随机选择一个点并试图连接到现有的树中，使得树逐渐扩展到整个搜索空间 

2.RRT的基本步骤如下：

1. 初始化：开始时，只有起始点在树中。
2. 随机抽样：在搜索空间中随机选择一个点，称为随机点(x_rand)。
3. 查找最近邻：遍历树中距离随机点最近的点，称为最近邻点(x_near)。
4. 扩展：从最近邻点朝随机点的方向扩展一个固定的小距离，产生一个新的点(x_new)。
5. 连接：将新点添加到树中，并连接到最近邻点。
6. 终止条件：当达到某个终止条件时（例如树达到一定大小、找到了终点或执行了一定次数的迭代），算法结束。
7. 返回路径：如果找到了一条从起点到终点的路径，返回这条路径。

伪代码 ：

 3.RRT的主要步骤：

        3.1树的结构T.v： 

 3.2 得到x_near

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

function [x_near ,ind_x_near] = Near(x_rand,T)
%在树中搜索离x_rand最近的点，并返回 x_near 的坐标以及 索引值
    len = size(T.v,2) ;
    dis_tmp = 10000 ;
    for node = 1:len
        get_dist = sqrt( ((T.v(node).x - x_rand(1))^2) + ((T.v(node).y - x_rand(2))^2));
        if(get_dist < dis_tmp)
            dis_tmp = get_dist ;
            
            x_near(1) = T.v(node).x;
            x_near(2) = T.v(node).y;
            ind_x_near = node ; 
        end
    end
end

3.3 扩展得到x_new

function x_new = Steer(x_rand,x_near,Stepsize)
        %使用线性插值的办法得到x_new
        dist = DIST(x_rand,x_near);
        x_new(1) =((dist - Stepsize)*x_near(1) + Stepsize*x_rand(1) )/ dist ;
        x_new(2) =((dist - Stepsize)*x_near(2) + Stepsize*x_rand(2) )/ dist ;
end

线性插值 （来源于gpd）

3.4检查是否是collision-free 

function feasible=collisionChecking(startPose,goalPose,map)
feasible=true;
dir=atan2(goalPose(1)-startPose(1),goalPose(2)-startPose(2));
for r=0:0.5:sqrt(sum((startPose-goalPose).^2))
    posCheck = startPose + r.*[sin(dir) cos(dir)];
    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
end
if ~feasiblePoint([floor(goalPose(1)),ceil(goalPose(2))],map), feasible=false; end

function feasible=feasiblePoint(point,map)
feasible=true;
if ~(point(1)>=1 && point(1)<=size(map,2) && ... % x in map
     point(2)>=1 && point(2)<=size(map,1) && ... % y in map
     map(point(2),point(1))==255) % x,y is Free
    feasible=false;
end

3.5 将x_new 插入树中 

function T = AddNode(T,x_new,x_near,near_indx)
    count = size(T.v,2)+1 ;
    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 =  DIST(x_new,x_near) + T.v(near_indx).dist ; 
    T.v(count).indPrev = near_indx ; 

end

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

 dist_goal =  sqrt(power(x_new(1)-x_G,2) + power(x_new(2) - y_G, 2) );
    if(dist_goal <Thr)
        bFind = true ; 
        break
    end

3.7 路径返回

%% 路径已经找到，反向查询
if bFind
    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
else
    disp('Error, no path found!');
end

3.8 结果 

 

RRT的优点：

• 能够快速地探索大的搜索空间。
• 能够找到复杂环境中的路径。
• 对于高维空间也能够工作。

RRT的缺点：

• 产生的路径可能不是最优的。
• 可能需要很多迭代才能找到一个解决方案