[发明专利]一种双目视觉定位方法、装置及系统

权利要求书

1.一种双目视觉定位方法，其特征在于，包括：

根据双目摄像单元在连续的两个时刻采集到的两帧图像，计算第一位姿变化信息；

根据所述双目摄像单元上的惯性测量单元在所述连续的两个时刻之间采集的惯性参数，计算第二位姿变化信息；

分别从所述两帧图像中提取两帧图像上相匹配的特征点，并计算各所述特征点的重投影误差；

将所述双目摄像单元在所述第一位姿变化信息作为通用图像优化算法的第一位姿节点；所述两帧图像中特征点坐标作为所述通用图像优化算法中的多个第一路标节点；将多个前帧特征点的重投影误差作为所述第一位姿节点分别连接所述多个第一路标节点的第一边；将所述第一位姿变化信息与所述第二位姿变化信息的差值作为连接所述第一位姿节点和第二位姿节点的第二边；将所述第一位姿节点、所述多个第一路标节点、所述第一位姿节点分别连接所述多个第一路标节点的第一边以及所述第二边作为输入，经所述通用图优化算法优化计算得到用于定位的第三位姿信息；所述多个前帧特征点为所述两帧图像的前一帧图像对应的特征点。

2.根据权利要求1所述的方法，其特征在于，所述根据双目摄像单元在连续的两个时刻采集到的两帧图像，计算第一位姿变化信息，包括：

从所述两帧图像的前一帧图像中，提取多个前帧特征点；

获取所述多个前帧特征点的坐标信息；

分别从所述两帧图像的后一帧图像中查找与所述多个前帧特征点相匹配的多个后帧特征点；

获取所述多个后帧特征点的坐标信息；

根据所述多个前帧特征点的坐标信息和所述多个后帧特征点的坐标信息，计算所述双目摄像单元的旋转变换矩阵及位移变化向量，以得到所述第一位姿变化信息。

3.根据权利要求2所述的方法，其特征在于，所述获取所述多个前帧特征点的坐标信息，包括：

获取所述多个前帧特征点在所述前一帧图像的左目图像和右目图像中的二维坐标；

根据所述多个前帧特征点的二维坐标、所述双目摄像单元的焦距及所述双目摄像单元的基线，计算所述多个前帧特征点的深度信息；

根据所述多个前帧特征点在所述前一帧图像的左目图像和右目图像中的二维坐标以及深度信息，得到所述多个前帧特征点的三维坐标。

4.根据权利要求2或3所述的方法，其特征在于，还包括：

若从所述两帧图像的后一帧图像中未查找到与所述前帧特征点相匹配的后帧特征点；

将所述第二位姿变化信息作为用于定位的第三位姿变化信息进行输出，直至从所述两帧图像的后一帧图像中查找到与所述多个前帧特征点匹配的多个后帧特征点为止。

5.根据权利要求1至3中任一项所述的方法，其特征在于，所述根据惯性测量单元在所述连续的两个时刻之间采集的惯性参数，计算第二位姿变化信息，包括：

对所述惯性测量单元在所述连续的两个时刻之间采集的惯性参数进行流形上的预积分，得到所述第二位姿变化信息。

6.根据权利要求5所述的方法，其特征在于，所述惯性参数包括：角速度和加速度；以及

所述对所述惯性测量单元在所述连续的两个时刻之间采集的惯性参数进行流形上的预积分，得到所述第二位姿变化信息，包括：

对所述惯性测量单元在所述连续的两个时刻之间采集的角速度进行流形上的预积分，得到所述惯性测量单元的旋转变换矩阵；

对所述惯性测量单元在所述连续两个时刻之间采集的加速度进行流形上的预积分，得到所述惯性测量单元的位移变化向量；

其中，所述第二位姿变化信息包括：所述惯性测量单元的旋转变换矩阵及所述惯性测量单元的位移变化向量。

7.根据权利要求1至3中任一项所述的方法，其特征在于，经所述通用图优化算法优化计算得到用来定位的所述第三位姿变化信息的同时，还得到所述两帧图像中相匹配的特征点的位置信息；以及

所述方法，还包括：

根据所述两帧图像中相匹配的特征点的位置信息，构建环境地图。