1. 它到底要解决什么问题想象你被蒙上眼睛在一个陌生的屋子里转悠。你小心翼翼地伸出手摸到一张矮矮的茶几又摸到一圈软软的沙发角落里还有个高高的大衣柜。突然你心里一亮“这个触感组合我刚刚摸到过我又转回来了”——这就是回环检测地点识别。机器人也是这么干的只不过它用的不是手而是激光雷达。雷达会向四面八方发射激光测量周围物体的距离和高度生成一片密密麻麻的点点云。 ScanContext 的任务就是把这堆复杂的点云变成一张简单、好比较的“记忆卡片”。每次机器人走到一个新地方就拍一张这样的卡片存起来。以后它再看到相似的“卡片”就知道自己回到了老地方这就完成了回环检测从而修正累积的定位误差。2. 怎么制作这张“记忆卡片”——从点云到矩阵我们把机器人站的位置想象成一个靶心开始画靶子。第一步切蛋糕扇形划分像切蛋糕一样把机器人周围 360° 的空间平均切成若干个扇形。比如我们切成 8 块每块覆盖 45° 的方向用来区分“东西南北”。第二步画同心圆圆环划分再像画靶子一样用不同半径的同心圆把空间从里到外分成若干个圆环。比如分成 3 环近处0 ~ 1.5 米中等1.5 ~ 3 米远处3 ~ 4.5 米这样一来“蛋糕块”和“圆环”交叉就把周围空间切成了一个个小格子比如 3 环 × 8 扇区 24 个格子。第三步给格子填数填入高度值激光雷达扫出的点云会落进这些小格子里。对于每个格子我们只记录里面最高的那个点的高度。 比如近处某个格子里有茶几的边角最高点是 0.5 米中等距离某格子里有沙发靠背最高点是 1 米远处某格子有柜子最高点是 2 米。把这些高度值按顺序填入一个矩阵表格里行代表第几个圆环距离远近列代表第几个扇区方向角度这个填满高度的矩阵就是ScanContext。你可以把它当成这个地点的“高度指纹”——它用最简练的方式记录了“某个方向上、某个距离处有多高的东西”。3. 两张“指纹”怎么比对——相似度和旋转不变性如果机器人两次都面对同一个方向那么只要把两个矩阵对应格子的高度一一比对算出整体差异就行了差异越小越相似。但问题来了机器人朝向很可能不同。这次它面朝北下次它面朝西。那么整个环境的“高度指纹”在矩阵上就相当于列发生了循环平移——原本北边的扇区下次可能跑到西边的扇区那一列去了。怎么办很简单把其中一张指纹“转”着比。我们把矩阵 B 的列循环左移或右移每移动一列就跟矩阵 A 比较一次相似度。移动一圈下来找到相似度最高的那个位置。这个最高分就代表了在某个旋转角度下两张图对得特别齐。如果这个分数超过一个阈值就认为它们来自同一个地点。 这个过程就天然实现了旋转不变性——不管你怎么转我总能找到对齐的方式。4. 太慢了怎么办——两阶段匹配粗筛 精排如果机器人存了成千上万张“指纹”每来一张新图都要跟历史上每一张做一遍全旋转对齐比较那计算量会爆炸机器人当场“卡死”。于是我们用了一个非常巧妙的两阶段加速法第一阶段Ring Key 粗筛快速海选我们给每张 ScanContext 矩阵生成一条极简的“摘要” 把每一行同一个距离环里所有扇区的高度取个平均值。 比如3 环就得到 3 个数近处平均高度中等距离平均高度远处平均高度。这个向量就叫Ring Key非常短比较起来极快。机器人走到新地方先算 Ring Key然后去数据库里快速过滤只找出 Ring Key 最相似的前几名比如前 10 名作为“候选人”。大部分明显不一样的地方在这一步就被淘汰了。第二阶段列平移精排终选验证对留下来的这少数几个候选人机器人再拿出它们的完整 ScanContext 矩阵跟新图的矩阵做细致的列平移匹配算出真正的最高相似度。 如果这个最终得分足够高就判定为“回环”否则就认为只是 Ring Key 凑巧有点像实则不是同一个地方。目的很简单粗筛保证速度精排保证准确。5. 客厅与卧室的假想例子现在我们来模拟一次完整的经历。假设机器人用的是 3 环近/中/远 × 8 扇区每 45° 一个方向的 ScanContext。我们记扇区 0 为正北顺时针编号。场景设定客厅中央半径 0.5 米的矮茶几高 0.5m一圈离茶几 2 米的沙发高 1m东北角扇区 1~2有个高柜高 2m。其他地方为空。卧室一张离中心 2 米的大床高 0.8m床头柜在近处高 0.6m远处只有墙没有高柜。首次到客厅 —— 生成矩阵 A机器人面朝北0° 方向为北。它观察到的“最高高度”如下表空白表示 0m环 \ 扇区0(北)1(东北)2(东)3(东南)4(南)5(西南)6(西)7(西北)近 (0-1.5m)0.50.50.50.50.50.50.50.5中 (1.5-3m)1.01.01.01.01.01.01.01.0远 (3-4.5m)02.02.000000矩阵 A 就是上面的数字阵列。我们顺便算出它的Ring Key每行均值近环平均0.5中环平均1.0远环平均(02200000)/8 0.5 →Ring Key_A (0.5, 1.0, 0.5)走到卧室 —— 生成矩阵 C对比用机器人溜达进卧室扫描得到完全不同的高度分布简化表示近环有些方向有床头柜0.6m中环有床0.8m远环全是 0它的 Ring Key 算出来大概是(0.3, 0.7, 0)。 机器人拿这个跟记忆里的 A 比较Ring Key 差别巨大粗筛就直接淘汰判定“没来过这儿”。回到客厅但转了方向 —— 生成矩阵 B机器人从卧室又逛回客厅中央但这次它面朝正西。也就是原来的正北扇区0现在跑到了它的正东相当于矩阵 A 的列向右循环平移了 2 格因为朝向转了 90°注意面朝西意味着北在右侧若以它自身坐标系原先北方向的环境特征会出现在扇区 2。我们直接假设列平移 2 列。矩阵 B 看起来像这样环 \ 扇区0(西)1(西北)2(北)3(东北)4(东)5(东南)6(南)7(西南)近0.50.50.50.50.50.50.50.5中1.01.01.01.01.01.01.01.0远0002.02.0000你可以看到高度值的分布规律跟 A 一模一样只是列的位置循环移动了原来在东北角的高柜现在跑到了东北偏东的扇区。 它的 Ring Key近环 0.5中环 1.0远环 (00022000)/8 0.5。Ring Key_B (0.5, 1.0, 0.5)与 A 完全相同。演示匹配过程粗筛机器人刚回到客厅生成矩阵 B 和 Ring Key_B。去数据库里查发现 Ring Key_A 与 Ring Key_B 几乎一模一样立马把 A 列为强候选人可能还有其他一两个 Ring Key 相似的也被拉进候选池。精排拿出矩阵 A 和矩阵 B开始做列平移匹配。固定 A把 B 的列挨个平移平移 0 列不对齐远环高柜对不上得分低。平移 1 列还是错位。平移 2 列时B 的扇区 2北移到了 A 的扇区 0北高柜在扇区 3/4 处也对齐瞬间匹配得分飙到 0.96。判定0.96 远超预设的阈值比如 0.85系统认定“这就是客厅” —— 一次漂亮的回环检测完成。总结ScanContext 就是把三维空间切成环和扇区用每个格子的最高高度拼成一张“高度指纹”。 比较指纹时通过循环平移列来克服机器人朝向的变化。 为了实时运行先用极简的Ring Key 粗筛再对少数候选做列平移精排。这样一来机器人就能像你凭触感认出熟悉的房间一样用激光高度信息认出自己“曾经来过的地方”。