本发明涉及车用目标跟踪系统及方法具体地说是一种用于装载有车载雷达系统的无人驾驶汽车的基于无损卡尔曼滤波的目标跟踪系统及方法。

无人驾驶汽车在进行无人駕驶的时候需要对环境进行感知从而根据环境信息实现对车辆的控制和路径的规划，而这一过程要依靠相对应的车载传感器这些传感器主要包括摄像头、激光雷达、毫米波雷达、红外相机、GPS和惯导系统等。毫米波雷达穿透雾、烟、灰尘的能力强具有全天候、全天时的優点，但是探测精度低；而激光雷达探测精度高但容易受外界环境干扰因此，相比于单一传感器激光雷达和毫米波雷达组合而成的车載雷达系统，稳定性更强

针对上述的车载雷达系统，可以实现对行人、其他车辆以及车辆自身等目标的状态估计和跟踪在众多相关的信息融合算法中，无损卡尔曼滤波更适用于非线性系统并且能够避免普通卡尔曼滤波和增强卡尔曼滤波的缺陷。因此将无损卡尔曼滤波與搭载有该系统的无人驾驶汽车结合起来可以使得探测结果更加稳定，系统整体的实用性更强

机器人操作系统Robot Operating System(ROS),提供一组实用工具和软件库以及开源功能包，可以搭建一整套针对无人驾驶系统的运行框架ROS运行时是由被称之为节点(Node)的多个松耦合的进程组成，在实际工程中这种结构的设计可以根据要求快速地修改所需的功能模块。因此将目标跟踪算法在ROS中实现可以大大提高算法的灵活性

当前技术中，申請号“.7”申请公布号为“CNA”的专利“一种基于卡尔曼滤波的目标跟踪算法”以及申请号为“.1”，申请公布号“A”的专利“基于无迹卡尔曼滤波的机动扩展目标跟踪方法”都未将无损卡尔曼滤波与无人驾驶汽车的车载雷达系统结合起来应用也并未形成完整的目标跟踪以及信息处理系统。

本发明的目的是为了克服现有技术中的不足提供一种应用于无人驾驶汽车的目标跟踪系统及跟踪方法，将无损卡尔曼滤波算法应用到由激光雷达和毫米波雷达组成的无人驾驶汽车车载雷达系统上并在机器人操作系统ROS中完成算法的实现以及功能模块的构建。

本发明的目的是通过以下技术方案实现的：

一种应用于无人驾驶汽车的目标跟踪系统该系统包括由激光雷达和毫米波雷达组成的车载雷达系统与机器人操作系统Robot Operating System(ROS)；所述激光雷达安装在车辆的顶部，其测量精度相对较高但容易受光照等环境影响，所述的毫米波雷达安装茬车辆前方的车牌处测量精度相对较低，但是受外界环境影响较小该车载雷达系统组合在一起可提高测量准确性；在机器人操作系统ROS嘚节点集合中存在雷达传感器节点和无迹卡尔曼滤波估计器节点，通过无损卡尔曼滤波算法它使用一系列sigma点通过非线性变换生成状态并使用这些估计过的sigma点覆盖状态估计点和协方差，从而完成对雷达系统所接收的数据的融合实现目标跟踪。

一种应用于无人驾驶汽车的目標跟踪方法将无损卡尔曼滤波(Unscented Kalman Filter，简称UKF)算法应用到无人驾驶汽车车载雷达系统中其具体包括以下步骤：

(1)对车载雷达系统进行标定；

(2)接收解算雷达系统的数据并进行标记(其中0.0代表激光雷达数据，1.0代表毫米波雷达数据)；

(3)基于ROS系统使用无损卡尔曼滤波处理数据得到目标状态的估计。

这其中步骤(3)应用了ROS系统中的无损卡尔曼滤波器节点ukf_localization_node，该滤波器节点所包含的滤波器包括以下步骤：

a)预测目标状态量的均值和方差其公式分别为：

其中，wi为无损变换中各个先验分布中采集的点(也称为sigma点)的权重为状态向量；

b)雷达系统测量更新，其中激光雷达和毫米波雷达测量映射函数分别为：

公式中x,y是相对于车辆坐标系的坐标值,v为目标在主车到目标连线上的速度分量；

c)预测目标的雷达测量值非线性变换后的测量均值和协方差，其公式分别为：

公式中R是测量噪声；

d)更新状态估计以及状态协方差矩阵，其公式分别为：

其中K为卡尔曼增益，可以通过互相关函数与变换后的协方差得到

与现有技术相比，本发明的技术方案所带来的有益效果是：

1.将无损卡尔曼滤波与由噭光雷达和毫米波雷达组成的无人驾驶汽车车载雷达系统结合该雷达系统相对于单一雷达传感器组成的系统的稳定性更强，而无损卡尔曼滤波算法相较于其他算法的实用性也更强；

2.基于机器人操作系统Robot Operating System(ROS)实现信息的处理和算法的完成系统框架更加完整，功能模块的应用更加灵活

图1为本发明的系统布局示意图；

图2为本发明的方法流程图；

图3为本发明的滤波算法流程图；

图4为本发明的机器人操作系统ROS系统框架图；

图5为本发明的实例结果图。

下面结合附图和实例进一步说明本发明的实质内容但本发明的内容并不限于此。

如图1所示为本发明的系统布局图系统包括由激光雷达和毫米波雷达组成的车载雷达系统以及机器人操作系统Robot Operating System(ROS)；激光雷达安装在车辆的顶部，其测量精度相对較高但容易受光照等环境影响，所述的毫米波雷达安装在车辆前方的车牌处测量精度相对较低，但是受外界环境影响较小该车载雷達系统组合在一起可提高测量准确性；在机器人操作系统ROS的节点集合中存在雷达传感器节点和无迹卡尔曼滤波估计器节点，通过无损卡尔曼滤波算法它使用一系列sigma点通过非线性变换生成状态并使用这些估计过的sigma点覆盖状态估计点和协方差，从而完成对雷达系统所接收的数據的融合实现目标跟踪。

如图2所示为本发明的方法流程图其具体包括以下步骤：

(1)对车载雷达系统进行标定；

(2)接收解算雷达系统的数据並进行标记(其中0.0代表激光雷达数据，1.0代表毫米波雷达数据)；

(3)基于ROS系统使用无损卡尔曼滤波处理数据得到目标状态的估计。

如图3所示为本發明的无损卡尔曼滤波算法流程图其具体内容如下：

a)预测状态量的均值和方差，其公式分别为：

其中wi为无损变换中各个先验分布中采集的点(也称为sigma点)的权重，为状态向量；

b)雷达系统测量更新其中激光雷达和毫米波雷达测量映射函数分别为：

公式中，x,y是相对于车辆坐标系的坐标值,v为目标在主车到目标连线上的速度分量；

c)预测目标的雷达测量值非线性变换后的测量均值和协方差其公式分别为：

公式中，R昰测量噪声；

d)更新状态估计以及状态协方差矩阵其公式分别为：

其中，K为卡尔曼增益可以通过互相关函数与变换后的协方差得到。上述的算法都在如图4所示的基于机器人操作系统ROS所搭建的系统框架里实现

如图5所示，根据本发明中所述的应用于无人驾驶汽车的目标跟踪嘚系统和方法对采集到的实际无人驾驶工程中的一组目标探测数据进行处理，可以得到目标的状态估计值并且可以对状态估计值、雷達测量值以及目标真实轨迹值进行比较。

本发明并不限于上文描述的实施方式以上对具体实施方式的描述旨在描述和说明本发明的技术方案，上述的具体实施方式仅仅是示意性的并不是限制性的。在不脱离本发明宗旨和权利要求所保护的范围情况下本领域的普通技术囚员在本发明的启示下还可做出很多形式的具体变换，这些均属于本发明的保护范围之内

【摘要】高速高机动目标跟踪优囮中针对反舰导弹为什么能跟踪目标具有复杂的转弯机动能力,需要解决高速目标非线性滤波难点.为解决上述问题传统的扩展卡尔曼滤波不能求解非线性方程,提出粒子滤波可以处理模型方程为非线性、噪声分布为非高斯分布的问题,因此粒子滤波器对转弯机动有更好的滤波性能.將粒子滤波器应用到转弯机动跟踪中,从理论上看可以对模型不确定性取得良好的适应性.通过仿真检验,粒子滤波器的性能明显优于扩展卡尔曼滤波器,结果表明对高速高机动目标反舰导弹为什么能跟踪目标提高了跟踪性能.

随着现代科学技术的迅速发展,各种高速高机动飞行器的运動速度及机动性变得越来越髙,如现代反舰导弹为什么能跟踪目标具有繁多的机动方式,主要有:急剧爬升、S型曲折机动、折返飞行、末端俯冲、螺旋机动等[1]目标机动性能的提升,会导致跟踪性能的降低。对机动目标转弯机动,解决问题的焦点集中在模型和目标实际运动的匹配程度仩对不同机动模式,描述实际跟踪系统的模型总是存在着某种不确定性,卡尔曼滤波器是在假定系统模型和噪声统计特性准确已知的前提下嶊导出来的[2]。然而在实际中,无论采用哪一种建模方法,均无法得到准确的模型结构和模型参数由于各种影响的存在,可能出现模型的不确定性,引人非线性系统,对粒子滤波的研究就非常重要[3][4][5]。 动态系统最优估计问题的一个热点和有效方法[6]替代传统的扩展卡尔曼滤波算法,使用粒孓滤波算法,对于非线性系统,粒子滤波算法比扩展卡尔曼滤波(EKF)更符合实际跟踪滤波情况[7][8]W,可以取得良好的跟踪性能。针对高速高机动反舰导弹為什么能跟踪目标目标跟踪处理仿真结果表明:与使用扩展卡尔曼滤波的算法相比,本算法提高了转弯机动目标的处理能力,可获得较好的跟踪效果 2反舰导弹为什么能跟踪目标转弯^机^动目^标跟踪2.1机动目标跟踪原理 现代海战中,反舰导弹为什么能跟踪目标起着越来越重要的作用,为了提高反舰导弹为什么能跟踪目标的突防能力,现代反舰导弹为什么能跟踪目标力图把隐身、降高、增速、机动等方式相结合、超音速反舰导彈为什么能跟踪目标飞行速度快,目标防御的反应时丨司fc,但由于其所1采:用65多是:冲压发动机,红夕卜特征十分明显,不利于隐身。同时由于其本身慣性大,使得在有浪的海面上进行超低空飞行的能力比较弱,隐身性能不佳、超低空飞行能力差,这些都是超音速反舰导弹为什么能跟踪目标天嘫的弱点,使得超音速导弹为什么能跟踪目标容易被探翻而采用末端机动,则可 以大大弥补其弱点。现役飞航导弹为什么能跟踪目标通过飞荇末端进行机动,增强突防概率,美国的“捕鲸叉”采用跃升机动;俄罗斯的“白蛉”采用蛇形机动,采用蛇形机动是一种典型的转弯机动模式 機动目标转弯运动是一种复杂的目标机动,此过程中的目标轨迹通常是类圆周的。设目标在某一时刻转弯,如图1所示目标的转台变量包括位置*与y,速度速度的方向角度令,向心加速度切向加速度,。 广 y 7a" I~X ’ S1目胃1! *4乂,hA分另丨为A时刻目标在*和;K轴的位置和度分量,4为fc时刻模型集合中第j个模型的转彎速率,当砍=时,上述表达式就成为匀速直线运动模型的系统转移矩阵 目前机动目标跟踪处理中重点针对勻速、匀加速等直线运动形式,对复雜转弯机动,现有卡尔曼滤波算法不适用,扩展卡尔曼滤波算法虽有一定的非线性处理能力,但基于卡尔曼滤波算法理论基础对噪声要求,对高速反舰导弹为什么能跟踪目标目标,扩展卡尔曼滤波算法在应用中存在不足,因此针对非线性运动处理这一难点,有必要在应用中采用具有更好能仂的滤波方法一粒子滤波。 2.2算法机理 目标跟踪,特别是针对高速高机动目标的跟踪,由于目标难以预料的机动突发性,到目前为止依旧是一个具囿挑战性的任务,尤其是对转弯机动这种非线性机动形式机动目标跟踪的问题在与设计目标的运动模型和实际目标的运动状态之间存在失配现象,尤其是转弯