用MATLAB复现RRT算法：从原理到动画演示，一步步教你搭建自己的路径规划仿真

MATLAB实战：从零构建RRT路径规划算法与动态可视化

在机器人导航和自动驾驶领域，路径规划算法扮演着至关重要的角色。RRT（快速随机树）算法因其高效的搜索能力和对高维空间的适应性，成为复杂环境下路径规划的利器。本文将带您从理论到实践，用MATLAB完整实现RRT算法，并通过动态可视化让算法每一步都清晰可见。

1. RRT算法核心原理剖析

RRT算法的本质是通过在配置空间中随机采样来构建一棵扩展树。与传统的网格搜索不同，RRT特别适合解决高维空间中的路径规划问题。让我们先理解几个关键概念：

• 随机采样：在自由空间中随机生成点，引导树的生长方向
• 最近邻搜索：为每个随机点找到树上最近的节点
• 碰撞检测：确保新生成的路径段不与环境中的障碍物相交
• 渐进式扩展：以固定步长向随机点方向生长树

算法的伪代码可以概括为：

1. 初始化树，仅包含起点 2. while 未到达目标区域: 3. 生成随机点 4. 找到树上最近的节点 5. 朝随机点方向生长新节点 6. 检查新路径段是否碰撞 7. 若无碰撞，将新节点加入树 8. 若新节点进入目标区域，终止 9. 从终点回溯到起点，得到路径

2. MATLAB环境准备与地图构建

2.1 基础环境配置

在开始编码前，我们需要配置MATLAB环境。建议使用R2018b或更高版本，以确保图形显示和结构体操作的兼容性。首先创建一个干净的工作空间：

clear all clc close all

2.2 自定义地图与障碍物

RRT算法的表现很大程度上取决于环境设置。我们将创建一个包含边界障碍和内部障碍的二维空间：

% 地图参数 resolution = 1; % 单位网格大小 map_bounds = [-15, 15, -15, 15]; % [x_min, x_max, y_min, y_max] % 边界障碍物(墙) wall_obstacles = [ map_bounds(1), map_bounds(3), 1, map_bounds(4)-map_bounds(3)-1; % 左墙 map_bounds(1)+1, map_bounds(3), map_bounds(2)-map_bounds(1)-1, 1; % 下墙 map_bounds(2)-1, map_bounds(3)+1, 1, map_bounds(4)-map_bounds(3)-1; % 右墙 map_bounds(1), map_bounds(4)-1, map_bounds(2)-map_bounds(1)-1, 1 % 上墙 ]; % 内部障碍物(矩形块) block_obstacles = [ 0, -10, 10, 5; % 障碍物1 -5, 5, 5, 9; % 障碍物2 -5, -2, 5, 4 % 障碍物3 ]; all_obstacles = [block_obstacles; wall_obstacles];

2.3 可视化地图

良好的可视化能帮助我们直观理解算法运行过程。下面代码绘制地图和障碍物：

figure('Name','RRT Path Planning','NumberTitle','off'); axis equal hold on grid on % 设置坐标轴范围 xlim([map_bounds(1) map_bounds(2)]) ylim([map_bounds(3) map_bounds(4)]) % 绘制障碍物 for i = 1:size(all_obstacles,1) rectangle('Position',all_obstacles(i,:),'FaceColor','k','EdgeColor','none'); end

3. RRT算法MATLAB实现

3.1 数据结构设计与初始化

RRT算法需要高效地存储和查询树结构。我们使用MATLAB结构体数组来实现：

% 定义起点和终点 start_point = struct('x',13,'y',10); goal_point = struct('x',-10,'y',-10); goal_radius = 1.5; % 目标区域半径 % 初始化树结构 tree = struct(); tree.nodes = start_point; % 节点坐标 tree.parent_indices = 0; % 父节点索引(0表示根节点) tree.costs = 0; % 从起点到该节点的累积成本 tree.num_nodes = 1; % 节点总数 % 绘制起点和终点 plot(start_point.x,start_point.y,'bo','MarkerFaceColor','b','MarkerSize',8); plot(goal_point.x,goal_point.y,'ro','MarkerFaceColor','r','MarkerSize',8); % 绘制目标区域 theta = 0:0.1:2*pi; goal_circle_x = goal_radius*cos(theta) + goal_point.x; goal_circle_y = goal_radius*sin(theta) + goal_point.y; plot(goal_circle_x,goal_circle_y,'r--');

3.2 核心算法循环实现

RRT的主循环包含几个关键步骤，我们逐步实现每个部分：

% 算法参数 max_iterations = 5000; % 最大迭代次数 step_size = 1.0; % 生长步长 goal_bias = 0.1; % 目标偏向概率(10%的概率直接采样目标点) for iter = 1:max_iterations % 1. 随机采样(带目标偏向) if rand() < goal_bias sample = goal_point; else sample = struct(... 'x', map_bounds(1) + (map_bounds(2)-map_bounds(1))*rand(), ... 'y', map_bounds(3) + (map_bounds(4)-map_bounds(3))*rand()); end % 2. 寻找最近节点 [nearest_node, nearest_idx] = findNearestNode(tree, sample); % 3. 生成新节点 new_node = steer(nearest_node, sample, step_size); % 4. 碰撞检测 if ~checkCollision(nearest_node, new_node, all_obstacles) % 5. 添加新节点到树 tree.nodes(end+1) = new_node; tree.parent_indices(end+1) = nearest_idx; tree.costs(end+1) = tree.costs(nearest_idx) + ... distance(nearest_node, new_node); tree.num_nodes = tree.num_nodes + 1; % 可视化新节点和连接 plot([nearest_node.x, new_node.x], [nearest_node.y, new_node.y], 'g-'); plot(new_node.x, new_node.y, 'g.'); drawnow limitrate % 加速动画显示 % 6. 检查是否到达目标 if distance(new_node, goal_point) <= goal_radius disp('路径找到!'); break; end end end

3.3 关键辅助函数实现

算法依赖几个重要的辅助函数，下面是它们的实现：

function [nearest, idx] = findNearestNode(tree, point) % 计算所有节点到采样点的距离 distances = arrayfun(@(n) distance(n, point), tree.nodes); [~, idx] = min(distances); nearest = tree.nodes(idx); end function d = distance(node1, node2) % 计算两点间欧氏距离 dx = node1.x - node2.x; dy = node1.y - node2.y; d = sqrt(dx^2 + dy^2); end function new_node = steer(from_node, to_node, step_size) % 从from_node向to_node方向生长step_size距离 d = distance(from_node, to_node); if d <= step_size new_node = to_node; else ratio = step_size / d; new_node = struct(... 'x', from_node.x + ratio*(to_node.x - from_node.x), ... 'y', from_node.y + ratio*(to_node.y - from_node.y)); end end function collision = checkCollision(node1, node2, obstacles) % 检查两点间线段是否与任何障碍物相交 collision = false; % 对每个障碍物进行检查 for i = 1:size(obstacles,1) obs = obstacles(i,:); % 线段与矩形障碍物的碰撞检测 if lineIntersectsRectangle(... node1.x, node1.y, node2.x, node2.y, ... obs(1), obs(2), obs(1)+obs(3), obs(2)+obs(4)) collision = true; return; end end end function intersects = lineIntersectsRectangle(x1,y1,x2,y2, rx1,ry1,rx2,ry2) % 实现线段与矩形的精确碰撞检测 % 这里简化实现，实际项目中应使用更健壮的几何计算方法 intersects = false; % 检查线段是否与矩形四条边相交 if lineIntersectsLine(x1,y1,x2,y2, rx1,ry1,rx1,ry2) || ... % 左边 lineIntersectsLine(x1,y1,x2,y2, rx1,ry1,rx2,ry1) || ... % 下边 lineIntersectsLine(x1,y1,x2,y2, rx2,ry1,rx2,ry2) || ... % 右边 lineIntersectsLine(x1,y1,x2,y2, rx1,ry2,rx2,ry2) % 上边 intersects = true; end end

4. 路径提取与优化

4.1 回溯获取最终路径

当算法找到目标后，我们需要从终点回溯到起点提取路径：

if iter <= max_iterations % 初始化路径 path = []; current_idx = tree.num_nodes; % 回溯直到起点 while current_idx ~= 0 path = [tree.nodes(current_idx), path]; current_idx = tree.parent_indices(current_idx); end % 绘制最终路径 for i = 1:length(path)-1 plot([path(i).x, path(i+1).x], [path(i).y, path(i+1).y], ... 'b-', 'LineWidth', 2); end title(sprintf('RRT Path Planning (Path Length: %.2f)', ... tree.costs(end))); else disp('达到最大迭代次数，未找到路径'); end

4.2 路径平滑处理

原始RRT算法生成的路径往往不够平滑，我们可以通过简单的后处理来优化：

function smoothed_path = smoothPath(path, obstacles, max_iter) % 简单的路径平滑算法 smoothed_path = path; for iter = 1:max_iter % 随机选择两个路径点 idx1 = randi([1, length(smoothed_path)-1]); idx2 = randi([idx1+1, length(smoothed_path)]); % 检查直接连接是否无碰撞 if ~checkCollision(smoothed_path(idx1), smoothed_path(idx2), obstacles) % 移除中间点 smoothed_path = [smoothed_path(1:idx1), smoothed_path(idx2:end)]; end end end

5. 高级技巧与性能优化

5.1 可视化增强技巧

为了让算法运行过程更加直观，我们可以添加更多可视化元素：

% 在循环中添加这些可视化元素 if mod(iter,50) == 0 % 每50次迭代更新一次 % 显示当前迭代信息 title(sprintf('RRT Iteration: %d, Nodes: %d', iter, tree.num_nodes)); % 随机采样点可视化 plot(sample.x, sample.y, 'y*'); % 最近节点可视化 plot(nearest_node.x, nearest_node.y, 'mo'); % 暂停一下让用户观察 pause(0.01); end

5.2 算法性能优化

对于大型地图或复杂环境，原始RRT可能效率不高。以下是几种优化策略：

1. KD-Tree加速最近邻搜索：

% 使用MATLAB的KDTreeSearcher加速最近邻查询 kdtree = KDTreeSearcher([[tree.nodes.x]', [tree.nodes.y]']); % 在findNearestNode函数中使用 function [nearest, idx] = findNearestNode(tree, point, kdtree) [idx, ~] = knnsearch(kdtree, [point.x, point.y]); nearest = tree.nodes(idx); end

2. 自适应步长调整：

% 根据环境复杂度动态调整步长 if tree.num_nodes < 100 step_size = 2.0; % 初始阶段使用较大步长快速探索 elseif tree.num_nodes < 500 step_size = 1.0; else step_size = 0.5; % 后期使用较小步长精细搜索 end

3. 并行碰撞检测：

% 使用parfor并行检查多个障碍物 function collision = checkCollisionParallel(node1, node2, obstacles) collision = false; parfor i = 1:size(obstacles,1) obs = obstacles(i,:); if lineIntersectsRectangle(... node1.x, node1.y, node2.x, node2.y, ... obs(1), obs(2), obs(1)+obs(3), obs(2)+obs(4)) collision = true; break; end end end

5.3 RRT变种算法实现

原始RRT有很多改进版本，下面是RRT*的简要实现思路：

% RRT*的节点添加逻辑 near_indices = findNearNodes(tree, new_node, radius); min_cost = tree.costs(nearest_idx) + distance(nearest_node, new_node); best_idx = nearest_idx; % 在邻近节点中寻找更优的父节点 for i = 1:length(near_indices) near_idx = near_indices(i); near_node = tree.nodes(near_idx); temp_cost = tree.costs(near_idx) + distance(near_node, new_node); if temp_cost < min_cost && ~checkCollision(near_node, new_node, obstacles) min_cost = temp_cost; best_idx = near_idx; end end % 添加新节点 tree.nodes(end+1) = new_node; tree.parent_indices(end+1) = best_idx; tree.costs(end+1) = min_cost; tree.num_nodes = tree.num_nodes + 1; % 重布线步骤 for i = 1:length(near_indices) near_idx = near_indices(i); near_node = tree.nodes(near_idx); new_cost = tree.costs(end) + distance(new_node, near_node); if new_cost < tree.costs(near_idx) && ~checkCollision(new_node, near_node, obstacles) tree.parent_indices(near_idx) = tree.num_nodes; tree.costs(near_idx) = new_cost; end end

6. 实际应用案例与调试技巧

6.1 典型问题与解决方案

在实现RRT算法时，常会遇到以下问题：

1. 算法运行缓慢：

   • 减少可视化更新频率
   • 使用更高效的数据结构(如KD-Tree)
   • 优化碰撞检测函数

2. 路径不够平滑：

   • 实现路径后处理平滑算法
   • 使用RRT*等改进算法
   • 增加目标偏向概率

3. 狭窄通道难以通过：

   • 减小步长
   • 增加迭代次数
   • 使用双向RRT(同时从起点和终点生长树)

6.2 参数调优指南

RRT算法性能受多个参数影响，下面是调优建议：

6.3 扩展到三维空间

将算法扩展到三维空间只需稍作修改：

% 三维节点定义 node3d = struct('x',0,'y',0,'z',0); % 三维距离计算 function d = distance3d(node1, node2) dx = node1.x - node2.x; dy = node1.y - node2.y; dz = node1.z - node2.z; d = sqrt(dx^2 + dy^2 + dz^2); end % 三维碰撞检测(使用长方体障碍物) function collision = checkCollision3D(node1, node2, obstacles3d) % 实现类似2D但扩展到三维的碰撞检测 ... end

7. 完整实现与进一步学习

7.1 完整代码结构

一个完整的RRT实现通常包含以下文件：

/RRT_Project │── main.m % 主脚本，设置参数并运行算法 │── initializeMap.m % 地图和障碍物初始化 │── rrtPlanner.m % RRT算法核心实现 │── findNearestNode.m % 最近邻搜索 │── steer.m % 节点生长 │── checkCollision.m % 碰撞检测 │── smoothPath.m % 路径平滑 │── visualizeRRT.m % 可视化函数 │── /utils % 实用工具函数 │── distance.m │── lineIntersectsRectangle.m │── ...

7.2 进一步学习资源

要深入理解RRT算法及其变种，可以参考：

• 原始论文：LaValle, S. M. (1998). Rapidly-exploring random trees: A new tool for path planning
• 进阶算法：RRT*, Informed RRT*, q-RRT
• 应用案例：机械臂运动规划、无人机导航、自动驾驶

在MATLAB中，Robotics System Toolbox提供了内置的路径规划算法实现，可以作为参考：

% 使用MATLAB内置的RRT实现 planner = plannerRRT(map, stateSpace); path = plan(planner, start, goal);