[发明专利]一种基于改进Hector SLAM算法的未知环境自主定位方法

说明书

本发明公开一种基于改进Hector SLAM算法的未知环境自主定位方法，该方法为：首先利用改进的Hector SLAM算法，得到机器人的位姿和实时地图，改进的Hector SLAM算法加入了回环检测，以提高系统的鲁棒性并消除建图时产生的积累误差；然后通过Lukas‑Kanade跟踪器追踪图像中的FAST特征实现视觉里程计；最后采用UKF算法融合激光SLAM和视觉里程计的信息，以补偿激光SLAM算法在隧道或长廊中纵向位置信息的输出。在此基础上，利用图优化对累积误差进行进一步优化，可有效解决机器人定位过程中的累积误差问题，从而实现在未知环境中的精确自主定位。

技术领域

本发明属于激光SLAM自主定位技术领域，特别是一种基于改进Hector SLAM算法的未知环境自主定位方法。

背景技术

SLAM是同步定位与地图构建(Simultaneous Localization And Mapping)的缩写，最早由Hugh Durrant-Whyte和John J.Leonard提出。SLAM主要用于解决移动机器人在未知环境中运行时定位导航与地图构建的问题。SLAM通常包括如下几个部分，特征提取，数据关联，状态估计，状态更新以及特征更新等。Hector SLAM算法是一种基于优化理论的高效、快速的在线SLAM算法。Hector SLAM算法由于未消除累计误差，因此在建图过程中，随着时间的推移建图误差会逐渐积累变大，降低了建图精度。再有，当所处环境为长廊或隧道时，由于激光雷达量程有限，激光SLAM算法也无法实现精确的定位。

发明内容

本发明的目的在于提供一种基于改进Hector SLAM算法的未知环境自主定位方法，通过增加融合视觉里程计补偿激光雷达定位的不足之处、及通过回环检测消除累积误差，提高输出实时地图的精度，实现机器人在未知环境中的精确自主定位。

本发明采用的技术方案为一种基于改进Hector SLAM算法的未知环境自主定位方法，其特征在于包括以下步骤：

(1)利用Hector SLAM算法，得到机器人的位姿数据实时建图；

(2)在步骤(1)实施的同时，实现视觉里程计；

(3)将步骤(1)得到的位姿数据和步骤(2)得到的视觉里程计的数据信息融合，以补偿激光SLAM算法在隧道或长廊中纵向位置信息的输出。

(4)通过回环检测消除实时地图累计误差。

还包括步骤(5)，通过g2O图优化库对实时地图进行优化。

所述步骤(1)包括：

A1、数据预处理：将获取的激光雷达数据转化，并进行精度处理，得到处理后的2D点云；

A2、提取每一帧2D点云中所有的几何特征，将几何特征用特征向量表示并存储作为历史数据；

A3、扫描匹配，得到预估位姿，进行实时建图；

所述步骤(2)中，采用Lukas-Kanade跟踪器追踪相机拍摄的图像中的FAST特征实现视觉里程计。

所述步骤(3)中，采用UKF算法，在扫描匹配的预估位姿数据中融合视觉里程计的信息，以补偿激光SLAM算法在隧道或长廊中纵向位置信息的输出。

所述步骤(4)中的回环检测的步骤如下：

S1，找出当前帧所对应特征向量与历史数据中特征向量之间的距离最小值；

S2，计算与当前帧对应特征向量距离最小值的历史帧的距离阈值，当该距离阈值大于S1步骤中的距离最小值时，且视觉里程计的当前帧的输出位移≥D，则判断形成了回环；所述D为形成回环检测机器人移动的最小距离。