因子图优化(Factor Graph Optimization)

因子图优化是现代机器人状态估计与多传感器融合的技术,广泛应用于以下场景:

  • SLAM:同时定位与建图、回环检测后的全局轨迹优化
  • VIO / LIO:融合相机、IMU、激光雷达,估计机器人运动状态
  • 三维重建:Bundle Adjustment(BA),联合优化相机位姿和空间点
  • 机器人标定:优化相机外参、传感器外参和手眼标定参数
  • 多传感器融合:综合里程计、GNSS、视觉等不同精度的观测

一、因子图优化

1.1 定义

因子图优化:把多个带噪声、可能相互矛盾的观测约束组织成图结构,通过联合优化找到最能同时满足所有约束的一组未知状态。

举一个典型的移动机器人例子:

假设机器人沿正方形路径运动并最终回到起点,里程计存在累计误差:

  • 第一次前进,距离多估计 2 cm
  • 第二次转弯,角度多估计 1°
  • 第三次前进,又产生 3 cm 误差
  • 最终里程计认为距离起点还有 40 cm,这就是累计漂移(Accumulated Drift)

此时视觉回环检测给出约束:当前位置就是之前经过的起点位置。系统出现两类约束:

  • 里程计约束:相邻时刻的相对运动
  • 回环约束:当前位姿与历史位姿的空间一致性

两类约束存在不一致,因子图优化不会强行把末端拉回起点,而是根据所有观测的置信度,将误差合理分配到整条轨迹上。

1.2 直观理解:从轨迹到图

因子图包含两类节点:

  • 变量节点(Variable Node) :代表待估计的状态,如机器人位姿 X0,X1,X2,X3X_0,X_1,X_2,X_3X0,X1,X2,X3
  • 因子节点(Factor Node):代表状态间的概率约束,如里程计、回环测量,通常用菱形表示

每个因子仅对其关联的变量施加约束,最终优化目标可概括为:

X∗=arg⁡min⁡X∑k=1M∣rk(Xk)∣∗Ωk2⏟∗第 k 个因子的加权误差 \boxed{ X^*=\arg\min_X \sum_{k=1}^{M} \underbrace{|r_k(X_k)|*{\Omega_k}^{2}}*{\text{第 }k\text{ 个因子的加权误差}} } X∗=argXmink=1∑M ∣rk(Xk)∣∗Ωk2∗第 k 个因子的加权误差

各符号含义:

符号 含义
XXX 所有待优化状态的集合
X∗X^*X∗ 优化后的最优状态
rkr_krk 第 kkk 个观测对应的残差
Ωk\Omega_kΩk 信息矩阵,描述观测精度
MMM 因子总数

其中马氏距离定义为:

∣r∣Ω2=r⊤Ωr |r|_\Omega^2=r^\top\Omega r ∣r∣Ω2=r⊤Ωr

💡 本节先给出优化目标形式,后续将从概率论出发推导其合理性,解释为何不能直接对所有残差求和。


二、数学基础:因子图转化为最小二乘

这是理解因子图优化的三层递进:

贝叶斯推断 → 最大后验估计 MAP → 概率因子分解 → 加权非线性最小二乘

2.1 从贝叶斯公式出发

定义:

  • XXX:机器人所有未知状态(位姿、速度、IMU零偏等)
  • ZZZ:所有传感器观测数据

我们的目标是找到观测条件下概率最大的状态,即最大后验概率估计(Maximum A Posteriori,MAP) :

X∗=arg⁡max⁡XP(X∣Z) X^*=\arg\max_X P(X\mid Z) X∗=argXmaxP(X∣Z)

根据贝叶斯公式:

P(X∣Z)=P(Z∣X)(P(X))(P(Z)) P(X\mid Z)=\frac{P(Z\mid X)(P(X))}{(P(Z))} P(X∣Z)=(P(Z))P(Z∣X)(P(X))

  • (P(X))(P(X))(P(X)):先验概率,代表对状态的初始认知
  • P(Z∣X)P(Z\mid X)P(Z∣X):似然概率,代表给定状态下观测出现的可能性
  • P(X∣Z)P(X\mid Z)P(X∣Z):后验概率,融合先验与观测后的状态分布

由于观测 ZZZ 已确定,(P(Z))(P(Z))(P(Z)) 与优化变量无关,因此:

X∗=arg⁡max⁡XP(Z∣X)(P(X)) X^*=\arg\max_X P(Z\mid X)(P(X)) X∗=argXmaxP(Z∣X)(P(X))

机器人状态估计本质上就是一个概率最大化问题。

2.2 概率的因子分解

以三个位姿的简单场景为例:X=X0,X1,X2X={X_0,X_1,X_2}X=X0,X1,X2,存在四类信息:

  1. 初始位姿的先验信息
  2. X0X_0X0 到 X1X_1X1 的里程计观测
  3. X1X_1X1 到 X2X_2X2 的里程计观测
  4. X2X_2X2 与 X0X_0X0 之间的回环观测

在条件独立假设下,后验分布可分解为多个因子的乘积:

P(X∣Z)∝ϕ0(X0) ⋅ϕ1(X0,X1) ⋅ϕ2(X1,X2) ⋅ϕ3(X0,X2) \begin{aligned} P(X\mid Z)\propto{}& \phi_0(X_0)\ &\cdot\phi_1(X_0,X_1)\ &\cdot\phi_2(X_1,X_2)\ &\cdot\phi_3(X_0,X_2) \end{aligned} P(X∣Z)∝ϕ0(X0) ⋅ϕ1(X0,X1) ⋅ϕ2(X1,X2) ⋅ϕ3(X0,X2)

其中 ϕi\phi_iϕi 就是因子函数(Factor),每个因子仅依赖少量变量。

推广到一般形式:

P(X∣Z)∝∏k=1Mϕk(Xk) P(X\mid Z)\propto\prod_{k=1}^{M}\phi_k(X_k) P(X∣Z)∝k=1∏Mϕk(Xk)

这里 XkX_kXk 代表第 kkk 个因子关联的变量子集,而非单个状态。这也是因子图高效的根本原因:绝大多数测量仅关联少量变量,而非系统全部状态。

2.3 高斯噪声下的最小二乘推导

假设传感器测量模型为:

zk=hk(Xk)+ϵk z_k=h_k(X_k)+\epsilon_k zk=hk(Xk)+ϵk

  • zkz_kzk:实际测量值
  • hk(Xk)h_k(X_k)hk(Xk):状态对应的预测测量值
  • ϵk\epsilon_kϵk:测量噪声,服从高斯分布 ϵk∼N(0,Σk)\epsilon_k\sim\mathcal N(0,\Sigma_k)ϵk∼N(0,Σk)

定义残差:

rk(Xk)=hk(Xk)−zk r_k(X_k)=h_k(X_k)-z_k rk(Xk)=hk(Xk)−zk

则观测似然为:

P(zk∣Xk)∝exp⁡(−12rk⊤Σk−1rk) P(z_k\mid X_k)\propto \exp\left( -\frac{1}{2}r_k^\top\Sigma_k^{-1}r_k \right) P(zk∣Xk)∝exp(−21rk⊤Σk−1rk)

将所有因子相乘后取负对数(最大化概率等价于最小化负对数):

X∗=arg⁡max⁡X∏kϕk(Xk) =arg⁡min⁡X−log⁡∏kϕk(Xk) =arg⁡min⁡X∑k−log⁡ϕk(Xk) \begin{aligned} X^* &=\arg\max_X\prod_k\phi_k(X_k)\ &=\arg\min_X-\log\prod_k\phi_k(X_k)\ &=\arg\min_X\sum_k-\log\phi_k(X_k) \end{aligned} X∗=argXmaxk∏ϕk(Xk) =argXmin−logk∏ϕk(Xk) =argXmink∑−logϕk(Xk)

代入高斯模型并去掉常数项,得到:

X∗=arg⁡min⁡X12∑k=1Mrk⊤Σk−1rk \boxed{ X^*=\arg\min_X \frac{1}{2}\sum_{k=1}^{M} r_k^\top\Sigma_k^{-1}r_k } X∗=argXmin21k=1∑Mrk⊤Σk−1rk

令信息矩阵 Ωk=Σk−1\Omega_k=\Sigma_k^{-1}Ωk=Σk−1,最终得到标准形式:

X∗=arg⁡min⁡X12∑k=1Mrk⊤Ωkrk \boxed{ X^*=\arg\min_X\frac{1}{2} \sum_{k=1}^{M}r_k^\top\Omega_k r_k } X∗=argXmin21k=1∑Mrk⊤Ωkrk

⚠️ 该形式基于高斯因子假设。因子图不局限于高斯分布,非高斯噪声、多峰分布、离散变量可采用其他因子模型与推断方法。

2.4 信息矩阵的物理意义

信息矩阵定义为协方差的逆:Ω=Σ−1\Omega=\Sigma^{-1}Ω=Σ−1。

一维场景下:Σ=σ2,Ω=1σ2\Sigma=\sigma^2,\quad \Omega=\frac{1}{\sigma^2}Σ=σ2,Ω=σ21,此时误差项为:

E=(x−z)2σ2 E=\frac{(x-z)^2}{\sigma^2} E=σ2(x−z)2

信息矩阵本质是观测的置信权重:精度越高的测量,对目标函数影响越大。

观测类型 标准差 信息权重
高精度定位 0.01 m 10,000
常规定位 0.10 m 100
粗糙定位 1.00 m 1

注意:信息权重仅代表建模时的置信度,不等于传感器真实精度。错误的协方差设置会拉偏优化结果。此外多维协方差可能包含交叉项,不能一概用对角矩阵近似。


三、数值例子:直观理解联合优化

考虑一维直线运动的机器人,x0=0x_0=0x0=0 已知,存在三个测量:

因子 测量值 标准差
里程计 0→1 前进 1.1 m 0.2 m
里程计 1→2 前进 1.0 m 0.2 m
回环 0→2 总位移 1.9 m 0.1 m

纯里程计累计结果:x1=1.1,x2=2.1x_1=1.1,\quad x_2=2.1x1=1.1,x2=2.1

回环观测结果:x2≈1.9x_2\approx1.9x2≈1.9

构造目标函数:

E(x1,x2)=(x1−1.1)20.22 +(x2−x1−1.0)20.22 +(x2−1.9)20.12 \begin{aligned} E(x_1,x_2)={}& \frac{(x_1-1.1)^2}{0.2^2}\ &+\frac{(x_2-x_1-1.0)^2}{0.2^2}\ &+\frac{(x_2-1.9)^2}{0.1^2} \end{aligned} E(x1,x2)=0.22(x1−1.1)2 +0.22(x2−x1−1.0)2 +0.12(x2−1.9)2

约束之间存在矛盾,无法让所有残差同时为0,优化器需要找到统计最优的折中解。

状态 位置(m) 说明
优化前末端 2.100 纯里程计累计结果
优化后末端 1.922 联合最优估计结果

对目标函数求导置零,可得解析解:

x1∗=1.0111,x2∗=1.9222 \boxed{x_1^*=1.0111,\qquad x_2^*=1.9222} x1∗=1.0111,x2∗=1.9222

为什么不直接让 x2=1.9x_2=1.9x2=1.9?因为回环同样存在噪声,不能完全丢弃里程计信息。因子图优化的思想就是基于不确定性,在所有约束间寻找统计最优解,而非单一信任某一个传感器。


四、因子图中的常见因子类型

在 SLAM、VIO 系统中,典型因子如下表:

因子类型 关联变量 约束内容
Prior Factor(先验因子) 1个状态 初始位姿、已知固定参数
Odometry Factor(里程计因子) 前后两个位姿 机器人相对运动约束
Loop Closure Factor(回环因子) 两个历史位姿 回环相对位姿约束
Reprojection Factor(重投影因子) 相机位姿 + 地图点 视觉重投影误差
IMU Factor(惯性因子) 多个相邻状态 旋转、速度、位置变化约束
GNSS Factor(全局定位因子) 位姿/位置 全球位置观测约束
Calibration Factor(标定因子) 位姿 + 外参等 传感器标定关系约束

因子不都是二元的。例如重投影因子可同时关联位姿、地图点、内外参;IMU预积分因子会约束两个时刻的多个状态量。

4.1 先验因子

先验因子约束单个状态靠近参考值:

rprior=X0−Xˉ0 r_{\text{prior}}=X_0-\bar X_0 rprior=X0−Xˉ0

先验的作用是固定参考坐标系 。若所有约束都是相对位姿,整张图可整体平移旋转而不改变残差,即存在规范自由度(Gauge Freedom),会导致矩阵秩亏。

4.2 里程计/相对位姿因子

设 Ti,Tj∈(SE(3))T_i,T_j\in (SE(3))Ti,Tj∈(SE(3)) 为世界坐标系下位姿,预测相对变换:

Tijpred=Ti−1Tj T_{ij}^{\text{pred}}=T_i^{-1}T_j Tijpred=Ti−1Tj

传感器测量相对变换为 ZijZ_{ij}Zij,构造残差:

rij=(Log⁡)⁡(Zij−1Ti−1Tj)∨ \boxed{ r_{ij}= \operatorname{(\operatorname{Log})} \left( Z_{ij}^{-1}T_i^{-1}T_j \right)^\vee } rij=(Log)(Zij−1Ti−1Tj)∨

  • (Log⁡)⁡\operatorname{(\operatorname{Log})}(Log):李群到李代数映射
  • (⋅)∨(\cdot)^\vee(⋅)∨:李代数矩阵转六维向量

此处为右侧相对误差定义,不同实现约定不同,需配套对应的雅可比和协方差。

4.3 视觉重投影因子

三维地图点 Pj=X,Y,Z⊤P_j=X,Y,Z^\topPj=X,Y,Z⊤,通过相机模型投影到像素:

z^ij=π(Ti−1Pj) \hat z_{ij}= \pi\left(T_i^{-1}P_j\right) z^ij=π(Ti−1Pj)

实际检测像素为 zij=uvz_{ij}=\begin{bmatrix} u \\ v \end{bmatrix}zij=uv,残差为:

rijvision=π(Ti−1Pj)−zij \boxed{ r_{ij}^{\text{vision}}= \pi(T_i^{-1}P_j)-z_{ij} } rijvision=π(Ti−1Pj)−zij

同时优化相机位姿和地图点,最小化重投影误差,就是Bundle Adjustment(光束法平差) 的核心。

4.4 回环因子

回环因子数学形式与相对位姿因子接近:

rloop=(Log⁡)⁡(Zij−1Ti−1Tj)∨ r_{\text{loop}}= \operatorname{(\operatorname{Log})} \left( Z_{ij}^{-1}T_i^{-1}T_j \right)^\vee rloop=(Log)(Zij−1Ti−1Tj)∨

区别在于来源:

  • 里程计约束相邻时刻状态
  • 回环约束时间间隔远、空间重合的状态

回环的价值在于建立远距离状态关联,让累计误差在全局重新分配。但错误回环比没有回环更糟糕,会导致地图整体变形。


五、优化求解:从雅可比到高斯牛顿、LM算法

5.1 为什么不能一步求解?

目标函数:

E(X)=12∑krk(Xk)⊤Ωkrk(Xk) E(X)=\frac12\sum_k r_k(X_k)^\top\Omega_k r_k(X_k) E(X)=21k∑rk(Xk)⊤Ωkrk(Xk)

但残差通常是非线性的:相机投影包含透视除法,位姿计算包含旋转矩阵等非线性运算。因此需要迭代线性化的思路:在当前估计点泰勒展开,求解增量,更新状态,反复迭代。

5.2 步骤一:计算残差

当前状态估计 X(t)X^{(t)}X(t),计算每个因子的残差:

rk(t)=rk(X(t)) r_k^{(t)}=r_k(X^{(t)}) rk(t)=rk(X(t))

以前面一维例子为例,初值为里程计累计值:

X(0)=1.12.1 X^{(0)}= \begin{bmatrix} 1.1 \\ 2.1 \end{bmatrix} X(0)=1.12.1

三个因子残差:

r(X(0))=1.1−1.12.1−1.1−1.02.1−1.9=000.2 r(X^{(0)})= \begin{bmatrix} 1.1-1.1 \\ 2.1-1.1-1.0 \\ 2.1-1.9 \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \\ 0.2 \end{bmatrix} r(X(0))= 1.1−1.12.1−1.1−1.02.1−1.9 = 000.2

5.3 步骤二:计算雅可比矩阵

残差一阶泰勒展开:

r(X+ΔX)≈r(X)+JΔX r(X+\Delta X)\approx r(X)+J\Delta X r(X+ΔX)≈r(X)+JΔX

其中雅可比 J=∂r∂XJ=\frac{\partial r}{\partial X}J=∂X∂r,描述状态微小变化对残差的影响。

一维例子的雅可比:

J=10−1101 \boxed{ J= \begin{bmatrix} 1 & 0 \\ -1 & 1 \\ 0 & 1 \end{bmatrix} } J= 1−10011

无关联的变量对应雅可比元素为0,这就是稀疏优化的数学基础。

5.4 步骤三:构造正规方程

代入线性化近似,目标函数变为:

E(ΔX)≈12(r+JΔX)⊤Ω(r+JΔX) E(\Delta X)\approx \frac12(r+J\Delta X)^\top \Omega(r+J\Delta X) E(ΔX)≈21(r+JΔX)⊤Ω(r+JΔX)

对增量 ΔX\Delta XΔX 求导并置零:

J⊤ΩJΔX=−J⊤Ωr J^\top\Omega J\Delta X=-J^\top\Omega r J⊤ΩJΔX=−J⊤Ωr

定义近似Hessian矩阵 H=J⊤ΩJH=J^\top\Omega JH=J⊤ΩJ,梯度 g=J⊤Ωrg=J^\top\Omega rg=J⊤Ωr,得到高斯牛顿(Gauss-Newton, GN) 正规方程:

HΔX=−g \boxed{H\Delta X=-g} HΔX=−g

此处 HHH 是真实Hessian的高斯牛顿近似,不等于完整二阶导。

5.5 数值例子完整计算

信息矩阵:

Ω=2500025000100 \Omega= \begin{bmatrix} 25 & 0 & 0 \\ 0 & 25 & 0 \\ 0 & 0 & 100 \end{bmatrix} Ω= 2500025000100

计算H矩阵:

H=J⊤ΩJ=50−25−25125 H=J^\top\Omega J= \begin{bmatrix} 50 & -25 \\ -25 & 125 \end{bmatrix} H=J⊤ΩJ=50−25−25125

梯度项:

−g=−J⊤Ωr=0−20 -g=-J^\top\Omega r= \begin{bmatrix} 0 \\ -20 \end{bmatrix} −g=−J⊤Ωr=0−20

求解方程:

50−25−25125\]\[Δx1Δx2\]=\[0−20\] \\begin{bmatrix} 50 \& -25 \\\\ -25 \& 125 \\end{bmatrix} \\begin{bmatrix} \\Delta x_1 \\\\ \\Delta x_2 \\end{bmatrix} = \\begin{bmatrix} 0 \\\\ -20 \\end{bmatrix} \[50−25−25125\]\[Δx1Δx2\]=\[0−20

得增量:

ΔX=−0.088889−0.177778 \Delta X= \begin{bmatrix} -0.088889 \\ -0.177778 \end{bmatrix} ΔX=−0.088889−0.177778

更新状态:

X(1)=X(0)+ΔX=1.0111111.922222 X^{(1)}=X^{(0)}+\Delta X = \begin{bmatrix} 1.011111 \\ 1.922222 \end{bmatrix} X(1)=X(0)+ΔX=1.0111111.922222

线性问题一步即可收敛;非线性问题需要多次迭代重新线性化。

5.6 高斯牛顿法流程

  1. 状态初始化 X(0)X^{(0)}X(0)
  2. 计算所有因子残差 rkr_krk 和雅可比 JkJ_kJk
  3. 累加构造 H=J⊤ΩJH=J^\top\Omega JH=J⊤ΩJ、g=J⊤Ωrg=J^\top\Omega rg=J⊤Ωr
  4. 求解正规方程 HΔX=−gH\Delta X=-gHΔX=−g
  5. 更新状态 X←X⊞ΔXX\leftarrow X\boxplus\Delta XX←X⊞ΔX(⊞\boxplus⊞ 对应流形上的更新)
  6. 判断收敛:满足则输出,否则返回步骤2

5.7 Levenberg-Marquardt(LM)算法

GN法在初值差、非线性强时容易步长过大、发散。LM算法引入阻尼机制:

(H+λI)ΔX=−g \boxed{ (H+\lambda I)\Delta X=-g } (H+λI)ΔX=−g

其中 λ\lambdaλ 为阻尼系数,可根据步长是否有效动态调整。

三种主流优化算法对比:

算法 优势 局限
GN 接近最优解时收敛快,计算直接 对初值敏感,强非线性下易发散
LM 阻尼控制步长,鲁棒性强 需多次试探调整阻尼,计算量更大
Dogleg 结合信赖域策略,搜索方向更优 实现复杂,步长管理难度高

⚠️ GN/LM 均为局部优化算法,不保证全局最优,初值质量至关重要。


六、因子图高效的原因------稀疏性与求解加速

因子图显式表达了局部依赖关系,让求解器可以充分利用稀疏性。

6.1 稀疏雅可比结构

4个位姿的场景,约束包括链式里程计和一个回环:

  • 里程计:r01(X0,X1),r12(X1,X2),r23(X2,X3)r_{01}(X_0,X_1),r_{12}(X_1,X_2),r_{23}(X_2,X_3)r01(X0,X1),r12(X1,X2),r23(X2,X3)
  • 回环:r03(X0,X3)r_{03}(X_0,X_3)r03(X0,X3)

雅可比矩阵中,仅关联变量对应块为非零,其余均为0。真实SLAM中单个位姿6维,轨迹上千个位姿,稠密矩阵会有大量零元素,稀疏存储可大幅节省内存和计算。

6.2 稀疏Hessian矩阵

纯链式里程计下,Hessian为块三对角结构 :

H=H00H0100H10H11H1200H21H22H2300H32H33 H= \begin{bmatrix} H_{00} & H_{01} & 0 & 0 \\ H_{10} & H_{11} & H_{12} & 0 \\ 0 & H_{21} & H_{22} & H_{23} \\ 0 & 0 & H_{32} & H_{33} \end{bmatrix} H= H00H1000H01H11H2100H12H22H3200H23H33

加入回环约束 X0↔X3X_0\leftrightarrow X_3X0↔X3 后,会新增 H03,H30H_{03},H_{30}H03,H30 非零块。新增约束会改变稀疏结构,也影响求解代价。

6.3 Fill-in 与变量消元顺序

稀疏矩阵做 Cholesky/QR 分解时,原本为零的位置可能产生新非零元,称为 Fill-in(填充)。消元顺序不同,Fill-in 数量差异极大。

大规模因子图优化的关键技术:

  • 变量消元排序(最小度排序等)
  • 稀疏 Cholesky / QR 分解
  • 多波前分解(Multifrontal Factorization)

这些是决定大规模SLAM性能的核心因素。

6.4 Schur 补:BA 加速的核心

Bundle Adjustment 同时优化相机位姿和地图点,正规方程分块形式:

BEE⊤C\]\[ΔXcΔXp\]=\[bcbp\] \\begin{bmatrix} B \& E \\\\ E\^\\top \& C \\end{bmatrix} \\begin{bmatrix} \\Delta X_c \\\\ \\Delta X_p \\end{bmatrix} = \\begin{bmatrix} b_c \\\\ b_p \\end{bmatrix} \[BE⊤EC\]\[ΔXcΔXp\]=\[bcbp

  • ΔXc\Delta X_cΔXc:相机位姿增量
  • ΔXp\Delta X_pΔXp:地图点增量

消去地图点变量:

S=B−EC−1E⊤ \boxed{S=B-EC^{-1}E^\top} S=B−EC−1E⊤

这就是 Schur 补(Schur Complement)。

典型BA中地图点之间无直接耦合,CCC 为块对角矩阵,可独立求逆,因此Schur补能大幅降低问题规模。但消元会在位姿子系统引入新非零项,最终效率仍取决于问题结构。


七、位姿优化与李群李代数

7.1 为什么不能直接优化旋转矩阵元素?

三维位姿 T∈(SE(3))T\in (SE(3))T∈(SE(3)):

T=Rt01,R∈SO(3), t∈R3 T= \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix},\quad R\in \mathrm{SO}(3),\ t\in\mathbb R^3 T=R0t1,R∈SO(3), t∈R3

旋转矩阵满足正交约束 R⊤R=I, det⁡(R)=1R^\top R=I,\ \det(R)=1R⊤R=I, det(R)=1,9个元素仅有3个自由度。若直接对元素做加法更新:

R′=R+ΔR R'=R+\Delta R R′=R+ΔR

更新后通常不再满足正交性,不再是合法旋转矩阵。

7.2 李代数:流形上的局部更新

解决方案:不在矩阵元素空间做加法,而在李群的切空间(李代数)用扰动向量更新。

  • (SO(3))(SO(3))(SO(3)) 对应三维旋转向量 δθ∈R3\delta\theta\in\mathbb R^3δθ∈R3,更新方式:
    Rnew=RExp⁡(δθ∧) R_{\text{new}} =R\operatorname{Exp}(\delta\theta^\wedge) Rnew=RExp(δθ∧)
    其中 (⋅)∧(\cdot)^\wedge(⋅)∧ 为向量转反对称矩阵。
  • (SE(3))(SE(3))(SE(3)) 对应六维扰动 δξ=δρ;δϕ∈R6\delta\xi=\\delta\\rho;\\delta\\phi\in\mathbb R^6δξ=δρ;δϕ∈R6,右扰动更新:
    Tnew=TExp⁡(δξ∧) \boxed{ T_{\text{new}} =T\operatorname{Exp}(\delta\xi^\wedge) } Tnew=TExp(δξ∧)

这样既保证更新后仍属于(SE(3)),又只用6个自由度做优化。

7.3 雅可比的对应关系

欧氏空间线性化:r(X+ΔX)≈r(X)+JΔXr(X+\Delta X)\approx r(X)+J\Delta Xr(X+ΔX)≈r(X)+JΔX

流形上的线性化:r(T⊞δξ)≈r(T)+Jδξr(T\boxplus\delta\xi) \approx r(T)+J\delta\xir(T⊞δξ)≈r(T)+Jδξ

雅可比是残差对局部扰动的导数,而非对矩阵元素的导数。左扰动和右扰动的雅可比通常不同,这也是手写优化代码最容易出错的地方。

  • (SE(3))(SE(3))(SE(3)):合法三维刚体位姿的集合(李群)
  • (se(3))(se(3))(se(3)):局部切空间,用于线性化和增量计算(李代数)
  • Exp⁡,Log⁡\operatorname{Exp},\quad \operatorname{Log}Exp,Log:李群与李代数之间的映射
  • Jacobian:残差对局部扰动的导数

李群李代数的工程价值:让非线性的位姿优化可以通过局部线性迭代求解。


八、实时状态估计:iSAM2 与增量优化

8.1 批量 vs 增量优化

  • 批量优化(Batch):构建完整因子图后统一求解,适合离线全局优化
  • 增量优化(Incremental):新观测到来时增量更新,适合在线实时估计

机器人持续运动时,若每来一帧就从头求解全部历史,计算量会越来越大,因此需要增量算法。

8.2 iSAM2

iSAM2(Incremental Smoothing and Mapping 2)是用 Bayes Tree(贝叶斯树) 存储变量消元后的概率结构,复用历史计算结果。

新因子加入时的流程:

  1. 定位受新约束影响的贝叶斯树分支
  2. 对相关变量按需重新线性化
  3. 仅对受影响部分重新消元更新
  4. 未受影响部分完全复用

iSAM2 无需周期性批量重算,通过增量重排序和流体重线性化提升效率。但大型回环等全局约束仍会触发较大范围的更新,并非所有更新都是常数时间。

8.3 滑动窗口与边缘化

VIO系统通常不保留全部历史状态,而是维护滑动窗口。旧状态移出窗口时不能直接丢弃,需要通过边缘化(Marginalization) 将其信息转化为保留状态的先验。

将状态分为待边缘化 XmX_mXm 和保留状态 XrX_rXr,信息矩阵分块:

H=HmmHmrHrmHrr H= \begin{bmatrix} H_{mm}&H_{mr}\\ H_{rm}&H_{rr} \end{bmatrix} H=HmmHrmHmrHrr

消去旧变量得到先验信息矩阵:

Hprior=Hrr−HrmHmm−1Hmr \boxed{ H_{\text{prior}} =H_{rr}-H_{rm}H_{mm}^{-1}H_{mr} } Hprior=Hrr−HrmHmm−1Hmr

该操作与Schur补数学本质一致。边缘化先验依赖当时的线性化点,后续状态变化过大可能出现线性化不一致,是VIO稳定性的关键影响因素。


九、应用:视觉惯性里程计(VIO)的因子图构建

9.1 VIO 状态定义

VIO关键帧状态包含15维:

Xi=Ri,pi,vi,bgi,bai X_i={R_i,p_i,v_i,b_{g_i},b_{a_i}} Xi=Ri,pi,vi,bgi,bai

变量 维度 含义
RiR_iRi 3 姿态(旋转自由度)
pip_ipi 3 位置
viv_ivi 3 速度
bgib_{g_i}bgi 3 陀螺仪零偏
baib_{a_i}bai 3 加速度计零偏

旋转以矩阵/四元数存储,优化时使用3自由度局部扰动。

9.2 视觉惯性因子图结构

因子图包含两类约束:

  • IMU因子:基于预积分,约束相邻关键帧间的运动学关系
  • 视觉重投影因子:约束相机位姿与地图点的几何关系

总目标函数:

E(X,L)=∑k∣rkIMU∣∗Ωk2+∑∗i,j∣rijvision∣∗Ω∗ij2+Eprior E(X,L)= \sum_k|r_k^{\text{IMU}}|*{\Omega_k}^2+ \sum*{i,j}|r_{ij}^{\text{vision}}|*{\Omega*{ij}}^2+ E_{\text{prior}} E(X,L)=k∑∣rkIMU∣∗Ωk2+∑∗i,j∣rijvision∣∗Ω∗ij2+Eprior

9.3 IMU 预积分的意义

IMU采样率(如200Hz)远高于相机(如30Hz),若每个IMU采样都建状态节点,计算量爆炸。

IMU预积分的思想:将两关键帧间的所有IMU测量,预先积分成相对运动约束,输出预积分量 ΔR~ij,Δv~ij,Δp~ij\Delta\tilde R_{ij},\Delta\tilde v_{ij},\Delta\tilde p_{ij}ΔR~ij,Δv~ij,Δp~ij。

典型残差形式:

rR=(Log⁡)⁡(ΔR~ij−1Ri⊤Rj)∨ rv=Ri⊤(vj−vi−gΔt)−Δv~ij rp=Ri⊤(pj−pi−viΔt−12gΔt2)−Δp~ij \begin{align*} r_R&= \operatorname{(\operatorname{Log})} \left( \Delta\tilde R_{ij}^{-1}R_i^\top R_j \right)^\vee \ r_v&= R_i^\top(v_j-v_i-g\Delta t)-\Delta\tilde v_{ij} \ r_p&= R_i^\top \left( p_j-p_i-v_i\Delta t-\frac12g\Delta t^2 \right)-\Delta\tilde p_{ij} \end{align*} rR=(Log)(ΔR~ij−1Ri⊤Rj)∨ rv=Ri⊤(vj−vi−gΔt)−Δv~ij rp=Ri⊤(pj−pi−viΔt−21gΔt2)−Δp~ij

一个IMU因子即可同时约束旋转、速度、位置三个量,大幅提升效率。


十、C++工程:GTSAM 最小位姿图优化示例

GTSAM 是因子图优化的经典库,其建模方式与理论完全对应。

GTSAM 类型 对应数学意义
NonlinearFactorGraph 约束集合,定义优化目标
Values 所有变量的当前取值
LevenbergMarquardtOptimizer 优化求解器

因子图不存储解,只描述约束;Values 才是状态的载体。

10.1 示例代码

以下代码构建3个二维位姿 + 回环约束的最小因子图:

cpp 复制代码
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <iostream>

using namespace gtsam;

int main() {
    // 1. 创建空因子图
    NonlinearFactorGraph graph;

    // 2. 定义噪声模型(Pose2维度:x, y, theta)
    auto priorNoise = noiseModel::Diagonal::Sigmas(
        Vector3(0.0001, 0.0001, 0.0001));
    auto odomNoise = noiseModel::Diagonal::Sigmas(
        Vector3(0.2, 0.2, 0.1));
    auto loopNoise = noiseModel::Diagonal::Sigmas(
        Vector3(0.1, 0.1, 0.05));

    // 3. 添加初始位姿先验
    graph.add(PriorFactor<Pose2>(
        0, Pose2(0.0, 0.0, 0.0), priorNoise));

    // 4. 添加里程计约束
    graph.add(BetweenFactor<Pose2>(
        0, 1, Pose2(1.1, 0.0, 0.0), odomNoise));
    graph.add(BetweenFactor<Pose2>(
        1, 2, Pose2(1.0, 0.0, 0.0), odomNoise));

    // 5. 添加回环约束
    graph.add(BetweenFactor<Pose2>(
        0, 2, Pose2(1.9, 0.0, 0.0), loopNoise));

    // 6. 设置初始估计
    Values initial;
    initial.insert(0, Pose2(0.0, 0.0, 0.0));
    initial.insert(1, Pose2(1.1, 0.0, 0.0));
    initial.insert(2, Pose2(2.1, 0.0, 0.0));

    std::cout << "Initial error: " 
              << graph.error(initial) << std::endl;

    // 7. LM 优化求解
    LevenbergMarquardtOptimizer optimizer(graph, initial);
    Values result = optimizer.optimize();

    std::cout << "Final error: " 
              << graph.error(result) << std::endl;

    // 8. 输出优化结果
    for (int i = 0; i < 3; ++i) {
        Pose2 pose = result.at<Pose2>(i);
        std::cout << "Pose " << i
                  << ": x=" << pose.x()
                  << ", y=" << pose.y()
                  << ", theta=" << pose.theta()
                  << std::endl;
    }

    // 9. 查询边缘协方差
    Marginals marginals(graph, result);
    std::cout << "Pose 2 covariance:\n"
              << marginals.marginalCovariance(2)
              << std::endl;

    return 0;
}

该例基于 GTSAM 4.x 接口,优化后x坐标应接近:

x0≈0, x1≈1.0111, x2≈1.9222x_0\approx0,\ x_1\approx1.0111,\ x_2\approx1.9222x0≈0, x1≈1.0111, x2≈1.9222

10.2 代码与数学的对应关系

  • graph.add(...) → 增加概率约束 ϕk(Xk)\phi_k(X_k)ϕk(Xk)
  • noiseModel::Diagonal::Sigmas(...) → 指定标准差,对应信息矩阵 Ωk=Σk−1\Omega_k=\Sigma_k^{-1}Ωk=Σk−1
  • initial.insert(...) → 设置初值 X(0)X^{(0)}X(0)
  • optimizer.optimize() → 执行非线性迭代优化
  • marginals.marginalCovariance() → 高斯近似下的边缘协方差

10.3 主流优化库对比

框架 定位 场景
GTSAM 概率因子图、平滑与增量推断 SLAM、VIO、多传感器融合
g2o 图结构非线性最小二乘 位姿图、SLAM、BA
Ceres Solver 通用非线性最小二乘、自动微分 BA、标定、通用参数估计

GTSAM更适合构建完整概率状态估计系统;Ceres更灵活,适合从残差出发构建通用优化问题。三者能力有重叠,无绝对边界。


十一、因子图优化常见问题

11.1 规范自由度(Gauge Freedom)

若所有约束都是相对位姿,则全局刚体变换不改变残差:

Ti′=GTi  ⟹  (Ti′)−1Tj′=Ti−1Tj T_i'=GT_i \implies (T_i')^{-1}T_j'=T_i^{-1}T_j Ti′=GTi⟹(Ti′)−1Tj′=Ti−1Tj

这导致信息矩阵秩亏,求解或协方差计算出错。

解决方案:添加先验、固定一个参考位姿,或使用专门的规范自由度处理方法。单目视觉还存在尺度不可观测性。

11.2 噪声模型设置不合理

若视觉实际精度3像素,却设为0.1像素,相当于告诉优化器该观测绝对可信,会拉偏结果。工程中应通过标定、残差统计估计噪声,而非随意设置。

11.3 异常值与稳健核

平方损失会放大大残差的影响,错误匹配、误回环会严重破坏结果。可引入稳健核函数:

E(X)=12∑kρ(rk⊤Ωkrk) E(X)=\frac12\sum_k \rho\left(r_k^\top\Omega_k r_k\right) E(X)=21k∑ρ(rk⊤Ωkrk)

例如Huber核:

ρ(s)={s,s≤δ2 2δs−δ2,s>δ2 \rho(s)= \begin{cases} s,&s\le\delta^2\ 2\delta\sqrt{s}-\delta^2,&s>\delta^2 \end{cases} ρ(s)={s,s≤δ2 2δs −δ2,s>δ2

稳健核能降低异常值权重,但不能替代前端的外点剔除和正确数据关联。

11.4 观测重复计数

若同一批视觉观测既被用来估计相对位姿,又被当作重投影因子加入图中,会重复计算信息,导致过度自信。因子数量增加不等于独立信息增加。

11.5 时间同步与外参误差

多传感器时间不同步、外参标定错误,会带来系统性偏差,优化算法再精准也无法消除。时间同步、运动插值、外参标定是状态估计系统的基础,而非无关前处理。

11.6 可观测性不足

约束多不代表所有状态都能估计。例如手眼标定+时间偏移估计,若机器人运动单一,参数会相互耦合无法区分。优化器不能创造观测中没有的信息。


十二、工业机器人视觉中的因子图应用

以手眼标定场景为例:机器人末端安装相机,TBT(t)T_{BT}(t)TBT(t) 为末端到Base的变换,TTCT_{TC}TTC 为相机到末端的固定外参,则相机到Base变换:

TBC(t)=TBT(t)TTC T_{BC}(t)=T_{BT}(t)T_{TC} TBC(t)=TBT(t)TTC

已知空间点 PjP_jPj,图像预测位置:

z^ij=π(TBT(ti+Δt)TTC−1Pj) \hat z_{ij}= \pi\left( T_{BT}(t_i+\\Delta t)T_{TC}^{-1}P_j \right) z^ij=π(TBT(ti+Δt)TTC−1Pj)

其中 Δt\Delta tΔt 为相机与机器人的时间偏移。

可联合优化的变量:

X=TTC,Δt,P1,...,Pn \boxed{ X={T_{TC},\Delta t,P_1,\ldots,P_n} } X=TTC,Δt,P1,...,Pn

目标函数:

E(X)=∑i,j∣z^ij(X)−zij∣∗Ω∗ij2+Eprior E(X)= \sum_{i,j} \left| \hat z_{ij}(X)-z_{ij} \right|*{\Omega*{ij}}^2 + E_{\text{prior}} E(X)=i,j∑∣z^ij(X)−zij∣∗Ω∗ij2+Eprior

若机器人位姿也有不确定性,可将位姿也加入优化,用运动学作为约束。

先从离线标定、误差诊断入手,验证充分后再接入实时控制闭环。


十三、因子图 vs EKF vs 传统最小二乘

对比维度 EKF 滤波 因子图平滑优化 传统最小二乘(LS)
核心思想 贝叶斯递推状态估计 概率图建模 + 联合优化 最小化观测残差平方和
数学基础 贝叶斯滤波、线性高斯近似 最大后验估计(MAP)、非线性最小二乘 最小二乘、数值优化
计算方式 预测 + 更新两步递推 构建约束因子,联合优化状态 构建残差函数,求解最优参数
状态组织 通常维护当前状态及协方差 多时刻状态节点 + 约束因子 单个或多个待估参数
历史状态 标准 EKF 仅维护当前状态 可保留多个历史状态 取决于优化变量设计
历史修正 标准 EKF 不回溯修正 可回溯优化历史状态 保留历史变量时可整体重优化
非线性处理 在当前估计点局部线性化 可迭代重新线性化 非线性 LS 可迭代线性化
线性化误差 标准 EKF 通常不重新修正历史线性化 可通过迭代重新线性化降低误差 非线性 LS 可通过迭代降低误差
迟到观测 需要缓存、回滚或特殊处理 可添加对应历史时刻的因子 可加入观测后重新求解
多传感器融合 通过观测更新融合 不同传感器构建不同因子 将不同观测残差纳入统一目标
异步、多频率观测 支持,但需处理时间对齐与更新顺序 易于表达跨时刻约束 可建模,需自行组织残差与变量
闭环约束 标准 EKF 不便于全局轨迹回溯修正 天然适合回环约束 联合优化历史位姿时也支持
异常值处理 创新门限、异常观测剔除、鲁棒更新 鲁棒核函数、异常因子剔除 鲁棒损失函数、加权最小二乘
不确定性表达 显式维护状态协方差矩阵 通过噪声模型描述,可计算边缘协方差 可根据噪声模型及 Hessian 近似估计
实时性 通常较高,适合高频递推 取决于优化窗口及增量求解策略 取决于问题规模及求解器
计算开销 通常较低,受状态维度影响 受图规模、稀疏性及求解策略影响 受参数规模、残差数量及算法影响
内存开销 通常较低 通常需要存储历史节点及因子 取决于历史数据及变量规模
增量计算 天然递推 支持 iSAM2 等增量优化算法 传统批量 LS 通常需要重新求解
稀疏性利用 一般不依赖稀疏图结构 可利用因子图稀疏结构加速求解 可通过稀疏矩阵方法加速求解
初始值依赖 依赖合理的初始状态与协方差 非线性优化通常依赖较好的初始值 非线性 LS 通常依赖较好的初始值
全局最优性 不保证全局最优 非线性优化通常只能保证局部收敛 线性 LS 可求全局最优;非线性 LS 通常为局部最优
典型算法 EKF、IEKF、ESKF iSAM2、固定滞后平滑、批量图优化 线性 LS、GN、LM、加权 LS
典型应用 IMU 融合、目标跟踪、实时运动状态估计 SLAM、VIO、多传感器融合、轨迹优化 PnP 精化、相机标定、参数拟合
主要优势 高频、低延迟、资源占用相对可控 约束表达灵活、支持历史修正、适合全局一致性优化 数学直观、通用性强、实现灵活
主要局限 线性化误差、估计一致性问题、历史修正困难 图管理复杂,计算和内存开销可能较高 复杂时序约束需要自行组织和管理
适用场景 高频实时状态估计 多传感器联合估计、历史轨迹优化 参数拟合、局部优化、批量估计

注意:因子图是建模方式 ,EKF是估计算法,不在完全同一层级。因子图既可做批量平滑,也可做增量推断;线性高斯下也可对应滤波解释。

实际选型需综合实时性、计算资源、是否需要历史修正、问题结构等因素。


总结

因子图优化的知识树:

复制代码
因子图优化 Factor Graph Optimization
├─ 概率理论:贝叶斯推断 · MAP · 高斯噪声 · 信息矩阵
├─ 图模型:变量节点 · 因子节点 · 约束分解 · 稀疏结构
├─ 数值优化:雅可比 · Hessian近似 · GN · LM · Dogleg
├─ 线性代数:QR · Cholesky · Schur补 · 变量消元 · Fill-in
├─ 几何数学:SO(3) · SE(3) · 李代数 · 流形扰动
├─ 实时估计:滑动窗口 · 边缘化 · iSAM2 · Bayes Tree
├─ 实际应用:SLAM · BA · VIO · LIO · 传感器标定
└─ 工程实践:GTSAM · g2o · Ceres · 噪声建模 · 异常值处理

四层认知框架

  1. 建模层:有哪些未知量?有哪些观测?它们的依赖关系是什么?
  2. 概率层:观测置信度如何?如何从概率模型导出残差和权重?
  3. 数值层:如何线性化、求雅可比、利用稀疏性求解最优状态?
  4. 系统层:初始化、时间同步、可观测性、外点剔除、实时性如何保障?

对于机器人开发者,最重要的是打通这条主线:

MAP估计 → 残差定义 → 雅可比推导 → 正规方程 → 李群更新 → 稀疏求解

吃透这条主线,再去学SLAM、BA、VIO、标定等应用,都会发现底层逻辑高度一致。

相关推荐
geats人山人海34 分钟前
现代c++4.1
java·开发语言·c++
硅谷秋水1 小时前
MorphMimic 方法分析
人工智能·机器学习·计算机视觉·机器人
Pniubi1 小时前
贪心算法入门
算法·贪心算法
MobotStone1 小时前
AI 能不能越用越会用?从“改答案”到“改进自己”,读懂 RSI
人工智能·算法
复杂网络与机器学习2 小时前
Meta 重写 K-Means:25 轮压成 2 轮,GPU 快 45 倍
人工智能·算法·数据挖掘
lunar_coder2 小时前
【算法】前缀和:把 O(n) 的重复劳动变成 O(1) 的一次查询
c++·算法
Pniubi2 小时前
力扣19 . 删除表的倒数第N个节点(栈)
算法·leetcode·职场和发展
青少儿编程课堂3 小时前
第十八届蓝桥杯青少组图形化 + 全国青少年信息素养大赛 + 电子学会图形化考级:校园三消乐大挑战解析
c++·python·算法·bfs·信息学竞赛
万俟淋曦3 小时前
[ROS 系列学习教程] 建模与仿真 - 使用 ros_control 控制六轴串联机械臂
人工智能·ai·机器人·ros·机械臂·robotics·具身智能