RRT（Rapidly-exploring Random Tree）2D & 3D 路径规划 — MATLAB

一、算法本质

从起点长出一棵随机树，每次朝随机采样点"试探迈一小步"，不撞障碍就接入树，直到够到目标。

---

二、2D 版本

2.1 主入口 rrt_2d_demo.m

%% rrt_2d_demo.m
clear; clc; close all;

%% ===== 1. 地图 & 障碍定义 =====
% 工作空间
xlim = [0 10]; ylim = [0 10];

start = [1, 1];
goal  = [9, 9];

% 障碍物：用"封闭多边形"描述（随便加）
% 每个 obstacle{i} = N×2 的顶点矩阵（顺时针/逆时针均可）
obstacles = cell(3,1);
% 矩形墙
obstacles{1} = [3 2; 5 2; 5 4; 3 4];
% 三角形台
obstacles{2} = [6 5; 7.5 3; 6 3];
% 厚墙
obstacles{3} = [1 6; 3 6; 3 7; 1 7];

%% ===== 2. RRT 参数 =====
stepSize = 0.5;     % 每步最大迈进距离
goalRadius = 0.4;    % 认为"到达目标"的半径
maxIter = 5000;     % 最大迭代次数
goalBias = 0.1;      % 10%概率直接采样目标（加速收敛）

%% ===== 3. 运行 RRT =====
[path, tree, success] = rrt_2d(start, goal, obstacles, xlim, ylim, ...
    stepSize, goalRadius, maxIter, goalBias);

fprintf('RRT 2D: %s\n', string(success));

%% ===== 4. 可视化 =====
figure('Color','w','Position',[100 100 900 800]); hold on; axis equal;
xlim(xlim); ylim(ylim); box on; grid on;
title('RRT 2D 路径规划');

% 画障碍
for i = 1:numel(obstacles)
    patch(obstacles{i}(:,1), obstacles{i}(:,2), [0.4 0.4 0.4], 'EdgeColor','k','LineWidth',1.5);
end

% 画树
for i = 2:length(tree)
    plot([tree(i).parent.x, tree(i).x], ...
         [tree(i).parent.y, tree(i).y], ...
         '-', 'Color',[0.6 0.8 1], 'LineWidth',0.4);
end

% 画路径
if success
    path = shortcut_2d(path, obstacles);
    plot([path.x], [path.y], 'r-', 'LineWidth',2.5);
end

% 起点/终点
plot(start(1),start(2),'go','MarkerSize',10,'LineWidth',2);
plot(goal(1),goal(2),'r*','MarkerSize',14,'LineWidth',2);
legend({'障碍','RRT树','路径','start','goal'},'Location','northwest');

---

2.2 核心 RRT（2D）

function [path, tree, success] = rrt_2d(start, goal, obstacles, xlim, ylim, ...
    stepSize, goalRadius, maxIter, goalBias)

tree = struct();
tree(1).x = start(1); tree(1).y = start(2); tree(1).parent = 0;  % parent=0 表示根

success = false;
goalIdx = 0;

for it = 1:maxIter
    % 随机采样（带goal bias）
    if rand < goalBias
        sample = goal;
    else
        sample(1) = xlim(1) + rand*(xlim(2)-xlim(1));
        sample(2) = ylim(1) + rand*(ylim(2)-ylim(1));
    end

    % 找树中最靠近sample的点
    [nearIdx, nearest] = nearest_node(tree, sample);

    % 朝sample方向步进一步
    newPt = steer(nearest, sample, stepSize);

    % 碰撞检测：线段 nearest->newPt 是否穿障碍
    if ~segment_hits_obstacle(nearest, newPt, obstacles)
        % 加入树
        nd.x = newPt(1); nd.y = newPt(2); nd.parent = nearIdx;
        tree = [tree; nd];   %#ok<AGROW>

        % 够到目标？
        if norm(newPt - goal) < goalRadius
            goalIdx = numel(tree);
            success = true;
            break;
        end
    end
end

%% 回溯路径
if success
    path = backtrack_path(tree, goalIdx);
else
    path = [];
end
end

%% ---------- 关键子函数 ----------
function [idx, pt] = nearest_node(tree, sample)
dmin = inf; idx = 1;
for i = 1:numel(tree)
    d = norm([tree(i).x tree(i).y] - sample);
    if d < dmin, dmin = d; idx = i; end
end
pt = [tree(idx).x tree(idx).y];
end

function newPt = steer(from, to, step)
dir = to - from;
dist = norm(dir);
if dist < 1e-12
    newPt = from; return;
end
dir = dir / dist;
if dist <= step
    newPt = to;
else
    newPt = from + dir * step;
end
end

function collide = segment_hits_obstacle(A, B, obstacles)
collide = false;
for oi = 1:numel(obstacles)
    poly = obstacles{oi};
    % 用线段-多边形边相交检测
    if line_poly_intersect(A, B, poly)
        collide = true; return;
    end
end
end

function hit = line_poly_intersect(A, B, poly)
hit = false;
n = size(poly,1);
for i = 1:n
    P1 = poly(i,:);
    P2 = poly(mod(i,n)+1,:);
    if seg_seg_intersect(A,B,P1,P2)
        hit = true; return;
    end
end
% 额外：线段完全在障碍内部也算撞
if point_in_polygon(A, poly)
    hit = true; return;
end
end

%% 线段相交（快速排斥 + 跨立试验）
function y = seg_seg_intersect(p1,q1,p2,q2)
% 叉积 z分量
cross = @(a,b) a(1)*b(2)-a(2)*b(1);
d1 = cross(p2-p1, q2-p1);
d2 = cross(q2-p1, p2-p1);
d3 = cross(p1-p2, q1-p2);
d4 = cross(q2-p2, p1-p2);
if ((d1>0 && d2<0)||(d1<0&&d2>0)) && ((d3>0&&d4<0)||(d3<0&&d4>0))
    y = true; return; end
% 共线情况：投影重叠
if abs(d1)<1e-12 && on_segment(p1,q1,p2) return; end
if abs(d2)<1e-12 && on_segment(p1,q1,q2) y=true; return; end
if abs(d3)<1e-12 && on_segment(p2,q2,p1) y=true; return; end
if abs(d4)<1e-12 && on_segment(p2,q2,q1) y=true; return; end
y=false;
end
function y=on_segment(p,q,r)
y = (r(1)-min(p(1),q(1)))*(max(p(1),q(1))-r(1))>=0 && ...
    (r(2)-min(p(2),q(2)))*(max(p(2),q(2))-r(2))>=0;
end

function y = point_in_polygon(pt, poly)
% 射线法（向右打射线）
y = false; n = size(poly,1);
for i = 1:n
    p1 = poly(i,:); p2 = poly(mod(i,n)+1,:);
    if (p1(2)>pt(2))~=(p2(2)>pt(2))
        xinters = (pt(2)-p1(2))*(p2(1)-p1(1))/(p2(2)-p1(2))+p1(1);
        if xinters > pt(1), y = ~y; end
    end
end
end

function path = backtrack_path(tree, idx)
xs = []; ys = [];
while idx ~= 0
    xs = [tree(idx).x; xs];
    ys = [tree(idx).y; ys];
    idx = tree(idx).parent;
end
path = struct('x',xs,'y',ys);
end

---

2.3 可选快捷捷径

function pts = shortcut_2d(path_struct, obs)
% 尝试跳过多余拐点（直接连线不断变短）
x = path_struct.x(:); y = path_struct.y(:);
keep = true(size(x));
i = 1;
while i < length(x)-1
    j = i+1;
    while j <= length(x) && segment_hits_obstacle([x(i),y(i)],[x(j),y(j)],obs)==false
        j = j+1;
    end
    j = j-1;
    keep(i+1:j-1) = false;
    i = j;
end
pts = struct('x',x(keep),'y',y(keep));
end

---

三、3D 版本（障碍用球体 + 长方体，带视角动画）

3.1 3D RRT 主程序 rrt_3d_demo.m

%% rrt_3d_demo.m
clear; clc; close all;

%% ===== 1. 地图 =====
bounds = [0 10; 0 10; 0 6];   % xyz
start = [1, 1, 0.5];
goal  = [9, 9, 5];

% 障碍：球体 {center,r} 和 轴对齐盒子 {[x1,x2],[y1,y2],[z1,z2]}
spheres.center = {[5 5 3], [7 3 2]};   spheres.radius = {1.2, 0.8};
boxes = { [3 4.5; 6 8; 1 3], [6 8; 1 3; 2 5] }; % 每个是3×2

stepSize   = 0.7;
goalRadius = 0.6;
maxIter    = 8000;
goalBias   = 0.08;

%% ===== 2. 运行 =====
[path, tree, ok] = rrt_3d(start,goal,bounds,spheres,boxes,stepSize,goalRadius,maxIter,goalBias);

fprintf('RRT 3D: %s\n', string(ok));

%% ===== 3. 可视化 =====
figure('Color','w','Position',[100 100 1000 700]);
hold on; grid on; box on; axis equal;
xlabel('x'); ylabel('y'); zlabel('z');
title('RRT 3D 路径规划');

% 障碍画半透明球/盒
for i=1:numel(spheres.center)
    draw_sphere(spheres.center{i}, spheres.radius{i}, [0.5 0.5 0.5]);
end
for i=1:numel(boxes)
    draw_box(boxes{i}, [0.4 0.4 0.4]);
end

% 树
for i=2:numel(tree)
    p1 = [tree(i).parent.x tree(i).parent.y tree(i).parent.z];
    p2 = [tree(i).x tree(i).y tree(i).z];
    plot3([p1(1) p2(1)],[p1(2) p2(2)],[p1(3) p2(3)],'-','Color',[0.65 0.85 1],'LineWidth',0.3);
end

% 路径
if ok
    plot3(path(:,1), path(:,2), path(:,3), 'r-', 'LineWidth',2.5);
end

plot3(start(1),start(2),start(3),'go','MarkerSize',10,'LineWidth',2);
plot3(goal(1),goal(2),goal(3),'r*','MarkerSize',14,'LineWidth',2);
view(45,30);
end

---

3.2 RRT（3D核心）--- 逻辑完全同2D，只是升维

function [path, tree, ok] = rrt_3d(start,goal,bounds,spheres,boxes,...
    step,goalR,maxIt,bias)

tree = struct();
tree(1).x=start(1); tree(1).y=start(2); tree(1).z=start(3); tree(1).parent=struct('x',0,'y',0,'z',0);
tree(1).parent = 0;

ok=false; goalIdx=0;

for it=1:maxIt
    % 采样
    if rand<bias
        s = goal;
    else
        s = [bounds(1,1)+rand*diff(bounds(1)), ...
             bounds(2,1)+rand*diff(bounds(2)), ...
             bounds(3,1)+rand*diff(bounds(3))];
    end

    % 最近点
    [nidx, nr] = nearest_3d(tree, s);
    dir = s - nr; dl = norm(dir);
    if dl<1e-12, continue; end
    newP = nr + dir/dl * min(dl, step);

    % 碰撞检测（线段 vs 球/盒）
    if ~collision_3d(nr, newP, spheres, boxes)
        nd.x=newP(1); nd.y=newP(2); nd.z=newP(3); nd.parent=nidx;
        tree(end+1)=nd;  %#ok<AGROW>
        if norm(newP-goal)<goalR
            goalIdx=numel(tree); ok=true; break; end
    end
end

path=[];
if ok
    idx=goalIdx; pts=zeros(0,3);
    while idx~=0
        pts=[tree(idx).x tree(idx).y tree(idx).z; pts];
        idx=tree(idx).parent;
    end
    path=pts;
end
end

function [idx,pt] = nearest_3d(tree, s)
dmin=inf; idx=1;
for i=1:numel(tree)
    d=norm([tree(i).x tree(i).y tree(i).z]-s);
    if d<dmin, dmin=d; idx=i; end
end
pt=[tree(idx).x tree(idx).y tree(idx).z];
end

function hit=collision_3d(A,B,sph,box)
% A,B: 3×1 线段端点
% 球体检测：若线段最近点到球心距离<r 则碰撞
for i=1:numel(sph.center)
    c=sph.center{i}; r=sph.radius{i};
    if seg_sphere_dist(A,B,c)<r, hit=true; return; end
end
% 长方体检测：保守策略------中点远离+端点在内任一盒
for i=1:numel(box)
    bx=box{i};
    if point_in_box(A,bx)||point_in_box(B,bx)||seg_box_endpoint_near(A,B,bx)
        hit=true; return;
    end
end
hit=false;
end

function d = seg_sphere_dist(A,B,c)
% 线段到球心最短距离（投影法，不需解方程求根）
AB=B-A; t=dot(c-A,AB)/max(norm(AB)^2,eps); t=max(0,min(1,t));
p=A+t*AB; d=norm(p-c);
end
function y=point_in_box(p,bx)
y=p(1)>=bx(1,1)&&p(1)<=bx(1,2)&&p(2)>=bx(2,1)&&p(2)<=bx(2,2)&&p(3)>=bx(3,1)&&p(3)<=bx(3,2); end
function y=seg_box_endpoint_near(A,B,bx)
y=point_in_box(A,bx)||point_in_box(B,bx); end

辅助画图

function draw_sphere(c,r,fc)
[x,y,z]=sphere(30); surf(x*r+c(1),y*r+c(2),z*r+c(3),'FaceColor',fc,'EdgeColor','none','FaceAlpha',0.35); end

function draw_box(bx,fc)
% bx = [x12; y12; z12]
x=linspace(bx(1,1),bx(1,2),2); y=linspace(bx(2,1),bx(2,2),2); z=linspace(bx(3,1),bx(3,2),2);
verts=[1 1 1;2 1 1;2 2 1;1 2 1;1 1 2;2 1 2;2 2 2;1 2 2];
faces={[1 2 3 4],[5 6 7 8],[1 2 6 5],[2 3 7 6],[3 4 8 7],[4 1 5 8]};
V(:,1)=[x(verts(:,1))';x(verts(:,2))';x(verts(:,3))';x(verts(:,4))'];
for f=1:6
    fill3(V(1,faces{f}),V(2,faces{f}),V(3,faces{f}),fc,'FaceAlpha',0.3,'EdgeColor','k'); end
end

参考代码 rrt算法在2维和3维环境的规划 www.youwenfan.com/contentcsw/81942.html

四、运行效果你会看到什么

版本	画面
2D	灰色多边形障碍 + 浅蓝树枝状树 + 红色最终路径
3D	半透明球体/盒子障碍 + 空间树枝 + 3D红线路径