CN109579849B

CN109579849B - 机器人定位方法、装置和机器人及计算机存储介质

Info

Publication number: CN109579849B
Application number: CN201910032487.4A
Authority: CN
Inventors: 胡立志; 林辉; 卢维; 殷俊; 穆方波
Original assignee: Zhejiang Dahua Technology Co Ltd
Current assignee: Zhejiang Huaray Technology Co Ltd
Priority date: 2019-01-14
Filing date: 2019-01-14
Publication date: 2020-09-29
Anticipated expiration: 2039-01-14
Also published as: CN109579849A

Abstract

本发明公开了一种机器人定位方法、装置和机器人及计算机存储介质，属于机器人技术领域，用于提升机器人定位的准确性。该方法包括：基于机器人的历史实际位姿和预设时间段内的位姿变化量，获取当前粒子群，其中，粒子群中的每一个粒子表示所述机器人的一个位姿，粒子群用于表示所述机器人位姿的概率分布；为所述当前粒子群中的每一个粒子分配权重，并根据分配的权重获得所述机器人的第一估计位姿；其中，所述权重用于表征每一个粒子为当前实际位姿的概率；基于所述第一估计位姿与所述机器人上设置的传感器采集的扫描数据，采用设定的扫描匹配模型进行扫描匹配，得到第二估计位姿；对所述第二估计位姿进行校正，得到所述机器人的当前实际位姿。

Description

机器人定位方法、装置和机器人及计算机存储介质

技术领域

本发明涉及机器人技术领域，特别涉及一种机器人定位方法、装置和机器人及计算机存储介质。

背景技术

机器人是一种能够自动执行任务的自动化装置，机器人可划分为室内机器人和室外机器人，室内机器人一般可应用于货物的搬运，例如在仓储物流等领域中，室内机器人可以应用在仓库中进行货物搬运，更加节省人力，效率更高。

机器人在执行任务时，需要知道自己在当前环境中的位置，从而才能够准确地执行任务。例如对于室内机器人，其需要实时的知道自己位于室内的位置，才能对自己的移动路线进行规划以及监控，从而准确地完成搬运任务，可见，机器人的定位是否准确并且实时，直接影响到任务执行的效果，因此，机器人如何准确并且实时的进行定位是目前亟待解决的问题。

发明内容

本发明实施例提供一种机器人定位方法、装置和机器人及计算机存储介质，用于提升机器人定位的准确性。

第一方面，提供一种机器人定位方法，所述方法包括：

基于机器人的历史实际位姿和预设时间段内的位姿变化量，获取当前粒子群，其中，粒子群中的每一个粒子表示所述机器人的一个位姿，粒子群用于表示所述机器人位姿的概率分布；

为所述当前粒子群中的每一个粒子分配权重，并根据分配的权重获得所述机器人的第一估计位姿；其中，所述权重用于表征每一个粒子为当前实际位姿的概率；

基于所述第一估计位姿与所述机器人上设置的传感器采集的扫描数据，采用设定的扫描匹配模型进行扫描匹配，得到第二估计位姿；

对所述第二估计位姿进行校正，得到所述机器人的当前实际位姿。

可选的，所述基于机器人的历史实际位姿和预设时间段内的位姿变化量，获取当前粒子群，包括：

以所述历史实际位姿为中心，通过高斯分布随机产生多个粒子组成第一粒子群；

确定所述位姿变化量是否大于预设变化量阈值；

若确定结果为是，则基于所述位姿变化量对所述第一粒子群中的每一个粒子进行更新得到第二粒子群，并将所述第二粒子群确定为所述当前粒子群；否则将所述第一粒子群确定为所述当前粒子群。

可选的，为所述当前粒子群中的每一个粒子分配权重，并根据分配的权重获得所述机器人的第一估计位姿，包括：

针对所述当前粒子群中的每一个粒子，将所述扫描数据基于每一个粒子表示的位姿映射到基于预设的栅格概率地图构建的似然域模型中，并根据扫描结果在所述似然域模型中的概率值为每一个粒子分配权重；

基于所述当前粒子群以及每一个粒子的权重值获得加权平均位姿，并将所述加权平均位姿确定为所述第一估计位姿。

可选的，所述基于所述第一估计位姿，采用设定的扫描匹配模型进行扫描匹配，得到第二估计位姿，包括：

根据所述第一估计位姿和预设的栅格概率地图生成虚拟点云，并基于所述扫描数据生成实际点云；

将所述虚拟点云与所述实际点云进行扫描匹配，得到所述第二估计位姿。

可选的，所述将所述虚拟点云与所述实际点云进行扫描匹配，得到所述第二估计位姿，包括：

初始化旋转平移变换值，并基于所述旋转平移变换值对所述实际点云中的每一个原始点进行旋转平移变换，得到每一个原始点对应的转换点；

针对每一个转换点，从所述虚拟点云中找到与该转换点之间的距离最近的第一点和第二点，记录该转换点对应的原始点与该转换点对应的第一点和第二点构成的参考线段之间的对应关系；

对所述旋转平移变换值进行迭代优化，以使得最终获取的旋转平移变换值能够使得所有转换点与对应的参考线段之间的距离之和最小，首次迭代将初始化的旋转平移变换值作为初始值；其中，每一次迭代包括如下步骤：

通过本次迭代的初始值对实际点云中的每一个原始点进行旋转平移变换，并确定所有转换点与对应的参考线段之间的距离之和是否收敛于一固定值；

若确定结果为是，则停止迭代，否则更新旋转平移变换值，并将更新后的旋转平移变换值作为下一次迭代的初始值；

根据最后一次更新的旋转平移变换值对所述第一估计位姿进行更新，得到所述第二估计位姿。

可选的，所述对所述第二估计位姿进行校正，得到所述机器人的当前实际位姿，包括：

根据所述第二估计位姿和预设的栅格概率地图生成虚拟点云；

对所述第二估计位姿进行多次校正，直至最后一次校正后得到的位姿的偏移量小于预设偏移量阈值，首次校正将所述第二估计位姿作为初始值；

其中，每一次校正包括如下步骤：

根据所述机器人上设置的传感器获取的实际点云中每一点的实际距离，以及根据上一次校正后得到的估计位姿生成的虚拟点云中每一点的估计距离，确定上一次校正后得到的估计位姿的偏移量；

通过所述偏移量对上一次校正后得到的估计位姿进行校正，并确定所述偏移量是否小于预设偏移量阈值；

若确定结果为是，则停止校正，并将本次校正后得到的估计位姿确定为所述机器人当前实际位姿，否则将本次校正后得到的估计位姿作为下一次校正的初始值继续进行校正。

可选的，在为所述当前粒子群中的每一个粒子分配权重之后，所述方法还包括：

确定与上一次粒子重采样之间的权重分配次数是否大于预设次数阈值；

若确定结果为是，则基于所述当前粒子群进行粒子重采样，其中，重采样得到的粒子与所述当前粒子群中的粒子的权重相关。

第二方面，提供一种机器人定位装置，包括：

粒子分布单元，用于基于机器人的历史实际位姿和预设时间段内的位姿变化量，获取当前粒子群，其中，粒子群中的每一个粒子表示所述机器人的一个位姿，粒子群用于表示所述机器人位姿的概率分布；

位姿估计单元，用于为所述当前粒子群中的每一个粒子分配权重，并根据分配的权重获得所述机器人的第一估计位姿；

扫描匹配单元，用于基于所述第一估计位姿与所述机器人上设置的传感器采集的扫描数据，采用设定的扫描匹配模型进行扫描匹配，得到第二估计位姿；

位姿校正单元，用于对所述第二估计位姿进行校正，得到所述机器人的当前实际位姿。

可选的，所述粒子分布单元，具体用于：

确定所述位姿变化量是否大于预设变化量阈值；

可选的，所述位姿估计单元，具体用于：

可选的，所述扫描匹配单元，具体用于：

可选的，所述位姿校正单元，具体用于：

其中，每一次校正包括如下步骤：

可选的，所述装置还包括重采样单元，用于：

第三方面，提供一种机器人，包括：

至少一个处理器；以及

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如第一方面所述的方法。

第四方面，提供一种计算机存储介质，

所述计算机存储介质存储有计算机指令，当所述计算机指令在计算机上运行时，使得计算机执行如第一方面所述的方法。

本发明实施例中，通过机器人的历史实际位姿以及位姿变化量构建当前粒子群，进而获取机器人的估计位姿，并通过扫描匹配以及再次校正对初次获取的估计位姿进行多次校正，并将最后校正后的位姿作为当前实际位姿。这样，通过对初始估计位姿的多次校正，能够使得最终获取的当前实际位姿更加准确，提高机器人定位的准确性，从而辅助机器人准确的完成各项搬运任务。

附图说明

图1为本发明实施例提供的机器人的结构示意图；

图2为本发明实施例提供的基于仓库环境生成的二维地图的示意图；

图3为本发明实施例提供的机器人定位的流程示意图；

图4为本发明实施例提供的获取当前粒子群的流程示意图；

图5为本发明实施例提供的虚拟点云的示意图；

图6为本发明实施例提供的扫描匹配的流程示意图；

图7为本发明实施例提供的对第二估计位姿进行校正的流程示意图；

图8为本发明实施例提供的机器人定位装置的一种结构示意图；

图9为本发明实施例提供的机器人的一种结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

下面介绍本发明实施例的技术背景。

目前，机器人是一种能够自动执行任务的自动化装置，机器人的应用十分广泛。机器人在执行任务时，需要知道自己在当前环境中的位置，从而才能够准确地执行任务。例如对于室内机器人，其需要实时的知道自己位于室内的位置，才能对自己的移动路线进行规划以及监控，从而准确地完成搬运任务，可见，机器人的定位是否准确并且实时，直接影响到任务执行的效果，因此，机器人如何准确并且实时的进行定位是亟待解决的问题。

鉴于此，本发明实施例提供一种机器人定位方法，在该方法中，通过机器人的历史实际位姿以及位姿变化量构建当前粒子群，进而获取机器人的估计位姿，并通过扫描匹配以及再次校正对初次获取的估计位姿进行多次校正，并将最后校正后的位姿作为当前实际位姿。这样，通过对初始估计位姿的多次校正，能够使得最终获取的当前实际位姿更加准确，提高机器人定位的准确性，从而辅助机器人准确的完成各项搬运任务。

本发明实施例中，通过采用适应性蒙特卡洛(adaptive Monte Carlolocalization，AMCL)算法来获取估计位姿，具体而言，机器人通过更新获取当前时刻的粒子群后，可以通过将激光雷达传感器的扫描结果基于每一个粒子表示的位姿映射到预设的栅格概率地图中，来构建似然域模型，并根据扫描结果在所述似然域模型中的概率值为每一个粒子分配权重，从而基于粒子群和权重获取到当前时刻的加权平均位姿，最终对加权平均位姿进行校正后获取当前时刻的实际位姿。其中，通过基于栅格概率地图构建的似然域模型的方式为粒子分配权重，算法所需的计算量较小，算法的收敛速度更快，从而使得获取当前时刻位姿的实时性更佳。此外，在似然域模型中，概率值的大小与栅格概率地图中障碍物的距离有关，也就是说，与栅格概率地图中障碍物的距离越小的，概率值越大，从而为该粒子分配的权重则可以更大，从而获取的加权平均位姿能够与实际位姿更为接近，进而最终获取的实际位姿更加准确。

本发明实施例中，还结合扫描匹配算法对获取的估计位姿进行纠正。其中，本发明实施例中通过基于点到线的迭代最近点(Point-to-Line Iterative Closest Point，PLICP)算法进行扫描匹配，能够在有限的迭代次数内快速收敛到一个固定值，提高收敛速度，从而进一步保障位姿计算的实时性。

此外，本发明实施例中最终还对扫描匹配后的位姿的偏移量进行进一步的矫正，直至最终获得的位姿的偏移量足够小，从而提升定位的精确度。

下面结合附图介绍本发明实施例提供的技术方案。

请参见图1，为本发明实施例的机器人的结构示意图，该机器人包括移动底盘10和激光雷达传感器20。其中，移动底盘10中可以包括运动控制器、电机、电池、嵌入式计算机以及里程计等部件，当然，还可以包括其他可能的部件，本发明实施例对此不做限制。

里程计能够估算出机器人运动的变化量，从而能够基于上一时刻的位姿集合里程计估算的变化量来估计出当前时刻机器人的位姿，但是由于里程计存在一定的误差，因此基于里程计估算的位姿的精度尚不满足要求。其中，里程计例如可以是轮式编码器。

激光雷达传感器20为一种能够获取二维平面信息的传感器，可以应用于获取机器人所处环境的二维平面轮廓信息。激光雷达传感器20例如可以是二维(2D)激光雷达传感器。

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

请参见图3，本发明实施例提供一种机器人定位方法，该方法的流程描述如下。

步骤301：基于机器人的初始位姿生成初始粒子群。

本发明实施例中，在机器人移动的初始时刻时，基于机器人的初始位姿初始化粒子滤波器，即以机器人的初始位姿为中心进行粒子群的分布得到初始粒子群中，在初始粒子群中每个粒子的权重也是相同的。

具体的，可以以机器人的初始位姿为中心，通过高斯分布随机生成多个粒子组成初始粒子群。当然，本发明实施例中还可以采用其他的分布方式来生成初始粒子群，本发明实施例对此不做限制。

其中，每一个粒子表示机器人的一个位姿，位姿表示机器人的位置和朝向，即二维坐标和角度，粒子群用于表示所述机器人位姿的概率分布。

步骤302：基于第一粒子群，获取当前粒子群。

本发明实施例中，第一粒子群是基于机器人的历史实际位姿所生成的粒子群，历史实际位姿则可以采用上一时刻的历史实际位姿，也就是上一次定位最终得到的实际位姿。在上一时刻为初始时刻时，则历史实际位姿则为机器人的初始位姿，相应的，第一粒子群则为初始粒子群。生成第一粒子群的过程与步骤301中生成初始粒子群的方式相同，因此在此不再过多赘述。

本发明实施例中，当机器人开始移动之后，机器人的位置和朝向都可能会发生变化，可以通过机器人中的传感器获取机器人的位姿变化量，进而根据第一粒子群和预设时间段内的位姿变化量，获取当前粒子群。其中，传感器例如可以为里程计，当然，还可以为其他可能的传感器，本发明实施例对此不做限制。

具体的请参见图4，获取当前粒子群的流程如下：

步骤3021：确定位姿变化量是否大于预设变化量阈值。

本发明实施例中，若是机器人的位姿变化量很小时，实际上基于历史实际位姿生成的第一粒子群和当前粒子群的差别很小，甚至可以忽略，因此，在获取当前粒子群之前，可以先判断位姿变化量是否大于预设变化量阈值，若是确定位姿变化量大于预设变化量阈值，则需要对第一粒子群进行更新；若是位姿变化量不大于预设变化量阈值，则可以将第一粒子群直接作为当前粒子群，不必进行更新，这样，在位姿变化量很小时，则无需通过计算来获取当前的粒子群，从而减少计算量，从而提升机器人定位的速度。

当然，在实际应用中也可以无论位姿变化量的大小，在获取当前粒子群时都基于位姿变化量来进行更新，也就是说，步骤3021并非是必选的步骤，在实际应用中可以根据需求进行选择，本发明实施例对此不做限制。

步骤3022：若步骤3021的确定结果为是，则基于位姿变化量更新第一粒子群，获得当前粒子群。

本发明实施例中，若是确定位姿变化量大于预设变化量阈值，则可以根据位姿变化量来更新第一粒子群，得到第二粒子群，并将第二粒子群作为当前粒子群。

以传感器为里程计为例，因此可以根据机器人的里程计运动模型，即根据里程计的读数和初始粒子群得到当前粒子群。具体的，针对第一粒子群中的每一个粒子x_t-1，在机器人的位姿变化量为u_t时，则可以得到该粒子所对应的当前时刻的位姿x_t，当前时刻位姿服从p(x_t|u_t,x_t-1)。

其中，x_t-1为机器人上一时刻在全局世界坐标系下的位姿，全局世界坐标系为固定坐标系，例如机器人所在环境为仓库时，则全局世界坐标系则为基于仓库地图建立的固定坐标系，x_t-1＝[x yθ]^T，x、y为2D位置，即在全局世界坐标系中横纵坐标的取值，θ为2D方向；u_t为位姿变化量，可以由里程计等传感器测量得到，

分别表示在里程计坐标系下上一时刻和当前时刻机器人的位姿，里程计坐标系为以里程计为中心建立的坐标系，由于里程计随机器人运行，里程计坐标系并非固定坐标系。

本发明实施例中，可以将每一次获取的粒子群中的粒子通过KD(k-dimension)树的形式进行存储，以便于后续粒子的查找。

步骤3023：若步骤302的确定结果为否，则将第一粒子群直接作为当前粒子群。

本发明实施例中，当步骤302的确定结果为否，即确定位姿变化量不大于预设变化量阈值，则可以将第一粒子群直接作为当前粒子群。

下面请继续参见图3。

步骤303：为当前粒子群中的每一个粒子分配权重值。

本发明实施例中，可以通过激光雷达传感器扫描到的平面数据，利用观测模型p(z_t|x_t ^m)对当前粒子群中的粒子进行权重分配。其中，观测模型可以反应粒子群中粒子的状态与激光雷达传感器采集到的激光雷达数据的匹配程度，进而根据匹配程度为每一个粒子分配权重。其中，观测模型可以采用似然域模型，对于似然域模型而言，只需要考虑激光雷达传感器发出的光束的终点，即光束和障碍物的交点。具体的，针对粒子群中的每一个粒子，将激光雷达传感器的扫描结果基于每一个粒子表示的位姿映射到基于预设的栅格概率地图构建的似然域模型中，并根据扫描结果在似然域模型中的概率值为每一个粒子分配权重。栅格概率地图为预先获取的地图数据，在栅格概率地图中，地图被划分成一系列的栅格，每一个栅格被赋予一个可能值，表示该栅格上存在障碍物的概率。

其中，p(z_t|x_t ^m)中x_t ^m表示在t时刻基于运动模型采样的机器人位姿，也就是粒子群中的粒子，例如当当前时刻为t时刻时，x_t ^m就是当前粒子群中的第m个粒子，m＝1,2,3……M，M表示粒子群中的粒子数量；z_t表示基于采样的位姿激光雷达传感器的观测值，观测值的大小跟激光雷达传感器的扫描结果基于每一个粒子表示的位姿映射到似然域模型中后，每一点与该点距离最近的障碍物之间的距离有关。具体的，每一个点距离最近的障碍物越近，则该点的概率值越大，观测值越大，则相应的粒子的权重越大，反之，若每一个点距离最近的障碍物越远，则该点的概率值越小，观测值越小，则相应的粒子的权重越小。

步骤304：确定是否需要重采样。

本发明实施例中，由于在权重分配之后，每个粒子的权重就变得不一样了，有些粒子的权重可能会变得很小，这样的粒子是可以忽略的，因此经过几次粒子滤波的迭代以后，粒子权值的方差不断增大，状态空间中的有效粒子数较少，使得粒子滤波器的估计性能下降，因此，为了保证粒子滤波器的估计性能，需要保证粒子滤波器中的粒子数量保证在一定的数量上，所以需要进行重采样(Multinomial resampling)，例如可以每隔几次机器人的定位流程之后，就进行一次重采样，或者说，每间隔几次权重分配后，就进行一次重采样。

具体的，在每一次权重分配之后，可以确定与上一次粒子重采样之间的权重分配次数是否大于预设次数阈值，若确定结果为是，则确定需要进行重采样，那么可以基于当前粒子群进行粒子重采样，否则确定不进行重采样。

步骤305：若步骤304的确定结果为是，则基于当前粒子群进行粒子重采样。

本发明实施例中，若确定需要进行重采样，则可以基于当前粒子群进行粒子重采样。其中，重采样是指从当前粒子群中随机复制粒子来添加至粒子群中，以扩充粒子群中粒子的数量，复制得到的粒子与当前粒子群中的粒子的权重相关，权重值越大的粒子被复制的概率更高，权重值越小的粒子被复制的概率更低。通过重采样能够使得粒子滤波器中的粒子数量始终保证在一定的数量上。当然，在进行重采样之后，还需要对粒子群中粒子的权重重新进行分配，再基于重采样并权重分配后的粒子群继续进行步骤306。

若步骤304的确定结果为否，则继续进行步骤306。

步骤306：根据为各粒子分配的权重获得机器人的第一估计位姿。

本发明实施例中，在进行权重分配之后，则可以根据粒子群中的粒子以及每一个粒子的权重值计算得到加权平均位姿，并将其作为当前时刻的第一估计位姿，当然，也可以通过其他的方式获取第一估计位姿。

步骤307：基于第一估计位姿与机器人上设置的传感器采集的扫描数据，采用设定的扫描匹配模型进行扫描匹配，得到第二估计位姿。

本发明实施例中，可以根据第一估计位姿和栅格概率地图生成虚拟点云，并基于激光雷达传感器实际测得的扫描数据生成实际点云，进而将虚拟点云与实际点云进行扫描匹配，从而对第一估计位姿进行矫正得到第二估计位姿。

具体的，根据第一估计位姿所表示的位置结合到栅格概率地图可以生成虚拟点云，虚拟点云中的每一点为假设机器人在第一估计位姿所表示的位置，机器人上的激光雷达传感器发出的光束与障碍物的交点，而实际点云则是根据激光雷达传感器实际测得的扫描数据生成的。如图5所示，为虚拟点云的示意图，其中，深色栅格和黑色栅格表示障碍物，黑色栅格表示虚拟点云中的一点。

其中，激光雷达传感器获取的实际点云中的每一个点可以表示为：

其中，x_mq、y_mq是激光雷达坐标系下第q个点的坐标，激光雷达坐标系即是以激光雷达传感器为中心建立的坐标系，激光雷达传感器随机器人移动，因此激光雷达坐标系属于非固定坐标系，d_mq是激光雷达传感器获取的第q束光束获取的数据，即第q束光束的起点到该光束与障碍物的交点之间的距离，α_min是激光雷达的第一个光束的角度，Δ表示激光雷达传感器的角分辨率。

虚拟点云中的每一个点可以表示为：

其中，x_oq、y_oq是在地图数据(全局世界坐标系)中的障碍物的坐标值，也就是第q个点的坐标值，d_vq是最短的虚拟点云测量值，即假设机器人位于第一估计位姿所表示的位置，激光雷达传感器发出的第q束光束的起点到该光束与障碍物的交点之间的距离，x_e、y_e、θ_e是为第一估计位姿的坐标表示和角度表示。

基于上述虚拟点云，能够得到在激光雷达坐标系下的虚拟点云中的每一个点可以表示为：

其中，x_vq、y_vq是激光雷达坐标系下第q个点的坐标，这里的激光雷达坐标系为假设机器人在第一估计位姿所表示的位置时，以该机器人上设置的激光雷达传感器为中心建立的坐标系。

本发明实施例中，依据激光雷达传感器与在机器人之间的相对位置关系，可以将实际点云和虚拟点云都转换到机器人坐标系下，进行PLICP扫描匹配，得到机器人当前时刻的第一估计位姿和实际位姿的相对校正关系。

理论上来讲，在不考虑里程计的误差的情况下，在同一坐标系下，实际点云和虚拟点云应是重合的，但是由于各种误差因素存在的情况下，实际点云和虚拟点云则存在一定的差异，因此我们的目标即是要找到这种差异，从而基于差异对上述得到的第一估计位姿进行矫正。具体的，假设实际点云与虚拟点云之间的变换关系为q_k＝[t_kθ_k]^T，则实际点云中的任意点p_q通过旋转平移变换后可得到

可表示为：

其中，q_k表示第k次迭代所得的实际点云与虚拟点云之间的变换关系，p_q表示实际点云中的第q个点，

表示实际点云经过旋转平移变换后的表示，

表示旋转平移变换，t_k表示平移变换，θ_k表示旋转变换。

具体的，可以通过PLICP扫描匹配算法获取上述的变换关系，请参见图6，为PLICP扫描匹配的流程示意图。

步骤601：初始化旋转平移变换值。

步骤602：基于旋转平移变换值对实际点云中的每一个原始点p_q进行旋转平移变换，得到每一个原始点对应的转换点

步骤603：针对每一个转换点

从虚拟点云中找到与该转换点

之间的距离最近的第一点

和第二点

记录该转换点

对应的原始点p_q与该转换点

对应的第一点

和第二点

构成的参考线段之间的对应关系C_k：

其中，

和

分别为第一点和第二点的索引，C_k表示为第k次迭代时，实际点云第q个点云与虚拟点云中

组成的线段的对应关系。

步骤604：根据转换点

与与该转换点

对应的第一点

和第二点

构成的参考线段之间的距离，剔除异常的对应关系。

本发明实施例中，由于在实际应用中一些转换点可能并不会存在与其对应的点，这样找到的对应关系则是异常的，异常关系可能会对变换关系的结果产生不良影响，因此，可以对点对转换点

与与该转换点

对应的第一点

和第二点

构成的参考线段之间的距离进行排序，从而剔除异常的对应关系。例如可以将距离大于一定值的对应关系剔除，或者可以按照预设比例关系抽取部分对应关系，而剩余的则剔除，例如可以抽取按照转换点

与与该转换点

对应的第一点

和第二点

构成的参考线段之间的距离从小到大进行排序，排在前70％的留下，作为后续计算数据，而其余的则剔除。

本发明实施例中，基于上述初始化的旋转平移变换值进行迭代优化，以使得最终获取的旋转平移变换值能够使得所有转换点与对应的参考线段之间的距离之和最小，首次迭代将初始化的旋转平移变换值作为初始值，迭代过程包括如下步骤：

步骤605：通过本次迭代的初始值对实际点云中的每一个原始点进行旋转平移变换，并确定所有转换点

与对应的参考线段之间的距离之和是否收敛于一固定值。

本发明实施例中，当转换点与对应的参考线段之间重合或者接近重合时，则可以认为获得该转换点的转换关系已经足够准确，这样，问题可转换为一个最小化的问题，目标函数如下：

其中，

为

组成的参考线段的法向量，θ_k+1、t_k+1是分别是第k+1次迭代时求得的角度变换值和位移变化值，

是虚拟点云中与

距离最近的点，J(q_k+1,C_k)用于表示在第k+1次迭代时所采用的对应关系为C_k。

本发明实施例中，最终问题可以转换为求取能够上述目标函数足够小的θ_k+1、t_k+1，上述目标函数实质上表示所有转换点

与对应的参考线段之间的距离之和，因此可以在每一次迭代时，可以计算所有转换点

与对应的参考线段之间的距离之和，并确定距离之和是否收敛于一固定值，若确定结果为是，则结束迭代，否则继续迭代。

步骤606：若步骤605的确定结果为否，则基于预设算法获取旋转平移变换值的梯度值，并基于所述梯度值更新旋转平移变换值，并将更新后的旋转平移变换值作为下一次迭代的初始值，继续进入步骤605。

本发明实施例中，可以通过预设算法对上述目标函数进行求解，以获取第k次迭代的旋转平移变换值与第k+1次迭代的旋转平移变换值之间的梯度值，这样，基于第k次迭代的旋转平移变换值和该梯度值，则可以获得第k+1次迭代的旋转平移变换值，并将获取的第k+1次迭代的旋转平移变换值作为下一次迭代的初始值，即k+1次迭代的初始值，继续进行步骤605。

其中，预设算法例如可以是高斯牛顿算法或者梯度下降算法等算法，当然，还可以是其他可能的算法，本发明实施例对此不做限制。

步骤607：若步骤605的确定结果为是，则流程结束。

本发明实施例中，在上述目标函数的结果收敛到一固定值之后，则停止迭代，并通过最后一次更新的旋转平移变换值对第一估计位姿进行矫正，以得到第二估计位姿。在实际应用中，矫正后得到的第二估计位姿的精度已经较高，也可作为机器人当前实际位姿。

下面请继续参见图3。

步骤308：对第二估计位姿的偏移量进行校正，得到机器人的当前实际位姿。

本发明实施例中，通过AMCL算法和PLICP扫描匹配之后，能够对机器人的朝向有较为准确的估计，但是机器人的位置估计有时不稳定，为了实现位置的稳定估计，本发明实施例中还通过基于快速傅里叶变换(Fast Fourier Transformation，FFT)算法来实现稳定的位置估计。

具体的，基于上述获得的第二估计位姿，再次结合栅格概率地图生成虚拟点云，则第二估计位姿和实际位姿之间的误差，可以由激光雷达传感器测得与障碍物之间的实际距离，和再次生成的虚拟点云中第二估计位姿表示的位置与障碍物之间的估计距离的离散傅里叶变换的首项表示为：

其中，d_mq为激光雷达传感器测得的实际距离，即激光雷达所采集的与障碍物的交点之间的距离，d_vq是再次生成的虚拟点云对应的估计距离，即假设机器人位于第二估计位姿所在位置，激光雷达所采集的与障碍物的交点之间的距离，Q为激光雷达测量数据中的有效点数，即上述剔除异常对应关系之后剩余的点的数量。将上述傅里叶首项展开可得下式：

在实际应用中，估计的机器人位姿(第二估计位姿)和真实的机器人位姿实质上距离很近，那么下列公式成立：

联合上述两式可得如下公式：

X₀≈-Q·x_err-Q·y_err

其中，x_err、y_err表示机器人真实位姿和第二估计位姿的位移偏差。

基于上述公式，对第二估计位姿进行多次校正，直至最后一次校正后得到的位姿的偏移量小于预设偏移量阈值，首次校正将第二估计位姿作为初始值，其中，请参见图7，每一次校正包括如下步骤：

步骤701：根据实际点云中每一点的实际距离，以及根据上一次校正后得到的估计位姿生成的虚拟点云中每一点的估计距离，确定上一次校正后得到的估计位姿的偏移量。

步骤702：通过偏移量对上一次校正后得到的估计位姿进行校正，并确定偏移量是否小于预设偏移量阈值。

步骤703：若确定结果为是，则将本次校正后得到的估计位姿作为下一次校正的初始值继续进行校正。

本发明实施例中，若偏移量不小于预设偏移量阈值，则表明估计的位姿尚不足够准确，则继续执行步骤701，即通过上一次校正后的估计位姿和栅格概率地图生成虚拟点云，再根据实际点云中每一点的实际距离，以及虚拟点云中每一点的估计距离，确定上一次校正后得到的估计位姿的偏移量，并对上一次校正后得到的估计位姿进行校正。这样，经过多次迭代之后，当校正的偏移量足够小时，也就是当根据最后一次校正后得到的位姿确定的偏移量小于预设偏移量阈值时，则认为最后一次校正后得到的位姿足够准确，则结束迭代，并将最后一次校正后得到的位姿确定为机器人的当前实际位姿。

步骤704：若确定结果为是，则停止校正，并将本次校正后得到的估计位姿确定为机器人的当前实际位姿。

下面请继续参见图3。

步骤309：基于当前实际位姿进行粒子群的分布，得到第一粒子群。

本发明实施例中，在获取当前实际位姿之后，可以基于当前实际位姿来进行粒子群的分布，得到用于下一时刻的位姿估计的第一粒子群，以为下一时刻的机器人的位姿估计提供正确的粒子群。具体的，粒子群的分布是基于粒子群的均值和协方差来生成的，使用该粒子群的均值和协方差当作高斯模型的均值和协方差来生成一定数量的粒子群，协方差表示位姿的不确定度，即粒子的集中程度，协方差越小，不确定度越低，粒子群的分布就越集中。那么基于当前实际位姿进行粒子群的分布时，则将当前实际位姿作为高斯模型的均值，以及继承上一次权重分配后的粒子群的不确定度，或者重采样后的粒子群的不确定度，利用高斯模型来进行粒子群的分布，得到第一粒子群。

在第一粒子群中，每个粒子的权重是相同的。第一粒子群所包括的粒子的数量与在获取上一时刻的位姿过程中权重分配之后的粒子数量或者重采样之后的粒子数量相同。

本发明实施例中，若是机器人还在持续移动中，在得到第一粒子群之后，则可以继续下一时刻的机器人的位姿估计，即继续进行步骤302(如图3中的虚线所示)。

本发明实施例中，当机器人停止移动或者停止工作时，则定位结束。

综上所述，本发明实施例中，在基于里程计模型进行粒子分布的基础上，采用观测模型中的似然域模型进行粒子的权重的分配，并将加权平均位姿来作为机器人的估计位姿。其中，通过似然域模型，即使用激光雷达的观测值与地图上最近障碍物的距离服从高斯分布来构建似然域，从而来对粒子进行权重分配，提高了权重分配的速度。

此外，本发明实施例中，在基于AMCL算法估计的位姿基础上，使用PLICP扫描匹配算法对估计位姿进行校正。PLICP算法最终可以收敛到一个固定值，或者进入到一个循环，所以经过PLICP扫描匹配过后，可以得到稳定的位姿纠正值，能够在有限的迭代次数内快速收敛到一个固定值，提高收敛速度，并且，即使给定的初始的旋转平移变换值不同，得到的最后纠正值都是相同的，稳定性更高。

最后，本发明实施例中，还使用FFT算法对经过扫描匹配后的位姿做进一步的位置纠正。在进行AMCL和PLICP进行位姿估计和方位纠正后，机器人的方向往往能够得到很好的估计，但位置有时还会出现不稳定的偏差，所以使用FFT算法能够对机器人的位置做进一步的纠正，从而得到更高精度的机器人定位。

请参见图8，基于同一发明构思，本发明实施例提供一种机器人定位装置80，包括：

粒子分布单元801，用于基于机器人的历史实际位姿和预设时间段内的位姿变化量，获取当前粒子群，其中，粒子群中的每一个粒子表示机器人的一个位姿，粒子群用于表示机器人位姿的概率分布；

位姿估计单元802，用于为当前粒子群中的每一个粒子分配权重，并根据分配的权重获得机器人的第一估计位姿

扫描匹配单元803，用于基于第一估计位姿与机器人上设置的传感器采集的扫描数据，采用设定的扫描匹配模型进行扫描匹配，得到第二估计位姿；

位姿校正单元804，用于对第二估计位姿进行校正，得到机器人的当前实际位姿。

可选的，粒子分布单元801，具体用于：

以历史实际位姿为中心，通过高斯分布随机产生多个粒子组成第一粒子群；

确定位姿变化量是否大于预设变化量阈值；

若确定结果为是，则基于位姿变化量对第一粒子群中的每一个粒子进行更新得到第二粒子群，并将第二粒子群确定为当前粒子群；否则将第一粒子群确定为当前粒子群。

可选的，位姿估计单元802，具体用于：

针对当前粒子群中的每一个粒子，将扫描数据基于每一个粒子表示的位姿映射到基于预设的栅格概率地图构建的似然域模型中，并根据扫描结果在似然域模型中的概率值为每一个粒子分配权重；

基于当前粒子群以及每一个粒子的权重值获得加权平均位姿，并将加权平均位姿确定为第一估计位姿。

可选的，扫描匹配单元803，具体用于：

根据第一估计位姿和预设的栅格概率地图生成虚拟点云，并基于扫描数据生成实际点云；

将虚拟点云与实际点云进行扫描匹配，得到第二估计位姿。

可选的，扫描匹配单元804，具体用于：

初始化旋转平移变换值，并基于旋转平移变换值对实际点云中的每一个原始点进行旋转平移变换，得到每一个原始点对应的转换点；

针对每一个转换点，从虚拟点云中找到与该转换点之间的距离最近的第一点和第二点，记录该转换点对应的原始点与该转换点对应的第一点和第二点构成的参考线段之间的对应关系；

对旋转平移变换值进行迭代优化，以使得最终获取的旋转平移变换值能够使得所有转换点与对应的参考线段之间的距离之和最小，首次迭代将初始化的旋转平移变换值作为初始值；其中，每一次迭代包括如下步骤：

根据最后一次更新的旋转平移变换值对第一估计位姿进行更新，得到第二估计位姿。

可选的，位姿校正单元804，具体用于：

根据第二估计位姿和预设的栅格概率地图生成虚拟点云；

对第二估计位姿进行多次校正，直至最后一次校正后得到的位姿的偏移量小于预设偏移量阈值，首次校正将第二估计位姿作为初始值；

其中，每一次校正包括如下步骤：

根据机器人上设置的传感器获取的实际点云中每一点的实际距离，以及根据上一次校正后得到的估计位姿生成的虚拟点云中每一点的估计距离，确定上一次校正后得到的估计位姿的偏移量；

通过偏移量对上一次校正后得到的估计位姿进行校正，并确定偏移量是否小于预设偏移量阈值；

若确定结果为是，则停止校正，并将本次校正后得到的估计位姿确定为机器人当前实际位姿，否则将本次校正后得到的估计位姿作为下一次校正的初始值继续进行校正。

可选的，装置还包括重采样单元805，用于：

若确定结果为是，则基于当前粒子群进行粒子重采样，其中，重采样得到的粒子与当前粒子群中的粒子的权重相关。

该设备可以用于执行图1-7所示的实施例所提供的方法，因此，对于该设备的各功能模块所能够实现的功能等可参考图1-7所示的实施例的描述，不多赘述。其中，重采样单元805虽然在图8中一并示出，但是并非是必选的功能单元，因此以虚线示出。

请参见图9，基于同一发明构思，本发明实施例提供一种机器人90，包括至少一个处理器901，至少一个处理器901用于执行存储器中存储的计算机程序时实现图1-7所示的实施例提供的机器人定位方法的步骤。

可选的，至少一个处理器901具体可以包括中央处理器(CPU)、特定应用集成电路(application specific integrated circuit，ASIC)，可以是一个或多个用于控制程序执行的集成电路，可以是使用现场可编程门阵列(field programmable gate array，FPGA)开发的硬件电路，可以是基带处理器。

可选的，至少一个处理器901可以包括至少一个处理核心。

可选的，该设备还包括存储器902，存储器902可以包括只读存储器(read onlymemory，ROM)、随机存取存储器(random access memory，RAM)和磁盘存储器。存储器902用于存储至少一个处理器901运行时所需的数据。存储器902的数量为一个或多个。其中，存储器902在图9中一并示出，但需要知道的是存储器902不是必选的功能模块，因此在图9中以虚线示出。

基于同一发明构思，本发明实施例提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，当所述计算机指令在计算机上运行时，使得计算机执行如图1-7所示的方法。

在具体的实施过程中，计算机可读存储介质包括：通用串行总线闪存盘(Universal Serial Bus flash drive，USB)、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的存储介质。

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

在本发明实施例中的各功能单元可以集成在一个处理单元中，或者各个单元也可以均是独立的物理模块。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备，例如可以是个人计算机，服务器，或者网络设备等，或处理器(processor)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：通用串行总线闪存盘(universal serial bus flash drive)、移动硬盘、ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，以上实施例仅用以对本发明的技术方案进行了详细介绍，但以上实施例的说明只是用于帮助理解本发明实施例的方法，不应理解为对本发明实施例的限制。本技术领域的技术人员可轻易想到的变化或替换，都应涵盖在本发明实施例的保护范围之内。

Claims

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

根据所述第一估计位姿和预设的栅格概率地图生成虚拟点云，并基于扫描数据生成实际点云；其中，所述扫描数据是基于所述第一估计位姿与所述机器人上设置的传感器采集的；

将所述虚拟点云与所述实际点云进行扫描匹配，得到第二估计位姿；

2.如权利要求1所述的方法，其特征在于，所述基于机器人的历史实际位姿和预设时间段内的位姿变化量，获取当前粒子群，包括：

确定所述位姿变化量是否大于预设变化量阈值；

3.如权利要求1所述的方法，其特征在于，为所述当前粒子群中的每一个粒子分配权重，并根据分配的权重获得所述机器人的第一估计位姿，包括：

4.如权利要求1所述的方法，其特征在于，所述将所述虚拟点云与所述实际点云进行扫描匹配，得到所述第二估计位姿，包括：

5.如权利要求1所述的方法，其特征在于，所述对所述第二估计位姿进行校正，得到所述机器人的当前实际位姿，包括：

其中，每一次校正包括如下步骤：

6.如权利要求1~5任一所述的方法，其特征在于，在为所述当前粒子群中的每一个粒子分配权重之后，所述方法还包括：

7.一种机器人定位装置，其特征在于，包括：

位姿估计单元，用于为所述当前粒子群中的每一个粒子分配权重，并根据分配的权重获得所述机器人的第一估计位姿

扫描匹配单元，用于根据所述第一估计位姿和预设的栅格概率地图生成虚拟点云，并基于扫描数据生成实际点云；其中，所述扫描数据是基于所述第一估计位姿与所述机器人上设置的传感器采集的；将所述虚拟点云与所述实际点云进行扫描匹配，得到第二估计位姿；

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

至少一个处理器；以及

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如权利要求1~6任一权利要求所述的方法。

9.一种计算机存储介质，其特征在于：

所述计算机存储介质存储有计算机指令，当所述计算机指令在计算机上运行时，使得计算机执行如权利要求1-6中任一项所述的方法。