论文笔记——基于多传感器融合的室内自主导航系统的设计与实现

创新点：

1、针对于全局定位需要人为干预的不足，提出一种利用UWB模块提出初始位置信息以及创建文档记录实时位姿和粒子自动更新的方式提供姿态信息的改进AMCL算法，实现不需要认为干预的自主全局定位。

2、融合建图方案：主要是利用贝叶斯推理公式融合深度相机滤波处理的3D点云与激光雷达转化格式的2D点云，最后将融合后的点云信息转换成极坐标形式输出给Cartographer算法建图。

知识点：

（1）周军等人提出一种UWB融合IMU的信息来修正非视距误差，首先利用IMU的预测位置与以及非视距误差概率曲线（由于在室内环境下会有较多障碍物，会使UWB模块造成非视距误差）修正测量距离，然后利用KF算法融合IMU预测的位置和经过最小二乘法处理的UWB的位置，最后通过实验仿真验证该定位算法可以有效缩小非视距误差。
（2）张毅等人利用贝叶斯递推公式融合了激光SLAM和VSLAM所创建的地图。 （3）建图时期进行多传感器融合：贝叶斯推理公式融合
（4）为了提高回环检测的效率，Cartographer算法中利用分支定界算法进行回环的搜索。该算法是一种比较常见的树形搜索剪枝算法，核心思想就是分支和定界，其中分支就是把所有的可行解分成各个子集，一直分解到最小的子集，称为叶子节点，叶子节点是最低层的值，只有叶子节点的值代表真实值，别的分支节点值都是上下界，在SLAM问题中，就是求上界，然后再从各个子集中去寻找最优解，定界就是对各个子集最高层的节点设置一个上界，即该节点的上界的值都要大于该节点下的子集的所有的值的大小。
（5）激光雷达与深度相机融合步骤：

（1）由于激光雷达的信息是极坐标格式，所以首先将激光雷达极坐标格式的数据转换成点云数据格式，相机产生的点云数据格式为pointcloud2，故就先转化成pointcloud2格式。

（2）将深度相机生成的三维点云进行滤波处理。

（3）通过联合标定获得雷达以及深度相机之间的坐标系转换关系，把他们统一到同一个坐标系下，在这是将雷达点云转换到深度相机的坐标系下。

（4）利用贝叶斯推理公式融合激光雷达点云和深度相机点云，从而过滤掉深度相机产生的误差点和激光雷达点云的重合点。

（5）在将点云数据的pointcloud2数据个数转成雷达极坐标数据格式，再将该数据传入建图算法进行建图。

（6）深度相机数据处理：

滤波处理的几种方式：

    直通滤波器：把一定高度以上的点云直接过滤了。

    体素滤波器：对点云进行降采样。

    统计滤波器：对孤立的点进行过滤，保留成团的点。

（7）Monte Carlo Localization，MCL算法的步骤：

（1）粒子群初始化，随机生成M个粒子，赋予每个粒子的相同的权重。

（2）粒子群状态转移，利用运动模型来模拟粒子群的状态转移。

（3）计算粒子权重，所有粒子都进行相同的评估，根据测量值给可能性大分配给较大的权重，可能性小的分配给较小的权重。

（4）粒子群重采样，抛弃旧粒子，重新生成的新粒子出现在权重大的旧粒子的位置上，产生的新粒子权重相同。之后返回第二步，循环往复。

（8）Adaptive Monte Carlo
Localization，AMCL：利用KLD采样粒子的粒子滤波定位算法。由于粒子数量M一般情况下与非空位的栅格数量成正比。再初始阶段，所有栅格为空，再开启AMCL算法后，粒子便占据了栅格，导致k值增大，M也就越大。k值越大说明粒子比较分散，则需要注入更多粒子，因为此时并没有定位到最佳的位置上。相反的，k值越小就越说明粒子此时已经收敛，定位基本完成，不需要过多的粒子数用来定位。