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

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

Info

Publication number
CN113075713A
CN113075713A CN202110333208.5A CN202110333208A CN113075713A CN 113075713 A CN113075713 A CN 113075713A CN 202110333208 A CN202110333208 A CN 202110333208A CN 113075713 A CN113075713 A CN 113075713A
Authority
CN
China
Prior art keywords
target vehicle
pose
vehicle
estimation value
predicted
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.)
Pending
Application number
CN202110333208.5A
Other languages
English (en)
Inventor
熊周兵
刘紫扬
倪俊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chongqing Innovation Center of Beijing University of Technology
Original Assignee
Chongqing Innovation Center of Beijing University of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Chongqing Innovation Center of Beijing University of Technology filed Critical Chongqing Innovation Center of Beijing University of Technology
Priority to CN202110333208.5A priority Critical patent/CN113075713A/zh
Publication of CN113075713A publication Critical patent/CN113075713A/zh
Pending legal-status Critical Current

Links

Images

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
    • 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/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

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)

Abstract

本发明提供一种车辆相对位姿测量方法、系统、设备及存储介质，其中，方法包括：通过DR航位推算系统和GPS定位模块获取目标车辆预测位姿、GPS观测值和绝对位置，根据预测位姿和绝对位置计算获取目标车辆的状态估计值，并对状态估计值进行加密发送至服务器，服务器将状态估计值转换到基准坐标系中，根据目标车辆的状态量建立状态转移方程，对状态估计值进行更新，同时建立车辆加速度预测模型，对目标车辆的定位数据进行预测，获取目标车辆预测定位数据，根据基准车辆和目标车辆的预测定位数据和更新后的状态估计值进行计算，获取目标车辆相对于基准车辆的相对位姿。本发明实现了低成本、高精度和低延迟的车辆相对位姿测量。

Description

一种车辆相对位姿测量方法、系统、设备及存储介质
技术领域
本发明涉及车辆位姿测量技术领域，尤其涉及一种车辆相对位姿测量方法、系统、设备及存储介质。
背景技术
随着汽车的电动化、智能化、网联化和共享化——新四化的快速发展，汽车产业甚至人类的出行方式正在发生革命性的变化。在智能汽车的诸多关键技术中，车辆的定位与测量技术是智能汽车最为重要的关键技术之一。目前，针对自动驾驶车辆相对位姿测量多采用虚拟场景车辆在环的测试方法，即利用GPS/IMU设备建立车辆运动学状态观测模型，注入虚拟测试场景，按照主驾驶位置参数，在虚拟场景中渲染生成场景可视化视角，再对车辆动力学状态参数(车速、加速度和位姿等)数据记录与综合评价。
该测试方法虽然可以使用虚拟场景数据，验证自动驾驶系统的性能表现，但一旦考虑到多台车辆之间数据传输，其系统响应与真实场景下的自动驾驶测试存在一定的差距，无法验证系统实际的状态及精度。
发明内容
基于此，有必要针对上述技术问题，提供一种车辆相对位姿测量方法、系统、设备及存储介质。
一种车辆相对位姿测量方法，包括以下步骤：通过DR航位推算系统获取目标车辆的预测位姿，所述预测位姿包括有多个惯性位姿数据；通过GPS定位模块获取目标车辆的GPS观测值和绝对位置，查找与所述GPS观测值的时间最接近的两个惯性位姿数据，将所述GPS观测值插入到所述最接近的两个惯性位姿数据之间；根据目标车辆的预测位姿和绝对位置计算所述目标车辆的状态估计值；将所述目标车辆的状态估计值进行加密并发送至服务器；服务器对目标车辆的状态估计值进行解密，并将目标车辆的状态估计值转换到基准车辆所在的基准坐标系中；根据目标车辆的预测位姿和绝对位置获取状态量建立状态转移方程，根据所述状态转移方程对目标车辆的状态估计值进行更新；根据所述目标车辆的定位数据建立车辆加速度预测模型并进行训练，根据训练后的车辆加速度预测模型对目标车辆的定位数据进行预测，获取目标车辆预测定位数据；根据基准车辆和目标车辆的预测定位数据和更新后的状态估计值进行计算，获取目标车辆相对于基准车辆的相对位姿。
在其中一个实施例中，所述根据DR航位推算系统获取目标车辆的预测位姿，所述预测位姿包括有多个惯性位姿数据，具体包括：根据目标车辆的后轮脉冲计算目标车辆在预设时间段内的位姿变化；根据陀螺仪计算得到目标车辆在所述预设时间段内运动的相对角度；将目标车辆在预设时间段内的位姿变换和相对角度进行融合，获取目标车辆的预测位姿。
在其中一个实施例中，所述通过DR航位推算系统获取目标车辆的预测位姿，所述预测位姿包括有多个惯性位姿数据，还包括：采用双端队列存储预设时间段内的惯性位姿数据。
在其中一个实施例中，所述根据目标车辆的预测位姿和绝对位置计算所述目标车辆的状态估计值，具体包括：根据所述目标车辆的预测位姿和绝对位置获取状态量即
Figure BDA0002997096890000021
其中，e1为目标车辆东向位置分量，n1为目标车辆北向位置分量，
Figure BDA0002997096890000022
为目标车辆航向，v1为目标车辆速度，ω1为目标车辆航向角速度，b1为陀螺仪零偏；根据所述状态量构建状态方程，根据所述状态方程计算得到预测值，所述状态方程为：
Y(k)＝F(k,k-1)·(k-1)+Z(k-1)； (1)
其中，F(k,k-1)为状态转移矩阵，Z(k-1)为系统噪声矩阵，均为零均值白噪声；根据所述目标车辆的GPS观测值获取观测量，所述观测量均由所述GPS定位模块输出，根据所述观测量构建观测方程，根据所述观测方程获取观测值，所述观测方程为：
Figure BDA0002997096890000023
其中，z1、z2……z5为分别为各观测值的零均值高斯白噪声，e2为GPS定位模块输出的目标车辆东向位置分量，n2为GPS定位模块输出的目标车辆北向位置分量，
Figure BDA0002997096890000031
为GPS定位模块输出的目标车辆航向，v2GPS定位模块输出的为目标车辆速度，ω2为DR航位推算系统输出的航向角速度，b2为DR航位推算系统输出的陀螺仪零偏；根据所述观测值和所述预测值计算得到所述目标车辆的状态估计值。
在其中一个实施例中，所述根据目标车辆的预测位姿和绝对位置建立状态转移方程，根据所述状态转移方程对目标车辆的状态估计值进行更新，具体包括：
在目标车辆沿直线前进，且以固定角度和恒定的速度大小移动时，目标车辆的状态量为
Figure BDA0002997096890000032
则目标车辆的状态转移方程为：
Figure BDA0002997096890000033
其中，x表示横向位置，y表示纵向位置，v3表示目标车辆速度大小，
Figure BDA0002997096890000034
表示偏航角度，即目标车辆在基准车辆下的x轴夹角，逆时针为正，参数ω3表示偏航角速度。
一种车辆相对位姿测量系统，包括：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具体包括：根据所述目标车辆的预测位姿和绝对位置获取状态量即
Figure BDA0002997096890000081
其中，e1为目标车辆东向位置分量，n1为目标车辆北向位置分量，
Figure BDA0002997096890000082
为目标车辆航向，v1为目标车辆速度，ω1为目标车辆航向角速度，b1为陀螺仪零偏；根据所述状态量构建状态方程，根据所述状态方程计算得到预测值，所述状态方程为：
Y(k)＝F(k,k-1)·(k-1)+Z(k-1)； (1)
其中，F(k,k-1)为状态转移矩阵，Z(k-1)为系统噪声矩阵，均为零均值白噪声；
根据所述目标车辆的GPS观测值获取观测量，所述观测量均由所述GPS定位模块输出，根据所述观测量构建观测方程，根据所述观测方程获取观测值，所述观测方程为：
Figure BDA0002997096890000083
其中，z1、z2……z5为分别为各观测值的零均值高斯白噪声，e2为GPS定位模块输出的目标车辆东向位置分量，n2为GPS定位模块输出的目标车辆北向位置分量，
Figure BDA0002997096890000084
为GPS定位模块输出的目标车辆航向，v2GPS定位模块输出的为目标车辆速度，ω2为DR航位推算系统输出的航向角速度，b2为DR航位推算系统输出的陀螺仪零偏；
根据所述观测值和所述预测值计算得到所述目标车辆的状态估计值。
其中，步骤S106具体包括：在目标车辆沿直线前进，且以固定角度和恒定的速度大小移动时，目标车辆的状态量为
Figure BDA0002997096890000085
则目标车辆的状态转移方程为：
Figure BDA0002997096890000091
其中，x表示横向位置，y表示纵向位置，v3表示目标车辆速度大小，
Figure BDA0002997096890000092
表示偏航角度，即目标车辆在基准车辆下的x轴夹角，逆时针为正，参数ω3表示偏航角速度。
如图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、磁碟、光盘)中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。所以，本发明不限制于任何特定的硬件和软件结合。
以上内容是结合具体的实施方式对本发明所做的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims (9)

1.一种车辆相对位姿测量方法，其特征在于，包括以下步骤：
通过DR航位推算系统获取目标车辆的预测位姿，所述预测位姿包括有多个惯性位姿数据；通过GPS定位模块获取目标车辆的GPS观测值和绝对位置，查找与所述GPS观测值的时间最接近的两个惯性位姿数据，将所述GPS观测值插入到所述最接近的两个惯性位姿数据之间；根据目标车辆的预测位姿和绝对位置计算所述目标车辆的状态估计值；将所述目标车辆的状态估计值进行加密并发送至服务器；服务器对目标车辆的状态估计值进行解密，并将目标车辆的状态估计值转换到基准车辆所在的基准坐标系中；根据目标车辆的预测位姿和绝对位置获取状态量建立状态转移方程，根据所述状态转移方程对目标车辆的状态估计值进行更新；根据所述目标车辆的定位数据建立车辆加速度预测模型并进行训练，根据训练后的车辆加速度预测模型对目标车辆的定位数据进行预测，获取目标车辆预测定位数据；根据基准车辆和目标车辆的预测定位数据和更新后的状态估计值进行计算，获取目标车辆相对于基准车辆的相对位姿。
2.根据权利要求1所述的一种车辆相对位姿测量方法，其特征在于，所述根据DR航位推算系统获取目标车辆的预测位姿，所述预测位姿包括有多个惯性位姿数据，具体包括：
根据目标车辆的后轮脉冲计算目标车辆在预设时间段内的位姿变化；
根据陀螺仪计算得到目标车辆在所述预设时间段内运动的相对角度；
将目标车辆在预设时间段内的位姿变换和相对角度进行融合，获取目标车辆的预测位姿。
3.根据权利要求2所述的一种车辆相对位姿测量方法，其特征在于，所述通过DR航位推算系统获取目标车辆的预测位姿，所述预测位姿包括有多个惯性位姿数据，还包括：
采用双端队列存储预设时间段内的惯性位姿数据。
4.根据权利要求1所述的一种车辆相对位姿测量方法，其特征在于，所述根据目标车辆的预测位姿和绝对位置计算所述目标车辆的状态估计值，具体包括：
根据所述目标车辆的预测位姿和绝对位置获取状态量即
Figure FDA0002997096880000011
其中，e1为目标车辆东向位置分量，n1为目标车辆北向位置分量，
Figure FDA0002997096880000012
为目标车辆航向，v1为目标车辆速度，ω1为目标车辆航向角速度，b1为陀螺仪零偏；根据所述状态量构建状态方程，根据所述状态方程计算得到预测值，所述状态方程为：
Y(k)＝F(k,k-1)·(k-1)+Z(k-1)； (1)
其中，F(k,k-1)为状态转移矩阵，Z(k-1)为系统噪声矩阵，均为零均值白噪声；
根据所述目标车辆的GPS观测值获取观测量，所述观测量均由所述GPS定位模块输出，根据所述观测量构建观测方程，根据所述观测方程获取观测值，所述观测方程为：
Figure FDA0002997096880000021
其中，z1、z2……z5为分别为各观测值的零均值高斯白噪声，e2为GPS定位模块输出的目标车辆东向位置分量，n2为GPS定位模块输出的目标车辆北向位置分量，
Figure FDA0002997096880000022
为GPS定位模块输出的目标车辆航向，v2GPS定位模块输出的为目标车辆速度，ω2为DR航位推算系统输出的航向角速度，b2为DR航位推算系统输出的陀螺仪零偏；
根据所述观测值和所述预测值计算得到所述目标车辆的状态估计值。
5.根据权利要求1所述的一种车辆相对位姿测量方法，其特征在于，所述根据目标车辆的预测位姿和绝对位置获取状态量建立状态转移方程，根据所述状态转移方程对目标车辆的状态估计值进行更新，具体包括：
在目标车辆沿直线前进，且以固定角度和恒定的速度大小移动时，目标车辆的状态量为
Figure FDA0002997096880000023
则目标车辆的状态转移方程为：
Figure FDA0002997096880000024
其中，x表示横向位置，y表示纵向位置，v3表示目标车辆速度大小，
Figure FDA0002997096880000025
表示偏航角度，即目标车辆在基准车辆下的x轴夹角，逆时针为正，参数ω3表示偏航角速度。
6.一种车辆相对位姿测量系统，其特征在于，包括：GPS/DR组合导航模块、无线通信模块和数据延迟处理模块；所述GPS/DR组合导航模块、无线通信模块和所述数据延迟处理模块之间均进行通信连接；
所述GPS/DR组合导航模块用于获取目标车辆的预测位姿、GPS观测值和绝对位姿，将GPS观测值插入到所述预测位姿中，计算获取目标车辆的状态估计值，并将目标车辆的状态估计值进行加密后通过无线通信模块发送至服务器；
所述无线通信模块用于实现目标车辆与目标车辆之间、目标车辆与基准车辆之间和目标车辆与服务器之间的数据传输；
所述数据延迟数据模块用于解析目标车辆加密后的状态估计值，根据状态转移方程对目标车辆的状态估计值进行更新，且根据加速度预测模型对目标车辆的定位数据进行预测，并根据基准车辆和目标车辆的预测定位数据和更新后的状态估计值进行计算，获取目标车辆相对于基准车辆的相对位姿。
7.根据权利要求6所述的一种车辆相对位姿测量系统，其特征在于，所述GPS/DR组合导航模块由GPS定位单元和DR航位推算单元经过自适应卡尔曼滤波算法融合而成。
8.一种设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至5中任一项所述方法的步骤。
9.一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至5中任一项所述的方法的步骤。
CN202110333208.5A 2021-03-29 2021-03-29 一种车辆相对位姿测量方法、系统、设备及存储介质 Pending CN113075713A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110333208.5A CN113075713A (zh) 2021-03-29 2021-03-29 一种车辆相对位姿测量方法、系统、设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110333208.5A CN113075713A (zh) 2021-03-29 2021-03-29 一种车辆相对位姿测量方法、系统、设备及存储介质

Publications (1)

Publication Number Publication Date
CN113075713A true CN113075713A (zh) 2021-07-06

Family

ID=76611045

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110333208.5A Pending CN113075713A (zh) 2021-03-29 2021-03-29 一种车辆相对位姿测量方法、系统、设备及存储介质

Country Status (1)

Country Link
CN (1) CN113075713A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113483769A (zh) * 2021-08-17 2021-10-08 清华大学 基于粒子滤波器的车辆自定位方法、系统、设备和介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100030470A1 (en) * 2008-07-02 2010-02-04 O2Micro, Inc. Global positioning system and dead reckoning (gps&dr) integrated navigation system
CN102410837A (zh) * 2011-07-29 2012-04-11 江苏大学 一种车辆组合定位导航系统及方法
CN104390643A (zh) * 2014-11-24 2015-03-04 上海美琦浦悦通讯科技有限公司 基于多信息融合实现室内定位的方法
CN109426259A (zh) * 2017-08-25 2019-03-05 本田技研工业株式会社 使用车辆通信进行同步车辆传感器数据获取处理的系统和方法
CN110264721A (zh) * 2019-07-01 2019-09-20 北京理工大学 一种城市交叉口周边车辆轨迹预测方法
CN110274589A (zh) * 2018-03-15 2019-09-24 高德信息技术有限公司 一种定位方法及装置
CN111308415A (zh) * 2019-11-01 2020-06-19 华为技术有限公司 一种基于时间延迟的在线估计位姿的方法和设备

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100030470A1 (en) * 2008-07-02 2010-02-04 O2Micro, Inc. Global positioning system and dead reckoning (gps&dr) integrated navigation system
CN102410837A (zh) * 2011-07-29 2012-04-11 江苏大学 一种车辆组合定位导航系统及方法
CN104390643A (zh) * 2014-11-24 2015-03-04 上海美琦浦悦通讯科技有限公司 基于多信息融合实现室内定位的方法
CN109426259A (zh) * 2017-08-25 2019-03-05 本田技研工业株式会社 使用车辆通信进行同步车辆传感器数据获取处理的系统和方法
CN110274589A (zh) * 2018-03-15 2019-09-24 高德信息技术有限公司 一种定位方法及装置
CN110264721A (zh) * 2019-07-01 2019-09-20 北京理工大学 一种城市交叉口周边车辆轨迹预测方法
CN111308415A (zh) * 2019-11-01 2020-06-19 华为技术有限公司 一种基于时间延迟的在线估计位姿的方法和设备

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113483769A (zh) * 2021-08-17 2021-10-08 清华大学 基于粒子滤波器的车辆自定位方法、系统、设备和介质
CN113483769B (zh) * 2021-08-17 2024-03-29 清华大学 基于粒子滤波器的车辆自定位方法、系统、设备和介质

Similar Documents

Publication Publication Date Title
US10705219B2 (en) Method and apparatus for updating maps
JP6913723B2 (ja) 車両制御パラメータのキャリブレーション方法、装置、車載コントローラ及び無人車両
CN107481292B (zh) 车载摄像头的姿态误差估计方法和装置
US10942521B2 (en) Method and apparatus for determining a vehicle control parameter, vehicle on-board controller and autonomous vehicle
US20150134181A1 (en) Position estimation and vehicle control in autonomous multi-vehicle convoys
CN111857152A (zh) 用于生成车辆控制信息的方法和装置
CN110851545A (zh) 地图绘制方法、装置及设备
CN112800159B (zh) 地图数据处理方法及装置
CN109213144A (zh) 人机接口（hmi）架构
CN111207740A (zh) 车辆定位的方法、装置、设备和计算机可读介质
WO2018225067A1 (en) Fusion and calibration of sensor signals in a moving vehicle
CN110849387B (zh) 一种传感器参数标定方法和装置
CN115534925B (zh) 车辆控制方法、装置、设备和计算机可读介质
CN114179825A (zh) 多传感器融合获取量测值置信度方法及自动驾驶车辆
CN115616937A (zh) 自动驾驶仿真测试方法、装置、设备和计算机可读介质
US20200363214A1 (en) Method for using a feature-based localization map for a vehicle
CN113934775A (zh) 车辆轨迹地图匹配方法、装置、设备和计算机可读介质
CN111469781B (zh) 用于输出信息的方法和装置
CN113306570B (zh) 用于控制自动驾驶车辆的方法、装置和自动驾驶配送车
CN113075713A (zh) 一种车辆相对位姿测量方法、系统、设备及存储介质
CN111461980B (zh) 点云拼接算法的性能估计方法和装置
CN108595095B (zh) 基于手势控制模拟目标体运动轨迹的方法和装置
CN114140538B (zh) 车载相机位姿调整方法、装置、设备和计算机可读介质
CN112595330B (zh) 车辆定位方法、装置、电子设备和计算机可读介质
CN112373471B (zh) 用于控制车辆行驶的方法、装置、电子设备和可读介质

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