毫米波雷达的扩展目标点云聚合算法

沈 磊，刘 康

（中国计量大学 机电工程学院，浙江 杭州 310018）

0 引 言

随着时代的发展，许多4S店期望基于毫米波雷达获取展馆内客户的参观路线、热点停留区域、滞留时长等信息，实现室内多目标跟踪，提高针对性导购的效率。因此，本文选用多毫米波雷达融合的方式实现多目标行人跟踪[1-2]，在保护隐私的同时，且不易受天气、光线影响。

由于低成本毫米波雷达的天线数目少且孔径小，雷达功率低以及数字信号处理（Digital Signal Processing，DSP）和室内复杂环境的局限，限制了雷达DOA（Direction of Arrival）估计[3]的性能，点云分布离散稀疏且误差大。单人真实占地面积在0.5～1 m2，而雷达检测的扩展目标的面积可达1～3 m2。多人场景中易混淆不同扩展目标间的点云，导致聚类环节生成错误的簇，增加目标关联时误判的风险。过程中试图将径向速度和信噪比等信息融入判断（存在谐波、多径、微多普勒等信号干扰），但不能杜绝此问题发生。

前 人 通 过ZoomFFT算 法[4]、MIMO（Multiple-input Multiple-output）阵列超分辨到达角估计算法[5]、将多个连续相干处理间隔（CPIs）的信号向量结合得到大的虚拟孔径[6]、引入k空间分解格式的加权核密度（WKD）估计[7]等方式实现分辨率方面的提升。相较上述对信号滤波处理的方案，本文从点云层面入手，通过结合最小二乘法曲线拟合[8]和EKF（Extended Kalman Filter）[9-10]，设计了针对低成本毫米波雷达稀疏点云的扩展目标聚合算法。该算法有效解决了点云离散和偏差大导致的扩展目标点云混叠的问题，提高了跟踪的性能。

1 扩展目标点云聚合算法

本文使用一种低成本77 GHz毫米波雷达，视场角（Field of View，FOV）为150°，探测距离为13 m。由于单毫米波雷达的精度有限且点数不稳定（空间频谱上由于信号的误差估计过大，导致点云无法通过恒虚警的检测），可以使用多个毫米波雷达，融合分布式毫米波雷达数据来降低跟踪误差，弥补单雷达反射点不稳定的问题[11]。

首先，同时驱动多个雷达（保证同步的时间戳），在本地进行滤波后整合到同一笛卡尔坐标系下，再经过点云配准拟合算法[12]，减小测量误差和雷达标定误差，为使同时刻下的扩展目标点云团尽可能完美重合。再对融合[13]后的点云做叠帧处理，使用滑动窗口算法[14]将多帧的点云数据叠加。如上操作缓解了点云数目不稳定和扩展目标点云缺失的问题，但数据融合也使得单目标的点云团面积更大，对跟踪层仍是挑战，由此提出一种扩展目标点云聚合算法。先对多个毫米波雷达所探测到的点云进行融合和叠帧处理，在增加点云数目的同时降低单一雷达丢失目标的风险；其次通过最小二乘法曲线拟合，构建EKF的目标位置预测模型，在预测值处自适应生成波门，得到假设目标关联位置，将此位置与预测值作加权修正处理后聚合点云。图1是扩展目标点云聚合算法的流程框图。

图1 算法流程图

1.1 最小二乘法拟合

首先将各个目标过往质心的x，y分量分别对T通过最小二乘法作二阶拟合，分别得到x，y与时间T的二次函数f(x,t)，f(y,t)，对T求二次导，即可以得到x，y方向上的加速度，以此加速度分量构建EKF加速度模型。

1.2 扩展卡尔曼滤波

卡尔曼滤波器（Kalman Filter，KF）是最小均方误差线性滤波器，但通常状态方程或观测方程为非线性关系。当非线性函数在工作点附近存在各阶导数或偏导数，且迭代时工作点仅在小范围内变化时，一种解决非线性的办法就是在状态估计值处利用泰勒（Taylor）级数展开，忽略其二阶及以上的部分，得到近似线性的模型，这种方法就是扩展卡尔曼滤波[15]。毫米波雷达的观测量与运动状态量呈非线性关系，因此选用扩展卡尔曼滤波。

将拟合求得的加速度分量用于构建加速度模型的扩展卡尔曼滤波。对稳定跟踪的目标（满足生命周期的目标，排除虚影目标）进行扩展卡尔曼滤波迭代，将最终预测的目标位置输出，以下是EKF设计。

状态量：

状态方程为：

这边将加速度分量作为Bu输入量输入，而不是将ax，ay也作为状态量，好处在于：从6维状态转移矩阵变为4维，降低了维度，提高了运行效率；在多人运动时，存在交错、交汇、急停等各种非匀变速情况，运动状态复杂多变。此外，由于跟踪目标采样数目有限（帧周期长），外部输入能更快速反映测量值，使模型更快地收敛。

观测方程：

观测量：

观测量与状态量非线性转换：

雅可比矩阵：

以EKF预测位置为质心，设计distance波门函数（主要通过欧氏距离度量），将满足波门半径范围内的点云都纳入进来（若同时满足两个目标波门，取近的目标波门），并且将这些点云标记到该簇下。如果场景中存在多个真实目标，需要遍历每个目标，重复上述操作。

然后计算每个簇内所有点云的x，y均值，作为假设的目标关联位置，再用此假设的目标关联位置和EKF得到的预测值作加权修正处理，分配两个质心之间的加权系数w。令修正质心为F_p，预测质心为EKF_p，假设目标关联位置为S_p。

做加权修正的原因：由于雷达自身检测的点云偏动较大，若不经处理，轨迹十分曲折。本算法为了保证点云聚合效率的同时，兼顾运动轨迹的顺滑性。

但仅通过EKF参数设定无法同时兼顾两个指标：

1）调大Q过程噪声，使模型更相信预测值，虽能缓解雷达测量值的偏动，轨迹更平缓且更接近真实行走轨迹，但是丧失了观测轨迹的特征，无法在此预测值周围准确地捕获下一时刻的目标点云，降低了扩展目标点云聚合的效率。

2）EKF得到的目标的预测值暂未经过观测值修正，若将此预测值与假设的目标关联位置加权修正后，可以更好地捕捉扩展目标点云，提高点云聚合的效率，兼顾了轨迹的顺滑性和点云聚合的精准性。

得到了最终的聚合质心p(ax,ay)后，令各簇内的点云成员都朝各自的预测质心比例聚合。将点云与聚合质心x，y分量的差值记为Δx，Δy，聚合后点的位置记为p(nx,ny)，聚合系数为d。

2 实验结果与分析

2.1 拟合效果对比与分析

拟合过程对比了最小二乘法一次拟合、二次拟合、三次拟合的效果，如图2所示。

图2 各阶次最小二乘法拟合效果对比图

对20个过往时刻的位置的x，y分量分别对T作不同阶次的最小二乘法拟合之后，再整合绘制成轨迹。由图2可见，坐标轴刻度偏动在5 cm之间，几乎可忽略。此外，虚线是实际运动直线，点折线是观测值的原始轨迹，圆圈直线作为一阶匀速模型拟合线丢失了轨迹的特征信息，无法反映趋势；菱形曲线反映的是三阶拟合后的效果，大部分情况下更能贴合原折线，并且在8～20个位置采样点的情况下（大概经历1～2 s的运动时间），哪怕是有两个凹凸性也能记录原轨迹的特性，但不难看出，还原出的曲线过度相信观测值，与实际行驶的轨迹相比无法兼顾轨迹的顺滑性；相对于二次拟合曲线，综合了一次和三次拟合的特性，既能反映轨迹特性的同时，更接近实际行走路线，轨迹顺滑性好，并且考虑到本身偏差在厘米级，实际跟踪效果并无明显差异，因此选用二次拟合，降低模型复杂度的同时也减轻了计算负担，提高了运算效率。

另外，还测试了RANSAC拟合函数效果，迭代次数少时拟合曲线非常混乱，暂不考虑。

2.2 效果分析与展示

系统参数设定如表1所示。

表1 系统参数设定

图3是测试场景及两个雷达安装情况（一个雷达前照，一个雷达侧照），两人沿着相隔0.8 m的直线并排行走的实况图（两人实际肩距在0.5 m左右）。

图3 场景布局及测试

图4a）展示的是两个人直线行走的点云聚合前后的效果，正方形是原始点云，小点是聚合后的点云，五角星是扩展目标聚合质心。可以看出，在算法前两团点云占地面积较大，且靠近时有混叠现象。此外，从理论上看，两团点云具有相似的SNR和Doppler信息，难以区分。在算法后的点云团占地面积明显减小且扩展目标数目清晰分明。图4b）展示了算法后的双人并排直线行走（肩距0.5 m）的跟踪及轨迹效果。

图4 双人并排行走点云及跟踪效果

图5是上述双人并排行走场景的点云积累图，可以看出点云在聚合前和聚合后有较明显的区别，占地面积更小、点云更紧凑集中、目标更加分明。多人在场景下扩展目标点云也互不干扰，减轻了目标匹配的负担。

图5 双人并排行走点云积累效果图

图6、图7展示的分别是两个人间隔0.5～0.8 m并排行走的算法处理前后的跟踪轨迹效果。从图6可以看出，两个人靠近时，目标轨迹有间断和目标ID切换。这是因为点云混叠，DBSCAN聚类环节把此时刻的点云聚成了一个簇（两团点云具有相似的SNR和Doppler信息，难以区分），在目标关联时，两个目标都关联到同一个簇，因此有一个目标丢失，后来出现的黑色轨迹其实是作为新目标出现。在算法后两个目标靠近时亦能完美跟踪两个目标，轨迹清晰、效果良好，如图7所示。

图6 两人靠近后并排行走跟踪效果（算法前）

图7 两人靠近后并排行走跟踪效果（算法后）

2.3 SIAP度量器定量验证分析

单一集成空情图（Single Integrated Air Picture，SIAP）通过融合多种传感器数据得到监视空中区域内的所有目标的轨迹[14]。虽多被用于航空、军事的评估，也可用来量度多目标跟踪算法的性能。本文选取其中的完备度和状态准确度两个指标展开验证。

2.3.1 完备度

完备度[14]指的是跟踪器中的轨迹数量与实际的目标数目之比，当所有的目标都被正确跟踪时，则称该跟踪器是完备的。

在k时刻的完备度如式（11）所示，JTm(k)是k时刻跟踪器中的轨迹数量，J(k)是k时刻的真实目标数量。当真实目标被完美跟踪时，完备性Ck为1。若完备度大于1，不排除在跟踪过程中多目标交互时存在目标分裂或目标混叠后，有一个假死目标停留在原地的可能性；如果完备度小于1，可能是两个目标交互后由于混叠目标丢失。

图8、图9分别是图6、图7实况的跟踪性能测试图，代表点云聚合前后完备度指标。由图8可知，两个目标在足够靠近时，点云混叠，两个目标聚类成一个簇，两目标关联到同一个簇，导致其中一个目标缺失，后来又产生，其实此时已经更换了ID，作为一个新目标出现。在算法后，两目标行走路线清晰分明，完备度测试无误。

图8 双人并排行走完备度测试（算法前）

图9 双人并排行走完备度测试（算法后）

2.3.2 状态准确度

状态准确度[14]主要评估跟踪器中轨迹的位置精度和速度精度，本文主要评估跟踪的位置精度。选取一个人沿直线行走的实况，测试算法前后轨迹的位置精度。式（12）是位置精度量度公式：

式中：D(k)是k时刻轨迹的集合；PAn(k)是k时刻第n条轨迹到真实值的欧氏距离；NA(k)是k时刻分配给真实目标的轨迹数。

由于测试人员沿着x=-0.2直线行走，将公式简化成单一轨迹到x=-0.2的垂直距离。单人直线行走位置误差如图10所示。

图10 单人直线行走位置误差

经过多组实验数据的总结，发现位置误差峰值减小且误差跨度减小，误差从30 cm的偏差修正到15 cm的偏差，多目标间干扰减小，轨迹的曲折程度降低，对轨迹的顺滑性及跟踪性能有一定的提升。

3 结语

为了解决低成本雷达自身缺陷导致的DOA估计误差大的问题，本文提出了一种扩展目标点云聚合算法。通过最小二乘法、扩展卡尔曼滤波、加权修正等方法得到扩展目标点云团聚合的质心，向此质心比例聚合。通过实验前后的效果对比证明，此设计提高了扩展目标点云的辨识度，接近的扩展目标点云团分布紧凑且清晰，为跟踪层的目标关联奠定了良好的基础。