CN111806467B - 一种基于车辆行驶规律的变速动态换道轨迹规划方法 - Google Patents

Abstract

本发明涉及一种基于车辆行驶规律的变速动态换道轨迹规划方法。通过加速度、加加速度控制车辆行驶安全性和舒适性，并将变速规律融合到轨迹规划方法中，采用回旋线^*和圆曲线^*作为运动本原规划换道轨迹，使自动驾驶车辆换道轨迹与自动巡航轨迹一致，再结合车辆最新位置和安全距离实时动态更新换道轨迹，直至自动驾驶车辆完成换道。本发明换道运动本原引入变速规律，提高车辆换道效率；轨迹生成的同时考虑舒适、安全所要求的加速度和加加速度，省去了以往设定优化目标寻找最优轨迹的过程，约束下直接计算最优轨迹，效率显著提高；一定时间间隔后动态迭代重新计算规划轨迹，既实时保证车辆规划轨迹终点位于安全距离外，又有效更正路径跟踪的误差。

Description

一种基于车辆行驶规律的变速动态换道轨迹规划方法

技术领域

本发明涉及一种基于车辆行驶规律的变速动态换道轨迹规划方法。

背景技术

自动驾驶是车辆发展的趋势，DavidGonzález等人指出动态环境下的实时计算是自动驾驶最大挑战，包含两个方面：一、车辆运动规划的高效实时计算；二、快速有效的对动态环境做出响应。

车辆速度和轨迹曲率与车辆运动的横向加速度密切相关，而横向加速度直接与车辆的安全性、舒适性和稳定性有关(Chuetal.,2012)。车辆运动规划是指生成起点与终点的几何路径，并给出车辆沿该路径的速度规划，常用的有轨迹生成法、基于采样的算法和基于搜索的算法。轨迹生成法包括直接构造法(Werlingetal.,2010)和路径-速度分解法(Phametal.,2017)，分解法使用几何曲线生成路径对速度规划和碰撞检查有利，求解速度快、效率高，满足自动驾驶车辆路径规划的实时性要求，应用相对成熟。基于采样的算法需对状态空间均匀随机采样构建连通图，典型算法有概率路图法(PRM)(Kavraki et al.,1998)和快速随机扩展树法(RRT)(LaValle and KuffnerJr,2001)，其主要缺陷是缺少准确的距离度量，相关算法效率有待验证。基于搜索的规划算法是将状态空间通过一定的方式构成离散网络，再利用搜索算法搜索可行解或最优解，典型算法有状态格子(Pivtoraikoetal.,2009)、Dijkstra算法(Dijkstra,1959)和A*搜索算法(Hartetal.,1968)，这类算法具有解析完备性，但需处理大量网格，效率低下。

基于完备性设定优化目标寻找最优轨迹的运动规划，需要在众多解中寻找最优解，本身就是一种效率低下的方法，不易用于高速行驶的车辆，本发明从应用相对成熟的路径-速度分解法出发，通过两个方面实现车辆运动规划的实时计算。

一、舍弃搜索最优解的方法，应用满足车辆动力学约束的几何曲线构造最优或近优曲线，虽然这种简单的解决方案需要以降低解的完备性为代价，但其以车辆运行安全、舒适为约束，以最小时间驾驶和低磨损为目标抵消大部分不合适的解，应用于高速运动状态下自动驾驶车辆是可行的且有益的。

二、改进路径-速度分解法，路径规划是自主移动车辆的一项基本任务，已在文献中广泛研究，但现有的大部分几何路径生成以匀速运动为前提，提供了一系列路径点，而车辆实际运行速度是变化的，这样规划出来的路径并不是最优的，并且加速度突然变化会导致控制跟踪错误和驾驶不舒适，往往是不可行的。本发明在轨迹生成的同时考虑车辆变速规律，结合车辆行驶规律生成轨迹不仅满足变速状态下加速度和加加速度的要求，保证车辆运行的安全、舒适，同时保证了生成轨迹在一定约束条件下时间最优。

动态环境包括车辆自身状态不确定性和周围环境带来的不确定性(Xu et al.,2014)。车辆自身状态不确定性包括感知、定位和控制方面的不确定性，例如，如果车辆使用低精度的GPS定位，而规划算法没有考虑到这一点，会给车辆行驶带来危险，使自动驾驶汽车可能会变得不安全。除了与自动驾驶车辆本身相关的不确定性，自动驾驶车辆系统还需要解决其他交通参与者带来的不确定性，例如：一辆自动驾驶汽车进行换道，如果规划算法能够预测前方车辆的运动，那么自动驾驶车辆可执行安全平稳的轨迹，当前方车辆突然刹车，自动驾驶车辆需及时改变轨迹或速度避免撞向前方车辆。因此，为了提高自动驾驶车辆系统的安全性，车辆规划算法需要同时考虑这两种不确定性。

自动驾驶车辆在动态环境下应及时调整自己的规划轨迹和速度，适应环境变化，本发明通过在一定时间间隔内(小于人的反应时间)重复迭代计算轨迹、速度，避免车辆自身状态不确定性和周围环境不确定性带来的安全隐患，时间间隔越小，对环境的适应能力就越强，根据动态环境实时更新轨迹、速度也更符合人类驾驶员的驾驶习惯。

发明内容

本发明的目的在于克服现有技术上的不足，对速度-时间分解法这一基本方法进行改进，提供一种基于车辆行驶规律的变速动态换道轨迹规划方法，在保证自动驾驶车辆换道轨迹与自动巡航轨迹一致的前提下，给定自动驾驶车辆初始姿态和目标姿态，将变速规律融合到轨迹规划方法中，通过加速度、加加速度控制车辆行驶安全性和舒适性，采用回旋线^*和圆曲线^*作为运动本原规划换道轨迹，获得时间最优或近优的车辆换道轨迹，使车辆高效完成换道，再结合车辆最新位置和安全距离实时动态更新换道轨迹，直至自动驾驶车辆完成换道。

为实现上述目的，本发明的技术方案是：一种基于车辆行驶规律的变速动态换道轨迹规划方法，包括如下步骤：

步骤S1、根据车辆周围实时环境信息，确定车辆换道或者车道保持，若换道执行步骤S2；

步骤S2、确定换道程序迭代间隔；

步骤S3、初始化换道纵、横向加速度和纵向加加速度，设自动驾驶车辆初始状态和目标状态，使用变速状态下的变异圆曲线本原、变异回旋线本原生成轨迹，并计算避免碰撞的安全距离；

步骤S4、轨迹终点位于安全距离内，调整换道纵、横向加速度和纵向加加速度值，进入步骤S3；

步骤S5、轨迹终点位于安全距离外，判断是否完成换道，若否进入步骤S3，若是进入车道保持程序。

在本发明一实施例中，车辆换道时，变速换道速度曲线生成方式为：

确定一个加加速度函数j(t)，使加速度a(t)函数在最短时间内达到a_max，对于加速换道速度曲线，将速度曲线划分为三个运动间隔：时间间隔[0，t_c1]、[t_c1，t_c2]和[t_c2，t_f](与之对应车辆换道弧长[s_o，s_c1]、[s_c1，s_c2]和[s_c2，s_f]，速度间隔[v_o，v_c1]、[v_c1，v_c2]和[v_c2，v_f]，纵向加速度间隔[a_o，a_c1]、[a_c1，a_c2]和[a_c2，a_f]，a_c1，a_c2即车辆所能达到的最大加速度a_max)，其中，[0，t_c1]车辆在最大纵向加加速度j_max约束下，纵向加速度从a₀增加到a_max，速度v_o加速到v_c1；[t_c1，t_c2]车辆在最大纵向加速度a_max约束下，从v_c1加速到v_c2；[t_c2，t_f]车辆在最大纵向加加速度j_max约束下，纵向加速度从a_max减少到a_f，速度v_c2减速到v_f，与车辆期望目标状态一致；可计算加速换道最优速线与车辆换道轨迹弧长s关系如下：

同理可得减速换道最优速度曲线。

在本发明一实施例中，变速状态下的变异圆曲线本原、变异回旋线本原生成方式为：

变异圆曲线本原表示车辆行驶在在一个圆弧上，受最大离心力γ_max约束，其半径随车辆换道时变速换道速度曲线v(s)沿着轨迹变化，车辆航向角变化如下：

θ(s)＝C+∫γ_max/[v(s)]²ds

其中，C表示常数；

变异回旋线本原表示车辆行驶在在一特殊的回旋曲线上，受前轮转向角速率

约束，其曲率变化率随车辆换道时变速换道速度曲线v(s)沿着轨迹变化，车辆航向角变化如下：

其中，C1、C2表示常数，s为车辆换道轨迹长度，b表示车辆轴距。

使用车辆航向角可计算得变异圆曲线本原、变异回旋线本原上任意一点坐标：

其中，ξ表示积分参数，{x_o，y_o)表示变异圆曲线本原、变异回旋线本原起点。

在本发明一实施例中，使用变速状态下的变异圆曲线本原、变异回旋线本原生成轨迹，即生成变速换道轨迹的方式为：

变速换道轨迹为满足横向加速度约束γ_max和前轮转向角速度变化率约束

约束的时间最优轨迹，通过前变异回旋线本原、后变异回旋线本原连接变异圆曲线本原，结合道路线形生成变速换道轨迹，将变速换道轨迹划分为七个轨迹本原：[s_o，s₁]，[s_i，s_i+1](i＝1，2，3，4，5)，和[s₆，s_f]，可求得最优轨迹函数P{x(s)，y(s)}：

其中，ξ表示积分参数。

在本发明一实施例中，安全距离确定方式为：

假设前车在(t-τ)时刻开始制动，在t′时刻停止，换道车辆经过一个反应时间τ后在t时刻制动，为了不与前车发生碰撞，换道车辆与前车应保持的安全距离是s_q：

其中，v_f，a_f分别表示当前换到车辆规划轨迹终点速度、减速度，v_q，a_q分别表示前车速度、最大制动减速度；

同理，换道车辆与后车应保持的安全距离是s_h：

其中，v_h，a_h分别表示前车速度、最大制动减速度。

在本发明一实施例中，车辆换道时，车辆行驶距离计算方式为：

换道终点坐标(X′，Y′)与换道长度S的关系可表示为下式，其中换道长度S包含直线段[0，S₁]，回旋线段[S₁，S₂]，圆曲线段[S₂，S₃]：

其中，A²表示回旋线变化率，K表示终点曲率，ξ表示积分参数；

求解上式可求得换道长度S。

相较于现有技术，本发明具有以下有益效果：

(1)高效实时性一：舍弃搜索最优解的方法，应用满足车辆动力学约束的几何曲线构造最优或近优曲线，虽然这种简单的解决方案需要以降低解的完备性为代价，但其以车辆运行安全、舒适为约束，以最小时间驾驶和低磨损为目标抵消大部分不合适的解，应用于高速运动状态下自动驾驶车辆是可行的且有益的。

(2)高效实时性二：轨迹生成的同时考虑车辆变速规律，结合车辆行驶规律生成轨迹不仅满足变速状态下加速度和加加速度的要求，保证车辆运行的安全、舒适，同时保证了生成轨迹在一定约束条件下时间最优，适用于高速运动车辆进行换道。

(3)动态性：一定时间间隔后迭代动态计算规划轨迹，即实时保证车辆行驶终点位于安全距离外，又有效更正路径跟踪的误差。

附图说明

图1为换道速度曲线生成方法示意图；

图2为轮式车辆模型；

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

图4为自动驾驶车辆起点状态为P_o{x_o，y_o，s_o，v_o＝100km/h，a_o＝0，j_o＝0}，终点状态为P_f{x_f，y_f，s_f，v_f，a_f＝0，j_f＝0}，A＝600，K＝1/1500m^-1，j_max＝2ms^-3，γ_max＝1.5ms^-2，a＝-1，0，1ms^-2时的轨迹规划相关函数图像。

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

图6为轨迹终点与安全距离关系示意图；

图7为变速动态轨迹规划原理框架图。

具体实施方式

下面结合附图，对本发明的技术方案进行具体说明。

本发明提供了一种基于车辆行驶规律的变速动态换道轨迹规划方法，包括如下步骤：

步骤S1、根据车辆周围实时环境信息，确定车辆换道或者车道保持，若换道执行步骤S2；

步骤S2、确定换道程序迭代间隔；

步骤S3、初始化换道纵、横向加速度和纵向加加速度，设自动驾驶车辆初始状态和目标状态，使用变速状态下的变异圆曲线本原、变异回旋线本原生成轨迹，并计算避免碰撞的安全距离；

步骤S4、轨迹终点位于安全距离内，调整换道纵、横向加速度和纵向加加速度值，进入步骤S3；

步骤S5、轨迹终点位于安全距离外，判断是否完成换道，若否进入步骤S3，若是进入车道保持程序。

以下为本发明的具体实现过程。

本发明一种基于车辆行驶规律的变速动态换道轨迹规划方法，包括如下内容：

一、轨迹规划模块

轨迹规划模块包括三个部分：

换道速度曲线生成方法使用三元多项式的轨迹平滑方法，精确考虑车辆本身的初始和最终运动及约束，保证自动驾驶汽车的安全性和舒适性，同时使运行时间最小化；变异圆曲线本原、变异回旋线本原生成方法将车辆变速规律融入轨迹规划方法，满足变速状态下加速度和加加速度约束，保证车辆运行的安全、舒适，同时保证了生成轨迹在一定约束条件下时间最优；变速轨迹生成方法使用七个车辆运动轨迹本原组合生成和道路线形一致的轨迹。

1、换道速度曲线生成方法

为了保证轨迹的可行性，轨迹和运动曲线不允许发生突变，速度和加速度曲线应是连续的光滑曲线。

P_f(s(t_f)＝s_f)，其轨迹Γ是自动驾驶车辆跟踪的有向曲线，s是Γ弧长参数，表示车辆距离起点位置的距离，t_o表示车辆离开起始位置的时间。给定速度、加速度和加加速度约束，可求满足约束的最小时间曲线v(t)∈P([t_o，t_f])，(Labakhua et al.，2008；Liu，2002；Villagra et al.，2012)。

自动驾驶车辆沿轨迹的最小时间曲线与最大速度曲线是一致的。因此，生成最大速度曲线需要确定一个加加速度函数j(t)，使加速度a(t)函数在最短时间内达到a_max，图1给出了采用三元多项式方法的加减速换道速度曲线生成方法，以加速换道速度曲线为例(减速换道速度曲线类似)，将速度曲线划分为三个运动间隔：时间间隔[0，t_c1]、[t_c1，t_c2]和[t_c2，t_f](与之对应车辆换道弧长[s_o，s_c1]、[s_c1，s_c2]和[s_c2，s_f]，速度间隔[v_o，v_c1]、[v_c1，v_c2]和[v_c2，v_f]，加速度间隔[a_o，a_c1]、[a_c1，a_c2]和[a_c2，a_f]，a_c1，a_c2即车辆所能达到的最大加速度amax)。

①[0，t_c1]，在最大纵向加加速度j_max约束下，加速度从a₀增加到a_max，速度v_o加速到v_c1；

②[t_c1，t_c2]，在最大纵向加速度a_max约束下，从v_c1加速到v_c2；

③[t_c2，t_f]，在最大纵向加加速度j_max约束下，纵向加速度从a_max减少到a_f，速度v_c2减速到v_f，与车辆期望目标状态一致。

运动方程如下：

其中：|j(t)|≤j_max，|a(t)|≤a_max，|v(t)|≤v_max。

求解可得最优速度曲线与车辆换道轨迹弧长s关系如下：

2、变异圆曲线本原、变异回旋线本原生成方法

车辆在匀速行驶条件下，圆曲线和回旋线是最优轨迹本原。为了得到变速条件下的最优变道轨迹，本发明利用上文的变速换道速度曲线生成轨迹本原，即变异圆曲线本原、变异回旋线本原。

如图2所示轮式车辆模型，设P＝(x(t)，y(t)，θ(t)，k(t))∈Γ是换道轨迹上任意一点，固定在Frenet标架{e₁，e₂，0}上，e₁是P在Γ上的单位切向量，e₂是P在Γ上的单位法向量。x轴与e₁的夹角表示车辆航向角θ(t)∈[-π，π]，k表示车辆在t时刻轨迹的曲率。运动方程可表示为：

φ(t)表示车辆前轮转向角，b表示车辆轴距，则：

车辆行驶轨迹的曲率与前轮的转向角有关，并且要精确地遵循这种类型的路径，车辆高速运动状态下，不可能在曲率不连续处停下来，重新定位其前轮转向角，因此，车辆行驶轨迹要求曲率是连续的是十分必要的(Fraichard and Scheuer，2004)。转向角受机械运动限制，即：

|φ(t)|≤φ_max

因此：

其次，当离心力超过侧向力时，由沿路面动量引起的侧滑事故是最常见的事故类型之一，离心力对车辆稳定性的影响很大，也表明了车辆在高速运动状态下，轨迹曲率应保持连续(Anderson，2010；Chang and Huh，2015)。当车辆以v₁(t)行驶，为保证车辆行驶稳定性，车辆所受离心力不能超过防止车辆侧滑的最大离心力γ_max，轨迹曲率应满足以下约束：

实践表明，尤其在车辆高速运行状态下，车辆行驶转向角将远小于φ_max。

此外，由于转向速度

有限，当车辆以v₂(t)行驶，轨迹曲率变化率

应满足以下约束：

其中：

和

是当车辆以单位速度运动时，转向角、转向角速度和轨迹曲率变化率所对应的值。

因此，生成车辆换道轨迹的约束条件为：

在车辆高速运行状态下，车辆达到γ_max比

容易得多，因此，一般情况下v₁(t)<v₂(t)，同时也说明，生成轨迹本原时应优先考虑横向加速度约束γ_max。

Boissonnat等人(Boissonnat et al.，1994a；Boissonnat et a1.，1994b)指出了当车辆运行速度为1时，圆曲线和回旋线是满足约束条件

的时间最优轨迹本原。在任意时间间隔[t，t+dt]，设自动驾驶车辆速度为v(t)，因为dt是一个无穷小量，v(t)可看成常数，式(4)的最优时间控制问题的约束条件变为：

当k_max在无穷小量dt中是常数，v(t)＝ds(t)/dt，则最优轨迹本原可通过下式求得：

式(4)表示车辆行驶在在一个特殊的圆弧上，本发明将其称为变异圆曲线本原，其半径随v(s)沿着轨迹变化，v(s)替换ds(t)/dt，最优轨迹本原为一个一阶线性微分方程式(5)的解。

式(5)的解如下：

θ(s)＝c+∫γ_max/[v(s)]²ds (6)

其中：C表示常数。

当

在无穷小量dt中是常数，

随着v(t)沿着轨迹本原变化，最优轨迹本院可表示为式(7)。

式(7)表示车辆行驶在在一个特殊的回旋曲线，本发明将其称为变异回旋线本原，其曲率变化率随v(s)沿着轨迹变化，v(s)替换ds(t)/dt，最优轨迹本原为一个二阶线性微分方程式(8)的解。

式(5)的解如下：

其中，C1、C2表示常数，s为车辆换道轨迹长度，b表示车辆轴距。

计算可得运动轨迹本原上任意一点坐标：

其中：ξ表示积分参数，{x_o，y_o)表示变异圆曲线本原、变异回旋线本原起点。

3、变速换道轨迹生成方法

自动驾驶车辆换道定义为从当前的车道到相邻车道的转换，换道发生时沿道路的纵向距离S和横向位移HD(即车道宽度)。如图3所示，P_o{x_o，y_o，s_o，v_o，a_o，j_o}和P_f{x_f，y_f，s_f，v_f，a_f，j_f}分别表示自动驾驶车辆在最优轨迹Γ上初始状态和目标状态。

如果自动驾驶车辆向左转弯，轨迹的曲率方向是向左的，反之亦然。车辆在P_o和P_f运动状态应该符合道路线形的变化。因此，最优轨迹直观上应该是若干变异圆曲线本原和变异回旋线本原的组合，P_c、P′_C为轨迹曲率方向发生变化的点。

变异圆曲线本原考虑横向加速度约束γ_max，提供了车辆最小转弯半径，变异回旋线本原考虑前轮转向角速度变化率约束

提供了车辆轨迹曲率最大变化率，通过前变异回旋线本原、后变异回旋线本原连接变异圆曲线本原，结合道路线形生成换道轨迹。不同的横向加速度约束γ_max和前轮转向角速度变化率约束

约束的选取提供给轨迹规划自由度，计算生成不同的换道轨迹。

把轨迹划分为七个轨迹本原：[s_o，s₁]，[s_i，s_i+1](i＝1，2，3，4，5)，和[s₆，s_f]，根据速度曲线v(s)、式(9)和式(10)可求得轨迹上任意一点的航向角θ(s)，和坐标P{x(s)，y(s)}。

最优轨迹终点坐标(X，Y)可表示为：

换道终点坐标(X′，Y′)与换道长度S(包含直线段[0，S₁]，回旋线段[S₁，S₂]，圆曲线段[S₂，S₃])的关系可表示为：

其中：A²表示回旋线变化率，K表示终点曲率，式(12)结合线形具体计算。

设终点车辆航向角为Θ，则最优轨迹终点与换道终点的关系为：

结合式(11)、式(12)和式(13)求解可得换道轨迹坐标函数P{x(s)，y(s)}，速度函数v(s)，车辆航向角函数目(s)，加速度函数a(s)，加加速度函数j(s)，轨迹曲率函数k(s)，横向加速度函数γ(s)，以及换道沿道路的纵向距离S。图4给出了自动驾驶车辆起点状态为Po{x_o，y_o，s_o，v_o＝100km/h，a_o＝0，j_o＝0}，终点状态为P_f{x_f，y_f，s_f，v_f，a_f＝0，j_f＝0}，A＝600，K＝1/1500m^-1，j_max＝2ms^-3，γ_max＝1.5ms^-2，a＝-1，0，1ms^-2时的轨迹规划相关函数图像。其中，图4(a)为变速换道轨迹函数图像，图4(b)为速度函数图像，图4(c)为航向角函数图像，图4(d)为纵向加速度函数图像，图4(e)为纵向加加速度函数图像，图4(f)为曲率函数图像，图4(g)为横向加速度函数图像。

二、实时换道模块

实时换道规划模块包括两个部分：安全距离计算，为防止动态环境下换道车辆与前车或后车相撞；实时换道决策模块，一定时间间隔后迭代动态计算规划轨迹，即实时保证车辆行驶终点位于安全距离外，又有效更正路径跟踪的误差。

1、安全距离计算

为了防止动态环境下换道车辆与前车或后车相撞，本发明采用经典的Gripps模型计算换道车辆与前车或后车的安全距离。

如图5所示，假设前车在(t-τ)时刻开始制动，在t′时刻停止，换道车辆经过一个反应时间τ后在t时刻制动，为了不与前车发生碰撞，换道车辆与前车应保持的安全距离是s_q：

其中，v_f，a_f分别表示当前换到车辆规划轨迹终点速度、减速度，v_q，a_q分别表示前车速度、最大制动减速度。

同理，换道车辆与后车应保持的安全距离是s_h：

其中，v_h，a_h分别表示前车速度、最大制动减速度。

只要换道车辆轨迹终点位于安全距离外，则不会发生碰撞，若轨迹终点位于安全距离内，则有可能发生碰撞。

2、实时换道

如图6所示，当换道车辆轨迹终点位于安全距离外，虽不会发生碰撞，但车辆在迭代间隔Δt后，其状态可能发生微小变化，需使用原来的需调整换道纵、横向加速度和纵向加加速度约束值重新计算换道轨迹；若轨迹终点位于安全距离内，使用原来的需调整换道纵、横向加速度和纵向加加速度约束值，则有可能发生碰撞，车辆需在下一迭代间隔Δt调整换道纵、横向加速度和纵向加加速度约束值，重新规划车辆换道轨迹，具体步骤如图7所示。

以上是本发明的较佳实施例，凡依本发明技术方案所作的改变，所产生的功能作用未超出本发明技术方案的范围时，均属于本发明的保护范围。

Claims (5)

1.一种基于车辆行驶规律的变速动态换道轨迹规划方法，其特征在于，包括如下步骤：

步骤S1、根据车辆周围实时环境信息，确定车辆换道或者车道保持，若换道执行步骤S2；

步骤S2、确定换道程序迭代间隔；

步骤S3、初始化换道纵、横向加速度和纵向加加速度，设自动驾驶车辆初始状态和目标状态，使用变速状态下的变异圆曲线本原、变异回旋线本原生成轨迹，并计算避免碰撞的安全距离；变速状态下的变异圆曲线本原、变异回旋线本原生成方式为：

变异圆曲线本原表示车辆行驶在一个圆弧上，受最大离心力γ_max约束，其半径随车辆换道时变速换道速度曲线v(s)沿着轨迹变化，车辆航向角变化如下：

θ(s)＝C+∫γ_max/[v(s)]²ds

其中，C表示常数；

变异回旋线本原表示车辆行驶在一特殊的回旋曲线上，受前轮转向角速率

约束，其曲率变化率随车辆换道时变速换道速度曲线v(s)沿着轨迹变化，车辆航向角变化如下：

其中，C1、C2表示常数，s为车辆换道轨迹弧长，b表示车辆轴距；

使用车辆航向角可计算得变异圆曲线本原、变异回旋线本原上任意一点坐标：

其中，ξ表示积分参数，(x_o，y_o)表示变异圆曲线本原、变异回旋线本原起点；

步骤S4、轨迹终点位于安全距离内，调整换道纵、横向加速度和纵向加加速度值，进入步骤S3；

步骤S5、轨迹终点位于安全距离外，判断是否完成换道，若否进入步骤S3，若是进入车道保持程序。

2.根据权利要求1所述的一种基于车辆行驶规律的变速动态换道轨迹规划方法，其特征在于，车辆换道时，变速换道速度曲线生成方式为：

确定一个加加速度函数j(t)，使加速度a(t)函数在最短时间内达到a_max，对于加速换道速度曲线，将速度曲线划分为三个运动间隔：时间间隔[0，t_c1]、[t_c1，t_c2]和[t_c2，t_f]，与之对应的车辆换道弧长为[s_o，s_c1]、[s_c1，s_c2]和[s_c2，s_f]，速度间隔为[v_o，v_c1]、[v_c1，v_c2]和[v_c2，v_f]，纵向加速度间隔为[a_o，a_c1]、[a_c1，a_c2]和[a_c2，a_f]，a_c1，a_c2即车辆所能达到的最大加速度a_max，其中，[0，t_c1]车辆在最大纵向加加速度j_max约束下，纵向加速度从a₀增加到a_c1＝a_max，速度v_o加速到v_c1；[t_c1，t_c2]车辆在最大纵向加速度a_max约束下，此时a_c2＝a_c1＝a_max，速度v_c1加速到v_c2；[t_c2，t_f]车辆在最大纵向加加速度j_max约束下，纵向加速度从a_max减少到a_f，速度v_c2减速到v_f，与车辆期望目标状态一致；可计算加速换道最优速度曲线与车辆换道轨迹弧长s关系如下：

同理可得减速换道最优速度曲线。

3.根据权利要求1所述的一种基于车辆行驶规律的变速动态换道轨迹规划方法，其特征在于，使用变速状态下的变异圆曲线本原、变异回旋线本原生成轨迹，即生成变速换道轨迹的方式为：

变速换道轨迹为满足横向加速度约束γ_max和前轮转向角速度变化率约束

约束的时间最优轨迹，通过前变异回旋线本原、后变异回旋线本原连接变异圆曲线本原，结合道路线形生成变速换道轨迹，将变速换道轨迹划分为七个轨迹本原：[s_o，s₁]，[s_i，s_i+1](i＝1，2，3，4，5)，和[s₆，s_f]，可求得最优轨迹函数P{x(s)，y(s)}：

其中，ξ表示积分参数。

4.根据权利要求1所述的一种基于车辆行驶规律的变速动态换道轨迹规划方法，其特征在于，安全距离确定方式为：

假设前车在(t-τ)时刻开始制动，在t′时刻停止，换道车辆经过一个反应时间τ后在t时刻制动，为了不与前车发生碰撞，换道车辆与前车应保持的安全距离是s_q：

其中，v_f，a_f分别表示当前换到车辆规划轨迹终点速度、减速度，v_q，a_q分别表示前车速度、最大制动减速度；

同理，换道车辆与后车应保持的安全距离是s_h：

其中，v_h，a_h分别表示前车速度、最大制动减速度。

5.根据权利要求4所述的一种基于车辆行驶规律的变速动态换道轨迹规划方法，其特征在于，车辆换道时，车辆行驶距离计算方式为：

换道终点坐标(X′，Y′)与换道长度S的关系可表示为下式，其中换道长度S包含直线段[0，S₁]，回旋线段[S₁，S₂]，圆曲线段[S₂，S₃]：

其中，A²表示回旋线变化率，K表示终点曲率，ξ表示积分参数；

求解上式可求得换道长度S。

Images