CN112363158A - 机器人的位姿估计方法、机器人和计算机存储介质 - Google Patents

Abstract

本申请公开了一种机器人的位姿估计方法、机器人和计算机存储介质。方法包括如下步骤：获取激光点云数据和匹配参考数据，匹配参考数据是基于环境信息筛选得到的，匹配参考数据包括占据栅格地图中占据栅格组成的具有语义信息的占据栅格集合，占据栅格集合用于表征处于静止状态的物体；剔除激光点云数据中非静止状态的物体对应的激光点数据，得到静态激光点云数据；筛选静态激光点云数据中与占据栅格集合距离最近且语义信息一致的激光点群；基于激光点群和占据栅格集合确定机器人的当前位姿。通过上述方式，本申请能够提高匹配参考数据和激光点云数据匹配的准确性。

Description

机器人的位姿估计方法、机器人和计算机存储介质

技术领域

本申请涉及机器视觉领域，特别是涉及一种机器人的位姿估计方法、机器人和计算机存储介质。

背景技术

目前使用移动机器人执行任务已有了广泛的应用，例如利用机器人搬运货物、投递快递及扫地拖地等。

移动机器人自主行走过程中，利用激光雷达获取根据当前环境生成的激光点云数据，并利用该激光点云数据对移动机器人进行定位，确定机器人的位姿。

目前常用的机器人定位方法通常基于先验地图或固定路标与实时激光点云数据匹配、或基于相机与固定路标匹配。基于先验地图这类方法的一大弊端是：动态环境中，环境会随时间逐渐变化，导致先验地图与环境差异性越来越大，很难保证机器人在该环境中长时间具备高精度的绝对定位精度。基于固定路标的方法中，路标可分为反光柱、反光板、二维码、纹理等标志；这类方法虽然能够保证绝对定位精度，但是应用时对场景改造大，长时间维护路标的成本高。

发明内容

本申请提供一种机器人的位姿估计方法、机器人和计算机存储介质，以解决现有技术中机器人定位不准以及需要对现实环境进行改造的问题。

为解决上述技术问题，本申请提供一种机器人的位姿估计方法。该方法包括：获取激光点云数据和匹配参考数据，匹配参考数据是基于环境信息筛选得到的，匹配参考数据包括占据栅格地图中占据栅格组成的具有语义信息的占据栅格集合，占据栅格集合用于表征处于静止状态的物体；剔除激光点云数据中非静止状态的物体对应的激光点数据，得到静态激光点云数据；筛选静态激光点云数据中与占据栅格集合距离最近且语义信息一致的激光点群；基于激光点群和占据栅格集合确定机器人的当前位姿。

为解决上述技术问题，本申请提供一种机器人。机器人包括处理器、摄像装置和激光雷达，处理器耦接摄像装置和激光雷达，在工作时执行指令，以配合存储器、激光雷达执行上述的机器人的位姿估计方法的步骤。

为解决上述技术问题，本申请提供一种计算机存储介质。计算机存储介质存储有计算机程序，计算机程序被处理器执行以实现上述的机器人的位姿估计方法的步骤。

本申请通过环境信息筛选匹配参考数据，能够提高匹配参考数据的准确性，在无路标的环境中仍然能够保证匹配参考数据的准确性，降低环境改造成本。进一步地，剔除激光点云数据中的动态物体对应的激光点，使得用于与匹配参考数据进行匹配的激光点云数据为相对静态的静态激光点云数据，从而提高匹配的准确率。此外，根据静态激光点云数据与占据栅格集合的距离和语义信息进行匹配，能够进一步提高配对正确率，从而提高机器人位姿估计的准确度。

附图说明

图1是本申请提供的机器人一实施例的结构示意图；

图2是本申请提供的基于仓库环境生成二维平面地图的示意图；

图3是本申请提供的机器人的位姿估计方法第一实施例的流程示意图；

图4是本申请提供的剔除激光点云数据中非静止状态的物体对应的激光点数据一实施方式的流程示意图；

图5是本申请提供的筛选静态激光点云数据中与占据栅格集合距离最近且语义信息一致的激光点群一实施方式的流程示意图；

图6是本申请提供的机器人的位姿估计方法第二实施例的流程示意图；

图7是本申请提供的基于当前位姿和静态点云数据更新环境信息一实施方式的流程示意图；

图8是本申请提供的计算机存储介质一实施例的结构示意图。

具体实施方式

为使本领域的技术人员更好地理解本发明的技术方案，下面结合附图和具体实施方式对本申请所提供的机器人的位姿估计方法、机器人和计算机存储介质做进一步详细描述。

在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其他实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其他实施例相结合。

请参阅图1，图1是本申请提供的机器人一实施例的结构示意图。

本申请中，机器人10可以是能够自动定位导航的机器人，例如扫地机器人、服务机器人或全自动搬运机器人(Automated Guided Vehicle，AGV)等。机器人10应用场景例如为仓库、餐厅、商场、图书馆或家庭等。

本实施例中的机器人10包括移动地盘11、激光传感器12和摄像装置13，其中移动地盘11包括里程计14、运动控制器(图未示)、电机(图未示)、电池(图未示)、处理器(图未示)。处理器耦接摄像装置和激光传感器12，处理摄像装置和激光传感器采集的数据，以计算机器人的当前位姿。

本实施例中的机器人10可以是能够获取到激光点云数据和里程计信息的机器人，对机器人10具体包括哪些部件不作过多限定。

其中，激光传感器12是用于获取二维平面信息的传感器，用于检测周围环境的二维平面轮廓信息，采集机器人坐标系下的激光点云数据，该激光传感器可以是2D激光传感器，本发明实施例不作过多限定。

里程计14可以采集机器人10角度和距离的变化量等里程计数据，可采用轮式编码器，从而能够根据前后时刻的位姿变化量和前一时刻的位姿来预测出当前时刻机器人10的位姿。

以该机器人10应用于仓储环境中为例，仓库中包括货架、工作台以及房屋支撑墙等部件，请参见图2，图2是本申请提供的基于仓库环境生成二维平面地图的示意图。机器人10中包括的里程计14可以估计出机器人10运动的变化量，但是由于里程计14存在误差，因而可以结合激光传感器12获取的二维平面轮廓信息。如图2所示，每一条直线表示激光传感器12发射的一束光束，基于光束激光传感器12能够感知障碍物的存在，从而扫描获取仓库的二维平面轮廓信息，进而通过二维平面轮廓信息对基于里程计14估算的位姿进行矫正，从而实现机器人10的高精度定位。

摄像装置13用于采集环境中的图像信息，从而基于图像信息提取环境中物体的图像特征，结合激光点云数据和里程计，能够更进一步地提高机器人10的位姿估计准确度。根据采集图像信息的时间、摄像装置13相对于机器人的角度、激光传感器12发出激光束的时间和角度，可进行图像信息和激光点云数据的匹配。

可选地，机器人10还可以包括惯性测量单元(图未示)，惯性测量单元是一种自主式的导航单元，能连续、实时地提供载体位置、姿态、速度等信息，特点是不依赖外界信息，不受气候条件和外部各种干扰因素。惯性测量单元例如包括陀螺仪、加速度计或磁力计等。惯性测量单元采集的惯性导航数据与里程计数据结合，能够提高对机器人10初始的预测位姿的准确度，从而基于初始的预测位姿计算得到的当前位姿的过程更加效率，当前位姿更加准确。

本实施例中的“位姿”表示机器人10在空间坐标系中的位置和姿态，可以用矩阵表示，如t时刻机器人10的估计位姿为x_t＝[x y θ]^T，x、y为机器人10在占据栅格坐标系下的二维平面的位置，x表示机器人在占据栅格地图中定位的横轴位置，y表示机器人在占据栅格地图中定位的纵轴位置，θ为机器人10的偏航角。

处理器获取激光传感器12采集的激光点云数据、摄像装置13采集的图像信息和里程计14采集的里程计数据，实现以下的机器人10的位姿估计方法的实施例：

请参阅图3，图3是本申请提供的机器人的位姿估计方法第一实施例的流程示意图。本实施例包括如下步骤：

S11：获取激光点云数据和匹配参考数据。

其中，激光点云数据是对安装于机器人上的激光传感器采集的当前时刻的激光传感器数据进行预处理得到的。

具体地，将激光传感器数据投影在当前时刻的机器人坐标系下，得到原始激光点云数据。进一步地，根据里程计数据和惯性测量数据估计得到的线速度和角速度补偿机器人运动造成的原始激光点云数据的畸变，得到激光点云数据。上述的预处理操作能够降低激光点云数据的误差，使得激光点云数据更加可靠。

匹配参考数据是基于环境信息筛选得到的，匹配参考数据包括占据栅格地图中占据栅格组成的具有语义信息的占据栅格集合。占据栅格集合用于表征处于静止状态的物体。

语义信息例如包括几何特征、图像特征和物体属性标签等。其中，几何特征包括几何形状、法向量或曲率等信息。图像特征包括图像边缘、角点、颜色或灰度等信息。物体属性标签表示某类型物体的唯一标识，不同的占据栅格集合可以根据物体属性标签确定其是否属于同一类型的物体。

例如，环境中存在横截面为矩形的货物，在占据栅格地图中该货物对应的位置的一组占据栅格组合所形成的几何形状，与该货物的横截面的形状大致相同，即大致为矩形。

提取占据栅格地图的语义信息具体包括：根据几何特征对占据栅格地图中的占据栅格进行初步聚类。在初步聚类的基础上，获取占据栅格在现实场景中对应位置的图像信息，提取图像信息中的图像特征，结合图像特征对占据栅格进一步分类，以确定分类后的占据栅格对应的物体属性，即现实场景中对位位置摆放的是哪种物体。

最后对每类占据栅格标记其物体属性标签(不同类别栅格为不同物体，可用数字、字母或符号等某种标记表示)，同一类的占据栅格的物体属性标签相同，并将物体属性标签与几何特征和图像特征相关联(即通过物体属性标签可以得到其几何特征和图像特征信息)。

S12：剔除激光点云数据中非静止状态的物体对应的激光点数据，得到静态激光点云数据。

一帧激光点云数据中，既可能存在环境中静止状态的物体对应的激光点，也可能存在非静止状态的物体对应的激光点。在使用激光点云数据与占据栅格地图进行匹配时，若激光点云数据中存在非静止状态的物体对应的激光点，由于该非静止状态的激光点在匹配参考数据中可能没有对应的参考数据，那么会造成该部分激光点无法配准，造成机器人位姿评估的分数低，从而影响估计的位姿的准确度。

本实施例中，剔除激光点云数据中非静止状态的物体对应的激光点数据，得到静态激光点云数据，从而基于静态激光点数据与匹配参考数据进行匹配求解机器人的位姿，能够提高机器人位姿估计的鲁棒性。

请参阅图4，图4是本申请提供的剔除激光点云数据中非静止状态的物体对应的激光点数据一实施方式的流程示意图，包括如下步骤：

S121：获取里程计数据和惯性导航数据。

S122：根据里程计数据和惯性导航数据，计算得到机器人的当前时刻的位姿与前一时刻的位姿之间的位姿变化量。

位姿变化量包括偏航角变化量和位置变化量。

惯性导航数据中包括加速度和惯性偏航角变化量。根据当前时刻与前一时刻之间的时间差，对加速度积分可得到机器人当前时刻相对于上一时刻的位移(惯性位置变化量)。

里程计数据包括里程增量(里程位置变化量)和偏航角增量(里程偏航角变化量)。

由于里程计数据可能由于机器人行驶的道路不平整导致误差，惯性导航数据可能存在累计误差。因此，可使用融合算法将里程计数据和惯性导航数据进行融合，得到机器人的当前时刻的位姿与前一时刻的位姿之间的位姿变化量。融合算法例如是卡尔曼滤波算法或互补滤波算法等。

S123：根据位姿变化量，将激光点云数据投影到前一时刻的机器人坐标系中，得到变换激光点云数据。

位姿变化量可以看作是当前时刻和前一时刻之间的平移变化和旋转变化。根据位姿变化量将当前时刻的机器人坐标系下的激光点云数据经过平移和旋转，投影到前一时刻的机器人坐标系中，得到变换激光点云数据。

S124：计算前一时刻激光点云数据中每一激光点与变换激光点云数据中距离最近的变换激光点之间的第一距离。

计算前一时刻的激光点云数据中的激光点与变换激光点之间的第一距离，以匹配前一时刻激光点云数据和当前时刻的激光点云数据。

前一时刻和当前时刻之间一静止状态物体，在当前时刻对应的激光点转换到前一时刻的机器人坐标系后，其应当与前一时刻该静止状态物体对应的激光点大致重合。若前一时刻和当前时刻之间存在移动状态的物体，在当前时刻对应的激光点转换到前一时刻的机器人坐标系后，其与前一时刻该移动状态物体对应的激光点距离较大。

S125：比较第一距离是否大于第十阈值。

在第一距离小于等于第十阈值时，可认为变换激光点对应的物体为静止状态物体；在第一距离大于第十阈值时，可认为变换激光点对应的物体为移动状态物体。

其中，第十阈值可以根据激光传感器采集激光数据的采集周期，和里程计的误差进行设置。那么第十阈值可根据以下公式计算得到：

T₁₀＝r·v₁·T

其中，T₁₀为第十阈值，T为激光传感器采集激光数据的采集周期，v₁为里程计速度误差，r为比例系数，例如r可设置为1.5。一般采用相邻帧激光数据(前一时刻的激光点云数据和当前时刻的激光点云数据)对比，若T比较大(>＝0.1s)，则r相应增大。

S126：若第一距离大于第十阈值，剔除激光点云数据中对应变换激光点对应的激光点，得到静态激光点云数据。

第一距离大于第十阈值，说明对应的变换激光点所表示的物体较上一时刻发生了移动。此类激光点会造成位姿迭代估算过程中位姿的误判，从而造成估计的当前位姿的误差，将此类激光点从激光点云数据中剔除，提高机器人的定位精度。

S13：筛选静态激光点云数据中与占据栅格集合距离最近且语义信息一致的激光点群。

计算与占据栅格集合距离最近的多个激光点，能够进行静态激光点云数据与占据栅格集合的初始匹配。

在静态激光点云数据与占据栅格集合初始匹配的情况下，进一步根据占据栅格集合与多个激光点的语义信息确定语义信息一致的多个激光点作为激光点群。

请参阅图5，图5是本申请提供的筛选静态激光点云数据中与占据栅格集合距离最近且语义信息一致的激光点群一实施方式的流程示意图，包括如下步骤：

S131：对静态激光点云数据进行分类，得到分类静态激光点云数据。

本申请中，匹配参考数据包括先验匹配参考数据和非先验匹配参考数据。

其中，先验匹配参考数据是指先验地图中根据语义信息提取的至少一个占据栅格集合。先验地图为占据栅格地图，先验地图中的占据栅格代表征实际场景中货架、工作台以及房屋支撑墙等位置相对固定、不易移动的物体。先验地图为离线地图，一般地，先验地图不会随着实时的激光点云数据更新，以保证现实场景中相对静止的物体在占据栅格地图中对应的位置稳定，不会因估计位姿累计误差而导致先验匹配参考数据失真。

非先验匹配参考数据能够根据环境信息动态调整，表示现实场景中可移动但存在保持静止状态的物体，例如短期摆放在仓库中的货物。非先验匹配参考数据的调整方式在下文进行详细描述。

分类静态激光点云数据包括与先验匹配参考数据距离最近的第一静态激光点云数据，和与非先验匹配参考数据距离最近的第二静态激光点云数据。

对静态激光点云数据进行分类具体包括：将静态激光点云数据投影到栅格地图坐标系中，得到地图坐标系激光点云数据。计算地图坐标系激光点云数据中与匹配参考数据距离最近地图坐标系激光点。将与先验匹配参考数据距离最近的地图坐标系激光点聚类为第一分类静态激光点云数据，以及将与非先验匹配参考数据距离最近的地图坐标系激光点聚类为第二分类静态激光点云数据。

S132：分别剔除分类静态激光点云数据中的外点，得到用于匹配的有效激光点云数据。

外点为分类静态激光点云数据中与匹配参考数据中最近的占据栅格的距离大于第三阈值的分类静态激光点。外点在占据栅格地图中相应的位置在前一时刻的占据栅格地图中未被占据，可能为现实场景中的空地上新出现的处于静止状态的物体。外点在匹配参考数据中没有与之配对的占据栅格，保留外点可能会导致基于分类静态激光点云数据计算机器人位姿时位姿的评估得分低，影响估算当前位姿的最优解。为提高位姿估计的准确度，本实施例将外点从分类静态激光点云数据中剔除。

具体地，计算第一分类静态激光点云数据中每一第一分类静态激光点与先验匹配参考数据中距离最近的占据栅格的第二距离。比较第二距离和第十一阈值的大小，对于第二距离大于第十一阈值的第一分类静态激光点，从第一分类静态激光点云数据中剔除；对于第二距离小于等于第十一阈值的第一分类静态激光点，仍保留在第一分类静态激光点云数据中。第一分类静态激光点云数据中剔除最第二距离大于第十一阈值的第一分类静态激光点得到第一有效激光点云数据。

其中，第十一阈值的取值范围可以为0.1～0.5米，具体取值例如为0.2米、0.3米或0.4米。

计算第二分类静态激光点云数据中每一第二分类静态激光点与先验匹配参考数据中距离最近的占据栅格的第三距离。比较第三距离和第十一阈值的大小，对于第三距离大于第十一阈值的第二分类静态激光点，从第二分类静态激光点云数据中剔除；对于第三距离小于等于第十一阈值的第二分类静态激光点，仍保留在第二分类静态激光点云数据中。第二分类静态激光点云数据中剔除第三距离大于第十一阈值的第二分类静态激光点得到第二有效激光点云数据。

S133：匹配有效激光点云数据中与占据栅格集合距离最近且语义信息一致的激光点群。

具体地，确定第一有效激光点云数据中与先验匹配参考数据中的先验占据栅格集合距离最近的第一激光点群。分别提取第一激光点群、先验占据栅格集合的语义信息。比对第一激光点群与先验占据栅格集合的语义信息，确定语义信息一致的第一激光点群与先验占据栅格集合。

确定第二有效激光点云数据中与非先验匹配参考数据中的非先验占据栅格集合距离最近的第二激光点群；分别提取第二激光点群和非先验占据栅格集合的语义信息；比对第二激光点群与非先验占据栅格集合的语义信息，确定语义信息一致的第二激光点群与非先验占据栅格集合。

S14：基于激光点群和占据栅格集合确定机器人的当前位姿。

将距离最近且语义信息一致的激光点群和占据栅格集合进行匹配，利用配对成功的激光点群和占据栅格集合对构建位姿约束项，以计算机器人的当前位姿。

基于激光点群和占据栅格集合进行匹配计算位姿的方法有多种，例如hectorslam、kartoslam或cartography等。

不同类型的匹配参考数据和激光点群的可靠程度不同，基于匹配参考数据和激光点群的可靠程度，为不同类型的激光点群配置不同的权重。例如，先验匹配参考数据和第一激光点群对应的物体一般不会发生移动，因此先验匹配参考数据和第一激光点群可靠程度最高；非先验匹配数据和第二激光点群对应的物体可能会发生移动，因此非先验匹配参考数据和第二激光点群的可靠程度低于先验匹配参考和第一激光点群。

具体地，获取第一激光点群的第一权重α_{opt_prior}，第二激光点群的第二权重α_{opt_st}。其中，第一权重α_{opt_prior}和第二权重α_{opt_st}满足如下关系：

1>α_{opt_prior}>α_{opt_st}≥0

α_{opt_prior}+α＝1

基于第一激光点群、先验占据栅格集合和第一权重，第二激光点群、非先验占据栅格集合和第二权重，计算机器人的当前位姿。

在另一个具体实施方式中，非先验匹配参考数据还可以进一步划分为近距离非先验匹配参考数据和远距离非先验匹配参考数据。近距离非先验匹配参考数据为与先验占据栅格集合的距离小于等于第十二阈值的占据栅格集合，远距离非先验匹配参考数据为与先验占据栅格集合的距离大于第十二阈值的占据栅格集合。其中，第十二阈值的取值范围可以为0.2米～0.8米，具体例如为0.2米，0.3米，0.5米和0.8米等。

近距离非先验匹配参考数据和远距离非先验匹配参考数据的可靠程度也不相同，近距离非先验匹配参考数据与先验匹配参考数据的距离更近，因此近距离非先验匹配参考数据的可靠程度大于远距离非先验匹配参考数据的可靠程度。近距离非先验匹配参考数据包括近距离非先验占据栅格集合，远距离非先验匹配参考数据包括远距离非先验占据栅格集合。第二激光点群包括与近距离非先验占据栅格集合对应的近距离激光点群和与远距离非先验占据栅格集合对应的远距离激光点群。

获取第一激光点群的第一权重α_{opt_prior}，近距离激光点群的第三权重α_{opt_st_close}，和远距离激光点群的第四权重α_{opt_st_far}。其中，第一权重α_{opt_prior}、第三权重α_{opt_st_close}和第四权重α_{opt_st_far}满足以下关系：

1>α_{opt_prior}>α_{opt_st_close}≥α_{opt_st_far}≥0

α_{opt_prior}+α_{opt_st_close}+α_{opt_st_far}＝1

第一权重α_{opt_prior}、第三权重α_{opt_st_close}和第四权重α_{opt_st_far}例如分别为0.5、0.3和0.2，或0.55、0.3和0.15，或0.6、0.3和0.1，或0.7、0.2和0.1等，具体数值满足上述关系即可，本申请对此不作限制。第一权重α_{opt_prior}、第三权重α_{opt_st_close}和第四权重α_{opt_st_far}可以是设置的固定值，也可以是求解当前位姿时优化调整得到的值。

基于第一激光点群、先验占据栅格集合和第一权重，近距离激光点群、近距离非先验占据栅格集合和第三权重，远距离激光点群、远距离非先验占据栅格集合和第四权重计算机器人的当前位姿。

本实施例通过环境信息筛选匹配参考数据，能够提高匹配参考数据的准确性，在无路标的环境中仍然能够保证匹配参考数据的准确性，降低环境改造成本。进一步地，剔除激光点云数据中的动态物体对应的激光点，使得用于与匹配参考数据进行匹配的激光点云数据为相对静态的静态激光点云数据，从而提高匹配的准确率。此外，根据静态激光点云数据与占据栅格集合的距离和语义信息进行匹配，能够进一步提高配对正确率，从而提高机器人位姿估计的准确度。

在确定机器人的当前位姿之后，根据当前位姿更新占据栅格地图中栅格的占据概率，以及根据当前位姿更新环境信息和匹配参考数据，以用于下一时刻的机器人位姿估计，提高动态环境中机器人在占据栅格地图中的绝对定位精度。请参阅图6，图6是本申请提供的机器人的位姿估计方法第二实施例的流程示意图，本实施例是基于机器人的位姿估计方法第一实施例，因此相同的步骤在此不再赘述。本实施例包括如下步骤：

S21：获取激光点云数据和匹配参考数据。

S22：剔除激光点云数据中非静止状态的物体对应的激光点数据，得到静态激光点云数据。

S23：筛选静态激光点云数据中与占据栅格集合距离最近且语义信息一致的激光点群。

S24：基于激光点群和占据栅格集合确定机器人的当前位姿。

S25：根据静态激光云数据和当前位姿，更新占据栅格地图中栅格的被占据概率。

本实施例中，为保证先验匹配参考数据的准确性，先验匹配参考数据对应的栅格的被占据概率不进行更新。对于占据栅格地图中处先验匹配参考数据对应的栅格外的其他栅格，需要更新其被占据概率。

在一个具体的实施方式中，更新占据栅格地图中栅格的占据概率可通过如下公式进行更新：

其中，p表示认为栅格被障碍物占据的概率值，x表示栅格地图中的位置，M_old(x)、M_new(x)表示同一栅格位置旧的概率值和新的概率值。odds(p_hit)表示被激光集中的栅格(即激光点数据对应的栅格)被占据的赢率，此时M_new(x)较上一时刻增大；odds(p_miss)表示激光穿过的栅格被占据的赢率，此时M_new(x)较上一时刻减小。

当占据概率值大于第四阈值时，认为此栅格是被障碍物占据的。其中，第四阈值可以是0.6、0.65、0.7或0.75等。

S26：评估当前位姿的准确度。

评估当前位姿的准确度能够确定当前位姿和当前时刻的静态激光点云数据是否准确，以在确定当前位姿的准确度符合预期时，执行S27。

评估当前位姿的准确度的方法有多种，例如，基于高精度到点任务执行情况进行评估。还例如，根据静态激光点云数据和匹配参考数据之间的匹配质量进行评估。

具体而言，机器人例如AGV运用于实际场景时，常见的任务为搬运、对接等任务，这些任务是否能完成一般取决于机器人是否能够精确到达目标点，能否到达目标点主要取决于机器人定位系统的绝对定位误差是否足够小。因此，可以根据机器人执行某些需要高精度到点的任务的情况评估机器人的定位准确度；能成功完成需要高精度到点的任务，则认为机器人到达任务点位附近时刻的定位准确度比较高。

根据匹配质量可以从位姿的位姿观测度、匹配残差和不确定度中的一个或多个进行评估当前位姿是否符合预期。

根据位姿观测度评估当前位姿是否符合预期具体包括：计算静态激光点云数据基于当前位姿得到的位姿观测度。其中，位姿观测度包括横轴位置观测度、纵轴位置观测度和偏航角观测度。判断横轴位置观测度是否大于第五阈值，纵轴位置观测度是否大于第六阈值，且偏航角观测度是否大于第七阈值。若横轴位置观测度大于第五阈值，且纵轴位置观测度大于第六阈值，且偏航角观测度大于第七阈值，则确定当前位姿的准确定度符合预期；若存在横轴位置观测度不大于第五阈值，且纵轴位置观测度不大于第六阈值，且偏航角观测度不大于第七阈值任一一项，则确定当前位姿的准确度不符合预期。

其中，计算位姿观测度例如是通过IMLS-SLAM中的观测度求解方法计算静态激光点云数据中每个激光点对当前位姿的观测度，并对静态激光点云数据中所有激光点的位姿观测度进行累加得到的。

根据匹配残差评估当前位姿是否符合预期具体包括：基于当前位姿，计算激光点群与占据栅格集合之间的匹配残差。若匹配残差小于第八阈值，则确定当前位姿的准确度符合预期；若匹配残差大于等于第八阈值，则确定当前位姿的准确度不符合预期。

根据不确定度评估当前位姿是否符合预期具体包括：基于当前位姿和匹配残差，计算当前位姿的不确定度。若不确定度小于第九阈值，则确定当前位姿的准确度符合预期；若不确定度大于等于第九阈值，则确定当前位姿的准确度不符合预期。

其中，基于当前位姿和匹配残差，计算当前位姿的不确定度，包括：计算当前位姿与前一时刻的估计位姿之间的位姿变化量；计算位姿变化量对匹配残差的雅可比矩阵J_k(n×3的矩阵)；基于雅可比矩阵，分别计算当前位姿中横轴坐标值的第一不确定度、纵轴坐标值的第二不确定度和偏航角的第三不确定度；若第一不确定度、第二不确定度和第三不确定度均小于第六阈值，则确定当前位姿的准确度符合预期。

基于雅可比矩阵计算第一不确定度、第二不确定度和第三不确定度包括：根据雅可比矩阵J_k计算

矩阵对角线上的三个数值，即为第一不确定度、第二不确定度和第三不确定度。

当基于位姿观测度、匹配残差和不确定度中的三者评估当前位姿是否符合预期时，在位姿观测度、匹配残差和不确定度均达到上述的预设条件时(位姿观测度不符合预期、匹配残差小于第八阈值以及不确定度小于第九阈值时)，确定当前位姿的准确度符合预期。

S27：基于当前位姿和静态点云数据更新环境信息。

在确定当前位姿准确度符合预期后，使用当前位姿和静态点云数据更新环境信息。

占据栅格地图的环境空间区域可以划分为静止状态区域和非静止状态区域。静止状态区域例如是存在物体保持静止状态的区域，匹配参考数据位于静止状态区域中；非静止状态区域例如是不存在物体保持过静止状态的区域，例如走廊等。占据栅格地图还可以根据占据栅格集合的聚类情况、区域功能和面积等划分为多个子区域，每一子区域中包括部分静止状态区域和/或部分非静止状态区域。

环境信息用于表示现实场景中各子区域物体的运动规律，从而基于环境信息能够最大概率筛选到静态物体，能够更好地更新匹配参考数据。环境信息例如包括静止状态区域内的所有物体的语义信息集合、各类物体属性标签的物体保持静止状态的时间和一天内静止状态区域内所有物体保持静止状态的时间和等。

请参阅图7，图7是本申请提供的基于当前位姿和静态点云数据更新环境信息一实施方式的流程示意图。本实施方式包括如下步骤：

S271：根据当前位姿，将静态激光点云数据投影到占据栅格地图中，得到静态激光点云数据在占据栅格地图中对应的第一激光占据区域。

根据当前位姿，将静态激光点云数据转换到占据栅格地图坐标系下，得到静态激光点云数据中每一激光点在占据栅格地图中对应的占据位置。静态激光点云数据中所有激光点对应的占据位置作为第一激光占据区域。

S272：根据第一激光占据区域，统计静止状态区域内每一占据栅格集合的语义信息，以更新静止状态区域的语义信息集合。

统计位于静止状态区域内的激光占据区域的激光语义信息，基于激光语义信息能够判断在静止状态区域内是否存在与占据栅格集合语义信息不同的激光语义信息，以确定是否更新静止状态区域的语义信息集合。

具体地，筛选第一激光占据区域中位于静止状态区域的第二激光占据区域。提取第二激光占据区域中激光占据栅格集合的激光语义信息。确定与激光占据栅格集合距离最近的占据栅格集合，比对激光语义信息和距离最近的占据栅格集合的语义信息。若激光语义信息和距离最近的占据栅格集合的语义信息不一致，则增加该激光语义信息到语义信息集合。

S273：更新环境空间区域。

更新环境空间区域是根据第一激光占据区域，对静止状态区域和非静止状态区域的范围进行调整。

具体地，筛选第一激光占据区域中位于非静止状态区域的第三激光占据区域，第三激光占据区域表示其在现实场景中对应的位置新摆放了物体，该区域已经存在静止状态的物体。由于在占据栅格地图中，距离障碍物距离越近的栅格被占据的概率相对较大，由障碍物往外延伸一定距离的栅格均有可能被占据，因此本实施例进一步确定与第三激光占据区域的距离小于第四阈值的第四激光占据区域，也即将第三激光占据区域周围一定范围内的栅格划分为第四激光占据区域。其中，第四阈值的取值范围可以为0.1～0.3米，具体例如为0.13米，0.15米，0.2米或0.25米等。

将第三激光占据区域和第四激光占据区域划分到静止状态区域，以及将第三激光占据区域和第四激光占据区域从非静止状态区域中剔除，以更新占据栅格地图的环境空间区域。

第三激光占据区域对应位置摆放的物体为新增的物体，因此还需提取第三激光占据区域中激光占据栅格集合的激光语义信息，并将该激光语义信息添加到语义信息集合中，以在后续统计该激光语义信息对应的物体在静止状态区域中存在的时间。

S274：统计第一预设时间段内语义信息集合中每一语义信息对应的占据栅格集合在栅格地图中的存在时间。

语义信息集合为静止状态区域内所有占据栅格集合对应的语义信息的集合。

第一预设时间例如是一个自然日、2个自然日、5个自然日或7个自然日等。

若存在多个语义信息一致的占据栅格集合，则语义信息一致的多个占据栅格集合的存在时间和作为该语义信息对应的存在时间。例如，仓库1中存在多个货物A，多个货物A的语义信息是一致的。那么仓库1内货物A的语义信息的存在时间，为多个货物A对应的占据栅格集合在占据栅格地图中存在时间之和。

每一语义信息对应的占据栅格集合的存在时间，能够反映每一物体在该区域的稳定性。一般地，某物体对应的占据栅格集合存在时间越长，某物体对应的占据栅格集合在用于计算机器人位姿时越可靠。

S275：统计第二预设时间段内语义信息集合中所有语义信息对应的占据栅格集合在栅格地图中的存在时间之和。

第二预设时间例如是一个自然日、2个自然日、5个自然日或7个自然日等。

第二预设时间可以与第一预设时间相同，也可以不同。

统计语义信息集合中所有语义信息对应的占据栅格集合在栅格地图中的存在时间之和，能够评估该区域的稳定性。

例如，有些区域允许长时间摆放货物，那么该区域的存在时间之和相对大一些，位于该区域的占据栅格集合更稳定，能够筛选出来作为匹配参考数据的概率较大一些。而有些区域只允许短暂地摆放货物，物体流动性大，该区域存在时间之和相对而言较短，位于该区域的占据栅格集合相对没那么稳定，能够筛选出来作为匹配参考数据的概率也就较小一些。

S28：基于更新后的环境信息，更新非先验匹配参考数据。

由于先验匹配参考数据表征位置相对固定、不易移动的物体，为保证先验匹配参考数据的稳定性和准确率，一般不会利用实时采集多激光点云数据对先验匹配参考数据进行更新，而是更新非先验匹配参考数据。

提取第一激光占据区域中激光占据栅格集合的激光语义信息。基于环境信息和激光语义信息，判断激光占据栅格集合是否满足作为匹配参考数据的预设条件。若激光占据栅格集合满足预设条件，则更新匹配参考数据。

具体而言，将激光占据栅格集合与占据栅格集合进行配对，分别记录配对成功的激光占据栅格集合和占据栅格集合对和未配对成功的激光占据栅格集合。配对成功的激光占据栅格集合的筛选条件和未配对成功的激光占据栅格集合的筛选条件不同。

对于配对成功的激光占据栅格集合，判断其是否满足替换与其配对占据栅格集合。具体地，比较激光占据栅格集合和占据栅格集合对中，激光占据栅格集合的激光语义信息和占据栅格集合的语义信息是否匹配。若激光语义信息和语义信息不匹配，则删除匹配参考数据中的与激光占据栅格集合配对的占据栅格集合。

当激光语义信息和语义信息中包括物体属性标签时，若二者的物体属性标签相同，则确定激光语义信息和语义信息相匹配；若二者的物体属性标签不相同，则确定激光语义信息和语义信息不匹配。

当激光语义信息和语义信息中包括几何特征或图像特征时，则比较二者的几何特征或图像特征之间的差异是否大于第一阈值，若二者的几何特征或图像特征之间的差异不大于第一阈值，说明激光语义信息和语义信息相似，则可以确定激光语义信息和语义信息相匹配；若二者的几何特征或图像特征之间的差异大于第一阈值，则确定激光语义信息和语义信息不匹配。

当激光语义信息和语义信息中包括物体属性标签、几何特征和图像特征中的两者及以上时，需同时满足物体属性标签相同，几何特征、图像特征不大于第一阈值。

在删除匹配参考数据中的与激光占据栅格集合配对的占据栅格集合后，获取环境信息，根据环境信息中的第一预设时间段内语义信息对应的占据栅格集合在栅格地图中的存在时间，判断第二预设时间段内匹配参考数据中存在的所有占据栅格集合在栅格地图中的第一存在时间之和是否大于第二阈值，以及语义信息与激光语义信息相似的占据栅格集合的第二存在时间之和是否大于第三阈值。其中，第一预设时间段可以大于第二预设时间段，也可以等于第二预设时间段。若第一存在时间和大于第二阈值，且第二存在时间和大于第三阈值，则将激光占据栅格集合作为新增匹配参考数据加入到非先验匹配参考数据中，以完成非先验匹配参考数据的更新。

其中，第二阈值取值范围可以为5～30小时，具体例如为5小时，10小时、15小时、20小时或30小时等。第三阈值取值范围可以为1～10小时，具体例如为1小时，2小时、5小时、8小时或10小时等。

对于未配对成功的激光占据栅格集合，则需要先判断激光占据栅格集合所处的区域内，是否存在与激光占据栅格集合的激光语义信息相似的语义信息。

若存在，则基于存在时间，判断第二预设时间段内匹配参考数据中存在的每一占据栅格集合在栅格地图中的第一存在时间和是否大于第二阈值，以及语义信息与激光语义信息相似的占据栅格集合的第二存在时间和是否大于第三阈值。

若第一存在时间和大于第二阈值，且第二存在时间和大于第三阈值，则将激光占据栅格集合作为新增匹配参考数据加入到非先验匹配参考数据中，以完成非先验匹配参考数据的更新。

匹配参考数据更新后，用于与下一时刻采集到的激光点云数据进行匹配，即应用于S23的步骤，以计算下一时刻的机器人的位姿。

本实施例中，构建新的匹配参考数据，剔除过时的匹配参考数据，更新发生了变化的匹配参考数据，使得匹配参考数据准确，能够提高动态环境中AGV在地图坐标系中的绝对定位精度。

在相关技术中，激光匹配的误差主要来源为：观测数据(用于与匹配参考数据匹配的激光点云数据)误差、匹配参考数据不准确(匹配参考数据变化了)、观测数据与匹配参考数据配对误差。激光传感器的观测误差较小，基于激光传感器数据的匹配定位方法由观测数据误差造成的定位误差较小；其定位误差主要有匹配参考数据不准确和观测数据与参考数据配对误差造成。基于相关技术中存在的上述问题，本申请提供的机器人的位姿估计方法从这两点进行优化：结合视觉图像特征和几何特征对观测数据和参考数据进行正确的配对，从而降低观测数据和参考数据的配对误差；实时更新环境信息，基于环境信息更新匹配参考数据，从而使参考数据准确。

具体而言，本申请充分利用机器人在固定空间中进行重复性工作的特点，通过采集历史的数据分析环境信息，得到环境中物体的运动规律。通过环境信息筛选匹配参考数据，即应用环境中每个区域物体运动规律指导从激光点云数据中筛选出可用于匹配的激光点在栅格地图上对应的栅格作为匹配参考数据，从而能够最大概率筛选到静态物体，从而能够新增较好的匹配参考数据，能够提高匹配参考数据的准确性，基于不断更新的匹配参考数据，机器人处于动态环境下也能够得到可靠性及鲁棒性较强的机器人定位，在无路标的环境中仍然能够保证匹配参考数据的准确，从而无需设置路标降低环境改造成本。匹配中最好的参考数据为静态物体，从而保证最大概率通过观测数据估计机器人自身的运动，剔除激光点云数据中的动态物体对应的激光点，使得用于与匹配参考数据进行匹配的激光点云数据为相对静态的静态激光点云数据，从而提高匹配的准确率。

对于上述实施例的方法，其可以计算机程序的形式存在，因而本申请提出一种计算机存储介质，请参阅图8，图8是本申请提供的计算机存储介质一实施例的结构示意图。本实施例计算机存储介质200中存储有计算机程序201，其可被执行以实现上述实施例中的方法。

本实施例计算机存储介质200可以是U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等可以存储程序指令的介质，或者也可以为存储有该程序指令的服务器，该服务器可将存储的程序指令发送给其他设备运行，或者也可以自运行该存储的程序指令。

在本申请所提供的几个实施例中，应该理解到，所揭露的方法和装置，可以通过其它的方式实现。例如，以上所描述的装置实施方式仅仅是示意性的，例如，模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施方式方案的目的。

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

集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上仅为本申请的实施方式，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (19)

1.一种机器人的位姿估计方法，其特征在于，所述方法包括：

获取激光点云数据和匹配参考数据，所述匹配参考数据是基于环境信息筛选得到的，所述匹配参考数据包括占据栅格地图中占据栅格组成的具有语义信息的占据栅格集合，所述占据栅格集合用于表征处于静止状态的物体；

剔除所述激光点云数据中非静止状态的物体对应的激光点数据，得到静态激光点云数据；

筛选所述静态激光点云数据中与所述占据栅格集合距离最近且所述语义信息一致的激光点群；

基于所述激光点群和所述占据栅格集合确定所述机器人的当前位姿。

2.根据权利要求1所述的方法，其特征在于，所述基于所述激光点群和所述占据栅格集合确定所述机器人的当前位姿之后，还包括：

基于所述当前位姿和所述静态点云数据更新所述环境信息。

3.根据权利要求2所述的方法，其特征在于，所述匹配参考数据包括非先验匹配参考数据和先验匹配参考数据，所述基于所述当前位姿和所述静态点云数据更新所述环境信息之后，还包括：

基于更新后的环境信息，更新所述非先验匹配参考数据。

4.根据权利要求3所述的方法，其特征在于，所述基于更新后的环境信息，更新所述非先验匹配参考数据，包括：

根据所述当前位姿，将所述静态激光点云数据投影到所述占据栅格地图中，得到所述静态激光点云数据在所述占据栅格地图中对应的第一激光占据区域；

提取所述第一激光占据区域中激光占据栅格集合的激光语义信息；

基于所述环境信息和所述激光语义信息，判断所述激光占据栅格集合是否满足作为匹配参考数据的预设条件；

若所述激光占据栅格集合满足所述预设条件，则更新所述非先验匹配参考数据。

5.根据权利要求4所述的方法，其特征在于，所述基于所述环境信息和所述激光语义信息，判断所述激光占据栅格集合是否满足作为匹配参考数据的预设条件，包括：

将所述激光占据栅格集合与所述占据栅格集合进行配对，记录配对成功的激光占据栅格集合和占据栅格集合对；

比较所述激光占据栅格集合和占据栅格集合对中，所述激光语义信息和所述语义信息是否匹配；

若所述激光语义信息和所述语义信息不匹配，则删除匹配参考数据中的所述占据栅格集合；

获取所述环境信息；其中，所述环境信息包括第一预设时间段内每一所述占据栅格集合在所述栅格地图中的存在时间；

基于所述存在时间，判断第二预设时间段内每一所述占据栅格集合的第一存在时间之和是否大于第二阈值，以及所述语义信息与所述激光语义信息相似的占据栅格集合的第二存在时间之和是否大于第三阈值；

所述若所述激光占据栅格集合满足所述预设条件，则更新所述非先验匹配参考数据，包括：

若所述第一存在时间之和大于所述第二阈值，且所述第二存在时间之和大于所述第三阈值，则将所述激光占据栅格集合加入到所述非先验匹配参考数据中。

6.根据权利要求4所述的方法，其特征在于，所述基于所述环境信息和所述激光语义信息，判断所述激光占据栅格集合是否满足作为匹配参考数据的预设条件，包括：

将所述激光占据栅格集合与所述占据栅格集合进行配对，记录未配对成功的激光占据栅格集合；

获取所述环境信息；其中，所述环境信息包括第一预设时间段内所述匹配参考数据中存在的每一占据栅格集合在所述栅格地图中的存在时间；

判断所述匹配参考数据中是否存在语义信息与所述激光语义信息相似的占据栅格集合；

若存在，则基于所述存在时间，判断第二预设时间段内所述匹配参考数据中存在的每一占据栅格集合在所述栅格地图中的第一存在时间之和是否大于第二阈值，以及所述语义信息与所述激光语义信息相似的占据栅格集合的第二存在时间之和是否大于第三阈值；

所述若所述激光占据栅格集合满足所述预设条件，则更新所述非先验匹配参考数据，包括：

若所述第一存在时间之和大于所述第二阈值，且所述第二存在时间之和大于所述第三阈值，则将所述激光占据栅格集合加入到所述非先验匹配参考数据中。

7.根据权利要求2所述的方法，其特征在于，所述占据栅格地图包括静止状态区域和非静止状态区域，所述匹配参考数据位于所述静止状态区域中，所述基于所述当前位姿和所述静态点云数据更新所述环境信息，包括：

根据所述当前位姿，将所述静态激光点云数据投影到所述占据栅格地图中，得到所述静态激光点云数据在所述占据栅格地图中对应的第一激光占据区域；

根据所述第一激光占据区域，统计所述静止状态区域内每一所述占据栅格集合的语义信息，以更新所述静止状态区域的语义信息集合；

统计第一预设时间段内语义信息集合中每一所述语义信息对应的占据栅格集合在所述栅格地图中的存在时间，所述语义信息集合包括所述静止状态区域内所述占据栅格集合对应的语义信息；

统计第二预设时间段内所述语义信息集合中所有语义信息对应的占据栅格集合在所述栅格地图中的存在时间之和。

8.根据权利要求7所述的方法，其特征在于，所述根据所述第一激光占据区域，统计所述静止状态区域内每一所述占据栅格集合的语义信息，以更新所述静止状态区域的语义信息集合，包括：

筛选所述第一激光占据区域中位于所述静止状态区域的第二激光占据区域；

提取所述第二激光占据区域中激光占据栅格集合的激光语义信息；

确定与所述激光占据栅格集合距离最近的占据栅格集合；

比对所述激光语义信息和所述距离最近的占据栅格集合的语义信息；

若所述激光语义信息和所述距离最近的占据栅格集合的语义信息不一致，则增加所述激光语义信息到所述语义信息集合。

9.根据权利要求7所述的方法，其特征在于，所述匹配参考数据位于所述静止状态区域中，所述根据所述第一激光占据区域，统计所述静止状态区域内每一所述占据栅格集合的语义信息，以更新所述静止状态区域的语义信息集合之前，包括：

筛选所述第一激光占据区域中位于所述非静止状态区域的第三激光占据区域；

确定与所述第三激光占据区域的距离小于第四阈值的第四激光占据区域；

将所述第三激光占据区域和所述第四激光占据区域划分到所述静止状态区域，以及将所述第三激光占据区域和所述第四激光占据区域从所述非静止状态区域中剔除。

10.根据权利要求9所述的方法，其特征在于，所述根据所述第一激光占据区域，统计所述静止状态区域内每一所述占据栅格集合的语义信息，以更新所述静止状态区域的语义信息集合，还包括：

提取所述第三激光占据区域中激光占据栅格集合的激光语义信息；

将所述激光语义信息添加到所述语义信息集合中。

11.根据权利要求8或10任一所述的方法，其特征在于，所述语义信息包括占据栅格的几何特征、图像特征和物体属性标签，所述提取所述第二激光占据区域/第三激光占据区域中激光占据栅格集合的激光语义信息，包括：

识别所述第二激光占据区域/第三激光占据区域中多个占据栅格的几何特征；

将所述第二激光占据区域/第三激光占据区域中构成所述几何特征的所述多个占据栅格聚类为所述激光占据栅格集合；

获取所述激光占据栅格集合对应的图像信息；

识别所述图像信息中的图像特征；

根据所述图像特征，确定所述激光占据栅格集合的物体属性标签。

12.根据权利要求1所述的方法，其特征在于，所述筛选所述静态激光点云数据中与所述占据栅格集合距离最近且所述语义信息一致的激光点群，包括：

对所述静态激光点云数据进行分类，得到分类静态激光点云数据；

分别剔除所述分类静态激光点云数据中的外点，得到用于匹配的有效激光点云数据；

匹配所述有效激光点云数据中与所述占据栅格集合距离最近且所述语义信息一致的激光点群。

13.根据权利要求12所述的方法，其特征在于，所述匹配参考数据包括先验匹配参考数据和非先验匹配参考数据，所述对所述静态激光点云数据进行分类，得到分类静态激光点云数据，包括：

将所述静态激光点云数据投影到栅格地图坐标系中，得到地图坐标系激光点云数据；

计算所述地图坐标系激光点云数据中与所述匹配参考数据距离最近地图坐标系激光点；

将与所述先验匹配参考数据距离最近的地图坐标系激光点聚类为第一分类静态激光点云数据，以及将与所述非先验匹配参考数据距离最近的地图坐标系激光点聚类为第二分类静态激光点云数据。

14.根据权利要求13所述的方法，其特征在于，所述分别剔除所述分类静态激光点云数据中的外点，得到用于匹配的有效激光点云数据，包括：

计算所述第一分类静态激光点云数据中第一分类静态激光点与所述先验匹配参考数据中距离最近的占据栅格的第二距离；

计算所述第二分类静态激光点云数据中第二分类静态激光点与所述非先验匹配参考数据中距离最近的占据栅格的第三距离；

剔除所述第一分类静态激光点数据中所述第二距离大于第十一阈值对应的第一分类静态激光点，得到第一有效激光点云数据；

剔除所述第二分类静态激光点数据中所述第三距离大于第十一阈值对应的第二分类静态激光点，得到第二有效激光点云数据。

15.根据权利要求14所述的方法，其特征在于，所述匹配所述有效激光点云数据中与所述占据栅格集合距离最近且所述语义信息一致的激光点群，包括：

确定第一有效激光点云数据中与所述先验匹配参考数据中的先验占据栅格集合距离最近的第一激光点群；

确定第二有效激光点云数据中与所述非先验匹配参考数据中的非先验占据栅格集合距离最近的第二激光点群；

分别提取所述第一激光点群、所述先验占据栅格集合、所述第二激光点群和所述非先验占据栅格集合的语义信息；

比对所述第一激光点群与所述先验占据栅格集合的语义信息，确定所述语义信息一致的所述第一激光点群与所述先验占据栅格集合；

比对所述第二激光点群与所述非先验占据栅格集合的语义信息，确定所述语义信息一致的所述第二激光点群与所述非先验占据栅格集合。

16.根据权利要求15所述的方法，其特征在于，所述基于所述激光点群和所述占据栅格集合确定所述机器人的当前位姿，包括：

获取所述第一激光点群的第一权重，所述第二激光点群的第二权重；

基于所述第一激光点群、所述先验占据栅格集合和所述第一权重，所述第二激光点群、所述非先验占据栅格集合和所述第二权重，计算所述机器人的当前位姿。

17.根据权利要求16所述的方法，其特征在于，所述非先验匹配数据包括近距离非先验匹配参考数据和远距离非先验匹配参考数据，所述近距离非先验匹配参考数据包括近距离非先验占据栅格集合，所述远距离非先验匹配参考数据包括远距离非先验占据栅格集合，所述第二激光点群包括与所述近距离非先验占据栅格集合对应的近距离激光点群和与所述远距离非先验占据栅格集合对应的远距离激光点群，所述基于所述激光点群和所述占据栅格集合确定所述机器人的当前位姿，包括：

获取所述第一激光点群的第一权重，所述近距离激光点群的第三权重，和所述远距离激光点群的第四权重，所述第一权重大于所述第三权重，所述第三权重大于所述第四权重；

基于所述第一激光点群、所述先验占据栅格集合和所述第一权重，所述近距离激光点群、近距离非先验占据栅格集合和所述第三权重，所述远距离激光点群、远距离非先验占据栅格集合和所述第四权重计算所述机器人的当前位姿。

18.一种机器人，其特征在于，所述设备包括：处理器、摄像装置和激光雷达，所述处理器耦接所述摄像装置和所述激光雷达，在工作时执行指令，以配合所述摄像装置、所述激光雷达执行权利要求1至17任一所述的机器人的位姿估计方法的步骤。

19.一种计算机存储介质，其特征在于，所述计算机存储介质存储有计算机程序，所述计算机程序被处理器执行以实现如权利要求1至17中任一项所述的机器人的位姿估计方法的步骤。

Images