关于“SLAM本质上是什么问题”的一点思考
2021-12-16 05:43:55
SLAM本质上是一个优化问题。具体分析如下：
状态估计视角下的SLAMSLAM首先是一个状态估计问题，在前端需从k-1时刻的状态量出发推断k时刻的状态。简单情况下，状态仅包含位姿，若用四元数表达旋转，状态向量是7维（位置3维 + 方向4维）。完成状态估计的信息有k-1时刻到k时刻之间的运动测量和k时刻的环境观测。在贝叶斯滤波框架下，状态估计问题可通过公式表示为：$$bel( bold x_{k} ) = p(bold x_{k}|z_k) propto p(z_k|bold x_{k} ) p(bold x_{k}|bold x_{k-1} ,u_k )$$其中，置信度是$bold x_{k}$的后验概率，与似然乘以先验成正比，先验即状态转移概率（预测），似然即测量概率（更新），且滤波方法下状态向量需同时维护一个对应的协方差矩阵。我们要做的是求一个使后验概率最大化的$bold x_{k}^$作为$bold x_{k}$的最优状态估计，公式为：$$bold x_{k}^ = arg min_{bold x_{k}}{p(bold x_{k}|z_k)} = arg min_{bold x_{k}}{ p(z_k|bold x_{k} ) p(bold x_{k}|bold x_{k-1} ,u_k ) }$$
最大后验概率与最小二乘的等价性最大后验概率估计问题可转化为完全等价的最小二乘问题。对任何一个高斯分布取负对数，会得到一个等价的二次型，高斯分布的最大化对应着二次型项的最小化。“$bold x_{k}$的后验概率”是高斯分布，对其套用取负对数操作，考虑到测量噪声，问题转化链条为后验概率最大化 = 二次型项最小化 = 关于所有误差项的最小二乘问题。这个最小二乘问题的求解需用到优化方法，以状态量为自变量，求总体误差的最小值，具体优化求解策略包括最速下降法、牛顿法、高斯 - 牛顿法（G - N）、基于信任区域的L - M法等，此时已切换到用优化方法解决问题，而非滤波。
优化方法在SLAM中的优势及本质体现- 优势：
贝叶斯滤波方法依赖马尔可夫性，每一时刻的状态仅与上一时刻有关，上一时刻之前的测量信息将被丢掉。
优化方法可考虑所有时刻的测量信息，把所有过往时刻（1, 2, ... k - 1, k）的误差项都添加到目标函数中，以所有时刻的状态为自变量，求目标函数（总体误差）的最小值，得到整体的最优状态估计，精度大大提升。虽然考虑所有时刻信息会使维度非常大且随时间推移越来越大，但目标函数中的单个误差项并非与所有时刻的状态都相关，求解过程中的相关矩阵会呈现出稀疏性，允许对高维问题实时求解。
- 具体应用形式：
滑窗优化：可只取相近若干时刻的状态进行优化，而非所有时刻，构建“滑窗”。更高级的做法是对滑窗边缘的时刻进行边缘化处理，使其融合更早时刻的信息，代表作如VINS。
后端全局优化：优化方法用于后端时通常是全局优化，即考虑所有时刻的状态和所有测量误差项的优化。发生闭环时，当前时刻状态和历史上某一时刻状态之间构建的误差项往往很大，对全局误差进行最小化时，需“微调”全局各个时刻的状态量，将误差“分散”到各个时刻，实现全局误差最小化，此“微调”过程也由优化完成。
- BA优化：考虑了所有测量状态量的优化，例如针对一个特征点进行优化。
- pose graph图优化：简化约束的构建，并不考虑所有的测量元素。
- factor graph因子图优化：更先进的思想，从贝叶斯网络衍生出因子图网络，典型应用于机器人领域大规模SLAM问题。
由于优化方法在SLAM中具有诸多优势，且视觉里程计、激光里程计、轮式、imu等的作用是构建误差项（约束），所以从各个角度来看，一个现代的SLAM系统本质上就是一个优化的系统，SLAM本质上就是一个优化问题。
热门标签
