具体实施方式

下面将结合附图对本发明作进一步的说明：

本发明公开了一种基于道路指纹空间关联矩阵的车辆定位方法，该方法包括道路指纹空间关联矩阵构建和定位两个阶段。在道路指纹空间关联矩阵构建阶段，首先对高精度地图数据进行道路指纹提取，道路指纹包含车道线、交通标志牌、减速带、井盖、路杆等信息，提取出道路指纹后，再计算每个指纹之间的空间距离构建空间关联矩阵，得到每个场景具有唯一性的空间特征。在定位阶段，通过激光雷达采集激光雷达点云，检测激光雷达点云中的道路指纹，并计算当前道路指纹的空间关联矩阵。之后再计算当前空间关联矩阵与地图数据库中空间关联矩阵之间的匹配度，选取匹配度最大的位置节点。最后利用当前点云道路指纹的坐标信息与该节点地图中道路指纹所包含的位置信息进行匹配，从而计算当前车辆的位置。本发明利用道路指纹关联矩阵的唯一性进行表征，保证了车辆定位的精确度。

图3是本发明实施例提供的一种方法流程示意图，包括地图数据库构建和车辆定位两个阶段；其中，地图数据库构建阶段主要包括激光雷达和高精度惯导数据，利用惯导数据和提取的道路指纹来得到位置和空间关联矩阵，定位阶段主要包括普通GPS的初定位、矩阵匹配的节点级定位和位置匹配的车辆定位。首先利用GPS做一个初定位，选择当前车辆所在的一个范围，获得多个节点的关联矩阵，例如当前位置前后40m范围的地图节点，这样可以减少全局搜索的范围，加快计算速率；之后对单帧点云进行处理，通过计算曲率和聚类等操作把道路指纹信息提取出来并获得当前的关联矩阵，利用这个关联矩阵与初定位选取的多个道路指纹关联矩阵库进行匹配，选择相似度最高的节点。最后利用当前位置的指纹位置信息与地图位置信息进行匹配实现车辆定位。

具体包括以下步骤：

S1：数据采集；

首先在本发明实施例路段上，如图1所示，利用数据采集车上装载的激光雷达1实时扫描得到当前环境包含道路指纹的三维激光点云，同时，数据采集车上还装载有高精度惯导定位模块，利用该惯导2得到当前的车辆位置信息；其中定位模块精度为厘米级。

S2：数据处理；

利用高精度惯导和激光雷达采集一段轨迹的GPS和激光雷达点云信息，然后将GPS数据转化成与激光坐标系尺度一样的UTM坐标系，如图2所示，坐标系统一后，需要对两轨迹进行拟合，首先构建损失函数，再利用最小二乘法来优化两轨迹之间的残差，得到两者之间的映射关系，即旋转平移矩阵。之后利用得到的两传感器的映射关系，将每帧点云利用对应的旋转平移矩阵将局部坐标系下的点云投影到全局坐标，得到包含道路指纹的高精度点云地图。

S3：道路指纹库构建；

S31：得到高精度地图后，对高精度点云地图进行道路指纹信息的提取，把道路指纹聚类成一个个类别，并计算出每个类别的聚类中心，用聚类中心代表指纹的空间位置。可以采用欧式聚类来提取指纹信息，聚类的目的在于将离散、无组织且信息量较少的单一激光点云聚合成一个个具有一定空间拓扑结构的且信息量丰富的三维语义目标，即道路指纹。本实施例采用的DBSCAN算法，它不需要指定簇的个数，具有擅长找到离群点，聚类结果没有偏倚等优点。

聚类完成后，使用聚类中心来表示每个类别的位置。为了减小点云扫描不均匀的影响，可以将点云投影至平面中，然后计算投影后点云的中心坐标，其计算公式可以表示成如下形式：

其中，x_n为该类别的某点数据，为计算的该类别中心坐标。

S32：计算每个指纹中心之间的欧式距离，利用这个欧式距离构建关联矩阵，同时保留每个指纹的位置信息，用于位置匹配。每个类别的欧式距离可以表示成如下形式：

得到距离后，就可以构建关联矩阵。空间关联矩阵是一个二维矩阵。本实施例采用路杆-路杆、路杆-交通标志牌、路杆-车道线、井盖和减速带(路杆-车道线、路杆-井盖、路杆-减速带视为一类)三种关联来构建矩阵。把这个矩阵保存在文本中，在定位时从文本读取用于匹配。关联矩阵可以表示成如下：

式中，H₁表示路杆-路杆形成的关联矩阵，H₂表示路杆-交通标志牌形成的关联矩阵，H₃表示路杆-车道线、井盖和减速带形成的关联矩阵。除了保存关联矩阵，我们还保留了每个类别的位置坐标。关联矩阵是为了寻找到相似节点，位置坐标是为了匹配实现车辆定位。

S4：初定位

利用待定位车辆上的普通GPS接收机来提供一个初始位置信息，利用这个信息来选择地图中距离当前位置最接近的多个节点并提取节点中所包含的点云和空间关联矩阵。在后续中，不再使用GPS数据，GPS只用于初始化阶段。

S5：道路指纹提取

道路指纹提取分为交通标志牌、车道线、减速带、井盖和路杆等标志。每个标志具有不同的提取方法。对当前激光雷达采集的点云进行特征提取时，可以只选择道路指纹信息，剔除不需要的冗余的其他信息，这样不仅减少了数据量，也可以加快匹配时间。

S51：交通标志牌提取

交通标志牌是道路中常见的道路标志，其布置一般具有高度要求，因此可以先设置一定的高度，将一些地面点滤除，也把包含车牌等高反射强度的物体滤除。同时考虑到交通标志的高反射率，激光雷达扫描到交通标志牌后其反射回来的激光束强度很大，因此利用这种特性将正对的交通标志牌提取出来。而对背对的情况，利用PCA算计进行提取。

PCA算法首先遍历三维激光点云数据以获取整体点云集，其次利用该点云集构造协方差矩阵Pcov，最后对协方差矩阵Pcov进行奇异值分解得到特征值λ₁，λ₂，λ₃，并根据分解结果分析点云维度特征。

当λ1＞＞λ2＝λ3时，D＝1，说明点集主要在1个维度上分散开来，则该点集具有线性维度特征；当λ1＝λ2＞＞λ3时，D＝2，说明点集主要在2个维度上分散开来，则该点集具有平面维度特征；当λ1＝λ2＝λ3时，D＝3，说明点集主要在3个维度上分散开来，则该点集为离散结构。对于交通标志牌，其点云集具有平面维度特征，因此提取D＝2的点集。

S52：车道线、井盖与减速带提取

车道线是附着在地面点中的，因此提取车道线前需要提取地面点云。在知道激光雷达放置高度的前提下，先设定高度阈值，将包含地面点的点云保留下来，之后再利用RANSAC算法对地面点进行提取，提取出地面点后，考虑到车道线、井盖等材质组成，其反射能力较沥青、水泥等材质有明显区别，其反射能力较强，可以利用强度信息提取车道线、井盖等点云。本实施例设定一个阈值threshold，对其进行如下操作；

式中，intensity表示包含地面点的点云的反射强度值。选取flag为1的点云，经过上述操作，车道线就被检测出来了。井盖采取同样的方法进行提取。

减速带的提取，利用曲率进行，定义某帧点云中某一点的曲率为：

式中，|S|为点集S中的点云数量，S为与该点在同一激光线束上的点集，且S表示的点集均匀分布在该点的两侧；ri为该点云与激光坐标系原点间的欧式距离，rj为另一侧与激光坐标系原点间的欧式距离。设定一个阈值，选择大于阈值的点云，这样减速带就被提取出来。

S53：路杆提取

对于路杆，考虑到其具有垂直特性，要将其投影至平面进行处理。将点云投影到平面后，在二维空间对点进行聚类，聚类的目的是把相似点划分为一类，聚类后路杆包含的点被划分在一起，鉴于路杆竖直方向的半径变化均匀且高度较高，对其进行横向切片处理，计算各切片的对角线长度，如果两两相邻切片的对角线长增长不大，则认为该聚类为路杆。

S6：关联矩阵匹配

在提取出指纹后，需要对其进行关联矩阵计算，获得待定位位置的关联矩阵。在得到当前位置的关联矩阵后，将该矩阵与地图中矩阵进行匹配，选择矩阵相似度最大的节点，并保存下该节点地图中的点云信息，实现节点定位。

S61：对于提取出来的道路指纹，首先计算每个指纹的质心，用质心来代替每个指纹的位置信息。然后计算每个指纹之间的欧式距离用于关联矩阵计算。

S62：得到指纹之间的距离后，先进行排序选择其中最大和最小的距离值d_max,d_min，然后两者相减得到间隔大小，然后将这大小分成N子空间，每个子空间为ΔI＝(d_max-d_min)/N，我们最后可以形成[d_min+k*ΔI,d_min+(k+1)*ΔI](k取值0-N-1之间)的区间，然后将各指纹之间的距离进行投影，计算落在每个区间内各个数，最后就形成了一个1xN维的道路指纹关联矩阵。对路杆-路杆、路杆-交通标志牌、路杆-车道线、井盖和减速带三种类别矩阵进行上述投影操作，最终可以形成3xN的矩阵。

S6.2：得到当前关联矩阵后，用这个矩阵和筛选得到的地图节点矩阵进行匹配，匹配采用的是余弦相似度。

其中表示当前矩阵的第i行，其中表示地图中关联矩阵的第i行，最终得分是将三行计算的得分进行相加：

两个关联矩阵越相似，向量夹角越小，cosθ绝对值越大，选取得分score最大的那个节点地图信息，实现最后的节点级定位

S7：车辆定位

每帧激光点云获取的道路指纹与地图匹配采用的是ICP算法，ICP算法不必对点云进行分割和特征提取，其精度和收敛性都不错。假设源点云P_i为激光实时采集的经过道路指纹提取的点云，目标点云Q_i为已经构建好的点云，则ICP算法公式为

通过迭代来搜索旋转矩阵R和平移向量T，当E小于给定阈值时，则认为ICP已配准完成，停止迭代，否则将通过计算得到的R、T重新获得新的变换点云集继续与目标点云集配准，直至满足迭代结束条件。当迭代结束，得到的R、T则为最优旋转平移变换矩阵，也表示当前车辆的位姿。

本领域的技术人员容易理解，以上仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。