SLAM中的非线性优化-2D图优化之零空间（十五）

这节在进行讲解SLAM中一个重要概念，零空间，讲它有啥用呢？因为SLAM中零空间的存在，才需要FEJ或固定约束存在，本节内容不属于2D图优化独有，先看看什么是零空间概念；零空间是一个核心概念，它深刻揭示了SLAM问题的内在结构和优化过程中的关键特性。简单来说，零空间指的是对系统状态（机器人的位姿和地图点的位置）进行某种整体变换时，不会改变传感器观测值（例如图像特征点重投影误差、激光点云匹配误差）的变换方向所构成的空间。

一、核心概念解析

不可观测的自由度：
SLAM的目标是同时估计机器人的运动轨迹（位姿序列）和环境地图（点云或特征点位置）。然而，仅凭机器人自身的运动传感器（IMU、轮速计）和外部环境观测传感器（相机、激光雷达）的相对测量，无法确定整个系统在全局坐标系中的绝对位置和朝向。想象一下：你闭着眼睛在一个未知的房间里走动，通过触摸墙壁和家具来构建房间地图并确定自己的位置。你构建的地图和你的路径在房间内的绝对位置和朝向是未知的（你不知道地图的“北”在哪里，也不知道起点在地球上的经纬度），但地图本身的结构和你路径的相对形状是正确的。
数学表达：
大多数SLAM后端（无论是基于滤波的如EKF-SLAM，还是基于优化的如Bundle Adjustment）最终都归结为一个最小二乘问题：最小化预测的观测值与实际观测值之间的误差。目标函数通常写作： min Σ ||hᵢ(X) - zᵢ||²，其中 X 是整个状态向量（包含所有位姿和地图点），hᵢ(X) 是基于状态预测的第 i 个观测值，zᵢ 是实际的第 i 个观测值。求解通常需要计算目标函数关于状态 X 的导数（雅可比矩阵 J）和海森矩阵近似 H ≈ JᵀJ。零空间就体现在海森矩阵 H 上：H 是一个对称半正定矩阵，但它不是满秩的。H 存在特征值为零（或非常接近零）的特征向量。这些特征向量对应的方向，就是系统的零空间方向。
零空间变换： 假设存在一个变换 T（例如，对整个地图和所有机器人位姿同时施加一个刚体变换——平移和旋转）。如果对这个变换后的状态 T(X) 进行观测预测 hᵢ(T(X))，其结果与对原始状态 X 的观测预测 hᵢ(X) 完全相等，那么 T 所对应的增量方向 ΔX 就位于海森矩阵 H 的零空间中。数学上： J * ΔX = 0 这意味着在这个方向 ΔX 上移动状态，观测误差的一阶变化为零。H * ΔX = JᵀJ * ΔX = Jᵀ * (J * ΔX) = Jᵀ * 0 = 0
维度： 在三维空间 中，一个自由漂浮的SLAM系统（没有固定到全局坐标系的锚点）的零空间维度是 6：3个自由度：全局旋转 (roll, pitch, yaw)；3个自由度：全局平移 (x, y, z)。在 二维空间 中，零空间维度是 3：1个自由度：全局旋转 (yaw)；2个自由度：全局平移 (x, y)。

零空间带来的影响和挑战

解的不唯一性：
由于存在零空间，最小二乘问题有无数个解。所有这些解只相差一个全局的刚体变换（零空间变换）。从观测误差的角度看，它们都是等价的“最优解”。后果： 如果优化算法不加约束，理论上最终解会漂移在零空间定义的流形上。数值计算中，由于噪声和线性化误差，解可能会在这个流形上缓慢漂移。
数值不稳定性：
海森矩阵 H 是奇异的（或病态的，条件数很大），因为它有零特征值。这使得在求解线性方程 H * ΔX = -b (高斯-牛顿/列文伯格-马夸尔特算法的核心步骤) 时变得困难。后果： 直接求逆或使用不稳定的线性求解器会导致数值溢出、解振荡或不收敛。

实践中如何处理零空间

SLAM算法必须显式或隐式地处理零空间问题：

固定先验（锚定）：最常见的方法。 固定第一个（或前几个）机器人位姿的6个自由度（或3个在2D中）。例如，设置 T0 = [I | 0] (单位旋转矩阵，零平移向量)。作用： 这相当于给系统添加了一个绝对的参考系，移除了零空间对应的自由度，使 H 矩阵变得满秩正定。优化问题有唯一解（在噪声范围内）。实现： 在优化问题中添加一个强先验因子（Prior Factor），将 T0 牢牢地“钉”在原点。
添加伪观测值：引入一些微弱的绝对测量（即使现实中不存在），比如一个非常弱的GPS先验（方差很大），或者一个假设的地图点固定在原点。作用： 这些微弱的观测理论上消除了零空间，使 H 变得正定（但可能条件数仍然很大）。数值稳定性提高，解被轻微地约束在原点附近，但允许在观测证据很强时发生轻微漂移。比固定先验更“软”一些。
使用特殊的线性求解器：使用能够处理奇异或病态矩阵的求解器，例如：（1）QR 分解： 数值稳定性较好。（2）Cholesky 分解（带阻尼/正则化）： 列文伯格-马夸尔特算法本身就通过添加 λI 项对 H 进行正则化 (H + λI)，使矩阵正定，间接处理了零空间问题。λ 控制阻尼强度。（3）SVD（奇异值分解）： 可以显式识别并丢弃（或阻尼）对应于零奇异值（零空间）的方向。作用： 这些方法不直接移除零空间自由度，而是在求解步骤中稳健地处理奇异性，找到一个有效的解（通常对应于某种最小范数解）。
流形优化：机器人位姿（旋转）本身存在于非欧几里得空间（流形）上（如SO(3)/SE(3)）。现代优化库（g2o, Ceres Solver, GTSAM）通常直接在流形上进行状态更新。作用： 这确保了位姿在优化过程中始终保持在有效的流形上，提高了数值稳定性和收敛性。虽然不直接解决零空间问题，但它是处理旋转相关数值问题的必要基础。
忽略（在特定算法中）：在某些基于滤波的方法（如EKF）中，初始协方差矩阵可能设置得足够大以包含零空间不确定性。随着过程噪声和观测更新，协方差矩阵中与零空间对应的部分可能会增长（如果没有任何绝对观测），但算法通常仍能给出相对一致的轨迹和地图（协方差反映了这种全局不确定性）。然而，数值问题仍然需要小心处理。

为什么零空间不影响SLAM的核心功能

关键在于 相对关系：（1）虽然整个轨迹和地图的绝对位置和朝向（零空间）无法确定，但轨迹的形状（机器人从一个位姿移动到下一个位姿的相对变换）和地图的结构（点与点之间的相对位置）是唯一确定的（在观测噪声范围内）。（2）SLAM的核心目标——让机器人在自己构建的地图中实现定位（知道自己在哪）和导航（知道怎么去目的地）——完全依赖于这些精确的相对关系。绝对的经纬度坐标对于机器人在其自身地图中行动通常不是必需的（除非需要与外部绝对坐标系对齐）。

总结：（1）零空间是SLAM问题的固有属性： 它源于仅依靠相对测量无法确定全局参考系（6个自由度）；（2）体现为奇异性： 优化问题的海森矩阵 H 不满秩，存在特征值为零的方向（零空间变换方向）；（3）导致解不唯一和数值困难： 解在零空间定义的流形上不唯一，数值求解不稳定。（4）必须处理： 常用方法包括固定第一个位姿（锚定）、添加弱先验、使用鲁棒的线性求解器（SVD, 带阻尼的Cholesky）以及在流形上进行优化。（5）不影响核心功能： SLAM提供的精确的相对轨迹和地图结构足以支持定位和导航。

二、SLAM 中的零空间（Null Space）数学解释

在 SLAM（Simultaneous Localization and Mapping）问题中，零空间（Null Space）描述了系统状态（机器人位姿和地图点）的某些变换不会影响观测误差（如重投影误差、点云匹配误差）。这些变换对应的方向在优化问题的 Hessian 矩阵中是 零特征值方向，导致系统无法唯一确定全局参考系。

1. SLAM优化的数学模型

1.1 线性化与Gauss-Newton优化

2.1 零空间的数学定义

2.1 零空间（Null Space）的定义

2.2 SLAM 零空间的物理意义

3. Hessian矩阵的奇异性

4. 数学示例

零空间是 SLAM 固有的自由度，对应全局刚体变换（平移+旋转）。
Hessian 矩阵奇异，导致优化问题有无穷多解（相差一个全局变换）。
解决方法：
- 固定先验（Gauge Fixing）。
- 使用伪逆或阻尼（LM 方法）。
- 流形优化（确保位姿在 SE(3) 上更新）。

数学上，零空间的存在反映了 SLAM 问题的 相对观测无法确定绝对参考系，但 不影响内部一致性（轨迹和地图的相对关系仍然正确）。

则

在 SLAM（Simultaneous Localization and Mapping）中，观测模型 hij(Ti,pj)hij(Ti,pj) 通常描述的是 相对测量（如相机观测到的地图点像素坐标、激光雷达匹配的点云距离等）。这些测量仅依赖于 机器人与环境之间的相对几何关系，而不是它们的绝对全局位置。