[发明专利]一种基于高斯滤波器信念传播的协作式车辆定位方法

说明书

本发明公开了一种基于高斯滤波器信念传播的协作式车辆定位方法，包括：初始化所有车辆和锚点的位置信息和协方差矩阵；根据t‑1时刻的车辆的状态信息，利用车辆的运动模型，计算得到t时刻的车辆预测位置信息；对距离测量模型进行线性化得到近似；根据近似的线性化公式计算车辆之间以及车辆和锚点之间的信念消息，并进行M次信念传播迭代计算；计算得到车辆的后验位置估计和协方差后，再进行L次的迭代计算，最终得到车辆的位置估值和协方差矩阵。本发明有效提高了车辆在无卫星信号驾驶环境和随机丢包通信网络下的定位精度。

技术领域

本发明涉及车辆通信的车辆协同定位技术领域，特别是一种基于高斯滤波器信念传播的协作式车辆定位方法。

背景技术

自动驾驶技术已经成为当前车辆技术的发展方向，实现车辆自动驾驶的前提是需要高精度和稳定的车辆定位。全球导航卫星系统是现在最为广泛使用的车辆定位技术，可以为车辆用户提供位置信息，但当车辆在城市道路和隧道等驾驶环境中，卫星信号会被遮挡导致定位信息的丢失。协同定位的提出有效改善了卫星信号的丢失问题以及提高定位精度。车辆通过雷达等车载仪器获取和周围车辆或是路侧锚点之间的距离信息，锚点为可以提供高精度定位的路侧单元或车辆等，并与周围车辆共享信息，车辆便可以利用这些信息来更新自身的位置信息从而提高定位精度。

现有技术的车辆协同定位通常采用分布式，并基于贝叶斯估计。由于协作定位需要用到车辆间的相对距离的数据，而距离的量测模型往往是非线性的，定位精度往往取决于线性化的精确度，非参数的信念传播通过大量粒子达到近似，会导致过多的计算复杂度和较大的信道带宽。参数化的信念传播能够有效降低计算复杂度和信道带宽，符合实际的应用。

文献(F.Garc′/L.Svensson,and S./“Cooperativelocalization using posterior linearization belief propagation,”IEEETrans.Veh.Technol.,vol.67,no.1,pp.832-836)将后验线性化和信念传播结合，将统计线性回归应用于距离量测模型，降低了对通信带宽的需求。但是协同定位对通信网络有着严苛的要求，大部分协同定位算法并没有考虑到车辆通信网络中的随机丢包问题。

发明内容

本发明所要解决的技术问题是克服卫星信号丢失和通信网络中的随机丢包问题，而提供一种基于高斯滤波器信念传播的协作式车辆定位方法，车辆通过信念传播算法进行合作，从而为车辆提供高精度的定位。

为解决上述技术问题，本发明采用以下技术方案。

一种基于高斯滤波器信念传播的协作式车辆定位方法，包括：

步骤1、初始化所有车辆和锚点的初始位置值和协方差矩阵

其中，令表示车辆集合，/表示锚点集合，/表示车辆和锚点两者的集合，即车辆和锚点均视作节点，/是初始时刻节点a的二维位置，/为待估计量/的均值，/为待估计量/的协方差矩阵；

步骤2、根据t-1时刻的车辆i的状态信息包括车辆的位置信息和速度信息，利用车辆的运动模型，计算得到t时刻的车辆预测位置信息/其中，/是t-1时刻的车辆i的二维速度；

步骤3、根据车辆i和其邻接节点j的位置信息和协方差矩阵/对距离测量模型进行线性化得到线性化模型参数；其中邻接节点j为和车辆i进行信息通信的其他车辆和锚点，/

步骤4、根据近似的线性化公式计算车辆之间以及车辆和锚点之间的信念消息并进行M次信念传播迭代计算；

步骤5、经过M次迭代计算后，根据车辆的边缘后验位置概率分布计算得到车辆的后验位置估计，返回步骤3进行L次的迭代计算；

步骤6、经过L次迭代计算得到车辆在t时刻的后验位置最小均方误差估值的收敛解。

具体地，所述步骤2的过程包括：