【技术实现步骤摘要】
本专利技术涉及无人系统自主导航和环境感知，特别是涉及一种基于红外视觉、毫米波雷达和imu融合的定位与建图方法及系统。
技术介绍
1、slam代表同时定位与地图构建(simultaneous localization and mapping)，是一种机器人或无人驾驶车辆能够在未知环境中实现自主定位和建立环境地图的技术。slam技术旨在通过使用移动机器人上的传感器来同时实现定位自身位置和构建周围环境的地图，而不需要预先获取地图信息。这对于自主导航、环境感知和路径规划等应用非常重要。
2、在传统的slam系统中，常用的传感器包括激光雷达、摄像头和imu。激光雷达可以提供高精度的距离测量，但对于低反射率的表面或者特殊材质的物体存在限制。摄像头可以获取丰富的视觉信息，但在光照不足或者纹理较弱的情况下表现不佳。imu可以提供高频率的姿态和运动状态测量，但存在漂移问题。
技术实现思路
1、基于此，有必要针对上述技术问题，提供一种环境感知更全面且动态物体处理能力好的基于红外视觉、毫米波雷达和imu融合的定位与建图方法及系统。
2、第一方面，本专利技术提供了一种基于红外视觉、毫米波雷达和imu融合的定位与建图方法，包括以下步骤：
3、将红外图像进行循环处理，同时将循环处理输出的红外相机的位姿数据与惯导器件的位姿数据融合，优化解算出自我平台的第一里程计信息，循环处理包括图像对齐、点初始化、点位优化、地标初始化和剪枝处理；
4、将4d毫米波雷达获得的4d点
5、根据稳定的4d点云数据解算出自我平台的三维体速度；
6、融合稳定的4d点云数据以及惯导器件的位姿数据，优化解算出自我平台的第二里程计信息；
7、融合自我平台的第一里程计信息和第二里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4d点云数据和最优里程计信息绘制出环境点云地图。
8、在其中一个实施例中，图像对齐处理为通过构建图像金字塔模型，利用imu的姿态数据作为信息先验，通过直接法执行红外图像的配准，根据配准结果计算出自我平台的位姿信息。
9、在其中一个实施例中，点初始化处理为将红外图像划分为32×32个像素块，在像素块中通过设定阈值筛选出图像关键帧。
10、在其中一个实施例中，点位优化处理为通过细化像素块位置计算残差和比率消除不受约束的点。
11、在其中一个实施例中，地标初始化和剪枝处理为根据红外图像中设定的对应关系点确定初始化3d地标，同时根据红外图像位置和质量优化剪枝算法对深度不一致的地标进行剪枝。
12、在其中一个实施例中，将循环处理输出的红外相机的位姿数据与惯导器件的位姿数据融合，优化解算出自我平台的第一里程计信息包括：
13、根据惯导器件的测量值和循环处理输出的红外相机的位姿数据建立重投影误差模型；
14、利用重投影误差模型构建非线性优化问题；
15、根据非线性优化问题求解出自我平台的里程计信息。
16、在其中一个实施例中，融合稳定的4d点云数据以及惯导器件的位姿数据以及融合第一里程计信息和第二里程计信息和三维体速度均采用图优化模型以滑动窗口方式进行融合。
17、在其中一个实施例中，重投影误差模型为
18、
19、式中，是相机坐标系中的点坐标，是世界坐标系中对应的3d地标位置，是从imu帧中imu到相机坐标系的变换，是imu在世界坐标系中的变换，k是固有相机矩阵。
20、在其中一个实施例中，非线性优化问题的成本函数为
21、
22、式中，k表示在包含时间帧和关键帧的滑动窗口中正在处理的帧，l(k)表示在帧k中可观察到的一组地标，表示在帧k中可见的每个地标l的重投影误差的叠加向量，表示帧k中地标测量的协方差矩阵，表示imu测量的协方差矩阵。
23、第二方面，本专利技术还提供了一种基于红外视觉、毫米波雷达和imu融合的定位与建图系统，系统包括：
24、循环解算模块，用于将红外图像进行循环处理，同时将循环处理输出的红外相机的位姿数据与惯导器件的位姿数据融合，优化解算出自我平台的第一里程计信息，循环处理包括图像对齐、点初始化、点位优化、地标初始化和剪枝处理；
25、点云预处理模块，用于将4d毫米波雷达获得的4d点云数据通过点云预处理算法进行去噪获得稳定的4d点云数据；
26、雷达自运动估计模块，用于根据稳定的4d点云数据解算出自我平台的三维体速度；
27、位姿解算模块，用于融合稳定的4d点云数据以及惯导器件的位姿数据，优化解算出自我平台的第二里程计信息；
28、基于图优化的多传感器融合slam模块，用于融合第一里程计信息和第二里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4d点云数据和最优里程计信息绘制出环境点云地图。
29、本专利技术的有益效果是：
30、(1)本专利技术通过融合红外视觉、毫米波雷达和imu的数据，可以获得更全面、多样化的环境感知能力。红外视觉提供独特的热特征感知，毫米波雷达提供高精度的距离和速度测量，imu提供快速的姿态和运动状态估计，从而增强系统对周围环境的感知能力。
31、(2)本专利技术中，红外视觉能够在光照较差的情况下提供可靠的环境感知，毫米波雷达具备强大的穿透能力，imu提供快速响应和姿态校正能力，共同提高系统的适应性和稳定性，能够在光照不足、恶劣天气和动态环境等挑战性条件下表现出良好的鲁棒性。
32、(3)本专利技术中，红外视觉和毫米波雷达的数据融合能够补充传统视觉slam系统的局限性，imu的姿态校正和状态估计提高了定位的精度和稳定性，能够提供更准确和稳定的定位结果。
33、(4)本专利技术中，通过红外视觉和毫米波雷达的信息融合，可以区分动态物体和静态环境，动态物体处理能力好，从而能够避免姿态漂移或地图构建错误。
本文档来自技高网...【技术保护点】
1.一种基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，图像对齐处理为通过构建图像金字塔模型，利用IMU的姿态数据作为信息先验，通过直接法执行红外图像的配准，根据配准结果计算出自我平台的位姿信息。
3.根据权利要求1所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，点初始化处理为将红外图像划分为32×32个像素块，在所述像素块中通过设定阈值筛选出图像关键帧。
4.根据权利要求1所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，点位优化处理为通过细化像素块位置计算残差和比率消除不受约束的点。
5.根据权利要求1所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，地标初始化和剪枝处理为根据红外图像中设定的对应关系点确定初始化3D地标，同时根据红外图像位置和质量优化剪枝算法对深度不一致的地标进行剪枝。
6.根据权利要求2至5任意一项所述的基于
7.根据权利要求6所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，融合所述稳定的4D点云数据以及惯导器件的位姿数据以及融合所述第一里程计信息和第二里程计信息和三维体速度均采用图优化模型以滑动窗口方式进行融合。
8.根据权利要求7所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，所述重投影误差模型为
9.根据权利要求8所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，非线性优化问题的成本函数为
10.一种基于红外视觉、毫米波雷达和IMU融合的定位与建图系统，其特征在于，所述系统包括：
...【技术特征摘要】
1.一种基于红外视觉、毫米波雷达和imu融合的定位与建图方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的基于红外视觉、毫米波雷达和imu融合的定位与建图方法，其特征在于，图像对齐处理为通过构建图像金字塔模型，利用imu的姿态数据作为信息先验，通过直接法执行红外图像的配准，根据配准结果计算出自我平台的位姿信息。
3.根据权利要求1所述的基于红外视觉、毫米波雷达和imu融合的定位与建图方法，其特征在于，点初始化处理为将红外图像划分为32×32个像素块，在所述像素块中通过设定阈值筛选出图像关键帧。
4.根据权利要求1所述的基于红外视觉、毫米波雷达和imu融合的定位与建图方法，其特征在于，点位优化处理为通过细化像素块位置计算残差和比率消除不受约束的点。
5.根据权利要求1所述的基于红外视觉、毫米波雷达和imu融合的定位与建图方法，其特征在于，地标初始化和剪枝处理为根据红外图像中设定的对应关系点确定初始化3d地标，同时根...
【专利技术属性】
技术研发人员：吕洋，花林，赵春晖，易世泉，刘准钆，潘泉，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。