本发明公开了一种基于增强现实的车辆换道全过程辅助驾驶系统，包括图像采集装置、数据处理装置和增强现实显示装置，其中：所述图像采集装置用于捕获换道车辆周边道路和车辆的图像；所述数据处理装置用于提取换道车辆与周边车辆的距离、相对速度、加速度信息，并将提取的信息输入所建模型，模型结果为车辆换道全过程(包括换道决策、准备、执行)提供建议；所述增强现实显示装置用于显示换道全过程的相关建议。与现有技术相比，本发明的积极效果是：本发明能够实时检测车辆的位置和速度，同时提供换道全过程的增强现实的信息和增强现实设备，如在增强现实装置上显示一个符号或换道操作的建议，最大程度上降低换道过程中发生交通事故的风险。
1.一种基于增强现实的车辆换道全过程辅助驾驶系统，其特征在于：包括图像采集装置、数据处理装置和增强现实显示装置，其中：所述图像采集装置用于捕获换道车辆周边道路和车辆的图像；所述数据处理装置用于提取换道车辆与周边车辆的距离、相对速度、加速度信息，并利用提取的信息为车辆换道全过程提供建议；所述增强现实显示装置用于显示换道建议；所述车辆换道全过程包括换道决策过程、换道准备过程和换道执行过程，在所述换道准备过程中，建立基于换道车辆与目标车道上的换道车辆前、后方车辆之间的期望车头时距的换道准备模型：an(t+τ)＝α(Gn-2(t)-GT1(t))+(1-α)(Gn+2(t)-GT2(t))式中，n为换道车辆，n-2为目标车道上的换道车辆n前方的车辆，n+2为目标车道上的换道车辆n后方的车辆；τ代表车辆n的反应时间，an(t+τ)代表车辆n在t+τ时刻的加速度，vn(t)代表t时刻车辆n的速度，vn-2(t)代表t时刻车辆n-2的速度，vn+2(t)代表t时刻车辆n+2的速度，Gn-2(t)代表t时刻车辆n与车辆n-2的车头时距，Gn+2(t)代表t时刻车辆n与车辆n+2的车头时距,△vn-2(t)代表t时刻车辆n与车辆n-2的速度差，△vn+2(t)代表t时刻车辆n与车辆n+
2的速度差，GT1(t)代表t时刻驾驶员期望的车辆n-2与车辆n的车头时距，a1，b1，c1表示GT1(t)中有关车辆速度的线性组合系数；GT2(t)代表t时刻驾驶员期望的车辆n+2与车辆n的车头时距，a2，b2，c2表示GT2(t)中有关车辆速度的线性组合系数；α表示换道车辆在总的加速度中对目标车道前车的考虑程度。
2.根据权利要求1所述的基于增强现实的车辆换道全过程辅助驾驶系统，其特征在于：所述图像采集装置包括分别安装在换道车辆正前方和换道车辆左、右侧镜子上的三个摄像机，分别用于捕捉当前车道前车，左侧车道前、后车，以及右侧车道前、后车的图像。
3.根据权利要求1所述的基于增强现实的车辆换道全过程辅助驾驶系统，其特征在于：在所述换道执行过程中，建立的换道执行模型包括速度生成模块，轨迹规划模块和轨迹生成模块。
4.根据权利要求3所述的基于增强现实的车辆换道全过程辅助驾驶系统，其特征在于：所述速度生成模块实现如下功能：
(1)计算换道车辆n与当前车道前车n-1不发生碰撞的安全速度：式中， 是车辆n相对当前车道前车n-1的纵向安全速度，ln-1是当前车道前车n-
1的车长，bn-1和bn是当前车道前车n-1和车辆n各自的最大刹车加速度，xn(t)是t时刻换道车辆n的纵向位置，xn-1(t)是t时刻当前车道前车n-1的纵向位置，τ是反应时间；
(2)计算换道车辆n与目标车道上的前车n-2和后车n+2不发生碰撞的安全速度：式中， 和 是换道车辆n相对目标车道前车n-2和目标车道后车n+2的纵向安全速度，ln-2是目标车道前车n-2车长，bn-2是目标车道前车n-2的最大刹车加速度，xn-2(t)是t时刻目标车道前车n-2的纵向位置，ln是换道车辆n的车长，bn+2是目标车道后车n+2的最大刹车加速度，xn+2(t)是t时刻目标车道后车n+2的纵向位置；
式中，vn(t+τ)为换道车辆n的纵向速度，α和β为权重系数，β表示车辆换道执行时，对目标车道中的前车n-2考虑的权重，1-β为对目标车道中的后车n+2考虑的权重；α为本车道车辆对换道车辆n速度影响的权重系数，1-α为目标车辆的车辆对车辆n速度影响的权重系数；
式中，un(t+τ)为换道车辆n的总速度，θ为换道车辆的航向角。
5.根据权利要求4所述的基于增强现实的车辆换道全过程辅助驾驶系统，其特征在于：所述α的计算公式如下：
其中，yf(t)表示换道车辆n在t时刻的横向位置，Y是目标车道与起始车道的横向距离，k是一个标定参数。
6.根据权利要求3所述的基于增强现实的车辆换道全过程辅助驾驶系统，其特征在于：所述轨迹规划模块实现如下功能：
其中，a0，a1，a2，a3均为待确定的参数，xn为纵向上换道车辆n的位置，yn为横向上换道车辆n的位置；
将每一步长的起点位置定义为(0,0)点，终点位置定义为(xf,yf)，任一步长起点的车辆航向角为θi，终点的车辆航向角为0，则有y′n(0)＝tanθi，y′n(xf)＝0；然后代入轨迹方程求得各参数的表达式如下：a1＝tanθi
式中，J为成本函数， 指的是一条换道执行轨迹中终点处的侧向加速度，为轨迹中的最大侧向加速度， 指换道执行过程中所有车辆的最大侧向加速度， 指的是所有车辆在换道执行过程中行驶过的最大纵向距离，ω为需要标定的权重参数；
式中，K(·)为换道执行轨迹的关于车辆纵向位置的曲率函数；K(·)与轨迹方程的关系为：其中，y′和y″分别为换道执行轨迹方程的一阶和二阶导数：代入一、二阶导数到K(·)与轨迹方程的关系中得出曲率K有关xn的函数(0≤xn≤xf)：则得到：
当J取到最小值时相应的xf即为该步长最优轨迹终点的纵向位置；在换道执行过程中，每一个步长都会产生一条最优轨迹。
7.根据权利要求6所述的基于增强现实的车辆换道全过程辅助驾驶系统，其特征在于：所述轨迹生成模块实现如下功能：
(1)计算在一个步长内，车辆沿规划出来的最优轨迹行驶过的弧长L：L＝un(t)τ
同时，对换道执行轨迹方程使用矩形区域积分quad方法计算弧长：式中，X为该步长结束的纵向坐标，同时也是下一个步长开始的纵向坐标；
至此，得到当前步长结束时的车辆状态，从而获得了下一步长开始时的车辆状态，经过多次迭代后，每一个步长的局部轨迹首尾相接，到达目标车辆中线，形成一个完整换道执行轨迹。
基于增强现实的车辆换道全过程辅助驾驶系统
技术领域
[0001] 本发明属于汽车、交通、电子技术领域，本发明涉及一种车道变换辅助系统，尤其涉及一种新的先进车辆换道辅助系统，采用先进的车辆换道算法，包括车辆换道决策算法，车道变换的准备算法和车辆换道执行算法。此外，也采用增强现实技术，以显示车辆换道的导向信息。
背景技术
[0002] 到目前为止，已经提出了若干种车辆换道辅助系统。为了减少车辆换道的风险，现有的解决方案，例如，由宝马和大众基于雷达的车辆换道预警系统。当车辆驾驶员进行换道决策时，在他们的盲区基于雷达的换道预警系统会做出提醒。然而，现有的换道辅助系统存在一些不足，主要有两个方面。首先，现有的车辆换道辅助系统主要是提供在目标车道上的车辆的信息状态，如位置，速度等，他们不是真正的“辅助”。如何完成车辆换道过程还取决于驾驶员自身的判断。该系统并没有提供任何建议，帮助司机安全和快速完成车辆换道过程。因此，现有的车辆换道辅助系统需要改进。其次，当前的车辆换道辅助驾驶系统主要是为驾驶员提供换道决策方面的辅助，而在换道的其他方面，比如换道准备和执行方面，并没有提供相关的辅助建议，而换道准备和执行也是保证换道可以顺利完成的重要组成内容，不适当的换道准备和执行行为同样可能造成交通事故。最后，现有的换道辅助系统容易使司机分心。现有的换道辅助系统要求司机把更多的注意力集中放在读取信息，这可能给司机带来危险。应采取新技术，可传送所需信息同时又能减少司机分心。一个完整的换道行为过程包括三个部分：换道决策、换道准备、换道执行。换道决策是指驾驶员决定是否需要进行换道。换道准备发生在换道决策后和换道执行前，即在车辆决定换道并选择好目标间隙之后，通过调整换道车辆自身的速度来实现和目标车道的前后车辆保持合适的间距，为下一步进行安全顺畅的换道执行做好准备。换道执行指的是车辆从本车道行驶到目标车道的过程。然而，目前大部分与之相关的研究工作，都只关注了换道决策部分。尚未有针对一般情况下的换道准备模型。在换道执行行为研究方面，也只有少数学者对其进行了分析和建模。学者们提出了横向和纵向的车辆换道执行模型，在两个行驶方向上的模型和真实数据拟合较好。但是当前的换道执行是将换道执行的两个方向分割开来，没有在整体上研究一个完整的换道执行模型。
发明内容
[0003] 为了克服现有技术的上述缺点，本发明提出一种基于增强现实的车辆换道全过程辅助驾驶系统。
[0004] 本发明解决其技术问题所采用的技术方案是：一种基于增强现实的车辆换道全过程辅助驾驶系统，包括图像采集装置、数据处理装置和增强现实显示装置，其中：所述图像采集装置用于捕获换道车辆周边道路和车辆的图像；所述数据处理装置用于提取换道车辆与周边车辆的距离、相对速度、加速度信息，并利用提取的信息为车辆换道行为提供建议；所述增强现实显示装置用于显示换道建议。
[0005] 与现有技术相比，本发明的积极效果是：
[0006] 本发明利用图形处理技术，能够实时检测车辆的位置和速度，同时提供换道全过程的增强现实信息和增强现实设备，如在增强现实装置上显示一个符号或换道操作的建议，能将将换道过程中碰撞的风险降到最低水平。
[0007] 相比现有的换道辅助系统，本发明有两大改进。首先，本发明提出的是一个车辆换道全过程的辅助驾驶系统，在现有的换道辅助系统基础上，增加了驾驶员的换道准备和换道执行辅助功能。不仅可以为驾驶员提供危险警告，而且还可以为驾驶员提供其他的换道建议，帮助其调整与周围车辆的相对速度、车间距等，为换道执行做好准备，并最终帮助驾驶员安全地完成从当前车道到目标车道的横向运动。其次，为了减少辅助驾驶系统对驾驶员注意力造成的干扰，本发明采用了增强现实技术。使用增强现实设备，驾驶员不需要转动头来观察后视镜，只需要遵循增强现实设备上的显示的指导，从而使驾驶员换道的效率增加，并且将换道碰撞的风险降低。
具体实施方式
[0008] 一种基于增强现实的车辆换道全过程辅助驾驶系统，内容主要包括三个方面：
[0009] 1)车辆换道辅助系统构建。本系统包括安装在换道车辆上的图像采集装置、数据处理装置和增强现实显示装置。首先通过图像采集装置捕获周边交通环境的图像，然后，通过数据处理装置提取车辆的距离、相对速度、加速度等信息，将提取的信息进行处理作为车辆辅助换道算法的输入，最后通过增强现实显示装置呈现出车辆辅助换道算法输出的换道建议。
[0010] 2)车辆换道算法。车辆换道行为作为公路基本驾驶行为中的关键部分，对交通安全有着显著的影响，车辆换道行为包括三个部分：换道决策，换道准备，换道执行。然而，目前大部分的研究工作和车辆换道辅助驾驶系统中，都只关注了换道决策部分，对换道准备和换道执行部分的研究和考虑很少。在本系统中要建立新的车辆换道准备和换道执行模型，并提供核心的算法流程。
[0011] 3)基于增强现实的图像处理算法。主要包括车道线定位与提取、车辆检测与识别、距离与相对速度计算、换道信息的增强现实显示等。通过摄像头捕获的图像需要转化为灰度图像，利用相关算法提取道路的边缘，也就是车道线。寻找合适的特征检测算法检测捕获图像中的车辆，进而确定车辆在图像中的位置，用于计算车辆之间的距离和相对速度。最后，在增强现实设备接口上显示最后的车道变化指导信息。
[0012] 以下将对本系统进行详细说明：
[0013] 该系统包括图像采集装置、数据处理装置和增强现实显示装置。利用图像采集装置的摄像机用于捕捉道路和车辆图像，数据处理装置的安卓设备用于对收集的图形处理并提取有效信息，其中OpenCV 2.4用来处理计算机视觉。处理时，首先检测车道，然后识别当前车道车辆和相邻车道车辆。经过信息处理，交通状况采集单元的图像处理输出结果是换道车辆周围的车辆距离和速度。除了图像处理功能外，数据处理装置还承担着车辆换道的工作，并为驾驶员进行车辆换道提供建议。增强现实装置具有显示功能，换道建议将显示在增强现实装置上，用于接收车辆换道建议。
[0014] 该系统的图像采集装置需要三个摄像机来覆盖整个感应区域。三个摄像机分别安装在车辆的正前方和车辆左、右侧的镜子上，分别用于捕捉当前车道前车，以及相邻车道上的前方和后方的车辆，即共有五辆车(前车，左侧车道前、后车，右侧车道前、后车)需要被监控。这五类车辆对车辆换道行为有影响，将被纳入到车辆换道模型单元中。如果五辆车中的其中一个不在感应区域，就不作考虑。
[0015] 一、车道检测：所拍摄的图片将被处理以定位并提取车道线。捕获的图像是RGB彩色图像，它需要大量的处理时间，所以需要将它们转化为灰度图像。然后定位道路区域的感应区域。不同区域有不同的灰度，与其他区域相比，道路区域的灰度级较低。利用Sobel方法进一步提取车道线。尽管车道线是从原始图像中提取，但仍然存在大量噪声。因此，通过采用Hough变换法降低噪声，获得道路边缘。最后，车道检测成功并输出显示。
[0016] 二、车辆检测过程：通过采用Haar-like特征检测算法检测捕获的图像中的车辆，误报率将会进一步降低。该系统将对相机拍摄的每一个图像(帧)进行预处理，以提高整体效率和精度。虽然输入图像经过调整，但对于特征检测器仍然过大。因此，有必要定位输入图像的感应区域以使系统实时响应。我们选择调整的灰色中心作为感应区域来检测车辆。同时，为确保我们的目标车辆总是出现在图像的中心，需进一步校准相机的位置。对于车辆检测，我们使用Haar-like特征检测器，使用人脸检测常用的AdaBoost算法，其速度快、精度高。为了使Haar-like特征检测器在车辆检测时有效，我们必须使用一个工具来执行训练数据的基本操作，此处采用Imglab训练特征检测器。误报源于Haar-like特征检测器的识别方法。它识别任何单一物体为一台车，这不符合事实。误报将大大降低系统的准确性，为此通过采用两种不同的技术(尺寸滤波器和纵横比滤波器)来降低误报。尺寸过滤器可将高度太大或太小的车辆过滤掉。纵横比滤波器利用车辆一般宽高比。大多数车辆的纵横比(宽度/高度)在0.4到1.6之间。如果检测到的车辆的纵横比不在此范围内，它可能是一个误报，则忽略该物体。在这之后，系统返回验证车辆的底部坐标以进一步确定该车辆的距离和相对速度。
[0017] 三、距离和速度计算过程：显示确定距离和相对速度的溢出，采用逆透视映射(IPM,Inverse Perspective Mapping)技术，可实现坐标系从一个角度到另一个角度的变换。为了实现IPM，一些参数应优先获取。为获取平面及俯视图，需获取摄像机的高度h、从相机到地面的角度θ和相机的焦距K。h和l通过调整摄像机的位置和方向获得，K需要通过校正摄像机进行估计。发现相机焦距正在使用的棋盘校准这是有据可查的，并包括在OpenCV库来计算相机的焦距K。一旦获得相机的高度h，从相机到地面的角度θ和相机的焦距K，转换可以进行。变换图像中两像素间距p将代表他们的距离。要与现实世界的距离的像素相关，应预先校准。这种校准的目的是估计一个像素的现实世界的长度。摄像机将被安装在一个固定的已知的高度h和角度θ，然后长度为l的字符串将被放置在的摄像头前面。相机拍摄字符串的图像后，图像将随着相机的焦距K被送入IPM算法。像素p由长度l串转换后的图像中占据的数量将表示现实世界长度l。通过用p除以l，可以估算出真实世界中的一个像素的长度。校准完成后，应安装在车辆的同一高度和角度。然后，换道车辆和任何目标车辆之间的相对距离都可以获得。相对速度可以通过对帧间距离差和时间差分进行计算，由于程序是确定车辆换道条件，需要相对速度而非绝对速度。
[0018] 四、在增强现实设备接口上显示最后的车道变化指导信息：在设备界面上显示与周围车辆(包括主道前面车辆、目标车道前面车辆和滞后车辆)的时间间隔、适当的车道变化点箭头指示，以及向司机发出的加速或减速指令等。
[0019] 五、换道过程：得到的车辆信息将被用于生成车辆换道引导。车辆换道包括车辆换道决策、车辆换道准备和车辆换道执行三个部分，在换道决策时，当司机产生换道意愿，首先它会做出决定，确定车道和目标距离的选择，即选择车道和目标间距。车道的决策模型使用由Gipps提出的模型。之后，司机决定换道时，需要确定自己的间距和速度是否适合进行进一步的换道，如果适合，车辆可以继续执行换道；否则，他们需要进一步调整其与其他车辆的间距或同步与目标车道周围车辆的速度。
[0020] 换道决策模型：
[0021] 车道的决策模型使用由Gipps提出的模型，其中：换道车辆为n，在当前车道上的换道车辆n前方的车辆为n-1，在目标车道上的换道车辆n前方的车辆为n-2，在目标车道上的换道车辆n后方的车辆为n+2。
[0022] 换道准备模型：
[0023] 换道准备行为是换道车辆n为了寻找与车辆n-2和车辆n+2之间的期望车头时距，为下一步的换道执行提前做好准备的过程。因此，本文建立的换道准备模型是基于期望车头时距的，模型的表达式如下：
[0024] an(t+τ)＝α(Gn-2(t)-GT1(t))+(1-α)(Gn+2(t)-GT2(t))
[0025]
[0026]
[0027] 式中，τ代表车辆n的反应时间，an(t+τ)代表车辆n在t+τ时刻的加速度，vn(t)代表t时刻车辆n的速度，vn-2(t)代表t时刻车辆n-2的速度，vn+2(t)代表t时刻车辆n+2的速度，Gn-2(t)代表t时刻车辆n与车辆n-2的车头时距，Gn+2(t)代表t时刻车辆n与车辆n+2的车头时距,△vn-2(t)代表t时刻车辆n与车辆n-2的速度差，△vn+2(t)代表t时刻车辆n与车辆n+2的速度差，GT1(t)代表t时刻驾驶员期望的车辆n-2与车辆n的车头时距，GT2(t)代表t时刻驾驶员期望的车辆n+2与车辆n的车头时距，α表示换道车辆在总的加速度中对目标车道前车的考虑程度，a1，b1，c1，a2，b2，c2表示车头时距表达式中有关车辆速度的线性组合系数，没有实际物理意义，会根据不同的换道情境发生变化。
[0028] 车辆换道执行模型：
[0029] 1.模型框架
[0030] 本发明提出的车辆换道执行模型整体框架主要分为三个子模块：速度生成模块，轨迹规划模块和轨迹生成模块。车辆感知周边车辆行驶状态(速度，位置等)，并以此作为系统输入，经过速度生成模块运算得到车辆每一个步长的速度。同时，周边车辆行驶状态输入到轨迹规划模块，再根据速度生成模块获得的车辆速度，经过轨迹规划得到每一步长中的最优轨迹，然后再以速度和最优轨迹为基础，使用轨迹生成模块获得车辆换道执行开始至结束的最终轨迹。
[0031] 各个子模块的主要功能如下：
[0032] (1)速度生成模块
[0033] 速度生成模块的目标是获得车辆每一个时间步长的实时速度。模型首先考虑周边车辆的行驶状态，假设当前换道车辆n换道执行时在纵向上尽量和其当前车道的车辆n的前车n-1以及目标车道上的车辆n的前车n-2和后车n+2保持一个安全距离，从而计算出纵向上的行驶速度，然后再根据车辆自身横向速度和纵向速度几何的关系，计算出车辆的总速度。
[0034] (2)轨迹规划模块
[0035] 车辆驾驶员在决策出其换道执行的速度后，会根据该速度预估出一条合适的车辆行驶轨迹，这个过程，我们称为轨迹规划。轨迹规划是根据车辆的当前位置、横向上的换道距离，以及根据速度生成模块计算得到的当前速度，综合考虑换道效率和换道舒适性，寻找到当前步长的最优换道执行轨迹。该轨迹规划模块为每一个时间步长规划产生一条新的最优换道轨迹，所以车辆在换道执行整个过程中可以实现实时调整换道轨迹，高效舒适地完成换道执行过程。
[0036] (3)轨迹生成模块
[0037] 按照速度生成模块和轨迹规划模块生成的换道执行速度与最优轨迹，车辆完成一个时间步长的行驶，最终获得步长结束时车辆新的位置和航向角，以作为下一步长的初始状态。经过多次迭代后，最终能够生成一条完整的车辆换道执行轨迹。
[0038] 2.速度生成模块
[0039] 本文中所用的换道执行速度模型认为车辆在换道执行过程中的纵向运动受到在当前车道上的换道车辆n前方的车辆n-1，在目标车道上的换道车辆n前方的车辆n-2和后方的车辆n+2的影响，根据这些车辆的实时行驶状态对自身速度进行调整。模型引入了Gipps的安全距离规则，即假设车辆在换道执行中总是试图和车辆n-1，n-2和n+2保持一个安全行驶速度，车辆在纵向的最终速度是由这三个速度加权获得的。如下对该模型进行简介。根据Gipps模型，为了不与当前车道前车n-1发生碰撞，车辆n需保持如下的安全速度：
[0040]
[0041] 式中， 是n车辆相对当前车道前车n-1的纵向安全速度，ln-1是当前车道前车n-1的车长，bn-1和bn是当前车道前车n-1和本车各自的最大刹车加速度。xn(t)是t时刻换道车辆n的纵向位置，xn-1(t)是t时刻当前车道前车n-1的纵向位置，τ是反应时间。
[0042] 同时，推导出了不与目标车道的前车n-2和后车n+2发生碰撞时应保持的安全速度，其表达式如下：
[0043]
[0044]
[0045] 式中 和 是换道车辆n相对目标车道前车n-2和目标车道后车n+2的纵向安全速度，ln-2是目标车道前车n-2车长，bn-2是目标车道前车n-2的最大刹车加速度，xn-2(t)是t时刻目标车道前车n-2的纵向位置，ln是换道车辆n的车长，bn+2是目标车道后车n+2的最大刹车加速度，xn+2(t)是t时刻目标车道后车n+2的纵向位置。
[0046] 然后，综合考虑车辆n-1，n-2和n+2对车辆n速度的影响，得到了换道车辆最终的纵向速度如下：
[0047]
[0048] 式中，vn(t+τ)为换道车辆n的纵向速度，α和β为权重系数。
[0049] 式(4)中β表示车辆换道执行时，对目标车道中的前车n-2考虑的权重，而1-β为对目标车道中的后车n+2考虑的权重。
[0050] 式(4)中α为本车道车辆对换道车辆n速度影响的权重系数，则1-α为目标车辆的车辆对车辆n速度影响的权重系数。α随着换道车辆距目标车道横向位置的减少而减小，它的取值范围为0和1之间。在换道车辆跨过车道线之前，α的值大于0.5，也就是说当前车道的车辆对换道执行车辆的影响较大；当换道车辆跨过车道线之后，α的值将小于0.5，目标车道的车辆对换道执行车辆的影响变得更大。α的计算公式如下式所示：
[0051] α＝tan(pn(t))/k (5)
[0052] pn(t)＝yf(t)/Y (6)
[0053] 其中，yf(t)表示换道车辆n在t时刻的横向位置，也就是当前位置距终点的横向距离，Y是目标车道与起始车道的横向距离，k是一个需要标定的参数。
[0054] 在获得的车辆换道执行过程中纵向速度的基础上，根据纵向速度和横向速度的固有几何关系，得到车辆行驶的总速度如下：
[0055]
[0056] 式中，un(t+τ)为换道车辆n车辆的总速度，θ为汽车的航向角，也就是车辆运动方向与x坐标轴的夹角。
[0057] 3.轨迹规划模块
[0058] 换道车辆在换道执行时进行轨迹的规划，每一时间步长会寻找一条新的最优轨迹。本发明利用多项式曲线来模拟车辆换道执行过程中的行驶轨迹，使用的是三次多项式，三次多项式的线形和车辆换道执行曲线十分相近，而且三次多项式轨迹既具有二阶光滑度的特点，又避免了高次多项式轨迹需要引入抽象参数来约束轨迹的复杂性，其表达示如下：
[0059]
[0060] 其中，a0，a1，a2，a3均为待确定的参数，xn为纵向上换道车辆n的位置，yn为横向上换道车辆n的位置。在换道执行过程中，车辆在每一个步长都会规划出一条新的换道执行轨迹，该轨迹以当前位置为起点，以目标车道中线上的某一位置为终点，在终点的时候车辆的运动方向与车道平行。在此，我们使用移动坐标系，将每一步长的起点位置定义为(0,0)点，终点位置定义为(xf,yf)，任一步长起点的车辆航向角为θi，终点的车辆航向角为0，则有：
[0061] y′n(0)＝tanθi (9)
[0062] y′n(xf)＝0 (10)
[0063] 将以上车辆换道执行的起点坐标(0,0)和终点坐标(xf,yf)以及式(9)和(10)代入轨迹方程(8)得到轨迹方程中各参数的表达式如下：
[0064] a1＝tanθi (11)
[0065] a0＝0 (12)
[0066]
[0067]
[0068] 将式(11)-(14)代入到轨迹方程(8)得到：
[0069]
[0070] 需要说明的是不同于之后的步长，车辆换道执行的第一个步长中，车辆开始驶离当前车道，此时车辆的运动方向与车道方向平行，它的起始航向角也为0。
[0071] 在式(15)中，tanθi可以起点航向角确定，yf为当前位置距终点的横向距离，为已知量。所以，轨迹方程由换道执行完成时车辆在纵向上所能达到的最大位置xf唯一确定。而在真实换道过程中，驾驶员在换道执行的过程中总是期望以更短的时间完成换道执行，而且尽量保证换道过程的舒适性最大。但是，这两个因素是相互矛盾的，驾驶员在换道执行过程中对这两个因素需进行权衡考虑。因此，本文构造了一个换道执行的成本函数来表达驾驶员在换道执行中对效率和舒适性的考虑。在该成本函数中用侧向最大加速度代表舒适度，侧向最大加速度越大，说明舒适度越差，用车辆换道执行的最大纵向位置xf代表效率，xf越大，说明换道执行的效率越低。为了使两个因素在一个数量级上进行评估，我们对它们进行归一化处理，具体表达式如下：
[0072]
[0073] 式中，J为成本函数， 指的是一条换道执行轨迹中终点处的侧向加速度，为轨迹中的最大侧向加速度， 指换道执行过程中所有车辆的最大侧向加速度， 指的是所有车辆的在换道执行过程中行驶过的最大纵向距离，ω为需要标定的权重参数。
[0074] 成本函数J在终点处的侧向加速度 可由下式计算：
[0075]
[0076] 式中，K(·)为换道执行轨迹的关于车辆纵向位置的曲率函数。而式(17)中建立了K(·)与轨迹方程得关系：
[0077]
[0078] 其中，y′和y″分别为换道执行轨迹方程(15)的一阶和二阶导数，它们的表达式如下：
[0079]
[0080]
[0081] 将式(19)和(20)代入到式(18)中得出曲率K有关xn的函数(0≤xn≤xf)，其表达式如下：
[0082]
[0083] 则式(17)为：
[0084]
[0085] 再将式(22)代入到成本函数J，也就是式(16)中，得到成本函数J的最终表达式，如下：
[0086]
[0087] 从式子(23)可以看出，成本函数J是关于xf的一元函数，J取到最小值时相应的xf即为该步长最优轨迹终点的纵向位置。在换道执行过程中，每一个步长都会产生一条最优轨迹。
[0088] 4.轨迹生成模块
[0089] 车辆换道执行过程是需要经过多个时间步长完成的，每一个步长上述的速度生成模块和轨迹规划模块都要执行一次，从而实现车辆从当前车道的中线移动到目标车道的中线。在这一部分我们将介绍换道车辆如何实现一个步长内的移动，计算出步长结束时车辆新的位置和航向角。
[0090] 在一个步长内，车辆沿规划出来的最优轨迹行驶过的弧长L如下：
[0091] L＝un(t)τ (24)
[0092] 同时，对换道执行轨迹方程式(15)使用矩形区域积分quad方法可计算弧长如下：
[0093]
[0094] 式中，X即为该步长结束的纵向坐标，也是下一个步长开始的纵向坐标。
[0095] 则有:
[0096]
[0097] 根据式(26)即可求得X的值，并代入到轨迹方程(15)中可得到步长结束时的横向坐标。
[0098] 接下来计算步长结束时的车辆的航向角，令轨迹方程的一阶导函数(19)为：
[0099]
[0100] 式中，θ为换道执行轨迹上的任意一点的车辆航向角。
[0101] 令xn＝X，则可以得到当前步长结束时车辆的航向角，如下：
[0102]
[0103] 到此，根据式(26)和(28)就可以得到当前步长结束时的车辆状态，从而也获得了下一步长开始的状态，经过多次迭代后，每一个步长的局部轨迹首尾相接，到达目标车辆中线，形成了一个完整换道执行轨迹。
