mcl_3dl阅读整理

算法简介

（这部分是github上ReadME搬运过来的）mcl_3dl节点是基于三维激光雷达的定位算法，核心是蒙特卡洛定位（MCL），也称为粒子滤波定位，通过用粒子的密度和权重来表示待估计位姿的概率分布。

观测：

1 LIDAR测量模型：

通常波束模型与似然域模型相比，可以减少地图中被测点的false positive匹配，但更复杂。mcl_3dl节点同时使用这两个模型来以良好的计算能力实现好的匹配结果。计算粒子的likelihood的时候，将用大量随机采样点做似然域模型得到的结果，与用少量随机采样点做波束模型预测的结果相乘，计算不复杂但是又减少了误匹配。
具体做法是：
在似然域模型中，likelihood是每个观测点到地图中最近点的距离之和，最近点的搜索使用分块Kd树，观测点进行体素滤波并随机采样以降低计算复杂度。具体实现在lidar_measurement_model_likelihood.cpp
在波束模型中，likelihood通过ray casting计算，N是观测点的射线数量，n是穿过地图中障碍物的射线数量，alpha是拒绝权重，计算公式是alpha ^（n / N）。具体实现在lidar_measurement_model_beam.cpp

2 轴相关距离权重

在墙壁稀疏只有地板的一些环境中，由于可以修正估计Z位置的点的数量相对较大，在Z方向上的距离就更少关注。距离计算的权重系数可以由参数更改，在上述环境下权重设置为：（（dist_weight_x，dist_weight_y，dist_weight_z）=（1.0，1.0，5.0））。这个在代码里面没有找到体现

3 加速度测量模型 imu产生的观测

通过获取IMU数据中的加速度矢量与粒子姿态之间的差来计算姿态的likelihood。

预测

根据带有噪声参数的里程计数据预测粒子的姿态，建议使用由IMU补偿的里程计数据。

重采样：

mcl_3dl节点通过复制较大概率的粒子来对粒子重新采样，复制的粒子会因高斯噪声而扩散。

扩展重置Expansion resetting

mcl_3dl临时实现了扩展重置方法。如果所有观测点中，具有相关联地图点的观测点的比例小于阈值，则通过给定的高斯噪声扩展粒子分布。一定的程度上可以解决局部极值问题。
代码里的实现就是在之前的基础上给粒子加噪声，然后等待重新收敛

执行

roslaunch mcl_3dl test.launch use_pointcloud_map:=false use_cad_map:=false \ use_bag_file:=true bag_file:=${HOME}/Downloads/short_test.bag

有几个参数：
use_neonavigation = true（默认false） 会执行下面两个其他地方的包：

trajectory_tracker trajectory_recorder path_recorder
dist_interval = 0.1
ang_interval = 0.2
trajectory_tracker trajectory_recorder poses_ref
dist_interval = 0.5
ang_interval = 100.0

use_cad_map = true默认! use_pointcloud_map 会执行下面这个包：

obj_to_pointcloud obj_to_pointcloud obj_to_pointcloud
    <param name="objs" value="$(arg map_objs)" />
    <param name="offset_x" value="$(arg map_offset_x)" />
    <param name="offset_y" value="$(arg map_offset_y)" />
    <param name="scale" value="$(arg map_scale)" />
    <remap from="~/cloud" to="mapcloud" />

use_pointcloud_map = true（默认true）会执行下面这个包：

<node pkg="pcl_ros" type="pcd_to_pointcloud" name="pcd_to_pointcloud"
      args="$(arg map_pcd)" if="$(arg use_pointcloud_map)">
    <remap from="cloud_pcd" to="mapcloud" />
  </node>

上面三个参数额外执行的包都不是mcl_3dl提供的

主要结构

这是mcl_3dl节点执行后的graph

Topic：
接收

• /cloud(雷达发布)
• /odom(码盘)
• /imu/data(imu)
• /tf(一系列的tf变换)
• /mapcloud(点云地图，插入到chunk kdtree，会随时检测地图更新)
• /initialpose(发布这个话题，粒子就在提供的位置处叠加噪声，如果不发布这个话题，粒子就在默认位置处叠加噪声（默认位置可以通过ros参数修改和读取）)

发布

• /tf(odom到map 还有一个floor到map)
• /amcl_pose(map坐标系下的位置)

服务：

• /resize_mcl_particle 可以实时调整粒子数（调整粒子数的操作和重采样很相似，都是大概率的粒子多复制）
• /global_localization 实时启动全局定位
• /expansion_resetting 实时进行扩展重置

在test/src/ test_global_localization.cpp文件中给出了全局定位的使用过程：

• 发布cloud_map；
• 发布生成的cloud imu odom数据 并等待初始化（status变成normal；
• 然后发了5次odom数据 用来那边做预测
• 调用重定位服务并等待状态变成GLOBAL_LOCALIZATION （下面的过程会一直发/cloud /odom /imu/data数据）
• 等待全局定位结束
• 等了40个循环提高位姿精度

部分代码

• chunked_kdtree.h 块kd树 在pcl kdtree基础上增加了分块 在每个分块内进行pcl的kdtree搜索：kdtree_->radiusSearch(p, radius, id, dist_sq, num) 点，半径，紧邻索引值，近邻中心距，

• raycast.h 定义了迭代器，与块kd树相结合的射线遍历 用在雷达波束模型中。这里有一个点：头文件里面重载了*操作符，因为for的范围循环在循环前，会对每一个对象进行一次" * "，再赋值 这个重载真牛逼；然后遍历射线 对射线的每个点在kdtree上搜索最近点。里面进行了两次搜索，没有看太懂 最后用两次的结果计算了sin_ang 做进一步判断

• filter.h 提供了两种滤波方式 LPF和HPF 低通滤波和高通滤波

• nd.h 正态分布 一维和多维的

• point_types.h 定义了pointcloud类型然后注册到点云库中

• point_conversion.h 定义了ros msg到pcl pointcloud转换

• point_cloud_random_sampler.h 随机生成一坨点云

• vec3.h 自己定义了位姿

• quat.h 自己定义了旋转 以四元数形式存储 中间又RPY和轴角的转换

• pf.h 定义了ParticleWeightedMean ParticleFilter（这个里面实现了重采样） Particle ParticleBase

• state_6dof.h 扩展到6自由度位姿

• parameters.h 参数集合

• imu_measurement_model.h 重力加速度计观测 用观测与predict的第三分量，求cos 然后作为方差 返回对应的高斯分布的概率；之前对IMU做观测了解的不多，第一次遇到这种做法。

• motion_prediction_model.h 差分轮式预测模型 设置前与当前的pose与时间间隔 预测下一时刻的pose 运动模型更新部分

• noise_generator.h 生成噪声 正态分布 只在对角线生成和全矩阵生成两种

• lidar_measurement_model.h 雷达观测模型 likelihood和beam模型

• compatibility.h ros subscribe和advertise等兼容性相关的文件

• voxel_grid.h 对点云体素化 然后进行滤波（取每个体素中所有三维点中间一点的点）输出

• lidar_measurement_model_beam.cpp 波束模型的具体实现

• lidar_measurement_model_likelihood.cpp 似然域模型的具体实现

• mcl_3dl.cpp 整个的ros节点，各种topic的回调函数，粒子滤波的过程在/cloud对应的回调函数里面，这个文件比较大 做了一部分注释贴上来

理解

1 mcl_3dl节点的全局定位实现就是和正常定位一样，在整个地图撒粒子，这样开始的时候计算效率肯定不高，但是随着收敛迭代，会不断减小粒子数，这一点想法很好。因为没法实际测试，所以不知道具体的效果怎么样，收敛效率怎么样。
2 全局定位似乎不一定能解决局部收敛问题，比如集中在某几个地方会存在大量的粒子；
3 在定位的过程中也会构建新的地图，但是这个地图没有与传入的地图融合

（最后就是发一些感慨，最近疫情期间，在家学习效率太低了……希望借这篇博客给自己提提神，接下来抓紧时间多学习啊啊啊啊 最后的最后 武汉加油！！！