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
相关推荐
江畔柳前堤7 小时前
大语言模型分布式训练:从并行策略到万卡工程的系统梳理
人工智能·分布式·深度学习·算法·目标检测·机器学习·语言模型
Forever Nore9 小时前
LeetCode 4 寻找两个正序数组的中位数 - 二分
算法·leetcode
罗西的思考10 小时前
【OpenClaw具身硬件】MiniClaw 阅读笔记---(1)基础
人工智能·算法·机器学习
蛋先生DX11 小时前
大模型参数存储格式揭秘:BF不是男朋友
深度学习·算法·llm
爱跳舞的烤冷面11 小时前
自学嵌入式第22天(数据结构——哈希)
数据结构·算法·哈希算法
猎嘤一号12 小时前
博弈论(Game Theory)的理论、算法与工程
人工智能·算法·安全·博弈论
月光船幽幽12 小时前
影子模式下保护 logits 不被修改
人工智能·python·算法
马拉AI12 小时前
腾讯开源 Agent 记忆系统,AI“换对话就忘”的问题有了新解法(附安装使用教程)
人工智能·算法·开源·科研
地平线开发者14 小时前
征程6工具链模型X86推理方式说明
算法
地平线开发者14 小时前
【征程6】校准量化中HistogramObserver解析
算法