基于激光传感器的无人驾驶汽车动态障碍物检测跟踪

本文创建的障碍物占用栅格图大小为512&TImes;512，每个栅格的大小为20cm&TImes;20cm，无人驾驶汽车车头朝向与y轴正方向同向且位于这个栅格地图中的（256，100）位置。栅格图和Velodyne、Ibeo的检测范围关系如图3所示。

　　图3 栅格图和Velodyne、Ibeo的检测范围关系示意图

　　3 Velodyne数据处理

　　3.1 Velodyne数据栅格化

　　将激光数据栅格化的方法有很多，无人驾驶汽车领域比较成熟的3维激光传感器数据栅格表示方法有均值高度图和最大最小值高度图两种。本文采用最大最小值高度图法对Velodyne数据进行栅格化处理，最大最小值高度图是由Thrun教授提出的一种高度图的变种，该方法在2007年的DARPA（美国国防部先进研究项目局）城市挑战赛中得到了广泛的应用，并且取得了很好的使用效果。在最大最小值高度图中地面被建模为一系列的栅格，这些栅格仅包含两个值：所有投影到同一栅格中的激光传感器返回值的最大值和最小值。然后将最大值和最小值之差大于预先设定的阈值D的栅格标记为障碍物状态；将差小于D的栅格标记为非障碍物状态。对于一个栅格X，如果其为障碍物状态，则设置该栅格的占用值T（X）=1；如果为非障碍物状态，则设置为T（X）=0．图4是用最大最小值高度图法对一个十字路口的Velodyne数据栅格化后所得到的障碍物占用栅格图。

　　3.2 障碍物聚类

　　对障碍物进行跟踪之前需要对栅格地图中的占用栅格进行聚类，本文采用的是一种区域生长聚类算法，见FuncTIon1．Addopen（X）和Addclosed（X）分别表示将栅格X添加进open列表和closed列表；Deleteopen（X）和Deleteclosed（X）分别表示将栅格X从open列表和closed列表中删除；Newlable（X）表示给栅格X赋一个新的标示值；Copylable（X，Y）表示将栅格X的标示值赋予给栅格Y；Selectopen（X）和Selectclosed（X）分别表示从open列表和closed列表中随机选出一个栅格X；L（X）表示栅格X是否已经被赋予了标示值，值为1表示已经被赋予，值为0则表示没有。

　　图4 用最大最小值高度图法栅格化得到的栅格图

　　经过FuncTIon1，栅格地图中设定聚类区域内的障碍物占用栅格就被聚类成一个个障碍物块，图5是对道路内的障碍物占用栅格聚类得到的结果。

　　3.3 障碍物跟踪

　　首先创建一个动态障碍物列表来存储上面聚类得到的障碍物块信息，并且实时更新这些障碍物块的跟踪结果。存储于这个动态障碍物列表中的每一个障碍物块包含以下信息：编号，最新一次聚类得到时的时间、占据位置、速度大小方向和加速度大小方向，速度协方差，加速度协方差以及存在置信度和运动置信度。

　　图5 障碍物占用栅格聚类结果

　　对障碍物块进行跟踪时，需要匹配当前时刻聚类得到的障碍物块和动态障碍物列表中存储的障碍物块，本文采用一种最大关联值法对其进行匹配。对于动态障碍物列表中的每一个障碍物块和当前时刻聚类得到的每一个障碍物块，存在一个关联值。这个关联值的大小与和的位置、大小、形状有关。如图6所示，对于每一个障碍物块，用一个能覆盖它的最小矩形对其进行参数化：长边长L，短边长R，中心位置O（x，y）和障碍物块对这个矩形的占用率k。

　　显然，和的中心位置不能直接拿来比较，因为对它们进行聚类的时间不同，所以需要对的中心位置进行修正，见式（1）、（2）：

　　其中，和分别是7最新一次聚类得到时的时间和当前时刻，和是在动态障碍物列表中存储的最新速度和加速度是对的中心位置进行修正后得到的中心位置。于是，可以根据式（3）得到：

　　其中，a、b和c是权值，通过实验可以得到比较合适的经验值。

　　然后就可以得到一个如下的决策矩阵并设置一个门限关联值：

　　接着从这个决策矩阵中找到最大的关联值，如果不小于门限值，则认为障碍物块和成功匹配，然后将与和相关的所有关联值从这个决策矩阵中删除，得到一个新的决策矩阵如下：

　　再从这个新的决策矩阵中寻找最大的关联值，依此类推，直到找到的最大关联值小于门限值或者决策矩阵变空为止。

　　最终的匹配结果有如下3种情况，分别对其进行处理：

　　①存储于动态障碍物列表中但是没有当前聚类得到的障碍物块与之匹配的障碍物块，将其存在置信度减1，其它值不变。

　　②当前聚类得到的但是没有存储于动态障碍物列表中的障碍物块与之匹配的障碍物块，将其添加进动态障碍物列表中，并将速度大小、方向和加速度大小、方向都置初值为0，速度协方差和加速度协方差都置初值为10，存在置信度置初值为10，运动置信度置初值为0。

③存储于动态障碍物列表中并且有当前聚类得到的障碍