SLAM——hector-slam算法原理解析

article/2025/8/28 2:48:00

1、hector-slam代码框架概述

下载源码：git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git

原理解读参照：https://blog.csdn.net/weixin_40047925/article/details/80679496

其中包含了许多用于仿真的文件，hector slam算法主要集中在hector mapping包中，同时核心的位姿估计和点云建立栅格图操作主要在文件HectorSlamProcessor.h的函数update中。如下：

函数newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov))根据输入的激光点云数据和现有的栅格图进行对齐，这部分是整个代码的核心，主要思想是使用高斯牛顿优化代价函数，求取让代价函数达到最优的位姿偏量，从而求出当前机器人的位姿态。

当然，如果已经知道机器人的准确位姿不需要进行上面的位姿解算，直接用函数mapRep->updateByScan(dataContainer, newPoseEstimateWorld)更新栅格地图。这部分算法原理比较简单，主要两个方面：一是，用对数表达每一个子栅格的概率值；二是，用Bresenham 画线算法来更新子栅格的概率值。

为了说明上述算法原理，先介绍一部分的预备知识，然后再切入正题。

2、hector-slam预备知识

2-1 占用栅格地图

这里直接使用上述参考文章进行说明，假设机器人的位姿已知，那么经过每一次激光数据扫描，真实障碍物在栅格地图上的占用概率不断提升最终无限接近1，而没有障碍物的地方则趋近于0。整个栅格地图被分成了一定分辨率的子栅格，最终地图的概率分布如下：

$p(m|z_{1:t},x_{1:t})=\prod p(m_{i}|z_{1:t},x_{1:t})$

整个栅格图构建需要计算每一个子栅格的占用概率值。实际在表达栅格占用概率使用对数形式表达，好处是避免在0和1附近概率值不稳定的问题。具体如下：

$l_{t,i}=log\frac{p(m_{i}|z_{1:t},x_{1:t})}{1-p(m_{i}|z_{1:t},x_{1:t})}$ $p(m_{i}|z_{1:t},x_{1:t})=1-\frac{1}{1+e^{l_{t,i}}}$

在实际使用栅格地图进行导航或者路径规划时，每一个栅格仅有三个状态：占用、空闲和未知，该三种状态仅需对 $p(m_{i}|z_{1:t},x_{1:t})$ 或者 $l_{t,i}$ 设定范围进行判断即可，比如hector slam中是 $l_{t,i}$ 值大于0为占用，小于0为空闲，等于0为未知。

2-2 非线性优化——高斯牛顿法

在处理优化问题时经常会求去某个指标的最值，比如最小二乘、最大似然、最小方差等，在hector slam求解位姿时也构造了一个最小二乘问题，但是使用的模型是关于位姿的非线性函数，所以需要使用非线性优化的方法，hector slam使用的是高斯牛顿法。这里对高斯牛顿法做个简单说明。

对于非线性最小二乘问题，可以用如下数学表达式来描述：

$x = argminF(x)=argmin\frac{1}{2}\left \|f(x))\right \|_{2}^{2}$

如果f(x)数学表达式比较简单，可以直接对F(x)求导，并领其等于0，可以求出极值。对于无法求导情况，一般用迭代求解 $\Delta x$ ，求解的迭代值不断的趋使F(x)趋近最小值，直到 $\Delta x$ 变化很小或者达到最大迭代次数。那么关键问题在于求取 $\Delta x$ ，这里有很多方法，如：梯度下降法、牛顿法、牛顿高斯法和LM法。简单比较一下：梯度下降法——对代价函数F(x)进行泰勒展开，仅保留一阶导数部分，按照梯度反方向进行迭代，即 $\Delta x=-J(x)$ ，但是这种方向并不一定是最合适的方向，有时可能迭代过头，反而会绕路；牛顿法——其实是上面F(x)泰勒展开，保留到二阶，增加了海森矩阵，因此其迭代的路径一般是比较短的，但是需要计算F(x)的二阶导数，这个是很难求得的，因此实际应用受限；牛顿高斯法——为了避免牛顿法求解海塞矩阵难的问题，牛顿高斯法先对f(x)进行一节泰勒展开，然后得到F(x)近似表达式，然后另其导数为0，求取F(x)局部最小时的 $\Delta x$ ，然后不断迭代，直至达到收敛条件；LM法是在牛顿高斯法上进行了修改，避免了牛顿高斯法求解正规方程时可能出现的奇异解，被广泛使用。

话不多说，直接介绍高斯牛顿法。

$f(x+\Delta x) \approx f(x) + J(x)\Delta x \Rightarrow$

$F(x) = \frac{1}{2}( f(x) + J(x)\Delta x )^{T}( f(x) + J(x)\Delta x ) \Rightarrow$

$F{}'(x) = J^{T}(x)( f(x) + J(x)\Delta x ) =0 \Rightarrow$

$J^{T}(x)J(x)\Delta x = - J^{T}f(x)\overset{H = J^{T}(x)J(x)}{\rightarrow}\Delta x=-H^{-1}J^{T}f(x)$

和牛顿法比较实际上是用雅克比矩阵二次来代替了海塞矩阵，省去了计算海塞矩阵。

3、hector-slam位姿解算

当我们传感器第一次接收到数据，我们可以人为设定当前位置，以此来第一次建立栅格地图m，可以认为是初始化操作。之后，我们在根据前一时刻建立的地图求解的位姿都是相对初始化时刻的位姿而言的，这一点需要明白。那么这个问题可以用贝叶斯估计描述为：

$p(x_{t}|m_{t-1},z_t)=\beta p(m_{t-1}|x_{t},z_{t})*p(x_{t}|x_{t-1})$

这里假设状态x符合马尔科夫过程，但是对于先验 $p(x_{t}|x_{t-1})$ 概率很多情况下是不知道的，如果有这部分的先验的话可以添加这部分代码。在没有这部分信息情况下，hector-slam是让似然概率 $p(m_{t-1}|x_{t},z_{t})$ 达到最大的最大似然估计。hector-slam通过最小二乘的方式来表达 $x_{t}=argmaxP(m_{t-1}|x_{t},z_{t})$ 最大似然问题(最小二乘实际上就是最大似然估计的一种)：