激光雷达点云的车辆航向提取

一、工程问题背景

在自动驾驶环境感知技术中，激光雷达是必不可少的传感器。在三维雷达点云图中，我们通过六面体包围框表征检测到的目标。除了识别障碍物与车辆外，车辆的航向检测是环境感知必不可少的部分，只有估算出车辆的航向才能预测车辆的行驶轨迹。如图1所示，标号0、1、2中，黄色框代表无向包围框（无航向信息），白色框代表有向包围盒（有车辆航向信息）。

 

图1 激光雷达点云图

 

图2 单个车辆点云

 

二、PCA算法确定航向原理

PCA降维的目的就是“降噪”和“去冗余”。“降噪”的目的就是使保留下来的维度间的相关性尽可能小，而“去冗余”的目的就是使保留下来的维度含有的“能量”即方差尽可能大。我们得需要知道各维度间的相关性以及各维度上的方差，而协方差矩阵是同时表现不同维度间的相关性以及各个维度上的方差的最好数据结构。

PCA算法可求得数据分布的主成分方向，所处理数据的协方差矩阵的最大特征值对应的特征向量即为主成分矢量方向。假设单个车辆的雷达点云分布符合高斯分布，则协方差矩阵的最大特征值对应的特征向量即为航向方向。

1、多维正态分布与协方差矩阵

（1）多维正态分布

（2）协方差与协方差矩阵

维基百科给出的协方差定义：

（3）原理

协方差矩阵度量的是维度与维度之间的关系，而非样本与样本之间。协方差矩阵的主对角线上的元素是各个维度上的方差(即能量)，其他元素是两两维度间的协方差(即相关性)。我们要的东西协方差矩阵都有了，让保留下的不同维度间的相关性尽可能小，也就是说让协方差矩阵中非对角线元素都基本为零。

而协方差矩阵为实对称方矩阵，是正规矩阵，可以直接对角化，对角化主线元素为特征值。根据正规矩阵的结构定理可知，存在以下矩阵分解：

其中U的列为协方差矩阵∑的单位正交特征向量，特征值按从大到小排列。

因此，保留前k个特征值及对应的特征向量，即为前k个主成分矢量方向。

三、总结

综上，通过计算单个车辆点云坐标数据(xk,yk)的协方差矩阵，然后取最大特征值，其对应的特征向量即为车辆航向方向.