CN115683100A - 机器人的定位方法、装置、机器人以及存储介质 - Google Patents

Abstract

本申请实施例公开了一种机器人的定位方法、装置、机器人以及存储介质，该定位方法公开的机器人可以包括惯性里程计及用于进行环境感知的传感器，基于此，机器人可以根据惯性里程计采集的里程计信息，确定该机器人当前时刻在世界坐标系下的第一位姿信息，然后将通过传感器采集的点云数据转换至世界坐标系下，得到当前时刻对应的目标点云数据；以及，从三维点云地图中查找与目标点云数据匹配的地图点云数据，并根据地图点云数据和目标点云数据，修正第一位姿信息，得到机器人在当前时刻对应的目标位姿信息。通过实施该方法，可以提高机器人定位的准确性。

Description

机器人的定位方法、装置、机器人以及存储介质

技术领域

本申请涉及机器人技术领域，尤其涉及一种机器人的定位方法、装置、机器人以及存储介质。

背景技术

随着科技的快速发展，机器人逐步出现在人们的视野中，例如，服务型机器人、搬运机器人、探测机器人等。由于市场对机器人的需求不断增大，对机器人的自主移动定位能力的需求也越来越强烈，因此，如何让机器人进行精准地定位成为了一个亟需解决的技术问题。

发明内容

本申请实施例提供了一种机器人的定位方法、装置、机器人以及存储介质，可以实现机器人的精准定位。

本申请实施例第一方面提供了一种机器人的定位方法，所述机器人包括惯性里程计及用于进行环境感知的传感器，所述方法包括：

根据所述惯性里程计采集的里程计信息，确定所述机器人当前时刻在世界坐标系下的第一位姿信息；

将通过所述传感器采集的点云数据转换至所述世界坐标系下，得到所述当前时刻对应的目标点云数据；

从三维点云地图中查找与所述目标点云数据匹配的地图点云数据；

根据所述目标点云数据及地图点云数据，对所述第一位姿信息进行修正，得到所述机器人在所述当前时刻对应的目标位姿信息。

本申请实施例第二方面提供了一种机器人的定位装置，所述机器人包括惯性里程计及用于进行环境感知的传感器，所述装置包括：

位姿确定模块，用于根据所述惯性里程计采集的里程计信息，确定所述机器人当前时刻在世界坐标系下的第一位姿信息；

点云转换模块，用于将通过所述传感器采集的点云数据转换至所述世界坐标系下，得到所述当前时刻对应的目标点云数据；

查找模块，用于从三维点云地图中查找与所述目标点云数据匹配的地图点云数据；

修正模块，用于根据所述目标点云数据及地图点云数据，对所述第一位姿信息进行修正，得到所述机器人在所述当前时刻对应的目标位姿信息。

本申请实施例第三方面提供了一种机器人，包括：

存储有可执行程序代码的存储器；

以及所述存储器耦合的处理器；

所述处理器调用所述存储器中存储的所述可执行程序代码，所述可执行程序代码被所述处理器执行时，使得所述处理器实现如本申请实施例第一方面所述的方法。

本申请实施例第四方面提供一种计算机可读存储介质，其上存储有可执行程序代码，所述可执行程序代码被处理器执行时，实现如本申请实施例第一方面所述的方法。

本申请实施例第五方面公开一种计算机程序产品，当该计算机程序产品在计算机上运行时，使得该计算机执行本申请实施例第一方面公开的任意一种所述的方法。

本申请实施例第六方面公开一种应用发布平台，该应用发布平台用于发布计算机程序产品，其中，当该计算机程序产品在计算机上运行时，使得该计算机执行本申请实施例第一方面公开的任意一种所述的方法。

从以上技术方案可以看出，本申请实施例具有以下优点：

在本申请实施例中，机器人可以包括惯性里程计及用于进行环境感知的传感器，基于此，机器人可以根据惯性里程计采集的里程计信息，确定该机器人当前时刻在世界坐标系下的第一位姿信息，然后将通过传感器采集的点云数据转换至世界坐标系下，得到当前时刻对应的目标点云数据；以及，从三维点云地图中查找与目标点云数据匹配的地图点云数据，并根据地图点云数据和目标点云数据，修正第一位姿信息，得到机器人在当前时刻对应的目标位姿信息。通过实施该方法，机器人可以根据传感器采集的当前时刻对应的点云数据及三维点云地图，对通过里程计信息得到的当前时刻的位姿进行修正，由于三维点云地图可以精确地反映机器人工作环境中各物体的空间位置，因此，基于三维点云地图对通过里程计信息得到的当前时刻的位姿修正，可以得到精确的位姿，有利于提高机器人定位的准确性。

附图说明

为了更清楚地说明本申请实施例技术方案，下面将对实施例和现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，还可以根据这些附图获得其它的附图。

图1是本申请实施例公开的一种足机器人10的示意图；

图2是本申请实施例公开的一种机器人的定位方法的流程图；

图3A是本申请实施例公开的另一种机器人的定位方法的流程图；

图3B是本申请实施例公开的一实施例中确定地图点云数据和目标点云数据之间的变换关系的流程图；

图4是本申请实施例公开的一种机器人的定位装置的结构框图；

图5是本申请实施例公开的一种机器人的结构框图。

具体实施方式

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

需要说明的是，本申请实施例及附图中的术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。

可以理解，本申请所使用的术语“第一”、“第二”等可在本文中用于描述各种元件，但这些元件不受这些术语限制。这些术语仅用于将第一个元件与另一个元件区分。举例来说，在不脱离本申请的范围的情况下，可以将第一点云数据称为第二点云数据，且类似地，可将第二点云数据称为第一点云数据。第一点云数据和第二点云数据两者都是点云数据，但其不是同一个点云数据。另外，需要说明的是，本申请实施例中所使用的术语“多个”指的是两个或两个以上。

可以理解的是，本申请实施例公开的机器人可以是移动机器人，移动机器人主要在于它的移动性，它扩大了机器人的作业范围，可应用于各个行业，例如：移动机器人可以在工业上进行搬运，在军事上进行作战、在家庭进行清洁，以及在人类难以到达的环境里面进行探测等。在本申请实施例中，移动机器人可以包括但不限于轮式机器人、履带式机器人以及足式机器人。

需要说明的是，在本申请实施例中，下述实施例主要以足式机器人为例进行示例性说明。请参阅图1，图1是本申请实施例公开的一种足机器人10的示意图。在本申请实施例中，该足式机器人可以包括但不限于2足机器人、4足机器人(如图1所示)、6足机器人及12足机器人中的任一种。

进一步需要说明的是，本申请公开的足式机器人可以包括惯性里程计和进行环境感知的传感器，该惯性里程计用于采集机器人的位姿信息。

在一些实施例中，该惯性里程计可以包括电机编码器和/或惯性测量单元(Inertial Measurement Unit，IMU)。

可选的，电机编码器可以包括但不限于光电编码器、绝对式编码器、增量式编码器或混合式绝对值编码器等。其中，电机编码器可以根据采样周期内脉冲的变化量，确定足式机器人不同时刻下位姿的相对变化量。IMU是测量机器人三轴姿态角(或角速率)以及加速度的装置，该IMU可以包括三个单轴的加速度计和3个单轴的陀螺，该加速度计用于测量机器人的加速度信息，陀螺仪用于测量机器人的角速度信息，根据机器人在不同时刻的加速度信息和角速度信息，可以解算出足式机器人不同时刻下位姿的相对变化量。

在一些实施例中，进行环境感知的传感器可以包括但不限于多线激光雷达、双目相机及毫米波雷达等中的一种或多种，该类传感器可用于对机器人周围的环境进行感知，采集周围的环境信息，例如，可通过多线激光雷达或毫米波雷达采集周围环境的三维点云数据，可通过双目相机采集周围环境的图像，进而通过处理采集到的图像得到周围环境的三维点云数据，实现对机器人周围环境中各种目标对象(如地面、植物、建筑、墙面、障碍物等)的检测，也可以是多种不同的传感器进行的组合。

在一些实施例中，多线激光雷达可以包括但限于4线、8线、16线、32线、64线以及128线中的任一种。

在一些实施例中，双目相机可以包括但不限于RGB(Red，Green，Blue)双目相机、TOF(Time of flight，飞行时间)相机及结构光双目相机中的任一种。

需要说明的是，在本申请实施例中，机器人可以通过设置于该机器人上的定位装置来实现对该机器人的定位，也即本申请所公开的机器人的定位方法可适用于该机器人的定位装置。

请参阅图2，图2是本申请实施例公开的一种机器人的定位方法的流程示意图。可以包括以下步骤：

201、根据惯性里程计采集的里程计信息，确定机器人当前时刻在世界坐标系下的第一位姿信息。

第一位姿信息可包括机器人当前时刻在世界坐标系下的位置及姿态信息。在本申请实施例中，机器人可以通过惯性里程计实时采集机器人的里程计信息。在一些实施例中，通过惯性里程计采集机器人的里程计信息可以包括但不限于以下方式：

方式1、惯性里程计可包括电机编码器，电机编码器可设置于机器人的行走部件上，(或与该行走部件的电机连接)，可用于实时采集对应电机的转角及转速信息，再根据对应电机的转角及转速信息，得到脉冲信号，从而根据该脉冲信号得到机器人实时的里程计信息。

方式2、惯性里程计可包括IMU，IMU可设置于机器人的行走部件上，IMU可以实时采集该行走部件的加速度和角速度信息，从而根据该行走部件的加速度和角速度信息，得到机器人实时的里程计信息。

方式3、惯性里程计可包括电机编码器及IMU，通过电机编码器可以实时采集脉冲信号，并通过IMU实时采集行走部件的加速度和角速度信息，可将同一时刻的脉冲信号、行走部件的加速度及角速度信息进行融合，得到机器人在各个时刻下对应的里程计信息。这种融合电机编码器和IMU实时获取里程计信息的方式，有利于提高第一位姿信息的精度。可选地，该融合的方式可包括但不限于以上方式。

在一些实施例中，机器人根据惯性里程计采集的里程计信息，确定机器人当前时刻在世界坐标系下的第一位姿信息，可以包括：机器人从惯性里程计采集的里程计信息中，获取上一时刻对应的第一里程计信息以及当前时刻对应的第二里程计信息，并根据该第一里程计信息和第二里程计信息，得到位姿偏移信息，以及根据机器人在上一时刻的目标位姿信息和该位姿偏移信息，得到机器人当前时刻在世界坐标系下的第一位姿信息。其中，位姿偏移信息可以包括位置偏移信息和姿态偏移信息。机器人在上一时刻的目标位姿信息指的是机器人在上一时刻的位置和姿态。

在一些实施例中，位置偏移信息指示的可以是机器人在三维空间中的位置偏移量，姿态偏移量指示的可以是机器人各姿态角的偏移量，可选地，各姿态角可采用世界坐标系进行表示，在该世界坐标系下，姿态角可包括翻滚角、俯仰角以及俯仰角，也可采用其它坐标系表示，在此不作限定。

202、将通过传感器采集的点云数据转换至世界坐标系下，得到当前时刻对应的目标点云数据。

通过传感器采集的点云数据可用于表征机器人周围环境的各个点相对传感器的三维空间位置。其中，多线激光雷达通过检测激光光束的反射时长，得到机器人周围环境的各个点相对多线激光雷的三维空间位置；毫米波雷达通过检测毫米波的反射时长，得到机器人周围环境的各个点相对毫米波雷达的三维空间位置；双目相机通过在同一时刻采集的左目图像和右目图像，进而根据该左目图像和右目图像，得到机器人周围环境的各个点相对双目相机的三维空间位置。

在本申请实施例中，通过多线激光雷达或毫米波雷达采集的点云数据可以包括每一点对应的三维位置坐标和激光反射强度；通过双目相机采集的点云数据可以包括每一点对应的三维位置坐标、激光反射强度及颜色信息。其中，需要说明的是，多线激光雷达和毫米波雷达在室内外环境中均适用，双目相机由于受光照影响，多适用于室内环境。需要说明的是，毫米波雷达和双目相机的不仅抗干扰能力强，而且成本小。

在本申请实施例中，通过感器采集的点云数据是在传感器坐标系下的。在一些实施例中，将通过传感器采集的点云数据转换至世界坐标系下，得到当前时刻对应的目标点云数据，可以包括：对通过传感器采集的当前时刻对应的原始点云数据进行滤波处理，并根据预先标定的变换关系，将滤波后的点云数据从传感器坐标系下转换至世界坐标系下，得到当前时刻对应的目标点云数据。需要说明的是，该预先标定的变换关系可以包括传感器坐标系和世界坐标系之间的变换矩阵。通过实施该方法，对通过传感器采集的当前时刻对应的原始点云数据进行滤波处理，可以有效降低点云数据的数量，有利于提高定位效率。

在一些实施例中，对当前时刻对应的原始点云数据进行滤波的方式可以包括但不限于双边滤波、高斯滤波、统计滤波、条件滤波、直通滤波及随机采样一致滤波中一种或几种的组合。

203、从三维点云地图中查找与目标点云数据匹配的地图点云数据。

其中，三维点云地图可用于表征机器人的真实工作环境的点云数据情况，该三维点云地图可以是预先存储的地图数据，该地图数据可以由足式机器人预先通过上述传感器(多线激光雷达、双目相机及毫米波雷达中的任一种或多个)探测得到。

在一些实施例中，可以根据目标点云数据的位置信息，从三维点云地图中查找与目标点云数据匹配的地图点云数据。该匹配的地图点云数据可指的是三维点云地图中距离目标点云数据中点小于第一距离阈值的点。

204、根据目标点云数据及地图点云数据，对第一位姿信息进行修正，得到机器人在当前时刻对应的目标位姿信息。

在一些实施例中，根据目标点云数据及地图点云数据，对第一位姿信息进行修正，得到机器人在当前时刻对应的目标位姿信息，可以包括但不限于以下方式：

方式1、根据地图点云数据解算当前时刻对应的第二位姿信息，并根据第二位姿信息和第一位姿信息，得到机器人在当前时刻对应的目标位姿信息。

方式2、根据地图点云数据和目标点云数据，确定地图点云数据和目标点云数据之间的变换关系；通过地图点云数据和目标点云数据之间的变换关系修正第一位姿信息，得到机器人在所述当前时刻对应的目标位姿信息。

在一些实施例中，地图点云数据和目标点云数据之间的变换关系可用于表示地图点云数据中第一点与目标点云数据中第二点的变换关系，需要说明的是，第一点为地图点云数据中的任一点，第一点与第二点之间的距离小于第一距离阈值。

在一些实施例中，地图点云数据和目标点云数据之间的变换关系可以包括地图点云数据和目标点云数据之间的变换矩阵，通过地图点云数据和目标点云数据之间的变换关系修正第一位姿信息，得到机器人在所述当前时刻对应的目标位姿信息，可以包括：将第一位姿信息包括的位置坐标及姿态坐标，与地图点云数据和目标点云数据之间的变换矩阵相乘，得到机器人在当前时刻对应的目标位姿信息。

通过实施上述方法，机器人可以根据传感器采集的当前时刻对应的点云数据及三维点云地图，对通过里程计信息得到的当前时刻的位姿进行修正，由于三维点云地图可以较为精确地反映机器人工作环境中各物体的空间位置，因此，基于三维点云地图对通过里程计信息得到的当前时刻的位姿修正，可以得到较为精确的位姿，有利于提高机器人定位的准确性。

请参阅图3A，图3A是本申请实施例公开的另一种机器人的定位方法的流程示意图。可以包括以下步骤：

301、根据惯性里程计采集的里程计信息，确定机器人当前时刻在世界坐标系下的第一位姿信息。

需要说明的是，在本申请实施例中，针对步骤301的详细描述，请参照图2所示的步骤201的描述，此处不再赘述。

302、获取通过传感器采集的当前时刻对应的原始点云数据。

在本申请实施例中，通过多线激光雷达或毫米波雷达可以直接采集得到当前时刻对应的原始点云数据。若传感器为双目相机，则先通过该双目相机采集当前时刻的左目图像和右目图像，然后根据左目图像和右目图像得到每一像素对应的深度信息，最后再根据每一像素对应的深度信息、坐标信息及双目相机的相机内参，得到当前时刻对应的原始点云数据。其中，相机内参的参数可以包括焦距、主点偏移量及轴倾斜度。

303、对原始点云数据进行聚类处理，得到聚类成功的第一点云数据。

在一些实施例中，对原始点云数据进行聚类的方式可以包括但不限于欧式聚类、K-Means(k均值)聚类、密度减法聚类及自适应密度聚类中任一种或几种的组合。

在一些实施例中，对原始点云数据进行聚类处理，得到聚类成功的第一点云数据可以包括：对原始点云数据进行聚类处理，得到聚类成功的多个聚类对象对应的点云数据；根据聚类对象对应的有效性条件，从该多个聚类对象中剔除无效的聚类对象，得到有效的聚类对象，以及根据有效的聚类对象对应的点云数据，得到第一点云数据。

可以理解的是，聚类对象可以表征点云数据的类别。在一些实施例中，聚类对象可以包括但不限于数字、字母及特殊字符中一种或几种的组合。聚类成功的第一点云数据可以包括满足有效性条件的聚类对象对应的点云数据。

在一些实施例中，聚类对象对应的有效性条件可以包括最小点数目和最最大点数目。

示例性的，最小点数目为1000、且最大点数目为25000，可以理解的是，若聚类对象对应的点云数据的个数小于1000或大于25000，则确定该聚类对象为无效的聚类对象，若聚类对象对应的点云数据的个数大于或等于1000，且小于或等于25000，则确定该聚类对象为有效的聚类对象。若对原始点云数据进行聚类处理得到多个聚类对象分别包括：聚类对象A、聚类对象B、聚类对象C、聚类对象D、聚类对象F以及聚类对象E，其中，聚类对象A、聚类对象B、聚类对象C以及聚类对象D对应的点云数据的个数均处于1000-25000之间，而聚类对象F的点云数据的个数小于1000，聚类对象E点云数据的个数大于25000，此时，第一点云数据可以包括聚类对象A、聚类对象B、聚类对象C以及聚类对象D对应的点云数据。

在一些实施例中，若采用欧式聚类原始点云数据，则对原始点云数据进行聚类处理，得到聚类成功的多个聚类对象对应的点云数据，可以包括：针对原始点云数据中的任一点O，利用最近邻搜索算法找到离点O最近的n个点，并获取该n个点分别与点O之间的距离，可从该n个点中确定出与点O之间的距离小于第二距离阈值的m个点，该m小于或等于n，该m个点和该O点则属于同一聚类对象U。可从该m个点中选取任一点作为新的点O，重复执行上述步骤直至聚类对象U中再无点加入，则可得到聚类对象U包含的点云数据。按照该方式遍历完原始点云数据的所有点，即可得到多个聚类对象对应的点云数据。

示例性的，上述有效的聚类对象可以包括地面聚类对象及非地面聚类对象，也即，第一点云数据可以包括地面点云数据和非地面点云数据。通过实施该方法，对原始点云数据进行聚类处理可以将微小物体当作噪音点处理掉，可以理解的是，微小物体对应的点云数据可以为无效的聚类对象对应的点云数据。示例性的，该微小物体可以包括随风飘动的树叶、草等，这些在实际中往往在前一帧出现，后一帧不出现，从而可减少相邻帧之间微小物体不重复出现造成的干扰。

304、提取第一点云数据中的特征信息，并根据该特征信息从第一点云数据中提取边缘点云和目标平面点云，以得到第二点云数据。

边缘点云指的是处于边界上的点，平面点云指的是组成平面的点。在一些实施例中，提取第一点云数据中的特征信息，并根据该特征信息从第一点云数据中提取边缘点云和目标平面点云，以得到第二点云数据，可以包括：从每一聚类对象对应的点云数据中提取每一聚类对象对应的特征信息，并根据每一聚类对象对应的特征信息，从该从每一聚类对象对应的点云数据中提取该聚类对象对应的边缘点云和目标平面点云。其中，每一聚类对象对应的特征信息用于表征该聚类对象的几何特征。在一些实施例中，每一聚类对象对应的特征信息可以包括点云法向量，点云法向量指示的是法线方向。

在一些实施例中，从每一聚类对象对应的点云数据中提取每一聚类对象对应的特征信息，可以包括：利用最小二乘法拟合每一聚类对象对应的点云数据，得到局部平面点云，并计算局部平面点云对应的法向量，得到点云法向量。需要说明的是，局部平面点云表征的是初步拟合得到的平面点云，局部平面点云存在较大误差。

进一步的，在一些实施例中，根据每一聚类对象对应的特征信息，从该每一聚类对象对应的点云数据中提取该聚类对象对应的边缘点云和目标平面点云，可以包括：根据点云法向量，从每一聚类对象对应的点云数据中提取目标平面点云，并根据目标平面点云对应的点云数据得到目标平面点云的法向量，最后再根据目标平面点云的法向量，得到边缘点云。可以理解的是，在本申请实施例中，第二点云数据可以包括每一聚类对象的平面点云数据和边缘点云数据。通过实施该方法，进行边缘点云和平面点云的提取，可以进一步降低点云数据的数量，有利于进一步提高定位效率。

305、将第二点云数据从传感器的传感器坐标系转换至世界坐标系，得到目标点云数据。

在一些实施例中，将第二点云数据由传感器的传感器坐标系转换为世界坐标系，得到目标点云数据，包括：将第二点云数据由传感器的传感器坐标系转换为机器人坐标系，得到第三点云数据；根据第一位姿信息，将第三点云数据从机器人坐标系转换至世界坐标系，得到目标点云数据。

其中，机器人坐标系分为关节坐标系和直角坐标系(笛卡尔坐标系)。机器人坐标系是为了确定机器人的位置和姿态而在机器人或空间上定义的位置指标系统。

在一些实施例中，传感器坐标系和机器人坐标系之间的变换关系可以是预先标定的，可以根据传感器坐标系和机器人坐标系之间的变换关系，将第二点云数据由传感器的传感器坐标系转换为机器人坐标系，得到第三点云数据。

其中，在本申请实施例中，第一位姿信息指示可以是的机器人在世界坐标系的位姿。在一些实施例中，根据第一位姿信息，将第三点云数据从机器人坐标系转换至世界坐标系，得到目标点云数据，可以包括：根据第一位姿信息包括的三维位置坐标及三维姿态坐标，确定机器人坐标系与世界坐标系之间的变换关系，并根据机器人坐标系与世界坐标系之间的变换关系，将第三点云数据从机器人坐标系转换至世界坐标系。可以理解的是，第三点云数据可以包括每一有效的聚类对象在机器人坐标系下的平面点云数据和边缘点云数据，目标点云数据可以包括每一有效的聚类对象在世界坐标系下的平面点云数据和边缘点云数据。

306、从三维点云地图中查找与目标点云数据匹配的地图点云数据。

在一些实施例中，从三维点云地图中查找与目标点云数据匹配的地图点云数据，可以包括：从三维点云地图中，查找目标点云数据中每一个点对应的最邻近点；根据每一最邻近点在三维点云地图中的位置信息，得到目标点云数据匹配的地图点云数据。

在本申请实施例中，三维点云地图中包含的三维点云数据也可以是以世界坐标系为基准的点云数据。因此，可直接在三维点云地图中查找与目标点云数据中每一个点对应的最邻近点。目标点云数据中每一个点对应的最邻近点可指的是三维点云地图中，与目标点云数据中的每一个点距离最小的点，可获取查找到的各个最邻近点在三维点云地图中的三维点云数据，从而可得到与目标点云数据匹配的地图点云数据。

在本申请实施例中，目标点云数据可以包括每一聚类对象在世界坐标系下的平面点云数据和边缘点云数据，可以理解的是，从三维点云地图中，查找目标点云数据中每一个点对应的最邻近点，可以包括：从三维点云地图中，查找每一有效的聚类对象在世界坐标系下的平面点云数据及边缘点云数据中每一点对应的最邻近点。

在一些实施例中，从三维点云地图中，查找每一有效的聚类对象在世界坐标系下的平面点云数据及边缘点云数据中每一点对应的最邻近点，可以包括：根据每一有效的聚类对象在世界坐标系下的平面点云数据及边缘点云数据中每一点的位置信息，从三维点云地图中确定目标点；根据三维点云地图，构建二叉查找树；在二叉查找树中查找目标点的最近邻点。其中，目标点可以是：每一有效的聚类对象在世界坐标系下的平面点云数据及边缘点云数据中每一点在三维点云地图中的映射点。

在一些实施例中，在二叉查找树中查找目标点的最近邻点的算法可以包括但不限于radiusSearch(搜索半径)算法或nearestKSearch(近邻搜索)算法。需要说明的是，查找最邻近点也可采用其它算法，在此不作限定。

307、根据地图点云数据和目标点云数据，确定地图点云数据和目标点云数据之间的变换关系。

在一些实施例中，地图点云数据和目标点云数据之间的变换关系包括变换矩阵，根据地图点云数据和目标点云数据，确定地图点云数据和目标点云数据之间的变换关系可以包括：根据地图点云数据和目标点云数据，得到多个匹配的点云对，每一点云对包括目标点云数据中的一个点及地图点云数据中匹配的最近邻近点；根据该多个匹配的点云对，解算第一旋转矩阵和第一平移矩阵；对第一旋转矩阵和第一平移矩阵进行优化，得到第二旋转矩阵和第二平移矩阵，第二旋转矩阵和第二平移矩阵组成地图点云数据和目标点云数据之间的变换矩阵。

在一些实施例中，根据该多个匹配的点云对，解算第一旋转矩阵和第一平移矩阵，可以包括：根据误差函数处理该多个匹配的点云对，得到第一旋转矩阵和第一平移矩阵。

进一步的，误差函数表征的可以是该多个匹配的点云对之间的平均距离、最小距离、最大距离或距离平方和。可以理解的是，旋转矩阵和平移矩阵为该误差函数的自变量参数，误差函数取最小值时对应的旋转矩阵和平移矩阵分别为第一旋转矩阵和第一平移矩阵。

示例性的，若误差函数表征的是距离平方和，该误差函数可以表示为

其中，

和

为一组点云对，N_P指示的是点云对的个数，R指示第一旋转矩阵，T指示第一平移矩阵。

在一些实施例中，对第一旋转矩阵和第一平移矩阵进行优化，得到第二旋转矩阵和第二平移矩阵，可以包括：利用第一旋转矩阵和第一平移矩阵，对目标点云数据进行旋转和平移变换，得到第四点云数据；计算第四点云数据和地图点云数据之间的距离；若该距离小于第一距离阈值，则确定第一旋转矩阵为第二旋转矩阵，以及确定第一平移矩阵为第二平移矩阵；若该距离大于或等于第一距离阈值，则根据第四点云数据和地图点云数据，得到新的多个匹配的点云对，并继续执行上述根据该多个匹配的点云对，解算第一旋转矩阵和第一平移矩阵的步骤，直至第四点云数据和地图点云数据之间的距离小于第一距离阈值。可以理解的是，在得到和地图点云数据之间的距离小于第一距离阈值的第四点云数据时，则终止循环，以及将终止循环时最后解算出的第一旋转矩阵和第一平移矩阵确定为第二旋转矩阵和第二平移矩阵。

在一些实施例中，利用第一旋转矩阵和第一平移矩阵，对目标点云数据进行旋转和平移变换，得到第四点云数据可以包括：将目标点云数据与第一旋转矩阵及第一平移矩阵相乘，得到第四点云数据。可以理解的是，第四点云数据和地图点云数据之间的距离可以包括第四点云数据和地图点云数据之间的平均距离、最小距离及最大距离中的任一种，对应的第一距离阈值表征的可以是平均距离阈值、最小距离阈值或最大距离阈值。

基于以上描述，在一些实施例中，请参阅图3B，图3B本申请实施例公开的一实施例中确定地图点云数据和目标点云数据之间的变换关系的流程图，可以包括：

3071、根据地图点云数据和目标点云数据，得到多个匹配的点云对。

3072、根据该多个匹配的点云对，解算第一旋转矩阵和第一平移矩阵。

3073、利用第一旋转矩阵和第一平移矩阵，对目标点云数据进行旋转和平移变换，得到第四点云数据。

3074、计算第四点云数据和地图点云数据之间的距离。

3075、判断第四点云数据和地图点云数据之间的距离是否小于第一距离阈值，若是，执行步骤3076，若否，执行步骤3077。

3076、确定第一旋转矩阵为第二旋转矩阵，以及确定第一平移矩阵为第二平移矩阵；

3077、利用第四点云数据更新目标点云数据，并重复执行步骤3071-3075。

308、通过变换关系修正第一位姿信息，得到机器人在当前时刻对应的目标位姿信息。

在一些实施例中，第一位姿信息可以包括三维位置坐标和三维姿态坐标，通过变换关系修正第一位姿信息，得到机器人在当前时刻对应的目标位姿信息，可以包括：将第一位姿信息指示的三维位置坐标及三维姿态坐标，与第二旋转矩阵及第二平移矩阵相乘，得到机器人在当前时刻对应的目标位姿信息。

通过实施上述方法，机器人可以根据传感器采集的当前时刻对应的点云数据及三维点云地图，对通过里程计信息得到的当前时刻的位姿进行修正，由于三维点云地图可以较为精确地反映机器人工作环境中各物体的空间位置，因此，基于三维点云地图对通过里程计信息得到的当前时刻的位姿修正，可以得到较为精确的位姿，有利于提高机器人定位的准确性。进一步的，对当前时刻对应的点云数据进行聚类处理，不仅可以将微小物体当作噪音点处理掉，解决了微小物体的干扰问题，还可以通过聚类剔除掉离散的点，有效降低了待处理的点云数据的数量，进而有利于提高定位效率。

请参阅图4，图4是本申请实施例公开的一种机器人的定位装置的结构框图，可以包括：位姿确定模块401、点云转换模块402、查找模块403以及修正模块404；其中：

位姿确定模块401，用于根据惯性里程计采集的里程计信息，确定机器人当前时刻在世界坐标系下的第一位姿信息；

点云转换模块402，用于将通过传感器采集的点云数据转换至世界坐标系下，得到当前时刻对应的目标点云数据；

查找模块403，用于从三维点云地图中查找与目标点云数据匹配的地图点云数据；

修正模块404，用于根据目标点云数据及地图点云数据，对第一位姿信息进行修正，得到机器人在当前时刻对应的目标位姿信息。

在一些实施例中，点云转换模块402用于将通过传感器采集的点云数据转换至世界坐标系下，得到当前时刻对应的目标点云数据的方式具体可以包括：点云转换模块402，用于获取通过传感器采集的当前时刻对应的原始点云数据；对原始点云数据进行聚类处理，得到聚类成功的第一点云数据；提取第一点云数据中的特征信息，并根据该特征信息从第一点云数据中提取边缘点云和目标平面点云，以得到第二点云数据；将第二点云数据从传感器的传感器坐标系转换至世界坐标系，得到目标点云数据。

在一些实施例中，点云转换模块402用于将第二点云数据从传感器的传感器坐标系转换至世界坐标系，得到目标点云数据的方式具体可以包括：点云转换模块402，用于将第二点云数据由传感器的传感器坐标系转换为机器人坐标系，得到第三点云数据；根据第一位姿信息，将第三点云数据从机器人坐标系转换至世界坐标系，得到目标点云数据。

在一些实施例中，查找模块403用于从三维点云地图中查找与目标点云数据匹配的地图点云数据的方式具体可以包括：查找模块403，用于从三维点云地图中，查找目标点云数据中每一个点对应的最邻近点；根据每一最邻近点在三维点云地图中的位置信息，得到与目标点云数据匹配的地图点云数据。

在一些实施例中，修正模块404用于根据目标点云数据及地图点云数据，对第一位姿信息进行修正，得到机器人在当前时刻对应的目标位姿信息的方式具体可以包括：修正模块404，用于根据地图点云数据和目标点云数据，确定地图点云数据和目标点云数据之间的变换关系；通过该变换关系修正第一位姿信息，得到机器人在当前时刻对应的目标位姿信息。

在一些实施例中，地图点云数据和目标点云数据之间的变换关系包括变换矩阵，修正模块404用于根据地图点云数据和目标点云数据，确定地图点云数据和目标点云数据之间的变换关系的方式具体可以包括：修正模块404，用于根据地图点云数据和目标点云数据，得到多个匹配的点云对，每一点云对包括目标点云数据中的一个点及地图点云数据中匹配的最近邻近点；根据该多个匹配的点云对，解算第一旋转矩阵和第一平移矩阵；对第一旋转矩阵和第一平移矩阵进行优化，得到第二旋转矩阵和第二平移矩阵；第二旋转矩阵和第二平移矩阵组成地图点云数据和目标点云数据之间的变换矩阵。

在一些实施例中，修正模块404用于对第一旋转矩阵和第一平移矩阵进行优化，得到第二旋转矩阵和第二平移矩阵的方式具体可以包括：修正模块404，利用第一旋转矩阵和第一平移矩阵，对目标点云数据进行旋转和平移变换，得到第四点云数据；计算第四点云数据和地图点云数据之间的距离；若该距离小于第一距离阈值，则确定第一旋转矩阵为第二旋转矩阵，以及确定第一平移矩阵为第二平移矩阵；若该距离大于或等于第一距离阈值，则根据第四点云数据和地图点云数据，得到新的多个匹配的点云对，并继续执行根据该多个匹配的点云对，解算第一旋转矩阵和第一平移矩阵的步骤，直至第四点云数据和地图点云数据之间的距离小于第一距离阈值。

在一些实施例中，第一位姿信息包括三维位置坐标及三维姿态坐标，修正模块404用于通过变换关系修正第一位姿信息，得到机器人当前时刻对应的目标位姿信息的方式具体可以包括：修正模块404，用于将三维位置坐标及三维姿态坐标，与第二旋转矩阵及第二平移矩阵相乘，得到机器人在当前时刻对应的目标位姿信息。

在一些实施例中，传感器可以包括多线激光雷达、双目相机及毫米波雷达中的任一种。

请参阅图5，图5是本申请实施例公开的一种机器人的结构框图。如图5所示，机器人可以包括一个或多个如下部件：处理器501、与处理器501耦合的存储器502，其中存储器502可存储有一个或多个计算机程序。

处理器501可以包括一个或者多个处理核。处理器501利用各种接口和线路连接整个机器人内的各个部分，通过运行或执行存储在存储器502内的指令、程序、代码集或指令集，以及调用存储在存储器502内的数据，执行机器人的各种功能和处理数据。可选地，处理器501可以采用数字信号处理(Digital Signal Processing，DSP)、现场可编程门阵列(Field－Programmable Gate Array，FPGA)、可编程逻辑阵列(Programmable LogicArray，PLA)中的至少一种硬件形式来实现。处理器501可集成中央处理器(CentralProcessing Unit，CPU)、图像处理器(Graphics Processing Unit，GPU)和调制解调器等中的一种或几种的组合。其中，CPU主要处理操作系统、用户界面和应用程序等；GPU用于负责显示内容的渲染和绘制；调制解调器用于处理无线通信。可以理解的是，上述调制解调器也可以不集成到处理器501中，单独通过一块通信芯片进行实现。

存储器502可以包括随机存储器(Random Access Memory，RAM)，也可以包括只读存储器(Read-Only Memory，ROM)。存储器502可用于存储指令、程序、代码、代码集或指令集。存储器502可包括存储程序区和存储数据区，其中，存储程序区可存储用于实现操作系统的指令、用于实现至少一个功能的指令(比如触控功能、声音播放功能、图像播放功能等)、用于实现上述各个方法实施例的指令等。存储数据区还可以存储机器人在使用中所创建的数据等。

可以理解地，机器人的定位装置可包括比上述结构框图中更多或更少的结构元件，例如，包括电源模块、物理按键、蓝牙模块等，在此不进行限定。

在本申请实施例中，处理器501还具有以下功能：

根据惯性里程计采集的里程计信息，确定机器人当前时刻在世界坐标系下的第一位姿信息；

将通过传感器采集的点云数据转换至世界坐标系下，得到当前时刻对应的目标点云数据；

从三维点云地图中查找与目标点云数据匹配的地图点云数据；

根据目标点云数据及地图点云数据，对第一位姿信息进行修正，得到机器人在当前时刻对应的目标位姿信息。

可选的，处理器501还具有以下功能：

获取通过传感器采集的当前时刻对应的原始点云数据；对原始点云数据进行聚类处理，得到聚类成功的第一点云数据；提取第一点云数据中的特征信息，并根据该特征信息从第一点云数据中提取边缘点云和目标平面点云，以得到第二点云数据；将第二点云数据从传感器的传感器坐标系转换至世界坐标系，得到目标点云数据。

可选的，处理器501还具有以下功能：

将第二点云数据由传感器的传感器坐标系转换为机器人坐标系，得到第三点云数据；根据第一位姿信息，将第三点云数据从机器人坐标系转换至世界坐标系，得到目标点云数据。

可选的，处理器501还具有以下功能：

从三维点云地图中，查找目标点云数据中每一个点对应的最邻近点；根据每一最邻近点在三维点云地图中的位置信息，得到与目标点云数据匹配的地图点云数据。

可选的，处理器501还具有以下功能：

根据地图点云数据和目标点云数据，确定地图点云数据和目标点云数据之间的变换关系；通过该变换关系修正第一位姿信息，得到机器人在当前时刻对应的目标位姿信息。

可选的，地图点云数据和目标点云数据之间的变换关系包括变换矩阵，处理器501还具有以下功能：

根据地图点云数据和目标点云数据，得到多个匹配的点云对，每一点云对包括目标点云数据中的一个点及地图点云数据中匹配的最近邻近点；根据该多个匹配的点云对，解算第一旋转矩阵和第一平移矩阵；对第一旋转矩阵和第一平移矩阵进行优化，得到第二旋转矩阵和第二平移矩阵，第二旋转矩阵和第二平移矩阵组成地图点云数据和目标点云数据之间的变换矩阵。

可选的，处理器501还具有以下功能：

利用第一旋转矩阵和第一平移矩阵，对目标点云数据进行旋转和平移变换，得到第四点云数据；计算第四点云数据和地图点云数据之间的距离；若该距离小于第一距离阈值，则确定第一旋转矩阵为第二旋转矩阵，以及确定第一平移矩阵为第二平移矩阵；若该距离大于或等于第一距离阈值，则根据第四点云数据和地图点云数据，得到新的多个匹配的点云对，并继续执行上述根据该多个匹配的点云对，解算第一旋转矩阵和第一平移矩阵的步骤，直至第四点云数据和地图点云数据之间的距离小于第一距离阈值。

可选的，第一位姿信息可以包括三维位置坐标及三维姿态坐标，处理器501还具有以下功能：

将三维位置坐标及三维姿态坐标，与第二旋转矩阵及第二平移矩阵相乘，得到机器人在当前时刻对应的目标位姿信息。

本申请实施例公开一种计算机可读存储介质，其存储计算机程序，其中，该计算机程序被处理器执行时实现如上述各实施例描述的方法。

本申请实施例公开一种计算机程序产品，该计算机程序产品包括存储了计算机程序的非瞬时性计算机可读存储介质，且该计算机程序可被处理器执行时实现如上述各实施例描述的方法。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一非易失性计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，该存储介质可为磁碟、光盘、ROM等。

如此处所使用的对存储器、存储、数据库或其它介质的任何引用可包括非易失性和/或易失性存储器。合适的非易失性存储器可包括ROM、可编程ROM(Programmable ROM，PROM)、可擦除PROM(Erasable PROM，EPROM)、电可擦除PROM(Electrically ErasablePROM，EEPROM)或闪存。易失性存储器可包括随机存取存储器(random access memory，RAM)，它用作外部高速缓冲存储器。作为说明而非局限，RAM可为多种形式，诸如静态RAM(Static RAM，SRAM)、动态RAM(Dynamic Random Access Memory，DRAM)、同步DRAM(synchronous DRAM，SDRAM)、双倍数据率SDRAM(Double Data Rate SDRAM，DDR SDRAM)、增强型SDRAM(Enhanced Synchronous DRAM，ESDRAM)、同步链路DRAM(Synchlink DRAM，SLDRAM)、存储器总线直接RAM(Rambus DRAM，RDRAM)及直接存储器总线动态RAM(DirectRambus DRAM，DRDRAM)。

应理解，说明书通篇中提到的“一个实施例”或“一实施例”意味着与实施例有关的特定特征、结构或特性包括在本申请的至少一个实施例中。因此，在整个说明书各处出现的“在一个实施例中”或“在一实施例中”未必一定指相同的实施例。此外，这些特定特征、结构或特性可以以任意适合的方式结合在一个或多个实施例中。本领域技术人员也应该知悉，说明书中所描述的实施例均属于可选实施例，所涉及的动作和模块并不一定是本申请所必须的。

在本申请的各种实施例中，应理解，上述各过程的序号的大小并不意味着执行顺序的必然先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。

在本申请各实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

上述集成的单元若以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可获取的存储器中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或者部分，可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储器中，包括若干请求用以使得一台计算机设备(可以为个人计算机、服务器或者网络设备等，具体可以是计算机设备中的处理器)执行本申请的各个实施例上述方法的部分或全部步骤。

以上对本申请实施例公开的一种机器人的定位方法、装置、机器人以及存储介质进行了详细介绍，本文中应用了具体个例对本申请的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本申请的方法及其核心思想。同时，对于本领域的一般技术人员，依据本申请的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本申请的限制。

Claims (12)

1.一种机器人的定位方法，其特征在于，所述机器人包括惯性里程计及用于进行环境感知的传感器，所述方法包括：

根据所述惯性里程计采集的里程计信息，确定所述机器人当前时刻在世界坐标系下的第一位姿信息；

将通过所述传感器采集的点云数据转换至所述世界坐标系下，得到所述当前时刻对应的目标点云数据；

从三维点云地图中查找与所述目标点云数据匹配的地图点云数据；

根据所述目标点云数据及地图点云数据，对所述第一位姿信息进行修正，得到所述机器人在所述当前时刻对应的目标位姿信息。

2.根据权利要求1所述的方法，其特征在于，所述将通过所述传感器采集的点云数据转换至所述世界坐标系下，得到所述当前时刻对应的目标点云数据，包括：

获取通过所述传感器采集的当前时刻对应的原始点云数据；

对所述原始点云数据进行聚类处理，得到聚类成功的第一点云数据；

提取所述第一点云数据中的特征信息，并根据所述特征信息从所述第一点云数据中提取边缘点云和目标平面点云，以得到第二点云数据；

将所述第二点云数据从所述传感器的传感器坐标系转换至所述世界坐标系，得到目标点云数据。

3.根据权利要求2所述的方法，其特征在于，所述将所述第二点云数据由所述传感器的传感器坐标系转换为世界坐标系，得到目标点云数据，包括：

将所述第二点云数据由所述传感器的传感器坐标系转换为机器人坐标系，得到第三点云数据；

根据所述第一位姿信息，将所述第三点云数据从所述机器人坐标系转换至所述世界坐标系，得到目标点云数据。

4.根据权利要求1-3任一项所述的方法，其特征在于，所述从三维点云地图中查找与所述目标点云数据匹配的地图点云数据，包括：

从三维点云地图中，查找所述目标点云数据中每一个点对应的最邻近点；

根据每一所述最邻近点在所述三维点云地图中的位置信息，得到与所述目标点云数据匹配的地图点云数据。

5.根据权利要求1-3任一所述的方法，其特征在于，所述根据所述目标点云数据及地图点云数据，对所述第一位姿信息进行修正，得到所述机器人在所述当前时刻对应的目标位姿信息，包括：

根据所述地图点云数据和所述目标点云数据，确定所述地图点云数据和所述目标点云数据之间的变换关系；

通过所述变换关系修正所述第一位姿信息，得到所述机器人在所述当前时刻对应的目标位姿信息。

6.根据权利要求5所述的方法，其特征在于，所述变换关系包括变换矩阵，所述根据所述地图点云数据和所述目标点云数据，确定所述地图点云数据和所述目标点云数据之间的变换关系，包括：

根据所述地图点云数据和所述目标点云数据，得到多个匹配的点云对，每一所述点云对包括所述目标点云数据中的一个点及所述地图点云数据中匹配的最近邻近点；

根据所述多个匹配的点云对，解算第一旋转矩阵和第一平移矩阵；

对所述第一旋转矩阵和所述第一平移矩阵进行优化，得到第二旋转矩阵和第二平移矩阵，所述第二旋转矩阵和所述第二平移矩阵组成所述变换矩阵。

7.根据权利要求6所述的方法，其特征在于，所述对所述第一旋转矩阵和所述第一平移矩阵进行优化，得到第二旋转矩阵和第二平移矩阵，包括：

利用所述第一旋转矩阵和所述第一平移矩阵，对所述目标点云数据进行旋转和平移变换，得到第四点云数据；

计算所述第四点云数据和所述地图点云数据之间的距离；

若所述距离小于第一距离阈值，则确定所述第一旋转矩阵为第二旋转矩阵，以及确定所述第一平移矩阵为第二平移矩阵；

若所述距离大于或等于所述第一距离阈值，则根据所述第四点云数据和所述地图点云数据，得到新的多个匹配的点云对，并继续执行所述根据所述多个匹配的点云对，解算第一旋转矩阵和第一平移矩阵的步骤，直至所述距离小于所述第一距离阈值。

8.根据权利要求6所述的方法，其特征在于，所述第一位姿信息包括三维位置坐标及三维姿态坐标，所述通过所述变换关系修正所述第一位姿信息，得到所述机器人当前时刻对应的目标位姿信息，包括：

将所述三维位置坐标及三维姿态坐标，与所述第二旋转矩阵及所述第二平移矩阵相乘，得到所述机器人在所述当前时刻对应的目标位姿信息。

9.根据权利要求1～3、5～6任一项所述的方法，其特征在于，所述传感器包括多线激光雷达、双目相机及毫米波雷达中的一种或多种。

10.一种机器人的定位装置，其特征在于，所述机器人包括惯性里程计及用于进行环境感知的传感器，所述装置包括：

位姿确定模块，用于根据所述惯性里程计采集的里程计信息，确定所述机器人当前时刻在世界坐标系下的第一位姿信息；

点云转换模块，用于将通过所述传感器采集的点云数据转换至所述世界坐标系下，得到所述当前时刻对应的目标点云数据；

查找模块，用于从三维点云地图中查找与所述目标点云数据匹配的地图点云数据；

修正模块，用于根据所述目标点云数据及地图点云数据，对所述第一位姿信息进行修正，得到所述机器人在所述当前时刻对应的目标位姿信息。

11.一种机器人，其特征在于，包括：

存储有可执行程序代码的存储器；

以及所述存储器耦合的处理器；

所述处理器调用所述存储器中存储的所述可执行程序代码，所述可执行程序代码被所述处理器执行时，使得所述处理器实现如权利要求1-9中任一所述的方法。

12.一种计算机可读存储介质，其上存储有可执行程序代码，其特征在于，所述可执行程序代码被处理器执行时，实现如权利要求1-9中任一所述的方法。

Images