RRT*-Smart 路径规划matlab实现

以下说明来自知乎GentleGamer​:

RRT*-Smart在RRT*的基础上做了改进，主要是优化了路径。通过RRT和RRT*探索出的路径往往是曲曲折折有些小波浪的（毕竟节点是随机生成的），但事实上上在空地中的最佳路径一般是直线。RRT*-Smart在运行的前一个阶段与RRT*完全一致，但是找到一条可行的从起点到终点的路径之后它就开始考虑优化路径，化曲为直。这个过程实际上就是从叶子节点开始，不断寻找能否无障碍地直接连接到先辈节点。如果直接往前连一层就多一条直线，少一段曲线。（为了增加运算速度，不妨直接把障碍物都视为矩形的）在化曲为止的过程中，我们能找到几个锚点，这些点往往是在障碍物附近的，它们无法帮助子孙们直接优化。

在之后的循环中，我们加入Smart采样的方式，而不全是随机采样，也就是在这些锚点附近增加采样，从而更有可能找到最优的路径。RRT*-Smart的规划效果下图(b)[1]。

知乎连接:

RRT,RRT*和RRT*-Smart - 知乎

代码下载:

https://github.com/howard789/RRT_star_smart

主要参考代码:

GitHub - adnanmunawar/matlab-rrt-variants: RRT*, RRT-connect, lazy RRT and RRT extend have been implemented for 2d and 3d c-spaces with visualization

运行结果

 

 

 main.m

clc;clear all;close all;
addpath(genpath('utils'),genpath('world'))
fixed_env=1;

%% set search range 
search_range=[250 250 250];

%% set start_point and goal

start_point=[10 10 10];
goal=[150 150 150];

%% create world with obstacle
if fixed_env
    
    [obstacles,min_obs_radius] =  general_fixed_obstacles();
    
else
    obstacle_num=5;
    min_obs_radius=30;
    obstacles = generate_obstacles(obstacle_num,min_obs_radius,search_range,start_point,goal);
end
%% find best path with rrt*-smart
step_length=10;
max_fail_attemps=500;
target_path_num=1;
[path_RRTstar,path_RRTsmart,tree,treeS] = rrt_start_smart(start_point,goal,search_range,obstacles,min_obs_radius,step_length,max_fail_attemps,target_path_num);

%% plot world
plot_world(obstacles,start_point,goal,path_RRTstar,path_RRTsmart,tree,treeS);

rrt_start_smart.m

function [path_RRTstar,path_RRTsmart,tree,treeS] = rrt_start_smart(start_point,goal,search_range,obstacles,min_obs_radius,step_length,max_fail_attemps,target_path_num)
     
     % (1) x  
     % (2) y
     % (3) z
     % (4) parent_idx
     % (5) total_distance_to_start_node
     % (6) is_lastest_node (one step before before_goal)
     % (7) is_via_node
     % (8) id 
     
     % a point contains only (x,y,z) 
     %a node contains all data including a point
     
     start_node=[start_point 0 0 0 0 1];
     end_node=[goal -1 -1 0 0 -1];
     tree =[start_node];
     if norm(start_point-goal)<=step_length
         if link_points_valid(new_point,parent_idx,tree,obstacles,min_obs_radius)
            path=[1;2];
         else
             path = [];
         end
     else
         fail_attemps = 0;
         path_num=0;
         % extend tree
         while 1

             [valid,is_last,tree]=extend_tree(tree,goal,min_obs_radius,obstacles,step_length,search_range);
             if is_last
                 path_num = path_num+1;             
             end
             
             if valid
                 fail_attemps =0;
             else
                fail_attemps = fail_attemps+1;      
             end

             %end
             if fail_attemps > max_fail_attemps
                 break
             end

             if path_num >= target_path_num
                %update end_node id
                 end_node(8)=size(tree,1)+1;
                 tree=[tree;end_node];
                 break
             end
         end

         % update_end_node_parent
         tree = update_end_node_parent (tree);
         path_RRTstar = get_path_from_tree(tree);
         

         
         % straighten best path
         treeS=tree(:,:);
         treeS = straighten_path(treeS,obstacles,min_obs_radius);
         path_RRTsmart = get_path_from_tree(treeS);

     end
end


function [valid,is_last,tree]=extend_tree(tree,goal,min_obs_radius,obstacles,step_length,search_range)
    is_last=0;

    % find new point and check whether it is valid
    [valid,new_point,parent_idx,min_total_dist] = find_new_point(goal,tree,obstacles,min_obs_radius,step_length,search_range);

    % update its parent and near points' (major difference between rrt* and rrt)
    if valid 
        [parent_idx,min_total_dist,near_points] = find_better_parent_idx(new_point,min_total_dist,parent_idx,min_obs_radius,obstacles,step_length,tree);
    end
    
    % add new point to the tree if valid
    if valid
        [new_node_idx,is_last,tree] = add_node(new_point,parent_idx,min_total_dist,tree,goal,step_length);
    end
    
    if valid
        tree = update_new_node_parent_idx(tree,new_node_idx,step_length,min_total_dist,parent_idx,min_obs_radius,obstacles,near_points);
    end
    

    
    
    
end

function [valid,new_point,parent_idx,min_total_dist] = find_new_point(goal,tree,obstacles,min_obs_radius,step_length,search_range)
    %random point
     if rand <0.5
        sample_point = rand(1,3).*search_range;
     else
        sample_point =goal;
     end

    %find the node closest to new_point_tmp
     distance = cal_leafs_to_point_distance(tree,sample_point);
     [~,parent_idx]=min(distance);
     closest_point = tree(parent_idx,1:3);
     
     % get new_point with step_length
     moving_directions = [sample_point(1)-closest_point(1) sample_point(2)-closest_point(2) sample_point(3)-closest_point(3)];
     moving_directions = moving_directions/sqrt(sum(moving_directions.^2));
     new_point = closest_point + step_length*moving_directions;
     new_point_line_dist = norm(new_point-closest_point);
     
     
     if new_point_valid(new_point,obstacles,search_range) && link_points_valid(new_point,parent_idx,tree,obstacles,min_obs_radius)
         min_total_dist=tree(parent_idx,5)+new_point_line_dist;
         valid=1;
     else
         min_total_dist=0;
         valid=0;
     end

end


function [parent_idx,min_total_dist,near_points] = find_better_parent_idx(new_point,min_total_dist,parent_idx,min_obs_radius,obstacles,step_length,tree)
    %find near points
    search_radius=step_length*2;
    
    dist_mat = cal_leafs_to_point_distance(tree,new_point);
    near_points = find(dist_mat <= search_radius);
    
    for i = 1:length(near_points)
        idx = near_points(i);
        
        leaf_point = tree(idx,1:3);
        total_dist = tree(idx,5) + norm(leaf_point-new_point);
       if total_dist<min_total_dist && link_points_valid(new_point,parent_idx,tree,obstacles,min_obs_radius)
           min_total_dist=total_dist;
           parent_idx=idx;
       end
        
    end  


end



function tree=update_new_node_parent_idx(tree,new_node_idx,step_length,min_total_dist,parent_idx,min_obs_radius,obstacles,near_points)
    % nodes close to the new node could get a shorter path with new node as its parent
    total_dist_of_new_point=tree(new_node_idx,5);
    new_point = tree(new_node_idx,1:3);
    for i = 1:length(near_points)
        idx = near_points(i);
        near_point = tree(idx,1:3);
        pre_total_dist = tree(idx,5);
        new_total_dist = norm(near_point-new_point)+total_dist_of_new_point;
        
       if new_total_dist<pre_total_dist && link_points_valid(new_point,parent_idx,tree,obstacles,min_obs_radius)
           tree(idx,4)=new_node_idx; %parent
           tree(idx,5)=new_total_dist; %total_dist
           tree(idx,6)=0; % it is not the lastest anymore if it was
       end
    end

end

function [idx,is_last,tree] = add_node(new_point,parent_idx,min_total_dist,tree,goal,step_length)
  %add new node to tree
    if found_last_node_before_goal(new_point,goal,step_length)
        is_last = 1;
    else
        is_last =0;
    end
    id = size(tree,1)+1;
    new_node = [new_point parent_idx min_total_dist is_last 0 id];
    tree = [tree;new_node];  
    idx=size(tree,1);
end




function valid = new_point_valid(new_point,obstacles,search_range)
    valid = 1;
    done = 0;
    
    if valid && out_of_range(new_point,search_range)
        valid=0;
        done =1;
    end
    
    if valid && will_collide(new_point,0,obstacles)
        valid=0;
        done =1;
    end
end
    
    
function valid = link_points_valid(new_point,parent_idx,tree,obstacles,min_obs_radius)
    done =0;
    valid =1;

  parent_point = tree(parent_idx,1:3);     
  if ~done && ~vaild_path(new_point,parent_point,obstacles,min_obs_radius)
        valid=0;             
        done =1;
  end
  
   
end




function distance = cal_leafs_to_point_distance(tree,point)
    a = tree(:,1:3);
    b = ones(size(tree,1),1)*point;
    diff = a-b;
    sqrt_diff = diff.*diff;
    sum=zeros(size(sqrt_diff,1),1);
    
    for i =1:3
        sum=sum+sqrt_diff(:,i);
    end
    
    distance=sum.^0.5;
   
end

function result = found_last_node_before_goal(new_point,goal,step_length)
    distance = norm(new_point-goal);
    if distance <= step_length
        result = 1;
    else
        result = 0;
    end
end

function tree = update_end_node_parent (tree)
    candidates=[];
    for idx = 1:size(tree,1)
        if tree(idx,6)==1
            candidates = [candidates;tree(idx,:)];
        end
    end
    
    if size(candidates,1)>0    
        %find the last_node (not goal)  with min cost
        [~,idx]=min(candidates(:,5),[],2);
        winner_id=candidates(idx,8);
        
        %update end_node data
        end_node_id=size(tree,1);
        tree(end_node_id,4) = winner_id;
        tree(end_node_id,5) = norm(tree(end_node_id,1:3)-tree(winner_id,1:3)) + tree(winner_id,5);
    end
end

function path = get_path_from_tree(tree)
    end_node_idx=size(tree,1);
    %put last_node to path
        path=[end_node_idx];
        parent_idx=tree(end_node_idx,4);
        % add points
        while parent_idx > 0
            path=[parent_idx;path];
            parent_idx=tree(parent_idx,4);
        end

end


function tree = straighten_path(tree,obstacles,min_obs_radius)

    end_node_idx=size(tree,1);

    while 1
        jump_step=2;
        
        current_idx=end_node_idx;
        success_num=0;
        while 1 
           parent_idx = get_parent_idx(tree,current_idx,jump_step);
           if parent_idx ==0
                break;
           end 
           
           new_point=tree(current_idx,1:3);
           if link_points_valid(new_point,parent_idx,tree,obstacles,min_obs_radius)
               tree(current_idx,4)=parent_idx;
               current_idx=parent_idx;
               jump_step=jump_step*2;
               success_num=success_num+1;
           elseif jump_step>2
               jump_step=max(floor(jump_step/2),2);
           else
               current_idx = tree(current_idx,4);
           end

        end
        
        if success_num ==0
            break;
        end
        
    end
end


function idx = get_parent_idx(tree,idx,jump_step)
    if idx > 0
        for i = 1:jump_step
           idx= tree(idx,4);
           if idx ==0
               break;
           end
        end
    end
    
end

will_collide.m

function result = will_collide(point,r,obstacles)
    
    
    if r>0
        test_num=19;
        points = [];
        % generate a boll 创建一个以point为中心的球体
        [x,y,z]=sphere(test_num);
        for i = 1:length(x(1,:))
             for j = 1:length(x(:,1))
                 new_point=[x(i,j)*r+point(1) y(i,j)*r+point(2) z(i,j)*r+point(3)];
                 points= [points;new_point];
             end
        end
    else
        points=point;
    end

    result = 0;
    if ~isempty(obstacles)
        for obstacle_idx =1:length(obstacles(:,1))
            obstacle = obstacles(obstacle_idx,:);
            for row = 1:length(points(:,1))
                point=points(row,:);  
                distance_to_center = norm(obstacle(1:3)-point);
                if distance_to_center <= obstacle(4)
                    result = 1;
                    break;
                end   
            end
        end
    end
end

vaild_path.m

function valid = vaild_path(point1,point2,obstacles,min_obs_radius)
    valid = 1;
    num = floor(norm(point1-point2)/(min_obs_radius/2));
    x_step = (point2(1)-point1(1))/num;
    y_step = (point2(2)-point1(2))/num;
    z_step = (point2(3)-point1(3))/num;
    done = 0;
    for i = 1:num
        if ~done
            point = [point1(1)+x_step*i point1(2)+y_step*i point1(3)+z_step*i] ;
            if will_collide(point,0,obstacles)
                valid=0;
                done =1;
            end
        end
     end
        
end

out_of_range.m

function result = out_of_range(point,search_range)
    result =0;
    if point(1)>search_range(1) || point(1)<0 || ...
       point(2)>search_range(2) || point(2)<0 || ...
       point(3)>search_range(3) || point(3)<0
       result = 1;
    end
end

plot_world.m

function plot_world(obstacles,start_point,goal,path_RRTstar,path_RRTsmart,tree,treeS)
    %obstacle_centers 是障碍物的中心点 x y z r(radius)
    [x,y,z]=sphere(); %创造一个球面,中心点是0
    for i = 1:length(obstacles(:,1))
        %调整位置和大小
        X=x*obstacles(i,4)+obstacles(i,1);
        Y=y*obstacles(i,4)+obstacles(i,2);
        Z=z*obstacles(i,4)+obstacles(i,3);
        mesh(X,Y,Z);
        hold on;
    end
    scatter3(start_point(1),start_point(2),start_point(3),'filled','g');
    scatter3(goal(1),goal(2),goal(3),'filled','b');
    xlabel('x-axis'),ylabel('y-axis'),zlabel('z-axis');
    title('rrt*-smart');
    grid on;
    axis equal;
    
    %plot tree
    if size(tree,1)>1
       plot_tree(tree);
    end
    
    %plot best path
    
    if length(path_RRTstar)>0
        end_node=tree(size(tree,1),:);
        plot_path(end_node,tree,3,'b');
    end
    
    if length(path_RRTsmart)>0
        end_node=treeS(size(treeS,1),:);
        plot_path(end_node,treeS,3,'r');
    end
    
    
end


function plot_tree(tree)
    idx = size(tree,1);
    while idx>1        
        node = tree(idx,:);
        plot_path(node,tree,0.5,'g');
        idx = idx - 1;
    end
end


function plot_path(end_node,tree,line_width,line_color)
    %end_node
    branch = [end_node];
    parent_idx=end_node(4);
    while parent_idx>0
       node = tree(parent_idx,:);
       branch = [node;branch];
       parent_idx=node(4);
    end
    if size(branch,1)>1
        X = branch(:,1);
        Y = branch(:,2);
        Z = branch(:,3);
        p = plot3(X,Y,Z);
        set(p,'Color',line_color,'LineWidth',line_width,'Marker','.','MarkerEdgeColor',line_color);
        hold on;
    end
    
end

generate_obstacles.m

function obstacles = generate_obstacles(obstacle_num,min_obs_radius,search_range,start_point,goal)
    max_iterate=500;
    %创建圆形障碍物
    obstacles=[];
    % if min_world_range=250
    % x+r>0 => x>r
    % x+r<250 => x<250-r
    % x = rand*(250-r)+r
    iterate = 0;
    
    while 1
        r=rand*(min(search_range)/2);
        if r<min_obs_radius
            r=min_obs_radius;
        end
        
        
        xyz_edge=min(search_range)-r;
        x=rand*xyz_edge;
        y=rand*xyz_edge;
        z=rand*xyz_edge;
        point=[x,y,z];
        if ~will_collide(point,r,obstacles) && ~will_collide(start_point,0,obstacles) && ~will_collide(goal,0,obstacles)
 
            new_data=[x,y,z,r];
            obstacles=[obstacles;new_data];
            if length(obstacles(:,1))==obstacle_num
                break
            end
            
        end
        
        iterate = iterate+1;
        if iterate>max_iterate
           break
        end

    end
end

general_fixed_obstacles.m

function  [obstacles,min_obs_radius] =  general_fixed_obstacles()
    centers =[125,125,50;];
    r=[100];
    obstacles=zeros(length(r),4);
    for i = 1:length(r)
        obstacles(i,:)=[centers(i,:) r(i)];
    end
    min_obs_radius = min(r);
end