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映射包上。 此外，核心的姿态估计和点云网格绘制操作主要位于HectorSlamProcessor.h的函数update中。 如下所示。
函数newposeestimateworld=(映射匹配数据(posehintworld，数据容器，lastScanMatchCov ) )是输入的激光点云数据和现有的这一部分是整个代码的中心，也是整个代码的中心
当然，如果知道机器人的正确姿势不需要进行上面的姿势计算，就用直接函数maprep-updatebyscan (data container，newPoseEstimateWorld )更新网格地图。 该算法的原理比较简单，主要有两个方面。 一种是用对数表示各子网格的概率值。 第二，使用Bresenham划线算法更新子网格的概率值。
为了说明上述算法的原理，先介绍一部分预备知识，再进入正题。
2、hector-slam的预备知识
2-1占用网格地图
在此直接使用上述参考报道进行说明，假设机器人的姿势已知，则每次扫描激光数据时，障碍物在网格图上的实际占有概率都会上升，最终无限接近1，没有障碍物的地方接近0 整个栅格贴图被划分为分辨率恒定的子栅格，最终贴图的概率分布如下：
p(m|z({1:t}，x_{1:t} ) prodp ) 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 _ { 133365365365374; t }
如果实际使用栅格地图进行导航和布线规划，则每个栅格只有三种状态：占用、空闲和未知。 这三种状态只要判断p(m_{i}|z_{1:t}、x_{1:t} )或l_{t，I}的设定范围即可，例如hectotor
2-2非线性优化——高斯差悟空法
在处理优化问题时，经常求出某些指标的最高值。 例如，最大jpdj次幂、最大似然、最小方差等。 虽然hector slam在求解姿态时也构造了最大的jpdj乘方问题，但由于使用的模型是姿态的非线性函数，因此需要使用非线性优化的方法。 hector slam使用高斯坏悟空法。 现在简单说明高斯糟糕的悟空法。
非线性最大jpdj次幂问题可以用以下公式描述：
x=argminf(x )=argmin ) frac {1} {2} (left )|f (x ) ) ) right(|_{2}^{2}
如果f(x )的数学式比较简单，则可以通过直接导出f(x )，并将其导出为等于0来求出极值。 如果求不到导，一般用迭代求解Delta x。 迭代值倾向于使f(x )接近最小值，直到Delta x的变化较小或达到最大迭代次数。 关键问题是寻求三角洲x。 这里有梯度下降法、坏悟空法、坏悟空sdhlb、LM法等多种方法。 简单来说，梯度下降法——对成本函数f(x )进行安静的睫毛展开，只留下一阶微分部分，在梯度的相反方向上进行迭代。 即，(Deltax=-j ) x )，但该方向不一定是最佳方向，在某些情况下反复过多，反而有可能绕远。 坏悟空法——其实是上面f(x )的静睫毛展开，一直留到二次，增加了axdlm矩阵，所以其迭代的路径一般较短，但需要计算f(x )的二次导数，这是很难求得的，所以实际应用受到限制差悟空sdhlb——为了避免差悟空法求解海塞矩阵难题，首先差悟空sdhlb对f(x )进行静睫毛展开，然后得到f ) x )近似表达式，其导数为0，f ) x )局部最小时间的LM法是对不良悟空sdhlb进行修改，以避免不良悟空sdhlb求解正规方程时可能出现的奇异解，被广泛使用。
不用说，我就直接介绍高斯糟糕的悟空法。
f(x(Deltax ) approxf ) x (j ) x ) Deltax(rightarrow
f(x )=(FRAC({1}{2} ) x ) )Delta x ) ^{t} ) x ) j ) )。
Delta x ) RightarrowF{}'(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通过最jpdj乘的方式来表达x_{t}=argmaxP(m_{t-1}|x_{t},z_{t})最大似然问题(最jpdj乘实际上就是最大似然估计的一种)：
x=argminsum [1-M(S_{i}(x))]^{2}
未完待续。。。。