本发明属于自动驾驶领域，尤其是涉及一种基于激光传感器的快速运动障碍物实时跟踪方法。
背景技术：
在电子信息化的当今社会，目标的检测与跟踪在军用领域和民用领域都发挥着非常重要的作用。在军事应用领域，精确的检测和跟踪敌方目标是精确打击对方的基础，也是军事实力的重要体现。在民用方面，检测与跟踪也广泛应用于自动驾驶、无人机、机器人等领域。
其中所运用的粒子滤波技术是在贝叶斯滤波框架下，通过动态搜寻出一组状态空间中随机的带有权重的样本点集合，对检测目标后验概率密度p(Xk|Z1:k)进行近似拟合，以样本点的加权平均值来替代概率密度函数的积分运算，从而获得目标动态运动状态的次优估计得一种算法。
如果在T时刻检测并跟踪到的车辆为快速运动车辆，则其真实状态和位置为跟踪到的假阳性结果。随着时间持续会衍生出更多的假阳性结果，这样会大大的造成不必要的计算负担，影响跟踪效果，甚至系统崩溃。
而且传统粒子滤波算法在检测和跟踪性能很大程度上取决于粒子的数量，特别是在真实世界的场景中，如果目标太小、检测环境嘈杂、障碍物存在遮挡隐蔽，以及物体的快速出现或者消失等情况下，这将直接导致计算量的指数级增加以及实时性跟踪性能的降低，往往导致跟踪的误检率也较高。
技术实现要素：
有鉴于此，本发明旨在提出一种可以降低计算量、减小系统功耗，降低环境感知误检率，提高系统稳定性和安全性的基于激光传感器的快速运动障碍物实时跟踪方法。
为达到上述目的，本发明的技术方案是这样实现的：
一种基于激光传感器的快速运动障碍物实时跟踪方法，包括如下步骤：
(1)将激光雷达安装在车身上，以激光雷达的中心建立坐标系，并对坐标系进行内参标定和外参标定；
(2)通过激光雷达扫描自车周围，利用空间数据聚类算法组成障碍物；
(3)对步骤(2)所述的障碍物进行状态分类，包括初观测组和非初观测组；
(4)初次观测组的障碍物设置初始状态规则；
(5)对非初次观测组和已设置初始状态的初次观测组的障碍物均进行动态跟踪；
(6)生成障碍物信息列表。
进一步，步骤(1)所述的内参标定包括旋转校正角度、垂直校正角度、距离校正角度、垂直便宜因子以及水平便宜因子；外参标定包括：激光雷达坐标系相对于自车坐标系的旋转矩阵R和平移矩阵T。
进一步，步骤(1)所述的激光雷达为单线激光雷达或多线激光雷达或全向激光雷达；所述的单线激光雷达安装在车身四周，，多线激光雷达安装在车身两侧，所述的全向激光雷达安装在车身顶部。
进一步，步骤(2)所述的空间数据聚类算法为K-means算法，包括通过激光点云聚类，组成符合障碍物规则的高斯分布，判断为障碍物。
进一步，步骤(5)所述的初始状态规则包括结构化道路障碍物、非结构化道路两侧障碍物和非结构化道路中间障碍物。
进一步，所述的车身上还安装有摄像头、惯导设备和GPS接收器。
进一步，步骤(5)所述的动态跟踪，包括将摄像头所采集的周围环境信息和激光雷达所采集的障碍物信息通过PF粒子滤波模型进行融合关联，使用KF扩展卡尔曼滤波模型生成跟踪信息。
相对于现有技术，本发明所述的一种基于激光传感器的快速运动障碍物实时跟踪方法具有以下优势：
本发明对跟踪障碍物的初始状态进行优化，加入了障碍物跟踪的新参考状态及观测状态，作为一种盒式粒子改进滤波跟踪算法，通过对区间进行分别处理，在获取同等检测跟踪效果的同时，有效的降低计算量和算法的总耗时量，大大提高跟踪的实时性；本发明计算量小、跟踪效果好，可以在激光跟踪领域中被广泛使用。
附图说明
构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1为本发明实施例所述的运行流程图。
具体实施方式
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”等的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以通过具体情况理解上述术语在本发明中的具体含义。
下面将参考附图并结合实施例来详细说明本发明。
一种基于激光传感器的快速运动障碍物实时跟踪方法，包括如下步骤：
(1)将激光雷达安装在车身上，以激光雷达的中心建立坐标系，并对坐标系进行内参标定和外参标定；
(2)通过激光雷达扫描自车周围，利用空间数据聚类算法组成障碍物；
(3)对步骤(2)所述的障碍物进行状态分类，包括初观测组和非初观测组；
(4)初次观测组的障碍物设置初始状态规则；
(5)对非初次观测组和已设置初始状态的初次观测组的障碍物均进行动态跟踪；
(6)生成障碍物信息列表。
步骤(1)所述的内参标定包括旋转校正角度、垂直校正角度、距离校正角度、垂直便宜因子以及水平便宜因子；外参标定包括：激光雷达坐标系相对于自车坐标系的旋转矩阵R和平移矩阵T；所述的激光雷达为单线激光雷达或多线激光雷达或全向激光雷达；所述的单线激光雷达安装在车身四周，多线激光雷达安装在车身两侧，坐标系中心均为自车，所述的全向激光雷达安装在车身顶部，以全向激光雷达为坐标系中心。
步骤(2)所述的空间数据聚类算法为K-means算法，包括通过激光点云聚类，组成符合障碍物规则的高斯分布，判断为障碍物。
步骤(5)所述的初始状态规则包括结构化道路障碍物、非结构化道路两侧障碍物和非结构化道路中间障碍物；所述的动态跟踪，包括将摄像头所采集的周围环境信息和激光雷达所采集的障碍物信息通过PF粒子滤波模型进行融合关联，使用KF扩展卡尔曼滤波模型生成跟踪信息。
所述的车身上还安装有摄像头、惯导设备和GPS接收器。
具体的工作运行流程：
首先，将三维激光雷达安装在车辆上，以激光雷达的中心为其坐标系，需要对其进行内参标定和外参标定。内参标定包括旋转校正角度、垂直校正角度、距离校正角度、垂直便宜因子以及水平便宜因子。外参标定包括：激光雷达坐标系相对于自车坐标系的旋转矩阵R和平移矩阵T。
其中，所述的激光雷达，如果采用单线激光雷达，则其具有结构简单、功耗低、使用方便等优点，用于障碍物检测、道路路沿检测、路面检测。但其只有一根扫描线，所以也存在信息量少等缺点，导致很难获得完整障碍物的大小信息，以及在道路复杂情况下，车辆的遮挡也很难被检测跟踪到；如果采用的多线激光雷达，则其不仅可以实现单线雷达全部功能，还可以检测到障碍物的高度信息，但精度不足、系统复杂度加大，数据量增加且处理难度加大。如果采用全向激光雷达，与单线、多线激光雷达相比，其可视范围和环境信息的丰富性、细节上都有极大提升，能够获得尽可能大范围的探测，同时获得尽可能小的遮挡区域。此外，为了避免车顶对三维全向激光雷达的遮挡，三维全向激光雷达的安装应至少高于车顶0.5米。
然后，激光雷达扫描自车周围环境并识别和分类：
通过自车装载的激光雷达传感器不停扫描自车周围环境，通过空间数据聚类方法，把点云数据分割成不同的空间子集，根据一些常用的聚类算法，如K-means算法，进行点云聚类。将可以聚在一起的点云数据抽象成一个障碍物。根据模型的宽度，将其进行分类：如车辆、自行车、行人等。
此后，根据道路模型优化跟踪目标初始状态：
对于障碍物(如跟踪车辆)，通过关联判断其是否为初次观测对象，如果不是，则根据卡尔曼滤波模型更新其状态(位置、速度、角速度、方向角)；如果是初次观测到的对象，则根据自车所在的道路模型(如车辆所在车道、限速信息)等，给被跟踪车辆的初始状态设置为一定规则下的随机状态。以下为赋予初始状态的实施例：
最后，障碍物的动态跟踪：
获取目标的运动信息，对目标进行跟踪。采用常速度卡尔曼滤波模型，在过程误差协方差中考虑加速度，设状态变量为
X-[x,y,vx,vy,ψ,ω]T
其中，(x,y)表示跟踪目标的位置，((vx,vy)为跟踪目标的速度，ψ为跟踪目标的方向角，ω为跟踪目标的角速度。
卡尔曼滤波器的运动模型和观测模型分别为：
xk＝Fkxk-1+wk-1
zk＝Hkxk+vk
其中，Fk与Hk是已知系统矩阵和测量矩阵，wk-1和vk分别为均值为0，方差为Qk-1和Rk并相互独立的过程噪声和测量噪声。
xk表示为跟踪障碍物的位置、速度、加速度、角速度等信息。zk表示为在k时刻，各类型传感器的测量值。
卡尔曼滤波时间更新方程为：
测量更新方程为：
Pk+1|k+1＝(I-Kk+1Hk+1)Pk+1|k
其中，K为卡尔曼滤波的增益矩阵。
本发明将一定规则加入到跟踪物体的状态，跟踪物体的观测状态，通过对区间进行分别处理，在获取同等检测跟踪效果的同时，有效的降低计算量和算法的总耗时量，大大提高跟踪的实时性。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。