8. 定位算法综述（Survey）

本文从学术发展史、算法体系、工程实践、Autonomy 定位四个维度，系统综述移动机器人定位（Localization）领域，并明确 autonomy/localization 的能力边界与选型依据。

公式推导见 §3 数学原理；实现细节见 §5 架构、§6 Atlas。

8.1 概述：定位在导航栈中的位置

子问题	典型方法	Autonomy 模块	频率
全局定位（已知地图）	AMCL / ICP	AMCL（配置就绪）	5–20 Hz
视觉 SLAM	ORB-SLAM / VINS	Atlas	15–30 Hz
激光 SLAM	Cartographer / Gmapping	Cartographer	5–10 Hz
融合定位	EKF / UKF / 因子图	规划中	—

相机 ──→ Atlas (VSLAM) ──→ T_cw / 稀疏地图 ──┐
                                           ├──→ TF ──→ Planning / Control
激光 + 地图 ──→ AMCL (待集成) ──→ map→odom ─┘
激光 ──→ Cartographer ──→ 子图 + 位姿图 + /map

8.2 定位问题的形式化

8.2.1 位姿估计

机器人在世界系中的位姿 \(\mathbf{x} = (x, y, z, \phi, \theta, \psi)\) 或 2D 简化 \((x, y, \theta)\)。

静态定位（已知地图 \(\mathcal{M}\)）：

\[ \hat{\mathbf{x}}_t = \arg\max_{\mathbf{x}} p(\mathbf{x} \mid z_t, \mathcal{M}) \]

SLAM（地图未知）：

\[ (\hat{\mathbf{x}}_{1:t}, \hat{\mathcal{M}}) = \arg\max p(\mathbf{x}_{1:t}, \mathcal{M} \mid z_{1:t}, u_{1:t}) \]

8.2.2 不确定性表示

表示	方法	特点
点估计	PnP / ICP	简单，无置信度
高斯	EKF / UKF	单峰，计算快
粒子集	AMCL	多峰，可全局定位
图优化	g2o / GTSAM	批量最优，可回环

8.3 算法分类

8.3.1 滤波方法

扩展卡尔曼滤波（EKF-SLAM）

\[ \mathbf{x}_t = f(\mathbf{x}_{t-1}, u_t) + \mathbf{w}, \quad \mathbf{z}_t = h(\mathbf{x}_t) + \mathbf{v} \]

线性化后递推均值与协方差。\(O(n^2)\) 路标关联，大规模地图受限。

粒子滤波（MCL / AMCL）

\[ p(\mathbf{x}_t \mid z_{1:t}) \approx \sum_m w_t^{(m)} \delta(\mathbf{x}_t - \mathbf{x}_t^{(m)}) \]

可表示多模态后验（全局定位）
KLD 自适应粒子数
Autonomy 配置：config/localization/amcl/amcl.lua

8.3.2 视觉方法

特征点 SLAM（Atlas 所属）

阶段	方法	Atlas 实现
前端	ORB + 匹配	`feature/orb_extractor`
初始化	5pt/8pt + 三角化	`solve/essential_solver`
跟踪	PnP + BA	`tracking_module`
建图	LBA	`mapping_module`
回环	BoW + Sim3 + GBA	`global_optimization_module`

直接法 / 半直接法

最小化光度误差，无需描述子。代表：DSO、SVO。Atlas 未采用，因 ORB 在纹理变化下更鲁棒且便于回环。

视觉-惯性（VI-SLAM）

预积分 IMU 约束 + 视觉重投影。代表：VINS-Mono、ORB-SLAM3 VI。Atlas 当前未融合 IMU。

8.3.3 激光方法

扫描匹配（Scan Matching）

\[ \hat{T} = \arg\min_T \sum_i \| T \mathbf{p}_i - \mathcal{M}_{\text{nearest}}(T \mathbf{p}_i) \|^2 \]

Ceres / GICP 求解。Cartographer 2D 前端核心。

子图 SLAM（Cartographer）

局部 scan-to-submap 匹配
位姿图优化（pose_graph.lua）
纯定位模式：pure_localization_trimmer

Autonomy 配置：config/localization/cartographer/

8.3.4 图优化方法

统一目标：

\[ \mathbf{x}^* = \arg\min \sum_k \| \mathbf{r}_k(\mathbf{x}) \|_{\mathbf{\Omega}_k}^2 \]

因子类型	约束	Atlas
重投影	2D ↔ 3D	g2o reproj edge
里程计	相对位姿	spanning tree
回环	Sim3	loop_detector
IMU	预积分	未实现
GPS	绝对位姿	未实现

8.4 传感器与算法匹配

传感器组合	推荐算法	Autonomy 状态
单目相机	特征 SLAM	Atlas ✓
双目相机	特征 SLAM / VI	Atlas ✓
RGB-D	特征 SLAM / ICP	Atlas ✓
2D 激光 + 地图	AMCL	配置 ✓
2D 激光 SLAM	Cartographer	已实现
3D 激光	Cartographer 3D	配置 ✓，3D 前端可选
相机 + IMU	VI-SLAM	待扩展
GPS + 激光	融合因子图	待扩展

8.5 经典系统对比

系统	传感器	后端	回环	许可证
Atlas	单/双/RGB-D	g2o	BoW	Apache 2.0
ORB-SLAM3	+ IMU	g2o	DBoW2	GPL-3.0
stella_vslam	单/双/RGB-D	g2o	BoW	MIT
nav2_amcl	2D 激光	粒子滤波	—	Apache 2.0
Cartographer	2D/3D 激光	位姿图	子图	Apache 2.0
LIO-SAM	激光+IMU	GTSAM	—	BSD
VINS-Mono	单目+IMU	Ceres	4-DOF	GPL-3.0

Autonomy 选择 Atlas lineage 的原因：Apache 2.0 许可、成熟 ORB 管线、g2o 生态、与 stella_vslam 架构兼容。

8.6 精度与漂移

8.6.1 误差来源

来源	视觉 SLAM	激光 AMCL
传感器噪声	特征匹配、标定	激光角分辨率
算法近似	线性化、RANSAC	粒子数不足
累积漂移	无回环时显著	里程计累积
环境	纹理弱、动态物体	对称走廊

8.6.2 漂移量级（典型）

方法	无回环	有回环
单目 VSLAM	1–5% 路径长度	< 0.1%
双目/RGB-D	0.5–2%	< 0.05%
AMCL（已知地图）	无累积（有界）	—
Cartographer	0.1–1%	子图回环

8.7 工程选型决策树

需要 3D / 视觉？
├── 是 → 有 IMU？
│       ├── 是 → [待扩展] VI-SLAM
│       └── 否 → Atlas (单/双/RGB-D)
└── 否 → 有先验 2D 地图？
        ├── 是 → AMCL (激光 + 里程计)
        └── 否 → Cartographer (激光 SLAM)

8.8 Autonomy 定位能力矩阵

能力	Atlas	AMCL	Cartographer
代码实现	✓	配置	✓
实时跟踪	✓	预期 ✓	✓
建图	✓ 稀疏 3D	✗	✓ 2D 子图 + OccupancyGrid
回环	✓ BoW	✗	✓ 位姿图
纯定位	✓ load map	预期 ✓	✓ load .pbstream
TF 输出	需桥接	预期原生	✓ map→odom
与 Map 集成	导出地图	消费 /map	发布 /map

8.9 发展趋势

方向	说明	Autonomy 路线
多传感器融合	视觉+激光+IMU 因子图	统一 LocalizationInterface
语义 SLAM	物体级路标	Atlas marker 已有基础
终身 SLAM	地图更新与遗忘	local_map_cleaner
学习特征	SuperPoint / NetVLAD	可替换 ORB 前端
云定位	全局地图匹配	远期

8.10 参考文献

8.11 延伸阅读

主题	Autonomy 文档
Atlas 实现	atlas/guide.md
数学公式	03_math.md
Cartographer 实现	cartographer/guide.md
AMCL 配置	07_amcl.md
架构设计	05_architecture.md
地图模块	../07_Map/05_survey.md
路径规划	../08_Planning/06_survey.md