自动驾驶环境感知之基于毫米波雷达稀疏点云的物体检测与跟踪

1. 基于毫米波雷达稀疏点云的物体检测与跟踪

（1）处理流程

• 输入数据：输入数据为点云，它包括静态目标点和动态目标点。另外，前面所得到的速度是点的相对速度，也就是说目标相对于车辆的相对速度，还需要根据车身实际速度将相对速度转化为绝对速度。
• 静态点过滤：按照绝对速度将静态点过滤掉
• 点云聚类：将过滤后的点进行聚类，获取目标级别的信息，检测运动目标
• 目标跟踪：利用多帧检测结果采用卡尔曼滤波进行目标跟踪

（2）聚类算法：DBSCAN

​ DBSCAN：Density-Based Spatial Clustering of Applications with Noise，它是基于密度的聚类方法，对样本分布的适应能力比K-Means更好。

• 基本思路：假定类别可以通过样本分布的紧密程度决定，通过将紧密相连的样本分为一类，得到不同的聚类类别。
• 基本概念- ( ϵ , M i n P t s ) (\epsilon,MinPts) (ϵ,MinPts)：用来描述邻域的密度- ϵ \epsilon ϵ：描述了某一样本的邻域距离阈值- MinPts：描述了 ϵ − \epsilon- ϵ−邻域中的最小样本数- 核心对象：对于任一样本，其 ϵ − \epsilon- ϵ−邻域至少包含MinPts个样本
• 算法流程- 找到所有的核心对象- 对于每一个未处理的核心对象，生成新的聚类- 搜索其 ϵ − \epsilon- ϵ−邻域，将 ϵ − \epsilon- ϵ−邻域中的点加入该聚类- 如果 ϵ − \epsilon- ϵ−邻域中包含新的核心对象，则重复步骤3，直到找不到新的核心对象- 注意：距离的度量不限于点的空间距离，还可以是其他点特征，比如速度、反射强度等

（3）聚类算法：DBSCAN vs K-Means

• K-Means - 需要手工指定cluster的数量，实际中事先很难准确确定- 所有点都进行聚类，不会去除outlier- 各个方向同等重要，只适合于球形的cluster- 具有随机性，每次运行结果都不一致
• DBSCAN - 不需要指定cluster个数- 可以排除outlier- 对样本分布的适应性更好- 每次运行结果是一致的

（4）目标跟踪：卡尔曼滤波

​ 利用当前时刻具有不确定性的测量数据，结合上一时刻跟踪系统的估计值来预测当前时刻系统的状态

• 基本概念（以一维雷达测距为例，假设速度恒定）- 系统状态 x t x_t xt​：t时刻飞机的航程- 测量值 z t z_t zt​：雷达测距的结果- 系统状态的估计值 x ^ t , t \hat{x}{t,t} x^t,t​：t时刻x的估计值（根据 z t z_t zt​估计）- 系统状态的预测值 x ^ t + 1 , t \hat{x}{t+1,t} x^t+1,t​：t+1时刻x的预测值（根据速度预测）- 状态更新模型： x ^ t , t = x ^ t , t − 1 + K t ( z t − x ^ t , t − 1 ) = ( 1 − K t ) x ^ t , t − 1 + K t z t \hat{x}{t,t}=\hat{x}{t,t-1}+K_t(z_t-\hat{x}{t,t-1})=(1-K_t)\hat{x}{t,t-1}+K_tz_t x^t,t​=x^t,t−1​+Kt​(zt​−x^t,t−1​)=(1−Kt​)x^t,t−1​+Kt​zt​， K t K_t Kt​称为卡尔曼增益，用来衡量系统的不确定性。当不确定性越大时，t-1时刻的预测权重（1- K t K_t Kt​）越小，说明系统更相信t时刻的测量值 z t z_t zt​。- 系统动态模型： x ^ t , t − 1 = x ^ t − 1 , t − 1 + v Δ t \hat{x}{t,t-1}=\hat{x}{t-1,t-1}+v\Delta t x^t,t−1​=x^t−1,t−1​+vΔt， v v v是飞机的速度， Δ t \Delta t Δt是两次测量之间的时间间隔，从t-1时刻的系统状态通过系统动态模型来预测系统在t时刻的状态
• 雷达目标跟踪：单目标- 系统状态 x t , v t x_t,v_t xt​,vt​：t时刻目标的位置和速度（均为二维向量）- 测量值 z x t , z v t z_{xt},z_{vt} zxt​,zvt​：t时刻目标的位置和速度的测量值（ z x t z_{xt} zxt​来自于点云聚类中心， z v t z_{vt} zvt​来自于聚类点的速度值，可认为是聚类邻域内所有点的速度均值）- 卡尔曼增益： K t = p t , t − 1 / ( p t , t − 1 + r t ) K_t=p_{t,t-1}/(p_{t,t-1}+r_t) Kt​=pt,t−1​/(pt,t−1​+rt​)- 估计的不确定性： p t , t = ( 1 − K t ) p t , t − 1 p_{t,t}=(1-K_t)p_{t,t-1} pt,t​=(1−Kt​)pt,t−1​， p t + 1 , t = p t , t p_{t+1,t}=p_{t,t} pt+1,t​=pt,t​，初始值可以根据经验设定，作为算法参数- 测量的不确定性 r t r_t rt​来自于雷达系统参数：包括距离和角度不确定性距离不确定性：根据距离分辨率来估计，比如距离分辨率是40厘米，距离不确定性可以设置为其一半，即20厘米角度测量不确定性：根据雷达安装的不同位置，角度不确定性也不同，如下图所示：- 状态更新模型： [ x ^ t , t , v ^ t , t ] = ( 1 − K t ) [ x ^ t , t − 1 , v ^ t , t − 1 ] + K t [ z x t , z v t ] [\hat{x}{t,t},\hat{v}{t,t}]=(1-K_t)[\hat{x}{t,t-1},\hat{v}{t,t-1}]+K_t[z_{xt},z_{vt}] [x^t,t​,v^t,t​]=(1−Kt​)[x^t,t−1​,v^t,t−1​]+Kt​[zxt​,zvt​]，其中 [ x ^ t , t − 1 , v ^ t , t − 1 ] [\hat{x}{t,t-1},\hat{v}{t,t-1}] [x^t,t−1​,v^t,t−1​]表示根据 t − 1 t-1 t−1时刻状态对 t t t时刻的估计值， [ z x t , z v t ] [z_{xt},z_{vt}] [zxt​,zvt​]表示 t t t时刻的测量值- 系统动态模型： x ^ t , t − 1 = x ^ t − 1 , t − 1 + v t − 1 , t − 1 Δ t \hat{x}{t,t-1}=\hat{x}{t-1,t-1}+v_{t-1,t-1}\Delta t x^t,t−1​=x^t−1,t−1​+vt−1,t−1​Δt， v ^ t , t − 1 = v ^ t − 1 , t − 1 \hat{v}{t,t-1}=\hat{v}{t-1,t-1} v^t,t−1​=v^t−1,t−1​（假定是匀速运动）

注意：系统中测量到的速度是目标相对于雷达的径向相对速度，目标的实际运动方向不一定是目标与雷达之间的径向，因此需要先将径向相对速度转换为实际相对速度。然后再结合雷达自身速度（车身速度），得到目标的绝对速度（相对于地面的速度）。

• 雷达目标跟踪：多目标（Tracking-by-Detection） - 由聚类算法在单帧点云中得到目标输出- 提取目标的特征，包括统计特征（如点位置的均值，方差等）和运动特征（如速度和加速度等）- 根据特征计算当前帧的检测目标（detections）与已跟踪的多个目标（tracks）的相似度- 按照相似度将当前帧的detections分配给tracks- 通过卡尔曼滤波更新tracks的状态参数（如位置、速度等），在卡尔曼滤波中，detections对应每一帧的测量值，track对应系统状态