实时相关扫描匹配Real-Time Correlative Scan Matching

引言

本文提出了一种基于概率是扫描匹配算法，可以提供更高质量以及更鲁棒的匹配效果。
扫描匹配算法的挑战是一边减小运算复杂度另一边提高计算结果的质量和健壮性，现有的前端扫描匹配算法为了降低运算复杂度，计算误差往往容易受到初始值的影响，原因是扫描匹配问题通过非线性最小二乘方法计算时，代价函数往往是非凸的，存在许多局部最小值，如果初始值存在一定的误差，那么在非线性优化的过程中计算结果往往陷入局部最小值，无法计算出全局最小值，从而降低了扫描匹配效果。
相关扫描匹配算法基于概率框架，在一定的可行区域内搜索所有可能位姿变换中找到最优的位姿变换，而不是通过非线性优化迭代计算。

概率公式

由贝叶斯定理可得，后验位姿正比于似然乘以先验位姿，扫描匹配的目标就是计算后验位姿分布，似然代表着观测方程，也就是当前先验位姿下可以产生怎样的测量数据，先验位姿对应着运动方程，通常由匀速运动模型或者里程计数据获得。因此如果要得到最佳的后验位姿，可以通过最大化似然的计算方法，非线性优化通过构建非线性最小二乘问题获得代价函数，通过迭代计算的方式最大化自然，从而得到后验位姿。相关方法通过一种更鲁棒的方法进行最大化似然的计算。

算法概述

在先验位姿附近构建一个搜索框，在搜索框内生成经过平移和旋转的一系列点云，将这些点云和子图进行匹配，从而从中挑选出匹配程度最高的点云，也就获得了最大似然，该点云对应的位姿就是后验位姿。

多分辨率地图匹配加速

暴力搜索

对于搜索框内众多的点云，计算似然需要四层循环，分别是该点云对应的航向角、X方向的位移、Y方向的位移、点云中的每个点，时间复杂度过高，无法实时计算。

改善循环结构

由于对点云进行旋转涉及三角函数，相比点云的平移计算量较大，因此为了减少三角函数的调用次数，可以将航向角放在遍历的最外层，可以对算法进行加速。

多分辨率地图

构建多张不同分辨率地图，分辨率越低的栅格地图中，每个低分辨率地图的栅格保留的是原始地图对应栅格中概率的最大值，这样可以保证在低分辨率匹配的时候不会漏掉最大值。
首先点云根据低分辨地图进行粗匹配，然后至选择其中匹配程度较好的点云，在分辨率更高的地图中再次进行匹配，最终计算得到匹配程度最高的点云，在计算过程中避免了很多不必要的计算，因此计算的速度大大提升，最终可以达到满意的实时性