CN111207753A - 一种多玻璃隔断环境下的同时定位与建图的方法 - Google Patents

Abstract

本发明实施例公开了一种多玻璃隔断环境下的同时定位与建图的方法。本发明实施例采用多种位置数据进行融合处理形成基础数据、再利用基础数据进行扫描建图并对新建地图进行回环优化，从而有效地识别玻璃，使得所建地图准确而全面，有利于移动机器人的后续路径规划等自主导航功能的实现。

Description

一种多玻璃隔断环境下的同时定位与建图的方法

技术领域

本发明涉及移动机器人的技术领域，特别是涉及一种多玻璃隔断环境下的同时定位与建图的方法。

背景技术

同步定位与建图(Simultaneous Localization and Mapping,简称SLAM)方法是移动机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和周边环境进行自身定位，同时在自身定位的基础上建造增量式地图，最终能实现移动机器人的自主定位和导航。

目前，现有SLAM方法只使用激光雷达或摄像头等光学传感器感知环境，难以准确识别周围透明障碍物，如透明玻璃或透明塑料物等。现今很多建筑采用玻璃隔断方式隔离空间(例如玻璃门、幕墙等)，由于现有SLAM方法无法识别透明障碍物，因此现有SLAM方法建立的地图是残缺的，会给移动机器人后续路径规划等自主导航功能带来困难。

因此，针对上述技术问题，有必要提供一种多玻璃隔断环境下的同时定位与建图的方法。

发明内容

有鉴于此，针对现有技术中的SLAM方法无法识别透明障碍物使得构建的地图具有缺陷的问题，本发明实施例采用多种位置数据进行融合处理形成基础数据、再利用基础数据进行扫描建图并对新建地图进行回环优化，从而有效地识别透明障碍物，使得所建地图准确而全面，有利于移动机器人的后续路径规划等自主导航功能的实现。

为了实现上述目的，本发明一实施例提供的技术方案如下：一种多玻璃隔断环境下的同时定位与建图的方法，包括步骤S1：采用多种位置信息传感器采集位置数据，并将所述位置数据进行处理而获得建图的基础数据；S2：将预设帧数量的基础数据组建一张概率栅格地图；将再接收到新一帧的基础数据与所述概率栅格地图进行匹配，并由位姿解算器提供一个位姿初值而获得所述新一帧的基础数据相对于概率栅格地图的最优位姿；将所述最优位姿输入运动比较器，判断此次位姿变化大小并根据判断结果来确定是否更新所述概率栅格地图；S3：将步骤S2处理后的概率栅格地图和步骤S2中的新一帧的基础数据进行回环检测，并由位姿解算器提供一个位姿初值来判断所述概率栅格地图与所述新一帧的基础数据之间是否存在点云匹配超过预设匹配度的部分，根据判断结果来优化位姿。

作为本发明的进一步改进，所述多种位置信息传感器包括超声波测距传感器、激光雷达传感器和位姿传感器。

作为本发明的进一步改进，将步骤S2中所获得的最优位姿与所述位姿传感器所获得数据同时作为所述位姿解算器的输入量。

作为本发明的进一步改进，所述位置数据进行处理包括数据预处理以及对所述数据预处理后获得的数据进行融合处理。

作为本发明的进一步改进，所述超声波测距传感器所获得的数据进行预处理为将所述数据进行格式转换。

作为本发明的进一步改进，所述激光雷达传感器所获得的数据进行预处理为将所述数据进行运动畸变修正和滤波去噪。

作为本发明的进一步改进，所述位姿传感器所获得的数据进行预处理为将所述数据进行数据校正和姿态融合。

作为本发明的进一步改进，步骤S2包括若此次位姿变化大小处于预设范围，则忽略所述新一帧的基础数据；若此次位姿变化大小超过预设范围，则将所述新一帧的基础数据***所述概率栅格地图以更新所述概率栅格地图。

作为本发明的进一步改进，若将所述新一帧的基础数据***时，融合点云的数据数量等于或大于设定的数量阈值，则组建一张新的概率栅格地图。

作为本发明的进一步改进，步骤S3包括若所述概率栅格地图与所述新一帧的基础数据之间存在点云匹配超过预设匹配度的部分，则产生回环约束并得到所述新一帧的基础数据相对所述概率栅格地图的位姿。

本发明具有以下优点：

本发明实施例采用多种位置数据进行融合处理形成基础数据、再利用基础数据进行扫描建图并对新建地图进行回环优化，从而有效地识别透明障碍物，使得所建地图准确而全面，有利于移动机器人的后续路径规划等自主导航功能的实现。进一步地，本发明实施例提供的同时定位与建图方法无需车轮里程计数据，可应用于多种(如轮式、履带式、足式)室内移动机器人，通用性强。

附图说明

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

图1为本发明实施例提供的一种多玻璃隔断环境下的同时定位与建图方法的流程示意图；

图2为图1所示实施例的另一种表达方式示意图；

图3为超声波测距数据的数据转换模型示意图；

图4为实验环境的走廊全景示意图；

图5为图4所示实验环境的走廊近景示意图；

图6为采用现有SLAM方法对图4所示实验环境建立的地图示意图；

图7为采用本发明实施例提出的SLAM方法对图4所示实验环境建立的地图示意图。

具体实施方式

为了使本技术领域的人员更好地理解本发明中的技术方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

参考图1所示，本发明实施例提供的一种多玻璃隔断环境下的同时定位与建图方法的流程示意图。结合图1和图2所示，本发明实施例提供的同时定位与建图方法大致包括三个步骤，每个步骤的具体内容如下详细描述。

步骤S1：采用多种位置信息传感器采集位置数据，并将所述位置数据进行处理而获得建图的基础数据。如图2所示，该步骤可以简称为数据处理。多种位置信息传感器包括超声波测距传感器、激光测距传感器和位姿传感器。多个超声波测距传感器分别分布在移动机器人(如六棱柱形)每个侧面，可以得到量程范围内与前方障碍物之间的距离。激光测距传感器可以获得移动机器人所处位置周围环境(360°)的距离信息(即轮廓)。位姿传感器固定在移动机器人的中央，可以高频率输出移动机器人自身三轴加速度、角速度、磁场强度以及气压高度信息。多种位置传感器全方位地提供移动机器人所处的环境及位置信息，避免单一种类传感器信息不全而无法提供有效分辨透明障碍物数据的问题。

位置数据进行处理包括数据预处理以及对所述数据预处理后获得的数据进行融合处理。不同种类传感器所获得的数据的数据预处理具体步骤为：超声波测距传感器所获得的数据进行预处理为将所述数据进行格式转换；激光传感器所获得的数据进行预处理为将所述数据进行运动畸变修正和滤波去噪；位姿传感器所获得的数据进行预处理为将所述数据进行数据校正和姿态融合。

超声波测距传感器所获得的数据(简称超声波测距数据)进行转换，根据每个超声波测距传感器安装的位置把得到的距离值转换成带角度的离散点距离值，保持与激光传感器所获得的数据格式一致，以用于后续点云融合。如图3所示，超声波测距数据的数据转换模型是根据所使用的超声波测距探头标定所得，具体计算表达式如公式1所示：

其中，

是超声波测距数据转换后θ角度对应的距离值；d^u是超声波测距数据原始值；θ对应数据转换时每个点相对中心线偏角。

激光传感器所获得的数据(通常为一帧扫描点云数据，简称激光测距数据)进行(低帧率单线扫描测距激光雷达必须)运动畸变修正和滤波。其中，滤波包括边界滤波和体素滤波，边界滤波主要去除超出量程的点云数据和噪点数据；体素滤波对数据进行采样，得到一帧数据的不同分辨率点云样本。

将超声波测距数据与激光测距数据进行点云融合的计算公式，如公式2所示：

其中，

是超声波测距数据转换后θ角度对应的距离值；

是激光测距数据修正滤波后θ角度对应的距离值；d_θ代表融合后点云中θ角度对应的距离值。

位姿传感器所获得的数据(简称姿态数据)的预处理，主要进行数据校正和姿态融合，用于上述激光测距数据的运动畸变修正以及后续位姿解算器的位姿解算。

S2：将预设帧数量的基础数据组建一张概率栅格地图(简称子图)；将再接收到新一帧的基础数据与所述概率栅格地图进行匹配，并由位姿解算器提供一个位姿初值而获得所述新一帧的基础数据相对于概率栅格地图的最优位姿；将所述最优位姿输入运动比较器，判断此次位姿变化大小并根据判断结果来确定是否更新所述概率栅格地图。预设帧数量的范围为10帧至50帧，具体帧数根据建图面积，传感器性能等参数调整。如图2所示，该步骤可以简称为扫描建图。

优选地，该步骤中所获得的最优位姿与位姿传感器所获得数据同时作为位姿解算器的输入量，通过闭环反馈可以得到一个更合适的位姿初值。

该步骤包括若此次位姿变化大小处于预设范围，则忽略所述新一帧的基础数据；若此次位姿变化大小超过预设范围，则将所述新一帧的基础数据***所述概率栅格地图以更新所述概率栅格地图。位姿变化包括位移变化和转角变化。预设范围为位移变化小于等于0.2m，转角变化小于等于1rad。根据建图精度，位姿变化大小的具体预设范围值可以进行微调。若将所述新一帧的基础数据***时，融合点云的数据数量等于或大于设定的数量阈值，则组建一张新的概率栅格地图。数量阈值与预设帧数量的范围一致，数量阈值为10帧至50帧。

该步骤中扫描匹配步骤具体采用的是非线性最小二乘法，具体计算表达式如公式3所示：

其中，p(x，y，θ)为该帧融合点云相对于当前子图的位姿；D_k＝{d_k}_{k＝1，…，K}为融合点云；T_p为坐标转换，将点云位置转换到对应子图坐标系中，转换公式如公式4所示：

M_smooth为栅格概率值平滑函数，使用双三次插值法提高精度，具体表达式如公式5所示：

其中，W(x)为双三次插值基函数；M(x，y)为当前子图中坐标(x，y)处对应栅格的概率值。

在该步骤中，采用占据栅格地图来表示子图，概率值的计算公式如公式6所示：

M(x，y)＝clamp{S^-1[S(M_old(x，y))+S(p)]} (公式6)

其中，M_old(x，y)，M(x，y)分别为当前子图中坐标(x，y)处对应栅格更新前，后概率值；S(p)中p一般取值为p_occu(被占据)、p_free(空闲)、p_init(初始值)；clamp{x}是M(x，y)的区间限定函数，取值范围为[p_min，p_max]。

S3：将步骤S2处理后的概率栅格地图和步骤S2中的新一帧的基础数据进行回环检测，并由位姿解算器提供一个位姿初值来判断所述概率栅格地图与所述新一帧的基础数据之间是否存在点云匹配超过预设匹配度的部分，根据判断结果来优化位姿。如图2所示，该步骤可以简称为回环优化。在一具体实施例中，预设匹配度具体为0.8k。

步骤S3包括若所述概率栅格地图与所述新一帧的基础数据之间存在点云匹配超过预设匹配度的部分，则产生回环约束并得到所述新一帧的基础数据相对所述概率栅格地图的位姿。通过回环优化步骤产生回环约束的融合点云和对应子图的位姿以及两者之间的相对位姿计算约束(即位姿误差值)，通过最小化位姿误差值的方式优化所有已经完成的子图位姿，使得所有位姿约束最合理。

回环优化中的检测与扫描匹配类似，具体的计算表达式如公式7所示：

其中，W是搜索空间，为提升搜索效率，该方法采用深度优先搜索的分枝定界方法。

位姿优化的过程可以构造成一个非线性最小二乘法，具体表达式如公式8所示：

其中，

代表了产生回环约束的m帧融合点云在全局坐标系下位姿的集合；

代表了回环约束对应的n个子图在全局坐标系下位姿的集合；p_i，j是产生回环约束的融合点云(i)在其对应的子图(j)中的位姿，∑_i，j是其协方差矩阵；ρ是损失函数，避免错误回环约束影响；误差函数E定义如公式9所示：

其中，

是全局坐标系相对于子图(j)坐标系的位姿转换矩阵。

本发明实施例采用多种位置数据进行融合处理形成基础数据、再利用基础数据进行扫描建图并对新建地图进行回环优化，从而有效地识别透明障碍物，使得所建地图准确而全面，有利于移动机器人的后续路径规划等自主导航功能的实现。

本发明实施例提供的同时定位与建图方法无需计算里程，可应用于多种(如轮式、履带式、足式)室内移动机器人，通用性强。进一步地，本发明实施例提供的同时定位与建图方法对硬件(传感器和处理器)的要求不高，有利于降低整个移动机器人的成本。

采用本发明实施例提供的同时定位与建图方法在实际室内多玻璃间隔环境中的实验，如下所述。如图4和图5所示，实验环境的走廊两边为不连续的玻璃门/隔断墙(下半截为全透明玻璃)和实体墙。图6是使用常规SLAM方法建立的地图，从图6可以发现：在走廊两边只识别出了实体墙和部分玻璃固定柱，并且直接透过玻璃扫描出了部分室内环境，走廊尽头落地窗户情况类似。图7是使用本发明实施例提出的同时定位与建图方法建立的地图，从图7可以发现：真实描绘了走廊两侧及尽头的环境情况，避免了透明玻璃对建图效果的影响，有助于直接使用此地图进行路径规划等自主导航功能。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (10)

1.一种多玻璃隔断环境下的同时定位与建图的方法，其特征在于，所述同时定位与建图的方法包括步骤：

S1：采用多种位置信息传感器采集位置数据，并将所述位置数据进行处理而获得建图的基础数据；

S2：将预设帧数量的基础数据组建一张概率栅格地图；将再接收到新一帧的基础数据与所述概率栅格地图进行匹配，并由位姿解算器提供一个位姿初值而获得所述新一帧的基础数据相对于概率栅格地图的最优位姿；将所述最优位姿输入运动比较器，判断此次位姿变化大小并根据判断结果来确定是否更新所述概率栅格地图；

S3：将步骤S2处理后的概率栅格地图和步骤S2中的新一帧的基础数据进行回环检测，并由位姿解算器提供一个位姿初值来判断所述概率栅格地图与所述新一帧的基础数据之间是否存在点云匹配超过预设匹配度的部分，根据判断结果来优化位姿。

2.根据权利要求1所述的一种多玻璃隔断环境下的同时定位与建图的方法，其特征在于，所述多种位置信息传感器包括超声波测距传感器、激光雷达传感器和位姿传感器。

3.根据权利要求2所述的一种多玻璃隔断环境下的同时定位与建图的方法，其特征在于，将步骤S2中所获得的最优位姿与所述位姿传感器所获得数据同时作为所述位姿解算器的输入量。

4.根据权利要求2所述的一种多玻璃隔断环境下的同时定位与建图的方法，其特征在于，所述位置数据进行处理包括数据预处理以及对所述数据预处理后获得的数据进行融合处理。

5.根据权利要求4所述的一种多玻璃隔断环境下的同时定位与建图的方法，其特征在于，所述超声波测距传感器所获得的数据进行预处理为将所述数据进行格式转换。

6.根据权利要求4所述的一种多玻璃隔断环境下的同时定位与建图的方法，其特征在于，所述激光雷达传感器所获得的数据进行预处理为将所述数据进行运动畸变修正和滤波去噪。

7.根据权利要求4所述的一种多玻璃隔断环境下的同时定位与建图的方法，其特征在于，所述位姿传感器所获得的数据进行预处理为将所述数据进行数据校正和姿态融合。

8.根据权利要求1所述的一种多玻璃隔断环境下的同时定位与建图的方法，其特征在于，步骤S2包括若此次位姿变化大小处于预设范围，则忽略所述新一帧的基础数据；若此次位姿变化大小超过预设范围，则将所述新一帧的基础数据***所述概率栅格地图以更新所述概率栅格地图。

9.根据权利要求8所述的一种多玻璃隔断环境下的同时定位与建图的方法，其特征在于，若将所述新一帧的基础数据***时，融合点云的数据数量等于或大于设定的数量阈值，则组建一张新的概率栅格地图。

10.根据权利要求1所述的一种多玻璃隔断环境下的同时定位与建图的方法，其特征在于，步骤S3包括若所述概率栅格地图与所述新一帧的基础数据之间存在点云匹配超过预设匹配度的部分，则产生回环约束并得到所述新一帧的基础数据相对所述概率栅格地图的位姿。

Images