一种基于ICP和遗传优化相结合的机器人导航扫描匹配算法

技术领域

本发明涉及机器人导航领域，具体涉及一种基于ICP和遗传优化相结合的机器人导航扫描匹配算法。

背景技术

由于现代社会对提高生产效率的需要和人们对智能生活方式的向往，移动机器人技术得到了快速发展，在生活中出现的频率越来越高，如高端酒店中的迎宾机器人、疫情期间医院中的消毒机器人和抢险救灾的救援机器人等。在移动机器人众多应用场景中，周围环境不是一成不变的，有时候甚至是未知的。作为移动机器人在未知环境下实现自主导航的关键技术，SLAM(即时定位与地图构建)因此显得尤为重要。

SLAM的目标是解决在未知环境中构建地图、不断更新地图以及在地图中进行目标物体定位的问题。基于图优化方法的激光SLAM，分为前端和后端。前端负责实现激光里程计算，包括帧间匹配、回环检测等，旨在根据激光雷达的观测，构建误差函数，用于后端优化。后端则负责进行优化，如根据前端建立的约束，优化位姿；根据回环检测的结果，优化位姿图；发布地图等。后端的作用是修正位姿和地图等。在进行运动估计之后，当固定了第一帧图像的初始位姿，理论上便可以递推出后续各帧的位姿，但是这样会造成累计误差的产生(对相邻两帧估计的位姿误差会累加到下一帧上致使误差越来越大)。因此，需要进行回环检测来消除这个累积误差。在基于图的SLAM中，闭环检测是获得全局姿态约束的关键步骤，该过程的实时性能确保当前帧的后端优化可以在下一时刻的数据到达之前顺利完成。

ICP(迭代最近点)算法是一种帧间匹配算法，它也可以用来做闭环检测，是激光SLAM核心部分。由于采集设备不同、拍摄视角不同等等因素的影响，即使是同一个物体所得到的点云也会有较大的差异，主要是旋转或者平移的变化。对于一组图像数据集中的两幅图像，需要通过寻找一种空间变换把一幅图像映射到另一幅图像，使得两图中对应于空间同一位置的点一一对应起来，从而达到信息融合的目的。但是，仅使用这种方法无法消除由完成的局部匹配引起的误差，并且累积误差在构建大比例尺地图时会导致明显的漂移。以及收敛速度慢，在源点云和目标点云初始姿态不佳时存在容易陷入局部最优解等问题。

NDT是将第一帧点云转换至栅格地图的算法，每个栅格计算其中点的正态分布，因此将第一帧点云转换为一个个栅格表示的分段连续可导的概率密度函数，使用正态分布概率密度函数描述点云的局部性质。然后将第二帧点云投影至栅格地图中，计算出第二帧点云在栅格地图中的概率，通过牛顿法找到最佳匹配位姿，使得第二帧点云投影至栅格地图后概率最大化，最终完成点云配准。具体流程为：建立第一帧点云的栅格地图，牛顿法迭代使得第二帧点云投影至栅格地图得到的概率最大化。但是，对于结构化点云，NDT对一块栅格高斯分布的假设不成立，效果不好，反而点线ICP或点面ICP效果更好一些。对于非结构大规模点云，NDT速度快一些，初值鲁棒性取决于栅格大小，越大精度越差，但对初值鲁棒性好一些，反之，对初值更依赖。

ICP变种相对于ICP最大的区别是其改进了误差方程。ICP是点对点的距离作为误差而ICP变种是采用点到其最近两个点连线的距离。其工作流程如下：将当前帧数据根据初始位姿投影到参考帧坐标系下；对于当前帧的点，用最近邻法则在参帧中找到最近的两个点(j1，j2)；计算误差，并去除误差过大的Outliers；根据最小化误差函数求解矩阵R和平移矩阵。该方式，会随着地图比例尺的增长而造成很大的计算消耗，直到无法承受为止。另外，ICP及其变种对初始值很敏感，不单独使用，其容易陷入局部循环。

Cartographer通过创建许多小的submap来处理较大的空间，优化所有扫描和子图的姿势，遵循稀疏姿势调整。所有创建完成的submap以及当前的laser scan都会用作回环检测的匹配。如果当前的scan和所有已创建完成的submap在距离上足够近，那么通过某种匹配策略就会找到该闭环。为了减少计算量，提高实时回环检测的效率，Cartographer应用了branch andbound(分支定界)优化方法进行优化搜索。该方式，内存占用较大，算法体量较大。

发明内容

本发明的目的是提供一种基于ICP和遗传优化相结合的机器人导航扫描匹配算法，其可以实现高效且鲁棒的闭环检测。

本发明采取的技术方案具体如下。

一种基于ICP和遗传优化相结合的机器人导航扫描匹配算法，其特征在于，包括如下操作：

S10：运行遗传算法，找到(x，y)平移和旋转φ，在两个不同的扫描之间获得最佳对准，扫描位置(x，y，φ)的每个参数在染色体中被编码为比特串，问题空间在有限解空间中离散化，设置搜索区域限制和分辨率，在姿势跟踪和可以进行测距测量的情况下，根据测距误差模型进行设置；

S20：将遗传算法输出的位姿结果给到ICP算法，ICP算法最小化一个目标函数；然后寻找对应点；利用最小二乘法对旋转矩阵R和平移矩阵T优化；

S30：最后不断迭代优化，便可实现精确位姿匹配。

详细的操作为：步骤S10中：根据手头的问题来设置搜索区域限制。

步骤S10中：根据搜索区域的扩展和每个基因的位数设置分辨率。

步骤S20中：目标函数为所有对应点之间的欧式距离的平方和。

步骤S20中对应点认定标准为：只要两个点云中存在距离小于阈值，便可以认定为对应点。亦即，只要将两个点云的距离最小化，便可以认定为对应点。

本发明提供的技术方案中，结合了遗传算法和ICP算法的优点，并克服了相互的缺点，可以在全局搜索空间中短期内搜索到ICP算法所需的最优初值，从而实现高效且鲁棒的闭环检测，激光SLAM也因此实现更好的建图。并且，在移动机器人中非常普遍地出现大扫描错误时，所提出的算法成功地克服了较大的定位误差带来的影响。

附图说明

图1为本发明的示意图。

具体实施方式

为了使本发明的目的及优点更加清楚明白，以下结合实施例对本发明进行具体说明。应当理解，以下文字仅仅用以描述本发明的一种或几种具体的实施方式，并不对本发明具体请求的保护范围进行严格限定。

如图1所示，本发明提供的基于ICP和遗传优化相结合的机器人导航扫描匹配算法，具体操作如下。

S10：首先，运行遗传算法，找到(x，y)平移和旋转φ，在两个不同的扫描之间获得最佳对准，扫描位置(x，y，φ)的每个参数在染色体中被编码为比特串，问题空间在有限解空间中离散化，根据搜索区域的扩展和每个基因的位数设置分辨率，根据手头的问题来设置搜索区域限制，在姿势跟踪和可以进行测距测量的情况下，根据测距误差模型进行设置。

步骤S10的作用是采用遗传算法求出ICP算法的最优初值，详细的操作如下。

S11：确定搜索窗口。

对于扫描匹配问题，使用W作为搜索窗口，设置线性和角度搜索窗口偏移W

其中，dmax是扫描点的最大范围。

得到覆盖给定线性和角度搜索窗口大小的步骤的整数:

S12：染色体编码。

将二维的线性搜索空间扩展到一维进行表示，即转换集合

使用

S13：在编码步骤中，确保染色体能够代表可行解中的所有x，y和θ进行全局优化。

让染色体的长度为l＝l

S15：遗传算子。

遗传算子包括变异算子、选择算子和交叉算子。传统轮盘策略用于选择操作，两点交叉用于交叉操作。突变操作采用基本位置突变方法。考虑到匹配过程的不连续性，降低交叉率并扩大突变率，这可以防止遗传算法陷入局部迭代。

S16：染色体解码。

在染色体

S20：其次，将遗传算法输出的位姿结果给到ICP算法，ICP算法最小化一个目标函数，目标函数为所有对应点之间的欧式距离的平方和；然后寻找对应点，对应点认定标准为：只要两个点云中存在距离小于阈值，便可以认定为对应点；随后利用最小二乘法对旋转矩阵R和平移矩阵T优化。

S30：最后不断迭代优化，便可实现精确位姿匹配。

步骤S30是基于遗传算法的ICP闭环检测，详细的操作为。

参考扫描的一个点和新扫描的一个点，设它们在环境中但是来自不同位置的相同的点进行采样。考虑一个在坐标系(x

如果在第一次扫描中点P1(x

扫描匹配问题在于通过找到两个因素来估计机器人的运动线性指数

帧间匹配算法直接影响激光SLAM的效果，后端优化只是消除该过程所积累的误差，帧间匹配估计的位姿越准确，后期建图效果越好。ICP算法则是一种常用的帧间匹配算法，可以获得非常精确的配准效果、不必对处理的点集进行分割和特征提取，并且在较好的初值情况下，可以得到很好的算法收敛性。但是ICP算法受初值影响很大，在源点云和目标点云初始姿态不佳时存在容易陷入局部最优解等问题。而遗传算法是一种可以在短时间内通过遗传迭代在全局搜索空间中找到最优解且不易陷入局部极小值的启发式算法。利用遗传算法全局寻优能力，将遗传算法输出的位姿结果给到ICP算法实现精确位姿匹配，即结合两者的优势实现高效率的闭环检测功能。

因此，本方案中将遗传算法和ICP算法的优点相结合，并克服了相互的缺点，可以在全局搜索空间中短期内搜索到ICP算法所需的最优初值，从而实现高效且鲁棒的闭环检测，激光SLAM也因此实现更好的建图。并且，在移动机器人中非常普遍地出现大扫描错误时，所提出的算法成功地克服了较大的定位误差带来的影响。

本发明中未详尽描述的措施，参照现有的遗传算法和ICP算法进行实施。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本发明中未具体描述和解释说明的结构、机构以及操作方法，如无特别说明和限定，均按照本领域的常规手段进行实施。