CN113376650A

CN113376650A - 移动机器人定位方法及装置、电子设备及存储介质

Info

Publication number: CN113376650A
Application number: CN202110905399.8A
Authority: CN
Inventors: 胡立志; 卢维; 王政; 李铭
Original assignee: Zhejiang Huaray Technology Co Ltd
Current assignee: Zhejiang Huaray Technology Co Ltd
Priority date: 2021-08-09
Filing date: 2021-08-09
Publication date: 2021-09-10
Anticipated expiration: 2041-08-09
Also published as: CN113376650B

Abstract

本发明提供了一种移动机器人定位方法及装置、电子设备、存储介质，所述方法包括：基于里程计数据和前一时刻的位姿，预测移动机器人的当前位姿；当确定里程计的数据变化量大于第一阈值时，利用当前时刻的激光点云数据中的长期激光数据特征与先验地图进行匹配，以及利用当前时刻的激光点云数据中的短期激光数据特征与局部子地图进行匹配，基于匹配结果对当前移动机器人的位姿进行估计；确定长期激光数据特征与先验地图不匹配时，构建激光里程计；基于长期激光数据特征与先验地图的匹配结果、激光里程计和轮式里程计的计数数据对移动机器人的位姿进行优化，将优化后的位姿作为移动机器人的位姿。本发明估算移动机器人的当前位姿更准确。

Description

移动机器人定位方法及装置、电子设备及存储介质

技术领域

本发明涉及移动机器人定位技术，尤其涉及一种移动机器人定位方法及装置、电子设备、存储介质。

背景技术

移动机器人在进行任务操作时—比如在仓库中进行物体搬运的情况下，需要在变换的环境中精确地知道自身在当前环境的位置，从而能够准确地执行任务；移动机器人可以通过里程计，初步估算出移动机器人角度和距离的变化量，常见的有轮式编码器，从而能够根据前后时刻的位姿变化量和前一时刻的位姿来估计出当前时刻机器人的位姿；或者，移动机器人通过二维激光雷达获取二维平面信息的传感器，用于检测周围环境的二维平面轮廓信息，以此确定当前位置。或者，机器人实时定位建图时，利用估算的位姿，将若干帧激光点云数据构建一张子地图，这样的子地图叫做submap。利用激光雷达数据与submap匹配，可以进一步估算机器人当前的位姿。但当环境变化较大时，可能会错失掉关键激光帧的匹配，从而得到错误的位姿估计。

发明内容

本发明提供一种移动机器人定位方法及装置、电子设备、存储介质，以至少解决现有技术中存在的以上技术问题。

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

基于所获取的里程计数据和前一时刻的位姿，预测移动机器人的当前位姿；

若确定里程计的数据变化量大于第一阈值或前后获取数据的时间差大于第二阈值，则利用当前时刻的激光点云数据中的长期激光数据特征与先验地图进行匹配，以及利用当前时刻的激光点云数据中的短期激光数据特征与局部子地图进行匹配，基于匹配结果对当前移动机器人的位姿进行估计；

确定长期激光数据特征与先验地图不匹配时，构建激光里程计；基于长期激光数据特征与先验地图的匹配结果、激光里程计和轮式里程计的计数数据对移动机器人的位姿进行优化，将优化后的位姿作为移动机器人的位姿。

可选地，所述方法还包括：

利用相邻激光全局匹配相对变化量、激光里程计计算的相对变化量与里程计计算的相对变化量进行比较，确定里程计计数异常时，去除位姿优化中的由里程计计算的约束因子；

提高激光匹配频率，当前帧的激光匹配频率的初值根据激光帧的匀速运动模型确定；

将当前所有约束因子进行优化，作为移动机器人的位姿。

可选地，所述方法还包括：

激光点云数据中的长期激光数据特征与先验地图的匹配得分低于设定阈值且未扫描到反光柱或反光条时，将长期激光数据特征与先验地图的匹配结果对应的约束因子从位姿优化计算中去除，并提升激光里程计计数数据对移动机器人的位姿的优化权重。

可选地，所述方法还包括：

确定基于里程计计数在t时刻的移动机器人的位姿，基于移动机器人的激光雷达的第k束激光数据相对于移动机器人坐标系的坐标，确定第k束激光数据在世界坐标系下的坐标；

基于世界坐标系下的坐标计算离最近栅格的距离，所计算距离小于第三阈值时，将第k束激光数据称为长期激光点云特征，所计算距离大于第三阈值时，将第k束激光数据称为短期激光点云特征。

可选地，所述基于匹配结果对当前移动机器人的位姿进行估计，包括：

基于长期激光数据特征与先验地图的匹配结果和里程计估计的移动机器人的位置，以及短期激光数据特征与局部子地图的匹配结果及里程计估计的移动机器人的位姿，分别确定基于里程计和激光里程计确定的不同时刻的全局优化节点的相对变化量；

基于所确定的全局优化节点的相对变化量、局部激光帧对应的优化变量、局部关键帧对应的优化变量，确定局部关键帧分别与全局优化节点、局部优化变量之间的相对约束；

基于相对约束及本次优化的全局优化变量的初值在匹配结果与优化节点之间、优化节点之间、局部优化节点之间分别构建残差约束；

将所述残差约束进行联合优化，确定最新激光时间戳对应的全局优化变量，利用当前时刻与全局优化变量对应时刻的里程计数据，递推出当前时刻的移动机器人的位姿。

本发明另一方面提供一种移动机器人定位装置，包括：

预测单元，用于基于所获取的里程计数据和前一时刻的位姿，预测移动机器人的当前位姿；

估计单元，用于在确定里程计的数据变化量大于第一阈值或前后获取数据的时间差大于第二阈值时，利用当前时刻的激光点云数据中的长期激光数据特征与先验地图进行匹配，以及利用当前时刻的激光点云数据中的短期激光数据特征与局部子地图进行匹配，基于匹配结果对当前移动机器人的位姿进行估计；

构建单元，用于确定长期激光数据特征与先验地图不匹配时，构建激光里程计；

优化单元，用于基于长期激光数据特征与先验地图的匹配结果、激光里程计和轮式里程计的计数数据对移动机器人的位姿进行优化，将优化后的位姿作为移动机器人的位姿。

可选地，所述优化单元，还用于：

将当前所有约束因子进行优化，作为移动机器人的位姿。

可选地，所述装置还包括：

去除单元，用于在激光点云数据中的长期激光数据特征与先验地图的匹配得分低于设定阈值且未扫描到反光柱或反光条时，将长期激光数据特征与先验地图的匹配结果对应的约束因子从位姿优化计算中去除，并提升激光里程计计数数据对移动机器人的位姿的优化权重。

可选地，所述估计单元，还用于：

本发明另一方面提供一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；存储器，用于存放计算机程序；处理器，用于执行存储器上所存放的程序时，实现所述的移动机器人定位方法的步骤。

本发明再一方面提供一种计算机可读存储介质，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现所述的移动机器人定位方法的步骤。

本发明当传感器数据未出现异常时，设计了一种简单通用的图优化模型用于融合轮式里程计数据，激光与先验地图匹配数据以及激光里程计数据。在不增加额外传感器的情况下降低了成本，利用激光与先验地图匹配的可靠性结果以及激光里程计结果来检测出里程计数据是否出现异常或者打滑情况。在检测到里程计数据异常时，更改图优化模型，更好地利用激光与全局地图匹配的结果和激光里程计结果估算机器人的位姿；当检测到当前激光与全局地图匹配不准确时，更改后端图优化模型以及相关残差的权重，更好地利用激光里程计和轮式里程计来估算出移动机器人的当前位姿，方便快捷。

附图说明

图1示出了本发明实施例的移动机器人定位方法的流程图；

图2示出了本发明实施例的移动机器人示意图；

图3示出了本发明实施例的移动机器人定位方法的流程图；

图4示出了本发明实施例的后端图优化模型示意图；

图5示出了本发明实施例的后端图优化模型示意图；

图6示出了本发明实施例的后端图优化模型示意图；

图7示出了本发明实施例的移动机器人定位装置的组成结构示意图；

图8示出了本发明实施例的电子设备的结构图。

具体实施方式

为使本发明的目的、特征点、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而非全部实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1示出了本发明实施例的移动机器人定位方法的流程图，如图1所示，本发明实施例的移动机器人定位方法包括以下处理步骤：

步骤101，基于所获取的里程计数据和前一时刻的位姿，预测移动机器人的当前位姿。

本发明实施例中，可以基于实时定位与建图算法（SLAM，Simultaneous Localization And Mapping）建立当前仓库环境的地图，生成概率栅格地图（M）。遍历整张概率栅格地图，计算地图中每一个栅格离地图中最近的障碍物点的距离生成距离地图(D)，而有障碍物处的栅格距离值直接设置为零。移动机器人在仓库环境中移动时，能够同时获取里程计数据和激光数据。首先利用里程计数据u_t和前一时刻的位姿

，其中x、y分别为世界坐标系下横纵坐标，表示机器人在x轴、y轴所在二维平面的位置，θ表示机器人在x轴、y轴所在二维平面的方向。利用运动模型，能够预测出当前时刻的位姿。这里的运动模型是事先建立的针对移动机器人的机器学习模型。

步骤102，当确定里程计的数据变化量大于第一阈值或前后获取数据的时间差大于第二阈值，则利用当前时刻的激光点云数据中的长期激光数据特征与先验地图进行匹配，以及利用当前时刻的激光点云数据中的短期激光数据特征与局部子地图进行匹配，基于匹配结果对当前移动机器人的位姿进行估计。

本发明实施例中，还包括：利用相邻激光全局匹配相对变化量、激光里程计计算的相对变化量与里程计计算的相对变化量进行比较，确定里程计计数异常时，去除位姿优化中的由里程计计算的约束因子；提高激光匹配频率，当前帧的激光匹配频率的初值根据激光帧的匀速运动模型确定；将当前所有约束因子进行优化，作为移动机器人的位姿。

当激光点云数据中的长期激光数据特征与先验地图的匹配得分低于设定阈值且未扫描到反光柱或反光条时，将长期激光数据特征与先验地图的匹配结果对应的约束因子从位姿优化计算中去除，并提升激光里程计计数数据对移动机器人的位姿的优化权重。

步骤103，确定长期激光数据特征与先验地图不匹配时，构建激光里程计；基于长期激光数据特征与先验地图的匹配结果、激光里程计和轮式里程计的计数数据对移动机器人的位姿进行优化，将优化后的位姿作为移动机器人的位姿。

本发明实施例中，基于长期激光数据特征与先验地图的匹配结果和里程计估计的移动机器人的位置，以及短期激光数据特征与局部子地图的匹配结果及里程计估计的移动机器人的位姿，分别确定基于里程计和激光里程计确定的不同时刻的全局优化节点的相对变化量；

以下通过具体示例，进一步阐明本发明实施例的技术方案的本质。

本发明实施例主要是基于里程计、激光传感器和先验地图估计移动机器人的位姿，首先对当前激光数据进行预处理，分离出环境中原有物体产生的长期激光数据和环境中变化物体产生的短期激光数据，利用长期数据与地图匹配，估计当前位姿；然后通过实时构建局部子地图，激光与局部地图匹配产生激光里程计。然后结合里程计信息、激光匹配结果和激光里程计联合优化得到机器人位姿。本专利主要针对当里程计数据异常比如出现里程计打滑或者没有里程计数据时，如何利用激光传感器数据进行更好地定位，或者当激光雷达与先验地图的匹配度低时，如果利用里程计和激光里程计更好地估计机器人位姿。

具体地，本发明实施例结合里程计、IMU和2D激光雷达传感器，进行移动机器人的高精度定位；通过检测里程计是否打滑、是否没有里程计数据来重新设计残差因子图，以及当激光与先验地图匹配得分很低时，再次重新构建后端残差因子图，从而保证在有单一传感器数据异常时，仍能较准确地估计机器人自身的位姿。

如图2所示，本发明实施例的移动机器人包括移动地盘（包括运动控制器、电机、电池、嵌入式计算机、里程计等）、2D激光雷达传感器。该机器人可以在基于仓储环境下生成的二维栅格地图中进行高精度定位。该地图包括货架、工作台、房屋支撑架以及需要搬运的货物等部件。移动机器人使用里程计估计出机器人运动的该变量，同时使用2D激光雷达扫描环境轮廓。移动机器人在该环境中前进过程中，对仓储环境进行充分扫描，融合里程计信息，并剔除动态障碍物以及新增加货物等产生的激光数据，对移动机器人进行高精度定位。

基于实时定位与建图算法（SLAM）建立当前仓库环境的地图，生成概率栅格地图（M）。遍历整张概率栅格地图，计算地图中每一个栅格离地图中最近的障碍物点的距离生成距离地图(D)，而有障碍物处的栅格距离值直接设置为零。

图3示出了本发明实施例的移动机器人定位方法的流程图，如图3所示，机器人在仓库环境中移动时，能够同时获取里程计数据和激光数据，具体包括以下处理：

首先利用里程计数据u_t和前一时刻的位姿

，其中x、y分别为世界坐标系下横纵坐标，表示机器人在x轴、y轴所在二维平面的位置，θ表示机器人在x 轴、y轴所在二维平面的方向。利用运动模型，能够预测出当前时刻的位姿。运动模型方程如公式（1）所示。

（1）

如果前一时刻和当前时刻的里程计变化量大于一定阈值如第一阈值或者该时间差大于一定阈值如第二阈值，则利用当前时刻的激光数据来估算当前时刻的位姿，具体包括：

里程计在t时刻估计位姿为x _t，记

，假设激光雷达的第k束激光数据相对于机器人坐标系的坐标为

，则第k束激光数据

在世界坐标系下的坐标为：

(2)

其中，(

、

)为第k束激光在世界坐标系下的坐标。计算该坐标离最近栅格坐标的距离，当该距离小于一定阈值m表示该激光点云是由地图中原有的物体产生的，称为长期激光点云特征；如果该距离大于阈值m，则表示该激光点云是由地图中新增的物体产生的，称为短期激光点云特征。

将上述长期激光数据特征与先验地图进行匹配，使用scan to map的方法，进一步估计出当前移动机器人的位姿。

在定位过程中，实时构建出能反映当前环境真实状态的局部子地图submap，激光与submap匹配，构建激光里程计，然后将激光匹配频率的结果、激光里程计和轮式里程计的数据联合优化，得到机器人优化后的位姿，联合优化对应的因子图如图4所示，图4中，第一行正方形方框表示激光里程计因子，激光之间的相对位姿约束；第二行中的圆点表示局部激光帧变量，第三行正方形方框表示局部激光帧与全局激光帧相同时间戳对应的变量之间的相对约束因子；第四行中的圆点表示全局激光帧变量，第四行中的正方形方框表示全局激光帧之间的轮式里程计相对约束因子；第五行中的正方形方框表示全局激光帧与先验地图之间的绝对约束关系。

记t1时刻激光与局部子地图匹配的结果为T1，与先验地图匹配的结果为G1，对应的全局优化节点为N1，里程计估计的位置为O1；t2时刻激光与局部子地图匹配的结果为T2，与先验地图匹配的结果为G2，对应的优化节点为N2，里程计估计的位姿为O2。这里的里程计是指轮式里程计，与激光里程计不同。则N1与N2间由里程计计算得到的相对变化量为：

（3）

N1与N2间由激光里程计计算得到的相对变化量

为：

（4）

局部激光帧对应的局部优化变量L1的时间戳若和G1的时间戳相同，则N1与L1之间的相对关系为单位阵；同理局部关键帧对应的局部优化变量L2的时间戳和G2的时间戳相同，则N2与L2的相对变化量为单位阵。若L1与L2间还有其他的局部关键帧Li，则需要在里程计数据队列中，找到离Li时间戳最近的里程计两个里程计数据OL1和OL2，然后利用线性插值的方法，算出Li时刻对应的里程计位姿OLi，同理利用该方法找到全局优化节点N1和N2对应的里程计位姿ON1和ON2，则局部激光优化变量Li与N1、N2间的相对约束为：

（5）

（6）

而Li与L1的相对约束关系为：

（7）

根据前一次优化结果、前一时刻到当前时刻的里程计计算的相对变化量，得到本次优化的全局优化变量的初值为：

（8）

则构建的残差项有：

全局匹配结果G2与N2构建的残差约束为：

（9）

轮式里程计在N1和N2之间构建的相对残差约束为：

（10）

激光里程计在局部优化节点之间构建的相对残差约束为：

（11）

若局部优化变量L1和L2之间有局部激光变量Li，则Li与全局优化节点N1、N2以及L1之间构建的相对残差约束为：

（12）

（13）

（14）

其中，p1、p2、p3和p4分别是不同约束残差对应的权重，不同权重将对应不同程度的约束强度，

表示平移残差上的权重，

表示角度残差上的权重，而公式11—14的相对残差展开公式和公式10类似。将上述残差进行联合优化，便能求出最新激光时间戳对应的N2，然后利用当前时刻与N2时刻的里程计数据，递推出当前时刻的位姿

，并将其发布出来。

如图5所示，示出了全局优化过程，第一行正方形方框表示激光里程计因子，激光之间的相对位姿约束；第二行中的圆点表示局部激光帧变量，第三行竖直线连接的正方形方框表示局部激光帧与全局激光帧相同时间戳对应的变量之间的相对约束因子，斜线连接的正方形方框表示局部激光帧之间的轮式里程计相对约束因子；第四行中的圆点表示全局激光帧变量，第四行中的正方形方框表示全局激光帧之间的轮式里程计相对约束因子；第五行中的正方形方框表示全局激光帧与先验地图之间的绝对约束关系。

里程计数据异常检测以及修改后端优化因子图。具体包括以下处理：

在移动机器人在直线运动或者旋转过程中，经常会出现打滑的情况，此时里程计的数据是不准的，需要将其信息从上述图因子中继续剔除。一般检测打滑的方法会利用相邻时刻的里程计数据和对应相邻时刻的IMU数据检测旋转打滑的情况，而对于原地空转的情况，无法利用IMU数据对其进行准确的检测。本发明实施例通过检测相邻激光时刻与先验地图匹配的位姿的匹配结果G1和G2，且位姿得分大于一定阈值如第三阈值(比如阈值为30分)或者得分低但能扫描到反光柱或者反光条，认为当前位姿可靠的情况下，计算其相对变化量：

而相邻激光时刻对应的里程计数据为O1和O2，对应的变化量为

，当在一定时间内连续检测到若干次（比如2s内检测到20次）

与

相差大，则认为里程计出现打滑。

当激光与先验地图匹配得分低且没有扫到反光柱或者反光条时，激光与先验地图匹配后的位姿可能不可靠，此时需要利用激光里程计计算的相对变化量来检测里程计数据是否出现异常。若上述相邻时刻激光与局部地图匹配的结果为L1和L2，则激光里程计计算出来的相对变化量为

，同理若在一定时间范围内检测到若干次

与

相差大，则认为里程计出现打滑，里程计数据不可靠。

通过不断检测到里程计数据没有进行更新，且满足100ms都没有数据更新；或者检测到里程计速度发生很大跳变，都将认为此时里程计数据不可靠，需要修改后端优化因子图，只要之前利用里程计信息计算的节点之间的相对约束都必须去除，更改后的因子图如图6所示，图6中，第一行正方形方框表示激光里程计因子，激光之间的相对位姿约束；第二行中的圆点表示局部激光帧变量，第三行竖直线连接的正方形方框表示局部激光帧与全局激光帧相同时间戳对应的变量之间的相对约束因子，斜线连接的正方形方框表示；第四行中的圆点表示全局激光帧变量，第四行中的正方形方框表示全局激光帧之间的轮式里程计相对约束因子；第四行中的正方形方框表示全局激光帧与先验地图之间的绝对约束关系。

其中，全局优化节点之间由里程计计算的相对位姿去除，若全局位姿节点对应的局部激光优化节点之间有额外的局部优化节点Li，则Li与全局节点通过里程计计算的相对位姿应该去除。同时，全局优化变量节点优化的初值需要通过上一帧的优化位姿和当前激光帧与上一帧激光帧与局部地图匹配计算的相对位姿计算而来。

当发生上述里程计位姿不可靠时，需要提高激光匹配频率，即认为每一帧激光点云数据都是关键帧，且激光匹配频率的初值不再由上一次优化结果与上一次优化时间和当前帧时间的里程计数据递推出本次匹配的初值，而是根据激光帧的匀速运动模型来计算当前帧的匹配初值，具体方法为：

记前前一帧的激光与局部地图匹配的结果为L_pp，对应的时间戳t_pp，前一帧激光与局部地图匹配的结果为L_p，联合优化的结果为T_p，对应的时间戳为t_p，当前激光帧对应的时间戳为t_c，则当前帧激光匹配频率的初值为：

激光与先验地图匹配得分低，修改后端优化因子图。

当移动机器人导航的环境变化大，激光与先验地图匹配得分很低且没有扫描到反光柱或者反光条时，此时认为激光与先验地图匹配的位姿不可靠，此时需要将激光匹配结果从后端优化因子图中舍去，对应的因子图如图6所示，图6中，第一行正方形方框表示激光里程计因子，激光之间的相对位姿约束；第二行中的圆点表示局部激光帧变量，第三行竖直线连接的正方形方框表示局部激光帧与全局激光帧相同时间戳对应的变量之间的相对约束因子；第四行中的圆点表示全局激光帧变量；第五行中的正方形方框表示全局激光帧与先验地图之间的绝对约束关系。

从上图6中可以看到，激光与先验地图匹配的约束因子为虚线、内部填充颜色为白色的方框，此时表示该约束因子对应的权重很小，甚至可以为0，此时完全依靠轮式里程计和激光里程计进行导航，由于轮式里程计容易产生累积误差，而激光里程计相对更可靠一些，此时要适当增大激光里程计的匹配权重，从而保证机器人位姿估计的准确性。当然，这种情况不能一直让设备持续走下去，当检测到设备运行10m左右，激光与先验地图匹配的得分还未升高或者还未检测到反光柱、反光条时，需要进入人工干预，从而保证设备的安全运行。当检测到激光与先验地图匹配的得分较高或者能扫到反光条、反光柱时，要恢复激光与先验地图匹配构建残差的权重。

图7示出了本发明实施例的移动机器人定位装置的组成结构示意图，如图7所示，本发明实施例的移动机器人定位装置包括：

预测单元70，用于基于所获取的里程计数据和前一时刻的位姿，预测移动机器人的当前位姿；

估计单元71，用于在确定里程计的数据变化量大于第一阈值或前后获取数据的时间差大于第二阈值时，利用当前时刻的激光点云数据中的长期激光数据特征与先验地图进行匹配，以及利用当前时刻的激光点云数据中的短期激光数据特征与局部子地图进行匹配，基于匹配结果对当前移动机器人的位姿进行估计；

构建单元72，用于确定长期激光数据特征与先验地图不匹配时，构建激光里程计；

优化单元73，用于基于长期激光数据特征与先验地图的匹配结果、激光里程计和轮式里程计的计数数据对移动机器人的位姿进行优化，将优化后的位姿作为移动机器人的位姿。

作为一种实现方式，所述优化单元73，还用于：

将当前所有约束因子进行优化，作为移动机器人的位姿。

作为一种实现方式，在图7所示的移动机器人定位装置的基础上，本发明实施例的移动机器人定位装置还包括：

去除单元（图7未示出），用于在激光点云数据中的长期激光数据特征与先验地图的匹配得分低于设定阈值且未扫描到反光柱或反光条时，将长期激光数据特征与先验地图的匹配结果对应的约束因子从位姿优化计算中去除，并提升激光里程计计数数据对移动机器人的位姿的优化权重。

作为一种实现方式，所述优化单元73，还用于：

作为一示例，具体实现方式为，若t1时刻激光点云数据中的短期激光数据特征与局部子地图匹配结果为T1，激光点云数据中的长期激光数据特征与先验地图匹配结果为G1，全局优化节点为N1，里程计估计的移动机器人位置为O1；t2时刻激光点云数据中的短期激光数据特征与局部子地图匹配结果为T2，激光点云数据中的长期激光数据特征与先验地图匹配结果为G2，全局优化节点为N2，里程计估计的位姿为O2；则在N1与N2之间由里程计计算得到的相对变化量为：

N1与N2之间由激光里程计计算得到的相对变化量为：

局部激光帧对应的优化变量L1的时间戳若与G1的时间戳相同，则N1与L1之间的相对关系为单位阵；若局部关键帧对应的优化变量L2的时间戳与G2的时间戳相同，则N2与L2的相对变化量为单位阵；

若L1与L2间还有其他的局部关键帧Li，则在里程计数据队列中查找距Li时间戳最近的里程计的两个里程计数据OL1和OL2，利用线性插值的装置，计算Li时刻对应的里程计位姿OLi，查找N1和N2对应的里程计位姿ON1和ON2，则Li与N1、N2间的相对约束为：

Li与L1的相对约束关系为：

则G2与N2构建的残差约束为：

轮式里程计在N1和N2之间构建的相对残差约束为：

激光里程计在局部优化节点之间构建的相对残差约束为：

若局部激光帧L1和L2之间有局部激光变量Li，则Li与N1、N2以及L1之间构建的相对残差约束为：

表示平移残差上的权重，

表示角度残差上的权重；将上述残差进行联合优化，确定最新激光时间戳对应的全局优化节点N2，然后利用当前时刻与N2时刻的里程计数据，递推出当前时刻的位姿

。

在示例性实施例中，预测单元70、估计单元71、构建单元72、优化单元73和去除单元等可以被一个或多个中央处理器（CPU，Central Processing Unit）、图形处理器（GPU，Graphics Processing Unit）、基带处理器（BP，Base Processor）、应用专用集成电路（ASIC，Application Specific Integrated Circuit）、数字信号处理器（Digital SignalProcessor，DSP）、可编程逻辑器件（PLD，Programmable Logic Device）、复杂可编程逻辑器件（CPLD，Complex Programmable Logic Device）、现场可编程门阵列（FPGA，Field-Programmable Gate Array）、通用处理器、控制器、微控制器（MCU，Micro ControllerUnit）、微处理器（Microprocessor）、或其他电子元件实现，用于执行前述实施例的移动机器人定位方法的步骤。

在本公开实施例中，图7示出的移动机器人定位装置中各个单元执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。

下面，参考图8来描述根据本申请实施例的电子设备11。

如图8所示，电子设备11包括一个或多个处理器111和存储器112。

处理器111可以是中央处理单元（CPU）或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制电子设备11中的其他组件以执行期望的功能。

存储器112可以包括一个或多个计算机程序产品，所述计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器（RAM）和/或高速缓冲存储器（cache）等。所述非易失性存储器例如可以包括只读存储器（ROM）、硬盘、闪存等。在所述计算机可读存储介质上可以存储一个或多个计算机程序指令，处理器111可以运行所述程序指令，以实现上文所述的本申请的各个实施例的方法以及/或者其他期望的功能。在所述计算机可读存储介质中还可以存储诸如输入信号、信号分量、噪声分量等各种内容。

在一个示例中，电子设备11还可以包括：输入装置113和输出装置114，这些组件通过总线系统和/或其他形式的连接机构（未示出）互连。

该输入装置113可以包括例如键盘、鼠标等等。

该输出装置114可以向外部输出各种信息，包括确定出的距离信息、方向信息等。该输出装置114可以包括例如显示器、扬声器、打印机、以及通信网络及其所连接的远程输出设备等等。

当然，为了简化，图8中仅示出了该电子设备11中与本申请有关的组件中的一些，省略了诸如总线、输入/输出接口等等的组件。除此之外，根据具体应用情况，电子设备11还可以包括任何其他适当的组件。

除了上述方法和设备以外，本申请的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的方法中的步骤。

所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本申请实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言，诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。

此外，本申请的实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的方法中的步骤。

所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子（非穷举的列表）包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器（RAM）、只读存储器（ROM）、可擦式可编程只读存储器（EPROM或闪存）、光纤、便携式紧凑盘只读存储器（CD-ROM）、光存储器件、磁存储器件、或者上述的任意合适的组合。

以上结合具体实施例描述了本申请的基本原理，但是，需要指出的是，在本申请中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本申请的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本申请为必须采用上述具体的细节来实现。

本申请中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“如但不限于”，且可与其互换使用。

还需要指出的是，在本申请的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本申请的等效方案。

提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本申请。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本申请的范围。因此，本申请不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。

为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本申请的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。

Claims

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

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

将当前所有约束因子进行优化，作为移动机器人的位姿。

3.根据权利要求1或2所述的方法，其特征在于，所述方法还包括：

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

5.根据权利要求1所述的方法，其特征在于，所述基于匹配结果对当前移动机器人的位姿进行估计，包括：

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

7.根据权利要求6所述的装置，其特征在于，所述优化单元，还用于：

将当前所有约束因子进行优化，作为移动机器人的位姿。

8.根据权利要求6或7所述的装置，其特征在于，所述装置还包括：

9.根据权利要求6所述的装置，其特征在于，所述估计单元，还用于：

10.根据权利要求6所述的装置，其特征在于，所述估计单元，还用于：

11.一种电子设备，其特征在于，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；存储器，用于存放计算机程序；处理器，用于执行存储器上所存放的程序时，实现权利要求1-5任一项所述的移动机器人定位方法的步骤。

12.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现权利要求1-5任一项所述的移动机器人定位方法的步骤。