一种基于多维度点云特征的目标检测与跟踪方法及系统

1.本发明属于移动平台目标检测跟踪技术领域，涉及一种基于多维度点云特征的目标检测与跟踪方法及系统。


背景技术：

2.目标检测与跟踪是无人车和自主移动机器人等导航平台的关键技术。目标检测是对目标当前确定状态的量测；目标跟踪是对目标未来可能状态的预测。检测与跟踪存在相互依存促进关系，检测可为跟踪提供量测值以修正跟踪模型参数；跟踪可为检测提供预测值以更新量测感兴趣区域。目标检测与跟踪方法，可实现对目标信息的量测与估计，在导航避障(例如无人驾驶)、目标跟随(例如多机协同)等应用中具有重要意义。
3.激光雷达(light detection and ranging,lidar)通过激光测距获取周围环境的空间信息和反射强度信息，具有精度高、距离远、速度快、不受光照影响等优点。三维激光雷达(3d lidar)具有多组激光发射器与接收器，能提供更丰富的环境信息，已广泛应用在无人车和机器人等领域。
4.目前，目标检测与跟踪主要包含两类：基于检测的跟踪和基于初始化的跟踪。
5.基于检测的跟踪(detection based tracking,dbt)包含检测器和跟踪器两个关键部件。从初始化开始，通过检测器识别原始数据中的目标，通过特征提取抽象目标模型，再使用跟踪器预测目标的状态，将实际量测数据与预测数据相关联，更新跟踪器参数。dbt通过对每帧数据进行“检测-跟踪”迭代实现闭环，持续输出跟踪目标的预测结果。
6.基于初始化的跟踪(detection free tracking,dft)需要人工指定初始跟踪目标，确定跟踪框，结合跟踪器共同完成检测跟踪任务。dft通过初始跟踪框与跟踪器迭代，持续输出跟踪目标的预测结果。
7.对比两种跟踪方式，基于检测的跟踪方法无需人工初始化，能够处理跟踪目标的新增、消失、合并与分裂等情况，具有良好的跟踪鲁棒性和功能完整性，更适合在无人车或机器人等移动平台应用。
8.具体地，对于基于检测的跟踪方法，其性能依赖检测方法的优劣，即依赖检测器对不同目标特征的区分。基于点云数据的传统目标检测方法主要集中于对目标位置、大小、形状等简单属性的归纳，抗干扰能力较差；基于点云数据的机器学习目标检测方法需要使用大量优质样本进行训练，且对于训练集中未出现的目标类型无法很好地检测。


技术实现要素：

9.针对上述技术问题，本发明的目的是提供一种基于多维度点云特征的目标检测跟踪方法，通过基于多维度点云特征的分析组合，能够实现无人车、机器人等移动平台在复杂地面环境下对动静态目标的准确、稳定检测与跟踪，为避障、行为预测等提供可靠参考。
10.为了实现上述目的，本发明提供了如下技术方案：
11.本发明的基于多维度点云特征的目标检测跟踪方法，包括如下步骤：
lidar稳固安装在无人驾驶拖拉机的顶部，传感器坐标系下的点云数据可通过刚体坐标变换到以无人驾驶拖拉机转向中心为原点的空间直角坐标系，即车体坐标系；通过无人驾驶拖拉机的实时位姿信息，车体坐标系下点云能变换到以平台初始位置为原点的平面直角坐标系，即大地坐标系。
26.经过时间同步与空间配准的点云数据，准确融合了周围环境信息与平台自身信息，即为融合点云数据。
27.s2、目标检测；
28.通过目标分割将步骤s1获得的融合点云数据划分成若干相互独立的点云团簇；并对各点云团簇进行包围盒拟合，获得贴合点云团簇的最小包围；对最小包围内的点云团簇进行特征提取，获取点云团簇的多维度点云特征；基于多维度点云特征中的分类特征筛选出跟踪目标和非跟踪目标，并将跟踪目标进一步划分为具体的类别。
29.s2.1、所述目标分割依次通过地面点滤波、离群点滤波、点云聚类对融合点云数据进行处理，获得点云团簇。具体包括如下步骤：
30.s2.1.1、地面点滤波；
31.融合点云数据包含大量地面点云，通过地面点滤波能够将归属不同目标的点云分隔开，同时减少与目标检测识别无关的干扰点，提高点云检测跟踪的准确性和实时性。
32.采用基于传感器地面点滤波方法、基于邻域分布地面点滤波方法或基于形态学地面点滤波方法对融合点云数据进行地面点滤波处理，获得非地面点云；
33.所述基于传感器地面点滤波方法通过3d lidar安装位置、3d lidar各层扫描线的角度、移动平台相对地面的姿态，利用圆锥曲线计算3d lidar各层扫描线与地平面点的交点的理论值(称为“脚点”)。进一步地，计算融合点云数据与理论“脚点”在径向的欧氏距离，滤除距离小于阈值的地面点云。
34.所述基于邻域分布地面点滤波方法通过将融合点云数据进行栅格空间划分，获得规则排列的独立栅格空间，对各个栅格中的点云分布进行统计，获取其高度的最大值、最小值、平均值和极差等特征数据；根据实际场景下的经验阈值筛选地面栅格，并将其包含的地面点从融合点云数据中滤除。
35.所述基于形态学地面点滤波方法将地面点云在形态学上近似为标准的平面；构造合适的平面模型，并通过平面拟合方法进行模型参数迭代，获得融合点云数据中符合平面模型的点云索引，区分模型的内点和外点，进而实现地面点云的分割。
36.在本发明中，采用限制法向量方向的随机采样一致性(ransac)平面拟合方法，提取地面点云索引，进而获取非地面点云。所述限制法向量方向是通过对ransac方法获得的拟合平面的法向量方向与竖直方向进行夹角计算，若夹角小于阈值则将拟合平面视为地面，否则将其视为干扰，继续寻找其他满足条件的拟合平面，直到满足条件。
37.s2.1.2、离群点滤波；
38.根据3d lidar的原理，点云数据的分辨率随采集距离的增加而减少，距3d lidar较远的点云数据会因过于稀疏而无法完整表征目标的特征，且存在的噪声点易造成误检。此外，超过移动平台最大高度的点云数据，对移动平台可通行区域的分析不产生贡献。
39.采用统计滤波方法、半径滤波方法或条件滤波方法对s2.1.1获得的非地面点云进行离群点滤波处理，获得已滤波点云数据；
与gt4，gt1与gt5)，其中最大值为水平方向向量主方向，即pbr的构成方向，方向向量表示为v1；
57.(3)以g
p
为直线上一点，向量v1为直线方向，根据直线点向式方程构建参考直线l1；同理，以g
p
为直线上一点，垂直于向量v1的向量v2为直线方向，构建参考直线l2；
58.直线的点向式方程如下：
[0059][0060]
其中，x0、y0分别为g
p
的横纵坐标；u、v分别为v1或v2的分量；
[0061]
(4)对地面投影点gt
1-8
分别计算到直线l1和l2的算数距离(即带有正负的距离)；
[0062]
点到直线的算数距离计算公式如下：
[0063][0064]
其中，x0、y0分别为gt
1-8
的横纵坐标；a、b、c为直线的一般式方程ax+by+c＝0的参数；d为点到直线的算术距离；
[0065]
(5)分别对参照两条直线的算数距离取最大值和最小值，得到4个最值x
max
、x
min
、y
max
、y
min
。基于4个最值，构造4个初始顶点(x
max
，y
max
)、(x
max
，y
min
)、(x
min
，y
max
)、(x
min
，y
min
)；
[0066]
(6)根据已知坐标原点和g
p
坐标构建平移向量，根据步骤(2)计算的主方向构建旋转矩阵，对步骤(5)中4个初始顶点进行旋转平移变换，即得到投影包围框pbr的4个顶点gt_boundary
1-4
；
[0067]
(7)将obb对应点云簇中高度最大值max_z_value与gt_boundary
1-4
结合，得到位于gt_boundary
1-4
垂直上方的另外4个顶点gt_boundary
5-8
。至此，即得到正立包围盒ubb的8个顶点gt_boundary
1-8
；
[0068]
在本发明中，对点云团簇的包围盒拟合过程结合obb、pbr和ubb包围盒(框)，以obb为包围拟合基础，通过pbr和ubb校正点云团簇可能存在的拟合失真，为多维度几何特征提取提供依据。
[0069]
s2.3、多维度点云特征提取；
[0070]
所述多维度点云特征提取从点云分布特征、点云邻域特征和点云材质特征三个不同的维度，对最小包围内的点云团簇的进行多维度点云特征提取，获取点云团簇的多维度点云特征；其中，
[0071]
所述点云分布特征为基于点云团簇的点特征，表征点云作为高密度点分布区域的数目、重心、高度最大值、高度最小值、高度平均值和高度极差；其中，数目为点云团簇中的总点数；重心为将点云团簇中所有点的x、y、z坐标均值；高度最大值为点云团簇中点的z坐标最大值；高度最小值为点云团簇中点的z坐标最小值；高度平均值为点云团簇中点的z坐标平均值；高度极差为高度最大值与高度最小值之差。
[0072]
所述点云邻域特征基于点云团簇的几何特征，表征点云团簇作为一个整体具备的形态学特征，包括定向包围盒obb、投影包围框pbr、正立包围盒ubb三种不同模型下的几何特征；其中，
[0073]
定向包围盒obb的几何特征包括顶点、方向、中心、长度、宽度、高度、体积和三维密度；顶点为obb的8个顶点xyz坐标；方向为obb的3个特征向量；中心为obb几何中心坐标；长
度为obb最长边的长度；宽度为obb最短边的长度；高度为obb次长边的长度；体积为obb的长宽高乘积；三维密度为点云团簇总点数与obb体积之比。
[0074]
投影包围框pbr的几何特征包括顶点，方向、中心、长度、宽度、面积、长宽比、二维密度和中心距离；顶点为pbr的4个顶点xy坐标；方向为pbr的2个特征向量；中心为pbr几何中心坐标；长度为pbr长边的长度；宽度为pbr短边的长度；面积为pbr的长宽乘积；长宽比为pbr的长度与宽度之比；二维密度为点云团簇总点数与pbr面积之比；中心距离为pbr几何中心与采集平台中心的线段长度。
[0075]
正立包围盒ubb的几何特征包括顶点、方向、中心、长度、宽度、高度、体积、三维密度和面积高度比；顶点为ubb的8个顶点xyz坐标；方向为ubb的3个特征向量；中心为ubb几何中心坐标；长度为ubb最长边的长度；宽度为ubb最短边的长度；高度为ubb次长边的长度；体积为ubb的长宽高乘积；三维密度为点云团簇总点数与ubb体积之比；面积高度比为pbr的面积与ubb的高度之比。
[0076]
所述点云材质特征基于点云团簇在反射率维度的信息，对点云集合的反射率进行统计分析，包括平均反射率、最大反射率、最小反射率、反射率极差和反射率方差。平均反射率为点云团簇中点的反射率平均值；最大反射率为点云团簇中点的反射率最大值；最小反射率为点云团簇中点的反射率最小值；反射率极差为反射率最大值与最小值之差；反射率方差为点云团簇中点的反射率方差。
[0077]
本发明中提及的多维度点云特征包含具有量纲的矢量/标量和无量纲的矢量/标量。其中，具有量纲的矢量/标量均采用对应国际单位制，不同量纲带来的影响通过归一化方法消除。
[0078]
在本发明中，通过一个具体的例子，说明多维度点云特征的优点。通过点云特征向量的特征因子构成的组合特征(线性、平面性、散度性、全方差、特征熵、迹、曲率变化)，具体定义如下：
[0079]
线性：
[0080]
平面性：
[0081]
散度性：
[0082]
全方差：
[0083]
特征熵：
[0084]
迹：
[0085]
曲率变化：
[0086]
式中，λ1，λ2，λ3为点云特征向量的特征值，且λ1≥λ2≥λ3；e1，e2，e3为特征值比例，通
过公式计算。
[0087]
s2.4、目标分类；
[0088]
基于多维度点云特征中的分类特征组筛选出跟踪目标和非跟踪目标，进一步地，将跟踪目标基于跟踪特征组划分到具体的类别。
[0089]
所述多维度点云特征通过解相关、归一化，可作为特征因子，构成更复杂的分类特征组和跟踪特征组。其中，分类特征组能够作为点云分类的判据；跟踪特征组能够构成差异度函数，为数据关联提供关键判据。
[0090]
具体地，s2.3中所述的多维度点云特征彼此之间存在相关性，通过主成分分析(principal component analysis,pca)、独立成分分析(independent component analysis,ica)、小波包变换(wavelet packet transform)等解相关操作，使其成为相互独立、量纲一致的归一化特征因子。对上述归一化特征因子进行加权组合，获得两类特征组：分类特征组和跟踪特征组。
[0091]
分类特征组：分类特征具有明显的类别相关性，相同类别差异小，不同类别差异大，通过分析可用来区分不同的目标类型。
[0092]
跟踪特征组：跟踪特征的类别相关性较弱，不同的目标个体之间存在显著差异，可用来构建差异度函数，以区分跟踪中的不同目标。
[0093]
所述目标分类包括基于分类特征组典型阈值的跟踪目标筛选和基于分类特征组训练学习的目标形态分类。
[0094]
根据经验构建具体场景下的分类特征组，通过阈值将待分类的点云团簇分为需要进一步分类的跟踪障碍和无需进一步分类的非跟踪障碍；利用机器学习(包括深度学习)方法，通过优质的各类目标点云监督训练，实现对跟踪目标的具体类别进行判别推测。
[0095]
s2.4.1、基于分类特征组典型阈值的跟踪目标筛选；
[0096]
通过目标分割保留的点云团簇，包含跟踪目标和非跟踪目标两类。跟踪目标是具有潜在运动能力的目标，需要对其运动状态进行跟踪；非跟踪目标是不具备运动能力的目标，不需要进行跟踪处理。
[0097]
在限定的应用场景，可能出现的跟踪目标种类是能够预期的，通过数据统计可得出经验阈值，利用典型阈值可筛选出大部分非跟踪目标。具体地，常见的非跟踪目标有如下几类：
[0098]
悬挂物：超过采集平台缓冲高度的目标(不影响通行，在环境地图中忽略)；
[0099]
残留地面：点云滤波处理后残留的地面目标(不影响通行，在环境地图中忽略)；
[0100]
建筑立面：建筑物的墙面、篱笆围栏等目标(不进行跟踪，在环境地图中垂直投影层表征)；
[0101]
多障碍团簇：较多距离相近的独立团簇构成的目标，且其pbr中包含较大可通行区域(不进行跟踪，在环境地图中垂直投影层表征)；
[0102]
s2.4.2、基于分类特征组训练学习的目标形态分类；
[0103]
经过跟踪目标筛选，跟踪目标需要进一步分类为具体的目标类型，根据点云的分类特征组，结合人工标记的类别标签，建立机器学习的训练集。根据具体的应用场景，跟踪
目标的备选类别可能会有一些差别。以城市道路环境为例，常见的跟踪目标类别有行人、车辆和其他目标。进行目标形态学分类的机器学习方法可选择adaboost或pointnet/pointnet++等。
[0104]
adaboost是目前最成功boost方法之一，通过构建一系列弱分类器(结合分类特征组中的特征)，在迭代训练学习的过程中，不断自适应调节各个弱分类器的权重，最终获得能够对训练样本进行准确分类的强分类器。
[0105]
pointnet/pointnet++是典型的深度学习方法，可直接以点云数据和训练标记为输入进行类型训练，相较于需要经过深度图转换进行的基于图像深度学习的点云目标检测识别方法，具有很大的优势，能够挖掘三维点云在目标分类上的深层关联。
[0106]
在本发明中采用自上而下的两级分类方法，对点云团簇进行目标分类。
[0107]
第一级：跟踪目标筛选，利用典型阈值将点云团簇划分为跟踪目标与非跟踪目标。其中，非跟踪目标不再进行进一步的分类(如：悬挂物，残留地面，建筑立面，多障碍团簇)，跟踪目标进行具体类别分类。
[0108]
第二级：目标形态分类，针对跟踪目标，利用分类特征组的独立平权特征组成弱分类器，再经过adaboost算法训练整合为强分类器，实现跟踪目标的具体类别分类(如：行人、车辆、其他)。
[0109]
s3、目标跟踪；
[0110]
根据跟踪目标的类别构造其形态学模型，通过交互式多模型(imm)确定其运动学模型；根据运动学模型选择对应的跟踪器预测目标状态；设计多级感兴趣区域，利用数据关联对跟踪区域中目标检测值与滤波器预测值进行匹配；通过跟踪状态机对目标跟踪标志进行管理；基于大地坐标系，判定目标动静态属性，并估计动态目标运动矢量。
[0111]
s3.1、跟踪模型；
[0112]
为稳定可靠地跟踪目标，需要建立跟踪模型来描述目标的位置和轮廓。因此，根据跟踪目标的具体类别，选择不同的形态学模型和运动学模型能够提高跟踪的准确性。
[0113]
所述形态学模型表征跟踪目标的中心、大小、形状等。对于具体的应用场景中已经分类的具体跟踪目标，选择圆柱体、长方体、平行六面体等形态学模型有利于准确掌握跟踪目标的位置和轮廓信息；
[0114]
所述运动学模型表征跟踪目标的运动方式，对于不同的跟踪目标，可通过交互式多模型(imm)进行自适应判断，结合匀速模型(cv)、匀加速模型(ca)、恒定转弯率和速度幅度(ctrv)等经典模型，描述目标的运动可能性。
[0115]
合适的跟踪模型对目标跟踪的性能具有积极意义。其中，贴合的形态学模型能够提高目标观测值的稳定性；准确的运动学模型能够提高状态估计的精度。
[0116]
s3.1.1、形态学模型；
[0117]
行人与车辆是具有代表性的两种跟踪目标，两者的形态学模型不能一概而论。
[0118]
在本发明中，以行人、车辆和其他类型为例介绍基于类别的形态学模型建立过程。
[0119]
行人目标采用圆柱模型进行形态学描述，且对圆柱模型的直径和高度进行规范约束，限制每个圆柱模型的大小使其仅包含独立的行人目标。行人运动随机性很大，可能短时间内改变朝向和运动方向，因此不设置圆柱模型的形态学朝向。
[0120]
车辆目标采用长方体模型进行形态学描述，且对长方体模型的边界进行拟合补
全，对中心点位置进行校准，避免由于目标之间的相互遮挡或自遮挡导致形态学拟合不完整的问题。车辆通常具有明确的朝向，转向也是连续的过程，因此设置平行于长方体长边的向量作为形态学朝向。
[0121]
其他目标采用直平行六面体模型进行形态学描述，其他类型目标可能是非典型类别的目标或者是多个行人、多个车辆、行人与车辆、车辆/行人与其他类别的聚合体，通过单独特定的模型难以描述其形态特征，因此保留其边界与中心点的位置，且不设置其形态学朝向。
[0122]
s3.1.2、运动学模型；
[0123]
行人与车辆的运动学模型不仅需要考虑其类别，还需要考虑具体的应用场景。
[0124]
在本发明中，使用交互式多模型(interacting multiple model,imm)等自适应运动学模型更符合实际使用需求。
[0125]
行人目标采用匀速模型(constant velocity,cv)进行运动学描述，以应对行人运动的随机性，在较高的采集、检测、跟踪频率下，使用匀速模型也能够对行人目标(相对慢速)进行及时跟踪。
[0126]
车辆目标采用匀加速模型(constant acceleration,ca)和恒定转弯率和速度幅度模型(constant turn rate and velocity,ctrv)进行描述，通过imm自适应判断车辆目标可能的行为(直线行驶或转向)，选择合适的运动学模型。
[0127]
其他目标采用匀速模型进行运动学描述，以应对复杂且无规律的非典型目标或聚合目标，选择匀速模型能够尽量减少模型的不确定性。
[0128]
s3.2、状态估计；
[0129]
状态估计是目标跟踪的关键技术之一，状态估计的预测值作为目标跟踪的结果直接影响跟踪性能。
[0130]
目前，状态估计主要基于经典的相关滤波方式，通过卡尔曼滤波、粒子滤波等跟踪器对跟踪目标进行估计。其中，卡尔曼滤波根据模型是否为线性，又分为线性卡尔曼(kf)、扩展卡尔曼(ekf)和无迹卡尔曼(ukf)。粒子滤波能够处理非高斯分布的问题，但具有更大的运算开销，并不适用于复杂环境下的实时跟踪应用。
[0131]
具体的场景下的状态估计问题，需要考虑方法与模型的适配，包括各传感器的量测模型和目标的状态模型。
[0132]
在本发明中，3d lidar的输出为空间直角坐标系下的数据点，具有线性的量测模型；跟踪目标的状态模型，根据不同的类别，通过自适应方式选择合适的状态模型及其对应的跟踪器。
[0133]
对于行人目标和其他目标，使用cv模型进行描述，其状态转移矩阵为线性，使用线性卡尔曼滤波进行状态估计。
[0134]
对于非转向状态下的车辆目标，使用ca模型进行描述，其状态转移矩阵为线性，使用线性卡尔曼滤波进行状态估计。
[0135]
对于转向状态下的车辆目标，使用ctrv模型进行描述，由于引入转向角，其状态转换矩阵为非线性，使用无迹卡尔曼滤波进行估计。
[0136]
未使用扩展卡尔曼滤波的主要原因是其雅克比矩阵计算的时间开销不稳定，在实时应用中效果不及无迹卡尔曼滤波。
[0137]
s3.3、数据关联；
[0138]
在目标跟踪方法的流程中，数据关联是联系检测器量测值与跟踪器预测值的关键步骤。数据关联将基于上一时刻状态预测的当前状态(先验概率)与实际通过传感器与检测器获取的当前量测进行匹配，进而给出当前时刻的状态(后验概率)。同时，依据当前时刻的状态对跟踪器的参数进行更新迭代，改进或维持跟踪器的可靠性。
[0139]
根据数据关联方法的原理，可以分为确定性方法(如：最近邻、全局最近邻)和概率性方法(如：联合概率数据关联、多假设跟踪)。前者通常构建差异度函数，通过求解差异度函数的最小值得到完备匹配；后者通过概率分布，以求解最优后验概率的角度分析完备匹配。
[0140]
在本发明中，通过构造多级感兴趣区域与目标跟踪窗口确定跟踪目标范围，采用全局最近邻方法进行数据关联二分图构造，并采用km算法进行最佳完备匹配求解。特别地，在进行差异度函数的构造中引入类型转换代价矩阵，提高目标类别对数据关联的影响。
[0141]
s3.3.1、跟踪窗口；
[0142]
在复杂的环境条件下，若将检测器确定的所有检测目标和跟踪器管理的全部跟踪轨迹进行交叉匹配假设，会造成“组合爆炸”的问题，消耗大量的运算资源且可能产生明显的错误。针对这种问题，需要对移动平台和跟踪轨迹两方面进行跟踪窗口的约束。
[0143]
对于移动平台约束，根据计算跟踪目标的pbr中心点到移动平台中心的距离远近，划分多级感兴趣区域(roi)：垂直投影roi、目标跟踪roi、紧急制动roi。其中，
[0144]
垂直投影roi：对roi中的目标仅进行垂直投影，构建环境栅格地图的垂直投影区域。
[0145]
目标跟踪roi：对roi中的目标进行目标跟踪和动静态判断，构建环境栅格地图的静态目标区域和动态目标区域。
[0146]
紧急制动roi：若roi中的出现目标，立即对移动平台发出制动指令，进行应急避障，构建环境栅格地图的紧急制动区域。
[0147]
对于跟踪轨迹约束，由于跟踪目标位置通常具有连续变化的特性，仅跟踪器周围检测到的目标才有可能是匹配跟踪器的检测。因此，以跟踪器为中心构建圆柱形的目标跟踪窗口，仅在目标跟踪窗口内部的检测值才有机会成为与该跟踪器匹配的检测目标。特别地，目标跟踪窗口的参数可能根据跟踪状态的不同进行动态调整以适应不同的跟踪预测置信度。
[0148]
在本发明中，使用由垂直投影roi、目标跟踪roi、紧急制动roi组成的多级感兴趣区域与基于跟踪状态动态调整参数的目标跟踪窗口共同确定跟踪目标的跟踪窗口，提高跟踪效率，避免“组合爆炸”。
[0149]
s3.3.2、全局最近邻；
[0150]
全局最近邻(global nearest neighbor,gnn)作为经典且高效的数据关联方法，在复杂的环境下也能快速反应。全局最近邻的核心是通过综合不同目标对象的跟踪特征，构建差异度函数，以表征关联准确性。通过对差异度函数的最小值进行寻优，得到最优的数据关联结果。
[0151]
在本发明中，差异度函数所采用的特征为跟踪特征组的加权组合，同时引进了一个关键的因子(类别因子)，其具体的表现形式为“类型转换代价矩阵”。根据不同的应用场
景和实际情况可构造针对性的差异度函数以寻求最优效果。下面为关于差异度函数的一个简单举例：
[0152][0153]
其中，change为差异度函数的值，其值越小，代表量测与预测的关联度越高；distance代表量测值与预测值的归一化距离；num
t+1
与num
t
分别代表t+1与t时刻目标点云数目；ai
t+1
、ai
t
、ai
range
分别代表t+1时刻点云平均反射率、t时刻点云平均反射率和反射率范围；classdifference代表类型转换代价矩阵的值；a、b、c、d分别代表各加项的权重值。
[0154]
s3.3.3、类型转换代价矩阵；
[0155]
类型转换代价矩阵是一个实对称方阵(行列顺序相同的前提下)，其行代表检测目标类别，列代表跟踪器类别，对应的值代表从一种类别转换到另外一种类别的转换代价，该代价与可能性成反比。通过设置不同类型之间的转换代价值，能够将“类别”这一个无量纲且难以归一化的重要参数引入差异度函数，进而作为其中的一个重要因子。
[0156]
下面为本发明关于类型转换代价矩阵的一个简单举例：
[0157][0158]
其中，classdifference代表类型转换代价矩阵的值；pedestrian、vehicle、other分别代表行人、车辆、其他跟踪目标，
“×”
连接代表两种不同类别的目标之间转换的代价值，矩阵的值仅作为举例参考。
[0159]
s3.3.4、最佳完备匹配求解；
[0160]
对于差异度函数最小值的求解，通常将其抽象为有权二分图描述，并求其最小权完备匹配。具体地，二分图的左侧顶点集合是检测目标，右侧顶点集合是跟踪器，其中位于跟踪器的目标跟踪窗口中的检测目标与其具有边连接，其边的权值为差异度函数的值；在跟踪器目标跟踪窗口外的检测目标与其不具备连接，其边的权值等价于一个极大值。
[0161]
一般地，如果二分图的每条边都有一个权(可以是负数)，求解一种完备匹配方案，使得所有匹配边的权和最大，记做最佳完备匹配。对于需要求解最小权值完备匹配的应用，只需要将各边权值取相反数，求解最佳完备匹配即可。
[0162]
若每条边的权值都相同，二分图的匹配问题可以使用经典的匈牙利方法求解；对于非等权重的二分图匹配问题，可以采用km(kuhn and munkres)方法，km方法是对匈牙利方法的一种贪心扩展，通过引入可行标杆和相等子图的概念实现对有权二分图最大(最小)权重的最佳完备匹配的求解。通过km方法，可对由差异度函数值构成的二分图进行求解，获得当前时刻检测器与跟踪器之间的最佳完备匹配。
[0163]
在本发明中，通过将检测目标与跟踪器间的匹配问题抽象为有权二分图最小权完备匹配问题，利用差异度函数的值与目标跟踪窗的连通性判断，获得最佳完备匹配。
[0164]
s3.4、跟踪管理；
[0165]
跟踪管理设计具有6种状态的跟踪状态机，状态包括跟踪初始、跟踪保持、跟踪丢失、跟踪寻回、跟踪终止、跟踪无效。同时，规定了各个跟踪状态之间的跟踪状态转换规则，实现对跟踪目标的可靠、稳定的跟踪管理。
[0166]
s3.4.1、跟踪状态机设计；
[0167]
跟踪roi中每个目标都可能出现各种状态，具有各自的生存周期，需要使用特定的跟踪状态标记进行管理。
[0168]
跟踪初始：目标被检测到并满足跟踪条件，进行跟踪器初始化；
[0169]
跟踪保持：目标被连续检测并满足跟踪条件，跟踪器利用匹配的检测器信息更新参数；
[0170]
跟踪丢失：已跟踪目标未能被检测或不满足跟踪条件，跟踪器继续预测但不进行更新；
[0171]
跟踪寻回：丢失跟踪的目标在寻回期限内被检测且满足跟踪条件，跟踪器再次匹配更新；
[0172]
跟踪终止：丢失跟踪的目标在寻回期限内未能被检测或不满足跟踪条件，跟踪器被删除；
[0173]
跟踪无效：目标尚未进行跟踪初始化或跟踪已终止产生的无效状态。
[0174]
本发明设计了跟踪状态机，并规定了6种跟踪状态以表征目标当前的跟踪状况，同时，为了定量描述各跟踪状态及其对应转换关系，建立了一套跟踪状态标记进行管理。
[0175]
定义跟踪状态标志为0到10的整数，其中0表示跟踪无效，1-4代表跟踪初始，5代表跟踪保持，6-9代表跟踪丢失/寻回，10代表跟踪终止。
[0176]
s3.4.2、跟踪状态转换规则；
[0177]
在复杂多变的环境中，符合逻辑、满足闭环的定量跟踪状态转换规则，能够很好地处理突然出现或消失的目标、目标之间的遮挡、合并、分裂等情况，提高跟踪的鲁棒性和效率。
[0178]
在本发明中，在6种典型状态的基础上，基于跟踪状态标志，构建一套完整闭环的跟踪状态转换规则。
[0179]
一个典型的跟踪状态转换周期的示例如下：
[0180]
跟踪状态标志从0开始，经过1-4逐渐建立跟踪；在跟踪初始阶段，有效的跟踪使标志递增，无效的跟踪使标志递减，直至到达5稳定跟踪状态或到达0无效；跟踪保持状态下目标，若持续有效跟踪，则保持标志为5；若出现无效跟踪，则标志递增，进入跟踪丢失状态，若在到达10之前出现有效跟踪，标志位递减；若经过跟踪寻回标志位递减到达5，则回到跟踪保持状态；若跟踪标志位到达10，则寻回目标失败，进入跟踪终止状态0，删除跟踪器并进入跟踪无效状态。
[0181]
特别地，对于跟踪丢失和寻回状态的跟踪器，将采用更大半径的目标跟踪窗口以弥补失去更新带来的跟踪器不确定性增加，提高跟踪目标寻回成功率。
[0182]
s3.5、动静目标判定；
[0183]
基于融合点云数据检测跟踪的目标信息，相比以移动平台为观测中心的相对观测，能够通过其绝对位置，直接判断其相对于大地坐标系的位移，即真实的动静态信息。动静态目标判定对于环境地图中静态目标区域和动态目标区域的生成十分关键，进而影响局部路径规划和行为预测等后续处理的可靠性和准确性。
[0184]
动静态目标判定，通过基于跟踪状态保持统计的稳定跟踪判断保证结果有效性；通过滑动窗口去除最值和次最值，消除单次奇异点带来的影响；通过计算平均有效位移，结
合典型阈值给出目标的动静态属性。
[0185]
s3.5.1、稳定跟踪判断；
[0186]
仅在稳定跟踪的情况下，目标的位置信息才具有参考价值。因此，结合跟踪管理状态机，若目标的跟踪状态为跟踪保持，则进行一次稳定计数。统计固定时间窗口内目标稳定计数的值，若超过预先设定的阈值，则视为稳定跟踪，可进行下一步的动静态判断，否则标记目标的运动状态为未知。
[0187]
s3.5.2、奇异点滤除；
[0188]
复杂环境中可能出现干扰因素，导致传感器的采集噪声或检测器的检测误判，主要表现为位姿或点云的突然变化带来的目标位置突变。动静态目标判断对可能出现的干扰因素采取滑动窗口处理，从中去除最大值、次大值、最小值和次小值，消除单个奇异点带来的位移影响。
[0189]
s3.5.3、平均有效位移；
[0190]
实际上，受限于传感器精度、时间同步精度、检测算法稳定性、特征提取算法准确度等因素，静止目标在两帧之间的位移也不完全为零。平均有效位移即为经过稳定跟踪判断为稳定状态，且数据经过奇异点滤除后的位移的平均值。通过设置阈值，将平均有效位移小于阈值的目标视为静态目标，反之，平均有效位移大于阈值的目标视为动态目标，并计算其运动的矢量(速度方向和大小)。
[0191]
在本发明中，跟踪目标的位移数据经过稳定跟踪判断、奇异点滤除的处理，计算出平均有效位移并与阈值进行比较，最终获得具有一定参考价值的跟踪目标基于大地坐标系下的动静态属性判断，以及相对于地面的运动矢量。
[0192]
s4、环境地图生成；
[0193]
环境地图作为目标跟踪的结果的直观表达形式，也是路径规划流程的输入，对地图要素的选择影响局部路径规划的效率和准确性。通常，根据表达形式，环境地图可分为栅格地图、几何地图和拓扑地图。其中栅格地图能够准确描述环境的细节，相比几何地图和拓扑地图在导航避障方面具有描述简单、复杂度可控、构成灵活等优点。
[0194]
根据移动平台和环境的具体情况，确定环境地图要素，将不同避障级别中的目标分别表示，最终输出矩阵形式的栅格环境地图，并通过位图形式进行直观可视化。
[0195]
s4.1、地图要素选择；
[0196]
地图要素选择，首先根据应用的具体场景，选择栅格地图作为载体，规定坐标原点、坐标轴和分辨率等关键要素。特别地，栅格地图的分辨率，即粒度是关键的参数，过大的粒度会导致地图信息精度下降，无法准确描述目标细节，而过小的粒度会造成存储和运算的负担，影响应用的实时性。
[0197]
在本发明中的栅格地图基于大地坐标系，坐标原点选择移动平台初始位置以减小地图面积开销；采用标准的东北天轴向，与大地坐标系一致；选择0.5m作为每个栅格的边长以平衡准确度与实时性。
[0198]
s4.2、地图表示形式；
[0199]
s4.2.1矩阵表示；
[0200]
栅格环境地图作为导航避障或行为预测等应用的输入，采用在时间和空间效率上较高的矩阵表示。具体地，将栅格中的每个格子代表的区域类型进行分类，并对跟踪目标进
行id编码。
[0201]
具体地，本发明对于区域类型进行了如下分类：
[0202]
可通行区域：使用数字0进行编码，代表可以通行安全区域。
[0203]
垂直投影区域：使用数字1进行编码，代表有非跟踪目标存在，不可通行的区域。
[0204]
静态目标区域：使用数字2后加id进行编码，代表跟踪目标中的静态障碍，设置较小的避障缓冲区。
[0205]
动态目标区域：使用数字3后加id进行编码，代表跟踪目标中的动态障碍，根据运动方向和速度，设置不对称的缓冲区。
[0206]
紧急制动区域：使用数字4进行编码，代表进入紧急制动范围的目标，作为紧急制动指令出发的条件之一。
[0207]
栅格环境地图的矩阵表示中每个栅格的内容构成如下：
[0208]
zonetype+targetid
[0209]
其中，zonetype代表区域类型，zonetype∈{0,1,2,3,4}；targetid代表目标的编号，可以规定其位数以适配具体的应用。
[0210]
s4.2.2、位图表示；
[0211]
在自动驾驶应用中，驾驶员(或操作员)的决策优先级必须高于自动驾驶系统，因此环境地图需要能够以直观的可视化形式以便驾驶员理解并做出决策。
[0212]
通过位图表示，将不同的区域标记上不同的颜色或填充，以描述当前环境中的目标分布情况。
[0213]
具体地，位图表示为：可通行区域(白色或无填充)、垂直投影区域(黑色或不透明全填充)、静态目标区域(蓝色或斜线填充)、动态目标区域(黄色或半透明全填充)、紧急制动区域(红色或交叉线填充)。
[0214]
本发明提供一种应用所述基于多维度点云特征的目标检测跟踪方法的系统，包括融合数据采集模块、目标检测模块、目标跟踪模块和环境地图生成模块。
[0215]
与现有技术相比，本发明的有益效果在于：
[0216]
本发明通过基于多维度点云特征的分析组合，能够实现无人车、机器人等移动平台在复杂地面环境下对动静态目标的准确、稳定检测与跟踪，为避障、行为预测等提供可靠参考。
附图说明
[0217]
图1为本发明的基于多维度点云特征的目标检测跟踪方法的流程图；
[0218]
图2为本发明的方法结构图；
[0219]
图3为本发明的多维度点云特征层次框图；
[0220]
图4为本发明的跟踪状态机的标志转换规则图；
[0221]
图5为本发明实施例中lidar采集到的点云数据；
[0222]
图6为本发明实施例中的经过时空校正的融合点云数据效果图；
[0223]
图7为本发明实施例中的已滤波点云数据效果图；
[0224]
图8为本发明实施例中的点云团簇效果图；
[0225]
图9为本发明实施例中目标检测效果图；
[0226]
图10为本发明实施例中目标跟踪效果图；
[0227]
图11为本发明实施例中环境地图效果图；
[0228]
图12为本发明的系统模块划分图。
具体实施方式
[0229]
下面结合附图和实施例对本发明进行进一步说明。
[0230]
本实施例以农田环境下的无人驾驶拖拉机为移动平台，对本发明目标检测与识别的数据采集与处理的具体实施方法进行举例说明。
[0231]
如图1和图2所示，本发明的基于多维度点云特征的目标检测跟踪方法，包括如下步骤：
[0232]
s1、融合数据采集；
[0233]
s1.1、多传感器安装配置；
[0234]
无人驾驶拖拉机顶部装配3d lidar，实时获取环境中的三维点云信息；所获得lidar点云数据如图5所示。
[0235]
无人驾驶拖拉机顶部装配双天线rtk-gnss移动站，接收基站偏差信号后获得高精度的移动平台的定位与航向信息。
[0236]
无人驾驶拖拉机顶部靠近转向中心的位置装配ahrs，获得移动平台的姿态信息。
[0237]
s1.2、多传感器时间同步；
[0238]
无人驾驶拖拉机的融合数据采集终端为车载移动工作站。通过软件同步方式，车载移动工作站读取自身硬件时钟，顺次快速读取3d lidar、rtk-gnss、ahrs缓冲区中的数据，保证各传感器数据采集时间上的一致性。具体地，数据采集的同步频率为10hz，能够满足农田环境下的无人驾驶拖拉机的实际应用实时性。
[0239]
s1.3、多坐标系空间配准；
[0240]
无人驾驶拖拉机采集的点云数据，经过坐标投影转换、刚体坐标变换、实时位姿转换实现点云数据从传感器坐标系到大地坐标系的空间配准。所获得经过时空校正的融合点云数据如图6所示。
[0241]
s2、目标检测；
[0242]
s2.1、目标分割；
[0243]
s2.1.1、地面点滤波；
[0244]
融合点云数据通过限制法向量方向的随机采样一致性的平面拟合方法，提取地面点云索引，进而获取非地面点云。具体地，对拟合平面的法向量方向进行限制：仅当拟合平面法向量方向与竖直方向小于30
°
情况下的拟合平面视为有效地地面模型，并依此滤除非地面点云；否则，重新选择拟合平面进行验证。
[0245]
s2.1.2、离群点滤波；
[0246]
通过条件滤波器建立两层剪裁窗口，外层剪裁窗口滤除过高过远点云，内层剪裁窗口滤除无人驾驶拖拉机自身点云，进而获得已滤波点云数据。具体地，外层剪裁窗口的形状为长方体，对处于长方体外的点云数据进行滤除，仅保留内部点云；内层剪裁窗口的形状为长方体，对处于长方体内的点云进行滤除，仅保留外部点云。
[0247]
经过地面点滤波与离群点滤波的已滤波点云数据中不同目标具有相对离散的分
布，可进行下一步的点云聚类。所获得已滤波点云数据如图7所示。
[0248]
s2.1.3、点云聚类；
[0249]
使用经典的欧氏聚类方法实现点云聚类，进而获得点云团簇。特别地，欧氏聚类距离阈值2.5m的设置基于无人驾驶拖拉机实际尺寸，即无人驾驶拖拉机的宽度2.05m(阈值的设置经过安全缓冲区膨胀)，降低了由虚假可通行区域产生的误报率。所获得点云团簇如图8所示。
[0250]
s2.2、包围盒拟合；
[0251]
在对点云团簇的包围盒拟合过程结合obb、pbr和ubb包围盒(框)，以obb为包围拟合基础，通过pbr和ubb校正点云团簇可能存在的拟合失真，为多维度几何特征提取提供依据。所获得点云团簇的ubb如图9中的长方形包围框(编号1和3)。
[0252]
s2.3、多维度点云特征提取；
[0253]
通过多维度点云特征，根据点云团簇的点云分布特征、点云邻域特征、点云材质特征进行系统归纳整理，通过多维度点云特征可组合出分类特征组和跟踪特征组，分别为点云分类与目标跟踪提供依据。所述多维度点云特征如图3所示。
[0254]
s2.4、目标分类；
[0255]
采用自上而下的两级分类方法，对点云团簇进行目标分类。
[0256]
s2.4.1、基于分类特征组典型阈值的跟踪目标筛选；
[0257]
第一级：跟踪目标筛选，利用典型阈值将点云团簇划分为跟踪目标与非跟踪目标。其中，非跟踪目标不再进行进一步的分类(如：悬挂物，残留地面，建筑立面，多障碍团簇)，跟踪目标进行具体类别分类。所述跟踪目标如图9中具有长方形包围框(编号1和3)的点云团簇，非跟踪目标如图9中不具备包围框的点云团簇。
[0258]
s2.4.2、基于分类特征组训练学习的目标形态分类；
[0259]
第二级：目标形态分类，针对跟踪目标，利用分类特征组的独立平权特征组成弱分类器，再经过adaboost算法训练整合为强分类器，实现跟踪目标的具体类别分类(如：行人、车辆、其他)。所述跟踪目标的具体类别如图9中的文字标注(tractor、pedestrian)所示。
[0260]
s3、目标跟踪；
[0261]
s3.1、跟踪模型；
[0262]
s3.1.1、形态学模型；
[0263]
以行人、车辆和其他类型为例介绍基于类别的形态学模型建立过程。其中，行人目标采用圆柱模型进行形态学描述，限制大小但不设置形态学朝向；车辆目标采用长方体模型进行形态学描述，进行边界补全和中心位置校准且设置长边方向为形态学朝向；其他目标采用直平行六面体模型进行形态学描述，不设置形态学朝向。
[0264]
s3.1.2、运动学模型；
[0265]
使用交互式多模型自适应运动学模型更符合实际使用需求。
[0266]
行人目标和其他目标采用cv模型进行运动学描述；车辆目标采用ca模型(直线行驶)或ctrv模型(转向)进行描述，通过imm选择合适的运动学模型。
[0267]
s3.2、状态估计；
[0268]
3d lidar的输出为空间直角坐标系下的数据点，具有线性的量测模型；跟踪目标的状态模型，根据不同的类别，通过自适应方式选择合适的状态模型及其对应的跟踪器。
[0269]
对于行人目标和其他目标，使用cv模型进行描述，其状态转移矩阵为线性，使用kf进行状态估计。
[0270]
对于非转向状态下的车辆目标，使用ca模型进行描述，其状态转移矩阵为线性，使用kf进行状态估计。
[0271]
对于转向状态下的车辆目标，使用ctrv模型进行描述，由于引入转向角，其状态转换矩阵为非线性，使用ukf进行估计。
[0272]
s3.3、数据关联；
[0273]
s3.3.1、跟踪窗口；
[0274]
使用由垂直投影roi、目标跟踪roi、紧急制动roi组成的多级感兴趣区域与基于跟踪状态动态调整参数的目标跟踪窗口共同确定跟踪目标的跟踪窗口。所述垂直投影roi与外层剪裁窗口的边界一致，目标跟踪roi与紧急制动roi如图10中的圆柱形边界(编号4，图中仅部分显示)和圆柱形边界(编号2)所示。目标跟踪窗口的中心点与边界如图10中的实心球体(编号5和6)和圆柱形边界(编号1和3)所示。
[0275]
s3.3.2、全局最近邻；
[0276]
采用基于差异度函数的全局最近邻方法进行检测目标与跟踪器间的数据关联。
[0277]
s3.3.3、类型转换代价矩阵；
[0278]
引入了一个关键的类型转换代价矩阵，作为类别因子定量描述不同类别的目标之间转换的代价值。
[0279]
s3.3.4、最佳完备匹配求解；
[0280]
通过将检测目标与跟踪器间的匹配问题抽象为有权二分图最小权完备匹配问题，利用差异度函数的值与目标跟踪窗的连通性判断，选择km算法进行求解，获得最佳完备匹配。所述最佳完备匹配结果如图10中“tracker”与“detection”后数字表示。
[0281]
s3.4、跟踪管理；
[0282]
s3.4.1、跟踪状态机设计；
[0283]
设计了跟踪状态机，并规定了6种跟踪状态以表征目标当前的跟踪状况。具体地，跟踪状态包括：跟踪初始、跟踪保持、跟踪丢失、跟踪寻回、跟踪终止、跟踪无效。为了定量描述各跟踪状态及其对应转换关系，建立了一套跟踪状态标记进行管理。所述跟踪状态对应的跟踪状态标记。
[0284]
s3.4.2、跟踪状态转换规则；
[0285]
在6种典型状态的基础上，基于跟踪状态标志，构建一套完整闭环的跟踪状态转换规则，如图4所示。
[0286]
s3.5、动静态目标判定；
[0287]
s3.5.1、稳定跟踪判断；
[0288]
跟踪目标的位移数据经过稳定跟踪判断，排除了因检测器不稳定等带来的无效数据的干扰，仅在规定时间间隔内具有超过阈值数量的跟踪保持状态的目标满足稳定跟踪的条件。
[0289]
s3.5.2、奇异点滤除；
[0290]
进一步通过去除有效位移数据中的最大值、次大值、最小值和次小值，排除单次奇异点数据给数据整体带来的干扰。
[0291]
s3.5.3、平均有效位移；
[0292]
通过对经过稳定跟踪判断和奇异点滤除处理的数据取平均值，得到平均有效位移，将其与设定的阈值进行参考，从而判断出跟踪目标的动静态属性及其运动矢量。所述动静态属性如图10中的“move”或“static”所示，所述运动矢量的速度值如图10中数字所示，单位“m/s”。
[0293]
s4、环境地图生成；
[0294]
s4.1、地图要素选择；
[0295]
栅格地图基于大地坐标系，坐标原点选择移动平台初始位置以减小地图面积开销；采用标准的东北天轴向，与大地坐标系一致；选择0.5m作为每个栅格的边长以平衡准确度与实时性。
[0296]
s4.2、地图表示形式；
[0297]
s4.2.1、矩阵表示；
[0298]
其中值zonetype代表区域类型和targetid代表目标id
[0299]
栅格环境地图的矩阵表示中每个栅格的内容构成如下：
[0300]
zonetype+targetid
[0301]
其中，zonetype代表区域类型，zonetype∈{0,1,2,3,4}；targetid代表目标的编号。具体地，
[0302]
可通行区域：使用数字0进行编码，代表可以通行安全区域。
[0303]
垂直投影区域：使用数字1进行编码，代表有非跟踪目标存在，不可通行的区域。
[0304]
静态目标区域：使用数字2后加id进行编码，代表跟踪目标中的静态障碍，设置较小的避障缓冲区。
[0305]
动态目标区域：使用数字3后加id进行编码，代表跟踪目标中的动态障碍，根据运动方向和速度，设置不对称的缓冲区。
[0306]
紧急制动区域：使用数字4进行编码，代表进入紧急制动范围的目标，作为紧急制动指令出发的条件之一。
[0307]
s4.2.2、位图表示；
[0308]
通过位图表示，将不同的区域标记上不同的颜色或填充，以描述当前环境中的目标分布情况。
[0309]
具体地，位图表示为：可通行区域(白色或无填充)、垂直投影区域(黑色或不透明全填充)、静态目标区域(蓝色或斜线填充)、动态目标区域(黄色或半透明全填充)、紧急制动区域(红色或交叉线填充)。
[0310]
所述位图表示的示例如图11所示，其中通过无填充代表可通行区域，通过不透明全填充代表垂直投影区域，通过半透明全填充代表动态目标区域。
[0311]
如图12所示，本发明提供一种应用所述基于多维度点云特征的目标检测跟踪方法的系统，包括融合数据采集模块、目标检测模块、目标跟踪模块和环境地图生成模块。