[发明专利]一种机器人双目视觉定位方法、装置和存储介质

权利要求书

1.一种机器人双目视觉定位方法，其特征在于，包括以下步骤：

获取机器人的当前双目图像以及当前位姿；

若所述当前双目图像为关键图像，则从关键库中获取与当前双目图像具有重叠视野的历史关键图像，历史关键图像关联于历史关键位姿；

根据所述当前位姿和历史关键位姿将所述当前双目图像和历史关键图像拼接为视觉点云地图；

获取激光点云地图，所述激光点云地图是预先构建的；

根据所述视觉点云地图、以及激光点云地图对所述当前位姿进行优化。

2.如权利要求1所述的机器人双目视觉定位方法，其特征在于，所述激光点云地图具体为通过以下步骤构建：

获取工作环境的激光点云序列和双目图像序列；所述激光点云序列包括多个带有时间戳的激光点云数据，各激光点云数据包括多个激光点，所述双目图像序列包括多个带有时间戳的双目图像，所述双目图像包括单目图像，所述单目图像包括多个图像特征；

根据所述激光点云序列或双目图像序列计算与时间戳关联的序列位姿；

计算各激光点在具有相同时间戳的单目图像上的点云投影；

判断所述点云投影的预设过滤范围内是否存在所述单目图像的图像特征；

过滤掉各激光点云数据中点云投影的预设过滤范围内不存在图像特征的激光点，生成激光特征点云数据；

将激光特征点云数据根据序列位姿拼接为激光点云地图。

3.如权利要求1所述的机器人双目视觉定位方法，其特征在于，所述视觉点云地图包括多个视觉特征点，所述激光点云地图包括多个激光特征点，当前双目图像、历史关键图像均包括二维图像特征点，且当前双目图像、历史关键图像均包括单目图像；所述根据所述视觉点云地图、以及激光点云地图对所述当前位姿进行优化，具体为根据以下算式进行优化：

其中，I表示待优化的位姿的集合，包括当前位姿和历史关键位姿，x_j为I中的第j个的位姿，M表示视觉点云地图，表示M中的第i个视觉特征点的坐标，p_i，j表示x_j对应的当前双目图像或历史关键图像上与对应的二维图像特征点的坐标，表示投影到x_j对应的单目图像上的坐标与p_i，j之间的误差；L表示激光点云地图，表示L中的第l个激光特征点的坐标，表示和之间的距离误差，表示M中的第K个视觉特征点的坐标。

4.如权利要求3所述的机器人双目视觉定位方法，其特征在于，具体为：

对于第k个视觉特征点，若预设阈值范围内与视觉特征点距离最近的预设数量的激光特征点满足平行条件，则视觉特征点的坐标与所述预设数量中的各激光特征点的坐标之间的距离误差为：

其中，Ω_kl为点云信息矩阵，为点的法向量；

与其余的激光特征点的坐标之间的距离误差为：

5.如权利要求1-4中任一项所述的机器人双目视觉定位方法，其特征在于，所述获取机器人的当前双目图像以及当前位姿，具体包括以下子步骤：

获取机器人的当前双目图像、以及机器人的前一双目图像和前一位姿；

根据所述当前双目图像、前一双目图像计算位姿变化量；

根据所述位姿变化量和前一位姿计算机器人的当前位姿。

6.如权利要求1-4中任一项所述的机器人双目视觉定位方法，其特征在于，所述根据所述视觉点云地图、以及激光点云地图对所述当前位姿进行优化时，也对历史关键位姿进行优化，所述机器人双目视觉定位方法还包括以下步骤：

根据优化后的历史关键位姿更新关键库中历史关键图像所关联的历史关键位姿。