CN114325682A

CN114325682A - 基于车载4d毫米波雷达的车辆速度状态估计方法

Info

Publication number: CN114325682A
Application number: CN202110477277.3A
Authority: CN
Inventors: 张煜东; 陈禹行; 范圣印; 金凌鸽
Original assignee: Beijing Yihang Yuanzhi Technology Co Ltd
Current assignee: Beijing Yihang Yuanzhi Technology Co Ltd
Priority date: 2021-04-29
Filing date: 2021-04-29
Publication date: 2022-04-12

Abstract

一种基于车载4D毫米波雷达的车辆速度状态估计方法，包括：1、实时读取4D毫米波雷达采集的目标数据；2、对所述目标数据进行转换，目标聚类得到目标簇及其属性，如果数据为第一帧则进行初始化，并结束本次计算；3、使用上一帧估计结果对当前帧目标簇进行匹配度计算，结合目标簇高度置信度得到目标簇置信度；4、建立速度状态与目标簇观测方程，用车辆速度状态和当前帧目标簇建立优化方程并求解；5、对估计的车辆速度状态进行评价，判断是否满足输出条件，如果满足则输出车辆速度状态的估计信息，不满足则降低目标簇高度置信度并返回到步骤4；实现了即使在多动态目标场景下，也能取得良好的速度估计的效果。

Description

基于车载4D毫米波雷达的车辆速度状态估计方法

技术领域

本公开涉及自动驾驶技术领域，具体涉及自动驾驶车辆自车速度估计技术，尤其涉及基于车载4D毫米波雷达的自车速度状态估计方法、装置、电子设备及存储介质。

背景技术

测量感知技术的不断进步，给自动驾驶技术的进一步发展提供了基础。自动驾驶车辆自身的运行状态，尤其是其自车速度，是自动驾驶车辆自主运行时必不可少的重要控制参量。而实时、准确地实现对自动驾驶车辆的自车速度状态估计是提高驾驶安全性和舒适性的必要前提。

目前自动驾驶领域中，多动态目标的场景是自车速度估计面临的挑战，严重影响自车速度估计的准确性。而多动态目标的场景又是实际自动驾驶过程中普遍存在的场景。在此场景下，动态目标将成为自车速度估计的干扰因素，严重影响自车速度估计的精度，甚至导致估计失效。虽然现有的技术方法有针对解决动态目标干扰问题的策略，在动态目标较少的场景具有一定鲁棒性，但对处理多动态目标场景十分不理想，尤其不适用于如行人车辆较多的开放道路场景和道路对向大型车辆较多的场景。因此，在多动态目标的场景下，如何实时、精确、可靠地提供自车速度参数成为亟待解决的技术问题。

现阶段，车载毫米波雷达的使用越来越广泛，很多量产的民用乘用车都搭载了毫米波雷达传感器。毫米波雷达也从传统3D毫米波雷达(提供目标方位角度、速度、距离)，发展到了4D毫米波雷达(提供目标俯仰角度、方位角度、速度、距离)。

然而，现有的毫米波雷达车辆速度状态估计技术主要使用安装在车辆正前方的2D车载毫米波雷达，利用对目标的多普勒速度测量，计算自身车辆相对地面的速度。与现有的SLAM(simultaneous localization and mapping)技术相比较，车辆速度状态估计相当于是一种提供速度里程计的方法。

为了解现有技术的发展状况，本公开对已有的专利文献和论文进行了检索、比较和分析：

方案1：专利文献CN 111308458 A(“一种基于车载毫米波雷达的自车速估计方法”)公开了一项发明技术，该发明公开了一种基于车载毫米波雷达的自车速度估计方法。该方法要求雷达安装于待测车辆前向水平位置，雷达主发射波束方向与待测车辆中轴线方向存在己知固定安装角度。通过车载毫米波雷达获得的目标速度对目标进行目标簇分类，将速度相同的目标分为一簇。按照每簇中的目标数量来估计目标簇置信度，数量越大置信度越高。如果有置信度高于阈值(目标簇目标数量高于某个值)的目标簇，则将目标簇的速度作为自车速度估计结果。然而，该方法在多动态目标场景中不可避免地会失效，例如开放道路上对向车道行驶的大型车辆，大型目标将带来数量较多的雷达观测目标，该方法会错误地将大型货车的速度作为自车速输出，从而得出错误结果。虽然采用了滑动窗口对每一帧的结果进行滤波，但误差已经被计算进去了，而且随着计算的增加，误差也会累计增大。此外，该方法的速度估计只有车辆行驶方向的线速度，没有角速度估计。

方案2：文献“Joint Spatial-and Doppler-based Ego-Motion Estimation forAutomotive Radars”介绍了一种基于车载毫米波雷达的自车速度估计方法，主要使用混合高斯(mixture-gaussian)来对毫米波雷达目标的空间位置信息和速度信息进行联合估计，利用非线性优化方法得出最佳结果。由于该技术方案对于动态目标没有进行充分过滤和剔除，将动态目标直接带入算法中，而混合高斯无法对动态目标进行特殊处理，在多动态目标环境中算法的结果必然会受到动态目标的影响，使计算精度下降。在面对多动态目标场景时，同样会面临技术1中出现的错误结果。

方案3：文献“Ego-Motion Estimation using Distributed Single-ChannelRadar Sensors”使用了多颗车载毫米波雷达，通过同一目标在不同雷达中的位置判断，剔除错误目标，过滤后的目标使用RANSAC过滤Outlier(离群目标)，再通过最小二乘非线性回归迭代计算，最终得到车辆水平运动速度和旋转速度的估计。该技术方案依赖RANSAC算法，RANSAC的迭代次数是固定的，而且得出正确结果是概率性的，所以在不同的多动态目标场景下做不到可靠性一致，会产生错误的结果。最小二乘线性回归计算对Outlier(离群目标)非常敏感，在复杂场景中如果RANSAC失效将直接导致最小二乘线性回归失效，最终得到错误的结果。所以该技术方案在多动态目标的场景中无法输出精确的车辆状态估计。

综上，方案1在多动态目标且动目标速度较一致的场景下必然会失效；方案2对动态目标没有很好地进行区分，而是使用混合高斯的计算过程来缓解动态目标所造成的影响，在动态目标所占比例较高的场景，算法将无法提供准确的估计；方案3使用了RANSAC算法，属于使用异常数据排除算法来解决动态目标的影响，但此类算法目前都不具备自适应性，且具有较高的结果随机性，最终导致算法鲁棒性不高。

可见，速度状态估计的鲁棒性主要在于解决动态目标问题，现有技术通过滤波技术或非线性回归算法剔除雷达观测数据中的动态目标，或减小动态目标对计算结果的影响；当车辆所处环境全部为静态物体时，现有技术能够得到较为准确的车辆速度估计；即在动态目标较少的环境，现有技术可以对动态目标具有一定的鲁棒性，在动态目标不多时可以得到准确的车辆速度估计；但是，对于以动态目标为主的多动态目标场景，现有技术则无法完全消除动态目标对结果的影响，动态目标将造成速度估计误差过大，甚至估计失效。

现有技术目前仍无法满足自动驾驶车辆在多动态目标场景下的自车速度状态估计，因此，如何提供一种适应于多动态目标场景的自车速度状态估计方法，在遇到交通复杂的多动态目标环境中仍然可以给出准确结果；在遇到道路对向车道多辆大型车辆等速行驶的场景中，有效减少大型车辆产生的大量动态目标数据对结果的影响，实时准确可靠地得到自车速度，成为当前非常有现实意义的研究内容。

发明内容

为达到上述发明目的，本公开在如下几个方面进行了改进：

使用安装在车辆周围的一颗或多颗4D毫米波雷达，通过目标簇识别、目标簇匹配、高度置信度计算、基于目标簇的多普勒估计等过程，有效解决多动态目标场景中速度估计不准确的问题，适应从多静态目标到多动态目标的各种实际场景，具有较高实时性、自适应性和鲁棒性；

针对大多数基于毫米波雷达目标的自车速度估计方法只限于单个毫米波雷达并且对雷达安装位置要求严格的技术问题，采用将毫米波雷达的目标转换到车辆坐标系下统一处理的方式，有效解决雷达数量受限的问题，故可以使用任意数量的毫米波雷达，由于将目标转换到车辆坐标系下统一处理，因此对雷达安装方位角度没有要求，通用性更强。

针对检测过程中多动态目标造成的干扰较大问题，常规的滤波算法和常见的异常数据剔除方法(例如RANSAC)无法有效剔除干扰的技术问题，采用历史估计结果预测当前目标簇的分布，并同当前帧目标簇的实际分布进行匹配计算；目标簇的匹配度与目标簇内的目标本身数量无关，只与位置匹配度和两帧中同一目标簇的数量变化有关的方法，有效区分大型动态目标形成的目标簇(例如对向行驶的大型货车)，保证了速度估计结果的准确可靠，大大提高了自动驾驶的安全性。由于采用历史估计结果预测当前目标簇的分布，并同当前帧目标簇的实际分布进行匹配计算，可以不依赖固定参数的设置(对比例如RANSAC)，因此使用范围更广泛，适用于多动态目标场景，且具有较高鲁棒性。

针对自动驾驶过程中环境场景复杂多变，导致速度估算结果偏差较大的问题，采用目标在三维空间中的高度置信度，在算法无法得到足够可靠的结果后，会根据大部分行人和车辆所处距地面的高度，降低该高度范围内的空间置信度，从而大概率的降低车辆在常用场景中的动态目标的影响，提高速度估计的准确性和实时性。对比现有的技术方案，本公开更具自适应性。

具体地，为了解决上述技术问题，本公开提供了一种基于车载4D毫米波雷达的车辆速度状态估计方法、装置、电子设备及存储介质。

根据本公开的一个方面，本公开提供了一种基于车载4D毫米波雷达的车辆速度状态估计方法，包括如下步骤：

步骤一、实时读取4D毫米波雷达采集的目标数据，所述目标数据包括距离、方向角度、俯仰角度和径向多普勒速度；

步骤二、对所述目标数据进行转换，目标聚类得到目标簇及其属性，如果数据为第一帧则进行初始化，并结束本次计算；

步骤三、使用上一帧估计结果对当前帧目标簇进行匹配度计算，结合目标簇高度置信度得到目标簇置信度；

步骤四、建立速度状态与目标簇观测方程，用车辆速度状态和当前帧目标簇建立优化方程并求解；

步骤五、对估计的车辆速度状态进行评价，判断是否满足输出条件，如果满足则输出车辆速度状态的估计信息，不满足则降低目标簇高度置信度并返回到步骤四。

优选地，所述4D毫米波雷达的数量为一颗或多颗。

优选地，设定：

所述4D毫米波雷达的个数为：N_r；

第j颗雷达内的目标簇个数为：N_j；

两帧之间时间间隔为：τ；

车辆的速度状态以车辆坐标系为基准；

所述速度状态包括：车辆行驶平面内沿行驶方向的纵向线速度V_x，与V_x垂直的横向线速度V_y，车辆转向过程中产生的绕车辆坐标系的旋转角速度ω，车辆坐标系的横向速度为0，则待估计的速度状态表示为：

其中S_t为第t帧的车辆速度状态，

为第t帧的纵向线速度，ω^t为第t帧的旋转角速度；

所述一颗或多颗4D毫米波雷达水平安装在车身周围观测车辆周围环境，所述目标数据是相对于雷达坐标系的。

优选地，如果所述雷达的数量为多颗，则对多颗4D毫米波雷达进行数据时间同步，保证每一帧数据包含所有雷达同一时刻附近的所有数据，且每帧数据以稳定频率输出。

优选地，车辆启动后进行系统初始化，此时车辆处于静止状态，系统上电并运行程序后收到雷达第一帧数据，此时车辆的速度状态各项为零，即S₀{v_x＝0,ω＝0}。

优选地，将每颗雷达的目标数据从雷达坐标系转换到车辆坐标系；

第j颗雷达中的每个目标在车辆坐标系下的三维空间坐标

表示如下：

得到车辆坐标系下的每颗雷达的目标；

其中，

为第j颗雷达的第i个目标的距离，

为第j颗雷达的第i个目标的水平角度，为

为为第j颗雷达的第i个目标的俯仰角度，

为第j颗雷达的第i个目标的多普勒速度；

为第j颗雷达相对车辆坐标系的安装外参，其中

分别为第j颗雷达坐标系相对车辆坐标系在x,y,z轴上的位置，其中

分别为第j颗雷达相对车辆坐标系绕z轴、y轴、x轴的角度；当雷达为水平安装时，其中

均为0，则安装外参简化为

优选地，使用DBSCAN(Density-based spatial clustering of applicationswith noise)算法对每颗雷达的目标点

进行聚类计算，使用kd-tree方法来加快三维空间的距离搜索，提高计算效率；通过DBSCAN算法得到目标的核心点、核心可达点和离群点；将每个核心点及其核心可达点作为一个目标簇，核心点的坐标即为目标簇坐标，表示为

每个离群点单独作为一个目标簇，离群点坐标即为目标簇坐标；最终得到车辆坐标系下每颗雷达的目标簇表示为如下形式:

其中

为第j颗雷达的第k个目标簇；N_r为雷达的个数，N_j为第j颗雷达的目标簇个数；

每个目标簇内的目标集合为：

其中

为第j颗雷达第k个目标簇的第i个目标，

为第j颗雷达的第k个目标簇的目标总数；

目标簇的速度为目标簇包含的所有目标的速度和目标到目标簇中心即目标簇的坐标点距离分布的加权平均值；

计算每个目标簇的速度；

目标簇中的目标距离目标簇中心的距离表示为如下公式：

其中，

为第j颗雷达的第k个目标簇的第i个目标距离该目标簇中心的距离；

中

为第j颗雷达的第k个目标簇的第i个目标的位置，其坐标为(x,y,z)；

为第j颗雷达的第k个目标簇的中心坐标；

表示其中最大距离，则计算第j颗雷达的第k个目标簇速度

公式如下：

其中

为第j颗雷达的第k个目标簇第i个目标的速度；得到每颗雷达的每个目标簇在车辆坐标系的位置坐标和目标簇速度，从而得到车辆坐标系下每颗雷达的目标簇及目标簇速度。

优选地，所述使用上一帧估计结果对当前帧目标簇进行匹配度计算包括：

首先，通过上一帧车辆速度状态估计和上一帧目标簇计算在当前帧车辆坐标系下的预测目标簇，得到预测目标簇；

然后，搜索当前帧目标簇一定范围内的预测目标簇；

最后，通过目标簇匹配度计算得到目标簇匹配度。

优选地，S_t-1{V_x ^t-1,ω^t-1}为上一帧的速度状态估计，V_x ^t-1为上一帧状态估计的纵向线速度，ω^t-¹为上一帧估计的车辆旋转速度；

为上一帧的目标簇集合，第j颗雷达的第k个目标簇相对车辆坐标系的坐标位置为

为第t-1帧中第j颗雷达的第k个目标簇，其位置的三维坐标为{g_x,g_y,g_z}；

根据上一帧的车辆速度状态得到变换参数为：

其中t_x＝V_x ^t-1τ，t_y＝0，φ＝ω^t-1τ；由于通常情况下相邻的两帧之间时间间隔τ很小，只考虑t_x和Φ即可；则变换参数如下所示：

其中V_x ^t-1为上一帧状态估计的纵向线速度，ω^t-1为上一帧估计的车辆旋转速度，τ为两帧之间时间间隔；

预测过程如下所示：

其中,

为上一帧第j颗雷达的目标簇的集合，集合中的目标簇为:

共N_j个；每个目标簇经过变换函数T，得到

为上一帧第j颗雷达的目标簇在当前帧车辆坐标系下的预测目标簇的集合

为第t-1帧第j颗雷达的第k个目标簇在第t帧中的预测目标簇，其位置的三维坐标为{g_px,g_py,g_pz}；变换函数T的表达式如下：

其中

为变换后的位置坐标，

为变换前的位置坐标；

为三维旋转矩阵，Φ为旋转角度，

为沿车辆前向的平移矢量；

得到当前车辆坐标系中的预测目标簇。

优选地，所述搜索当前帧目标簇一定范围内的预测目标簇包括：

搜索当前帧目标簇三维空间附近一定范围内是否存在预测目标簇，当前帧每个目标簇的搜索范围不同，表示为

其值可由以下公式得到：

其中

为第j颗雷达的第k个目标簇与预测目标簇的距离阈值，V_x ^t-1，ω^t-1为上一帧的状态估计，即上一帧的纵向线速度和上一帧的旋转角速度。

优选地，根据搜索结果计算目标簇匹配度：如果目标簇范围内不存在预测目标簇，则令匹配度为零即M＝0；如果存在f个(f＝1,2,3…)预测目标簇，则分别计算与这f个预测目标簇的匹配度M_f，并对匹配度进行排序，取最大匹配度M_f,f＝argMax(M_f)作为当前帧目标簇的匹配度。

优选地，计算目标簇的匹配度：计算预测目标簇与当前帧目标簇之间的距离匹配度

再计算目标簇目标数量匹配度Q，通过

和Q联合得出当前帧雷达的目标簇的匹配度M；

其中，目标簇距离匹配度的计算公式使用预测目标簇的正态分布表示为如下形式:

其中

为第j颗雷达的第k个目标簇的预测，

为相对于预测目标簇

的距离匹配度，x为三维空间中任意位置坐标，

为三维欧几里得空间；

其中，目标数量匹配度的计算公式使用数量差异分布表示为如下形式：

其中x₁,x₂为任意两个目标簇中的目标数量，Q为目标数量匹配度，

为正整数空间；

联合以上两公式(11)、(12)得出目标簇的匹配度公式如下所示：

从而得到第j颗雷达当前帧目标簇的置信度

优选地，根据车辆速度状态和雷达多普勒观测建立优化方程，使用雷达目标簇进行求解，包括：

为当前帧的车辆的速度状态，第j颗雷达的安装外参为

当前帧的多普勒速度观测方程如下所示：

其中v_d为第j颗雷达方位角α方向的目标径向观测速度；

第j颗雷达的第k个目标簇的径向观测速度为

误差函数如下所示：

其中e^j为第j颗雷达的误差，N_j为第j颗雷达中的目标簇个数，

为第j颗雷达当前帧第k个目标簇的置信度，误差函数将雷达观测目标簇的速度和车辆的速度状态建立关联，将求解当前帧速度状态变为对优化过程的求解，所述当前帧速度状态如下所示：

其中N_r为雷达数量，N_j为第j颗雷达的目标簇数量；

最后，使用非线性最小二乘方式求解得到当前帧车辆速度状态S_t＝(V_x ^t,ω^t)和残差值e_rest＝min(e)。

优选地，对估计的车辆速度状态进行评价，判断是否符合预期，包括：

使用评价函数对当前帧车辆速度状态进行评价，评价函数如下所示：

E＝T(S_t-1,S_t)W(S_t-1,S_t)e_rest (17)

其评价值为E，T{S_t-1,S_t)为纵向线速度评价值，W(S_t-1,S_t)为旋转角速度评价值，a_max为车辆纵向最大加速度阈值，Φ_max为车辆最大旋转角加速度阈值；如果评价值E达到阈值Et，直接输出车辆速度状态的结果S_t＝(V_x ^t,ω^t)如果评价值为未达到阈值Et，则将目标簇在匹配度计算的基础上增加目标高度置信度，重新计算目标簇匹配度。

优选地，目标高度置信度公式为：

其中ξ_h为置信度参数，初始值为ξ_h＝0.8，h_target为目标所在高度，h₁、h₂分别为高度区间的上限和下限，满足h₂＞h₁；即当h_target处于2h_target<h₁或2h_target＞h₂时，P_h值为1；当h_target处于2h_target∈[h₁,h₂]时，P_h值为ξ_h；

h_m为后轴中心距地面高度，H为行人和车辆高度的平均值，其中，h₁＝-h_m，h₂＝H-h_m；h₁和h₂区间内的目标将被降低置信度，而h₂和最高的目标所在高度h_w区间的静态物体置信度将保持不变。

优选地，每颗雷达的高度置信度分布为：

其中

为第j颗雷达的第k个目标簇，

为

中的z轴坐标值；

最终得到第j颗雷达的第k个目标簇的置信度

为：

迭代计算，如果评价值E达到阈值Et，输出结果S_t＝(V_x ^t,ω^t)；如果仍不满足，调整分配参数ξ_h的数值，将其减半，即ξ_h＝0.5ξ_h，继续返回迭代；

如果评价值E在有限的迭代次数后仍未达到阈值Et，那么使用上一帧估计结果S_t-1＝(V_x ^t-1,ω^t-1)作为本帧估计的结果输出，即使S_t＝S_t-1。

优选地，使用谱聚类算法对每颗雷达的目标点

进行聚类计算，包括如下步骤：

(1)、使用每颗雷达中的目标在车辆坐标系下的三维空间坐标构建无向图：

用点的集合和边的集合来描述构建无向图G，即为G)V,E)；其中V即为雷达在车辆坐标系中的目标(v₁,v₂,…,v_n)；E为边的集合；权重w_ij为点v_i和点v_j之间的权重；其中，w_ij＝w_ji；

(2)、根据相似度矩阵构建邻接矩阵W和度矩阵D：

度d_i定义为和它相连的所有边的权重之和，即

利用每个点度的定义，得到一个n×n的度矩阵D,它是一个对角矩阵，只有主对角线有值，对应第i行的第j个点的度数，表示如下：

使用全连接法定义邻接矩阵W，其矩阵元素表示如下：

其中v_i和v_j分别为两个雷达在车辆坐标系中的目标，σ为方差值；

(3)、计算拉普拉斯矩阵L：

无向图G的拉普拉斯矩阵为：

L＝D-W (26)

根据拉普拉斯矩阵的定义，对于任意向量f有：

设子图集合为A₁,A₂,…,A_k，满足

A₁∪A₂∪…∪A_k＝V；

切图权重为:

其中A，B为两个子图集合，i和j分别是A，B的子集序号；

使用NCut切图:

其中

为A_i的补集，即除子集A_i之外其它所有V的子集的并集；

优化目标函数为：

标准化后的拉普拉斯矩阵为：

(4)、计算L最小的k₁个特征值所对应的特征向量f，并对f组成的矩阵按行标准化；得到n×k₁维的特征矩阵F；

(5)、将特征矩阵F中的每一行作为k₁维的样本，共n个样本，使用k-means聚类方法进行聚类，聚类维数为k₂；

(6)最终得到簇划分C(c₁,c₂,…,c_k2)，每个目标簇内的目标位置均值做为目标簇位置，即得到目标簇。

根据本公开的另一个方面，本公开提供了一种基于车载4D毫米波雷达的车辆速度状态估计装置，包括：

数据采集装置，实时读取4D毫米波雷达采集的目标数据，所述目标数据包括距离、方向角度、俯仰角度和径向多普勒速度；

数据转换装置，对所述目标数据进行转换，目标聚类得到目标簇及其属性，如果数据为第一帧则进行初始化，并结束本次计算；

匹配度计算装置，使用上一帧估计结果对当前帧目标簇进行匹配度计算，结合目标簇高度置信度得到目标簇置信度；

优化求解装置，建立速度状态与目标簇观测方程，用车辆速度状态和当前帧目标簇建立优化方程并求解；

评价装置，对估计的车辆速度状态进行评价，判断是否满足输出条件，如果满足则输出车辆速度状态的估计信息，不满足则降低目标簇高度置信度并返回到优化求解装置重新计算。

根据本公开的再一个方面，本公开提供了一种电子设备，包括：

存储器，所述存储器存储执行指令；以及

处理器，所述处理器执行所述存储器存储的执行指令，使得所述处理器执行上述方法。

根据本公开的由一个方面，本公开提供了一种可读存储介质，所述可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现上述方法。

本公开的有益效果：

1、不依赖于毫米波雷达的数量及方向安装角度，适应性强，应用范围广；

2、在目标簇级别进行动态目标识别和计算，有效降低了多动态目标场景中动态目标对速度估计的影响；

3、充分利用4D毫米波雷达提供俯仰角度的特性，通过高度置信度的调整，将行人车辆高度范围内的目标权重降低，从通用场景中快速提高估计效果，具有自适应性；

4、对于在开放道路中行驶的车辆，也能像在固定封闭道路一样，可靠地估计自车速度状态，在遇到交通复杂的多动态目标环境中仍然可以给出准确的自车速度估计结果；

5、在遇到道路对向车道多辆大型车辆等速行驶的场景中，有效减少所述大型车辆产生的大量动态目标数据对结果的影响，自车速度估计结果准确可靠。

附图说明

附图示出了本公开的示例性实施方式，并与其说明一起用于解释本公开的原理，其中包括了这些附图以提供对本公开的进一步理解，并且附图包括在本说明书中并构成本说明书的一部分。通过结合附图对本公开的实施例进行详细描述，本公开的上述和其它目的、特征、优点将更加清楚。

图1车辆坐标系示意图

图2基于车载4D毫米波雷达的车辆速度状态估计方法的主流程图

图3基于车载4D毫米波雷达的车辆速度状态估计方法的细化流程图

图4雷达坐标系和车辆坐标系的关系图

图5为预测目标簇示意图

图6使用上一帧估计结果对当前帧目标簇进行匹配度计算的细化流程图

图7高度置信度表达示意图

具体实施方式

下面结合附图和实施方式对本公开作进一步的详细说明。可以理解的是，此处所描述的具体实施方式仅用于解释相关内容，而非对本公开的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本公开相关的部分。

需要说明的是，在不冲突的情况下，本公开中的实施方式及实施方式中的特征可以相互组合。下面将参考附图并结合实施方式来详细说明本公开的技术方案。

除非另有说明，否则示出的示例性实施方式/实施例将被理解为提供可以在实践中实施本公开的技术构思的一些方式的各种细节的示例性特征。因此，除非另有说明，否则在不脱离本公开的技术构思的情况下，各种实施方式/实施例的特征可以另外地组合、分离、互换和/或重新布置。

在附图中使用交叉影线和/或阴影通常用于使相邻部件之间的边界变得清晰。如此，除非说明，否则交叉影线或阴影的存在与否均不传达或表示对部件的具体材料、材料性质、尺寸、比例、示出的部件之间的共性和/或部件的任何其它特性、属性、性质等的任何偏好或者要求。此外，在附图中，为了清楚和/或描述性的目的，可以夸大部件的尺寸和相对尺寸。当可以不同地实施示例性实施例时，可以以不同于所描述的顺序来执行具体的工艺顺序。例如，可以基本同时执行或者以与所描述的顺序相反的顺序执行两个连续描述的工艺。此外，同样的附图标记表示同样的部件。

当一个部件被称作“在”另一部件“上”或“之上”、“连接到”或“结合到”另一部件时，该部件可以直接在所述另一部件上、直接连接到或直接结合到所述另一部件，或者可以存在中间部件。然而，当部件被称作“直接在”另一部件“上“、“直接连接到”或“直接结合到”另一部件时，不存在中间部件。为此，术语“连接”可以指物理连接、电气连接等，并且具有或不具有中间部件。

本公开使用的术语是为了描述具体实施例的目的，而不意图是限制性的。如这里所使用的，除非上下文另外清楚地指出，否则单数形式“一个(种、者)”和“所述(该)”也意图包括复数形式。此外，当在本说明书中使用术语“包含”和/或“包括”以及它们的变型时，说明存在所陈述的特征、整体、步骤、操作、部件、组件和/或它们的组，但不排除存在或附加一个或更多个其它特征、整体、步骤、操作、部件、组件和/或它们的组。还要注意的是，如这里使用的，术语“基本上”、“大约”和其它类似的术语被用作近似术语而不用作程度术语，如此，它们被用来解释本领域普通技术人员将认识到的测量值、计算值和/或提供的值的固有偏差。

在本说明书的描述中，参考术语“一个实施方式/方式”、“一些实施方式/方式”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施方式/方式或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施方式/方式或示例中。在本说明书中，对上述术语的示意性表述不必须的是相同的实施方式/方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施方式/方式或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施方式/方式或示例以及不同实施方式/方式或示例的特征进行结合和组合。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

本公开的目的之一是提供一种基于车载4D毫米波雷达的车辆速度状态估计方法。

车载毫米波雷达的发射天线发射周期性FMCW雷达波(FMCW,Frequency ModulatedContinuous Wave调频连续波)，雷达波遇到物体后返回形成回波，接收天线接收到回波后进行快速傅里叶变换(FFT,Fast Fourier Transform)解算出物体的距离、角度和速度，每个解算出的结果作为一个目标。4D毫米波雷达使用了水平和竖直两个方向的接收天线，可以得到目标的水平和俯仰两个角度，再加上目标的距离和径向速度，总共四个信息。

本公开使用一颗或多颗安装在车身周围的车载4D毫米波雷达，对车身的速度状态进行实时估计。图1为车辆坐标系示意图。车辆完成初始化后，参照前一帧的速度状态估计，在车辆坐标系(以车辆后轴中心为原点，如图1所示)下对毫米波雷达探测到的目标进行目标簇归类，对目标簇进行匹配度和目标高度的置信度评估，有效减少动态目标，尤其是速度相对一致的多动态目标对自车速度状态估计的影响。再使用带有置信度的目标簇的多普勒速度信息进行优化求解，得出对自车速状态估计的结果。

本公开使用安装在车辆周围的一颗或者多颗4D毫米波雷达提供的目标信息，实时估计车辆的速度状态，并适应从多静态物体到多动态物体的不同场景，保证估计结果的鲁棒性。

图2描述了基于车载4D毫米波雷达的车辆速度状态估计方法的主流程图。如图2所示(S100)，所述基于车载4D毫米波雷达的车辆速度状态估计方法包括如下步骤：

第一步(S110)，雷达数据转换，目标聚类得到目标簇及其属性，如果数据为第一帧则进行初始化，并结束本次计算；

第二步(S120)，使用上一帧估计结果对当前帧目标簇进行匹配度计算，结合目标簇高度置信度得到目标簇置信度；

第三步(S130)，建立速度状态与目标簇观测方程，用车辆速度状态和当前帧目标簇建立优化方程并求解；

第四步(S140)，对估计的车辆速度状态进行评价，判断是否满足输出条件，如果满足则输出车辆速度状态的估计信息，不满足则降低目标簇高度置信度并返回到第三步(S130)。

在进行步骤描述之前，定义以下数值：

N_r：雷达的个数；

N_j：第j颗雷达内的目标簇个数；

τ:两帧之间时间间隔；

本公开对车辆速度状态进行估计，车辆的速度状态以车辆坐标系(车辆后轴中心为坐标原点，如图1所示)为基准。速度状态包括车辆行驶平面内沿行驶方向的纵向线速度V_x，与V_x垂直的横向线速度V_y，车辆转向过程中产生的绕车辆坐标系的旋转角速度ω，根据阿克曼车辆运动模型，车辆坐标系的横向速度为0，则待估计的速度状态为：

S_t＝{V_x ^t,ω^t|t＝0,1,2…} (1)

其中S_t为第t帧的车辆速度状态，V_x ^t为第t帧的纵向线速度，ω^t为第t帧的旋转角速度。帧是按照雷达的数据发布来定义的，雷达按照固定的频率(周期)来发出一个时刻的所有目标数据(比如每50毫秒发布一次数据)，从系统启动到第一次接收到雷达的数据为第0帧数据，此时状态为S₀即为车辆的初始状态，此后接收到的雷达数据帧数依次递增，每帧的时间间隔即为雷达发布数据的周期。

本公开使用一颗或多颗水平安装在车身周围的4D毫米波雷(以下简称“雷达”)观测车辆周围环境，雷达输出目标数据，每颗4D毫米波雷达输出的目标数据都是相对于雷达坐标系的，包括目标的距离、方向角度、俯仰角度和径向多普勒速度(沿雷达观测方向的标量速度)。

多颗雷达需要做相应的数据时间同步(通常由雷达硬件的机制完成)，保证每一帧数据包含所有雷达同一时刻附近的所有数据，且每帧数据以稳定频率输出，两帧之间时间间隔即两帧之间的间隔时间为τ(比如50毫秒)。则每一帧的雷达目标数据为所有雷达同一时刻发出的数据。

本公开的正常使用需要车辆启动后进行系统初始化，车辆应处于静止状态，系统上电并运行程序后收到雷达第一帧数据，此时车辆的速度状态各项为零，即S₀{v_x＝0,ω＝0}。由于之后的每帧计算都是基于前一帧数据的相关结果，是一个马尔科夫过程，所以初始化是必须的。而正常车辆的使用都是从车辆静止开始启动，所以初始过程的条件是合理的。

图3为基于车载4D毫米波雷达的车辆速度状态估计方法的细化流程图，是对图2的具体步骤进行的细化描述。如图3所示，其流程如下：

(1)判断是否为第一帧数据(S111)

如果为第一帧数据则初始化车辆状态为：S₀{v_x＝0,ω＝0}(S112)，若已经不是第一帧数据则直接进入下一步计算。

(2)雷达数据转换，生成目标簇及其属性(S113)

下面开始雷达数据的转换，将每颗雷达的目标数据从雷达坐标系转换到车辆坐标系。设第j颗雷达的第i个目标的距离为

水平角度为

俯仰角度为

多普勒速度为

设第j颗雷达相对车辆坐标系的安装外参为

其中

分别为第j颗雷达相对车辆坐标系绕z轴、y轴、x轴的角度。由于雷达都为水平安装，所以其中

均为0，则安装外参简化为

根据外参关系和雷达目标信息计算第j颗雷达中的每个目标在车辆坐标系下的三维空间坐标

得到车辆坐标系下的每颗雷达的目标(1101)。

雷达坐标系和车辆坐标系的关系如图4所示，图4为车辆俯视图，只示意坐标系之间的平面关系，坐标z轴都是垂直本图朝向图外的，其中O为车辆坐标系，O_s1和O_s2分别为两颗雷达的坐标系，

分别为两颗雷达的外参，其中

和

为第一个雷达中的两个目标，

为第二个雷达中的目标，

都为相对于车辆坐标系O的坐标。

使用DBSCAN(Density-based spatial clustering of applications withnoise)算法对每颗雷达的目标点

进行聚类计算(S114)，使用kd-tree来加快三维空间的距离搜索，提高计算效率。通过DBSCAN算法得到目标的核心点、核心可达点和离群点。核心点一般由环境中体积较大的物体产生，如建筑和大型车辆；离群点通常由环境中体积较小物体产生，如路灯和行人。将每个核心点及其核心可达点作为一个目标簇，核心点的坐标即为目标簇坐标

每个离群点单独作为一个目标簇，离群点坐标即为目标簇坐标。最终得到车辆坐标系下每颗雷达的目标簇(1102):

其中

为第j颗雷达的第k个目标簇；N_r为雷达的个数，N_j为第j颗雷达的目标簇个数。

每个目标簇内的目标集合为：

其中

为第j颗雷达第k个目标簇的第i个目标，

为第j颗雷达的第k个目标簇的目标总数。每个目标的坐标表示为

计算每个目标簇的速度(S115)，目标簇速度为目标簇包含的所有目标的速度和目标到目标簇中心(即目标簇的坐标点)距离分布的加权平均值。

目标簇中的目标距离目标簇中心的距离为：

其中

为第j颗雷达的第k个目标簇的第i个目标距离该目标簇中心的距离，

为第j颗雷达的第k个目标簇的第i个目标的位置坐标，

为第j颗雷达的第k个目标簇的中心坐标。

设其中最大距离为

计算第j颗雷达的第k个目标簇速度

公式如下：

其中

为第j颗雷达的第k个目标簇第i个目标的速度。得到了每颗雷达的每个目标簇在车辆坐标系的位置坐标和目标簇速度。

综上所示，可以得到了车辆坐标系下每颗雷达的目标簇及目标簇速度(1103)。

(3)使用上一帧估计结果对当前帧目标簇进行匹配度计算(S120)

本步骤的流程如图6所示，为图2中的步骤S120的细化流程图。

首先通过上一帧车辆速度状态估计和上一帧目标簇计算在当前帧车辆坐标系下的预测目标簇(S121)，得到预测目标簇(1201)，然后搜索当前帧目标簇一定范围内的预测目标簇(S122)，最后通过目标簇匹配度计算(S123、S124、S125)得到目标簇匹配度(1202)。

目标簇匹配度计算的主要目的是将动态目标簇区分出来，降低其在后续计算中的权重。不同于现有技术的离群点剔除算法(如RANSAC)和滤波算法，本步骤的方法可以将动态目标中的大型目标(目标簇内目标数量较多)对计算精度的影响降至最低。

(2.1)目标簇的预测计算过程(S121)。

设上一帧的速度状态估计为S_t-1{V_x ^t-1,ω^t-1}。

设上一帧的目标簇为

每个目标簇相对车辆坐标系的坐标位置为

根据上一帧的车辆速度状态得到变换参数为：

其中t_x＝V_x ^t-1τ，t_y＝0，φ＝ω^t-1τ。由于通常情况下相邻的两帧之间时间间隔τ很小，所以只考虑t_x和φ即可。则变换参数为：

其中V_x ^t-1为上一帧状态估计的纵向线速度，ω^t-1为上一帧估计的车辆旋转速度，前已述及τ为两帧之间的间隔时间。

预测过程如下所示：

其中,

为上一帧第j颗雷达的目标簇的集合，集合中的目标簇为:

共N_j个；每个目标簇经过变换函数T，得到

第t-1帧中第j颗雷达的第i个目标簇

其位置的三维坐标为{g_x,g_y,g_z}；第t-1帧第j颗雷达的第i个目标簇在第t帧中的预测目标簇

其位置的三维坐标为{g_px,g_py,g_pz}，变换函数T的表达式如下：

其中

为变换后的位置坐标，

为变换前的位置坐标；

为三维旋转矩阵，φ为旋转角度，

为沿车辆前向的平移矢量。

综上所述，得到当前车辆坐标系中的预测目标簇(1201)。

如图5所示，在车辆坐标系O下，1、2、3为上一帧的目标簇，实线方框内的点为目标簇所包含的目标，1,2为包含多个目标的目标簇，3为离群点的目标簇；虚线框1预测、2预测、3预测分别为1、2、3在本帧中的预测目标簇；1’、2’、3’本帧中的目标簇，分别对应上一帧的1、2、3目标簇，即1和1`，2和2`，3和3`分别为同样的3个物体产生的3个上一帧目标簇和3当前帧目标簇。其中2预测与2`，3预测与3`的位置相近，2与2`为静止的车辆，3与3`为反射面积较小的静止物体；其中1’与1预测的位置相差较大，为一辆对向行驶的大型车辆的目标簇，可以被识别为动态目标。

(2.2)搜索当前帧目标簇一定范围内的预测目标簇(S122)。

首先搜索当前帧目标簇三维空间附近一定范围内是否存在预测目标簇，当前帧每个目标簇的搜索范围不同，设为

其值可由以下公式得到：

其中

(2.3)根据搜索结果计算目标簇匹配度。

如果目标簇范围内不存预测目标簇，设匹配度为零即M＝0(S123)；如果存在预测目标簇，比如f个(f＝1,2,3…)，则分别计算与这f个预测目标簇的匹配度M_f(S125)，对匹配度进行排序，取最大匹配度M_f,f＝argMax(M_f)作为当前帧目标簇的匹配度(S126)。

(2.4)计算目标簇的匹配度(S124)

计算预测目标簇与当前帧目标簇之间的距离匹配度

再计算目标簇目标数量匹配度Q，通过

和Q联合得出当前帧雷达的目标簇的匹配度M。

目标簇距离匹配度的计算公式，使用预测目标簇的正态分布为以下形式:

其中

为第j颗雷达的第k个目标簇的预测，

为相对于预测目标簇

的距离匹配度，x为三维空间中任意位置坐标，

为三维欧几里得空间；

目标数量匹配度的计算公式，数量差异分布为：

其中x₁,x₂为任意两个目标簇中的目标数量，Q为目标数量匹配度。

为正整数空间。

以上两公式联合得出目标簇的匹配度公式：

经过以上计算，得到当前帧目标簇的置信度(1202)

(4)根据车辆速度状态和雷达多普勒观测建立优化方程，使用雷达目标簇进行求解(S130)

本步骤的计算都是基于当前帧时刻，设当前帧的车辆的速度状态为S_t＝(V_x ^t,ω^t)，第j颗雷达的安装外参为

则当前帧的多普勒速度观测方程为：

其中v_d为第j颗雷达方位角α方向的目标径向观测速度。

第j颗雷达的第k个目标簇的径向观测速度为

误差函数为：

为上一步骤中已经计算出的第j颗雷达当前帧第k个目标簇置信度，误差函数将雷达观测目标簇的速度和车辆的速度状态建立关联，将求解当前帧速度状态变为对优化过程的求解：

其中N_r为雷达数量，N_j为第j颗雷达的目标簇数量，此处将多颗雷达的估计综合在了一起。

最后，使用非线性最小二乘等方式求解得到当前帧车辆速度状态S_t＝(V_x ^t,ω^t)和残差值e_rest＝min(e)。

(5)对估计的车辆速度状态进行评价，判断是否符合预期(S140)

使用评价函数对当前帧车辆速度状态进行评价，评价函数为：

E＝T(S_t-1,S_t)W(S_t-1,S_t)e_rest (17)

其评价值为E，T(S_t-1,S_t)为纵向线速度评价值，W(S_t-1,S_t)为旋转角速度评价值，a_max为车辆纵向最大加速度阈值，Φ_max为车辆最大旋转角加速度阈值。

如果评价值E达到阈值Et，直接输出车辆速度状态的结果S_t＝(V_x ^t,ω^t)如果评价值为未达到阈值，则将目标簇的匹配度计算的基础上增加目标高度置信度，重新计算目标簇匹配度。计算方式如下：

目标高度置信度置信度公式为：

其中ξ_h为置信度参数，初始值为ξ_h＝0.8，h_target为目标所在高度，h₁、h₂分别为高度区间的上限和下限，满足h₂＞h₁；即当h_target处于2h_target<h₁或2h_target＞h₂时，P_h值为1；当h_target处于2h_target∈[h₁,h₂]时，P_h值为ξ_h。

如图7所示，h_m为后轴中心距地面高度，H为行人和车辆高度的平均值，H取2米。令h₁＝-h_m，h₂＝H-h_m。h₁和h₂区间内的目标将被降低置信度，这个区间是行人和车辆所在的高度，而h₂和最高的目标所在高度h_w区间的静态物体，比如建筑物等，置信度将保持不变。

根据上式，每颗雷达的高度置信度分布为：

其中

为第j颗雷达的第k个目标簇，

为

中的z轴坐标。由于目标簇的坐标都是以车辆坐标系为基准的，所以无需使用雷达外参信息。

最终得到第j颗雷达的第k个目标簇的置信度

为：

根据以上加入了高度置信度的方法返回(S130)迭代计算，如果评价值E达到阈值Et，输出结果S_t＝(V_x ^t,ω^t)；如果仍不满足，调整分配参数ξ_h的数值，将其减半ξ_h＝0.5ξ_h，继续返回(S130)迭代。此过程每次迭代都将行人车辆高度范围内的置信度降低，也就是将可能的动态障碍物影响逐步降低，此过程使算法具有自适应性。

如果评价值E在有限的迭代次数K＝3次后仍未达到阈值Et，那么使用上一帧估计结果S_t-₁＝(V_x ^t-1,ω^t-1)作为本帧估计的结果输出，即使S_t＝S_t-1。通常情况下不会进行到这一步，但不排除毫米波雷达全部被遮挡这种极端情况。

至此，得到车辆坐标系下的速度估计结果。

可选地，使用谱聚类算法对每颗雷达的目标点

进行聚类计算(S114)，包括如下步骤：

(1)使用每颗雷达中的目标在车辆坐标系下的三维空间坐标构建无向图。

构建无向图G，用点的集合V和边的集合E来描述。即为G(V,E)。其中V即为雷达在车辆坐标系中的目标(v₁,v₂,…,v_n)。定义权重w_ij为点v_i和点v_j之间的权重。由于我们是无向图，所以w_ij＝w_ji。

(2)根据相似度矩阵构建邻接矩阵W和度矩阵D。

度d_i定义为和它相连的所有边的权重之和，即

利用每个点度的定义，我们可以得到一个n×n的度矩阵D,它是一个对角矩阵，只有主对角线有值，对应第i行的第j个点的度数，定义如下：

使用全连接法定义邻接矩阵W，其矩阵元素表示如下：

其中v_i和v_j分别为两个雷达在车辆坐标系中的目标，σ为方差值。

(3)计算拉普拉斯矩阵L。

图G的拉普拉斯矩阵定义为：

L＝D-W (26)

根据拉普拉斯矩阵的定义，对于任意向量f有：

设子图集合为A₁,A₂,…,A_k，满足

A₁∪A₂∪…∪A_k＝V。

切图权重为:

其中A，B为两个子图集合，i和j分别是A，B的子集序号。

使用NCut切图:

其中

为A_i的补集，即除子集A_i之外其它所有V的子集的并集。

优化目标函数为：

标准化后的拉普拉斯矩阵为：

(4)计算L最小的k₁个特征值所对应的特征向量f，并对f组成的矩阵按行标准化。

计算后会得到n×k₁维的特征矩阵F。

(5)将F中的每一行作为k₁维的样本，共n个样本，使用k-means聚类方法进行聚类，聚类维数为k₂。

(6)最终得到簇划分C(c₁,c₂,…,c_k2)，每个目标簇内的目标位置均值做为目标簇位置，即得到目标簇(1102)。

本公开的流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本公开的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本公开的实施方式所属技术领域的技术人员所理解。处理器执行上文所描述的各个方法和处理。例如，本公开中的方法实施方式可以被实现为软件程序，其被有形地包含于机器可读介质，例如存储器。在一些实施方式中，软件程序的部分或者全部可以经由存储器和/或通信接口而被载入和/或安装。当软件程序加载到存储器并由处理器执行时，可以执行上文描述的方法中的一个或多个步骤。备选地，在其他实施方式中，处理器可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行上述方法之一。

在流程图中表示或在此以其他方式描述的逻辑和/或步骤，可以具体实现在任何可读存储介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。

就本说明书而言，“可读存储介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。可读存储介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式只读存储器(CDROM)。另外，可读存储介质甚至可以是可在其上打印程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得程序，然后将其存储在存储器中。

应当理解，本公开的各部分可以用硬件、软件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路、具有合适的组合逻辑门电路的专用集成电路、可编程门阵列(PGA)、现场可编程门阵列(FPGA)等。

本公开领域的普通技术人员可以理解实现上述实施方式方法的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述程序可以存储于一种可读存储介质中，该程序在执行时，包括方法实施方式的步骤之一或其组合。

此外，在本公开各个实施方式中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个可读存储介质中。存储介质可以是只读存储器，磁盘或光盘等。

本公开还提供了一种电子设备，包括：存储器，存储器存储执行指令；以及处理器或其他硬件模块，处理器或其他硬件模块执行存储器存储的执行指令，使得处理器或其他硬件模块执行上述的方法。

本公开还提供了一种可读存储介质，可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现上述的方法。

综上所述，本公开所述方法具有如下优势：

(1)不同于大多数基于毫米波雷达目标的自车速度估计方法，本方法将毫米波雷达的目标转换到车辆坐标系下统一处理，故可以使用任意数量的毫米波雷达，对雷达安装方位角度没有要求，通用性更强。

(2)不同于使用滤波算法和常见的异常数据剔除方法(例如RANSAC)，本公开使用历史估计结果预测当前目标簇的分布，并同当前帧目标簇的实际分布进行匹配计算。目标簇的匹配度与目标簇内的目标本身数量无关，只与位置匹配度和两帧中同一目标簇的数量变化有关，可以有效区分大型动态目标形成的目标簇(例如对向行驶的大型货车)。本公开不依赖固定参数的设置(对比例如RANSAC)，使用范围更广泛，对比现有的技术方法，本公开可以使用在多动态目标场景，具有较高鲁棒性。

(3)使用目标在三维空间中的高度置信度，在算法无法得到足够可靠的结果后，会根据大部分行人和车辆所处距地面的高度，降低该高度范围内的空间置信度，从而大概率的降低车辆在常用场景中的动态目标的影响。对比现有的技术方法，本公开更具自适应性。

由此可见，所述基于4D毫米波雷达测量结果，通过上述车辆速度状态估计方法，自动驾驶车辆即使在多动态目标场景下，也能消除动态目标对自车速度估计结果的影响，其自车速度估计结果实时准确、可靠性高。不管车辆是处于全部为静态物体的环境下，还是处于以动态目标为主的多动态目标场景下，均能取得良好的速度估计效果。具有具有十分广阔的产业应用前景。

至此，已经结合附图所示的优选实施方法描述了本公开的技术方案，但是，本领域技术人员应当理解，上述实施方式仅仅是为了清楚地说明本公开，而并非是对本公开的范围进行限定，本公开的保护范围显然不局限于这些具体实施方式。在不偏离本公开的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案仍处于本公开的范围内。