CN108398704A - 一种贝叶斯滤波的多车辆协作定位方法 - Google Patents
一种贝叶斯滤波的多车辆协作定位方法 Download PDFInfo
- Publication number
- CN108398704A CN108398704A CN201810119662.9A CN201810119662A CN108398704A CN 108398704 A CN108398704 A CN 108398704A CN 201810119662 A CN201810119662 A CN 201810119662A CN 108398704 A CN108398704 A CN 108398704A
- Authority
- CN
- China
- Prior art keywords
- target vehicle
- probability density
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 230000004807 localization Effects 0.000 title abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 102
- 238000001914 filtration Methods 0.000 claims description 34
- 238000005259 measurement Methods 0.000 claims description 14
- 238000005070 sampling Methods 0.000 claims description 5
- 230000005284 excitation Effects 0.000 claims description 4
- 230000017105 transposition Effects 0.000 claims description 4
- 238000009499 grossing Methods 0.000 abstract 1
- 238000012804 iterative process Methods 0.000 abstract 1
- 238000005516 engineering process Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 101001093748 Homo sapiens Phosphatidylinositol N-acetylglucosaminyltransferase subunit P Proteins 0.000 description 1
- 206010039203 Road traffic accident Diseases 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000013398 bayesian method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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的位置测量值
其中，k时刻表示当前时刻，T表示转置，表示k时刻目标车辆X0在x轴方向的坐标，表示k时刻目标车辆X0在y轴方向的坐标，表示k时刻目标车辆X0运动方向与x轴方向所形成的夹角；
S12：获取所述相邻车辆的数目N；
S13：根据观测到的相邻车辆的数目N，构造系统的状态方程
其中，表示k时刻目标车辆V0观测到相邻车辆Vj时，目标车辆V0相对于相邻车辆Vj的位置；
进一步地，所述S2中构造系统的观测方程包括：
S21：获取k时刻相邻车辆Xj的位置测量值
其中，N表示相邻车辆的数目，xjk表示k时刻相邻车辆Xj在x轴方向的坐标，yjk表示k时刻相邻车辆Xj在y轴方向的坐标，θjk表示k时刻相邻车辆Xj运动方向与x轴所形成的夹角；
S22：计算k时刻目标车辆X0和相邻车辆Xj之间的相对位置信息表示如下；
其中，表示为目标车辆X0与相邻车辆Xj之间的相对距离，表示为目标车辆X0与相邻车辆Xj之间的相对方位角；
S23：根据计算得到的k时刻目标车辆X0和相邻车辆Xj之间的相对位置信息构造系统的观测方程；
其中，N表示相邻车辆的数目；
进一步地，所述S3具体包括：
S31：将状态方程Xk和观测方程Zk代入到扩展卡尔曼滤波中，得到目标车辆X0的位置预测值矩阵以及位置估计值矩阵
S32：计算当前时刻系统位置预测值的概率密度函数矩阵以及系统相对距离的概率密度函数矩阵
进一步地，所述S4中后验概率密度矩阵计算方法如下：
将当前时刻系统位置预测值的概率密度函数矩阵和系统相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵
进一步地，所述S5中根据最大后验概率估计得到目标车辆的位置估计值包括：
得到后验概率密度矩阵中最大后验概率值以及最大后验概率值所对应的行数Rmax，将最大行数Rmax所对应的位置估计值作为目标车辆最终的位置估计值
进一步地，所述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时刻系统状态协方差其中，I为单位矩阵；
S46：更新k+1时刻系统状态估计值；
其中，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的位置测量值
其中，k时刻表示当前时刻，T表示转置，表示k时刻目标车辆X0在x轴方向的坐标，表示k时刻目标车辆X0在y轴方向的坐标，表示k时刻目标车辆X0运动方向与x轴方向所形成的夹角；
获取所述相邻车辆的数目N；
根据观测到的相邻车辆的数目N，构造系统的状态方程
其中，表示k时刻目标车辆V0观测到相邻车辆Vj时，目标车辆V0相对于相邻车辆Vj的位置。
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述获取目标车辆和相邻车辆在当前时刻的相对位置信息以及相邻车辆的数目，并构造系统的观测方程包括：
获取k时刻相邻车辆Xj的位置测量值
其中，N表示相邻车辆的数目，xjk表示k时刻相邻车辆Xj在x轴方向的坐标，yjk表示k时刻相邻车辆Xj在y轴方向的坐标，θjk表示k时刻相邻车辆Xj运动方向与x轴所形成的夹角。
计算k时刻目标车辆X0和相邻车辆Xj之间的相对位置信息表示如下；
其中，表示为目标车辆X0与相邻车辆Xj之间的相对距离，表示为目标车辆X0与相邻车辆Xj之间的相对方位角；
根据计算得到的k时刻目标车辆X0和相邻车辆Xj之间的相对位置信息构造系统的观测方程其中，N表示相邻车辆的数目。
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到系统的位置预测值矩阵以及系统位置估计值矩阵，并计算系统位置预测值的概率密度函数矩阵以及系统相对距离的概率密度函数矩阵包括：
将状态方程Xk和观测方程Zk代入到扩展卡尔曼滤波中，得到目标车辆X0的位置预测值矩阵以及位置估计值矩阵
计算当前时刻系统位置预测值的概率密度函数矩阵以及系统相对距离的概率密度函数矩阵
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述将系统位置预测值的概率密度函数矩阵和系统相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵包括：
将当前时刻系统位置预测值的概率密度函数矩阵和系统相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述根据最大后验概率估计得到目标车辆的位置估计值包括：
求得后验概率密度矩阵中最大后验概率值以及最大后验概率值所对应的行数Rmax，将最大行数Rmax所对应的位置估计值作为目标车辆最终的位置估计值
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到系统的位置预测值矩阵以及系统位置估计值矩阵，并计算系统位置预测值的概率密度函数矩阵以及系统相对距离的概率密度函数矩阵包括：
S1、计算k+1时刻系统状态预测值和系统观测方程预测值其中，
所述表示为：
其中，Ak为系统状态Xk的雅可比矩阵，Bk为系统输入Uk的雅可比矩阵，W为系统的过程激励噪声；
所述表示为：
其中，
S2、计算k+1时刻系统状态预测值的概率密度函数矩阵以及系统相对距离的概率密度函数矩阵
S3、计算k+1时刻系统状态协方差的预测值其中,Pk为k时刻系统状态协方差，Q为系统位置观测值误差协方差，T为矩阵转置，Ts为采样周期；
S4、计算k+1时刻扩展卡尔曼滤波增益其中，Hk为系统观测方程Zk的雅可比矩阵，R为观测方程Zk的误差协方差；
S5、更新k+1时刻系统状态协方差其中，I为单位矩阵；
S6、更新k+1时刻系统状态估计值其中
Zk+1为k+1时刻系统观测方程的观测值；
在前述一种贝叶斯滤波的多车辆协作定位方法的具体实施方式中，进一步地，所述将系统位置预测值的概率密度函数矩阵和系统相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵包括：
S1、根据贝叶斯公式，目标车辆V0观测到N个相邻车辆Vj之后的后验概率密度函数表示为：
其中，Yj表示目标车辆V0测量到相邻车辆Vj的位置Xj，并且得到两辆车之间的相对距离Dj，即Yj＝{Xj,Dj}，表示目标车辆V0的位置的概率密度函数，表示相邻车辆相对于目标车辆V0的位置的概率密度函数，表示N个相邻车辆的位置的概率密度函数。
S2、根据全概率公式，可将后验概率密度函数表示为：
S3、根据目标车辆V0测量到相邻车辆Vj之间是相互独立，可将后验概率密度函数表示为：
S4、根据Yj表示为目标车辆V0测量到相邻车辆Vj的位置Xj，并且得到两辆车之间的相对距离Dj可得
S5、根据概率可将后验概率密度函数表示为：
S6、根据系统位置预测值的概率密度函数矩阵和系统相对距离的概率密度函数矩阵以及相邻车辆位置的密度函数计算k+1时刻后验概率密度矩阵表示如下：
其中，
如图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可以包括：
2.1)目标车辆V0通过DSRC获得相邻车辆Vj的位置信息
2.2)计算目标车辆V0与相邻车辆Vj之间的相对位置信息表示为：
其中，表示为目标车辆X0与相邻车辆Xj之间的相对距离，表示为目标车辆X0与相邻车辆Xj之间的相对方位角；
2.3)根据相邻车辆的个数N＝4，构造系统的观测方程
步骤3，将步骤1得到的系统状态方程Xk和步骤5得到的观测方程Zk代入到扩展卡尔曼滤波中，得到目标车辆X0的位置预测值矩阵以及位置估计值矩阵并计算当前时刻系统位置预测值的概率密度函数矩阵以及系统相对距离的概率密度函数矩阵具体的，步骤3可以包括：
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时刻系统状态估计值
其中，Zk+1为k+1时刻系统观测方程的观测值；
步骤4，将步骤3得到的系统位置预测值的概率密度函数矩阵和系统相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵具体的，步骤3可以包括：
4.1)根据贝叶斯公式，目标车辆V0观测到4个相邻车辆Vj之后的后验概率密度函数表示为：
其中，Yj(j＝1,2,3,4)表示目标车辆V0测量到相邻车辆Vj的位置Xj，并且得到两辆车之间的相对距离Dj，即Yj＝{Xj,Dj}(j＝1,2,3,4)，表示目标车辆V0的位置的概率密度函数，表示相邻车辆相对于目标车辆V0的位置的概率密度函数，表示4个相邻车辆的位置的概率密度函数。
4.2)根据全概率公式，可将4.1)中的后验概率密度函数表示为：
4.3)根据目标车辆V0测量到相邻车辆Vj之间是相互独立，可将4.2)中的后验概率密度函数表示为：
4.4)根据Yj表示为目标车辆V0测量到相邻车辆Vj的位置Xj，并且得到两辆车之间的相对距离Dj可得
4.5)根据4.4)得到的概率可将4.3)中的后验概率密度函数表示为：
4.6)将系统位置预测值的概率密度函数矩阵和系统相对距离的概率密度函数矩阵以及相邻车辆位置的密度函数代入到4.5)所示的后验概率密度函数表达式中，计算k+1时刻后验概率密度矩阵表示如下：
其中，
步骤5，根据步骤4得到的k+1时刻后验概率密度矩阵得到矩阵中最大后验概率值以及最大后验概率值所对应的行数Rmax，将最大行数Rmax所对应的位置估计矩阵中的估计值作为目标车辆最终的位置估计值
本实施例中，如图3所示，图3描述了本发明实施例测量的位置误差与GPS测量的位置误差对比示意图，图3中的横坐标是采样时间，纵坐标是位置误差大小，该结果是在信噪比为3dB下进行1000次的同条件重复实验得出的。由图3可知，本发明实施例提出的方法显著降低了位置误差，并给出了更加准确的位置估计值。这说明本发明实施例提出的基于扩展卡尔曼滤波和贝叶斯滤波的多车辆协作定位方法是准确并且有效的，同时对比结果也表明本发明采用的方法性能优于GPS测量的性能。
以上所述的实施例，只是本发明较优选的具体实施方式的一种，本领域的技术人员在本发明技术方案范围内进行的通常变化和替换都应包含在本发明的保护范围内。
Claims (8)
1.一种贝叶斯滤波的多车辆协作定位方法，其特征在于，所述方法利用目标车辆与相邻车辆位置的概率密度函数，以及车辆之间相对距离的概率密度函数，在扩展卡尔曼滤波的每次迭代过程中，通过贝叶斯滤波，得到目标车辆位置的后验概率估计，再根据最大后验估计值，确定目标车辆最终的位置估计，通过双重滤波，实现协作定位。
2.根据权利要求1所述的方法，其特征在于，所述方法包括：
S1：获取当前时刻目标车辆观测到相邻车辆时，目标车辆的位置测量值以及相邻车辆的数目，并构造系统的状态方程；
S2：获取目标车辆和相邻车辆在当前时刻的相对位置信息以及相邻车辆的数目，并构造系统的观测方程；
S3：将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到系统的位置预测值矩阵以及系统位置估计值矩阵，并计算系统位置预测值的概率密度函数矩阵以及系统相对距离的概率密度函数矩阵；
S4：将系统位置预测值的概率密度函数矩阵和系统相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵；
S5：根据最大后验概率估计得到目标车辆的位置估计值。
3.根据权利要求2所述的方法，其特征在于，所述S1中构造系统的状态方程包括：
S11：获取k时刻目标车辆X0的位置测量值
其中，k时刻表示当前时刻，T表示转置，表示k时刻目标车辆X0在x轴方向的坐标，表示k时刻目标车辆X0在y轴方向的坐标，表示k时刻目标车辆X0运动方向与x轴方向所形成的夹角；
S12：获取所述相邻车辆的数目N；
S13：根据观测到的相邻车辆的数目N，构造系统的状态方程
其中，表示k时刻目标车辆V0观测到相邻车辆Vj时，目标车辆V0相对于相邻车辆Vj的位置。
4.根据权利要求2所述的方法，其特征在于，所述S2中构造系统的观测方程包括：
S21：获取k时刻相邻车辆Xj的位置测量值
其中，N表示相邻车辆的数目，xjk表示k时刻相邻车辆Xj在x轴方向的坐标，yjk表示k时刻相邻车辆Xj在y轴方向的坐标，θjk表示k时刻相邻车辆Xj运动方向与x轴所形成的夹角；
S22：计算k时刻目标车辆X0和相邻车辆Xj之间的相对位置信息表示如下；
其中，表示为目标车辆X0与相邻车辆Xj之间的相对距离，表示为目标车辆X0与相邻车辆Xj之间的相对方位角；
S23：根据计算得到的k时刻目标车辆X0和相邻车辆Xj之间的相对位置信息构造系统的观测方程；
其中，N表示相邻车辆的数目。
5.根据权利要求2所述的方法，其特征在于，所述S3具体包括：
S31：将状态方程Xk和观测方程Zk代入到扩展卡尔曼滤波中，得到目标车辆X0的位置预测值矩阵以及位置估计值矩阵
S32：计算当前时刻系统位置预测值的概率密度函数矩阵以及系统相对距离的概率密度函数矩阵
6.根据权利要求2所述的方法，其特征在于，所述S4中后验概率密度矩阵计算方法如下：
将当前时刻系统位置预测值的概率密度函数矩阵和系统相对距离的概率密度函数矩阵以及相邻车辆位置的概率密度函数代入到贝叶斯滤波中，得到后验概率密度矩阵
7.根据权利要求2所述的方法，其特征在于，所述S5中根据最大后验概率估计得到目标车辆的位置估计值包括：
得到后验概率密度矩阵中最大后验概率值以及最大后验概率值所对应的行数Rmax，将最大行数Rmax所对应的位置估计值作为目标车辆最终的位置估计值
8.根据权利要求6所述的方法，其特征在于，所述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时刻系统状态协方差其中，I为单位矩阵；
S46：更新k+1时刻系统状态估计值；
其中，Zk+1为k+1时刻系统观测方程的观测值。
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 true CN108398704A (zh) | 2018-08-14 |
| CN108398704B 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) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109996205A (zh) * | 2019-04-12 | 2019-07-09 | 成都工业学院 | 传感器数据融合方法、装置、电子设备及存储介质 |
| CN113706854A (zh) * | 2021-08-20 | 2021-11-26 | 北京科技大学 | 一种智能车联网中的车辆协作定位方法 |
Citations (9)
| 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 | 北京科技大学 | 一种智能交通系统中多车辆协作定位方法 |
-
2018
- 2018-02-06 CN CN201810119662.9A patent/CN108398704B/zh not_active Expired - Fee Related
Patent Citations (10)
| 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)
| Title |
|---|
| DIETER FOX,ET AL: "Bayesian Filters for Location Estimation", 《 IEEE CS AND IEEE COMSOC 》 * |
| 田红兵等: "基于EKF和PF的多机器人协同定位技术", 《现代电子技术》 * |
| 白玉洁: "多机器人协作定位算法研究与仿真", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109996205A (zh) * | 2019-04-12 | 2019-07-09 | 成都工业学院 | 传感器数据融合方法、装置、电子设备及存储介质 |
| CN109996205B (zh) * | 2019-04-12 | 2021-12-07 | 成都工业学院 | 传感器数据融合方法、装置、电子设备及存储介质 |
| CN113706854A (zh) * | 2021-08-20 | 2021-11-26 | 北京科技大学 | 一种智能车联网中的车辆协作定位方法 |
| CN113706854B (zh) * | 2021-08-20 | 2023-03-07 | 北京科技大学 | 一种智能车联网中的车辆协作定位方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108398704B (zh) | 2020-11-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109946731B (zh) | 一种基于模糊自适应无迹卡尔曼滤波的车辆高可靠融合定位方法 | |
| CN107274721B (zh) | 一种智能交通系统中多车辆协作定位方法 | |
| Alam et al. | Relative positioning enhancement in VANETs: A tight integration approach | |
| Liu et al. | Improving positioning accuracy using GPS pseudorange measurements for cooperative vehicular localization | |
| CN107315413B (zh) | 一种车车通信环境下考虑车辆间相对位置的多车协同定位算法 | |
| Ghaleb et al. | Improved vehicle positioning algorithm using enhanced innovation-based adaptive Kalman filter | |
| CN101285686B (zh) | 一种农业机械导航分级定位的方法和系统 | |
| CN107247275B (zh) | 基于公交车的城市gnss脆弱性监测系统及其方法 | |
| US20100295726A1 (en) | Global positioning system error correction, vehicle tracking and object location | |
| CN105682222A (zh) | 一种基于车辆自组织网络的车辆位置定位信息融合方法 | |
| EP2955546A2 (en) | Toll object detection in a gnss system using particle filter | |
| CN104677361B (zh) | 一种综合定位的方法 | |
| CN109946648B (zh) | 一种车路协同下基于超宽带的车辆高精度定位方法 | |
| CN112346103A (zh) | 基于v2x的智能网联汽车动态协同定位方法与装置 | |
| CN108398704B (zh) | 一种贝叶斯滤波的多车辆协作定位方法 | |
| CN109343096B (zh) | 基于gsm-r通信系统的gnss辅助相对导航方法及系统 | |
| Baek et al. | Accurate vehicle position estimation using a Kalman filter and neural network-based approach | |
| CN108885269A (zh) | 导航方法、导航装置和导航系统 | |
| Saleh et al. | 5g-enabled vehicle positioning using ekf with dynamic covariance matrix tuning | |
| Dasanayaka et al. | Analysis of vehicle location prediction errors for safety applications in cooperative-intelligent transportation systems | |
| Liu et al. | A hybrid integrity monitoring method using vehicular wireless communication in difficult environments for GNSS | |
| Ahammed et al. | LICA: robust localization using cluster analysis to improve GPS coordinates | |
| Kong et al. | An accurate and reliable positioning methodology for land vehicles in tunnels based on UWB/INS integration | |
| Soatti et al. | Enhanced vehicle positioning in cooperative ITS by joint sensing of passive features | |
| Hsu et al. | Intelligent viaduct recognition and driving altitude determination using GPS data |
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 |