机器人定位方法、装置、机器人及存储介质与流程

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


背景技术：

2.智能巡检机器人在作业时主要通过采用即时定位与地图构建(simultaneous localization and mapping，slam)技术实现定位，其中基于3d激光雷达的slam技术需要3d点云地图，而3d点云地图的数据量及计算量较大，因此如何高效加载地图是定位技术中的关键问题。
3.目前，针对定位技术的地图加载主要包括离线地图分割和根据中心点遍历搜索等方式。其中，对于离线地图分割方式，一般是将离线地图分割成块并对每个块编号，以便根据实际定位需求加载对应编号的点云地图块，但是该方式需要额外算法对地图块进行管理，导致开发工作量加大，同时对于分块过多时容易导致分块出错。而对于根据中心点遍历搜索方式，虽然其可以通过限定地图范围以减少点云匹配，但是数据遍历方式需要消耗大量计算资源。


技术实现要素：

4.本技术提供了一种机器人定位方法、装置、机器人及存储介质，以解决当前定位方法存在计算资源消耗大的技术问题。
5.为了解决上述技术问题，第一方面，本技术提供了一种机器人定位方法，包括：
6.对机器人当前位置的第一点云地图进行直通滤波，得到在当前位置的预设范围内的第二点云地图；
7.对第二点云地图进行去地面化滤波处理，得到第三点云地图；
8.对第三点云地图进行去离群点处理，得到第四点云地图；
9.对第四点云地图进行下采样处理，得到第五点云地图；
10.将第五点云地图与机器人实时采集的激光点云进行匹配，确定机器人的最新当前位置。
11.本技术通过对机器人当前位置的第一点云地图进行直通滤波，得到在当前位置的预设范围内的第二点云地图，以避免加载地图范围大而导致加载效率低和计算资源消耗大的问题，同时采用直通滤波方式动态加载地图，能够保留点云地图精度；再对第二点云地图进行去地面化滤波处理，得到第三点云地图，对第三点云地图进行去离群点处理，得到第四点云地图，以及对第四点云地图进行下采样处理，得到第五点云地图，从而消除地面噪声和稀疏离群噪声以及降低点云密度，以提高点云地图的精度和降低运算量；最后将第五点云地图与机器人实时采集的激光点云进行匹配，确定机器人的最新当前位置，以利用去噪噪声和低密度的点云地图进行点云匹配，进一步降低计算机资源消耗。
12.作为优选，对机器人当前位置的第一点云地图进行直通滤波，得到在当前位置的
预设范围内的第二点云地图，包括：
13.以第一点云地图的目标坐标轴方向，对第一点云地图进行直通滤波，去除第一点云地图中不在目标坐标轴方向上的预设范围内的点集，得到第二点云地图。
14.作为优选，对第二点云地图进行去地面化滤波处理，得到第三点云地图，包括：
15.基于第二点云地图中的种子点集，建立初始平面，
16.确定第二点云地图中每个点与初始平面的正交投影距离；
17.去除第二点云地图中正交投影距离小于预设距离的点，得到第三点云地图。
18.作为优选，对第三点云地图进行去离群点处理，得到第四点云地图，包括：
19.确定第三点云地图中每个点与多个邻居点之间平均距离；
20.计算多个平均距离的均值和标准差；
21.根据均值和标准差，确定目标距离阈值；
22.去除第三点云地图中平均距离大于目标距离阈值的点，得到第四点云地图。
23.作为优选，将第五点云地图与机器人实时采集的激光点云进行匹配，确定机器人的最新当前位置，包括：
24.对激光点云进行网格化，得到多个点云网格；
25.计算每个点云网格的概率密度函数；
26.对第五点云地图中的点集与点云网格进行匹配，并根据概率密度函数，确定点集的目标打分函数；
27.利用牛顿法，根据目标打分函数，确定梯度矩阵和海森矩阵；
28.更新梯度矩阵和海森矩阵，直至目标打分函数达到预设收敛条件，将第五点云地图中与点云网格匹配的点集所对应的位置确定为机器人的最新当前位置。
29.作为优选，对机器人当前位置的第一点云地图进行直通滤波，得到在当前位置的预设范围内的第二点云地图之前，还包括：
30.利用全球定位系统gps，对机器人进行初始定位，得到机器人的初始当前位置。
31.作为优选，将第五点云地图与机器人实时采集的激光点云进行匹配，确定机器人的最新当前位置之后，还包括：
32.若机器人定位失效，则利用全球定位系统gps和实时动态载波相位差分技术或实时阵列校准技术，对机器人进行重定位。
33.第二方面，本技术提供一种机器人定位装置，包括：
34.滤波模块，用于对机器人当前位置的第一点云地图进行直通滤波，得到在当前位置的预设范围内的第二点云地图；
35.第一处理模块，用于对第二点云地图进行去地面化滤波处理，得到第三点云地图；
36.第二处理模块，用于对第三点云地图进行去离群点处理，得到第四点云地图；
37.第三处理模块，用于对第四点云地图进行下采样处理，得到第五点云地图；
38.匹配模块，用于将第五点云地图与机器人实时采集的激光点云进行匹配，确定机器人的最新当前位置。
39.第三方面，本技术提供一种机器人，包括处理器和存储器，存储器用于存储计算机程序，计算机程序被处理器执行时实现如第一方面的机器人定位方法。
40.第四方面，本技术提供一种计算机可读存储介质，其存储有计算机程序，计算机程
序被处理器执行时实现如第一方面的机器人定位方法。
41.需要说明的是，上述第二方面至第四方面的有益效果请参见上述第一方面的相关描述，在此不再赘述。
附图说明
42.图1为本技术一实施例示出的机器人定位方法的流程示意图；
43.图2为本技术另一实施例示出的机器人定位方法的流程示意图；
44.图3为本技术实施例示出的机器人定位装置的结构示意图；
45.图4为本技术实施例示出的机器人的结构示意图。
具体实施方式
46.下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本技术一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本技术保护的范围。
47.如相关技术记载，对于离线地图分割方式，一般是将离线地图分割成块并对每个块编号，以便根据实际定位需求加载对应编号的点云地图块，但是该方式需要额外算法对地图块进行管理，导致开发工作量加大，同时对于分块过多时容易导致分块出错。而对于根据中心点遍历搜索方式，虽然其可以通过限定地图范围以减少点云匹配，但是数据遍历方式需要消耗大量计算资源。
48.为此，本技术实施例提供一机器人定位方法，通过对机器人当前位置的第一点云地图进行直通滤波，得到在所述当前位置的预设范围内的第二点云地图，以避免加载地图范围大而导致加载效率低和计算资源消耗大的问题，同时采用直通滤波方式动态加载地图，能够保留点云地图精度；再对所述第二点云地图进行去地面化滤波处理，得到第三点云地图，对所述第三点云地图进行去离群点处理，得到第四点云地图，以及对所述第四点云地图进行下采样处理，得到第五点云地图，从而消除地面噪声和稀疏离群噪声以及降低点云密度，以提高点云地图的精度和降低运算量；最后将所述第五点云地图与所述机器人实时采集的激光点云进行匹配，确定所述机器人的最新当前位置，以利用去噪噪声和低密度的点云地图进行点云匹配，进一步降低计算机资源消耗。
49.请参照图1，图1为本技术实施例提供的一种机器人定位方法的流程示意图。本技术实施例的机器人定位方法可应用于机器人，该机器人包括但不限于巡检机器人和扫地机器人等设备。如图1所示，本实施例的机器人定位方法包括步骤s101至步骤s105，详述如下：
50.步骤s101，对机器人当前位置的第一点云地图进行直通滤波，得到在所述当前位置的预设范围内的第二点云地图。
51.在本步骤中，考虑到地图范围大而导致点云地图加载查找过慢的问题，本实施例采用动态加载地图的方式，在不以牺牲点云地图精度的前提下，根据机器人当前位置实现小范围内的地图发布与搜索，提高效率与准确性。而对于小范围地图提取，避免了暴力搜索点的方式，从而降低计算机资源消耗。
52.步骤s102，对所述第二点云地图进行去地面化滤波处理，得到第三点云地图。
53.在本步骤中，去地面化滤波处理可以包括基于地面射线过滤(ray ground filter)的处理方式和基于地平面拟合(ground plane fitting)的滤波方式。其中ray ground filter方式的核心是以射线(ray)的形式组织点云，通过判断在近似地面高度范围内，地面射线中前后两点的坡度是否大于坡度阈值，从而判断点是否为地面点。
54.步骤s103，对所述第三点云地图进行去离群点处理，得到第四点云地图。
55.在本步骤中，可以通过statisticaloutlierremoval滤波器移除离群点，以去除稀疏离群噪点。在采集点云的过程中，由于测量噪声的影响而引入部分离群噪点，它们在点云空间中分布稀疏。而在估算点云局部特征(例如计算采样点处的法向量和曲率变化率)时，这些噪点可能导致计算结果出错，从而导致点云配准等后期处理失败，所以本实施例通过去离群点处理以解决上述问题。
56.步骤s104，对所述第四点云地图进行下采样处理，得到第五点云地图。
57.在本步骤中，由于点云数据庞大，因此往往会对于点云进行下采样，即在尽可能保留点云信息与特征的前提下，降低点云密度，从而达到减少计算量的目的。点云下采样包括但不限于均匀下采样、体素下采样、曲率下采样、曲率均匀采样和泊松磁盘采样等。优选地，为了保证采样的均匀以及采样效率，下采样处理为体素下采样，通过将三维空间体素化，在每个体素里采样一个点，通常可用中心点或最靠近中心的点作为采样点。具体地：计算点云的包围盒，将包围盒离散成小体素，其中体素的长宽高可以用户设定，也可以通过设定包围盒三个方向的格点数来求得；每个小体素包含了若干个点，取中心点或离中心点最近的点为采样点。
58.步骤s105，将所述第五点云地图与所述机器人实时采集的激光点云进行匹配，确定所述机器人的最新当前位置。
59.在本步骤中，通过应用ndt点云匹配进行精确定位。确定初始定位后，会通过非线性卡尔曼滤波将imu与前时刻状态进行最新的机器人系统状态估计，结合雷达点云的匹配结果，更新获得准确的机器人定位。在定位结果更新后，系统会记录更新后的定位信息并根据更新后的位置重新进行直通滤波，获取机器人定位周边小范围地图，用于新一轮定位信息的迭代计算。
60.需要说明的是，本技术能够应用于室内或室外园区式环境，一般园区场景会有较多树木、房屋、物品等定位特征，当园区占地面积较大或环境非结构化程度较高时，经过激光雷达等传感器建立的雷达点云地图也会对内存空间与定位资源消耗造成较大的负担，因此本技术通过结合定位结果反馈，实现动态加载地图，在保留点云高分辨率的情况下时间与资源成本，并对于动态地图进行相关预处理以及辅助重定位等功能。
61.在一实施例中，在图1所示实施例的基础上，所述步骤s101，包括：
62.以所述第一点云地图的目标坐标轴方向，对所述第一点云地图进行直通滤波，去除所述第一点云地图中不在所述目标坐标轴方向上的预设范围内的点集，得到所述第二点云地图。
63.在本实施例中，直通滤波为根据预设范围遍历点云内的所有点，并将不在规定范围内的点全部去除，具体地，沿某个坐标轴方向的预设范围，应用直通滤波器选择并保留其中一段范围内的点集。相比于暴力遍历方式能够大大降低运算量，以降低计算机的资源消耗。
64.在一实施例中，在图1所示实施例的基础上，所述步骤s102，包括：
65.基于所述第二点云地图中的种子点集，建立初始平面，
66.确定所述第二点云地图中每个点与所述初始平面的正交投影距离；
67.去除所述第二点云地图中所述正交投影距离小于预设距离的点，得到所述第三点云地图。
68.在本实施例中，由于raygroundfilter方式存在一定不足，所以实施例采用平面模型(planemodel)来拟合当前的地面。其中现实地面的不平整和激光雷达的测量噪声，因此单一的平面模型并不足以描述实际情况。为处理这种情况，沿着x方向(机器人前进方向)将空间分割成若干个子平面，然后对每个子平面使用地面平面拟合算法(gpf)从而得到能够处理陡坡的地面分割方法。
69.可选地，首先选取一个种子点集(seedpointset)，这些种子点来源于点云中高度(即z值)较小的点，种子点集被用于建立描述地面的初始平面模型。随后初始化一个平面ax+by+cz+d＝0，计算点云中的点到这个平面的正交投影距离，设定阈值判断点集中的每个点是否在地面上，若点到平面高度差小于阈值，认为该点属于地面，反之则为非地面点。经过分类以后的所有地面点被当作下一次迭代的种子点集，迭代优化，最终获得地面点并进行去除。
70.在一实施例中，在图1所示实施例的基础上，所述步骤s103，包括：
71.确定所述第三点云地图中每个点与多个邻居点之间平均距离；
72.计算多个所述平均距离的均值和标准差；
73.根据所述均值和标准差，确定目标距离阈值；
74.去除所述第三点云地图中平均距离大于所述目标距离阈值的点，得到所述第四点云地图。
75.在本实施例中，可以利用统计滤波器实现去离群点处理。统计滤波器的主要思想是假设点云中所有的点与其最近的k个邻居点的平均距离满足高斯分布，那么，根据均值和方差可确定一个距离阈值，当某个点与其最近k个点的平均距离大于这个阈值时，判定该点为离群点并去除。
76.可选地，遍历点云，计算每个点与其最近的k个邻居点之间的平均距离；计算所有平均距离的均值μ与标准差σ，则距离阈值dmax可表示为dmax＝μ+α
×
σ，其中α为比例系数，属于常数，其取决于邻居点的数目；再次遍历点云，剔除与k个邻居点的平均距离大于dmax的点。
77.在一实施例中，在图1所示实施例的基础上，所述步骤s105，包括：
78.对所述激光点云进行网格化，得到多个点云网格；
79.计算每个所述点云网格的所述概率密度函数；
80.对所述第五点云地图中的点集与所述点云网格进行匹配，并根据所述概率密度函数，确定所述点集的目标打分函数；
81.利用牛顿法，根据所述目标打分函数，确定梯度矩阵和海森矩阵；
82.更新所述梯度矩阵和所述海森矩阵，直至所述目标打分函数达到预设收敛条件，将所述第五点云地图中与所述点云网格匹配的所述点集所对应的位置确定为所述机器人的最新当前位置。
83.在本实施例中，计算每个点云网格的多维正态分布参数(如均值、协方差矩阵)，利用多维正态分布参数确定概率密度函数pdf；根据pdf，确定第五点云地图中的点集与点云网格的匹配结果的打分函数；利用牛顿法，对打分函数进行梯度下降，得到梯度矩阵g和海森矩阵h；更新g和h，直至打分函数达到预设值或迭代次数达到预设次数，将打分函数收敛时与所述点云网格匹配的点集所对应的位置作为最新当前位置。
84.在一实施例中，在图1所示实施例的基础上，图2示出了本技术实施例提供的另一种机器人定位方法的流程示意图。可以理解的是，与图1实施例相同的步骤在此不再赘述。如图2所示，所述步骤s101之前，还包括：
85.利用全球定位系统gps，对所述机器人进行初始定位，得到所述机器人的初始当前位置。
86.在本实施例中，利用gps初始化机器人位置，以为后续基于点云地图定位提供初始定位依据。
87.在一实施例中，在图1所示实施例的基础上，如图2所示，所述步骤s105之后，还包括：
88.若所述机器人定位失效，则利用全球定位系统gps和实时动态载波相位差分技术或实时阵列校准技术，对所述机器人进行重定位。
89.在本实施例中，对于定位丢失时的重定位问题，在代码运行过程中，根据定位更新不断记录，在定位丢失后则会根据定位丢失前记录的最新位置发布地图，确保机器人位于发布地图范围内，重新尝点云匹配。同时为了提高匹配效率，本技术通过触发并接收机器人gps信息的方式，获取机器人大概位置从而较快实现重定位。
90.可以理解的是，本技术的动态地图加载除了初始化位置外，全部依赖机器人的定位结果，从而保证地图范围有效。同时为保证定位丢失后地图范围有效，对机器人的最新位置进行实时保存，并在定位丢失后，以最后的有效定位为中心进行动态地图发布，因此短时间内机器人不会超出地图有效范围，同时为加速重定位并提高其准确性，通过实时阵列校准技术rac或实时动态载波相位差分技术rtk辅助进行快速重定位，在定位丢失而gps信息有效时，会以gps位置为参考进行快速定位，并基于该结果进行后续的动态地图发布，及时恢复定位有效性。为了执行上述方法实施例对应的机器人定位方法，以实现相应的功能和技术效果。参见图3，图3示出了本技术实施例提供的一种机器人定位装置的结构框图。为了便于说明，仅示出了与本实施例相关的部分，本技术实施例提供的机器人定位装置，包括：
91.滤波模块301，用于对机器人当前位置的第一点云地图进行直通滤波，得到在所述当前位置的预设范围内的第二点云地图；
92.第一处理模块302，用于对所述第二点云地图进行去地面化滤波处理，得到第三点云地图；
93.第二处理模块303，用于对所述第三点云地图进行去离群点处理，得到第四点云地图；
94.第三处理模块304，用于对所述第四点云地图进行下采样处理，得到第五点云地图；
95.匹配模块305，用于将所述第五点云地图与所述机器人实时采集的激光点云进行匹配，确定所述机器人的最新当前位置。
96.在一实施例中，所述滤波模块301，具体用于：
97.以所述第一点云地图的目标坐标轴方向，对所述第一点云地图进行直通滤波，去除所述第一点云地图中不在所述目标坐标轴方向上的预设范围内的点集，得到所述第二点云地图。
98.在一实施例中，所述第一处理模块302，具体用于：
99.基于所述第二点云地图中的种子点集，建立初始平面，
100.确定所述第二点云地图中每个点与所述初始平面的正交投影距离；
101.去除所述第二点云地图中所述正交投影距离小于预设距离的点，得到所述第三点云地图。
102.在一实施例中，所述第二处理模块303，具体用于：
103.确定所述第三点云地图中每个点与多个邻居点之间平均距离；
104.计算多个所述平均距离的均值和标准差；
105.根据所述均值和标准差，确定目标距离阈值；
106.去除所述第三点云地图中平均距离大于所述目标距离阈值的点，得到所述第四点云地图。
107.在一实施例中，所述第三处理模块304，具体用于：
108.对所述激光点云进行网格化，得到多个点云网格；
109.计算每个所述点云网格的所述概率密度函数；
110.对所述第五点云地图中的点集与所述点云网格进行匹配，并根据所述概率密度函数，确定所述点集的目标打分函数；
111.利用牛顿法，根据所述目标打分函数，确定梯度矩阵和海森矩阵；
112.更新所述梯度矩阵和所述海森矩阵，直至所述目标打分函数达到预设收敛条件，将所述第五点云地图中与所述点云网格匹配的所述点集所对应的位置确定为所述机器人的最新当前位置。
113.在一实施例中，所述机器人定位装置，还包括：
114.初定位模块，用于利用全球定位系统gps，对所述机器人进行初始定位，得到所述机器人的初始当前位置。
115.在一实施例中，所述机器人定位装置，还包括：
116.重定位模块，用于若所述机器人定位失效，则利用全球定位系统gps和实时动态载波相位差分技术或实时阵列校准技术，对所述机器人进行重定位。
117.上述的机器人定位装置可实施上述方法实施例的机器人定位方法。上述方法实施例中的可选项也适用于本实施例，这里不再详述。本技术实施例的其余内容可参照上述方法实施例的内容，在本实施例中，不再进行赘述。
118.图4为本技术一实施例提供的机器人的结构示意图。如图4所示，该实施例的机器人4包括：至少一个处理器40(图4中仅示出一个)处理器、存储器41以及存储在所述存储器41中并可在所述至少一个处理器40上运行的计算机程序42，所述处理器40执行所述计算机程序42时实现上述任意方法实施例中的步骤。
119.所述机器人4可以是巡检机器人、扫地机器人等设备。该机器人可包括但不仅限于处理器40、存储器41。本领域技术人员可以理解，图4仅仅是机器人4的举例，并不构成对机
器人4的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如还可以包括输入输出设备、网络接入设备等。
120.所称处理器40可以是中央处理单元(central processing unit，cpu)，该处理器40还可以是其他通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路(application specific integrated circuit，asic)、现成可编程门阵列(field－programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
121.所述存储器41在一些实施例中可以是所述机器人4的内部存储单元，例如机器人4的硬盘或内存。所述存储器41在另一些实施例中也可以是所述机器人4的外部存储设备，例如所述机器人4上配备的插接式硬盘，智能存储卡(smart media card，smc)，安全数字(secure digital，sd)卡，闪存卡(flash card)等。进一步地，所述存储器41还可以既包括所述机器人4的内部存储单元也包括外部存储设备。所述存储器41用于存储操作系统、应用程序、引导装载程序(bootloader)、数据以及其他程序等，例如所述计算机程序的程序代码等。所述存储器41还可以用于暂时地存储已经输出或者将要输出的数据。
122.另外，本技术实施例还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述任意方法实施例中的步骤。
123.本技术实施例提供了一种计算机程序产品，当计算机程序产品在机器人上运行时，使得机器人执行时实现上述各个方法实施例中的步骤。
124.在本技术所提供的几个实施例中，可以理解的是，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意的是，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。
125.所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本技术的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台机器人执行本技术各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read－only memory)、随机存取存储器(ram，random access memory)、磁碟或者光盘等各种可以存储程序代码的介质。
126.以上所述的具体实施例，对本技术的目的、技术方案和有益效果进行了进一步的详细说明，应当理解，以上所述仅为本技术的具体实施例而已，并不用于限定本技术的保护范围。特别指出，对于本领域技术人员来说，凡在本技术的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本技术的保护范围之内。