📅 2026-04-16🤖 claude-opus-4factor-graphgraph-optimizationslamtrajectorynonlinear-least-squares

图优化 (Factor Graph Optimization) 研究报告

1. 直觉理解

想象在纸上画一条轨迹，每个时刻的位置是一个钉子，钉子之间用橡皮筋连接：

RSSI观测          RSSI观测          RSSI观测
  ↓弹簧              ↓弹簧              ↓弹簧
  x(1) ——橡皮筋—— x(2) ——橡皮筋—— x(3) ——橡皮筋—— x(4)
  • 弹簧（RSSI 因子）：把每个钉子拉向好心人观测暗示的位置
  • 橡皮筋（运动因子）：让相邻钉子不要离太远（人不能瞬移）

所有弹簧和橡皮筋同时作用，系统松手后自动找到总弹性势能最小的状态 → 这就是图优化的解。

2. 形式化定义

2.1 因子图结构

因子图 = 变量节点（圆）+ 因子节点（方）+ 边

变量节点：待求解的未知量
  ○ x(1), x(2), ..., x(T)  — 每个时刻的位置

因子节点：约束/观测
  ■ RSSI 因子：某好心人在某时刻的距离观测
  ■ 运动因子：相邻时刻的位移约束
  ■ 速度平滑因子：速度不应突变

2.2 图结构示意

好心人A  好心人B     好心人C  好心人D     好心人E
  ■        ■           ■        ■           ■
   \      /             \      /             |
    ○ x(1) ——■—— ○ x(2) ——■—— ○ x(3) ——■—— ○ x(4)
              运动        运动        运动
              因子        因子        因子

注意：x(1) 和 x(2) 的好心人完全不同——这正是众包场景的特点。

2.3 目标函数

X^=arg min⁡x(1)...x(T)∑t∑i∥f(rit)−∥x(t)−Iit∥∥Σr2⏟RSSI 因子+∑t∥x(t)−x(t ⁣− ⁣1)∥Σm2⏟运动因子\hat{X} = \argmin_{x(1)...x(T)} \underbrace{\sum_{t}\sum_{i} |f(r_i^t) - |x(t) - I_i^t||^2_{\Sigma_r}}{\text{RSSI 因子}} + \underbrace{\sum{t} |x(t) - x(t!-!1)|^2_{\Sigma_m}}_{\text{运动因子}}

其中 ∥⋅∥Σ2|\cdot|^2_\Sigma 表示马氏距离（按协方差加权）。

3. 与其他方法的本质区别

3.1 信息流向对比

单帧优化:    t=3 的观测 → x(3)     只用当前帧，每帧独立
EKF:         x(1) → x(2) → x(3)   前向累积，只能用过去信息
图优化:      x(1) ⇄ x(2) ⇄ x(3)   全局双向，未来观测能修正过去

关键区别：EKF 是因果的（只能前向），图优化是非因果的（前后向信息自由流动）。

3.2 具体例子

假设目标沿直线走 4 步，每步遇到不同好心人：

t=1: 好心人 A(5m), B(8m)  → 两个观测，勉强定位
t=2: 好心人 C(3m)         → 一个观测，单帧无法定位！
t=3: 好心人 D(6m), E(4m)  → 两个观测
t=4: 好心人 F(7m)         → 一个观测，单帧无法定位！
方法 t=2 处理 原因
单帧优化 无解 1 个观测无法确定 2D 位置
EKF 靠 t=1 预测 + 1 个观测修正，精度有限 只有前向信息
图优化 t=1 + t=3 约束 + 自身观测 + 运动平滑，等效 5+ 约束 双向信息流

4. 为什么特别适合移动众包场景

移动场景难点 图优化如何解决
单帧好心人太少（欠定） 运动因子从相邻帧"借"信息，即使某帧只有 1 个好心人，相邻帧观测也能约束
好心人每帧都不同 因子图天然支持：每个 RSSI 因子独立连接到对应时刻，不要求跨帧一致
RSSI 有异常值 可用鲁棒核函数（Huber/Cauchy）替代平方误差，自动降权异常因子
离线处理 图优化天然是批处理——有完整数据后一次求解，充分利用离线优势
运动模式未知 运动因子协方差 Σ_m 控制"松紧"：大 → 允许快速运动，小 → 强制平滑
易扩展 新增约束只需加因子节点（地图约束、速度上限等），不改框架

5. 因子类型详解

5.1 RSSI 距离因子

误差函数: e_rssi = f(r_i^t) - ‖x(t) - I_i^t‖

其中:
  f(r_i^t): RSSI → 距离映射（路径损耗模型）
  I_i^t: 好心人 i 在时刻 t 的位置
  x(t): 待估目标位置

信息矩阵: Σ_r⁻¹，反映 RSSI 观测的可靠性
  近距好心人 → 小方差 → 大权重
  远距好心人 → 大方差 → 小权重

5.2 运动因子（匀速模型）

误差函数: e_motion = x(t) - x(t-1) - v(t-1) · Δt

信息矩阵: Σ_m⁻¹，控制运动平滑程度
  行人（慢速）→ 小 Σ_m → 强平滑
  车辆（快速）→ 大 Σ_m → 弱约束

5.3 速度平滑因子（可选）

误差函数: e_smooth = v(t) - v(t-1)

约束速度不突变（匀加速假设）

5.4 鲁棒核函数

标准平方误差对异常值敏感，替换为鲁棒核：

核函数 公式 特点
平方（L2） ρ(e) = e² 异常值影响大
Huber ρ(e) = e²（小）/ 2δ|e|-δ²（大） 大误差线性惩罚
Cauchy ρ(e) = c² log(1 + e²/c²) 大误差影响趋于常数
Tukey ρ(e) = 截断 完全剔除大异常值

众包场景推荐 Huber 或 Cauchy：NLOS 好心人产生系统性大误差，需降权但不完全丢弃。

5.5 可扩展因子（创新点方向）

因子类型 约束内容 适用场景
地图约束因子 目标不能穿墙/进水 有地图信息时
速度上限因子 ‖v(t)‖ ≤ v_max 已知运动模式（行人 ~1.5m/s）
高度因子 z(t) = const 或 z 连续 单层建筑
好心人共观测因子 两个好心人同时观测到目标 → 距离三角约束 密集好心人
锚点不确定性因子 I_i 本身含误差，联合优化 好心人定位精度差

6. 求解方法

6.1 数学本质

图优化目标函数是非线性最小二乘问题。将所有因子误差堆叠：

X^=arg min⁡X∑k∥ek(X)∥Σk2=arg min⁡X∥r(X)∥2\hat{X} = \argmin_X \sum_k |e_k(X)|^2_{\Sigma_k} = \argmin_X |r(X)|^2

其中 r(X) 是残差向量。

6.2 迭代求解

每次迭代在当前估计 X₀ 处线性化：

r(X0+ΔX)≈r(X0)+JΔXr(X_0 + \Delta X) \approx r(X_0) + J \Delta X

ΔX∗=arg min⁡∥r(X0)+JΔX∥2\Delta X^* = \argmin |r(X_0) + J \Delta X|^2

正规方程：(JTJ)ΔX=−JTr(J^T J) \Delta X = -J^T r → Gauss-Newton 步

求解器 策略 特点
Gauss-Newton 直接解正规方程 快，需好初始值
Levenberg-Marquardt (JTJ+λI)ΔX=−JTr(J^T J + \lambda I) \Delta X = -J^T r 更鲁棒，GN + 梯度下降混合
Dogleg 信赖域方法 收敛性好

6.3 稀疏性——高效求解的关键

因子图的雅可比矩阵 J 极度稀疏：

  • RSSI 因子只涉及 1 个位置变量 → J 的该行只有 2 个非零元素
  • 运动因子只涉及 2 个相邻变量 → 4 个非零元素

正规方程 JTJJ^TJ 呈带状稀疏结构，可用稀疏 Cholesky 分解高效求解。

T=1000 帧、每帧 5 个好心人 → 2000 个变量、6000 个因子，毫秒级求解。

6.4 初始值策略

非线性优化需要合理初始值：

  1. EKF 前向滤波：先跑一遍 EKF，用其输出作为图优化初始值
  2. 独立单帧解：对观测充足的帧单独求解，缺失帧线性插值
  3. 粗糙轨迹：用 RSSI 最强的好心人位置作为粗估计

推荐方案 1：EKF 提供初始值 → 图优化全局精化。

7. 常用工具库

库 语言 特点 许可
Ceres Solver C++ Google 出品，通用非线性最小二乘，文档优秀 BSD
GTSAM C++ (Python 绑定) Georgia Tech，专为因子图设计，SLAM 领域标准 BSD
g2o C++ 图优化专用，轻量高效 BSD
scipy.optimize Python least_squares 函数，适合原型验证 BSD
jaxopt Python/JAX 可微优化，支持 GPU 加速 Apache 2.0

本项目推荐：原型用 scipy → 正式实现用 Ceres 或 GTSAM。

8. 与 SLAM 的关系

本项目和 SLAM（同步定位与建图）是完全相同的数学框架：

SLAM 概念 本项目对应
机器人位姿 目标位置 x(t)
路标观测 好心人 RSSI 观测
里程计因子 运动平滑因子
回环检测 同一好心人在不同时刻被观测（较少见）
路标位置 好心人位置 I_i（已知，无需联合估计）

区别：SLAM 需要同时估计路标位置（建图），本项目好心人位置已知（通过 GNSS），问题更简单。

可直接借用 SLAM 领域 20 年的成熟工具链和理论成果。

9. 本项目的图优化方案设计

9.1 推荐架构

输入: 时间序列 {t, 好心人ID, 好心人位置, RSSI}

Pipeline:
  1. 预处理: RSSI 滤波、时间对齐、异常检测
  2. 初始化: EKF 前向滤波 → 粗轨迹
  3. 图构建:
     ├── 每帧添加位置变量节点 x(t)
     ├── 每个 RSSI 观测添加距离因子（Huber 核）
     ├── 相邻帧添加运动因子
     └── 可选: 速度平滑因子、地图约束因子
  4. 求解: Levenberg-Marquardt
  5. 后处理: 异常因子检测 → 剔除 → 重解（迭代 2-3 次）

输出: 优化后的完整轨迹 x(1)...x(T)

9.2 创新点嵌入

创新方向 如何嵌入图优化
可学习 f(r) 距离因子中使用学习到的路径损耗模型
可学习 w_i RSSI 因子的信息矩阵 Σ_r⁻¹ 由网络预测
自适应运动模型 运动因子的 Σ_m 根据场景分类动态调整
鲁棒核参数 Huber 核的 δ 参数可学习或自适应调节

9.3 静态场景统一

静态场景 = 退化为单节点的图：

x^=arg min⁡∑i∥f(ri)−∥x−Ii∥∥Σr2\hat{x} = \argmin \sum_i |f(r_i) - |x - I_i||^2_{\Sigma_r}

与题目原始公式完全一致。图优化框架天然统一了静态和移动两种场景。

10. 复杂度分析

规模参数 典型值
时间帧数 T 100–10000
每帧好心人数 1–20
位置变量维度 2T（2D）或 3T（3D）
因子总数 ~10T
求解时间 毫秒–秒级（稀疏求解）

瓶颈不在求解，而在 RSSI 预处理和初始值质量。

11. 为什么"图优化"适合移动场景

11.1 从痛点出发

移动场景核心问题：很多时刻单帧观测不够（好心人太少），单独无法定位。

t=1: 好心人 A、B → 能定位（2个观测）
t=2: 好心人 C   → 不能定位（1个观测，2D需要≥2个）
t=3: 好心人 D、E → 能定位
t=4: 好心人 F   → 不能定位

最朴素的想法："t=2 观测不够，但人不能瞬移——t=1 和 t=3 的位置能限制 t=2 在哪。"

11.2 解决思路对比

EKF（只能前向传递）：x(1) 定位完 → 预测 x(2) → 用 C 修正 → 预测 x(3)... 问题：t=3 有很好的观测，但没法回头帮 t=2。

图优化（所有时刻一起解）：把所有约束堆在一起同时求解。t=2 同时受 t=1、t=3 的约束 + 自身观测 + 运动平滑，等效有 5+ 个约束。且 t=3 的好观测能回溯改善 t=2。

11.3 因果链

痛点：移动场景单帧观测不够
  ↓
想法：借相邻帧信息
  ↓
方法：把所有帧的所有约束写成一个大优化问题一起解
  ↓
发现：这个大优化问题的约束结构天然是个稀疏图
  ↓
名字：因子图优化

不是"因为有个图所以适合"，是"把问题自然地写出来后发现它恰好是个图结构"。

11.4 "图"的实际作用

"图"本身不是方法，而是描述约束结构的语言：

  • 运动因子只连相邻两帧 → 雅可比矩阵是带状稀疏的 → 求解快
  • 如果不在乎效率，完全可以不提"图"，直接用 scipy 的 least_squares 解那个大方程——结果完全一样，只是慢

12. "图"的含义辨析

12.1 此图非彼图

Factor Graph 里的图是二部图 (bipartite graph)：

  • ○ 变量节点和 ■ 因子节点两类
  • 边只连接不同类节点
  • 不涉及最短路径、图遍历等图算法

12.2 与概率图模型的关系

概率图模型（大家族）
  ├── 贝叶斯网络
  ├── 马尔可夫随机场
  └── 因子图  ←── 数学上最通用的一种
        │
        │  当因子都是高斯时，-log 后变成平方误差
        │
        ▼
  SLAM "图优化" ←── 是因子图的一个特例/应用

关键等式：

arg max⁡∏kfk  →−log⁡  arg min⁡∑k∥ek∥Σk2\argmax \prod_k f_k ;\xrightarrow{-\log}; \argmin \sum_k |e_k|^2_{\Sigma_k}

SLAM 图优化 = 因子图上的 MAP 推断（所有因子为高斯时）。两种说法数学等价，概率社区叫"推断"，SLAM 社区叫"优化"。

12.3 学术渊源

两条独立学术线：

线 1：概率图模型（信息论/ML 社区）

  • Kschischang, Frey & Loeliger (2001), IEEE Trans. on Information Theory — 因子图形式化定义
  • Loeliger (2004), IEEE Signal Processing Magazine — 因子图推广到信号处理

线 2：SLAM / 机器人定位（robotics 社区）

  • Dellaert & Kaess (~2006) 发现 SLAM 后验可写成 factor graph
  • Kaess et al. (2008), IEEE TRO — iSAM: Incremental Smoothing and Mapping

桥梁论文：

  • Dellaert (2012), Georgia Tech Technical Report — Factor Graphs and GTSAM: A Hands-on Introduction

BLE/无线定位相关：

  • Wymeersch, Lien & Win (2009), Proceedings of the IEEE — Cooperative Localization in Wireless Networks（协作定位建模为 factor graph）
  • 搜索关键词建议："cooperative localization factor graph" 或 "graph-based localization wireless"

12.4 提案引用建议

如果提案中使用图优化框架，引用 Wymeersch 2009 和 Dellaert 2012 即可建立理论基础。