[发明专利]一种基于三维激光雷达的目标车辆航向角计算方法

权利要求书

1.一种基于三维激光雷达的目标车辆航向角计算方法，其特征在于包括以下步骤：

1)采用安装在自车顶部的三维激光雷达采集目标车辆，得到目标车辆的点云数据；

2)在雷达坐标系中建立目标车辆某时刻所对应的航向角参数模型；

3)根据目标车辆的点云数据计算目标车辆的分布类型，具体过程为：

目标车辆的形状类型采用如下概率模型进行描述：

式中，M_t为目标车辆的分布类型，X_t为t时刻目标车辆的位姿,x_t,y_t为目标车辆中心参考点坐标；

基于目标车辆的目标位置与航向角独立性假设，可得：

由于此时刻目标方位未知，因此采用点云分布方差之比τ_t来近似替代，其计算方法为：视点云为二维高斯分布，则有：

式中，σ_max，σ_min分别为最大、最小正态方差，则可得：

p(M_t|X_t)＝p(M_t|x_t,y_t)p(M_t|τ_t)

对于p(M_t|x_t,y_t)，采用如下离散概率经验值：

式中，根据实际监测区域和矩形格子经验参数大小将雷达坐标系识别区域分为A₁,A₁₂,A₂,A₂₃,A₃,A₃₄,A₄,A₁₄八个矩形子区域，在A_i＝1,2,3,4区域包围体由于自遮挡导致部分尾部和侧部数据丢失，称此区域为自遮挡区；区域A₁₂,A₃₄则能够获取较准确的车辆长度参数，区域A₁₄,A₂₃则能够获取较准确宽度参数，称此区域为半自遮挡区，R1为A₁₄,A₂₃的并集，即R1＝A₁₄∪A₂₃， R2为剩余区域A₁,A₁₂,A₂,A₃,A₃₄,A₄的并集；

对于p(M|τ_t)采用如下经验值：

最终采用下式计算两种目标类型的概率：

如果p(M＝"L")>p(M＝"I")则认为目标车辆的分布类型为"L"，反之则认为目标车辆的分布类型为"I"；

4)根据目标车辆的分布类型分别对目标车辆点云进行聚类，获得感兴趣区域，其中，感兴趣区域指的是表征航向角的主要部分；

5)根据聚类结果按照目标车辆的分布类型分别将两类点云成分进行直线拟合；

6)结合目标车辆的分布类型及相应的两类点云成分直线拟合结果计算目标车辆的航向角。

2.如权利要求1所述的一种基于三维激光雷达的目标车辆航向角计算方法，其特征在于，对获取的目标车辆的航向角采用线性卡尔曼滤波器进行滤波。

3.如权利要求1或2所述的一种基于三维激光雷达的目标车辆航向角计算方法，其特征在于，步骤4)根据目标车辆的分布类型分别对目标车辆点云进行聚类，获得感兴趣区域，具体过程为：

4.1)基于K均值算法对某一分布类型的目标车辆点云进行初始聚类；

4.2)采用混合高斯算法对初始聚类后的目标车辆点云进行再聚类，提取得到相应分布类型的感兴趣区域；

4.3)经过再聚类后，从聚类结果中选择一类较优的点云进行航向角计算，具体选择过程为：对于“L”型点云，将两类点云视为二维正态分布，分别计算其方差比值，并选择方差比值较大的一类作为感兴趣区域点云；对于“I”型点云，选择靠近自车的部分作为感兴趣点云，如果目标车辆位于自车前方，则选择其尾部点云；如果位于后方则选择车头部分点云。

4.如权利要求1或2所述的一种基于三维激光雷达的目标车辆航向角计算方法，其特征在于，步骤6)结合目标车辆的分布类型及相应的两类点云成分直线拟合结果计算目标车辆的航向角，具体过程为：

6.1)对于“L”型目标车辆，直接以感兴趣区域内点云拟合的直线作为航向角方向；

6.2)对于“I”型目标车辆，以感兴趣区域内点云拟合直线的正交直线作为航向角方向。