cartographer三部曲(二):Efficient Sparse Pose Adjustment for 2D Mapping

本文探讨了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]ci=[t1,θi]=[xi,yi,θi]

约束:约束是一个节点cjc_jcj到另外一个参考节点cic_ici的观测量,及相对变换关系z‾ij\overline{z}_{ij}zij和协方差矩阵Σij\Sigma_{ij}Σij

误差计算

假设cic_icicjc_jcj两节点间有一个约束z‾ij\overline{z}_{ij}zij,由于z‾ij\overline{z}_{ij}zij代表在i坐标系下j节点的测量位置,需要计算出图中节点cjc_jcjcic_ici系下的位置
h(ci,cj)≡{Ri⊤(tj−ti)θj−θ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. h(ci,cj){Ri(tjti)θjθi
两者相减便是误差
eij≡z‾ij−h(ci,cj) e_{ij} \equiv \overline{z}_{ij}-h(c_i,c_j) eijzijh(ci,cj)
最终得到最小二乘误差为
E(c,p)=∑ijeijTΣij−1eij=∑ijeij⊤Λijeij 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} E(c,p)=ijeijTΣij1eij=ijeijΛijeij

线性系统

采用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+λdiagH)Δx=JΛe
其中H=J⊤ΛJ\mathbf{H}=\mathbf{J}^{\top}\mathbf{\Lambda}\mathbf{J}H=JΛJ,因此只需要求解误差的雅可比即J\mathbf{J}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}tieij[Ri00]θieij[Ri/θi(tjti)1]tjeij[Ri00]θjeij[001]
将其组合起来为
Jij=[⋯∂eij∂ci⋯∂eij∂ci⋯] \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] Jij=[cieijcieij]
J=∑ijJij \mathbf{J} = \sum_{ij}\mathbf{J}_{ij} J=ijJij

稀疏性

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

压缩储存列

采用CSS 格式保存稀疏矩阵使得矩阵占用内存大大减少,其中CSS格式就是只保存非零项以及非零项的位置。

可持续的LM系统

大概简述了一下LM算法的流程

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值