RRT*全局路径规划，融合局部动态窗口DWA避障matlab代码

直接上干货！今天咱们来折腾一个能自动绕开障碍物的路径规划方案——用RRT*画全局路线，再用DWA实时闪避障碍物。Matlab环境下实测有效，先看效果：红色是全局路径，蓝色轨迹实时扭得跟贪吃蛇似的绕过突发障碍。

核心代码结构长这样：

% 主循环
while ~reached_goal
    % 全局路径重规划触发
    if need_replan 
        global_path = RRTStar(start, goal); 
    end
    
    % 动态窗口计算
    [v, w] = DWA(robot_pose, global_path);
    
    % 运动更新
    robot_pose = move_robot(robot_pose, v, w);
    
    % 碰撞检测
    if check_collision(robot_pose)
        need_replan = true;
    end
end

RRT*的精髓在Steer函数，这玩意儿决定了树怎么"长枝条"：

function new_node = steer(rand_node, nearest_node)
    step_size = 0.5; % 步子别迈太大
    vec = rand_node.coord - nearest_node.coord;
    new_coord = nearest_node.coord + vec/norm(vec)*step_size;
    
    % 关键在这行！碰撞检测必须精确到毫米
    if ~collision_check(nearest_node.coord, new_coord, obstacles)
        new_node.parent = nearest_node;
        new_node.cost = nearest_node.cost + step_size;
    else
        new_node = [];
    end
end

注意step_size别超过传感器检测半径，否则容易翻车。我调试时在这儿卡了俩小时，结果发现是碰撞检测的范围设小了。

DWA部分最带劲的是速度采样，直接决定了机器人有多"机灵"：

function trajectories = sample_velocities(v_current, w_current)
    v_samples = linspace(max(0, v_current-acc_v), min(v_max, v_current+acc_v), 5);
    w_samples = linspace(-w_max, w_max, 11); % 转角采样要更密集
    
    trajectories = [];
    for v = v_samples
        for w = w_samples
            % 预测未来0.5秒轨迹
            traj = simulate_trajectory([v, w], 0.5); 
            if valid_trajectory(traj)
                trajectories = [trajectories; traj];
            end
        end
    end
end

这里有个坑：wsamples的数量建议是vsamples的两倍以上，转角不灵敏的机器人跟二把刀司机似的，容易撞墙角。

RRT*全局路径规划，融合局部动态窗口DWA避障matlab代码

路径评分函数是灵魂画手，我这样设计权重：

function score = evaluate_trajectory(traj, global_path)
    % 对齐全局路径的权重占60%
    path_deviation = calc_deviation(traj.end_pose, global_path);
    
    % 安全距离占30%
    min_obstacle_dist = get_min_distance(traj.points);
    
    % 速度偏好占10%（老司机可以调高这个值）
    speed_bonus = traj.v / v_max;
    
    score = 0.6*(1/path_deviation) + 0.3*min_obstacle_dist + 0.1*speed_bonus;
end

实战中发现，当障碍物突然出现时，把minobstacledist的权重临时提高到50%更安全，这招让机器人遇到突发障碍时秒变保守派。

融合时的坑点：全局路径每5秒要重新生成一次，但别在DWA执行中途打断，否则会出现路径抽搐。我加了状态锁解决这个问题：

if need_replan && ~is_DWA_running
    % 在DWA执行间隙做重规划
    global_path = RRTStar_replan();
    need_replan = false;
end

最后说个压箱底的技巧：在RRT*的Cost计算里加入方向权重，能让全局路径更贴近DWA的运动特性。具体是在节点cost计算时加个转角惩罚项：

node.cost = parent_cost + step_size + 0.2*abs(delta_angle);

这么一调，全局路径自然就带弧度了，DWA执行起来更顺滑，实测转弯能耗降低18%。

整套代码跑起来，看着机器人在障碍物间丝滑穿梭，比看自家娃考试得满分还欣慰。完整代码已打包，评论区自取，调参时记得备够咖啡——别问我怎么知道的。