CN106926844A

CN106926844A - 一种基于实时环境信息的动态自动驾驶换道轨迹规划方法

Info

Publication number: CN106926844A
Application number: CN201710186392.9A
Authority: CN
Inventors: 杨达; 郑施雨; 文成; 吴丹红
Original assignee: Southwest Jiaotong University
Current assignee: Southwest Jiaotong University
Priority date: 2017-03-27
Filing date: 2017-03-27
Publication date: 2017-07-07
Anticipated expiration: 2037-03-27
Also published as: CN106926844B

Abstract

本发明公开了一种基于实时环境信息的动态自动驾驶换道轨迹规划方法，计算出侧翻极限轨迹纵向坐标、最优轨迹以及避撞轨迹终点安全区间；然后通过比较侧翻极限轨迹纵向坐标与安全区间的关系、并进一步比较最优轨迹纵向坐标与安全区间的位置关系进行换道决策。本发明采用了不依赖时间的多项式轨迹方程来表征换道轨迹曲线，避免了对速度和加速度假设太强的问题，并引入了基于反应时间的避撞算法以及防侧翻算法来保障换道的安全性，然后在安全轨迹簇中通过实时调整速度来确定最优轨迹，最后通过轨迹生成模块输出一条实时动态的最终换道轨迹来指导自动驾驶车辆完成换道行为，从而构建了一套完整的动态自动驾驶车辆换道轨迹规划模型。

Description

一种基于实时环境信息的动态自动驾驶换道轨迹规划方法

技术领域

本发明涉及一种基于实时环境信息的动态自动驾驶换道轨迹规划方法。

背景技术

自动驾驶技术被认为是未来解决交通拥堵问题，提高交通安全性的重要手段，其对社会、驾驶员和行人均有益处。即使受其他车辆交通事故发生率的干扰，自动驾驶车辆市场份额的高速增长也会使整体交通事故发生率稳步下降。并且自动驾驶车辆的行驶模式可以更加节能高效，因此交通拥堵及对空气的污染将得以减弱。而近年来，自动驾驶技术发展势头迅猛，一方面以特斯拉、奔驰、沃尔沃为代表的各大汽车厂商联合汽车零部件厂商，并加强跨行业合作以开发自动驾驶技术，阶段性地推出驾驶辅助车辆，半自动化车辆等技术产品。另一方面，以谷歌为代表的IT科技企业结合信息技术另辟蹊径，探寻更为智能的自动驾驶生态系统，蕴含有颠覆传统汽车企业的创造力。

然而，近年来发生的一系列自动驾驶交通安全事故，比如2016年3月谷歌无人驾驶车辆与一辆公交巴士发生轻微碰擦，事故发生时该自动驾驶车辆正打算向右侧车道换道，在换道过程中没能对实时环境信息做出动态响应，错误地认为从后方接近的巴士会减速避让，最终低速撞上了巴士的侧面。从中可以看出，虽然自动驾驶技术发展迅速，但目前换道行为的自动驾驶在技术上仍然有很大的不足，安全性保障并不完善，而换道轨迹规划恰恰是自动驾驶中的核心问题之一，包括换道轨迹曲线的选择，最优轨迹的权衡(舒适性、便捷性、安全性)以及轨迹的可跟踪性等问题。因此只有构建了完备的换道轨迹规划模型，才能最大程度上地减少自动驾驶交通事故的发生，使自动驾驶车辆的安全性得到保障。图1即为车辆换道轨迹示意图，换道车辆SV在执行换道过程中会受到目标车道后车LV_t和目标车道前车PV_t的影响。

虽然有关自动驾驶的研究近几年受到了广泛的关注，但是大部分只关注了自动驾驶车辆的跟驰行为，而针对自动驾驶车辆换道行为的轨迹规划研究较少。现有的研究中，主要以几何曲线法为主，此外还有人工势场法，以及基于自动智能体的方法。

在几何曲线法中，根据用以表征换道轨迹的几何曲线类型的不同，又细分为多项式轨迹，圆轨迹，正余弦函数轨迹，螺旋线轨迹，B样条曲线轨迹等方法，此外，有些学者通过设计换道过程的速度或加速度曲线来表征换道轨迹，例如正反梯形加速度曲线。多项式方法最早由Nelson提出，旨在构建曲率连续的(AGVs)自动引导车辆轨迹，其提出了极坐标多项式轨迹和笛卡尔坐标多项式并证明多项式方法能提高轨迹跟踪的准确性。Piazzi和Bianco提出的基于五次多项式的轨迹规划方法，运用多项式参数化曲线进行轨迹生成，轨迹规划曲线的平坦性保证了自动驾驶车辆轨迹生成的可执行性，通过调节参数可表征不同几何线性曲线轨迹，然而文中个别参数实际物理意义不明确，无法方便地应用于实时控制。Papadimitriou和Tomizuka采用五次多项式来表征车道变换轨迹，把障碍物简化处理为圆形并考虑了动态约束，但这个策略只能对换道开始时刻障碍物进行处理，而不能解决在换道期间出现的障碍物。Chu等研究了弯曲路段环境下的局部轨迹规划，使用不依赖时间的三次多项式曲线来表征换道轨迹，再通过轨迹计算曲率与转向角进而实现控制，并考虑了舒适性，效率和安全性来实现避障。Shim等提出了六次多项式轨迹规划方法，创新性地引入车辆航向角与转向角作为边界条件，以待定轨迹方程。并采用(MPC)模型预测控制方法来实现轨迹跟踪。除此之外，Chen等提出利用二次贝塞尔曲线进行路径规划，得到的换道轨迹具有连续的曲率半径，但该方法未涉及车辆的碰撞检测，同时未解决存在障碍物情况下贝赛尔曲线控制点的选取问题。之后Milam采用了三次B样条曲线进行换道轨迹生成，但车辆行为动态变化时无法合理地确定B样条曲线段数，并且生成的换道轨迹在车辆实际换道过程中其横向最大加速度不易控制。Ren等提出了一种基于梯形加速度曲线的规划方法，导出参考航偏角、航偏率和偏航角加速度，然后设计偏航率跟踪器，通过应用非模态滑动模式技术实现车道改变操纵但是只考虑了横向加速度满足正反梯形曲线，而无法实现纵向速度的实时调整。Chee和Tomizuka对比研究了四种不同的期望轨迹，圆轨迹、余弦函数轨迹、多项式轨迹、梯形加速度轨迹与两种轨迹跟踪算法并选择了梯形加速度轨迹作为虚拟期望轨迹，并给出滑模控制器算法从而提高系统的稳定性，然而他们孤立地研究了换道车辆，假设周围没有任何其他车辆对换道过程进行影响。Sledge and Marshek基于选定的标准比较了几种常用的候选车道变换轨迹，并转化为边界条件约束下的最优化问题来解决，把最大速度作为额外的鉴别指标，比较结果显示多项式曲线在两个最优轨迹函数中形式较为简化。Zhang等使用了正弦曲线、正反梯形加速度曲线、螺旋线来表达换道轨迹，用初始点、终止点状态以及其他约束条件(如碰撞规避)来确定参数选择的范围，最后根据优化目标确定参数的值并在中加入了考虑驾驶舒适性与效率的成本函数来进行轨迹优化，并使用时间依赖的三次多项式方程来表征换道几何曲线。

此外，Wolf和Burdick首次提出了高速公路环境下的车辆换道的人工势场法，用势场来表征环境空间，通过求解势场中势能下降最快的方向来规划换道轨迹。其势场函数考虑了车道线对车辆的吸引力、道路边缘对车辆的斥力作用、车辆障碍物对本车的斥力作用、当前道路期望行驶车速对当前行驶车速的引力作用。一方面根据环境信息决定是否换道超过缓慢的车辆障碍物，另一方面考虑到了车速依赖性和偏好。但可能陷入局部最小点，无法稳定地输出最优轨迹，并且在高速环境下由于搜索空间规模急剧增大，势场法效率会大幅度下降。同时存在一个很强的假设，认为换道车辆速度仅与车道限速有关，而不受周围车辆的影响，这与真实不符。但是当前的研究存在着一些不足之处。第一，假设在换道过程中周围车辆的速度都不发生变化，这与真实的驾驶环境不符。第二、当前的模型都没有考虑在换道过程中换道车辆根据周围车辆状态的变化进行实时响应，实时对速度进行动态调整，这样这些模型在真实交通环境中可能失效。第三，在安全性方面，上述研究认为只要换道完成时，换道车辆和目标车道车辆不发生碰撞即可，而不需要保持一个应对紧急事件发生时的安全间距，这种方法事实上是不能真正地保证换道的安全性的。

鉴于此，Luo等提出了基于车联网技术的动态换道轨迹规划模型，采用了安全距离模型作为避撞机制，其中考虑了换道车辆与前后车的速度差与加速度差对安全距离的影响，并构建了基于时间的多项式轨迹来表征换道轨迹，并通过成本函数权衡换道过程中舒适性与效率从而得到最优轨迹，能够感知周围环境车辆行驶信息的实时变化并做出响应，第一次真正意义上实现了动态换道轨迹规划，实现了换道过程中换道车辆对环境信息的实时捕捉并做出相应的响应。然而，该模型也存在着一定的不足之处。首先，该模型没有考虑系统的反应时间，而反应时间对驾驶安全及系统稳定性分析有很重要的影响，不能轻易忽略；其次，由于采用了时间依赖的多项式轨迹方程，所以论文假设换道起点与终点车辆纵向加速度为零，并且假设换道终点速度为已知(论文设定为目标车道平均车速)，这些假设都与真实情况不符。

发明内容

为了克服现有技术的上述缺点，本发明提出了一种基于实时环境信息的动态自动驾驶换道轨迹规划方法，采用了不依赖时间的多项式轨迹方程来表征换道轨迹曲线，避免了对速度和加速度假设太强的问题，并引入了基于反应时间的避撞算法以及防侧翻算法来保障换道的安全性，然后在安全轨迹簇中通过实时调整速度来确定最优轨迹，最后通过轨迹生成模块输出一条实时动态的最终换道轨迹，作为自动驾驶车辆的换道轨迹，从而构建了一套完整的动态车辆换道轨迹规划模型。

本发明解决其技术问题所采用的技术方案是：一种基于实时环境信息的动态自动驾驶换道轨迹规划方法，包括如下步骤：

步骤一、根据上一时间步长的车辆速度v_n(t-τ)和舒适性权重参数ω_op，计算出与之对应的侧翻极限轨迹纵向坐标最优轨迹以及避撞轨迹终点安全区间；

步骤二、比较侧翻极限轨迹纵向坐标与安全区间的关系：

(1)当位于安全区间的左侧时，则安全区间保持不变；然后进入步骤三；

(2)当位于安全区间内时，则调整安全区间的下限为然后进入步骤六；

(3)当位于安全区间右侧时，则进入步骤四；

步骤三、比较最优轨迹纵向坐标与安全区间的位置关系：

(1)若大于安全区间的上界，则进入步骤四；

(2)若小于安全区间下界，则进入步骤五；

(3)若在安全区间内，则进入步骤七；

步骤四、首先减速，保持当前舒适性权重参数，使最优轨迹向安全区间方向靠拢，直至最优轨迹成为安全区间上界轨迹；若减速无法达到目标时，则减小舒适性权重参数，随后再调整速度以移动最优轨迹至安全区间上界；然后进入步骤七；

步骤五、首先加速，保持当前舒适性权重参数，使最优轨迹向安全区间方向靠拢，直至最优轨迹成为安全区间下界轨迹；若加速无法达到目标时，则增大舒适性权重参数，随后再调整速度以移动最优轨迹至安全区间下界；然后进入步骤七；

步骤六、比较最优轨迹纵向坐标与安全区间的位置关系：

(1)若大于安全区间上界，则进入步骤四；

(2)若在安全区间中，则进入步骤七；

步骤七、采用当前速度和轨迹执行换道；

步骤八、换道车辆与目标车道的距离是否小于设定的距离：

(1)如果否，则返回步骤一；

(2)如果是，则换道结束。

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

1)动态实时性：本发明中自动驾驶车辆在换道过程中能够对环境信息的实时变化做出动态响应，即通过对每次规划步长中生成的安全速度与最优轨迹进行实时调整来不断适应新的环境信息，并决定采用新的最优轨迹继续换道还是中止换道返回原车道。

2)安全性：本发明使用了基于反应时间的纵向避撞算法与基于最大侧向加速度的防侧翻算法，从而在纵向上即使在前车紧急制动的情景下依然可以保证不发生碰撞，在横向上不发生车辆侧翻。最大程度上保障了换道过程中自动驾驶车辆的安全性。

3)最优性：本发明采用了考虑舒适性与效率的最优轨迹算法来生成最优轨迹，再用安全性约束来调整最优轨迹，使决策出的最终换道轨迹在安全性前提下达到最优。因而本发明规划出的最优换道轨迹服务水平较高，能够保证乘客具有较高的舒适度体验与效率体验。

附图说明

本发明将通过例子并参照附图的方式说明，其中：

图1为换道过程示意图；

图2为车辆换道轨迹示意图；

图3为安全车距计算原理图；

图4为轨迹终点安全区间求解示意图；

图5为轨迹规划原理框架图；

图6为换道起点确定示意图；

图7为多步长轨迹生成示意图，其中：(a)为五次轨迹规划过程；(b)为五次轨迹规划后形成的一条完整的换道轨迹。

具体实施方式

一种基于实时环境信息的动态自动驾驶换道轨迹规划方法，包括如下内容：

一、轨迹规划模块

轨迹规划模块包括四个部分：最优轨迹算法、防侧翻算法、避撞算法、以及轨迹决策。其中最优轨迹算法用以计算给定速度和舒适性权重参数条件下的最优轨迹，防侧翻算法用以确定不发生侧翻轨迹簇的左边界，避撞算法用以确定轨迹终点安全区间，而在轨迹决策中则通过调整速度和舒适性权重参数来使最优轨迹满足安全性约束条件。

(1)最优轨迹算法

自动驾驶车辆在换道时进行轨迹规划，每一规划步长会根据实时环境信息寻找一条对应的最优轨迹。本文利用三次多项式曲线来模拟车辆换道过程中的行驶轨迹，三次多项式的线形和车辆换道曲线十分相近，而且三次多项式轨迹既具有曲率均匀变化的特点，又避免了高次多项式轨迹需要引入抽象参数来约束轨迹的麻烦，其表达式如下：

其中，a₀、a₁、a₂、a₃均为待确定的参数，x_n为纵向上车辆n的位置，y_n为横向上车辆n的位置。在换道过程中，车辆在每一个步长都会规划出一条新的换道轨迹，该轨迹以当前位置为起点，以目标车道中线上的某一位置为终点，在终点处车辆的运动方向与车道平行。本文中我们使用移动坐标系，将每一步长的起点位置依次定义为(0,0)点，终点位置定义为(x_f,y_f)，每一步长起点的车辆航向角为θ_i，终点的车辆航向角为0，则有：

y′_n(0)＝tanθ_i (2)

y′_n(x_f)＝0 (3)

将以上车辆换道的起点坐标(0,0)和终点坐标(x_f,y_f)，以及式(2)和(3)代入轨迹方程(1)得到轨迹方程中各参数的表达式如下：

a₁＝tanθ_i (4)

a₀＝0 (5)

将式(4)-(7)代入到轨迹方程(1)得到：

在式(8)中θ_i为规划步长起点的航向角，y_f为终点横向坐标，均为已知量。所以，轨迹方程由换道轨迹终点纵向坐标x_f唯一确定。而在真实换道过程中，驾驶员在换道的过程中总是期望以更短的时间完成换道，而且尽量保证换道过程的舒适性最大。但是，这两个因素是相互矛盾的，驾驶员需要在换道过程中对这两个因素需进行权衡考虑。因此，本文构造了一个换道成本函数来表达驾驶员在换道中对效率和舒适性的考虑，在该成本函数中用侧向最大加速度代表舒适度，侧向最大加速度越大，说明该换道轨迹舒适度越差，由于换道轨迹的最大侧向加速度出现在轨迹终点，故轨迹终点侧向加速度即为侧向最大加速度，另一方面采用换道终点纵向坐标x_f代表效率，x_f越大，说明该换道轨迹的效率越低。为了使两个因素在一个数量级上进行评估，本文对它们进行归一化处理，具体表达式如下：

式中，J为成本函数期望值，指车辆侧翻对应的临界侧向加速度，指的是换道轨迹的最大纵向距离经验值，ω为需要标定的舒适性权重参数。

成本函数J在终点处的侧向加速度可由下式计算：

式中，K(·)为换道轨迹的曲率函数，u_n为车辆速度。计算公式如下：

其中，y′和y″分别为换道轨迹方程(8)的一阶和二阶导数，它们的表达式如下：

将式(12)和(13)代入到式(11)中得出曲率K关于x_n的函数(0≤x_n≤x_f)，其表达式如下：

随着x_n的变化，轨迹曲率先均匀变小至零，然后均匀变大，所以多项式换道曲线中曲率最大点出现在换道曲线的终点处。因而式(10)为：

再将式(15)代入到成本函数(9)中，得到成本函数J的最终表达式，如下：

从式子(16)可以看出，成本函数J是关于x_f的一元函数，J取到最小值时相应的x_f即为该步长最优轨迹终点的纵向位置

(2)防侧翻算法

车辆在曲线运动时加速度可分解为侧向加速度a_s与轴向加速度a_f，其中侧向加速度过大会导致车辆侧翻，使换道安全性无法得到保障，因而需要利用一个防侧翻算法计算临界侧翻情景下的最大侧向加速度所对应的最优轨迹，作为最优轨迹簇的临界值。安全轨迹的纵向位移x_f均应大于该侧翻临界轨迹的纵向位移车辆模型见图2所示，其中v_n(t)为车辆SV在t时刻的纵向分速度，θ为车辆的航向角，也就是车辆运动方向与x坐标轴的夹角。R为车辆质心的运动的曲率半径。

首先计算车辆以最大侧向加速度与当前车速行驶时所能采用的最大轨迹曲率K(x_f)为其中u_n(t)为车辆SV在t时刻的总速度。同时K(x_f)又可以由换道轨迹的终点坐标(x_f,y_f)确定，公式如下：

故可以由来计算侧翻临界轨迹的终点纵向坐标x_f。令Q＝1/x_f，上式变形为，

由于在整个换道过程中θ_i取值范围在(0,π/2)，则-6y_fQ²+2tanθ_iQ<0，故上式展开为，

考虑到该关于Q的二次函数的Δ>0，故由二次函数求根公式得出：

取其正根，则防侧翻安全区间为其中为：

从而得到了理论上换道车辆不侧翻条件下的最小换道轨迹纵向位移只要换道轨迹的纵向位移大于则不会发生侧翻。

(3)避撞算法

为了防止紧急制动场景下换道车辆SV与目标车道前后车PV_t、LV_t的碰撞，本算法使用了经典Gipps模型来计算车辆SV在当前速度下应与车辆PV_t、车辆LV_t保持的安全行车间隙，从而确定换道轨迹终点的安全区间。其原理如图3所示。

图3中，假设车辆PV_t(n-2)在t-τ时刻开始制动并在t_F时刻停止，车辆SV(n)经过一个反应时间τ后在t时刻继而开始制动。为了不与车辆PV_t发生碰撞，车辆SV至少应与车辆PV_t保持的安全距离记为s_f，其可由车辆SV与车辆PV_t的位移做差得到，见下式(22)。与之类似，可通过式子(23)计算得到车辆LV_t至少应与车辆SV保持的安全距离s_r：

上式中，v_n(t-τ)为车辆SV在t-τ时刻的纵向速度，v_n+2(t-τ)表示t-τ时刻车辆LV_t的纵向速度，v_n-2(t-τ)表示车辆PV_t在t-τ时刻的纵向速度，b_n为车辆SV的制动最大减速度，b_n-2为车辆PV_t的制动最大减速度，b_n+2为车辆LV_t的制动最大减速度。

公式(22)和(23)中v_n(t-τ)可由图3中总速度与纵向分速度的固有三角关系计算得到，

v_n(t-τ)＝u_n(t-τ)·cos(θ(t-τ)) (24)

得到了与前后车的安全行驶间距后，为了得到换道轨迹终点的安全位置，还需要知道换道完成时刻目标车道前后车的预期位置信息，可通过下式计算得到(此处为了计算换道完成时刻t_F时刻车辆的预期位置，假设在t_a中车辆PV_t与车辆LV_t也执行匀速率运动)：

其中表示车辆LV_t在换道完成时的预期位置，表示PV_t车在换道完成时预期位置，x_n+2(t-τ)表示t-τ时刻车辆LV_t的位置。x_n-2(t-τ)表示t-τ时刻车辆PV_t的位置。t_a表示车辆SV从t-τ时刻以u_n(t)行驶至换道终点所需要的时间，计算公式如下：

由以上信息可计算出以车辆SV车头处为参考系的避撞轨迹终点安全区间为原理如图4所示，其中l_n-2表示车辆PV_t的车长。表示SV在t时刻的纵向位置。虚线车辆表示换道开始时刻各车辆的位置。只要换道轨迹终点落在该安全区间中，便不会与目标车道前后车发生碰撞。

(4)轨迹决策

在最优轨迹算法中，轨迹寻优过程是基于当前的车辆速度以及舒适度权重参数的，当其变化时最优轨迹随之发生变化。而轨迹决策的核心思路就在于通过调整速度与舒适度权重参数，从而使输出的最优轨迹满足防侧翻算法和避撞算法所确定的轨迹安全性约束。即令最优轨迹的终点落在安全区间内。首先我们设定当前步长速度v_n(t)与当前步长舒适度权重参数ω(t)的调整范围，设车辆的最大轴向加速度为a，则可由上一规划步长的速度确定当前步长速度的取值范围，即(v_n(t-τ)-aτ,v_n(t-τ)+aτ)，ω(t)取值范围设为(0,1)。两个参数的初始值分别为v_n(t-τ)和ω_op。ω_op是经过真实数据标定得到的舒适性权重参数。在轨迹决策算法中，每输入一组v_n(t)与ω(t)，都需要重新计算与之对应的侧翻极限轨迹纵向坐标最优轨迹以及避撞轨迹终点安全区间。

轨迹规划模块的算法原理见图5，算法输入端为上一时间步长的车辆速度v_n(t-τ)和标定得到的ω_op，计算出与之对应的侧翻极限轨迹纵向坐标最优轨迹以及避撞轨迹终点安全区间。然后进行第一层决策，比较侧翻极限轨迹纵向坐标与安全区间的关系，有如下三种情况：

1.当位于安全区间的左侧时，即则安全区间保持不变，仍为此时进行第二层决策，比较最优轨迹纵向坐标与安全区间的位置关系，也有如下三种情况：

(1)若大于安全区间的上界，则优先调整速度(减速)，保持当前舒适性权重参数，使最优轨迹向安全区间方向靠拢，直至最优轨迹成为安全区间上界轨迹，当调整速度无法达到目标时开始调整舒适性权重参数(变小)，再调整速度以移动最优轨迹至安全区间上界。此时的速度即为该步长规划输出的速度，而此时的轨迹即为输出的最优轨迹，该轨迹在安全性保证的同时，能够尽量靠近舒适性权重参数ω_op所确定的最优轨迹。

(2)若小于安全区间下界，则优先调整速度(加速)，保持当前舒适性权重参数，使最优轨迹向安全区间方向靠拢，直至最优轨迹成为安全区间下界轨迹，当调整速度无法达到目标时开始调整舒适性权重参数(变大)，再调整速度以移动最优轨迹至安全区间下界。此时的速度即为该步长规划输出的速度，而此时的轨迹即为输出的最优轨迹。

(3)若在安全区间内，则不需要调整速度和舒适性权重参数。

2.当位于安全区间内时，即此时安全区间改换为此时进行第二层决策，比较最优轨迹纵向坐标与安全区间的位置关系，有如下两种情况：

(1)若大于安全区间上界，则首先调整速度(减速)，保持当前舒适性权重参数，使最优轨迹向安全区间方向靠拢，直至最优轨迹成为安全区间上界轨迹，当调整速度无法达到目标时开始调整舒适性权重参数(变小)，之后再调整速度以移动最优轨迹至安全区间上界。此时的速度即为该步长规划输出的速度，而此时的轨迹即为输出的最优轨迹。

(2)若在安全区间中，则不需要调整速度和舒适性权重参数。

3.当位于安全区间右侧时，即则优先调整速度(减速)，保持当前舒适性权重参数，使最优轨迹向安全区间方向靠拢，直至最优轨迹成为安全区间上界轨迹，当调整速度无法达到目标时开始调整舒适性权重参数(变小)，再调整速度以移动最优轨迹至安全区间上界。此时的速度即为该步长规划输出的速度，而此时的轨迹即为输出的最优轨迹。经过如上的决策，便可以得到当前规划步长中期望的速度与最优轨迹。

此外，在以上算法中，在速度和舒适性权重系数的取值范围内仍然无法使最优轨迹出现在防侧翻安全区域中，则判定为当前交通环境不适合向目标车道换道，于是触发换道中止进程，将原车道指定为新的目标车道，从而重新规划返回原车道的换道轨迹。

二、轨迹生成模块

经过轨迹规划模块的计算可以得到当前规划步长终点的期望车速以及当前步长的期望舒适性权重参数，从而得到该步长的运动轨迹，最终获得当前步长结束时车辆新的位置和航向角。而轨迹生成模块中，上一步长中规划出的车辆位置与航向角将作为下一步长的初始状态进入下一次轨迹规划中。经过多次迭代后，最终能够生成一条完整的车辆换道轨迹。本文认为在每一步长中车辆执行匀变速曲线运动，接下来对具体过程进行介绍。

(1)换道起点确定

如图6所示，假设换道车辆SV在A点产生换道意图，SV在A点的速度为v_n(t_A)，此时需要对当前的交通环境进行判断，分析车辆速度是否满足换道要求，有如下两种情况：

(1)如果车辆SV的速度可以满足换道要求，则使用当前速度开始换道，换道起点为A，车辆SV执行换道轨迹l_A。

(2)如果车辆SV的速度不满足换道要求，则需要在当前车道上保持直线行驶并对速度进行调整，根据轨迹决策算法可以计算得到经过一个步长(反应时间)后车辆应该具有的安全换道速度，记为v_n(t_B)。此时，又有两种可能性，如果经过一个步长车辆到达B时，SV的速度不能从v_n(t_A)调整到安全换道速度v_n(t_B)，则不能进行换道，车辆可能还需要进行多个步长的调整；如果经过一个步长SV的速度可以调整到v_n(t_B)，则在B点使用v_n(t_B)开始换道，此时换道起点为B。在此，假定车辆从A到B是一个匀变速过程，故B点的位置可以使用下式进行计算：

(2)换道过程

在确定了换道的起点以后(以B点为例)，换道车辆在B点进行第二步轨迹规划来确定期望轨迹曲线l_B和经过一个步长后车辆到达C点时应该具有的安全换道速度v_n(t_C)。经过一个步长车辆到达C时，有两种情况：

(1)SV的速度不能从v_n(t_B)调整到安全换道速度v_n(t_C)，则不能继续换道。于是触发换道中止进程，车辆SV将原车道指定为新的目标车道，从而重新规划返回原车道的换道轨迹。

(2)如果经过一个步长SV的速度可以调整到v_n(t_C)，则车辆SV将沿着轨迹l_B行驶至C点(如图6所示)。于是可以计算该步长中车辆SV行驶的弧长L为：

同时，对换道轨迹公式(8)进行曲线积分可计算弧长L如下：

式中，X_C为以B点为坐标原点时，C点的纵向坐标。

令公式(29)与(30)相等可计算出X_C，再带入轨迹方程(8)得到Y_C，从而确定C点位置。接下来计算步长结束时车辆在C点的航向角，对于换道轨迹上的任意一点车辆的航向角θ，它和轨迹方程的一阶导函数(12)有如下关系式：

将x_n＝X_C代入到式(31)中可以得到当前步长结束时车辆在C点的航向角如下：

至此，就可以得到当前步长结束时C点的车辆状态，也就获得了下一步长开始的状态。经过多次迭代后，每一个步长的局部轨迹首尾相接最终到达目标车道中线，形成了一个完整的换道轨迹。图7为某一辆车在换道过程中的轨迹动态调整示意图，经过图7(a)中五次轨迹规划后，形成了图7(b)中一条完整的换道轨迹。

Claims

1.一种基于实时环境信息的动态自动驾驶换道轨迹规划方法，其特征在于：包括如下步骤：

步骤二、比较侧翻极限轨迹纵向坐标与安全区间的关系：

(3)当位于安全区间右侧时，则进入步骤四；

步骤三、比较最优轨迹纵向坐标与安全区间的位置关系：

(1)若大于安全区间的上界，则进入步骤四；

(2)若小于安全区间下界，则进入步骤五；

(3)若在安全区间内，则进入步骤七；

步骤六、比较最优轨迹纵向坐标与安全区间的位置关系：

(1)若大于安全区间上界，则进入步骤四；

(2)若在安全区间中，则进入步骤七；

步骤七、采用当前速度和轨迹执行换道；

步骤八、换道车辆与目标车道的距离是否小于设定的距离：

(1)如果否，则返回步骤一；

(2)如果是，则换道结束。

2.根据权利要求1所述的一种基于实时环境信息的动态自动驾驶换道轨迹规划方法，其特征在于：所述最优轨迹的计算方法为：

第1步、确定轨迹方程：

y_{n} (x_{n}) = x_{n} {tanθ}_{i} + \frac{3 y_{f} - 2 x_{f} {tanθ}_{i}}{x_{f}^{2}} x_{n}^{2} + \frac{x_{f} {tanθ}_{i} - 2 y_{f}}{x_{f}^{2}} x_{n}^{3}

其中，x_n为纵向上车辆n的位置，y_n为横向上车辆n的位置；在换道过程中使用移动坐标系，将每一步长的起点位置定义为(0,0)点，终点位置定义为(x_f,y_f)，每一步长起点的车辆航向角为θ_i；

第2步、构造换道成本函数：

J = ω {(\frac{u_{n}^{2} | \frac{2 x_{f} {tanθ}_{i} - 6 y_{f}}{x_{f}^{2}} |}{a_{s}^{\max}})}^{2} + (1 - ω) \frac{x_{f}}{x_{f}^{\max}}

式中：为车辆侧翻对应的临界侧向加速度，为换道轨迹的最大纵向距离经验值，ω为需要标定的舒适性权重参数，u_n为车辆速度；

第3步、计算出成本函数J取到最小值时相应的x_f值，从而求出该步长最优轨迹终点的纵向位置

3.根据权利要求2所述的一种基于实时环境信息的动态自动驾驶换道轨迹规划方法，其特征在于：所述侧翻极限轨迹纵向坐标的计算方法为：

第1步、计算车辆以最大侧向加速度与当前车速行驶时所能采用的最大轨迹曲率K(x_f)：

K (x_{f}) = | \frac{2 {tanθ}_{i}}{x_{f}} - \frac{6 y_{f}}{x_{f}^{2}} |

第2步、按如下公式计算

x_{f}^{\min} = \frac{12 y_{f}}{2 {tanθ}_{i} + \sqrt{4 {({tanθ}_{i})}^{2} + \frac{24 y_{f} a_{s}^{\max}}{u_{n}^{2} (t)}}} .

4.根据权利要求3所述的一种基于实时环境信息的动态自动驾驶换道轨迹规划方法，其特征在于：所述避撞轨迹终点安全区间的确定方法为：

第1步、计算车辆SV至少应与车辆PV_t保持的安全距离为s_f：

设车辆PV_t(n-2)在t-τ时刻开始制动并在换道完成时刻t_F停止，车辆SV(n)经过一个反应时间τ后在t时刻继而开始制动，则

s_{f} (t) = v_{n} (t - τ) τ + v_{n}^{2} (t - τ) / 2 b_{n} - v_{n - 2}^{2} (t - τ) / 2 b_{n - 2}

其中，v_n(t-τ)为车辆SV在t-τ时刻的纵向速度，v_n-2(t-τ)表示车辆PV_t在t-τ时刻的纵向速度，b_n为车辆SV的制动最大减速度，b_n-2为车辆PV_t的制动最大减速度；

第2步、按如下公式计算车辆LV_t至少应与车辆SV保持的安全距离s_r：

s_{r} (t) = v_{n + 2} (t - τ) τ + v_{n + 2}^{2} (t - τ) / 2 b_{n + 2} - v_{n}^{2} (t - τ) / 2 b_{n}

其中：v_n+2(t-τ)表示t-τ时刻车辆LV_t的纵向速度，b_n+2为车辆LV_t的制动最大减速度；

第3步、计算车辆LV_t在换道完成时刻的预期位置

x_{n + 2}^{F} = v_{n + 2} (t - τ) t_{a} + x_{n + 2} (t - τ)

其中：x_n+2(t-τ)表示t-τ时刻车辆LV_t的位置，t_a为车辆SV从t-τ时刻以u_n(t)行驶至换道终点所需要的时间；

第4步、计算车辆PV_t在换道完成时刻的预期位置

x_{n - 2}^{F} = v_{n - 2} (t - τ) t_{a} + x_{n - 2} (t - τ)

其中：x_n-2(t-τ)表示t-τ时刻车辆PV_t的位置；

第5步、计算出以车辆SV车头处为参考系的避撞轨迹终点安全区间为其中：l_n-2表示车辆PV_t的车长，表示SV在t时刻的纵向位置。

5.根据权利要求4所述的一种基于实时环境信息的动态自动驾驶换道轨迹规划方法，其特征在于：所述v_n(t-τ)按如下公式计算得到：

v_n(t-τ)＝u_n(t-τ)·cos(θ(t-τ))。

6.根据权利要求3所述的一种基于实时环境信息的动态自动驾驶换道轨迹规划方法，其特征在于：车辆SV从t-τ时刻以u_n(t)行驶至换道终点所需要的时间t_a的计算公式如下：

t_{a} = ({&Integral;}_{0}^{x_{f}^{o p t}} \sqrt{1 + y^{' 2}} d x) / u_{n} (t) + τ .