基于时间同步算法的环岛下自动驾驶车辆高精度定位算法

本发明属于高精度定位，涉及环岛场景下采用车路协同技术对自动驾驶车辆进行高精度定位的方法，具体是基于时间同步算法的环岛场景下自动驾驶车辆高精度定位算法。

背景技术：

1、随着当前的辅助自动驾驶和未来的无人驾驶技术的发展，自动驾驶车辆已经进入商业化试点落地阶段，自动驾驶车辆的技术要求也不断提高。其中高精度定位可以帮助自动驾驶车辆获得精确的位置信息，使自动驾驶车辆实现精准的控制，完成复杂的行为动作，已经成为自动驾驶领域的研究热点。

2、城市环岛作为一个重要的交通节点，多设置在多个交通路口交汇处，能让车辆按同一个方向行驶，可以有效地减少交通事故，提高通行率。但随着车流量的增多，环岛处的道路车道线往往可以达到三车道和四车道，同时环岛往往为繁华的城市中心，高大的高楼会形成峡谷效应，对gps信号产生遮挡，因此环岛是实现自动驾驶车辆高精度定位的典型场景。

3、然而现有的车辆定位技术，存在以下两种问题，一是无法满足自动驾驶车辆的车道级定位要求，造成自动驾驶车辆因为定位不准而发生交通事故。二是在gps信号缺失环境下，可能会造成自动驾驶车辆定位偏差过大甚至无法定位。为此，如何实现自动驾驶车辆在环岛场境下车道级高精度定位已经成为自动驾驶领域的技术难题。

4、环岛中自动驾驶车辆定位缺失场景图如图1所示，多辆自动驾驶车辆驶入环岛，但由于环岛中心和路边的高大树木，以及城市中的楼宇对gps信号产生了遮挡，导致自动驾驶车辆使用单一的gps定位时会产生定位偏差增大的情况，使得自动驾驶车辆不能做出正确的行驶决策，从而引发严重的交通事故。图1中对两种典型的由自动驾驶车辆定位偏差引起的交通事故进行介绍，图1中右上角自动驾驶车辆驶离环岛时因定位偏差驶向对向车道，导致逆行，与对向车道中正常行驶的车辆发生了严重的碰撞事故；图1中右下角自动驾驶车辆驶离环岛时因自身地理位置定位偏差，将内圈定位误识别为外圈车道，从而发出驶离环岛的指令，导致与外圈车来那个发生了严重的碰撞事故。

5、现有的自动驾驶车辆定位技术可以分为全球卫星导航系统定位(globalnavigation satellite system，gnss)、视觉定位、航迹推算定位和车路协同定位。

6、其中车路协同定位技术是一种基于车载自组织网络(vanet)的无线合作定位方法，即每辆车与智能路基等通过无线通信交互各自观测到的道路信息，并依据交互得到的信息对当前车辆定位的结果进行修正，提高车辆定位的精度。车路协同定位由于其不依赖于卫星定位设备且受周围环境因素的影响小，能够有效解决车辆在卫星定位信号缺失条件下的定位实时性低、有效精度差、抗干扰能力弱等问题，已经成为未来车辆自主定位的发展趋势。

7、在车路协同定位中，由于车辆与车辆间，或车辆与基站间各传感器由不同的硬件实现，每个传感器都有自己特定的时钟，而传感器内的时钟高度敏感，导致车辆与基站精准时钟的异步，节点上时钟的微小差异可能会导致严重的定位错误。因此，如何实现车辆和路基时间的同步成为车路协同定位算法的核心。目前车速协同定位主要使用的时间同步算法有平均时间同步算法(average time synchronization,ats)、加权最大时间同步算法(weighted maximum time synchronization,wmts)和最小二乘估计时同步算法(least-square estimation time synchronization,lsts)。

8、ats算法的主要思想是每个节点通过平均自己和邻居节点的逻辑时钟，从而达到一整个网络的时钟同步。ats算法相比分布式一致时间同步算法(distributed consensustime synchronization,dcts)实现了无延迟条件下时间同步误差的收敛性，但存在随机通讯时延时时间同步误差会发散。gang x等人提出了dcts算法，计算了同步误差的渐近期望和均方。l.schenat等人提出了ats算法，并给出了ats算法在无通信时延的理想环境中收敛的严格证明。tian等人证明了ats算法在无向拓扑无线传感器网络(wireless sensornetwork，wsn)含随机通信时延的环境中是几乎必然发散的。wang,h等人提出了一种相对偏度估计器以降低通信延迟，使得ats算法能够在存在通讯时延的情况下实现时间同步。

9、wmts算法基于最大一致的时钟同步协议(maximum time synchronization，mts)进行改进，主要思想是把整个网络中的节点都同步到拥有最快时钟的节点上去，即将拥有最快时钟的节点看做参考节点。与mts算法不同的是在更新节点逻辑时钟时，增加根据相邻节点逻辑时钟大小赋权值的过程，而非简单的平均随机逼近，wmts算法可以有效地处理随机通信延迟，提高了收敛速度。r.olfati-saber等人提出了一致性共识的定义和策略。jianping he等人首先提出了mts算法，并证明了该算法在有限时间内收敛，又对wsn含随机通信时延的问题提出了wmts算法，给出了收敛证明和有限收敛时间的上界，说明收敛时间远小于ats算法。tian等人证明了wmts算法在具有随机通信时延的环境中，wmts算法是均方发散的，其同步精度依赖于随机时延的均值和均方差。

10、lsts算法同ats算法类似，主要思想也是平均自己与邻居节点的逻辑时钟，从而达到整个网络的时钟同步。但lsts算法提出了新的估计相对时钟速度的方法—最小二乘法迭代估计，并引入了衰减因子。huang等人提出了逐步减小步长的lsts算法，并引入均方和强共识的概念，证明了lsts算法在存在随机通讯时间延迟时能够实现时间同步的渐进收敛。tian在等人在2015年证明了lsts算法克服了基于共识的时间同步算法(consensus-basedtime synchronization,cbts)的均方发散条件，以及lsts算法的时间同步误差和偏移补偿都是有界的。tian在等人在2021年提出了基于延迟补偿的时间同步算法(delaycompensation-based time synchronization,dcbts),引入延迟补偿机制，通过偏移估计的一阶差值调整延迟补偿,并证明了其有效性。

11、ats、wmts和lsts算法在有界通信时延情况下都可以实现不同程度的时钟同步。然而同步信息的传输也会因传输距离的不同产生不同大小的时间延迟，对于行驶中的车辆，会对这种因非对称传输而产生的时间延迟不断累加，这也是造成车辆定位同步发散的重要原因之一，但上述时间同步算法均不能实现存在非对称通信时延时的时钟同步。

技术实现思路

1、本发明所要解决的技术问题是针对上述现有技术的不足提供一种基于时间同步算法的环岛下自动驾驶车辆高精度定位算法，本算法将运动学公式和时间同步相结合对自动驾驶车辆进行相对定位，设计了c-ats算法，并创新性用于智能基站与自动驾驶车辆的时钟和定位协同同步，克服非对称通讯时延对定位精度造成的影响，提高了时间同步精度，进而提高了车路协同定位的精度，实现了自动驾驶车辆在环岛场境下车道级高精度定位。

2、为实现上述技术目的，本发明采取的技术方案为：

3、基于时间同步算法的环岛下自动驾驶车辆高精度定位算法，包括：

4、步骤1：在环岛中心处安装一个智能基站，智能基站能够获得精准的时间信息；智能基站和自动驾驶车辆ci之间能通过周期性广播的形式进行无线通信交互；

5、步骤2：自动驾驶车辆ci在首轮广播周期向智能基站广播了一个cam信息，cam信息包括当前自动驾驶车辆ci的本地时钟瞬时速度以及自动驾驶车辆ci在本地时钟下的地理位置估值

6、步骤3：智能基站接收到自动驾驶车辆ci广播的cam消息后，从cam消息中获取自动驾驶车辆ci当前的地理位置估值

7、步骤4：自动驾驶车辆ci通过预设的漂移修正系数和偏移修正系数的值对时间进行修正，得到将赋值给

8、步骤5：自动驾驶车辆ci在第k≥2轮广播周期向智能基站广播了一个cam信息，cam信息包括当前自动驾驶车辆ci的本地时钟瞬时速度以及自动驾驶车辆ci对自身的地理位置的估值其中：

9、

10、其中，表示自动驾驶车辆ci在第r轮广播周期内对自身地理位置估值的随机误差；

11、步骤6：智能基站接收到自动驾驶车辆ci广播的cam消息后，在时间下，对自动驾驶车辆ci当前的地理位置进行估计，得到估值其中：

12、

13、其中，表示智能基站在第r轮广播周期内对自动驾驶车辆ci地理位置估值的随机误差，表示智能基站的本地时钟；

14、智能基站将估值附带在cam消息中进行广播；

15、步骤7：自动驾驶车辆ci接收到智能基站广播的cam消息的时间为自动驾驶车辆ci判断：对于任意给定值ε>0，若则完成定位，为自动驾驶车辆ci在本地时钟下的准确地理位置，若则执行步骤8；

16、步骤8：自动驾驶车辆ci根据c-ats算法计算滤波比例因子，通过滤波比例因子计算漂移修正系数和偏移修正系数

17、步骤9：通过漂移修正系数和偏移修正系数对本地时钟进行修正，得到将赋值给

18、步骤10：k＝k+1；返回执行步骤6。

19、作为本发明进一步改进的技术方案，所述的瞬时速度或的获取方法为：

20、自动驾驶车辆ci通过自身上的轮速传感器获取瞬时速度的测量值，采用自适应卡尔曼滤波算法对瞬时速度的测量值进行滤波处理，得到更新后的瞬时速度或

21、作为本发明进一步改进的技术方案，所述的步骤4中：

22、作为本发明进一步改进的技术方案，所述的步骤8中的滤波比例因子为：

23、

24、其中，ρη为滤波比例因子的权重系数。

25、作为本发明进一步改进的技术方案，所述的步骤8中的偏移修正系数为：

26、

27、其中，ρo为偏移权重系数。

28、作为本发明进一步改进的技术方案，所述的步骤8中的漂移修正系数为：

29、

30、其中，ρα为漂移权重系数。

31、作为本发明进一步改进的技术方案，通过漂移修正系数和偏移修正系数对本地时钟进行修正，得到具体为：

32、本发明的有益效果为：

33、本发明不依靠gps定位，将运动学公式和时间同步模型结合对自动驾驶车辆进行相对定位，建立了环岛场景下自动驾驶车辆-智能路基协同交互定位模型，然后设计了基于ats改进权重的c-ats算法，并创新性用于智能基站与自动驾驶车辆的时钟和定位协同同步，克服非对称通讯时延对定位精度造成的影响。除此之外，设计了自适应kalman算法和时间同步算法c-ats结合，提高时间同步精度的同时，进一步减小车速估计的误差，提高车路协同定位的精度，实现了自动驾驶车辆在环岛场境下车道级高精度定位。