CN117315268A - 一种基于毫米波雷达的slam方法及系统 - Google Patents
一种基于毫米波雷达的slam方法及系统 Download PDFInfo
- Publication number
- CN117315268A CN117315268A CN202311586386.4A CN202311586386A CN117315268A CN 117315268 A CN117315268 A CN 117315268A CN 202311586386 A CN202311586386 A CN 202311586386A CN 117315268 A CN117315268 A CN 117315268A
- Authority
- CN
- China
- Prior art keywords
- image
- millimeter wave
- intensity
- wave radar
- frame
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 91
- 230000008569 process Effects 0.000 claims abstract description 26
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 16
- 238000005457 optimization Methods 0.000 claims abstract description 13
- 238000007781 pre-processing Methods 0.000 claims abstract description 10
- 230000009466 transformation Effects 0.000 claims description 29
- 238000001914 filtration Methods 0.000 claims description 24
- 239000011159 matrix material Substances 0.000 claims description 23
- 238000012937 correction Methods 0.000 claims description 16
- 238000013519 translation Methods 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 11
- 238000001514 detection method Methods 0.000 claims description 8
- 230000006870 function Effects 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 5
- 238000005315 distribution function Methods 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 abstract description 10
- 238000010586 diagram Methods 0.000 description 5
- 230000007613 environmental effect Effects 0.000 description 4
- 238000013507 mapping Methods 0.000 description 3
- 230000002411 adverse Effects 0.000 description 2
- 238000004590 computer program Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000007792 addition Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000006835 compression Effects 0.000 description 1
- 238000007906 compression Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013480 data collection Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000149 penetrating effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/20—Image preprocessing
- G06V10/30—Noise filtering
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/40—Extraction of image or video features
- G06V10/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Multimedia (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
本发明公开了一种基于毫米波雷达的SLAM方法及系统，包括使用毫米波雷达获取周围环境的极坐标图像，并对毫米波雷达图像进行预处理；基于预处理后的图像进行特征提取；使用RANSAC算法得到相邻帧间的位姿；使用前一帧图像的速度估计校正当前帧图像的特征点运动；使用因子图方法对毫米波雷达的位姿进行优化；根据优化后的毫米波雷达的位姿和图像中的特征点，构建周围环境的点云重建模型。本发明使用调频连续波技术的旋转式毫米波雷达，在位姿估计时结合运动缓冲区技术，后续因子图优化雷达位姿时额外加入运动缓冲区因子和GNSS因子进行整体优化，有效解决了常见的SLAM系统易受恶劣天气影响的问题，显著降低了由恶劣天气条件带来的定位建图误差的干扰。
Description
技术领域
本发明属于遥感影像处理技术领域，具体涉及一种基于毫米波雷达的SLAM方法及系统。
背景技术
现有的基于摄像机和激光雷达的即时定位与地图构建（SimultaneousLocalization and Mapping，SLAM）系统在采集环境数据时容易受到光照、恶劣天气以及物体遮挡的影响，导致视觉、激光SLAM系统在上述环境中运行时不能满足一定的定位精度要求。近年来，由于毫米波具有较强的穿透能力并且能够采集到密集数据的特性，使得基于毫米波雷达的SLAM技术备受关注。与激光不同，毫米波雷达信号波长较长，可以更好地绕过物体并提供被物体部分遮挡的环境数据，使得毫米波雷达SLAM具有抵抗不利天气条件的影响，如雨、雾等。
毫米波SLAM中常用的毫米波雷达技术有调频连续波雷达、脉冲压缩雷达等。本发明中毫米波雷达技术采取的是调频连续波雷达技术（Frequency Modulated ContinuousWave，FMCW）。调频连续波技术能够确保毫米波SLAM技术中能够采集到密集的数据点。FMCW技术可以使雷达发射更大能量的信号，从而通过调制信号的频率实现较长距离的探测。FMCW雷达可以用相位信息测量速度，本发明是基于旋转的FMCW雷达的SLAM结构，产生密集的距离测量。
发明内容
针对现有技术的不足，本发明提供一种基于毫米波雷达的SLAM方法及系统，使用基于调频连续波（FMCW）技术的毫米波雷达进行数据采集，可以有效抵抗不利天气条件的影响，确保采集到稳定的数据，采用高斯滤波、相邻帧特征关联等操作，能够提高位姿估计的鲁棒性，加入里程计因子、运动缓冲区因子、GNSS因子和回环因子对毫米波雷达的位姿进行整体优化，有助于提高定位与建图的精度。
为了实现上述目的，本发明提供一种基于毫米波雷达的SLAM方法，包括以下步骤：
步骤1，使用毫米波雷达获取周围环境的极坐标图像，并对毫米波雷达图像进行预处理；
步骤2，基于步骤1预处理后的图像进行特征提取；
步骤3，将步骤2提取的特征点转换到笛卡尔坐标系下；
步骤4，使用RANSAC算法得到相邻帧图像间的位姿；
步骤5，使用前一帧图像的速度估计校正当前帧图像的特征点运动；
步骤6，进行回环检测，判断在环境中前进时是否返回到了先前经过的位置；
步骤7，使用因子图方法对毫米波雷达的位姿进行优化；
步骤8，根据优化后的毫米波雷达的位姿和特征点，构建周围环境的点云重建模型。
而且，所述步骤1中毫米波雷达图像采用极坐标系，原点在图像左上角，横轴表示距离，纵轴表示方位角。预处理包括强度校正、高斯滤波、均值滤除，强度校正计算公式如下：
（1）
（2）
（3）
式中，为整幅毫米波雷达图像的平均强度，M、N表示毫米波雷达图像像素的行、列总数，/>表示毫米波雷达图像第r行、第c列像素的强度，/>表示毫米波雷达图像第r行强度的均值，/>表示毫米波雷达图像第r行、第k列像素的强度，/>为毫米波雷达图像第r行、第c列像素经过均衡化后的校正强度，/>为调整图像强度均衡化的参数。
图像经过强度校正后，每行的图像强度会形成一个强度曲线，设置高强度阈值，保留经过强度校正后图像中强度高于高强度阈值/>的峰值特征，然后对每行分别进行高斯滤波，滤除高频低强度噪声，高斯滤波计算公式如下：
（4）
式中，为滤波后的结果，/>表示圆周率，/>表示标准差，e表示自然常数，x表示图像的列数。
设置低强度阈值，保留高斯滤波后强度低于低强度阈值/>的低强度连续特征，然后计算图像各行强度的平均值，用各行经过高斯滤波后的图像强度减去对应各行的均值，完成均值移除。
而且，所述步骤2中先对步骤1预处理后的图像进行二项式滤波得到滤波后的强度图像，然后对预处理后的图像和二项式滤波后的图像中的每个像素进行加权得到总强度图像，具体计算方式如下：
（5）
式中，表示总强度图像中第u个像素的权值，u表示像素点序号，/>表示经过二项式滤波后第u个像素的强度，/>表示预处理后第u个像素的强度，/>表示正态分布函数。
计算步骤1中预处理后的图像强度的方差，将该值作为阈值，若加权得到的总强度图像中像素点的强度值大于阈值，则该像素点的波段是地物特征，取连续超过阈值的像素点构成的小块区域的中心作为特征点，在总强度图像中以此方式提取特征点，按强度大小对这些特征点进行降序排列，在极坐标系下从候选特征点云中选择强度最高的个特征点。
而且，所述步骤3中将步骤2提取出的特征点由极坐标转换为笛卡尔坐标，坐标变换公式如下：
（6）
式中，为笛卡尔坐标系下的坐标，分别表示横轴与纵轴坐标；/>为极坐标系下的坐标，分别表示距离与方位角。
而且，所述步骤4中基于相邻帧图像中坐标转换后的特征点，使用RANSAC算法获得相邻帧图像之间的姿态估计，然后使用该姿态估计值将上一帧图像中的特征点变换到当前帧图像的雷达局部坐标系下。雷达局部坐标系是以毫米波雷达中心为原点，水平朝前为x轴，水平朝右为y轴，垂直向上为z轴；利用RANSAC算法求得第帧图像到第i帧图像间位姿变换关系如下：
（7）
（8）
式中，是第/>帧图像到第i帧图像的位置姿态变换矩阵，/>、/>分别表示第/>帧图像到第i帧图像的旋转矩阵、平移矩阵，/>表示旋转矩阵中的角度，/>表示旋转矩阵/>的迹，/>是欧式变换，包含旋转与平移。
假设当前帧为第i帧，将第帧、第/>帧图像的点利用RANSAC算法求出的位姿变换关系转换到第i帧图像的局部坐标系下，将当前帧图像的前两帧图像转换到当前帧图像坐标系下的点云所在区域定义为运动缓冲区，将当前帧中提取到的特征点与运动缓冲区中的点云进行RANSAC匹配，求出位姿变换关系，作为里程计的位姿变换结果。
而且，所述步骤5中利用RANSAC算法可以计算第帧图像与第i帧图像的平移和旋转关系，由于雷达采集数据的帧率是已知的，因此可以根据相邻帧的位姿变换关系计算帧间的角速度和线速度关系，具体计算方式如下：
（9）
（10）
式中，为角速度，/>为线速度，/>表示第/>帧图像到第i帧图像的旋转矩阵，/>表示旋转矩阵中的角度，/>表示相邻帧的时间差，/>表示第/>帧图像到第i帧图像的平移矩阵。
将公式（9）和（10）计算出的角速度与线速度作为第i帧图像的速度估计，利用第i帧图像估计出的线速度和角速度，以及采集的点的时间变化量校正第帧图像中采集到的点云畸变。
而且，所述步骤7中将位姿状态估计问题定义为最大后验问题，并定义里程计因子、运动缓冲区因子、GNSS因子和回环因子四个因子对毫米波雷达的位姿进行整体优化。里程计因子是指将当前帧特征点与运动缓冲区中的点进行RANSAC匹配求出位姿变换关系，运动缓冲区因子是指转换到同一坐标系下的点云数据，GNSS因子是指搭载毫米波雷达传感器的车辆在运行过程中接收到的GNSS数据，回环因子是指检测到的回环信息，即在环境中前进时是否返回到了先前经过的位置。优化方程表示如下：
（11）
式中，表示机器人位姿状态，/>表示待优化的函数，/>、/>分别表示第/>帧图像和第i帧图像间的里程计因子、运动缓冲区因子，/>表示第i帧图像和第j帧图像间的回环因子，/>表示第k个全局GNSS因子，/>表示通过激光点云匹配获得的第/>帧与第i帧间的位姿变换结果，/>表示通过运动缓冲区匹配获得的第帧与第i帧间的位姿变换结果，/>表示经过回环检测获得的第i帧与第j帧间的位姿变换结果，/>表示从GNSS数据中获取到的第k个全局位姿，/>表示欧几里得距离。
使用非线性最小二乘方法求解出毫米波雷达位姿，使得上式优化函数/>值最小。
而且，所述步骤8中利用优化后每帧影像对应时刻的雷达位姿将每帧影像中的特征点转换到统一的坐标系下，最后得到一个总的环境点云模型。
本发明还提供一种基于毫米波雷达的SLAM系统，用于实现如上所述的一种基于毫米波雷达的SLAM方法。
而且，包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的程序指令执行如上所述的一种基于毫米波雷达的SLAM方法。
与现有技术相比，本发明具有如下优点：
1）对输入数据进行降噪处理，减少了原始数据中噪声的影响；2）在进行位姿优化时加入多个因子，定位与建图精度高；3）抗环境干扰能力强，降低了常见SLAM系统易受恶劣天气如雨、雾等天气的影响。
附图说明
图1为本发明实施例方法的流程图。
图2为本发明实施例特征关联示意图。
图3为本发明实施例因子图优化示意图。
图4（a）-（b）为本发明实施例构建的不同场景下的环境地图，其中图4（a）为DCC场景下运行的结果图，图4（b）为KAIST场景下运行的结果图。
具体实施方式
本发明提供一种基于毫米波雷达的SLAM方法及系统，下面结合附图和实施例对本发明的技术方案作进一步说明。
实施例1
如图1所示，本发明实施例提供一种基于毫米波雷达的SLAM方法，包括以下几个步骤：
步骤1，使用毫米波雷达获取周围环境的极坐标图像，并对毫米波雷达图像进行预处理。
Mulran数据集中包含多个场景，本实施例使用的是MulRan数据集中的DCC、KAIST和Riverside数据集，其中DCC（Dajeon Convention Center）数据集为包括高层建筑的市区场景，包含回环；KAIST数据集为包含少量动态对象的校园环境；Riverside数据集主要是河流和桥梁，这些数据集均为使用基于调频连续波技术的毫米波雷达采集的数据。数据集中的图像采用极坐标系，原点在图像左上角，横轴表示距离，纵轴表示方位角，图像大小为400×3360。由于原始毫米波雷达图像中除了背景噪声外，在某些方位角上的信号强度也会明显过高，为了除去部分背景噪声，对每个方位角进行强度校正、高斯滤波、均值滤除的预处理操作。
对每幅图像进行强度校正，具体计算公式如下：
（1）
（2）
（3）
式中，为整幅毫米波雷达图像的平均强度，M、N表示毫米波雷达图像像素的行、列总数，/>表示毫米波雷达图像第r行、第c列像素的强度，/>表示毫米波雷达图像第r行强度的均值，/>表示毫米波雷达图像第r行、第k列像素的强度，/>为毫米波雷达图像第r行、第c列像素经过均衡化后的校正强度，/>为调整图像强度均衡化的参数。
图像经过强度校正后，每行的图像强度会形成一个强度曲线，强度大的峰值更有可能是稳定的地物特征，设置高强度阈值（本实施例中/>取预处理后图像强度方差的三倍），保留经过强度校正后图像中强度高于高强度阈值/>的峰值特征，然后对每行分别进行高斯滤波，滤除高频低强度噪声。
（4）
式中，为滤波后的结果，/>表示圆周率，/>表示标准差，e表示自然常数，x表示图像的列数。
设置低强度阈值（本实施例中/>取预处理后图像强度方差的二分之一），保留高斯滤波后强度低于低强度阈值/>的低强度连续特征，然后计算图像各行强度的平均值，用各行经过高斯滤波后的图像强度减去对应各行的均值，完成均值移除。
步骤2，基于步骤1预处理后的图像进行特征提取。
对步骤1预处理后的图像进行二项式滤波得到滤波后的强度图像。对预处理后的图像和二项式滤波后的图像中的每个像素进行加权得到总强度图像，具体计算方式如下：
（5）
式中，表示总强度图像中第u个像素的权值，u表示像素点序号，/>表示经过二项式滤波后第u个像素的强度，/>表示预处理后第u个像素的强度，/>表示正态分布函数。
计算步骤1中预处理后的图像强度的方差，将该值作为阈值，若加权得到的总强度图像中像素点的强度值大于阈值，则该像素点的波段是地物特征，取连续超过阈值的像素点构成的小块区域的中心作为特征点，在总强度图像中以此方式提取特征点，按强度大小对这些特征点进行降序排列，在极坐标系下从候选特征点云中选择强度最高的（本实施例/>取30）个特征点。
步骤3，将步骤2提取的特征点转换到笛卡尔坐标系下。
为了更好地对特征点进行处理，将提取出的30个特征点由极坐标转换为笛卡尔坐标，输出为最终特征点。
坐标变换公式如下：
（6）
式中，为笛卡尔坐标系下的坐标，分别表示横轴与纵轴坐标；/>为极坐标系下的坐标，分别表示距离与方位角。
步骤4，使用RANSAC算法得到相邻帧图像间的位姿。
基于相邻帧图像中坐标转换后的特征点，使用RANSAC算法获得相邻帧图像之间的姿态估计，然后使用该姿态估计值将上一帧图像中的特征点变换到当前帧图像的雷达局部坐标系下。雷达局部坐标系是以毫米波雷达中心为原点，水平朝前为x轴，水平朝右为y轴，垂直向上为z轴。利用RANSAC算法求得第帧图像到第i帧图像间位姿变换关系如下：
（7）
（8）
式中，是第/>帧图像到第i帧图像的位置姿态变换矩阵，/>、/>分别表示第/>帧图像到第i帧图像的旋转矩阵、平移矩阵，/>表示旋转矩阵中的角度，/>表示旋转矩阵/>的迹，/>是欧式变换，包含旋转与平移。
假设当前帧为第i帧，将第帧、第/>帧图像的点利用RANSAC算法求出的位姿变换关系转换到第i帧图像的局部坐标系下。将当前帧图像的前两帧图像转换到当前帧图像坐标系下的点云所在区域定义为运动缓冲区，将当前帧中提取到的特征点与运动缓冲区中的点云进行RANSAC匹配，求出位姿变换关系，作为里程计的位姿变换结果。
步骤5，使用前一帧图像的速度估计校正当前帧图像的特征点运动。
利用RANSAC算法可以计算第帧图像与第i帧图像的平移和旋转关系，由于雷达采集数据的帧率是已知的，因此可以根据相邻帧的位姿变换关系计算帧间的角速度和线速度关系，具体计算方式如下：
（9）
（10）
式中，为角速度，/>为线速度，/>表示第/>帧图像到第i帧图像的旋转矩阵，/>表示旋转矩阵中的角度，/>表示相邻帧的时间差，/>表示第/>帧图像到第i帧图像的平移矩阵。
将公式（9）和（10）计算出的角速度与线速度作为第i帧图像的速度估计，由于毫米波雷达在采集数据时是运动的，不同时刻采集到的点存在着畸变，因此需要对点进行去畸变。利用第i帧图像估计出的线速度和角速度，以及采集的点的时间变化量校正第帧图像中采集到的点云畸变（见图2）。
步骤6，进行回环检测，判断在环境中前进时是否返回到了先前经过的位置。
本实施例中利用Scan-Context的方法进行回环检测，当检测到回环时，返回回环因子，检测不到回环时，返回0。Scan-Context是一种基于极坐标系的三维点云特征和匹配方法，可以快速实现场景重识别，主要思想是利用传感器数据来创建环境的局部特征描述子，并将这些描述子与先前获得的特征描述子进行比较，从而确定是否返回到了相同的位置。
步骤7，使用因子图方法对毫米波雷达的位姿进行优化。
在优化部分使用因子图的方法，加入里程计因子、运动缓冲区因子、GNSS因子和回环因子进行整体优化（见图3）。里程计因子是指将当前帧特征点与运动缓冲区中的点进行RANSAC匹配求出的位姿变换关系，运动缓冲区因子是指转换到同一坐标系下的点云数据，GNSS因子是指搭载毫米波雷达传感器的车辆在运行过程中接收到的GNSS数据，回环因子是指检测到的回环信息，即在环境中前进时是否返回到了先前经过的位置。
将位姿状态估计问题定义为最大后验（MAP）问题，并定义里程计因子、运动缓冲区因子、GNSS因子和回环因子四个因子优化状态。
（11）
式中，表示机器人位姿状态，/>表示待优化的函数，/>、/>分别表示第/>帧图像和第i帧图像间的里程计因子、运动缓冲区因子，/>表示第i帧图像和第j帧图像间的回环因子，/>表示第k个全局GNSS因子，/>表示通过激光点云匹配获得的第/>帧与第i帧间的位姿变换结果，/>表示通过运动缓冲区匹配获得的第帧与第i帧间的位姿变换结果，/>表示经过回环检测获得的第i帧与第j帧间的位姿变换结果，/>表示从GNSS数据中获取到的第k个全局位姿，/>表示欧几里得距离。
使用非线性最小二乘方法求解出毫米波雷达位姿，使得上式优化函数/>值最小。
步骤8，根据优化后的毫米波雷达的位姿和特征点，构建周围环境的点云重建模型。
利用优化后每帧影像对应时刻的雷达位姿将每帧影像中的特征点转换到统一的坐标系下，最后得到一个总的环境点云模型。图4（a）-（b）为使用本发明所提方法构建的不同场景下的环境地图，其中图4（a）为DCC场景下运行的结果图，图4（b）为KAIST场景下运行的结果图。
实施例2
基于同一发明构思，本发明还提供一种基于毫米波雷达的SLAM系统，包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的程序指令执行如上所述的一种基于毫米波雷达的SLAM方法。
具体实施时，本发明技术方案提出的方法可由本领域技术人员采用计算机软件技术实现自动运行流程，实现方法的系统装置例如存储本发明技术方案相应计算机程序的计算机可读存储介质以及包括运行相应计算机程序的计算机设备，也应当在本发明的保护范围内。
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施案例，做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。
Claims (10)
1.一种基于毫米波雷达的SLAM方法，其特征在于，包括以下步骤：
步骤1，使用毫米波雷达获取周围环境的极坐标图像，并对毫米波雷达图像进行预处理；
步骤2，基于步骤1预处理后的图像进行特征提取；
步骤3，将步骤2提取的特征点转换到笛卡尔坐标系下；
步骤4，使用RANSAC算法得到相邻帧图像间的位姿；
步骤5，使用前一帧图像的速度估计校正当前帧图像的特征点运动；
步骤6，进行回环检测，判断在环境中前进时是否返回到了先前经过的位置；
步骤7，使用因子图方法对毫米波雷达的位姿进行优化；
步骤8，根据优化后的毫米波雷达的位姿和特征点，构建周围环境的点云重建模型。
2.如权利要求1所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤1中毫米波雷达图像采用极坐标系，原点在图像左上角，横轴表示距离，纵轴表示方位角；预处理包括强度校正、高斯滤波、均值滤除，强度校正计算公式如下：
（1）
（2）
（3）
式中，为整幅毫米波雷达图像的平均强度，M、N表示毫米波雷达图像像素的行、列总数，/>表示毫米波雷达图像第r行、第c列像素的强度，/>表示毫米波雷达图像第r行强度的均值，/>表示毫米波雷达图像第r行、第k列像素的强度，/>为毫米波雷达图像第r行、第c列像素经过均衡化后的校正强度，/>为调整图像强度均衡化的参数。
3.如权利要求2所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤1中图像经过强度校正后，每行的图像强度会形成一个强度曲线，设置高强度阈值，保留经过强度校正后图像中强度高于高强度阈值/>的峰值特征，然后对每行分别进行高斯滤波，滤除高频低强度噪声，高斯滤波计算公式如下：
（4）
式中，为滤波后的结果，/>表示圆周率，/>表示标准差，e表示自然常数，x表示图像的列数；
设置低强度阈值，保留高斯滤波后强度低于低强度阈值/>的低强度连续特征，然后计算图像各行强度的平均值，用各行经过高斯滤波后的图像强度减去对应各行的均值，完成均值移除。
4.如权利要求1所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤2中先对步骤1预处理后的图像进行二项式滤波得到滤波后的强度图像，然后对预处理后的图像和二项式滤波后的图像中的每个像素进行加权得到总强度图像，具体计算方式如下：
（5）
式中，表示总强度图像中第u个像素的强度值，u表示像素点序号，/>表示经过二项式滤波后第u个像素的强度，/>表示预处理后第u个像素的强度，/>表示正态分布函数；
计算步骤1中预处理后的图像强度的方差，将该值作为阈值，若加权得到的总强度图像中像素点的强度值大于阈值，则该像素点的波段是地物特征，取连续超过阈值的像素点构成的小块区域的中心作为特征点，在总强度图像中以此方式提取特征点，按强度大小对这些特征点进行降序排列，在极坐标系下从候选特征点云中选择强度最高的个特征点。
5.如权利要求1所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤3中将步骤2提取出的特征点由极坐标转换为笛卡尔坐标，坐标变换公式如下：
（6）
式中，为笛卡尔坐标系下的坐标，分别表示横轴与纵轴坐标；/>为极坐标系下的坐标，分别表示距离与方位角。
6.如权利要求1所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤4中基于相邻帧图像中坐标转换后的特征点，使用RANSAC算法获得相邻帧图像之间的姿态估计，然后使用该姿态估计值将上一帧图像中的特征点变换到当前帧图像的雷达局部坐标系下，雷达局部坐标系是以毫米波雷达中心为原点，水平朝前为x轴，水平朝右为y轴，垂直向上为z轴；利用RANSAC算法求得第帧图像到第i帧图像间位姿变换关系如下：
（7）
（8）
式中，是第/>帧图像到第i帧图像的位置姿态变换矩阵，/>、/>分别表示第帧图像到第i帧图像的旋转矩阵、平移矩阵，/>表示旋转矩阵中的角度，/>表示旋转矩阵/>的迹，/>是欧式变换，包含旋转与平移；
假设当前帧为第i帧，将第帧、第/>帧图像的点利用RANSAC算法求出的位姿变换关系转换到第i帧图像的局部坐标系下，将当前帧图像的前两帧图像转换到当前帧图像坐标系下的点云所在区域定义为运动缓冲区，将当前帧中提取到的特征点与运动缓冲区中的点云进行RANSAC匹配，求出位姿变换关系，作为里程计的位姿变换结果。
7.如权利要求6所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤5中利用RANSAC算法计算第帧图像与第i帧图像的平移和旋转关系，由于雷达采集数据的帧率是已知的，因此能够根据相邻帧的位姿变换关系计算帧间的角速度和线速度关系，具体计算方式如下：
（9）
（10）
式中，为角速度，/>为线速度，/>表示第/>帧图像到第i帧图像的旋转矩阵，/>表示旋转矩阵中的角度，/>表示相邻帧的时间差，/>表示第/>帧图像到第i帧图像的平移矩阵；
将公式（9）和（10）计算出的角速度与线速度作为第i帧图像的速度估计，利用第i帧图像估计出的线速度和角速度，以及采集的点的时间变化量校正第帧图像中采集到的点云畸变。
8.如权利要求1所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤7中将位姿状态估计问题定义为最大后验问题，并定义里程计因子、运动缓冲区因子、GNSS因子和回环因子四个因子对毫米波雷达的位姿进行整体优化，里程计因子是指将当前帧特征点与运动缓冲区中的点进行RANSAC匹配求出位姿变换关系，运动缓冲区因子是指转换到同一坐标系下的点云数据，GNSS因子是指搭载毫米波雷达传感器的车辆在运行过程中接收到的GNSS数据，回环因子是指检测到的回环信息，即在环境中前进时是否返回到了先前经过的位置；优化方程表示如下：
（11）
式中，表示机器人位姿状态，/>表示待优化的函数，/>、/>分别表示第/>帧图像和第i帧图像间的里程计因子、运动缓冲区因子，/>表示第i帧图像和第j帧图像间的回环因子，/>表示第k个全局GNSS因子，/>表示通过激光点云匹配获得的第/>帧与第i帧间的位姿变换结果，/>表示通过运动缓冲区匹配获得的第/>帧与第i帧间的位姿变换结果，/>表示经过回环检测获得的第i帧与第j帧间的位姿变换结果，表示从GNSS数据中获取到的第k个全局位姿，/>表示欧几里得距离；
使用非线性最小二乘方法求解出毫米波雷达位姿，使得上式优化函数/>值最小。
9.如权利要求1所述的一种基于毫米波雷达的SLAM方法，其特征在于：步骤8中利用优化后每帧影像对应时刻的雷达位姿将每帧影像中的特征点转换到统一的坐标系下，最后得到一个总的环境点云模型。
10.一种基于毫米波雷达的SLAM系统，其特征在于，包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的程序指令执行如权利要求1-9任一项所述的一种基于毫米波雷达的SLAM方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311586386.4A CN117315268B (zh) | 2023-11-27 | 2023-11-27 | 一种基于毫米波雷达的slam方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311586386.4A CN117315268B (zh) | 2023-11-27 | 2023-11-27 | 一种基于毫米波雷达的slam方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117315268A true CN117315268A (zh) | 2023-12-29 |
| CN117315268B CN117315268B (zh) | 2024-02-09 |
Family
ID=89255503
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311586386.4A Active CN117315268B (zh) | 2023-11-27 | 2023-11-27 | 一种基于毫米波雷达的slam方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117315268B (zh) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110570449A (zh) * | 2019-09-16 | 2019-12-13 | 电子科技大学 | 一种基于毫米波雷达与视觉slam的定位与建图方法 |
| CN111929699A (zh) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统 |
| US20220383755A1 (en) * | 2021-05-28 | 2022-12-01 | Tongji University | Unmanned aerial vehicle positioning method based on millimeter-wave radar |
| CN116359905A (zh) * | 2023-05-11 | 2023-06-30 | 上海交通大学 | 基于4d毫米波雷达的位姿图slam计算方法及系统 |
-
2023
- 2023-11-27 CN CN202311586386.4A patent/CN117315268B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110570449A (zh) * | 2019-09-16 | 2019-12-13 | 电子科技大学 | 一种基于毫米波雷达与视觉slam的定位与建图方法 |
| CN111929699A (zh) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统 |
| US20220383755A1 (en) * | 2021-05-28 | 2022-12-01 | Tongji University | Unmanned aerial vehicle positioning method based on millimeter-wave radar |
| CN116359905A (zh) * | 2023-05-11 | 2023-06-30 | 上海交通大学 | 基于4d毫米波雷达的位姿图slam计算方法及系统 |
Non-Patent Citations (3)
| Title |
|---|
| YANG LI，ET AL: "The Millimeter-Wave Radar SLAM Assisted by the RCS Feature of the Target and IMU", SENSORS, vol. 20, no. 18 * |
| 刘铭哲，等: "激光雷达SLAM算法综述", 计算机工程与应用 * |
| 王亚波，等: "基于激光雷达的结构化道路障碍物检测方法", 中国惯性技术学报, vol. 6, no. 31 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117315268B (zh) | 2024-02-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104183127B (zh) | 交通监控视频检测方法和装置 | |
| CN114419825B (zh) | 基于毫米波雷达和摄像头的高铁周界入侵监测装置及方法 | |
| CN107560592B (zh) | 一种用于光电跟踪仪联动目标的精确测距方法 | |
| CN110706197A (zh) | 一种基于迁移学习的特殊场景下铁路异物侵限检测方法 | |
| CN109711256B (zh) | 一种低空复杂背景无人机目标检测方法 | |
| CN111666944A (zh) | 一种红外弱小目标检测方法及装置 | |
| CN104715474B (zh) | 基于标记分水岭算法的高分辨率合成孔径雷达图像线性建筑物检测方法 | |
| CN115097442B (zh) | 基于毫米波雷达的水面环境地图构建方法 | |
| CN103679167A (zh) | 一种ccd图像处理的方法 | |
| CN107194946B (zh) | 一种基于fpga的红外显著物体检测方法 | |
| CN115131420A (zh) | 基于关键帧优化的视觉slam方法及装置 | |
| CN105469428B (zh) | 一种基于形态学滤波和svd的弱小目标检测方法 | |
| CN114264297B (zh) | Uwb和视觉slam融合算法的定位建图方法及系统 | |
| CN112733886A (zh) | 样本图像的处理方法、装置、设备及存储介质 | |
| CN117315268B (zh) | 一种基于毫米波雷达的slam方法及系统 | |
| CN104166975A (zh) | 一种复杂环境下低空红外目标检测算法 | |
| CN112669332A (zh) | 一种基于双向局部极大值和峰值局部奇异性判断海天条件和检测红外目标的方法 | |
| CN117031424A (zh) | 一种基于导航雷达的水面目标检测跟踪方法 | |
| CN111127506A (zh) | 一种基于序列图像的海上运动目标综合检测方法 | |
| CN109918968A (zh) | 一种舰船目标检测方法 | |
| Cao et al. | Improving star centroiding accuracy in stray light base on background estimation | |
| CN112184764A (zh) | 一种基于傅里叶-梅林变换的目标跟踪方法 | |
| CN112037148A (zh) | 区块链的大数据运动目标检测识别方法及系统 | |
| Ji et al. | Corn Row Navigation Line Extraction Method Based on the Adaptive Edge Detection Algorithm | |
| CN113284135B (zh) | 基于全局与局部上下文信息的sar舰船检测方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |