一种基于雷达探测数据的车辆轨迹实时预测方法和装置

摘要

本发明公开了一种基于雷达探测数据的车辆轨迹实时预测方法和装置。其中，所述的车辆轨迹实时预测方法包括：根据车载传感器与雷达实时采集的数据，计算目标车绝对速度的大小、方向以及与本车速度方向的夹角和夹角变化率，再通过夹角变化率与本车横摆角速度计算目标车横摆角速度；分别使用本车和目标车的车速与横摆角速度计算各自的转弯半径；最后预测本车与目标车的行驶轨迹，并通过两车相对距离与车辆坐标系夹角将目标车轨迹转换到本车车辆坐标系中进行计算。所述的装置包括雷达、车载传感器、控制器和执行机构。本发明具有能够准确、实时地预测本车行驶轨迹与环境中目标物的行驶轨迹、及时判断碰撞危险、提高车辆行驶安全稳定性等优点。

说明书

技术领域

本发明涉及车辆轨迹预测领域，具体涉及一种基于雷达探测数据的车辆轨迹实时预测方法和装置。

背景技术

如何保证汽车行驶安全是出行的首要问题。对于驾驶员操控的车辆，驾驶员通过自己的感官对本车状态及周围车辆行驶情形进行观察，以判断本车行驶过程中是否有碰撞危险。此方法依赖于人的观察与主观判断，并不总是可靠。譬如，夜间或雨雾天气行车光线不好时，驾驶员对行车的安全状态可能出现误判。而对于高级驾驶辅助系统乃至脱离人工对驾驶参与控制的无人驾驶车辆，则需使用车载传感器与雷达探测数据，通过控制程序来综合判断本车行驶是否安全。为提前预知可能出现的危险碰撞情形来采取恰当操作避免碰撞危险的发生，就需要对本车轨迹及环境中目标物的轨迹进行实时预测。

现有的轨迹预测方法主要是对本车的弯道轨迹进行预测，或是在倒车或转弯时针对静止障碍物，预测本车车身上突出点的轨迹，以判断能否安全避开障碍物。而对于车辆行驶过程中，本车是否会与周围环境中的诸如其它车辆等移动障碍物发生碰撞则无法进行预测与判断。

发明内容

为了解决上述现有技术的问题，本发明的一个目的是提供一种基于雷达探测数据的本车及目标车辆行驶轨迹实时预测方法；本发明的另一个目的是提供一种应用雷达探测数据的本车及目标车辆行驶轨迹实时预测方法的装置，以解决本车对于自身行驶轨迹与环境中目标物行驶轨迹无法实现准确、实时的预测，以及提高车辆行驶安全稳定性等问题。

本发明的目的可以通过以下技术方案来实现：

一种基于雷达探测数据的车辆轨迹实时预测方法，所述的车辆上设有车载传感器和雷达，所述的车辆轨迹实时预测方法包括以下步骤：

a.由所述车载传感器实时采集本车的车速与横摆角速度，由所述雷达实时探测得到目标车辆的相对运动参数，包括目标车与本车的距离、目标车矢径方向相对速度以及目标车相对方位角；

b.根据采集的数据计算目标车绝对速度的大小、目标车绝对速度方向与本车速度方向的夹角；

c.根据目标车的绝对速度方向与本车速度方向的夹角，由连续采样值计算此夹角变化率，再通过夹角变化率与本车横摆角速度计算目标车横摆角速度；

d.根据本车的车速与横摆角速度计算本车的转弯半径，由目标车横摆角速度与绝对速度计算目标车的转弯半径；

e.利用本车转弯半径与横摆角速度、目标车转弯半径与横摆角速度预测本车与目标车的行驶轨迹，并通过两车相对距离与车辆坐标系夹角将目标车轨迹转换到本车车辆坐标系中进行计算；

f.判断轨迹预测是否终止，如果判断结果为是，则结束预测；如果判断结果为否，则执行步骤a。

优选地，所述的步骤b中，目标车绝对速度由本车车速、目标车矢径方向相对速度和目标车绕本车当前位置转动的速度三部分组成；所述的目标车绝对速度计算过程如下：

b1.计算目标车绝对速度沿本车坐标系x方向的速度分量：

式中：

u：本车车速，单位为m/s；

ω：本车横摆角速度，单位为rad/s；

ρ：目标车雷达探测点距本车距离，单位为：m；

v_r：目标车矢径方向相对速度，单位为：m/s；

θ：目标车相对方位角，单位为：rad；

目标车相对方位角导数，单位为：rad/s；v_tx：目标车绝对速度沿本车坐标系x轴分量，单位为：m/s；

b2.计算目标车绝对速度沿本车坐标系y方向的速度分量：

式中：

v_ty：目标车绝对速度沿本车坐标系y轴分量，单位为：m/s；

b3.目标车绝对速度的计算结果为：

式中：

v_t：目标车绝对速度，单位为：m/s；

b4.本车速度沿本车车辆坐标系x轴正方向，根据目标车绝对速度沿本车车辆坐标系x轴与y轴的速度分量即可计算出目标车绝对速度与本车速度的夹角

式中：

目标车绝对速度与本车速度夹角，单位为：rad。

优选地，令数据采样时间为Δt，由步骤b计算得到上一采样时刻两车速度夹角为则所述步骤c中目标车横摆角速度计算过程如下：

c1.计算目标车绝对速度与本车速度方向的夹角变化率：

式中：

Δt：采样时间间隔，单位为：s；

前一采样时刻目标车绝对速度与本车速度夹角，单位为：rad；

目标车绝对速度与本车速度方向的夹角变化率，单位为：rad/s；

c2.计算目标车横摆角速度：

式中：

ω_t：目标车横摆角速度，单位为：rad/s。

优选地，所述步骤d中本车和目标车转弯半径计算过程如下：

d1.由本车的车速与横摆角速度计算本车的转弯半径R_c：

R_c＝u/ω

式中：

R_c：本车转弯半径，单位为：m；

d2.由目标车横摆角速度与绝对速度计算目标车转弯半径R_t：

R_t＝v_t/ω_t

式中：

R_t：目标车转弯半径，单位为：m。

更加优选地，所述步骤e中本车与目标车的轨迹计算及坐标系转换过程如下：

e1.由本车横摆角速度与转弯半径计算本车轨迹为：

X_c＝R_c·sin(ω·T)

Y_C＝R_c·[1-cos(ω·T)]

式中：

T：轨迹预测时间，可根据需要取值，单位为：s；

X_c：本车预测轨迹横坐标，单位为：m；

Y_C：本车预测轨迹纵坐标，单位为：m；

e2.由目标车转弯半径与横摆角速度计算目标车行驶轨迹，再将目标车行驶轨迹转换到本车车辆坐标系中，计算得到相对于本车的目标车行驶轨迹；

目标车轨迹计算，并转换坐标系：

式中：

T：轨迹预测时间，可根据需要取值，单位为s；

a：本车质心到车头雷达安装位置的横向距离，单位为：m；

目标车速度方向与本车速度方向夹角，单位为：rad；

X_t：目标车预测轨迹横坐标，单位为：m；

Y_t：目标车预测轨迹纵坐标，单位为：m。

根据本发明的另一方面，提供一种实现上述车辆轨迹实时预测方法的装置，所述的装置包括雷达、车载传感器、控制器和执行机构，所述控制器的输入端分别连接雷达和车载传感器，输出端连接执行机构；

所述雷达用于探测目标车的相对运动参数，包括目标车与本车的距离、目标车矢径方向相对速度以及目标车相对方位角；

所述车载传感器用于探测本车的车速与横摆角速度；

所述控制器用于运行所述的车辆轨迹实时预测方法，并输出实时轨迹预测结果。

优选地，所述的雷达采用毫米波雷达；所述的车载传感器包括方向盘转角传感器和轮速传感器。

进一步优选地，所述的执行机构包括转向系统、动力传动系统和制动系统。

与现有技术相比，本发明具有以下优点：

1.本发明的车辆轨迹预测方法根据车载传感器探测数据与雷达探测周围环境数据不仅对本车轨迹进行预测计算，也对雷达探测到的环境中目标物的移动轨迹进行预测，无论是静止目标物还是移动目标物，通过本车与目标物的轨迹预测即可判断本车行驶过程未来数秒时间内是否会发生碰撞危险，进而对本车进行预警或运动干预控制，譬如刹车减速、变更行驶路径等；

2.假定环境中有多辆目标车，本车在行驶过程中可能会与其它车辆发生前向碰撞，通过本发明的车辆轨迹预测方法，利用本车运动状态与雷达探测到的目标车数据预测本车与环境中前方车辆的轨迹，计算本车与目标车之间的动态相对距离，为判断碰撞危险提供依据；

3.对于无人驾驶车辆，准确预测本车轨迹与环境中目标物轨迹，可为无人驾驶控制程序的安全驾驶操纵提供判断依据。因此，本轨迹预测算法也可用于无人车控制中的安全行驶决策；

4.本发明的车辆轨迹预测方法不仅适用于弯道行驶的情形，同样还适用于直道行驶的情形。

附图说明

图1为本发明车辆实时轨迹预测方法中目标车相关参数求解示意图；

图2为本发明雷达探测的目标车3项参数；

图3为本发明的应用示例中预测轨迹与实际轨迹对比图；

图4为本发明车辆的轨迹实时预测装置结构示意图；

图1中，目标车T的速度标注粗线表示本车速度，细线表示目标车相对于本车的速度，粗虚线表示目标车T合成的绝对速度；

图2中，相对速度中间阶段的波动是由于在换道过程中，目标车上的雷达探测位置的不断变化。

具体实施方式

以下结合附图和具体实施例对本发明的基于雷达探测数据的车辆轨迹实时预测方法和装置作进一步详细说明。

本发明所提出一种车辆轨迹实时预测方法，根据本车车速与横摆角速度，雷达探测目标车相对参数，可以计算出目标车绝对速度与横摆角速度。该方法包括以下步骤：

(1)通过采集的数据计算目标车绝对速度

如图1所示，目标车绝对速度由三部分组成：本车车速u、目标车矢径方向相对速度v_r，目标车绕本车当前位置转动的速度因此，目标车绝对速度沿本车坐标系的速度分量计算如下：

参照附图1，目标车速度沿本车坐标系x方向分量:

目标车速度沿本车坐标系y方向分量：

目标车绝对速度v_t计算：

(2)目标车横摆角速度计算

本车速度u沿本车车辆坐标系x轴正方向，根据目标车绝对速度沿本车车辆坐标系x轴与y轴的速度分量即可计算出目标车绝对速度与本车速度的夹角

考虑数据采样时间Δt，上一采样时刻两车速度夹角即可计算夹角变化率，此即为目标车相对于本车的角速度。因此目标车角速度计算如下：

(3)本车轨迹预测

通过传感器输出的本车车速与横摆角速度，计算出本车转弯半径R_c：

R_c＝u/ω

因此，可使用本车横摆角速度ω与转弯半径R_c计算本车轨迹：

X_c＝R_c·sin(ω·T)

Y_C＝R_c·[1-cos(ω·T)]

式中，T为预测时间，可根据需要取值。

(4)目标车轨迹预测计算与坐标系转换

根据之前计算的目标车横摆角速度ω_t与线速度v_t即可计算目标车转弯半径：

R_t＝v_t/ω_t

使用目标车转弯半径R_t与横摆角速度ω_t预测计算目标车行驶轨迹，在本车预测获取目标车行驶轨迹以供使用，因此需要将目标车行驶轨迹转换到本车车辆坐标系中。

目标车轨迹计算，并转换坐标系：

式中，T为预测时间，可根据需要取值。a为本车质心到车头雷达安装位置的横向距离，为目标车速度方向与本车速度方向夹角。

在车辆轨迹预测过程中，如果雷达探测目标消失，或者接收到外部的终止轨迹预测的命令，则停止车辆轨迹预测计算。

如图4所示，一种用于实现基于雷达探测数据的车辆轨迹实时预测方法的装置，包括雷达、车载传感器、控制器和执行机构，控制器的输入端分别连接毫米波雷达、方向盘转角传感器和轮速传感器，用于接收雷达信号、方向盘转角信号和轮速信号；输出端依次连接转向系统、动力传动系统和制动系统等执行机构；控制器运行车辆轨迹实时预测方法，并将实时轨迹预测结果输入给内置的决策与运动控制算法，得到车辆转向系统、动力传动系统和制动系统的控制量以控制执行机构工作。

下面针对具体示例介绍本发明中的轨迹预测算法应用。

假定环境中只有一辆目标车，本车与目标车均在半径为250米的弯道中行驶，本车在本车道正常行驶，目标车位于左侧车道前方90m行驶，并换道到本车道前方。本车车速60km/h，目标车车速30km/h。行驶过程中对两车轨迹进行3秒时长的实时预测。实施例中分别选取换道前、换道中、换道后，共3个时间点对两车预测轨迹绘图，对应图2中的时刻分别为2秒、5秒和7秒。该图还给出了车辆实际行驶轨迹并进行了对比。

如图3所示为车辆实际轨迹与预测轨迹对比图。其中本车预测轨迹以“□”作为起点，“*”作为预测终点，目标车预测轨迹以“o”为预测起点，“Δ”为预测终点，且预测轨迹为虚线。本车3段预测轨迹分别以1、2、3表示，目标车3段预测轨迹分别以a、b、c表示。