【技术实现步骤摘要】
本专利技术涉及无人系统自主导航和环境感知，特别是涉及一种基于4d毫米波雷达和imu融合的定位与建图方法及系统。
技术介绍
1、slam代表同时定位与地图构建(simultaneous localization and mapping)，是一种机器人或无人驾驶车辆能够在未知环境中实现自主定位和建立环境地图的技术。imu(惯性测量单元)是一种集成了加速度计和陀螺仪的传感器装置。它可以测量物体的加速度、角速度和姿态等动态信息。imu可以提供高频率的姿态更新，对于快速运动的机器人或车辆的定位至关重要。
2、传统的slam系统主要依赖于激光雷达进行环境感知和距离测量。激光雷达可以提供高精度的距离数据，配合imu的姿态和运动信息，可以实现较为准确的slam定位和地图构建。
3、但是，激光雷达不仅价格昂贵，且对于一些特定环境，如透明或反射表面的物体无法提供准确的数据，在雨雪等遮挡环境下数据量会骤降，这会导致使用slam获得的地图的不完整或不准确。另外，现有的slam方法在处理大规模环境时会存在计算和处理延迟，实时性能差。
技术实现思路
1、基于此，有必要针对上述技术问题，提供一种环境感知力广泛、成本低且实时性能好的基于4d毫米波雷达和imu融合的定位与建图方法及系统。
2、第一方面，本专利技术提供了一种基于4d毫米波雷达和imu融合的定位与建图方法，包括以下步骤：
3、将4d毫米波雷达获得的4d点云数据通过点云预处理算法进行去噪获得稳定的4d点云数
4、根据稳定的4d点云数据解算出自我平台的三维体速度；
5、融合稳定的4d点云数据以及惯导器件的位姿数据，优化解算出自我平台的里程计信息；
6、融合自我平台的里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4d点云数据和最优里程计信息绘制出环境点云地图。
7、在其中一个实施例中，根据稳定的4d点云数据解算出自我平台的三维体速度，包括：.
8、通过稳定的4d点云数据的多维信息构建最小二乘求解模型；
9、利用线性最小二乘求解方法对最小二乘求解模型进行求解获得自我平台的三维体速度。
10、在其中一个实施例中，融合稳定的4d点云数据以及惯导器件的位姿数据以及融合自我平台的里程计信息和三维体速度均采用图优化模型以滑动窗口方式进行融合。
11、在其中一个实施例中，最小二乘求解模型为
12、
13、式中，是测得的多普勒速度，n为测量值总个数，r代表雷达坐标系，vr表示雷达速度。
14、在其中一个实施例中，图优化模型的因子建模结果包括imu预积分因子、里程计因子和速度先验因子。
15、在其中一个实施例中，imu预积分因子为
16、
17、式中，运算符vec(·)用于提取四元数的矢量部分，m代表第m帧，代表旋转矩阵，分别代表系统的位置、速度、姿态状态，ba,m、bg,m代表加速度计和陀螺仪的偏置，代表预积分量，gw代表重力参数，δτm为高斯白噪声参数。
18、在其中一个实施例中，里程计因子为
19、
20、式中，表示四元数乘法运算符，vec(·)用于提取四元数的矢量部分，分别表示m帧的位姿的逆和预测位姿，δpm和分别表示位置变化量和预测位置变化量。
21、在其中一个实施例中，速度先验因子为
22、
23、式中，vradar是一个先验速度估计值，是估计速度。
24、第二方面，本专利技术还提供了一种基于4d毫米波雷达和imu融合的定位与建图装置。装置包括：
25、点云预处理模块，用于将4d毫米波雷达获得的4d点云数据通过点云预处理算法进行去噪获得稳定的4d点云数据；
26、雷达自运动估计模块，用于根据稳定的4d点云数据解算出自我平台的三维体速度；
27、位姿解算模块，用于融合稳定的4d点云数据以及惯导器件的位姿数据，优化解算出自我平台的里程计信息；
28、基于图优化的多传感器融合slam模块，用于融合自我平台的里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4d点云数据和最优里程计信息绘制出环境点云地图。
29、本专利技术的有益效果是：
30、(1)本专利技术通过将4d毫米波雷达与imu相结合，利用4d毫米波雷达的高精度距离信息和imu的高频率姿态信息，实现精确的定位和地图构建。4d毫米波雷达能够提供准确的环境感知和障碍物检测，而imu则可以提供连续的姿态更新。通过将两者进行融合，可以克服彼此的局限性，提高slam系统的鲁棒性和精度，增加了slam系统实时性能。
31、(2)4d毫米波雷达价格低，能够降低整个slam系统的成本，并且对透明和反射表面具有较好的测量能力，在雨雪等遮挡环境下数据量也比较充沛，保证slam系统最终建图的完整性和准确性。
本文档来自技高网...【技术保护点】
1.一种基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，根据所述稳定的4D点云数据解算出自我平台的三维体速度，包括：
3.根据权利要求2所述的基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，融合所述稳定的4D点云数据以及惯导器件的位姿数据以及融合所述自我平台的里程计信息和三维体速度均采用图优化模型以滑动窗口方式进行融合。
4.根据权利要求3所述的基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，所述最小二乘求解模型为
5.根据权利要求3所述的基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，所述图优化模型的因子建模结果包括IMU预积分因子、里程计因子和速度先验因子。
6.根据权利要求5所述的基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，所述IMU预积分因子为
7.根据权利要求5所述的基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，所述里程计因
8.根据权利要求5所述的基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，所述速度先验因子为
9.一种基于4D毫米波雷达和IMU融合的定位与建图系统，其特征在于，所述系统包括：
...【技术特征摘要】
1.一种基于4d毫米波雷达和imu融合的定位与建图方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的基于4d毫米波雷达和imu融合的定位与建图方法，其特征在于，根据所述稳定的4d点云数据解算出自我平台的三维体速度，包括：
3.根据权利要求2所述的基于4d毫米波雷达和imu融合的定位与建图方法，其特征在于，融合所述稳定的4d点云数据以及惯导器件的位姿数据以及融合所述自我平台的里程计信息和三维体速度均采用图优化模型以滑动窗口方式进行融合。
4.根据权利要求3所述的基于4d毫米波雷达和imu融合的定位与建图方法，其特征在于，所述最小二乘求解模型为
5...
【专利技术属性】
技术研发人员：吕洋，花林，赵春晖，易世泉，刘准钆，潘泉，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。