一种面向短时局部定位失效的无人车群瞬时协同定位方法

本发明属于无人驾驶，主要涉及一种面向短时局部定位失效的无人车群瞬时协同定位方法。

背景技术：

1、近年来，随着大数据技术的快速发展，无人驾驶汽车的技术已经从早期的概念验证阶段发展到现在的实际道路测试和部分商业化阶段。许多公司都在积极开发和测试无人驾驶技术，这些技术包括高级传感器、强大的数据处理能力和复杂的算法，以实现环境感知、决策制定和精确控制。其中，车辆的精准定位是无人驾驶技术的核心要素之一，无人驾驶汽车需要精确地知道自己在环境中的位置，以便安全、有效地导航和执行驾驶任务。

2、然而目前无人驾驶车辆在定位技术上还存在以下不足：现代定位技术如gnss（全球导航卫星系统）、激光雷达和视觉传感器在某些情况下，如高楼林立的城市峡谷、隧道或树木茂密的地区，其性能可能会受到影响，导致定位精度下降；在网络信号干扰的环境中，无人驾驶车辆与云监控平台之间的通信可能会遭遇中断，导致车辆无法实时且精确地接收其定位数据；当无人车辆失去自身定位能力，采用基于车辆间通信(v2v)技术的相对定位方法时，参考车辆的定位数据质量又会对其最终定位精度产生不利影响。因此，如何在复杂、动态变化的环境条件下，对区域内无人车群的车辆位置信息进行精确量化评估，并采用基于v2v的相对定位技术，以补偿由环境干扰和网络延迟等因素导致的定位失效，已成为当前亟待解决问题。

技术实现思路

1、本发明为了克服以上现有技术的不足，提出一种面向短时局部定位失效的无人车群瞬时协同定位方法，以期能提高无人车群定位数据的质量，增强无人车在复杂场景下的定位精度和定位抗干扰能力，从而为无人车的定位提供一种新的思路。

2、本发明为解决上述问题采用如下技术方案：

3、本发明一种面向短时局部定位失效的无人车群瞬时协同定位方法的特点在于，包括：

4、步骤1、第个失效车辆在失效的时刻向周围车辆广播协同请求帧并接收周围任意第个应答车辆回复的协同响应帧；

5、步骤2、构建失效车辆在时刻的协同定位应答矩阵，并计算与应答车辆的降噪空间距离；

6、步骤3、失效车辆从所有应答车辆中筛选出满足有效应答范围的有效应答车辆，并构建有效协同定位应答矩阵，从而得到失效车辆与有效应答车辆的降噪空间距离向量；

7、步骤4、利用与，计算第个失效车辆在时刻的定位信息可信度集合；

8、步骤5、判断是否成立，若成立，则构建协同定位车辆在时刻的协同定位信息矩阵，从而推算失效车辆在时刻的部分位置参数并修正，否则，增大值后，返回步骤3，其中，表示无人车群内的有效应答车辆总数；

9、步骤6：构建失效车辆在时刻的协同请求帧与协同响应帧，用于失效车辆与周围车辆之间的交互。

10、本发明所述的一种面向短时局部定位失效的无人车群瞬时协同定位方法的特点也在于，所述步骤2是按如下步骤进行：

11、步骤2.1、基于无人车辆间的交互帧信息，利用式(1)构建第个失效车辆在时刻的无人车群内车辆协同定位应答矩阵；

12、(1)

13、式(1)中，表示第个失效车辆在时刻向第个应答车辆发出协同请求帧的时间戳，为第个失效车辆接收第个应答车辆回复的协同响应帧的时间戳，为第个失效车辆的无线电信号传输速度，为第个失效车辆的通信设备编号，为第个应答车辆在时刻的位置参数，表示第个应答车辆处理时刻的协同请求帧以及发出协同响应帧的时延，，，表示无人车群内的车辆总数；

14、步骤2.2、利用式(2)计算第个失效车辆与第个应答车辆在时刻的降噪空间距离；

15、(2)

16、式(2)中，为交互信息网络传输的噪声修正值。

17、进一步的，所述步骤3中是按如下步骤进行：

18、步骤3.1、定义失效车辆的有效应答范围为主定位失效时刻下应答车辆的降噪空间距离小于等于距离阈值，则所述第个失效车辆从所有应答车辆中筛选出满足有效应答范围的个有效应答车辆，从而利用式(3)构建第个失效车辆的有效应答范围内在时刻的有效协同定位应答矩阵；

19、(3)

20、式(3)中，表示第个失效车辆的有效应答范围内在时刻向第个有效应答车辆发出协同请求帧的时间戳，为第个失效车辆的有效应答范围内接收第个有效应答车辆回复的协同响应帧的时间戳，为有效应答范围内第个有效应答车辆在时刻的位置参数，表示有效应答范围内第个有效应答车辆处理时刻的协同请求帧以及发出协同响应帧的时延；

21、步骤3.2、计算第个失效车辆与有效应答车辆在时刻的降噪空间距离向量；其中，表示第个失效车辆与有效应答范围内第个有效应答车辆在时刻的降噪空间距离，。

22、进一步的，所述步骤4是按如下步骤进行：

23、步骤4.1、所述第个失效车辆提取中个有效应答车辆的位置参数，从而建立第个失效车辆的有效应答范围内有效应答车辆的位置参数信息集合，其中，表示有效应答范围内在时刻第个有效应答车辆的位置参数矩阵，且，表示有效应答范围内第个有效应答车辆在时刻的经度坐标，表示有效应答范围内第个有效应答车辆在时刻的纬度坐标，表示有效应答范围内第个有效应答车辆在时刻的高度坐标，表示有效应答范围内第个有效应答车辆在时刻的方向上的行驶方向，且，表示正东方向，表示正南方向，表示正西方向，表示正北方向，表示有效应答范围内第个应答车辆在时刻的车头偏转角度，且，表示有效应答范围内第个有效应答车辆在时刻的行驶方向q下的行驶速度，且，当时，表示第个有效应答车辆的行驶方向与第个失效车辆的行驶方向同向，当时，表示第个有效应答车辆的行驶方向与第个失效车辆的行驶方向反向，表示有效应答范围内第个有效应答车辆在时刻的加速度方向e下的加速度，且，当时，表示第个有效应答车辆的加速度方向与第个失效车辆的加速度方向同向，当时，表示第个有效应答车辆的加速度方向与第个失效车辆的加速度方向反向；

24、步骤4.2、利用式(4)计算第个失效车辆在时刻下与第个有效应答车辆间的伪距以及第个有效应答车辆位置参数的质量量化评估值，从而得到第个失效车辆在时刻下的有效应答车辆伪距集合以及有效应答车辆位置参数的质量量化评估值集合；

25、(4)

26、式(4)中，表示第个失效车辆在时刻的经度坐标，表示第个失效车辆在时刻的纬度坐标，表示第个失效车辆在时刻的高度坐标；

27、步骤4.3、利用式(5)构建有效应答范围内的第个有效应答车辆对于第个失效车辆在时刻的位置信息偏离度值，从而得到有效应答范围内的所有有效应答车辆对于第个失效车辆在时刻位置信息偏离度值集合；

28、(5)

29、式(5)中，、、、分别表示4个不同的设定值，且>>>；、、分别表示3个不同的设定值；且；

30、步骤4.4、将中第个失效车辆的任一位置参考信息记为，将中第个有效应答车辆的任一位置参考信息记为，，从而利用式(6)计算第个失效车辆的任一位置参考信息与第个有效应答车辆的任一位置参考信息间的模糊关联度，从而得到第个失效车辆在时刻与有效应答范围内第个应答车辆间的模糊关联度量化矩阵；

31、(6)

32、式(6)中，表示第个失效车辆在时刻的方向上的行驶方向，表示第个失效车辆在时刻的车头偏转角度，，表示第个失效车辆在时刻的行驶方向下的行驶速度，表示第个失效车辆在时刻的加速度方向下的行驶加速度；

33、步骤4.5、设置每辆应答车辆经度坐标的优先值为，每辆应答车辆纬度坐标的优先值为，每辆应答车辆高度坐标的优先值为，每辆应答车辆行驶方向的优先值为，每辆应答车辆车头偏转角度的优先值为，每辆应答车辆行驶速度的优先值为，每辆应答车辆加速度的优先值为，从而构建每辆应答车辆的位置参考信息的优先级矩阵；t表示转置；

34、步骤4.6、利用式(7)计算第个失效车辆的有效应答范围内与第个应答车辆在时刻的定位信息可信度值，从而得到第个失效车辆的有效应答范围内所有有效应答车辆的定位信息的可信度集合；

35、(7)。

36、进一步的，所述步骤5包括：

37、步骤5.1、利用式(8)构建协同定位车辆在时刻的协同定位信息矩阵；

38、(8)

39、式(8)中，表示有效应答范围内第个协同定位车辆在时刻的经度坐标，表示有效应答范围内第个协同定位车辆在时刻的纬度坐标，表示有效应答范围内第个协同定位车辆在时刻的高度坐标，；

40、步骤5.2、利用式(9)计算第个失效车辆在时刻的经度坐标、纬度坐标和高度坐标；

41、(9)

42、式(9)中，，，，，，，，分别表示在时刻的8个过程量，并有：

43、(10)

44、步骤5.3、将时刻、时刻、时刻和时刻中的任意时刻记为t时刻，利用式(11)计算第个失效车辆在时刻下修正后的部分位置参数；

45、(11)

46、式(11)中，为第个失效车辆在时刻修正后的经度坐标，为第个失效车辆在时刻修正后的纬度坐标，为第个失效车辆在时刻修正后的高度坐标，表示第个失效车辆在时刻在经度方向的速度分量，表示第个失效车辆在时刻在纬度方向的速度分量，表示第个失效车辆在时刻在高度方向的速度分量。

47、进一步的，所述步骤6是按如下步骤进行：

48、步骤6.1、将时刻修正后的部分位置参数与时刻的剩余位置参数组合为第个失效车辆在时刻的位置参数矩阵，从而将分别写入时刻的协同请求帧与时刻的协同响应帧；

49、步骤6.2、第个失效车辆在时刻判断自身主定位是否恢复正常，若未恢复正常，则将赋值给后，返回步骤1顺序执行，同时，当第个失效车辆在时刻感知到周围车辆的协同请求帧时，向其反馈协同响应帧。

50、与已有技术相比，本发明的有益效果在：

51、1、本发明提出了一种区域内无人车群位置参数的质量量化评估方法，对所研究无人车的定位数据质量进行精准评估，该方法为之后基于车辆间通信(v2v)技术的相对定位方法排除了受干扰的定位数据源，从而为无人群瞬时协同定位奠定基础。

52、2、本发明设计了一种无人车群内定位失效车辆与应答车辆模糊关联度的计算方法，如图1所示，选取无人车多个维度的位置参数作为判断因素，分析定位失效车辆与应答车辆在经度、纬度、高度、行驶方向、车头偏转角度、速度、加速度七个维度的关联性，最终形成模糊关联度量化矩阵，精确反映了各车辆间的动态关系，从而为基于车辆间通信(v2v)的相对定位方法中协同定位车辆的选择提供了可靠的量化依据。

53、3、本发明为无人车群在复杂行车环境下提供了一种补偿定位方法，当无人车自身的gnss定位信号受到干扰且因网络延时等问题失去与云监控服务器的联系时，可以通过建立与应答车辆的通信，接收应答车辆位置参数，使用过滤后的协同位置坐标计算自身的定位坐标，并在持续迭代计算自身位置坐标的过程中，使用协同定位信息修正模型能有效降低坐标计算误差，从而实现了无人车群间的瞬时协同定位，为无人车的发展开辟一种新gnss定位抗扰策略。