一种实时轨迹规划方法

1.本技术涉及智能交通技术领域，尤其涉及一种实时轨迹规划方法。


背景技术：

2.自动驾驶汽车凭借先进的感知、规划与控制技术，能够有效减少交通事故、提高舒适性、解决交通堵塞、提高燃油经济性等，是未来汽车行业发展的重大战略方向之一。随着自动驾驶技术的普及以及人们对其的认可，道路上自动驾驶汽车的渗透率将会逐步上升，因此，未来道路上将长期或持续存在人类驾驶汽车与自动驾驶汽车共存的局面。人类驾驶员的驾驶行为与意图往往难以预测且实时变化，这对轨迹的规划的实时性提出了更高的要求。
3.目前，对于换道而言，大多数轨迹规划算法假设换道过程中其他环境车辆的状态保持不变，仅在换道初始时刻规划了轨迹。然而在实际的动态交通环境之中，其他环境车辆往往根据周围环境实时调整车速，如突然的加速或减速，使得初始规划的换道轨迹不再安全。因此，在确保安全、舒适和通行效率的基础上，提高算法的实时性，根据动态变化的交通环境自适应的规划换道轨迹是一个亟待解决的问题。


技术实现要素：

4.本技术提供了一种实时轨迹规划方法，其技术目的是针对动态变化的交通环境，兼顾安全性、舒适性、效率的同时提高轨迹规划方法的实时性，自适应地规划换道轨迹。
5.本技术的上述技术目的是通过以下技术方案得以实现的：
6.一种实时轨迹规划方法，包括：
7.s1：通过对时间采样的五次多项式构建纵横向换道轨迹模型；
8.s2：构建所述纵横向换道轨迹模型的约束条件，得到安全换道模型；
9.s3：对所述纵横向换道轨迹模型中的换道时间进行等间隔采样，将不同换道时间下的换道轨迹规划问题转换成二次规划问题，最后通过二次规划求解器得到不同换道时间tk下的换道轨迹；其中，所述二次规划问题的代价函数包括舒适性代价函数和经泰勒二阶展开后的安全性代价函数，所述舒适性代价函数通过加速度构建，所述安全性代价函数通过势场函数构建；
10.s4：基于安全性、舒适性以及换道通行效率从不同换道时间tk下的换道轨迹中筛选出最优换道轨迹；
11.s5：对周围车辆的未来状态进行预测，在预测步数内对主车与当前车道前车、目标车道前车及目标车道后车之间的间距是否满足所述安全换道模型进行判断，若满足则进行换道，否则重复步骤s1-s4对换道轨迹进行重新规划，直至主车进入目标车道。
12.本技术的有益效果在于：本技术所述的动态交通环境下实时轨迹规划方法，基于五次多项式建立了安全换道模型，综合考虑了换道的安全性、舒适性和通行效率，通过二次规划方法对换道轨迹进行优化，并从基于不同换道时间的优化轨迹中筛选出最优换道轨
迹，并考虑了换道过程中交通环境动态变化时决策是否对换道轨迹进行重新规划。该方法能够实现动态交通场景下自动驾驶车辆的安全换道，算法具有较高的实时性以及很强的实用性，以及广阔的商业应用前景。
附图说明
13.图1为本技术所述一般性换道场景示意图；
14.图2为本技术所述方法的流程图；
15.图3为本技术设计非线性紧急制动避撞安全约束线性简化示意图；
16.图4为本技术设计不同采样换道时间下的最优换道轨迹簇示意图；
17.图5为本技术得出的目标车道后车不同加速度时主车与其他车辆相对位置示意图；
18.图6为本技术得出的目标车道后车加速度为2m/s2时主车仿真结果示意图；
19.图7为本技术得出的目标车道后车加速度为3m/s2时主车仿真结果示意图。
具体实施方式
20.下面将结合附图对本技术技术方案进行详细说明。
21.图1为本技术所述的一般性换道场景示意图，其中hc、pc、tp和tf分别表示主车、当前车道前车、目标车道前车和目标车道后车，而主车驾驶行为不受当前车道后车影响，故不加以考虑。图2为本技术所述实时轨迹规划方法的流程图，该实时轨迹规划方法包括：
22.s1：通过对时间采样的五次多项式构建纵横向换道轨迹模型。
23.所述纵横向换道轨迹模型表示为：
[0024][0025]
其中，x(t)表示主车的纵向轨迹；y(t)表示主车的横向轨迹，ai表示纵向轨迹的五次多项式系数；bi表示横向轨迹的五次多项式系数；i＝0,1,2,3,4,5。
[0026]
通过主车的换道时间tf、换道初始时刻状态和换道结束时刻状态确定ai和bi，表示为：
[0027][0028][0029]
其中，x0表示换道初始时刻主车的纵向位置；v
x,0
表示换道初始时刻主车的纵向速度；a
x,0
表示换道初始时刻主车的纵向加速度；y0表示换道初始时刻主车的横向位置；v
y,0
表示换道初始时刻主车的横向速度；a
y,0
表示换道初始时刻主车的横向加速度；xf表示换道结束时刻主车的纵向位置；v
x,f
表示换道结束时刻主车的纵向速度；a
x,f
表示换道结束时刻主车的纵向加速度；yf表示换道结束时刻主车的横向位置；v
y,f
表示换道结束时刻主车的横向速度；a
y,f
表示换道结束时刻主车的横向加速度。
[0030]
主车初始时刻的状态可以直接从传感器获得，换道结束时刻的横向位置设置为目
标车道中心线所在的位置，横向速度与加速度均设为0，故只有换道结束时刻纵向的位置、速度与加速度需要进行优化。
[0031]
s2：构建所述纵横向换道轨迹模型的约束条件，得到安全换道模型。
[0032]
考虑到换道过程中主车与当前车道前车pc、目标车道前车tp及目标车道后车tf可能出现的碰撞情况，建立安全的换道模型，所述安全换道模型包括：
[0033]
换道过程中，当前车道前车可能发生紧急制动情况，则主车与当前车道前车应保持安全距离，故主车与当前车道前车的安全约束条件表示为：
[0034][0035]
其中，xh表示主车的纵向坐标；lh表示主车的长度；x
pc
表示当前车道前车的纵向坐标；l
pc
表示当前车道前车的长度；v
x,hc
表示主车的车速；v
x,pc
表示当前车道前车的车速；b
max
表示主车允许的最大减速度；b
pc,max
表示当前车道前车允许的最大减速度，ts表示最小安全车头时距。
[0036]
换道过程中，主车与目标车道前车的安全约束条件表示为：
[0037][0038]
换道过程中，主车与目标车道后车的安全约束条件表示为：
[0039][0040]
其中，x
tp
表示目标车道前车的纵向位置；v
x,tp
表示目标车道前车的车速；l
tp
表示目标车道前车的长度；b
tp,max
表示目标车道前车的最大减速度；x
tf
表示目标车道后车的纵向位置；v
x,tf
表示目标车道后车的车速；l
tf
表示目标车道后车的长度；b
tf,max
表示目标车道后车的最大减速度；
[0041]
则式(3)至式(5)构成所述安全换道模型。
[0042]
对于上述式(3)和式(4)中存在的非线性二次安全约束，提出一种采用线性约束进行简化替代的方法以提高求解效率。如图3所示，其中虚线为紧急制动避撞二次非线性安全约束边界，对式(3)和式(4)中的非线性紧急制动碰撞安全约束条件分别采用多段直线进行逼近，如图3中直线l1、l2、l3所示，则式(3)和式(4)转换为：
[0043]ki'vx,hc
+x
hc
≤m
i'
；
ꢀꢀꢀ
(6)
[0044]
其中，k
i'
表示直线l
i'
的斜率；m
i'
表示直线l
i'
的截距；i'＝1,2,3；则所述安全换道模型表示成式(6)和式(5)。
[0045]
s3：对所述纵横向换道轨迹模型中的换道时间进行等间隔采样，将不同换道时间下的换道轨迹规划问题转换成二次规划问题，最后通过二次规划求解器得到不同换道时间tk下的换道轨迹；其中，所述二次规划问题的代价函数包括舒适性代价函数和经泰勒二阶
展开后的安全性代价函数，所述舒适性代价函数通过加速度构建，所述安全性代价函数通过势场函数构建。
[0046]
换道时间对安全性、舒适性和换道通行效率有很大的影响。换道时间过短则会导致横向加速度过大，舒适性较差，换道时间过长则会降低道路的通行效率，因此换道时间应该控制在合理的范围之内：t
min
≤tf≤t
max
。
[0047]
而任意指定换道时间显然是不合理的，故对换道时间tf在t
min
与t
max
之间进行等间隔采样，表示为：
[0048][0049]
其中，t
min
表示允许的最小换道时间；t
max
表示允许的最大换道时间；δt表示采样时间间隔；n表示采样个数。
[0050]
考虑到实际的驾驶情况，车辆应满足运动学和动力学要求，表示为：
[0051][0052]
其中，lw表示车道宽度，车辆应保持在车道内，车速不应超过道路允许的最大车速v
x,max
，纵横向加速度应保持在a
x,max
和a
y,max
之内。
[0053]
所述二次规划问题表示为：
[0054][0055]
其中，jc表示舒适性代价函数，js表示经泰勒二阶展开后的安全性代价函数。
[0056]
舒适性是车辆换道性能的重要指标，通常采用加速度来衡量，则舒适性代价函数表示为：
[0057][0058]
其中，j
x
表示纵向加速度；jy表示横向加速度；w
lon
表示纵向权重系数；w
lat
表示横向权重系数。
[0059]
在以往的规划算法中，对安全性通过硬约束保证，代价函数中仅考虑了舒适性导致规划出的轨迹往往趋向安全约束边界，然而在动态交通环境下安全约束是实时变化地，轨迹很容易突破安全约束边界。本技术通过构造势场函数，以提高轨迹的安全性，所述势场函数表示为：
[0060][0061]
其中，u
tp
表示目标车道前车产生的势场；u
tf
表示目标车道后车产生的势场。hc与tp和tf距离越近、相对速度越大，则u
tp
和u
tf
值越大。则总的安全性代价函数表示为：
[0062]
j(xf,v
x,f
)＝w
putp
+wfu
tf
；
ꢀꢀꢀ
(11)
[0063]
其中，w
p
和wf均表示权重系数。
[0064]
构造的势场函数为非二次型，为提高算法的求解效率，本技术对安全性代价函数采用泰勒二阶展开，表示为：
[0065][0066]
其中，x＝[xf,v
x,f
]。
[0067]
最后，通过二次规划求解器则可以得到不同换道时间tk(k＝0,1,...,n)下最优换道轨迹末时刻纵向的位置、速度和加速度，则换道轨迹即可确定。
[0068]
s4：基于安全性、舒适性以及换道通行效率从不同换道时间tk下的换道轨迹中筛选出最优换道轨迹。
[0069]
如图4所示，通过步骤s3，则可以得到在不同换道时间tk下的最优换道轨迹簇，然而不同的换道时间规划出轨迹的安全性、舒适性和通行效率也各有差异，因此需要综合从中筛选出最终的最优换道轨迹，表示为：
[0070][0071]
其中，j
*
表示换道时间tk下的舒适性代价函数和经泰勒二阶展开的安全性代价函数，wd表示换道通行效率的权重系数。
[0072]
s5：对周围车辆的未来状态进行预测，在预测步数内对主车与当前车道前车、目标车道前车及目标车道后车之间的间距是否满足安全约束条件进行判断，若满足则进行换道，否则重复步骤s1-s4对换道轨迹进行重新规划，直至主车进入目标车道。
[0073]
通过步骤s1-s4即能得到优化的车辆换道轨迹，但在换道过程中，周围的驾驶环境是实时变化地，使得规划出的轨迹不再满足安全需求，因此需要根据周围环境对换道轨迹进行自适应的重规划。首先，对周围车辆的未来状态进行预测，若周围车辆的加速度保持恒定，则周围车辆的运动轨迹表示为：
[0074][0075]
其中，表示周围车辆的当前纵向位置；表示周围车辆的速度；表示周围车辆的加速度；i＝pc,tp,tf，pc表示当前车道前车，tp表示目标车道前车，tf表示目标车道后
车。
[0076]
定义预测时间间隔δt、预测步数n
p
，则得到当前车道前车、目标车道前车及目标车道后车的预测位置序列表示为：
[0077]
xi(t0+δt|t0),xi(t0+δt|t0),...,xi(t0+δt|t0)；
ꢀꢀꢀ
(17)
[0078]
在预测步数n
p
步中，主车与当前车道前车、目标车道前车及目标车道后车之间的间距不满足所述安全换道模型时，表示为：
[0079][0080]
不满足安全约束条件时，则再次执行步骤s1-s4对换道轨迹进行重新规划，直到主车进入目标车道。
[0081]
作为具体实施例，本技术通过matlab/simulink-carsim联合仿真验证本技术所述方法的有效性。
[0082]
仿真实验中，hc与pc分别以20m/s、16m/s的速度行驶在右车道，pc在hc前方35m，tp与tf都以20m/s的速度行驶在左车道，分别在hc的前方与后方20m处。由于hc速度大于pc，因此hc需要决定换道还是减速继续跟随pc。在实际的交通场景中，如果hc换道，tf可能会减速礼让，也可能加速防止hc汇入左车道。在本实验中，当hc开始换道后，设置tf分别以0.5、2和3m/s2的加速度加速，hc也随之作出不同的决策，对轨迹进行重规划。图5中，不同形状代表不同的车不同时刻所在的位置，圆形代表hc，三角形代表pc，正方形代表tp，菱形代表tf。
[0083]
如图5中(a)所示，当检测到前方缓慢运动的pc时，hc与tp、tf车速相同，且间距满足安全要求，于是规划出一条换道轨迹。当hc开始换道后，tf以0.5m/s2加速度加速，由于加速度较小，初始规划出的换道轨迹依然安全。换道结束后，hc速度略大于tp，hc轻微减速度以调整车速对tp进行跟车。hc并与其他车保持安全距离，满足安全性要求。
[0084]
当tf加速度为2m/s2时，如图5中(b)所示，第一次规划出的轨迹与加速度为0.5m/s时相同。然而，由于加速度比较大，如果hc继续按照原来轨迹行驶，将会与tf发生碰撞，hc规划出一条更紧急的换道轨迹。hc检测到tf加速，如图6所示，hc加速汇入到目标车道，但速度变化较小。汇入目标车道后，由于与前车存在速度差，hc第三次规划轨迹调整速度以跟随前车。hc与其他车辆保持安全距离，且纵向和横向加速度分别小于0.1g和0.2g，满足舒适性要求。
[0085]
当tf加速度为3m/s2时，初始的换道轨迹不再满足安全要求，然而由于tf加速度过大导致并不存在安全的可行换道轨迹。如图5中(c)所示，hc规划出一条返回原车道的安全轨迹，并在返回原车道后，第三次进行轨迹规划减速来调整车速，继续跟随pc。如图7所示，hc的纵向和横向加速度都不超过0.1g，满足舒适性要求。
[0086]
本领域的技术人员应该了解,本技术不受上述实例的限制,上述实例和说明书中描述的只是说明本发明的原理,在不脱离本发明精神和范围的前提下本发明还会有各种变化和改进,这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等同物界定。