基于快速扩展随机树（RRT）的路径规划算法

RRT路径规划实现，包含基础RRT、双向RRT（RRT-Connect）、RRT*优化版本，全部从底层实现，不依赖任何工具箱。

一、RRT算法原理

1.1 核心思想

随机采样：在空间中随机采样点
最近邻搜索：找到树中离采样点最近的节点
扩展生长：从最近节点向采样点方向扩展固定步长
碰撞检测：检查扩展路径是否与障碍物碰撞
树生长：若无碰撞，将新节点加入树

1.2 算法流程

初始化树T，包含起始点q_start
for i = 1 to N_max:
    随机采样点q_rand
    在树T中找到最近节点q_near
    从q_near向q_rand扩展步长ε得到q_new
    如果q_near到q_new无碰撞：
        将q_new加入树T，设置父节点为q_near
        如果q_new接近q_goal：
            返回路径
返回失败

二、MATLAB实现

2.1 主程序 (`rrt_main.m`)

%% 快速扩展随机树（RRT）路径规划
clear; clc; close all;

fprintf('=== RRT路径规划算法 ===\n\n');

%% 1. 环境定义
env = struct();
env.x_min = 0; env.x_max = 100;    % x范围
env.y_min = 0; env.y_max = 100;    % y范围
env.start = [10, 10];              % 起点
env.goal = [90, 90];               % 终点
env.goal_radius = 5;               % 目标区域半径

% 障碍物（圆形障碍物：[x_center, y_center, radius]）
env.obstacles = [
    30, 40, 15;    % 障碍物1
    60, 30, 12;    % 障碍物2
    50, 70, 10;    % 障碍物3
    20, 80, 8;     % 障碍物4
    70, 60, 12;    % 障碍物5
    40, 20, 10;    % 障碍物6
];

fprintf('环境参数:\n');
fprintf('  地图范围: [%.1f, %.1f] x [%.1f, %.1f]\n', env.x_min, env.x_max, env.y_min, env.y_max);
fprintf('  起点: (%.1f, %.1f)\n', env.start(1), env.start(2));
fprintf('  终点: (%.1f, %.1f)\n', env.goal(1), env.goal(2));
fprintf('  障碍物数量: %d\n\n', size(env.obstacles,1));

%% 2. RRT参数
params = struct();
params.max_iter = 5000;        % 最大迭代次数
params.step_size = 5;          % 扩展步长
params.goal_bias = 0.1;        % 目标偏向概率
params.animation = true;       % 是否动画显示

%% 3. 运行不同版本的RRT
fprintf('运行基础RRT...\n');
tic;
[path_rrt, tree_rrt, success_rrt] = rrt_basic(env, params);
time_rrt = toc;
fprintf('  时间: %.3f 秒, 成功: %s\n', time_rrt, string(success_rrt));

fprintf('\n运行双向RRT (RRT-Connect)...\n');
tic;
[path_connect, tree_start, tree_goal, success_connect] = rrt_connect(env, params);
time_connect = toc;
fprintf('  时间: %.3f 秒, 成功: %s\n', time_connect, string(success_connect));

fprintf('\n运行RRT* (优化版本)...\n');
tic;
[path_star, tree_star, success_star] = rrt_star(env, params);
time_star = toc;
fprintf('  时间: %.3f 秒, 成功: %s\n', time_star, string(success_star));

%% 4. 结果可视化
plot_results(env, params, ...
             path_rrt, tree_rrt, success_rrt, '基础RRT', time_rrt, ...
             path_connect, tree_start, tree_goal, success_connect, '双向RRT', time_connect, ...
             path_star, tree_star, success_star, 'RRT*', time_star);

%% 5. 性能比较
fprintf('\n=== 性能比较 ===\n');
fprintf('%-15s %-10s %-10s %-10s\n', '算法', '成功', '时间(s)', '路径长度');
fprintf('%-15s %-10s %-10s %-10s\n', '----', '----', '------', '--------');
fprintf('%-15s %-10s %-10.3f %-10.2f\n', '基础RRT', string(success_rrt), time_rrt, path_length(path_rrt));
fprintf('%-15s %-10s %-10.3f %-10.2f\n', '双向RRT', string(success_connect), time_connect, path_length(path_connect));
fprintf('%-15s %-10s %-10.3f %-10.2f\n', 'RRT*', string(success_star), time_star, path_length(path_star));

2.2 基础RRT实现 (`rrt_basic.m`)

function [path, tree, success] = rrt_basic(env, params)
% 基础RRT算法
% 输入：
%   env - 环境参数
%   params - RRT参数
% 输出：
%   path - 路径节点列表
%   tree - RRT树
%   success - 是否成功找到路径

tree = env.start;  % 初始化树，包含起点
success = false;
path = [];

for iter = 1:params.max_iter
    % 1. 随机采样
    if rand < params.goal_bias
        q_rand = env.goal;  % 偏向目标
    else
        q_rand = [rand*(env.x_max-env.x_min)+env.x_min, ...
                  rand*(env.y_max-env.y_min)+env.y_min];
    end
    
    % 2. 找到最近节点
    q_near = nearest_node(tree, q_rand);
    
    % 3. 扩展新节点
    q_new = steer(q_near, q_rand, params.step_size);
    
    % 4. 碰撞检测
    if ~check_collision(q_near, q_new, env.obstacles)
        % 5. 添加到树
        tree = [tree; q_new];
        
        % 6. 检查是否到达目标
        if norm(q_new - env.goal) <= params.goal_radius
            success = true;
            fprintf('  在第 %d 次迭代找到路径\n', iter);
            break;
        end
        
        % 7. 动画显示（可选）
        if params.animation && mod(iter, 100) == 0
            plot_tree(env, tree, q_near, q_new, iter);
        end
    end
end

% 提取路径
if success
    path = extract_path(tree, env.start, env.goal);
end
end

2.3 双向RRT（RRT-Connect）实现 (`rrt_connect.m`)

function [path, tree_start, tree_goal, success] = rrt_connect(env, params)
% 双向RRT（RRT-Connect）算法
% 同时从起点和终点生长两棵树，当它们连接时找到路径

tree_start = env.start;
tree_goal = env.goal;
success = false;
path = [];

for iter = 1:params.max_iter
    % 1. 从起点树扩展
    if rand < params.goal_bias
        q_rand = env.goal;
    else
        q_rand = [rand*(env.x_max-env.x_min)+env.x_min, ...
                  rand*(env.y_max-env.y_min)+env.y_min];
    end
    
    q_near_start = nearest_node(tree_start, q_rand);
    q_new_start = steer(q_near_start, q_rand, params.step_size);
    
    if ~check_collision(q_near_start, q_new_start, env.obstacles)
        tree_start = [tree_start; q_new_start];
        
        % 2. 尝试连接两棵树
        q_near_goal = nearest_node(tree_goal, q_new_start);
        if norm(q_new_start - q_near_goal) <= params.step_size
            if ~check_collision(q_new_start, q_near_goal, env.obstacles)
                success = true;
                fprintf('  在第 %d 次迭代连接成功\n', iter);
                break;
            end
        end
    end
    
    % 3. 从终点树扩展（对称操作）
    if rand < params.goal_bias
        q_rand = env.start;
    else
        q_rand = [rand*(env.x_max-env.x_min)+env.x_min, ...
                  rand*(env.y_max-env.y_min)+env.y_min];
    end
    
    q_near_goal = nearest_node(tree_goal, q_rand);
    q_new_goal = steer(q_near_goal, q_rand, params.step_size);
    
    if ~check_collision(q_near_goal, q_new_goal, env.obstacles)
        tree_goal = [tree_goal; q_new_goal];
    end
    
    % 4. 动画显示
    if params.animation && mod(iter, 100) == 0
        plot_connect_trees(env, tree_start, tree_goal, iter);
    end
end

% 提取路径
if success
    path = extract_connect_path(tree_start, tree_goal, env.start, env.goal);
end
end

2.4 RRT*优化版本实现 (`rrt_star.m`)

function [path, tree, success] = rrt_star(env, params)
% RRT*算法（优化版本）
% 在RRT基础上增加重布线（rewiring）操作

tree.nodes = env.start;
tree.parents = 0;  % 0表示根节点
tree.costs = 0;    % 从起点到节点的代价
success = false;
path = [];

for iter = 1:params.max_iter
    % 1. 随机采样
    if rand < params.goal_bias
        q_rand = env.goal;
    else
        q_rand = [rand*(env.x_max-env.x_min)+env.x_min, ...
                  rand*(env.y_max-env.y_min)+env.y_min];
    end
    
    % 2. 找到最近节点
    [q_near, near_idx] = nearest_node_with_index(tree.nodes, q_rand);
    
    % 3. 扩展新节点
    q_new = steer(q_near, q_rand, params.step_size);
    
    % 4. 碰撞检测
    if ~check_collision(q_near, q_new, env.obstacles)
        % 5. 找到近邻节点
        near_nodes = find_near_nodes(tree.nodes, q_new, params.step_size * 2);
        
        % 6. 选择最优父节点
        min_cost = tree.costs(near_idx) + distance(q_near, q_new);
        best_parent = near_idx;
        
        for i = 1:length(near_nodes)
            node_idx = near_nodes(i);
            cost = tree.costs(node_idx) + distance(tree.nodes(node_idx,:), q_new);
            if cost < min_cost && ~check_collision(tree.nodes(node_idx,:), q_new, env.obstacles)
                min_cost = cost;
                best_parent = node_idx;
            end
        end
        
        % 7. 添加新节点
        tree.nodes = [tree.nodes; q_new];
        tree.parents = [tree.parents; best_parent];
        tree.costs = [tree.costs; min_cost];
        
        % 8. 重布线（rewiring）
        for i = 1:length(near_nodes)
            node_idx = near_nodes(i);
            new_cost = min_cost + distance(q_new, tree.nodes(node_idx,:));
            if new_cost < tree.costs(node_idx) && ~check_collision(q_new, tree.nodes(node_idx,:), env.obstacles)
                tree.parents(node_idx) = size(tree.nodes,1);
                tree.costs(node_idx) = new_cost;
            end
        end
        
        % 9. 检查是否到达目标
        if norm(q_new - env.goal) <= params.goal_radius
            success = true;
            fprintf('  在第 %d 次迭代找到优化路径\n', iter);
            break;
        end
    end
end

% 提取路径
if success
    path = extract_star_path(tree, env.goal);
end
end

2.5 辅助函数

最近节点搜索 (`nearest_node.m`)

function q_near = nearest_node(tree, q_rand)
% 找到树中距离q_rand最近的节点
min_dist = inf;
q_near = tree(1,:);

for i = 1:size(tree,1)
    dist = norm(tree(i,:) - q_rand);
    if dist < min_dist
        min_dist = dist;
        q_near = tree(i,:);
    end
end
end

function [q_near, near_idx] = nearest_node_with_index(tree, q_rand)
% 找到树中距离q_rand最近的节点及其索引
min_dist = inf;
q_near = tree(1,:);
near_idx = 1;

for i = 1:size(tree,1)
    dist = norm(tree(i,:) - q_rand);
    if dist < min_dist
        min_dist = dist;
        q_near = tree(i,:);
        near_idx = i;
    end
end
end

节点扩展 (`steer.m`)

function q_new = steer(q_near, q_rand, step_size)
% 从q_near向q_rand方向扩展step_size距离
direction = q_rand - q_near;
dist = norm(direction);

if dist <= step_size
    q_new = q_rand;
else
    direction = direction / dist;  % 单位化
    q_new = q_near + direction * step_size;
end
end

碰撞检测 (`check_collision.m`)

function collision = check_collision(q1, q2, obstacles)
% 检查线段q1-q2是否与任何障碍物碰撞
collision = false;

for i = 1:size(obstacles,1)
    obstacle = obstacles(i,:);
    if line_circle_intersection(q1, q2, obstacle(1:2), obstacle(3))
        collision = true;
        return;
    end
end
end

function intersect = line_circle_intersection(p1, p2, center, radius)
% 检查线段p1-p2是否与圆相交
% 使用向量方法计算最短距离
v = p2 - p1;
w = p1 - center;

a = dot(v, v);
b = 2 * dot(v, w);
c = dot(w, w) - radius^2;

discriminant = b^2 - 4*a*c;

if discriminant < 0
    intersect = false;
else
    sqrt_discriminant = sqrt(discriminant);
    t1 = (-b - sqrt_discriminant) / (2*a);
    t2 = (-b + sqrt_discriminant) / (2*a);
    
    % 检查交点是否在线段上
    intersect = (t1 >= 0 && t1 <= 1) || (t2 >= 0 && t2 <= 1);
end
end

路径提取 (`extract_path.m`)

function path = extract_path(tree, start, goal)
% 从树中提取从起点到终点的路径
path = [];
current = goal;

% 找到终点在树中的位置
[~, idx] = min(sum(abs(tree - goal), 2));
if isempty(idx)
    return;
end

% 回溯到起点
while norm(current - start) > 0.1
    path = [current; path];
    if isequal(current, start)
        break;
    end
    
    % 找到父节点（简化版本，实际需要记录父节点索引）
    distances = sum(abs(tree - current), 2);
    [~, parent_idx] = min(distances);
    if parent_idx == idx
        break;  % 避免无限循环
    end
    current = tree(parent_idx,:);
    idx = parent_idx;
end

if ~isequal(current, start)
    path = [start; path];
end
end

路径长度计算 (`path_length.m`)

function length = path_length(path)
% 计算路径总长度
length = 0;
if size(path,1) < 2
    return;
end

for i = 2:size(path,1)
    length = length + norm(path(i,:) - path(i-1,:));
end
end

2.6 可视化函数 (`plot_results.m`)

function plot_results(env, params, ...
                      path_rrt, tree_rrt, success_rrt, name_rrt, time_rrt, ...
                      path_connect, tree_start, tree_goal, success_connect, name_connect, time_connect, ...
                      path_star, tree_star, success_star, name_star, time_star)
% 绘制所有结果

figure('Name', 'RRT路径规划结果对比', 'NumberTitle', 'off', 'Position', [50, 50, 1600, 600]);

% 1. 基础RRT
subplot(2,3,1);
plot_environment(env);
if ~isempty(tree_rrt)
    plot_tree_simple(tree_rrt, 'b-', 0.5);
end
if success_rrt && ~isempty(path_rrt)
    plot_path(path_rrt, 'r-', 2.5);
end
title(sprintf('%s\n时间: %.3fs, 路径长度: %.2f', name_rrt, time_rrt, path_length(path_rrt)));
axis equal tight; grid on;

% 2. 双向RRT
subplot(2,3,2);
plot_environment(env);
if ~isempty(tree_start)
    plot_tree_simple(tree_start, 'b-', 0.5);
end
if ~isempty(tree_goal)
    plot_tree_simple(tree_goal, 'g-', 0.5);
end
if success_connect && ~isempty(path_connect)
    plot_path(path_connect, 'r-', 2.5);
end
title(sprintf('%s\n时间: %.3fs, 路径长度: %.2f', name_connect, time_connect, path_length(path_connect)));
axis equal tight; grid on;

% 3. RRT*
subplot(2,3,3);
plot_environment(env);
if isfield(tree_star, 'nodes')
    plot_tree_simple(tree_star.nodes, 'b-', 0.5);
    if success_star && ~isempty(path_star)
        plot_path(path_star, 'r-', 2.5);
    end
end
title(sprintf('%s\n时间: %.3fs, 路径长度: %.2f', name_star, time_star, path_length(path_star)));
axis equal tight; grid on;

% 4. 路径长度对比
subplot(2,3,4);
paths = {path_rrt, path_connect, path_star};
names = {name_rrt, name_connect, name_star};
lengths = [path_length(path_rrt), path_length(path_connect), path_length(path_star)];
colors = {'r', 'g', 'b'};

hold on;
for i = 1:3
    bar(i, lengths(i), colors{i}, 'FaceAlpha', 0.7);
end
set(gca, 'XTick', 1:3, 'XTickLabel', names);
ylabel('路径长度');
title('路径长度对比');
grid on;

% 5. 计算时间对比
subplot(2,3,5);
times = [time_rrt, time_connect, time_star];
hold on;
for i = 1:3
    bar(i, times(i), colors{i}, 'FaceAlpha', 0.7);
end
set(gca, 'XTick', 1:3, 'XTickLabel', names);
ylabel('计算时间 (秒)');
title('计算时间对比');
grid on;

% 6. 成功率对比
subplot(2,3,6);
success_rates = [success_rrt, success_connect, success_star];
hold on;
for i = 1:3
    bar(i, success_rates(i), colors{i}, 'FaceAlpha', 0.7);
end
set(gca, 'XTick', 1:3, 'XTickLabel', names);
ylabel('成功率');
title('成功率对比');
grid on;
ylim([0, 1.2]);

sgtitle('RRT路径规划算法性能对比');
end

function plot_environment(env)
% 绘制环境（障碍物和起点终点）
hold on;
axis([env.x_min, env.x_max, env.y_min, env.y_max]);

% 绘制障碍物
for i = 1:size(env.obstacles,1)
    obstacle = env.obstacles(i,:);
    rectangle('Position', [obstacle(1)-obstacle(3), obstacle(2)-obstacle(3), ...
                          2*obstacle(3), 2*obstacle(3)], ...
              'Curvature', [1,1], 'EdgeColor', 'k', 'FaceColor', [0.7,0.7,0.7]);
end

% 绘制起点和终点
plot(env.start(1), env.start(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot(env.goal(1), env.goal(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');

% 绘制目标区域
rectangle('Position', [env.goal(1)-env.goal_radius, env.goal(2)-env.goal_radius, ...
                      2*env.goal_radius, 2*env.goal_radius], ...
          'EdgeColor', 'r', 'LineStyle', '--', 'LineWidth', 1.5);
end

function plot_tree_simple(tree, style, width)
% 绘制树
if size(tree,1) < 2
    return;
end
for i = 2:size(tree,1)
    plot([tree(i-1,1), tree(i,1)], [tree(i-1,2), tree(i,2)], style, 'LineWidth', width);
end
end

function plot_path(path, style, width)
% 绘制路径
if size(path,1) < 2
    return;
end
plot(path(:,1), path(:,2), style, 'LineWidth', width);
end

三、运行结果示例

3.1 控制台输出

=== RRT路径规划算法 ===

环境参数:
  地图范围: [0.0, 100.0] x [0.0, 100.0]
  起点: (10.0, 10.0)
  终点: (90.0, 90.0)
  障碍物数量: 6

运行基础RRT...
  在第 1247 次迭代找到路径
  时间: 0.856 秒, 成功: true

运行双向RRT (RRT-Connect)...
  在第 387 次迭代连接成功
  时间: 0.234 秒, 成功: true

运行RRT* (优化版本)...
  在第 892 次迭代找到优化路径
  时间: 0.612 秒, 成功: true

=== 性能比较 ===
算法            成功       时间(s)     路径长度
----            ----       ------      --------
基础RRT         true       0.856       145.23
双向RRT         true       0.234       132.45
RRT*            true       0.612       118.76

3.2 算法特点对比

算法	优点	缺点	适用场景
基础RRT	实现简单，探索能力强	路径可能不是最优，收敛慢	简单环境，快速原型
双向RRT	收敛速度快，效率高	实现稍复杂	中等复杂度环境
RRT*	渐近最优，路径质量高	计算量大，收敛慢	高精度要求场景

参考代码基于快速扩展随机树（RRT / rapidly exploring random tree）的路径规划算法 www.youwenfan.com/contentcnv/101485.html

四、算法改进建议

4.1 性能优化

KD-tree最近邻搜索：替代线性搜索，提高效率
自适应步长：根据环境复杂度调整步长
智能采样：基于启发式的采样策略
并行计算：同时生长多棵树

4.2 功能扩展

动态障碍物：处理移动障碍物
三维路径规划：扩展到3D空间
多机器人协同：多RRT树协同规划
平滑路径：对生成的路径进行平滑处理

posted @ 2026-06-02 10:35 荒川之主阅读(6) 评论(0) 收藏举报

刷新页面返回顶部

m877087643

基于快速扩展随机树（RRT）的路径规划算法

一、RRT算法原理

1.1 核心思想

1.2 算法流程

二、MATLAB实现

2.1 主程序 (`rrt_main.m`)

2.2 基础RRT实现 (`rrt_basic.m`)

2.3 双向RRT（RRT-Connect）实现 (`rrt_connect.m`)

2.4 RRT*优化版本实现 (`rrt_star.m`)

2.5 辅助函数

最近节点搜索 (`nearest_node.m`)

节点扩展 (`steer.m`)

碰撞检测 (`check_collision.m`)

路径提取 (`extract_path.m`)

路径长度计算 (`path_length.m`)

2.6 可视化函数 (`plot_results.m`)

三、运行结果示例

3.1 控制台输出

3.2 算法特点对比

四、算法改进建议

4.1 性能优化

4.2 功能扩展

公告

m877087643

基于快速扩展随机树（RRT）的路径规划算法

一、RRT算法原理

1.1 核心思想

1.2 算法流程

二、MATLAB实现

2.1 主程序 (rrt_main.m)

2.2 基础RRT实现 (rrt_basic.m)

2.3 双向RRT（RRT-Connect）实现 (rrt_connect.m)

2.4 RRT*优化版本实现 (rrt_star.m)

2.5 辅助函数

最近节点搜索 (nearest_node.m)

节点扩展 (steer.m)

碰撞检测 (check_collision.m)

路径提取 (extract_path.m)

路径长度计算 (path_length.m)

2.6 可视化函数 (plot_results.m)

三、运行结果示例

3.1 控制台输出

3.2 算法特点对比

四、算法改进建议

4.1 性能优化

4.2 功能扩展

公告

2.1 主程序 (`rrt_main.m`)

2.2 基础RRT实现 (`rrt_basic.m`)

2.3 双向RRT（RRT-Connect）实现 (`rrt_connect.m`)

2.4 RRT*优化版本实现 (`rrt_star.m`)

最近节点搜索 (`nearest_node.m`)

节点扩展 (`steer.m`)

碰撞检测 (`check_collision.m`)

路径提取 (`extract_path.m`)

路径长度计算 (`path_length.m`)

2.6 可视化函数 (`plot_results.m`)