CN111578959B - 一种基于改进Hector SLAM算法的未知环境自主定位方法 - Google Patents

Abstract

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

Description

一种基于改进Hector 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为形成回环检测机器人移动的最小距离。

本发明的优点：在移动机器人上搭载相机作为激光雷达的辅助，增加相机提供的实时图像数据，通过融合视觉里程计补偿激光量程有限的不足之处，提高定位准确性，增加回环检测消除累计误差；利用g2o图优化库对地图进行优化，使建立的实时地图精度更高鲁棒性更好，对未知环境自主定位精度更高。

附图说明

图1，为改进的Hector SLAM算法流程图。

图2，为改进的Hector SLAM算法的回环检测流程图。

图3，为UKF融合2D激光SLAM算法与视觉里程计信息的具体实施示意图。

图4，为利用g2o图优化库对地图进行优化的主要步骤示意图。

具体实施方式

下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

首先，在移动机器人上搭载有激光雷达及相机，通过激光雷达扫描获取激光雷达数据。参看图1至图4，基于改进Hector SLAM算法的未知环境自主定位方法，具体步骤如下：

(1)数据预处理

Hector SLAM算法使用能够代表任意环境的栅格地图，首先将通过激光雷达的获得扫描数据转换为扫描端点的2D点云，由于占用栅格地图的离散性带来的精度问题，需对数据进行预处理。为解决占用栅格地图的离散性带来的精度问题采用双线性滤波估计占用概率。

连续地图坐标P(m),使用占用值M(P_m)表示该点被映射为占据点的概率，其梯度为M(P_m)使用四个点P₀₀、P₁₀、P₀₁、P₁₁分别在X轴和Y轴进行线性插值后求得：

P₀₀、P₁₀在x轴上进行插值得到I₀：

P₀₁、P₁₁在y轴上进行插值得到I₁：

对I₀和I₁在y轴上进行线性插值得到P_m：

即：

插值的四个点处于一个网络，单位距离为1，可认为x₁-x₀＝1、y₁-y₀＝1，倒数可近似为：

(2)特征点提取

对预处理后的激光雷达2D点云数据，进行特征点提取。采用PCL算法提取每一帧2D点云中所有的几何特征，包括半径R、点密度l、面积S。将每一帧中所提取的几何特征用特征向量F表示。

F_i＝[R_i l_i S_i]^T

F_i为第i帧的特征向量；将所有的特征向量存储起来形成历史数据。()(3)扫

描匹配，实时建图

通过扫描匹配，得到预估位姿。在栅格地图中求位姿ε＝[x y θ]^T,构造目标函数：

当目标函数的值越小，相对位姿就越准确。

通过最新的激光雷达数据与当前地图进行对齐求解相对位姿变化，S_i(ε)为在第t时刻将第i个激光点映射到地图坐标系下，机器人相对于地图的位姿为，激光端点相对于机器人的坐标为s_i＝(s_i.x,s_i.y)^T，激光数据经旋转平移变换转换到全局(地图)坐标系：

M(S_i(ε))表示返回坐标S_i(ε)的地图值。

利用高斯牛顿法求解非线性最小二乘问题，目标函数r_i如式(8)所示：

r_i＝1-M(S_i(ε)) (8)

高斯-牛顿法的递推关系如式(9)所示：

ε_t＝ε_t-1-H^-1G (9)

式中G为目标函数的梯度向量，H表示海森矩阵，假设系统做了很微小的运动，位姿变化为Δε且小到可以忽略，则：

式(11)中代表在全局地图中S_i(ε)点的梯度，S_i(ε)的导数用矩阵表示的结果如(12)所示：

根据和/>可以解出目标函数,解出的相对位姿变换接近最小值，Δε为：

(4)消除激光扫描建图的误差

为了消除激光扫描建图的误差，在移动机器人上还搭载有相机，通过相机获取的里程计信息和激光雷达扫描数据的融合，补偿激光SLAM算法在隧道或长廊中纵向位置信息的输出，减小误差，提高地图精度。

①运行Lukas-Kanade跟踪器追踪相机拍摄的图像中的FAST特征实现视觉里程计；

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

基于UKF的激光SLAM与视觉里程计融合定位方法，参看图3，UKF算法具体分为两个部分。

第一步：初始化状态变量及其均方差

式中x₀为初始状态变量，是初始状态的估计值，P₀是估计方差。

第二步：时间更新和量测更新

本发明中激光雷达和下视相机用来估计机器人在水平面的相对运动，相对测量将当前和过去的状态与传感器输出连接起来，可以表示为：

z_t+m＝h_a(X_t+m,X_t,n_t+m) (14)

式中n_t+m是传感器噪声，h_a(·)是一个非线性函数

UKF中每一个状态都可以用几个关键点表示，当作用于非线性函数时，只需要将关键点作用于非线性函数即可计算新的状态。让x假设向量y是x的非线性函数值，则向量y，x与y的互协方差可近似为：

式中ω_i ^m和ω_i ^c是关键点的权重，为量测变量的预测均值，P^yy、P^yx是x与y的互协方差。这种无迹变换可用于跟踪状态传播和测量更新过程中的协方差，从而避免了计算雅可比矩阵。

(5)增加回环检测，消除实时建图的累计误差，提高地图精度。

加入回环检测的目的在于通过找到已经走过的地方然后构建一个约束，从而消除累积误差。参看图2，回环检测的具体步骤如下：

S1，输入激光雷达数据。

S2，采用PCL算法提取每一帧2D点云中所有的几何特征，包括半径R、点密度l、面积S。

S3，将每一帧中所提取的几何特征用特征向量F表示。

F_i＝[R_i l_i S_i]^T

F_i为第i帧的特征向量；

S4，将所有的特征向量存储起来形成历史数据。

S5，找出当前帧(第i帧)所对应特征向量与历史数据中特征向量之间的最小值，最小值用dist_min(F_i,F_y)表示，假设第y帧为特征向量距离最小帧。

两个特征向量F_i、F_n之间的距离用马氏距离表示：

其中F_i为当前帧(第i帧)所对应的特征向量，F_n为历史数据中任意帧(第n帧)所对应的特征向量。

S6：计算第y帧对应的距离阈值L，并判断是否形成回环。

考虑到在高度相似、对称的环境中，由于几何特征单一，可能会引起回环误检，因此加入以下约束：

若视觉里程计输出的位移信息y≥D，且：

dist_min(F_i,F_y)＜L

则认为改进的Hector SLAM算法形成了回环。

其中F_i为当前帧(第i帧)所对应的特征向量，y是视觉里程计的输出位移信息，D∈[20m,50m]，是形成回环检测机器人移动的最小距离，L是判断检测到回环时特征向量之间马氏距离的阈值，dist_min(F_i,F_y)是当前帧(第i帧)所对应特征向量与历史数据中特征向量之间的最小值，假设第y帧为特征向量距离最小帧。

检测到回环时特征向量之间的阈值，通过如下方法确定：

通过计算最小距离帧与相邻两帧特征向量之间的距离来确定阈值L。机器人以速度v向前运动，受相邻特征向量之间距离变化率影响，表达式如下：

v₀是机器人的初始运动速度，取初始速度为1m/s,K_v∈[-0.005,0.005]为速度调节系数，f为激光雷达的扫描频率。

假设第y帧，是当前帧(第i帧)与历史帧的特征向量距离最小帧。

在这里分为两种情况：

第一种情况：当前帧(第i帧)，不是与第1帧的距离最近：

阈值：

第二种情况：当前帧(第i帧)，是与第1帧的距离最近：

阈值：

其中k_p∈[0,1]是调节系数。

(6)进一步图优化

利用g2o图优化库对地图进行进一步优化，其主要分为两步：

构建地图。机器人在移动过程中，它在某时刻t的位姿是一个顶点，位姿之间的关系构成一个边，时刻t和时刻t+1之间的相对位姿变换矩阵就是边，边通常表示误差项。

优化地图：调整机器人的位姿(顶点)来尽量满足边的约束，使得误差最小。

在栅格地图中t时刻位姿(ε)表示为T_t，其观测方程为：

T_t＝A(ε_t)

由于误差的存在，T_t不可能精确的等于A(ε_t)，于是就有了误差：

e_t＝T_t-A(ε_t)

假设一个带有n条边的图，其误差函数可以写成:

其中Z为对角矩阵，对角的元素大小表明对此误差的重视程度。由于T_t已知可将上式写为：

由于g2o图优化方法是很成熟的技术，参看图4，其具体步骤如下：

a、创建线性方程求解器；

b、创建一个块求解器(BlockSolver)，并利用a创建的线性方程求解器初始化；

c、创建一个总求解器，并利用b创建的块求解器(BlockSolver)进行初始化；

d、创建稀疏优化器；

e、定义地图的顶点和边并添加到稀疏优化器中；

f、开始优化地图。

(7)将经过优化的地图输出。

改进的Hector SLAM算法将最新的激光雷达数据与当前地图进行对齐，求解系统的相对位姿变化，通过使用多分辨率栅格地图来提高算法的运算速度并规避算法陷入局部极值，并加入回环检测进行图优化消除累计误差；再通过g2o图优化库对地图进行优化，使建立的实时地图精度更高鲁棒性更好，对未知环境自主定位精度更高。

Claims (3)

1.一种基于改进Hector SLAM算法的未知环境自主定位方法，其特征在于包括以下步骤：

步骤1：利用改进的Hector SLAM算法，得到机器人的实时位姿并建立实时地图；改进的Hector SLAM算法分为前端和后端两部分，前端包括建图和回环检测，后端采用g2o图优化库进行地图优化；

步骤2：通过运行Lukas-Kanade跟踪器追踪图像中的FAST特征实现视觉里程计；

步骤3：采用UKF算法融合改进的Hector SLAM算法和视觉里程计的信息以补偿激光SLAM算法在隧道或长廊中纵向位置信息的输出；

步骤4：通过回环检测消除实时地图累计误差，回环检测步骤包括：

⑴获取激光雷达数据；

⑵采用PCL算法提取每一帧2D点云中所有的几何特征，包括半径R、点密度l、面积S；

⑶将每一帧中所提取的几何特征用特征向量表示；

F_i＝[R_i l_i S_i]^T

⑷将所有的特征向量存储起来形成历史数据；

⑸找出当前帧(第i帧)所对应特征向量与历史数据中特征向量之间的最小值，最小值用dist_min(F_i,F_y)表示，假设第y帧为特征向量距离最小帧；

⑹计算第y帧对应的距离阈值L，并判断是否形成回环；

步骤(5)和步骤(6)中，找出当前帧(第i帧)所对应特征向量与历史数据中特征向量之间的最小值并判断是否形成回环检测的具体方法如下：

两个特征向量F_i、F_n之间的距离用马氏距离表示：

其中F_i为当前帧(第i帧)所对应的特征向量，F_n为历史数据中任意帧(第n帧)所对应的特征向量；

考虑到在高度相似、对称的环境中，由于几何特征单一，可能会引起回环误检，因此加入以下约束：

若视觉里程计输出的位移信息y≥D，且：

dist_min(F_i,F_y)＜L

则认为改进的Hector SLAM算法形成了回环；

其中F_i为当前帧(第i帧)所对应的特征向量，y是视觉里程计的输出位移信息，D∈[20m,50m]，是形成回环检测机器人移动的最小距离，L是判断检测到回环时特征向量之间马氏距离的阈值，dist_min(F_i,F_y)是当前帧(第i帧)所对应特征向量与历史数据中特征向量之间的最小值，假设第y帧为特征向量距离最小帧；

步骤5：利用g2o图优化库进行图优化对累积误差进行进一步优化，从而实现机器人在未知环境中的精确自主定位。

2.根据权利要求1所述的一种基于改进Hector SLAM算法的未知环境自主定位方法，其特征在于，检测到回环时特征向量之间的阈值，通过如下方法确定：

⑴通过计算最小距离帧与相邻两帧特征向量之间的距离来确定阈值L，机器人以速度v向前运动，受相邻特征向量之间距离变化率影响，表达式如下：

v₀是机器人的初始运动速度，取初始速度为1m/s,K_v∈[-0.005,0.005]为速度调节系数，f为激光雷达的扫描频率；

⑵假设第y帧，是当前帧(第i帧)与历史帧的特征向量距离最小帧；

在这里分为两种情况：

A1:当前帧(第i帧)，不是与第1帧的距离最近：

阈值：

A2:当前帧(第i帧)，是与第1帧的距离最近：

阈值：

其中k_p∈[0,1]是调节系数。

3.根据权利要求1所述的一种基于改进Hector SLAM算法的未知环境自主定位方法，其特征在于，步骤2中所述的通过运行Lukas-Kanade跟踪器追踪图像中的FAST特征实现视觉里程计输出机器人运动过程中的位移和速度信息，并采用UKF算法融合激光SLAM和视觉里程计的信息以补偿激光SLAM算法在隧道或长廊中纵向位置信息的输出。