（雷达数据处理中的)跟踪算法(1) --- 整体&目录

墨@#≯

已于 2024-07-24 16:21:57 修改

阅读量5.8k

点赞数 47

分类专栏：自动驾驶全栈工程师的毫米波雷达部分文章标签：算法车载毫米波雷达目标跟踪

于 2024-07-24 15:53:07 首次发布

本文链接：https://blog.youkuaiyun.com/xhblair/article/details/140664767

版权

自动驾驶全栈工程师的毫米波雷达部分专栏收录该内容

66 篇文章

订阅专栏

说明

目标跟踪是雷达数据处理中核心的步骤之一，基于雷达的各项应用往往需要跟踪模块所输出的结果。比如在车载雷达领域，目标跟踪位于点云聚类和ADAS功能实现之间(关于聚类，可以参考我之前的博文[1]：（毫米波雷达数据处理中的）聚类算法(2) – DBSCAN算法及其实践-优快云博客)。跟踪模块的结果是直接给到后端的ADAS功能实现模块的，其重要性不言而喻。

针对跟踪算法，我规划了一系列的博文，试图从理论分析、仿真实践、基于数据集的实测数据处理等角度比较系统和详尽地捋清该算法。

【跟踪算法背后一整套理论体系和方法的应用范围是十分广泛的，远不限于雷达！】

Blog

20240724 本文第一次写作

一、跟踪算法概述

1.1 跟踪算法要解决问题

首先需要明确跟踪算法要做的事情：以车载雷达为例(其它应用领域可以类比)，车载雷达有两个最基本的特征：a.雷达有一定的视场角，且存在于雷达视场范围内的目标基本都会被检测到（如果不考虑遮挡或者低于分辨率的两个目标）。b.雷达有其更新率(一般为20Hz)，也即雷达每隔一段时间(50ms)会刷新一次检测结果。在上述两个特征下，不难理解的是：雷达会连续地在其视场范围内检测出诸多目标！虽然雷达每次的检测结果中会得到每个目标的距离、速度和角度，但是如果我们直接将检测的结果给到后端的功能应用模块，是可能会出问题的！主要原因有以下几个(不限于后续几点)： 1.由于各类噪声的存在，检测结果的准确性(精度)不是最优的。 2.实际应用中，在某段时间内(或某几帧下)，可能由于遮挡或者背景杂波等原因，导致雷达无法检测到目标(实际上目标还是存在的)，如果我们不对这种情况做处理，雷达的功能实现会大打折扣。 3.由于噪声或干扰的存在，检测结果中极可能出现虚假目标，如果不做处理直接给到后端的应用模块，雷达的虚警率会增加。 4.直接将检测的结果不做处理给到后端的应用模块，不方便我们对目标做一些差异化的处理(比如设定优先级等)。

跟踪模块介于雷达检测结果和后端应用之间，跟踪模块所做的事情主要就是优化检测结果，管理检测结果以使检测结果更好服务于应用。具体地：跟踪算法中的滤波操作可以帮助我们修正目标测量误差，使检测结果的准确性得到提高；跟踪算法中我们会为每个目标设计一条航迹，其航迹管理操作可以帮助我们有效关联前后帧的检测结果、有效处理间歇性目标丢失的情况、有效规避虚假目标被送入后端的情况，此外，我们也可以在航迹管理中设计一些评判目标优先级的算法，以差异化处理各目标。

在后文对跟踪算法的具体介绍中，根据“职责”的不同，我将跟踪算法细分成了三个小模块进行介绍：关联模块、滤波模块、航迹管理模块。关联模块实现现有航迹和雷达本帧检测结果之间的关联；滤波模块完成对检测结果的滤波优化；航迹管理模块完成目标航迹的建立、维护、删除等任务。不过这三个小模块并非完全独立，它们是耦合成一个有机的整体完成目标跟踪任务。

1.2 跟踪算法的历史和发展

滤波是跟踪算法的核心，跟踪算法的发展对应的其实就是滤波算法的发展。 1937年，随着世界上第一部跟踪雷达的诞生，目标跟踪技术也开始在军事领域得到应用。1940年，Wiener提出了著名的维纳滤波，该方法主要是为了解决火力控制系统精确跟踪的问题，它是目标跟踪领域的里程碑，并为现代滤波理论奠定了基础。1955年，Wax为解决以往的单个传感器只能对单个目标进行跟踪的问题，提出了多目标跟踪这个全新的概念。此后，随着科技得到发展和各个学科的逐步完善，各种跟踪系统不断发展，目标跟踪也逐渐形成了一套独立的系统理论。1960年，R.E.Kalman提出了一种基于维纳滤波理论的卡尔曼滤波方法，起先主要用于离散系统，随后又将其逐步应用到连续时间系统中，形成了较为完善的卡尔曼滤波理论。卡尔曼滤波使用最小均方误差估计目标的运动，其一经提出就倍广泛应用于各个领域。然而，卡尔曼滤波主要针对线性系统，实际情况中许多系统并不是线性的，于是扩展卡尔曼滤波(EKF)和无迹卡尔曼滤波算法(UKF)等针对非线性系统的滤波方法被相继提出。进入21世纪，不敏卡尔曼滤波和粒子滤波凭借在非线性、非高斯问题中的优势，成为目标跟踪新的发展方向。(本小节内容主要摘自[2])。

二、滤波模块

2.1 滤波算法基本概念

滤波是一个很宽泛的概念，更为大家所熟知的应该是硬件中的滤波：我们有高通、低通、带通等滤波器完成对信号频率区间的选择。此外在信号处理中，比较常见的是匹配滤波：使得输出的SNR尽可能最大，我在之前的博文中对该话题有过一点讨论[3]：PMCW体制雷达系列文章(3) – PMCW雷达之匹配滤波-优快云博客。总之，滤波要做的事情是：消除我们不想要的东西！对应到目标跟踪中的滤波，其本质上就是消除噪声对目标测量的影响，这里的噪声来源于两个方面：一是观测噪声，其来源于目标所处环境、传感器自身；二是状态噪声，状态噪声是我们想要对目标状态进行预测所带来的，换句话说，我们不做跟踪，这个噪声其实不存在… 但是我们需要通过跟踪算法抑制观测噪声，所以在滤波时我们需要同时抑制这两类噪声。

滤波的方法有很多，如：卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、粒子滤波等等。不过现阶段工程应用上落地最多的还是经典的卡尔曼滤波，本章对卡尔曼滤波进行梳理和介绍，后文的仿真和数据集实践也暂时使用卡尔曼滤波算法。(其它的滤波算法后续有机会用到再做补充吧)

网上关于卡尔曼滤波的介绍有很多，要完全弄清楚卡尔曼滤波的方方面面还是比较繁琐和困难的，(实话我自己也不是太理解清楚…),这部分的内容读者如果有比较费解的，可以找一些相关的硕博论文看看。

人们对系统的认识是逐次深入的，在上世纪40年代，对系统的认识主要来自维纳的观点，即用传递函数H(w)来沟通输入输出，这是一种黑匣子观点，该观点忽视了系统内的状态变量，或认为状态变量不可知，所以选择用函数的形式来联系输入和输出。到了60年代，卡尔曼提出：系统内有状态，有其运行规则，在运行规则当中将状态嵌入进去，让系统内部形成一个自我封闭的运动规律，这种运动规律与外界交互，产生了输入输出，这是一种白盒观点，即认为系统内部的规律可以在一定程度上被掌握，我们可以基于已知的各类知识，根据对系统状态的分析建模，得到状态方程(state equation)。所以卡尔曼滤波有两个核心的方程，系统状态方程：

（2-1）

式中, X(K)为k时刻的状态向量，F(K)表示状态转移矩阵(表征的是系统自身的运行规律)，V(k)表示状态估测噪声。

量测方程：

（2-2）

式中，Z(k)表示k时刻的量测值，H(k)表示量测矩阵，W(k)表示量测噪声。卡尔曼滤波中我们假定噪声服从高斯分布，且认为系统状态估测噪声和量测噪声相互独立，此外当目标状态方程和量测方程均为线性时，卡尔曼滤波器可以达到最优性能。

2.2 卡尔曼滤波算法流程和实现细节

在前述模型和假设的基础上，经过一系列的推导(推导的过程读者可以参考一些书籍资料[4][5])，我们可以得到关于卡尔曼滤波的一系列方程以及典型的处理流程,如下图所示：(下图为一个目标，或者说一条航迹的滤波流程)

图2.1 典型的卡尔曼滤波流程图

对上图中各公式说明如下：

为上一帧的输出状态值，为状态转移矩阵，为本帧的状态预测值。假设目标运动模型是简单的一维匀速运动，那么状态值应该是一个二维向量（包括位置值和速度值）：

（2-3）

状态转移矩阵应该是一个2*2的矩阵：

（2-4）

当目标的运动模型确定下来时，该状态转移矩阵在整个跟踪过程中不会发生改变！此时预测值是：

（2-5）

其它的如：一维匀加速运动、二维匀速运动、二维匀加速运动等目标运动模型下的状态向量和状态转移矩阵，读者可以在上述基础上简单拓展即可。

是该目标本帧下的测量值。(需要注意的是：加速度值是无法测量出来的，如果我们选用的目标运动模型是匀加速的，那么需要在后续滤波过程中需要对其中的“加速度测量值”做特殊处理)

是本帧下状态预测误差协方差矩阵的初步估测结果，是预测过程的噪声协方差矩阵，这个矩阵是初始化时给定的，在整个跟踪过程中不发生变化。

是本帧下的量测误差(又被称为新息或残差)，是量测矩阵，比如对于一维匀速运动模型：

(2-6)

该量测矩阵在跟踪过程中不发生变化！

为本帧下量测误差的协方差矩阵(新息协方差)，是量测过程中噪声的协方差矩阵，该矩阵也是初始化时给定的，在整个跟踪算法的实现过程中不会发生变化。

为卡尔曼增益，我们用该值对量测误差进行修正。

是本帧下目标最终的输出状态(也是给到后端应用模块的结果)。在最后，我们还需要更新本帧的状态预测协方差矩阵。

【小结】: 卡尔曼滤波的主要思路就是：用前一时刻的状态估计值对当前时刻状态进行预测，然后再利用当前时刻的观测数据对预测值进行校正，最后得到当前时刻状态的更新值。整个过程按照：预测 à 量测(得到当前时刻的量测值) à 校正的形式依次递推，不需要知道全部过去的数据，只利用前一时刻的状态值进行预测，然后使用当前时刻的量测值来减小噪声的影响。

理想情况下(噪声是高斯分布的，系统是线性的)，上述算法得到的滤波值

是状态的线性无偏最小方差估计。是最小均方误差矩阵。卡尔曼滤波不仅能得到状态的估计值，同时也对的得到的状态误差协方差进行误差分析，这是它的一个重要优点。

需要注意的是：卡尔曼滤波会受到量测噪声、过程噪声以及状态初始值等影响。模型的选择至关重要，若选取的模型接近实际模型，滤波结果会随着滤波过程的进行越来越接近真实值，但是，若选取的模型与实际情况有较大偏差时，滤波均方误差会随着滤波过程的进行越来越大，甚至出现滤波发散的现象，滤波发散的出现会导致整个滤波失去意义！

2.3 关于参数的初始化

卡尔曼滤波的整体流程和实现细节(公式)前面已经描述得很清楚了，按照前述流程走我们就可以完成对一条目标航迹的滤波。不过从前述流程来看，还有一个非常重要的问题需要我们解决：各参数的初始化！ 包括：状态转移矩阵和量测矩阵(不过这两矩阵是基于目标运动模型来设计的，目标运动模型确定了，这两矩阵也就定下来了)、目标的初始状态向量(我们需要基于起始时刻多帧下的量测结果计算初始状态中的速度、加速度参数)、量测过程的噪声协方差矩阵、预测过程的噪声协方差矩阵。

关于状态转移矩阵和量测矩阵的初始化在上一节中已经做过介绍。

对于目标的初始状态向量，如果我们预设目标的运动模型为一维匀加速运动，那么状态向量为一个三维向量：

(2-7)

我们可以使用最简单的两点差分法进行初始化：

(2-8)

式中，Z(1)为第2帧的目标位置测量值，v(1)和v(0)是目标前两帧的速度测量值，t是帧周期。后续的跟踪算法中，滤波器从第三帧开始工作。其它的如二维匀速、二维匀加速运动等可以此类推。

量测过程的噪声协方差其来源是传感器的测量误差，如果假设目标的运动模型是一维匀速运动，则量测过程的噪声协方差矩阵应该是：

(2-9)

式中，r11是距离测量的协方差值(也就是距离测量的方差值)，r22则是速度测量的协方差值(也即速度测量的方差值)，r12和r21是距离和速度之间的协方差，一般我们认为它们之间是相互独立的关系，所以这两值为0。量测噪声的协方差矩阵变成：

(2-10)

一般传感器都会给出其测量的精度指标(不过精度一般是指标准差？这个需要注意一下)，我们可以直接用到这里面。如果要更准确地给出该矩阵，考虑到在雷达领域，雷达测量的是相对距离和相对速度(一般传感器说明里面给出的也是相对距离和速度下的精度)！当目标的运动是二维的，那么我们可能需要再结合测量的角度以及角度测量精度把前述传感器给的精度值细分到协方差矩阵中(如果不追求那么高的精度直接把距离精度给到x和y方向也能用)。

预测过程的噪声协方差其来源主要是系统内部随机过程的不确定性。其初始值可以通过系统的动力学模型(比如雷达所在载体的运动导致的误差)和过程噪声特性来设定，一般采用经验估计或者基于工程常数进行设置。

此外，还有一个状态预测的协方差矩阵P(1|1)需要初始化，该矩阵中的元素表征的是前后状态之间的协方差。该矩阵和前述几个矩阵不同的是，状态预测的协方差矩阵会随着跟踪的进行不断变化，其初始化可以不用太过苛求精准度，因为会随着迭代的进行越来越接近真实值。甚至可以用量测误差的协方差矩阵来初始化？

【从前述对滤波流程的介绍来看，卡尔曼滤波的实现其实是比较固定的，所以对于卡尔曼滤波来说，不同算法代码之间差异性其实主要体现在目标运动模型的选择以及各参数的初始化上，这里面其实有很多细节可供探讨。本节上述对初始化方法的描述比较简单，后续找机会再做补充。】

三、关联模块

关联模块是将雷达本帧的检测结果和现有航迹进行关联：其本质就是将前后帧下的目标对应起来。关联后我们可能会有如下几种结果： 1.一部分本帧的检测结果和一部分航迹关联起来了，此时对应到前述滤波模块中，我们便可以把这些检测结果给到滤波模块进行滤波处理。 2.一部分检测结果没有关联上现有航迹，此时需要为这些目标新建航迹？如何新建？ 3.一部分航迹没有关联上本帧的检测结果，此时需要对这些航迹做何种处理？第2、3两种情况需要在航迹管理模块中做出相应处理。

关联算法最典型的就是最近邻域法。早在1973年，Singer和Sea就在前人研究工作的基础上提出了最近邻域法，最近邻域法是最早被提出的数据关联算法，经过多年的验证，已经成为一个理论上十分成熟并且很简单有效的数据关联算法。最近邻法的核心思想是：得到目标的预测位置值，然后找到离该位置的统计距离最近的量测值。

(3-1)

比如在车载雷达中，如上式所示，我们可以用相对距离和相对速度的加权来计算统计距离，表示第i个点云与第j条航迹之间的统计距离，上式中，权值α和β的选取是一个需要打磨的地方。最近邻法具有运算量小，方便操作的优点，但是也有其不适用性和不全面性，比如，在强杂波环境中，相关波门内的量测数据可能包含有很多无关点迹和虚警，或者目标运动速度比较低，或者出现航迹交叉的情况时，距离目标预测位置最近的那个量测值可能并不是目标的真实量测值，此时便很可能出现航迹误跟和漏跟的情况，从而造成目标航迹跟丢的情况。

在最近邻算法的基础上，为了提高算法性能，2005年，T.L.Song等人提出了概率最近邻域法，该算法是在最近邻域法中运用了概率论的思想，同样是将相关波门内距离预测值最近的量测值作为真实值用于更新，但是，该算法还考虑了相关波门内的量测值可能来自虚警和相关波门内没有量测值得情况，并对状态协方差的更新式进行了修改。此外还有如：全局最近邻关联算法、联合概率数据关联算法等等。不过本系列博文后续的仿真和实测数据实践中都使用最典型的最近邻法。

前文说到：最近邻法的核心思想是找到离航迹在本帧的预测值统计距离最近的量测值做关联，不过该统计距离也是需要被约束的：我们需要给该统计距离限定一个范围！如果超过这个范围，即便相隔最近也不能说这是同一个目标！(比如车载雷达的应用场景下，当前帧下某条航迹的目标位置预测值与最近的量测目标间隔十几米，或者速度相差几米/秒，那它们就不可能是一个目标，也就不应该被关联起来！)。对于车载毫米波雷达的数据关联约束，一个可行的参考如下：

表3.1 数据关联约束条件列表

约束变量	约束值
距离	abs(dR) ≤ 3m
速度	abs(dV) ≤ 2m/s

也即：我们在做数据关联时，如果某条航迹和某个点云可以被关联上，那么上表中的两个条件首先要得到满足。

四、航迹管理模块

如前所述，航迹管理部分要完成目标航迹的建立、维护、删除等任务。但是不同于前述滤波和关联模块中有比较简洁和明确的处理流程，航迹管理部分面临的问题要更多，比如： 1.满足何种条件时我们才可以为新目标新建一条航迹？ 2.满足何种条件时我们才可以判断目标已经丢失并删除其航迹？ 3.如果因内存或计算资源限制，现有航迹数量已经达到饱和，此时如果出现新的目标我们该如何处理？ 4.新建航迹时我们如何初始化各滤波器所需参数？等等。所以，航迹管理模块是整个跟踪算法中自由度最高的部分！在实际应用中我们需要结合使用场景做一些定制化的设计。

在本系列后续实践部分的博文中，我将航迹分成了四大类：”Notconfirmed”、”Normal”、”Noise”、”Lost”，其转换关系如下图所示：

图4.1 航迹状态转换关系图

航迹被新建时，其被标识为”Notconfirmed”，此时我们还不确定它对应的目标是干扰/噪声产生的，还是正常的目标，当满足特定条件时，其会被转换成”Noise”或者”Normal”，标识为”noise”的航迹会被删除，只有标识为”Normal”的航迹其输出的结果才会被送入后端的应用模块，当满足一定条件后(比如在车载雷达中，前车从雷达视野中连续消失了很多帧)，我们认为目标已经不在探测范围内，此时将这条航迹标识为”Lost”并将之删除。

关于航迹管理更多的细节我在本系列博文后面的仿真和基于数据集的实践部分给出了一些简单的示例，具体请参考后续的博文。