智能车辆毫米波雷达多目标跟踪方法、装置及设备

技术领域

本发明属于智能车辆环境感知领域，具体涉及智能车辆毫米波雷达多目标跟踪方法、装置及设备。

背景技术

多目标跟踪是智能汽车环境感知的重要组成部分之一，多目标跟踪是指为了维持对多个目标当前状态的估计而对所及收到的测量信息进行处理的过程，其概念是由Wax在1955年首次提出，并由Sittler在1964年对理论框架进行了系统地整理与扩充。多目标跟踪问题主要包括：机动目标模型、数据关联、跟踪门形成、跟踪的起始与终结等。

目前应用于智能车辆毫米波雷达的多目标跟踪算法主要包括：“全局最近邻算法”、“联合概率数据关联算法”和“多假设跟踪算法”，所述三种多目标跟踪算法都能较为有效地进行毫米波雷达目标跟踪，但对于智能车辆而言，由于其感知的外部世界是非线性系统，而传统的智能车辆多目标跟踪算法主要针对的是线性系统，使其对实际系统的估计存在一定偏差；另外，在具体的工程实践中，不论是计算机还是处理传感器数据的车载处理器，在计算过程中都会存在舍入误差，这将导致在计算中关键的参数矩阵不满足算法要求，从而致使整个多目标跟踪算法失效。

发明内容

针对上述现有技术中存在的缺陷，本发明提供了一种智能车辆毫米波雷达多目标跟踪方法、装置及设备，以满足毫米波雷达多目标跟踪的准确性与稳定性要求。结合说明书附图，本发明的技术方案如下：

第一方面，本发明公开了智能车辆毫米波雷达多目标跟踪方法，所述方法步骤如下：

S110：对目标初始距离和目标初始速度进行初始化设置；

S120：根据前一时刻目标的距离与速度，利用非线性方法预测当前时刻距离与速度；

S130：获取包括跟踪目标在极坐标下的径向相对距离、极角与径向相对速度在内的毫米波雷达传感器信息；

S140：利用非线性方法，结合毫米波雷达传感器信息对当前时刻预测的目标距离与速度进行修正。

进一步地，所述智能车辆毫米波雷达多目标跟踪方法为循环跌迭代过程，所述循环跌迭代过程总中的基础变量包括：

P(k)为任意时刻k目标状态估计误差协方差；

w为服从均值为q，协方差为Q的正态分布的过程噪声；

v为服从均值为r，协方差为R的正态分布的测量噪声。

更进一步地，所述步骤S110中，初始化目标状态向量

所述步骤S120中，对前一时刻得到的目标状态估计误差协方差P(k-1)进行乔里斯基分解，得到误差协方差平方根系数S(k-1)：

P(k-1)＝S(k-1)S(k-1)

产生容积点，容积点个数为状态向量维度的两倍，所述状态向量为4维，则容积点个数为8个：

其中，[1]

选择预测状态容积点：

计算经过过程非线性函数f(·)传递后的预测状态容积点：

χ

加权计算得到包含目标的速度与位置的预测状态向量：

计算状态向量的加权中心矩阵：

对过程噪声协方差Q进行乔里斯基分解得到其平方根系数：

Q＝C

计算上述公式所述的状态向量的误差协方差平方根系数：

S(k|k-1)＝Tria([χ

所述步骤S130中，将任意时刻k某个目标i的毫米波雷达传感器信息用量测向量z

其中：

ρ为目标在极坐标下的径向相对距离；

毫米波雷达检测到的所有目标信息用Z(k)表示，即：

其中，m

任意时刻k毫米波雷达检测到的各个目标与持续跟踪的目标关联概率用β

所述步骤S140中，选择预测的量测容积点：

计算经过量测非线性函数h(·)传递后的预测量测容积点：

z

加权计算得到预测量测向量：

计算量测向量的加权中心矩阵：

对量测噪声协方差R进行乔里斯基分解得到其平方根系数：

R＝C

计算量测向量的误差协方差平方根系数：

S

计算预测量测向量与预测状态向量的加权中心矩阵：

计算预测状态向量和预测量测向量的互协方差：

P

利用所述量测向量的误差协方差以及所述互协方差，计算增益矩阵：

K(k)＝[P

计算量测向量残差：

计算包含目标距离与速度的修正状态向量：

β

对所述修正状态向量误差协方差的噪声进行乔里斯基分解得到其平方根系数：

所述修正的状态向量的误差协方差平方根系数：

第二方面，本发明公开了智能车辆毫米波雷达多目标跟踪装置，所述装置包括：

毫米波雷达目标信息获取模块，用于获取作为目标信息的毫米波雷达传感器信息，所述传感器信息包括：跟踪目标在极坐标下的径向相对距离、极角与径向相对速度；

毫米波雷达目标预处理模块，用于筛选有效的目标，所述有效目标为：距离出现在毫米波雷达视野范围内且速度在毫米波雷达检测范围之内的的目标；

目标距离与速度预测模块，用于根据前一时刻目标的距离与速度利用非线性方法预测当前时刻距离与速度；

目标距离与速度修正模块，用于根据所述的经过预处理的毫米波雷达目标的信息，利用非线性方法对所述的目标当前时刻预测的距离与速度进行修正。

进一步地，所述毫米波雷达目标预处理模块包括：

有效目标筛选单元，用于将出现在毫米波雷达视野范围外的目标与速度检测超出毫米波雷达检测范围的目标进行剔除；

目标聚合单元，用于处理大目标被毫米波雷达识别成多个目标的情况。

进一步地，所述目标距离与速度预测模块包括：

预测状态容积点生成单元，用于产生预测状态容积点、选择预测状态容积点和利用过程非线性函数进行预测状态容积点传递；

预测状态向量加权单元，用于加权得到包含目标距离和速度的预测状态向量，并计算预测状态向量误差协方差的平方根系数。

进一步地，所述目标距离与速度修正模块包括：

预测量测容积点生成单元，用于产生预测量测容积点、选择预测量测容积点和利用量测非线性函数进行预测量测容积点传递；

预测量测向量加权单元，用于加权得到预测量测向量；

修正增益计算单元，用于计算量测向量对预测向量的修正比例；

目标关联概率计算单元，用来计算毫米波雷达当前时刻检测到的目标量测向量与持续跟踪目标预测状态向量的关联概率；

修正状态向量计算单元，用于计算包含目标距离与速度的修正状态向量及修正状态向量误差协方差平方根系数。

第三方面，本发明公开了一种智能车辆毫米波雷达多目标跟踪车辆设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-3中任一所述的智能车辆毫米波雷达多目标跟踪方法。

与现有技术相比，本发明的有益效果在于：

1、本发明所述智能车辆毫米波雷达多目标跟踪方法使用了非线性模型函数代替了传统的线性模型函数，对目标的跟踪精度更高。

2、本发明所述智能车辆毫米波雷达多目标跟踪方法利用生成容积点进行加权求和，这种数值积分的方法提高了运算效率。

3、本发明所述智能车辆毫米波雷达多目标跟踪方法使用状态向量误差协方差的平方根系数进行迭代运算提高了处理器进行矩阵运算的稳定性。

4、本发明所述智能车辆毫米波雷达多目标跟踪方法简单易行，易于实施。

附图说明

图1为本发明实施例一中的智能车辆毫米波雷达多目标跟踪方法流程图；

图2为本发明实施例二中的智能车辆毫米波雷达多目标跟踪装置结构示意图；

图3为本发明实施例三中的车辆设备结构示意图；

图4为本发明所述实施例一中的智能车辆毫米波雷达多目标跟踪方法的应用场景示意图；

图5为标准卡尔曼线性多目标跟踪方法与本发明实施例一所述智能车辆毫米波雷达多目标跟踪方法对目标纵向相对距离跟踪效果比较图；

图6为标准卡尔曼线性多目标跟踪方法与本发明实施例一所述智能车辆毫米波雷达多目标跟踪方法对目标侧向相对距离跟踪效果比较图；

图7为标准卡尔曼线性多目标跟踪方法与本发明实施例一所述智能车辆毫米波雷达多目标跟踪方法对目标纵向相对速度跟踪效果比较图；

图8为标准卡尔曼线性多目标跟踪方法与本发明实施例一所述智能车辆毫米波雷达多目标跟踪方法对目标侧向相对速度跟踪效果比较图。

具体实施方式

为清楚、完整地描述本发明所述技术方案及其具体工作过程，结合说明书附图，本发明的具体实施方式如下：

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本发明的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

实施例一：

本实施例一公开了一种智能车辆毫米波雷达多目标跟踪方法，图1为本发明实施例一的智能车辆毫米波雷达多目标跟踪方法流程图，本实例可适用于车辆前方多目标跟踪过程，如图1所示，本实施例一所述智能车辆毫米波雷达多目标跟踪方法的具体步骤如下：

S110：对目标初始距离和目标初始速度进行初始化设置；

本实施例一中的智能车辆毫米波雷达多目标跟踪方法是一个循环跌迭代的过程，该过程中的基础变量包括：

P(k)为任意时刻k目标状态估计误差协方差；

w为服从均值为q，协方差为Q的正态分布的过程噪声；

v为服从均值为r，协方差为R的正态分布的测量噪声；

本步骤S110中，初始化目标状态向量

S120：根据前一时刻目标的距离与速度，利用非线性方法预测当前时刻距离与速度；

对前一时刻得到的目标状态估计误差协方差P(k-1)进行乔里斯基分解，得到误差协方差平方根系数S(k-1)：

P(k-1)＝S(k-1)S(k-1)

产生容积点，容积点个数为状态向量维度的两倍，所述的状态向量为4维，则容积点个数为8个：

其中，[1]

选择预测状态容积点：

计算经过过程非线性函数f(·)传递后的预测状态容积点：

χ

加权计算得到包含目标的速度与位置的预测状态向量：

计算公式(6)所述的状态向量的加权中心矩阵：

对过程噪声协方差Q进行乔里斯基分解得到其平方根系数：

Q＝C

计算公式(6)所述的状态向量的误差协方差平方根系数：

S(k|k-1)＝Tria([χ

S130：获取包括跟踪目标在极坐标下的径向相对距离、极角与径向相对速度在内的毫米波雷达传感器信息；

本实施例一中，将任意时刻k某个目标i的毫米波雷达传感器信息用量测向量z

其中：

ρ为目标在极坐标下的径向相对距离；

毫米波雷达检测到的所有目标信息用Z(k)表示，即：

其中，m

任意时刻k毫米波雷达检测到的各个目标与持续跟踪的目标关联概率用β

S140：利用非线性方法，结合毫米波雷达传感器信息对当前时刻预测的目标距离与速度进行修正；

选择预测的量测容积点，如前所述，容积点个数为8个：

计算经过量测非线性函数h(·)传递后的预测量测容积点：

z

加权计算得到预测量测向量：

计算公式(12)所述的量测向量的加权中心矩阵：

对量测噪声协方差R进行乔里斯基分解得到其平方根系数：

R＝C

计算公式(12)所述的量测向量的误差协方差平方根系数：

S

计算预测量测向量与预测状态向量的加权中心矩阵：

计算预测状态向量和预测量测向量的互协方差：

P

利用公式(15)所述量测向量的误差协方差以及公式(17)所述互协方差，计算增益矩阵：

K(k)＝[P

计算量测向量残差：

计算包含目标距离与速度的修正状态向量：

β

对公式(20)所述的修正状态向量误差协方差的噪声进行乔里斯基分解得到其平方根系数：

计算公式(20)所述的修正的状态向量的误差协方差平方根系数：

为了更加清楚地说明书本实施例一所述非线性多目标跟踪方法的实施效果，在如图4所示的交通场景下，以主车所承载的毫米波雷达检测范围内的目标一、目标二和目标三为例，分别通过与标准卡尔曼线性多目标跟踪方法相对比的跟踪效果比较图和表征相对跟踪精度的指标，进一步说明本发明所述非线性多目标跟踪方法的有益效果如下：

如图4所述，主车按照主车行走路线Ⅰ行驶，目标一按照目标一行走路线Ⅱ行驶，目标二按照目标二行走路线Ⅲ行驶，目标三按照目标三行走路线Ⅳ行驶；

如图5所示，在标准卡尔曼线性多目标跟踪方法与本实施例一所述非线性多目标跟踪方法对目标纵向相对距离跟踪效果比较图中：

A1为通过本实施例一所述非线性多目标跟踪方法获得到的目标一的纵向相对距离；

A2为目标一的纵向相对距离真实值；

A3为通过标准卡尔曼线性多目标跟踪方法获得到的目标一的纵向相对距离；

B1为通过本实施例一所述非线性多目标跟踪方法获得到的目标三的纵向相对距离；

B2为通过标准卡尔曼线性多目标跟踪方法获得到的目标三的纵向相对距离；

B3为目标三的纵向相对距离真实值；

C1为目标二的纵向相对距离真实值；

C2为通过标准卡尔曼线性多目标跟踪方法获得到的目标二的纵向相对距离；

C3为通过本实施例一所述非线性多目标跟踪方法获得到的目标二的纵向相对距离；

如图6所示，在标准卡尔曼线性多目标跟踪方法与本实施例一所述非线性多目标跟踪方法对目标侧向相对距离跟踪效果比较图中：

D1为通过标准卡尔曼线性多目标跟踪方法获得到的目标一的侧向相对距离；

D2为通过本实施例一所述非线性多目标跟踪方法获得到的目标一的侧向相对距离；

D3为目标一的侧向相对距离真实值；

E1为通过本实施例一所述非线性多目标跟踪方法获得到的目标二的侧向相对距离；

E2为通过标准卡尔曼线性多目标跟踪方法获得到的目标二的侧向相对距离；

E3为目标二的侧向相对距离真实值；

F1为目标三的侧向相对距离真实值；

F2为通过标准卡尔曼线性多目标跟踪方法获得到的目标三的侧向相对距离；

F3为通过本实施例一所述非线性多目标跟踪方法获得到的目标二的侧向相对距离；

通过上述图5和图6中标准卡尔曼线性多目标跟踪方法与实施例一所述非线性多目标跟踪方法对目标纵向和侧向相对距离跟踪效果比较图可知，通过本实施例一所述非线性多目标跟踪方法所获得的跟踪效果更加平滑且更加贴近目标真实的相对距离，具有稳定性好且纵向和侧向相对距离跟踪精度高的有益效果。

如图7所示，在标准卡尔曼线性多目标跟踪方法与本实施例一所述非线性多目标跟踪方法对目标纵向相对速度跟踪效果比较图中：

G1为目标三纵向相对速度真实值；

G2为通过标准卡尔曼线性多目标跟踪方法获得到的目标三的纵向相对速度；

G3为通过本实施例一所述非线性多目标跟踪方法获得到的目标三的纵向相对速度；

H1为目标一纵向相对速度真实值；

H2为通过标准卡尔曼线性多目标跟踪方法获得到的目标一的纵向相对速度；

H3为通过本实施例一所述非线性多目标跟踪方法获得到的目标一的纵向相对速度；

I1为通过标准卡尔曼线性多目标跟踪方法获得到的目标二的纵向相对速度；

I2为通过本实施例一所述非线性多目标跟踪方法获得到的目标二的纵向相对速度；

I3为目标二纵向相对速度真实值；

如图8所示，在标准卡尔曼线性多目标跟踪方法与本实施例一所述非线性多目标跟踪方法对目标侧向相对速度跟踪效果比较图中：

J1为目标一侧向相对速度真实值；

J2为通过本实施例一所述非线性多目标跟踪方法获得到的目标一的侧向相对速度；

J3为通过标准卡尔曼线性多目标跟踪方法获得到的目标一的侧向相对速度；

K1为通过标准卡尔曼线性多目标跟踪方法获得到的目标三的侧向相对速度；

K2为通过本实施例一所述非线性多目标跟踪方法获得到的目标三的侧向相对速度；

K3为目标三侧向相对速度真实值；

L1为通过本实施例一所述非线性多目标跟踪方法获得到的目标二的侧向相对速度；

L2为通过标准卡尔曼线性多目标跟踪方法获得到的目标二的侧向相对速度；

L3为目标二侧向相对速度真实值；

通过上述图7和图8中标准卡尔曼线性多目标跟踪方法与实施例一所述非线性多目标跟踪方法对目标纵向和侧向相对速度跟踪效果比较图可知，通过本实施例一所述非线性多目标跟踪方法所获得的跟踪效果更加平滑且更加贴近目标真实的相对速度，具有稳定性好且纵向和侧向相对速度跟踪精度高的有益效果。

此外，通过对比表征包括：纵向或侧向相对距离均方根误差，和纵向或侧向相对速度均方根误差在内的相对跟踪精度的指标，本发明实施例一所述非线性多目标跟踪算法比传统的标准卡尔曼线性多目标跟踪方法有在跟踪精度上有较大的提升，根据传统的标准卡尔曼线性多目标跟踪方法，与基于上述实施例一所述非线性多目标跟踪方法所获得的目标相对距离及速度均方根误差对比详见下表一：

表一

由上表一可知：

对于目标一、目标二、目标三在纵向相对距离精度上，实施例一非线性多目标跟踪算法比标准卡尔曼线性多目标跟踪方法依次分别提高了约17％、44％、31％；

对于目标一、目标二、目标三在侧向相对距离精度上，实施例一非线性多目标跟踪算法比标准卡尔曼线性多目标跟踪方法依次分别提高了约18％、45％、14％；

对于目标一、目标二、目标三在纵向相对速度精度上，实施例一非线性多目标跟踪算法比标准卡尔曼线性多目标跟踪方法依次分别提高了约30％、40％、24％；

对于目标一、目标二、目标三在侧向精度上，实施例一非线性多目标跟踪算法比标准卡尔曼线性多目标跟踪方法依次分别提高了约22％、9％、10％。

实施例二：

本实施例二公开了一种智能车辆毫米波雷达多目标跟踪装置，如图2所示，所述智能车辆毫米波雷达多目标跟踪装200具体组成结构如下：

所述智能车辆毫米波雷达多目标跟踪装200包括：

毫米波雷达目标信息获取模块210，用于获取作为目标信息的毫米波雷达传感器信息，所述传感器信息包括：跟踪目标在极坐标下的径向相对距离、极角与径向相对速度；

毫米波雷达目标预处理模块220，用于筛选有效的目标，所述有效目标为：距离出现在毫米波雷达视野范围内且速度在毫米波雷达检测范围之内的的目标；

更具体的，毫米波雷达目标预处理模块220包括：

有效目标筛选单元，用于将出现在毫米波雷达视野范围外的目标与速度检测超出毫米波雷达检测范围的目标进行剔除；

目标聚合单元，用于处理较大目标，如半挂车辆等较长的车辆等，被毫米波雷达识别成多个目标的情况。

目标距离与速度预测模块230，用于根据前一时刻目标的距离与速度利用非线性方法预测当前时刻距离与速度；

更具体的，目标距离与速度预测模块230包括：

预测状态容积点生成单元，用于产生预测状态容积点、选择预测状态容积点和利用过程非线性函数进行预测状态容积点传递；

预测状态向量加权单元，用于加权得到包含目标距离和速度的预测状态向量，并计算预测状态向量误差协方差的平方根系数。

目标距离与速度修正模块240，用于根据所述的经过预处理的毫米波雷达目标的信息，利用非线性方法对所述的目标当前时刻预测的距离与速度进行修正；

更具体的，目标距离与速度修正模块240包括：

预测量测容积点生成单元，用于产生预测量测容积点、选择预测量测容积点和利用量测非线性函数进行预测量测容积点传递；

预测量测向量加权单元，用于加权得到预测量测向量；

修正增益计算单元，用于计算量测向量对预测向量的修正比例；

目标关联概率计算单元，用来计算毫米波雷达当前时刻检测到的目标量测向量与持续跟踪目标预测状态向量的关联概率；

修正状态向量计算单元，用于计算包含目标距离与速度的修正状态向量及修正状态向量误差协方差平方根系数。

实施例三：

本实施例三公开了一种智能车辆毫米波雷达多目标跟踪车辆设备，如图3所示，所述车辆设备包括：处理器30、存储器31、输入装置32和输出装置33；设备中处理器30的数量可以是一个或者多个，图3中以一个处理器30为例；设备中的处理器30、存储器31、输入装置32和输出装置33可以通过总线或者其他方式连接，图3中以总线连接为例。

所述存储器31作为一种计算机可读存储介质，可用于存储软件程序、计算机可执行程序及模块，如本发明实施例一中的智能车辆毫米波雷达多目标跟踪方法对应的程序指令，以及本发明实施例二中的智能车辆毫米波雷达多目标跟踪装置对应的模块(例如：毫米波雷达目标信息获取模块210、毫米波雷达目标预处理模块220、目标距离与速度预测模块230、目标距离与速度修正模块240等)；

所述处理器30通过运行存储在存储器31中的软件程序、指令以及模块，从而执行设备的各种功能应用以及数据处理，即实现上述任实施例一中所述的毫米波雷达多目标跟踪方法；

所述存储器31主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序；存储数据区可存储根据设备的使用所创建的数据等；此外，存储器31可以包括高速随机存取存储器，还可以包括非易失性存储器，例如一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中，存储器31可进一步包括相对于处理器30远程设置的存储器，这些远程存储器可以通过网络连接至设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

输入装置32可用于接收输入的数字或字符信息，以及产生与设备的相应设置以及功能控制有关的键信号输入。输出装置43可包括显示屏等显示设备。

本实施例三中所述车辆设备可执行本发明实施例一提供的毫米波雷达多目标跟踪方法，具备执行方法对应的功能模块和有益效果。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

以上所述本发明的具体实施方式，并不构成对本发明保护范围的限定。任何根据本发明的技术构思所作出的各种其他相应的改变与变形，均应包含在本发明权利要求的保护范围内。