06-定位与SLAM-定位与状态估计

作者：姚博文 · 2026-09-15

定位与状态估计 — "我在哪"：坐标管理、EKF 融合与 AMCL 定位

定位：从坐标系管理（TF）到轮式里程计+IMU 的 EKF 融合，再到已知地图上的粒子滤波定位（AMCL），附 UWB/视觉定位入门——小车"我在哪"问题的完整解法 | 适用：智能小车/移动机器人开发

1\. 概述

1.1 这篇解决什么问题

自主导航里最基础的问题是 **"我在哪"**（Localization）。它有三个层次，难度和精度递增：

| 层次 | 方法 | 回答的问题 | 参考系 | 漂移 |

| --- | --- | --- | --- | --- |

| 相对定位 | 轮式里程计（Odometry） | 我比上一刻多走了多少 | 上一刻自己 | 随时间累积（典型 5%–15% 路径长度误差） |

| 融合定位 | EKF 融合里程计+IMU | 我大概在哪、朝向多少、有多大把握 | 开机原点（odom 系） | 仍会漂，但被 IMU 约束住朝向 |

| 绝对定位 | AMCL 粒子滤波（已知地图） | 我在地图的哪里 | 地图（map 系） | 基本不漂，但依赖地图与传感器 |

"状态估计"（State Estimation）是比"定位"更宽的词：定位估计的是**位置+朝向**，而真实系统还要估计**速度**（vx, vy, ω，甚至姿态 roll/pitch），合称"状态"。估计状态的工具就是**滤波器**（EKF/UKF/粒子滤波）。

💡 一句话分工：**里程计提供"怎么动的"（运动模型），IMU 约束"朝向"，地图/信标提供"绝对锚点"，滤波器把它们缝成一个可信的位姿。** 缺了绝对锚点，小车绕圈 10 分钟后就"不知道自己在哪"了。

1.2 在自主导航链路中的位置

对应 `00-路线图/学习路线图.md` 的全链路图：

关键点：**下游（Nav2、RViz、激光扫描的坐标换算）从不直接读里程计**，它们只通过 TF（Transform Framework，变换框架）查 `map → base_link` 这条变换链。所以这篇的四个主题——TF、里程计、EKF、AMCL——拼起来就是"喂给导航栈一个可信位姿"的完整工程。

1.3 本文覆盖

1. **TF 与三坐标系约定**：`map / odom / base_link` 各自是谁、谁发布、怎么查、怎么调（变换数学细节见 `01-基础概念/坐标系与变换.md` §4.2，这里只讲定位侧的用法）；

2. **EKF 定位**：预测-更新两步在小车场景怎么落地——状态向量选什么、里程计/IMU 各"管"哪些维度、Q/R 怎么调（滤波器数学推导见 `04-传感器与感知/传感器融合.md` §2.2，这里讲应用）；

3. **AMCL 粒子滤波**：已知地图上"我在哪"的概率解法——运动模型、似然场观测模型、重采样、收敛判据；

4. **UWB / 视觉定位入门**：两种"绝对锚点"方案——UWB 三边测量、AprilTag PnP，附可运行计算示例；

5. **纯 Python EKF 小实验**（标准库，直接跑）：里程计(带比例误差) + IM