Hector SLAM算法学习与代码解析

最新推荐文章于 2025-06-22 19:37:41 发布

不学好学什么

最新推荐文章于 2025-06-22 19:37:41 发布

阅读量2.3w

点赞数 13

CC 4.0 BY-SA版权

分类专栏：移动机器人 slam的原理文章标签： hectorSLAM 2d-slam

本文链接：https://blog.youkuaiyun.com/Nksjc/article/details/72779564

本文详细介绍了Hector SLAM算法的学习过程，包括Occupancy Grid Map、Map Access、Scan Matching和Multi-Resolution Map Representation。通过双线性插值方法评估激光点在栅格地图中的匹配程度，并通过迭代匹配优化位姿增量。文章还讨论了算法的局限性，如双线性插值的不连续性和缺乏地图修正能力。同时提到了Cartographer的三线性差值改进。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

Hector SLAM算法学习与代码解析

最初了解到Hector SLAM 是从https://www.youtube.com/embed/F8pdObV_df4看到手持激光建图，被吸引到了也就想看一下具体的实现。关于Hector SLAM算法有一篇论文：A Flexible and Scalable SLAM System with Full 3D Motion Estimation；以及开源代码：https://github.com/tu-darmstadt-ros-pkg/hector_slam。对照着论文和代码，一睹算法真容，主要分享对论文的第四部分2D SLAM的理解。

Hector SLAM算法学习与代码解析

Occupancy Grid Map

这里写图片描述

Map Access

初始化的时候，将获取的激光数据作为第一帧处理映射到地图中。在t时刻，激光获取到新的激光数据，想要与t-1时刻的地图匹配，首先要把激光数据变换到栅格地图之中；例如激光点Pm变换到栅格地图中，我们希望的是激光点能够到一个“被占据”的栅格（灰色）中，如果t时刻所有的激光点都能够变换到被占据的栅格中，也就意味着匹配成功。
为了刻画激光点映射到栅格地图中是否对应被占据栅格中以及对应的程度，采用了双线性插值的方法，求出M(Pm)。M（P00)->M(P11)的范围是0 到 1，表示被占据的概率。

Map Access

对应的程序实现：

 Eigen::Vector3f interpMapValueWithDerivatives(const Eigen::Vector2f& coords)
  {
    //check if coords are within map limits.
    if (concreteGridMap->pointOutOfMapBounds(coords)){
      return Eigen::Vector3f(0.0f, 0.0f, 0.0f);
    }

    //map coords are always positive, floor them by casting to int
    Eigen::Vector2i indMin(coords.cast<int>());

    //get factors for bilinear interpolation
    Eigen::Vector2f factors(coords - indMin.cast<float>());

    int sizeX = concreteGridMap->getSizeX();

    int index = indMin[1] * sizeX + indMin[0];

    // get grid values for the 4 grid points surrounding the current coords. Check cached data first, if not contained
    // filter gridPoint with gaussian and store in ca