卫星弱观测条件下的多传感器融合高精度位姿估计算法

技术领域

本发明涉及多传感器融合高精度位姿估计算法领域，具体为卫星弱观测条件下的多传感器融合高精度位姿估计算法。

背景技术

针对卫星拒止以及卫星观测条件较差的超大型城市复杂环境，设计基于LiDAR/IMU/Camera的多源传感器融合的紧耦合LIV(LiDAR-Inertial-Vision)定位框架。开展视觉/惯性融合位姿估计、低成本激光雷达畸变问题、激光/惯性融合位姿估计、激光/惯性/视觉融合位姿估计等内容的研究。

发明内容

针对上述问题，本发明提供了卫星弱观测条件下的多传感器融合高精度位姿估计算法。

为实现上述目的，本发明提供如下技术方案：卫星弱观测条件下的多传感器融合高精度位姿估计算法，包括以下步骤：

步骤(1)：视觉/惯性融合位姿估计算法；

步骤(2)：激光雷达点云去畸变算法；

步骤(3)：激光/惯性/视觉融合位姿估计算法；

步骤(4)：北斗/激光雷达/视觉/惯导松耦合的全局轨迹优化算法。

优选的，所述步骤(1)具体包括：针对惯性器件在图像序列帧之间的相对测量进行研究，开发基于地球坐标系力学编排INS和双速率求解的IMU预积分算法，实现INS与VO的时间和空间对齐，围绕视觉传感器前端位姿解算方法进行研究，实现基于灰度不变性假设的直接法和基于纹理特征突出的间接法；依据VIO在城市挑战场景中的位姿发散情况，研制基于视觉/惯性传感器下的VIO工作状态机，实现依据发散情况自主切换VIO中位姿解算算法，提升VIO在挑战环境下的精度与鲁棒性。

优选的，所述步骤(2)具体包括：利用局部点云曲率变化提取出激光特征点信息，实现基于曲率大小的自适应平面特征和角点特征区分算；依据其他多源传感器解算出的位姿信息用于点云畸变位置的校正。

优选的，所述步骤(3)具体包括：围绕视觉传感器前端特征提取与匹配过程，研究实现用于空间描述信息的LK光流算法和特征点算法；对激光传感器前端提取的点特征、线特征、面特征的参数化展开研究。

优选的，所述步骤(4)具体包括：通过不同传感器解算得到的位姿作为图节点，空间几何约束构建节点间边约束来搭建图优化模型。面对RTK位姿解算的精度不固定问题，设计基于自适应的图网络权重调整算法。

优选的，所述步骤(4)还包括：针对闭环检测过程，研究基于视觉传感器和激光传感器的场景相似计算方法，基于视觉传感器，实现基于SIFT、SURF、ORB的传统视觉词袋场景描述算法和基于深度学习的场景匹配算法进行研究；基于激光传感器，对点云地图分割和匹配过程进行研究。同时项目拟通过RTK解的置信度缩小场景匹配过程中的搜索范围，提升闭环检测精度，消除长时间运行过程中的累积误差，实现全局轨迹位姿精度的融合优化。

与现有技术相比，本发明的有益效果如下：

1、针对惯性器件在图像序列帧之间的相对测量进行研究，开发基于地球坐标系力学编排INS和双速率求解的IMU预积分算法，实现INS与VO的时间和空间对齐。围绕视觉传感器前端位姿解算方法进行研究，实现基于灰度不变性假设的直接法和基于纹理特征突出的间接法。依据VIO在城市挑战场景中的位姿发散情况，研制基于视觉/惯性传感器下的VIO工作状态机，实现依据发散情况自主切换VIO中位姿解算算法，提升VIO在挑战环境下的精度与鲁棒性。

2、在局部优化算法中主要对点云配准中误差项进行研究，依据空间几何约束，实现基于点到直线配准、点到平面配准、直线到平面配准等多种ICP点云配准算法，验证ICP、PL-ICP、NICP、IMLS-ICP算法对畸变点云的抗干扰能力。在全局优化算法中，实现高斯牛顿的优化方法、NDT方法、分支定界匹配的优化算法和GO-ICP的点云配准算法，验证在离线情况下位姿解算精度提升。

附图说明

图1为本发明的卫星弱观测条件下的多传感器融合高精度位姿估计算法的流程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参照图1所示，本发明提供的卫星弱观测条件下的多传感器融合高精度位姿估计算法，包括以下步骤：

步骤(1)：视觉/惯性融合位姿估计算法；

步骤(2)：激光雷达点云去畸变算法；

步骤(3)：激光/惯性/视觉融合位姿估计算法；

步骤(4)：北斗/激光雷达/视觉/惯导松耦合的全局轨迹优化算法。

本发明针对卫星拒止以及卫星观测条件较差的超大型城市复杂环境，设计基于LiDAR/IMU/Camera的多源传感器融合的紧耦合LIV(LiDAR-Inertial-Vision)定位框架。开展视觉/惯性融合位姿估计、低成本激光雷达畸变问题、激光/惯性融合位姿估计、激光/惯性/视觉融合位姿估计等内容的研究。

在本发明一优选的实施例中，面向城市场景中光照变化和重复纹理的问题，研制基于视觉和惯性传感器的VIO算法，开发基于直接法和间接法的视觉惯性融合位姿解算算法。针对惯性器件在图像序列帧之间的相对测量进行研究，开发基于地球坐标系力学编排INS和双速率求解的IMU预积分算法，实现INS与VO的时间和空间对齐。围绕视觉传感器前端位姿解算方法进行研究，实现基于灰度不变性假设的直接法和基于纹理特征突出的间接法。依据VIO在城市挑战场景中的位姿发散情况，研制基于视觉/惯性传感器下的VIO工作状态机，实现依据发散情况自主切换VIO中位姿解算算法，提升VIO在挑战环境下的精度与鲁棒性。

在本发明一优选的实施例中，针对车辆快速运动造成的点云畸变问题，围绕激光SLAM前端的数据关联进行分析。依据激光SLAM前端解算位姿流程，展开对畸变点云特征、存在畸变信息下的点云配准算法进行研究；围绕三维激光点云特征提取问题，利用局部点云曲率变化提取出激光特征点信息，实现基于曲率大小的自适应平面特征和角点特征区分算；依据其他多源传感器解算出的位姿信息用于点云畸变位置的校正。围绕点云配准问题，分别从局部优化算法和全局优化算法两个方面进行研究。在局部优化算法中主要对点云配准中误差项进行研究，依据空间几何约束，实现基于点到直线配准、点到平面配准、直线到平面配准等多种ICP点云配准算法，验证ICP、PL-ICP、NICP、IMLS-ICP算法对畸变点云的抗干扰能力。在全局优化算法中，实现高斯牛顿的优化方法、NDT方法、分支定界匹配的优化算法和GO-ICP的点云配准算法，验证在离线情况下位姿解算精度提升。

在本发明一优选的实施例中，本发明将围绕激光/惯性/视觉传感器不同组合方式的融合位姿估计算法进行研究。面向城市场景中重复几何特征和弱纹理挑战情况，项目对不同传感器下的场景空间信息感知能力和数据融合方法展开研究。围绕视觉传感器前端特征提取与匹配过程，研究实现用于空间描述信息的LK光流算法和特征点算法；对激光传感器前端提取的点特征、线特征、面特征的参数化展开研究。依据空间几何一致性约束，设计基于雷达深度信息对传感器关键帧的投影约束方程，实现不同传感器下的时间和空间对齐算法。围绕激光数据帧与图像帧间的运动约束开展，实现不同传感器感知帧间的IMU预积分算法，用于整体的位姿优化矫正。

在本发明一优选的实施例中，针对基于GNSS/IMU/Visual/Lidar的SLAM后端优化问题，本项目将围绕构建基于多源传感器的图优化算法、GNSS辅助下的闭环检测算法和三维激光点云场景重构算法进行研究。研究基于GNSS/IMU/Visual/Lidar的多源传感器构建后端图优化算法，围绕多源传感器后端优化整体流程，对图网络优化模型构建、闭环检测展开研究。通过不同传感器解算得到的位姿作为图节点，空间几何约束构建节点间边约束来搭建图优化模型。面对RTK位姿解算的精度不固定问题，设计基于自适应的图网络权重调整算法。针对闭环检测过程，研究基于视觉传感器和激光传感器的场景相似计算方法。基于视觉传感器，实现基于SIFT、SURF、ORB的传统视觉词袋场景描述算法和基于深度学习的场景匹配算法进行研究；基于激光传感器，对点云地图分割和匹配过程进行研究。同时项目拟通过RTK解的置信度缩小场景匹配过程中的搜索范围，提升闭环检测精度，消除长时间运行过程中的累积误差，实现全局轨迹位姿精度的融合优化。

以下提供一具体的实施方式

实施例1

参照图1所示；卫星弱观测条件下的多传感器融合高精度位姿估计算法，具体包括：

步骤(1)：视觉/惯性融合位姿估计算法；

步骤(2)：激光雷达点云去畸变算法；

步骤(3)：激光/惯性/视觉融合位姿估计算法；

步骤(4)：北斗/激光雷达/视觉/惯导松耦合的全局轨迹优化算法。

在本发明中，步骤(1)具体包括：针对惯性器件在图像序列帧之间的相对测量进行研究，开发基于地球坐标系力学编排INS和双速率求解的IMU预积分算法，实现INS与VO的时间和空间对齐，围绕视觉传感器前端位姿解算方法进行研究，实现基于灰度不变性假设的直接法和基于纹理特征突出的间接法；依据VIO在城市挑战场景中的位姿发散情况，研制基于视觉/惯性传感器下的VIO工作状态机，实现依据发散情况自主切换VIO中位姿解算算法，提升VIO在挑战环境下的精度与鲁棒性。

在本发明中，步骤(2)具体包括：利用局部点云曲率变化提取出激光特征点信息，实现基于曲率大小的自适应平面特征和角点特征区分算；依据其他多源传感器解算出的位姿信息用于点云畸变位置的校正。

在本发明中，步骤(3)具体包括：围绕视觉传感器前端特征提取与匹配过程，研究实现用于空间描述信息的LK光流算法和特征点算法；对激光传感器前端提取的点特征、线特征、面特征的参数化展开研究。

在本发明中，步骤(4)具体包括：通过不同传感器解算得到的位姿作为图节点，空间几何约束构建节点间边约束来搭建图优化模型。面对RTK位姿解算的精度不固定问题，设计基于自适应的图网络权重调整算法。

在本发明中，步骤(4)还包括：针对闭环检测过程，研究基于视觉传感器和激光传感器的场景相似计算方法，基于视觉传感器，实现基于SIFT、SURF、ORB的传统视觉词袋场景描述算法和基于深度学习的场景匹配算法进行研究；基于激光传感器，对点云地图分割和匹配过程进行研究。同时项目拟通过RTK解的置信度缩小场景匹配过程中的搜索范围，提升闭环检测精度，消除长时间运行过程中的累积误差，实现全局轨迹位姿精度的融合优化。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。