(12)发明专利申请
(10)申请公布号 CN 112731371 A(43)申请公布日 2021.04.30
(21)申请号 202011509307.6(22)申请日 2020.12.18
(71)申请人 重庆邮电大学
地址 400065 重庆市南岸区南山街道崇文
路2号(72)发明人 岑明 刘孟鑫 徐沾林 赵崇峻 (74)专利代理机构 重庆市恒信知识产权代理有
限公司 50102
代理人 陈栋梁(51)Int.Cl.
G01S 13/72(2006.01)G01S 13/86(2006.01)
权利要求书3页 说明书10页 附图4页
CN 112731371 A(54)发明名称
一种激光雷达与视觉融合的集成化目标跟踪系统及方法(57)摘要
本发明请求保护一种激光雷达与视觉融合
该系统包括固态的集成化目标跟踪系统及方法。
激光雷达、单目视觉传感器和融合跟踪器。其中融合跟踪器包括单目视觉目标检测模块、激光雷达目标检测模块、激光雷达‑视觉融合跟踪模块和通信模块。单目视觉目标检测模块从图像中获取目标信息；激光雷达目标检测模块从点云中获取目标信息；激光雷达‑视觉融合跟踪模块对量测进行空间配准，建立目标状态模型和量测模型，通过目标状态一步预测建立跟踪门，再通过数据关联、目标状态滤波完成对图像目标和点云目标的融合跟踪。本发明提高了智能移动平台的集成度与开发效率，改善了融合跟踪结果的准确性。
CN 112731371 A
权 利 要 求 书
1/3页
1.一种激光雷达与视觉融合的集成化目标跟踪系统，其特征在于，包括：固态激光雷达、单目视觉传感器及融合跟踪器，其中：所述固态激光雷达用于获取并输出外部环境点云数据，所述单目视觉传感器用于获取并输出外部环境图像数据，所述融合跟踪器包括单目视觉目标检测模块、激光雷达目标检测模块、激光雷达‑视觉融合跟踪模块和通信模块；
所述融合跟踪器的单目视觉目标检测模块用于从图像中获取目标信息；所述激光雷达目标检测模块用于从点云中获取目标信息；所述激光雷达‑视觉融合跟踪模块首先通过将图像目标和点云目标统一到同一坐标系，然后根据目标运动状态模型和量测模型，对目标运动状态进行一步预测，建立跟踪门进行量测筛选，再对融合目标与量测目标进行数据关联，最后进行目标状态滤波完成对图像目标和点云目标的融合跟踪；所述通信模块用于输出融合目标信息；
其中，固态激光雷达、单目视觉传感器及融合跟踪器集成安装在同一个机械结构中，固态激光雷达、单目视觉传感器的相对位置固定不变。
2.根据权利要求1所述的一种激光雷达与视觉融合的集成化目标跟踪系统，其特征在于，所述融合跟踪器的融合跟踪流程包括：
2.1单目视觉目标检测模块接收来自单目视觉传感器的原始图像信息，利用图像目标检测算法得到图像目标类型、目标边框、目标方位角信息，利用单目视觉测距原理获取单目测距信息；
2.2激光雷达目标检测模块接收来自固态激光雷达的原始点云信息，检测得到点云目标位置信息及目标长宽高信息；
2.3激光雷达‑视觉融合跟踪模块接收来自单目视觉目标检测模块的从图像中获取的目标信息和来自激光雷达目标检测模块的从点云中获取的目标信息，通过将图像目标和点云目标统一到同一坐标系，建立目标状态模型和量测模型，对目标状态进行一步预测，建立跟踪门进行量测筛选，对融合目标与量测目标进行数据关联，进行目标状态滤波完成对图像目标和点云目标的融合，得到融合后的包括目标类型、位置信息及长宽高信息的融合目标；
2.4通信模块与外部设备进行通信，将融合目标结果发送给外部设备。
3.根据权利要求2所述的一种激光雷达与视觉融合的集成化目标跟踪系统，其特征在于，所述步骤2.3目标融合方法包括以下步骤：
3.1空间配准：设定智能移动平台坐标系，利用旋转平移变换公式将图像目标和点云目标变换到该坐标系上，将图像目标和点云目标统一到同一坐标系；
3.2系统建模：在智能移动平台坐标系上建立扩展目标运动状态模型以及图像目标和点云目标的量测模型：
其中表示k时刻目标的x位置、x速度、y位置、y速度、宽、高、信
2
CN 112731371 A
权 利 要 求 书
2/3页
任度，表示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时刻目标运动状态的最优估计差的协方差矩阵
4.根据权利要求6所述的一种激光雷达与视觉融合的集成化目标跟踪系统，其特征在于，所述3.5数据关联方法包括以下步骤：
4.1假定单目视觉传感器和固态激光雷达的量测误差统计独立，单目视觉传感器和固态激光雷达的联合事件概率
为：
其中率
为每个传感器的事件概率，由位置、宽高、信任度三个特征对应的量测关联概
加权计算得到：
其中q1,q2,q3为各个特征所占权重，由具体应用场景确定；其中对于每个特征对应的量测关联概率，是在跟踪门内计算每个量测关联事件的概率得到：
其中，映射ai表示目标t与来自于传感器i的量测之间的互联映射，Zk为直到k时刻的累
3
CN 112731371 A
权 利 要 求 书
3/3页
计候选量测集，互不相容事件
表示k时刻ai反应的量测与目标映射对应关系是正确的事件，表示的并，
表示Zk下的后验概率。
4.2关联后的目标信任度修正；
4.3按照信任度对关联后的目标进行分类。
5.根据权利要求4所述的一种激光雷达与视觉融合的集成化目标跟踪系统，其特征在于，所述步骤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时刻目标的信任度。
6.根据权利要求4所述的一种激光雷达与视觉融合的集成化目标跟踪系统，其特征在于，所述步骤4.3按照信任度大小，设定两个信任度阈值目标以及可丢弃目标：
和
将目标分为融合目标、潜在
融合目标表示该目标的信任度符合融合结果的期望值，可以通过通信模块对外发布；潜在目标表示该目标的信任度略低于融合结果的期望值，但差距不大，不发布这类目标，但保留在系统中继续做后续关联；可丢弃目标表示该目标的信任度远低于融合结果的期望值，视该目标已消失，终止对该目标的跟踪。
4
CN 112731371 A
说 明 书
1/10页
一种激光雷达与视觉融合的集成化目标跟踪系统及方法
技术领域
[0001]本发明属于智能移动平台环境感知技术领域，具体涉及一种激光雷达与视觉融合的集成化目标跟踪系统及方法。
背景技术[0002]智能移动平台是一个集环境感知、动态决策与规划、行为控制与执行等多功能于一体的综合系统，智能移动平台包括智能车辆、智能机器人等平台。其中环境感知是智能移动平台安全进行决策控制的基础要求和前提条件，它能依靠自身的多种传感器实时采集智能移动平台周围的路面、障碍物信息实现决策控制。如何利用多种传感器进行数据融合，提高智能移动平台的环境感知能力具有重要意义与实用价值。[0003]中国专利申请：激光雷达图像与摄像机图像的匹配方法及系统(申请号：CN 201810801137.5)公开了一种激光雷达图像与摄像机图像的匹配方法，该系统利用卷积神经网络提取的位姿参数将激光雷达图像投影到摄像机图像上，得到激光雷达图像和摄像机图像的融合图。该系统仅仅只是完成了点云到图像的匹配，并没有后续的检测处理。中国专利申请：基于激光雷达和双目相机的车前行人检测方法及系统(申请号：CN 201410841099.8)公开了一种基于激光雷达和双目相机的车前行人检测方法及系统，该系统利用激光雷达数据检测出疑似行人目标区域确定双目相机的感兴趣区域，并获得疑似行人目标距离、方位角和速度，然后双目相机利用疑似行人目标的检测，融合这二者的检测结果进行融合。该系统利用激光雷达获取疑似行人目标，容易造成漏检和误检。中国专利申请：用于自动化车辆的激光雷达和照相机数据融合(申请号：CN 201810391971.1)公开了一种融合自动化车辆的激光雷达数据和照相机数据的数据融合系统，该系统利用来自照相机的图像确定物体的反射率特征，并在物体的反射率特征使得来自激光雷达的光能中的照射到物体上并被该物体朝向激光雷达反射的光能所占的反射份数一低于份数阈值时，对激光雷达的检测特征进行调整。该系统只是利用图像的反射率特征调整激光雷达检测特征，从而获取更准确的目标方向和距离，融合比较初步，不能改善激光雷达识别效果差的结果。中国专利申请：目标探测系统和数据融合方法(申请号：CN 201910402856.4)公开了一种目标探测系统和数据融合方法，该方法主要介绍了其融合系统的硬件设计方案，但该系统使用的主要是机械旋转式激光雷达，设计的框架与本发明有所不同，而且也没有后续的目标融合算法以及数据预处理器。
[0004]本发明针对智能移动平台环境感知系统中融合目标跟踪系统的应用需求，以及针对单独的图像对目标距离估计存在较大误差和单独的激光雷达目标检测因为目标识别能力不够导致检测太多无用目标等问题，设计了一种激光雷达与视觉融合的集成化目标跟踪系统及方法。在该系统中，利用视觉传感器从图像中获取目标信息，利用激光雷达从点云中获取目标信息，利用融合跟踪器完成图像目标和点云目标的融合，得到融合后信息更加准确全面的目标，并将其发布给外部设备，如此，可提升智能移动平台的集成度与开发效率，融合跟踪结果的准确性。
5
CN 112731371 A
说 明 书
2/10页
发明内容
[0005]本发明旨在解决以上现有技术的问题。提出了一种激光雷达与视觉融合的集成化目标跟踪系统及方法。本发明的技术方案如下：
[0006]一种激光雷达与视觉融合的集成化目标跟踪系统，其包括：[0007]固态激光雷达、单目视觉传感器及融合跟踪器，其中：[0008]所述固态激光雷达用于获取并输出外部环境点云数据，所述单目视觉传感器用于获取并输出外部环境图像数据，所述融合跟踪器包括单目视觉目标检测模块、激光雷达目标检测模块、激光雷达‑视觉融合跟踪模块和通信模块；
[0009]所述融合跟踪器的单目视觉目标检测模块用于从图像中获取目标信息；所述激光雷达目标检测模块用于从点云中获取目标信息；所述激光雷达‑视觉融合跟踪模块首先通过将图像目标和点云目标统一到同一坐标系，然后根据目标运动状态模型和量测模型，对目标运动状态进行一步预测，建立跟踪门进行量测筛选，再对融合目标与量测目标进行数据关联，最后进行目标状态滤波完成对图像目标和点云目标的融合跟踪；所述通信模块用于输出融合目标信息；[0010]其中，固态激光雷达、单目视觉传感器及融合跟踪器集成安装在同一个机械结构中，固态激光雷达、单目视觉传感器的相对位置固定不变。[0011]进一步的，所述融合跟踪器的融合跟踪流程包括：
[0012]2.1单目视觉目标检测模块接收来自单目视觉传感器的原始图像信息，利用图像目标检测算法得到图像目标类型、目标边框、目标方位角信息，利用单目视觉测距原理获取单目测距信息。
[0013]2.2激光雷达目标检测模块接收来自固态激光雷达的原始点云信息，检测得到点云目标位置信息及目标长宽高信息。
[0014]2.3激光雷达‑视觉融合跟踪模块接收来自单目视觉目标检测模块的从图像中获取的目标信息和来自激光雷达目标检测模块的从点云中获取的目标信息，通过将图像目标和点云目标统一到同一坐标系，建立目标状态模型和量测模型，对目标状态进行一步预测，建立跟踪门进行量测筛选，对融合目标与量测目标进行数据关联，进行目标状态滤波完成对图像目标和点云目标的融合，得到融合后的包括目标类型、位置信息及长宽高信息的融合目标。
[0015]2.4通信模块与外部设备进行通信，将融合目标结果发送给外部设备。[0016]进一步的，所述步骤2.1中，当单目视觉目标检测模块采用基于相似三角形的单目测距方法时，假设摄像头的焦距为f，高度为H，摄像头和目标的距离为Z，目标的检测框着地点在图像上的投影是y，由相似三角形可得：
[0017]
[0018][0019]
从而可得摄像头与前方目标的距离
进一步的，所述步骤2.2激光雷达目标检测模块接收来自固态激光雷达的原始点云信息，检测得到点云目标位置信息及目标长宽高信息，采用地平面拟合的方法先分割出激光雷达地面点与非地面点，具体包括：
6
CN 112731371 A[0020]
说 明 书
3/10页
a)种子点集的选取：选取一个种子点集，这些种子点来源于点云中高度即z值较小
的点，引入最低点代表LPR，即NLPR个最低高度点的平均值来选取种子点集，LPR保证了平面拟合阶段不受测量噪声的影响，LPR被当作是整个点云P的最低点，点云P中高度在阈值Thseed内的点被当作是种子点，由这些种子点构成一个种子点集合；[0021]b)平面拟合模型建立：地平面拟合用于分割地平面，采用如下线性模型用于平面模型估计：
[0022]ax+by+cz+d＝0 (11)[0023]其点法式形式为：[0024]nTw＝‑d (12)
[0025]其中n＝[a,b,c]T,w＝[x,y,z]T，a,b,c，d为平面方程系数。通过初始点集的协方差矩阵C∈R3×3来求解n，从而确定一个平面，采用种子点集C∈R3作为初始点集，其协方差矩阵为：
[0026][0027]
其中，是所有点的均值，S表示点云集合，通过奇异值分解求得垂直于平面的法向
量n，d可以通过代入种子点集的平均值(它代表属于地面的点)直接求得，代表属于地面的点。
进一步的，采用四邻域聚类的方法完成点云聚类，从而得到聚类后的目标：
[0029]a)建立点云栅格地图：考虑到激光雷达点云数近稠密远稀疏的特性，建立极坐标栅格地图，以激光雷达坐标原点为圆心，每个栅格的分辨率设置为θ(°)×r(cm)，对于落入栅格的点数p，满足p＞Td，其中Td为占据栅格所需点数的阈值，则将栅格标记为占据，否则标记为非占据；[0030]b)四邻域聚类方法：首先遍历建立的每个栅格，对于已占据的栅格，先判断其上下左右四个领域是否已被占据，若已被占据，再判断是否已被打上标签，若无，则将这些栅格打上相同标签，并将其放入一个容器中，下次从该容器中选取新的栅格重复上述步骤直到容器为空，遍历下一个栅格。[0031]进一步的，所述步骤2.3目标融合方法包括以下步骤：[0032]3.1空间配准：设定智能移动平台坐标系，利用旋转平移变换公式将图像目标和点云目标变换到该坐标系上，将图像目标和点云目标统一到同一坐标系；[0033]3.2系统建模：在智能移动平台坐标系上建立扩展目标运动状态模型以及图像目标和点云目标的量测模型：
[0028]
[0034]
[0035]其中表示k时刻目标的x位置、x速度、y位置、y速度、宽、
表示k时刻图像目标的量测，N表示自然数集合，t表示图像目标
表示k时刻点云目标的量测，Fk为状态转移矩阵，Γk为噪声矩阵，
7
高、信任度，类型，
CN 112731371 A
说 明 书
4/10页
为k时刻图像目标量测矩阵，为k时刻点云目标量测矩阵，ωk为k时刻过程噪声，为k时刻图像目标量测噪声，为k时刻点云目标量测噪声；
[0036]
3.3目标运动状态预测：根据步骤3.2所述的目标运动状态方程及前一时刻目标的
与协方差矩阵Pk‑1|k‑1，分别计算目标状态的一步预测值
及一步预测误
状态估计
差的协方差矩阵Pk|k‑1；[0037]3.4量测筛选：设置一个二重跟踪门，首先以坐标轴原点为扇形顶点，以目标边框为扇形两边上的点建立扇形跟踪门进行筛选，然后对扇形跟踪门内的目标量测根据目标类型以及目标宽度做二次筛选，得到候选量测集
[0038]
3.5数据关联：采用联合概率数据关联JPDA算法对目标轨迹预测值和候选量
测进行关联，能同时关联图像目标量测和点云目标量测的目标为成功融合目标，其余
则为未成功融合目标；未成功融合目标又分为图像检测到而激光雷达没有检测到、激光雷达检测到而图像没有检测到、以及两种传感器都没有检测到三种情况；根据关联结果对不同目标的信任度p进行修正，并根据信任度p分为融合目标、潜在目标以及可丢弃目标；
[0039]
3.6目标状态滤波：根据计算的关联事件概率更新目标状态及目标量测，利用
与滤
卡尔曼滤波对关联后的目标状态进行滤波，得到k时刻目标运动状态的最优估计波误差的协方差矩阵
[0040][0041]
所述3.5数据关联方法包括以下步骤：进一步的，
4.1假定单目视觉传感器和固态激光雷达的量测误差统计独立，单目视觉传感器
为：
和固态激光雷达的联合事件概率
[0042][0043]
其中为每个传感器的事件概率，由位置、宽高、信任度三个特征对应的量测关
加权计算得到：
联概率
[0044]
其中q1,q2,q3为各个特征所占权重，由具体应用场景确定；
[0046]其中对于每个特征对应的量测关联概率，是在跟踪门内计算每个量测关联事件的概率得到：
[0047][0048]
[0045]
其中，映射ai表示目标t与来自于传感器i的量测之间的互联映射，Zk为直到k时刻
表示k时刻ai反应的量测与目标映射对应关系是正确的事件，的并，
表示Zk下的后验概率。
的累计候选量测集，
表示互不相容事件
[0049]
4.2关联后的目标信任度修正；
8
CN 112731371 A[0050]
说 明 书
5/10页
4.3按照信任度对关联后的目标进行分类。
[0051]进一步的，所述步骤4.2对每一个目标，信任度的修正值p'按照以下规则设置：对于成功关联融合的目标，其信任度修正值为p'＝ω1，对未成功关联融合目标又分为图像检测到而激光雷达没有检测到、激光雷达检测到而图像没有检测到、以及两种传感器都没有检测到三种情况；对于图像检测出目标而激光雷达没有检测出目标的情况，设定一个距离阈值Td，信任度修正值设置为
其中d为目标相对于坐标原点的距离；对于激光雷
达检测出目标而图像没有检测出目标的情况，设定其信任度修正值p'＝ω3；对于两种传感器都没有检测到的情况，设定其信任度修正值p'＝ω4；其中ω1,ω2,ω3,ω4为信任度修正ω1,ω2,ω3,ω4满足值的大小，
后的目标信任度进行修正：[0052]pk＝ap'+bpk‑1，a+b＝1,p'＝ω1时,a＝1； (5)[0053]其中pk表示k时刻目标的信任度。
[0054]
利用目标数据关联结果对关联
进一步的，所述步骤4.3按照信任度大小，设定两个信任度阈值和将目标分为
融合目标、潜在目标以及可丢弃目标：
[0055]
融合目标表示该目标的信任度符合融合结果的期望值，可以通过通信模块对外发
布；潜在目标表示该目标的信任度略低于融合结果的期望值，但差距不大，不发布这类目标，但保留在系统中继续做后续关联；可丢弃目标表示该目标的信任度远低于融合结果的期望值，视该目标已消失，终止对该目标的跟踪。[0057]一种基于所述系统的跟踪方法，其包括以下步骤：[0058]固态激光雷达获取并输出外部环境点云数据，单目视觉传感器获取并输出外部环境图像数据；
[0059]单目视觉目标检测模块从图像中获取目标信息；[0060]激光雷达目标检测模从点云中获取目标信息；
[0061]激光雷达‑视觉融合跟踪模块首先通过将图像目标和点云目标统一到同一坐标系，然后根据目标运动状态模型和量测模型，对目标运动状态进行一步预测，建立跟踪门进行量测筛选，再对融合目标与量测目标进行数据关联，最后进行目标状态滤波完成对图像目标和点云目标的融合跟踪；通信模块输出融合目标信息。[0062]本发明的优点及有益效果如下：
[0063]本发明提出一种激光雷达与视觉融合的集成化目标跟踪系统及方法，该系统将固
实现了智能移态激光雷达、单目视觉传感器及融合跟踪器集成为一个融合目标跟踪系统，
动平台目标融合跟踪系统的集成化，可以改善现有智能移动平台上各类传感器彼此独立、需要开发者耗费大量时间以及人力资源进行标定、配准、融合等工作的情况，开发者可以将本系统快速集成到智能移动平台中，提高了智能移动平台的开发效率、可靠性。本发明还通
9
[0056]
CN 112731371 A
说 明 书
6/10页
过引入目标信任度，实现对目标类型的细分，对环境中的目标提供更充分的认知，从而改善了融合跟踪结果的准确性。
附图说明
[0064]图1是本发明提供优选实施例一种激光雷达与视觉融合的集成化目标跟踪系统及方法的总体框架；
[0065]图2是本发明提出的融合跟踪器的流程图；[0066]图3是本发明提出的目标融合方法；
[0067]图4是本发明提出的融合目标与量测目标之间的数据关联方法；[0068]图5表示基于相似三角形的单目测距示意图。
具体实施方式
[0069]下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。[0070]本发明解决上述技术问题的技术方案是：
[0071]本发明针对智能移动平台环境感知系统中融合目标跟踪系统的应用需求，以及针对单独的图像对目标距离估计存在较大误差和单独的激光雷达目标检测因为目标识别能力不够导致检测太多无用目标等问题，设计了一种激光雷达与视觉融合的集成化目标跟踪系统及方法。
[0072]本发明解决上述技术问题的技术方案是：
[0073]以下结合附图和具体实例对本发明的具体实施方式进行描述。
[0074]图1所示为激光雷达与视觉融合的集成化目标跟踪系统及方法的总体框架，该框架如下所述：
[0075]所述固态激光雷达用于获取并输出外部环境点云数据，所述单目视觉传感器用于获取并输出外部环境图像数据，所述融合跟踪器包括单目视觉目标检测模块、激光雷达目标检测模块、激光雷达‑视觉融合跟踪模块和通信模块。
[0076]所述融合跟踪器的单目视觉目标检测模块用于从图像中获取目标信息；所述激光雷达目标检测模块用于从点云中获取目标信息；所述激光雷达‑视觉融合跟踪模块首先通过将图像目标和点云目标统一到同一坐标系，然后根据目标运动状态模型和量测模型，对目标运动状态进行一步预测，建立跟踪门进行量测筛选，再对融合目标与量测目标进行数据关联，最后进行目标状态滤波完成对图像目标和点云目标的融合跟踪；所述通信模块用于输出融合目标信息。[0077]其中，固态激光雷达、单目视觉传感器及融合跟踪器集成安装在同一个机械结构中，固态激光雷达、单目视觉传感器的相对位置固定不变。[0078]图2所示为融合跟踪器的流程图，该流程包含如下步骤：[0079](1)单目视觉目标检测模块接收来自单目视觉传感器的原始图像信息。[0080](2)单目视觉目标检测模块利用图像目标检测算法得到图像目标类型、目标边框、目标方位角信息，实施例采用包括但不限于模板匹配方法、深度神经网络方法等。单目视觉目标检测模块利用单目视觉测距原理获取单目测距信息。实施例采用包括但不限于基于相
10
CN 112731371 A
说 明 书
7/10页
似三角形的单目测距方法。[0081](3)激光雷达目标检测模块接收来自固态激光雷达的原始点云信息。[0082](4)激光雷达目标检测模块检测得到点云目标位置信息及目标长宽高信息。实施例采用包括但不限于地平面拟合的方法先分割出激光雷达地面点与非地面点：[0083]a)种子点集的选取：选取一个种子点集，这些种子点来源于点云中高度(即z值)较小的点。引入最低点代表(LPR)，即NLPR个最低高度点的平均值来选取种子点集，LPR保证了平面拟合阶段不受测量噪声的影响，LPR被当作是整个点云P的最低点，点云P中高度在阈值Thseed内的点被当作是种子点，由这些种子点构成一个种子点集合。[0084]b)平面拟合模型建立：地平面拟合用于分割地平面，采用包括但不限于如下线性模型用于平面模型估计：[0085]ax+by+cz+d＝0 (11)[0086]即：[0087]nTw＝‑d (12)
[0088]其中n＝[a,b,c]T,w＝[x,y,z]T，通过初始点集的协方差矩阵C∈R3×3来求解n，从而确定一个平面，采用种子点集C∈R3作为初始点集，其协方差矩阵为：
[0089][0090][0091]
其中，是所有点的均值，S为点云集合。通过奇异值分解求得垂直于平面的法向量
n，d可以通过代入种子点集的平均值(它代表属于地面的点)直接求得。
然后实施例采用包括但不限于四邻域聚类的方法完成点云聚类，从而得到聚类后的目标：
[0092]a)建立点云栅格地图：考虑到激光雷达点云数近稠密远稀疏的特性，建立极坐标栅格地图。以激光雷达坐标原点为圆心，每个栅格的分辨率设置为θ(°)×r(cm)，对于落入
满足p＞Td，其中Td为占据栅格所需点数的阈值，则将栅格标记为占据，否则标栅格的点数p，
记为非占据。[0093]b)四邻域聚类方法：首先遍历建立的每个栅格，对于已占据的栅格，先判断其上下左右四个领域是否已被占据，若已被占据，再判断是否已被打上标签，若无，则将这些栅格打上相同标签，并将其放入一个容器中，下次从该容器中选取新的栅格重复上述步骤直到容器为空，遍历下一个栅格。[0094](5)激光雷达‑视觉融合跟踪模块接收来自单目视觉目标检测模块的从图像中获取的目标信息和来自激光雷达目标检测模块的从点云中获取的目标信息，通过将图像目标和点云目标统一到同一坐标系，建立目标状态模型和量测模型，对目标状态进行一步预测，建立跟踪门进行量测筛选，对融合目标与量测目标进行数据关联，进行目标状态滤波完成对图像目标和点云目标的融合，得到融合后的包括目标类型、位置信息及长宽高信息的融合目标。[0095](6)通信模块与外部设备进行通信，将融合目标结果发送给外部设备。[0096]图3所示为目标融合方法，该方法包含以下步骤：[0097](1)空间配准。设定智能移动平台坐标系，利用旋转平移变换公式将图像目标和点云目标变换到该坐标系上，将图像目标和点云目标统一到同一坐标系。
11
CN 112731371 A[0098]
说 明 书
8/10页
(2)系统建模。在智能移动平台坐标系上建立扩展目标运动状态模型以及图像目
标和点云目标的量测模型：
[0099]
[0100]其中表示k时刻目标的x位置、x速度、y位置、y速度、宽、
表示k时刻图像目标的量测，t表示图像目标类型，
高、信任度，
表示k时刻点云目标的量测。Fk为状态转移矩阵，Γk为噪声矩阵，为k时刻
图像目标量测矩阵，为k时刻点云目标量测矩阵，ωk为k时刻过程噪声，为k时刻图像目标量测噪声，为k时刻点云目标量测噪声。
[0101]
(3)目标运动状态预测。根据步骤(2)所述的目标运动状态方程及前一时刻目标的
与协方差矩阵Pk‑1|k‑1，分别计算目标状态的一步预测值
及一步预测误差
状态估计
的协方差矩阵Pk|k‑1。
[0102](4)量测筛选。设置一个二重跟踪门，首先以坐标轴原点为扇形顶点，以目标边框为扇形两边上的点建立扇形跟踪门进行筛选，然后对扇形跟踪门内的目标量测根据目标类型以及目标宽度做二次筛选，得到候选量测集
[0103]
Data (5)数据关联。采用包括但不限于联合概率数据关联(Joint Probabilistic
和候选量测
进行关联。能同时关联图
Association，JPDA)算法对目标轨迹预测值
像目标量测和点云目标量测的目标为成功融合目标，其余则为未成功融合目标。未成功融合目标又分为图像检测到而激光雷达没有检测到、激光雷达检测到而图像没有检测到、以及两种传感器都没有检测到三种情况。根据关联结果对不同目标的信任度p进行修正，并根据信任度p分为融合目标、潜在目标以及可丢弃目标。
[0104]
(6)目标状态滤波。根据计算的关联事件概率更新目标状态及目标量测，利用
与滤
卡尔曼滤波对关联后的目标状态进行滤波，得到k时刻目标运动状态的最优估计波误差的协方差矩阵
[0105][0106]
该方法包含以下步骤：图4所示为融合目标与量测目标之间的数据关联方法，
(1)计算每个传感器量测的关联事件概率。每个传感器量测的关联事件概率
加权计算得到：
由
位置、宽高、信任度三个特征对应的量测关联概率
[0107]
其中q1,q2,q3为各个特征所占权重，由具体应用场景确定。[0109]其中对于每个特征对应的量测关联概率，是在跟踪门内计算每个量测关联事件的概率得到：
[0108]
12
CN 112731371 A[0110][0111]
说 明 书
9/10页
其中，映射ai表示目标t与来自于传感器i的量测之间的互联映射，Zk为直到k时刻
表示k时刻ai反应的量测与目标映射对应关系是正确的事件，的并，
表示Zk下的后验概率。
的累计候选量测集，
表示互不相容事件
[0112]
(2)计算联合事件概率。假定单目视觉传感器和固态激光雷达的量测误差是统计
独立的。则单目视觉传感器和固态激光雷达的联合事件概率
[0113]
(3)关联后的目标信任度修正。对每一个目标，信任度的修正值p'按照以下规则设
置：对于成功关联融合的目标，其信任度修正值为p'＝ω1。对未成功关联融合目标又分为图像检测到而激光雷达没有检测到、激光雷达检测到而图像没有检测到、以及两种传感器都没有检测到三种情况。对于图像检测出目标而激光雷达没有检测出目标的情况，设定一个距离阈值Td，信任度修正值设置为
其中d为目标相对于坐标原点的距离；对于
[0114]
激光雷达检测出目标而图像没有检测出目标的情况，设定其信任度修正值p'＝ω3；对于两种传感器都没有检测到的情况，设定其信任度修正值p'＝ω4。其中ω1,ω2,ω3,ω4为信任度修正值的大小，ω1,ω2,ω3,ω4满足
利用目标数据关联结果
对关联后的目标信任度进行修正：
[0115]pk＝ap'+bpk‑1(a+b＝1,p'＝ω时,a＝1) (5)1[0116]其中pk表示k时刻目标的信任度。[0117](4)按照信任度对关联后的目标进行分类。按照信任度大小，设定两个信任度阈值和
将目标分为融合目标、潜在目标以及可丢弃目标：
[0118]
融合目标表示该目标的信任度符合融合结果的期望值，可以通过通信模块对外发布；潜在目标表示该目标的信任度略低于融合结果的期望值，但差距不大，不发布这类目标，但保留在系统中继续做后续关联；可丢弃目标表示该目标的信任度远低于融合结果的期望值，视该目标已消失，终止对该目标的跟踪。
[0120]如图5所示为单目视觉目标检测模块基于相似三角形的单目测距示意图。实施例
假设本移动平台为A，前方目标为B，摄采用包括但不限于基于相似三角形的单目测距方法，
像头P的焦距为f，高度为H，摄像头和障碍物的距离为Z，目标B的检测框着地点在图像上的投影是y。那么由相似三角形可得：
[0119]
13
CN 112731371 A[0121]
说 明 书
10/10页
[0122][0123]
从而可得摄像头与前方目标B的距离
以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在
技术人员可以对本发明作各种改动或修改，这些等效变阅读了本发明的记载的内容之后，
化和修饰同样落入本发明权利要求所限定的范围。
14
CN 112731371 A
说 明 书 附 图
1/4页
图1
15
CN 112731371 A
说 明 书 附 图
2/4页
图2
16
CN 112731371 A
说 明 书 附 图
3/4页
图3
17
CN 112731371 A
说 明 书 附 图
4/4页
图4
图5
18
因篇幅问题不能全部显示，请点此查看更多更全内容