CN104200695A - 一种基于车路专用短程通信的车辆协同定位方法 - Google Patents

Abstract

本发明公开了一种基于车路专用短程通信的车辆协同定位方法，在车辆中部署车载通信单元，在路侧部署路侧通信单元，车路通信单元之间通过专用短程通信技术实现信息交互；路侧通信单元对接收到的车辆发出的专用短程通信信号进行处理，通过直连信号决策条件，获得直连信号参数信息，联合多个路侧通信单元，融合信息资源，实现更加精准的车辆协同定位；本发明充分利用了智能车路协同系统中存在的专用短程通信信号资源，可以有效解决或缓解弱卫星信号覆盖区域内的车辆定位问题。

Description

一种基于车路专用短程通信的车辆协同定位方法

技术领域

本发明涉及一种基于车路专用短程通信的车辆协同定位方法，属于智能车路协同系统和车路无线通信技术交叉技术领域。

背景技术

智能车路协同系统(Intelligent Vehicle-Infrastructure Cooperative System，I-VICS)是近年来交通科学发展的前沿方向。该系统依靠车载、路侧传感设施感知车辆、道路的状态信息，通过车路无线通信技术，尤其是目前各国大力投入研究、政策推进，为车路通信系统制定的专用短程通信技术(Dedicated Short Range Communications，DSRC)，来实现车车、车路之间的信息交互共享。在信息共享的交通系统中：一方面，对于车车(Vehicle-to-Vehicle，V2V)系统，车辆收到邻居车辆节点的状态信息，如邻居车辆的位置与速度信息，以此做出智能决策并将决策结果转化为控制信号，操控车辆下一步姿态，实现车辆碰撞避免、队列跟驰、换道超车等智能车车协同应用；另一方面，对于车路(Vehicle-to-Infrastructure，V2I)系统，布设于十字交叉路口的信号灯可以通过专用短程通信技术向四周广播其相位状态信息，车辆将收到信号灯相位信息与自身状态信息融合，如车辆自身的位置与速度信息，计算出车辆顺利通过十字交叉路口的最优建议车速，保证驾驶员以较为均匀的速度顺利通过十字交叉路口，减少或避免驾驶员不当的“起-停”操作，从而正确引导驾驶员的驾驶行为，有效降低区域路网中车辆的排放与油耗。

在智能车路协同系统中，车辆位置信息是实现车车、车路典型场景中最为重要的信息之一。最为常见的获取车辆位置信息的方式是通过全球卫星定位系统，该系统的顺利运行需要保证同时收到四颗以上的卫星信号。然而，在城市或隧道中，由于“高楼峡谷”现象以及隧道的封闭结构，卫星信号被严重遮挡，卫星信号与接收端的链路处于严重的“非视距”情况，此时卫星定位系统将不能提供位置信息，或者可提供的位置信息与真实位置信息有较大偏差，不能满足车路协同系统的应用需求。随着车车、车路通信技术的普及发展，具备专用短程通信技术的车载通信单元、路侧通信单元将在交通路网中逐步铺开。由于车车、车路链路往往处于良好“视距”情况，这将十分有利于运用无线信号的测距与测角原理，以获得车辆较为精确的位置信息。

多重信号分类方法(Multiple Signal Classification，MUSIC)，是信号谱分析和统计信号处理中经典的信号波达角估计方法，在绝大数情况下，具有极好的估计性能。压缩感知(Compressive Sensing，CS)方法是近来国际上较为热门的一个研究方向，主要应用于雷达成像、医学成像、图像处理等研究领域，其主要思想是利用原始信号的稀疏特征，通过较少的观测次数由观测信号恢复原始信号。贝叶斯压缩感知(Bayesian Compressive Sensing，BCS)是压缩感知研究方向中的一个分支，它采用传统的贝叶斯和基于相关向量机(RelevanceVector Machine，RVM)的机器学习方法，来恢复原始信号，该方法在无信号先验知识条件下，仍然具备良好的恢复性能，并且在有噪声条件下具备良好的鲁棒性。近年来，已有相关研究将贝叶斯压缩感知方法运用于信号波达角估计，该方法在无先验知识情况下，与经典的多重信号分类方法比较，具有更好的估计性能。

由此，如何有效利用现有的技术资源，来解决城市或隧道等弱卫星信号覆盖区域的车辆位置信息获取问题，是当前智能车路协同系统发展中需要迫切解决的问题。

车辆位置信息是智能车路协同系统中重要的信息资源，其一，微观的看，车辆位置信息可用于实现车队管理、车辆定位导航、车车协同避撞等功能；其二，宏观的看，一条线或一个面的车辆位置信息，可以转化为一条路或一个网的交通态势信息，如交通流量、排队时间等，这是实现交通拥堵预测、路径动态诱导等功能的前提和基础。但是，目前城市中常见的车辆定位方法，如卫星、视频等定位技术，在具体实施应用中都存在一定的缺陷，难以全时空、全天候的保障数据的准确性。

发明内容

本发明的目的是为了解决上述问题，提出一种基于车路专用短程通信的车辆协同定位方法，充分利用城市或隧道中车辆周围与道路两侧的专用短程通信信号资源，解决城市或隧道等卫星信号弱覆盖区域内车辆位置信息获取问题。

本发明是一种基于车路专用短程通信的车辆协同定位方法，通过下述步骤实现：

步骤一、建立直连信号因子和路径衰减指数的函数关系；

步骤二，在t时刻，路侧通信单元i接收车辆k发出的专用短程通信信号；

步骤三，路侧通信单元i对接收到车辆k发出的专用短程通信信号进行处理，获得车辆k与路侧通信单元i间通信链路的直连信号因子DSF_ki，获得车辆k与路侧通信单元i的相对距离d_ki，以及车辆k发出的专用短程通信信号到达路侧通信单元i的波达方向角θ_ki；

步骤四，计算时刻t车辆k的位置坐标；

步骤五，使用扩展卡尔曼滤波器对车辆位置信息(X_{Vehicle_ki}(t)，Y_{Vehicle_ki}(t))和车速信息v_k(t)滤波，滤波系统将时刻t路侧通信单元i对车辆k的观测值作为系统输入状态向量X，滤波后的预测值作为系统的输出状态向量X⁺；

步骤六，将处于车辆k的无线覆盖范围内的m个路侧通信单元，对车辆k滤波后的车辆位置信息加权融合。

本发明的优点在于：

(1)本发明基于车路专用短程通信的车辆协同定位方法，解决了在城市或隧道等弱卫星信号覆盖区域的车辆定位问题。充分利用了智能车路协同系统中广泛存在的车载通信单元和路侧通信单元，在不影响车路正常的通信链路前提下，利用车路专用短程通信信号实现精准的车辆定位，保障智能车路协同系统具体应用的顺利实施；

(2)本发明基于车路专用短程通信的车辆协同定位方法，极大程度智能判断了车辆与路侧通信单元之间无线链路并非处于恶劣的“非视距”情况，随后进行进一步的信号处理，保证了车辆定位精度。同时，也极大程度智能判断了车辆与路侧通信单元之间无线链路处于恶劣的“非视距”情况。在这个易产生较大位置估计偏差的环境，路侧通信单元将停止进一步的信号处理工作，避免了计算资源和能耗的浪费；

(3)本发明基于车路专用短程通信的车辆协同定位方法，可通过单个或多个路侧通信单元独立或协同实现车辆的高精度定位；一方面，在路侧通信单元资源有限的路网区域，通过单个设备即可实现车辆定位；另一方面，在路侧通信单元资源充足的路网区域，可利用多个路侧通信单元实现协同定位，进一步提高车辆定位精度。

附图说明

图1是本发明车辆协同定位方法流程图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明是一种基于车路专用短程通信的车辆协同定位方法，如图1所示，通过下述步骤实现：

步骤一、建立直连信号因子和路径衰减指数的函数关系；

A、在路侧通信单元i的无线网络覆盖范围内，随机选择若干测试点，确定测试点与路侧通信单元i的相对距离d′；在测试点与路侧通信单元i的相对距离d′测定前提下，在测试点放置车载通信单元发送专用短程通信信号，路侧通信单元i实时接收车载通信单元发出的专用短程通信信号；其中测试点个数为5～10个；

B、从路侧通信单元i提取接收信号强度指示(Received Signal Strength Indicator，RSSI)，记为RSSI′_i，该值为路侧通信单元i收到相对距离d′的测试点发出的专用短程通信信号的信号强度的平均值，这些信号包含路侧通信单元i和测试点间的直连信号，以及经过多次反射，折射，衍射等受到多径、阴影效应影响后的非直连信号；

C、路侧通信单元i对接收信号进行傅里叶逆变换(Inverse Fast FourierTransformation，IFFT)，将该频域信号转化为时域的信号功率延迟分布，该分布为离散的柱状图，横坐标为接收到信号的延迟，纵坐标为对应延迟接收到信号的信号功率；提取信号功率延迟分布中首个延迟时刻的柱形高度值-信号功率，该值以极大概率对应步骤B中所述的直连信号的信号功率，记为直连信号功率POD′_i；

D、对于路侧通信单元i，将其直连信号因子Direct Signal Factor-DSF′_i，定义为：

${\mathrm{DSF}}_i^{\′}=\frac{{\mathrm{POD}}_i^{\′}\left(d^{\′}\right)}{{\mathrm{RSSI}}_i^{\′}\left(d^{\′}\right)}---\left(1\right)$

通过步骤B、C得到RSSI′_i、POD′_i值，并由(1)式计算DSF′_i，DSF′_i∈(0，1]，DSF′_i越接近0表明通信链路越接近严重的“非视距”情况，相反该值越接近1表明通信链路越接近良好的“视距”情况；

E、测试点与路侧通信单元i的相对距离为d′，当d′＝1m时，路侧通信单元i处接收到的直连信号功率为POD′_i(1)，令POD′_i(1)为测试基准功率值P₀；

F、当d′≠1m时，路侧通信单元i处接收到的直连信号功率为POD′_i(d′)，将POD′_i(d′)带入(2)式左端P_r，d′值带入(2)式右端d中，P₀由步骤E确定；其中，(2)式中log(·)是以10为底的取对数算子；γ为路径衰减指数，取值范围一般为2～4；

P_r＝P₀-10γlog(d)  (2)

联合(1)、(2)式可得(3)式：

$\mathrm{\γ}=-\frac{{\mathrm{DSF}}_i^{\′}\·{\mathrm{RSSI}}_i^{\′}\left(d^{\′}\right)}{10\log \left(d^{\′}\right)}+\frac{P_0}{10\log \left(d^{\′}\right)}---\left(3\right)$

由(3)式可知γ与DSF′_i成线性关系；因此，联合路侧通信单元i处的多个测试点数据，由(1)式得到DSF′_i值，由(2)式得到γ值，进行线性函数拟合，得到如(4)式函数关系：

γ＝a·DSF′_i+b  (4)

其中，a和b为利用DSF′_i和γ值线性拟合确定的线性函数的斜率和截距，a和b为常数。

步骤二，在t时刻，路侧通信单元i接收车辆k发出的专用短程通信信号；

在区域路网中建立参考坐标系，本发明采用直角坐标系；令正东方向为x轴正方向，正北方向为y轴正方向；令时刻t，处于路侧通信单元i的以通信距离L为半径的无线网络覆盖范围内的，具备专用短程通信能力的全部车辆数目为n；车辆标识为k，k＝1，2，…，n；则通过车辆k上的车载通信单元发送专用短程通信信号；令时刻t，处于车辆k的以通信距离L为半径的无线网络覆盖范围内的路侧通信单元数目为m；路侧通信单元标识为i，i＝1，2，…，m；路侧通信单元i接收车辆k发出的专用短程通信信号，具体为：

A、在时刻t车辆k发出专用短程通信信号，该信号为包含车辆实时速度信息v的数据包，该速度信息可从车载速度传感器或者车辆仪表盘中直接读取；

B、处于车辆k以通信距离L为半径的无线网络覆盖范围内的路侧设备i，在时刻t后的极短延迟内，将接收到车辆k发出的专用短程通信信号，该延迟数量级为纳秒级，在本方法应用中不考虑这一延迟；此外，本方法中L＝300m。

步骤三，路侧通信单元i对接收到车辆k发出的专用短程通信信号进行处理，获得车辆k与路侧通信单元i间通信链路的直连信号因子DSF_ki，获得车辆k与路侧通信单元i的相对距离d_ki，以及车辆k发出的专用短程通信信号到达路侧通信单元i的波达方向角θ_ki，该角度为车辆k和路侧通信单元i两点间连线与x轴正方向夹角；

A、时刻t，路侧通信单元i收到车辆k发出的专用短程通信信号，从路侧通信单元i提取接收信号强度指示RSSI_ki(t)；

B、时刻t，对接收信号进行傅里叶逆变换，将该频域信号转化为时域的信号功率延迟分布，提取直连信号功率POD_ki(t)；

C、将步骤A、B实时测得的RSSI_ki(t)和POD_ki(t)值代入(1)式，计算时刻t车辆k与路侧通信单元i间通信链路的直连信号因子DSF_ki(t)；

令直连信号因子DSF_ki的判别门限为DSF_Threshold，本方法中DSF_Threshold＝0.5；若步骤C中得到的DSF_ki(t)，满足DSF_ki(t)＞DSF_Threshold，则时刻t车辆k与路侧通信单元i间通信链路，并非处于恶劣的“非视距”情况，因此，继续执行本方法后续步骤；若不满足DSF_ki(t)＞DSF_Threshold，则时刻t车辆k与路侧通信单元i间通信链路极大可能处于恶劣的“非视距”情况，因此，将不执行后续步骤，即在时刻t，路侧通信单元i将不计算车辆k的位置；

D、获得时刻t车辆k与路侧通信单元i的相对距离d_ki(t)；

将步骤C中获得的直连信号因子DSF_ki(t)代入(4)式右端DSF′_i处，实时更新时刻t的γ(t)，将POD_ki(t)代入(2)式左端P_r，将P₀和更新后的γ(t)值代入(2)式右端，计算d的值-时刻t，车辆k与路侧通信单元i的相对距离d_ki(t)；即联合(1)、(2)、(4)式，得到(5)式，即d_ki(t)的计算式：

$d_{\mathrm{ki}}\left(t\right)={10}^{\frac{P_0-{\mathrm{POD}}_{\mathrm{ki}}\left(t\right)}{10\·(a\·\frac{{\mathrm{POD}}_{\mathrm{ki}}\left(t\right)}{{\mathrm{RSSI}}_{\mathrm{ki}}\left(t\right)}+b)}}---\left(5\right)$

E、获得时刻t车辆k发出的专用短程通信信号到达路侧通信单元i的波达方向角θ_ki；

I、在路侧通信单元i处，如果车辆数目n已知，使用经典的多重信号分类方法，估计可能的波达方向角θ_ki；如果车辆数目n未知，使用贝叶斯压缩感知方法，估计可能的波达方向角θ_ki；令时刻t到时刻t+T，处于车辆k无线网络覆盖范围内的路侧通信单元i，在时间段T内获得所有的直连波达方向角θ_ki的集合为SET_D，本方法中T＝0.1s；

II、令直连信号角度变化的判别门限为Δθ，本方法中Δθ＝5°；在t时刻，路侧通信单元i收到车辆k的车速v_k(t)，则根据v_k(t)进行如下判断：

1)若车辆速度v_k(t)＝0，则进一步考察t+Δt时刻的信号波达角θ_ki(t+Δt)，本方法中Δt＝50ms，Δt＜T；若|θ_ki(t+Δt)-θ_ki(t)|＜Δθ，则认为θ_ki(t)、θ_ki(t+Δt)∈SET_D；否则， ${\mathrm{\θ}}_{\mathrm{ki}}\left(t\right)\∉{\mathrm{SET}}_D;$

2)若车辆速度v_k(t)≠0，则进一步考察t+Δt时刻的信号波达角θ_ki(t+Δt)，其中Δt＜T；在t和t+Δt时刻，由步骤三D分别获得的车辆k与路侧通信单元i的相对距离d_ki(t)和d_ki(t+Δt)，以及Δt时间段内车辆移动距离v_k(t)·Δt，令Δt时间段内，路侧通信单元i处车辆k的波达角变化为θ_ki(t)-θ_ki(t+Δt)，与此角度变化值相应的对比角度-车辆移动方位角变化(Vehicle Movement Bearings Alteration，VMBA)为：

$\mathrm{VMBA}\left(t\right)=\arccos \frac{d_{\mathrm{ki}}^2\left(t\right)+d_{\mathrm{ki}}^2(t+\mathrm{\Δt})-{(v_k\left(t\right)\·\mathrm{\Δt})}^2}{2\·d_{\mathrm{ki}}\left(t\right)\·d_{\mathrm{ki}}(t+\mathrm{\Δt})}---\left(6\right)$

若||θ_ki(t+Δt)-θ_ki(t)|-VMBA(t)|＜Δθ，则认为θ_ki(t)，θ_ki(t+Δt)∈SET_D；否则， ${\mathrm{\θ}}_{\mathrm{ki}}\left(t\right)\∉{\mathrm{SET}}_D;$

步骤四，计算时刻t车辆k的位置坐标；其中，路侧通信单元i的坐标为本方法中为已知量；车辆k的位置坐标为(X_{Vehicle_ki}(t)，Y_{Vehicle_ki}(t))，如(7)式：

$\left\{\begin{array}{c}{X}_{\mathrm{Vehicle}\_\mathrm{ki}}\left(t\right)=X_{\mathrm{RSU}\_i}+d_{\mathrm{ki}}\left(t\right)\cos \left({\mathrm{\θ}}_{\mathrm{ki}}\right)\\ Y_{\mathrm{Vehicle}\_\mathrm{ki}}\left(t\right)=Y_{\mathrm{RSU}\_i}+d_{\mathrm{ki}}\left(t\right)\sin \left({\mathrm{\θ}}_{\mathrm{ki}}\right)\end{array}\right.---\left(7\right)$

步骤五，使用扩展卡尔曼滤波器(Extended Kalman Filter，EKF)对车辆位置信息(X_{Vehicle_ki}(t)，Y_{Vehicle_ki}(t))和车速信息v_k(t)滤波，滤波系统将时刻t路侧通信单元i对车辆k的观测值作为系统输入状态向量X，X＝{X_{Vehicle_ki}(t)，Y_{Vehicle_ki}(t)，v_k(t)·cos(θ_ki(t))，v_k(t)·sin(θ_ki(t))}^T，滤波后的预测值作为系统的输出状态向量X⁺：

$X^{+}={\{X_{\mathrm{Vehicle}\_\mathrm{ki}}^{+}\left(t\right),Y_{\mathrm{Vehicle}\_\mathrm{ki}}^{+}\left(t\right),{[v_k\left(t\right)\·\cos \left({\mathrm{\θ}}_{\mathrm{ki}}\left(t\right)\right)]}^{+},{[v_k\left(t\right)\·\sin \left({\mathrm{\θ}}_{\mathrm{ki}}\left(t\right)\right)]}^{+}\}}^T$

通过扩展卡尔曼滤波器滤波，提高路侧通信单元i，对其无线覆盖范围内的车辆k的单点位置信息，以及连续运动轨迹的预测能力；

步骤六，将处于车辆k的无线覆盖范围内的m个路侧通信单元，对车辆k滤波后的车辆位置信息加权融合；加权融合后车辆k的位置信息为(Weight_X_{vehicle_k}(t)，Weight_Y_{Vehicle_k}(t))，加权融合过程如(8)式：

$\left\{\begin{array}{c}\mathrm{Weight}\_X_{\mathrm{Vehicle}\_k}\left(t\right)=\frac{{\mathrm{\Σ}}_{i=1}^m{X}_{\mathrm{Vehicle}\_\mathrm{ki}}^{+}\left(t\right)\·{\mathrm{POD}}_{\mathrm{ki}}\left(t\right)}{{\mathrm{\Σ}}_{i=1}^m{\mathrm{POD}}_{\mathrm{ki}}\left(t\right)}\\ \mathrm{Weight}\_Y_{\mathrm{Vehicle}\_k}\left(t\right)=\frac{{\mathrm{\Σ}}_{i=1}^m{Y}_{\mathrm{Vehicle}\_\mathrm{ki}}^{+}\left(t\right)\·{\mathrm{POD}}_{\mathrm{ki}}\left(t\right)}{{\mathrm{\Σ}}_{i=1}^m{\mathrm{POD}}_{\mathrm{ki}}\left(t\right)}\end{array}\right.---\left(8\right)$

Claims (1)

1.一种基于车路专用短程通信的车辆协同定位方法，通过下述步骤实现：

步骤一、建立直连信号因子和路径衰减指数的函数关系；

A、在路侧通信单元i的无线网络覆盖范围内，随机选择若干测试点，确定测试点与路侧通信单元i的相对距离d′；在测试点与路侧通信单元i的相对距离d′测定前提下，在测试点放置车载通信单元发送专用短程通信信号，路侧通信单元i实时接收车载通信单元发出的专用短程通信信号；

B、从路侧通信单元i提取接收信号强度指示，记为RSSI′_i，该值为路侧通信单元i收到相对距离d′的测试点发出的专用短程通信信号的信号强度的平均值，包含路侧通信单元i和测试点间的直连信号以及非直连信号；

C、路侧通信单元i对接收信号进行傅里叶逆变换，将该频域信号转化为时域的信号功率延迟分布，该分布为离散的柱状图，横坐标为接收到信号的延迟，纵坐标为对应延迟接收到信号的信号功率；提取信号功率延迟分布中首个延迟时刻的柱形高度值-信号功率，该值以极大概率对应步骤B中所述的直连信号的信号功率，记为直连信号功率POD′_i；

D、对于路侧通信单元i，其直连信号因子为：

${\mathrm{DSF}}_i^{\′}=\frac{{\mathrm{POD}}_i^{\′}\left(d^{\′}\right)}{{\mathrm{RSSI}}_i^{\′}\left(d^{\′}\right)}---\left(1\right)$

其中：DSF′_i∈(0，1]；

E、测试点与路侧通信单元i的相对距离为d′，当d′＝1m时，路侧通信单元i处接收到的直连信号功率为POD′_i(1)，令POD′_i(1)为测试基准功率值P₀；

F、当d′≠1m时，路侧通信单元i处接收到的直连信号功率为POD′_i(d′)，将POD′_i(d′)带入(2)式左端P_r，d′值带入(2)式右端d中，P₀由步骤E确定；其中，(2)式中log(·)是以10为底的取对数算子；γ为路径衰减指数；

P_r＝P₀-10γlog(d)  (2)

联合(1)、(2)式得(3)式：

$\mathrm{\γ}=-\frac{{\mathrm{DSF}}_i^{\′}\·{\mathrm{RSSI}}_i^{\′}\left(d^{\′}\right)}{10\log \left(d^{\′}\right)}+\frac{P_0}{10\log \left(d^{\′}\right)}---\left(3\right)$

联合路侧通信单元i处的多个测试点数据，由(1)式得到DSF′_i值，由(2)式得到γ值，进行线性函数拟合，得到如(4)式函数关系：

γ＝a·DSF′_i+b  (4)

其中，a和b为利用DSF′_i和γ值线性拟合确定的线性函数的斜率和截距，a和b为常数；

步骤二，在t时刻，路侧通信单元i接收车辆k发出的专用短程通信信号；

在区域路网中建立参考坐标系，采用直角坐标系；令正东方向为x轴正方向，正北方向为y轴正方向；令时刻t，处于路侧通信单元i的以通信距离L为半径的无线网络覆盖范围内的，具备专用短程通信能力的全部车辆数目为n；车辆标识为k，k＝1，2，…，n；则通过车辆k上的车载通信单元发送专用短程通信信号；令时刻t，处于车辆k的以通信距离L为半径的无线网络覆盖范围内的路侧通信单元数目为m；路侧通信单元标识为i，i＝1，2，…，m；路侧通信单元i接收车辆k发出的专用短程通信信号，具体为：

A、在时刻t车辆k发出专用短程通信信号，该信号为包含车辆实时速度信息v的数据包，该速度信息从车载速度传感器或者车辆仪表盘中直接读取；

B、处于车辆k以通信距离L为半径的无线网络覆盖范围内的路侧设备i，在时刻t后的极短延迟内，将接收到车辆k发出的专用短程通信信号；

步骤三，路侧通信单元i对接收到车辆k发出的专用短程通信信号进行处理，获得车辆k与路侧通信单元i间通信链路的直连信号因子DSF_ki，获得车辆k与路侧通信单元i的相对距离d_ki，以及车辆k发出的专用短程通信信号到达路侧通信单元i的波达方向角θ_ki，该角度为车辆k和路侧通信单元i两点间连线与x轴正方向夹角；

A、时刻t，路侧通信单元i收到车辆k发出的专用短程通信信号，从路侧通信单元i提取接收信号强度指示RSSI_ki(t)；

B、时刻t，对接收信号进行傅里叶逆变换，将该频域信号转化为时域的信号功率延迟分布，提取直连信号功率POD_ki(t)；

C、将步骤A、B实时测得的RSSI_ki(t)和POD_ki(t)值代入(1)式，计算时刻t车辆k与路侧通信单元i间通信链路的直连信号因子DSF_ki(t)；

令直连信号因子DSF_ki的判别门限为DSF_Threshold，若步骤C中得到的DSF_ki(t)，满足DSF_ki(t)＞DSF_Threshold，继续执行后续步骤；若不满足DSF_ki(t)＞DSF_Threshold，不执行后续步骤，即在时刻t，路侧通信单元i将不计算车辆k的位置；

D、获得时刻t车辆k与路侧通信单元i的相对距离d_ki(t)；

将步骤C中获得的直连信号因子DSF_ki(t)代入(4)式右端DSF′_i处，实时更新时刻t的γ(t)，将POD_ki(t)代入(2)式左端P_r，将P₀和更新后的γ(t)值代入(2)式右端，计算d的值-时刻t，车辆k与路侧通信单元i的相对距离d_ki(t)；即联合(1)、(2)、(4)式，得到(5)式，即d_ki(t)的计算式：

$d_{\mathrm{ki}}\left(t\right)={10}^{\frac{P_0-{\mathrm{POD}}_{\mathrm{ki}}\left(t\right)}{10\·(a\·\frac{{\mathrm{POD}}_{\mathrm{ki}}\left(t\right)}{{\mathrm{RSSI}}_{\mathrm{ki}}\left(t\right)}+b)}}---\left(5\right)$

E、获得时刻t车辆k发出的专用短程通信信号到达路侧通信单元i的波达方向角θ_ki；

I、在路侧通信单元i处，如果车辆数目n已知，采用多重信号分类方法，估计波达方向角θ_ki；如果车辆数目n未知，使用贝叶斯压缩感知方法，估计波达方向角θ_ki；令时刻t到时刻t+T，处于车辆k无线网络覆盖范围内的路侧通信单元i，在时间段T内获得所有的直连波达方向角θ_ki的集合为SET_D；

II、令直连信号角度变化的判别门限为Δθ，在t时刻，路侧通信单元i收到车辆k的车速v_k(t)，则根据v_k(t)进行如下判断：

1)若车辆速度v_k(t)＝0，则进一步考察t+Δt时刻的信号波达角θ_ki(t+Δt)，Δt＜T；若|θ_ki(t+Δt)-θ_ki(t)|＜Δθ，则认为θ_ki(t)、θ_ki(t+Δt)∈SET_D；否则，

2)若车辆速度v_k(t)≠0，则进一步考察t+Δt时刻的信号波达角θ_ki(t+Δt)，其中Δt＜T；在t和t+Δt时刻，由步骤三D分别获得的车辆k与路侧通信单元i的相对距离d_ki(t)和d_ki(t+Δt)，以及Δt时间段内车辆移动距离v_k(t)·Δt，令Δt时间段内，路侧通信单元i处车辆k的波达角变化为θ_ki(t)-θ_ki(t+Δt)，与此角度变化值相应的对比角度-车辆移动方位角变化为：

$\mathrm{VMBA}\left(t\right)=\arccos \frac{d_{\mathrm{ki}}^2\left(t\right)+d_{\mathrm{ki}}^2(t+\mathrm{\Δt})-{(v_k\left(t\right)\·\mathrm{\Δt})}^2}{2\·d_{\mathrm{ki}}\left(t\right)\·d_{\mathrm{ki}}(t+\mathrm{\Δt})}---\left(6\right)$

若||θ_ki(t+Δt)-θ_ki(t)|-VMBA(t)|＜Δθ，则认为θ_ki(t)，θ_ki(t+Δt)∈SET_D；否则， ${\mathrm{\θ}}_{\mathrm{ki}}\left(t\right)\∉{\mathrm{SET}}_D;$

步骤四，计算时刻t车辆k的位置坐标；其中，路侧通信单元i的坐标为为已知量；车辆k的位置坐标为(X_{Vehicle_ki}(t)，Y_{Vehicle_ki}(t))，如(7)式：

$\left\{\begin{array}{c}{X}_{\mathrm{Vehicle}\_\mathrm{ki}}\left(t\right)=X_{\mathrm{RSU}\_i}+d_{\mathrm{ki}}\left(t\right)\cos \left({\mathrm{\θ}}_{\mathrm{ki}}\right)\\ Y_{\mathrm{Vehicle}\_\mathrm{ki}}\left(t\right)=Y_{\mathrm{RSU}\_i}+d_{\mathrm{ki}}\left(t\right)\sin \left({\mathrm{\θ}}_{\mathrm{ki}}\right)\end{array}\right.---\left(7\right)$

步骤五，使用扩展卡尔曼滤波器对车辆位置信息(X_{Vehicle_ki}(t)，Y_{Vehicle_ki}(t))和车速信息v_k(t)滤波，滤波系统将时刻t路侧通信单元i对车辆k的观测值作为系统输入状态向量X，X＝{X_{Vehicle_ki}(t)，Y_{Vehicle_ki}(t)，v_k(t)·cos(θ_ki(t))，v_k(t)·sin(θ_ki(t))}^T，滤波后的预测值作为系统的输出状态向量X⁺：

$X^{+}={\{X_{\mathrm{Vehicle}\_\mathrm{ki}}^{+}\left(t\right),Y_{\mathrm{Vehicle}\_\mathrm{ki}}^{+}\left(t\right),{[v_k\left(t\right)\·\cos \left({\mathrm{\θ}}_{\mathrm{ki}}\left(t\right)\right)]}^{+},{[v_k\left(t\right)\·\sin \left({\mathrm{\θ}}_{\mathrm{ki}}\left(t\right)\right)]}^{+}\}}^T$

步骤六，将处于车辆k的无线覆盖范围内的m个路侧通信单元，对车辆k滤波后的车辆位置信息加权融合；加权融合后车辆k的位置信息为(Weight_X_{Vehicle_k}(t)，Weight_Y_{Vehicle_k}(t))，加权融合过程如(8)式：

$\left\{\begin{array}{c}\mathrm{Weight}\_X_{\mathrm{Vehicle}\_k}\left(t\right)=\frac{{\mathrm{\Σ}}_{i=1}^m{X}_{\mathrm{Vehicle}\_\mathrm{ki}}^{+}\left(t\right)\·{\mathrm{POD}}_{\mathrm{ki}}\left(t\right)}{{\mathrm{\Σ}}_{i=1}^m{\mathrm{POD}}_{\mathrm{ki}}\left(t\right)}\\ \mathrm{Weight}\_Y_{\mathrm{Vehicle}\_k}\left(t\right)=\frac{{\mathrm{\Σ}}_{i=1}^m{Y}_{\mathrm{Vehicle}\_\mathrm{ki}}^{+}\left(t\right)\·{\mathrm{POD}}_{\mathrm{ki}}\left(t\right)}{{\mathrm{\Σ}}_{i=1}^m{\mathrm{POD}}_{\mathrm{ki}}\left(t\right)}\end{array}\right.---\left(8\right)$

。