CN108387242B - 自动驾驶换道准备和执行一体化轨迹规划方法 - Google Patents

Abstract

本发明公开了一种自动驾驶换道准备和执行一体化轨迹规划方法，包括建立换道车辆在换道准备过程的运动轨迹模型；引入三次多项式建立换道车辆在换道执行过程的轨迹方程；建立换道准备和换道执行过程的安全距离模型和安全速度模型；建立一体化轨迹优化模型，对换道准备和执行过程中的时间和加速度进行优化，得到满足安全、效率和舒适性的最优轨迹。本发明首先根据车辆运动特性建立一体化换道轨迹集，再用安全约束挑选出满足换道安全的安全换道轨迹集，最后再考虑换道舒适性和效率对轨迹进行优化，得到满足安全、舒适和效率的最优一体化轨迹。因而本发明规划出的最优换道轨迹服务水平较高，能够保证乘客具有较高的舒适度体验与效率体验。

Description

自动驾驶换道准备和执行一体化轨迹规划方法

技术领域

本发明涉及一种自动驾驶换道准备和执行一体化轨迹规划方法。

背景技术

近年来，自动驾驶受到了越来越多的关注，被认为是缓解交通拥堵，减少交通事故和环境污染的重要技术。许多企业和高校也已经开始了研究并完成了大量的车辆测试，但是测试的环境相对简单，无法保证车辆在真实和复杂的交通环境中的安全性，以特斯拉为例，车辆在2016年就发生了7起交通事故，其中还包括两起死亡事故。可见，现有的自动驾驶技术还需要进一步的研究和完善。车辆换道是车辆驾驶的基本操作之一，也是造成交通事故的重要原因，根据中国公路交通安全管理局报告显示，60％以上的高速公路交通事故与车辆换道有关。自动驾驶车辆换道是自动驾驶研究的重点，是有待解决的技术难点。2016年2月谷歌自动驾驶车辆在左转时与公交车碰撞，造成此次事故的原因正是自动驾驶车辆换道技术不完善造成的，可见，自动驾驶车辆换道技术还不成熟，还需要进一步的研究和完善。

现有自动车辆换道研究主要包括换道决策和换道执行两个阶段，以换道决策、轨迹规划和轨迹跟踪控制为研究重点。研究人员在研究车辆换道问题时往往将换道决策和换道执行分开研究，忽略了决策和执行过程之间的联系。此外，极少有关于自动驾驶车辆换道准备的研究，忽略换道准备的重要作用。

现有的自动驾驶车辆换道决策研究主要包括了换道意图的产生、换道条件的判断和目标车道的选择三部分，现有车辆换道决策方法主要包括基于人工智能和效用函数。Chen等人研究复杂环境下的自动驾驶车辆换道决策问题，认为车辆受到前车速度限制为追求行驶效率而产生换道意图，建立多属性规则决策模型，生成满足安全和高效的换道决策，但是未解决换道加速度和速度的选择问题。Du等人建立横向加速度、速度分布和换道执行时间的约束规则，利用混合逻辑动态系统进行Cplex求解，得到安全、稳定和高效的自动驾驶换道决策。Talebpour等人认为车辆之间相互作用影响，在车辆通信环境下基于博弈论进行换道决策，Meng等人在Talebpour结构平衡理论基础上，考虑车辆是否值得换道和换道是否安全，建立滚动时域控制的博弈论决策模型，使车辆得到实时最优决策。

基于人工智能的自动驾驶车辆换道决策方法主要包括基于贝叶斯和马尔可夫等方法。Schubert等人使用贝叶斯网络对换道环境进行评估和决策，以降低安全时间(DST)作为换道评估标准，另外，他对所提出的贝叶斯网路进行了更加深入的实际评估，并评估“预期的车道改变效用随着时间的推移而变化”，还说明了随着时间的推移模糊不清的后果。Ulbrich和Maure认为车辆换道子行为是自动驾驶车辆整体结构的重要部分，重点研究车辆换道环境需要评估的内容和如何实现换道，论文利用当前检测信息评估换道是否可行，使用动态贝叶斯网络对收益进行评估，确定车辆跟驰和换道的概率，在确定需要换道后调整车辆速度从而得到最合适的换道间距，以实现最优效率的车辆换道。Wang等人基于贝叶斯网络提出最小风险和误差的决策，Deshpande和Cse则将贝叶斯和决策树方法组合建立强制性换道决策系统，根据车辆的位置、速度、加/减速度等参数，判断车辆换道的间距是否安全。Ulbrich和Maure使用部分连续可观察马尔可夫决策(POMDP)对进行换道决策，将车辆间的相对速度，相对距离和加速度作为输入，评估换道是否可以，并计算车辆换道成功概率进行决策。Brechtel等人在Ulbrich之后，认为周围车辆驾驶行为无法预测，引入奖励函数将换道中不确定的因素归纳为一个全局驱动目标，再利用部分连续可观察马尔可夫决策将换道决策转变为概率问题。

利用效用函数制定自动驾驶车辆换道决策，综合考虑安全性、舒适性和效率进行换道决策。Wei等人基于预测和成本函数算法选择最佳的控制策略，不直接比较策略成本，利用滞后块来切换，同时，左右车道的阈值也不相同，车辆优先选择左转换道或超车。Nilsson和

在双车道单向道路上，基于模型预测控制(Model PredictiveControl，MPC)建立混合逻辑动态系统的换道决策模型，选择合适的目标车道和速度分布，确定车辆是否换道。之后，Nilsson等人在此基础上，进一步解决混合整数规划的局限性，将车辆换道问题分解成是否换道、何时换道及如何换道三个步骤，对车辆换道进行了更加系统全面的研究。

现有自动驾驶换道轨迹规划研究方法主要为几何曲线方法，以及搜索算法和模型预测控制算法。几何曲线具有良好的平滑性，可以模拟自动驾驶车辆换道的轨迹，根据表征轨迹的几何曲线类型不同，细分为多项式曲线，螺纹曲线、正弦函数、B样条曲线、贝塞尔曲线和横向加速度等方法。多项式方法最早由Nelson提出，为了弥补圆弧线段在换道轨迹描述中不连续的问题，提出利用连续曲率线段替换圆弧线段作为车辆换道的轨迹，旨在构建曲率连续的换道轨迹。Zhang等人引用螺纹曲线作为自动驾驶车辆换道轨迹，对变化的换道环境进行实时检测规划。之后，利用动态规划的思想，提出时间三次多项式规划车辆换道的执行轨迹。Sun等人在三次多项式的基础上，结合轨迹的碰撞概率，建立不确定环境下的车辆换道轨迹。此外，Piazzi和Bianco首次应用五次多项式规划车辆对换道轨迹，保证了车辆换道过程中加速度的连续性，Papadimitriou和Tomizuk在Piazzi等人研究上，同样使用五次多项式进行避障换道轨迹规划，但是缺乏对执行过程中潜在的障碍物的研究。Luo等人则在车联网环境下，首次将车辆换道过程中潜在的障碍物考虑在内，根据环境进行实时动态轨迹规划。

Yang等人基于线性偏移和正弦函数规划车辆换道的轨迹，换道轨迹光滑且速度连续。Wang等人在Yang的基础上采用车载一体化的现场数据，用同样的方法规划出满足车辆性能和乘坐舒适性的轨迹。Milam和Li等人将三次B样条曲线作为车辆的换道轨迹，但是车辆换道的动态变化过程中无法合理地确定B样条曲线段数，因此B样条曲线方法不适用于高速、复杂的换道场景。Chen等人针对B样条曲线的不完善，将贝塞尔曲线作为车辆换道执行轨迹，但是没有解决贝塞尔曲线控制点的选取问题。

基于搜索算法的自动驾驶车辆换道轨迹规划，是利用外在障碍物或道路基础设施对换道车辆的影响，应用弹性带法和人工势场法，搜索出安全可靠的换道轨迹。Hilgert等人将弹性带方法应用到车辆紧急制动下的轨迹规划中，弹性带能够满足紧急轨迹的柔性和最小局部曲率变化。Wolf和Burdick首次利用人工势场法规划高速公路环境下的车辆换道轨迹，利用势场表征环境空间，速度势能使车辆以期望的速度移动，求解势场中势能下降最快的方向获得自动驾驶车辆换道轨迹。另外，Kim等人则利用模型预测控制方法规划出车辆换道轨迹，将车辆相对速度和相对位移等参数作为模型输入，利用滚动时域不断优化，得到车辆换道最佳轨迹和期望速度。之后，Nilsson等人为简化轨迹规划问题，将车辆横纵向解耦分别进行轨迹规划，将车辆换道问题转换为车辆横纵向耦合问题。还在分析了模型预测控制模型后，对仿真实验进行了改进，使用沃尔沃V60在测试轨道上执行和检验了轨迹规划模型的稳定性，验证了模型预测控制的实用性。

通过对现有研究的分析，现在文献对自动驾驶车辆换道决策和轨迹规划都有一定的研究，但是极少研究者将这两部分联合起来分析，也缺少对车辆换道准备进行研究。Zhang等人提出驾驶员模型概念和框架，利用模糊理论模拟驾驶员对换道环境进行决策，考虑换道环境利用螺旋曲线规划换道轨迹并确定安全舒适的最佳轨迹。Ulbrich和Maurer在使用动态贝叶斯建立车辆换道状态评估和判断模型中，提到在换道决策阶段对车辆速度进行调整，以获得一个合适的换道间距，使换道效率更高，但是在论文中只提出了理论，没有建立合适的模型进行验证。Nilsson等人论文中通过效用函数实现换道决策，以及使用模型预测控制方法进行轨迹规划。并将换道过程分为三步执行，是否需要换道、选择合适的间隙和时间执行换道、规划横纵向轨迹。另外在换道行为研究领域，Zhu等人提出了换道准备的研究，利用车头时距作为车辆换道准备的判别条件。

综上，现有自动驾驶车辆换道研究往往将换道决策和换道执行分开，忽略了决策和执行过程之间的联系。此外，极少有关于自动驾驶车辆换道准备的研究，忽略换道准备的重要作用。换道准备作为车辆换道的重要过程，很好的将换道决策和执行过程连接起来，并为换道执行提供换道条件，提高自动驾驶车辆换道安全性和适用性范围。

发明内容

为了克服现有技术的上述缺点，本发明提出了一种自动驾驶换道准备和执行一体化轨迹规划方法，首次在自动驾驶领域提出换道准备过程，并分析了换道准备过程中车辆的运动特性，以及换道准备对于换道执行过程的重要作用；本发明首先假设周围车辆速度保持不变，分析了车辆换道准备和执行过程中车辆运动学特性，将车辆换道准备和执行相结合，对于车辆换道执行过程，本发明采用了不依赖时间的多项式轨迹方程来表征换道轨迹曲线，避免了对速度和加速度假设太强的问题；另外，引入了基于反应时间的避撞算法建立车辆安全模型，用以保障换道准备和执行过程的安全性；最后，再对一体化换道轨迹中的效率和舒适性进行优化，得到最优一体化轨迹参数，从而构建了一套满足安全性、效率和舒适性的自动驾驶车辆换道准备执行一体化轨迹规划模型。

本发明解决其技术问题所采用的技术方案是：一种自动驾驶换道准备和执行一体化轨迹规划方法，包括如下步骤：

步骤一、建立换道车辆在换道准备过程的运动轨迹模型；

步骤二、引入三次多项式建立换道车辆在换道执行过程的轨迹方程；

步骤三、建立换道准备和换道执行过程的安全距离模型和安全速度模型；

步骤四、建立一体化轨迹优化模型，对换道准备和执行过程中的时间和加速度进行优化，得到满足安全、效率和舒适性的最优轨迹。

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

1)完整性：本发明首次在自动驾驶研究领域提出换道准备过程，并分析了换道准备在换道执行过程中的重要作用。本发明提出的换道准备和执行一体化轨迹规划方法实现了车辆换道的完整性，换道准备可以很好的连接换道决策和执行过程，保证了自动驾驶车辆换道的完整性。

2)安全性：本发明使用了基于反应时间的避撞算法，保证了车辆在换道准备和执行过程的安全性，换道车辆紧急制动或前车紧急制动的情景下依然可以保证不发生碰撞，最大程度的保障了自动驾驶车辆在换道准备和执行过程中的安全性。

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

附图说明

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

图1为换道准备和执行一体化换道过程示意图；

图2为安全车距计算原理图。

具体实施方式

一种自动驾驶换道准备和执行一体化轨迹规划方法，包括如下步骤：

步骤一、根据换道过程中周围车辆运动状态，确定当前车道前车、目标车道前后车的纵向位置x_i(t)和时间的关系；

步骤二、假设换道车辆在换道准备过程中匀速运动或匀加/减运动，确定换道车辆在换道准备过程的运动轨迹x_n(t)，得到车辆在换道执行点的纵向位置坐标x_n(t₁)，和换道准备时间t₁，以及换道准备过程的加速度a_OP；

步骤三、引入三次多项式对换道执行过程进行轨迹规划，根据车辆换道执行点和换道完成点位置求得轨迹方程；

步骤四、换道车辆在执行过程中匀速运动或匀加/减速运动，根据车辆运动学原理和曲线弧长求解车辆在换道执行过程中的纵向位移x_f；

步骤五、根据换道车辆在换道执行点和换道完成点的速度计算换道车辆在执行过程中的切向加速度

和换道执行时间t₂，从而可以得到换道总时间t_f，其中t_f＝t1+t₂；

步骤六、根据换道车辆的速度和曲率求解车辆在换道过程中的最大法向加速度

根据法向加速度和切向加速度得到换道执行过程中车辆的加速度a_PD，并将这一步带入步骤九中；

步骤七、根据车辆位置和速度建立避撞的安全距离模型，将对车辆位置的安全约束带入到步骤五和步骤六中，保证车辆在换道准备和执行过程的安全性；

步骤八、根据安全距离模型建立车辆换道执行过程的安全速度模型，将对换道车辆速度的安全约束带入到步骤五和步骤六中，确保车辆在换道准备和换道执行过程的安全性；

步骤九、根据第五、六步得到换道轨迹方程，建立一体化轨迹优化模型，对换道准备和执行过程中的换道时间、加速度进行优化，得到满足安全、效率和舒适性的最优轨迹。

以下将结合图1和图2对本发明内容详细描述如下：

一、轨迹规划模块

自动驾驶车辆换道是一个快速的过程，为了确保车辆换道的安全性，假设车辆n在换道准备和执行过程中对周围车辆不产生影响，周围车辆保持匀速行驶。为简化运算，假设换道意图产生时刻t₀＝0。

其中，n表示换道车辆，n-1表示当前车道前车，n-2表示目标车道前车，n+2表示目标车道后车。

(1)换道准备轨迹规划

自动驾驶车辆换道准备作为车辆换道过程的重要组成部分，对车辆换道起到了非常重要的作用。首先，换道准备将换道决策和换道执行过程连接起来，保证了车辆换道过程的完整性，其次，换道准备可以改善车辆换道环境，对于无法直接执行换道的环境，车辆n可以在换道准备阶段对调整车辆的速度和相对位置，使换道环境满足换道需求后再执行换道操作。

首先分析换道准备过程中车辆n的行驶状态，假设换道准备的加速度a_OP保持不变。

上式中，x_n(t₁)为车辆n在P点的纵向坐标位置，v_n(t₀)为车辆n在O点的速度，a_OP表示车辆n换道准备阶段的加速度，v_n(t₁)为车辆n在点P的速度，t₁表示车辆n换道准备的时间。

(2)换道执行轨迹规划

三次多项式的线形与车辆换道执行曲线十分相近，而且三次多项式轨迹既具有曲率均匀变化的特点，又避免了高次多项式轨迹需要引入抽象参数来约束轨迹的麻烦，因此本发明利用三次多项式曲线模拟车辆换道执行轨迹，其表达示如下：

其中，a₀、a₁、a₂、a₃均为待确定的参数，x_n和y_n分别表示换道车辆n在换道执行过程中纵向(即道路长度方向)和横向(即道路宽度方向)的坐标位置。

为保证换道执行轨迹的适用性，车辆n在P点和D点的运动方向与车道线保持平行，设车辆n在P点的速度为v_n(t₁)，位置坐标为(x_n(t₁)，y_n(t₁))。车辆n在D点的速度为v_n(t_f)，位置坐标为(x_n(t_f)，y_n(t_f))。为了简化运算，降低换道车辆在换道执行过程中的计算难度，假定车辆n在P点的位置坐标为(0，0)，换道车辆n在D点位置坐标为(x_f，y_f)。根据换道车辆n在P点和D点位置坐标和横向和纵向坐标间的关系yn′(0)＝0、yn′(xf)＝0，求得换道执行轨迹的待定参数：

将待定参数的值带入到公式(5)中得到车辆n的轨迹方程：

根据车辆n在P点位置坐标和车辆换道执行轨迹，求得车辆n在换道完成点D点的纵坐标x_n(t_f)。

x_n(t_f)＝x_n(t₁)+x_f (8)

在换道执行过程中车辆n的纵向位移x_f，可以根据车辆的运动学原理和曲线弧长求解得出，利用运动学原理，车辆n在换道执行过程中行驶的弧长为：

式中，t₂为车辆n换道执行时长，

为车辆n在换道执行过程的切向加速度，

方向与速度v_n(t)方向一致。

使用矩形区域积分quad方法可计算弧长如下：

根据车辆n的换道轨迹方程公式(9)和轨迹曲线特性公式(10)求出车辆n在换道执行过程中的纵向位移x_f。

车辆n在换道执行过程中匀变速行驶，即执行过程中车辆的切向加速度

保持不变，根据车辆n在P点和D点的速度得到

将车辆n在换道执行过程中的加速度a_PD分解为切向加速度

和法向加速度

与速度方向一致，大小恒定不变，因此，车辆n的法向加速度

越大，车辆n的总加速度a_PD越大。

式中，

为车辆n在换道执行过程中的最大法向加速度，

式中，K为换道轨迹的曲率函数，计算公式如下：

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

将y′和y″的表达式，带入到轨迹的曲率函数K的表达式中，得到曲率K关于x_n的函数(0≤x_n≤x_f，y_f≥0)，其表达式如下：

随着x_n的变化，轨迹曲率先均匀变小至零，然后均匀变大，车辆n在换道执行开始和结束时刻换道轨迹的曲率半径最大。换道执行过程中

大小保持不变，车辆n的速度v_n(t)均匀变化，

为负时，车辆在换道执行起点速度最大，相反

为正时，车辆在换道完成点速度最大，根据公式(13)，车辆n在换道执行起点或换道完成点处法向加速度

最大，因此换道执行过程的总加速度a_PD在换道执行起点或换道完成点处最大。

二、安全模型

(1)安全距离模型

利用Gipps理论建立车辆换道安全规则，保证自动驾驶车辆换道准备和执行过程的安全性。Gipps理论建立的安全距离是指当前车n-1突然紧急刹车时，后车n为了避免与其发生碰撞，必须保持的一个安全间距。安全间距与车辆自身的最大制动力，位置关系和反应时间有关，其中车辆最大减速b为负值，

上式中，

分别为车辆n-1和车辆n的刹车距离，x_n-1(t)、x_n-1(t)分别表示车辆n-1和车辆n在t时刻的纵向位置坐标，v_n-1(t)、v_n(t)分别表示车辆n-1和车辆n在t时刻的速度，b_n-1、b_n分别表示车辆n-1和车辆n的最大减速度，τ为后车n的反应时间。

车辆n在换道准备过程中受到当前车道前车n-1的影响，为保证换道车辆的安全性，换道车辆n和车辆n-1之间的车辆间隙必须满足安全性，车辆紧急制动而不发生碰撞，因此车辆间间隙必须满足最小安全间隙

同理可得到车辆n与目标车道前车n-2的最小安全

上式中，l_n-1，l_n-2分别是车辆n-1、车辆n-2的车身长度，x_n-2(t)为车辆n-2在t时刻的纵向位置坐标，v_n-2(t)表示车辆n-2在t时刻的速度，b_n-2表示车辆n-2的最大减速度。

利用相同的方法，换道车辆n在系统反应时间τ后执行紧急刹车操作，车辆n+2同样在系统反应时间τ后，也执行紧急刹车，因此考虑换道车辆n和目标车道后车n+2的安全间隙

上式中，l_n为车辆n的车身长度，x_n+2(t)表示车辆n+2在t时刻的纵向位置坐标，v_n-2(t)表示车辆n+2在t时刻的速度，b_n+2表示车辆n+2的最大减速度。

自动驾驶车辆n在换道准备和换道执行过程中都必须满足安全距离约束，车辆n在换道准备过程中需要实时考虑车辆n-1的影响，车辆n在P点需要考虑目标车道的安全距离约束。因此车辆n在换道准备阶段的纵向坐标x_n(t)必须满足

车辆n在P点得纵向坐标x_n(t₁)还需要满足

换道车辆n在换道执行过程中的横向位置开始发生位移，换道车辆n在执行过程中需要满足目标车道前后车的安全约束，因此换道车辆n的位置x_n(t_f)必须满足

(2)安全速度模型

根据换道车辆n和车辆n-1、车辆n-2和车辆n+2不发生碰撞的最小距离

和

推算出换道车辆n在车辆换道执行点P的安全速度区间。为了确保换道车辆n在换道执行点P点的安全性，换道车辆n的速度在换道准备整个过程中速度v_n(t)应该小于受到车辆n-1影响的期望安全速度

车辆n在P点开始发生横向位移，因此需要考虑车辆n-2和n+2的影响，根据车辆n和车辆n-2、车辆n-2的最小距离

和

推算出车辆n分别是在车辆n-2和车辆n+2影响下车辆n的期望安全速度

和

为保证车辆换道执行过程的安全性，车辆n在执行换道过程中的速度必须满足

三、一体化轨迹优化

车辆在换道过程中除了满足安全性外，还需要考虑换道过程的效率和舒适性，通过对换道准备和执行两个过程的效率和舒适性的优化得到最优一体化轨迹。本发明用换道完成总时间t_f表示车辆换道的效率，其中t_f＝t₁+t₂，车辆换道的舒适性用换道准备和执行两个过程的加速度a_OP和a_PD中较大的值表示。利用车辆换道效率和舒适性对一体化换道轨迹进行优化，构造一体化车辆换道成本函数J，α为需要标定的舒适性权重参数。α值越大，生成的一体化换道轨迹的舒适性更好，反之轨迹更加追求换道效率：

式中，

表示车辆n在换道准备过程的最大加速度，

表示车辆n在换道执行过程的最大加速度，t_fmax表示车辆n最大的换道完成总时间。

在安全模型(即安全距离模型和安全速度模型)的约束下，根据车辆换道准备和执行的轨迹和一体化轨迹优化模型计算出满足车辆安全、舒适性和效率的参数，即车辆在换道准备和执行过程的加速度和时间。

Claims (4)

1.一种自动驾驶换道准备和执行一体化轨迹规划方法，其特征在于：包括如下步骤：

步骤一、建立换道车辆在换道准备过程的运动轨迹模型：

(1)根据换道过程中周围车辆运动状态，确定当前车道前车、目标车道前车、目标车道后车的纵向位置和时间的关系：

x_i(t)＝x_i(t₀)+v_it

s.t.i＝n-1,n-2,n+2

其中，n表示换道车辆，n-1表示当前车道前车，n-2表示目标车道前车，n+2表示目标车道后车，t₀表示换道意图产生时刻，t₀＝0；

(2)假设换道车辆在换道准备过程中匀速运动或匀加/减运动，根据换道车辆在换道准备过程的运动轨迹x_n(t)，得到车辆n在换道执行点的纵向位置坐标x_n(t₁)、换道准备时间t₁，以及换道准备过程的加速度a_OP：

其中：x_n(t₀)为车辆n在换道意图产生点的纵向位置坐标，v_n(t₀)为车辆n在换道意图产生点的速度，v_n(t₁)为车辆n在换道执行点的速度，t₁表示车辆n换道准备的时间；

步骤二、引入三次多项式建立换道车辆在换道执行过程的轨迹方程：

(1)利用三次多项式曲线模拟车辆换道执行轨迹，得到如下表达示：

其中，a₀、a₁、a₂、a₃均为待确定的参数，x_n和y_n分别表示换道车辆n在换道执行过程中纵向位置坐标和横向位置坐标；

(2)求解得到换道执行轨迹的待定参数：

其中：x_f，y_f分别为换道车辆n在换道完成点的纵向位置坐标和横向位置坐标；

(3)将待定参数的值带入到第(1)步的换道执行轨迹中得到车辆n的轨迹方程：

(4)按如下公式计算车辆n在换道执行过程中行驶的弧长：

式中，t₂为车辆n换道执行时长，

为车辆n在换道执行过程的切向加速度，

方向与v_n(t)方向一致；

(5)使用矩形区域积分quad方法计算弧长L：

(6)利用第(4)步和第(5)步得到的弧长求解得到车辆n在换道执行过程中的纵向位移x_f；

(7)按如下公式计算换道执行过程中车辆的切向加速度

其中：v_n(t₁)为车辆n在换道执行点的速度，v_n(t_f)为车辆n在换道完成点的速度，t₂为车辆换道执行时间；

(8)按如下公式计算车辆n在换道执行过程中的加速度a_PD：

其中：

为车辆换道执行过程的切向加速度，与车辆速度方向一致，

大小恒定不变；

为车辆换道执行过程的法向加速度，

K为换道轨迹的曲率函数；

步骤三、建立换道准备和换道执行过程的安全距离模型和安全速度模型；

步骤四、建立一体化轨迹优化模型，对换道准备和执行过程中的时间和加速度进行优化，得到满足安全、效率和舒适性的最优轨迹：

引入舒适性权重参数α构造在安全距离模型和安全速度模型约束下的一体化车辆换道成本函数J：

式中，

表示车辆n在换道准备过程的最大加速度，

表示车辆n在换道执行过程的最大加速度，t_fmax表示车辆n最大的换道完成总时间，t_f表示换道总时间，

表示换道车辆n在换道准备过程中受到车辆n-1影响的期望安全速度，

分别表示换道车辆n在换道执行过程中受到车辆n-2和车辆n+2影响的期望安全速度。

2.根据权利要求1所述的自动驾驶换道准备和执行一体化轨迹规划方法，其特征在于：所述换道轨迹的曲率函数K的表达式为：

其中，0≤x_n≤x_f，y_f≥0。

3.根据权利要求1所述的自动驾驶换道准备和执行一体化轨迹规划方法，其特征在于：步骤三所述建立换道准备和换道执行过程的安全距离模型的方法为：

(1)按如下公式计算车辆n-1和车辆n的刹车距离

其中：x_n-1(t)、x_n(t)分别表示车辆n-1和车辆n在t时刻的纵向位置坐标，v_n-1(t)、v_n(t)分别表示车辆n-1和车辆n在t时刻的速度，b_n-1、b_n分别表示车辆n-1和车辆n的最大减速度，τ为后车n的反应时间；

(2)按如下公式计算换道车辆n与当前车道前车n-1和与目标车道前车n-2的最小安全

其中，l_n-1，l_n-2分别是车辆n-1、车辆n-2的车身长度，x_n-2(t)为车辆n-2在t时刻的纵向位置坐标，v_n-2(t)表示车辆n-2在t时刻的速度，b_n-2表示车辆n-2的最大减速度；

(3)计算换道车辆n和目标车道后车n+2的安全间隙

其中，l_n为车辆n的车身长度，x_n+2(t)表示车辆n+2在t时刻的纵向位置坐标，v_n+2(t)表示车辆n+2在t时刻的速度，b_n+2表示车辆n+2的最大减速度。

4.根据权利要求3所述的自动驾驶换道准备和执行一体化轨迹规划方法，其特征在于：步骤三所述建立换道准备和换道执行过程的安全速度模型的方法为：

(1)计算换道车辆n在换道准备过程中受到车辆n-1影响的期望安全速度

车辆在换道准备过程中的v_n(t)小于

(2)计算换道车辆n在换道执行过程中受到车辆n-2影响的期望安全速度

车辆在执行过程中的速度小于

(3)计算换道车辆n在换道执行过程中受到车辆n+2影响的期望安全速度

车辆在执行过程中的速度大于

Images