CN112731371B - 一种激光雷达与视觉融合的集成化目标跟踪系统及方法 - Google Patents

一种激光雷达与视觉融合的集成化目标跟踪系统及方法 Download PDF

Info

Publication number
CN112731371B
CN112731371B CN202011509307.6A CN202011509307A CN112731371B CN 112731371 B CN112731371 B CN 112731371B CN 202011509307 A CN202011509307 A CN 202011509307A CN 112731371 B CN112731371 B CN 112731371B
Authority
CN
China
Prior art keywords
target
fusion
laser radar
image
measurement
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011509307.6A
Other languages
English (en)
Other versions
CN112731371A (zh
Inventor
岑明
刘孟鑫
徐沾林
赵崇峻
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chongqing University of Post and Telecommunications
Original Assignee
Chongqing University of Post and Telecommunications
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Chongqing University of Post and Telecommunications filed Critical Chongqing University of Post and Telecommunications
Priority to CN202011509307.6A priority Critical patent/CN112731371B/zh
Publication of CN112731371A publication Critical patent/CN112731371A/zh
Application granted granted Critical
Publication of CN112731371B publication Critical patent/CN112731371B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/66—Radar-tracking systems; Analogous systems
    • G01S13/72—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation

Abstract

本发明请求保护一种激光雷达与视觉融合的集成化目标跟踪系统及方法。该系统包括固态激光雷达、单目视觉传感器和融合跟踪器。其中融合跟踪器包括单目视觉目标检测模块、激光雷达目标检测模块、激光雷达‑视觉融合跟踪模块和通信模块。单目视觉目标检测模块从图像中获取目标信息；激光雷达目标检测模块从点云中获取目标信息；激光雷达‑视觉融合跟踪模块对量测进行空间配准，建立目标状态模型和量测模型，通过目标状态一步预测建立跟踪门，再通过数据关联、目标状态滤波完成对图像目标和点云目标的融合跟踪。本发明提高了智能移动平台的集成度与开发效率，改善了融合跟踪结果的准确性。

Description

一种激光雷达与视觉融合的集成化目标跟踪系统及方法
技术领域
本发明属于智能移动平台环境感知技术领域，具体涉及一种激光雷达与视觉融合的集成化目标跟踪系统及方法。
背景技术
智能移动平台是一个集环境感知、动态决策与规划、行为控制与执行等多功能于一体的综合系统，智能移动平台包括智能车辆、智能机器人等平台。其中环境感知是智能移动平台安全进行决策控制的基础要求和前提条件，它能依靠自身的多种传感器实时采集智能移动平台周围的路面、障碍物信息实现决策控制。如何利用多种传感器进行数据融合，提高智能移动平台的环境感知能力具有重要意义与实用价值。
中国专利申请：激光雷达图像与摄像机图像的匹配方法及系统(申请号：CN201810801137.5)公开了一种激光雷达图像与摄像机图像的匹配方法，该系统利用卷积神经网络提取的位姿参数将激光雷达图像投影到摄像机图像上，得到激光雷达图像和摄像机图像的融合图。该系统仅仅只是完成了点云到图像的匹配，并没有后续的检测处理。中国专利申请：基于激光雷达和双目相机的车前行人检测方法及系统(申请号：CN201410841099.8)公开了一种基于激光雷达和双目相机的车前行人检测方法及系统，该系统利用激光雷达数据检测出疑似行人目标区域确定双目相机的感兴趣区域，并获得疑似行人目标距离、方位角和速度，然后双目相机利用疑似行人目标的检测，融合这二者的检测结果进行融合。该系统利用激光雷达获取疑似行人目标，容易造成漏检和误检。中国专利申请：用于自动化车辆的激光雷达和照相机数据融合(申请号：CN 201810391971.1)公开了一种融合自动化车辆的激光雷达数据和照相机数据的数据融合系统，该系统利用来自照相机的图像确定物体的反射率特征，并在物体的反射率特征使得来自激光雷达的光能中的照射到物体上并被该物体朝向激光雷达反射的光能所占的反射份数一低于份数阈值时，对激光雷达的检测特征进行调整。该系统只是利用图像的反射率特征调整激光雷达检测特征，从而获取更准确的目标方向和距离，融合比较初步，不能改善激光雷达识别效果差的结果。中国专利申请：目标探测系统和数据融合方法(申请号：CN 201910402856.4)公开了一种目标探测系统和数据融合方法，该方法主要介绍了其融合系统的硬件设计方案，但该系统使用的主要是机械旋转式激光雷达，设计的框架与本发明有所不同，而且也没有后续的目标融合算法以及数据预处理器。
本发明针对智能移动平台环境感知系统中融合目标跟踪系统的应用需求，以及针对单独的图像对目标距离估计存在较大误差和单独的激光雷达目标检测因为目标识别能力不够导致检测太多无用目标等问题，设计了一种激光雷达与视觉融合的集成化目标跟踪系统及方法。在该系统中，利用视觉传感器从图像中获取目标信息，利用激光雷达从点云中获取目标信息，利用融合跟踪器完成图像目标和点云目标的融合，得到融合后信息更加准确全面的目标，并将其发布给外部设备，如此，可提升智能移动平台的集成度与开发效率，融合跟踪结果的准确性。
发明内容
本发明旨在解决以上现有技术的问题。提出了一种激光雷达与视觉融合的集成化目标跟踪系统及方法。本发明的技术方案如下：
一种激光雷达与视觉融合的集成化目标跟踪系统，其包括：
固态激光雷达、单目视觉传感器及融合跟踪器，其中：
所述固态激光雷达用于获取并输出外部环境点云数据，所述单目视觉传感器用于获取并输出外部环境图像数据，所述融合跟踪器包括单目视觉目标检测模块、激光雷达目标检测模块、激光雷达-视觉融合跟踪模块和通信模块；
所述融合跟踪器的单目视觉目标检测模块用于从图像中获取目标信息；所述激光雷达目标检测模块用于从点云中获取目标信息；所述激光雷达-视觉融合跟踪模块首先通过将图像目标和点云目标统一到同一坐标系，然后根据目标运动状态模型和量测模型，对目标运动状态进行一步预测，建立跟踪门进行量测筛选，再对融合目标与量测目标进行数据关联，最后进行目标状态滤波完成对图像目标和点云目标的融合跟踪；所述通信模块用于输出融合目标信息；
其中，固态激光雷达、单目视觉传感器及融合跟踪器集成安装在同一个机械结构中，固态激光雷达、单目视觉传感器的相对位置固定不变。
进一步的，所述融合跟踪器的融合跟踪流程包括：
2.1单目视觉目标检测模块接收来自单目视觉传感器的原始图像信息，利用图像目标检测算法得到图像目标类型、目标边框、目标方位角信息，利用单目视觉测距原理获取单目测距信息。
2.2激光雷达目标检测模块接收来自固态激光雷达的原始点云信息，检测得到点云目标位置信息及目标长宽高信息。
2.3激光雷达-视觉融合跟踪模块接收来自单目视觉目标检测模块的从图像中获取的目标信息和来自激光雷达目标检测模块的从点云中获取的目标信息，通过将图像目标和点云目标统一到同一坐标系，建立目标状态模型和量测模型，对目标状态进行一步预测，建立跟踪门进行量测筛选，对融合目标与量测目标进行数据关联，进行目标状态滤波完成对图像目标和点云目标的融合，得到融合后的包括目标类型、位置信息及长宽高信息的融合目标。
2.4通信模块与外部设备进行通信，将融合目标结果发送给外部设备。
进一步的，所述步骤2.1中，当单目视觉目标检测模块采用基于相似三角形的单目测距方法时，假设摄像头的焦距为f，高度为H，摄像头和目标的距离为Z，目标的检测框着地点在图像上的投影是y，由相似三角形可得：
从而可得摄像头与前方目标的距离
进一步的，所述步骤2.2激光雷达目标检测模块接收来自固态激光雷达的原始点云信息，检测得到点云目标位置信息及目标长宽高信息，采用地平面拟合的方法先分割出激光雷达地面点与非地面点，具体包括：
a)种子点集的选取：选取一个种子点集，这些种子点来源于点云中高度即z值较小的点，引入最低点代表LPR，即NLPR个最低高度点的平均值来选取种子点集，LPR保证了平面拟合阶段不受测量噪声的影响，LPR被当作是整个点云P的最低点，点云P中高度在阈值Thseed内的点被当作是种子点，由这些种子点构成一个种子点集合；
b)平面拟合模型建立：地平面拟合用于分割地平面，采用如下线性模型用于平面模型估计：
ax+by+cz+d＝0 (11)
其点法式形式为：
nTw＝-d (12)
其中n＝[a,b,c]T,w＝[x,y,z]T，a,b,c，d为平面方程系数。通过初始点集的协方差矩阵C∈R3×3来求解n，从而确定一个平面，采用种子点集C∈R3作为初始点集，其协方差矩阵为：
其中，是所有点的均值，S表示点云集合，通过奇异值分解求得垂直于平面的法向量n，d可以通过代入种子点集的平均值/>(它代表属于地面的点)直接求得，/>代表属于地面的点。
进一步的，采用四邻域聚类的方法完成点云聚类，从而得到聚类后的目标：
a)建立点云栅格地图：考虑到激光雷达点云数近稠密远稀疏的特性，建立极坐标栅格地图，以激光雷达坐标原点为圆心，每个栅格的分辨率设置为θ(°)×r(cm)，对于落入栅格的点数p，满足p＞Td，其中Td为占据栅格所需点数的阈值，则将栅格标记为占据，否则标记为非占据；
b)四邻域聚类方法：首先遍历建立的每个栅格，对于已占据的栅格，先判断其上下左右四个领域是否已被占据，若已被占据，再判断是否已被打上标签，若无，则将这些栅格打上相同标签，并将其放入一个容器中，下次从该容器中选取新的栅格重复上述步骤直到容器为空，遍历下一个栅格。
进一步的，所述步骤2.3目标融合方法包括以下步骤：
3.1空间配准：设定智能移动平台坐标系，利用旋转平移变换公式将图像目标和点云目标变换到该坐标系上，将图像目标和点云目标统一到同一坐标系；
3.2系统建模：在智能移动平台坐标系上建立扩展目标运动状态模型以及图像目标和点云目标的量测模型：
其中表示k时刻目标的x位置、x速度、y位置、y速度、宽、高、信任度，/>表示k时刻图像目标的量测，N表示自然数集合，t表示图像目标类型，/>表示k时刻点云目标的量测，Fk为状态转移矩阵，Γk为噪声矩阵，/>为k时刻图像目标量测矩阵，/>为k时刻点云目标量测矩阵，ωk为k时刻过程噪声，/>为k时刻图像目标量测噪声，/>为k时刻点云目标量测噪声；
3.3目标运动状态预测：根据步骤3.2所述的目标运动状态方程及前一时刻目标的状态估计与协方差矩阵Pk-1|k-1，分别计算目标状态的一步预测值/>及一步预测误差的协方差矩阵Pk|k-1；
3.4量测筛选：设置一个二重跟踪门，首先以坐标轴原点为扇形顶点，以目标边框为扇形两边上的点建立扇形跟踪门进行筛选，然后对扇形跟踪门内的目标量测根据目标类型以及目标宽度做二次筛选，得到候选量测集
3.5数据关联：采用联合概率数据关联JPDA算法对目标轨迹预测值和候选量测/>进行关联，能同时关联图像目标量测和点云目标量测的目标为成功融合目标，其余则为未成功融合目标；未成功融合目标又分为图像检测到而激光雷达没有检测到、激光雷达检测到而图像没有检测到、以及两种传感器都没有检测到三种情况；根据关联结果对不同目标的信任度p进行修正，并根据信任度p分为融合目标、潜在目标以及可丢弃目标；
3.6目标状态滤波：根据计算的关联事件概率更新目标状态及目标量测，利用卡尔曼滤波对关联后的目标状态进行滤波，得到k时刻目标运动状态的最优估计/>与滤波误差的协方差矩阵/>
进一步的，所述3.5数据关联方法包括以下步骤：
4.1假定单目视觉传感器和固态激光雷达的量测误差统计独立，单目视觉传感器和固态激光雷达的联合事件概率为：
其中为每个传感器的事件概率，由位置、宽高、信任度三个特征对应的量测关联概率/>加权计算得到：
其中q1,q2,q3为各个特征所占权重，由具体应用场景确定；
其中对于每个特征对应的量测关联概率，是在跟踪门内计算每个量测关联事件的概率得到：
其中，映射ai表示目标t与来自于传感器i的量测之间的互联映射，Zk为直到k时刻的累计候选量测集，表示k时刻ai反应的量测与目标映射对应关系是正确的事件，表示互不相容事件/>的并，/>表示Zk下的后验概率。
4.2关联后的目标信任度修正；
4.3按照信任度对关联后的目标进行分类。
进一步的，所述步骤4.2对每一个目标，信任度的修正值p'按照以下规则设置：对于成功关联融合的目标，其信任度修正值为p'＝ω1，对未成功关联融合目标又分为图像检测到而激光雷达没有检测到、激光雷达检测到而图像没有检测到、以及两种传感器都没有检测到三种情况；对于图像检测出目标而激光雷达没有检测出目标的情况，设定一个距离阈值Td，信任度修正值设置为其中d为目标相对于坐标原点的距离；对于激光雷达检测出目标而图像没有检测出目标的情况，设定其信任度修正值p'＝ω3；对于两种传感器都没有检测到的情况，设定其信任度修正值p'＝ω4；其中ω1,ω2,ω3,ω4为信任度修正值的大小，ω1,ω2,ω3,ω4满足/>利用目标数据关联结果对关联后的目标信任度进行修正：
pk＝ap'+bpk-1，a+b＝1,p'＝ω1时,a＝1； (5)
其中pk表示k时刻目标的信任度。
进一步的，所述步骤4.3按照信任度大小，设定两个信任度阈值和/>将目标分为融合目标、潜在目标以及可丢弃目标：
融合目标表示该目标的信任度符合融合结果的期望值，可以通过通信模块对外发布；潜在目标表示该目标的信任度略低于融合结果的期望值，但差距不大，不发布这类目标，但保留在系统中继续做后续关联；可丢弃目标表示该目标的信任度远低于融合结果的期望值，视该目标已消失，终止对该目标的跟踪。
一种基于所述系统的跟踪方法，其包括以下步骤：
固态激光雷达获取并输出外部环境点云数据，单目视觉传感器获取并输出外部环境图像数据；
单目视觉目标检测模块从图像中获取目标信息；
激光雷达目标检测模从点云中获取目标信息；
激光雷达-视觉融合跟踪模块首先通过将图像目标和点云目标统一到同一坐标系，然后根据目标运动状态模型和量测模型，对目标运动状态进行一步预测，建立跟踪门进行量测筛选，再对融合目标与量测目标进行数据关联，最后进行目标状态滤波完成对图像目标和点云目标的融合跟踪；通信模块输出融合目标信息。
本发明的优点及有益效果如下：
本发明提出一种激光雷达与视觉融合的集成化目标跟踪系统及方法，该系统将固态激光雷达、单目视觉传感器及融合跟踪器集成为一个融合目标跟踪系统，实现了智能移动平台目标融合跟踪系统的集成化，可以改善现有智能移动平台上各类传感器彼此独立、需要开发者耗费大量时间以及人力资源进行标定、配准、融合等工作的情况，开发者可以将本系统快速集成到智能移动平台中，提高了智能移动平台的开发效率、可靠性。本发明还通过引入目标信任度，实现对目标类型的细分，对环境中的目标提供更充分的认知，从而改善了融合跟踪结果的准确性。
附图说明
图1是本发明提供优选实施例一种激光雷达与视觉融合的集成化目标跟踪系统及方法的总体框架；
图2是本发明提出的融合跟踪器的流程图；
图3是本发明提出的目标融合方法；
图4是本发明提出的融合目标与量测目标之间的数据关联方法；
图5表示基于相似三角形的单目测距示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。
本发明解决上述技术问题的技术方案是：
本发明针对智能移动平台环境感知系统中融合目标跟踪系统的应用需求，以及针对单独的图像对目标距离估计存在较大误差和单独的激光雷达目标检测因为目标识别能力不够导致检测太多无用目标等问题，设计了一种激光雷达与视觉融合的集成化目标跟踪系统及方法。
本发明解决上述技术问题的技术方案是：
以下结合附图和具体实例对本发明的具体实施方式进行描述。
图1所示为激光雷达与视觉融合的集成化目标跟踪系统及方法的总体框架，该框架如下所述：
所述固态激光雷达用于获取并输出外部环境点云数据，所述单目视觉传感器用于获取并输出外部环境图像数据，所述融合跟踪器包括单目视觉目标检测模块、激光雷达目标检测模块、激光雷达-视觉融合跟踪模块和通信模块。
所述融合跟踪器的单目视觉目标检测模块用于从图像中获取目标信息；所述激光雷达目标检测模块用于从点云中获取目标信息；所述激光雷达-视觉融合跟踪模块首先通过将图像目标和点云目标统一到同一坐标系，然后根据目标运动状态模型和量测模型，对目标运动状态进行一步预测，建立跟踪门进行量测筛选，再对融合目标与量测目标进行数据关联，最后进行目标状态滤波完成对图像目标和点云目标的融合跟踪；所述通信模块用于输出融合目标信息。
其中，固态激光雷达、单目视觉传感器及融合跟踪器集成安装在同一个机械结构中，固态激光雷达、单目视觉传感器的相对位置固定不变。
图2所示为融合跟踪器的流程图，该流程包含如下步骤：
(1)单目视觉目标检测模块接收来自单目视觉传感器的原始图像信息。
(2)单目视觉目标检测模块利用图像目标检测算法得到图像目标类型、目标边框、目标方位角信息，实施例采用包括但不限于模板匹配方法、深度神经网络方法等。单目视觉目标检测模块利用单目视觉测距原理获取单目测距信息。实施例采用包括但不限于基于相似三角形的单目测距方法。
(3)激光雷达目标检测模块接收来自固态激光雷达的原始点云信息。
(4)激光雷达目标检测模块检测得到点云目标位置信息及目标长宽高信息。实施例采用包括但不限于地平面拟合的方法先分割出激光雷达地面点与非地面点：
a)种子点集的选取：选取一个种子点集，这些种子点来源于点云中高度(即z值)较小的点。引入最低点代表(LPR)，即NLPR个最低高度点的平均值来选取种子点集，LPR保证了平面拟合阶段不受测量噪声的影响，LPR被当作是整个点云P的最低点，点云P中高度在阈值Thseed内的点被当作是种子点，由这些种子点构成一个种子点集合。
b)平面拟合模型建立：地平面拟合用于分割地平面，采用包括但不限于如下线性模型用于平面模型估计：
ax+by+cz+d＝0 (11)
即：
nTw＝-d (12)
其中n＝[a,b,c]T,w＝[x,y,z]T，通过初始点集的协方差矩阵C∈R3×3来求解n，从而确定一个平面，采用种子点集C∈R3作为初始点集，其协方差矩阵为：
其中，是所有点的均值，S为点云集合。通过奇异值分解求得垂直于平面的法向量n，d可以通过代入种子点集的平均值/>(它代表属于地面的点)直接求得。
然后实施例采用包括但不限于四邻域聚类的方法完成点云聚类，从而得到聚类后的目标：
a)建立点云栅格地图：考虑到激光雷达点云数近稠密远稀疏的特性，建立极坐标栅格地图。以激光雷达坐标原点为圆心，每个栅格的分辨率设置为θ(°)×r(cm)，对于落入栅格的点数p，满足p＞Td，其中Td为占据栅格所需点数的阈值，则将栅格标记为占据，否则标记为非占据。
b)四邻域聚类方法：首先遍历建立的每个栅格，对于已占据的栅格，先判断其上下左右四个领域是否已被占据，若已被占据，再判断是否已被打上标签，若无，则将这些栅格打上相同标签，并将其放入一个容器中，下次从该容器中选取新的栅格重复上述步骤直到容器为空，遍历下一个栅格。
(5)激光雷达-视觉融合跟踪模块接收来自单目视觉目标检测模块的从图像中获取的目标信息和来自激光雷达目标检测模块的从点云中获取的目标信息，通过将图像目标和点云目标统一到同一坐标系，建立目标状态模型和量测模型，对目标状态进行一步预测，建立跟踪门进行量测筛选，对融合目标与量测目标进行数据关联，进行目标状态滤波完成对图像目标和点云目标的融合，得到融合后的包括目标类型、位置信息及长宽高信息的融合目标。
(6)通信模块与外部设备进行通信，将融合目标结果发送给外部设备。
图3所示为目标融合方法，该方法包含以下步骤：
(1)空间配准。设定智能移动平台坐标系，利用旋转平移变换公式将图像目标和点云目标变换到该坐标系上，将图像目标和点云目标统一到同一坐标系。
(2)系统建模。在智能移动平台坐标系上建立扩展目标运动状态模型以及图像目标和点云目标的量测模型：
其中表示k时刻目标的x位置、x速度、y位置、y速度、宽、高、信任度，/>表示k时刻图像目标的量测，t表示图像目标类型，表示k时刻点云目标的量测。Fk为状态转移矩阵，Γk为噪声矩阵，/>为k时刻图像目标量测矩阵，/>为k时刻点云目标量测矩阵，ωk为k时刻过程噪声，/>为k时刻图像目标量测噪声，/>为k时刻点云目标量测噪声。
(3)目标运动状态预测。根据步骤(2)所述的目标运动状态方程及前一时刻目标的状态估计与协方差矩阵Pk-1|k-1，分别计算目标状态的一步预测值/>及一步预测误差的协方差矩阵Pk|k-1。
(4)量测筛选。设置一个二重跟踪门，首先以坐标轴原点为扇形顶点，以目标边框为扇形两边上的点建立扇形跟踪门进行筛选，然后对扇形跟踪门内的目标量测根据目标类型以及目标宽度做二次筛选，得到候选量测集
(5)数据关联。采用包括但不限于联合概率数据关联(Joint Probabilistic DataAssociation，JPDA)算法对目标轨迹预测值和候选量测/>进行关联。能同时关联图像目标量测和点云目标量测的目标为成功融合目标，其余则为未成功融合目标。未成功融合目标又分为图像检测到而激光雷达没有检测到、激光雷达检测到而图像没有检测到、以及两种传感器都没有检测到三种情况。根据关联结果对不同目标的信任度p进行修正，并根据信任度p分为融合目标、潜在目标以及可丢弃目标。
(6)目标状态滤波。根据计算的关联事件概率更新目标状态及目标量测，利用卡尔曼滤波对关联后的目标状态进行滤波，得到k时刻目标运动状态的最优估计/>与滤波误差的协方差矩阵/>
图4所示为融合目标与量测目标之间的数据关联方法，该方法包含以下步骤：
(1)计算每个传感器量测的关联事件概率。每个传感器量测的关联事件概率由位置、宽高、信任度三个特征对应的量测关联概率/>加权计算得到：
其中q1,q2,q3为各个特征所占权重，由具体应用场景确定。
其中对于每个特征对应的量测关联概率，是在跟踪门内计算每个量测关联事件的概率得到：
其中，映射ai表示目标t与来自于传感器i的量测之间的互联映射，Zk为直到k时刻的累计候选量测集，表示k时刻ai反应的量测与目标映射对应关系是正确的事件，表示互不相容事件/>的并，/>表示Zk下的后验概率。
(2)计算联合事件概率。假定单目视觉传感器和固态激光雷达的量测误差是统计独立的。则单目视觉传感器和固态激光雷达的联合事件概率
(3)关联后的目标信任度修正。对每一个目标，信任度的修正值p'按照以下规则设置：对于成功关联融合的目标，其信任度修正值为p'＝ω1。对未成功关联融合目标又分为图像检测到而激光雷达没有检测到、激光雷达检测到而图像没有检测到、以及两种传感器都没有检测到三种情况。对于图像检测出目标而激光雷达没有检测出目标的情况，设定一个距离阈值Td，信任度修正值设置为其中d为目标相对于坐标原点的距离；对于激光雷达检测出目标而图像没有检测出目标的情况，设定其信任度修正值p'＝ω3；对于两种传感器都没有检测到的情况，设定其信任度修正值p'＝ω4。其中ω1,ω2,ω3,ω4为信任度修正值的大小，ω1,ω2,ω3,ω4满足/>利用目标数据关联结果对关联后的目标信任度进行修正：
pk＝ap'+bpk-1(a+b＝1,p'＝ω1时,a＝1) (5)
其中pk表示k时刻目标的信任度。
(4)按照信任度对关联后的目标进行分类。按照信任度大小，设定两个信任度阈值和/>将目标分为融合目标、潜在目标以及可丢弃目标：
融合目标表示该目标的信任度符合融合结果的期望值，可以通过通信模块对外发布；潜在目标表示该目标的信任度略低于融合结果的期望值，但差距不大，不发布这类目标，但保留在系统中继续做后续关联；可丢弃目标表示该目标的信任度远低于融合结果的期望值，视该目标已消失，终止对该目标的跟踪。
如图5所示为单目视觉目标检测模块基于相似三角形的单目测距示意图。实施例采用包括但不限于基于相似三角形的单目测距方法，假设本移动平台为A，前方目标为B，摄像头P的焦距为f，高度为H，摄像头和障碍物的距离为Z，目标B的检测框着地点在图像上的投影是y。那么由相似三角形可得：
从而可得摄像头与前方目标B的距离
以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (3)

1.一种激光雷达与视觉融合的集成化目标跟踪系统，其特征在于，包括：
固态激光雷达、单目视觉传感器及融合跟踪器，其中：
所述固态激光雷达用于获取并输出外部环境点云数据，所述单目视觉传感器用于获取并输出外部环境图像数据，所述融合跟踪器包括单目视觉目标检测模块、激光雷达目标检测模块、激光雷达-视觉融合跟踪模块和通信模块；
所述融合跟踪器的单目视觉目标检测模块用于从图像中获取目标信息；所述激光雷达目标检测模块用于从点云中获取目标信息；所述激光雷达-视觉融合跟踪模块首先通过将图像目标和点云目标统一到同一坐标系，然后根据目标运动状态模型和量测模型，对目标运动状态进行一步预测，建立跟踪门进行量测筛选，再对融合目标与量测目标进行数据关联，最后进行目标状态滤波完成对图像目标和点云目标的融合跟踪；所述通信模块用于输出融合目标信息；
其中，固态激光雷达、单目视觉传感器及融合跟踪器集成安装在同一个机械结构中，固态激光雷达、单目视觉传感器的相对位置固定不变；
所述融合跟踪器的融合跟踪流程包括：
2.1单目视觉目标检测模块接收来自单目视觉传感器的原始图像信息，利用图像目标检测算法得到图像目标类型、目标边框、目标方位角信息，利用单目视觉测距原理获取单目测距信息；
2.2激光雷达目标检测模块接收来自固态激光雷达的原始点云信息，检测得到点云目标位置信息及目标长宽高信息；
2.3激光雷达-视觉融合跟踪模块接收来自单目视觉目标检测模块的从图像中获取的目标信息和来自激光雷达目标检测模块的从点云中获取的目标信息，通过将图像目标和点云目标统一到同一坐标系，建立目标状态模型和量测模型，对目标状态进行一步预测，建立跟踪门进行量测筛选，对融合目标与量测目标进行数据关联，进行目标状态滤波完成对图像目标和点云目标的融合，得到融合后的包括目标类型、位置信息及长宽高信息的融合目标；
2.4通信模块与外部设备进行通信，将融合目标结果发送给外部设备；
所述步骤2.3目标融合方法包括以下步骤：
3.1空间配准：设定智能移动平台坐标系，利用旋转平移变换公式将图像目标和点云目标变换到该坐标系上，将图像目标和点云目标统一到同一坐标系；
3.2系统建模：在智能移动平台坐标系上建立扩展目标运动状态模型以及图像目标和点云目标的量测模型：
其中表示k时刻目标的x位置、x速度、y位置、y速度、宽、高、信任度，/>表示k时刻图像目标的量测，N表示自然数集合，t表示图像目标类型，表示k时刻点云目标的量测，Fk为状态转移矩阵，Γk为噪声矩阵，/>为k时刻图像目标量测矩阵，/>为k时刻点云目标量测矩阵，ωk为k时刻过程噪声，/>为k时刻图像目标量测噪声，/>为k时刻点云目标量测噪声；
3.3目标运动状态预测：根据步骤3.2所述的目标运动状态方程及前一时刻目标的状态估计与协方差矩阵Pk-1|k-1，分别计算目标状态的一步预测值/>及一步预测误差的协方差矩阵Pk|k-1；
3.4量测筛选：设置一个二重跟踪门，首先以坐标轴原点为扇形顶点，以目标边框为扇形两边上的点建立扇形跟踪门进行筛选，然后对扇形跟踪门内的目标量测根据目标类型以及目标宽度做二次筛选，得到候选量测集
3.5数据关联：采用联合概率数据关联JPDA算法对目标轨迹预测值和候选量测进行关联，能同时关联图像目标量测和点云目标量测的目标为成功融合目标，其余则为未成功融合目标；未成功融合目标又分为图像检测到而激光雷达没有检测到、激光雷达检测到而图像没有检测到、以及两种传感器都没有检测到三种情况；根据关联结果对不同目标的信任度p进行修正，并根据信任度p分为融合目标、潜在目标以及可丢弃目标；
3.6目标状态滤波：根据计算的关联事件概率更新目标状态及目标量测，利用卡尔曼滤波对关联后的目标状态进行滤波，得到k时刻目标运动状态的最优估计/>与滤波误差的协方差矩阵/>
所述3.5数据关联方法包括以下步骤：
4.1假定单目视觉传感器和固态激光雷达的量测误差统计独立，单目视觉传感器和固态激光雷达的关联事件概率为：
其中为每个传感器的事件概率，由位置、宽高、信任度三个特征对应的量测关联概率/>加权计算得到：
其中q1,q2,q3为各个特征所占权重，由具体应用场景确定；
其中对于每个特征对应的量测关联概率，是在跟踪门内计算每个量测关联事件的概率得到：
其中，映射ai表示目标t与来自于传感器i的量测之间的互联映射，Zk为直到k时刻的累计候选量测集，表示k时刻ai反应的量测与目标映射对应关系是正确的事件，/>表示互不相容事件/>的并，/>表示Zk下的后验概率；
4.2关联后的目标信任度修正；
4.3按照信任度对关联后的目标进行分类。
2.根据权利要求1所述的一种激光雷达与视觉融合的集成化目标跟踪系统，其特征在于，所述步骤4.2对每一个目标，信任度的修正值p'按照以下规则设置：对于成功关联融合的目标，其信任度修正值为p'＝ω1，对未成功关联融合目标又分为图像检测到而激光雷达没有检测到、激光雷达检测到而图像没有检测到、以及两种传感器都没有检测到三种情况；对于图像检测出目标而激光雷达没有检测出目标的情况，设定一个距离阈值Td，信任度修正值设置为其中d为目标相对于坐标原点的距离；对于激光雷达检测出目标而图像没有检测出目标的情况，设定其信任度修正值p'＝ω3；对于两种传感器都没有检测到的情况，设定其信任度修正值p'＝ω4；其中ω1,ω2,ω3,ω4为信任度修正值的大小，ω1,ω2,ω3,ω4满足/>利用目标数据关联结果对关联后的目标信任度进行修正：
pk＝ap'+bpk-1，a+b＝1,p'＝ω1时,a＝1； (5)
其中pk表示k时刻目标的信任度。
3.根据权利要求1所述的一种激光雷达与视觉融合的集成化目标跟踪系统，其特征在于，所述步骤4.3按照信任度大小，设定两个信任度阈值和/>将目标分为融合目标、潜在目标以及可丢弃目标：
融合目标表示该目标的信任度符合融合结果的期望值，通过通信模块对外发布；潜在目标表示该目标的信任度略低于融合结果的期望值，但差距不大，不发布这类目标，但保留在系统中继续做后续关联；可丢弃目标表示该目标的信任度远低于融合结果的期望值，视该目标已消失，终止对该目标的跟踪。
CN202011509307.6A 2020-12-18 2020-12-18 一种激光雷达与视觉融合的集成化目标跟踪系统及方法 Active CN112731371B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011509307.6A CN112731371B (zh) 2020-12-18 2020-12-18 一种激光雷达与视觉融合的集成化目标跟踪系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011509307.6A CN112731371B (zh) 2020-12-18 2020-12-18 一种激光雷达与视觉融合的集成化目标跟踪系统及方法

Publications (2)

Publication Number Publication Date
CN112731371A CN112731371A (zh) 2021-04-30
CN112731371B true CN112731371B (zh) 2024-01-23

Family

ID=75603901

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011509307.6A Active CN112731371B (zh) 2020-12-18 2020-12-18 一种激光雷达与视觉融合的集成化目标跟踪系统及方法

Country Status (1)

Country Link
CN (1) CN112731371B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113253735B (zh) * 2021-06-15 2021-10-08 同方威视技术股份有限公司 跟随目标的方法、装置、机器人及计算机可读存储介质
CN113988228B (zh) * 2021-09-14 2024-04-09 中国科学院信息工程研究所 一种基于rfid和视觉融合的室内监控方法及系统
CN113804182B (zh) * 2021-09-16 2023-09-29 重庆邮电大学 一种基于信息融合的栅格地图创建方法
CN115993597A (zh) * 2021-10-18 2023-04-21 长沙中车智驭新能源科技有限公司 一种视觉雷达感知融合方法及终端设备

Citations (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0643241A (ja) * 1992-07-27 1994-02-18 Tech Res & Dev Inst Of Japan Def Agency 多目標追尾方法及びその装置
CN101655561A (zh) * 2009-09-14 2010-02-24 南京莱斯信息技术股份有限公司 基于联合卡尔曼滤波的多点定位数据与雷达数据融合方法
JP2011047882A (ja) * 2009-08-28 2011-03-10 Toshiba Corp 目標追跡装置
WO2012024516A2 (en) * 2010-08-18 2012-02-23 Nearbuy Systems, Inc. Target localization utilizing wireless and camera sensor fusion
CN103278813A (zh) * 2013-05-02 2013-09-04 哈尔滨工程大学 一种基于高阶无迹卡尔曼滤波的状态估计方法
KR101402206B1 (ko) * 2014-04-10 2014-05-30 국방과학연구소 운동학 및 형상 정보를 활용한 다중 표적 추적 방법
CN105701479A (zh) * 2016-02-26 2016-06-22 重庆邮电大学 基于目标特征的智能车辆多激光雷达融合识别方法
CN106101590A (zh) * 2016-06-23 2016-11-09 上海无线电设备研究所 雷达视频复合数据探测与处理系统及探测与处理方法
JP2017067624A (ja) * 2015-09-30 2017-04-06 三菱電機株式会社 目標追尾装置及びレーダ装置
CN106680806A (zh) * 2016-11-24 2017-05-17 清华大学 一种多雷达点迹融合方法
KR20180080004A (ko) * 2017-01-03 2018-07-11 국방과학연구소 가림환경에서의 표적 형상정보를 활용한 표적 추적 방법
CN108445480A (zh) * 2018-02-02 2018-08-24 重庆邮电大学 基于激光雷达的移动平台自适应扩展目标跟踪系统及方法
CN109085570A (zh) * 2018-06-10 2018-12-25 南京理工大学 基于数据融合的车辆检测跟踪算法
CN109212521A (zh) * 2018-09-26 2019-01-15 同济大学 一种基于前视相机与毫米波雷达融合的目标跟踪方法
CN110675431A (zh) * 2019-10-08 2020-01-10 中国人民解放军军事科学院国防科技创新研究院 一种融合图像和激光点云的三维多目标跟踪方法
CN110726990A (zh) * 2019-09-23 2020-01-24 江苏大学 一种基于ds-gnn算法的多传感器融合方法
CN111007454A (zh) * 2019-10-28 2020-04-14 重庆邮电大学 一种基于合作目标信息的扩展目标跟踪方法
CN111142100A (zh) * 2020-01-06 2020-05-12 南京鹰目电子科技有限公司 一种雷达目标跟踪方法
CN111505624A (zh) * 2020-04-30 2020-08-07 中国汽车工程研究院股份有限公司 一种基于机器视觉和毫米波雷达数据融合的环境感知方法
KR102168288B1 (ko) * 2019-05-20 2020-10-21 충북대학교 산학협력단 다중 라이다를 이용한 다중 물체 추적 시스템 및 방법
CN111951305A (zh) * 2020-08-20 2020-11-17 重庆邮电大学 一种基于视觉和激光雷达的目标检测和运动状态估计方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10565468B2 (en) * 2016-01-19 2020-02-18 Aptiv Technologies Limited Object tracking system with radar/vision fusion for automated vehicles
US11422253B2 (en) * 2018-11-19 2022-08-23 Tdk Corportation Method and system for positioning using tightly coupled radar, motion sensors and map information

Patent Citations (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0643241A (ja) * 1992-07-27 1994-02-18 Tech Res & Dev Inst Of Japan Def Agency 多目標追尾方法及びその装置
JP2011047882A (ja) * 2009-08-28 2011-03-10 Toshiba Corp 目標追跡装置
CN101655561A (zh) * 2009-09-14 2010-02-24 南京莱斯信息技术股份有限公司 基于联合卡尔曼滤波的多点定位数据与雷达数据融合方法
WO2012024516A2 (en) * 2010-08-18 2012-02-23 Nearbuy Systems, Inc. Target localization utilizing wireless and camera sensor fusion
CN103278813A (zh) * 2013-05-02 2013-09-04 哈尔滨工程大学 一种基于高阶无迹卡尔曼滤波的状态估计方法
KR101402206B1 (ko) * 2014-04-10 2014-05-30 국방과학연구소 운동학 및 형상 정보를 활용한 다중 표적 추적 방법
JP2017067624A (ja) * 2015-09-30 2017-04-06 三菱電機株式会社 目標追尾装置及びレーダ装置
CN105701479A (zh) * 2016-02-26 2016-06-22 重庆邮电大学 基于目标特征的智能车辆多激光雷达融合识别方法
CN106101590A (zh) * 2016-06-23 2016-11-09 上海无线电设备研究所 雷达视频复合数据探测与处理系统及探测与处理方法
CN106680806A (zh) * 2016-11-24 2017-05-17 清华大学 一种多雷达点迹融合方法
KR20180080004A (ko) * 2017-01-03 2018-07-11 국방과학연구소 가림환경에서의 표적 형상정보를 활용한 표적 추적 방법
CN108445480A (zh) * 2018-02-02 2018-08-24 重庆邮电大学 基于激光雷达的移动平台自适应扩展目标跟踪系统及方法
CN109085570A (zh) * 2018-06-10 2018-12-25 南京理工大学 基于数据融合的车辆检测跟踪算法
CN109212521A (zh) * 2018-09-26 2019-01-15 同济大学 一种基于前视相机与毫米波雷达融合的目标跟踪方法
KR102168288B1 (ko) * 2019-05-20 2020-10-21 충북대학교 산학협력단 다중 라이다를 이용한 다중 물체 추적 시스템 및 방법
CN110726990A (zh) * 2019-09-23 2020-01-24 江苏大学 一种基于ds-gnn算法的多传感器融合方法
CN110675431A (zh) * 2019-10-08 2020-01-10 中国人民解放军军事科学院国防科技创新研究院 一种融合图像和激光点云的三维多目标跟踪方法
CN111007454A (zh) * 2019-10-28 2020-04-14 重庆邮电大学 一种基于合作目标信息的扩展目标跟踪方法
CN111142100A (zh) * 2020-01-06 2020-05-12 南京鹰目电子科技有限公司 一种雷达目标跟踪方法
CN111505624A (zh) * 2020-04-30 2020-08-07 中国汽车工程研究院股份有限公司 一种基于机器视觉和毫米波雷达数据融合的环境感知方法
CN111951305A (zh) * 2020-08-20 2020-11-17 重庆邮电大学 一种基于视觉和激光雷达的目标检测和运动状态估计方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Improved Distributed Multi-target Tracking Algorithm;Xu Zhi-gang等;Journal of System Simulation;第24卷(第5期);全文 *
基于毫米波雷达与摄像头融合的目标跟踪研究;吴宪;吴宇君;邵建旺;;机电一体化(第06期);全文 *
基于激光雷达深度信息和视觉HOG特征的车辆识别与跟踪方法;贾子永;任国全;李冬伟;赵爱德;;装甲兵工程学院学报(第06期);全文 *
基于视觉和毫米波雷达融合的前方车辆检测算法研究;秦汉;中国优秀硕士学位论文全文数据库工程科技Ⅱ辑;全文 *
雷达-红外分布式融合多目标跟踪算法;张万顺;张安清;齐海明;;无线电工程(第09期);全文 *

Also Published As

Publication number Publication date
CN112731371A (zh) 2021-04-30

Similar Documents

Publication Publication Date Title
CN112731371B (zh) 一种激光雷达与视觉融合的集成化目标跟踪系统及方法
CN110988912B (zh) 自动驾驶车辆的道路目标与距离检测方法、系统、装置
CN108868268B (zh) 基于点到面距离和互相关熵配准的无人车位姿估计方法
CN113269098A (zh) 一种基于无人机的多目标跟踪定位与运动状态估计方法
WO2018142900A1 (ja) 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム
CN112396650A (zh) 一种基于图像和激光雷达融合的目标测距系统及方法
CN112233177B (zh) 一种无人机位姿估计方法及系统
JP2022113746A (ja) 判定装置
CN112001958B (zh) 基于有监督单目深度估计的虚拟点云三维目标检测方法
CN112700470A (zh) 一种基于交通视频流的目标检测和轨迹提取方法
CN108332752B (zh) 机器人室内定位的方法及装置
Konrad et al. Localization in digital maps for road course estimation using grid maps
CN114758504B (zh) 一种基于滤波校正的网联车超速预警方法及系统
CN114325634A (zh) 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法
CN114280611A (zh) 一种融合毫米波雷达与摄像头的路侧感知方法
CN115451948A (zh) 一种基于多传感器融合的农业无人车定位里程计方法及系统
Yu et al. Camera-radar data fusion for target detection via Kalman filter and Bayesian estimation
CN114119659A (zh) 一种多传感器融合的目标跟踪方法
CN113848545A (zh) 一种基于视觉和毫米波雷达的融合目标检测和跟踪方法
CN114137562B (zh) 一种基于改进全局最邻近的多目标跟踪方法
CN117218350A (zh) 一种基于固态雷达的slam实现方法及系统
Meier et al. Object detection and tracking in range image sequences by separation of image features
CN114035187A (zh) 一种自动驾驶系统的感知融合方法
CN115471526A (zh) 基于多源异构信息融合的自动驾驶目标检测与跟踪方法
US20220404170A1 (en) Apparatus, method, and computer program for updating map

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant