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

1.爬山法(拟梯度法)

1.1 示意图

1.2 基本思想

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

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

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

1.3 得分函数定义

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

公式
Score(T)=∑i=1nM(Si(T))Score(T)=\sum_{i=1}^n M(S_i(T)) Score(T)=i=1∑n​M(Si​(T))

注意

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

1.4 算法流程

• 搜索从一个初始位姿开始T(0)T(0)T(0)
• 对于当前最优位姿搜索位姿T(i)T(i)T(i)，计算周围10个待搜索位姿的得分，并得到最高得分对应的位姿T(∗)T(*)T(∗)
• 如果最高得分比当前搜索位姿得分更高，则更新当前搜索位姿T(i+1)=T(∗)T(i+1)=T(*)T(i+1)=T(∗)
• 否则，搜索步长减半，继续从当前搜 索位姿进行搜索T(i+1)=T(i)T(i+1)=T(i)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)=argmin⁡T∑[1−M(Si(T))]2E(T)=arg \min_T \sum[1-M(S_i(T))]^2 E(T)=argTmin​∑[1−M(Si​(T))]2
其中
T=(Tx,Ty,Tθ)T=(T_x,T_y,T_{\theta})T=(Tx​,Ty​,Tθ​)表示机器人的位姿
pi=(pix,piy)p_i=(p_{ix},p_{iy})pi​=(pix​,piy​)表示第iii个激光点的坐标
M(x)M(x)M(x)表示似然域地图中坐标xxx处的值，表示了栅格地图栅格被障碍物占用的概率
Si(T)S_i(T)Si​(T)表示第iii个激光点位姿变换TTT后的坐标
Si(T)=[cos⁡(Tθ)−sin⁡(Tθ)Txsin⁡(Tθ)cos⁡(Tθ)Ty001][pixpiy1]S_i(T)= \begin{bmatrix} \cos(T_\theta) & -\sin(T_\theta) & T_x \\ \sin(T_\theta) & \cos(T_\theta) & T_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} p_{ix} \\ p_{iy} \\ 1 \end{bmatrix} Si​(T)=​cos(Tθ​)sin(Tθ​)0​−sin(Tθ​)cos(Tθ​)0​Tx​Ty​1​​​pix​piy​1​​

2.3 优化方法的求解

误差函数
E(T)=argmin⁡T∑[1−M(Si(T))]2E(T)=arg \min_T \sum[1-M(S_i(T))]^2 E(T)=argTmin​∑[1−M(Si​(T))]2
M(Si(T))M(S_i(T))M(Si​(T))为非线性函数，进行一阶泰勒展开
f(T+ΔT)≈f(T)+f′(T)ΔTf(T+\Delta T)\approx f(T)+f'(T)\Delta T f(T+ΔT)≈f(T)+f′(T)ΔT
可得
E(T+ΔT)≈argmin⁡T∑[1−M(Si(T))−∇M(Si(T))∂Si(T)∂TΔT]2E(T+\Delta T)\approx arg \min_T \sum[1-M(S_i(T))-\nabla M(S_i(T)) \frac{\partial S_i(T)}{\partial T}\Delta T]^2 E(T+ΔT)≈argTmin​∑[1−M(Si​(T))−∇M(Si​(T))∂T∂Si​(T)​ΔT]2
求E(T+ΔT)E(T+\Delta T)E(T+ΔT)对ΔT\Delta TΔT的导数，并令其等于0，可得
∑[∇M(Si(T))∂Si(T)∂T]T[1−M(Si(T))−∇M(Si(T))∂Si(T)∂TΔ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 ∑[∇M(Si​(T))∂T∂Si​(T)​]T[1−M(Si​(T))−∇M(Si​(T))∂T∂Si​(T)​ΔT]=0
展开
∑[∇M(Si(T))∂Si(T)∂T]T[1−M(Si(T))]=∑[∇M(Si(T))∂Si(T)∂T]T[∇M(Si(T))∂Si(T)∂TΔT]\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] ∑[∇M(Si​(T))∂T∂Si​(T)​]T[1−M(Si​(T))]=∑[∇M(Si​(T))∂T∂Si​(T)​]T[∇M(Si​(T))∂T∂Si​(T)​ΔT]
令H=[∇M(Si(T))∂Si(T)∂T]T[∇M(Si(T))∂Si(T)∂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=[∇M(Si​(T))∂T∂Si​(T)​]T[∇M(Si​(T))∂T∂Si​(T)​]
等式两边同时乘以H−1H^{-1}H−1，可得
ΔT=H−1∑[∇M(Si(T))∂Si(T)∂T]T[1−M(Si(T))]\Delta T=H^{-1}\sum [\nabla M(S_i(T)) \frac{\partial S_i(T)}{\partial T}]^T[1-M(S_i(T))] ΔT=H−1∑[∇M(Si​(T))∂T∂Si​(T)​]T[1−M(Si​(T))]
ΔT\Delta TΔT表达式中似然域地图M(Si(T))M(S_i(T))M(Si​(T))已知，未知量为∇M(Si(T))\nabla M(S_i(T))∇M(Si​(T))、∂Si(T)∂T\frac{\partial S_i(T)}{\partial T}∂T∂Si​(T)​
求未知量

1. ∂Si(T)∂T\frac{\partial S_i(T)}{\partial T}∂T∂Si​(T)​
   Si(T)=[cos⁡(Tθ)−sin⁡(Tθ)Txsin⁡(Tθ)cos⁡(Tθ)Ty001][pixpiy1]=[cos⁡(Tθ)∗pix−sin⁡(Tθ)∗piy+Txsin⁡(Tθ)∗pix+cos⁡(Tθ)∗piy+Ty1]⟹∂Si(T)∂T=[10−sin⁡(Tθ)∗pix−cos⁡(Tθ)∗piy01cos⁡(Tθ)∗pix−sin⁡(Tθ)∗piy]S_i(T)= \begin{bmatrix} \cos(T_\theta) & -\sin(T_\theta) & T_x \\ \sin(T_\theta) & \cos(T_\theta) & T_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} p_{ix} \\ p_{iy} \\ 1 \end{bmatrix}= \begin{bmatrix} \cos(T_\theta) *p_{ix} - \sin(T_\theta)*p_{iy} +T_x \\ \sin(T_\theta) *p_{ix} + \cos(T_\theta)*p_{iy} +T_y \\ 1 \end{bmatrix} \\ \implies \frac{\partial S_i(T)}{\partial T}= \begin{bmatrix} 1 & 0 & -\sin(T_\theta) *p_{ix} - \cos(T_\theta)*p_{iy} \\ 0 & 1 & \cos(T_\theta) *p_{ix} - \sin(T_\theta)*p_{iy} \end{bmatrix} Si​(T)=​cos(Tθ​)sin(Tθ​)0​−sin(Tθ​)cos(Tθ​)0​Tx​Ty​1​​​pix​piy​1​​=​cos(Tθ​)∗pix​−sin(Tθ​)∗piy​+Tx​sin(Tθ​)∗pix​+cos(Tθ​)∗piy​+Ty​1​​⟹∂T∂Si​(T)​=[10​01​−sin(Tθ​)∗pix​−cos(Tθ​)∗piy​cos(Tθ​)∗pix​−sin(Tθ​)∗piy​​]
2. ∇M(Si(T))\nabla M(S_i(T))∇M(Si​(T))
   M(Si(T))M(S_i(T))M(Si​(T))表示似然域地图中坐标Si(T)S_i(T)Si​(T)处的值，∇M(Si(T))\nabla M(S_i(T))∇M(Si​(T))表示似然域对坐标位置的导数。
   需要通过插值求得地图插值函数，再对坐标位置进行求导。

2.4 地图插值

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

2.4.1 拉格朗日插值

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

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

2.4.2 进行地图插值

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

直接求

1. 在xxx方向进行一次线性插值
   令R1=(x,y0),R2=(x,y1)R_1=(x,y_0),R_2=(x,y_1)R1​=(x,y0​),R2​=(x,y1​)
   f(R1)≈x−x1x0−x1Z1+x−x0x1−x0Z2f(R2)≈x−x1x0−x1Z4+x−x0x1−x0Z3f(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 f(R1​)≈x0​−x1​x−x1​​Z1​+x1​−x0​x−x0​​Z2​f(R2​)≈x0​−x1​x−x1​​Z4​+x1​−x0​x−x0​​Z3​
2. 在yyy方向进行一次线性插值
   令P=(x,y)P=(x,y)P=(x,y)
   f(P)≈y−y1y0−y1f(R1)+y−y0y1−y0f(R2)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) f(P)≈y0​−y1​y−y1​​f(R1​)+y1​−y0​y−y0​​f(R2​)
3. 综合
   f(x,y)≈y−y1y0−y1f(R1)+y−y0y1−y0f(R2)=y−y1y0−y1(x−x1x0−x1Z1+x−x0x1−x0Z2)+y−y0y1−y0(x−x1x0−x1Z4+x−x0x1−x0Z3)=(x−x1)(x0−x1)(y−y1)(y0−y1)Z1+(x−x0)(x1−x0)(y−y1)(y0−y1)Z2+(x−x0)(x1−x0)(y−y0)(y1−y0)Z3+(x−x1)(x0−x1)(y−y0)(y1−y0)Z4\begin{aligned} 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{aligned} f(x,y)​≈y0​−y1​y−y1​​f(R1​)+y1​−y0​y−y0​​f(R2​)=y0​−y1​y−y1​​(x0​−x1​x−x1​​Z1​+x1​−x0​x−x0​​Z2​)+y1​−y0​y−y0​​(x0​−x1​x−x1​​Z4​+x1​−x0​x−x0​​Z3​)=(x0​−x1​)(x−x1​)​(y0​−y1​)(y−y1​)​Z1​+(x1​−x0​)(x−x0​)​(y0​−y1​)(y−y1​)​Z2​+(x1​−x0​)(x−x0​)​(y1​−y0​)(y−y0​)​Z3​+(x0​−x1​)(x−x1​)​(y1​−y0​)(y−y0​)​Z4​​

简化求

令u=x−x0x1−x0,v=y−y0y1−y0u=\frac{x-x_0}{x_1-x_0},v=\frac{y-y_0}{y_1-y_0}u=x1​−x0​x−x0​​,v=y1​−y0​y−y0​​
则对应的四个点的坐标变为
(x0,y0)=(0,0),(x1,y0)=(1,0)(x1,y1)=(1,1),(x0,y1)=(0,1)(x_0,y_0)=(0,0) ,(x_1,y_0)=(1,0) \\ (x_1,y_1)=(1,1) ,(x_0,y_1)=(0,1) (x0​,y0​)=(0,0),(x1​,y0​)=(1,0)(x1​,y1​)=(1,1),(x0​,y1​)=(0,1)
构造基函数
l1(u,v)=(1−u)(1−v)l2(u,v)=u(1−v)l3(u,v)=uvl4(u,v)=(1−u)v\begin{aligned} 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{aligned} l1​(u,v)l2​(u,v)l3​(u,v)l4​(u,v)​=(1−u)(1−v)=u(1−v)=uv=(1−u)v​
构造拉格朗日插值多项式得到插值函数
L(u,v)=Z1l1(u,v)+Z2l2(u,v)+Z3l3(u,v)+Z4l4(u,v)L(u,v)=Z_1l_1(u,v)+Z_2l_2(u,v)+Z_3l_3(u,v)+Z_4l_4(u,v) L(u,v)=Z1​l1​(u,v)+Z2​l2​(u,v)+Z3​l3​(u,v)+Z4​l4​(u,v)
将(u,v)(u,v)(u,v)替换回(x,y)(x,y)(x,y)可得
L(x,y)=(x−x1)(x0−x1)(y−y1)(y0−y1)Z1+(x−x0)(x1−x0)(y−y1)(y0−y1)Z2+(x−x0)(x1−x0)(y−y0)(y1−y0)Z3+(x−x1)(x0−x1)(y−y0)(y1−y0)Z4\begin{aligned} 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{aligned} L(x,y)​=(x0​−x1​)(x−x1​)​(y0​−y1​)(y−y1​)​Z1​+(x1​−x0​)(x−x0​)​(y0​−y1​)(y−y1​)​Z2​+(x1​−x0​)(x−x0​)​(y1​−y0​)(y−y0​)​Z3​+(x0​−x1​)(x−x1​)​(y1​−y0​)(y−y0​)​Z4​​

2.4.3 用插值函数对坐标求导

对xxx的偏导数
∂L(x,y)∂x=y−y0y1−y0(Z3−Z4x1−x0)+y−y1y0−y1(Z2−Z1x1−x0)\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}) ∂x∂L(x,y)​=y1​−y0​y−y0​​(x1​−x0​Z3​−Z4​​)+y0​−y1​y−y1​​(x1​−x0​Z2​−Z1​​)
对yyy的偏导数
∂L(x,y)∂y=1y1−y0((x−x0)Z3−(x−x1)Z4x1−x0)+1y0−y1((x−x0)Z2−(x−x1)Z1x1−x0)\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}) ∂y∂L(x,y)​=y1​−y0​1​(x1​−x0​(x−x0​)Z3​−(x−x1​)Z4​​)+y0​−y1​1​(x1​−x0​(x−x0​)Z2​−(x−x1​)Z1​​)

3.NDT方法

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

论文：

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

3.1 示意图

3.2 基本思想

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

3.3 数学描述

T=(Tx,Ty,Tθ)T=(T_x,T_y,T_{\theta})T=(Tx​,Ty​,Tθ​)表示需要求解的位姿变换
Xi=[xiyi]X_i=\begin{bmatrix} x_i \\ y_i \end{bmatrix}Xi​=[xi​yi​​]表示第iii个激光点的坐标
Xi′X_i'Xi′​表示第iii个激光点经过位姿变换TTT后的坐标
Xi′=[cos⁡(Tθ)−sin⁡(Tθ)sin⁡(Tθ)cos⁡(Tθ)][xiyi]+[TxTy]X_i'= \begin{bmatrix} \cos(T_{\theta}) & -\sin(T_{\theta}) \\ \sin(T_{\theta}) & \cos(T_{\theta}) \end{bmatrix} \begin{bmatrix} x_i \\ y_i \end{bmatrix} + \begin{bmatrix} T_x \\ T_y \end{bmatrix} Xi′​=[cos(Tθ​)sin(Tθ​)​−sin(Tθ​)cos(Tθ​)​][xi​yi​​]+[Tx​Ty​​]
qi,Σiq_i,\Sigma_iqi​,Σi​表示点XiX_iXi​对应的高斯分布的均值和协方差矩阵
当前帧激光点iii的得分为：
scorei=exp⁡(−(Xi′−qi)TΣi−1(Xi′−qi)2)score_i=\exp(-\frac{(X_i'-q_i)^T\Sigma_i^{-1}(X_i'-q_i)}{2}) scorei​=exp(−2(Xi′​−qi​)TΣi−1​(Xi′​−qi​)​)
因此匹配的目标函数：
max⁡∑scorei⟹min⁡∑−scorei\max \sum score_i \implies \min \sum -score_i max∑scorei​⟹min∑−scorei​
令q=Xi′−qiq=X_i'-q_iq=Xi′​−qi​，则
scorei=exp⁡(−qTΣi−1q2)score_i=\exp(-\frac{q^T\Sigma_i^{-1}q}{2}) scorei​=exp(−2qTΣi−1​q​)
特点

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

3.4 算法流程

1. 将空间环境进行cell分割(若是二维空间，则离散为栅格，若是三维空间则离散划分为立方体)，这样就可以将点云划分到不同cell中
2. 用高斯分布刻画每个cell点云的分布，其均值和协方差根据下方公式计算
   μis=1∣Vi∣∑pi∈VipiΣis=1∣Vi∣∑pi∈Vi(pi−μi)T(pi−μi)\begin{aligned} \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{aligned} μis​Σis​​=∣Vi​∣1​pi​∈Vi​∑​pi​=∣Vi​∣1​pi​∈Vi​∑​(pi​−μi​)T(pi​−μi​)​
3. 根据初始解把当前帧的激光点转换到参考帧，并确定在哪一个cell中
4. 计算当前帧每个激光点在所属cell中的scoreiscore_iscorei​
5. 计算目标函数max⁡∑scorei\max \sum score_imax∑scorei​，通过牛顿方法进行迭代求解，得到位姿转换矩阵TTT

3.5 NDT求解

3.5.1 牛顿方法

假设目标函数为：
min⁡f(x)\min f(x) minf(x)
等价于求：
g(x)=f′(x)=0g(x)=f'(x)=0 g(x)=f′(x)=0
进行泰勒展开，取一阶进行：
g(x+Δx)=g(x)+∂g(x)∂xΔx=0∂g(x)∂xΔx=−g(x)\begin{aligned} 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{aligned} g(x+Δx)∂x∂g(x)​Δx​=g(x)+∂x∂g(x)​Δx=0=−g(x)​
其中迭代x=x+Δxx=x+\Delta xx=x+Δx
于是上述等价于：
HΔx=−JH\Delta x=-J HΔx=−J
因此，用牛顿法进行迭代求解时，关键是要计算目标函数f(x)f(x)f(x)的Hessian矩阵和Jacobian矩阵。
同时，若：
f(x)=∑fi(x)f(x)=\sum f_i(x) f(x)=∑fi​(x)
则：
J=∑JiH=∑Hi\begin{aligned} J&=\sum J_i \\ H&=\sum H_i \end{aligned} JH​=∑Ji​=∑Hi​​

3.5.2 求解

显然：
fi=−scorei=−exp⁡(−qTΣi−1q2)f_i=-score_i=-\exp(-\frac{q^T\Sigma_i^{-1}q}{2}) fi​=−scorei​=−exp(−2qTΣi−1​q​)
因此，根据链式法则：
Ji=∂fi∂T=−exp⁡(−qTΣi−1q2)(−qTΣi−1)∂q∂THi=∂Ji∂T=−exp⁡(−qTΣi−1q2)(−qTΣi−1∂q∂T)(−qTΣi−1∂q∂T)−exp⁡(−qTΣi−1q2)(−∂qT∂TΣi−1∂q∂T)−exp⁡(−qTΣi−1q2)(−qTΣi−1∂2q∂T2)\begin{aligned} 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{aligned} Ji​Hi​​=∂T∂fi​​=−exp(−2qTΣi−1​q​)(−qTΣi−1​)∂T∂q​=∂T∂Ji​​=−exp(−2qTΣi−1​q​)(−qTΣi−1​∂T∂q​)(−qTΣi−1​∂T∂q​)−exp(−2qTΣi−1​q​)(−∂T∂qT​Σi−1​∂T∂q​)−exp(−2qTΣi−1​q​)(−qTΣi−1​∂T2∂2q​)​
其中，q=Xi′−qi,∂q∂T=∂Xi′∂Tq=X_i'-q_i,\frac{\partial q}{\partial T}=\frac{\partial X_i'}{\partial T}q=Xi′​−qi​,∂T∂q​=∂T∂Xi′​​
Xi′=[cos⁡θ−sin⁡θsin⁡θcos⁡θ][xiyi]+[txty]⟹∂Xi′∂T=[10−xisin⁡θ−yicos⁡θ01xicos⁡θ−yisin⁡θ]X_i'= \begin{bmatrix} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{bmatrix} \begin{bmatrix} x_i \\ y_i \end{bmatrix} + \begin{bmatrix} t_x \\ t_y \end{bmatrix} \implies \frac{\partial X_i'}{\partial T}= \begin{bmatrix} 1 & 0 & -x_i\sin\theta-y_i\cos\theta \\ 0 & 1 & x_i\cos\theta-y_i\sin\theta \end{bmatrix} Xi′​=[cosθsinθ​−sinθcosθ​][xi​yi​​]+[tx​ty​​]⟹∂T∂Xi′​​=[10​01​−xi​sinθ−yi​cosθxi​cosθ−yi​sinθ​]

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,θ)(x,y,\theta)(x,y,θ)枚举每一个位姿，分别计算每一个位姿的得分，计算量巨大。因为激光雷达数据在每一个位姿都要重新投影，投影需要计算sin⁡\sinsin和cos⁡\coscos函数。
   特点：
     计算量巨大，投影矩阵生成需要计算sin⁡\sinsin和cos⁡\coscos，计算的次数为：nxnynθn_xn_yn_{\theta}nx​ny​nθ​；
     点的投影涉及到矩阵乘法，计算的次数为：nxnynθnn_xn_yn_{\theta}nnx​ny​nθ​n。
   代码：
   

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

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

4.4 分枝定界法介绍

用途

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

特点

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

分枝和定界

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

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

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

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

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

• 多分辨率地图

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

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

1. 激光SLAM入门笔记(一):概述

   概述 激光SLAM pipeline 1.数据处理(非常重要) 里程计标定 激光雷达运动畸变去除 2.帧间匹配(核心部分) 3.回环检测 4.后端优化 2D激光SLAM 数据处理(非常重要) 帧间匹配 ...

2. 激光SLAM入门学习笔记

   激光SLAM入门学习笔记 激光SLAM入门学习笔记 一.推荐阅读书籍 二.推荐公众号.知乎.博客 1.公众号 2.知乎 3.博客 三.推荐阅读论文&代码(参考泡泡机器人) 2D激光SLAM 3 ...

3. 激光slam学习笔记——基于图优化的激光slam方法

   激光slam学习笔记--基于图优化的激光slam方法 1.slam基础 整体来说,在激光slam中,滤波器的误差要小于图优化的误差. 图优化通俗点说就是里程计计算的位姿与观测到的位姿之间会形成一个误差 ...

4. slam学习笔记五----视觉里程计的学习1

   一,什么是视觉里程计 SLAM系统分为前端和后端,前端也称为是视觉里程计,主要功能是根据相邻图像的信息粗略的估计出相机的运动,为后端提供较好的初始值. 视觉里程计的算法有两大类特征点法和直接法.特征点 ...

5. 小萨村的激光SLAM学习笔记

   在之前的学习过程中,稀里糊涂的学了快一年了,一直没有对自己已经学到的东西做好梳理,很多东西杂七杂八的混在一起,导致自己回想起来,知识都很不体系,没有办法把学到的东西串联起来. 我的研究生导师本身也不是 ...

6. 激光slam学习笔记１－－RTK组合惯导、激光雷达传感器一些经验知识分享

   前言:跟组合惯导和激光雷达打交道半年了,过程中查找学习了这两方面的资料,这里来个小结.如果有理解错误的,望大佬们不吝赐教. 一.RTK组合惯导 个人理解有两部分组成,一个提供gps信息的rtk,另外一 ...

7. [官方Flink入门笔记 ] 五、客户端操作

   一 .环境说明 在前面几期的课程里面讲过了 Flink 开发环境的搭建和应用的部署以及运行,今天的课程主要是讲 Flink 的客户端操作. 本次讲解以实际操作为主.这次课程是基于社区的 Flink 1 ...

8. 激光SLAM导航系列(五)局部路径规划

   局部路径规划 局部路径规划简介 机器人在获得目的地信息后,首先经过全局路径规划规划出一条大致可行的路线,然后调用局部路径规划器根据这条路线及costmap的信息规划出机器人在局部时做出具体行动策略,R ...

9. 激光SLAM入门（1）：坐标变换

   0. 预备知识:齐次坐标 齐次坐标是整个系列的基础.如果不理解齐次坐标系的概念,强烈建议先阅读https://zhuanlan.zhihu.com/p/74597564 1.机器人位姿表示 二维情况下 ...

最新文章

1. 深入挖掘Hyperledger Fabric中的私有数据
2. 嵌入式linux设计报告,嵌入式linux课程设计报告
3. 虚函数和纯虚函数的区别？
4. Django Views（视图函数）
5. java类无法调用值,Kotlin无法调用到Java中定义的interface类的问题记录
6. JQuery 基础：4.JQuery对象
7. 【自用】手工编译lnmp环境
8. 水木周平：就业难？招人更难！
9. fydeos 安装linux程序,FydeOS安装教程-电脑系统安装手册
10. 300字美文摘抄加赏析
11. 给定一个成绩a，使用switch结构求出a的等级。A：90-100，B：80-89，C：70-79，D：60-69，E：0~59
12. opencv滤波函数简介
13. Linux命令学习资料总结
14. Haskell语言学习笔记（30）MonadCont, Cont, ContT
15. 2021年Java发展怎么样？现在学了Java技术出来是否还能找到工作？
16. Linux知识点求职应聘必备
17. 公众号小伙伴“进群”学习交流
18. PDF压缩在线怎么操作？这几个操作谁还不知道
19. 外企, 私企和国企, 不同的软件开发流程感受
20. 简单的车辆检测代码（opencv+python4.2.0 ） win10+64bit

热门文章

1. Python机器学习入门笔记（1）—— Scikit-learn与特征工程
2. Python项目:基于Python+Django实现药品管理系统
3. Docker学习——DockerFile
4. 【opencv学习】【图像直方图和均衡化】
5. 关于matlab中矩阵的运算
6. 5.下载器-Download
7. Linux双硬盘引导,Linux系统下双硬盘多系统引导深入探究
8. Java核心技术卷一笔记
9. Python攻关之模块（1）
10. python-求两个字符串的连续最长公共子串