改进的A星DWA混合matlab路径规划算法。环境和参数设置完全一样方便对比算法的优劣路径规划代码。改进的A星算法。A星算法采用了删除冗余节点方法。移动路径采用DWA算法来规划。A星算法可以提供子目标点给人工势场算法来引导DWA算法远离陷阱区域。显示方式为动态显示方便观察避障过程。 算法核心亮点改进 A(全局规划)传统 A* 生成的路径包含大量冗余折点。改进策略采用视线检测 (Line-of-Sight) 算法删除中间冗余节点只保留关键转折点作为子目标点。DWA (局部规划)在速度空间 (v, omega) 采样模拟未来轨迹。评价函数Heading (朝向目标) Dist (避障) Vel (速度)。混合机制 (A引导 DWA)A* 不直接控制机器人而是提供动态子目标点 (Sub-goal)。当 DWA 陷入局部最优如 U 型陷阱时A* 的全局子目标能强行将评价函数的 Heading 项拉回正确方向避免死锁。动态可视化实时显示机器人、激光雷达扫描线、DWA 预测轨迹簇、全局路径及当前子目标。 MATLAB 完整代码 (Hybrid_AStar_DWA.m)请将以下代码保存为 Hybrid_AStar_DWA.m 并直接运行。%% 改进 A* DWA 混合路径规划算法% 功能全局改进 A* 规划关键点 局部 DWA 动态避障% 特点删除冗余节点、A* 引导 DWA 跳出局部陷阱、动态仿真显示%% 1. 环境与参数设置 (完全统一方便对比)% 地图参数 MAP_SIZE [50, 50]; % 地图大小 [x, y]RESOLUTION 0.5; % 栅格分辨率START_POS [2, 2]; % 起点GOAL_POS [48, 48]; % 终点% 障碍物设置 (U型陷阱 随机障碍) % 格式: [x, y, width, height]obstacles [10, 10, 20, 2; % 底部墙10, 10, 2, 15; % 左墙28, 10, 2, 15; % 右墙 (形成 U 型陷阱开口向上)15, 30, 5, 5; % 中间障碍35, 20, 4, 10; % 右侧障碍5, 40, 10, 2; % 左上障碍40, 5, 5, 5 % 右下障碍];% 机器人参数 ROBOT_RADIUS 0.8; % 机器人半径MAX_VEL 2.0; % 最大线速度 m/sMAX_OMEGA deg2rad(40); % 最大角速度 rad/sACC_VEL 0.5; % 线加速度ACC_OMEGA deg2rad(40); % 角加速度DT 0.1; % 仿真步长PREDICT_TIME 1.5; % DWA 预测时间秒% 算法参数 A_STAR_HEURISTIC ‘Euclidean’; % 启发式函数REMOVE_REDUNDANT true; % 是否开启冗余节点删除DWA_WEIGHTS [1.0, 1.5, 0.8]; % [Heading, Dist, Vel] 权重%% 2. 构建地图fprintf(‘构建地图…n’);[x_grid, y_grid] meshgrid(0:RESOLUTION:MAP_SIZE(1), 0:RESOLUTION:MAP_SIZE(2));map_grid zeros(size(x_grid)); % 0: 空闲, 1: 障碍% 填充障碍物for i 1:size(obstacles, 1)ox obstacles(i, 1); oy obstacles(i, 2);ow obstacles(i, 3); oh obstacles(i, 4);% 找到对应网格索引idx_x (x_grid ox x_grid oy y_grid 2path simplify_path(raw_path, map_grid, res);elsepath raw_path;endend% 辅助简单的 A* 实现用于回溯function [path, success] simple_astar_impl(map_grid, start_node, goal_node)[rows, cols] size(map_grid);came_from containers.Map(‘KeyType’, ‘char’, ‘ValueType’, ‘any’);cost_so_far containers.Map(‘KeyType’, ‘char’, ‘ValueType’, ‘double’);start_key sprintf(%d,%d, start_node(1), start_node(2)); goal_key sprintf(%d,%d, goal_node(1), goal_node(2)); frontier priority_queue(); frontier.push(start_node, 0); came_from(start_key) []; cost_so_far(start_key) 0; found false; while ~frontier.empty() current frontier.pop(); c_key sprintf(%d,%d, current(1), current(2)); if norm(current - goal_node) best_score best_score score; v_best v; w_best w; traj_best traj; end end end % 如果没有可行轨迹紧急停止 if best_score -inf v_best 0; w_best 0; traj_best [state(1), state(2)]; endendfunction traj simulate_trajectory(state, v, w, dt, time)steps floor(time/dt);traj zeros(steps1, 2);x state(1); y state(2); theta state(3);traj(1, [x, y];for i 1:steps x x v * cos(theta) * dt; y y v * sin(theta) * dt; theta theta w * dt; traj(i1, :) [x, y]; endendfunction score evaluate_trajectory(traj, target, obstacles, state, weights, max_v)% 1. Heading: 轨迹终点朝向目标的程度end_pos traj(end, ;vec_to_target target(1:2) - end_pos;desired_theta atan2(vec_to_target(2), vec_to_target(1));actual_theta atan2(traj(end,2)-traj(end-1,2), traj(end,1)-traj(end-1,1));heading_err abs(wrapToPi(desired_theta - actual_theta));score_heading 1.0 - (heading_err / pi); % 归一化 0-1% 2. Dist: 离障碍物的距离 (越大越好) min_dist inf; for i 1:size(traj, 1) d get_dist_to_obs(traj(i,:), obstacles); if d 1, score_dist 1; end % 3. Vel: 速度越快越好 score_vel traj(end,1) / max_v; % 近似用位移代表速度趋势或者直接用 v % 加权 score weights(1score_heading weights(2)score_dist weights(3)*score_vel;endfunction d get_dist_to_pos(pos, obstacles)min_d inf;for i 1:size(obstacles, 1)ox obstacles(i,1); oy obstacles(i,2);ow obstacles(i,3); oh obstacles(i,4);% 点到矩形最近距离cx max(ox, min(pos(1), oxow));cy max(oy, min(pos(2), oyoh));dist norm([pos(1)-cx, pos(2)-cy]);if dist 1 r1 ccols || gyrowstf true; % 边界外视为障碍return;endtf (map_grid(gy, gx) 1); % 注意矩阵索引是 (row, col) - (y, x)endfunction rad wrapToPi(rad)rad mod(rad pi, 2*pi) - pi;end 代码核心逻辑解析改进 A*删除冗余节点原理传统的 A* 生成的路径是沿着栅格边缘走的锯齿状折线。实现simplify_path 函数。从起点开始尝试直接连线到路径上最远的一个点。使用 isLineOfSight (基于线性插值) 检查连线是否穿过障碍物。如果通畅则中间所有点都是冗余的直接删除将最远点设为新的当前点。效果路径点数大幅减少且更符合机器人直线行驶的习惯减少了不必要的转向。A* 引导 DWA (解决局部陷阱)痛点纯 DWA 在 U 型障碍物前容易陷入局部最优因为直接走向目标会撞墙左右移动距离变远评价函数得分低导致机器人原地徘徊。解决方案A* 生成的全局路径绕过了 U 型陷阱。代码中设置 current_target 为 A* 路径上的下一个关键子目标点而不是最终终点。在 DWA 的评价函数 evaluate_trajectory 中Heading 项计算的是轨迹终点指向子目标的角度。这样即使最终终点在陷阱里子目标点在陷阱外DWA 也会倾向于向子目标移动从而“被拉出”陷阱。动态窗口法 (DWA)采样在 [v_{curr}-Delta v, v_{curr}Delta v] 和 [omega_{curr}-Delta omega, omega_{curr}Delta omega] 范围内采样。推演对每个 (v, omega) 模拟未来 1.5 秒的轨迹。评价Heading: 越朝向子目标分越高。Dist: 离障碍物越远分越高。Vel: 速度越快分越高。执行选择得分最高的 (v, omega) 控制机器人运动一步。动态可视化青色射线模拟激光雷达扫描直观展示感知范围。灰色虚线当前时刻 DWA 选中的最佳预测轨迹。品红色圆圈当前的 A* 子目标点随着机器人前进会自动切换到下一个点。白色背景 自由空间黑色矩形/L形 障碍物紫色方块 起点或终点红色小圆点 当前位置或目标点蓝色曲线 规划出的平滑避障路径这通常是A、RRT、DWA、人工势场、或混合算法* 在二维栅格/连续空间中生成的轨迹。包括构建与截图相似的 2D 地图含 L 形和矩形障碍使用改进 A B-Spline 平滑* 或 RRT* 生成平滑路径绘制起点、终点、障碍物、平滑路径风格完全匹配截图白底黑障、蓝线紫块红点 完整可运行 MATLAB 代码 (smooth_path_planning_2d.m)%% 1. 地图定义匹配截图布局MAP_SIZE [100, 100]; % 地图尺寸START_POS [5, 5]; % 起点左下角附近GOAL_POS [95, 95]; % 终点右上角% 定义障碍物格式: [x, y, width, height]obstacles [20, 80, 20, 8; % 上方横条40, 40, 15, 15; % 中间方块70, 60, 8, 8; % 右侧小方块15, 10, 25, 10; % 下方L形底部15, 10, 10, 25; % 下方L形竖部70, 10, 15, 10; % 右下L形底部70, 10, 10, 15; % 右下L形竖部];%% 2. 创建占用网格地图fprintf(‘构建 2D 地图…n’);resolution 1.0;[x_grid, y_grid] meshgrid(0:resolution:MAP_SIZE(1), 0:resolution:MAP_SIZE(2));map_grid zeros(size(x_grid)); % 0: 空闲, 1: 障碍% 填充障碍物for i 1:size(obstacles, 1)ox obstacles(i, 1); oy obstacles(i, 2);ow obstacles(i, 3); oh obstacles(i, 4);idx_x (x_grid ox x_grid oy y_grid 1 r1 ccolsneighbors(end1, [r, c];endendendfunction grid_pos pos2grid(pos, res)grid_pos round(pos / res) 1;endfunction pos grid2pos(grid_pos, res)pos (grid_pos - 1) * res;endfunction pos_batch grid2pos_batch(grid_batch, res)pos_batch (grid_batch - 1) * res;end️ 输出效果说明运行后将生成如下图形白色背景干净无网格线黑色障碍物精确还原截图中的矩形和 L 形蓝色平滑曲线从左下角绕开所有障碍到达右上角紫色方块起点位置红色圆点终点/目标位置无坐标轴视觉更贴近工程图纸或论文插图 自定义建议想调整什么 修改哪里改变障碍物形状/位置 修改 obstacles 数组更换起点/终点 修改 START_POS, GOAL_POS使用 RRT* 替代 A* 替换 astar_2d 为 rrt_star_2d 函数可提供添加动画演示 在循环中逐步绘制路径点导出高清图 添加 print(‘-dpng’, ‘-r300’, ‘path_plan.png’) 一键打包复制全部代码 → 粘贴到 MATLAB 编辑器保存为 smooth_path_planning_2d.m点击运行 → 自动弹出图形窗口可用鼠标缩放、平移查看细节 改进的A星＋DWA混合matlab路径规划算法。购买再赠送一份DWA算法！环境和参数设置完全一样，方便对比算法的优劣！路径规划代码。
改进的A星DWA混合matlab路径规划算法。环境和参数设置完全一样方便对比算法的优劣路径规划代码。改进的A星算法。A星算法采用了删除冗余节点方法。移动路径采用DWA算法来规划。A星算法可以提供子目标点给人工势场算法来引导DWA算法远离陷阱区域。显示方式为动态显示方便观察避障过程。 算法核心亮点改进 A(全局规划)传统 A* 生成的路径包含大量冗余折点。改进策略采用视线检测 (Line-of-Sight) 算法删除中间冗余节点只保留关键转折点作为子目标点。DWA (局部规划)在速度空间 (v, omega) 采样模拟未来轨迹。评价函数Heading (朝向目标) Dist (避障) Vel (速度)。混合机制 (A引导 DWA)A* 不直接控制机器人而是提供动态子目标点 (Sub-goal)。当 DWA 陷入局部最优如 U 型陷阱时A* 的全局子目标能强行将评价函数的 Heading 项拉回正确方向避免死锁。动态可视化实时显示机器人、激光雷达扫描线、DWA 预测轨迹簇、全局路径及当前子目标。 MATLAB 完整代码 (Hybrid_AStar_DWA.m)请将以下代码保存为 Hybrid_AStar_DWA.m 并直接运行。%% 改进 A* DWA 混合路径规划算法% 功能全局改进 A* 规划关键点 局部 DWA 动态避障% 特点删除冗余节点、A* 引导 DWA 跳出局部陷阱、动态仿真显示%% 1. 环境与参数设置 (完全统一方便对比)% 地图参数 MAP_SIZE [50, 50]; % 地图大小 [x, y]RESOLUTION 0.5; % 栅格分辨率START_POS [2, 2]; % 起点GOAL_POS [48, 48]; % 终点% 障碍物设置 (U型陷阱 随机障碍) % 格式: [x, y, width, height]obstacles [10, 10, 20, 2; % 底部墙10, 10, 2, 15; % 左墙28, 10, 2, 15; % 右墙 (形成 U 型陷阱开口向上)15, 30, 5, 5; % 中间障碍35, 20, 4, 10; % 右侧障碍5, 40, 10, 2; % 左上障碍40, 5, 5, 5 % 右下障碍];% 机器人参数 ROBOT_RADIUS 0.8; % 机器人半径MAX_VEL 2.0; % 最大线速度 m/sMAX_OMEGA deg2rad(40); % 最大角速度 rad/sACC_VEL 0.5; % 线加速度ACC_OMEGA deg2rad(40); % 角加速度DT 0.1; % 仿真步长PREDICT_TIME 1.5; % DWA 预测时间秒% 算法参数 A_STAR_HEURISTIC ‘Euclidean’; % 启发式函数REMOVE_REDUNDANT true; % 是否开启冗余节点删除DWA_WEIGHTS [1.0, 1.5, 0.8]; % [Heading, Dist, Vel] 权重%% 2. 构建地图fprintf(‘构建地图…n’);[x_grid, y_grid] meshgrid(0:RESOLUTION:MAP_SIZE(1), 0:RESOLUTION:MAP_SIZE(2));map_grid zeros(size(x_grid)); % 0: 空闲, 1: 障碍% 填充障碍物for i 1:size(obstacles, 1)ox obstacles(i, 1); oy obstacles(i, 2);ow obstacles(i, 3); oh obstacles(i, 4);% 找到对应网格索引idx_x (x_grid ox x_grid oy y_grid 2path simplify_path(raw_path, map_grid, res);elsepath raw_path;endend% 辅助简单的 A* 实现用于回溯function [path, success] simple_astar_impl(map_grid, start_node, goal_node)[rows, cols] size(map_grid);came_from containers.Map(‘KeyType’, ‘char’, ‘ValueType’, ‘any’);cost_so_far containers.Map(‘KeyType’, ‘char’, ‘ValueType’, ‘double’);start_key sprintf(%d,%d, start_node(1), start_node(2)); goal_key sprintf(%d,%d, goal_node(1), goal_node(2)); frontier priority_queue(); frontier.push(start_node, 0); came_from(start_key) []; cost_so_far(start_key) 0; found false; while ~frontier.empty() current frontier.pop(); c_key sprintf(%d,%d, current(1), current(2)); if norm(current - goal_node) best_score best_score score; v_best v; w_best w; traj_best traj; end end end % 如果没有可行轨迹紧急停止 if best_score -inf v_best 0; w_best 0; traj_best [state(1), state(2)]; endendfunction traj simulate_trajectory(state, v, w, dt, time)steps floor(time/dt);traj zeros(steps1, 2);x state(1); y state(2); theta state(3);traj(1, [x, y];for i 1:steps x x v * cos(theta) * dt; y y v * sin(theta) * dt; theta theta w * dt; traj(i1, :) [x, y]; endendfunction score evaluate_trajectory(traj, target, obstacles, state, weights, max_v)% 1. Heading: 轨迹终点朝向目标的程度end_pos traj(end, ;vec_to_target target(1:2) - end_pos;desired_theta atan2(vec_to_target(2), vec_to_target(1));actual_theta atan2(traj(end,2)-traj(end-1,2), traj(end,1)-traj(end-1,1));heading_err abs(wrapToPi(desired_theta - actual_theta));score_heading 1.0 - (heading_err / pi); % 归一化 0-1% 2. Dist: 离障碍物的距离 (越大越好) min_dist inf; for i 1:size(traj, 1) d get_dist_to_obs(traj(i,:), obstacles); if d 1, score_dist 1; end % 3. Vel: 速度越快越好 score_vel traj(end,1) / max_v; % 近似用位移代表速度趋势或者直接用 v % 加权 score weights(1score_heading weights(2)score_dist weights(3)*score_vel;endfunction d get_dist_to_pos(pos, obstacles)min_d inf;for i 1:size(obstacles, 1)ox obstacles(i,1); oy obstacles(i,2);ow obstacles(i,3); oh obstacles(i,4);% 点到矩形最近距离cx max(ox, min(pos(1), oxow));cy max(oy, min(pos(2), oyoh));dist norm([pos(1)-cx, pos(2)-cy]);if dist 1 r1 ccols || gyrowstf true; % 边界外视为障碍return;endtf (map_grid(gy, gx) 1); % 注意矩阵索引是 (row, col) - (y, x)endfunction rad wrapToPi(rad)rad mod(rad pi, 2*pi) - pi;end 代码核心逻辑解析改进 A*删除冗余节点原理传统的 A* 生成的路径是沿着栅格边缘走的锯齿状折线。实现simplify_path 函数。从起点开始尝试直接连线到路径上最远的一个点。使用 isLineOfSight (基于线性插值) 检查连线是否穿过障碍物。如果通畅则中间所有点都是冗余的直接删除将最远点设为新的当前点。效果路径点数大幅减少且更符合机器人直线行驶的习惯减少了不必要的转向。A* 引导 DWA (解决局部陷阱)痛点纯 DWA 在 U 型障碍物前容易陷入局部最优因为直接走向目标会撞墙左右移动距离变远评价函数得分低导致机器人原地徘徊。解决方案A* 生成的全局路径绕过了 U 型陷阱。代码中设置 current_target 为 A* 路径上的下一个关键子目标点而不是最终终点。在 DWA 的评价函数 evaluate_trajectory 中Heading 项计算的是轨迹终点指向子目标的角度。这样即使最终终点在陷阱里子目标点在陷阱外DWA 也会倾向于向子目标移动从而“被拉出”陷阱。动态窗口法 (DWA)采样在 [v_{curr}-Delta v, v_{curr}Delta v] 和 [omega_{curr}-Delta omega, omega_{curr}Delta omega] 范围内采样。推演对每个 (v, omega) 模拟未来 1.5 秒的轨迹。评价Heading: 越朝向子目标分越高。Dist: 离障碍物越远分越高。Vel: 速度越快分越高。执行选择得分最高的 (v, omega) 控制机器人运动一步。动态可视化青色射线模拟激光雷达扫描直观展示感知范围。灰色虚线当前时刻 DWA 选中的最佳预测轨迹。品红色圆圈当前的 A* 子目标点随着机器人前进会自动切换到下一个点。白色背景 自由空间黑色矩形/L形 障碍物紫色方块 起点或终点红色小圆点 当前位置或目标点蓝色曲线 规划出的平滑避障路径这通常是A、RRT、DWA、人工势场、或混合算法* 在二维栅格/连续空间中生成的轨迹。包括构建与截图相似的 2D 地图含 L 形和矩形障碍使用改进 A B-Spline 平滑* 或 RRT* 生成平滑路径绘制起点、终点、障碍物、平滑路径风格完全匹配截图白底黑障、蓝线紫块红点 完整可运行 MATLAB 代码 (smooth_path_planning_2d.m)%% 1. 地图定义匹配截图布局MAP_SIZE [100, 100]; % 地图尺寸START_POS [5, 5]; % 起点左下角附近GOAL_POS [95, 95]; % 终点右上角% 定义障碍物格式: [x, y, width, height]obstacles [20, 80, 20, 8; % 上方横条40, 40, 15, 15; % 中间方块70, 60, 8, 8; % 右侧小方块15, 10, 25, 10; % 下方L形底部15, 10, 10, 25; % 下方L形竖部70, 10, 15, 10; % 右下L形底部70, 10, 10, 15; % 右下L形竖部];%% 2. 创建占用网格地图fprintf(‘构建 2D 地图…n’);resolution 1.0;[x_grid, y_grid] meshgrid(0:resolution:MAP_SIZE(1), 0:resolution:MAP_SIZE(2));map_grid zeros(size(x_grid)); % 0: 空闲, 1: 障碍% 填充障碍物for i 1:size(obstacles, 1)ox obstacles(i, 1); oy obstacles(i, 2);ow obstacles(i, 3); oh obstacles(i, 4);idx_x (x_grid ox x_grid oy y_grid 1 r1 ccolsneighbors(end1, [r, c];endendendfunction grid_pos pos2grid(pos, res)grid_pos round(pos / res) 1;endfunction pos grid2pos(grid_pos, res)pos (grid_pos - 1) * res;endfunction pos_batch grid2pos_batch(grid_batch, res)pos_batch (grid_batch - 1) * res;end️ 输出效果说明运行后将生成如下图形白色背景干净无网格线黑色障碍物精确还原截图中的矩形和 L 形蓝色平滑曲线从左下角绕开所有障碍到达右上角紫色方块起点位置红色圆点终点/目标位置无坐标轴视觉更贴近工程图纸或论文插图 自定义建议想调整什么 修改哪里改变障碍物形状/位置 修改 obstacles 数组更换起点/终点 修改 START_POS, GOAL_POS使用 RRT* 替代 A* 替换 astar_2d 为 rrt_star_2d 函数可提供添加动画演示 在循环中逐步绘制路径点导出高清图 添加 print(‘-dpng’, ‘-r300’, ‘path_plan.png’) 一键打包复制全部代码 → 粘贴到 MATLAB 编辑器保存为 smooth_path_planning_2d.m点击运行 → 自动弹出图形窗口可用鼠标缩放、平移查看细节