[发明专利]一种基于多传感器的障碍物速度融合方法和装置

权利要求书

1.一种基于多传感器的障碍物速度融合方法，其特征在于，该方法包括：

步骤101)获取自车车身信息、车辆运行周围环境中的激光点云数据和毫米波雷达的障碍物数据；

步骤102)分别对激光点云和毫米波雷达的数据进行处理，得到障碍物的速度及其置信度；

步骤103)基于障碍物历史轨迹信息和自车车身信息，判断障碍物相对自车的运动趋势，采取不同方法融合障碍物的速度；

步骤104)基于自车车身信息确定障碍物的绝对速度。

2.根据权利要求1所述的基于多传感器的障碍物速度融合方法，其特征在于，所述步骤102)中：

对毫米波雷达的数据进行处理时，选择对毫米波雷达的速度进行跟踪处理，计算得到较为准确的毫米波雷达障碍物的横纵向速度，并分别计算其横纵向速度的置信度；

对激光点云的数据进行处理，得到激光雷达障碍物的位置，并根据位置对时间微分求得速度，分别计算其横纵向速度的置信度。

3.根据权利要求2所述的基于多传感器的障碍物速度融合方法，其特征在于，对所述激光点云的数据进行处理时，选择对激光点云的数据进行聚类处理和跟踪，求得激光雷达障碍物的速度，具体包括：

对激光点云进行聚类，计算聚类后障碍物的几何重心，作为障碍物的位置，对该位置进行跟踪处理，得到准确的激光雷达障碍物的位置；

对激光雷达障碍物的位置与时间进行微分求得速度，对速度进行跟踪处理，得到准确的激光雷达障碍物的速度，并分别计算其横纵向速度的置信度。

4.根据权利要求1所述的基于多传感器的障碍物速度融合方法，其特征在于，所述步骤103)中：

障碍物，为传感器测量的障碍物与融合列表中障碍物进行关联匹配上的障碍物；

障碍物历史轨迹信息，为融合列表中保存的障碍物的位置、速度、类别和形状大小信息；

自车车身信息，至少包括自车的速度、航向角、航向角速度信息；

根据障碍物历史轨迹信息，计算其速度的方向角与自车的航向角的差异，判断障碍物相对自车的运动趋势；

根据障碍物相对自车的运动趋势，采取不同的方法融合障碍物的速度。

5.根据权利要求4所述的基于多传感器的障碍物速度融合方法，其特征在于，所述判断障碍物相对自车的运动趋势的方法如下：

首先，保存自车的航向角历史帧数据，对数据进行过滤平滑，计算出自车当前航向角与历史帧航向角差异，若差异超过设定的阈值，则判断自车相对障碍物在转弯状态，障碍物与自车同时转弯情形也作此处理；

其次，保存障碍物历史轨迹信息，并判断与自车的相对运动状态，障碍物相对自车速度方向为theta，取x和y分别为相对自车的位置，其计算公式为：theta＝arcsin(y/sqrt(x*x+y*y))，如果thetaPI/2.0,并且theta＝PI,则theta＝PI-diff_angle；如果thetaPI,并且theta＝3*PI/2.0,则theta＝diff_angle-PI；如果theta3*PI/2.0,并且theta＝2*PI,则theta＝2*PI-theta，其中PI为圆周率；

最后，判断theta是否超过设定的阈值，若超过，则判断障碍物与自车在相对转弯状态，否则，判断障碍物与自车在同向或者相向状态。

6.根据权利要求4所述的基于多传感器的障碍物速度融合方法，其特征在于，采取不同的方法融合障碍物的速度，包括：

根据障碍物相对自车的运动趋势，分为障碍物与自车运动是同向或者相向，以及除此之外的情形；

当障碍物与自车运动是同向或者相向时，优先选择毫米波雷达的纵向速度以及激光雷达的横向速度作为测量值，与预测值进行加权融合；

当障碍物与自车运动为上述之外的情形时，优先选择激光雷达的纵向速度和横向速度作为测量值，与预测值进行加权融合；

更新历史列表上障碍物速度的滤波器，以及历史轨迹信息。