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

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

0、说明

 世界坐标系：

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

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

1.  
   std_msgs/Header header # 数据的消息头
2.  
   uint32 seq # 数据的序号
3.  
   time stamp # 数据的时间戳
4.  
   string frame_id # 地图的坐标系
5.  
   nav_msgs/MapMetaData info # 地图的一些信息
6.  
   time map_load_time # 加载地图的时间
7.  
   float32 resolution # 地图的分辨率，一个格子代表着多少米，[m/cell]
8.  
   uint32 width # 地图的宽度，像素的个数, [cells]
9.  
   uint32 height # 地图的高度，像素的个数, [cells]
10.  
    geometry_msgs/Pose origin # 地图左下角的格子对应的物理世界的坐标，[m, m, rad]
11.  
     
12.  
    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[]：按照那张地图图片的自底向上，自左至右逐个像素点存储的（行逆序、列顺序）

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

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

1.  
   #include <ros/ros.h>
2.  
   #include <nav_msgs/OccupancyGrid.h>
3.  
    
4.  
   int main(int argc, char **argv)
5.  
   {
6.  
   ros::init(argc, argv, "GridMap_test");
7.  
   ros::NodeHandle n;
8.  
   ros::Publisher map_pub = n.advertise<nav_msgs::OccupancyGrid>("/gridMap", 1);
9.  
    
10.  
    nav_msgs::OccupancyGrid map;
11.  
    map.header.frame_id = "map";
12.  
    map.header.stamp = ros::Time::now();
13.  
    map.info.resolution = 0.05; // m/ceil
14.  
    map.info.width = 100; // ceil
15.  
    map.info.height = 100;
16.  
    map.info.origin.position.x = 2.0;
17.  
    map.info.origin.position.y = 2.0;
18.  
    map.info.origin.position.z = 0.0;
19.  
    map.info.origin.orientation.x = 0.0;
20.  
    map.info.origin.orientation.y = 0.0;
21.  
    map.info.origin.orientation.z = 0.0;
22.  
    map.info.origin.orientation.w = 0.0;
23.  
    map.data.resize(map.info.width * map.info.height);
24.  
     
25.  
    for (int i=0; i<map.info.width*map.info.height; i) {
26.  
    if (i<70)
27.  
    map.data[i] = 0;
28.  
    else {
29.  
    map.data[i] = -1;
30.  
    }
31.  
    }
32.  
    ros::Rate loop_rate(1);
33.  
    while (ros::ok())
34.  
    {
35.  
    map_pub.publish(map);
36.  
    loop_rate.sleep();
37.  
    }
38.  
    return 0;
39.  
    }

一、覆盖栅格法

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

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

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

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

1.  
   // 机器人坐标 对应的栅格坐标
2.  
   GridIndex robotIndex = ConvertWorld2GridIndex(robotPose(0),robotPose(1));
3.  
   // 一帧
4.  
   for(int id = 0; id < scan.range_readings.size();id ) {
5.  
   double dist = scan.range_readings[id]; // 激光深度
6.  
   double angle = scan.angle_readings[id]; // 在激光系下的偏角
7.  
   // 去除异常数据
8.  
   if(std::isinf(dist) || std::isnan(dist)) continue;
9.  
    
10.  
    // 2d平面的激光点， 激光点在激光系下的坐标
11.  
    double theta = robotPose(2); // 机器人在地图下的偏角
12.  
    double laser_x = dist * cos(angle);
13.  
    double laser_y = dist * sin(angle);
14.  
    // 激光点在地图下的坐标 2d空间的旋转矩阵
15.  
    double world_x = cos(theta) * laser_x - sin(theta) * laser_y robotPose(0);
16.  
    double world_y = sin(theta) * laser_x cos(theta) * laser_y robotPose(1);
17.  
    // 激光点的栅格坐标
18.  
    GridIndex pointGridIndex = ConvertWorld2GridIndex(world_x, world_y);
19.  
     
20.  
    //
21.  
    //算法调用。。。直接调用后面三个方法各自封装的函数即可。。
22.  
    //
23.  
     
24.  
    }

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

1.  
   // 激光点对应的栅格坐标
2.  
   // 机器人对应的栅格坐标
3.  
   // 占据栅格法
4.  
   void OccGridMethod(GridIndex& pointGridIndex, GridIndex& robotIndex) {
5.  
    
6.  
   //如果二维平面，该机器人可以获得该激光点，那么机器人与激光点连线上肯定是空闲的
7.  
   std::vector<GridIndex> freeTraceindexes = TraceLine(robotIndex.x, robotIndex.y, pointGridIndex.x, pointGridIndex.y); //Bresenham算法
8.  
   for(auto& index : freeTraceindexes) // 遍历机器人位置到激光点之间的栅格(不包含激光点所在栅格)
9.  
   {
10.  
    if(!isValidGridIndex(index))
11.  
    continue;
12.  
     
13.  
    int linearIndex = GridIndexToLinearIndex(index); // 取出该栅格对应的值
14.  
    // 1-根据空闲规则 初始值默认50
15.  
    if(pMap[linearIndex] == 0)
16.  
    continue;
17.  
    pMap[linearIndex] = mapParams.log_free; // mapParams.log_free = -1
18.  
     
19.  
    }
20.  
    // 激光点所在栅格肯定是障碍物，机器人所走过的路径都是空闲的
21.  
    if(isValidGridIndex(pointGridIndex)) // 激光点对应的栅格
22.  
    {
23.  
    int linearIndex = GridIndexToLinearIndex(pointGridIndex);
24.  
    // 2-根据占据规则
25.  
    pMap[linearIndex] = mapParams.log_occ; // mapParams.log_occ = 2
26.  
    if(pMap[linearIndex] > 100)
27.  
    pMap[linearIndex] = 100;
28.  
    }
29.  
    }

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

二、计数建图法

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

1、计数法更新策略：

1.  
   // 计数栅格法
2.  
   // input: 激光点的像素坐标、机器人的像素坐标、占据率
3.  
   void CntGridMethod(GridIndex& pointGridIndex, GridIndex& robotIndex, double occRate) {
4.  
    
5.  
   std::vector<GridIndex> freeTraceindexes = TraceLine(robotIndex.x, robotIndex.y, pointGridIndex.x, pointGridIndex.y);
6.  
   for(auto& index : freeTraceindexes) // 遍历机器人位置到激光点之间的栅格(不包含激光点所在栅格),机器人所走过的路径都是空闲的
7.  
   {
8.  
   if(!isValidGridIndex(index))
9.  
   continue;
10.  
     
11.  
    int linearIndex = GridIndexToLinearIndex(index); // 取出该栅格对应的值
12.  
    // 根据空闲规则
13.  
    Misses_cnt[linearIndex] ;
14.  
     
15.  
    }
16.  
    // 激光点所在栅格肯定是障碍物
17.  
    if(isValidGridIndex(pointGridIndex)) // 激光点对应的栅格
18.  
    {
19.  
    int linearIndex = GridIndexToLinearIndex(pointGridIndex);
20.  
    // 根据占据规则
21.  
    Hits_cnt[linearIndex] ;
22.  
    }
23.  
    // // 统计每个栅格的占有率 最后计算完，再统一算占有率
24.  
    // for (int i=0; i< mapParams.height*mapParams.width; i) {
25.  
     
26.  
    // if ((Misses_cnt[i] Hits_cnt[i])!=0) { // 未扫到栅格=50 未知
27.  
    // double r = double(Hits_cnt[i] / (Misses_cnt[i] Hits_cnt[i]));
28.  
    // if (r >= occRate) // 占有
29.  
    // pMap[i] = 100;
30.  
    // else
31.  
    // pMap[i] = r*100; // 空闲
32.  
    // }
33.  
    // }
34.  
    }

2、建图效果如下：

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

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

更新公式：

，laser传感器测量的距离，dist栅格离传感器原点的距离；

，t截止距离(大于截止距离的点不会用于曲面重建)；

，多次观测的融合更新方程；

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

code：

1、tsdf法则进行更新：

1.  
   // tsdf栅格法：阶段式带符号距离函数（加权最小线性二乘）
2.  
   // 激光点栅格坐标、机器人地图坐标、当前帧激光的深度
3.  
   void TSDFGridMethod(GridIndex& pointGridIndex, GridIndex& robotIndex, Eigen::Vector3d& robotPose, double& dist) {
4.  
    
5.  
   //111
6.  
   std::vector<GridIndex> freeTraceindexes = TraceLine(robotIndex.x, robotIndex.y, pointGridIndex.x, pointGridIndex.y);
7.  
   for(auto& index : freeTraceindexes) // 遍历机器人位置到激光点之间的栅格(不包含激光点所在栅格),机器人所走过的路径都是空闲的
8.  
   {
9.  
   if(!isValidGridIndex(index))
10.  
    continue;
11.  
    // 栅格坐标系转换到map下
12.  
    double x = (index.x - mapParams.offset_x) * mapParams.resolution mapParams.origin_x;
13.  
    double y = (index.y - mapParams.offset_y) * mapParams.resolution mapParams.origin_y;
14.  
    // 计算map下，激光点所在栅格与机器人位置的距离
15.  
    // 激光距离-计算的距离
16.  
    double d = std::sqrt( pow((x-robotPose(0)), 2) pow((y-robotPose(1)), 2) ); // 栅格里传感器原点的距离
17.  
    double sdf = dist-d;
18.  
    double t = 0.1; // 截止距离
19.  
    double tsdf = std::max(-1.0, std::min(1.0, sdf/t)); // tsdfi(x)，范围-1 to 1
20.  
     
21.  
    int linearIndex = GridIndexToLinearIndex(index);
22.  
    // 测量更新
23.  
    // 第一次 TSDFi = tsdfi，Wi = 1
24.  
    // 小w=1
25.  
    pMapTSDF[linearIndex] = ( pMapW[linearIndex]*pMapTSDF[linearIndex] 1.0*tsdf ) / (pMapW[linearIndex] 1); // 更新TSDFi(x)
26.  
    pMapW[linearIndex] = 1; // 更新Wi(x)
27.  
     
28.  
    }
29.  
    }

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

1.  
   for(int i= 0; i<mapParams.height-2; i ) //假设i为y轴
2.  
   {
3.  
   for(int j = 0; j<mapParams.width-2; j ) //假设j为x轴
4.  
   {
5.  
   int line_value = j i*mapParams.height;
6.  
   int line_x = line_value 1; //往x轴移动一格 右侧
7.  
   int line_y = line_value mapParams.height;//往y轴方向移动一格 下侧
8.  
   //转map坐标
9.  
   double A_x = GridIndex2ConvertWorld(j);
10.  
    double A_y = GridIndex2ConvertWorld(i);
11.  
    double B_x = GridIndex2ConvertWorld(j 1);
12.  
    double B_y = GridIndex2ConvertWorld(i 1);
13.  
    double a,b,b1,x,y;
14.  
    a = pMapTSDF[line_value]; b= pMapTSDF[line_x]; b1 = pMapTSDF[line_y]; // 判断该点的下边、右边点是否为交界处
15.  
     
16.  
    if( a*b1 < 0){ //x方向 乘积为负数，交界处
17.  
    x = A_x;
18.  
    y = interpolation(A_y,B_y,a,b1); //插值
19.  
    pMap[GridIndexToLinearIndex(ConvertWorld2GridIndex(x,y))] = 100;
20.  
    }
21.  
    else if( a*b < 0){ //y方向
22.  
    x = interpolation(A_x,B_x,a,b);
23.  
    y = A_y;
24.  
    pMap[GridIndexToLinearIndex(ConvertWorld2GridIndex(x,y))] = 100;
25.  
    }
26.  
     
27.  
    }
28.  
    }

3、建图效果如下：

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

这篇好文章是转载于：学新通技术网