1.基于异构多模态数据融合的同步定位与建图方法，其特征在于，包括如下步骤：步骤1.通过时间同步和外参标定实现毫米波雷达与激光雷达的点云对齐，并标定IMU与毫米波雷达、激光雷达的相对位姿；其中，毫米波雷达采用4D毫米波雷达；

步骤2.采用激光雷达、毫米波雷达分别对环境进行三维感知，得到感知3D点云；

步骤3.对毫米波雷达数据进行预处理，过滤动态物体；

步骤4.将激光雷达获取的高分辨率点云及毫米波雷达获取并经过预处理后的低分辨率静态点云，使用基于距离的插值法，实现激光雷达和毫米波雷达数据的融合与匹配；

计算激光雷达点转换到毫米波雷达点的差异，并作为误差项引入LIO‑SAM的因子图中，该误差项即毫米波雷达的融合误差；

步骤5.提取毫米波雷达的速度信息，并计算IMU的速度；通过构建误差函数，计算毫米波雷达速度与IMU速度之间的差异，为LIO‑SAM因子图提供额外的速度约束；

步骤6.使用毫米波雷达点云为LIO‑SAM因子图提供静态点云约束；

步骤7.使用毫米波雷达补充回环约束，使用加权投票机制，对毫米波雷达的回环检测结果与激光雷达的回环检测结果进行融合，确定回环检测最终结果；

步骤8.基于回环检测最终结果，构建激光雷达与毫米波雷达的联合回环约束；

步骤9.将步骤4的融合误差、步骤5的速度约束、步骤6的静态点云约束以及步骤8的联合回环约束作为约束因子，并添加到LIO‑SAM的因子图中，从而构建因子图；对因子图进行优化，获得系统的位姿估计和地图特征点分布，实现SLAM的同步定位与建图。

2.根据权利要求1所述的基于异构多模态数据融合的同步定位与建图方法，其特征在于，所述步骤1中，激光雷达和毫米波雷达处于不同的坐标系；

假设通过外参标定得到两个坐标系之间的相对位姿，即旋转矩阵R和平移向量t；

其中，旋转矩阵R描述了激光雷达坐标系相对于毫米波雷达坐标系的旋转；平移向量t描述了激光雷达坐标系原点相对于毫米波雷达坐标系原点的平移。

3.根据权利要求1所述的基于异构多模态数据融合的同步定位与建图方法，其特征在于，所述步骤3具体为：步骤3.1.首先进行目标提取，提取毫米波雷达点云中具有高反射强度的点；然后利用多普勒效应，提取目标的速度信息；

针对毫米波雷达数据中每个点云数据pi＝(xi,yi,zi)，(xi,yi,zi)表示点云坐标，毫米波雷达提供一个与该点云相关联的速度信息vi，表示该点云的移动速度；

步骤3.2.设定一个速度阈值vthresh，毫米波雷达数据中移动速度小于该速度阈值的点云被视为静态点云，移动速度超过该速度阈值的点云被视为动态点云；

步骤3.3.根据预设的速度阈值vthresh，分离毫米波雷达数据中的动态点云与静态点云；

将毫米波雷达数据中的动态点云移除，仅保留静态点云。

4.根据权利要求2所述的基于异构多模态数据融合的同步定位与建图方法，其特征在于，所述步骤4具体为：步骤4.1.通过外参标定获得的矩阵将激光雷达点云转换到毫米波雷达坐标系；

T T

假设激光雷达点pL＝[xL,yL,zL]和毫米波雷达点pR＝[xR,yR,zR]为在各自坐标系中的坐标，通过外参标定得到的旋转矩阵R和平移向量t进行坐标转换：pL‑R＝RpL+t；

其中，pL‑R是转换后的激光雷达点在毫米波雷达坐标系中的坐标；

对于转换后的特征点pL‑R＝[xL‑R,yL‑R,zL‑R]；

计算激光雷达点转换到毫米波雷达点的差异efusion‑lidar，并将其作为误差项引入LIO‑SAM的因子图中，该误差项即毫米波雷达的融合误差，其表达公式如下：efusion‑lidar＝||pL‑R‑pR||；

步骤4.2.对于每个激光雷达点，计算该激光雷达点在毫米波雷达坐标系中的相对位置，并根据毫米波雷达的分辨率进行插值；

假设毫米波雷达的数据是以从雷达到点的距离r和角度(θ,φ)来表示的，对于转换后的激光雷达点pR，计算该点pR在毫米波雷达网格中的相应坐标，公式如下：其中，θ表示目标点相对于毫米波雷达的水平角度，即毫米波雷达探测到目标点的方位角，φ表示目标点相对于毫米波雷达的俯仰角，适用于三维空间中的极坐标系；

步骤4.3.对于每个激光雷达点，找到在毫米波雷达点云中通过kd树查找到的最近的点或最接近的区域，并使用线性插值方法填充缺失的点；

对于每个激光雷达点pL，转换后的激光雷达点在毫米波雷达坐标系中的坐标点为pR，使用KD树查找在毫米波雷达点云中最近邻点 计算其空间距离：其中d是转换后的激光雷达点在毫米波雷达坐标系中的坐标点，即pR和毫米波雷达点云中最近邻点 之间的距离；

步骤4.4.使用线性插值来计算毫米波雷达数据中与激光雷达点云相对应的值；根据激光雷达点的空间位置，在毫米波雷达的输出数据网格中找到对应的值来填充；

假设在毫米波雷达的点云中，激光雷达点pL对应的值是vL，而毫米波雷达点云中最近邻点 对应的值是vR，则线性插值按以下公式计算：其中， 表示激光雷达点云在毫米波雷达坐标系插值所在的点云坐标，dmax表示当前激光雷达点与毫米波雷达点云中最近邻点对插值有效的最大范围。

5.根据权利要求1所述的基于异构多模态数据融合的同步定位与建图方法，其特征在于，所述步骤5具体为：步骤5.1.提取毫米波雷达的速度信息；

假设毫米波雷达获取到的速度信息为vmmWave，其表示从雷达到目标物体的相对速度；

步骤5.2.估算IMU的速度；

根据IMU提供的加速度数据得到速度vIMU，IMU速度估计的公式如下：其中，a(τ)是IMU在时间τ的加速度函数；

步骤5.3.基于毫米波雷达的速度vmmWave和IMU的速度vIMU，构建误差函数，通过误差函数计算二者误差，并作为速度约束引入LIO‑SAM的因子图优化中；

度量IMU估计的速度与毫米波雷达速度之间的差异的误差函数公式如下：espeed＝vIMU(t)‑vmmWave(t)；

其中，espeed是IMU和毫米波雷达速度估计之间的误差函数，vIMU(t)是IMU通过加速度积分得到的速度，vmmWave(t)是通过毫米波雷达测量得到的物体相对速度。

6.根据权利要求1所述的基于异构多模态数据融合的同步定位与建图方法，其特征在于，所述步骤6中，使用步骤3预处理后的毫米波雷达静态点云，构建如下误差函数：其中，emmWave为毫米波雷达静态点云引入的误差， 为毫米波雷达观测到的点或目标特征的位置， 为根据当前估计状态计算出的毫米波雷达目标特征的位置。

7.根据权利要求1所述的基于异构多模态数据融合的同步定位与建图方法，其特征在于，所述步骤7具体为：步骤7.1.目标特征匹配；

提取毫米波雷达点云的目标特征，对毫米波雷达点云提取的目标特征，建立特征描述符，使用特征描述符在已构建SLAM历史地图中搜索最匹配的目标特征；

步骤7.2.使用毫米波雷达补充回环约束，使用加权投票机制，对毫米波雷达的回环检测结果与激光雷达的回环检测结果进行融合，确定最终回环检测结果。

8.根据权利要求7所述的基于异构多模态数据融合的同步定位与建图方法，其特征在于，所述步骤7.2中，采用加权投票机制确定回环检测结果的具体过程如下：步骤7.2.1.设置权重；

根据环境和传感器特性，设置激光雷达和毫米波雷达的权重wLiDAR和wmmWave；

在特征丰富环境中，激光雷达的权重比毫米波雷达的权重高，即wLiDAR>wmmWave，在低特征或动态环境中，毫米波雷达的权重比激光雷达的权重高，即wLiDAR

步骤7.2.2.计算综合得分；

基于当前帧与历史地图帧的匹配关系，筛选出所有可能的与当前帧形成回环的历史帧或地图点的集合的点作为候选匹配点i；

计算候选匹配点i的综合相似性得分 公式如下：

其中，i为候选匹配点的索引；

表示激光雷达和毫米波雷达相似性得分的加权和；

为激光雷达对于候选匹配点i的相似性得分， 得分越高，表示激光雷达认为该历史帧i是当前帧的回环匹配的可能性越大；

为毫米波雷达对于候选匹配点i的相似性得分， 得分越高，表示毫米波雷达认为该历史帧i是当前帧的回环匹配的可能性越大；

步骤7.2.3选取综合得分最高的候选帧作为最终回环匹配；

依据步骤7.2.2得到的综合相似性得分，选取最匹配的历史帧作为回环点，为后续的回环约束建立基础，最终选定的回环匹配点索引的计算公式如下：其中ibest为最终选定的回环匹配点索引，表示综合相似性得分 最高的候选匹配点， 表示从所有候选匹配点i中选取综合相似性得分 最大的那个点的索引。

9.根据权利要求8所述的基于异构多模态数据融合的同步定位与建图方法，其特征在于，所述步骤8具体为：激光雷达的回环约束如下所示：

eloopLiDAR＝||Tcurr·Trel‑TloopLiDAR||；

其中eloopLiDAR表示激光雷达的回环约束，Tcurr为当前帧激光雷达的位姿，TloopLiDAR为激光雷达回环帧的位姿，Trel为激光雷达两帧之间的相对变换；

毫米波雷达的约束如下所示：

其中eloopmmWave表示毫米波雷达的约束， 表示当前帧毫米波雷达的位姿，表示为毫米波雷达的回环帧的位姿；prel为毫米波雷达两帧之间的相对变换；

构建激光雷达与毫米波雷达的联合回环约束如下所示：etotal＝wLiDAR·eloopLiDAR+wmmWave·eloopmmWave；

其中etotal为联合回环约束，wLiDAR为激光雷达权重，wmmWave为毫米波雷达权重。

10.根据权利要求1所述的基于异构多模态数据融合的同步定位与建图方法，其特征在于，所述步骤9具体为：在LIO‑SAM的因子图中通过添加毫米波雷达约束因子，与激光雷达约束、IMU约束，速度以及联合回环约束共同优化全局一致性，优化目标为最小化联合误差：其中，参数X表示SLAM系统状态变量，该参数X是需要估计的未知量，包含在时间序列中的位姿和环境中的特征点位置，位姿用于定位，地图特征用于建图；

表示激光雷达约束的平方和； 表示毫米波雷达约束平方和； 约束表示IMU的约束平方和； 为毫米波雷达与激光雷达的联合回环约束的平方和；espeed是IMU和毫米波雷达速度估计之间的误差函数；

为激光雷达与毫米波雷达的融合误差的平方和；

激光雷达误差项eLiDAR为激光雷达的观测与当前估计状态之间的误差；

其中， 为激光雷达观测的特征点或地图点的位置，

为根据当前估计状态计算出的特征点或地图点的位置；

毫米波雷达误差项emmWave为毫米波雷达观测结果与当前估计状态之间的误差；

eIMU为IMU误差项，为IMU数据与当前状态之间的误差；

其中， 为IMU测得的加速度， 为IMU

测得的角速度， 和 分别为根据当前估计状态计算出的加速度和角速度；

通过最小化联合误差函数并使用非线性最小二乘求解SLAM系统状态变量X，从而获得系统的位姿估计和地图特征点分布，进而实现SLAM的同步定位与建图。