CN115060280A - 一种车辆轨迹预测方法和装置 - Google Patents

一种车辆轨迹预测方法和装置 Download PDF

Info

Publication number
CN115060280A
CN115060280A CN202210705025.6A CN202210705025A CN115060280A CN 115060280 A CN115060280 A CN 115060280A CN 202210705025 A CN202210705025 A CN 202210705025A CN 115060280 A CN115060280 A CN 115060280A
Authority
CN
China
Prior art keywords
track
historical
point
vehicle
obstacle vehicle
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.)
Granted
Application number
CN202210705025.6A
Other languages
English (en)
Other versions
CN115060280B (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.)
Wuxi Institute Of Intelligent Control Hunan University
Original Assignee
Wuxi Institute Of Intelligent Control Hunan 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 Wuxi Institute Of Intelligent Control Hunan University filed Critical Wuxi Institute Of Intelligent Control Hunan University
Priority to CN202210705025.6A priority Critical patent/CN115060280B/zh
Publication of CN115060280A publication Critical patent/CN115060280A/zh
Application granted granted Critical
Publication of CN115060280B publication Critical patent/CN115060280B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3453—Special cost functions, i.e. other than distance or default speed limit of road segments
    • G01C21/3492—Special cost functions, i.e. other than distance or default speed limit of road segments employing speed data or traffic data, e.g. real-time or historical
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
    • G06V20/58—Recognition of moving objects or obstacles, e.g. vehicles or pedestrians; Recognition of traffic objects, e.g. traffic signs, traffic lights or roads
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Multimedia (AREA)
  • Theoretical Computer Science (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

本发明公开了一种车辆轨迹预测方法和装置，其包括：S1，获取障碍物车辆的实际历史轨迹；S2，根据障碍物车辆的预测位置和当前实际位置，计算得到预测位置误差，选择本次预测中所使用的历史轨迹时长；S3，根据实际历史轨迹，获得接近实际历史轨迹的拟合轨迹；S4，根据实际历史轨迹，计算符合障碍物车辆历史运动状态的障碍物车辆状态；S5，使用曲线拟合方程计算获得第一历史轨迹、使用运动学方程计算获得第二历史轨迹，再计算第一历史轨迹、第二历史轨迹分别与实际历史轨迹上对应轨迹点之间的误差；S6，在预设范围内对误差进行比较，选择本帧预测方法，以预测障碍物车辆的未来轨迹。

Description

一种车辆轨迹预测方法和装置
技术领域
本发明涉及自动驾驶技术领域，特别是关于一种车辆轨迹预测方法和装置。
背景技术
自动驾驶技术的应用可显著减少人为因素导致的交通事故的发生，保障交通安全；还可以缓解交通拥堵现象，改善通行效率并减少燃料消耗。因此，自动驾驶技术是近年来学术界和工业界研究的热点。其中，对行驶车辆未来轨迹的可靠预测对自动驾驶决策规划模块生成安全可靠的行为决策和路径规划来说至关重要，准确的车辆轨迹预测将确保自动驾驶车辆沿不碰撞的安全路径行驶。
目前常用的车辆轨迹预测方法存在的缺点较多，比如，有的方法只能对障碍物的换道意图进行了推理，忽视了障碍物在车道内的运动轨迹，降低了轨迹预测的准确性。还有方法需要提供高精度地图和语义地图信息作为输入，训练和实际使用要求条件严格，难以规模应用。也有方法只使用一种模型进行预测，难以覆盖车辆行驶的所有情况。
发明内容
本发明的目的在于提供一种车辆轨迹预测方法和装置，其能够准确地对障碍物车辆的未来轨迹进行预测，而无需大规模数据的训练和高精度地图的采集，便于实现。
为实现上述目的，本发明提供一种车辆轨迹预测方法，其包括：S1，获取障碍物车辆的实际历史轨迹；S2，根据障碍物车辆的预测位置和当前实际位置，计算得到预测位置误差，并依此选择本次预测中所使用的历史轨迹时长；S3，根据所述真实历史轨迹，采用曲线拟合方法获得最接近所述实际历史轨迹的拟合轨迹；S4，根据所述真实历史轨迹，计算符合障碍物车辆历史运动状态的障碍物车辆状态；S5，根据所述拟合轨迹和所述障碍物车辆状态，在所述历史轨迹时长内按预设时间间隔进行采样，并使用曲线拟合方程计算获得第一历史轨迹、以及使用运动学方程计算获得第二历史轨迹，再计算所述第一历史轨迹与所述实际历史轨迹上对应轨迹点之间的第一误差、以及计算所述第二历史轨迹与所述实际历史轨迹上对应轨迹点之间的第二误差；S6，在预设范围内对所述第一误差与第二误差进行比较，并根据比较的结果选择本帧预测方法，在预测时域内按预设采样时间预测障碍物车辆的未来轨迹。
进一步地，所述步骤S2中的“选择本次预测中所使用的历史轨迹时长”的方法具体包括：步骤S21，判断障碍物车辆是否为首次出现，如果是，则进入步骤S22，否则进入步骤23；步骤S22，进入正常时长预测模式，所述历史轨迹时长设置正常观测时长T；步骤23，判断障碍物车辆的上一帧预测误差是否大于设定阈值V，如果是，则进入步骤S24；步骤S24，开始进入短时长预测模式，则所述历史轨迹时长设置为短时域观测时长H，H＜T,且计数器置为1；步骤S25，判断障碍物车辆是否已经进入所述短时长预测模式，如果是，则进入步骤S26；步骤S26，将计数器加1，当所述计数器达到设定阈值E时，退出所述短时长预测模式，并将所述历史轨迹时长恢复为T；其中，所述阈值E的数值设置应能够保证从所述短时域观测时长H增长到所述正常观测时长T。
进一步地，所述步骤S3之前还包括：构建用于考虑轨迹点之间的偏移距离的目标函数、并对所述实际历史轨迹进行优化的步骤；所述实际历史轨迹包括相对历史轨迹和绝对历史轨迹，所述步骤S3采用所述相对历史轨迹，所述步骤S4采用所述绝对历史轨迹；其中，所述目标函数设置为式(10)：
Figure BDA0003704978610000021
其中，xi＝(xi，yi)，i∈1，2，...，n，为所述实际历史轨迹上的原始轨迹点yi被优化后的坐标，Δxi＝xi-xi-1为所述实际历史轨迹上第i个轨迹点xi与第i-1个轨迹点xi-1之间的位移矢量，α，β为权重系数。
进一步地，所述实际历史轨迹上的原始轨迹点yi被优化为xi的方法采用梯度下降法，各项梯度的表达式如下：
Figure BDA0003704978610000022
Figure BDA0003704978610000023
梯度下降法迭代公式如下：
Figure BDA0003704978610000031
式中，η为学习率，f(xi)为所述目标函数，Δxi+1＝xi+1-xi为所述实际历史轨迹上第i+1个轨迹点xi+1与第i个轨迹点xi之间的位移矢量，Δxi+2＝xi+2xi+1为所述实际历史轨迹上第i+2个轨迹点xi+2与第i+1个轨迹点xi+1之间的位移矢量，Δxi-1＝xi-1-xi-2为所述实际历史轨迹上第i-1个轨迹点xi-1与第i-2个轨迹点xi-2之间的位移矢量，Δxi-2＝xi-2-xi-3为所述实际历史轨迹上第i-2个轨迹点xi-2与第i-3个轨迹点xi-3之间的位移矢量。
进一步地，所述相对历史轨迹根据从障碍物车辆的位置相对于自车的相对位置而获得；其中，所述相对历史轨迹上的轨迹点的坐标表示为(xr，yr)，r为所述相对历史轨迹上轨迹点的序号；述绝对历史轨迹根据下式获得，其中，所述绝对历史轨迹上的轨迹点的坐标表示为(xa，ya)，a为所述绝对历史轨迹上轨迹点的序号；
Figure BDA0003704978610000032
式中，(xe，ye)、θ分别为自车的绝对位置信息和朝向信息。
进一步地，所述步骤S6的根据比较的结果选择本帧预测方法的方法具体包括：所述第一误差小于所述第二误差的情形下，选择的本帧预测方法为如式(16)所示的最小二乘曲线拟合预测所述未来轨迹；
Figure BDA0003704978610000033
所述第一误差大于或等于所述第二误差的情形下，选择的本帧预测方法为如式(17)所示的运动学模型预测所述未来轨迹；
Figure BDA0003704978610000034
式中，(xj，yj)为所述未来轨迹上的轨迹点在UTM坐标系下的坐标，j为所述未来轨迹上轨迹点的序号j＝1，2，...m，(x0，y0)为UTM坐标系下障碍物车辆当前所在位置点的坐标，r为所述拟合轨迹的半径，ω为所述障碍物车辆状态中的障碍物车辆的转动速度，Δt为采样时间间隔，θ为障碍物车辆当前所在位置点在所述拟合轨迹上的初始角度，(xn，yn)为障碍物车辆当前所在位置点的坐标，vx、vy分别为障碍物车辆的当前的横向速度和纵向速度，ax、ay分别为障碍物车辆的当前的横向加速度和纵向加速度。
本发明还提供一种车辆轨迹预测装置，其包括：信息获取单元，其用于获取障碍物车辆的实际历史轨迹；观测时长单元，其用于根据障碍物车辆的预测位置和当前实际位置，计算得到预测位置误差，并依此选择本次预测中所使用的历史轨迹时长；曲线拟合单元，其用于根据所述实际历史轨迹，采用曲线拟合方法获得最接近所述实际历史轨迹的拟合轨迹；状态计算单元，其用于根据所述实际历史轨迹，计算符合障碍物车辆历史运动状态的障碍物车辆状态；轨迹预测单元，其用于根据所述拟合轨迹和所述障碍物车辆状态，在所述历史轨迹时长内按预设时间间隔进行采样，并使用曲线拟合方程计算获得第一历史轨迹、以及使用运动学方程计算获得第二历史轨迹，再计算所述第一历史轨迹与所述相对历史轨迹上对应轨迹点之间的第一误差、以及计算所述第二历史轨迹与所述绝对历史轨迹上对应轨迹点之间的第二误差，最后在预设范围内对所述第一误差与第二误差进行比较，并根据比较的结果选择本帧预测方法，在预测时域内按预设采样时间预测障碍物车辆的未来轨迹。
进一步地，所述观测时长单元具体包括：计算误差子单元，其用于根据障碍物车辆的预测位置和当前实际位置，计算得到预测位置误差；轨迹更新子单元，其用于根据所述实际历史轨迹中的时间戳信息和所述历史轨迹时长的变化信息，完成对应障碍物车辆的所述实际历史轨迹的更新。
进一步地，所述的车辆轨迹预测装置还包括：轨迹平滑单元，其用于构建用于考虑轨迹点之间的偏移距离的目标函数，并对所述实际历史轨迹进行平滑优化；所述实际历史轨迹包括相对历史轨迹和绝对历史轨迹，所述曲线拟合单元采用所述相对历史轨迹，所述状态计算单元采用所述绝对历史轨迹；其中，所述目标函数设置为式(10)：
Figure BDA0003704978610000041
其中，所述实际历史轨迹上的原始轨迹点yi被优化为xi的方法采用梯度下降法，各项梯度的表达式如下：
Figure BDA0003704978610000051
Figure BDA0003704978610000052
梯度下降法迭代公式如下：
Figure BDA0003704978610000053
式中，xi＝(xi，yi)，i∈1，2，...，n，为所述实际历史轨迹上的原始轨迹点yi被优化后的坐标，Δxi＝xi-xi-1为所述实际历史轨迹上第i个轨迹点xi与第i-1个轨迹点xi-1之间的位移矢量，α，β为权重系数，η为学习率，f(xi)为所述目标函数，Δxi+1＝xi+1-xi为所述实际历史轨迹上第i+1个轨迹点xi+1与第i个轨迹点xi之间的位移矢量，Δxi+2＝xi+2-xi+1为所述实际历史轨迹上第i+2个轨迹点xi+2与第i+1个轨迹点xi+1之间的位移矢量，Δxi-1＝xi-1-xi-2为所述实际历史轨迹上第i-1个轨迹点xi-1与第i-2个轨迹点xi-2之间的位移矢量，Δxi-2＝xi-2-xi-3为所述实际历史轨迹上第i-2个轨迹点xi-2与第i-3个轨迹点xi-3之间的位移矢量。
进一步地，所述轨迹预测单元具体包括：模型选择子单元，其用于在所述第一误差小于所述第二误差的情形下，选择的本帧预测方法为如式(16)所示的最小二乘曲线拟合预测所述未来轨迹；
Figure BDA0003704978610000054
在所述第一误差大于或等于所述第二误差的情形下，选择的本帧预测方法为如式(17)所示的运动学模型预测所述未来轨迹；
Figure BDA0003704978610000055
式中，(xj，yj)为所述未来轨迹上的轨迹点在UTM坐标系下的坐标，j为所述未来轨迹上轨迹点的序号j＝1，2，...m，(x0，y0)为UTM坐标系下障碍物车辆当前所在位置点的坐标，r为所述拟合轨迹的半径，ω为所述障碍物车辆状态中的障碍物车辆的转动速度，Δt为采样时间间隔，θ为障碍物车辆当前所在位置点在所述拟合轨迹上的初始角度，(xn，yn)为障碍物车辆当前所在位置点的坐标，vx、vy分别为障碍物车辆的当前的横向速度和纵向速度，ax、ay分别为障碍物车辆的当前的横向加速度和纵向加速度；轨迹输出子单元，其用于根据选择的预测模型，在预测时域内进行轨迹点计算，将预测轨迹与上帧轨迹加权平均后输出。
本发明由于采取以上技术方案，其具有以下优点：
1、本发明技术方案需要的外界信息仅仅来自于步骤S1获取的障碍物车辆的实际历史轨迹，而无需再提供其他额外信息，算法轻便，适合应用在多种场景下。
2、同时使用曲线拟合方程和运动学方程来计算历史轨迹并将之与实际历史轨迹进行对比，并基于对比误差来选择确定当前帧的轨迹预测方法。由此提高了预测精度。具体地，现有的车辆轨迹预测方法往往是一种单模态的预测方法，即只考虑一种运动模式，难以覆盖所有的运动情况。本发明提出一种运动学模型和曲线拟合结合的多模态车辆轨迹预测方法，实时计算两种方法对车辆历史轨迹的预测情况与误差情况，根据误差选择合适的模型进行输出，当第二误差小于第一误差时，判定车辆处于小曲率运动模态，使用运动学模型进行预测，当第二误差大于第一误差时，判定车辆处于大曲率运动模态，使用曲线拟合模型进行预测。从而，综合实现了两种预测方法的优点。
3、本发明技术方案可以在无需大规模数据训练的前提下使用，使用时也无需高精度地图提供辅助信息。
附图说明
图1是本发明实施例采用的最小二乘曲线拟合示意图。
图2是本发明实施例采用的车辆运动学模型示意图。
图3是本发明实施例提出的轨迹预测模型选择示意图。
图4是大曲率场景预测实验图。
图5是小曲率场景预测实验图。
图6是采用本发明方法与现有的方法预测误差对比图。
具体实施方式
在附图中，使用相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面结合附图对本发明的实施例进行详细说明。
在本发明的描述中，术语“中心”、“纵向”、“横向”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明保护范围的限制。
在不冲突的情况下，本发明各实施例及各实施方式中的技术特征可以相互组合，并不局限于该技术特征所在的实施例或实施方式中。
下面结合附图以及具体实施例对本发明做进一步的说明，需要指出的是，下面仅以一种最优化的技术方案对本发明的技术方案以及设计原理进行详细阐述，但本发明的保护范围并不仅限于此。
本文涉及下列术语，为便于理解，对其含义说明如下。本领域技术人员应当理解，下列术语也可能有其它名称，但在不脱离其含义的情形下，其它任何名称都应当被认为与本文所列术语一致。
“自车”为装备了本发明提供的轨迹预测装置的自动驾驶汽车。
“障碍物车辆”为自车环境中需要对其运动轨迹进行预测的对自车车辆行驶安全有影响的其他车辆。
本发明实施例提供的车辆轨迹预测方法包括：
S1，获取障碍物车辆的实际历史轨迹。所述实际历史轨迹分为相对历史轨迹和绝对历史轨迹。
S2，根据储存的障碍物车辆的预测位置和当前实际位置，计算得到预测位置误差，并依此选择本次预测中所使用的历史轨迹时长。
在一个实施例中，预测位置误差e可以通过如下公式计算获得，误差计算公式如下式(1)所示，(xi，yi)为障碍物车辆在UTM(Universal Transverse Mercator，通用横轴墨卡托)坐标系下表示的当前实际位置，i为相应轨迹点的序号，储存的障碍物车辆在UTM坐标系下表示的预测位置(xj，yj)，j为相应轨迹点的序号，并依此选择本次预测中所使用的历史轨迹时长。
Figure BDA0003704978610000071
在一个实施例中，“选择本次预测中所使用的历史轨迹时长”的方法包括：
步骤S21，判断障碍物车辆是否为首次出现，如果是，则进入步骤S22，否则进入步骤23。
步骤S22，进入正常时长预测模式，并将所述历史轨迹时长设置正常观测时长T。
步骤23，判断障碍物车辆的上一帧预测误差是否大于设定阈值V，如果是，则进入步骤S24。根据经验以及目前短时域轨迹预测方法的一般性能，该误差阈值V一般设置在3米到5米之间，说明T时长内的历史轨迹已经不能正确反映障碍物车辆的实际历史轨迹，因此需要对预测所使用的轨迹时长进行缩短，则进入短时长预测模式。
步骤S24，开始进入短时长预测模式，并将所述历史轨迹时长设置为短时域观测时长H，H＜T,且计数器置为1。H一般设置为正常观测时长的一半，即
Figure BDA0003704978610000081
步骤S25，判断障碍物车辆是否已经进入所述短时长预测模式，如果是，则进入步骤S26。
步骤S26，将计数器加1，当所述计数器达到设定阈值E时，说明已在短时长预测模式下经过了足够时间，目前观测时长T内的历史轨迹能够反映障碍物车辆新的运动趋势，因此退出所述短时长预测模式，并将所述历史轨迹时长恢复为T。其中，所述阈值E的数值设置应能够保证从所述短时域观测时长H增长到所述正常观测时长T。通过及时调整合适的历史轨迹时长，使得对障碍物车辆运动状态的计算更加准确，增加预测结果的准确性。
根据储存预测轨迹中的时间戳信息和步骤S2中的预测选用时长信息对储存轨迹信息进行更新，若该障碍物车辆不存在于储存预测轨迹中，则说明其首次出现，则对该障碍物车辆的本帧预测轨迹进行储存；若当前障碍物车辆在储存预测轨迹中的时间戳已到达预测时域值，则已完成整个预测时域内的误差计算，因此将该障碍物车辆的储存预测轨迹更新为当前帧对其的预测轨迹，供下一个预测时域计算预测误差。若当前障碍物的预测选用时长发生了变化，则说明先前储存的预测轨迹已不能正确反映当前本方法对障碍物进行的预测了，因此将该障碍物车辆的储存预测轨迹更新为当前帧对其的预测轨迹。目前常用的轨迹预测方法往往不会实时对预测误差进行计算和记录，即使记录也仅供使用结束后的分析改进用。而本实施例提出的预测误差计算和预测选用时长实时变化方法，将实时观测预测误差情况并实时改变预测选用时长以适应预测效果的变化，在动态环境下拥有更好的表现。
当然，本领域技术人员也可以选择现有技术中实现“选择本次预测中所使用的历史轨迹时长”的其它方法。
S3，根据储存过往时刻上游感知模块发出的障碍物车辆相对自车位置形成的所述实际历史轨迹，采用曲线拟合方法获得最接近所述实际历史轨迹的拟合轨迹。
上述实施例中，使用最小二乘法拟合最接近所述实际历史轨迹的拟合轨迹，其呈现为曲线方程，可以选择拟合圆形轨迹或者各阶多项式轨迹，本实施例中以在二维坐标系中拟合圆形轨迹为例，拟合过程如下：
(x-x0)2+(y-y0)2＝r2 (2)
其中，(x0，y0)为圆心坐标，r为圆半径。对于最小二乘的圆拟合，其误差平方的优化目标函数(3)表示为：
Figure BDA0003704978610000091
其中，(xi，yi)，i＝1，2，...，n为用于拟合的障碍物车辆相对实际历史轨迹中的点，n为轨迹点的个数，对目标函数(3)求偏导，即可得到使目标函数最小的最优拟合圆心和半径，即得到了拟合圆方程，如图1所示。
此时得到的仅是路径方程，而无时间关系，因此在车辆极限转动速度[-W,W]之间采样，计算各转动速度下圆形轨迹与实际历史轨迹间的误差，取最小者为障碍物车辆转动速度，于是，可以获得基于最小二乘圆拟合的障碍物车辆轨迹方程。
当然，上述实施例中所使用的最小二乘曲线拟合方法拟合的是圆曲线，本领域技术人员也可以拟合为二次曲线、三次多项式曲线、四次多项式曲线或者五次多项式曲线等。
S4，根据所述实际历史轨迹，计算符合障碍物车辆历史运动状态的障碍物车辆状态。
其中，障碍物车辆状态包括符合障碍物车辆历史运动状态的速度和加速度信息，包括UTM坐标系的x方向和y方向上的速度vx、vy和加速度ax、ay，如图2所示。为减少感知误差带来的不确定性，使用实际历史轨迹中的k个点，通过下式(4)和式(5)计算障碍物车辆的当前速度vx、vy；使用2k个点，通过下式(6)和式(7)来计算其加速度ax、ay，这样有利于提升计算出运动状态的稳定性，如下：
Figure BDA0003704978610000092
Figure BDA0003704978610000101
Figure BDA0003704978610000102
Figure BDA0003704978610000103
其中，(xn，yn)为实际历史轨迹中最后一个轨迹点，即障碍物车辆当前所在位置点的坐标，Δt为采样时间间隔，(xn-k，yn-k)为k个时刻前的障碍物车辆所在位置点的坐标，(xn-2k，yn-2k)为2k个时刻前的障碍物车辆所在位置点的坐标。
S5，根据所述拟合轨迹和所述障碍物车辆状态，在所述历史轨迹时长内按预设时间间隔Δt进行采样，并使用曲线拟合方程(8)计算获得第一历史轨迹、以及使用运动学方程(9)计算获得第二历史轨迹，再计算所述第一历史轨迹与所述实际历史轨迹上对应轨迹点之间的第一误差、以及计算所述第二历史轨迹与所述实际历史轨迹上对应轨迹点之间的第二误差：
Figure BDA0003704978610000104
Figure BDA0003704978610000105
其中，所述第一历史轨迹上的轨迹点在UTM坐标系下的坐标表示为
Figure BDA0003704978610000106
所述第二历史轨迹上的轨迹点在UTM坐标系下的坐标表示为
Figure BDA0003704978610000107
(x0，y0)为UTM坐标系下障碍物车辆当前所在位置点的坐标，r为所述拟合轨迹的半径，ω为障碍物车辆状态中的障碍物车辆的转动速度，θ为障碍物车辆当前所在位置点在所述拟合轨迹上的初始角度，vx、vy为基于运动学模型所预测得到UTM坐标系下障碍物车辆的预测横、纵向速度，ax、ay为基于运动学模型所预测得到UTM坐标系下障碍物车辆的预测横、纵向加速度。
S6，在预设范围U内对所述第一误差与第二误差进行比较，并根据比较的结果选择本帧预测方法获取在预测时域Tp内按预设采样时间Δt对障碍物车辆的未来轨迹进行预测。其中，所述未来轨迹上的轨迹点在UTM坐标系下的坐标表示为(xj，yj)，j为所述未来轨迹上轨迹点的序号j＝1，2，...m。U的数值选取标准基于所述未来轨迹的长度，一般在5m-15m之间。
在一个实施例中，所述步骤S3之前还包括：构建用于考虑轨迹点之间的偏移距离的目标函数、并对所述实际历史轨迹进行优化的步骤。
所述实际历史轨迹包括相对历史轨迹和绝对历史轨迹，对于曲线拟合方法来说，绝对坐标的数值太大，无法直接拟合，必须使用相对坐标；而对于运动学模型来说，相对坐标中蕴含了自车的运动趋势而不仅仅使障碍物车辆的运动趋势，因此要使用绝对坐标，从而更精确的计算出障碍物车辆的运动状态。因此，所述步骤S3采用所述相对历史轨迹，所述步骤S4采用所述绝对历史轨迹。
其中，目标函数的构建形式可以如式(10)所示：
Figure BDA0003704978610000111
式中，xi＝(xi，yi)，i∈1，2，...，n，为所述步骤S1获得的所述实际历史轨迹优化后的坐标，Δxi＝xi-xi-1为所述步骤S1中获得的实际历史轨迹中各轨迹点之间的位移矢量，yi为所述步骤S1中获得的实际历史轨迹上的原始轨迹点坐标，原始轨迹点可以理解为有感知模块或车载传感器直接采集而未作任何处理的数据，α，β为各项权重系数。
需要说明的是，目标函数(10)的第一项是为了尽量减少平滑后的轨迹与原轨迹的偏移，第二项则使轨迹尽量平滑，由于偏移项
Figure BDA0003704978610000112
比平滑项
Figure BDA0003704978610000113
的量纲大一个级别，因此为平衡二者在优化时的影响，一种合理的方案是将α设为0.1，β设为1，可选的，为满足不同行驶场景下的环境要求，可以调整权重的设置以达到不同的优化效果。
目标函数也可以是如式(11)所示，由以下三项组成，分别是偏移项、平滑项和曲率项，这三项可以任意删减组合，设置不同的权重组成不同的目标函数(11)。γ为曲率项权重系数，
Figure BDA0003704978610000114
为各轨迹点之间切向角的变化量。
Figure BDA0003704978610000115
在一个实施例中，使用梯度下降法对最优化问题进行求解，各项梯度的表达式如下：
Figure BDA0003704978610000116
Figure BDA0003704978610000121
梯度下降法迭代公式如下式(14)得到目标函数各项的梯度后，即可使用梯度下降法进行迭代优化，对历史轨迹进行平滑优化，消除感知误差带来的扰动，使历史轨迹更加符合障碍物车辆的真实运动情况。
Figure BDA0003704978610000122
式中，η为学习率，为一大于0的数，一般设为0.1到0.2之间；f(xi)即为目标函数(10)或(11)，Δxi＝xi-xi-1为所述实际历史轨迹上第i个轨迹点xi与第i-1个轨迹点xi-1之间的位移矢量，Δxi+1＝xi+1-xi为所述实际历史轨迹上第i+1个轨迹点xi+1与第i个轨迹点xi之间的位移矢量，Δxi+2＝xi+2-xi+1为所述实际历史轨迹上第i+2个轨迹点xi+2与第i+1个轨迹点xi+1之间的位移矢量，Δxi-1＝xi-1-xi-2为所述实际历史轨迹上第i-1个轨迹点xi-1与第i-2个轨迹点xi-2之间的位移矢量，Δxi-2＝xi-2-xi-3为所述实际历史轨迹上第i-2个轨迹点xi-2与第i-3个轨迹点xi-3之间的位移矢量。
也可以使用使用二次规划方法或现有的其他方法进行优化，从而对障碍物车辆的历史绝对位置组成的历史轨迹进行平滑处理，消除感知误差带来的扰动。
在一个实施例中，所述相对历史轨迹根据从上游感知模块获得的障碍物位置坐标(xr，yr)相对于自车的相对位置而获得。其中，所述相对历史轨迹上的轨迹点的坐标表示为(xr，yr)，r为所述相对历史轨迹上轨迹点的序号。
所述绝对历史轨迹根据下式(15)获得，其中，所述绝对历史轨迹上的轨迹点的坐标表示为(xa，ya)，a为所述绝对历史轨迹上轨迹点的序号。
Figure BDA0003704978610000123
式中，(xe，ye)、θ分别为自车绝对位置信息和朝向信息。
实际上，从上游感知模块获得的障碍物位置坐标(xr，yr)是相对于自车的相对位置，对该坐标进行储存得到的就是相对历史轨迹，而对相对坐标(xr，yr)按上式进行坐标转换就可以得到UTM坐标系下的绝对坐标(xa，ya)，对绝对坐标进行储存得到的就是绝对历史轨迹。
需要说明的是，所述相对历史轨迹上的轨迹点(xr，yr)和所述绝对历史轨迹上的轨迹点的坐标(xa，ya)，均可以理解为优化后的坐标。
在一个实施例中，步骤S6的根据比较的结果选择本帧预测方法的方法具体包括：
所述第一误差小于所述第二误差的情形下，选择的本帧预测方法为如式(16)所示的最小二乘曲线拟合预测所述未来轨迹；
Figure BDA0003704978610000131
所述第一误差大于或等于所述第二误差的情形下，选择的本帧预测方法为如式(17)所示的运动学模型预测所述未来轨迹；
Figure BDA0003704978610000132
式中，(xj，yj)为所述未来轨迹上的轨迹点在UTM坐标系下的坐标，j为所述未来轨迹上轨迹点的序号j＝1，2，...m，(x0，y0)为UTM坐标系下障碍物车辆当前所在位置点的坐标，r为所述拟合轨迹的半径，ω为所述障碍物车辆状态中的障碍物车辆的转动速度，Δt为采样时间间隔，(xe，ye)、θ分别为通过上游定位模块获得的自车的绝对位置信息和障碍物车辆当前所在位置点在所述拟合轨迹上的初始角度，(xn，yn)为障碍物车辆当前所在位置点的坐标vx、vy分别为障碍物车辆的当前的横向速度和纵向速度，ax、ay分别为障碍物车辆的当前的横向加速度和纵向加速度。
由于曲线拟合是由最小二乘方法得到的，拟合误差很小，因此需要计算第一历史轨迹与实际历史轨迹之间的第一误差来判断是否需要将预测结果切换成曲线拟合的结果。而运动学模型得到的轨迹点虽然与实际历史轨迹点间的第二误差更大，但在预测直线或小曲率运动的障碍物车辆时表现更好，因此当运动学模型误差在一定阈值内不大于最小二乘曲线拟合误差时，本帧预测方法都选用运动学模型，否则选用最小二乘曲线拟合，如图3所示，图3中A点到B点之间的曲线为实际历史轨迹，B点到C点之间的曲线表示式(16)对应的轨迹，B点到D点之间的曲线表示式(17)对应的轨迹。也就是说，本发明实施例在障碍物车辆直线行驶时使用运动学模型进行预测，而在障碍物车辆做大曲率转向行驶时采用曲线拟合预测，这样根据工况有区别地选择预测方法，可以提高轨迹预测的准确性。
由于本实施例提出的方法使用障碍物车辆历史轨迹来计算运动状态继而预测轨迹，而障碍物历史位置由上游感知模块提供并且存在跳动误差，因此预测轨迹易随之产生帧间跳动现象。为提供更可靠的预测轨迹，根据计算出的预测轨迹，与上一帧预测的结果加权进行输出，提升预测轨迹的稳定性。
需要说明的是，上述实施例中的运动学模型可以替代方案包括恒速模型(Constant Velocity，CV)、恒转动恒加速度模型(Constant Turn Rate andAcceleration，CTRA)、恒转动恒速模型(Constant Turn Rate and Velocity，CTRV)、恒转动模型(Constant Turn Rate，CT)。
本实施例的技术效果是：考虑了感知误差带来的扰动，对障碍物车辆历史轨迹构建最优化问题，使用梯度下降法进行平滑处理，使历史轨迹更接近真实情况；此外，本实施例构建了基于运动学模型的轨迹预测方法，使预测轨迹符合障碍物车辆历史运动趋势，对于运动学模型预测效果差的大曲率转向情况，构建了基于最小二乘曲线拟合的轨迹预测方法，弥补了运动学模型预测的短板，以轻量级化的算法，在仅需自车位姿信息和障碍物车辆位置信息的条件下，实现了对障碍物车辆未来轨迹的精确可靠预测。
本发明实施例还提供一种车辆轨迹预测装置，其可以执行本发明实施例所提供的车辆轨迹预测方法，具备执行该方法相应的功能模块和有益效果。
具体的，该车辆轨迹预测装置以单元的形式表现，可以包括但不限于：信息获取单元、观测时长单元、曲线拟合单元、状态计算单元和轨迹预测单元，其中：
信息获取单元用于获取自动驾驶上游模块发出的定位、感知信息，其用于获取障碍物车辆的实际历史轨迹。
在一个实施例中，信息获取单元具体包括：定位获取子单元、感知获取子单元和坐标转换子单元，其中：
定位获取子单元用于获取自动驾驶定位模块发出的自车绝对位置信息和朝向信息。
感知获取子单元用于获取自动驾驶感知模块发出的障碍物车辆对于自车的相对位置信息信息。
坐标转换子单元用于完成障碍物车辆绝对位置的转换计算和对应历史轨迹的维护更新。其中，所述相对历史轨迹根据从上游感知模块获得的障碍物位置坐标(xr，yr)相对于自车的相对位置而获得；其中，所述相对历史轨迹上的轨迹点的坐标表示为(xr，yr)，r为所述相对历史轨迹上轨迹点的序号；
所述绝对历史轨迹根据上式(15)获得，其中，所述绝对历史轨迹上的轨迹点的坐标表示为(xa，ya)，a为所述绝对历史轨迹上轨迹点的序号。
观测时长单元用于根据障碍物车辆的预测位置和当前实际位置，计算得到预测位置误差，并依此选择本次预测中所使用的历史轨迹时长，及时适应障碍物车辆运动趋势的变化。
曲线拟合单元用于根据所述实际历史轨迹中的相对历史轨迹，采用曲线拟合方法获得最接近所述实际历史轨迹的拟合轨迹。
状态计算单元用于根据所述实际历史轨迹中的绝对历史轨迹，计算符合障碍物车辆历史运动状态的障碍物车辆状态，障碍物车辆状态包括速度、加速度和转动角速度等。
轨迹预测单元用于根据所述拟合轨迹和所述障碍物车辆状态，在所述历史轨迹时长内按预设时间间隔进行采样，并使用曲线拟合方程计算获得第一历史轨迹、以及使用运动学方程计算获得第二历史轨迹，再计算所述第一历史轨迹与所述相对历史轨迹上对应轨迹点之间的第一误差、以及计算所述第二历史轨迹与所述绝对历史轨迹上对应轨迹点之间的第二误差，最后在预设范围内对所述第一误差与第二误差进行比较，并根据比较的结果选择本帧预测方法，在预测时域内按预设采样时间预测障碍物车辆的未来轨迹。
在一个实施例中，轨迹预测单元具体包括模型选择子单元和轨迹输出子单元，其中：
模型选择子单元用于用于在所述第一误差小于所述第二误差的情形下，选择的本帧预测方法为如上式(16)所示的最小二乘曲线拟合预测所述未来轨迹。在所述第一误差大于或等于所述第二误差的情形下，选择的本帧预测方法为如上式(17)所示的运动学模型预测所述未来轨迹。
轨迹输出子单元用于根据选择的预测模型，在预测时域内进行轨迹点计算，将预测轨迹与上帧轨迹加权平均后输出。
观测时长单元用于根据障碍物车辆位置信息和历史预测轨迹信息，计算障碍物车辆实际位置与预测位置之间的误差，并对用来计算误差的历史预测轨迹进行更新。
在一个实施例中，观测时长单元具体包括计算误差子单元和轨迹更新子单元，其中：
计算误差子单元用于根据障碍物当前位置、储存的历史轨迹和对应的时间戳，计算预测轨迹与真实轨迹之间的误差。
轨迹更新子单元根据储存历史轨迹中的时间戳信息和历史轨迹时长变化信息，完成对应障碍物车辆的储存历史轨迹的更新。
在一个实施例中，所述车辆轨迹预测装置还包括轨迹平滑单元，其用于根据障碍物车辆历史轨迹信息，构建目标函数对历史轨迹进行平滑处理，可以使用梯度下降法进行迭代，或者使用二次规划方法、牛顿法、共轭梯度法、拟牛顿法或内点法进行求解，用于消除感知误差带来的扰动。
具体实施时，以上各个单元可以作为独立的实体来实现，也可以进行任意组合，作为同一或若干实体来实现，以上各个单元的具体实施可参见前面的方法实施例，在此不再赘述。
由上可知，本实施例提供的车辆轨迹预测装置由信息获取单元获得障碍物车辆的历史轨迹；由轨迹预测单元使用最小二乘曲线拟合和恒加速度运动学两种模型来进行轨迹预测；由误差计算单元根据障碍物车辆的当前位置和储存历史轨迹来计算预测误差。
上述实施例的模型组合方法结合了运动学模型和曲线拟合二者的优势，提高了预测精度。
现有的车辆轨迹预测方法往往是一种单模态的预测方法，即只考虑一种运动模式，难以覆盖所有的运动情况。例如，如果采用单一的恒加速运动模型，在车辆处于大曲率运行的情况下，会产生较大误差。如果采用单一的曲线拟合模型，在车辆处于小曲率运行的情况下，会产生较大误差。
本发明的方法提出一种运动学模型和最小二乘曲线拟合结合的多模态车辆轨迹预测方法，实时计算两种方法对车辆历史轨迹的预测情况及误差情况，根据误差选择合适的模型进行输出。第二误差小于第一误差时，认为车辆处于小曲率运动模态，使用运动学模型进行预测，第二误差大于第一误差时，认为车辆处于大曲率运动模态，使用曲线拟合模型进行预测。从而提高了预测精度。换句话说，本发明弥补了运动学模型预测的短板，以轻量级化的算法，在仅需自车位姿信息和障碍物车辆位置信息的条件下，实现了对障碍物车辆未来轨迹的精确可靠预测。
由于上述实施例可以通过历史轨迹平滑来消除感知误差，还可以使用最小二乘拟合圆形轨迹弥补运动学模型无法预测大曲率运动缺陷，并实时计算预测误差以调整合适的历史轨迹时长。因此，该方案可以在无需大规模数据训练的前提下使用，使用时也无需高精度地图提供辅助信息。
为证明本实施例提出的轨迹预测方法的技术效果，在国内外知名的自动驾驶数据集KITTI数据集(即卡尔斯鲁厄理工学院和丰田美国技术研究院数据集)中进行了实验测试。实验中本实施例提出的轨迹预测方法能在车辆做大曲率运动时做出符合车辆转向趋势的轨迹预测(如图4所示)，也能在车辆做小曲率运动时做出符合车辆直线行驶的轨迹预测(如图5所示)。实验还对比了本实施例提出的轨迹预测方法与现有的曲线拟合和运动学模型两种预测方法的预测误差，见图6，误差数据见表1。由预测误差数据分析可见，两种现有的预测方法中，相较于曲线拟合模型，运动学模型的预测误差整体要更小，但由于无法对大曲率运动做出准确预测，因此在车辆做大曲率运动时出现误差显著增大的尖点，预测效果仍不理想。而本实施例提出的预测方法由于提出了优化历史轨迹以减小感知扰动以及动态变化历史轨迹选用时长以实时反映障碍物实时运动趋势的创新方法，因此平均误差的表现要好于运动学模型；并且本实施例提出的预测方法结合了运动学模型和曲线拟合模型的优点，始终使用两者间表现更好的模型进行预测，因此在车辆做大曲率运动时，也没有出现误差显著增大的尖点。综合而言，本实施例提出的预测方法显著提升了车辆轨迹预测的精度。
表1本实施例方法与常见方法预测误差对比表
本实施例提出的方法 运动学模型 曲线拟合模型
3秒内平均误差 1.7米 2.5米 4.6米
3秒内最大预测误差 2.9米 5.2米 6.1米
3秒末预测误差 2.9米 3.2米 5.9米
最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种车辆轨迹预测方法，其特征在于，包括：
S1，获取障碍物车辆的实际历史轨迹；
S2，根据障碍物车辆的预测位置和当前实际位置，计算得到预测位置误差，并依此选择本次预测中所使用的历史轨迹时长；
S3，根据所述真实历史轨迹，采用曲线拟合方法获得最接近所述实际历史轨迹的拟合轨迹；
S4，根据所述真实历史轨迹，计算符合障碍物车辆历史运动状态的障碍物车辆状态；
S5，根据所述拟合轨迹和所述障碍物车辆状态，在所述历史轨迹时长内按预设时间间隔进行采样，并使用曲线拟合方程计算获得第一历史轨迹、以及使用运动学方程计算获得第二历史轨迹，再计算所述第一历史轨迹与所述实际历史轨迹上对应轨迹点之间的第一误差、以及计算所述第二历史轨迹与所述实际历史轨迹上对应轨迹点之间的第二误差；
S6，在预设范围内对所述第一误差与第二误差进行比较，并根据比较的结果选择本帧预测方法，在预测时域内按预设采样时间预测障碍物车辆的未来轨迹。
2.如权利要求1所述的车辆轨迹预测方法，其特征在于，所述步骤S2中的“选择本次预测中所使用的历史轨迹时长”的方法具体包括：
步骤S21，判断障碍物车辆是否为首次出现，如果是，则进入步骤S22，否则进入步骤23；
步骤S22，进入正常时长预测模式，所述历史轨迹时长设置正常观测时长T；
步骤23，判断障碍物车辆的上一帧预测误差是否大于设定阈值V，如果是，则进入步骤S24；
步骤S24，开始进入短时长预测模式，则所述历史轨迹时长设置为短时域观测时长H，H＜T，且计数器置为1；
步骤S25，判断障碍物车辆是否已经进入所述短时长预测模式，如果是，则进入步骤S26；
步骤S26，将计数器加1，当所述计数器达到设定阈值E时，退出所述短时长预测模式，并将所述历史轨迹时长恢复为T；其中，所述阈值E的数值设置应能够保证从所述短时域观测时长H增长到所述正常观测时长T。
3.如权利要求1所述的车辆轨迹预测方法，其特征在于，所述步骤S3之前还包括：
构建用于考虑轨迹点之间的偏移距离的目标函数、并对所述实际历史轨迹进行优化的步骤；
所述实际历史轨迹包括相对历史轨迹和绝对历史轨迹，所述步骤S3采用所述相对历史轨迹，所述步骤S4采用所述绝对历史轨迹；
其中，所述目标函数设置为式(10)：
Figure FDA0003704978600000021
其中，xi＝(xi，yi)，i∈1，2，...，n，为所述实际历史轨迹上的原始轨迹点yi被优化后的坐标，Δxi＝xi-xi-1为所述实际历史轨迹上第i个轨迹点xi与第i-1个轨迹点xi-1之间的位移矢量，α，β为权重系数。
4.如权利要求3所述的车辆轨迹预测方法，其特征在于，所述实际历史轨迹上的原始轨迹点yi被优化为xi的方法采用梯度下降法，各项梯度的表达式如下：
Figure FDA0003704978600000022
Figure FDA0003704978600000023
梯度下降法迭代公式如下：
Figure FDA0003704978600000024
式中，η为学习率，f(xi)为所述目标函数，Δxi+1＝xi+1-xi为所述实际历史轨迹上第i+1个轨迹点xi+1与第i个轨迹点xi之间的位移矢量，Δxi+2＝xi+2-xi+1为所述实际历史轨迹上第i+2个轨迹点xi+2与第i+1个轨迹点xi+1之间的位移矢量，Δxi-1＝xi-1-xi-2为所述实际历史轨迹上第i-1个轨迹点xi-1与第i-2个轨迹点xi-2之间的位移矢量，Δxi-2＝xi-2-xi-3为所述实际历史轨迹上第i-2个轨迹点xi-2与第i-3个轨迹点xi-3之间的位移矢量。
5.如权利要求3或4中任一项所述的车辆轨迹预测方法，其特征在于，所述相对历史轨迹根据从障碍物车辆的位置相对于自车的相对位置而获得；其中，所述相对历史轨迹上的轨迹点的坐标表示为(xr，yr)，r为所述相对历史轨迹上轨迹点的序号；所述绝对历史轨迹根据下式获得，其中，所述绝对历史轨迹上的轨迹点的坐标表示为(xa，ya)，a为所述绝对历史轨迹上轨迹点的序号；
Figure FDA0003704978600000031
式中，(xe，ye)、θ分别为自车的绝对位置信息和朝向信息。
6.如权利要求1至4中任一项所述的车辆轨迹预测方法，其特征在于，所述步骤S6的根据比较的结果选择本帧预测方法的方法具体包括：
所述第一误差小于所述第二误差的情形下，选择的本帧预测方法为如式(16)所示的最小二乘曲线拟合预测所述未来轨迹；
Figure FDA0003704978600000032
所述第一误差大于或等于所述第二误差的情形下，选择的本帧预测方法为如式(17)所示的运动学模型预测所述未来轨迹；
Figure FDA0003704978600000033
式中，(xj，yj)为所述未来轨迹上的轨迹点在UTM坐标系下的坐标，j为所述未来轨迹上轨迹点的序号j＝1，2，...m，(x0，y0)为UTM坐标系下障碍物车辆当前所在位置点的坐标，r为所述拟合轨迹的半径，ω为所述障碍物车辆状态中的障碍物车辆的转动速度，Δt为采样时间间隔，θ为障碍物车辆当前所在位置点在所述拟合轨迹上的初始角度，(xn，yn)为障碍物车辆当前所在位置点的坐标，vx、vy分别为障碍物车辆的当前的横向速度和纵向速度，ax、ay分别为障碍物车辆的当前的横向加速度和纵向加速度。
7.一种车辆轨迹预测装置，其特征在于，包括：
信息获取单元，其用于获取障碍物车辆的实际历史轨迹；
观测时长单元，其用于根据障碍物车辆的预测位置和当前实际位置，计算得到预测位置误差，并依此选择本次预测中所使用的历史轨迹时长；
曲线拟合单元，其用于根据所述实际历史轨迹，采用曲线拟合方法获得最接近所述实际历史轨迹的拟合轨迹；
状态计算单元，其用于根据所述实际历史轨迹，计算符合障碍物车辆历史运动状态的障碍物车辆状态；
轨迹预测单元，其用于根据所述拟合轨迹和所述障碍物车辆状态，在所述历史轨迹时长内按预设时间间隔进行采样，并使用曲线拟合方程计算获得第一历史轨迹、以及使用运动学方程计算获得第二历史轨迹，再计算所述第一历史轨迹与所述相对历史轨迹上对应轨迹点之间的第一误差、以及计算所述第二历史轨迹与所述绝对历史轨迹上对应轨迹点之间的第二误差，最后在预设范围内对所述第一误差与第二误差进行比较，并根据比较的结果选择本帧预测方法，在预测时域内按预设采样时间预测障碍物车辆的未来轨迹。
8.如权利要求7所述的车辆轨迹预测装置，其特征在于，所述观测时长单元具体包括：
计算误差子单元，其用于根据障碍物车辆的预测位置和当前实际位置，计算得到预测位置误差；
轨迹更新子单元，其用于根据所述实际历史轨迹中的时间戳信息和所述历史轨迹时长的变化信息，完成对应障碍物车辆的所述实际历史轨迹的更新。
9.如权利要求7或8所述的车辆轨迹预测装置，其特征在于，还包括：
轨迹平滑单元，其用于构建用于考虑轨迹点之间的偏移距离的目标函数，并对所述实际历史轨迹进行平滑优化；
所述实际历史轨迹包括相对历史轨迹和绝对历史轨迹，所述曲线拟合单元采用所述相对历史轨迹，所述状态计算单元采用所述绝对历史轨迹；
其中，所述目标函数设置为式(10)：
Figure FDA0003704978600000041
其中，所述实际历史轨迹上的原始轨迹点yi被优化为xi的方法采用梯度下降法，各项梯度的表达式如下：
Figure FDA0003704978600000042
Figure FDA0003704978600000051
梯度下降法迭代公式如下：
Figure FDA0003704978600000052
式中，xi＝(xi，yi)，i∈1，2，...，n，为所述实际历史轨迹上的原始轨迹点yi被优化后的坐标，Δxi＝xi-xi-1为所述实际历史轨迹上第i个轨迹点xi与第i-1个轨迹点xi-1之间的位移矢量，α，β为权重系数，η为学习率，f(xi)为所述目标函数，Δxi+1＝xi+1-xi为所述实际历史轨迹上第i+1个轨迹点xi+1与第i个轨迹点xi之间的位移矢量，Δxi+2＝xi+2-xi+1为所述实际历史轨迹上第i+2个轨迹点xi+2与第i+1个轨迹点xi+1之间的位移矢量，Δxi-1＝xi-1-xi-2为所述实际历史轨迹上第i-1个轨迹点xi-1与第i-2个轨迹点xi-2之间的位移矢量，Δxi-2＝xi-2-xi-3为所述实际历史轨迹上第i-2个轨迹点xi-2与第i-3个轨迹点xi-3之间的位移矢量。
10.如权利要求9所述的车辆轨迹预测装置，其特征在于，所述轨迹预测单元具体包括：
模型选择子单元，其用于在所述第一误差小于所述第二误差的情形下，选择的本帧预测方法为如式(16)所示的最小二乘曲线拟合预测所述未来轨迹；
Figure FDA0003704978600000053
在所述第一误差大于或等于所述第二误差的情形下，选择的本帧预测方法为如式(17)所示的运动学模型预测所述未来轨迹；
Figure FDA0003704978600000054
式中，(xj，yj)为所述未来轨迹上的轨迹点在UTM坐标系下的坐标，j为所述未来轨迹上轨迹点的序号j＝1，2，...m，(x0，y0)为UTM坐标系下障碍物车辆当前所在位置点的坐标，r为所述拟合轨迹的半径，ω为所述障碍物车辆状态中的障碍物车辆的转动速度，Δt为采样时间间隔，θ为障碍物车辆当前所在位置点在所述拟合轨迹上的初始角度，(xn，yn)为障碍物车辆当前所在位置点的坐标，vx、vy分别为障碍物车辆的当前的横向速度和纵向速度，ax、ay分别为障碍物车辆的当前的横向加速度和纵向加速度；
轨迹输出子单元，其用于根据选择的预测模型，在预测时域内进行轨迹点计算，将预测轨迹与上帧轨迹加权平均后输出。
CN202210705025.6A 2022-06-21 2022-06-21 一种车辆轨迹预测方法和装置 Active CN115060280B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210705025.6A CN115060280B (zh) 2022-06-21 2022-06-21 一种车辆轨迹预测方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210705025.6A CN115060280B (zh) 2022-06-21 2022-06-21 一种车辆轨迹预测方法和装置

Publications (2)

Publication Number Publication Date
CN115060280A true CN115060280A (zh) 2022-09-16
CN115060280B CN115060280B (zh) 2023-07-25

Family

ID=83202529

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210705025.6A Active CN115060280B (zh) 2022-06-21 2022-06-21 一种车辆轨迹预测方法和装置

Country Status (1)

Country Link
CN (1) CN115060280B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115342817A (zh) * 2022-10-20 2022-11-15 北京理工大学 无人履带车辆轨迹跟踪状态监测方法、系统、设备、介质
CN117775078A (zh) * 2024-02-28 2024-03-29 山西阳光三极科技股份有限公司 一种基于深度学习的矿内货运列车行驶方向判断方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109631915A (zh) * 2018-12-19 2019-04-16 百度在线网络技术（北京）有限公司 轨迹预测方法、装置、设备及计算机可读存储介质
CN111062372A (zh) * 2020-03-13 2020-04-24 北京三快在线科技有限公司 一种预测障碍物轨迹的方法及装置
CN112109704A (zh) * 2020-09-22 2020-12-22 同济大学 一种基于精准轨迹预测的车辆避撞动态安全路径规划方法
KR20210065075A (ko) * 2020-05-14 2021-06-03 베이징 바이두 넷컴 사이언스 앤 테크놀로지 코., 엘티디. 차량 주행 궤적의 예측 방법, 장치, 기기 및 저장 매체
CN113022580A (zh) * 2021-03-17 2021-06-25 地平线(上海)人工智能技术有限公司 轨迹预测方法、装置、存储介质及电子设备
CN113335276A (zh) * 2021-07-20 2021-09-03 中国第一汽车股份有限公司 障碍物的轨迹预测方法、装置、电子设备及存储介质
CN113844446A (zh) * 2021-10-14 2021-12-28 安徽江淮汽车集团股份有限公司 融合长短程的车辆轨迹预测方法
CN114228746A (zh) * 2022-01-17 2022-03-25 北京经纬恒润科技股份有限公司 一种预测车辆运动轨迹的方法及装置

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109631915A (zh) * 2018-12-19 2019-04-16 百度在线网络技术（北京）有限公司 轨迹预测方法、装置、设备及计算机可读存储介质
CN111062372A (zh) * 2020-03-13 2020-04-24 北京三快在线科技有限公司 一种预测障碍物轨迹的方法及装置
KR20210065075A (ko) * 2020-05-14 2021-06-03 베이징 바이두 넷컴 사이언스 앤 테크놀로지 코., 엘티디. 차량 주행 궤적의 예측 방법, 장치, 기기 및 저장 매체
CN112109704A (zh) * 2020-09-22 2020-12-22 同济大学 一种基于精准轨迹预测的车辆避撞动态安全路径规划方法
CN113022580A (zh) * 2021-03-17 2021-06-25 地平线(上海)人工智能技术有限公司 轨迹预测方法、装置、存储介质及电子设备
CN113335276A (zh) * 2021-07-20 2021-09-03 中国第一汽车股份有限公司 障碍物的轨迹预测方法、装置、电子设备及存储介质
CN113844446A (zh) * 2021-10-14 2021-12-28 安徽江淮汽车集团股份有限公司 融合长短程的车辆轨迹预测方法
CN114228746A (zh) * 2022-01-17 2022-03-25 北京经纬恒润科技股份有限公司 一种预测车辆运动轨迹的方法及装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
张一鸣;周兵;吴晓建;崔庆佳;柴天;: "基于前车轨迹预测的高速智能车运动规划", 汽车工程, no. 05 *
贾运红;马浩楠;: "基于空间模型预测控制的矿井车辆避障控制器的研究", 煤炭学报, no. 2 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115342817A (zh) * 2022-10-20 2022-11-15 北京理工大学 无人履带车辆轨迹跟踪状态监测方法、系统、设备、介质
CN115342817B (zh) * 2022-10-20 2023-02-03 北京理工大学 无人履带车辆轨迹跟踪状态监测方法、系统、设备、介质
CN117775078A (zh) * 2024-02-28 2024-03-29 山西阳光三极科技股份有限公司 一种基于深度学习的矿内货运列车行驶方向判断方法
CN117775078B (zh) * 2024-02-28 2024-05-07 山西阳光三极科技股份有限公司 一种基于深度学习的矿内货运列车行驶方向判断方法

Also Published As

Publication number Publication date
CN115060280B (zh) 2023-07-25

Similar Documents

Publication Publication Date Title
EP3638981B1 (en) Fusion framework of navigation information for autonomous navigation
CN115060280A (zh) 一种车辆轨迹预测方法和装置
CN110728014B (zh) 使用具有加权几何成本的分段螺旋曲线的参考线平滑方法
US20200149906A1 (en) Path planning method, system and device for autonomous driving
CN109955853B (zh) 用于操作自动驾驶车辆的方法、系统和存储介质
CN110568758B (zh) 一种自动驾驶汽车的参数自适应横向运动lqr控制方法
CN109080631A (zh) 用于车辆的低速横向控制的系统和方法
CN105292116B (zh) 自动驾驶车辆的车道变换路径规划算法
US11499834B2 (en) Aligning road information for navigation
US20200369293A1 (en) Autonomous driving apparatus and method
CN110361013B (zh) 一种用于车辆模型的路径规划系统及方法
CN111845774A (zh) 一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法
JP2019182414A (ja) 自動運転車に対して障害物の予測軌跡を生成するための方法
CN110597245A (zh) 基于二次型规划和神经网络的自动驾驶换道轨迹规划方法
JP2019194073A (ja) 自動運転車の軌跡候補を評価するための方法
Xiong et al. Application improvement of A* algorithm in intelligent vehicle trajectory planning
CN101837781A (zh) 用于自动车道对正或换道的控制系统的基于模型的预测控制
CN111123947B (zh) 机器人行进的控制方法、装置、电子设备、介质和机器人
CN109669463A (zh) 一种考虑agv速度和前轮偏向角可变的区间轨迹跟踪方法
CN111137298B (zh) 一种车辆自动驾驶方法、装置、系统和存储介质
CN114442630B (zh) 一种基于强化学习和模型预测的智能车规划控制方法
CN114942642A (zh) 一种无人驾驶汽车轨迹规划方法
CN116337045A (zh) 一种基于karto和teb的高速建图导航方法
CN116373851A (zh) 一种自动泊车的路径规划方法、自动泊车方法及相关装置
Jiang et al. A dynamic motion planning framework for autonomous driving in urban environments

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