CN110703272B - 一种基于车车通信和gmphd滤波的周边目标车辆状态估计方法 - Google Patents

一种基于车车通信和gmphd滤波的周边目标车辆状态估计方法 Download PDF

Info

Publication number
CN110703272B
CN110703272B CN201910921766.6A CN201910921766A CN110703272B CN 110703272 B CN110703272 B CN 110703272B CN 201910921766 A CN201910921766 A CN 201910921766A CN 110703272 B CN110703272 B CN 110703272B
Authority
CN
China
Prior art keywords
target
vehicle
coordinate system
gaussian
filtering
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.)
Active
Application number
CN201910921766.6A
Other languages
English (en)
Other versions
CN110703272A (zh
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.)
Jiangsu University
Original Assignee
Jiangsu 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 Jiangsu University filed Critical Jiangsu University
Priority to CN201910921766.6A priority Critical patent/CN110703272B/zh
Publication of CN110703272A publication Critical patent/CN110703272A/zh
Application granted granted Critical
Publication of CN110703272B publication Critical patent/CN110703272B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/66—Tracking systems using electromagnetic waves other than radio waves

Landscapes

  • Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，包括：初始化多目标高斯混合后验强度、获取点云数据、对点云数据进行预处理、转换到自车车体坐标系、预测多目标先验强度、得到多目标后验强度、周边目标车辆进行信息交流、坐标时间同步、将同步后的量测集合变换到自车车体坐标系、应用量测集合更新多目标后验强度，得到最终的多目标后验强度；设定舍弃门限值、合并门限值以及最大高斯混合个数，多目标后验强度进行修剪和合并；提取目标状态得到多目标状态合集。与现有技术相比，本发明能在车联网环境下，融合自车感知信息和周边目标车辆发送的状态信息，提高目标定位、跟踪的准确性和鲁棒性。

Description

一种基于车车通信和GMPHD滤波的周边目标车辆状态估计 方法
技术领域
本发明涉及一种基于车联网的周边目标车辆协同感知方法，特别是涉及一种基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，属于智能车辆领域。
背景技术
智能车辆具有环境感知、规划决策、车间通信等功能。其中，周边目标车辆跟踪是智能车的一种重要环境感知能力，指自车通过车载传感器实时地采集周围环境的各种信息，识别感兴趣的目标车辆，并跟踪其行驶轨迹，从而得到目标车辆准确的状态信息。对周边车辆状态的可靠估计是实现自主驾驶、辅助驾驶、碰撞检测、安全预警等应用的重要基础。
基于激光雷达的目标车辆跟踪，首先利用激光雷达传感器对周围环境进行扫描，再对扫描的数据进行初步的聚类实现目标检测，最后对聚类后的目标进行跟踪。传统跟踪方法将多目标跟踪过程分解为数据关联和状态估计两部分,先利用关联算法进行“航迹-量测”关联，再依据关联结果进行状态估计。但这类方法存在难以克服的内在缺陷：一方面，“组合爆炸”会导致计算上的瓶颈，另一方面，数据关联与状态估计之间存在“耦合”关系，关联误差与状态估计误差容易相互影响。
在真实交通场景下，需要同时对多个目标车辆准确跟踪。然而，由于目前的车载传感器在感知能力、感知范围等方面的限制，以及目标车辆之间的相互遮挡，仅利用自车车载传感器难以实现对多个目标车辆的持续、精确跟踪，从而限制了智能车的环境感知能力。
发明内容
发明目的：针对现有技术中存在的问题，本发明提出一种基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，能通过车间通信技术获取周边目标车辆自身的状态信息，并与自车通过激光雷达感知的信息进行融合，得到周边目标车辆的最终状态。该方法既避免了多目标跟踪时数据关联的困难，又可以利用车车通信改进目标车辆状态估计的准确性，显著提升智能车的目标跟踪能力，在碰撞预警、车辆行为预测等方面具有广阔的应用前景。
技术方案：一种基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，其特征在于，所述方法包括如下步骤：
步骤1：在初始时刻，设定k＝0，初始化周边目标车辆的高斯混合后验强度为
Figure BDA0002217801060000011
步骤2：在k+1时刻，自车通过车载激光雷达传感器获得激光雷达点云数据；
步骤3：对点云数据进行预处理得到k+1时刻的目标量测集合
Figure BDA0002217801060000012
步骤4：将所述目标量测集合
Figure BDA0002217801060000013
转换到自车车体坐标系，得到自车车体坐标系下的目标量测集合
Figure BDA0002217801060000014
步骤5：自车应用GMPHD滤波预测k+1时刻的目标先验强度
Figure BDA0002217801060000015
步骤6：应用目标量测集合
Figure BDA0002217801060000016
更新目标先验强度
Figure BDA0002217801060000017
得到k+1时刻的多目标后验强度
Figure BDA0002217801060000026
步骤7：通过车辆之间的通信，自车接收周边目标车辆通过车载定位设备获取并发送的状态信息，包括目标车辆的GPS坐标、时间、速度和航向角；
步骤8：将上述GPS坐标同步到k+1时刻，得到时间同步的GPS量测集合
Figure BDA0002217801060000027
步骤9：将同步后的GPS量测集合
Figure BDA0002217801060000028
变换到自车车体坐标系，得到量测集合
Figure BDA0002217801060000029
步骤10：将多目标后验强度
Figure BDA00022178010600000210
视为多目标先验强度，应用量测集合
Figure BDA00022178010600000211
进行更新，得到最终的多目标后验强度
Figure BDA00022178010600000212
步骤11：设定舍弃门限值TTh，合并门限值UTh以及最大高斯混合个数Jmax，将上述得到的目标后验强度
Figure BDA00022178010600000213
进行修剪和合并，并保留前Jmax个权重最大的高斯混合项；
步骤12：根据得到的高斯混合项提取高斯分量中大于0.5的高斯分量作为滤波器的输出，相应高斯分量中的均值和方差分别为存活目标的状态估计和误差估计，将提取得高斯分量均值作为目标状态存入多目标状态集合
Figure BDA00022178010600000214
进一步，所述步骤1初始化周边目标车辆的高斯混合后验强度为
Figure BDA00022178010600000215
包括假设多目标高斯混合后验强度为
Figure BDA0002217801060000021
其中，
Figure BDA00022178010600000216
为高斯分布，其中
Figure BDA00022178010600000217
为目标状态，ox,oy为目标车在自车车体坐标系下的坐标，v,a分别为速度和加速度；
Figure BDA00022178010600000218
为初始均值，
Figure BDA00022178010600000219
为初始协方差矩阵；Jk，
Figure BDA00022178010600000220
分别为高斯分量的个数和权重。
进一步，所述步骤3包括基于高斯滤波将点云数据进行点云滤波去除散列点与孤立点，然后对点云数据进行分割和拟合，得到周边目标车辆的量测集合
Figure BDA0002217801060000022
其中，ρ为目标与激光雷达的距离，α为目标所在扫描线的俯仰角，θ为水平方向的航向角，N为目标个数。
进一步，所述步骤4将量测集合
Figure BDA00022178010600000221
从激光雷达极坐标系转换到自车车体坐标系下，包括
(a)将量测集合
Figure BDA00022178010600000222
从激光雷达极坐标转换到激光雷达笛卡尔坐标系，转换公式如下：
Figure BDA0002217801060000023
(b)将上述坐标从激光雷达笛卡尔坐标系转换到自车车体坐标系，得到目标量测集合
Figure BDA0002217801060000024
转换公式如下：
Figure BDA0002217801060000025
其中，Rx,Ry,Rz为激光雷达笛卡尔坐标系相对于自车车体坐标系的旋转矩阵，Txyz为平移矩阵，可通过外参标定法确定旋转矩阵和平移矩阵。
进一步，所述步骤5基于GMPHD滤波预测k+1时刻的目标先验强度，预测公式如下：
Figure BDA0002217801060000031
其中，
Figure BDA00022178010600000315
为存活目标的后验强度，
Figure BDA00022178010600000316
为新生目标的后验强度；
Figure BDA0002217801060000032
Figure BDA0002217801060000033
Figure BDA0002217801060000034
其中，Fk为状态转移矩阵，Qk为过程噪声协方差矩阵，ps,k+1为目标的存活概率；
Figure BDA0002217801060000035
预测的后验概率满足如下形式的高斯混合模型：
Figure BDA0002217801060000036
进一步，所述步骤6用量测集合
Figure BDA00022178010600000317
更新目标先验强度
Figure BDA00022178010600000318
更新公式如下：
Figure BDA0002217801060000037
式中：
Figure BDA0002217801060000038
Figure BDA0002217801060000039
Figure BDA00022178010600000310
Figure BDA00022178010600000311
Figure BDA00022178010600000312
Figure BDA00022178010600000313
其中，pD,k+1为目标发现概率，Hk+1为观测矩阵，Kk+1为卡尔曼增益，Rk+1为量测
Figure BDA00022178010600000319
的误差协方差矩阵，I为单位矩阵。
进一步，所述步骤8将自车通过车车通信接收到的目标车GPS坐标
Figure BDA00022178010600000320
同步到k+1时刻，
Figure BDA00022178010600000321
表示第i个目标车辆的GPS坐标(L,B,H)在ki时刻采集，同步后的GPS量测集合为
Figure BDA00022178010600000314
同步公式如下：
Figure BDA0002217801060000041
其中，Δlon和Δlat分别为纬度和经度插值的偏移量，计算公式如下：
Figure BDA0002217801060000042
ΔT为同步时刻k+1与GPS数据采集时刻ki之间的时间间隔，v为目标车速度，
Figure BDA00022178010600000411
为目标车航向角，l为同一经度下纬度增加一度的距离。
进一步，所述步骤9将同步后的GPS量测集合
Figure BDA00022178010600000412
变换到自车车体坐标系下，得到量测集合
Figure BDA00022178010600000413
包括：
(a)将GPS量测(L,B,H)从大地坐标系转换到地心地固坐标系，转换公式如下：
Figure BDA0002217801060000043
其中，F为法线长度，E为偏心率：
Figure BDA0002217801060000044
Figure BDA0002217801060000045
(b)将地心地固坐标系中的坐标(X,Y,Z)转换到局部相切平面坐标系，转换公式为：
Figure BDA0002217801060000046
其中，
Figure BDA0002217801060000047
[X0,Y0,Z0]为自车车体坐标系原点在地心地固坐标系中的坐标；
(c)将局部相切平面坐标系的坐标(xd,yd)转换到自车车体坐标系下，转换公式如下：
Figure BDA0002217801060000048
其中，φ为自车航向角。
进一步，所述步骤10将目标后验强度
Figure BDA00022178010600000414
视为多目标先验强度，并用量测集合
Figure BDA00022178010600000415
进行更新，更新后的目标后验强度
Figure BDA00022178010600000416
也具有高斯混合形式，高斯混合项为
Figure BDA0002217801060000049
进一步，所述步骤11基于修剪融合算法减少目标后验强度
Figure BDA00022178010600000417
中高斯混合项的个数，得到精简的高斯混合项集合
Figure BDA00022178010600000410
包括：
(a)根据舍弃门限值TTh，保留
Figure BDA00022178010600000418
的高斯项，删除
Figure BDA00022178010600000419
的高斯项；
(b)根据合并门限值UTh，计算高斯项的距离
Figure BDA0002217801060000051
将dij<UTh的高斯项进行合并，更新对应项的权重、状态和误差协方差矩阵；
(c)选出权重最大的Jmax个高斯项。
步骤12：根据得到的高斯混合项
Figure BDA0002217801060000052
提取目标状态得到多目标状态合集
Figure BDA0002217801060000053
包括：
12.1提取
Figure BDA0002217801060000054
个高斯分量中
Figure BDA0002217801060000055
的高斯分量；
12.2相应高斯分量中的均值和方差分别为存活目标的状态估计和误差估计，将提取得高斯分量均值作为目标状态存入多目标状态集合
Figure BDA0002217801060000056
有益效果：本发明能通过车间通信技术获取周边目标车辆自身的状态信息，并与自车通过激光雷达感知的信息进行融合，得到周边目标车辆的最终状态。该方法既避免了多目标跟踪时数据关联的困难，又可以利用车车通信改进目标车辆状态估计的准确性，显著提升智能车的目标跟踪能力，在碰撞预警、车辆行为预测等方面具有广阔的应用前景。
附图说明
图1为本发明的控制原理图。
具体实施方式
下面将参照附图详细地描述实施例。
如图所示，一种基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，该方法的关键技术步骤如下：
步骤1：在k＝0时刻，初始化目标高斯混合后验强度为
Figure BDA0002217801060000057
其中
Figure BDA0002217801060000058
为高斯分布，其中
Figure BDA00022178010600000516
为目标状态，ox,oy为目标车在自车车体坐标系下的坐标，v,a分别为速度和加速度；
Figure BDA0002217801060000059
为初始均值，
Figure BDA00022178010600000510
为初始协方差矩阵；Jk，
Figure BDA00022178010600000511
分别为高斯分量的个数和权重。
步骤2：在k+1时刻，自车通过车载激光雷达传感器获得激光雷达点云数据；
步骤3：将点云数据进行点云滤波去除散列点与孤立点，然后，对点云数据进行分割和拟合，得到周边目标车辆的量测集合
Figure BDA00022178010600000512
其中，ρ为目标车辆距离激光雷达的距离，α为俯仰角，即垂直方向的角度，θ为水平方向的方位角,N为目标个数；
步骤4：将所述目标量测集合
Figure BDA00022178010600000513
转换到自车车体坐标系，得到自车车体坐标系下的目标量测集合
Figure BDA00022178010600000514
4.1将量测集合
Figure BDA00022178010600000515
从激光雷达极坐标转换到激光雷达笛卡尔坐标系，转换公式如下：
Figure BDA0002217801060000061
4.2将上述坐标从激光雷达笛卡尔坐标系转换到自车车体坐标系，得到目标量测集合
Figure BDA00022178010600000611
转换公式如下：
Figure BDA0002217801060000062
其中，Rx,Ry,Rz为激光雷达空间直角坐标相对于自车车体坐标系的旋转矩阵，Txyz为相应的平移矩阵，旋转矩阵和平移矩阵可通过外参标定确定；
步骤5：自车通过GMPHD滤波预测k+1时刻的多目标先验强度，预测公式如下：
Figure BDA0002217801060000063
其中，
Figure BDA00022178010600000612
为存活目标的先验强度，
Figure BDA00022178010600000613
为新生目标的先验强度。
Figure BDA0002217801060000064
Figure BDA0002217801060000065
Figure BDA0002217801060000066
其中，Fk为状态转移矩阵，Qk为过程噪声协方差矩阵，pS,k+1为目标的存活概率。
Figure BDA0002217801060000067
k+1时刻的多目标先验强度(3)也可以写成如下高斯混合形式：
Figure BDA0002217801060000068
其中，Jk+1|k＝Jk+Jγ,k+1为预测的高斯分量个数。
本实施例中，状态转移矩阵为：
Figure BDA0002217801060000069
Δt为激光雷达每帧数据的采样周期；目标的存活概率pS,k+1＝0.99。
步骤6：用量测集合
Figure BDA00022178010600000614
更新目标先验强度
Figure BDA00022178010600000616
得到k+1时刻的多目标后验强度
Figure BDA00022178010600000615
更新公式如下：
Figure BDA00022178010600000610
式中：
Figure BDA0002217801060000071
Figure BDA0002217801060000072
Figure BDA0002217801060000073
Figure BDA0002217801060000074
Figure BDA0002217801060000075
Figure BDA0002217801060000076
其中，pD,k+1为目标被检测到的概率，Hk+1为观测矩阵，Kk+1为卡尔曼增益，Rk+1为量测
Figure BDA00022178010600000713
的误差协方差矩阵，I为单位矩阵。
具体实施例中：目标被检测到的概率PD,k+1＝0.95；观测矩阵为：
Figure BDA0002217801060000077
步骤7：通过车辆之间的通信，自车接收周边目标车辆通过车载定位设备获取并发送的状态信息，包括目标车辆的GPS量测集合
Figure BDA00022178010600000714
速度和航向角
Figure BDA00022178010600000715
其中，M为发送信息的周边目标车辆数，L,B,H,v,
Figure BDA00022178010600000716
为目标车辆在大地坐标系中的经度、纬度、高度，v,
Figure BDA00022178010600000717
为目标车辆的速度和航向角，
Figure BDA00022178010600000718
表示第i个目标车辆的GPS坐标(L,B,H)在ki时刻采集，ki(1≤i≤M)；
步骤8：将目标车辆的GPS量测集合
Figure BDA00022178010600000719
同步到k+1时刻，同步后的GPS量测集合为
Figure BDA00022178010600000720
同步公式如下：
Figure BDA0002217801060000078
其中，Δlon和Δlat分别为纬度和经度插值的偏移量，计算公式如下：
Figure BDA0002217801060000079
ΔT为同步时刻k+1与GPS数据采集时刻ki之间的时间间隔，l为同一经度下纬度增加一度的距离，取值111.32千米。
步骤9：将同步后的GPS量测集合
Figure BDA00022178010600000721
变换到自车车体坐标系下，得到量测集合
Figure BDA00022178010600000722
9.1首先将GPS量测(L,B,H)从大地坐标系转换到地心地固坐标系，转换公式如下：
Figure BDA00022178010600000710
其中，F为法线长度，E为偏心率：
Figure BDA00022178010600000711
Figure BDA00022178010600000712
9.2将地心地固坐标系中的坐标(X,Y,Z)转换到局部相切平面坐标系，转换公式为：
Figure BDA0002217801060000081
其中，
Figure BDA0002217801060000082
[X0,Y0,Z0]为自车车体坐标系原点在地心地固坐标系中的坐标；
9.3将局部相切平面坐标系的坐标(xd,yd)转换到自车车体坐标系下，转换公式如下：
Figure BDA0002217801060000083
其中，φ为自车航向角
步骤10：将步骤6得到的多目标后验强度
Figure BDA0002217801060000088
视为多目标先验强度，并用量测集合
Figure BDA0002217801060000089
进行更新，更新公式与步骤6类似，更新后的目标后验强度
Figure BDA00022178010600000810
也具有高斯混合形式，高斯混合项为
Figure BDA0002217801060000084
步骤11：基于修剪融合算法减少目标后验强度
Figure BDA00022178010600000811
中高斯混合项的个数，得到精简的高斯混合项集合
Figure BDA0002217801060000085
11.1设置舍弃门限值TTh，保留
Figure BDA00022178010600000812
的高斯项，删除
Figure BDA00022178010600000813
的高斯项；
11.2设置合并门限值UTh，计算高斯项的距离
Figure BDA0002217801060000086
将dij<UTh的高斯项进行合并，更新对应项的权重、状态和误差协方差矩阵；
11.3设置保留高斯项个数Jmax，选出权重最大的Jmax个高斯项
具体实施例中：TTh＝10-5，UTh＝5，Jmax＝100
步骤12：根据得到的高斯混合项
Figure BDA0002217801060000087
提取目标状态得到多目标状态合集
Figure BDA00022178010600000814
包括：
12.1提取
Figure BDA00022178010600000815
个高斯分量中
Figure BDA00022178010600000816
的高斯分量
12.2相应高斯分量中的均值和方差分别为存活目标的状态估计和误差估计，将提取得高斯分量均值作为目标状态存入多目标状态集合
Figure BDA00022178010600000817
以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明所记载的内容之后，技术人员可以对本发明作出合理范围内的改动或修改，比如各类参数的改动，这些等效变化和修饰同样落入本发明权利要求所限定的范围之内。

Claims (10)

1.一种基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，其特征在于，所述方法包括如下步骤：
步骤1：在初始时刻，设定k＝0，初始化周边目标车辆的高斯混合后验强度为
Figure FDA0002217801050000011
步骤2：在k+1时刻，自车通过车载激光雷达传感器获得激光雷达点云数据；
步骤3：对点云数据进行预处理得到k+1时刻的目标量测集合
Figure FDA0002217801050000012
步骤4：将所述目标量测集合
Figure FDA0002217801050000013
转换到自车车体坐标系，得到自车车体坐标系下的目标量测集合
Figure FDA0002217801050000014
步骤5：自车应用GMPHD滤波预测k+1时刻的目标先验强度
Figure FDA0002217801050000015
步骤6：应用目标量测集合
Figure FDA0002217801050000016
更新目标先验强度
Figure FDA0002217801050000017
得到k+1时刻的多目标后验强度
Figure FDA0002217801050000018
步骤7：通过车辆之间的通信，自车接收周边目标车辆通过车载定位设备获取并发送的状态信息，包括目标车辆的GPS坐标、时间、速度和航向角；
步骤8：将上述GPS坐标同步到k+1时刻，得到时间同步的GPS量测集合
Figure FDA0002217801050000019
步骤9：将同步后的GPS量测集合
Figure FDA00022178010500000110
变换到自车车体坐标系，得到量测集合
Figure FDA00022178010500000111
步骤10：将多目标后验强度
Figure FDA00022178010500000112
视为多目标先验强度，应用量测集合
Figure FDA00022178010500000113
进行更新，得到最终的多目标后验强度
Figure FDA00022178010500000114
步骤11：设定舍弃门限值TTh，合并门限值UTh以及最大高斯混合个数Jmax，将上述得到的目标后验强度
Figure FDA00022178010500000122
进行修剪和合并，并保留前Jmax个权重最大的高斯混合项；
步骤12：根据得到的高斯混合项提取高斯分量中大于0.5的高斯分量作为滤波器的输出，相应高斯分量中的均值和方差分别为存活目标的状态估计和误差估计，将提取得高斯分量均值作为目标状态存入多目标状态集合
Figure FDA00022178010500000115
2.根据权利要求1所述的基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，其特征在于：所述步骤1初始化周边目标车辆的高斯混合后验强度为
Figure FDA00022178010500000116
包括假设多目标高斯混合后验强度为
Figure FDA00022178010500000117
其中，
Figure FDA00022178010500000118
为高斯分布，其中
Figure FDA00022178010500000119
为目标状态，ox，oy为目标车在自车车体坐标系下的坐标，v，a分别为速度和加速度；
Figure FDA00022178010500000120
为初始均值，
Figure FDA00022178010500000121
为初始协方差矩阵；Jk，
Figure FDA0002217801050000021
分别为高斯分量的个数和权重。
3.根据权利要求1所述的基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，其特征在于：所述步骤3包括基于高斯滤波将点云数据进行点云滤波去除散列点与孤立点，然后对点云数据进行分割和拟合，得到周边目标车辆的量测集合
Figure FDA0002217801050000022
其中，ρ为目标与激光雷达的距离，α为目标所在扫描线的俯仰角，θ为水平方向的航向角，N为目标个数。
4.根据权利要求1所述的基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，其特征在于：所述步骤4将量测集合
Figure FDA0002217801050000023
从激光雷达极坐标系转换到自车车体坐标系下，包括
(a)将量测集合
Figure FDA0002217801050000024
从激光雷达极坐标转换到激光雷达笛卡尔坐标系，转换公式如下：
Figure FDA0002217801050000025
(b)将上述坐标从激光雷达笛卡尔坐标系转换到自车车体坐标系，得到目标量测集合
Figure FDA0002217801050000026
转换公式如下：
Figure FDA0002217801050000027
其中，Rx，Ry，Rz为激光雷达笛卡尔坐标系相对于自车车体坐标系的旋转矩阵，Txyz为平移矩阵，可通过外参标定法确定旋转矩阵和平移矩阵。
5.根据权利要求1所述的基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，其特征在于：所述步骤5基于GMPHD滤波预测k+1时刻的目标先验强度，预测公式如下：
Figure FDA0002217801050000028
其中，
Figure FDA0002217801050000029
为存活目标的后验强度，
Figure FDA00022178010500000210
为新生目标的后验强度；
Figure FDA00022178010500000211
Figure FDA00022178010500000212
Figure FDA00022178010500000213
其中，Fk为状态转移矩阵，Qk为过程噪声协方差矩阵，pS，k+1为目标的存活概率；
Figure FDA00022178010500000214
预测的后验概率满足如下形式的高斯混合模型：
Figure FDA00022178010500000215
6.根据权利要求1所述的基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，其特征在于：所述步骤6用量测集合
Figure FDA0002217801050000031
更新目标先验强度
Figure FDA0002217801050000032
更新公式如下：
Figure FDA0002217801050000033
式中：
Figure FDA0002217801050000034
Figure FDA0002217801050000035
Figure FDA0002217801050000036
Figure FDA0002217801050000037
Figure FDA0002217801050000038
Figure FDA0002217801050000039
其中，pD，k+1为目标发现概率，Hk+1为观测矩阵，Kk+1为卡尔曼增益，Rk+1为量测
Figure FDA00022178010500000310
的误差协方差矩阵，I为单位矩阵。
7.根据权利要求1所述的基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，其特征在于：所述步骤8将自车通过车车通信接收到的目标车GPS坐标
Figure FDA00022178010500000311
同步到k+1时刻，
Figure FDA00022178010500000312
表示第i个目标车辆的GPS坐标(L，B，H)在ki时刻采集，同步后的GPS量测集合为
Figure FDA00022178010500000313
同步公式如下：
Figure FDA00022178010500000314
其中，Δlon和Δlat分别为纬度和经度插值的偏移量，计算公式如下：
Figure FDA00022178010500000315
ΔT为同步时刻k+1与GPS数据采集时刻ki之间的时间间隔，v为目标车速度，
Figure FDA00022178010500000316
为目标车航向角，l为同一经度下纬度增加一度的距离。
8.根据权利要求1所述的基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，其特征在于：所述步骤9将同步后的GPS量测集合
Figure FDA00022178010500000317
变换到自车车体坐标系下，得到量测集合
Figure FDA00022178010500000318
包括：
(a)将GPS量测(L，B，H)从大地坐标系转换到地心地固坐标系，转换公式如下：
Figure FDA0002217801050000041
其中，F为法线长度，E为偏心率：
Figure FDA0002217801050000042
Figure FDA0002217801050000043
(b)将地心地固坐标系中的坐标(X，Y，Z)转换到局部相切平面坐标系，转换公式为：
Figure FDA0002217801050000044
其中，
Figure FDA0002217801050000045
[X0，Y0，Z0]为自车车体坐标系原点在地心地固坐标系中的坐标；
(c)将局部相切平面坐标系的坐标(xd，yd)转换到自车车体坐标系下，转换公式如下：
Figure FDA0002217801050000046
其中，φ为自车航向角。
9.根据权利要求1所述的基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，其特征在于：所述步骤10将目标后验强度
Figure FDA0002217801050000047
视为多目标先验强度，并用量测集合
Figure FDA0002217801050000048
进行更新，更新后的目标后验强度
Figure FDA0002217801050000049
也具有高斯混合形式，高斯混合项为
Figure FDA00022178010500000410
10.根据权利要求1所述的基于车车通信和GMPHD滤波的周边目标车辆状态估计方法，其特征在于：所述步骤11基于修剪融合算法减少目标后验强度
Figure FDA00022178010500000411
中高斯混合项的个数，得到精简的高斯混合项集合
Figure FDA00022178010500000412
包括：
(a)根据舍弃门限值TTh，保留
Figure FDA00022178010500000413
的高斯项，删除
Figure FDA00022178010500000414
的高斯项；
(b)根据合并门限值UTh，计算高斯项的距离
Figure FDA00022178010500000415
将dij＜UTh的高斯项进行合并，更新对应项的权重、状态和误差协方差矩阵；
(c)选出权重最大的Jmax个高斯项。
CN201910921766.6A 2019-09-27 2019-09-27 一种基于车车通信和gmphd滤波的周边目标车辆状态估计方法 Active CN110703272B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910921766.6A CN110703272B (zh) 2019-09-27 2019-09-27 一种基于车车通信和gmphd滤波的周边目标车辆状态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910921766.6A CN110703272B (zh) 2019-09-27 2019-09-27 一种基于车车通信和gmphd滤波的周边目标车辆状态估计方法

Publications (2)

Publication Number Publication Date
CN110703272A CN110703272A (zh) 2020-01-17
CN110703272B true CN110703272B (zh) 2022-05-17

Family

ID=69197625

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910921766.6A Active CN110703272B (zh) 2019-09-27 2019-09-27 一种基于车车通信和gmphd滤波的周边目标车辆状态估计方法

Country Status (1)

Country Link
CN (1) CN110703272B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111220998B (zh) * 2020-02-26 2022-10-28 江苏大学 一种基于车车通信的多目标协同跟踪的方法
CN112147651B (zh) * 2020-08-28 2023-10-10 江苏大学 一种异步多车协同目标状态鲁棒估计方法
CN113219507B (zh) * 2021-01-29 2024-02-23 重庆长安汽车股份有限公司 基于rt3000的自动驾驶车辆感知融合数据的精度测量方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104849702B (zh) * 2015-04-30 2017-10-27 中国民航大学 利用ads‑b数据的gm‑ephd滤波雷达系统误差联合估计方法
CN106372646B (zh) * 2016-08-30 2020-07-14 上海交通大学 基于srck-gmcphd滤波的多目标跟踪方法
CN107544067B (zh) * 2017-07-06 2020-05-19 西北工业大学 一种基于高斯混合近似的高超声速再入飞行器跟踪方法
CN109886305B (zh) * 2019-01-23 2021-05-04 浙江大学 一种基于gm-phd滤波的多传感器非顺序量测异步融合方法
CN109946694A (zh) * 2019-03-22 2019-06-28 哈尔滨工业大学 基于随机有限集的圆周sar多目标跟踪方法

Also Published As

Publication number Publication date
CN110703272A (zh) 2020-01-17

Similar Documents

Publication Publication Date Title
CN110850403B (zh) 一种多传感器决策级融合的智能船水面目标感知识别方法
CN110988912B (zh) 自动驾驶车辆的道路目标与距离检测方法、系统、装置
CN111369541B (zh) 一种智能汽车恶劣天气条件下车辆检测方法
WO2022141914A1 (zh) 一种基于雷视融合的多目标车辆检测及重识别方法
CN108226951B (zh) 一种基于激光传感器的快速运动障碍物实时跟踪方法
CN110703272B (zh) 一种基于车车通信和gmphd滤波的周边目标车辆状态估计方法
CN103064086B (zh) 一种基于深度信息的车辆跟踪方法
CN108509972A (zh) 一种基于毫米波和激光雷达的障碍物特征提取方法
Rawashdeh et al. Collaborative automated driving: A machine learning-based method to enhance the accuracy of shared information
CN106093951B (zh) 基于超声波传感器阵列的物体跟踪方法
JP4984659B2 (ja) 自車両位置推定装置
CN110501006B (zh) 一种异类传感器联合航迹关联与跟踪方法
CN113848545B (zh) 一种基于视觉和毫米波雷达的融合目标检测和跟踪方法
CN112147651B (zh) 一种异步多车协同目标状态鲁棒估计方法
CN113721253B (zh) 基于fmcw激光雷达的运动物体速度检测方法
CN114818916B (zh) 一种基于毫米波雷达多帧点云序列的道路目标分类方法
CN116736322B (zh) 融合相机图像与机载激光雷达点云数据的速度预测方法
CN116978009A (zh) 基于4d毫米波雷达的动态物体滤除方法
CN116359905A (zh) 基于4d毫米波雷达的位姿图slam计算方法及系统
CN117095029A (zh) 空中飞行小目标检测方法及装置
CN113379732B (zh) 一种基于机载激光雷达的线缆目标检测方法
CN115471526A (zh) 基于多源异构信息融合的自动驾驶目标检测与跟踪方法
CN114839628A (zh) 利用多个距离和分辨率的物体检测
CN112883846A (zh) 一种用于检测车辆前方目标的三维数据采集成像系统
AU2019236606B2 (en) Pseudo-range estimation from a passive sensor

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
GR01 Patent grant
GR01 Patent grant