SLAM2d栅格地图构建的常用方法
最近在总结一些平时做过的东西，记录一下，哈哈哈。。。相信很多做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还有疑问的话可以自己去测试一下：
#include
#include
int main(int argc, char **argv)
{
ros::init(argc, argv, "GridMap_test");
ros::NodeHandle n;
ros::Publisher map_pub = n.advertise("/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 一、覆盖栅格法
- 每个栅格有两种状态：占用(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 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 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的零点位置来获取环境曲面的精确位置，最终得到障碍物仅会占用一个栅格。算力要求较以上两种高。
更新公式：
，laser传感器测量的距离，dist栅格离传感器原点的距离；
，t截止距离(大于截止距离的点不会用于曲面重建)；
，多次观测的融合更新方程；
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 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; i3、建图效果如下：
由于电脑比较拉跨，读入的数据点有限(只能读2000个点)，建出来的图有点尴尬。。。