MATLAB实战：用动态可视化拆解RRT路径规划算法

第一次看到RRT算法在屏幕上"生长"出路径时，那种随机与有序交织的美感让我彻底着迷。作为机器人路径规划领域的经典算法，RRT（快速随机树）通过随机采样和树形扩展的方式，在复杂环境中为机器人寻找可行路径。本文将带你用MATLAB实现这个算法的完整可视化过程，从空白地图到最终路径生成，每个步骤都能实时观察算法的"思考"过程。

1. 环境搭建与基础配置

在开始编码前，我们需要配置好算法的基础环境。MATLAB强大的图形处理能力让我们可以实时观察算法的每一步决策，这是理解RRT精髓的关键。

% 地图参数设置
map.resolution = 1;  % 栅格分辨率
map.bounds = [-15, 15, -15, 15]; % [x_min, x_max, y_min, y_max]

% 障碍物定义（矩形区域）
obstacles = [
    % 边界障碍物
    -15, -15, 1, 29;   % 左墙
    -14, -15, 28, 1;   % 下墙
    14, -14, 1, 28;    % 右墙
    -15, 14, 28, 1;    % 上墙
    
    % 内部障碍物
    0, -10, 10, 5;     % 横向障碍
    -5, 5, 5, 9;       % 竖向障碍
    -5, -2, 5, 4       % 小型障碍
];

% 起点和终点设置
start = struct('x', 13, 'y', 10);
goal = struct('x', -10, 'y', -10);
goal_radius = 1.5;  % 到达目标的判定半径

这段代码建立了我们的"实验沙盒"——一个30×30单位的二维空间，包含边界墙和三个内部障碍物。注意到我们使用结构体存储点坐标，这种数据结构在后续的树形扩展中会非常有用。

提示：障碍物定义采用[left_down_x, left_down_y, width, height]格式，与MATLAB的rectangle函数参数一致

2. RRT算法核心实现

RRT的核心在于"随机采样-最近邻搜索-安全扩展"的循环过程。下面我们分步骤实现这个逻辑，并添加可视化效果。

2.1 初始化随机树

% 初始化图形窗口
figure('Name','RRT路径规划','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(obstacles,1)
    rectangle('Position',obstacles(i,:),'FaceColor','k');
end

% 绘制起点和终点
plot(start.x, start.y, 'bo', 'MarkerFaceColor', 'b', 'MarkerSize', 8);
plot(goal.x, goal.y, 'ro', 'MarkerFaceColor', 'r', 'MarkerSize', 8);

% 初始化树结构
tree.nodes = start;  % 节点集
tree.parents = 0;    % 父节点索引(0表示根节点)
tree.costs = 0;      % 路径成本

树结构采用三个平行数组存储：

• nodes: 存储所有节点坐标
• parents: 存储每个节点的父节点索引
• costs: 存储从起点到该节点的路径长度

2.2 主循环实现

% 算法参数
max_iter = 5000;      % 最大迭代次数
step_size = 1.0;      % 扩展步长
goal_bias = 0.1;      % 目标偏向概率

for iter = 1:max_iter
    % 随机采样（带目标偏向）
    if rand() < goal_bias
        sample = goal;
    else
        sample.x = map.bounds(1) + diff(map.bounds(1:2)) * rand();
        sample.y = map.bounds(3) + diff(map.bounds(3:4)) * rand();
    end
    
    % 寻找最近邻节点
    [nearest_node, nearest_idx] = findNearest(tree.nodes, sample);
    
    % 向采样点方向扩展
    new_node = steer(nearest_node, sample, step_size);
    
    % 碰撞检测
    if ~checkCollision(nearest_node, new_node, obstacles)
        % 添加到树中
        tree.nodes(end+1) = new_node;
        tree.parents(end+1) = nearest_idx;
        tree.costs(end+1) = tree.costs(nearest_idx) + ...
                            norm([new_node.x-nearest_node.x, new_node.y-nearest_node.y]);
        
        % 可视化新节点和连接
        plot([nearest_node.x, new_node.x], [nearest_node.y, new_node.y], 'g-');
        plot(new_node.x, new_node.y, 'g.', 'MarkerSize', 10);
        drawnow;
        
        % 检查是否到达目标
        if norm([new_node.x-goal.x, new_node.y-goal.y]) < goal_radius
            disp('路径找到!');
            break;
        end
    end
end

这段代码实现了RRT的核心循环，几个关键点值得注意：

1. 目标偏向采样：以10%的概率直接采样目标点，加速收敛
2. 渐进式扩展：通过steer函数控制每次扩展的步长
3. 实时可视化：每次成功扩展都立即更新图形显示

2.3 关键辅助函数

function [nearest, idx] = findNearest(nodes, sample)
    % 计算所有节点到采样点的距离
    distances = arrayfun(@(n) norm([n.x-sample.x, n.y-sample.y]), nodes);
    [~, idx] = min(distances);
    nearest = nodes(idx);
end

function new = steer(from, to, step_size)
    % 从from节点向to方向扩展step_size距离
    direction = [to.x-from.x, to.y-from.y];
    dist = norm(direction);
    if dist <= step_size
        new = to;
    else
        direction = direction / dist;
        new.x = from.x + direction(1) * step_size;
        new.y = from.y + direction(2) * step_size;
    end
end

function collision = checkCollision(node1, node2, obstacles)
    % 线段与矩形障碍物的碰撞检测
    collision = false;
    
    % 参数化线段：p = node1 + t*(node2-node1), t∈[0,1]
    dx = node2.x - node1.x;
    dy = node2.y - node1.y;
    
    for i = 1:size(obstacles,1)
        % 障碍物边界
        obs_left = obstacles(i,1);
        obs_right = obstacles(i,1) + obstacles(i,3);
        obs_bottom = obstacles(i,2);
        obs_top = obstacles(i,2) + obstacles(i,4);
        
        % 计算线段与障碍物边界的交点参数t
        t_values = [...
            (obs_left - node1.x)/dx, (obs_right - node1.x)/dx, ...
            (obs_bottom - node1.y)/dy, (obs_top - node1.y)/dy];
        
        % 找出在[0,1]范围内的t值
        valid_t = t_values(t_values >= 0 & t_values <= 1);
        
        % 检查这些t值对应的点是否在障碍物边界上
        for t = valid_t'
            x = node1.x + t*dx;
            y = node1.y + t*dy;
            
            if (x >= obs_left && x <= obs_right && ...
                y >= obs_bottom && y <= obs_top)
                collision = true;
                return;
            end
        end
    end
end

这三个辅助函数构成了RRT的"神经系统"：

• findNearest实现最近邻搜索
• steer控制树的生长方向和步长
• checkCollision确保路径安全性

3. 路径提取与优化

当算法找到目标后，我们需要从树结构中提取出最终路径。

% 路径回溯
if iter < max_iter
    path = [];
    current_idx = length(tree.nodes);
    
    while current_idx ~= 0
        path = [tree.nodes(current_idx), path];
        current_idx = tree.parents(current_idx);
    end
    
    % 绘制最终路径
    for i = 1:length(path)-1
        plot([path(i).x, path(i+1).x], [path(i).y, path(i+1).y], ...
            'r-', 'LineWidth', 2);
    end
    
    % 计算路径长度
    path_length = sum(arrayfun(@(i) norm([path(i+1).x-path(i).x, ...
                       path(i+1).y-path(i).y]), 1:length(path)-1));
    title(['RRT路径规划 - 路径长度: ', num2str(path_length)]);
else
    disp('达到最大迭代次数，未找到路径');
end

路径提取采用反向回溯的方式，从目标节点开始，沿着parent指针一直回溯到起点。这种方式得到的路径是树结构中从起点到目标的最短路径。

4. 高级技巧与调试建议

在实际实现RRT时，有几个常见问题需要注意：

1. 碰撞检测精度：
   • 离散化步长过大会漏检细小障碍物
   • 建议采用自适应步长或几何计算方法

% 改进的碰撞检测示例
function collision = improvedCollisionCheck(node1, node2, obstacles)
    % 使用分离轴定理进行精确碰撞检测
    collision = false;
    segment = [node1.x, node1.y; node2.x, node2.y];
    
    for i = 1:size(obstacles,1)
        obstacle = [obstacles(i,1), obstacles(i,2);
                    obstacles(i,1)+obstacles(i,3), obstacles(i,2);
                    obstacles(i,1)+obstacles(i,3), obstacles(i,2)+obstacles(i,4);
                    obstacles(i,1), obstacles(i,2)+obstacles(i,4)];
        
        if satCollision(segment, obstacle)
            collision = true;
            return;
        end
    end
end

2. 可视化优化：
   • 使用drawnow limitrate提高动画流畅度
   • 选择性绘制，避免图形界面卡顿

% 在循环开始前设置
h_fig = figure;
set(h_fig, 'DoubleBuffer', 'on');

% 在循环内部
if mod(iter, 10) == 0  % 每10次迭代更新一次图形
    drawnow limitrate;
end

3. 参数调优经验值：

参数	推荐值	作用
step_size	地图尺寸的5-10%	控制扩展步长
goal_bias	0.05-0.2	目标导向采样概率
max_iter	1000-5000	最大迭代次数

4. 常见问题排查：

• 树不生长：检查碰撞检测函数，可能是误判所有扩展都为碰撞
• 路径绕远：尝试增加goal_bias或实现RRT*等优化版本
• 运行缓慢：优化最近邻搜索，考虑使用KD-tree等数据结构

% KD-tree加速最近邻搜索示例
function [nearest, idx] = kdTreeSearch(tree, sample)
    % 将节点转换为点矩阵
    points = [[tree.nodes.x]', [tree.nodes.y]'];
    
    % 创建KD-tree（需要Statistics and Machine Learning Toolbox）
    Mdl = KDTreeSearcher(points);
    
    % 搜索最近邻
    [idx, ~] = knnsearch(Mdl, [sample.x, sample.y], 'K', 1);
    nearest = tree.nodes(idx);
end

通过本实现的完整RRT算法，你不仅能得到机器人的可行路径，更重要的是能直观理解算法如何在复杂环境中通过随机探索找到解决方案。这种理解对于后续学习更高级的路径规划算法（如RRT*、Informed RRT等）奠定了坚实基础。