无人驾驶（移动机器人）路径规划之RRT与RRTStar算法及其matlab实现

Joey.Chao2024-05-02 16:50

在自动驾驶与移动机器人路径规划时，必定会用到经典的算法RRT与RRT Star。下面是RRT与RRTStar的matlab实现效果。可以发现RRTStar效果明显改善。

目录

一、效果比较

[1.1 RRT算法效果（黑色为障碍物，红色线为最终路径，蓝色三角形为开始点，粉色三角形为目标点）](#1.1 RRT算法效果（黑色为障碍物，红色线为最终路径，蓝色三角形为开始点，粉色三角形为目标点）)

[1.2 RRTStar算法效果（黑色为障碍物，红色线为最终路径，蓝色三角形为开始点，粉色三角形为目标点）](#1.2 RRTStar算法效果（黑色为障碍物，红色线为最终路径，蓝色三角形为开始点，粉色三角形为目标点）)

二、RRT与RRTStar算法

[2.1 RRT算法](#2.1 RRT算法)

[2.2 RRTStar算法](#2.2 RRTStar算法)

三、核心代码（完整代码见上述链接）

[3.1 地图创建](#3.1 地图创建)

[3.2 RRT主代码](#3.2 RRT主代码)

[3.3 RRTStar主代码](#3.3 RRTStar主代码)

[3.4 rewire代码。](#3.4 rewire代码。)

一、效果比较

1.1 RRT算法效果（黑色为障碍物，红色线为最终路径，蓝色三角形为开始点，粉色三角形为目标点）

代码链接：

移动机器人自主路径规划之RRT算法MATLAB实现代码资源-CSDN文库

（1）原始地图信息。

（2）规划路径信息。

1.2 RRTStar算法效果（黑色为障碍物，红色线为最终路径，蓝色三角形为开始点，粉色三角形为目标点）

代码链接:

移动机器人自主路径规划之RRTStar算法MATLAB实现代码资源-CSDN文库

（1）原始地图信息。

（2）规划路径信息。

二、RRT与RRTStar算法

2.1 RRT算法

RRT算法，全称为Rapidly-Exploring Random Tree（快速探索随机树），是一种在路径规划和运动规划领域广泛应用的算法，特别是在自动驾驶、机器人导航等场景下。该算法由Steven M. LaValle于1998年提出，其核心在于利用随机采样技术构建一个能够快速覆盖配置空间（Configuration Space）的随机树结构，从而找到从初始状态到目标状态的可行路径。

  1. 初始化：算法开始时，从起始点创建一个单一的节点作为随机树的根节点。

  2. 随机采样：算法在每一步中都会随机选取一个状态空间中的点，这个点通常是在整个可行区域或者配置空间中随机产生的。

  3. 接近性判断与树扩展：计算新采样点与树中最近节点的距离，并检查从最近节点沿直线向采样点方向延伸是否遇到障碍物。如果没有障碍阻挡，就在该方向上添加一个新节点至采样点的位置，这个新节点成为树的一部分。

  4. 目标检测与终止条件：如果新添加的节点或采样点落在目标区域，或者与目标点足够接近（依据预设的阈值），则认为找到了一条可行路径，并停止扩展。此时可以通过回溯随机树从起始节点到目标节点来获取路径。

  5. 重复采样与扩展：如果上述步骤未达到终止条件，则重复步骤2至4，直至找到路径或满足其他停止条件（如达到最大迭代次数、计算资源限制等）。

RRT算法具有概率完备性，即只要存在可行路径，随着迭代次数的增加，算法总能以概率1找到这条路径。然而，RRT算法生成的路径通常不是最优的，且可能包含很多不必要的转折。因此，RRT经常与其他算法（如优化算法）结合使用，以改进路径的质量。

RRT算法在许多领域都有广泛的应用，包括机器人导航、无人机飞行路径规划、自动驾驶等。它特别适用于那些难以用传统方法（如网格法）处理的复杂环境，能够高效地处理障碍物多且分布不规则的情况。

2.2 RRTStar算法

RRTStar（Rapidly-exploring Random Tree Star）算法是RRT（Rapidly-exploring Random Tree）算法的一个改进版本，旨在提高路径规划的质量和效率。RRTStar算法继承了RRT算法的基本框架，即通过在空间中随机采样和构建树来探索可行路径。然而，与RRT相比，RRTStar引入了一些关键的改进，使得它在路径优化和避免局部最优解方面更具优势。

以下是RRTStar算法的主要特点和改进之处：

  1. 路径优化：RRTStar算法在构建树的过程中，不仅关注如何快速找到一条从起点到终点的路径，还致力于优化这条路径。它采用了重连策略，即在添加新节点后，会检查是否存在更优的连接方式，如果有，则重新连接树中的节点，以形成更平滑、更短的路径。这种优化过程随着采样点的增加而持续进行，使得最终得到的路径质量更高。

  2. 避免局部最优解：RRT算法有时可能陷入局部最优解，即找到一条看似不错的路径但实际上并不是全局最优的。RRTStar通过引入启发式函数和重新布线策略来避免这个问题。启发式函数用于估计当前节点与目标点之间的距离，引导树的扩展方向，使其更有可能找到全局最优解。重新布线策略则通过调整树的结构，使得路径更加平滑，并减少不必要的转折。

  3. 适应性和鲁棒性：RRTStar算法对不同类型的环境和问题具有较好的适应性和鲁棒性。无论是障碍物分布密集还是稀疏的环境，无论是二维还是三维空间，RRTStar都能有效地进行路径规划。此外，由于它是基于随机采样的方法，因此不依赖于特定的地图表示或环境模型，使得它在实际应用中更加灵活和方便。

需要注意的是，虽然RRTStar算法在路径优化和避免局部最优解方面表现出色，但它仍然存在一定的局限性。例如，在复杂环境中，RRTStar可能需要较长的计算时间来找到一条满意的路径。此外，由于算法基于随机采样，因此生成的路径可能不是严格的最优解，而是在可接受范围内的次优解。在实际应用中，需要根据具体需求和环境特点来选择合适的算法参数和策略。

RRT*的实现通常涉及复杂的启发式搜索策略、成本函数的设计以及高效的邻近节点查询机制，比如使用KD树或平衡二叉树来加速最近节点的查找过程，确保算法的高效执行。此外，算法还可能包括对采样策略的优化，以更好地探索环境并避免陷入局部最优。

三、核心代码（完整代码见上述链接）

3.1 地图创建

RRT与RRTStar所用到的地图是相同的。

Matlab 复制代码 
function [start,target,max_x,max_y,wall,resolution]=creat_map_ob()%创建障碍物地图
%创建地图
clear all;
resolution = 1;%每个格子的长度

%map的边界
max_x = 50;
max_y = 50;

%obstacle
obstacle(1,:) = [25,5,10,5];
obstacle(2,:) = [15,25,5,25];
obstacle(3,:) = [40,20,5,10];
obstacle(4,:) = [30,25,5,10];
obstacle(5,:) = [5,0,5,20];
figure;%创建新窗口
axis([0 max_x 0 max_y]);
set(gca,'XTick',0:resolution:max_x);%x轴的间隔为1
set(gca,'YTick',0:resolution:max_y);%y轴的间隔为1
grid on;
title('RRT');
xlabel('x');
ylabel('y');
hold on;

wall = [obstacle];
for i = 1:1:size(wall,1)
    fill([wall(i,1),wall(i,1) + wall(i,3),wall(i,1) + wall(i,3),wall(i,1)],...
        [wall(i,2),wall(i,2),wall(i,2) + wall(i,4),wall(i,2) + wall(i,4)],'k');
end
pause(1);%延时一秒
%初始点
h=msgbox('初始位置标记！');%弹出初始框提示标记初始位置
uiwait(h,3);%3s后关闭对话框
if ishandle(h) == 1%删除对话框
    delete(h);
end
xlabel('请设置初始点X轴！ ','Color','black');%设置x轴
but = 0;
while(but ~= 1)%收到左键点击
    [xval,yval,but] = ginput(1);
    xval=floor(xval);
    yval=floor(yval);
end
start.x = xval;%初始位置
start.y = yval;
plot(xval,yval,'b^','MarkerFaceColor','b','MarkerSize',10*resolution);%绘制初始点
text(xval + 1,yval + 0.5,'Start');
pause(1);%延时一秒
%目标点
h=msgbox('目标位置标记！');%弹出目标提示标记目标位置
uiwait(h,3);%3s后关闭对话框
if ishandle(h) == 1%删除对话框
    delete(h);
end
xlabel('请设置目标点X轴！ ','Color','black');%设置x轴
but1 = 0;
while(but1 ~= 1)%收到左键点击
    [xval,yval,but1] = ginput(1);
    xval=floor(xval);
    yval=floor(yval);
end
target.x = xval;%目标位置
target.y = yval;
plot(xval,yval,'m^','MarkerFaceColor','m','MarkerSize',10*resolution);%绘制目标点
text(xval + 1,yval + 0.5,'Target');
hold on;

3.2 RRT主代码

Matlab 复制代码 
function RRT()
clc
clear all
close all;
[start,target,max_x,max_y,wall,resolution] = creat_map_ob();
figure;%创建新窗口
axis([0 max_x 0 max_y]);
set(gca,'XTick',0:resolution:max_x);%x轴的间隔为1
set(gca,'YTick',0:resolution:max_y);%y轴的间隔为1
grid on;
title('RRT');
xlabel('x');
ylabel('y');
hold on;
for i = 1:1:size(wall,1)
    fill([wall(i,1),wall(i,1) + wall(i,3),wall(i,1) + wall(i,3),wall(i,1)],...
        [wall(i,2),wall(i,2),wall(i,2) + wall(i,4),wall(i,2) + wall(i,4)],'k');
end
plot(start.x,start.y,'b^','MarkerFaceColor','b','MarkerSize',20*resolution);%绘制初始点
text(start.x + 1,start.y + 0.5,'Start');
plot(target.x,target.y,'m^','MarkerFaceColor','m','MarkerSize',20*resolution);%绘制目标点
text(target.x + 1,target.y + 0.5,'Target');
%初始化随机树
tree.child = [];%存储所有节点
tree.parent = [];%存储节点的父节点
tree.distance = [];%存储child到起点的距离

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

%存储新衍生的节点，起点初始化
%target_distance用于存储new_node到终点的距离
new_node.x = start.x;
new_node.y = start.y;

target_distance = sqrt((target.x-new_node.x)^2+(target.y-new_node.y)^2);
grow_distance = 1;
target_radius = 1.5;
while target_distance > target_radius
    random_point.x = max_x*rand();
    random_point.y = max_y*rand();

    handle1 = plot(random_point.x,random_point.y,'p','MarkerEdgeColor',[0.9290 0.6940 0.1250],'MarkerFaceColor',[0.9290 0.6940 0.1250],'MarkerSize',10*resolution);
    [angle,min_idx] = find_closet_node(random_point.x,random_point.y,tree);
    pause(0.001);
    handle2 = 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); 
    %根据步长选择新节点
    pause(0.001);
    new_node.x = tree.child(min_idx).x + grow_distance*cos(angle);
    new_node.y = tree.child(min_idx).y + grow_distance*sin(angle);
    handle3 = plot(new_node.x,new_node.y,'.r','MarkerFaceColor','r','MarkerSize',10*resolution);
    obflag = 1;
    step = 0.01;
    %判断路径是否位于障碍物之中
    if new_node.x < tree.child(min_idx).x
        step = -step;
    end
    for k = 1:1:size(wall,1)
        for i = tree.child(min_idx).x:step:new_node.x
            if angle>pi/2-5e-03 && angle<pi/2+5e-03
                j = tree.child(min_idx).y+i-tree.child(min_idx).x;
            elseif angle>-pi/2-5e-03 && angle<-pi/2+5e-03
                j = tree.child(min_idx).y-i+tree.child(min_idx).x;
            else
                j = tree.child(min_idx).y+(i-tree.child(min_idx).x)*tan(angle);
            end
            if i >= wall(k,1) - 0.005 && i <= (wall(k,1)+wall(k,3)) + 0.005
                if j >= wall(k,2) - 0.005 && j <= (wall(k,2) + wall(k,4)) + 0.005
                    obflag = 0;
                    break
                end
            end
        end
        if obflag == 0
            break
        end
    end
    %更新
    pause(0.001);
    if obflag == 1
        tree.child(end + 1) = new_node;
        tree.parent(end + 1) = tree.child(min_idx);
        tree.distance(end + 1) = 1 + tree.distance(min_idx);
        target_distance = sqrt((target.x-new_node.x)^2 + (target.y-new_node.y)^2);
        %删除红色点
        delete(handle3);
        plot(new_node.x,new_node.y,'.g','MarkerFaceColor','g','MarkerSize',10*resolution); % draw the new node
        %画新连接线
        plot([tree.child(min_idx).x,new_node.x],[tree.child(min_idx).y,new_node.y],'-k','LineWidth',0.8*resolution);
        
    end
    pause(0.001);
    %删除随机点
    delete(handle1);
    %删除最近点与随机点连线。
    delete(handle2);
    pause(0.001);
end
tree.child(end + 1) = target;
tree.parent(end + 1) = new_node;
tree.distance(end + 1) = 2 + tree.distance(min_idx);
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',3*resolution,'Color',[0.8500 0.3250 0.0980]);
     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

3.3 RRTStar主代码

Matlab 复制代码 
function RRT()
clc
clear all
close all;
[start,target,max_x,max_y,wall,resolution] = creat_map_ob();
figure;%创建新窗口
axis([0 max_x 0 max_y]);
set(gca,'XTick',0:resolution:max_x);%x轴的间隔为1
set(gca,'YTick',0:resolution:max_y);%y轴的间隔为1
grid on;
title('RRT');
xlabel('x');
ylabel('y');
hold on;
for i = 1:1:size(wall,1)
    fill([wall(i,1),wall(i,1) + wall(i,3),wall(i,1) + wall(i,3),wall(i,1)],...
        [wall(i,2),wall(i,2),wall(i,2) + wall(i,4),wall(i,2) + wall(i,4)],'k');
end
plot(start.x,start.y,'b^','MarkerFaceColor','b','MarkerSize',20*resolution);%绘制初始点
text(start.x + 1,start.y + 0.5,'Start');
plot(target.x,target.y,'m^','MarkerFaceColor','m','MarkerSize',20*resolution);%绘制目标点
text(target.x + 1,target.y + 0.5,'Target');
%初始化随机树
tree.child = [];%存储所有节点
tree.parent = [];%存储节点的父节点
tree.distance = [];%存储child到起点的距离

tree.child = start;
tree.parent = start;
tree.distance = 0;
radius = 5;
%存储新衍生的节点，起点初始化
%target_distance用于存储new_node到终点的距离
new_node.x = start.x;
new_node.y = start.y;

target_distance = sqrt((target.x-new_node.x)^2+(target.y-new_node.y)^2);
grow_distance = 1;
target_radius = 1.5;

line1.l = [];
line1.id = [];
while target_distance > target_radius
    random_point.x = max_x*rand();
    random_point.y = max_y*rand();

    handle1 = plot(random_point.x,random_point.y,'p','MarkerEdgeColor',[0.9290 0.6940 0.1250],'MarkerFaceColor',[0.9290 0.6940 0.1250],'MarkerSize',10*resolution);
    [angle,min_idx] = find_closet_node(random_point.x,random_point.y,tree);
    %pause(0.01);
    handle2 = 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); 
    %根据步长选择新节点
    %pause(0.01);
    new_node.x = tree.child(min_idx).x + grow_distance*cos(angle);
    new_node.y = tree.child(min_idx).y + grow_distance*sin(angle);
    handle3 = plot(new_node.x,new_node.y,'.r','MarkerFaceColor','r','MarkerSize',10*resolution);
    step = 0.005;
    %判断路径是否位于障碍物之中
    %更新
    pause(0.001);
    if isobstacle(wall,step,tree,new_node,min_idx,angle) == 1
        target_distance = sqrt((target.x-new_node.x)^2 + (target.y-new_node.y)^2);
        %删除红色点
        delete(handle3);
        plot(new_node.x,new_node.y,'.g','MarkerFaceColor','g','MarkerSize',10*resolution); % draw the new node
        %画新连接线
        handle4 = plot([tree.child(min_idx).x,new_node.x],[tree.child(min_idx).y,new_node.y],'-k','LineWidth',0.8*resolution);
       % pause(0.001);
        %删除随机点
        delete(handle1);
        %删除最近点与随机点连线。
        delete(handle2);
        %pause(0.001);
        theta = linspace(0, 2*pi, 100);
        handle6 = plot(new_node.x+radius*cos(theta),new_node.y+radius*sin(theta),'b--');
        [minid,min_dis_fs,newid] = chooseparent(wall,tree,radius,min_idx,new_node,step,resolution);
        tree.child(end + 1) = new_node;
        tree.parent(end + 1) = tree.child(minid);
        tree.distance(end + 1) = min_dis_fs;
        delete(handle4);

        line1.l(end+1) = plot([tree.child(minid).x,new_node.x],[tree.child(minid).y,new_node.y],'-k','LineWidth',0.8*resolution);
        line1.id(end+1) = length(tree.child);
        [deid] = rewire(wall,tree,minid,new_node,step,newid,min_dis_fs);
        if(deid ~= 0)
           for j = 1:1:length(line1.id)
               if(line1.id(j) == deid)
                   %pause(2);
                 delete (line1.l(j));
                 break
               end
           end
       % pause(2);
       line1.l(j) = plot([tree.child(deid).x,new_node.x],[tree.child(deid).y,new_node.y],'-k','LineWidth',0.8*resolution);
       %pause(2);
        end
        delete(handle6);
    else
        %删除随机点
    %pause(0.001);
    delete(handle3);
    delete(handle1);
    %删除最近点与随机点连线。
    delete(handle2);
    %pause(0.001);
    end
    
end
tree.child(end + 1) = target;
tree.parent(end + 1) = new_node;
tree.distance(end + 1) = 2 + tree.distance(min_idx);
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',3*resolution,'Color',[0.8500 0.3250 0.0980]);
     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

3.4 rewire代码。

Matlab 复制代码 
function [deid] = rewire(wall,tree,minid,new_node,step,newid,min_dis_fs)
deid = 0;
for i = 1:1:length(newid)
    if(newid(i) ~= minid)
        newcost = min_dis_fs + sqrt((tree.child(newid(i)).x-new_node.x)^2+(tree.child(newid(i)).y-new_node.y)^2);
        if(newcost < tree.distance(newid(i)))
            angle = atan2(new_node.y-tree.child(newid(i)).y,new_node.x-tree.child(newid(i)).x);
            if isobstacle(wall,step,tree,new_node,newid(i),angle) == 1
                tree.parent(newid(i)) = new_node;
                tree.distance(newid(i)) = newcost;
                deid = newid(i);
            end
        end
    end
end
相关推荐
IT古董10 分钟前
【机器学习】机器学习的基本分类-强化学习-策略梯度（Policy Gradient，PG）
人工智能·机器学习·分类
落魄君子11 分钟前
GA-BP分类-遗传算法（Genetic Algorithm）和反向传播算法（Backpropagation）
算法·分类·数据挖掘
mahuifa11 分钟前
混合开发环境---使用编程AI辅助开发Qt
人工智能·vscode·qt·qtcreator·编程ai
四口鲸鱼爱吃盐13 分钟前
Pytorch | 从零构建GoogleNet对CIFAR10进行分类
人工智能·pytorch·分类
菜鸡中的奋斗鸡→挣扎鸡19 分钟前
滑动窗口 + 算法复习
数据结构·算法
信号处理学渣25 分钟前
matlab画图，选择性显示legend标签
开发语言·matlab
蓝天星空26 分钟前
Python调用open ai接口
人工智能·python
睡觉狂魔er27 分钟前
自动驾驶控制与规划——Project 3: LQR车辆横向控制
人工智能·机器学习·自动驾驶
Lenyiin28 分钟前
第146场双周赛：统计符合条件长度为3的子数组数目、统计异或值为给定值的路径数目、判断网格图能否被切割成块、唯一中间众数子序列 Ⅰ
c++·算法·leetcode·周赛·lenyiin
郭wes代码35 分钟前
Cmd命令大全（万字详细版）
python·算法·小程序
热门推荐
01〔AI 绘画〕Stable Diffusion 之 解决绘制多人或面部很小的人物时面部崩坏问题 篇02YOLOv8训练好模型后，追加轮数继续训练、或者提前终止训练，缩减训练轮数03玄机平台应急响应—webshell查杀04机器人编程怎么入门？05HCIA-datacom数通题库和录播视频资料06Docker 夺命连环 15 问07SQL 主从数据库实时备份08校验 GPT-4 真实性的三个经典问题：快速区分 GPT-3.5 与 GPT-4，并提供免费测试网站09(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明10VSCode插件 —— Cody AI （免费AI助手！）