CN112197770B - 一种机器人的定位方法及其定位装置 - Google Patents

Abstract

本发明公开了一种机器人的定位方法及其定位装置，通过结合IMU、轮速里程计、图像、激光和UWB技术，可以实现多传感器的融合，实时得到机器人的当前关键帧的位姿，并且，在确定出与当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧后，可以对当前关键帧的位姿进行实时地滑窗闭环优化处理，以实现对确定出的当前关键帧的位姿进行快速重定位，以便于提高机器人实时定位的速度和精度，从而提高机器人定位的效率。

Description

一种机器人的定位方法及其定位装置

技术领域

本发明涉及机器人技术领域，尤指一种机器人的定位方法及其定位装置。

背景技术

随着对制造灵活性的需求增加、产品周期缩短和加快、人力成本上升、以及人类安全趋势的需求增加，全球移动机器人的市场正在快速发展，技术需求越来越高，例如，自主导引车（Automated Guided Vehicles AGV）已广泛应用于自动化仓库、工厂物料传输系统、物流拣配系统、柔性装配系统和其他智能运输站点。相对于基于磁条、反光板、二维码等AGV，自主移动机器人（Autonomous Mobile Robot, AMR）的技术难度更高和使用场景更广，AMR导航的核心技术即SLAM（Simultaneous Localization and Mapping），也即使得机器人知道环境地图，以及知道自己实时位置，这是全程不需要人干预、高效智能移动到达目标的基础。

目前，单传感器SLAM的定位精度不高，在退化环境中容易发生丢失，导致无法对机器人的位置进行定位，从而导致机器人无法正常有效地工作。

基于此，如何提高机器人的定位的精度，是本领域技术人员亟待解决的技术问题。

发明内容

本发明实施例提供了一种机器人的定位方法及其定位装置，用以提高机器人的定位的精度。

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

根据所述机器人当前采集到的惯性测量单元IMU（Inertial Measuring Unit）数据、轮速里程计数据、图像数据和激光数据、以及通过超宽带通信UWB技术确定出的所述机器人的中间位姿，按照预设的多传感器融合算法，确定当前关键帧及所述当前关键帧的位姿；

根据当前处于滑窗中图像帧的位姿、以及确定出的与所述当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对所述当前关键帧的位姿进行滑窗闭环优化处理，得到处理后的所述当前关键帧的位姿；

根据处理后的所述当前关键帧的位姿，确定所述机器人的位姿；

其中，所述多传感器融合算法包括：根据确定出的所述机器人采集到的每一帧图像中角点的匹配结果、采集到的所有连续两帧图像得到的状态数据、以及所述中间位姿，对每一个图像帧进行滑窗优化处理后，确定出所述当前关键帧的位姿；所述状态数据包括：激光位姿信息、IMU角度和位移的积分量信息、轮速里程计的角度和位移的积分量信息。

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

第一单元，用于根据所述机器人当前采集到的惯性测量单元IMU数据、轮速里程计数据、图像数据和激光数据、以及通过超宽带通信UWB技术确定出的所述机器人的中间位姿，按照预设的多传感器融合算法，确定当前关键帧及所述当前关键帧的位姿；

第二单元，用于根据当前处于滑窗中图像帧的位姿、以及确定出的与所述当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对所述当前关键帧的位姿进行滑窗闭环优化处理，得到处理后的所述当前关键帧的位姿；

第三单元，用于根据处理后的所述当前关键帧的位姿，确定所述机器人的位姿；

其中，所述多传感器融合算法包括：根据确定出的所述机器人采集到的每一帧图像中角点的匹配结果、采集到的所有连续两帧图像得到的状态数据、以及所述中间位姿，对每一个图像帧进行滑窗优化处理后，确定出所述当前关键帧的位姿；所述状态数据包括：激光位姿信息、IMU角度和位移的积分量信息、轮速里程计的角度和位移的积分量信息。

本发明有益效果如下：

本发明实施例提供的一种机器人的定位方法及其定位装置，通过结合IMU、轮速里程计、图像、激光和UWB技术，可以实现多传感器的融合，多方位全方面的得到机器人的当前关键帧的位姿，并且，在确定出与当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧后，可以对当前关键帧的位姿进行实时地滑窗闭环优化处理，以实现对确定出的当前关键帧的位姿进行快速重定位，以便于提高机器人实时定位的速度和精度，同时还可以提高机器人定位的速度，从而提高机器人定位的效率。

附图说明

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

图2为本发明实施例中提供的具体实施例的流程图；

图3为本发明实施例中提供的一种机器人的定位装置的结构示意图；

图4为本发明实施例中提供的一种机器人的结构示意图。

具体实施方式

下面将结合附图，对本发明实施例提供的一种机器人的定位方法及其定位装置的具体实施方式进行详细地说明。需要说明的是，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明实施例提供了一种机器人的定位方法，如图1所示，可以包括：

S101、根据机器人当前采集到的惯性测量单元IMU数据、轮速里程计数据、图像数据和激光数据、以及通过超宽带通信UWB技术确定出的机器人的中间位姿，按照预设的多传感器融合算法，确定当前关键帧及当前关键帧的位姿；

其中，多传感器融合算法包括：根据确定出的机器人采集到的每一帧图像中角点的匹配结果、采集到的所有连续两帧图像得到的状态数据、以及中间位姿，对每一个图像帧进行滑窗优化处理后，确定出当前关键帧的位姿；状态数据包括：激光位姿信息、IMU角度和位移的积分量信息、轮速里程计的角度和位移的积分量信息。

其中，激光位姿信息可以但不限于包括：激光位姿增量（可以包括位置增量和角度增量）、对应的不确定性方差、以及对应的一阶导数；IMU角度和位移的积分量信息可以但不限于包括：IMU角度和位移的积分量、对应的不确定性方差、以及对应的一阶导数；轮速里程计的角度和位移的积分量信息可以但不限于包括：轮速里程计的角度和位移的积分量、对应的不确定性方差、以及对应的一阶导数。

说明一点，在本发明实施例中，每一帧图像可以理解为：IMU、轮速里程计、激光或UWB采集到的每一个数据帧时刻对应的图像采集设备采集到的一帧图像，每一个图像帧可以理解为图像采集设备采集到的每一帧图像。

可选地，在本发明实施例中，在采集图像数据时，所使用的图像采集设备（例如但不限于相机）可以为单目相机、双目相机和多目相机。

对于单目相机而言，具有体积小、制作成本低等优势；

对于双目相机和多目相机而言，可以仅通过单帧图像，确定出图像中特征点坐标的真实尺度，并且随着相机目数的增加，确定出的尺度也就越准确；此外，多目相机还可以拍到更多的预设标签和特征点，有利于提高后续定位结果的准确度和稳定性。

其中，上述尺度可以理解为：图像中的某个特征点在相机坐标系下的坐标放大因子。

对于本发明实施例提供的上述定位方法而言，不仅可以兼容上述三类相机，还可以弥补单目相机中尺度模糊/漂移、以及双目相机和多目相机难以获取相机之间的外参的问题，从而实现尺度准确可靠，实时在线估计出各相机之间的外参。

因此，在使用单目相机时，采集到的每一帧图像只包括一目图像；在使用双目相机时，采集到的每一帧图像可以包括第一目图像（可以记为图像1）和第二目图像（可以记为图像2），并且，第一目图像可以为左目图像，第二目图像可以为右目图像；或者，第一目图像可以为右目图像，第二目图像可以为左目图像。

进而，在使用双目相机或多目相机时，可以确定出采集到的每一帧图像中包括的每一目图像中角点的匹配结果。

下面的内容均是以双目相机为例进行说明。

说明一点，可选地，在采集IMU数据、轮速里程计数据、图像数据和激光数据时，可以采用以下方式：

方式1：同步采集。

也就是说，可以同步采集IMU数据、轮速里程计数据、图像数据、激光数据、UWB的相关数据，以便于可以快速确定出当前关键帧的位姿。

方式2：非同步采集。

也就是说，可以非同步采集IMU数据、轮速里程计数据、图像数据、激光数据、UWB的相关数据，此时，采集到的IMU数据、轮速里程计数据、图像数据、激光数据、UWB的相关数据之间可能会存在时间差，此时：

根据采集到的各数据中携带的时间戳信息，利用预设的插值法，进行时间对齐处理，使得确定出同一时间对应的各传感器的数据，从而确定出当前关键帧的位姿。

因此，在实际情况中，在采用各传感器采集数据时，可以按照上述方式1和方式2进行采集，具体采用哪种方式可以根据实际需要进行设计，在此并不限定。

具体地，在本发明实施例中，为了实现上述步骤S101，可以具体包括以下内容：

过程1、通过IMU、轮速里程计、相机（相机采集到的数据为图像数据）、以及激光采集第n帧数据，并分别对采集到的数据进行预处理，得到初始数据；以及通过UWB技术，确定出的机器人的中间位姿；

1、对于IMU数据而言：

在对IMU采集到的数据进行预处理后，得到：两个图像帧之间的IMU角度和位移的积分量、描述IMU不确定性的测量模型、以及预积分误差的雅克比矩阵（也即一阶导数）；

具体地，预处理过程可以包括：

步骤1.1、确定获取到的连续两帧图像之间的惯性测量单元（Inertialmeasurement unit, IMU）数据，并存储至预设的缓存列表中；

步骤1.2、将IMU数据的积分起始时刻和结束时刻与对应的一帧图像的时刻信息进行对齐处理；

步骤1.3、根据中值法，将对齐处理后的IMU数据进行迭代积分处理，得到连续两帧图像之间的状态增量信息（即IMU角度和位移的积分量）；

步骤1.4、根据确定出的状态增量信息，确定对应的雅克比矩阵和方差矩阵。

具体地，在确定采集到的连续两帧图像之间的状态增量信息（即IMU角度和位移的积分量）时，所采用的过程并不限于上述步骤1.1至步骤1.4的具体实现过程，还可以为其他实现过程，在此并不限定。

2、对于图像数据而言：

在对相机采集到的数据进行预处理后，得到：每一帧图像中特征点的匹配信息；

可选地，在对图像中的特征点进行处理时，可以为：根据采集到的每一帧图像包括的图像1（即在采用双目相机时其中的左目相机（也即相机1）采集到的左目图像）和图像2（即在采用双目相机时其中的右目相机（也即相机2）采集到的右目图像）中特征点的匹配结果、以及与上一帧特征点的匹配结果，确定每一帧图像中的特征点匹配信息。

其中，特征点匹配信息可以包括：特征点的编号、特征点的坐标信息。

在确定每一帧图像中的特征点匹配信息时，可以采用如下步骤：

步骤2.1、对每一帧图像中的左目图像和相邻的上一帧图像中的左目图像进行特征点匹配处理、以及对每一帧图像中的右目图像和相邻的上一帧图像中的右目图像进行特征点匹配处理，得到每一帧图像中左目图像中的特征点、以及每一帧图像中右目图像中的特征点；

步骤2.2、对左目图像或右目图像进行特征点补充处理；

说明一点，执行该步骤2.2的目的在于：

以左目图像为例，对于第i帧图像而言，在执行上述步骤2.1之前，假设第i帧图像中左目图像中的角点具有150个，在执行上述步骤2.1之后，假设确定出的第i帧图像中左目图像中的特征点为100个；

那么，对于第i+1帧图像而言，如果将具有100个特征点的第i帧图像中左目图像与第i+1帧图像中左目图像进行特征点匹配处理时，在执行上述步骤2.1之后，确定出的第i+1帧图像中左目图像中的特征点的数量很可能会少于100个，假设为50个时，那么对于第i+2帧图像而言，经过特征点匹配处理后，确定出的第i+2帧图像中左目图像中的特征点的数量会更少，即少于50个；如此循环下去，在后续获取到的图像中，确定出的特征点数量越少，随着获取的图像的增加，确定出的特征点数量可能会为零，显然，这是与实际不相符的。

因此，为了避免出现上述情况，需要执行步骤2.2，即对特征点进行补充处理，也即，在确定出的第i帧图像中左目图像中的特征点为100个的基础上，可以对执行完上述步骤2.1的第i帧图像中左目图像中的特征点进行补充，假设可以补充50个特征点，即补充到150个，以保证后续获取到的图像中，可以确定出足够数量的特征点。

其中，补充的特征点可以从执行完上述步骤2.1的第i帧图像中左目图像中重新提取，且提取的过程，可以采用现有技术，只要能够提取出特征点即可，对于提取的过程在此并不限定。

步骤2.3、对执行完上述步骤2.1和步骤2.2的每一帧图像的左目图像和右目图像进行特征点匹配处理，得到每一帧图像的左目图像和右目图像中特征点的匹配关系；

步骤2.4、根据得到的匹配关系，确定每一帧图像中特征点的匹配信息。

说明一点，上述步骤2.1至步骤2.4的匹配过程，可以理解为采用描述子匹配算法来实现，但在实际情况中，并不限于此种算法，还可以采用其他算法来实现匹配过程，例如但不限于：光流跟踪匹配算法。

3、对于轮速里程计数据而言：

在对轮速里程计采集到的数据进行预处理后，得到：两个图像帧之间的轮速里程计的角度和位移的积分量、描述轮速里程计不确定性的测量模型、以及预积分误差的雅克比矩阵（也即一阶导数）；

并且，可选地，在基于轮速里程计数据，确定采集到的连续两帧图像之间的状态增量时，具体可以采用以下步骤：

步骤4.1、确定获取到的连续两帧图像之间的轮速里程计数据，并存储至预设的缓存列表中；

步骤4.2、将轮速里程计数据的积分起始时刻和结束时刻与对应的一帧图像的时刻信息进行对齐处理；

步骤4.3、根据中值法，将对齐处理后的轮速里程计数据进行迭代积分处理，得到连续两帧图像之间的状态增量信息（即轮速里程计的角度和位移的积分量）；

步骤4.4、根据确定出的状态增量信息，确定对应的雅克比矩阵和方差矩阵。

具体地，在确定采集到的连续两帧图像之间的状态增量信息（即轮速里程计的角度和位移的积分量）时，所采用的过程并不限于上述步骤1.1至步骤1.4的具体实现过程，还可以为其他实现过程，在此并不限定。

4、对于激光数据而言：

可以采用常用的激光SLAM算法，利用激光数据得到连续两个图像帧之间的激光位姿增量、不确定性方差、以及一阶导数，以便于在后续滑窗优化处理过程中使用。

并且，可选地，在基于激光数据，确定采集到的连续两个图像帧之间的激光位姿增量时，可以采用现有技术中的激光算法确定，在此并不限定。

5、对于UWB（Ultra Wide Band，超宽带）技术而言：

UWB技术的基本工作原理可以包括：

UWB定位标签（可以理解为设置在机器人上的终端设备）发射超宽带信号，UWB定位基站可以接收到该超宽带信号，并将接收到的该超宽带信号通过网线或者WIFI传输到软件控制系统，最后由软件控制系统进行分析和定位，以确定出UWB定位标签的位置信息。

进一步地，根据确定出的UWB定位标签的位置信息、以及部署在工作场地的UWB基站的位置信息，通过外参转换方法，可以确定出机器人的中间位姿（也可以称之为全局中间位姿）。

可选地，在本发明实施例中，根据中间位姿，确定图像帧时刻的UWB位姿，具体包括：

根据IMU数据中的速度数据和轮速里程计数据中的速度数据，将中间位姿插值到当前图像帧中，得到采集当前图像帧时刻对应的机器人的位姿；

将得到的采集当前图像帧时刻对应的机器人的位姿，确定为UWB位姿。

如此，可以通过UWB技术在确定出器人的中间位姿之后，可以进一步地确定出采集当前图像帧时刻对应的机器人的位姿，有利于构建位姿约束，从而有利于后续通过滑窗BA（Bundle Adjustment，集束调整）优化处理过程和/或位姿图优化处理过程优化当前关键帧的位姿。

过程2、根据上述预处理后的数据，对每一个图像帧进行滑窗优化处理，以确定出当前关键帧的位姿。

滑窗的主要目的为：在多个数据帧中，采用滑窗BA优化处理，优化数据帧的位姿、机器人的速度、IMU偏置、特征点深度、以及各传感器（传感器包括：IMU、轮速里程计、激光、图像采集设备（例如但不限于相机）和UWB）之间的外参等信息。

因此，在多传感器融合过程中，实现了IMU、轮速里程计、相机（或者称之为视觉）、激光和UWB等多传感器之间的外参在线实时优化估计。

可选地，在滑窗中需要优化的状态，可以表示为：

其中，χ _k 表示第k个关键帧的位姿，n表示当前滑窗中关键帧的数量，w表示世界坐 标系，

表示第k个关键帧在世界坐标系中的位移量，

表示第k个关键帧在世界坐标 系中的速度信息，

表示第k个关键帧在世界坐标系中的角度信息（或者称之为旋转量信 息），b _a 表示加速度的偏置，b _g 表示陀螺仪的偏置，

表示图像采集设备（如摄像头或称之 为相机）的坐标系c相对于IMU坐标系b之间的转换关系（转换关系也可以称之为外参），λ _i 表 示第i个特征点，m代表滑窗中观测到的特征点数量，

表示左目摄像头（即上述内容提及 的相机1）的坐标系相对于IMU坐标系的位置转换关系，

表示左目摄像头的坐标系相对 于IMU坐标系的相对旋转关系，

表示右目摄像头（即上述内容提及的相机2）的坐标系 相对于IMU坐标系的位置转换关系，

表示右目摄像头的坐标系相对于IMU坐标系的相对 旋转关系，

表示轮速里程计坐标系e相对于IMU坐标系b之间的转换关系，

表示该转 换关系的位置分量，

表示该转换关系的旋转分量，

表示UWB坐标系u相对于IMU坐标 系b之间的转换关系，

表示该转换关系的位置分量，

表示该转换关系的旋转分量，

表示激光坐标系l相对于IMU坐标系b之间的转换关系，

表示该转换关系的位置分 量，

表示该转换关系的旋转分量。

具体地，在进行滑窗优化处理时，具体可以包括：

步骤1、根据滑窗BA非线性优化处理，得到当前滑窗所有约束下的最优状态（即下面内容中提及的X*），并确定采集到的当前第n个数据帧（记为当前数据帧，也即采集到的最新的数据帧）的位姿；

其中，滑窗BA优化处理过程，可以包括：

按照以下公式（可以记为公式1），对包括当前数据帧的m+1个数据帧进行滑窗BA优化处理：

X ^* =argmin（Res1+Res2+Res3+Res4+Res5+Res6）

滑窗BA优化公式中共涉及六项残差，分别对应括号中的六项，按照顺序依次分别表示：先验残差（即Res1）、IMU预积分残差（即Res2）、视觉（即图像）重投影残差（即Res3）、轮速里程计预积分残差（即Res4）、激光残差（即Res5）、以及UWB残差（即Res6）；

每项中双竖线右下角的符号表示对应项的噪声协方差，该噪声协方差的逆即为信息矩阵，代表该项残差约束的可信度。

其中，在上述公式1中，各符号所表示的含义如下：

X*表述滑窗优化的状态（状态也可以称之为系统状态），可以包含优化后的各帧位姿、速度、各传感器的外参、以及IMU偏置等；

Res1表示被边缘化的图像帧的先验残差，γ _p 表示先验残差的线性化残差，H _p 表示先验残差的一阶导数矩阵（即Jacobian矩阵）；

Res2表示滑窗中所有IMU预积分的约束（约束可以理解为残差），

表示第k帧和第k+1帧之间的IMU预积分约束，B表示具备有效IMU预积分的图像帧的列表，

表示IMU预积分的协方差，

表示第k帧和第k+1帧之间的IMU预积分量；

Res3表示滑窗中所有视觉重投影误差的约束，C表示滑窗中图像帧的列表，

表示滑窗中单个特征点的重投影误差，(j, l)表示在滑窗中第l帧中观测到 的第j个特征点，

表示第j个特征点在第l帧的观测值，

表示视觉观测的协方差；该 项残差在坐标转换中需要将特征点从视觉坐标系通过转换关系（即

）转换到IMU坐标系；

Res4表示滑窗中所有轮速里程计预积分的约束，E表示具备有效轮速里程计预积 分的图像帧的列表，

表示第k帧和第k+1帧之间的轮速里程计预积分约束，

表示该项预积分的观测协方差，

表示第k帧和第k+1帧之间的轮速里程计预积 分观测量；该项残差在坐标转换中需要将轮速里程计位姿从轮速里程计坐标系通过转换关 系（即

）转换到IMU坐标系；

Res5表示滑窗中所有激光位姿的约束，L表示具备有效激光位姿的图像帧的列表，

表示第k帧和第k+1帧之间的激光位姿相对约束，

表示该项激光相 对位姿的观测协方差，

表示第k帧和第k+1帧之间的激光相对位姿的观测值；该项残差 在坐标转换中需要将激光位姿从激光坐标系通过转换关系（即

）转换到IMU坐标系；

Res6表示滑窗中所有UWB位姿的约束，U表示具备有效UWB位姿的图像帧的列表，

表示第k帧的UWB位姿的全局约束，

表示该项UWB位姿的观测协方差，

表示第k帧的UWB位姿的观测值；该项残差在坐标转换中需要将UWB位姿从UWB坐标系通 过转换关系（即

）转换到IMU坐标系。

具体地，在本发明实施例中，涉及到了多个传感器，例如：图像采集设备、IMU、激光、轮速里程计和UWB；在上述公式1中，需要将这些传感器对应的数据通过外参转换到同一个坐标系下，再计算各传感器数据对应的残差。

并且，在上述坐标的转换过程中，可以先通过离线标定的方式，确定出转换外参的初始值，然后再利用上述公式1对确定出的初始值、位姿等其他状态信息进行不断地优化，最终可以在线实时确定出各传感器坐标系的最优外参和各数据帧的位姿。

因此，通过上述公式1，可以实时确定出当前数据帧的位姿和各传感器坐标系的外参。

步骤2、确定当前数据帧为关键帧；

其中，在确定当前数据帧是否为关键帧时，可以根据帧与帧之间的平均视差确定。

需要说明的是，如果确定出当前数据帧不是关键帧时，可以依然执行上述步骤1，只是在滑窗BA优化处理之后将当前数据帧丢掉；或者，如果确定出当前数据帧是关键帧时，那么则可以对当前数据帧进行后续处理。

在此步骤2中，在筛选关键帧时，需要保证在后续滑窗处理过程中具有快速性和鲁棒性等特点，以提高滑窗处理结果的有效性。

步骤3、根据边缘化策略，对预设的滑窗中的数据帧进行边缘化处理，并将当前关键帧加入至滑窗中。

其中，可以假设滑窗中最多可以包括m个数据帧，若在边缘化处理之前，滑窗中已经包括m个数据帧时，此时将当前关键帧加入至滑窗中后，会导致滑窗中包括的数据帧的数量超出预设的m个，所以在将当前关键帧加入至滑窗中之前，需要对滑窗进行边缘化处理。

具体地，可以通过边缘化策略，从滑窗中包括的m个数据帧中删除掉一个数据帧，然后再将当前关键帧加入中滑窗中，使得滑窗中始终包括m个数据帧。

其中，边缘化策略包括：

对于滑窗中的m个数据帧，分别占据第1位置、第2位置、…、第m位置；

根据占据第m-1位置的数据帧与占据第m位置的数据帧的视差量，确定占据第m-1位置的数据帧是否为关键帧；

其中，如果视差量较小，说明占据第m-1位置的数据帧是关键帧，如果视差量较大，说明占据第m-1位置的数据帧不是关键帧。因此：

若占据第m-1位置的数据帧是关键帧，则将占据第1位置的数据帧剔除掉（即剔除掉最早获取的数据帧），并将占据第2位置的数据帧移动至第1位置、将占据第3位置的数据帧移动至第2位置、…、将占据第m位置的数据帧移动至第m-1位置，使得滑窗中第m位置被空出来；

若占据第m-1位置的数据帧不是关键帧，则将占据第m-1位置的数据帧剔除掉，此时说明当前关键帧与剔除掉的这一数据帧很相似，即当前关键帧与路标点之间的约束与剔除的这一数据帧与路标点之间的约束很接近，直接剔除掉并不会导致整个约束关系丢失很多信息，所以可以直接将这一数据帧剔除掉；说明一点，虽然剔除掉了这一数据帧，但需要保留这一数据帧的预积分数据，从而保证下一帧预积分的连贯性；同时，将占据第m位置的数据帧移动至第m-1位置，使得滑窗中第m位置被空出来。

因此，通过上述边缘化过程，可以空出滑窗中的第m位置，再将当前关键帧加入至滑窗中时，可以使得当前关键帧占据滑窗中的第m位置，使得滑窗中始终保持m个数据帧。

并且，使得滑窗中始终包含m帧数据帧，且保证滑窗中的数据均为最新的数据帧图像，以有利于保证后续定位更加精准；并且，还可以减少计算量，降低计算成本。

此外，对滑窗进行边缘化处理，利用边缘化处理可以去掉滑窗中的最老帧或者次新帧，目的是希望不再计算这一帧，以减少计算量，同时通过舒尔补操作等方式，保留该帧对滑窗内其他帧状态的约束关系。

在实际情况中，在进行滑窗优化处理（也即进行滑窗BA优化处理）时，因涉及到的传感器的种类较多，可能会出现某个传感器工作异常的情况，此时需要考虑各传感器的工作状态，确定不同工况的状态，以使得滑窗优化处理更加准确，从而更加准确地确定出当前关键帧的位姿。

因此，具体地，在本发明实施例中，对每一个图像帧进行滑窗优化处理，具体包括：

根据IMU数据、轮速里程计数据、图像数据和激光数据，确定当前所处的工况状态；其中，工况状态包括：IMU失效工况、轮速里程计失效工况、图像失效工况、以及激光失效工况；

根据确定出的当前所处的工况状态、匹配结果、状态数据、以及中间位姿，对每一个图像帧进行滑窗优化处理。

具体地，在本发明实施例中，根据确定出的当前所处的工况状态、匹配结果、状态数据、以及中间位姿，对每一个图像帧进行滑窗优化处理，具体包括：

在确定出当前处于IMU失效工况时，根据匹配结果、中间位姿、以及状态数据中激光位姿信息、轮速里程计的角度和位移的积分量信息，对每一个图像帧进行滑窗优化处理；

或，确定出当前处于轮速里程计失效工况时，根据匹配结果、中间位姿、以及状态数据中激光位姿信息、IMU角度和位移的积分量信息，对每一个图像帧进行滑窗优化处理；

或，确定出当前处于激光失效工况时，根据匹配结果、中间位姿、以及状态数据中IMU角度和位移的积分量信息、轮速里程计的角度和位移的积分量信息，对每一个图像帧进行滑窗优化处理；

或，确定出当前处于图像失效工况时，根据状态数据、以及中间位姿，对每一个图像帧进行滑窗优化处理。

如此，可以根据不同的工况，对滑窗优化处理过程进行相应地处理，使得滑窗优化处理结果更加准确，从而使得确定出的当前关键帧的位姿更加准确。

下面分别对各工况进行介绍。

1、轮速里程计失效工况：

1.1、工况判断

在根据边缘化策略，对滑窗中的m个数据帧进行边缘化处理后，滑窗中保留了m-1个数据帧；

此时，基于滑窗中m-1个数据帧的位姿，根据得加IMU预积分的增量，可以得到当前图像帧的位姿的初始值；

根据该初始值，确定轮速里程计预积分的马氏距离D1，若马氏距离D1大于阈值1、则认为轮速里程计失效。

其中，最常见的轮速里程计失效的情况为：轮子打滑。

1.2相关处理

当判断出轮速里程计失效后，将公式1中去掉轮速里程计预积分残差（即Res4）后，可以且不限于为以下公式2：

X ^* =argmin（Res1+Res2+Res3+Res5+Res6）

当马氏距离D1不大于阈值1时，则可以认为判断轮速里程计没有失效，此时按上述公式1进行优化处理。

2、激光失效工况：

2.1、工况判断

在根据边缘化策略，对滑窗中的m个数据帧进行边缘化处理后，滑窗中保留了m-1个数据帧；

此时，基于滑窗中m-1个数据帧的位姿，根据得加IMU预积分的增量，可以得到当前图像帧的位姿的初始值；

根据该初始值，确定通过激光获取到的连续两个图像帧之前的马氏距离D2，若马氏距离D2大于阈值2、则认为激光失效。

其中，常见的激光失效的情况包括：激光扫描匹配时瞬时失效，例如长走廊、空旷场地等。

2.2、相关处理

当判断出激光失效后，将公式1中去掉激光残差（即Res5）后，可以且不限于为以下公式3：

X ^* =argmin（Res1+Res2+Res3+Res4+Res6）

当马氏距离D2不大于阈值2时，则可以认为判断激光没有失效，此时按上述公式1进行优化处理。

3、视觉失效（即图像失效）工况：

3.1、工况判断

当滑窗中视觉跟踪的特征点相关指标不符合预设条件时，则可以认为视觉失效；

其中，预设条件包括：

连续N个图像中的匹配特征点的数量少于阈值3，N为大于1的整数；

和/或，当前图像帧中的特征点的数量少于阈值4；

和/或，特征点在不同图像帧之间的重投影误差大于阈值5。

其中，视觉失效的情况包括：当无人车处于弱纹理环境中、环境光变化或发生剧烈运动等情况。

3.2、相关处理

当判断出视觉失效后，将公式1中去掉视觉重投影残差（即Res3）后，可以且不限于为以下公式4：

X ^* =argmin（Res1+Res2+Res4+Res5+Res6）

4、IMU失效工况：

4.1、工况判断

滑窗中占据相邻位置的两个数据帧对应的获取时间之间的时间间隔大于阈值6，时，则可以认为IMU失效。

其中，IMU失效的情况包括：当无人车长期处于静止状态时，由于IMU的加速度计和陀螺仪的偏置存在，此时使得偏置增加。

4.2、相关处理

当判断出IMU失效后，将公式1中去掉IMU预积分残差（即Res2）后，可以且不限于为以下公式5：

X ^* =argmin（Res1+Res3+Res4+Res5+Res6）

综上，上面给出了四种传感器在分别失效时的情况以及处理方式，以便于适用于特殊工况，提高最终机器人的定位精度。

需要说明的是，可选地，若IMU中的偏置预测值的变化量大于阈值6时，此时可以不采用IMU预积分值去判断轮速里程计失效和激光失效这两种特殊工况。

S102、根据当前处于滑窗中图像帧的位姿、以及确定出的与当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对当前关键帧的位姿进行滑窗闭环优化处理，得到处理后的当前关键帧的位姿；

其中，为了实现步骤S102，需要首先确定出与当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，因此：

可选地，在本发明实施例中，确定与当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，具体包括：

在确定出已确定出的多个图像关键帧中与当前关键帧处于候选闭环匹配关系的候选关键帧时，确定当前关键帧与候选关键帧之间的图像相对位姿，并将图像相对位姿小于第一预设值对应的候选关键帧，确定为与当前关键帧处于闭环匹配状态的图像闭环关键帧；

确定当前关键帧与已确定出的多个激光关键帧之间的激光相对位姿、以及当前关键帧与多个激光关键帧之间的点云匹配程度，并将激光相对位姿小于第二预设值、和/或点云匹配程度大于第三预设值对应的激光关键帧，确定为与当前关键帧处于闭环匹配状态的激光闭环关键帧。

如此，通过上述方式，可以确定出当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，以便于后续进行滑窗闭环优化处理，对当前关键帧的位姿进行更新修正处理，得到更加准确地当前关键帧的位姿，从而有利于更加准确地定位出机器人的位姿。

需要说明的是，在滑窗闭环优化处理时，不仅对当前关键帧的位姿进行优化，同时还可以对获取当前关键帧之前的就已经确定出的关键帧（当前关键帧之前的就已经确定出的关键帧，可以称之为老帧）的位姿进行优化，从而实现了基于激光和视觉的全局闭环优化，以实现更新所有关键帧的位姿，得到全局一致的最优位姿。

其中，在确定与当前关键帧处于闭环匹配状态的图像闭环关键帧时，具体过程可以包括：

根据预设的视觉闭环检测方法，分别对当前关键帧与预设的数据库中存储的每一个图像关键帧进行视觉闭环检测，确定出与当前关键帧处于候选闭环匹配关系的候选关键帧；其中，候选关键帧可以理解为：可能与当前关键帧存在闭环匹配关系的图像关键帧；

确定当前关键帧与候选关键帧之间的第一相对位姿（也即前述内容中提及的图像相对位姿）；

将第一相对位姿小于第一预设值对应的候选关键帧，确定为与当前关键帧处于闭环匹配状态的图像闭环关键帧。

其中，第一预设值的具体设置，可以根据实际情况进行设置，在此并不限定。

并且，在计算第一相对位姿时，可以采用预设PNP算法进行计算，且具体的计算过程可参见现有技术，在此不再详述。

说明一点，第一数据库中的每一个图像关键帧即为在这之前已经确定出的关键帧，所以由之前已经确定出的关键帧构成了第一数据库。

在确定当前关键帧处于闭环匹配状态的激光闭环关键帧时，具体过程可以包括：

根据预设的激光闭环检测方法，分别对当前关键帧与预设的第二数据库中存储的每一个激光关键帧进行激光闭环检测，确定出当前关键帧与每一个激光关键帧之间的第二相对位姿（即前述内容中提及的激光相对位姿）；

将满足预设规则的第二相对位姿对应的激光关键帧，确定出为当前关键帧处于闭环匹配状态的激光闭环关键帧；

其中，预设规则可以包括：

第二相对位姿小于第二预设值；

和/或，当前关键帧与多个激光关键帧之间的点云匹配程度大于第三预设值。

具体地，第二预设值和第三预设值均可以根据实际情况进行设置，在此并不限定。

需要说明的是，可选地，点云匹配程度可以理解为：

当前关键帧为通过激光采集到的一组轮廓点云（记为点云1），激光关键帧同样为通过激光采集到的一组轮廓点云（记为点云2），点云1和点云2中的点的匹配程度。

还有，第二数据库中的激光关键帧可以采用以下方式确定：

例如但并不限于，假设激光数据的采集频率为30Hz，即1s可以采集30帧数据，同时假设装载有激光的机器人的移动速度为1s移动0.1m，那么：

可以将每移动0.1m采集到的一帧数据定义为激光关键帧，那么其余29帧数据则不是激光关键帧。

还需要注意的是，可选地，视觉闭环检测（即确定与当前关键帧处于闭环匹配状态的图像闭环关键帧的过程）与激光闭环检测（即确定与当前关键帧处于闭环匹配状态的激光闭环关键帧的过程），为两个互不影响，相互独立的过程，所以这两个过程可以并行进行，还可以串行进行，可以根据实际需要进行设计，在此并不限定。

可选地，在本发明实施例中，根据当前处于滑窗中图像帧的位姿、以及确定出的与当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对当前关键帧的位姿进行滑窗闭环优化处理，具体包括：

采用以下公式（即公式6），对当前关键帧的位姿进行滑窗闭环优化处理：

X ^* =argmin（Res1+Res2+Res3+Res4+Res5+Res6+Res7+Res8）

其中，

表示被边缘化的图像帧的先验残差，γ _p 表示先验残差的线性化 残差，H _p 表示先验残差的一阶导数矩阵，

表示第k个数据帧和第k+1个数据帧 之间的IMU预积分约束，

表示第k个数据帧和第k+1个数据帧之间的IMU预积分量，

表示第j个特征点在第l个数据帧中的观测值，

表示该IMU预积分的协方差，

表示视觉观测的协方差，X ^* 表示滑窗中优化的状态，B表示具备有效IMU预积分的图像帧的 列表，C表示滑窗中图像帧的列表，

表示滑窗中单个特征点的重投影误差，

表示第k个数据帧和第k+1个数据帧之间的轮速里程计预积分约束，

表示该项轮速里程计预积分的观测协方差，

表示第k个数据帧和第k+1个数据帧之间 的轮速里程计预积分观测量，E表示具备有效轮速里程计预积分的图像帧的列表，

表示第k个数据帧和第k+1个数据帧之间的激光位姿相对约束，

表 示该项激光相对位姿的观测协方差，

表示第k个数据帧和第k+1个数据帧之间的激光 相对位姿的观测值，L表示具备有效激光位姿的图像帧的列表，

表示第k个数 据帧的UWB位姿的全局约束，

表示该项UWB位姿的观测协方差，

表示第k个数据帧 的UWB位姿的观测值，U表示具备有效UWB位姿的图像帧的列表，

表示当前滑窗中特征点和图像闭环关键帧之 间的重投影残差（即滑窗优化中的视觉闭环约束，且该约束包含了图像特征点在图像坐标 系和IMU坐标系之间的外参转换），

表示与当前关键帧处于闭环 匹配状态的图像闭环关键帧中的单个特征点的重投影误差，M表示当前滑窗中和图像闭环 关键帧中匹配成功的特征点组合，(w, v)表示在第v个数据帧上的第w个特征点，

表示第w个特征点在第v个数据帧的观测值，

和

分别表示图像闭环关键帧的姿态和位置，

表示当前滑窗中的关键帧和激光闭环关键帧 之间的位姿残差（即滑窗优化中的激光闭环约束，且该约束包含了激光位姿在激光坐标系 和IMU坐标系之间的外参转换），

表示单个激光闭环关键帧位姿 的残差，N表示当前滑窗中和激光闭环关键帧中匹配成功的关键帧组合，(z, v)表示滑窗中 第z个数据帧和第v个数据帧构成了激光闭环，

表示滑窗中第z个数据帧和第v个数据帧 之间的位姿约束，ρ(.)表示鲁棒核函数；UWB位姿为根据中间位姿确定。

也就是说，在进行视觉闭环检测和激光闭环检测之后，首先需要将视觉闭环约束和激光闭环约束加入至滑窗中，以便于进行滑窗闭环优化处理，实现对当前关键帧的位姿的快速重定位，从而在将过往位姿看作是固定值时，快速有效地更新修正当前滑窗。

说明一点，在滑窗闭环优化过程中，即滑窗快速度重定位过程中，可以将闭环老帧 （即较早采集到的数据帧）作为常值【即

】。

若当前滑窗中存在多个闭环约束，可以同时约束上述方程，进而可以得到多源多视角的约束，从而得到更加准确和更加平滑的位姿精度。

S103、根据处理后的当前关键帧的位姿，确定机器人的位姿；

如此，通过结合IMU、轮速里程计、图像、激光和UWB技术，可以实现多传感器的融合，多方位全方面的得到机器人的当前关键帧的位姿，并且，在确定出与当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧后，可以对当前关键帧的位姿进行实时地滑窗闭环优化处理，以实现对确定出的当前关键帧的位姿进行快速重定位，以便于提高机器人实时定位的速度和精度，同时还可以提高机器人定位的速度，从而提高机器人定位的效率。

需要说明的是，在本发明实施例中，在实现机器人定位时，是基于外参（即各传感器之间的外参）在线优化实现的机器人定位，且在上述内容提及的滑窗BA优化处理过程、滑窗闭环优化过程、以及后续内容中提及的位姿图优化过程中，在实现对位姿优化的同时，也均会对外参进行优化，以使得机器人定位的准确性更高更有效。

可选地，在本发明实施例中，还包括：

根据已经确定出的全部关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、已经确定出的全部关键帧与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、已经确定出的全部关键帧的位姿、以及每个图像关键帧对应的UWB位姿，对各图像关键帧进行位姿图优化处理，得到处理后的各图像关键帧的位姿；

其中，UWB位姿为根据中间位姿确定。

说明一点，可选地，在本发明实施例中，已经确定出的全部关键帧可以包括：当前关键帧、以及按照多传感器融合算法，在确定出当前关键帧之前就已经确定出的各关键帧。

如此，通过全局位姿图优化处理，可以对第一数据库中的各图像关键帧进行优化处理，用于更新所有图像关键帧的位姿，得到全局一致的最优位姿和轨迹。

具体地，在本发明实施例中，对各图像关键帧进行位姿图优化处理，具体包括：

采用如下公式（可以记为公式7），对各图像关键帧进行全局位姿图优化处理：

Y ^* =argmin（Err1+Err2+Err3+Err4）

其中，S表示位姿图优化处理过程中所有从滑窗中得到的相邻两个数据帧之间的 相对位姿约束的组合，i表示第i个图像关键帧，j表示第j个图像关键帧，r _i,j 表示第i个图像 关键帧与第j个图像关键帧之间的残差，

表示从滑窗中得到的相邻两个数据帧之间的 相对位姿约束的协方差，L _U 表示位姿图优化处理过程中所有具有UWB位姿的关键帧组合，r _k 表示第k个关键帧具有UWB位姿的残差（该残差包含了UWB坐标系和IMU坐标系之间的外参），

表示UWB位姿约束的协方差，L _V 表示位姿图优化处理过程中所有视觉闭环关键帧的 组合，r _m,n 表示第m个关键帧和第n个关键帧形成视觉闭环匹配时的位姿约束，

表示第m个关键帧和第n个关键帧形成视觉闭环匹配时位姿约束的协方差，L _L 表示位姿图优化处理 过程中所有激光闭环关键帧的组合，r _g,h 表示第g个关键帧和第h个关键帧形成激光闭环匹 配时的位姿约束（该位姿约束还可以称之为位姿约束残差，该残差包含了激光坐标系和IMU 坐标系之间的外参），

表示第g个关键帧和第h个关键帧形成激光闭环匹配时位姿约 束的协方差，Y ^* 表示经过位姿图优化处理的状态。

其中，从滑窗中得到的相邻两个数据帧之间的相对位姿约束，可以理解为序列边约束，(m, n)表示第m个关键帧和第n个关键帧构成了视觉闭环约束，(g, h)表示第g个关键帧和第h个关键帧构成了激光闭环约束。

并且，经过位姿图优化处理后的状态Y ^* 可以包括：各图像关键帧的位姿、激光坐标系和IMU坐标系之间的外参、UWB坐标系和IMU坐标系之间的外参。

也就是说，为了达到输出位姿和轨迹的全局一致性，后端处理过程中维护了一个大规模的全局位姿图来约束和优化全局姿态。

可选地，在本发明实施例中，对各图像关键帧进行位姿图优化处理，具体包括：

判断当前关键帧与对应的图像闭环关键帧是否闭环匹配有效；若有效，则根据已经确定出的全部关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、已经确定出的全部关键帧与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、已经确定出的全部关键帧的位姿、以及每个图像关键帧对应的UWB位姿，各图像关键帧进行位姿图优化处理；若无效，则根据已经确定出的全部关键帧中除当前关键帧之外的其余各关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、已经确定出的全部关键帧中与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、已经确定出的全部关键帧的位姿、以及每个图像关键帧对应的UWB位姿，对各图像关键帧进行位姿图优化处理；

和/或，判断当前关键帧与对应的激光闭环关键帧是否闭环匹配有效；若有效，则根据已经确定出的全部关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、已经确定出的全部关键帧与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、已经确定出的全部关键帧的位姿、以及每个图像关键帧对应的UWB位姿，对各图像关键帧进行位姿图优化处理；若无效，则根据已经确定出的全部关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、已经确定出的全部关键帧中除当前关键帧之外的其余各关键帧与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、已经确定出的全部关键帧的位姿、以及每个图像关键帧对应的UWB位姿，对各图像关键帧进行位姿图优化处理；

同时，对当前关键帧的位姿进行滑窗闭环优化处理，具体包括：

判断当前关键帧与对应的图像闭环关键帧是否闭环匹配有效；若有效，则根据当前处于滑窗中图像帧的位姿、以及确定出的与当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对当前关键帧的位姿进行滑窗闭环优化处理；若无效，则根据当前处于滑窗中图像帧的位姿、以及确定出的与当前关键帧处于闭环匹配状态的激光闭环关键帧，对当前关键帧的位姿进行滑窗闭环优化处理；

和/或，判断当前关键帧与对应的激光闭环关键帧是否闭环匹配有效；若有效，则根据当前处于滑窗中图像帧的位姿、以及确定出的与当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对当前关键帧的位姿进行滑窗闭环优化处理；若无效，则根据当前处于滑窗中图像帧的位姿、以及确定出的与当前关键帧处于闭环匹配状态的图像闭环关键帧，对当前关键帧的位姿进行滑窗闭环优化处理。

也就是说，在进行位姿图优化处理和滑窗闭环优化处理之前，均需要判断当前关键帧与对应的图像闭环关键帧之间的视觉匹配是否有效，以及判断当前关键帧与对应的激光闭环关键帧是否闭环匹配有效。

具体地，可以包括：

1、对于视觉闭环检测而言：

判断当前关键帧与对应的图像闭环关键帧之间的视觉匹配是否有效；

若有效，则在进行位姿图优化处理时，可以在公式7中包括图像闭环约束，即包括“当前关键帧与其处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿”这一项，并且，在进行滑窗闭环优化处理时，可以在公式6中可以包括当前关键帧与其处于闭环匹配状态的图像闭环关键帧之间的图像闭环约束（即重投影误差的约束）；

若无效，则在进行位姿图优化处理时，在公式7中去掉图像闭环约束这一项，即去掉“当前关键帧与其处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿”这一项，并且，在进行滑窗闭环优化处理时，可以在公式6中可以去掉当前关键帧与其处于闭环匹配状态的图像闭环关键帧之间的图像闭环约束（即重投影误差的约束）。

其中，在判断当前关键帧与对应的图像闭环关键帧之间的视觉匹配是否有效时，具体过程可以包括：

当检测到第i个关键帧和第j个关键帧（其中i大于j）存在视觉闭环匹配后，可以采用PnP（Perspective-N-Point)算法求解第i个关键帧相对于第j个关键帧之间的相对位姿，再叠加上第j个关键帧的全局位姿，就可以得到第i个关键帧的全局位姿的视觉闭环理论值；

将该视觉闭环全局位姿的视觉闭环理论值和UWB全局位姿进行求差操作，得到位置差和角度差；

如果得到的角度差超过阈值7（该阈值7可以根据UWB不确定性的角度协方差确定），和/或，位置差超过阈值8（该阈值8可以根据UWB不确定性的位置协方差），即认为该本次视觉匹配无效；

如果得到的角度差小于阈值7，和位置差小于阈值8，即认为该本次视觉匹配有效。

2、对于激光闭环检测而言：

判断当前关键帧与对应的激光闭环关键帧之间的激光匹配是否有效；

若有效，则在进行位姿图优化处理时，在公式7中包括激光闭环约束，即包括“前关键帧与其处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿”这一项；并且，在进行滑窗闭环优化处理时，可以在公式6中可以包括当前关键帧与其处于闭环匹配状态的激光闭环关键帧之间的激光闭环约束（即激光位姿相对约束）；

若无效，则在进行位姿图优化处理时，在公式7中去掉激光闭环约束这一项，即去掉“前关键帧与其处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿”这一项；并且，在进行滑窗闭环优化处理时，可以在公式6中可以去掉当前关键帧与其处于闭环匹配状态的激光闭环关键帧之间的激光闭环约束（即激光位姿相对约束）。

其中，在判断当前关键帧与对应的激光闭环关键帧之间的激光匹配是否有效时，具体过程可以包括：

当检测到第i个关键帧和第j个关键帧（其中i大于j）存在激光闭环匹配后，可以采用ICP算法求解第i个关键帧相对于第j个关键帧之间的相对位姿，再叠加上第j个关键帧的全局位姿，就可以得到第i个关键帧的全局位姿的激光闭环理论值；

将该激光闭环全局位姿的激光闭环理论值和UWB全局位姿进行求差操作，得到位置差和角度差；

如果得到的角度差超过阈值9（该阈值9可以根据UWB不确定性的角度标准差确定），和/或，位置差超过阈值10（该阈值10可以根据UWB不确定性的位置标准差），即认为激光匹配无效；

如果得到的角度差小于阈值9，和位置差小于阈值10，即认为激光匹配有效。

由于在存在重复轮廓、单一轮廓(如长走廊等)情况，使得激光闭环匹配结果容易出现错误的闭环；同样地，在出现相同或相似的图案时，视觉闭环匹配结果同样容易出现错误的闭环；这样错误的闭环在实际应用危害很大。

因此，通过对闭环匹配是否有效进行判断，实现了对视觉闭环检测和激光闭环匹配检测进行了二次校验，使得在后续进行位姿图优化处理时采用相应的处理措施，避免因闭环匹配无效而造成位姿图优化结果出现偏差，从而有利于进一步提高后续确定出的关键帧的位姿，进而进一步地提高机器人的定位精度。

说明一点，可选地，当运行时间越来越长，数据库（即第一数据库）将变得越来越大，导致闭环检测（包括视觉闭环检测和激光闭环检测）和闭环优化（包括滑窗闭环优化和位姿图优化）的耗时也会越来越长。虽然在前面介绍的内容中已经仅保留每个图像帧的位姿和特征点描述子，已扔掉原始图像，但是当运行几小时后，仍将难以做到实时的机器人定位。

因此，可以根据分布密度对数据库（即第一数据库）进行下采样，保留那些与周围帧的位置和角度不同的关键帧，而丢弃集中在一起的关键帧，进而减少数据库包括的图像关键帧的数量，减少闭环检测和闭环优化的处理时间，提高处理效率，从而实现机器人的实时准确地定位。

可选地，在本发明实施例中，对每一个图像帧进行滑窗优化处理后，确定出当前关键帧的位姿，具体包括：

对每一个图像帧进行滑窗优化处理后，得到优化后的系统状态；其中，系统状态包括：IMU、轮速里程计、激光、图像采集设备和UWB之间的外参；

根据优化后的系统状态，确定出当前关键帧的位姿。

当然，系统状态除了可以包括外参之外，还可以包括：数据帧的位姿、机器人的速度、IMU偏置、以及特征点深度等信息。

在实际情况中，在前端跟踪部分（包括各传感器采集数据、对采集到的数据进行预处理、以及确定当前关键帧的位姿这些过程），其位姿的输出频率取决于图像帧的采集频率。

为了进一步提高位姿的输出频率，并降低计算量，可以采取以下措施：

在前端跟踪部分解算后，且在新的图像帧来临之前，采用IMU预积分相对位姿，在上一个图像帧的位姿的基础上，进行递推得到IMU频率的高频位姿。

其中，IMU预积分相对位姿可以理解为：

先对IMU数据进行预积分，然后计算预积分结果与上一个图像帧的位姿之间的相对位姿。

或者，如果IMU不存在或失效时，还可以采用以下方式：

采用轮速里程计的预积分相对位姿，在上一个图像帧的位姿的基础上，进行递推得到轮速里程计频率的高频位姿。

其中，轮速里程计的预积分相对位姿可以理解为：

先对角速度/线速度进行预积分，然后计算预积分结果与上一个图像帧的位姿之间的相对位姿。

也就是说，可以将输出相邻两个数据帧之间的时间定义为增加时间，然后根据增加时间内IMU预积分相对位姿或轮速里程计的预积分的相对位姿、以及输出的上一个数据帧的位置信息，确定增加时间内补充数据帧的位置信息；最后，将补充数据帧插入至待输出的两个数据帧之间时，将插入补充数据帧之后的结果输出，从而提高了数据帧的输出频率，进而提高了位姿的输出频率。

在具体实施时，在确定机器人的位姿时，可以根据适用场景的不同，采用以下两种方式：

方式1：适用于低频输出场景。

其中，可以根据以下两种数据确定机器人的最终位姿，两种数据包括：

A、当前关键帧在经过滑窗优化后的位姿；

B、最近时刻采集到的图像帧的位姿。

其中，最近时刻采集到的图像帧的位姿，具体为：

最近时刻采集到的图像帧在经过滑窗优化处理后得到的位姿。

在根据上述两种数据确定最终位姿时，具体过程可以但不限于为：

通过当前关键帧在经过滑窗优化后的位姿，可以确定出在进行滑窗优化处理前后产生的位姿增量；

将产生的位姿增量与最近时刻采集到的图像帧的位姿进行叠加，得到机器人的最终位姿，从而实现机器人定位。

方式2：适用于高频输出场景。

其中，可以根据以下三种数据确定机器人的最终位姿，三种数据包括：

A、当前关键帧在经过滑窗优化后的位姿；

B、最近时刻采集到的图像帧的位姿；

C、最近采集到的IMU数据与最近采集到的图像数据之间的预积分结果。

其中，在C中，因IMU的采集频率高于轮速里程计的采集频率大于图像的采集频率，所以利用预积分结果，可以增加位姿的输出频率，从而实现高频输出，使得最终确定出的机器人的位姿更加准确和实时。

说明一点，最近采集IMU数据的时间晚于最近采集图像数据的时间。

也就是说，在此方式2中，将方式1中得到的最终位姿与高频递推过程中得到的增加时间内补充数据帧的位姿进行组合，从而输出高频的位姿，确定机器人的位姿，提高机器人定位的实时性和有效性。

下面结合具体实施例对本发明实施例提供的定位方法进行说明。

结合图2所示。

S201、通过IMU、轮速里程计、相机、以及激光采集第n个数据帧，并分别对采集到的数据进行预处理，得到初始数据；以及，通过UWB技术，确定机器人的中间位姿；

S202、根据不同工况以及上述确定出的初始数据和中间位姿，按照滑窗BA优化处理算法，确定第n个数据帧的位姿，并在确定出第n个数据帧为当前关键帧时，根据边缘化策略，对预设的滑窗中的m个数据帧进行边缘化处理，再将当前关键帧加入至滑窗中；

S203、根据预设的视觉闭环检测方法和激光闭环检测方法，确定与当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧；

S204、根据当前关键帧与对应图像闭环关键帧之间是否闭环匹配有效、当前关键帧与对应图像闭环关键帧之间的图像相对位姿、当前关键帧与对应激光闭环关键帧之间的激光相对位姿、以及当前处于滑窗中数据帧的位姿，对当前关键帧进行滑窗闭环优化处理，得到处理后的当前关键帧的位姿；以及，根据当前关键帧与对应图像闭环关键帧之间是否闭环匹配有效、已经确定出的全部关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、已经确定出的全部关键帧与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、已经确定出的全部关键帧的位姿、以及每个图像关键帧对应的UWB位姿，对各图像关键帧进行位姿图优化处理，得到处理后的各图像关键帧的位姿；

S205、根据处理后的当前关键帧的位姿，确定机器人的位姿。

基于同一发明构思，本发明实施例提供了一种机器人的定位装置，该定位装置的具体实现原理与前述一种定位方法的实现原理类似，该定位装置的具体实施方式可参见前述定位方法的具体实施例，重复之处不再赘述。

具体地，本发明实施例提供了一种机器人的定位装置，如图3所示，包括：

第一单元301，用于根据机器人当前采集到的惯性测量单元IMU数据、轮速里程计数据、图像数据和激光数据、以及通过超宽带通信UWB技术确定出的机器人的中间位姿，按照预设的多传感器融合算法，确定当前关键帧及当前关键帧的位姿；

第二单元302，用于根据当前处于滑窗中图像帧的位姿、以及确定出的与当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对当前关键帧的位姿进行滑窗闭环优化处理，得到处理后的当前关键帧的位姿；

第三单元303，用于根据处理后的当前关键帧的位姿，确定机器人的位姿；

其中，多传感器融合算法包括：根据确定出的机器人采集到的每一帧图像中角点的匹配结果、采集到的所有连续两帧图像得到的状态数据、以及中间位姿，对每一个图像帧进行滑窗优化处理后，确定出当前关键帧的位姿；状态数据包括：激光位姿信息、IMU角度和位移的积分量信息、轮速里程计的角度和位移的积分量信息。

可选地，在本发明实施例中，第一单元301，具体用于：

根据IMU数据、轮速里程计数据、图像数据和激光数据，确定当前所处的工况状态；其中，工况状态包括：IMU失效工况、轮速里程计失效工况、图像失效工况、以及激光失效工况；

根据确定出的当前所处的工况状态、匹配结果、状态数据、以及中间位姿，对每一个图像帧进行滑窗优化处理。

可选地，在本发明实施例中，第一单元301，具体用于：

在确定出当前处于IMU失效工况时，根据匹配结果、中间位姿、以及状态数据中激光位姿信息、轮速里程计的角度和位移的积分量信息，对每一个图像帧进行滑窗优化处理；

或，确定出当前处于轮速里程计失效工况时，根据匹配结果、中间位姿、以及状态数据中激光位姿信息、IMU角度和位移的积分量信息，对每一个图像帧进行滑窗优化处理；

或，确定出当前处于激光失效工况时，根据匹配结果、中间位姿、以及状态数据中IMU角度和位移的积分量信息、轮速里程计的角度和位移的积分量信息，对每一个图像帧进行滑窗优化处理；

或，确定出当前处于图像失效工况时，根据状态数据、以及中间位姿，对每一个图像帧进行滑窗优化处理。

可选地，在本发明实施例中，第二单元302，具体用于：

在确定出已确定出的多个图像关键帧中与当前关键帧处于候选闭环匹配关系的候选关键帧时，确定当前关键帧与候选关键帧之间的图像相对位姿，并将图像相对位姿小于第一预设值对应的候选关键帧，确定为与当前关键帧处于闭环匹配状态的图像闭环关键帧；

确定当前关键帧与已确定出的多个激光关键帧之间的激光相对位姿、以及当前关键帧与多个激光关键帧之间的点云匹配程度，并将激光相对位姿小于第二预设值、和/或点云匹配程度大于第三预设值对应的激光关键帧，确定为与当前关键帧处于闭环匹配状态的激光闭环关键帧。

可选地，在本发明实施例中，第二单元302，还用于：

根据已经确定出的全部关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、已经确定出的全部关键帧与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、已经确定出的全部关键帧的位姿、以及每个图像关键帧对应的UWB位姿，对各图像关键帧进行位姿图优化处理，得到处理后的各图像关键帧的位姿；

其中，UWB位姿为根据中间位姿确定。

可选地，在本发明实施例中，第二单元302，具体用于：

判断当前关键帧与对应的图像闭环关键帧是否闭环匹配有效；若有效，则根据已经确定出的全部关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、已经确定出的全部关键帧与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、已经确定出的全部关键帧的位姿、以及每个图像关键帧对应的UWB位姿，各图像关键帧进行位姿图优化处理；若无效，则根据已经确定出的全部关键帧中除当前关键帧之外的其余各关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、已经确定出的全部关键帧中与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、已经确定出的全部关键帧的位姿、以及每个图像关键帧对应的UWB位姿，对各图像关键帧进行位姿图优化处理；

和/或，判断当前关键帧与对应的激光闭环关键帧是否闭环匹配有效；若有效，则根据已经确定出的全部关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、已经确定出的全部关键帧与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、已经确定出的全部关键帧的位姿、以及每个图像关键帧对应的UWB位姿，对各图像关键帧进行位姿图优化处理；若无效，则根据已经确定出的全部关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、已经确定出的全部关键帧中除当前关键帧之外的其余各关键帧与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、已经确定出的全部关键帧的位姿、以及每个图像关键帧对应的UWB位姿，对各图像关键帧进行位姿图优化处理；

第二单元302，具体用于：

判断当前关键帧与对应的图像闭环关键帧是否闭环匹配有效；若有效，则根据当前处于滑窗中图像帧的位姿、以及确定出的与当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对当前关键帧的位姿进行滑窗闭环优化处理；若无效，则根据当前处于滑窗中图像帧的位姿、以及确定出的与当前关键帧处于闭环匹配状态的激光闭环关键帧，对当前关键帧的位姿进行滑窗闭环优化处理；

和/或，判断当前关键帧与对应的激光闭环关键帧是否闭环匹配有效；若有效，则根据当前处于滑窗中图像帧的位姿、以及确定出的与当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对当前关键帧的位姿进行滑窗闭环优化处理；若无效，则根据当前处于滑窗中图像帧的位姿、以及确定出的与当前关键帧处于闭环匹配状态的图像闭环关键帧，对当前关键帧的位姿进行滑窗闭环优化处理。

可选地，在本发明实施例中，第一单元301，具体用于：

根据IMU数据中的速度数据和轮速里程计数据中的速度数据，将中间位姿插值到当前图像帧中，得到采集当前图像帧时刻对应的机器人的位姿；

将得到的采集当前图像帧时刻对应的机器人的位姿，确定为UWB位姿。

可选地，在本发明实施例中，第一单元301，具体用于：

对每一个图像帧进行滑窗优化处理后，得到优化后的系统状态；其中，系统状态包括：IMU、轮速里程计、激光、图像采集设备和UWB之间的外参；

根据优化后的系统状态，确定出当前关键帧的位姿。

基于同一发明构思，本发明实施例提供了一种机器人，如图4所示，可以包括：如本发明实施例提供的上述定位装置401。

可选地，在本发明实施例中，机器人除了可以包括定位装置401之外，还可以包括其他用于实现机器人功能的结构，具体可根据实际需要进行设计，在此不再详述。

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

Claims (10)

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

根据所述机器人当前采集到的IMU数据、轮速里程计数据、图像数据和激光数据、以及通过UWB技术确定出的所述机器人的中间位姿，按照预设的多传感器融合算法，确定当前关键帧及所述当前关键帧的位姿；

在确定出已确定出的多个图像关键帧中与当前关键帧处于候选闭环匹配关系的候选关键帧时，确定当前关键帧与所述候选关键帧之间的图像相对位姿，并将所述图像相对位姿小于第一预设值对应的所述候选关键帧，确定为与所述当前关键帧处于闭环匹配状态的图像闭环关键帧；

确定所述当前关键帧与已确定出的多个激光关键帧之间的激光相对位姿、以及所述当前关键帧与所述多个激光关键帧之间的点云匹配程度，并将所述激光相对位姿小于第二预设值、和/或所述点云匹配程度大于第三预设值对应的所述激光关键帧，确定为与所述当前关键帧处于所述闭环匹配状态的激光闭环关键帧；

根据当前处于滑窗中图像帧的位姿、以及确定出的与所述当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对所述当前关键帧的位姿进行滑窗闭环优化处理，得到处理后的所述当前关键帧的位姿；

根据处理后的所述当前关键帧的位姿，确定所述机器人的位姿；

其中，所述多传感器融合算法包括：根据确定出的所述机器人采集到的每一帧图像中角点的匹配结果、采集到的所有连续两帧图像得到的状态数据、以及所述中间位姿，对每一个图像帧进行滑窗优化处理后，确定出所述当前关键帧的位姿；所述状态数据包括：激光位姿信息、IMU角度和位移的积分量信息、轮速里程计的角度和位移的积分量信息。

2.如权利要求1所述的定位方法，其特征在于，根据当前处于滑窗中图像帧的位姿、以及确定出的与所述当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对所述当前关键帧的位姿进行滑窗闭环优化处理，具体包括：

采用以下公式，对所述当前关键帧的位姿进行滑窗闭环优化处理：

X ^* =argmin（Res1+Res2+Res3+Res4+Res5+Res6+Res7+Res8）

其中，

表示被边缘化的所述图像帧的先验残差，γ _p 表示先验残差的线性化残差，H _p 表示所述先验残差的一阶导数矩阵，

表示第k个数据帧和第k+1个数据帧之间的IMU预积分约束，

表示所述第k个数据帧和所述第k+1个数据帧之间的IMU预积分量，

表示第j个特征点在第l个数据帧中的观测值，

表示该IMU预积分的协方差，

表示视觉观测的协方差，X ^* 表示滑窗中优化的状态，B表示具备有效IMU预积分的图像帧的列表，C表示滑窗中图像帧的列表，

表示滑窗中单个特征点的重投影误差，

表示所述第k个数据帧和所述第k+1个数据帧之间的轮速里程计预积分约束，

表示轮速里程计预积分的观测协方差，

表示所述第k个数据帧和所述第k+1个数据帧之间的轮速里程计预积分观测量，E表示具备有效轮速里程计预积分的图像帧的列表，

表示所述第k个数据帧和所述第k+1个数据帧之间的激光位姿相对约束，

表示激光相对位姿的观测协方差，

表示所述第k个数据帧和所述第k+1个数据帧之间的激光相对位姿的观测值，L表示具备有效激光位姿的图像帧的列表，

表示所述第k个数据帧的UWB位姿的全局约束，

表示UWB位姿的观测协方差，

表示所述第k个数据帧的UWB位姿的观测值，U表示具备有效UWB位姿的图像帧的列表，

表示当前滑窗中特征点和图像闭环关键帧之间的重投影残差，

表示与当前关键帧处于闭环匹配状态的图像闭环关键帧中的单个特征点的重投影误差，M表示当前滑窗中和图像闭环关键帧中匹配成功的特征点组合，(w, v)表示在第v个数据帧上的第w个特征点，

表示第w个特征点在第v个数据帧的观测值，

和

分别表示图像闭环关键帧的姿态和位置，

表示当前滑窗中的关键帧和激光闭环关键帧之间的位姿残差，

表示单个激光闭环关键帧位姿的残差，N表示当前滑窗中和激光闭环关键帧中匹配成功的关键帧组合，(z, v)表示滑窗中第z个数据帧和第v个数据帧构成了激光闭环，

表示滑窗中第z个数据帧和第v个数据帧之间的位姿约束，ρ(.)表示鲁棒核函数；所述UWB位姿为根据所述中间位姿确定。

3.如权利要求1所述的定位方法，其特征在于，对每一个图像帧进行滑窗优化处理，具体包括：

根据所述IMU数据、所述轮速里程计数据、所述图像数据和所述激光数据，确定当前所处的工况状态；其中，所述工况状态包括：IMU失效工况、轮速里程计失效工况、图像失效工况、以及激光失效工况；

根据确定出的当前所处的工况状态、所述匹配结果、所述状态数据、以及所述中间位姿，对每一个图像帧进行所述滑窗优化处理。

4.如权利要求3所述的定位方法，其特征在于，根据确定出的当前所处的工况状态、所述匹配结果、所述状态数据、以及所述中间位姿，对每一个图像帧进行所述滑窗优化处理，具体包括：

在确定出当前处于所述IMU失效工况时，根据所述匹配结果、所述中间位姿、以及所述状态数据中激光位姿信息、轮速里程计的角度和位移的积分量信息，对每一个图像帧进行滑窗优化处理；

或，确定出当前处于所述轮速里程计失效工况时，根据所述匹配结果、所述中间位姿、以及所述状态数据中激光位姿信息、IMU角度和位移的积分量信息，对每一个图像帧进行滑窗优化处理；

或，确定出当前处于所述激光失效工况时，根据所述匹配结果、所述中间位姿、以及所述状态数据中IMU角度和位移的积分量信息、轮速里程计的角度和位移的积分量信息，对每一个图像帧进行滑窗优化处理；

或，确定出当前处于所述图像失效工况时，根据所述状态数据、以及所述中间位姿，对每一个图像帧进行滑窗优化处理。

5.如权利要求1所述的定位方法，其特征在于，还包括：

根据已经确定出的全部关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、所述已经确定出的全部关键帧与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、所述已经确定出的全部关键帧的位姿、以及每个所述图像关键帧对应的UWB位姿，对各所述图像关键帧进行位姿图优化处理，得到处理后的各所述图像关键帧的位姿；

其中，所述UWB位姿为根据所述中间位姿确定。

6.如权利要求5所述的定位方法，其特征在于，对各所述图像关键帧进行位姿图优化处理，具体包括：

判断所述当前关键帧与对应的所述图像闭环关键帧是否闭环匹配有效；若有效，则根据已经确定出的全部关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、所述已经确定出的全部关键帧与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、所述已经确定出的全部关键帧的位姿、以及每个所述图像关键帧对应的UWB位姿，各所述图像关键帧进行位姿图优化处理；若无效，则根据已经确定出的全部关键帧中除所述当前关键帧之外的其余各关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、所述已经确定出的全部关键帧中与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、所述已经确定出的全部关键帧的位姿、以及每个所述图像关键帧对应的UWB位姿，对各所述图像关键帧进行位姿图优化处理；

和/或，判断所述当前关键帧与对应的所述激光闭环关键帧是否闭环匹配有效；若有效，则根据已经确定出的全部关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、所述已经确定出的全部关键帧与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、所述已经确定出的全部关键帧的位姿、以及每个所述图像关键帧对应的UWB位姿，对各所述图像关键帧进行位姿图优化处理；若无效，则根据已经确定出的全部关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、所述已经确定出的全部关键帧中除所述当前关键帧之外的其余各关键帧与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、所述已经确定出的全部关键帧的位姿、以及每个所述图像关键帧对应的UWB位姿，对各所述图像关键帧进行位姿图优化处理；

对所述当前关键帧的位姿进行滑窗闭环优化处理，具体包括：

判断所述当前关键帧与对应的所述图像闭环关键帧是否闭环匹配有效；若有效，则根据当前处于滑窗中图像帧的位姿、以及确定出的与所述当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对所述当前关键帧的位姿进行滑窗闭环优化处理；若无效，则根据当前处于滑窗中图像帧的位姿、以及确定出的与所述当前关键帧处于闭环匹配状态的激光闭环关键帧，对所述当前关键帧的位姿进行滑窗闭环优化处理；

和/或，判断所述当前关键帧与对应的所述激光闭环关键帧是否闭环匹配有效；若有效，则根据当前处于滑窗中图像帧的位姿、以及确定出的与所述当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对所述当前关键帧的位姿进行滑窗闭环优化处理；若无效，则根据当前处于滑窗中图像帧的位姿、以及确定出的与所述当前关键帧处于闭环匹配状态的图像闭环关键帧，对所述当前关键帧的位姿进行滑窗闭环优化处理。

7.如权利要求5所述的定位方法，其特征在于，根据已经确定出的全部关键帧与处于闭环匹配状态的图像闭环关键帧之间的图像相对位姿、所述已经确定出的全部关键帧与处于闭环匹配状态的激光闭环关键帧之间的激光相对位姿、所述已经确定出的全部关键帧的位姿、以及每个所述图像关键帧对应的UWB位姿，对各所述图像关键帧进行位姿图优化处理，具体包括：

采用如下公式，对各所述图像关键帧进行位姿图优化处理：

Y ^* =argmin（Err1+Err2+Err3+Err4）

其中，S表示所述位姿图优化处理过程中所有从滑窗中得到的相邻两个数据帧之间的相对位姿约束的组合，i表示第i个图像关键帧，j表示第j个图像关键帧，r _i,j 表示所述第i个图像关键帧与所述第j个图像关键帧之间的残差，

表示从滑窗中得到的相邻两个数据帧之间的相对位姿约束的协方差，L _U 表示所述位姿图优化处理过程中所有具有UWB位姿的关键帧组合，r _k 表示第k个关键帧具有UWB位姿的残差，

表示UWB位姿约束的协方差，L _V 表示所述位姿图优化处理过程中所有视觉闭环关键帧的组合，r _m,n 表示第m个关键帧和第n个关键帧形成视觉闭环匹配时的位姿约束，(m, n)表示第m个关键帧和第n个关键帧构成了视觉闭环约束，

表示所述第m个关键帧和所述第n个关键帧形成视觉闭环匹配时位姿约束的协方差，L _L 表示所述位姿图优化处理过程中所有激光闭环关键帧的组合，r _g,h 表示第g个关键帧和第h个关键帧形成激光闭环匹配时的位姿约束，(g, h)表示第g个关键帧和第h个关键帧构成了激光闭环约束，

表示所述第g个关键帧和所述第h个关键帧形成激光闭环匹配时位姿约束的协方差，Y ^* 表示经过位姿图优化处理的状态。

8.如权利要求2或5所述的定位方法，其特征在于，根据所述中间位姿确定所述UWB位姿，具体包括：

根据IMU数据中的速度数据和轮速里程计数据中的速度数据，将所述中间位姿插值到当前图像帧中，得到采集当前图像帧时刻对应的机器人的位姿；

将得到的采集当前图像帧时刻对应的机器人的位姿，确定为所述UWB位姿。

9.如权利要求1所述的定位方法，其特征在于，对每一个图像帧进行滑窗优化处理后，确定出所述当前关键帧的位姿，具体包括：

对每一个图像帧进行滑窗优化处理后，得到优化后的系统状态；其中，所述系统状态包括：所述IMU、所述轮速里程计、所述激光、图像采集设备和所述UWB之间的外参；

根据所述优化后的系统状态，确定出所述当前关键帧的位姿。

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

第一单元，用于根据所述机器人当前采集到的IMU数据、轮速里程计数据、图像数据和激光数据、以及通过UWB技术确定出的所述机器人的中间位姿，按照预设的多传感器融合算法，确定当前关键帧及所述当前关键帧的位姿；

第二单元，用于在确定出已确定出的多个图像关键帧中与当前关键帧处于候选闭环匹配关系的候选关键帧时，确定当前关键帧与所述候选关键帧之间的图像相对位姿，并将所述图像相对位姿小于第一预设值对应的所述候选关键帧，确定为与所述当前关键帧处于闭环匹配状态的图像闭环关键帧；确定所述当前关键帧与已确定出的多个激光关键帧之间的激光相对位姿、以及所述当前关键帧与所述多个激光关键帧之间的点云匹配程度，并将所述激光相对位姿小于第二预设值、和/或所述点云匹配程度大于第三预设值对应的所述激光关键帧，确定为与所述当前关键帧处于所述闭环匹配状态的激光闭环关键帧；根据当前处于滑窗中图像帧的位姿、以及确定出的与所述当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对所述当前关键帧的位姿进行滑窗闭环优化处理，得到处理后的所述当前关键帧的位姿；

第三单元，用于根据处理后的所述当前关键帧的位姿，确定所述机器人的位姿；

其中，所述多传感器融合算法包括：根据确定出的所述机器人采集到的每一帧图像中角点的匹配结果、采集到的所有连续两帧图像得到的状态数据、以及所述中间位姿，对每一个图像帧进行滑窗优化处理后，确定出所述当前关键帧的位姿；所述状态数据包括：激光位姿信息、IMU角度和位移的积分量信息、轮速里程计的角度和位移的积分量信息。

Images