CN111693053A - 一种基于移动机器人的重定位方法及系统 - Google Patents

Abstract

本发明公开了一种基于移动机器人的重定位方法及系统。该方法包括：获取移动机器人扫描环境中的物体得到的第一激光点数据集，确定第一激光点数据集中的第一激光点在全局坐标系中的位置；当移动机器人在全局坐标系中位姿发生变化后再次扫描环境中的物体得到第二激光点数据集，根据重定位位姿点确定第二激光点在全局坐标系中的位置；计算第二激光点在全局坐标系中的位置与第一激光点在全局坐标系中的位置的匹配度，并确定最大匹配度对应的重定位位姿点为移动机器人在全局栅格地图中重定位后的位姿。采用本发明的方法及系统，根据激光点数据与已构建的全局栅格地图进行匹配，能够提高移动机器人的重定位准确度和速度，便于后续导航和定位任务。

Description

一种基于移动机器人的重定位方法及系统

技术领域

本发明涉及重定位技术领域，特别是涉及一种基于移动机器人的重定位方法及系统。

背景技术

重定位技术是移动机器人实现自主导航和智能化环境扫描探索的一个重要的前提，移动机器人重定位就是当移动机器人在全局坐标系中的位姿发生了变化(如移动机器人打滑、被抱起)，装载在移动机器人上的激光传感器在全局坐标系中的位置则发生了变化，激光传感器发射激光束扫描周围环境得到的激光点数据与环境地图不匹配，则需要进行重定位。现有的移动机器人重定位方法大多是通过机器人的移动进行重定位，这会导致移动机器人定位精度不高，直接影响到机器人执行任务时的精度，并且重定位时的操作复杂度较大，计算时间较长。

发明内容

本发明的目的是提供一种基于移动机器人的重定位方法及系统，不需要机器人移动，而是根据激光点数据与已构建的全局栅格地图进行匹配，根据匹配结果确认移动机器人在真实环境中和构建的全局栅格地图中的对应关系，提高了移动机器人的重定位准确度和速度，便于后续导航和定位任务。

为实现上述目的，本发明提供了如下方案：

一种基于移动机器人的重定位方法，包括：

获取移动机器人扫描环境中的物体得到的第一激光点数据集；所述第一激光点数据集包括多个第一激光点；

根据所述第一激光点数据集构建环境的全局栅格地图，确定所述第一激光点在全局坐标系中的位置；所述全局坐标系为所述全局栅格地图的坐标系；

获取移动机器人在所述全局坐标系中位姿发生变化后再次扫描环境中的物体得到的第二激光点数据集；所述第二激光点数据集包括多个第二激光点；

根据移动机器人在环境中的位姿对应在所述全局坐标系中的位姿点选取多个移动机器人重定位位姿点；

分别根据每一个所述重定位位姿点确定所述第二激光点在所述全局坐标系中的位置；

计算所述第二激光点在所述全局坐标系中的位置与所述第一激光点在所述全局坐标系中的位置的匹配度，并确定最大匹配度对应的重定位位姿点为移动机器人在所述全局栅格地图中重定位后的位姿。

可选的，所述根据所述第一激光点数据集构建环境的全局栅格地图，确定所述第一激光点在全局坐标系中的位置，具体包括：

根据所述第一激光点数据集采用SLAM算法构建环境的全局栅格地图；

获取移动机器人在全局坐标系中的位姿；

根据所述位姿采用如下公式确定所述第一激光点在全局坐标系中的位置：

X_j＝X+d_j·cos(θ_j+θ)

Y_j＝Y+d_j·sin(θ_j+θ)

式中，(X_j，Y_j)为第一激光点在全局坐标系中的位置，移动机器人在全局坐标系中的位姿为(X，Y，θ)，(d_j，θ_j)为在激光传感器坐标系下的第一激光点；X_j为第一激光点在全局坐标系中的横坐标，Y_j为第一激光点在全局坐标系中的纵坐标，X为在全局坐标系中移动机器人位置点的横坐标，Y为在全局坐标系中移动机器人位置点的纵坐标，θ为在全局坐标系中移动机器人位置点与全局坐标系原点的连线与全局坐标系X轴所成角度，d_j为在激光传感器坐标系下的第一激光点与激光传感器坐标系原点的距离，θ_j为在激光传感器坐标系下的第一激光点与激光传感器坐标系原点连线和激光传感器坐标系X轴所成角度。

可选的，所述根据移动机器人在环境中的位姿对应在所述全局坐标系中的位姿点选取多个移动机器人重定位位姿点，具体包括：

以移动机器人在环境中的位姿对应在所述全局坐标系中的位姿点为中心选取N*N个移动机器人重定位位姿点；所述移动机器人重定位位姿点均在所述全局栅格地图内。

可选的，所述根据每一个所述重定位位姿点确定所述第二激光点在所述全局坐标系中的位置，具体包括：

根据如下公式确定所述第二激光点在所述全局坐标系中的位置：

X_newj＝X₁+d_newj·cos(θ_newj+θ₁)

Y_newj＝Y₁+d_newj·sin(θ_newj+θ₁)

式中，(X_newj，Y_newj)为第二激光点在全局坐标系中的位置，移动机器人重定位位姿点为(X₁，Y₁，θ₁)，(d_newj，θ_newj)为在激光传感器坐标系下的第二激光点；X_newj为第二激光点在全局坐标系中的横坐标，Y_newj为第二激光点在全局坐标系中的纵坐标，X₁为在全局坐标系中移动机器人重定位位姿点的横坐标，Y₁为在全局坐标系中移动机器人重定位位姿点的纵坐标，θ₁为在全局坐标系中移动机器人重定位位姿点与全局坐标系原点的连线与全局坐标系X轴所成角度，d_newj为在激光传感器坐标系下的第二激光点与激光传感器坐标系原点的距离，θ_newj为在激光传感器坐标系下的第二激光点与激光传感器坐标系原点连线和激光传感器坐标系X轴所成角度。

可选的，所述计算所述第二激光点在所述全局坐标系中的位置与所述第一激光点在所述全局坐标系中的位置的匹配度，具体包括：

以所述第一激光点在所述全局坐标系中的位置为中心选取K*K个位置形成的区域，得到高斯判断区域；所述高斯判断区域包括多个子区域，每一个所述子区域根据其与高斯判断区域中心距离的远近程度均设置有高斯模糊值；所述高斯判断区域中心为所述第一激光点在所述全局坐标系中的位置；

将所述第二激光点在所述全局坐标系中的位置映射到所述高斯判断区域中，根据所述第二激光点在所述全局坐标系中的位置所在的子区域确定与所述第二激光点在所述全局坐标系中的位置对应的高斯模糊值；

根据与所述第二激光点在所述全局坐标系中的位置对应的高斯模糊值采用如下公式计算匹配度：

式中，η为匹配度，a_i为第i个第二激光点在所述全局坐标系中的位置对应的高斯模糊值，n为高斯模糊值限值。

本发明还提供一种基于移动机器人的重定位系统，包括：

第一激光点数据集获取模块，用于获取移动机器人扫描环境中的物体得到的第一激光点数据集；所述第一激光点数据集包括多个第一激光点；

第一激光点在全局坐标系中的位置确定模块，用于根据所述第一激光点数据集构建环境的全局栅格地图，确定所述第一激光点在全局坐标系中的位置；所述全局坐标系为所述全局栅格地图的坐标系；

第二激光点数据集获取模块，用于获取移动机器人在所述全局坐标系中位姿发生变化后再次扫描环境中的物体得到的第二激光点数据集；所述第二激光点数据集包括多个第二激光点；

移动机器人重定位位姿点选取模块，用于根据移动机器人在环境中的位姿对应在所述全局坐标系中的位姿点选取多个移动机器人重定位位姿点；

第二激光点在全局坐标系中的位置确定模块，用于分别根据每一个所述重定位位姿点确定所述第二激光点在所述全局坐标系中的位置；

重定位模块，用于计算所述第二激光点在所述全局坐标系中的位置与所述第一激光点在所述全局坐标系中的位置的匹配度，并确定最大匹配度对应的重定位位姿点为移动机器人在所述全局栅格地图中重定位后的位姿。

可选的，所述第一激光点在全局坐标系中的位置确定模块，具体包括：

全局栅格地图构建单元，用于根据所述第一激光点数据集采用SLAM算法构建环境的全局栅格地图；

位姿获取单元，用于获取移动机器人在全局坐标系中的位姿；

第一激光点在全局坐标系中的位置确定单元，用于根据所述位姿采用如下公式确定所述第一激光点在全局坐标系中的位置：

X_j＝X+d_j·cos(θ_j+θ)

Y_j＝Y+d_j·sin(θ_j+θ)

式中，(X_j，Y_j)为第一激光点在全局坐标系中的位置，移动机器人在全局坐标系中的位姿为(X，Y，θ)，(d_j，θ_j)为在激光传感器坐标系下的第一激光点；X_j为第一激光点在全局坐标系中的横坐标，Y_j为第一激光点在全局坐标系中的纵坐标，X为在全局坐标系中移动机器人位置点的横坐标，Y为在全局坐标系中移动机器人位置点的纵坐标，θ为在全局坐标系中移动机器人位置点与全局坐标系原点的连线与全局坐标系X轴所成角度，d_j为在激光传感器坐标系下的第一激光点与激光传感器坐标系原点的距离，θ_j为在激光传感器坐标系下的第一激光点与激光传感器坐标系原点连线和激光传感器坐标系X轴所成角度。

可选的，所述移动机器人重定位位姿点选取模块，具体包括：

移动机器人重定位位姿点选取单元，用于以移动机器人在环境中的位姿对应在所述全局坐标系中的位姿点为中心选取N*N个移动机器人重定位位姿点；所述移动机器人重定位位姿点均在所述全局栅格地图内。

可选的，所述第二激光点在全局坐标系中的位置确定模块，具体包括：

第二激光点在全局坐标系中的位置确定单元，用于根据如下公式确定所述第二激光点在所述全局坐标系中的位置：

X_newj＝X₁+d_newj·cos(θ_newj+θ₁)

Y_newj＝Y₁+d_newj·sin(θ_newj+θ₁)

式中，(X_newj，Y_newj)为第二激光点在全局坐标系中的位置，移动机器人重定位位姿点为(X₁，Y₁，θ₁)，(d_newj，θ_newj)为在激光传感器坐标系下的第二激光点；X_newj为第二激光点在全局坐标系中的横坐标，Y_newj为第二激光点在全局坐标系中的纵坐标，X₁为在全局坐标系中移动机器人重定位位姿点的横坐标，Y₁为在全局坐标系中移动机器人重定位位姿点的纵坐标，θ₁为在全局坐标系中移动机器人重定位位姿点与全局坐标系原点的连线与全局坐标系X轴所成角度，d_newj为在激光传感器坐标系下的第二激光点与激光传感器坐标系原点的距离，θ_newj为在激光传感器坐标系下的第二激光点与激光传感器坐标系原点连线和激光传感器坐标系X轴所成角度。

可选的，所述重定位模块，具体包括：

高斯判断区域确定单元，用于以所述第一激光点在所述全局坐标系中的位置为中心选取K*K个位置形成的区域，得到高斯判断区域；所述高斯判断区域包括多个子区域，每一个所述子区域根据其与高斯判断区域中心距离的远近程度均设置有高斯模糊值；所述高斯判断区域中心为所述第一激光点在所述全局坐标系中的位置；

高斯模糊值确定单元，用于将所述第二激光点在所述全局坐标系中的位置映射到所述高斯判断区域中，根据所述第二激光点在所述全局坐标系中的位置所在的子区域确定与所述第二激光点在所述全局坐标系中的位置对应的高斯模糊值；

匹配度计算单元，用于根据与所述第二激光点在所述全局坐标系中的位置对应的高斯模糊值采用如下公式计算匹配度：

式中，η为匹配度，a_i为第i个第二激光点在所述全局坐标系中的位置对应的高斯模糊值，n为高斯模糊值限值。

与现有技术相比，本发明的有益效果是：

本发明提出了一种基于移动机器人的重定位方法及系统，获取移动机器人扫描环境中的物体得到的第一激光点数据集，根据第一激光点数据集构建环境的全局栅格地图，确定第一激光点数据集中的第一激光点在全局坐标系中的位置；当移动机器人在全局坐标系中位姿发生变化后再次扫描环境中的物体得到第二激光点数据集，根据移动机器人在环境中的位姿对应在全局坐标系中的位姿点选取多个移动机器人重定位位姿点，分别根据每一个重定位位姿点确定第二激光点在全局坐标系中的位置；计算第二激光点在全局坐标系中的位置与第一激光点在全局坐标系中的位置的匹配度，并确定最大匹配度对应的重定位位姿点为移动机器人在全局栅格地图中重定位后的位姿。本发明不需要机器人移动，而是根据激光点数据与已构建的全局栅格地图进行匹配，根据匹配结果确认移动机器人在真实环境中和构建的全局栅格地图中的对应关系，提高了移动机器人的重定位准确度和速度，便于后续导航和定位任务。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例中基于移动机器人的重定位方法流程图；

图2为本发明实施例中激光传感器测量数据示意图；

图3为本发明实施例中选取的重定位位姿点示意图；

图4为本发明实施例中高斯判断区域示意图；

图5为本发明实施例中基于移动机器人的重定位系统结构图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种基于移动机器人的重定位方法及系统，不需要机器人移动，而是根据激光点数据与已构建的全局栅格地图进行匹配，根据匹配结果确认移动机器人在真实环境中和构建的全局栅格地图中的对应关系，提高了移动机器人的重定位准确度和速度，便于后续导航和定位任务。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例

图1为本发明实施例中基于移动机器人的重定位方法流程图，如图1所示，本发明提供的一种基于移动机器人的重定位方法，包括：

步骤101：获取移动机器人扫描环境中的物体得到的第一激光点数据集；第一激光点数据集包括多个第一激光点。

图2为本发明实施例中激光传感器测量数据示意图，图2中激光传感器坐标系横轴为X1，纵轴为Y1，如图2所示，移动机器人在环境中移动，通过装载在移动机器人上的激光传感器发射激光束实时扫描周围环境，在激光传感器坐标系下测量环境中的各物体得到第一激光点数据集，激光传感器坐标系原点O1在激光传感器物理中心，第一激光点数据集包括m个第一激光点数据，第一激光点数据集为：{d_i，θ_i}，i＝1…m。

步骤102：根据第一激光点数据集构建环境的全局栅格地图，确定第一激光点在全局坐标系中的位置；全局坐标系为全局栅格地图的坐标系。

步骤102，具体包括：

根据第一激光点数据集采用SLAM算法构建环境的全局栅格地图。即根据第一激光点数据集运用SLAM算法构建环境的全局栅格地图，定义栅格地图的全局坐标系为XOY，原点O为任意指定，构建的全局栅格地图是将第一激光点数据映射到全局坐标系为XOY中，即计算出第一激光点数据在全局坐标系XOY的位置并用栅格标识出来。

获取移动机器人在全局坐标系中的位姿；移动机器人在全局坐标系XOY中的位姿为(X，Y，θ)。

根据位姿采用如下公式确定第一激光点在全局坐标系中的位置：

X_j＝X+d_j·cos(θ_j+θ)

Y_j＝Y+d_j·sin(θ_j+θ)

式中，(X_j，Y_j)为第一激光点在全局坐标系中的位置，移动机器人在全局坐标系中的位姿为(X，Y，θ)，(d_j，θ_j)为在激光传感器坐标系下的第一激光点；X_j为第一激光点在全局坐标系中的横坐标，Y_j为第一激光点在全局坐标系中的纵坐标，X为在全局坐标系中移动机器人位置点的横坐标，Y为在全局坐标系中移动机器人位置点的纵坐标，θ为在全局坐标系中移动机器人位置点与全局坐标系原点的连线与全局坐标系X轴所成角度，d_j为在激光传感器坐标系下的第一激光点与激光传感器坐标系原点的距离，θ_j为在激光传感器坐标系下的第一激光点与激光传感器坐标系原点连线和激光传感器坐标系X轴所成角度。

步骤103：获取移动机器人在全局坐标系中位姿发生变化后再次扫描环境中的物体得到的第二激光点数据集；第二激光点数据集包括多个第二激光点。

当移动机器人被移动了位置或被抬走离开环境之后，移动机器人下一次进入所扫描的环境中时，装载在移动机器人上的激光传感器会再一次扫描环境中的物体，得到第二激光点数据集，第二激光点数据集同样包括m个第二激光点数据，第二激光点数据集为：

{d_newi，θ_newi}，i＝1...m

每个第二激光点数据与第一激光点数据一一对应，即第i个激光点数据有：

d_i＝d_newi

θ_i＝θ_newi

步骤104：根据移动机器人在环境中的位姿对应在全局坐标系中的位姿点选取多个移动机器人重定位位姿点。

步骤104，具体包括：

以移动机器人在环境中的位姿对应在全局坐标系中的位姿点为中心选取N*N个移动机器人重定位位姿点；移动机器人重定位位姿点均在全局栅格地图内。

即：在全局坐标系XOY中指定一个初始位姿点，初始位姿点是人为根据移动机器人在实际环境中的位姿对应在全局坐标系XOY中取的初始值，在全局坐标系XOY中以初始位姿点为中心取N×N个重定位位姿点，这些重定位位姿点以矩阵的形式排列，矩阵的中心位置即为初始位姿点，选取的重定位位姿点参见图3，形成重定位迭代判断区域，N一般取奇数，重定位位姿点不应该超过所述构建的环境栅格地图。

步骤105：分别根据每一个重定位位姿点确定第二激光点在全局坐标系中的位置。

步骤105，具体包括：

根据如下公式确定第二激光点在全局坐标系中的位置：

X_newj＝X₁+d_newj·cos(θ_newj+θ₁)

Y_newj＝Y₁+d_newj·sin(θ_newj+θ₁)

式中，(X_newj，Y_newj)为第二激光点在全局坐标系中的位置，移动机器人重定位位姿点为(X₁，Y₁，θ₁)，(d_newj，θ_newj)为在激光传感器坐标系下的第二激光点；X_newj为第二激光点在全局坐标系中的横坐标，Y_newj为第二激光点在全局坐标系中的纵坐标，X₁为在全局坐标系中移动机器人重定位位姿点的横坐标，Y₁为在全局坐标系中移动机器人重定位位姿点的纵坐标，θ₁为在全局坐标系中移动机器人重定位位姿点与全局坐标系原点的连线与全局坐标系X轴所成角度，d_newj为在激光传感器坐标系下的第二激光点与激光传感器坐标系原点的距离，θ_newj为在激光传感器坐标系下的第二激光点与激光传感器坐标系原点连线和激光传感器坐标系X轴所成角度。

步骤106：计算第二激光点在全局坐标系中的位置与第一激光点在全局坐标系中的位置的匹配度，并确定最大匹配度对应的重定位位姿点为移动机器人在全局栅格地图中重定位后的位姿。

步骤106，具体包括：

以第一激光点在全局坐标系中的位置为中心选取K*K个位置形成的区域，得到高斯判断区域，K一般取奇数；高斯判断区域包括多个子区域，每一个子区域根据其与高斯判断区域中心距离的远近程度均设置有高斯模糊值；高斯判断区域中心为第一激光点在全局坐标系中的位置；

将第二激光点在全局坐标系中的位置映射到高斯判断区域中，根据第二激光点在全局坐标系中的位置所在的子区域确定与第二激光点在全局坐标系中的位置对应的高斯模糊值；

根据与第二激光点在全局坐标系中的位置对应的高斯模糊值采用如下公式计算匹配度：

式中，η为匹配度，a_i为第i个第二激光点在全局坐标系中的位置对应的高斯模糊值，n为高斯模糊值限值，n一般取100。

具体的，分别以N×N个重定位位姿点计算第二激光点数据集与构建的环境栅格地图的匹配度，以匹配度η表示。匹配度η越高，表示重定位位姿越接近移动机器人在环境中的真实位姿。图4为本发明实施例中高斯判断区域示意图，如图4所示，将高斯判断区域分为若干部分，距离构建的环境栅格地图中对应的激光点的位置越近，其高斯模糊值越大(分值为70、80、90或者其他分数)。因此，当每个激光点数据以当前的重定位位姿点计算得到在全局坐标系XOY中的位置后，映射到高斯判断区域后就得到当前的高斯模糊值。

每个重定位位姿点计算完之后都可以得到一个置信度(匹配度)，重定位迭代判断区域中重定位位姿点计算完之后得到N×N个匹配度；取N×N个匹配度中的最大值：

将η_max所对应的重定位位姿点(X_{rl_suit}，Y_{rl_suit}，θ_{rl_suit})作为当前移动机器人在所述构建的环境栅格地图中的位姿，此位姿与移动机器人在环境中的真实位姿对应，移动机器人在真实环境中的移动与其在所构建的栅格地图中的移动对应起来。

本发明提供的基于移动机器人的重定位方法，移动机器人通过激光传感器扫描周围环境地图，运用SLAM算法构建周围环境的全局栅格地图，若移动机器人遇到打滑、被抱起等情况，造成移动机器人局部栅格地图与全局栅格地图不匹配，则需要对移动机器人进行重新定位，比对移动机器人当前帧局部栅格地图与已经构建好的全局栅格地图，指定移动机器人在栅格地图中的估计位姿。然后将激光当前帧局部栅格地图与全局栅格地图进行匹配，匹配过程中分为两种情况，其一是移动机器人相对局部地图的位置是固定的，根据匹配结果此时移动机器人在全局栅格地图中显示的位姿就是移动机器人重定位后的位姿；其二是移动机器人原地旋转，通过旋转过程激光传感器扫描周围环境得到的局部栅格地图与已构建好的全局栅格地图进行匹配，根据匹配结果此时移动机器人在全局栅格地图中显示的位姿就是移动机器人重定位后的位姿。若在已经构建好的地图中估计移动机器人的位姿，如果预估的机器人的位姿和全局栅格地图中移动机器人准确的位姿误差过大，则不能通过匹配得到匹配后移动机器人在全局栅格地图中的位姿。如果移动机器人位置定位失败，需要重新判断移动机器人在全局栅格地图中的位姿或位置，重新进行重定位。

本发明为后续移动机器人进行路径规划，准确的执行任务提供基础，可以有效快速的实现重定位，同时此重定位方法操作简洁，鲁棒性好。

图5为本发明实施例中基于移动机器人的重定位系统结构图。如图5所示，一种基于移动机器人的重定位系统，包括：

第一激光点数据集获取模块201，用于获取移动机器人扫描环境中的物体得到的第一激光点数据集；第一激光点数据集包括多个第一激光点。

第一激光点在全局坐标系中的位置确定模块202，用于根据第一激光点数据集构建环境的全局栅格地图，确定第一激光点在全局坐标系中的位置；全局坐标系为全局栅格地图的坐标系。

第一激光点在全局坐标系中的位置确定模块202，具体包括：

全局栅格地图构建单元，用于根据第一激光点数据集采用SLAM算法构建环境的全局栅格地图；

位姿获取单元，用于获取移动机器人在全局坐标系中的位姿；

第一激光点在全局坐标系中的位置确定单元，用于根据位姿采用如下公式确定第一激光点在全局坐标系中的位置：

X_j＝X+d_j·cos(θ_j+θ)

Y_j＝Y+d_j·sin(θ_j+θ)

式中，(X_j，Y_j)为第一激光点在全局坐标系中的位置，移动机器人在全局坐标系中的位姿为(X，Y，θ)，(d_j，θ_j)为在激光传感器坐标系下的第一激光点；X_j为第一激光点在全局坐标系中的横坐标，Y_j为第一激光点在全局坐标系中的纵坐标，X为在全局坐标系中移动机器人位置点的横坐标，Y为在全局坐标系中移动机器人位置点的纵坐标，θ为在全局坐标系中移动机器人位置点与全局坐标系原点的连线与全局坐标系X轴所成角度，d_j为在激光传感器坐标系下的第一激光点与激光传感器坐标系原点的距离，θ_j为在激光传感器坐标系下的第一激光点与激光传感器坐标系原点连线和激光传感器坐标系X轴所成角度。

第二激光点数据集获取模块203，用于获取移动机器人在全局坐标系中位姿发生变化后再次扫描环境中的物体得到的第二激光点数据集；第二激光点数据集包括多个第二激光点。

移动机器人重定位位姿点选取模块204，用于根据移动机器人在环境中的位姿对应在全局坐标系中的位姿点选取多个移动机器人重定位位姿点。

移动机器人重定位位姿点选取模块204，具体包括：

移动机器人重定位位姿点选取单元，用于以移动机器人在环境中的位姿对应在全局坐标系中的位姿点为中心选取N*N个移动机器人重定位位姿点；移动机器人重定位位姿点均在全局栅格地图内。

第二激光点在全局坐标系中的位置确定模块205，用于分别根据每一个重定位位姿点确定第二激光点在全局坐标系中的位置。

第二激光点在全局坐标系中的位置确定模块205，具体包括：

第二激光点在全局坐标系中的位置确定单元，用于根据如下公式确定第二激光点在全局坐标系中的位置：

X_newj＝X₁+d_newj·cos(θ_newj+θ₁)

Y_newj＝Y₁+d_newj·sin(θ_newj+θ₁)

式中，(X_newj，Y_newj)为第二激光点在全局坐标系中的位置，移动机器人重定位位姿点为(X₁，Y₁，θ₁)，(d_newj，θ_newj)为在激光传感器坐标系下的第二激光点；X_newj为第二激光点在全局坐标系中的横坐标，Y_newj为第二激光点在全局坐标系中的纵坐标，X₁为在全局坐标系中移动机器人重定位位姿点的横坐标，Y₁为在全局坐标系中移动机器人重定位位姿点的纵坐标，θ₁为在全局坐标系中移动机器人重定位位姿点与全局坐标系原点的连线与全局坐标系X轴所成角度，d_newj为在激光传感器坐标系下的第二激光点与激光传感器坐标系原点的距离，θ_newj为在激光传感器坐标系下的第二激光点与激光传感器坐标系原点连线和激光传感器坐标系X轴所成角度。

重定位模块206，用于计算第二激光点在全局坐标系中的位置与第一激光点在全局坐标系中的位置的匹配度，并确定最大匹配度对应的重定位位姿点为移动机器人在全局栅格地图中重定位后的位姿。

重定位模块206，具体包括：

高斯判断区域确定单元，用于以第一激光点在全局坐标系中的位置为中心选取K*K个位置形成的区域，得到高斯判断区域；高斯判断区域包括多个子区域，每一个子区域根据其与高斯判断区域中心距离的远近程度均设置有高斯模糊值；高斯判断区域中心为第一激光点在全局坐标系中的位置；

高斯模糊值确定单元，用于将第二激光点在全局坐标系中的位置映射到高斯判断区域中，根据第二激光点在全局坐标系中的位置所在的子区域确定与第二激光点在全局坐标系中的位置对应的高斯模糊值；

匹配度计算单元，用于根据与第二激光点在全局坐标系中的位置对应的高斯模糊值采用如下公式计算匹配度：

式中，η为匹配度，a_i为第i个第二激光点在全局坐标系中的位置对应的高斯模糊值，n为高斯模糊值限值。

对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上，本说明书内容不应理解为对本发明的限制。

Claims (10)

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

获取移动机器人扫描环境中的物体得到的第一激光点数据集；所述第一激光点数据集包括多个第一激光点；

根据所述第一激光点数据集构建环境的全局栅格地图，确定所述第一激光点在全局坐标系中的位置；所述全局坐标系为所述全局栅格地图的坐标系；

获取移动机器人在所述全局坐标系中位姿发生变化后再次扫描环境中的物体得到的第二激光点数据集；所述第二激光点数据集包括多个第二激光点；

根据移动机器人在环境中的位姿对应在所述全局坐标系中的位姿点选取多个移动机器人重定位位姿点；

分别根据每一个所述重定位位姿点确定所述第二激光点在所述全局坐标系中的位置；

计算所述第二激光点在所述全局坐标系中的位置与所述第一激光点在所述全局坐标系中的位置的匹配度，并确定最大匹配度对应的重定位位姿点为移动机器人在所述全局栅格地图中重定位后的位姿。

2.根据权利要求1所述的基于移动机器人的重定位方法，其特征在于，所述根据所述第一激光点数据集构建环境的全局栅格地图，确定所述第一激光点在全局坐标系中的位置，具体包括：

根据所述第一激光点数据集采用SLAM算法构建环境的全局栅格地图；

获取移动机器人在全局坐标系中的位姿；

根据所述位姿采用如下公式确定所述第一激光点在全局坐标系中的位置：

X_j＝X+d_j·cos(θ_j+θ)

Y_j＝Y+d_j·sin(θ_j+θ)

式中，(X_j，Y_j)为第一激光点在全局坐标系中的位置，移动机器人在全局坐标系中的位姿为(X，Y，θ)，(d_j，θ_j)为在激光传感器坐标系下的第一激光点；X_j为第一激光点在全局坐标系中的横坐标，Y_j为第一激光点在全局坐标系中的纵坐标，X为在全局坐标系中移动机器人位置点的横坐标，Y为在全局坐标系中移动机器人位置点的纵坐标，θ为在全局坐标系中移动机器人位置点与全局坐标系原点的连线与全局坐标系X轴所成角度，dj为在激光传感器坐标系下的第一激光点与激光传感器坐标系原点的距离，θ_j为在激光传感器坐标系下的第一激光点与激光传感器坐标系原点连线和激光传感器坐标系X轴所成角度。

3.根据权利要求2所述的基于移动机器人的重定位方法，其特征在于，所述根据移动机器人在环境中的位姿对应在所述全局坐标系中的位姿点选取多个移动机器人重定位位姿点，具体包括：

以移动机器人在环境中的位姿对应在所述全局坐标系中的位姿点为中心选取N*N个移动机器人重定位位姿点；所述移动机器人重定位位姿点均在所述全局栅格地图内。

4.根据权利要求3所述的基于移动机器人的重定位方法，其特征在于，所述根据每一个所述重定位位姿点确定所述第二激光点在所述全局坐标系中的位置，具体包括：

根据如下公式确定所述第二激光点在所述全局坐标系中的位置：

X_newj＝X₁+d_newj·cos(θ_newj+θ₁)

Y_newj＝Y₁+d_newj·sin(θ_newj+θ₁)

式中，(X_newj，Y_newj)为第二激光点在全局坐标系中的位置，移动机器人重定位位姿点为(X₁，Y₁，θ₁)，(d_newj，θ_newj)为在激光传感器坐标系下的第二激光点；X_newj为第二激光点在全局坐标系中的横坐标，Y_newj为第二激光点在全局坐标系中的纵坐标，X₁为在全局坐标系中移动机器人重定位位姿点的横坐标，Y₁为在全局坐标系中移动机器人重定位位姿点的纵坐标，θ₁为在全局坐标系中移动机器人重定位位姿点与全局坐标系原点的连线与全局坐标系X轴所成角度，d_newj为在激光传感器坐标系下的第二激光点与激光传感器坐标系原点的距离，θ_newj为在激光传感器坐标系下的第二激光点与激光传感器坐标系原点连线和激光传感器坐标系X轴所成角度。

5.根据权利要求4所述的基于移动机器人的重定位方法，其特征在于，所述计算所述第二激光点在所述全局坐标系中的位置与所述第一激光点在所述全局坐标系中的位置的匹配度，具体包括：

以所述第一激光点在所述全局坐标系中的位置为中心选取K*K个位置形成的区域，得到高斯判断区域；所述高斯判断区域包括多个子区域，每一个所述子区域根据其与高斯判断区域中心距离的远近程度均设置有高斯模糊值；所述高斯判断区域中心为所述第一激光点在所述全局坐标系中的位置；

将所述第二激光点在所述全局坐标系中的位置映射到所述高斯判断区域中，根据所述第二激光点在所述全局坐标系中的位置所在的子区域确定与所述第二激光点在所述全局坐标系中的位置对应的高斯模糊值；

根据与所述第二激光点在所述全局坐标系中的位置对应的高斯模糊值采用如下公式计算匹配度：

式中，η为匹配度，a_i为第i个第二激光点在所述全局坐标系中的位置对应的高斯模糊值，n为高斯模糊值限值。

6.一种基于移动机器人的重定位系统，其特征在于，包括：

第一激光点数据集获取模块，用于获取移动机器人扫描环境中的物体得到的第一激光点数据集；所述第一激光点数据集包括多个第一激光点；

第一激光点在全局坐标系中的位置确定模块，用于根据所述第一激光点数据集构建环境的全局栅格地图，确定所述第一激光点在全局坐标系中的位置；所述全局坐标系为所述全局栅格地图的坐标系；

第二激光点数据集获取模块，用于获取移动机器人在所述全局坐标系中位姿发生变化后再次扫描环境中的物体得到的第二激光点数据集；所述第二激光点数据集包括多个第二激光点；

移动机器人重定位位姿点选取模块，用于根据移动机器人在环境中的位姿对应在所述全局坐标系中的位姿点选取多个移动机器人重定位位姿点；

第二激光点在全局坐标系中的位置确定模块，用于分别根据每一个所述重定位位姿点确定所述第二激光点在所述全局坐标系中的位置；

重定位模块，用于计算所述第二激光点在所述全局坐标系中的位置与所述第一激光点在所述全局坐标系中的位置的匹配度，并确定最大匹配度对应的重定位位姿点为移动机器人在所述全局栅格地图中重定位后的位姿。

7.根据权利要求6所述的基于移动机器人的重定位系统，其特征在于，所述第一激光点在全局坐标系中的位置确定模块，具体包括：

全局栅格地图构建单元，用于根据所述第一激光点数据集采用SLAM算法构建环境的全局栅格地图；

位姿获取单元，用于获取移动机器人在全局坐标系中的位姿；

第一激光点在全局坐标系中的位置确定单元，用于根据所述位姿采用如下公式确定所述第一激光点在全局坐标系中的位置：

X_j＝X+d_j·cos(θ_j+θ)

Y_j＝Y+d_j·sin(θ_j+θ)

式中，(X_j，Y_j)为第一激光点在全局坐标系中的位置，移动机器人在全局坐标系中的位姿为(X，Y，θ)，(d_j，θ_j)为在激光传感器坐标系下的第一激光点；X_j为第一激光点在全局坐标系中的横坐标，Y_j为第一激光点在全局坐标系中的纵坐标，X为在全局坐标系中移动机器人位置点的横坐标，Y为在全局坐标系中移动机器人位置点的纵坐标，θ为在全局坐标系中移动机器人位置点与全局坐标系原点的连线与全局坐标系X轴所成角度，d_j为在激光传感器坐标系下的第一激光点与激光传感器坐标系原点的距离，θ_j为在激光传感器坐标系下的第一激光点与激光传感器坐标系原点连线和激光传感器坐标系X轴所成角度。

8.根据权利要求7所述的基于移动机器人的重定位系统，其特征在于，所述移动机器人重定位位姿点选取模块，具体包括：

移动机器人重定位位姿点选取单元，用于以移动机器人在环境中的位姿对应在所述全局坐标系中的位姿点为中心选取N*N个移动机器人重定位位姿点；所述移动机器人重定位位姿点均在所述全局栅格地图内。

9.根据权利要求8所述的基于移动机器人的重定位系统，其特征在于，所述第二激光点在全局坐标系中的位置确定模块，具体包括：

第二激光点在全局坐标系中的位置确定单元，用于根据如下公式确定所述第二激光点在所述全局坐标系中的位置：

X_newj＝X₁+d_newj·cos(θ_newj+θ₁)

Y_newj＝Y₁+d_newj·sin(θ_newj+θ₁)

式中，(X_newj，Y_newj)为第二激光点在全局坐标系中的位置，移动机器人重定位位姿点为(X₁，Y₁，θ₁)，(d_newj，θ_newj)为在激光传感器坐标系下的第二激光点；X_newj为第二激光点在全局坐标系中的横坐标，Y_newj为第二激光点在全局坐标系中的纵坐标，X₁为在全局坐标系中移动机器人重定位位姿点的横坐标，Y₁为在全局坐标系中移动机器人重定位位姿点的纵坐标，θ₁为在全局坐标系中移动机器人重定位位姿点与全局坐标系原点的连线与全局坐标系X轴所成角度，d_newj为在激光传感器坐标系下的第二激光点与激光传感器坐标系原点的距离，θ_newj为在激光传感器坐标系下的第二激光点与激光传感器坐标系原点连线和激光传感器坐标系X轴所成角度。

10.根据权利要求9所述的基于移动机器人的重定位系统，其特征在于，所述重定位模块，具体包括：

高斯判断区域确定单元，用于以所述第一激光点在所述全局坐标系中的位置为中心选取K*K个位置形成的区域，得到高斯判断区域；所述高斯判断区域包括多个子区域，每一个所述子区域根据其与高斯判断区域中心距离的远近程度均设置有高斯模糊值；所述高斯判断区域中心为所述第一激光点在所述全局坐标系中的位置；

高斯模糊值确定单元，用于将所述第二激光点在所述全局坐标系中的位置映射到所述高斯判断区域中，根据所述第二激光点在所述全局坐标系中的位置所在的子区域确定与所述第二激光点在所述全局坐标系中的位置对应的高斯模糊值；

匹配度计算单元，用于根据与所述第二激光点在所述全局坐标系中的位置对应的高斯模糊值采用如下公式计算匹配度：

式中，η为匹配度，a_i为第i个第二激光点在所述全局坐标系中的位置对应的高斯模糊值，n为高斯模糊值限值。

Images