自己动手写全套无人驾驶算法系列（四）机器人2D SLAM

自己动手写全套无人驾驶算法系列（四）机器人2D SLAM

目录:
一、概述
1.1 系列整体概述
二、传感器层
2.1 轮式里程计
2.2 IMU
2.3 激光雷达
2.4 视觉VO
三、建图层
3.1 静态二值贝叶斯滤波
3.2 Ray casting算法
3.3 Beam end model算法
3.4 Likelihood range finder算法
3.5 多分辨率地图及内存管理问题
3.5.1 ROS存储
3.5.2 稀疏存储
3.5.3 块存储
3.5.4 直接点云存储
四、匹配算法
4.1 Brute force（含分数计算的三种方法）
4.2 Compute 2D slice
4.3 Multiple resolution match
4.4 Fast scanmatching
4.5 ICP
4.6 NDT
4.7 NDT+ICP
4.8 插值匹配
4.9 总结
五、前端融合算法
5.1 EKF
5.2 比例融合
六、后端融合算法
6.1 EKF
6.2 UKF
6.3 PF
6.4 Graph Optimizer

一、概述

1.1 系列整体概述：
本文主要介绍无人驾驶SLAM各个环节的各个算法，主要包括4个方面：传感器层，建图层，匹配层和融合定位层。运动模型为差分驱动模型，目前代码已经做完，但因为要对其专利保护，日后再放出，特此先作介绍，并提供交流。

欢迎大家对我的github项目提出BUG，或者需要我更新新的算法,或者需要技术合作，都可以联系我：
163邮箱：wujiazheng2020@163.com
gmail:wujiazheng2020@gmail.com
QQ群：710805413

二、传感器层

2.1 轮式里程计
轮式里程计的来源主要是编码器或者CAN传入PC端的小车的轮子速度和加速度信息，我们通过轮子的速度积分可以得到车辆已经行走的坐标信息，或者编码器也可以直接提供小车已经行走的坐标信息，同时我们也可以得到车辆的速度信息，将其可以转换为twist，即成了估计车辆位姿的重要来源。但是我们要注意，因为时间的延迟，我们必须将时间对齐固定时钟源，比如我的代码里对齐的是激光雷达的时间，如果轮式早到则要按运动学模型推到相应时间，同时按v及w可以得到相应写方差矩阵

2.2 IMU
imu可以提供3个轴的旋转信息和3个方向的加速度信息，加速度积分两次可以得到位移，但一般考虑精度问题不这么做，除非你的IMU是上千上万那种可以考虑这么做，我们主要从IMU得到旋转速度及旋转角度两个数据，同时按IMU误差模型可以得到其协方差矩阵，可见IMU误差模型。同样的，imu也需要时间对齐。

2.3 激光雷达
激光雷达信息往往是一个角度+一个距离，我们通过距离×cos（角度）或sin(角度)既可以把每个激光束所表示的点的坐标算出来，又因为运动的原因，比如激光雷达的时间间隔是0.2s，一共360个点，那么第1个点就需要做0.2/360 s的运动畸变矫正，我们知道车辆的速度和角速度，即可通过运动模型矫正激光点云，其他点也是按同样的方法矫正。如下：

for(U4 i = 0;i<scan_t.size();i++){
    yaw = scan_t[i].th;
    if(yaw < 0){
        yaw += 2*TT;
    }
    yaw_res = end_angle - yaw;
    time_res = lidar_intv*(yaw_res/all_angle);
    wdt = wt*time_res;
    scan_t[i].x -= ( vbw*sin(wheel_th) + vbw*sin(wheel_th+wdt) );
    scan_t[i].y += ( vbw*cos(wheel_th) - vbw*cos(wheel_th+wdt) );
}

而vw的获得主要有两种方法：
1.轮式里程计及IMU辅助
2.雷达两桢速度不变假设。

2.4 视觉VO
本人的代码尚未提及，对于扫地机器人等2d小型机器人，直接激光雷达方案，或者视觉VIO以MSCKF都可以，这属于单独的内容了，故不作提及。

三、建图层
本人的代码主要提供3大见图算法，都用到了bresenhamline，其实就是电脑中画直线的算法，具体可见bresenham算法，其过程一般是先用建图算法建图再用静态二值贝叶斯滤波融合。如下：

3.1 静态二值贝叶斯滤波

ROS中将全局地图与局部地图对齐后如果，全局地图值为-1（未探测），即把局部地图的值直接赋过去即可，但若全局地图有值，那么则要进行静态二值贝叶斯滤波：

P(x)就是原来的全局地图对应栅格概率0-1，P(X|Zt)就是局部地图对应栅格概率，L0就是全局为未探测时候覆盖的局部地图值，之后一步步更新就可了。效果如下图：(让局部地图向着右上不断移动，并扩张和融合到全局地图)。


3.2 Ray casting算法

对每个激光雷达点转换到局部地图坐标系后直接使用bresenham探测，在击中点附近半径小于hit_r的范围概率置为0.95,（ROS值是95,扩大了100倍节省内存）,其他点都置为0.01即可。每次见图完用静态二值贝叶斯滤波融合即可，如下：

3.3 Beam end model算法

波束模型，使用前我们要用EM算法算出激光雷达的Zhit,Zshort,Zmax,Zrand及hit的方差和均值，short分布的lambda：如下：

以如下代码：

void Mapper::Static_EM_Estimation_For_Z(std::vector<R8> z_m,std::vector<R8> z_r,Mapper_Param &param){
        U4 z_size = z_m.size(); // the size of z_m and z_r must be equal
        if(z_r.size() != z_size || z_size == 0){
            return ;
        }
        R8 z_hit   = param.z_hit;
        R8 z_short = param.z_short;
        R8 z_max   = param.z_max;
        R8 z_rand  = param.z_rand;
        R8 z_hit_abs = z_hit*z_size;
        R8 z_short_abs = z_short*z_size;
        R8 sigma2 = 0,lamda = 0;
        for(U4 i = 0; i< z_size; i++){
            sigma2 += (z_m[i] - z_r[i])*(z_m[i] - z_r[i]);
            lamda  += z_m[i];
        }
        sigma2 = sigma2/z_hit_abs;
        lamda = z_short_abs/lamda;
        R8 Eta = 0;
        R8 pre_sigma2 = 0,pre_lamda = 0;
        R8 p_hit_i,p_short_i,p_max_i,p_rand_i,b2;
        R8 e_hit_i,e_short_i,e_max_i,e_rand_i;
        R8 abs_z,e_hit_all,e_short_all,e_max_all,e_rand_all;
        R8 e_hit_sqr_all,e_short_z_all;
        p_max_i = 0;
        p_rand_i = param.min_rslo/param.range_max;

        U4 times = 0;
        while(fabs(pre_sigma2 - sigma2) >= 1e-3 || fabs(pre_lamda - lamda) >= 1e-3){
            abs_z = 0,e_hit_all = 0,e_short_all = 0,e_max_all = 0,e_rand_all = 0;
            e_hit_sqr_all = 0,e_short_z_all = 0;
            pre_sigma2 = sigma2;
            pre_lamda  = lamda;
            for(U4 i = 0; i<z_size;i++){
                b2 = (z_m[i] - z_r[i])*(z_m[i] - z_r[i])/(2*sigma2);
                p_hit_i = exp(-b2)/(2*TT*sigma2);
                p_short_i = lamda*exp(-lamda*z_m[i]);
                Eta = 1/(p_hit_i + p_short_i + p_max_i + p_rand_i);
                e_hit_i = Eta*p_hit_i;
                e_short_i = Eta*p_short_i;
                e_max_i = Eta*p_max_i;
                e_rand_i = Eta*p_rand_i;

                abs_z += (e_hit_i + e_short_i + e_max_i + e_rand_i);
                e_hit_all += e_hit_i;
                e_short_all += e_short_i;
                e_rand_all += e_rand_i;
                e_hit_sqr_all += (e_hit_i*(z_m[i]-z_r[i])*(z_m[i]-z_r[i]));
                e_short_z_all += (e_short_i*z_m[i]);
            }
            z_hit   = e_hit_all/(abs_z);
            z_short = e_short_all/(abs_z);
            z_max   = 0;
            z_rand  = e_rand_all/(abs_z);
            sigma2  = e_hit_sqr_all/e_hit_all;
            lamda   = e_short_all/e_short_z_all;
            times ++;
            if(times >= 100){
                break;
            }
        }

        param.z_hit    = z_hit;
        param.z_short  = z_short;
        param.z_max    = z_max;
        param.z_rand   = z_rand;
        param.sigma2   = sigma2;
        param.lambda   = lamda;
    }

最后需要融合各个分布到如下分布：

然后在bresenham line 上根据离终点和起点的距离算出其概率填入局部地图即可。如下：

其中数值积分按如下代码：

    R8 Mapper::Integral_From_Function(X1 mode,R8 start,R8 end,Mapper_Param &param){
        R8 result=0,u,sigma2,lambda;
        R8 interval = param.intg_intv;
        if(mode != Gaussian && mode != Exp){
            return 0;
        }
        if(mode == Gaussian){
            u = param.u;
            sigma2 = param.sigma2;
            for(R8 i = start;i<end;){
                result += 1/sqrt(2*TT*sigma2)*exp(-(i-u)*(i-u)/(2*sigma2))*interval;
                i += interval;
            }
        }
        if(mode == Exp){
            lambda = param.lambda;
            start = start<0? 0:start;
            result = exp(-lambda*start)-exp(-lambda*end);
        }
        return result;
    }

这里有个小技巧，当激光雷达不是很准的时候，主要依赖图优化去优化，这时就可以把最高概率的点设置为Pmax比如95,其他概率按最高概率按比例放缩，这样的图更具有信息，不容易被静态二值滤掉。

3.4 Likelihood range finder算法

说白了在bresenham line上每个点，以波束模型算出的那个分布取Phit那个分布，用kdtree搜索其离最近点的距离，代入分布中即得到概率，得到局部地图同样用SBF融合即可，容易出现穿墙问题，是gmapping的建图算法,如下：

核心是使用如下pcl结构：

 pcl::PointCloud<pcl::PointXY>::Ptr cloud (new pcl::PointCloud<pcl::PointXY>);
 pcl::KdTreeFLANN<pcl::PointXY> kdtree;

效果如下：

3.5 多分辨率地图及内存管理问题

建图的时候需要在多分辨率建图，这时就需要考虑内存问题，少内存必然带来的是低速度，所以必须就内存和速度进行权衡，权衡是按如下公式计算，
假设电脑内存为M,而一个地图点是1字节，故地图边长最大栅格数G_length = sqrt(M)，设地图最小分辨率为rslo，那么地图最大边长L为G_length*rslo, 多分辨率下，其等比数列收敛于1/4，即最大边长为L/(5/4),如果你内存不够就要用磁盘，现在固态的速度300MB-2000MB/s，速度其实是够的，如果是机械就比较麻烦。

3.5.1 ROS存储
存储方式可以用ROS这种，就是一维数组，x = i%width y = i/width;

3.5.2 稀疏存储
也可以是稀疏存储，当地图填充率小于1/9的时候即使用，结构体如下：

class Map_Data{
	I4 x;
	I4 y;
	X1 val;
}

3.5.3 块存储
比较常用，就是分割地图为一个个正方形块，可以存储到磁盘中，使用的时候按当前归化坐标索引即可，花费只是索引时间，存储也是一样，最多的时候只需取4块即可，保存的时候文件的名字可以帮助你快速索引，并且排序由系统命令完成即可。
文件名如下: 5_10.ff 我用了不同文件夹放不同分辨率的地图，所以每个文件直接以栅格坐标xy命名即可。而地图边长这些都是在程序定义好的，不需要再读取。

3.5.4 直接点云存储
把点云经过VOXEL filter后按时间顺序直接存下来，对于图优化的算法，这种可以在图优化回环结束后再快速建图，故如果你用图优化算法，就用这种存储是可以的，当然也可以加块算法，结构如下：

std::vector<std::vector<Pose_2d>> scan_set;

四、匹配算法
主要有8大算法，其他衍生算法我也会提到。

4.1、Brute force

说白就是暴力匹配，就是在当前位置附近取abc个位置探索分数最高的一个，然后把位置更新过去即可，a是x窗口大小，b是y窗口大小，c是yaw的窗口大小，很耗内存，其中分数的计算方法有两种：1.相关性测量模型，2.插值模型：
相关性测量模型如下：

即协方差的算法，很简单。
插值模型分为双线性插值和双三次插值：
双线性插值如下：双线性插值，这是hector slam用的插值匹配方法。本用于图像缩放。
双三次插值如下：双三次插值，这是cartographer的插值匹配方法。本用于图像缩放。
然后我们就获得了那个分数。相关性和插值的函数实现如下：

 R8 Get_Score(nav_msgs::OccupancyGrid &local_map,nav_msgs::OccupancyGrid &global_map,X1 mode);
 R8 Get_Score(Pose_2d &now_pose,std::vector<Pose_2d> &scan,nav_msgs::OccupancyGrid &global_map,X1 mode);

注：gmapping和cartographer在分数上还加入了离初始点的距离作为分数项，越近分数越高。我的代码沿用这种做法

4.2、Compute 2D slice

说白就是相关性模型你算分数要先算出局部地图，xy的变化可以通过变换地图的起点坐标即可，而angle那个要重新建图，那我直接提前算好不同角度的地图不就可以了。如下：

 for(R8 a = -angle_s;a<=angle_s;a+=angle_inc){
      tmp_pose.th = pred_pose.th + a;
      map_maker.Get_Local_Map(tmp_pose,scan,param.map_param);
      //3.move map and search
      for(R8 y = -length_y/2;y <= length_y/2;y+=rslo){
          for(R8 x = -length_x/2;x <= length_x/2;x+=rslo){
          //待上传后
          }
      }
}

4.3、Multiple resolution match

可见hector slam，这本是hector slam的算法，先在低分辨率匹配得到粗略的结果再在高分辨率匹配，结果是次优而非最优。其匹配用的是我后面要说的插值匹配。

4.4、Fast scanmatching

这是cartographer的匹配算法，cartographer主要有4种匹配算法，CSM，real time CSM，ceres scanmatching（用ceres做的插值匹配），Fast match（Branch and bound）。real time CSM那个类似compute 2D slice，主要就是branch and bound，说白了粗分辨率的分数要高于细分辨率的分数，因为粗分辨率类似最大采样，细分辨率上打在概率为0上的点，粗分率上可能就是打在障碍物的栅格上，即使是在栅格边缘。有了这个理解，我们先对当前层结点分数算出来排序，取最大的按DFS去搜索，对于叶子节点取最大的一个分数，与我们之前存储的best score比较（初始化为-1），如果大于他就把best node更新为这个叶子节点，回到上一层，如果上一层的老二比这个best score还低，就不进入DFS了，直接再退一层，就按这种递归方式就加速了暴力匹配的速度。参考函数如下：

//2.4 branch and bound like cartographer
Tree_node best_node;
void Branch_And_Brand_DFS(Pose_2d &pred_pose, std::vector<Pose_2d> &scan,Matcher_Param &param,
                          std::vector<nav_msgs::OccupancyGrid> &multi_map);
Pose_2d Fast_CSM_Matcher(Pose_2d &pred_pose, std::vector<Pose_2d> &scan,Matcher_Param &param,
                         std::vector<nav_msgs::OccupancyGrid> &multi_map);

4.5、ICP

就是迭代最近点，这个经典算法听的太多了，具体见SLAM十四讲，我是用PCL实现，唯一要注意的就是存储上个点的分配方式：

std::vector<pcl::PointXYZ,Eigen::aligned_allocator<pcl::PointXYZ>> last_scan;

注：实际用的时候必须加入RANSAC，不然难以得到很好的效果。

4.6、NDT

正态分布变换，可见正态分布变换，类似插值匹配，不过是用高斯分布模型。常用于粗匹配与ICP结合使用。

4.7、NDT+ICP

先NDT得到一个粗略的值，再ICP，参考如下：

Pose_2d Matcher::NDT_MIX_ICP_Matcher(Pose_2d &now_pose,Pose_2d &pred_pose, std::vector<Pose_2d> &scan,Matcher_Param &param){
   if(first_match){
       last_scan = Scan_To_Point(scan);
       return pred_pose;
   } else {
       Pose_2d tmp_pose = NDT_Matcher(now_pose,pred_pose,scan,param);
       return ICP_Matcher(now_pose,tmp_pose,scan,param);
   }
}

4.8、插值匹配

主要有两种算法：1.线性插值匹配（hector slam） 2.双三次插值匹配（cartographer）。
其形式都类似hectorslam。
其中双三次插值的插值函数和微分函数如下：

R8 Matcher::BiCubic(R8 x,R8 a){
    R8 abs_x = fabs(x);
    R8 x2 = abs_x*abs_x;
    R8 x3 = x2*abs_x;
    if(abs_x <= 1){
        return (a+2)*x3 - (a+3)*x2 + 1;
    }else if(abs_x < 2 && abs_x >1){
        return a*x3 - 5*a*x2 + 8*a*abs_x - 4*a;
    }else{
        return 0;
    }
}

R8 Matcher::D_BiCubic(R8 x,R8 a){
    R8 x2 = x*x;
    if(x > -2 && x < -1){
        return -3*a*x2 - 10*a*x - 8*a;
    } else if(x >= -1 && x < 0){
        return -3*(a+2)*x2 - 2*(a+3)*x;
    } else if(x >= 0 && x < 1){
        return 3*(a+2)*x2 - 2*(a+3)*x;
    } else if(x > 1 && x < 2){
        return 3*a*x2 - 10*a*x + 8*a;
    }
    return 0;
}

带入其公式eigen即可，而cartographer用ceres完成了这个步骤。损失函数即使插值之平方和，自动求导，具体见cartographer源码。

4.9、总结

这就是匹配的算法，匹配算法至关重要，图优化中匹配如果太差也做不起来，故值得斟酌。完整的匹配过程如下：



即重合了。

五、前端融合算法

主要用于IMU和轮子前端融合，也可以不在前端，直接紧耦合或者在后端融合

5.1、EKF

属于高斯滤波。
之前写过不作过多论述，主要代码实现就是J的写法：

    JF << 1.0,1.0,-dt*v*sin(yaw),dt*cos(yaw),
          0.0,1.0,dt*v*cos(yaw) ,dt*sin(yaw),
          0.0,0.0,1.0           ,0.0        ,
          0.0,0.0,0.0           ,1.0        ;

5.2、比例融合

这个没啥难的，就是yaw按轮子和imu给的那个按比列分配即可。

六、后端融合算法

主要有4种，本还想加入SEIF，IF主要的优势是不知道权重Omega给0即可，而KF协方差要给无限大，不过现在主要图优化时代，SEIF实际用的少。其实这个我之前就写过了，见我之前写的，写过我就不说了。

6.1、EKF

就上面讲过，关键是imu，轮子，匹配的误差模型，imu误差模型 ，轮子误差模型就是av×v + b×W×W，这个在概率机器人中说的很明白了。

激光雷达误差模型一是概率机器人波束模型那样算，二是直接按分数，归一化后×length即可。

6.2、UKF

这个我之前也写了，就取2n+1个点变换过去，再重新计算均值和方差即可，可见我之前写的UKF。现实情况如果误差模型建的好， 其误差残项十分符合高斯分布，那么UKF好于EKF，否则则不一定

6.3、PF

这个我也写过，particle filter，粒子滤波，gmapping用的是改进版粒子滤波，其在预测粒子生成后还进行了前后左右，左转右转6次的CSM分数探测，取了分数最大的那个，这样预测粒子分布就好了很多，不过严重依赖里程计，说白了就是6次探测跟不不够，有时候误差高达10个栅格长度，那就没戏了要么加大粒子数，不过因为gmapping一个粒子维持一个地图，其内存和计算量都会变得十分大，这也导致了gmapping在数据集处理旋转很好，但是实际运用过程中还是受不了不停旋转，比如intel数据集下表现很好，但是你拿到实际场景就不行，效果甚至不如ICP，（说ICP过时了的可以看ICP+RANSAC+图优化论文，scan to map是担心scan to scan的累计误差，但加入图优化就得到很好的解决）最后gmapping 用Neff统计粒子分散度，如果过于分散就重采样，但是重采样是个败笔，数据集中可以，比如MIT那个数据集，但是实际过程中，重采样往往整个位姿就坏了。
然后在实现过程中，按权重采样可以按如下实现,使用 random库：

//之前是 W += count W，所以每个粒子占有一个区间，随机出来粒子看在哪个区间就取哪个粒子

std::default_random_engine e;
e.seed(time(NULL));
std::uniform_int_distribution<unsigned> nw(0, W);
for(U4 i = 0;i<param.x_set_size;i++){
    I4 wp = nw(e);
    R8 low_b = 0;
    for(U4 j = 0;j<x_pred_set.size();j++){
        if(wp<x_pred_set[j].Wc && wp >= low_b){
            x_set.push_back(x_pred_set[j]);
            break;
        }
        low_b += x_pred_set[j].Wc;
    }
}

6.4、Graph Optimizer
图优化算法是cartographer的主要核心，见图优化，主要就是把误差项填入边，然后按SPA求解稀疏矩阵即可，求解的时候按如下：

Xx = Hx.colPivHouseholderQr().solve(bx);

也可用LU分解，或者Cholesky，LLT都可以，普通的边就是当前位姿与最近距离小于dis且索引小于n的点，而回环边就是那些不是最近索引点的距离与当前匹配位姿小于dis的点，回环优化后地图要重新生成，并把回环边去掉，如下：

void Fusion::Reconstruct_Map(Mapper &mapper,Mapper_Param &mapper_param,Fusion_Param & fusion_param){
   mapper.multi_map.clear();
   mapper.Init_Map(node_set[0],scan_set[0],mapper_param);
   for(I4 i = 1;i<node_set.size();i++){
       mapper.Get_Local_Map(node_set[i],scan_set[i],mapper_param);
       mapper.Mix_Local_Map_With_SBF(mapper_param);
   }
   for(I4 i = 0;i<extra_edge;i++){
       edge_set.pop_back();
   }
}

而优化可见lsslamPDF，激光slam和视觉不同的在于，激光slam不需要用DBow3或者深度学习回环，但不代表不可以，你用这个添加环也是可以的，cartographer中用ceres优化自动求导，当然如果你懂雅可比矩阵求导的话，为了效率建议用Eigen。有些人觉得图优化要优化那么多速度肯定很慢，其实不然，如果是加入landmark的graph确实慢，但现在是Pose graph只有node，所以速度不比gmapping慢，内存也没gmapping消耗高。其保存形式如下：

class Pose_Edge{
public:
    Pose_Edge();
    ~Pose_Edge();

    U4 x_i;
    U4 x_j;
    Pose_2d edge;
    Pose_2d omega;
};
std::vector<Pose_Edge> edge_set;
std::vector<Pose_2d> node_set;
std::vector<std::vector<Pose_2d>> scan_set;

故其实消耗内存也很少,最终效果如下：