CN108549084B - 一种基于稀疏二维激光雷达的目标检测与姿态估计方法 - Google Patents

Abstract

一种基于稀疏二维激光雷达的目标检测与姿态估计方法，包括利用点云中点之间的距离统计对激光雷达数据的离群点去除、基于密度扩充的点云数据目标聚类、点云凹形判断和利用姿态判据的目标姿态搜索方法。上述的各个部分依次连接，最终输出当前帧的目标的姿态信息。本发明设计最优包围盒贴合判据，并以此进行关于朝向角θ的最优搜索，从而得到目标的朝向角信息(即二维姿态)，实施方便，操作简单，满足自主车的实时性要求，具有较强的实时性。

Description

一种基于稀疏二维激光雷达的目标检测与姿态估计方法

技术领域

本发明涉及智能汽车领域，具体涉及一种基于稀疏二维激光雷达的目标检测与姿态估计方法。

背景技术

激光雷达(Light Detection and Ranging,LiDAR)，即激光探测及测距系统，是以发射激光束探测目标的位置、速度等特征量的雷达系统，一般由激光发射器、激光接收器、控制系统、信息处理系统和显示装置组成，可以大范围、高精度地获取三维或二维空间的深度信息，即点云数据。激光雷达的工作原理是由激光发射器发射一个激光脉冲信号，当此激光束接触到目标表面上会产生回波信号并由接收器捕获到，然后经由信息处理系统进行处理，计算出目标深度L，并将得到信息加以传输保存，目标深度L按照下式计算：L＝Δt×c。其中c为光速，Δt为时间差。多线激光雷达具有多条扫描线，可同时进行测量。

自主驾驶智能汽车是一种集环境感知、规划决策、运动控制功能于一体的智能驾驶平台。无人驾驶汽车在行驶的过程中，对周围环境的感知需求是多方面的，其中对目标检测是必要的，如行人、车辆等。因此在无人驾驶技术研究中，对无人驾驶汽车周围环境中的目标检测是一个极为重要的任务。现有技术中，可以利用单目相机、双目相机、激光雷达、毫米波雷达等传感器对无人驾驶车辆周围得目标进行检测，而激光雷达由于其高精度测距、抗干扰能力强、检测范围大等其他传感器不具备的优点，在无人驾驶汽车周围环境的目标检测任务中发挥着主要作用。由于高精度三维激光雷达(比如64线激光雷达)价格昂贵，目前还不能广泛的用于商用无人驾驶汽车，因此，使用性价比高、检测精度也相对较高的稀疏二维激光雷达具有很广阔的前景。

在使用激光雷达检测目标的过程中，对激光雷达扫描获取的点云数据进行离群点剔除处理和目标点云聚类之后，需要对目标点云数据进行姿态估计，这也是最为重要的一个步骤。目标一般使用矩形包围盒模型来描述，这种表示方法可以准确表示目标的大小、位置、朝向等信息，也对后续无人驾驶汽车的规划决策有着很大的帮助，因此需要针对点云分布求取最佳贴合点云轮廓的矩形包围盒。

稀疏二维激光雷达得到的是二维点云数据，通常是检测范围内目标能够被激光扫描到的部分轮廓点云数据。其数据通常呈现出L形、I形特征。目标的检测、姿态估计和跟踪是无人驾驶环境感知的一个关键组成部分，会对无人驾驶车辆后续的规划决策和运动控制有极大的影响，所以对于这些点云数据需要很好的矩形包围盒贴合来才能得到准确的目标描述。

发明内容

本发明的目的在于提供一种能够针对激光雷达数据中目标形态的特点，求取最贴合的包围盒，进而得到其准确的姿态信息的基于稀疏二维激光雷达的目标检测与姿态估计方法。

为达到上述目的，本发明采用的技术方案包括以下步骤：

步骤一、利用稀疏二维激光雷达扫描前方180°范围环境，获得雷达坐标系下的二维点云数据，并对二维点云数据进行去除离群点处理；

步骤二、利用聚类方法对去除离群点后的二维点云数据进行分割得到感兴趣的目标点云数据，并对于每一个目标点云数据即聚类点云在车辆坐标系下给出其初始目标包围盒；

步骤三、使用最优姿态搜索算法，根据姿态判据选择姿态最为准确，即判据值最大的包围盒并投射回原坐标系，然后计算得到目标的姿态。

所述步骤一获取激光雷达的二维点云数据，即点的x-y坐标后，对所有点云数据需要转换坐标系并进行预处理操作，首先利用x_car＝-y_LiDAR与y_car＝x_LiDAR+Δl将点云坐标转移到车辆坐标系，其中x_car，y_car是点云数据在车辆坐标系中的坐标值，x_LiDAR和y_LiDAR点云数据在雷达坐标系中的坐标值，Δl是两坐标系原点之间的水平距离，然后统计其中每一个点p_i的k邻域点的均值距离dis_i，对均值距离集合Dis＝{dis₁，dis₂，·，dis_N}计算其均值μ和标准差σ，通过预先设定的权重ω，若dis_i＜μ+ω·σ，则认为p_i为离群点并剔除，ω和k是通过在采集好的离线数据集中人工标定离群点，然后在所有离线数据上遍历ω、k(0＜ω≤3，1≤k≤n/2)得到剔除离群点效果最优的ω、k，选定为最优参数。

所述步骤二的初始目标包围盒是在车辆坐标系下分别以其x，y的最大最小值组成初始包围盒的顶点坐标(x_min，y_min)、(x_max，y_max)、(x_max，y_min)、(x_min，y_max)，然后旋转坐标系，根据姿态判据Sc＝ω₁·R_θ+ω₂·S_θ得到准确的包围盒，估计出目标的中心位置(x，y)和朝向角θ。

本发明设计最优包围盒贴合判据，并以此进行关于朝向角θ的最优搜索，从而得到目标的朝向角信息(即二维姿态)，实施方便，操作简单，满足自主车的实时性要求，具有较强的实时性。

附图说明

图1为稀疏二维激光扫描原理图；

图2为本发明的流程结构示意框图；

图3为雷达坐标系与车辆坐标系位置关系图；

图4为凹形判断示例图；

图5为旋转角度为θ时的点云包围盒示意图。

具体实施方式

下面将结合附图，对本发明实施例中的技术方案进行更为清楚、完整的描述。然而应当理解，可以以各种形式实现本公开而不应该被这里阐述的实施例所限制，相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整地传达给本领域的技术人员。

结合图1，稀疏二维激光扫描到目标轮廓并反射获得相应的点云数据。因此在点云数据中，车辆大多呈现“L”形(少数有“I”或“U”形)，而行人等其他小物体则通常为不规则的形状。

结合图2，本实施例记录了一种基于稀疏二维激光雷达的目标检测与姿态估计方法，包括读取数据、点云预处理、点云聚类、凹形判断，最优姿态搜索。

步骤200：根据具体激光雷达(这里以八线激光雷达为例)的标准应用程序接口实时读取每一帧的点云数据，这些数据在激光坐标系下以x-y二维坐标形式保存。由于雷达坐标系与车辆坐标系不同，为了给道路规划与车辆控制提供直接可用的环境感知依据，需要将雷达坐标系下的点云数据转化到车辆坐标系中。

结合图3，两坐标系位置关系可以利用x_car＝-y_LiDAR，y_car＝x_LiDAR+Δl完成转换。

然后是点云数据的预处理，包括离群点的去除等。激光雷达在实际应用中，由于种种不确定因素，获得的雷达数据通常会包含部分噪声，会对后续处理判断造成一定的影响，所以要对雷达数据进行一定的预处理，在保证不破坏点云数据特征的基础上剔除噪声点。

激光雷达的点云数据一般是成堆密集出现的，故噪声通常表现为稀疏离群点。它们产生的原因很多，比如视野内有茂密的灌木丛或散乱的石堆；有时候因为传感器自身问题或物体表面材质也会导致出现异常反射的数据点。本发明在实施的时候使用基于统计学原理的方法来进行离群点的去除。具体过程为：

(1)设激光雷达实时采集到的某一帧点云数据集合为P，其中的点为p_i(i＝1，2，……，n)。对于P中每一个点计算其k个近邻点距离的均值，记为一个样本个体：

(2)由(1)遍历完P中所有点，即可得到集合Dis＝{dis₁，dis₂…，dis_n}。

(3)计算上述样本集合Dis的均值μ和标准差σ：

(4)设滤波之后的点晕集合为P^*，那么它可以通过下式对样本个体集合Dis的遍历得到：

P^*＝{p_i∈P|dis_i≤μ+ω·σ，1≤i≤n} (4)

其中ω为权重因子。ω、k的取值取决于邻近点的个数，ω和k是通过在采集好的离线数据集中人工标定离群点，然后在所有离线数据上遍历ω、k(0＜ω≤3，1≤k≤n/2)得到剔除离群点效果最优的ω、k，选定为最优参数。

步骤201：本发明示例性地给出一个聚类方法以分割出感兴趣的目标点云数据。点云聚类的主要过程是：先从数据点集合中任意选取一个点判断其是否为核心点(如果该数据点ε邻域内的样本点数大于给定阈值m，ε和m是通过在采集好的离线数据集中，人工标定每一帧数据中的所有聚类，因为通常每个聚类内的数据点都紧密相邻，且与其他聚类内的点有着较大的距离，所以在所有离线数据上遍历ε、m(0＜∈＜1，m≥2)得到聚类效果最好的ε、m，选定为最终使用的参数)。若为核心点，则从这个核心点出发与该点密度可达的所有核心点，这些核心点与它们的ε邻域内的数据点所构成的并集点集就是一个聚类，然后重复上述操作，直到找出整个集合中的所有聚类。

步骤202：在对每一个聚类后的点云进行最优姿态估计之前，由于矩形包围盒自身刚性和凸性的局限，在用它表述一些凹形点云时，紧密型较差，冗余空间过多，有时候会覆盖部分道路，从而有可能引起无人驾驶车辆急停或对行驶决策产生干扰，因此需要对每一个聚类进行凹形判断。考虑目标点云为直线的情况，即凹形和凸形的中间情况，连接该点云的首尾两点可以得到一条直线，那么点云中大部分点会落在这条直线上。可以想到，当目标点云为凹形的时候，其大部分点会落在直线的某一侧，且与原点处在直线的不同侧；若为凸形，则与原点处在直线的相同侧。

结合图4，依次计算点云中的每一个点到首尾相连的直线l的有向距离集合Dis。设原点到该直线的有向距离为d₀，Dis中与d₀符号相反的点，即与原点处在直线的不同侧。提取出这些点，也就是点云中的凹形部分，记其有向距离为集合dis。然后以点云首尾两点中y值较小的点为坐标原点构建坐标系x’Oy’，其中x轴与直线l重合。那么在此坐标系下，记dis对应点的横坐标集合X到dis的映射为g(x)，若dis占Dis的50％以上，且g(x)满足：

g(tx₁+(1-t)x₂)＜tg(x₁)+(1-t)g(x₂) (5)

其中x₁，x₂∈X且x₁≠x₂，t∈(0，1)。则判断此点云为凹形。

对于凹形点云，本方法对其进行k-means重聚类，即将凹形点云分为多个小聚类，从而消除用单一矩形包围盒描述而产生的缺陷。

步骤203：对于上述步骤后的每一个聚类点云，在车辆坐标系下首先获取计算出初始的目标包围盒，即分别以其x，y的最大最小值组成初始包围盒的顶点坐标(x_min，y_min)、(x_max，y_max)、(x_max，y_min)、(x_min，y_max)，这样的包围盒四边分别与x，y平行，因为一般情况下目标很少直接与车辆坐标系直对，所以由于姿态不准确，初始包围盒会占用大量的可行驶区间，不能直接使用。

步骤204：为了得到贴合更好的包围盒并进一步计算出准确的姿态信息，本发明实施最优姿态的搜索方法，即对点云P逐次旋转一定角度θ(θ从0到π/2逐渐递增)，公式为

这相当于将坐标系进行了旋转。然后在新的点集P’上依照步骤202的做法取新的包围盒的顶点坐标为(x_min，y_min)、(x_max，y_max)、(x_max，y_min)、(x_min，y_max)。对每一次旋转得到的包围盒，使用姿态判据Sc＝ω₁·R_θ+ω₂.S_θ进行评估。上式中S_θ即包围盒面积，R_θ为贴合度。然后在旋转结束后就可以得到判据值最优的姿态。

结合图5，下面说明R_θ的计算过程。当包围盒姿态正确时，点云数据中的点大多数会充分贴靠包围盒的边界。于是本发明需要构造这样一个度量函数——当点越靠近包围盒的边界，对此点的评价分数就应当越高，那么姿态正确时对所有点的评价分数总和就应该是最大的，即

其中f(x_i，y_i)即为我们所构造的评价点(xi，yi)对包围盒边界贴合度的函数，其形式如下：

其中η为增益常数，D*为有效距离阈值，超过此阈值则认为点距离边界太远，不能参与的R_θ计算。d_i为点(x_i，y_i)到边界线Ax+By+C＝0的欧式距离。k值可通过实验或经验取适当值。

记录下每次旋转状态下的Sθ和R_θ，我们的目的是为了找到最大的判据评估，那么若下一次旋转的评估Sc值更大，就保留其Sθ和R_θ，即ΔSc＝ω₁·δR-ω₂·δS-∈＞0。在旋转结束后，将最大评估值对应的包围盒坐标逆旋转回车辆坐标系，公式为：

Cor是2*4的矩阵，存储了包围盒四个顶点的坐标,即P_i＝{p_i1，p_i2，p_i3，p_i4}。然后可以计算得到目标的姿态，中心位置(x,y)和朝向角α。朝向角定义为目标的长边与车辆坐标系y轴正向的夹角，取值范围为(-π/2,π/2]。

Claims (2)

1.一种基于稀疏二维激光雷达的目标检测与姿态估计方法，其特征在于包括以下步骤：

步骤一、利用稀疏二维激光雷达扫描前方180°范围环境，获得雷达坐标系下的二维点云数据，并对二维点云数据进行去除离群点处理；

其中，获取激光雷达的二维点云数据，即点的x-y坐标后，对所有点云数据需要转换坐标系并进行预处理操作，首先利用x_car＝-y_LiDAR与y_car＝x_LiDAR+Δl将点云坐标转移到车辆坐标系，其中x_car，y_car是点云数据在车辆坐标系中的坐标值，x_LiDAR，y_LiDAR是点云数据在雷达坐标系中的坐标值，Δl是两坐标系原点之间的水平距离，然后统计其中每一个点p_i的k邻域点的均值距离dis_i，对均值距离集合Dis＝{dis₁，dis₂，·，dis_N}计算其均值μ和标准差σ，通过预先设定的权重ω，若dis_i＜μ+ω·σ，则认为p_i为离群点并剔除，ω和k是通过在采集好的离线数据集中人工标定离群点，然后在所有离线数据上遍历ω、k得到剔除离群点效果最优的ω、k，选定为最优参数，0＜ω≤3，1≤k≤n/2；

步骤二、利用聚类方法对去除离群点后的二维点云数据进行分割得到感兴趣的目标点云数据，并对于每一个目标点云数据即聚类点云在车辆坐标系下给出其初始目标包围盒；具体步骤如下：

步骤201，点云聚类的主要过程是：先从数据点集合中任意选取一个点判断其是否为核心点，如果数据点ε邻域内的样本点数大于给定阈值m，ε和m是通过在采集好的离线数据中，人工标定每一帧数据中的所有聚类，因为通常每个聚类内的数据点都紧密相邻，且与其他聚类内的点有着较大的距离，所以在所有离线数据上遍历ε、m得到聚类效果最好的ε、m，0＜∈＜1，m≥2，选定为最终使用的参数；若为核心点，则从这个核心点出发与该点密度可达的所有核心点，这些核心点与它们的ε邻域内的数据点所构成的并集点集就是一个聚类，然后重复上述操作，直到找出整个集合中的所有聚类；

步骤202：在对每一个聚类后的点云进行最优姿态估计之前，由于矩形包围盒自身刚性和凸性的局限，在用它表述一些凹形点云时，紧密型较差，冗余空间过多，有时候会覆盖部分道路，从而有可能引起无人驾驶车辆急停或对行驶决策产生干扰，因此需要对每一个聚类进行凹形判断；

依次计算点云中的每一个点到首尾相连的直线l的有向距离集合Dis；设原点到该直线的有向距离为d₀，Dis中与d₀符号相反的点，即与原点处在直线的不同侧；提取出这些点，也就是点云中的凹形部分，记其有向距离为集合dis；然后以点云首尾两点中y值较小的点为坐标原点构建坐标系x’Oy’，其中x轴与直线l重合；那么在此坐标系下，记dis对应点的横坐标集合X到dis的映射为g(x)，若dis占Dis的50％以上，且g(x)满足：

g(tx₁+(1-t)x₂)＜tg(x₁)+(1-t)g(x₂)

其中x₁，x₂∈X且x₁≠x₂，t∈(0，1)；则判断此点云为凹形；

对于凹形点云，本方法对其进行k-means重聚类，即将凹形点云分为多个小聚类，从而消除用单一矩形包围盒描述而产生的缺陷；

步骤三、使用最优姿态搜索算法，根据姿态判据选择姿态最为准确，即判据值最大的包围盒并投射回原坐标系，然后计算得到目标的姿态；

为了得到贴合更好的包围盒并进一步计算出准确的姿态信息，实施最优姿态的搜索方法，即对点云P逐次旋转一定角度θ，θ从0到π/2逐渐递增，公式为

这相当于将坐标系进行了旋转；然后在新的点集P’上依照步骤202的做法取新的包围盒的顶点坐标为(x_min，y_min)、(x_max，y_max)、(x_max，y_min)、(x_min，y_max)；对每一次旋转得到的包围盒，使用姿态判据Sc＝ω₁·R_θ+ω₂·S_θ进行评估；上式中S_θ即包围盒面积，R_θ为贴合度；然后在旋转结束后就可以得到判据值最优的姿态；

R_θ的计算过程：当包围盒姿态正确时，点云数据中的点大多数会充分贴靠包围盒的边界；构造一个度量函数——当点越靠近包围盒的边界，对此点的评价分数就应当越高，那么姿态正确时对所有点的评价分数总和就应该是最大的，即

其中f(x_i，y_i)即为所构造的评价点(xi，yi)对包围盒边界贴合度的函数，其形式如下：

其中η为增益常数，D*为有效距离阈值，超过此阈值则认为点距离边界太远，不能参与的R_θ计算；d_i为点(x_i，y_i)到边界线Ax+By+C＝0的欧式距离，k值通过实验或经验取适当值。

2.根据权利要求1所述的基于稀疏二维激光雷达的目标检测与姿态估计方法，其特征在于：

步骤二的初始目标包围盒是在车辆坐标系下分别以其x，y的最大最小值组成初始包围盒的顶点坐标(x_min，y_min)、(x_max，y_max)、(x_max，y_min)、(x_min，y_max)，然后旋转坐标系，根据姿态判据Sc＝ω₁·R_θ+ω₂·S_θ得到准确的包围盒，估计出目标的中心位置(x，y)和朝向角θ。

Images