当前位置：服务支持 > 软件文章 > 激光SLAM后端优化总结：图优化核心方法与技巧

激光SLAM后端优化总结：图优化核心方法与技巧

阅读数 16

激光SLAM后端优化总结之图优化

1. 图的稀疏性与边缘化
2. 位姿图
3. 因子图
4. 滤波与图优化

优化问题，可以用图的方式表示，图的节点是需要优化的变量，边是优化变量之间的约束。节点可以是位姿，也可以是landmark；边可以是位姿之间的关系也可以是观测量；

1. 图的稀疏性与边缘化

考虑到位姿 i i i处对路标 j j j的一次观测 z i j z_{ij} zij:
e i j = Z i j − h ( x i , y j ) e_{ij}=Z_{ij}-h(x_i,y_j) eij=Zij−h(xi,yj)

我们根据Gauss-Newton 或 L-M 非线性优化方法，最终我们需要求解一个 H Δ x = b H\Delta x=b HΔx=b形式的方程。其中 𝐻 = J T J 𝐻 = J^TJ H=JTJ 为一个稀疏矩阵；
在这里插入图片描述

考虑多个位姿和路标的情况：

所以 H H H的表示为：

求解 H Δ x = b H\Delta x=b HΔx=b方程可以表示为：

在这里插入图片描述

利用舒尔补消元计算 Δ x c \Delta x_c Δxc:

然后把解得的 Δ x c \Delta x_c Δxc代入到原方程，然后求解 Δ x p \Delta x_p Δxp。

相比于直接解线性方程的做法，它的优势在于：

在消元过程中，由于 C C C 为对角块，所以 C − 1 C^{-1} C−1 容易解得。
求解了 Δ x c \Delta x_c Δxc之后，路标部分的增量方程由 Δ x p = C − 1 ( w − E T Δ x c ) \Delta x_p=C^{-1}(w-E^T\Delta x_c) Δxp=C−1(w−ETΔxc)给出。这依然用到了 C − 1 C^{-1} C−1 易于求解的特性。

2. 位姿图

图优化是把位姿和空间点一起，进行优化。随着轨迹越走越长，特征点的增长也很快。因此位姿图优化的意义在于：再优化几次以后把特征点固定住不再优化，只当作 位姿估计 的约束，之后主要优化位姿；
在这里插入图片描述

在基于图优化激光 SLAM 中，可以看作在激光里程计部分是不断累积图中的节点(位姿) 和边（位姿变换关系）；在后端优化部分通过构建图来最小化位姿之间的误差；
在这里插入图片描述

3. 因子图

因子图中，圆圈表示待优化的变量，如位姿；红框表示一次观测，蓝框表示一次运动；
在这里插入图片描述

蓝色框可以表示为：
P ( x 3 ∣ x 2 , u 3 ) P(x_3|x_2,u_3) P(x3∣x2,u3)

红色框可以表示为：
P ( z 1 ∣ x 1 , l 1 ) P(z_1|x_1,l_1) P(z1∣x1,l1)

所以因子图模型可以表示为：
{ x , l } ∗ = a r g m a x ( x 0 ) ∏ P ( x k ∣ x k − 1 , u k ) ∏ P ( z k ∣ x i , l j ) \{x,l\}^*=arg max(x_0)\prod P(x_k|x_{k-1},u_k) \prod P(z_k|x_{i},l_j) {x,l}∗=argmax(x0)∏P(xk∣xk−1,uk)∏P(zk∣xi,lj)
在这里插入图片描述

因子图是增量的处理后端优化，每加入一个节点，普通图优化是对整个图进行优化。因子图相当于保留中间结果，每加入一个点，对不需要重新计算的就直接用之前的中间结果，对需要重新计算的再去计算。