CN111383261B - 移动机器人、及其位姿估计方法和位姿估计装置 - Google Patents

Abstract

一种移动机器人、及其位姿估计方法和位姿估计装置，其中，所述位姿估计方法包括：通过一基于一TOF(Time of Flight，飞行时间)深度相机设备获取一场景的第一帧点云和第二帧点云，其中，所述移动机器人具有一初始位姿；构建一第一分辨率栅格地图和一第二分辨率栅格地图，其中，第二分辨率大于第一分辨率；以所述初始位姿为中心，以一预设半径，构建一搜索空间；基于一第一分辨率栅格地图和所述搜索空间对第二帧点云进行配准，获取一第一搜索位置；基于所述第一搜索位置和第二分辨率栅格地图对第二帧点云进行配准，获取估计位姿。这样，提高了移动机器人的定位精确度以及降低了算法复杂度。

Description

移动机器人、及其位姿估计方法和位姿估计装置

技术领域

本发明涉及深度视觉领域，尤其涉及一种移动机器人、及其位姿估计方法和位姿估计装置。

背景技术

移动机器人逐渐称为目前科学技术发展最活跃的领域之一。移动机器人是将环境感知、动态决策与规划、行为控制于执行等多功能于一体的综合系统。移动机器人主要应用到传感器技术、信息处理技术、电子工程技术计算机工程、自动化控制工程以及人工智能等多学科的研究成果，代表机电一体化的最高成就。随着移动机器人性能的不断完善，移动机器人的应用范围不断扩展，其不仅在工业、农业、医疗、服务等行业中得到广泛的应用，还在城市安全、国防和空间探测领域得到很好的应用。

移动机器人为了实现自主移动，首先要具备感知所在环境和对自身位置和位姿进行准确估计的功能。现有的移动机器人主要应用基于脉冲或相位法的激光传感器进行估计。然而，激光传感器的成本普遍较高。因此，基于脉冲或相位法的激光传感器进行识别和估计的方式，会增加移动机器人的制造成本。同时，激光传感器并不能提供图像信息，以使得对移动机器人的位姿的识别和估计存在一定局限性。

现有的一些移动机器人还采用基于多个单点式TOF(Time ofFlight，飞行时间)相机通过逐点扫描进行位姿识别和估计。这种识别和估计方式，虽然通过单点式TOF相机能够提供图像信息，但是依然存在一些缺陷，导致移动机器人的性能受到限制，主要体现在以下几个方面：

首先，单点式TOF相机的视场角较小，水平方向一般只有70度，因此，单点式TOF相机扫描的范围较小，这将会限制移动机器人的扫描场景的范围；

其次，通过多个单点式TOF相机进行逐点扫描之后，对获取的图像进行处理和计算的过程十分复杂，计算量也比较大；

最后，在多个单点式TOF相机进行逐点扫描的过程中，单点式TOF相机对光照比较敏感，因此，通过单点式TOF相机进行逐点扫描的识别估计方式稳定性较差。

据此，用于识别和估计的硬件设备对整个移动机器人的性能具有重要影响。

发明内容

本发明的一个目的在于提供一种移动机器人，及其位姿估计方法和位姿估计装置，其中，所述位姿估计方法通过一TOF深度相机设备对所述移动机器人进行位姿估计，其中所述TOF深度相机设备能够发出一面阵光，以使得移动机器人可以通过所述面阵光获取场景的点云图像。

本发明的另一个目的在于提供一种移动机器人，及其位姿估计方法和位姿估计装置，其中，所述位姿估计方法通过所述TOF深度相机设备获取所述场景的点云图像，其中，所述激光雷达水平方向的视场角是125度，以使得移动机器人可以扫描较大范围的场景。

本发明的另一个目的在于提供一种移动机器人，及其位姿估计方法和位姿估计装置，其中，所述位姿估计方法通过所述TOF深度相机设备获取所述场景的点云图像，其中，所述TOF深度相机设备受环境光照影响较小，从而提升了移动机器人识别估计功能的稳定性。

本发明的另一个目的在于提供一种移动机器人，及其位姿估计方法和位姿估计装置，其中，所述位姿估计方法可以对获取的场景点云进行快速准确的配准，以对所述移动机器人的位姿进行精准估计。

本发明的另一个目的在于提供一种移动机器人，及其位姿估计方法和位姿估计装置，其中，所述位姿估计方法采用多分辨率的配准方式，在确保精度的基础上有效地降低了算法的复杂度。

本发明的另一个目的在于提供一种移动机器人，及其位姿估计方法和位姿估计装置，其中，所述移动机器人采用TOF深度相机设备进行位姿估计，可以降低制造成本。

为了实现上述至少一发明目的，本发明提供了一种移动机器人的位姿估计方法，包括：

通过一基于一TOF(Time ofFlight，飞行时间)深度相机设备获取一场景的第一帧点云和第二帧点云，其中，所述TOF深度相机设备通过发射面阵光对所述场景进行扫描，以获得所述场景的点云，以及，所述移动机器人具有一初始位姿；

以具有第一分辨率的高斯概率栅格对所述第一帧点云进行处理，以获得一第一高斯概率栅格地图，其中，所述第一高斯概率栅格地图用于获取所述第二帧点云中各点云的概率得分；

以具有第二分辨率的高斯概率栅格对所述第一帧点云进行处理，以获得第二高斯概率栅格地图，其中，第二分辨率大于第一分辨率，所述第二高斯概率栅格地图用于获取所述第二帧点云中各点云的概率得分；

以所述初始位姿为中心，构建具有预设半径的搜索空间；

基于所述初始位姿，对第二帧点云进行旋转变换和平移变换，以生成一第一变换点云；

以第一搜索步长对所述第一变换点云在以所述初始位姿为中心的所述搜索空间内进行处理，以获得一系列第一搜索点云；

遍历各所述第一搜索点云，并基于所述第一高斯概率栅格地图获得各搜索点云对应的总概率得分；

确定各所述第一搜索点云中总概率分值最大值对应的所述第一搜索点云的搜索位置为第一搜索位置，所述第一搜索位置对应的位姿为第二位姿；

基于所述第二位姿对所述第二帧点云进行旋转变换和平移变换，以生成一第二变换点云；

以第二搜索步长对所述第二变换点云在以所述第二位姿为中心的搜索空间进行处理，以获得一系列第二搜索点云；

遍历所述第二搜索点云，并基于所述第二高斯概率栅格地图获得各第二搜索点云对应总概率得分；以及

确定各所述第二搜索点云中总概率分值最大值对应的第二搜索点云的搜索位置为目标位置，其中，所述目标位置对应的位姿为所述移动机器人的估计位姿。

根据本发明的一个实施例，所述TOF深度相机设备水平方向的视场角是125度。

根据本发明的一个实施例，所述第一高斯概率栅格地图，具有一第一分辨率得分函数，用于获取所述第二帧点云中各点云的概率得分，其中，所述第二分辨率栅格地图具有一第二分辨率得分函数，用于获取所述第二帧点云中各点云的概率得分。

根据本发明的一个实施例，以所述初始位姿为中心，以所述初始位姿为中心，构建具有预设半径的搜索空间步骤包括：

基于该场景的丰富程度，获取所述预设半径。

根据本发明的一个实施例，通过一基于一TOF(Time ofFlight，飞行时间)深度相机设备获取一场景的第一帧点云和第二帧点云，其中，所述TOF深度相机设备通过发射面阵光对所述场景进行扫描，以获得所述场景的点云，以及，所述移动机器人具有一初始位姿步骤，包括：

基于所述移动机器人的一惯性传感器，获取所述初始位姿。

依本发明的另一个方面，本发明进一步提供一种移动机器人的位姿估计装置，包括：

一场景点云获取单元，用于通过一基于一TOF深度相机设备获取一场景的第一帧点云和第二帧点云，其中，所述TOF深度相机设备通过发射面阵光对所述场景进行扫描，以获得所述场景的点云，以及，基于所述移动机器人的一惯性传感器，获取所述移动机器人的一初始位姿；

一地图构建单元，用于构建一第一分辨率栅格地图和一第二分辨率栅格地图，其中，所述第二分辨率大于所述第一分辨率；

一搜索空间构建单元，用于以所述初始位姿为中心，构建一预设半径的搜索空间，其中，基于该场景的丰富程度，获取所述预设半径；

一第一分辨率搜索单元，用于基于所述初始位姿，对第二帧点云进行旋转和平移变换，以生成一第一变换点云，和，以第一搜索步长对所述第一变换点云在以所述初始位姿为中心的所述搜索空间内进行处理，以获得一系列第一搜索点云，和遍历各所述第一搜索点云，并基于所述第一高斯概率栅格地图获得各搜索点云对应的总概率得分；和确定各所述第一搜索点云中总概率分值最大值对应的所述第一搜索点云的搜索位置为第一搜索位置，所述第一搜索位置对应的位姿为第二位姿；以及

一第二分辨率搜索单元，用于基于所述第二位姿对所述第二帧点云进行旋转变换和平移变换，以生成一第二变换点云，和以第二搜索步长对所述第二变换点云在以所述第二位姿为中心的搜索空间进行处理，以获得一系列第二搜索点云，和遍历所述第二搜索点云，并基于所述第二高斯概率栅格地图获得各第二搜索点云对应总概率得分，和确定各所述第二搜索点云中总概率分值最大值对应的第二搜索点云的搜索位置为目标位置，其中，所述目标位置对应的位姿为所述移动机器人的估计位姿。

根据本发明的一个实施例，所述TOF深度相机设备的水平视场角是125度。

根据本发明的一个实施例，所述第一高斯概率栅格地图，具有一第二分辨率概率得分函数，用于基于所述第一分辨率获取所述第一帧点云中各点云的一概率得分，其中，所述第二分辨率栅格地图具有一第二分辨率得分函数，用于基于所述第二分辨率获取所述第一帧点云中各点云的概率得分。

依本发明的另一个方面，本发明进一步提供一种移动机器人，包括：

一移动系统；

一TOF深度相机设备；

一处理设备，所述处理设备包括处理器和存储器，在所述存储器中存储有计算机程序指令，所述计算机程序指令在被所述处理器运行时使得所述处理器执行如上所述的基于TOF深度相机设备的移动机器人的位姿估计方法。

依本发明的另一个方面，本发明进一步提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序指令，当所述计算机程序指令被计算装置执行时，可操作来执行如上所述的移动机器人的位姿估计方法。

依本发明的另一个方面，本发明进一步提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序指令，当所述计算机程序指令被计算装置执行时，可操作来执行如上所述的移动机器人的位姿估计方法。

通过对随后的描述和附图的理解，本发明进一步的目的和优势将得以充分体现。

本发明的这些和其它目的、特点和优势，通过下述的详细说明，附图和权利要求得以充分体现。

附图说明

图1是根据本发明的一个较佳实施例的所述移动机器人的位姿估计方法的框图示意图。

图2是根据本发明的一个较佳实施例的所述移动机器人的位姿估计装置的示意性框图。

图3是根据本发明的一个较佳实施例的所述移动机器人的示意性框图。

具体实施方式

以下描述用于揭露本发明以使本领域技术人员能够实现本发明。以下描述中的优选实施例只作为举例，本领域技术人员可以想到其他显而易见的变型。在以下描述中界定的本发明的基本原理可以应用于其他实施方案、变形方案、改进方案、等同方案以及没有背离本发明的精神和范围的其他技术方案。

本领域技术人员应理解的是，在本发明的揭露中，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系是基于附图所示的方位或位置关系，其仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此上述术语不能理解为对本发明的限制。

可以理解的是，术语“一”应理解为“至少一”或“一个或多个”，即在一个实施例中，一个元件的数量可以为一个，而在另外的实施例中，该元件的数量可以为多个，术语“一”不能理解为对数量的限制。

如上所述，现有的移动机器人估计方法，只能通过多个单点式TOF相机对场景进行逐点扫描，然而，单点式的TOF相机的视场角最大只有70度，这将限制了单点式TOF相机扫描场景的范围。也就是说，单点式TOF相机水平方向的视场角只有70度，也只能扫描水平方向70度范围内的场景。扫描的场景范围的大小，也会影响到估计的准确性。逐点扫描的方式也会增加估计算法的复杂度。另外，逐点扫描的单点式TOF相机受场景中光照影响较大，从而导致移动机器人在具有光照的场景中，估计装置的稳定性较差。

如图1所示，根据本发明的一个较佳实施例的所述移动机器人的位姿估计方法的框图示意图。所述移动机器人的位姿估计方法100包括：通过一基于一TOF(Time ofFlight，飞行时间)深度相机设备获取一场景的第一帧点云和第二帧点云，其中，所述TOF深度相机设备通过发射面阵光对所述场景进行扫描，以获得所述场景的点云，以及，所述移动机器人具有一初始位姿101；以具有第一分辨率的高斯概率栅格对所述第一帧点云进行处理，以获得一第一高斯概率栅格地图，其中，所述第一高斯概率栅格地图用于获取所述第二帧点云中各点云的概率得分102；以具有第二分辨率的高斯概率栅格对所述第一帧点云进行处理，以获得第二高斯概率栅格地图，其中，第二分辨率大于第一分辨率，所述第二高斯概率栅格地图用于获取所述第二帧点云中各点云的概率得分103；以所述初始位姿为中心，构建具有预设半径的搜索空间104；基于所述初始位姿，对第二帧点云进行旋转变换和平移变换，以生成一第一变换点云105；以第一搜索步长对所述第一变换点云在以所述初始位姿为中心的所述搜索空间内进行处理，以获得一系列第一搜索点云106；遍历各所述第一搜索点云，并基于所述第一高斯概率栅格地图获得各搜索点云对应的总概率得分107；确定各所述第一搜索点云中总概率分值最大值对应的所述第一搜索点云的搜索位置为第一搜索位置，所述第一搜索位置对应的位姿为第二位姿108；基于所述第二位姿对所述第二帧点云进行旋转变换和平移变换，以生成一第二变换点云109；以第二搜索步长对所述第二变换点云在以所述第二位姿为中心的搜索空间进行处理，以获得一系列第二搜索点云110；遍历所述第二搜索点云，并基于所述第二高斯概率栅格地图获得各第二搜索点云对应总概率得分111；以及，确定各所述第二搜索点云中总概率分值最大值对应的第二搜索点云的搜索位置为目标位置，其中，所述目标位置对应的位姿为所述移动机器人的估计位姿112。

较佳地，所述TOF深度相机设备通过发出一面阵光，获取所述场景的所述第一帧点云，优势主要体现在以下方面：首先，TOF深度相机设备不受场景中的光照影响，因此，基于TOF深度相机设备的估计装置稳定性较好。其次，发出面阵光的所述TOF深度相机设备的视场角最大是125度，远大于单点式TOF相机的70度，因此，TOF深度相机设备可以扫描更大范围的场景，从而增强估计的准确性。

更进一步地，如上所述，采用激光传感器会导致移动机器人的制造成本增加。值得一提的是，所述TOF深度相机设备的成本较低，相比之于采用激光传感器的移动机器人，采用TOF深度相机设备的移动机器人在提升性能的基础上可以降低移动机器人的制造成本。

在本实施例的一种可能的实现方式中，所述第一高斯概率栅格地图，具有一第二分辨率概率得分函数，用于基于所述第一分辨率获取所述第一帧点云中各点云的一概率得分，其中，所述第二分辨率栅格地图具有一第二分辨率得分函数，用于基于所述第二分辨率获取所述第一帧点云中各点云的概率得分。

也就是说，当所述场景的所述第二帧点云的任一点云落入所述第一分辨率栅格地图的任一栅格时，可以获取一个与这个点云对应的第一分辨率得分，其中，所述得分是概率得分，代表所述场景的所述第一帧点云中物体占据栅格的概率，得分越高，物体占据栅格的概率就越高，得分越小，物体占据栅格的概率就越小。所述第二分辨率得分同理。通过所述第一分辨率得分函数和所述第二分辨率得分函数获取的所述概率得分，是衡量估计准确的一个标准，得分越高，估计的效果就越理想。

所述步骤104中，构建所述搜索空间的半径，是一预设距离。所述预设距离由场景的丰富程度决定。当场景的丰富程度越高时，所述预设距离，也就是所述搜索空间的半径越小；当场景的丰富程度越低时，所述搜索空间的半径越大。例如，当所述移动机器人位于摆放物品较多的室内环境时，所述场景的丰富程度就比较高，此时，所述搜索空间的半径可以设置为1米；当所述移动机器人位于空旷的室外环境时，所述场景的丰富程度就较低，此时，所述搜索空间的半径可以设置为3米。另外，构建适当的所述搜索空间，可以使移动机器人的估计更加准确。

在本实施例的一种可能的实现方式中，所述第一分辨率步长，被设置为所述第一分辨率栅格的一半。在所述步骤101中，所述移动机器人的初始位姿通过一惯性传感器被获取，在二维坐标系中包括三个参数，分别是基于世界坐标系的横坐标，纵坐标和角坐标，其中，所述角坐标由横坐标和纵坐标基于余弦定理获取。本领域技术人员应理解，在三维坐标系中，所述初始位姿则包括六个参数。

以二维坐标系为例(三维坐标系同理)，设所述移动机器人的初始位姿为p(x1，y1，θ1)，所述搜索空间是以所述初始位姿为p(x1，y1，θ1)为中心，(x2，y2，θ2)为半径构建的三维空间，其中，所述第一分辨率步长为(x3，y3，θ3)。如步骤105所述，基于所述搜索空间，和所述第一分辨率步长(x3，y3，θ3)，对所述初始位姿p(x1，y1，θ1)进行平移和旋转，获取所述第一变换点云的变换矩阵T1，以及所述第一变换点云的位姿p1。另外，基于所述第一分辨率得分函数，获取与所述第变换点云的位姿p1对应的所述第一分辨率得分。

如步骤106至108所述，基于所述搜索空间和所述第一分辨率步长(x3，y3，θ3)，对所述第一变换点云进行平移和旋转，以获得一系列的第一搜索点云(p1,p2,…,pi)，和一组对应的变换矩阵(T1,T2,…,Ti)，其中，每一个点云pi都对应一个第一分辨率概率得分。当遍历所述点云(p1,p2,…,pi)之后，累加所述第一分辨率得分，获取一总概率得分。对所述总概率得分进行标准化处理，获取一标准得分。当所述标准得分大于一第一阈值时，获取对应的变换矩阵设为Tk，生成对应的一第一搜索位置，此时获取的点云设为pk，pk的位姿被设置为第二位姿,其中，所述Tk是扰动变换矩阵。

所述第二分辨率步长被设置为(x4，y4，θ4)，如步骤109至步骤111所述，基于所述pk与Tk对应的所述第一搜索位置，以所述(x4，y4，θ4)为步长，对所述pk进行旋转和平移，获取所述第二变换点云的变换矩阵Tk1，和对应的所述第二变换点云pk1，以及，基于所述第二分辨率概率得分函数，获取一个第二分辨率得分。

同样地，基于与所述pk对应的第一搜索位置和所述第二分辨率步长(x4，y4，θ4)，对上一步获取的位姿进行平移和旋转，从而获取一组变换矩阵(Tk1,Tk2,…，Tkn)以及一系列第二搜索点云(pk1,pk2,…,pkn)。当遍历所述一系列第二搜索点云(pk1,pk2,…,pkn)之后，累加所述第二分辨率得分，获取一总概率得分。对所述总概率得分进行标准化处理，获取一标准得分。当所述标准得分大于一第二阈值时，获取对应的转换矩阵Tkk以及移动机器人的估计位姿pkk。

较佳地，本发明通过所述TOF深度相机设备，以及多分辨率的配准算法，提供了一种精度较高且算法复杂度较小的移动机器人的位姿估计方法，其中，所述配准方法通过构建多分辨率搜索空间，将与所述场景的所述第一帧点云的配准分为两个阶段，从而提升了配准的准确性，以及降低了算法的复杂度，从而实现所述移动机器人快速准确的估计。

在本实施例的一种可能的实现方式中，在获取所述第一分辨率位姿、和所述第二分辨率位姿的过程中，应用ICP(Iterative Closest Point，迭代最近点)算法。当然，也可以通过其他算法获取所述第一变换点云的位姿、和所述第二变换点云的位姿。点云位姿的变换算法已经发展成熟，本发明在此不做限制。

如图2所示，根据本发明的一个较佳实施例的移动机器人的位姿估计装置得以被阐明。所述位姿估计装置200包括：一场景点云获取单元201，用于通过一基于一TOF深度相机设备获取一场景的第一帧点云和第二帧点云，其中，所述TOF深度相机设备通过发射面阵光对所述场景进行扫描，以获得所述场景的点云，以及，基于所述移动机器人的一惯性传感器，获取所述移动机器人的一初始位姿；一地图构建单元202，用于构建一第一分辨率栅格地图和一第二分辨率栅格地图，其中，所述第二分辨率大于所述第一分辨率；一搜索空间构建单元203，用于以所述初始位姿为中心，构建一预设半径的搜索空间，其中，基于该场景的丰富程度，获取所述预设半径；一第一分辨率搜索单元204，用于基于所述初始位姿，对第二帧点云进行旋转和平移变换，以生成一第一变换点云，和，以第一搜索步长对所述第一变换点云在以所述初始位姿为中心的所述搜索空间内进行处理，以获得一系列第一搜索点云，和遍历各所述第一搜索点云，并基于所述第一高斯概率栅格地图获得各搜索点云对应的总概率得分；和确定各所述第一搜索点云中总概率分值最大值对应的所述第一搜索点云的搜索位置为第一搜索位置，所述第一搜索位置对应的位姿为第二位姿；一第二分辨率搜索单元205，用于基于所述第二位姿对所述第二帧点云进行旋转变换和平移变换，以生成一第二变换点云，和以第二搜索步长对所述第二变换点云在以所述第二位姿为中心的搜索空间进行处理，以获得一系列第二搜索点云，和遍历所述第二搜索点云，并基于所述第二高斯概率栅格地图获得各第二搜索点云对应总概率得分，和确定各所述第二搜索点云中总概率分值最大值对应的第二搜索点云的搜索位置为目标位置，其中，所述目标位置对应的位姿为所述移动机器人的估计位姿。

所述地图构建单元202进一步用于，提供一第二分辨率概率得分函数，与所述第二分辨率栅格地图相对应，以及，用于提供一第一分辨率概率得分函数，与所述第一分辨率栅格地图相对应。

所述搜索空间构建单元203进一步用于，基于该场景的丰富程度，获取一预设距离。所述场景点云获取单元201还用于，基于所述移动机器人的一惯性传感器，获取所述初始位姿。

图3图示了根据本发明的一个较佳实施例的移动机器人。如图3所示，根据本发明的一个较佳实施例的移动机器人的位姿估计装置300，包括：一移动系统310；一TOF深度相机设备320，以及，一处理设备330，包括一处理器331和一存储器332，在所述存储器332中存储有一计算机程序指令，所述计算机程序指令在被所述处理器331运行时使得所述处理器331执行如上所述的移动机器人的位姿估计方法100。

所述处理器331可以是中央处理单元(CPU)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制所述移动机器人的位姿估计装置300中的其他组件以执行期望的功能。

所述存储器332可以包括一个或多个计算机程序产品，所述计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(cache)等。所述非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。在所述计算机可读存储介质上可以存储一个或多个计算机程序指令，处理器321可以运行所述程序指令，以实现上文所述的本发明的各个实施例的移动机器人的位姿估计方法100以及/或者其他期望的功能。在所述计算机可读存储介质中还可以存储诸如外参标定结果等各种内容。

除了上述方法和设备以外，本发明的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本发明的各种实施例的移动机器人的位姿估计方法中的步骤。

所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本发明实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。

此外，本发明的实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本发明的各种实施例的移动机器人的位姿估计方法中的步骤。

所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。

以上结合具体实施例描述了本发明的基本原理，但是，需要指出的是，在本发明中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本发明的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本发明为必须采用上述具体的细节来实现。

本发明中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“诸如但不限于”，且可与其互换使用。

还需要指出的是，在本发明的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本发明的等效方案。

提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本发明。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本发明的范围。因此，本发明不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。

为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本发明的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。

本领域的技术人员应理解，上述描述及附图中所示的本发明的实施例只作为举例而并不限制本发明。本发明的目的已经完整并有效地实现。本发明的功能及结构原理已在实施例中展示和说明，在没有背离所述原理下，本发明的实施方式可以有任何变形或修改。

Claims (9)

1.一种移动机器人的位姿估计方法，包括：

通过一基于一TOF深度相机设备获取一场景的第一帧点云和第二帧点云，其中，所述TOF深度相机设备通过发射面阵光对所述场景进行扫描，以获得所述场景的点云，以及，所述移动机器人具有一初始位姿；

以具有第一分辨率的高斯概率栅格对所述第一帧点云进行处理，以获得一第一高斯概率栅格地图，其中，所述第一高斯概率栅格地图用于获取所述第二帧点云中各点云的概率得分；

以具有第二分辨率的高斯概率栅格对所述第一帧点云进行处理，以获得第二高斯概率栅格地图，其中，第二分辨率大于第一分辨率，所述第二高斯概率栅格地图用于获取所述第二帧点云中各点云的概率得分；

以所述初始位姿为中心，构建具有预设半径的搜索空间；

基于所述初始位姿，对第二帧点云进行旋转变换和平移变换，以生成一第一变换点云；

以第一搜索步长对所述第一变换点云在以所述初始位姿为中心的所述搜索空间内进行处理，以获得一系列第一搜索点云；

遍历各所述第一搜索点云，并基于所述第一高斯概率栅格地图获得各搜索点云对应的总概率得分；

确定各所述第一搜索点云中总概率分值最大值对应的所述第一搜索点云的搜索位置为第一搜索位置，所述第一搜索位置对应的位姿为第二位姿；

基于所述第二位姿对所述第二帧点云进行旋转变换和平移变换，以生成一第二变换点云；

以第二搜索步长对所述第二变换点云在以所述第二位姿为中心的搜索空间进行处理，以获得一系列第二搜索点云；

遍历所述第二搜索点云，并基于所述第二高斯概率栅格地图获得各第二搜索点云对应总概率得分；以及确定各所述第二搜索点云中总概率分值最大值对应的第二搜索点云的搜索位置为目标位置，其中，所述目标位置对应的位姿为所述移动机器人的估计位姿。

2.根据权利要求1所述的移动机器人的位姿估计方法，其中，所述TOF深度相机设备的水平方向的视场角是125度。

3.根据权利要求2所述的移动机器人的位姿估计方法，其中，所述第一高斯概率栅格地图，具有一第一分辨率得分函数，用于获取所述第二帧点云中各点云的概率得分，其中，所述第二分辨率栅格地图具有一第二分辨率得分函数，用于获取所述第二帧点云中各点云的概率得分。

4.根据权利要求1所述的移动机器人的位姿估计方法，其中，以所述初始位姿为中心，构建具有预设半径的搜索空间步骤包括：基于该场景的丰富程度，获取所述预设半径。

5.根据权利要求4所述的移动机器人的位姿估计方法，其中，通过一基于一TOF深度相机设备获取一场景的第一帧点云和第二帧点云，其中，所述TOF深度相机设备通过发射面阵光对所述场景进行扫描，以获得所述场景的点云，以及，所述移动机器人具有一初始位姿步骤，包括：基于所述移动机器人的一惯性传感器，获取所述初始位姿。

6.一种移动机器人的位姿估计装置，包括：

一场景点云获取单元，用于通过一基于一TOF深度相机设备获取一场景的第一帧点云和第二帧点云，其中，所述TOF深度相机设备通过发射面阵光对所述场景进行扫描，以获得所述场景的点云，以及，基于所述移动机器人的一惯性传感器，获取所述移动机器人的一初始位姿；

一地图构建单元，用于以具有第一分辨率的高斯概率栅格对所述第一帧点云进行处理，以获得一第一高斯概率栅格地图，其中，所述第一高斯概率栅格地图用于获取所述第二帧点云中各点云的概率得分；还用于以具有第二分辨率的高斯概率栅格对所述第一帧点云进行处理，以获得一第二高斯概率栅格地图，其中，第二分辨率大于第一分辨率，所述第二高斯概率栅格地图用于获取所述第二帧点云中各点云的概率得分；

一搜索空间构建单元，用于以所述初始位姿为中心，构建一预设半径的搜索空间，其中，基于该场景的丰富程度，获取所述预设半径；

一第一分辨率搜索单元，用于基于所述初始位姿，对第二帧点云进行旋转和平移变换，以生成一第一变换点云，和，以第一搜索步长对所述第一变换点云在以所述初始位姿为中心的所述搜索空间内进行处理，以获得一系列第一搜索点云，和遍历各所述第一搜索点云，并基于所述第一高斯概率栅格地图获得各搜索点云对应的总概率得分；和确定各所述第一搜索点云中总概率分值最大值对应的所述第一搜索点云的搜索位置为第一搜索位置，所述第一搜索位置对应的位姿为第二位姿；以及一第二分辨率搜索单元，用于基于所述第二位姿对所述第二帧点云进行旋转变换和平移变换，以生成一第二变换点云，和以第二搜索步长对所述第二变换点云在以所述第二位姿为中心的搜索空间进行处理，以获得一系列第二搜索点云，和遍历所述第二搜索点云，并基于所述第二高斯概率栅格地图获得各第二搜索点云对应总概率得分，和确定各所述第二搜索点云中总概率分值最大值对应的第二搜索点云的搜索位置为目标位置，其中，所述目标位置对应的位姿为所述移动机器人的估计位姿。

7.根据权利要求6所述的移动机器人的位姿估计装置，其中，所述TOF深度相机设备的水平方向的视场角是125度。

8.一种移动机器人，包括：

一移动系统；

一TOF激光雷达，其中，所述TOF深度相机设备的水平方向的视场角是125度；

一位姿估计装置，包括一处理设备，所述处理设备包括处理器和存储器，在所述存储器中存储有计算机程序指令，所述计算机程序指令在被所述处理器运行时使得所述处理器执行如权利要求1-5中任一项所述的移动机器人的位姿估计方法。

9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序指令，当所述计算机程序指令被计算装置执行时，可操作来执行根据权利要求1-5中任一项所述的移动机器人的位姿估计方法。

Images