基于网联的无人驾驶测试环境地图搭建方法及系统

摘要

本发明提供了一种基于网联的无人驾驶测试环境地图搭建系统及方法，包括：通过V2X通信技术，实现无人驾驶测试环境中的车‑车、车‑人、车‑路、车‑网的信息交换，获得超视距环境信息；对超视距环境信息进行预处理，得到预处理后的超视距环境信息；基于预处理后的超视距环境信息利用OBR算法提取和匹配ORB特征点，并进行镜像相机位姿估计；基于扩展卡尔曼滤波算法对估计的镜像相机位姿进行优化处理，得到全局一致的相机轨迹和全局地图；基于V2X通信技术采集的环境信息构建词袋模型；利用词袋模型通过优化消除累积误差修正全局一致的相机轨迹与相机位姿，减少建图误差，确保全局地图准确性；基于修正后的轨迹与相机位姿，建立无人驾驶测试环境地图。

说明书

技术领域

本发明涉及高精度地图构建的技术领域，具体地，涉及基于网联的无人驾驶测试环境地图搭建方法及系统，更为具体地，涉及基于网联的无人驾驶测试环境高精度地图构架及搭建方法。

背景技术

汽车的发明给社会带来了诸多便捷和效率，汽车工业的发展也进一步促进了

经济的发展与人类的创新。而交通安全事故却时有发生，很难完全避免，对我们的人身安全构成了严重威胁。一方面由于自然环境的恶化，雾、霾、雨、雪等恶劣天气经常导致能见度大幅度降低，严重影响汽车的安全行驶；另一方面高速道路上的长时间驾驶会造成司机驾驶疲劳，注意力不集中和判断能力下降导致发生道路交通事故。人们对于生活品质的不断需求使得汽车的年产量和保有量不断增加，安全性能更高、乘坐更加舒适、驾驶更加便捷,已成为人们对汽车的普遍需求。互联网和人工智能技术的高速发展给汽车带来了革命性的变化，新能源汽车和自动驾驶汽车因此成为其中最热门、最典型的应用。受基础科学的发展限制，自动驾驶比新能源汽车更可能在短期内走入大众生活。

搭载了激光雷达、相机、毫米波雷达、GPS等设备的无人车不需要人为干预就能对环境进行感知，从复杂的交通流中提取导航信息，并以先进的决策和控制系统计算并执行适当的指令。无人车的研究在辅助人们驾驶方面具有得天独厚的优势，能有效降低交通事故的发生率。无人车能够感知车辆周围的环境，它能准确识别路口的交通信号灯，根据无人车的智能决策系统，遇到红灯时如果司机没有及时做出刹车反应，无人车能自主做出刹车指令，这样就能避免司机未及时刹车而闯红灯的行为；无人车也能够感知到周围的车辆和行人等障碍物，快速识别判断障碍物，比人类更快速的做出避障决策。能见度极低的恶劣天气下，人眼能见范围降低，但是无人车上的激光雷达不受天气的影响，在恶劣天气中依然能准确感知周围的环境信息，极大增强了人和车辆行驶的安全性，交通事故的减少对保障人们的生命财产安全具有重大意义。

自主驾驶系统主要由感知、规划、控制三大模块构成，其中感知模块是其实现自主的基础，而精准复杂的感知系统离不开高精度地图的构建与精确的定位定姿。

高精度地图是一种具有厘米级精度、包含路网结构信息的特殊地图，主要应用于高精定位、环境感知、路径规划以及仿真实验，其中高精定位是将车辆在线获取的环境特征与事先建好的高精度地图进行复杂的特征匹配，来获取当前车辆在地图上的确切位置；自动驾驶车辆通过摄像头、激光雷达等传感器进行环境感知的能力有限，高精度地图上的三维语义信息如红绿灯、交通标志牌等可以缩小目标的检测范围、降低感知难度；另外，高精度地图上的路网信息可以在始发地与目的地之间规划出厘米级的最优路径。和传统导航地图相比，高精度地图拥有更为精细的环境描述和度量，方便计算机实时解析地图数据库并与传感器信息关联。

当前面向无人驾驶的高精度建图的主流方法大致分为以下几个部分：数据采集，数据处理，对象检测，手动验证以及地图发布。对高精度建图效果及精度影响最大的一个步骤就是数据采集部分，换句话说，数据采集的结果很大程度上决定了建图效果。

当前全球范围内的该领域专家学者和无人汽车厂商采用的主流传感器为激光雷达，但是激光雷达的成本较高，但也不乏部分学者采用相机，比如单目相机，双目相机，RGBD相机等。通过这些传感器往往能获得相对丰富的环境信息，面向无人驾驶的高精度建图也取得了可观的效果。但是这些单一传感器有一个共同的缺陷，即它们所检测到的范围是有限的，摄像头根据不同的俯角看到的距离约为150-60m不等，激光雷达所看到的范围大约40-80m范围。而当无人驾驶汽车达到一定的行驶速度时，面对复杂多变以及可能出现弯折的道路环境时，单一车载传感器就难以保证获得充足的环境信息来保证良好的高精度建图效果。于是，本文提出一种基于网联的面向无人驾驶的高精度地图的构建方法。旨在解决单一位置传感器无法获得超视距环境信息的问题。

C-V2X技术是融合了汽车与信息技术2大产业，成为了新一轮科技革命和产业变革背景下的一种新生技术。V2X通信是车联网实现车路协同的重要关键技术，已成为当前世界各国解决道路安全问题的一个研究热点。V2X技术借助车与人之间V2P(Vehicle toPedestrian)，车与车之间V2V(Vehicle to Vehicle)，车与路之间V2I(Vehicle toInfrastructure)，车与网络之间V2N(Vehicle to Network)的无线通信，及时感知车辆周边的环境状况。如图2所示：

C-V2X针对于车联网应用场景，定义了2种通信方式，广域蜂窝式与短程直通式。广域蜂窝式采用终端和基站之间的网络通信接口(Uu)，可实现长距离和更大范围的可靠通信。另一种短程直通式采用的是车辆之间的短距离直接通信接口(PC5)。如图3所示：

SALM(同步定位与地图构建Simultaneous Localization And Mapping),最早由Hugh Durrant-Whyte和John J.Leonard提出。SLAM主要用于解决移动机器人在未知环境中运行时定位导航与地图构建的问题。框架如图4所示。

专利文献CN109920246A(申请号：201910133334.9)公开了一种基于V2X通信与双目视觉的协同局部路径规划方法。OBU通过双目前置摄像头、OBD设备和V2X通信获取车身信息和环境信息并传输给协同局部路径规划控制器；然后通过交通对象混合态势估计模型对交通对象状态进行状态预测，再通过多源数据融合算法将态势估计后的交通对象信息与交通规则进行时空数据融合，然后将融合后的交通数据通过坐标映射模型构建具有高可信度的交通路况地图，再结合自车车辆信息通过自车态势估计模型计算自车态势，然后通过改进局部路径规划算法计算出当前车辆最优平滑行驶路径；最后将该路径传输给OBU，并通过V2X广播给其他车辆。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于网联的无人驾驶测试环境地图搭建系统及方法。

根据本发明提供的一种基于网联的无人驾驶测试环境地图搭建系统，包括：

模块M1：通过V2X通信技术，实现无人驾驶测试环境中的车-车、车-人、车-路、车-网的信息交换，获得超视距环境信息；

模块M2：对获得的超视距环境信息进行预处理，得到预处理后的超视距环境信息；

模块M3：基于预处理后的超视距环境信息利用OBR算法提取和匹配ORB特征点，基于匹配后的OBR特征点进行镜像相机位姿估计；

模块M4：基于扩展卡尔曼滤波算法对估计的镜像相机位姿进行优化处理，得到全局一致的相机轨迹和全局地图；

模块M5：基于V2X通信技术采集的环境信息构建词袋模型；

模块M6：利用词袋模型通过优化消除累积误差修正全局一致的相机轨迹与相机位姿，减少建图误差，确保全局地图准确性；

模块M7：基于修正后的轨迹与相机位姿，建立无人驾驶测试环境地图。

优选地，所述模块M2中预处理包括对超视距环境信息去除冗余信息和噪声信息。

优选地，所述模块M3包括：

模块M3.1：基于预处理后的超视距环境信息利用OBR算法检测出OBR特征点；

模块M3.2：基于检测出的OBR特征点利用OBR算法计算出特征点描述子；

模块M3.3：通过汉明距离计算两个OBR特征点的描述子的相似度；当两个OBR特征点的描述子的相似度大于预设阈值时，则当前两个OBR特征点匹配成功；当两个OBR特征点的描述子的相似度小于等于预设阈值时，则当前两个OBR特征点未匹配成功；

模块M3.4：基于匹配后的OBR特征点利用ICP算法通过线性代数或非线性代数估计镜像相机位姿。

优选地，所述模块M4包括：基于扩展卡尔曼滤波算法，根据前一时刻的相机位姿估计值推断当前时刻的SLAM过程的运行方程和观测方程，计算卡尔曼增益，根据卡尔曼增益更新当前相机位姿，计算后验概率分布进入下一次迭代优化，重复触发执行，直至得到全局一致的相机轨迹和全局地图。

优选地，所述模块M6包括：将得到的全局一致的相机轨迹和全局地图基于词袋模型，用词袋模型中的特征元素描述当前位置采集的图像，计算在当前位置和曾经在相同位置采集的两个图像的相似度，当相似度超过阈值时，则认为出现回环，则把当前的位姿估计恢复至原位置。

根据本发明提供的一种基于网联的无人驾驶测试环境地图搭建方法，包括：

步骤S1：通过V2X通信技术，实现无人驾驶测试环境中的车-车、车-人、车-路、车-网的信息交换，获得超视距环境信息；

步骤S2：对获得的超视距环境信息进行预处理，得到预处理后的超视距环境信息；

步骤S3：基于预处理后的超视距环境信息利用OBR算法提取和匹配ORB特征点，基于匹配后的OBR特征点进行镜像相机位姿估计；

步骤S4：基于扩展卡尔曼滤波算法对估计的镜像相机位姿进行优化处理，得到全局一致的相机轨迹和全局地图；

步骤S5：基于V2X通信技术采集的环境信息构建词袋模型；

步骤S6：利用词袋模型通过优化消除累积误差修正全局一致的相机轨迹与相机位姿，减少建图误差，确保全局地图准确性；

步骤S7：基于修正后的轨迹与相机位姿，建立无人驾驶测试环境地图。

优选地，所述步骤S2中预处理包括对超视距环境信息去除冗余信息和噪声信息。

优选地，所述步骤S3包括：

步骤S3.1：基于预处理后的超视距环境信息利用OBR算法检测出OBR特征点；

步骤S3.2：基于检测出的OBR特征点利用OBR算法计算出特征点描述子；

步骤S3.3：通过汉明距离计算两个OBR特征点的描述子的相似度；当两个OBR特征点的描述子的相似度大于预设阈值时，则当前两个OBR特征点匹配成功；当两个OBR特征点的描述子的相似度小于等于预设阈值时，则当前两个OBR特征点未匹配成功；

步骤S3.4：基于匹配后的OBR特征点利用ICP算法通过线性代数或非线性代数估计镜像相机位姿。

优选地，所述步骤S4包括：基于扩展卡尔曼滤波算法，根据前一时刻的相机位姿估计值推断当前时刻的SLAM过程的运行方程和观测方程，计算卡尔曼增益，根据卡尔曼增益更新当前相机位姿，计算后验概率分布进入下一次迭代优化，重复执行，直至得到全局一致的相机轨迹和全局地图。

优选地，所述步骤S6包括：将得到的全局一致的相机轨迹和全局地图基于词袋模型，用词袋模型中的特征元素描述当前位置采集的图像，计算在当前位置和曾经在相同位置采集的两个图像的相似度，当相似度超过阈值时，则认为出现回环，则把当前的位姿估计恢复至原位置。

与现有技术相比，本发明具有如下的有益效果：

1、本发明基于车联网构建面向无人驾驶的高精度地图，将车联网技术引入数据采集过程，通过V2X通信技术，进行无人驾驶测试环境中的车-车，车-人，车-路，车-网的信息交换，摆脱传统构建高精度地图单一位置传感器的约束，可以获得超视距的环境信息；

2、通过V2X通信技术使得用于建图的数据更加丰富，可以获得好的建图效果；

3、传统高精度建图所使用的一些传感器采集图像是有频率限制的，如：激光雷达的频率通常设为10Hz，意味着激光雷达扫描一周花费0.1s,如果此时达到一定车速，比如80km/h,那么激光雷达扫描一周，车就已经行驶了2.2m了，意味着有很多环境信息会被忽略，甚至包含着一些潜在的危险，而基于车联网技术，可以完美解决这个问题，车周围环境在车联网下，车能够完全感知到；

4、本发明基于特征点法，建立前端视觉里程计模块，用ORB算法检测ORB特征点，提取特征；相机位姿估计用ICP匹配算法来求解，SVD方法求解位姿，非线性优化方法寻找最优值。ORB算法同时兼顾性能和精度。ICP算法采用用迭代的方法，不断细化两次扫描之间的位姿矩阵，求出使得两帧雷达数据之间所有对应点距离之和最小位姿矩阵，很好地实现估计相机位姿功能；

5、本发明基于扩展卡尔曼滤波算法，进行后端优化，根据输入信息推断当前时刻的状态分布，计算卡尔曼增益，根据卡尔曼增益更新当前数据，计算后验概率分布为下一次迭代做准备。根据扩展卡尔曼滤波，进一步对全局进行优化处理，得到全局一致的相机轨迹和全局地图；

6、本发明基于词袋算法(BOW),进行回环检测，如果测试无人驾驶汽车回到了以前经过的地方，但因为误差积累的存在，会造成相机位姿轨迹漂移的问题，即视觉SLAM系统估计出的位置与该位置并不重合。回环检测的目的就是通过优化消除这样的累积误差，通过把位置信息作为额外约束提供给后端进行优化，正确的检测信息能够帮助系统修正轨迹与相机位姿，减少建图误差，从而保证全局地图的准确性。

7、本发明能够在无人驾驶测试环境内很好地描述三维地图，将车联网应用到信息读取环节，实现了超视距环境信息的读取，使得采集来的信息更加丰富，使得建图效果更佳。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为基于网联的无人驾驶测试环境地图搭建系统示意图；

图2为本发明提供的V2X示意图；

图3为本发明提供的V2X通信的两种信号接口示意图；

图4为本发明提供的视觉SLAM基本框架示意图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

根据本发明提供的一种基于网联的无人驾驶测试环境地图搭建系统，如图1至4所示，包括：

基于网联的信息采集模块：通过V2X通信技术，实现无人驾驶测试环境中的车-车、车-人、车-路、车-网的信息交换，获得超视距环境信息；

预处理模块：对获得的超视距环境信息进行预处理，得到预处理后的超视距环境信息；

前端视觉里程计模块：基于预处理后的超视距环境信息利用OBR算法提取和匹配ORB特征点，基于匹配后的OBR特征点进行镜像相机位姿估计；

后端优化模块：基于扩展卡尔曼滤波算法对估计的镜像相机位姿进行优化处理，得到全局一致的相机轨迹和全局地图；

回环检测模块：基于V2X通信技术采集的环境信息构建词袋模型，利用词袋模型通过优化消除累积误差，把当前的位置信息作为额外约束提供给后端进行优化，无人车曾经经过的位置帮助修正全局一致的相机轨迹与相机位姿，减少建图误差，确保全局地图准确性；

构建地图模块：基于修正后的轨迹与相机位姿，建立无人驾驶测试环境地图。

具体地，所述基于网联的信息采集模块包括：通过前景广阔的V2X通信技术，实现无人驾驶测试环境中的车与车，车与基础设施，车与互联网的信息交流，当测试道路上具备V2X通信能力的车、基础设施等足够多时，面向无人驾驶的高精度建图的信息采集模块就摆脱了单一位置采集数据的约束，每辆车都成了分布式自动驾驶网络的传感器，网络服务器或者其他传感器(比如治安摄像头)感知到道路状况，为实时绘制和修正高精度地图提供丰富的道路信息。

具体地，所述预处理模块中预处理包括对超视距环境信息去除冗余信息和噪声信息。

具体地，所述前端视觉里程计模块包括：

前端视觉里程计模块1：基于预处理后的超视距环境信息利用OBR算法检测出OBR特征点；

前端视觉里程计模块2：基于检测出的OBR特征点利用OBR算法计算出特征点描述子；

前端视觉里程计模块3：通过汉明距离计算两个OBR特征点的描述子的相似度；当两个OBR特征点的描述子的相似度大于预设阈值时，则当前两个OBR特征点匹配成功；当两个OBR特征点的描述子的相似度小于等于预设阈值时，则当前两个OBR特征点未匹配成功；

前端视觉里程计模块4：基于匹配后的OBR特征点利用ICP(iterative closestpoint迭代最近点)算法通过线性代数的求解(SVD)或非线性代数估计镜像相机位姿。

具体地，所述后端优化模块包括：基于扩展卡尔曼滤波算法，根据前一时刻的相机位姿估计值推断当前时刻的SLAM过程的运行方程和观测方程，计算卡尔曼增益，根据卡尔曼增益更新当前相机位姿，计算后验概率分布进入下一次迭代优化，重复触发执行，直至得到全局一致的相机轨迹和全局地图。

扩展卡尔曼滤波算法假设系统符合马尔可夫性，将整个非线性系统分为线性的预测过程和观测过程。其中预测过程是将当前位姿状态与当前控制信息作为输入，得到下一时刻的位姿状态。而观测过程是将当前位姿状态与当前观测信息作为输入，输出当前状态。

具体地，所述回环检测模块包括：将得到的全局一致的相机轨迹和全局地图基于词袋模型，用词袋模型中的特征元素描述当前位置采集的图像，计算在当前位置和曾经在相同位置采集的两个图像的相似度，当相似度超过阈值时，则认为出现回环，则把当前的位姿估计恢复至原位置。根据传感器数据采集环境建立词袋模型。

根据本发明提供的一种基于网联的无人驾驶测试环境地图搭建方法，包括：

基于网联的信息采集步骤：通过V2X通信技术，实现无人驾驶测试环境中的车-车、车-人、车-路、车-网的信息交换，获得超视距环境信息；

预处理步骤：对获得的超视距环境信息进行预处理，得到预处理后的超视距环境信息；

前端视觉里程计步骤：基于预处理后的超视距环境信息利用OBR算法提取和匹配ORB特征点，基于匹配后的OBR特征点进行镜像相机位姿估计；

后端优化步骤：基于扩展卡尔曼滤波算法对估计的镜像相机位姿进行优化处理，得到全局一致的相机轨迹和全局地图；

回环检测步骤：基于V2X通信技术采集的环境信息构建词袋模型，利用词袋模型通过优化消除累积误差，把当前的位置信息作为额外约束提供给后端进行优化，无人车曾经经过的位置帮助修正全局一致的相机轨迹与相机位姿，减少建图误差，确保全局地图准确性；

构建地图步骤：基于修正后的轨迹与相机位姿，建立无人驾驶测试环境地图。

具体地，所述基于网联的信息采集步骤包括：通过前景广阔的V2X通信技术，实现无人驾驶测试环境中的车与车，车与基础设施，车与互联网的信息交流，当测试道路上具备V2X通信能力的车、基础设施等足够多时，面向无人驾驶的高精度建图的信息采集步骤就摆脱了单一位置采集数据的约束，每辆车都成了分布式自动驾驶网络的传感器，网络服务器或者其他传感器(比如治安摄像头)感知到道路状况，为实时绘制和修正高精度地图提供丰富的道路信息。

具体地，所述预处理步骤中预处理包括对超视距环境信息去除冗余信息和噪声信息。

具体地，所述前端视觉里程计步骤包括：

前端视觉里程计步骤1：基于预处理后的超视距环境信息利用OBR算法检测出OBR特征点；

前端视觉里程计步骤2：基于检测出的OBR特征点利用OBR算法计算出特征点描述子；

前端视觉里程计步骤3：通过汉明距离计算两个OBR特征点的描述子的相似度；当两个OBR特征点的描述子的相似度大于预设阈值时，则当前两个OBR特征点匹配成功；当两个OBR特征点的描述子的相似度小于等于预设阈值时，则当前两个OBR特征点未匹配成功；

前端视觉里程计步骤4：基于匹配后的OBR特征点利用ICP(iterative closestpoint迭代最近点)算法通过线性代数的求解(SVD)或非线性代数估计镜像相机位姿。

具体地，所述后端优化步骤包括：基于扩展卡尔曼滤波算法，根据前一时刻的相机位姿估计值推断当前时刻的SLAM过程的运行方程和观测方程，计算卡尔曼增益，根据卡尔曼增益更新当前相机位姿，计算后验概率分布进入下一次迭代优化，重复执行，直至得到全局一致的相机轨迹和全局地图。

扩展卡尔曼滤波算法假设系统符合马尔可夫性，将整个非线性系统分为线性的预测过程和观测过程。其中预测过程是将当前位姿状态与当前控制信息作为输入，得到下一时刻的位姿状态。而观测过程是将当前位姿状态与当前观测信息作为输入，输出当前状态。

具体地，所述回环检测步骤包括：将得到的全局一致的相机轨迹和全局地图基于词袋模型，用词袋模型中的特征元素描述当前位置采集的图像，计算在当前位置和曾经在相同位置采集的两个图像的相似度，当相似度超过阈值时，则认为出现回环，则把当前的位姿估计恢复至原位置。基于网联的信息采集步骤采集环境信息建立词袋模型。

实施例2是实施例1的变化例

本发明是一种新型的基于车联网的面向无人驾驶测试环境的高精度地图架构及搭建方法，涉及到无人驾驶领域，更为具体的说是涉及到无人驾驶的高精度建图领域。

高精度地图对无人驾驶有着至关重要的作用，主要为无人驾驶汽车提供精准的定位，辅助环境感知，同时提供自动驾驶的安全性，而这些普通的地图(如GPS)无法提供。这种新型的基于车联网的面向无人驾驶测试环境的高精度地图架构及搭建方法主要基于视觉slam算法，将车联网引入到环境信息采集过程，具体地，通过V2X通信技术，实现无人驾驶测试环境中的车-车，车-人，车-路，车-网的信息交换，最终获得超视距环境信息。采用基于特征点的方法，检测ORB特征点，提取特征并匹配特征点，进行相机位姿估计。采用扩展卡尔曼滤波(EKF)算法，进一步对全局进行优化处理，得到全局一致的相机轨迹和全局地图。采用词袋模型，通过优化，消除累计误差，把位置信息作为额外约束提供给后端进行优化，正确的检测信息帮助系统修正轨迹与相机位姿，减少建图误差，确保全局地图准确性。本发明能够在无人驾驶测试环境内很好地描述三维地图。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。