一种室内环境地图创建的闭环校正方法

摘要

本发明提供了一种室内环境地图创建的闭环校正方法，包括：步骤1获取环境信息，标记机器人的初始位姿信息；步骤2机器人行走距离超过规定范围后记录机器人的实时位姿信息，采用Gmapping算法根据初始位姿信息和实时位姿信息生成环境地图；步骤3检测环境地图是否出现闭环，若是，则继续执行以下步骤，若不是，则重复执行步骤2；步骤4采用状态估计算法重新估计机器人的实时位姿信息，并更新实时位姿信息生成校正地图。本发明提供了一种基于低成本、低精度激光传感器的地图创建中的闭环校正方法，一方面，提高地图创建的精度；另一方面，推进低成本激光传感器在日常生活及科学研究中的应用。

说明书

技术领域

本发明属于移动机器人导航领域，尤其涉及一种基于低成本、低精度激光 传感器数据实现地图创建及闭环校正的方法。

背景技术

随着技术的发展进步，移动机器人逐渐走出科学研究的神坛，越来越广泛 的出现在日常生活的各个方面，辅助或替代人类完成一些重复性较高、技术含 量较低的任务，如产品装配、扫地等，扫地机是移动机器人的典型应用。

目前，机器人主要通过声呐、激光或视觉传感器感知并建立外部环境的模 型，实现与外部世界的“交流”。由于扩散角较大，导致声呐传感器获取的环 境数据存在显著误差；视觉传感器一方面对硬件有较高要求(存储图片需要较 大空间，处理图片需要较高的计算能力)，另一方面，其对光线变化敏感，当 同一场景的光线发生变化时，可能引入额外的误差。相对而言，激光传感器的 精度高于声呐，对硬件的要求低于视觉传感器，因此在机器人导航中获得了广 泛应用。但是，精度越高，测量范围越大，激光传感器的成本越高，这一缺陷 严重的限制了激光传感器在日常生活及科学研究中的推广使用。因此，寻求低 成本，精度可接受的替代激光传感器，并发明新的算法对低精度带来的误差加 以补偿，成为一种合理并可行的道路。

图1为利用SICK LMS激光传感器建立的环境地图。图2为利用RPLidar 激光传感器建立的环境地图。对比图1和图2建立的环境地图，发现图2利用 RPLidar数据建立的环境地图存在闭环失败的情况，而图1利用SICK LMS激光 数据建立的环境地图精度高，闭环效果良好。但高精度的激光传感器SICK LMS 存在高成本的问题；而成本低的激光传感器RPLidar存在精度相对较、低闭环 失败的问题。

发明内容

为此，本发明提供的室内环境地图创建的闭环校正方法，采用基于卡尔曼 滤波器的状态估计算法，对Gmapping算法获取的机器人位姿进行校正，并基 于校正后的机器人位姿，重新建立环境地图获得校正地图。反复的实验结果表 明，本发明提供的室内环境地图创建的闭环校正方法能够显著改善基于 RPLidar数据建立地图的闭环效果。从长远来说，能够推进低成本、低精度激 光传感器在日常生活及科学研究中的广泛使用。

本发明的技术方案以如下方式实现：

一种室内环境地图创建的闭环校正方法，包括：

步骤1获取环境信息，标记机器人的初始位姿信息；

步骤2机器人行走距离超过规定范围后记录机器人的实时位姿信息，采用 Gmapping算法根据所述初始位姿信息和所述实时位姿信息生成环境地图；

步骤3检测所述环境地图是否出现闭环，若是，则继续执行以下步骤，若 不是，则重复执行步骤2；

步骤4采用状态估计算法重新估计机器人的所述实时位姿信息，并更新所 述实时位姿信息生成校正地图。

本发明利用传统的Gmapping算法处理成本较低、精度较低的传感器采集 的环境数据，建立环境地图，再通过利用基于卡尔曼滤波器的状态估计算法， 对Gmapping算法估计所得的机器人位姿进行校正，根据校正后的机器人位姿， 重新建立环境地图得到校正地图，有效改善环境地图的闭环效果。

进一步优选的，所述环境数据由里程计和激光传感器获取。

进一步优选的，所述激光传感器为RPLidar激光雷达。

本发明采用低精度的RPLidar激光雷达，并通过状态估计算法对低精度带 来的误差加以补偿，能够推进低成本、低精度激光传感器在日常生活及科学研 究中的广泛使用。

进一步优选的，所述闭环检测通过场景标定实现。

进一步优选的，步骤2还包括记录多个机器人的所述实时位姿信息生成机 器人的状态向量。

进一步优选的，所述步骤4采用状态估计算法进行闭环校正，具体包括：

首先通过状态转移矩阵计算上一时刻的状态向量和输入对当前时刻状态 的影响，得到当前时刻的状态预测值；以及通过状态转移矩和上一时刻的协方 差矩阵值以及状态噪声协方差矩阵得到协方差矩阵的预测值；

再通过状态方程和观测方程实现观测向量的更新，根据实际观测值和当前 时刻的状态预测值计算新息；所述状态转移矩阵当前时刻的状态预测值计算新 息；以及，通过状态转移矩阵计算协方差矩阵预测值和观测误差的协方差矩阵， 得到新息的协方差矩阵；

通过状态转移矩阵计算当前时刻的协方差矩阵预测值，并计算新息的协方 差矩阵的逆矩阵，得到滤波器增益；通过上一时刻的状态向量结合滤波器增益 和新息计算出当前时刻的状态状态向量；以及根据上一时刻的协方差矩阵值和 增益计算出当前时刻的协方差矩阵值；

更新所述状态向量和所述协方差矩阵；根据更新所得的所述状态向量生成 所述校正地图。

本发明采用的状态估计算法能够显著改善基于RPLidar激光传感器获取的 环境数据建立地图的闭环效果。

进一步优选的，所述Gmapping算法和所述状态估计算法在安装有ROS的 Linux系统下并发执行。

本发明提供了一种基于低成本、精度相对较低的激光传感器的室内环境地 图创建的闭环校正方法，通过对比分析不同精度及成本激光传感器创建地图的 精度及闭环效果，给出利用低成本、精度可接受的激光传感器实现地图创建的 必要性及可行性，然后提出一种基于卡尔曼滤波器的状态估计算法，对利用低 成本、低精度激光传感器数据创建地图过程中机器人的位姿进行校正，并基于 校正后的机器人的位姿重新创建地图，实现了地图的闭环，提高了地图创建的 精度，同时推进低成本激光传感器在日常生活及科学研究中的应用。

附图说明

下面结合附图和具体实施方式对本发明作进一步详细说明：

图1为利用SICK LMS激光传感器创建的环境地图；

图2为利用RPLidar激光雷达建立的环境地图；

图3为本发明室内环境地图创建的闭环校正方法的步骤示意图；

图4为本发明室内环境地图创建的闭环校正方法的Gmapping算法流程图；

图5为本发明室内环境地图创建的闭环校正方法更新的校正地图；

图6为本发明室内环境地图创建的闭环校正方法最终的校正地图。

具体实施方式

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施 例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述 中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来说，在不付 出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图3为本发明室内环境地图创建的闭环校正方法的步骤示意图，作为本发 明的一个具体实施例，如图3所示，本发明提供了一种室内环境地图创建的闭 环校正方法，包括：

步骤1获取环境信息，标记机器人的初始位姿信息；

步骤2机器人行走距离超过规定范围后记录机器人的实时位姿信息，采用 Gmapping算法根据所述初始位姿信息和所述实时位姿信息生成环境地图；

步骤3检测所述环境地图是否出现闭环，若是，则继续执行以下步骤，若 不是，则重复执行步骤2；

步骤4采用状态估计算法重新估计机器人的所述实时位姿信息，并更新所 述实时位姿信息生成校正地图。

具体的，本发明中环境数据由里程计和激光传感器获取，其中，激光传感 器选用低成本、低精度的RPLidar激光雷达，通过RPLidar激光雷达获取环境 信息，RPLidar激光雷达的测量长度范围为0.2-6.0米，角度范围为0-360度， 采样频率超过2000Hz。本发明采用低精度的RPLidar激光雷达，并通过状态估 计算法对低精度带来的误差加以补偿，能够推进低成本、低精度激光传感器在 日常生活及科学研究中的广泛使用。

具体的，本发明利用传统Gmapping算法处理低精度激光数据，建立环境 地图，再利用基于卡尔曼滤波器的状态估计算法，对Gmapping算法估计所得 的机器人位姿进行校正，最后根据校正后的机器人位姿，重新建立环境地图， 改善地图的闭环效果。

下面以具体实例阐述本发明的工作过程。

本实施例利用机器人在有走廊、办公室及大厅的环境中实现地图创建及闭 环校正，该机器人装备有里程计和陀螺仪获取机器人的距离和方向信息。在机 器人上装配了RPLidar激光雷达创建环境地图。在室内环境中，利用Visual  Studio 2010在Linux系统下编程，并通过ROS控制机器人实现环境地图创建 及闭环，图4为本发明室内环境地图创建的闭环校正方法的Gmapping算法流 程图。

本发明具体执行步骤如下：

步骤1，控制机器人在目标环境中行走，获取里程计和激光传感器的环境 信息，标记机器人的初始位姿信息，记为p₀(x₀，y₀，θ₀)。其中，x表示横坐标、y 表示纵坐标，θ表示朝向角。本发明中可以选择人为控制或根据环境特点，设 计合理的机器人环境探索算法。

步骤2，假设机器人i时刻的驱动输入为u_i＝(0，0，0，...，Δx_i，Δy_i，Δθ_i，0，0，0，...)，当 满足时，Δ表示变化量。机器人记录一次激光传感器和里程计 的环境信息，并记录机器人实时位姿信息，采用Gmapping算法根据初始位姿 信息和实时位姿信息生成环境地图。

需要说明的是，对于传统的Gmapping算法，当机器人行走距离大于一定 范围d，或朝向角变化大于一定角度θ，或超过一定时间t为处理激光传感器或 里程计数据后，机器人将激光传感器和里程计数据融合入环境地图中，也即在 满足上述条件后，机器人会记录一次里程计或激光传感器获取的环境信息。而 在本发明中，为避免引入额外的误差，同时便于实现机器人的控制，机器人只 有在行走距离超过一定范围Δ后才对激光传感器和里程计的环境信息进行处 理。

步骤3，实时检测是否有闭环出现，若未出现闭环，则重复步骤2；若出 现闭环，记录机器人的状态向量。假设机器人第一次回到p₀(x₀，y₀，θ₀)时，共记 录了k个状态，形成状态向量，记为x_k＝{x₀，y₀，θ₀，...，x_k，y_k，θ_k}＝{p₀，...，p_k}；

需要说明的是，闭环检测(loop detection)目前仍然是一个开放性的研 究课题，目前并没有完美的解决解决方案。另外，闭环检测也不是本发明的核 心内容，因此在本发明中，闭环检测通过场景标定实现。

步骤4，实现Gmapping算法记录的机器人状态的校正。本发明中，Gmapping 算法和状态估计算法在安装有ROS系统的Linux系统下并发执行，在机器人回 到p₀(x₀，y₀，θ₀)后，Gmapping算法仍然使用粒子滤波器实现地图信息的更新和机 器人位姿的估计，按照原算法继续执行，本发明采用的状态估计算法则对 Gmapping算法估计所得的机器人状态进行重新估计。

记机器人的状态方程和观测方程分别为：

x_k＝I_k+1x_k-1+I_k+1u_k+w_k-1，  (1)

z_k＝I_k+1x_k+v_k，  (2)

其中，x_k为机器人的状态向量，z_k为观测向量，u_k为系统输入，w和v分 别为机器人的状态和观测噪声(系统噪声向量和观测噪声向量，向量中每个维 度的噪声一般可认为服从高斯分布)，可假设为高斯白噪声，I_k+1为k+1维的单 位矩阵。由于本发明中直接对状态向量，因此可以认为x_k和z_k存在等价关系。

当机器人再次返回某个状态p_i后(可假设其观测到的该状态为)，则基于 卡尔曼滤波器的状态估计算法进行闭环校正，具体包括：

设当前时刻表示为k，上一时刻表示为k-1。

(一)通过状态转移矩阵计算上一时刻的状态向量和输入对当前时刻状态 的影响，得到当前时刻的状态预测值；以及通过状态转移矩和上一时刻的协方 差矩阵值以及状态噪声协方差矩阵得到协方差矩阵的预测值。

具体分为两步：

1.计算状态预测值

$\hat{x}\left(k\right|k-1)=I_{k+1}\hat{x}(k-1|k-1)+I_{k+1}u(k-1);$

其中，为k时刻的状态预测值，为k-1时刻的状态 值，u(k-1)为k-1时刻的输入，I_k+1为状态转移矩阵。本质上是 是矩阵的乘法，指k-1时刻状态向k时刻的转移，I_k+1u(k-1)同 理。

2.计算协方差矩阵的预测值

P(k|k-1)＝I_k+1P(k-1|k-1)I_k+1+Q(k)；

其中，P(k|k-1)为协方差矩阵预测值；

根据k-1时刻的值预测k时刻的值。I_k+1P(k-1|k-1)I_k+1表示的是 I_k+1.P(k-1|k-1).I_k+1，即矩阵的乘法。在状态转移的过程中要考虑误差的影响， Q(k)是状态噪声协方差矩阵。

(二)通过状态方程和观测方程实现观测向量的更新，根据实际观测值和 当前时刻的状态预测值计算新息；所述状态转移矩阵当前时刻的状态预测值计 算新息；以及，通过状态转移矩阵计算协方差矩阵预测值和观测误差的协方差 矩阵，得到新息的协方差矩阵。

具体包括以下三步：

1.通过状态方程(1)和观测方程(2)获取z_k＝{...，p_i，...}并将其重新 设置为$z_k=\{...,{\tilde{p}}_i,...\};$

2.通过状态预测计算新息

$\tilde{y}\left(k\right)=z\left(k\right)-\hat{x}\left(k\right|k-1);$

其中，是没有噪声影响时的状态预测值，z(k)是实际的观测值， 二者的差就为新息，新息为新的有指导意义的信息，详情可参考经典卡尔曼滤 波器。

3.实现协方差矩阵的处理

S(k)＝I_k+1P(k|k-1)I_k+1+R(k)；

其中，S(k)是新息的协方差矩阵；

在公式(2)中，定义了观测噪声v，观测噪声为传感器本身带有一定 的误差引入的，这个在任何领域均不可避免。R(k)就是观测噪声的协方差矩 阵。

I_k+1P(k|k-1)I_k+1中的P(k|k-1)已在步骤2中计算获得，通过矩阵乘法直 接计算即得到新息的协方差矩阵。

(三)通过状态转移矩阵计算当前时刻的协方差矩阵预测值，并计算新息 的协方差矩阵的逆矩阵，得到滤波器增益；通过上一时刻的状态向量结合滤波 器增益和新息计算出当前时刻的状态状态向量；以及根据上一时刻的协方差矩 阵值和增益计算出当前时刻的协方差矩阵值。

具体包括以下三步：

1.计算卡尔曼滤波器增益

K(k)＝P(k|k-1)I_k+1S(k)^-1；

其中P(k|k-1)为当前时刻的协方差矩阵预测值；

S(k)是新息的协方差矩阵；

首先计算P(k|k-1)I_k+1，等价于P(k|k-1).I_k+1(矩阵乘法)。然后计算S(k) 的逆矩阵S(k)^-1，最后得到滤波器增益K(k)。

2.实现状态向量的更新

$x\left(k\right|k)=\hat{x}\left(k\right|k-1)+K\left(k\right)\tilde{y}\left(k\right);$

其中x(k|k)为k时刻的状态向量；

为k时刻的状态预测值；

通过矩阵乘法计算滤波器增益K(k)和新息再与做加法计 算得到k时刻的状态向量。

3.实现协方差矩阵的更新

P(k|k)＝(I-K(k)I_k+1)P(k|k-1)；

其中P(k|k)为k时刻的协方差矩阵；

I是与K(k)I_k+1维数相同的单位矩阵；

通过单位矩阵计算K(k)和P(k|k-1)即得到协方差矩阵的更新。

(四)生成新的地图，具体包括以下一步：

在得到k时刻的状态向量x(k|k)和k时刻的协方差矩阵P(k|k)后算法就 可以重新迭代了。根据上述步骤更新所得的机器人状态向量，重新记录里程计 和激光传感器获取的环境信息，生成新的环境地图，即为校正地图。

机器人再次返回{p₀，...，p_k}后，在已标定过的点p_i，i＝1，...，k处，依然按照 Gmapping算法记录里程计和激光传感器的环境信息，实现地图的进一步更新， 得到更新的校正地图，图5为本发明室内环境地图创建的闭环校正方法更新的 校正地图。

机器人再次返回{p₀，...，p_k}后，分别执行步骤4中基于卡尔曼滤波器的状态 估计算法进行闭环校正，得到最终的校正地图。图6为本发明室内环境地图创 建的闭环校正方法最终的校正地图。

需要说明的是，机器人首次在环境中行走时，在满足的条 件下，会记录相应的里程计和激光传感器的环境信息，这些记录的点记为机器 人的实时位姿信息，记为{p₀，...，p_k}。所以，当机器人再次进入环境中时，一般 是在点{p₀，...，p_k}处或其周围的满足一定误差条件的区域内(由于当时机器人是 人为控制的，因此这个条件是能够满足的)进行里程计和激光传感器采集的环 境信息的记录。

本发明提供了一种基于低成本、精度相对较低的激光传感器的室内环境地 图创建的闭环校正方法。首先，通过对比分析不同精度及成本激光传感器创建 地图的精度及闭环效果，给出利用低成本、精度可接受的激光传感器实现地图 创建的必要性及可行性；然后提出一种基于卡尔曼滤波器的状态估计算法，对 利用低成本、低精度的激光传感器数据创建地图过程中机器人的位姿进行校 正，并基于校正后的机器人的位姿重新创建地图，实现了地图的闭环，提高了 地图创建的精度。本发明提供了一种基于低成本、低精度激光传感器的地图创 建中的闭环校正方法，一方面，提高地图创建的精度；另一方面，推进低成本 激光传感器在日常生活及科学研究中的应用。

显然，本领域的技术人员可以对本发明进行各种改动和变形，而不脱离本 发明的精神和范围。这样，倘若本发明的这些修改属于本发明权利要求及其同 等技术的范围之内，则本发明也意图包含这些改动和变形在内。