本发明公开一种基于道路指纹空间关联矩阵的车辆定位方法，该方法包括道路指纹空间关联矩阵构建和定位两个阶段。在道路指纹空间关联矩阵构建阶段，首先对高精度地图数据进行道路指纹提取，然后计算每个指纹之间的空间距离构建空间关联矩阵，得到每个场景具有唯一性的空间特征。在定位阶段，检测激光雷达点云中的道路指纹，并计算当前道路指纹的空间关联矩阵；再计算当前空间关联矩阵与地图数据库中空间关联矩阵之间的匹配度，选取匹配度最大的位置节点；最后利用当前点云道路指纹的坐标信息与该节点地图中道路指纹所包含的位置信息进行匹配，从而计算当前车辆的位置。本发明利用道路指纹关联矩阵的唯一性进行表征，保证了车辆定位的精确度。
1.一种基于道路指纹空间关联矩阵的车辆定位方法，其特征在于，包括以下步骤：道路指纹关联矩阵构建：
(1)对高精度点云地图进行道路指纹信息提取，以将离散、无组织且信息量较少的单一激光点云聚合成一个个具有一定空间拓扑结构且信息量丰富的三维语义目标，即道路指纹；把道路指纹聚类成一个个类别后，计算出每个类别的聚类中心，用聚类中心代表指纹的空间位置；
(2)计算每个指纹中心之间的欧式距离，利用欧式距离构建道路指纹关联矩阵，同时保留每个指纹的位置信息，结合道路指纹关联矩阵以及指纹的位置信息构建地图节点数据库；
(3)利用GPS获取车辆的初始位置信息，基于初始位置信息获取高精度点云地图中距离当前位置最接近的多个节点并提取节点中所包含的道路指纹关联矩阵；
(4)对当前位置所获取的点云数据进行道路指纹信息提取，获取当前位置的道路指纹关联矩阵；将当前位置的道路指纹关联矩阵与节点的道路指纹关联矩阵进行匹配，获取与当前位置最接近的节点；
(5)将当前位置的道路指纹位置信息和与当前位置最接近的节点的地图点云进行位置匹配，实现车辆精确定位。
2.根据权利要求1所述的基于道路指纹空间关联矩阵的车辆定位方法，其特征在于，高精度点云地图的构建包括以下步骤：在特定路段上，利用激光雷达采集包含道路指纹信息的三维激光点云，同时利用高精度惯导获取车辆的位置数据；
将位置数据转化成与激光坐标系尺度一样的UTM坐标系，接着利用最小二乘法来优化两坐标系轨迹之间的残差，得到两者之间的映射关系，即旋转平移矩阵；最后利用得到的激光雷达和高精度惯导之间的映射关系，将每帧点云利用对应的旋转平移矩阵将局部坐标系下的点云投影到全局坐标系下，获得包含道路指纹的高精度点云地图。
3.根据权利要求1所述的基于道路指纹空间关联矩阵的车辆定位方法，其特征在于，采用欧式聚类来提取指纹信息，以将离散、无组织且信息量较少的单一激光点云聚合成一个个具有一定空间拓扑结构且信息量丰富的三维语义目标；聚类完成后，会获得多个类别，即多个指纹；
将每个类别的点云数据投影至平面，然后计算该类别中所有点云数据的中心坐标，以计算得到的中心坐标作为该类别所对应的指纹的空间位置。
4.根据权利要求1所述的基于道路指纹空间关联矩阵的车辆定位方法，其特征在于，利用欧式距离构建道路指纹关联矩阵包括以下步骤：得到指纹的空间位置之后，计算各个指纹之间的欧式距离，计算公式如下：
式中，d表示欧式距离，(x1，y1)和(x2，y2)分别表示不同指纹的空间位置；
得到指纹之间的距离后，选择其中最大和最小的距离值dmax,dmin，然后两者相减得到间隔大小，进而将这大小分成N个子空间，每个子空间的大小为ΔI＝(dmax‑dmin)/N，最后可以形成[dmin+k*ΔI,dmin+(k+1)*ΔI]的区间，k取值0至N‑1之间；
然后将各指纹之间的距离进行投影，计算落在每个区间内的个数，最后就形成了一个
5.根据权利要求4所述的基于道路指纹空间关联矩阵的车辆定位方法，其特征在于，选取路杆‑路杆、路杆‑交通标志牌、路杆‑车道线、井盖或减速带这三种关联来构建关联矩阵，矩阵表示如下：式中，H1表示路杆‑路杆形成的关联矩阵，H2表示路杆‑交通标志牌形成的关联矩阵，H3表示路杆‑车道线、井盖或减速带形成的关联矩阵。
6.根据权利要求1所述的基于道路指纹空间关联矩阵的车辆定位方法，其特征在于，对当前位置所获取的点云数据进行道路指纹信息提取时，只选择道路指纹信息，剔除其他冗余信息。
7.根据权利要求1所述的基于道路指纹空间关联矩阵的车辆定位方法，其特征在于，将当前位置的道路指纹关联矩阵与节点的道路指纹关联矩阵进行匹配，获取与当前位置最接近的节点具体为：匹配采用的是余弦相似度，计算公式如下：其中， 表示当前位置的道路指纹关联矩阵的第i行， 表示地图中节点的道路指纹关联矩阵的第i行，将每行计算的得分进行相加；
两个关联矩阵越相似，向量夹角越小，cosθ绝对值越大，选取得分最大的那个节点地图信息，实现节点匹配定位。
8.根据权利要求1所述的基于道路指纹空间关联矩阵的车辆定位方法，其特征在于，道路指纹分为交通标志牌、车道线、减速带、井盖和路杆。
9.根据权利要求8所述的基于道路指纹空间关联矩阵的车辆定位方法，其特征在于，交通标志牌的提取方法为：设置一个高度阈值和一个强度阈值，将符合高度阈值和强度阈值的点云提取出来，即可得到正对的交通标志牌；对于背对的交通标志牌，首先利用PCA算法遍历三维激光点云数据以获取整体点云集，其次利用该点云集构造协方差矩阵Pcov，最后对协方差矩阵Pcov进行奇异值分解得到特征值λ1，λ2，λ3，并根据分解结果分析点云维度特征：当λ1>>λ2＝λ3时，D＝1，说明点集主要在1个维度上分散开来，则该点集具有线性维度特征；当λ1＝λ2>>λ3时，D＝2，说明点集主要在2个维度上分散开来，则该点集具有平面维度特征；当λ1＝λ2＝λ3时，D＝3，说明点集主要在3个维度上分散开来，则该点集为离散结构；
对于交通标志牌，其点云集具有平面维度特征，因此提取D＝2的点集；
车道线与井盖的提取方法为：车道线是附着在地面点中的，故提取车道线前需要提取地面点云，即设定高度阈值，将包含地面点的点云保留下来；之后再利用RANSAC算法对地面点进行提取，提取出地面点后，设定一个阈值threshold，对包含地面点的点云进行如下操作；
式中，intensity表示包含地面点的点云发射强度值；
选取flag为1的点云，得到车道线；井盖采取同样的方法进行提取；
减速带的提取方法为：利用曲率进行，定义某帧点云中某一点的曲率为：
式中，|S|为点集S中的点云数量，S为与该点在同一激光线束上的点集，且S表示的点集均匀分布在该点的两侧；ri为该点云与激光坐标系原点间的欧式距离，rj为另一侧与激光坐标系原点间的欧式距离；设定一个阈值，选择大于阈值的点云，提取出减速带；
路杆具有垂直特性，故将点云投影至平面进行处理；将点云投影到平面后，在二维空间对点进行聚类，聚类的目的是把相似点划分为一类，聚类后路杆包含的点被划分在一起，鉴于路杆竖直方向的半径变化均匀且高度较高，对聚类后的点进行横向切片处理，计算各切片的对角线长度，如果两两相邻切片的对角线长增长在预设范围内，则认为该聚类为路杆。
10.根据权利要求1所述的基于道路指纹空间关联矩阵的车辆定位方法，其特征在于，将当前位置的道路指纹位置信息和与当前位置最接近的节点的地图点云进行位置匹配，实现车辆精确定位具体为：获取与当前位置最接近的节点的地图点云集Q，以及当前位置经过道路指纹提取的目标点云集P，按照一定的约束条件，找到最邻近点对(pi，qi)，采用ICP算法计算出最优匹配参数R和T，使得误差函数E最小，计算公式如下：式中，Pi、Qi分别代表地图点云集与目标点云集中的对应点云点对，n表示对应的点云的总对数；
最终得到的R、T则为最优旋转平移变换矩阵，也表示当前车辆的位姿。
一种基于道路指纹空间关联矩阵的车辆定位方法
技术领域
[0001] 本发明属于车辆定位领域，具体涉及一种基于道路指纹空间关联矩阵的车辆定位方法。
背景技术
[0002] 随着经济发展与社会进步，智能汽车已逐渐进入人们的视野并取得了实质性的发展。而其中，车辆的自定位是智能车最核心的问题之一。目前，GPS技术被广泛应用于车辆定位，但低成本GPS定位精度较低，而高精度的GPS因成本较高而无法普遍使用。
[0003] 近年来，开始出现了基于相机和激光雷达的SLAM定位方法，该方法通过提取和匹配特征点来计算运动状态而实现定位，但该方法依然存在着误差累积问题，由于没有先验地图信息进行矫正，随着运动距离的增加，该方法的误差将越来越大。
发明内容
[0004] 本发明的目的在于，提供一种基于道路指纹空间关联矩阵的车辆定位方法，消除SLAM定位的误差累积问题，实现精度更高的定位。
[0005] 本发明所采用的技术方案如下：
[0006] 一种基于道路指纹空间关联矩阵的车辆定位方法，包括以下步骤：
[0007] 道路指纹关联矩阵构建：
[0008] (1)对高精度点云地图进行道路指纹信息提取，把道路指纹聚类成一个个类别，并计算出每个类别的聚类中心，用聚类中心代表指纹的空间位置；
[0009] (2)计算每个指纹中心之间的欧式距离，利用欧式距离构建道路指纹关联矩阵，同时保留每个指纹的位置信息；
[0010] 车辆定位：
[0011] (3)利用GPS获取车辆的初始位置信息，基于初始位置信息获取高精度点云地图中距离当前位置最接近的多个节点并提取节点中所包含的道路指纹关联矩阵；
[0012] (4)对当前位置所获取的点云数据进行道路指纹信息提取，获取当前位置的道路指纹关联矩阵；将当前位置的道路指纹关联矩阵与节点的道路指纹关联矩阵进行匹配，获取与当前位置最接近的节点；
[0013] (5)将当前位置的道路指纹位置信息和与当前位置最接近的节点的地图点云进行位置匹配，实现车辆精确定位。
[0014] 进一步的，高精度点云地图的构建包括以下步骤：
[0015] 在特定路段上，利用激光雷达采集包含道路指纹信息的三维激光点云，同时利用高精度惯导获取车辆的位置数据；
[0016] 将位置数据转化成与激光坐标系尺度一样的UTM坐标系，接着利用最小二乘法来优化两坐标系轨迹之间的残差，得到两者之间的映射关系，即旋转平移矩阵；最后利用得到的激光雷达和高精度惯导之间的映射关系，将激光点云数据投影到激光坐标系下，获得高精度点云地图。
[0017] 进一步的，采用欧式聚类来提取指纹信息，以将离散、无组织且信息量较少的单一激光点云聚合成一个个具有一定空间拓扑结构且信息量丰富的三维语义目标；聚类完成后，会获得多个类别，即多个指纹；
[0018] 将每个类别的点云数据投影至平面，然后计算该类别中所有点云数据的中心坐标，以计算得到的中心坐标作为该类别所对应的指纹的空间位置。
[0019] 进一步的，利用欧式距离构建道路指纹关联矩阵包括以下步骤：
[0020] 得到指纹的空间位置之后，计算各个指纹之间的欧式距离，计算公式如下：
[0021]
[0022] 式中，d表示欧式距离，(x1，y1)和(x2，y2)分别表示不同指纹的空间位置；
[0023] 得到指纹之间的距离后，选择其中最大和最小的距离值dmax,dmin，然后两者相减得到间隔大小，进而将这大小分成N个子空间，每个子空间的大小为ΔI＝(dmax‑dmin)/N，最后可以形成[dmin+k*ΔI,dmin+(k+1)*ΔI]的区间，k取值0至N‑1之间；
[0024] 然后将各指纹之间的距离进行投影，计算落在每个区间内的个数，最后就形成了一个1xN维的道路指纹关联矩阵。
[0025] 进一步的，选取路杆‑路杆、路杆‑交通标志牌、路杆‑车道线、井盖或减速带这三种关联来构建关联矩阵，矩阵表示如下：
[0026]
[0027] 式中，H1表示路杆‑路杆形成的关联矩阵，H2表示路杆‑交通标志牌形成的关联矩阵，H3表示路杆‑车道线、井盖或减速带形成的关联矩阵。
[0028] 进一步的，对当前位置所获取的点云数据进行道路指纹信息提取时，只选择道路指纹信息，剔除其他冗余信息。
[0029] 进一步的，将当前位置的道路指纹关联矩阵与节点的道路指纹关联矩阵进行匹配，获取与当前位置最接近的节点具体为：匹配采用的是余弦相似度，计算公式如下：
[0030]
[0031] 其中， 表示当前位置的道路指纹关联矩阵的第i行， 表示地图中节点的道路指纹关联矩阵的第i行，将每行计算的得分进行相加；
[0032] 两个关联矩阵越相似，向量夹角越小，cosθ绝对值越大，选取得分最大的那个节点地图信息，实现节点匹配定位。
[0033] 进一步的，道路指纹分为交通标志牌、车道线、减速带、井盖和路杆。
[0034] 进一步的，交通标志牌的提取方法为：设置一个高度阈值和一个强度阈值，将符合高度阈值和强度阈值的点云提取出来，即可得到正对的交通标志牌；对于背对的交通标志牌，首先利用PCA算法遍历三维激光点云数据以获取整体点云集，其次利用该点云集构造协方差矩阵Pcov，最后对协方差矩阵Pcov进行奇异值分解得到特征值λ1，λ2，λ3，并根据分解结果分析点云维度特征：
[0035]
[0036]
[0037] 当λ1＞＞λ2＝λ3时，D＝1，说明点集主要在1个维度上分散开来，则该点集具有线性维度特征；当λ1＝λ2＞＞λ3时，D＝2，说明点集主要在2个维度上分散开来，则该点集具有平面维度特征；当λ1＝λ2＝λ3时，D＝3，说明点集主要在3个维度上分散开来，则该点集为离散结构；对于交通标志牌，其点云集具有平面维度特征，因此提取D＝2的点集；
[0038] 车道线、井盖与减速带的提取方法为：车道线是附着在地面点中的，故提取车道线前需要提取地面点云，即设定高度阈值，将包含地面点的点云保留下来；之后再利用RANSAC算法对地面点进行提取，提取出地面点后，设定一个阈值threshold，对包含地面点的点云进行如下操作；
[0039]
[0040] 式中，intensity表示包含地面点的点云发射强度值；
[0041] 选取flag为1的点云，得到车道线；井盖采取同样的方法进行提取；
[0042] 减速带的提取方法为：利用曲率进行，定义某帧点云中某一点的曲率为：
[0043]
[0044] 式中，|S|为点集S中的点云数量，S为与该点在同一激光线束上的点集，且S表示的点集均匀分布在该点的两侧；ri为该点云与激光坐标系原点间的欧式距离，rj为另一侧与激光坐标系原点间的欧式距离；设定一个阈值，选择大于阈值的点云，提取出减速带；
[0045] 路杆的提取方法为：
[0046] 路杆具有垂直特性，故将点云投影至平面进行处理；将点云投影到平面后，在二维空间对点进行聚类，聚类的目的是把相似点划分为一类，聚类后路杆包含的点被划分在一起，鉴于路杆竖直方向的半径变化均匀且高度较高，对聚类后的点进行横向切片处理，计算各切片的对角线长度，如果两两相邻切片的对角线长增长在预设范围内，则认为该聚类为路杆。
[0047] 进一步的，将当前位置的道路指纹位置信息和与当前位置最接近的节点的地图点云进行位置匹配，实现车辆精确定位具体为：
[0048] 获取与当前位置最接近的节点的地图点云集Q，以及当前位置经过道路指纹提取的目标点云集P，按照一定的约束条件，找到最邻近点对(pi，qi)，采用ICP算法计算出最优匹配参数R和T，使得误差函数E最小，计算公式如下：
[0049]
[0050] 式中，Pi、Qi分别代表地图点云集与目标点云集中的对应点云点对，n表示对应的点云的总对数；
[0051] 最终得到的R、T则为最优旋转平移变换矩阵，也表示当前车辆的位姿。
[0052] 本发明的有益效果为：
[0053] 本发明首先在离线状态下利用高精度惯导和激光雷达来构建地图数据库，地图之中包含了位置信息和道路指纹关联矩阵。在定位时，首先利用普通GPS传感器采集得到的位置信息进行初定位，得到相近的多个节点地图信息，然后利用当前点云数据得到的道路指纹关联矩阵与节点地图中的矩阵进行匹配，选择相似度得分最大的一个节点，完成节点级定位，最终利用最近节点中的位置信息与当前点云数据的位置信息进行匹配，最终获得车辆高精度的定位。本发明提出的定位方法精度高、不易受到光线干扰、鲁棒性强。
附图说明
[0054] 图1是本发明一实施例的车载设备示意图。
[0055] 图2是本发明一实施例的坐标标定示意图。
[0056] 图3是本发明一实施例的方法流程示意图。
[0057] 图中：1‑激光雷达，2‑惯导。
具体实施方式
[0058] 下面将结合附图对本发明作进一步的说明：
[0059] 本发明公开了一种基于道路指纹空间关联矩阵的车辆定位方法，该方法包括道路指纹空间关联矩阵构建和定位两个阶段。在道路指纹空间关联矩阵构建阶段，首先对高精度地图数据进行道路指纹提取，道路指纹包含车道线、交通标志牌、减速带、井盖、路杆等信息，提取出道路指纹后，再计算每个指纹之间的空间距离构建空间关联矩阵，得到每个场景具有唯一性的空间特征。在定位阶段，通过激光雷达采集激光雷达点云，检测激光雷达点云中的道路指纹，并计算当前道路指纹的空间关联矩阵。之后再计算当前空间关联矩阵与地图数据库中空间关联矩阵之间的匹配度，选取匹配度最大的位置节点。最后利用当前点云道路指纹的坐标信息与该节点地图中道路指纹所包含的位置信息进行匹配，从而计算当前车辆的位置。本发明利用道路指纹关联矩阵的唯一性进行表征，保证了车辆定位的精确度。
[0060] 图3是本发明实施例提供的一种方法流程示意图，包括地图数据库构建和车辆定位两个阶段；其中，地图数据库构建阶段主要包括激光雷达和高精度惯导数据，利用惯导数据和提取的道路指纹来得到位置和空间关联矩阵，定位阶段主要包括普通GPS的初定位、矩阵匹配的节点级定位和位置匹配的车辆定位。首先利用GPS做一个初定位，选择当前车辆所在的一个范围，获得多个节点的关联矩阵，例如当前位置前后40m范围的地图节点，这样可以减少全局搜索的范围，加快计算速率；之后对单帧点云进行处理，通过计算曲率和聚类等操作把道路指纹信息提取出来并获得当前的关联矩阵，利用这个关联矩阵与初定位选取的多个道路指纹关联矩阵库进行匹配，选择相似度最高的节点。最后利用当前位置的指纹位置信息与地图位置信息进行匹配实现车辆定位。
[0061] 具体包括以下步骤：
[0062] S1：数据采集；
[0063] 首先在本发明实施例路段上，如图1所示，利用数据采集车上装载的激光雷达1实时扫描得到当前环境包含道路指纹的三维激光点云，同时，数据采集车上还装载有高精度惯导定位模块，利用该惯导2得到当前的车辆位置信息；其中定位模块精度为厘米级。
[0064] S2：数据处理；
[0065] 利用高精度惯导和激光雷达采集一段轨迹的GPS和激光雷达点云信息，然后将GPS数据转化成与激光坐标系尺度一样的UTM坐标系，如图2所示，坐标系统一后，需要对两轨迹进行拟合，首先构建损失函数，再利用最小二乘法来优化两轨迹之间的残差，得到两者之间的映射关系，即旋转平移矩阵。之后利用得到的两传感器的映射关系，将每帧点云利用对应的旋转平移矩阵将局部坐标系下的点云投影到全局坐标，得到包含道路指纹的高精度点云地图。
[0066] S3：道路指纹库构建；
[0067] S31：得到高精度地图后，对高精度点云地图进行道路指纹信息的提取，把道路指纹聚类成一个个类别，并计算出每个类别的聚类中心，用聚类中心代表指纹的空间位置。可以采用欧式聚类来提取指纹信息，聚类的目的在于将离散、无组织且信息量较少的单一激光点云聚合成一个个具有一定空间拓扑结构的且信息量丰富的三维语义目标，即道路指纹。本实施例采用的DBSCAN算法，它不需要指定簇的个数，具有擅长找到离群点，聚类结果没有偏倚等优点。
[0068] 聚类完成后，使用聚类中心来表示每个类别的位置。为了减小点云扫描不均匀的影响，可以将点云投影至平面中，然后计算投影后点云的中心坐标，其计算公式可以表示成如下形式：
[0069]
[0070] 其中，xn为该类别的某点数据，为计算的该类别中心坐标。
[0071] S32：计算每个指纹中心之间的欧式距离，利用这个欧式距离构建关联矩阵，同时保留每个指纹的位置信息，用于位置匹配。每个类别的欧式距离可以表示成如下形式：
[0072]
[0073] 得到距离后，就可以构建关联矩阵。空间关联矩阵是一个二维矩阵。本实施例采用路杆‑路杆、路杆‑交通标志牌、路杆‑车道线、井盖和减速带(路杆‑车道线、路杆‑井盖、路杆‑减速带视为一类)三种关联来构建矩阵。把这个矩阵保存在文本中，在定位时从文本读取用于匹配。关联矩阵可以表示成如下：
[0074]
[0075] 式中，H1表示路杆‑路杆形成的关联矩阵，H2表示路杆‑交通标志牌形成的关联矩阵，H3表示路杆‑车道线、井盖和减速带形成的关联矩阵。除了保存关联矩阵，我们还保留了每个类别的位置坐标。关联矩阵是为了寻找到相似节点，位置坐标是为了匹配实现车辆定位。
[0076] S4：初定位
[0077] 利用待定位车辆上的普通GPS接收机来提供一个初始位置信息，利用这个信息来选择地图中距离当前位置最接近的多个节点并提取节点中所包含的点云和空间关联矩阵。在后续中，不再使用GPS数据，GPS只用于初始化阶段。
[0078] S5：道路指纹提取
[0079] 道路指纹提取分为交通标志牌、车道线、减速带、井盖和路杆等标志。每个标志具有不同的提取方法。对当前激光雷达采集的点云进行特征提取时，可以只选择道路指纹信息，剔除不需要的冗余的其他信息，这样不仅减少了数据量，也可以加快匹配时间。
[0080] S51：交通标志牌提取
[0081] 交通标志牌是道路中常见的道路标志，其布置一般具有高度要求，因此可以先设置一定的高度，将一些地面点滤除，也把包含车牌等高反射强度的物体滤除。同时考虑到交通标志的高反射率，激光雷达扫描到交通标志牌后其反射回来的激光束强度很大，因此利用这种特性将正对的交通标志牌提取出来。而对背对的情况，利用PCA算计进行提取。
[0082] PCA算法首先遍历三维激光点云数据以获取整体点云集，其次利用该点云集构造协方差矩阵Pcov，最后对协方差矩阵Pcov进行奇异值分解得到特征值λ1，λ2，λ3，并根据分解结果分析点云维度特征。
[0083]
[0084]
[0085] 当λ1＞＞λ2＝λ3时，D＝1，说明点集主要在1个维度上分散开来，则该点集具有线性维度特征；当λ1＝λ2＞＞λ3时，D＝2，说明点集主要在2个维度上分散开来，则该点集具有平面维度特征；当λ1＝λ2＝λ3时，D＝3，说明点集主要在3个维度上分散开来，则该点集为离散结构。对于交通标志牌，其点云集具有平面维度特征，因此提取D＝2的点集。
[0086] S52：车道线、井盖与减速带提取
[0087] 车道线是附着在地面点中的，因此提取车道线前需要提取地面点云。在知道激光雷达放置高度的前提下，先设定高度阈值，将包含地面点的点云保留下来，之后再利用RANSAC算法对地面点进行提取，提取出地面点后，考虑到车道线、井盖等材质组成，其反射能力较沥青、水泥等材质有明显区别，其反射能力较强，可以利用强度信息提取车道线、井盖等点云。本实施例设定一个阈值threshold，对其进行如下操作；
[0088]
[0089] 式中，intensity表示包含地面点的点云的反射强度值。选取flag为1的点云，经过上述操作，车道线就被检测出来了。井盖采取同样的方法进行提取。
[0090] 减速带的提取，利用曲率进行，定义某帧点云中某一点的曲率为：
[0091]
[0092] 式中，|S|为点集S中的点云数量，S为与该点在同一激光线束上的点集，且S表示的点集均匀分布在该点的两侧；ri为该点云与激光坐标系原点间的欧式距离，rj为另一侧与激光坐标系原点间的欧式距离。设定一个阈值，选择大于阈值的点云，这样减速带就被提取出来。
[0093] S53：路杆提取
[0094] 对于路杆，考虑到其具有垂直特性，要将其投影至平面进行处理。将点云投影到平面后，在二维空间对点进行聚类，聚类的目的是把相似点划分为一类，聚类后路杆包含的点被划分在一起，鉴于路杆竖直方向的半径变化均匀且高度较高，对其进行横向切片处理，计算各切片的对角线长度，如果两两相邻切片的对角线长增长不大，则认为该聚类为路杆。
[0095] S6：关联矩阵匹配
[0096] 在提取出指纹后，需要对其进行关联矩阵计算，获得待定位位置的关联矩阵。在得到当前位置的关联矩阵后，将该矩阵与地图中矩阵进行匹配，选择矩阵相似度最大的节点，并保存下该节点地图中的点云信息，实现节点定位。
[0097] S61：对于提取出来的道路指纹，首先计算每个指纹的质心，用质心来代替每个指纹的位置信息。然后计算每个指纹之间的欧式距离用于关联矩阵计算。
[0098] S62：得到指纹之间的距离后，先进行排序选择其中最大和最小的距离值dmax,dmin，然后两者相减得到间隔大小，然后将这大小分成N子空间，每个子空间为ΔI＝(dmax‑dmin)/N，我们最后可以形成[dmin+k*ΔI,dmin+(k+1)*ΔI](k取值0‑N‑1之间)的区间，然后将各指纹之间的距离进行投影，计算落在每个区间内各个数，最后就形成了一个1xN维的道路指纹关联矩阵。对路杆‑路杆、路杆‑交通标志牌、路杆‑车道线、井盖和减速带三种类别矩阵进行上述投影操作，最终可以形成3xN的矩阵。
[0099] S6.2：得到当前关联矩阵后，用这个矩阵和筛选得到的地图节点矩阵进行匹配，匹配采用的是余弦相似度。
[0100]
[0101] 其中 表示当前矩阵的第i行，其中 表示地图中关联矩阵的第i行，最终得分是将三行计算的得分进行相加：
[0102]
[0103] 两个关联矩阵越相似，向量夹角越小，cosθ绝对值越大，选取得分score最大的那个节点地图信息，实现最后的节点级定位
[0104] S7：车辆定位
[0105] 每帧激光点云获取的道路指纹与地图匹配采用的是ICP算法，ICP算法不必对点云进行分割和特征提取，其精度和收敛性都不错。假设源点云Pi为激光实时采集的经过道路指纹提取的点云，目标点云Qi为已经构建好的点云，则ICP算法公式为
[0106]
[0107] 通过迭代来搜索旋转矩阵R和平移向量T，当E小于给定阈值时，则认为ICP已配准完成，停止迭代，否则将通过计算得到的R、T重新获得新的变换点云集继续与目标点云集配准，直至满足迭代结束条件。当迭代结束，得到的R、T则为最优旋转平移变换矩阵，也表示当前车辆的位姿。
[0108] 本领域的技术人员容易理解，以上仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
