一种车辆相对位姿测量方法、系统、设备及存储介质

技术领域

本发明涉及车辆位姿测量技术领域，尤其涉及一种车辆相对位姿测量方法、系统、设备及存储介质。

背景技术

随着汽车的电动化、智能化、网联化和共享化——新四化的快速发展，汽车产业甚至人类的出行方式正在发生革命性的变化。在智能汽车的诸多关键技术中，车辆的定位与测量技术是智能汽车最为重要的关键技术之一。目前，针对自动驾驶车辆相对位姿测量多采用虚拟场景车辆在环的测试方法，即利用GPS/IMU设备建立车辆运动学状态观测模型，注入虚拟测试场景，按照主驾驶位置参数，在虚拟场景中渲染生成场景可视化视角，再对车辆动力学状态参数(车速、加速度和位姿等)数据记录与综合评价。

该测试方法虽然可以使用虚拟场景数据，验证自动驾驶系统的性能表现，但一旦考虑到多台车辆之间数据传输，其系统响应与真实场景下的自动驾驶测试存在一定的差距，无法验证系统实际的状态及精度。

发明内容

基于此，有必要针对上述技术问题，提供一种车辆相对位姿测量方法、系统、设备及存储介质。

一种车辆相对位姿测量方法，包括以下步骤：通过DR航位推算系统获取目标车辆的预测位姿，所述预测位姿包括有多个惯性位姿数据；通过GPS定位模块获取目标车辆的GPS观测值和绝对位置，查找与所述GPS观测值的时间最接近的两个惯性位姿数据，将所述GPS观测值插入到所述最接近的两个惯性位姿数据之间；根据目标车辆的预测位姿和绝对位置计算所述目标车辆的状态估计值；将所述目标车辆的状态估计值进行加密并发送至服务器；服务器对目标车辆的状态估计值进行解密，并将目标车辆的状态估计值转换到基准车辆所在的基准坐标系中；根据目标车辆的预测位姿和绝对位置获取状态量建立状态转移方程，根据所述状态转移方程对目标车辆的状态估计值进行更新；根据所述目标车辆的定位数据建立车辆加速度预测模型并进行训练，根据训练后的车辆加速度预测模型对目标车辆的定位数据进行预测，获取目标车辆预测定位数据；根据基准车辆和目标车辆的预测定位数据和更新后的状态估计值进行计算，获取目标车辆相对于基准车辆的相对位姿。

在其中一个实施例中，所述根据DR航位推算系统获取目标车辆的预测位姿，所述预测位姿包括有多个惯性位姿数据，具体包括：根据目标车辆的后轮脉冲计算目标车辆在预设时间段内的位姿变化；根据陀螺仪计算得到目标车辆在所述预设时间段内运动的相对角度；将目标车辆在预设时间段内的位姿变换和相对角度进行融合，获取目标车辆的预测位姿。

在其中一个实施例中，所述通过DR航位推算系统获取目标车辆的预测位姿，所述预测位姿包括有多个惯性位姿数据，还包括：采用双端队列存储预设时间段内的惯性位姿数据。

在其中一个实施例中，所述根据目标车辆的预测位姿和绝对位置计算所述目标车辆的状态估计值，具体包括：根据所述目标车辆的预测位姿和绝对位置获取状态量即

Y(k)＝F(k,k-1)·(k-1)+Z(k-1)； (1)

其中，F(k,k-1)为状态转移矩阵，Z(k-1)为系统噪声矩阵，均为零均值白噪声；根据所述目标车辆的GPS观测值获取观测量，所述观测量均由所述GPS定位模块输出，根据所述观测量构建观测方程，根据所述观测方程获取观测值，所述观测方程为：

其中，z

在其中一个实施例中，所述根据目标车辆的预测位姿和绝对位置建立状态转移方程，根据所述状态转移方程对目标车辆的状态估计值进行更新，具体包括：

在目标车辆沿直线前进，且以固定角度和恒定的速度大小移动时，目标车辆的状态量为

其中，x表示横向位置，y表示纵向位置，v

一种车辆相对位姿测量系统，包括：GPS/DR组合导航模块、无线通信模块和数据延迟处理模块；所述GPS/DR组合导航模块、无线通信模块和所述数据延迟处理模块之间均进行通信连接；所述GPS/DR组合导航模块用于获取目标车辆的预测位姿、GPS观测值和绝对位姿，将GPS观测值插入到所述预测位姿中，计算获取目标车辆的状态估计值，并将目标车辆的状态估计值进行加密后通过无线通信模块发送至服务器；所述无线通信模块用于实现目标车辆与目标车辆之间、目标车辆与基准车辆之间和目标车辆与服务器之间的数据传输；所述数据延迟数据模块用于解析目标车辆加密后的状态估计值，根据状态转移方程对目标车辆的状态估计值进行更新，且根据加速度预测模型对目标车辆的定位数据进行预测，并根据基准车辆和目标车辆的预测定位数据和更新后的状态估计值进行计算，获取目标车辆相对于基准车辆的相对位姿。

一种设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述各个实施例中所述的一种车辆相对位姿测量方法的步骤。

一种存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述各个实施例中所述的一种车辆相对位姿测量方法的步骤。

相比于现有技术，本发明的优点及有益效果在于：

1、本发明克服了虚拟场景汽车在环测试方法高成本、低精度和高延迟等的不足。

2、本发明在不新增任何传感器的基础上，能够对车辆的无线通信延迟进行处理，进一步提高测量精度。

3、本发明能够定量反映在研单体传感器甚至多传感器系统对目标车辆位姿、速度等状态数据影响的准确性。

附图说明

图1为一个实施例中一种车辆相对位姿测量方法的流程示意图；

图2为一个实施例中一种车辆相对位姿测量系统的结构示意图；

图3为一个实施例中设备的内部结构示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，下面通过具体实施方式结合附图对本发明做进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

在一个实施例中，如图1所示，提供了一种车辆相对位姿测量方法，包括以下步骤：

步骤S101，通过DR航位推算系统获取目标车辆的预测位姿，预测位姿包括有多个惯性位姿数据。

具体地，DR航位推算系统通过目标车辆的后轮脉冲计算目标车辆在一定时间内的位姿变化，并通过陀螺仪计算得到目标车辆运动的相对角度，通过里程计与陀螺仪的数据进行融合，从而产生目标车辆的预测位姿，预测位姿包括有目标车辆的多个惯性位姿数据。

由于在对自动驾驶车辆进行测试时，需要多辆车辆同时进行运动，因此，目标车辆可以是多个车辆，多个车辆能够通过搭载的DR航位推算系统获取对应的预测位姿。此外，由于还需要采用基准车辆对目标车辆的相对位姿进行比较，因此，目标车辆获取的所有数据信息，也同时需要在基准车辆中获取，不仅仅限于预测位姿，还包括下述步骤中目标车辆获取的各类数据，达到比较的目标。

步骤S102，通过GPS定位模块获取目标车辆的GPS观测值和绝对位置，查找与GPS观测值的时间最接近的两个惯性位姿数据，将GPS观测值插入到最接近的两个惯性位姿数据之间。

具体地，根据GPS定位模块获取目标车辆的GPS观测值和绝对位置后，需要与DR航位推算系统作时间对齐。一般来说，GPS定位模块的位姿发布频率低于DR航位推算系统，鉴于此，可以通过数据插值的方法，即通过查找与GPS观测值的时间最接近的两个惯性位姿数据，将GPS观测值插入最接近的两个惯性位姿数据之间，从而完成时间对齐。

其中，绝对位置还可以用于为DR航位推算系统提供推算定位的初始值并进行误差校正；另一方面，DR航位推算的结果也可以用于补偿GPS定位模块定位中的随机误差，从而平滑轨迹。通过定位信息的互补，从而实现较高的定位精度及可靠性。

步骤S103，根据目标车辆的预测位姿和绝对位置计算目标车辆的状态估计值。

具体地，将DR航位推算系统和GPS定位模块通过卡尔曼滤波算法进行融合，从而实现GPS/DR组合导航，并根据GPS/DR组合导航计算目标车辆的状态估计值。

步骤S104，将目标车辆的状态估计值进行加密并发送至服务器。

具体地，若干目标车辆和基准车辆之间通过无线网络进行通信连接，实现多台自动驾驶车辆之间的定位状态等数据的交换传输。在无线传输技术支持下，单台自动驾驶车辆加密打包后的数据可以在服务器的控制下进行交换，实现数据的传输。同样，其他自动驾驶车辆通过移动蜂窝等无线通信技术实现与车联网服务平台的信息传输，接受平台下达的控制指令，实时共享车辆数据。

因此，目标车辆将其状态估计值进行加密，发送至服务器，当然也可以是发送至车辆网服务平台，通过服务器或车辆网服务平台获取目标车辆的状态估计值并进行计算。

其中，目标车辆的状态估计值可以通过编码指定不同的代码页，将字符串转化为不同代码页对应的编码，变现为字节的形式进行加密处理。

步骤S105，服务器对目标车辆的状态估计值进行解密，并将目标车辆的状态估计值转换到基准车辆所在的基准坐标系中。

具体地，假设目标车辆按照1，2，…,n进行编号且为客户端，基准车辆为服务器端。首先基准车辆与目标车辆i(i＝1，2，…,n)建立通信连接，例如Socket连接，且持续监听其他目标的服务请求，若接收到服务请求，则目标车辆i申请与基准车辆的连接；若未接收到服务请求，则返回继续进行监听，至接收到服务请求。接收到服务请求后，判断服务请求中是否存在加密后的数据文件，若存在则接收文件，关闭通信连接；若不存在加密后的数据文件，则持续监听与接收，直至接收到加密后的数据文件。

具体地，服务器接收到加密后的数据文件后，将字节形式的明文转换为适当字符串明文，即获取目标车辆的状态估计值，并将状态估计值转换到基准车辆所在的基准坐标系中。

其中，可以选取若干目标车辆的其中之一作为基准车辆；基准坐标系可以是以基准车辆的后轴中心为零点建立的坐标系。

步骤S106，根据目标车辆的预测位姿和绝对位置获取状态量建立状态转移方程，根据状态转移方程对目标车辆的状态估计值进行更新。

具体地，为了处理目标车辆之间接收定位状态数据的延迟问题，可以根据目标车辆的预测位姿和绝对位置建立状态转移方程，根据状态转移方程对目标车辆的状态估计值进行更新，并以基准车辆的时间为基准对目标车辆进行时间对齐，从而纠正时间延迟带来的状态误差。

步骤S107，根据所述目标车辆的定位数据建立车辆加速度预测模型并进行训练，根据训练后的车辆加速度预测模型对目标车辆的定位数据进行预测，获取目标车辆预测定位数据。

具体地，定位数据可以包括有目标车辆的预测位姿、GPS观测值和绝对位置，从而计算出目标车辆的加速度，建立车辆加速度预测模型，并对车辆加速度模型进行训练。根据训练后的车辆加速度预测模型对目标车辆的定位数据进行预测，获取目标车辆预测定位数据。

步骤S108，根据基准车辆和目标车辆的预测定位数据和更新后的状态估计值进行计算，获取目标车辆相对于基准车辆的相对位姿。

具体地，根据基准车辆和目标车辆的预测定位数据和更新后的状态估计值，计算获取目标车辆相对于基准车辆的相对位姿，从而提高车辆相对位姿的车辆精度。

在本实施例中，通过DR航位推算系统获取目标车辆的预测位姿，且通过GPS定位模块获取目标车辆的GPS观测值和绝对位姿，将GPS观测值插入所述预测位姿中，根据目标车辆的预测位姿和绝对位姿计算目标车辆的状态估计值，实现较高的定位精度和可靠性，并对其进行加密发送至服务器，服务器对其进行机密获取目标车辆的状态估计值，并将其转换到基准车辆所在的基准坐标系中，建立状态转移方程，用于对目标车辆的状态估计值进行更新，同时建立并训练车辆加速度模型，根据车辆加速度模型对目标车辆的定位数据进行预测，获取目标车辆预测定位数据，最后根据目标车辆和基准车辆的预测定位数据与更新后状态估计值，计算获取目标车辆相对于基准车辆的相对位姿，克服了虚拟场景汽车在环测试方法高成本、低精度和高延迟等的不足，且在不新增任何传感器的基础上，能够对车辆的无线通信延迟进行处理，进一步提高测量精度。

其中，步骤S101具体包括：根据目标车辆的后轮脉冲计算目标车辆在预设时间段内的位姿变化；根据陀螺仪计算得到目标车辆在所述预设时间段内运动的相对角度；将目标车辆在预设时间段内的位姿变换和相对角度进行融合，获取目标车辆的预测位姿。

具体地，DR航位推算系统可以根据目标车辆的后轮脉冲计算目标车辆在预设时间段的位姿变化；并根据陀螺仪计算得到目标车辆在预设时间段内运动的相对角度，根据位姿变化和相对角度进行融合从而获取目标车辆的预测位姿。

其中，步骤S101还包括：采用双端队列存储预设时间段内的惯性位姿数据。

具体地，目标车辆在接收到GPS定位模块的绝对位置等信息后，需要与DR航位推算系统进行时间对齐。由于GPS定位模块的位置发布频率低于DR航位推算系统，因此可以采用数据插值的方法进行时间对齐操作。即采用双端存储预设时间段内的惯性位姿数据，通过GPS定位模块获取GPS观测值后，在双端队列中查找与GPS观测值的时间最接近的两个惯性位姿数据，由此将GPS观测值插入，完成时间对齐。

其中，步骤S103具体包括：根据所述目标车辆的预测位姿和绝对位置获取状态量即

Y(k)＝F(k,k-1)·(k-1)+Z(k-1)； (1)

其中，F(k,k-1)为状态转移矩阵，Z(k-1)为系统噪声矩阵，均为零均值白噪声；

根据所述目标车辆的GPS观测值获取观测量，所述观测量均由所述GPS定位模块输出，根据所述观测量构建观测方程，根据所述观测方程获取观测值，所述观测方程为：

其中，z

根据所述观测值和所述预测值计算得到所述目标车辆的状态估计值。

其中，步骤S106具体包括：在目标车辆沿直线前进，且以固定角度和恒定的速度大小移动时，目标车辆的状态量为

其中，x表示横向位置，y表示纵向位置，v

如图2所示，提供了一种车辆相对位姿测量系统20，包括：GPS/DR组合导航模块21、无线通信模块22和数据延迟处理模块23，其中：

GPS/DR组合导航模块21、无线通信模块22和数据延迟处理模块23之间均进行通信连接；

GPS/DR组合导航模块21，用于获取目标车辆的预测位姿、GPS观测值和绝对位置，将GPS观测值插入到所述预测位姿中，计算获取目标车辆的状态估计值，并将目标车辆的状态估计值加密后通过无线通信模块22发送至服务器；

无线通信模块22，用于实现目标车辆与目标车辆之间、目标车辆与基准车辆之间和目标车辆与服务器之间的数据传输；

数据延迟处理模块23，用于解析目标车辆加密后的状态估计值，根据状态转移方程对目标车辆的状态估计值进行更新，且根据加速度预测模型对目标车辆的定位数据进行预测，并根据基准车辆和目标车辆的预测定位数据和更新后的状态估计值进行计算，获取目标车辆相对于基准车辆的相对位姿。

其中，GPS/DR组合导航模块21由GPS定位单元和DR航位推算单元经过自适应卡尔曼滤波算法融合而成。

具体地，一方面GPS定位单元提供的绝对位置信息可以为DR航位推算单元提供推算定位的初始值并进行误差校正；另一方面，DR航位推算单元的推算结果可以用于补偿GPS定位单元中的随机误差，从而平滑轨迹。通过定位信息的互补，实现较高的定位精度及可靠性。

在一个实施例中，GPS/DR组合导航模块21还用于：根据目标车辆的后轮脉冲计算目标车辆在预设时间段内的位姿变化；根据陀螺仪计算得到目标车辆在预设时间段内运动的相对角度；将目标车辆在预设时间段内的位姿变换和相对角度进行融合，获取目标车辆的预测位姿。

在一个实施例中，提供了一种设备，该设备可以是服务器，其内部结构图可以如图3所示。该设备包括通过系统总线连接的处理器、存储器、网络接口和数据库。其中，该设备的处理器用于提供计算和控制能力。该设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该设备的数据库用于存储配置模板，还可用于存储目标网页数据。该设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种车辆相对位姿测量方法。

本领域技术人员可以理解，图3中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的设备的限定，具体的设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在一个实施例中，还可以提供一种存储介质，所述存储介质存储有计算机程序，所述计算机程序包括程序指令，所述程序指令当被计算机执行时使所述计算机执行如前述实施例所述的方法，所述计算机可以为上述提到的一种车辆相对位姿测量系统的一部分。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(RandomAccessMemory，RAM)等。

显然，本领域的技术人员应该明白，上述本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在计算机存储介质(ROM/RAM、磁碟、光盘)中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。所以，本发明不限制于任何特定的硬件和软件结合。

以上内容是结合具体的实施方式对本发明所做的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。