CN112810630A

CN112810630A - 一种自动驾驶车辆轨迹规划方法及系统

Info

Publication number: CN112810630A
Application number: CN202110167369.1A
Authority: CN
Inventors: 周风余; 高阳天; 万方; 尹磊; 夏英翔
Original assignee: Shandong University
Current assignee: Shandong University
Priority date: 2021-02-05
Filing date: 2021-02-05
Publication date: 2021-05-18
Anticipated expiration: 2041-02-05
Also published as: CN112810630B

Abstract

本发明属于车辆轨迹规划领域，提供了一种自动驾驶车辆轨迹规划方法及系统。其中，该方法包括获取车辆行驶参考线与障碍物地图；对终点位置进行纵向采样和横向采样，得到位置采样集合和速度采样集合；从位置采样集合中遍历采样点，启发式估算轨迹长度；从速度采样集合中遍历采样速度，估算目标时间，从而计算出轨迹方程；根据轨迹时间长度来判断轨迹的完整性，计算完整轨迹的轨迹代价并存入有序队列，当速度采样遍历结束后，从有序队列中依次取出代价最小的轨迹并进行碰撞检测，直至有序队列为空；基于碰撞检测及轨迹危险度，输出规划的自动驾驶车辆轨迹。

Description

一种自动驾驶车辆轨迹规划方法及系统

技术领域

本发明属于车辆轨迹规划领域，尤其涉及一种自动驾驶车辆轨迹规划方法及系统。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

随着电商发展速度越来越快、人力成本不断上涨，物流配送的“最后一公里”问题越发突出：运输物流的最后一段距离占据了总体运输成本的三分之一，尤其是对于地广人稀的农村地区而言这是一个更大的挑战。因此，物流行业对拥有能够在城市、农村复杂道路环境通行能力的低速无人驾驶车辆提出了越来越大的需求。其中，轨迹规划是无人驾驶技术的核心组成部分，在整个系统中起到承上启下的作用。

对于解决规划问题：最开始的时候，一些机器人规划算法被直接应用在车辆上，如动态窗口法。该算法给予了车辆比较大的灵活性，能够进行灵活的避障。但是该算法是利用高频率的规划来应对环境的变化和保证车辆路径的可执行性，两次规划之间可能变化比较大，车身控制输出因此会有波动，尤其是车身状态的高阶导数项会有很严重的跳变，造成车辆运行不平稳。该算法输出的结果虽然能确保被车辆执行，但是输出不平稳，导致车辆故障率提高、寿命降低；过高的灵活性会导致与其交互的车辆很难估计自动驾驶车辆的运动方式，增加了事故率、降低了通行效率，不符合自动驾驶车辆的初衷。

基于搜索的方法是比较常用的算法，例如Hybrid A*算法，但是该算法要求终点固定，一般难以应用在道路行驶上；生成的路径曲率不连续，需要对路径进行进一步的优化；路径没有速度信息，难以躲避动态障碍物；搜索时难以控制搜索方向，无法充分利用道路信息，导致效率比较低；需要进行大量碰撞检测，难以满足实时性。一般该类算法只用于泊车场景。

在道路上行驶时，由于有明确的道路概念，车辆的规划一般由全局规划和局部规划组成。全局规划给出车辆的道路指引线，车辆需要尽量追随指引线行驶。车辆需要具有一定的避障能力以躲避路上的障碍物，这给自动驾驶车辆的规划带来了很大的挑战。自动车辆是一个非线性系统，且具有横向位移跟纵向位移耦合的特点，并且对于大部分驾驶行为而言更需要考虑长度比较长的规划行为(百米级别)，直接在笛卡尔坐标系上进行规划性能非常低。为此，目前大部分规划算法都会利用车辆参考线的特点，将笛卡尔坐标系转为Frenet坐标系进行规划，例如樊昊阳提出的Em planner、王沙晶提出的一种基于Frenet坐标系规划方法、Nolte提出的MPC方法、刘畅流提出的CFS算法、Lattice planner等。在Frenet坐标系下进行规划可以很容易的将问题解耦，分为横向规划和纵向规划两个部分进行处理，这样可以规划非常长的距离。然而，在Frenet坐标系和笛卡尔坐标系进行转换的过程中将车辆模型进行了大量的简化，对于车辆前轮转角较大的情况无法正确的进行转换，导致实际规划结果无法执行。同时，低速车辆并不需要规划比较长的距离，往往只需要规划20米左右即可，更关注规划结果能否执行和能否机动避障，因此需要直接在笛卡尔坐标系下进行规划。

在笛卡尔坐标系上进行规划时，大部分算法都是使用对状态进行采样，然后使用曲线进行连接的方法。一类是以RRT算法为代表的随机采样算法，该类算法能够很快的找到可行解，但是很难保证轨迹平滑，也不能保证充分搜索配置空间，有可能出现两帧输出轨迹差距较大而导致车辆运行不稳定；同时由于搜索状态的随机性，如果希望能够希望较优的轨迹，时间消耗很难控制。

另外一类采样算法是以Lattice planner为代表的均匀采样算法，该算法结合了搜索算法和采样算法的优势，通过对搜索空间进行采样，缩小了状态空间的数量，减少了搜索时间，保证了搜索效率；通过设置代价函数，以获得最优轨迹；利用多项式对状态采样进行连接来保证路径的平滑。但是该算法依然是基于Frenet坐标系进行规划，并不适用于低速规划。

综上所述，由于低速物流车辆在很多情况下运行在没有明确的道路范围、环境多变、障碍物较多的环境中，例如园区、校园等，所以需要尽可能的利用允许范围内的灵活度来实现机动避障，但是发明人发现，目前的自动驾驶车辆的轨迹规划方法大部分都是针对长距离的规划，基本无需考虑实际能否执行；而使用机器人的灵活避障方法则会造成车辆的不稳定，并且更有可能造成交通事故。

发明内容

为了解决上述背景技术中存在的技术问题，本发明提供一种自动驾驶车辆轨迹规划方法及系统，其专门针对低速物流车辆进行轨迹规划，通过设置合理的轨迹表达方式和采样方式，能够确保输出的曲线能够被车辆执行，同时也能保证轨迹有一定的长度，减少重新规划的频率。其中，车速低于4m/s的情况下视为低速。

为了实现上述目的，本发明采用如下技术方案：

本发明的第一个方面提供一种自动驾驶车辆轨迹规划方法。

一种自动驾驶车辆轨迹规划方法，包括：

获取车辆行驶参考线与障碍物地图；

对终点位置进行纵向采样和横向采样，得到位置采样集合和速度采样集合；

从位置采样集合中遍历采样点，启发式估算轨迹长度；从速度采样集合中遍历采样速度，估算目标时间，从而计算出轨迹方程；

根据轨迹时间长度来判断轨迹的完整性，计算完整轨迹的轨迹代价并存入有序队列，当速度采样遍历结束后，从有序队列中依次取出代价最小的轨迹并进行碰撞检测，直至有序队列为空；

基于碰撞检测及轨迹危险度，输出规划的自动驾驶车辆轨迹；其中，自动驾驶车辆轨迹满足以下任一条件：(1)无碰撞且轨迹危险度为0的轨迹；(2)无碰撞且轨迹危险度不大于设定危险度的代价最小的轨迹。

进一步地，在碰撞检测的过程中，若有序队列中取出的轨迹发生碰撞，则重新从有序队列中取出代价最小的轨迹。

进一步地，在碰撞检测的过程中，若有序队列中取出的轨迹未发生碰撞，则计算轨迹危险度，输出危险度为0的轨迹并作为规划的自动驾驶车辆轨迹。

进一步地，当轨迹危险度不是0时，判断危险度是否大于设定危险度，若是，则从有序队列中取出代价最小的轨迹，继续进行碰撞检测及轨迹危险度判断；若否，将轨迹加入优先队列，当优先队列为空时，停车，优先队列为非空时，从优先队列中获取代价最小的轨迹并作为规划的自动驾驶车辆轨迹。

进一步地，轨迹代价函数为轨迹上每个点到参考线距离的加权平均、最大向心加速度和轨迹最终到达的速度与对应权重的累加和。

进一步地，轨迹危险度采用危险度总代价来表示，危险度总代价表示为：

所有动态碍物到轨迹的最近距离平方和的倒数的累加和项以及所有静态障碍物到轨迹的最近距离的倒数的累加和项，这两个累加和项与对应权重系数的相乘后的累加和。

进一步地，轨迹方程采用五次多项式表示。

本发明的第二个方面提供一种自动驾驶车辆轨迹规划系统。

一种自动驾驶车辆轨迹规划系统，包括：

参考线及地图获取模块，其用于获取车辆行驶参考线与障碍物地图；

采样模块，其用于对终点位置进行纵向采样和横向采样，得到位置采样集合和速度采样集合；

轨迹方程计算模块，其用于从位置采样集合中遍历采样点，启发式估算轨迹长度；从速度采样集合中遍历采样速度，估算目标时间，从而计算出轨迹方程；

轨迹代价计算模块，其用于根据轨迹时间长度来判断轨迹的完整性，计算完整轨迹的轨迹代价并存入有序队列，当速度采样遍历结束后，从有序队列中依次取出代价最小的轨迹并进行碰撞检测，直至有序队列为空；

规划轨迹输出模块，其用于基于碰撞检测及轨迹危险度，输出规划的自动驾驶车辆轨迹；其中，自动驾驶车辆轨迹满足以下任一条件：(1)无碰撞且轨迹危险度为0的轨迹；(2)无碰撞且轨迹危险度不大于设定危险度的代价最小的轨迹。

本发明的第三个方面提供一种计算机可读存储介质。

一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上述所述的自动驾驶车辆轨迹规划方法中的步骤。

本发明的第四个方面提供一种计算机设备。

一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述所述的自动驾驶车辆轨迹规划方法中的步骤。

与现有技术相比，本发明的有益效果是：

本发明的适用于低速情况下车辆的轨迹规划方法，能够快速生成运行轨迹，找出车辆可以通行的近似最优可行解，既可以直接作为输出结果让执行器执行，也可以作为优化器的种子轨迹用以求解最优轨迹，实现容易、对运算资源要求低，可以在低速车辆或者机器人上进行快速部署，通过设置合理的轨迹表达方式和采样方式，确保了输出的曲线能够被车辆执行，同时也保证了轨迹有一定的长度，减少了重新规划的频率。同时，也可以根据算力限制，对参数进行修改以获得满意的性能表现。

本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1是本发明实施例的自动驾驶车辆轨迹规划方法流程图；

图2是本发明实施例的车辆运动学模型；

图3是本发明实施例的弯道纵向采样；

图4是本发明实施例的最终获得的采样点；

图5(a)是本发明实施例的弯道距离估算；

图5(b)是本发明实施例的插值估算方法；

图6是本发明实施例的一清创新夸父2无人车；

图7(a)是本发明实施例的虚拟大学园平台大楼停车坪实验场地点云地图；

图7(b)是本发明实施例的富士康园区实验场地点云地图；

图8(a)是本发明实施例的在不满足车辆运动学的弯道处规划的轨迹输出；

图8(b)是本发明实施例的在不满足车辆运动学的弯道处规划的前轮转角变化和速度变化；

图9(a)是本发明实施例的直行避障轨迹输出；

图9(b)是本发明实施例的直行避障参数输出。

具体实施方式

下面结合附图与实施例对本发明作进一步说明。

应该指出，以下详细说明都是例示性的，旨在对本发明提供进一步的说明。除非另有指明，本实施例使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

实施例一

如图1所示，本实施例提供了一种自动驾驶车辆轨迹规划方法，其具体包括如下步骤：

获取车辆行驶参考线与障碍物地图；

根据轨迹时间长度来判断轨迹的完整性，计算完整轨迹的轨迹代价并存入有序队列q，当速度采样遍历结束后，从有序队列q中依次取出代价最小的轨迹并进行碰撞检测，直至有序队列q为空；

在具体实施中，在碰撞检测的过程中，若有序队列q中取出的轨迹发生碰撞，则重新从有序队列q中取出代价最小的轨迹。

若有序队列q中取出的轨迹未发生碰撞，则计算轨迹危险度，输出危险度为0的轨迹并作为规划的自动驾驶车辆轨迹。

当轨迹危险度不是0时，判断危险度是否大于设定危险度，若是，则从有序队列q中取出代价最小的轨迹，继续进行碰撞检测及轨迹危险度判断；若否，将轨迹加入优先队列qobs，当优先队列qobs为空时，停车，优先队列qobs为非空时，从优先队列qobs中获取代价最小的轨迹并作为规划的自动驾驶车辆轨迹。

在本实施例中，车辆在运行时有其运动学限制，因此需要对其运动学进行推导。

目前常见的车辆结构都为阿克曼结构，如图2所示。在低速情况下，一般认为前轮的运动方向沿着前轮朝向，所以可以使用自行车模型来替代阿克曼模型。

以车辆后轴为车辆的原点，前轮转角为

设轴距为L，车辆转向半径可以表示为

曲率为半径的倒数

由此可得：

车辆的运动学限制主要包括加速度限制、车辆的速度限制、前轮角速度限制与车辆最大曲率限制，下标ego代表自车：

设车体坐标系与世界坐标系的夹角为θ，那么任意时刻车辆的状态可以用q＝(x,y,θ)来表示。在求取速度分量的时候，可以认为在一个非常短的时间dt内，车辆的运动方向不发生改变。因此可以得到车辆在世界坐标系下的速度分量：

由式(2)可以得到车辆纵向加速度在世界坐标系下的分量。在考虑加速度时，需要考虑车辆在转向时受到的向心加速度。有向心加速度：

向心加速度方向垂直于车身运动方向，对其进行分解即可得到加速度方程：

在车辆行驶的时候，希望保证车辆的位置、速度、加速度、前轮转角连续，减小运行时的振动与不稳定。根据上述推导，首尾的车辆状态可以分别转换到x和y方向上，当终点状态被确定的时候，可以获得六个约束方程。为了加快运算时间，使用闭式求解，表达成时间t的五次多项式形式。

设0时刻车辆的状态在x轴上的分量为

采样的t时刻在x轴分量为

可以得到式(7)。

很容易得到a＝(P^-1·X)^T。当时间t被确定且不为0时，参数可以被唯一确定。

综上，求解一条轨迹需要对终点的车辆状态和达到终点状态的时间t进行采样。

终点状态采样：

车辆在行驶的时候，路由规划层会下发参考指引线，车辆需要沿着该指引线来前进来到达最终的目的地，车辆允许一定程度的偏离参考线，但是总的方向需要保证和参考线一致。本算法使用的参考线已经事先进行过平滑。每帧的参考线原点为离车辆后轴垂线最近的参考点，每个参考点的信息包括位置、方向、曲率以及距离原点的参考线距离，在此基础上对轨迹进行采样。首先对车辆的轨迹末端状态进行采样。

车辆状态采样需要遵循三个原则：

1、计算的轨迹应当尽量贴近参考线；

2、能够尽量躲避障碍物；

3、尽量保证采样的结果是可用的，提高规划成功率。

由原则1，车辆状态应该尽量和参考线上的点一致，确保车辆能够沿着参考线行驶，因此采样状态的角度和曲率与采样点保持一致。

在曲率较大的弯道进行采样时，沿着参考线采样距离过近或者过远都会导致远离参考线，如图3所示，在一些连续的弯道也会出现相同的问题，只有在合适的范围内进行采样的轨迹效果才会比较好。因此需要在纵向采样多个点，以保证采样的轨迹包含尽量多的状态，再从中选出最优轨迹。以车辆所在位置对应的参考点为原点，沿着参考线每隔一段距离s_n取一个参考线采样点p_n，下标表示第n个采样点。沿着参考线方向进行多次采样称为纵向采样。s_n为第n个点的纵向采样距离，长度通过对时间和终点速度采样来获得，避免采样的长度过长或者过短。假设采样的时间长度为t_sample，采样的终点速度为v_sample，中间为匀加速，那么采样距离为：

当采样速度和车辆速度比较小的时候，采样距离s₁可能比较小，两个相邻的采样间距Δs也比较小，采样效果不理想，应该限制最小采样长度。即：

由原则2，车辆应该能够横向机动避障。因此需要沿着垂直于参考线的方向进行采样，以实现机动避障，称为横向采样。以p_n为基准向垂直方向进行采样，设采样宽度为d_n，采样k个点，那么采样点p_nk距离p_n的距离d_nk可以表示为式(5)。k一般需要取奇数，以保证中间的采样点在参考线上。d_n称为横向采样距离。由同一个纵向采样点pn进行横向采样出来的一系列采样点称为第n层采样点。

以此可以得到每个采样点的位置与曲率参数：

一般情况下，每一层采样点都可以使用相同的横向采样距离。但是对于比较靠近车辆的采样点，使用与其他采样点相同的横向采样距离会导致大部分轨迹无法满足运动学。可以通过加长这些点的纵向采样距离，或者减少横向采样距离来解决这个问题。

最后获得的采样点如图4所示。

对终点速度的采样，可以根据需要和算力进行选择，一般设定两到三个终点速度即可。由于最终的目标是让车能够达到指定的速度，平稳运行，因此终点处的车辆加速度一般设为0。

启发式时间采样：

完成了轨迹终点状态的采样后，还需要对到达终点的时间进行采样。这里使用了一种启发式的采样方法来获得终点的时间。

假设从起点到终点为匀加速运动，起点速度为v₀，终点速度为v₁，从起点到终点的距离为s，那么可以得到通过的时间t：

其中s为从起始点到终点的距离。但是，在轨迹计算出来之前，轨迹的长度是不可知的，因此需要估计计算输出的轨迹长度。一种方法是直接使用欧式距离作为输出的轨迹，该方法可以近似估算在直道上进行采样的轨迹，但是在弯道上进行采样的时候来说，估算的距离与实际距离差距很大，如图5(a)所示，欧式距离远小于实际轨迹长度，计算的时间t远小于真实时间，造成输出的轨迹速度会先剧烈加速再减速，运行时不够平稳，也不符合一般驾驶习惯。本实施例使用一种基于参考线插值的方法来估算从起点到终点的距离，如图5(b)所示。

已知车辆距离参考线最近的参考点距离为d₀，而采样点p_nk距离p_n的距离为d_nk，离车辆的参考线距离为s_n，设从该参考点到车辆有a₁个参考点

其中j∈[1,a₁]，可以以此获得插值点坐标

然后可以由此获得插值点形成的曲线的长度s_traj：

用该长度作为式6中的距离s，得到每段曲线启发式的估计时间。图5(a)展示了该插值方法在弯道的插值效果，可以看出该启发式方法能够较好的估算输出的曲线长度。以此，可以获得比较准确的到达采样点时的时间，以获得平稳的速度剖面。

代价函数与轨迹选择：

获得了轨迹方程后，还要将轨迹表达成离散点的形式输出，方便底层控制输入和计算每条轨迹的代价。对于低速规划车辆而言，一方面由于传感器性能限制，能够准确预测和识别的动态障碍物范围较近，另一方面由于车速不高，不需要对非常长时间内的轨迹进行规划，因此能够保证规划一定时间内即可。这个规划时间被称为采样时间窗口t_win。由计算出的启发式估计时间可能大于采样时间窗口，这种情况下需要对输出的轨迹进行截断；也有可能采样的距离较短，导致采样的轨迹时间不足时间窗口，需要对该段轨迹进行补足，再次进行计算。为了保证计算效率，此时只计算和轨迹终点采样点拥有相同横向采样距离的采样点的轨迹点，如图5(a)和图5(b)所示。

输出的轨迹点主要包括以下信息：位置x、速度v、方向β、车辆朝向、前轮转角变化率、车辆加速度。假设第a个轨迹点的采样时刻为t，可以得到其位置、速度、加速度、方向、沿着轨迹的长度、曲率：

速度、加速度、曲率公式使用了多次根号，计算效率很低，设两个轨迹点采样时间间隔为dt，对于非首尾的轨迹点，可以使用式14近似替代。

经过采样之后，需要从采样的轨迹中选择出最优的轨迹，也即需要设计一个代价函数，来判断轨迹的质量。轨迹的代价函数遵循以下原则：

(1)轨迹需要尽量贴近参考线；

(2)轨迹的最大向心加速度越小越好，以提升转弯时的舒适性；

(3)为保证效率，在采样时间窗口内能达到的速度越大越好；

(4)轨迹应该满足式1的运动学约束，并且不能发生碰撞；

(5)轨迹尽量离障碍物有一定的安全距离。

由轨迹的代价函数遵循的原则1～3，可以设计出轨迹在贴近参考线、舒适度和效率这三个方面的代价值，如表1。其中a2为输出的轨迹点的数目，

表1轨迹代价的定义

最终的轨迹代价函数为几个代价加权平均获得：

c＝w_dc_d+w_ρc_ρ+w_vc_v (15)

其中w_d，w_ρ和w_v为每项代价的权重，在使用时根据需要进行调整，c为整个轨迹的代价，该值越低代表轨迹质量越高。

由轨迹的代价函数遵循的原则4，可以将不满足约束和预测会发生碰撞的轨迹去除。实际使用时，可以将运动学约束放宽，个别点的约束不满足不影响整体轨迹的可行性。

经过以上几个步骤之后可以获得一个轨迹代价优先队列，再从轨迹代价较小的轨迹中尽量选择离障碍物相对较远的轨迹，确保运行的安全性。从所有轨迹中依次选出代价最小的轨迹，对轨迹进行碰撞检测，获得轨迹距离所有障碍物的距离，根据距离障碍物的距离对轨迹进行安全性评估。对于行人、运动中的车辆等动态障碍物，选用动态安全距离d_dynamic；对于其他障碍物，例如路沿、路边停泊的车辆，选用静态安全距离d_static。如果轨迹离障碍物的最小距离在安全距离内，认为该轨迹是有危险的，需要从剩余轨迹中再选择出代价最小的轨迹进行评估，如此反复，直到找到没有处于危险区的轨迹。如果所有的轨迹都有危险，那么将会采取一个新的代价函数进行择优选择，以确保安全性，如式(16)。

其中，a₃为所有与轨迹距离小于d_static的静态障碍物的距数量；a₄为所有与轨迹距离小于d_dynamic的动态障碍物的数量；d_i、d_j分别为每个动态和静态障碍物到轨迹的最近距离，w₀和w₁为对应的系数。一般来说，w₁需要设置的比w₀大很多，以确保面对动态障碍物时有足够的冗余，降低危险性，同时保证能够穿过狭窄的道路。C_obs为危险度总代价，该代价值越低代表轨迹危险度越低。

此外，还需要设置一个危险度阈值c_danger，对于c_obs<c_danger的轨迹，视为危险度过高，不可使用。

实验验证：

本实施例使用深圳一清创新有限公司的夸父无人车作为实验平台，图6为实物照片。车辆使用四个激光雷达和四个摄像头来实现对周围环境的感知，能够准确的识别其他障碍物的位置和状态，并且对可能移动的障碍物的行为进行预测。

实验平台的处理器为i7-8700，内存为16Gb，实验系统为Ubuntu16.04，使用C++作为编程语言，在ROS kinetic中运行。实验环境为深圳虚拟大学园平台大楼停车坪和富士康龙华科技园。停车坪的场地比较开阔，可以视为开放地域，没有左右车道线约束；富士康道路为普通非结构化道路，道路情况较为复杂，有比较多的障碍。图7(a)和图7(b)为构建的点云地图。车辆参数如表2所示。

表2车辆参数表

参数	数值
		长度	3560mm
宽度	1560mm
		轴距	1900mm
最大速度	8m/s
		最大加速度	2.2m/s<sup>2</sup>
前轮最大转角	30°
		前轮最大角速度	10°/s

运行时算法的参数如表3所示。

表3算法参数表

在不满足车辆运动学的弯道处的规划如图8(a)所示，其前轮转角变化和速度变化如图8(b)所示。

在直道上规划的结果如图9(a)所示，其前轮转角变化和速度变化如图9(b)所示。

可以看出规划出的结果可以让车辆能够很好的沿着参考线运行，车速平稳。

从实验结果可以看出，车辆能够在复杂环境中找到合理的通行路径，躲避动静态障碍物，实现灵活的运行策略；同时，输出的轨迹平滑，车辆能够平稳的执行。

该算法的运行效率如表4所示，设采样的速度值有h个。

表4运行效率与算法参数的关系

可以看出本算法的运算时间基本和采样的轨迹数目成正比，采样的数量越多，计算的速度越慢。该算法运行时间非常平稳，波动很小，波动范围只和碰撞检测耗费的时间相关，算法稳定性非常强，这对于自动驾驶车辆至关重要，尤其是在面对紧急情况时，稳定的规划速度能够及时对危险做出反应，

防止出现意外。在应用的时候，可以根据系统的算力和实时性要求，合理的设置采样参数，以获得最为平衡的实时性要求和轨迹效果。

本实施例在实际的自动驾驶车辆上验证了该算法的有效性和实时性。实验证明该算法能够适应复杂的环境和非标准化的道路，能够快速生成能够满足车辆运动学的轨迹，是一种非常实用的算法。

实施例二

本实施例提供了一种自动驾驶车辆轨迹规划系统，其具体包括如下模块：

轨迹代价计算模块，其用于根据轨迹时间长度来判断轨迹的完整性，计算完整轨迹的轨迹代价并存入有序队列q，当速度采样遍历结束后，从有序队列q中依次取出代价最小的轨迹并进行碰撞检测，直至有序队列q为空；

此处需要说明的是，本实施例的自动驾驶车辆轨迹规划系统中的各个模块与实施例一中的自动驾驶车辆轨迹规划方法中的各个步骤一一对应，其具体实施过程相同，此处不再累述。

实施例三

本实施例提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上述实施例一所述的自动驾驶车辆轨迹规划方法中的步骤。

实施例四

本实施例提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述实施例一所述的自动驾驶车辆轨迹规划方法中的步骤。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用硬件实施例、软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(RandomAccessMemory，RAM)等。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims

1.一种自动驾驶车辆轨迹规划方法，其特征在于，包括：

获取车辆行驶参考线与障碍物地图；

2.如权利要求1所述的自动驾驶车辆轨迹规划方法，其特征在于，在碰撞检测的过程中，若有序队列中取出的轨迹发生碰撞，则重新从有序队列中取出代价最小的轨迹。

3.如权利要求1所述的自动驾驶车辆轨迹规划方法，其特征在于，在碰撞检测的过程中，若有序队列中取出的轨迹未发生碰撞，则计算轨迹危险度，输出危险度为0的轨迹并作为规划的自动驾驶车辆轨迹。

4.如权利要求1所述的自动驾驶车辆轨迹规划方法，其特征在于，当轨迹危险度不是0时，判断危险度是否大于设定危险度，若是，则从有序队列中取出代价最小的轨迹，继续进行碰撞检测及轨迹危险度判断；若否，将轨迹加入优先队列，当优先队列为空时，停车，优先队列为非空时，从优先队列中获取代价最小的轨迹并作为规划的自动驾驶车辆轨迹。

5.如权利要求1所述的自动驾驶车辆轨迹规划方法，其特征在于，轨迹代价函数为轨迹上每个点到参考线距离的加权平均、最大向心加速度和轨迹最终到达的速度与对应权重的累加和。

6.如权利要求1所述的自动驾驶车辆轨迹规划方法，其特征在于，轨迹危险度采用危险度总代价来表示，危险度总代价表示为：

7.如权利要求1所述的自动驾驶车辆轨迹规划方法，其特征在于，轨迹方程采用五次多项式表示。

8.一种自动驾驶车辆轨迹规划系统，其特征在于，包括：

9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-7中任一项所述的自动驾驶车辆轨迹规划方法中的步骤。

10.一种计算机设备，其特征在于，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如权利要求1-7中任一项所述的自动驾驶车辆轨迹规划方法中的步骤。