一种车车通信环境下考虑车辆间相对位置的多车协同定位算法的制作方法

本发明涉及汽车技术领域，特别涉及一种考虑车辆间相对位置的多车协同定位算法。

背景技术：

随着车联网技术的发展，车辆安全、服务应用变得越来越丰富，同时精准的车辆位置信息也变得愈加重要。gps(globalpositioningsystem)全球定位系统被广泛的应用于车辆导航领域，成为了车辆位置信息的主要来源。然而，由于城市环境下高楼林立造成卫星信号被遮挡，且多径信号严重干扰，造成gps定位误差高有时达到数十米到上百米，甚至不能够定位。尽管在定位效果较好时，定位误差也会达到5到10米。定位精度低以及较差的定位稳定性限制了gps系统在车辆安全、防碰撞检测等关系生命安全领域的使用。

现有的提升定位精度的方法大多从卫星导航定位系统的原理出发，包括基于载波相位的位置估算和基于伪距的位置估算，但这些方法处理后定位误差仍然存在并且无法解决gps信号丢失的问题。在车载终端方面，一般运用组合导航的方法，利用惯性导航与gps定位结合以提升定位精度，但是具有足够精度的惯性导航传感器价格昂贵，而即使是很贵的传感器，也需要较长的初始化时间，并且在长时间后会有误差累计。

现阶段人们对车辆的功能性和安全性愈加重视，因此车上装载多传感器已成为必然趋势，汽车可以通过can总线以及gps天线、惯性导航等传感器对自车的位置、速度、加速度和航向角等信息进行采集和存储，并且能够通过雷达等传感器获取目标车辆与自车的相对位置信息。

与此同时，随着通信技术和车车通信技术的愈加成熟，车辆与车辆之间可以通过无线通信设备将所有检测到的车辆信息在一定范围内进行交互，使得每辆车可以同时感知自车与目标车辆的所有信息。这为进行多车协同定位以提升自车的定位精度提供了新的可能。

技术实现要素：

发明目的：一种车车通信环境下考虑车辆间相对位置的多车协同定位算法，以至少解决现有技术存在的部分问题，在现有技术的条件下，利用车辆间测量出较为准确的相对位置信息以及自车和目标车辆的位置、速度和加速度信息进一步提升定位精度和可靠性。

技术方案：一种车车通信环境下考虑车辆间相对位置的多车协同定位算法，包括以下步骤：

在参与本多车协同定位算法的车辆，需要装有车车通信信息交互设备(如dsrc，wifi等)、车载gps设备和前置毫米波雷达，在车载控制系统中需加入一个车辆信息采集与存储模块、一个多车协同定位计算模块以及一个信息发送模块。

在车辆信息采集与存储模块中，包含信息采集单元、滤波单元和信息存储单元。模块对自车的行驶信息、自车与目标车辆相对位置信息、目标车辆信息以及目标车辆对自车定位预测信息进行采集、滤波与存储。

在多车协同定位计算模块中，利用采集到的信息进行计算以提升自车定位精度和预测目标车辆位置信息。该模块包含自车定位计算单元、信息补足单元、相对位置计算单元和目标车辆定位预测单元。自车定位计算单元提取t时刻自车a的测量信息矩阵(其中包含自车a的定位信息，速度信息和加速度信息)以及目标车辆bn对自车a的预测信息矩阵(其中包含目标车辆bn中计算得到的目标车辆bn对自车a的位置估计值以及目标车辆bn的速度信息和加速度信息)，之后通过联邦卡尔曼滤波器，进行信息融合，得到车辆a在t时刻的信息矩阵(其中包含自车a计算得到的位置信息，自车a的速度信息和加速度信息)。

在多车协同定位计算模块中，信息补足单元对两类信息进行补足，第一类是目标车辆信息的通讯设备因设备问题或信号干扰而发生某一时刻目标车辆信息没有传到自车。第二类是自车gps信号某一时刻产生丢失，无法得到本时刻自车的位置信息。在上述两类情况发生时，信息补足单元将会对丢失数据进行处理。

在多车协同定位计算模块中，相对位置计算单元用来计算任一目标车辆bn相对于自车a的相对位置模块的输入量为车车通信下得到的各车与前车的相对位置信息通过所获得的所需相对位置的叠加来获得自车与任一目标车辆的相对位置信息。

在多车协同定位计算模块中，因为每辆车采集数据的时间点不可能完全相同，为提升定位精度的可靠性和安全性，需要将自车对目标车辆的位置估计执行进一步预测。目标车辆定位预测单元利用t时刻得到的车辆信息矩阵相对位置向量和目标车辆bn的速度以及加速度信息，来预测目标车辆bn在t+1时刻的信息矩阵(其中包含自车a对目标车辆bn在t+1时刻的位置估计以及自车a的速度信息和加速度信息)。

在信息发送模块中，将向其他可接收到自车信息的所有目标车辆发送三组信息，分别为t+1时刻自车a预测目标车辆bn的信息矩阵t时刻自车a的信息矩阵以及t+1时刻雷达能够测量到的前车bn与自车a的相对位置信息分别对应信息采集单元采集的目标车辆对自车定位预测信息、目标车辆信息和自车与目标车辆相对位置信息，形成闭环过程。

附图说明

图1是本发明结构组成图

图2是本发明工作流程图

图3是本发明仿真结果：卡尔曼滤波和多车协同定位算法平均误差对比图

具体实施方案

下面结合附图和实施方案对本发明进行详细描述。

如图1所示，本发明由硬件部分和车载控制系统组成，硬件部分包括通信设备、车载gps设备、前置毫米波雷达和can总线。车载控制系统主要包括三个模块，分别为车辆信息采集与存储模块、多车协同定位计算模块和信息发送模块。

本发明的组成中的硬件部分，通信设备用来完成车车之间信息交互，车载gps设备用来测量车辆位置信息，前置毫米波雷达用来测量自车与前车的相对位置信息，can总线用来测量读取车辆的速度信息。

如图2的工作流程图所示，本发明的的车载控制系统的信息采集与存储模块中，信息采集单元利用can总线与车载gps设备采集自车的行驶信息、利用毫米波雷达和车车通信设备采集自车与目标车辆相对位置信息、利用车车通信设备采集目标车辆信息以及目标车辆对自车定位预测信息，之后滤波单元对所得信息进行滤波处理以提升信息可靠度，最后存储单元将信息进行存储与打包。

本发明的下述所有公式中，(x,y),分别代表车辆的横纵位置坐标、横纵向速度和横纵向加速度，设定自车为车辆a，自车定位计算单元将会提取车辆信息采集与存储模块中t时刻自车a的测量信息矩阵以及目标车辆bn对自车a预测信息矩阵之后利用联邦卡尔曼滤波器，将自车a的信息和其他所有车辆传输来对a的预测信息分别作为联邦卡尔曼滤波器的独立子系统进行信息融合，得到我们需要的车辆a在t时刻的相对准确的信息矩阵

本发明多车协同定位计算模块中的信息补足单元在两个工况下开始工作：1.当目标车辆bn在t时刻的信息因通信问题没有传输到车辆a时，信息补足单元利用t-1时刻采集到的目标车辆bn的信息矩阵来计算t时刻目标车辆bn的信息矩阵其中设定t-1时刻的加速度将不会产生变化，仍沿用至t时刻，其余的位置和速度信息运用运动学公式进行计算来补足缺失信息；2.当自车a在t时刻无法采集到gps信息时，信息补足单元利用t-1时刻自车定位计算单元计算出的车辆信息矩阵来计算t时刻车辆a的信息矩阵同样设定t-1时刻的加速度将不会产生变化，仍沿用至t时刻，其余的位置和速度信息运用运动学公式进行计算来补足缺失信息。

本发明的多车协同定位计算模块中，相对位置计算单元用来计算任一目标车辆bn相对于自车a的相对位置若自车雷达可以测量到与目标车辆相对位置，则直接使用测量数据若目标车辆无法直接测量相对位置，则用车车通信获得的其目标车辆能够测量到与目标车的相对位置进行矢量叠加，计算得到自车与目标车辆的相对位置。

本发明的多车协同定位计算模块中，目标车辆定位预测单元将提取自车定位计算单元中t时刻得到的车辆信息矩阵相对位置计算单元得到的向量和目标车辆bn的速度以及加速度信息，仍设定加速度不变，沿用至t+1时刻，结合运动学公式来计算自车a预测目标车辆bn的信息矩阵

在信息发送模块中，将发送三组信息，分别为t+1时刻自车a预测目标车辆bn的信息矩阵t时刻自车a的信息矩阵以及t+1时刻雷达能够测量到的前车bn与自车a的相对位置信息发送至所有通信距离内的目标车辆。

对本发明的算法进行仿真分析，生成30辆车gps定位信息，给30辆车均附加方差为5m的高斯噪声，每辆车均与周围距离最近的6辆车进行通信并执行一种车车通信环境下考虑车辆间相对位置的多车协同定位算法，算出30辆车的平均误差，并与只做卡尔曼滤波的平均误差相比较。

结果图图3说明一种车车通信环境下考虑车辆间相对位置的多车协同定位算法效果较好，具有提升定位精度速度快，定位精度提升较大的特点。

本发明通过车车通信技术以及车载传感器采集、处理和存储自车和目标车辆的的运动状态信息、位置信息以及两车相对位置信息后，对获得的数据进行融合和预测，并发送至其他通讯车辆，简单有效地在考虑了车辆间的相对位置的情况下于车载终端提升了定位精度，优于现有的技术方法。

以上详细描述了本发明的实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围和整体框架内，具体的细节是可以改变替换的，例如本发明的自车与目标车辆信息采集方式，通讯方式，滤波方式以及信息融合方式都属于本发明的保护范围之内。