酸菜鱼的SLAM之旅（3）

2019年3月24日：非线性优化

状态估计问题

最大后验和最大似然

经典SLAM模型：

观测方程：

考虑数据受噪声影响:

状态估计问题：

• 扩展卡尔曼滤波器（EKF）：关心当前时刻的状态估计X_k,而对之前的状态则不多考虑。

• 非线性优化方法:使用所有时刻采集到的数据进行状态估计。（主流方法)

  • 似然：在现在的位姿下，可能产生怎样的观测数据

  • 最大似然估计（Maximize Likelihood Estimation,MLE)：在什么样的状态下，最可能产生现在观测到的数据

  • 求解最大后验概率 - 最大化似然和先验的乘积

  • arg maxP(z|x) —— 最大似然估计

最小二乘

• 最小化负对数：对等式两边同时取负对数 -ln(f(x))

• 总体意义下的最小二乘问题（Least Square Problem）

  • 最优解等价于状态的最大似然估计。
  • 但由于噪声存在使得估计的轨迹和地图代入SLAM运动、观测方程时不会完美成立 —— 需要把状态的估计值进行微调，使得整体误差下降（有限度，会到达一个极小值）。

• SLAM中的最小二乘问题——(6.12)式为目标函数

  • 总体状态变量维数高，每个误差项简单（仅与一两个状态变量有关)
  • 每个误差项是一个小规模的约束，称每个误差项对应的优化变量为参数块（Parameter Block)
  • 线性近似、雅可比矩阵
  • 稀疏性
  • 李代数-无约束
  • 平方形式（二范数）度量误差 - 欧氏空间中距离的平方

非线性最小二乘

不方便直接求解的最小二乘问题使用迭代优化：

问题： 确定增量ΔX_k

一阶和二阶梯度法

1. 最速下降法（一阶）、牛顿法（二阶）
   • 将目标函数在x附近进行泰勒展开
   • 参数：雅可比矩阵J、海塞（Hessian）矩阵H、步长 λ
   • 选择保留泰勒展开的一阶/二阶项 —— 对应一阶梯度法/二阶梯度法
   • 问题：过于贪心，容易走出锯齿路线，增加迭代次数

Gauss-Newtown（高斯-牛顿）

思想：将f（x)进行一阶的泰勒展开（不是目标函数f(x)²）

问题:

• 原则上要求所用的近似H矩阵可逆且正定，但实际上（J^TJ）只有半正定性。
• 可能出现J^TJ为奇异矩阵或者病态（ill-condition)的情况，此时增量稳定性较差，导致算法不收敛。
• 求出来的步长Δx太大，会导致采用的局部近似不够准确，无法保证迭代的结果是收敛。

Levenberg-Marquadt

信赖区域方法（Trust Region Method）：在信赖区域里边，我们认为近似是有效的；出了这个区域，近似可能会出问题。

确认信赖区域的范围：根据近似模型和实际函数之间的差异来确定，差异小→范围尽可能大；差异大→近似范围缩小

改良后的非线性优化框架:

实践（挖坑，且不打算填）

Ceres

Ceres库面向通用的最小二乘问题的求解：

• 用户定义优化问题，设置选项，输入至Ceres求解

---

g2o（General Graphic Optimization)

基于图优化的库。