CN116359905A - 基于4d毫米波雷达的位姿图slam计算方法及系统 - Google Patents

基于4d毫米波雷达的位姿图slam计算方法及系统 Download PDF

Info

Publication number
CN116359905A
CN116359905A CN202310532630.2A CN202310532630A CN116359905A CN 116359905 A CN116359905 A CN 116359905A CN 202310532630 A CN202310532630 A CN 202310532630A CN 116359905 A CN116359905 A CN 116359905A
Authority
CN
China
Prior art keywords
radar
point cloud
pose
point
map
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.)
Pending
Application number
CN202310532630.2A
Other languages
English (en)
Inventor
张晗
李星熠
陈卫东
王景川
刘昌赫
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zf China Investment Co ltd
Shanghai Jiaotong University
Original Assignee
Zf China Investment Co ltd
Shanghai Jiaotong University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zf China Investment Co ltd, Shanghai Jiaotong University filed Critical Zf China Investment Co ltd
Priority to CN202310532630.2A priority Critical patent/CN116359905A/zh
Publication of CN116359905A publication Critical patent/CN116359905A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/50—Systems of measurement based on relative movement of target
    • G01S13/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/89—Radar or analogous systems specially adapted for specific applications for mapping or imaging
    • 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)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明提供了一种基于4D毫米波雷达的位姿图SLAM计算方法及系统，包括：步骤S1：提取地面点云，比较连续两帧点云，去除地面下的鬼影点以及随机点；步骤S2：利用4D雷达点云的多普勒速度信息，估计出设备的线速度和角速度；步骤S3：利用相对位姿变换估计并构建位姿图，基于正态分布变换的点云配准，利用估计的设备速度进行预积分，回环检测，利用图优化估计出最优的位姿。本发明为了在点云稀疏的情况下达到更好的精度，将估计的自车速度进行预积分，获得额外的相对位姿估计；本发明引入了回环检测，识别重新到达过的地方，从全局的角度优化历史位姿，以减少累积漂移，实现精确、鲁棒的建图和定位。

Description

基于4D毫米波雷达的位姿图SLAM计算方法及系统
技术领域
本发明涉及机器人技术领域，具体地，涉及一种基于4D毫米波雷达的位姿图SLAM计算方法及系统。
背景技术
毫米波雷达已被广泛用于同步定位和建图技术(SLAM)，特别是在自动驾驶领域。与相机或激光雷达等光学传感器相比，毫米波雷达对天气和光线条件的敏感度更低，而且价格便宜得多。但尽管有这些优势，与激光雷达相比，毫米波雷达的点云通常更稀疏，噪声更大，这给精确的定位和建图带来了挑战。近年来，最新的毫米波雷达传感器，即4D成像雷达(4D雷达)，由于其相对于传统雷达的独特优势，在自动驾驶中受到越来越多的关注。
传统的毫米波雷达可以分为两种：扫描雷达和汽车雷达。扫描雷达对周围环境进行360度扫描，提供不含速度信息的二维雷达能量图像。另一方面，汽车雷达通常具有较小的视场角，并提供具有多普勒速度的稀疏2D点云。与传统毫米波雷达传感器相比，4D雷达具有更长的探测距离和更大的视场角。4D雷达提供的4D点云具有4个维度的信息：距离、方位角、高度角以及多普勒速度。此外，4D雷达还提供一些低层次的特征，如能量强度或雷达散射截面。因为具有了额外的信息和更高的分辨率，4D雷达为SLAM的应用提供了新的机会。
对于毫米波雷达SLAM的问题，国内外研究者也做了大量研究，根据应用到的毫米波雷达类型，可大致分为两类：
一类是基于扫描雷达的SLAM算法,这在毫米波雷达SLAM中占据了主流地位。这类算法使用扫描雷达获得的2维图像作为输入，通常采用一些特征提取算法从图像中提取特征点，然后进行基于特征点的扫描配准。其中最有名的是Hong等人[1]提出的RadarSLAM算法(Z.Hong,Y.Petillot,and S.Wang,“RadarSLAM:Radar based Large-Scale SLAM inAll Weathers,”in 2020IEEE/RSJ International Conference on Intelligent Robotsand Systems(IROS),Oct.2020,pp.5164–5170,iSSN:2153-086)。他们受到视觉SLAM相关方法的启发，从雷达图像中提取SURF特征点并进行配准。除此之外，也有一些利用深度学习的实现扫描雷达SLAM的方法。然而，这些方法并不适用于4D雷达点云，因为它们采用2维的扫描雷达图像作为输入。
二是基于汽车雷达的SLAM算法。这类算法采用传统汽车雷达获得的稀疏雷达点云作为输入，利用点云的多普勒速度信息或者利用点云的扫描配准估计自车运动。Kellner等人[2](D.Kellner,M.Barjenbruch,J.Klappstein,J.Dickmann,and K.Dietmayer,“Instantaneous ego-motion estimation using Doppler radar,”in 16th International IEEE Conference on Intelligent Transportation Systems(ITSC 2013),Oct.2013,pp.869–874,iSSN:2153-0017.)利用雷达点云的多普勒速度信息，计算出自车速度，从而实现自车运动估计。Kung等人提出了一种适应于扫描雷达和汽车雷达的雷达里程计算法[3](P.-C.Kung,C.-C.Wang,and W.-C.Lin,“A Normal Distribution Transform-Based Radar Odometry Designed For Scanning and Automotive Radars,”in 2021IEEEInternational Conference on Robotics and Automation(ICRA).Xi’an,China:IEEE,May 2021,pp.14 417–14 423.)。他们利用了Kellner等人提出的自车速度估计方法，将多帧雷达点云合并为一个雷达子地图。然后，应用基于正态分布变换的扫描匹配来匹配两个雷达子地图，实现自车运动的估计。然而，雷达子地图只是利用自车速度估计对连续雷达点云的简单叠加，因此其精度在很大程度上取决于自车速度估计的准确性。
专利文献CN111522043A公开了一种无人车激光雷达快速重新匹配定位方法，属于无人驾驶领域。该发明一共分为多传感器标定、融合位姿和融合定位三个模块。通过多传感器联合标定，获得对目标的一致性描述；将GPS传感器和激光雷达传感器对无人车解算的位姿信息进行数据融合，获得GPS传感器和激光雷达传感器融合的车辆位姿信息；当激光雷达SLAM定位模块点云匹配失败时，使用融合位姿来替代点云匹配算法的定位预测矩阵，实现激光雷达SLAM算法的快速重新匹配，实现无人车SLAM算法的持续定位。但该发明没有引入回环检测识别重新到达过的地方，不能从全局的角度优化历史位姿。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种基于4D毫米波雷达的位姿图SLAM计算方法及系统。
根据本发明提供的一种基于4D毫米波雷达的位姿图SLAM计算方法，包括：
步骤S1：提取地面点云，比较连续两帧点云，去除地面下的鬼影点以及随机点；
步骤S2：利用4D雷达点云的多普勒速度信息，估计出设备的线速度和角速度；
步骤S3：利用相对位姿变换估计并构建位姿图，基于正态分布变换的点云配准，利用估计的设备速度进行预积分，回环检测，利用图优化估计出最优的位姿。
优选地，在所述步骤S1中：
通过提取地面点云以及比较连续两帧点云，去除地面下的鬼影点以及不稳定的随机点，减少4D雷达点云中的噪声；
步骤S1.1：鬼影点去除，从原始雷达点云中提取出地面点云，滤除地面之下的鬼影点；
对于4D雷达测量的原始雷达点云
Figure BDA0004224981640000031
n为点云中点的数量；
其中每个点在雷达坐标系中的位置表示为
Figure BDA0004224981640000032
其中ri为4D雷达测量得到的该点的距离，θi为4D雷达测量得到的该点的方位角，/>
Figure BDA0004224981640000033
为4D雷达测量得到的该点的高度角；
对于原始的雷达点云，保留距离小于阈值δr且高度在雷达安装高度附近阈值δh内的点，利用主成分分析法，计算出每个点的朝上的法向量，并保留法向量和z轴正向单位向量夹角小于δn的点，利用随机采样一致性算法提取出地面点，并滤除在地面之下的鬼影点；
步骤S1.2：随机点去除，通过对比前后两帧点云识别和滤除随机点，对于当前点云和紧邻的前一帧点云，通过设备速度计算出前后两帧点云的位姿变换，将前一帧点云变换到当前帧的坐标系下，如果当前帧的某个点附近预设范围内不存在变换后的前一帧点云，则将该点归类为随机点并滤除：
计算出当前点云
Figure BDA0004224981640000034
和紧邻的前一帧点云/>
Figure BDA0004224981640000035
之间的旋转变换Rk-1和平移变换tk-1：
Figure BDA0004224981640000036
Figure BDA0004224981640000037
其中，
Figure BDA0004224981640000038
和/>
Figure BDA0004224981640000039
分别是前一帧的设备线速度和角速度估计，Δt是两帧之间的时间差，Exp(·):R3→SO(3)是三维旋转的指数映射，R3是三维实数向量空间，SO(3)是三维特殊正交群，利用Rk-1和tk-1将前一帧点云变换到当前帧的坐标系下得到/>
Figure BDA0004224981640000041
如果当前帧的某个点预设范围内不存在/>
Figure BDA0004224981640000042
中的点，则将该当前点归类为随机点并滤除。
优选地，在所述步骤S2中：
步骤S2.1：静态点提取，对于4D雷达点云中的第i个点，该点的多普勒速度和雷达自身速度之间的关系如下：
vr,i＝-di·vs
其中，vr,i是第i个点的多普勒速度，
Figure BDA0004224981640000043
是该点相对于雷达的单位方向向量，vs是雷达的速度；
雷达速度和设备速度的关系为：
Figure BDA0004224981640000044
其中，ts和Rs分别为4D雷达在设备坐标系中的安装位置和姿态，
Figure BDA0004224981640000045
和/>
Figure BDA0004224981640000046
分别为设备线速度和角速度，对于4D雷达点云中的所有静态点，多普勒速度和设备速度之间均符合如下方程：
Figure BDA0004224981640000047
其中，m为点云中所有静态点的数量，I3×3为3×3大小的单位矩阵，
Figure BDA0004224981640000048
是ts的反对称矩阵，R3×3表示3×3实矩阵的集合，根据雷达点云中所有静态点的多普勒速度和设备速度之间的关系，利用随机采样一致性算法去除动态的离群点并提取静态点；
步骤S2.2：最小二乘估计，提取出静态点后，利用所有静态点的多普勒速度和设备速度之间的关系，利用最小二乘法，计算出设备速度。
优选地，在所述步骤S3中：
位姿图优化，利用点云配准、速度预积分和回环检测，得到相对位姿变换估计并构建位姿图，最后利用图优化估计出最优的位姿；
步骤S3.1：点云配准，基于正态分布变换，通过当前4D雷达点云和关键帧子地图的匹配估计相对变换；
使用滑动窗口利用多个关键帧点云建立雷达子地图，如果从最新的关键帧到当前点云的平移或旋转超过阈值，则选择当前帧作为一个新的关键帧，将新的关键帧添加到雷达子地图中，当子地图中的关键帧数量超过窗口大小时，最早的关键帧被丢弃；
建立雷达子地图后，将子地图均匀划分为大小相同的网格，将每个网格中的点云建模为局部正态分布，在计算正态分布的均值和协方差时，考虑每个点的测量不确定性，将当前的4D雷达点云与雷达子地图进行配准，计算出使得当前点云在子地图中分布概率最大的相对位姿变换，该位姿变换估计的误差为eO；
步骤S3.2：速度预积分，利用估计的设备速度，计算出相对位姿变换，引入额外的可靠相对位姿估计，将t时刻估计的设备线速度和角速度记为
Figure BDA0004224981640000051
和/>
Figure BDA0004224981640000052
估计值为真实值叠加一个零均值的高斯白噪声，即：
Figure BDA0004224981640000053
Figure BDA0004224981640000054
其中，ωt和Wvt是世界坐标系下的角速度和线速度真实值，
Figure BDA0004224981640000055
和/>
Figure BDA0004224981640000056
是对应的噪声项，Rt是设备在世界坐标系下的朝向，通过积分得到i时刻和j时刻之间的相对旋转变换ΔRij和相对平移变换Δpij，该位姿变换估计的误差为eV。
优选地，步骤S3.3：回环检测，识别之前到达过的位置，在极坐标上，将4D雷达点云按照网格进行划分，将3D点云映射为一个2维矩阵，矩阵中每个元素的值为对应的网格中雷达点的最大能量强度，随着设备的运动，不断搜索和计算当前帧的2维矩阵和之前所有关键帧生成的2维矩阵的余弦距离，距离小于设定阈值时认为检测到回环；当检测到一个回环，计算当前帧和由回环帧以及附近关键帧组成的子地图之间的相对位姿变换；该位姿变换估计的误差为eL；
步骤S3.4：图优化，建立位姿图，考虑所有时刻相对位姿估计的误差项，通过图优化估计出所有时刻设备的最优位姿，位姿中所有的节点对应不同时刻的位姿，节点间的边对应不同时刻之间的相对位姿变换；将所有位姿记为
Figure BDA0004224981640000057
通过解非线性最小二乘问题估计最优位姿/>
Figure BDA0004224981640000058
Figure BDA0004224981640000059
其中，
Figure BDA00042249816400000510
包含所有的时刻，eO,eV,eL对应不同步骤获得的相对位姿变换的误差。
根据本发明提供的一种基于4D毫米波雷达的位姿图SLAM计算系统，包括：
模块M1：提取地面点云，比较连续两帧点云，去除地面下的鬼影点以及随机点；
模块M2：利用4D雷达点云的多普勒速度信息，估计出设备的线速度和角速度；
模块M3：利用相对位姿变换估计并构建位姿图，基于正态分布变换的点云配准，利用估计的设备速度进行预积分，回环检测，利用图优化估计出最优的位姿。
优选地，在所述模块M1中：
通过提取地面点云以及比较连续两帧点云，去除地面下的鬼影点以及不稳定的随机点，减少4D雷达点云中的噪声；
模块M1.1：鬼影点去除，从原始雷达点云中提取出地面点云，滤除地面之下的鬼影点；
对于4D雷达测量的原始雷达点云
Figure BDA0004224981640000061
n为点云中点的数量；
其中每个点在雷达坐标系中的位置表示为
Figure BDA0004224981640000062
其中ri为4D雷达测量得到的该点的距离，θi为4D雷达测量得到的该点的方位角，/>
Figure BDA0004224981640000063
为4D雷达测量得到的该点的高度角；
对于原始的雷达点云，保留距离小于阈值δr且高度在雷达安装高度附近阈值δh内的点，利用主成分分析法，计算出每个点的朝上的法向量，并保留法向量和z轴正向单位向量夹角小于δn的点，利用随机采样一致性算法提取出地面点，并滤除在地面之下的鬼影点；
模块M1.2：随机点去除，通过对比前后两帧点云识别和滤除随机点，对于当前点云和紧邻的前一帧点云，通过设备速度计算出前后两帧点云的位姿变换，将前一帧点云变换到当前帧的坐标系下，如果当前帧的某个点附近预设范围内不存在变换后的前一帧点云，则将该点归类为随机点并滤除：
计算出当前点云
Figure BDA0004224981640000064
和紧邻的前一帧点云/>
Figure BDA0004224981640000065
之间的旋转变换Rk-1和平移变换tk-1：
Figure BDA0004224981640000066
Figure BDA0004224981640000067
其中，
Figure BDA0004224981640000068
和/>
Figure BDA0004224981640000069
分别是前一帧的设备线速度和角速度估计，Δt是两帧之间的时间差，Exp(·):R3→SO(3)是三维旋转的指数映射，R3是三维实数向量空间，SO(3)是三维特殊正交群，利用Rk-1和tk-1将前一帧点云变换到当前帧的坐标系下得到/>
Figure BDA00042249816400000610
如果当前帧的某个点预设范围内不存在/>
Figure BDA00042249816400000611
中的点，则将该当前点归类为随机点并滤除。
优选地，在所述模块M2中：
模块M2.1：静态点提取，对于4D雷达点云中的第i个点，该点的多普勒速度和雷达自身速度之间的关系如下：
vr,i＝-di·vs
其中，vr,i是第i个点的多普勒速度，
Figure BDA00042249816400000612
是该点相对于雷达的单位方向向量，vs是雷达的速度；
雷达速度和设备速度的关系为：
Figure BDA0004224981640000071
其中，ts和Rs分别为4D雷达在设备坐标系中的安装位置和姿态，
Figure BDA0004224981640000072
和/>
Figure BDA0004224981640000073
分别为设备线速度和角速度，对于4D雷达点云中的所有静态点，多普勒速度和设备速度之间均符合如下方程：
Figure BDA0004224981640000074
其中，m为点云中所有静态点的数量，I3×3为3×3大小的单位矩阵，
Figure BDA0004224981640000075
是ts的反对称矩阵，R3×3表示3×3实矩阵的集合，根据雷达点云中所有静态点的多普勒速度和设备速度之间的关系，利用随机采样一致性算法去除动态的离群点并提取静态点；
模块M2.2：最小二乘估计，提取出静态点后，利用所有静态点的多普勒速度和设备速度之间的关系，利用最小二乘法，计算出设备速度。
优选地，在所述模块M3中：
位姿图优化，利用点云配准、速度预积分和回环检测，得到相对位姿变换估计并构建位姿图，最后利用图优化估计出最优的位姿；
模块M3.1：点云配准，基于正态分布变换，通过当前4D雷达点云和关键帧子地图的匹配估计相对变换；
使用滑动窗口利用多个关键帧点云建立雷达子地图，如果从最新的关键帧到当前点云的平移或旋转超过阈值，则选择当前帧作为一个新的关键帧，将新的关键帧添加到雷达子地图中，当子地图中的关键帧数量超过窗口大小时，最早的关键帧被丢弃；
建立雷达子地图后，将子地图均匀划分为大小相同的网格，将每个网格中的点云建模为局部正态分布，在计算正态分布的均值和协方差时，考虑每个点的测量不确定性，将当前的4D雷达点云与雷达子地图进行配准，计算出使得当前点云在子地图中分布概率最大的相对位姿变换，该位姿变换估计的误差为eO；
模块M3.2：速度预积分，利用估计的设备速度，计算出相对位姿变换，引入额外的可靠相对位姿估计，将t时刻估计的设备线速度和角速度记为
Figure BDA0004224981640000076
和/>
Figure BDA0004224981640000077
估计值为真实值叠加一个零均值的高斯白噪声，即：
Figure BDA0004224981640000078
Figure BDA0004224981640000079
其中，ωt和Wvt是世界坐标系下的角速度和线速度真实值，
Figure BDA00042249816400000710
和/>
Figure BDA00042249816400000711
是对应的噪声项，Rt是设备在世界坐标系下的朝向，通过积分得到i时刻和j时刻之间的相对旋转变换ΔRij和相对平移变换Δpij，该位姿变换估计的误差为eV。
优选地，模块M3.3：回环检测，识别之前到达过的位置，在极坐标上，将4D雷达点云按照网格进行划分，将3D点云映射为一个2维矩阵，矩阵中每个元素的值为对应的网格中雷达点的最大能量强度，随着设备的运动，不断搜索和计算当前帧的2维矩阵和之前所有关键帧生成的2维矩阵的余弦距离，距离小于设定阈值时认为检测到回环；当检测到一个回环，计算当前帧和由回环帧以及附近关键帧组成的子地图之间的相对位姿变换；该位姿变换估计的误差为eL；
模块M3.4：图优化，建立位姿图，考虑所有时刻相对位姿估计的误差项，通过图优化估计出所有时刻设备的最优位姿，位姿中所有的节点对应不同时刻的位姿，节点间的边对应不同时刻之间的相对位姿变换；将所有位姿记为
Figure BDA0004224981640000081
通过解非线性最小二乘问题估计最优位姿/>
Figure BDA0004224981640000082
Figure BDA0004224981640000083
其中，
Figure BDA0004224981640000084
包含所有的时刻，eO,eV,eL对应不同步骤获得的相对位姿变换的误差。
与现有技术相比，本发明具有如下的有益效果：
1、本发明为了在点云稀疏的情况下达到更好的精度，将估计的自车速度进行预积分，获得额外的相对位姿估计；
2、本发明引入了回环检测，识别重新到达过的地方，从全局的角度优化历史位姿，以减少累积漂移，实现精确、鲁棒的建图和定位；
3、本发明能够利用4D毫米波雷达，在未知环境下进行精确、鲁棒的同步定位和建图。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为本发明的方法流程示意图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
实施例1：
基于前人的工作，本发明创新性地提出了一种基于4D毫米波雷达的位姿图SLAM算法，该算法可以应用于载人智能轮椅、自动驾驶汽车等，实现精确、鲁棒的同步定位和建图任务。
本发明提供了一种基于4D毫米波雷达的位姿图SLAM算法。该算法包括三个模块：雷达点云滤波、自车速度估计和位姿图优化。针对4D毫米波雷达(4D雷达)点云噪声较大的问题，设计了一个滤波步骤来减少原始4D雷达点云中的鬼影点和随机噪声；接着，从滤波后的点云的多普勒速度中估计出自车速度，这在接下来的位姿图优化中发挥了重要作用；在位姿图优化中，通过基于正态分布变换的点云配准实现雷达里程计，估计相对位姿变换；为了在点云稀疏的情况下达到更好的精度，将估计的自车速度进行预积分，获得额外的相对位姿估计；此外，引入了回环检测，识别重新到达过的地方，从全局的角度优化历史位姿，以减少累积漂移，实现精确、鲁棒的建图和定位。
根据本发明提供的一种基于4D毫米波雷达的位姿图SLAM计算方法，如图1所示，包括：
步骤S1：提取地面点云，比较连续两帧点云，去除地面下的鬼影点以及随机点；
具体地，在所述步骤S1中：
通过提取地面点云以及比较连续两帧点云，去除地面下的鬼影点以及不稳定的随机点，减少4D雷达点云中的噪声；
步骤S1.1：鬼影点去除，从原始雷达点云中提取出地面点云，滤除地面之下的鬼影点；
对于4D雷达测量的原始雷达点云
Figure BDA0004224981640000091
n为点云中点的数量；
其中每个点在雷达坐标系中的位置表示为
Figure BDA0004224981640000092
其中ri为4D雷达测量得到的该点的距离，θi为4D雷达测量得到的该点的方位角，/>
Figure BDA0004224981640000093
为4D雷达测量得到的该点的高度角；
对于原始的雷达点云，保留距离小于阈值δr且高度在雷达安装高度附近阈值δh内的点，利用主成分分析法，计算出每个点的朝上的法向量，并保留法向量和z轴正向单位向量夹角小于δn的点，利用随机采样一致性算法提取出地面点，并滤除在地面之下的鬼影点；
步骤S1.2：随机点去除，通过对比前后两帧点云识别和滤除随机点，对于当前点云和紧邻的前一帧点云，通过设备速度计算出前后两帧点云的位姿变换，将前一帧点云变换到当前帧的坐标系下，如果当前帧的某个点附近预设范围内不存在变换后的前一帧点云，则将该点归类为随机点并滤除：
计算出当前点云
Figure BDA0004224981640000101
和紧邻的前一帧点云/>
Figure BDA0004224981640000102
之间的旋转变换Rk-1和平移变换tk-1：
Figure BDA0004224981640000103
Figure BDA0004224981640000104
其中，
Figure BDA0004224981640000105
和/>
Figure BDA0004224981640000106
分别是前一帧的设备线速度和角速度估计，Δt是两帧之间的时间差，Exp(·):R3→SO(3)是三维旋转的指数映射，R3是三维实数向量空间，SO(3)是三维特殊正交群，利用Rk-1和tk-1将前一帧点云变换到当前帧的坐标系下得到/>
Figure BDA0004224981640000107
如果当前帧的某个点预设范围内不存在/>
Figure BDA0004224981640000108
中的点，则将该当前点归类为随机点并滤除。
步骤S2：利用4D雷达点云的多普勒速度信息，估计出设备的线速度和角速度；
具体地，在所述步骤S2中：
步骤S2.1：静态点提取，对于4D雷达点云中的第i个点，该点的多普勒速度和雷达自身速度之间的关系如下：
vr,i＝-di·vs
其中，vr,i是第i个点的多普勒速度，
Figure BDA0004224981640000109
是该点相对于雷达的单位方向向量，vs是雷达的速度；
雷达速度和设备速度的关系为：
Figure BDA00042249816400001010
其中，ts和Rs分别为4D雷达在设备坐标系中的安装位置和姿态，
Figure BDA00042249816400001011
和/>
Figure BDA00042249816400001012
分别为设备线速度和角速度，对于4D雷达点云中的所有静态点，多普勒速度和设备速度之间均符合如下方程：
Figure BDA00042249816400001013
其中，m为点云中所有静态点的数量，I3×3为3×3大小的单位矩阵，
Figure BDA00042249816400001014
是ts的反对称矩阵，R3×3表示3×3实矩阵的集合，根据雷达点云中所有静态点的多普勒速度和设备速度之间的关系，利用随机采样一致性算法去除动态的离群点并提取静态点；
步骤S2.2：最小二乘估计，提取出静态点后，利用所有静态点的多普勒速度和设备速度之间的关系，利用最小二乘法，计算出设备速度。
步骤S3：利用相对位姿变换估计并构建位姿图，基于正态分布变换的点云配准，利用估计的设备速度进行预积分，回环检测，利用图优化估计出最优的位姿。
具体地，在所述步骤S3中：
位姿图优化，利用点云配准、速度预积分和回环检测，得到相对位姿变换估计并构建位姿图，最后利用图优化估计出最优的位姿；
步骤S3.1：点云配准，基于正态分布变换，通过当前4D雷达点云和关键帧子地图的匹配估计相对变换；
使用滑动窗口利用多个关键帧点云建立雷达子地图，如果从最新的关键帧到当前点云的平移或旋转超过阈值，则选择当前帧作为一个新的关键帧，将新的关键帧添加到雷达子地图中，当子地图中的关键帧数量超过窗口大小时，最早的关键帧被丢弃；
建立雷达子地图后，将子地图均匀划分为大小相同的网格，将每个网格中的点云建模为局部正态分布，在计算正态分布的均值和协方差时，考虑每个点的测量不确定性，将当前的4D雷达点云与雷达子地图进行配准，计算出使得当前点云在子地图中分布概率最大的相对位姿变换，该位姿变换估计的误差为eO；
步骤S3.2：速度预积分，利用估计的设备速度，计算出相对位姿变换，引入额外的可靠相对位姿估计，将t时刻估计的设备线速度和角速度记为
Figure BDA0004224981640000111
和/>
Figure BDA0004224981640000112
估计值为真实值叠加一个零均值的高斯白噪声，即：
Figure BDA0004224981640000113
Figure BDA0004224981640000114
其中，ωt和Wvt是世界坐标系下的角速度和线速度真实值，
Figure BDA0004224981640000115
和/>
Figure BDA0004224981640000116
是对应的噪声项，Rt是设备在世界坐标系下的朝向，通过积分得到i时刻和j时刻之间的相对旋转变换ΔRij和相对平移变换Δpij，该位姿变换估计的误差为eV。
具体地，步骤S3.3：回环检测，识别之前到达过的位置，在极坐标上，将4D雷达点云按照网格进行划分，将3D点云映射为一个2维矩阵，矩阵中每个元素的值为对应的网格中雷达点的最大能量强度，随着设备的运动，不断搜索和计算当前帧的2维矩阵和之前所有关键帧生成的2维矩阵的余弦距离，距离小于设定阈值时认为检测到回环；当检测到一个回环，计算当前帧和由回环帧以及附近关键帧组成的子地图之间的相对位姿变换；该位姿变换估计的误差为eL；
步骤S3.4：图优化，建立位姿图，考虑所有时刻相对位姿估计的误差项，通过图优化估计出所有时刻设备的最优位姿，位姿中所有的节点对应不同时刻的位姿，节点间的边对应不同时刻之间的相对位姿变换；将所有位姿记为
Figure BDA0004224981640000121
通过解非线性最小二乘问题估计最优位姿/>
Figure BDA0004224981640000122
Figure BDA0004224981640000123
其中，
Figure BDA0004224981640000124
包含所有的时刻，eO,eV,eL对应不同步骤获得的相对位姿变换的误差。
实施例2：
实施例2为实施例1的优选例，以更为具体地对本发明进行说明。
本发明还提供一种基于4D毫米波雷达的位姿图SLAM计算系统，所述基于4D毫米波雷达的位姿图SLAM计算系统可以通过执行所述基于4D毫米波雷达的位姿图SLAM计算方法的流程步骤予以实现，即本领域技术人员可以将所述基于4D毫米波雷达的位姿图SLAM计算方法理解为所述基于4D毫米波雷达的位姿图SLAM计算系统的优选实施方式。
根据本发明提供的一种基于4D毫米波雷达的位姿图SLAM计算系统，包括：
模块M1：提取地面点云，比较连续两帧点云，去除地面下的鬼影点以及随机点；
具体地，在所述模块M1中：
通过提取地面点云以及比较连续两帧点云，去除地面下的鬼影点以及不稳定的随机点，减少4D雷达点云中的噪声；
模块M1.1：鬼影点去除，从原始雷达点云中提取出地面点云，滤除地面之下的鬼影点；
对于4D雷达测量的原始雷达点云
Figure BDA0004224981640000125
n为点云中点的数量；
其中每个点在雷达坐标系中的位置表示为
Figure BDA0004224981640000126
其中ri为4D雷达测量得到的该点的距离，θi为4D雷达测量得到的该点的方位角，/>
Figure BDA0004224981640000127
为4D雷达测量得到的该点的高度角；
对于原始的雷达点云，保留距离小于阈值δr且高度在雷达安装高度附近阈值δh内的点，利用主成分分析法，计算出每个点的朝上的法向量，并保留法向量和z轴正向单位向量夹角小于δn的点，利用随机采样一致性算法提取出地面点，并滤除在地面之下的鬼影点；
模块M1.2：随机点去除，通过对比前后两帧点云识别和滤除随机点，对于当前点云和紧邻的前一帧点云，通过设备速度计算出前后两帧点云的位姿变换，将前一帧点云变换到当前帧的坐标系下，如果当前帧的某个点附近预设范围内不存在变换后的前一帧点云，则将该点归类为随机点并滤除：
计算出当前点云
Figure BDA0004224981640000131
和紧邻的前一帧点云/>
Figure BDA0004224981640000132
之间的旋转变换Rk-1和平移变换tk-1：
Figure BDA0004224981640000133
Figure BDA0004224981640000134
其中，
Figure BDA0004224981640000135
和/>
Figure BDA0004224981640000136
分别是前一帧的设备线速度和角速度估计，Δt是两帧之间的时间差，Exp(·):R3→SO(3)是三维旋转的指数映射，R3是三维实数向量空间，SO(3)是三维特殊正交群，利用Rk-1和tk-1将前一帧点云变换到当前帧的坐标系下得到/>
Figure BDA0004224981640000137
如果当前帧的某个点预设范围内不存在/>
Figure BDA0004224981640000138
中的点，则将该当前点归类为随机点并滤除。
模块M2：利用4D雷达点云的多普勒速度信息，估计出设备的线速度和角速度；
具体地，在所述模块M2中：
模块M2.1：静态点提取，对于4D雷达点云中的第i个点，该点的多普勒速度和雷达自身速度之间的关系如下：
vr,i＝-di·vs
其中，vr,i是第i个点的多普勒速度，
Figure BDA0004224981640000139
是该点相对于雷达的单位方向向量，vs是雷达的速度；
雷达速度和设备速度的关系为：
Figure BDA00042249816400001310
其中，ts和Rs分别为4D雷达在设备坐标系中的安装位置和姿态，
Figure BDA00042249816400001311
和/>
Figure BDA00042249816400001312
分别为设备线速度和角速度，对于4D雷达点云中的所有静态点，多普勒速度和设备速度之间均符合如下方程：
Figure BDA00042249816400001313
其中，m为点云中所有静态点的数量，I3×3为3×3大小的单位矩阵，
Figure BDA00042249816400001314
是ts的反对称矩阵，R3×3表示3×3实矩阵的集合，根据雷达点云中所有静态点的多普勒速度和设备速度之间的关系，利用随机采样一致性算法去除动态的离群点并提取静态点；
模块M2.2：最小二乘估计，提取出静态点后，利用所有静态点的多普勒速度和设备速度之间的关系，利用最小二乘法，计算出设备速度。
模块M3：利用相对位姿变换估计并构建位姿图，基于正态分布变换的点云配准，利用估计的设备速度进行预积分，回环检测，利用图优化估计出最优的位姿。
具体地，在所述模块M3中：
位姿图优化，利用点云配准、速度预积分和回环检测，得到相对位姿变换估计并构建位姿图，最后利用图优化估计出最优的位姿；
模块M3.1：点云配准，基于正态分布变换，通过当前4D雷达点云和关键帧子地图的匹配估计相对变换；
使用滑动窗口利用多个关键帧点云建立雷达子地图，如果从最新的关键帧到当前点云的平移或旋转超过阈值，则选择当前帧作为一个新的关键帧，将新的关键帧添加到雷达子地图中，当子地图中的关键帧数量超过窗口大小时，最早的关键帧被丢弃；
建立雷达子地图后，将子地图均匀划分为大小相同的网格，将每个网格中的点云建模为局部正态分布，在计算正态分布的均值和协方差时，考虑每个点的测量不确定性，将当前的4D雷达点云与雷达子地图进行配准，计算出使得当前点云在子地图中分布概率最大的相对位姿变换，该位姿变换估计的误差为eo；
模块M3.2：速度预积分，利用估计的设备速度，计算出相对位姿变换，引入额外的可靠相对位姿估计，将t时刻估计的设备线速度和角速度记为
Figure BDA0004224981640000141
和/>
Figure BDA0004224981640000142
估计值为真实值叠加一个零均值的高斯白噪声，即：
Figure BDA0004224981640000143
Figure BDA0004224981640000144
其中，ωt和Wvt是世界坐标系下的角速度和线速度真实值，
Figure BDA0004224981640000145
和/>
Figure BDA0004224981640000146
是对应的噪声项，Rt是设备在世界坐标系下的朝向，通过积分得到i时刻和j时刻之间的相对旋转变换ΔRij和相对平移变换Δpij，该位姿变换估计的误差为eV。
具体地，模块M3.3：回环检测，识别之前到达过的位置，在极坐标上，将4D雷达点云按照网格进行划分，将3D点云映射为一个2维矩阵，矩阵中每个元素的值为对应的网格中雷达点的最大能量强度，随着设备的运动，不断搜索和计算当前帧的2维矩阵和之前所有关键帧生成的2维矩阵的余弦距离，距离小于设定阈值时认为检测到回环；当检测到一个回环，计算当前帧和由回环帧以及附近关键帧组成的子地图之间的相对位姿变换；该位姿变换估计的误差为eL；
模块M3.4：图优化，建立位姿图，考虑所有时刻相对位姿估计的误差项，通过图优化估计出所有时刻设备的最优位姿，位姿中所有的节点对应不同时刻的位姿，节点间的边对应不同时刻之间的相对位姿变换；将所有位姿记为
Figure BDA0004224981640000147
通过解非线性最小二乘问题估计最优位姿/>
Figure BDA0004224981640000148
Figure BDA0004224981640000151
其中，
Figure BDA0004224981640000152
包含所有的时刻，eO,eV,eL对应不同步骤获得的相对位姿变换的误差。/>
实施例3：
实施例3为实施例1的优选例，以更为具体地对本发明进行说明。
针对原有技术的不足，本文创新性地提出了一种基于4D毫米波雷达的位姿图SLAM算法，适用于未知环境下汽车或者移动机器人的同步定位和建图问题。
本发明提出的基于4D毫米波雷达的位姿图SLAM算法，主要包括以下步骤：
步骤1：雷达点云滤波，原始的4D雷达点云中主要存在鬼影点和随机点两种噪声，通过提取地面点云以及比较连续两帧点云，去除地面下的鬼影点以及不稳定的随机点，减少4D雷达点云中的噪声；
步骤2：自车速度估计，4D毫米波雷达可以获得每个点的多普勒速度，多普勒速度是雷达和目标点之间相对速度的径向分量。利用4D雷达点云的多普勒速度信息，估计出自车的线速度和角速度；
步骤3：位姿图优化，基于正态分布变换对当前雷达点云和局部的雷达关键帧子地图进行配准，得到雷达里程计因子；利用估计的自车速度进行预积分得到速度预积分因子；并进行回环检测得到回环因子；最后利用三种因子构建位姿图，并基于图优化估计出最优的位姿。
具体地，所述步骤1的雷达点云滤波过程主要包括以下步骤：
步骤1.1：鬼影点去除，鬼影点是由于毫米波的多径效应，在原始4D雷达点云中存在的地面之下的虚假测量，属于4D雷达测量噪声之一。通过从原始雷达点云中提取出地面点云，然后滤除地面之下的鬼影点。
对于4D雷达获得的原始雷达点云
Figure BDA0004224981640000153
n为点云中点的数量，其中每个点在雷达坐标系中的位置都可以表示为
Figure BDA0004224981640000154
其中ri,θi,/>
Figure BDA0004224981640000155
分别为4D雷达测量得到的该点的空间信息：距离、方位角和高度角。对于原始的雷达点云，首先保留距离小于阈值δr且高度在雷达安装高度H附近一定阈值δh内的点,即对于所有的/>
Figure BDA0004224981640000156
保留
Figure BDA0004224981640000157
且zi∈[-H-Δh,-H+Δh]的点，因为这些点最有可能属于地面。然后利用主成分分析法，估计出每个点的朝上的法向量ni。接着，保留法向量和z轴正向单位向量夹角小于δn的点，即ni·(0,0,1)T≥cosδn的点。最后,利用随机采样一致性算法提取出平面Ax+By+Cz+D＝0，认为该平面为地面，并滤除在地面之下的鬼影点，即Axi+Byi+Czi+D<0的点；
步骤1.2：随机点去除，随机点是原始4D雷达点云中包含的不稳定的、闪烁出现的虚假测量，属于4D雷达测量噪声之一。由于随机点在连续多帧雷达点云中不会连续出现，因此通过对比前后两帧点云来识别和滤除随机点。
首先计算出当前点云
Figure BDA0004224981640000161
和紧邻的前一帧点云/>
Figure BDA0004224981640000162
之间的旋转变换Rk-1和平移变换tk-1：
Figure BDA0004224981640000163
Figure BDA0004224981640000164
其中，
Figure BDA0004224981640000165
和/>
Figure BDA0004224981640000166
分别是前一帧的自车线速度和角速度估计，Δt是两帧之间的时间差，Exp(·)∶R3→SO(3)是三维旋转的指数映射(R3是三维实数向量空间，SO(3)是三维特殊正交群)。然后利用Rk-1和tk-1将前一帧点云变换到当前帧的坐标系下得到/>
Figure BDA0004224981640000167
如果当前帧的某个点附近不存在/>
Figure BDA0004224981640000168
中的点，则将该当前点归类为随机点并滤除。
具体地，所述步骤2的自车速度估计过程主要包括以下步骤：
步骤2.1：静态点提取。除了空间信息外，4D雷达还可以获得点云中每个点的多普勒速度。对于4D雷达点云中的第i个点，若它是真实世界中绝对速度为0的静止点，则它与雷达的相对速度和雷达的自身速度大小相等、方向相反，且多普勒速度是被测量的点与4D雷达之间相对速度的径向分量，因此该点的多普勒速度和雷达自身速度之间的关系如下：
vr,i＝-di·vs,
其中，vr,i是第i个点的多普勒速度，
Figure BDA0004224981640000169
是该点相对于雷达的单位方向向量，vs是雷达的速度。
此外，由于4D雷达安装在汽车上，因此雷达速度和汽车的自车速度的关系为
Figure BDA00042249816400001610
其中，ts和Rs分别为4D雷达在汽车坐标系中的安装位置和姿态，
Figure BDA00042249816400001611
和/>
Figure BDA00042249816400001612
分别为自车线速度和角速度。在真实世界中，4D雷达点云中的大多数点都是静态点，因此，对于4D雷达点云中的所有静态点，多普勒速度和自车速度之间均符合如下方程：
Figure BDA00042249816400001613
其中，m为点云中所有静态点的数量，I3×3为3×3大小的单位矩阵，
Figure BDA00042249816400001614
是ts的反对称矩阵(R3×3表示3×3实矩阵的集合)。根据雷达点云中所有静态点的多普勒速度和自车速度之间的关系，利用随机采样一致性算法来去除动态的离群点并提取静态点。
步骤2.2：最小二乘估计，提取出静态点后，再次利用所有静态点的多普勒速度和自车速度之间的关系，利用最小二乘法，估计出自车速度。
具体地，所述步骤3的位姿图优化过程主要包括以下步骤：
步骤3.1：点云配准，基于正态分布变换，通过当前4D雷达点云和关键帧子地图的直接匹配来估计相对变换。
由于4D雷达点云过于稀疏，无法只通过单帧点云的匹配来正确估计姿态，因此使用滑动窗口方法，利用多个关键帧点云建立一个更稠密的雷达子地图。具体地，如果从最新的关键帧到当前点云的平移或旋转超过一个阈值，则选择当前帧作为一个新的关键帧。然后，将新的关键帧添加到雷达子地图中，并且在位姿图中加入一个和此关键帧关联的节点。当子地图中的关键帧数量超过窗口大小时，最早的关键帧将被丢弃。通过这种方法建立的雷达子地图比单帧的雷达点云能更清晰反映了局部的环境特征，提高了点云配准的准确性和鲁棒性。
建立雷达子地图后，将子地图均匀划分为大小相同的网格，将每个网格中的点云建模为局部正态分布。由于4D雷达点云非常稀疏，经过网格分割后容易会出现一个网格中的点不足三个或共线(面)的情况，导致正态分布的协方差难以求逆。因此在计算正态分布的均值和协方差时，综合考虑每个点的测量不确定性。这样做的好处是减少了协方差的退化效应，并且在网格中点很少的情况下也能充分利用该网格的点云信息。然后，将当前的4D雷达点云与雷达子地图进行配准，估计出使得当前点云在子地图中分布概率最大的相对位姿变换，并作为雷达里程计因子加入到位姿图中。该位姿变换估计的误差为eO。
步骤3.2：速度预积分，利用估计的自车速度，计算出相对位姿变换，从而引入额外的可靠相对位姿估计，提高SLAM的准确性和鲁棒性。将t时刻估计的自车线速度和角速度记为
Figure BDA0004224981640000171
和/>
Figure BDA0004224981640000172
且估计值可以看作是真实值叠加一个零均值的高斯白噪声，即：
Figure BDA0004224981640000173
Figure BDA0004224981640000174
其中，ωt和Wvt是世界坐标系下的角速度和线速度真实值，
Figure BDA0004224981640000175
和/>
Figure BDA0004224981640000176
是对应的噪声项，Rt是汽车在世界坐标系下的朝向。由此可以得到i时刻和j时刻之间的相对旋转变换ΔRij和相对平移变换Δpij:
Figure BDA0004224981640000177
Figure BDA0004224981640000181
Figure BDA0004224981640000182
其中，
Figure BDA0004224981640000183
Jr,k是/>
Figure BDA0004224981640000184
的右雅克比矩阵，/>
Figure BDA0004224981640000185
是向量/>
Figure BDA0004224981640000186
的反对称矩阵，/>
Figure BDA0004224981640000187
和/>
Figure BDA0004224981640000188
分别为通过自车速度预积分得到的相对旋转和平移，并作为时刻i和j对应的节点之间的速度预积分因子加入到位姿图中，δφij和δpij分别为对应的噪声。该位姿变换估计的误差为eV。
步骤3.3：回环检测，识别之前到达过的位置，减少累积漂移。在极坐标上，将4D雷达点云按照网格进行划分，将3D点云映射为一个2维矩阵，矩阵中每个元素的值为对应的网格中雷达点的最大能量强度。随着车辆的运动，不断搜索和计算当前帧的2维矩阵和之前所有关键帧生成的2维矩阵的余弦距离，距离小于设定阈值时认为检测到回环。一旦检测到一个回环，由点云配准步骤介绍的配准方法，计算当前帧和由回环帧以及附近关键帧组成的子地图之间的相对位姿变换。该位姿变换估计的误差为eL。
步骤3.4：图优化，建立位姿图，综合考虑所有时刻相对位姿估计的误差项，通过图优化估计出所有时刻自车的最优位姿。位姿中所有的节点对应不同时刻的位姿，节点间的边对应不同时刻之间的相对位姿变换。将所有位姿记为
Figure BDA0004224981640000189
通过解非线性最小二乘问题估计最优位姿/>
Figure BDA00042249816400001810
/>
Figure BDA00042249816400001811
其中，
Figure BDA00042249816400001812
包含所有的时刻，eO,eV,eL对应不同步骤获得的相对位姿变换的误差。
在具体的实施过程中，如图1所示，本发明分为(1)雷达点云滤波(2)自车速度估计(3)位姿图优化三个部分进行，主要有以下步骤：
步骤1：雷达点云滤波，原始的4D雷达点云中主要存在鬼影点和随机点两种噪声，通过提取地面点云以及比较连续两帧点云，去除地面下的鬼影点以及不稳定的随机点，减少4D雷达点云中的噪声。
步骤1.1：鬼影点去除，鬼影点是由于毫米波的多径效应，在原始4D雷达点云中存在的地面之下的虚假测量，属于4D雷达测量噪声之一。通过从原始雷达点云中提取出地面点云，然后滤除地面之下的鬼影点。
对于4D雷达获得的原始雷达点云
Figure BDA0004224981640000191
n为点云中点的数量，其中每个点在雷达坐标系中的位置都可以表示为
Figure BDA0004224981640000192
其中ri,θi,/>
Figure BDA0004224981640000193
分别为4D雷达测量得到的该点的空间信息：距离、方位角和高度角。对于原始的雷达点云，首先保留距离小于阈值δr且高度在雷达安装高度H附近一定阈值δh内的点,即对于所有的/>
Figure BDA0004224981640000194
保留
Figure BDA0004224981640000195
且zi∈[-H-δh,-H+δh]的点，因为这些点最有可能属于地面。然后利用主成分分析法，估计出每个点的朝上的法向量ni。接着，保留法向量和z轴正向单位向量夹角小于δn点，即ni·(0,0,1）T≥cosδn的点。最后,利用随机采样一致性算法提取出平面Ax+By+Cz+D＝0，认为该平面为地面，并滤除在地面之下的鬼影点，即Axi+Bti+Czi+D<0的点；
步骤1.2：随机点去除，随机点是原始4D雷达点云中包含的不稳定的、闪烁出现的虚假测量，属于4D雷达测量噪声之一。由于随机点在连续多帧雷达点云中不会连续出现，因此通过对比前后两帧点云来识别和滤除随机点。
首先计算出当前点云
Figure BDA0004224981640000196
和紧邻的前一帧点云/>
Figure BDA0004224981640000197
之间的旋转变换Rk-1和平移变换tk-1：
Figure BDA0004224981640000198
Figure BDA0004224981640000199
其中，
Figure BDA00042249816400001910
和/>
Figure BDA00042249816400001911
分别是前一帧的自车线速度和角速度估计，Δt是两帧之间的时间差，Exp(·):R3→SO(3)是三维旋转的指数映射(R3是三维实数向量空间，SO(3)是三维特殊正交群)。然后利用Rk-1和tk-1将前一帧点云变换到当前帧的坐标系下，得到变换后的前一帧点云/>
Figure BDA00042249816400001912
接着，利用/>
Figure BDA00042249816400001913
构建KD树以实现最近邻搜索。如果当前点云/>
Figure BDA00042249816400001914
的某个点一定范围内不存在/>
Figure BDA00042249816400001915
中的点，则将该当前点归类为随机点并滤除。
步骤2：自车速度估计，4D毫米波雷达可以获得每个点的多普勒速度，多普勒速度是雷达和目标点之间相对速度的径向分量。基于多普勒速度的这个性质，利用4D雷达点云的多普勒速度信息，估计出自车的线速度和角速度。
步骤2.1：静态点提取。除了空间信息外，4D雷达还可以获得点云中每个点的多普勒速度。对于4D雷达点云中的第i个点，若它是真实世界中绝对速度为0的静止点，则它与雷达的相对速度和雷达的自身速度大小相等、方向相反，且多普勒速度是被测量的点与4D雷达之间相对速度的径向分量，因此该点的多普勒速度和雷达自身速度之间的关系如下：
vr,i＝-di·vs,
其中，vr,i是第i个点的多普勒速度，
Figure BDA0004224981640000201
是该点相对于雷达的单位方向向量，vs是雷达的速度。
此外，由于4D雷达安装在汽车上，因此雷达速度和汽车的自车速度的关系为
Figure BDA0004224981640000202
其中，ts和Rs分别为4D雷达在汽车坐标系中的安装位置和姿态，
Figure BDA0004224981640000203
和/>
Figure BDA0004224981640000204
分别为自车线速度和角速度。在真实世界中，4D雷达点云中的大多数点都是静态点，因此，对于4D雷达点云中的所有静态点，多普勒速度和自车速度之间均符合如下方程：
Figure BDA0004224981640000205
其中，m为点云中所有静态点的数量，I3×3为3×3大小的单位矩阵，
Figure BDA0004224981640000206
是ts的反对称矩阵(R3×3表示3×3实矩阵的集合)。由于动态点的多普勒速度和自车速度之间的关系不符合上述方程，因此利用随机采样一致性算法去除动态的离群点，并提取静态点。
步骤2.2：最小二乘估计，提取出静态点后，再次利用所有静态点的多普勒速度和自车速度之间的关系，利用最小二乘法，估计出自车速度。
步骤3：位姿图优化，基于正态分布变换对当前雷达点云和局部的雷达关键帧子地图进行配准，得到毫米波雷达里程计因子；利用估计的自车速度进行预积分得到速度预积分因子；并进行回环检测得到回环因子；最后利用三种因子构建位姿图，并基于图优化估计出最优的位姿。
步骤3.1：点云配准，基于正态分布变换，通过当前4D雷达点云和关键帧子地图的直接匹配来估计相对变换。
由于4D雷达点云过于稀疏，无法只通过单帧点云的匹配来正确估计姿态，因此使用滑动窗口方法，利用多个关键帧点云建立一个更稠密的雷达子地图。具体地，如果从最新的关键帧到当前点云的平移或旋转超过一个阈值，则选择当前帧作为一个新的关键帧。然后，将新的关键帧添加到雷达子地图中，并且在位姿图中加入一个和此关键帧关联的节点。当子地图中的关键帧数量超过窗口大小时，最早的关键帧将被丢弃。通过这种方法建立的雷达子地图比单帧的雷达点云能更清晰反映了局部的环境特征，提高了点云配准的准确性和鲁棒性。
建立雷达子地图后，将子地图均匀划分为大小相同的网格，将每个网格中的点云建模为局部正态分布。由于4D雷达点云非常稀疏，经过网格分割后容易会出现一个网格中的点不足三个或共线(面)的情况，导致正态分布的协方差难以求逆。因此在计算正态分布的均值和协方差时，综合考虑每个点的测量不确定性。这样做的好处是减少了协方差的退化效应，并且在网格中点很少的情况下也能充分利用该网格的点云信息。然后，将当前的4D雷达点云与雷达子地图进行配准，估计出使得当前点云在子地图中分布概率最大的相对位姿变换
Figure BDA0004224981640000211
并作为时刻i和j对应的节点之间的雷达里程计因子加入到位姿图中。该位姿变换估计的误差为eO，形式如下：
Figure BDA0004224981640000212
其中，
Figure BDA0004224981640000213
是对应的协方差矩阵，Log(·)∶SE(3)→R6是三维变换的对数映射(SE(3)是三维特殊欧式群，R6是六维实数向量空间)。
步骤3.2：速度预积分，利用估计的自车速度，计算出相对位姿变换，从而引入额外的可靠相对位姿估计，提高SLAM的准确性和鲁棒性。将t时刻估计的自车线速度和角速度记为
Figure BDA0004224981640000214
和/>
Figure BDA0004224981640000215
且估计值可以看作是真实值叠加一个零均值的高斯白噪声，即：
Figure BDA0004224981640000216
Figure BDA0004224981640000217
其中，ωt和Wvt是世界坐标系下的角速度和线速度真实值，
Figure BDA0004224981640000218
和/>
Figure BDA0004224981640000219
是对应的噪声项，Rt是汽车在世界坐标系下的朝向。假设在一个很短的时间段[t,t+Δt]内，ωt和Wvt保持不变，则在t+Δt时刻的位置pt+Δt和旋转Rt+Δt可以通过t时刻的位置pt和旋转Rt计算得到：
Figure BDA00042249816400002110
Figure BDA00042249816400002111
对于连续的两个时刻i和j对应的关键帧之间的每一帧的点云，都可以得到一个自车速度估计。因此对于时刻i和j之间的所有时间段Δt进行积分，在j时刻的位置pj和旋转Rj可以通过i时刻的位置pi和旋转Ri计算得到：
Figure BDA00042249816400002112
Figure BDA00042249816400002113
由此可以得到i时刻和j时刻之间的相对旋转变换ΔRij和相对平移变换Δpij:
Figure BDA0004224981640000221
/>
Figure BDA0004224981640000222
其中，
Figure BDA0004224981640000223
Jr,k是/>
Figure BDA0004224981640000224
的右雅克比矩阵，/>
Figure BDA0004224981640000225
是向量/>
Figure BDA0004224981640000226
的反对称矩阵，/>
Figure BDA0004224981640000227
和/>
Figure BDA0004224981640000228
分别为通过自车速度预积分得到的相对旋转和平移，并作为时刻i和j对应的节点之间的速度预积分因子加入到位姿图中。δφij和δpij分别为对应的噪声，形式为：
Figure BDA0004224981640000229
Figure BDA00042249816400002210
由于δφij是零均值高斯噪声
Figure BDA00042249816400002211
的线性组合，因此δφij为零均值高斯噪声。同理，δpij是零均值高斯噪声φik和/>
Figure BDA00042249816400002212
的线性组合，因此也是零均值高斯噪声。记高斯噪声
Figure BDA00042249816400002213
则该位姿变换估计的误差为
Figure BDA00042249816400002214
Figure BDA00042249816400002215
其中，Log(·)∶SO(3)→R3是三维旋转的对数映射。
步骤3.3：回环检测，识别之前到达过的位置，减少累积漂移。在极坐标上，将4D雷达点云按照网格进行划分，将3D点云映射为一个2维矩阵，矩阵中每个元素的值为对应的网格中雷达点的最大能量强度。随着车辆的运动，不断搜索和计算当前帧的2维矩阵和之前所有关键帧生成的2维矩阵的余弦距离，距离小于设定阈值时认为检测到回环。一旦检测到一个回环，由点云配准步骤介绍的配准方法，计算当前帧j和由回环帧k以及附近关键帧组成的子地图之间的相对位姿变换
Figure BDA0004224981640000231
然后将/>
Figure BDA0004224981640000232
作为时刻j和k对应的节点之间的回环因子加入到位姿图中。该位姿变换估计的误差为
Figure BDA0004224981640000233
其中，
Figure BDA0004224981640000234
包含所有的回环帧，/>
Figure BDA0004224981640000235
是对应的协方差矩阵，Log(·)∶SE(3)→R6是三维变换的对数映射。
步骤3.4：图优化，通过建立位姿图，综合考虑所有相对位姿估计的误差项，并利用图优化估计出所有时刻自车的最优位姿，实现更加精确、鲁棒的定位和建图。位姿中所有的节点对应不同时刻的位姿，节点间的边对应不同时刻之间的相对位姿变换。将位姿记作x＝(R,p)∈SE(3)，其中R和p分别为旋转和平移。将所有时刻的位姿记为
Figure BDA0004224981640000236
通过解非线性最小二乘问题，估计最优的位姿/>
Figure BDA0004224981640000237
Figure BDA0004224981640000238
其中，K包含所有的时刻，eO,eV,eL对应不同步骤获得的相对位姿变换的误差。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种基于4D毫米波雷达的位姿图SLAM计算方法，其特征在于，包括：
步骤S1：提取地面点云，比较连续两帧点云，去除地面下的鬼影点以及随机点；
步骤S2：利用4D雷达点云的多普勒速度信息，估计出设备的线速度和角速度；
步骤S3：利用相对位姿变换估计并构建位姿图，基于正态分布变换的点云配准，利用估计的设备速度进行预积分，回环检测，利用图优化估计出最优的位姿。
2.根据权利要求1所述的基于4D毫米波雷达的位姿图SLAM计算方法，其特征在于，在所述步骤S1中：
通过提取地面点云以及比较连续两帧点云，去除地面下的鬼影点以及不稳定的随机点，减少4D雷达点云中的噪声；
步骤S1.1：鬼影点去除，从原始雷达点云中提取出地面点云，滤除地面之下的鬼影点；
对于4D雷达测量的原始雷达点云
Figure QLYQS_1
n为点云中点的数量；
其中每个点在雷达坐标系中的位置表示为
Figure QLYQS_2
其中ri为4D雷达测量得到的该点的距离，θi为4D雷达测量得到的该点的方位角，/>
Figure QLYQS_3
为4D雷达测量得到的该点的高度角；
对于原始的雷达点云，保留距离小于阈值δr且高度在雷达安装高度附近阈值δh内的点，利用主成分分析法，计算出每个点的朝上的法向量，并保留法向量和z轴正向单位向量夹角小于δn的点，利用随机采样一致性算法提取出地面点，并滤除在地面之下的鬼影点；
步骤S1.2：随机点去除，通过对比前后两帧点云识别和滤除随机点，对于当前点云和紧邻的前一帧点云，通过设备速度计算出前后两帧点云的位姿变换，将前一帧点云变换到当前帧的坐标系下，如果当前帧的某个点附近预设范围内不存在变换后的前一帧点云，则将该点归类为随机点并滤除：
计算出当前点云
Figure QLYQS_4
和紧邻的前一帧点云/>
Figure QLYQS_5
之间的旋转变换Rk-1和平移变换tk-1：
Figure QLYQS_6
Figure QLYQS_7
其中，
Figure QLYQS_8
和/>
Figure QLYQS_9
分别是前一帧的设备线速度和角速度估计，Δt是两帧之间的时间差，Exp(·):3→SO(3)是三维旋转的指数映射，R3是三维实数向量空间，SO(3)是三维特殊正交群，利用Rk-1和tk-1将前一帧点云变换到当前帧的坐标系下得到/>
Figure QLYQS_10
如果当前帧的某个点预设范围内不存在/>
Figure QLYQS_11
中的点，则将该当前点归类为随机点并滤除。
3.根据权利要求1所述的基于4D毫米波雷达的位姿图SLAM计算方法，其特征在于，在所述步骤S2中：
步骤S2.1：静态点提取，对于4D雷达点云中的第i个点，该点的多普勒速度和雷达自身速度之间的关系如下：
vr,i＝-di·vs
其中，vr,i是第i个点的多普勒速度，
Figure QLYQS_12
是该点相对于雷达的单位方向向量，vs是雷达的速度；
雷达速度和设备速度的关系为：
Figure QLYQS_13
其中，ts和Rs分别为4D雷达在设备坐标系中的安装位置和姿态，
Figure QLYQS_14
和/>
Figure QLYQS_15
分别为设备线速度和角速度，对于4D雷达点云中的所有静态点，多普勒速度和设备速度之间均符合如下方程：
Figure QLYQS_16
其中，m为点云中所有静态点的数量，I3×3为3×3大小的单位矩阵，
Figure QLYQS_17
是ts的反对称矩阵，R3×3表示3×3实矩阵的集合，根据雷达点云中所有静态点的多普勒速度和设备速度之间的关系，利用随机采样一致性算法去除动态的离群点并提取静态点；
步骤S2.2：最小二乘估计，提取出静态点后，利用所有静态点的多普勒速度和设备速度之间的关系，利用最小二乘法，计算出设备速度。
4.根据权利要求1所述的基于4D毫米波雷达的位姿图SLAM计算方法，其特征在于，在所述步骤S3中：
位姿图优化，利用点云配准、速度预积分和回环检测，得到相对位姿变换估计并构建位姿图，最后利用图优化估计出最优的位姿；
步骤S3.1：点云配准，基于正态分布变换，通过当前4D雷达点云和关键帧子地图的匹配估计相对变换；
使用滑动窗口利用多个关键帧点云建立雷达子地图，如果从最新的关键帧到当前点云的平移或旋转超过阈值，则选择当前帧作为一个新的关键帧，将新的关键帧添加到雷达子地图中，当子地图中的关键帧数量超过窗口大小时，最早的关键帧被丢弃；
建立雷达子地图后，将子地图均匀划分为大小相同的网格，将每个网格中的点云建模为局部正态分布，在计算正态分布的均值和协方差时，考虑每个点的测量不确定性，将当前的4D雷达点云与雷达子地图进行配准，计算出使得当前点云在子地图中分布概率最大的相对位姿变换，该位姿变换估计的误差为eO；
步骤S3.2：速度预积分，利用估计的设备速度，计算出相对位姿变换，引入额外的可靠相对位姿估计，将t时刻估计的设备线速度和角速度记为
Figure QLYQS_18
和/>
Figure QLYQS_19
估计值为真实值叠加一个零均值的高斯白噪声，即：
Figure QLYQS_20
Figure QLYQS_21
其中，ωt和Wvt是世界坐标系下的角速度和线速度真实值，
Figure QLYQS_22
和/>
Figure QLYQS_23
是对应的噪声项，Rt是设备在世界坐标系下的朝向，通过积分得到i时刻和j时刻之间的相对旋转变换ΔRij和相对平移变换Δpij，该位姿变换估计的误差为eV。
5.根据权利要求4所述的基于4D毫米波雷达的位姿图SLAM计算方法，其特征在于：
步骤S3.3：回环检测，识别之前到达过的位置，在极坐标上，将4D雷达点云按照网格进行划分，将3D点云映射为一个2维矩阵，矩阵中每个元素的值为对应的网格中雷达点的最大能量强度，随着设备的运动，不断搜索和计算当前帧的2维矩阵和之前所有关键帧生成的2维矩阵的余弦距离，距离小于设定阈值时认为检测到回环；当检测到一个回环，计算当前帧和由回环帧以及附近关键帧组成的子地图之间的相对位姿变换；该位姿变换估计的误差为eL；
步骤S3.4：图优化，建立位姿图，考虑所有时刻相对位姿估计的误差项，通过图优化估计出所有时刻设备的最优位姿，位姿中所有的节点对应不同时刻的位姿，节点间的边对应不同时刻之间的相对位姿变换；将所有位姿记为
Figure QLYQS_24
通过解非线性最小二乘问题估计最优位姿/>
Figure QLYQS_25
Figure QLYQS_26
其中，
Figure QLYQS_27
包含所有的时刻，eO,eV,eL对应不同步骤获得的相对位姿变换的误差。
6.一种基于4D毫米波雷达的位姿图SLAM计算系统，其特征在于，包括：
模块M1：提取地面点云，比较连续两帧点云，去除地面下的鬼影点以及随机点；
模块M2：利用4D雷达点云的多普勒速度信息，估计出设备的线速度和角速度；
模块M3：利用相对位姿变换估计并构建位姿图，基于正态分布变换的点云配准，利用估计的设备速度进行预积分，回环检测，利用图优化估计出最优的位姿。
7.根据权利要求6所述的基于4D毫米波雷达的位姿图SLAM计算系统，其特征在于，在所述模块M1中：
通过提取地面点云以及比较连续两帧点云，去除地面下的鬼影点以及不稳定的随机点，减少4D雷达点云中的噪声；
模块M1.1：鬼影点去除，从原始雷达点云中提取出地面点云，滤除地面之下的鬼影点；
对于4D雷达测量的原始雷达点云
Figure QLYQS_28
n为点云中点的数量；
其中每个点在雷达坐标系中的位置表示为
Figure QLYQS_29
其中ri为4D雷达测量得到的该点的距离，θi为4D雷达测量得到的该点的方位角，/>
Figure QLYQS_30
为4D雷达测量得到的该点的高度角；
对于原始的雷达点云，保留距离小于阈值δr且高度在雷达安装高度附近阈值δh内的点，利用主成分分析法，计算出每个点的朝上的法向量，并保留法向量和z轴正向单位向量夹角小于δn的点，利用随机采样一致性算法提取出地面点，并滤除在地面之下的鬼影点；
模块M1.2：随机点去除，通过对比前后两帧点云识别和滤除随机点，对于当前点云和紧邻的前一帧点云，通过设备速度计算出前后两帧点云的位姿变换，将前一帧点云变换到当前帧的坐标系下，如果当前帧的某个点附近预设范围内不存在变换后的前一帧点云，则将该点归类为随机点并滤除：
计算出当前点云
Figure QLYQS_31
和紧邻的前一帧点云/>
Figure QLYQS_32
之间的旋转变换Rk-1和平移变换tk-1：
Figure QLYQS_33
Figure QLYQS_34
其中，
Figure QLYQS_35
和/>
Figure QLYQS_36
分别是前一帧的设备线速度和角速度估计，Δt是两帧之间的时间差，Exp(·):3→SO(3)是三维旋转的指数映射，R3是三维实数向量空间，SO(3)是三维特殊正交群，利用Rk-1和tk-1将前一帧点云变换到当前帧的坐标系下得到/>
Figure QLYQS_37
如果当前帧的某个点预设范围内不存在/>
Figure QLYQS_38
中的点，则将该当前点归类为随机点并滤除。
8.根据权利要求6所述的基于4D毫米波雷达的位姿图SLAM计算系统，其特征在于，在所述模块M2中：
模块M2.1：静态点提取，对于4D雷达点云中的第i个点，该点的多普勒速度和雷达自身速度之间的关系如下：
vr,＝-di·s
其中，vr,i是第i个点的多普勒速度，
Figure QLYQS_39
是该点相对于雷达的单位方向向量，vs是雷达的速度；
雷达速度和设备速度的关系为：
Figure QLYQS_40
其中，ts和Rs分别为4D雷达在设备坐标系中的安装位置和姿态，
Figure QLYQS_41
和/>
Figure QLYQS_42
分别为设备线速度和角速度，对于4D雷达点云中的所有静态点，多普勒速度和设备速度之间均符合如下方程：
Figure QLYQS_43
其中，m为点云中所有静态点的数量，I3×3为3×3大小的单位矩阵，
Figure QLYQS_44
是ts的反对称矩阵，R3×3表示3×3实矩阵的集合，根据雷达点云中所有静态点的多普勒速度和设备速度之间的关系，利用随机采样一致性算法去除动态的离群点并提取静态点；
模块M2.2：最小二乘估计，提取出静态点后，利用所有静态点的多普勒速度和设备速度之间的关系，利用最小二乘法，计算出设备速度。
9.根据权利要求6所述的基于4D毫米波雷达的位姿图SLAM计算系统，其特征在于，在所述模块M3中：
位姿图优化，利用点云配准、速度预积分和回环检测，得到相对位姿变换估计并构建位姿图，最后利用图优化估计出最优的位姿；
模块M3.1：点云配准，基于正态分布变换，通过当前4D雷达点云和关键帧子地图的匹配估计相对变换；
使用滑动窗口利用多个关键帧点云建立雷达子地图，如果从最新的关键帧到当前点云的平移或旋转超过阈值，则选择当前帧作为一个新的关键帧，将新的关键帧添加到雷达子地图中，当子地图中的关键帧数量超过窗口大小时，最早的关键帧被丢弃；
建立雷达子地图后，将子地图均匀划分为大小相同的网格，将每个网格中的点云建模为局部正态分布，在计算正态分布的均值和协方差时，考虑每个点的测量不确定性，将当前的4D雷达点云与雷达子地图进行配准，计算出使得当前点云在子地图中分布概率最大的相对位姿变换，该位姿变换估计的误差为eO；
模块M3.2：速度预积分，利用估计的设备速度，计算出相对位姿变换，引入额外的可靠相对位姿估计，将t时刻估计的设备线速度和角速度记为
Figure QLYQS_45
和/>
Figure QLYQS_46
估计值为真实值叠加一个零均值的高斯白噪声，即：
Figure QLYQS_47
Figure QLYQS_48
其中，ωt和Wvt是世界坐标系下的角速度和线速度真实值，
Figure QLYQS_49
和/>
Figure QLYQS_50
是对应的噪声项，Rt是设备在世界坐标系下的朝向，通过积分得到i时刻和j时刻之间的相对旋转变换ΔRij和相对平移变换Δpij，该位姿变换估计的误差为eV。
10.根据权利要求9所述的基于4D毫米波雷达的位姿图SLAM计算系统，其特征在于：
模块M3.3：回环检测，识别之前到达过的位置，在极坐标上，将4D雷达点云按照网格进行划分，将3D点云映射为一个2维矩阵，矩阵中每个元素的值为对应的网格中雷达点的最大能量强度，随着设备的运动，不断搜索和计算当前帧的2维矩阵和之前所有关键帧生成的2维矩阵的余弦距离，距离小于设定阈值时认为检测到回环；当检测到一个回环，计算当前帧和由回环帧以及附近关键帧组成的子地图之间的相对位姿变换；该位姿变换估计的误差为eL；
模块M3.4：图优化，建立位姿图，考虑所有时刻相对位姿估计的误差项，通过图优化估计出所有时刻设备的最优位姿，位姿中所有的节点对应不同时刻的位姿，节点间的边对应不同时刻之间的相对位姿变换；将所有位姿记为
Figure QLYQS_51
通过解非线性最小二乘问题估计最优位姿/>
Figure QLYQS_52
Figure QLYQS_53
其中，
Figure QLYQS_54
包含所有的时刻，eO,eV,eL对应不同步骤获得的相对位姿变换的误差。
CN202310532630.2A 2023-05-11 2023-05-11 基于4d毫米波雷达的位姿图slam计算方法及系统 Pending CN116359905A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310532630.2A CN116359905A (zh) 2023-05-11 2023-05-11 基于4d毫米波雷达的位姿图slam计算方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310532630.2A CN116359905A (zh) 2023-05-11 2023-05-11 基于4d毫米波雷达的位姿图slam计算方法及系统

Publications (1)

Publication Number Publication Date
CN116359905A true CN116359905A (zh) 2023-06-30

Family

ID=86938103

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310532630.2A Pending CN116359905A (zh) 2023-05-11 2023-05-11 基于4d毫米波雷达的位姿图slam计算方法及系统

Country Status (1)

Country Link
CN (1) CN116359905A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117315268A (zh) * 2023-11-27 2023-12-29 武汉大学 一种基于毫米波雷达的slam方法及系统
CN117590371A (zh) * 2024-01-18 2024-02-23 上海几何伙伴智能驾驶有限公司 基于4d毫米波成像雷达实现全局车位状态检测的方法
CN117593650A (zh) * 2024-01-18 2024-02-23 上海几何伙伴智能驾驶有限公司 基于4d毫米波雷达与sam图像分割的动点滤除视觉slam方法

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117315268A (zh) * 2023-11-27 2023-12-29 武汉大学 一种基于毫米波雷达的slam方法及系统
CN117315268B (zh) * 2023-11-27 2024-02-09 武汉大学 一种基于毫米波雷达的slam方法及系统
CN117590371A (zh) * 2024-01-18 2024-02-23 上海几何伙伴智能驾驶有限公司 基于4d毫米波成像雷达实现全局车位状态检测的方法
CN117593650A (zh) * 2024-01-18 2024-02-23 上海几何伙伴智能驾驶有限公司 基于4d毫米波雷达与sam图像分割的动点滤除视觉slam方法
CN117590371B (zh) * 2024-01-18 2024-03-29 上海几何伙伴智能驾驶有限公司 基于4d毫米波成像雷达实现全局车位状态检测的方法
CN117593650B (zh) * 2024-01-18 2024-04-26 上海几何伙伴智能驾驶有限公司 基于4d毫米波雷达与sam图像分割的动点滤除视觉slam方法

Similar Documents

Publication Publication Date Title
CN111258313B (zh) 多传感器融合slam系统及机器人
CN109993113B (zh) 一种基于rgb-d和imu信息融合的位姿估计方法
Zhao et al. A robust laser-inertial odometry and mapping method for large-scale highway environments
CN111275763B (zh) 闭环检测系统、多传感器融合slam系统及机器人
Lenac et al. Fast planar surface 3D SLAM using LIDAR
CN116359905A (zh) 基于4d毫米波雷达的位姿图slam计算方法及系统
Rawashdeh et al. Collaborative automated driving: A machine learning-based method to enhance the accuracy of shared information
Zhang et al. Multiple vehicle-like target tracking based on the velodyne lidar
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
Qimin et al. A methodology of vehicle speed estimation based on optical flow
CN112945233A (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
CN116608873A (zh) 一种自动驾驶车辆的多传感器融合定位建图方法
CN113554705B (zh) 一种变化场景下的激光雷达鲁棒定位方法
CN115113170A (zh) 一种基于室内特征退化环境的激光雷达边缘特征预测方法
CN111239761B (zh) 一种用于室内实时建立二维地图的方法
CN115355904A (zh) 一种针对地面移动机器人的Lidar-IMU融合的slam方法
Liu et al. A multi-sensor fusion with automatic vision-LiDAR calibration based on Factor graph joint optimization for SLAM
Zhu et al. Camera, LiDAR, and IMU Based Multi-Sensor Fusion SLAM: A Survey
Li et al. Dynamic obstacle tracking based on high-definition map in urban scene
TWI748678B (zh) 基於地面資訊優化的車輛自主定位系統
Grinstead et al. A comparison of pose estimation techniques: Hardware vs. video
CN114323038A (zh) 融合双目视觉和2d激光雷达的室外定位方法
Kramer et al. Radar-Based Localization For Autonomous Ground Vehicles In Suburban Neighborhoods
von Falkenhausen et al. Fusion of raw sensor data for testing applications in autonomous driving
Zuev et al. Mobile system for road inspection and 3D modelling

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