简介基于Python实现的轮式机器人路径规划工程融合A星全局路径规划与DWA动态窗口法适用于机器人导航、自动驾驶等教学科研与个人学习场景。压缩包共50个文件以13个Python脚本为核心分别实现A星、DWA与主程序流程同时包含rviz可视化、launch启动、yaml参数及xacro模型等ROS配置便于在Gazebo仿真环境中直接运行调试整包体积仅148KB轻量且结构清晰。代码采用模块化设计主程序负责两点间路径规划DWA模块实现动态避障支持鼠标左键设置起始点、右键设置终点、中键放置障碍物空格键启动规划交互直观。附带使用说明文档方便快速上手。目前已有280人学习下载源码均经本地编译验证评审分95分以上并由助教审定难度适中适合需要学习路径规划算法结合应用的开发者直接运行或二次扩展。1. 全局规划加局部避障Astar与DWA这套组合为什么是轮式机器人的标配在轮式机器人路径规划的课程设计和毕设选题里最尴尬的现状往往是Astar算法全局规划做了一堆却在小车起步后面对动态障碍物无能为力DWA算法能避开局部障碍但缺了Astar给的全局路径就变成无头苍蝇绕了半天甚至回不到目标点。这个资源把两套算法按工程惯例串成完整链路Astar在栅格地图上先算出全局路径DWA动态窗口负责跟随时绕开突发障碍并且用鼠标交互和matplotlib可视化把整条链路直观呈现出来另外还带一组Gazebo仿真工程可以转成ROS环境下的AGV演示。对准备答辩、做课程设计的本科生来说这类源码最怕拿到手跑不起来对从裸算法想过渡到完整导航栈的入门者来说它又是一个二维验证环境你可以先调参再迁移。两拨人都能用。2. 源码结构拆解四个Python文件的分工与Astar、DWA调用链路2.1 四个Python文件到底谁依赖谁这个压缩包里核心代码一共四个Python文件main.py是程序入口AStarPlanner.py实现Astar全局路径搜索Vplanner.py实现DWA动态窗口算法本体dwa.py则是在Vplanner之上做的一层封装把DWA的核心方法接到main.py主流程里。很多人第一次看会误以为dwa.py和Vplanner.py是两个平行的算法实现其实不是。Vplanner.py里保存的是DWA核心数据结构动态窗口计算、轨迹外推、评价函数全在里头dwa.py更像个门面负责初始化DWA对象并对外暴露一个计算下一时刻速度指令的接口。main.py只管交互和调度具体算法逻辑都在另外三个文件里。我建议的阅读顺序是AStarPlanner.py → Vplanner.py → dwa.py → main.py先理解单层算法再看耦合。别在没读Vplanner.py之前直接啃main.py否则里面那个DWA对象像黑匣子你只知道调用它返回一个速度却说不清这个速度是怎么从几千条候选轨迹里挑出来的。2.2 AStarPlanner.pyAstar寻路核心的实现骨架这个类干的事情很纯粹把地图抽象成栅格把起点、终点、障碍物映射成网格坐标用启发式搜索从起点向外扩展直到命中终点。下面这段是Astar核心逻辑的重写关键结构跟这个资源里的实现一致import heapq import math class AStarPlanner: def __init__(self, width, height, resolution1.0): self.width width self.height height self.resolution resolution self.obstacles set() # 保存障碍物栅格坐标 def plan(self, start, goal): start (int(start[0]), int(start[1])) goal (int(goal[0]), int(goal[1])) if start goal: return [start] open_list [] # 优先队列按f值从小到大弹出 heapq.heappush(open_list, (0.0, start)) came_from {} # 记录父节点回溯路径用 g_score {start: 0.0} f_score {start: self._heuristic(start, goal)} while open_list: _, current heapq.heappop(open_list) if current goal: return self._reconstruct_path(came_from, current) for neighbor in self._neighbors(current): # 四邻域搜索每走一格g代价为1 tentative_g g_score[current] 1.0 if neighbor not in g_score or tentative_g g_score[neighbor]: came_from[neighbor] current g_score[neighbor] tentative_g f_score[neighbor] tentative_g self._heuristic(neighbor, goal) heapq.heappush(open_list, (f_score[neighbor], neighbor)) return [] # open list耗尽没找到返回空路径 def _neighbors(self, node): x, y node candidates [(x 1, y), (x - 1, y), (x, y 1), (x, y - 1)] result [] for nx, ny in candidates: if 0 nx self.width and 0 ny self.height: if (nx, ny) not in self.obstacles: result.append((nx, ny)) return result def _heuristic(self, a, b): # 欧氏距离作为启发函数保证可采纳性 return math.hypot(a[0] - b[0], a[1] - b[1]) def _reconstruct_path(self, came_from, current): path [current] while current in came_from: current came_from[current] path.append(current) path.reverse() return path几个细节值得说明。启发式用了欧氏距离这是轮式机器人栅格导航里最常见的选型因为四邻域移动下欧氏距离不会高估代价Astar一定能搜出最优路径。代价方面相邻栅格移动固定为1g值体现的是栅格步数而不是物理距离这一步逻辑简单后面DWA接上来时Astar用的分辨率能不能对上就决定了整体效果。实际使用这个类非常直接创建对象后反复调plan障碍物集合变了直接重跑不需要重建对象。鼠标每点一次障碍物都触发一次plan也没问题搜索只在空格键按下时执行点击障碍物本身不触发计算这也是交互不卡顿的原因。2.3 Vplanner.py动态窗口采样、轨迹外推和评价计分DWA的立身之本在于它不在路径空间里搜索而在速度空间里采样。机器人当前速度和角速度受电机加速度上限约束下一时刻能取到的速度被限制在一个动态窗口内超出这个窗口的速度即使在地图上是合理的物理上也执行不了。Vplanner.py里最核心的函数就是干这件事import math class DWA: def __init__(self, config): self.max_vel config[max_vel] # 最大线速度 m/s self.max_omega config[max_omega] # 最大角速度 rad/s self.max_accel config[max_accel] # 最大线加速度 m/s^2 self.max_omega_accel config[max_omega_accel] # 最大角加速度 self.dt config.get(dt, 0.1) # 仿真步长 self.predict_time config.get(predict_time, 3.0) self.heading_w config.get(heading_w, 1.0) self.vel_w config.get(vel_w, 0.8) self.clearance_w config.get(clearance_w, 0.2) def calc_dynamic_window(self, v, w): # 受加速度限制下一时刻的速度有明确上下界 v_max min(self.max_vel, v self.max_accel * self.dt) v_min max(-self.max_vel, v - self.max_accel * self.dt) w_max min(self.max_omega, w self.max_omega_accel * self.dt) w_min max(-self.max_omega, w - self.max_omega_accel * self.dt) return [v_min, v_max, w_min, w_max] def predict_trajectory(self, state, v, w): # 用运动学模型外推一段时间内的位姿序列 traj [] x, y, theta state for _ in range(int(self.predict_time / self.dt)): x v * math.cos(theta) * self.dt y v * math.sin(theta) * self.dt theta w * self.dt traj.append((x, y, theta)) return traj两个参数在这里极为关键。max_accel如果设成0动态窗口秒变固定速度范围DWA退化成随机暴力采样看起来在跑但效果全凭运气。predict_time决定每条候选轨迹外推多长设太大轨迹会穿进远处障碍物clearance项误判设太小看不到前方弯道容易撞墙。差速轮AGV场景我一般取3到5秒再结合机器人制动距离去微调。窗口算出来后在窗口内按速度分辨率和角速度分辨率采样每组速度都外推一条轨迹然后交给评价函数打分。三条评分项缺一不可轨迹末端朝向与目标方向的角度差轨迹平均速度轨迹离最近障碍物的距离。def evaluate(self, traj, target_theta): # 把三项指标归一化到0~1后再加权求和 heading self._calc_heading(traj, target_theta) velocity self._calc_velocity(traj) clearance self._calc_clearance(traj) return (self.heading_w * heading self.vel_w * velocity self.clearance_w * clearance)权重直接决定小车性格。heading_w大小车倾向快速转向目标路径短但容易抖动clearance_w大小车离障碍物远远的路径变绕。资源里默认参数heading_w最大适合开阔地图真遇到狭窄走廊场景必须把clearance_w提上来否则小车贴墙刷过去的画面很难看。2.4 main.py的鼠标交互与空格触发流程main.py把两个算法组装成一个可交互程序。它初始化地图创建AStarPlanner对象和DWA对象然后用matplotlib事件接口监听鼠标和键盘import matplotlib.pyplot as plt def on_click(event): global start, goal, obstacles if event.inaxes is None: return x int(round(event.xdata)) y int(round(event.ydata)) if event.button 1: # 左键设置起点 start (x, y) elif event.button 3: # 右键设置终点 goal (x, y) elif event.button 2: # 中键添加障碍物 obstacles.append((x, y)) redraw() def on_key(event): if event.key : path astar.plan(start, goal) run_dwa_tracking(path) fig.canvas.mpl_connect(button_press_event, on_click) fig.canvas.mpl_connect(key_press_event, on_key) plt.show()事件回调里改的是全局变量真正执行规划在空格键事件里。如果二次开发时把plan调用直接塞进on_click每次点击都触发一次完整搜索界面疯狂闪烁体验极差。现在这个设计把场景构建和执行分离先摆好障碍物再统一规划很符合演示场景的操作直觉。run_dwa_tracking内部是个循环从全局路径中取前瞻节点作为DWA局部目标计算速度和角速度更新位姿直到抵达终点。前瞻节点取太远小车抄近道偏离全局路径取太近每个节点附近不断转向路径抖成波浪线。比较稳的折中方案是取“当前索引加固定步长”的节点步长通常3到5个栅格。3. 把二维路径规划跑起来环境准备、鼠标交互与场景验证3.1 装依赖和确认Python版本这份源码面向Python 3我用3.7以上版本都能跑3.8到3.11都行依赖只有numpy和matplotlib。命令行执行python -m pip install numpy matplotlib cd path/to/project python main.py第一次跑会弹出一个空白栅格图。macOS用户注意matplotlib默认后端是macosx远程ssh看不到窗口得在本地跑。Windows下弹不出窗口多半是IDE内置控制台的问题改成系统Terminal执行就行。这个坑我在Windows上踩过好多次PyCharm自带console跑matplotlib交互窗口经常卡事件循环。还有一种情况机器上同时装了python和python3命令pip装给了python3但python main.py用的却是另一个解释器直接报No module named numpy。遇到这种玄学报错先敲python --version确认路径别急着重装环境。3.2 鼠标三个键加空格键的交互规则交互规则五条鼠标左键设置起点右键设置终点鼠标中键逐个添加障碍物空格键触发规划。起点显示为蓝色圆点终点显示为红色标记障碍物是黑色方块规划出来的全局路径是绿色线条。这个设计没有提供删除障碍物的鼠标操作想重新摆地图只能关掉重启交互上有点遗憾。二次开发想加清除功能可以在on_click里加一个判断比如双击中键把最后一个障碍物弹出去再重绘改动量不大。3.3 一次完整的规划流程演示设地图尺寸13×11期望从左上角走到右下角中间摆一面L形障碍墙。操作顺序如下# 第一步鼠标左键点击左上角区域起点出现蓝色标记 # 第二步鼠标右键点击右下角区域终点出现红色标记 # 第三步鼠标中键点击以下栅格位置逐个添加障碍物 # (4, 4) (5, 4) (6, 4) (7, 4) # (4, 5) (7, 5) # 第四步按下空格键开始路径规划按下空格后界面上先出现Astar算出的绿色全局路径然后小车形状的标记沿着路径前进DWA在遇到障碍物时调整方向绕过。如果小车接近障碍物时速度没有明显下降而是硬生生贴墙刷过去基本能判断是clearance权重太小如果小车频繁原地转向、路径扭曲则是heading权重压过了velocity项这两个现象是调参时最先要看的。3.4 地图尺寸与障碍物预置方式地图尺寸在main.py开头一般用MAP_WIDTH和MAP_HEIGHT常量定义。这个参数的影响比想象中大Astar的open list规模跟地图面积近似成正比从10×10放大到50×50之后鼠标点击添加障碍物也变难点不准。如果只是验证算法效果建议把网格数控制在40×40以内保证交互精度。障碍物不一定要靠鼠标一个个点main.py里直接预置坐标列表同样是合法输入这个方式适合自动化测试和场景复现obstacles [(2, 3), (2, 4), (2, 5), (7, 8), (7, 9)] astar AStarPlanner(20, 20) astar.obstacles set(obstacles) path astar.plan((0, 0), (19, 19))这段代码我经常拿来压测不同地图尺寸下的规划耗时。把地图放大用time.time()包住plan调用前后时间差就能大致摸清Astar在大地图上的性能边界。测试下来相同障碍物密度下30×30地图基本秒出80×80开始有明显停顿。3.5 规划结果合法性验证跑完一次规划别急着看路径漂不漂亮先验证路径有没有穿过障碍物。写个几行的校验逻辑def check_path_valid(path, obstacles): for node in path: if node in obstacles: return False, node return True, None这个函数在调参阶段非常实用。Astar本身保证最优但如果鼠标点击产生的坐标跟内部栅格索引有off-by-one误差路径可能画在障碍物格子上把校验结果打印出来能第一时间发现是坐标换算问题还是膨胀参数问题不用肉眼盯着屏幕数格子。4. 从Python二维模拟到Gazebo仿真course_agv包的编译与启动4.1 course_agv_* 目录分别对应哪些ROS包压缩包里除了四个Python文件还有一组看起来像ROS工程的文件gazebo目录、course_agv_control、course_agv_nav、course_agv_description、course_agv_gazebo外加顶层CMakeLists.txt。按ROS功能包的命名习惯course_agv_description放URDF和xacro模型文件描述AGV底盘、轮子和传感器course_agv_gazebo放Gazebo仿真world和模型插入配置course_agv_control是底盘运动控制插件或控制器course_agv_nav大概率是导航配置和launch文件。把这几个包放进同一个ROS工作空间编译能在Gazebo里搭出一辆完整的差速AGV。能看出这份资源的完整度——不是只在二维图上跑算法还配套了一整套可仿真的AGV模型。从这里也能推导出二维Python版里的Astar和DWA算法可以直接复用Gazebo工程只是把底层的位姿更新替换成物理引擎。4.2 编译和启动流程要用这些ROS包得先把工程放进catkin工作空间编译常见做法cd ~/catkin_ws/src cp -r /path/to/course_agv_source ./ cd ~/catkin_ws catkin_make source devel/setup.bash roslaunch course_agv_gazebo course_agv_world.launch注意这里launch文件名是工程惯例写法实际操作以压缩包里README.md记录的文件名为准。编译报错先查Python环境Ubuntu 20.04加ROS Noetic要求Python 3.8如果手工把系统默认Python切换到3.10很多ROS包直接编译失败这类问题比代码本身的bug难排查得多。启动Gazebo之前还要确认环境变量已经source过不然roslaunch会报“RLException: xxx.launch is neither a launch file nor a package”这种误导性错误实际只是环境没加载。4.3 Python二维模拟与Gazebo仿真的核心差异二维Python版和Gazebo仿真最大的差距在数据链路上。二维版里坐标更新直接用运动学公式递推位置精确无噪声DWA算出来的速度跟实际位移完全对得上。Gazebo里位置由物理引擎计算里程计数据由插件发布链路长了许多。第一个坎是坐标系变换。AGV的base_link和odom之间TF不广播控制节点读到错误位置DWA会朝错误目标规划小车乱跑找不到原因。第二个差异在传感器二维版没有真实传感器小车对障碍物的感知靠地图静态坐标Gazebo里如果激光雷达话题/scan没有正常发布DWA的clearance项算出来的就是没有任何意义的数值。启动后第一件事先看话题rostopic list rostopic echo /odom -n 1 rostopic echo /scan -n 1这是排查一切Gazebo导航问题的起点三个话题有数据流动控制链路才可能正常。物理引擎还带来几个二维模拟体会不到的差异点。轮胎摩擦会造成轻微打滑DWA输出速度和现实速度差几个百分点位置反馈逐渐漂移。小车质量过大时电机最大力矩限制会让实际加速度达不到max_accel设定值动态窗口里的理论速度区间不可达表现在仿真里就是小车响应变慢、轨迹预测跟真实运动偏离越来越大。传感器采样频率如果只有10HzDWA控制循环却设成50Hz障碍物信息更新跟不上控制频率很容易撞上刚进入安全距离的物体。应对办法是把DWA控制周期降到跟传感器对齐或者对障碍物距离做一帧缓存外推。4.4 两种运行方式的选型建议二维Python版调试成本极低改一个参数重启一秒适合前期验证算法权重Gazebo仿真每次启动加载模型和world半分钟起步不适合在里头反复试参数。我的习惯是先在二维版把三个权重系数调到效果满意记录下来再去Gazebo核对物理表现通常只需要改速度限制和加速度限制两个参数就能对齐。对比项二维Python版Gazebo仿真位置更新运动学公式递推无噪声物理引擎计算有打滑和漂移障碍物感知静态坐标理想感知激光雷达话题受采样率限制调试周期秒级重启环境加载慢迭代成本高坐标系无base_link、odom、map三者变换适用阶段算法验证和调参集成测试和演示5. 避坑与常见问题运行、规划、避障三个环节的翻车记录5.1 现象python main.py一运行就报No module named numpy原因新环境没装依赖或者python和python3命令指向不同解释器。解决python -m pip install numpy matplotlib装完仍然报错先敲python看解释器路径再用pip list确认numpy装到哪个环境里了。有些机器上python指向系统自带旧版本python3指向自装新版装到python3里的包python调不出来。敲python3 main.py基本能解决。5.2 现象Astar规划路径贴着障碍物边缘甚至直接穿模原因栅格地图没做障碍物膨胀。路径计算只要求格子不被Occupancy标记但真实机器人有半径贴着障碍物走必然刮蹭。解决构建障碍物集合时做膨胀处理把障碍物周围一圈全标记为不可通行def inflate_obstacles(obstacles, radius, width, height): inflated set() for ox, oy in obstacles: for dx in range(-radius, radius 1): for dy in range(-radius, radius 1): nx, ny ox dx, oy dy if 0 nx width and 0 ny height: inflated.add((nx, ny)) return inflated半径一般取机器人半径除以栅格分辨率再向上取整。代价是地图拥挤时可能搜不出路径这时需要降分辨率或缩小机器人半径参数。这个坑在二维Python版里不明显因为matplotlib画线本身有宽度路径看着好像没碰上障碍物实际上换到Gazebo一仿真就露馅。5.3 现象DWA在路径后半段原地打转原因heading权重太大或前瞻点离得太近。DWA靠近目标时目标点就在当前位姿旁边heading项持续要求转向转向产生位移位移又改变偏角形成震荡。解决把heading_w从1.0降到0.5以下前瞻节点改成路径上距离当前位置大于0.5m的第一个节点并且加抵达判断距离小于阈值时直接切换停止控制。这个问题的根因在于Astar全局路径节点间距不一致靠近目标时节点加密DWA的局部目标从“路径上某个较远点”变成了“终点本身”评价函数里heading权重瞬间失控。调参时如果发现后半段开始画圈先看前瞻节点是不是被设置成了path[-1]。5.4 现象matplotlib窗口上点击起点终点都没反应原因事件绑定没成功或者回调函数里event.xdata是None。某些macOS后端不走默认交互某些IDE的交互后端不支持鼠标事件。解决在程序开头强制指定后端import matplotlib matplotlib.use(TkAgg)还不行就在on_click回调里print(event.button, event.xdata)如果字段全是None说明事件压根没进回调基本可以确定是后端问题。之前帮人排查过他用的Linux服务器上没装图形库matplotlib用的Agg后端根本不支持交互换成TkAgg并且装上python3-tk包就正常了。5.5 现象Gazebo启动后小车纹丝不动原因cmd_vel话题没有数据流或者控制插件没加载成功。解决先订阅话题看有没有数据再用rostopic pub手动发布一个速度测试rostopic pub -r 10 /cmd_vel geometry_msgs/Twist \ {linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {z: 0.2}}小车动了说明控制链路正常问题在导航节点还是不动就检查URDF里的diff_drive插件配置joint名字必须跟URDF定义一致差一个下划线插件都会静默失败这是Gazebo仿真里最玄学的坑之一gazebo plugin namediff_drive filenamelibgazebo_ros_diff_drive.so leftJointleft_wheel_joint/leftJoint rightJointright_wheel_joint/rightJoint commandTopiccmd_vel/commandTopic odometryTopicodom/odometryTopic /plugin /gazebo6. 进阶玩法参数外置、左右轮速度换算与真机迁移6.1 把DWA权重抽成参数文件源码里权重直接写在类的构造函数参数里跑一组实验改一次代码非常低效。我拿到手第一件事就是把参数外置成json文件运行时加载做对比实验只改文件不动代码{ max_vel: 1.0, max_omega: 1.2, max_accel: 0.5, max_omega_accel: 0.8, heading_w: 1.0, vel_w: 0.8, clearance_w: 0.2, predict_time: 3.0 }main.py里初始化DWA时改成import json with open(dwa_config.json) as f: config json.load(f) dwa DWA(config)换成配置化之后做调参实验效率高得不是一点半点。我一般会建一个results目录每组实验存一份json和对应的运行截图复盘时直接对比参数组合和路径形态不用靠记忆猜上次跑的哪组参数。6.2 从二维模拟到真实差速底盘的改动三步没有真车之前可以先把DWA输出换算成左右轮速度这个公式在差速轮AGV上通用left_wheel_vel (v - omega * wheel_base / 2.0) / wheel_radius right_wheel_vel (v omega * wheel_base / 2.0) / wheel_radius迁移到真实底盘时做三件事第一把二维版的位姿更新替换成从里程计读取位置第二把DWA输出的线速度和角速度按上面公式转成左右轮转速下发到电机驱动第三加一层急停保护激光测距低于安全阈值时直接把速度清零。我在实验室做这个迁移时原来二维版跑得很顺的权重在真车上几乎直接可用唯一的问题出在底盘电机驱动板的串口通信周期和DWA控制周期不一致反馈抖动严重把两者统一成50ms才稳定下来。从那以后我每次调路径规划参数都强制走一遍“二维验证、半实物仿真、真机复核”的流程每一步的参数记录都留档不然真机出了状况根本无法回溯。希望帮到你。本文还有配套的精品资源点击获取
简介基于Python实现的轮式机器人路径规划工程融合A星全局路径规划与DWA动态窗口法适用于机器人导航、自动驾驶等教学科研与个人学习场景。压缩包共50个文件以13个Python脚本为核心分别实现A星、DWA与主程序流程同时包含rviz可视化、launch启动、yaml参数及xacro模型等ROS配置便于在Gazebo仿真环境中直接运行调试整包体积仅148KB轻量且结构清晰。代码采用模块化设计主程序负责两点间路径规划DWA模块实现动态避障支持鼠标左键设置起始点、右键设置终点、中键放置障碍物空格键启动规划交互直观。附带使用说明文档方便快速上手。目前已有280人学习下载源码均经本地编译验证评审分95分以上并由助教审定难度适中适合需要学习路径规划算法结合应用的开发者直接运行或二次扩展。1. 全局规划加局部避障Astar与DWA这套组合为什么是轮式机器人的标配在轮式机器人路径规划的课程设计和毕设选题里最尴尬的现状往往是Astar算法全局规划做了一堆却在小车起步后面对动态障碍物无能为力DWA算法能避开局部障碍但缺了Astar给的全局路径就变成无头苍蝇绕了半天甚至回不到目标点。这个资源把两套算法按工程惯例串成完整链路Astar在栅格地图上先算出全局路径DWA动态窗口负责跟随时绕开突发障碍并且用鼠标交互和matplotlib可视化把整条链路直观呈现出来另外还带一组Gazebo仿真工程可以转成ROS环境下的AGV演示。对准备答辩、做课程设计的本科生来说这类源码最怕拿到手跑不起来对从裸算法想过渡到完整导航栈的入门者来说它又是一个二维验证环境你可以先调参再迁移。两拨人都能用。2. 源码结构拆解四个Python文件的分工与Astar、DWA调用链路2.1 四个Python文件到底谁依赖谁这个压缩包里核心代码一共四个Python文件main.py是程序入口AStarPlanner.py实现Astar全局路径搜索Vplanner.py实现DWA动态窗口算法本体dwa.py则是在Vplanner之上做的一层封装把DWA的核心方法接到main.py主流程里。很多人第一次看会误以为dwa.py和Vplanner.py是两个平行的算法实现其实不是。Vplanner.py里保存的是DWA核心数据结构动态窗口计算、轨迹外推、评价函数全在里头dwa.py更像个门面负责初始化DWA对象并对外暴露一个计算下一时刻速度指令的接口。main.py只管交互和调度具体算法逻辑都在另外三个文件里。我建议的阅读顺序是AStarPlanner.py → Vplanner.py → dwa.py → main.py先理解单层算法再看耦合。别在没读Vplanner.py之前直接啃main.py否则里面那个DWA对象像黑匣子你只知道调用它返回一个速度却说不清这个速度是怎么从几千条候选轨迹里挑出来的。2.2 AStarPlanner.pyAstar寻路核心的实现骨架这个类干的事情很纯粹把地图抽象成栅格把起点、终点、障碍物映射成网格坐标用启发式搜索从起点向外扩展直到命中终点。下面这段是Astar核心逻辑的重写关键结构跟这个资源里的实现一致import heapq import math class AStarPlanner: def __init__(self, width, height, resolution1.0): self.width width self.height height self.resolution resolution self.obstacles set() # 保存障碍物栅格坐标 def plan(self, start, goal): start (int(start[0]), int(start[1])) goal (int(goal[0]), int(goal[1])) if start goal: return [start] open_list [] # 优先队列按f值从小到大弹出 heapq.heappush(open_list, (0.0, start)) came_from {} # 记录父节点回溯路径用 g_score {start: 0.0} f_score {start: self._heuristic(start, goal)} while open_list: _, current heapq.heappop(open_list) if current goal: return self._reconstruct_path(came_from, current) for neighbor in self._neighbors(current): # 四邻域搜索每走一格g代价为1 tentative_g g_score[current] 1.0 if neighbor not in g_score or tentative_g g_score[neighbor]: came_from[neighbor] current g_score[neighbor] tentative_g f_score[neighbor] tentative_g self._heuristic(neighbor, goal) heapq.heappush(open_list, (f_score[neighbor], neighbor)) return [] # open list耗尽没找到返回空路径 def _neighbors(self, node): x, y node candidates [(x 1, y), (x - 1, y), (x, y 1), (x, y - 1)] result [] for nx, ny in candidates: if 0 nx self.width and 0 ny self.height: if (nx, ny) not in self.obstacles: result.append((nx, ny)) return result def _heuristic(self, a, b): # 欧氏距离作为启发函数保证可采纳性 return math.hypot(a[0] - b[0], a[1] - b[1]) def _reconstruct_path(self, came_from, current): path [current] while current in came_from: current came_from[current] path.append(current) path.reverse() return path几个细节值得说明。启发式用了欧氏距离这是轮式机器人栅格导航里最常见的选型因为四邻域移动下欧氏距离不会高估代价Astar一定能搜出最优路径。代价方面相邻栅格移动固定为1g值体现的是栅格步数而不是物理距离这一步逻辑简单后面DWA接上来时Astar用的分辨率能不能对上就决定了整体效果。实际使用这个类非常直接创建对象后反复调plan障碍物集合变了直接重跑不需要重建对象。鼠标每点一次障碍物都触发一次plan也没问题搜索只在空格键按下时执行点击障碍物本身不触发计算这也是交互不卡顿的原因。2.3 Vplanner.py动态窗口采样、轨迹外推和评价计分DWA的立身之本在于它不在路径空间里搜索而在速度空间里采样。机器人当前速度和角速度受电机加速度上限约束下一时刻能取到的速度被限制在一个动态窗口内超出这个窗口的速度即使在地图上是合理的物理上也执行不了。Vplanner.py里最核心的函数就是干这件事import math class DWA: def __init__(self, config): self.max_vel config[max_vel] # 最大线速度 m/s self.max_omega config[max_omega] # 最大角速度 rad/s self.max_accel config[max_accel] # 最大线加速度 m/s^2 self.max_omega_accel config[max_omega_accel] # 最大角加速度 self.dt config.get(dt, 0.1) # 仿真步长 self.predict_time config.get(predict_time, 3.0) self.heading_w config.get(heading_w, 1.0) self.vel_w config.get(vel_w, 0.8) self.clearance_w config.get(clearance_w, 0.2) def calc_dynamic_window(self, v, w): # 受加速度限制下一时刻的速度有明确上下界 v_max min(self.max_vel, v self.max_accel * self.dt) v_min max(-self.max_vel, v - self.max_accel * self.dt) w_max min(self.max_omega, w self.max_omega_accel * self.dt) w_min max(-self.max_omega, w - self.max_omega_accel * self.dt) return [v_min, v_max, w_min, w_max] def predict_trajectory(self, state, v, w): # 用运动学模型外推一段时间内的位姿序列 traj [] x, y, theta state for _ in range(int(self.predict_time / self.dt)): x v * math.cos(theta) * self.dt y v * math.sin(theta) * self.dt theta w * self.dt traj.append((x, y, theta)) return traj两个参数在这里极为关键。max_accel如果设成0动态窗口秒变固定速度范围DWA退化成随机暴力采样看起来在跑但效果全凭运气。predict_time决定每条候选轨迹外推多长设太大轨迹会穿进远处障碍物clearance项误判设太小看不到前方弯道容易撞墙。差速轮AGV场景我一般取3到5秒再结合机器人制动距离去微调。窗口算出来后在窗口内按速度分辨率和角速度分辨率采样每组速度都外推一条轨迹然后交给评价函数打分。三条评分项缺一不可轨迹末端朝向与目标方向的角度差轨迹平均速度轨迹离最近障碍物的距离。def evaluate(self, traj, target_theta): # 把三项指标归一化到0~1后再加权求和 heading self._calc_heading(traj, target_theta) velocity self._calc_velocity(traj) clearance self._calc_clearance(traj) return (self.heading_w * heading self.vel_w * velocity self.clearance_w * clearance)权重直接决定小车性格。heading_w大小车倾向快速转向目标路径短但容易抖动clearance_w大小车离障碍物远远的路径变绕。资源里默认参数heading_w最大适合开阔地图真遇到狭窄走廊场景必须把clearance_w提上来否则小车贴墙刷过去的画面很难看。2.4 main.py的鼠标交互与空格触发流程main.py把两个算法组装成一个可交互程序。它初始化地图创建AStarPlanner对象和DWA对象然后用matplotlib事件接口监听鼠标和键盘import matplotlib.pyplot as plt def on_click(event): global start, goal, obstacles if event.inaxes is None: return x int(round(event.xdata)) y int(round(event.ydata)) if event.button 1: # 左键设置起点 start (x, y) elif event.button 3: # 右键设置终点 goal (x, y) elif event.button 2: # 中键添加障碍物 obstacles.append((x, y)) redraw() def on_key(event): if event.key : path astar.plan(start, goal) run_dwa_tracking(path) fig.canvas.mpl_connect(button_press_event, on_click) fig.canvas.mpl_connect(key_press_event, on_key) plt.show()事件回调里改的是全局变量真正执行规划在空格键事件里。如果二次开发时把plan调用直接塞进on_click每次点击都触发一次完整搜索界面疯狂闪烁体验极差。现在这个设计把场景构建和执行分离先摆好障碍物再统一规划很符合演示场景的操作直觉。run_dwa_tracking内部是个循环从全局路径中取前瞻节点作为DWA局部目标计算速度和角速度更新位姿直到抵达终点。前瞻节点取太远小车抄近道偏离全局路径取太近每个节点附近不断转向路径抖成波浪线。比较稳的折中方案是取“当前索引加固定步长”的节点步长通常3到5个栅格。3. 把二维路径规划跑起来环境准备、鼠标交互与场景验证3.1 装依赖和确认Python版本这份源码面向Python 3我用3.7以上版本都能跑3.8到3.11都行依赖只有numpy和matplotlib。命令行执行python -m pip install numpy matplotlib cd path/to/project python main.py第一次跑会弹出一个空白栅格图。macOS用户注意matplotlib默认后端是macosx远程ssh看不到窗口得在本地跑。Windows下弹不出窗口多半是IDE内置控制台的问题改成系统Terminal执行就行。这个坑我在Windows上踩过好多次PyCharm自带console跑matplotlib交互窗口经常卡事件循环。还有一种情况机器上同时装了python和python3命令pip装给了python3但python main.py用的却是另一个解释器直接报No module named numpy。遇到这种玄学报错先敲python --version确认路径别急着重装环境。3.2 鼠标三个键加空格键的交互规则交互规则五条鼠标左键设置起点右键设置终点鼠标中键逐个添加障碍物空格键触发规划。起点显示为蓝色圆点终点显示为红色标记障碍物是黑色方块规划出来的全局路径是绿色线条。这个设计没有提供删除障碍物的鼠标操作想重新摆地图只能关掉重启交互上有点遗憾。二次开发想加清除功能可以在on_click里加一个判断比如双击中键把最后一个障碍物弹出去再重绘改动量不大。3.3 一次完整的规划流程演示设地图尺寸13×11期望从左上角走到右下角中间摆一面L形障碍墙。操作顺序如下# 第一步鼠标左键点击左上角区域起点出现蓝色标记 # 第二步鼠标右键点击右下角区域终点出现红色标记 # 第三步鼠标中键点击以下栅格位置逐个添加障碍物 # (4, 4) (5, 4) (6, 4) (7, 4) # (4, 5) (7, 5) # 第四步按下空格键开始路径规划按下空格后界面上先出现Astar算出的绿色全局路径然后小车形状的标记沿着路径前进DWA在遇到障碍物时调整方向绕过。如果小车接近障碍物时速度没有明显下降而是硬生生贴墙刷过去基本能判断是clearance权重太小如果小车频繁原地转向、路径扭曲则是heading权重压过了velocity项这两个现象是调参时最先要看的。3.4 地图尺寸与障碍物预置方式地图尺寸在main.py开头一般用MAP_WIDTH和MAP_HEIGHT常量定义。这个参数的影响比想象中大Astar的open list规模跟地图面积近似成正比从10×10放大到50×50之后鼠标点击添加障碍物也变难点不准。如果只是验证算法效果建议把网格数控制在40×40以内保证交互精度。障碍物不一定要靠鼠标一个个点main.py里直接预置坐标列表同样是合法输入这个方式适合自动化测试和场景复现obstacles [(2, 3), (2, 4), (2, 5), (7, 8), (7, 9)] astar AStarPlanner(20, 20) astar.obstacles set(obstacles) path astar.plan((0, 0), (19, 19))这段代码我经常拿来压测不同地图尺寸下的规划耗时。把地图放大用time.time()包住plan调用前后时间差就能大致摸清Astar在大地图上的性能边界。测试下来相同障碍物密度下30×30地图基本秒出80×80开始有明显停顿。3.5 规划结果合法性验证跑完一次规划别急着看路径漂不漂亮先验证路径有没有穿过障碍物。写个几行的校验逻辑def check_path_valid(path, obstacles): for node in path: if node in obstacles: return False, node return True, None这个函数在调参阶段非常实用。Astar本身保证最优但如果鼠标点击产生的坐标跟内部栅格索引有off-by-one误差路径可能画在障碍物格子上把校验结果打印出来能第一时间发现是坐标换算问题还是膨胀参数问题不用肉眼盯着屏幕数格子。4. 从Python二维模拟到Gazebo仿真course_agv包的编译与启动4.1 course_agv_* 目录分别对应哪些ROS包压缩包里除了四个Python文件还有一组看起来像ROS工程的文件gazebo目录、course_agv_control、course_agv_nav、course_agv_description、course_agv_gazebo外加顶层CMakeLists.txt。按ROS功能包的命名习惯course_agv_description放URDF和xacro模型文件描述AGV底盘、轮子和传感器course_agv_gazebo放Gazebo仿真world和模型插入配置course_agv_control是底盘运动控制插件或控制器course_agv_nav大概率是导航配置和launch文件。把这几个包放进同一个ROS工作空间编译能在Gazebo里搭出一辆完整的差速AGV。能看出这份资源的完整度——不是只在二维图上跑算法还配套了一整套可仿真的AGV模型。从这里也能推导出二维Python版里的Astar和DWA算法可以直接复用Gazebo工程只是把底层的位姿更新替换成物理引擎。4.2 编译和启动流程要用这些ROS包得先把工程放进catkin工作空间编译常见做法cd ~/catkin_ws/src cp -r /path/to/course_agv_source ./ cd ~/catkin_ws catkin_make source devel/setup.bash roslaunch course_agv_gazebo course_agv_world.launch注意这里launch文件名是工程惯例写法实际操作以压缩包里README.md记录的文件名为准。编译报错先查Python环境Ubuntu 20.04加ROS Noetic要求Python 3.8如果手工把系统默认Python切换到3.10很多ROS包直接编译失败这类问题比代码本身的bug难排查得多。启动Gazebo之前还要确认环境变量已经source过不然roslaunch会报“RLException: xxx.launch is neither a launch file nor a package”这种误导性错误实际只是环境没加载。4.3 Python二维模拟与Gazebo仿真的核心差异二维Python版和Gazebo仿真最大的差距在数据链路上。二维版里坐标更新直接用运动学公式递推位置精确无噪声DWA算出来的速度跟实际位移完全对得上。Gazebo里位置由物理引擎计算里程计数据由插件发布链路长了许多。第一个坎是坐标系变换。AGV的base_link和odom之间TF不广播控制节点读到错误位置DWA会朝错误目标规划小车乱跑找不到原因。第二个差异在传感器二维版没有真实传感器小车对障碍物的感知靠地图静态坐标Gazebo里如果激光雷达话题/scan没有正常发布DWA的clearance项算出来的就是没有任何意义的数值。启动后第一件事先看话题rostopic list rostopic echo /odom -n 1 rostopic echo /scan -n 1这是排查一切Gazebo导航问题的起点三个话题有数据流动控制链路才可能正常。物理引擎还带来几个二维模拟体会不到的差异点。轮胎摩擦会造成轻微打滑DWA输出速度和现实速度差几个百分点位置反馈逐渐漂移。小车质量过大时电机最大力矩限制会让实际加速度达不到max_accel设定值动态窗口里的理论速度区间不可达表现在仿真里就是小车响应变慢、轨迹预测跟真实运动偏离越来越大。传感器采样频率如果只有10HzDWA控制循环却设成50Hz障碍物信息更新跟不上控制频率很容易撞上刚进入安全距离的物体。应对办法是把DWA控制周期降到跟传感器对齐或者对障碍物距离做一帧缓存外推。4.4 两种运行方式的选型建议二维Python版调试成本极低改一个参数重启一秒适合前期验证算法权重Gazebo仿真每次启动加载模型和world半分钟起步不适合在里头反复试参数。我的习惯是先在二维版把三个权重系数调到效果满意记录下来再去Gazebo核对物理表现通常只需要改速度限制和加速度限制两个参数就能对齐。对比项二维Python版Gazebo仿真位置更新运动学公式递推无噪声物理引擎计算有打滑和漂移障碍物感知静态坐标理想感知激光雷达话题受采样率限制调试周期秒级重启环境加载慢迭代成本高坐标系无base_link、odom、map三者变换适用阶段算法验证和调参集成测试和演示5. 避坑与常见问题运行、规划、避障三个环节的翻车记录5.1 现象python main.py一运行就报No module named numpy原因新环境没装依赖或者python和python3命令指向不同解释器。解决python -m pip install numpy matplotlib装完仍然报错先敲python看解释器路径再用pip list确认numpy装到哪个环境里了。有些机器上python指向系统自带旧版本python3指向自装新版装到python3里的包python调不出来。敲python3 main.py基本能解决。5.2 现象Astar规划路径贴着障碍物边缘甚至直接穿模原因栅格地图没做障碍物膨胀。路径计算只要求格子不被Occupancy标记但真实机器人有半径贴着障碍物走必然刮蹭。解决构建障碍物集合时做膨胀处理把障碍物周围一圈全标记为不可通行def inflate_obstacles(obstacles, radius, width, height): inflated set() for ox, oy in obstacles: for dx in range(-radius, radius 1): for dy in range(-radius, radius 1): nx, ny ox dx, oy dy if 0 nx width and 0 ny height: inflated.add((nx, ny)) return inflated半径一般取机器人半径除以栅格分辨率再向上取整。代价是地图拥挤时可能搜不出路径这时需要降分辨率或缩小机器人半径参数。这个坑在二维Python版里不明显因为matplotlib画线本身有宽度路径看着好像没碰上障碍物实际上换到Gazebo一仿真就露馅。5.3 现象DWA在路径后半段原地打转原因heading权重太大或前瞻点离得太近。DWA靠近目标时目标点就在当前位姿旁边heading项持续要求转向转向产生位移位移又改变偏角形成震荡。解决把heading_w从1.0降到0.5以下前瞻节点改成路径上距离当前位置大于0.5m的第一个节点并且加抵达判断距离小于阈值时直接切换停止控制。这个问题的根因在于Astar全局路径节点间距不一致靠近目标时节点加密DWA的局部目标从“路径上某个较远点”变成了“终点本身”评价函数里heading权重瞬间失控。调参时如果发现后半段开始画圈先看前瞻节点是不是被设置成了path[-1]。5.4 现象matplotlib窗口上点击起点终点都没反应原因事件绑定没成功或者回调函数里event.xdata是None。某些macOS后端不走默认交互某些IDE的交互后端不支持鼠标事件。解决在程序开头强制指定后端import matplotlib matplotlib.use(TkAgg)还不行就在on_click回调里print(event.button, event.xdata)如果字段全是None说明事件压根没进回调基本可以确定是后端问题。之前帮人排查过他用的Linux服务器上没装图形库matplotlib用的Agg后端根本不支持交互换成TkAgg并且装上python3-tk包就正常了。5.5 现象Gazebo启动后小车纹丝不动原因cmd_vel话题没有数据流或者控制插件没加载成功。解决先订阅话题看有没有数据再用rostopic pub手动发布一个速度测试rostopic pub -r 10 /cmd_vel geometry_msgs/Twist \ {linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {z: 0.2}}小车动了说明控制链路正常问题在导航节点还是不动就检查URDF里的diff_drive插件配置joint名字必须跟URDF定义一致差一个下划线插件都会静默失败这是Gazebo仿真里最玄学的坑之一gazebo plugin namediff_drive filenamelibgazebo_ros_diff_drive.so leftJointleft_wheel_joint/leftJoint rightJointright_wheel_joint/rightJoint commandTopiccmd_vel/commandTopic odometryTopicodom/odometryTopic /plugin /gazebo6. 进阶玩法参数外置、左右轮速度换算与真机迁移6.1 把DWA权重抽成参数文件源码里权重直接写在类的构造函数参数里跑一组实验改一次代码非常低效。我拿到手第一件事就是把参数外置成json文件运行时加载做对比实验只改文件不动代码{ max_vel: 1.0, max_omega: 1.2, max_accel: 0.5, max_omega_accel: 0.8, heading_w: 1.0, vel_w: 0.8, clearance_w: 0.2, predict_time: 3.0 }main.py里初始化DWA时改成import json with open(dwa_config.json) as f: config json.load(f) dwa DWA(config)换成配置化之后做调参实验效率高得不是一点半点。我一般会建一个results目录每组实验存一份json和对应的运行截图复盘时直接对比参数组合和路径形态不用靠记忆猜上次跑的哪组参数。6.2 从二维模拟到真实差速底盘的改动三步没有真车之前可以先把DWA输出换算成左右轮速度这个公式在差速轮AGV上通用left_wheel_vel (v - omega * wheel_base / 2.0) / wheel_radius right_wheel_vel (v omega * wheel_base / 2.0) / wheel_radius迁移到真实底盘时做三件事第一把二维版的位姿更新替换成从里程计读取位置第二把DWA输出的线速度和角速度按上面公式转成左右轮转速下发到电机驱动第三加一层急停保护激光测距低于安全阈值时直接把速度清零。我在实验室做这个迁移时原来二维版跑得很顺的权重在真车上几乎直接可用唯一的问题出在底盘电机驱动板的串口通信周期和DWA控制周期不一致反馈抖动严重把两者统一成50ms才稳定下来。从那以后我每次调路径规划参数都强制走一遍“二维验证、半实物仿真、真机复核”的流程每一步的参数记录都留档不然真机出了状况根本无法回溯。希望帮到你。本文还有配套的精品资源点击获取
