CN112068545B - 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质 - Google Patents

一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质 Download PDF

Info

Publication number
CN112068545B
CN112068545B CN202010718402.0A CN202010718402A CN112068545B CN 112068545 B CN112068545 B CN 112068545B CN 202010718402 A CN202010718402 A CN 202010718402A CN 112068545 B CN112068545 B CN 112068545B
Authority
CN
China
Prior art keywords
path
vehicle
obstacle
planning
optimization
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
CN202010718402.0A
Other languages
English (en)
Other versions
CN112068545A (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 Graduate School Harbin Institute of Technology
Original Assignee
Shenzhen Graduate School Harbin Institute 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 Shenzhen Graduate School Harbin Institute of Technology filed Critical Shenzhen Graduate School Harbin Institute of Technology
Priority to CN202010718402.0A priority Critical patent/CN112068545B/zh
Publication of CN112068545A publication Critical patent/CN112068545A/zh
Application granted granted Critical
Publication of CN112068545B publication Critical patent/CN112068545B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0251—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle

Abstract

本发明提供了一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质，该行驶轨迹规划方法包括无障碍物场景决策规划步骤和障碍物十字路口场景决策规划步骤。本发明的有益效果是:本发明采用路径‑速度迭代优化算法、动态规划、二次规划等方法来研究与进行十字路口场景下车辆的决策规划，仿真结果表明在轨迹舒适度方面起到了较大的提升效果，可以为无人驾驶车辆控制理论提供有价值的参考。

Description

一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及 存储介质
技术领域
本发明涉及无人驾驶技术领域，尤其涉及一种无人驾驶车辆在十字路口的行驶轨迹规划方法。
背景技术
随着汽车电子化和高级辅助驾驶技术的快速发展，自动驾驶作为辅助驾驶技术的高级阶段，俨然成为未来解决交通出行的重要方式，成为全球范围内的一个新的技术研究热点和重点。决策规划作为无人驾驶中主要模块，通过获取感知、定位、预测模块提供的丰富外部信息(障碍物信息)，进行分析处理，并优化出一条最适合车辆行进的轨迹。十字路口场景作为多道路汇合的交通要塞，由于其特殊的道路特点(中心不存在车道线、行驶策略的多样性)，在决策规划的过程中需要进行更进一步的分析和研究。
发明内容
本发明提供了一种无人驾驶车辆在十字路口的行驶轨迹规划方法，包括无障碍物场景决策规划步骤和障碍物十字路口场景决策规划步骤；
所述无障碍物场景决策规划步骤：在无障碍物十字路口场景中，首先根据车辆目的地确定车辆的行驶策略并在高精度地图上得到相应的标准路径；然后对于不满足车辆动力学模型的标准路径进行平滑优化，得到最优可行路径；最后结合标准速度，得到无障碍物环境下的最优可行轨迹；
所述障碍物十字路口场景决策规划步骤：在障碍物十字路口场景中，首先基于无障碍物轨迹建立三维Frenet框架，并通过感知、预测模块得到静态、动态障碍物的信息；然后进行轨迹优化；最后得到障碍物环境下最优可行轨迹，并循环优化。
本发明的有益效果是：本发明采用路径-速度迭代优化算法、动态规划、二次规划等方法来研究与进行十字路口场景下车辆的决策规划，并通过仿真实验，取得了不错的效果。
附图说明
图1-1a是实际十字路口图；图1-1b是简化十字路口模型图；图1-2是车辆的自行车模型图；图1-3是静态障碍物的状态量图；图1-4a是无障碍物场景图；图1-4b是障碍物场景图；图2-1是无障碍物场景决策规划方案流程图；图2-2是十字路口不同行驶策略的标准路径示意图；图2-3是基于二维分段高阶多项式的二次规划方案原理图；图2-4a是对标准路径进行采样的示意图；图2-4b是设置边界得到搜索空间示意图；图2-5a是左转无障碍物最优路径示意图；图2-5b是右转无障碍物最优路径示意图；图2-5c是掉头无障碍物最优路径示意图；图2-6a是左转车辆朝向变化的优化效果示意图；图2-6b是右转车辆朝向变化的优化效果示意图；图2-6c是掉头车辆朝向变化的优化效果示意图；图2-7a是左转车辆曲率变化的优化效果示意图；图2-7b是右转车辆曲率变化的优化效果示意图；图2-7c是掉头车辆曲率变化的优化效果示意图；图2-8a是无障碍物十字路口场景中左转策略的路径舒适性指标示意图；图2-8b是无障碍物十字路口场景中右转策略的路径舒适性指标示意图；图2-8c是无障碍物十字路口场景中掉头策略的路径舒适性指标示意图；图3-1是障碍物场景决策规划方案流程图；图3-2是SLT坐标系示意图；图3-3是优化模块流程图；图3-4是障碍物投影示意图；图3-5是路径优化的结果图；图3-6是速度优化的结果图；图3-7a是路径舒适性指标示意图；图3-7b是路径偏移量指标示意图；图3-7c是速度舒适性指标示意图；图3-7d是速度偏移量指标示意图；图3-8a是t＝0s时刻车辆路径和速度的规划结果示意图；图3-8b是t＝1s时刻车辆路径和速度的规划结果示意图；图3-8c是t＝2s时刻车辆路径和速度的规划结果示意图；图3-8d是t＝2s时刻车辆路径和速度的规划结果示意图。
具体实施方式
本发明公开了一种无人驾驶车辆在十字路口的行驶轨迹规划方法，以下进行具体介绍。
一.模型
1.十字路口模型
十字路口是两条道路的相交处，根据道路的特点十字路口可以拆分成两个区域，如图1-1a。第一个区域为中心区域，是处在平面交叉口中心位置的矩形区域。该区域的特点是此范围内不存在车道线，所以对于车辆确定行驶路径造成困难；同时该区域为多车辆、多行人汇聚之地，交通环境复杂，易发生碰撞，所以在此区域需要考验决策规划方案的精准性。第二个区域为缓冲区域，是车辆进入十字路口中心区域之前的一段直行路径，为进入中心区域的缓冲。与中心区域的区别是在该区域内存在车道线，所以决策规划方案需要考虑两个区域路径的融合问题。缓冲区域可能存在多车道，在实际情况下，缓冲区域可能是双向单车道、双向两车道、双向多车道。车辆通过十字路口时存在4种行驶策略：直行、左转、右转、掉头。缓冲区域的每个车道对应一个或者多个行驶策略。本发明将十字路口模型进行简化，如图1-1b。该模型的缓冲区域为双向单车道，单车道中可以选择全4种行驶策略(直行、左转、右转、掉头)。决策规划方案根据车辆的目的地确定相应的行驶策略，并规划出一条最优的通过十字路口的路径。现实中其他不同的十字路口结构可以在该模型的基础上进行扩展，决策规划方案的原理一致。
2.车辆动力学模型
对于本发明，首先车辆运动是一个二维平面运动，即车辆不存在或者不考虑垂直方向的运动；其次本发明针对的车辆是普通的前轮转向车辆，且左右车轮具有相同的转速和转向角ω；同时车辆是一个刚体；另外本发明所研究的城市十字路口场景是一个较低速的场景，车辆的行驶速度不存在剧烈地变化，故可以忽略前后轴载荷的转移。因此，本发明的场景符合自行车模型的要求。
车辆的自行车模型如图1-2，动力学方程如下：
Figure GDA0002764096380000031
其中，车辆后轴中心的坐标为(x，y)，车辆在当前时刻的朝向θ(heading)，转向角ω，车辆对应路径点上的曲率κ。
Figure GDA0002764096380000032
t表示时间，L表示前轮中心和后轮中心之间的距离，a表示车辆的加速度。
基于此自行车模型，在某个时刻的输入车辆的转向角和加速度(ω，a)后，可以估算车辆在下一个时刻的状态(x，y，θ，v)(空间坐标、车辆朝向、速度)。由于转向角ω很难直接通过车辆轨迹得到，所以用在当前时刻车辆对应轨迹的路径点上的曲率κ(可以通过车辆的轨迹方程得到)来代替转向角ω，从而表示车辆的转向趋势。
综上所述，当得到车辆在当前时刻的状态量(x,y,θ,v)和车辆在当前时刻和未来时刻的连续输入量(κ,a)时，就可以计算得到车辆未来时刻的连续状态量，进而完成车辆的行驶控制。
3.车辆轨迹模型
车辆轨迹具有以下特点：
(1)车辆的轨迹是一个三维曲线。考虑到整个十字路口存在着动态障碍物且车辆的感知范围有限，因此车辆感知范围内的环境是运动的、时变的。所以车辆的行驶轨迹不单单只考虑二维空间维度的路径，还需要增加考虑时间维度。增加了时间维度后，可以得到车辆在不同时间点时的位置，同时可以以此得到车辆的速度特征。因此车辆的轨迹是一个包括二维空间维度和时间维度的三维曲线。
(2)车辆轨迹可以拆分为路径、速度两个问题进行研究。直接对三维曲线进行规划通常是复杂的，所以在解决车辆的规划问题的时候，通常会对问题进行拆分从而达到降维的目的。对于三维车辆轨迹，本发明选择将三维规划问题简化为路径、速度两个二维问题进行研究。路径指的是在二维平面空间中一条车辆可行的曲线，在实际车辆行驶的时候，车辆的后轴中心沿着路径行驶；速度问题研究是车辆延路径行驶的距离与时间的关系，在实际车辆行驶的时候，可以得到车辆在每个时间点所行驶的距离。通过决策规划可以分别求得可行的路径曲线和速度曲线，将两者相结合可以得到车辆的轨迹。
(3)一条可行的车辆轨迹需要满足车辆的动力学要求。本发明采用的是自行车模型。对于自行车模型，轨迹需要满足：由于车辆的朝向θ、转向角ω(可以等价为曲率κ)、速度v、加速度a的变化是连续的，不可能出现阶跃的情况，所以需要路径曲线、速度曲线和它的一阶导数、二阶导数连续。因此在代价搜索的时候，代价函数需要考虑导数的连续性，此要求在本发明最小化舒适性代价的时候被满足。
(4)函数优化法求解可行轨迹原理是：将轨迹用多个函数来表示，求解对于目标函数最优的多个函数的表达式，即为最佳的轨迹。本发明对于目标函数的设定主要考虑轨迹舒适性、偏移量和碰撞避免。
三维笛卡尔坐标系(XYT)下的车辆轨迹模型：
本发明对于无障碍物场景的决策规划采用的是三维笛卡尔坐标系。在此坐标系下，除了出了原本需要考虑的平面坐标XY，还需要增加考虑时间维度T，所以此坐标系包括XYT三个维度。
本发明采用的基于函数优化的决策规划方法，需要将轨迹抽象成函数。由于在XYT坐标系下，X坐标、Y坐标、T坐标都是变化的，所以需要用三个函数来抽象轨迹：对于路径，用x＝f(s)，y＝g(s)来表示；对于速度，用s＝u(t)来表示。其中s为车辆沿着路径行驶的弧长。
故三维笛卡尔坐标系下的车辆轨迹模型为：
Figure GDA0002764096380000041
Speed:s＝u(t)
因此车辆除空间坐标(x，y)外的其他状态量和输入量都可以由这三个轨迹函数经过运算得到：
Figure GDA0002764096380000042
Figure GDA0002764096380000043
Figure GDA0002764096380000044
Figure GDA0002764096380000045
综上所述，在三维笛卡尔坐标系下，给定x＝f(s)，y＝g(s)，s＝u(t)三个轨迹函数，就可以表达在任意时刻车辆的状态量和输入量，从而控制车辆沿着轨迹运动。
三维Frenet坐标系(SLT)下的车辆轨迹模型：
本发明对于障碍物场景的决策规划采用的是三维Frenet坐标系。此坐标系包括SLT三个维度。
此坐标的特点是通过一条参考路径降低了描述路径的函数数量。Frenet坐标系中，需要以一条参考路径(本文用的是无障碍物路径)为基础，车辆沿该路径的纵向行驶距离为S坐标，车辆偏离该路径的横向距离为L坐标，从而用SL这两个维度来表示平面空间。由于车辆是不断沿着参考路径向前行驶的(不考虑倒车或者逆行)，所以车辆的横向位移l是随着纵向位移s的变化而变化的，只需要用一个函数l＝p(s)就可以抽象平面空间的路径。同时在三维Frenet坐标系下，需要增加考虑时间维度，用s＝u(t)来描述车辆的速度特征。
故三维Frenet坐标系下的车辆轨迹模型为：
Path:l＝p(s)
Speed:s＝u(t)
车辆状态量、输入量与三维Frenet坐标系下轨迹函数的映射关系如下：
Figure GDA0002764096380000051
综上所述，在三维Frenet坐标系下，给定参考路径线(无障碍物路径)和l＝p(s)，s＝u(t)两个轨迹函数，就可以表达在任意时刻车辆的状态量和输入量，从而控制车辆沿着轨迹运动。
4.障碍物模型
十字路口场景中，存在静态、动态障碍物。静态障碍物指的是在规划时间范围内状态量不随着时间的变化而变化的障碍物，比如乱停放的车辆、三角警示牌、施工区域等；动态障碍物是在规划时间范围内状态量随着时间的变化而变化的障碍物，比如环境中其他的行驶车辆、运动的行人、运动的自行车等。所以需要合理的设置障碍物模型以及决策规划中的代价函数，使得形式车辆可以避开障碍物。
假定所有的障碍物为凸多边形的行驶，车载传感器通过感知模块可以获得凸多边形各个顶点的坐标。
对于静态障碍物，需要将障碍物以bounding box的形式表示为一个矩形区域。如图1-3，静态障碍物的状态可以由以下参数表示：
(1)障碍物中心点的坐标(xob,yob)。该坐标可以由凸多边形各个顶点的坐标经过计算得到。
xob＝[min(x1,x2,...,xn)+max(x1,x2,...,xn)]/2
yob＝[min(y1,y2,...,yn)+max(y1,y2,...,yn)]/2
其中n为凸多边形的顶点数。
(2)矩形区域的长、宽(ρ，w)。
(3)矩形区域的朝向θob(heading)。
所以静态障碍物的状态可以由一个5维向量来表示(xob,yob,ρ,w,θob)。
对于动态障碍物，其状态不能只用当前时刻的状态来表示，需要包括未来时刻预测的状态。所以通过车辆的预测模块获得动态障碍物的预测轨迹并将其离散化，得到动态障碍物的状态量{(xob,i,yob,i,ρi,wi,θob,i,ti)|i＝0,1,...,m}，其中t0为当前时刻，t1～tm为未来时刻。
5.决策规划模型
决策规划的目标是在存在静态、动态障碍物的十字路口环境下，在车辆感知范围有限的前提下，求解从起始点到终止点的最优的可行平滑轨迹。
由于十字路口环境的特殊特点(无法确定道路中心线)，所以在决策规划方案中考虑将问题拆分成两个场景分步解决：无障碍物十字路口场景和障碍物十字路口场景，如图1-4a，1-4b。
建立无障碍物十字路口场景的原因是：需要以三维笛卡尔坐标系下得到的平滑无障碍物可行路径作为参考路径建立起三维Frenet框架，用于障碍物场景；平滑的无障碍物轨迹将作为基础轨迹，在遇到障碍物时，基于该轨迹进行优化修正。
因此决策规划方案的原理如下：
(1)首先在无障碍物场景下通过高精度地图得到一条最优的可行平滑轨迹，称为无障碍物轨迹，并依据无障碍物路径建立Frenet坐标系。
(2)通过车辆上的其他感知模块(摄像头、激光雷达等)对于环境中可能存在的障碍物进行部分感知，利用此感知信息在三维Frenet坐标系下对于无障碍物轨迹进行迭代修正，得到真正行驶的轨迹。
二.无障碍十字路口场景决策规划方案
无障碍物十字路口场景是指在十字路口中不存在静态或者动态障碍物，车辆仅仅通过高精度地图获得外部的道路环境信息。无障碍物轨迹是在无障碍物场景中，给车辆的一条可行的平滑的行驶轨迹。该轨迹的坐标系采用的是三维笛卡尔坐标系(XYT)，所以可以用两条路径函数x＝f(s)，y＝g(s)，一条速度函数s＝u(t)来表达该轨迹。
在一般场景中，无人车在无障碍物时候的行驶路径是道路的中心线(通过识别两侧的车道线来确定道路的中心线)，行驶速度是参考速度。但在十字路口场景中，由于在十字路口中心不存在车道线，同时行驶策略存在多样性(直行、左转、右转、掉头)，所以需要运用合理的决策规划方法来适应此特殊的环境特点。
如图2-1，方法如下：
步骤1：通过目的地的位置，可以确定车辆的行驶策略，是直行、左转、右转还是掉头。根据车辆的行驶策略，通过高精度地图上提供的车辆定位和道路信息，生成相应的标准路径。
步骤2：对不满足动力学约束的标准路径利用平滑优化在其附近搜索出一条最优的可行平滑路径。
步骤3：由于环境中不存在障碍物，具有时不变性，车辆的行驶速度恒定为标准速度，即u′＝Vsta。将最优的可行平滑路径与标准速度结合，得到无障碍物可行轨迹。
标准路径是最直观、最容易生成的线圆式车辆路径。对于标准路径，转弯采用Dubins曲线的思想，沿着圆弧转弯。所以在十字路口场景下，标准路径是由线段、圆弧和切线构成，如图2-2所示。但是该路径并不一定满足本发明所采用的车辆自行车模型对于曲率连续性的要求(直行策略标准路径的曲率恒为0，满足曲率连续；左转、右转、掉头策略标准路径的曲率均存在阶跃，不满足要求)，故对于这些曲率不连续的情况，需要进行平滑优化。
平滑优化的原理是在标准路径附近搜索出一条最优的可行平滑路径，即找到对于代价而言最优的x＝f(s)，y＝g(s)。由于场景中不存在障碍物，所以整个搜索空间为凸的，可以采用二次规划的优化方案进行平滑优化。如图2-3。
二次规划的优化方案搜索的先决条件是确定路径参数化方式、确定代价函数、确定线性约束，然后利用二次规划快速收敛到最优的可行平滑路径。
1.路径参数化：
由于连续空间中的路径具有很高的复杂性，直接进行优化很困难且计算量较大。故考虑将路径参数化，用分段高阶多项式来表示路径，这样路径搜索问题就可以简化为对于高阶多项式系数的搜索。分段的原因是为了避免使用单一多项式造成的龙格效应，避免车辆连续转向造成的舒适度下降。
在该场景中，需要对笛卡尔坐标系下的路径进行参数化。由于笛卡尔空间中的路径需要用两个函数来表示，所以采用二维的分段高阶多项式(5阶)来参数化路径。参数化的实现方式：首先以一个较大的间隔ds对于路径的总弧长进行分割，得到n1+1个节点{si|i＝0,1,...,n1}；然后将每两个节点之间的路径用二维5阶多项式来表示：
Figure GDA0002764096380000071
其中s为车辆沿路径行驶的弧长；
Figure GDA0002764096380000072
是行驶弧长与其所在多项式的头节点进行差分运算后的结果，
Figure GDA0002764096380000073
二维分段多项式的段数为n1；Ai＝[ai0ai1 ai2 ai3 ai4 ai5]T,Bi＝[bi0 bi1 bi2 bi3 bi4 bi5]T为第i段高阶多项式的系数。
路径参数化后，优化问题转化为对于分段高阶多项式系数取值的搜索，即对于最优的系数矩阵
Figure GDA0002764096380000074
的求解。
2.代价函数：
二次规划需要根据场景的要求和关注的指标，设定一个合理的目标函数(代价函数)，使得搜索出来的路径满足需求。对于无障碍物十字路口场景，采用的是舒适性代价，目的是：(1)使得搜索出来的轨迹满足车辆的动力学要求，即车辆的朝向θ、路径的曲率κ连续；(2)搜索出来的路径尽可能的平滑，从而降低车辆转向的速度和频率，提升乘客的舒适感。
车辆朝向θ与路径曲线一阶导有关；路径曲率κ与路径曲线的二阶导有关。所以舒适性代价需要考虑最小化曲线的二阶导数、三阶导数平方的积分。最小化二阶导数平方的积分满足了车辆朝向θ的连续性，同时尽可能地使车辆朝向的变化更加缓慢；最小化三阶导数平方的积分满足了路径曲率κ的连续性，同时尽可能地使车辆路径的曲率变化更加缓慢，即等价于转向角ω的变化更加缓慢。
因此，舒适性代价是两条路径函数不同阶导数的线性加权和，表示如下：
Costsmooth,path＝ws1∫(f″)2(s)+ws2∫(f″′)2(s)+ws3∫(g″)2(s)+ws4∫(g″′)2(s)。
3、线性约束
平滑优化的线性约束包括三个部分，搜索空间约束、节点约束、车辆自身约束。
搜索空间约束的目的是将搜索范围限制在标准路径的附近，目的是限制搜索区域以防出现搜索区域违反交通规则的情况(压双黄线、行驶出可行道路范围等)，同时不让优化得到的路径距离标准路径过远。实现方法是：首先以一个较小的间隔ds′对标准路径进行采样(ds′＜ds)，得到n2+1个采样点{sj|j＝0,1,...,n2}，并通过sj的采样值在标准路径上得到{(xj,yj)|j＝0,1,...,n2}，如图2-4a，然后在采样点附近框定一个范围，将搜索区域限定在以采样点为中心，边长为boundary的正方形中，如图2-4b。搜索空间约束表达式如下：
Figure GDA0002764096380000081
节点约束包括两个部分，连接处约束和起始终止点约束。连接处约束的目的是保证分段高阶多项式在不同段的连接处也足够的平滑，所以需要在多项式段的连接点处保证左右两段多项式的取值相等且一阶导数、二阶导数、三阶导数相等。起始终止点约束的目的是保证车辆在起始点处的车辆位置和朝向满足初始和终止条件。节点约束表达式如表2-1所示：
表2-1节点约束表达式
Figure GDA0002764096380000082
Figure GDA0002764096380000091
车辆的自身约束是由于车辆自身机械结构的限制而导致的约束。对于路径的平滑优化而言，由于车辆的方向盘存在转向范围的约束和转向灵敏度的约束，所以需要将车辆曲率和曲率变化速度约束在一个范围内，该范围的边界由车辆本身的机械结构决定。
4、二次规划搜索
通过推导可以证明，代价函数满足QP的形式。同时所有约束皆为线性约束。故问题可以转换为如下的二次规划形式：
Figure GDA0002764096380000092
其中，
Figure GDA0002764096380000093
为分段高阶多项式系数的集合，H为化简后得到的分块对角矩阵，表示为：
Figure GDA0002764096380000094
其中i＝0,1,...,n1-1，s(z)表示高阶多项式矩阵的z阶导数：
Figure GDA0002764096380000095
因此可以用二次规划来求解规划问题，可以得到二维分段高阶多项式的最优系数
Figure GDA0002764096380000096
将s带入二维分段高阶多项式中计算，则可以得到平滑的无障碍物可行路径。
无障碍物场景的仿真以及评价指标分析：
由于无障碍物场景是时不变的，速度为标准速度(恒定值)，所以评价指标只需要考虑路径的舒适性。无障碍物场景下的路径舒适性指标：为满足车内乘客的舒适感，车辆要尽可能的缓慢转向，避免急转,从而提升路径曲线的平滑度。所以对于路径函数x＝f(s)和y＝g(s)舒适性指标需要考虑的是两者不同阶导数平方积分的加权和：
Epath,comfort＝ws1∫(f″)2(s)+ws2∫(f″′)2(s)+ws3∫(g″)2(s)+ws4∫(g″′)2(s)
路径舒适性指标的值越小表示车辆的转向越缓慢，路径曲线越平滑，车内乘客的舒适感越高；反之路径舒适性指标的值越高表示车辆的转向越频繁、越急促，会降低车内乘客的舒适感
图2-5abc分别为无障碍物十字路口场景中左转、右转和掉头策略的路径曲线。三幅图中的黑色虚线分别为三种行驶策略的标准路径，三幅图中的黑色实线分别为三种行驶策略优化后的无障碍物最优可行路径。通过对比可以看出，经过平滑优化后的路径在十字路口缓冲区域和中心区域之间的转向连接处更加平滑；从整体上来看，相较于标准路径的急转，最优可行路径的转向更加缓慢，有助于乘客舒适感的提升。
图2-6abc分别为无障碍物十字路口场景中左转、右转和掉头策略的车辆朝向θ变化曲线。三幅图中的黑色虚线分别为三种行驶策略标准路径的车辆朝向θ变化曲线，三幅图中的黑色实线分别为三种行驶策略优化后无障碍物最优可行路径的车辆朝向θ变化曲线。通过对比可以看出，标准路径中车辆朝向是曲折变化的，经过平滑优化后路径的车辆朝向曲线变得更加平滑，这表明优化后车辆朝向的变化更加缓慢，提升了舒适感。
图2-7abc分别为无障碍物十字路口场景中左转、右转和掉头策略的路径曲率κ变化曲线。三幅图中的黑色虚线分别为三种行驶策略标准路径的曲率κ变化曲线，三幅图中的黑色实线分别为三种行驶策略优化后无障碍物最优可行路径的曲率κ变化曲线。通过对比可以看出，标准路径的曲率是阶跃的、不连续的，在十字路口缓冲区域和中心区域之间的转向连接处会存在突变；经过优化后的路径曲率曲线变得连续且平滑，不仅使得路径满足了车辆的动力学要求，是可行的，同时也提升了路径的舒适度。
图2-8abc分别为无障碍物十字路口场景中左转、右转和掉头策略的路径舒适性指标。本实验采用两种方法进行对比，第一种方法对标准路径进行降采样，然后用平滑曲线(这里采用的是Spline曲线)进行插值，这是简单且普遍运用的方法；第二种方法是本发明利用标准路径进行二次规划的方法。利用车辆起始点的随机性，进行循环的仿真实验，得到图2-8abc所示路径舒适性指标的累计分布函数(CDF)。三幅图中的黑色虚线分别为三种行驶策略采用第一种决策规划方法(标准路径+插值曲线)的路径舒适性指标的CDF；三幅图中的黑色实线分别为三种行驶策略采用第二种决策规划方法(标准路径+二次规划)的路径舒适性指标的CDF。通过对比可以看出，本发明采用的方法有效地降低了路径舒适性指标的值，即提高了车内乘客的舒适度。
表2-2无障碍物场景中两种决策规划方法路径舒适性指标的均值对比
Figure GDA0002764096380000101
表2-2为无障碍物场景中两种决策规划方法路径舒适性指标的均值对比。从表中可以看出，本发明采用的第二种方法(标准路径+二次规划)相较于第一种(标准路径+插值曲线)而言，降低了路径舒适性指标的均值。由于路径舒适性指标的值与乘客舒适度是成反比的关系，所以可以计算第二种方法相较于第一种方法的路径舒适度提升率：
Figure GDA0002764096380000111
由此可得，对于左转、右转、掉头策略，本发明采用的标准路径+二次规划方法的路径舒适度提升率分别为10.97％，13.10％，18.80％。因此该方法一定程度上提升了车内乘客的乘车体验感。
三.障碍物十字路口场景决策规划方案
障碍物场景是指在十字路口环境下存在静态障碍物和动态障碍物。静态障碍物的状态量是时不变的；动态障碍物的状态量包括当前时刻的状态和未来时刻的预测状态，是时变的。为此，障碍物场景下的决策规划有如下的特点：
1、基于无障碍物轨迹迭代修正。
由于环境中存在障碍物，本发明所得的无障碍物轨迹可能会与障碍物产生相交情况，从而导致碰撞，所以需要根据感知到的障碍物信息对于无障碍物轨迹进行迭代修正，从而得到障碍物场景下的行驶轨迹。
2、无人车对于环境部分感知。
无人车感知静态、动态障碍物是通过车载传感器，例如摄像头、激光雷达等。但是这些车载传感器的感知范围是有一定限制的，如距离超过一定范围，感知的精度会达不到决策规划的要求。因此，设定一个阈值，即感知半径r，感知范围是以车辆中心为圆心，以r为半径的圆形区域。在感知范围之内，可以保证车辆对于环境的感知是精确的(感知误差在可接受范围内)且不会出现漏检现象；在感知范围外，车辆感知误差可能会超出范围且漏掉部分感知目标。本发明只关注感知范围内的感知信息。
3、决策规划循环进行。
无人车不能对环境进行全局感知，所以随着车辆的移动和环境的动态变化，感知范围内障碍物也是变化的。当传感器感知到新的障碍物后，需要再次对于轨迹进行修正。所以障碍物场景下的决策规划需要循环求解。
如图3-1所示，中障碍物十字路口场景决策规划步骤包括：
步骤A：以无障碍物轨迹为基础，建立三维Frenet框架。
步骤B：感知模块(激光雷达、摄像头等)和预测模块将感知到的静态障碍物状态(xob,yob,ρ,w,θob)、感知和预测到的动态障碍物状态{(xob,i,yob,i,ρi,wi,θob,i,ti)|i＝0,1,...,m}、上一循环求得的车辆轨迹输入优化模块。
步骤C：优化模块基于步骤A得到的三维Frenet框架，并根据输入的障碍物信息，进行优化。优化模块分为路径优化、速度优化两个部分，通过优化可以得到平滑可行的路径曲线和速度曲线，并迭代求解。
步骤D：将优化后得到的路径曲线和速度曲线相结合，并转换回笛卡尔坐标系，得到障碍物轨迹下的行驶轨迹。
步骤E：当感知模块感知到新的障碍物后，返回步骤B，循环求解。
三维Frenet框架：
本发明对于障碍物场景的研究中选择基于上一章求得的无障碍物路径，建立三维Frenet坐标系，让优化问题在三维Frenet框架进行。原因如下：
(1)三维Frenet坐标系可以减少规划中轨迹函数的个数。在三维笛卡尔坐标系中，对于轨迹的描述需要使用x＝f(s),y＝g(s),s＝u(t)三个函数表示；但在三维Frenet坐标系下，可以依据无障碍物路径，将路径简化为l＝p(s),s＝u(t)两个函数，从而简化后续的路径优化。
(2)三维Frenet坐标系可以根据纵向位移s、横向位移l更好地描述车辆与障碍物的关系。同时在设置碰撞代价的时候可以更好地描述车辆和障碍物的横向、纵向距离与横向、纵向最小安全距离的关系。
(3)三维Frenet坐标系更适用于十字路口这种较曲折的道路环境。本发明优化方法中的动态规划需要将路径的平面空间网格化，在笛卡尔坐标系很难将曲折道路平整地网格化；而Frenet坐标系考虑的是行驶距离和行驶偏移量的关系，可以降低曲折道路的影响。
三维Frenet坐标系可以拆分为SL坐标系和ST坐标系。路径优化问题在SL坐标系下进行，S维度表示的是车辆沿无障碍物路径行驶的纵向距离，L维度表示的是车辆偏离无障碍物路径的横向距离；速度优化问题在ST坐标系下进行，S维度同样是纵向行驶距离，T维度表示的是车辆的行驶时间。如图3-2所示。
障碍物场景下的轨迹优化方法的原理基于EM Motion Planner：对于SLT的三维优化问题采用路径-速度迭代优化算法，求解优化问题采用动态规划结合二次规划的联合优化算法。如图3-3。
路径-速度迭代优化算法：
对于本发明的十字路口场景，由于其环境的复杂性和多变性，决策规划方案的关注点在于轨迹的精细度和规划速率，对于轨迹是全局最优还是局部最优并不需要特别的重视。因此本发明采用的是路径-速度迭代优化算法，方法如下：
步骤S1，障碍物投影(SL坐标系)：静态障碍物直接投影；对于动态障碍物，提取出上一个循环的轨迹，用来估计车辆与感知范围内的动态障碍物预测轨迹之间的相交情况，并寻找相同时间点有重叠的部分，为期望碰撞区域。
步骤S2，路径优化：通过路径规划求解出最优的可行平滑路径曲线。
步骤S3，障碍物投影(ST坐标系)：根据这个新的路径曲线，将与新的路径曲线有相交的障碍物投影到ST坐标系下。
步骤S4，速度优化：通过速度规划求解出最优的可行平滑速度曲线。
步骤S5，最后将最优路径曲线和最优速度曲线结合，得到平滑的可行轨迹。该轨迹将被用于下一循环的优化上，这样反复迭代。
动态规划结合二次规划的联合优化算法：
路径优化、速度优化的求解方法均采用函数优化方法，本质是分别在SL坐标系、ST坐标系下寻找对于代价而言最优的函数，即最优l＝p(s)，s＝u(t)。考虑到十字路口场景轨迹的精细度和规划速率的同时要求，以及十字路口可能存在大量搜索空间是非凸空间的情形(障碍物在路中间的情形，例如：行人过路口)，所以本发明采用动态规划结合二次规划的求解方法。
1、第一步，动态规划(Dynamic Programming,DP)：对于SL坐标系/ST坐标系进行网格化采样，通过动态规划搜索出一个粗略的平滑曲线(此步骤是粗略的，采样频率不用太密)，通过该粗略曲线可以得到一个可行凸空间。该步骤的目的是得到车辆面对障碍物时候的决策，同时将原本可能的非凸搜索空间转换为凸空间。
2、第二步，二次规划(Quadratic Programming,QP)：以采样点为基础用一维分段高阶多项式(5阶)将曲线参数化，以动态规划的结果作为启发，在动态规划得到的可行凸空间中用二次规划搜索出一条最优的可行平滑曲线。
动态规划结合二次规划联合优化算法的代价函数如表3-1。舒适性代价目的是让路径曲线、速度曲线尽可能地平滑。偏移量代价是为了让保证车辆不能偏移无障碍轨迹太远。碰撞代价是为了让车辆和障碍物不发生碰撞。本发明将碰撞代价设置为一个等级为0/Inf的决策：对于路径优化，碰撞代价是在SL坐标系下让车辆与障碍物之间的S方向、L方向距离大于横向、纵向最小安全距离；对于速度优化，碰撞代价是在ST坐标系下的同一时间点t，车辆和障碍物之间的S方向距离大于纵向最小安全距离。最小安全距离的设置可以基于责任敏感安全模型(Responsibility Sensitive Safety,RSS)。
表3-1代价函数
Figure GDA0002764096380000131
Figure GDA0002764096380000141
优化模块仿真：
如图3-4为障碍物投影，将静态障碍物、动态障碍物的期望碰撞区域投影到Frenet坐标系下；。图3-5中为SL坐标系下路径优化的仿真，黑色方框为障碍物投影，图中有两条曲线，一条是动态规划后的路径，以此路径为基础进行二次规划，得到另一条动态规划结合二次规划联合优化后的路径；图3-6中为ST坐标系下路径优化的仿真，黑色方框为障碍物投影，图中有两条曲线，一条是动态规划后的ST曲线，以此路径为基础进行二次规划，得到另一条动态规划结合二次规划联合优化后ST曲线。
障碍物路径评价指标：
障碍物十字路口场景的环境是时变的(优化包含路径、速度两个部分)，且存在一个参考轨迹(无障碍物轨迹)，所以对于轨迹中路径曲线的评价主要在于路径舒适性指标、路径偏移量指标；对于轨迹中速度曲线的评价主要在于速度舒适性指标、速度偏移量指标。
具体表达式如表3-2。路径/速度舒适性指标是为了使优化后路径/速度曲线更加平滑，从而让车辆缓慢转向和缓慢加减速，提升乘客体验感，所以指标表示为路径/速度函数不同阶导数平方积分的加权和，其值越小表示乘客的舒适度越好；路径/速度偏移量指标是为了让车辆的路径/速度尽可能地靠近无障碍物路径/标准速度，从而减小车辆的行驶距离、油耗等，所以指标表示为与无障碍物轨迹之间的差的平方的积分，其值越小表示拟合度越好。
表3-2评价指标
Figure GDA0002764096380000142
在仿真中采用两种决策规划方案进行对比分析。第一种采用的是动态规划+平滑曲线插值的方法，首先通过动态规划得到对于代价而言最优的采样点集合，然后用平滑的曲线(这里采用的是Spline曲线)进行插值，得到无障碍物轨迹；第二种是本文采用的动态规划+二次规划联合优化算法，先通过动态规划得到可行凸空间，然后通过二次规划快速收敛到平滑可行轨迹。利用车辆起始点的随机性，对于路径优化和速度优化进行循环实验得到路径舒适性指标、路径偏移量指标、速度舒适性指标、速度偏移量指标的累计分布函数(CDF)：
图3-7abcd分别为路径舒适性指标、路径偏移量指标、速度舒适性指标、速度偏移量指标的累计分布函数。其中四幅图中的黑色虚线为采用第一种决策规划方法(动态规划+平滑曲线插值)的评价指标CDF；四幅图中的黑色实线为本发明采用第二种决策规划方法(动态规划+二次规划)的评价指标CDF。通过对比可以看出，本发明采用的方法有效地降低了路径舒适性指标、速度舒适性指标，即提高了车内乘客的舒适度，本方法还降低了速度偏移量指标，使行驶速度更贴近标准速度；但同时，该方法使得路径偏移量指标有一定的提高，车辆的行驶路径会距离标准路径更远。
表3-3障碍物场景中两种决策规划方法舒适性指标的均值对比
Figure GDA0002764096380000151
表3-4障碍物场景中两种决策规划方法偏移量指标的均值对比
Figure GDA0002764096380000152
表3-3、3-4分别为障碍物场景中两种决策规划方法的舒适性指标和偏移量指标的均值对比。可以看出，本文采用的第二种方法(动态规划+二次规划)相较于第一种方法(动态规划+平滑插值曲线)，降低了路径舒适性指标、速度舒适性指标、路径偏移量指标的均值，提高了路径偏移量指标。同时由于舒适性指标的值与乘客舒适度是成反比的关系，偏移量指标的值与曲线和无障碍物轨迹的拟合度也是成反比的关系，所以可以计算第二种方法相较于第一种方法的舒适度提升率和拟合度提升率：
Figure GDA0002764096380000153
由此可得，本发明采用的动态规划+二次规划联合算法的路径、速度舒适度提升率分别为52.00％、57.01％，速度拟合度提升率为2.03％；作为代价，该方法的路径拟合度下降了42.37％。
完整十字路口场景仿真：
仿真以车辆的左转策略为例。在场景中设置一个静态障碍物，一个动态障碍物，动态障碍物自北朝南行驶，行驶速度为5.2m/s；车辆的感知半径设置为15m，标准速度为4m/s，规划时间间隔为0.5s。
如图3-8a，在t＝0s时刻，车辆均只感知到了静态障碍物，动态障碍物并不在车辆的感知范围内。所以车辆只在路径优化中调整了车辆的行驶路径，以避开该静态障碍物；速度方面，车辆仍是以标准速度行驶。
如图3-8b，在t＝1s时刻，车辆感知到了动态障碍物，并预测了未来时刻的轨迹，并与上一循环的车辆轨迹进行相互作用的分析，预计在t＝3s发生碰撞。所以，如图所示，速度上车辆有一个明显的加速以避开障碍物。
如图3-8c，在t＝2s时刻，车辆再次进行规划，不光速度进行加速，同时车辆的路径有一个明显的内收，从而更好地躲避障碍物。
如图3-8d，在t＝3s时刻，车辆成功已经避开动态障碍物。在路径方面，车辆需要平滑地重新靠近无障碍物路径；在速度方面，车辆速度同样需要平滑地下降回标准速度。
本发明还公开了一种无人驾驶车辆在十字路口的行驶轨迹规划系统，包括无障碍物场景决策规划模块和障碍物十字路口场景决策规划模块，所述无障碍物场景决策规划模块：用于在无障碍物十字路口场景中，首先根据车辆目的地确定车辆的行驶策略并在高精度地图上得到相应的标准路径；然后对于不满足车辆动力学模型的标准路径进行平滑优化，得到最优可行路径；最后结合标准速度，得到无障碍物环境下的最优可行轨迹。所述障碍物十字路口场景决策规划模块：用于在障碍物十字路口场景中，首先基于无障碍物轨迹建立三维Frenet框架，并通过感知、预测模块得到静态、动态障碍物的信息；然后进行轨迹优化；最后得到障碍物环境下最优可行轨迹，并循环优化。
本发明还公开了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现本发明所述的行驶轨迹规划方法的步骤。
在本发明中，结合十字路口场景的特殊道路特点，进行决策规划方案的研究。同时进行仿真实验，并从对于得到的最优的可行平滑轨迹进行评价指标的分析。研究取得了一定的成果，总结为如下几点：
(1)在无障碍物十字路口场景中，提出一种基于二维分段高阶多项式的二次规划算法，对于不同的行驶策略(直行、左转、右转、掉头)，规划并得到相应的最优可行轨迹。方案是：首先根据车辆目的地确定车辆的行驶策略并在高精度地图上得到相应的标准路径；然后对于不满足车辆动力学模型的标准路径进行平滑优化，采用基于二维分段高阶多项式的二次优化，得到最优可行路径；最后结合标准速度，得到无障碍物环境下的最优可行轨迹。
(2)在障碍物十字路口场景中，提出一种基于EM Planner、路径-速度迭代优化算法、动态规划+二次规划联合优化算法、RSS安全模型的决策规划方案，目的是避开感知范围内的静态、动态障碍物，对于无障碍物轨迹进行迭代修正，得到障碍物环境下的最优可行轨迹。方案是：首先基于无障碍物轨迹建立三维Frenet框架，并通过感知、预测模块得到静态、动态障碍物的信息；然后利用路径-速度迭代优化算法、动态规划+二次规划联合优化算法进行轨迹优化；最后得到障碍物环境下最优可行轨迹，并循环优化。
(3)对两个场景进行方案仿真，同时对无障碍物轨迹进行路径舒适性的评价指标进行分析；对障碍物轨迹进行路径舒适性、路径偏移量、速度舒适性、速度偏移量四个评价指标进行分析。
综上所述，本发明采用路径-速度迭代优化算法、动态规划、二次规划等方法来研究与进行十字路口场景下车辆的决策规划，并通过仿真实验，取得了不错的效果。
以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims (7)

1.一种无人驾驶车辆在十字路口的行驶轨迹规划方法，其特征在于，包括无障碍物场景十字路口场景决策规划步骤和障碍物十字路口场景决策规划步骤；
所述无障碍物场景十字路口场景决策规划步骤：在无障碍物十字路口场景中，首先根据车辆目的地确定车辆的行驶策略并在高精度地图上得到相应的标准路径；然后对于不满足车辆动力学模型的标准路径进行平滑优化，得到最优可行路径；最后结合标准速度，得到无障碍物环境下的最优可行轨迹；
所述障碍物十字路口场景决策规划步骤：在障碍物十字路口场景中，首先基于无障碍物轨迹建立三维Frenet框架，并通过感知、预测模块得到静态、动态障碍物的信息；然后进行轨迹优化；最后得到障碍物环境下最优可行轨迹，并循环优化；
基于十字路口环境的驾驶特点，所述无障碍物十字路口场景决策规划步骤包括：
步骤1：通过目的地的位置，确定车辆在十字路口环境下的行驶策略，根据车辆的行驶策略，行驶策略包括直行、左转、右转、掉头，通过高精度地图上提供的车辆定位和道路信息，生成相应的标准路径；
步骤2：对不满足动力学约束的标准路径利用平滑优化在其附近搜索出一条最优的可行平滑路径；
步骤3：车辆的行驶速度使用标准速度，将最优的可行平滑路径与标准速度结合，得到无障碍物可行轨迹；
在所述步骤2中，采用二次规划的优化方案进行平滑优化，二次规划的优化方案搜索的先决条件是确定路径参数化方式、确定代价函数、确定线性约束，然后利用二次规划快速收敛到最优的可行平滑路径；
将路径参数化用分段高阶多项式来表示路径，首先以一个较大的间隔ds对于路径的总弧长进行分割，得到n1+1个节点{si|i＝0,1,...,n1}；然后将每两个节点之间的路径用二维5阶多项式来表示：
Figure FDA0003888039010000011
其中s为车辆沿路径行驶的弧长；
Figure FDA0003888039010000012
是行驶弧长与其所在多项式的头节点进行差分运算后的结果，
Figure FDA0003888039010000013
二维分段多项式的段数为n1；Ai＝[ai0 ai1ai2 ai3 ai4 ai5]T,Bi＝[bi0 bi1 bi2 bi3 bi4 bi5]T为第i段高阶多项式的系数；路径参数化后，优化问题转化为对于分段高阶多项式系数取值的搜索，即对于最优的系数矩阵
Figure FDA0003888039010000021
的求解；
代价函数如下：
Costsmooth,path＝ws1∫(f″)2(s)+ws2∫(f″′)2(s)+ws3∫(g″)2(s)+ws4∫(g″′)2(s)
其中f,g为参数化后的路径函数，两者二阶导数f″,g″对应车辆行驶的曲率，两者的三阶导数f″′,g″′对应车辆行驶曲率的变化速度；ws1,ws2,ws3,ws4为权重；
所述线性约束包括搜索空间约束、节点约束、车辆自身约束，所述搜索空间约束是将搜索范围限制在标准路径的附近，目的是限制搜索区域以防出现搜索区域违反交通规则的情况，同时不让优化得到的路径距离标准路径过远；节点约束包括两个部分，连接处约束和起始终止点约束，连接处约束的目的是保证分段高阶多项式在不同段的连接处也足够的平滑，所以需要在多项式段的连接处相等且一阶导数、二阶导数、三阶导数相等，起始终止点约束的目的是保证车辆在起始点处的车辆位置和朝向满足初始和终止条件；车辆的自身约束是由于车辆自身机械结构的限制而导致的约束；对于路径的平滑优化而言，由于车辆的方向盘存在转向范围的约束和转向灵敏度的约束，所以需要将车辆曲率和曲率变化速度约束在一个范围内，该范围的边界由车辆本身的机械结构决定。
2.根据权利要求1所述的行驶轨迹规划方法，其特征在于，搜索空间约束的实现方法是：首先以一个较小的间隔ds′对十字路口标准路径进行采样ds′＜ds，得到n2+1个采样点{sj|j＝0,1,...,n2}，并通过sj的采样值在标准路径上得到路径点{(xj,yj)|j＝0,1,...,n2}，然后在采样点附近框定一个范围，将搜索区域限定在以采样点为中心，边长为boundary的正方形中，搜索空间约束表达式如下：
Figure FDA0003888039010000022
其中(xj,yj)为标准路径上采样点的坐标；(f(sj),g(sj))为相同行驶距离sj的情况下，规划路径对应的路径点坐标。
3.根据权利要求1所述的行驶轨迹规划方法，其特征在于，所述障碍物十字路口场景决策规划步骤包括：
步骤A：以无障碍物十字路口轨迹为基础，建立三维Frenet框架；
步骤B：感知模块和预测模块将感知到的静态障碍物状态、感知和预测到的动态障碍物状态、上一循环求得的车辆轨迹输入优化模块；
步骤C：优化模块基于步骤A得到的三维Frenet框架，并根据输入的障碍物信息，进行优化，优化模块分为路径优化、速度优化两个部分，迭代求解，得到优化后的路径曲线和速度曲线；
步骤D：将优化后得到的路径曲线和速度曲线相结合，并转换回笛卡尔坐标系，得到障碍物轨迹下的行驶轨迹；
步骤E：当感知模块感知到新的障碍物后，返回步骤B，循环求解。
4.根据权利要求3所述的行驶轨迹规划方法，其特征在于，在所述步骤A中，以无障碍物轨迹建立SLT坐标系；
在所述步骤C中，路径优化在SL坐标系下进行，S维度表示的是车辆沿无障碍物轨迹的行驶距离，L维度表示车辆偏离无障碍物轨迹的距离；速度优化在ST坐标系下进行，S维度同样是行驶距离，T维度是行驶时间；
在所述步骤C中，对于SLT的三维优化采用路径-速度迭代优化算法，求解优化采用动态规划结合二次规划的联合优化算法；
所述路径-速度迭代优化算法包括：
步骤S1，障碍物投影SL：静态障碍物直接投影，对于动态障碍物，提取出上一个循环的轨迹，用来估计车辆与感知范围内的动态障碍物预测轨迹之间的相交情况，并虚招相同时间点有重叠的部分，为期望碰撞区域；
步骤S2，路径优化：通过路径规划求解出最优的可行平滑路径曲线；
步骤S3，障碍物投影ST：根据步骤S2的可行平滑路径曲线，将与新的路径曲线有相交的障碍物投影到ST坐标系下；
步骤S4，速度优化：通过速度规划求解出最优的可行平滑速度曲线；
步骤S5，最后将最优路径曲线和最优速度曲线结合，得到最优的可行平滑轨迹。
5.根据权利要求4所述的行驶轨迹规划方法，其特征在于，所述动态规划结合二次规划的联合优化算法包括：
第一步，动态规划：对于SL坐标系/ST坐标系进行网格化采样，通过动态规划搜索出一个粗略的平滑曲线，通过该粗略的平滑曲线可以得到一个可行凸空间；
第二步，二次规划：以采样点为基础用一维分段高阶多项式将曲线参数化，以动态规划的结果作为启发，在动态规划得到的可行凸空间中用二次规划搜索出一条最优的可行平滑曲线。
6.一种无人驾驶车辆在十字路口的行驶轨迹规划系统，其特征在于，包括无障碍物十字路口场景决策规划模块和障碍物十字路口场景决策规划模块，
所述无障碍物十字路口场景决策规划模块：用于在无障碍物十字路口场景中，首先根据车辆目的地确定车辆的行驶策略并在高精度地图上得到相应的标准路径；然后对于不满足车辆动力学模型的标准路径进行平滑优化，得到最优可行路径；最后结合标准速度，得到无障碍物环境下的最优可行轨迹；
所述障碍物十字路口场景决策规划模块：用于在障碍物十字路口场景中，首先基于无障碍物轨迹建立三维Frenet框架，并通过感知、预测模块得到静态、动态障碍物的信息；然后进行轨迹优化；最后得到障碍物环境下最优可行轨迹，并循环优化；
基于十字路口环境的驾驶特点，所述无障碍物十字路口场景决策规划模块包括：
路径生成模块：用于通过目的地的位置，确定车辆在十字路口环境下的行驶策略，根据车辆的行驶策略，行驶策略包括直行、左转、右转、掉头，通过高精度地图上提供的车辆定位和道路信息，生成相应的标准路径；
搜索模块：用于对不满足动力学约束的标准路径利用平滑优化在其附近搜索出一条最优的可行平滑路径；
处理模块：车辆的行驶速度使用标准速度，将最优的可行平滑路径与标准速度结合，得到无障碍物可行轨迹；
在所述搜索模块中，采用二次规划的优化方案进行平滑优化，二次规划的优化方案搜索的先决条件是确定路径参数化方式、确定代价函数、确定线性约束，然后利用二次规划快速收敛到最优的可行平滑路径；
将路径参数化用分段高阶多项式来表示路径，首先以一个较大的间隔ds对于路径的总弧长进行分割，得到n1+1个节点{si|i＝0,1,...,n1}；然后将每两个节点之间的路径用二维5阶多项式来表示：
Figure FDA0003888039010000041
其中s为车辆沿路径行驶的弧长；
Figure FDA0003888039010000042
是行驶弧长与其所在多项式的头节点进行差分运算后的结果，
Figure FDA0003888039010000043
二维分段多项式的段数为n1；Ai＝[ai0 ai1ai2 ai3 ai4 ai5]T,Bi＝[bi0 bi1 bi2 bi3 bi4 bi5]T为第i段高阶多项式的系数；路径参数化后，优化问题转化为对于分段高阶多项式系数取值的搜索，即对于最优的系数矩阵
Figure FDA0003888039010000044
的求解；代价函数如下：
Costsmooth,path＝ws1∫(f″)2(s)+ws2∫(f″′)2(s)+ws3∫(g″)2(s)+ws4∫(g″′)2(s)
其中f,g为参数化后的路径函数，两者二阶导数f″,g″对应车辆行驶的曲率，两者的三阶导数f″′,g″′对应车辆行驶曲率的变化速度；ws1,ws2,ws3,ws4为权重；
所述线性约束包括搜索空间约束、节点约束、车辆自身约束，所述搜索空间约束是将搜索范围限制在标准路径的附近，目的是限制搜索区域以防出现搜索区域违反交通规则的情况，同时不让优化得到的路径距离标准路径过远；节点约束包括两个部分，连接处约束和起始终止点约束，连接处约束的目的是保证分段高阶多项式在不同段的连接处也足够的平滑，所以需要在多项式段的连接处相等且一阶导数、二阶导数、三阶导数相等，起始终止点约束的目的是保证车辆在起始点处的车辆位置和朝向满足初始和终止条件；车辆的自身约束是由于车辆自身机械结构的限制而导致的约束；对于路径的平滑优化而言，由于车辆的方向盘存在转向范围的约束和转向灵敏度的约束，所以需要将车辆曲率和曲率变化速度约束在一个范围内，该范围的边界由车辆本身的机械结构决定。
7.一种计算机可读存储介质，其特征在于：所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现权利要求1－5中任一项所述的行驶轨迹规划方法的步骤。
CN202010718402.0A 2020-07-23 2020-07-23 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质 Active CN112068545B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010718402.0A CN112068545B (zh) 2020-07-23 2020-07-23 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010718402.0A CN112068545B (zh) 2020-07-23 2020-07-23 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质

Publications (2)

Publication Number Publication Date
CN112068545A CN112068545A (zh) 2020-12-11
CN112068545B true CN112068545B (zh) 2022-12-27

Family

ID=73656625

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010718402.0A Active CN112068545B (zh) 2020-07-23 2020-07-23 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质

Country Status (1)

Country Link
CN (1) CN112068545B (zh)

Families Citing this family (37)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112577506B (zh) * 2020-10-30 2023-04-07 上汽大众汽车有限公司 一种自动驾驶局部路径规划方法和系统
CN112631295B (zh) * 2020-12-17 2023-06-06 北京汽车研究总院有限公司 路径规划方法、装置及车辆
CN112859849A (zh) * 2021-01-06 2021-05-28 北京三快在线科技有限公司 自动行驶设备的路口运动规划方法、装置、电子设备
CN112987721B (zh) * 2021-02-01 2022-12-13 哈尔滨工业大学 一种多agv调度装置及其全局规划与局部规划融合方法
CN112896187B (zh) * 2021-02-08 2022-07-26 浙江大学 一种考虑社会兼容的自动驾驶决策的系统与方法
CN112905849A (zh) * 2021-02-18 2021-06-04 中国第一汽车股份有限公司 一种车辆数据处理的方法及装置
CN115083139B (zh) * 2021-03-12 2023-11-24 上海交通大学 多车辆调度方法
CN113055821B (zh) * 2021-03-15 2023-01-31 北京京东乾石科技有限公司 用于发送信息的方法和装置
CN113044029B (zh) * 2021-03-19 2022-03-15 北京理工大学 一种保证无人车在三维地形上安全行驶的运动规划方法
CN112953780B (zh) * 2021-03-23 2022-04-19 同济大学 封闭场景下无人驾驶车群构建及评价方法
WO2022199855A1 (en) * 2021-03-26 2022-09-29 Embotech Ag Method and system for controlling autonomous or semi-autonomous vehicle
CN113110489B (zh) * 2021-04-30 2023-03-10 清华大学 一种轨迹规划方法、装置、电子设备和存储介质
WO2022241771A1 (zh) * 2021-05-21 2022-11-24 华为技术有限公司 一种预测方法、装置、车辆和智能驾驶系统
CN113085850B (zh) * 2021-06-08 2021-09-21 新石器慧通（北京）科技有限公司 车辆避障方法、装置、电子设备及存储介质
CN113553255A (zh) * 2021-06-16 2021-10-26 阿波罗智联(北京)科技有限公司 路线场景文件的生成方法、装置、电子设备及存储介质
CN113483776A (zh) * 2021-07-01 2021-10-08 广州小鹏自动驾驶科技有限公司 路径规划方法、装置及汽车
CN113654556A (zh) * 2021-07-05 2021-11-16 的卢技术有限公司 一种基于改进em算法的局部路径规划方法、介质及设备
CN113433947B (zh) * 2021-07-15 2022-04-01 天津大学 基于障碍车估计与预测的交叉路口轨迹规划与控制方法
CN113295181B (zh) * 2021-07-28 2022-04-15 国汽智控(北京)科技有限公司 车辆的路径规划方法、装置和系统
CN113997951A (zh) * 2021-08-17 2022-02-01 北京百度网讯科技有限公司 车辆的行驶轨迹的确定方法、装置及自动驾驶车辆
CN113411752B (zh) * 2021-08-19 2021-11-16 智己汽车科技有限公司 一种车辆循迹过路口的方法及设备
CN113460091B (zh) * 2021-08-23 2022-10-21 吉林大学 一种无保护十字路口无人车滚动优化决策方法
KR20240025632A (ko) * 2021-08-25 2024-02-27 우이시 테크놀로지스 (베이징) 리미티드. 차량 의사결정 계획 방법, 장치, 기기 및 매체
CN113844445B (zh) * 2021-09-14 2022-12-30 浙江零跑科技股份有限公司 基于预测参考线坐标系的车辆自动紧急制动系统与方法
CN113895463B (zh) * 2021-11-25 2023-06-06 北京航空航天大学 一种适用于自动驾驶车辆掉头的路径规划方法
CN113885771B (zh) * 2021-12-09 2022-03-18 北京车网科技发展有限公司 一种轨迹信息的可视化处理方法
CN114234993A (zh) * 2021-12-15 2022-03-25 北京福田戴姆勒汽车有限公司 车辆局部路径规划方法、自动驾驶系统和自动驾驶车辆
CN114235441A (zh) * 2021-12-31 2022-03-25 深兰人工智能(深圳)有限公司 无人车实时碰撞检测方法及相关装置
CN114543825A (zh) * 2022-01-12 2022-05-27 华为技术有限公司 引导车辆行驶的方法、地图生成方法及相关系统
CN114115298B (zh) * 2022-01-25 2022-05-17 北京理工大学 一种无人车路径平滑方法及系统
US20230242142A1 (en) * 2022-02-01 2023-08-03 Weize Zhang Systems, methods, and computer-readable media for spatio-temporal motion planning
CN114537435A (zh) * 2022-03-03 2022-05-27 湖州快飞智能科技有限公司 一种自动驾驶中的实时整车轨迹规划方法
CN114740849B (zh) * 2022-04-07 2023-07-04 哈尔滨工业大学（深圳） 基于行人步行决策规则的移动机器人自主导航方法及装置
CN115171414B (zh) * 2022-06-10 2023-07-14 哈尔滨工业大学重庆研究院 一种基于Frenet坐标系的CACC跟驰车流控制系统
CN115200604B (zh) * 2022-09-16 2023-05-16 广州小鹏自动驾驶科技有限公司 转弯路径规划方法、设备、车辆及存储介质
CN117032201B (zh) * 2023-03-07 2024-04-12 山西省智慧交通研究院有限公司 一种基于车路协同的矿井下自动驾驶车辆协调规划方法
CN116499486B (zh) * 2023-06-26 2023-09-22 北京理工大学 一种复杂越野环境路径规划方法、系统及电子设备

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9720415B2 (en) * 2015-11-04 2017-08-01 Zoox, Inc. Sensor-based object-detection optimization for autonomous vehicles
CN105741595B (zh) * 2016-04-27 2018-02-27 常州加美科技有限公司 一种基于云端数据库的无人驾驶车辆导航行车方法
CN108877268B (zh) * 2018-08-07 2021-05-25 南京大学 一种面向无人驾驶的无红绿灯十字路口智能调度方法
CN109324620A (zh) * 2018-09-25 2019-02-12 北京主线科技有限公司 基于车道线平行偏移进行避障及超车的动态轨迹规划方法
CN109976355B (zh) * 2019-04-26 2021-12-10 腾讯科技（深圳）有限公司 轨迹规划方法、系统、设备及存储介质
CN110304074B (zh) * 2019-06-28 2020-07-31 电子科技大学 一种基于分层状态机的混合式驾驶方法
CN110749333B (zh) * 2019-11-07 2022-02-22 中南大学 基于多目标优化的无人驾驶车辆运动规划方法

Also Published As

Publication number Publication date
CN112068545A (zh) 2020-12-11

Similar Documents

Publication Publication Date Title
CN112068545B (zh) 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质
US11561542B2 (en) Safety and comfort constraints for navigation
US20210394757A1 (en) Vehicle trajectory modification for following
US11708093B2 (en) Trajectories with intent
US9405293B2 (en) Vehicle trajectory optimization for autonomous vehicles
US7085633B2 (en) System and method for improving vehicle operator driving assistance of automotive vehicle
WO2020178639A1 (en) Aligning road information for navigation
CN112639849A (zh) 路径选择方法和路径选择装置
US11586209B2 (en) Differential dynamic programming (DDP) based planning architecture for autonomous driving vehicles
CN114258366A (zh) 对于自主车辆的折线轮廓表示
US20220185266A1 (en) Velocity-based relevance filter
US20210181750A1 (en) Blocked region guidance
EP3869342A1 (en) System and method for generating simulation scenario definitions for an autonomous vehicle system
Andersen et al. Trajectory optimization and situational analysis framework for autonomous overtaking with visibility maximization
CN111959498A (zh) 一种自动驾驶车辆的垂直泊车方法及其装置、车辆
CN113703436A (zh) 用于路径规划的并行障碍物缓冲系统
US11628858B2 (en) Hybrid planning system for autonomous vehicles
WO2023024542A1 (zh) 车辆决策规划方法、装置、设备及介质
CN116867675A (zh) 车辆运动规划系统与方法
US11810365B1 (en) Perception error modeling
CN114217601B (zh) 自驾车的混合决策方法及其系统
US20230053243A1 (en) Hybrid Performance Critic for Planning Module's Parameter Tuning in Autonomous Driving Vehicles
CN114074657B (zh) 基于视觉的避免碰撞的系统和方法
Bajić et al. Trajectory planning for autonomous vehicle using digital map
WO2024026241A1 (en) Reference trajectory validating and collision checking management

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