这篇解决SLAM 课学完了、真机还是迷路的问题。读完你能带走三样东西一张 SLAM 全景图搞懂前端、后端、回环各自在兜什么底两段纯 numpy 可跑的实验代码亲手复现52 秒偏出 1.38 米和FastSLAM 收敛到 0.139 米一条从建图到让机器人真的走起来的 Nav2 最小流水线每一步带可操作的检查点。系列导航《VLA 模型到底解决了什么从 RT-2 到 π0 三个关键跨越》误区二留了个钩子——“机器人要在户外或大空间里自主移动地图、定位、长程规划依然绕不开”。本篇就是来正面回答这个问题的。本篇有先验地图的室内路线——SLAM 怎么建图、怎么定位Nav2 怎么让机器人走起来。《LM-Nav 精读》没有先验地图大模型怎么在户外把几个地标串成一条路——与本文是一对互补方案一个管没图怎么办一个管有图怎么走。后续《OpenVLA 单卡推理与 LoRA 微调》在消费级显卡上把 VLA 跑起来。先说一个扎心的场景。SLAM 的网课你刷完了公式推导你看过了仿真里秒跑通作业分也不低。然后你把激光雷达装上真机遥控机器人推了一圈——rviz2 里建出来的地图像一幅抽象画墙是斜的一扇门建出了两扇的宽度走一圈回到起点起点和终点在地图里差出去半米。更糟的是下一步你让机器人导航回起点它在自己的地图里当场迷路原地扭来扭去就是走不回去。仿真和真机之间隔的不是运气是三个真凶完美里程计幻觉你以为轮式里程计报出来的位移是准的、回环没触发建歪了没有任何机制去纠错、定位丢失图是图我是谁我在哪。下面按这三个真凶的顺序从原理拆到可跑代码再到真机流水线。一、SLAM 到底在解什么一张图看懂鸡生蛋SLAMSimultaneous Localization And Mapping同时定位与建图这个名字本身就说明了尴尬没有地图你没法定位——连图都没有我在图里哪儿无从谈起没有定位你也没法建图——每帧观测往哪儿叠都不知道。鸡生蛋、蛋生鸡所以只能同时解。这是机器人在陌生环境里的睁开双眼时刻。如图 1 所示整张 SLAM 全景可以拆成三层来看。输入端有两种信息里程计和 IMU 告诉你这一步大概走了多远短时间准、长期必漂LiDAR 和相机告诉你四周长什么样是观测没有累积误差。处理端分两截前端做帧间匹配扫描匹配或特征跟踪负责实时输出位姿但会漂后端做位姿图加全局优化回环检测兜底负责全局自洽。输出端是一对互为前提的东西地图是环境的记忆定位回答我在哪儿两者互相喂。记一个分工口诀前端管这一秒别跟丢后端管全局别建歪回环管认出老地方一次性清账。后面你会看到真机翻车几乎全是这三层里某一层没干活。顺手回答一个新手常见疑问既然里程计长期必漂为什么不全靠激光雷达因为帧间匹配需要一个像样的初值盲搜又慢又容易锁错里程计就是那个初值。所以两者是搭档不是对手——里程计管短时观测管长程回环管兜底谁也替代不了谁。二、开源方案怎么选五大门派一张表开源 SLAM 生态已经很成熟问题不是有没有是选哪个。先说结论选型第一问不是哪个最强而是我的传感器是什么、室内还是户外、地图要不要长期维护。一张表看懂五大门派方案传感器一句话点评LOAM 系列Zhang Singh, RSS 2014LiDARLiDAR 里程计与建图分开做的经典户外大场景 3D 建图的常客ORB-SLAM 系列ORB-SLAM3: Campos et al., 2021相机视觉特征点法代表作纯视觉路线绕不开RTAB-MapRGB-D / 双目图优化 外观回环适合地图要长期维护的场景slam_toolbox2D LiDARROS2 社区 2D 建图主流和 Nav2 无缝衔接Cartographer2D/3D LiDAR可融合里程计 / IMU谷歌系回环是硬功夫下一节的主角给个直接的入口建议室内 2D ROS2 技术栈slam_toolbox 或 Cartographer 任选一个先跑通再说户外大空间 3D 看 LOAM 系没 LiDAR 只有相机看 ORB-SLAM 系RGB-D 相机而且希望机器人边用边维护地图看 RTAB-Map。本文走室内 2D这条最常见也最适合入门的线。另外提醒一句表里的点评都是定性判断不是跑分结论——先用最小代价把一条路跑通再谈换件和调优比在选型上内耗划算得多。三、Cartographer 拆解Local SLAM 保实时回环兜全局Cartographer 是 Google 2016 年发表并开源Apache 2.0的 2D/3D LiDAR SLAMHess、Kohler、Rapp、Andor、Dellaert“Real-Time Loop Closure in 2D LIDAR SLAM”ICRA 2016。它值得精读因为它把前端保实时、后端保全局的工程哲学做到了教科书级别。如图 2 所示它的机制可以拆成四步。第一地图不是一整张大图而是一系列 submap——每个 submap 是一小块概率栅格。第二每帧扫描进来先做实时相关性扫描匹配粗对齐再用 Ceres 优化扫描匹配细对齐然后才插入当前 submap。为什么要两步相关性匹配快但糙Ceres 准但贵粗匹配给细匹配提供初值实时和精度就都有了。第三回环检测用多分辨率预计算栅格加分支定界branch-and-bound搜索回环候选不在实时线程里慢慢搜。第四位姿图节点是 submap 位姿边是约束在后台周期性做稀疏位姿图优化SPA底层用 Ceres最终得到全局一致的大地图。这里有一条最容易被忽略的工程启示Local SLAM 只保证相邻几秒不漂漂移是靠回环检测加全局优化兜底的。所以地图建歪了最有效的土办法是回去再走一遍——让机器人重新经过走过的地方它才有机会认出错来的地方触发回环、一次性清账。很多人建图失败就删图重来其实先试试把没走好的那段再推一遍。四、动手实验一只靠里程计52 秒偏出 1.4 米完美里程计幻觉值得用一个实验亲手戳破。设定机器人在一个 16×11 m 的矩形回路上走角点从 (2,2) 到 (18,2) 到 (18,13) 再到 (2,13)速度约 1.2 m/s控制周期dt0.2 s260 步约 52 秒——总里程比一圈多出约 7 米终点故意落在过起点约 7 米的底边上就是要看误差在闭环之外继续滚大。里程计不是理想的测到的线速度带σ_v0.04 m/s的噪声角速度带σ_w0.06 rad/s的噪声——这已经是轮式底盘相当体面的水平了。真实运动由理想底盘精确执行里程计读数则在真实控制量上加噪声。dead_reckoning.py —— 只靠里程计的航位推算误差是积分出来的只会涨不会消 走一个 16x11 m 的矩形回路里程计速度带噪声看推算终点偏离真实终点多远importnumpyasnp rngnp.random.default_rng(11)dt,T0.2,260# 0.2 s 一拍260 步 ≈ 52 s一圈多 7 mcornersnp.array([[2,2],[18,2],[18,13],[2,13],[2,2]])posenp.array([2.0,2.0,0.0])# 真实位姿 x, y, thetaodopose.copy()# 里程计推算位姿wp1fortinrange(1,T):dcorners[wp]-pose[:2]# 朝下一个角点走e(np.arctan2(d[1],d[0])-pose[2]np.pi)%(2*np.pi)-np.pi wfloat(np.clip(2.0*e,-1.5,1.5))v0.0ifnp.linalg.norm(d)0.4else1.2ifnp.linalg.norm(d)0.6:wpwp%41# 真实运动理想底盘精确执行控制量poseposenp.array([v*dt*np.cos(pose[2]w*dt/2),v*dt*np.sin(pose[2]w*dt/2),w*dt])# 里程计测到的速度带噪声sigma_v0.04 m/s, sigma_w0.06 rad/sv_o,w_ovrng.normal(0,0.04),wrng.normal(0,0.06)odoodonp.array([v_o*dt*np.cos(odo[2]w_o*dt/2),v_o*dt*np.sin(odo[2]w_o*dt/2),w_o*dt])print(真实终点: (%.2f, %.2f) 推算终点: (%.2f, %.2f)%(pose[0],pose[1],odo[0],odo[1]))print(终点位置误差: %.2f m%np.linalg.norm(odo[:2]-pose[:2]))本地实跑输出随机种子 11与出图脚本一致真实终点: (9.00, 1.57) 推算终点: (10.09, 0.73) 终点位置误差: 1.38 m输出块第一行的真实终点 (9.00, 1.57) 就是上面说的过起点约 7 米的位置——不是代码写错了是故意多走这一段。如图 3 所示本地实验脚本随文复现左图是真实轨迹和推算轨迹的对比刚跑完一圈、还多走了约 7 米推算轨迹已经整体变形终点偏飞右图是位置误差随时间的变化——误差越滚越大朝向误差此消彼长会让曲线局部回落大势一直向上52 秒累计到 1.38 米推算轨迹画出来的回路早就合不拢了。这个实验要说透一句话里程计误差是积分出来的只涨难消。σ_v0.04 m/s听起来很小但每一步的误差都会被累加到位姿上而且朝向误差还会通过速度乘方向放大位移误差——漂移是带复利的。瞬时误差可能因为朝向误差的此消彼长小幅回落但只要不引入外部观测这个账永远没有归零的机会。这就是第一个真凶完美里程计幻觉也是为什么 SLAM 必须存在。那怎么办三条路其一把里程计标定得更好但这只是让误差涨得慢一点涨本身停不下来其二引入全局观测路标、扫描匹配让误差有机会被一步步拉回来其三走回老地方触发回环一次性清账。实验二演示的就是第二条路的极简版第六节的流水线里你会见到第三条路的工程版。五、动手实验二纯 numpy 跑通 FastSLAM里程计会漂但环境里有路标。FastSLAMMontemerlo、Thrun、Koller、WegbreitAAAI 2002的洞察是把 SLAM 这个鸡生蛋问题拆开用一堆粒子表示机器人位姿的不确定性我在哪儿每个粒子挂一套独立的路标高斯估计器世界长什么样。已知数据关联时整个问题复杂度是 O(M log N)M 是粒子数、N 是路标数。位姿和地图就这么被同时估计出来了——不需要先有图也不需要先知道我在哪。每个时间步做三件事。第一运动采样同一份里程计读数发给所有粒子每个粒子自己加一份噪声粒子云就表达了我可能在哪。第二路标 EKF 更新每个粒子用它自己估计的路标位置预测观测然后拿 innovation量测减预测距离分量必须减去预测值角度分量要绕回正负 π 以内去修路标的均值和协方差同时按这次观测的似然更新粒子权重——路标对的准的粒子留下错的淘汰。第三重采样有效粒子数掉到一半以下就按权重重采避免退化。先诚实交代简化本实验假设数据关联已知路标编号直接对得上。真实 2D LiDAR SLAM 还要做扫描匹配与关联搜索那是另外两大块工程量但位姿与地图同时估计的骨架就是下面这几十行。顺带说一句粒子数它不是越大越好本质是你愿意花多少算力去买粒子云里总有一颗落在真实位姿附近这份保险——本实验用 200 个在这块场地上够用。fastslam_toy.py —— 纯 numpy 的 FastSLAM 简化版数据关联已知 200 个粒子同时估计机器人位姿 10 个路标位置走 16x11 m 矩形回路importnumpyasnp rng_trajnp.random.default_rng(11)# 轨迹/里程计专属随机源与实验一、图3 一致rng_slamnp.random.default_rng(42)# FastSLAM 专属随机源与图4 一致dt,T,N,M0.2,260,200,10cornersnp.array([[2,2],[18,2],[18,13],[2,13],[2,2]])Rnp.diag([0.15**2,np.deg2rad(1.5)**2])# 测距/测角噪声RMAX8.0# 量测半径landmarksrng_slam.uniform([3,3],[17,12],size(M,2))# 路标真实位置#---- 生成真实轨迹和带噪里程计控制与实验一同一套轨迹、同一个随机源gtnp.zeros((T,3));ctrlnp.zeros((T,2))pose,wpnp.array([2.0,2.0,0.0]),1gt[0]posefortinrange(1,T):dcorners[wp]-pose[:2]e(np.arctan2(d[1],d[0])-pose[2]np.pi)%(2*np.pi)-np.pi wfloat(np.clip(2.0*e,-1.5,1.5))v0.0ifnp.linalg.norm(d)0.4else1.2ifnp.linalg.norm(d)0.6:wpwp%41poseposenp.array([v*dt*np.cos(pose[2]w*dt/2),v*dt*np.sin(pose[2]w*dt/2),w*dt])gt[t]pose ctrl[t][vrng_traj.normal(0,0.04),wrng_traj.normal(0,0.06)]defobserve(p):# 机器人看到的路标距离 相对方位角z[]forj,(lx,ly)inenumerate(landmarks):rnp.hypot(lx-p[0],ly-p[1])ifrRMAX:b(np.arctan2(ly-p[1],lx-p[0])-p[2]np.pi)%(2*np.pi)-np.pi z.append((j,rrng_slam.normal(0,0.15),brng_slam.normal(0,np.deg2rad(1.5))))returnz#---- FastSLAM 主循环位姿粒子 x 每粒子独立路标 EKFpartsnp.tile(gt[0],(N,1))np.array([0.05,0.05,0.01])*rng_slam.standard_normal((N,3))munp.full((N,M,2),np.nan)# 每粒子每路标的均值Pnp.zeros((N,M,2,2))# 每粒子每路标的协方差logwnp.zeros(N)estnp.zeros((T,3));est[0]gt[0]defh(m,p):# 观测预测路标在机器人坐标系的位置dx,dym[0]-p[0],m[1]-p[1]returnnp.array([np.hypot(dx,dy)1e-9,np.arctan2(dy,dx)-p[2]])defHjac(m,p):# h 的雅可比dx,dym[0]-p[0],m[1]-p[1]r2dx*dxdy*dy1e-9;rnp.sqrt(r2)returnnp.array([[dx/r,dy/r],[-dy/r2,dx/r2]])fortinrange(1,T):v,wctrl[t]zsobserve(gt[t])foriinrange(N):# 1) 运动采样同一份里程计读数每个粒子加自己的噪声vs,wsvrng_slam.normal(0,0.04),wrng_slam.normal(0,0.06)pparts[i]np.array([vs*dt*np.cos(parts[i,2]ws*dt/2),vs*dt*np.sin(parts[i,2]ws*dt/2),ws*dt])p[2](p[2]np.pi)%(2*np.pi)-np.pi parts[i]pforj,r_m,b_minzs:# 数据关联已知路标编号直接对上ifnp.isnan(mu[i,j,0]):# 第一次见到用观测反推位置来初始化ap[2]b_m mu[i,j][p[0]r_m*np.cos(a),p[1]r_m*np.sin(a)]P[i,j]np.diag([0.3,0.3])else:# EKF 更新innovation 量测 - 预测HjHjac(mu[i,j],p)SHj P[i,j] Hj.TR KP[i,j] Hj.T np.linalg.inv(S)hbh(mu[i,j],p)innovnp.array([r_m-hb[0],(b_m-hb[1]np.pi)%(2*np.pi)-np.pi])mu[i,j]mu[i,j]K innov P[i,j](np.eye(2)-K Hj) P[i,j]logw[i]-0.5*(innov np.linalg.solve(S,innov)np.log(np.linalg.det(2*np.pi*S)))# 2) 权重归一化有效粒子数太少就重采样wtsnp.exp(logw-logw.max());wts/wts.sum()if1.0/np.sum(wts**2)N/2:idxrng_slam.choice(N,N,pwts)parts,mu,Pparts[idx],mu[idx],P[idx]logw[:]0.0# 3) 加权均值出估计航角必须用圆周均值直接平均会掉进 ±pi 回卷坑est[t,0],est[t,1]wts parts[:,0],wts parts[:,1]est[t,2]np.arctan2(np.sum(wts*np.sin(parts[:,2])),np.sum(wts*np.cos(parts[:,2])))pose_errnp.linalg.norm(est[-1,:2]-gt[-1,:2])lm_errnp.sqrt(np.nanmean((mu-landmarks)**2))print(终点位姿误差: %.3f m 路标 RMSE: %.3f m%(pose_err,lm_err))本地实跑输出随机源与出图脚本一致轨迹种子 11、FastSLAM 种子 42终点位姿误差: 0.139 m 路标 RMSE: 0.187 m如图 4 所示本地实验脚本随文复现同一条一圈多 7 米的轨迹、同一份带噪里程计——就是实验一里 52 秒偏出 1.38 米的那份数据——再加上 10 个路标的测距测角观测测距噪声σ_r0.15 m、测角噪声σ_θ1.5°、量测半径 8 m200 个粒子跑完全程终点位姿误差 0.139 米路标位置 RMSE 0.187 米。粒子云收成一团估计路标和真实路标几乎重叠——运动预测 路标观测一夹就收敛。给两个新手必踩的坑。其一粒子航角的加权平均必须用圆周均值arctan2(Σw·sinθ, Σw·cosθ)直接算术平均会在正负 π 的回卷处出鬼一个粒子朝 179°、一个朝 −179°平均成 0°完全错误的方向。其二EKF 的 innovation 里距离分量必须拿量测减预测值角度分量必须先绕回正负 π 再用否则滤波器会在角度回卷处抽风。这两处写错现象都是偶尔收敛、偶尔发散非常折磨人。六、Nav2从建图到走路的最小流水线原理打通了上工程。以下操作建议在 Ubuntu 22.04 ROS2 Humble 环境下完成Nav2 对 Humble 支持最好具体命令参数请以官方文档为准。一键安装三件套就是图6 底部那条命令sudoaptinstallros-humble-navigation2 ros-humble-nav2-bringup ros-humble-slam-toolbox先看 Nav2 里都有谁。如图 5 所示BT Navigator 是行为树编排的导航任务入口负责接下来该干嘛Planner Server 规划全局路径插件有 NavFn、Smac、Theta* 可换Controller Server 做局部跟踪插件有 DWB、MPPI、TEB、Regulated Pure Pursuit 可换Behavior Server 管恢复行为原地旋转 spin、后退 backup、等待 wait卡住时自救Smoother 和 Waypoint Follower 负责路径平滑与多点巡航属于可选件Costmap 2D 维护全局和局部两张代价地图由静态层、障碍层、膨胀层等叠出来map_server 提供静态地图AMCL 是自适应蒙特卡洛定位——就是实验二那套粒子滤波的工程版。如图 6 所示从零到机器人真的走起来最小流水线就五步每一步都给你一个看得见的检查点别黑盒往前推建图起 slam_toolbox用手柄或键盘遥控机器人把场地推一圈。检查点rviz2 里地图轮廓和激光扫描点应该重合走回起点后墙线没有重影说明回环闭合了。有重影回到上一节那句话——把没走好的那段再推一遍。存图用 map_saver 把地图存成map.pgm加map.yaml。检查点图片打开能看到清晰的墙线yaml 里的分辨率和原点对得上你的场地。定位map_server 加载静态地图AMCL 负责在图里找我在哪。检查点给一个粗略初始位姿后rviz2 里激光扫描和地图轮廓重合就说明 AMCL 收敛了。这一步对应实验二的粒子滤波——你已经手写过它的最小版了。导航起 Nav2 全家桶在 rviz2 里发一个目标点2D Goal Pose。检查点rviz2 里能看到全局路径Costmap 里障碍物周围有正确的膨胀区。走起来Planner 出全局路径Controller 实时跟踪避障卡住了 Behavior Server 介入自救最终以/cmd_vel发给底盘执行。检查点机器人能沿路径走、能绕开你临时扔的障碍、堵死时自动原地旋转或后退再重规划。这套流水线跑通之后你对建图—定位—导航的理解就不再是三个名词而是三段你能亲手诊断的链路。第一次跑还有两个实操建议遥控建图时放慢速度、沿场地边缘走大圈别一上来就钻桌子腿建图质量决定了后面所有步骤的上限图歪了定位和导航怎么调都白搭。七、真机四大坑与系列呼应最后把真机上最常见的四个坑摆一摆每个坑都能对应到前面某一节。里程计标定轮径不准、轮距不准、地面打滑都会把实验一里的漂移成倍放大。这是性价比最高的投资——标定做好了SLAM 的下限直接抬高。玻璃墙与长走廊LiDAR 的激光会穿过玻璃测不到墙建出来的图缺一大块导航就敢往玻璃上撞长走廊则相反四面都是平行墙几何特征退化纵向定位会漂。这两种环境要么加补充传感器要么人为放参考物。动态障碍物建图时人来人往人会被当成墙建进地图图里多出一堆不存在的墙。建图时段尽量清场运行时的动态物交给 Costmap 的障碍层实时规避别指望地图本身处理它。回环失败与定位丢失场地太大、特征太像回环候选找不到或者认错图歪了没人纠定位丢失则常发生在长时间无观测之后。对策前面都给过建图路线设计成多回老地方建歪了就回去再走一遍。收个系列的大尾。一的误区二说过VLA 模型再强“机器人要在户外或大空间里自主移动地图、定位、长程规划依然绕不开”——本篇就是给 VLA 补的那条腿。二LM-Nav 走的是无先验地图的户外路线靠 GPS 定位、靠大模型把地标串成导航指令本篇走的是有先验地图的室内路线SLAM 先建图AMCL 再定位Nav2 负责走。两条路线互补不重复室内没 GPS、但有条件先建图用本篇户外大空间没图可用去看二。八、总结三句话收束全篇。第一SLAM 的本质是建图和定位互为前提只能同时解前端保实时、后端保全局、回环清账缺一层就翻车。第二里程计误差是积分出来的账只涨难消——52 秒就能偏出 1.38 米——解药永远是外部观测FastSLAM 用 10 个路标把误差夹回 0.139 米。第三工程落地的最小闭环是 slam_toolbox 建图、存图、map_server 加 AMCL 定位、Nav2 导航每一步盯住检查点。下一篇预告二LM-Nav 拆解——没有先验地图时大模型怎么指挥机器人在户外串联地标如果你更想回地基一VLA 概念地基随时欢迎。学完这一篇别再让机器人在自己的地图里迷路了。专栏直达tag 建议#具身智能 #SLAM #ROS2 #Nav2 #FastSLAM参考Hess W., Kohler D., Rapp H., Andor D., Dellaert F. Real-Time Loop Closure in 2D LIDAR SLAM. ICRA 2016.IEEE Xplore 可下载 PDFMontemerlo M., Thrun S., Koller D., Wegbreit B. FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem. AAAI 2002.aaai.org 论文库可检索 PDFNav2 官方文档https://docs.nav2.org/navigation2 源码仓库https://github.com/ros-navigation/navigation2slam_toolboxhttps://github.com/SteveMacenski/slam_toolboxCartographer 文档https://cartographer.readthedocs.io/
