CN115840454B - 非结构化道路冲突区域的多车轨迹协同规划方法及装置 - Google Patents

Abstract

本发明公开了一种非结构化道路冲突区域的多车轨迹协同规划方法及装置，其包括：步骤1，确定进入到非结构化道路冲突区域的车辆的优先级顺序；步骤2，根据确定的优先级顺序进行单车轨迹规划，获得初级轨迹；步骤3，优化平滑步骤2输出的初级轨迹，获得最终轨迹；步骤2包括：步骤21，结合车辆运动学模型，拓展节点；步骤22，根据拓展出的每一个节点，选取最优的节点作为下一父节点；步骤23，以在目标点附近的父节点为起点，目标点为终点，使用终点位姿拟合的方式，拟合轨迹，若拟合成功，则输出初级轨迹，否则，跳转步骤22继续拓展下一父节点。本发明涉及自动驾驶技术领域，本发明高效、安全，能够在很大程度上避免停车等待。

Description

非结构化道路冲突区域的多车轨迹协同规划方法及装置

技术领域

本发明涉及自动驾驶技术领域，特别是关于一种非结构化道路冲突区域的多车轨迹协同规划方法及装置。

背景技术

多车轨迹协同规划是提高道路通行效率，保证冲突区域行驶安全性的关键技术。该技术以车辆通行时间、规划成功率等作为优化目标，规划出各车无碰撞且满足车辆运动学约束的行驶轨迹。

目前，匝道合流处、十字路口、三岔路口等结构化道路冲突区域的多车轨迹协同规划已有大量研究，例如：基于优先级、基于反应、基于数值优化和基于速度协调的规划方法，形成了较为成熟的通行策略。对于非结构化道路冲突区域的车辆行驶轨迹不规则特点而言，基于优先级的规划方法很难设计出一种满足复杂行驶环境的优先级通行规则。鉴于非结构化道路冲突区域车辆的轨迹冲突点过多，随着车辆的数量增加，各车的避碰约束呈指数增加的特点，基于数值优化的规划方法往往无法成功求解。而基于反应的规划方法，只能保证局部的避碰，调整局部的冲突点，其它轨迹交集处会出现新的冲突点。由此可见，目前成熟的结构化道路冲突区域的多车轨迹协同规划方法无法直接适用于非结构化道路。

因此，非结构化道路冲突区域的多车轨迹协同规划方法开始逐步出现，目前主要有如下三种：

一种是采用RRT*算法在时间域的范围内同时进行多车的轨迹规划，最后采用杜宾斯曲线拓展这些节点。而在使用RRT*进行拓展的过程中，会出现很多无效的节点，这些无效节点会互相影响，当地图区域狭小或者车辆数量大时，处理这些无效节点会浪费不少的算力，而且生成的轨迹不满足车辆运动学约束，实际跟踪时会产生较大误差，不一定能够保证各车轨迹之间彼此无碰撞。

另一种是设计一种启发式函数，根据车辆的同伦类数量动态地决定车辆优先级顺序，以降低死锁发生的概率，再调用三维A*算法在时空内搜索出各车轨迹。然而在进行动态优先级排序时，各个车辆优先级频繁变换，这种不稳定性的决策最终导致各车辆速度频繁切换，浪费了大量的能量，不满足节能的要求。而且三维的A*算法规划出来的轨迹速度是不平滑的，整个规划算法也未考虑车辆的运动学约束。

还有一种方法是一种防失败的混合A*，首先搜索出来一条初始路径，再设计一个圆环型的风险区来缩放避碰约束，构建中间最优化问题以降低计算维度，通过不断的迭代得到满足完全约束的最优解。这种方法设置了中间最优控制问题，采用迭代方式进行求解，能够在一定程度上增加最优控制问题的求解成功率，但是难以保证较快的求解速度。而且，随着车辆数量增加，最优控制问题的避碰约束维度呈指数增加，往往无法求解成功。

发明内容

本发明的目的在于提供一种高效、安全的非结构化道路冲突区域的多车轨迹协同规划方法及装置，其能够在很大程度上避免停车等待。

为实现上述目的，本发明提供一种非结构化道路冲突区域的多车轨迹协同规划方法，其包括：

步骤1，确定进入到非结构化道路冲突区域的车辆的优先级顺序；

步骤2，根据步骤1确定的优先级顺序进行单车轨迹规划，获得初级轨迹；

步骤3，优化平滑步骤2输出的初级轨迹，获得一条平滑连续可跟踪的最终轨迹；

其中，步骤2具体包括：

步骤21，结合车辆运动学模型，拓展节点；

步骤22，根据拓展出的每一个节点，选取最优的节点作为下一父节点；

步骤23，以在目标点附近的父节点为起点，目标点为终点，使用终点位姿拟合的方式，拟合轨迹，若拟合成功，则输出初级轨迹，否则，跳转步骤22继续拓展下一父节点；

步骤22具体包括：

步骤221，初始化open集和close集节点，初始化后的open集用于存储步骤21拓展出的所有节点，初始化后的close集为空集；

步骤222，将利用下式(3)计算的open集的代价值f(n)最小的节点作为父节点，并加入到closed集中；

f(n)＝g(n)+h(n)                 (3)

式中，n表示open集中节点的索引，h(n)为距目标点的预估距离代价，g(n)为距离起始点的累计代价；

步骤223，判断步骤222获得的父节点是否在目标点附近，如果是，则进入步骤23，否则进入步骤224；

步骤224，从父节点开始，再次拓展出若干子节点，先判断每一个子节点是否能够通过碰撞检测，若判定为是，则继续判断该通过碰撞检测的子节点是否处于open集合中，如果判定为是，则进一步判断此次拓展的累计代价值g(n)是否小于open集合中记录过的g(n)值的大小，如果判定为是，则更新该通过碰撞检测的子节点的累计代价g(n)和父节点，最后，将判定为不在closed集合与open集合中的子节点放入open集合中，跳转步骤222。

进一步地，步骤1具体包括：

步骤11，根据非结构化道路冲突区域的面积大小预设控制区；

步骤12，初始化计数变量p＝1，计算各车到达控制区的时刻，记录第一个车辆到达控制区的轨迹点时刻t₀；

步骤13，计算第p批次的截止时刻t₀+p·ΔT，寻找到达时刻t_p满足t₀+(p-1)·ΔT＜t_p＜t₀+p·ΔT条件的车辆，将其放入第p批次车辆集合中；

步骤14，在第p批次车辆集合中，按照车辆重量级降序排列，对于相同重量级车辆，按照进入控制区的时刻，升序排列；

步骤15，判断是否还存在车辆未分配到批次的集合中，如果有，则令p＝p+1，跳转步骤14，否则进入步骤16；

步骤16，从第1批次到最后批次依次为车辆排序，输出车辆的优先级顺序。

进一步地，步骤21为：根据m个不同前轮转角、n个不同速度以及1个停车等待节点，按照如下方式之一，拓展获得总计m·n+1个节点；

第一种方式：同一速度等级按照m个前轮转角分层拓展；

第二种方式：同一前轮转角按照n个速度等级分层拓展。

进一步地，按照同一速度等级按照m个前轮转角分层拓展，获得总计m·n+1个节点的方法包括：

子步骤211，将n个不同速度等级的其中之一作为车辆当前速度v_current，按下式(1)计算搜索拓展步长s与该拓展步长内的车身横摆角变化量

式(1)中，ΔT为时间分辨率，l表示前后车轮轴的轴距，α表示车辆的前轮转角；

子步骤212，根据步长s内车身横摆角变化量

通过车辆运动学约束公式(2)进行拓展，获得节点；

式(2)中，θ_k+1、(x_k+1,y_k+1)、t_k+1分别表示车辆在拓展出的第k+1个节点的航向角、中心点的位置坐标、时刻，θ_k、(x_k,y_k)、t_k分别表示车辆在拓展出的第k个节点的航向角、中心点的位置坐标、时刻。

进一步地，步骤222中的累计代价g(n)通过下式(4)计算得到：

式(4)中，

分别表示车辆在第n+1个节点、第n个节点的车身横摆角，v_n+1、v_n分别表示车辆在第n+1个节点、第n个节点的速度，w_step、w_θchange、w_vchange、w_stop分别为步长惩罚系数、角度切换惩罚系数、速度切换惩罚系数和停车惩罚系数，P_stop为停车惩罚值。

进一步地，步骤23具体包括：

步骤231，设置步骤22中得到的目标点附近的父节点为起始点P_i，目标点为终止点P_f；

步骤232，按照杜宾斯曲线拟合的方式，从起始点P_i向终止点P_f进行拟合。

步骤233，检测拟合的杜宾斯曲线的路径是否会与静态障碍物发生碰撞，如果判定为不与静态障碍物发生碰撞，则按照车辆当前速度v_current，上下采样多种速度赋予拟合的杜宾斯曲线的路径得到多个拟合轨迹，根据步骤22中得到的目标点附近的父节点的拓展时刻，计算多个拟合轨迹的各路点时刻，如果在父节点的拓展时刻与拟合轨迹终点的时刻间，判定为不与其它高优先级车辆发生碰撞，则从步骤22中得到的目标点附近的父节点开始回溯得到搜索轨迹，将搜索轨迹与拟合轨迹进行拼接得到初级轨迹，如果采样的多个轨迹均不满足动静态障碍物的避碰要求，那么返回步骤22继续拓展；其中，高优先级的车辆指的是优先级比本车高的所有车辆。

进一步地，步骤3具体包括：

步骤31，建立高优先级车辆占据本车的ST图；

步骤32，设定如下式描述的代价函数和建立如下约束：

其中，F是代价函数function，s_i是第i个优化点的frenet坐标系下的距离，s_i'是s_i的一阶导数，s_i”是s_i的二阶导数，s_i+1”是第i+1个优化点的frenet坐标系下的距离s_i+1的一阶导数，第一项

为优化后速度与原始速度差的积分，第二项

为加速度的积分，第三项为

加加速度的积分，W_{cos_ref}，W_{cos_dds}，W_{cos_ddds}分别为这三项指标的惩罚系数；

第一约束，设置速度约束：0≤s_i'≤V_max，V_max是当前道路的最大限速；

第二约束，建立曲率约束：

其中，kappa为轨迹点的曲率大小，a_ymax是当前道路的最大横向加速度；

第三约束，建立倒车约束：s_i+1≥s_i；

第四约束，建立加速度约束：a_min≤s_i”≤a_max，a_min、a_max分别指车辆平顺行驶的最小和最大加速度；

第五约束，建立避碰约束：

t₁≤t≤t₂时，0＜s_i＜s_obs1；

t₃≤t≤t₄时；0＜s_i＜s_obs2；

式中，t₁、t₂分别是第一辆车的侵入时刻与驶离时刻，t₃、t₄分别是第二辆车的侵入时刻与驶离时刻，s_obs1，s_obs2分别泛指动态障碍物1和动态障碍物2；

第六约束，建立两点边值约束：

s₀＝0，s₀'＝v_start，s₀”＝a_start；

s_T＝s_end，s_T'＝v_end，s_T”＝0；

式中，s₀是起点frenet坐标系下的距离值，v_start指起点速度，a_start指起点加速度，s_T指目标点frenet坐标系下的距离值，s_end指目标点frenet坐标系下的距离值，v_end指目标点的速度大小；

第七约束，对s_i+1,s_i+1'进行有限项的泰勒展开，忽略无穷小项：

根据代价函数与约束情况，求解得到s_i，s_i'，s_i”，结合步骤21得到的节点的坐标值，形成带有速度和加速度信息的最终轨迹。

本发明还提供一种非结构化道路冲突区域的多车轨迹协同规划装置，其包括：

车辆优先级确定单元，其用于确定进入到非结构化道路冲突区域的车辆的优先级顺序；

初级轨迹获取单元，其用于根据优先级顺序进行单车轨迹规划，获得初级轨迹；

最终轨迹获取单元，其用于优化平滑初级轨迹，获得一条平滑连续可跟踪的最终轨迹；

其中，初级轨迹获取单元具体包括：

节点拓展子单元，其用于结合车辆运动学模型，拓展节点；

父节点选取子单元，其用于根据拓展出的每一个节点，选取最优的节点作为下一父节点；

终点位姿拟合子单元，其用于以在目标点附近的父节点为起点，目标点为终点，使用终点位姿拟合的方式，拟合轨迹，若拟合成功，则输出初级轨迹，否则，由父节点选取子单元继续拓展下一父节点；

父节点选取子单元具体包括：

初始化模块，其用于初始化open集和close集节点，初始化后的open集用于存储步骤21拓展出的所有节点，初始化后的close集为空集；

代价值计算模块，其用于将利用下式(3)计算的open集的代价值f(n)最小的节点作为父节点，并加入到closed集中；

f(n)＝g(n)+h(n)            (3)

式中，n表示open集中节点的索引，h(n)为距目标点的预估距离代价，g(n)为距离起始点的累计代价；

父节点筛选模块，其用于判断代价值计算模块获得的父节点是否在目标点附近，如果是，则由终点位姿拟合子单元拟合轨迹；否则，从父节点开始，再次拓展出若干子节点，先判断每一个子节点是否能够通过碰撞检测，若判定为是，则继续判断该通过碰撞检测的子节点是否处于open集合中，如果判定为是，则进一步判断此次拓展的累计代价值g(n)是否小于open集合中记录过的g(n)值的大小，如果判定为是，则更新该通过碰撞检测的子节点的累计代价g(n)和父节点，最后，将判定为不在closed集合与open集合中的子节点放入open集合中，并由代价值计算模块将利用式(3)计算的open集的代价值f(n)最小的节点作为父节点，并加入到closed集中。

进一步地，节点拓展子单元为：根据m个不同前轮转角、n个不同速度以及1个停车等待节点，按照如下方式之一，拓展获得总计m·n+1个节点；

第一种方式：同一速度等级按照m个前轮转角分层拓展；

第二种方式：同一前轮转角按照n个速度等级分层拓展；

其中，按照同一速度等级按照m个前轮转角分层拓展，获得总计m·n+1个节点的方法包括：

子步骤211，将n个不同速度等级的其中之一作为车辆当前速度v_current，按下式(1)计算搜索拓展步长s与该拓展步长内的车身横摆角变化量

式(1)中，ΔT为时间分辨率，l表示前后车轮轴的轴距，α表示车辆的前轮转角；

子步骤212，根据步长s内车身横摆角变化量

通过车辆运动学约束公式(2)进行拓展，获得节点；

式(2)中，θ_k+1、(x_k+1,y_k+1)、t_k+1分别表示车辆在拓展出的第k+1个节点的航向角、中心点的位置坐标、时刻，θ_k、(x_k,y_k)、t_k分别表示车辆在拓展出的第k个节点的航向角、中心点的位置坐标、时刻，R_r表示后轮转角半径。

进一步地，终点位姿拟合子单元具体包括：

杜宾斯曲线拟合模块，其用于以设置父节点选取子单元中得到的目标点附近的父节点为起始点P_i，目标点为终止点P_f，按照杜宾斯曲线拟合的方式，从起始点P_i向终止点P_f进行拟合；

初始轨迹输出模块，其用于检测拟合的杜宾斯曲线的路径是否会与静态障碍物发生碰撞，如果判定为不与静态障碍物发生碰撞，则按照车辆当前速度v_current，上下采样多种速度赋予拟合的杜宾斯曲线的路径得到多个拟合轨迹，根据父节点选取子单元中得到的目标点附近的父节点的拓展时刻，计算多个拟合轨迹的各路点时刻，如果在父节点的拓展时刻与拟合轨迹终点的时刻间，判定为不与其它高优先级车辆发生碰撞，则从父节点选取子单元中得到的目标点附近的父节点开始回溯得到搜索轨迹，将搜索轨迹与拟合轨迹进行拼接得到初级轨迹，如果采样的多个轨迹均不满足动静态障碍物的避碰要求，则由父节点选取子单元继续拓展；其中，高优先级的车辆指的是优先级比本车高的所有车辆。

本发明由于采取以上技术方案，其具有以下优点：

1.由于本发明设置了非结构化道路冲突区域的优先级分配原则，有利于提高非结构化道路冲突区域路口的通行效率；

2.由于本发明提供了一种拓展时间维度的混合A*算法，增加了轨迹冲突点调整的空间，降低死锁的可能，所采取的速度协调和多车避碰策略，保证了规划轨迹的最优性和安全性。

3.由于本发明还对速度曲线进行平滑处理，提高车辆行驶的平顺性与轨迹的可跟踪性。

附图说明

图1为本发明实施例中3车辆分别从不同的路口驶入的路况示意图。

图2为本发明实施例中非结构化道路冲突区域的多车轨迹协同规划的框架示意图。

图3为本发明实施例中非结构化道路冲突区域设置成圆形控制区的示意图。

图4为自行车模型。

图5为本发明实施例中节点的示意图。

图6为本发明实施例中步骤22和步骤23的流程图。

图7为本发明实施例中的杜宾斯曲线组合图。

图8为本发明实施例中车辆避碰的虚拟圆示意图。

图9为本发明实施例中高优先级车辆的侵入应用场景示意图。

图10为本发明实施例中被侵入车辆的ST图的示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

非结构化道路是指无明显道路边界及车道线的道路(如乡村道路、矿区、港口、停车场和园区等)。非结构化道路冲突区域是指在非结构化道路中两个以上车辆的行驶路径存在交叉，重叠等空间冲突，因配置不合理速度而可能导致车辆发生碰撞的区域。

如图1所示，图中示出了非结构化道路冲突区域，有5个不同方向的路口，车辆a、车辆b和车辆c分别从不同路口，欲驶入该非结构化道路冲突区域。从图中的这三辆车行驶方向延伸的虚线意味着是该车辆的行驶轨迹，行驶轨迹延伸到目标驶出路口。车辆b和车辆c分别在t₁时刻各自到达p₁位置和p₃位置，在t₂时刻分别到达p₂位置和p₄位置，同时，车辆a恰好在t₁时刻至t₂时时刻的时间段内到达p_a位置附近，那么在t₁时刻至t₂时时刻的时间段内，这三辆车之间会发生碰撞。此时，至少调整其中两辆车的轨迹，才能避免发生碰撞。那么，调整车辆原始冲突轨迹，直至各车均不发生碰撞的过程称作为多车轨迹协同规划。

如图2所示，多车轨迹协同规划由图中的轨迹规划模块完成。结合图2和图6，轨迹规划模块在实施多车轨迹协同规划前，轨迹规划模块从任务规划模块获取车辆整个驾驶任务的总体目标，如图6中示出的获取整个驾驶任务的起始点、目标点、车辆参数、地图边界信息和初始时刻t₀。轨迹规划模块也从定位模块获取车辆位置，即车辆在大地坐标系中的坐标。轨迹规划模块还从地图模块获取整个驾驶任务所需的高精度地图(包括地图边界信息)，并对获取得到的地图进行栅格化处理，获得H值查询表，由此可以获取车辆中心的位置(通常指车辆的后轴中心)的横坐标x、纵坐标y，航向角θ和时刻t。

轨迹规划模块在实施多车轨迹协同规划的过程基本可以概括成：根据获取到的整个驾驶任务的总体目标、高精度地图和车辆位置，规划出一条最优的轨迹，该轨迹信息包括大地坐标系下的车辆的位置的横坐标x、纵坐标y，航向角θ，曲率k，速度v和时刻t等信息。最后，轨迹规划模块将规划出的轨迹信息发送给运动控制模块，由运动控制模块根据运动轨迹控制车辆的油门、刹车、方向盘等执行机构，使车辆按照轨迹规划模块将规划出的轨迹行驶。这里的“最优”也就是根据任务规划***的总体目标而确定的，通常为路径最短，或者到达时间最快等目标。

在本发明实施例中，轨迹规划模块进行多车轨迹协同规划方法包括：

步骤1，确定进入到非结构化道路冲突区域的车辆的优先级顺序。

作为步骤1的一种优选实施方式，可以先采用规划时间段区分进入控制区的每一批次的车辆的优先级，再在同一批次中按照车辆的重量大小进一步区分优先级，其中，同一重量等级的车辆按照进入预设控制区的时间顺序确定优先级。本实施例提供的确定进入到非结构化道路冲突区域的车辆的优先级顺序的方法具体包括：

步骤11，根据非结构化道路冲突区域的面积大小预设控制区，例如：如图3将预设控制区设置为半径为R的圆形区域，道路限速v_max。

步骤12，初始化计数变量p＝1，计算各车到达控制区的时刻，记录第一个车辆到达控制区的轨迹点时刻t₀。

步骤13，计算第p批次的截止时刻t₀+p·ΔT，寻找到达时刻t_p满足t₀+(p-1)·ΔT＜t_p＜t₀+p·ΔT条件的车辆，将其放入第p批次车辆集合中。采用规划时间段来区分每一批次的车辆，第一批次的优先级一定高于第二批次，以此类推对每辆车进行批次分类。其中，规划时间段

预设控制区的平均通行速度

步骤14，在第p批次车辆集合中，按照车辆重量级降序排列，对于相同重量级车辆，按照进入控制区的时刻，升序排列。也就是说，在同一批次中，按照车辆的重量大小继续分类，重量大的车辆优先级更高，同一重量等级的车辆，按照进入路口控制区的时间分类，进入时刻越早，优先级越高。本实施例考虑到：重量大的车辆刹车加速将损耗更多的能量，从多车的燃油经济型考虑，需要减少大重量车辆的速度变化。

步骤15，判断是否还存在车辆未分配到批次的集合中，如果有，则令p＝p+1，跳转步骤14，否则进入步骤16。

步骤16，从第1批次到最后批次依次为车辆排序，输出车辆的优先级顺序。

任务开始时，各车辆获得原始轨迹仅避让地图中的静态障碍物，未考虑与其它车辆之间的碰撞。根据各车原始轨迹可以计算到达圆形控制区的时刻t_p，根据优先级顺序为各车辆重新规划轨迹，车辆实际到达控制区后按照新规划的轨迹行驶，可以快速安全的通过冲突区域。

除了上述方式之外，还可以采用其他方式确定进入到非结构化道路冲突区域的车辆的优先级顺序，例如，不考虑车辆重量，也可以将预设控制区域设置成其它形状等等。

步骤2，根据步骤1确定的优先级顺序进行单车轨迹规划，获得初级轨迹。

在一个实施例中，步骤2可以采用传统混合A*算法、随机树算法，跳点算法，迪杰斯特拉算法进行轨迹规划，也可以采用一种扩展时间维度的混合A*算法，按照各车优先级顺序依次进行轨迹规划，这样在每一个规划步长内低优先级的车辆需避让静态障碍物与高优先级的车辆。本实施例提供的扩展时间维度的混合A*算法具体包括：

步骤21，结合车辆运动学模型，拓展节点。例如，根据m个不同前轮转角、n个不同速度以及1个停车等待节点，拓展获得总计m·n+1个节点。

在一个实施例中，车辆运动学模型可以选择满足阿克曼转向约束的自行车模型，将车辆在状态空间的状态描述为

本实施例中在状态空间中增加时间维度t，则车辆的状态空间表示为

当然，也可以根据实际场景适应调整为其他车辆运动学模型。如图4示出的满足阿克曼转向约束的自行车模型，center表示车辆的中心点

为车辆的车身横摆角，α表示车辆的前轮转角，α_max表示车辆的前轮转角最大值，l表示前后车轮轴的轴距，R_f表示前轮转角半径，R_r表示后轮转角半径。于是，可以在车辆运动学模型中，通过前轮转角最大值α_max对节点的角度范围进行约束，根据车辆轴距l、搜索步长s以及前轮转角α计算车身横摆角变化量

在一个实施例中，“根据m个不同前轮转角、n个不同速度等级以及1个停车等待节点，拓展获得总计m·n+1个节点”的方法可以参考图5示出的例子加以说明。

图5中的矩形框代表车辆，实心圆点为本实施例中的节点，该节点的所在位置对应为车辆中心点所在位置。m＝3，意味着该示例中有3个不同前轮转角，例如左、中、右三个转角方向。n＝3，意味着该示例中有3个不同速度等级，例如：高速、中速、低速。那么，根据3个不同前轮转角、3个不同速度等级，可以按照图中示出的同一前轮转角按照n＝3个速度等级分层，为n＝3层，同一速度等级按照m＝3个前轮转角分层，为m＝3层，由此可以拓展出的节点数量为m·n个，同时还有1个停车等待节点，那么，总共有(m·n+1)＝10个节点。

更具体地，同一速度等级按照m个前轮转角分层拓展，获得节点的方法包括如下子步骤：

子步骤211，将n个不同速度等级的其中之一作为车辆当前速度v_current，按下式(1)计算搜索拓展步长s与该拓展步长内的车身横摆角变化量

式(1)中，ΔT为时间分辨率，其获取方法为：根据空间连通性和车辆运动学约束，将空间离散为四维离散空间(x,y,θ,t)，再在离散空间的四个维度上分别设置时间分辨率ΔT、栅格分辨率Δx*Δy、航向角分辨率Δθ，以控制拓展出的节点的数量，保证搜索速度。

子步骤212，根据步长s内车身横摆角变化量

通过车辆运动学约束公式(2)进行拓展，获得节点。例如，下式(2)中，节点(x_k+1,y_k+1,θ_k+1,t_k+1)表示由其父节点(x_k,y_k,θ_k,t_k)在前轮转角α下经过ΔT拓展得到的节点：

式(2)中，θ_k+1、(x_k+1,y_k+1)、t_k+1分别表示车辆在拓展出的第k+1个节点的航向角、中心点的位置坐标、时刻，θ_k、(x_k,y_k)、t_k分别表示车辆在拓展出的第k个节点的航向角、中心点的位置坐标、时刻。

当然，“同一前轮转角按照n个速度等级分层拓展，获得节点”的方法也可以采用子步骤211和212实现，因此，不再赘述。

在结构化道路冲突区域行驶，不应该存在倒车情况，并且需要调整车辆速度避让动态障碍物。本实施例取消了倒车节点的拓展，在m个不同前轮转角的情况下，按照n个不同的速度向前进行分层拓展。当车辆数量增加时，仅仅通过减速，很大概率找不到无碰撞的多车轨迹，增加一个拓展用的停车等待节点，这样可以增大规划的成功率。

步骤22，根据步骤21拓展出的每一个节点，选取最优的节点作为下一父节点。

在一个实施例中，如图6所示，步骤22具体包括：

步骤221，初始化open集和close集节点，初始化后的open集用于存储步骤21拓展出的所有节点，初始化后的close集为空集。

步骤222，从open集中取出所有的节点，利用下式(3)计算所有的节点的代价值f(n)，将代价值f(n)最小的节点作为父节点，并加入到closed集中。

f(n)＝g(n)+h(n)                 (3)

式中，n表示open集中节点的索引，f(n)为代价函数，代价函数f(n)由距目标点的预估距离代价h(n)以及距离起始点的累计代价g(n)组成，用以引导节点朝着目标方向搜索，避免不必要的节点拓展，从而提高路径搜索效率；h(n)为各栅格由距目标点的预估距离代价，由Dijkstra算法以目标点为起始位置预先计算得到，后期在路径搜索过程中可通过查表得到各节点距目标点的预估距离代价h(n)；g(n)为距离起始点的累计代价。

在一个实施例中，累计代价g(n)包括角度代价、累计距离代价、停车代价和速度代价，其中：停车代价为停车等待节点额外设置的负激励，速度代价根据父节点和当前拓展节点之间的速度差计算得到，如下式(4)所示，用来惩罚过大的速度变化，其通过下式在节点拓展过程中实时计算得到：

式(4)中，

分别表示车辆在第n+1个节点、第n个节点的车身横摆角，v_n+1、v_n分别表示车辆在第n+1个节点、第n个节点的速度，w_step、w_θchange、w_vchange、w_stop分别为步长惩罚系数、角度切换惩罚系数、速度切换惩罚系数和停车惩罚系数，P_stop为停车惩罚值，这些系数的数值可以自适应的调整以适应不同的规划目标，在使用时需要根据实际规划任务调试获得，数值遵循的调试原则是：步长惩罚w_step数值设置越大，节点则更倾向于向远处拓展；角度切换惩罚w_θchange数值设置越大，更容易规划出直线；速度切换惩罚w_vchange数值设置越大，车辆的速度越平稳；停车惩罚w_stop数值设置越大，越不容易出现停车的状态，但是各参数过大又会影响计算速度。

步骤223，判断步骤222获得的父节点是否在目标点附近，如果是，则进入步骤23，否则进入步骤224。其中，“附近”可以理解为两个点之间的间距小于预设值(例如30m)，视为父节点在目标点附近。

步骤224，从步骤222确定的父节点开始，再次拓展出若干子节点，先判断每一个子节点是否能够通过碰撞检测，若判定为是，即通过碰撞检测，则继续判断该通过碰撞检测的子节点是否处于open集合中，如果判定为是，即处于open集合中，则进一步判断此次拓展的累计代价值g(n)是否小于open集合中记录过的g(n)值的大小，如果判定为是，则更新该通过碰撞检测的子节点的累计代价g(n)和父节点，否则不做处理。最后，将判定为不在closed集合与open集合中的子节点放入open集合中，跳转步骤222。

其中，“更新父节点”时，由于父节点只是在记录他是由哪个节点拓展过来的，直接在存储结构中更改下即可。

步骤23，结合图6，以在目标点附近的父节点为起点，目标点为终点，使用终点位姿拟合的方式，拟合轨迹，若拟合成功，则输出初级轨迹，否则，跳转步骤22继续拓展下一父节点。

在一个实施例中，采用杜宾斯(Dubins)曲线，结合速度协调策略，为拟合轨迹的路径的每一个路点赋予速度，从而能够避免与动态障碍物的碰撞。如图7所示，图7中，S代表直线，L代表左旋方向的圆，R代表右旋方向的圆，步骤23具体包括：

步骤231，设置步骤22中得到的目标点附近的父节点为起始点P_i，目标点为终止点P_f。

步骤232，按照杜宾斯曲线拟合的方式，从起始点P_i向终止点P_f进行拟合，杜宾斯曲线拟合的原理如下：

可以根据起始点P_i、终止点P_f以及引入的切线点1与切线点2中间点，将拟合轨迹的路径分成三段，分别是：从起始点P_i到切线点1之间的第一线段，切线点1到切线点2之间的第二线段，切线点2到终止点P_f之间的第三线段。这三段线段的类型属于dubins曲线集合D＝{LSL，RSR，RSL，LSR，RLR，LRL}中的一种，其中，L表示绕逆时针转动的圆弧段，S表示直线段，R表示绕顺时针转动的圆弧段。

步骤233，检测拟合的杜宾斯曲线的路径是否会与静态障碍物发生碰撞，如果判定为不与静态障碍物发生碰撞，则按照子步骤211中拓展节点时选取的车辆当前速度v_current，上下采样多种速度赋予拟合的杜宾斯曲线的路径得到多个拟合轨迹，根据步骤22中得到的目标点附近的父节点的拓展时刻，计算多个拟合轨迹的各路点时刻，如果在父节点的拓展时刻与拟合轨迹终点的时刻间，判定为不与其它高优先级车辆发生碰撞，则从步骤22中得到的目标点附近的父节点开始回溯得到搜索轨迹，将搜索轨迹与拟合轨迹进行拼接得到初级轨迹，如果采样的多个轨迹均不满足动静态障碍物的避碰要求，那么返回步骤22继续拓展。其中。高优先级的车辆指的是优先级比本车高的所有车辆。

步骤3，优化平滑步骤2输出的初级轨迹，获得一条平滑连续可跟踪的最终轨迹。

步骤31，建立ST(英文全称为“distance time”；中文全称为“路程时间”)图。

如图8所示，采用一个虚拟圆包裹车辆的车身，该虚拟圆需要满足：1.不重叠；2.可保证车辆之间的无碰撞轨迹；3.对于相同尺寸的车辆拥有相同的避碰半径R_{col_i}。

图9提供了一个高优先级车辆的侵入类型的侵入应用场景示意图，展示了高优先级车辆三种主要侵入形式，车辆a的初级轨迹表示为La，车辆b、车辆c和车辆d均为高优先级车辆，其中：车辆b的初级轨迹表示为Lb，车辆c的初级轨迹表示为Lc，车辆d的初级轨迹表示为Ld，这4车辆为同一尺寸。

如果这四辆车均按照各自的初级轨迹行驶，则会有结果：车辆b横向侵入车辆a的初级轨迹La，车辆c同向侵入车辆a的初级轨迹La，车辆d逆向侵入车辆a的初级轨迹La。

如果按图9中的双向箭头方向示意地，将车辆a的初级轨迹La分别向其左、右各偏移距离R_{col_1}+R_{col_i}，得到碰撞风险区域边界Lal和Laf。同时，车辆a与其他车辆的碰撞风险区域重合，均为图9中碰撞区域风险边界中间的区域，，其它车辆的虚拟圆的中心到达碰撞区域风险边界中间的区域的点为侵入点，离开此区域的点为驶离点。P_{2_in}为车辆b的侵入点、P_{2_out}为车辆b的驶离点、P_{3_in}为车辆c的侵入点、P_{4_in}为车辆d的侵入点，由于车辆c与车辆d为纵向侵入，驶离点分别为车辆a的目标点和起始点。

根据各车侵入情况，制作高优先级车辆占据车辆a的ST图。如图10所示，图中，纵坐标轴S的坐标Send表示轨迹目标点的距离，横坐标t表示时间，原点O表示初始时刻0s时，车辆位于起点位置，P代表的是侵入或者驶离点，与之对应地，侵入点时刻在图中表示为下标in和驶出时刻在图中表示为下标out。高优先级车辆的速度投影至车辆a的轨迹上得到初始的入侵曲线，该初始的入侵曲线被向上、下各平移一个R_{col_1}+R_{col_i}的距离，得到各高优先级车辆占据车辆a的ST图，即图中呈现出的类四边形。例如，图10中，L_{p2_u}与L_{p2_l}分别表示车辆b向上平移R_{col_1}+R_{col_i}的距离的入侵曲线、向下平移R_{col_1}+R_{col_i}的距离的入侵曲线，侵入点P_{2_in}和驶离点P_{2_out}所在的类四边形为车辆b占据车辆a的ST图。L_{p3_u}与L_{p3_l}分别表示车辆c向上平移R_{col_1}+R_{col_i}的距离的入侵曲线、向下平移R_{col_1}+R_{col_i}的距离的入侵曲线，侵入点P_{3_in}和驶离点P_{3_out}所在的类四边形为车辆c占据车辆a的ST图。L_{p4_u}与L_{p4_l}分别表示车辆d向上平移R_{col_1}+R_{col_i}的距离的入侵曲线、向下平移R_{col_1}+R_{col_i}的距离的入侵曲线，侵入点P_{4_in}和驶离点P_{4_out}所在的类四边形为车辆d占据车辆a的ST图。

鉴于此，车辆a的轨迹只要不与上述3个类四边形干涉便可以防止发生碰撞。

图9示例中的场景不大，轨迹长度短，可以做出全部的ST图，如果冲突区域较大(一般半径1km以上的圆形区域属于冲突区域大)，车辆形成的轨迹可能比较长，使用整段轨迹建立ST图进行速度规划可能有太大的计算量导致求解速度下降，此时应当采取匀加速度规划与数值优化相结合的方法，先求解几个冲突点之间的速度曲线，其它无冲突的地方匀加速度行驶即可。假设图10中的ST图轨迹较长，首先，求解t_{2_in}至t_{2_out}时间段与t_{3_in}至目标点时刻时间段的速度曲线。车辆在起始点时刻到t_{2_in}将以0时刻的初速度为起始速度，求解出来的t_{2_in}时刻的速度为目标点速度作匀加减速运动，在t_{2_out}到t_{3_in}之间以t_{2_out}时刻的速度为起始速度，t_{3_in}时刻的速度为目标点速度作匀加减速运动。将两段匀加减速速度曲线与数值求解的速度曲线拼接起来，得到车辆最终的速度曲线。

步骤32，设定如下式(5)描述的代价函数，对初级轨迹进行优化，优化需要满足三个指标：第一，优化后轨迹与初级轨迹的速度差值要小；第二，优化后的速度尽可能大以保证车辆快速通行；第三，加速度小以保证行驶的平顺性。

其中，F是代价函数function，二次规划中的代价函数，s_i是第i个优化点的frenet坐标系下的距离，s_i'是s_i的一阶导数，s_i”是s_i的二阶导数，s_i+1”是第i+1个优化点的frenet坐标系下的距离s_i+1的一阶导数，第一项

为优化后速度与原始速度差的积分，第二项

为加速度的积分，第三项为

加加速度的积分，W_{cos_ref}，W_{cos_dds}，W_{cos_ddds}分别为这三项指标的惩罚系数，三者具体数值互相影响，需要根据具体的规划任务确定具体值的大小，设置原则包括：W_{cos_ref}影响靠近原始速度的程度，数值越大，则计算出来的速度越靠近原始速度；W_{cos_dds}影响车辆的速度大小，数值越大，则车辆速度越快；W_{cos_ddds}影响车辆的加速度，数值越大，则加速度越小。

建立约束：

第一约束，优化过程需要考虑最大限速，设置速度约束：0≤s_i'≤V_max，V_max是当前道路的最大限速；

第二约束，轨迹的曲率过大时，车辆的速度不能过高，否则会出现侧滑，建立曲率约束：

a_ymax＝0.2*9.8，其中kappa为轨迹点的曲率大小，a_ymax是当前道路的最大横向加速度；

第三约束，路口通行不允许倒车，建立倒车约束：s_i+1≥s_i；

第四约束，保证车辆行驶平顺性，加速度约束：a_min≤s_i”≤a_max，a_min、a_max分别指车辆平顺行驶的最小和最大加速度，通常a_max＝2m/s²，a_min＝-4m/s²；

第五约束，根据ST图获得车辆的避碰约束，即在每个离散时间点，车辆的轨迹不与其它车辆占据ST图的多边形干涉，建立避碰约束：

t₁≤t≤t₂时，0＜s_i＜s_obs1；

t₃≤t≤t₄时；0＜s_i＜s_obs2；

式中，t₁、t₂分别是第一辆车的侵入时刻与驶离时刻，t₃、t₄分别是第二辆车的侵入时刻与驶离时刻，s_obs1，s_obs2分别泛指动态障碍物1和动态障碍物2；

第六约束，保证起始点和目标点状态的一致性，建立两点边值约束：

s₀＝0，s₀'＝v_start，s₀”＝a_start；

s_T＝s_end，s_T'＝v_end，s_T”＝0；

式中，s₀是起点frenet坐标系下的距离值一般等于0，v_start指根据规划场景预设得到的起点速度，a_start指根据规划场景预设的起点加速度，s_T指目标点frenet坐标系下的距离值，s_end指目标点frenet坐标系下的距离值，v_end指根据规划场景预设的目标点的速度；

第七约束，保证速度和加速度的连续性，即三阶导数

Δt是二次规划的求解分辨率，其具体数值根据实际设定的方程决定大小，一般为0.1s；四阶导数及以上为0，对s_i+1,s_i+1'进行有限项的泰勒展开，忽略无穷小项。

整理得满足二阶导数连续的等式约束：

根据代价函数与约束情况，求解得到s_i，s_i'，s_i”，结合步骤21得到的节点的坐标值，形成带有速度和加速度信息的最终轨迹。将平滑后的轨迹进行存储，根据这些轨迹信息可以预估车辆在每个时刻所处位置，低优先级车辆每个时刻将避让高优先级车辆，并按照这些存储的轨迹信息进行碰撞检测和速度规划。

本发明实施例还提供一种非结构化道路冲突区域的多车轨迹协同规划装置，其包括车辆优先级确定单元、初级轨迹获取单元和初级轨迹获取单元，其中：

车辆优先级确定单元用于确定进入到非结构化道路冲突区域的车辆的优先级顺序。

初级轨迹获取单元用于根据优先级顺序进行单车轨迹规划，获得初级轨迹。

最终轨迹获取单元用于优化平滑初级轨迹，获得一条平滑连续可跟踪的最终轨迹。

其中，初级轨迹获取单元具体包括节点拓展子单元、父节点选取子单元和终点位姿拟合子单元，其中：

节点拓展子单元用于结合车辆运动学模型，拓展节点。

父节点选取子单元用于根据拓展出的每一个节点，选取最优的节点作为下一父节点。

终点位姿拟合子单元用于以在目标点附近的父节点为起点，目标点为终点，使用终点位姿拟合的方式，拟合轨迹，若拟合成功，则输出初级轨迹，否则，由父节点选取子单元继续拓展下一父节点。

其中，父节点选取子单元具体包括初始化模块、代价值计算模块和父节点筛选模块，其中：

初始化模块用于初始化open集和close集节点，初始化后的open集用于存储步骤21拓展出的所有节点，初始化后的close集为空集。

代价值计算模块，其用于将利用式(3)计算的open集的代价值f(n)最小的节点作为父节点，并加入到closed集中。

父节点筛选模块用于判断代价值计算模块获得的父节点是否在目标点附近，如果是，则由终点位姿拟合子单元拟合轨迹；否则，从父节点开始，再次拓展出若干子节点，先判断每一个子节点是否能够通过碰撞检测，若判定为是，则继续判断该通过碰撞检测的子节点是否处于open集合中，如果判定为是，则进一步判断此次拓展的累计代价值g(n)是否小于open集合中记录过的g(n)值的大小，如果判定为是，则更新该通过碰撞检测的子节点的累计代价g(n)和父节点，最后，将判定为不在closed集合与open集合中的子节点放入open集合中，并由代价值计算模块将利用式(3)计算的open集的代价值f(n)最小的节点作为父节点，并加入到closed集中。

在一个实施例中，节点拓展子单元为：根据m个不同前轮转角、n个不同速度以及1个停车等待节点，按照如下方式之一，拓展获得总计m·n+1个节点；

第一种方式：同一速度等级按照m个前轮转角分层拓展；

第二种方式：同一前轮转角按照n个速度等级分层拓展；

其中，按照同一速度等级按照m个前轮转角分层拓展，获得总计m·n+1个节点的方法包括上述子步骤211和子步骤212。

在一个实施例中，终点位姿拟合子单元具体包括杜宾斯曲线拟合模块和初始轨迹输出模块，其中：

杜宾斯曲线拟合模块用于以设置父节点选取子单元中得到的目标点附近的父节点为起始点P_i，目标点为终止点P_f，按照杜宾斯曲线拟合的方式，从起始点P_i向终止点P_f进行拟合。

初始轨迹输出模块用于检测拟合的杜宾斯曲线的路径是否会与静态障碍物发生碰撞，如果判定为不与静态障碍物发生碰撞，则按照车辆当前速度v_current，上下采样多种速度赋予拟合的杜宾斯曲线的路径得到多个拟合轨迹，根据父节点选取子单元中得到的目标点附近的父节点的拓展时刻，计算多个拟合轨迹的各路点时刻，如果在父节点的拓展时刻与拟合轨迹终点的时刻间，判定为不与其它高优先级车辆发生碰撞，则从父节点选取子单元中得到的目标点附近的父节点开始回溯得到搜索轨迹，将搜索轨迹与拟合轨迹进行拼接得到初级轨迹，如果采样的多个轨迹均不满足动静态障碍物的避碰要求，则由父节点选取子单元继续拓展；其中，高优先级的车辆指的是优先级比本车高的所有车辆。

上述各实施例中，对本发明中的优先级分配规则进行简单修改，或者采取其它图搜索的路径规划算法进行替代，对数值优化的参数和优化指标进行调整，约束的形式进行改变都可以达到大致相同的效果，但均属于本发明的构思范围。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (6)

1.一种非结构化道路冲突区域的多车轨迹协同规划方法，其特征在于，包括：

步骤1，确定进入到非结构化道路冲突区域的车辆的优先级顺序；

步骤2，根据步骤1确定的优先级顺序进行单车轨迹规划，获得初级轨迹；

步骤3，优化平滑步骤2输出的初级轨迹，获得一条平滑连续可跟踪的最终轨迹；

其中，步骤2具体包括：

步骤21，结合车辆运动学模型，拓展节点；

步骤22，根据拓展出的每一个节点，选取最优的节点作为下一父节点；

步骤23，以在目标点附近的父节点为起点，目标点为终点，使用终点位姿拟合的方式，拟合轨迹，若拟合成功，则输出初级轨迹，否则，跳转步骤22继续拓展下一父节点；

步骤22具体包括：

步骤221，初始化open集和close集节点，初始化后的open集用于存储步骤21拓展出的所有节点，初始化后的close集为空集；

步骤222，将利用下式(3)计算的open集的代价值f(n)最小的节点作为父节点，并加入到closed集中；

f(n)＝g(n)+h(n)                 (3)

式中，n表示open集中节点的索引，h(n)为距目标点的预估距离代价，g(n)为距离起始点的累计代价；

步骤223，判断步骤222获得的父节点是否在目标点附近，如果是，则进入步骤23，否则进入步骤224；

步骤224，从父节点开始，再次拓展出若干子节点，先判断每一个子节点是否能够通过碰撞检测，若判定为是，则继续判断该通过碰撞检测的子节点是否处于open集合中，如果判定为是，则进一步判断此次拓展的累计代价值g(n)是否小于open集合中记录过的g(n)值的大小，如果判定为是，则更新该通过碰撞检测的子节点的累计代价g(n)和父节点，最后，将判定为不在closed集合与open集合中的子节点放入open集合中，跳转步骤222；

步骤21为：根据m个不同前轮转角、n个不同速度以及1个停车等待节点，按照如下方式之一，拓展获得总计m·n+1个节点；

第一种方式：同一速度等级按照m个前轮转角分层拓展；

第二种方式：同一前轮转角按照n个速度等级分层拓展；

按照同一速度等级按照m个前轮转角分层拓展，获得总计m·n+1个节点的方法包括：

子步骤211，将n个不同速度等级的其中之一作为车辆当前速度v_current，按下式(1)计算搜索拓展步长s与该拓展步长内的车身横摆角变化量

式(1)中，l表示前后车轮轴的轴距，α表示车辆的前轮转角，ΔT为时间分辨率；

子步骤212，根据步长s内车身横摆角变化量

通过车辆运动学约束公式(2)进行拓展，获得节点；

式(2)中，θ_k+1、(x_k+1,y_k+1)、t_k+1分别表示车辆在拓展出的第k+1个节点的航向角、中心点的位置坐标、时刻，θ_k、(x_k,y_k)、t_k分别表示车辆在拓展出的第k个节点的航向角、中心点的位置坐标、时刻，R_r表示后轮转角半径；

步骤222中的累计代价g(n)通过下式(4)计算得到：

式(4)中，

分别表示车辆在第n+1个节点、第n个节点的车身横摆角，v_n+1、v_n分别表示车辆在第n+1个节点、第n个节点的速度，w_step、w_θcnange、w_vchange、w_stop分别为步长惩罚系数、角度切换惩罚系数、速度切换惩罚系数和停车惩罚系数，P_stop为停车惩罚值。

2.如权利要求1所述的非结构化道路冲突区域的多车轨迹协同规划方法，其特征在于，步骤1具体包括：

步骤11，根据非结构化道路冲突区域的面积大小预设控制区；

步骤12，初始化计数变量p＝1，计算各车到达控制区的时刻，记录第一个车辆到达控制区的轨迹点时刻t₀；

步骤13，计算第p批次的截止时刻t₀+p·ΔT，寻找到达时刻t_p满足t₀+(p-1)·ΔT<t_p<t₀+p·ΔT条件的车辆，将其放入第p批次车辆集合中；

步骤14，在第p批次车辆集合中，按照车辆重量级降序排列，对于相同重量级车辆，按照进入控制区的时刻，升序排列；

步骤15，判断是否还存在车辆未分配到批次的集合中，如果有，则令p＝p+1，跳转步骤14，否则进入步骤16；

步骤16，从第1批次到最后批次依次为车辆排序，输出车辆的优先级顺序。

3.如权利要求1所述的非结构化道路冲突区域的多车轨迹协同规划方法，其特征在于，步骤23具体包括：

步骤231，设置步骤22中得到的目标点附近的父节点为起始点P_i，目标点为终止点P_f；

步骤232，按照杜宾斯曲线拟合的方式，从起始点P_i向终止点P_f进行拟合；

步骤233，检测拟合的杜宾斯曲线的路径是否会与静态障碍物发生碰撞，如果判定为不与静态障碍物发生碰撞，则按照车辆当前速度v_current，上下采样多种速度赋予拟合的杜宾斯曲线的路径得到多个拟合轨迹，根据步骤22中得到的目标点附近的父节点的拓展时刻，计算多个拟合轨迹的各路点时刻，如果在父节点的拓展时刻与拟合轨迹终点的时刻间，判定为不与其它高优先级车辆发生碰撞，则从步骤22中得到的目标点附近的父节点开始回溯得到搜索轨迹，将搜索轨迹与拟合轨迹进行拼接得到初级轨迹，如果采样的多个轨迹均不满足动静态障碍物的避碰要求，那么返回步骤22继续拓展；其中，高优先级的车辆指的是优先级比本车高的所有车辆。

4.如权利要求3所述的非结构化道路冲突区域的多车轨迹协同规划方法，其特征在于，步骤3具体包括：

步骤31，建立高优先级车辆占据本车的ST图；

步骤32，设定如下式描述的代价函数和建立如下约束：

其中，F是代价函数function，s_i是第i个优化点的frenet坐标系下的距离，s_i'是s_i的一阶导数，s_i”是s_i的二阶导数，s_i+1”是第i+1个优化点的frenet坐标系下的距离s_i+1的二阶导数，第一项

为优化后速度与原始速度差的积分，第二项

为加速度的积分，第三项为

加加速度的积分，W_{cos_ref}，W_{cos_dds}，W_{cos_ddds}分别为这三项指标的惩罚系数；

第一约束，设置速度约束：0≤s_i'≤V_max，V_max是当前道路的最大限速；

第二约束，建立曲率约束：

其中，kappa为轨迹点的曲率大小，a_ymax是当前道路的最大横向加速度；

第三约束，建立倒车约束：s_i+1≥s_i；

第四约束，建立加速度约束：a_min≤s_i”≤a_max，a_min、a_max分别指车辆平顺行驶的最小和最大加速度；

第五约束，建立避碰约束：

t₁≤t≤t₂时，0＜s_i＜s_obs1；

t₃≤t≤t₄时；0＜s_i＜s_obs2；

式中，t₁、t₂分别是第一辆车的侵入时刻与驶离时刻，t₃、t₄分别是第二辆车的侵入时刻与驶离时刻，s_obs1，s_obs2分别泛指动态障碍物1和动态障碍物2；

第六约束，建立两点边值约束：

s₀＝0，s₀'＝v_start，s₀”＝a_start；

s_T＝s_end，s_T'＝v_end，s_T”＝0；

式中，s₀是起点frenet坐标系下的距离值，v_start指起点速度，a_start指起点加速度，s_T指目标点frenet坐标系下的距离值，s_end指目标点frenet坐标系下的距离值，v_end指目标点的速度大小；

第七约束，对s_i+1,s_i+1'进行有限项的泰勒展开，忽略无穷小项：

根据代价函数与约束情况，求解得到s_i，s_i'，s_i”，结合步骤21得到的节点的坐标值，形成带有速度和加速度信息的最终轨迹。

5.一种非结构化道路冲突区域的多车轨迹协同规划装置，其特征在于，包括：

车辆优先级确定单元，其用于确定进入到非结构化道路冲突区域的车辆的优先级顺序；

初级轨迹获取单元，其用于根据优先级顺序进行单车轨迹规划，获得初级轨迹；

最终轨迹获取单元，其用于优化平滑初级轨迹，获得一条平滑连续可跟踪的最终轨迹；

其中，初级轨迹获取单元具体包括：

节点拓展子单元，其用于结合车辆运动学模型，拓展节点；

父节点选取子单元，其用于根据拓展出的每一个节点，选取最优的节点作为下一父节点；

终点位姿拟合子单元，其用于以在目标点附近的父节点为起点，目标点为终点，使用终点位姿拟合的方式，拟合轨迹，若拟合成功，则输出初级轨迹，否则，由父节点选取子单元继续拓展下一父节点；

父节点选取子单元具体包括：

初始化模块，其用于初始化open集和close集节点，初始化后的open集用于存储节点拓展子单元拓展出的所有节点，初始化后的close集为空集；

代价值计算模块，其用于将利用下式(3)计算的open集的代价值f(n)最小的节点作为父节点，并加入到closed集中；

f(n)＝g(n)+h(n)                 (3)

式中，n表示open集中节点的索引，h(n)为距目标点的预估距离代价，g(n)为距离起始点的累计代价；

父节点筛选模块，其用于判断代价值计算模块获得的父节点是否在目标点附近，如果是，则由终点位姿拟合子单元拟合轨迹；否则，从父节点开始，再次拓展出若干子节点，先判断每一个子节点是否能够通过碰撞检测，若判定为是，则继续判断该通过碰撞检测的子节点是否处于open集合中，如果判定为是，则进一步判断此次拓展的累计代价值g(n)是否小于open集合中记录过的g(n)值的大小，如果判定为是，则更新该通过碰撞检测的子节点的累计代价g(n)和父节点，最后，将判定为不在closed集合与open集合中的子节点放入open集合中，并由代价值计算模块将利用式(3)计算的open集的代价值f(n)最小的节点作为父节点，并加入到closed集中；

节点拓展子单元为：根据m个不同前轮转角、n个不同速度以及1个停车等待节点，按照如下方式之一，拓展获得总计m·n+1个节点；

第一种方式：同一速度等级按照m个前轮转角分层拓展；

第二种方式：同一前轮转角按照n个速度等级分层拓展；

其中，按照同一速度等级按照m个前轮转角分层拓展，获得总计m·n+1个节点的方法包括：

子步骤211，将n个不同速度等级的其中之一作为车辆当前速度v_current，按下式(1)计算搜索拓展步长s与该拓展步长内的车身横摆角变化量

式(1)中，ΔT为时间分辨率，l表示前后车轮轴的轴距，α表示车辆的前轮转角；

子步骤212，根据步长s内车身横摆角变化量

通过车辆运动学约束公式(2)进行拓展，获得节点；

式(2)中，θ_k+1、(x_k+1,y_k+1)、t_k+1分别表示车辆在拓展出的第k+1个节点的航向角、中心点的位置坐标、时刻，θ_k、(x_k,y_k)、t_k分别表示车辆在拓展出的第k个节点的航向角、中心点的位置坐标、时刻，R_r表示后轮转角半径。

6.如权利要求5所述的非结构化道路冲突区域的多车轨迹协同规划装置，其特征在于，终点位姿拟合子单元具体包括：

杜宾斯曲线拟合模块，其用于以设置父节点选取子单元中得到的目标点附近的父节点为起始点P_i，目标点为终止点P_f，按照杜宾斯曲线拟合的方式，从起始点P_i向终止点P_f进行拟合；

初始轨迹输出模块，其用于检测拟合的杜宾斯曲线的路径是否会与静态障碍物发生碰撞，如果判定为不与静态障碍物发生碰撞，则按照车辆当前速度v_current，上下采样多种速度赋予拟合的杜宾斯曲线的路径得到多个拟合轨迹，根据父节点选取子单元中得到的目标点附近的父节点的拓展时刻，计算多个拟合轨迹的各路点时刻，如果在父节点的拓展时刻与拟合轨迹终点的时刻间，判定为不与其它高优先级车辆发生碰撞，则从父节点选取子单元中得到的目标点附近的父节点开始回溯得到搜索轨迹，将搜索轨迹与拟合轨迹进行拼接得到初级轨迹，如果采样的多个轨迹均不满足动静态障碍物的避碰要求，则由父节点选取子单元继续拓展；其中，高优先级的车辆指的是优先级比本车高的所有车辆。

Images