[发明专利]一种机器人地图的构建方法、机器人的定位方法及装置

权利要求书

1.一种机器人地图的构建方法，其特征在于，包括：

根据所述机器人实时同步采集到的图像数据和惯性测量单元IMU数据，按照预设的关键帧数据确定规则，确定关键帧数据；其中所述关键帧数据包括：关键帧的位姿信息、以及关键帧对应图像中的特征点数据，所述特征点为所在图像中的角点，且与相邻一帧图像中的角点相匹配的角点；

根据所述关键帧数据，确定所述关键帧对应的闭环关键帧、以及闭环特征点对；其中所述闭环特征点对包括：位于该所述关键帧对应图像中的第一特征点、以及位于对应所述闭环关键帧对应图像中的第二特征点，所述第一特征点与所述第二特征点所描述的图像信息相匹配；

在确定所述关键帧与对应所述闭环关键帧满足预设的闭环条件时，对所述所有所述关键帧对应的特征点数据、所述闭环特征点对、以及所述IMU数据对进行第一集束调整BA优化处理后，得到用于表示所述机器人所在环境的地图；

其中，所述关键帧数据确定规则包括：在确定出采集到的每一帧图像包括的左目图像和右目图像中角点的匹配情况、以及采集到的连续两帧图像之间的状态增量信息时，且对每一帧图像进行滑窗优化处理后，根据所述滑窗优化处理的次数、以及所述特征点的重投影误差，确定出所述关键帧，并确定出所述关键帧的位姿信息、以及所述关键帧对应图像中所述特征点数据；所述状态增量信息包括：位置增量信息、角度增量信息和速度增量信息。

2.如权利要求1所述的构建方法，其特征在于，所述闭环条件包括：

在根据所述闭环特征点对、以及预设的PNP算法，确定出所述关键帧与对应所述闭环关键帧之间的前向PNP相对姿态和后向PNP相对姿态时，所述前向PNP相对姿态与所述后向PNP相对姿态的差值小于预设的第一阈值；

其中，所述前向PNP相对姿态包括：

所述闭环关键帧对应图像中的所述第二特征点的三维位置信息与对应所述关键帧对应图像中对应的所述第一特征点的二维位置信息之间的相对姿态；

所述后向PNP相对姿态包括：

任一所述关键帧对应图像中所述第一特征点的三维位置信息与对应所述闭环关键帧对应图像中对应的所述第二特征点的二维位置信息之间的相对姿态。

3.如权利要求1所述的构建方法，其特征在于，在确定所述关键帧与对应所述闭环关键帧不满足所述闭环条件时，还包括：

对所述关键帧对应的特征点数据、以及所述IMU数据对进行所述第一BA优化处理后，得到所述地图。

4.如权利要求3所述的构建方法，其特征在于，对所述关键帧对应的特征点数据、以及所述IMU数据对进行所述第一BA优化处理，具体包括：

根据如下公式，对所述关键帧对应的特征点数据、以及所述IMU数据对进行所述第一BA优化处理：

其中，k表示所述第k帧关键帧，B表示在确定所述状态增量信息时执行过预积分的关键帧的列表，表示采集到的左目图像上的特征点的观测值，表示采集到的右目图像上的特征点的观测值，j表示第j帧关键帧，P_l表示特征点l在世界坐标系中的坐标，表示在确定所述状态增量信息时IMU数据的预积分结果，χ表示所述滑窗需要优化的状态，表示所述状态增量信息对应的雅克比矩阵，表示左目图像的方差矩阵，表示右目图像的方差矩阵，表示所述IMU数据的残差，表示所述关键帧对应图像中的左目图像的残差，表示所述关键帧对应图像中的右目图像的残差。