CN116513245B - 一种基于行车环境的快速自动驾驶轨迹规划方法 - Google Patents

一种基于行车环境的快速自动驾驶轨迹规划方法 Download PDF

Info

Publication number
CN116513245B
CN116513245B CN202310805585.3A CN202310805585A CN116513245B CN 116513245 B CN116513245 B CN 116513245B CN 202310805585 A CN202310805585 A CN 202310805585A CN 116513245 B CN116513245 B CN 116513245B
Authority
CN
China
Prior art keywords
track
planning
coordinate system
vehicle
planned
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
CN202310805585.3A
Other languages
English (en)
Other versions
CN116513245A (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.)
Shenzhen Anngic Technology Co ltd
Original Assignee
Shenzhen Anngic Technology Co ltd
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 Shenzhen Anngic Technology Co ltd filed Critical Shenzhen Anngic Technology Co ltd
Priority to CN202310805585.3A priority Critical patent/CN116513245B/zh
Publication of CN116513245A publication Critical patent/CN116513245A/zh
Application granted granted Critical
Publication of CN116513245B publication Critical patent/CN116513245B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • B60W60/00—Drive control systems specially adapted for autonomous road vehicles
    • B60W60/001—Planning or execution of driving tasks
    • B60W60/0011—Planning or execution of driving tasks involving control alternatives for a single driving scenario, e.g. planning several paths to avoid obstacles
    • 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
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units, or advanced driver assistance systems for ensuring comfort, stability and safety or drive control systems for propelling or retarding the vehicle
    • B60W30/08—Active safety systems predicting or avoiding probable or impending collision or attempting to minimise its consequences
    • B60W30/095—Predicting travel path or likelihood of collision
    • B60W30/0956—Predicting travel path or likelihood of collision the prediction being responsive to traffic or environmental parameters
    • 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
    • B60W2552/00—Input parameters relating to infrastructure
    • B60W2552/50—Barriers
    • 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
    • B60W2555/00—Input parameters relating to exterior conditions, not covered by groups B60W2552/00, B60W2554/00
    • B60W2555/20—Ambient conditions, e.g. wind or rain
    • 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)
  • Automation & Control Theory (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种基于行车环境的快速自动驾驶轨迹规划方法，包括下述步骤：S1：确定车辆规划轨迹起点坐标；S2：获取自车周围环境信息；S3：根据车辆规划轨迹起点坐标确定自然坐标系终点参考状态；S4：根据自车周围环境信息和自然坐标系终点参考状态自适应更新轨迹代价权重并为轨迹打分；S5：对轨迹进行车辆动力学约束检测和碰撞检测，并获得自动驾驶轨迹规划方案。本发明提出的自适应调整轨迹代价权重及轨迹规划序列时长的方法不受当前行车环境的影响，能结合实际行车环境自动调整轨迹权重及轨迹规划序列时长，避免固定代价权重在复杂工况下发生换道激烈或者长时间压线行驶以及无法找到满足约束的参考轨迹等情况，具备较好的鲁棒性能。

Description

一种基于行车环境的快速自动驾驶轨迹规划方法
技术领域
本发明属于自动驾驶技术领域，更具体地，涉及一种基于行车环境的快速自动驾驶轨迹规划方法。
背景技术
基于多项式曲线拟合的运动规划方法由于具备规划计算耗时短、规划路径曲率连续，且规划速度及加速度不发生突变等优势现已在车辆自动驾驶局部路径规划领域有了非常广泛的研究。基于多项式曲线拟合的算法输出部分包含带参考时间的路径坐标序列，带参考时间的速度序列，带参考时间的加速度序列，带参考时间的航向序列以及带参考时间的曲率序列。该算法核心思想为根据环境信息确定自车终点状态位姿，用多项式函数连接起点及终点状态，生成拟合曲线后输出给控制模块跟踪。多项式曲线拟合生成运动规划轨迹具备根据环境情况实时更新轨迹曲线的能力，环境干扰对轨迹生成影响小，具备极好的鲁棒性。
当前学术界及工业界常见的基于多项式拟合运动规划轨迹的方法是将自车位姿状态转换到以基础参考路径的自然坐标系上，根据预设的轨迹预测时域长度，对纵向进行时间采样以获取纵向轨迹函数，由于车辆横向运动是由纵向运动诱发的，因此对横向进行纵向弧长采样，获取横向轨迹函数。合并横纵向轨迹后，基于自车固有性能对生成的参考轨迹簇做车辆动力学约束检测及碰撞检测，最终挑选出最优参考轨迹并输出给控制模块跟踪。目前常见的轨迹生成方法其轨迹代价权重及规划预测时长是固定的，在复杂的行车工况下可能会导致无法生成轨迹的情况，进而导致潜在驾驶危险。此外，由于发送给控制跟踪的带时间序列参考轨迹需要较为密集的采样才能使控制准确找到对应时刻的跟踪点，生成所有轨迹簇后再根据约束去挑选出最优轨迹无疑会大大增加规划模块的计算耗时。
发明内容
针对现有技术的缺陷，本发明的目的在于提供一种基于行车环境的快速自动驾驶轨迹规划方法，旨在解决自动驾驶车辆在复杂工况下无法快速求解出规划轨迹的问题。
本发明提供了一种基于行车环境的快速自动驾驶轨迹规划方法，包括下述步骤：
S1：确定车辆规划轨迹起点坐标；
S2：获取自车周围环境信息；
S3：根据所述车辆规划轨迹起点坐标确定自然坐标系终点参考状态；
S4：根据所述自车周围环境信息和所述自然坐标系终点参考状态自适应更新轨迹代价权重并为轨迹打分；
S5：对轨迹进行车辆动力学约束检测和碰撞检测，自适应调整轨迹规划序列时长，并获得自动驾驶轨迹规划方案。
更进一步地，步骤S1具体为：
（1）确定全局坐标系（GCS）下轨迹规划起点坐标；
（2）确定自然坐标系的规划起点坐标。
更进一步地，步骤S2具体为：
（1）根据传感器信息确定当前行车场景为停车场景还是非停车场景；
（2）获取自车周围障碍物在GCS下的状态信息，并将其转换到以自车在全局参考路径下的投影点为原点，参考路径为纵轴的自然坐标系下，获取自然坐标系下障碍物的投影点信息。
更进一步地，步骤S4具体为：
S4.1 确定轨迹代价函数表达形式为
；
其中表示终点横向位置偏移与道路中心线的距离权重，/>表示到达规划终点的时间权重，/>表示规划的终点速度权重，/>表示规划的终点横向偏移与道路边界的距离权重，/>表示规划的终点横向偏移值与基准参考道路中心线距离的权重，/>表示与道路中心线的横向距离，/>表示规划时间，/>表示与巡航速度的相对速度，表示与道路边界的横向距离，/>表示与基准参考中心线的横向距离；
S4.2：确定生成轨迹序列可选基准预设时长；
S4.3：根据自车周围环境信息自适应调整S4.1中的代价权重值；
S4.4：计算更新代价权重后的代价轨迹代价函数值，对待生成的轨迹预打分，并依次排序。
更进一步地，步骤S5具体为：
S5.1：从得分最低的轨迹开始计算生成带时间的轨迹；
S5.2：判断是否有轨迹生成，若无，则说明基准预测时长序列均不满足，此时在最大预设时长上以1秒为间隔增加轨迹生成时长，重复S5.1，生成参考轨迹；
S5.3：对轨迹进行碰撞检测。
其中，若轨迹序列预测时长增加到20秒仍无法生成轨迹，说明此时前方无可行道路，需要为驾驶员发出提醒信号。
更进一步地，步骤5.1具体为：
S5.1.1：提取轨迹自然坐标系的初始状态及终点状态的纵向相关参数，生成纵向参考轨迹，若终点状态不存在参考纵向距离，则纵向位移轨迹由4次多项式连接，否则由5次多项式连接；，，/>；
其中，/>，/>分别表示规划起点离原点的距离（m），规划起点的速度（/>），规划起点的加速度（/>）；/>，/>，/>分别表示规划终点的参考位移距离（m），规划终点的速度（/>），规划终点的加速度（/>）；t表示规划时域（s），离散时间间隔为dt（s）；均为函数参数；
S5.1.2判断步骤5.1.1生成的参考速度轨迹在第/>时刻时是否超过设定的巡航速度，若超过，则其对应时刻的位移及加速度应分别使用积分和微分求解；
S5.1.3：设定车辆动力学约束，最小行驶速度，最大加速度/>，最大制动加速度/>，/>；
S5.1.4：判断生成的速度轨迹曲线及加速度轨迹曲线是否满足速度及加速度的动力学约束，若不满足则返回至S5.1进行下一条轨迹的生成判断，否则进入步骤5.1.5；
S5.1.5：提取规划起点及终点的横向相关参数，结合s（t）函数生成自然坐标系横向参考轨迹；，；其中/>，/>，/>分别表示规划起点时自然坐标系下的横向偏移，横向偏移对弧长的一阶导数及二阶导数；/>表示参考位移，与/>一一对应（m）；/>为五次多项式系数；
S5.1.6：判断生成的L（S（t））轨迹在规划的前n秒的序列点是否超过道路的最大边界，若超过道路最大边界，说明该条规划的轨迹自车不可达，返回S5.1进行下一条轨迹的筛选，否则进入S5.1.7；
S5.1.7：合并横纵向轨迹生成GCS坐标系下带时间的参考轨迹；
S5.1.8：判断第t时刻下的曲率是否满足曲率约束，若满足/>，则重复S5.1.7，直至遍历完前n秒内的所有时间点；若不满足/>，则返回S5.1进行下一条轨迹的筛选。
更进一步地，步骤S5.1.7具体包括：
S5.1.7.1：依次找到s（t）中与时间一一对应的位移s在全局参考路径上的投影点信息；
S5.1.7.2：计算GCS下第t时刻的参考x，y坐标；
S5.1.7.3：计算GCS下第t时刻的参考航向，
；
S5.1.7.4：计算GCS下第t时刻的参考曲率，
。
其中，步骤5.3具体包括：若当前自车周围不存在障碍物，则输出S5.1.8生成的GCS参考轨迹给控制模块跟踪执行；若当前自车周围存在障碍物，则结合传感器获取的障碍物信息预测障碍物在规划轨迹时域内的运动轨迹，检测障碍物轨迹与规划轨迹在对应时刻是否发生重叠，若不重叠，则认为不发生碰撞，输出S5.1.8生成的轨迹给控制模块跟踪执行；否则，存在碰撞风险，需要返回S5.1进行下一条轨迹筛选。
通过本发明所构思的以上技术方案，与现有技术相比，具备如下技术效果：
（1）本发明提出了自适应调整轨迹代价权重的方法不受当前行车环境的影响，能结合实际行车环境自动调整轨迹权重，避免固定代价权重在复杂工况下发生换道激烈或者长时间压线行驶以及无法找到满足约束的参考轨迹等情况，具备较好的鲁棒性能。
（2）本发明提出了同时进行轨迹生成及轨迹约束检测的方法，相比传统生成所有轨迹后再做约束检测能极大程度的降低轨迹规划耗时，具备较强的工程应用能力。
（3）本发明提出了基于行车场景主动增加轨迹预测时长的方法可以避免规划时域不可调整而出现轨迹无解的情况（特别是在停车场景时，快速制动不安全也不舒适），可在一定程度上提高安全性及舒适性。
附图说明
图1为本发明提供的基于行车环境的快速自动驾驶轨迹规划方法的实现流程图。
图2为本发明实施例提供的基于行车环境的快速自动驾驶轨迹规划方法的算法框架示意图。
图3为本发明实施例提供的综合仿真测试道路场景示意图。
图4为本发明实施例提供的基于行车环境的快速自动驾驶轨迹规划方法在自然坐标系的轨迹规划结果示意图。
图5为本发明实施例提供的基于行车环境的快速自动驾驶轨迹规划方法规划的参考轨迹速度及加速度信息示意图。
图6为本发明实施例提供的基于传统多项式拟合轨迹规划方法在自然坐标系的轨迹规划结果。
图7为本发明实施例提供的基于传统多项式拟合轨迹规划方法的参考轨迹速度及加速度信息示意图。
图8为本发明实施例提供的基于传统多项式拟合轨迹规划方法在复杂行车环境下的轨迹规划结果示意图。
图9为本发明实施例提供的基于行车环境的快速自动驾驶轨迹规划方法在复杂行车环境下的轨迹规划结果示意图。
图10为本发明实施例提供的基于行车环境的快速自动驾驶轨迹规划方法在自车巡航速度为15m/s时自车的速度仿真曲线示意图。
图11为本发明实施例提供的基于行车环境的快速自动驾驶轨迹规划方法在自车巡航速度为25m/s时自车的速度仿真曲线示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
本发明针对当前自动驾驶车辆运动轨迹规划使用多项式拟合方式存在的缺陷，提出了一种基于车辆行车周围环境的自适应调整预测时域长度及代价权重的快速运动轨迹规划方法，可以解决自动驾驶车辆在复杂工况下无法快速求解出规划轨迹的问题。
本发明所提出的一种基于行车环境的快速轨迹规划方法如图1所示，可按照最优轨迹生成过程分成5个部分：确定轨迹规划起点状态，获取自车周围环境信息，确定行车终点状态，自适应更新轨迹代价权重并为轨迹打分，轨迹车辆动力学约束检测及碰撞检测。确定规划起点状态实现自车全局位姿及车辆坐标系数据与自然坐标系的转化，确定轨迹规划在自然坐标系的起点状态。获取自车周围环境信息实现自车行驶场景的判断（停车/非停车），同时获取自车周围传感器检测区域内障碍物的运动信息。确定终点参考状态模块实现接收行车场景信息确定自然坐标系下的终点状态，并根据终点状态确定轨迹多项式表现形式；自适应更新轨迹代价权重及轨迹打分模块根据行车环境信息及终点状态自适应更新代价权重，并对轨迹预打分；车辆动力学约束及碰撞检测模块实现自然坐标系轨迹转化为全局坐标系轨迹并自适应更新轨迹规划预测时域并生成最优参考轨迹。
为了更进一步的说明本发明实施例提供的基于行车环境的快速自动驾驶轨迹规划方法，现结合如图2所示的算法框架详述如下：
一．确定车辆轨迹规划起点坐标：
步骤1.1：确定全局坐标系（Global Coordinate System, GCS）下轨迹规划起点坐标。
步骤1.2：确定自然坐标系的规划起点坐标
其中，步骤1.1具体为：
步骤1.1.1：获取当前时刻下自车在GCS的位姿；其中/>，/>，/>，/>，/>，/>，，/>分别表示GCS下自车的横纵坐标（m），航向（rad），曲率，横纵向速度（m/s），横纵向加速度（/>）。
步骤1.1.2：找到上一个规划周期生成的轨迹在当前时刻轨迹规划的参考位姿，记为。
步骤1.1.3：计算当前自车位姿与参考位置的横向误差及纵向误差。
步骤1.1.4：判断横纵向误差是否均满足预设误差阈值。
更进一步地，步骤1.1.3具体包括：
步骤1.1.3.1：参考位置的方向向量及法向量/>为：/>，；
步骤1.1.3.2：当前自车位置与参考位姿的位置误差为：；
步骤1.1.3.3：当前自车位置与参考位置的横向误差表示为：，/>。
更进一步地，步骤1.1.4具体包括：
步骤1.1.4.1：如果横向误差小于等于预设最大横向误差且纵向误差小于等于预设纵向误差值，即：；
步骤1.1.4.2：横向误差或纵向误差大于最大预设误差值，即：。
作为本发明的一个实施例，步骤1.1.4.1具体包括：
步骤1.1.4.1.1：找到上一个规划周期规划的轨迹在规划时间起点的参考位置，/>，/>；其中/>表示规划起点时间（s）；/>表示当前时间（s）；/>表示规划周期（s）；/>，/>，/>，/>分别表示GCS规划起点的x（m），y（m），航向（rad），曲率；/>，/>分别表示参考轨迹点的速度（m/s）及加速度（），方向为轨迹切线方向。
步骤1.1.4.1.2：计算GCS规划起点的横向速度及纵向速度/>，，/>；
步骤1.1.4.1.3：计算GCS规划起点的横向加速度及纵向加速度/>。
步骤1.1.4.1.4：GCS规划起点位姿状态为：，其中/>，/>，/>，/>分别表示规划起点的x坐标（m），y坐标（m），航向（rad），曲率。
其中，步骤1.1.4.1.3具体为：
步骤1.1.4.1.3.1：规划起点的方向向量及法向量/>；/>，；
步骤1.1.4.1.3.2：计算规划起点的横纵向加速度，。
作为本发明的一个实施例，步骤1.1.4.2具体包括：
步骤1.1.4.2.1：以当前速度做匀速行驶后的位置作为GCS下的规划起点。
步骤1.1.4.2.2：GCS规划起点的位姿状态为：。
其中，步骤1.1.4.2.1具体包括：
1.1.4.2.1.1规划起点坐标：，/>；
1.1.4.2.1.2规划起点时间：/>。
在本发明实施例中，步骤1.2中确定自然坐标系的规划起点坐标具体包括：
步骤1.2.1：计算当前时刻自车位姿在全局参考路径上的投影点，以该投影点作为自然坐标的原点，全局参考路径点为自然坐标系的参考纵轴。
步骤1.2.2：计算GCS规划起点位姿在全局参考路径的投影点。
步骤1.2.3：计算GCS规划起点在自然坐标系下的状态。
其中，步骤1.2.2具体包括：
步骤1.2.2.1：找到GCS规划起点在全局参考路径的匹配点；
步骤1.2.2.2：根据匹配点计算规划起点在全局参考路径上的投影点位姿，，/>，/>，/>，/>。
更进一步地，步骤1.2.2.1具体包括：
步骤1.2.2.1.1：规划起点与匹配点的距离；
步骤1.2.2.1.2：匹配点的方向向量及法向量/>，/>，；
步骤1.2.2.1.3：规划起点与匹配点位置矢量，/>；
步骤1.2.2.1.4：规划起点与匹配点纵向位置弧长，/>；其中/>表示规划起点与参考路径上离散点最近的距离（m）；/>，/>分别表示匹配点的坐标（m）。/>表示匹配点的参考航向（rad）。
在本发明实施例中，步骤1.2.3中计算GCS规划起点在自然坐标系下的状态具体包括：
步骤1.2.3.1：通过插值找到规划起点坐标在自然坐标系的初始（m）；
步骤1.2.3.2：计算规划起点的横向偏离（m），
；
步骤1.2.3.3：计算规划起点的速度，/>；
步骤1.2.3.3：计算规划起点的横向位移对弧长的一阶导数，
；
步骤1.2.3.4：计算规划起点的加速度，
；
步骤1.2.3.5：计算规划起点横向位移对弧长的二阶导数，
。
二．获取自车周围环境信息：
步骤2.1：接收传感器信息，确定当前行车场景为停车场景还是非停车场景；
步骤2.2：获取自车周围障碍物在GCS下的状态信息，并将其转换到以自车在全局参考路径下的投影点为原点，参考路径为轴的自然坐标系下，获取自然坐标系下障碍物的投影点信息（参考步骤1.2）。
三．确定自然坐标系终点参考状态：
步骤3.1：非停车场景终点状态（e.g.前方非红灯且非终点）；
步骤3.1.1：自然坐标系下巡航终点状态：；
步骤3.1.2：自然坐标系下换道终点状态：；
步骤3.1.3：自然坐标系下跟车终点状态：，，其中/>表示自车预设巡航速度/>，/>为规划起点车道中心线与参考轴的横向偏移值（m）；/>表示规划起点相邻车道中心线与参考轴的横向偏移值（m）；表示跟踪目标的速度/>；/>表示障碍车与参考轴的横向偏移值（m）；/>表示障碍车横向偏移对弧长的一阶导数；/>表示跟踪距离（m）；/>表示与跟车目标的距离（m）；/>表示安全跟车距离（m）。
步骤3.2：停车场景终点状态：
步骤3.2.1：自然坐标系下巡航终点状态；
步骤3.2.2：自然坐标系下换道终点状态；
步骤3.2.3：自然坐标系下跟车终点状态；其中表示到终点的距离（m）。
四．自适应更新轨迹代价权重并为轨迹打分
步骤4.1 确定轨迹代价函数表达形式；其中/>表示终点横向位置偏移与道路中心线的距离权重；/>表示到达规划终点的时间权重；/>表示规划的终点速度权重；/>表示规划的终点横向偏移与道路边界的距离权重；/>表示规划的终点横向偏移值与基准参考道路中心线距离的权重；/>表示与道路中心线的横向距离；表示规划时间；/>表示与巡航速度的相对速度；/>表示与道路边界的横向距离；/>表示与基准参考中心线的横向距离。
步骤4.2：确定生成轨迹可选基准预设时长（s）。
步骤4.3：结合传感器获取的自车周围环境信息，根据自车周围环境信息，自适应调整步骤4.1的代价权重值。
步骤4.4：计算更新代价权重后的轨迹代价函数值，对待生成的轨迹预打分，并依次排序。
五．轨迹车辆动力学约束检测及碰撞检测：
步骤5.1：从得分最低的轨迹开始计算生成带时间的轨迹；
步骤5.1.1：提取轨迹自然坐标系的初始状态及终点状态的纵向相关参数，生成纵向参考轨迹，若终点状态不存在参考纵向距离，则纵向位移轨迹由4次多项式连接，否则由5次多项式连接。，，/>；其中/>，/>，/>分别表示规划起点离原点的距离（m），规划起点的速度（/>），规划起点的加速度（/>）；/>，/>，/>分别表示规划终点的参考位移距离（m），规划终点的速度（），规划终点的加速度（/>）；t表示规划时域（s），离散时间间隔为dt（s）；/>均为函数参数。
步骤5.1.2判断步骤5.1.1生成的参考速度轨迹在第/>时刻时是否超过设定的巡航速度，若超过，则其对应时刻的位移及加速度应分别使用积分和微分求解。
步骤5.1.3：设定车辆动力学约束，最小行驶速度，最大加速度/>，最大制动加速度/>，/>。
步骤5.1.4：判断生成的速度轨迹曲线及加速度轨迹曲线是否满足速度及加速度的动力学约束，若不满足则返回步骤5.1进行下一条轨迹的生成判断，否则进入步骤5.1.5；
步骤5.1.5：提取规划起点及终点的横向相关参数，结合s（t）函数生成自然坐标系横向参考轨迹。，/>；其中/>，/>，/>表示规划起点时自然坐标系下的横向偏移，横向偏移对弧长的一阶导数及二阶导数；/>表示参考位移，与/>一一对应（m）；/>为五次多项式系数。
步骤5.1.6：判断生成的L（S（t））轨迹在规划的前n秒的序列点是否超过道路的最大边界，若超过道路最大边界，说明该条规划的轨迹自车不可达，因此需要返回步骤5.1进行下一条轨迹的筛选，否则进入步骤5.1.7。
步骤5.1.7：合并横纵向轨迹生成GCS坐标系下带时间的参考轨迹。
步骤5.1.7.1：依次找到s（t）中与时间一一对应的位移s在全局参考路径上的投影点信息；
步骤5.1.7.2：计算GCS下第t时刻的参考x，y坐标；
步骤5.1.7.3：计算GCS下第t时刻的参考航向
，
步骤5.1.7.4：计算GCS下第t时刻的参考曲率
；
步骤5.1.8：判断第t时刻下的曲率是否满足最大曲率约束，若满足，则重复步骤5.1.7.1~5.1.7.4，直至遍历完前n秒内的所有时间点。若不满足，则返回步骤5.1进行下一条轨迹的筛选。
步骤5.1.9：轨迹碰撞检测。若当前自车周围不存在障碍物，则输出步骤5.1.8生成的GCS参考轨迹给控制模块跟踪执行。若当前自车周围存在障碍物，则结合传感器获取的障碍物信息预测障碍物在规划轨迹时域内的运动轨迹，检测障碍物轨迹与规划轨迹在对应时刻是否发生重叠，若不重叠，则认为不发生碰撞，输出步骤5.1.8生成的轨迹给控制模块跟踪执行；否则，存在碰撞风险，需要返回步骤5.1进行下一条轨迹筛选。
步骤5.2：判断是否有轨迹生成，若无则说明基准预测时长序列均不满足，此时在最大预测时长上以1秒为间隔增加轨迹生成时长，重复步骤5.1，生成参考轨迹给控制跟踪。若预测时长增加到20秒仍无法生成轨迹，说明此时前方无可行道路，需要为驾驶员发出提醒信号。
本发明不受当前行车环境的影响，能结合实际行车环境自动调整轨迹权重，避免固定代价权重在复杂工况下发生换道激烈或者长时间压线行驶以及无法找到满足约束的参考轨迹等情况，具备较好的鲁棒性能。相比传统生成所有轨迹后再做约束检测能极大程度的降低轨迹规划耗时，具备较强的工程应用能力。避免规划时域不可调整而出现轨迹无解的情况（特别是在停车场景时，快速制动不安全也不舒适），可在一定程度上提高安全性及舒适性。
为了更进一步说明本发明实施例提供的采用一种基于行车环境的快速自动驾驶轨迹规划方法对纯电动汽车在仿真场景中的自动驾驶轨迹规划能力进行仿真测试。
测试场景为单向无坡道的四车道，单车车道宽3.75m，车道内存在多辆与测试车辆同向的静止及运动障碍车。道路两侧无障碍物。测试道路包含直道及不同曲率弯道。车辆初始位置速度加速度及航向均为0。轨迹跟踪控制算法横向采用线性二次型算法纵向控制采用PID算法。轨迹规划频率为10HZ，控制频率为100HZ，仿真模型刷新频率为1000HZ。仿真道路场景如图3所示。
1.算法模型初始化。行车最小速度，最大加速度/>，制动最大减速度/>，最大轨迹曲率/>。从行驶方向看，依次从左向右数将第三根车道的车道中心线设置为参考中心线，并导入该车道中心线的参考全局路径点坐标，横坐标序列/>，纵坐标系列/>。轨迹规划基准采样时间为秒，四条车道上均设置有不同速度的障碍车辆。
2.定位及感知数据输入。获取当前时刻自车的定位信息，找到自车投影点在全局参考路径点的位置，算出全局坐标系下的参考航向序列。根据自车周围环境情况，判断当前的行车环境（停车/非停车）并筛选出危险区域内障碍物的信息（速度，位置信息），同时更新终点轨迹代价权重。
3.坐标转换。将当前时刻自车在全局坐标系的投影转换到自然坐标系下，该自然坐标系以自车投影点为原点，全局参考路径为s轴。
4.计算规划起点在自然坐标系的初始状态及障碍物当前时刻在自然坐标系下的初始状态。根据步骤1.2.3计算规划起点初始状态以及障碍物的当前时刻的状态。
5.确定自然坐标系下的终点状态。根据规划起点及道路约束确定巡航跟车及换道的终点状态，并对终点状态依次匹配上基准规划时间。
6.轨迹打分。合并轨迹终点状态，根据代价函数对轨迹预打分并按得分从低到高依次排序。
7.纵向轨迹采样。从打分最低的轨迹开始，离散采样时间，时间间隔，对时间采样，采用多项式拟合生成纵向参考轨迹/>，/>，/>。检测纵向轨迹的速度，加速度是否满足车辆动力学约束。
8.横向轨迹采样。根据纵向采样结果，多项式拟合生成横向参考轨迹,,/>。判断横向轨迹/>是否满足道路边界约束。
9.合并横纵向轨迹。将自然坐标系轨迹转换到全局坐标系，获得全局坐标系下的带时间的轨迹序列，同时判断规划的轨迹曲率是否满足车辆动力学约束。
10.碰撞检测。预测障碍物轨迹，判断规划轨迹与障碍物轨迹是否发生重合。
11.轨迹存在检测。若轨迹存在，则将生成的全局坐标轨迹信息发送给控制模块执行，否则，增加基准预测时长再次循环生成参考轨迹。
本发明所提的方法在自动驾驶轨迹规划的仿真测试结果如图4~11所示。
如图3所示，测试场景包含多种道路类型，自车初始位置在直道车道中心线1的起点位置，测试车道上随机分布了多个静止及运动障碍物，自车预设巡航速度为25m/s。在行驶过程中截取自车车速为20m/s，加速度2为规划起点时，轨迹规划在自然坐标系下的规划结果。基于本发明所提的优化算法在自然坐标系的规划轨迹如图4，图5所示。图4为在自然坐标系下的规划轨迹信息，图5中的（a）是对应规划的纵向速度信息，图5中的（b）是对应的规划的纵向加速度信息。如图4所示，在当前规划时刻，自车检测区域内存在3个危险障碍目标，分布在本车车道及本车相邻车道，其中一个障碍物为静止状态，其余两个为运动状态。根据本发明思想，结合图4示意图，自车在该场景下的最优轨迹为换道轨迹。基于本发明所提方法，轨迹生成与车辆动力学约束判断，碰撞检测及道路边界检测是同时进行的。传统的基于多项式轨迹拟合的方法是生成所有轨迹后再逐条判断轨迹是否满足所有约束，如图6，图7所示。如图6所示，在当前时刻，根据规划起点状态信息轨迹规划器同时规划出了所有可能的轨迹，轨迹对应其对应的速度及加速度信息如图7所示，图7中的（a）表示轨迹对应的速度信息，图7中的（b）表示轨迹对应的加速度信息。生成所有轨迹后再判断逐条判断轨迹约束性能，由于发送给控制执行的参考轨迹点需要较为密集，这无疑增加了规划器的计算强度。表1为基于本发明所提的基于行车环境的快速自动驾驶轨迹规划方法及传统轨迹规划方法在仿真过程中规划器的计算耗时。从表1的结果可看出本发明所提的方法相比传统轨迹规划器能在极大程度上降低轨迹规划器的计算耗时，传统规划器在一些极复杂的场景下可能会导致单次计算的耗时超过规划器的刷新频率，在高速行驶中存在潜在危险，本发明所提方法单次计算耗时远远小于规划器刷新频率，具备在线应用的能力。如下表所示：
/>
图8，图9分别表示在相对复杂的行车环境下，传统轨迹规划器规划的结果及本发明所提方法规划的轨迹结果。从图8可以看出，传统轨迹规划器在该行车场景下规划的结果是激烈的换道轨迹，在高速行驶过程，尤其是在弯道行驶时，激烈换道容易导致车辆失控进而引发交通事故。如图9所示，本发明所提方法是在本车道内做跟车行驶（根据预测障碍车轨迹提前减速跟车），显然本发明所提基于行车环境自适应调整轨迹代价权重的规划方法能在一定程度上提高行车的舒适性及安全性。
图10，图11分别表示自车预设巡航速度为15m/s和 25m/s从起点到终点的仿真速度曲线。如图10所示，当自车巡航速度为15m/s时，在切换到停车场景时，自车从巡航速度减速到静止耗时8.2秒。当自车巡航速度为25m/s时，自车从巡航速度减速到静止耗时12.3秒，两种巡航速度下减速耗时均大于最大基准预设时间，说明本发明所提的方法能根据当前自车行驶状态及周围环境信息自适应调整轨迹规划时间，轨迹生成并不局限在基准时间域内，在保证安全的情况下可有效避免行驶过程中急加速急减速情况的发生。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于行车环境的快速自动驾驶轨迹规划方法，其特征在于，包括下述步骤：
S1：确定车辆规划轨迹起点坐标；
S2：获取自车周围环境信息；
S3：根据所述车辆规划轨迹起点坐标确定自然坐标系终点参考状态；
S4：根据所述自车周围环境信息和所述自然坐标系终点参考状态自适应更新轨迹代价权重并为轨迹打分；
S5：对轨迹进行车辆动力学约束检测和碰撞检测，自适应调整轨迹规划序列时长，并获得自动驾驶轨迹规划方案；
步骤S4具体为：
S4.1：确定轨迹代价函数表达形式为
；
其中表示终点横向位置偏移与道路中心线的距离权重，/>表示到达规划终点的时间权重，/>表示规划的终点速度权重，/>表示规划的终点横向偏移与道路边界的距离权重，/>表示规划的终点横向偏移值与基准参考道路中心线距离的权重，/>表示与道路中心线的横向距离，/>表示规划时间，/>表示与巡航速度的相对速度，/>表示与道路边界的横向距离，/>表示与基准参考中心线的横向距离；
S4.2：确定生成轨迹序列可选基准预设时长；
S4.3：根据自车周围环境信息自适应调整S4.1中的代价权重值；
S4.4：计算更新代价权重后的轨迹代价函数值，对待生成的轨迹预打分，并依次排序。
2.如权利要求1所述的基于行车环境的快速自动驾驶轨迹规划方法，其特征在于，步骤S1具体为：
（1）确定全局坐标系下轨迹规划起点坐标；
（2）确定自然坐标系的规划起点坐标。
3.如权利要求1所述的基于行车环境的快速自动驾驶轨迹规划方法，其特征在于，步骤S2具体为：
（1）根据传感器信息确定当前行车场景为停车场景还是非停车场景；
（2）获取自车周围障碍物在全局坐标系下的状态信息，并将其转换到以自车在全局参考路径下的投影点为原点，参考路径为纵轴的自然坐标系下，获取自然坐标系下障碍物的投影点信息。
4.如权利要求1所述的基于行车环境的快速自动驾驶轨迹规划方法，其特征在于，步骤S5具体为：
S5.1：从得分最低的轨迹开始计算生成带时间的轨迹；
S5.2：判断是否有轨迹生成，若无，则说明基准预测时长序列均不满足，此时在最大预设时长上以1秒为间隔增加轨迹生成时长，重复S5.1，生成参考轨迹；
S5.3：对轨迹进行碰撞检测。
5.如权利要求4所述的基于行车环境的快速自动驾驶轨迹规划方法，其特征在于，若轨迹序列预测时长增加到20秒仍无法生成轨迹，说明此时前方无可行道路，需要为驾驶员发出提醒信号。
6.如权利要求4所述的基于行车环境的快速自动驾驶轨迹规划方法，其特征在于，步骤5.1具体为：
S5.1.1：提取轨迹自然坐标系的初始状态及终点状态的纵向相关参数，生成纵向参考轨迹，若终点状态不存在参考纵向距离，则纵向位移轨迹由4次多项式连接，否则由5次多项式连接；， />，；
其中，/>，/>分别表示规划起点离原点的距离（m），规划起点的速度（/>），规划起点的加速度（/>）；/>，/>，/>分别表示规划终点的参考位移距离（m），规划终点的速度（/>），规划终点的加速度（/>）；t表示规划时域（s），离散时间间隔为dt（s）；/>均为函数参数；
S5.1.2：判断步骤5.1.1生成的参考速度轨迹在第/>时刻时是否超过设定的巡航速度，若超过，则其对应时刻的位移及加速度应分别使用积分和微分求解；
S5.1.3：设定车辆动力学约束，最小行驶速度，最大加速度/>，最大制动加速度，/>；
S5.1.4：判断生成的速度轨迹曲线及加速度轨迹曲线是否满足速度及加速度的动力学约束，若不满足则返回至S5.1进行下一条轨迹的生成判断，否则进入步骤5.1.5；
S5.1.5：提取规划起点及终点的横向相关参数，结合s（t）函数生成自然坐标系横向参考轨迹；，/>；其中/>，/>，分别表示规划起点时自然坐标系下的横向偏移，横向偏移对弧长的一阶导数及二阶导数，l t、l t '、l t ''分别表示在自然坐标系下终点状态的横向偏移、横向偏移对弧长的一阶导数及横向偏移对弧长的二阶导数；/>表示参考位移，与/>一一对应（m）；/>为五次多项式系数；
S5.1.6：判断生成的L（S（t））轨迹在规划的前n秒的序列点是否超过道路的最大边界，若超过道路最大边界，说明该轨迹自车不可达，返回S5.1进行下一条轨迹的筛选，否则进入S5.1.7；
S5.1.7：合并横纵向轨迹生成全局坐标系下带时间的参考轨迹；
S5.1.8：判断第t时刻下的曲率是否满足曲率约束，若满足/>，则重复S5.1.7，直至遍历完前n秒内的所有时间点；若不满足/>，则返回S5.1进行下一条轨迹的筛选。
7.如权利要求6所述的基于行车环境的快速自动驾驶轨迹规划方法，其特征在于，步骤S5.1.7具体包括：
S5.1.7.1：依次找到s（t）中与时间一一对应的位移s在全局参考路径上的投影点信息，其中，x pt 、y pt 、ɵ pt 、k pt分别表示自然坐标系下规划的纵向轨迹点在参考路径的全局坐标系下的X坐标值、Y坐标值、航向角、道路曲率，下标中的t表示规划轨迹点对应的时刻；
S5.1.7.2：计算全局坐标系下第t时刻的参考x，y坐标；
S5.1.7.3：计算全局坐标系下第t时刻的参考航向，；
S5.1.7.4：计算全局坐标系下第t时刻的参考曲率，。
8.如权利要求4所述的快速自动驾驶轨迹规划方法，其特征在于，步骤S5.3具体包括：若当前自车周围不存在障碍物，则输出S5.1.9生成的全局坐标系参考轨迹给控制模块跟踪执行；若当前自车周围存在障碍物，则结合传感器获取的障碍物信息预测障碍物在规划轨迹时域内的运动轨迹，检测障碍物轨迹与规划轨迹在对应时刻是否发生重叠，若不重叠，则认为不发生碰撞，输出S5.1.9生成的轨迹给控制模块跟踪执行；否则，存在碰撞风险，需要返回S5.1进行下一条轨迹筛选。
CN202310805585.3A 2023-07-03 2023-07-03 一种基于行车环境的快速自动驾驶轨迹规划方法 Active CN116513245B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310805585.3A CN116513245B (zh) 2023-07-03 2023-07-03 一种基于行车环境的快速自动驾驶轨迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310805585.3A CN116513245B (zh) 2023-07-03 2023-07-03 一种基于行车环境的快速自动驾驶轨迹规划方法

Publications (2)

Publication Number Publication Date
CN116513245A CN116513245A (zh) 2023-08-01
CN116513245B true CN116513245B (zh) 2023-10-03

Family

ID=87390713

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310805585.3A Active CN116513245B (zh) 2023-07-03 2023-07-03 一种基于行车环境的快速自动驾驶轨迹规划方法

Country Status (1)

Country Link
CN (1) CN116513245B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101894302A (zh) * 2009-05-18 2010-11-24 北京首科软件及系统集成有限责任公司 一种轨迹生成方法
CN101944096A (zh) * 2009-07-08 2011-01-12 广东融讯信息科技有限公司 用于提供公交线路的方法和系统
CN102854880A (zh) * 2012-10-08 2013-01-02 中国矿业大学 面向混合地形区域不确定环境的机器人全局路径规划方法
CN102865874A (zh) * 2011-07-05 2013-01-09 爱信艾达株式会社 导航系统、导航方法及计算机可读存储介质
CN105549597A (zh) * 2016-02-04 2016-05-04 同济大学 一种基于环境不确定性的无人车动态路径规划方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101894302A (zh) * 2009-05-18 2010-11-24 北京首科软件及系统集成有限责任公司 一种轨迹生成方法
CN101944096A (zh) * 2009-07-08 2011-01-12 广东融讯信息科技有限公司 用于提供公交线路的方法和系统
CN102865874A (zh) * 2011-07-05 2013-01-09 爱信艾达株式会社 导航系统、导航方法及计算机可读存储介质
CN102854880A (zh) * 2012-10-08 2013-01-02 中国矿业大学 面向混合地形区域不确定环境的机器人全局路径规划方法
CN105549597A (zh) * 2016-02-04 2016-05-04 同济大学 一种基于环境不确定性的无人车动态路径规划方法

Also Published As

Publication number Publication date
CN116513245A (zh) 2023-08-01

Similar Documents

Publication Publication Date Title
US11467596B2 (en) Target abnormality determination device
CN109855639B (zh) 基于障碍物预测与mpc算法的无人驾驶轨迹规划方法
US10996679B2 (en) Method to evaluate trajectory candidates for autonomous driving vehicles (ADVs)
Göhring et al. Radar/lidar sensor fusion for car-following on highways
CN109002041B (zh) 一种车辆避障方法
CN108536149A (zh) 一种基于Dubins路径的无人驾驶车辆避障控制装置及控制方法
JP2019123377A (ja) 車両制御装置
CN109270933A (zh) 基于圆锥曲线的无人驾驶避障方法、装置、设备及介质
JPWO2018066560A1 (ja) 車両制御装置
JP7112658B2 (ja) 車両運転支援システム及び方法
CN111634292B (zh) 一种用于矿区的碰撞预测方法
CN113848914B (zh) 动态环境下碰撞系数人工势场法局部路径规划方法
CN113050646B (zh) 一种用于室内移动机器人动态环境路径规划方法
WO2019003302A1 (ja) 車両制御装置
JP2020111302A (ja) 車両運転支援システム及び方法
CN116513245B (zh) 一种基于行车环境的快速自动驾驶轨迹规划方法
CN113060118A (zh) 用于柔和降速的速度规划引导线
CN208061025U (zh) 一种基于Dubins路径的无人驾驶车辆避障控制装置
WO2023076343A1 (en) Autonomous vehicle trajectory determination based on state transition model
CN114877904A (zh) 一种道路曲率的估计方法、测量装置和计算机设备
EP3925844B1 (en) Relative speed based speed planning for buffer area
JP2010217988A (ja) 運転支援装置、運転支援方法および運転支援プログラム
WO2021189350A1 (en) A point cloud-based low-height obstacle detection system
JP2018193039A (ja) 車両制御装置
CN116300970B (zh) 车辆自主编队方法及装置

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