CN112230243B - 一种移动机器人室内地图构建方法 - Google Patents

Abstract

本发明公开了一种移动机器人室内地图构建方法，利用环境中的里程计、WiFi设备及激光雷达作为感知单元，实现WiFi指纹数据、里程计数据和激光数据的采集，里程计提供了机器人大致的位姿变化，融合WiFi指纹数据能够提供更精确的位姿，在此基础上结合激光数据进行室内地图构建，是一种融合上述数据进行精确室内环境地图构建的方法；在进行室内地图构建过程中融合WiFi数据，训练“相似度‑距离”模型，进行指纹闭环检测，有效降低里程计的累积误差，可以辅助激光雷达在复杂多变环境下的地图构建，提高地图构建精度与效率本方法可广泛的应用于各种复杂环境中的地图构建，具有较高的普适性。

Description

一种移动机器人室内地图构建方法

技术领域

本发明属于移动机器人室内建图技术领域，具体涉及一种移动机器人室内地图构建方法。

背景技术

近年来，移动机器人技术在工业、医疗和服务等许多领域都发挥了重要的作用，而且在国防和空间探测领域等有害与危险场合也得到了很好的应用。

在移动机器人的研究领域中，SLAM一直是一个热门的研究话题，它为机器人提供导航地图和实时位置，而这些都是机器人执行路径规划、路径跟踪的前提，因此它在移动机器人导航中占有十分重要的位置。

由于激光雷达具有精度高、范围广、传输速度快等优点，激光雷达在移动机器人导航应用中日益广泛，基于激光扫描系统的室内环境地图的构建技术被广泛地应用到机器人的导航中，常用于机器人定位，环境地图的构建以及路径规划。激光距离测量比较准确，误差模型简单，在强光直射以外的环境中运行稳定，点云的处理也比较容易，激光可以通过地图匹配算法实现精确定位，但对环境依赖性较大，传统的激光构建地图需要执行扫描匹配的计算开销很大，尤其是当环境很复杂的时候，激光构建地图在相似或者走廊中的环境往往会失败，因为激光匹配会很难找到好的特征用于识别某一场景，在弱纹理特征少的环境中(例如长廊)匹配精度较差。另外，一般的激光扫描仪价格十分昂贵，虽然目前市面上有很多便宜的激光出现，但是往往测量范围有限，而且分辨率较低。

机器人的里程计往往可以通过光电编码器获取，其所带来的误差，会随着时间的增长越来越大，从而造成机器人出现严重偏差。因此，需要引入外部传感器，通过对周围环境的观测来修正这些误差，提高机器人的地图构建精度。

WiFi定位技术在实时性能、环境适应性等方面具有很大的优势，并且抗干扰能力更强，定位精度高(2m)，低成本、低功耗，数据传输速度快。WiFi可以辅助里程计在复杂多变的环境下的地图构建，解决激光在特征稀疏、对称环境等情况下特征匹配失败的问题，增加回环检测准确率，进而有效的降低累积误差与系统漂移。WiFi通过指纹地图匹配实现设备的快速定位，不受环境纹理影响，但由于信号传播受环境多径效应的影响，定位精度较差，难以用于机器人精确定位与导航。因此，本发明提供了一种融合里程计数据、激光数据和WiFi数据实现精确的室内地图构建。

发明内容

针对现有技术中的上述不足，本发明提供的移动机器人室内地图构建方法解决了现有的机器人室内地图构建过程中对环境依赖大、定位不准确及建图精度不高的问题。

为了达到上述发明目的，本发明采用的技术方案为：一种移动机器人室内地图构建方法，包括以下步骤：

一种移动机器人室内地图构建方法，包括以下步骤：

S1、分别采集移动机器人的里程计数据、指纹信息及激光扫描数据；

S2、基于里程计数据及指纹信息，构建并训练指纹“相似度-距离”模型；

S3、基于训练完成的指纹“相似度-距离”模型，进行“指纹-闭环”检测；

S4、将里程计数据中移动机器人的位姿信息作为顶点，将基于里程计数据得到的边和“指纹-闭环”的边作为约束构建位姿图，并进行位姿图优化；

S5、将优化后的位姿图中的轨迹作为移动机器人的运动轨迹，基于激光扫描数据构建“激光-闭环”的边，并结合构建的“激光-闭环”的边对运动轨迹进行优化；

S6、基于优化后的运动轨迹，通过激光雷达完成室内地图的构建。

进一步地，所述步骤S1中：

通过搭载在移动机器人上的编码器采集移动机器人的里程计数据；

通过手机终端利用WiFi采集任意位置的移动机器人的指纹信息；

通过搭载在移动机器人上的激光雷达采集移动机器人的激光扫描数据。

进一步地，所述步骤S2具体为：

S21、计算移动机器人在任意两个位置的指纹信息的相似度；

S22、基于里程计数据，结合指纹信息的相似度数据，训练数据集并构建指纹“相似度-距离”模型。

进一步地，所述步骤S21中，任意两个位置的指纹信息的相似度的计算公式为：

式中，S_ij为移动机器人t时刻在x_i处的指纹信息F_i和在x_j处的指纹信息F_j的相似度，sim(·)为求解相似度的表达式，每个所述指纹信息包括t时刻扫描到AP的硬件地址以及该位置x_i的信号强度f_t＝{f_i,1,...,f_i,l,...,f_i,L}，下标l为t时刻扫描到AP序数，L为t时刻扫描到的AP总数。

进一步地，所述步骤S22中的指纹“相似度-距离”模型为移动机器人轨迹上任意两个位置点对应的相似度与该两点位置之间距离的关系；

所述步骤S22具体为：

A1、基于任意两个指纹信息的相似度及其对应的里程计数据，构建K个训练数据

其中，s_k即S_ij，为任意两个指纹信息的相似度，d_k为基于里程计数据得到的任意两个指纹信息之间的欧氏距离；

A2、基于训练数据

使用分箱方法对任意两个指纹信息相似度进行训练，计算小于设定间隔b的所有相似度的均值和方差，进而确定指纹“相似度-距离”模型中每个相似度与对应距离关系的不确定性，即得到相似度在(s_k-b,s_k+b)范围内对应的距离关系，完成指纹“相似度-距离”模型的构建。

进一步地，所述步骤S3具体为：

B1、设置指纹信息相似度的阈值；

B2、当指纹“相似度-距离”模型中的相似度小于设定阈值时，检测到形成“指纹-闭环”。

进一步地，所述步骤S4中，进行位姿图优化时的优化公式为：

式中，z_ij为顶点i到j之间的刚体变换，

为顶点i到j之间的刚体变换的预测值，∑_ij(·)为该刚体变换不确定性的协方差矩阵，C为构建的约束集合。

进一步地，所述步骤S5中，构建“激光-闭环”的边的方法具体为：

C1、基于激光扫描数据，设定目标点云Q＝{q₁,q₂,…,q_N}和源点云P＝{p₁,p₂,…,p_N}；

C2、通过最邻接法对目标点云和源点云进行匹配，并将源点云进行旋转平移使目标函数E(R,T)的值最小；

式中，N为点云中的点的数目，R,T分别表示两个点云之间的旋转矩阵和平移矩阵；

C3、当目标函数E(R,T)的值的值小于设定阈值时，检测到“激光-闭环”，即构建出“激光-闭合”的边。

进一步地，述步骤S5中，对移动机器人的运动轨迹进行优化的方法具体为：

将“激光-闭合”的边作为优化后的位姿图的约束，使用图优化SLAM方法来优化位姿图，使“激光-闭合”的边带来的误差最小，进而完成对机器人运动轨迹的优化。

进一步地，所述步骤S6中，通过激光雷达采集到的激光扫描数据构建室内的二维栅格地图；

构建方法具体为：

将激光雷达采集的数据z_t用若干组不同方向的激光束表示，针对每个激光束计算其对应的栅格被占用的概率，进而构建出二维栅格地图；

每个栅格被占用的概率的计算公式为：

式中，l₀为初始值，l_t,ij为t时刻的占用概率的对数值，p(m_ij|z_t,x_t)为反向预测模型函数，m_ij为横坐标为i纵坐标为j的栅格单元，x_t为t时刻的激光测量值，z_t为t时刻移动机器人的位姿。

本发明的有益效果为：

(1)本发明利用环境中的里程计、WiFi设备及激光雷达作为感知单元，实现WiFi指纹数据、里程计数据和激光数据的采集，里程计提供了机器人大致的位姿变化，融合WiFi指纹数据能够提供更精确的位姿，在此基础上结合激光数据进行室内地图构建，是一种融合上述数据进行精确室内环境地图构建的方法；

(2)融合上述各种数据进行室内地图构建，解决了单一采用激光雷达进行建图时，对室内环境依赖程度高，建图精度低的问题；

(3)融合上述各种数据进行室内地图构建，解决单一的使用里程计进行位姿估计出现的误差；

(4)在进行室内地图构建过程中融合WiFi数据，可以辅助里程计在复杂多变环境下的地图构建，提高地图构建精度与效率，通过指纹的相似性来进行回环检测，解决激光在特征稀疏、对称环境等情况下特征匹配失败的问题，增加回环检测准确率，进而有效的降低累积误差与系统漂移。

(5)本发明方法可广泛的应用于各种复杂环境中的地图构建，具有较高的普适性。

附图说明

图1为本发明提供的移动机器人室内地图构建方法流程图。

图2为本发明提供的两种方法的建图效果示意图。

具体实施方式

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

如图1所示，一种移动机器人室内地图构建方法，包括以下步骤：

S1、分别采集移动机器人的里程计数据、指纹信息及激光扫描数据；

S2、基于里程计数据及指纹信息，构建并训练指纹“相似度-距离”模型；

S3、基于训练完成的指纹“相似度-距离”模型，进行“指纹-闭环”检测；

S4、将里程计数据中移动机器人的位姿信息作为顶点，将基于里程计数据得到的边和“指纹-闭环”的边作为约束构建位姿图，并进行位姿图优化；

S5、将优化后的位姿图中的轨迹作为移动机器人的运动轨迹，基于激光扫描数据构建“激光-闭环”的边，并结合构建的“激光-闭环”的边对运动轨迹进行优化；

S6、基于优化后的运动轨迹，通过激光雷达完成室内地图的构建。

在上述步骤S1中，采集各种信息的方法的具体为：

通过搭载在移动机器人上的编码器采集移动机器人的里程计数据；

通过手机终端利用WiFi采集任意位置的移动机器人的指纹信息；

通过搭载在移动机器人上的激光雷达采集移动机器人的激光扫描数据。

单纯的采用激光匹配算法效率较低，在弱纹理环境下(例如走廊)特征点稀疏导致激光雷达定位精度较低，WiFi指纹描述了在环境中的某一位置扫描到的AP以及对应的信号强度，WiFi通过指纹地图匹配实现设备的快速定位，不受环境纹理影响，所以利用WiFi指纹相似度信息可以使误差减少，但当信号传播受环境多径效应的影响时，定位精度只有3-5米，难以用于机器人精确定位与导航，此时激光可以提供更精确的距离信息，因此本发明将上述三种信息结合起来，进行机器人精确定位进而准确构建移动机器人室内地图。

上述步骤S2具体为：

S21、计算移动机器人在任意两个位置的指纹信息的相似度；

S22、基于里程计数据，结合指纹信息的相似度数据，训练数据集并构建指纹“相似度-距离”模型。

在该过程中，雷达通常无法在弱纹理环境中识别足够的特征进行闭环检测，导致地图构建的失败，WiFi指纹描述了在环境中的某一位置扫描到的AP以及对应的信号强度，能够很好的克服环境对无线信号传播的影响，因此指纹定位方法被广泛的应用于室内定位中，通过WiFi信号强度(RSS)估算两个测量点的相对位置关系非常困难，因为RSS本身没有包含任何距离或方向信息。本专利使用指纹相似度来描述两个点之间的位置关系，两个位置的远近程度可以通过指纹的相似度来进行衡量。

其中，步骤S21中，任意两个位置的指纹信息的相似度的计算公式为：

式中，S_ij为移动机器人t时刻在x_i处的指纹信息F_i和在x_j处的指纹信息F_j的相似度，sim(·)为求解相似度的表达式，每个所述指纹信息包括t时刻扫描到AP的硬件地址以及该位置x_i的信号强度f_t＝{f_i,1,...,f_i,l,...,f_i,L}，下标l为t时刻扫描到AP序数，L为t时刻扫描到的AP总数。

步骤S22中的指纹“相似度-距离”模型为移动机器人轨迹上任意两个位置点对应的相似度与该两点位置之间距离的关系。

通过图优化SLAM方法优化位姿图中的每一个边都需要指定其不确定性，因此需要指定指纹约束(指纹-闭环)的不确定性，本方案是利用里程计数据来训练这一不确定性模型，虽然里程计在长时间内会有累计误差，但里程计在短时间内是非常精确的，据实验经验所知一般里程计在较远的范围仍然很精确，因此里程计计算较远距离的指纹相似性。因此，上述步骤S22具体为：

A1、基于任意两个指纹信息的相似度及其对应的里程计数据，构建K个训练数据

其中，s_k即S_ij，为任意两个指纹信息的相似度，d_k为基于里程计数据得到的任意两个指纹信息之间的欧氏距离；

A2、基于训练数据

使用分箱方法对任意两个指纹信息相似度进行训练，计算小于设定间隔b的所有相似度的均值和方差，进而确定指纹“相似度-距离”模型中每个相似度与对应距离关系的不确定性，即得到相似度在(s_k-b,s_k+b)范围内对应的距离关系，完成指纹“相似度-距离”模型的训练；

其中，均值μ(d|s)的计算公式为：

方差νar(d|s)的计算公式为：

式中，b(s,r)为在b间隔内位于s附近的一个小区间r内的样本，c(b(s,r))为样本b(s,r)的个数。

上述步骤S3具体为：

B1、设置指纹信息相似度的阈值；

B2、当指纹“相似度-距离”模型中的相似度小于设定阈值时，检测到形成“指纹-闭环”，从而消除里程计的累积误差。

在上述步骤S4中，进行位姿图优化时，用位姿图表示传感器数据之间的关联，图中的顶点表示位姿，边表示顶点之间的约束，包括里程计中相邻时刻移动机器人的位姿变换约束和基于WiFi扫描周围环境AP的信号强度形成的“指纹-闭环”的边，基于位姿图的SLAM的目的找到位姿的最佳配置，从而实现位姿图的优化，进行位姿图优化时的优化公式为：

式中，z_ij为顶点i到j之间的刚体变换，

为顶点i到j之间的刚体变换的预测值，∑_ij(·)为该刚体变换不确定性的协方差矩阵，C为构建的约束集合。

上述所述步骤S5中，构建“激光-闭环”的边的方法具体为：

B1、基于激光扫描数据，设定目标点云Q＝{q₁,q₂,…,q_N}和源点云P＝{p₁,p₂,…,p_N}；

B2、通过最邻接法对目标点云和源点云进行匹配，并将源点云进行旋转平移使目标函数E(R,T)的值最小；

式中，N为点云中的点的数目，R,T分别表示两个点云之间的旋转矩阵和平移矩阵；

将此R,T代入源点云P得到一组新的点集M，再将M点集与目标点云Q匹配求解新的旋转平移矩阵，多次迭代后得E(R,T)趋于收敛，得到此时的源点云和目标点云的位置变换关系，此时E(R,T)即为两个点云闭环检测的得到分值；

B3、当目标函数E(R,T)的值的值小于设定阈值时，检测到“激光-闭环”，即构建出“激光-闭合”的边；

上述步骤S5中，对移动机器人的运动轨迹进行优化的方法具体为：

将“激光-闭合”的边作为优化后的位姿图的约束，使用图优化SLAM方法来优化位姿图，使“激光-闭合”的边带来的误差最小，进而完成对机器人运动轨迹的优化。

上述步骤S6中，通过激光雷达采集到的激光扫描数据构建室内的二维栅格地图；

构建方法具体为：

将激光雷达采集的数据z_t用若干组不同方向的激光束表示，针对每个激光束计算其对应的栅格被占用的概率，进而构建出二维栅格地图；

每个栅格被占用的概率的计算公式为：

式中，l₀为初始值，l_t,ij为t时刻的占用概率的对数值，p(m_ij|z_t,x_t)为反向预测模型函数，m_ij为横坐标为i纵坐标为j的栅格单元，x_t为t时刻的激光测量值，z_t为t时刻移动机器人的位姿。

在本发明实施例的地图构建过程中，常常使用里程计数据估计机器人位姿，而里程计数据来源于对速度的不断积分，这就导致不可避免的存在累积误差，从而影响建图精度，所以发明引入了“指纹-闭环”纠正累积误差，优化机器人的位姿估计，并且在“指纹-闭环”的基础上，利用激光数据进行闭环检测，构建“激光-闭环”边，从而构建更高精度的地图，图优化分为两层进行，第一层，基于里程计短时间的运动距离信息，构建指纹“相似度-距离”数理模型，在WiFi SLAM中，设定指纹相似度阈值，实现“指纹-闭环”检测；采用g2o图优化算法与指纹“相似度-距离”模型，完成机器人轨迹的初步优化。第二层，提高了从第一层所得轨迹的精度，利用激光雷达数据进行扫描匹配，构建“激光-闭环”边，可以得到精确的位姿估计，虽然这需要大量的计算，但第一步产生的轨迹减少了激光扫描所需的扫描次数，从而在保持精度的同时提高了计算效率。

如图2所示，分别列出了只采用WiFi的建图效果(a图)和采用WiFi和激光雷达的建图效果(b图)对比示意图，通过对比可以看出里程计提供了机器人大致的位姿变化，随着时间的累积，误差也会累积。融合WiFi信息能够提供更精确的位姿。对激光进行闭环检测，可以提供更精确和全面的周围环境扫描信息。所以，融合里程计数据、激光数据、WiFi数据可以实现精确的室内地图构建。

Claims (8)

1.一种移动机器人室内地图构建方法，其特征在于，包括以下步骤：

S1、分别采集移动机器人的里程计数据、指纹信息及激光扫描数据；

S2、基于里程计数据及指纹信息，构建并训练指纹“相似度-距离”模型；

S3、基于训练完成的指纹“相似度-距离”模型，进行“指纹-闭环”检测；

S4、将里程计数据中移动机器人的位姿信息作为顶点，将基于里程计数据得到的边和“指纹-闭环”的边作为约束构建位姿图，并进行位姿图优化；

S5、将优化后的位姿图中的轨迹作为移动机器人的运动轨迹，基于激光扫描数据构建“激光-闭环”的边，并结合构建的“激光-闭环”的边对运动轨迹进行优化；

S6、基于优化后的运动轨迹，通过激光雷达完成室内地图的构建；

所述步骤S2中的指纹“相似度-距离”模型为移动机器人轨迹上任意两个位置点对应的相似度与该两点位置之间距离的关系；

所述步骤S2中构建并训练指纹“相似度-距离”模型的方法为：

A1、基于任意两个指纹信息的相似度及其对应的里程计数据，构建K个训练数据

其中，s_k即S_ij，为任意两个指纹信息的相似度，d_k为基于里程计数据得到的任意两个指纹信息之间的欧氏距离；

A2、基于训练数据

使用分箱方法对任意两个指纹信息相似度进行训练，计算小于设定间隔b的所有相似度的均值和方差，进而确定指纹“相似度-距离”模型中每个相似度与对应距离关系的不确定性，即得到相似度在(s_k-b,s_k+b)范围内对应的距离关系，完成指纹“相似度-距离”模型的构建；

所述步骤S3具体为：

B1、设置指纹信息相似度的阈值；

B2、当指纹“相似度-距离”模型中的相似度小于设定阈值时，检测到形成“指纹-闭环”。

2.根据权利要求1所述的移动机器人室内地图构建方法，其特征在于，所述步骤S1中：

通过搭载在移动机器人上的编码器采集移动机器人的里程计数据；

通过手机终端利用WiFi采集任意位置的移动机器人的指纹信息；

通过搭载在移动机器人上的激光雷达采集移动机器人的激光扫描数据。

3.根据权利要求2所述的移动机器人室内地图构建方法，其特征在于，所述步骤S2具体为：

S21、计算移动机器人在任意两个位置的指纹信息的相似度；

S22、基于里程计数据，结合指纹信息的相似度数据，训练数据集并构建指纹“相似度-距离”模型。

4.根据权利要求3所述的移动机器人室内地图构建方法，其特征在于，所述步骤S21中，任意两个位置的指纹信息的相似度的计算公式为：

式中，S_ij为移动机器人t时刻在x_i处的指纹信息F_i和在x_j处的指纹信息F_j的相似度，sim(·)为求解相似度的表达式，每个所述指纹信息包括t时刻扫描到AP的硬件地址以及该位置x_i的信号强度f_t＝{f_i,1,...,f_i,L}，l为t时刻扫描到的AP序数，L为t时刻扫描到的AP总数。

5.根据权利要求1所述的移动机器人室内地图构建方法，其特征在于，所述步骤S4中，进行位姿图优化时的优化公式为：

式中，z_ij为顶点i到j之间的刚体变换，

为位置x_i处的顶点i到位置x_j处的顶点j之间的刚体变换的预测值，∑_ij(·)为该刚体变换不确定性的协方差矩阵，C为构建的约束集合。

6.根据权利要求1所述的移动机器人室内地图构建方法，其特征在于，所述步骤S5中，构建“激光-闭环”的边的方法具体为：

C1、基于激光扫描数据，设定目标点云Q＝{q₁,q₂,…,q_N}和源点云P＝{p₁,p₂,…,p_N}；

C2、通过最邻接法对目标点云和源点云进行匹配，并将源点云进行旋转平移使目标函数E(R,T)的值最小；

式中，N为点云中的点的数目，R,T分别表示两个点云之间的旋转矩阵和平移矩阵；

C3、当目标函数E(R,T)的值的值小于设定阈值时，检测到“激光-闭环”，即构建出“激光-闭合”的边。

7.根据权利要求6所述的移动机器人室内地图构建方法，其特征在于，所述步骤S5中，对移动机器人的运动轨迹进行优化的方法具体为：

将“激光-闭合”的边作为优化后的位姿图的约束，使用图优化SLAM方法来优化位姿图，使“激光-闭合”的边带来的误差最小，进而完成对机器人运动轨迹的优化。

8.根据权利要求7所述的移动机器人室内地图构建方法，其特征在于，所述步骤S6中，通过激光雷达采集到的激光扫描数据构建室内的二维栅格地图；

构建方法具体为：

将激光雷达采集的数据z_t用若干组不同方向的激光束表示，针对每个激光束计算其对应的栅格被占用的概率，进而构建出二维栅格地图；

每个栅格被占用的概率的计算公式为：

式中，l₀为初始值，l_t,ij为t时刻的占用概率的对数值，p(m_ij|z_t,x_t)为反向预测模型函数，m_ij为横坐标为i纵坐标为j的栅格单元，x_t为t时刻的激光测量值，z_t为t时刻移动机器人的位姿。

Images