一种基于激光测距仪的扫描匹配方法
一种基于激光测距仪的扫描匹配方法
作者:刘俊;李霖;
激光测距仪因其快速 、成本低 、环境适应力强等特点 ,常被用作移动机器人的外部传感器 ,扫描匹配通过计算使相邻扫描重叠最大的最优刚体变换 ,以此获取相邻时刻的运动量估计 ,广泛应用于移动机器人同步定位与环境建图 (SimultaneousLocalization and Mapping ,SLAM )领域。 目前应用最广泛的扫描匹配算法是迭代最近邻算法(It‐erative ClosestPoint ,ICP),但该算法对离群扫描点与稀疏扫描点较为敏感 ,由于缺少实际对应点 ,这些扫描点容易建立错误对应关系 。 此外 ,ICP算法根据最近点规则建立对应关系 ,可能存在一对多和对应距离极端大的问题 ,这会影响最优变换计算的准确性 ,从而影响匹配的准确性 。
为了减少扫描数据中的离群扫描点和稀疏扫描点 ,本文提出一种基于连通格序列的方法 ,通过对扫描点进行空间网格划分并建立连通格序列 ,认为连通度较低的连通格是扫描数据的离群或稀疏部分 ,予以剔除 ,为 ICP 算法提供较为理想的输入 ;针对对应关系中的一对多和距离极端大的问题 ,本文通过建立唯一对应 ,避免建立一对多对应关系 ,并基于四分位数法计算对应距离的上截断点 ,对应距离超过该截断点的为错误对应的可能性较大 ,予以剔除 ,减少错误对应数量 ,从而提高匹配的准确性 。
1 ICP 算法
ICP 算法是目前应用最广泛的扫描匹配方法 ,其本质是一个“建立对应关系‐求解最优变换”的迭代优化过程 ,在每次迭代中 ,寻找参考扫描中最近点作为对应点 ,建立目标扫描与参考扫描的对应关系 ,基于该对应关系 ,求解一个使得目标函数最小的最优变换 ,并应用于目标扫描 ,作为下一次迭代的输入 ,直到达到收敛条件 ,最终实现两幅扫描贴合 ,并输出一个最优刚体变换 。 ICP 算法步骤如下 :
1)建立对应。描中距离最近的扫描点作为对应点 ,建立对应关系。一般使用 K‐d 树以加速最近点搜索过程 ;
2)计算最优变换。计算一个最优变换 ,使以下目标函数最小,一般使用单位四元数法求解旋转分量R和平移分量t;
3)应用最优变换。对目标扫描应用最优变换,得到新点集,作为下一次迭代的输入 。
重复上述步骤 ,直到满足收敛条件或迭代次数超出限制 。
2 扫描预处理
ICP 算法采取点-点对应,对离群扫描点较为敏感,然而,由于室内环境的复杂性,激光测距仪在采集数据时可能会产生离群扫描点 ,由于不存在实际对应点 ,这些离群扫描点容易建立错误的对应关系 。 此外 ,在距离激光测距仪较远的区域 ,扫描点较为稀疏 ,这些稀疏扫描点容易建立方向上矛盾的对应关系,因此 ,本文提出一种基于连通格序列的方法 ,通过对扫描点进行空间网格划分并建立连通格序列 ,认为连通度较大的连通格序列是扫描数据的主体部分 ,保留其范围内的扫描点 ,认为连通度较小的连通格序列是扫描数据的离群或稀疏部分 ,剔除其范围内的扫描点,从而为 ICP 算法提供较为理想的输入 。
首先 ,对所有扫描点进行空间网格划分 。 以传感器为坐标原点 ,计算所有扫描点的空间坐标 ,得到扫描点的最小外接矩形 ,根据一定的网格边长进行空间划分,网格边长根据激光测距仪的分辨率与最大扫描距离决定 ,本文使用UTM‐3LX 激光测距仪 ,测量范围R=3m ,角度分辨率 θ= 0.25°,网格边长l=Rsinθ,取值为0.13 m 。
接下来 ,获取连通格序列。网格分为占据格和空格 ,占据格内包含若干扫描点 ,空格内不包含扫描点。首先 ,以第一个占据格 G0 为起点 ,初始化一个连通格序列 ,搜索G0的8邻域范围是否存在其他占据格G1;存在 ,则将 G1 加入连通格序列 ,继续搜索 G1的8邻域范围是否存在其他占据格 ;若不存在 ,该连通格序列建立完毕 ,以下一个占据格为起点 ,初始化一个新的连通格序列 ,重复上述过程。上述步骤一直执行 ,直到所有占据格都归属于某个连通格序列。
最后 ,剔除离群扫描点与稀疏扫描点。定义连通格序列包含的占据格的数量为该序列的连通度,主体部分的扫描点较为连续,因此所归属的连通格序列的连通度较大 ,离群扫描点与主体部分距离较远,稀疏扫描点之间相隔较远,因此所归属的连通格序列的连通度较小。因此,通过剔除连通度较小的连通格序列内的扫描点,可以达到剔除离群扫描点与稀疏扫描点的目标。本文取最小连通度为5,判定连通度小于5的连通格序列内的扫描点为离群扫描点,予以剔除,结果可以看出,离群点与稀疏扫描点被成功去除,扫描点的主体部分得以保留 。
3 改进 ICP 算法
3.1 唯一对应
ICP 算法假设目标扫描中每个点都存在实际对应点,然而,随着移动机器人探索新环境 ,以及环境中的动态障碍物等因素,该假设在实际应用中很难成立 。 例如 ,移动机器人在经过拐角时 ,传感器在上一时刻无法扫描到的障碍物 ,在当前时刻却可以扫描到。ICP 算法依据最近点规则建立对应关系 ,对于目标扫描中每一个点 ,选取参考扫描中距离最近的作为对应点 ,可以看出 ,多个扫描点错误地对应参考扫描中同一个点,这是由于这些扫描点不存在实际对应点 ,仍根据最近点规则建立对应关系所致 ,这些对应关系会显著影响最优变换计算的准确性 。
因此 ,本文提出一种建立对应关系的新规则 ,通过限定每个参考点只能分配给最近的扫描点作为对应点 ,从而保证对应关系的唯一性 。 例如 ,给定目标扫描中某扫描点 pi ,寻找参考根据该规则建立对应关系 ,可以看出 ,一对多对应问题被成功解决 。 然而 ,该方法只能保证对应的唯一性 ,无法保证正确性 ,因此仍可能建立少数对应距离极端大的对应关系 ,在下一节将介绍一种稳健的剔除方法予以剔除 。
4、试验结果及分析
4.1 实验设备
实验设备由一台移动小车和一个激光测距仪(U T M‐30LX ,Hokuyo Automatic Co. Ltd)组成 ,激光测距仪的最大测量距离为30m ,扫描角度范围270° ,角度分辨率为 0.25° ,安装在移动小车前方 ,对室内环境进行水平扫描 ,获取二维扫描数据。
4.2 实验过程及成果
4.2.1局部匹配实验
为了验证本文算法对于相邻时刻扫描的匹配效果 ,本文设计一组实验 ,实验选取一组相邻时刻的扫描数据 ,可以看出 ,当前时刻扫描在矩形 1 区域中存在离群点 ,在矩形 2区域中存在部分稀疏扫描点 。
为了验证扫描预处理对于匹配结果的影响 ,实验 1 以当前时刻原始扫描为目标扫描 ,上一时刻原始扫描为参考扫描 ,应用 ICP 算法进行匹配实验 ,实验 2 对当前时刻扫描进行预处理 ,以处理后的扫描作为目标扫描 ,上一时刻原始扫描作为参考扫描 ,同样应用 ICP 算法进行匹配实验 。 在建立对应关系阶段 ,实验 1 建立的对应关系可以看出 ,由于目标扫描中存在离群点 ,且在参考扫描中缺少其实际对应点 ,从而产生错误对应 ,实验 2 建立的对应关系可以看出 ,扫描预处理成功剔除了离群点与部分稀疏扫描点 ,但仍存在一对多对应和部分距离极端大的对应 。
验 1 和实验 2 的匹配结果可以看出 ,目标扫描与参考扫描间均存在较为明显的偏离 ,匹配结果都不理想 ,这说明扫描预处理仅能减少扫描中的离群点和稀疏点 ,但无法解决一对多对应和少数对应距离极端大的问题 。
为了验证 ICP 改进算法处理一对多对应和距离极端大对应的效果 ,实验 3 与实验 4 均以扫描预处理后的当前时刻扫描为目标扫描 ,以上一时刻原始扫描为参考扫描 ,实验 3 应用 ICP 标准算法进行匹配 ,建立的对应关系可以看出 ,对应关系中仍存在一对多对应和少数极端大的对应关系 ,其匹配结果可以看出 ,扫描之间存在明显的偏离 ,匹配结果并不理想 。 实验4 应用 ICP 改进算法进行匹配 ,建立的对应可以看出 ,一对多对应和少数极端大的对应关系被去除 ,其匹配结果可以看出 ,目标扫描与参考扫描紧密贴合 ,匹配结果明显改善 。
4.2.2 全局匹配实验
为了验证本文方法在实际应用中的匹配表现 ,我们使用移动小车与激光测距仪采集了武汉大学化学院三楼部分走廊的扫描数据 ,在缺少里程计提供匹配初值的条件下 ,以第一幅扫描数据的坐标系为参考 ,分别应用 ICP 标准算法和本文算法 ,对所有相邻时刻扫描数据进行匹配实验 ,得到实验结果 。 ICP 算法的匹配结果可以看出 ,累计误差较大 ,匹配结果在走廊的后半段出现弯曲 。 本文方法的匹配结果可以看出 ,匹配结果未出现明显弯曲 ,较好地表达了室内的平面结构 ,说明本文算法在实际应用中具有较好的匹配表现 。
5 结束语
为了剔除扫描数据中的离群扫描点和稀疏扫描点 ,本文提出一种基于连续格序列的方法 ,通过对扫描点进行网格划分并建立连续格序列 ,剔除连通度较小的网格内的扫描点 ,实现扫描数据的预处理 ,为扫描匹配提供较为理想的输入 ,实验结果表明 ,该方法能够有效剔除扫描数据中的离群扫描点和稀疏扫描点 。 此外 ,针对匹配过程中产生的一对多对应和距离极端大的对应 ,本文通过限定对应关系的唯一性 ,从而避免建立一对多对应 ,然后基于一种稳健的四分位数法计算距离阈值 ,剔除对应距离大于阈值的对应关系 ,从而减少对应距离极端大的对应关系 ,提高匹配的准确性 ,实验结果表明 ,本文方法能够较好地匹配相邻时刻扫描数据 ,且在实际应用中具有较好的全局匹配表现 。
本文章转自爱学术(aixueshu.com),如有侵权,请联系删除