CN115220041A

CN115220041A - 一种带有多普勒补偿的毫米波雷达尺度定位方法和系统

Info

Publication number: CN115220041A
Application number: CN202210703470.9A
Authority: CN
Inventors: 王巍; 高鹏恩
Original assignee: Huazhong University of Science and Technology
Current assignee: Huazhong University of Science and Technology
Priority date: 2022-06-21
Filing date: 2022-06-21
Publication date: 2022-10-21

Abstract

一种带有多普勒补偿的毫米波雷达尺度定位方法，包括：对毫米波雷达点云进行多普勒畸变补偿；采用RANSAC算法对毫米波雷达目标点进行动态目标去除,通过叠加相邻时刻的雷达点云的方法增加点云密度；根据极坐标系下的雷达目标的距离、速度以及角度的测量误差,估计笛卡尔坐标系下毫米波雷达点云的不确定度分布；根据步骤得到的预处理后点云以及目标点的不确定估计，采用基于RSD描述子的特征点匹配算法关联雷达数据，并根据关联结果采用极大似然估计算法计算当前雷达坐标系相对于关键帧的位姿变换关系，得到当前时刻雷达的尺度定位结果。本发明有效提高了毫米波雷达点云配准的成功率，降低误差较大测量值的影响。

Description

一种带有多普勒补偿的毫米波雷达尺度定位方法和系统

技术领域

本发明涉及的是同步定位与建图领域，特别涉及一种基于多普勒补偿的毫米波雷达尺度定位方法和系统。

背景技术

尺度定位是自动驾驶车辆实现自主定位导航的关键技术之一，该技术可利用历史关键帧的传感器数据，并结合当前车辆的传感器测量值，估计当前时刻自身的全局位姿信息。该技术可以有效降低车辆里程计由于累计误差带来的定位漂移问题。

现有的尺度定位技术大多基于激光雷达或者相机数据实现，然而这些传感器容易受到天气条件(如雨、雾、雪)以及光照条件的影响，无法在全天候条件下稳定工作。相比于上述传感器，毫米波雷达具有更长的发射波长，因此穿透能力更强，可以适应绝大部分的天气状况。这使得它成为了一种非常理想的定位导航传感器。同时随着近年来基于FMCW技术(调频连续波)的毫米波雷达在硬件以及生产工艺上的不断发展，毫米波雷达在尺寸、价格及精度上都体现出了其特有的优势。使用毫米波雷达进行尺度定位已逐渐成为了学术界以及工业界在定位导航领域的研究热点之一。

现有的毫米波雷达尺度定位工作虽然可以取得不错的定位效果，但是这些工作大多都低估了多普勒效应在定位过程中的影响。当搭载有毫米波雷达的车辆高速运动时，多普勒效应会改变FMCW毫米波雷达的原始测量信息，使得雷达的测量结果变得不再准确，具体体现在多普勒频率会改变雷达反射点的测距信息，从而造成雷达扫描图的多普勒畸变。该畸变所造成的影响在FMCW雷达尺度定位应用中尤为明显。这是因为我们无法保证传感器在采集两帧雷达扫描图时速度一致，这将导致我们在后续点云配准的过程中无法正确关联两帧雷达点云，进一步使得尺度定位出现明显的误差。

发明内容

鉴于上述问题，提出了本发明以便提供一种克服上述问题或者至少部分地解决上述问题的一种带有多普勒补偿的毫米波雷达尺度定位方法和系统。

为了解决上述技术问题，本申请实施例公开了如下技术方案：

一种带有多普勒补偿的毫米波雷达尺度定位方法，其特征在于，包括：

S100.对毫米波雷达点云进行多普勒畸变补偿，其中,畸变补偿值由毫米波雷达目标点的测量速度以及雷达参数确定；

S200.采用RANSAC算法对毫米波雷达目标点进行动态目标去除,通过叠加相邻时刻的雷达点云的方法增加点云密度；采用均值漂移算法聚合叠帧后的雷达点云，降低毫米波雷达测量误差的影响；

S300.根据极坐标系下的雷达目标的距离、速度以及角度的测量误差,估计笛卡尔坐标系下毫米波雷达点云的不确定度分布；

S400.根据步骤S100-S300中得到的预处理后点云以及目标点的不确定估计，采用基于RSD描述子的特征点匹配算法关联雷达数据，并根据关联结果采用极大似然估计算法计算当前雷达坐标系相对于关键帧的位姿变换关系，进一步得到当前时刻雷达的尺度定位结果。

进一步地，S100的具体方法包括：

S101.根据毫米波雷达的信号发射频率f_c和每个线性调频信号的斜率K，计算毫米波雷达的多普勒系数β,其中，

S102.根据雷达目标速度测量值v以及S101中得到的多普勒系数β，计算相应的多普勒补偿值r_d＝βv，其中v是雷达目标的径向测量速度；

S103.根据S102中计算得到的多普勒补偿值，恢复原始激光雷达目标点测量值

其中

为带有多普勒畸变的目标距离测量值，r为恢复后的雷达目标距离。

进一步地，毫米波雷达的每个线性调频信号的斜率K的计算公式为：

其中，B和T分别为线性调频信号的带宽和持续时间。

进一步地，S200中，采用RANSAC算法对毫米波雷达目标点进行动态目标去除的具体方法包括：获取雷达目标的径向速度v_r、目标的角度θ，汽车的运动速度v_s以及行驶角度α，当目标点的径向速度v_r满足公式v_r＝-v_scos(α-θ)时，将当前目标点标记为静止雷达目标，将不符合公式v_r＝-v_scos(α-θ)的点作为动态雷达目标并进行去除。

进一步地，S200中，通过叠加相邻时刻的雷达点云的方法增加点云密度的方法包括：设当前时刻为t，则叠加雷达点云之后的t时刻点云

表示为：

其中,P_t-i(i＝1,2,…K)表示i-K时刻的测量点云，

表示t-i时刻的雷达相对于t时刻的坐标转换矩阵，

表示旋转矩阵，

表示平移向量。

进一步地，S300的具体方法包括：

S301.将极坐标系下的目标位置转化到笛卡尔坐标系下，并将测量误差转化为笛卡尔系下的表示形式；

S302.根据S103补偿之后的目标距离，计算笛卡尔坐标系下目标位置相对于测量误差的雅各比矩阵，并计算目标点的协方差矩阵。

进一步地，S400的具体方法包括：

S401.采用预设方法，得到每个雷达目标点的相对标准偏差描述子；

S402.采用K最近邻算法，得到当前雷达点云相对于关键帧雷达点云的对应关系；

403.采用随机采样一致性算法，通过随机采样并不断迭代去除S402中错误的匹配关系；

S404.采用极大似然估计算法计算当前雷达坐标系相对于关键帧的姿态变换关系，并利用关键帧的全局位姿信息得到当前时刻雷达的尺度定位结果。

进一步地，S401中，采用预设方法，得到每个雷达目标点的相对标准偏差描述子的预设方法为：

以雷达目标点o_i为圆心，统计不同半径r_j(j＝1,2,…m)上的雷达目标点数量n_j(j＝1,2,…m)，将统计结果写成向量形式(n₁,n₂,…,n_m)^T，并进行归一化得到目标点o_i的描述子d_i。

本发明还公开了一种带有多普勒补偿的毫米波雷达尺度定位系统，包括：雷达点云多普勒畸变补偿计算模块、动态目标去除及点云增强模块、雷达目标点的不确定度估计模块和雷达的尺度定位结果计算模块；其中：

雷达点云多普勒畸变补偿计算模块，用于对毫米波雷达点云进行多普勒畸变补偿，其中,畸变补偿值由毫米波雷达目标点的测量速度以及雷达参数确定；

动态目标去除及点云增强模块，用于采用RANSAC算法对毫米波雷达目标点进行动态目标去除,通过叠加相邻时刻的雷达点云的方法增加点云密度；采用均值漂移算法聚合叠帧后的雷达点云，降低毫米波雷达测量误差的影响；

雷达目标点的不确定度估计模块，用于根据极坐标系下的雷达目标的距离、速度以及角度的测量误差,估计笛卡尔坐标系下毫米波雷达点云的不确定度分布；

雷达的尺度定位结果计算模块，用于根据雷达点云多普勒畸变补偿计算模块、动态目标去除及点云增强模块和雷达目标点的不确定度估计模块得到的预处理后点云以及目标点的不确定估计，采用基于RSD描述子的特征点匹配算法关联雷达数据，并根据关联结果采用极大似然估计算法计算当前雷达坐标系相对于关键帧的位姿变换关系，进一步得到当前时刻雷达的尺度定位结果。

本发明还公开了一种电子设备，包括：

存储器，用于存储可由处理器执行的指令；

处理器，配置为执行所述指令以实现所述的一种带有多普勒补偿的毫米波雷达尺度定位方法。

本发明实施例提供的上述技术方案的有益效果至少包括：

本发明补偿了多普勒效应带来的距离偏移，减少后续点云配准过程中由于不一致性造成的误匹配现象，实验证明本方法有效提高了毫米波雷达点云配准的成功率。此外，本发明提出了带有多普勒补偿的毫米波雷达目标点不确定度估计方法，该方法考虑了加入多普勒补偿后的目标点不确定度分布。该不确定度估计结果可以为每个目标点在后续的极大似然估计中赋予相应的权值，从而降低误差较大测量值的影响。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：

图1为本发明实施例1中，一种带有多普勒补偿的毫米波雷达尺度定位方法的流程图；

图2为本发明实施例1中，雷达点云多普勒补偿及叠帧效果图；

图3为本发明实施例1中，本发明实施例与其他毫米波雷达尺度定位算法性能对比图。

具体实施方式

下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。

为了解决现有技术中存在的问题，本发明实施例提供一种带有多普勒补偿的毫米波雷达尺度定位方法和系统。

实施例1

一种带有多普勒补偿的毫米波雷达尺度定位方法，如图1，包括：

具体的，本发明的毫米波雷达采用FMCW毫米波雷达，毫米波雷达是工作在毫米波波段探测的雷达，车载毫米波雷达的工作频段为77GHz-81GHz，波长在4mm左右。介于微波和厘米波之间，兼具有微波雷达和光电雷达的一些优点。毫米波雷达基本工作原理是利用高频电路产生特定调制频率(FMCW)的电磁波，并通过天线发送电磁波和接收从目标反射回来的电磁波，可以同时对多个目标进行测距、测速以及方位角测量；测速是根据多普勒效应，通过计算返回接收天线的雷达波的频率变化就可以得到目标相对于雷达的运动速度，和飞行时间，来得到目标物距离。而方位测量(包括水平角度和垂直角度)是通过天线的阵列收到同一目标反射的雷达波的相位差计算得到目标的方位角。

在本实施例中，S100的具体方法包括：

其中

具体地，FMCW毫米波雷达的目标距离r以及速度v的测量值表示为：

其中，f_b为测量中间信号的频率，f_v表示目标点测量相位的变化频率，λ表示信号波长，c为光传播速度，T_c表示每两个chirp信号的间隔时间，K表示chirp信号的斜率(B为chirp信号带宽，T_r为chirp信号的持续时间)。

当毫米波雷达在测量过程中受到多普勒效应干扰时，雷达的中间频率会在原先的测量值f_b的基础上叠加一个多普勒频率

此时距离的测量值变为：

为了消除多普勒效应的影响，需要对原有的距离测量值

补偿由于多普勒效应带来的多普勒偏移r_d，此时的补偿后的雷达测距结果表示为

值得说明的是，雷达的速度测量值由中间频率的相位变化得出，因此不受多普勒效应的影响，使用上述方法理论上可以完全消除多普勒偏移。通过对所有的雷达目标点使用上述补偿方法，我们进一步得到带有多普勒补偿的雷达点云P：

P＝{p_i|i＝1,2,…N}

其中，θ_i,φ_i分别为第i个目标点的水平角和俯仰角测量值，N为点云中的目标点总数。

在本实施例中，S200中，采用RANSAC算法对毫米波雷达目标点进行动态目标去除的具体方法包括：获取雷达目标的径向速度v_r、目标的角度θ，汽车的运动速度v_s以及行驶角度α，当目标点的径向速度v_r满足公式v_r＝-v_scos(α-θ)时，将当前目标点标记为静止雷达目标，将不符合公式v_r＝-v_scos(α-θ)的点作为动态雷达目标并进行去除。通过随机采样一致性算法，我们可以得到v_s，α的大致估计，同时将离群点视为运动物体和噪声从而去除。

为应对毫米波雷达点云稀疏以及测量误差大的问题，我们采用了叠加相邻K时刻点云的方法，为当前雷达帧的提供更多的信息，具体而言，设当前时刻为t，则叠加雷达点云之后的t时刻点云

表示为：

其中,P_t-i(i＝1,2,…K)表示i-K时刻的测量点云，

表示t-i时刻的雷达相对于t时刻的坐标转换矩阵，

表示旋转矩阵，

表示平移向量。

我们随后对叠帧后的点云

使用均值漂移算法进一步降低雷达测量误差。图2展示了雷达点云增强的效果，从图中不难看出，叠帧后的雷达点云密度明显增加，当雷达相对于目标点移动时，多普勒效应会缩小真实测量距离，反之则放大。使用本发明提供的方法可以有效应对多普勒偏移以及雷达的低分辨率和测量误差

本实施S300的具体方法包括：

具体而言，目标点的距离r、速度v和角度θ,φ在极坐标系下的测量误差写为向量形式为n_m＝[n_r,n_v,n_θ,n_φ]^T，设

其中Σ_m为测量值的协方差。此时该目标点在笛卡尔坐标系下的坐标可以写为：

p＝f(r,v,θ,φ,n_m)≈f(r,v,θ,φ)+Jn_m

其中J为转化过后的目标点关于误差项的雅各比矩阵：

其中，

定义为多普勒系数，

表示补偿之后的距离测量值。则带有多普勒补偿的目标点在笛卡尔坐标系下的不确定度分布写为：

Σ_c＝E[(p-f(r,v,θ,φ))(p-f(r,v,θ,φ))^T]≈JΣ_mJ^T

采用本方法获取的点云不确定度，可以得到距离雷达较远的目标点具有更大的不确定度，这符合我们先前所做出的预期。在随后的点云配准过程中，系统会赋予不确定度小的点更高的权值，从而减小低质量测量点对定位结果的影响。

本实施例的S400具体方法包括：

具体的，S401中，采用预设方法，得到每个雷达目标点的相对标准偏差描述子的预设方法为：

具体而言，本实施例采用了一种基于特征点法的点云配准算法用于计算采集两帧点云时雷达的相对位姿。首先，我们计算了每个雷达目标点的RSD(相对标准偏差)描述子。

图3展示了多普勒补偿前后点云关联的效果对比图，其中绿点表示当前匹配成功的雷达目标点，红点为匹配成功的关键帧雷达目标点，红点和绿点之间的白线为对应关系，白点表示没有寻找到有效匹配的雷达当前雷达目标点。从图中不难看出，本发明所提出的多普勒补偿算法可以有效提高雷达目标点匹配的成功率。

最后，本发明采用极大似然估计法求取当前雷达关于关键帧的相对位姿变换矩阵

其中y_i∈P_k,x_i∈P_b为采用K-最近临算法得到的当前雷达点云P_b和关键帧雷达点云P_k的第i对匹配目标点。本发明采用了列文伯格-马夸尔特算法求解该优化问题，并采用了柯西鲁棒核函数进一步降低误匹配及测量误差较大点的影响。

由关键帧相对于世界坐标系的位姿

以及求得的

最终本方法可以得到当前时刻的雷达在世界坐标系下的尺度定位结果

图3显示了本方法与其他毫米波雷达尺度定位算法的定位结果，用于对比的数据为毫米波雷达尺度定位的平移和旋转误差均值。其中MC-RANSAC算法为目前的最优方法。可以看出，随着当前雷达自身速度和采集关键帧时雷达速度差值的上升，用于对比的方法的尺度定位误差也在不断增大，这说明多普勒效应会显著影响高速运动情况下的雷达尺度定位精度。相比于这些方法，本发明所提出方法达到了最低的平均平移误差以及接近目前最优方法的旋转误差。该结果验证了本发明所提出算法的有效性。

实施例2

本实施公开了一种带有多普勒补偿的毫米波雷达尺度定位系统，包括：雷达点云多普勒畸变补偿计算模块、动态目标去除及点云增强模块、雷达目标点的不确定度估计模块和雷达的尺度定位结果计算模块；其中：

在本实施例中，雷达点云多普勒畸变补偿计算模块、动态目标去除及点云增强模块、雷达目标点的不确定度估计模块和雷达的尺度定位结果计算模块具体工作的方法已在实施例1中进行了详细描述，因此，本实施例再此不在进行赘述。

实施例3

本实施例公开了一种电子设备，包括：

存储器，用于存储可由处理器执行的指令；

处理器，配置为执行所述指令以实现如实施例1所述的一种带有多普勒补偿的毫米波雷达尺度定位方法。

应该明白，公开的过程中的步骤的特定顺序或层次是示例性方法的实例。基于设计偏好，应该理解，过程中的步骤的特定顺序或层次可以在不脱离本公开的保护范围的情况下得到重新安排。所附的方法权利要求以示例性的顺序给出了各种步骤的要素，并且不是要限于所述的特定顺序或层次。

在上述的详细描述中，各种特征一起组合在单个的实施方案中，以简化本公开。不应该将这种公开方法解释为反映了这样的意图，即，所要求保护的主题的实施方案需要清楚地在每个权利要求中所陈述的特征更多的特征。相反，如所附的权利要求书所反映的那样，本发明处于比所公开的单个实施方案的全部特征少的状态。因此，所附的权利要求书特此清楚地被并入详细描述中，其中每项权利要求独自作为本发明单独的优选实施方案。

本领域技术人员还应当理解，结合本文的实施例描述的各种说明性的逻辑框、模块、电路和算法步骤均可以实现成电子硬件、计算机软件或其组合。为了清楚地说明硬件和软件之间的可交换性，上面对各种说明性的部件、框、模块、电路和步骤均围绕其功能进行了一般地描述。至于这种功能是实现成硬件还是实现成软件，取决于特定的应用和对整个系统所施加的设计约束条件。熟练的技术人员可以针对每个特定应用，以变通的方式实现所描述的功能，但是，这种实现决策不应解释为背离本公开的保护范围。

结合本文的实施例所描述的方法或者算法的步骤可直接体现为硬件、由处理器执行的软件模块或其组合。软件模块可以位于RAM存储器、闪存、ROM存储器、EPROM存储器、EEPROM存储器、寄存器、硬盘、移动磁盘、CD-ROM或者本领域熟知的任何其它形式的存储介质中。一种示例性的存储介质连接至处理器，从而使处理器能够从该存储介质读取信息，且可向该存储介质写入信息。当然，存储介质也可以是处理器的组成部分。处理器和存储介质可以位于ASIC中。该ASIC可以位于用户终端中。当然，处理器和存储介质也可以作为分立组件存在于用户终端中。

对于软件实现，本申请中描述的技术可用执行本申请所述功能的模块(例如，过程、函数等)来实现。这些软件代码可以存储在存储器单元并由处理器执行。存储器单元可以实现在处理器内，也可以实现在处理器外，在后一种情况下，它经由各种手段以通信方式耦合到处理器，这些都是本领域中所公知的。

上文的描述包括一个或多个实施例的举例。当然，为了描述上述实施例而描述部件或方法的所有可能的结合是不可能的，但是本领域普通技术人员应该认识到，各个实施例可以做进一步的组合和排列。因此，本文中描述的实施例旨在涵盖落入所附权利要求书的保护范围内的所有这样的改变、修改和变型。此外，就说明书或权利要求书中使用的术语“包含”，该词的涵盖方式类似于术语“包括”，就如同“包括，”在权利要求中用作衔接词所解释的那样。此外，使用在权利要求书的说明书中的任何一个术语“或者”是要表示“非排它性的或者”。