CN113865588A - 一种机器人定位方法和装置 - Google Patents

Abstract

本发明公开了一种机器人定位方法和装置。其中，所述方法包括：在机器人行驶的过程中，根据机器人的里程计数据、前一时刻的位姿以及当前时刻与前一时刻之间的时间差，得到当前时刻机器人的预测位姿；在所述预测位姿附近随机撒粒子，根据优化的观测模型，确定当前时刻机器人的最终位姿。本发明在计算粒子权重的时候增加了高度数据的权重，一定程度上降低了距离远、高度低的激光点参与定位结果计算的比例，提升了定位的鲁棒性，降低了定位丢失的风险。

Description

一种机器人定位方法和装置

技术领域

本发明实施例涉及机器人定位技术，尤其涉及一种机器人定位方法和装置。

背景技术

近年来，随着同步建图与定位(SLAM，Simultaneous localization and mapping)技术的发展，其应用领域从扫地机行业逐步扩展到服务机器人、自动导航装置(AutomatedGuided Vehicle，AGV)、无人机、自动驾驶等领域。随着制造业以及电商仓储领域对柔性化搬运的需求不断上升，SLAM导航迎来了广阔的市场。

然而实际应用过程中，工业机器人的应用环境会相当复杂，典型的便是AGV行业。不同于实验环境那般理想，它的工作环境会更加多变一些，要求机器人具备更高的鲁棒性，对于定位精度的要求也高于服务机器人等其他行业。

现有技术中，通常采用3D激光SLAM来进行机器人定位，但是由于环境的动态性，当前的机器人定位方法存在定位精度不够甚至定位信息丢失的情况，严重的话有可能造成安全事故。

发明内容

本发明提供一种机器人定位方法和装置，以提高机器人在动态环境中定位的鲁棒性，降低定位丢失的风险。

第一方面，本发明实施例提供了一种机器人定位方法，包括：

在机器人行驶的过程中，根据机器人的里程计数据、前一时刻的位姿以及当前时刻与前一时刻之间的时间差，得到当前时刻机器人的预测位姿；

在所述预测位姿附近随机撒粒子，根据优化的观测模型，确定当前时刻机器人的最终位姿。

可选的，所述优化后的观测模型公式为：

P(z_t ^k|x_t,m)＝z_hitp_hit+z_rand/z_max+z_heightp_height；

其中，x_t为t时刻机器人的位姿，m为点云地图，z_t ^k为t时刻激光测量端点，p_hit为测量误差，p_height为与高度成正比的数据，z_hit、z_rand、z_max、z_height分别为测量误差、，最大测量距离、背景噪声以及高度数据的权重。

可选的，所述根据优化的观测模型，确定当前时刻机器人的最终位姿包括：

根据优化后的观测模型，计算各粒子的权重；

根据每个粒子的权重以及每个粒子对应的机器人位姿对所有粒子进行加权平均，以确定当前时刻机器人的最终位姿。

可选的，在计算各粒子的权重之后，还包括：

当计算得到的粒子权重小于权重下限阈值，则删除该粒子；

当计算得到的粒子权重大于权重上限阈值，则复制该粒子。

可选的，所述在机器人行驶的过程中，根据机器人的里程计数据、前一时刻的位姿以及当前时刻与前一时刻之间的时间差，得到当前时刻机器人的预测位姿之前，还包括：

根据安装在机器人身上的图像采集装置，获取机器人工作环境的点云地图；

根据机器人行驶过程中采集到的三维点云数据以及里程计数据，确定机器人的初始位姿。

第二方面，本发明实施例还提供了一种机器人定位装置，包括：

预测模块，用于在机器人行驶的过程中，根据机器人的里程计数据、前一时刻的位姿以及当前时刻与前一时刻之间的时间差，得到当前时刻机器人的预测位姿；

定位模型，用于在所述预测位姿附近随机撒粒子，根据优化的观测模型，确定当前时刻机器人的最终位姿。

可选的，所述所述定位模块具体用于：

根据优化后的观测模型，计算各粒子的权重；

根据每个粒子的权重以及每个粒子对应的机器人位姿对所有粒子进行加权平均，以确定当前时刻机器人的最终位姿。

本发明通过在机器人行驶的过程中，根据机器人的里程计数据、前一时刻的位姿以及当前时刻与前一时刻之间的时间差，对当前时刻的机器人位姿进行预测，进而在预测位姿附近随机撒粒子，并根据优化的观测模型，来确定当前机器人的最终位姿。本发明在计算粒子权重的时候增加了高度数据的权重，一定程度上降低了距离远、高度低的激光点参与定位结果计算的比例，提升了定位的鲁棒性。

附图说明

图1为本发明实施例提供的一种机器人定位方法的流程图；

图2为本发明实施例提供的另一种机器人定位方法的流程图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。

实施例

图1为本发明实施例提供的一种机器人定位方法的流程图，本实施例可适用于对机器人进行定位的情况，典型的，该方法适用于对工业领域的工业机器人进行定位。该方法可以由机器人定位装置来执行，具体包括如下步骤：

步骤S1、根据安装在机器人身上的图像采集装置，获取机器人工作环境的点云地图。

其中，图像采集装置可以为机器人上安装的三维激光传感器，当机器人在工作环境中走一圈后，即可获得工作环境对应的点云地图。

本实施例中，为了提高点云数据的准确性，对三维激光传感器获取的点云数据进行滤波处理。由于本实施例采用的是现有技术中的滤波方法，故不对滤波方法进行赘述。

步骤S2、根据机器人行驶过程中采集到的三维点云数据以及里程计数据，确定机器人的初始位姿。

其中，里程计数据为机器人行驶过程中的物理量，例如行驶距离、速度以及角速度等。本实施例中的里程计数据可以通过编码器得到。三维点云数据是指在一个三维坐标系统中的一组向量的集合。

在获取到机器人的三维点云数据以及里程计数据之后，根据上述数据可以人为的给定机器人的初始位姿。

步骤S3、在机器人行驶的过程中，根据机器人的里程计数据、前一时刻的位姿以及当前时刻与前一时刻之间的时间差，得到当前时刻机器人的预测位姿。

时间差与行驶速度的乘积为机器人当前时刻相比于上一时刻行驶的位移，通过将机器人前一时刻的位姿与上述位移进行加和，可以得到当前时刻机器人的预测位姿。

步骤S4、在所述预测位姿附近随机撒粒子，根据优化的观测模型，确定当前时刻机器人的最终位姿。

其中，粒子滤波就是指：通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数，用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程。

由于传感器的测量值不可避免会有误差，包含测量过程得出的机器人当前时刻位姿不一定准确。在其附近撒粒子，对目标区域中车辆可能出现的位置概率的进行估测，每个粒子都有可能是机器人所在的真实位置，需要通过重要性重采样和权重计算筛选出概率最大的粒子。

本实施例中采用的是优化的激光点云观测模型，对应的公式为：

P(z_t ^k|x_t,m)＝z_hitp_hit+z_rand/z_max+z_heightp_height；

其中，x_t为t时刻机器人的位姿，m为点云地图，z_t ^k为t时刻激光测量端点，p_hit为测量误差，p_height为与高度成正比的数据，z_hit、z_rand、z_max、z_height分别为测量误差、，最大测量距离、背景噪声以及高度数据的权重。

优化后的点云观测模型将激光点的测距值以及高度考虑进去，对点云的高度区域进行划分，由于机器人实际的工作环境中普遍高度越高，环境的动态性越低，并且检测距离越远，激光的测距精度越差，可靠性越差。因此，优化后的观测模型增加了高度数据的权重，一定程度降低了距离远、高度低的激光点参与定位结果计算的比例，定位鲁棒性明显有所提升。

进一步的，所述根据优化的观测模型，确定当前时刻机器人的最终位姿包括：

根据优化后的观测模型，计算各粒子的权重；根据每个粒子的权重以及每个粒子对应的机器人位姿对所有粒子进行加权平均，以确定当前时刻机器人的最终位姿。

本实施例中，粒子根据当前激光的观测数据和全局环境地图的匹配程度被赋予权重，权重表征粒子位姿的好坏。粒子的权重计算由测量概率模型给出，具体的计算公式如下：

其中，z_t为激光观测值，x_t为粒子的位姿。

进一步的，当粒子出现权重退化现象时，需要进行重采样，具体的重采样步骤包括：

当计算得到的粒子权重小于权重下限阈值，则删除该粒子；当计算得到的粒子权重大于权重上限阈值，则复制该粒子。

进一步参见图2，在粒子进行重采样之后，根据粒子权重以及每个粒子对应的机器人位姿计算出机器人当前位姿；最终的位姿输出由所有粒子的加权平均求得。

本实施例中，考虑到需要有一个对于全局定位结果的好坏，即定位的不确定度的评判标准，在定位的同时也实时输出了当前定位所用到的所有粒子的位置的总方差，使得定位结果的稳定性更为直观。

在上述实施例的基础上，在计算机器人当前时刻位姿的同时，还同时通过里程计差值做位姿平滑。

本实施例中，为了降低点云计算量，获取激光点云的频率降为5Hz，通过激光点云去匹配已知点云地图，会降低位姿计算发布的实时性。相对激光点云而言，里程计发布频率20Hz显然高很多，因此在位姿计算过程中，通过激光点云和里程计的时间戳以及实时的里程计数据，进行运动补偿，并且使用里程计做线性插值，提高位姿发布的频率，提高定位实时性。

本发明实施例还提供一种机器人定位装置，该装置具体包括：预测模块，用于在机器人行驶的过程中，根据机器人的里程计数据、前一时刻的位姿以及当前时刻与前一时刻之间的时间差，得到当前时刻机器人的预测位姿；

定位模型，用于在所述预测位姿附近随机撒粒子，根据优化的观测模型，确定当前时刻机器人的最终位姿。

其中，所述优化后的观测模型公式为：

P(z_t ^k|x_t,m)＝z_hitp_hit+z_rand/z_max+z_heightp_height；

其中，x_t为t时刻机器人的位姿，m为点云地图，z_t ^k为t时刻激光测量端点，p_hit为测量误差，p_height为与高度成正比的数据，z_hit、z_rand、z_max、z_height分别为测量误差、，最大测量距离、背景噪声以及高度数据的权重。

具体的，所述定位模块具体用于：根据优化后的观测模型，计算各粒子的权重；

根据每个粒子的权重以及每个粒子对应的机器人位姿对所有粒子进行加权平均，以确定当前时刻机器人的最终位姿。

可选的，该装置还包括重采样模块，具体用于当计算得到的粒子权重小于权重下限阈值，则删除该粒子；当计算得到的粒子权重大于权重上限阈值，则复制该粒子。

可选的，该装置还包括地图获取模块，用于根据安装在机器人身上的图像采集装置，获取机器人工作环境的点云地图；

初始位姿确定模块，用于根据机器人行驶过程中采集到的三维点云数据以及里程计数据，确定机器人的初始位姿。

本发明实施例所提供的机器人定位装置可执行本发明任意实施例所提供的机器人定位方法，具备执行方法相应的功能模块和有益效果。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (7)

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

在机器人行驶的过程中，根据机器人的里程计数据、前一时刻的位姿以及当前时刻与前一时刻之间的时间差，得到当前时刻机器人的预测位姿；

在所述预测位姿附近随机撒粒子，根据优化的观测模型，确定当前时刻机器人的最终位姿。

2.根据权利要求1所述的方法，其特征在于，所述优化后的观测模型公式为：

P(z_t ^k|x_t,m)＝z_hitp_hit+z_rand/z_max+z_heightp_height；

其中，x_t为t时刻机器人的位姿，m为点云地图，z_t ^k为t时刻激光测量端点，p_hit为测量误差，p_height为与高度成正比的数据，z_hit、z_rand、z_max、z_height分别为测量误差、最大测量距离、背景噪声以及高度数据的权重。

3.根据权利要求2所述的方法，其特征在于，所述根据优化的观测模型，确定当前时刻机器人的最终位姿包括：

根据优化后的观测模型，计算各粒子的权重；

根据每个粒子的权重以及每个粒子对应的机器人位姿对所有粒子进行加权平均，以确定当前时刻机器人的最终位姿。

4.根据权利要求3所述的方法，其特征在于，在计算各粒子的权重之后，还包括：

当计算得到的粒子权重小于权重下限阈值，则删除该粒子；

当计算得到的粒子权重大于权重上限阈值，则复制该粒子。

5.根据权利要求1所述的方法，其特征在于，在机器人行驶的过程中，根据机器人的里程计数据、前一时刻的位姿以及当前时刻与前一时刻之间的时间差，得到当前时刻机器人的预测位姿之前，还包括：

根据安装在机器人身上的图像采集装置，获取机器人工作环境的点云地图；

根据机器人行驶过程中采集到的三维点云数据以及里程计数据，确定机器人的初始位姿。

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

预测模块，用于在机器人行驶的过程中，根据机器人的里程计数据、前一时刻的位姿以及当前时刻与前一时刻之间的时间差，得到当前时刻机器人的预测位姿；

定位模型，用于在所述预测位姿附近随机撒粒子，根据优化的观测模型，确定当前时刻机器人的最终位姿。

7.根据权利要求6所述的装置，其特征在于，所述定位模块具体用于：

根据优化后的观测模型，计算各粒子的权重；

根据每个粒子的权重以及每个粒子对应的机器人位姿对所有粒子进行加权平均，以确定当前时刻机器人的最终位姿。

Images