CN105223583A - 一种基于三维激光雷达的目标车辆航向角计算方法 - Google Patents

Abstract

本发明涉及一种基于三维激光雷达的目标车辆航向角计算方法，其特征在于包括以下步骤：1)采用安装在自车顶部的三维激光雷达采集目标车辆，得到目标车辆的点云数据；2)在雷达坐标系中建立目标车辆某时刻所对应的航向角参数模型；3)根据目标车辆的点云数据计算目标车辆的分布类型；4)根据目标车辆的分布类型分别对目标车辆点云进行聚类，获得感兴趣区域，其中，感兴趣区域指的是表征航向角的主要部分；5)根据聚类结果按照目标车辆的分布类型分别将两类点云成分进行直线拟合；6)结合目标车辆的分布类型及相应的两类点云成分直线拟合结果计算目标车辆的航向角。本发明可以广泛应用于智能车环境感知技术领域。

Description

一种基于三维激光雷达的目标车辆航向角计算方法

技术领域

本发明涉及现代交通智能车辆环境感知技术领域，特别是关于一种基于三维激光雷达的目标车辆航向角计算方法。

背景技术

三维激光雷达能够捕获目标物基本形状特征及丰富局部细节，具有可靠性及测量精度高等优点，目前被广泛应用在智能车辆环境感知中。作为感知的重要内容，车辆目标航向角在判断目标换道意图等行为中具有重要作用，是智能车辆路径规划及决策算法的基础。但是由于三维激光雷达的固有特性，车辆目标点云分布受到观测点位置及自遮挡等因素的影响，使得目标航向角难以精确计算。

当前研究中车辆目标主要使用轴平行包围体(AixeAlignBoundingBox，AABB)来表示，即无论目标处于何种姿态，其矩形包围体边界一直平行于雷达坐标轴。这种方法忽略了车辆的航向角属性，显然不利于分析车辆的横向行为，对感知而言也是不完整的。针对这种情况，部分研究采用主成分分析方法计算航向角：首先将原始三维点云俯视投影降维，而后采用主成分分析来计算二维点云特性向量，选用对应特征值较大的特征向量方向作为包围体航向角方向。在此基础上，一些研究先采用统计分析方法去除异值点云，考虑目标自遮挡特性，主成分分析方法往往不能获取到目标真实运动方向，因此采用随机抽样一致性算法来计算车辆航向角，但是实际应用中难以去除异值点云，因此限制了该方法的应用。

发明内容

针对上述问题，本发明的目的是提供一种检测精度较高、可靠性较强的基于三维激光雷达的目标车辆航向角计算方法。

为实现上述目的，本发明采取以下技术方案：一种基于三维激光雷达的目标车辆航向角计算方法，其特征在于包括以下步骤：1)采用安装在自车顶部的三维激光雷达采集目标车辆，得到目标车辆的点云数据；2)在雷达坐标系中建立目标车辆某时刻所对应的航向角参数模型；3)根据目标车辆的点云数据计算目标车辆的分布类型；4)根据目标车辆的分布类型分别对目标车辆点云进行聚类，获得感兴趣区域，其中，感兴趣区域指的是表征航向角的主要部分；5)根据聚类结果按照目标车辆的分布类型分别将两类点云成分进行直线拟合；6)结合目标车辆的分布类型及相应的两类点云成分直线拟合结果计算目标车辆的航向角。

进一步，对获取的目标车辆的航向角采用线性卡尔曼滤波器进行滤波。

进一步，步骤3)根据目标车辆的点云数据计算目标车辆的分布类型，具体过程为：目标车辆的形状类型采用如下概率模型进行描述：

式中，M_t为目标车辆的分布类型，X_t为t时刻目标车辆的位姿；基于目标车辆的目标位置与航向角独立性假设，可得：

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

$\mathrm{\τ}=\frac{{\mathrm{\σ}}_{max}}{{\mathrm{\σ}}_{min}}$

式中，σ_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)，采用如下离散概率经验值：

$\left\{\begin{array}{c}p\left(M{=}^{\′\′}{L}^{\′\′}\right|R1)=0.1\\ p\left(M{=}^{\′\′}{I}^{\′\′}\right|R1)=0.9\\ p\left(M{=}^{\′\′}{L}^{\′\′}\right|R2)=0.9\\ p\left(M{=}^{\′\′}{I}^{\′\′}\right|R2)=0.1\end{array}\right.$

式中，A_i＝1,2,3,4区域包围体为自遮挡区，A₁₂,A₃₄A₁₄,A₂₃区域为半自遮挡区；R1＝A₁₄∪A₂₃，R2为剩余区域A₁,A₁₂,A₂,A₃,A₃₄,A₄的并集；

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

$\left\{\begin{array}{c}p\left(M{=}^{\&prime;\&prime;}{L}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t<2)=0.3\\ p\left(M{=}^{\&prime;\&prime;}{I}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t<2)=0.7\\ p\left(M{=}^{\&prime;\&prime;}{L}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t\&GreaterEqual;2)=0.7\\ p\left(M{=}^{\&prime;\&prime;}{I}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t\&GreaterEqual;2)=0.3\end{array}\right.$

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

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

进一步，步骤4)根据目标车辆的分布类型分别对目标车辆点云进行聚类，获得感兴趣区域，具体过程为：4.1)基于K均值算法对某一分布类型的目标车辆点云进行初始聚类；4.2)采用混合高斯算法对初始聚类后的目标车辆点云进行再聚类，提取得到相应分布类型的感兴趣区域；4.3)经过再聚类后，从聚类结果中选择一类较优的点云进行航向角计算，具体选择过程为：对于“L”型点云，将两类点云视为二维正态分布，分别计算其方差比值，并选择方差比值较大的一类作为感兴趣区域点云；对于“I”型点云，选择靠近自车的部分作为感兴趣点云，如果目标车辆位于自车前方，则选择其尾部点云；如果位于后方则选择车头部分点云。

进一步，步骤6)结合目标车辆的分布类型及相应的两类点云成分直线拟合结果计算目标车辆的航向角，具体过程为：6.1)对于“L”型目标车辆，直接以感兴趣区域内点云拟合的直线作为航向角方向；6.2)对于“I”型目标车辆，以感兴趣区域内点云拟合直线的正交直线作为航向角方向。

本发明由于采取以上技术方案，其具有以下优点：1、本发明根据目标车辆的分布类型分别对目标车辆点云进行聚类，获得感兴趣区域，根据聚类结果按照目标车辆的分布类型分别将两类点云成分进行直线拟合；并结合目标车辆的分布类型及相应的两类点云成分直线拟合结果计算目标车辆的航向角，由于本发明针对目标点云采用先分块聚类，后进行主成分拟合，因此受目标自遮挡影响较小，能够有效提高航向角计算精度，检测精度较高。2、本发明对获取的目标车辆的航向角采用线性卡尔曼滤波器进行滤波，因此能够避免受点云分布复杂性影响，进而得到稳定、可靠的航向角。本发明可以广泛应用于智能车环境感知技术领域。

附图说明

图1是本发明的目标车辆航向角计算方法流程示意图；

图2是本发明的目标航向角参数示意图；

图3是本发明的目标形状类型示意图，其中，图(a)是ROI为“L”型示意图，图(b)是ROI为“I”型示意图；

图4是本发明的目标车辆位置分布示意图，OBB指的是目标车辆；

图5是本发明的点云聚类流程示意图；

图6是本发明的航向角计算流程示例示意图，其中，图(a)是“L”模型点云航向角计算流程示意图，图(b)是“I”模型点云航向角计算流程示意图。

具体实施方式

以下结合附图来对本发明进行详细的描绘。然而应当理解，附图的提供仅为了更好地理解本发明，它们不应该理解成对本发明的限制。

如图1所示，本发明的基于三维激光雷达的目标车辆航向角计算方法，包括以下步骤：

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

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

如图2所示，本发明中目标车辆采用矩形定向包围体表示，在雷达坐标系Oxy下，t时刻目标车辆的参数可以表示为：

B_t＝(X_t,G_t)

式中，为目标车辆位姿模型，x_t,y_t为目标车辆中心参考点坐标，单位为m；为目标车辆航向角，即目标车辆行驶方向同x轴夹角，单位为度；G_t＝(l_t,w_t)为目标车辆几何形状模型，其中，l_t,w_t为t时刻测量的矩形定向包围体长宽，单位为m。

3、根据目标车辆的点云数据计算目标车辆的分布类型。

如图3所示，目标车辆点云数据的平面形状主要呈“L”及“I”型。由于点云分布主要受目标车辆的位置、姿态影响，因此目标车辆的形状类型采用如下概率模型进行描述：

式中，M_t为目标车辆的分布类型(“L”或“I”型)，X_t为t时刻目标车辆的位姿。

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

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

$\mathrm{\&tau;}=\frac{{\mathrm{\&sigma;}}_{max}}{{\mathrm{\&sigma;}}_{min}}$

式中，σ_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)，由于难以用概率密度函数来精确表示，本发明采用如下离散概率经验值：

$\left\{\begin{array}{c}p\left(M{=}^{\&prime;\&prime;}{L}^{\&prime;\&prime;}\right|R1)=0.1\\ p\left(M{=}^{\&prime;\&prime;}{I}^{\&prime;\&prime;}\right|R1)=0.9\\ p\left(M{=}^{\&prime;\&prime;}{L}^{\&prime;\&prime;}\right|R2)=0.9\\ p\left(M{=}^{\&prime;\&prime;}{I}^{\&prime;\&prime;}\right|R2)=0.1\end{array}\right.$

如图4所示，根据实际监测区域和矩形格子经验参数大小将雷达坐标系识别区域分为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)，本发明同样采用如下经验值：

$\left\{\begin{array}{c}p\left(M{=}^{\&prime;\&prime;}{L}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t<2)=0.3\\ p\left(M{=}^{\&prime;\&prime;}{I}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t<2)=0.7\\ p\left(M{=}^{\&prime;\&prime;}{L}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t\&GreaterEqual;2)=0.7\\ p\left(M{=}^{\&prime;\&prime;}{I}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t\&GreaterEqual;2)=0.3\end{array}\right.$

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

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

4、如图5所示，根据目标车辆的分布类型分别对目标车辆点云进行聚类，获得感兴趣区域(ROI)，如图3所示，对目标车辆点云聚类的目的是为表征航向角的主要部分提取出来作为感兴趣区域(ROI)，以作为下一步直线拟合的基础，具体步骤为：

1)基于K均值(Kmeans)算法对某一分布类型的目标车辆点云进行初始聚类，聚类数量设置为2；

2)采用混合高斯(GMM)算法对初始聚类后的目标车辆点云进行再聚类，提取得到相应分布类型的感兴趣区域，本发明实施例中混合高斯算法迭代次数设置为10。如图6(a)和(b)中的第二列所示，实心、空心圆分别表示两类点云成分，可见混合高斯算法能够将“L”与“I”两种类型目标的点云成分较好的分离开。

3)经过再聚类后，感兴趣区域的选择实际上是从聚类结果中(实心、空心点云)选择一类较优的点云进行航向角计算，具体选择步骤如下：

对于“L”型点云，将两类点云(实心、空心)视为二维正态分布，分别计算其方差比值，并选择方差比值较大的一类作为感兴趣区域点云，例如图6(a)中的实心点云。

对于“I”型点云，选择靠近自车的部分作为感兴趣点云，如果目标车辆位于自车前方，则选择其尾部点云；如果位于后方则选择车头部分点云，如图6(b)中的实心点云所示。

5、根据聚类结果按照目标车辆的分布类型分别将两类点云成分进行直线拟合，本发明所采用的直线拟合方式为随机抽样一致性算法(RANSAC)，直线拟合所选用的参数如表1所示：

表1

6、结合目标车辆的分布类型及相应的两类点云成分直线拟合结果计算目标车辆的航向角，具体过程为：

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

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

7、对计算得到的目标车辆的航向角进行滤波。

本发明是基于单帧目标点云的航向角计算，但是受点云分布复杂性影响，需要对计算得到的航向角值进行滤波，进而得到稳定、可靠的航向角值，具体过程为：

1)建立常横摆角速度模型：

式中，为目标航向角，ω为横摆角速度，为高斯白噪声，T为相邻两帧时间间隔。

2)t时刻的状态预估。

在t-1时刻，根据步骤3～6计算车辆目标航向角，假设航向角计算值表示为根据常横摆角速度模型对t时刻目标航向角值进行预估，设预估航向角表示为

3)t时刻数据关联。

对于t时刻的一个车辆目标，基于最近邻关联方法搜索其在t-1时刻的关联目标，可通过调节跟踪门限大小限来提高效率和关联准确率；

4)t时刻状态更新。

在t-1时刻，根据步骤1～5对已关联的目标航向角进行计算，设航向角测量计算值为结合步骤2)中的航向角预估值来计算滤波值由于所建立的常横摆速度模型为线性，在各初始状态呈高斯分布的假定下，可以直接利用线性卡尔曼滤波器来处理及最终得到t时刻航向角滤波值

上述各实施例仅用于说明本发明，其中方法的各实施步骤等都是可以有所变化的，凡是在本发明技术方案的基础上进行的等同变换和改进，均不应排除在本发明的保护范围之外。

Claims (6)

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

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

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

3)根据目标车辆的点云数据计算目标车辆的分布类型；

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

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

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

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

3.如权利要求1所述的一种基于三维激光雷达的目标车辆航向角计算方法，其特征在于，步骤3)根据目标车辆的点云数据计算目标车辆的分布类型，具体过程为：

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

式中，M_t为目标车辆的分布类型，X_t为t时刻目标车辆的位姿；

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

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

$\mathrm{\&tau;}=\frac{{\mathrm{\&sigma;}}_{max}}{{\mathrm{\&sigma;}}_{min}}$

式中，σ_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)，采用如下离散概率经验值：

$\left\{\begin{array}{c}p\left(M{=}^{\&prime;\&prime;}{L}^{\&prime;\&prime;}\right|R1)=0.1\\ p\left(M{=}^{\&prime;\&prime;}{I}^{\&prime;\&prime;}\right|R1)=0.9\\ p\left(M{=}^{\&prime;\&prime;}{L}^{\&prime;\&prime;}\right|R2)=0.9\\ p\left(M{=}^{\&prime;\&prime;}{I}^{\&prime;\&prime;}\right|R2)=0.1\end{array}\right.$

式中，A_i＝1,2,3,4区域包围体为自遮挡区，A₁₂,A₃₄A₁₄,A₂₃区域为半自遮挡区；R1＝A₁₄∪A₂₃，R2为剩余区域A₁,A₁₂,A₂,A₃,A₃₄,A₄的并集；

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

$\left\{\begin{array}{c}p\left(M{=}^{\&prime;\&prime;}{L}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t<2)=0.3\\ p\left(M{=}^{\&prime;\&prime;}{I}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t<2)=0.7\\ p\left(M{=}^{\&prime;\&prime;}{L}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t\&GreaterEqual;2)=0.7\\ p\left(M{=}^{\&prime;\&prime;}{I}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t\&GreaterEqual;2)=0.3\end{array}\right.$

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

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

4.如权利要求2所述的一种基于三维激光雷达的目标车辆航向角计算方法，其特征在于，步骤3)根据目标车辆的点云数据计算目标车辆的分布类型，具体过程为：

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

式中，M_t为目标车辆的分布类型，X_t为t时刻目标车辆的位姿；

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

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

$\mathrm{\&tau;}=\frac{{\mathrm{\&sigma;}}_{max}}{{\mathrm{\&sigma;}}_{\min }}$

式中，σ_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)，采用如下离散概率经验值：

$\left\{\begin{array}{c}p\left(M{=}^{\&prime;\&prime;}{L}^{\&prime;\&prime;}\right|R1)=0.1\\ p\left(M{=}^{\&prime;\&prime;}{I}^{\&prime;\&prime;}\right|R1)=0.9\\ p\left(M{=}^{\&prime;\&prime;}{L}^{\&prime;\&prime;}\right|R2)=0.9\\ p\left(M{=}^{\&prime;\&prime;}{I}^{\&prime;\&prime;}\right|R2)=0.1\end{array}\right.$

式中，A_i＝1,2,3,4区域包围体为自遮挡区，A₁₂,A₃₄A₁₄,A₂₃区域为半自遮挡区；R1＝A₁₄∪A₂₃，R2为剩余区域A₁,A₁₂,A₂,A₃,A₃₄,A₄的并集；

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

$\left\{\begin{array}{c}p\left(M{=}^{\&prime;\&prime;}{L}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t<2)=0.3\\ p\left(M{=}^{\&prime;\&prime;}{I}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t<2)=0.7\\ p\left(M{=}^{\&prime;\&prime;}{L}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t\&GreaterEqual;2)=0.7\\ p\left(M{=}^{\&prime;\&prime;}{I}^{\&prime;\&prime;}\right|{\mathrm{\&tau;}}_t\&GreaterEqual;2)=0.3\end{array}\right.$

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

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

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

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

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

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

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

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

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