MATLAB 2022a实战：手把手教你用A*+DWA算法给机器人做动态路径规划（附源码）

MATLAB 2022a实战：A*与DWA融合算法实现机器人动态路径规划

在机器人导航领域，路径规划一直是核心挑战之一。想象一下，当你需要让机器人在一个充满障碍物的仓库中自主移动时，它既需要知道从A点到B点的全局最优路线，又要在行进过程中灵活避开突然出现的人员或货物。这正是A*算法与动态窗口法(DWA)融合的价值所在——前者提供全局视野，后者处理局部应变。

本文将带你从零开始，在MATLAB 2022a环境中实现这一融合算法。不同于理论讲解，我们会聚焦于实际代码实现和参数调优技巧，确保你能直接复现结果并应用于自己的项目。无论你是 robotics 方向的学生，还是正在开发AGV系统的工程师，这套方法都能为你提供即插即用的解决方案。

1. 环境搭建与基础准备

1.1 MATLAB 2022a配置要点

在开始前，请确保你的MATLAB 2022a已安装以下工具箱：

• Robotics System Toolbox
• Navigation Toolbox (可选，但推荐)

验证安装：

ver('robotics')

如果未安装，可通过以下命令获取：

% 安装Robotics System Toolbox matlab.addons.toolbox.installToolbox('Robotics_System_Toolbox.mltbx')

1.2 创建仿真环境

我们将使用栅格地图表示环境，其中：

• 0表示自由空间
• 1表示障碍物

生成一个20x20的测试地图：

map = zeros(20,20); map(5:15, 5) = 1; % 垂直障碍物 map(10, 5:15) = 1; % 水平障碍物 map(18:20, 15:18) = 1; % 障碍物方块 % 可视化 figure; imagesc(map); colormap([1 1 1; 0 0 0]); % 白-黑对应0-1 axis equal tight

2. A*算法全局路径规划实现

2.1 A*核心代码解析

A*算法的关键在于启发式函数的选择。我们实现一个支持多种启发式的版本：

function path = aStar(map, start, goal, heuristic) % 参数说明: % map: 二维栅格地图 % start: [row,col] 起始点 % goal: [row,col] 目标点 % heuristic: 'euclidean'或'manhattan' [rows, cols] = size(map); closedSet = false(rows, cols); openSet = false(rows, cols); openSet(start(1), start(2)) = true; gScore = inf(rows, cols); gScore(start(1), start(2)) = 0; fScore = inf(rows, cols); fScore(start(1), start(2)) = heuristicCost(start, goal, heuristic); cameFrom = zeros(rows, cols, 2); while any(openSet(:)) [~, idx] = min(fScore(openSet)); [currentRow, currentCol] = ind2sub([rows, cols], idx); if isequal([currentRow, currentCol], goal) path = reconstructPath(cameFrom, goal); return; end openSet(currentRow, currentCol) = false; closedSet(currentRow, currentCol) = true; % 检查8邻域 for i = -1:1 for j = -1:1 if i == 0 && j == 0 continue; end neighbor = [currentRow+i, currentCol+j]; % 边界检查 if neighbor(1) < 1 || neighbor(1) > rows || ... neighbor(2) < 1 || neighbor(2) > cols continue; end % 障碍物检查 if map(neighbor(1), neighbor(2)) == 1 continue; end % 已在closed set中 if closedSet(neighbor(1), neighbor(2)) continue; end % 对角线移动成本为sqrt(2)，其他为1 tentative_gScore = gScore(currentRow, currentCol) + ... (abs(i)+abs(j)==2)*0.4142 + 1; if ~openSet(neighbor(1), neighbor(2)) openSet(neighbor(1), neighbor(2)) = true; elseif tentative_gScore >= gScore(neighbor(1), neighbor(2)) continue; end cameFrom(neighbor(1), neighbor(2), :) = [currentRow, currentCol]; gScore(neighbor(1), neighbor(2)) = tentative_gScore; fScore(neighbor(1), neighbor(2)) = gScore(neighbor(1), neighbor(2)) + ... heuristicCost(neighbor, goal, heuristic); end end end path = []; % 未找到路径 end

2.2 启发式函数实现

function cost = heuristicCost(point, goal, type) switch type case 'euclidean' cost = norm(point - goal); case 'manhattan' cost = sum(abs(point - goal)); otherwise error('未知启发式类型'); end end

2.3 路径回溯函数

function path = reconstructPath(cameFrom, current) path = current; while any(cameFrom(current(1), current(2), :)) current = squeeze(cameFrom(current(1), current(2), :))'; path = [current; path]; end end

3. DWA局部避障实现

3.1 机器人运动模型

定义差分驱动机器人模型：

classdef DiffDriveRobot < handle properties pose = [0; 0; 0]; % x, y, theta radius = 0.5; % 碰撞检测半径 max_lin_vel = 1.0; % m/s max_ang_vel = pi/2; % rad/s acc_lin = 0.2; % 线加速度限制 acc_ang = pi/4; % 角加速度限制 end methods function move(obj, v, w, dt) % 更新位姿 obj.pose(3) = obj.pose(3) + w*dt; obj.pose(1) = obj.pose(1) + v*cos(obj.pose(3))*dt; obj.pose(2) = obj.pose(2) + v*sin(obj.pose(3))*dt; end end end

3.2 DWA核心算法

function [best_v, best_w] = dynamicWindowApproach(robot, goal, obstacles, dt) % 参数: % robot: 机器人对象 % goal: 目标点[x,y] % obstacles: Nx2矩阵，障碍物位置 % dt: 时间步长 % 动态窗口计算 v_min = max(0, robot.vel(1) - robot.acc_lin*dt); v_max = min(robot.max_lin_vel, robot.vel(1) + robot.acc_lin*dt); w_min = max(-robot.max_ang_vel, robot.vel(2) - robot.acc_ang*dt); w_max = min(robot.max_ang_vel, robot.vel(2) + robot.acc_ang*dt); % 采样空间 v_samples = linspace(v_min, v_max, 15); w_samples = linspace(w_min, w_max, 15); best_score = -inf; best_v = 0; best_w = 0; for v = v_samples for w = w_samples % 模拟轨迹 [traj, collision] = simulateTrajectory(robot, v, w, dt, obstacles); if collision continue; end % 计算得分 heading_score = 1.0 * headingCost(traj, goal); dist_score = 0.5 * clearanceCost(traj, obstacles); vel_score = 0.2 * v; total_score = heading_score + dist_score + vel_score; if total_score > best_score best_score = total_score; best_v = v; best_w = w; end end end end

3.3 轨迹模拟与碰撞检测

function [traj, collision] = simulateTrajectory(robot, v, w, dt, obstacles) traj = []; temp_robot = copy(robot); collision = false; for t = 0:dt:3 % 预测3秒 temp_robot.move(v, w, dt); traj = [traj; temp_robot.pose']; % 碰撞检测 for i = 1:size(obstacles,1) if norm(temp_robot.pose(1:2) - obstacles(i,:)) < robot.radius collision = true; return; end end end end

4. 算法融合与实战调优

4.1 全局与局部规划协同

实现融合控制器：

function hybridPlanner(map, start, goal, obstacles) % 全局规划 global_path = aStar(map, start, goal, 'euclidean'); % 初始化机器人 robot = DiffDriveRobot(); robot.pose = [start(2); start(1); 0]; % 转换为x,y,theta % 可视化 figure; hold on; imagesc(map); colormap([1 1 1; 0 0 0]); plot(global_path(:,2), global_path(:,1), 'b-', 'LineWidth', 2); plot(goal(2), goal(1), 'go', 'MarkerSize', 10, 'LineWidth', 2); % 主循环 while norm(robot.pose(1:2) - [goal(2); goal(1)]) > 0.5 % 获取局部目标点(全局路径上距离机器人1m的点) [local_goal, ~] = getLocalGoal(robot, global_path); % DWA规划 [v, w] = dynamicWindowApproach(robot, local_goal, obstacles, 0.1); % 执行移动 robot.move(v, w, 0.1); % 可视化 plot(robot.pose(1), robot.pose(2), 'ro'); drawnow; % 检查是否需要重新规划全局路径 if checkCollision(robot, map) disp('检测到新障碍物，重新规划全局路径...'); global_path = aStar(map, round([robot.pose(2), robot.pose(1)]), goal, 'euclidean'); plot(global_path(:,2), global_path(:,1), 'b-', 'LineWidth', 2); end end disp('到达目标!'); end

4.2 关键参数调优指南

DWA性能主要取决于三个权重参数：

典型调优过程：

1. 首先设置α=1.5, β=0.5, γ=0.1
2. 观察机器人在简单环境中的表现
3. 如果机器人经常太靠近障碍物，增加β
4. 如果机器人朝向目标不够积极，增加α
5. 如果移动速度太慢，适当增加γ

4.3 常见问题排查

问题1：机器人在障碍物前振荡

• 可能原因：β值过大导致对障碍物过度反应
• 解决方案：降低β值或增加机器人半径的缓冲值

问题2：机器人无法到达精确目标位置

• 可能原因：终止条件过于严格
• 修改代码：

% 原终止条件 while norm(robot.pose(1:2) - goal) > 0.5 % 可改为两阶段条件 if norm(robot.pose(1:2) - goal) < 1.0 v = min(v, 0.2); % 接近时减速 end

问题3：全局路径频繁重新规划

• 可能原因：碰撞检测过于敏感
• 优化方法：

function collision = checkCollision(robot, map) % 获取机器人所在栅格 x = round(robot.pose(1)); y = round(robot.pose(2)); % 检查3x3邻域 for i = -1:1 for j = -1:1 if y+i > 0 && y+i <= size(map,1) && ... x+j > 0 && x+j <= size(map,2) if map(y+i, x+j) == 1 collision = true; return; end end end end collision = false; end

5. 高级技巧与性能优化

5.1 多分辨率路径规划

对于大型地图，可以采用分层规划策略：

1. 首先在低分辨率地图上进行全局规划
2. 然后在局部高分辨率地图上细化路径

实现代码片段：

% 创建低分辨率地图 low_res_map = imresize(map, 0.2, 'nearest'); % 在低分辨率下规划 low_res_path = aStar(low_res_map, round(start/5), round(goal/5), 'euclidean'); % 转换回高分辨率坐标 high_res_path = low_res_path * 5; % 在高分辨率下进行局部优化 optimized_path = localPathOptimization(high_res_path, map);

5.2 实时动态障碍物处理

扩展DWA以处理移动障碍物：

function [v, w] = dynamicWindowApproachWithMovingObstacles(robot, goal, obstacles, obstacle_velocities, dt) % ...原有代码... for v = v_samples for w = w_samples % 模拟轨迹时考虑障碍物运动 [traj, collision] = simulateTrajectoryWithMovingObstacles(... robot, v, w, dt, obstacles, obstacle_velocities); % ...其余逻辑不变... end end end function [traj, collision] = simulateTrajectoryWithMovingObstacles(robot, v, w, dt, obstacles, obs_vel) traj = []; temp_robot = copy(robot); collision = false; for t = 0:dt:3 % 移动机器人 temp_robot.move(v, w, dt); traj = [traj; temp_robot.pose']; % 移动障碍物并检测碰撞 moved_obs = obstacles + t * obs_vel; for i = 1:size(moved_obs,1) if norm(temp_robot.pose(1:2) - moved_obs(i,:)) < robot.radius collision = true; return; end end end end

5.3 可视化增强技巧

添加更丰富的可视化元素帮助调试：

function visualizeNavigation(robot, global_path, local_path, obstacles) clf; hold on; % 绘制地图 imagesc(map); colormap([1 1 1; 0 0 0]); % 全局路径 plot(global_path(:,2), global_path(:,1), 'b-', 'LineWidth', 1.5); % 局部路径 if ~isempty(local_path) plot(local_path(:,1), local_path(:,2), 'g--', 'LineWidth', 1); end % 障碍物 plot(obstacles(:,1), obstacles(:,2), 'ks', 'MarkerSize', 8); % 机器人 drawRobot(robot); % 动态窗口 if ~isempty(robot.dynamic_window) rectangle('Position', [robot.dynamic_window(1), robot.dynamic_window(3), ... robot.dynamic_window(2)-robot.dynamic_window(1), ... robot.dynamic_window(4)-robot.dynamic_window(3)], ... 'EdgeColor', 'm', 'LineStyle', '--'); end axis equal tight title(sprintf('机器人位置: [%.2f, %.2f], 朝向: %.1f°', ... robot.pose(1), robot.pose(2), rad2deg(robot.pose(3)))); drawnow; end