基于路侧相机与车载GNSS/INS的车路协同定位导航方法及系统

本发明涉及自动驾驶领域中车辆的定位与导航关键技术，具体涉及一种基于路侧相机与车载gnss/ins的车路协同定位导航方法及系统。

背景技术：

1、随着自动驾驶、高精地图等技术的快速发展与落地应用，车辆的定位与导航相应地向着高精度（车道级）、高可靠性（多场景）方向发展。现有较为先进的车辆定位导航方法是利用车载的多源传感器（相机/gnss/ins等）进行组合导航。尽管这种方法综合利用了各类传感器的互补特性，在一定程度上提高了定位导航的可靠性，但它要求单车配置多型传感器、算力更强的计算单元以及经历更长的标定工序与时耗，最终可见地推高了单车成本。更重要的是，从宏观原理上看，基于车载单元（车载gnss/ins）的定位导航仅仅是利用了单车智能和单域信源，其精度和可靠性容易达到上限。

2、近年来，智能网联汽车的概念一经提出便被接纳并付诸实践，车路协同成为各项相关技术的主流实现模式。车路协同模式汇集了车端和路侧的多域信源，综合利用了单车智能和网联智能，所含潜能远超单车模式。受限于体积和成本，车载传感器的定位和探测精度不高，探测视野和距离小，易受环境干扰（城市峡谷、隧道、强磁场），单车所获的信源在精度和可靠性上并不甚理想。而安装于路侧的传感器（相机/雷达）则采用了一种位置固定的鸟瞰模式，具有足够高的探测高度和足够宽广的视野，能够更稳定地采集到精度更高、更加完整的场景数据，即实现超视距、无盲区的感知。如通过车-路通信，将路侧域和车端域的信源进行关联和融合，可持续获取自车位置以及周边交通态势（行人他车状态、公路设施、交通信号），进而实现足够提前的避障预警以及精确至车道级的路径规划。显然，这种车路协同的定位导航能够大幅提高车辆导航定位的精度和可靠性。

3、相较于其他传感器，相机具有采样频率高、信息丰富、功耗低、易部署等诸多优势，且在道路交通监控中已有广泛部署和实际应用。改造和升级已部署的路侧相机、增补必要点位的路侧相机，可在投入有限的前提下构建一个路侧视觉感知网。通过与该感知网的信息交互，车辆能够实现车路协同的高精度、高可靠性定位导航。

技术实现思路

1、发明针对现有技术的不足，提供一种基于路侧相机与车载gnss/ins的车路协同定位导航方法及系统，用以解决或者部分解决现有导航方法中存在的精度和可靠性不高的技术问题，并提高车辆对周围动态交通目标感知测量的精度，以提高车辆路径规划准确性和精细度。

2、为了实现上述目的，本发明提供一种基于路侧相机与车载gnss/ins的车路协同定位导航方法，包括以下步骤：

3、步骤1，当gnss信号可用时，使用卡尔曼滤波算法，将车载gnss和ins原始观测值采用紧组合的方式进行解算，得到车辆定位结果；

4、步骤2，当gnss信号不可用时，使用ins原始观测值，根据导航微分方程计算车辆的位置、速度和姿态信息；

5、步骤3，利用路侧单元对车辆及周围交通目标进行拍摄、识别和定位，获取全局交通态势及车辆的三方定位信息；

6、步骤3.1，通过路侧安装的gnss天线和gnss接收板获取pps脉冲信号，并传入到时间同步模块进行倍频处理，然后传输给工业相机作为其触发信号进行拍照；

7、步骤3.2，通过安装在路侧的工业相机拍摄获取车辆及其周围区域的图像，经过筛选和图像调光、调色等预处理后，利用神经网络识别图像中的车辆，得到车辆的关键点图像坐标和2d物体框；

8、步骤3.3，建立常见车型的3d车辆cad模型数据库，通过神经网络学习3d模型在2d空间中的映射，将步骤3.2识别到的2d关键点与数据库中3d车辆cad模型的2d映射进行匹配，选择匹配误差最小的模型作为3d物体检测的输出，并采用该3d cad物体模型进行深度估计，根据获取的车辆关键点2d-3d坐标对和相机内外参数，基于深度学习的单相机单视图三维定位技术，计算车辆及其周围交通目标在世界坐标系下的坐标；

9、步骤3.4，通过通信模块将解算的车辆及其周围交通目标的定位信息发送给车辆；

10、步骤4，将步骤2使用ins获取的车辆定位信息与步骤3使用路侧单元获取的车辆及周围交通目标定位信息融合，进行组合导航滤波解算，得到gnss信号不可用时车辆的最终定位结果；

11、步骤4.1，完成路侧相机和车载传感器的时间同步；

12、步骤4.2，利用卡尔曼滤波器，对ins获取的车辆定位信息和路侧单元获取的车辆及其周围交通目标定位信息进行组合导航滤波解算；

13、步骤5，根据步骤1或步骤4解算得到的车辆定位结果和步骤3路侧单元获取的全局交通态势进行车辆路径规划；

14、步骤5.1，根据步骤1或4解算得到的车辆定位结果和步骤3路侧单元获取的车辆周围动态交通目标位置信息，基于神经网络预测车辆及其周围交通目标的运动速度及运动轨迹；

15、步骤5.2，基于步骤5.1预测的车辆及其周围动态交通目标的运动轨迹，为车辆提供避障防碰预警；

16、步骤5.3，对车辆的行驶路线进行规划，给出最优的行驶路径。

17、而且，所述步骤1中当gnss能同时、连续接收到m颗以上导航卫星信号时，认为此时的gnss信号可用，使用卡尔曼滤波算法，包括滤波初始化、时间更新和测量更新，将车载gnss和ins原始观测值采用紧组合的方式进行计算，得到车辆定位结果。

18、而且，所述步骤2中当gnss不能同时、连续接收到m颗以上导航卫星信号时，认为此时的gnss信号不可用，使用ins原始观测值，即imu测量车辆在惯性空间的线运动和角运动信息，根据导航微分方程计算车辆的位置、速度和姿态信息。

19、而且，所述步骤3中路侧单元包括gnss天线、gnss接收板、时间同步模块、工业相机、接口与控制模块、路侧计算单元、通信模块和电源。其中，路侧计算单元内搭载有目标识别、基于深度学习的单相机单视图三维定位、多相机验核算法。

20、而且，所述步骤4.1中将ins获取的车辆定位信息与路侧单元获取的车辆及其周围交通目标定位信息进行时间匹配，使同一时间使用两种方式获得的定位信息相对应。

21、而且，所述步骤4.2中利用卡尔曼滤波器对车载ins得到的位置、速度信息和路侧单元获取的车辆及周围交通目标定位信息进行融合，对ins误差进行在线估计，用以修正机械编排的导航结果，并进行imu误差补偿，得到组合导航结果。其中，机械编排用于完成ins的初始对准，并按照ins机械编排算法，完成姿态、速度和位置的更新。

22、而且，所述步骤5.2中如果车辆和动态目标在基于现有信息的轨迹预测中存在同一时间位于同一位置，即判定车辆和目标存在碰撞可能，便将预警信息发送给车辆。

23、而且，所述步骤5.3中根据步骤5.1得到的车辆及其周围动态交通目标的轨迹预测信息，按照无碰撞预警、最快抵达准则，对车辆进行路径规划。

24、本发明还提供一种基于路侧相机与车载gnss/ins的车路协同定位导航系统，用于实现如上所述的一种基于路侧相机与车载gnss/ins的车路协同定位导航方法。

25、而且，包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的程序指令执行如上所述的一种基于路侧相机与车载gnss/ins的车路协同定位导航方法。

26、与现有技术相比，本发明具有如下优点：

27、1）本发明利用路侧相机和车载gnss/ins的良好互补性，通过多域信息关联融合的方式，提出一种基于车-路信息融合的组合导航算法，解决了复杂环境下gnss不可用时，仅使用ins进行导航定位精度和可靠性较差的问题，提高了车辆定位精度，优化了算法性能和可用性。

28、2）采用本发明提供的组合导航方法能够拓展环境感知范围并减少盲点，提高车辆对周围动态交通目标感知测量的准确性和完整性，实现更准确的路径规划和避障预警，能够显著提高城市环境下导航性能的可用性、可靠性和精度，提高自动驾驶的行业的应用范围和安全性。

29、3）本发明利用路侧相机进行车路协同定位感知，整体改造成本低，数据采集自动化，作业效率高，实时性能好。