因子图优化是现代机器人状态估计与多传感器融合的技术,广泛应用于以下场景:
- 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∗=argminX∑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∗=argmaxXP(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∗=argmaxXP(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,存在四类信息:
- 初始位姿的先验信息
- X0X_0X0 到 X1X_1X1 的里程计观测
- X1X_1X1 到 X2X_2X2 的里程计观测
- 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∗=argmaxX∏kϕk(Xk) =argminX−log∏kϕk(Xk) =argminX∑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∗=argminX12∑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∗=argminX12∑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 高斯牛顿法流程
- 状态初始化 X(0)X^{(0)}X(0)
- 计算所有因子残差 rkr_krk 和雅可比 JkJ_kJk
- 累加构造 H=J⊤ΩJH=J^\top\Omega JH=J⊤ΩJ、g=J⊤Ωrg=J^\top\Omega rg=J⊤Ωr
- 求解正规方程 HΔX=−gH\Delta X=-gHΔX=−g
- 更新状态 X←X⊞ΔXX\leftarrow X\boxplus\Delta XX←X⊞ΔX(⊞\boxplus⊞ 对应流形上的更新)
- 判断收敛:满足则输出,否则返回步骤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(贝叶斯树) 存储变量消元后的概率结构,复用历史计算结果。
新因子加入时的流程:
- 定位受新约束影响的贝叶斯树分支
- 对相关变量按需重新线性化
- 仅对受影响部分重新消元更新
- 未受影响部分完全复用
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−1initial.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 · 噪声建模 · 异常值处理
四层认知框架
- 建模层:有哪些未知量?有哪些观测?它们的依赖关系是什么?
- 概率层:观测置信度如何?如何从概率模型导出残差和权重?
- 数值层:如何线性化、求雅可比、利用稀疏性求解最优状态?
- 系统层:初始化、时间同步、可观测性、外点剔除、实时性如何保障?
对于机器人开发者,最重要的是打通这条主线:
MAP估计 → 残差定义 → 雅可比推导 → 正规方程 → 李群更新 → 稀疏求解
吃透这条主线,再去学SLAM、BA、VIO、标定等应用,都会发现底层逻辑高度一致。