---

前言

毫米波雷达最近在多目标识别上取得了很多成果，可以应用在自动驾驶和一些隐私场景中的人员检测(例如厕所、卧室等私密环境中的人员检测)，因此本文将分享利用毫米波雷达对多目标进行识别跟踪的一个小demo，算法流程涉及基础的测速，测距测角以及静态目标滤除和CFAR，卡尔曼滤波，最近邻数据关联算法。本文将重点介绍卡尔曼滤波和最近邻数据关联算法。文章后面将展示demo效果。

---

一、卡尔曼滤波

以多个行人为例，我们可以将行人运动简化为CV(Constant Velocity, CV)模型，也就是恒加速度模型，我们利用毫米波雷达采集到行人的二维数据，因为使用的传感器为德州仪器的IWR1642，它的天线阵列只能采集水平角，无法采集俯仰角。因此仅对二维平面上的位置和速度进行估计。以下是简化的模型和公式：

1.初始化阶段

(1) 状态向量
目标的二维位置和速度构成状态向量：
${\mathbf{S}}_k=\left[\begin{array}{c}{x}_k\\ {\dot{x}}_k\\ y_k\\ {\dot{y}}_k\end{array}\right]$
其中：
$x_k$ $y_k$表示目标的二维位置。​
${\dot{x}}_k$ ${\dot{y}}_k$表示目标在 $x$和 $y$ 方向上的速度。

(2)状态转移矩阵
状态转移矩阵 $F$ 表示目标的运动模型，在恒速运动模型下为：

$\mathbf{F}=\left[\begin{array}{cccc}1& 0& \Delta t& 0\\ 0& 1& 0& \Delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

2. 预测阶段

(1) 状态预测
根据上一时刻的状态和状态转移矩阵，预测下一时刻的状态：

${\mathbf{S}}_{k|k-1}=\mathbf{F}\cdot {\mathbf{S}}_{k-1}$

预测的状态向量包括位置和速度。

(2)协方差预测
协方差矩阵随时间传播，描述了状态的不确定性：

${\mathbf{P}}_{k|k-1}=\mathbf{F}\cdot {\mathbf{P}}_{k-1}\cdot {\mathbf{F}}^{\top }+\mathbf{Q}$

3. 测量更新阶段

在测量更新阶段，通过传感器的观测数据对预测结果进行校正。
(1)观测模型*(之所以将观测模型改为极坐标是为了数据关联算法那里更好计算)*
雷达测量数据在极坐标系下表示为：

${\mathbf{z}}_k=\left[\begin{array}{c}r\\ \theta \\ v_r\end{array}\right]$

其中$r=\sqrt{x^2+y^2}$：为目标距离。

$\theta =\arctan (y/x):$为目标方位角。

$v_r=\frac{x\cdot \dot{x}+y\cdot \dot{y}}{\sqrt{x^2+y^2}}:$为目标径向速度。

(2)将状态向量映射到观测空间，定义观测矩阵$H$：

$\mathbf{H}=\left[\begin{array}{cccc}\frac{x}{r}& \frac{y}{r}& 0& 0\\ -\frac{x}{r^2}& \frac{x}{r^2}& 0& 0\\ \frac{\dot{x}y-x\dot{y}}{r^2}& \frac{\dot{y}x-\dot{x}y}{r^2}& \frac{x}{r}& \frac{y}{r}\end{array}\right]$

(3)计算卡尔曼增益 $K$，用于更新预测的状态：

${\mathbf{K}}_k={\mathbf{P}}_{k|k-1}{\mathbf{H}}^{\top }{\left({\mathbf{HP}}_{k|k-1}{\mathbf{H}}^{\top }+\mathbf{R}\right)}^{-1}$

(4)状态更新
结合测量值$z_k$和预测值，更新状态向量：

${\mathbf{S}}_k={\mathbf{S}}_{k|k-1}+{\mathbf{K}}_k\left(\begin{array}{c}{\mathbf{z}}_k-\mathbf{H}{\mathbf{S}}_{k|k-1}\end{array}\right)$

更新协方差矩阵，减少对状态的不确定性：

${\mathbf{P}}_k=\left(\mathbf{I}-{\mathbf{K}}_k\mathbf{H}\right){\mathbf{P}}_{k|k-1}$

二、数据关联算法

最近邻算法的目标是找到观测值和目标预测之间的最近邻匹配。假设有 M 个目标和 N 个观测值，算法计算每个目标与每个观测值之间的距离（或代价），然后选择距离最小的匹配对。

1.计算残差

残差（创新向量）表示观测值与目标预测值的差异：

${\mathbf{y}}_i^j={\mathbf{z}}_j-\mathbf{H}{\mathbf{S}}_i$

其中：$z_j$为第$j$个观测值
$S_i$第$i$个目标的状态预测值

2.计算马氏距离

为了度量目标和观测值的匹配度，使用马氏距离（Mahalanobis distance），定义为：

$d_{i,j}^2={\mathbf{y}}_i^{j\top }{\mathbf{S}}^{-1}{\mathbf{y}}_i^j$
$\mathbf{S}=\mathbf{HP}{\mathbf{H}}^{\top }+\mathbf{R}$

马氏距离综合考虑了观测噪声和目标预测的不确定性。

3.数据关联步骤

(1)距离矩阵计算： 构造距离矩阵$D$，其中$D_{i,j}=d_{i,j}$是第 $i$ 个目标和第 $j$ 个观测值之间的马氏距离。

(2)寻找最近邻匹配： 对每个目标 $i$，找到与其最近的观测值 $j$：
$j=\arg {\min }_j{D}_{i,j}$
其中 argmin 表示找到使距离最小的观测索引。

(3)关联门限检查： 为减少误匹配，设置关联门限 𝜏。仅当最近距离满足以下条件时，才进行关联：
$D_{i,j}<\tau$
否则认为该目标未被观测到（丢失目标）。

(4)多目标情况处理： 如果一个观测值被多个目标同时选中，只保留距离最小的关联，解除其他目标的关联。

三.效果展示

总结

以上就是今天分享的内容，本文简单介绍了毫米波雷达多目标跟踪的应用，详细给出了卡尔曼滤波和最近邻数据关联算法的公式，最后给出了实现效果图，若想要代码学习可以私聊我。