CN108398704B - 一种贝叶斯滤波的多车辆协作定位方法 - Google Patents

一种贝叶斯滤波的多车辆协作定位方法 Download PDF

Info

Publication number
CN108398704B
CN108398704B CN201810119662.9A CN201810119662A CN108398704B CN 108398704 B CN108398704 B CN 108398704B CN 201810119662 A CN201810119662 A CN 201810119662A CN 108398704 B CN108398704 B CN 108398704B
Authority
CN
China
Prior art keywords
vehicle
probability density
target vehicle
matrix
density function
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Expired - Fee Related
Application number
CN201810119662.9A
Other languages
English (en)
Other versions
CN108398704A (zh
Inventor
杜利平
侯晓田
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Science and Technology Beijing USTB
Original Assignee
University of Science and Technology Beijing USTB
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Science and Technology Beijing USTB filed Critical University of Science and Technology Beijing USTB
Priority to CN201810119662.9A priority Critical patent/CN108398704B/zh
Publication of CN108398704A publication Critical patent/CN108398704A/zh
Application granted granted Critical
Publication of CN108398704B publication Critical patent/CN108398704B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明提供一种贝叶斯滤波的多车辆协作定位方法，所述方法利用目标车辆与相邻车辆位置的概率密度函数，以及车辆之间相对距离的概率密度函数，在扩展卡尔曼滤波的每次迭代过程中，通过贝叶斯滤波，得到目标车辆位置的后验概率估计，再根据最大后验估计值，确定目标车辆最终的位置估计，通过双重滤波，实现协作定位，降低车辆的位置误差，从而得到更加精确的车辆位置信息。

Description

一种贝叶斯滤波的多车辆协作定位方法
技术领域
本发明属于无线定位技术领域，具体涉及一种贝叶斯滤波的多车辆协作定位方法。
背景技术
车辆位置信息是智能交通***中安全类应用的关键组成部分。智能驾驶、生活服务、位置服务、用车服务、安全防护以及无人驾驶等，这些应用很大程度上依赖于车辆的位置信息。目前，车辆定位技术有很多，全球卫星导航***(GNSS)地面接收机成本低，卫星覆盖范围广，可以为汽车用户提供位置和速度信息。当GNSS定位技术在卫星信号覆盖良好的情况下，能为用户提供15米左右定位精度的业务服务，基本可以满足当前的非安全应用的定位要求。但是，GNSS信号容易被高层建筑和立体式交通遮挡，信号较弱或者无信号时，GNSS定位是不准确的甚至是不可用的。这极大的影响了车辆对于位置服务的需求体验，容易导致车辆导航出错，道路拥塞，交通事故的发生，以及其他一些基于位置服务的应用也受到影响。车辆定位技术虽然有很多种，但都在一定程度上存在着定位精度不佳或者成本过高的问题，这对于车辆主动安全***来说误差太大。
为了应对单节点定位精度不高，一种全新的协作定位方法应运而生。协作定位是不完全依靠自身所携带的定位设备进行定位，而是同周围具有定位信息来源的设备进行通信连接，通过数据融合，从而推算出自身的位置。目前常用的提高车辆定位性能的技术有基于贝叶斯滤波的方法。基于贝叶斯滤波的方法主要思想是通过贝叶斯方法，由先验概率分布求出位置信息的后验概率分布，在通过最大后验概率估计计算出位置信息，但这种方法所使用的车辆的位置信息的先验概率分布，是基于车载导航***定位所得，本身就存在一定的误差，并且还没有结合周边车辆的位置信息，因此求出的后验概率分布误差较大，从而得到的滤波位置信息精度不高。
发明内容
为了解决上述问题，本发明提供一种贝叶斯滤波的多车辆协作定位方法，所述方法利用目标车辆与相邻车辆位置的概率密度函数，以及车辆之间相对距离的概率密度函数，在扩展卡尔曼滤波的每次迭代过程中，通过贝叶斯滤波，得到目标车辆位置的后验概率估计，再根据最大后验估计值，确定目标车辆最终的位置估计，通过双重滤波，实现协作定位；
进一步地，所述方法包括：
S1：获取当前时刻目标车辆观测到相邻车辆时，目标车辆的位置测量值以及相邻车辆的数目，并构造***的状态方程；
S2：获取目标车辆和相邻车辆在当前时刻的相对位置信息以及相邻车辆的数目，并构造***的观测方程；
S3：将***的运动方程和观测方程代入到扩展卡尔曼滤波中，得到***的位置预测值矩阵以及***位置估计值矩阵，并计算***位置预测值的概率密度函数矩阵以及***相对距离的概率密度函数矩阵；
S4：将***位置预测值的概率密度函数矩阵和***相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵；
S5：根据最大后验概率估计得到目标车辆的位置估计值；
进一步地，所述S1中构造***的状态方程包括：
S11：获取k时刻目标车辆X0的位置测量值
Figure BDA0001571700440000031
其中，k时刻表示当前时刻，T表示转置，
Figure BDA0001571700440000032
表示k时刻目标车辆X0在x轴方向的坐标，
Figure BDA0001571700440000033
表示k时刻目标车辆X0在y轴方向的坐标，
Figure BDA0001571700440000034
表示k时刻目标车辆X0运动方向与x轴方向所形成的夹角；
S12：获取所述相邻车辆的数目N；
S13：根据观测到的相邻车辆的数目N，构造***的状态方程
Figure BDA0001571700440000035
其中，
Figure BDA0001571700440000036
表示k时刻目标车辆V0观测到相邻车辆Vj时，目标车辆V0相对于相邻车辆Vj的位置；
进一步地，所述S2中构造***的观测方程包括：
S21：获取k时刻相邻车辆Xj的位置测量值
Figure BDA0001571700440000037
其中，N表示相邻车辆的数目，xjk表示k时刻相邻车辆Xj在x轴方向的坐标，yjk表示k时刻相邻车辆Xj在y轴方向的坐标，θjk表示k时刻相邻车辆Xj运动方向与x轴所形成的夹角；
S22：计算k时刻目标车辆X0和相邻车辆Xj之间的相对位置信息
Figure BDA0001571700440000038
表示如下；
Figure BDA0001571700440000041
其中，
Figure BDA0001571700440000042
表示为目标车辆X0与相邻车辆Xj之间的相对距离，
Figure BDA0001571700440000043
表示为目标车辆X0与相邻车辆Xj之间的相对方位角；
S23：根据计算得到的k时刻目标车辆X0和相邻车辆Xj之间的相对位置信息
Figure BDA0001571700440000044
构造***的观测方程；
Figure BDA0001571700440000045
其中，N表示相邻车辆的数目；
进一步地，所述S3具体包括：
S31：将状态方程Xk和观测方程Zk代入到扩展卡尔曼滤波中，得到目标车辆X0的位置预测值矩阵
Figure BDA0001571700440000046
以及位置估计值矩阵
Figure BDA0001571700440000047
S32：计算当前时刻***位置预测值
Figure BDA0001571700440000048
的概率密度函数矩阵
Figure BDA0001571700440000049
以及***相对距离
Figure BDA00015717004400000410
的概率密度函数矩阵
Figure BDA00015717004400000411
进一步地，所述S4中后验概率密度矩阵计算方法如下：
将当前时刻***位置预测值
Figure BDA00015717004400000412
的概率密度函数矩阵
Figure BDA00015717004400000413
和***相对距离
Figure BDA00015717004400000414
的概率密度函数矩阵
Figure BDA00015717004400000415
以及相邻车辆位置的概率密度函数
Figure BDA00015717004400000416
代入到贝叶斯滤波中，得到后验概率密度矩阵
Figure BDA00015717004400000417
进一步地，所述S5中根据最大后验概率估计得到目标车辆的位置估计值包括：
得到后验概率密度矩阵
Figure BDA00015717004400000418
中最大后验概率值以及最大后验概率值所对应的行数Rmax，将最大行数Rmax所对应的位置估计值作为目标车辆最终的位置估计值
Figure BDA0001571700440000051
进一步地，所述S4具体包括：
S41：计算k+1时刻***状态预测值
Figure BDA0001571700440000052
和***观测方程预测值
Figure BDA0001571700440000053
其中，
Figure BDA0001571700440000054
表示为
Figure BDA0001571700440000055
其中，Ak为***状态Xk的雅可比矩阵，Bk为***输入Uk的雅可比矩阵，W为***的过程激励噪声；
所述
Figure BDA0001571700440000056
表示为：
Figure BDA0001571700440000057
其中，
Figure BDA0001571700440000058
S42：计算k+1时刻***状态预测值
Figure BDA0001571700440000059
的概率密度函数矩阵
Figure BDA00015717004400000510
以及***相对距离
Figure BDA00015717004400000511
的概率密度函数矩阵
Figure BDA00015717004400000512
S43：计算k+1时刻***状态协方差的预测值：
Figure BDA00015717004400000513
其中,Pk为k时刻***状态协方差，Q为***位置观测值误差协方差，T为矩阵转置，Ts为采样周期；
S44：计算k+1时刻扩展卡尔曼滤波增益：
Figure BDA0001571700440000061
其中，Hk为***观测方程Zk的雅可比矩阵，R为观测方程Zk的误差协方差；
S45：更新k+1时刻***状态协方差
Figure BDA0001571700440000062
其中，I为单位矩阵；
S46：更新k+1时刻***状态估计值；
Figure BDA0001571700440000063
其中，
Figure BDA0001571700440000064
Zk+1为k+1时刻***观测方程的观测值；
本发明的有益效果如下：
在扩展卡尔曼滤波的每次迭代过程中，通过贝叶斯滤波的方法将扩展卡尔曼滤波得到的目标车辆预测位置的概率密度函数与相邻车辆位置的概率密度函数以及相对距离的概率密度函数相结合，计算出目标车辆在观测到相邻车辆之后的后验概率密度函数，在根据后验概率中的最大值，求出目标车辆最终的位置估计值，实现多车辆协作定位，降低车辆的位置误差，从而得到更加精确的车辆位置信息。
附图说明
图1为本发明实施例提供的一种贝叶斯滤波的多车辆协作定位方法的流程示意图；
图2为本发明实施例提供的多车辆协作***模型示意图；
图3为本发明实施例测量的位置误差与GPS测量的位置误差对比示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细描述。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，在下文对本发明的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也可以完全理解本发明。
下面结合附图和具体实施例对本发明作进一步说明，但不作为对本发明的限定。下面为本发明的举出最佳实施例：
如图1-图3所示，本发明提供一种贝叶斯滤波的多车辆协作定位方法，所述方法通过获取当前时刻目标车辆观测到相邻车辆时，目标车辆的位置测量值以及相邻车辆的数目，并构造***的状态方程；获取目标车辆和相邻车辆在当前时刻的相对位置信息以及相邻车辆的数目，并构造***的观测方程；将***的运动方程和观测方程代入到扩展卡尔曼滤波中，得到***的位置预测值矩阵以及***位置估计值矩阵，并计算***位置预测值的概率密度函数矩阵以及***相对距离的概率密度函数矩阵；将***位置预测值的概率密度函数矩阵和***相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵；根据最大后验概率估计得到目标车辆的位置估计值。这样，在扩展卡尔曼滤波的每次迭代过程中，通过贝叶斯滤波的方法将扩展卡尔曼滤波得到的目标车辆预测位置的概率密度函数与相邻车辆位置的概率密度函数以及相对距离的概率密度函数相结合，计算出目标车辆在观测到相邻车辆之后的后验概率密度函数，在根据后验概率中的最大值，求出目标车辆最终的位置估计值，实现多车辆协作定位，降低车辆的位置误差，从而得到更加精确的车辆位置信息。
本发明实施例所述的一种贝叶斯滤波的多车辆协作定位方法，在信噪比较低，噪声较大的环境下，估计的位置结果比GPS测量结果更加准确，有效地提高了车辆的定位精度。在实际应用中，目标车辆周围的相邻车辆是不断变化的，本发明可以根据相邻车辆的实际情况自适应地选择相邻车辆个数以及相应的相邻车辆的概率密度函数。
本实施例中，目标车辆V0可以通过车载导航(例如，GPS设备或北斗设备)获取k时刻目标车辆V0的位置测量值；同时，目标车辆V0可以通过短程距离通信(Dedicated Short-Range Communication，DSRC)获取k时刻相邻车辆Vj的位置观测值。
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述获取当前时刻目标车辆观测到相邻车辆时，目标车辆的位置测量值以及相邻车辆的数目，并构造***的状态方程包括：
获取k时刻目标车辆X0的位置测量值
Figure BDA0001571700440000081
其中，k时刻表示当前时刻，T表示转置，
Figure BDA0001571700440000082
表示k时刻目标车辆X0在x轴方向的坐标，
Figure BDA0001571700440000091
表示k时刻目标车辆X0在y轴方向的坐标，
Figure BDA0001571700440000092
表示k时刻目标车辆X0运动方向与x轴方向所形成的夹角；
获取所述相邻车辆的数目N；
根据观测到的相邻车辆的数目N，构造***的状态方程
Figure BDA0001571700440000093
其中，
Figure BDA0001571700440000094
表示k时刻目标车辆V0观测到相邻车辆Vj时，目标车辆V0相对于相邻车辆Vj的位置。
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述获取目标车辆和相邻车辆在当前时刻的相对位置信息以及相邻车辆的数目，并构造***的观测方程包括：
获取k时刻相邻车辆Xj的位置测量值
Figure BDA0001571700440000095
其中，N表示相邻车辆的数目，xjk表示k时刻相邻车辆Xj在x轴方向的坐标，yjk表示k时刻相邻车辆Xj在y轴方向的坐标，θjk表示k时刻相邻车辆Xj运动方向与x轴所形成的夹角。
计算k时刻目标车辆X0和相邻车辆Xj之间的相对位置信息
Figure BDA0001571700440000096
表示如下；
Figure BDA0001571700440000097
其中，
Figure BDA0001571700440000098
表示为目标车辆X0与相邻车辆Xj之间的相对距离，
Figure BDA0001571700440000099
表示为目标车辆X0与相邻车辆Xj之间的相对方位角；
根据计算得到的k时刻目标车辆X0和相邻车辆Xj之间的相对位置信息
Figure BDA0001571700440000101
构造***的观测方程
Figure BDA0001571700440000102
其中，N表示相邻车辆的数目。
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述将***的运动方程和观测方程代入到扩展卡尔曼滤波中，得到***的位置预测值矩阵以及***位置估计值矩阵，并计算***位置预测值的概率密度函数矩阵以及***相对距离的概率密度函数矩阵包括：
将状态方程Xk和观测方程Zk代入到扩展卡尔曼滤波中，得到目标车辆X0的位置预测值矩阵
Figure BDA0001571700440000103
以及位置估计值矩阵
Figure BDA0001571700440000104
计算当前时刻***位置预测值
Figure BDA0001571700440000105
的概率密度函数矩阵
Figure BDA0001571700440000106
以及***相对距离
Figure BDA0001571700440000107
的概率密度函数矩阵
Figure BDA0001571700440000108
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述将***位置预测值的概率密度函数矩阵和***相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵包括：
将当前时刻***位置预测值
Figure BDA0001571700440000109
的概率密度函数矩阵
Figure BDA00015717004400001010
和***相对距离
Figure BDA00015717004400001011
的概率密度函数矩阵
Figure BDA00015717004400001012
以及相邻车辆位置的概率密度函数
Figure BDA00015717004400001013
代入到贝叶斯滤波中，得到后验概率密度矩阵
Figure BDA00015717004400001014
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述根据最大后验概率估计得到目标车辆的位置估计值包括：
求得后验概率密度矩阵
Figure BDA0001571700440000111
中最大后验概率值以及最大后验概率值所对应的行数Rmax，将最大行数Rmax所对应的位置估计值作为目标车辆最终的位置估计值
Figure BDA0001571700440000112
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述将***的运动方程和观测方程代入到扩展卡尔曼滤波中，得到***的位置预测值矩阵以及***位置估计值矩阵，并计算***位置预测值的概率密度函数矩阵以及***相对距离的概率密度函数矩阵包括：
S1、计算k+1时刻***状态预测值
Figure BDA0001571700440000113
和***观测方程预测值
Figure BDA0001571700440000114
其中，
所述
Figure BDA0001571700440000115
表示为：
Figure BDA0001571700440000116
其中，Ak为***状态Xk的雅可比矩阵，Bk为***输入Uk的雅可比矩阵，W为***的过程激励噪声；
所述
Figure BDA0001571700440000117
表示为：
Figure BDA0001571700440000118
其中，
Figure BDA0001571700440000119
S2、计算k+1时刻***状态预测值
Figure BDA00015717004400001110
的概率密度函数矩阵
Figure BDA0001571700440000121
以及***相对距离
Figure BDA0001571700440000122
的概率密度函数矩阵
Figure BDA0001571700440000123
S3、计算k+1时刻***状态协方差的预测值
Figure BDA0001571700440000124
其中,Pk为k时刻***状态协方差，Q为***位置观测值误差协方差，T为矩阵转置，Ts为采样周期；
S4、计算k+1时刻扩展卡尔曼滤波增益
Figure BDA0001571700440000125
其中，Hk为***观测方程Zk的雅可比矩阵，R为观测方程Zk的误差协方差；
S5、更新k+1时刻***状态协方差
Figure BDA0001571700440000126
其中，I为单位矩阵；
S6、更新k+1时刻***状态估计值
Figure BDA0001571700440000127
其中
Figure BDA0001571700440000128
Zk+1为k+1时刻***观测方程的观测值；
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述将***位置预测值的概率密度函数矩阵和***相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵包括：
S1、根据贝叶斯公式，目标车辆V0观测到N个相邻车辆Vj之后的后验概率密度函数表示为：
Figure BDA0001571700440000129
其中，Yj表示目标车辆V0测量到相邻车辆Vj的位置Xj，并且得到两辆车之间的相对距离Dj，即Yj＝{Xj,Dj}，
Figure BDA0001571700440000131
表示目标车辆V0的位置的概率密度函数，
Figure BDA0001571700440000132
表示相邻车辆相对于目标车辆V0的位置的概率密度函数，
Figure BDA0001571700440000133
表示N个相邻车辆的位置的概率密度函数。
S2、根据全概率公式，可将后验概率密度函数表示为：
Figure BDA0001571700440000134
S3、根据目标车辆V0测量到相邻车辆Vj之间是相互独立，可将后验概率密度函数表示为：
Figure BDA0001571700440000135
S4、根据Yj表示为目标车辆V0测量到相邻车辆Vj的位置Xj，并且得到两辆车之间的相对距离Dj可得
Figure BDA0001571700440000136
S5、根据概率
Figure BDA0001571700440000137
可将后验概率密度函数表示为：
Figure BDA0001571700440000138
S6、根据***位置预测值的概率密度函数矩阵
Figure BDA0001571700440000139
和***相对距离的概率密度函数矩阵
Figure BDA00015717004400001310
以及相邻车辆位置的密度函数
Figure BDA00015717004400001311
计算k+1时刻后验概率密度矩阵
Figure BDA00015717004400001312
表示如下：
Figure BDA0001571700440000141
其中，
Figure BDA0001571700440000142
如图2所示，以具体的例子对本实施例所述的一种贝叶斯滤波的多车辆协作定位方法进行详细说明，并使用matlab仿真平台，对本实施例所述的一种贝叶斯滤波的多车辆协作定位方法的性能进行仿真分析：
步骤1，如图2所示，考虑到多车辆***中一个目标车辆V0观测到周边有四个相邻车辆V1,V2,V3,V4，行驶在道路上，目标车辆V0通过自身GPS设备接收到k时刻目标车辆V0相对于相邻车辆V1,V2,V3,V4时，目标车辆V0的位置测量值
Figure BDA0001571700440000143
则可构造***的状态方程为
Figure BDA0001571700440000144
步骤2，计算目标车辆V0与相邻车辆V1,V2,V3,V4之间的相对位置信息，构造观测方程
Figure BDA0001571700440000145
具体的，步骤2可以包括：
2.1)目标车辆V0通过DSRC获得相邻车辆Vj的位置信息
Figure BDA0001571700440000146
2.2)计算目标车辆V0与相邻车辆Vj之间的相对位置信息
Figure BDA0001571700440000151
表示为：
Figure BDA0001571700440000152
其中，
Figure BDA0001571700440000153
表示为目标车辆X0与相邻车辆Xj之间的相对距离，
Figure BDA0001571700440000154
表示为目标车辆X0与相邻车辆Xj之间的相对方位角；
2.3)根据相邻车辆的个数N＝4，构造***的观测方程
Figure BDA0001571700440000155
步骤3，将步骤1得到的***状态方程Xk和步骤5得到的观测方程Zk代入到扩展卡尔曼滤波中，得到目标车辆X0的位置预测值矩阵
Figure BDA0001571700440000156
以及位置估计值矩阵
Figure BDA0001571700440000157
并计算当前时刻***位置预测值
Figure BDA0001571700440000158
的概率密度函数矩阵
Figure BDA0001571700440000159
以及***相对距离
Figure BDA00015717004400001510
的概率密度函数矩阵
Figure BDA00015717004400001511
具体的，步骤3可以包括：
3.1)计算k+1时刻***状态预测值
Figure BDA00015717004400001512
和***观测方程预测值
Figure BDA00015717004400001513
Figure BDA00015717004400001514
其中，Ak为***状态Xk的雅可比矩阵，Bk为***输入Uk的雅可比矩阵，W为***的过程激励噪声；
Figure BDA00015717004400001515
其中，
Figure BDA0001571700440000161
3.2)计算k+1时刻***状态预测值
Figure BDA0001571700440000162
的概率密度函数矩阵
Figure BDA0001571700440000163
以及***相对距离
Figure BDA0001571700440000164
的概率密度函数矩阵
Figure BDA0001571700440000165
3.3)计算k+1时刻***状态协方差的预测值
Figure BDA0001571700440000166
Figure BDA0001571700440000167
其中,Pk为k时刻***状态协方差，Q为***位置观测值误差协方差，T为矩阵转置，Ts为采样周期；
3.4)计算k+1时刻扩展卡尔曼滤波增益Kk+1：
Figure BDA0001571700440000168
其中，Hk为***观测方程Zk的雅可比矩阵，R为观测方程Zk的误差协方差；
3.5)更新k+1时刻***状态协方差Pk+1：
Figure BDA0001571700440000169
其中，I为单位矩阵；
3.6)更新k+1时刻***状态估计值
Figure BDA00015717004400001610
Figure BDA00015717004400001611
其中，
Figure BDA00015717004400001612
Zk+1为k+1时刻***观测方程的观测值；
步骤4，将步骤3得到的***位置预测值的概率密度函数矩阵
Figure BDA0001571700440000171
和***相对距离的概率密度函数矩阵
Figure BDA0001571700440000172
以及相邻车辆位置的概率密度函数
Figure BDA0001571700440000173
代入到贝叶斯滤波中，得到后验概率密度矩阵
Figure BDA0001571700440000174
具体的，步骤3可以包括：
4.1)根据贝叶斯公式，目标车辆V0观测到4个相邻车辆Vj之后的后验概率密度函数表示为：
Figure BDA0001571700440000175
其中，Yj(j＝1,2,3,4)表示目标车辆V0测量到相邻车辆Vj的位置Xj，并且得到两辆车之间的相对距离Dj，即Yj＝{Xj,Dj}(j＝1,2,3,4)，
Figure BDA0001571700440000176
表示目标车辆V0的位置的概率密度函数，
Figure BDA0001571700440000177
表示相邻车辆相对于目标车辆V0的位置的概率密度函数，
Figure BDA0001571700440000178
表示4个相邻车辆的位置的概率密度函数。
4.2)根据全概率公式，可将4.1)中的后验概率密度函数表示为：
Figure BDA0001571700440000179
4.3)根据目标车辆V0测量到相邻车辆Vj之间是相互独立，可将4.2)中的后验概率密度函数表示为：
Figure BDA00015717004400001710
4.4)根据Yj表示为目标车辆V0测量到相邻车辆Vj的位置Xj，并且得到两辆车之间的相对距离Dj可得
Figure BDA0001571700440000181
4.5)根据4.4)得到的概率
Figure BDA0001571700440000182
可将4.3)中的后验概率密度函数表示为：
Figure BDA0001571700440000183
4.6)将***位置预测值的概率密度函数矩阵
Figure BDA0001571700440000184
和***相对距离的概率密度函数矩阵
Figure BDA0001571700440000185
以及相邻车辆位置的密度函数
Figure BDA0001571700440000186
代入到4.5)所示的后验概率密度函数表达式中，计算k+1时刻后验概率密度矩阵
Figure BDA0001571700440000187
表示如下：
Figure BDA0001571700440000188
其中，
Figure BDA0001571700440000189
步骤5，根据步骤4得到的k+1时刻后验概率密度矩阵
Figure BDA00015717004400001810
得到矩阵中最大后验概率值以及最大后验概率值所对应的行数Rmax，将最大行数Rmax所对应的位置估计矩阵
Figure BDA00015717004400001811
中的估计值作为目标车辆最终的位置估计值
Figure BDA0001571700440000191
本实施例中，如图3所示，图3描述了本发明实施例测量的位置误差与GPS测量的位置误差对比示意图，图3中的横坐标是采样时间，纵坐标是位置误差大小，该结果是在信噪比为3dB下进行1000次的同条件重复实验得出的。由图3可知，本发明实施例提出的方法显著降低了位置误差，并给出了更加准确的位置估计值。这说明本发明实施例提出的基于扩展卡尔曼滤波和贝叶斯滤波的多车辆协作定位方法是准确并且有效的，同时对比结果也表明本发明采用的方法性能优于GPS测量的性能。
以上所述的实施例，只是本发明较优选的具体实施方式的一种，本领域的技术人员在本发明技术方案范围内进行的通常变化和替换都应包含在本发明的保护范围内。

Claims (7)

1.一种贝叶斯滤波的多车辆协作定位方法，其特征在于，所述方法利用目标车辆与相邻车辆位置的概率密度函数，以及车辆之间相对距离的概率密度函数，在扩展卡尔曼滤波的每次迭代过程中，通过贝叶斯滤波，得到目标车辆位置的后验概率估计，再根据最大后验估计值，确定目标车辆最终的位置估计，通过双重滤波，实现协作定位，所述方法包括：
S1：获取当前时刻目标车辆观测到相邻车辆时，目标车辆的位置测量值以及相邻车辆的数目，并构造***的状态方程；
S2：获取目标车辆和相邻车辆在当前时刻的相对位置信息以及相邻车辆的数目，并构造***的观测方程；
S3：将***的状态方程和观测方程代入到扩展卡尔曼滤波中，得到***的状态预测值矩阵以及***位置估计值矩阵，并计算***状态预测值的概率密度函数矩阵以及***相对距离的概率密度函数矩阵；
S4：将***状态预测值的概率密度函数矩阵和***相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵；
S5：根据最大后验概率估计得到目标车辆的位置估计值。
2.根据权利要求1所述的方法，其特征在于，所述S1中构造***的状态方程包括：
S11：获取k时刻目标车辆X0的位置测量值
Figure FDA0002546433500000011
其中，k时刻表示当前时刻，T表示转置，
Figure FDA0002546433500000012
表示k时刻目标车辆X0在x轴方向的坐标，
Figure FDA0002546433500000013
表示k时刻目标车辆X0在y轴方向的坐标，
Figure FDA0002546433500000021
表示k时刻目标车辆X0运动方向与x轴方向所形成的夹角；
S12：获取所述相邻车辆的数目N；
S13：根据观测到的相邻车辆的数目N，构造***的状态方程
Figure FDA0002546433500000022
其中，
Figure FDA0002546433500000023
表示k时刻目标车辆X0观测到相邻车辆Xj时，目标车辆X0相对于相邻车辆Xj的位置，j＝1，2....N。
3.根据权利要求2所述的方法，其特征在于，所述S2中构造***的观测方程包括：
S21：获取k时刻相邻车辆Xj的位置测量值；
Figure FDA0002546433500000024
其中，j＝1，2....N，N表示相邻车辆的数目，
Figure FDA0002546433500000025
表示k时刻相邻车辆Xj在x轴方向的坐标，
Figure FDA0002546433500000026
表示k时刻相邻车辆Xj在y轴方向的坐标，
Figure FDA0002546433500000027
表示k时刻相邻车辆Xj运动方向与x轴所形成的夹角；
S22：计算k时刻目标车辆X0和相邻车辆Xj之间的相对位置信息
Figure FDA0002546433500000028
表示如下；
Figure FDA0002546433500000029
j＝1，2....N，N表示相邻车辆的数目，
Figure FDA00025464335000000210
表示为目标车辆X0与相邻车辆Xj之间的相对距离，
Figure FDA00025464335000000211
表示为目标车辆X0与相邻车辆Xj之间的相对方位角；
S23：根据计算得到的k时刻目标车辆X0和相邻车辆Xj之间的相对位置信息
Figure FDA0002546433500000031
构造***的观测方程；
Figure FDA0002546433500000032
其中，j＝1，2....N，N表示相邻车辆的数目。
4.根据权利要求2所述的方法，其特征在于，所述S3具体包括：
S31：将状态方程Xk和观测方程Zk代入到扩展卡尔曼滤波中，得到目标车辆X0的状态预测值矩阵
Figure FDA0002546433500000033
以及位置估计值矩阵
Figure FDA0002546433500000034
S32：计算当前时刻***状态预测值
Figure FDA0002546433500000035
的概率密度函数矩阵
Figure FDA0002546433500000036
以及***相对距离
Figure FDA0002546433500000037
的概率密度函数矩阵
Figure FDA0002546433500000038
5.根据权利要求2所述的方法，其特征在于，所述S4中后验概率密度矩阵计算方法如下：
将当前时刻***状态预测值
Figure FDA0002546433500000039
的概率密度函数矩阵
Figure FDA00025464335000000310
和***相对距离
Figure FDA00025464335000000311
的概率密度函数矩阵
Figure FDA00025464335000000312
以及相邻车辆位置的概率密度函数
Figure FDA00025464335000000313
代入到贝叶斯滤波中，得到后验概率密度矩阵
Figure FDA00025464335000000314
6.根据权利要求2所述的方法，其特征在于，所述S5中根据最大后验概率估计得到目标车辆的位置估计值包括：
得到后验概率密度矩阵
Figure FDA00025464335000000315
中最大后验概率值以及最大后验概率值所对应的行数Rmax，将最大行数Rmax所对应的位置估计值作为目标车辆最终的位置估计值
Figure FDA00025464335000000316
7.根据权利要求5所述的方法，其特征在于，所述S4具体包括：
S41：计算k+1时刻***状态预测值
Figure FDA0002546433500000041
和***观测方程预测值
Figure FDA0002546433500000042
其中，
Figure FDA0002546433500000043
表示为
Figure FDA0002546433500000044
其中，Ak为***状态方程Xk的雅可比矩阵，Bk为***输入Uk的雅可比矩阵，W为***的过程激励噪声；
所述
Figure FDA0002546433500000045
表示为：
Figure FDA0002546433500000046
其中，
Figure FDA0002546433500000047
其中，j＝1，2....N，N表示相邻车辆的数目，
Figure FDA0002546433500000048
为在k+1时刻，目标车辆X0的坐标；
Figure FDA0002546433500000049
为在k+1时刻相邻车辆Xj的坐标；
S42：计算k+1时刻***状态预测值
Figure FDA00025464335000000410
的概率密度函数矩阵
Figure FDA00025464335000000411
以及***相对距离
Figure FDA00025464335000000412
的概率密度函数矩阵
Figure FDA00025464335000000413
S43：计算k+1时刻***状态协方差的预测值：
Figure FDA00025464335000000414
其中,Pk为k时刻***状态协方差，Q为***位置观测值误差协方差，T为矩阵转置，Ts为采样周期；
S44：计算k+1时刻扩展卡尔曼滤波增益：
Figure FDA00025464335000000415
其中，Hk为***观测方程Zk的雅可比矩阵，R为观测方程Zk的误差协方差；
S45：更新k+1时刻***状态协方差
Figure FDA0002546433500000051
其中，I为单位矩阵；
S46：更新k+1时刻***位置估计值；
Figure FDA0002546433500000052
其中，
Figure FDA0002546433500000053
j＝1，2....N，N表示相邻车辆的数目，Zk+1为k+1时刻***观测方程的观测值。
CN201810119662.9A 2018-02-06 2018-02-06 一种贝叶斯滤波的多车辆协作定位方法 Expired - Fee Related CN108398704B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810119662.9A CN108398704B (zh) 2018-02-06 2018-02-06 一种贝叶斯滤波的多车辆协作定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810119662.9A CN108398704B (zh) 2018-02-06 2018-02-06 一种贝叶斯滤波的多车辆协作定位方法

Publications (2)

Publication Number Publication Date
CN108398704A CN108398704A (zh) 2018-08-14
CN108398704B true CN108398704B (zh) 2020-11-06

Family

ID=63095871

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810119662.9A Expired - Fee Related CN108398704B (zh) 2018-02-06 2018-02-06 一种贝叶斯滤波的多车辆协作定位方法

Country Status (1)

Country Link
CN (1) CN108398704B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109996205B (zh) * 2019-04-12 2021-12-07 成都工业学院 传感器数据融合方法、装置、电子设备及存储介质
CN113706854B (zh) * 2021-08-20 2023-03-07 北京科技大学 一种智能车联网中的车辆协作定位方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101256715A (zh) * 2008-03-05 2008-09-03 中科院嘉兴中心微***所分中心 无线传感器网络中基于粒子滤波的多车辆声信号分离方法
CN101644758A (zh) * 2009-02-24 2010-02-10 中国科学院声学研究所 一种目标定位跟踪***及方法
CN105657822A (zh) * 2014-12-02 2016-06-08 英特尔公司 使用混合滤波器的增强定位***
CN105898865A (zh) * 2016-06-17 2016-08-24 杭州电子科技大学 非线性非高斯条件下基于ekf和pf的协同定位方法
CN106525042A (zh) * 2016-09-27 2017-03-22 哈尔滨工程大学 一种基于蚁群与扩展卡尔曼滤波相结合的多auv协同定位方法
DE102016011366A1 (de) * 2016-09-21 2017-04-06 Daimler Ag Verfahren zur Positionsbestimmung eines Fahrzeugs
CN107064865A (zh) * 2017-04-07 2017-08-18 杭州电子科技大学 基于深度聚类的极坐标动态规划无源协同定位方法
CN107255795A (zh) * 2017-06-13 2017-10-17 山东大学 基于ekf/efir混合滤波的室内移动机器人定位方法和装置
CN107274721A (zh) * 2017-06-07 2017-10-20 北京科技大学 一种智能交通***中多车辆协作定位方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101256715A (zh) * 2008-03-05 2008-09-03 中科院嘉兴中心微***所分中心 无线传感器网络中基于粒子滤波的多车辆声信号分离方法
CN101644758A (zh) * 2009-02-24 2010-02-10 中国科学院声学研究所 一种目标定位跟踪***及方法
CN105657822A (zh) * 2014-12-02 2016-06-08 英特尔公司 使用混合滤波器的增强定位***
US9841491B2 (en) * 2014-12-02 2017-12-12 Intel Corporation Enhanced positioning system using hybrid filter
CN105898865A (zh) * 2016-06-17 2016-08-24 杭州电子科技大学 非线性非高斯条件下基于ekf和pf的协同定位方法
DE102016011366A1 (de) * 2016-09-21 2017-04-06 Daimler Ag Verfahren zur Positionsbestimmung eines Fahrzeugs
CN106525042A (zh) * 2016-09-27 2017-03-22 哈尔滨工程大学 一种基于蚁群与扩展卡尔曼滤波相结合的多auv协同定位方法
CN107064865A (zh) * 2017-04-07 2017-08-18 杭州电子科技大学 基于深度聚类的极坐标动态规划无源协同定位方法
CN107274721A (zh) * 2017-06-07 2017-10-20 北京科技大学 一种智能交通***中多车辆协作定位方法
CN107255795A (zh) * 2017-06-13 2017-10-17 山东大学 基于ekf/efir混合滤波的室内移动机器人定位方法和装置

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Bayesian Filters for Location Estimation;Dieter Fox,et al;《 IEEE CS and IEEE ComSoc 》;20031231;p24-33 *
基于EKF和PF的多机器人协同定位技术;田红兵等;《现代电子技术》;20131201;第95-98页 *
多机器人协作定位算法研究与仿真;白玉洁;《中国优秀硕士学位论文全文数据库 信息科技辑》;20140915;全文 *

Also Published As

Publication number Publication date
CN108398704A (zh) 2018-08-14

Similar Documents

Publication Publication Date Title
CN107274721B (zh) 一种智能交通***中多车辆协作定位方法
CN109946731B (zh) 一种基于模糊自适应无迹卡尔曼滤波的车辆高可靠融合定位方法
Alam et al. Relative positioning enhancement in VANETs: A tight integration approach
CN110176153B (zh) 一种基于边缘计算的盲区车辆碰撞预警方法
CN107315413B (zh) 一种车车通信环境下考虑车辆间相对位置的多车协同定位算法
Liu et al. Improving positioning accuracy using GPS pseudorange measurements for cooperative vehicular localization
Ghaleb et al. Improved vehicle positioning algorithm using enhanced innovation-based adaptive Kalman filter
CN105682222B (zh) 一种基于车辆自组织网络的车辆位置定位信息融合方法
CN101285686B (zh) 一种农业机械导航分级定位的方法和***
US20100295726A1 (en) Global positioning system error correction, vehicle tracking and object location
CN107247275B (zh) 基于公交车的城市gnss脆弱性监测***及其方法
CN104677361B (zh) 一种综合定位的方法
JP2011209268A (ja) 位置推定装置及びプログラム
CN109946648B (zh) 一种车路协同下基于超宽带的车辆高精度定位方法
CN106772516A (zh) 一种基于模糊理论的复合定位新方法
CN108885269B (zh) 导航方法、导航装置和导航***
CN108398704B (zh) 一种贝叶斯滤波的多车辆协作定位方法
CN109343096B (zh) 基于gsm-r通信***的gnss辅助相对导航方法及***
Baek et al. Accurate vehicle position estimation using a Kalman filter and neural network-based approach
Dasanayaka et al. Analysis of vehicle location prediction errors for safety applications in cooperative-intelligent transportation systems
Ahammed et al. LICA: robust localization using cluster analysis to improve GPS coordinates
Liu et al. A hybrid integrity monitoring method using vehicular wireless communication in difficult environments for GNSS
Kong et al. An accurate and reliable positioning methodology for land vehicles in tunnels based on UWB/INS integration
Hsu et al. Intelligent viaduct recognition and driving altitude determination using GPS data
Soatti et al. Enhanced vehicle positioning in cooperative ITS by joint sensing of passive features

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20201106

CF01 Termination of patent right due to non-payment of annual fee