[发明专利]一种机器人的定位方法及其定位装置

权利要求书

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

根据当前采集到的惯性测量单元IMU数据、轮速里程计数据和图像数据，对确定出的当前关键帧进行滑窗优化处理，确定所述当前关键帧的位姿信息；其中，所述图像数据包括：图像中特征点的匹配信息、以及在所述图像中具有预设标签时所述预设标签的索引和坐标信息；将多个图像帧的多个预设标签的坐标信息通过紧耦合方式融入至所述滑窗优化处理过程；

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

根据处理后的所述当前关键帧的位姿信息、以及在确定出所述当前关键帧之后最近时刻采集到的一帧图像的位姿信息，确定所述机器人的位姿；

所述将多个图像帧的多个预设标签的坐标信息通过紧耦合方式融入至所述滑窗优化处理过程，包括：

根据所述预设标签的索引和坐标信息，确定所述预设标签的各个角点和/或内点的位置信息和图像像素信息；

根据所述滑窗中多个图像帧中多个预设标签的角点和/或内点的位置信息和图像像素信息，确定所述预设标签的角点和/或内点在所述多个图像帧的重投影误差；

使用所述重投影误差进行滑窗优化处理。

2.如权利要求1所述的定位方法，其特征在于，所述预设标签的坐标信息包括：

在用于表示所述预设标签在所在图像中位置的角点具有多个时，各所述角点的坐标信息。

3.如权利要求2所述的定位方法，其特征在于，所述预设标签的坐标信息还包括：

在用于形成所述预设标签的图案的内点具有多个时，各所述内点的坐标信息。

4.如权利要求1所述的定位方法，其特征在于，根据当前采集到的IMU数据、轮速里程计数据、图像数据，对确定出的当前关键帧进行滑窗优化处理，具体包括：

采用如下公式，对确定出的当前关键帧进行滑窗优化处理：

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

其中，表示被边缘化的图像帧的先验残差，表示IMU数据的预积分约束，B表示具备所述IMU数据的预积分约束的图像帧的列表，表示所述IMU数据的预积分状态增量对应的协方差矩阵，表示所述IMU数据的预积分结果，表示滑窗中第j个特征点在第l个图像帧中的重投影残差，表示第j个特征点在第l个数据帧中的观测值，表示视觉观测的协方差，C表示滑窗中所有图像帧的列表，表示轮速里程计数据的预积分残差，E表示具备轮速里程计预积分约束的图像帧的列表，表示轮速里程计预积分对应的协方差矩阵，表示轮速里程计数据的预积分结果，表示所述预设标签的重投影误差，表示所述预设标签的观测噪声的协方差，k表示第k个数据帧，t表示观测到的第t个所述预设标签，表示所述预设标签的坐标信息，T表示观测到的所述预设标签的列表，χ^*表示滑窗优化后的最优状态，argmin表示使目标误差函数取最小值时的变量值，所述目标误差函数为Res1+Res2+Res3+Res4+Res5。