本发明涉及车用目标跟踪系统及方法，具体地说是一种用于装载有车载雷达系统的无人驾驶汽车的基于无损卡尔曼滤波的目标跟踪系统及方法。
背景技术：
无人驾驶汽车在进行无人驾驶的时候需要对环境进行感知，从而根据环境信息实现对车辆的控制和路径的规划，而这一过程要依靠相对应的车载传感器。这些传感器主要包括摄像头、激光雷达、毫米波雷达、红外相机、gps和惯导系统等。毫米波雷达穿透雾、烟、灰尘的能力强，具有全天候、全天时的优点，但是探测精度低；而激光雷达探测精度高但容易受外界环境干扰。因此，相比于单一传感器，激光雷达和毫米波雷达组合而成的车载雷达系统，稳定性更强。
针对上述的车载雷达系统，可以实现对行人、其他车辆以及车辆自身等目标的状态估计和跟踪。在众多相关的信息融合算法中，无损卡尔曼滤波更适用于非线性系统，并且能够避免普通卡尔曼滤波和增强卡尔曼滤波的缺陷。因此将无损卡尔曼滤波与搭载有该系统的无人驾驶汽车结合起来，可以使得探测结果更加稳定，系统整体的实用性更强。
机器人操作系统robotoperatingsystem(ros),提供一组实用工具和软件库以及开源功能包，可以搭建一整套针对无人驾驶系统的运行框架。ros运行时是由被称之为节点(node)的多个松耦合的进程组成，在实际工程中，这种结构的设计可以根据要求快速地修改所需的功能模块。因此将目标跟踪算法在ros中实现可以大大提高算法的灵活性。
当前技术中，申请号“201210555170.7”，申请公布号为“cn103889047a”的专利“一种基于卡尔曼滤波的目标跟踪算法”以及申请号为“201610809777.1”，申请公布号“106443661a”的专利“基于无迹卡尔曼滤波的机动扩展目标跟踪方法”都未将无损卡尔曼滤波与无人驾驶汽车的车载雷达系统结合起来应用，也并未形成完整的目标跟踪以及信息处理系统。
技术实现要素：
本发明的目的是为了克服现有技术中的不足，提供一种应用于无人驾驶汽车的目标跟踪系统及跟踪方法，将无损卡尔曼滤波算法应用到由激光雷达和毫米波雷达组成的无人驾驶汽车车载雷达系统上，并在机器人操作系统ros中完成算法的实现以及功能模块的构建。
本发明的目的是通过以下技术方案实现的：
一种应用于无人驾驶汽车的目标跟踪系统，该系统包括由激光雷达和毫米波雷达组成的车载雷达系统与机器人操作系统robotoperatingsystem(ros)；所述激光雷达安装在车辆的顶部，其测量精度相对较高，但容易受光照等环境影响，所述的毫米波雷达安装在车辆前方的车牌处，测量精度相对较低，但是受外界环境影响较小，该车载雷达系统组合在一起可提高测量准确性；在机器人操作系统ros的节点集合中存在雷达传感器节点和无迹卡尔曼滤波估计器节点，通过无损卡尔曼滤波算法它使用一系列sigma点通过非线性变换生成状态，并使用这些估计过的sigma点覆盖状态估计点和协方差，从而完成对雷达系统所接收的数据的融合，实现目标跟踪。
一种应用于无人驾驶汽车的目标跟踪方法，将无损卡尔曼滤波(unscentedkalmanfilter，简称ukf)算法应用到无人驾驶汽车车载雷达系统中，其具体包括以下步骤：
(1)对车载雷达系统进行标定；
(2)接收解算雷达系统的数据并进行标记(其中0.0代表激光雷达数据，1.0代表毫米波雷达数据)；
(3)基于ros系统使用无损卡尔曼滤波处理数据，得到目标状态的估计。
这其中，步骤(3)应用了ros系统中的无损卡尔曼滤波器节点ukf_localization_node，该滤波器节点所包含的滤波器包括以下步骤：
a)预测目标状态量的均值和方差，其公式分别为：
其中，wi为无损变换中各个先验分布中采集的点(也称为sigma点)的权重，
b)雷达系统测量更新，其中激光雷达和毫米波雷达测量映射函数分别为：
zl,k+1/k＝(x,y)t
公式中，x,y是相对于车辆坐标系的坐标值,v为目标在主车到目标连线上的速度分量；
c)预测目标的雷达测量值非线性变换后的测量均值和协方差，其公式分别为：
公式中，r是测量噪声；
d)更新状态估计以及状态协方差矩阵，其公式分别为：
xk+1/k+1＝xk+1/k-kk+1/k(zk+1-zk+1/k)
其中，k为卡尔曼增益，可以通过互相关函数与变换后的协方差得到。
与现有技术相比，本发明的技术方案所带来的有益效果是：
1.将无损卡尔曼滤波与由激光雷达和毫米波雷达组成的无人驾驶汽车车载雷达系统结合。该雷达系统相对于单一雷达传感器组成的系统的稳定性更强，而无损卡尔曼滤波算法相较于其他算法的实用性也更强；
2.基于机器人操作系统robotoperatingsystem(ros)实现信息的处理和算法的完成，系统框架更加完整，功能模块的应用更加灵活。
附图说明
图1为本发明的系统布局示意图；
图2为本发明的方法流程图；
图3为本发明的滤波算法流程图；
图4为本发明的机器人操作系统ros系统框架图；
图5为本发明的实例结果图。
具体实施方式
下面结合附图和实例进一步说明本发明的实质内容，但本发明的内容并不限于此。
如图1所示为本发明的系统布局图，系统包括由激光雷达和毫米波雷达组成的车载雷达系统以及机器人操作系统robotoperatingsystem(ros)；激光雷达安装在车辆的顶部，其测量精度相对较高，但容易受光照等环境影响，所述的毫米波雷达安装在车辆前方的车牌处，测量精度相对较低，但是受外界环境影响较小，该车载雷达系统组合在一起可提高测量准确性；在机器人操作系统ros的节点集合中存在雷达传感器节点和无迹卡尔曼滤波估计器节点，通过无损卡尔曼滤波算法它使用一系列sigma点通过非线性变换生成状态，并使用这些估计过的sigma点覆盖状态估计点和协方差，从而完成对雷达系统所接收的数据的融合，实现目标跟踪。
如图2所示为本发明的方法流程图，其具体包括以下步骤：
(1)对车载雷达系统进行标定；
(2)接收解算雷达系统的数据并进行标记(其中0.0代表激光雷达数据，1.0代表毫米波雷达数据)；
(3)基于ros系统使用无损卡尔曼滤波处理数据，得到目标状态的估计。
如图3所示为本发明的无损卡尔曼滤波算法流程图，其具体内容如下：
a)预测状态量的均值和方差，其公式分别为：
其中，wi为无损变换中各个先验分布中采集的点(也称为sigma点)的权重，
b)雷达系统测量更新，其中激光雷达和毫米波雷达测量映射函数分别为：
zl,k+1/k＝(x,y)t
公式中，x,y是相对于车辆坐标系的坐标值,v为目标在主车到目标连线上的速度分量；
c)预测目标的雷达测量值非线性变换后的测量均值和协方差，其公式分别为：
公式中，r是测量噪声；
d)更新状态估计以及状态协方差矩阵，其公式分别为：
xk+1/k+1＝xk+1/k-kk+1/k(zk+1-zk+1/k)
其中，k为卡尔曼增益，可以通过互相关函数与变换后的协方差得到。上述的算法都在如图4所示的基于机器人操作系统ros所搭建的系统框架里实现。
如图5所示，根据本发明中所述的应用于无人驾驶汽车的目标跟踪的系统和方法，对采集到的实际无人驾驶工程中的一组目标探测数据进行处理，可以得到目标的状态估计值，并且可以对状态估计值、雷达测量值以及目标真实轨迹值进行比较。
本发明并不限于上文描述的实施方式。以上对具体实施方式的描述旨在描述和说明本发明的技术方案，上述的具体实施方式仅仅是示意性的，并不是限制性的。在不脱离本发明宗旨和权利要求所保护的范围情况下，本领域的普通技术人员在本发明的启示下还可做出很多形式的具体变换，这些均属于本发明的保护范围之内。