基于高精度地图和场景检索的车道线辅助定位方法

摘要

本发明公开了一种基于高精度地图和场景检索的车道线辅助定位方法，其中，主要包括以下步骤：建立高精度车道线地图；建立车道场景库，其中，将车道场景库中的场景图像作为参考图像，且参考图像中标记有参考车道序号；车辆采集当前场景图像为当前图像，将当前图像与车道场景库中的参考图像比对，得到最接近的参考图像，并从最接近的参考图像中获取参考车道序列号；车辆采用低精度GPS获得低精度位置信息，并将低精度位置信息及参考车道序列号发送至高精度GPS，高精度GPS结合低精度位置信息与参考车道序列号，给出车辆当前正确的车道线；车辆检测当前车道线，并以正确的车道线为标准调整车辆当前的位姿，以使车辆与正确的车道线匹配。

权利要求

1.一种基于高精度地图和场景检索的车道线辅助定位方法，其中，主要包括以下步骤：步骤1、采用高精度GPS采集道路的车道线位置，建立高精度车道线地图；

步骤2、采集高精度车道线地图中不同车道的不同位点的包含场景图像的场景数据，建立车道场景库，其中，将所述车道场景库中的场景图像作为参考图像，且所述参考图像中标记有参考车道序号；

步骤3、车辆采集当前场景图像为当前图像，将当前图像与车道场景库中的参考图像比对，得到最接近的参考图像，并从所述最接近的参考图像中获取参考车道序列号；

其中，获取最接近的参考图像的具体方法主要包括以下步骤：

步骤4.1、获得车辆当前场景图像，并利用低精度GPS获得当前位置；

步骤4.2、查询车道场景库中在当前位置10-30米范围之内的场景图像，作为候选场景图像；

步骤4.3、计算当前场景图像与候选场景图像的相似度，将候选场景图像中得分最高的作为参考场景图像，依照参考场景图像得到车辆当前的参考车道序列号；

步骤4、车辆采用低精度GPS获得低精度位置信息，并将低精度位置信息及参考车道序列号发送至高精度GPS，高精度GPS结合低精度位置信息与参考车道序列号，给出车辆当前正确的车道线；

步骤5、车辆检测当前车道线，并以正确的车道线为标准调整车辆当前的位姿，以使车辆与正确的车道线匹配，其中，车辆位姿调整的方法为先进行车辆的航向优化，再进行车辆的横向位置优化；

所述横向位置优化方法为：使用Levenberg-Marquardt算法，逐步用Δt补偿车辆位姿中的横向位置，使得车辆坐标系下的正确的车道线和当前车道线的距离最小化，最后，把计算得到的Δt还原回世界坐标系下，得到车辆在优化后的横向位置；将当前车道线在车辆坐标系下平移一段距离Δt，与正确的车道线匹配，即完成横向位置优化，其中，Δt表示当前车道线的平移距离，用Δt补偿车辆位姿中的横向位置的计算公式为公式3：公式3

其中，[0，1，0]T表示车辆坐标系的左向单位向量， 表示车辆的姿态即车辆坐标系的坐标轴相对世界坐标系的坐标轴的旋转角度， 表示世界坐标系下的车道线点， 表示车辆坐标系相对于世界坐标系的位置， 表示该单位向量在世界坐标系下的表示， 表示补偿的横向距离在世界坐标系下的表示； 表示车道线点投影到车辆坐标系下后相对于车辆的位置。

2.如权利要求1所述的基于高精度地图和场景检索的车道线辅助定位方法，其中，步骤

2中所述场景数据还包括：采集场景图像时车辆的位置、姿态和所在车道的序列号；采集场景图像时的天气情况和交通状况，以及场景图像的词袋向量。

3.如权利要求2所述的基于高精度地图和场景检索的车道线辅助定位方法，其中，同一位点的场景图像包括在不同天气情况下以及不同交通状况下采集的多个场景图像。

4.如权利要求1所述的基于高精度地图和场景检索的车道线辅助定位方法，其中，步骤

4.3通过计算当前场景图像与候选场景图像的词袋向量的相似度，得到场景图像的相似度，词袋向量的相似度计算公式为公式1：公式1

其中，s(v0，v1)表示两个词袋向量v0和v1的相似度，假设v0和v1的维度是W，||v0-v1||1表示v0和v1的曼哈顿距离，即v0和v1每一项元素的差的总和。

5.如权利要求1所述的基于高精度地图和场景检索的车道线辅助定位方法，其中，步骤

4.2中排除所选范围内场景图像的航向角与当前场景图像的航向角差距大于5°的场景图像。

6.如权利要求1所述的基于高精度地图和场景检索的车道线辅助定位方法，其中，所述航向优化通过最小化正确的车道线与当前车道线的车道线夹角完成，所述车道线夹角计算公式为公式2：公式2

其中，Θ为车道线夹角；是正确车道线点的斜率； 为当前车道线的斜率；

在计算出正确车道线点的斜率和当前车道线的斜率后，使用Levenberg-Marquardt算法逐步优化车辆位姿中的航向角项，同时固定其他项，使Θ的值最小。

说明书

基于高精度地图和场景检索的车道线辅助定位方法

技术领域

[0001] 本发明涉及自动驾驶汽车定位技术领域，特别涉及一种基于高精度地图和场景检索的车道线辅助定位方法。

背景技术

[0002] 目前，在自动驾驶汽车定位技术领域，有两种最具有代表性的方案：1)使用低成本低精度的基于全球定位系统的组合导航模块(下文简称低精度GPS)；2)使用高成本高精度的基于全球定位系统的组合导航模块(下文简称高精度GPS)。方案一受GPS性能的制约，定位误差可达到几米到几十米，不能够满足自动驾驶的需求。方案二使用的高精度 GPS成本往往在几十万元，高昂的价格很难实现量产，更无法被用户和市场接受。此外，方案一和方案二都是根据GPS直接给出自身的定位，而忽略了引入外界物体作为定位的参照物，而预先测量好精确位置的参照物又恰恰是车辆定位重要的先验信息。

[0003] 目前，还出现一种基于高精度车道线地图的低精度GPS定位技术，基本工作流程是：首先，高精度车道线地图会接收到低精度GPS发送的车辆位置，然后高精度车道线地图会基于该位置发送出参考车道线；与此同时，自动驾驶汽车上的摄像头会自动检测出当前的车道线。最后，将检测的车道线和参考的高精度车道线地图的车道线进行匹配，只有最接近真实的位置和最接近真实的姿态才能使车道线匹配成功，所以车辆的位姿得到了优化。

[0004] 显而易见，基于高精度车道线地图的低精度GPS定位本身也存在一个问题，那就是高精度车道线地图会接收到一个不准确的位置并基于这个位置发送参考车道线，而该参考车道线很有可能已经偏离了正确的车道，从而使当前检测到的车道线参照了错误的车道线，导致定位信息依然不可靠。基于高精度车道线地图的高精度GPS定位可以完全避免这个问题，但是又会重新面临高成本难量产的弊端。

发明内容

[0005] 本发明的一个目的是解决至少上述问题，并提供至少后面将说明的优点。

[0006] 本发明还有一个目的是提供一种基于高精度地图和场景检索的车道线辅助定位方法，能够对自动驾驶汽车进行准确定位，并降低准确定位的成本。

[0007] 为了实现根据本发明的这些目的和其它优点，提供一种基于高精度地图和场景检索的车道线辅助定位方法，其中，主要包括以下步骤：

[0008] 步骤1、采用高精度GPS采集道路的车道线位置，建立高精度车道线地图；

[0009] 步骤2、采集高精度车道线地图中不同车道的不同位点的包含场景图像的场景数据，建立车道场景库，其中，将所述车道场景库中的场景图像作为参考图像，且所述参考图像中标记有参考车道序号；

[0010] 步骤3、车辆采集当前场景图像为当前图像，将当前图像与车道场景库中的参考图像比对，得到最接近的参考图像，并从所述最接近的参考图像中获取参考车道序列号；

[0011] 步骤4、车辆采用低精度GPS获得低精度位置信息，并将低精度位置信息及参考车道序列号发送至高精度GPS，高精度GPS结合低精度位置信息与参考车道序列号，给出车辆当前正确的车道线；

[0012] 步骤5、车辆检测当前车道线，并以正确的车道线为标准调整车辆当前的位姿，以使车辆与正确的车道线匹配。

[0013] 优选的是，步骤2中所述场景数据还包括：采集场景图像时车辆的位置、姿态和所在车道的序列号；采集场景图像时的天气情况和交通状况，以及场景图像的词袋向量。

[0014] 优选的是，同一位点的场景图像包括在不同天气情况下以及不同交通状况下采集的多个场景图像。

[0015] 优选的是，步骤3中，获取最接近的参考图像的具体方法主要包括以下步骤：

[0016] 步骤4.1、获得车辆当前场景图像，并利用低精度GPS获得当前位置；

[0017] 步骤4.2、查询车道场景库中在当前位置10-30米范围之内的场景图像，作为候选场景图像；

[0018] 步骤4.3、计算当前场景图像与候选场景图像的相似度，将候选场景图像中得分最高的作为参考场景图像，依照参考场景图像得到车辆当前的参考车道序列号。

[0019] 优选的是，步骤4.3通过计算当前场景图像与候选场景图像的词袋向量的相似度，得到场景图像的相似度，词袋向量的相似度计算公式为公式1：

[0020]

[0021] 其中，s(v0，v1)表示两个词袋向量v0和v1的相似度，假设v0和v1的维度是W，||v0- v1||1表示v0和v1的曼哈顿距离，即v0和v1每一项元素的差的总和。

[0022] 优选的是，步骤4.2中排除所选范围内场景图像的航向角与当前场景图像的航向角差距大于5°的场景图像。

[0023] 优选的是，步骤5中车辆位姿调整的方法为先进行车辆的航向优化，再进行车辆的横向位置优化。

[0024] 优选的是，所述航向优化通过最小化正确的车道线与当前车道线的车道线夹角完成，所述车道线夹角计算公式为公式2：

[0025]

[0026] 其中，Θ为车道线夹角；是正确车道线点的斜率； 为当前车道线的斜率；

[0027] 在计算出正确车道线点的斜率和当前车道线的斜率后，使用Levenberg-Marquardt算法逐步优化车辆位姿中的航向角项，同时固定其他项，使Θ的值最小。

[0028] 优选的是，所述横向位置优化方法为：使用Levenberg-Marquardt算法，逐步用Δt补偿车辆位姿中的横向位置，使得车辆坐标系下的正确的车道线和当前车道线的距离最小化，最后，把计算得到的Δt还原回世界坐标系下，得到车辆在优化后的横向位置；将当前车道线在车辆坐标系下平移一段距离Δt，与正确的车道线匹配，即完成横向位置优化，其中，Δt表示当前车道线的平移距离，用Δt补偿车辆位姿中的横向位置的计算公式为公式3：

[0029]

[0030] 其中，[0，1，0]T表示车辆坐标系的左向单位向量， 表示该单位向量在世界坐标系下的表示， 表示补偿的横向距离在世界坐标系下的表示。

[0031] 本发明至少包括以下有益效果：

[0032] 本发明通过采集高精度车道线地图中不同车道的不同位点的场景数据并建立车道场景库，将车辆拍摄的当前场景与车道场景库中的场景图像进行匹配，再将匹配得到的最接近的场景图像与高精度车道线地图结合，得到精确的车辆位置定位，解决了高精度车道线地图与低精度GPS结合易发送错误的参考车道线，以及高精度GPS使用成本高，低精度GPS难以实现精准定位等技术问题。该方案中高精度GPS仅仅用于采集车道线的标准位置和建立高精度车道线地图，作为共享资源在后台应用，而不必每辆车上都要安装高精度GPS，从而大大降低了高精度GPS的使用门槛和自动驾驶汽车的生产成本。

[0033] 本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。

附图说明

[0034] 图1为本发明所述的基于高精度地图和场景检索的车道线辅助定位方法的基本工作流程图；

[0035] 图2为本发明中车道判别流程图；

[0036] 图3为分步优化车辆位姿流程图。

具体实施方式

[0037] 下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

[0038] 应当理解，本文所使用的诸如“具有”、“包含”以及“包括”术语并不排除一个或多个其它元件或其组合的存在或添加。

[0039] 如图1-3所示，本发明提供一种基于高精度地图和场景检索的车道线辅助定位方法，其中，主要包括以下步骤：

[0040] 步骤1、采用高精度GPS采集道路的车道线位置，建立高精度车道线地图。

[0041] 步骤2、采集高精度车道线地图中不同车道的不同位点的包含场景图像的场景数据，建立车道场景库，其中，将所述车道场景库中的场景图像作为参考图像，且所述参考图像中标记有参考车道序号。

[0042] 步骤3、车辆采集当前场景图像为当前图像，将当前图像与车道场景库中的参考图像比对，得到最接近的参考图像，并从所述最接近的参考图像中获取参考车道序列号。

[0043] 步骤4、车辆采用低精度GPS获得低精度位置信息，并将低精度位置信息及参考车道序列号发送至高精度GPS，高精度GPS结合低精度位置信息与参考车道序列号，给出车辆当前正确的车道线。

[0044] 步骤5、车辆检测当前车道线，并以正确的车道线为标准调整车辆当前的位姿，以使车辆与正确的车道线匹配。

[0045] 该方案中，通过采集高精度车道线地图中不同车道的不同位点的场景数据并建立车道场景库，将车辆拍摄的当前场景与车道场景库中的场景图像进行匹配，再将匹配得到的最接近的场景图像与高精度车道线地图结合，得到精确的车辆位置定位，解决了高精度车道线地图与低精度GPS结合易发送错误的参考车道线，以及高精度GPS使用成本高，低精度GPS难以实现精准定位等技术问题。该方案中高精度GPS仅仅用于采集车道线的标准位置和建立高精度车道线地图，作为共享资源在后台应用，车辆上仅安装低精度 GPS就可满足车辆精确定位的要求，从而大大降低了高精度GPS的使用门槛和自动驾驶汽车的生产成本。图1为本发明所述的基于高精度地图和场景检索的车道线辅助定位方法的基本工作流程图，其中圆形中的内容表示预先建立好的数据库，椭圆形中的内容表示输入或者输出，矩形中的内容表示具体执行的算法步骤。

[0046] 一个优选方案中，步骤2中所述场景数据还包括：采集场景图像时车辆的位置、姿态和所在车道的序列号；采集场景图像时的天气情况和交通状况，以及场景图像的词袋向量。

[0047] 以上方案中，为了更加精确的判别车辆所在的车道，在自动驾驶汽车运行的路段，需要预先采集一系列的车道数据，并建立一个车道场景库。一组完整的车道数据包括：车道的图像(场景图像)，采集该场景图像时车辆的位置、姿态和所在车道的序列号，以及采集该图像时的天气情况(包括不同季节、不同天气状况等)和交通状况(车流量高峰期及车流量较小的情况等)，和车道图像的词袋向量。车道场景库便是由无数组这样的数据构成的。

[0048] 车道场景库中的数据务必要精确和全面，要满足精确性的要求。采集车道数据时，要使用高精度的GPS和高分辨率、自适应曝光度的摄像头。全面性的第一层含义是：采集的图像的区域要涵盖自动驾驶汽车在运行的路段所有可能行驶到的区域，并且采集图像时车辆的行驶速度越低，采集到的图像越多。全面性另一层含义是：对于同一个位置，在不同的天气情况和不同的交通状况下，图像的内容是不尽相同的，所以为了避免查询图像时把同一位置的图像仅仅因为场景发生了变化而误判为不相似的图像这种情况，采集图像涉及的场景越多越好。

[0049] 关于词袋向量是为了便于储存和检索的一种场景图像的处理途径，如果把图像直接存储在车道场景库中，会消耗大量的内存，所以我们要用词袋向量去表示一张图。词袋是基于“图像上出现了哪些词汇”的思想来描述一幅图像，词袋一般用一个稀疏向量表示。向量的维度即为词汇表中词汇的总量，向量中的每一个索引即代表词汇表中的每一个单词，该索引对应的值即为该词汇在图像上出现的频率。每张图像不可能包含词汇表中所有词汇，大部分索引对应的值为0，所以词袋向量是一个稀疏向量。词袋向量的稀疏性不仅大大降低了存储所需要的内存，而且词袋向量本身也提供了一种度量图像相似度的简单有效的方法。

[0050] 一个优选方案中，同一位点的场景图像包括在不同天气情况下以及不同交通状况下采集的多个场景图像。

[0051] 一个优选方案中，步骤3中，获取最接近的参考图像的具体方法主要包括以下步骤：

[0052] 步骤4.1、获得车辆当前场景图像，并利用低精度GPS获得当前位置。

[0053] 步骤4.2、查询车道场景库中在当前位置10-30米范围之内的场景图像，作为候选场景图像。

[0054] 步骤4.3、计算当前场景图像与候选场景图像的相似度，将候选场景图像中得分最高的作为参考场景图像，依照参考场景图像得到车辆当前的参考车道序列号。

[0055] 以上方案中，首先，需要获取低精度GPS输出的位置，误差约为5米，所以查询车道场景库中所有位置在当前位置10-30米范围之内的图像(一般为20米)，先通过低精度GPS定位再进行查询的方法，能够缩小查询范围，便于快速查询并提高查询的准确率。

[0056] 一个优选方案中，步骤4.3通过计算当前场景图像与候选场景图像的词袋向量的相似度，得到场景图像的相似度，词袋向量的相似度计算公式为公式1：

[0057]

[0058] 其中，s(v0，v1)表示两个词袋向量v0和v1的相似度，假设v0和v1的维度是W，||v0- v1||1表示v0和v1的曼哈顿距离，即v0和v1每一项元素的差的总和。

[0059] 以上方案中，需要计算当前图像对应的词袋向量，并评估当前场景图像的词袋向量和每一个候选车道场景的词袋向量的相似度。把每一条候选车道场景的相似度评分，累加到该候选车道对应的车道序列号，统计完毕后，对每条车道的总分取平均值，平均分最高的车道，被判别为当前车辆所在的车道。图2为车道判别流程图。

[0060] 一个优选方案中，步骤4.2中排除所选范围内场景图像的航向角与当前场景图像的航向角差距大于5°的场景图像。

[0061] 以上方案中，如果该图像对应的航向角与当前的航向角差距较大，则排除该图像，因为该图像很有可能是对面车道的图像，没有参考意义。剩余的所有查询到的图像，均被列为候选车道场景。

[0062] 一个优选方案中，步骤5中车辆位姿调整的方法为先进行车辆的航向优化，再进行车辆的横向位置优化。

[0063] 以上方案中，车辆位姿是指自动驾驶汽车的GPS组合导航模块输出的车辆的姿态和位置信息，位姿优化的基本思想是：车辆的位置和姿态越接近真实值，检测到的车道线和地图车道线匹配重合度越高。研究结果表明，同时优化车辆的位置和姿态不能达到很好的效果。所以，一个改进的方案是分步优化车辆的位姿，即先优化车辆的航向(姿态)，再优化车辆的横向位置(位置)。分步优化车辆位姿的流程如图3所示，矩形的方块1表示车辆的位姿，实线条的车道线2表示检测的车道线，虚线条的车道线3表示地图车道线在车辆坐标系下的投影，双线条的车道线4表示检测的车道线和地图车道线重合，即检测的车道线与车辆坐标系下的地图车道线匹配成功；最左边是车辆位姿未经过优化的状态，中间是航向角优化后的结果，最右边是横向位置优化后的结果。

[0064] 补充介绍几个基本概念：

[0065] 1)世界坐标系和车辆坐标系

[0066] 世界坐标系是一个以地球的地心为原点的坐标系，其中，z轴从地心指向北极点，x 轴从地心指向赤道与0°子午线的交点，y轴从地心指向赤道与90°东经子午线的交点。世界坐标系一般用world表示，简写为w。

[0067] 车辆坐标系是一个以车辆内的某一点为原点的坐标系，前向为x轴方向，左向为y 轴方向，上方为z轴方向。车辆坐标系一般用body表示，简写为b。

[0068] 2)GPS组合导航模块

[0069] GPS组合导航模块输出的信息是车辆的姿态和位置。车辆的姿态即车辆坐标系的坐标轴相对世界坐标系的坐标轴的旋转角度，一般写为 车辆的位置即车辆坐标系原点相对世界坐标系的原点的距离，一般写为 车辆的位置和姿态可以统一表示为可以将世界坐标系下的任何一个点转换到车辆坐标系下。同时， 也是主要要优化的内容。

[0070] 3)车道线的表示

[0071] 高精度地图中的车道线是离散的，是由一系列车道线点的集合构成。在高精度地图中，车道线点的坐标是在世界坐标系下表示的，一个在世界坐标系下的车道线点可以表示为 位姿优化是在车辆坐标系下进行的，所以需要将地图车道线点需要转换到车辆坐标系：

[0072] 检测到的车道线是连续的，并且每一条检测到的车道线是由一条四次曲线拟合成的，该曲线是在车辆坐标系下表示的，可以写做： 其中，(c0，c1，c2，c3用于表示拟合出的四次曲线的系数) 表示检测到的车道线点在车辆坐标系下的坐标。

[0073] 一个优选方案中，所述航向优化通过最小化正确的车道线与当前车道线的车道线夹角完成，所述车道线夹角计算公式为公式2：

[0074]

[0075] 其中，Θ为车道线夹角；是正确车道线点的斜率；为当前车道线的斜率；

[0076] 在计算出正确车道线点的斜率和当前车道线的斜率后，使用Levenberg-Marquardt算法(1.Levenberg,Kenneth(1944)“. A Method for the Solution ofCertain Non-Linear Problems in Least Squares”；2.Marquardt,Donald(1963)“.An Algorithm for Least-Squares Estimation of Nonlinear Parameters”)逐步优化车辆位姿中的航向角项，同时固定其他项，使Θ的值最小。

[0077] 以上方案中，是当前地图车道线点 的斜率：和 是 在地图上前后相邻的两个车道
线点。 是由检测的车道线曲线在 处的导数得出：  (其
中，c3，c2，c1表示拟合出的四次曲线的系数)

[0078] 一个优选方案中，所述横向位置优化方法为：使用Levenberg-Marquardt算法，逐步用Δt补偿车辆位姿中的横向位置，使得车辆坐标系下的正确的车道线和当前车道线的距离最小化，最后，把计算得到的Δt还原回世界坐标系下，得到车辆在优化后的横向位置；将当前车道线在车辆坐标系下平移一段距离Δt，与正确的车道线匹配，即完成横向位置优化，其中，Δt表示当前车道线的平移距离，用Δt补偿车辆位姿中的横向位置的计算公式为公式3：

[0079]

[0080] 其中，[0,1,0]T表示车辆坐标系的左向单位向量， 表示该单位向量在世界坐标系下的表示， 表示补偿的横向距离在世界坐标系下的表示，补偿后车辆在世界中的位置即为 是高精度地图提供的车道线点位置在世界坐标系下的位置， 是把该车道线点投影到车辆坐标系下后相对于车辆的位置。所以，在车辆前 位置处检测到的的车道线点相对于车辆的横向位置为不断左移或右移Δt，使检测到的车道线和地图车
道线在车辆坐标系下的投影在横向上趋于重合，即 其中，
表示将 视为关于Δt的变量，即公式3所表述的形式。

[0081] 本发明所述方案中，自动驾驶车辆不再需要安装高精度的全球定位系统(高精度 GPS)，从而大大降低了自动驾驶汽车的生产成本，使自动驾驶汽车的商业推广成为了可能。借助完善的高精度车道线地图和车道场景库信息，自动驾驶车辆本身不再需要复杂的维护和升级，从而使用户可以得到更好的用户体验。

[0082] 尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用，它完全可以被适用于各种适合本发明的领域，对于熟悉本领域的人员而言，可容易地实现另外的修改，因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。