CN113267188A - 基于v2x通信的车辆协同定位方法及系统 - Google Patents

基于v2x通信的车辆协同定位方法及系统 Download PDF

Info

Publication number
CN113267188A
CN113267188A CN202110492018.8A CN202110492018A CN113267188A CN 113267188 A CN113267188 A CN 113267188A CN 202110492018 A CN202110492018 A CN 202110492018A CN 113267188 A CN113267188 A CN 113267188A
Authority
CN
China
Prior art keywords
vehicle
target vehicle
side unit
road side
packet data
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
CN202110492018.8A
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.)
Changan University
Original Assignee
Changan University
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 Changan University filed Critical Changan University
Priority to CN202110492018.8A priority Critical patent/CN113267188A/zh
Publication of CN113267188A publication Critical patent/CN113267188A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04L—TRANSMISSION OF DIGITAL INFORMATION, e.g. TELEGRAPHIC COMMUNICATION
    • H04L67/00—Network arrangements or protocols for supporting network services or applications
    • H04L67/01—Protocols
    • H04L67/12—Protocols specially adapted for proprietary or special-purpose networking environments, e.g. medical networks, sensor networks, networks in vehicles or remote metering networks
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/40—Services specially adapted for particular environments, situations or purposes for vehicles, e.g. vehicle-to-pedestrians [V2P]

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Health & Medical Sciences (AREA)
  • Computing Systems (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了基于V2X通信的车辆协同定位方法及系统，包括：目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据和环境车辆发送的环境车辆信标包数据；目标车辆获取目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离；利用卡尔曼滤波算法将路侧单元信标包数据、环境车辆信标包数据、目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离进行融合，得到目标车辆的位置信息。本发明能够更好的实现车辆的精确定位。

Description

基于V2X通信的车辆协同定位方法及系统
技术领域
本发明属于车辆定位技术领域，具体涉及基于V2X通信的车辆协同定位方法及系统。
背景技术
定位技术主要用来获取精确且可靠的物体位置信息。全球定位系统(GPS)具有成熟、低成本等优点而被广泛应用。然而，GPS技术仍一些局限性。一方面，GPS信号容易受到外界环境因素的干扰，例如城市峡谷街区和室内停车场会造成卫星可见度中断、车辆动力学和局部原因(例如接收器噪声、多路径)等导致GPS定位准确性与可靠性的下降。另一方面，其误差水平一般在10米左右，无法满足自动驾驶等高精度要求的业务。车联网以车内网、车际网和车载移动互联网为基础，实现车与人、车与车、车与路、车与服务平台等的全方位网络连接(V2X)，提升汽车智能化水平和自动驾驶能力。车联网无线通信技术是实现车联网的关键，主要分为DSRC/IEEE802.11 p和LTE-V2X两条技术路线。
发明内容
针对现有技术中存在的技术问题，本发明提供了基于V2X通信的车辆协同定位方法及系统，能够更好的实现车辆的精确定位。
为了解决上述技术问题，本发明通过以下技术方案予以实现：
基于V2X通信的车辆协同定位方法，包括：
目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据；
目标车辆获取目标车辆与路侧单元之间的相对距离；
利用卡尔曼滤波算法将路侧单元信标包数据和目标车辆与路侧单元之间的相对距离进行融合，得到目标车辆的位置信息。
进一步地，路侧单元信标包数据包括路侧单元的ID和路侧单元的坐标。
进一步地，所述利用卡尔曼滤波算法将路侧单元信标包数据和目标车辆与路侧单元之间的相对距离进行融合，具体包括：预测阶段和更新阶段；
所述预测阶段包括根据上一时刻目标车辆的自身位置、速度和加速度对当前时刻目标车辆的自身位置、速度和加速度的先验估计值以及误差协方差先验估计值进行预测；目标车辆的自身位置、速度和加速度的初始值通过目标车辆惯性导航系统获得；
所述更新阶段具体如下：
采用所述预测阶段中的当前时刻目标车辆的自身位置、速度和加速度的先验估计值以及当前时刻目标车辆速度测量值，得到当前时刻目标车辆的自身位置、速度和加速度的后验估计值；所述目标车辆速度测量值通过目标车辆惯性导航系统获得；
利用所述目标车辆与路侧单元之间的相对距离和路侧单元的坐标匹配更新目标车辆的自身位置信息。
进一步地，所述利用所述目标车辆与路侧单元之间的相对距离和路侧单元的坐标匹配更新目标车辆的自身位置信息，具体如下：
计算目标车辆与路侧单元之间的角度值，并将计算得到的角度值与目标车辆上的传感器测得的目标车辆与路侧单元之间的角度值进行匹配，得到路侧单元的ID与传感器所测量路侧单元之间的对应关系。
基于V2X通信的车辆协同定位系统，包括：
信标包数据获取模块，用于目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据；
相对距离获取模块，用于目标车辆获取目标车辆与路侧单元之间的相对距离；
数据信息融合模块，用于利用卡尔曼滤波算法将路侧单元信标包数据和目标车辆与路侧单元之间的相对距离进行融合，得到目标车辆的位置信息。
基于V2X通信的车辆协同定位方法，包括：
目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据和环境车辆发送的环境车辆信标包数据；
目标车辆获取目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离；
利用卡尔曼滤波算法将路侧单元信标包数据、环境车辆信标包数据、目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离进行融合，得到目标车辆的位置信息。
进一步地，路侧单元信标包数据包括路侧单元的ID和路侧单元的坐标；
环境车辆信标包数据包括环境车辆的ID、环境车辆的自身位置、速度和加速度。
进一步地，所述利用卡尔曼滤波算法将路侧单元信标包数据、环境车辆信标包数据、目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离进行融合，具体包括：预测阶段和更新阶段；
所述预测阶段包括根据上一时刻目标车辆的自身位置、速度和加速度对当前时刻目标车辆的自身位置、速度和加速度的先验估计值以及误差协方差先验估计值进行预测；还包括根据上一时刻环境车辆的自身位置、速度和加速度对当前时刻环境车辆的自身位置、速度和加速度的先验估计值以及误差协方差先验估计值进行预测；目标车辆的自身位置、速度和加速度的初始值通过目标车辆全球定位系统、惯性导航系统获得，环境车辆的自身位置、速度和加速度的初始值通过环境车辆信标包数据获得；
所述更新阶段具体如下：
采用预测阶段中目标车辆位置、速度和加速度的先验估计值以及当前时刻目标车辆速度测量值结合得到目标车辆当前时刻位置、速度和加速度的后验估计值；采用预测阶段中环境车辆位置、速度和加速度的先验估计值以及当前时刻环境车辆速度测量值结合得到环境车辆当前时刻位置、速度和加速度的后验估计值；所述目标车辆速度测量值通过目标车辆惯性导航系统获得，所述环境车辆速度测量值通过环境车辆信标包数据获得；
利用所述目标车辆获取目标车辆与路侧单元之间的相对距离、目标车辆与环境车辆之间的相对距离、环境车辆的自身位置和路侧单元的坐标匹配更新目标车辆的位置信息。
进一步地，所述利用所述目标车辆获取目标车辆与路侧单元之间的相对距离、目标车辆与环境车辆之间的相对距离、环境车辆的自身位置和路侧单元的坐标匹配更新如下：计算目标车辆与路侧单元间的角度值，并将计算得到的角度值与目标车辆上的传感器测得的目标车辆与路侧单元之间的角度值进行匹配，得到路侧单元的ID与传感器所测量路侧单元之间的对应关系；
计算目标车辆与环境车辆之间的角度值，并将计算得到的角度值与目标车辆上的传感器测得的目标车辆与环境车辆之间的角度值进行匹配，得到环境车辆的ID与传感器所测量环境车辆之间的对应关系。
基于V2X通信的车辆协同定位系统，包括：
信标包数据获取模块，用于目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据和环境车辆发送的环境车辆信标包数据；
相对距离获取模块，目标车辆获取目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离；
数据信息融合模块，利用卡尔曼滤波算法将路侧单元信标包数据、环境车辆信标包数据、目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离进行融合，得到目标车辆的位置信息。
与现有技术相比，本发明至少具有以下有益效果：本发明提供的一种基于V2X通信的车辆协同定位方法，通过V2X通信、惯性导航系统的应用，实现了连续地精准定位，可应用于卫星可见度中断、接收器噪声干扰等GNSS定位异常的场景，且采用的定位算法算力较低，无需高昂计算平台。
为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本发明具体实施方式中的技术方案，下面将对具体实施方式描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是卡尔曼滤波器更新的触发机制图；
图2是本发明实施例车辆定位方法流程示意图；
图3为传感器测量角度图；
图4为目标车辆与路测单元节点间的计算角度图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
基于V2X通信的车辆协同定位方法，包括：
目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据；作为本发明的优选实施方式，本发明中，路侧单元信标包数据包括路侧单元的ID、路侧单元的坐标和路侧单元信标发送时间戳。
目标车辆获取目标车辆与路侧单元之间的相对距离。
利用卡尔曼滤波算法将路侧单元信标包数据和目标车辆与路侧单元之间的相对距离进行融合，得到目标车辆的位置信息；
具体包括：预测阶段和更新阶段；
所述预测阶段包括根据上一时刻目标车辆的自身位置、速度和加速度对当前时刻目标车辆的自身位置、速度和加速度的先验估计值和误差协方差先验估计值进行预测；目标车辆的自身位置、速度和加速度的初始值通过目标车辆全球定位系统、惯性导航系统获得；
结合公式更加详细的说明：
预测阶段上一时刻(记为tk-1)的状态向量仅包括目标车辆的位置、速度、加速度：
xk-1＝[pxk-1,vxk-1,axk-1,pyk-1,vyk-1,ayk-1]
pzk-1,vzk-1,azk-1分别是目标车辆在z轴上的位置、速度和加速度分量。由于路侧单元位置固定，故路侧单元信标包信息未记录在过滤器状态中。
路侧单元信标包数据具有失效时间T。T值大小与VANET的通讯范围，信号强度有关，在这里设阈值为2秒。当路侧单元信标包数据的失效时间达到阈值时(即目标车辆在一段时间内没有收到来自路侧单元的信标包数据)，滤波器判定路侧单元发送的路侧单元信标包数据无效。
对于车辆运动学，假设采用匀变速模型：
xk＝Axk-1+wk-1
其中A是一个6×6的状态转移对角矩阵；wk-1是考虑迁移率模型变化的过程噪声；假设wk-1是随时间变化的独立随机向量。
Figure BDA0003052802530000071
特别地，wk-1是具有协方差矩阵Q的零均值随机向量。
在这里，我们离散化了频谱密度为q[m2/s3]的连续白噪声加速模型：
Figure BDA0003052802530000072
注意，q是一个设计参数，必须根据采样间隔内速度的变化进行设置。
综上，时间更新方程为：
Figure BDA0003052802530000073
Figure BDA0003052802530000074
公式中xk-1|k-1表示k-1时刻的后验状态估计值；
Figure BDA0003052802530000075
为k时刻的先验状态估计值；
Figure BDA0003052802530000076
表示k时刻的先验估计协方差；Pk-1|k-1为k-1时刻的后验估计协方差。
所述更新阶段具体如下：
采用预测阶段中的当前时刻目标车辆的自身位置、速度和加速度的先验估计值以及当前时刻目标车辆速度测量值结合，得到当前时刻目标车辆当前时刻的自身位置、速度和加速度的后验估计值；所述目标车辆速度测量值通过目标车辆惯性导航系统获得；
利用目标车辆与路侧单元之间的相对距离和路侧单元的坐标匹配更新目标车辆的自身位置信息，具体如下：
计算目标车辆与路侧单元之间的角度值，并将计算得到的角度值与目标车辆上的传感器测得的目标车辆与路侧单元之间的角度值进行匹配，得到路侧单元的ID与传感器所测量路侧单元之间的对应关系。
结合公式更加详细的说明：
路侧单元不是卡尔曼状态的一部分，因为它们的位置是固定且已知的。因此，测量矢量为目标车辆惯导系统获取的速度值：
Figure BDA0003052802530000081
测量的协方差矩阵为：
Figure BDA0003052802530000082
式中，
Figure BDA0003052802530000083
和
Figure BDA0003052802530000084
分别是目标车辆在tk处沿x和y方向速度测量的标准偏差。
因此，卡尔曼滤波器的更新阶段执行为
Figure BDA0003052802530000085
Figure BDA0003052802530000086
Figure BDA0003052802530000087
H是状态变量到测量(观测)的转换矩阵：
Figure BDA0003052802530000088
状态向量xk|k及其协方差矩阵Pk|k初始化为：
xk|k＝[x1]
Figure BDA0003052802530000089
式中，x1为使用最佳估计位置、速度和加速度值构造的目标车辆运动学信息的向量。
目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据时，利用所述路侧单元的坐标减去所述目标车辆与路侧单元之间的相对距离得到本车位置的估计。
目标车辆得到多个位置计算值，将其按下式加权平均，并以此进行卡尔曼滤波器中位置估计值的更新。
Figure BDA0003052802530000091
根据表1，我们对每个距离范围使用不同的权重。
表1加权平均法的权重
权重 范围
90％ 对于距离不超过10m的节点
80％ 适用于10.01m至20m距离的节点
10％ 适用于20.01m至30m距离的节点
1％ 对于距离大于30.01m处的节点
目标车辆获取目标车辆与路侧单元之间的相对距离与路侧单元坐标的匹配如下：计算目标车辆与路侧单元间的角度值，并将该值与目标车辆上传感器测得的方位角进行匹配。
结合公式更加详细的说明：
Figure BDA0003052802530000092
是目标车辆与路侧单元间的角度值，固定的路侧单元坐标已知：
p(R)＝[pX(R),pY(R)]T
Figure BDA0003052802530000093
Figure BDA0003052802530000094
Figure BDA0003052802530000095
与
Figure BDA0003052802530000096
的量纲不一样，需进行单位化：
Figure BDA0003052802530000097
Figure BDA0003052802530000098
其中
Figure BDA0003052802530000101
Figure BDA0003052802530000102
Figure BDA0003052802530000103
基于V2X通信的车辆协同定位系统，包括：
信标包数据获取模块，用于目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据；
相对距离获取模块，用于目标车辆获取目标车辆与路侧单元之间的相对距离；
数据信息融合模块，用于利用卡尔曼滤波算法将路侧单元信标包数据和目标车辆与路侧单元之间的相对距离进行融合，得到目标车辆的位置信息。
作为本发明的另一实施方式，基于V2X通信的车辆协同定位方法，其特征在于，包括：
目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据和环境车辆发送的环境车辆信标包数据；优选的，本发明中，路侧单元信标包数据包括路侧单元的ID、路侧单元的坐标和路侧单元信标发送时间戳；环境车辆信标包数据包括环境车辆的ID、环境车辆的自身位置、速度、加速度和环境车辆信标发送时间戳。
目标车辆获取目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离。
利用卡尔曼滤波算法将路侧单元信标包数据、环境车辆信标包数据、目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离进行融合，得到目标车辆的位置信息。
具体包括：预测阶段和更新阶段；
所述预测阶段包括根据上一时刻目标车辆的自身位置、速度和加速度对当前时刻目标车辆的自身位置、速度和加速度的先验估计值以及误差协方差先验估计值进行预测；还包括根据上一时刻环境车辆的自身位置、速度和加速度对当前时刻环境车辆的自身位置、速度和加速度的先验估计值以及误差协方差先验估计值进行预测；目标车辆的自身位置、速度和加速度的初始值通过目标车辆全球定位系统、惯性导航系统获得，环境车辆的自身位置、速度和加速度的初始值通过环境车辆信标包数据获得；
结合公式更加详细的说明：
预测阶段上一时刻(记为tk-1)的状态向量实质上是参加协同定位的车辆信息，顾将其定义为
Figure BDA0003052802530000111
其中1，2，…,m是车辆ID。由于路侧单元位置固定，顾路侧单元信标包信息未记录在过滤器状态中。
路侧单元信标包数据具有失效时间T。T值大小与VANET的通讯范围，信号强度有关，在这里设阈值为2秒。当路侧单元信标包数据的失效时间达到阈值时(即目标车辆在一段时间内没有收到来自路侧单元的信标包数据)，滤波器判定路侧单元发送的路侧单元信标包数据无效。
环境车辆信标包数据也具有失效时间Ti(2≤i≤m)。Ti的值与VANET的通讯范围、信号强度有关，在这里设阈值为2秒。当环境车辆信标包数据的失效时间达到阈值时(即目标车辆在一段时间内没有收到来自环境车辆的信标包数据)，滤波器可以删除与该环境车辆有关的状态来缩减维度。
每辆车的状态向量由位置、速度、加速度表示：
Figure BDA0003052802530000112
Figure BDA0003052802530000113
分别是车辆i的z轴上的位置、速度和加速度分量。
对于车辆运动学，假设采用匀变速模型：
xk＝Axk-1+wk-1
其中A是一个6m×6m的状态转移对角矩阵；wk-1是考虑迁移率模型变化的过程噪声；假设wk-1是随时间变化的独立随机向量。
A＝diag(Aa,...,Aa)
Figure BDA0003052802530000121
特别地，wk-1是具有协方差矩阵Q的零均值随机向量：
Q＝diag(Qa,...,Qa)
在这里，我们离散化了频谱密度为q[m2/s3]的连续白噪声加速模型：
Figure BDA0003052802530000122
注意，q是一个设计参数，必须根据采样间隔内速度的变化进行设置。
综上，时间更新方程为：
Figure BDA0003052802530000123
Figure BDA0003052802530000124
公式中xk-1|k-1表示k-1时刻的后验状态估计值；
Figure BDA0003052802530000125
为k时刻的先验状态估计值；
Figure BDA0003052802530000126
表示k时刻的先验估计协方差；Pk-1|k-1为k-1时刻的后验估计协方差。
所述更新阶段具体如下：
采用预测阶段中目标车辆位置、速度和加速度的先验估计值以及当前时刻目标车辆速度测量值结合得到目标车辆当前时刻位置、速度和加速度的后验估计值；采用预测阶段中环境车辆位置、速度和加速度的先验估计值以及当前时刻环境车辆速度测量值结合得到环境车辆当前时刻位置、速度和加速度的后验估计值；所述目标车辆速度测量值通过目标车辆惯性导航系统获得，所述环境车辆速度测量值通过环境车辆信标包数据获得；
利用所述目标车辆获取目标车辆与路侧单元之间的相对距离、目标车辆与环境车辆之间的相对距离、环境车辆的自身位置和路侧单元的坐标匹配更新目标车辆的位置信息，具体如下：计算目标车辆与路侧单元间的角度值，并将计算得到的角度值与目标车辆上的传感器测得的目标车辆与路侧单元之间的角度值进行匹配，得到路侧单元的ID与传感器所测量路侧单元之间的对应关系；
结合公式更加详细的说明：
涉及协同定位任务的车辆速度被视为整个测量向量：
zk＝Hz+vk
目标车辆速度由本地车载惯性导航系统读取，环境车辆速度从接收到的信标数据包中提取。Hz可以表示为：
Figure BDA0003052802530000131
Hz的行向量数是参与协同定位任务的车辆数。
Figure BDA0003052802530000132
Figure BDA0003052802530000133
Figure BDA0003052802530000134
关于vk，它是一个零均值高斯随机向量，代表带有协方差矩阵Rk的测量噪声：
Figure BDA0003052802530000135
式中，
Figure BDA0003052802530000136
和
Figure BDA0003052802530000137
分别是目标车辆在tk处沿x和y方向速度测量的标准偏差。而
Figure BDA0003052802530000138
和
Figure BDA0003052802530000139
是其他车辆i在tk处沿x和y方向速度测量的标准偏差。
因此，卡尔曼滤波器的更新阶段执行为
Figure BDA0003052802530000141
Figure BDA0003052802530000142
Figure BDA0003052802530000143
公式中Kk是Kalman增益；
Figure BDA0003052802530000144
是实际观测值和预测观测值的残差，加上Kalman增益，修改先验(预测)得到后验值；H是状态变量到测量(观测)的转换矩阵，表示卡尔曼滤波器的状态和观测值之间的线性关系：
Figure BDA0003052802530000145
Figure BDA0003052802530000146
其中0n*m是由零组成的n*m矩阵。
P0|0的设置，取决于过滤器的初始工作状态。如果接收的数据包来自环境车辆，则状态向量及其协方差矩阵初始化为：
xk|k＝[x1，x2]T
Figure BDA0003052802530000147
其中，x1为使用最佳估计位置、速度和加速度值构造的目标车辆运动学信息的向量，而
Figure BDA0003052802530000148
是包含相关标准偏差的6×6对角线矩阵，即
Figure BDA0003052802530000149
类似地，x2和
Figure BDA00030528025300001410
是其他车辆的运动学信息和相关标准偏差，它们是从接收到的信息包中提取出来的。如果第一个信标数据包来自路侧单元，则状态向量将仅包含目标车辆的运动学信息。
目标车辆通过V2X通信获取环境车辆发送的环境车辆信标包数据时，利用所述环境车辆的自身位置减去所述目标车辆与环境车辆之间的相对距离得到本车位置的估计。
目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据时，利用所述路侧单元的坐标减去所述目标车辆与路侧单元之间的相对距离得到本车位置的估计。
目标车辆得到多个位置计算值，将其按下式加权平均，并以此进行卡尔曼滤波器中位置估计值的更新。
Figure BDA0003052802530000151
根据表1，我们对每个距离范围使用不同的权重。需注意，如果同一范围内有多个节点，我们将根据信标包数据的发送对象类型分配权重(路侧单元总占比为0.9,环境车辆总占比为0.1，同类型同距离范围内节点平均分配)。
目标车辆获取目标车辆与路侧单元之间的相对距离与路侧单元坐标的匹配如下：计算目标车辆与路侧单元间的角度值，并将该值与传感器设备本身测得的方位角进行匹配。
目标车辆获取目标车辆与环境车辆之间的相对距离与环境车辆自身位置的匹配如下：计算目标车辆与环境车辆间的角度值，并将该值与目标车辆上传感器测得的方位角进行匹配。
结合公式更加详细的说明：
Figure BDA0003052802530000152
是目标车辆与环境车辆间的角度值，其计算如下：
Figure BDA0003052802530000153
Figure BDA0003052802530000154
Figure BDA0003052802530000155
和
Figure BDA0003052802530000156
的量纲不一样，需进行单位化：
Figure BDA0003052802530000157
Figure BDA0003052802530000158
其中
Figure BDA0003052802530000161
Figure BDA0003052802530000162
基于V2X通信的车辆协同定位系统，包括：
信标包数据获取模块，用于目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据和环境车辆发送的环境车辆信标包数据；
相对距离获取模块，目标车辆获取目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离；
数据信息融合模块，利用卡尔曼滤波算法将路侧单元信标包数据、环境车辆信标包数据、目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离进行融合，得到目标车辆的位置信息。
最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (10)

1.基于V2X通信的车辆协同定位方法，其特征在于，包括：
目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据；
目标车辆获取目标车辆与路侧单元之间的相对距离；
利用卡尔曼滤波算法将路侧单元信标包数据和目标车辆与路侧单元之间的相对距离进行融合，得到目标车辆的位置信息。
2.根据权利要求1所述的基于V2X通信的车辆协同定位方法，其特征在于，路侧单元信标包数据包括路侧单元的ID和路侧单元的坐标。
3.根据权利要求2所述的基于V2X通信的车辆协同定位方法，其特征在于，所述利用卡尔曼滤波算法将路侧单元信标包数据和目标车辆与路侧单元之间的相对距离进行融合，具体包括：预测阶段和更新阶段；
所述预测阶段包括根据上一时刻目标车辆的自身位置、速度和加速度对当前时刻目标车辆的自身位置、速度和加速度的先验估计值以及误差协方差先验估计值进行预测；目标车辆的自身位置、速度和加速度的初始值通过目标车辆惯性导航系统获得；
所述更新阶段具体如下：
采用所述预测阶段中的当前时刻目标车辆的自身位置、速度和加速度的先验估计值以及当前时刻目标车辆速度测量值，得到当前时刻目标车辆的自身位置、速度和加速度的后验估计值；所述目标车辆速度测量值通过目标车辆惯性导航系统获得；
利用所述目标车辆与路侧单元之间的相对距离和路侧单元的坐标匹配更新目标车辆的自身位置信息。
4.根据权利要求3所述的基于V2X通信的车辆协同定位方法，其特征在于，所述利用所述目标车辆与路侧单元之间的相对距离和路侧单元的坐标匹配更新目标车辆的自身位置信息，具体如下：
计算目标车辆与路侧单元之间的角度值，并将计算得到的角度值与目标车辆上的传感器测得的目标车辆与路侧单元之间的角度值进行匹配，得到路侧单元的ID与传感器所测量路侧单元之间的对应关系。
5.基于V2X通信的车辆协同定位系统，其特征在于，用于实现如权利要求1至4任一项所述的方法，包括：
信标包数据获取模块，用于目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据；
相对距离获取模块，用于目标车辆获取目标车辆与路侧单元之间的相对距离；
数据信息融合模块，用于利用卡尔曼滤波算法将路侧单元信标包数据和目标车辆与路侧单元之间的相对距离进行融合，得到目标车辆的位置信息。
6.基于V2X通信的车辆协同定位方法，其特征在于，包括：
目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据和环境车辆发送的环境车辆信标包数据；
目标车辆获取目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离；
利用卡尔曼滤波算法将路侧单元信标包数据、环境车辆信标包数据、目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离进行融合，得到目标车辆的位置信息。
7.根据权利要求6所述的基于V2X通信的车辆协同定位方法，其特征在于，路侧单元信标包数据包括路侧单元的ID和路侧单元的坐标；
环境车辆信标包数据包括环境车辆的ID、环境车辆的自身位置、速度和加速度。
8.根据权利要求6所述的基于V2X通信的车辆协同定位方法，其特征在于，所述利用卡尔曼滤波算法将路侧单元信标包数据、环境车辆信标包数据、目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离进行融合，具体包括：预测阶段和更新阶段；
所述预测阶段包括根据上一时刻目标车辆的自身位置、速度和加速度对当前时刻目标车辆的自身位置、速度和加速度的先验估计值以及误差协方差先验估计值进行预测；还包括根据上一时刻环境车辆的自身位置、速度和加速度对当前时刻环境车辆的自身位置、速度和加速度的先验估计值以及误差协方差先验估计值进行预测；目标车辆的自身位置、速度和加速度的初始值通过目标车辆全球定位系统、惯性导航系统获得，环境车辆的自身位置、速度和加速度的初始值通过环境车辆信标包数据获得；
所述更新阶段具体如下：
采用预测阶段中目标车辆位置、速度和加速度的先验估计值以及当前时刻目标车辆速度测量值结合得到目标车辆当前时刻位置、速度和加速度的后验估计值；采用预测阶段中环境车辆位置、速度和加速度的先验估计值以及当前时刻环境车辆速度测量值结合得到环境车辆当前时刻位置、速度和加速度的后验估计值；所述目标车辆速度测量值通过目标车辆惯性导航系统获得，所述环境车辆速度测量值通过环境车辆信标包数据获得；
利用所述目标车辆获取目标车辆与路侧单元之间的相对距离、目标车辆与环境车辆之间的相对距离、环境车辆的自身位置和路侧单元的坐标匹配更新目标车辆的位置信息。
9.根据权利要求8所述的基于V2X通信的车辆协同定位方法，其特征在于，所述利用所述目标车辆获取目标车辆与路侧单元之间的相对距离、目标车辆与环境车辆之间的相对距离、环境车辆的自身位置和路侧单元的坐标匹配更新如下：计算目标车辆与路侧单元间的角度值，并将计算得到的角度值与目标车辆上的传感器测得的目标车辆与路侧单元之间的角度值进行匹配，得到路侧单元的ID与传感器所测量路侧单元之间的对应关系；
计算目标车辆与环境车辆之间的角度值，并将计算得到的角度值与目标车辆上的传感器测得的目标车辆与环境车辆之间的角度值进行匹配，得到环境车辆的ID与传感器所测量环境车辆之间的对应关系。
10.基于V2X通信的车辆协同定位系统，其特征在于，用于实现如权利要求6至9任一项所述的方法，包括：
信标包数据获取模块，用于目标车辆通过V2X通信获取路侧单元发送的路侧单元信标包数据和环境车辆发送的环境车辆信标包数据；
相对距离获取模块，目标车辆获取目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离；
数据信息融合模块，利用卡尔曼滤波算法将路侧单元信标包数据、环境车辆信标包数据、目标车辆与路侧单元之间的相对距离以及目标车辆与环境车辆之间的相对距离进行融合，得到目标车辆的位置信息。
CN202110492018.8A 2021-05-06 2021-05-06 基于v2x通信的车辆协同定位方法及系统 Pending CN113267188A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110492018.8A CN113267188A (zh) 2021-05-06 2021-05-06 基于v2x通信的车辆协同定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110492018.8A CN113267188A (zh) 2021-05-06 2021-05-06 基于v2x通信的车辆协同定位方法及系统

Publications (1)

Publication Number Publication Date
CN113267188A true CN113267188A (zh) 2021-08-17

Family

ID=77229982

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110492018.8A Pending CN113267188A (zh) 2021-05-06 2021-05-06 基于v2x通信的车辆协同定位方法及系统

Country Status (1)

Country Link
CN (1) CN113267188A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113763738A (zh) * 2021-09-14 2021-12-07 上海智能网联汽车技术中心有限公司 车路协同系统路侧感知与车端感知实时匹配的方法及系统

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110046843A1 (en) * 2009-08-24 2011-02-24 Toyota Motor Engin. & Manufact. N.A.(TEMA) Systems and methods of vehicular path prediction for cooperative driving applications through digital map and dynamic vehicle model fusion
US20170106861A1 (en) * 2015-10-15 2017-04-20 Hyundai Motor Company Vehicle and method for controlling distance between traveling vehicles
CN106767783A (zh) * 2016-12-15 2017-05-31 东软集团股份有限公司 基于车载通信的定位修正方法和装置
CN109920246A (zh) * 2019-02-22 2019-06-21 重庆邮电大学 一种基于v2x通信与双目视觉的协同局部路径规划方法
US20200374053A1 (en) * 2017-12-20 2020-11-26 Lg Electronics Inc. Device and method for v2x communication
CN112019997A (zh) * 2020-08-05 2020-12-01 锐捷网络股份有限公司 一种车辆定位方法及装置
CN112346103A (zh) * 2020-10-29 2021-02-09 清华大学 基于v2x的智能网联汽车动态协同定位方法与装置

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110046843A1 (en) * 2009-08-24 2011-02-24 Toyota Motor Engin. & Manufact. N.A.(TEMA) Systems and methods of vehicular path prediction for cooperative driving applications through digital map and dynamic vehicle model fusion
US20170106861A1 (en) * 2015-10-15 2017-04-20 Hyundai Motor Company Vehicle and method for controlling distance between traveling vehicles
CN106767783A (zh) * 2016-12-15 2017-05-31 东软集团股份有限公司 基于车载通信的定位修正方法和装置
US20200374053A1 (en) * 2017-12-20 2020-11-26 Lg Electronics Inc. Device and method for v2x communication
CN109920246A (zh) * 2019-02-22 2019-06-21 重庆邮电大学 一种基于v2x通信与双目视觉的协同局部路径规划方法
CN112019997A (zh) * 2020-08-05 2020-12-01 锐捷网络股份有限公司 一种车辆定位方法及装置
CN112346103A (zh) * 2020-10-29 2021-02-09 清华大学 基于v2x的智能网联汽车动态协同定位方法与装置

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113763738A (zh) * 2021-09-14 2021-12-07 上海智能网联汽车技术中心有限公司 车路协同系统路侧感知与车端感知实时匹配的方法及系统
CN113763738B (zh) * 2021-09-14 2022-11-11 上海智能网联汽车技术中心有限公司 车路协同系统路侧感知与车端感知实时匹配的方法及系统

Similar Documents

Publication Publication Date Title
KR100532589B1 (ko) 무선인식/위성측위/관성항법을 결합한 통합 측위 장치 및그 방법
Drawil et al. Intervehicle-communication-assisted localization
Al‐Turjman A novel approach for drones positioning in mission critical applications
AU2010304861B2 (en) Improvements in or relating to radio navigation
KR102034527B1 (ko) 와이-파이 위치와 센서정보 결합을 통한 단말 위치 필터링 시스템 및 그 방법
US8326521B2 (en) Traffic situation determination systems, methods, and programs
CN112073909B (zh) 基于uwb/mems组合的uwb基站位置误差补偿方法
US9285408B2 (en) Determination of positions
GB2536846A (en) Position measurement method, own position measurement device, and in-vehicle device
JP2001183439A (ja) 統計的フィルタの使用に基づいた汎用位置決めシステム
CN102436004A (zh) 定位系统及定位方法
CN113063425B (zh) 车辆定位方法、装置、电子设备及存储介质
US8880001B1 (en) Collaborative positioning, navigation and timing
CN110851545A (zh) 地图绘制方法、装置及设备
CN109506647B (zh) 一种基于神经网络的ins和磁力计组合定位方法
Lee et al. Indoor localization by Kalman filter based combining of UWB-positioning and PDR
CN110203253A (zh) 一种非固定式虚拟应答器实现方法
US20170201865A1 (en) Location estimation apparatus and method using combination of different positioning resources
CN116086448A (zh) 基于uwb、imu、gnss融合的无人设备多场景无缝定位方法
CN109444928B (zh) 一种定位方法及系统
WO2004034080A1 (en) Method and apparatus for calculating a figure of merit for gps position using nmea 0183 output
CN102203554B (zh) 具有过滤机制的导航系统及操作该导航系统的方法
CN113267188A (zh) 基于v2x通信的车辆协同定位方法及系统
Richter et al. Cooperative relative localization using vehicle-to-vehicle communications
Choi et al. An adaptive tracking estimator for robust vehicular localization in shadowing areas

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20210817