SLAM2d栅格地图构建的常用方法

已于 2022-09-15 08:42:05 修改
阅读量3.1k 收藏 29
点赞数 3
分类专栏： 0_1SLAM 文章标签： c++ 机器人 自动驾驶
于 2022-09-14 17:17:46 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_42911741/article/details/126791536
版权

        最近在总结一些平时做过的东西，记录一下，哈哈哈。。。相信很多做SLAM的小伙伴初学肯定都跑过gmapping，没错，它建立的那就是栅格地图，可以用于导航规划。下面就记录一下，学过的三种构建方法（在机器人位姿已知且能拿到传感器的观测数据）:

0、说明

 世界坐标系：

  由于相机/雷达可安放在环境中的任意位置，在环境中选择一个基准坐标系来描述摄像机的位置（一般是起始帧），并用它描述环境中任何物体的位置，该坐标系称为世界坐标系。

nav_msgs::OccupancyGrid消息结构：（注意单位）

std_msgs/Header header          # 数据的消息头
  uint32 seq                    # 数据的序号
  time stamp                    # 数据的时间戳
  string frame_id               # 地图的坐标系
nav_msgs/MapMetaData info       # 地图的一些信息
  time map_load_time            # 加载地图的时间
  float32 resolution            # 地图的分辨率，一个格子代表着多少米，[m/cell]
  uint32 width                  # 地图的宽度，像素的个数, [cells]
  uint32 height                 # 地图的高度，像素的个数, [cells]
  geometry_msgs/Pose origin     # 地图左下角的格子对应的物理世界的坐标，[m, m, rad]

int8[] data # 地图数据，占用值的范围为[0,100]，未知为-1。实际上也是可以赋予大于100的值，显示杂七杂八的颜色。。。

        特别说明的参数是：

frame_id：地图像素坐标系关联到的坐标系（world/map），是一个物理世界的坐标系；用于将像素坐标转换为真实的物理坐标；

origin：地图本身是只有像素坐标系的，其像素坐标系原点为左下角为(0, 0) ，不设置偏移参数时，轴向与frame_id是重合的；

        origin设置（x,y）=（2,2）:

        所以，origin应该设置的是像素坐标系在frame_id那个坐标系下的位姿，这里也可以称为像素系到frame_id系的坐标变换；通过左下角栅格对应的物理坐标 origin 以及 分辨率，再通过 像素 * 分辨率 + origin , 将像素坐标转成物理世界的坐标，从而确定了整个地图的物理坐标。另外栅格坐标就可以理解为像素坐标；

data[]：按照那张地图图片的自底向上，自左至右逐个像素点存储的（行逆序、列顺序）

 // ros栅格地图消息格式
    nav_msgs::OccupancyGrid rosMap;
 // 分辨率， 单位 m/pixel，一个栅格多少米
    rosMap.info.resolution = 0.05;  
 // 长宽， 单位：像素pixel ---> 地图实际是长和宽各 900x0.04 = 36m长度    
    rosMap.info.width = 900;    
    rosMap.info.height = 900;
 // 原点位置，pgm文件的左下角像素坐标在真实坐标系下的位置（frame_id），也就是栅格地图yaml文件的origin参数信息）
    rosMap.info.origin.position.x = 0.0;   
    rosMap.info.origin.position.y = 0.0;
    rosMap.info.origin.position.z = 0.0;
    rosMap.info.origin.orientation.x = 0.0;
    rosMap.info.origin.orientation.y = 0.0;
    rosMap.info.origin.orientation.z = 0.0;
    rosMap.info.origin.orientation.w = 1.0;
//  栅格值（像素值0-100，白色到黑色渐变，-1表示未知） 
    rosMap.data.resize(rosMap.info.width * rosMap.info.height); 
    for (int i =0; i<rosMap.info.width * rosMap.info.height; ++i )
        rosMap.data[i] = 50;

 还有疑问的话可以自己去测试一下：

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "GridMap_test");
    ros::NodeHandle n;
    ros::Publisher map_pub = n.advertise<nav_msgs::OccupancyGrid>("/gridMap", 1);

    nav_msgs::OccupancyGrid map;
    map.header.frame_id = "map";
    map.header.stamp = ros::Time::now(); 
    map.info.resolution = 0.05;          // m/ceil
    map.info.width      = 100;           // ceil
    map.info.height     = 100;          
    map.info.origin.position.x = 2.0;
    map.info.origin.position.y = 2.0;
    map.info.origin.position.z = 0.0;
    map.info.origin.orientation.x = 0.0;
    map.info.origin.orientation.y = 0.0;
    map.info.origin.orientation.z = 0.0;
    map.info.origin.orientation.w = 0.0;
    map.data.resize(map.info.width * map.info.height);

    for (int i=0; i<map.info.width*map.info.height; ++i) {
        if (i<70)
            map.data[i] = 0;
        else {
            map.data[i] = -1; 
        }
    }
    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        map_pub.publish(map);
        loop_rate.sleep();
    }
    return 0;
}

一、覆盖栅格法

  • 每个栅格有两种状态：占用(Occupied)或者空闲(free)；
  • 该算法对某一个栅格进行操作的时候，只有加法操作，更新速度快；
  • 设置栅格每次占有/空闲的加减值；
  • code：

1、地图坐标到像素坐标的转换：

// input： 地图坐标系下的坐标
// return： 像素坐标系的坐标
// xy与origin都是map下的坐标
// 像素坐标系 = 栅格坐标系
GridIndex ConvertWorld2GridIndex(double x,double y)
{
    GridIndex index;
    // std::ceil   -->向上取整数   std::floor -->向下取整数
    index.x = std::ceil((x - mapParams.origin_x) / mapParams.resolution) + mapParams.offset_x;  // 设置了一个像素坐标的便宜值
    index.y = std::ceil((y - mapParams.origin_y) / mapParams.resolution) + mapParams.offset_y;
    // index.x = std::ceil((x - mapParams.origin_x) / mapParams.resolution);
    // index.y = std::ceil((y - mapParams.origin_y) / mapParams.resolution);
    return index;
}

2、将机器人和激光点在map下的坐标转换为像素坐标（robotIndex和pointGridIndex）：

 // 机器人坐标 对应的栅格坐标
GridIndex robotIndex = ConvertWorld2GridIndex(robotPose(0),robotPose(1)); 
 // 一帧
for(int id = 0; id < scan.range_readings.size();id++) {
            double dist = scan.range_readings[id];  // 激光深度
            double angle = scan.angle_readings[id]; // 在激光系下的偏角
            // 去除异常数据
            if(std::isinf(dist) || std::isnan(dist)) continue;

            // 2d平面的激光点， 激光点在激光系下的坐标
            double theta = robotPose(2);    // 机器人在地图下的偏角
            double laser_x = dist * cos(angle);  
            double laser_y = dist * sin(angle);
            // 激光点在地图下的坐标 2d空间的旋转矩阵
            double world_x = cos(theta) * laser_x - sin(theta) * laser_y + robotPose(0);
            double world_y = sin(theta) * laser_x + cos(theta) * laser_y + robotPose(1);
            // 激光点的栅格坐标
            GridIndex pointGridIndex = ConvertWorld2GridIndex(world_x, world_y);  
            
            // 
            //算法调用。。。直接调用后面三个方法各自封装的函数即可。。
            //

}

3、根据策略，进行栅格值的更新：

// 激光点对应的栅格坐标
// 机器人对应的栅格坐标
// 占据栅格法
void OccGridMethod(GridIndex& pointGridIndex, GridIndex& robotIndex) { 

//如果二维平面，该机器人可以获得该激光点，那么机器人与激光点连线上肯定是空闲的
    std::vector<GridIndex> freeTraceindexes = TraceLine(robotIndex.x, robotIndex.y, pointGridIndex.x, pointGridIndex.y);    //Bresenham算法
    for(auto& index : freeTraceindexes) // 遍历机器人位置到激光点之间的栅格(不包含激光点所在栅格)
    {
        if(!isValidGridIndex(index))
          continue;
        
        int linearIndex = GridIndexToLinearIndex(index); // 取出该栅格对应的值
        // 1-根据空闲规则  初始值默认50
        if(pMap[linearIndex] == 0)  
            continue;
        pMap[linearIndex] += mapParams.log_free;    // mapParams.log_free = -1 
        
    }
    // 激光点所在栅格肯定是障碍物，机器人所走过的路径都是空闲的
    if(isValidGridIndex(pointGridIndex))    // 激光点对应的栅格
    {
        int linearIndex = GridIndexToLinearIndex(pointGridIndex);
        // 2-根据占据规则 
        pMap[linearIndex] += mapParams.log_occ;     // mapParams.log_occ = 2
        if(pMap[linearIndex] > 100)   
            pMap[linearIndex] = 100;
    }
}

4、可以根据建图的效果和传感器的噪声，调节log_free和log_occ的值；建图效果如下：

二、计数建图法

  • 每一个栅格统计两个量：𝑚𝑖𝑠𝑠𝑒𝑠(𝑖)和ℎ𝑖𝑡𝑠(𝑖)；
  • 𝑚𝑖𝑠𝑠𝑒𝑠(𝑖)表示栅格𝑖被激光束通过的次数，即被标为free的次数；
  • ℎ𝑖𝑡𝑠(𝑖)表示栅格𝑖被激光束击中的次数，即被标为occupied的次数；
  • 当ℎ𝑖𝑡𝑠(𝑖) / (𝑚𝑖𝑠𝑠𝑒𝑑(𝑖) + ℎ𝑖𝑡𝑠(𝑖))超过阈值则认为该栅格为Occupied，否则认为栅格是Free的；
  • 设置占有率阈值；
  • code：

1、计数法更新策略：

// 计数栅格法
// input: 激光点的像素坐标、机器人的像素坐标、占据率
void CntGridMethod(GridIndex& pointGridIndex, GridIndex& robotIndex, double occRate) { 

    std::vector<GridIndex> freeTraceindexes = TraceLine(robotIndex.x, robotIndex.y, pointGridIndex.x, pointGridIndex.y);
    for(auto& index : freeTraceindexes) // 遍历机器人位置到激光点之间的栅格(不包含激光点所在栅格),机器人所走过的路径都是空闲的
    {
        if(!isValidGridIndex(index))
          continue;
        
        int linearIndex = GridIndexToLinearIndex(index); // 取出该栅格对应的值
        // 根据空闲规则
        Misses_cnt[linearIndex]++;
        
    }
    // 激光点所在栅格肯定是障碍物
    if(isValidGridIndex(pointGridIndex))    // 激光点对应的栅格
    {
        int linearIndex = GridIndexToLinearIndex(pointGridIndex);
        // 根据占据规则 
        Hits_cnt[linearIndex]++;
    }
    // // 统计每个栅格的占有率  最后计算完，再统一算占有率
    // for (int i=0; i< mapParams.height*mapParams.width; ++i) {

    //   if ((Misses_cnt[i] + Hits_cnt[i])!=0) {   // 未扫到栅格=50 未知
    //     double r = double(Hits_cnt[i] / (Misses_cnt[i] + Hits_cnt[i]));
    //     if (r >= occRate)      // 占有
    //       pMap[i] = 100;
    //     else
    //       pMap[i] = r*100;     // 空闲
    //   }
    // }
}

2、建图效果如下：

三、构建tsdf（截断符号距离函数）

        上述两种方法都没有考虑到传感器的不确定性，导致建立的栅格地图，边界灰常 “厚”。TSDF方法使用加权线性最小二乘，使用多帧观测值进行平均计算来融合，能够有效减小测量噪声的影响。同时可以通过线性插值得到TSDF的零点位置来获取环境曲面的精确位置，最终得到障碍物仅会占用一个栅格。算力要求较以上两种高。

更新公式：

sdf_{i}(x) = laser_{i}(x) - dist_{i}(x)，laser传感器测量的距离，dist栅格离传感器原点的距离；

tsdf(x) = max(-1, min(1, sdf_{i}(x)/t))，t截止距离(大于截止距离的点不会用于曲面重建)；

TSDF_{i}(x) = \frac{W_{i-1}(x)TSDF_{i-1}(x)+w_{i}(x)tsdf_{i}(x)}{W_{i-1}(x)+w_{i}(x)}

W_{i}(x) = W_{i-1}(x) + w_{i}(x)，多次观测的融合更新方程；

        wi设置为1，每个栅格初始状态时，TSDF0(x) = 0，W0 = 0；每次有观测数据（即激光击中或穿过）都按照上面的方程更新栅格的TSDF值。该方程实际上就是更新每个栅格的TSDF平均值。物体边界就是寻找TSDF值正负交替的栅格，通过在两个栅格之间进行插值可以得到TSDF值为0的栅格坐标，而该坐标就是曲面的精确位置。TSDF场示意图如下所示：

code：

1、tsdf法则进行更新：

// tsdf栅格法：阶段式带符号距离函数（加权最小线性二乘）
// 激光点栅格坐标、机器人地图坐标、当前帧激光的深度
void TSDFGridMethod(GridIndex& pointGridIndex, GridIndex& robotIndex, Eigen::Vector3d& robotPose, double& dist) { 

  //111
    std::vector<GridIndex> freeTraceindexes = TraceLine(robotIndex.x, robotIndex.y, pointGridIndex.x, pointGridIndex.y);
    for(auto& index : freeTraceindexes) // 遍历机器人位置到激光点之间的栅格(不包含激光点所在栅格),机器人所走过的路径都是空闲的
    {
        if(!isValidGridIndex(index))
          continue;
        // 栅格坐标系转换到map下
        double x = (index.x - mapParams.offset_x) * mapParams.resolution + mapParams.origin_x;
        double y = (index.y - mapParams.offset_y) * mapParams.resolution + mapParams.origin_y;
        // 计算map下，激光点所在栅格与机器人位置的距离
        // 激光距离-计算的距离
        double d = std::sqrt( pow((x-robotPose(0)), 2) + pow((y-robotPose(1)), 2) );  // 栅格里传感器原点的距离
        double sdf = dist-d;
        double t = 0.1;   // 截止距离
        double tsdf = std::max(-1.0, std::min(1.0, sdf/t));    // tsdfi(x)，范围-1 to 1

        int linearIndex = GridIndexToLinearIndex(index);
        // 测量更新
        // 第一次 TSDFi = tsdfi，Wi = 1
        // 小w=1
        pMapTSDF[linearIndex] = ( pMapW[linearIndex]*pMapTSDF[linearIndex] + 1.0*tsdf ) / (pMapW[linearIndex] + 1); // 更新TSDFi(x)
        pMapW[linearIndex] += 1;         // 更新Wi(x)

    }
}

2、利用遍历更新后的TSDF场 ，找边界（正负值交界处）：

    for(int i= 0; i<mapParams.height-2; i++)  //假设i为y轴
    {
        for(int j = 0; j<mapParams.width-2; j++)  //假设j为x轴  
        { 
            int line_value = j + i*mapParams.height;
            int line_x = line_value +1;               //往x轴移动一格   右侧
            int line_y = line_value +mapParams.height;//往y轴方向移动一格   下侧
            //转map坐标
            double A_x = GridIndex2ConvertWorld(j);
            double A_y = GridIndex2ConvertWorld(i);
            double B_x = GridIndex2ConvertWorld(j+1);
            double B_y = GridIndex2ConvertWorld(i+1);
            double a,b,b1,x,y;
            a = pMapTSDF[line_value];  b= pMapTSDF[line_x];  b1 = pMapTSDF[line_y]; // 判断该点的下边、右边点是否为交界处

            if( a*b1 < 0){ //x方向   乘积为负数，交界处
                   x = A_x;
                   y = interpolation(A_y,B_y,a,b1);     //插值
                  pMap[GridIndexToLinearIndex(ConvertWorld2GridIndex(x,y))] = 100;
            }
            else if( a*b < 0){ //y方向
                   x = interpolation(A_x,B_x,a,b);
                   y = A_y;
                   pMap[GridIndexToLinearIndex(ConvertWorld2GridIndex(x,y))] = 100;
            }
                   
        }
    }

3、建图效果如下：

由于电脑比较拉跨，读入的数据点有限(只能读2000个点)，建出来的图有点尴尬。。。

bug大湿
关注
  • 3
    点赞
  • 踩
  • 29
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
占据栅格地图的构建（内含数据）
09-24
SLAM中有关占据栅格地图的的表示方法和利用激光传感器构建占据栅格地图的方法
3D激光SLAM点云地图pcd转导航可用的2D栅格地图
热门推荐
studentu的博客
08-14 1万+
本文旨在帮助读者将激光点云地图转为2D栅格地图，以便完成路径规划与导航。本方法将pcd转为pgm的原理是将接收到的点云信息以"/map"话题的形式发布，用map_server来接收"/map"话题，保存2D栅格地图！ 废话不多说，直接开始！ 一、安装pcd2pgm #创建工作空间 mkdir -p ~/pcd2pgm_ws/src cd ~/pcd2pgm/src catkin_init_workspace #克隆代码 git clone https://github.com/hujiax380/
占据栅格地图构建（Occupancy Grid Map）
「小白学移动机器人」一个专注分享移动机器人相关知识的公众号！
10-13 1万+
占据栅格地图构建（Occupancy Grid Map） 上一篇文章介绍了，移动机器人地图构建问题，主要以gmapping为例，讲解了地图构建的整个流程。看过前面文章的小伙伴肯定都知道，gmapping算法把SLAM问题分解成两个部分，定位问题和地图构建问题。而gmapping中的地图构建就是采用占据栅格地图构建算法实现的。 之前文章：https://blog.csdn.net/zhao_ke_xue/article/details/108944811 之前一直说地图构建问题是个简单问题，但它真的十分有用。
Octomap建立二维栅格地图
最新发布
Perfect_dream的博客
02-26 1465
使用Octomap生成二维占据栅格导航地图三维点云地图转二维栅格地图A-LOAM/LOAM/Lego-LOAM/SC_Lego_LOAM实时构建3d点云地图与2d栅格地图（octomap）
2D栅格-3D八叉树地图及其概率更新
dbdxnuliba的博客
12-15 1261
八叉树数学原理
Cartographer源码解析——2D栅格地图构建
m0_67976789的博客
04-22 3562
到目前为止，对于点云数据的预处理过程已经介绍完毕，如：点云数据多传感器时间同步、运动畸变校正、重力校正、体素滤波等。做完这一系列的预备工作之后，实际上呢，就可以进行点云的扫描匹配了。在讲解扫描匹配之前，先来看看 Cartographer 2D 的栅格地图，其不像3D点云地图有很多成熟的库可以调用，具有统一的标准。大多数 2D Slam 的栅格地图都是需要自己编写代码进行构建的。下面就来看看 Cartographer 中时如何构建的。
velodyne＋cartographer 2D构建栅格地图
aidimoli123的博客
06-20 1734
  基于VLP16 多线激光雷达的cartographer 2d 建图实现
【SLAM基础】地图类型：点云地图、栅格地图
欢迎时常光顾我的干货超市，热衷于AIoT！！
05-15 7944
一、点云地图； 二、栅格地图； 2.1二维占据栅格地图 2.2三维占据栅格地图 2.3 维高程栅格地图
5.如何利用ORBSLAM3生成可用于机器人/无人机导航的二维/三维栅格地图--以octomap为例
qq_41694024的博客
11-29 5738
如何利用ORBSLAM3生成可用于机器人/无人机导航的二维/三维栅格地图--以octomap为例
ORBSLAM2创建栅格地图方法复现
mmeeed的博客
04-19 5803
开源代码地址：https://github.com/abhineet123/ORB_SLAM2 我只复现了使用自己的相机进行orbslam2创建栅格地图并保存，并对复现过程进行记录。如果想深入理解代码，跑数据集或其他的，可以在代码主目录中的step文件中查看了解。 1.下载代码 git clone https://github.com/abhineet123/ORB_SLAM2 2.安装依赖库 和普通orbslam2的库安装过程一样，这里提供一个好用的博客，可以照着里面安装库的方法安好Pangolin O
占据栅格地图构建，原理介绍+源码分享，2D激光雷达
07-08
占据栅格地图构建是移动机器人领域中的关键技术，它在2D激光SLAM（Simultaneous Localization And Mapping，同时定位与建图）中扮演着至关重要的角色。本文将深入讲解占据栅格地图的基本原理，并结合源码分析，帮助...
基于ORB-SLAM2的实时网格地图构建
04-30
针对目前视觉SLAM（同时定位与地图构建）系统只能输出相机的运动轨迹图而不能生成用于路径规划和导航地图的缺点，提出了一种基于ORB-SLAM2的网格地图实时构建算法。首先，建立了一个适用于视觉SLAM的逆传感器模型...
占据栅格地图构建分享.zip
04-12
Gmapping库就是利用这种方法构建室内或室外环境的2D地图。 1. **Gmapping工作流程**： Gmapping主要包含以下步骤： - **激光数据输入**：首先，Gmapping接收来自机器人上的激光雷达（LIDAR）的数据，这些数据通常...
cartographer栅格地图保存
06-20
本篇文章将详细介绍如何像使用gmapping那样保存Cartographer生成的2D栅格地图，并探讨相关的`map_server`工具。 首先，我们需要理解Cartographer的工作原理。Cartographer通过连续扫描数据（如激光雷达或IMU数据）...
SLAM地图构建与定位算法
08-13
- **地图构建**：将环境特征存储为地图，可以是关键点、特征点云或2D/3D栅格地图。 5. **应用场景** SLAM技术广泛应用于自动驾驶汽车、无人机、服务机器人、增强现实等领域，帮助这些智能系统在未知环境中实现...
A-LOAM(后端1)基于栅格点云地图构建-算法流程+代码注释
zysss_的博客
08-13 3108
不同于前端的scan-to-scan的过程，LOAM的后端是scan-to-map的算法，具体来说就是把当前帧和地图进⾏匹配，得到更准的位姿同时也可以构建更好的地图。由于是scan-to-map的算法，因此计算量会明显⾼于scan-to-scan的前端，所以，后端通常处于⼀个低频的运⾏频率，但是由于scan-to-map的精度往往优于scan-to-scan，因此后端也有着⽐起前端来说更⾼的精度。.........
ROS之旅(二) 在激光雷达仿真环境下使用古月居提供的cartographer算法SLAM，最终导出 .pgm 与 .yaml 地图文件全过程
qq_40789719的博客
04-06 2811
仿真环境下使用cartographer算法建图并导出
ORB-SLAM2栅格地图构建
工大机器人工作室
04-25 6083
ORB-SLAM2栅格地图构建
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值