VI-SLAM的优化方法、装置、设备及计算机可读介质制造方法及图纸

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

【技术实现步骤摘要】
VI-SLAM的优化方法、装置、设备及计算机可读介质
本专利技术涉及定位与制图
，尤其涉及一种VI-SLAM的增量式优化方法及装置、设备和计算机可读介质。
技术介绍
移动机器人，如仓储、物流、服务机器人、无人车、无人机等，能够协助或取代人类工作，有着广泛的应用前景，是目前科学技术发展最为活跃的领域之一。移动机器人的应用中一个至关重要的环节，是需要实时定位机器人在场景中的方位。只有获取了机器人的方位信息，才能控制其如何移动。目前主流的方案需要依靠激光雷达、差分GPS、高精度惯导系统等昂贵的硬件设备，或事先在场景中部署信号源或电磁轨道，才能实现精确定位和导航，极大限制了移动机器人的应用场景。SLAM(SimultaneousLocalizationandMapping，同时定位与制图)技术仅通过低成本的相机、IMU(Inertialmeasurementunit，惯性测量单元)等传感器，即可在未知环境中实现移动机器人的自身定位，从而突破上述方案在硬件成本、应用场景方面的局限。其中，纯视觉SLAM(简称V-SLAM)融合视觉和IMU数据的SLAM称为Visual-InertialSLAM，简称VI-SLAM。在实际应用中，SLAM的定位精度和计算效率是最为重要的两项指标。现有的SLAM方法在精度和效率方面存在权衡。高精度的SLAM方案计算量大、功耗高，需要配备高性能计算设备，加大了硬件成本；高效的SLAM方案又往往需要牺牲定位精度，无法满足高精度的定位需求。VI-SLAM的计算分为前端和后端两个部分。前端负责从传感器中提取可用于判断机器人运动的观测量，比如提取场景在图像中的运动；后端由前端提取的观测量，同时推测机器人的运动以及场景的三维结构，比如随着机器人运动，场景在图像中不断放大，则可推测出机器人正在靠近场景。前端本质上可以看成是一个图像处理的过程，可以通过特定的硬件(比如GPU、FPGA等)提升计算效率。后端本质上是一个优化问题，只有通过降低优化算法复杂度才能提升计算效率。因此，如何在不牺牲优化精度的前提下，提升后端优化效率，是一个急需解决的技术问题。
技术实现思路
本专利技术实施例提供一种VI-SLAM的优化方法、装置、设备及计算机可读介质，以解决或缓解现有技术中的一个或多个技术问题。第一方面，本专利技术实施例提供了一种VI-SLAM的优化方法，包括：如果发生变化的观测量满足更新条件，则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化；根据重新线性化后的观测方程，对法向方程进行增量式更新；根据更新后的法向方程，对舒尔补方程进行增量式更新；采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解，获得观测量的最优解。结合第一方面，本专利技术在第一方面的第一种实施方式中，采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解时，所采用的预条件子为对角线带状子矩阵B，其中，所述对角线带状子矩阵B与更新后的舒尔补方程的原矩阵S维度相同，对角带状子矩阵B的对角线处宽度为b的部分与原矩阵S相等，其余部分为0，b为正整数。结合第一方面的第一种实施方式，本专利技术在第一方面的第二种实施方式中，采用预条件共轭梯度法对更新后舒尔补方程进行增量式求解时，若更新后的舒尔补方程的上一次迭代结果|Ci|大于设定阈值，则本次迭代初值Ci＝0，否则用上一次迭代结果Ci作为本次迭代初值。结合第一方面，本专利技术在第一方面的第三种实施方式中，一组观测量包括一组相机参数和一个三维点坐标，所述方法还包括：根据多组观测量构造集束调整的优化方程，所述优化方程中的每个观测方程关联一组相机参数和一个三维点坐标；其中，如果m组相机参数为C＝{C1，C2，···，Cm}，n个三维点坐标为X＝{X1，X2，···，Xn}，则所述优化方程为：其中，fij为观测方程，每个观测方程fij关联一组相机参数Ci和一个三维点Xj；将观测方程fij线性化为：其中，eij为重投影误差项，为相机参数的雅可比矩阵，为三维点的雅可比矩阵；根据线性化后的观测方程，构造法向方程：其中，根据法向方程构造舒尔补方程：其中，矩阵向量i1在1-m之间，i2在1-m之间。结合第一方面的第三种实施方式，本专利技术在第一方面的第四种实施方式中，如果发生变化的观测量满足更新条件，则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化的步骤，包括：当相机参数差异|Ci|大于设定阈值、三维点坐标差异|Xj|大于设定阈值或接收到最新一帧的观测量作为增量时，重新线性化观测方程；所述根据重新线性化后的观测方程，对法向方程进行增量式更新的步骤，包括：根据重新线性化的观测方程，更新法向方程中的和并采用更新后的和更新法向方程。结合第一方面的第四种实施方式，本专利技术在第一方面的第五种实施方式中，所述根据重新线性化的观测方程，更新法向方程中的和并采用更新后的和更新法向方程的步骤，包括：如果重新线性化的观测方程为则采用eij+、和更新法向方程中的和如果和在更新前上标为-，更新后的上标为+，则根据更新前后的和增量式更新法向方程中的和其中：上标-表示更新前的值，上标+表示更新后的值。结合第一方面的第五种实施方式，本专利技术在第一方面的第六种实施方式中，所述根据重新线性化后的观测方程，对舒尔补方程进行增量式更新的步骤，包括：如果观测方程或需要更新，或三维点j关联于需要更新的观测方程，则更新和采用更新后的和更新舒尔补方程的和其中，上标-表示更新前的值，上标+表示更新后的值。第二方面，本专利技术实施例提供了一种VI-SLAM的优化装置，包括：重新线性化模块，用于如果发生变化的观测量满足更新条件，则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化；法向方程更新模块，用于根据重新线性化后的观测方程，对法向方程进行增量式更新；舒尔补方程更新模块，用于根据更新后的法向方程，对舒尔补方程进行增量式更新；求解模块，用于采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解，获得观测量的最优解。结合第二方面，本专利技术在第二方面的第一种实施方式中，所述求解模块在求解过程中，所采用的预条件子为对角线带状子矩阵B，其中，所述对角线带状子矩阵B与更新后的舒尔补方程的原矩阵S维度相同，对角带状子矩阵B的对角线处宽度为b的部分与原矩阵S相等，其余部分为0，b为正整数。结合第二方面的第一种实施方式，本专利技术在第二方面的第二种实施方式中，所述求解模块在求解过程中，若更新后的舒尔补方程的上一次迭代结果|Ci|大于设定阈值，则本次迭代初值Ci＝0，否则用上一次迭代结果Ci作为本次迭代初值。结合第二方面，本专利技术在第二方面的第三种实施方式中，一组观测量包括一组相机参数和一个三维点坐标，所述装置还包括：优化方程构造模块，用于根据多组观测量构造集束调整的优化方程，所述优化方程中的每个观测方程关联一组相机参数和一个三维点坐标；其中，如果m组相机参数为C＝{C1，C2，···，Cm}，n个三维点坐标为X＝{X1，X2，···，Xn}，则所述优化方程为：其中，fij为观测方程，每个观测方程fij关联一组相机参数Ci和一个三维点Xj；线性化模块，用于将观测方程fij线性化为：其中，eij为重投影误差项，为相机参数的雅可比矩阵，为三维点的雅可比矩阵；法向方本文档来自技高网...

【技术保护点】
1.一种VI‑SLAM的优化方法，其特征在于，包括：如果发生变化的观测量满足更新条件，则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化；根据重新线性化后的观测方程，对法向方程进行增量式更新；根据更新后的法向方程，对舒尔补方程进行增量式更新；采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解，获得观测量的最优解。

【技术特征摘要】
1.一种VI-SLAM的优化方法，其特征在于，包括：如果发生变化的观测量满足更新条件，则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化；根据重新线性化后的观测方程，对法向方程进行增量式更新；根据更新后的法向方程，对舒尔补方程进行增量式更新；采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解，获得观测量的最优解。2.根据权利要求1所述的方法，其特征在于，采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解时，所采用的预条件子为对角线带状子矩阵B，其中，所述对角线带状子矩阵B与更新后的舒尔补方程的原矩阵S维度相同，对角带状子矩阵B的对角线处宽度为b的部分与原矩阵S相等，其余部分为0，b为正整数。3.根据权利要求2所述的方法，其特征在于，采用预条件共轭梯度法对更新后舒尔补方程进行增量式求解时，若更新后的舒尔补方程的上一次迭代结果|Ci|大于设定阈值，则本次迭代初值Ci＝0，否则用上一次迭代结果Ci作为本次迭代初值。4.根据权利要求1所述的方法，其特征在于，一组观测量包括一组相机参数和一个三维点坐标，所述方法还包括：根据多组观测量构造集束调整的优化方程，所述优化方程中的每个观测方程关联一组相机参数和一个三维点坐标；其中，如果m组相机参数为C＝{C1，C2，…，Cm}，n个三维点坐标为X＝{X1，X2，…，Xn}，则所述优化方程为：其中，fij为观测方程，每个观测方程fij关联一组相机参数Ci和一个三维点Xj；将观测方程fij线性化为：其中，eij为重投影误差项，为相机参数的雅可比矩阵，为三维点的雅可比矩阵；根据线性化后的观测方程，构造法向方程：其中，根据法向方程构造舒尔补方程：其中，矩阵向量i1在1-m之间，i2在1-m之间。5.根据权利要求4所述的方法，其特征在于，如果发生变化的观测量满足更新条件，则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化的步骤，包括：当相机参数差异|Ci|大于设定阈值、三维点坐标差异|Xj|大于设定阈值或接收到最新一帧的观测量作为增量时，重新线性化观测方程；所述根据重新线性化后的观测方程，对法向方程进行增量式更新的步骤，包括：根据重新线性化的观测方程，更新法向方程中的和并采用更新后的和更新法向方程。6.根据权利要求5所述的方法，其特征在于，所述根据重新线性化的观测方程，更新法向方程中的和并采用更新后的和更新法向方程的步骤，包括：如果重新线性化的观测方程为则采用eij+、和更新法向方程中的和如果和在更新前上标为-，更新后的上标为+，则根据更新前后的和增量式更新法向方程中的和其中：上标-表示更新前的值，上标+表示更新后的值。7.根据权利要求6所述的方法，其特征在于，所述根据重新线性化后的观测方程，对舒尔补方程进行增量式更新的步骤，包括：如果观测方程或需要更新，或三维点j关联于需要更新的观测方程，则更新和采用更新后的和更新舒尔补方程的和其中，上标-表示更新前的值，上标+表示更新后的值。8.一种VI-SLAM的优化装置，其特征在于，包括：重新线性化模块，用于如果发生变化的观测量满足更新条件，则根据发生变化的观测量对集束调整的优化方程中...

【专利技术属性】
技术研发人员：刘浩敏，陈明裕，包英泽，王志昊，
申请(专利权)人：百度在线网络技术北京有限公司，
类型：发明
国别省市：北京,11