[发明专利]VI-SLAM的优化方法、装置、设备及计算机可读介质

说明书

本发明提出一种VI‑SLAM的优化方法，包括：如果发生变化的观测量满足更新条件，则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化；根据重新线性化后的观测方程，对法向方程进行增量式更新；根据更新后的法向方程，对舒尔补方程进行增量式更新；采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解，获得观测量的最优解。本发明实施例通过利用VI‑SLAM问题特殊的稀疏结构提升优化效率，以及通过利用SLAM问题的增量特性提升优化效率。同时，本发明优化效率的提升不依赖于运动方式，既适用于持续扩展地图的运动方式，又适用于在已扩展的地图中来回运动。

技术领域

本发明涉及定位与制图技术领域，尤其涉及一种VI-SLAM的增量式优化方法及装置、设备和计算机可读介质。

背景技术

移动机器人，如仓储、物流、服务机器人、无人车、无人机等，能够协助或取代人类工作，有着广泛的应用前景，是目前科学技术发展最为活跃的领域之一。移动机器人的应用中一个至关重要的环节，是需要实时定位机器人在场景中的方位。只有获取了机器人的方位信息，才能控制其如何移动。目前主流的方案需要依靠激光雷达、差分GPS、高精度惯导系统等昂贵的硬件设备，或事先在场景中部署信号源或电磁轨道，才能实现精确定位和导航，极大限制了移动机器人的应用场景。SLAM(Simultaneous Localization and Mapping，同时定位与制图)技术仅通过低成本的相机、IMU(Inertial measurement unit，惯性测量单元)等传感器，即可在未知环境中实现移动机器人的自身定位，从而突破上述方案在硬件成本、应用场景方面的局限。其中，纯视觉SLAM(简称V-SLAM)融合视觉和IMU数据的SLAM称为Visual-Inertial SLAM，简称VI-SLAM。

在实际应用中，SLAM的定位精度和计算效率是最为重要的两项指标。现有的SLAM方法在精度和效率方面存在权衡。高精度的SLAM方案计算量大、功耗高，需要配备高性能计算设备，加大了硬件成本；高效的SLAM方案又往往需要牺牲定位精度，无法满足高精度的定位需求。VI-SLAM的计算分为前端和后端两个部分。前端负责从传感器中提取可用于判断机器人运动的观测量，比如提取场景在图像中的运动；后端由前端提取的观测量，同时推测机器人的运动以及场景的三维结构，比如随着机器人运动，场景在图像中不断放大，则可推测出机器人正在靠近场景。前端本质上可以看成是一个图像处理的过程，可以通过特定的硬件(比如GPU、FPGA等)提升计算效率。后端本质上是一个优化问题，只有通过降低优化算法复杂度才能提升计算效率。因此，如何在不牺牲优化精度的前提下，提升后端优化效率，是一个急需解决的技术问题。

发明内容

本发明实施例提供一种VI-SLAM的优化方法、装置、设备及计算机可读介质，以解决或缓解现有技术中的一个或多个技术问题。

第一方面，本发明实施例提供了一种VI-SLAM的优化方法，包括：

如果发生变化的观测量满足更新条件，则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化；

根据重新线性化后的观测方程，对法向方程进行增量式更新；

根据更新后的法向方程，对舒尔补方程进行增量式更新；

采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解，获得观测量的最优解。

结合第一方面，本发明在第一方面的第一种实施方式中，采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解时，所采用的预条件子为对角线带状子矩阵B，其中，所述对角线带状子矩阵B与更新后的舒尔补方程的原矩阵S维度相同，对角线带状子矩阵B的对角线处宽度为b的部分与原矩阵S相等，其余部分为0，b为正整数。