一种基于单帧毫米波雷达点云的运动估计方法及系统与流程

1.本发明属于信号与信息处理技术领域，更具体地，涉及一种基于单帧毫米波雷达点云的运动估计方法及系统。


背景技术：

2.毫米波雷达是一种以毫米电磁波作为载波的雷达传感器，通过分析发射波与目标反射回波间的属性差异，实现目标的距离、角度、速度等状态的实时检测。
3.毫米波雷达具有抗干扰性强、测速精度高、测量距离远、测角范围大、能够全时工作的优点，被广泛的应用于汽车辅助驾驶领域。其中较为典型的应用为基于毫米波雷达的多目标跟踪，以毫米波雷达检测数据作为输入，在多目标跟踪算法如：jpda、cjpda、mht等算法的作用下，将雷达点云数据转换为包含目标实时位置、速度的航迹信息输出，实现多目标跟踪。
4.然而由于毫米波雷达仅能测量出目标与自身的相对径向速度，当被测目标相对切向速度较大时，在传统的基于毫米波雷达的目标运动估计方法作用下，由于测量结果中存在的误差导致目标跟踪算法中航迹起始以及目标运动预测等环节中出现较大误差，当系统调节能力有限时甚至会导致跟踪目标丢失的不可逆结果，大幅降低跟踪系统性能和可靠性。


技术实现要素：

5.针对现有技术的缺陷，本发明的目的在于提供一种基于单帧毫米波雷达点云的运动估计方法及系统，旨在解决传统的毫米波雷达目标运动估计方法在多目标跟踪应用中存在误差大、可靠性差的问题。
6.本发明提供了一种基于单帧毫米波雷达点云的运动估计方法，包括下述步骤：s1读取点云数据并进行数据聚类处理；s2对聚类数据进行运动学逆解、运动学正解和随机抽样仿真后实现目标运动估计；s3对目标运动估计后的数据进行数据细化分类和速度转化实现数据后处理。
7.更进一步地，步骤s1具体包括：s1.1 对参数进行初始化，所述参数包括能量筛选阈值p
min
，聚类算法参数，速度估计误差阈值v
tol
，随机抽样仿真次数n
max
，当前自体运动速度v
ego
，自体运动角速度ω
ego
，雷达纵向偏移距离h，雷达横向偏移距离l，雷达初始安装角度α和速度补偿加权系数β；s1.2读取点云数据，将能量值i大于能量筛选阈值p
min
的数据保留并根据r由小到大排序后按照设定格式data
raw
存储；所述点云数据包括极坐标系下的极径r，角度θ，多普勒速度v和能量i；s1.3根据当前时刻自体速度和角速度，根据运动学原理转化至量测坐标并在进行自体运动补偿；
s1.4以点云数据data为基础，调用聚类算法并根据距离将数据划分为各自独立的类从而实现点云数据聚类。
8.更进一步地，步骤s1.3具体为：s1.3.1将自体运动速度、自体运动角速度转换至雷达量测坐标系；s1.3.2在所述雷达量测坐标系下对data
raw
数据进行速度补偿；步骤1.3.3：将速度补偿后的数据记为data，其中，式中符号下角标表示点云数据编号。
9.更进一步地，步骤s1.4具体为：s1.4.1计算点云数据距离矩阵；其中，；s1.4.2以距离矩阵为输入对点云数据进行数据聚类处理。
10.更进一步地，步骤s2具体为：s2.1读取聚类数据：读取第k个聚类中的所有数据标号，m为聚类中所有数据的个数，若m《2则跳转至步骤s3.3，否则设置随机抽样最大次数为；s2.2随机抽样：每次抽样随机从聚类中抽取两个数据点标号，令其为，式中t代表随机抽样次数，l、m为随机抽取的聚类中数据标号且；s2.3运动学逆解：计算第t次随机抽样下第k个聚类关联目标速度估计值；s2.4运动学正解：根据所述目标速度估计值计算并统计数据点，获得第t轮满足运动学正解速度误差小于v
tol
的数据点集合；
s2.5重复步骤s2.2~s2.4直至完成次随机抽样仿真。
11.更进一步地，步骤s2.3中，根据公式
12.计算第t次随机抽样下第k个聚类关联目标速度估计值，式中为x轴方向速度估计值，为y轴方向速度估计值。
13.更进一步地，步骤s2.4中，根据公式
14.计算并统计数据点。
15.更进一步地，步骤s3具体为：步骤3.1：数据细化分类：若集合不为空，则将第k个聚类中标号属于集合的所有数据分类为运动目标，其他数据分类为噪声点，否则跳转至步骤3.3；步骤3.2：估计速度转化：将data中标号属于集合的所有数据的估计速度转化至雷达量测坐标系中；步骤3.3：令k值加1，若k≤n
total
则并跳转至步骤s2.1，否则结束并输出数据。
16.本发明还提供了一种基于单帧毫米波雷达点云的运动估计系统，包括依次连接的点云数据前处理单元、目标运动估计单元和数据后处理单元；点云数据前处理单元用于读取点云数据并进行数据聚类处理；目标运动估计单元用于对聚类数据进行运动学逆解、运动学正解和随机抽样仿真后实现目标运动估计；数据后处理单元用于对目标运动估计后的数据进行数据细化分类和速度转化实现数据后处理。
17.通过本发明所构思的以上技术方案，与现有技术相比，能够取得如下有益效果：（1）通过引入自体速度、角速度补偿，对于自身存在速度、角速度的情况，采用本发明所提出的方法能够有效的降低由于自体运动导致的被测目标速度测量误差，提升了速度测量精度。
18.（2）通过引入了随机抽样算法，当聚类数据中存在噪声时，采用本发明所提出的方法，能够在识别出目标速度的前提下实现噪声数据筛选，提升了点云数据处理算法性能。
附图说明
19.图1为本发明实施例提供的一种基于单帧毫米波雷达点云的运动估计系统的模块结构示意图。
20.图2为本发明实施例提供的一种基于单帧毫米波雷达点云的运动估计方法实现流程图。
21.图3为测试场景现场照片。
22.图4为本发明实施例提供的基于单帧毫米波雷达点云的运动估计方法仿真结果示意图。
23.图5为本发明实施例提供的基于单帧毫米波雷达点云的运动估计方法速度估计误差示意图。
24.图6为本发明实施例提供的基于单帧毫米波雷达点云的运动估计方法货车速度估计变化曲线示意图。
25.图7为本发明实施例提供的基于单帧毫米波雷达点云的运动估计方法货车速度估计误差变化曲线示意图。
26.图8为本发明实施例提供的基于单帧毫米波雷达点云的运动估计方法轿车速度估计变化曲线示意图。
27.图9为本发明实施例提供的基于单帧毫米波雷达点云的运动估计方法轿车速度估计误差变化曲线示意图。
具体实施方式
28.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
29.本发明克服了传统的毫米波雷达目标运动估计方法在多目标跟踪应用中存在的缺陷，以小幅增加系统算力损耗为代价，大幅提升了毫米波雷达目标运动估计方法性能。
30.本发明所提出的基于单帧毫米波雷达点云的运动估计系统的结构如图1所示，按功能划分为三个模块：点云数据前处理模块、目标运动估计模块、运动估计数据后处理模块。其中点云数据前处理模块主要用于实现点云数据输入及预处理、自体运动补偿、点云数据聚类等操作；目标运动估计模块为本发明所提出的算法核心，执行随机抽样一致框架下的运动学逆解、运动学正解；运动估计数据后处理模块运行数据细化分类、运动估计速度转化等算法，输出包含运动估计信息的点云数据，用于多目标跟踪中的航迹起始或关联等。
31.本发明实施例提供的基于单帧毫米波雷达点云的运动估计方法的实现流程如图2所示，具体包括：步骤1：点云数据前处理步骤1.1： 算法参数初始化：设定能量筛选阈值p
min
，聚类算法参数（根据所采用的聚类算法不同有所差异），速度估计误差阈值v
tol
，随机抽样仿真次数n
max
，当前自体运动速度v
ego
，自体运动角速度ω
ego
，雷达纵向偏移距离h，雷达横向偏移距离l，雷达初始安装角度α和速度补偿加权系数β；步骤1.2：点云数据输入及预处理：读取点云数据（包括极坐标系下的极径r，角度θ，多普勒速度v和能量值i，），将能量值i大于能量筛选阈值p
min
的数据保留并根据r由小到大排序，最终按照式（1）格式存储，记为data
raw
；
（1），式中符号下角标表示点云数据编号。
32.步骤1.3: 自体运动补偿。根据当前时刻自体速度和角速度，根据运动学原理转化至量测坐标并在进行补偿，具体步骤如下：步骤1.3.1：将自体运动速度、自体运动角速度转换至雷达量测坐标系中，如公式（2）所示（2）式中为x轴方向速度补偿值，为y轴方向速度补偿值步骤1.3.2：以式（2）的运算结果按照式（3）对data
raw
作速度补偿（3）步骤1.3.3：将速度补偿后的数据记为data，如式（4）所示（4）步骤1.4：点云数据聚类。以点云数据data为基础，调用聚类算法，根据距离将数据划分为各自独立的类，具体步骤为：步骤1.4.1：计算点云数据距离矩阵d（5）式中（6）步骤1.4.2：数据聚类。调用聚类算法如optics、dbscan、最近邻聚类、dpc等，以式（6）所示的距离矩阵为输入，对点云数据进行聚类，令聚类总数为n
total
，由于聚类算法较成
熟，在此不作赘述。
33.步骤2：目标运动估计若步骤1中划分的聚类总数n
total
小于1，则跳转至步骤1.2；若n
total
不小于1，则遍历每一个所划分的聚类进行目标运动估计，具体步骤如下：步骤2.1：读取聚类数据。读取第k个聚类中的所有数据标号，m为聚类中所有数据的个数。若m《2则跳转至步骤3.3，否则设置随机抽样最大次数为（7）步骤2.2：随机抽样。每次抽样随机从聚类中抽取两个数据点标号，令其为，式中t代表随机抽样次数，l、m为随机抽取的聚类中数据标号且。
34.步骤2.3：运动学逆解。由公式（8）计算第t次随机抽样下第k个聚类关联目标速度估计值；（8）式中为x轴方向速度估计值，为y轴方向速度估计值步骤2.4：运动学正解。根据步骤2.3所获得的目标速度，根据公式（9）计算并统计数据点，得第t轮满足运动学正解速度误差小于v
tol
的数据点集合；（9）步骤2.5：重复执行步骤2.2~2.4，直至完成次随机抽样仿真。设值如公式（10）所示；（10）其中，为取集合长度算子；3. 运动估计数据后处理步骤3.1：数据细化分类。若集合不为空，则将第k个聚类中标号属于集合的所有数据分类为运动目标，其他数据分类为噪声点，否则跳转至步骤3.3；步骤3.2：估计速度转化。根据式（11）将data中标号属于集合的所有数据的估计速度转化至雷达量测坐标系中。
35.（11）步骤3.3：令k值加1，若k≤n
total
则跳转至步骤2.1，否则算法结束，输出数据。
36.本发明通过引入自体速度、角速度补偿，对于自身存在速度、角速度的情况，采用本发明所提出的方法能够有效的降低由于自体运动导致的被测目标速度测量误差，提升了速度测量精度。同时，通过引入了随机抽样算法，当聚类数据中存在噪声时，采用本发明所提出的方法，能够在识别出目标速度的前提下实现噪声数据筛选，提升了点云数据处理算法性能。
37.为了更进一步的说明本发明实施例提供的基于单帧毫米波雷达点云的运动估计方法，现结合具体实例详述如下：采用本发明实施例提供的基于单帧毫米波雷达点云的运动估计方法对77ghz车载雷达数据进行处理，测试场景为辅路汇入主路路口，存在横向行驶机动车，自身车辆缓慢右转；具体过程如下：（1）算法参数初始化。设定能量阈值p
min
=1e-3、聚类算法采用optics算法，设置参数为——核心密度ρ=3、邻域距离=0.2，速度估计误差阈值v
tol
=0.2m/s，随机抽样仿真次数n
max
=100，测试场景中自体运动速度v
ego
=2m/s，自体运动角速度ω
ego
=0.03rad/s，雷达纵向偏移距离h=3m、雷达横向偏移距离l=0.01m、雷达初始安装角度α=0.01rad、速度补偿加权系数β=0.1。
38.（2）点云数据前处理。根据步骤1.2~1.4中式（1）~式（6） 所列算法，采用optics算法对点云数据进行聚类。
39.（3）目标运动估计。以点云数据前处理输出作为输入，按照步骤2.1~步骤2.5遍历所有聚类。
40.（4）运动估计数据后处理。根据步骤3.1~3.3，计算点云数据前处理输出的聚类数据与目标运动估计误差，对误差较大的数据点分类为噪声。
41.本发明所提出的算法在应用例中的测试结果如图3~图9所示。
42.图3为测试场景现场环境照片，如图所示当前自身车辆前方为辅路汇入主路入口处，自身车辆处于辅路路侧，有向右前方驶入汇入口趋势。车辆前方存在由辅路汇入主路的小型货车，右前方存在由辅路汇入主路的小型轿车，在传统的雷达数据处理算法中，由于货车与轿车均相对于自身车辆作横向运动，由于雷达对法向速度敏感，无法测量出切向速度，此场景中若采用雷达的量测速度对目标进行运动跟踪将产生较大的误差，目标极易丢失。图4为采用所提出的基于单帧毫米波雷达点云的运动估计方法对测试场景中所采集数据的处理结果对比图，图中四边形为货车点云数据，圆形为小轿车点云数据，实线箭头为毫米波雷达所测得多普勒速度，货车多普勒速度量测结果范围为0.03m/s~0.58m/s，轿车多普勒速度量测结果范围为0.85m/s~1.45m/s，可见在当前场景中，毫米波雷达无法准确测量目标的实际运动。图中点划线箭头为所提出的算法对货车和轿车速度的估计结果，货车的横向估计速度为1.85m/s，纵向估计速度为0.14m/s，轿车的横向估计速度为2.815m/s，纵向估计速度为0.5m/，可见此场景下毫米波雷达的速度量测误差较大。图中虚线箭头为目标在点云位置处对应的速度估计值，通过对比可见所提出的算法能够以单帧点云数据为输入对被测车
辆的实际运动速度进行估计，提升了速度测量精度。图5为速度估计误差结果。图中点划线为货车运动速度估计误差，虚线为轿车运动速度估计误差，由图可见，所提出的算法能够较准确的对目标的实际运动状态进行估计，测试例中在轿车点云产生位置处的速度估计误差低于0.02m/s，货车的点云速度估计误差曲线中，最大值接近0.25m/s且明显高于其他数据点，故该点为错误聚类的概率极高，故在聚类中将其去除，实现了对聚类算法的矫正，提升了点云数据处理算法性能。在去除该错误数据点后，在轿车点云产生位置处的速度估计误差低于0.07m/s，证明了算法的有效性。图6为随机抽样仿真过程中货车的估计速度变化曲线，可见由于错误聚类的影响，估计速度波动性较大，而随机抽样仿真能够在一定程度上消除错误数据的影响，提升算法的性能和稳定性。图7为随机抽样仿真过程中货车的估计速度误差曲线，由于在随机过程中已经对所有数据组合进行遍历，故选取误差最低的速度（第3次随机仿真结果）输出为货车的实际速度估计值。图8为随机抽样仿真过程中轿车的估计速度变化曲线，由于轿车的点云数据中无错误分类数据的影响，故相对于图6的结果波动性有明显降低，但不合理的数据组合仍会产生较大的波动，如第9次随机仿真结果。图9为随机抽样仿真过程中轿车的估计速度误差曲线，除第9次随机仿真结果外，其余误差均低于0.2m/s，选取误差最小的随机仿真数据（第1次仿真数据）作为轿车的实际速度估计值。
43.本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种基于单帧毫米波雷达点云的运动估计方法，其特征在于，包括下述步骤：s1读取点云数据并进行数据聚类处理；s2对聚类数据进行运动学逆解、运动学正解和随机抽样仿真后实现目标运动估计；s3对目标运动估计后的数据进行数据细化分类和速度转化实现数据后处理。2.如权利要求1所述的运动估计方法，其特征在于，步骤s1具体包括：s1.1 对参数进行初始化，所述参数包括能量筛选阈值p
min
，聚类算法参数，速度估计误差阈值v
tol
，随机抽样仿真次数n
max ，当前自体运动速度v
ego
，自体运动角速度ω
ego
，雷达纵向偏移距离h，雷达横向偏移距离l，雷达初始安装角度α和速度补偿加权系数β；s1.2读取点云数据，将能量值i大于能量筛选阈值p
min
的数据保留并根据r由小到大排序后按照设定格式data
raw
存储；所述点云数据包括极坐标系下的极径r，角度θ，多普勒速度v和能量i；s1.3根据当前时刻自体速度和角速度，根据运动学原理转化至量测坐标并在进行自体运动补偿；s1.4以点云数据data为基础，调用聚类算法并根据距离将数据划分为各自独立的类从而实现点云数据聚类。3.如权利要求2所述的运动估计方法，其特征在于，步骤s1.3具体为：s1.3.1将自体运动速度、自体运动角速度转换至雷达量测坐标系；s1.3.2在所述雷达量测坐标系下对data
raw
数据进行速度补偿；步骤1.3.3：将速度补偿后的数据记为data，其中，式中符号下角标表示点云数据编号。4.如权利要求2所述的运动估计方法，其特征在于，步骤s1.4具体为：s1.4.1计算点云数据距离矩阵；其中，；s1.4.2以距离矩阵为输入对点云数据进行数据聚类处理。5.如权利要求1所述的运动估计方法，其特征在于，步骤s2具体为：s2.1读取聚类数据：读取第k个聚类中的所有数据标号，m为聚类中所有数据的个数，若m<2
则跳转至步骤s3.3，否则设置随机抽样最大次数为；s2.2随机抽样：每次抽样随机从聚类中抽取两个数据点标号，令其为，式中t代表随机抽样次数，l、m为随机抽取的聚类中数据标号且且；s2.3运动学逆解：计算第t次随机抽样下第k个聚类关联目标速度估计值；s2.4运动学正解：根据所述目标速度估计值计算并统计数据点，获得第t轮满足运动学正解速度误差小于v
tol
的数据点集合；s2.5重复步骤s2.2~s2.4直至完成次随机抽样仿真。6.如权利要求5所述的运动估计方法，其特征在于，步骤s2.3中，根据公式计算第t次随机抽样下第k个聚类关联目标速度估计值，式中为x轴方向速度估计值，为y轴方向速度估计值。7.如权利要求5所述的运动估计方法，其特征在于，步骤s2.4中，根据公式计算并统计数据点。8.如权利要求1-7任一项所述的运动估计方法，其特征在于，步骤s3具体为：步骤3.1：数据细化分类：若集合不为空，则将第k个聚类中标号属于集合的所有数据分类为运动目标，其他数据分类为噪声点，否则跳转至步骤3.3；步骤3.2：估计速度转化：将data中标号属于集合的所有数据的估计速度转化至雷达量测坐标系中；步骤3.3：令k值加1，若k≤n
total
则并跳转至步骤s2.1，否则结束并输出数据。9.一种基于单帧毫米波雷达点云的运动估计系统，其特征在于，包括依次连接的点云数据前处理单元、目标运动估计单元和数据后处理单元；所述点云数据前处理单元用于读取点云数据并进行数据聚类处理；所述目标运动估计单元用于对聚类数据进行运动学逆解、运动学正解和随机抽样仿真后实现目标运动估计；所述数据后处理单元用于对目标运动估计后的数据进行数据细化分类和速度转化实现数据后处理。

技术总结
本发明公开了一种基于单帧毫米波雷达点云的运动估计方法及系统，运动估计方法包括：S1读取点云数据并进行数据聚类处理；S2对聚类数据进行运动学逆解、运动学正解和随机抽样仿真后实现目标运动估计；S3对目标运动估计后的数据进行数据细化分类和速度转化实现数据后处理。本发明通过引入自体速度、角速度补偿，对于自身存在速度、角速度的情况，采用本发明所提出的方法能够有效的降低由于自体运动导致的被测目标速度测量误差，提升了速度测量精度。同时，通过引入了随机抽样算法，当聚类数据中存在噪声时，采用本发明所提出的方法，能够在识别出目标速度的前提下实现噪声数据筛选，提升了点云数据处理算法性能。提升了点云数据处理算法性能。提升了点云数据处理算法性能。


技术研发人员：刘杰
受保护的技术使用者：深圳安智杰科技有限公司
技术研发日：2023.09.04
技术公布日：2023/10/11