HectorSlam学习

# HectorSlam

## 一、安装

git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam

克隆到工作空间，编译无误后即可

+ 注意：git branch检查分支

+ 注意：git checkout -b 分支名 切换到分支

+ 注意：git branch -a 查看所有分支

+ 注意：git checkout 分支名 切换到分支

+ 注意：git branch -d 分支名 删除分支

## 二、原理

### 1、概述


Hector SLAM 是一种结合了激光雷达和惯性传感器数据的SLAM系统，它通过高效的扫描匹配和多分辨率地图表示方法，实现了对未知环境的快速学习和精确定位。该系统特别适用于计算资源有限且需要实时地图更新的应用场景，如城市搜救、室内导航等，并且已经在ROS平台上开源，便于研究者和开发者集成和使用。

Hector SLAM 设计之初是为了融合来自激光雷达和惯性传感器（IMU）的数据，以实现更准确和鲁棒的定位和建图。IMU 提供了关于机器人运动的重要信息，如角速度和加速度，这些信息对于估计机器人的3D位姿至关重要。然而，是否“必须”使用 IMU 取决于具体的应用场景和需求。

在某些情况下，如果环境特征丰富且机器人的运动模式相对简单，可能可以通过仅使用激光雷达数据的其他SLAM方法（如GMapping或Cartographer）来实现定位和建图。这些方法可能不需要IMU数据，但在动态环境或机器人运动较为复杂时，它们可能不如融合了IMU数据的SLAM系统那样鲁棒。

总的来说，虽然Hector SLAM 可以不依赖IMU数据运行，但为了提高系统的鲁棒性和定位精度，特别是在机器人运动较快或者环境特征稀疏的情况下，使用IMU是非常有帮助的。因此，是否使用IMU应根据具体的应用需求和环境条件来决定。


### 2、Hector SLAM 高斯牛顿法  

Hector SLAM 在其核心的扫描匹配过程中采用了高斯-牛顿法来优化位姿估计。高斯-牛顿法是一种非线性最小二乘优化方法，常用于机器人定位和地图构建中，以最小化误差函数。在 Hector SLAM 的上下文中，高斯-牛顿法用于将激光雷达扫描数据与已有的地图进行匹配，从而估计机器人的位姿变化。

> 高斯-牛顿法的基本原理

高斯-牛顿法通过迭代的方式逐步改进参数估计。在每次迭代中，它利用当前的参数估计值来计算观测数据的预测值，并与实际观测值进行比较。然后，它构建一个雅可比矩阵（或称为海森矩阵），该矩阵包含了预测值对参数的偏导数。通过最小化预测值与实际观测值之间的差异（即误差函数），算法更新参数估计值。

> 在 Hector SLAM 中的应用

在 Hector SLAM 中，高斯-牛顿法用于解决扫描匹配问题。具体来说，算法的目标是找到一个最佳的位姿变换，使得当前的激光雷达扫描数据与地图中最匹配。这个过程涉及到构建一个目标函数，该函数衡量了激光雷达扫描点与地图中对应位置的匹配程度。通过迭代优化这个目标函数，Hector SLAM 能够估计出机器人的位姿变化。

> 优化过程

初始化：算法从一个初始的位姿估计开始，这通常是基于之前的位姿估计和机器人的运动模型。

构建雅可比矩阵：对于每个激光雷达扫描点，计算其在地图上的对应点，并构建雅可比矩阵，该矩阵包含了扫描点位置对位姿参数的偏导数。

计算误差：计算当前位姿估计下，激光雷达扫描点与地图中对应点之间的误差。

更新位姿估计：使用高斯-牛顿法更新位姿估计，即通过最小化误差函数来调整位姿参数。

迭代：重复上述步骤，直到达到收敛条件，即位姿估计不再显著变化，或误差函数的改进小于某个阈值。

> 优势与局限性

+ 优势：

+ 精度高：高斯-牛顿法能够在非线性问题中提供高精度的解。

+ + 适用于SLAM：它适用于处理SLAM中的扫描匹配问题，尤其是在机器人运动较快时。

+ 局限性：

+ + 计算量大：高斯-牛顿法需要计算和更新雅可比矩阵，这在大规模问题中可能导致较大的计算负担。

+ + 可能陷入局部最小值：虽然高斯-牛顿法通常能够找到全局最优解，但在某些情况下，它可能会收敛到局部最小值，尤其是在地图特征稀疏或噪声较大的情况下。

### (1)知识点：泰勒公式

泰勒展开的就是在函数一个特定的点附近用多项式函数去逼近原函数，并且在该点处这个多项式的若干阶导数与原函数保持相等，具体多少阶取决于泰勒展开的阶数。也就是说，一阶导数决定了函数的变化趋势，二阶导数决定了一阶导数的变化趋势，n 阶导数决定了n-1阶导数的变化趋势。这样我们通过函数某一点的若干阶导数便知道了函数在邻域内的取值。甚至如果函数在足够远的邻域内光滑，我们便可以通过它在该点上的若干阶导数知道它足够远处的取值。

> 带皮亚诺余项的一阶泰勒展开

>

我们知道对于一元函数$f(x)$ , 假定$f(x)$一阶可导，我们有

$$

\lim_{x \to x_0} \frac{f(x)-f(x_0)}{x-x_0}=f'(x_0)

$$

即当$x \to x_0$时，有

$$

f(x) = f(x_0) + f'(x_0)(x-x_0)+o(x-x_0)

$$

其中$o(x-x_0)$表示比$x-x_0$更高阶的无穷小。它便是一阶泰勒展开的皮亚诺余项。

用一阶泰勒展开去近似原函数相当于用线性函数去逼近，如图中黄色直线所示，下图中

$x_0=0$,$f'(0)=1$


### (2)知识点：最小二乘法

+ 最小二乘法的原理与要解决的问题

最小二乘法形如下式：

$$

目标函数= \sum (观测值-理论值)^2

$$

观测值就是我们的多组样本，理论值就是我们的假设拟合函数。目标函数也就是在机器学习中常说的损失函数，我们的目标是得到使目标函数最小化时候的拟合函数的模型。

举一个最简单的线性回归的简单例子，比如我们有m个只有一个特征的样本：$(x_i,y_i)(i=1,2,3...,m)$样本采用一般的$h_0(x)$为n的多项式拟合，

$$

h_\theta(x)=\theta_0+\theta_1x+\theta_2x^2...+\theta_nx^n,\theta(\theta_0,\theta_1,\theta_2...,\theta_n)为参数

$$

+ 最小二乘法的矩阵法解法

最小二乘法的代数法解法就是对$\theta_i$求偏导数，另得为0，然后再解方程组，得到$\theta_i$。矩阵法比代数法要简洁，下面主要讲解下矩阵法解法，这里用多元线性回归例子来描述：

假设函数$h_0(x)=\theta_0+\theta_1x+\theta_2x^2...+\theta_nx^n$的矩阵表达式为：

$$

h_\theta(x)=X\theta

$$

其中，假设函数$h_\theta(x)=X\theta$为$m \times 1$的向量,可以理解为有m个样本，$\theta$为$n \times 1$的向量，这个由$h_0(x)=\theta_0+\theta_1x+\theta_2x^2...+\theta_nx^n$确定的n维，里边由n个代数法的模型参数。X为$m \times n$维矩阵。m代表样本个数，n代表样本的特征数。

损失函数定义为

$$

J(\theta)= \frac{1}{2}(X\theta-Y)^T(X\theta-Y)

$$

其中，Y是样本的输出向量，维度为$m \times 1$， $\frac{1}{2}$主要是为了求导为1，方便计算。

根据最小二乘法的原理，我们要对这个损失函数对$\theta$向量求导取0.结果如下：

$$

\frac{\partial J(\theta)}{\partial \theta}=X^T(X\theta-Y)=0

$$

对上式整理后，得到：

$$

\theta=(X^TX)^{-1}X^TY

$$

+ 最小二乘法的几何解释

最小二乘法的几何意义是高维空间中的一个向量在低维子空间的投影

+ + 简单问题

一元二次方程组：

$$

\begin{cases}

x_1+x_2=3 \leftarrow a\\

-x_1+x_2=1 \leftarrow b

\end{cases}

$$

方程组的解就是直线a，b的交点，并且$x_1=1,x_2=2$,写为矩阵形式为：

$$

\begin{bmatrix}

1\\

-1

\end{bmatrix}

\times x_1+

\begin{bmatrix}

1\\

1

\end{bmatrix}

\times x_2=b

\Leftrightarrow

a_1 \times x_1+

a_2 \times x_2=b

$$

表示$x_1$倍的向量$a_1$加上向量$a_2$等于向量b。或者说b是向量$a_1$与$a_2$的线性组合。

如果有更多的点$（0,2）,（1,2）,（2,3）$,假设一条直线穿过$y=kx+b$,用$x_1$代替k，用$x_2$代替b，那么这条直线可以表示为：

$$

\begin{cases}

0\times k+b=2\\

1\times k+b=2\\

2\times k+b=3

\end{cases}

\Leftrightarrow

\begin{cases}

0\times x_1+x_2=2\\

1\times x_1+x_2=2\\

2\times x_1+x_2=3

\end{cases}

\Leftrightarrow

\begin{bmatrix}

1&1\\

0&1\\

2&1

\end{bmatrix}

\begin{bmatrix}

x_1\\

x_2

\end{bmatrix}

=

\begin{bmatrix}

2\\

2\\

3

\end{bmatrix}

\Leftrightarrow

A\times x=b

$$

进一步有：

$$

\begin{bmatrix}

0\\1\\2

\end{bmatrix}

\times x_1+

\begin{bmatrix}

1\\1\\1

\end{bmatrix}

\times x_2=

\begin{bmatrix}

2\\2\\3

\end{bmatrix}

\Leftrightarrow

a_1 \times x_1+

a_2 \times x_2=b

$$

作图后发现，无论怎样更改$a_1,a_2$系数都不可能得到b，因为$a_1,a_2$的线性组合只能落在他们组成的子空间s平面中，也就是说b向量不在平面s中。


虽然找不到，但是在s中存在一个较为近似的向量，也就是b向量在s平面中的投影，即投影在s平面上的投影P就是b的近似向量，并且方程$A\widetilde{x}=P$是有解的。


这个误差最小的时候就是e正交平面s，也正交于s平面中的$a_1,a_2$（矩阵A的列向量），即点乘为0，$a_1^{T}e=0,a_2^{T}e=0$矩阵表示：

$$

A^{T}e=0\\

A^{T}(b-A\widehat{x})=0\\

A^{T}A\widehat{x}=A^{T}b

$$

我们可以得出，它的几何意义就是高维空间中的一个向量在低维子空间上的投影。

从上面可以看出，最小二乘法适用简洁高效，比梯度下降这样的迭代法似乎方便很多。但是这里我们就聊聊最小二乘法的局限性。

首先，最小二乘法需要计算$X^{T}X$的逆矩阵，有可能它的逆矩阵不存在，这样就没有办法直接用最小二乘法了，此时梯度下降法仍然可以使用。当然，我们可以通过对样本数据进行整理，去掉冗余特征。让$X^{T}X$的行列式不为0，然后继续使用最小二乘法。

第二，当样本特征n非常大时，计算$X^{T}X$的逆矩阵是一个非常耗时的工作，$n\times n$的矩阵求逆甚至不可行。此时以梯度下降为代表的迭代法仍然可以使用。那这个n到底多大呢？如果你没有很多的分布式大数据计算资源，建议超过10000个特征就用迭代法吧。或者通过主成分分析降低特征的维度后再用最小二乘法。

第三，如果拟合函数不是线性的，这时无法使用最小二乘法，需要通过一些技巧转化为线性才能使用，此时梯度下降仍然可以用。

> 最小二乘法就是对依据最小二乘问题构建的损失函数进行求导，令其等于0，然后解方程组。

$$

J(\theta)= \frac{1}{2}(X\theta-Y)^T(X\theta-Y)

$$

解得

$$

\theta=(X^TX)^{-1}X^TY

$$

+ 代码流程

1、给定初始值$x_0$;

2、对于第k次迭代，寻找一个增量$\Delta x_k$，使得$x_k+\Delta x_k$使得$J(x_k+\Delta x_k)$最小；

3、若$\Delta x_k$足够小就停止迭代；

4、否则，令$x_{k+1}=x_k+\Delta x_k$，返回第2步。

### (3)牛顿法

牛顿法（Newton's Method）是一种用于求解数值优化和非线性方程求解问题的迭代数值方法。它基于泰勒级数展开，通过不断逼近函数的根或极小值点，以寻找函数的最优解。牛顿法在机器学习、数值分析和优化领域广泛应用。

+ 牛顿法原理

首先定义一个实变量$x$，和其单变量函数$f(x)$，函数的一阶导数$f'(x)$和二阶导数$f''(x)$。

> 一元函数求根

假设有根的近似解$x_0$，则满足假设且接近根的值为：

$$

x_1=x_0-\frac{f(x_0)}{f'(x_0)}

$$

以此类推，可以得到：

$$

x_{n+1}=x_n-\frac{f(x_n)}{f'(x_n)}

$$

如下图所示：牛顿法的逼近过程

](牛顿法.gif)

牛顿法通过迭代逼近函数的根或极小值点，即找到一个$x$，使得$f(x)=0$。

因此，根据上面的流程，可以逐步求得$f(x)=0$的根。

假设当前的近似解为$x_n$，那么迭代后的下一个解，列出当前的切线方程为：

$$

f(x)=f(x_n)+f'(x_n)(x-x_n)

$$

设定其在$x$轴的截距为$x_n+1$,将（x_n+1,0）$代入方程可以得到：

$$

f(x)=f(x_n)+f'(x_n)(x_{n+1}-x_n)=0

$$

即：

$$

x_{n+1}=x_n-\frac{f(x_n)}{f'(x_n)}

$$

因此，得到了在函数$f(x)$上求解$f(x)=0$的方法。

> 求函数极值

我们知道函数的极值的必要条件是其一阶微分等于0，那么求解微分方程为0时候的值，从而函数的极值了。

一元函数为例$f(x)$:

首先在$x_0$处进行二阶泰勒展开，得到：

$$

f(x)=f(x_0)+f'(x_0)(x-x_0)+\frac{1}{2}f''(x_0)(x-x_0)^2+o(x-x_0)^2

$$

其中，由于泰勒展开的特性，后面$o(x-x_0)^2$部分不考虑，我们只考虑前面展开部分的极值问题：

$$

g(x)=f(x_0)+f'(x_0)(x-x_0)+\frac{1}{2}f''(x_0)(x-x_0)^2

$$

上面的式子是一个一元二次函数，那么其极值就是一阶导数为0的时候，我们可以先微分：

$$

g'(x)=f'(x_0)+f''(x_0)(x-x_0)

$$

令$g'(x)=0$，则此时的$x_1$就是极值（为了方便说明，不考虑鞍点情况）。

故$f'(x_0)+f''(x_0)(x_1-x_0)=0$

即：

$$

x_1=x_0-\frac{f'(x_0)}{f''(x_0)}

$$

以此类推可以得到迭代公式：

$$

x_{n+1}=x_n-\frac{f'(x_n)}{f''(x_n)}

$$

根据这个方法就可以不断的迭代下去直到收敛，最终找到极值了。

如果是多元的情况，则一阶导数$f'(x)$被叫做梯度，也称之为雅可比矩阵$J$（这里不太严谨。严格来说，矩阵的梯度为一阶导的转置，函数的梯度为一阶导，这里并没有进行详细的区分），

二阶导数矩阵$f''(x)$叫做海森矩阵$H$（Hessian Matrix）。如果是收敛的话，$\Delta x=x_{n+1}-x_n\approx 0$则式子可以转化为：

$$

\Delta x=-\frac{f'(x_n)}{f''(x_n)}=-\frac{J(x_n)}{H(x_n)}

$$

也就是说：

$$

H\Delta x=-J

$$

这样，就可以求出能够取得函数$f(x)$的极值点，继而算出函数$f(x)$的极值。

由于牛顿法需要算二阶导数，如果高阶的话，需要算海塞矩阵，这里是有三个缺陷：

要求给定的方程需要二阶可导

非凸函数的海森矩阵不一定有逆

数据较大的时候，海塞矩阵的计算量偏大

因此，需要思考别的方法来进行最小二乘问题的优化和求解。

### （4）梯度下降法

梯度下降是用于找到可微函数的局部最小值的一阶迭代优化算法。为了使用梯度下降找到函数的局部最小值，我们采取与该数在当前点的梯度（或近似梯度）的负值成比例的步骤。


梯度下降算法是一种迭代算法，它通过迭代来不断逼近函数的局部最小值。

梯度下降法是基于观测原理而来的：

如果实值函数$f(x)$在点$x=a$处可微且有定义，那么函数$f(x)$在点$x=a$处，沿着梯度的方向$-\Delta f(a)$下降最快。

因此，假设有个点b满足：

$$

b=a-\gamma\Delta f(a)  \quad \quad s.t \quad \gamma>0 ,\gamma \rightarrow 0

$$

那么我们就可以得到：

$$

f(a) \ge f(b)

$$

通过这种方法，就可以找到极小值。

因此，得到迭代公式：

$$

x_{n+1}=x_n-\gamma\Delta f(x_n)

$$

其中，$\gamma$是人为设定的参数，通过迭代，就可以得到极值。

梯度下降法每次都以梯度的反方向下降，所以，有可能会容易走出锯齿路线，从而增加迭代次数。


### （5）高斯牛顿法

如果代入到最小二乘问题中，牛顿法和梯度下降法都是针对目标函数$F(X_K)$来进行求解的，这样就不可避免的需要求得海森矩阵$H(X_K)$，为了避免这个问题，我们选取了误差函数$f(X)$来进行优化求解：

$$

minF(X)=\frac{1}{2} ||f(x)||_2^2

$$

那么，我们从上面的迭代步骤2中可以看到：

最小二乘，迭代步骤：

1、给定初始值$x_0$;

2、对于第k次迭代，寻找一个增量$\Delta x_k$，使得$x_k+\Delta x_k$使得$||f(x_k+\Delta x_k)||_2^2$达到极小值；

3、若$\Delta x_k$足够小就停止迭代；

4、否则，令$x_{k+1}=x_k+\Delta x_k$，返回第2步。

那么，我们对$f(x+\Delta x)$进行一阶泰勒展开：

$$

f(x+\Delta x) \approx f(x)+J(x)^T\Delta x +o(\Delta x)

$$

需要求$\Delta x$使得上边的式子$||f(x_k+\Delta x_k)||_2^2$有最小值，所以可以得到最小二乘问题为：

$$

\Delta x^*=\arg\min ||f(x_k+\Delta x_k)||_2^2 \approx \arg\min ||f(x)+J(x)^T\Delta x||_2^2

$$

为了求极值，对其求导：

$$

m(x)=\frac{1}{2}||f(x)+J(x)^T\Delta x||^2\\=\frac{1}{2}(f(x)+J(x)^T\Delta x)^T(f(x)+J(x)^T\Delta x)\\

=\frac{1}{2}(||f(x))||^2+2f(x)^TJ(x)^T\Delta x+\Delta x^TJ(x)J(x)^T\Delta x\\

$$

故，对其求导可以得到：

$$

m'(x)=J(x)f(x)+J(x)J(x)^T\Delta x=0

$$

则，此时可以转化为线性求解问题：

$$

m'(x)=0 \rightarrow J(x)J(x)^T\Delta x=-J(x)f(x)

$$

令$J(x)J(x)^T$定义为$H(x)$，令$-J(x)f(x)$定义为$g(x)$，则有：

$$

H(x)\Delta x=g(x) \quad \quad s.t \quad H(x)=J(x)J(x)^T  ,g(x)=-J(x)f(x)

$$

这样，就可以优化求解了。上面的最小二乘的优化步骤就可以变为:

1、给定初始值$x_0$;

2、对于第k次迭代，求出当前的雅可比矩阵$J(x_k)$和误差函数$f(x_k)$；

3、求解增量方程：$H(x_k)\Delta x=g(x_k)$；

4、若$\Delta x$足够小就停止迭代；

5、否则，令$x_{k+1}=x_k+\Delta x$，返回第2步。

相比较于传统的最小二乘求解方法，只更改了两个步骤。该方法的优点和缺点如下：

> 优点：

+ 避免了求海塞矩阵，大大减少了计算量。

>缺点：

+ 为了求解$H^-$,需要$H$矩阵可逆，但是实际上$JJ_T$只有半正定性，所以，当为奇异矩阵的时候，稳定性较差，算法不收敛。

+ 如果求出来的步长$\Delta x_k$太大，会导致其局部近似不精确，严重的时候，可能无法保证迭代收敛。

+ 容易和梯度下降法一样，陷入锯齿状，导致迭代次数较长。

不过，为了能够更好的进行最小二乘问题的求解，我们可以使用列文伯格-马夸特法(LM)来进行求解。

### （6）列文伯格-马夸特法

该方法是在高斯牛顿法的基础上进行的改进，基本的思路和原理和高斯牛顿法一样。

在高斯牛顿法的缺点中，可以看到，有一点使容易进入锯齿状，导致迭代的次数较长。所以，为了避免其步长过大导致的问题，该方法提出了信赖区域，设定一个区域。使得步长能够受到控制。

在更新迭代的过程中，为了判定近似值的好坏，我们设定了一个评判指标：

$$

\rho=\frac{f(x+\Delta x_k)-f(x)}{J(x)\Delta x}

$$

这个指标就是我们的近似指标，可以看到其分为以下几种情况：

+ $\rho$接近1，近似是好的，不需要更改；

+ $\rho$太小，则实际减少的值小于近似减少的值，近似较大，需要缩小近似的范围；

+ $\rho$太大，则实际减少的值大于近似减少的值，近似较小，需要扩大近似的范围。

这样的话，就可以动态调整步长了。

当找到符合要求的近似结果后，就可以进行后续正常的迭代更新了。

因此，使用该信赖区域后，可以更新算法流程：

> 算法流程：

1、给定初始值$x_0$;

2、对于第k次迭代，在高斯牛顿法的基础上加入信赖区域：

$$

min\frac{1}{2}||f(x_k)+J(x_k)^T\Delta x_k||_2^2 \quad \quad \quad s.t \quad ||D\Delta x_k||_2^2 \le \mu

$$

其中，$\mu$是信赖区域的宽度（信赖半径），$D$是系数矩阵。

3、计算近似指标$\rho$:

$$

\rho=\frac{f(x_k+\Delta x_k)-f(x_k)}{J(x_k)\Delta x_k}

$$

4、根据经验值，设定：

+ 若$\rho > \frac{3}{4}$,则设置$\mu=2\mu$;，跳转至第6步；

+ 若$\rho < \frac{1}{4}$,则设置$\mu=\mu/2$;，跳转至第6步；

+ 若$\rho$大于设定的阈值，则跳转至第5步，求解$\Delta x_{k+1}=x_k+\Delta x_k$；

5、求解增量方程：$(H+\lambda I)\Delta x_k=g$；

6、若$\Delta x_k$足够小就停止迭代；，否则返回第2步。

### （7）拟牛顿法

拟牛顿法（Quasi-Newton）是一种用于求解非线性优化问题的迭代算法。它通过使用拟牛顿矩阵来近似海塞矩阵，从而避免计算海塞矩阵的复杂性。

### （8）L-BFGS

L-BFGS（Limited-memory BFGS）是一种拟牛顿法，它使用内存中的有限数量的函数和梯度来近似海塞矩阵，从而避免计算海塞矩阵的复杂性。

### （9）共轭梯度法

共轭梯度法（Conjugate Gradient Method）是一种用于求解线性方程组的迭代算法。它通过使用共轭方向来加速收敛，并避免在每次迭代时重新计算矩阵的逆。

### （10）BFGS

BFGS（Broyden-Fletcher-Goldfarb-Shanno）是一种用于求解非线性优化问题的迭代算法。它通过使用二阶导数的信息来加速收敛，并避免在每次迭代时重新计算矩阵的逆。

### （11）牛顿-拉格朗日法

牛顿-拉格朗日法（Newton-Raphson Method）是一种用于求解非线性优化问题的迭代算法。它通过使用牛顿法来求解函数的根，并使用拉格朗日乘子法来求解约束条件。

### （12）OWL-QN

OWL-QN（Optimization With Limited Workspace and Quasi-Newton）是一种用于求解非线性优化问题的迭代算法。它通过使用有限的工作空间和拟牛顿法来加速收敛。

## hectorslam中高斯牛顿法

扫描匹配就是使用当前帧与已有地图数据构建误差函数，并用Gauss_Newton法得到最优解和偏差量。其工作是实现激光点到栅格地图的转换，t时刻所有的激光点都能变换到栅格地图中，也就意味着匹配成功。