[发明专利]一种车车通信环境下考虑车辆间相对位置的多车协同定位算法

权利要求书

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

S1.参与本多车协同定位方法的车辆，装备有车车通信信息交互设备、车载GPS设备和前置毫米波雷达，车载控制系统中包含一个车辆信息采集与存储模块、一个多车协同定位计算模块以及一个信息发送模块；

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

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

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

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

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

S7.在信息发送模块中，将向其他可接收到自车信息的所有目标车辆发送三组信息，分别为t+1时刻自车A预测目标车辆B_n的信息矩阵t时刻自车A的信息矩阵以及t+1时刻雷达能够测量到的前车B_n与自车A的相对位置信息

2.如权利要求1所述的一种车车通信环境下考虑车辆间相对位置的多车协同定位方法，其特征在于：在步骤S3中自车定位计算单元将会提取车辆信息采集与存储模块中t时刻自车A的测量信息矩阵以及目标车辆B_n对自车A预测信息矩阵之后利用联邦卡尔曼滤波器，将自车A的信息和其他所有车辆传输来对A的预测信息分别作为联邦卡尔曼滤波器的独立子系统进行信息融合，得到车辆A在t时刻的信息矩阵

3.如权利要求1所述的一种车车通信环境下考虑车辆间相对位置的多车协同定位方法,其特征在于：在步骤S4中，多车协同定位计算模块中的信息补足单元在两个工况下开始工作：1.当目标车辆B_n在t时刻的信息因通信问题没有传输到车辆A时，信息补足单元利用t-1时刻采集到的车辆B_n的信息矩阵来计算t时刻目标车辆B_n的信息矩阵其中设定t-1时刻的加速度将不会产生变化，仍沿用至t时刻，其余的位置和速度信息运用运动学公式进行计算来补足缺失信息；2.当自车A在t时刻无法采集到GPS信息时，信息补足单元利用t-1时刻自车定位计算单元计算出的车辆信息矩阵来计算t时刻车辆A的信息矩阵同样设定t-1时刻的加速度将不会产生变化，仍沿用至t时刻，其余的位置和速度信息运用运动学公式进行计算来补足缺失信息。

4.如权利要求1所述的一种车车通信环境下考虑车辆间相对位置的多车协同定位方法，其特征在于：在步骤S7中，在信息发送模块中，将发送三组信息，分别为t+1时刻自车A预测目标车辆B_n的信息矩阵t时刻自车A的信息矩阵以及t+1时刻雷达能够测量到的前车B_n与自车A的相对位置三组数据分别对应信息采集单元采集的目标车辆对自车定位预测信息、目标车辆信息和自车与目标车辆相对位置信息，信息发送至所有通信距离内的目标车辆，形成闭环过程。