RRT算法学习及MATLAB演示

文章目录

  • [1 前言](#1 前言)
  • [2 算法简介](#2 算法简介)
  • [3 MATLAB实现](#3 MATLAB实现)
    • [3.1 定义地图](#3.1 定义地图)
    • [3.2 绘制地图](#3.2 绘制地图)
    • [3.3 定义参数](#3.3 定义参数)
    • [3.4 绘制起点和终点](#3.4 绘制起点和终点)
    • [3.5 RRT算法](#3.5 RRT算法)
      • [3.5.1 代码](#3.5.1 代码)
      • [3.5.2 效果](#3.5.2 效果)
      • [3.5.3 代码解读](#3.5.3 代码解读)
  • [4 参考](#4 参考)
  • [5 完整代码](#5 完整代码)

1 前言

RRT(Rapid Random Tree)算法,即快速随机树算法,是LaValle在1998年首次提出的一种高效的路径规划算法。RRT算法以初始的一个根节点,通过随机采样的方法在空间搜索,然后添加一个又一个的叶节点来不断扩展随机树。当目标点进入随机树里面后,随机树扩展立即停止,此时能找到一条从起始点到目标点的路径。

两个代码文件见最后一节。

2 算法简介

效果预览图

算法的计算过程如下:

step1:初始化随机树。将环境中起点作为随机树搜索的起点,此时树中只包含一个节点即根节点;

stpe2:在环境中随机采样。在环境中随机产生一个点,若该点不在障碍物范围内则计算随机树中所有节点到的欧式距离,并找到距离最近的节点,若在障碍物范围内则重新生成并重复该过程直至找到;

stpe3:生成新节点。在和连线方向,由指向固定生长距离生成一个新的节点,并判断该节点是否在障碍物范围内,若不在障碍物范围内则将添加到随机树 中,否则的话返回step2重新对环境进行随机采样;

step4:停止搜索。当和目标点之间的距离小于设定的阈值时,则代表随机树已经到达了目标点,将作为最后一个路径节点加入到随机树中,算法结束并得到所规划的路径 。

3 MATLAB实现

3.1 定义地图

地图是模拟的栅格地图,resolution表示每个格子的长度,这里设置为1,地图范围为 x ∈ [ − 15 , 15 ] x\in[-15,15] x∈[−15,15], y ∈ [ − 15 , 15 ] y\in[-15,15] y∈[−15,15]。障碍物的形状为矩形,定义方式为矩形的左下角坐标及其水平长度和竖直长度。wall_obstacle位于地图边界,block_obstacle位于地图内部。

matlab 复制代码
%% Define the map
resolution = 1; % resolution, cell length

% Map boundaries
left_bound = -15;
right_bound = 15;
lower_bound = -15;
upper_bound = 15;

% Wall obstacle [left_down_x,left_down_y,horizontal_length,vertical_length]
wall_obstacle(1,:) = [   left_bound,   lower_bound,                        1, upper_bound-lower_bound-1]; % left boundary
wall_obstacle(2,:) = [ left_bound+1,   lower_bound, right_bound-left_bound-1,                         1]; % bottom boundary
wall_obstacle(3,:) = [right_bound-1, lower_bound+1,                        1, upper_bound-lower_bound-1]; % right boundary
wall_obstacle(4,:) = [   left_bound, upper_bound-1, right_bound-left_bound-1,                         1]; % up boundary

% Blcok obstacle [left_down_x,left_down_y,horizontal_length,vertical_length]
block_obstacle(1,:) = [0,-10,10,5]; % block obstacle 1
block_obstacle(2,:) = [-5,5,5,9]; % block obstacle 2
block_obstacle(3,:) = [-5,-2,5,4]; % block obstacle 3

ob = [block_obstacle; wall_obstacle];

3.2 绘制地图

matlab 复制代码
%% Draw the map
figure(1); % create a figure

% Figure setting
set(gca,'XLim',[left_bound right_bound]); % x axis range
set(gca,'XTick',[left_bound:resolution:right_bound]); % x axis tick
set(gca,'YLim',[lower_bound upper_bound]); % y axis range
set(gca,'YTick',[lower_bound:resolution:upper_bound]); % y axis tick
grid on
axis equal
title('RRT');
xlabel('x');
ylabel('y');

hold on

% Draw the obstacles
for i=1:1:size(ob,1)
    fill([ob(i,1),ob(i,1)+ob(i,3),ob(i,1)+ob(i,3),ob(i,1)],...
         [ob(i,2),ob(i,2),ob(i,2)+ob(i,4),ob(i,2)+ob(i,4)],'k');
end

结果如下图所示,这里用红框标出了wall_obstacle,用绿色数字表示block_obstacle。

3.3 定义参数

grow_distance指新生长出的节点与其父节点的距离,这里设为1;goal_distance指的是如果新生长出的节点落在这个范围里,则认为已经到达终点;goal的位置设为 [ − 10 , − 10 ] [-10,-10] [−10,−10],start的位置设为 [ 13 , 10 ] [13,10] [13,10]。

matlab 复制代码
%% Initialize parameters
grow_distance = 1; % distance between parent node and the derived child node
goal_radius = 1.5; % can be considered as reaching the goal once within this range
% Goal point position
goal.x = -10;
goal.y = -10;
% Start point position
start.x = 13;
start.y = 10;

3.4 绘制起点和终点

matlab 复制代码
%% Draw the start point and the end point
h_start = plot(start.x,start.y,'b^','MarkerFaceColor','b','MarkerSize',5*resolution);
h_goal = plot(goal.x,goal.y,'m^','MarkerFaceColor','m','MarkerSize',5*resolution);

% Draw the goal circle
theta = linspace(0,2*pi);
goal_circle.x = goal_radius*cos(theta) + goal.x;
goal_circle.y = goal_radius*sin(theta) + goal.y;
plot(goal_circle.x,goal_circle.y,'--k','LineWidth',0.8*resolution);

3.5 RRT算法

这一部分主要是用于演示RRT算法是怎么建树,怎么到达给定终点,侧重于展示RRT的思想,如果用于工程实现,则需要用C++等高级语言重写,并且使用严谨的数据结构。

3.5.1 代码

注意需要另外写一个函数find_closet_node.m

matlab 复制代码
function [angle,min_idx] = find_closet_node(rd_x,rd_y,tree)
    distance = [];
    i = 1;
    while i <= length(tree.child) % should not use size() function
        dx = rd_x - tree.child(i).x;
        dy = rd_y - tree.child(i).y;
        distance(i) = sqrt(dx^2 + dy^2);
        i = i+1;
    end
    [~,min_idx] = min(distance);
    angle = atan2(rd_y-tree.child(min_idx).y, rd_x-tree.child(min_idx).x);
end

下面的代码承接3.4节即可

matlab 复制代码
%% RRT Algorithm
% Initialize the random tree(in the form of struct)
tree.child = []; % current node
tree.parent = []; % current node's parent
tree.distance = []; % current node's distance to the start

tree.child = start;
tree.parent = start;
tree.distance = 0;

new_node.x = start.x;
new_node.y = start.y;

goal_distance = sqrt((goal.x - new_node.x)^2 + (goal.y - new_node.y)^2);

% Main loop
while goal_distance > goal_radius
    random_point.x = (right_bound - left_bound) * rand() + left_bound; % random x value between x limit
    random_point.y = (upper_bound - lower_bound) * rand() + lower_bound; % random y value between y limit
    handle_1 = plot(random_point.x,random_point.y,'p','MarkerEdgeColor',[0.9290 0.6940 0.1250],'MarkerFaceColor',[0.9290 0.6940 0.1250],'MarkerSize',8*resolution); % draw the randomly generated point
    [angle,min_idx] = find_closet_node(random_point.x,random_point.y,tree);
    
    % pause(0.5)
    handle_2 = plot([tree.child(min_idx).x,random_point.x],[tree.child(min_idx).y,random_point.y],'-','Color',[0.7 0.7 0.7],'LineWidth',0.8*resolution); % draw the segment between the closest tree node and the randomly generated point
    
    % pause(0.5)
    new_node.x = tree.child(min_idx).x + grow_distance*cos(angle);
    new_node.y = tree.child(min_idx).y + grow_distance*sin(angle);
    handle_3 = plot(new_node.x,new_node.y,'.r','MarkerFaceColor','r','MarkerSize',10*resolution); % draw the potential new node
    
    flag = 1; % default: valid node

    % Judge if the new node is inside the obstacle
    step = 0.01;
    if new_node.x < tree.child(min_idx).x
        step = -step;
    end
    for k=1:1:size(ob,1)
        for i=tree.child(min_idx).x:step:new_node.x
            if angle>pi/2-5e-02 && angle<pi/2+5e-02
                j = tree.child(min_idx).y+1;
            elseif angle>-pi/2-5e-02 && angle<-pi/2+5e-02
                j = tree.child(min_idx).y-1;
            else
                j=tree.child(min_idx).y+(i-tree.child(min_idx).x)*tan(angle);
            end
            if i>=ob(k,1) && i<=(ob(k,1)+ob(k,3))
                if j >=ob(k,2) && j<=ob(k,2)+ob(k,4)
                    flag = 0; % invalid node
                    break
                end
            end
        end
        if flag==0
            break
        end
    end

    % pause(0.5)
    if flag==1
        tree.child(end+1) = new_node;
        tree.parent(end+1) = tree.child(min_idx);
        tree.distance(end+1) = 1 + tree.distance(min_idx);
        goal_distance = sqrt((goal.x - new_node.x)^2 + (goal.y - new_node.y)^2);
        delete(handle_3)
        plot(new_node.x,new_node.y,'.g','MarkerFaceColor','g','MarkerSize',10*resolution); % draw the new node
        % pause(0.2)
        plot([tree.child(min_idx).x,new_node.x],[tree.child(min_idx).y,new_node.y],'-k','LineWidth',0.8*resolution); % draw the segment between the closest tree node and the new node
    end
    
    % pause(0.5)
    delete(handle_1);
    delete(handle_2);
    % pause(0.5)
end

3.5.2 效果

3.5.3 代码解读

  • 首先是初始化一个tree结构体,含有child, parent, distance三个成员,三者均为列表。child用于存储所有节点,在相同索引位置,parent存储child的父节点,distance存储child到起点的距离(沿着树的距离,不是直线距离)。然后对这三个成员进行初始化。

    matlab 复制代码
    % Initialize the random tree(in the form of struct)
    tree.child = []; % current node
    tree.parent = []; % current node's parent
    tree.distance = []; % current node's distance to the start
    
    tree.child = start;
    tree.parent = start;
    tree.distance = 0;
  • 定义全局变量,new_node,用于存储新衍生出来的节点,用起点对其初始化。

    定义全局变量,goal_distance,用于存储new_node到终点的距离。

    matlab 复制代码
    new_node.x = start.x;
    new_node.y = start.y;
    
    goal_distance = sqrt((goal.x - new_node.x)^2 + (goal.y - new_node.y)^2);
  • 进入主循环,只要new_node尚未到达终点范围,则循环继续。

    • 每个循环中,在地图范围内生成一个随机点,然后找到距离该随机点最近的树上的节点(借助自定义函数find_closet_node实现),返回该点的索引,以及这两点连线的角度。【生成的随机点用黄色五角星表示】【随机点与最近的树上节点的连线用灰色表示】

      matlab 复制代码
      random_point.x = (right_bound - left_bound) * rand() + left_bound; % random x value between x limit
      random_point.y = (upper_bound - lower_bound) * rand() + lower_bound; % random y value between y limit
      handle_1 = plot(random_point.x,random_point.y,'p','MarkerEdgeColor',[0.9290 0.6940 0.1250],'MarkerFaceColor',[0.9290 0.6940 0.1250],'MarkerSize',8*resolution); % draw the randomly generated point
      [angle,min_idx] = find_closet_node(random_point.x,random_point.y,tree);
      
      % pause(0.5)
      handle_2 = plot([tree.child(min_idx).x,random_point.x],		[tree.child(min_idx).y,random_point.y],'-','Color',[0.7 0.7 0.7],'LineWidth',0.8*resolution); % draw the segment between the closest tree node and the randomly generated point
      matlab 复制代码
      function [angle,min_idx] = find_closet_node(rd_x,rd_y,tree)
          distance = [];
          i = 1;
          while i <= length(tree.child) % should not use size() function
              dx = rd_x - tree.child(i).x;
              dy = rd_y - tree.child(i).y;
              distance(i) = sqrt(dx^2 + dy^2);
              i = i+1;
          end
          [~,min_idx] = min(distance);
          angle = atan2(rd_y-tree.child(min_idx).y, rd_x-tree.child(min_idx).x);
      end
    • 在这两点连线上,生成一个新节点,新节点与树上的节点距离为1,默认该节点是有效的,也即不会与障碍物干涉的。【新节点用红色实心点表示】

      matlab 复制代码
      	% pause(0.5)
          new_node.x = tree.child(min_idx).x + grow_distance*cos(angle);
          new_node.y = tree.child(min_idx).y + grow_distance*sin(angle);
          handle_3 = plot(new_node.x,new_node.y,'.r','MarkerFaceColor','r','MarkerSize',10*resolution); % draw the potential new node
          
          flag = 1; % default: valid node
    • 然后判断生成的新节点与树上节点的连线上的点是否位于障碍物内,也即判断新节点是否会导致路径与障碍物干涉,如果发生干涉,则把flag设置为0。

      matlab 复制代码
      % Judge if the new node is inside the obstacle
      step = 0.01;
      if new_node.x < tree.child(min_idx).x
          step = -step;
      end
      for k=1:1:size(ob,1)
          for i=tree.child(min_idx).x:step:new_node.x
              if angle>pi/2-5e-02 && angle<pi/2+5e-02
                  j = tree.child(min_idx).y+1;
              elseif angle>-pi/2-5e-02 && angle<-pi/2+5e-02
                  j = tree.child(min_idx).y-1;
              else
                  j=tree.child(min_idx).y+(i-tree.child(min_idx).x)*tan(angle);
              end
              if i>=ob(k,1) && i<=(ob(k,1)+ob(k,3))
                  if j >=ob(k,2) && j<=ob(k,2)+ob(k,4)
                      flag = 0; % invalid node
                      break
                  end
              end
          end
          if flag==0
              break
          end
      end
    • 如果没有发生干涉,则将该点加入child列表,并将上一个点加入parent列表,该点距离起点的距离等于grow_distance加上上一个点距离起点的距离。【如果新节点在可行区域,则将该节点画为绿色】【该可行新节点与上一个节点的连线为黑色】【擦除之前生成的五角星随机点】【擦除之前五角星随机点与树上节点的连线】

      matlab 复制代码
      % pause(0.5)
      if flag==1
      	 tree.child(end+1) = new_node;
      	 tree.parent(end+1) = tree.child(min_idx);
      	 tree.distance(end+1) = grow_distance + tree.distance(min_idx);
      	 goal_distance = sqrt((goal.x - new_node.x)^2 + (goal.y - new_node.y)^2);
      	 delete(handle_3)
      	 plot(new_node.x,new_node.y,'.g','MarkerFaceColor','g','MarkerSize',10*resolution); % draw the new node
      	 % pause(0.2)
      	 plot([tree.child(min_idx).x,new_node.x],[tree.child(min_idx).y,new_node.y],'-k','LineWidth',0.8*resolution); % draw the segment between the closest tree node and the new node
      end
      
      % pause(0.5)
      delete(handle_1);
      delete(handle_2);
      % pause(0.5)

4 参考

RRT, RRT* & Random Trees
全局路径规划 - RRT算法原理及实现

5 完整代码

将下面两个文件放在同一文件夹下,运行(或分节运行)RRT_learn.m即可。此外,需要动态观察算法效果则把所有pause语句取消注释。
find_closest_node.m

matlab 复制代码
function [angle,min_idx] = find_closest_node(rd_x,rd_y,tree)
    distance = [];
    i = 1;
    while i <= length(tree.child) % should not use size() function
        dx = rd_x - tree.child(i).x;
        dy = rd_y - tree.child(i).y;
        distance(i) = sqrt(dx^2 + dy^2);
        i = i+1;
    end
    [~,min_idx] = min(distance);
    angle = atan2(rd_y-tree.child(min_idx).y, rd_x-tree.child(min_idx).x);
end

RRT_learn.m

matlab 复制代码
%%
clear all
clc
%% Notification
% 1,用户自定义的内容:地图范围,障碍物数量和大小,起点和终点的位置,终点范围的阈值,RRT树生长一次的长度,和绘图相关的设置
% 2,需要演示算法效果的时候,把所有pause取消注释;不需要演示算法效果的时候,把所有pause加上注释
%% Define the map
resolution = 1; % resolution, cell length

left_bound = -15;
right_bound = 15;
lower_bound = -15;
upper_bound = 15;

% Wall obstacle [left_down_x,left_down_y,horizontal_length,vertical_length]
wall_obstacle(1,:) = [   left_bound,   lower_bound,                        1, upper_bound-lower_bound-1]; % left boundary
wall_obstacle(2,:) = [ left_bound+1,   lower_bound, right_bound-left_bound-1,                         1]; % bottom boundary
wall_obstacle(3,:) = [right_bound-1, lower_bound+1,                        1, upper_bound-lower_bound-1]; % right boundary
wall_obstacle(4,:) = [   left_bound, upper_bound-1, right_bound-left_bound-1,                         1]; % up boundary

% Blcok obstacle [left_down_x,left_down_y,horizontal_length,vertical_length]
block_obstacle(1,:) = [0,-10,10,5]; % block obstacle 1
block_obstacle(2,:) = [-5,5,5,9]; % block obstacle 2
block_obstacle(3,:) = [-5,-2,5,4]; % block obstacle 3

ob = [block_obstacle; wall_obstacle];
%% Draw the map
figure(1); % create a figure

% Figure setting
set(gca,'XLim',[left_bound right_bound]); % x axis range
set(gca,'XTick',[left_bound:resolution:right_bound]); % x axis tick
set(gca,'YLim',[lower_bound upper_bound]); % y axis range
set(gca,'YTick',[lower_bound:resolution:upper_bound]); % y axis tick
grid on
axis equal
title('RRT');
xlabel('x');
ylabel('y');

hold on

% Draw the obstacles
for i=1:1:size(ob,1)
    fill([ob(i,1),ob(i,1)+ob(i,3),ob(i,1)+ob(i,3),ob(i,1)],...
         [ob(i,2),ob(i,2),ob(i,2)+ob(i,4),ob(i,2)+ob(i,4)],'k');
end

%% Initialize parameters
grow_distance = 1; % distance between parent node and the derived child node
goal_radius = 1.5; % can be considered as reaching the goal once within this range
% Goal point position
goal.x = -10;
goal.y = -10;
% Start point position
start.x = 13;
start.y = 10;
%% Draw the start point and the end point
h_start = plot(start.x,start.y,'b^','MarkerFaceColor','b','MarkerSize',5*resolution);
h_goal = plot(goal.x,goal.y,'m^','MarkerFaceColor','m','MarkerSize',5*resolution);

% Draw the goal circle
theta = linspace(0,2*pi);
goal_circle.x = goal_radius*cos(theta) + goal.x;
goal_circle.y = goal_radius*sin(theta) + goal.y;
plot(goal_circle.x,goal_circle.y,'--k','LineWidth',0.8*resolution);
%% RRT Algorithm
% Initialize the random tree(in the form of struct)
tree.child = []; % current node
tree.parent = []; % current node's parent
tree.distance = []; % current node's distance to the start

tree.child = start;
tree.parent = start;
tree.distance = 0;

new_node.x = start.x;
new_node.y = start.y;

goal_distance = sqrt((goal.x - new_node.x)^2 + (goal.y - new_node.y)^2);

% Main loop
while goal_distance > goal_radius
    random_point.x = (right_bound - left_bound) * rand() + left_bound; % random x value between x limit
    random_point.y = (upper_bound - lower_bound) * rand() + lower_bound; % random y value between y limit
    handle_1 = plot(random_point.x,random_point.y,'p','MarkerEdgeColor',[0.9290 0.6940 0.1250],'MarkerFaceColor',[0.9290 0.6940 0.1250],'MarkerSize',8*resolution); % draw the randomly generated point
    [angle,min_idx] = find_closest_node(random_point.x,random_point.y,tree);
    
    % pause(0.5)
    handle_2 = plot([tree.child(min_idx).x,random_point.x],[tree.child(min_idx).y,random_point.y],'-','Color',[0.7 0.7 0.7],'LineWidth',0.8*resolution); % draw the segment between the closest tree node and the randomly generated point
    
    % pause(0.5)
    new_node.x = tree.child(min_idx).x + grow_distance*cos(angle);
    new_node.y = tree.child(min_idx).y + grow_distance*sin(angle);
    handle_3 = plot(new_node.x,new_node.y,'.r','MarkerFaceColor','r','MarkerSize',10*resolution); % draw the potential new node
    
    flag = 1; % default: valid node

    % Judge if the new node is inside the obstacle
    step = 0.01;
    if new_node.x < tree.child(min_idx).x
        step = -step;
    end
    for k=1:1:size(ob,1)
        for i=tree.child(min_idx).x:step:new_node.x
            if angle>pi/2-5e-02 && angle<pi/2+5e-02
                j = tree.child(min_idx).y+1;
            elseif angle>-pi/2-5e-02 && angle<-pi/2+5e-02
                j = tree.child(min_idx).y-1;
            else
                j=tree.child(min_idx).y+(i-tree.child(min_idx).x)*tan(angle);
            end
            if i>=ob(k,1) && i<=(ob(k,1)+ob(k,3))
                if j >=ob(k,2) && j<=ob(k,2)+ob(k,4)
                    flag = 0; % invalid node
                    break
                end
            end
        end
        if flag==0
            break
        end
    end

    % pause(0.5)
    if flag==1
        tree.child(end+1) = new_node;
        tree.parent(end+1) = tree.child(min_idx);
        tree.distance(end+1) = 1 + tree.distance(min_idx);
        goal_distance = sqrt((goal.x - new_node.x)^2 + (goal.y - new_node.y)^2);
        delete(handle_3)
        plot(new_node.x,new_node.y,'.g','MarkerFaceColor','g','MarkerSize',10*resolution); % draw the new node
        % pause(0.2)
        plot([tree.child(min_idx).x,new_node.x],[tree.child(min_idx).y,new_node.y],'-k','LineWidth',0.8*resolution); % draw the segment between the closest tree node and the new node
    end
    
    % pause(0.5)
    delete(handle_1);
    delete(handle_2);
    % pause(0.5)
end

%% Draw the final path
final_distance = tree.distance(end);
title('RRT, distance:',num2str(final_distance));

current_index = length(tree.child);
while current_index ~= 1
    plot([tree.child(current_index).x,tree.parent(current_index).x],[tree.child(current_index).y,tree.parent(current_index).y],'-','LineWidth',1.5*resolution,'Color',[0.8500 0.3250 0.0980]); % draw the segment between the closest tree node and the new node    
    for i=1:length(tree.child)
        if tree.child(i).x == tree.parent(current_index).x
            if tree.child(i).y == tree.parent(current_index).y
                current_index = i;
                break
            end
        end
    end
end
相关推荐
网易独家音乐人Mike Zhou3 小时前
【卡尔曼滤波】数据预测Prediction观测器的理论推导及应用 C语言、Python实现(Kalman Filter)
c语言·python·单片机·物联网·算法·嵌入式·iot
Swift社区7 小时前
LeetCode - #139 单词拆分
算法·leetcode·职场和发展
Kent_J_Truman7 小时前
greater<>() 、less<>()及运算符 < 重载在排序和堆中的使用
算法
IT 青年8 小时前
数据结构 (1)基本概念和术语
数据结构·算法
Dong雨8 小时前
力扣hot100-->栈/单调栈
算法·leetcode·职场和发展
SoraLuna8 小时前
「Mac玩转仓颉内测版24」基础篇4 - 浮点类型详解
开发语言·算法·macos·cangjie
liujjjiyun9 小时前
小R的随机播放顺序
数据结构·c++·算法
¥ 多多¥9 小时前
c++中mystring运算符重载
开发语言·c++·算法
trueEve10 小时前
SQL,力扣题目1369,获取最近第二次的活动
算法·leetcode·职场和发展
Evand J10 小时前
集合卡尔曼滤波(Ensemble Kalman Filter),用于二维滤波(模拟平面上的目标跟踪),MATLAB代码
matlab·平面·目标跟踪