CN114670851A - 基于寻优跟踪算法的辅助驾驶系统、方法、终端及介质 - Google Patents

基于寻优跟踪算法的辅助驾驶系统、方法、终端及介质 Download PDF

Info

Publication number
CN114670851A
CN114670851A CN202210415589.6A CN202210415589A CN114670851A CN 114670851 A CN114670851 A CN 114670851A CN 202210415589 A CN202210415589 A CN 202210415589A CN 114670851 A CN114670851 A CN 114670851A
Authority
CN
China
Prior art keywords
target
speed
vehicle
displacement
estimation
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
CN202210415589.6A
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.)
Beijing Institute of Technology BIT
Chongqing Innovation Center of Beijing University of Technology
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT, Chongqing Innovation Center of Beijing University of Technology filed Critical Beijing Institute of Technology BIT
Priority to CN202210415589.6A priority Critical patent/CN114670851A/zh
Publication of CN114670851A publication Critical patent/CN114670851A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
    • B60W40/105—Speed
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/10—Longitudinal speed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/10—Longitudinal speed
    • B60W2520/105—Longitudinal acceleration

Landscapes

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

Abstract

本发明提供一种基于寻优跟踪算法的辅助驾驶系统、方法、终端及介质，通过将局部寻优后被使用的所有点迹分类且不参与后续目标聚类、拟合等过程，能有效使每个目标在每一帧数据中都能得到最优估计的状态信息。因上一帧所有航迹经局部寻优处理后都预先在关联模块进行对应目标点迹寻优关联处理，使当前帧只有新出现的目标点迹数据进入聚类模块，可明显降低多目标跟踪方法所需要处理的数据量，降低数据处理所需要实际算力，实现算力低负载条件下，也能满足实际需要，同时也降低了车载雷达本身的算力成本。

Description

基于寻优跟踪算法的辅助驾驶系统、方法、终端及介质
技术领域
本发明涉及智能驾驶领域，尤其涉及基于寻优跟踪算法的辅助驾驶系统、方法、终端及介质。
背景技术
随着汽车保有量不断增加，频繁发生的交通事故已成为日益严重的公共安全问题。车辆行驶环境复杂和频发的道路交通安全事故对智能汽车的ADAS(Advanced DriverAssistant System，高级驾驶辅助系统)功能需求日益增长。ADAS功能有效辅助驾驶员的眼睛、耳朵等感官，扩大驾驶员对驾驶环境中存在危险驾驶的感知能力，更好的探测车辆自身周围的环境信息及在交通中与其他车辆或基础设施进行信息交互，辅助驾驶员对车辆进行精准的控制。
车辆的驾驶任务由信息感知和决策控制等功能组成，提升车辆对环境信息的感知能力，并提升对环境信息中存在危险工况时的告警率，帮助驾驶员做出准确的决策，更好的避免因危险导致事故的发生。作为车辆感知环境信息的关键部件之一的车载雷达传感器，基于车载雷达开发的ADAS功能，往往需要经过严格的时间与空间定义处理才能采集到有效的雷达数据，然后使用有效的雷达数据开发ADAS功能。
目前已有的基于车载雷达开发的ADAS告警功能中，多目标跟踪方法主要对雷达输出的目标点迹进行聚类、起始航迹、然后对多航迹做预测、关联和更新，实现多目标跟踪。在多目标跟踪方法中，会将当前帧所有点迹处理在每个模块都进行遍历处理，聚类时对所有目标点迹进行遍历，然后在预测、关联及更新等模块仍要遍历所有数据信息，使这一过程所需要算力较高，往往考虑的是增加硬件，如FPGA、GPU等来提升算力。同时因每个模块都在对所有数据进行遍历计算，尤其是每一帧数据都在进行聚类后再进行与航迹关联，无法做到航迹与下一帧时目标点迹中最优点的关联记录处理，当关联不到最优点迹时，在状态更新时就无法更新到目标当前时刻的最优状态，即目标位置、速度、角度并不是最优。
因此，为了解决ADAS系统中功能更准确告警问题，提高车辆在道路行驶过程中对危险的预判能力，并降低算法所需要的算力消耗，本发明开发了一种基于车载雷达多目标跟踪方法的辅助驾驶方案，应用智能汽车辅助驾驶。
发明内容
为解决上述技术问题，本发明提供一种基于寻优跟踪算法的辅助驾驶系统，包括车辆行驶数据采集模块、雷达点迹数据采集模块、多目标跟踪算法模块；所述车辆行驶数据采集模块用于采集车辆行驶数据，所述车辆行驶数据包括自车速度、自车位移、横摆角速率；所述雷达点迹数据采集模块用于采集目标的状态数据，所述目标的状态数据包括目标相对于自车的方位角、距离、位移、速度、加速度；所述多目标跟踪算法模块用于基于当前时刻的所述车辆行驶数据和所述目标的状态数据，对所述目标在下一时刻的位置和速度进行最优状态估计，得到目标的最优位置估计和最优速度估计；将目标的最优位置估计和最优速度估计输入ADAS系统进行碰撞预警，以辅助驾驶。
可选的，所述多目标跟踪算法模块用于基于当前时刻目标相对于自车的位移、速度和加速度，计算得到第一评估位移，利用当前时刻自车的横摆角速率对所述第一评估位移进行补偿估计，得到所述目标的最优位置估计。
可选的，所述多目标跟踪算法模块用于基于当前时刻目标相对于自车的位移、速度和加速度，计算得到第一评估位移包括：
按照如下公式(1)-(2)计算得到下一时刻的第一评估位移：
xk＝xk-1+vx*t+0.5*ax*t2 (1)
其中，xk为下一时刻目标纵向位移，xk-1为当前时刻目标纵向位移，vx为当前时刻目标纵向速度，ax为当前时刻目标纵向加速度，t为单位时间；
yk＝yk-1+vy*t+0.5*ay*t2 (2)
其中,yk为下一时刻目标横向位移，yk-1为当前时刻目标横向位移，vy为当前时刻目标横向速度，ay为当前时刻目标横向加速度；所述第一评估位移由所述下一时刻目标纵向位移和所述下一时刻目标横向位移构成；
所述多目标跟踪算法模块用于利用当前时刻自车的横摆角速率对所述第一评估位移进行补偿估计，得到所述目标的最优位置估计包括：
按照如下公式(3)对所述第一评估位移进行补偿估计，得到所述目标的最优位置估计:
Figure BDA0003605779300000031
其中，x补偿后的纵向位移，y表示补偿后的横向位移，θyawrate为横摆角速率；所述最优位置估计由所述补偿后的纵向位移和补偿后的横向位移构成。
可选的，所述多目标跟踪算法模块用于基于目标相对于自车的方位角、速度、以及自车速度计算得到第一评估速度，利用当前时刻自车的横摆角速率对所述第一评估速度进行补偿估计，得到所述目标的最优速度估计。
可选的，所述多目标跟踪算法模块用于基于目标相对于自车的方位角、速度、以及自车速度计算得到第一评估速度包括：
按照如下公式(4)将所述目标相对于自车的速度转化为目标相对于地的绝对径向速度：
Vobj＝Vvehicle cos(theta)+Vradar (4)
其中,Vvehicle为自车当前时刻速度，theta为目标相对自车的方位角，Vradar为雷达探测到的目标速度，Vobj为目标相对于地的绝对径向速度；
依据目标相对于地的绝对径向速度Vobj，利用目标相对于自车的方位角，将其分解到车辆直角坐标系下，获取得到目标纵向速度与目标横向速度，具体计算如下公式(5)：
Vx＝Vobj cos(theta)
Vy＝Vobj sin(theta) (5)
其中,Vx为目标纵向速度，Vy为目标横向速度；所述第一评估速度包括所述目标纵向速度与所述目标横向速度；
利用车辆横摆角速率θyawrate在单位时间产生的角度变化量，对所述第一评估速度进行目标速度补偿得到下一时刻的最优速度估计：
Figure BDA0003605779300000041
其中，所述最优速度估计包括补偿后的vx'目标纵向速度和补偿后的目标vy'横向速度，
可选的，所述多目标跟踪算法模块还用于在将目标的最优位置估计和最优速度估计输入ADAS系统之前，根据目标相对于自车的距离和速度，确定该目标是否满足筛选条件，如是，将该目标的最优位置估计和最优速度估计输入ADAS系统进行碰撞预警。
可选的，所述辅助驾驶系统还包括多目标跟踪可视化模块、跟踪算法耗时评估模块和危险告警装置；所述多目标跟踪可视化模块用于对目标的状态数据进行图像化展示；所述跟踪算法耗时评估模块用于统计及时跟踪算法所耗费时间，以用于判定系统的实时性；所述危险告警装置用于接收ADAS系统输出的危险告警信号，针对危险目标进行告警。
本发明还提供一种基于寻优跟踪算法的辅助驾驶方法，包括：
采集车辆行驶数据，所述车辆行驶数据包括自车速度、自车位移、横摆角速率；
采集目标的状态数据，所述目标的状态数据包括目标相对于自车的方位角、距离、位移、速度、加速度；
基于当前时刻的所述车辆行驶数据和所述目标的状态数据，对所述目标在下一时刻的位置和速度进行最优状态估计，得到目标的最优位置估计和最优速度估计；
将目标的最优位置估计和最优速度估计输入ADAS系统进行碰撞预警，以辅助驾驶。
本发明还提供一种车载终端，包括处理器、存储器及通信总线；
所述通信总线用于实现处理器和存储器之间的连接通信；
所述处理器用于执行存储器中存储的一个或者多个程序，以实现如上所述的基于寻优跟踪算法的辅助驾驶方法的步骤。
本发明还提供一种存储介质，所述存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如上所述的基于寻优跟踪算法的辅助驾驶方法的步骤。
本发明的有益效果是：
根据本发明提供的基于寻优跟踪算法的辅助驾驶系统、方法、终端及介质，通过将局部寻优后被使用的所有点迹分类且不参与后续目标聚类、拟合等过程，能有效使每个目标在每一帧数据中都能得到最优估计的状态信息。因上一帧所有航迹经局部寻优处理后都预先在关联模块进行对应目标点迹寻优关联处理，使当前帧只有新出现的目标点迹数据进入聚类模块，可明显降低多目标跟踪方法所需要处理的数据量，降低数据处理所需要实际算力，实现算力低负载条件下，也能满足实际需要，同时也降低了车载雷达本身的算力成本。
附图说明
图1为本发明实施例一的基于寻优跟踪算法的辅助驾驶系统结构示意图；
图2为本发明实施例一的多目标跟踪流程示意图；
图3为本发明实施例一的本系统与真值数据关于纵向历史轨迹对比示意图；
图4为本发明实施例一的本系统与真值数据关于横向历史轨迹对比示意图；
图5为本发明实施例一的多目标跟踪各模块耗时统计示意图；
图6为本发明实施例二的基于寻优跟踪算法的辅助驾驶方法流程示意图；
图7为本发明实施例三的车载终端结构示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，下面通过具体实施方式结合附图对本发明作进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
实施例一：
为解决车载雷达多目标跟踪方法中目标状态估计非最优、算力消耗较高、辅助驾驶系统功能验证效率不高、准确率低的问题，需要利用车载雷达多目标跟踪算法将目标的位置、速度等状态信息估计到最优，耗时降到最低，使危险目标信息能实时或更快、更准传递给辅助驾驶系统中每个功能模块。跟踪的目标需要计算目标最优状态信息。本发明提供一种基于寻优跟踪算法的辅助驾驶系统，通过局部寻优提高跟踪目标状态信息估计准确性，单帧数据中大量减少点迹在全局参与计算，使算法耗时更低，实时性更强。
请参见图1，辅助驾驶系统包括车辆行驶数据采集模块11、雷达点迹数据采集模块12、多目标跟踪算法模块13；其中，车辆行驶数据采集模块11用于采集车辆行驶数据，车辆行驶数据包括自车速度、自车位移、横摆角速率；雷达点迹数据采集模块12用于采集目标的状态数据，目标的状态数据包括目标相对于自车的方位角、距离、位移、速度、加速度；多目标跟踪算法模块13用于基于当前时刻的车辆行驶数据和目标的状态数据，对目标在下一时刻的位置和速度进行最优状态估计，得到目标的最优位置估计和最优速度估计；将目标的最优位置估计和最优速度估计输入ADAS系统进行碰撞预警，以辅助驾驶。
可选的，多目标跟踪算法模块13用于基于当前时刻目标相对于自车的位移、速度和加速度，计算得到第一评估位移包括：
按照如下公式(6)-(7)计算得到下一时刻的第一评估位移：
xk＝xk-1+vx*t+0.5*ax*t2 (6)
其中，xk为下一时刻目标纵向位移(纵向可以理解为自车行进方向)，xk-1为当前时刻目标纵向位移，vx为当前时刻目标纵向速度，ax为当前时刻目标纵向加速度，t为单位时间；
yk＝yk-1+vy*t+0.5*ay*t2 (7)
其中，yk为下一时刻目标横向位移(横向可以理解为垂直于车身方向)，yk-1为当前时刻目标横向位移，vy为当前时刻目标横向速度，ay为当前时刻目标横向加速度；第一评估位移由下一时刻目标纵向位移和下一时刻目标横向位移构成；
多目标跟踪算法模块13用于利用当前时刻自车的横摆角速率对第一评估位移进行补偿估计，得到目标的最优位置估计包括：
按照如下公式(8)对第一评估位移进行补偿估计，得到目标的最优位置估计:
Figure BDA0003605779300000071
其中，x补偿后的纵向位移，y表示补偿后的横向位移，θyawrate为横摆角速率；最优位置估计由补偿后的纵向位移和补偿后的横向位移构成。
可选的，多目标跟踪算法模块13用于基于目标相对于自车的方位角、速度、以及自车速度计算得到第一评估速度，利用当前时刻自车的横摆角速率对第一评估速度进行补偿估计，得到目标的最优速度估计。
可选的，多目标跟踪算法模块13用于基于目标相对于自车的方位角、速度、以及自车速度计算得到第一评估速度包括：
按照如下公式(9)将目标相对于自车的速度转化为目标相对于地的绝对径向速度：
Vobj＝Vvehicle cos(theta)+Vradar (9)
其中,Vvehicle为自车当前时刻速度，theta为目标相对自车的方位角，Vradar为雷达探测到的目标速度，Vobj为目标相对于地的绝对径向速度；
依据目标相对于地的绝对径向速度Vobj，利用目标相对于自车的方位角，将其分解到车辆直角坐标系下(车辆直角坐标系平行于大地)，获取得到目标纵向速度与目标横向速度，具体计算如下公式(10)：
Vx＝Vobj cos(theta)
Vy＝Vobj sin(theta) (10)
其中,Vx为目标纵向速度，Vy为目标横向速度；所述第一评估速度包括所述目标纵向速度与所述目标横向速度；
利用车辆横摆角速率θyawrate在单位时间产生的角度变化量，对所述第一评估速度进行目标速度补偿得到下一时刻的最优速度估计：
Figure BDA0003605779300000072
其中，最优速度估计包括补偿后的vx'目标纵向速度和补偿后的vy'目标横向速度，
其中横摆角速率θyawrate按车辆转向方向有正负，当左转向时横摆角速率为正，右转向时横摆角速率为负。依据推理进行速度补偿估计，实现目标状态量补偿最优估计计算。
在本发明的其他实施例中，多目标跟踪算法模块13还用于在将目标的最优位置估计和最优速度估计输入ADAS系统之前，根据目标相对于自车的距离和速度，确定该目标是否满足筛选条件，如是，将该目标的最优位置估计和最优速度估计输入ADAS系统进行碰撞预警。目标相对于自车的距离越近，速度越大，则表明与自车发生碰撞的概率越大，也就越危险，优先将危险等级较高的目标，优先输出给ADAS功能系统，验证在ADAS功能系统中是否会触发生成行车危险告警信号，通过危险告警信号触发告警设备，危险告警设备主动将危险反馈给驾驶员，完成驾驶辅助验证。辅助驾驶系统功能主要针对盲区碰撞危险、后方来车碰撞危险、后方来车换道碰撞危险、倒车入库碰撞危险、前碰撞危险等场景应用。
在本发明的其他实施例中，辅助驾驶系统还包括多目标跟踪可视化模块14、跟踪算法耗时评估模块15和危险告警装置16；多目标跟踪可视化模块14用于对目标的状态数据进行图像化展示；跟踪算法耗时评估模块15用于统计及时跟踪算法所耗费时间，以用于判定系统的实时性；危险告警装置16用于接收ADAS系统输出的危险告警信号，针对危险目标进行告警。
因跟踪算法第一步则是需要从点迹数据中聚类出目标，为每一个目标起始新航迹，针对每个目标航迹则可进行最优估计，此后结合目标运动的历史轨迹，使用目标当前最优的状态量与下一时刻目标点迹相关联。在距离维寻最优，即点迹到航迹当前位置的距离最短，并判断是否满足最优条件(即满足雷达分辨率条件下，横向最优距离为0.5m，纵向1m)，当没有最优距离条件时，则调整策略为次优距离，按照车辆物理尺寸为基准，横向次优距离为2m，纵向为5m。
在距离维寻优关联，仍有可能将杂波干扰点关联上，此时利用速度维的匹配关联，求解点迹与航迹最小误差是否满足设定速度阈值，例如1m/s，即雷达探测目标速度误差允许范围最小值，同时也满足速度分辨率。
请参加如图2所示的多目标跟踪过程，该过程主要是通过航迹与输入点迹寻优关联，并非先将所有点迹进行聚类为需要的目标后，利用聚类目标与航迹目标进行关联的方法。局部寻优关联跟踪算法过程中，首先在距离维寻找匹配的点迹，然后在速度维寻找匹配点迹，在关联到与目标相似度较高的点迹时，将被寻优关联上的点迹分类标记为以被处理点迹，即该点迹已匹配到对应的目标航迹，其他的点迹则被分类为未被处理点迹，未被处理点迹当做新出现的目标点迹。利用聚类算法在距离维和速度维将属于同一个目标的点迹聚在一起成簇，并利用簇内所有目标点迹计算表征该目标的中心点坐标，然后利用中心点起始目标航迹。针对已关联到目标点迹的航迹目，则利用目标相对自车坐标系的航向角计算出目标拟合框距离自车最近的点，利用该点信息更新航迹距离、速度等状态信息及扩展信息，完成航迹目标状态最优估计输出。
当存在碰撞危险，ADAS系统利用状态标志位表示告警标志，生成告警信号，利用CAN总线发送给车辆上告警装置，触发告警。
本发明提供辅助驾驶系统，验证雷达在车辆行驶过程中对道路环境存在危险预判的一种功能，以解决车辆对道路环境危险工况判断不准确或发现不及时的危险问题，降低道路交通安全事故的发生，保持车辆在近距离80米以内，都能实时准确跟踪预判危险，生成告警信号，并通过CAN总线将危险信号发送给车辆告警装置，及时对告警提醒驾驶员安全行车的危险信号进行验证。
为了实现上述目的，本发明采用车载雷达实际路测数据，开发基于历史轨迹进行点迹局部寻优的多目标跟踪算法，使算法能对雷达探测到的每一个目标状态信息进行最优估计，并依据目标相对自车位置关系和速度进行打分处理，计算出危险等级越高的目标即得分越高，将危险等级较高的目标，优先输出给ADAS功能系统，验证在ADAS功能系统中是否会触发生成行车危险告警信号，通过危险告警信号触发车辆告警装置，车辆告警装置主动将危险反馈给驾驶员，完成驾驶辅助验证。
应当理解的是，车载毫米波雷达能准确探测目标的距离、速度、角度，并且依据目标自身的材料对电磁波反射特性和反射距离，耦合输出不同能量强度的数据，而道路环境中不同目标对应距离、速度、角度、能量强度等信息在数据中表征也不同。针对道路环境目标，静止目标没有速度，没有航向角；运动的目标有速度及航向角，其中低速的目标速度小，航向角在运动场景中变化较慢；行人等非刚体目标在雷达探测到的数据中姿态会发生变化，而刚体目标在雷达探测到的数据中姿态不会发生变化，物理尺寸大小固定等。
雷达探测到的车辆行驶道路环境中的目标在距离、速度、角度和能量强度方面都具有自身的特征。而目标相对自车存在危险而产生的告警，主要考虑为是否会发生碰撞，涉及距离及速度上的相对运动关系，距离无论是以目标的质心还是最近点表征目标，最终产生碰撞危险的仍是最近距离，所述选择最近点作为目标表征点。
多目标跟踪算法采用卡尔曼滤波算法对表征的目标信息做最优估计，在车辆运动过程中，分为向前运动或向后运动或静止状态，向前或向后运动又分为直行、左转、右转三种运动状态，雷达只能直接探测目标的距离速度等信息，无法估计自车转向运动对雷达探测目标产生的影响，本实施例通过融合自车转向产生的误差影响补偿给雷达探测的目标，转向时会有一个转弯半径及自车会产生一个横摆角速率，横摆角速率在单位时间内会产生一个小角度范围的航向变化，将纵向x轴方向与横向y轴方向的位置速度分别按横摆角速率在单位时间内产生的角度分解到纵向和横向，如同向运动则进行相加补偿，反向运动则相减补偿。当对目标下一时刻状态进行预测时，首先将速度与加速度在单位时间内变化的位移加上当前位移，预测到目标在单位时间后实际的位移；在速度预测时，将加速度在单位时间内产生的速度变化量与当前时刻速度相加，预测到目标在单位时间后的实际速度。
可选的，在对航迹做最优预测处理后，则可进行点迹与航迹关联时，首先筛选掉被关联点与被关联航迹，然后针对未被关联的点迹与航迹，进行点迹与航迹运动过的历史轨迹的当前局部位置进行距离关联，计算关联到满足波门阈值的点迹与航迹，然后进行点迹与航迹当前速度的差值是否满足速度关联阈值，匹配为同一个目标的速度。如果距离、速度同时满足关联条件时，则完成局部寻最优的关联计算，将寻优到的最优点迹记录到航迹对应结构化内存中，并分类为已处理点迹。
利用最优点迹对目标状态进行更新，估计目标最优状态信息。此时将静止或停止的目标加速度置为零，因为非运动目标没有加速度；针对近距离纵横向20m范围内的运动目标，则限制加速度在较小的范围内，依据雷达距离维分辨率和速度分辨率，纵向限制为正负0.2m/s范围，横向为正负0.05m/s，当在加速度值在该范围内时，则保持不变，如果超过范围，则选择对应范围内的最大或最小值作为当前值，确保近距离加速度对速度与距离在单位时间内的变化量影响范围较稳定。远距离目标依据正常更新迭代计算的加速度值进入下一次预测与更新使用。最终依据跟踪算法对目标最优估计的状态信息，将危险等级较高的目标信息优先输出给辅助驾驶功能模块，验证各种危险碰撞场景中是否会发生告警信号生成，并通过告警设备反馈危险信号给驾驶员，高效实现辅助驾驶功能验证。
可选的，为了提高告警系统的效率及准确性，首先利用车辆行驶工况进行多目标跟踪算法应用场景划分，自车运动工况分别划分为静止(不会产生运动趋势，一直保持静止的目标)、停止(有将运动趋势的目标，比如车辆准备起始运动)、低速、高速。
在对应不同车辆运动工况下，雷达探测到目标的距离、速度、角度都在随时间连续变化，即目标信息相对自车运动变化随工况的不同而存在差异，通过CAN线读取自车实时运动信息补偿到雷达探测目标信息，则可计算得到雷达探测到目标对地绝对状态信息。
自车在静止与停止、低速、高速状态下，雷达探测到相同目标在同一帧点迹离散性与连续帧位置变化率都是不同。在同一帧处理相同目标时，对应自车运动工况配置不同参数，使同一帧内同一目标多个点迹被分类为一个目标，用于起始航迹进行状态最优估计及跟踪；连续帧内处理同一目标时，因历史航迹存在，首先针对不同工况先配置距离、速度、角度参数，关联当前时刻目标信息用于更新航迹信息，然后针对没有被处理到的目标信息再进行目标分类起始航迹，用于跟踪。
同时，设计划分目标的不同属性，将速度较小或者为零的设定为静止或停止目标，将有速度划分为运动目标，因为运动目标相对自车有较高的危险性，所以将运动目标设计为优先级较高的危险目标，同时将距离较近的目标设定为危险级较高的目标，因为距离较近，更容易发生危险碰撞。结合优先级的设计，优先处理上报危险等级较高的目标。并采用雷达内部DSP浮点运算方法进行计算优化，降低跟踪算法对DSP算力负载需求。
最后，依据跟踪输出数据到辅助驾驶功能系统进行验证。在多目标跟踪算法输出目标历史轨迹，并与真值系统同步量测的目标轨迹进行对比，评估验证车载雷达多目标跟踪算法输出目标轨迹的准确性，请参见图3-4。
利用时钟计时计算，请参见图5，在跟踪目标时计时统计每个算法子模块及整个集成模块耗时，针对耗时评估验证算法的实时性。在有效验证多目标跟踪算法准确性与实时性的同时，将多目标跟踪算法最优估计的数据输出给辅助驾驶系统功能模块中，验证对应场景辅助驾驶功能是否准确触发告警信号反馈给驾驶员，完成辅助驾驶验证。
本发明提供的辅助驾驶系统能有效处理跟踪车辆行驶场景中较危险目标，并将危险等级越高的目标按照碰撞距离关系进行打分，依据得分高低排列在最优先触发告警的位置。此时利用多目标跟踪结果数据可进行辅助驾驶功能验证，观测辅助驾驶功能在危险目标触发时，是否发生告警信号生成，触发装置响应进行告警。本发明对智能汽车的车载雷达多目标跟踪算法开发及辅助驾驶验证系统开发具有较好的理论指导及应用价值。
实施例二：
本实施例在上述实施例一的基础上，提供一种基于寻优跟踪算法的辅助驾驶方法，请参见图6，主要包括：
S601、采集车辆行驶数据，车辆行驶数据包括自车速度、自车位移、横摆角速率；
S602、采集目标的状态数据，目标的状态数据包括目标相对于自车的方位角、距离、位移、速度、加速度；
S603、基于当前时刻的车辆行驶数据和目标的状态数据，对目标在下一时刻的位置和速度进行最优状态估计，得到目标的最优位置估计和最优速度估计；
按照如下公式(11)-(12)计算得到下一时刻的第一评估位移：
xk＝xk-1+vx*t+0.5*ax*t2 (11)
其中，xk为下一时刻目标纵向位移(纵向可以理解为自车行进方向)，xk-1为当前时刻目标纵向位移，vx为当前时刻目标纵向速度，ax为当前时刻目标纵向加速度，t为单位时间；
yk＝yk-1+vy*t+0.5*ay*t2 (12)
其中，yk为下一时刻目标横向位移(横向可以理解为垂直于车身方向)，yk-1为当前时刻目标横向位移，vy为当前时刻目标横向速度，ay为当前时刻目标横向加速度；第一评估位移由下一时刻目标纵向位移和下一时刻目标横向位移构成；
按照如下公式(13)对第一评估位移进行补偿估计，得到目标的最优位置估计:
Figure BDA0003605779300000131
其中，x补偿后的纵向位移，y表示补偿后的横向位移，θyawrate为横摆角速率；最优位置估计由补偿后的纵向位移和补偿后的横向位移构成。
按照如下公式(14)将目标相对于自车的速度转化为目标相对于地的绝对径向速度：
Vobj＝Vvehicle cos(theta)+Vradar (14)
其中,Vvehicle为自车当前时刻速度，theta为目标相对自车的方位角，Vradar为雷达探测到的目标速度，Vobj为目标相对于地的绝对径向速度；
依据目标相对于地的绝对径向速度Vobj，利用目标相对于自车的方位角，将其分解到车辆直角坐标系下(车辆直角坐标系平行于大地)，获取得到目标纵向速度与目标横向速度，具体计算如下公式(15)：
Vx＝Vobj cos(theta)
Vy＝Vobj sin(theta) (15)
其中,Vx为目标纵向速度，Vy为目标横向速度；所述第一评估速度包括所述目标纵向速度与所述目标横向速度；
利用车辆横摆角速率θyawrate在单位时间产生的角度变化量，对所述第一评估速度进行目标速度补偿得到下一时刻的最优速度估计：
Figure BDA0003605779300000132
其中，最优速度估计包括补偿后的vx'目标纵向速度和补偿后的vy'目标横向速度，
其中横摆角速率θyawrate按车辆转向方向有正负，当左转向时横摆角速率为正，右转向时横摆角速率为负。依据推理进行速度补偿估计，实现目标状态量补偿最优估计计算。
S604、将目标的最优位置估计和最优速度估计输入ADAS系统进行碰撞预警，以辅助驾驶。
具体请参见上述实施例一中的描述，在此不再赘述。
实施例三：
本实施例在上述实施例一/实施例二的基础上，提供一种车载终端，请参见图7，该车载终端包括处理器71、存储器72及通信总线73；
通信总线73用于实现处理器71和存储器72之间的连接通信；
处理器71用于执行存储器72中存储的一个或者多个程序，以实现如实施例二中所述的基于寻优跟踪算法的辅助驾驶方法的步骤。
具体请参见上述实施例二中的描述，在此不再赘述。
实施例四：
本实施例在上述实施例一/实施例二的基础上，提供一种计算机可读存储介质，该计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如实施例二中所述的基于寻优跟踪算法的辅助驾驶方法的步骤。具体请参见上述实施例二中的描述，在此不再赘述。
显然，本领域的技术人员应该明白，上述本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在计算机存储介质(ROM/RAM、磁碟、光盘)中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。所以，本发明不限制于任何特定的硬件和软件结合。
以上内容是结合具体的实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims (10)

1.一种基于寻优跟踪算法的辅助驾驶系统，其特征在于，包括车辆行驶数据采集模块、雷达点迹数据采集模块、多目标跟踪算法模块；所述车辆行驶数据采集模块用于采集车辆行驶数据，所述车辆行驶数据包括自车速度、自车位移、横摆角速率；所述雷达点迹数据采集模块用于采集目标的状态数据，所述目标的状态数据包括目标相对于自车的方位角、距离、位移、速度、加速度；所述多目标跟踪算法模块用于基于当前时刻的所述车辆行驶数据和所述目标的状态数据，对所述目标在下一时刻的位置和速度进行最优状态估计，得到目标的最优位置估计和最优速度估计；将目标的最优位置估计和最优速度估计输入ADAS系统进行碰撞预警，以辅助驾驶。
2.如权利要求1所述的基于寻优跟踪算法的辅助驾驶系统，其特征在于，所述多目标跟踪算法模块用于基于当前时刻目标相对于自车的位移、速度和加速度，计算得到第一评估位移，利用当前时刻自车的横摆角速率对所述第一评估位移进行补偿估计，得到所述目标的最优位置估计。
3.如权利要求2所述的基于寻优跟踪算法的辅助驾驶系统，其特征在于，所述多目标跟踪算法模块用于基于当前时刻目标相对于自车的位移、速度和加速度，计算得到第一评估位移包括：
按照如下公式(1)-(2)计算得到下一时刻的第一评估位移：
xk＝xk-1+vx*t+0.5*ax*t2 (1)
其中，xk为下一时刻目标纵向位移，xk-1为当前时刻目标纵向位移，vx为当前时刻目标纵向速度，ax为当前时刻目标纵向加速度，t为单位时间；
yk＝yk-1+vy*t+0.5*ay*t2 (2)
其中,yk为下一时刻目标横向位移，yk-1为当前时刻目标横向位移，vy为当前时刻目标横向速度，ay为当前时刻目标横向加速度；所述第一评估位移由所述下一时刻目标纵向位移和所述下一时刻目标横向位移构成；
所述多目标跟踪算法模块用于利用当前时刻自车的横摆角速率对所述第一评估位移进行补偿估计，得到所述目标的最优位置估计包括：
按照如下公式(3)对所述第一评估位移进行补偿估计，得到所述目标的最优位置估计:
Figure FDA0003605779290000021
其中，x补偿后的纵向位移，y表示补偿后的横向位移，θyawrate为横摆角速率；所述最优位置估计由所述补偿后的纵向位移和补偿后的横向位移构成。
4.如权利要求1所述的基于寻优跟踪算法的辅助驾驶系统，其特征在于，所述多目标跟踪算法模块用于基于目标相对于自车的方位角、速度、以及自车速度计算得到第一评估速度，利用当前时刻自车的横摆角速率对所述第一评估速度进行补偿估计，得到所述目标的最优速度估计。
5.如权利要求4所述的基于寻优跟踪算法的辅助驾驶系统，其特征在于，所述多目标跟踪算法模块用于基于目标相对于自车的方位角、速度、以及自车速度计算得到第一评估速度包括：
按照如下公式(4)将所述目标相对于自车的速度转化为目标相对于地的绝对径向速度：
Vobj＝Vvehiclecos(theta)+Vradar (4)
其中,Vvehicle为自车当前时刻速度，theta为目标相对自车的方位角，Vradar为雷达探测到的目标速度，Vobj为目标相对于地的绝对径向速度；
依据目标相对于地的绝对径向速度Vobj，利用目标相对于自车的方位角，将其分解到车辆直角坐标系下，获取得到目标纵向速度与目标横向速度，具体计算如下公式(5)：
Vx＝Vobjcos(theta)
Vy＝Vobjsin(theta) (5)
其中,Vx为目标纵向速度，Vy为目标横向速度；所述第一评估速度包括所述目标纵向速度与所述目标横向速度；
利用车辆横摆角速率θyawrate在单位时间产生的角度变化量，对所述第一评估速度进行目标速度补偿得到下一时刻的最优速度估计：
Figure FDA0003605779290000022
其中，所述最优速度估计包括补偿后的vx'目标纵向速度和补偿后的vy'目标横向速度。
6.如权利要求1-5任一项所述的基于寻优跟踪算法的辅助驾驶系统，其特征在于，所述多目标跟踪算法模块还用于在将目标的最优位置估计和最优速度估计输入ADAS系统之前，根据目标相对于自车的距离和速度，确定该目标是否满足筛选条件，如是，将该目标的最优位置估计和最优速度估计输入ADAS系统进行碰撞预警。
7.如权利要求6所述的基于寻优跟踪算法的辅助驾驶系统，其特征在于，所述辅助驾驶系统还包括多目标跟踪可视化模块、跟踪算法耗时评估模块和危险告警装置；所述多目标跟踪可视化模块用于对目标的状态数据进行图像化展示；所述跟踪算法耗时评估模块用于统计及时跟踪算法所耗费时间，以用于判定系统的实时性；所述危险告警装置用于接收ADAS系统输出的危险告警信号，针对危险目标进行告警。
8.一种基于寻优跟踪算法的辅助驾驶方法，其特征在于，包括：
采集车辆行驶数据，所述车辆行驶数据包括自车速度、自车位移、横摆角速率；
采集目标的状态数据，所述目标的状态数据包括目标相对于自车的方位角、距离、位移、速度、加速度；
基于当前时刻的所述车辆行驶数据和所述目标的状态数据，对所述目标在下一时刻的位置和速度进行最优状态估计，得到目标的最优位置估计和最优速度估计；
将目标的最优位置估计和最优速度估计输入ADAS系统进行碰撞预警，以辅助驾驶。
9.一种车载终端，其特征在于，所述车载终端包括处理器、存储器及通信总线；
所述通信总线用于实现处理器和存储器之间的连接通信；
所述处理器用于执行存储器中存储的一个或者多个程序，以实现如权利要求8中所述的基于寻优跟踪算法的辅助驾驶方法的步骤。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如权利要求8中任一项所述的基于寻优跟踪算法的辅助驾驶方法的步骤。
CN202210415589.6A 2022-04-20 2022-04-20 基于寻优跟踪算法的辅助驾驶系统、方法、终端及介质 Pending CN114670851A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210415589.6A CN114670851A (zh) 2022-04-20 2022-04-20 基于寻优跟踪算法的辅助驾驶系统、方法、终端及介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210415589.6A CN114670851A (zh) 2022-04-20 2022-04-20 基于寻优跟踪算法的辅助驾驶系统、方法、终端及介质

Publications (1)

Publication Number Publication Date
CN114670851A true CN114670851A (zh) 2022-06-28

Family

ID=82078665

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210415589.6A Pending CN114670851A (zh) 2022-04-20 2022-04-20 基于寻优跟踪算法的辅助驾驶系统、方法、终端及介质

Country Status (1)

Country Link
CN (1) CN114670851A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114994633A (zh) * 2022-08-04 2022-09-02 四川九洲空管科技有限责任公司 一种用于二次雷达点航迹相关处理的方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114994633A (zh) * 2022-08-04 2022-09-02 四川九洲空管科技有限责任公司 一种用于二次雷达点航迹相关处理的方法
CN114994633B (zh) * 2022-08-04 2022-11-08 四川九洲空管科技有限责任公司 一种用于二次雷达点航迹相关处理的方法

Similar Documents

Publication Publication Date Title
US11630197B2 (en) Determining a motion state of a target object
Lee et al. Moving object detection and tracking based on interaction of static obstacle map and geometric model-free approachfor urban autonomous driving
Cosgun et al. Towards full automated drive in urban environments: A demonstration in gomentum station, california
US10035508B2 (en) Device for signalling objects to a navigation module of a vehicle equipped with this device
CN113228040B (zh) 多级对象行进方向估计的系统和方法
US11767010B2 (en) Driving assist system
CN110632617B (zh) 一种激光雷达点云数据处理的方法及装置
US10845813B2 (en) Route setting method and route setting device
CN113165670A (zh) 一种智能驾驶方法、装置、存储介质及计算机程序
CN113715814A (zh) 碰撞检测方法、装置、电子设备、介质及自动驾驶车辆
KR20210038852A (ko) 조기 경보 방법, 장치, 전자 기기, 컴퓨터 판독 가능 저장 매체 및 컴퓨터 프로그램
US11685379B2 (en) Vehicle control device and storage medium storing computer program for vehicle control
Pietzsch et al. Results of a precrash application based on laser scanner and short-range radars
Lee et al. Moving objects tracking based on geometric model-free approach with particle filter using automotive LiDAR
Kim et al. Multi-sensor-based detection and tracking of moving objects for relative position estimation in autonomous driving conditions
Lindenmaier et al. GM-PHD filter based sensor data fusion for automotive frontal perception system
US12097900B2 (en) Vehicle control device, vehicle control method, and non-transitory computer-readable recording medium recording program
US11402487B2 (en) Joint radon transform association
KR20200133122A (ko) 차량 충돌 방지 장치 및 방법
CN114670851A (zh) 基于寻优跟踪算法的辅助驾驶系统、方法、终端及介质
CN116994436B (zh) 一种智慧矿山道路碰撞预警方法
CN118033622A (zh) 目标跟踪方法、装置、设备及计算机可读存储介质
CN113176562A (zh) 多目标跟踪方法、装置、电子设备及可读存储介质
Liu et al. A detachable and expansible multisensor data fusion model for perception in level 3 autonomous driving system
Li et al. An overview of factors influencing the mass adoption of self-driving vehicles

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
CB03 Change of inventor or designer information
CB03 Change of inventor or designer information

Inventor after: Zheng Le

Inventor after: Bai Lin

Inventor after: Liu Tianxiao

Inventor after: Zhang Liang

Inventor after: Wang Yanhua

Inventor before: Bai Lin

Inventor before: Zhang Liang

Inventor before: Wang Yanhua