激光SLAM入门笔记(五):前端配准 II

内容基于深蓝学院课件，如有错误，烦请斧正，不胜感激。

1.爬山法(拟梯度法)

1.1 示意图

1.2 基本思想

• 搜索方向固定(只在有限的几个方向中选取，平移8个，旋转2个)
  

• 搜索时，保持得分单调递增;不满足 则把搜索步长减半

• 当搜索步长减半的次数大于一定的次数时，则认为收敛

1.3 得分函数定义

符号
$T$: 机器人的位姿
$S_i(T)$: 激光点$i$位姿变换$T$后的坐标
$M(x)$: 似然域地图中坐标$x$处的值
$n$: 当前帧激光点的数量

公式
$$Score(T)=\sum _{i=1}^{n}M(S_i(T))$$

注意

• $Score(T)$函数表示位姿$T$在似然域地图中的得分
• 似然域地图由前一帧或前几帧激光数据生成

1.4 算法流程

• 搜索从一个初始位姿开始$T(0)$
• 对于当前最优位姿搜索位姿$T(i)$，计算周围10个待搜索位姿的得分，并得到最高得分对应的位姿$T(\ast )$
• 如果最高得分比当前搜索位姿得分更高，则更新当前搜索位姿$T(i+1)=T(\ast )$
• 否则，搜索步长减半，继续从当前搜 索位姿进行搜索$T(i+1)=T(i)$
• 如果搜索步长减半的次数超过阈值，则认为搜索结束，返回当前的最优位姿

1.5 伪代码

2.高斯牛顿优化方法

误差项与似然域有关，实际上是把帧间匹配问题转换为求解与似然域有关的极值问题。
（⚠️这里的似然与第3部分NDT里模拟出的似然域中的似然不是同一种）

论文：

• 《A Flexible and Scalable SLAM System with Full 3D Motion Estimation》

2.1 示意图

2.2 数学描述

构造误差函数
$$E(T)=arg\underset{T}{\min }\sum [1-M(S_i(T)){]}^2$$
其中
$T=(T_x,T_y,T_{\theta })$表示机器人的位姿
$p_i=(p_{ix},p_{iy})$表示第$i$个激光点的坐标
$M(x)$表示似然域地图中坐标$x$处的值，表示了栅格地图栅格被障碍物占用的概率
$S_i(T)$表示第$i$个激光点位姿变换$T$后的坐标
$$S_i(T)=\left[\begin{array}{ccc}\cos (T_{\theta })& -\sin (T_{\theta })& T_x\\ \sin (T_{\theta })& \cos (T_{\theta })& T_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{p}_{ix}\\ p_{iy}\\ 1\end{array}\right]$$

2.3 优化方法的求解

误差函数
$$E(T)=arg\underset{T}{\min }\sum [1-M(S_i(T)){]}^2$$
$M(S_i(T))$为非线性函数，进行一阶泰勒展开
$$f(T+\Delta T)\approx f(T)+f^{\prime }(T)\Delta T$$
可得
$$E(T+\Delta T)\approx arg\underset{T}{\min }\sum [1-M(S_i(T))-\nabla M(S_i(T))\frac{\partial S_i(T)}{\partial T}\Delta T{]}^2$$
求$E(T+\Delta T)$对$\Delta T$的导数，并令其等于0，可得
$$\sum [\nabla M(S_i(T))\frac{\partial S_i(T)}{\partial T}{]}^T[1-M(S_i(T))-\nabla M(S_i(T))\frac{\partial S_i(T)}{\partial T}\Delta T]=0$$
展开
$$\sum [\nabla M(S_i(T))\frac{\partial S_i(T)}{\partial T}{]}^T[1-M(S_i(T))]=\sum [\nabla M(S_i(T))\frac{\partial S_i(T)}{\partial T}{]}^T[\nabla M(S_i(T))\frac{\partial S_i(T)}{\partial T}\Delta T]$$
令$H=[\nabla M(S_i(T))\frac{\partial S_i(T)}{\partial T}{]}^T[\nabla M(S_i(T))\frac{\partial S_i(T)}{\partial T}]$
等式两边同时乘以$H^{-1}$，可得
$$\Delta T=H^{-1}\sum [\nabla M(S_i(T))\frac{\partial S_i(T)}{\partial T}{]}^T[1-M(S_i(T))]$$
$\Delta T$表达式中似然域地图$M(S_i(T))$已知，未知量为$\nabla M(S_i(T))$、$\frac{\partial S_i(T)}{\partial T}$
求未知量

1. $\frac{\partial S_i(T)}{\partial T}$
   $$\begin{gathered} S_i(T)=\left[\begin{array}{ccc}\cos (T_{\theta })& -\sin (T_{\theta })& T_x\\ \sin (T_{\theta })& \cos (T_{\theta })& T_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{p}_{ix}\\ p_{iy}\\ 1\end{array}\right]=\left[\begin{array}{c}\cos (T_{\theta })\ast p_{ix}-\sin (T_{\theta })\ast p_{iy}+T_x\\ \sin (T_{\theta })\ast p_{ix}+\cos (T_{\theta })\ast p_{iy}+T_y\\ 1\end{array}\right] \\ ⟹\frac{\partial S_i(T)}{\partial T}=\left[\begin{array}{ccc}1& 0& -\sin (T_{\theta })\ast p_{ix}-\cos (T_{\theta })\ast p_{iy}\\ 0& 1& \cos (T_{\theta })\ast p_{ix}-\sin (T_{\theta })\ast p_{iy}\end{array}\right] \end{gathered}$$
2. $\nabla M(S_i(T))$
   $M(S_i(T))$表示似然域地图中坐标$S_i(T)$处的值，$\nabla M(S_i(T))$表示似然域对坐标位置的导数。
   需要通过插值求得地图插值函数，再对坐标位置进行求导。

2.4 地图插值

在进行地图插值求地图插值函数之前，先介绍一下拉格朗日插值法，介绍完毕之后用拉格朗日插值法求得地图插值函数。

2.4.1 拉格朗日插值

详细介绍可见：如何直观地理解拉格朗日插值法？
主要思想：

1. 写出插值基函数
2. 构造拉格朗日插值多项式得到插值函数

2.4.2 进行地图插值

由于这里的地图是二维的，因此需要插值得到函数$L(x,y)$，故使用双线性插值的办法。

直接求

1. 在$x$方向进行一次线性插值
   令$R_1=(x,y_0),R_2=(x,y_1)$
   $$\begin{gathered} f(R_1)\approx \frac{x-x_1}{x_0-x_1}{Z}_1+\frac{x-x_0}{x_1-x_0}{Z}_2 \\ f(R_2)\approx \frac{x-x_1}{x_0-x_1}{Z}_4+\frac{x-x_0}{x_1-x_0}{Z}_3 \end{gathered}$$
2. 在$y$方向进行一次线性插值
   令$P=(x,y)$
   $$f(P)\approx \frac{y-y_1}{y_0-y_1}f(R_1)+\frac{y-y_0}{y_1-y_0}f(R_2)$$
3. 综合
   $$\begin{array}{rl}f(x,y)& \approx \frac{y-y_1}{y_0-y_1}f(R_1)+\frac{y-y_0}{y_1-y_0}f(R_2)\\ & =\frac{y-y_1}{y_0-y_1}(\frac{x-x_1}{x_0-x_1}{Z}_1+\frac{x-x_0}{x_1-x_0}{Z}_2)\\ & +\frac{y-y_0}{y_1-y_0}(\frac{x-x_1}{x_0-x_1}{Z}_4+\frac{x-x_0}{x_1-x_0}{Z}_3)\\ & =\frac{(x-x_1)}{(x_0-x_1)}\frac{(y-y_1)}{(y_0-y_1)}{Z}_1+\frac{(x-x_0)}{(x_1-x_0)}\frac{(y-y_1)}{(y_0-y_1)}{Z}_2\\ & +\frac{(x-x_0)}{(x_1-x_0)}\frac{(y-y_0)}{(y_1-y_0)}{Z}_3+\frac{(x-x_1)}{(x_0-x_1)}\frac{(y-y_0)}{(y_1-y_0)}{Z}_4\end{array}$$

简化求

令$u=\frac{x-x_0}{x_1-x_0},v=\frac{y-y_0}{y_1-y_0}$
则对应的四个点的坐标变为
$$\begin{gathered} (x_0,y_0)=(0,0),(x_1,y_0)=(1,0) \\ (x_1,y_1)=(1,1),(x_0,y_1)=(0,1) \end{gathered}$$
构造基函数
$$\begin{array}{rl}{l}_1(u,v)& =(1-u)(1-v)\\ l_2(u,v)& =u(1-v)\\ l_3(u,v)& =uv\\ l_4(u,v)& =(1-u)v\end{array}$$
构造拉格朗日插值多项式得到插值函数
$$L(u,v)=Z_1{l}_1(u,v)+Z_2{l}_2(u,v)+Z_3{l}_3(u,v)+Z_4{l}_4(u,v)$$
将$(u,v)$替换回$(x,y)$可得
$$\begin{array}{rl}L(x,y)& =\frac{(x-x_1)}{(x_0-x_1)}\frac{(y-y_1)}{(y_0-y_1)}{Z}_1+\frac{(x-x_0)}{(x_1-x_0)}\frac{(y-y_1)}{(y_0-y_1)}{Z}_2\\ & +\frac{(x-x_0)}{(x_1-x_0)}\frac{(y-y_0)}{(y_1-y_0)}{Z}_3+\frac{(x-x_1)}{(x_0-x_1)}\frac{(y-y_0)}{(y_1-y_0)}{Z}_4\end{array}$$

2.4.3 用插值函数对坐标求导

对$x$的偏导数
$$\frac{\partial L(x,y)}{\partial x}=\frac{y-y_0}{y_1-y_0}(\frac{Z_3-Z_4}{x_1-x_0})+\frac{y-y_1}{y_0-y_1}(\frac{Z_2-Z_1}{x_1-x_0})$$
对$y$的偏导数
$$\frac{\partial L(x,y)}{\partial y}=\frac{1}{y_1-y_0}(\frac{(x-x_0)Z_3-(x-x_1)Z_4}{x_1-x_0})+\frac{1}{y_0-y_1}(\frac{(x-x_0)Z_2-(x-x_1)Z_1}{x_1-x_0})$$

3.NDT方法

似然域中的似然是用高斯分布模拟出的似然，表示了方格中点云分布的概率。

论文：

• 《The Normal Distributions Transform: A New Approach to Laser Scan Matching》

3.1 示意图

3.2 基本思想

• 把空间划分为一个个的cell
• 用高斯分布去模拟似然域，形成一个天然分段连续的似然域
• 在得到连续的似然域之后，直接用牛顿方法进行迭代即可
• 似然域连续，不受离散化带来的影响

3.3 数学描述

$T=(T_x,T_y,T_{\theta })$表示需要求解的位姿变换
$X_i=\left[\begin{array}{c}{x}_i\\ y_i\end{array}\right]$表示第$i$个激光点的坐标
$X_i^{\prime }$表示第$i$个激光点经过位姿变换$T$后的坐标
$$X_i^{\prime }=\left[\begin{array}{cc}\cos (T_{\theta })& -\sin (T_{\theta })\\ \sin (T_{\theta })& \cos (T_{\theta })\end{array}\right]\left[\begin{array}{c}{x}_i\\ y_i\end{array}\right]+\left[\begin{array}{c}{T}_x\\ T_y\end{array}\right]$$
$q_i,{\Sigma }_i$表示点$X_i$对应的高斯分布的均值和协方差矩阵
当前帧激光点$i$的得分为：
$$scor{e}_i=\exp (-\frac{(X_i^{\prime }-q_i{)}^T{\Sigma }_i^{-1}(X_i^{\prime }-q_i)}{2})$$
因此匹配的目标函数：
$$\max \sum scor{e}_i⟹\min \sum -scor{e}_i$$
令$q=X_i^{\prime }-q_i$，则
$$scor{e}_i=\exp (-\frac{q^T{\Sigma }_i^{-1}q}{2})$$
特点

• 目标函数并没有取误差的平方
• 不对$score()$函数进行线性化
• 本式中可以直接求解Hessian矩阵

3.4 算法流程

1. 将空间环境进行cell分割(若是二维空间，则离散为栅格，若是三维空间则离散划分为立方体)，这样就可以将点云划分到不同cell中
2. 用高斯分布刻画每个cell点云的分布，其均值和协方差根据下方公式计算
   $$\begin{array}{rl}{\mu }_i^s& =\frac{1}{|V_i|}\sum _{p_i\in V_i}{p}_i\\ {\Sigma }_i^s& =\frac{1}{|V_i|}\sum _{p_i\in V_i}(p_i-{\mu }_i{)}^T(p_i-{\mu }_i)\end{array}$$
3. 根据初始解把当前帧的激光点转换到参考帧，并确定在哪一个cell中
4. 计算当前帧每个激光点在所属cell中的$scor{e}_i$
5. 计算目标函数$\max \sum scor{e}_i$，通过牛顿方法进行迭代求解，得到位姿转换矩阵$T$

3.5 NDT求解

3.5.1 牛顿方法

假设目标函数为：
$$\min f(x)$$
等价于求：
$$g(x)=f^{\prime }(x)=0$$
进行泰勒展开，取一阶进行：
$$\begin{array}{rl}g(x+\Delta x)& =g(x)+\frac{\partial g(x)}{\partial x}\Delta x=0\\ \frac{\partial g(x)}{\partial x}\Delta x& =-g(x)\end{array}$$
其中迭代$x=x+\Delta x$
于是上述等价于：
$$H\Delta x=-J$$
因此，用牛顿法进行迭代求解时，关键是要计算目标函数$f(x)$的Hessian矩阵和Jacobian矩阵。
同时，若：
$$f(x)=\sum f_i(x)$$
则：
$$\begin{array}{rl}J& =\sum J_i\\ H& =\sum H_i\end{array}$$

3.5.2 求解

显然：
$$f_i=-scor{e}_i=-\exp (-\frac{q^T{\Sigma }_i^{-1}q}{2})$$
因此，根据链式法则：
$$\begin{array}{rl}{J}_i& =\frac{\partial f_i}{\partial T}=-\exp (-\frac{q^T{\Sigma }_i^{-1}q}{2})(-q^T{\Sigma }_i^{-1})\frac{\partial q}{\partial T}\\ H_i& =\frac{\partial J_i}{\partial T}=-\exp (-\frac{q^T{\Sigma }_i^{-1}q}{2})(-q^T{\Sigma }_i^{-1}\frac{\partial q}{\partial T})(-q^T{\Sigma }_i^{-1}\frac{\partial q}{\partial T})\\ & -\exp (-\frac{q^T{\Sigma }_i^{-1}q}{2})(-\frac{\partial q^T}{\partial T}{\Sigma }_i^{-1}\frac{\partial q}{\partial T})-\exp (-\frac{q^T{\Sigma }_i^{-1}q}{2})(-q^T{\Sigma }_i^{-1}\frac{{\partial }^{2}q}{\partial T^2})\end{array}$$
其中，$q=X_i^{\prime }-q_i,\frac{\partial q}{\partial T}=\frac{\partial X_i^{\prime }}{\partial T}$
$$X_i^{\prime }=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]\left[\begin{array}{c}{x}_i\\ y_i\end{array}\right]+\left[\begin{array}{c}{t}_x\\ t_y\end{array}\right]⟹\frac{\partial X_i^{\prime }}{\partial T}=\left[\begin{array}{ccc}1& 0& -x_i\sin \theta -y_i\cos \theta \\ 0& 1& x_i\cos \theta -y_i\sin \theta \end{array}\right]$$

4.相关匹配方法(CSM)及分支定界加速

论文：

• 《Real-Time Correlative Scan Matching》
• 《Real-Time Loop Closure in 2D LIDAR SLAM》

4.1 帧间匹配似然域

4.2 算法流程

1. 构造似然域
2. 在指定的位姿搜索空间内进行搜索，计算每一个位姿的得分
3. 根据步骤2中位姿的得分，得分最高的位姿即为要求解的位姿，同时计算本次位姿匹配的方差

4.3 位姿搜索

1. 暴力搜索
   流程：
     三层for循环$(x,y,\theta )$枚举每一个位姿，分别计算每一个位姿的得分，计算量巨大。因为激光雷达数据在每一个位姿都要重新投影，投影需要计算$\sin$和$\cos$函数。
   特点：
     计算量巨大，投影矩阵生成需要计算$\sin$和$\cos$，计算的次数为：$n_x{n}_y{n}_{\theta }$；
     点的投影涉及到矩阵乘法，计算的次数为：$n_x{n}_y{n}_{\theta }n$。
   代码：
   

2. 预先投影搜索
   特点：
     交换暴力搜索中的循环顺序，最外层对$\theta$进行搜索，生成投影矩阵时，$\sin$和$\cos$的计算从$n_x{n}_y{n}_{\theta }$降低到$n_{\theta }$；
     $Score$函数中的点投影运算乘法运算消除，只剩下加法运算。
   代码：
   

3. 多分辨率搜索
   流程：
     (1)构造粗分辨率(25cm)和细分辨率(2.5cm)两个似然域
     (2)首先在粗分辨率似然域上进行搜索，获取最优位姿
     (3)把粗分辨率最优位姿对应的栅格进行细分辨率划分，然后再进行细分辨率搜索，再次得到最优位姿
     (4)粗分辨率地图的栅格的似然值为对应的细分辨率地图对应空间的所有栅格的最大值
   图示：
   
   （先找“最优大格子”，再从“最优大格子”中找“最优小格子”）

4.4 分枝定界法介绍

用途

把最优解求解问题转换为树形搜索问题。
根节点表示整个解空间，叶子节点表示最优解，中间的节点表示解空间的某一部分子空间。

特点

• 常用的树形搜索剪枝算法
• 求解整数规划问题
• 解的数量为有限个

分枝和定界

分枝：
  即根节点(深度为n)表示整个解深度n-1子节点表示解空间的子空间，深度n-2的节点表示深度n-1节点的子空间，这样层层划分，直到划分到真实解，也就是叶子节点(深度为0)为止。
定界：
  对于搜索树中的每一个节点，确定以该节点为根节点的子树的界。对于最小值问题，确定下界;对于最大值问题，确定上界(SLAM中为上界)。
⚠️SLAM中一个多棵树同时搜索，每棵树表示一个固定的角度。
算法伪代码

4.5 分枝定界法在相关方法中的加速作用

• 搜索树中的节点表示一个正方形的搜索范围$(c_x,c_y,c_{\theta },c_h)$
  
  $(c_x,c_y)$表示搜索空间的起点
  $c_h$表示树的深度
  $c_{\theta }$表示该节点对应的角度

• 分枝
  对于一个高度为$c_h>1$的节点$c=(c_x,c_y,c_{\theta },c_h)$，将其分枝为高度为$c_h-1$的四个子节点集合$(c_x,c_y,c_{\theta },c_h)$
  

• 定界
  构造得分函数，使得得分函数对于节点$c$的打分，是以节点$c$为根节点的子树的上界
  

• 多分辨率地图

• 算法伪代码
  节点选择
  
  分枝规则