CN114612665B - 基于法向量直方图特征的位姿估计及动态车辆检测方法 - Google Patents

Abstract

本发明公开了一种基于法向量直方图特征的位姿估计及动态车辆检测方法，包括以下步骤：S1.通过激光雷达扫描获取检测区域的激光点云数据，并对激光点云数据进行预处理，得到地上点云簇；S2.根据预处理后得到的地上点云簇进行动态目标检测；S3.对检测得到的动态目标进行位姿估计，并判断检测得到的动态目标是否为动态车辆。本发明利用车辆表面法向量分布特性提取目标特征，并利用模型和特征拟合车辆所处位姿，以提高位姿估计准确性，进而提高了远距离稀疏点云场景下动态车辆检测准确性。

Description

基于法向量直方图特征的位姿估计及动态车辆检测方法

技术领域

本发明涉及动态车辆检测，特别是涉及基于法向量直方图特征的位姿估计及动态车辆检测方法。

背景技术

基于模型的方法是一种有效的车辆检测方法。例如可利用矩形作为车辆测量模型，结合方向向量、角点、垂直边中心点等表示车辆；可利用多个嵌套矩形构建似然场测量模型，利用似然函数表征模型拟合程度，并结合一些规则进行目标筛选；由于激光雷达视角下车辆多呈现两条边可见的状态，也可构建L-型模型进行车辆检测。因此，车辆模型以及拟合函数的构建是基于模型方法需重点解决的问题，而目前已有的模型在进行拟合过程中存在易陷入局部极值，以及存在环境干扰或远距离稀疏点云下车辆特征不明显等问题。鉴于此，本发明提出了一种新的车辆测量模型，利用车辆表面法向量分布特性提取车辆特征，拟合车辆所处位姿，并基于此完成场景中的动态车辆提取。

似然场车辆测量模型如图1所示，四个矩形嵌套构成车辆测量模型，其中R0表示车辆可见长边区域，R1表示车辆可见短边区域，R2表示车辆内部，R3表示车辆外部。利用似然概率表示某点云簇与模型拟合程度，具体实现方案如下：

将3D点云投影到2D平面，将点云建模为二维正态分布：

其中σ表示测量噪声方差。假设点云簇中的任一测量值相互独立，则车辆测量模型拟合程度可定义为：

其中，η_i为归一化常数，

为积分归一化常数，c_j,j＝0,...,3为各区域权重，I(·)为积分操作。

但是现有似然场测量模型在确定目标位姿时容易陷入局部极值，出现位姿估计错误，尤其对于稀疏点云场景下，目标信息缺失，位姿估计误差更大，降低了目标检测准确性。

发明内容

本发明的目的在于克服现有技术的不足，提供一种基于法向量直方图特征的位姿估计及动态车辆检测方法，利用车辆表面法向量分布特性提取目标特征，并利用模型和特征拟合车辆所处位姿，以提高位姿估计准确性，进而提高了远距离稀疏点云场景下动态车辆检测准确性。

本发明的目的是通过以下技术方案来实现的：基于法向量直方图特征的位姿估计及动态车辆检测方法，包括以下步骤：

S1.通过激光雷达扫描获取检测区域的激光点云数据，并对激光点云数据进行预处理，得到地上点云簇；

所述步骤S1包括：

S101.地面点云分割：

通过激光雷达扫描获取检测区域的激光点云数据后，通过RANSAC、EKF方法对地面进行平面拟合，并获取点云与拟合平面间的垂距离，对于任一点云，若与拟合平面的垂距离小于预先设定的阈值T_g，则为地面点云，反之则为地上点云；或是根据GPR构建地面高度模型，将各点云处的预测高度与真实高度进行比较，若预测高度大于等于真实高度，则该点为地面点云，反之，则该点为地上点云；

对地面点云进行分割滤除，保留地上点云；

S102.地上点云聚类:

使用RBNN聚类方法将地上点云划分为不同的地上点云簇。

S2.根据预处理后得到的地上点云簇进行动态目标检测；

所述步骤S2包括：

S201.将地上点云簇进行水平面极坐标网格投影，没有点云投影的网格标记为“自由”状态，点云簇投影中距离激光雷达最近的一层网格定义为“占用”状态，其他包含该点云簇的网格定义为“遮挡”状态；

S202.将连续两帧点云首先利用GPS/INS数据进行坐标系统一，随后比较极坐标网格中的状态变化，并设定状态变化的网格的数量阈值，若网格变化数量大于阈值，则该点云簇被标记为动态目标，若网格变化数量不足阈值，则认为是测量噪声或环境干扰引起，将点云簇标记为非动态目标。

S3.对检测得到的动态目标进行位姿估计，并判断检测得到的动态目标是否为动态车辆。

所述步骤S3包括：

S301.对于检测得到的动态目标进行位姿估计，包括点云法向量求解、法向量直方图特征计算和利用法向量直方图特征进行位姿估计；

所述点云法向量求解的过程包括：

针对某动态目标o，假设其包含的点云为P，P中任一点p_i的法向量计算过程为：以p_i为球心，r为球半径做虚拟球，该球包围的所有点表示为P_r＝{p_j|p_j∈P,||p_j-p_i||＜r}，矩阵P_r中的每一行表示一个点，使用

分别表示P_r中的各列，表示球中所有点的x,y,z坐标；

使用P_r的协方差矩阵C_Pr＝(c_mn)_3×3获取p_i的法向量，其中：

其中τ表示球P_r中点的个数，m,n∈{1,2,3}；计算

的特征值和特征向量，最小特征值对应的特征向量

为点p_i的法向量；

根据球坐标表示，单位法向量n_i通过n_i与+z轴间的角

n_i在x-y平面投影与+x轴间的夹角

来确定，n_i坐标与该角关系如下：

其中

为了表示上的统一，使用n_i与+x轴间的夹角

以及与+y轴间的夹角

来替代

其中

n_i与各坐标轴正向的夹角由下式得出：

其中d_c表示各坐标轴单位向量，n_i为单位法向量，<·>表示内积运算，||·||表示向量求模运算。

所述法向量直方图特征计算的过程包括：

构建法向量直方图特征测量模型：设R0为激光雷达视角下的可见长边，R1为可见短边，R2为车辆内部，R3为可见角点；目标局部坐标系原点O位于目标中心位置，y轴与不可见短边法向量方向同向，x轴为y轴顺时针旋转90度；L为模型长度，W为模型宽度，Lv为可见边宽度；

将区间[0,π]等分为b_num个小区间，统计落入每个小区间中的角度的数量，得到直方图表示：

其中b＝1,2,...,b_num，card(·)表示集合的基数，c∈{x,y,z}；

根据车辆测量模型，分别统计落入R₀-R₃区域中的点，计算各点法向量与各坐标轴夹角的分布情况，每个区域中的直方图特征定义为三个轴的夹角的分布直方图拼接：

其中特征

的长度为3×b_num。

所述利用法向量直方图特征进行位姿估计的过程包括：

对于某地上动态目标oc，首先获得该动态目标中各点的法向量Nc，随后将三维点云投影到水平面中，利用Graham算法获取该动态目标投影的凸包络P_K，随后利用凸包络确定包围该动态目标的K种不同位姿下的包围框，具体过程如下：

首先从P_K中选取相邻两点，计算两点确定的直线line1，随后从P_K中挑选距离line1最远的点，过该点做直线平行于line1，记为line2，接下来，从P_K中挑选line1投影中横坐标最小和最大的点，分别过这两点，做垂直于line1的直线，记为line3和line4；至此，四条直线可确定一种方位下的包围框，P_K共可确定K个不同位姿的包围框；

确定包围框后，以该包围框中心坐标作为初始位置，所处方向角作为初始方向角，确定初始位姿(c_x0,c_y0,θ₀)；随后提取该位姿下动态目标的法向量直方图特征，并与事先手动选择的车辆真值法向量直方图特征进行模板匹配，使用K-L散度计算匹配程度，K-L散度公式如下：

其中ω_k为权重系数，b_num为直方图间隔数；κ越小，表示特征与模板越匹配；令

若当前动态目标存在运动朝向信息t_p,则与模板匹配得分计算如下：

其中系数η₁+η₂＝1，

表示该动态目标局部坐标系y轴方向向量；总得分S越小，说明该位姿估计越准确；

随后从凸包络P_K中选择随后的两相邻点完成上述过程，共计算获得κ个K-L散度值；从中选择最小的K-L散度，对应位姿下的包围框确定为最佳包围框，所处位姿确定为该动态目标位姿。

S302.在当前时刻获取某点云簇的位姿后，沿该点云簇所处方向角进行位置关联；

所述步骤S302包括：

在当前时刻获取某动态目标的位姿后，沿该动态目标所处方向角进行位置关联，寻找前一时刻该动态目标可能的所处位置，预测位置如下：

其中θ为上述过程得出的最佳姿态角，Δk为时间间隔，v_rand为[-v,v]中随机选择的速度值，v可取25m/s；

表示k时刻动态目标所处位置；

在预测位置处进行点云簇搜索，如果该位置附近存在点云簇，则认为在上一时刻存在关联点云簇，利用前述位姿估计方法估计上一时刻关联点云簇的位姿，如果点云簇与当前时刻动态目标的方向角差值小于设定阈值，则较大概率点云簇与动态目标为同一物体，进行后续处理；若方向角偏差过大，则认为点云簇与动态目标不是同一物体，尝试m次位置预测，若都不满足方向角变化小于某阈值，则当前时刻的动态目标不是车辆；

同理，在后一时刻进行运算时，同样选取关联点云簇，与上式不同的是，v_rand替换为根据上一时刻点云簇和当前时刻动态目标位姿差计算得到的速度值，同样，若存在关联的点云簇，对后一时刻关联点云簇进行位姿估计。

S303.通过运动一致性原理进行是否属于车辆的判断，从而实现动态车辆的检测。

根据步骤S302获取连续三个时刻中具有关联性的点云簇和动态目标的三个位姿值，并通过计算得到两个时间间隔该点云簇的速度值，随后通过运动一致性原理对该点云簇是否属于车辆进行判断：

运动一致性是指车辆在运动过程中的速度和方向角应具有一定稳定性，变化在一定阈值范围内；若违反，则该动态目标不是动态车辆，因此，若连续三帧数据中某动态目标及其前后两时刻的关联点云簇状态满足这一原则，则判断为动态车辆。

本发明的有益效果是：本发明利用车辆表面法向量分布特性确定车辆位姿，提高位姿估计准确性，随后将位姿估计方法引入动态车辆检测过程中，提高远距离稀疏点云场景下动态车辆检测准确性。

附图说明

图1为车辆似然场测量模型示意图；

图2为本发明的方法流程图；

图3为动态车辆检测过程示意图；

图4为车辆在激光雷达视角下法向量分布示意图；

图5为法向量直方图特征测量模型示意图；

图6为模板直方图分布示意图；

图7为基于法向量直方图特征的位姿估计算法流程示意图；

图8为包围框确定过程示意图。

具体实施方式

下面结合附图进一步详细描述本发明的技术方案，但本发明的保护范围不局限于以下所述。

如图2所示，基于法向量直方图特征的位姿估计及动态车辆检测方法，包括以下步骤：

S1.通过激光雷达扫描获取检测区域的激光点云数据，并对激光点云数据进行预处理，得到地上点云簇；

所述步骤S1包括：

S101.地面点云分割：

通过激光雷达扫描获取检测区域的激光点云数据后，通过RANSAC、EKF方法对地面进行平面拟合，并获取点云与拟合平面间的垂距离，对于任一点云，若与拟合平面的垂距离小于预先设定的阈值T_g，则为地面点云，反之则为地上点云；或是根据GPR构建地面高度模型，将各点云处的预测高度与真实高度进行比较，若预测高度大于等于真实高度，则该点为地面点云，反之，则该点为地上点云；

对地面点云进行分割滤除，保留地上点云；

S102.地上点云聚类:

使用RBNN聚类方法将地上点云划分为不同的地上点云簇。

S2.根据预处理后得到的地上点云簇进行动态目标检测；

所述步骤S2包括：

S201.将地上点云簇进行水平面极坐标网格投影，没有点云投影的网格标记为“自由”状态，点云簇投影ong距离激光雷达最近的一层网格定义为“占用”状态，其他包含该点云簇的网格定义为“遮挡”状态；

S202.将连续两帧点云首先利用GPS/INS数据进行坐标系统一，随后比较极坐标网格中的状态变化，并设定状态变化的网格的数量阈值，若网格变化数量大于阈值，则该点云簇被标记为动态目标，若网格变化数量不足阈值，则认为是测量噪声或环境干扰引起，将点云簇标记为非动态目标。

S3.对检测得到的动态目标进行位姿估计，并判断检测得到的动态目标是否为动态车辆。

如图3所示，所述步骤S3包括：

S301.对于检测得到的动态目标进行位姿估计，包括点云法向量求解、法向量直方图特征计算和利用法向量直方图特征进行位姿估计；

所述点云法向量求解的过程包括：

针对某动态目标o，假设其包含的点云为P，P中任一点p_i的法向量计算过程为：以p_i为球心，r为球半径做虚拟球，该球包围的所有点表示为P_r＝{p_j|p_j∈P,||p_j-p_i||＜r}，矩阵P_r中的每一行表示一个点，使用

分别表示P_r中的各列，表示球中所有点的x,y,z坐标；

使用P_r的协方差矩阵

获取p_i的法向量，其中：

其中τ表示球P_r中点的个数，m,n∈{1,2,3}；计算

的特征值和特征向量，最小特征值对应的特征向量

为点p_i的法向量；

根据球坐标表示，单位法向量n_i通过n_i与+z轴间的角

n_i在x-y平面投影与+x轴间的夹角

来确定，n_i坐标与该角关系如下：

其中

为了表示上的统一，使用n_i与+x轴间的夹角

以及与+y轴间的夹角

来替代

其中

n_i与各坐标轴正向的夹角由下式得出：

其中d_c表示各坐标轴单位向量，n_i为单位法向量，<·>表示内积运算，||·||表示向量求模运算。

所述法向量直方图特征计算的过程包括：

图4所示为车辆在激光雷达视角下可见表面法向量分布情况示意图；

如图5所述构建法向量直方图特征测量模型：设R0为激光雷达视角下的可见长边区域，R1为可见短边区域，R2为车辆内部，R3为可见角点；目标局部坐标系原点O位于目标中心位置，y轴与不可见短边法向量方向同向，x轴为y轴顺时针旋转90度；L为模型长度，W为模型宽度，Lv为可见边区域的宽度；随后依据各区域相应点云法向量分布情况确定模型拟合程度，利用各区域点云法向量与三坐标轴夹角的直方图构建法向量直方图特征，如图6所示为模型法向量分布示意图。

将区间[0,π]等分为b_num个小区间，统计落入每个小区间中的角度的数量，得到直方图表示：

其中b＝1,2,...,b_num，card(·)表示集合的基数，c∈{x,y,z}；

根据车辆测量模型，分别统计落入R₀-R₃区域中的点，计算各点法向量与各坐标轴夹角的分布情况，每个区域中的直方图特征定义为三个轴的夹角的分布直方图拼接：

其中特征

的长度为3×b_num。

如图7所示，所述利用法向量直方图特征进行位姿估计的过程包括：

对于某地上动态目标o，首先获得该动态目标中各点的法向量Nc，随后将其三维点云投影到水平面中，利用Graham算法获取该动态目标投影的凸包络P_K，随后利用凸包络确定包围该动态目标的K种不同位姿下的包围框，具体过程如下：

首先从P_K中选取相邻两点，如图8(1)所示，以P1和P2为例，计算两点确定的直线line1,如图8(2)所示，随后从P_K中挑选距离line1最远的点，图8(3)可知为P6，过该点做直线平行于line1，记为line2，接下来，从P_K中挑选line1投影中横坐标最小和最大的点，如图8(4)-(5)可知为P5和P1，分别过这两点，做垂直于line1的直线，记为line3和line4。至此，四条直线可确定一种方位下的包围框，如图8(6)所示。P_K共可确定K个不同位姿的包围框；

确定包围框后，以该包围框中心坐标作为初始位置，所处方向角作为初始方向角，确定初始位姿(c_x0,c_y0,θ₀)；随后提取该位姿下动态目标的法向量直方图特征，并与事先手动选择的车辆真值法向量直方图特征进行模板匹配，使用K-L散度计算匹配程度，K-L散度公式如下：

其中ω_k为权重系数，b_num为直方图间隔数；κ越小，表示特征与模板越匹配；令

若当前动态目标存在运动朝向信息t_p,则与模板匹配得分计算如下：

其中系数η₁+η₂＝1，

表示该动态目标局部坐标系y轴方向向量；总得分S越小，说明该位姿估计越准确；

随后从凸包络P_K中选择随后的两相邻点完成上述过程，共计算获得κ个K-L散度值；从中选择最小的K-L散度，对应位姿下的包围框确定为最佳包围框，所处位姿确定为该动态目标位姿。

S302.在当前时刻获取某动态目标的位姿后，沿该动态目标所处方向角进行位置关联；

所述步骤S302包括：

在当前时刻获取某动态目标的位姿后，沿该动态目标所处方向角进行位置关联，寻找前一时刻该动态目标可能的位置，预测位置如下：

其中θ为上述过程得出的最佳姿态角，Δk为时间间隔，v_rand为[-v,v]中随机选择的速度值，v可取25m/s；

表示k时刻动态目标所处位置；

在预测位置处进行点云簇搜索，如果该位置附近存在点云簇，则认为在上一时刻存在关联点云簇，利用前述位姿估计方法估计上一时刻关联点云簇的位姿，如果点云簇与当前时刻动态目标的方向角差值小于设定阈值，则较大概率点云簇与动态目标为同一物体，进行后续处理；若方向角偏差过大，则认为点云簇与动态目标不是同一物体，尝试m次(通常小于5次，依据硬件算力进行设置)位置预测，若都不满足方向角变化小于某阈值，则当前时刻的动态目标不是车辆；

同理，在后一时刻进行运算时，同样选取关联点云簇，与上式不同的是，v_rand替换为根据上一时刻点云簇和当前时刻动态目标位姿差计算得到的速度值，同样，若存在关联的点云簇，对后一时刻关联点云簇进行位姿估计。

S303.通过运动一致性原理进行是否属于车辆的判断，从而实现动态车辆的检测。

根据步骤S302获取连续三个时刻中具有关联性的点云簇的三个位姿值，并通过计算得到两个时间间隔该点云簇的速度值，随后通过运动一致性原理对该点云簇是否属于车辆进行判断：

运动一致性是指车辆在运动过程中的速度和方向角应具有一定稳定性，变化在一定阈值范围内；若违反，则该动态目标不是动态车辆，因此，若连续三帧数据中某动态目标及其前后两时刻的关联点云簇状态满足这一原则，则判断为动态车辆。

综上，本发明提出一种对点云密度具有鲁棒性的车辆特征，使用模板匹配的方法能够扩展该算法的适用场景，且不需大量数据进行模型训练，在现有算法基础上提高了动态车辆检测性能，并将检测范围扩展到80m内。使用KITTI公开数据集对车辆检测性能进行验证，可得结果如下表所示。可以看出使用法向量直方图特征位姿估计的方法获得了很好的检测性能提升，提高了正确检测数量，并有效降低了检测虚警数量，综合正确检测和虚警指标，获得了更好的F1得分，验证了算法的优越性；

本发明依据车辆在激光雷达视角下的可见表面法向量分布特性构建车辆测量模型，能够更准确地表征车辆反射点云分布特性；基于法向量直方图特征，对点云密度具有较好鲁棒性，能够对交通场景下的车辆目标进行有效表征；基于法向量直方图特征的位姿估计方法，利用K-L散度进行模板匹配，从系列包围框中选择最佳包围框，进而确定目标所处位姿；基于法向量直方图特征位姿估计的动态车辆检测过程，将该位姿估计方法引入动态车辆检测过程，提高了算法对动态车辆的检测性能。

Claims (7)

1.基于法向量直方图特征的位姿估计及动态车辆检测方法，其特征在于：包括以下步骤：

S1.通过激光雷达扫描获取检测区域的激光点云数据，并对激光点云数据进行预处理，得到地上点云簇；

S2.根据预处理后得到的地上点云簇进行动态目标检测；

S3.对检测得到的动态目标进行位姿估计，并判断检测得到的动态目标是否为动态车辆：

所述步骤S3包括：

S301.对于检测得到的动态目标进行位姿估计，包括点云法向量求解、法向量直方图特征计算和利用法向量直方图特征进行位姿估计；

所述点云法向量求解的过程包括：

针对某动态目标o，假设其包含的点云为P，P中任一点p_i的法向量计算过程为：以p_i为球心，r为球半径做虚拟球，该球包围的所有点表示为P_r＝{p_j|p_j∈P,||p_j-p_i||＜r}，矩阵P_r中的每一行表示一个点，使用

分别表示P_r中的各列，表示球中所有点的x,y,z坐标；

使用P_r的协方差矩阵

获取p_i的法向量，其中：

其中τ表示球P_r中点的个数，m,n∈{1,2,3}；计算

的特征值和特征向量，最小特征值对应的特征向量

为点p_i的法向量；

根据球坐标表示，单位法向量n_i通过n_i与+z轴间的角

n_i在x-y平面投影与+x轴间的夹角

来确定，n_i坐标与该角关系如下：

其中

为了表示上的统一，使用n_i与+x轴间的夹角

以及与+y轴间的夹角

来替代

其中

n_i与各坐标轴正向的夹角由下式得出：

其中d_c表示各坐标轴单位向量，n_i为单位法向量，<·>表示内积运算，||·||表示向量求模运算；

S302.在当前时刻获取某动态目标的位姿后，沿该目标所处方向角进行位置关联；

S303.通过运动一致性原理进行是否属于车辆的判断，从而实现动态车辆的检测。

2.根据权利要求1所述的基于法向量直方图特征的位姿估计及动态车辆检测方法，其特征在于：所述步骤S1包括：

S101.地面点云分割：

通过激光雷达扫描获取检测区域的激光点云数据后，通过RANSAC、EKF方法对地面进行平面拟合，并获取点云与拟合平面间的垂距离，对于任一点云，若与拟合平面的垂距离小于预先设定的阈值T_g，则为地面点云，反之则为地上点云；或是根据GPR构建地面高度模型，将各点云处的预测高度与真实高度进行比较，若预测高度大于等于真实高度，则该点为地面点云，反之，则该点为地上点云；

对地面点云进行分割滤除，保留地上点云；

S102.地上点云聚类:

使用RBNN聚类方法将地上点云划分为不同的地上点云簇。

3.根据权利要求1所述的基于法向量直方图特征的位姿估计及动态车辆检测方法，其特征在于：所述步骤S2包括：

S201.将地上点云簇进行水平面极坐标网格投影，没有点云投影的网格标记为“自由”状态，点云簇投影中距离激光雷达最近的一层网格定义为“占用”状态，其他包含该点云簇的网格定义为“遮挡”状态；

S202.将连续两帧点云首先利用GPS/INS数据进行坐标系统一，随后比较极坐标网格中的状态变化，并设定状态变化的网格的数量阈值，若网格变化数量大于阈值，则该点云簇被标记为动态目标，若网格变化数量不足阈值，则认为是测量噪声或环境干扰引起，将点云簇标记为非动态目标。

4.根据权利要求1所述的基于法向量直方图特征的位姿估计及动态车辆检测方法，其特征在于：所述法向量直方图特征计算的过程包括：

构建车辆测量模型：将车辆的水平面投影建模为矩形框，其中设R0为激光雷达视角下的可见长边区域，R1为可见短边区域，R2为车辆内部，R3为可见角点；目标局部坐标系原点O位于目标中心位置，y轴与不可见短边法向量方向同向，x轴为y轴顺时针旋转90度；L为模型长度，W为模型宽度，Lv为可见边区域的宽度；

将区间[0,π]等分为b_num个小区间，统计落入每个小区间中的角度的数量，得到直方图表示：

其中b＝1,2,...,b_num，card(·)表示集合的基数，c∈{x,y,z}；

根据车辆测量模型，分别统计落入R₀-R₃区域中的点，计算各点法向量与各坐标轴夹角的分布情况，每个区域中的直方图特征定义为三个轴的夹角的分布直方图拼接：

其中特征

的长度为3×b_num。

5.根据权利要求1所述的基于法向量直方图特征的位姿估计及动态车辆检测方法，其特征在于：所述利用法向量直方图特征进行位姿估计的过程包括：

对于某地上动态目标o，首先获得该点云簇中各点的法向量Nc，随后将其三维点云投影到水平面中，利用Graham算法获取该点云簇投影的凸包络P_K，随后利用凸包络确定包围该点云簇的K种不同位姿下的包围框，具体过程如下：

首先从P_K中选取相邻两点，计算两点确定的直线line1，随后从P_K中挑选距离line1最远的点，过该点做直线平行于line1，记为line2，接下来，从P_K中挑选line1投影中横坐标最小和最大的点，分别过这两点，做垂直于line1的直线，记为line3和line4；至此，四条直线可确定一种方位下的包围框，P_K共可确定K个不同位姿的包围框；

确定包围框后，以该包围框中心坐标作为初始位置，所处方向角作为初始方向角，确定初始位姿(c_x0,c_y0,θ₀)；随后提取该位姿下动态目标的法向量直方图特征，并与事先手动选择的车辆真值法向量直方图特征进行模板匹配，使用K-L散度计算匹配程度，K-L散度公式如下：

其中ω_k为权重系数，b_num为直方图间隔数；κ越小，表示特征与模板越匹配；令

若当前动态目标存在运动朝向信息t_p,则与模板匹配得分计算如下：

其中系数η₁+η₂＝1，

表示该动态目标局部坐标系y轴方向向量；总得分S越小，说明该位姿估计越准确；

随后从凸包络P_K中选择随后的两相邻点完成上述过程，共计算获得κ个K-L散度值；从中选择最小的K-L散度，对应位姿下的包围框确定为最佳包围框，所处位姿确定为该动态目标位姿。

6.根据权利要求1所述的基于法向量直方图特征的位姿估计及动态车辆检测方法，其特征在于：所述步骤S302包括：

在当前时刻获取某动态目标的位姿后，沿该动态目标所处方向角进行位置关联，寻找前一时刻该动态目标可能的位置，预测位置如下：

其中θ为上述过程得出的最佳姿态角，Δk为时间间隔，v_rand为[-v,v]中随机选择的速度值，v取25m/s；

表示k时刻动态目标所处位置；

在预测位置处进行点云簇搜索，如果该位置附近存在点云簇，则认为在上一时刻存在关联点云簇，利用前述位姿估计方法估计上一时刻关联点云簇的位姿，如果点云簇与当前时刻动态目标的方向角差值小于设定阈值，则点云簇与动态目标为同一物体，进行后续处理；若方向角偏差过大，则认为点云簇与动态目标不是同一物体，尝试m次位置预测，若都不满足方向角变化小于某阈值，则当前时刻的动态目标不是车辆；

同理，在后一时刻进行运算时，同样选取关联点云簇，与上式不同的是，v_rand替换为根据上一时刻点云簇和当前时刻动态目标位姿差计算得到的速度值，同样，若存在关联的点云簇，对后一时刻关联点云簇进行位姿估计。

7.根据权利要求1所述的基于法向量直方图特征的位姿估计及动态车辆检测方法，其特征在于：所述步骤S303包括：

根据步骤S302获取连续三个时刻中具有关联性的点云簇和动态目标的三个位姿值，并通过计算得到两个时间间隔该点云簇的速度值，随后通过运动一致性原理对该点云簇是否属于车辆进行判断：

运动一致性是指车辆在运动过程中的速度和方向角应具有一定稳定性，变化在一定阈值范围内；若违反，则该动态目标不是动态车辆，因此，若连续三帧数据中某动态目标及其前后两时刻的关联点云簇状态满足这一原则，则判断为动态车辆。

Images