SLAM全局重定位：机器人在陌生环境中找回自己

可视化演示

问题定义

想象你再回到那个商场：一个人站在陌生的楼层，手里只有一张地图，没有任何关于“我从哪里来”的记忆。机器人遇到的情况完全一致——它可能被人抱起来搬到别处、可能轮子打滑后里程计彻底失真、也可能传感器被遮挡太久导致定位漂移。这类瞬间丢失位姿的问题，在机器人圈里被形象地称为“绑架机器人问题”。而全局重定位，就是专门应对这种“从头找自己”的定位技术。

要理解全局重定位，先要分清它与局部定位的分工。局部定位默认“我知道上一时刻的位置”，然后靠车轮里程计、IMU 等增量推算下一步位置；一旦这个“已知上一刻位置”的前提不成立，局部定位就会一路错下去。全局重定位则不同：它不信任任何先验位置，直接在整个已有地图上搜索机器人最可能站在哪里。

从概率视角看，全局重定位求解的是这样一个分布：

它表示：在已知地图 \(m\)、到当前为止的所有观测 \(z_{1:t}\) 和控制指令 \(u_{1:t}\) 的条件下，机器人处于位姿 \(x_t\) 的概率。定位成功，就是让这个概率分布从“到处都是可能”收缩成“集中在一个点附近”。

回到商场比喻：全局重定位的目标，就是把你在地图上的位置从“可能在 A 区、B 区、C 区”的不确定分布，一步步收敛成“我就站在这个扶梯口”的确定结论。理解了这层含义，后面所有的算法都是在为这个“收敛”服务。

数学建模

把商场地图画成坐标纸，事情就清晰多了。你的位置写成 \((x, y)\)，脸朝向的角度写成 \(\theta\)，三者合起来叫位姿。你眼睛看到的“前方 8 米是扶梯”，这种感知结果叫做观测。全局重定位要解决的，是“已知观测，反推位姿”的逆问题。

逆问题不好直接解，但我们可以先建立正问题：假如机器人确实站在某个位姿 \(x_t\)，那么它“应该看到什么”是可以用模型预测的。这个从位姿到预测观测的映射，就叫观测模型。机器人学里最常用的观测模型形如：

这个公式看起来简单，却是连接世界与传感器的桥梁。\(h\) 根据地图 \(m\) 和位姿 \(x_t\) 计算出“理论上应测到的值”，\(\delta_t\) 则代表传感器的噪声——毕竟激光会跳点、相机有噪点，真实观测永远不可能和理论值完全一致。

用商场的说法：你猜“我在这家奶茶店门口”，观测模型就替你预测“那右前方应当出现扶梯口，左边 3 米应当有玻璃橱窗”。拿这个预测和真实看到的画面一比，猜得对不对立刻见分晓。于是，全局重定位从“猜位置”变成了“用观测模型验证每个猜测”。

粒子滤波原理

一个人猜不准自己在哪里，那就放出一千个分身。粒子滤波的思路正是如此：在地图上均匀撒 \(N\) 个粒子，每个粒子携带一个位姿假设，代表“我可能在这个位置”。一群分身同时行动，谁和观测对得上，谁就活下来。

分身们每走一步，先按运动模型散步——因为轮子打滑、碰撞等因素，位置更新要加上随机噪声；到达新位置后，再用观测模型给每个分身打分：预测观测与实际观测越吻合，这个分身就越值得相信。这个“分数”就是粒子的权重。权重更新的核心公式是：

其中 \(w_t^{[i]}\) 是第 \(i\) 个粒子在 \(t\) 时刻的权重；等号右侧第一项是它上一时刻的“历史可信度”，第二项是本次观测的“吻合度”。两者的乘积构成了粒子新的可信度，然后再把所有粒子权重归一化，进入下一轮“预测—更新—重采样”循环。

这就像在商场里喊一嗓子“看见扶梯的分身举手”，然后让没举手的分身逐渐散开、举手的原地繁衍更多后代。经过几轮筛选，一群分身就会挤在你真正站的位置附近——这就是粒子滤波的全过程。

特征匹配方法

粒子滤波是“广撒网、慢慢筛”的笨办法，而特征匹配则是更主动的认路法。想想你在商场里定位时，真正给力的线索往往是“那家店招牌是蓝底白字”“扶梯旁的圆柱子有凹槽”这类独特特征——特征匹配就是让机器人学会认这种“地标”，从而一步到位。

特征匹配方法首先从当前传感器数据中提取局部特征，例如视觉角点、激光反射柱、墙面线段等；再为每个特征构建描述子——一串能概括局部形状的数字向量，相当于地标的“身份证号”。接下来，把这串描述子与地图中存储的特征库逐一比对，找到对应关系，就能用多组“观测—地图”匹配对反解出位姿。位姿估计通常写成最大化总相似度的形式：

这里 \(\rho\) 是相似度函数，衡量观测特征 \(z_j\) 与地图特征在假设位姿 \(x\) 下的预测值 \(h_j(x)\) 之间有多“配”。

认出“蓝底白字”和“圆柱凹槽”之后，你在商场地图上扫一眼就能锁定自己。特征匹配的优势是速度快、直接给候选位姿；缺点是特征稀少或环境变化时容易失配。因此，实践中它经常和粒子滤波配合使用：先特征匹配得到候选区域，再用粒子滤波精细收敛。

全局定位流程

无论用哪种算法，完整的全局重定位流程都可以概括成四步曲：初始化粒子、读取观测、评估权重、判断收敛。就像在商场里寻找自己：先怀疑自己在哪、再看周围线索、逐个排除、最后确定答案。

流程的第一步是初始化：在可行区域内均匀撒粒子，保证“全地图无死角”。第二步是观测：读取激光、视觉、轮速等传感器数据。第三步是评估：用观测模型给每个粒子打分并更新权重。第四步是收敛判定——如果粒子已经聚拢成一个紧密的簇，就输出加权平均或最大权重粒子作为最终位姿。

怎么用数学判断“粒子已经聚拢”？最常用的指标是有效粒子数 \(N_{\text{eff}}\)：

当所有粒子权重均匀时，\(N_{\text{eff}}\) 接近 \(N\)，说明系统还很迷茫；当少数粒子垄断了大部分权重时，\(N_{\text{eff}}\) 会明显变小，说明已经收敛到一个小区域。

商场里几十个分身最后都挤在同一个扶梯口，你心里就有底了：“我就在这里。”收敛判定正是把这种直觉变成可以写进代码的数值条件——当 \(N_{\text{eff}}\) 小于某个阈值时，系统认定定位完成，可以输出结果。

工程实现要点

理论算法再漂亮，落成产品时还要算钱、算命。这里的“钱”是算力，“命”是可靠性。真正的全局重定位系统，往往从这几个细节里抠出性能。

第一是粒子数量选择：粒子越多，对后验分布的近似越准，但每个粒子都要做一次观测似然计算，算力消耗线性增长。地图大、传感器帧率高、算力强就多撒一些；反之则少撒一些。第二是重采样时机——过早会让粒子多样性丧失，过晚会让权重退化。工程上常用一个简单判据：

其中 \(\alpha\) 通常取 \(0.5\)，低于这个值就触发重采样，把计算资源重新集中到高权重粒子附近。

除了粒子数和重采样，工程实现还必须包含失效恢复机制：当所有粒子都严重不吻合观测时，说明机器人可能又“被绑架”了，要立即重新初始化全局重定位，而不是继续硬算。同时，多传感器融合（激光 + 视觉 + 轮速）能提供更丰富的观测信息，让权重评估更稳健。这些经验就像在商场找人的调度方案——不是盲目增加人手，而是提前定好“多久找不到就重新广播寻人”的规则。

局部定位配合

全局重定位不是每时每刻都要跑的“重活”。正常工作时，机器人更像“跟着朋友走”：知道上一个位置，轮子转多少、方向偏多少都能推算出来，用计算量很小的局部跟踪就足够了。

但系统必须时刻保持警觉：故障检测模块持续监控定位质量——协方差是否变大、观测得分是否下降、里程计与地图是否矛盾。一旦数据显示“我跟丢了”，就立即从局部跟踪切换到全局重定位。状态切换的规则可以抽象为：

其中 \(\operatorname{cov}(x_t)\) 是当前位姿估计的协方差，反映不确定性；\(\tau\) 是容忍阈值。协方差小于阈值时，说明定位置信，继续局部跟踪；反之则启动全局重定位，在地图范围内重新找回位置。

朋友没丢时，跟着走就行；朋友不见了，停下来掏地图重新定位；找回后又有底气继续走路。这种“跟—找—跟”的节律，正是 SLAM 系统稳定运行的核心。全局重定位不是取代局部定位，而是在它失效时兜底的“安全网”。

AI 应用场景

Python 演示

import numpy as np  class ParticleLocalizer:    def __init__(self, landmarks, true_pos, n=3000):        self.landmarks = np.asarray(landmarks, dtype=float)        self.true_pos = np.asarray(true_pos, dtype=float)        self.n = n        self.particles = np.random.rand(n, 2)        self.weights = np.ones(n) / n     def observe(self):        # 模拟一次观测: 真实位置到各地标的距离(带噪声)        dists = np.linalg.norm(self.landmarks - self.true_pos, axis=1)        return dists + np.random.randn(len(dists)) * 0.02     def update_weights(self, z):        # 根据观测更新权重: 越接近真实观测的粒子权重越高        for i in range(self.n):            pred = np.linalg.norm(self.landmarks - self.particles[i], axis=1)            llh = np.exp(-0.5 * ((pred - z) ** 2).sum())            self.weights[i] *= llh        total = self.weights.sum()        if total > 0:            self.weights /= total     def resample(self):        # 按权重重新抽取粒子, 并加少量随机扰动        idx = np.random.choice(self.n, size=self.n, p=self.weights)        self.particles = self.particles[idx] + np.random.randn(self.n, 2) * 0.005        self.weights = np.ones(self.n) / self.n     def estimate(self):        best = self.weights.argmax()        return self.particles[best]     def run(self, steps=30):        # 完整流程: 观测-更新-按需重采样, 返回最终估计        for _ in range(steps):            z = self.observe()            self.update_weights(z)            neff = 1.0 / np.sum(self.weights ** 2)            if neff < self.n * 0.5:                self.resample()        return self.estimate()  if __name__ == "__main__":    landmarks = [(0.2, 0.2), (0.8, 0.3), (0.4, 0.8)]    true_pos = (0.6, 0.55)    localizer = ParticleLocalizer(landmarks, true_pos, n=5000)    est = localizer.run(steps=40)    err = np.linalg.norm(est - localizer.true_pos)    print("估计位置:", est)    print("真实位置:", localizer.true_pos)    print("定位误差:", err)

1. self.particles = np.random.rand(n, 2)在地图范围内均匀撒粒子，对应“初始化”。
2. dists = np.linalg.norm(self.landmarks - self.true_pos, axis=1)模拟真实观测：机器人到每个地标的距离。
3. llh = np.exp(-0.5 * ((pred - z) ** 2).sum())计算观测似然：粒子预测与真实观测越接近，分值越高。
4. self.weights /= total归一化权重，让所有粒子权重之和等于 1。
5. neff = 1.0 / np.sum(self.weights ** 2)计算有效粒子数，判断是否退化。
6. if neff < self.n * 0.5:有效粒子数低于一半时触发重采样，防止粒子退化。
7. idx = np.random.choice(self.n, size=self.n, p=self.weights)按权重抽取新粒子集，让高分粒子繁衍更多后代。

常见误区

面试题