【技术实现步骤摘要】
本专利技术属于遥感影像处理，具体涉及一种基于毫米波雷达的slam方法及系统。
技术介绍
1、现有的基于摄像机和激光雷达的即时定位与地图构建（simultaneouslocalization and mapping，slam）系统在采集环境数据时容易受到光照、恶劣天气以及物体遮挡的影响，导致视觉、激光slam系统在上述环境中运行时不能满足一定的定位精度要求。近年来，由于毫米波具有较强的穿透能力并且能够采集到密集数据的特性，使得基于毫米波雷达的slam技术备受关注。与激光不同，毫米波雷达信号波长较长，可以更好地绕过物体并提供被物体部分遮挡的环境数据，使得毫米波雷达slam具有抵抗不利天气条件的影响，如雨、雾等。
2、毫米波slam中常用的毫米波雷达技术有调频连续波雷达、脉冲压缩雷达等。本专利技术中毫米波雷达技术采取的是调频连续波雷达技术（frequency modulated continuouswave，fmcw）。调频连续波技术能够确保毫米波slam技术中能够采集到密集的数据点。fmcw技术可以使雷达发射更大能量的信号，从而通过调制信号的频率实现较长距离的探测。fmcw雷达可以用相位信息测量速度，本专利技术是基于旋转的fmcw雷达的slam结构，产生密集的距离测量。
技术实现思路
1、针对现有技术的不足，本专利技术提供一种基于毫米波雷达的slam方法及系统，使用基于调频连续波（fmcw）技术的毫米波雷达进行数据采集，可以有效抵抗不利天气条件的影响，确保采集到稳定的数据，
2、为了实现上述目的，本专利技术提供一种基于毫米波雷达的slam方法，包括以下步骤：
3、步骤1，使用毫米波雷达获取周围环境的极坐标图像，并对毫米波雷达图像进行预处理；
4、步骤2，基于步骤1预处理后的图像进行特征提取；
5、步骤3，将步骤2提取的特征点转换到笛卡尔坐标系下；
6、步骤4，使用ransac算法得到相邻帧图像间的位姿；
7、步骤5，使用前一帧图像的速度估计校正当前帧图像的特征点运动；
8、步骤6，进行回环检测，判断在环境中前进时是否返回到了先前经过的位置；
9、步骤7，使用因子图方法对毫米波雷达的位姿进行优化；
10、步骤8，根据优化后的毫米波雷达的位姿和特征点，构建周围环境的点云重建模型。
11、而且，所述步骤1中毫米波雷达图像采用极坐标系，原点在图像左上角，横轴表示距离，纵轴表示方位角。预处理包括强度校正、高斯滤波、均值滤除，强度校正计算公式如下：
12、（1）
13、（2）
14、（3）
15、式中，为整幅毫米波雷达图像的平均强度，m、n表示毫米波雷达图像像素的行、列总数，表示毫米波雷达图像第r行、第c列像素的强度，表示毫米波雷达图像第r行强度的均值，表示毫米波雷达图像第r行、第k列像素的强度，为毫米波雷达图像第r行、第c列像素经过均衡化后的校正强度，为调整图像强度均衡化的参数。
16、图像经过强度校正后，每行的图像强度会形成一个强度曲线，设置高强度阈值，保留经过强度校正后图像中强度高于高强度阈值的峰值特征，然后对每行分别进行高斯滤波，滤除高频低强度噪声，高斯滤波计算公式如下：
17、（4）
18、式中，为滤波后的结果，表示圆周率，表示标准差，e表示自然常数， x表示图像的列数。
19、设置低强度阈值，保留高斯滤波后强度低于低强度阈值的低强度连续特征，然后计算图像各行强度的平均值，用各行经过高斯滤波后的图像强度减去对应各行的均值，完成均值移除。
20、而且，所述步骤2中先对步骤1预处理后的图像进行二项式滤波得到滤波后的强度图像，然后对预处理后的图像和二项式滤波后的图像中的每个像素进行加权得到总强度图像，具体计算方式如下：
21、（5）
22、式中，表示总强度图像中第u个像素的权值，u表示像素点序号，表示经过二项式滤波后第u个像素的强度，表示预处理后第u个像素的强度，表示正态分布函数。
23、计算步骤1中预处理后的图像强度的方差，将该值作为阈值，若加权得到的总强度图像中像素点的强度值大于阈值，则该像素点的波段是地物特征，取连续超过阈值的像素点构成的小块区域的中心作为特征点，在总强度图像中以此方式提取特征点，按强度大小对这些特征点进行降序排列，在极坐标系下从候选特征点云中选择强度最高的个特征点。
24、而且，所述步骤3中将步骤2提取出的特征点由极坐标转换为笛卡尔坐标，坐标变换公式如下：
25、（6）
26、式中，为笛卡尔坐标系下的坐标，分别表示横轴与纵轴坐标；为极坐标系下的坐标，分别表示距离与方位角。
27、而且，所述步骤4中基于相邻帧图像中坐标转换后的特征点，使用ransac算法获得相邻帧图像之间的姿态估计，然后使用该姿态估计值将上一帧图像中的特征点变换到当前帧图像的雷达局部坐标系下。雷达局部坐标系是以毫米波雷达中心为原点，水平朝前为x轴，水平朝右为y轴，垂直向上为z轴；利用ransac算法求得第帧图像到第 i帧图像间位姿变换关系如下：
28、（7）
29、（8）
30、式中，是第帧图像到第 i帧图像的位置姿态变换矩阵，、分别表示第帧图像到第 i帧图像的旋转矩阵、平移矩阵，表示旋转矩阵中的角度，表示旋转矩阵的迹，是欧式变换，包含旋转与平移。
31、假设当前帧为第 i帧，将第帧、第帧图像的点利用ransac算法求出的位姿变换关系转换到第 i帧图像的局部坐标系下，将当前帧图像的前两帧图像转换到当前帧图像坐标系下的点云所在区域定义为运动缓冲区，将当前帧中提取到的特征点与运动缓冲区中的点云进行ransac匹配，求出位姿变换关系，作为里程计的位姿变换结果。
32、而且，所述步骤5中利用ransac算法可以计算第帧图像与第 i帧图像的平移和旋转关系，由于雷达采集数据的帧率是已知的，因此可以根据相邻帧的位姿变换关系计算帧间的角速度和线速度关系，具体计算方式如下：
33、（9）
34、（10）
35、式中，为角速度，为线速度，表示第帧图像到第 i帧图像的旋转矩阵，表示旋转矩阵中的角度，表示相邻帧的时间差，表示第帧图像到第 本文档来自技高网...
【技术保护点】
1.一种基于毫米波雷达的SLAM方法，其特征在于，包括以下步骤：
2.如权利要求1所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤1中毫米波雷达图像采用极坐标系，原点在图像左上角，横轴表示距离，纵轴表示方位角；预处理包括强度校正、高斯滤波、均值滤除，强度校正计算公式如下：
3.如权利要求2所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤1中图像经过强度校正后，每行的图像强度会形成一个强度曲线，设置高强度阈值，保留经过强度校正后图像中强度高于高强度阈值的峰值特征，然后对每行分别进行高斯滤波，滤除高频低强度噪声，高斯滤波计算公式如下：
4.如权利要求1所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤2中先对步骤1预处理后的图像进行二项式滤波得到滤波后的强度图像，然后对预处理后的图像和二项式滤波后的图像中的每个像素进行加权得到总强度图像，具体计算方式如下：
5.如权利要求1所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤3中将步骤2提取出的特征点由极坐标转换为笛卡尔坐标，坐标变换公式如下：
6.
7.如权利要求6所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤5中利用RANSAC算法计算第帧图像与第i帧图像的平移和旋转关系，由于雷达采集数据的帧率是已知的，因此能够根据相邻帧的位姿变换关系计算帧间的角速度和线速度关系，具体计算方式如下：
8.如权利要求1所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤7中将位姿状态估计问题定义为最大后验问题，并定义里程计因子、运动缓冲区因子、GNSS因子和回环因子四个因子对毫米波雷达的位姿进行整体优化，里程计因子是指将当前帧特征点与运动缓冲区中的点进行RANSAC匹配求出位姿变换关系，运动缓冲区因子是指转换到同一坐标系下的点云数据，GNSS因子是指搭载毫米波雷达传感器的车辆在运行过程中接收到的GNSS数据，回环因子是指检测到的回环信息，即在环境中前进时是否返回到了先前经过的位置；优化方程表示如下：
9.如权利要求1所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤8中利用优化后每帧影像对应时刻的雷达位姿将每帧影像中的特征点转换到统一的坐标系下，最后得到一个总的环境点云模型。
10.一种基于毫米波雷达的SLAM系统，其特征在于，包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的程序指令执行如权利要求1-9任一项所述的一种基于毫米波雷达的SLAM方法。
...【技术特征摘要】
1.一种基于毫米波雷达的slam方法，其特征在于，包括以下步骤：
2.如权利要求1所述的一种基于毫米波雷达的slam方法，其特征在于：步骤1中毫米波雷达图像采用极坐标系，原点在图像左上角，横轴表示距离，纵轴表示方位角；预处理包括强度校正、高斯滤波、均值滤除，强度校正计算公式如下：
3.如权利要求2所述的一种基于毫米波雷达的slam方法，其特征在于：步骤1中图像经过强度校正后，每行的图像强度会形成一个强度曲线，设置高强度阈值，保留经过强度校正后图像中强度高于高强度阈值的峰值特征，然后对每行分别进行高斯滤波，滤除高频低强度噪声，高斯滤波计算公式如下：
4.如权利要求1所述的一种基于毫米波雷达的slam方法，其特征在于：步骤2中先对步骤1预处理后的图像进行二项式滤波得到滤波后的强度图像，然后对预处理后的图像和二项式滤波后的图像中的每个像素进行加权得到总强度图像，具体计算方式如下：
5.如权利要求1所述的一种基于毫米波雷达的slam方法，其特征在于：步骤3中将步骤2提取出的特征点由极坐标转换为笛卡尔坐标，坐标变换公式如下：
6.如权利要求1所述的一种基于毫米波雷达的slam方法，其特征在于：步骤4中基于相邻帧图像中坐标转换后的特征点，使用ransac算法获得相邻帧图像之间的姿态估计，然后使用该姿态估计值将上一帧图像中的特征点变换到当前帧图像的雷达局部坐标系下，雷达局部坐标系是以毫米波雷达中心为原点，水平朝前为x轴，水...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。