cartographer三部曲（二）：Efficient Sparse Pose Adjustment for 2D Mapping-优快云博客

本文链接：https://blog.youkuaiyun.com/RoboChengzi/article/details/106392112

本文探讨了SPA算法在SLAM问题中的应用，通过构建图结构优化全局位姿，利用矩阵稀疏性加速求解。文章详细介绍了SPA算法的原理，包括误差公式、线性系统求解以及稀疏矩阵的存储优化。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

前言：前端多技巧，后端多理论是slam的一个特性。因此要理解本论文需要一定的理论基础。即需要对加权LM算法十分熟悉，对矩阵求导、SO（3）上的导数、稀疏线性方程组求解等理论非常熟悉，还有slam问题中图结构的构造。

总体评价：创新点不大，将视觉中的BA问题直接类比到了激光中，使用了一些工程技巧提高了算法性能。

主要工作：采用和视觉BA问题类似的方法构建图结构进行优化，利用矩阵的稀疏结构对算法进行加速。

优势：

考虑约束中的协方差信息使结构更加精确
SPA对于初始值不敏感，只有非常小的概率陷入局部最优
收敛非常快，仅仅需要几次迭代即可
不像EKF或者UKF将非线性问题转换成线性问题求解，SPA是完全非线性的估计

算法实现

文中采用著名的L-M算法作为框架，利用了矩阵的稀疏性进行加速求解，称为SPA。

误差公式

优化变量：一系列的全局位姿 $ci=[t1,θi]=[xi,yi,θi]c_i=[t_1,\theta_i]=[x_i,y_i,\theta_i]$

约束：约束是一个节点 $c_j$ 到另外一个参考节点 $c_i$ 的观测量，及相对变换关系 $z‾ij\overline{z}_{ij}$ 和协方差矩阵 $Σij\Sigma_{ij}$ 。

误差计算

假设 $c_i$ 和 $c_j$ 两节点间有一个约束 $z‾ij\overline{z}_{ij}$ ，由于 $z‾ij\overline{z}_{ij}$ 代表在i坐标系下j节点的测量位置，需要计算出图中节点 $c_j$ 在 $c_i$ 系下的位置
$h(c_i,c_j) \equiv \left\{\begin{array}{l} R_{i}^{\top}\left(t_{j}-t_{i}\right) \\ \theta_{j}-\theta_{i} \end{array}\right.$
两者相减便是误差
$e_{ij} \equiv \overline{z}_{ij}-h(c_i,c_j)$
最终得到最小二乘误差为
$E(c,p)=\sum_{ij}e_{ij}^T\Sigma_{ij}^{-1}e_{ij} = \sum_{i j} e_{i j}^{\top} \Lambda_{i j} e_{i j}$

线性系统

采用LM算法求解该非线性最小二乘问题需要在迭代过程中不断求解如下线性方程
$(H+λdiag⁡H)Δx=J⊤Λe(\mathbf{H}+\lambda \operatorname{diag} \mathbf{H}) \Delta \mathbf{x}=\mathbf{J}^{\top} \mathbf{\Lambda} \mathbf{e}$
其中 $H=J⊤ΛJ\mathbf{H}=\mathbf{J}^{\top}\mathbf{\Lambda}\mathbf{J}$ ,因此只需要求解误差的雅可比即 $J\mathbf{J}$ 即可构建方程并求解。

误差Jacobians

$∂eij∂ti≡[−Ri⊤00]∂eij∂θi≡[−∂Ri⊤/∂θi(tj−ti)−1]∂eij∂tj≡[Ri⊤00]∂eij∂θj≡[001]⊤\begin{array}{l} \frac{\partial e_{i j}}{\partial t_{i}} \equiv\left[\begin{array}{c} -R_{i}^{\top} \\ 00 \end{array}\right] \quad \frac{\partial e_{i j}}{\partial \theta_{i}} \equiv\left[\begin{array}{c} -\partial R_{i}^{\top} / \partial \theta_{i}\left(t_{j}-t_{i}\right) \\ -1 \end{array}\right] \\ \frac{\partial e_{i j}}{\partial t_{j}} \equiv\left[\begin{array}{c} R_{i}^{\top} \\ 00 \end{array}\right] \quad \frac{\partial e_{i j}}{\partial \theta_{j}} \equiv[001]^{\top} \end{array}$
将其组合起来为
$\mathbf{J}_{ij} = \left[\begin{array}{c} \cdots & \frac{\partial e_{ij}}{\partial c_i }\cdots \frac{\partial e_{ij}}{\partial c_i }\cdots \end{array}\right]$
$\mathbf{J} = \sum_{ij}\mathbf{J}_{ij}$

稀疏性

由于在优化过程中可能有几千个节点同时被优化，这就导致LM算法在求解过程中对超大矩阵 $(H+λdiag⁡H)(\mathbf{H}+\lambda \operatorname{diag} \mathbf{H})$ 求逆时需要花费大量的时间，好在由于SLAM问题构成的图具有稀疏性，通常可以将算法复杂度从 $O(n^3)$ 降低到 $O (n)$ 。