CN112918486A - 一种时空行为决策及轨迹规划系统及方法 - Google Patents

Abstract

本公开的时空行为决策及轨迹规划系统及方法，长周期行为决策和轨迹规划模块用于在三维行驶时空中生成并拓展体素，利用搜索方法寻找代价最低的体素序列，进而确定参考轨迹，其中，体素为行驶车辆在规划时间内在所述三维行驶时空中的相应车道上的位置集合；短周期动态规划模块，利用短周期内的环境信息优化参考轨迹以得到最优轨迹。能够解决在包含动态障碍物的高速环境下的安全无碰撞的行为决策和轨迹规划任务的问题、平衡长周期的决策规划和短周期的轨迹规划之间的关系以提高规划和决策的效率、以及满足将决策模块和规划模块紧密结合起来的需求，使之能够更加适应动态环境，实现自主智能的规划以及重规划。

Description

一种时空行为决策及轨迹规划系统及方法

技术领域

本公开属于自动控制技术领域，特别是涉及到一种时空行为决策及轨迹规划系统及方法。

背景技术

自动驾驶及相关研究在过去的几十年中取得了很大进展。然而，在复杂的城市环境中实现高水平的自动驾驶仍然面临着巨大的挑战。其中，无人车的智能的行为决策以及高效的轨迹规划在充满着动态障碍物的高速路环境下尤其重要，二者紧密相关。因此，为了实现决策规划任务的安全性、高效性、灵活性以及自动化，亟需构建融合行为决策以及轨迹规划的统一的模型框架。

无人驾驶技术中的决策模块和规划模块一直被视为是无人驾驶技术中的核心技术。无人车决策过程，也称作行为规划和策略规划，实际上是基于无人车自身的状态以及周围的环境信息，选择出一个最优的行为动作。而轨迹规划的任务是基于决策产生的局部最优目标点以及目标状态，生成一条安全无碰撞、高效舒适的驾驶轨迹。

尽管目前已经有大量的单独涉及到行为决策和轨迹规划的研究，但是实际上这两个模块是很难被割裂的。因为规划模块需要决策模块产生的局部目标作为规划终点，同时，决策结果的灵活性以及最优性也需要依托轨迹规划的结果来评价。在极端的情况下，轨迹规划模块无法为指定的策略生成一条合适的轨迹，可能会导致无人车驾驶行为过于保守或者激进。

在现有的决策规划框架中，主流方法有两种，分别是基于规则的决策规划方法和基于学习的决策规划方法。基于规则的决策规划方法利用驾驶规则、专家知识来决策出一个最优的逻辑行为，其代表性方法包括有限状态机(Finite State Machine，FSM)和马尔可夫决策过程(Markov Decision Process，MDP)。基于学习的方法主要是利用不同的神经网络架构基于环境信息去生成奖励最优的策略，例如强化学习的方法。综上所述，不同的框架结构实现及应用的侧重点有所不同，但以下几点共性问题没有被清楚地解决：(1)如何实现在包含动态障碍物的高速环境下的安全无碰撞的行为决策和轨迹规划任务；(2)如何平衡长周期的决策规划和短周期的轨迹规划之间的关系以提高规划和决策的效率；(3)如何将决策模块和规划模块紧密结合起来，使之能够更加适应动态环境，实现自主智能的规划以及重规划。

发明内容

有鉴于此，本公开提出了一种时空行为决策及轨迹规划系统及方法，解决了如何实现在包含动态障碍物的高速环境下的安全无碰撞的行为决策和轨迹规划任务的问题、平衡长周期的决策规划和短周期的轨迹规划之间的关系以提高规划和决策的效率、以及满足将决策模块和规划模块紧密结合起来的需求，使之能够更加适应动态环境，实现自主智能的规划以及重规划。

根据本公开的一方面，本公开提出了一种时空行为决策及轨迹规划系统，所述系统包括：长周期行为规划模块和短周期动态规划模块；

所述长周期行为决策和轨迹规划模块，用于在三维行驶时空中生成并拓展体素，利用搜索方法寻找代价最低的体素序列，进而确定参考轨迹，其中，所述体素为行驶车辆在规划周期内在所述三维行驶时空中的相应车道上的位置集合；

所述短周期动态规划模块，利用短周期内的动态环境信息优化所述参考轨迹，以得到无碰撞的最优轨迹。

在一种可能的实现方式中，所述短周期动态规划模块包括所述RSS检测模块检测，所述RSS检测模块检测用于检测优化的所述参考轨迹的潜在危险。

在一种可能的实现方式中，当检测到优化的所述参考轨迹存在潜在危险，触发所述短周期动态规划模块重新优化所述参考轨迹，若优化成功，解除触发所述短周期动态规划模块；否则，触发所述长周期行为决策和轨迹规划模块重新确定参考轨迹。

根据本公开的另一方面，提出了一种时空行为决策及轨迹规划方法，所述方法包括：

在三维行驶时空中的规划时间内生成体素；

在每一个单位规划时间内对体素在时间和/或空间上进行拓展为N_v个体素，利用所述N_v个体素构成N_v*N_v的体素邻接矩阵，所述体素邻接矩阵表示所述体素之间的拓扑关系，其中，N_v为正整数；

根据所述体素邻接矩阵搜索出最优体素序列，在所述最优体素序列中利用分段Bezier曲线生成三维行驶时空中行驶车辆的轨迹，利用评价函数对所述轨迹进行评价，选择代价最低的轨迹作为参考轨迹；

跟踪所述参考轨迹并根据基于短规划周期的预测信息对所述参考轨迹进行二次优化得到最优轨迹。

在一种可能的实现方式中，所述在每一个单位规划时间内对体素在时间和/或空间上进行拓展包括：

对当前车道的体素，则在每一个单位规划时间内对体素在时间和空间上进行拓展；

对所述当前车道的相邻车道的体素，则在每一个单位规划时间内对体素在时间上进行拓展。

本公开的时空行为决策及轨迹规划系统，通过包括：长周期行为规划模块和短周期动态规划模块；长周期行为决策和轨迹规划模块，用于在三维行驶时空中生成并拓展体素，利用搜索方法寻找代价最低的体素序列，进而确定参考轨迹，其中，所述体素为行驶车辆在规划时间内在所述三维行驶时空中的相应车道的位置集合；短周期动态规划模块，利用短周期内的环境信息优化所述参考轨迹以得到最优轨迹。能够解决在包含动态障碍物的高速环境下的安全无碰撞的行为决策和轨迹规划任务的问题、平衡长周期的决策规划和短周期的轨迹规划之间的关系以提高规划和决策的效率、以及满足将决策模块和规划模块紧密结合起来的需求，使之能够更加适应动态环境，实现自主智能的规划以及重规划。

根据下面参考附图对示例性实施例的详细说明，本公开的其它特征及方面将变得清楚。

附图说明

包含在说明书中并且构成说明书的一部分的附图与说明书一起示出了本公开的示例性实施例、特征和方面，并且用于解释本公开的原理。

图1示出了根据本公开一实施例的时空行为决策及轨迹规划系统结构图；

图2示出了根据本公开一实施例的时空行为决策及轨迹规划系统与无人驾驶之间的关系示意图；

图3示出了根据本公开另一实施例的时空行为决策及轨迹规划系统结构图；

图4示出了根据本公开一实施例的时空行为决策及轨迹规划方法流程图；

图5示出了根据本公开一实施例的体素之间拓扑连接关系示意图；

图6示出了根据本公开另一实施例的时空行为决策及轨迹规划方法流程图。

图7示出了根据本公开一实施例的在静态障碍物存在的场景下，行驶车辆使用时空行为决策及轨迹规划系统及方法的行驶轨迹示意图；

图8示出了根据本公开一实施例的在静态障碍物存在的场景下，行驶车辆使用时空行为决策及轨迹规划系统及方法的行驶决策结果示意图；

图9示出了根据本公开一实施例的在动态场景1中，行驶车辆使用时空行为决策及轨迹规划系统及方法的行驶轨迹示意图；

图10示出了根据本公开一实施例的在动态场景1中，行驶车辆使用时空行为决策及轨迹规划系统及方法的行驶决策结果示意图。

具体实施方式

以下将参考附图详细说明本公开的各种示例性实施例、特征和方面。附图中相同的附图标记表示功能相同或相似的元件。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。

在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。

另外，为了更好的说明本公开，在下文的具体实施方式中给出了众多的具体细节。本领域技术人员应当理解，没有某些具体细节，本公开同样可以实施。在一些实例中，对于本领域技术人员熟知的方法、手段、元件和电路未作详细描述，以便于凸显本公开的主旨。

本公开的时空行为决策及轨迹规划系统是一个多层级的模型框架，包括一个长周期的行为规划模块(Long-term Behavior Planning，LTBP)和一个短周期的动态规划模块(Short-term Decision Planning，STDP)。二者采用不同的规划周期，在两个并行的线程中运行，两个模块之间通过时间和事件双触发机制紧密地结合，最终形成一个统一行为决策和轨迹规划的，能够高效应对动态场景实时变化特性的闭环系统。

在LTBP中，为了构建车辆在三维空间内的行为驾驶空间，即时空隧道，提出一种结合周围环境静态及动态障碍物的新型的体素的结构及其扩展方法。通过利用Dijkstra搜索方法，针对每一种可能的策略(保持车道、左换道、右换道)，寻找到代价最低的体素序列，构建二次规划(Quadratic Programming，QP)问题，求解出约束在体素序列内部的最优轨迹。

在STDP中，基于短期内感知预测的周围障碍物的信息以及LTBP模块规划出的参考轨迹，构建另一个小规模的QP问题来实现短期内的轨迹跟踪以及必要情况下的重规划。此外，STDP模块中还包括一个责任安全检测模块(Responsibility-Sensitive Safety，RSS)，运行在10Hz的高频率下，从而实现对于潜在危险的实时检测，确保安全。

图1示出了根据本公开一实施例的时空行为决策及轨迹规划系统结构图。如图1所示，该系统可以包括：长周期行为规划模块和短周期动态规划模块；其中，长周期行为决策和轨迹规划模块可以用于在三维行驶时空中生成并拓展体素，利用搜索方法寻找代价最低的体素序列，进而确定参考轨迹，其中，所述体素为是行驶车辆在规划时间内在所述三维行驶时空中的相应车道的位置集合；短周期动态规划模块可以利用短周期内的环境信息优化所述参考轨迹以得到最优轨迹。

图2示出了根据本公开一实施例的时空行为决策及轨迹规划系统与无人驾驶之间的关系示意图；图3示出了根据本公开另一实施例的时空行为决策及轨迹规划系统结构图。

如图2和图3所示，该系统为双触发机制的带有闭环反馈机制的多层级决策规划框架，包含了LTBP(长周期行为规划模块)和STDP(短周期动态规划模块)两个双线程核心模块。其中，LTBP的运行频率为0.25Hz，主要负责在三维行驶时空中生成拓展体素并且利用搜索方法寻找代价最低的体素序列，最后通过解最优化问题确定最优轨迹。STDP的运行频率为1Hz，主要负责利用短时间内相对准确的环境预测信息，进一步跟踪和优化参考轨迹。

如图2所示，当该系统运用在无人驾驶领域时，时空行为决策及轨迹规划系统属于决策规划模块，其上游模块是感知和预测模块，负责感知并预知周围静态和动态环境信息；下游模块是控制模块，负责将规划出的轨迹转化为控制指令并执行。

在一示例中，如图2和图3所示，短周期动态规划模块包括所述RSS检测模块检测，所述RSS检测模块检测用于检测优化的所述参考轨迹的潜在危险。且当检测到优化的所述参考轨迹存在潜在危险，触发所述短周期动态规划模块重新优化所述参考轨迹，若优化成功，解除触发所述短周期动态规划模块；否则，触发所述长周期行为决策和轨迹规划模块重新确定参考轨迹。

如图3所示，短周期动态规划模块内部包含一个RSS安全检测模块，用于检测潜在的危险，运行在10Hz的频率。一旦发现潜在危险，STDP的事件触发使能，重新优化。若STDP优化成功，则事件触发解除，系统回归正常运行，若规划失败，则LTBP的事件触发使能，系统重新进行行决策及参考轨迹的规划。整个系统最终形成一个带有反馈机制的双触发的闭环结构。

本公开的时空行为决策及轨迹规划系统包括：长周期行为规划模块和短周期动态规划模块；长周期行为决策和轨迹规划模块，用于在三维行驶时空中生成并拓展体素，利用搜索方法寻找代价最低的体素序列，进而确定参考轨迹，其中，所述体素为是行驶车辆在规划时间内在所述三维行驶时空中的相应车道的位置集合；短周期动态规划模块，利用短周期内的环境信息优化所述参考轨迹以得到最优轨迹。能够解决如何实现在包含动态障碍物的高速环境下的安全无碰撞的行为决策和轨迹规划任务、如何平衡长周期的决策规划和短周期的轨迹规划之间的关系以提高规划和决策的效率、以及如何将决策模块和规划模块紧密结合起来，使之能够更加适应动态环境，实现自主智能的规划以及重规划的问题。

图4示出了根据本公开一实施例的时空行为决策及轨迹规划方法流程图。

根据本公开另一方面，提出了一种时空行为决策及轨迹规划方法，应用于上述的时空行为决策及轨迹规划系统，如图4所示，该方法可以包括：

步骤S1：在三维行驶时空中的规划时间内生成体素。

体素V是一个三维的立方体，包括(v_s，v_d，v_t，v_l)，其中v_t指的是在单位规划时间t内生成的体素。(v_s，v_d)是体素中心点在Frenet系下的坐标。v_l是体素在s轴方向的长度(体素的宽度默认为是车道的宽度d)。体素V实际上是车辆在规划时间t内在相应车道的所有可能位置的集合，将体素按照时间的顺序排布成一个序列就可以得到车辆在规划周期内的所有轨迹的分布空间，同时也构成轨迹优化问题的可行域。若体素序列中包含的体素的横向位置v_d都相同，说明行驶车辆对应的策略是保持车道，否则说明行驶车辆会发生换道行为。

LTBP的规划时间周期为T_l＝5s，单位规划时间为Δt_L＝1s，将单位规划时间记为t₁，t₂，t₃，t₄，t₅。由于司机不会频繁地做出换道的决定，假设在一个决策规划周期内，规划器最多做出一次换道的决策，即体素只在当前车道(本决策规划周期开始的车道)以及左右相邻车道(如果存在)中生成和扩展。

假设在t_i＝t时间内体素V_i已经生成，则在t_i+1＝t+Δt_L时间内体素V_i+1的生成由式(1)和式(2)决定，其中v₀是规划周期开始时间的初始速度，

代表在规划时间t_i+1起始时刻的最小速度和纵向位移，

代表在规划时间t_i+1结束时刻的最大速度和纵向位移。

式(2)；

由此可知，体素的起始位置位于时间t_i+1的初始时刻t的累计最小位移，即行驶车辆从本规划周期开始，以最小加速度(最大减速度)行驶至t_i+1的初始时刻t的位置(速度减为0后保持静止)，同理，体素的末尾位置是位于时间t_i+1的末尾时刻t+Δt_L的累计最大位移，即行驶车辆从本规划周期开始，以最大加速度行驶至t_i+1结尾时刻的位置(达到最大速度限制后匀速)。

理论上，体素内的各个位置对于行驶车辆而言，在速度和加速度的约束下都是可达的。利用式(1)和式(2)，行驶车辆的最大的行驶空间O_drivable可以得到。将周围N_s辆行驶车辆在规划周期内的预测状态信息考虑进来，得到占据空间O_occupied，进而得到最终提纯的自由空间O_purified，如式(3)所示,

对于每个体素而言，将占据空间和体素的交集去除,在s方向上，若去除占据空间后的体素的长度大于一定的阈值，保留为有效体素，否则，删除体素，

经过上述，由式(4)v_l≥L_min+L_v能够确定体素是否保留，其中L_v是车身的长度，L_min为最小体素长度的阈值。最终生成的体素包括相应车道内的每一个单位规划时间的有效体素。

步骤S2：在每一个单位规划时间内对体素在时间和/或空间上进行拓展，共生成N_v个体素，利用所述N_v个体素构成N_v*N_v的体素邻接矩阵，所述体素邻接矩阵表示所述体素之间的拓扑关系，N_v为正整数。

可以将每个体素视为一个节点，根据其扩展特性，将当前车道上的节点视为主车中心节点，具有时间、空间扩展特性，即在当前车道的每一个单位规划时间内对体素在时间和空间上进行拓展；而相邻车道上的节点被视为非主车中心节点，只具有时间扩展特性(在当前规划周期不可换道或者返回先前车道)，即在相邻车道的每一个单位规划时间内对体素在时间上进行拓展。

图5示出了根据本公开一实施例的体素之间拓扑连接关系示意图。

在一示例中，体素邻接矩阵的边包括时间边和空间边；其中，所述时间边为同一行驶车道内连续规划时间内的相邻的两个体素之间的边；所述空间边为不同行驶车道内的同一单位规划时间两个体素之间的边。

假设共生成并拓展了N_V个节点(体素)，包括当前车道体素、左右车道体素，接下来我们构造了一个N_V*N_V的邻接矩阵A来表示节点之间的拓扑关系。如图5所示，因为障碍物的存在，在t₄和t₅单位规划时间内，原自由空间被分割成了两个独立的体素。其分别与t₃规划时间内的体素相连，构成了两条拓扑关系，如图6所示的轨迹(1)和轨迹(2)，轨迹(1)和轨迹(2)分别对应策略(1)减速让行并跟在前车之后和策略(2)加速超越并行驶在前车之前。

若体素V_i和V_j之间满足式(4)

的关系，邻接矩阵A的元素a(i，j)＝1，说明体素V_i和体素V_j在物理上是相连的。节点之间形成的边分成两种，一种边存在于同一行驶车道内连续单位规划时间的两个节点之间，称为时间边(temporal edge)；另一种边是存在于同一单位规划时间内不同行驶车道的两个节点之间，称为空间边(spatial edge)。

若时间边存在，则其权重根据下式(5)计算，

时间边的权重与“转移因子”有关，转移因子越大意味着两个体素之间重叠的区域越大，行驶车辆可以以一个更加宽泛的速度变化范围实现在两个体素之间的过渡，一定程度上减轻了规划的约束。在当前考虑的行驶车道的每个单位规划时间内，由于社会车辆的存在，可能生成不止一个的体素。如图6所示，在单位规划时间t₄和t₅内，由于出现障碍物，原体素空间被分成了两份，分别生成体素

由于时间边拓扑关系：

和

的存在，导致无人车产生两种可能的行为：(1)减速并跟在社会车辆的身后行驶；(2)加速并超越社会车辆。

对于空间边来说，意味着当前体素存在换道的可能。可以从与障碍物的距离以及换道代价的角度去评估换道的损失，以作为空间边的权重。尽管空间边的权重一般高于时间边的代价权重，但是在某些情况下，换道对于无人车而言是非常必要的，以确保安全驾驶或是获得理想的速度。为了维持策略的多样性，生成了包括保持车道、左换道、右换道的三种策略。在每一种策略中，位于末端规划时间层的体素被当作是目标点。为了使生成的行驶轨迹更加平滑，放宽对于局部目标位置的约束，仅提供目标位置的范围约束。

通过体素的生成和拓展，将无人车在规划周期内可行的行驶间隙抽象为节点，将节点之间的拓扑关系表示成‘时间边’和‘空间边’，这样就将复杂的动态交通环境转化为简单的‘带权重有向图(Directed Weighted Traffic Graph,DWTG)’的结构，简化了决策和规划过程。进一步利用Dijkstra搜索方法在图中搜索目标体素对应的代价最低的体素序列，将体素序列合并成无人车行驶的时空隧道。

步骤S3：根据所述体素邻接矩阵利用分段Bezier曲线生成三维行驶时空中行驶车辆的轨迹，利用评价函数对所述轨迹进行评价，选择代价最低的轨迹作为参考轨迹。

体素序列形成后，行驶车辆轨迹的分布空间随之确定，利用分段Bezier曲线生成车辆的轨迹。Bezier曲线b(t)是一种特殊的样条曲线，其定义在区间t∈[0，1]上，由控制点C＝[c₀，...，c_j，...，c_m]和函数

组成，如式子(8)所示，其中m是曲线的阶数。

在无人车轨迹规划中，Bezier曲线被广泛使用，Bezier曲线具备优良的凸包性质、对其求导仍为Bezier曲线。凸包性质指Bezier曲线被严格地控制在由控制点形成的凸包内部(包括边缘)，生成的轨迹是安全无碰撞的。速度特性指Bezier曲线的导数(即物理量速度)仍是Bezier曲线，其控制点是原曲线控制点的线性组合，可以将优化问题的不等式约束转化为线性不等式约束。

可以将每一个体素V_i视为一条Bezier曲线段b_i(t)，体素的时间长度为单位时间1s，无需对时间变量t做归一化的缩放处理，直接平移至[0,1]区间即可。在μ方向上的第i段Bezier曲线的表达式如式(9)所示，

为了使规划出的行驶轨迹尽可能的平滑和舒适，例如将加加速度(jerk)作为目标函数，相应的优化函数次数(导数)为3。为了保证相邻两个规划周期的行驶轨迹的平滑，二次优化需要满足以下条件：可以将行驶车辆的初始位置、初始速度、初始加速度作为轨迹的等式约束；Bezier曲线段衔接处的位置、速度、加速度连续一致；优化轨迹需满足速度约束、加速度约束；优化轨迹必须分布在体素内部，受到体素尺寸的制约。可以根据式(10)所示实现，其中

k＝0，1，2分别代表位置、速度、加速度最大值最小值约束。p_e为设定的终点的一个可能的形式，例如前方不存在障碍物时，采用位于体素内部的理想前进距离作为终点，若前方存在障碍物，则采用体素内部与障碍物保持安全距离的点作为终点，终点同样也可以不固定，而使用范围来约束。

当生成所有策略及其对应的最优轨迹，需要对这些策略生成的轨迹做一个安全性和高效性的评估，来选出最优策略及其轨迹作为参考轨迹。定义一个由安全因子的代价

轨迹速度和理想速度的差值的代价

以及换道策略产生的代价f_M的一个线性组合，该线性组合的权重可以根据实际的交通环境和决策规划的需要去合理地调节。例如，从行驶车辆安全的角度出发，在有安全保障的前提下，如果换道可能使车辆更加接近理想车速的状态，那么规划器会支持换道，从而提高车辆行驶的效率。整个评估函数如式(11)所示，代价最低的轨迹将会被作为参考轨迹Tr^*，其相应的策略将会是当前最优策略。

步骤S4：跟踪所述参考轨迹并根据基于短规划周期的预测信息对所述参考轨迹进行二次优化得到最优轨迹。

由于LTBP的规划周期较长，其周围的行驶车辆未来状态预测可能会不准确，则STDP线程负责进行1s的短周期的轨迹优化，且RSS管理器可以基于实时观测数据及一步预测信息(0.2s)以10Hz的频率检测参考轨迹潜在的危险。

例如，RSS计算目标车辆和障碍物车辆之间最小车距d_min，如式(12)所示，其中v为无人车当前车速，t_r为反应时间，v_f为前车车速，当检测到前方车辆车距小于最小车距时，会重启STDP规划。STDP的规划任务主要包括参考轨迹的跟踪以及基于短时间预测信息的轨迹优化，因STDP的规划周期只有1s，只对应一段Bezier曲线。STDP构造一个与LTBP类似的小规模的二次优化问题，其目标函数是与LTBP生成的参考轨迹的误差，因为参考轨迹足够平滑，因此STDP也易于生成平滑的轨迹。

应用示例：

图6示出了根据本公开另一实施例的时空行为决策及轨迹规划方法流程图。

图6中(a)描述的驾驶场景，其中，主车行驶在中间车道，社会车辆1行驶在右边车道，社会车辆2行驶在左车道。图6中(b)描述的是在三维时空坐标系下生成并拓展的体素，这些体素实际构成无碰撞的主车行驶空间的集合。图6中(c)描述的是将体素抽象为节点的效果图。图6中(d)描述的是交通图的组成成分：节点分为主车体素节点(具有时间方向和空间方向的拓展性)和非主车体素节点(仅具有空间方向拓展性)；边分为空间边(权重为换道代价加碰撞时间代价)以及时间边(权重为体素转移系数代价)。利用Dijkstra搜索方法寻找出每个策略对应的代价最低的体素序列，如图6中(e)所示。最终通过解最优化问题得到每个策略对应的最优轨迹，如图6中(f)所示。

为验证本发明方法的有效性，使用高速路环境下采集的公开数据集合NGSIM进行实验。实验平台为装配有Intel I5-3470K中央处理器和NVIDIA GeForce GTX 2050-Ti显卡的台式计算机，使用的操作平台为Robot Operating System(ROS)，使用的编程语言为Python3.6。

图7示出了根据本公开一实施例的在静态障碍物存在的场景下，行驶车辆使用时空行为决策及轨迹规划系统及方法的行驶轨迹示意图；图8示出了根据本公开一实施例的在静态障碍物存在的场景下，行驶车辆使用时空行为决策及轨迹规划系统及方法的行驶决策结果示意图。

如图7所示，速度变化由黑色颜色带表示，共设置4个障碍物，分别位于第2车道的s方向100m的位置(障碍物1)，第3车道s方向220m的位置(障碍物2)，第一车道s方向320m的位置(障碍物3)以及横跨1-5车道的s方向500m处的障碍物4。主车全程为避障，共换道两次，减速至停车一次，证明了本发明中时空行为决策及轨迹规划系统躲避静态障碍物以及紧急停车的能力。

如图8所示，时空行为决策及轨迹规划系统决策规划的效果，由上到下依次是，时空行为决策及轨迹规划系统的纵向的位置变化、横向的位置变化、纵向的速度以及加速度的变化。例如，为躲避3号障碍物，行驶车辆进行了一次加速过程；为躲避4号障碍物，行驶车辆被迫减速至停车。在优化问题中，将车辆的加加速度作为优化目标，因此速度以及加速度的变化都较为平滑。

图9示出了根据本公开一实施例的在动态场景1中，行驶车辆使用时空行为决策及轨迹规划系统及方法的行驶轨迹示意图；图10示出了根据本公开一实施例的在动态场景1中，行驶车辆使用时空行为决策及轨迹规划系统及方法的行驶决策结果示意图。

如图9所示，主车在每个规划周期内以黑色矩形展示，社会车辆在第1，5，10个规划周期初始时刻的状态表示为矩形、三角形和椭圆形。主车的速度变化由黑色颜色带表示，社会车辆的轨迹由细虚线表示。从轨迹中可以看出，由于主车周围存在大量社会车辆，左右车道可通行区域受限，考虑换道代价后，主车认为保持车道行驶最优，因此，主车全程的决策结果均为保持车道行驶，在图9子图a)中，可以看出轨迹在Frenet坐标系下近似为一条直线。子图b),c),d)分别展示了第1,5,10个规划周期在三维空间的轨迹规划结果，其中主车规划轨迹由相连圆圈的表示，社会车辆轨迹由分离的圆圈表示。

如图10所示，该系统的决策规划效果分别是纵向的位置变化、横向的位置变化、纵向的速度以及加速度的变化。由于主车初始时刻所在的车道一直保持较好的行驶条件，因此主车决策保持车道行驶，其纵向速度基本保持匀速，速度以及加速度的变化都较为平滑。

本公开的时空行为决策及轨迹规划方法，能够在解空间内，生成三维体素并在时空维度进行拓展，基于体素序列进行最优策略及轨迹的搜索求解，从而将二维平面上的动态规划问题转化到了三维空间的静态规划问题；并行的双线程的设计平衡和决策规划和轨迹规划的关系，提高了决策规划的效率；在信息流方面，采用时间和事件双触发的机制，实现了决策和规划信息的高效沟通，实现了智能规划及重规划。

以上已经描述了本公开的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实施例。

Claims (6)

1.一种时空行为决策及轨迹规划系统，其特征在于，所述框架包括：长周期行为规划模块和短周期动态规划模块；

所述长周期行为决策和轨迹规划模块，用于在三维行驶时空中生成并拓展体素，利用搜索方法寻找代价最低的体素序列，进而确定参考轨迹，其中，所述体素为行驶车辆在规划时间内在所述三维行驶时空中的相应车道上的位置集合；

所述短周期动态规划模块，利用短周期内的环境信息优化所述参考轨迹以得到最优轨迹。

2.根据权利要求1所述的时空行为决策及轨迹规划系统，其特征在于，所述短周期动态规划模块包括所述RSS检测模块检测，所述RSS检测模块检测用于检测优化的所述参考轨迹的潜在危险。

3.根据权利要求2所述的时空行为决策及轨迹规划系统，其特征在于，当检测到优化的所述参考轨迹存在潜在危险，触发所述短周期动态规划模块重新优化所述参考轨迹，若优化成功，解除触发所述短周期动态规划模块；否则，触发所述长周期行为决策和轨迹规划模块重新确定参考轨迹。

4.一种时空行为决策及轨迹规划方法，其特征在于，所述方法包括：

在三维行驶时空中的规划时间内生成体素；

在每一个单位规划时间内对体素在时间和/或空间上进行拓展为N_v个体素，利用所述N_v个体素构成N_v*N_v的体素邻接矩阵，所述体素邻接矩阵表示所述体素之间的拓扑关系，其中，N_v为正整数；

根据所述体素邻接矩阵搜索最优体素序列，在所述最优体素序列的约束下利用分段Bezier曲线生成三维行驶时空中行驶车辆的轨迹，利用评价函数对所述轨迹进行评价，选择代价最低的轨迹作为参考轨迹；

跟踪所述参考轨迹并根据基于短规划周期的预测信息对所述参考轨迹进行二次优化得到最优轨迹。

5.根据权利要求4所述的时空行为决策及轨迹规划方法，其特征在于，所述在每一个单位规划时间内对体素在时间和/或空间上进行拓展包括：

对当前车道的体素，则在每一个单位规划时间内对体素在时间和空间上进行拓展；

对所述当前车道的相邻车道的体素，则在每一个单位规划时间内对体素在时间上进行拓展。

6.根据权利要求4所述的时空行为决策及轨迹规划方法，其特征在于，所述体素邻接矩阵的边包括时间边和空间边；

其中，所述时间边为同一行驶车道内连续规划时间内的相邻的两个体素之间的边；

所述空间边为不同行驶车道内同一单位规划时间的两个体素之间的边。

Images