MATLAB / OctaveAI算法
RRT* 算法详解与 MATLAB 代码实现
RRT* 是 RRT 算法的渐进最优改进版本，通过引入父节点优化选择和近邻节点重连接机制，使路径规划从“能找到路”升级为“能找到好路”。本文详细解析了 RRT* 的核心思想、算法步骤、优势特点及与 RRT 的对比，并提供了完整的 MATLAB 代码实现。代码涵盖了参数设置、障碍物定义、树结构初始化、主算法循环、碰撞检测、可视化系统及收敛曲线绘制等功能模块，适用于机器人运动规划等场景。
RRT* 是 RRT 算法的渐进最优改进版本，通过引入父节点优化选择和近邻节点重连接机制，使路径规划从“能找到路”升级为“能找到好路”。本文详细解析了 RRT* 的核心思想、算法步骤、优势特点及与 RRT 的对比，并提供了完整的 MATLAB 代码实现。代码涵盖了参数设置、障碍物定义、树结构初始化、主算法循环、碰撞检测、可视化系统及收敛曲线绘制等功能模块，适用于机器人运动规划等场景。
核心思想与改进
RRT* 算法是 RRT (快速扩展随机树) 算法的渐进最优改进版本，是基于采样的路径规划算法中的核心算法，旨在解决高维空间中的运动规划问题。
RRT**通过在 RRT 的扩展步骤中引入'父节点优化选择'和'近邻节点重连接'两大机制，赋予路径规划过程自我优化的能力，使其从'能找到路'升级为'能找到好路'，是采样规划领域一个里程碑式的算法。
RRT 算法能找到一条可行路径，但不保证质量（路径可能很长、很绕）。
RRT* 在 RRT 的基础上，通过代价函数和'重连接'机制，对搜索树进行持续的优化，使其在运行时间内收敛到一条渐进最优的路径。
简单说，RRT* 不仅能'找到'路，还能'优化'这条路。
假设我们有一个起点（X_start）和一个目标区域（X_goal）。
初始化：创建一个只包含起点
X_start的树T，设置其代价为 0。 主循环： a. 采样 (Sample)：在状态空间内随机采样一个点X_rand。 b. 最近邻 (Nearest)：在树T中找到距离X_rand最近的点X_nearest。 c. 扩展 (Steer)：从X_nearest朝X_rand的方向迈出一步（步长为step_size），得到一个新点X_new。检查从X_nearest到X_new的路径是否碰撞。若无碰撞，进入下一步。 d. 近邻搜索 (Near)：在树T中，以X_new为圆心，半径为r的范围内，找到所有已有的节点，这些节点称为'近邻节点集'。这个半径r通常与节点数量和维度有关，以保证渐进最优性。 e. 选择最优父节点 (ChooseParent)：这是 RRT* 的第一个关键优化步骤。RRT 中，X_new的父节点固定为X_nearest。但在 RRT* 中，我们从近邻节点集中，为X_new选择一个'最好的父节点'。计算从起点X_start出发，经过近邻节点集中的每一个节点X_near，再到X_new的总路径代价。检查从每个X_near到X_new的路径是否碰撞。从所有无碰撞且能使X_new获得最小累计代价的X_near中，选出最优的那个作为X_new的父节点。将X_new加入树T，并记录其最小累计代价。 f. 重连接优化 (Rewire)：这是 RRT* 的第二个关键优化步骤，也是实现渐进最优的核心。对于近邻节点集中的每一个节点X_near，我们考虑：**如果让X_new作为X_near的父节点，X_near的累计代价是否能变得更小？**即，比较：Cost(X_new) + Distance(X_new, X_near)与原来的Cost(X_near)。如果前者更小，并且从X_new到X_near的路径无碰撞，那么就进行'重连接'：断开X_near原来的父节点连接，将其父节点重新指向X_new，并更新X_near及其后代的累计代价。这个过程就像一个'剪枝'操作，不断修剪掉绕远的枝杈，使整棵树的生长方向在满足无碰撞的前提下，始终向更低代价的区域汇聚。 终止：当X_new进入目标区域X_goal时，我们就得到了一条从起点到目标的可行路径。由于'重连接'机制，算法不会终止，只要继续运行，就会不断优化这条路径，使其代价越来越低，直到时间耗尽或达到最优。
渐进最优性：随着采样点数量趋于无穷，算法找到的路径代价以概率 1 收敛到全局最优解。这是 RRT* 与 RRT 最根本的区别。 '重连接'机制：这是实现最优性的核心。它允许树在生长过程中不断'修正'之前的连接，使整体结构更优。 概率完备性：如果解存在，随着采样次数增加，找到解的概率趋于 1。
| 特性 | RRT | RRT* |
|---|---|---|
| 目标 | 找到可行路径 | 找到最优/渐进最优路径 |
| 父节点选择 | 固定为最近邻 X_nearest | 在近邻集中选择代价最小的节点 |
| 优化操作 | 无 | 包含'重连接'优化步骤 |
| 路径质量 | 通常不是最优 | 随时间推移越来越优 |
| 计算成本 | 较低 | 较高（需要近邻搜索和多次代价计算） |
| 结果 | 一条可行但可能绕远的路径 | 一条不断被优化的平滑路径 |
优点：能处理高维、复杂约束的规划问题。具备渐进最优性，路径质量高。概率完备，理论保证强。 缺点：收敛到最优解的速度可能较慢。在狭窄通道或复杂环境中，初始找到解的效率可能不如 RRT。近邻搜索在高维空间中计算开销大。
RRT* 及其后续变种（如 Informed RRT*）广泛应用于：机器人运动规划：机械臂抓取、无人机导航、自动驾驶。动画与游戏：角色路径规划。虚拟原型与 CAD：在复杂装配体中规划运动序列。
%% RRT* 算法 MATLAB 实现
cclc; clear; close all;
fprintf('=== RRT* 路径规划算法演示 ===\n');
%% 1. 参数设置
max_iter = 3000; % 最大迭代次数
step_size = 0.5; % 扩展步长 (减小步长以提高成功率)
goal_radius = 0.5; % 目标区域半径
search_radius = 2.0; % 近邻搜索半径 (重连接用)
pause_time = 0.001; % 可视化暂停时间 (秒)
% 地图边界
[x_min, x_max, y_min, y_max] map_bounds = [0, 10, 0, 10];
% 起点和目标点
start = [1, 1];
goal = [9, 9];
fprintf('起点：(%.1f, %.1f)\n', start(1), start(2));
fprintf('目标：(%.1f, %.1f)\n', goal(1), goal(2));
fprintf('最大迭代次数：%d\n', max_iter);
%% 2. 障碍物定义 (圆形障碍物，可自行修改)
% 每个障碍物：[x, y, radius]
obstacles = [ 3, 3, 1.5; 7, 2, 1.0; 5, 5, 1.2; 2, 7, 1.0; 8, 6, 1.5; 4, 8, 1.0; 6, 4, 0.8 ];
num_obstacles = size(obstacles, 1);
fprintf('障碍物数量：%d\n', num_obstacles);
%% 3. 初始化树结构
% 使用结构体存储树
tree.nodes = start; % 节点坐标
tree.parent = 0; % 父节点索引 (起点为 0)
tree.cost = 0; % 从起点到该节点的代价
tree.count = 1; % 节点数量
% 路径记录
best_path = [];
best_cost = Inf;
goal_reached = false;
goal_node_idx = 0; % 记录每次迭代的最优代价
cost_history = Inf(1, max_iter);
fprintf('\n开始 RRT*规划...\n');
%% 4. 创建图形界面
figure('Position', [100, 100, 1200, 500]); % 子图 1: 动态规划过程
subplot(1, 2, 1);
hold on; grid on; axis equal; xlim([map_bounds(1), map_bounds(2)]); ylim([map_bounds(3), map_bounds(4)]); xlabel('X'); ylabel('Y'); title('RRT* 动态规划过程');
% 绘制障碍物
for i = 1:num_obstacles rectangle('Position', [obstacles(i,1)-obstacles(i,3), obstacles(i,2)-obstacles(i,3), ... obstacles(i,3)*2, obstacles(i,3)*2], ... 'Curvature', [1, 1], 'FaceColor', [0.8, 0.2, 0.2], 'EdgeColor', 'k'); end
% 绘制起点和目标
plot(start(1), start(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot(goal(1), goal(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
rectangle('Position', [goal(1)-goal_radius, goal(2)-goal_radius, ... goal_radius*2, goal_radius*2], 'EdgeColor', 'r', 'LineStyle', '--');
% 子图 2: 最终路径
subplot(1, 2, 2);
hold on; grid on; axis equal; xlim([map_bounds(1), map_bounds(2)]); ylim([map_bounds(3), map_bounds(4)]); xlabel('X'); ylabel('Y'); title('最优路径');
% 绘制障碍物
for i = 1:num_obstacles rectangle('Position', [obstacles(i,1)-obstacles(i,3), obstacles(i,2)-obstacles(i,3), ... obstacles(i,3)*2, obstacles(i,3)*2], ... 'Curvature', [1, 1], 'FaceColor', [0.8, 0.2, 0.2], 'EdgeColor', 'k'); end
plot(start(1), start(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot(goal(1), goal(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
rectangle('Position', [goal(1)-goal_radius, goal(2)-goal_radius, ... goal_radius*2, goal_radius*2], 'EdgeColor', 'r', 'LineStyle', '--');
%% 5. 辅助函数定义
% 距离计算函数
dist = @(p1, p2) sqrt(sum((p1 - p2).^2));
% 计算搜索半径
calc_search_radius = @(n) min(search_radius, 3.0 * (log(n+1)/max(1,n))^(1/2));
%% 6. RRT* 主算法
for iter = 1:max_iter
% 显示进度
if mod(iter, 100) == 0 fprintf('迭代：%d/%d, 节点数：%d, 当前最优代价：%.2f\n', ... iter, max_iter, tree.count, best_cost); end
% 6.1 采样
if rand < 0.1 && goal_reached % 有一定概率直接采样目标点
x_rand = goal;
else
x_rand = [map_bounds(1) + rand*(map_bounds(2)-map_bounds(1)), ... map_bounds(3) + rand*(map_bounds(4)-map_bounds(3))]; end
% 6.2 寻找最近邻
min_dist = Inf; nearest_idx = 1;
for i = 1:tree.count d = dist(x_rand, tree.nodes(i,:)); if d < min_dist min_dist = d; nearest_idx = i; end end
x_nearest = tree.nodes(nearest_idx, :);
% 6.3 向随机点扩展
direction = x_rand - x_nearest;
if norm(direction) > 0 direction = direction / norm(direction); end
x_new = x_nearest + step_size * direction;
% 6.4 检查碰撞
if check_collision(x_nearest, x_new, obstacles) continue; % 如果碰撞，放弃这个采样点 end
% 6.5 寻找近邻节点
current_radius = calc_search_radius(tree.count); near_indices = []; near_costs = [];
for i = 1:tree.count if dist(tree.nodes(i,:), x_new) <= current_radius if ~check_collision(tree.nodes(i,:), x_new, obstacles) near_indices = [near_indices; i]; near_costs = [near_costs; tree.cost(i) + dist(tree.nodes(i,:), x_new)]; end end end
% 6.6 选择最优父节点
if isempty(near_indices) x_min = x_nearest; min_cost = tree.cost(nearest_idx) + dist(x_nearest, x_new); min_idx = nearest_idx; else [min_cost, min_idx_in_near] = min(near_costs); x_min = tree.nodes(near_indices(min_idx_in_near), :); min_idx = near_indices(min_idx_in_near); end
% 6.7 添加新节点
tree.count = tree.count + 1; tree.nodes(tree.count, :) = x_new; tree.parent(tree.count) = min_idx; tree.cost(tree.count) = min_cost;
% 6.8 重连接
for i = 1:length(near_indices) near_idx = near_indices(i); if near_idx == min_idx continue; end new_cost = tree.cost(tree.count) + dist(tree.nodes(near_idx,:), x_new); if new_cost < tree.cost(near_idx) && ~check_collision(x_new, tree.nodes(near_idx,:), obstacles) % 重新连接 tree.parent(near_idx) = tree.count; tree.cost(near_idx) = new_cost; % 更新所有子节点的代价 update_child_costs(near_idx, tree); end end
% 6.9 检查是否到达目标
if ~goal_reached && dist(x_new, goal) <= goal_radius && ~check_collision(x_new, goal, obstacles) goal_reached = true; goal_node_idx = tree.count; best_cost = tree.cost(goal_node_idx); % 回溯得到路径 best_path = reconstruct_path(goal_node_idx, tree); fprintf('>>> 首次找到路径！迭代：%d, 代价：%.2f\n', iter, best_cost); % 在子图 1 绘制路径 subplot(1, 2, 1); plot(best_path(:,1), best_path(:,2), 'b-', 'LineWidth', 2); plot(best_path(:,1), best_path(:,2), 'ko', 'MarkerSize', 4, 'MarkerFaceColor', 'k'); end
% 6.10 动态可视化
if mod(iter, 100) == 0 subplot(1, 2, 1); % 只清除图形，不清除数据 cla; % 重新绘制障碍物 for i = 1:num_obstacles rectangle('Position', [obstacles(i,1)-obstacles(i,3), obstacles(i,2)-obstacles(i,3), ... obstacles(i,3)*2, obstacles(i,3)*2], ... 'Curvature', [1, 1], 'FaceColor', [0.8, 0.2, 0.2], 'EdgeColor', 'k'); end % 绘制起点和目标 plot(start(1), start(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g'); plot(goal(1), goal(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r'); rectangle('Position', [goal(1)-goal_radius, goal(2)-goal_radius, ... goal_radius*2, goal_radius*2], 'EdgeColor', 'r', 'LineStyle', '--'); % 绘制树结构 for i = 2:tree.count parent_idx = tree.parent(i); if parent_idx > 0 line([tree.nodes(i,1), tree.nodes(parent_idx,1)], ... [tree.nodes(i,2), tree.nodes(parent_idx,2)], ... 'Color', [0.7, 0.7, 0.7], 'LineWidth', 0.5); end end % 绘制当前最优路径 if ~isempty(best_path) plot(best_path(:,1), best_path(:,2), 'b-', 'LineWidth', 2); plot(best_path(:,1), best_path(:,2), 'ko', 'MarkerSize', 4, 'MarkerFaceColor', 'k'); end title(sprintf('RRT* 规划过程 (迭代：%d, 节点：%d)', iter, tree.count)); drawnow; if pause_time > 0 pause(pause_time); end end
% 6.11 检查是否有更优路径
if goal_reached for i = 1:tree.count if dist(tree.nodes(i,:), goal) <= goal_radius if tree.cost(i) < best_cost best_cost = tree.cost(i); goal_node_idx = i; best_path = reconstruct_path(goal_node_idx, tree); if mod(iter, 100) == 0 fprintf('>>> 找到更优路径！迭代：%d, 新代价：%.2f\n', iter, best_cost); end end end end end
% 记录代价历史
cost_history(iter) = best_cost; end
%% 7. 最终结果显示
fprintf('\n========== 规划结果 ==========\n');
fprintf('总迭代次数：%d\n', max_iter);
fprintf('总节点数：%d\n', tree.count);
fprintf('是否找到路径：%s\n', mat2str(goal_reached));
if goal_reached
fprintf('最优路径代价：%.4f\n', best_cost);
fprintf('最优路径节点数：%d\n', length(best_path));
% 在子图 2 绘制最终树和路径
subplot(1, 2, 2); % 绘制树结构 for i = 2:tree.count parent_idx = tree.parent(i); if parent_idx > 0 plot([tree.nodes(i,1), tree.nodes(parent_idx,1)], ... [tree.nodes(i,2), tree.nodes(parent_idx,2)], ... 'Color', [0.8, 0.8, 0.8], 'LineWidth', 0.3); end end % 绘制最优路径 plot(best_path(:,1), best_path(:,2), 'b-', 'LineWidth', 3); plot(best_path(:,1), best_path(:,2), 'ko', 'MarkerSize', 6, 'MarkerFaceColor', 'k'); % 绘制起点和目标 plot(start(1), start(2), 'go', 'MarkerSize', 12, 'MarkerFaceColor', 'g'); plot(goal(1), goal(2), 'ro', 'MarkerSize', 12, 'MarkerFaceColor', 'r'); title(sprintf('最优路径 (代价：%.2f, 节点数：%d)', best_cost, length(best_path))); % 显示路径坐标
fprintf('\n最优路径坐标 (前 10 个节点):\n');
num_to_show = min(10, length(best_path));
for i = 1:num_to_show fprintf(' 节点 %2d: (%.3f, %.3f)\n', i, best_path(i,1), best_path(i,2)); end
if length(best_path) > 10 fprintf(' ... 还有 %d 个节点\n', length(best_path)-10); end
% 更新子图 1 的标题
subplot(1, 2, 1); title(sprintf('RRT* 最终结果 (迭代：%d, 代价：%.2f)', max_iter, best_cost));
else
fprintf('未找到可行路径!\n');
fprintf('建议:\n');
fprintf(' 1. 增加迭代次数 (max_iter)\n');
fprintf(' 2. 减小步长 (step_size)\n');
fprintf(' 3. 调整障碍物位置\n');
fprintf(' 4. 增加目标采样概率\n');
% 绘制最终的树
subplot(1, 2, 1); for i = 2:tree.count parent_idx = tree.parent(i); if parent_idx > 0 plot([tree.nodes(i,1), tree.nodes(parent_idx,1)], ... [tree.nodes(i,2), tree.nodes(parent_idx,2)], ... 'b-', 'LineWidth', 1); end end title(sprintf('RRT* 规划失败 (迭代：%d)', max_iter)); end
%% 8. 绘制代价收敛曲线
figure('Position', [100, 100, 800, 400]); hold on; grid on; title('路径代价收敛曲线'); xlabel('迭代次数'); ylabel('路径代价');
if goal_reached % 找到第一个非 Inf 值的索引 valid_indices = find(~isinf(cost_history)); if ~isempty(valid_indices) first_valid = valid_indices(1); plot(1:max_iter, cost_history, 'b-', 'LineWidth', 2); plot(first_valid, cost_history(first_valid), 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r'); text(first_valid, cost_history(first_valid), sprintf(' 首次找到路径\n 迭代:%d', first_valid)); xlim([0, max_iter]); end
else text(0.5, 0.5, '未找到路径', 'HorizontalAlignment', 'center', ... 'FontSize', 16, 'Color', 'r'); end
%% 辅助函数定义
function collides = check_collision(p1, p2, obstacles) % 检查线段 p1-p2 是否与任何障碍物碰撞 collides = false; for i = 1:size(obstacles, 1) center = obstacles(i, 1:2); radius = obstacles(i, 3); % 计算线段到圆心的最短距离 [dist, ~, ~] = point_to_line_distance(center, p1, p2); if dist < radius collides = true; return; end end end
function [distance, t, closest] = point_to_line_distance(point, p1, p2) % 计算点到线段的最短距离 line_vec = p2 - p1; point_vec = point - p1; line_len = norm(line_vec); if line_len < 1e-10 distance = norm(point - p1); t = 0; closest = p1; return; end line_unitvec = line_vec / line_len; projection_length = dot(point_vec, line_unitvec); if projection_length < 0 t = 0; closest = p1; elseif projection_length > line_len t = 1; closest = p2; else t = projection_length / line_len; closest = p1 + t * line_vec; end distance = norm(point - closest); end
function update_child_costs(parent_idx, tree) % 更新子节点代价 for i = 1:tree.count if tree.parent(i) == parent_idx % 更新子节点代价 tree.cost(i) = tree.cost(parent_idx) + ... norm(tree.nodes(i,:) - tree.nodes(parent_idx,:)); % 递归更新孙节点 update_child_costs(i, tree); end end end
function path = reconstruct_path(node_idx, tree) % 从目标节点回溯到起点 path = []; current = node_idx; while current > 0 path = [tree.nodes(current, :); path]; current = tree.parent(current); end end
RRT* MATLAB 程序 ├── 主程序部分 (85%) │ ├── 1. 参数设置模块 │ ├── 2. 障碍物定义模块 │ ├── 3. 数据结构初始化 │ ├── 4. 图形界面初始化 │ ├── 5. 辅助函数定义（局部） │ ├── 6. RRT*主算法核心循环 │ ├── 7. 结果显示模块 │ └── 8. 收敛曲线绘制 └── 辅助函数部分 (15%) ├── check_collision() - 碰撞检测 ├── point_to_line_distance() - 点到线段距离计算 ├── update_child_costs() - 子节点代价更新 └── reconstruct_path() - 路径重构
% 控制参数
max_iter = 3000; % 最大迭代次数 - 控制算法运行时间
step_size = 0.5; % 扩展步长 - 控制每次扩展的距离
goal_radius = 0.5; % 目标区域半径 - 到达目标的容差
search_radius = 2.0; % 近邻搜索半径 - 重连接的范围
pause_time = 0.001; % 可视化暂停时间 - 动画速度
% 环境参数
map_bounds = [0, 10, 0, 10]; % 地图边界
start = [1, 1]; % 起点坐标
goal = [9, 9]; % 目标坐标
% 圆形障碍物：[x 坐标，y 坐标，半径]
obstacles = [ 3, 3, 1.5; % 障碍物 1 7, 2, 1.0; % 障碍物 2 ... % 更多障碍物 ];
% 树结构 - 存储 RRT*搜索树
tree.nodes = []; % 所有节点坐标矩阵 (n×2)
tree.parent = []; % 父节点索引数组
tree.cost = []; % 从起点到各节点的代价
tree.count = 0; % 节点计数器
% 路径记录变量
best_path = []; % 当前最优路径
best_cost = Inf; % 当前最优路径代价
goal_reached = false; % 是否到达目标标志
goal_node_idx = 0; % 到达目标的节点索引
% 创建双子图窗口
figure('Position', [100, 100, 1200, 500]);
% 子图 1: 动态规划过程 - 实时显示树生长
subplot(1, 2, 1);
% 绘制障碍物、起点、目标
% 动态更新树结构和路径
% 子图 2: 最终路径 - 显示最优结果
subplot(1, 2, 2);
% 显示最终优化的路径
% 距离计算函数 (匿名函数)
dist = @(p1, p2) sqrt(sum((p1 - p2).^2));
% 动态搜索半径计算 (匿名函数)
calc_search_radius = @(n) min(search_radius, 3.0*(log(n+1)/max(1,n))^(1/2));
这是算法的核心逻辑，按 RRT*的标准步骤实现：
for iter = 1:max_iter
% 6.1 采样 (Sample)
% 6.2 寻找最近邻 (Nearest)
% 6.3 向随机点扩展 (Steer)
% 6.4 检查碰撞 (Collision Check)
% 6.5 寻找近邻节点 (Near)
% 6.6 选择最优父节点 (ChooseParent) - RRT*关键步骤 1
% 6.7 添加新节点
% 6.8 重连接 (Rewire) - RRT*关键步骤 2
% 6.9 检查是否到达目标
% 6.10 动态可视化
% 6.11 检查是否有更优路径
end
% 打印规划结果统计
fprintf('总迭代次数：%d\n', max_iter);
fprintf('总节点数：%d\n', tree.count);
fprintf('是否找到路径：%s\n', mat2str(goal_reached));
fprintf('最优路径代价：%.4f\n', best_cost);
% 在图形界面绘制最终结果
% 显示路径坐标
% 绘制代价随迭代的变化曲线
% 显示算法收敛情况
figure('Position', [100, 100, 800, 400]);
plot(1:max_iter, cost_history, 'b-', 'LineWidth', 2);
1.
check_collision(p1, p2, obstacles)功能: 检查线段是否与障碍物碰撞 输入:p1,p2: 线段端点obstacles: 障碍物列表 输出: 布尔值，true 表示碰撞 原理: 计算线段到每个圆形障碍物圆心的最短距离2.
point_to_line_distance(point, p1, p2)功能: 计算点到线段的最短距离 输入: 点坐标和线段端点 输出: 距离、参数 t、最近点坐标 算法: 向量投影法3.
update_child_costs(parent_idx, tree)功能: 递归更新子节点的累积代价 原理: 当父节点被重连接后，所有子节点的代价都需要更新 时间复杂度: O(n) 在最坏情况下4.
reconstruct_path(node_idx, tree)功能: 从目标节点回溯到起点重建路径 输入: 目标节点索引 输出: 路径坐标序列 算法: 沿着 parent 指针反向遍历
% 使用结构体存储，包含四个字段
tree = struct( 'nodes', [], % n×2 矩阵，每行是一个节点坐标 'parent', [], % n×1 向量，parent[i]是节点 i 的父节点索引 'cost', [], % n×1 向量，cost[i]是从起点到节点 i 的代价 'count', 0 % 标量，当前节点总数 );
% 简单矩阵表示
best_path = [ x1, y1; % 起点 x2, y2; % 中间点 1 x3, y3; % 中间点 2 ... % 更多中间点 xn, yn; % 终点 ];
开始 ↓ 初始化参数和环境 ↓ 创建搜索树（仅含起点） ↓ 进入主循环（迭代 max_iter 次） │ ├─ 采样随机点 ├─ 找到最近邻节点 ├─ 向随机点扩展得到新节点 ├─ 碰撞检测 ├─ 寻找新节点的近邻节点 ├─ 在近邻中选择最优父节点 ├─ 将新节点加入树 ├─ 对近邻节点进行重连接优化 ├─ 检查是否到达目标 ├─ 更新最优路径 ├─ 动态可视化 │ ↓ 循环结束 ↓ 输出结果和绘制图形 ↓ 结束
实时可视化组件 树结构显示: 灰色细线表示搜索树 障碍物: 红色圆形 起点: 绿色圆点 目标区域: 红色虚线圆 当前最优路径: 蓝色粗线 路径节点: 黑色小圆点
双视图设计 左视图: 实时显示规划过程，每 100 次迭代更新一次 右视图: 只显示最终的最优路径和最终的树结构
运行后结果：
=== RRT* 路径规划算法演示 === 起点：(1.0, 1.0) 目标：(9.0, 9.0) 最大迭代次数：3000 障碍物数量：7 开始 RRT*规划... 迭代：100/3000, 节点数：45, 当前最优代价：Inf 迭代：200/3000, 节点数：99, 当前最优代价：Inf 迭代：300/3000, 节点数：169, 当前最优代价：Inf 迭代：400/3000, 节点数：242, 当前最优代价：Inf >>> 首次找到路径！迭代：428, 代价：14.91 迭代：500/3000, 节点数：318, 当前最优代价：14.91 迭代：600/3000, 节点数：379, 当前最优代价：14.91 迭代：700/3000, 节点数：448, 当前最优代价：14.91 迭代：800/3000, 节点数：528, 当前最优代价：14.91 迭代：900/3000, 节点数：593, 当前最优代价：14.91 迭代：1000/3000, 节点数：661, 当前最优代价：14.91 迭代：1100/3000, 节点数：736, 当前最优代价：14.91 迭代：1200/3000, 节点数：810, 当前最优代价：14.91 迭代：1300/3000, 节点数：879, 当前最优代价：14.91 迭代：1400/3000, 节点数：945, 当前最优代价：14.91 迭代：1500/3000, 节点数：1013, 当前最优代价：14.91 迭代：1600/3000, 节点数：, 当前最优代价： 迭代：, 节点数：, 当前最优代价： 迭代：, 节点数：, 当前最优代价： 迭代：, 节点数：, 当前最优代价： 迭代：, 节点数：, 当前最优代价： 迭代：, 节点数：, 当前最优代价： 迭代：, 节点数：, 当前最优代价： 迭代：, 节点数：, 当前最优代价： 规划结果 总迭代次数： 总节点数： 是否找到路径： 最优路径代价： 最优路径节点数： 最优路径坐标 (前 个节点): 节点 : (, ) 节点 : (, ) 节点 : (, ) 节点 : (, ) 节点 : (, ) 节点 : (, ) 节点 : (, ) 节点 : (, ) 节点 : (, ) 节点 : (, ) 还有 个节点
可视化：
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
生成新的随机RSA私钥和公钥pem证书。 在线工具，RSA密钥对生成器在线工具，online
基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。 在线工具，Mermaid 预览与可视化编辑在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online
将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML 转 Markdown 互为补充。 在线工具，Markdown 转 HTML在线工具，online