AGV激光SLAM导航这套东西圈外人听起来高大上圈内人知道底细之后会发现其实就三板斧建图、定位、路径规划。真正折磨人的不是算法本身而是环境依赖、参数调优和那些说不清道不明的玄学bug。这篇文章不打算从SLAM原理的数学公式讲起那玩意儿《视觉SLAM十四讲》写得更明白。我直接从实战出发讲讲用PythonROS实现一台差速AGV的激光SLAM导航重点放在路径规划配置和避障代码怎么写把我实际跑车过程中踩过的坑、验证过的参数一并分享出来。文中的方案基于ROS Noetic Ubuntu 20.04机器人平台是差速驱动激光雷达用的2D单线雷达。对刚接触ROS的小白和正在做毕设、竞赛项目的同学来说这套技术栈最稳妥社区资料多出了问题也容易搜到答案。1. 项目整体思路从需求到技术选型1.1 AGV激光SLAM导航到底做了什么一台AGV要自主行走核心要解决三个问题我在哪、我要去哪、我该怎么去。对应到技术层面就是定位、目标点设置、路径规划。激光SLAM导航的完整闭环是这样跑的激光雷达扫描周围环境SLAM算法根据扫描数据构建地图导航时激光数据与已有地图匹配算出机器人在地图中的位置也就是定位收到目标点后全局路径规划器在已知地图上规划出一条从当前位置到目标点的可行路径局部路径规划器再根据实时激光数据在全局路径的基础上规划出当前时刻的实际运动轨迹并实时避障。听起来不复杂但这里面的任何一个环节出了问题整个系统都会罢工。我记得第一次跑通完整的自主导航时心里想的是就这但调试过程却花了两周时间大部分时间都耗费在TF树配置、坐标变换和参数调整上。1.2 为什么是ROS Python 2D激光雷达选型这个问题被问过无数次我直接说结论ROS是这个领域的事实标准它帮你把传感器驱动、数据分发、模块通信、可视化这些基础工作全做了你只需要关注算法本身。自己从头写一套机器人操作系统不是不行但没必要重复造轮子。Python作为开发语言优点是上手快、调试方便。ROS的rospy库虽然性能和C的roscpp有差距但对于AGV这种对实时性要求没那么极端的场景完全够用。导航框架move_base的核心是C写的Python只是调用接口和写业务逻辑性能瓶颈不在Python这一层。激光雷达选2D单线而不是3D视觉或毫米波雷达主要考虑三点成本、稳定性和算力需求。一台像样的2D激光雷达比如镭神、思岚的入门款千把块钱单线扫描的数据量远小于点云树莓派级别的算力就能跑起来。对室内AGV来说2D激光已经能覆盖大多数需求。对比一下几种感知方案的优缺点方案成本鲁棒性算力需求适用场景2D激光雷达低高不受光照影响低结构化的室内环境3D激光雷达很高高高室外复杂环境、自动驾驶视觉SLAM中中受光照、纹理影响高特征丰富的环境毫米波雷达中高中恶劣天气、远距离选2D激光还有一个实际原因ROS的navigation栈对2D激光的支持最成熟gmapping、cartographer、move_base、AMCL这些核心工具全是围绕2D激光设计的配置起来事半功倍。1.3 导航系统的四个核心模块整套系统拆开来看就是四个模块协作建图用gmapping或cartographer把激光数据里程计数据融合成一张二维栅格地图定位AMCL自适应蒙特卡洛定位通过粒子滤波估计机器人在已知地图中的位姿全局路径规划navfn或global_planner插件在地图上规划出全局路径局部路径规划与避障DWA或TEB实时计算速度指令规避动态障碍物这四个模块的通信关系是SLAM建图阶段产出地图文件导航阶段加载地图AMCL发布机器人在地图中的位姿move_base订阅位姿和激光数据经过全局规划与局部规划后输出/cmd_vel速度指令驱动底盘运动。理解了这个数据流你就知道调试时的排查方向了速度指令不对先看局部规划器地图对不上先看定位定位飘了先看里程计和TF树。2. 环境搭建与硬件准备2.1 ROS环境安装与Python环境配置环境这块我吃过不少亏先说结论ROS Noetic只支持Ubuntu 20.04ROS2 Humble对应Ubuntu 22.04。这套项目用的Noetic因为navigation栈在ROS1上更成熟网上资料也更多。ROS安装如果按官网教程一步步来会经历漫长痛苦的依赖编译过程。实际开发中推荐用鱼香ROS的一键安装脚本这个工具圈内用得很多能省掉大量重复劳动。安装完成后记得验证环境source /opt/ros/noetic/setup.bash roscore能正常启动roscore说明ROS核心装好了。Python这边确保是Python3Noetic默认支持Python3直接装依赖就行sudo apt install python3-rosdep python3-rospy python3-sensor-msgs python3-geometry-msgs sudo apt install ros-noetic-navigation ros-noetic-slam-gmapping ros-noetic-amcl关键一点工作空间一定要用catkin_make或catkin build编译过Python节点才能被rosrun找到。如果你用VSCode写Python节点记得配置好解释器路径指向ROS的Python环境否则import rospy会报错。2.2 激光雷达选型与驱动配置激光雷达选型我推荐从思岚A1/A2或镭神N10起步这两个型号社区资料多驱动代码完善。雷达驱动装好后先单独测试一下数据输出roslaunch rplidar_ros rplidar_a1.launch rostopic echo /scan能看到激光数据话题后再做可视化验证rosrun rviz rviz添加LaserScan显示把Fixed Frame设为laser_frame就能看到雷达扫描的点云图像了。配置雷达驱动时有个关键细节frame_id要和URDF模型里的雷达坐标系名称一致。我刚开始跑的时候雷达数据一直显示不出来就是因为驱动默认的frame_id是laser而我的URDF里写的laser_frame花了半天才排查出来。2.3 里程计与机器人模型的关键配置里程计在SLAM和导航中的地位被很多人低估。激光SLAM看起来是激光说了算实际上里程计提供了帧间运动的初始估计尤其在雷达退化场景长走廊、空旷区域中里程计是唯一的依靠。差速机器人的里程计通常由电机编码器计算得到。你需要发布odom到base_link的TF变换这个变换描述了机器人在世界坐标系中的位姿变化。写一个Python里程计节点核心伪代码如下# 订阅电机编码器数据 # 计算左右轮速度差得到线速度和角速度 # 积分计算位置变化 # 发布odom话题和TF变换URDF模型里需要定义base_link、laser_frame等坐标系以及它们之间的相对位置关系。激光雷达的安装高度、前后偏移量必须准确否则建图会变形。TF树是ROS机器人调试中最容易出问题的环节。完整的TF树应该长这样map - odom - base_link - laser_frame。建图时map和odom重合导航时map由AMCL发布odom由里程计节点发布。3. 建图与导航核心实操3.1 用gmapping构建环境地图建图是整个流程的第一步地图质量直接决定后续导航效果。gmapping是ROS中最经典的2D激光SLAM算法对小场景建图效果好参数简单适合入门。启动建图roslaunch agv_navigation gmapping.launch rosrun teleop_twist_keyboard teleop_twist_keyboard.py用键盘控制机器人缓慢移动把整个环境扫描一遍。建图时注意几点移动速度要慢转弯要平稳让激光数据充分匹配场景中不要有人走动动态物体会在地图上留下鬼影扫描完一个区域再移动避免快速转向导致匹配失败建图完成后保存地图rosrun map_server map_saver -f ~/map/agv_map会生成agv_map.pgm图像和agv_map.yaml地图配置这两个文件后续导航要用。如果用gmapping在小场景下出现明显畸变可以换cartographer试试。cartographer对激光数据质量要求更高但回环检测能力强大场景表现更好。代价是配置复杂需要调一堆参数新手不建议一上来就用。3.2 move_base全局与局部路径规划配置导航的核心是move_base节点它整合了全局规划器和局部规划器。我用的配置方案是NavfnROS做全局规划DWA做局部规划这套组合最稳定对差速机器人最友好。全局规划器的任务是在静态地图上找一条从起点到目标点的路径。NavfnROS底层用的是Dijkstra算法计算所有节点到目标点的最短距离然后沿梯度下降找到路径。它不考虑机器人运动学约束所以规划出来的路径可能是折线。参数配置中需要关注的是GlobalPlanner: use_grid_path: false allow_unknown: true neutral_cost: 66局部规划器DWA动态窗口法是真正的大脑它在速度空间中采样多组线速度和角速度根据代价函数打分选最优的作为实际运动指令。DWA会考虑机器人的运动学约束最大速度、加速度所以规划出的轨迹是平滑的能实时避开障碍物。DWA的核心代价函数包含三项目标方向代价朝向目标点、速度代价鼓励快速移动、障碍物距离代价远离障碍物。三个权重需要根据实际场景调DWAPlannerROS: max_vel_x: 0.3 min_vel_x: -0.1 max_vel_trans: 0.3 max_vel_theta: 1.0 xy_goal_tolerance: 0.15 yaw_goal_tolerance: 0.2 path_distance_bias: 32.0 goal_distance_bias: 24.0 occdist_scale: 0.01这几个参数是调参中影响最大的。max_vel_x决定了机器人最大线速度室内环境0.3左右就够快了太快会导致刹车距离太长定位跟不上。occdist_scale是避障权重太小会撞墙太大会绕远路甚至停在原地不动。3.3 导航启动与真实场景调参启动导航的命令roslaunch agv_navigation navigation.launch在RVIZ中通过2D Nav Goal工具点击地图上的目标点机器人就能自主规划路径并移动过去。第一次跑通的感觉很爽但紧接着你就会发现问题路径走得歪歪扭扭、到了目标点转圈、遇到障碍物就卡死。调参我总结了一个顺序先调速度参数再调规划器权重最后调代价地图参数。不要一上来就调一堆参数改一个参数跑一次测试记录效果再改下一个。改了参数后要重新加载rosrun dynamic_reconfigure dynparam set /move_base/DWAPlannerROS max_vel_x 0.25这样不用重启节点线上就能调试。这个功能在调参时太有用了强烈建议新手学会。代价地图的参数也会影响导航效果。膨胀半径inflation_radius决定了机器人离障碍物多远就开始避让一般设置为机器人宽度的三分之一到二分之一。设置太小机器人会紧贴障碍物通过容易剐蹭设置太大窄通道过不去。local_costmap: inflation_radius: 0.2 cost_scaling_factor: 3.04. 避障代码的实现与接入4.1 为什么还需要自定义避障节点看到这你可能会问move_base的DWA局部规划器不是已经能避障了吗为什么还要自己写避障代码有三个原因第一DWA是软避障它通过代价函数尽量避开障碍物但代价权重设置不当或机器人惯性过大时仍然可能撞上障碍物。尤其紧急出现的障碍物比如行人突然走到机器人前方DWA的反应不够快。第二自定义避障节点可以作为独立的安全层运行。如果move_base崩溃了、或者没有收到目标点机器人停在那行人撞上来怎么办安全层应该独立于导航逻辑持续监控激光数据有碰撞风险就强制停车。第三在某些特殊工况下比如AGV要进入货架底部、通过狭窄通道DWA的通用参数不一定适用需要写特殊逻辑。所以我的做法是move_base负责正常情况下的导航避障自定义安全节点作为最后一道防线两者并行运行。4.2 基于激光分区的紧急避障Python代码这个避障节点的思路很简单把激光雷达的扫描数据分成左、中、右三个区域分别计算每个区域内的最近障碍物距离。如果某个方向的障碍物距离低于安全阈值就发布减速或转向指令优先级高于move_base的正常指令。先创建一个Python文件emergency_stop.py#!/usr/bin/env python3 import rospy import math from sensor_msgs.msg import LaserScan from geometry_msgs.msg import Twist class EmergencyStop: def __init__(self): rospy.init_node(emergency_stop) # 安全距离阈值单位米 self.safe_distance 0.3 # 减速距离阈值单位米 self.slow_distance 0.6 self.cmd_pub rospy.Publisher(/emergency_cmd_vel, Twist, queue_size1) rospy.Subscriber(/scan, LaserScan, self.scan_callback) rospy.loginfo(Emergency stop node started) def scan_callback(self, msg): # 将激光数据分成三个区域 # 假设激光雷达正前方为0度逆时针增加 # 前方区域-30度到30度之间 # 左方区域30度到90度之间 # 右方区域-90度到-30度之间 front_dist float(inf) left_dist float(inf) right_dist float(inf) n len(msg.ranges) angle_min msg.angle_min angle_increment msg.angle_increment for i in range(n): angle angle_min i * angle_increment dist msg.ranges[i] # 跳过无效数据NaN或inf if not math.isfinite(dist): continue # 跳过过远的点减小计算量 if dist 3.0: continue # 将角度归一化到 -pi 到 pi while angle math.pi: angle - 2 * math.pi while angle -math.pi: angle 2 * math.pi # 前方区域-30度到30度 if -math.pi/6 angle math.pi/6: if dist front_dist: front_dist dist # 左方区域30度到90度 elif math.pi/6 angle math.pi/2: if dist left_dist: left_dist dist # 右方区域-90度到-30度 elif -math.pi/2 angle -math.pi/6: if dist right_dist: right_dist dist twist Twist() # 前方障碍物太近紧急停车 if front_dist self.safe_distance: twist.linear.x 0.0 twist.angular.z 0.0 rospy.logwarn(Emergency stop! Front obstacle: %.2f m, front_dist) # 前方障碍物接近减速 elif front_dist self.slow_distance: # 线性降低速度 speed_ratio (front_dist - self.safe_distance) / (self.slow_distance - self.safe_distance) twist.linear.x 0.15 * speed_ratio # 如果左右有空间微调转向避开 if left_dist right_dist and left_dist self.slow_distance: twist.angular.z 0.3 elif right_dist left_dist and right_dist self.slow_distance: twist.angular.z -0.3 rospy.loginfo(Slowing down, front: %.2f, speed: %.2f, front_dist, twist.linear.x) # 正常情况不干预move_base else: twist.linear.x float(nan) twist.angular.z float(nan) self.cmd_pub.publish(twist) if __name__ __main__: try: EmergencyStop() rospy.spin() except rospy.ROSInterruptException: pass这段代码的逻辑是只在紧急情况下接管速度控制正常情况下发布NaN让上层节点知道我不干预。通过不同的安全阈值实现分级响应先减速、再停车离障碍物越近反应越强烈。4.3 如何把避障节点接入move_base写好的节点单独发布速度指令还不够还需要一个仲裁机制来决定到底听谁的。我在实际项目中用的是话题合并的方式创建一个cmd_vel_merge.py节点订阅move_base发布的/cmd_vel和避障节点的/emergency_cmd_vel经过仲裁后发布最终的/cmd_vel_out给底盘。#!/usr/bin/env python3 import rospy from geometry_msgs.msg import Twist class CmdVelMerger: def __init__(self): rospy.init_node(cmd_vel_merger) self.nav_cmd Twist() self.emergency_cmd Twist() self.has_emergency False rospy.Subscriber(/cmd_vel, Twist, self.nav_callback) rospy.Subscriber(/emergency_cmd_vel, Twist, self.emergency_callback) self.cmd_pub rospy.Publisher(/cmd_vel_out, Twist, queue_size1) rospy.Timer(rospy.Duration(0.1), self.timer_callback) def nav_callback(self, msg): self.nav_cmd msg def emergency_callback(self, msg): if not math.isnan(msg.linear.x): self.emergency_cmd msg self.has_emergency True else: self.has_emergency False def timer_callback(self, event): if self.has_emergency: self.cmd_pub.publish(self.emergency_cmd) else: self.cmd_pub.publish(self.nav_cmd) if __name__ __main__: try: CmdVelMerger() rospy.spin() except rospy.ROSInterruptException: pass仲裁逻辑是避障节点一旦发布有效指令就覆盖move_base的指令实现安全优先。时间间隔0.1秒对应10Hz的控制频率对AGV来说足够了。底盘驱动那边把原来订阅的/cmd_vel改成/cmd_vel_out。这样整个控制链就是move_base发布指令 - 避障节点判断是否介入 - 合并节点仲裁 - 底盘执行。这套机制的响应速度大约在100毫秒级别对室内AGV来说在0.3m/s速度下100毫秒的运动距离是3厘米加上激光雷达扫描频率和计算延迟能在撞上障碍物之前停下来。5. 常见问题与排查经验5.1 建图时地图漂移地图漂移是SLAM建图最常遇到的问题现象是墙壁重影、走廊扭曲、地图越来越歪。原因通常是里程计误差累积太快或者激光匹配失败。排查顺序先看TF树的odom到base_link变换是否平滑如果有跳变说明里程计数据有问题再看激光雷达的安装是否牢固雷达松动会导致每帧数据角度偏移最后检查雷达的扫描频率和角分辨率是否与gmapping参数匹配。我遇到过一个很隐蔽的问题建图时地图会整体缓慢旋转检查了一圈发现是底盘左右轮的编码器分辨率不一致导致机器人以为自己走直线实际在画弧。换用高精度编码器后问题才解决。5.2 导航时定位漂移建图没问题导航时AMCL定位漂移是另一个高频问题。常见表现是机器人在RVIZ中显示的位置与实际位置偏差越来越大到达目标点时姿态对不上。这种情况优先检查AMCL的初始位姿。AMCL是概率定位算法如果初始位姿不对粒子群收敛不到正确位置后续定位就会一直偏。在RVIZ中用2D Pose Estimate给一个准确的初始位姿能解决大部分问题。还有一个容易被忽视的点AMCL只适用于轮式机器人的平面运动如果地面不平导致机器人有颠簸激光数据会有噪声定位会变差。这时可以调整AMCL的odom_alpha参数增大里程计噪声模型让它更信任激光数据。5.3 RVIZ卡顿与可视化问题导航调试时RVIZ卡顿很影响效率。地图大了之后RVIZ渲染有延迟是正常的。我习惯关闭不需要的显示项只保留Map、RobotModel、Path、Local Costmap这几个关键项。激光数据在RVIZ中显示不全通常是frame_id不匹配。检查Fixed Frame和LaserScan消息头里的frame_id是否一致。记得修改后要重启RVIZ有时候勾选操作不会立即刷新。查看TF树用rqt_tf_tree比在RVIZ里看直观得多。所有坐标系之间的连接关系一目了然缺了哪个变换一眼就找出来。5.4 速度指令冲突问题汇总接入避障节点后可能会出现机器人向一个方向直冲不停的情况。大概率是话题名配置错了底盘实际订阅的话题不是合并节点的输出导致避障指令根本没生效。检查方法# 查看所有cmd_vel相关话题 rostopic list | grep cmd_vel # 查看底盘驱动订阅的话题 rosnode info /your_base_driver确保用的是一致的topic不要出现一个节点发/cmd_vel、另一个节点发/cmd_vel_out而底盘只听/cmd_vel的情况。6. 写在最后关于这套方案的一点心得整个AGV激光SLAM导航项目做下来我最深的体会是技术方案的选择往往不是选最先进的而是选最可控的。2D激光ROS Navigation这套组合算法成熟、工具链完善、社区资料丰富出了问题能找到答案。视觉SLAM、3D激光方案确实更前沿但调试复杂度也更高新手很容易陷在环境配置里出不来。给新手两点具体建议第一先在仿真环境里跑通整个流程。Gazebo里配好机器人模型和激光雷达先把建图、导航、避障这些逻辑跑清楚再上真机。真机调试的变量太多了如果逻辑本身没验证过出了问题很难定位。第二调参时一次只改一个参数用rosbag录下数据回放对比每调一次记录实际效果不要凭感觉乱调。最后一个实用技巧在navigation的launch文件里加上requiredtrue、respawntrue这些参数保证核心节点崩溃后能自动重启。AGV要长时间运行稳定性比花哨的功能重要得多。这个细节我在项目后期才意识到当初如果能早点加上能省掉不少半夜被叫醒的麻烦。