4、位姿图优化：图优化理论、顶点与边、信息矩阵、非线性最小二乘求解
位姿图优化，说白了就是解决一个「大家都不太准，怎么一起调准」的问题。我在做机器人定位时，最头疼的就是长时间运行后，轨迹像喝醉了酒一样越飘越远。这时候，位姿图优化就是那个能把轨迹拉回来的「定海神针」。
4.1 图优化理论：把问题画出来
先说说图优化到底是个啥。你想想看，我们有一堆位姿（机器人不同时刻的位置和朝向），还有一堆约束（比如回环检测说「你5分钟前到过这里」）。这些约束之间可能有矛盾——回环说你应该在A点，但里程计说你明明在B点。怎么办？
图优化的思路很直观：把每个位姿看作一个顶点，把位姿之间的约束看作边。整个问题就变成了一个图。然后我们调整所有顶点的位置，让所有边的误差总和最小。
核心思想：图优化 = 用图结构表达优化问题，用最小二乘法求解最优位姿。
我个人习惯把图优化理解成「弹簧系统」。每个约束就是一根弹簧，弹簧的松紧程度由信息矩阵决定。我们调整顶点位置，让整个弹簧系统达到平衡状态。嗯，这个比喻在项目中帮不少新人快速理解了概念。
4.2 顶点与边：构建你的图
在SLAM里，顶点通常就是机器人的位姿。一个位姿包含位置(x, y, z)和朝向(roll, pitch, yaw)，或者用四元数表示。边则代表两个位姿之间的相对变换关系。
边有两种主要类型：
- 顺序边：相邻位姿之间的约束，来自里程计或IMU。这些边构成轨迹的主干。
- 回环边：非相邻位姿之间的约束，来自回环检测。这些边是修正漂移的关键。
我的经验：回环边的数量不用太多，但质量一定要高。我曾经在项目中加了十几个弱回环，结果优化后轨迹反而更差了。后来只保留置信度高的回环，效果立竿见影。
每条边都带有一个观测值，也就是我们「认为」两个位姿之间应该有的相对变换。比如回环检测告诉你，当前位姿和5分钟前的位姿之间，相对变换应该是T。这个T就是边的观测值。
4.3 信息矩阵：给每条边「称重」
信息矩阵，说白了就是告诉优化器「这条边有多可信」。它是协方差矩阵的逆矩阵。
为什么需要信息矩阵？因为不是所有约束都同样可靠。比如：
- 里程计短距离约束：比较可靠，信息矩阵大
- 里程计长距离约束：漂移累积，不太可靠，信息矩阵小
- 回环检测约束：如果匹配质量高，信息矩阵可以设大一些
信息矩阵的维度取决于位姿的自由度。对于3D SLAM，通常是6×6的矩阵。对角线元素越大，表示对应方向上的约束越强。
关键点：信息矩阵决定了优化结果的「信任分配」。设置不合理，优化结果就会偏向不可靠的约束。
我记得有一次调试，轨迹优化后反而更差了。排查了半天，发现是信息矩阵设置反了——我把不可靠的回环设了高权重，可靠的里程计反而权重低。嗯，这种低级错误，犯过一次就再也不会忘了。
4.4 非线性最小二乘求解：让误差最小化
图优化最终归结为一个数学问题：最小化所有边误差的加权平方和。
数学形式是这样的：
min F(x) = Σ e_ij^T * Ω_ij * e_ij
其中：
- x 是所有位姿组成的向量
- e_ij 是边ij的误差向量（观测值 - 预测值）
- Ω_ij 是边ij的信息矩阵这个优化问题是非线性的，因为位姿变换涉及旋转，而旋转是非线性的。常用的求解方法是高斯-牛顿法或LM（列文伯格-马夸尔特）法。
求解步骤大致如下：
- 线性化：在当前估计值附近，对误差函数做一阶泰勒展开
- 构建正规方程：计算雅可比矩阵J，构建Hessian矩阵近似 H ≈ J^T * Ω * J
- 求解增量：解线性方程 H * Δx = -b，得到位姿修正量
- 更新：用Δx更新所有位姿
- 迭代：重复1-4步，直到收敛
避坑指南：我曾经在求解时忽略了Hessian矩阵的稀疏性，直接用稠密矩阵求解。结果一个只有几百个节点的图，优化一次要好几秒。后来改用稀疏求解器（比如SuiteSparse），速度提升了两个数量级。
这里给一个简化的代码示例，展示核心逻辑：
// 伪代码：位姿图优化核心流程
void PoseGraphOptimization::optimize() {
for (int iter = 0; iter < max_iterations; ++iter) {
// 1. 构建Hessian矩阵和b向量
MatrixXd H = MatrixXd::Zero(dim, dim);
VectorXd b = VectorXd::Zero(dim);
// 2. 遍历所有边，累加贡献
for (auto& edge : edges) {
// 计算误差和雅可比
VectorXd e = edge.computeError();
MatrixXd J = edge.computeJacobian();
// 累加到H和b
H += J.transpose() * edge.infoMatrix * J;
b += J.transpose() * edge.infoMatrix * e;
}
// 3. 求解线性方程
VectorXd delta = H.ldlt().solve(-b);
// 4. 更新所有顶点
for (auto& vertex : vertices) {
vertex.update(delta.segment(vertex.index, 6));
}
// 5. 检查收敛
if (delta.norm() < convergence_threshold) break;
}
}实际工程中，我们一般用现成的优化库，比如g2o、Ceres或GTSAM。但理解底层原理很重要——出了问题才知道怎么调试。
4.5 实践中的注意事项
| 环节 | 常见问题 | 我的建议 |
|---|---|---|
| 顶点初始化 | 初始值太差，优化陷入局部最优 | 先用里程计做粗略初始化，再优化 |
| 信息矩阵设置 | 权重不合理，优化结果偏差 | 根据传感器噪声模型计算，不要拍脑袋 |
| 回环边添加 | 错误回环导致图被「拉歪」 | 加回环前做几何一致性检查 |
| 求解器选择 | 稠密求解太慢，稀疏求解不稳定 | 小图用稠密，大图（>100节点）用稀疏 |
最后说一句，位姿图优化不是万能的。它修正的是轨迹的「形状」，而不是绝对精度。如果所有约束都有系统性偏差，优化结果也会跟着偏。这时候就需要引入绝对观测，比如GPS或 landmarks，才能把轨迹拉回真实位置。
好了，这一节的内容就到这里。位姿图优化是SLAM后端中非常核心的一块，理解了它，你就掌握了轨迹修正的「手术刀」。