一种基于三维激光检测车道线的车辆多尺度定位方法与流程

本发明属于智能交通系统领域，更具体地，涉及一种基于三维激光检测车道线的车辆多尺度定位方法。

背景技术：

智能车辆的研究已经逐渐成为智能交通系统发展的新方向，而其中，车辆的自定位是智能车最核心的问题之一。目前，gps技术被广泛应用于车辆定位，但gps存在信号遮挡的问题，在隧道等一些不能很好接收到gps信号的盲区，单纯的gps不能满足车辆自定位需求。

近年来出现的智能车定位方法，如公开号为cn108413971a的专利申请，公开了一种利用车载相机获取地面标志以实现车辆定位的方法，公开号为cn108845343a的专利申请，公开了一种利用视觉计算与路面表征的距离，并通过kalman滤波融合gps定位信息实现车辆定位的方法。但这些方法均依赖于车载相机，存在视觉检测视角范围小、受环境光线影响较大的问题。而三维激光雷达作为一种具有精度高、鲁棒性强等优点的传感器，在车辆环境感知方面展现出良好的研究前景。

技术实现要素：

针对现有技术的以上缺陷或改进需求，本发明提出了一种基于三维激光检测车道线的车辆多尺度定位方法，由此解决单一gps存在信号盲区且视觉相机检测易受环境光线干扰的技术问题。

为实现上述目的，本发明提供了一种基于三维激光检测车道线的车辆多尺度定位方法，包括：

(1)建立包含路面三维激光点云、车道线点云投影以及对应位置信息的地图节点；

(2)通过待定位车辆当前gps信息进行粗定位，并将当前位置的车道线点云投影图与所述地图节点中的车道线点云投影图进行匹配，得到最接近待定位位置的地图节点以实现帧级定位；

(3)利用待定位车辆当前位置激光扫描得到的三维激光点云与所述最接近待定位位置的地图节点中的三维激光点云配准，获得相对位置关系，并结合帧级定位结果确定的所述最接近待定位位置的地图节点对应的位置信息得到待定位车辆所在位置信息，实现车辆的度量级定位。

优选地，步骤(1)包括：

(1.1)获取数据采集车上的激光雷达实时扫描得到的当前环境包含路面车道线的三维激光点云，获取利用rtk-gps传感器得到的车辆位置信息及imu惯性测量系统得到的车辆惯导信息；

(1.2)对所述三维激光点云进行预处理，利用路面和车道线反射率的不同提取出路面车道线点云，将所述路面车道线点云投影至平面图像，同时，对所述三维激光点云、所述车辆位置信息及所述车辆惯导信息进行融合，其中，融合过程包括时间同步与空间同步；

(1.3)结合实际车速与传感器频率，将同步后的传感器数据每隔固定频率保存至地图节点数据库，其中，在所述地图节点数据库中的每个节点包含：三维激光点云、车道线点云投影以及对应的位置信息。

优选地，步骤(1.2)包括：

(1.2.1)基于扫描线方法从所述三维激光点云中提取路面点云，提取所述路面点云中激光反射强度大于预设激光反射强度阈值的点作为车道线点云；

(1.2.2)选定x-y平面作为投影平面，基于预设分辨率将所述车道线点云投影至所述投影平面，得到车道线点云投影图像；

(1.2.3)基于所述激光雷达、所述rtk-gps传感器及所述imu惯性测量系统采集到的每帧数据的时间戳，当各传感器中任意两个的时间戳之差小于预设时间阈值，则认定为同一时刻数据，以实现时间同步；将所述三维激光点云、所述车辆位置信息及所述车辆惯导信息通过坐标系变换转换至世界坐标系下，以实现空间同步。

优选地，步骤(2)包括：

(2.1)待定位车辆在行驶过程中，通过激光传感器获取当前位置的三维激光点云，同时，由所述待定位车辆上的gps接收机得到所述待定位车辆位置的粗定位范围，通过与所述地图节点数据库中保存的位置信息比较，得到所述粗定位范围内多个候选地图节点；

(2.2)提取所述待定位车辆当前位置的三维激光点云中的车道线点云，并投影至平面，得到所述待定位车辆的车道线点云投影图，将所述待定位车辆的车道线点云投影图与所述多个候选地图节点中的车道线点云投影进行匹配，得到与所述待定位车辆当前位置最接近的地图节点。

优选地，步骤(2.2)包括：

(2.2.1)在所述待定位车辆的车道线点云投影图中随机设定一个像素点为圆心，选取以所述圆心构造的领域圆上分布的若干个像素点与圆心像素点进行像素值差值比较，若差值绝对值超过设定差值阈值的像素点个数满足要求，则将圆心像素点作为候选特征点，若预设范围内有多个所述候选特征点，则保留候选特征点的响应值最大时对应的候选特征点，得到目标候选特征点；

(2.2.2)基于灰度质心法确定各所述目标候选特征点的方向；

(2.2.3)以目标候选特征点为中心，设定一个s×s的窗口，以窗口内部为范围，在提取出的目标候选特征点中随机选取若干对特征点对，通过比较每对特征点对的像素值大小，得到中心特征点对应的二进制编码作为中心特征点的brief描述子；

(2.2.4)将选取的特征点的方向作为brief描述子方向，得到orb特征点描述子；

(2.2.5)根据每个中心特征点对应的orb特征点描述子，计算出当前位置待定位车辆的车道线激光点云投影图像中每个特征点的orb特征点描述子与候选地图节点中的车道线点云投影图像中每个特征点的orb特征点描述子的汉明距离，当两个特征描述子的汉明距离小于预设距离阈值时，则认为它们对应的特征点匹配，并利用ransac算法剔除错误匹配点，通过比较最终匹配点对的数量，在候选地图节点中找到匹配特征点数量最多的车道线点云投影对应的地图节点，作为与待定位车辆当前位置最接近的地图节点。

优选地，步骤(2.2.2)包括：

由确定目标候选特征点邻域的(p+q)阶矩，其中，i(x,y)代表图像在坐标点(x,y)处的灰度值，b表示目标候选特征点邻域范围；

由确定目标候选特征点的方向，其中，(cx,cy)为目标候选特征点邻域的质心坐标，θ为质心与目标候选特征点的夹角，即为目标候选特征点的方向。

优选地，在步骤(2.2.3)中，由确定中心特征点的brief描述子，其中，ip、iq分别是随机选取的特征点对p、q的像素值，fn(p)为中心特征点对应的二进制编码，n表示步骤(2.2.3)中随机选取的特征点对数。

优选地，在步骤(2.2.4)中，由gn(p,θ)＝fn(p)∣(pi,qi)∈kθ得到中心特征点p的描述子gn(p,θ)，其中，k矩阵为随机选取的目标候选特征点对的坐标点对构成的矩阵，pi,qi(i∈(1,n))表示随机选取的第i个目标候选特征点对在x、y轴的坐标，rθ为仿射变换矩阵，θ为目标候选特征点的方向。

优选地，步骤(3)包括：

(3.1)将所述待定位车辆的车道线点云投影图特征点索引至三维激光点云数据的三维点中，利用若干组对应的特征点坐标完成对点云数据的粗配准，得到初始旋转矩阵r和平移向量t；

(3.2)通过迭代搜索2个待配准激光点云集中距离最近的对应点，使得它们欧氏距离的平方和最小，以此计算出待配准点云集间的旋转矩阵r和平移向量t，e代表目标函数，表示2个点云数据集之间的差异程度，pi、qi分别代表源点集与目标点集中的对应点云点对，n表示对应的点云个数，当e取最小值时即得到最优旋转平移变换矩阵r、t；

(3.3)根据待定位位置的三维激光点云与最接近的地图节点对应的三维点云图之间的最优位置变换矩阵r、t，通过地图节点中对应的位置信息，计算出待定位车辆的位置，实现对车辆的度量级定位。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：本发明首先在地图节点数据库构建阶段建立包含三维激光点云、车道线点云投影以及对应位置信息的节点地图，然后在定位阶段，通过车辆当前gps信息进行粗定位，并将当前位置的车道线点云投影图与地图节点中的投影图进行匹配，得到最接近待定位位置的地图节点实现帧级定位，最后利用当前位置激光扫描得到的激光点云与地图节点的三维点云配准，获得相对位置关系，并结合帧级定位结果确定的地图节点对应的位置信息得到待定位车辆所在位置信息，实现车辆的度量级定位。相比较于传统方法，本发明提出的定位方法精度高、鲁棒性强、不易受到环境天气、光线等干扰。

附图说明

图1是本发明实施例提供的一种方法流程示意图；

图2是本发明实施例提供的一种车载激光线束扫描示意图；

图3是本发明实施例提供的一种车载激光线束扫描俯视图；

图4是本发明实施例提供的一种gps粗定位示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

在本发明实例中，“第一”、“第二”等是用于区别不同的对象，而不必用于描述特定的顺序或先后次序。

如图1所示是本发明实施例提供的一种方法流程示意图，包括地图节点数据库构建和车辆定位两个阶段；其中，地图节点数据库构建阶段主要包括数据采集、数据预处理和数据库构建，车辆定位阶段主要包括粗定位、帧级定位和度量级定位。

在本发明实施例中的地图节点数据库构建阶段，具体包括以下步骤：

s1：数据采集；

首先在本发明实施例路段上，利用数据采集车上装载的velodyne16线激光雷达实时扫描得到当前环境包含路面车道线的三维激光点云，同时，数据采集车上还装载有rtk-gps传感器和imu惯性测量系统组成的定位模块，利用该定位模块分别得到当前的rtk-gps位置信息和惯导信息；其中数据采集车上的定位模块精度为0.2-0.8m。

s2：数据预处理；

在本发明实施例中，对velodyne16线激光雷达扫描得到的三维激光点云数据进行预处理，利用激光扫描到的路面与车道线反射率的不同提取出车道线点云，并将其投影至平面图像。同时，对三维激光雷达、rtk-gps差分定位系统与imu惯性测量系统采集到的数据进行融合，融合过程包括时间同步与空间同步。具体步骤如下：

s2.1：提取车道线点云；

考虑到车载激光扫描道路环境获取的点云场景复杂且目标多样，会对车道线点云提取产生一定困难。为解决上述问题，在基于图2所示激光扫描线束的原理后，对velodyne16线激光扫描得到的场景点云数据进行路面分割，来排除复杂环境的影响，公式如下所示：

h＝|μ1-μ2|

其中，x、y为激光点云坐标，radius代表该点到雷达的水平距离，μ表示该点相对于车头正方向的角度，即与x轴的夹角，h表示点云中两点的坡度差。

如图3所示，θ表示激光雷达水平角度分辨率，在本发明实施例中，velodyne16线激光雷达水平角度分辨率设定为0.2°。根据点云到雷达距离radius的大小，对θ上的线束点进行排序，并计算前后两点的坡度差h，当坡度差h小于设定阈值，则认为该点为路面点。

在分割出道路点云后，统计分割后的点云三维空间坐标(xmin、ymin、zmin、xmax、ymax、zmax)并开始提取车道线点云。对比于路面，车道线因其涂料介质特殊，往往对激光呈高亮反射，反射强度值较高，利用激光反射强度可以有效改善传统方法中利用几何特征进行车道线提取的结果。通过设定激光反射强度阈值，从车载激光velodyne16扫描得到的环境点云中提取出属于车道线的点云。在本发明实施实例中，车道线激光反射强度阈值范围设定为15-35。

s2.2：点云投影；

在提取出车道线点云后，选定x-y平面作为投影平面，在本发明实施实例中，分辨率设置为0.2m，并基于该分辨率将车道线点云坐标投影至xoy平面，得到车道线投影图像。

s2.3：传感器数据融合；

传感器的数据融合主要包括时间同步和空间同步。

时间同步：三维激光雷达、rtk-gps差分定位系统与imu惯性测量系统采集到的每帧数据都有其对应的时间戳，当传感器中任意两个的时间戳之差小于阈值，则认定为同一时刻数据。

空间同步：将三个传感器采集到的数据通过坐标系变换转换至世界坐标系下，以实现空间同步。

在本实施实例中，定位模块中的传感器采集到的位置信息均在转换后由经纬度表示。

s3：节点数据库构建；

结合实际车速与传感器频率，将处理后已同步的传感器数据每隔固定频率保存至地图节点数据库，其中，每个节点包含：三维点云、车道线点云投影以及对应的位置信息。在本发明实施实例中，数据库的每个节点间隔为1m。

车辆定位阶段：

s4：粗定位；

在本发明实施实例中，待定位车上装载有激光雷达以及gps接收机，其中该gps接收机定位精度为10-20m。

在待定位车行驶过程中，velodyne16线车载激光雷达扫描获取当前位置的三维激光点云，同时，如图4所示，待定位车上的gps接收机得到当前车辆位置的粗定位范围，通过与地图节点数据库中保存的位置信息返回范围内多个候选地图节点。

s5：帧级定位；

将车辆当前位置的激光点云中的车道线点云提取并投影至平面，得到对应的投影图像。在利用orb提取投影图像特征点后，通过与多个候选地图节点中的车道线点云投影匹配，得到与车辆当前位置最接近的地图节点。其中车道线点云投影图像匹配过程具体包括：

s5.1：提取投影图像特征点；

通过fast算法提取车道线点云投影图像的特征点，即随机设定一个像素点为圆心，选取以3像素为半径构造的圆上分布的16个像素点与圆心像素点进行像素值差值比较。若其中有12个差值绝对值超过设定的阈值，则判断该圆心点为候选特征点，公式如下：

其中，i(p)为中心像素点p的灰度值，i(xi)(i∈[1,16])为p点周围邻域circle(p)像素点的灰度值，ε为设定的阈值。当n大于12，则认为p点为角点。

其中，像素半径、像素点个数及阈值大小可以根据实际需要确定，本发明实施例不做唯一性限定。

为避免提取到的特征点过于集中，采用非极大值抑制方法对候选特征点进行筛选，即对于一定范围内有多个候选特征点的情况，对所有候选特征点的响应值进行计算并保留最大响应值对应的候选特征点，从而得到最终候选特征点的集合。其中响应值计算公式如下：

其中，v为响应值。

考虑到fast算法提取的角点不具备方向性，引入灰度质心法确定特征点方向，即计算特征点的矩来确定方向。某特征点邻域的(p+q)阶矩公式如下：

其中，i(x,y)代表图像在坐标点(x,y)处的灰度值，b表示目标候选特征点邻域范围，再通过图像块的矩计算得到质心方向，过程如下：

其中，c为特征点邻域的质心坐标(cx,cy)，θ为质心与特征点的夹角，即为特征点方向。

s5.2：描述特征点；

以特征点为中心，选取建立s×s的窗口，以窗口内部为范围随机选取n对特征点对，通过比较每对特征点对的像素值大小，得到中心特征点对应的二进制编码，即中心特征点的brief描述子，其中，本发明实施例中n取值为256。其公式为：

其中，ip、iq分别是随机选取的特征点对p、q的像素值，fn(p)为中心特征点p对应的二进制编码。

为了使brief描述子方向不会因为旋转而改变，将fast角点方向作为brief描述子方向，得到中心特征点p的描述子：

gn(p,θ)＝fn(p)∣(pi,qi)∈kθ

其中，k矩阵为随机选取的坐标点对构成的矩阵，pi,qi(i∈(1,n))表示随机选取的第i个特征对在x，y轴的坐标，rθ为仿射变换矩阵，θ为fast特征点方向，利用仿射变换矩阵计算获得新的描述矩阵kθ，结合brief描述子得到orb特征描述子gn(p,θ)。

s5.3：特征点匹配；

根据每个中心特征点对应的orb特征描述子，计算出当前位置车道线激光点云投影图像中每个特征点描述子与候选地图节点中的车道线点云投影图像中每个特征点描述子的汉明距离，当两个特征描述子之间的汉明距离小于一定阈值时，则认为它们对应特征点匹配。

根据汉明距离实现特征点粗匹配后，再利用ransac算法剔除错误匹配点做进一步筛选，即从带有误匹配的数据中不断迭代，以获得最优模型以及该模型下的正确匹配点。具体步骤如下所示：

s5.3.1：随机选取通过汉明距离粗匹配得到的特征点对中的4对匹配特征点计算投影图像之间的相对变换关系，公式如下：

其中，(x,y)、(x′,y′)分别为选取出的投影图像上匹配特征点的坐标，h矩阵表示投影图像之间的单应性矩阵，即相对变换关系。

s5.3.2：通过相对变换矩阵计算特征点对之间的误差，通过判断该误差是否小于设定阈值来剔除误匹配，公式如下：

其中，(xi,yi)、(xi′,yi′)为粗匹配中的所有特征点对对应的坐标，t为设定的阈值，当小于该阈值时，则认为该点为正确匹配，反之则剔除。

s5.3.3：设定迭代次数，对s5.3.2和s5.3.3进行迭代，迭代结束后，选取正确匹配点数最多的迭代结果作为最终特征匹配结果。

在本发明实施例中，完成车道线点云投影图像匹配后，通过比较匹配点对的数量，在候选地图节点中找到匹配特征点数量最多的车道线点云投影对应的地图节点，即为与当前位置最接近的地图节点。

s6：度量级定位；

对待定位位置的三维点云与当前位置最接近的地图节点对应的三维点云进行点云配准，计算二者的位姿关系，并根据地图节点中对应的位置信息得到待定位车辆位置的精确定位结果。

在本发明实施例中，度量级定位采用迭代最近邻点算法进行三维点云配准，得到当前位置与最接近的地图节点之间的旋转平移变换关系，具体过程如下所示：

s6.1：粗配准；

将得到的投影图像特征点索引至三维激光点云数据的三维点中，利用4组对应的特征点坐标完成对点云数据的粗配准，得到初始旋转矩阵r和平移向量t；

s6.2：icp精确配准；

通过迭代搜索2个待配准激光点云集中距离最近的所有对应点对，使得它们欧氏距离的平方和最小，假定激光点云中2个三维点pi＝(xi,yi,zi)、qi＝(xj,yj,zj)，则它们的欧氏距离为：

通过上述搜索得到的对应点对来计算旋转矩阵r和平移向量t，公式如下：

其中，e代表目标函数，表示2个点云数据集之间的差异程度，pi、qi分别代表源点集与目标点集中的对应点云点对，n表示对应的点云个数，当e小于给定阈值时，则认为icp已配准完成，停止迭代，否则将通过计算得到的r、t重新获得新的变换点云集继续与目标点云集配准，直至满足迭代结束条件。

当迭代结束，即e取最小值时，得到的r、t则为最优旋转平移变换矩阵，也表示待定位位置的激光点云与最接近的地图节点对应的三维点云之间的相对位置关系。根据该相对位置关系以及地图节点中对应的位置信息，计算出待定位车辆的位置，实现对车辆的度量级定位。

需要指出，根据实施的需要，可将本申请中描述的各个步骤/部件拆分为更多步骤/部件，也可将两个或多个步骤/部件或者步骤/部件的部分操作组合成新的步骤/部件，以实现本发明的目的。

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