机器人路径规划算法之RRT算法详解＋MATLAB代码实现
本文介绍了RRT（快速随机扩展树）算法的原理与实现。RRT是一种适用于高维空间路径规划的采样型算法，通过随机采样、最近邻搜索和逐步扩展的方式构建搜索树。算法流程包括初始化树结构、随机采样、节点扩展、碰撞检测等步骤，直到找到连接起点到终点的可行路径。MATLAB实现版本具有目标偏置、障碍物避碰、路径提取和平滑等特性，通过可视化功能可直观观察树的生长过程及最终路径。用户可通过调整起点、终点、障碍物等参
·
目录
一、算法原理
RRT（Rapidly-exploring Random Tree，快速随机扩展树）是一种用于路径规划的采样型算法，特别适用于高维空间和非完整约束系统的路径规划。
二、核心思想
随机采样：在配置空间中随机采样点
最近邻搜索：在现有树中找到距离采样点最近的节点
扩展树：从最近节点向采样点方向扩展一小步
碰撞检测：确保新节点是可行的
连接起点和终点：直到找到连接起点和终点的路径
三、算法步骤
1. 初始化树T，只包含起始点q_start
2. for k = 1 to K do
3. 在配置空间中随机采样点q_rand
4. 在T中找到距离q_rand最近的节点q_near
5. 从q_near向q_rand方向扩展步长ε，得到新节点q_new
6. 如果q_new到q_near的路径无碰撞
7. 将q_new添加到T中，记录父节点为q_near
8. 如果q_new接近目标点q_goal
9. 找到路径，返回成功
10. 返回失败四、MATLAB实现
function [path, smoothed_path, tree] = RRT_algorithm(varargin)
% RRT_ALGORITHM 快速探索随机树路径规划算法
% 用法:
% [path, smoothed_path, tree] = RRT_algorithm()
% [path, smoothed_path, tree] = RRT_algorithm('Param1', Value1, ...)
%
% 可选参数:
% 'start' - 起点坐标 [x, y] (默认: [10, 10])
% 'goal' - 终点坐标 [x, y] (默认: [90, 90])
% 'map_width' - 地图宽度 (默认: 100)
% 'map_height' - 地图高度 (默认: 100)
% 'max_iter' - 最大迭代次数 (默认: 5000)
% 'step_size' - 步长 (默认: 5)
% 'goal_bias' - 目标偏置概率 (默认: 0.1)
% 'goal_radius' - 目标区域半径 (默认: 5)
% 'obstacles' - 障碍物矩阵 [x, y, width, height; ...] (默认: 4个矩形障碍物)
% 'visualize' - 是否可视化 (默认: true)
% 参数解析
params = parse_params(varargin{:});
%% 初始化RRT树
tree.vertices = params.start; % 顶点列表
tree.edges = []; % 边列表
tree.parents = 0; % 父节点索引
found_path = false; % 路径找到标志
path = []; % 最终路径
smoothed_path = []; % 平滑路径
%% 可视化初始化
if params.visualize
figure('Position', [100, 100, 800, 600]);
axis([0 params.map_width 0 params.map_height]);
axis equal;
grid on;
hold on;
% 绘制起点和终点
plot(params.start(1), params.start(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot(params.goal(1), params.goal(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
rectangle('Position', [params.goal(1)-params.goal_radius, params.goal(2)-params.goal_radius, ...
2*params.goal_radius, 2*params.goal_radius], 'Curvature', [1,1], ...
'EdgeColor', 'r', 'LineWidth', 2);
% 绘制障碍物
for i = 1:size(params.obstacles, 1)
rectangle('Position', params.obstacles(i, :), 'FaceColor', [0.5, 0.5, 0.5]);
end
title('RRT路径规划算法');
xlabel('X坐标');
ylabel('Y坐标');
end
%% RRT主循环
fprintf('开始RRT路径规划...\n');
for iter = 1:params.max_iter
% 生成随机点（带目标偏置）
if rand() < params.goal_bias
rand_point = params.goal; % 直接以目标点作为随机点
else
rand_point = [rand() * params.map_width, rand() * params.map_height];
end
% 寻找最近的节点
distances = sum((tree.vertices - rand_point).^2, 2);
[~, nearest_idx] = min(distances);
nearest_point = tree.vertices(nearest_idx, :);
% 向随机点方向扩展
direction = rand_point - nearest_point;
distance = norm(direction);
if distance > 0
new_point = nearest_point + (direction / distance) * min(params.step_size, distance);
% 检查路径是否与障碍物碰撞
if ~check_collision(nearest_point, new_point, params.obstacles)
% 添加到树中
tree.vertices = [tree.vertices; new_point];
tree.edges = [tree.edges; nearest_idx, size(tree.vertices, 1)];
tree.parents = [tree.parents; nearest_idx];
% 可视化
if params.visualize
plot([nearest_point(1), new_point(1)], ...
[nearest_point(2), new_point(2)], 'b-', 'LineWidth', 1.5);
plot(new_point(1), new_point(2), 'b.', 'MarkerSize', 8);
end
% 检查是否到达目标区域
if norm(new_point - params.goal) <= params.goal_radius
fprintf('在第%d次迭代找到路径！\n', iter);
found_path = true;
break;
end
end
end
% 动态显示
if params.visualize && mod(iter, 100) == 0
drawnow;
title(sprintf('RRT路径规划 (迭代次数: %d/%d)', iter, params.max_iter));
end
end
%% 提取并显示路径
if found_path
% 回溯路径
current_idx = size(tree.vertices, 1);
while current_idx > 0
path = [tree.vertices(current_idx, :); path];
current_idx = tree.parents(current_idx);
end
if params.visualize
% 绘制最终路径
plot(path(:, 1), path(:, 2), 'r-', 'LineWidth', 3);
plot(path(:, 1), path(:, 2), 'yo', 'MarkerSize', 6, 'MarkerFaceColor', 'y');
title(sprintf('RRT路径规划完成！迭代次数: %d, 路径长度: %.2f', iter, calculate_path_length(path)));
end
fprintf('路径节点数: %d\n', size(path, 1));
fprintf('路径长度: %.2f\n', calculate_path_length(path));
% 平滑路径
if size(path, 1) > 2
smoothed_path = smooth_path(path, params.obstacles, params.visualize);
fprintf('平滑后路径节点数: %d\n', size(smoothed_path, 1));
fprintf('平滑后路径长度: %.2f\n', calculate_path_length(smoothed_path));
end
else
fprintf('未找到路径！已达到最大迭代次数%d\n', params.max_iter);
if params.visualize
title('RRT路径规划失败');
end
end
if params.visualize
drawnow;
end
end
%% 辅助函数
function params = parse_params(varargin)
% 解析输入参数
p = inputParser;
addParameter(p, 'start', [10, 10]);
addParameter(p, 'goal', [90, 90]);
addParameter(p, 'map_width', 100);
addParameter(p, 'map_height', 100);
addParameter(p, 'max_iter', 5000);
addParameter(p, 'step_size', 5);
addParameter(p, 'goal_bias', 0.1);
addParameter(p, 'goal_radius', 5);
addParameter(p, 'visualize', true);
addParameter(p, 'obstacles', [30, 20, 20, 40; 60, 60, 15, 25; 20, 70, 25, 15; 70, 20, 15, 30]);
parse(p, varargin{:});
params = p.Results;
end
function collision = check_collision(p1, p2, obstacles)
% 检查线段与障碍物是否碰撞
collision = false;
num_points = 10;
for t = linspace(0, 1, num_points)
point = p1 + t * (p2 - p1);
for i = 1:size(obstacles, 1)
obs = obstacles(i, :);
if point(1) >= obs(1) && point(1) <= obs(1) + obs(3) && ...
point(2) >= obs(2) && point(2) <= obs(2) + obs(4)
collision = true;
return;
end
end
end
end
function length = calculate_path_length(path)
% 计算路径长度
length = 0;
for i = 1:size(path, 1)-1
length = length + norm(path(i+1, :) - path(i, :));
end
end
function smoothed_path = smooth_path(path, obstacles, visualize)
% 平滑路径
smoothed_path = path(1, :);
current_idx = 1;
while current_idx < size(path, 1)
next_idx = size(path, 1);
for test_idx = size(path, 1):-1:current_idx+1
if ~check_collision(path(current_idx, :), path(test_idx, :), obstacles)
next_idx = test_idx;
break;
end
end
smoothed_path = [smoothed_path; path(next_idx, :)];
if visualize
plot([path(current_idx, 1), path(next_idx, 1)], ...
[path(current_idx, 2), path(next_idx, 2)], ...
'm--', 'LineWidth', 2);
end
current_idx = next_idx;
end
end这个RRT算法实现包含以下功能：
主要特性：
基本RRT算法：标准的快速探索随机树实现
目标偏置：通过
goal_bias参数提高收敛速度障碍物避碰：检查路径是否与矩形障碍物碰撞
可视化：实时显示树的生长过程
路径提取：找到目标后回溯生成完整路径
路径平滑：可选的后处理步骤，缩短路径
使用说明：
修改
start和goal设置起点和终点调整
obstacles数组添加或修改障碍物通过
max_iter控制最大迭代次数
step_size控制树的生长步长
goal_bias控制直接向目标扩展的概率
运行结果：
蓝色线条：RRT树的生长过程
红色线条：找到的最终路径
黄色点：路径节点
紫色虚线：平滑后的路径（如果启用）
开始RRT路径规划...
在第144次迭代找到路径！
路径节点数: 28
路径长度: 135.00
平滑后路径节点数: 3
平滑后路径长度: 122.01
ans =
10.0000 10.0000
12.5998 14.2710
17.4190 12.9387
22.0416 14.8444
26.2167 12.0933
31.2150 11.9631
36.1765 11.3439
41.1575 10.9083
44.9475 14.1695
49.3802 16.4829
51.7982 20.8593
53.0981 25.6874
54.6910 30.4269
57.1897 34.7577
57.6446 39.7370
60.7246 43.6758
63.3957 47.9025
67.5643 50.6634
71.0269 54.2704
73.3719 58.6864
77.4086 61.6368
79.4374 66.2067
78.5828 71.1331
76.2139 75.5363
80.4189 78.2415
83.5773 82.1176
86.7356 85.9938
89.8940 89.8699
代码主要组成部分
1. 参数初始化部分
%% 1. 参数设置
map.width = 100; % 地图尺寸
start = [10, 10]; % 起点
goal = [90, 90]; % 终点
goal_radius = 5; % 目标容差半径
max_iter = 5000; % 最大迭代次数
step_size = 5; % 扩展步长
goal_bias = 0.1; % 目标偏置概率-
goal_bias：每次有10%的概率直接向目标点扩展，加速收敛
-
step_size：控制树的生长速度，步长过小效率低，过大可能错过狭窄通道
2. 数据结构初始化
%% 2. 初始化RRT树
tree.vertices = start; % 存储所有节点坐标
tree.edges = []; % 存储边（节点间连接）
tree.parents = 0; % 记录每个节点的父节点索引树结构包含：
-
vertices：所有节点的位置坐标
-
edges：节点之间的连接关系
-
parents：每个节点的父节点编号（用于回溯路径）
3. 主循环逻辑（核心算法）
步骤1：采样随机点
if rand() < goal_bias
rand_point = goal; % 有goal_bias概率直接采样目标点
else
rand_point = [rand() * map.width, rand() * map.height]; % 随机采样
end-
目标偏置机制提高算法收敛速度
步骤2：寻找最近节点
distances = sum((tree.vertices - rand_point).^2, 2);
[~, nearest_idx] = min(distances);
nearest_point = tree.vertices(nearest_idx, :);-
计算随机点到树中所有节点的欧氏距离
-
选择距离最近的节点作为生长起点
步骤3：向随机点方向扩展
direction = rand_point - nearest_point;
distance = norm(direction);
new_point = nearest_point + (direction / distance) * min(step_size, distance);-
计算方向向量
-
从最近节点沿该方向移动step_size距离（或直接到达随机点）
步骤4：碰撞检测
if ~check_collision(nearest_point, new_point, obstacles)
% 无碰撞，添加新节点-
调用
check_collision函数检查路径段是否穿过障碍物 -
无碰撞才添加新节点
步骤5：更新树结构
tree.vertices = [tree.vertices; new_point];
tree.edges = [tree.edges; nearest_idx, size(tree.vertices, 1)];
tree.parents = [tree.parents; nearest_idx];-
添加新节点坐标
-
记录边（从最近节点到新节点）
-
记录父节点索引
步骤6：检查是否到达目标
if norm(new_point - goal) <= goal_radius
found_path = true; % 找到路径
break;
end-
新节点进入目标区域即视为成功
4. 路径回溯与提取
if found_path
current_idx = size(tree.vertices, 1); % 从终点开始
while current_idx > 0
path = [tree.vertices(current_idx, :); path]; % 添加到路径
current_idx = tree.parents(current_idx); % 回溯到父节点
end
end-
从终点节点开始，沿着
parents链回溯到起点 -
逆序存储形成完整路径
5. 路径平滑处理
while current_idx < size(path, 1)
next_idx = size(path, 1);
for test_idx = size(path, 1):-1:current_idx+1
if ~check_collision(path(current_idx, :), path(test_idx, :), obstacles)
next_idx = test_idx; % 找到可直达的最远点
break;
end
end
smoothed_path = [smoothed_path; path(next_idx, :)];
current_idx = next_idx;
end-
贪心算法：尝试连接尽可能远的节点
-
用直线替换折线，缩短路径长度
6. 辅助函数说明
碰撞检测函数：
function collision = check_collision(p1, p2, obstacles)
% 在线段上均匀采样10个点
% 检查每个点是否在任意障碍物内-
将线段离散化为10个点
-
逐个检查点是否在障碍物矩形内
路径长度计算：
function length = calculate_path_length(path)
% 计算路径上相邻点的欧氏距离之和算法流程图
开始
↓
初始化RRT树（起点为根节点）
↓
进入主循环（最多max_iter次）：
1. 采样随机点（有goal_bias概率直接采样目标点）
2. 找到树中距离随机点最近的节点
3. 从最近节点向随机点方向扩展step_size
4. 碰撞检测：检查新线段是否与障碍物相交
5. 无碰撞：添加新节点到树中
6. 检查新节点是否在目标区域内
是 → 跳出循环，找到路径
否 → 继续下一次迭代
↓
路径找到？ → 是 → 路径回溯
↓ ↓
否 路径平滑
↓ ↓
失败报告 成功输出关键参数调优建议
step_size（步长）：
值大：探索快，但可能错过狭窄通道
值小：路径精细，但搜索慢
建议：地图尺寸的1/20到1/10
goal_bias（目标偏置）：
值大：收敛快，但可能陷入局部最优
值小：探索充分，但可能收敛慢
建议：0.05-0.2
max_iter（最大迭代次数）：
复杂环境需要更多迭代
建议：1000-10000
goal_radius（目标半径）：
值大：容易成功，但路径终点不精确
值小：更精确，但更难成功
建议：step_size的1-2倍
立足具身智能前沿赛道，致力于搭建全球化、开源化、全栈式技术交流与实践共创平台。
更多推荐
11
0- 0
已为社区贡献7条内容
所有评论(0)