CN108332758A - 一种移动机器人的走廊识别方法及装置 - Google Patents

Abstract

本发明实施例公开了一种移动机器人的走廊识别方法及装置。其中，该方法包括：依据里程计检测结果预测移动机器人的第一位姿信息；基于迭代就近点法ICP算法将所述移动机器人采集的激光数据与已有地图进行匹配，得到所述移动机器人的第二位姿信息；计算所述第一位姿信息与所述第二位姿信息的差值；若预先更新的期望位姿模型收敛，则依据所述期望位姿模型以及所述差值从所述第一位姿信息和所述第二位姿信息中选择当前位姿信息；依据选择的当前位姿信息以及采集的激光数据更新地图。本发明实施例提供的技术方案，解决了移动机器人基于激光雷达进行定位建图中的走廊问题，从而使得所建地图更加准确。

Description

一种移动机器人的走廊识别方法及装置

技术领域

本发明涉及智能移动机器人技术领域，尤其涉及一种移动机器人的走廊识别方法及装置。

背景技术

在信息时代，随着智能移动机器人的发展和普及，基于激光雷达进行定位建图的技术由于其较高的精度且无需对环境进行任何修改也得到广泛的应用。

目前，智能移动机器人基于激光雷达在一条长廊中移动采用的ICP(迭代就近点法，Iterative Closest Point)算法进行定位建图，由于采集的走廊图像具有相似性，使得ICP法在判断机器人是否发生移动时容易出现误判现象，即ICP法判断机器人没有移动，但实际上机器人可能发生了移动。

发明内容

本发明实施例提供一种移动机器人的走廊识别方法及装置，根据预先更新的期望位姿模型来判断是采用ICP算法的输出结果还是里程计输出的结果来更新地图，解决了移动机器人基于激光雷达进行定位建图中的走廊问题，从而使得所建地图更加准确。

第一方面，本发明实施例提供了一种移动机器人的走廊识别方法，该方法包括：

依据里程计检测结果预测移动机器人的第一位姿信息；

基于迭代就近点法ICP算法将所述移动机器人采集的激光数据与已有地图进行匹配，得到所述移动机器人的第二位姿信息；

计算所述第一位姿信息与所述第二位姿信息的差值；

若预先更新的期望位姿模型收敛，则依据所述期望位姿模型以及所述差值从所述第一位姿信息和所述第二位姿信息中选择当前位姿信息；

依据选择的当前位姿信息以及采集的激光数据更新地图。

第二方面，本发明实施例还提供了一种移动机器人的走廊识别装置，该装置包括：

第一位姿预测模块，用于依据里程计检测结果预测移动机器人的第一位姿信息；

第二位姿获取模块，用于基于迭代就近点法ICP算法将所述移动机器人采集的激光数据与已有地图进行匹配，得到所述移动机器人的第二位姿信息；

差值计算模块，用于计算所述第一位姿信息与所述第二位姿信息的差值；

当前位姿获取模块，用于若预先更新的期望位姿模型收敛，则依据所述期望位姿模型以及所述差值从所述第一位姿信息和所述第二位姿信息中选择当前位姿信息；

地图更新模块，用于依据选择的当前位姿信息以及采集的激光数据更新地图。

本发明实施例提供的移动机器人的走廊识别方法及装置，将由里程计预测移动机器人得到的第一位姿信息与ICP算法输出的机器人的第二位姿信息的做差得到差值，依据预先更新的期望位姿模型的收敛性以及该差值选择当前的位姿信息即根据预先更新的期望位姿模型来判断是采用ICP算法的输出结果还是里程计输出的结果来更新地图，解决了移动机器人基于激光雷达进行定位建图中的走廊问题，从而使得所建地图更加准确。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1是本发明实施例一中提供的一种移动机器人的走廊识别方法的流程图；

图2是本发明实施例二中提供的一种移动机器人的走廊识别方法的流程图；

图3是本发明实施例三中提供的一种移动机器人的走廊识别方法的流程图；

图4是本发明实施例四中提供的一种移动机器人的走廊识别装置的结构框图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部内容。

实施例一

图1为本发明实施例一提供的一种移动机器人的走廊识别方法的流程图，本实施例可适用于智能移动机器人在基于激光雷达在走廊中进行定位建地图的情况。该方法可以由本发明实施例提供的移动机器人的走廊识别装置来执行，该装置可采用软件和/或硬件的方式实现。参见图1，该方法具体包括：

S110，依据里程计检测结果预测移动机器人的第一位姿信息。

其中，位姿信息指移动机器人的位置和姿态，位置表示移动机器人相对世界坐标的位置(平移)，一般用坐标(x，y)表示，姿态表示移动机器人的偏航角即机器人实际前进方向与期望前进方向之间的偏差角度，可选用Φ表示。由此，位姿信息对应的即为三维空间信息可用(x，y，Φ)表示。对应的第一位姿信息可用n1＝(x1，y1，Φ1)表示。

里程计是一种用于测量行程及速度的装置，移动机器人中设置有里程计，可用于估测移动机器人的位姿信息即第一位姿信息。

S120，基于迭代就近点法ICP算法将移动机器人采集的激光数据与已有地图进行匹配，得到移动机器人的第二位姿信息。

其中，迭代就近点法ICP算法用于寻求点集之间的匹配关系，求解的结果是两点集之间的平移及旋转量。激光数据即由位于移动机器人上的激光雷达采集的经过平滑滤波过滤掉噪点后的数据。

具体的，将移动机器人采集的激光数据作为第一点集，已有地图中对应的数据作为第二点集，将三维空间中第一点集中各点经过三维空间变换后与第二点集中点一一对应，求解得到的两点之间的平移及旋转量即为移动机器人的第二位姿信息。相应的，第二位姿信息可以用n2＝(x2，y2，Φ2)。

S130，计算第一位姿信息与第二位姿信息的差值。

将里程计获得的移动机器人的第一位姿信息n1减去ICP算法得到的移动机器人的第一位姿信息n2即可得到差值n3，n3＝(x3，y3，Φ3)。

S140，若预先更新的期望位姿模型收敛，则依据期望位姿模型以及差值从第一位姿信息和第二位姿信息中选择当前位姿信息。

其中，期望位姿模型是基于第一位姿信息与第二位姿信息的差值建立的三维高斯模型，模型中的元素有三维均值向量和三维均值向量的协方差矩阵；三维均值向量即为第一位姿信息与第二位姿信息的差值的均值对应的向量；协方差矩阵为3ⅹ3矩阵。判断三维高斯模型收敛的条件即为协方差矩阵的迹值是否足够小，协方差的迹值是指所有主对角线上的元素之和。初始化条件下期望位姿模型中的元素值均为0，对应的预先更新的期望位姿模型是将第一位姿信息与第二位姿信息的差值输入后得到的。

当预先更新的期望位姿模型中协方差矩阵的迹值小于预先设定的阈值如小于10^-6时，则可确定预先更新的期望位姿模型收敛，此时，可依据一定的规则从第一位姿信息和第二位姿信息中选择一个位姿信息作为当前位姿信息。如可以根据期望位姿模型以及差值确定里程计和ICP算法哪个精度更高，从而确定将第一位姿信息还是第二位姿信息作为当前位姿信息。

示例性的，还可以包括：若预先更新的期望位姿模型不收敛，则将第二位姿信息选作当前位姿信息。

需要说明的是，一般情况下，里程计累计误差大，如移动机器人的轮子打滑时，ICP算法输出的数据的精度相对较高。因此，当预先更新的期望位姿模型不收敛，可将第二位姿信息即ICP算法输出的移动机器人的位姿信息选作当前位姿信息，同时更新里程预测值。

S150，依据选择的当前位姿信息以及采集的激光数据更新地图。

具体的，根据得到的移动机器人的当前位姿信息，将激光雷达采集的激光数据插入到已有地图中相应的位置并更新该地图。

本发明实施例提供的移动机器人的走廊识别方法，将由里程计预测移动机器人得到的第一位姿信息与ICP算法输出的机器人的第二位姿信息的做差得到差值，依据预先更新的期望位姿模型的收敛性以及该差值选择当前的位姿信息即根据预先更新的期望位姿模型来判断是采用ICP算法的输出结果还是里程计输出的结果来更新地图，解决了移动机器人基于激光雷达进行定位建图中的走廊问题，从而使得所建地图更加准确。

实施例二

图2为本发明实施例二提供的一种移动机器人的走廊识别方法流程图。本实施例以本发明实施例一为基础，进一步的提供了一种依据期望位姿模型以及差值从第一位姿信息和第二位姿信息中选择当前位姿信息的方法。参见图2，该方法具体包括：

S210，依据里程计检测结果预测移动机器人的第一位姿信息。

S220，基于迭代就近点法ICP算法将移动机器人采集的激光数据与已有地图进行匹配，得到移动机器人的第二位姿信息。

S230，计算第一位姿信息与第二位姿信息的差值。

S240，若预先更新的期望位姿模型收敛，则计算差值与期望位姿模型中的均值向量之间的马氏距离。

其中，期望位姿模型中的均值向量即三维均值向量是第一位姿信息与第二位姿信息的差值的均值对应的向量；马氏距离表示数据的协方差距离，是一种有效的计算两个未知样本集的相似度的方法。

因此，当预先更新的期望位姿模型收敛时，本实施例中通过计算由第一位姿信息与第二位姿信息得到的差值与三维均值向量之间的马氏距离，来选择移动机器人的当前位姿信息。

S250，若马氏距离大于距离阈值，则将第一位姿信息选作当前位姿信息；否则，将第二位姿信息选作当前位姿信息。

其中，距离阈值是预先设置的，可根据实际需求进行修正；具体的，马氏距离越小即马氏距离在阈值范围内，建立的期望位姿模型越好，则ICP算法输出的位姿信息越准确可选其作为当前的位姿信息，同时将里程计预测的位姿信息进行更新；否则，马氏距离越大，建立的期望位姿模型越偏离真实的情况，则选里程计预测的位姿信息作为当前的位姿信息，同时依据里程计结果更新移动机器人的位姿信息。

示例性的，从第一位姿信息和第二位姿信息中选择当前位姿信息之后，还可以包括依据得到的差值更新期望位姿模型。具体操作过程可以是：依据得到的差值更新差值均值，并依据更新的差值均值更新期望位姿模型中的均值向量；依据更新的均值向量更新期望位姿模型中的协方差矩阵。

由于依据马氏距离确定当前位姿的同时，也在更新里程计预测的位姿信息，因此，里程计预测的位姿信息与ICP算法输出的位姿信息的差值也会发生变化，而期望位姿模型是基于差值建立的，所以当差值变化的同时，对应的期望位姿模型也需更新，即依据得到的差值通过更新期望位姿模型中的元素均值向量和协方差矩阵更新更新期望位姿模型。

S260，依据选择的当前位姿信息以及采集的激光数据更新地图。

本发明实施例提供的移动机器人的走廊识别方法，将由里程计预测移动机器人得到的第一位姿信息与ICP算法输出的机器人的第二位姿信息的做差得到差值，依据预先更新的期望位姿模型的收敛性以及该差值选择当前的位姿信息即根据预先更新的期望位姿模型来判断是采用ICP算法的输出结果还是里程计输出的结果来更新地图，解决了移动机器人基于激光雷达进行定位建图中的走廊问题，从而使得所建地图更加准确。

实施例三

图3为本发明实施例二提供的一种移动机器人的走廊识别方法流程图。本实施例以上述实施例为基础，提供了一种优选示例。参见图3，该方法具体包括：

S310，依据里程计检测结果预测移动机器人的第一位姿信息。

S320，基于迭代就近点法ICP算法将移动机器人采集的激光数据与已有地图进行匹配，得到移动机器人的第二位姿信息。

S330，计算第一位姿信息与第二位姿信息的差值。

S340，判断预先更新的期望位姿模型收敛是否收敛，若收敛，则执行步骤S350；否则，执行步骤S370。

S350，计算差值与期望位姿模型中的均值向量之间的马氏距离。

S360，若马氏距离大于距离阈值，则将第一位姿信息选作当前位姿信息；否则，将第二位姿信息选作当前位姿信息。

S370，将第二位姿信息选作当前位姿信息。

S380，依据选择的当前位姿信息以及采集的激光数据更新地图。

本发明实施例提供的移动机器人的走廊识别方法，将由里程计预测移动机器人得到的第一位姿信息与ICP算法输出的机器人的第二位姿信息的做差得到差值，依据预先更新的期望位姿模型的收敛性以及该差值选择当前的位姿信息即根据预先更新的期望位姿模型来判断是采用ICP算法的输出结果还是里程计输出的结果来更新地图，解决了移动机器人基于激光雷达进行定位建图中的走廊问题，从而使得所建地图更加准确。

实施例四

图4为本发明实施例四提供的一种移动机器人的走廊识别装置的结构框图，该装置可执行本发明任意实施例所提供的移动机器人的走廊识别方法，具备执行方法相应的功能模块和有益效果。如图4所示，该装置可以包括：

第一位姿预测模块410，用于依据里程计检测结果预测移动机器人的第一位姿信息；

第二位姿获取模块420，用于基于迭代就近点法ICP算法将移动机器人采集的激光数据与已有地图进行匹配，得到移动机器人的第二位姿信息；

差值计算模块430，用于计算第一位姿信息与第二位姿信息的差值；

当前位姿获取模块440，用于若预先更新的期望位姿模型收敛，则依据期望位姿模型以及差值从第一位姿信息和第二位姿信息中选择当前位姿信息；

地图更新模块450，用于依据选择的当前位姿信息以及采集的激光数据更新地图。

本发明实施例提供的移动机器人的走廊识别装置，将由里程计预测移动机器人得到的第一位姿信息与ICP算法输出的机器人的第二位姿信息的做差得到差值，依据预先更新的期望位姿模型的收敛性以及该差值选择当前的位姿信息即根据预先更新的期望位姿模型来判断是采用ICP算法的输出结果还是里程计输出的结果来更新地图，解决了移动机器人基于激光雷达进行定位建图中的走廊问题，从而使得所建地图更加准确。

可选的，当前位姿获取模块440具体可以用于：

计算差值与期望位姿模型中的均值向量之间的马氏距离；

若马氏距离大于距离阈值，则将第一位姿信息选作当前位姿信息；否则，将第二位姿信息选作当前位姿信息。

可选的，当前位姿获取模块还用440还可以用于：若预先更新的期望位姿模型不收敛，则将第二位姿信息选作当前位姿信息。

示例性的，上述装置还可以包括：

模型更新模块，用于从第一位姿信息和第二位姿信息中选择当前位姿信息之后，依据得到的差值更新期望位姿模型。

可选的，模型更新模块具体可以用于：

依据得到的差值更新差值均值，并依据更新的差值均值更新期望位姿模型中的均值向量；

依据更新的均值向量更新期望位姿模型中的协方差矩阵。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (10)

1.一种移动机器人的走廊识别方法，其特征在于，包括：

依据里程计检测结果预测移动机器人的第一位姿信息；

基于迭代就近点法ICP算法将所述移动机器人采集的激光数据与已有地图进行匹配，得到所述移动机器人的第二位姿信息；

计算所述第一位姿信息与所述第二位姿信息的差值；

若预先更新的期望位姿模型收敛，则依据所述期望位姿模型以及所述差值从所述第一位姿信息和所述第二位姿信息中选择当前位姿信息；

依据选择的当前位姿信息以及采集的激光数据更新地图。

2.根据权利要求1所述的方法，其特征在于，依据所述期望位姿模型以及所述差值从所述第一位姿信息和所述第二位姿信息中选择当前位姿信息，包括：

计算所述差值与所述期望位姿模型中的均值向量之间的马氏距离；

若所述马氏距离大于距离阈值，则将所述第一位姿信息选作当前位姿信息；否则，将所述第二位姿信息选作当前位姿信息。

3.根据权利要求1所述的方法，其特征在于，还包括：

若预先更新的期望位姿模型不收敛，则将所述第二位姿信息选作当前位姿信息。

4.根据权利要求2所述的方法，其特征在于，从所述第一位姿信息和所述第二位姿信息中选择当前位姿信息之后，还包括：

依据得到的差值更新所述期望位姿模型。

5.根据权利要求4所述的方法，其特征在于，依据得到的差值更新所述期望位姿模型，包括：

依据得到的差值更新差值均值，并依据更新的差值均值更新所述期望位姿模型中的均值向量；

依据更新的均值向量更新所述期望位姿模型中的协方差矩阵。

6.一种移动机器人的走廊识别装置，其特征在于，包括：

第一位姿预测模块，用于依据里程计检测结果预测移动机器人的第一位姿信息；

第二位姿获取模块，用于基于迭代就近点法ICP算法将所述移动机器人采集的激光数据与已有地图进行匹配，得到所述移动机器人的第二位姿信息；

差值计算模块，用于计算所述第一位姿信息与所述第二位姿信息的差值；

当前位姿获取模块，用于若预先更新的期望位姿模型收敛，则依据所述期望位姿模型以及所述差值从所述第一位姿信息和所述第二位姿信息中选择当前位姿信息；

地图更新模块，用于依据选择的当前位姿信息以及采集的激光数据更新地图。

7.根据权利要求6所述的装置，其特征在于，所述当前位姿获取模块具体用于：

计算所述差值与所述期望位姿模型中的均值向量之间的马氏距离；

若所述马氏距离大于距离阈值，则将所述第一位姿信息选作当前位姿信息；否则，将所述第二位姿信息选作当前位姿信息。

8.根据权利要求6所述的装置，其特征在于，所述当前位姿获取模块还用于：

若预先更新的期望位姿模型不收敛，则将所述第二位姿信息选作当前位姿信息。

9.根据权利要求7所述的装置，其特征在于，还包括：

模型更新模块，用于从所述第一位姿信息和所述第二位姿信息中选择当前位姿信息之后，依据得到的差值更新所述期望位姿模型。

10.根据权利要求9所述的装置，其特征在于，所述模型更新模块具体用于：

依据得到的差值更新差值均值，并依据更新的差值均值更新所述期望位姿模型中的均值向量；

依据更新的均值向量更新所述期望位姿模型中的协方差矩阵。