CN110530368B

CN110530368B - 一种机器人定位方法及设备

Info

Publication number: CN110530368B
Application number: CN201910777618.1A
Authority: CN
Inventors: 胡立志; 林辉; 卢维; 殷俊; 穆方波
Original assignee: Zhejiang Huaray Technology Co Ltd
Current assignee: Zhejiang Huaray Technology Co Ltd
Priority date: 2019-08-22
Filing date: 2019-08-22
Publication date: 2021-06-15
Anticipated expiration: 2039-08-22
Also published as: CN110530368A

Abstract

本发明公开了一种机器人定位方法及设备，用于根据机器人当前环境的变化对激光传感器获取的点云数据进行剔除，剔除环境中动态障碍物的影响，提高创建的地图精度和机器人的定位精度。该方法包括：获取激光点云数据，并结合里程计信息估计当前位姿；根据获取的激光点云数据和当前占据栅格地图，确定存在新增激光点云数据时，从所述激光点云数据中去除新增激光点云数据得到有效激光点云数据，所述新增激光点云数据为由当前占据栅格地图上标定的障碍物之外的物体产生的激光点云数据，所述占据栅格地图用于标定当前环境对应的栅格位置是否有障碍物；利用有效激光点云数据基于当前占据栅格地图对当前位姿进行校正。

Description

一种机器人定位方法及设备

技术领域

本发明涉及定位技术领域，尤其涉及一种机器人定位方法及设备。

背景技术

现有技术能够通过移动机器人执行任务，例如在仓库中搬运货物等，机器人利用激光传感器获取根据当前环境生成的激光点云数据，创建当前环境的地图，并利用该激光点云数据对机器人进行定位，确定机器人的位姿，当移动机器人所在的环境发生变化，例如搬运货物、工作人员走动导致原来的环境发生变化时，激光传感器接收到的激光点云数据发生变化，此时并未根据当前环境的变化相应的对激光点云数据进行筛选，直接使用接收到的激光点云数据对机器人进行定位，导致利用激光点云数据确定的机器人的位姿精度较低，造成机器人无法更精确的完成任务。

发明内容

本发明提供一种机器人定位方法及设备，用于根据机器人当前环境的变化对激光传感器获取的点云数据进行剔除，剔除环境中动态障碍物的影响，利用剔除后的点云数据对机器人的位姿进行纠正，提高机器人的定位精度。

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

获取激光点云数据，并结合里程计信息估计当前位姿；

根据获取的激光点云数据和当前占据栅格地图，确定存在新增激光点云数据时，从所述激光点云数据中去除新增激光点云数据得到有效激光点云数据，所述新增激光点云数据为由当前占据栅格地图上标定的障碍物之外的物体产生的激光点云数据，所述占据栅格地图用于标定当前环境对应的栅格位置是否有障碍物；

利用有效激光点云数据基于当前占据栅格地图对当前位姿进行校正。

第二方面，本发明提供一种机器人定位设备，该设备包括：处理器以及存储器，其中，所述存储器存储有程序代码，当所述程序代码被所述处理器执行时，使得所述处理器执行如下步骤：

获取激光点云数据，并结合里程计信息估计当前位姿；

第三方面，本发明提供另一种机器人定位设备，该设备包括：获取激光点云单元、获取有效激光点云单元、位姿校正单元，其中：

获取激光点云单元，用于获取激光点云数据，并结合里程计信息估计当前位姿；

获取有效激光点云单元，用于根据获取的激光点云数据和当前占据栅格地图，确定存在新增激光点云数据时，从所述激光点云数据中去除新增激光点云数据得到有效激光点云数据，所述新增激光点云数据为由当前占据栅格地图上标定的障碍物之外的物体产生的激光点云数据，所述占据栅格地图用于标定当前环境对应的栅格位置是否有障碍物；

位姿校正单元，用于利用有效激光点云数据基于当前占据栅格地图对当前位姿进行校正。

第四方面，本发明提供一种计算机存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述第一方面所述方法的步骤。

本发明提供的一种机器人定位方法及设备，具有以下有益效果：

用于根据机器人当前环境的变化对激光传感器获取的点云数据进行剔除，获取有效激光点云数据，剔除环境中动态障碍物的影响，利用有效激光点云数据对机器人的位姿进行纠正，提高机器人的定位精度。

附图说明

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

图2为本发明实施例提供的机器人激光扫描示意图；

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

图4为本发明实施例提供的一种反光板定位流程图；

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

图6为本发明实施例提供的一种机器人定位设备示意图；

图7为本发明实施例提供的另一种机器人定位设备示意图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

实施例1

本实施例提出一种机器人定位方法，本实施例提到多个专用名词，对本实施例中提到的专用名词分别进行如下解释：

本实施例中的“机器人”如图1所示，包括移动地盘10和激光传感器11，其中移动地盘10包括里程计12、运动控制器、电机、电池、嵌入式计算机。本实施例中的机器人可以是能够获取到激光点云数据和里程计信息的机器人，对机器人具体包括哪些部件不作过多限定。

本实施例中的“激光传感器11”，用于获取二维平面信息的传感器，用于检测周围环境的二维平面轮廓信息，该激光传感器的扫描示意图如图2所示，该激光传感器可以是2D激光雷达传感器，本发明实施例不作过多限定。

本实施例中的“里程计12”，用于估算移动机器人角度和距离的变化量，可采用轮式编码器，从而能够根据前后时刻的位姿变化量和前一时刻的位姿来估计出当前时刻机器人的位姿。

本实施例中的“位姿”表示机器人在空间坐标系中的位置和姿态，可以用矩阵表示，如位姿为[x y θ]^T，x、y为机器人在二维平面的位置，θ为机器人在二维平面的方向。

如图3所示，本实施例方法具体实施步骤如下：

步骤300：获取激光点云数据，并结合里程计信息估计当前位姿。

本实施例中获取激光点云数据的方式如下：

可以采用机器人连续发射激光对当前环境进行扫描，连续获取激光点云数据的方式；也可以采用间隔设定时间发射激光进行扫描，间隔设定时间的获取激光点云数据方式；不论采用上述哪一种方式，都是每获取一次激光点云数据，都可以结合里程计信息估计一次当前位姿。

本实施例采用自适应蒙特卡洛定位AMCL算法，基于里程计信息以及获取的激光点云数据，根据前一时刻位姿估计当前时刻位姿。

本实施例基于确定的机器人的初始位姿的基础上，结合里程计信息估计当前位姿，本实施例中确定机器人的初始位姿可采用如下任一种算法：自定义的算法、AMCL算法、SLAM算法等，本实施例不作过多限定。

每次获取到激光点云数据时，基于里程计信息，根据前一时刻位姿估计当前时刻位姿。

当前时刻位姿服从概率分布函数p(x_t|u_t,x_t-1)；

x_t为当前时刻机器人在世界坐标系下的位姿，x_t-1为前一时刻机器人在世界坐标系下的位姿；其中所述世界坐标系为预定义的坐标系。

x、y分别为世界坐标系下横纵坐标，表示机器人在x轴、y轴所在二维平面的位置，θ表示机器人在x轴、y轴所在二维平面的方向；

u_t为里程计信息，

分别表示在里程计坐标系下前一时刻位姿和当前时刻位姿，里程计信息可由里程计传感器直接测得。

本实施例在利用上述步骤确定当前时刻位姿时，通过粒子滤波器对获取的激光点云数据以粒子群的形式进行分布，本实施例中的“粒子滤波器”，是一种数据优化方法，其中“粒子”表示机器人一种可能的位姿状态，“粒子群”表示机器人位姿的所有可能性，通过机器人运动的改变量进行粒子群的重新分布，同时，可以使用激光传感器获取的激光点云数据来进行粒子权重的分配。

本实施例中只要获取激光点云数据就可以对粒子权重进行分配，可以是设定间隔的获取激光点云数据进行粒子权重的分配，也可以是连续的获取激光点云数据进行粒子权重的分配，本实施例不作过多限定。

在初始时刻时，基于机器人的初始位姿进行粒子群的分布，此时粒子群中的每个粒子的权重相等。

机器人移动的过程中，确定获取激光点云数据时，同时监测到里程计信息中的运动改变量大于阈值时，根据获取的激光点云数据，利用预设观测模型调整粒子群中的粒子权重。

其中，所述预设观测模型可以是概率分布函数

上述观测模型

可采用激光传感器的似然域模型，其中：

表示在t时刻通过粒子群采样的位姿，m表示通过粒子群采样的粒子数量；

表示基于t时刻采用的位姿，激光传感器观测到的位姿的观测值。

由于在机器人的移动过程中，粒子群不断的进行重新分布的过程，从而使得粒子群的粒子数量可能会逐渐减少，如果当前粒子数量减少到阈值或者影响位姿确定的精度时，需要采取对粒子群重采样的方式，增加粒子数量。

本实施例中对粒子群重采样可以是每间隔预设时间对粒子群重采样，也可以是确定当前粒子数量减少到阈值时，对粒子群重采样。

对粒子群重采样后，根据获取的激光点云数据，利用预设观测模型再次重新调整重采样的粒子群中粒子权重的分配。

可选的，将重新调整后的粒子群中粒子分布情况存储在KD树中便于后期进行搜索遍历。

对上述粒子群中粒子权重重新进行调整后，根据重新调整后的粒子权重，对根据里程计信息估计的位姿加权得到当前位姿。

基于上述方法确定当前位姿后，如果当前环境中新增静态障碍物或移动物体时，会影响对机器人定位精度，本实施例通过如下方法能够去除新增静态障碍物或移动物体产生的激光点云数据，提高机器人定位精度。

步骤301：根据获取的激光点云数据和当前占据栅格地图，确定存在新增激光点云数据时，从所述激光点云数据中去除新增激光点云数据得到有效激光点云数据，所述新增激光点云数据为由当前占据栅格地图上标定的障碍物之外的物体产生的激光点云数据，所述占据栅格地图用于标定当前环境对应的栅格位置是否有障碍物。

本实施例中的占据栅格地图用于标定当前环境对应的栅格位置是否有障碍物，本实施例中可采用同步定位与建图SLAM算法，基于获取的激光点云数据生成占据栅格地图，每个栅格以概率的形式表示被障碍物占据的概率。本实施例中将大于预设概率值的占据栅格，标定为所述当前占据栅格地图上标定的障碍物。

作为一种可选的实施方式，判断是否存在新增激光点云数据，包括如下步骤：

确定所述激光点云数据在当前占据栅格地图上映射的占据位置；

根据所述占据位置与当前占据栅格地图上最近障碍物的距离，确定是否存在新增激光点云数据；

在其中一个距离大于预设阈值时，确定存在新增激光点云数据。

为方便计算激光点云的占据位置与当前占据栅格地图上最近障碍物的距离，本实施例中可以将占据栅格地图中空白栅格与最近障碍物之间的距离存储到队列文件，队列文件索引值可以是栅格坐标，其中，有障碍物的栅格存储值可设置为零。

本实施例中确定存在新增激光点云数据后，还可以将新增的激光点云数据进行存储，便于后续确定的新增激光点云数据和历史的新增激光点云数据进行匹配计算。

本实施例中的新增激光点云数据为由当前占据栅格地图上标定的障碍物之外的物体产生的激光点云数据，即为当前环境中新增物体所对应的激光点云数据，当前环境发生变化时，新增物体导致对应的激光点云数据与上一时刻的占据栅格地图上标定的障碍物不相符，其中所述物体(新增物体)可以是新增静态障碍物/移动的物体/行人。

本实施例还可以进一步确定所述新增激光点云数据是当前机器人所在的环境与上一次环境相比，新增静态障碍物产生的激光点云数据，还是移动的物体/行人产生的激光点云数据，判断步骤如下：

1)确定激光点云数据的占据位置与当前占据栅格地图上最近障碍物的距离；

2)该距离大于预设阈值时，确定该占据位置的激光点云数据为新增激光点云数据；

3)将2)中确定的新增激光点云数据和历史的新增激光点云数据，转换到占据栅格地图进行匹配计算；

4)确定是否多次在同一占据位置检测到上述确定的新增激光点云数据，如果是执行5)，否则执行6)；

具体的确定是否多次在同一占据位置检测到上述确定的新增激光点云数据，可通过下述公式判断，当d小于预设阈值时，确定多次在同一占据位置检测到上述确定的新增激光点云数据。

d＝||T_ip_im-T_kp_km||；

上述T_i、T_k表示i时刻、k时刻机器人的位姿；

p_im、p_km表示i时刻、k时刻激光雷达检测到的激光点云数据。

5)确定该新增激光点云数据为新增静态障碍物产生的激光点云数据。

6)确定该新增激光点云数据为移动的物体/行人产生的激光点云数据。

步骤302：利用有效激光点云数据基于当前占据栅格地图对当前位姿进行校正。

本实施例中利用有效激光点云数据基于当前占据栅格地图对当前位姿进行校正的过程中，还利用了去除多余的虚拟点云数据得到对应的虚拟点云数据，利用有效激光点云数据与对应的虚拟点云数据进行PL-ICP匹配，从而对当前位姿进行校正。

本实施例中的虚拟点云数据是在确定当前占据栅格地图之后生成的，基于当前位姿和当前占据栅格地图，确定当前占据栅格地图上标定障碍物在激光坐标系下对应的虚拟点云数据。

本实施例中在激光坐标系下对应的虚拟点云数据具体可通过如下公式获得：

上述公式(1)中，d_vi是虚拟点云数据，x_e、y_e、θ_e是机器人的当前位姿参数，x_oi、y_oi是当前占据栅格地图上障碍物的坐标值；

上述公式(2)中，x_vi、y_vi是激光坐标系下对应的虚拟点云数据；

上述公式(1)、公式(2)中的α_min是激光传感器的第一个光束的角度，Δ表示激光传感器的角分辨率，i表示激光传感器发出光束的数量或激光点云的数量，i为正整数。

本实施例还提供一种判断是否存在多余的虚拟点云数据的方法，该多余的虚拟点云数据表示当前时刻环境与上一时刻环境相比，环境中减少的障碍物对应生成的虚拟点云数据。

具体判断是否存在多余的虚拟点云数据的步骤如下：

a)将虚拟点云数据和获取的激光点云数据转换到世界坐标系下并比对；

由于上述确定的虚拟点云数据为激光坐标系下对应的虚拟点云数据，利用机器人当前位姿，将激光坐标系下对应的虚拟点云数据，以及激光点云数据都转换到世界坐标系下，坐标转换公式如下：

上述公式(3)中，x_e、y_e、θ_e是机器人的当前位姿参数，^rx_e、^ry_e是激光点云数据，^wx_e、^wy_e是激光点云数据在世界坐标系下的数据；

上述公式(4)中，x_e、y_e、θ_e是机器人的当前位姿参数，^rx_vi、^ry_vi是激光坐标系下对应的虚拟点云数据，^wx_vi、^wy_vi是虚拟点云数据在世界坐标系下的数据。

b)检测到虚拟点云数据存在多余的虚拟点云数据时，确定获取的激光点云数据缺少激光点云数据；

c)从所述虚拟点云数据中去除多余的虚拟点云数据得到对应的虚拟点云数据。

本实施例中检测到虚拟点云数据存在多余的虚拟点云数据时，确定获取的激光点云数据缺少激光点云数据，说明当前机器人所在的环境与上一次环境相比减少了静态障碍物/移动的物体/行人。从所述虚拟点云数据中去除多余的虚拟点云数据得到对应的虚拟点云数据，以使后续利用有效激光点云数据和虚拟点云数据对机器人位姿进行校正时，可以减少由于机器人所在环境中新增障碍物或新减少的障碍物对机器人位姿的影响，本实施例中校正后的机器人位姿精度更精确。

利用上述方法获取有效激光点云数据，以及对应的虚拟点云数据，对当前位姿进行校正，具体步骤如下：

1)利用有效激光点云及当前占据栅格地图得到第一估计位姿，利用对应的虚拟点云数据及当前占据栅格地图得到第二估计位姿；

其中，对应的虚拟点云数据为去除了多余虚拟点云数据后，得到的对应的虚拟点云数据。

2)将有效激光点云数据及对应的虚拟点云数据，转换到机器人坐标系下进行PL-ICP匹配，得到所述第一估计位姿与第二估计位姿的相对校正关系；

所述PL-ICP匹配方法是一种计算不同坐标系之间的相对位置关系的算法。

3)根据所述第一估计位姿与第二估计位姿的相对校正关系对当前位姿进行校正。

通过上述步骤确定的新增激光点云数据和/或多余的虚拟点云数据后，可以认为当前环境发生了变化，在满足地图更新条件时，本实施例中的占据栅格地图是可以不断更新的，由于虚拟点云数据是基于当前位姿和当前占据栅格地图获得的，因此利用有效激光点云数据及对应的虚拟点云数据进行位姿校正时，能够使得校正的位姿更加精准。

作为一种可选的实施方式，对所述占据栅格地图进行更新，包括如下任一或任多种方式：

方式1、确定多次在同一占据位置检测到新增激光点云数据时，对所述占据栅格地图进行更新；

该方式中设定的多次能够说明该新增激光点云数据对应的障碍物已经在当前环境中存在了一段时间，可以排除该新增激光点云数据是移动的物体/行人形成的，认为该新增激光点云数据是新增加静态障碍物，此时需要对占据栅格地图进行更新。即当机器人所在的当前环境中新增加静态障碍物时，确定满足地图更新条件，而由于移动的物体/行人导致的当前占据栅格地图与当前实际环境不一致时，不对占据栅格地图进行更新；

可选的，确定多次在同一占据位置检测到新增激光点云数据时，可以增加新增激光点云数据所在占据位置为障碍物的概率值。

方式2、确定多次在同一占据位置检测到多余虚拟点云数据时，确定占据栅格地图满足地图更新条件；

该方式中设定的多次能够说明该多余虚拟点云数据对应的障碍物已经在当前环境中消失了一段时间，可以排除该多余虚拟点云数据是移动的物体/行人形成的，认为该新增激光点云数据是新减少的静态障碍物，此时需要对占据栅格地图进行更新。即当机器人所在的当前环境中减少了静态障碍物时，确定满足地图更新条件，而由于移动的物体/行人导致的当前占据栅格地图与当前实际环境不一致时，不对占据栅格地图进行更新；

可选的，确定多次在同一占据位置检测到多余虚拟激光点云数据时，可以减少多余虚拟激光点云数据所在占据位置为障碍物的概率值。

本实施例中占据栅格地图中概率值更新方式可采用如下公式：

上述公式(5)中，p为占据栅格地图中的占据位置为障碍物的概率值；M_old(x)、M_new(x)分别是占据栅格地图中的同一个占据位置的更新前的旧概率值、更新后的新概率值。

实施例2

本实施例还提供利用反光板反射回来的激光点云数据求取位姿的方法，本实施例中的“反光板”，是一种具有激光强反射率的材质，当激光数据扫描到反光板时，传感器接收的点云有强反射率，能与周围的材质进行区分。

本实施中可遍历初始时刻获取的激光点云数据，当激光点云数据的反射率大于预设阈值时，将该激光点云数据作为反光板数据，表明该激光点云数据是从反光板反射回来的，可以判断连续设定次数的反光板数据的数量、反光板的长度以及当前反光板的曲率，用于过滤环境中的干扰点，如通过反光板的长度过滤环境中与所述反光板反射率一致但长度不一致的材料。

如图4所示，利用反光板反射回来的激光点云数据求取位姿的具体步骤如下：

步骤400：确定初始时刻获取激光点云数据时，基于同步定位与建图SLAM算法确定检测到反光板时机器人初始时刻在世界坐标系下的位置；

步骤401：根据从反光板反射回来的激光点云数据，确定所述反光板在机器人坐标系下的位置；

标定反光板在机器人坐标系下的位置的具体步骤如下：

1)设定初始时刻帧其中一块反光板的点集坐标为{p₁，p₂，…，p_n}，该点集的中心点坐标为(x_m、y_m)，该块反光板的实际长度为L，该块反光板上点的实际数量为N；

2)根据激光点云数据的反射率大于预设阈值时，检测到的反光板的长度l为：

上述公式(6)中，(p_1x、p_1y)为检测到该块反光板第一个点坐标，(p_nx、p_ny)为检测到该块反光板最后一个点坐标；

3)该块反光板上检测到的点p_i到该块反光板点集的中心点的距离l_i为：

4)该块反光板的曲率c为：

公式(8)中，p_ix、p_iy表示检测到的反光板上某点i的横、纵坐标，n表示连续检测到的反光板上的点的数量。

5)确定所述反光板在机器人坐标系下的位置，避免环境中的干扰点。

检测到的反光板长度l、反光板上点的数量n分别满足如下条件：

L>l>0.4×L；N>n>0.4×N；

且为了避免环境中的圆柱产生干扰点，要求检测到的反光板的曲率满足c<0.1。

步骤402：利用所述机器人在世界坐标系下的位置，初始标定所述反光板在世界坐标系下的位置。

具体的，将该块反光板数据利用机器人当前位姿转换到世界坐标系下：

公式(9)中，^wp_ix、^wp_iy为该块反光板在世界坐标系下的坐标，^rp_ix、^rp_iy为该块反光板在机器人坐标系下的坐标，θ、p_rx、p_ry为机器人在世界坐标系下的位姿，其中θ是机器人在世界坐标系下的朝向，p_rx、p_ry是机器人在世界坐标系下的位置。

本实施例中为了后续利用反光板反射回来的激光点云数据求取位姿，可以将检测到的反光板数据转换到世界坐标系下并存储到KDtree中，同时将该反光板数据写入反光板文件中，便于后续求取匹配的反光板数据，并利用最近邻搜索法，依次计算检测到的反光板质心与初始标定的反光板质心的距离。

本实施例中如果存储的反光板文件为空，则将该块反光板数据写入反光板文件中，如果存储的反光板文件不为空，还可以判断当前检测的反光板数据与历史的反光板数据是否重复，如果不重复就将该块反光板数据写入反光板文件中，同时计算出该块反光板的法向量坐标写入反光板文件中，便于后期利用发光板数据求取位姿。

其中，判断当前检测的反光板数据与历史的反光板数据是否重复的方法如下：

通过KDtree中的最近邻搜索得到与当前检测的反光板数据最接近的历史反光板数据，计算当前检测的反光板数据的质心和历史反光板数据的质心之间的距离是否小于预设值，如果是则确定该块反光板数据与历史的反光板数据重复。

步骤403：利用最近一次满足地图更新条件时获取的有效激光点云数据生成占据栅格地图之后，确定获取的激光点云数据的占据位置，与所述占据栅格地图上标定的障碍物不匹配的比例是否超过预设比例。

本实施例中所述预设比例可为70％。

步骤404：确定获取的激光点云数据的占据位置，与所述占据栅格地图上标定的障碍物不匹配的比例超过预设比例时，确定获取的激光点云数据中从反光板反射回来的激光点云数据。

步骤405：利用从反光板反射回来的激光点云数据求取位姿，采用扩展卡尔曼滤波算法EKF对最近一次确定的位姿进行校正。

作为一种可选的实施方式，利用从反光板反射回来的激光点云数据求取位姿，包括如下步骤：

1)将激光点云数据检测到的反光板位置转换到世界坐标系下，与初始标定反光板在世界坐标系下的位置进行匹配；

2)利用最近邻搜索法，依次计算检测到的反光板质心与初始标定的反光板质心的距离，确定最小距离的初始标定的反光板；

本实施例中利用检测到的所有反光板数据进行计算，首先从存储的初始标定的反光板数据的反光板文件中，读取反光板数据存储到KD树，然后将当前检测到的反光板数据转换到世界坐标系下，对当前帧检测到的每块反光板依次进行最近邻搜索，确定最小距离的初始标定的反光板。

3)根据激光点云数据检测到的反光板位置，以及最小距离的初始标定的反光板位置，利用LM算法求取位姿。

本实施例中可以检测到至少三块反光板数据，根据激光点云数据检测到的反光板位置，以及最小距离的初始标定的反光板位置，利用LM算法求取下述代价函数f1、f2的非线性最小二乘方程的最优解即求取位姿，具体公式如下：

其中，c_xj、c_yj为当前激光传感器检测到的某一块反光板的质心坐标，该质心坐标可由公式(12)表示：

(n_rx，n_ry)为初始标定的反光板法向量，^wp_ix、^wp_iy为该块反光板上每个点i在世界坐标系下的坐标；n为检测到的反光板上点的数量；

c_rx、c_ry为初始标定的反光板质心坐标，m为当前激光雷达检测到的反光板的块数。

本实施例中的EKF由预测方程和观测方程构成，将最近一次确定的位姿作为EKF中的预测方程的结果，将反光板数据求取的位姿作为EKF的观测方程对预测方程的结果进行纠正，从而解决机器人由最近一次确定的位姿切换到由反光板数据求取的位姿时，出现的抖动问题。

实施例3

本实施例将上述实施例1、实施例2的实施方式进行结合，一方面，利用有效激光点云数据、对应的虚拟点云数据进行位姿校正，另一方面，在满足地图更新条件时，可以对当前占据栅格地图进行更新，最后一方面，利用从反光板反射回来的激光点云数据求取位姿，并对最近一次确定的位姿进行纠正。

如图5所示，具体的实施流程如下：

步骤500：初始时刻获取激光点云数据，对反光板的位置进行标定，并基于初始位姿结合里程计信息估计当前位姿，同时生成占据栅格地图；

对反光板的位置进行标定：确定初始时刻获取激光点云数据时，基于SLAM算法确定检测到反光板时机器人初始时刻在世界坐标系下的位置；根据从反光板反射回来的激光点云数据，确定所述反光板在机器人坐标系下的位置；利用所述机器人在世界坐标系下的位置，初始标定所述反光板在世界坐标系下的位置。

步骤501：机器人移动过程中，间隔设定时间的获取激光点云数据，每次获取激光点云数据的同时，会判断里程计信息中的运动改变量是否大于阈值；

如果所述运动改变量大于阈值时，执行步骤502，否则执行步骤503。

步骤502：根据最近一次获取的激光点云数据，基于初始位姿分布的粒子群，重新对粒子群进行分布；

其中，对粒子群重新分布的过程是依据当前时刻获取的激光点云数据，调整粒子群的概率分布的过程，即调整机器人所有可能位姿的概率分布的过程。

同时，可以将重新分布的粒子群存储在KD树中便于后期搜索遍历。

步骤503：根据最近一次获取的激光点云数据，利用预设观测模型

调整粒子群中的粒子权重。

步骤504：确定粒子数量减少到阈值或者对粒子群重采样的间隔大于阈值时，对当前粒子群进行重采样。

其中，对粒子群进行重采样的过程相当于去除粒子权重低于预设值的粒子，复制粒子权重高于预设值的粒子，从而尽可能的得到真实状态的概率分布，尽可能的保证根据该粒子群中粒子权重估计得到的当前位姿接近真实状态。

对粒子群进行重采样的间隔时间可以是预设的，与获取激光点云数据的间隔时间可以是不同的间隔时间，重采样的过程和获取激光点云数据的过程可以并列执行。

步骤505：根据最近一次获取的激光点云数据，利用预设观测模型

再次重新调整重采样的粒子群中粒子权重的分配；

步骤506：根据重新调整后的粒子权重，对根据里程计信息估计的位姿加权得到当前位姿。

步骤507：基于当前位姿和当前占据栅格地图，确定当前占据栅格地图上标定障碍物在激光坐标系下对应的虚拟点云数据。

步骤508：判断是否存在新增激光点云数据，如果是执行步骤509，否则执行步骤510。

步骤509：去除新增激光点云数据得到有效激光点云数据，并存储该新增激光点云数据。

步骤510：判断是否存在多余的虚拟点云数据，如果是执行步骤511，否则执行步骤512。

步骤511：去除多余的虚拟点云数据得到对应的虚拟点云数据，并存储该多余的虚拟点云数据。

步骤512：利用有效激光点云数据与对应的虚拟点云数据进行PL-ICP匹配，从而对当前位姿进行校正。

步骤513：确定多次在同一占据位置检测到新增激光点云数据时，更新当前占据栅格地图。

步骤514：确定多次在同一占据位置检测到多余虚拟点云数据时，更新当前占据栅格地图。

步骤515：确定获取的激光点云数据的占据位置，与所述占据栅格地图上标定的障碍物不匹配的比例是否超过预设比例，如果是执行步骤516，否则执行步骤518。

步骤516：利用从反光板反射回来的激光点云数据求取位姿。

步骤517：采用EKF对最近一次确定的位姿进行校正。

步骤518：获取机器人最终位姿。

实施例4

基于相同的发明构思，本发明实施例提供了一种机器人定位设备，由于该设备即是本发明实施例中的方法中的设备，并且该设备解决问题的原理与该方法相似，因此该设备的实施可以参见方法的实施，重复之处不再赘述。

如图6所示，该设备包括：处理器600以及存储器601，其中，所述存储器601存储有程序代码，当所述程序代码被所述处理器600执行时，使得所述处理器600用于执行如下步骤：

获取激光点云数据，并结合里程计信息估计当前位姿；

作为一种可选的实施方式，所述处理器600具体用于：

根据所述占据位置与当前占据栅格地图上最近障碍物的距离，确定是否存在新增激光点云数据。

作为一种可选的实施方式，所述处理器600具体用于：

采用自适应蒙特卡洛定位AMCL算法，基于里程计信息以及获取的激光点云数据，根据前一时刻位姿估计当前时刻位姿。

作为一种可选的实施方式，所述处理器600具体还用于：

基于当前位姿和当前占据栅格地图，确定当前占据栅格地图上标定障碍物在激光坐标系下对应的虚拟点云数据。

作为一种可选的实施方式，所述处理器600具体还用于：

将虚拟点云数据和获取的激光点云数据转换到世界坐标系下并比对；

检测到虚拟点云数据存在多余的虚拟点云数据时，确定获取的激光点云数据缺少激光点云数据；

从所述虚拟点云数据中去除多余的虚拟点云数据得到对应的虚拟点云数据。

作为一种可选的实施方式，所述处理器600具体用于：

利用有效激光点云及当前占据栅格地图得到第一估计位姿，利用对应的虚拟点云数据及当前占据栅格地图得到第二估计位姿；

将有效激光点云数据及对应的虚拟点云数据，转换到机器人坐标系下进行PL-ICP匹配，得到所述第一估计位姿与第二估计位姿的相对校正关系；

根据所述第一估计位姿与第二估计位姿的相对校正关系对当前位姿进行校正。

作为一种可选的实施方式，所述处理器600具体还用于：

确定多次在同一占据位置检测到新增激光点云数据时，对所述占据栅格地图进行更新；或者

确定多次在同一占据位置检测到多余虚拟点云数据时，对所述占据栅格地图进行更新。

作为一种可选的实施方式，所述处理器600具体用于：

确定多次在同一占据位置检测到新增激光点云数据时，增加新增激光点云数据所在占据位置为障碍物的概率值；或者

确定多次在同一占据位置检测到多余虚拟激光点云数据时，减少多余虚拟激光点云数据所在占据位置为障碍物的概率值。

作为一种可选的实施方式，所述处理器600具体用于：

确定初始时刻获取激光点云数据时，基于同步定位与建图SLAM算法确定检测到反光板时机器人初始时刻在世界坐标系下的位置；

根据从反光板反射回来的激光点云数据，确定所述反光板在机器人坐标系下的位置；

利用所述机器人在世界坐标系下的位置，初始标定所述反光板在世界坐标系下的位置。

作为一种可选的实施方式，所述处理器600具体还用于：

确定获取的激光点云数据的占据位置，与所述占据栅格地图上标定的障碍物不匹配的比例超过预设比例时，确定获取的激光点云数据中从反光板反射回来的激光点云数据；

利用从反光板反射回来的激光点云数据求取位姿，采用扩展卡尔曼滤波算法EKF对最近一次确定的位姿进行校正。

作为一种可选的实施方式，所述处理器600具体用于：

将激光点云数据检测到的反光板位置转换到世界坐标系下，与初始标定反光板在世界坐标系下的位置进行匹配；

利用最近邻搜索法，依次计算检测到的反光板质心与初始标定的反光板质心的距离，确定最小距离的初始标定的反光板；

根据激光点云数据检测到的反光板位置，以及最小距离的初始标定的反光板位置，利用LM算法求取位姿。

作为一种可选的实施方式，所述处理器600具体还用于：

根据获取的激光点云数据，利用预设观测模型调整粒子群中的粒子权重；

根据调整后的粒子权重，对根据里程计信息估计的位姿加权得到当前位姿。

实施例5

基于相同的发明构思，本发明实施例提供了另一种机器人定位设备，由于该设备即是本发明实施例中的方法中的设备，并且该设备解决问题的原理与该方法相似，因此该设备的实施可以参见方法的实施，重复之处不再赘述。

如图7所示，该设备包括获取激光点云单元700、获取有效激光点云单元701、位姿校正单元702，其中：

获取激光点云单元700，用于获取激光点云数据，并结合里程计信息估计当前位姿；

获取有效激光点云单元701，用于根据获取的激光点云数据和当前占据栅格地图，确定存在新增激光点云数据时，从所述激光点云数据中去除新增激光点云数据得到有效激光点云数据，所述新增激光点云数据为由当前占据栅格地图上标定的障碍物之外的物体产生的激光点云数据，所述占据栅格地图用于标定当前环境对应的栅格位置是否有障碍物；

位姿校正单元702，用于利用有效激光点云数据基于当前占据栅格地图对当前位姿进行校正。

作为一种可选的实施方式，所述获取有效激光点云单元701具体用于：

作为一种可选的实施方式，所述获取激光点云单元700具体用于：

作为一种可选的实施方式，所述设备还包括确定虚拟点云单元具体用于：

作为一种可选的实施方式，所述设备还包括去除虚拟点云单元具体用于：

作为一种可选的实施方式，所述位姿校正单元702具体用于：

作为一种可选的实施方式，所述获取有效激光点云单元701具体还用于：

作为一种可选的实施方式，所述设备还包括标定反光板单元具体用于：

作为一种可选的实施方式，所述设备还包括反光板校正单元具体用于：

作为一种可选的实施方式，所述反光板校正单元具体用于：

作为一种可选的实施方式，所述获取激光点云单元700具体还用于：

实施例6

基于相同的发明构思，提供一种计算机存储介质，其上存储有计算机程序，该程序被处理器执行时实现如下步骤：

获取激光点云数据，并结合里程计信息估计当前位姿；

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的设备。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令设备的制造品，该指令设备实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims

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

获取激光点云数据，并结合里程计信息估计当前位姿；

利用有效激光点云数据基于当前占据栅格地图对当前位姿进行校正；

其中，确定存在新增激光点云数据，包括：

根据所述占据位置与当前占据栅格地图上最近障碍物的距离，在其中一个距离大于预设阈值时，确定存在新增激光点云数据；

确定多次在同一占据位置检测到新增激光点云数据时，确定占据栅格地图满足地图更新条件，对所述占据栅格地图进行更新。

2.根据权利要求1所述的方法，其特征在于，获取激光点云数据，并结合里程计信息估计当前位姿，包括：

采用自适应蒙特卡洛定位AMCL算法，基于里程计信息以及所述获取的激光点云数据，根据前一时刻位姿估计当前时刻位姿。

3.根据权利要求1所述的方法，其特征在于，确定所述激光点云数据在当前占据栅格地图上映射的占据位置之后，还包括：

4.根据权利要求3所述的方法，其特征在于，确定当前占据栅格地图上标定障碍物在激光坐标系下对应的虚拟点云数据之后，还包括：

5.根据权利要求3或4所述的方法，其特征在于，利用有效激光点云数据基于当前占据栅格地图对当前位姿进行校正，包括：

6.根据权利要求4所述的方法，其特征在于，还包括：

确定多次在同一占据位置检测到多余虚拟点云数据时，确定占据栅格地图满足地图更新条件，对所述占据栅格地图进行更新。

7.根据权利要求1所述的方法，其特征在于，利用获取的有效激光点云数据生成占据栅格地图，包括：

确定多次在同一占据位置检测到新增激光点云数据时，增加新增激光点云数据所在占据位置为障碍物的概率值。

8.根据权利要求6所述的方法，其特征在于，利用获取的有效激光点云数据生成占据栅格地图，包括：

9.根据权利要求1或4所述的方法，其特征在于，获取激光点云数据时，还包括：

10.根据权利要求9所述的方法，其特征在于，利用最近一次满足地图更新条件时获取的有效激光点云数据生成占据栅格地图之后，还包括：

11.根据权利要求10所述的方法，其特征在于，利用从反光板反射回来的激光点云数据求取位姿，包括：

12.根据权利要求1所述的方法，其特征在于，确定获取激光点云数据时，还包括：

13.一种机器人定位设备，其特征在于，该设备包括：处理器以及存储器，其中，所述存储器存储有程序代码，当所述程序代码被所述处理器执行时，使得所述处理器执行权利要求1～12任一所述方法的步骤。

14.一种计算机存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1～12任一所述方法的步骤。