一种多传感器目标融合、追踪方法及系统与流程
本发明涉及自动驾驶领域，特别是涉及自动驾驶中多传感器的融合以及对目标的追踪。
背景技术：
随着科学技术的发展，自动驾驶、无人车等新兴概念应运而生。辅助驾驶与自动驾驶都依赖一个准确的环境感知，而对动态障碍物的高精度探测识别、跟踪则是道路场景分析与环境感知中的重要元素。通过动态障碍物感知，车辆可以实时获取周围车辆、非机动车、行人等障碍物的位置、速度、姿态等信息，作为行为规划的重要依据之一。
在对环境中的障碍物目标进行检测、跟踪的过程中，动态障碍物感知使用的传感器十分多样，包括摄像头、毫米波雷达、激光雷达等等。而为了提高精度与可靠性，经常需要结合多种传感器的结果，共同做出判断。
现有的融合方案有些仅用于辅助确认目标的存在性，例如在毫米波返回有目标的大致区域进行视觉检测；有些使用统一的框架进行融合，如基于卡尔曼滤波的框架，对不同传感器赋予不同的误差范围，并在任意一个传感器获得数据后进行序贯更新。这些方案都可以实现多传感器的数据融合，但因为只是将不同传感器等同看待，进行加权融合并滤波处理，从而得到检测结果，方法直接但效率低，因此亟待需要一种提高效率的感知检测方法。
技术实现要素：
本发明以广泛采用的摄像头和毫米波作为主要的传感器类型，提出一种摄像头与毫米波雷达的融合方法。
摄像头与人眼原理类似，利用视觉检测障碍物，并利用摄像头参数、障碍物本身先验信息估计其位置速度；毫米波雷达利用电磁波感知障碍物的存在并探测其位置速度等信息。如何将两种传感器的目标追踪信息进行高效融合并对辅助驾驶以及自动驾驶过程进行指导是本发明要解决的主要技术问题。
现有的处理方法是将不同传感器进行等同看待，并对数据进行加权处理，再进行单一的滤波处理并融合。而本发明的方法充分利用了各个传感器的特点，结合多个传感器的最优测量结果，大大提升了追踪精度。在优选的实施例中，本发明选择对摄像头传感器和毫米雷达波传感器进行信息融合。根据不同传感器的特性，对每项检测结果直接使用相对误差更低的数据进行融合，从而降低了高误差的传感器对融合效果的干扰，给出比较精确的追踪结果，从而对车辆辅助驾驶以及自动驾驶进行指导。
本发明的一个方面，是提供一种多传感器匹配融合方法，用于自动驾驶时目标追踪，所述方法包括：
步骤101，判断到达的多传感器中各数据时间是否有效；若时间有效，则进入步骤102；否则终止匹配融合；
步骤102，将所述多传感器中的至少两个有效数据进行时间、空间配准；
步骤103，判断目标匹配的准入条件，从而剔除掉不应参与目标匹配融合的对象；
步骤104，计算融合系数及误差分数；其中对于所述步骤103中满足所述准入条件的传感器数据计算融合系数，并计算每两个目标之间两个传感器间的误差分数；
步骤105，计算最优匹配；其中包括利用步骤104生成的所述误差分数的数据，生成m×n的矩阵a；所述矩阵a的各个元素为两个传感器目标的所述误差分数；利用所述矩阵a最小化所有匹配的误差分数之和，求解所述两个传感器目标的最优匹配。
优选的，所述两个传感器为摄像头和毫米波雷达。
优选的，所述步骤102中的所述时间配准是指利用算法得到两种传感器在同一时间下的结果，该结果仅用于目标匹配；所述空间配准包括通过寻找一种变换把其中一份传感器数据映射到另一份传感器数据上，使得两份所述传感器数据中对应于空间同一位置的点一一对应。
优选的，所述步骤104中所述误差分数计算公式如下：
在上式中，
在上式中，k为一个正常数，t为匹配时间。
优选的，所述步骤105中当最小化所有匹配的误差分数之和时，即为一种所述最优匹配。
优选的，所述步骤105中若m小于n，则m中每一个目标，都可以在n中找到一个互不重复的目标进行匹配，计算这m对两两对应的目标的误差分数之和；若n小于m，则n中每一个目标，都可以在m中找到一个互不重复的目标进行匹配，并计算这n对两两对应的目标的误差分数之和；若m、n相等，则可以让m、n中的目标两两配对且互不重复，计算这m或n对目标的误差分数之和；对于每一种匹配方式，都有一个对应的误差分数和。
优选的，其中所述多传感器包括摄像头和毫米波雷达；以及在执行所述多传感器匹配融合方法步骤105后，还包括：
步骤106：对跟踪目标进行状态更新；其中包括对匹配上的雷达目标点和视觉目标点进行处理：挑选不同传感器的优势结果进行跟踪目标的状态更新。
步骤107：筛选处理跟踪结果；其中包括结合实际路测数据结果对跟踪结果进行筛选，去掉误差大的所述跟踪结果。
优选的，所述步骤106中直接将视觉目标的纵向距离和纵向速度替换为雷达目标对应数据，然后再进行低通滤波和卡尔曼滤波，优化纵向速度和纵向加速度。
本发明的第二方面，是提供一种上述的融合方法在目标追踪中的应用。
本发明的第三方面，是提供一种多传感器匹配融合系统，用于自动驾驶时目标追踪；该系统包括：
数据时间判断单元，所述数据时间判断单元用于判断到达的多传感器中各数据时间是否有效；若判断时间有效，则激活一配准单元进行数据配准；否则终止匹配融合；
所述配准单元用于将多传感器中的至少两个有效数据进行时间、空间配准；
准入条件判断单元：所述准入条件单元用于判断经所述时间、空间配准后的目标匹配的准入条件，从而剔除掉不应参与目标匹配融合的对象；
融合计算单元，所述融合计算单元包括对于所述准入条件判断单元判断后满足准入条件的传感器数据计算融合系数，以及计算每两个目标之间两个传感器间的误差分数；
最优匹配计算单元，所述最优匹配计算单元用于计算最优匹配；其中包括利用所述融合计算单元计算的所述误差分数的数据，生成m×n的矩阵a；所述矩阵a的各个元素为两个传感器目标的所述误差分数；利用所述矩阵a最小化所有匹配的误差分数之和，求解所述两个传感器目标的最优匹配。
优选的，所述两个传感器为摄像头和毫米波雷达。
优选的，所述配准单元的所述时间配准是指利用算法得到两种传感器在同一时间下的结果，该结果仅用于目标匹配；所述空间配准包括通过寻找一种变换把其中一份传感器数据映射到另一份传感器数据上，使得两份所述传感器数据中对应于空间同一位置的点一一对应。
优选的，所述融合计算单元中所述误差分数计算公式如下：
在上式中，
在上式中，k为一个正常数，t为匹配时间。
优选的，所述最优匹配计算单元中当最小化所有匹配的误差分数之和时，即为一种所述最优匹配。
优选的，所述最优匹配计算单元中若m小于n，则m中每一个目标，都可以在n中找到一个互不重复的目标进行匹配，计算这m对两两对应的目标的误差分数之和；若n小于m，则n中每一个目标，都可以在m中找到一个互不重复的目标进行匹配，并计算这n对两两对应的目标的误差分数之和；若m、n相等，则可以让m、n中的目标两两配对且互不重复，计算这m或n对目标的误差分数之和；对于每一种匹配方式，都有一个对应的误差分数和。
优选的，其中所述多传感器包括摄像头和毫米波雷达；所述融合系统还包括：
目标状态更新单元，所述目标状态更新单元用于对跟踪目标进行状态更新；其中包括对匹配上的雷达目标点和视觉目标点进行处理：挑选不同传感器的优势结果进行跟踪目标的状态更新；
结果处理单元，所述结果处理单元用于筛选处理跟踪结果；其中包括结合实际路测数据结果对跟踪结果进行筛选，去掉误差大的所述跟踪结果。
优选的，所述目标状态更新单元中直接将视觉目标的纵向距离和纵向速度替换为雷达目标对应数据，然后再进行低通滤波和卡尔曼滤波，优化纵向速度和纵向加速度。
本发明的第四方面，是提供一种用于驾驶的目标追踪系统，其包括权利要求10-17任一项所述的融合系统。
本发明的优点包括如下方面但不限于以下方面:
1.本发明与现有技术相比，本发明深入考虑每种传感器的不同特性，基于每种传感器各自的物理原理、算法原理，以及实际测试中表现出的特性，设计了专门适用于摄像头与毫米波雷达的数据融合算法，并非采用传统的加权处理的融合方式，而是充分发挥传感器各自的优势，弥补不足，选择性地使用信息并更新信息。
2.在融合信息的时候，进行综合滤波处理，尽可能减少了滤波算法的误差，最终在跟踪结果的可靠性与精度上有较大提高。
3.对于传感器信息融合，现有技术一般通过极大似然作为理论基础，以加权的方式对传感器数据进行融合。而在本发明中则认为雷达的纵向距离和速度为真值，在融合的过程中以max的方式直接替换，减少后续滤波算法带来的误差。对于融合数据更新，现有技术由于在传感器融合时进行加权了，更新时一般只通过单一的滤波算法。而在本发明中，结合了低通滤波和卡尔曼滤波两种滤波优化各项融合后的数据，有效提高了最终目标跟踪的精确程度和效率。另外，使用毫米波雷达的纵向距离数据，以及视觉结果的横向距离数据，从而能尽可能地降低高误差的传感器结果对于融合结果的干扰。
附图说明
图1为本申请实施例1中一种多传感器融合进行目标跟踪的主循环示意图；
图2为本申请实施例2中一种多传感器融合进行目标跟踪的主循环示意图；
图3为本申请实施例2中一种多传感器融合进行目标跟踪的流程示意图。
具体实施方式
本发明提出了一种基于多传感器融合进行目标跟踪的方法。本申请实例通过提出一种摄像头传感器与毫米波雷达传感器对于障碍物进行目标跟踪的目标匹配与数据融合方法，实现了相对于传统方法(只将多个传感器的信息进行简单融合)所能得到的检测效果的大幅提升。在本申请实例提出的方法中，在目标跟踪的过程时，首先判断两个传感器的数据传输时间是否有效，然后将判定为有效的两个数据进行时间、空间配准，并剔除掉不应参与目标匹配融合的对象；再计算两种传感器传来的数据的融合系数，根据以上求得的融合系数计算其最优匹配；最后根据最优匹配结果对跟踪的目标状态进行更新，并根据跟踪的历史结果以及先验信息对误差较大的跟踪结果进行筛选并处理，最终实现对于动态障碍物的高精度检测和追踪。
本申请实例提供了一种多传感器融合进行目标追踪的方法。上述多传感器融合进行目标追踪的方法，可以应用于终端、服务器或者二者的结合。其中，终端可以是现有的、正在研发的或将来研发的、能够通过任何形式的有线和/或无线连接(例如，wi-fi、lan、蜂窝、同轴电缆等)实现与服务器交互的任何用户设备，包括但不限于：现有的、正在研发的或将来研发的智能手机、非智能手机、平板电脑、膝上型个人计算机、桌面型个人计算机、小型计算机、中型计算机、大型计算机等。本申请实施例中服务器可以是现有的、正在研发的或将来研发的、能够向用户提供信息推荐的应用服务的设备的一个实例。本申请的实施方式在此方面不受任何限制。
下面结合附图对本申请实施例的具体实现方式进行介绍。
首先，对本申请实施例中提供的一种多传感器融合进行目标追踪的方法进行介绍。
图1所示为本申请实施例1提供的一种多传感器融合进行目标追踪的方法的流程图，应用于辅助驾驶以及自动驾驶领域，该数据融合方法主循环在任意一个传感器数据到达时触发。本方法的实施流程图参见图1，该方法包括：
步骤101：判断到达的传感器数据时间是否有效。
本专利主要讨论的传感器融合是应用广泛的摄像头与毫米波雷达的融合，即传感器数据主要来自摄像头数据和毫米波雷达数据。设摄像头数据超时为t1，毫米波雷达数据超时为t2，摄像头数据超时阈值为τ1，毫米波雷达数据超时阈值为τ2，φ代表处理方式，则本步骤的超时判断公式如下：
如上式所示，当其中任意一个传感器数据到达时即触发整个循环。首先对于数据源传来的数据时间进行判断：如果两个数据源均超时过久则放弃匹配；若某个数据源超时，则只用另一个数据源的结果，不进行融合过程；若两个数据源传来的数据均有效，则进入步骤102。
步骤102：将两个有效数据进行时间、空间配准。
首先，将两个数据源传来的数据进行空间配准。空间配准是目标检测研究领域中的一个典型问题和技术难点，其主要内容在于比较或融合针对同一对象在不同设备或者不同条件(例如不同时间或者不同角度)下获取的图像或其他数据。在本申请实施例中，因为两个传感器得到的数据类型以及为了得到数据传感器内部自行建立的参考坐标系不一样，因此两份坐标数据不能直接利用，需要将两种传感器数据统一到同一个车体坐标系下再进行处理。具体而言，是将摄像头传感器以及毫米波传感器获取的两种针对同一目标的追踪数据，通过寻找一种变换把其中一份数据映射到另一份数据上，使得两份数据中对应于空间同一位置的点一一对应起来，从而达到信息融合的目的。这一步被称为空间配准。
在本申请实例中，空间配准主要包含以下几个步骤：
一、图像特征提取。对图像目标进行关键点检测，对于不同的别的图像障碍物，检测包围框中不同种类的关键点，为3d估计提供辅助信息；
二、图像3d位置估计。即对于视频中同一个跟踪目标，估计其在3d空间中的位置、速度、姿态等。算法利用了相机内外参，以及近大远小规律，结合卡尔曼滤波的框架，实现单目图像的3d信息估计。
三、毫米波雷达3d位置转换。通过3d仿射变换，使得毫米波雷达数据和3d图像数据在同一3d空间。
四、在同一空间内对毫米波雷达数据和图像目标数据进行匹配。
其次，将两个数据源传来的数据进行时间配准。因为两个传感器得到的数据时间不统一，时间轴上会有一定的误差，因此需要利用一些算法，例如插值算法，得到两种传感器在同一时间下的结果，该结果仅用于目标匹配，从而实现两个传感器数据的时间配对，即时间配准。
步骤103：判断目标匹配的准入条件。
在对两个传感器的数据进行融合之前，首先要判断每个传感器所追踪的目标数据的准入条件。准入条件指的是通过考虑不同传感器的特性，剔除掉不应参与目标匹配融合的对象，以免造成错误匹配。具体而言，一些应该考虑因素包括：传感器的视野范围不同，有些传感器返回的目标位于另一传感器视野之外，需要剔除；毫米波雷达经常返回栏杆、立交桥等静止目标，这些静止目标不会出现在摄像头返回的目标结果中，故与视觉结果距离、速度误差较大的毫米波雷达目标需要剔除，等等。现有技术中对于视觉传感器和毫米波雷达数据的融合并没有考虑到这两种传感器各自的特点，没有采用上述方式剔除掉不应参与目标匹配的融合的对象，对后期数据融合带来了错误。
步骤104：计算融合系数及误差分数。
对于步骤103满足准入条件的传感器数据，计算融合系数。计算每两个目标之间毫米波雷达和视觉结果的误差，考虑横纵向距离和纵向相对速度等，同时考虑历史匹配，对于之前成功匹配的目标赋予更低的误差分数，有助于目标匹配的稳定性。误差分数计算公式如下：
在上式中，
在上式中，k为一个正常数，t为匹配时间,可以看出，保持匹配的点对匹配时间越长，融合系数越低。
为了避免临时性的遮挡导致误匹配，几次失去匹配可能只会增加融合系数。融合系数的目的是为了能够让之前匹配的数据有更大的可能性匹配成功，能够有效地利用历史信息。
另外，匹配成功的判定准则是误差分数在一定范围内。此范围及阈值可以根据过去经验以及已知匹配的数据进行处理和调整。
对于摄像头跟踪到的m个目标以及毫米波雷达跟踪到的n个目标，每两个目标分别进行计算其相对误差分数，即一共能得到mn个误差分数。
步骤105：计算最优匹配。
使用最优分配算法，将分配矩阵对应元素设置为上述融合评分，求解获得目标最优匹配。具体方法如下：利用步骤104生成的误差分数的数据，生成一个m×n的矩阵a。矩阵a示意如下：
其中m代表视觉目标的数量，n代表雷达目标的数量，矩阵的各个元素为视觉目标和雷达目标的误差分数，一共有mn个元素，例如aij指的是第i个视觉目标与第j个雷达目标的误差分数，其计算公式如下：
在上式中，θij代表第i个视觉目标与第j个雷达目标的融合系数；δyij代表第i个视觉目标与第j个雷达目标的纵向距离相对误差；δxij代表第i个视觉目标与第j个雷达目标的横向距离相对误差。
最后，利用矩阵a求解视觉目标和雷达目标的一个最优匹配。使得目标函数，即所有匹配的误差分数的和的值最小，从而求得一个最优匹配。
具体而言，在本问题中，目标函数指的是，若m小于n，则m中每一个目标，都可以在n中找到一个互不重复的目标进行匹配，计算这m对两两对应的目标的误差分数之和；若n小于m，则n中每一个目标，都可以在m中找到一个互不重复的目标进行匹配，并计算这n对两两对应的目标的误差分数之和；若m、n相等，则可以让m、n中的目标两两配对且互不重复，计算这m(或者说n)对目标的误差分数之和。显然，对于每一种匹配方式，都有一个对应的误差分数和，即目标函数值。当最小化目标函数，也即最小化所有匹配的误差分数之和时，就认为找到了一种最优匹配方式。直观来看，最优匹配意味着找到一种所有视觉目标点与雷达监测目标点的监测误差分数和最小的匹配方式，从而最大限度地减小了误差，改善了目标追踪的精度。
用g表示目标函数，则目标函数的求解公式表示如下：
至此，本问题可以转化为一个最优匹配问题，也就可以利用解决最优匹配问题的经典算法进行解决，例如，可以利用匈牙利算法或者km算法进行解决。因为这属于图论经典的解决算法，因此在本发明中不再进行赘述。显然，如何将原始问题转化为一个可以解决的、且合适的最优匹配问题也是本发明的创新点之一。
实施例2为实施例1基础上，对传感器目标融合的进一步改进。可选的，在计算最优匹配的基础上增加了对跟踪目标进行状态更新的步骤以及筛选处理跟踪结果步骤。
具体如图2所示，其包括了实施例1中步骤101-步骤105。此外，该实施例2还可以包括步骤106：对跟踪目标进行状态更新。
对于步骤105求解出的最佳匹配，对匹配上的雷达目标点和视觉目标点进行处理：挑选不同传感器的优势结果进行跟踪目标的状态更新。
本发明综合考虑两种传感器的特性和优缺点，对两种数据进行不同的处理：因为毫米波雷达对于纵向距离测量具有更高的测量精确度，而摄像头传感器测量横向距离时精度更高，因此可以直接将视觉目标的纵向距离和纵向速度替换为雷达目标对应数据，然后再进行低通滤波和卡尔曼滤波，优化纵向速度和纵向加速度。
其中，使用低通滤波器(low-passfilter)滤波，可以达到容许低频信号通过，但减弱(或减少)频率高于截止频率的信号的通过的目的。对于不同滤波器而言，每个频率的信号的减弱程度不同。低通滤波器在信号处理中的作用等同于移动平均数所起的作用：通过剔除短期波动、保留长期发展趋势提供了信号的平滑形式。经过低通滤波可以保持信号的稳定性。
而卡尔曼滤波(kalmanfilter)是一种高效率的递归滤波器(自回归滤波器)，它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量变量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量变量为基础的估计方式要准。对于本发明中提到的毫米波雷达传感器来说，测量得到目标的位置、速度、加速度的测量值往往在任何时候都有噪声。而卡尔曼滤波可以利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。利用卡尔曼滤波有助于建立符合实际的监测系统，可以更好地估计运动系统的目前状态，并且更新命令。
卡尔曼滤波模型假设k时刻的真实状态是从(k-1)时刻的状态演化而来，一般的卡尔曼滤波模型符合下式：
xk＝fkxk-1+bkuk+wk
其中，fk是作用在xk-1上的状态变换模型(矩阵/矢量)；bk是作用在控制器向量uk上的输入—控制模型；wk是过程噪声，并假定其符合均值为零，协方差矩阵为qk的多元正态分布，即wk～n(0,qk)。对每个时刻k，对真实状态xk的一个测量zk满足下式：
zk＝hkxk+vk
其中hk是观测模型，它把真实状态空间映射成观测空间，vk是观测噪声，其均值为零，协方差矩阵为rk,且服从正态分布，即vk～n(0,rk)。初始状态以及每一时刻的噪声{x0,w1,…,wk,v1,…,vk}都认为是互相独立的。
经过低通滤波以及卡尔曼滤波的联合处理后，可以提高最终输出的结果的精度，减小噪声误差。将最终输出数据用下标real表示，设低通滤波器为f1，卡尔曼滤波器为f2；设视觉目标的横向距离为xvision，横向速度为wversion，雷达目标的纵向距离为yrader，纵向速度为vrader,整体融合过程用公式表示如下：
yreal＝f2{f1(yrader)}
vreal＝f2{f1(vrader)}
xreal＝f2{f1(xversion)}
wreal＝f2{f1(wversion)}
综上，与一般融合框架中使用所有结果进行融合并更新不同，本算法根据不同传感器的特性，使用相对误差更低的数据进行融合，再进行滤波处理，如上述所说，使用毫米波雷达的纵向距离数据，以及视觉结果的横向距离数据，从而尽可能地降低高误差的传感器结果对于融合结果的干扰。
可选的，还包括步骤107：筛选处理跟踪结果。
这一步是结合实际路测数据结果以及经验，对于跟踪结果进行筛选，去掉误差大的跟踪结果。
例如，对于被前方车辆严重遮挡并且没有成功匹配的目标结果，因为单纯的视觉估计对于被遮挡目标误差较大，而被遮挡的目标对于本车规划来说重要性较低，则直接将其丢弃，以避免其不准的距离、速度信息影响车辆的后续规划决策。
对于有标注的数据，可以直接知道跟踪误差；对于没有标注的数据，通过人工粗筛选可以发现误差大的跟踪结果，再进行筛选处理。通过筛选这一步骤，可以避免输出误差较大的追踪结果。
最后，结合经过筛选后的追踪结果，就可以实现对于目标的实时追踪，并更新车辆行驶的实时信息，从而达到辅助驾驶以及自动驾驶的目的。该实施例的主循环的直观逻辑流程图可参见图3。
由此可见，本申请实施例中提供了一种多传感器融合进行目标追踪的方法。在进行目标追踪时，如何处理多个传感器的数据将很大程度上影响追踪的精度。
以上所述，以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例中所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。
- 还没有人留言评论。精彩留言会获得点赞！