三次B样条RRT代码_最小曲率三次b样条-优快云博客

本文链接：https://blog.youkuaiyun.com/weixin_55617272/article/details/145907963

function [path, real_iterations, path_cost, node_num, effect_node_num, Time_Cost] = RRT(start, goal, obstacles, maxIterations, stepSize, threshold, r_min)
% 带曲率约束的RRT路径规划算法
% 输入参数：
%   start: 起点坐标 [x, y]
%   goal: 终点坐标 [x, y]
%   obstacles: 障碍物矩阵，每行表示一个矩形障碍物[x_min, y_min, x_max, y_max]
%   maxIterations: 最大迭代次数
%   stepSize: 扩展步长
%   threshold: 连接目标的距离阈值
%   r_min: 最小转弯半径
% 输出参数：
%   path: 规划路径坐标矩阵
%   real_iterations: 实际迭代次数
%   path_cost: 路径总成本
%   node_num: 树节点总数
%   effect_node_num: 有效路径节点数
%   Time_Cost: 算法运行时间

    tic; % 开始计时
    
    % 初始化树的第一个节点（起点）
    startTheta = atan2(goal(2)-start(2), goal(1)-start(1)); % 初始朝向目标点
    treeA = struct('node', start, 'parent', 0, 'NP_Cost', 0, 'theta', startTheta);
    
    % 可视化初始化
    figure;
    hold on;
    axis equal;
    % 绘制起点（蓝色圆形）
    rectangle('Position', [start(1)-5, start(2)-5, 10, 10],...
              'Curvature', [1,1], 'FaceColor', 'b');
    % 绘制终点（红色圆形） 
    rectangle('Position', [goal(1)-5, goal(2)-5, 10, 10],...
              'Curvature', [1,1], 'FaceColor', 'r');
    
    real_iterations = 0; % 实际迭代计数器
    % 主循环
    for i = 1:maxIterations
        % 生成随机采样点
        randomNode = GenerateRandomNode();
        real_iterations = real_iterations + 1;
        
        % 扩展树（带曲率约束）
        [newNodeA, nearestNodeAIndex] = Extend_Steer(treeA, randomNode, obstacles, stepSize, r_min);
        
        if ~isempty(newNodeA)
            % 将新节点添加到树中
            treeA(end+1) = struct('node', newNodeA.node, 'parent', nearestNodeAIndex,'NP_Cost', stepSize, 'theta', newNodeA.theta);
            
            % 绘制扩展路径（绿色线段）
            [arcPoints, ~] = generateArc(treeA(nearestNodeAIndex).node,treeA(nearestNodeAIndex).theta,stepSize,newNodeA.curvature, 20);
            plot(arcPoints(:,1), arcPoints(:,2), 'g-', 'LineWidth', 0.8);
            drawnow limitrate; % 实时更新图形
            % 检查是否可连接到目标（传递newNodeA的theta）
            [connect_flag, Last_NP_Cost] = isConnect_Steer(newNodeA.node, newNodeA.theta, goal, obstacles, threshold, r_min);

            % 主循环中构建路径后调用三次B样条平滑
            if connect_flag
                % 构建原始路径
                [path, path_cost] = ConstructPath_Steer(treeA, numel(treeA), goal, Last_NP_Cost, stepSize);
                % 三次B样条平滑处理（关键修改）
                smooth_path = path_smoothing_with_b_spline(path, r_min);
                path = smooth_path;
        
                % 碰撞检测（必须对平滑后的路径重新检测）
                collision = check_smooth_path_collision(smooth_path, obstacles);
                if collision
                    continue; % 如果碰撞则放弃此路径
                end
        
                node_num = numel(treeA);
                effect_node_num = size(path,1);
                Time_Cost = toc;  % 记录总耗时
                path = smooth_path;
                return;   % 找到路径，提前返回
            end % 闭合 if connect_flag
      end % 闭合 if ~isempty(newNodeA)
end % 闭合 for 循环
    
    % 未找到路径的默认返回值
    path = [];
    path_cost = 0;
    node_num = 0;
    effect_node_num = 0;
    Time_Cost = 0;
end

%% ====================== 关键子函数实现 ======================
function [newNode, nearestNodeIndex] = Extend_Steer(tree, target, obstacles, stepSize, r_min)
% 带曲率约束的树扩展函数
% 输入：
%   tree: 当前搜索树
%   target: 目标点坐标
%   obstacles: 障碍物信息
%   stepSize: 扩展步长
%   r_min: 最小转弯半径
% 输出：
%   newNode: 新节点信息（包含位置、朝向、曲率）
%   nearestNodeIndex: 最近节点索引

    % 寻找最近节点
    [nearestNode, nearestNodeIndex] = FindNearestNode(tree, target);
    currentPos = nearestNode.node;
    currentTheta = tree(nearestNodeIndex).theta;
    
    % 生成三种可能路径（左转/右转/直行）
    curvatures = [1/r_min, -1/r_min, 0]; % 曲率集合
    bestDist = inf; % 最佳距离初始化
    bestNode = [];  % 最佳节点初始化
    
    % 遍历所有可能曲率
    for kappa = curvatures
        % 生成圆弧路径终点
        [newPos, newTheta] = generateArc(currentPos, currentTheta, stepSize, kappa);
        dist = norm(target - newPos); % 计算到目标的距离
        % 更新最佳节点
        if dist < bestDist
            % 碰撞检测
            collision = checkArcCollision(currentPos, newPos, currentTheta, kappa, stepSize, obstacles);
            if ~collision
                bestDist = dist;
                bestNode = struct('node', newPos, 'theta', newTheta, 'curvature', kappa);
            end
        end
    end
    newNode = bestNode;
end

function [out1, out2] = generateArc(varargin)
% 重载函数：圆弧路径生成
% 模式1：生成单个圆弧终点（输入4参数）
% 输入：
%   startPos: 起点坐标
%   startTheta: 初始朝向（弧度）
%   stepSize: 步长
%   curvature: 曲率（1/r_min）
% 输出：
%   newPos: 终点坐标
%   newTheta: 新的朝向角度
%
% 模式2：生成离散采样点（输入5参数）
% 输入：
%   前4参数同上 + numSegments: 采样段数
% 输出：
%   points: 路径点坐标矩阵
%   thetas: 各点朝向角度向量

    if nargin == 4
        % ========== 模式1：生成单个圆弧终点 ==========
        startPos = varargin{1};
        startTheta = varargin{2};
        stepSize = varargin{3};
        curvature = varargin{4};
    if curvature == 0 % 直行情况
            newPos = startPos + stepSize * [cos(startTheta), sin(startTheta)];
            newTheta = startTheta;
    else % 圆弧运动
            r = 1/abs(curvature); % 转弯半径
            deltaTheta = stepSize * curvature; % 角度变化量
            
            % 计算圆心坐标
            if curvature > 0 % 左转
                center = startPos + r * [-sin(startTheta), cos(startTheta)];
            else % 右转
                center = startPos + r * [sin(startTheta), -cos(startTheta)];
            end
            
            % 计算终点位置和方向
            newTheta = startTheta + deltaTheta;
            newPos = center + r * [sin(newTheta), -cos(newTheta)];
    end
        out1 = newPos;
        out2 = newTheta;
        
    elseif nargin == 5
        % ========== 模式2：生成离散采样点 ==========
        startPos = varargin{1};
        startTheta = varargin{2};
        stepSize = varargin{3};
        curvature = varargin{4};
        numSegments = varargin{5};
  
        % 初始化输出矩阵
        points = zeros(numSegments+1, 2);
        thetas = zeros(numSegments+1, 1);
        points(1,:) = startPos;
        thetas(1) = startTheta;
        
        % 分段生成路径点
        for i = 1:numSegments
            segStep = stepSize/numSegments*i; % 当前段步长
            [pos, theta] = generateArc(startPos, startTheta, segStep, curvature);
            points(i+1,:) = pos;
            thetas(i+1) = theta;
        end
        out1 = points;
        out2 = thetas;
    end
end


function collision = checkArcCollision(startPos, endPos, startTheta, curvature, stepSize, obstacles)
% 圆弧碰撞检测（离散采样法）
% 输入：
%   startPos: 起点坐标
%   endPos: 终点坐标
%   startTheta: 初始朝向
%   curvature: 曲率
%   stepSize: 步长
%   obstacles: 障碍物信息
% 输出：
%   collision: 是否发生碰撞（true/false）

    collision = false;
    numSegments = 20; % 采样点数
    
    % 生成离散路径点
    [arcPoints, ~] = generateArc(startPos, startTheta, stepSize, curvature, numSegments);
    
    % 逐段检测碰撞
    for i = 1:size(arcPoints,1)-1
        if CollisionCheck(arcPoints(i,:), arcPoints(i+1,:), obstacles)
            collision = true;
            return;
        end
    end
end

%% ====================== 修改isConnect_Steer函数 ======================
function [flag, NP_Cost] = isConnect_Steer(newNodeA, thetaA, goal, obstacles, threshold, r_min)
% 带曲率约束的终点连接检查
% 输入：
%   newNodeA: 当前节点坐标
%   thetaA:   当前节点的朝向角
%   goal:     目标点坐标
%   obstacles: 障碍物信息
%   threshold: 连接阈值
%   r_min:    最小转弯半径
% 输出：
%   flag:     是否可连接
%   NP_Cost:  连接路径成本

    NP_Cost = norm(newNodeA - goal); % 直线距离
    if NP_Cost > threshold
        flag = false;
        return;
    end
    
    % 生成Dubins路径（使用传入的thetaA作为起点方向）
    q0 = [newNodeA, thetaA]; % 起点坐标及方向
    q1 = [goal, atan2(newNodeA(2)-goal(2), newNodeA(1)-goal(1))]; % 终点朝向起点
    [path, ~] = dubins_curve(q0, q1, r_min, 0.1); % step=0.1米
    
    % 检测路径是否存在
    if isempty(path)
        flag = false;
        return;
    end
    
    % 碰撞检测
    for i = 1:size(path,1)-1
        if CollisionCheck(path(i,1:2), path(i+1,1:2), obstacles)
            flag = false;
            return;
        end
    end
    flag = true;
end

%% ====================== 基础功能函数 ======================
function randomNode = GenerateRandomNode()
% 生成随机节点（500x500环境）
    randomNode = randi([1,500],1,2); % 生成1x2的随机整数坐标
end

function [nearestNode, nearestNodeIndex] = FindNearestNode(tree, targetNode)
% 寻找最近节点
% 输入：
%   tree: 当前搜索树
%   targetNode: 目标点坐标
% 输出：
%   nearestNode: 最近节点结构体
%   nearestNodeIndex: 最近节点索引

    % 计算所有节点到目标的距离
    distances = arrayfun(@(x) norm(x.node - targetNode), tree);
    [~, nearestNodeIndex] = min(distances);
    nearestNode = tree(nearestNodeIndex);
end

function collision = CollisionCheck(nodeA, nodeB, obstacles)
% 直线段碰撞检测
% 输入：
%   nodeA, nodeB: 线段端点坐标
%   obstacles: 障碍物矩阵
% 输出：
%   collision: 是否碰撞

    collision = false;
    % 遍历所有障碍物
     for i = 1:size(obstacles,1)
        if Intersect(nodeA, nodeB, obstacles(i,:))
            collision = true;
            return;
        end
    end
end

function [path,path_cost] = ConstructPath_Steer(treeA,AendIndex,goal,end_NP_Cost,input_stepSize)
% 路径重建函数
% 输入：
%   treeA: 搜索树
%   AendIndex: 终点节点索引
%   goal: 目标点坐标
%   end_NP_Cost: 终点连接成本
%   input_stepSize: 步长
% 输出：
%   path: 路径坐标矩阵
%   path_cost: 路径总成本

    path = [];
    path_cost = 0;
    path = [path; goal]; % 添加目标点
    path_cost = path_cost + end_NP_Cost;
    
    % 逆向遍历树结构
    nodeA = AendIndex;
    while nodeA ~= 0
        path = [treeA(nodeA).node; path]; % 向前插入节点
        nodeA = treeA(nodeA).parent;      % 移动到父节点
        path_cost = path_cost + input_stepSize;
    end
    path_cost = path_cost - input_stepSize; % 修正最后一步多余成本
end

%% ====================== 辅助函数 ======================
function intersect = Intersect(nodeA, nodeB, obstacle)
% 线段与矩形障碍物相交检测
% 输入：
%   nodeA, nodeB: 线段端点
%   obstacle: 障碍物坐标[x_min,y_min,x_max,y_max]
% 输出：
%   intersect: 是否相交

    lineSegment = [nodeA; nodeB];
    % 定义障碍物的四条边
    obstacleEdges = [ 
        obstacle(1), obstacle(2), obstacle(1), obstacle(4);  % 左边
        obstacle(1), obstacle(2), obstacle(3), obstacle(2);  % 下边
        obstacle(3), obstacle(2), obstacle(3), obstacle(4);  % 右边
        obstacle(1), obstacle(4), obstacle(3), obstacle(4)]; % 上边
    
    intersect = false;
    % 检查与各边是否相交
    for i = 1:4
        if DoLinesIntersect(lineSegment, obstacleEdges(i,:))
            intersect = true;
            return;
        end
    end
end

function intersect = DoLinesIntersect(line1, line2)
% 线段相交检测
% 输入：
%   line1: 线段1端点坐标矩阵[2x2]
%   line2: 线段2端点坐标向量[x1,y1,x2,y2]
% 输出：
%   intersect: 是否相交

    % 解析坐标
    x1 = line1(1,1); y1 = line1(1,2);
    x2 = line1(2,1); y2 = line1(2,2);
    x3 = line2(1);   y3 = line2(2);
    x4 = line2(3);   y4 = line2(4);
    
    % 计算向量叉乘判断相交
    intersect = ((x1 - x2)*(y3 - y1) - (y1 - y2)*(x3 - x1)) * ...
                ((x1 - x2)*(y4 - y1) - (y1 - y2)*(x4 - x1)) < 0 && ...
                ((x3 - x4)*(y1 - y3) - (y3 - y4)*(x1 - x3)) * ...
                ((x3 - x4)*(y2 - y3) - (y3 - y4)*(x2 - x3)) < 0;
end

%% ====================== 关键修改部分 ======================
%% ====================== 修改后的路径平滑函数 ======================
function smooth_path = path_smoothing_with_b_spline(path, r_min)
    % 参数说明：
    %   interp_step : 插值步长 (增大此值可降低精度，取值范围0.05-0.2)
    %   max_iter    : 最大迭代次数 (减少此值可加速收敛，取值范围10-30)
    %   alpha       : 控制点调整系数 (减小此值可降低调整幅度)
    
    interp_step = 0.1;    % 原值0.01 → 增大10倍（精度降低）
    max_iter = 15;        % 原值50 → 减少迭代次数
    alpha = 0.05;         % 原值0.1 → 减小调整幅度
    
    n = size(path,1);
    if n < 4
        smooth_path = path;
        return; 
    end

    control_points = path;
    
    for iter = 1:max_iter
        % 生成粗粒度路径点
        [smooth_points, derivs] = generate_b_spline(control_points, interp_step);
        
        % 快速曲率检查（采样检测代替全路径检测）
        curvature = compute_curvature(smooth_points(1:5:end,:), derivs(1:5:end,:));
        
        if min(1./curvature) >= r_min * 0.9  % 放宽曲率约束阈值
            break;
        end
        
        % 简化控制点调整（仅调整关键节点）
        for i = 2:3:n-1  % 间隔调整控制点
            tangent = (derivs(i,1) - derivs(i-1,1)) / interp_step; 
            if norm(tangent) < 1e-6
                continue;
            end
            normal = [-tangent(2), tangent(1)] / norm(tangent);
            move_dist = (1/r_min - curvature(i)) * alpha;
            
            % 限制调整幅度
            move_dist = sign(move_dist) * min(abs(move_dist), stepSize*0.5);
            
            control_points(i,:)   = control_points(i,:)   + normal * move_dist;
            control_points(i+1,:) = control_points(i+1,:) + normal * move_dist;
        end
    end
    
    % 生成最终低精度路径
    smooth_path = generate_b_spline(control_points, interp_step);
end

%% ====================== 配套修改的B样条生成函数 =====================
function [points, derivs] = generate_b_spline(control_points, step)
    % 参数说明：
    %   step : 参数空间步长 (建议值0.05-0.2)
    
    % 添加控制点数量校验
    if size(control_points,1) < 4
        points = control_points;
        derivs = zeros(size(control_points));
        return;
    end
    
    % 计算实际需要的插值点数（限制最大数量）
    max_points = 100;  % 限制最大插值点数
    t_values = linspace(0, 1, ceil(1/step));
    if length(t_values) > max_points
        t_values = linspace(0, 1, max_points);
    end
    
    points = [];
    derivs = [];
    
    for t = t_values
                % 计算贝塞尔基函数和导数
        [b0,b1,b2,b3, db0, db1, db2, db3] = compute_bspline_basis(t_norm);

        % 计算位置和导数
        x = control_points(1,1)*b0 + control_points(2,1)*b1 + control_points(3,1)*b2 + control_points(4,1)*b3;
        y = control_points(1,2)*b0 + control_points(2,2)*b1 + control_points(3,2)*b2 + control_points(4,2)*b3;

        dx = control_points(1,1)*db0 + control_points(2,1)*db1 + control_points(3,1)*db2 + control_points(4,1)*db3;
        dy = control_points(1,2)*db0 + control_points(2,2)*db1 + control_points(3,2)*db2 + control_points(4,2)*db3;        
        
        % 计算贝塞尔基函数和导数
        [b0,b1,b2,b3, db0, db1, db2, db3] = compute_bspline_basis(t_norm);

        % 计算位置和导数
        x = control_points(1,1)*b0 + control_points(2,1)*b1 + control_points(3,1)*b2 + control_points(4,1)*b3;
        y = control_points(1,2)*b0 + control_points(2,2)*b1 + control_points(3,2)*b2 + control_points(4,2)*b3;

        dx = control_points(1,1)*db0 + control_points(2,1)*db1 + control_points(3,1)*db2 + control_points(4,1)*db3;
        dy = control_points(1,2)*db0 + control_points(2,2)*db1 + control_points(3,2)*db2 + control_points(4,2)*db3;
    end
end

function [b0,b1,b2,b3, db0, db1, db2, db3] = compute_bspline_basis(t)
    t = max(min(t, 1), 0);
    b0 = 1 - 3*t + 3*t^2 - t^3;
    b1 = 3*t - 6*t^2 + 3*t^3;
    b2 = 3*t^2 - 2*t^3;
    b3 = t^3;

    db0 = -3 + 6*t - 3*t^2;
    db1 = 3 - 12*t + 6*t^2;
    db2 = 6*t - 6*t^2;
    db3 = 6*t^2 - 6*t;
end

function kappa = compute_bspline_curvature(control_points, t_param, derivs)
    % 计算三次B样条在参数t处的曲率
    p0 = control_points(1,:);
    p1 = control_points(2,:);
    p2 = control_points(3,:);
    p3 = control_points(4,:);

    [b0,b1,b2,b3, db0, db1, db2, db3] = compute_bspline_basis(t_param);

    x = p0(1)*b0 + p1(1)*b1 + p2(1)*b2 + p3(1)*b3;
    y = p0(2)*b0 + p1(2)*b1 + p2(2)*b2 + p3(2)*b3;

    dx = p0(1)*db0 + p1(1)*db1 + p2(1)*db2 + p3(1)*db3;
    dy = p0(2)*db0 + p1(2)*db1 + p2(2)*db2 + p3(2)*db3;

    d2x = p0(1)*(6*t_param - 6) + p1(1)*(12*t_param - 12) + p2(1)*(6 - 12*t_param) + p3(1)*(12*t_param - 6);
    d2y = p0(2)*(6*t_param - 6) + p1(2)*(12*t_param - 12) + p2(2)*(6 - 12*t_param) + p3(2)*(12*t_param - 6);

    numerator = (dx*dy' - dy*dx')^2;
    denominator = (dx^2 + dy^2)^(3/2);
    if denominator < 1e-9
        kappa = 0;
    else
        kappa = sqrt(numerator) / denominator;
    end
end

% 3. 新增碰撞检测函数
function collision = check_smooth_path_collision(smooth_path, obstacles)
    % 对平滑后的路径进行碰撞检测
    collision = false;
    for i = 1:size(smooth_path,1)-1
        if CollisionCheck(smooth_path(i,:), smooth_path(i+1,:), obstacles)
            collision = true;
            return;
        end
    end
end

%function [path, cost] = dubins_curve(q0, q1, rho, step)
% 简化的Dubins路径生成（示例实现）
% 注意：此为示意实现，实际应使用完整Dubins算法
% 输入：
%   q0: 起点[x,y,theta]
%   q1: 终点[x,y,theta]
%   rho: 最小转弯半径
%   step: 采样步长
% 输出：
%   path: 路径坐标矩阵
%   cost: 路径成本

function [path, cost] = dubins_curve(q0, q1, rho, step)
% 修正后的Dubins路径生成函数（鲁棒性增强）
% 输入：
%   q0: 起点 [x,y] 或 [x,y,theta]
%   q1: 终点 [x,y] 或 [x,y,theta]
%   rho: 最小转弯半径
%   step: 步长（基于实际距离）

    % 自动补充方向角（朝向目标点）
    if numel(q0) == 2
        theta0 = atan2(q1(2)-q0(2), q1(1)-q0(1)); % 起点朝向终点
        q0 = [q0, theta0];
    end
    if numel(q1) == 2
        theta1 = atan2(q0(2)-q1(2), q0(1)-q1(1)); % 终点朝向起点
        q1 = [q1, theta1];
    end

    % 创建Dubins连接对象
    conn = dubinsConnection('MinTurningRadius', rho);
    
    % 生成路径段（捕获可能的错误）
    try
        [pathSegObj, cost] = connect(conn, q0, q1);
    catch
        path = [];
        cost = inf;
        return;
    end
    
    % 检查路径是否有效
    if isempty(pathSegObj) || ~isobject(pathSegObj) || ~isfield(pathSegObj, 'Length')
        path = [];
        cost = inf;
        return;
    end
    
    % 插值获取路径点（基于实际长度）
    try
        if pathSegObj.Length == 0
            path = [q0(1:2); q1(1:2)];
        else
            path = interpolate(pathSegObj, 0:step:pathSegObj.Length);
        end
    catch
        path = [];
        cost = inf;
        return;
    end
    
    % 提取二维坐标（空值保护）
    if ~isempty(path) && size(path,2) >=2
        path = path(:,1:2); 
    else
        path = [];
    end
end

% ====================== 基于RRT的路径规划调用示例 ======================
% 该代码演示如何调用RRT算法进行带曲率约束的路径规划
% 包含环境配置、算法参数设置、结果可视化全流程

clc; clear; close all;  % 清空工作区，关闭所有图形窗口

%% ====================== 环境参数配置 ======================
% 定义起点和终点坐标（单位：像素/米）
start = [30, 30];       % 起点坐标 (蓝色圆形标记)
goal = [470, 470];      % 终点坐标 (红色圆形标记)

% 定义障碍物矩阵（每行定义一个矩形障碍物[x_min, y_min, x_max, y_max]）
obstacles = [           
    120,  80, 180, 220;    % 障碍物1（横向长条形）
    250, 300, 350, 450;    % 障碍物2（纵向障碍）
    200, 150, 400, 200;    % 障碍物3（横向中央障碍）
    100, 400, 300, 430     % 障碍物4（L型组合障碍）
]; 

%% ====================== 算法参数配置 ======================
% 核心参数说明：
maxIterations = 5000;   % 最大迭代次数（典型范围500-10000，复杂环境需增大）
stepSize = 15;          % 扩展步长（建议值5-20，需小于障碍物间隙）
threshold = 20;         % 连接目标的距离阈值（建议值10-20，需大于步长）
r_min = 20;              % 最小转弯半径（需满足r_min > stepSize/2）

%% ====================== 执行路径规划 ======================
% 调用RRT主函数（返回值说明见函数注释）
[path, real_iters, cost, nodes, eff_nodes, time] = ...
    RRT(start, goal, obstacles, maxIterations, stepSize, threshold, r_min);

%% ====================== 结果可视化 ======================
% 创建可视化图形窗口
figure('Name','RRT路径规划演示','NumberTitle','off') 
hold on; axis equal;
xlabel('X坐标'); ylabel('Y坐标'); 
title(sprintf('RRT路径规划结果(耗时%.1f秒)', time));
axis([0 500 0 500]);  % 设置坐标系范围

% 绘制障碍物（灰色填充矩形）
for i = 1:size(obstacles,1)
    obs = obstacles(i,:);
    rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)],...
              'FaceColor', [0.8 0.8 0.8], 'EdgeColor','k', 'LineWidth',1);
end

% 绘制起点和终点（带方向指示）
plot(start(1), start(2), 'bo', 'MarkerSize',8, 'MarkerFaceColor','b');
plot(goal(1), goal(2), 'ro', 'MarkerSize',8, 'MarkerFaceColor','r');
text(start(1)+5, start(2)-10, '起点','Color','b','FontSize',10);
text(goal(1)+5, goal(2)-10, '终点','Color','r','FontSize',10);

% 绘制规划路径（如果找到）
if ~isempty(path)
    plot(path(:,1), path(:,2), 'g-', 'LineWidth', 2);  % 原始路径（绿色）
    plot(path(:,1), path(:,2), 'b.', 'MarkerSize',12); % 路径节点（蓝色圆点）
    fprintf('============= 算法统计信息 =============\n');
    fprintf('实际迭代次数: %d次\n', real_iters);
    fprintf('路径总长度: %.2f单位\n', cost);
    fprintf('生成节点总数: %d个\n', nodes);
    fprintf('有效路径节点: %d个\n', eff_nodes);
else
    fprintf('============= 路径规划失败 =============\n');
    fprintf('未找到可行路径! 建议调整以下参数:\n');
    fprintf('1. 增大迭代次数(当前:%d)\n', maxIterations);
    fprintf('2. 减小步长(当前:%.1f)\n', stepSize);
    fprintf('3. 增大最小转弯半径(当前:%.1f)\n', r_min);
end

%% ====================== 参数调整指南 ======================
% 1. 障碍物密集时：增大maxIterations(至8000+)和减小stepSize(至5-8)
% 2. 路径曲率过大时：增大r_min或减小stepSize
% 3. 连接失败时：适当增大threshold(15-25)
% 4. 算法速度优化：降低B样条插值精度（修改path_smoothing_with_b_spline函数）