CN115416693A - 一种基于时空走廊的自动驾驶轨迹规划方法及系统 - Google Patents

一种基于时空走廊的自动驾驶轨迹规划方法及系统 Download PDF

Info

Publication number
CN115416693A
CN115416693A CN202211211074.0A CN202211211074A CN115416693A CN 115416693 A CN115416693 A CN 115416693A CN 202211211074 A CN202211211074 A CN 202211211074A CN 115416693 A CN115416693 A CN 115416693A
Authority
CN
China
Prior art keywords
time
track
longitudinal
dimensional convex
trajectory
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
CN202211211074.0A
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.)
Chongqing Chang'an Wangjiang Industry Group Co ltd
Chongqing University
University of Shanghai for Science and Technology
Original Assignee
Chongqing Chang'an Wangjiang Industry Group Co ltd
Chongqing University
University of Shanghai for Science and 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 Chongqing Chang'an Wangjiang Industry Group Co ltd, Chongqing University, University of Shanghai for Science and Technology filed Critical Chongqing Chang'an Wangjiang Industry Group Co ltd
Priority to CN202211211074.0A priority Critical patent/CN115416693A/zh
Publication of CN115416693A publication Critical patent/CN115416693A/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
    • B60W60/00—Drive control systems specially adapted for autonomous road vehicles
    • B60W60/001—Planning or execution of driving tasks
    • 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
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • 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
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0019—Control system elements or transfer functions
    • 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
    • B60W2554/00—Input parameters relating to objects
    • B60W2554/40—Dynamic objects, e.g. animals, windblown objects
    • B60W2554/402—Type
    • B60W2554/4029—Pedestrians
    • 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
    • B60W2554/00—Input parameters relating to objects
    • B60W2554/40—Dynamic objects, e.g. animals, windblown objects
    • B60W2554/404—Characteristics
    • B60W2554/4041—Position

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Human Computer Interaction (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及一种基于时空走廊的自动驾驶轨迹规划方法及系统，包括：实时获取自车的位姿信息和自车周围的障碍物；根据所述自车的位姿信息确定所述自车行驶的参考线并构建三维配置空间；在三维配置空间中标识障碍物并在每一目标时间点处截取s‑l平面，得到二维凸子空间；对每相邻两个目标时间点处的所有二维凸子空间构建三维凸子空间；根据自车在所述规划时域内的粗略轨迹和所有三维凸子空间确定轨迹规划的时空走廊；根据时空走廊和所述粗略轨迹进行纵向轨迹的优化和横向轨迹的优化，并根据优化后的纵向轨迹和优化后的横向轨迹确定自动驾驶轨迹。本发明能够得到最优规划轨迹且最优规划轨迹中不存在障碍物，提高了自动驾驶轨迹规划的准确性。

Description

一种基于时空走廊的自动驾驶轨迹规划方法及系统
技术领域
本发明涉及自动驾驶轨迹规划技术领域，特别是涉及一种基于时空走廊的自动驾驶轨迹规划方法及系统。
背景技术
自动驾驶轨迹规划的任务主要是利用地图数据、感知数据、定位数据以及车辆状态等信息，计算出一个无碰撞可执行的轨迹，保证自动驾驶车辆从起点安全地驾驶到目的地，并尽可能高效。其问题的本质是一个多目标的数学优化问题，其主要的优化目标主要包括：安全性、稳定性、舒适性、驾驶效率，然而在实际的驾驶场景中，还应包括车辆的加减速度约束、非完整性约束以及动力学约束等各种物理约束。
若不考虑自动驾驶车辆在z方向的移动，轨迹规划问题是一个三维的问题，即规划模块输出的轨迹应该是位置关于时间的函数。目前主要有两种自动驾驶轨迹规划框架：在Frenét坐标系下的解耦规划以及直接在三维时空下的轨迹规划。前者将三维规划解耦为路径规划和速度规划(或者纵向规划和横向规划)，然后合并为一条时空轨迹。这种方法虽然减少了规划问题的复杂度，但是缺少特定维度的信息，容易产生次优解。后者则直接考虑时空中的机动，直接在三维空间中处理规划问题，因此结果一般是全局最优的。虽然三维轨迹规划器的速度比解耦方法慢，但它能更好地平衡计算速度和求解质量之间的关系。
现有技术无论是在笛卡尔坐标系还是在Frenét下进行规划，大多均采用采样的方法获得轨迹。然而，通过采样得到的轨迹均是次优的，无法满足最优性，且可能存在原本存在可行的轨迹，但因其并未被采样到，从而产生规划失败的可能。除此之外，采样的方法并不能确定采样轨迹之间是否存在障碍物的情况，因其只能检测被采样轨迹的安全性，故若采样轨迹之间存在不确定性的微小障碍物，其无法处理。
发明内容
本发明的目的是提供一种基于时空走廊的自动驾驶轨迹规划方法及系统，通过获取时空走廊进行轨迹规划，能够得到最优的规划轨迹，并且得到的最优规划轨迹中不会存在有障碍物的情况，提高了自动驾驶轨迹规划的准确性。
为实现上述目的，本发明提供了如下方案：
一种基于时空走廊的自动驾驶轨迹规划方法，包括：
实时获取自车的位姿信息和自车周围的障碍物；
根据所述自车的位姿信息确定所述自车行驶的参考线，将所述参考线由笛卡尔坐标系转换到Frenét坐标系下，基于所述Frenét坐标系的s轴和l轴并引入时间t轴构建三维配置空间；所述s轴、所述l轴和所述t轴相互垂直；
根据所述障碍物出现的时间将规划时域内的所述障碍物在所述三维配置空间中进行表示；
在每一目标时间点处截取所述三维配置空间的s-l平面，得到每一所述目标时间点处的二维凸子空间；截取的所述s-l平面中除所述障碍物构成区域外的其他区域包括多个所述二维凸子空间；所述目标时间点为至少一个所述障碍物首次出现的时间点和/或至少一个所述障碍物末次出现的时间点或所述规划时域内的时间边界点；
对每相邻两个所述目标时间点处的所有所述二维凸子空间根据位置对应关系进行匹配并由每一匹配的二维凸子空间构成三维凸子空间；
根据所述自车在所述规划时域内的粗略轨迹和所有所述三维凸子空间确定轨迹规划的时空走廊；
根据所述时空走廊和所述粗略轨迹进行纵向轨迹的优化和横向轨迹的优化，并根据优化后的纵向轨迹和优化后的横向轨迹确定自动驾驶轨迹。
可选的，所述在每一目标时间点处截取所述三维配置空间的s-l平面，得到每一所述目标时间点处的二维凸子空间，具体包括：
在所述规划时域内的时间边界点处分别进行一次所述s-l平面的截取，得到起始时间点处的所述s-l平面和终止时间点处的所述s-l平面；所述起始时间点处的所述s-l平面中包括所述起始时间处存在的所有所述障碍物对应的区域；所述终止时间点处的所述s-l平面包括所述终止时间处存在的所有所述障碍物对应的区域；
在每一所述目标时间点tj处分别进行两次所述s-l平面的截取；
当所述目标时间点tj处为所述障碍物首次出现的时间点，则第一次截取的所述s-l平面中不包括首次出现的所述障碍物，第二次截取的所述s-l平面中包括首次出现的所述障碍物；
当所述目标时间点处为所述障碍物末次出现的时间点，则第一次截取的所述s-l平面中包括末次出现的所述障碍物，第二次截取的所述s-l平面中不包括末次出现的所述障碍物。
可选的，所述对每相邻两个所述目标时间点处的所有所述二维凸子空间根据位置对应关系进行匹配并由每一匹配的二维凸子空间构成三维凸子空间，具体包括：
定义所述目标时间点tj处，第一次截取的所述二维凸子空间记为目标时间点tj，1处的所述二维凸子空间，第二次截取的所述二维凸子空间记为目标时间点tj，2处的所述二维凸子空间；j＝1，...，m；
将所述目标时间点tj，1处的所有所述二维凸子空间与目标时间点tj-1，2处的所有所述二维凸子空间根据位置对应关系进行匹配并由每一匹配的二维凸子空间构成三维凸子空间；当j＝1时，所述目标时间点tj-1，2处的所有所述二维凸子空间为所述起始时间点处的所述s-l平面包括的所述二维凸子空间；
将所述目标时间点tj，2处的所有所述二维凸子空间与目标时间点tj+1，1处的所有所述二维凸子空间根据位置对应关系进行匹配并由每一匹配的二维凸子空间构成三维凸子空间；当j＝m时，所述目标时间点tj+1，1处的所有所述二维凸子空间为所述终止时间点处的所述s-l平面包括的所述二维凸子空间。
可选的，所述根据所述自车在所述规划时域内的粗略轨迹和所有所述三维凸子空间确定轨迹规划的时空走廊，具体包括：
对每一所述三维凸子空间，判断所述三维凸子空间中是否存在粗略轨迹点；
根据所有存在所述粗略轨迹点的所述三维凸子空间构建所述时空走廊。
可选的，所述根据所述时空走廊和所述粗略轨迹进行纵向轨迹的优化和横向轨迹的优化，具体包括：
将所述时空走廊和所述粗略轨迹沿所述配置空间的l轴投影到s-t平面，得到S-T图；
对所述S-T图的时间轴进行离散得到离散时间点；
对每一所述离散时间点处的纵向位置、纵向速度和纵向加速度依据纵向轨迹优化代价函数、纵向轨迹不等式约束和纵向轨迹等式约束进行优化，得到所述优化后的纵向轨迹；
将所述时空走廊和所述粗略轨迹沿所述配置空间的s轴投影到l-t平面，得到L-T图；
对所述L-T图的时间轴进行离散得到离散时间点；
对每一所述离散时间点处的横向位置、横向速度和横向加速度依据横向轨迹优化代价函数、横向轨迹不等式约束和横向轨迹等式约束进行优化，得到所述优化后的横向轨迹。
可选的，所述纵向轨迹优化代价函数的表达式为：
Figure BDA0003875134070000041
式中，
Figure BDA0003875134070000042
wac、
Figure BDA0003875134070000043
与
Figure BDA0003875134070000044
分别为纵向参考位置Sref、纵向参考速度
Figure BDA0003875134070000045
离心加速度、纵向加速度与纵向加加速度的权重项；κr(si)表示在Si处的参考曲率；Si为时间ti处自车的纵向位置；
Figure BDA0003875134070000046
为时间ti处自车的纵向速度；
Figure BDA0003875134070000047
为时间ti处自车的纵向加速度；
Figure BDA0003875134070000048
为时间ti处自车的纵向加加速度；n为规划时域的时间点总数；
所述横向轨迹优化代价函数的表达式为：
Figure BDA0003875134070000049
式中，wl、
Figure BDA00038751340700000410
以及wobs分别表示横向位置、横向速度、横向加速度、横向加加速度、与横向参考位置lref的距离以及与障碍物的距离的权重项；li为时间ti处自车的横向位置；
Figure BDA0003875134070000051
为时间ti处自车的横向速度；
Figure BDA0003875134070000052
为时间ti处自车的横向加速度；
Figure BDA0003875134070000053
为时间ti处自车的横向加加速度；
Figure BDA0003875134070000054
与
Figure BDA0003875134070000055
分别为li的下界与上界。
可选的，所述纵向轨迹不等式约束的表达式为：
Figure BDA0003875134070000056
Figure BDA0003875134070000057
Figure BDA0003875134070000058
Figure BDA0003875134070000059
Figure BDA00038751340700000510
式中，
Figure BDA00038751340700000511
与
Figure BDA00038751340700000512
分别为Si的下界与上界；
Figure BDA00038751340700000513
为规划的最高限速；
Figure BDA00038751340700000514
知
Figure BDA00038751340700000515
分别为规划的最小线加速度和最大线加速度；
Figure BDA00038751340700000516
知
Figure BDA00038751340700000517
分别为规划的最小纵向加加速度和最大纵向加加速度；
Figure BDA00038751340700000518
为最大向心加速度；
所述横向轨迹不等式约束的表达式为：
Figure BDA00038751340700000519
Figure BDA00038751340700000520
Figure BDA00038751340700000521
Figure BDA00038751340700000522
tan(αmax)*κr(li)*li-tan(αmax)+|κr(li)|*L≤0
式中，
Figure BDA00038751340700000523
和
Figure BDA00038751340700000524
分别为规划的最小横向速度和最大横向速度；
Figure BDA00038751340700000525
和
Figure BDA00038751340700000526
分别为规划的最小横向加速度和最大横向加速度；
Figure BDA00038751340700000527
和
Figure BDA00038751340700000528
分别为规划的最小横向加加速度和最大横向加加速度；αmax为自车最大的转向角；L为自车的轴距；κr(li)表示在li处的参考曲率。
可选的，所述纵向轨迹等式约束的表达式为：
Figure BDA0003875134070000061
Figure BDA0003875134070000062
Figure BDA0003875134070000063
Figure BDA0003875134070000064
所述横向轨迹等式约束的表达式为：
Figure BDA0003875134070000065
Figure BDA0003875134070000066
Figure BDA0003875134070000067
Figure BDA0003875134070000068
可选的，所述根据优化后的纵向轨迹和优化后的横向轨迹确定自动驾驶轨迹，具体包括：
合并所述优化后的纵向轨迹和所述优化后的横向轨迹得到所述Frenét坐标系下的轨迹；
对所述Frenét坐标系下的轨迹进行安全检查，并将通过安全检查的所述Frenét坐标系下的轨迹变换到笛卡尔坐标系下，得到所述自动驾驶轨迹。
本发明还提供一种基于时空走廊的自动驾驶轨迹规划系统，包括：
信息获取模块，用于实时获取自车的位姿信息和自车周围的障碍物；
三维配置空间构建模块，用于根据所述自车的位姿信息确定所述自车行驶的参考线，将所述参考线由笛卡尔坐标系转换到Frenét坐标系下，基于所述Frenét坐标系的s轴和l轴并引入时间t轴构建三维配置空间；所述s轴、所述l轴和所述t轴相互垂直；
障碍物表示模块，用于根据所述障碍物出现的时间将规划时域内的所述障碍物在所述三维配置空间中进行表示；
平面截取模块，用于在每一目标时间点处截取所述三维配置空间的s-l平面，得到每一所述目标时间点处的二维凸子空间；截取的所述s-l平面中除所述障碍物构成区域外的其他区域包括多个所述二维凸子空间；所述目标时间点为至少一个所述障碍物首次出现的时间点和/或至少一个所述障碍物末次出现的时间点或所述规划时域内的时间边界点；
三维凸子空间构建模块，用于对每相邻两个所述目标时间点处的所有所述二维凸子空间根据位置对应关系进行匹配并由每一匹配的二维凸子空间构成三维凸子空间；
时空走廊构建模块，用于根据所述自车在所述规划时域内的粗略轨迹和所有所述三维凸子空间确定轨迹规划的时空走廊；
轨迹规划模块，用于根据所述时空走廊和所述粗略轨迹进行纵向轨迹的优化和横向轨迹的优化，并根据优化后的纵向轨迹和优化后的横向轨迹确定自动驾驶轨迹。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明涉及一种基于时空走廊的自动驾驶轨迹规划方法及系统，包括：实时获取自车的位姿信息和自车周围的障碍物；根据所述自车的位姿信息确定所述自车行驶的参考线并构建三维配置空间；在三维配置空间中标识障碍物并在每一目标时间点处截取s-l平面，得到二维凸子空间；对每相邻两个目标时间点处的所有二维凸子空间构建三维凸子空间；根据自车在所述规划时域内的粗略轨迹和所有三维凸子空间确定轨迹规划的时空走廊；根据时空走廊和所述粗略轨迹进行纵向轨迹的优化和横向轨迹的优化，并根据优化后的纵向轨迹和优化后的横向轨迹确定自动驾驶轨迹。本发明能够得到最优规划轨迹且最优规划轨迹中不存在障碍物，提高了自动驾驶轨迹规划的准确性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例1提供的一种基于时空走廊的自动驾驶轨迹规划方法流程图；
图2为本发明实施例1提供的Frenét坐标系示意图；
图3为本发明实施例1提供的已进行障碍物表示的slt配置空间；
图4为本发明实施例1提供的t2时间点处截取的s-l平面中的障碍物区域和二维凸子空间；
图5为本发明实施例1提供的t0与t2之间构造的三维凸子空间；
图6为本发明实施例1提供的时空走廊示意图；
图7为本发明实施例1提供的S-T图；
图8为本发明实施例1提供的L-T图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
现有的采样方法只能适用于较简单的场景，如高速公路或简单的城市道路，在复杂的场景中很难采样得到可行的轨迹。
本发明的目的是提供一种基于时空走廊的自动驾驶轨迹规划方法及系统，通过获取时空走廊进行轨迹规划，能够得到最优的规划轨迹，并且得到的最优规划轨迹中不会存在有障碍物的情况，提高了自动驾驶轨迹规划的准确性。并且本发明能够在复杂场景下规划准确规划自动驾驶轨迹。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
实施例1
如图1所示，本实施例提供一种基于时空走廊的自动驾驶轨迹规划方法，包括：
S1：实时获取自车的位姿信息和自车周围的障碍物。
除了获取自车的位姿信息，还包括获取自车的几何形状、速度、加速度、底盘等信息。
自车周围的障碍物包括障碍物的位姿，几何形状，类型(如行人、车辆等)速度，预测轨迹等信息。其中，障碍物的预测轨迹由上层预测模块给出。
S2：根据所述自车的位姿信息确定所述自车行驶的参考线，将所述参考线由笛卡尔坐标系转换到Frenét坐标系下，基于所述Frenét坐标系的s轴和l轴并引入时间t轴构建三维配置空间。所述S轴、所述l轴和所述t轴相互垂直。
如图2所示的Frenét坐标系中，自车状态(x，y，θ，κ，υ，a)分别代表位置、朝向、转角、速度和加速度。
Figure BDA0003875134070000091
代表沿着参考线的纵向状态(即纵向位置、速度和加速度)，
Figure BDA0003875134070000092
代表垂直参考线的横向状态(横向位置、速度和加速度)。s轴就代表参考线，由路由模块提供，参考线的纵向状态为自车沿着参考线的纵向位置、速度和加速度。
参考线由路由模块确定，基于所述Frenét坐标系的s轴和l轴并引入时间t轴构建三维配置空间。所述s轴、所述l轴和所述t轴相互垂直。如图3所示，由s轴、l轴、t轴以及各轴的边界值共同构成一个长方体的三维配置空间。
S3：根据所述障碍物出现的时间将规划时域内的所述障碍物在所述三维配置空间中进行表示。
对于动态障碍物本方法仅考虑在规划时域内，动态障碍物为匀速运动的情况。每个障碍物在时间t处均可以被建模为一个在Frenét坐标系平行与s轴的矩形。为了便于安全区域的表示，矩形沿着l(s)轴的长度等于障碍物的宽度(长度)加上自车的宽度(长度)的一半。
在本发明所考虑的slt配置空间中，动态障碍物可以被建模为一个出现在时间段[tstart，tend]范围内的梯形体，静态障碍物可以被建模为一个贯穿整个时间轴的长方体。此外，本发明所建立的配置空间可以很容易加入道路中的语义元素(红绿灯、停车线、减速区域等)。如图3所示，图中动态车辆被表示为出现时间段[t2，t3]和[t0，t3]的两个梯形体，行人被表示为出现时间段[t0，t3]的一个梯形体，交通灯被表示为出现在时间段[t1，t3]的静态障碍物，而静态障碍物被表示为出现在[t0，t3]的一个长方体。
根据障碍物的预测信息，可以得到障碍物在某一个时刻的出现的位置，可以将其表示为在s-l坐标系下的一个不同的梯形体或长方体。如图3所示。
S4：在每一目标时间点处截取所述三维配置空间的s-l平面，得到每一所述目标时间点处的二维凸子空间；截取的所述s-l平面中除所述障碍物构成区域外的其他区域包括多个所述二维凸子空间；所述目标时间点为至少一个所述障碍物首次出现的时间点和/或至少一个所述障碍物末次出现的时间点或所述规划时域内的时间边界点。
在给定S3中配置空间中的障碍物表示以后，在障碍物出现的时间点处(如图3中的t0，t1，t2与t3)，对所有障碍物与配置空间所构成的s-l平面进行分解，从而得到在时间ti处的一系列二维凸子空间，如图4所示。在图4中示出了6个二维凸子空间。
其中，步骤S4具体包括：
S41：在所述规划时域内的时间边界点处分别进行一次所述s-l平面的截取，得到起始时间点处的所述s-l平面和终止时间点处的所述s-l平面；所述起始时间点处的所述s-l平面中包括所述起始时间处存在的所有所述障碍物对应的区域；所述终止时间点处的所述s-l平面包括所述终止时间处存在的所有所述障碍物对应的区域。
S42：在每一所述目标时间点tj处分别进行两次所述s-l平面的截取。
S43：当所述目标时间点tj处为所述障碍物首次出现的时间点，则第一次截取的所述s-l平面中不包括首次出现的所述障碍物，第二次截取的所述s-l平面中包括首次出现的所述障碍物。
S44：当所述目标时间点处为所述障碍物末次出现的时间点，则第一次截取的所述s-l平面中包括末次出现的所述障碍物，第二次截取的所述s-l平面中不包括末次出现的所述障碍物。
为了便于说明本实施例的方案，以图3中虚线框内的元素进行分解为例说明生成的二维凸子空间。实际情况下，不仅仅只对虚线框内的元素进行分解，需要对整个配置空间内部的所有元素进行分解。
在t2时刻，图3中的第一个动态车辆首次出现，t2时刻为一个目标时间点，在该时间点处，第一次截取时，不考虑第一个动态车辆的出现，截取的s-l平面中仅包括第二个动态车辆(障碍物)所在的区域，所以第一次截取s-l平面中包括一个障碍物区域，四个二维凸子空间区域，如图5所示，t2时刻第一次截取的s-l平面对应着图5中t2-0时刻的s-l平面。第二次截取时，考虑第一个动态车辆的出现，所以第二次截取的s-l平面中包括两个障碍物区域，六个二维凸子空间区域，如图4所示和图5所示，图5中对应的是t2-1时刻的s-l平面。
当某一时间点处有障碍物最后一次出现，则该时间点也为目标时间点，截取s-l平面的方式与有障碍物首次出现时两次截取方式相反。当某一时间点处有障碍物最后一次出现时，第一次截取的s-l平面中需要包括最后一次出现的障碍物，而第二次截取的s-l平面中则忽略最后一次出现的障碍物。
S5：对每相邻两个所述目标时间点处的所有所述二维凸子空间根据位置对应关系进行匹配并由每一匹配的二维凸子空间构成三维凸子空间。
其中，步骤S5具体包括：
S51：定义所述目标时间点tj处，第一次截取的所述二维凸子空间记为目标时间点tj，1处的所述二维凸子空间，第二次截取的所述二维凸子空间记为目标时间点tj，2处的所述二维凸子空间；j＝1，…，m。
S52：将所述目标时间点tj，1处的所有所述二维凸子空间与目标时间点tj-1，2处的所有所述二维凸子空间根据位置对应关系进行匹配并由每一匹配的二维凸子空间构成三维凸子空间；当j＝1时，所述目标时间点tj-1，2处的所有所述二维凸子空间为所述起始时间点处的所述s-l平面包括的所述二维凸子空间。
S53：将所述目标时间点tj，2处的所有所述二维凸子空间与目标时间点tj+1，1处的所有所述二维凸子空间根据位置对应关系进行匹配并由每一匹配的二维凸子空间构成三维凸子空间；当j＝m时，所述目标时间点tj+1，1处的所有所述二维凸子空间为所述终止时间点处的所述s-l平面包括的所述二维凸子空间。
为了便于本领域技术人员理解本实施例构建三维凸子空间的过程，结合图5中的t2时刻的二维凸子空间和起始时刻t0的二维凸子空间构建三维凸子空间。
结合图3中的虚线框内的部分进行说明，起始时刻t0的s-l平面中包括一个障碍物区域和4个二维凸子空间。将起始时刻t0的4个二维凸子空间和t2时刻第一次截取的s-l平面(t2-1时刻的s-l平面)中4个二维凸子空间依据位置对应关系，得到了4个三维凸子空间，如图5所示，图5中的4个三维凸子空间均为梯形体。t2时刻第二次截取的s-l平面(t2-2时刻的s-l平面)中包括6个二维凸子空间。
在虚线框内，障碍物首次出现的时间为t2时刻，而t0和t3为时间边界，需要在t0、t2和t3处均截取s-l平面。图5仅说明了t0与t2之间如何构造梯形体，t2与t3处的构造方法与t0与t2处相同。
在虚线框内，由于t1时刻既不存在障碍物首次出现，也不存在障碍物末次出现，所以t1时刻不能视为目标时间点，所以不对t1时刻截取s-l平面。但是若从整个配置空间上考虑，由于t1时刻时，交通灯首次出现，此时t1时刻为目标时间点，需要对t1时刻截取s-l平面。
S6：根据所述自车在所述规划时域内的粗略轨迹和所有所述三维凸子空间确定轨迹规划的时空走廊。
对所有的三维凸子空间进行遍历，对于每一个三维凸子空间，对粗略轨迹进行遍历，然后判断每一个粗略轨迹点是否位于三维凸子空间中，如果粗略轨迹点位于三维凸子空间，则所有存在粗略轨迹点的三维凸子空间(梯形体)构成了时空走廊。具体的，步骤S6包括：
对每一所述三维凸子空间，判断所述三维凸子空间中是否存在粗略轨迹点；
根据所有存在所述粗略轨迹点的所述三维凸子空间构建所述时空走廊。
如图6(a)中，t0到t3所有二维凸子空间合并得到的三维凸子空间。如图6(b)中虚线框的部分为时空走廊。
S7：根据所述时空走廊和所述粗略轨迹进行纵向轨迹的优化和横向轨迹的优化，并根据优化后的纵向轨迹和优化后的横向轨迹确定自动驾驶轨迹。
在得到时空走廊后，提出了一种在时空走廊内部优化轨迹的优化方法。使用分段加加速度的轨迹形式来生成二维轨迹(包含纵向轨迹s(t)和横向轨迹l(t))。
对于S6中所述的分段加加速度的轨迹形式，本方法使用具有一个分辨率Δt的时间t来离散纵向轨迹和横向轨迹。假设起始点的时间t＝0，则纵向(横向)轨迹可以表示为一下形式：
Figure BDA0003875134070000121
其中，在进行纵向(横向)轨迹优化时，使用符号s(l)代替符号x。因此，优化问题的变量包含每个离散点的纵向(横向)位置s(l)，速度
Figure BDA0003875134070000122
以及加速度
Figure BDA0003875134070000131
此外，本方法使用一个加加速度常量项
Figure BDA0003875134070000132
来连接两个相邻的离散点，因此这些离散点之间的等式约束可以表示为：
Figure BDA0003875134070000133
Figure BDA0003875134070000134
Figure BDA0003875134070000135
Figure BDA0003875134070000136
Figure BDA0003875134070000137
因此，在给定分段加加速度的轨迹形式下，纵向轨迹与横向轨迹均可以使用该轨迹形式进行表达，然后通过离散时间t对二者进行合并。
所述纵向轨迹等式约束的表达式为：
Figure BDA0003875134070000138
Figure BDA0003875134070000139
Figure BDA00038751340700001310
Figure BDA00038751340700001311
所述横向轨迹等式约束的表达式为：
Figure BDA00038751340700001312
Figure BDA00038751340700001313
Figure BDA00038751340700001314
Figure BDA00038751340700001315
具体的，步骤S7中，根据所述时空走廊和所述粗略轨迹进行纵向轨迹的优化和横向轨迹的优化具体包括：
S711：将所述时空走廊和所述粗略轨迹沿所述配置空间的l轴投影到s-t平面，得到S-T图，如图7所示。
S712：对所述S-T图的时间轴进行离散得到离散时间点。通过以分辨率Δt离散时间t。
S713：对每一所述离散时间点处的纵向位置、纵向速度和纵向加速度依据纵向轨迹优化代价函数、纵向轨迹不等式约束和纵向轨迹等式约束进行优化，得到所述优化后的纵向轨迹。
其中，所述纵向轨迹优化代价函数的表达式为：
Figure BDA0003875134070000141
式中，
Figure BDA0003875134070000142
wac、
Figure BDA0003875134070000143
与
Figure BDA0003875134070000144
分别为纵向参考位置Sref、纵向参考速度
Figure BDA0003875134070000145
离心加速度、纵向加速度与纵向加加速度的权重项；κr(si)表示在Si处的参考曲率；Si为时间ti处自车的纵向位置；
Figure BDA0003875134070000146
为时间ti处自车的纵向速度；
Figure BDA0003875134070000147
为时间ti处自车的纵向加速度；
Figure BDA0003875134070000148
为时间ti处自车的纵向加加速度；n为规划时域的时间点总数；
其中，式中的1-2项分别鼓励纵向轨迹快速到达一个纵向参考位置sref和纵向参考速度
Figure BDA0003875134070000149
3-5项分别限制了轨迹的离心加速度、线加速度和加加速度的大小。
所述纵向轨迹不等式约束的表达式为：
Figure BDA0003875134070000151
Figure BDA0003875134070000152
Figure BDA0003875134070000153
Figure BDA0003875134070000154
Figure BDA0003875134070000155
式中，
Figure BDA0003875134070000156
与
Figure BDA0003875134070000157
分别为Si的下界与上界(如图7中箭头所示)；
Figure BDA0003875134070000158
为规划的最高限速；
Figure BDA0003875134070000159
知
Figure BDA00038751340700001510
分别为规划的最小线加速度和最大线加速度；
Figure BDA00038751340700001511
和
Figure BDA00038751340700001512
分别为规划的最小纵向加加速度和最大纵向加加速度；
Figure BDA00038751340700001513
为最大向心加速度，假设其为常量。
S714：将所述时空走廊和所述粗略轨迹沿所述配置空间的s轴投影到l-t平面，得到L-T图，如图8所示。
S715：对所述L-T图的时间轴进行离散得到离散时间点。
S716：对每一所述离散时间点处的横向位置、横向速度和横向加速度依据横向轨迹优化代价函数、横向轨迹不等式约束和横向轨迹等式约束进行优化，得到所述优化后的横向轨迹。
根据所获得到的时空走廊，可以很容易地确定每个离散点(ti，si)位于时空走廊中的哪一个梯形体内部，从而可以获得离散点(ti，si)处的横向边界(是用来确定横向轨迹的离散点形式的不等式约束)，如图8所示。
所述横向轨迹优化代价函数的表达式为：
Figure BDA00038751340700001514
式中，wl、
Figure BDA0003875134070000161
以及wobs分别表示横向位置、横向速度、横向加速度、横向加加速度、与横向参考位置lref的距离以及与障碍物的距离的权重项；li为时间ti处自车的横向位置；
Figure BDA0003875134070000162
为时间ti处自车的横向速度；
Figure BDA0003875134070000163
为时间ti处自车的横向加速度；
Figure BDA0003875134070000164
为时间ti处自车的横向加加速度；
Figure BDA0003875134070000165
与
Figure BDA0003875134070000166
分别为li的下界与上界(如图8中箭头所示)。
式中的1-4项分别限制了横向轨迹的位置、速度、加速度以及加加速度的大小；第5项鼓励横向轨迹的位置尽可能地接近参考位置lref；第6项鼓励横向轨迹尽量远离障碍物。
所述横向轨迹不等式约束的表达式为：
Figure BDA0003875134070000167
Figure BDA0003875134070000168
Figure BDA0003875134070000169
Figure BDA00038751340700001610
tan(αmax)*κr(li)*li-tan(αmax)+|κr(li)|*L≤0
式中，
Figure BDA00038751340700001611
知
Figure BDA00038751340700001612
分别为规划的最小横向速度和最大横向速度；
Figure BDA00038751340700001613
知
Figure BDA00038751340700001614
分别为规划的最小横向加速度和最大横向加速度；
Figure BDA00038751340700001615
知
Figure BDA00038751340700001616
分别为规划的最小横向加加速度和最大横向加加速度；αmax为自车最大的转向角；L为自车的轴距；κr(li)表示在li处的参考曲率。
纵向轨迹优化与横向轨迹优化均可表示成一个二次规划形式，如下：
Figure BDA00038751340700001617
xTPx+qTx
s.t.l≤Acx≤u
其中，式中的x为S7横向轨迹优化中的l(s)(或者S7纵向轨迹优化中的s(t))，P为横向轨迹优化(或纵向轨迹优化)中代价函数的二次项，qm为横向轨迹优化(或纵向轨迹优化)中代价函数的线性项，l与u分别为横向轨迹优化(或纵向轨迹优化)中约束条件的下边界与上边界，其中等式约束的下边界与上边界均为0，Ac为横向轨迹优化(或纵向轨迹优化)中约束条件形成的约束矩阵。
步骤S7中，所述根据优化后的纵向轨迹和优化后的横向轨迹确定自动驾驶轨迹，具体包括：
S721：合并所述优化后的纵向轨迹和所述优化后的横向轨迹得到所述Frenét坐标系下的轨迹。
S722：对所述Frenét坐标系下的轨迹进行安全检查，并将通过安全检查的所述Frenét坐标系下的轨迹变换到笛卡尔坐标系下，得到所述自动驾驶轨迹。
按一定分辨率对时间t进行离散，得到一系列时间点ti；根据S7中的曲线，可以得到时间ti处的纵向位移si、纵向速度
Figure BDA0003875134070000171
以及纵向加速度
Figure BDA0003875134070000172
以及根据S8中的曲线，可以得到时间ti处的横向位移li、横向速度
Figure BDA0003875134070000173
以及横向加速度
Figure BDA0003875134070000174
从而得到在Frenét坐标系下的一个轨迹点
Figure BDA0003875134070000175
然后对每一轨迹点进行安全检查：检查每个轨迹点处的速度、加速度、加加速度、曲率等约束是否超过限制；在上述条件均不超过限制的情况下检查每个轨迹点是否与障碍物发生碰撞。
最后，将安全的可行轨迹轨迹点转换到笛卡尔坐标系下得到笛卡尔坐标系下的一个轨迹点(ti，xi，yi，si，θi，κi，vi，ai)，其中xi与yi为笛卡尔坐标系下的位姿，si与Frenét坐标系下的含义相同，θi为笛卡尔坐标下的朝向，κi为曲率，vi与ai分别为速度与加速度。笛卡尔坐标系下的可行轨点发送给控制模块。
本实施例中，提出了一种基于时空走廊的自动驾驶轨迹规划方法，通过对配置空间中的障碍物进行分解，从而得到一系列三维的凸子空间(梯形体)；随后根据粗略轨迹，从所得到的一系列凸子空间中得到一条可行的时空走廊；然后将时空走廊和粗略轨迹分别进行纵向和横向投影，然后进行纵向优化和横向优化；最后将横向轨迹和纵向轨迹进行合并得到时空轨迹。本方法利用障碍物的预测信息，快速地构建一条可行的时空走廊，并且将三维的规划问题转换为两个二维的二次规划问题进行求解，能够在满足最优性的前提下大大地降低规划问题的复杂度以及确保规划的实时性和安全性。解决了结构化环境下由于受到交通规则、非完整性以及避碰约束等限制，使得自动驾驶轨迹规划问题的三维可行集为非凸，从而使得在线实时求解原始轨迹规划问题非常问难的问题。
实施例2
本实施例提供一种基于时空走廊的自动驾驶轨迹规划的系统，包括：
信息获取模块M1，用于实时获取自车的位姿信息和自车周围的障碍物。
三维配置空间构建模块M2，用于根据所述自车的位姿信息确定所述自车行驶的参考线，将所述参考线由笛卡尔坐标系转换到Frenét坐标系下，基于所述Frenét坐标系的s轴和l轴并引入时间t轴构建三维配置空间；所述s轴、所述l轴和所述t轴相互垂直。
障碍物表示模块M3，用于根据所述障碍物出现的时间将规划时域内的所述障碍物在所述三维配置空间中进行表示。
平面截取模块M4，用于在每一目标时间点处截取所述三维配置空间的s-1平面，得到每一所述目标时间点处的二维凸子空间；截取的所述s-1平面中除所述障碍物构成区域外的其他区域包括多个所述二维凸子空间；所述目标时间点为至少一个所述障碍物首次出现的时间点和/或至少一个所述障碍物末次出现的时间点或所述规划时域内的时间边界点。
三维凸子空间构建模块M5，用于对每相邻两个所述目标时间点处的所有所述二维凸子空间根据位置对应关系进行匹配并由每一匹配的二维凸子空间构成三维凸子空间。
时空走廊构建模块M6，用于根据所述自车在所述规划时域内的粗略轨迹和所有所述三维凸子空间确定轨迹规划的时空走廊。
轨迹规划模块M7，用于根据所述时空走廊和所述粗略轨迹进行纵向轨迹的优化和横向轨迹的优化，并根据优化后的纵向轨迹和优化后的横向轨迹确定自动驾驶轨迹。
对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种基于时空走廊的自动驾驶轨迹规划方法，其特征在于，包括：
实时获取自车的位姿信息和自车周围的障碍物；
根据所述自车的位姿信息确定所述自车行驶的参考线，将所述参考线由笛卡尔坐标系转换到Frenét坐标系下，基于所述Frenét坐标系的s轴和l轴并引入时间t轴构建三维配置空间；所述s轴、所述l轴和所述t轴相互垂直；
根据所述障碍物出现的时间将规划时域内的所述障碍物在所述三维配置空间中进行表示；
在每一目标时间点处截取所述三维配置空间的s-l平面，得到每一所述目标时间点处的二维凸子空间；截取的所述s-l平面中除所述障碍物构成区域外的其他区域包括多个所述二维凸子空间；所述目标时间点为至少一个所述障碍物首次出现的时间点和/或至少一个所述障碍物末次出现的时间点或所述规划时域内的时间边界点；
对每相邻两个所述目标时间点处的所有所述二维凸子空间根据位置对应关系进行匹配并由每一匹配的二维凸子空间构成三维凸子空间；
根据所述自车在所述规划时域内的粗略轨迹和所有所述三维凸子空间确定轨迹规划的时空走廊；
根据所述时空走廊和所述粗略轨迹进行纵向轨迹的优化和横向轨迹的优化，并根据优化后的纵向轨迹和优化后的横向轨迹确定自动驾驶轨迹。
2.根据权利要求1所述的方法，其特征在于，所述在每一目标时间点处截取所述三维配置空间的s-l平面，得到每一所述目标时间点处的二维凸子空间，具体包括：
在所述规划时域内的时间边界点处分别进行一次所述s-l平面的截取，得到起始时间点处的所述s-l平面和终止时间点处的所述s-l平面；所述起始时间点处的所述s-l平面中包括所述起始时间处存在的所有所述障碍物对应的区域；所述终止时间点处的所述s-l平面包括所述终止时间处存在的所有所述障碍物对应的区域；
在每一所述目标时间点tj处分别进行两次所述s-l平面的截取；
当所述目标时间点ti处为所述障碍物首次出现的时间点，则第一次截取的所述s-l平面中不包括首次出现的所述障碍物，第二次截取的所述s-l平面中包括首次出现的所述障碍物；
当所述目标时间点处为所述障碍物末次出现的时间点，则第一次截取的所述s-l平面中包括末次出现的所述障碍物，第二次截取的所述s-l平面中不包括末次出现的所述障碍物。
3.根据权利要求2所述的方法，其特征在于，所述对每相邻两个所述目标时间点处的所有所述二维凸子空间根据位置对应关系进行匹配并由每一匹配的二维凸子空间构成三维凸子空间，具体包括：
定义所述目标时间点tj处，第一次截取的所述二维凸子空间记为目标时间点tj，1处的所述二维凸子空间，第二次截取的所述二维凸子空间记为目标时间点tj，2处的所述二维凸子空间；j＝1，...，m；
将所述目标时间点tj，1处的所有所述二维凸子空间与目标时间点tj-1，2处的所有所述二维凸子空间根据位置对应关系进行匹配并由每一匹配的二维凸子空间构成三维凸子空间；当j＝1时，所述目标时间点tj-1，2处的所有所述二维凸子空间为所述起始时间点处的所述s-l平面包括的所述二维凸子空间；
将所述目标时间点tj，2处的所有所述二维凸子空间与目标时间点tj+1，1处的所有所述二维凸子空间根据位置对应关系进行匹配并由每一匹配的二维凸子空间构成三维凸子空间；当j＝m时，所述目标时间点tj+1，1处的所有所述二维凸子空间为所述终止时间点处的所述s-l平面包括的所述二维凸子空间。
4.根据权利要求1所述的方法，其特征在于，所述根据所述自车在所述规划时域内的粗略轨迹和所有所述三维凸子空间确定轨迹规划的时空走廊，具体包括：
对每一所述三维凸子空间，判断所述三维凸子空间中是否存在粗略轨迹点；
根据所有存在所述粗略轨迹点的所述三维凸子空间构建所述时空走廊。
5.根据权利要求1所述的方法，其特征在于，所述根据所述时空走廊和所述粗略轨迹进行纵向轨迹的优化和横向轨迹的优化，具体包括：
将所述时空走廊和所述粗略轨迹沿所述配置空间的l轴投影到s-t平面，得到S-T图；
对所述S-T图的时间轴进行离散得到离散时间点；
对每一所述离散时间点处的纵向位置、纵向速度和纵向加速度依据纵向轨迹优化代价函数、纵向轨迹不等式约束和纵向轨迹等式约束进行优化，得到所述优化后的纵向轨迹；
将所述时空走廊和所述粗略轨迹沿所述配置空间的s轴投影到l-t平面，得到L-T图；
对所述L-T图的时间轴进行离散得到离散时间点；
对每一所述离散时间点处的横向位置、横向速度和横向加速度依据横向轨迹优化代价函数、横向轨迹不等式约束和横向轨迹等式约束进行优化，得到所述优化后的横向轨迹。
6.根据权利要求5所述的方法，其特征在于，所述纵向轨迹优化代价函数的表达式为：
Figure FDA0003875134060000031
式中，
Figure FDA0003875134060000032
wac、
Figure FDA0003875134060000033
与
Figure FDA0003875134060000034
分别为纵向参考位置sref、纵向参考速度
Figure FDA0003875134060000035
离心加速度、纵向加速度与纵向加加速度的权重项；kr(si)表示在Si处的参考曲率；Si为时间ti处自车的纵向位置；
Figure FDA0003875134060000036
为时间ti处自车的纵向速度；
Figure FDA0003875134060000037
为时间ti处自车的纵向加速度；
Figure FDA0003875134060000038
为时间ti处自车的纵向加加速度；n为规划时域的时间点总数；
所述横向轨迹优化代价函数的表达式为：
Figure FDA0003875134060000041
式中，wl、
Figure FDA0003875134060000042
以及wobs分别表示横向位置、横向速度、横向加速度、横向加加速度、与横向参考位置lref的距离以及与障碍物的距离的权重项；li为时间ti处自车的横向位置；
Figure FDA0003875134060000043
为时间ti处自车的横向速度；
Figure FDA0003875134060000044
为时间ti处自车的横向加速度；
Figure FDA0003875134060000045
为时间ti处自车的横向加加速度；
Figure FDA0003875134060000046
与
Figure FDA0003875134060000047
分别为li的下界与上界。
7.根据权利要求6所述的方法，其特征在于，所述纵向轨迹不等式约束的表达式为：
Figure FDA0003875134060000048
Figure FDA0003875134060000049
Figure FDA00038751340600000410
Figure FDA00038751340600000411
Figure FDA00038751340600000412
式中，
Figure FDA00038751340600000413
与
Figure FDA00038751340600000414
分别为Si的下界与上界；
Figure FDA00038751340600000415
为规划的最高限速；
Figure FDA00038751340600000416
和
Figure FDA00038751340600000417
分别为规划的最小线加速度和最大线加速度；
Figure FDA00038751340600000418
和
Figure FDA00038751340600000419
分别为规划的最小纵向加加速度和最大纵向加加速度；
Figure FDA00038751340600000420
为最大向心加速度；
所述横向轨迹不等式约束的表达式为：
Figure FDA0003875134060000051
Figure FDA0003875134060000052
Figure FDA0003875134060000053
Figure FDA0003875134060000054
Figure FDA0003875134060000055
式中，
Figure FDA0003875134060000056
和
Figure FDA0003875134060000057
分别为规划的最小横向速度和最大横向速度；
Figure FDA0003875134060000058
和
Figure FDA0003875134060000059
分别为规划的最小横向加速度和最大横向加速度；
Figure FDA00038751340600000510
和
Figure FDA00038751340600000511
分别为规划的最小横向加加速度和最大横向加加速度；αmax为自车最大的转向角；L为自车的轴距；κr(li)表示在li处的参考曲率。
8.根据权利要求6所述的方法，其特征在于，所述纵向轨迹等式约束的表达式为：
Figure FDA00038751340600000512
Figure FDA00038751340600000513
Figure FDA00038751340600000514
Figure FDA00038751340600000515
所述横向轨迹等式约束的表达式为：
Figure FDA00038751340600000516
Figure FDA00038751340600000517
Figure FDA00038751340600000518
Figure FDA00038751340600000519
9.根据权利要求1所述的方法，其特征在于，所述根据优化后的纵向轨迹和优化后的横向轨迹确定自动驾驶轨迹，具体包括：
合并所述优化后的纵向轨迹和所述优化后的横向轨迹得到所述Frenét坐标系下的轨迹；
对所述Frenét坐标系下的轨迹进行安全检查，并将通过安全检查的所述Frenét坐标系下的轨迹变换到笛卡尔坐标系下，得到所述自动驾驶轨迹。
10.一种基于权利要求1至9任一项所述的方法的系统，其特征在于，包括：
信息获取模块，用于实时获取自车的位姿信息和自车周围的障碍物；
三维配置空间构建模块，用于根据所述自车的位姿信息确定所述自车行驶的参考线，将所述参考线由笛卡尔坐标系转换到Frenét坐标系下，基于所述Frenét坐标系的s轴和l轴并引入时间t轴构建三维配置空间；所述s轴、所述l轴和所述t轴相互垂直；
障碍物表示模块，用于根据所述障碍物出现的时间将规划时域内的所述障碍物在所述三维配置空间中进行表示；
平面截取模块，用于在每一目标时间点处截取所述三维配置空间的s-l平面，得到每一所述目标时间点处的二维凸子空间；截取的所述s-l平面中除所述障碍物构成区域外的其他区域包括多个所述二维凸子空间；所述目标时间点为至少一个所述障碍物首次出现的时间点和/或至少一个所述障碍物末次出现的时间点或所述规划时域内的时间边界点；
三维凸子空间构建模块，用于对每相邻两个所述目标时间点处的所有所述二维凸子空间根据位置对应关系进行匹配并由每一匹配的二维凸子空间构成三维凸子空间；
时空走廊构建模块，用于根据所述自车在所述规划时域内的粗略轨迹和所有所述三维凸子空间确定轨迹规划的时空走廊；
轨迹规划模块，用于根据所述时空走廊和所述粗略轨迹进行纵向轨迹的优化和横向轨迹的优化，并根据优化后的纵向轨迹和优化后的横向轨迹确定自动驾驶轨迹。
CN202211211074.0A 2022-09-30 2022-09-30 一种基于时空走廊的自动驾驶轨迹规划方法及系统 Pending CN115416693A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211211074.0A CN115416693A (zh) 2022-09-30 2022-09-30 一种基于时空走廊的自动驾驶轨迹规划方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211211074.0A CN115416693A (zh) 2022-09-30 2022-09-30 一种基于时空走廊的自动驾驶轨迹规划方法及系统

Publications (1)

Publication Number Publication Date
CN115416693A true CN115416693A (zh) 2022-12-02

Family

ID=84205864

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211211074.0A Pending CN115416693A (zh) 2022-09-30 2022-09-30 一种基于时空走廊的自动驾驶轨迹规划方法及系统

Country Status (1)

Country Link
CN (1) CN115416693A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115933701A (zh) * 2023-01-06 2023-04-07 北京理工大学 一种基于二次规划的安全走廊优化生成方法及系统

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115933701A (zh) * 2023-01-06 2023-04-07 北京理工大学 一种基于二次规划的安全走廊优化生成方法及系统

Similar Documents

Publication Publication Date Title
Marin-Plaza et al. Global and local path planning study in a ROS-based research platform for autonomous vehicles
De Lima et al. Navigation of an autonomous car using vector fields and the dynamic window approach
EP4141736A1 (en) Lane tracking method and apparatus
KR20190119237A (ko) 로봇 주행용 위상 지도 생성 장치 및 방법
JP2020126619A (ja) Ｖ２ｘ通信及びイメージ処理を利用した情報融合によって自律走行の短期経路をプランニングするための方法及び装置｛ｍｅｔｈｏｄ ａｎｄ ｄｅｖｉｃｅ ｆｏｒ ｓｈｏｒｔ−ｔｅｒｍ ｐａｔｈ ｐｌａｎｎｉｎｇ ｏｆ ａｕｔｏｎｏｍｏｕｓ ｄｒｉｖｉｎｇ ｔｈｒｏｕｇｈ ｉｎｆｏｒｍａｔｉｏｎ ｆｕｓｉｏｎ ｂｙ ｕｓｉｎｇ ｖ２ｘ ｃｏｍｍｕｎｉｃａｔｉｏｎ ａｎｄ ｉｍａｇｅ ｐｒｏｃｅｓｓｉｎｇ｝
US20200034351A1 (en) Source Authentication And Changed Primitive Verification Systems And Methods For Real Time Updating Of Cloud-Based HD 3-D Map
US11260857B2 (en) Polyline contour representations for autonomous vehicles
Laghmara et al. Obstacle avoidance, path planning and control for autonomous vehicles
CN110553660A (zh) 一种基于a*算法和人工势场的无人车轨迹规划方法
Liu et al. Deep learning-based localization and perception systems: Approaches for autonomous cargo transportation vehicles in large-scale, semiclosed environments
Xu et al. System and experiments of model-driven motion planning and control for autonomous vehicles
Andersen et al. Trajectory optimization and situational analysis framework for autonomous overtaking with visibility maximization
US20230150549A1 (en) Hybrid log simulated driving
CN112406905B (zh) 基于交通工具的数据处理方法、装置、计算机及存储介质
DE102023104789A1 (de) Verfolgung mehrerer objekte
Wang et al. Real-time safe stop trajectory planning via multidimensional hybrid A*-algorithm
CN115416693A (zh) 一种基于时空走廊的自动驾驶轨迹规划方法及系统
CN116266380A (zh) 一种环境数据重建方法、装置、系统及存储介质
US20220340177A1 (en) Systems and methods for cooperative driving of connected autonomous vehicles in smart cities using responsibility-sensitive safety rules
US20230242142A1 (en) Systems, methods, and computer-readable media for spatio-temporal motion planning
WO2023039707A1 (en) Motion planning using spatio-temporal convex corridors
Hongbo et al. Relay navigation strategy study on intelligent drive on urban roads
Matsui et al. Local and global path planning for autonomous mobile robots using hierarchized maps
Qian et al. Building and climbing based visual navigation framework for self-driving cars
Chen et al. Improving Autonomous Vehicle Mapping and Navigation in Work Zones Using Crowdsourcing Vehicle Trajectories

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