当前位置：首页 > news >正文

为什么LIO-SAM的残差项使用对数映射

news 2025/9/7 12:00:17

1. 概念解释

1.1 概念1：群

首先，一个群是一个代数结构，包含一个集合和一种运算，这个组合需要满足四个条件：

封闭性：集合内任意两个元素进行运算后，结果仍然在集合内。
结合律：运算满足结合律 (a◦b)◦c = a◦(b◦c)。
单位元：存在一个单位元 e，使得任何元素 a 满足 e◦a = a◦e = a。
逆元：集合中每个元素 a 都存在一个逆元 a⁻¹，使得 a◦a⁻¹ = a⁻¹◦a = e。

例子：所有整数和加法运算构成一个群，单位元是0，任何整数的逆元是它的相反数。

1.2 概念2：特殊欧氏群 SE(3)

特殊正交群 SO(3)： 所有3x3的正交矩阵（即 RᵀR = RRᵀ = I, det® = +1）组成的群，代表了所有可能的三维旋转。

反对称矩阵： 是一种特殊的方阵，其转置等于其负数，对于三维向量 $θ=[θx,θy,θz]T\theta=[\theta_x, \theta_y, \theta_z]^T$ ，其反对称j矩阵定义为：

$ϕ^=[0−θzθyθz0−θx−θyθx0]\hatϕ=\begin{bmatrix}0 & -\theta_z & \theta_y\\\theta_z&0&-\theta_x\\-\theta_y&\theta_x&0\end{bmatrix}$

在机器人和SLAM中，特殊欧氏群 SE(3)，代表了所有可能的三维刚体变换（旋转 + 平移），也就是机器人的位姿。

它由一个旋转矩阵 R ∈ SO(3) 和一个平移向量 t ∈ R³ 构成，是所有合法的 4x4 变换矩阵 T：

$T=[Rt0T1]T=\begin{bmatrix}R & t\\0^T&1\end{bmatrix}$

其中 R 是一个 3x3 的旋转矩阵，属于特殊正交群 SO(3)（满足 RᵀR = I 且 det® = 1），t 是一个 3x1 的平移向量。

它的运算是矩阵乘法，两个变换矩阵相乘 T₁ ◦ T₂ 表示先经过 T₂ 变换，再经过 T₁ 变换。SE(3)满足群的四个条件，因此SE(3) 中的变换满足 T₁ ◦ T₂ ∈ SE(3)，T⁻¹ ∈ SE(3)，完美代表了机器人的位姿（旋转+平移），所以，SE(3) 就是所有机器人位姿的集合，每一个位姿都是这个群中的一个元素。

1.3 概念3：李群与李代数

李群： 指连续且光滑的群，SO(3) 和 SE(3) 都是李群，SO(3)可以想象成一个光滑的球面或SE(3)想作一个光滑的高维曲面，可以在上面进行连续、光滑的运动。
关键点： 李群描述的是最终的、有限的变换结果。

李代数： 对应李群在单位元（即原点）处的切空间，记为小写的so(3) / se(3)，其中so(3)可以想象一个球体（李群SO(3)），在它的北极点（单位元，即无旋转）放一块平坦的纸板，这块纸板就是它的切空间（即李代数so(3)）。这个切空间里的向量 φ（3维）或 ξ = [ρ, φ]ᵀ（6维）代表了瞬时运动：φ 的方向是旋转轴，模长是旋转速度；ρ 与线速度相关。同理，se(3)：对应 SE(3) 的李代数，其元素是一个6维向量 ξ = [ρ, φ]ᵀ。其中 φ 代表旋转，ρ 与平移相关。
关键点： 李代数描述的是“速度”或“瞬时运动”。

感性理解：
想象一个粒子在一条直线上运动：

速度 v（一个标量）就是一个“瞬时运动”，类似于李代数的元素
位置 p（一个标量）就是一个“有限状态”，类似于李群的元素。
如果速度 v 是恒定的，那么经过时间 t=1 后，粒子位置的变化就是：Δp = v * 1

1.4 连接二者的桥梁：指数与对数映射

这是最核心的一对操作，将瞬时运动和有限变换联系起来，选择指数映射的原因是指数映射的导数在单位元上是恒等映射，即 $ddxex=ex\frac d{dx}e^x=e^x$ ，更一般的微分方程可以表示为： $dTdt=T⋅X\frac{dT}{dt}=T\cdot X$ ，其中X是常数矩阵。

指数映射: exp: se(3) → SE(3)，物理意义是对瞬时速度进行积分，得到有限时间内的位置变化，比如一个恒定的角速度 ω（李代数 so(3) 中的一个向量），持续旋转 1 秒钟，最终得到的旋转矩阵 R（李群 SO(3)）就是 exp(ω)。

对数映射: log: SE(3) → se(3)，反过来的物理意义就是指数映射的逆过程，从一个有限的位置变化，求出其对应的瞬时运动，比如一个旋转矩阵 R，可以通过 log® 求出一个旋转向量 φ，它的方向是这次旋转的轴，模长是旋转的角度。

感性理解：
如果运动发生在复杂的空间里（比如在一个球面上旋转），就需要一个能推广前面例子这种积分关系的工具，想象一个更贴近的例子：复平面上的旋转。

单位复数 $e^{iθ}$ 表示一个旋转（李群U(1)的元素）。
纯虚数 iθ 可以看作一个“旋转指令”（李代数u(1)的元素）。

通过指数映射可以连接它们，著名的欧拉公式：
$e^{iθ} = cosθ + i sinθ$
这个公式完美地将一个“旋转速度”（iθ，以多快的速度绕原点旋转）积分为了一个“最终的旋转状态”（e^(iθ)，一个具体的复数），所以，指数映射的第一个直观原因：它是在许多空间中，从“瞬时生成元”到“有限变换”的自然推广。
几何解释：复平面上的单位圆， $eiθe^{i\theta}$ 对应复平面上的点：

实部 cosθ：横坐标
虚部 sinθ ：纵坐标
模长： $∣eiθ∣=cos2θ+sin2θ=1|e^{i\theta}|=\sqrt{cos^2\theta+sin^2\theta}=1$ （单位圆）
角度θ ：从正实轴逆时针旋转的角度
复平面示意图：

虚部
^
|   e^{iθ} = (cosθ, sinθ)
|    /|
|   / |
|  /  | sinθ
| /θ  |
|/____|____> 实部cosθ

2：LIO-SAM的应用

优化算法的核心是求雅可比矩阵（残差对状态变量的导数），但是直接在 SE(3) 上定义导数非常困难。使用四元数时，定义残差关于四元数的导数非常复杂且不直观，而在李代数这个向量空间求导，则是简单、标准且自然，可以在李代数这个切空间里做所有熟悉的向量空间微积分。

2.1 SO(3) 旋转示例

假设有一个绕 Z 轴旋转 θ 角度的变换，使用李群 (结果)表示的旋转矩阵为：

$T=[cosθ−sinθ0sinθcosθ0001]∈SO(3)T=\begin{bmatrix}cos\theta & -sin\theta & 0\\sin\theta&cos\theta&0\\0&0&1\end{bmatrix} ∈SO(3)$

使用李代数 (瞬时运动)表示的旋转向量 φ = [0, 0, θ]ᵀ ∈ so(3)，其反对称矩阵为：

$φ^=[0−θ0θ00000]∈SO(3)\hatφ=\begin{bmatrix}0 & -\theta & 0\\\theta&0&0\\0&0&0\end{bmatrix} ∈SO(3)$

指数映射： $R=exp(φ^)R = exp(\hatφ)$ 对于这个简单的轴对齐旋转，它确实成立。

对数映射： $φ^=log(R)\hatφ= log(R)$ 。

2.2 在LIO-SAM中的应用：优化中的位姿更新

这是李群李代数在SLAM中最关键的应用，回顾LIO-SAM的因子图优化，需要调整位姿估计 T 以最小化残差。

目标：对一个位姿 T ∈ SE(3)（一个受约束的矩阵）进行求导和更新

错误做法（欧拉角/四元数加法）：
$T_{new} = T_{old} + ΔT 或 q_{new} = q_{old} + Δq$
直接对旋转矩阵的元素进行加法，会导致 T_new 不再是一个合法的变换矩阵（不满足正交性），或者四元数的四个数加法更新，则q_new 不再是一个单位四元数，不再满足单位范数约束，最终则破坏了位姿的约束。
如果对上面的变化进行处理，比如，在每次更新后，需要对旋转矩阵进行重新正交化（计算量大有误差），或对四元数进行重新归一化（虽然可行，但只是解决方案的一部分，没解决求导问题）

正确做法（李群李代数）：
在切空间（李代数）中计算增量：将优化问题转换到李代数空间 se(3) 中进行，在这里，增量是一个无约束的 6 维向量 δξ = [δρ, δφ]ᵀ，可以自由地对其加减，最后通过指数映射将增量作用回李群：位姿的更新公式为：

$T(new)=T(old)⋅exp(δξ^)T^{(new)}=T{(old)}\cdot exp(\delta\hat\xi)$
这个操作的优势在于无论 δξ 这个增量是多少，exp(δξ^) 永远是一个合法的 SE(3) 变换，因为T^{(old)} 本身是合法的，两个合法变换相乘结果依然是合法的，这就保证了优化过程中位姿的合法性。