一种同步定位与建图方法及系统

本发明涉及计算机视觉、机器人导航与状态估计，尤其涉及一种同步定位与建图方法及系统。

背景技术：

1、随着自动驾驶、移动机器人、增强现实等技术的飞速发展，能够在未知环境中实现自身精确定位与环境地图构建的即时定位与地图构建（simultaneous localization andmapping，slam）技术变得至关重要。视觉slam通过相机采集图像信息进行位姿估计与建图，具有成本低、信息丰富的优点。当前大多数视觉惯性slam系统采用传统的特征点法或直接法。其中，特征点法提取每个图像中的显著图像特征，使用不变特征描述在连续帧中进行特征点匹配，使得对极几何结构稳健地恢复相机的姿态和结构，采用关联的特征完成视觉组合计算和基于最小化投影误差的位姿优化，这些提取的显著特征可以聚类来描述整幅图像用以回环检测，但是图像特征点的提取与关联匹配是比较繁琐且耗时的工作。直接法可通过光度误差直接恢复相机的姿态和结构，而无需提取特征。但是，直接法不具备回环检测能力，致使其在长期导航中具有较大的位姿漂移和积累误差的问题。

技术实现思路

1、为了解决现有技术所存在的技术问题，本发明提供一种同步定位与建图方法及系统，将视觉惯性紧耦合，显著提升了在场景下的定位精度和系统鲁棒性。

2、本发明提供一种同步定位与建图方法，包括：确定视觉位姿扰动与惯性测量单元状态扰动之间的转换关系；确定视觉残差和确定惯性预积分残差；基于所述转换关系，对所述视觉残差和所述惯性预积分残差进行耦合联合优化迭代求解，直至收敛，得到同步定位与建图结果。

3、根据本发明提供的一种同步定位与建图方法，所述确定视觉位姿扰动与惯性测量单元状态扰动之间的转换关系，包括：采用左扰动模型，对相机位姿进行参数化；通过局部平移和旋转增量线性化，确定惯性测量单元状态；基于参数化的相机位姿和惯性测量单元状态，通过相机与惯性测量单元之间的外参，建立视觉位姿扰动与惯性测量单元位姿扰动之间的转换关系。

4、根据本发明提供的一种同步定位与建图方法，所述确定视觉残差，包括：根据输入的视频流确定帧图；所述帧图用于存储共视图像帧之间的边；对于所述帧图中相连的边，通过特征提取网络提取关联特征和光流特征，通过上下文网络提取上下文特征；所述关联特征用于提供视觉相似性信息；根据当前估计的位姿和深度，计算帧间稠密对应关系场；根据所述关联特征、光流特征和所述上下文特征，通过卷积更新算子，预测光流的修正项及置信权重，并对所述帧间稠密对应关系场进行修正，得到修正后的对应关系场；根据修正后的对应关系场优化位姿和深度，通过优化的位姿和深度计算投影误差，得到所述视觉残差。

5、根据本发明提供的一种同步定位与建图方法，所述确定惯性预积分残差，包括：对于连续图像帧之间的惯性测量单元的测量值进行预积分，得到相对运动增量；根据所述相对运动增量和所述视觉残差，计算所述惯性预积分残差。

6、根据本发明提供的一种同步定位与建图方法，还包括：执行视觉惯性初始化步骤；所述视觉惯性初始化步骤包括陀螺仪偏置标定、速度、重力、尺度初始化、重力细化和世界坐标系对齐中的至少一种。

7、根据本发明提供的一种同步定位与建图方法，进一步包括：利用视觉旋转与惯性测量单元预积分旋转构建最小二乘问题，标定初始陀螺仪偏置；联立imu预积分的位置、速度残差与视觉几何关系，构建线性方程，求解各帧速度、重力向量方向及尺度因子。

8、根据本发明提供的一种同步定位与建图方法，所述基于所述转换关系，对所述视觉残差和所述惯性预积分残差进行耦合联合优化迭代求解，直至收敛，得到同步定位与建图结果，包括：基于所述转换关系，将所述视觉残差和所述惯性预积分残差置于同一个非线性最小二乘问题中联合优化，迭代求解所有状态变量的最优增量，直至收敛，得到位姿轨迹和地图。

9、本发明还提供一种同步定位与建图系统，包括：第一模块，用于确定视觉位姿扰动与惯性测量单元状态扰动之间的转换关系；第二模块，用于确定视觉残差和确定惯性预积分残差；第三模块，用于基于所述转换关系，对所述视觉残差和所述惯性预积分残差进行耦合联合优化迭代求解，直至收敛，得到同步定位与建图结果。

10、根据本发明提供的一种同步定位与建图系统，所述第二模块用于：根据输入的视频流确定帧图；所述帧图用于存储共视图像帧之间的边；对于所述帧图中相连的边，通过特征提取网络提取关联特征和光流特征，通过上下文网络提取上下文特征；所述关联特征用于提供视觉相似性信息；根据当前估计的位姿和深度，计算帧间稠密对应关系场；根据所述关联特征、光流特征和所述上下文特征，通过卷积更新算子，预测光流的修正项及置信权重，并对所述帧间稠密对应关系场进行修正，得到修正后的对应关系场；根据修正后的对应关系场优化位姿和深度，通过优化的位姿和深度计算投影误差，得到所述视觉残差；对于连续图像帧之间的惯性测量单元的测量值进行预积分，得到相对运动增量；根据所述相对运动增量和所述视觉残差，计算所述惯性预积分残差。

11、根据本发明提供的一种同步定位与建图系统，还包括：第四模块，用于执行视觉惯性初始化步骤；所述视觉惯性初始化步骤包括陀螺仪偏置标定、速度、重力、尺度初始化、重力细化和世界坐标系对齐中的至少一种。

12、本发明提供的一种同步定位与建图方法及系统，将视觉惯性紧耦合，显著提升了在场景下的定位精度和系统鲁棒性。

1.一种同步定位与建图方法，其特征在于，包括：

2.根据权利要求1所述的同步定位与建图方法，其特征在于，所述确定视觉位姿扰动与惯性测量单元状态扰动之间的转换关系，包括：

3.根据权利要求1所述的同步定位与建图方法，其特征在于，所述确定视觉残差，包括：

4.根据权利要求1所述的同步定位与建图方法，其特征在于，所述确定惯性预积分残差，包括：

5.根据权利要求1所述的同步定位与建图方法，其特征在于，还包括：

6.根据权利要求5所述的同步定位与建图方法，其特征在于，进一步包括：

7.根据权利要求1至6任一项所述的同步定位与建图方法，其特征在于，所述基于所述转换关系，对所述视觉残差和所述惯性预积分残差进行耦合联合优化迭代求解，直至收敛，得到同步定位与建图结果，包括：

8.一种同步定位与建图系统，其特征在于，包括：

9.根据权利要求8所述的同步定位与建图系统，其特征在于，所述第二模块用于：

10.根据权利要求8或9所述的同步定位与建图系统，其特征在于，还包括：