RRT* 算法详解与 MATLAB 代码实现

三、算法核心优势与特点

渐进最优性：随着采样点数量趋于无穷，算法找到的路径代价以概率 1 收敛到全局最优解。这是 RRT* 与 RRT 最根本的区别。 '重连接'机制：这是实现最优性的核心。它允许树在生长过程中不断'修正'之前的连接，使整体结构更优。 概率完备性：如果解存在，随着采样次数增加，找到解的概率趋于 1。

四、RRT vs RRT* 直观对比

五、优缺点总结

优点：能处理高维、复杂约束的规划问题。具备渐进最优性，路径质量高。概率完备，理论保证强。 缺点：收敛到最优解的速度可能较慢。在狭窄通道或复杂环境中，初始找到解的效率可能不如 RRT。近邻搜索在高维空间中计算开销大。

六、典型应用

RRT* 及其后续变种（如 Informed RRT*）广泛应用于：机器人运动规划：机械臂抓取、无人机导航、自动驾驶。动画与游戏：角色路径规划。虚拟原型与 CAD：在复杂装配体中规划运动序列。

七、MATLAB 实现

%% 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

7.1 代码结构层次

RRT* MATLAB 程序 ├── 主程序部分 (85%) │ ├── 1. 参数设置模块 │ ├── 2. 障碍物定义模块 │ ├── 3. 数据结构初始化 │ ├── 4. 图形界面初始化 │ ├── 5. 辅助函数定义（局部） │ ├── 6. RRT*主算法核心循环 │ ├── 7. 结果显示模块 │ └── 8. 收敛曲线绘制 └── 辅助函数部分 (15%) ├── check_collision() - 碰撞检测 ├── point_to_line_distance() - 点到线段距离计算 ├── update_child_costs() - 子节点代价更新 └── reconstruct_path() - 路径重构

7.2 详细模块说明

1. 参数设置模块 (第 1 部分)

% 控制参数 
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]; % 目标坐标

2. 障碍物定义模块 (第 2 部分)

% 圆形障碍物：[x 坐标，y 坐标，半径] 
obstacles = [ 3, 3, 1.5; % 障碍物 1 7, 2, 1.0; % 障碍物 2 ... % 更多障碍物 ];

3. 数据结构初始化 (第 3 部分)

% 树结构 - 存储 RRT*搜索树 
tree.nodes = []; % 所有节点坐标矩阵 (n×2) 
tree.parent = []; % 父节点索引数组 
tree.cost = []; % 从起点到各节点的代价 
tree.count = 0; % 节点计数器 
% 路径记录变量 
best_path = []; % 当前最优路径 
best_cost = Inf; % 当前最优路径代价 
goal_reached = false; % 是否到达目标标志 
goal_node_idx = 0; % 到达目标的节点索引

4. 图形界面初始化 (第 4 部分)

% 创建双子图窗口 
figure('Position', [100, 100, 1200, 500]); 
% 子图 1: 动态规划过程 - 实时显示树生长 
subplot(1, 2, 1); 
% 绘制障碍物、起点、目标 
% 动态更新树结构和路径 
% 子图 2: 最终路径 - 显示最优结果 
subplot(1, 2, 2); 
% 显示最终优化的路径

5. 局部辅助函数定义 (第 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*主算法核心循环 (第 6 部分 - 最重要)

这是算法的核心逻辑，按 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

7. 结果显示模块 (第 7 部分)

% 打印规划结果统计 
fprintf('总迭代次数：%d\n', max_iter); 
fprintf('总节点数：%d\n', tree.count); 
fprintf('是否找到路径：%s\n', mat2str(goal_reached)); 
fprintf('最优路径代价：%.4f\n', best_cost); 
% 在图形界面绘制最终结果 
% 显示路径坐标

8. 收敛曲线绘制 (第 8 部分)

% 绘制代价随迭代的变化曲线 
% 显示算法收敛情况 
figure('Position', [100, 100, 800, 400]); 
plot(1:max_iter, cost_history, 'b-', 'LineWidth', 2);

7.3 辅助函数详解

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 指针反向遍历

7.4 关键数据结构

1. 树结构表示

% 使用结构体存储，包含四个字段 
tree = struct( 'nodes', [], % n×2 矩阵，每行是一个节点坐标 'parent', [], % n×1 向量，parent[i]是节点 i 的父节点索引 'cost', [], % n×1 向量，cost[i]是从起点到节点 i 的代价 'count', 0 % 标量，当前节点总数 );

2. 路径表示

% 简单矩阵表示 
best_path = [ x1, y1; % 起点 x2, y2; % 中间点 1 x3, y3; % 中间点 2 ... % 更多中间点 xn, yn; % 终点 ];

7.5 算法流程控制

开始 ↓ 初始化参数和环境 ↓ 创建搜索树（仅含起点） ↓ 进入主循环（迭代 max_iter 次） │ ├─ 采样随机点 ├─ 找到最近邻节点 ├─ 向随机点扩展得到新节点 ├─ 碰撞检测 ├─ 寻找新节点的近邻节点 ├─ 在近邻中选择最优父节点 ├─ 将新节点加入树 ├─ 对近邻节点进行重连接优化 ├─ 检查是否到达目标 ├─ 更新最优路径 ├─ 动态可视化 │ ↓ 循环结束 ↓ 输出结果和绘制图形 ↓ 结束

7.6 可视化系统

实时可视化组件 树结构显示: 灰色细线表示搜索树 障碍物: 红色圆形 起点: 绿色圆点 目标区域: 红色虚线圆 当前最优路径: 蓝色粗线 路径节点: 黑色小圆点

双视图设计 左视图: 实时显示规划过程，每 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, 节点数：1082, 当前最优代价：14.91 迭代：1700/3000, 节点数：1146, 当前最优代价：14.91 迭代：1800/3000, 节点数：1217, 当前最优代价：14.91 迭代：1900/3000, 节点数：1288, 当前最优代价：14.91 迭代：2000/3000, 节点数：1361, 当前最优代价：14.91 迭代：2100/3000, 节点数：1428, 当前最优代价：14.79 迭代：2200/3000, 节点数：1496, 当前最优代价：14.79 迭代：2300/3000, 节点数：1557, 当前最优代价：14.79 迭代：2400/3000, 节点数：1618, 当前最优代价：14.79 迭代：2500/3000, 节点数：1686, 当前最优代价：14.79 迭代：2600/3000, 节点数：1750, 当前最优代价：14.79 迭代：2700/3000, 节点数：1818, 当前最优代价：14.79 迭代：2800/3000, 节点数：1881, 当前最优代价：14.79 迭代：2900/3000, 节点数：1950, 当前最优代价：14.79 迭代：3000/3000, 节点数：2017, 当前最优代价：14.79 ========== 规划结果 ========== 总迭代次数：3000 总节点数：2018 是否找到路径：true 最优路径代价：14.7917 最优路径节点数：24 最优路径坐标 (前 10 个节点): 节点 1: (1.000, 1.000) 节点 2: (1.849, 1.254) 节点 3: (3.238, 1.423) 节点 4: (4.521, 1.927) 节点 5: (5.530, 2.503) 节点 6: (6.188, 2.987) 节点 7: (6.758, 3.485) 节点 8: (6.930, 3.954) 节点 9: (6.640, 4.659) 节点 10: (6.508, 5.141) ... 还有 14 个节点

可视化：