无人驾驶技术——聚类和跟踪（Radar）

clustering

为了增强自动驾驶时的感知能力，需要单独跟踪多个目标。目标跟踪计算量大，同时跟踪多个目标需要大量的处理能力和内存。

由于雷达技术的进步和传感分辨率的提高，雷达可以从目标上的大量散射点产生探测。如果一个跟踪器被分配给来自同一目标的每一个检测，那么它会使处理单元负担过重。因此，对每个目标的检测进行聚类并为每个目标分配一个单独的跟踪是很重要的。

这就是聚类算法对于成功跟踪目标变得重要的地方。

在此，我们将讨论基于欧几里得距离的基本聚类算法。这里的算法根据检测点之间的欧几里得距离测量的接近度对检测点进行分组。

所有在目标尺寸范围内的检测点被视为一个簇，合并成一个质心位置。现在，每个集群都被分配一个新的范围和速度，这是构成集群的所有检测点的测量范围和速度的平均值。

这允许对每个目标进行有效跟踪。

上面是聚类场景的一个例子。在图片中，蓝色汽车是一辆自我汽车（带传感器的车辆），检测是由橙色和黄色车辆生成的。使用聚类算法，将与单个目标相关的所有检测合并为一个点。这有助于检测和分配目标的轨迹。

for i = 1:N
    for j = i+1:N
        if detections{i}.SensorIndex == detections{j}.SensorIndex
            distances(i,j) = norm(detections{i}.Measurement(1:2) - detections{j}.Measurement(1:2))
        else
            distances(i,j) = inf;
        end
    end
end

leftTocheck = 1:N;
i=0;
detectionClusters = cell(N,1);

上面的集群实现使用以下步骤：

  1.如果检测来自同一传感器，则通过每个检测点进行循环，并测量所有检测点之间的欧氏距离。        
    2.继续运行循环，直到检测列表为空        
    在while循环中实现以下功能：
    
    3.在检查列表中选择第一个检测并检查其群集邻居。
   4. 如果第一次拾取和剩余检测之间的距离小于车辆尺寸，则对这些检测及其各自的雷达传感器测量进行分组，包括范围和速度。
   5. 对于组，取范围和速度测量值的平均值。   
    注：雷达测量矢量有6个值-其中x和y坐标的范围和速度位于指数1、2、4和5上：[x、y、-、vx、vy、-]
    6.创建一个新的群集ID。然后，将所有组检测分配给同一个ID。
    7.此外，还要指定集群、平均范围和速度。
    8.最后，从列表中删除已经分配给集群的检测。
    9.继续重复此过程，直到检测列表为空。

Tracking

卡尔曼跟踪公式：

卡尔曼滤波的目的是估计履带车辆的状态。在这里，“状态”可以包括被跟踪车辆的位置、速度、加速度或其他特性。卡尔曼滤波器使用随时间观察到的测量值，这些测量值包含噪声或随机变化和其他不精确性，并产生接近测量值的真实值及其相关计算值的值。它是大多数现代雷达跟踪系统的核心算法。

在这里，我们将保留卡尔曼滤波器的基本介绍。你将在这个纳米课程的第四个课程中详细介绍卡尔曼滤波器。

卡尔曼滤波过程有两个步骤：预测和更新。

预测步骤

利用目标车辆的运动模型，利用当前状态预测目标车辆的下一个状态。因为我们从上一个时间戳中知道目标的当前位置和速度，所以我们可以预测下一个时间戳中目标的位置。

例如，使用等速模型，目标车辆的新位置可以计算为：

更新步骤：

在这里，卡尔曼滤波器使用来自传感器的噪声测量数据，并将这些数据与上一步的预测相结合，以产生对状态的最佳估计。

Kalman Tracking and MATLAB

trackingkf类创建一个离散时间线性卡尔曼滤波器，用于跟踪自动驾驶场景中可能遇到的物体的位置和速度，如汽车、行人、自行车和静止结构或障碍物。

您可以在这里了解更多关于滤波器不同参数的信息，也可以在这里了解更多关于卡尔曼滤波器背后的理论。

在Matlab中实现

下面的指导方针可用于为下一个项目实现一个基本的卡尔曼滤波器。

您将使用trackingkf函数定义卡尔曼滤波器。函数定义如下：

filter=trackingkf（'motionmodel'，model，'state'，state，'measurementmodel'，measurementmodel，'statecovariance'，statecovariance，'measurementnoise'，measurementnoise）

在此函数中，每个属性（例如“motionmodel”）后面都是该属性（例如model）的值。

对于model变量，可以传递字符串“2D Constant Velocity”，该字符串将提供2d恒速运动模型。

对于二维恒速模型，状态向量（x）可定义为：

   [x；vx；y；vy]

这里，x和y是二维位置坐标。变量svx和vy提供二维速度。

RadarDetectionGenerator函数用于根据反射后的返回生成检测点。每个雷达检测都会生成一个检测测量和测量噪声矩阵：Detection.Measurement 和Detection.Measurement Noise。检测测量矢量（Z）的格式为[X；Y；VX；VY]。

测量模型

测量是你观察到的你的系统。测量依赖于状态向量，但并不总是与状态向量相同。测量模型假设任何时候的实际测量都与当前状态相关

Z=H*X

因此，对于上述情况，测量模型为

h=[1 0 0 0；0 0 1 0；0 1 0 0；0 0 0 0 1]

使用这个测量模型，可以从测量中导出状态。

x = H'*z
state = H'*detection.Measurement

此外，使用生成的测量噪声和测量模型定义状态协方差矩阵：

stateCovariance =H'*detection.MeasurementNoise*H

可参考
https://blog.csdn.net/nickchow005/article/details/83573212
Matlab R2017b 自动驾驶工具箱学习笔记（4）_Tutorials_Sensor Fusion Using Synthetic Radar and Vision Data