基于4D毫米波雷达的位姿图SLAM计算方法及系统技术方案

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

【技术实现步骤摘要】
International Conference on Robotics and Automation(ICRA).Xi
’
an,China:IEEE,May 2021,pp.14 417
–
14 423.)。他们利用了Kellner等人提出的自车速度估计方法，将多帧雷达点云合并为一个雷达子地图。然后，应用基于正态分布变换的扫描匹配来匹配两个雷达子地图，实现自车运动的估计。然而，雷达子地图只是利用自车速度估计对连续雷达点云的简单叠加，因此其精度在很大程度上取决于自车速度估计的准确性。
[0007]专利文献CN111522043A公开了一种无人车激光雷达快速重新匹配定位方法，属于无人驾驶领域。该专利技术一共分为多传感器标定、融合位姿和融合定位三个模块。通过多传感器联合标定，获得对目标的一致性描述；将GPS传感器和激光雷达传感器对无人车解算的位姿信息进行数据融合，获得GPS传感器和激光雷达传感器融合的车辆位姿信息；当激光雷达SLAM定位模块点云匹配失败时，使用融合位姿来替代点云匹配算法的定位预测矩阵，实现激光雷达SLAM算法的快速重新匹配，实现无人车SLAM算法的持续定位。但该专利技术没有引入回环检测识别重新到达过的地方，不能从全局的角度优化历史位姿。

技术实现思路

[0008]针对现有技术中的缺陷，本专利技术的目的是提供一种基于4D毫米波雷达的位姿图SLAM计算方法及系统。
[0009]根据本专利技术提供的一种基于4D毫米波雷达的位姿图SLAM计算方法，包括：
[0010]步骤S1：提取地面点云，比较连续两帧点云，去除地面下的鬼影点以及随机点；
[0011]步骤S2：利用4D雷达点云的多普勒速度信息，估计出设备的线速度和角速度；
[0012]步骤S3：利用相对位姿变换估计并构建位姿图，基于正态分布变换的点云配准，利用估计的设备速度进行预积分，回环检测，利用图优化估计出最优的位姿。
[0013]优选地，在所述步骤S1中：
[0014]通过提取地面点云以及比较连续两帧点云，去除地面下的鬼影点以及不稳定的随机点，减少4D雷达点云中的噪声；
[0015]步骤S1.1：鬼影点去除，从原始雷达点云中提取出地面点云，滤除地面之下的鬼影点；
[0016]对于4D雷达测量的原始雷达点云n为点云中点的数量；
[0017]其中每个点在雷达坐标系中的位置表示为其中r
i
为4D雷达测量得到的该点的距离，θ
i
为4D雷达测量得到的该点的方位角，为4D雷达测量得到的该点的高度角；
[0018]对于原始的雷达点云，保留距离小于阈值δ
r
且高度在雷达安装高度附近阈值δ
h
内的点，利用主成分分析法，计算出每个点的朝上的法向量，并保留法向量和z轴正向单位向量夹角小于δ
n
的点，利用随机采样一致性算法提取出地面点，并滤除在地面之下的鬼影点；
[0019]步骤S1.2：随机点去除，通过对比前后两帧点云识别和滤除随机点，对于当前点云和紧邻的前一帧点云，通过设备速度计算出前后两帧点云的位姿变换，将前一帧点云变换到当前帧的坐标系下，如果当前帧的某个点附近预设范围内不存在变换后的前一帧点云，则将该点归类为随机点并滤除：
[0020]计算出当前点云和紧邻的前一帧点云之间的旋转变换R
k
‑1和平移变换t
k
‑1：
[0021][0022][0023]其中，和分别是前一帧的设备线速度和角速度估计，Δt是两帧之间的时间差，Exp(
·
):R3→
SO(3)是三维旋转的指数映射，R3是三维实数向量空间，SO(3)是三维特殊正交群，利用R
k
‑1和t
k
‑1将前一帧点云变换到当前帧的坐标系下得到如果当前帧的某个点预设范围内不存在中的点，则将该当前点归类为随机点并滤除。
[0024]优选地，在所述步骤S2中：
[0025]步骤S2.1：静态点提取，对于4D雷达点云中的第i个点，该点的多普勒速度和雷达自身速度之间的关系如下：
[0026]v
r,i
＝
‑
d
i
·
v
s
[0027]其中，v
r,i
是第i个点的多普勒速度，是该点相对于雷达的单位方向向量，v
s
是雷达的速度；
[0028]雷达速度和设备速度的关系为：
[0029][0030]其中，t
s
和R
s
分别为4D雷达在设备坐标系中的安装位置和姿态，和分别为设备线速度和角速度，对于4D雷达点云中的所有静态点，多普勒速度和设备速度之间均符合如下方程：
[0031][0032]其中，m为点云中所有静态点的数量，I3×3为3
×
3大小的单位矩阵，是t
s
的反对称矩阵，R3×3表示3
×
3实矩阵的集合，根据雷达点云中所有静态点的多普勒速度和设备速度之间的关系，利用随机采样一致性算法去除动态的离群点并提取静态点；
[0033]步骤S2.2：最小二乘估计，提取出静态点后，利用所有静态点的多普勒速度和设备速度之间的关系，利用最小二乘法，计算出设备速度。
[0034]优选地，在所述步骤S3中：
[0035]位姿图优化，利用点云配准、速度预积分和回环检测，得到相对位姿变换估计并构建位姿图，最后利用图优化估计出最优的位姿；
[0036]步骤S3.1：点云配准，基于正态分布变换，通过当前4D雷达点云和关键帧子地图的匹配估计相对变换；
[0037]使用滑动窗口利用多个关键帧点云建立雷达子地图，如果从最新的关键帧到当前点云的平移或旋转超过阈值，则选择当前帧作为一个新的关键帧，将新的关键帧添加到雷达子地图中，当子地图中的关键帧数量超过窗口大小时，最早的关键帧被丢弃；
[0038]建立雷达子地图后，将子地图均匀划分为大小相同的网格，将每个网格中的点云建模为局部正态分布，在计算正态分布的均值和协方差时，考虑每个点的测量不确定性，将当前的4D雷达点云与雷达子地图进行配准，计算出使得当前点云在子地图中分布概率最大
的相对位姿变换，该位姿变换估计的误差为e
O
；
[0039]步骤S3.2：速度预积分，利用估计的设备速度，计算出相对位姿变换，引入额外的可靠相对位姿估计，将t时刻估计的设备线速度和角速度记为和估计值为真实值叠加一个零均值的高斯白噪声，即：
[0040][0041][0042]其中，ω
t
和
W
v
t
是世界坐标系下的角速度和线速度真实值，和是对应的噪声项，R
t
是设备在世界坐标系下的朝向，通过积分得到i时刻和j时刻之间的相对旋转变换ΔR
ij
和相对平移变换Δp
ij<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于4D毫米波雷达的位姿图SLAM计算方法，其特征在于，包括：步骤S1：提取地面点云，比较连续两帧点云，去除地面下的鬼影点以及随机点；步骤S2：利用4D雷达点云的多普勒速度信息，估计出设备的线速度和角速度；步骤S3：利用相对位姿变换估计并构建位姿图，基于正态分布变换的点云配准，利用估计的设备速度进行预积分，回环检测，利用图优化估计出最优的位姿。2.根据权利要求1所述的基于4D毫米波雷达的位姿图SLAM计算方法，其特征在于，在所述步骤S1中：通过提取地面点云以及比较连续两帧点云，去除地面下的鬼影点以及不稳定的随机点，减少4D雷达点云中的噪声；步骤S1.1：鬼影点去除，从原始雷达点云中提取出地面点云，滤除地面之下的鬼影点；对于4D雷达测量的原始雷达点云n为点云中点的数量；其中每个点在雷达坐标系中的位置表示为其中r
i
为4D雷达测量得到的该点的距离，θ
i
为4D雷达测量得到的该点的方位角，为4D雷达测量得到的该点的高度角；对于原始的雷达点云，保留距离小于阈值δ
r
且高度在雷达安装高度附近阈值δ
h
内的点，利用主成分分析法，计算出每个点的朝上的法向量，并保留法向量和z轴正向单位向量夹角小于δ
n
的点，利用随机采样一致性算法提取出地面点，并滤除在地面之下的鬼影点；步骤S1.2：随机点去除，通过对比前后两帧点云识别和滤除随机点，对于当前点云和紧邻的前一帧点云，通过设备速度计算出前后两帧点云的位姿变换，将前一帧点云变换到当前帧的坐标系下，如果当前帧的某个点附近预设范围内不存在变换后的前一帧点云，则将该点归类为随机点并滤除：计算出当前点云和紧邻的前一帧点云之间的旋转变换R
k
‑1和平移变换t
k
‑1：：其中，和分别是前一帧的设备线速度和角速度估计，Δt是两帧之间的时间差，Exp(
·
):3→
SO(3)是三维旋转的指数映射，R3是三维实数向量空间，SO(3)是三维特殊正交群，利用R
k
‑1和t
k
‑1将前一帧点云变换到当前帧的坐标系下得到如果当前帧的某个点预设范围内不存在中的点，则将该当前点归类为随机点并滤除。3.根据权利要求1所述的基于4D毫米波雷达的位姿图SLAM计算方法，其特征在于，在所述步骤S2中：步骤S2.1：静态点提取，对于4D雷达点云中的第i个点，该点的多普勒速度和雷达自身速度之间的关系如下：v
r,i
＝
‑
d
i
·
v
s
其中，v
r,i
是第i个点的多普勒速度，是该点相对于雷达的单位方向向量，v
s
是雷达的速度；雷达速度和设备速度的关系为：
其中，t
s
和R
s
分别为4D雷达在设备坐标系中的安装位置和姿态，和分别为设备线速度和角速度，对于4D雷达点云中的所有静态点，多普勒速度和设备速度之间均符合如下方程：其中，m为点云中所有静态点的数量，I3×3为3
×
3大小的单位矩阵，是t
s
的反对称矩阵，R3×3表示3
×
3实矩阵的集合，根据雷达点云中所有静态点的多普勒速度和设备速度之间的关系，利用随机采样一致性算法去除动态的离群点并提取静态点；步骤S2.2：最小二乘估计，提取出静态点后，利用所有静态点的多普勒速度和设备速度之间的关系，利用最小二乘法，计算出设备速度。4.根据权利要求1所述的基于4D毫米波雷达的位姿图SLAM计算方法，其特征在于，在所述步骤S3中：位姿图优化，利用点云配准、速度预积分和回环检测，得到相对位姿变换估计并构建位姿图，最后利用图优化估计出最优的位姿；步骤S3.1：点云配准，基于正态分布变换，通过当前4D雷达点云和关键帧子地图的匹配估计相对变换；使用滑动窗口利用多个关键帧点云建立雷达子地图，如果从最新的关键帧到当前点云的平移或旋转超过阈值，则选择当前帧作为一个新的关键帧，将新的关键帧添加到雷达子地图中，当子地图中的关键帧数量超过窗口大小时，最早的关键帧被丢弃；建立雷达子地图后，将子地图均匀划分为大小相同的网格，将每个网格中的点云建模为局部正态分布，在计算正态分布的均值和协方差时，考虑每个点的测量不确定性，将当前的4D雷达点云与雷达子地图进行配准，计算出使得当前点云在子地图中分布概率最大的相对位姿变换，该位姿变换估计的误差为e
O
；步骤S3.2：速度预积分，利用估计的设备速度，计算出相对位姿变换，引入额外的可靠相对位姿估计，将t时刻估计的设备线速度和角速度记为和估计值为真实值叠加一个零均值的高斯白噪声，即：零均值的高斯白噪声，即：其中，ω
t
和
W
v
t
是世界坐标系下的角速度和线速度真实值，和是对应的噪声项，R
t
是设备在世界坐标系下的朝向，通过积分得到i时刻和j时刻之间的相对旋转变换ΔR
ij
和相对平移变换Δp
ij
，该位姿变换估计的误差为e
V
。5.根据权利要求4所述的基于4D毫米波雷达的位姿图SLAM计算方法，其特征在于：步骤S3.3：回环检测，识别之前到达过的位置，在极坐标上，将4D雷达点云按照网格进行划分，将3D点云映射为一个2维矩阵，矩阵中每个元素的值为对应的网格中雷达点的最大能量强度，随着设备的运动，不断搜索和计算当前帧的2维矩阵和之前所有关键帧生成的2维矩阵的余弦距离，距离小于设定阈值时认为检测到回环；当检测到一个回环，计算当前帧和由回环帧以及附近关键帧组成的子地图之间的相对位姿变换；该位姿变换估计的误差为
e
L
；步骤S3.4：图优化，建立位姿图，考虑所有时刻相对位姿估计的误差项，通过图优化估计出所有时刻设备的最优位姿，位姿中所有的节点对应不同时刻的位姿，节点间的边对应不同时刻之间的相对位姿变换；将所有位姿记为通过解非线性最小二乘问题估计最优位姿题估计最优位姿其中，包含所有的时刻，e
O
,e
V
,e
L
对应不同步骤获得的相对位姿变换...

【专利技术属性】
技术研发人员：张晗，李星熠，陈卫东，王景川，刘昌赫，
申请(专利权)人：采埃孚中国投资有限公司，
类型：发明
国别省市：