【《基于滤波的激光SLAM建图方法分析案例》4900字】

[28]。（2-SEQ2-\*ARABIC6）上文求出的Neff是一个可以很好估计重要性权重分散的度量，当Neff小于Cartographer算法原理Cartographer算法的核心部分包括传感器数据输入（InputSensorData）、局部建图（LocalSLAM）和全局建图（GlobalSLAM）。首先Cartographer结合了许多不同的传感器数据（惯性测量单元数据、雷达数据、里程计数据等）创建Submap子图，具体过程是将一个激光扫描帧（Laserscan）插入相应的Submap，利用Submap子图已有的Laserscan以及其他的传感器估计其在Submap中最佳的位置和姿态，这其实是一种局部的位姿优化。随着Submap数量的不断增多，累计误差不断的增大，这时就需要对位姿进行进一步的优化。Cartographer算法会隔一段时间，进行全局优化，也就是当新的激光扫描帧插入时，利用Scanmatch策略寻找匹配的闭环（闭环会考虑之前所有创建的Submap），并将匹配的闭环加入位姿优化图2-SEQ图2-\*ARABIC3Cartographer算法原理流程传感器数据首先经过体素滤波器（VoxelFilter）和自适应体素滤波器（AdaptiveVoxelFilter）过滤的距离信息（RangeData），其中体素滤波器的作用：激光雷达扫描点云数目大，重叠相近的点会占用很多内存空间，体素滤波器在保证点云的形状特征的前提下对点云采取降采样操作,极大缩减了点云的数量量级。里程计数据为轮式移动机器人左右轮的行驶距离，通过解算可提供机器人位置信息和转角信息，可用于估计位姿。IMU数据可提供线加速度和角速度信息，通过IMUTracker进行数据解算，也可得到机器人的位姿PoseExtrapolator结合里程计和IMU的位姿数据，得到更准确的位姿数据。局部SLAM局部SLAM的作用是优化扫描帧位姿建立每一个良好的子图(Submap)。Submap构造是将激光雷达数据scans坐标帧与Submap坐标帧(简称帧)反复对齐的迭代过程。扫描坐标系下的位姿ξ在子图坐标系要经过转换𝑻,它将扫描点从扫描帧严格变换为子图帧，定义为公式REF_Ref71920827\h（2-7）所示，左边矩阵为旋转矩阵，右边矩阵代表平移矩阵。（2-SEQ2-\*ARABIC7）每一个扫描帧在形成子图前都要经过静止滤波器的过滤，静止滤波器的作用是：为了避免在每个子图像中插入过多的激光扫描数据，一旦扫描匹配器生成一个新的姿态，它计算姿态和姿态变化，并调用静态过滤器。如果更改不够重要（或更改非常小），这一帧扫描数据将被删除。减少了数据处理量，降低了内存消耗。局部地图是以分辨率为边长的概率栅格，由连续的激光扫描帧匹配组合而成，当创建完子图时，遍历所有栅格的概率值，当栅格概率＜pmin则该栅格处于自由空间，当概率在pmin,图2-SEQ图2-\*ARABIC4概率栅格激光扫描的每一帧数据都会两组数据：“击中栅格点”与“未击中栅格点”,如REF_Ref71921122\hREF_Ref72154362\h图2-5概率栅格地图所示。带叉的是激光扫中的栅格，阴影的是原点到激光扫中点的线段掠过的所有栅格。图中既在阴影范围内有有×符号的栅格点为“击中栅格点”,只带阴影不带×符号的栅格点为“未击中栅格点”,每个“击中栅格点”都初始化概率值,每个“未击中栅格点”被初始化概率值为。如果该栅格点赋值之前已经有了相应的概率值，那么将此栅格点的概率值进行如公式REF_Ref71921212\h（2-8）的更新，其中clamp限定了函数的输出数值在一定的区域范围内。（2-SEQ2-\*ARABIC8）图2-SEQ图2-\*ARABIC5概率栅格概率更新方法Scanmatching指将不同时刻的传感器数据关联起来，寻找其对应关系，获得被测物体的完整几何信息。每一个扫描帧在插入到子图中都要经过一个扫描匹配优化器，通过这个优化器来获取扫描帧相对于当前子图位姿的最佳估计值ξ。扫描匹配器寻找最佳位姿的估计的本质也是非线性最小二乘优化为题，当扫描真以这个估计姿态ξ插入到子图中时，可以最大化扫描点在子图中的概率值。最小二乘问题由谷歌自家的ceres库解决。其问题可以描述为：（2-SEQ2-\*ARABIC9）Tξ将点云转换到概率栅格坐标下，M全局SLAM全局SLAM的目的是将所有已经创建好的子图的位姿进行全局优化，将子图正确拼接成环境地图。Cartographer在构建巨大场景的地图时以子图为小的建图单元，子图在相对较短的时间内时相对准确的，随着子图数目的增加误差会不断的积累，为了减少随着时间而积累的误差项，需要全局的回环检测来提供约束关系进行全局的图优化修正。闭环检测是指：如果目前正在处理的扫描和所有已创建完成的子图中的某个扫描帧的位置和姿态足够的接近，就可以检测到闭环，本质上是新的scan与已构成submap的scan的一种匹配。图优化的过程：根据当前扫描的姿态和最近的一个子地图优化所有子地图的姿态，即使残差E最小，残差E的计算如REF_Ref72246544\h（2-10所示，实际上也是一个非线性最小二乘优化的过程。ξim为子图的位姿；ξis为扫描的位姿；Σij相关协方差矩阵；（2-SEQ2-\*ARABIC10）图优化的效果如REF_Ref72149117\h图2-6所示，cartographer官方的整体框架如REF_Ref72149133\h图2-7所示(a)图优化之前(b)图优化之后图2-SEQ图2-\*ARABIC6全局SLAM效果图2-SEQ图2-\*ARABIC7Cartographer算法官方框架Cartographer的整体代码结构如REF_Ref72149198\h图2-8所示，Cartographer_ros作为ROS环境下的接口，可以调用ROS中的工具tf，订阅ROS环境下相应传感器的话题（/odom、/imu、/scan），并且发布相应的建图数据到/map话题中。而Cartographer_ros调用