利索能及
我要发布
收藏
专利号: 2021100286051
申请人: 浙江欣奕华智能科技有限公司
专利类型:发明专利
专利状态:已下证
更新日期:2025-06-27
缴费截止日期: 暂无
联系人

摘要:

权利要求书:

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。

5.如权利要求4所述的定位方法,其特征在于,所述预设标签的重投影误差的确定方法,包括:根据当前滑窗的优化状态,计算所述预设标签包括的全部标签点在所在图像坐标系中的二维坐标的预测值;其中,所述全部标签点包括:用于表示所述预设标签在所在图像中位置的多个角点,和/或用于形成所述预设标签的图案的多个内点;

计算观测到的所述标签点在所在图像坐标系中的二维坐标、以及该所述标签点的二维坐标的预测值之间的差值;

根据计算后得到的各所述标签点的差值,确定所述预设标签的重投影误差。

6.如权利要求5所述的定位方法,其特征在于,在所述预设标签占据所在图像中的面积比例大于预设阈值时,所述全部标签点包括各所述角点和所述内点。

7.如权利要求1所述的定位方法,其特征在于,对确定出的当前关键帧进行滑窗优化处理,具体包括:若判断出当前处于滑窗中所有关键帧满足预设的优化状态固定切换条件时,将所述所有关键帧中采集时间最早的关键帧定义为最老关键帧,针对所述所有关键帧中除所述最老关键帧之外的其他关键帧和所述当前关键帧,进行所述滑窗优化处理;

其中,所述优化状态固定切换条件包括:

所述所有关键帧对应图像中均不包括预设标签;

或,所述所有关键帧对应图像中至少一张图像包括预设标签,且所述图像中包括的预设标签在所述滑窗中均约束无效。

8.如权利要求1所述的定位方法,其特征在于,在对确定出的当前关键帧进行滑窗优化处理之前,还包括:确定所述预设标签在所述滑窗中的约束有效。

9.如权利要求7或8所述的定位方法,其特征在于,判断所述预设标签在所述滑窗中的约束是否有效,具体包括:确定所述预设标签在当前处于所述滑窗中的各所述关键帧对应图像中的观测次数;其中,所述观测次数为:根据当前处于所述滑窗中包括所述预设标签的图像对应的所述关键帧的帧数、以及包括所述预设标签的图像包括的各目图像中观测到所述预设标签的次数确定;

判断所述观测次数是否大于第一预设值;

若是,则确定所述预设标签在所述滑窗中的约束有效;

若否,则确定所述预设标签在所述滑窗中的约束无效。

10.如权利要求1所述的定位方法,其特征在于,在对确定出的当前关键帧进行滑窗优化处理之前,还包括:在所述机器人的初始化阶段,按照预设规则,确定所述滑窗中的m个数据帧的初始位姿信息;其中m为大于1的整数;

对接收到的第m+1个数据帧和所述滑窗中的所述m个数据帧进行所述滑窗优化处理,得到m+1个数据帧优化后的位姿信息;

所述预设规则包括:

采集到的首个数据帧的初始位姿信息为:根据所述首个数据帧对应图像中具有预设标签的坐标信息和索引确定;

所述m+1个数据帧中除所述首个数据帧之外的其他任一数据帧的初始位姿信息为:在该数据帧对应图像中具有预设标签时,根据所述预设标签的坐标信息和索引确定;或,根据该数据帧相对于所述首个数据帧的IMU预积分数据确定。

11.如权利要求1所述的定位方法,其特征在于,根据处理后的所述当前关键帧的位姿信息、以及在确定出所述当前关键帧之后最近时刻采集到的一帧图像的位姿信息,确定所述机器人的位姿,具体包括:确定所述当前关键帧在进行所述位姿图优化处理前后产生的位姿增量;

将所述位姿增量与在确定出所述当前关键帧之后最近时刻采集到的一帧图像的位姿信息进行叠加,确定所述机器人的位姿。

12.如权利要求1所述的定位方法,其特征在于,对确定出的当前关键帧进行滑窗优化处理,确定当前关键帧的位姿信息,具体包括:对确定出的所述当前关键帧进行滑窗优化处理后,得到优化后的系统状态;其中,所述系统状态包括:IMU、轮速里程计和图像采集器之间的外参;

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

13.一种机器人的定位装置,其特征在于,包括:

第一单元,用于根据当前采集到的惯性测量单元IMU数据、轮速里程计数据和图像数据,对确定出的当前关键帧进行滑窗优化处理,确定所述当前关键帧的位姿信息;其中,所述图像数据包括:图像中特征点的匹配信息、以及在所述图像中具有预设标签时所述预设标签的索引和坐标信息;

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

第三单元,用于根据处理后的所述当前关键帧的位姿信息、以及在确定出所述当前关键帧之后最近时刻采集到的一帧图像的位姿信息,确定所述机器人的位姿。