机器人定位方法、装置、机器人和可读存储介质

技术领域

本申请涉及机器人定位技术领域，尤其涉及一种机器人定位方法、装置、机器人和可读存储介质。

背景技术

为了让机器人能够在不同的场景满足不同的需求，机器人定位精度是一个重要的指标。机器人激光定位技术的原理在于，通过激光雷达扫描到的特征反算出机器人自身的位置，这是一种相对定位技术。

这种相对定位技术非常容易受到周围环境变化的影响，同时周围环境形成的地图也是由激光雷达预先扫描建立的，在地图生成过程中会给地图设置一个分辨率(如一个像素代表实际当中的5cm)，通常这个分辨率会兼顾导航精度和计算量，受地图分辨率的影响，故导航精度也将受到一定的制约。所以针对需要达到高精度定位(如3cm内)且对定位的稳定性要求也较高的场景，如地面不平导致定位偏移，需要迅速校正定位等，此时有个高精度且鲁棒的定位系统是一项迫切需要解决的技术问题。

发明内容

有鉴于此，本申请为了克服现有技术中的不足，提供一种机器人定位方法、装置、机器人和可读存储介质。

本申请的实施例提供一种机器人定位方法，所述机器人搭载有激光雷达，所述方法包括：

对机器人的位置进行一次定位以得到第一位姿；

将所述第一位姿作为所述机器人的初始位姿，利用所述初始位姿将当前激光点云在栅格地图中的坐标转换到机器人坐标系下的坐标以得到所述激光点云的转换坐标；

基于所述初始位姿和所述转换坐标对相应激光点云的得分优化函数进行梯度寻优以求解所述激光雷达的扫描数据与所述栅格地图的匹配位姿，所述匹配位姿作为所述机器人的第二位姿；其中，所述得分优化函数根据激光点云的转换坐标及激光点云在栅格地图中所在栅格的高斯分布信息构建；

在所述第二位姿满足预设匹配条件的情况下，将所述第二位姿作为所述机器人当前的最终位姿，在不满足所述预设匹配条件的情况下，将所述第一位姿作为所述机器人当前的最终位姿。

在一些实施例中，所述第二位姿满足预设匹配条件，包括：

所述第二位姿下所述激光点云的得分大于或等于置信度阈值，并且所述第二位姿和所述第一位姿之间的差值小于或等于预设误差阈值。

在一些实施例中，所述基于所述初始位姿和所述转换坐标对相应激光点云的得分优化函数进行梯度寻优以求解所述激光雷达扫描的数据与所述栅格地图的匹配位姿，包括：

基于所述转换坐标对所述激光点云的得分优化函数利用高斯牛顿迭代算法求解出对应的梯度和黑森矩阵；

将所有激光点云的所述梯度和所述黑森矩阵叠加以计算出当前的位姿增量；

将所述初始位姿与所述当前位姿增量相加得到第二位姿，其中，在所述第二位姿下激光雷达的扫描数据与所述栅格地图的匹配程度最高。

在一些实施例中，求解所述机器人的第二位姿之前，所述方法还包括：

对所述机器人预先加载的地图进行栅格化，得到由若干个栅格构成的栅格地图，以及根据所述栅格地图中障碍物的位置计算出每个栅格的高斯分布的均值和方差。

在一些实施例中，所述对机器人的位置进行一次定位以得到第一位姿，包括：

在所述机器人的初始位置周围随机生成相应数量的粒子，每个粒子具有各自的初始位姿及相同的初始权重；

根据移动指令使所述机器人在所述栅格地图中移动，并在每次移动后更新各个粒子的位姿，得到各个粒子的更新位姿；

根据所述粒子的更新位姿和所述机器人通过搭载的激光雷达测量得到的观测数据对对应粒子进行权重更新，得到各个粒子的更新权重；

按照所有粒子的所述更新权重的分布进行粒子重采样以得到重采样的所述数量的粒子，返回上述的移动步骤进行预设次数的迭代；

根据最终迭代得到的各个粒子的更新位姿和更新权重计算出所述机器人的第一位姿。

在一些实施例中，所述坐标转换的转换公式为：

其中，(x

在上述实施例中，所述第i个激光点云的得分优化函数的表达式为：

其中，q

本申请的实施例还提供一种机器人定位装置，所述机器人搭载有激光雷达，所述装置包括：

一次定位模块，用于对机器人的位置进行一次定位以得到第一位姿；

转换模块，用于将所述第一位姿作为所述机器人的初始位姿，利用所述初始位姿将当前激光点云在栅格地图中的坐标转换到机器人坐标系下的坐标以得到所述激光点云的转换坐标；

二次定位模块，用于基于所述初始位姿和所述转换坐标对所述激光点云的得分优化函数进行梯度寻优以求解所述激光雷达的扫描数据与所述栅格地图的匹配位姿，所述匹配位姿作为所述机器人的第二位姿；其中，所述得分优化函数根据激光点云的转换坐标及激光点云在栅格地图中所在栅格的高斯分布信息构建；

定位选取模块，用于在所述第二位姿满足预设匹配条件的情况下，将所述第二位姿作为所述机器人当前的最终位姿，在不满足所述预设匹配条件的情况下，将所述第一位姿作为所述机器人当前的最终位姿。

本申请的实施例还提供一种机器人，所述机器人包括处理器和存储器，所述存储器存储有计算机程序，所述计算机程序在所述处理器上执行时，实施上述的机器人定位方法。

本申请的实施例还提供一种可读存储介质，其存储有计算机程序，所述计算机程序被处理器执行时，实施上述的机器人定位方法。

本申请的实施例具有如下有益效果：

本申请实施例的机器人定位方法先得到机器人的第一次定位位置；进而以该第一次定位位置为基础，结合机器人的周围环境信息并利用迭代优化的方法来对机器人的位置进行再次求解，得到机器人的第二次定位位置，通过比对这两个定位位置并从中选取出机器人当前的最终定位结果，该方法通过两次定位来确定最终定位位置，能够适应不同的环境和场景，在不需要非常高的精度时能够定位稳定，而有些场景又能达到所需的精度，具有较强的定位鲁棒性等。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1示出了本申请实施例的机器人定位方法的流程示意图；

图2示出了本申请实施例的机器人定位方法的一次定位的流程示意图；

图3示出了本申请实施例的机器人定位方法的二次定位的流程示意图；

图4示出了本申请实施例的机器人定位装置的结构示意图。

具体实施方式

下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。

通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

在下文中，可在本申请的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。

除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本申请的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本申请的各种实施例中被清楚地限定。

现有技术中，对于基于粒子滤波的实时定位技术，采用粒子滤波的方法将里程计数据，惯性测量单元(IMU)数据以及激光雷达的测量数据进行数据融合，再通过多个粒子的状态值计算出平均位置作为最终机器人的位置。然而，由于粒子滤波的特性，会保留非常多的次优粒子，用来提高定位的鲁棒性，让粒子滤波不容易因为环境的变化而产生定位跳变，但是次优粒子的存在，就让机器人的定位精度打了折扣，它得到最终的位置是所有粒子的一个平均值，这个值在最优值的附近，因此它达不到很高的定位精度。

对于另一种方法，基于优化匹配的实时定位技术，如牛顿优化匹配，相关性匹配等方法，这种方法是利用传感器的运动模型估计一个初始位姿，再通过牛顿迭代或者最小二乘优化等方法，在初始位姿的附近的一定范围内找出一个最优的位姿，这种方法当环境特征明显时，由于它能够计算出最优的位置，故可以得到较高的精度，但是当环境有一定的变化，或者环境相似的区域，位置就会变得不稳定，产生位置跳动，而且当位置跳到错误位置之后由于机器人的运动，会越来越偏离正确的位置，当超过匹配的范围后，会导致无法恢复定位，例如匹配范围为1米，此方法位置漂移超过1米后便无法恢复。

由于上述两种导航定位技术分别存在相应缺陷，为此，本申请实施例提出一种机器人定位方法，不仅能够抗环境变化，具有较强定位鲁棒性，同时还能够得到高精度的定位结果。本申请实施例的方法将结合粒子滤波算法对机器人进行一次定位，进而以一次定位的结果为二次定位的基础，结合优化匹配的方法来对机器人进行二次定位，通过比对两次定位的结果来选取出最终的定位结果。

实施例1

请参照图1，本实施例提出一种机器人定位方法，可用于搭配有激光雷达的机器人在不同的场合下的自定位等。

下面对该机器人定位方法进行详细说明。

步骤S110，对机器人的位置进行一次定位以得到第一位姿。

示范性地，以粒子群滤波算法为例，对机器人的位置进行一次定位，以保证定位的鲁棒性。如图2所示，该第一位姿的获取步骤，包括：

子步骤S111，在机器人的初始位置周围随机生成相应数量的粒子，每个粒子具有各自的初始位姿及相同的初始权重。

示范性地，可在机器人的初始位置附近生成一系列粒子(即粒子群)，这些粒子通常符合相应的分布规律，如高斯分布等。其中，每个粒子的初始位姿包括与机器人之间的距离r和方向角度θ，而距离r可通过二维平面坐标(x,y)来表示。权重w代表该粒子的位姿为机器人的真实位置的概率，在初始状态下，将对所有粒子进行权重归一化处理，可使得每个粒子的初始权重值相同。

进而，通过控制机器人运动，即利用运动模型对这些粒子进行位姿更新，并根据机器人上搭载的激光雷达对周围环境的观测数据，即利用观测模型对这些粒子进行打分，得到各个粒子的权重分布。通过对这些粒子进行权重筛选，最终得到的粒子位姿即作为机器人的位置。

子步骤S112，根据移动指令使机器人在栅格地图中移动，并在每次移动后更新各个粒子的位姿，得到各个粒子的更新位姿。

示范性地，若总的粒子数量为M，对于M个粒子，其在t时刻的位姿可表示如下：

子步骤S113，根据粒子的更新位姿和机器人通过搭载的激光雷达测量得到的观测数据对对应粒子进行权重更新，得到各个粒子的更新权重。

示范性地，通过观测数据对这些粒子的权重进行更新，可以使得粒子的位姿更加趋近于机器人真实的位置。例如，对于第m个粒子的更新权重，可表示如下：

子步骤S114，按照所有粒子的所述更新权重的分布进行粒子重采样以得到重采样的所述数量的粒子，返回上述的移动步骤进行预设次数的迭代。

示范性地，在对所有粒子的权重进行更新后，通过排序粒子权重大小，并按照这些粒子的权重分布进行粒子重采样，得到数量不变的M个粒子，进而返回上述步骤S112，重复执行上述的位姿更新及权重更新步骤，直到执行所述预设次数时停止。

可以理解，对于权重高的粒子，即越趋近于机器人真实位置的粒子，在重新采样过程中将被保留，而对于权重低的粒子，即位姿不可信的粒子，则将被逐渐丢弃。上述的预设次数可以由机器人预先设置，也可以由机器人预先生成，或者由用户根据实际需求预先设置。当然，上述的迭代次数也可以根据其他条件来约束，如使粒子的权重值满足相应要求等，而并不限于设置的迭代次数。

子步骤S115，根据最终迭代得到的各个粒子的更新位姿和更新权重计算出机器人的第一位姿。

示范性地，对于最终得到的这些粒子的位姿和权重，例如，可利用加权平均算法等计算出该机器人的第一位姿。当然，基于这些粒子的位姿也可以采用其他的方法来计算出机器人的位置，在此并不作限定。

考虑到采用粒子滤波采用了加权平均的方法来求解位姿，可能会失去了一定的定位精度，为此，本实施例将在求解到第一位姿的基础上，对机器人的位置进行再次求解，此时将根据机器人的周围环境信息，采用高斯牛顿迭代的优化方法，求出机器人的高精度的定位位置。

在进行梯度寻优求解之前，本实施例先对机器人预先加载的地图进行栅格化，以得到由若干个栅格构成的栅格地图。进而，根据该地图中的障碍物位置计算出每个栅格的高斯分布的均值和方差。其中，这些栅格的高斯分布的均值和方差将留作后续的匹配优化时使用，而该均值和方差可通过相应的高斯分布的均值和方差计算公式计算得到。

步骤S120，将第一位姿作为机器人的初始位姿，利用初始位姿将当前激光点云在栅格地图中的坐标转换到机器人坐标系下的坐标以得到激光点云的转换坐标。

示范性地，通过将当前激光点云的坐标转换到地图上，以确定各个激光点云在哪个栅格中，进而可计算每个激光点云在栅格中的得分。可以理解，得分越高，则表示在其代表的位姿下激光扫描与地图的匹配程度越高，即意味着位姿越接近机器人的真实位姿，反之，匹配程度越低。

在一种实施方式中，若激光点云在栅格地图中的坐标为(x

其中，对于机器人的位姿T＝(T

步骤S130，基于初始位姿和所述转换坐标对激光点云的得分优化函数进行梯度寻优以求解激光雷达的扫描数据与栅格地图的匹配位姿，所述匹配位姿作为机器人的第二位姿。

示范性地，每个激光点云的得分优化函数根据激光点云的转换坐标及激光点云在栅格地图中所在栅格的高斯分布信息预先构建，例如，该高斯分布信息可包括均值和方差等。在一种实施方式中，第i个激光点云的得分优化函数可表示为：

其中，q

基于预先构建的得分优化函数，可对该得分优化函数进行梯度求解以得到最优位姿。示范性地，如图3所示，第二位姿的求解步骤，包括：

子步骤S131，基于所述转换坐标对相应激光点云的得分优化函数利用高斯牛顿迭代算法求解出对应的梯度和黑森矩阵。

示范性地，通过对得分优化函数的表达式中机器人的位姿进行一阶求导，可得到由一阶导数构成的雅克比(Jacobian)矩阵，其中，梯度为一阶导数构成的方向向量。进而，对一阶导数的表达式进行二阶求偏导，可得到由二阶偏导数构成的黑森(Hessian)矩阵。

子步骤S132，将所有激光点云的所述梯度和所述黑森矩阵叠加以计算出当前的位姿增量。

例如，仍以上述的得分优化函数的表达式为例，机器人的位姿为T，对于第i个激光点云，其一阶导数的表达式有：

以及，二阶偏导数的表达式为：

于是，求解的梯度dTr为：

示范性地，在求解出每个激光点云的梯度和黑森矩阵叠加，可得到最终的位姿优化增量ΔT，其中，ΔT＝H

子步骤S133，将所述初始位姿与所述当前位姿增量相加得到第二位姿。

示范性地，若初始位姿为T

本实施例中，根据第二位姿是否满足预设匹配条件来选取第一位姿或第二位姿作为机器人当前的最终位姿。通过将两个定位位置进行比较并从中选取出一个作为机器人当前的最终定位结果，可以实现在不同的场景得到最优的定位结果。

步骤S140，在第二位姿满足预设匹配条件的情况下，将第二位姿作为机器人当前的最终位姿。

在一种实施方式中，该预设匹配条件可为：第二位姿对应的得分大于或等于预先设置的置信度阈值，并且第二位姿和第一位姿之间的差值小于或等于预设误差阈值。可以理解为，当第二位姿本身的得到较高，同时与第一位姿的误差在可接受的偏差范围内，则选取后面得到的定位位置作为最终定位结果。

步骤S150，在第二位姿不满足预设匹配条件的情况下，将第一位姿作为机器人当前的最终位姿。

示范性地，若第二位姿对应的得分小于上述的置信度阈值，或者，第二位姿对应的得分大于或等于上述置信度阈值并且第二位姿和第一位姿之间的差值大于上述的误差阈值时，则将第一位姿作为机器人当前的最终定位位置。

由于采用迭代优化的方法可以得到一个精准位置，但是考虑到对于环境大幅度变化的场景，在进行迭代优化时是出现发散现象，此时所得到的位置将是错误的。为此，本实施例结合两种定位结果来选取更准确的定位结果。

可以理解，当两种定位结果的误差较大时，即表明此时的环境变化较大，故将通过粒子滤波方法得到的第一位姿作为最终的定位结果；反之，当两种定位结果的误差较小时，即表明优化的结果比较可靠，故将通过优化的方法得到的高精度的第二位姿作为最终的定位结果。

本实施例的机器人定位方法先通过粒子群滤波的方法进行定位，由于机器人的位置是一个缓慢变化的过程，不会在环境发散时发生跳变，故具有较好的鲁棒性；而第二次定位方法则以该第一次定位位置为基础，根据机器人的周围环境信息，采用迭代优化的方法，得到较高精度的定位，该方法能够适应不同的环境和场景，具有较好的鲁棒性和较高的定位精度等。

实施例2

请参照图4，基于上述实施例1的方法，本实施例提出一种机器人定位装置，该机器人搭载有激光雷达，示范性地，该机器人定位装置100包括：

一次定位模块110用于对机器人的位置进行一次定位以得到第一位姿；

转换模块120用于将所述第一位姿作为所述机器人的初始位姿，利用所述初始位姿将当前激光点云在栅格地图中的坐标转换到机器人坐标系下的坐标以得到所述激光点云的转换坐标。

二次定位模块130用于基于所述初始位姿和所述转换坐标对所述激光点云的得分优化函数进行梯度寻优以求解所述激光雷达的扫描数据与所述栅格地图的匹配位姿，所述匹配位姿作为所述机器人的第二位姿。其中，所述得分优化函数根据激光点云的转换坐标及激光点云在栅格地图中所在栅格的高斯分布信息构建。

定位选取模块140用于在所述第二位姿满足预设匹配条件的情况下，将所述第二位姿作为所述机器人当前的最终位姿，以及，在不满足所述预设匹配条件的情况下，将所述第一位姿作为所述机器人当前的最终位姿。

可以理解，本实施例的装置对应于上述实施例1的方法，上述实施例1中的可选项同样适用于本实施例，故在此不再重复描述。

本申请还提供了一种机器人，例如，该机器人为移动机器人，在移动过程中能够实现自定位。示范性地，该机器人包括处理器和存储器，其中，存储器存储有计算机程序，处理器通过运行所述计算机程序，从而使机器人执行上述的机器人定位方法或者上述机器人定位装置中的各个模块的功能。

本申请还提供了一种可读存储介质，用于储存上述机器人中使用的所述计算机程序。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本申请各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。

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

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。