CN117014815A - 一种多传感器多车辆协同定位系统与方法 - Google Patents

Abstract

本发明提供一种多传感器多车辆协同定位系统与方法，涉及车辆定位技术领域；该系统包括GPS接收模块、IMU模块、车内数据采集模块、车外数据采集模块、强化学习模块和滤波融合模块；GPS接收模块用于接收卫星信号；所述IMU模块用于获得车辆的角速度、速度和加速度估计；车内数据采集模块用于采集车辆的轮速数据、转向角及里程数据；车外数据采集模块用于得到相对于其他合作车辆的相对位置和速度估计；强化学习模块，用以提高GPS设备的定位精度；滤波融合模块对多种信息进行融合，得到车辆准确的位置信息；本发明融合了来自本地车载传感器的信息，以及其他车辆或现有智能交通系统基础设施(如道路侧单元)的观测，以提高定位和移动跟踪的精度和稳定性。

Description

一种多传感器多车辆协同定位系统与方法

技术领域

本发明涉及车辆定位技术领域，尤其涉及一种多传感器多车辆协同定位系统与方法。

背景技术

车辆定位是自动驾驶的基本任务，为指导智能交通系统(ITS)的运行提供必要的信息，如多车协作时的准确位置的确定。近年来，许多研究人员一直在尝试开发新的技术来提供准确的车辆定位信息，包括早期简单且经济的航位推算技术，目前流行的基于GPS的技术，以及最近的基于标记和高精度地图的技术。GPS定位容易受到工作环境的影响，在某些情况下(如拥挤的城市、桥下、高层建筑旁、隧道等)无法提供所需的精度。虽然基于地图的定位技术能够提供更精确的定位，但是其依赖于高精度的地图，这会产生额外的高成本，因此不是大多数ITS应用的最佳选择。

针对仅基于单个传感器的技术的限制，研究人员尝试使用多个传感器提供的信息进行定位和移动跟踪，以获得更高的准确性。在这个框架内，一些传统的滤波算法，例如粒子滤波器和卡尔曼滤波器等，被应用于融合来自不同传感器的测量和它们的先验信息，以获得更准确的估计值。然而，在现有文献中，大多数解决方案仅在所有传感器都位于同一车辆时才能融合和利用不同传感器的信息。此外，尽管该过程涉及了各种传感器，但准确性仍然严重依赖于GPS，并且对环境非常敏感。总之，现有的单车多传感器策略仍无法应对主要挑战，例如在密集的交通环境中，一个或多个传感器信号可能经常被阻塞，从而导致车辆定位精度大幅降低进而影响车辆行驶安全。为克服上述限制，一些研究人员已提出解决方案，利用其他车辆的信息来增强单车传感器的稳定性。然而，现有的解决方案大多依赖于传统的定位传感器，因此需要在智能交通系统中构建其他支持基础设施来辅助定位过程。此外，这些基于传统定位传感器的技术假设车辆采用静态模型，因此无法进行有效的移动跟踪。换句话说，它们只能基于每个时间点的单个快照观测报告定位结果，并且无法利用一段时间内观测之间的时态关系。

对于未来“网联车”的愿景，如何使不同车辆之间相互协作，并结合来自这些车辆的传感器信息，以提高定位的准确性是一个重要的任务。最常见的技术是车对车(V2V)多基站技术，即车辆互相传输自身位置并计算与其他车辆之间的距离，然后获得更准确的自我定位估计。然而，大多数基于多基站的研究只使用了GPS、IMU和V2V无线通信模块，而不能应用其他车辆的雷达和激光雷达等传感器。综上所述，目前还没有一个通用的框架将从传统定位设备中获得的信息与更先进的传感设备相结合，来提高智能网联车辆的定位精度。

发明内容

针对现有技术存在的不足，本发明提供一种多传感器多车辆协同定位系统与方法。

一种多传感器多车辆协同定位系统，包括信号采集装置、中央处理单元以及通信装置；其中，信号采集装置负责获取车辆所处的位置信息，通信装置负责各车辆所采集信号的交互，中央处理单元负责对来自自身信号采集装置和通信装置的信号进行融合滤波从而得到车辆自身准确的位置信息；

所述信号采集装置包括GPS接收模块、IMU模块、车内数据采集模块以及车外数据采集模块；中央处理单元包括强化学习模块、自定位滤波融合模块、相对定位滤波融合模块以及全局定位滤波融合模块；

所述GPS接收模块，用于接收卫星信号，并将接收到的信号送入强化学习模块，并最终送入自定位滤波融合模块；

所述IMU模块，用于获得车辆的角速度、速度和加速度估计，并将测量得到的数据送入自定位滤波融合模块；

所述车内数据采集模块，用于采集车辆的轮速数据、转向角及里程数据，并将这些数据作为输入变量输入到自定位滤波融合模块；

所述车外数据采集模块，包括相机和雷达，用于得到相对于其他合作车辆的相对位置和速度估计；

所述强化学习模块，用于提高GPS的定位精度，以便在自动驾驶或半自动驾驶的情况下更准确地进行车辆定位；目标是找到关于观测到的GPS经度和纬度坐标的最佳校正策略，产生更准确的位置信息；

所述自定位滤波融合模块，通过将来自强化学习模块、IMU模块、车内数据采集模块以及车外数据采集模块的信息进行滤波融合，得到车辆自身准确的位置信息；

所述相对定位滤波融合模块，通过将自定位融合滤波模块得到的车辆自身位置信息与来自车外数据采集模块的相对于其他合作车辆的相对位置估计和速度估计进行滤波融合，得到其他合作车辆的位置信息；

所述全局定位滤波融合模块，通过将车辆自身位置信息与来自其他合作车辆的相对位置估计信息进行滤波融合，得到更加准确的车辆全局位置信息；

所述通信装置，用于现有智能交通系统中实现智能车之间的相互通信或智能车与路旁单元RSU之间的通信；

一种多传感器多车辆协同定位方法，基于上述一种多传感器多车辆协同定位系统实现，包括如下步骤：

步骤1、GPS接收模块进行卫星的捕获、跟踪、位同步、帧同步和定位解算，得到车辆所在位置的经度、维度和高程，并使用IMU模块获得车辆的角速度、速度和加速度估计，使用车内数据采集模块采集得到车辆的轮速数据、转向角以及里程数据；同时，通过车外数据采集模块获得相对于其他合作车辆的相对位置和速度估计；

步骤2、强化学习模块将GPS接收设备的经度和维度输出作为输入，并对估计的经度和维度执行校正操作，以提供更加准确的位置输出；

当收到一个新的数据点时，强化学习模型训练一个代理来确定观测到的经度和纬度需要调整的“单位”数量，以返回更准确的定位；这个顺序决策问题建模为一个部分可观察的马尔可夫决策过程POMDP；模型的目标是学习一个策略π(a|z,θ)，其中a表示动作向量，z表示观测向量，θ表示模型参数向量；策略的目标是参数化在给定某个观测值z时执行动作a的条件概率，以最大化自身的奖励；

以下介绍所提出的强化学习模型：1)动作空间：将动作定义为经度-纬度的更新操作；为了减少计算复杂度，将连续的经度和纬度值离散化为小步长；

2)观测和模型输入：GPS设备以一定的频率报告其位置；在提出的强化学习模型中，一个观测不仅限于最后报告的GPS位置，而是一个堆叠的向量，其中包含最后报告的位置以及最近一次预测的点的历史；不使用报告的GPS轨迹，而是使用模型预测来形成观测历史向量；预测频率设置为高于GPS数据收集频率的值；通过形成观测向量的这种方法，模型利用GPS设备的历史轨迹信息和模型的性能，从而学习一个高质量的策略来纠正报告的GPS点；

将时刻t的GPS报告点表示为q_t，其真实位置表示为g_t；由于真实位置是未知的，该问题被表述为一个部分可观察马尔可夫决策过程POMDP；在这个POMDP中，使用p_t表示GPS报告点q_t的置信状态，即在RL模型中表示为p_t；部分可观察的状态被其估计值，即置信状态，替代以形成MDP；使用大小为N的观测缓冲区Z_t来存储最近N-1个GPS报告点的历史模型估计和当前的q_t；即Z_t＝{p_t-N-1,…,p_t-1,q_t}；使用S_t和b_t分别表示Z_t的隐藏状态和置信状态；给定大小为N的观测缓冲区，在时间t，向量S_t包含这些点的相应的真实位置缓冲区，即S_t＝{g_t-N,…,g_t}。向量b_t包含最近N个点的估计，即b_t＝{p_t-N,…,p_t}；Z_t和b_t仅在其最后一个元素上有所不同，Z_t的最后一个元素是q_t，而向量b_t的最后一个元素是p_t；模型基于Z_t估计b_t；

根据上述的POMDP设置，强化学习代理在每个时间戳t的目标是找到最优的校正动作以纠正q_t；该过程基于一个滑动窗口进行；一旦接收到新的q_t，滑动窗口向前移动一步，形成一个新的观测向量，大小为N，其中q_t构成最后一个元素，最后的N-1个信念构成观测向量Z_t的其余部分；

每当GPS设备报告一个新的位置点q_t时，模型就会进行训练，并移动到下一个观测缓冲区；当通过GPS设备获取到p_t时，它被推送到观测缓冲区，以取代q_t，而观测则移动到时间t+1；在每个训练步骤中，强化学习模型的观测包含观察到的GPS点和一系列历史估计值；

3)置信度奖励：假设GPS观测的噪声是白噪声高斯分布，信心椭圆显示了模型预测的不确定性；当模型对其预测有把握时，信心椭圆较小；否则，信心椭圆的面积较大；当在时间t观察到一个新的GPS点时，不止做一次预测，而是训练模型k次，根据t-1时刻得到的模型参数和新的观测输入得到k个可能的输出预测；使用两种指标来衡量不确定性对强化学习模型性能的影响；这两种指标都基于代理的预测的协方差矩阵；该协方差矩阵是一个二维矩阵，表示对GPS观测的经度和纬度进行校正动作的方向的确定程度；设这个协方差矩阵的特征值为a和b；如果a和b都与零足够不同，使用它们的乘积计算置信椭圆的面积，公式为πab；否则，如果其中至少一个值接近于零，使用a+b作为预测置信度的代理度量；置信度测量越大，不确定性越低；使用不确定性的概念构建奖励函数；

尽管最大化置信度测量使预测的协方差最小化，但结果预测的位置不一定在道路限制范围内；因此，该模型利用数字地图信息来改善预测，将地图匹配纳入奖励函数中；地图匹配算法使用匹配过程将观测数据点(x，y)映射到一条道路上；因此，它们被看作是一个简单的搜索问题；当道路网络过于复杂或观测数据点与地面真实位置差距过大时，地图匹配可能不准确；因此，将地图匹配作为正则化项仅在提出的模型中纳入；假设观测集合(X，Y)的地图匹配结果为定义一步奖励为：

r＝z+γ×D

其中Z是置信度测量，γ是正则化参数，D是观测值与地图匹配结果之间平方误差的负值总和；即

在给定策略π下，定义τ＝{a₀,O₀,r₁,a₁,O₁,r₂,…,a_T-1,O_T-1,r_T-1,}为POMDP的一条轨迹，将轨迹τ上的总奖励作为策略的最终奖励返回；使用折扣因子η，奖励函数表示为：

模型的目标是学习一个策略，以在每次预测中最大化折扣奖励返回值R_τ；设置T＝8以通过R_t来近似R_τ，进行“n步回报”；

4)A3C训练架构：使用的A3C训练架构需要较短的训练会话，并提供更稳健的策略，A3C在多个线程上并行运行智能体；它通过让每个智能体保留状态或观测的副本，并独立地训练一个独立的模型，为智能体提供多样化的训练环境；

步骤3、自定位滤波融合模块将强化学习模块、IMU模块、车内数据采集模块的输出作为输入，利用卡尔曼滤波器KF进行滤波融合，得到车辆自身的位置信息；

设定卡尔曼滤波器KF的状态变量

其中，x_i y_i是车辆i在笛卡尔坐标系中的坐标，是车辆i的速度；则卡尔曼滤波器KF的状态转移方程和量测方程如下公式所示：

x[k]＝f(x[k-1],u[k],ω[k])

z[k]＝g(x[k],v[k])

其中，k为离散时点指标，x为车辆包含位置和速度的状态，u为命令过程，等效为驾驶输入，表示加速度，ω为命令噪声或状态噪声，来自命令过程的不确定性；z为IMU、GPS、Radar、相机等各种传感器上报的测量数据，v为测量和传输过程中的数据噪声；f和g分别是由运动的物理动力学和传感装置的固有特性得到的状态方程和测量模型；

卡尔曼滤波器KF进行滤波融合包括以下两个步骤：

步骤S1：时间更新:计算先验状态与状态转移雅可比矩阵来评估预测协方差；

步骤S2：测量更新：计算观测雅可比矩阵和卡尔曼滤波增益；

步骤4、相对定位滤波融合模块将自定位滤波融合模块的输出以及信号采集得到的相对于其他合作车辆的相对位置和速度估计作为输入，利用卡尔曼滤波器KF进行滤波融合，得到其他合作车辆的位置信息；

步骤5、通过通信装置，进行智能网联车之间的通信，交互步骤4中得到的合作车辆之间的位置信息；并且，如果车辆所处智能交通系统中存在RSU，则在车辆定位过程中会识别RSU，并获得从RSU到车辆的相对位置，同时RSU会向其通信范围内的车辆广播自身的绝对位置坐标；通过计算这些信息，车辆获得自己的绝对位置估计；由于RSU的位置信息是可信且准确的，误差只来自于车辆传感设备；因此，将RSU纳入协作中，提高定位的准确性；

步骤6、全局滤波融合模块将步骤3中自定位滤波融合模块的输出以及步骤5中来自其他合作车辆或RSU的关于自身的位置估计作为输入，利用卡尔曼滤波器KF进行滤波融合，车辆自身更加准确的位置估计；

全局滤波的目标是根据局部滤波器的输出计算车辆V_S的最优位置估计；车辆V_S的状态的自估计用表示，并带有协方差矩阵P_S，而来自车辆V_i的局部估计则用/>表示，并带有协方差矩阵P_i；局部滤波结果也是高斯分布的；因此，全局最优状态估计表示为局部估计的线性组合；

在上述假设下，全局估计记为：

其中A_i和A_s为待解线性组合的未知权重，A_i为其他车辆估计的权重，A_s为自估计的权重；的方差为：

为了保证全局估计的无偏性，估计的均值不能改变；那么A_i的约束条件：

根据上述高斯假设，的最大似然估计就是使方差最小的估计；因此，全局滤波成为一个优化问题：

用拉格朗日乘子法来求解凸优化问题，因此得到目标函数：

最后，在全局滤波器处得到线性组合的最优权值为：

权值与局部滤波性能成反比；

因此，全局最优估计的最终结果为：

对于感知RSU并与RSU通信的车辆，考虑到对RSU的测量，全局最优估计表达为：

其中：

由于RSU具有可靠和准确的位置信息，RSU提高RSU通信范围内车辆的定位精度；另一方面，由于车辆之间的合作，得到RSU的帮助的车辆进一步帮助不能直接感知和与RSU通信的车辆，从而提高它们的定位精度，从而改善整个网络中车辆的定位和追踪性能。

本发明有益技术效果：

本发明提供的一种多传感器多车辆(MSMV)定位框架。所提出的框架具有一个新颖的两层结构，包括全局滤波和局部滤波。在局部滤波器中提出了一个强化学习框架来提高GPS定位精度，一方面，使用传统的IMU和GPS数据来获得车辆自身状态的估计。另一方面，车辆观察其他车辆，并通过使用集成感知系统通过另一组局部滤波器生成感知数据来获得它们的状态。然后，来自所有车辆对同一目标车辆相关的局部估计被输入到全局滤波器中，以获得该目标的全局估计值，从而大大提高了估计的鲁棒性及准确性。定位过程是在车辆的动态模型的指导下进行的，因此也可以实现移动跟踪。所提出的MSMV框架是通用的，它不仅适用于各种类型的传感器，而且可以通过不同的技术实现局部滤波器来利用动态运动模型。此外，该框架还可以在合作定位和移动跟踪过程中包括智能基础设施，如路旁单元(RSUs)，以进一步提高性能。

附图说明

图1为本发明实施例提供的一种多传感器多车辆协同定位系统的结构示意图；

图2为本发明实施例提供的GPS精度提升强化学习模块的示意图；

图3为本发明实施例提供的一种多传感器多车辆协同定位方法的流程图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

本实例将待定位车辆记为V_S，它可以通过自身的车载传感器进行自身状态的自主估计，而在车联网中还有N辆其他车辆进行协作，它们可以对V_S进行观测以测量其状态。基于V_S自身的车载传感器数据，例如IMU和GPS等常用的传统定位设备，它可以通过局部滤波获得对自身状态的局部估计同时，每个协作车辆也可以观测到V_S，并通过局部滤波器获得对V_S的状态估计/>这些估计是基于它们通过传感设备进行的测量得出的V_S与它们自身之间的相对状态。结合自身定位的估计/>它们可以获得对V_S状态的估计这些局部滤波器的独立估计结果可以与V_S共享，然后通过在V_S处进行全局滤波将所有局部估计融合，得到x_S的全局估计。对于所有车辆都适用相同的情况，这形成了一个去中心化的框架。

一种多传感器多车辆协同定位系统，如图1所示，包括信号采集装置、中央处理单元以及通信装置；其中，信号采集装置负责获取车辆所处的位置信息，通信装置负责各车辆所采集信号的交互，中央处理单元负责对来自自身信号采集装置和通信装置的信号进行融合滤波从而得到车辆自身准确的位置信息；

所述信号采集装置包括GPS接收模块、IMU模块、车内数据采集模块以及车外数据采集模块；中央处理单元包括强化学习模块、自定位滤波融合模块、相对定位滤波融合模块以及全局定位滤波融合模块；

所述GPS接收模块，用于接收卫星信号，并将接收到的信号送入强化学习模块，并最终送入自定位滤波融合模块；

所述IMU模块，用于获得车辆的角速度、速度和加速度估计，并将测量得到的数据送入自定位滤波融合模块；

所述车内数据采集模块，用于采集车辆的轮速数据、转向角及里程数据，并将这些数据作为输入变量输入到自定位滤波融合模块；

所述车外数据采集模块，包括相机和雷达，用于得到相对于其他合作车辆的相对位置和速度估计；

所述强化学习模块，用于提高GPS设备的定位精度，以便在自动驾驶或半自动驾驶的情况下更准确地进行车辆定位；目标是找到关于观测到的GPS经度和纬度坐标的最佳校正策略，产生更准确的位置信息；整个过程类似于滤波—强化学习RL模型将GPS设备收集到的实时经纬度坐标作为输入，并利用该模型来改善定位；模型的输出是一个关于如何校正观测数据以生成更准确位置的动作策略；

在典型的GPS信息流中，GPS设备接收来自多个卫星的信号，每个信号都包含传输卫星的位置和信号传输时间。GPS设备可以根据信号发送时卫星的位置和信号传播的时间来定位自身。该过程如图2(a)所示。为了准确定位，GPS接收器需要从至少四颗卫星接收到延迟可忽略的高质量信号。虽然在大多数情况下接收器可以访问到所需数量的卫星，但由于各种因素的影响，信号质量通常会下降，尤其是在城市地区的高楼大厦或植被密集地区。

建议的框架类似于一个滤波器，它将GPS设备的典型经度和纬度输出作为输入，并对估计的经度和纬度执行“校正操作”，以提供更准确的输出。当收到一个新的数据点时，强化学习模型训练一个代理来确定观测到的经度和纬度需要调整的“单位”数量，以返回更准确的定位。该过程如图2(b)所示。从决策理论的角度来看，这个顺序决策问题可以建模为一个部分可观察的马尔可夫决策过程(POMDP)。模型的目标是学习一个策略π(a|z,θ)，其中a表示动作向量，z表示观测向量，θ表示模型参数向量。策略的目标是参数化在给定某个观测值z时执行动作a的条件概率，以最大化自身的奖励。

所述自定位滤波融合模块，通过将来自强化学习模块、IMU模块、车内数据采集模块以及车外数据采集模块的信息进行滤波融合，得到车辆自身准确的位置信息；

所述相对定位滤波融合模块，通过将自定位融合滤波模块得到的车辆自身位置信息与来自车外数据采集模块的相对于其他合作车辆的相对位置估计和速度估计进行滤波融合，得到其他合作车辆的位置信息；

所述全局定位滤波融合模块，通过将车辆自身位置信息与来自其他合作车辆的相对位置估计信息进行滤波融合，得到更加准确的车辆全局位置信息；

所述通信装置，用于现有智能交通系统中实现智能车之间的相互通信或智能车与路旁单元RSU之间的通信；

一种多传感器多车辆协同定位方法，如图3所示，基于上述一种多传感器多车辆协同定位系统实现，包括如下步骤：

步骤1、GPS接收模块进行卫星的捕获、跟踪、位同步、帧同步和定位解算，得到车辆所在位置的经度、维度和高程，并使用IMU模块获得车辆的角速度、速度和加速度估计，使用车内数据采集模块采集得到车辆的轮速数据、转向角以及里程数据；同时，通过车外数据采集模块获得相对于其他合作车辆的相对位置和速度估计；

步骤2、强化学习模块将GPS接收设备的经度和维度输出作为输入，并对估计的经度和维度执行校正操作，以提供更加准确的位置输出。当收到一个新的数据点时，强化学习模型训练一个代理来确定观测到的经度和纬度需要调整的“单位”数量，以返回更准确的定位。这个顺序决策问题可以建模为一个部分可观察的马尔可夫决策过程(POMDP)。模型的目标是学习一个策略π(a|z,θ)，其中a表示动作向量，z表示观测向量，θ表示模型参数向量。策略的目标是参数化在给定某个观测值z时执行动作a的条件概率，以最大化自身的奖励；

以下介绍所提出的强化学习模型的细节：1)动作空间：将动作定义为经度-纬度的更新操作。为了减少计算复杂度，将连续的经度和纬度值离散化为小步长。一般情况下，不建议离散化动作空间的每个维度，因为这样会指数级增加策略表的大小。然而，在低维动作空间中，离散化动作空间可以有助于降低算法的计算复杂度，正如本问题所示；

2)观测和模型输入：GPS设备以一定的频率报告其位置。在提出的强化学习模型中，一个观测不仅限于最后报告的GPS位置，而是一个堆叠的向量，其中包含最后报告的位置以及最近一次预测的点的历史。也就是说，不使用报告的GPS轨迹，而是使用模型预测来形成观测历史向量。需要注意的是，预测频率可以设置为高于GPS数据收集频率的值。通过形成观测向量的这种方法，模型可以利用GPS设备的历史轨迹信息和模型的性能，从而学习一个高质量的策略来纠正报告的GPS点。

将时刻t的GPS报告点表示为q_t，其真实位置表示为g_t。由于真实位置是未知的，该问题可以被表述为一个部分可观察马尔可夫决策过程(POMDP)。在这个POMDP中，使用p_t表示GPS报告点q_t的置信状态，即在RL模型中表示为p_t。Poupart和Boutilier指出，在POMDP中，可以通过考虑一个完全可观察的置信状态马尔可夫决策过程(MDP)来确定最优的行动策略，其中置信状态形成了状态，策略π将行动映射到置信状态；也就是说，部分可观察的状态被其估计值，即置信状态，替代以形成MDP(POMDP视作MDP)。使用大小为N的观测缓冲区Z_t来存储最近N-1个GPS报告点的历史模型估计和当前的q_t；即Z_t＝{p_t-N-1,…,p_t-1,q_t}。让使用S_t和b_t分别表示Z_t的隐藏状态和置信状态。给定大小为N的观测缓冲区，在时间t，向量S_t包含这些点的相应的真实位置缓冲区，即S_t＝{g_t-N,…,g_t}。向量b_t包含最近N个点的估计，即b_t＝{p_t-N,…,p_t}。请注意，Z_t和b_t仅在其最后一个元素上有所不同，Z_t的最后一个元素是q_t，而向量b_t的最后一个元素是p_t。模型基于Z_t估计b_t。

根据上述的POMDP设置，强化学习代理在每个时间戳t的目标是找到最优的校正动作以纠正q_t。该过程基于一个滑动窗口进行。一旦接收到新的q_t，滑动窗口向前移动一步，形成一个新的观测向量，大小为N，其中q_t构成最后一个元素，最后的N-1个信念构成观测向量Z_t的其余部分。

每当GPS设备报告一个新的位置点q_t时，模型就会进行训练，并移动到下一个观测缓冲区。当通过GPS设备获取到p_t时，它被推送到观测缓冲区，以取代q_t，而观测则移动到时间t+1。也就是说，在每个训练步骤中，强化学习模型的观测包含了观察到的GPS点和一系列历史估计值；

3)置信度奖励：在机器人领域，信心椭圆用于测量强化学习模型在同时定位和地图构建(SLAM)问题中的预测性能。假设GPS观测的噪声是白噪声高斯分布，信心椭圆显示了模型预测的不确定性。当模型对其预测有把握时，信心椭圆较小；否则，信心椭圆的面积较大。当在时间t观察到一个新的GPS点时，不止做一次预测，而是训练模型k次，根据t-1时刻得到的模型参数和新的观测输入得到k个可能的输出预测。使用两种指标来衡量不确定性对强化学习模型性能的影响。这两种指标都基于代理的预测的协方差矩阵。该协方差矩阵是一个二维矩阵，表示对GPS观测的经度和纬度进行校正动作的方向的确定程度。设这个协方差矩阵的特征值为a和b。如果a和b都与零足够不同，使用它们的乘积计算置信椭圆的面积，公式为πab。否则，如果其中至少一个值接近于零，使用a+b作为预测置信度的代理度量。置信度测量越大，不确定性越低。稍后使用不确定性的概念构建奖励函数。

尽管最大化置信度测量可以使预测的协方差最小化，但结果预测的位置不一定在道路限制范围内。因此，该模型利用数字地图信息来改善预测，将地图匹配纳入奖励函数中。地图匹配算法使用匹配过程将观测数据点(x，y)映射到一条道路上。因此，它们可以被看作是一个简单的搜索问题。当道路网络过于复杂或观测数据点与地面真实位置差距过大时(例如在城市中)，地图匹配可能不准确。因此，该工作将地图匹配作为正则化项仅在提出的模型中纳入。假设观测集合(X，Y)的地图匹配结果为定义一步奖励为：

r＝z+γ×D

其中Z是置信度测量，γ是正则化参数，D是观测值与地图匹配结果之间平方误差的负值总和；即(x_i,y_i)∈(X,Y)，/>

在给定策略π下，定义τ＝{a₀,O₀,r₁,a₁,O₁,r₂,…,a_T-1,O_T-1,r_T-1,}为POMDP的一条轨迹，将轨迹τ上的总奖励作为策略的最终奖励返回。使用折扣因子η，奖励函数表示为：

模型的目标是学习一个策略，以在每次预测中最大化折扣奖励返回值R_τ。设置T＝8以通过R_t来近似R_τ，进行“n步回报”；

4)A3C训练架构：使用的A3C训练架构需要较短的训练会话，并提供更稳健的策略，同时比传统的强化学习算法(如DQN和一步Q-learning)表现更好(即产生更高的奖励)。A3C在多个线程上并行运行智能体。它通过让每个智能体保留状态/观测的副本，并独立地训练一个独立的模型，为智能体提供多样化的训练环境。然后，智能体量化“优势”，一种衡量其动作质量的指标。

步骤3、自定位滤波融合模块将强化学习模块、IMU模块、车内数据采集模块的输出作为输入，利用卡尔曼滤波器KF进行滤波融合，得到车辆自身的位置信息；

设定卡尔曼滤波器KF的状态变量

其中，x_i y_i是车辆i在笛卡尔坐标系中的坐标，是车辆i的速度；则卡尔曼滤波器KF的状态转移方程和量测方程如下公式所示：

x[k]＝f(x[k-1],u[k],ω[k])

z[k]＝g(x[k],v[k])

其中，k为离散时点指标，x为车辆包含位置和速度的状态，u为命令过程，等效为驾驶输入，表示加速度，ω为命令噪声或状态噪声，来自命令过程的不确定性；z为IMU、GPS、Radar、相机等各种传感器上报的测量数据，v为测量和传输过程中的数据噪声；f和g分别是由运动的物理动力学和传感装置的固有特性得到的状态方程和测量模型；

卡尔曼滤波器KF进行滤波融合包括以下两个步骤：

步骤S1：时间更新:计算先验状态与状态转移雅可比矩阵来评估预测协方差；

步骤S2：测量更新：计算观测雅可比矩阵和卡尔曼滤波增益；

步骤4、相对定位滤波融合模块将自定位滤波融合模块的输出以及信号采集得到的相对于其他合作车辆的相对位置和速度估计作为输入，利用卡尔曼滤波器KF进行滤波融合，得到其他合作车辆的位置信息；

步骤5、通过通信装置，进行智能网联车之间的通信，交互步骤4中得到的合作车辆之间的位置信息；并且，如果车辆所处智能交通系统中存在RSU，则在车辆定位过程中会识别RSU，并获得从RSU到车辆的相对位置，同时RSU会向其通信范围内的车辆广播自身的绝对位置坐标；通过计算这些信息，车辆获得自己的绝对位置估计；由于RSU的位置信息是可信且准确的，误差只来自于车辆传感设备；因此，将RSU纳入协作中，提高定位的准确性；

步骤6、全局滤波融合模块将步骤3中自定位滤波融合模块的输出以及步骤5中来自其他合作车辆或RSU的关于自身的位置估计作为输入，利用卡尔曼滤波器KF进行滤波融合，车辆自身更加准确的位置估计；

全局滤波的目标是根据局部滤波器的输出计算车辆V_S的最优位置估计；车辆V_S的状态的自估计用表示，并带有协方差矩阵P_S，而来自车辆V_i的局部估计则用/>表示，并带有协方差矩阵P_i；对于大多数传感设备的数据而言，测量结果是符合高斯分布的，因此局部滤波结果也是高斯分布的；因此，全局最优状态估计表示为局部估计的线性组合；也就是说，问题变成了一个线性高斯系统下的数据融合问题，这类似于单车多传感器的数据融合。

在上述假设下，全局估计记为：

其中A_i和A_s为待解线性组合的未知权重，A_i为其他车辆估计的权重，A_s为自估计的权重；的方差为：/>

为了保证全局估计的无偏性，估计的均值不能改变；那么A_i的约束条件：

根据上述高斯假设，的最大似然估计就是使方差最小的估计；因此，全局滤波成为一个优化问题：

用拉格朗日乘子法来求解凸优化问题，因此得到目标函数：

最后，在全局滤波器处得到线性组合的最优权值为：

权值与局部滤波性能成反比；

因此，全局最优估计的最终结果为：

对于感知RSU并与RSU通信的车辆，考虑到对RSU的测量，全局最优估计表达为：

其中：

/>

由于RSU具有可靠和准确的位置信息，RSU提高RSU通信范围内车辆的定位精度；另一方面，由于车辆之间的合作，得到RSU的帮助的车辆进一步帮助不能直接感知和与RSU通信的车辆，从而提高它们的定位精度，从而改善整个网络中车辆的定位和追踪性能。

Claims (8)

1.一种多传感器多车辆协同定位系统，其特征在于，包括信号采集装置、中央处理单元以及通信装置；其中，信号采集装置负责获取车辆所处的位置信息，通信装置负责各车辆所采集信号的交互，中央处理单元负责对来自自身信号采集装置和通信装置的信号进行融合滤波从而得到车辆自身准确的位置信息。

2.根据权利要求1所述的一种多传感器多车辆协同定位系统，其特征在于，所述信号采集装置包括GPS接收模块、IMU模块、车内数据采集模块以及车外数据采集模块；中央处理单元包括强化学习模块、自定位滤波融合模块、相对定位滤波融合模块以及全局定位滤波融合模块。

3.根据权利要求1所述的一种多传感器多车辆协同定位系统，其特征在于，所述GPS接收模块，用于接收卫星信号，并将接收到的信号送入强化学习模块，并最终送入自定位滤波融合模块；

所述IMU模块，用于获得车辆的角速度、速度和加速度估计，并将测量得到的数据送入自定位滤波融合模块；

所述车内数据采集模块，用于采集车辆的轮速数据、转向角及里程数据，并将这些数据作为输入变量输入到自定位滤波融合模块；

所述车外数据采集模块，包括相机和雷达，用于得到相对于其他合作车辆的相对位置和速度估计；

所述强化学习模块，用于提高GPS的定位精度，以便在自动驾驶或半自动驾驶的情况下更准确地进行车辆定位；目标是找到关于观测到的GPS经度和纬度坐标的最佳校正策略，产生更准确的位置信息；

所述自定位滤波融合模块，通过将来自强化学习模块、IMU模块、车内数据采集模块以及车外数据采集模块的信息进行滤波融合，得到车辆自身准确的位置信息；

所述相对定位滤波融合模块，通过将自定位融合滤波模块得到的车辆自身位置信息与来自车外数据采集模块的相对于其他合作车辆的相对位置估计和速度估计进行滤波融合，得到其他合作车辆的位置信息；

所述全局定位滤波融合模块，通过将车辆自身位置信息与来自其他合作车辆的相对位置估计信息进行滤波融合，得到更加准确的车辆全局位置信息；

所述通信装置，用于现有智能交通系统中实现智能车之间的相互通信或智能车与路旁单元RSU之间的通信。

4.一种多传感器多车辆协同定位方法，基于权利要求1所述的一种多传感器多车辆协同定位系统实现，其特征在于，包括如下步骤：

步骤1、GPS接收模块进行卫星的捕获、跟踪、位同步、帧同步和定位解算，得到车辆所在位置的经度、维度和高程，并使用IMU模块获得车辆的角速度、速度和加速度估计，使用车内数据采集模块采集得到车辆的轮速数据、转向角以及里程数据；同时，通过车外数据采集模块获得相对于其他合作车辆的相对位置和速度估计；

步骤2、强化学习模块将GPS接收设备的经度和维度输出作为输入，并对估计的经度和维度执行校正操作，以提供更加准确的位置输出；

步骤3、自定位滤波融合模块将强化学习模块、IMU模块、车内数据采集模块的输出作为输入，利用卡尔曼滤波器KF进行滤波融合，得到车辆自身的位置信息；

步骤4、相对定位滤波融合模块将自定位滤波融合模块的输出以及信号采集得到的相对于其他合作车辆的相对位置和速度估计作为输入，利用卡尔曼滤波器KF进行滤波融合，得到其他合作车辆的位置信息；

步骤5、通过通信装置，进行智能网联车之间的通信，交互步骤4中得到的合作车辆之间的位置信息；并且，如果车辆所处智能交通系统中存在RSU，则在车辆定位过程中会识别RSU，并获得从RSU到车辆的相对位置，同时RSU会向其通信范围内的车辆广播自身的绝对位置坐标；通过计算这些信息，车辆获得自己的绝对位置估计；由于RSU的位置信息是可信且准确的，误差只来自于车辆传感设备；因此，将RSU纳入协作中，提高定位的准确性；

步骤6、全局滤波融合模块将步骤3中自定位滤波融合模块的输出以及步骤5中来自其他合作车辆或RSU的关于自身的位置估计作为输入，利用卡尔曼滤波器KF进行滤波融合，车辆自身更加准确的位置估计。

5.根据权利要求4所述的一种多传感器多车辆协同定位方法，其特征在于，步骤2具体为：

当收到一个新的数据点时，强化学习模型训练一个代理来确定观测到的经度和纬度需要调整的“单位”数量，以返回更准确的定位；这个顺序决策问题建模为一个部分可观察的马尔可夫决策过程POMDP；模型的目标是学习一个策略π(a|z,θ)，其中a表示动作向量，z表示观测向量，θ表示模型参数向量；策略的目标是参数化在给定某个观测值z时执行动作a的条件概率，以最大化自身的奖励。

6.根据权利要求4所述的一种多传感器多车辆协同定位方法，其特征在于，步骤2所述的强化学习模型：

1)动作空间：将动作定义为经度-纬度的更新操作；为了减少计算复杂度，将连续的经度和纬度值离散化为小步长；

2)观测和模型输入：GPS设备以一定的频率报告其位置；在提出的强化学习模型中，一个观测不仅限于最后报告的GPS位置，而是一个堆叠的向量，其中包含最后报告的位置以及最近一次预测的点的历史；不使用报告的GPS轨迹，而是使用模型预测来形成观测历史向量；预测频率设置为高于GPS数据收集频率的值；通过形成观测向量的这种方法，模型利用GPS设备的历史轨迹信息和模型的性能，从而学习一个高质量的策略来纠正报告的GPS点；

将时刻t的GPS报告点表示为q_t，其真实位置表示为g_t；由于真实位置是未知的，该问题被表述为一个部分可观察马尔可夫决策过程POMDP；在这个POMDP中，使用p_t表示GPS报告点q_t的置信状态，即在RL模型中表示为p_t；部分可观察的状态被其估计值，即置信状态，替代以形成MDP；使用大小为N的观测缓冲区Z_t来存储最近N-1个GPS报告点的历史模型估计和当前的q_t；即Z_t＝{p_t-N-1,…,p_t-1,q_t}；使用S_t和b_t分别表示Z_t的隐藏状态和置信状态；给定大小为N的观测缓冲区，在时间t，向量S_t包含这些点的相应的真实位置缓冲区，即S_t＝{g_t-N,…,g_t}；向量b_t包含最近N个点的估计，即b_t＝{p_t-N,…,p_t}；Z_t和b_t仅在其最后一个元素上有所不同，Z_t的最后一个元素是q_t，而向量b_t的最后一个元素是p_t；模型基于Z_t估计b_t；

根据上述的POMDP设置，强化学习代理在每个时间戳t的目标是找到最优的校正动作以纠正q_t；该过程基于一个滑动窗口进行；一旦接收到新的q_t，滑动窗口向前移动一步，形成一个新的观测向量，大小为N，其中q_t构成最后一个元素，最后的N-1个信念构成观测向量Z_t的其余部分；

每当GPS设备报告一个新的位置点q_t时，模型就会进行训练，并移动到下一个观测缓冲区；当通过GPS设备获取到p_t时，它被推送到观测缓冲区，以取代q_t，而观测则移动到时间t+1；在每个训练步骤中，强化学习模型的观测包含观察到的GPS点和一系列历史估计值；

3)置信度奖励：假设GPS观测的噪声是白噪声高斯分布，信心椭圆显示了模型预测的不确定性；当模型对其预测有把握时，信心椭圆较小；否则，信心椭圆的面积较大；当在时间t观察到一个新的GPS点时，不止做一次预测，而是训练模型k次，根据t-1时刻得到的模型参数和新的观测输入得到k个可能的输出预测；使用两种指标来衡量不确定性对强化学习模型性能的影响；这两种指标都基于代理的预测的协方差矩阵；该协方差矩阵是一个二维矩阵，表示对GPS观测的经度和纬度进行校正动作的方向的确定程度；设这个协方差矩阵的特征值为a和b；如果a和b都与零足够不同，使用它们的乘积计算置信椭圆的面积，公式为πab；否则，如果其中至少一个值接近于零，使用a+b作为预测置信度的代理度量；置信度测量越大，不确定性越低；使用不确定性的概念构建奖励函数；

尽管最大化置信度测量使预测的协方差最小化，但结果预测的位置不一定在道路限制范围内；因此，该模型利用数字地图信息来改善预测，将地图匹配纳入奖励函数中；地图匹配算法使用匹配过程将观测数据点(x，y)映射到一条道路上；因此，它们被看作是一个简单的搜索问题；当道路网络过于复杂或观测数据点与地面真实位置差距过大时，地图匹配可能不准确；因此，将地图匹配作为正则化项仅在提出的模型中纳入；假设观测集合(X，Y)的地图匹配结果为定义一步奖励为：

r＝z+γ×D

其中Z是置信度测量，γ是正则化参数，D是观测值与地图匹配结果之间平方误差的负值总和；即

在给定策略π下，定义τ＝{a₀,O₀,r₁,a₁,O₁,r₂,…,a_T-1,O_T-1,r_T-1,}为POMDP的一条轨迹，将轨迹τ上的总奖励作为策略的最终奖励返回；使用折扣因子η，奖励函数表示为：

模型的目标是学习一个策略，以在每次预测中最大化折扣奖励返回值R_τ；设置T＝8以通过R_t来近似R_τ，进行“n步回报”；

4)A3C训练架构：使用的A3C训练架构需要较短的训练会话，并提供更稳健的策略，A3C在多个线程上并行运行智能体；它通过让每个智能体保留状态或观测的副本，并独立地训练一个独立的模型，为智能体提供多样化的训练环境。

7.根据权利要求4所述的一种多传感器多车辆协同定位方法，其特征在于，步骤3具体为：

设定卡尔曼滤波器KF的状态变量

其中，x_i y_i是车辆i在笛卡尔坐标系中的坐标，是车辆i的速度；则卡尔曼滤波器KF的状态转移方程和量测方程如下公式所示：

x[k]＝f(x[k-1],u[k],ω[k])

z[k]＝g(x[k],v[k])

其中，k为离散时点指标，x为车辆包含位置和速度的状态，u为命令过程，等效为驾驶输入，表示加速度，ω为命令噪声或状态噪声，来自命令过程的不确定性；z为IMU、GPS、Radar、相机等各种传感器上报的测量数据，v为测量和传输过程中的数据噪声；f和g分别是由运动的物理动力学和传感装置的固有特性得到的状态方程和测量模型；

卡尔曼滤波器KF进行滤波融合包括以下两个步骤：

步骤S1：时间更新:计算先验状态与状态转移雅可比矩阵来评估预测协方差；

步骤S2：测量更新：计算观测雅可比矩阵和卡尔曼滤波增益。

8.根据权利要求4所述的一种多传感器多车辆协同定位方法，其特征在于，步骤6具体为：

全局滤波的目标是根据局部滤波器的输出计算车辆V_S的最优位置估计；车辆V_S的状态的自估计用表示，并带有协方差矩阵P_S，而来自车辆V_i的局部估计则用/>表示，并带有协方差矩阵P_i；局部滤波结果也是高斯分布的；因此，全局最优状态估计表示为局部估计的线性组合；

在上述假设下，全局估计记为：

其中A_i和A_s为待解线性组合的未知权重，A_i为其他车辆估计的权重，A_s为自估计的权重；的方差为：

为了保证全局估计的无偏性，估计的均值不能改变；那么A_i的约束条件：

根据上述高斯假设，的最大似然估计就是使方差最小的估计；因此，全局滤波成为一个优化问题：

用拉格朗日乘子法来求解凸优化问题，因此得到目标函数：

最后，在全局滤波器处得到线性组合的最优权值为：

权值与局部滤波性能成反比；

因此，全局最优估计的最终结果为：

对于感知RSU并与RSU通信的车辆，考虑到对RSU的测量，全局最优估计表达为：

其中：

由于RSU具有可靠和准确的位置信息，RSU提高RSU通信范围内车辆的定位精度；另一方面，由于车辆之间的合作，得到RSU的帮助的车辆进一步帮助不能直接感知和与RSU通信的车辆，从而提高它们的定位精度，从而改善整个网络中车辆的定位和追踪性能。