一种机器人地图的构建方法、机器人的定位方法及装置与流程

1.本发明涉及机器人技术领域，尤指一种机器人地图的构建方法、机器人的定位方法及装置。


背景技术：

2.随着对制造灵活性的需求增加、产品周期缩短和加快、人力成本上升、以及人类安全趋势的需求增加，全球移动机器人的市场正在快速发展，技术需求越来越高，例如，自主导引车(automated guided vehicles agv)已广泛应用于自动化仓库、工厂物料传输系统、物流拣配系统、柔性装配系统和其他智能运输站点。相对于基于磁条、反光板、二维码等agv，自主移动机器人(autonomous mobile robot,amr)的技术难度更高和使用场景更广，amr导航的核心技术即slam(simultaneous localization and mapping)，也即使得机器人知道环境地图，以及知道自己实时位置，这是全程不需要人干预、高效智能移动到达目标的基础。
3.amr在实际应用中，可以包括建图阶段，在该建图阶段，主要考虑能用尽量多的信息构建一个高精度且全局一致性的地图，而对定位的实时性、实时定位精度和频率则可以忽略不计。
4.那么，如何构建具有高精度且全局一致性的地图，是本领域技术人员亟待解决的技术问题。


技术实现要素：

5.本发明实施例提供了一种机器人地图的构建方法、机器人的定位方法及装置，用以构建具有高精度且全局一致性的地图。
6.第一方面，本发明实施例提供了一种机器人地图的构建方法，包括：
7.根据所述机器人实时同步采集到的图像数据和惯性测量单元imu数据，按照预设的关键帧数据确定规则，确定关键帧数据；其中所述关键帧数据包括：关键帧的位姿信息、以及关键帧对应图像中的特征点数据，所述特征点为所在图像中的角点，且与相邻一帧图像中的角点相匹配的角点；
8.根据所述关键帧数据，确定所述关键帧对应的闭环关键帧、以及闭环特征点对；其中所述闭环特征点对包括：位于该所述关键帧对应图像中的第一特征点、以及位于对应所述闭环关键帧对应图像中的第二特征点，所述第一特征点与所述第二特征点所描述的图像信息相匹配；
9.在确定所述关键帧与对应所述闭环关键帧满足预设的闭环条件时，对所述所有所述关键帧对应的特征点数据、所述闭环特征点对、以及所述imu数据对进行第一集束调整ba优化处理后，得到用于表示所述机器人所在环境的地图；
10.其中，在确定出采集到的每一帧图像包括的左目图像和右目图像中角点的匹配情况、以及采集到的连续两帧图像之间的状态增量信息时，且对每一帧图像进行滑窗优化处
理后，根据所述滑窗优化处理的次数、以及所述特征点的重投影误差，确定出所述关键帧，并确定出所述关键帧的位姿信息、以及所述关键帧对应图像中所述特征点数据；所述状态增量信息包括：位置增量信息、角度增量信息和速度增量信息。
11.可选地，在本发明实施例中，所述闭环条件包括：
12.在根据所述闭环特征点对、以及预设的pnp算法，确定出该所述关键帧与对应所述闭环关键帧之间的前向pnp相对姿态和后向pnp相对姿态时，所述前向pnp相对姿态与所述后向pnp相对姿态的差值小于预设的第一阈值；
13.其中，所述前向pnp相对姿态包括：
14.所述闭环关键帧对应图像中的所述第二特征点的三维位置信息与对应所述关键帧对应图像中对应的所述第一特征点的二维位置信息之间的相对姿态；
15.所述后向pnp相对姿态包括：
16.任一所述关键帧对应图像中所述第一特征点的三维位置信息与对应所述闭环关键帧对应图像中对应的所述第二特征点的二维位置信息之间的相对姿态。
17.可选地，在本发明实施例中，在确定所述关键帧与对应所述闭环关键帧不满足所述闭环条件时，还包括：
18.对所述关键帧对应的特征点数据、以及所述imu数据对进行所述第一ba优化处理后，得到所述地图。
19.可选地，在本发明实施例中，对所述关键帧对应的特征点数据、以及所述imu数据对进行所述第一ba优化处理，具体包括：
20.根据如下公式，对所述关键帧对应的特征点数据、以及所述imu数据对进行所述第一ba优化处理：
[0021][0022]
其中，k表示所述第k帧关键帧，b表示在确定所述状态增量信息时执行过预积分的关键帧的列表，表示采集到的左目图像上的特征点的观测值，表示采集到的右目图像上的特征点的观测值，j表示第j帧关键帧，p
l
表示特征点l在世界坐标系中的坐标，表示在确定所述状态增量信息时imu数据的预积分结果，χ表示所述滑窗需要优化的状态，表示所述状态增量信息对应的雅克比矩阵，表示左目图像的方差矩阵，表示右目图像的方差矩阵，表示所述imu数据的残差，表示所述关键帧对应图像中的左目图像的残差，表示所述关键帧对应图像中的右目图像的残差。
[0023]
可选地，在本发明实施例中，对所有所述关键帧对应的特征点数据、所述闭环特征点对、以及所述imu数据对进行第一集束调整ba优化处理，具体包括：
[0024]
根据如下公式，对所有所述关键帧对应的特征点数据、所述闭环特征点对、以及所述imu数据对进行第一集束调整ba优化处理：
[0025][0026]
其中，k表示第k帧关键帧，b表示在确定所述状态增量信息时执行过预积分的关键帧的列表，表示采集到的左目图像上的特征点的观测值，表示采集到的右目图像上的特征点的观测值，j表示第j帧关键帧，p
l
表示特征点l在世界坐标系中的坐标，表示在确定所述状态增量信息时imu数据的预积分结果，χ表示滑窗需要优化的状态，表示所述状态增量信息对应的雅克比矩阵，表示左目图像的方差矩阵，表示右目图像的方差矩阵，c
loopl
表示接受闭环的左目闭环帧图像的列表，c
loopr
表示接受闭环的右目闭环帧图像的列表，表示所述imu数据的残差，表示所述关键帧对应图像中的左目图像的残差，表示所述关键帧对应图像中的右目图像的残差，表示所述关键帧对应的闭环帧的图像中的左目图像的残差，表示所述关键帧对应的闭环帧的图像中的右目图像的残差。
[0027]
可选地，在本发明实施例中，根据所述关键帧数据，确定任一所述关键帧对应的闭环关键帧、以及闭环特征点对，具体包括：
[0028]
从所述关键帧中提取出特征点数据和描述符数据，并构建词袋数据集合；
[0029]
根据所述词袋数据集合，计算第一关键帧与每一个第二关键帧之间的相似度，并将所述相似度大于预设的第二阈值对应的所述第二关键帧，确定为所述第一关键帧对应的所述闭环关键帧；其中，所述第一关键帧为：任一所述关键帧，所述第二关键帧为：全部所述关键帧中除所述第一关键帧之外的所述关键帧；
[0030]
根据所述词袋数据集合，确定所述第一关键帧对应图像的特征点中与对应所述闭环关键帧对应图像的特征点中相匹配的特征点对，计算所述相匹配的特征点对之间的汉明距离，并将所述汉明距离小于预设的第三阈值对应的所述相匹配的特征点对，确定为所述闭环特征点对。
[0031]
可选地，在本发明实施例中，所述滑窗优化处理的过程，具体包括：
[0032]
根据确定出的每一帧图像中角点的匹配情况、以及连续两帧图像之间的状态增量信息，对n 1帧图像、以及所述n 1帧图像对应imu数据进行第二ba优化处理，确定出所述n 1帧图像的位姿信息、以及所述n 1帧图像中角点的坐标信息；n为大于0的整数；
[0033]
在根据所述n 1帧图像的位姿信息、以及所述n 1帧图像中角点的坐标信息，确定出需要将第n 1帧图像输入至预设的滑窗中时，将当前所述滑窗中采集时间最早的第i帧图像确定为待定帧图像并从所述滑窗中剔除；其中，所述滑窗包括n帧图像，i为大于0的整数；
[0034]
根据所述滑窗优化处理的次数、以及所述特征点的重投影误差，确定出所述关键帧，具体包括：
[0035]
将所述滑窗优化处理的次数不小于第一预设值，以及包括的所述特征点的重投影
误差不大于第二预设值对应的待定帧确定为所述关键帧。
[0036]
可选地，在本发明实施例中，根据确定出的每一帧图像中角点的匹配情况、以及连续两帧图像之间的状态增量信息，对n 1帧图像、以及所述n 1帧图像对应imu数据进行所述第二ba优化处理，具体包括：
[0037]
采用如下公式，根据确定出的每一帧图像中角点的匹配情况、以及连续两帧图像之间的状态增量信息，对n 1帧图像、以及所述n 1帧图像对应imu数据进行所述第二ba优化处理：
[0038][0039]
其中，表示所述imu数据的残差，表示所述图像数据的残差，表示在确定所述状态增量信息时所述imu数据的预积分结果，表示所述图像数据，表示所述状态增量信息对应的雅克比矩阵，表示所述状态增量信息对应的方差矩阵，χ表示滑窗需要优化的状态，r
p-h
p
χ表示被边缘化的所述待定帧的先验残差，k表示第k帧，b表示在确定所述状态增量信息时执行过预积分的图像的列表，l表示特征点l，j表示第j帧，c表示采集到的所述图像的列表。
[0040]
第二方面，本发明实施例提供了一种机器人的定位方法，包括：
[0041]
获取预先构建的地图，所述地图采用如本发明实施例提供的上述构建方法进行构建；
[0042]
根据所述机器人实时同步采集到的图像数据和惯性测量单元imu数据，按照预设的关键帧数据确定规则，确定关键帧数据；其中所述关键帧数据包括：关键帧的位姿信息、以及所述关键帧对应图像中的特征点数据，所述特征点为所在图像中的角点，且与相邻一帧图像中的角点相匹配的角点；
[0043]
根据所述地图、以及所述关键帧数据，确定任一所述关键帧对应的闭环关键帧；
[0044]
在确定所述关键帧与对应所述闭环关键帧满足预设的闭环条件时，对所述闭环关键帧进行位姿图优化处理后，对所述机器人进行定位；
[0045]
其中，在确定出采集到的每一帧图像包括的左目图像和右目图像中角点的匹配情况、以及采集到的连续两帧图像之间的状态增量信息时，且对每一帧图像进行滑窗优化处理后，确定出所述关键帧的位姿信息、以及所述关键帧对应图像中所述特征点数据；所述状态增量信息包括：位置增量信息、角度增量信息和速度增量信息。
[0046]
可选地，在本发明实施例中，所述闭环条件包括：
[0047]
若所述闭环关键帧对应图像为构建所述地图的其中一帧图像时，在根据所述闭环特征点对、以及预设的pnp算法，确定出该所述关键帧与对应所述闭环关键帧之间的第一pnp相对姿态时，所述第一pnp相对姿态小于预设的第四阈值；
[0048]
或，若所述闭环关键帧对应图像为在获取所述地图之后所述机器人采集到的图像时，在根据所述闭环特征点对、以及所述pnp算法，确定出该所述关键帧与对应所述闭环关键帧之间的第二pnp相对姿态时，所述第二pnp相对姿态小于预设的第五阈值。
[0049]
可选地，在本发明实施例中，对所述闭环关键帧进行位姿图优化处理，具体包括：
[0050]
采用如下公式，对已确定出的全部所述闭环关键帧进行位姿图优化处理：
[0051][0052]
其中，s表示具有滑窗约束的关键帧列表，l表示具有闭环约束的关键帧列表，i表示第i帧关键帧，j表示第j帧关键帧，r
i,j
表示所述第i帧关键帧与所述第j帧关键帧之间的残差，ρ(.)表示鲁棒核函数，p表示全部被优化的所述关键帧的位姿，ψ表示全部被优化的所述关键帧的角度信息。
[0053]
第三方面，本发明实施例提供了一种机器人地图的构建装置，包括：
[0054]
第一单元，用于根据所述机器人实时同步采集到的图像数据和惯性测量单元imu数据，按照预设的关键帧数据确定规则，确定关键帧数据；其中所述关键帧数据包括：关键帧的位姿信息、以及关键帧对应图像中的特征点数据，所述特征点为所在图像中的角点，且与相邻一帧图像中的角点相匹配的角点；
[0055]
第二单元，用于根据所述关键帧数据，确定所述关键帧对应的闭环关键帧、以及闭环特征点对；其中所述闭环特征点对包括：位于该所述关键帧对应图像中的第一特征点、以及位于对应所述闭环关键帧对应图像中的第二特征点，所述第一特征点与所述第二特征点所述描述的图像信息相匹配；
[0056]
第三单元，用于在确定所述关键帧与对应所述闭环关键帧满足预设的闭环条件时，对所述所有所述关键帧对应的特征点数据、所述闭环特征点对、以及所述imu数据对进行第一集束调整ba优化处理后，得到用于表示所述机器人所在环境的地图；
[0057]
其中，在确定出采集到的每一帧图像包括的左目图像和右目图像中角点的匹配情况、以及采集到的连续两帧图像之间的状态增量信息时，且对每一帧图像进行滑窗优化处理后，根据所述滑窗优化处理的次数、以及所述特征点的重投影误差，确定出所述关键帧，并确定出所述关键帧的位姿信息、以及所述关键帧对应图像中所述特征点数据；所述状态增量信息包括：位置增量信息、角度增量信息和速度增量信息。
[0058]
第四方面，本发明实施例提供了一种机器人的定位装置，包括：
[0059]
第一单元，用于获取预先构建的地图，所述地图采用如本发明实施例提供的上述构建方法进行构建；
[0060]
第二单元，用于根据所述机器人实时同步采集到的图像数据和惯性测量单元imu数据，按照预设的关键帧数据确定规则，确定关键帧数据；其中所述关键帧数据包括：关键帧的位姿信息、以及所述关键帧对应图像中的特征点数据，所述特征点为所在图像中的角点，且与相邻一帧图像中的角点相匹配的角点；
[0061]
第三单元，用于根据所述地图、以及所述关键帧数据，确定任一所述关键帧对应的闭环关键帧；
[0062]
第四单元，用于在确定所述关键帧与对应所述闭环关键帧满足预设的闭环条件时，对所述闭环关键帧进行位姿图优化处理后，对所述机器人进行定位；
[0063]
其中，在确定出采集到的每一帧图像包括的左目图像和右目图像中角点的匹配情况、以及采集到的连续两帧图像之间的状态增量信息时，且对每一帧图像进行滑窗优化处理后，确定出所述关键帧的位姿信息、以及所述关键帧对应图像中所述特征点数据；所述状态增量信息包括：位置增量信息、角度增量信息和速度增量信息。
[0064]
第五方面，本发明实施例提供了一种机器人，包括：如本发明实施例提供的上述构建装置、如本发明实施例提供的上述定位装置、以及图像采集装置；
[0065]
其中，所述图像采集装置用于：
[0066]
实时采集用于表示所述机器人所在环境的图像，并将采集到的图像发送至所述构建装置和所述定位装置中，以使所述构建装置根据所述采集到的图像构建地图，且所述定位装置根据所述采集到的图像和构建出的地图进行定位。
[0067]
本发明有益效果如下：
[0068]
本发明实施例提供的一种机器人地图的构建方法、机器人的定位方法及装置，在确定关键帧数据时，可以在确定出采集到的每一帧图像包括的左目图像和右目图像中角点的匹配情况、以及采集到的连续两帧图像之间的状态增量信息时，且对每一帧图像进行滑窗优化处理后，根据滑窗优化处理的次数、以及特征点的重投影误差，确定出关键帧，并确定出关键帧的位姿信息、以及关键帧对应图像中特征点数据，以便于根据关键帧数据，确定任一关键帧对应的闭环关键帧、以及闭环特征点对，进而可以对关键帧对应的特征点数据、闭环特征点对、以及imu数据进行第一ba优化处理后，得到用于表示机器人所在环境的地图；如此，得到的地图更加符合实际情况，从而可以生成高精度且全局一致性的地图，为后期机器人的定位提供有效的全局地图。并且，通过确定闭环关键帧和闭环特征点对，也即执行闭环检测过程，通过闭环检测可以关联特征点，并构造闭环点的重投影误差，从而可以有效消除累积的误差，使得构建的地图更加精准。
附图说明
[0069]
图1为本发明实施例中提供的一种机器人地图的构建方法的流程图；
[0070]
图2为本发明实施例中提供的另一种机器人地图的构建方法的流程图；
[0071]
图3为本发明实施例中提供的一种机器人的定位方法的流程图；
[0072]
图4为本发明实施例中提供的一种机器人地图的构建装置的结构示意图；
[0073]
图5为本发明实施例中提供的一种机器人的定位装置的结构示意图；
[0074]
图6为本发明实施例中提供的一种机器人的结构示意图。
具体实施方式
[0075]
下面将结合附图，对本发明实施例提供的一种机器人地图的构建方法、机器人的定位方法及装置的具体实施方式进行详细地说明。需要说明的是，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0076]
发明人在研究中发现，目前，主要的slam技术可以包括：激光slam和视觉slam，其中，激光slam较为成熟且被应用广，与激光slam相比，视觉slam的优势主要体现在两个方面：一方面是摄像头成本要远低于激光传感器；另一方面相对于2d激光slam而言，视觉slam能获得6dof的位姿，所以成本相对低廉的视觉slam技术在amr中得到了快速发展。
[0077]
amr在实际应用中，主要包括建图阶段和定位阶段(或者还可以称之为地图复用阶段)，这两个阶段的目标不同，进而对视觉slam技术的需求各有不同。其中，在建图阶段，主要考虑能用尽量多的信息构建一个高精度且全局一致性的地图，而对定位的实时性、实时
定位精度和频率则可以忽略不计；在定位阶段，amr主要考虑的是离线全局地图加载后，获取实时高精度高频率的全局定位精度，且一般不需要再重新构建高精度地图，对当前轨迹中过去时刻的位姿也可以忽略不计。
[0078]
然而，目前将视觉slam技术应用到amr时，无法兼顾建图和定位两个阶段的需求，使得无法对amr进行有效控制。
[0079]
基于此，本发明实施例提供了一种机器人地图的构建方法和定位方法，用以构建具有高精度且全局一致性的地图，且在后续机器人的定位的过程中，在采用视觉slam技术时，可以兼顾建图和定位两个阶段的需求。
[0080]
具体地，本发明实施例提供了一种机器人地图的构建方法，如图1所示，可以包括：
[0081]
s101、根据机器人实时同步采集到的图像数据和惯性测量单元imu数据，按照预设的关键帧数据确定规则，确定关键帧数据；其中关键帧数据包括：关键帧的位姿信息、以及关键帧对应图像中的特征点数据，特征点为所在图像中的角点，且与相邻一帧图像中的角点相匹配的角点；
[0082]
其中，关键帧数据确定规则包括：在确定出采集到的每一帧图像包括的左目图像和右目图像中角点的匹配情况、以及采集到的连续两帧图像之间的状态增量信息时，且对每一帧图像进行滑窗优化处理后，根据滑窗优化处理的次数、以及特征点的重投影误差，确定出关键帧，并确定出关键帧的位姿信息、以及关键帧对应图像中特征点数据；状态增量信息包括：位置增量信息、角度增量信息和速度增量信息。
[0083]
具体地，在确定关键帧数据时，可以采用以下过程：
[0084]
过程1：获取机器人实时同步采集到的图像数据和imu数据；
[0085]
其中，图像数据可以理解为多帧图像，imu数据也是一帧一帧的，且一帧imu数据可以理解为：一帧图像对应的imu数据。
[0086]
并且，如图2所示，采集到的每一帧图像可以包括左目图像(如图2中的“左目图像”)和右目图像(如图2中的“右目图像”)，且一帧图像中的左目图像和右目图像具有相同的时间戳信息。
[0087]
过程2：根据采集到的每一帧图像包括的左目图像和右目图像中角点的匹配情况，确定每一帧图像中的特征点匹配信息；
[0088]
其中，特征点匹配信息可以包括：特征点的编号、特征点的位置信息。
[0089]
在确定每一帧图像中的特征点匹配信息时，可以采用如下步骤：
[0090]
步骤2.1、如图2所示，对每一帧图像中的左目图像和相邻的上一帧图像中的左目图像进行特征点匹配处理(如图2中的“特征匹配”)、以及对每一帧图像中的右目图像和相邻的上一帧图像中的右目图像进行特征点匹配处理(如图2中的“特征匹配”)，得到每一帧图像中左目图像中的特征点、以及每一帧图像中右目图像中的特征点；
[0091]
步骤2.2、对左目图像或右目图像进行特征点补充处理；
[0092]
说明一点，执行该步骤2.2的目的在于：
[0093]
以左目图像为例，对于第i帧图像而言，在执行上述步骤2.1之前，假设第i帧图像中左目图像中的角点具有150个，在执行上述步骤2.1之后，假设确定出的第i帧图像中左目图像中的特征点为100个；
[0094]
那么，对于第i 1帧图像而言，如果将具有100个特征点的第i帧图像中左目图像与
第i 1帧图像中左目图像进行特征点匹配处理时，在执行上述步骤2.1之后，确定出的第i 1帧图像中左目图像中的特征点的数量很可能会少于100个，假设为50个时，那么对于第i 2帧图像而言，经过特征点匹配处理后，确定出的第i 2帧图像中左目图像中的特征点的数量会更少，即少于50个；如此循环下去，在后续获取到的图像中，确定出的特征点数量越少，随着获取的图像的增加，确定出的特征点数量可能会为零，显然，这是与实际不相符的。
[0095]
因此，为了避免出现上述情况，需要执行步骤2.2，即对特征点进行补充处理，也即，在确定出的第i帧图像中左目图像中的特征点为100个的基础上，可以对执行完上述步骤2.1的第i帧图像中左目图像中的特征点进行补充，假设可以补充50个特征点，即补充到150个，以保证后续获取到的图像中，可以确定出足够数量的特征点。
[0096]
其中，补充的特征点可以从执行完上述步骤2.1的第i帧图像中左目图像中重新提取，且提取的过程，可以采用现有技术，只要能够提取出特征点即可，对于提取的过程在此并不限定。
[0097]
步骤2.3、如图2所示，对执行完上述步骤2.1和步骤2.2的每一帧图像的左目图像和右目图像进行特征点匹配处理(如图2中的“特征匹配”)，得到每一帧图像的左目图像和右目图像中特征点的匹配关系；
[0098]
步骤2.4、根据得到的匹配关系，确定每一帧图像中特征点的匹配信息，并输出至滑窗中。
[0099]
过程3：确定采集到的连续两帧图像之间的状态增量信息；
[0100]
其中，状态增量信息包括：位置增量信息、角度增量信息和速度增量信息。
[0101]
说明一点，状态增量信息表示的是两帧图像之间的关系，并不是两帧imu数据之间的关系。
[0102]
可选地，在具体执行过程3时，具体可以采用以下步骤：
[0103]
步骤3.1、确定获取到的连续两帧图像之间的惯性测量单元(inertial measurement unit,imu)数据(如图2中的“imu”)，并存储至预设的缓存列表中；
[0104]
步骤3.2、将imu数据的积分起始时刻和结束时刻与对应的一帧图像的时刻信息进行对齐处理；
[0105]
步骤3.3、根据中值法，将对齐处理后的imu数据进行迭代积分处理(如图2中的“imu预积分”)，得到连续两帧图像之间的状态增量信息；
[0106]
步骤3.4、根据确定出的状态增量信息，确定对应的雅克比矩阵和方差矩阵，并输出至滑窗中。
[0107]
具体地，上述步骤3.1至步骤3.4的具体实现过程，可以参见现有技术，在此并不做具体限定。
[0108]
过程4：对每一帧图像进行滑窗优化处理，从采集到的各帧图像中筛选出待定帧；
[0109]
其中，在筛选待定帧时，可以具体依据以下因素：
[0110]
帧与帧之间的特征点匹配数量；
[0111]
帧与帧之间的平均视差；
[0112]
帧与帧之间的采集时间间隔；
[0113]
帧与帧之间的新增的特征点数量。
[0114]
在此过程4中，在筛选待定帧时，需要保证在后续滑窗处理过程中具有快速性和鲁
棒性等特点，以提高滑窗处理结果的有效性。
[0115]
过程5：根据滑窗优化处理的次数、以及特征点的重投影误差，从待定帧中确定出关键帧，并确定出关键帧的位姿信息、以及关键帧中特征点数据，如图2中的“滑窗优化处理”，以及“是否为关键帧”的判断。
[0116]
说明一点，过程5中提及的关键帧数据，可以理解为：从采集到的多帧图像中找到的比较重要的图像对应的一些数据。
[0117]
其中，对于滑窗而言，可以采用边缘化旧帧处理。
[0118]
例如，假设滑窗中包含k1帧待定帧图像，且按照各帧待定帧图像的采集顺序，确定出采集时间最早的一帧待定帧图像为第i帧待定帧图像；若机器人采集到了一帧新的图像(假设为第i a帧图像)，且经过过程1至过程4处理后，确定需要将第i a帧图像输入至滑窗中时，可以将第i帧图像确定为待定帧图像。
[0119]
如此，使得滑窗中始终包含k1帧待定帧图像，且保证滑窗中的图像均为最新的待定帧图像，以有利于保证后续构建的地图更加精准；并且，还可以减少计算量，降低计算成本。
[0120]
可选地，在滑窗中，k1帧待定帧图像、以及k1帧待定帧图像包含的k2个特征点的状态，也即滑窗需要优化的状态，可以表示为：
[0121][0122][0123][0124]
其中，χ
k1
表示第k1个关键帧的位姿信息，w表示世界坐标系，表示第i个关键帧在世界坐标系中的位移量，表示第i个关键帧在世界坐标系中的速度信息，表示第i个关键帧在世界坐标系中的角度信息(或者称之为旋转量信息)，b
a
表示加速度的偏置，b
g
表示陀螺仪的偏置，表示一个外参矢量，也即图像采集装置(如摄像头)的坐标系相对于imu坐标系之间的转换量，λ
k2
表示第k2个特征点，表示左目摄像头的坐标系相对于imu坐标系的位置转换关系，表示左目摄像头的坐标系相对于imu坐标系的相对旋转关系，表示右目摄像头的坐标系相对于imu坐标系的位置转换关系，表示右目摄像头的坐标系相对于imu坐标系的相对旋转关系。
[0125]
可选地，在本发明实施例中，滑窗优化处理的过程，具体包括：
[0126]
根据确定出的每一帧图像中角点的匹配情况、以及连续两帧图像之间的状态增量信息，对n 1帧图像、以及n 1帧图像对应imu数据进行第二ba优化处理，确定出n 1帧图像的位姿信息、以及n 1帧图像中角点的坐标信息；n为大于0的整数；
[0127]
在根据n 1帧图像的位姿信息、以及n 1帧图像中角点的坐标信息，确定出需要将第n 1帧图像输入至预设的滑窗中时，将当前滑窗中采集时间最早的第i帧图像确定为待定帧图像并从滑窗中剔除；其中，所述滑窗包括n帧图像，i为大于0的整数；
[0128]
因此，根据滑窗优化处理的次数、以及特征点的重投影误差，确定出关键帧，具体
包括：
[0129]
将滑窗优化处理的次数不小于第一预设值，以及包括的特征点的重投影误差不大于第二预设值对应的待定帧确定为关键帧。
[0130]
其中，根据n 1帧图像的位姿信息、以及n 1帧图像中角点的坐标信息，确定出需要将第n 1帧图像输入至预设的滑窗中，可以具体理解为：
[0131]
若第n 1帧图像与第n帧图像的位姿信息较接近，说明第n 1帧图像与第n帧图像的位姿未发生较大变化，此时，则可以无需将第n 1帧图像输入至滑窗中；
[0132]
若第n 1帧图像与第n帧图像的位姿信息差异较大，说明第n 1帧图像与第n帧图像的位姿发生了较大变化，此时，则需要将第n 1帧图像输入至滑窗中。
[0133]
由于滑窗中包括的图像的数量是固定的，若确定将第n 1帧图像输入至滑窗中时，需要将滑窗中输入最早的那一帧图像(如第i帧图像)从滑窗中删除，此时可以将第i帧图像确定为待定帧图像。
[0134]
并且，以滑窗中包括10帧图像为例，那么：
[0135]
可以对第1帧图像至第11帧图像进行第二ba优化处理，然后确定出不需要将第11帧图像输入至滑窗中，且可以将第1帧图像至第10帧图像输入至滑窗中时，对于滑窗中的第1帧图像而言，完成了第一次滑窗优化处理过程；
[0136]
对第1帧图像至第10帧图像、以及第12帧图像进行第二ba优化处理，然后确定出不需要将第12帧图像输入至滑窗中时，滑窗中包括的图像不变，且对于滑窗中的第1帧图像而言，完成了第二次滑窗优化处理过程；
[0137]
对第1帧图像至第10帧图像、以及第13帧图像进行第二ba优化处理，然后确定出需要将第13帧图像输入至滑窗中时，滑窗中包括的图像变为第2帧图像至第10帧图像和第13帧图像，且将第1帧图像从滑窗中剔除；此时，对于滑窗中的第1帧图像而言，完成了第三次滑窗优化处理过程；
[0138]
因此，对于第1帧图像而言，进行了三次滑窗优化处理过程，且第1帧图像为待定帧图像。
[0139]
若假设第一预设值为3，且确定出第1帧图像包括的特征点的重投影误差不大于第二预设值时，则可以将第1帧图像确定为关键帧图像。
[0140]
当然，第一预设值和第二预设值的具体数值，可以根据实际情况进行设置，在此并不限定。
[0141]
如此，可以依据滑窗优化处理的处理次数和重投影误差，选择出较为准确的关键帧，以便于后续地后端处理过程中，可以构建出高精度且全局一致的地图。
[0142]
可选地，在本发明实施例中，根据确定出的每一帧图像中角点的匹配情况、以及连续两帧图像之间的状态增量信息，对n 1帧图像、以及n 1帧图像对应imu数据进行第二ba优化处理，具体包括：
[0143]
采用如下公式，根据确定出的每一帧图像中角点的匹配情况、以及连续两帧图像之间的状态增量信息，对n 1帧图像、以及n 1帧图像对应imu数据进行第二ba优化处理：
[0144]
[0145]
其中，表示imu数据的残差，表示图像数据的残差，表示在确定状态增量信息时imu数据的预积分结果，表示图像数据，表示状态增量信息对应的雅克比矩阵，表示状态增量信息对应的方差矩阵，χ表示滑窗需要优化的状态，r
p-h
p
χ表示被边缘化的待定帧的先验残差，k表示第k帧，b表示在确定状态增量信息时执行过预积分的图像的列表，l表示特征点l，j表示第j帧，c表示采集到的图像的列表。
[0146]
说明一点，上述内容中提及的“imu数据的残差”，可以理解为：对观测值的imu预积分结果与待优化的状态之间的误差。
[0147]
也就是说，在第二ba优化处理过程(也可以称之为局部ba优化处理过程)中，结合非线性最小二乘法算法执行优化，以确定出关键帧的位姿信息、以及关键帧对应图像中的特征点数据(例如但不限于特征点的深度信息)。
[0148]
说明一点，在从关键帧对应图像中选择特征点时，在对待定帧进行滑窗优化处理后，可以从滑窗内的3d角点中进行选取，且选取原则可以为：滑窗中各关键帧图像的平均重投影的误差足够小、被观测的次数足够过、以及你深度估计要足够收敛等。当然，在选取特征点还可以根据其他原则进行确定和选取，只要能够选择出特征点，以便于后续构建地图即可，在此并不做具体限定。
[0149]
s102、根据关键帧数据，确定任一关键帧对应的闭环关键帧、以及闭环特征点对；其中闭环特征点对包括：位于该关键帧对应图像中的第一特征点、以及位于对应闭环关键帧对应图像中的第二特征点，第一特征点与第二特征点所描述的图像信息相匹配；
[0150]
说明一点，关键帧与关键帧图像是一一对应关系，且该步骤s102中提及的关键帧和闭环关键帧，均可以理解为关键帧数据和闭环关键帧数据。
[0151]
其中，s102中确定闭环关键帧和闭环特征点对的过程，可以理解为闭环检测，具体的确定过程可以参见下面内容。
[0152]
s103、在确定关键帧与对应闭环关键帧满足预设的闭环条件时，对所有关键帧对应的特征点数据、闭环特征点对、以及imu数据进行第一ba优化处理后，得到用于表示机器人所在环境的地图。
[0153]
其中，如图2所示，其中的“基于外参评估的全局ba优化”即为第一ba优化处理过程，经过该优化处理之后，即可得到地图(如图2中的“得到地图”)，且得到的地图可以但不限于为三维点云地图。
[0154]
在本发明实施例中，首先，可以在确定出采集到的每一帧图像包括的左目图像和右目图像中角点的匹配情况、以及采集到的连续两帧图像之间的状态增量信息时，且对每一帧图像进行滑窗优化处理后，根据滑窗优化处理的次数、以及特征点的重投影误差，从待定帧中确定出关键帧，并确定出关键帧的位姿信息、以及关键帧对应图像中特征点数据，以便于根据关键帧数据，确定任一关键帧对应的闭环关键帧、以及闭环特征点对，进而可以对关键帧对应的特征点数据、闭环特征点对、以及imu数据进行第一ba优化处理后，得到用于表示机器人所在环境的地图；如此，得到的地图更加符合实际情况，从而可以生成高精度且全局一致性的地图，为后期机器人的定位提供有效的全局地图。
[0155]
其次，通过确定闭环关键帧和闭环特征点对，也即执行闭环检测过程，通过闭环检
测可以关联特征点，并构造闭环点的重投影误差，从而可以有效消除累积的误差，使得构建的地图更加精准。
[0156]
说明一点，上述s101的处理过程，可以理解为前端处理过程(如图2中虚线框1内的过程)，上述s102和s103的处理过程，可以理解为后端处理过程(如图2中虚线框2内的过程)，也即，通过前端处理可以确定出关键帧数据，然后将关键帧数据发送至后端，在后端处理中通过对关键帧数据的处理构建地图，将基于紧耦合的滑窗优化的强大的双目视觉惯性里程计、与后端的第一ba优化处理相结合，从而构建出高精度且全局一致性的地图。
[0157]
可选地，在本发明实施例中，根据关键帧数据，确定关键帧对应的闭环关键帧、以及闭环特征点对，具体包括以下过程：
[0158]
过程1、从关键帧中提取出特征点数据和描述符数据，并构建词袋数据集合；
[0159]
其中，关键帧是通过上述s101确定出的，且上述s101确定出的关键帧数据不仅包括关键帧的位姿信息、以及关键帧对应图像中的特征点数据，还可以包括特征点的三维位置信息、以及滑窗中特征点的匹配信息，以便于后续确定闭环关键帧、以及闭环特征点对。
[0160]
并且，在提取特征点数据和描述符数据时，可以基于现有fast算法提取特征点数据，基于brief算法提取描述符数据，当然，还可以依据其他算法提取特征点数据和描述符数据，在此并不限定。
[0161]
过程2、根据词袋数据集合，计算第一关键帧与每一个第二关键帧之间的相似度，并将相似度大于预设的第二阈值对应的第二关键帧，确定为第一关键帧对应的闭环关键帧；其中，第一关键帧为：任一关键帧，第二关键帧为：全部关键帧中除第一关键帧之外的关键帧；
[0162]
其中，第二阈值的设置可以根据实际需要进行设置，在此并不限定。
[0163]
并且，在确定闭环关键帧时，可以基于dbow模型的词袋数据集合进行确定，当然并不限于此，只要能够确定出闭环关键帧即可，对于基于的模型在此并不限定。
[0164]
过程3、根据词袋数据集合，确定第一关键帧对应图像的特征点中与对应闭环关键帧对应图像的特征点中相匹配的特征点对，计算相匹配的特征点对之间的汉明距离，并将汉明距离小于预设的第三阈值对应的相匹配的特征点对，确定为闭环特征点对。
[0165]
其中，第三阈值的设置可以根据实际需要进行设置，在此并不限定。
[0166]
并且，在计算相匹配的特征点对之间的汉明距离时，可以基于brief描述符进行计算，具体计算过程可参见现有技术，在此不再详述。
[0167]
如此，通过上述过程即可确定出任一关键帧对应的闭环关键帧、以及闭环特征点对，从而有利于执行后续过程，保证地图的有效构建。
[0168]
可选地，在本发明实施例中，在确定关键帧与对应闭环关键帧不满足闭环条件时，还包括：
[0169]
对关键帧对应的特征点数据、以及imu数据对进行第一ba优化处理后，得到地图。
[0170]
如此，可以不满足闭环条件时，仅对关键帧对应的特征点数据和imu数据进行第一ba处理，从而实现分情况进行优化处理，不仅可以提高构建出的地图的精确度，还可以有利于降低设备的运算量，提高地图的构建效率。
[0171]
具体地，在本发明实施例中，闭环条件包括：
[0172]
在根据闭环特征点对、以及预设的pnp算法，确定出该关键帧与对应闭环关键帧之
间的前向pnp相对姿态和后向pnp相对姿态时，前向pnp相对姿态与后向pnp相对姿态的差值小于预设的第一阈值；
[0173]
其中，前向pnp相对姿态包括：
[0174]
闭环关键帧对应图像中的第二特征点的三维位置信息与对应关键帧对应图像中对应的第一特征点的二维位置信息之间的相对姿态；
[0175]
后向pnp相对姿态包括：
[0176]
任一关键帧对应图像中第一特征点的三维位置信息与对应闭环关键帧对应图像中对应的第二特征点的二维位置信息之间的相对姿态。
[0177]
其中，闭环关键帧对应图像中的特征点均称之为第二特征点，关键帧对应图像中的特征点均称之为第一特征点，以便于将关键帧对应图像中的特征点与闭环关键帧对应图像中的特征点区分开来。
[0178]
还有，在计算前向pnp相对姿态和后向pnp相对姿态时，具体的计算过程可以参见现有技术，在此不再详述。
[0179]
并且，第一阈值的设置可以根据实际需要进行设置，在此并不限定。
[0180]
如此，通过pnp ransac算法，可以确定出关键帧与对应闭环关键帧之间是否接受闭环(如图2中的“闭环检测”)，若接受闭环，那么闭环特征点对可以认为是同一个角点，匹配的特征点可以用于后续第一ba优化处理，计算重投影误差(如图2中的“闭环点在闭环关键帧的重投影误差”)，从而有利于进行地图的构建；并且，还可以消除闭环检测时出现的误匹配，从而有利于提高构建的地图的精确度。
[0181]
具体地，在本发明实施例中，对所有关键帧对应的特征点数据、闭环特征点对、以及imu数据对进行第一集束调整ba优化处理，具体包括：
[0182]
根据如下公式，对所有关键帧对应的特征点数据、闭环特征点对、以及imu数据对进行第一集束调整ba优化处理：
[0183][0184]
其中，k表示第k帧关键帧，b表示在确定状态增量信息时执行过预积分的关键帧的列表，表示采集到的左目图像上的特征点的观测值，表示采集到的右目图像上的特征点的观测值，j表示第j帧关键帧，p
l
表示特征点l在世界坐标系中的坐标，表示在确定状态增量信息时imu数据的预积分结果，χ表示滑窗需要优化的状态，表示状态增量信息对应的雅克比矩阵，表示左目图像的方差矩阵，表示右目图像的方差矩阵，c
loopl
表示接受闭环的左目闭环帧图像的列表，c
loopr
表示接受闭环的右目闭环帧图像的列表，表示imu数据的残差，表示关键帧对应图像中的左目图像的残差，表示关键帧对应图像中的右目图像的残差，表示关键帧对应
的闭环帧的图像中的左目图像的残差，表示关键帧对应的闭环帧的图像中的右目图像的残差。
[0185]
也就是说，在对所有关键帧对应的特征点数据、闭环特征点对、以及imu数据对进行第一集束调整ba优化处理时：
[0186]
1、考虑到前端处理中估计外参的不稳定性，在第一ba优化处理过程中加入了外参(即imu与摄像头之间存在一个恒定的转换关系)优化；
[0187]
2、同时为了进一步利用更多的信息来提高建图精度，在第一ba优化处理过程中加入了imu约束(或者，称之为imu时域约束，即)；
[0188]
3、经过前端处理得到的关键帧均被用来进行第一ba优化处理，以构建高精度的地图。
[0189]
其中，在上述第一ba优化处理过程中，主要考虑到了五部分残差，分别为：
[0190]
关键帧对应图像观测的特征点的三维坐标信息，在左目图像的投影残差(也即与左目图像的真实观测结果之间的误差)、在右目图像的投影残差，如图2中的“观测到的特征点的重投影误差”；
[0191]
闭环点(即在接收闭环(具体参见上述内容介绍)的情况下，闭环特征点对中的特征点)在闭环关键帧对应图像中的左目图像的投影残差、右目图像的投影残差，如图2中的“闭环点在闭环关键帧的重投影误差”；
[0192]
帧与帧之间的imu约束残差，如图2中的“帧与帧之间的imu约束”；
[0193]
其中，帧与帧之间的imu约束残差，主要用于体现时序的约束，同时可用来约束尺度。
[0194]
并且，由于后端处理过程中关键帧之间的相距时间较长，可以用滑窗内的位姿差来近似imu约束残差。
[0195]
此外，由于在对关键帧、闭环特征点对、以及imu数据对进行第一集束调整ba优化处理时，已经判断出该关键帧与对应闭环关键帧满足预设的闭环条件，也即关键帧与对应闭环关键帧接受闭环，所以在进行第一ba优化处理时，需要考虑上述五部分残差。
[0196]
可选地，在本发明实施例中，对关键帧对应的特征点数据、以及imu数据对进行第一ba优化处理，具体包括：
[0197]
根据如下公式，对关键帧对应的特征点数据、以及imu数据对进行第一ba优化处理：
[0198][0199]
其中，k表示第k帧关键帧，b表示在确定状态增量信息时执行过预积分的关键帧的列表，表示采集到的左目图像上的特征点的观测值，表示采集到的右目图像上的特征点的观测值，j表示第j帧关键帧，p
l
表示特征点l在世界坐标系中的坐标，表示在确定状态增量信息时imu数据的预积分结果，χ表示滑窗需要优化的状态，表示状态增量信息
对应的雅克比矩阵，表示左目图像的方差矩阵，表示右目图像的方差矩阵，表示imu数据的残差，表示关键帧对应图像中的左目图像的残差，表示关键帧对应图像中的右目图像的残差。
[0200]
由于在对关键帧对应的特征点数据、以及imu数据对进行第一ba优化处理时，已经判断出该关键帧与对应闭环关键帧不满足预设的闭环条件，也即关键帧与对应闭环关键帧不接受闭环，所以在进行第一ba优化处理时，只需要考虑以下三部分残差，分别为：
[0201]
关键帧对应图像观测到的特征点的三维坐标信息，在左目图像的投影残差(也即与左目图像的真实观测结果之间的误差)、在右目图像的投影残差；
[0202]
帧与帧之间的imu约束残差。
[0203]
并且，不管是针对哪些数据进行第一ba优化处理，均是将第一ba优化处理过程构造为非线性最小二乘法进行优化，且上述公式中提及的χ可以表示为：
[0204][0205][0206][0207]
其中，x
k
表示第k个关键帧的位姿信息，w表示世界坐标系，表示第k个关键帧在世界坐标系中的位移量，表示第k个关键帧在世界坐标系中的角度信息(或者称之为旋转量信息)，表示一个外参矢量，也即图像采集装置(如摄像头)的坐标系相对于imu坐标系之间的转换量，p
m
表示第m个特征点在世界坐标系中的位置，表示左目摄像头的坐标系相对于imu坐标系的位置转换关系，表示左目摄像头的坐标系相对于imu坐标系的相对旋转关系，表示右目摄像头的坐标系相对于imu坐标系的位置转换关系，表示右目摄像头的坐标系相对于imu坐标系的相对旋转关系。
[0208]
可选地，在本发明实施例中，在得到用于表示机器人所在环境的地图之后，还包括：
[0209]
将得到的地图进行保存。
[0210]
其中，在保存地图时，同时还保存了与地图相关的信息，具体可以包括：
[0211]
1、所有经过第一ba优化处理后的关键帧的位姿信息；
[0212]
2、各关键帧对应图像中可以观测到的特征点在三维世界坐标系中的位置信息，各特征点在归一化的二维坐标系中的位置信息，各特征点对应的brief描述子；
[0213]
3、各关键帧对应图像中的提取到的且用于构建词袋数据集合的特征点在归一化的二维坐标系中的位置信息，以及对应的brief描述子；
[0214]
4、第一ba优化过程后的外参(即左目图像和imu，右目图像和imu的外参)。
[0215]
需要说明的是，第一ba优化处理过程需要在建图线程完成时执行，即在执行完s101和s102之后进行第一ba优化处理，所以得到的地图为优化处理后的地图，使得得到的
地图精度更高，具有全局一致性的优点。
[0216]
并且，为了能够降低计算量，可以在执行第一ba优化处理之前，执行降采样处理，以删除某些关键帧，即删除邻近的关键帧之间的距离太近或方向非常相似的关键帧，且邻近的关键帧之间的空间密度，决定着是否需要删除其中的某些关键帧，从而减少关键帧的数量，在执行第一ba优化处理时，减少第一ba优化处理的处理量，提高处理效率。
[0217]
基于同一发明构思，本发明实施例提供了一种机器人的定位方法，如图3所示，可以包括：
[0218]
s301、获取预先构建的地图，地图采用如本发明实施例提供的上述构建方法进行构建；
[0219]
其中，在获取地图时，可以但不限于通过加载的方式来获取，且获取到的地图格式，与之前构建地图之后保存的地图的格式相同。
[0220]
还有，在后续地处理过程中，与地图相关的所有状态，在后续的处理过程中均被视为恒定，且在采用相同的传感器套件时，可以在amr的定位阶段将优化的外部参数加载为初始值，在实现后续地定位处理的同时，保证定位的准确性。
[0221]
s302、根据机器人实时同步采集到的图像数据和惯性测量单元imu数据，按照预设的关键帧数据确定规则，确定关键帧数据；其中关键帧数据包括：关键帧的位姿信息、以及关键帧对应图像中的特征点数据，特征点为所在图像中的角点，且与相邻一帧图像中的角点相匹配的角点；
[0222]
其中，在确定出采集到的每一帧图像包括的左目图像和右目图像中角点的匹配情况、以及采集到的连续两帧图像之间的状态增量信息时，且对每一帧图像进行滑窗优化处理后，确定出关键帧的位姿信息、以及关键帧对应图像中特征点数据；状态增量信息包括：位置增量信息、角度增量信息和速度增量信息。
[0223]
说明一点，可选地，在进行滑窗优化处理时，可以仅执行一次滑窗优化处理，以获取amr的全局位姿并尽快消除当前轨迹的累积误差。
[0224]
对于s302的具体实施方式，可以参见前述s102的具体实施方式，重复之处不再赘述。
[0225]
s303、根据地图、以及关键帧数据，确定任一关键帧对应的闭环关键帧；
[0226]
s304、在确定关键帧与对应闭环关键帧满足预设的闭环条件时，对闭环关键帧进行位姿图优化处理后，对机器人进行定位。
[0227]
说明一点，上述s302的处理过程，可以理解为前端处理过程，上述s303和s304的处理过程，可以理解为后端处理过程，也即，通过前端处理可以确定出关键帧数据，然后将关键帧数据和地图均发送至后端，在后端处理中通过对关键帧数据和地图的处理进行定位，将基于紧耦合的滑窗优化的强大的双目视觉惯性里程计、与后端的位姿图优化处理相结合，从而实现实时且高精度的定位。
[0228]
并且，在s302中确定出关键帧数据之后，在前端，依然会对关键帧进行多次滑窗优化处理，类似与构建地图中的前端处理过程，在对关键帧进行多次滑窗优化处理后，以便于对最新获取到的图像与已经进行滑窗优化处理后的关键帧进行处理，以判断出最新获取到的图像是否为关键帧。也就是说，在确定出第i帧关键帧对应的关键帧数据之后，对于第i帧关键帧的前端处理过程并未结束，依然继续执行。
[0229]
可选地，在本发明实施例中，在确定关键帧与对应闭环关键帧不满足预设的闭环条件时，还包括：
[0230]
停止对闭环关键帧对进行位姿图优化处理。
[0231]
如此，可以减少不必要的操作，提高定位的效率，减少设备的运算量。
[0232]
具体地，在本发明实施例中，闭环条件包括：
[0233]
若闭环关键帧对应图像为构建地图的其中一帧图像时，在根据闭环特征点对、以及预设的pnp算法，确定出该关键帧与对应闭环关键帧之间的第一pnp相对姿态时，第一pnp相对姿态小于预设的第四阈值；
[0234]
或，若闭环关键帧对应图像为在获取地图之后机器人采集到的图像时，在根据闭环特征点对、以及pnp算法，确定出该关键帧与对应闭环关键帧之间的第二pnp相对姿态时，第二pnp相对姿态小于预设的第五阈值。
[0235]
其中，若闭环关键帧对应图像为构建地图的其中一帧图像时，说明闭环关键帧对应图像来源于预先构建的地图中的某一帧图像，此时：闭环约束的计算与前端处理过程中的状态无关(即可以直接确定出位姿，使得计算过程更简单和更准确)；并且由于当前关键帧的位姿，主要依赖于高精度的地图，所以不会受到前端处理过程中累积的错误的影响。
[0236]
说明一点，第二条件中提及的pnp相对姿态的计算过程和具体含义，可以参见现有技术，在此不再赘述。
[0237]
如此，通过pnp ransac算法，可以确定出关键帧与对应闭环关键帧之间是否接受闭环，若接受闭环，那么闭环特征点对可以认为是同一个角点，匹配的特征点可以用于后续位姿图优化处理，计算重投影误差，从而有利于进行机器人的定位；并且，还可以消除闭环检测时出现的误匹配，从而有利于提高定位的精确度。
[0238]
可选地，在本发明实施例中，对闭环关键帧进行位姿图优化处理，具体包括：
[0239]
采用如下公式，对已确定出的全部闭环关键帧进行位姿图优化处理：
[0240][0241]
其中，s表示具有滑窗约束的关键帧列表，l表示具有闭环约束的关键帧列表，i表示第i帧关键帧，j表示第j帧关键帧，r
i,j
表示第i帧关键帧与第j帧关键帧之间的残差，ρ(.)表示鲁棒核函数，p表示全部被优化的关键帧的位姿，ψ表示全部被优化的关键帧的角度信息。
[0242]
也就是说，为了达到输出位姿的稳定性，后端处理过程中维护了一个小规模的位姿图来约束和平滑全局姿态，从而可以有效避免因误匹配闭环检测而引起的姿态跳变。
[0243]
具体地，可以将一个关键帧视为姿势图中的一个顶点，每两个顶点(由关键帧构成)由一个约束边连接，约束边的类型主要有两种，分别为：顺序约束边和闭环约束边，其中，顺序约束边可以表示前端处理结果中的两个顶点之间的相对转换约束，且关键帧可以使用顺序约束边连接到几个邻近已经确定出的关键帧之上；闭环约束边可以表示关键帧和对应的闭环关键帧之间的相对转换约束，且闭环关键帧可以通过闭环约束边与对应的关键帧进行连接。
[0244]
因此，在确定关键帧与对应闭环关键帧满足预设的闭环条件时，说明该关键帧与对应闭环关键帧之间接受闭环，所以对闭环关键帧进行位姿图优化处理时，需要考虑闭环
约束边的残差和顺序约束边的残差这两个因素。
[0245]
说明一点，如果关键帧对应的闭环关键帧对应图像来自于已经构建的地图，此时，则无需进行上述位姿图优化处理，直接进行定位即可；如果关键帧对应的闭环关键帧对应图像并不是来自于已经构建的地图，此时，则需要进行上述位姿图优化处理，以实现机器人的定位。
[0246]
并且，随着amr的移动，位姿图的大小会逐渐增加，这会影响定位的实时性，因此，在执行位姿图优化处理之前，或者在执行位姿图优化处理之后，当amr的运动轨迹仍然处于已构建的地图范围内时，可以仅保留n个最新的关键帧，以减少计算量；若amr的运动轨迹超出了已构建的地图范围时，则需要根据邻近的两个关键帧之间的距离，进行降采样处理，也即删除邻近的关键帧之间的距离太近或方向非常相似的关键帧，且邻近的关键帧之间的空间密度，决定着是否需要删除其中的某些关键帧，从而减少关键帧的数量，在执行位姿图优化处理时，减少位姿图优化处理的处理量，提高处理效率。
[0247]
基于同一发明构思，本发明实施例提供了一种机器人地图的构建装置，该构建装置的实现原理与前述一种机器人地图的构建方法的实现原理类似，所以该构建装置的实施方式可以参见前述构建方法的具体实施例，重复之处不再赘述。
[0248]
具体地，本发明实施例提供的一种机器人地图的构建装置，如图4所示，可以包括：
[0249]
第一单元401，用于根据机器人实时同步采集到的图像数据和惯性测量单元imu数据，按照预设的关键帧数据确定规则，确定关键帧数据；其中关键帧数据包括：关键帧的位姿信息、以及关键帧对应图像中的特征点数据，特征点为所在图像中的角点，且与相邻一帧图像中的角点相匹配的角点；
[0250]
第二单元402，用于根据关键帧数据，确定关键帧对应的闭环关键帧、以及闭环特征点对；其中闭环特征点对包括：位于该关键帧对应图像中的第一特征点、以及位于对应闭环关键帧对应图像中的第二特征点，第一特征点与第二特征点描述的图像信息相匹配；
[0251]
第三单元403，用于在确定关键帧与对应闭环关键帧满足预设的闭环条件时，对所有关键帧对应的特征点数据、闭环特征点对、以及imu数据对进行第一集束调整ba优化处理后，得到用于表示机器人所在环境的地图；
[0252]
其中，在确定出采集到的每一帧图像包括的左目图像和右目图像中角点的匹配情况、以及采集到的连续两帧图像之间的状态增量信息时，且对每一帧图像进行滑窗优化处理后，根据滑窗优化处理的次数、以及特征点的重投影误差，确定出关键帧，并确定出关键帧的位姿信息、以及关键帧对应图像中特征点数据；状态增量信息包括：位置增量信息、角度增量信息和速度增量信息。
[0253]
基于同一发明构思，本发明实施例提供了一种机器人的定位装置，该定位装置的实现原理与前述一种机器人的定位方法的实现原理类似，所以该定位装置的实施方式可以参见前述定位方法的具体实施例，重复之处不再赘述。
[0254]
具体地，本发明实施例提供的一种机器人的定位装置，如图5所示，可以包括：
[0255]
第一单元501，用于获取预先构建的地图，地图采用如本发明实施例提供的上述构建方法进行构建；
[0256]
第二单元502，用于根据机器人实时同步采集到的图像数据和惯性测量单元imu数据，按照预设的关键帧数据确定规则，确定关键帧数据；其中关键帧数据包括：关键帧的位
姿信息、以及关键帧对应图像中的特征点数据，特征点为所在图像中的角点，且与相邻一帧图像中的角点相匹配的角点；
[0257]
第三单元503，用于根据地图、以及关键帧数据，确定任一关键帧对应的闭环关键帧；
[0258]
第四单元504，用于在确定关键帧与对应闭环关键帧满足预设的闭环条件时，对闭环关键帧进行位姿图优化处理后，对机器人进行定位；
[0259]
其中，在确定出采集到的每一帧图像包括的左目图像和右目图像中角点的匹配情况、以及采集到的连续两帧图像之间的状态增量信息时，且对每一帧图像进行滑窗优化处理后，确定出关键帧的位姿信息、以及关键帧对应图像中特征点数据；状态增量信息包括：位置增量信息、角度增量信息和速度增量信息。
[0260]
基于同一发明构思，本发明实施例提供了一种机器人，如图6所示，可以包括：如本发明实施例提供的上述构建装置601、如本发明实施例提供的上述定位装置602、以及图像采集装置603；
[0261]
其中，图像采集装置603用于：
[0262]
实时采集用于表示机器人所在环境的图像，并将采集到的图像发送至构建装置601和定位装置602中，以使构建装置601根据采集到的图像构建地图，且定位装置602根据采集到的图像和构建出的地图进行定位。
[0263]
可选地，在本发明实施例中，图像采集装置可以但不限于为摄像头，还可以为其他形式的基于视觉的图像采集装置，在此并不限定。
[0264]
可选地，在本发明实施例中，机器人除了包括构建装置、定位装置和图像采集装置之外，还可以包括其他用于实现机器人功能的结构，具体可参见现有技术，在此并不限定。
[0265]
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。