本发明属于无线定位技术领域，具体涉及一种贝叶斯滤波的多车辆协作定位方法。
背景技术：
车辆位置信息是智能交通系统中安全类应用的关键组成部分。智能驾驶、生活服务、位置服务、用车服务、安全防护以及无人驾驶等，这些应用很大程度上依赖于车辆的位置信息。目前，车辆定位技术有很多，全球卫星导航系统(gnss)地面接收机成本低，卫星覆盖范围广，可以为汽车用户提供位置和速度信息。当gnss定位技术在卫星信号覆盖良好的情况下，能为用户提供15米左右定位精度的业务服务，基本可以满足当前的非安全应用的定位要求。但是，gnss信号容易被高层建筑和立体式交通遮挡，信号较弱或者无信号时，gnss定位是不准确的甚至是不可用的。这极大的影响了车辆对于位置服务的需求体验，容易导致车辆导航出错，道路拥塞，交通事故的发生，以及其他一些基于位置服务的应用也受到影响。车辆定位技术虽然有很多种，但都在一定程度上存在着定位精度不佳或者成本过高的问题，这对于车辆主动安全系统来说误差太大。
为了应对单节点定位精度不高，一种全新的协作定位方法应运而生。协作定位是不完全依靠自身所携带的定位设备进行定位，而是同周围具有定位信息来源的设备进行通信连接，通过数据融合，从而推算出自身的位置。目前常用的提高车辆定位性能的技术有基于贝叶斯滤波的方法。基于贝叶斯滤波的方法主要思想是通过贝叶斯方法，由先验概率分布求出位置信息的后验概率分布，在通过最大后验概率估计计算出位置信息，但这种方法所使用的车辆的位置信息的先验概率分布，是基于车载导航系统定位所得，本身就存在一定的误差，并且还没有结合周边车辆的位置信息，因此求出的后验概率分布误差较大，从而得到的滤波位置信息精度不高。
技术实现要素：
为了解决上述问题，本发明提供一种贝叶斯滤波的多车辆协作定位方法，所述方法利用目标车辆与相邻车辆位置的概率密度函数，以及车辆之间相对距离的概率密度函数，在扩展卡尔曼滤波的每次迭代过程中，通过贝叶斯滤波，得到目标车辆位置的后验概率估计，再根据最大后验估计值，确定目标车辆最终的位置估计，通过双重滤波，实现协作定位；
进一步地，所述方法包括：
s1：获取当前时刻目标车辆观测到相邻车辆时，目标车辆的位置测量值以及相邻车辆的数目，并构造系统的状态方程；
s2：获取目标车辆和相邻车辆在当前时刻的相对位置信息以及相邻车辆的数目，并构造系统的观测方程；
s3：将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到系统的位置预测值矩阵以及系统位置估计值矩阵，并计算系统位置预测值的概率密度函数矩阵以及系统相对距离的概率密度函数矩阵；
s4：将系统位置预测值的概率密度函数矩阵和系统相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵；
s5：根据最大后验概率估计得到目标车辆的位置估计值；
进一步地，所述s1中构造系统的状态方程包括：
s11：获取k时刻目标车辆x0的位置测量值
其中，k时刻表示当前时刻，t表示转置，
s12：获取所述相邻车辆的数目n；
s13：根据观测到的相邻车辆的数目n，构造系统的状态方程
其中，
进一步地，所述s2中构造系统的观测方程包括：
s21：获取k时刻相邻车辆xj的位置测量值
其中，n表示相邻车辆的数目，xjk表示k时刻相邻车辆xj在x轴方向的坐标，yjk表示k时刻相邻车辆xj在y轴方向的坐标，θjk表示k时刻相邻车辆xj运动方向与x轴所形成的夹角；
s22：计算k时刻目标车辆x0和相邻车辆xj之间的相对位置信息
其中，
s23：根据计算得到的k时刻目标车辆x0和相邻车辆xj之间的相对位置信息
其中，n表示相邻车辆的数目；
进一步地，所述s3具体包括：
s31：将状态方程xk和观测方程zk代入到扩展卡尔曼滤波中，得到目标车辆x0的位置预测值矩阵
s32：计算当前时刻系统位置预测值
进一步地，所述s4中后验概率密度矩阵计算方法如下：
将当前时刻系统位置预测值
进一步地，所述s5中根据最大后验概率估计得到目标车辆的位置估计值包括：
得到后验概率密度矩阵
进一步地，所述s4具体包括：
s41：计算k+1时刻系统状态预测值
其中，
其中，ak为系统状态xk的雅可比矩阵，bk为系统输入uk的雅可比矩阵，w为系统的过程激励噪声；
所述
其中，
s42：计算k+1时刻系统状态预测值
s43：计算k+1时刻系统状态协方差的预测值：
其中,pk为k时刻系统状态协方差，q为系统位置观测值误差协方差，t为矩阵转置，ts为采样周期；
s44：计算k+1时刻扩展卡尔曼滤波增益：
其中，hk为系统观测方程zk的雅可比矩阵，r为观测方程zk的误差协方差；
s45：更新k+1时刻系统状态协方差
s46：更新k+1时刻系统状态估计值；
其中，
本发明的有益效果如下：
在扩展卡尔曼滤波的每次迭代过程中，通过贝叶斯滤波的方法将扩展卡尔曼滤波得到的目标车辆预测位置的概率密度函数与相邻车辆位置的概率密度函数以及相对距离的概率密度函数相结合，计算出目标车辆在观测到相邻车辆之后的后验概率密度函数，在根据后验概率中的最大值，求出目标车辆最终的位置估计值，实现多车辆协作定位，降低车辆的位置误差，从而得到更加精确的车辆位置信息。
附图说明
图1为本发明实施例提供的一种贝叶斯滤波的多车辆协作定位方法的流程示意图；
图2为本发明实施例提供的多车辆协作系统模型示意图；
图3为本发明实施例测量的位置误差与gps测量的位置误差对比示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细描述。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，在下文对本发明的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也可以完全理解本发明。
下面结合附图和具体实施例对本发明作进一步说明，但不作为对本发明的限定。下面为本发明的举出最佳实施例：
如图1-图3所示，本发明提供一种贝叶斯滤波的多车辆协作定位方法，所述方法通过获取当前时刻目标车辆观测到相邻车辆时，目标车辆的位置测量值以及相邻车辆的数目，并构造系统的状态方程；获取目标车辆和相邻车辆在当前时刻的相对位置信息以及相邻车辆的数目，并构造系统的观测方程；将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到系统的位置预测值矩阵以及系统位置估计值矩阵，并计算系统位置预测值的概率密度函数矩阵以及系统相对距离的概率密度函数矩阵；将系统位置预测值的概率密度函数矩阵和系统相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵；根据最大后验概率估计得到目标车辆的位置估计值。这样，在扩展卡尔曼滤波的每次迭代过程中，通过贝叶斯滤波的方法将扩展卡尔曼滤波得到的目标车辆预测位置的概率密度函数与相邻车辆位置的概率密度函数以及相对距离的概率密度函数相结合，计算出目标车辆在观测到相邻车辆之后的后验概率密度函数，在根据后验概率中的最大值，求出目标车辆最终的位置估计值，实现多车辆协作定位，降低车辆的位置误差，从而得到更加精确的车辆位置信息。
本发明实施例所述的一种贝叶斯滤波的多车辆协作定位方法，在信噪比较低，噪声较大的环境下，估计的位置结果比gps测量结果更加准确，有效地提高了车辆的定位精度。在实际应用中，目标车辆周围的相邻车辆是不断变化的，本发明可以根据相邻车辆的实际情况自适应地选择相邻车辆个数以及相应的相邻车辆的概率密度函数。
本实施例中，目标车辆v0可以通过车载导航(例如，gps设备或北斗设备)获取k时刻目标车辆v0的位置测量值；同时，目标车辆v0可以通过短程距离通信(dedicatedshort-rangecommunication，dsrc)获取k时刻相邻车辆vj的位置观测值。
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述获取当前时刻目标车辆观测到相邻车辆时，目标车辆的位置测量值以及相邻车辆的数目，并构造系统的状态方程包括：
获取k时刻目标车辆x0的位置测量值
其中，k时刻表示当前时刻，t表示转置，
获取所述相邻车辆的数目n；
根据观测到的相邻车辆的数目n，构造系统的状态方程
其中，
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述获取目标车辆和相邻车辆在当前时刻的相对位置信息以及相邻车辆的数目，并构造系统的观测方程包括：
获取k时刻相邻车辆xj的位置测量值
其中，n表示相邻车辆的数目，xjk表示k时刻相邻车辆xj在x轴方向的坐标，yjk表示k时刻相邻车辆xj在y轴方向的坐标，θjk表示k时刻相邻车辆xj运动方向与x轴所形成的夹角。
计算k时刻目标车辆x0和相邻车辆xj之间的相对位置信息
其中，
根据计算得到的k时刻目标车辆x0和相邻车辆xj之间的相对位置信息
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到系统的位置预测值矩阵以及系统位置估计值矩阵，并计算系统位置预测值的概率密度函数矩阵以及系统相对距离的概率密度函数矩阵包括：
将状态方程xk和观测方程zk代入到扩展卡尔曼滤波中，得到目标车辆x0的位置预测值矩阵
计算当前时刻系统位置预测值
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述将系统位置预测值的概率密度函数矩阵和系统相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵包括：
将当前时刻系统位置预测值
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述根据最大后验概率估计得到目标车辆的位置估计值包括：
求得后验概率密度矩阵
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到系统的位置预测值矩阵以及系统位置估计值矩阵，并计算系统位置预测值的概率密度函数矩阵以及系统相对距离的概率密度函数矩阵包括：
s1、计算k+1时刻系统状态预测值
所述
其中，ak为系统状态xk的雅可比矩阵，bk为系统输入uk的雅可比矩阵，w为系统的过程激励噪声；
所述
其中，
s2、计算k+1时刻系统状态预测值
s3、计算k+1时刻系统状态协方差的预测值
s4、计算k+1时刻扩展卡尔曼滤波增益
s5、更新k+1时刻系统状态协方差
s6、更新k+1时刻系统状态估计值
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述将系统位置预测值的概率密度函数矩阵和系统相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵包括：
s1、根据贝叶斯公式，目标车辆v0观测到n个相邻车辆vj之后的后验概率密度函数表示为：
其中，yj表示目标车辆v0测量到相邻车辆vj的位置xj，并且得到两辆车之间的相对距离dj，即yj＝{xj,dj}，
s2、根据全概率公式，可将后验概率密度函数表示为：
s3、根据目标车辆v0测量到相邻车辆vj之间是相互独立，可将后验概率密度函数表示为：
s4、根据yj表示为目标车辆v0测量到相邻车辆vj的位置xj，并且得到两辆车之间的相对距离dj可得
s5、根据概率
s6、根据系统位置预测值的概率密度函数矩阵
其中，
如图2所示，以具体的例子对本实施例所述的一种贝叶斯滤波的多车辆协作定位方法进行详细说明，并使用matlab仿真平台，对本实施例所述的一种贝叶斯滤波的多车辆协作定位方法的性能进行仿真分析：
步骤1，如图2所示，考虑到多车辆系统中一个目标车辆v0观测到周边有四个相邻车辆v1,v2,v3,v4，行驶在道路上，目标车辆v0通过自身gps设备接收到k时刻目标车辆v0相对于相邻车辆v1,v2,v3,v4时，目标车辆v0的位置测量值
步骤2，计算目标车辆v0与相邻车辆v1,v2,v3,v4之间的相对位置信息，构造观测方程
2.1)目标车辆v0通过dsrc获得相邻车辆vj的位置信息
2.2)计算目标车辆v0与相邻车辆vj之间的相对位置信息
其中，
2.3)根据相邻车辆的个数n＝4，构造系统的观测方程
步骤3，将步骤1得到的系统状态方程xk和步骤5得到的观测方程zk代入到扩展卡尔曼滤波中，得到目标车辆x0的位置预测值矩阵
3.1)计算k+1时刻系统状态预测值
其中，ak为系统状态xk的雅可比矩阵，bk为系统输入uk的雅可比矩阵，w为系统的过程激励噪声；
其中，
3.2)计算k+1时刻系统状态预测值
3.3)计算k+1时刻系统状态协方差的预测值
其中,pk为k时刻系统状态协方差，q为系统位置观测值误差协方差，t为矩阵转置，ts为采样周期；
3.4)计算k+1时刻扩展卡尔曼滤波增益kk+1：
其中，hk为系统观测方程zk的雅可比矩阵，r为观测方程zk的误差协方差；
3.5)更新k+1时刻系统状态协方差pk+1：
其中，i为单位矩阵；
3.6)更新k+1时刻系统状态估计值
其中，
步骤4，将步骤3得到的系统位置预测值的概率密度函数矩阵
4.1)根据贝叶斯公式，目标车辆v0观测到4个相邻车辆vj之后的后验概率密度函数表示为：
其中，yj(j＝1,2,3,4)表示目标车辆v0测量到相邻车辆vj的位置xj，并且得到两辆车之间的相对距离dj，即yj＝{xj,dj}(j＝1,2,3,4)，
4.2)根据全概率公式，可将4.1)中的后验概率密度函数表示为：
4.3)根据目标车辆v0测量到相邻车辆vj之间是相互独立，可将4.2)中的后验概率密度函数表示为：
4.4)根据yj表示为目标车辆v0测量到相邻车辆vj的位置xj，并且得到两辆车之间的相对距离dj可得
4.5)根据4.4)得到的概率
4.6)将系统位置预测值的概率密度函数矩阵
其中，
步骤5，根据步骤4得到的k+1时刻后验概率密度矩阵
本实施例中，如图3所示，图3描述了本发明实施例测量的位置误差与gps测量的位置误差对比示意图，图3中的横坐标是采样时间，纵坐标是位置误差大小，该结果是在信噪比为3db下进行1000次的同条件重复实验得出的。由图3可知，本发明实施例提出的方法显著降低了位置误差，并给出了更加准确的位置估计值。这说明本发明实施例提出的基于扩展卡尔曼滤波和贝叶斯滤波的多车辆协作定位方法是准确并且有效的，同时对比结果也表明本发明采用的方法性能优于gps测量的性能。
以上所述的实施例，只是本发明较优选的具体实施方式的一种，本领域的技术人员在本发明技术方案范围内进行的通常变化和替换都应包含在本发明的保护范围内。