CN118089721A - 一种车辆定位方法、装置、设备和计算机可读存储介质 - Google Patents

Abstract

本发明公开了一种车辆定位方法、装置、设备和计算机可读存储介质，当信息数据包对应的车辆数量超过扩展卡尔曼滤波器算法的状态矩阵维度时，利用选择策略选择出与状态矩阵维度相同数量的车辆，并利用扩展卡尔曼滤波器算法对选择出的每个车辆对应的信息数据包和每个车辆距路侧设备的相对距离进行融合，得到选择出的每个车辆的位置信息；利用选择出的每个车辆的位置信息和对应的信息数据包更新GNSS误差预测模型，将更新后的GNSS误差预测模型共享给未通过扩展卡尔曼滤波器算法融合定位的车辆，未通过扩展卡尔曼滤波器算法融合定位的车辆通过更新后的GNSS误差预测模型得到自身位置。本发明的目的在于更加精确的对车辆进行定位。

Description

一种车辆定位方法、装置、设备和计算机可读存储介质

技术领域

本发明属于车辆定位技术领域，具体涉及一种车辆定位方法、装置、设备和计算机可读存储介质。

背景技术

面对日益复杂的城市路网，传统的道路管理已难以应对严峻的交通问题。此时，集成大数据、云计算和人工智能等先进技术的智能交通系统应运而生。其中，准确、及时的车辆位置信息是智能交通系统可靠工作的首要前提。目前，关于车辆定位的方法分为三类：单车定位、协同定位和基于误差预测的定位。

相较而言，基于车辆与基础设施(Vehicle-to-Infrastructure，V2I)的协同定位可实现更准确的位置估计，得益于固定、已知的路侧设备节点。但其受限于通信范围、安装成本等因素，只通过城市路网中RSU(道路交通设施)全覆盖实现车辆定位是不切实际的；其次，当路侧单元安装的定位计算设备进入满负荷状态，会影响部分车辆的V2I定位过程。基于误差预测的定位中，机器学习模型可寻找全球导航卫星系统(Global NavigationSatellite System，GNSS)误差与传感器测量信息的关系，以进一步改善GNSS的定位性能。然而，多传感器信息的融合、误差模型的训练对车载端的计算性能提出了较高的要求。综上，现有技术在单独使用时有着无法克服的缺点，有必要研究一种更为先进、全面的车辆定位方法。

发明内容

针对现有技术中存在的问题，本发明提供了一种车辆定位方法、装置、设备和计算机可读存储介质，其目的在于更加精确的对车辆进行定位。

为了解决上述技术问题，本发明通过以下技术方案予以实现：

根据本发明的第一方面，提供一种车辆定位方法，应用于配置有扩展卡尔曼滤波器算法的路侧设备，定位方法包括：

接收位于所述路侧设备通信范围内的车辆广播的信息数据包；

当所述信息数据包对应的车辆数量不超过所述扩展卡尔曼滤波器算法的状态矩阵维度时，利用所述扩展卡尔曼滤波器算法分别对每个车辆对应的信息数据包和每个车辆距所述路侧设备的相对距离进行融合，得到每个车辆的位置信息；

当所述信息数据包对应的车辆数量超过所述扩展卡尔曼滤波器算法的状态矩阵维度时，利用选择策略选择出与所述状态矩阵维度相同数量的车辆，并利用所述扩展卡尔曼滤波器算法对选择出的每个车辆对应的信息数据包和每个车辆距所述路侧设备的相对距离进行融合，得到选择出的每个车辆的位置信息；

利用选择出的每个车辆的位置信息和对应的信息数据包更新GNSS误差预测模型，将更新后的GNSS误差预测模型共享给未通过所述扩展卡尔曼滤波器算法融合定位的车辆，未通过所述扩展卡尔曼滤波器算法融合定位的车辆通过更新后的GNSS误差预测模型得到自身位置。

在第一方面的一种可能的实现方式中，利用所述扩展卡尔曼滤波器算法分别对每个车辆对应的信息数据包和每个车辆距所述路侧设备的相对距离进行融合，得到每个车辆的位置信息，具体如下：

所述扩展卡尔曼滤波器算法包括预测阶段、解算器处理阶段和更新阶段；

在所述预测阶段，利用每个车辆对应的信息数据包，估计当前时刻该车辆的位置信息预测值；

在所述解算器处理阶段，判断解算器中的信息数据包是否满足当前时刻该车辆的位置信息解算条件，若满足，则将解算器求解的该车辆的位置信息作为当前时刻该车辆的位置信息测量值；

若不满足，则将当前时刻该车辆的信息数据包作为当前时刻该车辆的位置信息测量值；

在所述更新阶段，利用当前时刻该车辆的位置信息预测值和当前时刻该车辆的位置信息测量值，估计得到每个车辆的位置信息。

在第一方面的一种可能的实现方式中，所述判断解算器中的信息数据包是否满足当前时刻该车辆的位置信息解算条件，具体采用下式进行：

式中，p_x(t_k)、p_y(t_k)分别为t_k时刻x、y方向上的车辆位置；Δx_n和Δy_n为车辆在n(n<k)时刻与t_k时刻x、y方向上在的距离差；和/>为路侧设备位置；d_n为车辆距路侧设备的相对距离。

在第一方面的一种可能的实现方式中，所述选择策略具体为：

将所述路侧设备通信范围内的每个车辆距所述路侧设备的相对距离，以及最新信息数据包中的时间戳和当前时刻的时间间隔，作为替换指标计算公式的考虑因素，所述替换指标计算公式如下：

式中，T代表当前时刻距离接收车辆信息数据的时间间隔；s为路侧设备根据车辆与路侧设备的相对距离而设置的权重；σ_x、σ_y分别表示与x、y方向GNSS位置信息相关的标准差。

在第一方面的一种可能的实现方式中，所述未通过所述扩展卡尔曼滤波器算法融合定位的车辆通过更新后的GNSS误差预测模型得到自身位置，具体如下：

将未通过所述扩展卡尔曼滤波器算法融合定位的车辆的信息数据包输入更新后的GNSS误差预测模型，得到未通过所述扩展卡尔曼滤波器算法融合定位的车辆的位置信息。

根据本发明的第二方面，提供一种车辆定位装置，应用于配置有扩展卡尔曼滤波器算法的路侧设备，定位装置包括：

接收模块，用于接收位于所述路侧设备通信范围内的车辆广播的信息数据包；

第一融合定位模块，用于当所述信息数据包对应的车辆数量不超过所述扩展卡尔曼滤波器算法的状态矩阵维度时，利用所述扩展卡尔曼滤波器算法分别对每个车辆对应的信息数据包和每个车辆距所述路侧设备的相对距离进行融合，得到每个车辆的位置信息；

第二融合定位模块，用于当所述信息数据包对应的车辆数量超过所述扩展卡尔曼滤波器算法的状态矩阵维度时，利用选择策略选择出与所述状态矩阵维度相同数量的车辆，并利用所述扩展卡尔曼滤波器算法对选择出的每个车辆对应的信息数据包和每个车辆距所述路侧设备的相对距离进行融合，得到选择出的每个车辆的位置信息；

第三定位模块，用于利用选择出的每个车辆的位置信息和对应的信息数据包更新GNSS误差预测模型，将更新后的GNSS误差预测模型共享给未通过所述扩展卡尔曼滤波器算法融合定位的车辆，未通过所述扩展卡尔曼滤波器算法融合定位的车辆通过更新后的GNSS误差预测模型得到自身位置。

根据本发明的第三方面，提供一种设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现所述的一种车辆定位方法的步骤。

根据本发明的第四方面，提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现所述的一种车辆定位方法的步骤。

与现有技术相比，本发明至少具有以下有益效果：本发明提供的一种车辆定位方法，通过将车辆距路侧设备的相对距离纳入扩展卡尔曼滤波器算法中，避免了短时间内车辆信息数据包丢失对定位结果带来的影响，通过向未通过扩展卡尔曼滤波器算法融合定位的车辆共享GNSS误差预测模型参数，扩大了路侧设备的定位作用范围，GNSS误差预测模型在路侧设备训练，对车辆计算性能要求较低，易于推广。

为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

为了更清楚地说明本发明具体实施方式中的技术方案，下面将对具体实施方式描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例一种车辆定位方法的流程示意图；

图2是车辆定位信息校正系统组成及功能图；

图3是解算器滑动窗口格式；

图4是SEL建模过程；

图5是二次数据集形成过程；

图6是市区和郊区场景中两个轴向上的RMSE(关于直线轨迹中的距离误差信息)，其中，(a)为x轴向上的RMSE(郊区场景)，(b)为y轴向上的RMSE(郊区场景)，(c)为x轴向上的RMSE(市区场景)，(d)为y轴向上的RMSE(市区场景)；

图7是市区和郊区场景中两个轴向上的RMSE(关于曲线轨迹中的距离误差信息)，其中，(a)为x轴向上的RMSE(郊区场景)，(b)为y轴向上的RMSE(郊区场景)，(c)为x轴向上的RMSE(市区场景)，(d)为y轴向上的RMSE(市区场景)。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本发明实施例提供的一种车辆定位方法，应用于配置有扩展卡尔曼滤波器算法的路侧设备，定位方法具体包括以下步骤：

步骤一、接收位于路侧设备(RSU)通信范围内的车辆广播的信息数据包。

具体而言，道路上的待定位车辆通过无线电设备周期性广播信息数据包，RSU对其进行接收。与此同时，还通过传感器获取RSU与每个车辆的相对距离。

步骤二、当信息数据包对应的车辆数量不超过扩展卡尔曼滤波器算法的状态矩阵维度时，利用扩展卡尔曼滤波器算法分别对每个车辆对应的信息数据包和每个车辆距路侧设备的相对距离进行融合，得到每个车辆的位置信息。

也就是说，路侧设备接收到车辆信息数据包后利用EKF滤波器对车辆位置进行估计，并和RSU自身感知的车辆相对距离进行融合实现定位精度的提升，具体包括滤波器预测阶段、解算器处理阶段和滤波器更新阶段。

1、滤波器预测阶段

在预测阶段，利用每个车辆对应的信息数据包，估计当前时刻该车辆的位置信息预测值。

预测阶段是EKF进入处理的第一阶段，其本质是寻找状态向量上一时刻t_k-1后验估计值与当前时刻t_k先验估计值之间的对应关系。在t_k-1时刻，状态向量由RSU接收到的车辆实时运动信息组成。因而，状态向量的维度取决于参与定位的车辆数目m。

t_k-1时刻的状态向量定义为：

针对车辆v_i，i＝1,2,…,m是ID，用于唯一标识信息；T表示对矩阵进行转置运算；xⁱ _k-1表示在t_k-1时刻与车辆v_i相关的运动信息。更准确的说，xⁱ _k-1包含车辆v_i的二维笛卡尔位置分量、速度、加速度、航向角：

其中，pⁱ _x(t_k-1)、pⁱ _y(t_k-1)是通用横墨卡托格网系统(Universal TransverseMercator Grid System，UTM)转换为以m为单位的东北方坐标，x轴正方向代表正东，y轴正方向代表正北；vⁱ(t_k-1)、aⁱ(t_k-1)、θⁱ(t_k-1)分别为车辆v_i的速度、加速度、航向角。

需要说明的是，每辆车的状态向量xⁱ _k-1都绑定着一个有效时间T_v(1≤i≤m)，这里统一设置为5s。在每次进入预测前，滤波器计算从车辆v_i接收最新信息数据包所经过的时间T_i(即当前时刻t_k与数据包时间戳之间的差值)，通过比较该值与有效时间T_v的大小关系，以判别是否丢弃与车辆v_i相关的状态向量。从而，减少计算资源的浪费和保证状态信息的时效性。

对于车辆运动学，定位算法假设遵循匀变速模型：

x_k＝g(x_k-1,w_k-1) (3)

在外界因素干扰下，w_k-1是上一状态向当前状态变化过程中的噪声，这里假设为一个随时间变化的独立向量；g(x_k-1,w_k-1)是一个5m×1的非线性状态转移矩阵：

对于匀变速模型，状态估计中的过程噪声为加速度的一阶导数与转弯速率基于/>推导出位移与加速度一阶导数的对应关系进而得到过程噪声wⁱ _k：

具体来说，w_k-1是一个具有协方差矩阵Q的零均值随机向量：

w～P(0,Q) (7)

Q＝diag(Q¹,Q²,...,Q^m) (8)

其中，diag(Q¹,Q²,…,Q^m)表示对角矩阵，Q¹,Q²,…,Q^m为对角元素。关于协方差矩阵Qⁱ矩阵的公式推导：

可取：

综上，时间更新方程为：

其中，x_k-1|k-1为t_k-1时刻测量校正后的估计状态值；x^- _k|k-1为t_k时刻测量校正前的估计状态值；P_k-1|k-1是t_k-1时刻测量校正后的估计协方差；P^- _k|k-1为t_k时刻测量校正前的估计协方差；J是以J¹,J²,…,J^m为对角元素的矩阵，而Jⁱ为关于车辆v_i的状态转移函数的雅可比矩阵。这是因为上述矩阵中描述的问题是非线性的，可以应用EKF来线性化它。为此，用雅可比矩阵(偏导数)估计J：

J＝diag(J¹,J²,...,J^m)(13)

其中，

2、解算器处理阶段

在解算器处理阶段，判断解算器中的信息数据包是否满足当前时刻该车辆的位置信息解算条件，若满足，则将解算器求解的该车辆的位置信息作为当前时刻该车辆的位置信息测量值；若不满足，则将当前时刻该车辆的信息数据包作为当前时刻该车辆的位置信息测量值。

(1)距离测量

使用毫米波雷达、激光雷达和照相机作为传感器，激光雷达和照相机相比于毫米波雷达，计算成本高、同时目标识别分辨率更高，结合每种传感器各自的优势进行RSU与目标车辆的相对距离感知。

(2)解算器设计

较短时间间隔内，目标车辆v_i的运动状态不会有较大变化，航向角、加速度可视为恒定值。即θⁱ(t_k)、aⁱ(t_k)为T∈[t_k,t_k+1)期间的航向角、加速度，可由t_k时刻的车载INS得到。而pⁱ(t_k)＝[p_x(t_k)p_x(t_k)]^T为t_k时刻车辆v_i的位置。

RSU接收得到θⁱ(t_k-1)、vⁱ(t_k-1)和aⁱ(t_k-1)后，未知车辆位置pⁱ(t_k)被描述为：

由此得到任意时刻(1≤t_n≤t_k)的运动学模型：

对应的距离差信息定义为：

式(16)可简化为：

设d_n为t_n时刻车辆v_i与RSU之间的相对距离信息，得：

假设从滤波器工作开始，已知车辆v_i任意两个时刻(以t_k-1，t_k-2为例)的航向角、加速度，可由pⁱ(t_k)反推出{pⁱ(t_k-1),pⁱ(t_k-2)}。其中，待估计的未知参数为p(t_k)＝[p_x(t_k)p_x(t_k)]^T。将式(19)代入(20)中，得：

对于最大设计尺寸为m_max的滤波器，解算器内置m_max个5×2大小的滑动窗口，用于存储前3s内的最新车辆运动信息与RSU感知距离。单个滑动窗口格式见图3，具体为从信息数据包中提取的速度、加速度、航向角和时间戳，以及源自传感器测量的相对距离。

在没有接收到任何信息数据包时，解算器按照计算频率f_s对每个存储窗口进行检查，如符合式(21)，则得到4组关于车辆位置的解，并选择与当前时刻滤波器预测最接近的值。

3、滤波器更新阶段

在更新阶段，利用当前时刻该车辆的位置信息预测值和当前时刻该车辆的位置信息测量值，估计得到每个车辆的位置信息。

更新阶段主要通过状态向量的先验估计值和当前时刻t_k测量值改进车辆v_i位置。在这个过程中，当前时刻t_k(对应于RSU感知目标车辆v_i相对距离数据的时间戳)与目标车辆v_i运动信息相关联的时刻(对应于目标车辆v_i发送信息数据包的时间戳)之间存在间隔。然而，这种差异通常非常小，可以认为速度、航向角信息是相同的。

需要注意的是，RSU接收的信息数据包中包含GNSS接收机提供的位置信息。但是，在高楼大厦林立的城市街区场景中，GNSS坐标通常是不准确的。因此，V2X车辆定位方法不考虑该信息，仅选择速度、航向角作为观测值，以获取更高的定位精度。

过滤器t_k时刻的状态更新是根据下面两种情况之一进行的。

情况1：RSU接收到车辆v_i发送的信息数据包，但解算器不满足式(21)中的计算条件。

获取信息数据包时，毫米波雷达、激光雷达和照相机会同步感知RSU与车辆v_i间的相对距离。随后，该信息与数据包提取的航向角、速度和加速度一起输入到解算器中，以为未来时刻的位置估计作准备。

滤波器将参与定位车辆的速度、航向角作为整个测量矢量，表示为：

z_k＝h_k(x_k)+v_k (22)

h_k(x_k)中行向量的个数与参与定位的车辆数一致：

由式(2)得：

v_k是一个具有协方差矩阵R_k的零均值随机向量：

v～P(0,R)(26)

其中，和/>分别为车辆v_i在t_k处行驶速度、航向角的标准差。

EKF的更新阶段执行如下：

其中，K_k是卡尔曼增益，也称卡尔曼系数；z_k-Hx^- _k|k-1是传感器实际测量值和滤波器估计测量值的残差，乘以卡尔曼增益，修正t_k时刻测量校正前的估计状态值x^- _k|k-1；H为状态向量到测量矢量的转换矩阵：

其中，表示克罗内克积；矩阵B的定义为：

情况2：RSU没有接收到车辆v_i发送的信息数据包，但解算器能够求解车辆位置。

如果解算器的求解(如前所述，由本地时钟每1/f_s秒触发，独立于任何V2I通信)发生在系统不忙于处理数据包时，那么滤波器将标记t_k这样的时刻进行独立更新。为了达到目的，它执行与上面讨论的相同操作，即(28)、(29)和(30)。而对于线性映射h_k(x_k)，仅包括解算器得到的二维笛卡尔位置分量：

z_k＝h_k(x_k)+v_k (34)

h_k(x_k)中行向量的个数与参与定位的车辆数一致：

由式(2.2)得：

v_k是一个零均值高斯随机向量，代表带有协方差矩阵R_k的测量噪声：

v～P(0,R)(38)

其中，和/>分别为车辆v_i在t_k处x、y轴向上位置的标准差。

步骤三、当信息数据包对应的车辆数量超过扩展卡尔曼滤波器算法的状态矩阵维度时，利用选择策略选择出与状态矩阵维度相同数量的车辆，并利用扩展卡尔曼滤波器算法对选择出的每个车辆对应的信息数据包和每个车辆距路侧设备的相对距离进行融合，得到选择出的每个车辆的位置信息。

通俗的说，当状态向量与测量矢量的维度达到滤波器最大设计尺寸，RSU就会进入满负荷工作模式：保持由接收到的信息数据包触发更新滤波器，并依据V2X目标车辆选择策略将某已追踪车辆相关的状态信息替换为陌生车辆vj的。

1、RSU初始工作模式

RSU初始工作模式主要涉及协方差矩阵R_k和P_0|0的设置。关于R_k，已知要过滤的测量矢量由参与定位车辆的速度、航向角或解算器求解坐标组成。RSU接收的信息数据包携带着速度、航向角测量以及对应的方差，由配备在车辆v_i上的INS获取。

至于P_0|0的设置，取决于滤波器的工作状态。刚开始，在没有与任何车辆进行通信的情况下，滤波器在睡眠模式下运行，毫米波雷达、激光雷达和照相机也处于待机状态。直至RSU收到第一个信息数据包时，滤波器才进行初始化。此时，状态向量和协方差矩阵可表示为：

x_k|k＝[xⁱ]^T (40)

其中，xⁱ为车辆v_i的实时运动学信息，由最佳位置、速度、加速度和航向角估计值组成；Pⁱ _k|k是一个5×5的对角矩阵，记录了相关的标准偏差：

2、状态替换算法

RSU首次接收到车辆v_j广播的信息数据包时，滤波器就会检查当前参与定位的车辆数m。当m<m_max时，车辆v_j的实时运动信息被选择纳入滤波器状态。而当m＝m_max时，必须做出决定：如果估计当前滤波器已追踪车辆的位置都比陌生车辆v_j的位置更为有效，应丢弃接收到的信息数据包；相反，如果估计陌生车辆v_j的位置至少比已追踪车辆中的一辆有益，则与车辆v_i(即“最差”车辆)相关的运动信息替换为车辆v_j。

可以设想一种策略来确定车辆v_j是否能给V2X车辆定位方法带来优势，后面会进行详细介绍。在车辆v_i运动信息替换的情况下，需要重新定义当前状态向量。为了实现这一目标，算法首先执行预测/更新步骤，然后进行以下交换：

也就是说，与车辆v_i相关的二维笛卡尔位置分量、速度、加速度和航向角被与车辆v_j相关的新部分所取代：

协方差矩阵P_k|k也要进行替换。但不同的是，P_k|k具有非对角元素，这些元素与在t_k时刻前滤波器状态中创建的变量相关。而由于交换，非对角元素将不再代表t_k时刻状态下新变量集之间的实际相关性。因此，新的P_k|k将忽略与已追踪车辆v_i相关的非对角元素，并更换为对应陌生车辆v_j的5×5维对角块：

其中，和/>分别为信息数据包中陌生车辆v_j在t_k处沿x方向和y方向位置的标准偏差。而/>和/>分别为陌生车辆v_j在t_k处行驶速度、加速度和航向角的标准偏差。

3、V2X目标车辆选择策略

前面已知，滤波器每当目标车辆发送的信息数据包到达时执行更新步骤。在这个过程中，滤波器对所有发送车辆的位置进行估计，其状态维数是逐步增加的。其中，每辆车会向状态向量贡献5个变量，意味着m辆车需进行大小为5m×5m的矩阵非线性运算。考虑到大规模非线性处理的复杂程度，m_max不可能是一个无限大的数值。这样，需要设计一个策略从滤波器状态中允许/排除，以确定在满负荷工作模式下被替换的车辆。

显然，当定位过程基于滤波器时，传感器距离测量精度与目标车辆运动信息质量对于算法性能至关重要。一方面，RSU感知较远车辆的相对距离信息不如较近车辆的准确。这说明尽可能接收由靠近RSU车辆发送的信息数据包，舍弃由远离RSU车辆发送的信息数据包。因此，对于每种距离范围使用不同的分数s，以评价相对距离对传感器测量精度的影响，如表1所示。

表1距离范围的权重

另一方面，GNSS坐标的准确性对算法性能也有一定影响，因为它被用于初始化滤波器中相应车辆的位置状态值。同时，每辆车的状态向量都绑定着T_i，T_i越大意味着距离接收车辆v_i数据包的时间越长。

考虑上述因素的影响，定义了一个替换指标：

其中，σ_x和σ_y分别表示与x、y方向GNSS位置信息相关的标准差。当T_i超过5s，判定目标车辆已远离RSU的通信范围。此时，通过设置T＝-∞，将自动使用任何新的车辆进行替换。

基于上述信息，可以设计一种RSU满负荷工作模式下的选择策略。步骤如下：(1)标准偏差σ_x和σ_y使用P_k|k中对应矩阵块的当前值进行更新；s根据传感器实际测距值d在表1中寻找；T_i则是当前时刻t_k与信息数据包时间戳之间的差值；(2)将σ_x、σ_y、w和T_i代入计算S_v；(3)将S_v与最小S_vi进行比较。如果S_v>S_vi，则替换对应车辆的实时运动信息。否则，将接收到的信息数据包丢弃。

将车辆按照定位方式不同进行分组，由RSU通过步骤二中的方法进行定位的车辆划分为V2I定位车辆组，RSU进入满负荷工作模式而未能通过V2I方式定位的车辆划分为V2V校正车辆组。

按照不同的定位方式，将行驶车辆划分为V2V校正车辆组V_c＝{v_g}_g＝1,2,..,n与V2I定位车辆组V_h＝{v_i}_i＝1,2,..,m。对于车辆v∈V_h，在时刻t_k具有准确的位置坐标P_k＝[p_x(t_k)，p_y(t_k)]，源自RSU滤波器中的相应状态值。当然，所有车辆都能获取初始GNSS坐标P_g，但通常以经度和纬度为单位进行显示。为了与滤波器中的状态信息保持相同的数值尺度，假设RSU在接收车辆v的初始GNSS坐标后，对其UTM转换为以m为单位的量。这样一来，问题就是如何使用{P_g}和{P_k|v∈V_h}来近似{P_k|v∈V_c}。

步骤四、利用选择出的每个车辆的位置信息和对应的信息数据包更新GNSS误差预测模型，将更新后的GNSS误差预测模型共享给未通过扩展卡尔曼滤波器算法融合定位的车辆，未通过扩展卡尔曼滤波器算法融合定位的车辆通过更新后的GNSS误差预测模型得到自身位置。

具体而言，对GNSS误差进行分析，使用SEL框架搭建GNSS误差预测模型，用于模拟这种复杂的非线性映射，实现对RSU临近区域内所有车辆定位误差的校正。

1、GNSS误差定义

GNSS误差E根据性质可以分为系统误差E_s和随机误差E_r：

E＝E_s+E_r (47)

系统误差主要来自卫星轨道和时钟误差、电离层延迟、对流层延迟以及接收机时钟和定位误差，在一定范围内(约50–200km)为每个GNSS接收机提供相同的误差幅度和方向。随机误差主要包括多径效应和接收机噪声导致的误差。

对于任意时刻t_k行驶在相同道路上的车辆v_m和v_n，其相对误差||ΔE_mn||满足：

||ΔE_mn||＝||(P_m-P_n)||-||(P′_m-P′_n)||

≤||(P_m-P_n)-(P′_m-P′_n)|| (48)

≤||E_m-E_n||

其中，P_m、P_n、P′_m、P′_n、E_m和E_n分别是车辆m和车辆n的GNSS坐标、准确的位置坐标和GNSS误差。由(47)与(48)可得：

其中，E_ms、E_ns、E_mr和E_nr分别为车辆m和车辆n关于GNSS定位的系统误差和随机误差。

对于相同类型的GNSS接收机，定位误差半径是固定的。且同一时刻，每辆车可以观测到的卫星组合基本相同。因此，由卫星时钟差、大气延迟等引起的卫星位置偏差几乎一样。此外，对于处于相同路段或区域内的两辆车，相较于车辆到卫星的距离，它们之间的位置差异可以忽略不计。综上，对于任意t_k时刻处于同一区域内的车辆，可将卫星、大气和多径效应作用于GNSS的扰动视作相同量，即系统误差E_s几乎相同：

随机误差E_r与系统误差E_s相比非常小，因此，可将同一路段或区域内车辆的GNSS误差视作相同的量。

2、GNSS误差预测模型

V2V校正车辆组V_c＝{v_g}_g＝1,2,..,n的定位，本质上是寻找同一路段或区域内车辆运动信息与GNSS误差的关系。流行的深度学习算法需要大量数据进行训练，而RSU中滤波器的最大设计尺寸m_max通常不是很大，能追踪V2I定位车辆组V_h＝{v_i}_i＝1,2,..,m的状态信息有限。因此，应选择优秀的机器学习方法来建立GNSS误差预测模型。

如果输入特征具有复杂的结构和非线性关系，单独的预测模型可能无法满足精度需求。目前，许多研究通过混合或集成两种及两种以上的算法提高预测性能。鉴于系统误差E_s是多因素共同干扰的结果，本文考虑使用堆叠集成学习(Stacking Ensemble Learning，SEL)构建GNSS误差预测模型。模型搭建过程如下：

V2I定位车辆组V_h＝{v_i}_i＝1,2,..,m的数据集为D＝{(x₁,y₁),(x₂,y₂),…,(x_m,y_m)}。x_i代表样本特征，即车辆v_i的状态向量[pⁱ _gx,pⁱ _gy,vⁱ,aⁱ,θⁱ]。其中，pⁱ _gx与pⁱ _gy表示在x、y轴上的GNSS分量。y_i代表目标值，即车辆v_i在x、y轴上的位置分量偏差[Eⁱ _x,Eⁱ _y]，由滤波器中的位置状态值与初始GNSS坐标作差得到。三种基学习算法SVR、XGBoost和Catboost分别定义为ζ₁、ζ₂和ζ₃。作为元学习算法，Lasso被定义为ζ。

(1)原始数据集D＝{(x₁,y₁),(x₂,y₂),…,(x_m,y_m)}划分为原始训练集Train和原始测试集Test，且原始训练集Train的所有数据被随机等分为5个子集Train₁，Train₂，Train₃，Train₄，Train₅。

(2)每次选择4个子数据集训练基学习器，剩余1个子数据集用于验证预测的性能，然后从第i个学习算法ζ_i中优化基学习器f_i ^(j)的具体参数。该过程重复执行5次。

(3)使用训练好的基学习器f_i ^(j)对不参与建模的样本进行预测，结果表示为f_i ^(j)(x_i)。

(4)形成二次训练集Train′＝{(x₁′,y₁),(x₂′,y₂),…,(x_m′,y_m)}，以用于训练元学习器f′＝ζ(Train′)。其中，x_i′＝(f₁ ^(j)(x_i),f₂ ^(j)(x_i),f₃ ^(j)(x_i))，y_i仍然是原始训练集Train中的目标值。

(5)对于测试过程，基学习器f_i ^(j)获得5次原始测试集Test相应的预测结果，描述为f_i ⁽¹⁾(x_t)、f_i ⁽²⁾(x_t)、f_i ⁽³⁾(x_t)、f_i ⁽⁴⁾(x_t)和f_i ⁽⁵⁾(x_t)。通过平均所有预测结果，得到预测向量：

三个基学习器产生三个预测向量，即因为原始训练集Train中的y_i′没有改变，所以二次测试集可以表示为Test′＝{(x₁″,y₁′),(x₂″,y₂′),…,(x_n″,y_n′)}。

通过将Test′应用于元学习器f′，获得最终预测结果F(x)＝f′(Test′)。此时，由于所有数据都经过了一次训练和预测，数据相似度已经很高，如果继续选择更复杂的模型进行训练，很容易导致数据的过度拟合。因此，通常使用Lasso作为元学习器。

SEL建模过程和二次数据集形成过程分别如图4和图5所示。

为了评估所提EKF算法可达到的定位精度，使用城市交通仿真(Simulation OfUrban Mobility，SUMO)构建车辆行驶场景，C++目标模块化网络测试平台(ObjectiveModular Network Testbed in C++，Omnet++)以及Veins框架模拟RSU与车辆之间的通信，并基于矩阵实验室(Matrix Laboratory，MATLAB)进行数据分析与计算。选择西安市市区电子城和西安市郊区凤城五路街道、兴中东路街道作为实验场景，旨在城市环境中验证算法的可行性，图6和图7展示了城市和郊区场景中所提定位方法与原始位置信息在直线和曲线轨迹下的均方根误差，结果显示本发明方法具有良好的定位效果。

本发明实施例还提供了一种车辆定位装置，应用于配置有扩展卡尔曼滤波器算法的路侧设备，定位装置包括：

接收模块，用于接收位于路侧设备通信范围内的车辆广播的信息数据包。

第一融合定位模块，用于当信息数据包对应的车辆数量不超过扩展卡尔曼滤波器算法的状态矩阵维度时，利用扩展卡尔曼滤波器算法分别对每个车辆对应的信息数据包和每个车辆距路侧设备的相对距离进行融合，得到每个车辆的位置信息。

第二融合定位模块，用于当信息数据包对应的车辆数量超过扩展卡尔曼滤波器算法的状态矩阵维度时，利用选择策略选择出与状态矩阵维度相同数量的车辆，并利用扩展卡尔曼滤波器算法对选择出的每个车辆对应的信息数据包和每个车辆距路侧设备的相对距离进行融合，得到选择出的每个车辆的位置信息。

第三定位模块，用于利用选择出的每个车辆的位置信息和对应的信息数据包更新GNSS误差预测模型，将更新后的GNSS误差预测模型共享给未通过扩展卡尔曼滤波器算法融合定位的车辆，未通过扩展卡尔曼滤波器算法融合定位的车辆通过更新后的GNSS误差预测模型得到自身位置。

其中，关于上述各个模块的具体工作流程可以参考前述实施例中公开的相应内容，在此不再进行赘述。

本发明在一个实施例中，提供了一种计算机设备，该计算机设备包括处理器以及存储器，所述存储器用于存储计算机程序，所述计算机程序包括程序指令，所述处理器用于执行所述计算机存储介质存储的程序指令。处理器可以是中央处理单元(CentralProcessing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital SignalProcessor、DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable GateArray，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等，其是终端的计算核心以及控制核心，其适于实现一条或一条以上指令，具体适于加载并执行一条或一条以上指令从而实现相应方法流程或相应功能；本发明实施例所述的处理器可以用于实现一种车辆定位方法的操作。

本发明在一个实施例中，一种车辆定位方法如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读存储介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。

所述计算机存储介质可以是计算机能够存取的任何可用介质或数据存储设备，包括但不限于磁性存储器(例如软盘、硬盘、磁带、磁光盘(MO)等)、光学存储器(例如CD、DVD、BD、HVD等)、以及半导体存储器(例如ROM、EPROM、EEPROM、非易失性存储器(NANDFLASH)、固态硬盘(SSD))等。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (8)

1.一种车辆定位方法，其特征在于，应用于配置有扩展卡尔曼滤波器算法的路侧设备，定位方法包括：

接收位于所述路侧设备通信范围内的车辆广播的信息数据包；

当所述信息数据包对应的车辆数量不超过所述扩展卡尔曼滤波器算法的状态矩阵维度时，利用所述扩展卡尔曼滤波器算法分别对每个车辆对应的信息数据包和每个车辆距所述路侧设备的相对距离进行融合，得到每个车辆的位置信息；

当所述信息数据包对应的车辆数量超过所述扩展卡尔曼滤波器算法的状态矩阵维度时，利用选择策略选择出与所述状态矩阵维度相同数量的车辆，并利用所述扩展卡尔曼滤波器算法对选择出的每个车辆对应的信息数据包和每个车辆距所述路侧设备的相对距离进行融合，得到选择出的每个车辆的位置信息；

利用选择出的每个车辆的位置信息和对应的信息数据包更新GNSS误差预测模型，将更新后的GNSS误差预测模型共享给未通过所述扩展卡尔曼滤波器算法融合定位的车辆，未通过所述扩展卡尔曼滤波器算法融合定位的车辆通过更新后的GNSS误差预测模型得到自身位置。

2.根据权利要求1所述的一种车辆定位方法，其特征在于，利用所述扩展卡尔曼滤波器算法分别对每个车辆对应的信息数据包和每个车辆距所述路侧设备的相对距离进行融合，得到每个车辆的位置信息，具体如下：

所述扩展卡尔曼滤波器算法包括预测阶段、解算器处理阶段和更新阶段；

在所述预测阶段，利用每个车辆对应的信息数据包，估计当前时刻该车辆的位置信息预测值；

在所述解算器处理阶段，判断解算器中的信息数据包是否满足当前时刻该车辆的位置信息解算条件，若满足，则将解算器求解的该车辆的位置信息作为当前时刻该车辆的位置信息测量值；

若不满足，则将当前时刻该车辆的信息数据包作为当前时刻该车辆的位置信息测量值；

在所述更新阶段，利用当前时刻该车辆的位置信息预测值和当前时刻该车辆的位置信息测量值，估计得到每个车辆的位置信息。

3.根据权利要求2所述的一种车辆定位方法，其特征在于，所述判断解算器中的信息数据包是否满足当前时刻该车辆的位置信息解算条件，具体采用下式进行：

式中，p_x(t_k)、p_y(t_k)分别为t_k时刻x、y方向上的车辆位置；Δx_n和Δy_n为车辆在n时刻与t_k时刻x、y方向上在的距离差；和/>为路侧设备位置；d_n为车辆距路侧设备的相对距离。

4.根据权利要求1所述的一种车辆定位方法，其特征在于，所述选择策略具体为：

将所述路侧设备通信范围内的每个车辆距所述路侧设备的相对距离，以及信息数据包中的时间戳和当前时刻的时间间隔，作为替换指标计算公式的考虑因素，所述替换指标计算公式如下：

式中，T代表当前时刻距离接收车辆信息数据的时间间隔；s为根据车辆与路侧设备的相对距离而设置的权重；σ_x、σ_y分别表示与x、y方向GNSS位置信息相关的标准差。

5.根据权利要求1所述的一种车辆定位方法，其特征在于，所述未通过所述扩展卡尔曼滤波器算法融合定位的车辆通过更新后的GNSS误差预测模型得到自身位置，具体如下：

将未通过所述扩展卡尔曼滤波器算法融合定位的车辆的信息数据包输入更新后的GNSS误差预测模型，得到未通过所述扩展卡尔曼滤波器算法融合定位的车辆的位置信息。

6.一种车辆定位装置，其特征在于，应用于配置有扩展卡尔曼滤波器算法的路侧设备，定位装置包括：

接收模块，用于接收位于所述路侧设备通信范围内的车辆广播的信息数据包；

第一融合定位模块，用于当所述信息数据包对应的车辆数量不超过所述扩展卡尔曼滤波器算法的状态矩阵维度时，利用所述扩展卡尔曼滤波器算法分别对每个车辆对应的信息数据包和每个车辆距所述路侧设备的相对距离进行融合，得到每个车辆的位置信息；

第二融合定位模块，用于当所述信息数据包对应的车辆数量超过所述扩展卡尔曼滤波器算法的状态矩阵维度时，利用选择策略选择出与所述状态矩阵维度相同数量的车辆，并利用所述扩展卡尔曼滤波器算法对选择出的每个车辆对应的信息数据包和每个车辆距所述路侧设备的相对距离进行融合，得到选择出的每个车辆的位置信息；

第三定位模块，用于利用选择出的每个车辆的位置信息和对应的信息数据包更新GNSS误差预测模型，将更新后的GNSS误差预测模型共享给未通过所述扩展卡尔曼滤波器算法融合定位的车辆，未通过所述扩展卡尔曼滤波器算法融合定位的车辆通过更新后的GNSS误差预测模型得到自身位置。

7.一种设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至5任一项所述的一种车辆定位方法的步骤。

8.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至5任一项所述的一种车辆定位方法的步骤。