机器人地图模块重构：SLAM、代价地图与路径规划的工程实践

我最近把团队项目里的一个地图与定位模块单独抽出来重写代号就叫 lingbot-map。起因很简单机器人主程序里塞的地图代码越来越多建图、定位、路径规划、避障全混在一起改一个参数就要重新编译整个主程序出了问题也不知道该查哪一层。抽出来之后整个流程反而顺了很多这篇就把我在这个模块上的设计取舍、实现细节、踩坑记录和实测数据一起梳理出来给正在做机器人地图相关模块的同学一个参考。lingbot-map 不是一个完整机器人系统而是一个专注于地图这一层的小型基础模块。它负责传感器数据接入、地图构建与维护、位姿估计、代价地图更新以及对外提供全局路径规划所需的地图接口。主程序通过调用这个模块的地图查询接口完成导航任务两者完全解耦。如果你也在做室内巡检机器人、自动引导车或者想在低算力平台上自己搭一套地图定位子系统这篇文章应该能帮上忙。1. lingbot-map 要解决的核心问题地图不再是谁都能改的公共变量1.1 单体架构下地图模块的混乱状态最早我们做某室内巡检机器人项目时地图相关代码是直接写在主程序里的。主循环里先接收激光雷达数据再调用一个建图线程更新栅格地图接着另一个线程读取地图做定位最后还有一个线程负责路径搜索。听起来像是分层了实际上所有线程共享同一份全局地图变量任何一个线程改了一个像素值其他线程都得跟着遭殃。有一次排查定位漂移问题最后发现是路径规划线程为了临时避障把全局地图上的一块障碍物区域直接清掉了建图线程又把同一块区域重新标为障碍两个线程互相覆盖定位结果自然不稳定。这类问题很难通过加锁解决因为加锁只是保证同一时刻只有一个线程在改地图但改的语义对不对加锁根本管不了。所以 lingbot-map 的设计目标很明确地图数据只能由模块内部统一管理外部只能通过查询接口读取不能直接拿到地图内存地址去改。主程序想改地图状态必须调用模块提供的接口比如更新动态障碍物或标记任务禁区。1.2 lingbot-map 对外暴露的接口边界抽成独立模块之后我特意把接口收得比较窄。核心就几个注册传感器回调接收激光雷达点云和里程计数据请求当前全局栅格地图或局部代价地图的只读视图请求某个坐标系下的位姿估计请求从 A 点到 B 点的全局路径点序列接收任务层下发的动态地图修改指令。这里有一个设计原则值得分享宁可让接口少也不要把内部细节漏出去。比如外部不能直接设置地图分辨率因为全局分辨率在建图开始之前已经定好运行时改分辨率会让历史地图全部作废。把这些决策关进模块内部主程序那边反而少了犯错的机会。调用方拿到的地图视图我用了版本号机制来管理。每次地图发生合法更新版本号加一外部读取时带上上次版本号模块只返回变化的部分。这样一方面减少数据传输量另一方面也避免了外部缓存的地图长期不刷新。最初版本没有这个机制外部拿到的栅格地图永远是几秒钟前的旧状态机器人已经绕过一个障碍物了任务层还在按旧路径指挥后来补上版本号才解决。2. 地图数据结构选型为什么用了栅格代价拓扑三层2.1 底层栅格地图负责物理占用而不是直接上三维点云很多人一听到建图就想上三维点云地图实际上对室内轮式机器人来说二维栅格地图在大多数场景下更稳、更省事。我们在 lingbot-map 里底层用的就是二维栅格地图每个栅格记录的是该位置被物理占用的概率。激光雷达把每一帧点云投影到栅格平面命中一个栅格就增加该栅格的占用概率激光穿过的栅格则降低占用概率。选择二维栅格而不是三维点云的原因有三个。第一轮式机器人基本在平面上运动垂直方向的信息对路径规划贡献不大第二三百平方米的厂房二维栅格地图在 5 厘米分辨率下只需要几兆字节内存三维点云地图随便就是数百兆第三后续做路径规划时从二维栅格提取可通行区域极其直接从三维点云里反而要先做地面分割和投影凭空多出不少算力开销。当然如果有机械臂需要抓取货架高层物体那还得额外配三维局部地图但那是另一个模块的事lingbot-map 守住地面导航这一层就够了。2.2 局部代价地图负责动态变化和全局地图分开存全局栅格地图反映的是静态环境比如墙壁、柱子、固定货架。但这些地方经常会被临时占用叉车开过来、工人蹲在地上捡零件、托盘被换了个位置。如果把动态物体直接写进全局地图几分钟后物体离开地图里残留的障碍还会持续影响路径规划。lingbot-map 的做法是把全局栅格地图和局部代价地图拆成两张图。全局图一个线程负责更新刷新频率大约 1 到 2 赫兹局部代价地图以机器人为中心范围大约是 6 米乘 6 米每 200 到 500 毫秒刷新一次。局部代价地图的初始值来自全局地图的裁剪再叠加激光雷达实时检测到的障碍物膨胀区域。这样设计之后全局路径规划看到一个时延较高但稳定的环境局部路径规划看到的是实时变化的近场环境。两层路径规划器配合既避免了全局路径频繁重规划也不会撞上突然出现的人或车。2.3 拓扑地图只是任务层的索引第三层拓扑地图或者说语义地图本质上是一张稀疏的节点图。我们在仓库里把通道交叉口、货架入口、充电桩位置抽象成节点节点之间用可通行的边连接。任务层决定让机器人去 3 号货架补货时先查拓扑地图找到最近的入口节点再沿着拓扑边走到目标节点附近最后用底层栅格地图做精确的局部导航。这一层在 lingbot-map 里其实可以随时替换。有的场景用不到语义任务直接给一个目标点坐标也行。但加上拓扑层之后任务调度变得非常直观而且能在没有激光雷达大范围扫描的情况下让机器人快速构建一套大概怎么走的路线再由局部规划去细化和避障。3. 建图与定位的实际落地跑通 SLAM 只是万里长征第一步3.1 建图阶段最容易出现的畸变问题很多新手第一次跑建图看到地图慢慢变出来会很兴奋但没多久就发现图越来越歪。尤其是推小车的人走得快了拐弯的时候突然转 90 度地图上的墙直接糊成一片。这个问题的根源在于建图时每一帧激光数据都要和已有地图做匹配匹配结果的准确性高度依赖当前帧和上一帧之间的位姿估计误差。如果只有激光雷达和一个精度很一般的轮式里程计快速转动雷达时里程计给出的角度变化往往滞后于实际转角激光匹配就会基于一个错误的初始猜测去找最优解找到的往往是局部最优。我在 lingbot-map 里的处理方式比较务实建图时把速度限制住角速度不超过每秒 0.4 弧度线速度不超过每秒 0.5 米。同时在代码里加了一个质量监控连续几帧匹配得分低于阈值时暂停更新地图并提示操作者把小车退回上一段路径。这里有一个容易被忽略的细节激光雷达本身需要加热或旋转稳定之后再开始建图。我们用的是一款 360 度二维雷达刚上电的前 30 秒数据会有小幅偏移直接用会导致初始地图边界模糊。后来在启动脚本里加了一个预热流程雷达数据流稳定后模块才开始消费。3.2 回环检测与全局位姿修正建图跑一圈回到起点时如果地图闭合得严丝合缝说明整个路径的累计误差很小如果起点区域和终点区域错开了几十厘米就说明中间某一段累积了较大的漂移。lingbot-map 里面加了一个相对轻量的回环检测模块核心思路是当机器人运动到某个位置附近把当前激光帧和该位置的历史关键帧做一次匹配如果匹配得分足够高就认为检测到了回环然后对全局位姿图做一次优化把各关键帧的位姿拉回一致位置。这个策略在厂房环境里表现还不错因为货架和柱子的几何特征比较明显不容易出现误匹配。但如果是白白一片的大平层或者只有几根柱子的大厅回环检测很容易匹配到错误位置。后来我加了一个限制回环检测结果必须和里程计推算位置在一定距离范围内超过这个范围就认为不可信宁可不闭环也不能闭错环。有一次测试回环修正把地图拉正了但代价是之前一段路径上的障碍物坐标全部平移了几厘米。这个误差对路径规划来说可以接受因为机器人本身宽度和障碍物膨胀半径都远大于这个量级。3.3 长走廊与特征稀疏区域的定位退化定位退化是模组上线之后最棘手的问题之一。在一条二十几米长、两侧墙面平整的走廊里激光雷达沿着走廊方向的观测几乎没有任何区别。雷达看到前方是墙侧边也是墙机器人往前挪半米激光帧几乎不变。这时候如果只靠激光匹配位置估计会在走廊方向自由漂移表现为机器人在地图上原地踏步但位姿在缓慢前移或后退。我在 lingbot-map 里应对这个问题主要靠三条腿走路第一轮式里程计作为短期预测主力给它一个较高权重第二IMU 提供角度稳定参考保障行走直线时不转向第三如果检测到走廊方向特征严重不足就主动切换到沿走廊方向的运动模型避免位置拟合发散。前一阵我们在某仓储场地测试有一段 12 米的通道定位误差在走廊方向最终控制在 0.15 米以内这在只靠二维激光和里程计的条件下已经算不错了。如果你后续想进一步提升可以考虑加一个单线激光朝天花板的方案通过天花板灯孔和横梁做垂直方向的辅助定位。3.4 多传感器融合的时序对齐问题多传感器融合听起来高级实际做起来最麻烦的不是算法而是时间戳对齐。轮式里程计通常以 20 到 50 赫兹更新激光雷达以 10 到 15 赫兹更新IMU 以 200 到 400 赫兹更新。每帧数据到达模块的时间点不一样如果直接用到达顺序融合稍微有点延迟的传感器就会引入相位误差。我做过一个很小的实验把轮式里程计的更新人为延迟 50 毫秒结果同样是走 10 米直线融合位姿角度偏移比没有延迟时多了将近 0.6 度。这个误差一开始不明显但导航走走停停一整天最后机器人到点位的朝向会偏得很厉害。所以 lingbot-map 内部专门维护了一个小型的传感器数据缓冲队列每进来一帧数据先按时间戳插入队列融合器每次取同一时间窗内的数据做组合。综合考虑计算负担和效果时间窗宽度我设成了 80 毫秒。这个值不是拍脑袋定的我在三组不同数据集上对比过 40、80、150 毫秒的表现80 毫秒在多数场景里精度和实时性平衡得最好。4. 路径规划与避障全局规划负责大致方向局部规划负责别撞上4.1 全局路径规划的目标不是最短而是最安全很多新手把路径规划想成走迷宫最短路径用搜索算法找一条总长度最短的路线。但机器人不是质点它有宽度而且最短路往往贴着障碍物走稍微有一点定位误差就会蹭到货架或墙面。lingbot-map 的做法是先对全局栅格地图做一次障碍物膨胀处理膨胀半径至少等于机器人半径加安全余量。路径搜索是在膨胀后的地图上进行的这样规划出来的路径天然离障碍物有一段距离。膨胀半径我给了两个可配置参数基础安全半径以及遇到速度和载重变化时的动态调整量。搜索算法用的是经典 A*但做了两点改进。第一启发函数不用纯欧氏距离而是用曼哈顿距离和欧氏距离中较大的值这样可以减少搜索扩展节点的数量第二如果机器人当前速度较快还会在相邻路径点之间做转向平滑代价判断避免规划出连续急转弯的路线。这里有一个实际问题全局规划频率不能太高。我设了最高 1 赫兹重规划普通情况下只有当目标点变化、地图大范围更新或者全局路径被阻塞时才触发重规划。原因很简单如果全局路径每秒重算一次机器人还没走出半米路径就变了后面局部规划也会跟着震荡整体运动看起来很犹豫。4.2 局部规划器的实时避障与速度输出局部规划部分我用的是动态窗口法这一类思路核心思想是在每个控制周期内根据当前速度生成一组未来短时间内可能达到的速度组合然后对每个速度组合模拟一条圆弧轨迹再根据代价函数选出最优轨迹。代价函数里除了碰撞代价和目标方向代价还加了一项离全局路径距离的代价。如果没有这一项局部规划器在遇到临时障碍物时会为了绕障走出一个很大的弧线绕完之后很难回到全局路径上来。加了距离代价后局部轨迹会尽量贴近全局路径绕障结束自然就并线回来了。速度输出方面lingbot-map 给底盘控制器的指令不是目标点坐标而是线速度和角速度。这样底层的电机控制和顶层的路径规划完全解耦换底盘硬件时只需要改少数适配代码。在实测中觉得动态窗口法的一个参数对灵活性影响特别大就是最大线加速度。调得太小机器人面对突然出现的障碍物会减速过快容易停在障碍物前调得太大机器人启停冲击明显带动载货托盘滑动。我们在标准物体搬运场景下调到 0.4 米每平方秒能平衡舒适性和响应性。这个参数建议你对自己的底盘进行现场标定不要照搬别人的值。4.3 调参实例减速带、货架缝隙与动态人流分享一个我们真实遇到过的调参案例。某仓储场地的通道中央有一条减速带高约 1.5 厘米机器人底盘离地间隙只比减速带高一点点每次通过时轮胎会短时间打滑轮式里程计会产生一个突跳误差。最初 lingbot-map 的局部规划把减速带视为小障碍物机器人每次走到附近就急停等一秒钟又重新启动反复几次才能通过。后来我把减速带区域在全局地图中标记为允许通过但需要降速的特殊区域局部规划器在这个区域内的最大速度被限制在每秒 0.25 米并且增加了轨迹平滑权重。这样机器人经过减速带时平稳很多里程计的突跳也减轻了。另一个问题是货架缝隙。在仓库里两排货架之间经常有一条不到一个机器人宽度的缝隙全局地图上看起来是通的但机器人实际开进去很容易卡住。我在 lingbot-map 里加了一条规则全局路径只允许通过宽度大于机器人宽度 1.3 倍的缝隙不够宽的直接视为不可通行。这个倍率来自我们对定位误差和控制误差的综合估计低于 1.2 倍成功率明显下降。动态人流是最难处理的。我们的机器人会主动避让人但人移动方向随机局部规划器需要频繁更新代价地图。经过一段时间测试发现把激光雷达检测障碍物的最大距离限制在 8 米内可以显著减少误报因为远处的人或货物反射点太多容易让局部代价地图出现虚胖障碍物。近距离 6 米内的数据更新则保持最高优先级。5. 低算力平台上的性能调优从卡顿到流畅的完整记录5.1 先搞清楚算力到底花在哪儿了lingbot-map 最早跑在一块 ARM 架构的开发板上四核处理器内存只有 4GB跑完整建图定位导航流程后系统负载经常接近满载。直接优化之前我先把模块各个线程的 CPU 占用拉了出来结果出乎意料最吃 CPU 的不是地图构建而是激光点云的预处理和栅格地图更新的循环。激光雷达一圈数据大概有 900 到 1000 个点如果每帧都完整遍历一遍整张地图做射线更新CPU 占用率很夸张。当时全局地图是 500 乘 500 的栅格每来一帧雷达数据都要更新几百个栅格再加上多个传感器线程的锁竞争整体延迟就上去了。于是我把地图更新逻辑改成了按需更新只有当机器人在某个区域内运动并且局部地图块确实需要重新计算时才更新对应区域的栅格。静态区域的地图保持不变动态区域单独走快速更新通道。这个改动上线后前端雷达数据处理的 CPU 占用直接下降了大约 40%。5.2 降采样、缓存与合理的数据搬运我后来做了一项很简单的降采样优化让雷达点云在进入地图模块之前先按角分辨率抽稀。原来一圈 1000 个点抽稀到 720 个点地图质量几乎没有下降但点云匹配和地图更新耗时明显缩短。这里不要贪心抽稀太狠会导致几何特征丢失在特征稀疏环境下更容易匹配失败。内存抖动方面我发现地图更新模块频繁申请和释放小对象时间一长内存碎片越来越多。后来我把关键数据结构做了预分配在模块启动阶段就按地图尺寸一次性分好内存运行过程中不再重新申请。这个改动解决了长时间运行后偶尔出现的延迟尖峰问题。还有一个容易被忽略的坑是传感器数据拷贝。机器人中间件传输点云数据时如果每一层处理逻辑都拷贝一份完整点云内存带宽会被白白浪费。lingbot-map 里统一用只读引用传递点云谁需要消费谁就加一个引用计数只有真的需要修改数据的环节才做拷贝。5.3 长期运行的累积误差与地图老化策略机器人每天在同一片区域跑十几个小时地图里会积累一些动态物体的残留痕迹。比如某个位置曾经放过一个箱子后来移走了但由于全局地图的历史更新权重那个栅格的占用概率仍然偏高。我给 lingbot-map 设计了一个地图老化机制。局部代价地图里的障碍物如果连续一段时间内没有被激光再次确认就会逐渐降低占用概率最终消失。全局地图则保留了静态环境和半静态环境的区分墙壁柱子这类实心障碍不受老化影响货架托盘这类可移动障碍物按一个较慢的老化速度衰减。这个机制上线后再也没出现过之前的托盘位置挡住今晚上路线的尴尬局面。不过也有副作用如果机器人频繁经过某个比较窄的通道激光反射角度导致某些边缘栅格被错误标注为障碍物老化机制又会把这些真实障碍物慢慢洗掉。后来我引入了确认次数阈值低于阈值的占用标记老化很快高于阈值的则必须由更强的反证才能清除。6. lingbot-map 在真实场景中的表现与几个易错点6.1 某仓储场地的实际测试结果我们在一个大约 3000 平方米的仓储场地做了连续五天的测试场地内有高货架、低货架、通道、充电桩和偶尔穿梭的人工叉车。最终统计结果如下建图阶段全场走一遍地图闭合误差在 0.12 米以内正常导航任务中机器人到达目标点的横向误差中位数小于 0.08 米朝向误差中位数小于 2 度全流程 CPU 占用稳定在 45% 到 60% 之间低算力 ARM 平台动态障碍物从出现到局部规划器重新规划出新路径的平均响应时间约 0.4 秒连续运行 8 小时无内存泄漏位姿漂移在可接受范围内。这些数据在同类方案里不算极限但对我们的业务场景已经够用。值得说明的是这些结果高度依赖场地特征。如果换到一个墙面特别光滑、货架反射率低的环境激光匹配得分会明显下降误差数字也会变大。6.2 新手接入 lingbot-map 时最容易踩的三个坑第一个坑是坐标系统一没做好。经常有同事来问为什么地图看起来对了但机器人定位始终偏一个固定角度最终查到是雷达坐标系和底盘坐标系之间的安装角没有校准差了 2 度。雷达安装不可能绝对对齐一定要做一次外参标定把安装偏差 T 到初始化配置里。我们后来做了一个简单的标定流程让机器人原地旋转 360 度利用激光帧的匹配误差反推安装角误差自动化完成标定。第二个坑是帧率不匹配导致的地图撕裂。如果雷达驱动线程以 10 赫兹发布数据但建图线程处理不过来后面的帧会被丢弃或者排队导致实际输入建图算法的帧率不均匀。我在代码里加了一个数据源健康度监测实测输入帧率和理论帧率相差超过 15% 就输出告警提示用户检查驱动配置或计算负载。很多莫名其妙地图变差的问题其实都是帧率掉下去了导致定位更新跟不上。第三个坑是地图保存和加载时的分辨率不一致。有人在一台设备上建图时用了 5 厘米分辨率拿到另一台环境时加载地图却用了 10 厘米分辨率结果机器人看到的世界被压缩了一半路径规划全部出错。lingbot-map 在加载地图时会校验地图元信息里的分辨率、坐标原点和尺寸字段不一致直接拒绝加载并给出提示。6.3 后续升级方向从单机地图到多机共享与任务结合lingbot-map 目前的定位还是单机地图服务。但实际使用中我已经看到了两个值得做的方向。第一个方向是多机地图共享多台机器人共同工作在同一个仓库如果它们能共享同一张全局地图并把各自发现的动态障碍物上传到共享代价层整体的避障效率会高很多。这需要把代价地图的数据结构从内存扩展到网络要考虑数据同步延迟和冲突合并策略。第二个方向是地图与任务调度的更深度结合。现在任务层只告诉 lingbot-map从这里到那里地图模块返回一条路径。实际上任务调度还需要知道这个区域当前有没有人这排货架是否已经满了这类语义信息。把这些语义属性挂到地图的拓扑层上地图就不再只是一个几何工具而会变成整个机器人系统的信息中枢。这两个方向也是 lingbot-map 后续迭代的主要计划。不过从我这几年的经验来看越往后做越会发现地图模块的价值不在于算法多先进而在于它能稳定地、干净地提供一个环境表示让所有上层模块都信任它。把这个基础打牢了上面的应用自然做得又快又稳。