如何计算迭代次数和路径成本，针对本人所写的引导RRT算法

在您提供的RRT算法代码中，迭代次数和路径成本的计算可以通过以下方式实现：
迭代次数
迭代次数指的是RRT算法主循环执行的次数，即从开始到找到目标点或达到最大迭代次数为止。在您的代码中，这个值可以通过变量i来获取，当循环结束时，i的值就是实际执行的迭代次数。
您可以在循环结束后添加如下代码来显示迭代次数：
real_iterations = i; % 实际迭代次数
disp(['实际迭代次数：', num2str(real_iterations)]);
路径成本
路径成本是指从起点到终点所经过的路径的总长度。在RRT算法中，这通常是通过将路径上所有段的长度累加来计算的。在您的代码中，可以在提取路径后，通过计算路径中相邻点之间的距离并累加来得到路径成本。
您可以在extract_path函数调用后添加如下代码来计算和显示路径成本：
% 提取路径
if exist('goal_idx', 'var')
    rrt_path = extract_path(tree, goal_idx);
    % 计算路径成本
    path_cost = 0;
    for j = 1:(length(rrt_path) - 1)
        path_cost = path_cost + norm(rrt_path(j+1, :) - rrt_path(j, :));
    end
    % 显示路径成本
    disp(['路径成本：', num2str(path_cost)]);
    % 打印语句
    fprintf('RRT路径已生成，路径长度: %d步\n', length(rrt_path));
else
    rrt_path = [];
    warning('RRT规划失败，未到达目标');
end
在这段代码中，norm函数用于计算两个点之间的欧几里得距离，然后通过循环将所有相邻点之间的距离累加起来，得到总路径成本。
请将上述代码段集成到您的RRT算法主函数中的适当位置，以确保正确计算和显示迭代次数和路径成本。