激光SLAM入门笔记(六):图优化相关知识

路左

已于 2023-01-31 11:12:02 修改

阅读量2.2k

点赞数 1

CC 4.0 BY-SA版权

分类专栏： SLAM 文章标签：算法人工智能 python

于 2020-09-22 16:44:34 首次发布

本文链接：https://blog.csdn.net/u010507357/article/details/108540110

SLAM 专栏收录该内容

8 篇文章

订阅专栏

本文介绍SLAM中图优化的概念、构建方法及非线性最小二乘的应用，涵盖位姿图表示、回环检测、误差函数定义及线性化过程。

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

1.位姿图

1.1 概念

在这里插入图片描述

用一个图(Graph)来表示SLAM问题
图中的节点来表示机器人的位姿 $(x, y, y a w)$
两个节点之间的边表示两个位姿的空间约束(相对位姿关系以及对应方差)
一旦形成回环即可进行优化消除误差
里程积分的相对位姿视为预测值
回环计算的相对位姿视为观测值
Graph-based SLAM:构建图并调整各节点的位姿，让预测与观测的误差最小

1.2 构建

帧间边

里程计测量：
相邻节点之间的相对位姿关系，可以由里程计、IMU、帧间匹配计算得到

回环边

通过回环检测得到:
节点 $i$ 和节点 $j$ 在空间上相邻(观测到同样的数据)，但是时间上不相邻
用帧间匹配算法计算一个相对位姿

2.回环检测方法

一种简单的回环检测方法
在这里插入图片描述
$\Darr$

图示对应步骤

把节点分为active和inactive两部分
跟当前节点(红色节点)时间相近的节点称为active node(黄色节点)，其他的称为inactive node
$\Darr$
找到当前节点周围一定范围内所有inactive节点，作为回环候选帧(绿色节点)
当前节点和回环候选帧进行匹配，根据得分判断是否形成回环

3.非线性最小二乘原理

3.1 要解决的问题

给定一个系统，其状态方程为: $f (x) = z$

$x$ 表示系统的状态向量—即需要估计的值
$z$ 表示系统的观测值，可以通过传感器进行直接观测
$f (x)$ 表示一个非线性的映射函数，状态向量 $x$ 可以通过非线性函数 $f (x)$ 映射得到 $z$

目的就是给定该系统的 $n$ 个混有噪声的观测值 $(z_1,\dots,z_n)$ ，估计状态向量 $x$ ，使得其经过 $f (x)$ 映射之后的预测值和观测值的误差最小。
原理跟线性最小二乘基本相同，只是状态方程 $f (x)$ 是一个非线性函数。

3.2 示意图

在这里插入图片描述

$x$ 表示机器人的位置
$f (x)$ 为观测模型，节点之间相对位姿计算函数
$z$ 为帧间匹配或者回环检测计算出来的相对位姿
找到最优的 $x$ ，让预测和观测的误差最小

3.3 误差函数

目标为最小化预测和观测的差，因此误差即为预测和观测的差:
$e_i(x)=f_i(x)-z_i'$
假设误差服从高斯分布，即 $e_i(x)\sim N(0,\Omega_i)$ ， $\Omega_i$ 为对应的信息矩阵。
我们定义误差的联合概率分布为:
$G(e_i(x))=\prod_i \frac{1}{(2\pi)^{D/2}|\Omega_i|^{1/2}} \exp[-\frac{1}{2}e_i(x)^T\Omega_ie_i(x)]$
最终目标是使得误差尽可能趋近于0(均值)，等价于每个高斯分布取得最大值。
因此误差的联合概率分布 $G(e_i(x))$ 取得最大值。
对 $G(e_i(x))$ 取对数：
$\ln(G(e_i(x)))=\sum \frac{1}{(2\pi)^{D/2}|\Omega_i|^{1/2}} - \frac{1}{2} \sum e_i(x)^T\Omega_ie_i(x)$
即若让 $G(e_i(x))$ 取最大值，就是让 $\sum e_i(x)^T\Omega_ie_i(x)$ 取最小值。
令非线性最小二乘的目标函数为：
$\min F(x)=\min \sum e_i(x)^T\Omega_ie_i(x)$

3.4 求解

目标函数：
$min_x F(x)$
直接想法：
求 $F (x)$ 关于变量 $x$ 的导数，令其等于0，求解方程即可。

对于凸函数来说，上述想法是可行的，但对于非凸函数，通常采用基于梯度的优化方法。

3.4.1 线性化

$F (x)$ 为关于 $x$ 的非线性方程，将其化为关于 $x$ 的线性方程。
$F(x)=\sum e_i(x)^T\Omega_ie_i(x)$
误差函数 $e_i(x)$ 是非线性函数，因此 $F (x)$ 是关于 $x$ 的非线性函数。对误差函数 $e_i(x)$ 进行线性化得：
$e_i(x+\Delta x)=e_i(x)+J_i(x)\Delta x$
其中， $J$ 为映射函数 $F (x)$ 对状态向量 $x$ 的导数，称之为Jacobian矩阵。
$J_i(x)=(\frac{\partial f_i(x)}{\partial x_1},\frac{\partial f_i(x)}{\partial x_2},\dots,\frac{\partial f_i(x)}{\partial x_n})$
因此，函数 $F (x)$ 可线性化为：
$\begin{aligned} F(x+\Delta x)&=\sum e_i^T(x+\Delta x)\Omega_ie_i(x+\Delta x) \\ &=\sum (e_i(x)+J_i\Delta x)^T\Omega_i(e_i(x)+J_i\Delta x) \\ &=\sum (e_i^T\Omega_ie_i+e_i^T\Omega_iJ_i\Delta x+\Delta x^TJ_i^T\Omega_ie_i+\Delta x^TJ_i^T\Omega_iJ_i\Delta x) \\ &=\sum (e_i^T\Omega_ie_i+2e_i^T\Omega_iJ_i\Delta x+\Delta x^TJ_i^T\Omega_iJ_i\Delta x) \\ &=\sum c_i + \sum (2b_i^T\Delta x+\Delta x^T H_i \Delta x) \end{aligned}$
其中， $b_i^T=e_i^T\Omega_iJ_i,H_i=J_i^T\Omega_iJ_i$ 。
$\begin{aligned} F(x+\Delta x)&=\sum c_i + \sum (2b_i^T\Delta x+\Delta x^T H_i \Delta x) \\ &=\sum c_i + \sum 2b_i^T\Delta x+\Delta x^T \sum H_i \Delta x \\ &=\sum c_i + 2b^T\Delta x+\Delta x^T H \Delta x \end{aligned}$
$F(x+\Delta x)$ 为关于变量 $\Delta x$ 的二次函数，令其关于 $\Delta x$ 的导数等于0，可求解得到 $F(x+\Delta x)$ 的极值，即
$\frac{\partial F(x+\Delta x)}{\partial \Delta x}=2b+2H\Delta x$
令 $\frac{\partial F(x+\Delta x)}{\partial \Delta x}=0$ ，得
$H\Delta x=-b \implies \Delta x^*=-H^{-1}b$
令 $x=x+\Delta x^*$ ，然后不断迭代，直至收敛即可。

3.5 流程总结

线性化误差函数:
$e_i(x+\Delta x)=e_i(x)+J_i(x)\Delta x$
构建线性系统:
$\begin{aligned} &b^T=\sum e_i^T\Omega_iJ_i \\ &H=\sum J_i^T\Omega_iJ_i \\ &H\Delta x=b \end{aligned}$
求解线性系统:
$\Delta x^*=-H^{-1}b$
更新解，并不断迭代直至收敛:
$x=x+\Delta x^*$

4.非线性最小二乘原理在SLAM中的应用

4.1 误差函数

4.1.1 定义

在这里插入图片描述

观测值为匹配计算得到的节点 $i$ 和节点 $j$ 的相对位姿
$\begin{aligned} z_{ij}'&=(t_{ij},\theta_{ij}) \\ Z_{ij}'&=V2T(z_{ij}') \end{aligned}$
预测值为里程积分得到的当前节点 $i$ 和节点 $j$ 的相对位姿
$\begin{aligned} Z_{ij}&=f(x_i,x_j)=X_i^{-1}X_j \\ X_i&=V2T(x_i) \\ X_j&=V2T(x_j) \end{aligned}$
已知:
$\begin{aligned} X_i&= \begin{bmatrix} R_i & t_i \\ 0 & 1 \end{bmatrix} \implies X_i^{-1}= \begin{bmatrix} R_i^T & -R_i^Tt_i \\ 0 & 1 \end{bmatrix} \\ X_j&= \begin{bmatrix} R_j & t_j \\ 0 & 1 \end{bmatrix} \end{aligned}$
则预测值：
$\begin{aligned} Z_{ij}&=X_i^{-1}X_j= \begin{bmatrix} R_i^T & -R_i^Tt_i \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R_j & t_j \\ 0 & 1 \end{bmatrix}= \begin{bmatrix} R_i^TR_j & R_i^T(t_j-t_i) \\ 0 & 1 \end{bmatrix} \\ z_{ij}&=T2V(Z_{ij})= \begin{bmatrix} R_i^T(t_j-t_i) \\ \theta_j-\theta_i \end{bmatrix} \end{aligned}$
误差函数的定义
$e_{ij}(x)=T2V(Z_{ij}'^{-1}Z_{ij})$
误差函数的矩阵形式
$e_{ij}(x)= \begin{bmatrix} R_{ij}^T(R_i^T(t_j-t_i)-t_{ij}) \\ \theta_j-\theta_i-\theta_{ij} \end{bmatrix}$
对应的Jacobian矩阵
$\begin{aligned} \frac{\partial e_{ij}(x)}{\partial x_i}&= \begin{bmatrix} -R_{ij}^TR_i^T & R_{ij}^T \frac{\partial R_i^T}{\partial \theta}(t_j-t_i)\\ 0 & -1 \end{bmatrix} \\ \frac{\partial e_{ij}(x)}{\partial x_j}&= \begin{bmatrix} R_{ij}^TR_i^T & 0 \\ 0 & 1 \end{bmatrix} \end{aligned}$

4.1.2 线性化

误差函数：
$e_{ij}(x+\Delta x)=e_{ij}(x)+J_{ij}(x)\Delta x \\ J_{ij}=\frac{\partial e_{ij}(x)}{\partial x}$
因为误差函数只跟 $x_i$ 和 $x_j$ 有关，因此具有下列性质：
$\begin{aligned} \frac{\partial e_{ij}(x)}{\partial x}&=(0,\dots,\frac{\partial e_{ij}(x)}{\partial x_i},\dots,\frac{\partial e_{ij}(x)}{\partial x_j},\dots,0) \\ J_{ij}&=(0,\dots,A_{ij},\dots,B_{ij},\dots,0) \end{aligned}$
在这里插入图片描述

4.2 固定坐标系

观测值观测到的两个位姿之间的相对位姿
满足相对位姿约束的解有无穷多组
为了让解唯一，必须加入一个约束条件让某一个位姿固定，一般选择第一个位姿，即:
$\Delta x_1=0$
等价于：
$\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \Delta x_1= \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix}$
加入的约束为：
$\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \Delta x_1= \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix}$
求解的线性系统为:
$H\Delta x=-b$
因此等价于:
$H_{11}+= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}$

4.3 构建线性系统

已知误差项和Jacobian矩阵 $A_{ij}$ 和 $B_{ij}$
向量 $b$ 的更新为：
$\begin{aligned} b_i^T&+=e_{ij}^T\Omega_{ij}A_{ij} \\ b_j^T&+=e_{ij}^T\Omega_{ij}B_{ij} \\ \end{aligned}$
矩阵 $H$ 的更新为：
$\begin{aligned} H_{ii}&+=A_{ij}^T\Omega_{ij}A_{ij} \\ H_{ij}&+=A_{ij}^T\Omega_{ij}B_{ij} \\ H_{ji}&+=B_{ij}^T\Omega_{ij}A_{ij} \\ H_{jj}&+=B_{ij}^T\Omega_{ij}B_{ij} \\ \end{aligned}$