一种基于改进Hector SLAM算法的未知环境自主定位方法技术

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

【技术实现步骤摘要】
一种基于改进HectorSLAM算法的未知环境自主定位方法
本专利技术属于激光SLAM自主定位
，特别是一种基于改进HectorSLAM算法的未知环境自主定位方法。
技术介绍
SLAM是同步定位与地图构建(SimultaneousLocalizationAndMapping)的缩写，最早由HughDurrant-Whyte和JohnJ.Leonard提出。SLAM主要用于解决移动机器人在未知环境中运行时定位导航与地图构建的问题。SLAM通常包括如下几个部分，特征提取，数据关联，状态估计，状态更新以及特征更新等。HectorSLAM算法是一种基于优化理论的高效、快速的在线SLAM算法。HectorSLAM算法由于未消除累计误差，因此在建图过程中，随着时间的推移建图误差会逐渐积累变大，降低了建图精度。再有，当所处环境为长廊或隧道时，由于激光雷达量程有限，激光SLAM算法也无法实现精确的定位。
技术实现思路
本专利技术的目的在于提供一种基于改进HectorSLAM算法的未知环境自主定位方法，通过增加融合视觉里程计补偿激光雷达定位的不足之处、及通过回环检测消除累积误差，提高输出实时地图的精度，实现机器人在未知环境中的精确自主定位。本专利技术采用的技术方案为一种基于改进HectorSLAM算法的未知环境自主定位方法，其特征在于包括以下步骤：(1)利用HectorSLAM算法，得到机器人的位姿数据实时建图；(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为形成回环检测机器人移动的最小距离。本专利技术的优点：在移动机器人上搭载相机作为激光雷达的辅助，增加相机提供的实时图像数据，通过融合视觉里程计补偿激光量程有限的不足之处，提高定位准确性，增加回环检测消除累计误差；利用g2o图优化库对地图进行优化，使建立的实时地图精度更高鲁棒性更好，对未知环境自主定位精度更高。附图说明图1，为改进的HectorSLAM算法流程图。图2，为改进的HectorSLAM算法的回环检测流程图。图3，为UKF融合2D激光SLAM算法与视觉里程计信息的具体实施示意图。图4，为利用g2o图优化库对地图进行优化的主要步骤示意图。具体实施方式下面对本专利技术的具体实施方式进行描述，以便于本
的技术人员理解本专利技术，但应该清楚，本专利技术不限于具体实施方式的范围，对本
的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本专利技术的精神和范围内，这些变化是显而易见的，一切利用本专利技术构思的专利技术创造均在保护之列。首先，在移动机器人上搭载有激光雷达及相机，通过激光雷达扫描获取激光雷达数据。参看图1至图4，基于改进HectorSLAM算法的未知环境自主定位方法，具体步骤如下：(1)数据预处理HectorSLAM算法使用能够代表任意环境的栅格地图，首先将通过激光雷达的获得扫描数据转换为扫描端点的2D点云，由于占用栅格地图的离散性带来的精度问题，需对数据进行预处理。为解决占用栅格地图的离散性带来的精度问题采用双线性滤波估计占用概率。连续地图坐标P(m),使用占用值M(Pm)表示该点被映射为占据点的概率，其梯度为M(Pm)使用四个点P00、P10、P01、P11分别在X轴和Y轴进行线性插值后求得：P00、P10在x轴上进行插值得到I0：P01、P11在y轴上进行插值得到I1：对I0和I1在y轴上进行线性插值得到Pm：即：插值的四个点处于一个网络，单位距离为1，可认为x1-x0＝1、y1-y0＝1，倒数可近似为：(2)特征点提取对预处理后的激光雷达2D点云数据，进行特征点提取。采用PCL算法提取每一帧2D点云中所有的几何特征，包括半径R、点密度l、面积S。将每一帧中所提取的几何特征用特征向量F表示。Fi＝[RiliSi]TFi为第i帧的特征向量；将所有的特征向量存储起来形成历史数据。()(3)扫描匹配，实时建图通过扫描匹配，得到预估位姿。在栅格地图中求位姿ε＝[xyθ]T,构造目标函数：当目标函数的值越小，相对位姿就越准确。通过最新的激光雷达数据与当前地图进行对齐求解相对位姿变化，Si(ε)为在第t时刻将第i个激光点映射到地图坐标系下，机器人相对于地图的位姿为，激光端点相对于机器人的坐标为si＝(si.x,si.y)T，激光数据经旋转平移变换转换到全局(地图)坐标系：M(Si(ε))表示返回坐标Si(ε)的地图值。利用高斯牛顿法求解非线性最小二乘问题，目标函数ri如式(8)所示：ri＝1-M(Si(ε))(8)高斯-牛顿法的递推关系如式(9)所示：εt＝εt-1-H-1G(9)式中G为目标函数的梯度向量，H表示海森矩阵，假设系统做了很微小的运动，位姿变化为Δε且小到可以忽略，则：式(11)中代表在全局地图中Si(ε)点的梯度，Si(ε)的导数用矩阵表示的结果如(12)所示：根据和可以解出目标函数,解出的相对位姿变换接近最小值，Δε为：(4)消除激光扫描建图的误差为了消除激光扫描建图的误差，在移动机器人上还搭载有相机，通过相机获取的里程计信息和激光雷达扫描数据的融合，补偿激光SLAM算法在隧道或长廊中纵向位置信息的输出，减小误差，提高地图精度。①运行Lukas-Kanade跟踪器追踪相机拍摄的图像中的FAST特征实现视觉里程计；②采用UKF算法，在扫描匹配的预估位姿数据中融合视觉里程计的信息，以补偿激光SLAM算法在隧道或长廊中纵向位置信息的输本文档来自技高网...

【技术保护点】
1.一种基于改进Hector SLAM算法的未知环境自主定位方法，其特征在于包括以下步骤：/n(1)利用Hector SLAM算法，得到机器人的位姿数据实时建图；/n(2)在步骤(1)实施的同时，实现视觉里程计；/n(3)将步骤(1)得到的位姿数据和步骤(2)得到的视觉里程计的数据信息融合，以补偿激光SLAM算法在隧道或长廊中纵向位置信息的输出。/n(4)通过回环检测消除实时地图累计误差。/n

【技术特征摘要】
1.一种基于改进HectorSLAM算法的未知环境自主定位方法，其特征在于包括以下步骤：
(1)利用HectorSLAM算法，得到机器人的位姿数据实时建图；
(2)在步骤(1)实施的同时，实现视觉里程计；
(3)将步骤(1)得到的位姿数据和步骤(2)得到的视觉里程计的数据信息融合，以补偿激光SLAM算法在隧道或长廊中纵向位置信息的输出。
(4)通过回环检测消除实时地图累计误差。


2.根据权利要求1所述基于改进HectorSLAM算法的未知环境自主定位方法，其特征在于，还包括步骤(5)，通过g2O图优化库对实时地图进行优化。


3.根据权利要求1所述基于改进HectorSLAM算法的未知环境自主定位方法，其特征在于所述步骤(1)包括：
A1、数据预处理：将获取的激光雷达数据转化，并进行精度处理，得到处理后的2D点云；
A2、提取每一帧2D点云中所有的几何特征，将几何特征用特征向量表示并存储作为历史数据；
A3、扫描匹配，得到预估位姿，进行实时建图。


4.根据权利要求1所述基于改进HectorSLAM算法的未知环境自主定位方法，其特征在于所述步骤(2)中，采用Lukas-Kanade跟踪器追踪相机拍摄的图像中的FAST特征实现视觉里程计。


5.根据权利要求3所述基于改进HectorSLAM算法的未知环境自主定位方法，其特征在于所述步骤(3)中，采用UKF算法，在扫描匹配的预估位姿数据中融合视觉里程计...

【专利技术属性】
技术研发人员：楚红雨，王阳，王亮，
申请(专利权)人：鲲鹏通讯昆山有限公司，昆山鲲鹏无人机科技有限公司，苏州明逸智库信息科技有限公司，
类型：发明
国别省市：江苏;32