从零构建机器人地图模块：2D激光雷达、栅格地图与粒子滤波定位实践-北京尧图网络

1. 先聊聊 lingbot-map 到底在解决什么问题1.1 机器人没有地图就像人没有方向感做机器人的朋友应该都遇到过这个场景底盘能跑、电机能转、遥控能走但一旦你下达“去厨房门口等我”这种带位置的指令它就傻在当场。原因很简单——它不知道自己在哪里也不知道厨房在哪。没有地图机器人能做的只有“见障就躲”永远只能原地打转。lingbot-map 就是我自己在维护的机器人平台“lingbot”里长出来的地图模块。它要解决的是一条完整链路用一颗便宜的 2D 激光雷达加上两个轮子的编码器让小车在室内跑上几圈后自己构建出可用的栅格地图地图建好后再通过粒子滤波定位让小车知道自己在地图里的精确位置最后交给路径规划器让它真正能从 A 点走到 B 点而不是撞墙。这个模块最适合两类人一类是刚把底盘跑通、想给机器人加“脑子”的硬件派另一类是看过一大堆算法公式但始终不明白激光数据到底怎么变成一张图像的算法派。看完这篇你可以照着代码把整条链路搭起来至少不会在“建图炸了、定位飘了、路径撞墙”这三个问题上不知所措。1.2 为什么我选了栅格地图而不是别的方案地图的表示方式有很多种拓扑地图、特征地图、栅格地图是最常见的三兄弟。拓扑地图用节点和边表示“房间—走廊—房间”的关系内存小、规划快但它丢掉了几何细节机器人很难回答“这扇门我能不能钻过去”这种问题。特征地图依赖墙面、柱子的几何特征在规则走廊里很稳定可一旦环境空旷或者特征重复匹配就容易崩。我最后选了栅格地图因为它把空间切成一个个固定大小的格子每个格子用概率表示“这里有没有东西”。它直观、好实现、也方便做路径规划更关键的是激光雷达的数据天然适合往栅格里塞。你把一条射线经过的区域标记为“空”把射线末端打到的点标记为“占”扫上几圈地图自然就出来了。栅格地图也不是没有缺点地图分辨率越高内存和计算量涨得越夸张。比如一个 20m×20m 的空间用 5cm 栅格就是 400×400 个格子换算成浮点数也才几百 KB完全能接受但如果用 1cm 栅格直接飙到 2000×2000粒子滤波器算一轮就要多等好几倍时间。所以我的默认分辨率是 5cm室内导航足够机身又不会太大。1.3 一个地图模块我是怎么拆成四块的lingbot-map 在实现时没有把“建图”和“定位”揉成一团而是拆成四块相互独立的部分数据预处理层、地图构建层、粒子滤波定位层、路径规划层。数据预处理层负责把激光雷达和里程计的原始数据收拾干净。雷达收到的不一定都是有效点有的太近、有的太远、有的是无效值这些脏数据如果直接进建图算法地图上会出现大量黑色噪点。地图构建层接收处理后的激光和里程计位姿负责维护那张概率栅格图。定位层和建图层分开很重要建图时我可以开环跑定位时必须有一张已经建好的地图再去做匹配两者混在一起出了问题很难定位到是哪个环节。路径规划层放的是 A* 和局部避障。A* 负责在全局栅格上找一条粗路径局部避障负责响应突然出现的障碍。这样的拆分方式让我可以单独给某一层换实现方法比如把 A* 换成 JPS粒子滤波换成扫描匹配其他层不用动。2. 核心细节解析这几个技术环节不能不较真2.1 坐标系统一所有数据必须在同一个世界里说话很多同学第一次建图失败不是算法不行而是坐标系乱了。雷达测出来的是“以雷达为原点、极坐标形式”的距离和角度里程计算出来的是“以底盘初始位置为原点”的位移和转角栅格地图则用自己的像素坐标系和原点。如果这三者之间的转换关系没有一个固定的约定你扫出来的一堵墙在地图上是斜的、散开的甚至一段一段错开。我的做法是统一到一个“地图坐标系”。以机器人第一次通电的位置为原点x 轴朝机器人正前方y 轴朝左地图的像素原点放在左下角。所有激光点先根据雷达在车体上的安装位置从“雷达系”转到“车体系”再由里程计给出的车体在地图系中的位姿转到“地图系”。这个转换链在代码里只是一个坐标变换函数但它决定了后面所有计算的正确性。这里有一个特别容易踩的坑雷达不一定安装在底盘正中心。很多成品小车把雷达装在车体前侧如果你忽略这个偏移就会看到同一堵墙在地图上有两条平行的影子。我自己用一个很土的办法检查外参让小车贴着长墙直行 2 米理想情况下地图上应该是一根单线墙如果出现了平行双线那个距离差就是雷达安装偏移量直接在配置里补上就行。这个土办法比任何标定板都有效。2.2 激光雷达数据预处理比你想的更影响地图质量激光原始数据并不是拿过来就能用的。廉价雷达在强光下、透明障碍物前、甚至高速旋转时都会产生坏点。最常见的两类问题一是近距离的“飞点”本来 30cm 外是墙偶尔冒出一个 2m 的读数二是边缘跳变点在两个物体交界处读数会在两个距离之间反复横跳。如果这些点直接进入建图循环栅格地图上会不断出现小概率的占据更新表现为随机分布的黑色噪点。我的预处理流程做了三件事第一把无效值、NaN、Inf 直接丢弃第二把雷达最大量程以内的点保留超过量程的点视作“没有返回不参与更新”第三对距离做一关中值滤波把明显离群的点剔除。判断离群点的方法是与相邻 5 个点的距离差异超过 20%就认为它是异常点。还有一个看似不起眼的问题机器人静止时雷达数据也会有抖动。尤其是电池电压不稳的时候一圈数据的起始角度和上一圈差那么零点几度。处理方式是在读取扫描数据时只取完整一圈的等间距角度数组不做部分扫描的融合。那些为了省时间只拼半圈数据的做法在地图边缘会产生很明显的锯齿看着难受后面做定位时匹配分数也会不稳定。2.3 占据栅格地图的更新概率的加法不是加概率栅格地图的核心是“概率更新”。每一格存的是“这格被占据的概率”一开始我们不知道就设成 0.5。雷达射线穿过的格子我们认为它是“空”的概率压低射线末端点所在的格子我们认为它是“占”的概率拉高。问题来了如果直接拿概率去乘第一次更新后是 0.4第二次 0.32很快接近 0再来一两次就饱和了反过来占据概率也一样很快就会刷到 1。那时候“概率”就失去了区分度新数据也改不动它。我用的是 log-odds 形式也就是把概率变换到对数赔率空间。一个格子的状态不是概率 p而是 L log(p / (1-p))。更新时只需要做加法L_new L_old l_free 或者 l_occ。l_free 是负数l_occ 是正数。想恢复概率看的时候再套一次 sigmoid 变换。这样做的好处非常明显新旧观测的权重是线性叠加旧的观测慢慢被新的观测“抹平”格子不会因为扫过一次就永远卡死。代码实现上其实非常简单一个 float64 的二维数组就够了。我建议你不要图省事用 uint8 存概率值log-odds 连续累加用整数表达很容易溢出。实测下来float64 就是在 20m×20m、400×400 格子上也才占 1MB 左右完全没必要省。2.4 路径规划A* 算出来的最短路径小车不一定走得出来纯 A* 算法处理的是“格子是否可通行”的问题。它会把地图里的占据格子、未知格子都标成不可通行然后找一条从起点到终点避开所有障碍的最短路径。这里有个细节A* 找到的是几何最短但不一定是运动学上最好走的路径。如果路径贴着墙根走轮式机器人一把方向盘拐不过来很容易在拐角处刮蹭。所以我在地图上方又做了一层“膨胀地图”。把每个占据格子的影响范围向外扩展一个半径半径至少等于机器人半径加上 10cm 安全余量。这样 A* 规划出来的路径天然与障碍物保持了距离全局路径看起来不那么“极限”了。但是全局路径只是在静态地图上规划的真实环境里可能刚扫完地图就有人放了一把椅子所以还需要一层局部避障。我的方案是全局规划只在收到新目标时重算局部避障每 100ms 执行一次读取最新激光数据在机器人前方生成一个速度空间选择一条不与障碍碰撞、又尽量贴合全局路径的轨迹去执行。这个思路业内叫“全局规划局部实时避障”我用它在模拟环境里测试中途乱塞纸箱也能绕过去。3. 实操过程从零把 lingbot-map 跑起来3.1 最小硬件与软件清单先看看你需要什么。底盘我用的是带双路编码器的差速小车编码器用于推算里程计这是整个定位系统的基础。激光雷达选的是 2D 三角测距原理的入门款扫描距离 6m 以上、频率 10Hz 就够了。再配一块能跑 Python 和 NumPy 的小主机实验阶段用笔记本也行。如果没有真机也没有关系。我在开发 lingbot-map 时先用模拟器跑通算法再上真机。模拟器里会生成一个包含几面墙的房间雷达扫描数据通过射线仿真产生里程计数据通过给真实位移叠加高斯噪声产生。这样可以在完全可控的条件先验证算法逻辑再上真机排硬件问题。所有调试用的代码我都是先写脚本输出地图图像和定位轨迹看一眼结果再继续调。代码本身只依赖 Python 3、NumPy 和 Matplotlib。Matplotlib 只用来把栅格地图和轨迹可视化不是算法依赖。整个地图构建模块的核心代码大概 300 行就能写清楚。这个体量很适合做嵌入式树莓派这个级别的小主机跑 10Hz 的雷达完全不吃力。3.2 地图构建核心代码Map 类与射线更新先定义栅格地图类。核心成员就三样log-odds 数组、地图原点、分辨率。数组的每个元素表示一格索引 (gy, gx) 对应世界坐标 (x, y) 的格子。import numpy as np import math class GridMap: def __init__(self, map_size20.0, resolution0.05): self.resolution resolution self.size int(map_size / resolution) self.log_odds np.full((self.size, self.size), 0.0, dtypenp.float64) self.origin (-map_size / 2, -map_size / 2) self.l_occ 0.6 self.l_free -0.4 def world_to_grid(self, x, y): gx int((x - self.origin[0]) / self.resolution) gy int((y - self.origin[1]) / self.resolution) return gx, gy def inside(self, gx, gy): return 0 gx self.size and 0 gy self.size def update_hit_ray(self, x0, y0, x1, y1): gx0, gy0 self.world_to_grid(x0, y0) gx1, gy1 self.world_to_grid(x1, y1) for gx, gy in bresenham_lines(gx0, gy0, gx1, gy1): if not self.inside(gx, gy): continue if gx gx1 and gy gy1: self.log_odds[gy, gx] self.l_occ else: self.log_odds[gy, gx] self.l_free def to_image(self): prob 1.0 / (1.0 np.exp(-self.log_odds)) return (prob * 255).astype(np.uint8)看代码你会发现更新逻辑真的很简单一条射线沿途格子都加 l_free末端格子加 l_occ。但这里有个必须注意的点激光打中的端点格子的判定。如果激光打到 5m 处的墙面你却把 5.05m 外的格子当成端点地图边缘就会向外糊一层阴影。所以我在循环里判断“当前格子是否等于端点格子”时用的是整数格坐标不是浮点世界坐标这样能保证端点落在哪个格子就更新哪个格子。这里还要说明一个被很多人忽略的问题激光射线没有打中任何障碍时端点是不可靠的。比如射线的最大量程是 6m6m 内什么都没有此时端点并不是真实的障碍物不应该做“占据”更新。处理方式是只更新射线经过的所有格子为“空”不更新端点。我在 update_hit_ray 外面再包一层逻辑只有激光距离小于最大量程时才调用完整版本超出量程时只更新射线上的空格。3.3 Bresenham 光线投射栅格地图的底层地基射线更新里最关键的函数是 bresenham_lines它负责把一条 2D 射线经过的所有整数格子列出来。可能有人会问为什么不直接在每个格子里做几何相交判断那样当然可以但计算量太大。激光雷达一圈 360 个点每点射线的长度从几格到一百多格不等如果用浮点直线方程逐格求交点一帧数据就要做上万次三角函数。Bresenham 算法只做整数加减法没有乘除没有三角函数。它的思路用一句话概括从起点到终点每次在 x 方向或 y 方向走一步判断哪个方向更贴近理想直线就走哪个方向。代码实现如下def bresenham_lines(x0, y0, x1, y1): dx abs(x1 - x0) dy -abs(y1 - y0) sx 1 if x0 x1 else -1 sy 1 if y0 y1 else -1 err dx dy while True: yield x0, y0 if x0 x1 and y0 y1: break e2 2 * err if e2 dy: err dy x0 sx if e2 dx: err dx y0 sy注意这段代码里的 dy 是负数这是标准实现不是笔误。它巧妙地用 err 的符号来决定下一次往哪个方向走。我把这段函数放在建图模块里之后一帧雷达 360 条射线完整更新 400×400 的栅格在我的测试小主机上大约只要 15 到 20ms。这个性能完全够在 10Hz 雷达的帧间隔内完成建图。有一点提醒如果你的地图分辨率调到 1cm射线经过的格子数量会暴增性能会明显下降。我建议先用 5cm 跑通确认地图质量没问题再根据实际需求调高分辨率否则很容易在生产环境被性能问题搞到怀疑人生。3.4 粒子滤波定位让机器人在地图里找到自己建图完成只是第一步。机器人要导航得能实时知道“我现在在哪”。我的定位模块用的是粒子滤波核心思想很直白在地图里撒上一把随机粒子每个粒子都代表一种“我可能在这里”的假说然后不断用里程计预测粒子的移动、用激光观测打分最后把分数低的粒子淘汰、让分数高的粒子复制自己。每个粒子就是一组坐标和姿态 (x, y, yaw)。预测阶段根据里程计给出的位移增量给每个粒子加上高斯噪声用来模拟轮子打滑和里程计漂移。观测阶段把该粒子位置下“虚拟雷达应该看到的距离”和真实雷达数据做对比越一致权重越高。这个相似度计算不需要逐格做完整的射线投射可以预先对地图计算一张“似然场”每个栅格存一个“离最近障碍物的距离”粒子的激光点打在似然场上取对应位置的数值数值越小说明这个点越接近障碍物分数越高。def measurement_score(x, y, yaw, ranges, angles, likelihood_field): score 0.0 for r, a in zip(ranges, angles): if not 0.2 r 6.0: continue beam yaw a px x r * np.cos(beam) py y r * np.sin(beam) gx, gy world_to_grid(px, py) if inside(gx, gy): score likelihood_field[gy, gx] return score这段代码看起来简单但里面有两个工程细节。一是距离太近的激光点不用来打分因为近距离点噪声大二是超出地图范围的激光点直接跳过避免墙外的点破坏分数。实际落地的粒子滤波中我每个周期会计算有效粒子数 N_eff当 N_eff 低于粒子总数的 10% 时才做重采样而不是每帧都重采样。这样可以避免粒子“早熟”防止所有粒子瞬间挤到一个错误位置后面再也拉不回来。3.5 规划参数实测哪些参数最值得调路径规划模块我直接用前面提到的 A* 加膨胀地图。最值得花时间调的参数不是 A* 本身而是膨胀半径和规划分辨率。参数数值说明栅格分辨率0.05 m建图和规划共用的地图分辨率地图尺寸20m x 20m适合单层室内粒子数量5002D 小车定位足够粒子重采样阈值N_eff 50低于总数 10% 触发重采样激光最大匹配距离6.0 m超过此距离的数据不参与打分膨胀半径0.25 m车身半径 0.15m 加 0.1m 余量A* 搜索方向8 方向允许斜向移动但检查拐角我把 5cm 和 10cm 两种解析度都实测过。10cm 解析度下A* 的节点数量少很多规划速度快但路径会更早贴近墙边。5cm 下路径质量明显更平滑代价是每次规划多花几毫秒。这个差异在单层房间里基本可以忽略所以我选了 5cm 作为统一分辨率。调规划参数时我习惯先开膨胀半径让小车在真机环境中走上几圈观察它离墙的最近距离。如果接近或小于安全距离就说明膨胀半径不够。4. 常见问题与排查技巧实录4.1 地图重影错位先查里程计再查雷达外参地图重影是建图阶段最典型的故障。表现是同一面墙在地图上被扫成两条甚至多条平行线或者整个房间的轮廓像被“揉”过一样。遇到这种情况我的排查顺序永远是先里程计再雷达外参。先让机器人走一条直线左右轮编码器反馈的位移应该一致如果地图上的直线变成了圆弧说明左右轮半径或编码器脉冲数不一致需要重新标定轮径。再让机器人原地旋转 360 度在地图上画一个圆如果圆偏扁说明轮距标定不对。除了标定还要检查里程计噪声参数。我在运动模型里加的高斯噪声标准差如果调太小粒子滤波会把错误的位姿当成“可信位置”地图照样会漂。如果里程计没问题那就是雷达外参。前面提到的“贴墙走直线看双线”方法可以快速定位雷达安装偏移。注意偏移不仅是前后左右还有角度。雷达装歪了零点几度远处墙体的重影会非常明显。这个角度误差在距离越远时放大得越厉害所以我总是检查 3m 外的墙体轮廓是否连续。4.2 地图上的黑噪点和白飞点数据预处理没有做到位地图上出现随机分布的黑色噪点常见原因是激光坏点直接参与了占据更新。那些离群飞点打在哪一格哪一格就被更新成“占据”而且 log-odds 累加很慢才消掉。我遇到黑噪点时的第一反应不是调地图参数而是回去看雷达原始数据。给一条经验把雷达数据可视化出来用 Matplotlib 画出每一帧的散点图盯着看一分钟。你很快会发现某些角度、某些距离下点云一直在抖。特别是阳光直射窗边的时候三角测距雷达经常产生周期性坏点。我在预处理里加了“距离突变过滤”后这类噪点基本消失。透明物体造成的白墙穿洞就比较麻烦。玻璃门、亚克力板这种材质雷达波束会直接穿过去地图上该有墙的地方变成空白。我的处理办法不是修改算法而是把这类区域标记为“已知动态区域”让规划器默认导航路径避开。遇到透明障碍物视觉或者超声波传感器会比激光更可靠如果项目预算允许再加一个超声波探头专门补盲区。4.3 粒子退化与早熟一种难察觉的定位失控粒子滤波最怕两件事退化与早熟。退化指粒子都集中到极少数点多样性消失机器人一旦被推一下或撞到障碍粒子就再也追不上真实位姿。早熟指粒子在错误区域聚集但因为观测噪声太大错误区域的得分和正确区域差不多系统一直以为自己定位成功实际上距离真实位置差了好几米。判断退化不能只靠眼睛盯着粒子簇。我在每次观测更新后计算有效粒子数 N_eff。N_eff 越低说明粒子越集中。集中本身不一定坏事但如果 N_eff 长期低于 50 且定位误差没有收敛就要考虑是不是测量模型给错了权重。我遇到过一种情况激光匹配时把动态行人当作静态墙结果每帧都有几条高权重射线把粒子往行人身上拉N_eff 一直很低。后来我把距离变化率过大的激光点临时剔除掉问题才解决。4.4 规划路径贴着墙甚至切角膨胀半径和拐角检查没做好A* 出来的路径贴着墙走十有八九是膨胀半径设小了。膨胀半径至少要盖住机器人底盘的最大外接圆半径。我建议在半径基础上加 4 到 5 厘米给控制误差留余量。完成膨胀后再检查一下起点和终点本身是否落在不可通行区域。常见现象是目标点设在了紧贴墙的位置膨胀后起点或终点变成了障碍A* 直接无解。还有一个容易被忽略的细节斜向移动。A* 如果允许 8 方向搜索斜向前进时会穿过两个正交障碍格子的“角”。机器人跑到那个角上身体会同时碰到两堵墙。很多初学者以为膨胀地图能解决这个问题其实膨胀只能保证格子中心远离障碍不能保证路径对角安全。我的做法是在扩展斜向邻居时额外检查相邻的两个正交格子是否都可通行如果有一个被占据就禁止斜向走。这个小检查能避免大量“切角”事故。4.5 一圈实测下来需要检查的数据文件调试 lingbot-map 的时候我有一个固定的“数据留档”习惯把每帧雷达原始数据、里程计数据、粒子滤波后的最优位姿全部存成文本文件。这样后面调参数时可以重放同一段数据来对比不同参数的效果不会因为今天的实验环境跟昨天不一样而得出错误结论。重放数据比真机测试有用得多。真机上你无法精确重复同一个动作但重放数据可以。我一般会固定跑三组数据直行 3 米、绕 180 度弯、绕一个房间走一圈。每组数据用不同的参数组合跑十遍对比最终地图与标准地图的重合率。我用的是最简单的方法把生成地图二值化之后计算与手工标注标准地图的重叠像素比例。重合率低于 95% 的参数组合直接淘汰只保留最好的几个组合上真机验证。这个习惯帮我避过了一个大坑有一次地图上出现规律性波纹我一直怀疑是激光雷达有问题后来重放数据才发现是里程计更新频率和雷达帧率不同步导致每次建图位姿都滞后了一段距离。如果当时不重放数据光靠真机反复试可能要排查好几天才能想到是时间戳对齐的问题。说实话做 lingbot-map 这个模块最磨人的并不是算法本身。那些数学看起来复杂代码写起来反而快真正烦人的是硬件噪声、时间戳对齐、参数耦合这种琐碎问题。踩过几次坑之后我最大的体会是地图模块的数据链路一定要保持简单、可重放。任何环节出问题都要能快速定位到是“数据问题”还是“算法问题”。这套地图代码现在运行在 lingbot 平台上每次实际部署时我还会重复一遍“贴墙走直线—看地图重影”的土办法它一点都不过时反而比很多高端标定工具更直观。

从零构建机器人地图模块：2D激光雷达、栅格地图与粒子滤波定位实践

相关推荐

Docker+MySQL+WordPress搭建个人博客：配置、部署与备份全指南

MapReduce核心原理与实战：从WordCount到分布式计算

富文本编辑器 rea：从技术选型到渲染性能的工程实践踩坑记录

更多资讯

嵌入式C与标准C的差异：内存、运行时与编译器行为解析

UWB定位锚点三合一实测：中继、定位、信标如何兼顾部署效率

PLC工程师实战错题本：90条现场踩坑经验与避坑指南

从零理解电机旋转电压矢量：Clarke/Park变换与SVPWM仿真实践

从零搭建步兵机器人：硬件选型、供电与通信全解析

读懂HCU控制模块说明书：状态机、标定与故障码排查

今日更新

微信云开发旅游小程序实战：LBS打卡+UGC社交全栈方案

金融AI代理平台核心拆解：从多代理协作到财报分析实战

基于大数据的智慧旅游推荐与可视化平台架构实践

本周精选

金融AI落地指南：从调研报告到技术选型与工程实践

大模型上下文成本与注意力优化技术深度解析——为何O(n²)平方复杂度无法被常规优化消除｜TaoToken 统一 Key 通道实测

基于GMM的语音识别系统原理与MATLAB实现详解

本月热门

我发现了一个新思路：用 Remotion + Claude Code 像写代码一样自动化生成短视频

Windows下 Codex 中 Chrome 和 Computer Use 插件不可用问题排查及解决参考方式：TaoToken 统一 Key 配置与验证

2026 大模型集体涨价：用 Python 做企业 Token 成本测算与选型避坑（附配置）

读完文章，想聊聊您的网站？