《概率机器人》读书笔记——第七章_移动机器人定位-马宁-20261101

xa_lanyve

268人浏览 · 2026-11-04 18:15:00

xa_lanyve · 2026-11-04 18:15:00 发布

机器人要自主行动，首先得回答"我在哪"。定位是导航与规划的地基，而初始位姿未知、运行中被"搬走"等场景，都让问题颇为棘手。本文把运动模型与观测模型组装成马尔可夫定位框架，讲清位姿跟踪、全局定位与绑架问题的统一解法。读完你将掌握网格定位与 EKF 定位的原理与边界，为粒子滤波定位打好基础。

从两块拼图说起

前面几章我们已经准备好了两块关键拼图：第五章的运动模型 $p(x_t|x_{t-1},u_t)$ 和第六章的观测模型 $p(z_t|x_t,m)$ 。这一次我们把它们组装成第一个完整应用：马尔可夫定位（Markov Localization）。

它的做法很直接——在整个状态空间（机器人所有可能位姿）上维护置信度 $bel(xt)\mathrm{bel}(x_t)$ ，无论这个分布是单峰还是多峰。正因如此，它成了第 8 章 Monte Carlo 定位的理论母版，也为全局定位、绑架问题提供了统一的讨论框架。

阅读提示：本文建立在第二、三、四、五、六章的基础上；第 8 章的 MCL 会把这里的离散/网格化思想改造成粒子滤波形式。

定位问题的三种面孔

拿到一个定位需求，先问一句：**机器人的初始位姿，我们知道多少？**按已知程度，定位问题可以分成三类：

类型	含义	例子
位姿跟踪（position tracking）	已知初始位姿，只需跟踪小扰动	工业 AGV
全局定位（global localization）	初始位姿完全未知，需在整张地图中找回自己	开机后的服务机器人
绑架问题（kidnapped robot）	运行中突然被搬到新位置，需重新恢复	被人搬动的机器人

为什么这个划分重要？因为高斯滤波（KF/EKF）只适合位姿跟踪——它假设分布单峰、不确定性小；而马尔可夫定位天然能处理多峰的全局定位与绑架。这正是我们需要新框架的理由。

马尔可夫定位 = 贝叶斯滤波 + 定位状态

思路其实朴素：把贝叶斯滤波直接套到定位问题上，状态变量取机器人位姿 $xt=(x,y,θ)x_t=(x,y,\theta)$ ：

$dxt−1\overline{\mathrm{bel}}(x_t) = \int p(x_t\mid x_{t-1},u_t)\,\mathrm{bel}(x_{t-1})\,dx_{t-1}$

$bel‾(xt)\mathrm{bel}(x_t) = \eta\, p(z_t\mid x_t,m)\,\overline{\mathrm{bel}}(x_t)$

拆开看，每一件都是老朋友：

运动模型来自第五章；
观测模型来自第六章，含地图 $m$ ；
$bel\mathrm{bel}$ 是定义在全部可能位姿上的分布——可以是网格上的离散概率，也可以是高斯或粒子集合。

直觉：马尔可夫定位就是"对每个可能位姿都问一遍：在这个位姿下，按运动模型应该到哪？按观测模型应该看到什么？再据此更新它的概率"。

三种实现形式

理论定了，工程上怎么落地？区别只在 $bel\mathrm{bel}$ 用什么表示：

实例	$bel\mathrm{bel}$ 的表示	出处
网格马尔可夫定位	把位姿空间切成 3D 网格 $(x,y,θ)(x,y,\theta)$ ，每格存概率	本文
EKF 定位（地标）	单峰高斯 $N(μ,Σ)\mathcal{N}(\mu,\Sigma)$	本文后半部分
Monte Carlo 定位（MCL）	粒子集合	第 8 章

下面我们逐一展开前两种。

网格马尔可夫定位：最直观的实现

把位姿空间切块、每格存一个概率，算法便是贝叶斯滤波的逐格执行：

算法: Grid_Markov_localization
─────────────────────────────────────────────
1. 初始化: bel(x_0) 在所有网格上均匀分布（全局定位）
2. 每个时间步:
   a. 预测: 对每个网格 x_t:
        bel̄(x_t) = Σ_{x_{t-1}} p(x_t | x_{t-1}, u_t) bel(x_{t-1})
   b. 测量更新:
        bel(x_t) = η p(z_t | x_t, m) bel̄(x_t)
   c. 归一化
─────────────────────────────────────────────

全局定位是如何一步步收敛的

初始 $bel\mathrm{bel}$ 均匀分布 → 任何位姿等概率；
收到首次观测后，只有与地图匹配的位姿拿到高概率， $bel\mathrm{bel}$ 出现多峰；
随着运动 + 观测不断迭代，多峰逐步收敛到唯一的真实位姿。

直觉：每次观测像"用地图做筛子"，把不匹配的位姿筛掉；运动则把幸存者向前推。多峰会因后续观测/运动的差异逐渐瓦解。

EKF 定位：高效但单峰的选择

如果环境里有已知地标、初始位姿也大致已知，就没必要在整个网格上算——用 EKF（第 3 章）实现马尔可夫定位会高效得多：

状态 $xt=(x,y,θ)⊤x_t=(x,y,\theta)^\top$ ；
运动模型来自第五章（线性化得到雅可比 $G_t$ ）；
观测 $zt=(r,ϕ)z_t=(r,\phi)$ 来自第六章地标模型，雅可比 $H_t$ 对 $(x,y,θ)(x,y,\theta)$ 求导。

算法: EKF_localization(μ_{t-1},Σ_{t-1},u_t,z_t,{c_t})
─────────────────────────────────────────────
1. 预测:
     μ̄_t = g(u_t, μ_{t-1})        // 运动模型
     Σ̄_t = G_t Σ_{t-1} G_t^T + R_t
2. 对每个观测到的地标 c_t:
     ĥ_t = h(μ̄_t, m_{c_t})
     H_t = ∂h/∂x |_{μ̄_t}
     K_t = Σ̄_t H_t^T (H_t Σ̄_t H_t^T + Q_t)^{-1}
     μ̄_t ← μ̄_t + K_t (z_t - ĥ_t)
     Σ̄_t ← (I - K_t H_t) Σ̄_t
3. 返回 μ_t=μ̄_t, Σ_t=Σ̄_t
─────────────────────────────────────────────