局部路径规划方法和装置

1.本技术涉及车辆智能驾驶领域，尤指一种局部路径规划方法和装置。


背景技术：

2.随着社会经济和汽车工业的发展和进步，人们越来越注重汽车的行驶安全。无人驾驶汽车作为减少交通事故的有力手段吸引了众多学者的研究。无人驾驶汽车包括环境感知，全局路径规划，局部路径规划及轨迹跟踪等环节，其中局部路径规划需要输出合适的纵向力和转向角等确保无人驾驶汽车可以安全到达目的地。
3.目前已有一些学者对局部路径规划进行了研究。郭洪艳(申请号cn202010647239.3)以区域虚拟力场对道路环境进行建模，利用模型预测控制对车辆未来状态进行预测，并基于此完成横纵向耦合路径规划的任务，但该发明未考虑周围障碍物的运动变化，实时性不足；余伶俐(申请号cn201911081063.3)建立无人驾驶车辆多目标路径规划问题的数学模型；利用线性动态规划方法进行路径规划，解决了基于随机撒点的路径规划方法得到的路径难以符合车辆运动学约束的问题，但该发明没有对自车与周围障碍物的碰撞风险进行准确评估；江昆提出一种基于学习采样式的汽车自动驾驶运动规划方法及系统，通过计算每条前向仿真轨迹的评价值，选取评价值最高的轨迹作为规则最优轨迹，但该发明计算量大，算法实时性难以保证。


技术实现要素：

4.本技术提供了一种局部路径规划方法及装置，解决了以往的局部路径规划算法不够准确，实时性差，对于突发的碰撞风险应对不足的问题，提高了驾驶效率和安全性能。
5.本技术提供的局部路径规划方法，包括：
6.预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列；其中，周围车辆是指距离第一车辆在预设范围内的车辆，所述周围车辆集合是指全部周围车辆的集合；
7.根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型；根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角，以实现最优化的局部路径规划。
8.一种示例性实施例中，所述预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列，包括：
9.根据所述周围车辆集合中的每个周围车辆在当前时刻之前预设第二时间段内的第二横向速度序列和第二纵向速度序列通过预先建立的lstm网络预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列。
10.一种示例性实施例中，所述根据所述第一横向速度序列、第一纵向速度序列建立
碰撞风险评估系数模型包括：
11.根据所述第一横向速度序列、第一纵向速度序列建立第一车辆与周围车辆集合的碰撞风险评估系数模型；
12.根据预设的道路边界建立第一车辆与道路边界的碰撞风险评估系数模型；
13.根据所述第一车辆与周围车辆集合的碰撞风险评估系数模型与所述第一车辆与道路边界的碰撞风险评估系数模型建立碰撞风险评估系数模型。
14.一种示例性实施例中，所述碰撞风险评估系数模型为：
[0015][0016]
其中，k
nc
为rai
nc
的权重，k
r
为rai
r
的权重，rai
nc,i
表示第一车辆与周围车辆集合中的第i个周围车辆的碰撞风险评估系数；rai
r,k
表示第一车辆与第k条道路的道路边界的碰撞风险评估系数。
[0017]
一种示例性实施例中，所述预设的第一车辆的动力学模型为：
[0018][0019]
其中，v
x
,v
y
和ω分别表示第一车辆的纵向速度，侧向速度和横摆角速度，m表示第一车辆的总质量，f
x
表示驱动轮纵向合力，f
y,f
和f
y,r
表示前轮和后轮的侧向力，l
f
和l
r
分别表示前轮和后轮的轮距，i
z
表示第一车辆的横摆转动惯量，δ表示前轮转向角。
[0020]
一种示例性实施例中，根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角，包括：
[0021]
将所述第一车辆的动力学模型进行线性化和离散化，得到线性化和离散化后的第一车辆的动力学模型；
[0022]
根据所述碰撞风险评估系数模型、预设的全局轨迹跟踪项建立mpc代价函数；
[0023]
通过模型预测控制算法求解满足所述mpc代价函数以及预设的车辆动力学和运动学约束的所述线性化和离散化后的第一车辆的动力学模型，得到第一车辆跟踪全局路径的当前时刻的纵向力和转向角。
[0024]
一种示例性实施例中，线性化和离散化后的第一车辆的动力学模型为：
[0025][0026]
其中，a
d
，b
d
和c
d
表示离散化后的状态空间矩阵，x(k)和u(k)表示离散化后的状态变量和控制变量，ns,nu和ny分别表示状态量的个数、控制量的个数和模型输出量的个数。
[0027]
一种示例性实施例中，所述预设的车辆动力学和运动学约束包括道路规则约束、控制量约束、输出量约束。
[0028]
一种示例性实施例中，所述预设的全局轨迹跟踪项用标准二次式表示为：
[0029][0030]
其中，
[0031][0032][0033][0034][0035][0036][0037]
s1＝n
p
‑
1,s2＝n
p
‑
2,s
n
＝n
p
‑
n
c
‑
2，
[0038]
h1和g1表示代价函数项j1的hessian矩阵和梯度矩阵，a
～
和b
～
表示预测时域内的预测矩阵，q和r表示状态变量的权重矩阵；y
pre
为预测时域内第一车辆的动力学模型的输出状态量，y
ref
为第一车辆的动力学模型的输出状态量的期望值，反映对控制量平稳变化的要求；为q，r系数矩阵，q、r是mpc代价函数中固有的系数矩阵，n
p
为预测时域，n
c
为控制时域，ny为第一车辆的动力学模型输出状态量的个数；
[0039]
所述mpc代价函数为：
[0040]
其中，ε
k
表示k步处的松弛变量。
[0041]
本技术还提供了一种局部路径规划装置，包括存储器和处理器，
[0042]
所述存储器，用于保存用于局部路径规划的程序；
[0043]
所述处理器，用于读取执行所述用于局部路径规划的程序，执行如下操作：
[0044]
预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列；其中，周围车辆是指距离第一车辆在预设范围内的车辆，所述周围车辆集合是指全部周围车辆的集合；
[0045]
根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型；根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角，以实现最优化的局部路径规划。
[0046]
一种示例性实施例中，所述预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列，包括：
[0047]
根据所述周围车辆集合中的每个周围车辆在当前时刻之前预设第二时间段内的第二横向速度序列和第二纵向速度序列通过预先建立的lstm网络预测周围车辆集合中的
每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列。
[0048]
一种示例性实施例中，所述根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型包括：
[0049]
根据所述第一横向速度序列、第一纵向速度序列建立第一车辆与周围车辆集合的碰撞风险评估系数模型；
[0050]
根据预设的道路边界建立第一车辆与道路边界的碰撞风险评估系数模型；
[0051]
根据所述第一车辆与周围车辆集合的碰撞风险评估系数模型与所述第一车辆与道路边界的碰撞风险评估系数模型建立碰撞风险评估系数模型。
[0052]
一种示例性实施例中，所述碰撞风险评估系数模型为：
[0053][0054]
其中，k
nc
为rai
nc
的权重，k
r
为rai
r
的权重，rai
nc,i
表示第一车辆与周围车辆集合中的第i个周围车辆的碰撞风险评估系数；rai
r,k
表示第一车辆与第k条道路的道路边界的碰撞风险评估系数。
[0055]
一种示例性实施例中，所述预设的第一车辆的动力学模型为：
[0056][0057]
其中，v
x
,v
y
和ω分别表示第一车辆的纵向速度，侧向速度和横摆角速度，m表示第一车辆的总质量，f
x
表示驱动轮纵向合力，f
y,f
和f
y,r
表示前轮和后轮的侧向力，l
f
和l
r
分别表示前轮和后轮的轮距，i
z
表示第一车辆的横摆转动惯量，δ表示前轮转向角。
[0058]
一种示例性实施例中，根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角，包括：
[0059]
将所述第一车辆的动力学模型进行线性化和离散化，得到线性化和离散化后的第一车辆的动力学模型；
[0060]
根据所述碰撞风险评估系数模型、预设的全局轨迹跟踪项建立mpc代价函数；
[0061]
通过模型预测控制算法求解满足所述mpc代价函数以及预设的车辆动力学和运动学约束的所述线性化和离散化后的第一车辆的动力学模型，得到第一车辆跟踪全局路径的当前时刻的纵向力和转向角。
[0062]
一种示例性实施例中，线性化和离散化后的第一车辆的动力学模型为：
[0063][0064]
其中，a
d
，b
d
和c
d
表示离散化后的状态空间矩阵，x(k)和u(k)表示离散化后的状态变量和控制变量，n
s
,n
u
和n
y
分别表示状态量的个数、控制量的个数和模型输出量的个数。
[0065]
一种示例性实施例中，所述预设的车辆动力学和运动学约束包括道路规则约束、控制量约束、输出量约束。
[0066]
一种示例性实施例中，所述预设的全局轨迹跟踪项用标准二次式表示为：
[0067][0068]
其中，
[0069][0070][0071][0072][0073][0074][0075]
s1＝n
p
‑
1,s2＝n
p
‑
2,s
n
＝n
p
‑
n
c
‑
2，
[0076]
h1和g1表示代价函数项j1的hessian矩阵和梯度矩阵，a
～
和b
～
表示预测时域内的预测矩阵，q和r表示状态变量的权重矩阵；y
pre
为预测时域内第一车辆的动力学模型的输出状态量，y
ref
为第一车辆的动力学模型的输出状态量的期望值，反映对控制量平稳变化的要求；为q，r系数矩阵，q、r是mpc代价函数中固有的系数矩阵，n
p
为预测时域，n
c
为控制时域，ny为第一车辆的动力学模型输出状态量的个数；
[0077]
所述mpc代价函数为：
[0078]
其中，ε
k
表示k步处的松弛变量。
[0079]
本技术的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本技术而了解。本技术的其他优点可通过在说明书以及附图中所描述的方案来实现和获得。
附图说明
[0080]
附图用来提供对本技术技术方案的理解，并且构成说明书的一部分，与本技术的实施例一起用于解释本技术的技术方案，并不构成对本技术技术方案的限制。
[0081]
图1为本技术实施例的局部路径规划方法的原理图；
[0082]
图2为本技术实施例的局部路径规划方法的流程图；
[0083]
图3为本技术实施例的lstm层的原理图。
[0084]
图4为本技术实施例的自车的三自由度动力学模型。
[0085]
图5a为本技术实施例的紧急换道场景示意图；
[0086]
图5b为本技术实施例的基于lstm预测的局部路径规划方法输出的轨迹图；
[0087]
图6为本技术实施例的局部路径规划装置的示意图。
具体实施方式
[0088]
图1为本技术实施例的局部路径规划方法的原理图，如图1所示，本实施例的局部路径规划方法，包括s11
‑
s13步骤：
[0089]
s11、预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列；
[0090]
s12、根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型；
[0091]
s13、根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角，以实现最优化的局部路径规划。
[0092]
其中，周围车辆是指距离第一车辆在预设范围内的车辆，所述周围车辆集合是指全部周围车辆的集合。
[0093]
一种示例性的实施例中，所述预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列，包括：
[0094]
根据所述周围车辆集合中的每个周围车辆在当前时刻之前预设第二时间段内的第二横向速度序列和第二纵向速度序列通过预先建立的lstm网络预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列。
[0095]
一种示例性的实施例中，基于highd数据集构建和设计lstm网络，实现对周围车辆横纵向速度的预测。其中，highd数据集是德国高速公路的大型自然车辆轨迹数据集。lstm网络的输入是周围车辆过去一段时间的横纵向速度(通过v2v通信获取)，输出为预测时域内的横纵向速度序列。
[0096]
一种示例性的实施例中，所述根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型包括：
[0097]
根据所述第一横向速度序列、第一纵向速度序列建立第一车辆与周围车辆集合的碰撞风险评估系数模型；
[0098]
根据预设的道路边界建立第一车辆与道路边界的碰撞风险评估系数模型；
[0099]
根据所述第一车辆与周围车辆集合的碰撞风险评估系数模型与所述第一车辆与道路边界的碰撞风险评估系数模型建立碰撞风险评估系数模型。
[0100]
一种示例性的实施例中，所述碰撞风险评估系数模型为：
[0101][0102]
其中，k
nc
为rai
nc
的权重，k
r
为rai
r
的权重，rai
nc,i
表示第一车辆与周围车辆集合中的第i个周围车辆的碰撞风险评估系数；rai
r,k
表示第一车辆与第k条道路的道路边界的碰撞风险评估系数。
[0103]
一种示例性的实施例中，所述预设的第一车辆的动力学模型为：
[0104][0105]
其中，v
x
,v
y
和ω分别表示第一车辆的纵向速度，侧向速度和横摆角速度，m表示第一车辆的总质量，f
x
表示驱动轮纵向合力，f
y,f
和f
y,r
表示前轮和后轮的侧向力，l
f
和l
r
分别表示前轮和后轮的轮距，i
z
表示第一车辆的横摆转动惯量，δ表示前轮转向角。
[0106]
一种示例性的实施例中，根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角，包括：
[0107]
将所述第一车辆的动力学模型进行线性化和离散化，得到线性化和离散化后的第一车辆的动力学模型；
[0108]
根据所述碰撞风险评估系数模型、预设的全局轨迹跟踪项建立mpc代价函数；
[0109]
通过模型预测控制算法求解满足所述mpc代价函数以及预设的车辆动力学和运动学约束的所述线性化和离散化后的第一车辆的动力学模型，得到第一车辆跟踪全局路径的当前时刻的纵向力和转向角。
[0110]
一种示例性的实施例中，线性化和离散化后的第一车辆的动力学模型为：
[0111][0112]
其中，a
d
，b
d
和c
d
表示离散化后的状态空间矩阵，x(k)和u(k)表示离散化后的状态变量和控制变量，n
s
,n
u
和n
y
分别表示状态量的个数、控制量的个数和模型输出量的个数。
[0113]
一种示例性的实施例中，所述预设的车辆动力学和运动学约束包括道路规则约束、控制量约束、输出量约束。
[0114]
一种示例性的实施例中，所述预设的全局轨迹跟踪项用标准二次式表示为：
[0115][0116]
其中，
[0117][0118][0119][0120][0121]
[0122][0123]
s1＝n
p
‑
1,s2＝n
p
‑
2,s
n
＝n
p
‑
n
c
‑
2，
[0124]
h1和g1表示代价函数项j1的hessian矩阵和梯度矩阵，和表示预测时域内的预测矩阵，q和r表示状态变量的权重矩阵；y
pre
为预测时域内第一车辆的动力学模型的输出状态量，y
ref
为第一车辆的动力学模型的输出状态量的期望值，反映对控制量平稳变化的要求；为q，r系数矩阵，q、r是mpc代价函数中固有的系数矩阵，n
p
为预测时域，n
c
为控制时域，ny为第一车辆的动力学模型输出状态量的个数；
[0125]
所述mpc代价函数为：
[0126]
其中，ε
k
表示k步处的松弛变量。
[0127]
本技术实施例的局部路径规划方法通过预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列；根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型；根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角，从而实现了最优化的局部路径规划，解决了以往的局部路径规划算法不够准确，实时性差，对于突发的碰撞风险应对不足的问题，提高了驾驶效率和安全性能，有利于在实车中应用。
[0128]
图2为本技术实施例的局部路径规划方法的流程图，主要分为3个部分：
[0129]
第一部分：基于highd数据集构建和设计lstm网络，实现对周围车辆横纵向速度的预测。lstm网络的输入是周围车辆过去一段时间的横纵向速度，输出为预测时域内的横纵向速度序列。lstm网络的预测时域n
p
＝20，由输入层，lstm层，全连接层以及输出层组成，其中lstm层的原理如图3所示，包括：
[0130]
遗忘门，以上一个单元的输出和当前单元的输入组合为输入的sigmoid函数，产生一个在[0,1]内的值，来控制上一单元状态被遗忘的程度，如式(1)所示：
[0131][0132]
输入门，和一个tanh函数配合控制新信息的通过，产生一个新的候选向量。输入门产生一个在[0,1]内的值，控制新信息被通过的多少，如式(2)所示：
[0133][0134]
输出门，用来控制当前的单元状态有多少被过滤掉。先将单元状态激活，输出门为其中每一项产生一个在[0,1]内的值，控制单元状态被过滤的程度,如式(3)所示：
[0135][0136]
结合遗忘门的输出f、输入门的输入i以及tanh激活函数可实现存储信息的更新,如式(4)所示：
[0137]
[0138][0139]
lstm层的输出可表示为：
[0140][0141]
其中，w
x,i
、w
x,f
、w
x,o
、w
x,g
(维度均为r
100
×1),以及w
h,i
、w
h,f
、w
h,o
、w
h,g
(维度均为r
100
×
100
)表示lstm的线性变换矩阵，b
i
、b
f
、b
o
、b
g
表示偏差向量。
[0142]
第二部分：结合第一部分所预测的周围车辆的横纵向速度，以及道路边界约束，设计碰撞风险评估系数rai。碰撞风险评估系数的值越大，表示自车越接近周围车辆或道路边界，越容易发生碰撞；
[0143]
碰撞风险评估系数rai中考虑周围车辆及道路边界对自车行驶安全的影响，如式(6)所示：
[0144][0145]
rai
nc
表示自车与周围车辆的碰撞风险评估系数，rai
r
表示自车与道路边界的碰撞风险评估系数，k
nc
和k
r
分别为rai
nc
和k
r
的权重。
[0146]
rai具体计算方法包括以下步骤：
[0147]
(2.1)自车与周围车辆的碰撞风险评估系数rai
nc
，如式(7)所示：
[0148][0149]
s
i
表示第i个周围车辆与自车之间的安全距离，a
i
和b
i
表示rai
nc
的形状和强度参数(a
i
和b
i
表示两个系数，形状和强度参数仅表示名字，无特殊含义)，dx和dy表示自车与周围车辆纵向和横向距离，x0和y0表示最小纵向距离和最小横向距离，t0表示安全时间间隔，θ表示自车与周围车辆航向角的夹角，u
o,i
表示第i个周围车辆的纵向速度，u表示自车(即上文的第一车辆)的纵向速度，a
n
表示舒适系数，δu
a,i
表示自车和周围车辆的纵向相对速度，δv
a,i
表示自车和周围车辆的横向相对速度，w、w0、k
u
和k
w
表示权重系数，i为正整数。
[0150]
(2.2)自车与道路边界的碰撞风险评估系数rai
r
，如式(8)所示：
[0151][0152]
a
k
表示rai
r
的形状参数，d
a
表示道路边界的允许距离，s
r,k
表示第k条道路边界与自车之间的安全距离。
[0153]
第三部分：应用模型预测控制算法(mpc)计算实现碰撞风险评估系数最小化以及在车辆动力学和运动学约束内跟踪全局路径的纵向力和转向角，避免自车与周围车辆发生碰撞。mpc算法设计包括以下步骤
[0154]
(3.1)预测模型：
[0155]
如图4，考虑自车的纵向运动，侧向运动及横摆运动建立三自由度动力学模型，如
式(9)所示：
[0156][0157]
自车的运动学公式为：
[0158][0159]
其中，v
x
、v
y
和ω分别表示自车纵向速度，侧向速度和横摆角速度，x、y和表示自车的纵向位置，横向位置和航向角。m表示自车总质量，f
x
表示驱动轮纵向合力，f
y,f
和f
y,r
表示前轮和后轮的侧向力。l
f
和l
r
分别表示前轮和后轮的轮距，i
z
表示自车横摆转动惯量。
[0160]
基于线性轮胎模型计算车轮侧偏力，
[0161][0162]
其中，c
f
和c
r
表示前轮和后轮的侧偏刚度，δ表示前轮转向角。
[0163]
分别采用泰勒多项式和欧拉方法对上述非线性车辆动力学模型进行线性化和离散化，线性化和离散化后的模型如式(12)所示：
[0164][0165]
其中，a
d
、b
d
和c
d
表示离散化后的状态空间矩阵，x(k)和u(k)表示离散化后的状态变量和控制变量。n
s
,n
u
和n
y
分别表示状态量的个数、控制量的个数和模型输出量的个数。表示向量或矩阵。
[0166]
(3.2)车辆动力学和运动学约束：
[0167]
mpc算法中考虑了道路规则，控制量和输出量的约束。道路规则约束了行驶车速，
[0168][0169]
控制量约束包括边界限制和增量限制，如下所示
[0170][0171]
轮胎的侧向和纵向力受摩擦椭圆限制，
[0172]
[0173]
f
x_max
表示轮胎纵向力的最大值，f
y_max
表示轮胎侧向力的最大值，μ表示路面附着系数。
[0174]
同时，摩擦椭圆随垂向载荷的转移而变化。考虑纵向载荷转移，前后轮的垂向载荷如下所示：
[0175][0176]
其中，g和h表示重力加速度和质心高度。
[0177]
因此，轮胎力的椭圆约束如下：
[0178][0179]
(3.3)最优求解：
[0180]
将rai项、全局轨迹跟踪项以及为扩展可行域而设计的松弛变量引入mpc代价函数。全局轨迹跟踪项可以转化为标准二次式，如式(18)所示
[0181]
其中，
[0182][0183][0184][0185][0186][0187][0188]
s1＝n
p
‑
1,s2＝n
p
‑
2,s
n
＝n
p
‑
n
c
‑
2，
[0189]
h1和g1表示代价函数项j1的hessian矩阵和梯度矩阵，a
～
和b
～
表示预测时域内的预测矩阵，q和r表示状态变量的权重矩阵。
[0190]
碰撞风险评估系数rai是一个非线性非凸函数，传统的qp求解器无法直接求解。通
常，这类问题可以用序列二次规划(sqp)方法求解，其主要思想是将原非线性非凸问题近似为一系列凸子问题，rai函数采用式(19)中的泰勒近似法离线逼近，
[0191][0192]
其中，
[0193][0194][0195][0196][0197]
其中，x(k)表示k步长的纵向位置，x
obs
(k)表示障碍物的纵向位置。
[0198]
最终，优化问题描述如下:
[0199][0200]
s.t.(k＝1,
…
n
p
)，
[0201]
(此式用于计算模型在预测时域内的状态)，
[0202][0203][0204]
(此式表示代价函数中状态量的软约束)，
[0205]
(此式表示t之后k
‑
1步处的车速预测值介于最高速和最低速之间)，
[0206][0207]
(表示求解过程的约束)，
[0208]
其中，s.t表示受限制于，t+k表示t之后k步处的预测值；表示在t步之后k
‑
1步的控制量，x
t+k,t
表示在t步之后k步的状态量，x
t+k
‑
1,t
表示在t步之后k
‑
1步的状态量，x为预测模型的状态量，x为纵向位置；表示代价函数中状态量的等式约束，即上述摩擦椭圆约束的抽象形式；εk表示k步处的松弛变量，c
s
和d
s
分别表示约束变量的输出矩阵和前馈矩阵。
[0209]
将上述优化问题转化成近似二次凸优化问题，应用序列二次规划(sqp)求解器得到所需的车轮纵向力和转向角，完成局部路径规划，确保自车安全到达目的地。
[0210]
图5a为本技术实施例的紧急换道场景示意图；图5b为本技术实施例的基于lstm预
测的局部路径规划方法输出的轨迹图。从图5b中可以看出自车在无碰撞的情况下完成了换道操作，验证了本技术的有效性。
[0211]
图6为本技术实施例的局部路径规划装置的示意图，如图6所示，局部路径规划装置包括存储器和处理器，其特征在于：
[0212]
所述存储器，用于保存用于局部路径规划的程序；
[0213]
所述处理器，用于读取执行所述用于局部路径规划的程序，执行如下方法：
[0214]
预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列；其中，周围车辆是指距离第一车辆在预设范围内的车辆，所述周围车辆集合是指全部周围车辆的集合；
[0215]
根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型；根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角，以实现最优化的局部路径规划。
[0216]
其中，周围车辆是指距离第一车辆在预设范围内的车辆，所述周围车辆集合是指全部周围车辆的集合。
[0217]
一种示例性的实施例中，所述预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列，包括：
[0218]
根据所述周围车辆集合中的每个周围车辆在当前时刻之前预设第二时间段内的第二横向速度序列和第二纵向速度序列通过预先建立的lstm网络预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列。
[0219]
一种示例性的实施例中，基于highd数据集构建和设计lstm网络，实现对周围车辆横纵向速度的预测。其中，highd数据集是德国高速公路的大型自然车辆轨迹数据集。lstm网络的输入是周围车辆过去一段时间的横纵向速度(通过v2v通信获取)，输出为预测时域内的横纵向速度序列。
[0220]
一种示例性的实施例中，所述根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型包括：
[0221]
根据所述第一横向速度序列、第一纵向速度序列建立第一车辆与周围车辆集合的碰撞风险评估系数模型；
[0222]
根据预设的道路边界建立第一车辆与道路边界的碰撞风险评估系数模型；
[0223]
根据所述第一车辆与周围车辆集合的碰撞风险评估系数模型与所述第一车辆与道路边界的碰撞风险评估系数模型建立碰撞风险评估系数模型。
[0224]
一种示例性的实施例中，所述碰撞风险评估系数模型为：
[0225][0226]
其中，k
nc
为rai
nc
的权重，k
r
为rai
r
的权重，rai
nc,i
表示第一车辆与周围车辆集合中的第i个周围车辆的碰撞风险评估系数；rai
r,k
表示第一车辆与第k条道路的道路边界的碰撞风险评估系数。
[0227]
一种示例性的实施例中，所述预设的第一车辆的动力学模型为：
[0228][0229]
其中，v
x
,v
y
和ω分别表示第一车辆的纵向速度，侧向速度和横摆角速度，m表示第一车辆的总质量，f
x
表示驱动轮纵向合力，f
y,f
和f
y,r
表示前轮和后轮的侧向力，l
f
和l
r
分别表示前轮和后轮的轮距，i
z
表示第一车辆的横摆转动惯量，δ表示前轮转向角。
[0230]
一种示例性的实施例中，根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角，包括：
[0231]
将所述第一车辆的动力学模型进行线性化和离散化，得到线性化和离散化后的第一车辆的动力学模型；
[0232]
根据所述碰撞风险评估系数模型、预设的全局轨迹跟踪项建立mpc代价函数；
[0233]
通过模型预测控制算法求解满足所述mpc代价函数以及预设的车辆动力学和运动学约束的所述线性化和离散化后的第一车辆的动力学模型，得到第一车辆跟踪全局路径的当前时刻的纵向力和转向角。
[0234]
一种示例性的实施例中，线性化和离散化后的第一车辆的动力学模型为：
[0235][0236]
其中，a
d
，b
d
和c
d
表示离散化后的状态空间矩阵，x(k)和u(k)表示离散化后的状态变量和控制变量，n
s
,n
u
和n
y
分别表示状态量的个数、控制量的个数和模型输出量的个数。
[0237]
一种示例性的实施例中，所述预设的车辆动力学和运动学约束包括道路规则约束、控制量约束、输出量约束。
[0238]
一种示例性的实施例中，所述预设的全局轨迹跟踪项用标准二次式表示为：
[0239][0240]
其中，
[0241][0242][0243][0244][0245]
[0246][0247]
s1＝n
p
‑
1,s2＝n
p
‑
2,s
n
＝n
p
‑
n
c
‑
2，
[0248]
h1和g1表示代价函数项j1的hessian矩阵和梯度矩阵，a
～
和b
～
表示预测时域内的预测矩阵，q和r表示状态变量的权重矩阵；y
pre
为预测时域内第一车辆的动力学模型的输出状态量，y
ref
为第一车辆的动力学模型的输出状态量的期望值，反映对控制量平稳变化的要求；为q，r系数矩阵，q、r是mpc代价函数中固有的系数矩阵，n
p
为预测时域，n
c
为控制时域，ny为第一车辆的动力学模型输出状态量的个数；
[0249]
所述mpc代价函数为：
[0250]
其中，ε
k
表示k步处的松弛变量。
[0251]
本技术实施例的局部路径规划装置通过预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列；根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型；根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角，从而实现了最优化的局部路径规划，解决了以往的局部路径规划算法不够准确，实时性差，对于突发的碰撞风险应对不足的问题，提高了驾驶效率和安全性能，有利于在实车中应用。
[0252]
本技术描述了多个实施例，但是该描述是示例性的，而不是限制性的，并且对于本领域的普通技术人员来说显而易见的是，在本技术所描述的实施例包含的范围内可以有更多的实施例和实现方案。尽管在附图中示出了许多可能的特征组合，并在具体实施方式中进行了讨论，但是所公开的特征的许多其它组合方式也是可能的。除非特意加以限制的情况以外，任何实施例的任何特征或元件可以与任何其它实施例中的任何其他特征或元件结合使用，或可以替代任何其它实施例中的任何其他特征或元件。
[0253]
本技术包括并设想了与本领域普通技术人员已知的特征和元件的组合。本技术已经公开的实施例、特征和元件也可以与任何常规特征或元件组合，以形成由权利要求限定的独特的发明方案。任何实施例的任何特征或元件也可以与来自其它发明方案的特征或元件组合，以形成另一个由权利要求限定的独特的发明方案。因此，应当理解，在本技术中示出和/或讨论的任何特征可以单独地或以任何适当的组合来实现。因此，除了根据所附权利要求及其等同替换所做的限制以外，实施例不受其它限制。此外，可以在所附权利要求的保护范围内进行各种修改和改变。
[0254]
此外，在描述具有代表性的实施例时，说明书可能已经将方法和/或过程呈现为特定的步骤序列。然而，在该方法或过程不依赖于本文所述步骤的特定顺序的程度上，该方法或过程不应限于所述的特定顺序的步骤。如本领域普通技术人员将理解的，其它的步骤顺序也是可能的。因此，说明书中阐述的步骤的特定顺序不应被解释为对权利要求的限制。此外，针对该方法和/或过程的权利要求不应限于按照所写顺序执行它们的步骤，本领域技术人员可以容易地理解，这些顺序可以变化，并且仍然保持在本技术实施例的精神和范围内。
[0255]
本领域普通技术人员可以理解，上文中所公开方法中的全部或某些步骤、系统、装置中的功能模块/单元可以被实施为软件、固件、硬件及其适当的组合。在硬件实施方式中，在以上描述中提及的功能模块/单元之间的划分不一定对应于物理组件的划分；例如，一个物理组件可以具有多个功能，或者一个功能或步骤可以由若干物理组件合作执行。某些组件或所有组件可以被实施为由处理器，如数字信号处理器或微处理器执行的软件，或者被实施为硬件，或者被实施为集成电路，如专用集成电路。这样的软件可以分布在计算机可读介质上，计算机可读介质可以包括计算机存储介质(或非暂时性介质)和通信介质(或暂时性介质)。如本领域普通技术人员公知的，术语计算机存储介质包括在用于存储信息(诸如计算机可读指令、数据结构、程序模块或其他数据)的任何方法或技术中实施的易失性和非易失性、可移除和不可移除介质。计算机存储介质包括但不限于ram、rom、eeprom、闪存或其他存储器技术、cd
‑
rom、数字多功能盘(dvd)或其他光盘存储、磁盒、磁带、磁盘存储或其他磁存储装置、或者可以用于存储期望的信息并且可以被计算机访问的任何其他的介质。此外，本领域普通技术人员公知的是，通信介质通常包含计算机可读指令、数据结构、程序模块或者诸如载波或其他传输机制之类的调制数据信号中的其他数据，并且可包括任何信息递送介质。