基于全约束的3D激光SLAM方法、系统、介质及设备

本发明属于机器人同时定位和建图领域，提供了基于全约束的3D激光SLAM方法、系统、介质及设备，其技术方案为通过计算点云几何约束和强度图像强度约束对优化方向约束强度的贡献值，选择其中贡献值更大的一方约束优化方向，得到对系统的全约束观测，实现几何约束和强度约束互补的强约束系统在退化方向上添加额外约束实现全约束的LIO系统；在系统更新步骤，通过推导强度图像帧间匹配的残差及相应的雅可比矩阵，结合点云点面匹配的残差和雅可比矩阵，共同实现对系统的全约束状态更新。在退化方向上额外引入约束，使用拉格朗日乘子法，修正系统的全约束状态更新。通过以上步骤，实现各方向均稳健的SLAM系统，提高了SLAM系统的稳健性。