CN111765883A - 机器人蒙特卡罗定位方法、设备及存储介质 - Google Patents

Abstract

本申请公开了一种机器人蒙特卡罗定位方法、设备及存储介质，该方法包括获取机器人测量的点云数据和里程计数据；基于里程计数据利用运动模型获取粒子集中每个粒子的当前位姿；基于点云数据结合使用两种观测模型计算每个粒子的权重；利用粒子的当前位姿和权重获取机器人的位姿。通过上述方式，本申请能够提高机器人的定位精度。

Description

机器人蒙特卡罗定位方法、设备及存储介质

技术领域

本申请涉及智能机器人技术领域，特别是涉及机器人蒙特卡罗定位方法、设备及存储介质。

背景技术

随着机器人相关技术的快速发展，对机器人自主导航功能需求越来越高，对于移动机器人来说，定位就是通过传感器信息确定机器人在环境地图中的位姿(包括位置和姿态)，它是移动机器人实现自主导航的关键技术。

目前解决机器人估计自身在环境地图中的位置通常采用蒙特卡罗定位算法和马尔可夫定位算法，其中蒙特卡罗定位算法实现简单、定位精度高、所需计算资源少而被广泛应用。但是现有利用蒙特卡罗定位算法对机器人进行定位时还存在定位精度低的问题。

发明内容

本申请主要解决的技术问题是提供一种机器人蒙特卡罗定位方法、设备及存储介质，能够提高机器人的定位精度。

为解决上述技术问题，本申请采用的一个技术方案是：提供一种机器人蒙特卡罗定位方法，该方法包括获取机器人测量的点云数据和里程计数据；基于里程计数据利用运动模型获取粒子集中每个粒子的当前位姿；基于点云数据结合使用两种观测模型计算每个粒子的权重；利用粒子的当前位姿和权重获取机器人的位姿。

为解决上述技术问题，本申请采用的另一个技术方案是：提供一种机器人蒙特卡罗定位设备，该设备包括处理器，处理器用于执行指令以实现上述的机器人蒙特卡罗定位方法。

为解决上述技术问题，本申请采用的又一个技术方案是：提供一种计算机可读存储介质，该计算机可读存储介质用于存储指令/程序数据，指令/程序数据能够被执行以实现上述的机器人蒙特卡罗定位方法。

本申请的有益效果是：区别于现有技术的情况，本申请通过结合使用两种观测模型计算粒子权重，能够在减少计算量的同时，还可以减少地图中观测数据的假阳性匹配，提高定位精度。

附图说明

图1是本申请实施方式中一机器人蒙特卡罗定位方法的流程示意图；

图2是本申请实施方式中结合使用两种观测模型计算粒子权重的流程示意图；

图3是本申请实施方式中对点云数据进行网格划分的流程示意图；

图4是本申请实施方式中对点云数据进行网格划分的示意图；

图5是本申请实施方式中另一机器人蒙特卡罗定位方法的流程示意图；

图6是本申请实施方式中机器人蒙特卡罗定位装置的结构示意图；

图7是本申请实施方式中机器人蒙特卡罗定位设备的结构示意图；

图8为本申请实施方式中计算机可读存储介质的结构示意图。

具体实施方式

为使本申请的目的、技术方案及效果更加清楚、明确，以下参照附图并举实施例对本申请进一步详细说明。

本申请提供一种机器人的定位方法，可用于对移动机器人定位，具体是基于蒙特卡罗定位方法对移动机器人进行定位，该方法中结合使用了似然域模型和波束模型两种观测模型计算粒子权重，能够在减少计算量的同时，还可以减少地图中观测数据的假阳性匹配，提高定位精度。

请参阅图1，图1是本申请实施方式中一机器人蒙特卡罗定位方法的流程示意图。需注意的是，若有实质上相同的结果，本实施例并不以图1所示的流程顺序为限。如图1所示，本实施方式包括：

S110：获取机器人测量的点云数据和里程计数据。

其中，可以利用三维激光雷达获取点云数据，通过获取光脉冲打在物体上并反射回到接收器的传播时间，再根据光速已知的原理，将传播时间转换成据测量物的距离；然后对数据进行记录、分类和采集，形成三维激光点云数据，点云中每一个点包含有三维坐标和激光反射强度信息。在其他实施方式中，还可以利用其他方式获取点云数据，本申请中不对点云数据的获取方式做限定。下文中将以点云数据为激光点云数据为例对本申请的技术方案进行说明。

S120：基于里程计数据利用运动模型获取粒子集中每个粒子的当前位姿。

其中，蒙特卡罗定位算法是一种利用粒子表示置信度的定位算法。通过获取粒子集

中每个粒子的位姿和权重，对机器人的位姿进行估计。

该实施方式中，利用里程计运动模型来计算粒子位姿。

其中，里程计运动模型使用相对运动信息(relative motion information)，该信息由机器人内部的里程计测量。更具体地，在时间间隔(t-1,t]内，机器人从位姿x_t-1前进到位姿x_t，里程计反馈了从

到

的相对前进。这里“—”代表的是嵌在机器人内部的坐标。

为了提取相对距离，运动信息u_t被转变成三个步骤的序列：旋转、直线运动(平移)和另一个旋转，即初始旋转δ_rot1、平移δ_trans和第二次旋转δ_rot2。获取粒子的位姿包括：

根据里程计数据

计算初次旋转δ_rot1、平移δ_trans和第二次旋转δ_rot2，其中：

其中，里程计数据u_t表示从t-1时刻到t时刻机器人的相对运动。

为t-1时刻的机器人在其内部坐标系下的位姿，

为

的横坐标，

为

的纵坐标，

为

的航向角。

为t时刻的机器人在其内部坐标系下的位姿，

为

的横坐标，

为

的纵坐标，

为

的航向角。

对初次旋转δ_rot1、平移δ_trans和第二次旋转δ_rot2进行高斯采样：

其中，sample(σ)表示均值为0，偏差为σ的高斯采样，α_i表示给定的参数。根据上一时刻位姿x_t-1＝(x′_t-1y′_t-1θ′_t-1)和进行高斯采样的旋转平移量更新粒子的位姿x_t＝(x′_ty′_tθ′_t)：

S130：基于点云数据结合使用两种观测模型计算粒子集中每个粒子的权重。

粒子的权重是观测量z_t在粒子

下的概率，用

给定。

观测模型描述了根据运动模型预计的机器人状态与观测结果的一种匹配程度，并以此来校正机器人状态。在移动机器人中，常用的传感器是测距传感器，如超声波测距传感器和激光测距传感器。无论是哪一种传感器都不可避免得存在噪声，观测模型能够将传感器的各类噪声定量描述出来。概率机器人可为传感器测量噪声建立模型，所建立的模型考虑了机器人传感器固有的不确定性。形式上，观测模型定义为一个条件概率分布p(z_t|x_t,m)，这里，x_t是机器人的位姿，z_t是t时刻的观测量，m是环境地图。

利用观测模型可获得粒子的权重。常用观测模型有似然域模型、波束模型、基于相关性的测量模型、基于特征的测量模型等。但是各个观测模型都存在一定的优点和缺点。如波束模型是测距仪的近似物理模型，与测距仪的几何学和物理学紧密联系，能够减少地图中观测数据的假阳性匹配；但是波束模型缺乏光滑性，会导致定位结果不准确，同时波束模型还需要很大的计算量。似然域模型能够克服上述波束模型的两大缺点，但是似然域模型缺乏一个合理的物理解释，地图中的观测数据容易出现假阳性匹配。因此，单一使用某一观测模型来计算粒子的权重效果不好，进而会影响机器人的定位精度。本申请中提供的定位方法，结合了两种观测模型来计算粒子的权重，可综合两种观测模型的优点，以提高所得数据的精度。

S140：利用粒子的当前位姿和权重获取机器人的位姿。

其中，根据粒子集χ_t中粒子的权重和位姿，计算粒子位姿的期望作为机器人的估计位姿，估计位姿的计算公式为：

其中，

为粒子权重

归一化，

为第n个粒子的位姿。

通过该实施方式的实施，在利用蒙特卡罗定位方法对机器人进行定位时，结合使用了两种观测模型计算粒子权重，可综合两种观测模型的优点，能够在减少计算量的同时，提高定位精度。

在一实施方式中，可结合使用似然域模型和波束模型两种观测模型来计算粒子的权重，能够实现在好的匹配结果的同时减少计算量。但不限于此，在其他实施方式中，还可以结合使用其他观测模型来计算粒子的权重，如结合使用基于相关性的测量模型和基于特征的测量模型来计算粒子的权重。为了进一步提高定位精度，还可以结合使用三个或更多个的观测模型来计算粒子的权重。下面，将以结合使用似然域模型和波束模型两种观测模型来计算粒子的权重为例对本申请的技术方案进行说明。

具体地，可分别对点云数据进行似然域采样和波束采样，得到第一数量M_likelihood的似然域采样点和第二数量M_beam的波束采样点。再利用似然域模型计算每个似然域采样点的测量概率值，利用波束模型计算每个波束采样点的测量概率值。然后将所有采样点的测量概率值进行累乘，得到总采样点的测量概率值。再利用总采样点的测量概率值计算观测模型的测量概率值(即粒子的权重)。

其中，波束采样点的数量应远小于似然域采样点的数量，以减少总计算量，提高定位精度。波束采样点可以与似然域采样点重复，也可以不与似然域采样点重复。

在一实施方式中，可以将得到的采样点划分为地图中障碍物点云和非地图中障碍物点云(C＝{mapped,unmapped})，分别计算地图中障碍物点云的测量概率值和非地图中障碍物点云的测量概率值。在其他实施方式中，也可以是在采样前对点云进行分类。

其中，由于激光数据存在噪声、移动机器人在测量过程中存在激光打地、环境中新增了障碍物等原因，每一帧的点云数据并不一定都是地图中障碍物，这些非地图中障碍物点云会给机器人位姿状态估计带来影响。本申请中所提供的定位方法，可先对点云进行分类，分别计算各类点云的测量概率值，能够减少非地图中障碍物点云给机器人位姿状态估计带来的影响，提高定位精度。

可设置地图中障碍物在栅格地图中所在栅格距离其最近占据栅格的最大距离阈值λ_max(即第一阈值)，计算栅格地图中每个栅格距离其最近占据栅格的距离λ，将栅格地图转化成距离地图。获取激光点所落的栅格距离其最近占据栅格的距离λ，若λ<λ_max则该点云为mapped,否则为unmapped。栅格地图是预先获取的，可通过获取机器人所在环境的三维点云地图，然后根据设定的分辨率转换成三维栅格地图。

请参阅图2，图2是本申请实施方式中结合使用两种观测模型计算粒子权重的流程示意图。需注意的是，若有实质上相同的结果，本实施例并不以图2所示的流程顺序为限。该实施方式中，结合使用似然域模型和波束模型两种观测模型计算粒子权重，具体包括：

S131：对点云数据进行似然域采样，得到似然域采样点。

其中，似然域采样点的数量为M_likelihood，似然域采样点的观测量ⁱz_t包括

似然域采样点的分类ⁱc_t包括ⁱc_t＝mapped和ⁱc_t＝unmapped。

S132：利用似然域模型计算似然域采样点中地图中障碍物点云的测量概率值。

似然域模型采用三类测量误差，这三类测量误差包括测量噪声、无效测量和随机测量，且似然域模型会将最大距离读数丢弃。该实施方式中，似然域模型的条件概率分布函数为：

其中，Ω为地图观测变量，γ是指相邻两个采样点之间相互独立的最小距离，p_hit，p_max，p_rand分别表示测量噪声、无效测量、随机测量误差的概率分布，z_hit，z_max，z_rand为权重因子，z_hit+z_max+z_rand＝1。

该实施方式中，似然域模型假设各个距离测量(即激光点云)之间是条件独立的。因此需剔除可能使该假设不成立的点云数据。具体地，设置相邻两个采样点之间相互独立的最小距离γ(即第二阈值)，若相邻两个测量值之间的距离小于γ，则说明这两个测量值之间可能会相互影响，两者不独立。因此，当相邻两个测量值之间的距离小于γ时，定义该采样点的测量概率值为1(即p_likelihood(ⁱz_t|x_t,ⁱc_t＝mapped,Ω)＝1)。通过剔除可能使该假设不成立的点云数据，能够在减少计算量的同时保证概率估计不受相邻测量值噪声的影响。

该实施方式中，似然域采样点中地图中障碍物点云的测量概率值的计算包括：按照激光雷达测量点云索引从小到大的顺序读取似然域采样点中地图中障碍物点云的测量数据，如果测量值为0或为最大距离读数则直接舍弃，然后比较相邻测量值ⁱz_t和^i-1z_t间的距离大小，若小于最小距离阈值γ，则该采样点的测量概率值为1(即p_likelihood(ⁱz_t|x_t,ⁱc_t＝mapped,Ω)＝1)，否则按照式(1)计算似然域采样点中地图中障碍物点云的测量概率值。

具体地，初始化测量概率p_likelihood(ⁱz_t|x_t,ⁱc_t＝mapped,Ω)＝1，假设激光雷达相对于机器人车体中心的坐标为(x_s,y_s)，根据粒子当前位姿x_t＝(x′_ty′_tθ′_t)计算该似然域采样点云在地图坐标系中位置的

其中，θ′_i,s表示第i束激光相对于当前粒子的角度。

计算该似然域采样点到地图中最近占据栅格的距离

由式(1)可知，似然域采样点中地图中障碍物点云的测量概率值为：

z_hit·p_hit+z_max·P_max+z_rand·P_rand

其中，由测量过程引起的噪声使用高斯进行建模。在x-y空间，它涉及寻找地图上最近的障碍物。令dist表示测量坐标

与地图m上最近物体之间的欧氏距离。那么传感器测量的概率可以由一个以0为中心的高斯函数给出，该高斯具有传感器噪声：

无效测量误差采用点群分布建模。假设最大距离度数具有非常大的似然，使用以z_max为中心的点群分布来建立无效测量误差模型：

其中，I为一个指示函数，当其参数为真时取值为1，否则取值为0。

随机测量误差采用均匀分布建模。使用一个分布在完整传感器测量范围[0；Z_max]的均匀分布来建立模型：

结合式(1)-(7)，得似然域采样点中地图中障碍物点云的测量概率值为：

其中

表示均值为0(即以0为中心)，标准方差为σ_hit的高斯分布的距离概率。

S133：对点云数据进行波束采样，得到波束采样点。

其中，波束采样点的数量为M_beam，波束采样点的观测量ⁱz_t包括{¹z_t,²z_t,...,^mz_t}，波束采样点的分类ⁱc_t包括ⁱc_t＝mapped和ⁱc_t＝unmapped。波束采样点的数量应远小于似然域采样点的数量，波束采样点可以与似然域采样点重复，也可以不与似然域采样点重复。

S134：利用波束模型计算波束采样点中地图中障碍物点云的测量概率值。

波束模型采用四类测量误差，这四类测量误差包括测量噪声、意外对象引起的误差、无效测量和随机测量。该实施方式中，波束模型的条件概率分布函数为：

z_hit·p_hit+z_short·p_short+z_max·p_max+z_rand·p_rand

其中，Ω为地图观测变量，p_hit，p_short，p_max，p_rand分别表示测量噪声、意外对象引起的误差、无效测量、随机测量误差的概率分布，z_hit，z_short，z_max，z_rand为权重因子，z_hit+z_short+z_max+z_rand＝1。

其中，测量噪声通常由一个窄的均值为ⁱz_t ^*、标准偏差为σ_hit的高斯建模，测量噪声的概率分布为：

式中ⁱz_t ^*从x_t和Ω经过射线投射计算；

为具有均值ⁱz_t ^*、标准偏差σ_hit的单变量正态分布，η为归一化因子。

其中，意外对象可以作为传感器噪声来处理。无效测量误差和随机测量误差可以采用与上述似然域模型一样的方式建模。

因此，波束采样点中地图中障碍物点云的测量概率值的计算包括：通过光线投射法统计波束采样点中激光点云落在地图中障碍物上的数量总和M_mapped，则该波束采样点的测量概率值为：

其中α为权重值。

上述过程，图中所示步骤仅是示意，可以先利用似然域模型计算每个似然域采样点的测量概率值，也可以先利用波束模型计算每个波束采样点的测量概率值，也可以并行进行，在此不作限定。

S135：计算采样点中所有地图中障碍物点云的测量概率值。

将各似然域采样点中地图中障碍物点云的测量概率值与各波束采样点中地图中障碍物点云的测量概率值进行累乘，得到采样点中所有地图中障碍物点云的测量概率值：

S136：计算采样点中非地图中障碍物点云的测量概率值。

该实施方式中，非地图中障碍物点云的条件概率分布为：

p_unmapped(ⁱz_t|x_t,ⁱc_t＝unmapped,Ω)＝

η(max_mapped-p_mapped(ⁱz_t|x_t,ⁱc_t＝mapped,Ω))

其中，max_mapped为根据概率分布p_mapped(ⁱz_t|x_t,ⁱc_t＝mapped,Ω)求得的最大值，η为归一化因子。

S137：计算粒子的权重。

其中，测量时，许多传感器会产生不止一个测量值。例如，相机生成完整的数组(亮度、饱和度、颜色)；类似地，测距传感器一次测量通常会产生一系列测量值。用K表示在一个测量z_t内的测最值的数目，即有z_t＝{¹z_t,²z_t,...,^kz_t}，用ⁱz_t表示某一独立的测量(即一个测量值)，将每一个单一测量值的可能性相乘得到观测模型的测量概率值p(z_t|x_t,Ω)，其中：

其假设先验概率分布为常数，且p(c_t＝mapped)＝p(c_t＝unmapped)＝0.5。

观测模型的测量概率值p(z_t|x_t,Ω)即为粒子的权重：

请结合参阅图3和图4，图3是本申请实施方式中对点云数据进行网格划分的流程示意图，图4是本申请实施方式中对点云数据进行网格划分的示意图。该实施方式中，本申请所提供的定位方法，在对点云数据进行采样前还可以对点云进行网格划分。如图3和图4所示，该实施方式中，利用八叉树分割规则将三维点云数据集划分成若干个立体网格单元，每个立体网格单元格中包含一定数量的点云。

S210：构建母立方体。

具体地，获取点云数据在X、Y、Z三个方向上的最大值和最小值，分别为X_max、X_min、Y_max、Y_min、Z_max、Z_min，构建母立方体以将点云包围，设母立方体的棱长分别为l_X、l_Y、l_Z，其中：

随后，以该母立方体为根节点进行八叉树分割，一级划分后得到八个子立方体作为根节点的下一级别的八个完全一致的子节点，迭代进行，直到所有的叶节点立方体满足预先设定的划分终止条件。该实施方式中，将每个立体网格单元中的点云数量和点云的平整度作为立体网格单元是否需要继续划分的终止条件。

S220：判断立体网格单元中的点云数量是否小于第三阈值。

设定每个立体网格单元中点云数量的最小阈值ξ_min(即第三阈值)。统计获取各立体网格单元中的点云数量ξ，若立体网格单元中的点云数量ξ小于点云数量的最小阈值ξ_min，则不再需要对该立体网格单元进行进一步的网格划分。若立体网格单元中的点云数量ξ大于点云数量的最小阈值ξ_min，则执行步骤S230。

S230：判断立体网格单元中点云的平整度是否小于第四阈值。

定义点云的平整度为立体网格单元内点云到拟合平面的距离标准差，设定平整度的最小阈值d_threshold(即第四阈值)。

选择待划分的立体网格单元，使用RANSAC算法求得该立体网格内点云的拟合平面，然后计算立体网格单元内其他各点云到拟合平面的距离d_i，并计算距离标准差

其中

为平均距离，当d_σ>d_threshold时，说明立体网格单元内点云数据起伏变化较大，立体网格单元内含丰富的几何特征需要进一步细分。

重复迭代执行步骤S220、S230，直到所有立体网格单元中的点云数量小于ξ_min或者d_σ<d_threshold。

该实施方式中，经过多次动态体素划分后点云数据的平坦区域网格分辨率较大，而点云几何特征丰富的区域则被分辨率较小的网格包围。这样在网格内进行采样时能够保留点云的几何特征。对点云数据进行网格划分后，在对点云数据进行采样时，可在每个网格单元间进行等间隔采样。

请参阅图5，图5是本申请实施方式中另一机器人蒙特卡罗定位方法的流程示意图。需注意的是，若有实质上相同的结果，本实施例并不以图5所示的流程顺序为限。如图5所示，本实施方式包括：

S310：指定机器人初始化位置区域，采集粒子，形成粒子集。

其中，从移动机器人状态初始分布采集N个粒子，形成粒子集

并初始化粒子滤波器参数。

S320：获取机器人测量的点云数据和里程计数据。

该实施方式中，点云数据是利用三维激光雷达获取的激光点云数据。

S330：对点云数据进行网格划分，并对点云进行分类。

S340：基于里程计数据利用运动模型获取粒子集中每个粒子的当前位姿。

S350：基于点云数据结合使用似然域模型和波束模型计算粒子集中每个粒子的权重。

S360：利用粒子集中粒子的当前位姿和粒子的权重获取机器人的位姿。

S370：粒子重采样。

其中，从所有粒子中根据权重值判断是否在下一个循环继续使用这个粒子，对权重高的粒子根据高斯噪声模型重新复制撒播，保持粒子群数量基本不变。

其中，图中所示步骤仅是示意，对点云数据进行网格划分和对点云进行分类是对点云数据预处理的过程，可以在计算粒子位姿之前执行，也可以是在计算粒子位姿之后执行，还可以与计算粒子位姿并行执行，可以是先点云分类再网格划分，也可以是先网格划分再点云分类，还可以是网格划分和点云分类并行进行，在此不作限定。

以上实施方式中，通过采用空间八叉树动态网格调整方法对点云数据进行网格划分，使得在网格内进行采样时能够保留点云的几何特征；通过将点云数据按照是否是地图上的障碍物进行分类，计算各自的似然分布值，能够减少非地图上的障碍物点云给机器人位姿状态估计带来影响；结合使用似然域模型和波束模型两种观测模型计算粒子权重，能够在减少计算量的同时，还可以减少地图中观测数据的假阳性匹配，提高定位精度。

请参阅图6，图6是本申请实施方式中机器人蒙特卡罗定位装置的结构示意图。该实施方式中，机器人蒙特卡罗定位装置包括获取模块41、第一计算模块42、第二计算模块43和位姿估计模块44。

其中，获取模块41用于获取机器人测量的点云数据和里程计数据。

第一计算模块42用于基于里程计数据利用运动模型获取粒子集中每个粒子的当前位姿。

第二计算模块43用于基于点云数据结合使用两种观测模型计算粒子集中每个粒子的权重。

位姿估计模块44用于利用粒子的当前位姿和权重获取机器人的位姿。

该机器人蒙特卡罗定位装置对机器人进行定位时，结合使用了似然域模型和波束模型两种观测模型计算粒子权重，能够在减少计算量的同时，还可以减少地图中观测数据的假阳性匹配，提高定位精度。

请参阅图7，图7是本申请实施方式中机器人蒙特卡罗定位设备的结构示意图。该实施方式中，机器人蒙特卡罗定位设备10包括处理器11。

处理器11还可以称为CPU(Central Processing Unit，中央处理单元)。处理器11可能是一种集成电路芯片，具有信号的处理能力。处理器11还可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器11也可以是任何常规的处理器等。

机器人蒙特卡罗定位设备10可以进一步包括存储器(图中未示出)，用于存储处理器11运行所需的指令和数据。

处理器11用于执行指令以实现上述本申请机器人蒙特卡罗定位方法任一实施例及任意不冲突的组合所提供的方法。

请参阅图8，图8为本申请实施方式中计算机可读存储介质的结构示意图。本申请实施例的计算机可读存储介质20存储有指令/程序数据21，该指令/程序数据21被执行时实现本申请机器人蒙特卡罗定位方法任一实施例以及任意不冲突的组合所提供的方法。其中，该指令/程序数据21可以形成程序文件以软件产品的形式存储在上述存储介质20中，以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式方法的全部或部分步骤。而前述的存储介质20包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质，或者是计算机、服务器、手机、平板等终端设备。

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

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

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

Claims (12)

1.一种机器人蒙特卡罗定位方法，其特征在于，包括：

获取机器人测量的点云数据和里程计数据；

基于所述里程计数据利用运动模型获取粒子集中每个粒子的当前位姿；

基于所述点云数据至少结合使用两种观测模型计算每个所述粒子的权重；

利用所述粒子的当前位姿和权重获取所述机器人的位姿。

2.根据权利要求1所述的机器人蒙特卡罗定位方法，其特征在于，所述基于所述点云数据结合使用两种观测模型计算每个所述粒子的权重包括：

基于所述点云数据结合使用似然域模型和波束模型计算每个所述粒子的权重。

3.根据权利要求2所述的机器人蒙特卡罗定位方法，其特征在于，所述基于点云数据结合使用似然域模型和波束模型计算每个所述粒子的权重包括：

对所述点云数据进行采样，得到多个采样点，所述采样点包括似然域采样点和波束采样点；

利用所述似然域模型计算每个所述似然域采样点的测量概率值，利用所述波束模型计算每个所述波束采样点的测量概率值；

将所有所述似然域采样点和所述波束采样点的测量概率值进行累乘，得到所述多个采样点的测量概率值；

利用所述多个采样点的测量概率值计算所述粒子的权重。

4.根据权利要求3所述的机器人蒙特卡罗定位方法，其特征在于，所述对点云数据进行采样，得到多个采样点包括：

将所述多个采样点划分为地图中障碍物点云和非地图中障碍物点云；

其中，所述地图中障碍物点云在栅格地图中所在栅格距离其最近占据栅格的距离小于或等于第一阈值，所述非地图中障碍物点云在所述栅格地图中所在栅格距离其最近占据栅格的距离大于所述第一阈值，所述第一阈值为地图中障碍物点云在栅格地图中所在栅格距离其最近占据栅格的最大距离阈值。

5.根据权利要求4所述的机器人蒙特卡罗定位方法，其特征在于，所述利用似然域模型计算每个所述似然域采样点的测量概率值，利用所述波束模型计算每个所述波束采样点的测量概率值包括：

利用所述似然域模型计算所述似然域采样点中地图中障碍物点云的测量概率值，利用所述波束模型计算所述波束采样点中地图中障碍物点云的测量概率值；

将所有所述似然域采样点和所述波束采样点中地图中障碍物点云的测量概率值进行累乘，得到所有采样点中地图中障碍物点云的测量概率值；

利用所述所有采样点中地图中障碍物点云的测量概率值计算所述所有采样点中非地图中障碍物点云的测量概率值。

6.根据权利要求5所述的机器人蒙特卡罗定位方法，其特征在于，所述利用所有采样点中地图中障碍物点云的测量概率值计算所述所有采样点中非地图中障碍物点云的测量概率值包括：

所述非地图中障碍物点云的概率分布函数为：

p_unmapped(ⁱz_t|x_t,ⁱc_t＝unmapped,Ω)＝η(max_mapped-p_mapped(ⁱz_t|x_t,ⁱc_t＝mapped,Ω))

其中，max_mapped为p_mapped(ⁱz_t|x_t,ⁱc_t＝mapped,Ω)的最大值，η为归一化因子。

7.根据权利要求3所述的机器人蒙特卡罗定位方法，其特征在于，所述利用似然域模型计算每个所述似然域采样点的测量概率值包括：

响应于某一似然域采样点与相邻的似然域采样点之间的距离小于第二阈值，定义该似然域采样点的测量概率为1；

响应于某一似然域采样点与相邻的似然域采样点之间的距离大于或等于第二阈值，利用似然域概率分布函数计算该似然域采样点的测量概率值，所述第二阈值为相邻两个采样点之间相互独立的最小距离阈值。

8.根据权利要求3所述的机器人蒙特卡罗定位方法，其特征在于，所述对点云数据进行采样，得到多个采样点包括：

所述采样点包括第一数量的所述似然域采样点和第二数量的所述波束采样点，所述第一数量大于所述第二数量。

9.根据权利要求3所述的机器人蒙特卡罗定位方法，其特征在于，所述对点云数据进行采样，得到多个采样点包括：

利用八叉树分割规则将所述点云数据划分成若干个立体网格单元，每个所述立体网格单元中包含一定数量的点云，每个所述立体网格单元的体积相同/不同；

从每个所述立体网格单元中选取所述采样点。

10.根据权利要求9所述的机器人蒙特卡罗定位方法，其特征在于，所述利用八叉树分割规则将所述点云数据划分成若干个立体网格单元包括：

每个所述立体网格单元中点云的数量小于第三阈值，或每个所述立体网格单元中点云的平整度小于第四阈值，所述平整度为所述立体网格单元中点云到拟合平面的距离标准差。

11.一种机器人蒙特卡罗定位设备，其特征在于，包括处理器，所述处理器用于执行指令以实现如权利要求1-10中任一项所述的机器人蒙特卡罗定位方法。

12.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质用于存储指令/程序数据，所述指令/程序数据能够被执行以实现如权利要求1-10中任一项所述的机器人蒙特卡罗定位方法。

Images