基于点云场景流特征提取的运动目标位置估计方法

张瑞宾，郭应时，陈元华，龙云泽

（1.长安大学 汽车学院，西安 710064；2.桂林航天工业学院 汽车与交通工程学院，桂林 541004）

0 引言

无人驾驶车辆通过激光雷达等环境感知系统对周围动态目标位置准确估计，可有效提高危险交通场景下的安全性。三维信息采集和点云数据处理技术是目标检测的基础，广泛应用于环境探测、目标识别跟踪、导航定位、三维重建与状态估计等领域。向易等人[1]提出了一种基于无损卡尔曼滤波的算法，根据获得的标记雷达数据预测并更新目标的位置状态，从而估计出无人车双雷达系统的目标位置。李帅印[2]等人针对复杂环境中待跟踪目标数量不确定性问题，提出了一种基于多层激光雷达的目标检测与跟踪算法，对采用地平面拟合算法滤波后的点云进行栅格化，采用改进的匈牙利算法建立相应的跟踪器。冯玉朋等人[3]针对车辆嵌入式系统中视觉里程计算方法实时性差、计算资源有限的问题，提出了一种基于Harris和SIFT（Scale Invariant Feature Transform）的图像匹配方法，通过在特征检测模块中减少算法计算量，提高特征点提取的速度。范强等人[4]针对传统点云配准3DT-NDT（3D-Normal Distribution Transform）和ICP（Iterative Closest Point）算法配准效果差、配准时间长、误差大等缺点，提出了一种准确、相对高效的点云匹配算法，使室内场景点云数据配准精度高，受噪声影响小。白亚光等人[5]针对传统三维重建方法费时、费力、精度低的缺点，提出了一种将ISS算法和CPD（Coherent Point Drift）算法相结合的建筑物LIDAR点云配准方法，提高点云配准的精度。侯小红等人[6]针对LBP（Local Binary Patterns）算法在提取全局特征时不具有针对性的缺点，提出一种基于Harris-SIFT特征点检测的LBP人脸表情识别算法，通过引入Harris得到表情图像的角点，同时运用SIFT检测算法得到图像的局部最大最小值。禄盛等人[7]针对车辆主动安全控制系统难以实时准确获取控制变量的问题，分别提出了基于非线性全阶观测器和无迹卡尔曼滤波的车辆状态估计方法，并利用稳定性理论对所设计的车辆状态估计方法的稳定性进行了分析。韩宇星等人[8]为了提高遮挡情况下目标跟踪的可靠性，提出了一种基于改进粒子滤波的目标跟踪策略。根据目标的运动信息建立状态方程，采用径向基函数神经网络建立观测模型。根据模板与定位区域之间的Hellinger距离判断目标是否被遮挡。

综合上述三维信息处理及检测跟踪技术的研究进展，本文以激光雷达的获取的点云场景流为研究对象，利用关键点检测器和特征描述子对复杂场景中的运动目标特征信息进行提取，通过滤波算法对目标的位置进行预测和更新，实现对运动目标位置估计。

1 ISS-SHOT-UKF方法模型

本文设计了如图1所示的ISS-SHOT-UKF方法模型，是通过ISS关键点检测器、SHOT特征描述子及UKF滤波算法，估计来自点云场景流连续帧的运动目标状态。系统的输入是两组从动态3D场景流中采样的点，连续两个时间帧：P=｛xi｜i=1，…，n1｝（point cloud1）和Q=｛yi｜i=1，…，n1｝（point cloud2），xi，yi∈R3是XYZ三维坐标系内的点。利用关键点检测器筛选场景流内的待测目标点云的关键点，通过特征描述子提取关键点特征，通过滤波对待测目标位置进行预测和更新，最终确定运动目标位置。由于目标运动和传感器观测视点的变化，两个点云不一定具有相同的点数或其点之间有任何对应关系。为简化起见，仅关注点云的XYZ坐标信息。

图1 ISS-SHOT-UKF方法模型

2 场景流特征提取方法的确定

2.1 ISS关键点检测器

关键点是检测器通过一定标准获取的稳定而独特的点集，数量远少于激光雷达采集的原始点云，与局部特征描述子相结合可以加快数据处理效率。关键点检测器仅关注原始点云局部表面特征的重要结构以减少特征提取时间。关键点检测应考虑提取的关键点判别信息的丰富性，使关键点满足重复性和特殊性的要求，因此，有必要依据特殊性原则去检测关键点[9～11]。

ISS关键点检测器是通过协方差矩阵运算获取查询点与其邻域点间的特征值来表示查询点特征程度的方法。利用ISS检测器提取点云数据的关键点步骤如下[12]：

1）在所获取的点云集合上定义每个查询点pi的局部坐标系，并给出各点的搜索半径r。

2）针对每个查询点pi在其搜索半径r范围内计算相对于其他点pi的欧式距离，并计算其权重Wij。

3）计算查询点邻域范围内pi与pj间的协方差矩阵cov(pi)。

2.2 SHOT特征描述子

特征描述子是对利用关键点检测器所获取三维点云集合内关键点邻域的几何特征进行的描述，通常用向量表示[13]。

SHOT特征描述子对ISS关键点球形邻域局部坐标系内的空间进行划分，编码所获得的子空间的所有点的法向特征直方图，联合所有子空间直方图得到特征描述子。SHOT特征描述子具有尺度、平移、旋转不变性，对目标物较多复杂场景具有高辨识度[14，15]。利用SHOT特征描述子进行特征提取的步骤如下：

1）以ISS关键点为中心建立球形邻域，并将此球形分别按照经线、纬线及径向方向划分成32个子空间。

2）计算ISS关键点与子空间所有点的夹角余弦值，得到11维直方图，再将32个子空间的直方图组合形成352维的高维直方图特征，如图2所示ISS-SHOT特征提取原理。

图2 ISS-SHOT特征提取原理

2.3 ISS-SHOT场景流特征提取方法

为了分析各种关键点检测器及特征描述子的优缺点，采用遍历验证的方法，对比组合特性，测试中待识别场景选自Bologna[16]数据库，场景内的物体表面光滑，颜色差异明显，物体可能会受到光照、视角、遮挡的影响。

由表1关键点检测器与特征描述子的组合特性可知，采用ISS+SHOT的组合方式，在用时最短的情况下，提取最多个数的场景关键点，并且特征描述子计算时间最少，其处理场景流效果如图3所示，对场景流的相邻两帧点云进行关键点提取，图3（左）为某帧场景及关键点提取效果，图3（中）为临近帧场景及关键点提取效果，图3（右）为根据相邻两帧特征提取进行匹配的效果，表现出较好鲁棒性和良好效率。因此，综合考虑局部可重复性（数量）、尺度可重复性、效率，本文选择ISS+SHOT检测算法进行场景流信息提取。

表1 关键点检测器与特征描述子的组合特性

图3 ISS+SHOT场景流处理效果

3 运动目标位置估计方法的确定

由于待跟踪车辆运行状态是一个典型的非线性系统，可以用式(3)进行描述。

式(3)中，X是随机变量，Z是观测变量，f是非线性状态方程函数，h是非线性观测方程函数；W(k)是状态方程中噪声，V(k)是观测方程中的噪声。

在常用的目标跟踪算法中，KF滤波算法不适用于非线性系统，EKF滤波算法广泛应用于非线性状态估计系统中，然而，由于雅可比矩阵计算过程复杂，计算量大，在精度提高有限的情况下，计算效率大大降低。UKF滤波算法基于无迹变换来处理均值和协方差的非线性传递问题，且非线性越强，其估计效果越好。UKF滤波算法的步骤如下所示：

为了进一步对比UKF和EKF两种算法的滤波效果，选用一维非线性系统，如图4所示为EKF和UKF两种运动目标位置估计结果对比。为了更直观反应出位置估计的准确性，将每个时刻EKF和UKF的估计值与真实值做差，得到绝对值偏差，即得到图5EKF和UKF的估计值偏差，可知在相同条件下，UKF具有更高的滤波精度。因此，本文选择UKF作为目标位置估计算法。

图4 EKF和UKF两种位置估计结果对比

图5 EKF和UKF的估计值偏差对比

4 ISS-SHOT-UKF方法实验验证

为了测试ISS-SHOT-UKF方法性能，通过如图6所示搭载有多种传感器的智能车辆实验平台采集数据，平台通过集成激光雷达、GPS定位模块、惯导系统、工控机（包含CAN通讯卡）等，通过上下位机控制系统可以开展智能汽车相关实验任务，所采集的交通场景流如图7所示，椭圆框内代表车载激光雷达采集的场景流内相邻两帧的运动目标的位置变化情况。实验过程中，系统采集数据周期为0.5s，实验的数据采集地点为某城市的绕城高速，基于同一车载激光雷达系统的前提下，点云场景流路况选取图8所示典型弯道和直道路段样本。

图6 智能车辆实验平台

图7 交通场景流

图8 典型弯道和直道路段样本

图9所示为典型弯道和直道路段动态目标位置估计，黑色标记为目标的真实位置，蓝色标记为激光雷达探测到的目标位置，红色标记为UKF算法获得的目标估计位置。

图9 典型弯道和直道路段动态目标位置估计

为了验证算法的有效性，采用均方根误差（Root Mean Square Error，RMSE）衡量预测值同实际值之间的偏差。RMSE的值越小，代表位置估计的测量精度值越高。

如表2典型弯道和直道路段目标位置估计误差可知，该ISS-SHOT-UKF方法在直道路段上对待测运动目标的横向位置、纵向位置的RMSE均低于弯道路段，且表现均优于传统EKF方法。

表2 典型弯道和直道路段目标位置估计误差

5 结语

针对城市复杂交通场景下对运动车辆进行状态估计可靠性较低的问题，本文提出基于点云场景流特征提取技术的运动目标位置估计方法，对比不同关键点检测器与特征描述子的组合特性，确定出根据ISS算法检测点云中关键特征点，再计算关键点处的SHOT特征描述子的方式最为高效，并选用无迹卡尔曼滤波算法对所处理的场景流中的动态目标的进行状态估计，并在某城市绕城高速的典型弯道和直道路段进行算法验证，实验结果表现出较好的鲁棒性。由于本方法仅估计动态刚性目标实时位置状态，后续应对动态目标行为预测方法展开深入研究。