CN110646007B

CN110646007B - 一种基于形式化表示的车辆驾驶方法

Info

Publication number: CN110646007B
Application number: CN201910894640.4A
Authority: CN
Inventors: 高洪波; 李智军; 刘康; 储晓丽; 郝正源
Original assignee: University of Science and Technology of China USTC
Current assignee: University of Science and Technology of China USTC
Priority date: 2019-09-20
Filing date: 2019-09-20
Publication date: 2021-10-01
Anticipated expiration: 2039-09-20
Also published as: CN110646007A

Abstract

本发明公开了一种基于形式化表示的车辆驾驶方法，属于智能驾驶技术领域，包括：构建公共数据库，该公共数据库中存储有由一次规划路径、驾驶态势图、驾驶水平及交通规则信息构成的时序数据；基于公共数据库中的时序数据，利用贝塞尔曲线拟合生成二次规划路径；根据二次规划路径与车辆正前向所形成的航向角，实现车辆的寻的驾驶。通过将人类驾驶水平和驾驶行为赋予智能车辆，并对其形式化，基于驾驶态势图进行自主决策生成精细的二次规划路径，实现从起点到终点的寻的驾驶。

Description

一种基于形式化表示的车辆驾驶方法

技术领域

本发明涉及智能驾驶技术领域，特别涉及一种基于形式化表示的车辆驾驶方法。

背景技术

智能驾驶技术涉及信息工程、控制科学与工程、计算机科学、机械工程、数理科学、生命科学等诸多学科，是衡量一个国家科研实力和工业水平的重要标志。智能驾驶的出现，从根本上改变了传统的车辆驾驶方式，将驾驶员从“车-路-人”闭环系统中解放出来。其利用先进的电子与信息技术控制车辆行驶，让驾驶活动中常规的、持久且疲劳的操作自动完成，人仅仅做高级的目的性操作，能够极大地提高交通系统的效率和安全性，具有广阔的应用前景。同时，智能驾驶技术的研究将极大地增强我国在汽车主动安全系统方面的核心竞争力，对提升我国汽车电子产品和汽车产业自主创新能力具有重大的战略意义。

随着信息技术，尤其是传感器、通信、计算机和互联网技术的迅猛发展和广泛应用，人类获取数据的手段越来越多、速度大大加快、成本急剧降低，揭示自然规律更加深刻，曾经的“大定律”必然不会具有普遍性，出现越来越多的特例或者反例，形式化越来越难。只能够用更多的“小定律”来解释各种小范围的自然现象、描述其规律和形式化约束。如何对驾驶水平和驾驶行为进行描述和形式化约束，来改善城市日趋拥堵的交通状况是亟待解决的一大难题。

发明内容

本发明的目的在于克服现有技术存在的不足或缺陷，以实现模拟各类驾驶员的驾驶行为，最大程度的解决城市交通拥堵问题。

为实现以上目的，本发明采用一种基于形式化表示的车辆驾驶方法，包括如下步骤：

构建公共数据库，该公共数据库中存储有由一次规划路径、驾驶态势图、驾驶水平及交通规则信息构成的时序数据；

基于公共数据库中的时序数据，利用贝塞尔曲线拟合生成二次规划路径；

根据二次规划路径与车辆正前向所形成的航向角，实现车辆的寻的驾驶。

进一步地，在所述构建公共数据库之前，还包括：

基于道路地图数据库提供的道路形态和车辆周边环境信息，将起点位置和目的地位置匹配到道路地图中道路网络的节点上，得到从起点到目的地的地理空间规划路径，作为所述一次规划路径。

进一步地，在所述构建公共数据库之前，还包括：

获取车载传感器所检测的信息，并利用驾驶认知的图表达语言对车载传感器所检测的信息统一表示，得到实时驾驶态势；

将车辆的实时位置及朝向映射到所述驾驶态势中，得到驾驶态势图。

进一步地，在所述构建公共数据库之前，还包括：

S11、对驾驶员的驾驶状态进行分类，并设定各类驾驶状态的评价指标；

S12、基于专家评价方法，得到每种驾驶状态的评估值；

S13、将同一驾驶水平下每种驾驶状态的评估值输入至逆向云发生器中，计算每种驾驶状态中云模型的表征数；

S14、将同一驾驶水平下的每种驾驶状态中云模型的表征数输入到正向云发生器中，获得该驾驶水平的云滴在数域空间的定量位置和定性概念的确定度，构建该驾驶水平的评估云图；

S15、重复执行所述步骤S12～S14，获得不同驾驶水平对应的评估云图；

S16、获取待评估驾驶员的实时驾驶数据，并执行所述步骤S12～S14，获得该待评估驾驶员的水平评估云图；

S17、将该待评估驾驶员的水平评估云图与所述不同驾驶水平对应的评估云图进行比较，确定该待评估驾驶员的驾驶水平。

进一步地，所述将同一驾驶水平下的每种驾驶状态的评估值输入至逆向云发生器中，计算每种驾驶状态中云模型的表征数，包括：

获取N个同一驾驶水平驾驶员的同一种驾驶状态的评估值，作为该驾驶状态对应的云滴Y₁＝(Y₁₁,Y₁₂,…,Y_1N,)；

根据该驾驶状态对应的云滴Y₁＝(Y₁₁,Y₁₂,…,Y_1N,)，分别计算云滴的均值E_1x、云滴的方差

和云滴的熵E_1n；

根据所述云滴的方差

和云滴的熵E_1n，计算云滴的超熵H_1e；

将所述云滴的均值E_1x、云滴的熵E_1n以及云滴的超熵H_1e作为该驾驶状态对应云模型A₁＝(E_1x,E_1n,H_1e)的表征数。

进一步地，所述将同一驾驶水平下的每种驾驶状态中云模型的表征数输入到正向云发生器中，获得该驾驶水平的云滴在数域空间的定量位置和定性概念的确定度，构建该驾驶水平的评估云图，包括：

S21、根据同一驾驶水平下的每种驾驶状态中云模型的表征数，建立用于评估该驾驶水平的综合云模型A＝(E_x,E_n,H_e)，该综合云模型的表征数分别为期望值E_x、熵E_n和超熵H_e；

S22、根据期望值E_x、熵E_n和超熵H_e和给定的云滴数N，得到一个均值为E_x、标准差为H_e的正态随机数

以及一个均值为E_n，标准差为

的正态随机数x；

S23、计算

令x是定性概念的一次具体量化值，令y是x的确定度；

S24、重复执行步骤S22～S23，直至产生N个云滴；

S25、输出N个同一水平驾驶员的云滴在数域空间的定量位置和定性概念的确定度(x,y)；

S26、获得该驾驶水平的云滴在数域空间的定量位置和定性概念的确定度，构建该驾驶水平的评估云图。

进一步地，所述将该待评估驾驶员的水平评估云图与所述不同驾驶水平对应的评估云图进行比较，确定该待评估驾驶员的驾驶水平，包括：

计算所述待评估驾驶员的水平评估云图与所述不同驾驶水平对应的评估云图之间的相似度；

将取得最大相似度的评估云图所对应的驾驶水平作为所述待评估驾驶员的驾驶水平。

进一步地，所述基于公共数据库中的时序数据，利用贝塞尔曲线拟合生成二次规划路径，包括：

按照时序查找所述公共数据库中的关键数据点；

利用贝塞尔曲线拟合算法将所述关键数据点平滑拟合成曲率变化的弯曲线，作为所述二次规划路径。

进一步地，还包括：

判断所述二次规划路径是否满足设定的检测条件，该设定的检测条件为不会碰撞到障碍物或满足车辆的转角结构要求；

若否，则在所述公共数据库中增加或删除关键数据点，直至生成的二次规划路径满足设定的检测条件；

若是，则根据所述满足设定的检测条件的二次规划路径与车辆正前向所形成的航向角，实现车辆的寻的驾驶。

进一步地，所述根据二次规划路径与车辆正前向所形成的航向角，实现车辆的寻的驾驶，包括：

将所述二次规划路径与车辆正前向所形成的航向角作为控制角；

控制车辆行驶使控制角为零，实现车辆的寻的驾驶。

与现有技术相比，本发明存在以下技术效果：本发明将人类驾驶行为和驾驶水平赋予智能车辆，并对其形式化，用于“寻的驾驶”模式覆盖所有的驾驶水平和驾驶行为。通过将车辆的运动轨迹简化为平面上一维的弯曲线即一次规划路径，将一次规划路径、驾驶态势图、驾驶水平、交通规则及驾驶经验信息等信息构成公共数据库，利用贝塞尔曲线拟合算法，对公共数据库中所提供的实时数据进行曲线拟合，获得一条光滑且符合智能车辆转向结构的二次规划路径，为实现寻的驾驶提供良好的基础，利用二次规划路径与智能车辆正前向形成的航向角作为控制角，控制车辆行驶使当前控制角为零的过程，成为寻的驾驶，当达到二次规划路径的终点时，汽车速度为零，完成车辆从起点到目的地的寻的驾驶。

附图说明

下面结合附图，对本发明的具体实施方式进行详细描述：

图1是一种基于形式化表示的车辆驾驶方法的流程示意图；

图2是驾驶态势图；

图3是智能车驾驶行为评估体系图；

图4是驾驶状态表征参数示意图；

图5是贝赛尔曲线绘制示意图；

图6是巡线驾驶中的寻的驾驶示意图；

图7是跟驰驾驶中的寻的驾驶示意图；

图8是换道驾驶中的寻的驾驶示意图；

图9是路口驾驶中的寻的驾驶示意图；

图10是泊车中的寻的驾驶示意图；

图11是超车中的寻的驾驶示意图。

具体实施方式

为了更进一步说明本发明的特征，请参阅以下有关本发明的详细说明与附图。所附图仅供参考与说明之用，并非用来对本发明的保护范围加以限制。

如图1所示，本实施例公开了一种基于形式化表示的车辆驾驶方法，包括如下步骤S100～S300：

S100、构建公共数据库，该公共数据库中存储有由一次规划路径、驾驶态势图、驾驶水平及交通规则信息构成的时序数据；

需要说明的是，本实施例中交通规则相当于先验知识。交通规则为从交通管理局获取车辆在不同驾驶环境下行驶时应该遵守的交通规则要点，比如：车速要求、交通标志线、以及信号灯指示等。

本实施例中时序数据还可包括驾驶经验等信息，本领域技术人员也可根据实际情况设置公共数据库中的具体信息。

S200、基于公共数据库中的时序数据，利用贝塞尔曲线拟合生成二次规划路径；

S300、根据二次规划路径与车辆正前向所形成的航向角，实现车辆的寻的驾驶。

具体来说，所述公共数据库中一次规划路径的计算过程为：

对于车辆在行驶过程中，通过车载计算机输入起始地和目的地，基于道路地图数据库提供的道路形态和路边环境信息，将起始地数据和目的地位置匹配到地图中道路网络的节点上，规划出一条从起始地到目的地的地理空间路径，从而确定一次规划路径。

需要说明的是，车辆在城市道路上行驶，原本是在6自由度空间的运动学和动力学问题。但如果把整车视为刚体，把城市地面道路视为平面，用车体几何中心的轨迹线表达车体的运动轨迹，通过这样的约简抽象，车的运动轨迹可视为平面上的一条曲率在变化的弯曲线，即一次规划路径。但一次规划路径是出行任务规划，其基于道路地图一次性给出从起点到目的地的地理空间规划轨迹，精确到分米级，可附带有语义等标识。一次规划不与包括GPS在内的任何传感器信息直接挂钩，不一定就是GPS给出的离散点坐标形成的路网文件，还可以是带有语义的、地理特征的、图像特征等的道路地图文件，用笛卡尔坐标系表达。由于没有考虑驾驶过程中周边出现的各种随机情况，所确定的一次规划路径对周围环境并不具有适应性，不具备时间性和随机性，也未必具有实际操作性。也就是说，当智能车前方遇到障碍物或者其他随机情况时，该一次规划路径并不能满足行驶要求。

本实施例中，通过将交通规则、一次规划路径、驾驶经验、驾驶水平和驾驶脑认知等信息融合，制定一个公共数据库，自主决策模块根据公共数据库所提供的时序数据，利用贝塞尔曲线拟合生成二次规划路径，二次规划是实时精细的路径规划，基于公共数据库提供的时序信息数据，采用优化的贝塞尔曲线拟合算法，形成一条待行驶的光滑且满足车辆转角结构的局部路径，它可能偏离一次规划，但不会太远。二次规划以驾驶员认知为中心，依附在运动车辆上的一个相对坐标系里用先验知识理解当前驾驶态势簇，形成决策从而确定驾驶行为，实现车辆从起点到终点的寻的驾驶。

具体来说，所述公共数据库中的驾驶态势图的计算过程为：

其中，车载传感器包括三目摄像机、三维激光雷达传感器、惯性导航传感器和GPS定位系统等。其中，三目摄像机用于获取视觉图象，如移动车辆、行人、信号灯状态等；三维激光雷达传感器用于测量探测智能车与周围环境的距离和角度信息；惯性导航传感器用于检测智能车当前翻滚角、俯仰角以及侧倾角等姿态信息；GPS定位系统用于获取智能车实时位置信息。

通过不同的车载传感器获取路面情况和周围障碍物信息，计算障碍物与智能车的相对位置。以及根据GPS定位信息获得视觉、激光和位置姿态传感器信息数据与世界坐标系的对应关系。

具体根据各传感器输出的信息，通过驾驶认知的图表达语言进行统一，构成驾驶态势实时信息。同时将智能车实时的位置以及朝向映射到驾驶态势中，实现驾驶态势图实时信息融合，构成驾驶脑认知的驾驶态势图。如图所示2，该驾驶态势图反映了周边路权大小及其变化，具有可变粒度。近处粒度颜色深，远处粒度颜色浅，且各向同性。图2中以智能车为中心，与智能车距离越接近颜色越深(本实例中以黑色表示与智能车最接近的区域)，与智能车距离越远颜色越浅(本实例中以浅灰色表示与智能车最远的区域)，没有颜色的区域表示该区域没有障碍物。

需要说明的是，将从人脑中剥离出来的、能完成各种驾驶认知的机器，称为“驾驶脑”。所述的驾驶脑是全局认知，不仅融合了感官知识，还融合了大脑中已有的先验知识和驾驶经验中的时空关联知识，有短时记忆，即周边动态信息，包括移动车辆、行人、信号灯状态等，还有长期记忆，即先验知识，包括道路地图、路口特征、危险场景等。

具体来说，驾驶方式的形式化表示由驾驶水平和驾驶行为组成，驾驶行为由驾驶动作组成。除了正常的驾驶水平，还有低驾驶水平(菜鸟)和高驾驶水平(赛车手)等。基本的驾驶行为有：巡线驾驶，跟驰驾驶，换道驾驶，路口驾驶，泊车等以及超车等。基本的驾驶动作有：踩油门，踩刹车，打方向等。所述公共数据库中的驾驶水平的检测过程为：

其中，如图3所示，本实施例通过对大量实际车辆行驶时的道路录像，以及不同驾驶员的驾驶动作和心理特征分析的研究，可将驾驶员的驾驶状态可分类为：违章情况、行驶方位和长期状态。其中每种驾驶状态的评价指标分别为：

(1)违章情况：表示驾驶员在驾驶过程中交通规则的遵守情况，分为无违章、一般违章以及严重违章。该指标反映了驾驶员对规章制度掌握能力以及驾驶素质能力。可通过路口电子眼、车载摄像机以及交警部门的协助，得到不同驾驶员在驾驶时的交通违章情况。评价指标包括：(1-1)按交通信号灯规定通行情况；(1-2)按规定的速度行驶情况；(1-3)按规定车道行驶情况。

(2)行驶方位：表示驾驶员在驾驶过程中车辆与道路中央的习惯性相对位置，分为道路正中间行驶、道路偏左或偏右行驶以及左右晃动行驶。该指标反映了驾驶员的驾驶习惯，驾驶习惯好则应在道路正中间行驶。通过实际车辆信息采集系统获取的道路信息，利用车道线检测方法或道路检测方法，检测出智能车当前行驶道路上的本车道的左右两条车道线。

如图4所示，用侧向偏差和方向偏差两个表征参数来测评智能车当前的行驶方位和中心偏离程度，具体计算公式为：侧向偏差＝右侧向偏差-左侧向偏差；方位偏差＝右方位偏差-左方位偏差。评价指标包括：(2-1)侧向偏差情况；(2-2)方位偏差情况。

(3)长期状态：表示驾驶员在连续驾驶过程中驾驶状态的保持程度，分为保持状态良好、保持状态一般以及保持状态差。该指标反映了驾驶熟练程度和驾驶时心理状态的变化。可通过心里测试、面试以及车辆监控记录数据来分析，以获取不同驾驶员在经历长期驾驶后的状态情况。评价指标包括：(3-1)驾驶情绪稳定性；(3-2)运行速度判断能力；(3-3)处理突发事件反应能力；(3-4)不同驾驶环境时空判断能力。

需要说明的是，本实施例中所设置的每个评价指标是互不连通的。每个细化的评价指标根据具体的测评标准有相应的、直接定性的协议。但应当理解的是，本实施例原理不限于步骤S11中提出的驾驶状态指标分类和其评价指标，可包括用于分析驾驶状态的所有合理的指标分类和评价指标。

S12、基于专家评价方法，获得评价指标对驾驶状态的影响程度的权重系数，并得到每种驾驶状态的评估值；

S13、将同一驾驶水平下的每种驾驶状态的评估值输入至逆向云发生器中，计算每种驾驶状态中云模型的表征数，具体包括：

和云滴的熵E_1n；

根据所述云滴的方差

和云滴的熵E_1n，计算云滴的超熵H_1e；

下面以低驾驶水平驾驶员为例，建立驾驶状态评估的综合数学模型：

(1)根据图3所示评价系统中所有评价指标对各种驾驶状态的影响程度来确定权重系数，以驾驶状态中的违章情况为例，各驾驶指标的权重系数w_i定义为：

其中，基于综合机理分析各指标的重要性，确定违章情况中各评价指标“质量”为M_i。针对评估对象的指标值设定一个理想值

计算指标值x＝(x₁,x₂,x₃)与理想值的欧式距离，得到第j个低水平驾驶员的驾驶状态中交通违章的评估值。具体的评估公式为：

其中j＝1,2,...,N表示第j个低水平驾驶员，i＝1,2,3分别表示违章情况的评价指标(1-1)按交通信号灯规定通行情况；(1-2)按规定的速度行驶情况；(1-3)按规定车道行驶情况。各评价指标“质量”M_i和理想值

可以基于专家评价方法来设定。

(2)基于步骤(1)获得的评估值Y_1j,利用逆向云发生器计算关于违章情况云模型中的三个表征数，即期望值E_x、熵E_n和超熵H_e，具体步骤如下：

(2-1)输入N个低水平驾驶员的违章情况的云滴Y₁＝(Y₁₁,Y₁₂,…,Y_1N,)具体值。

(2-2)计算云滴均值，公式如下：

(2-3)计算云滴方差，公式如下：

(2-4)计算云滴的熵，公式如下：

(2-5)计算云滴的超熵，公式如下：

(2-6)输出云滴的数字特征A₁＝(E_1x,E_1n,H_1e)。

(3)重复上述步骤(1)至步骤(2)，获得驾驶状态中行驶方位和长期状态的云模型数字特征，分别为A₂＝(E_2x,E_2n,H_2e)和A₃＝(E_3x,E_3n,H_3e)。

(4)基于上述步骤(2)和步骤(3)生成的云模型A₁＝(E_1x,E_1n,H_1e)，A₂＝(E_2x,E_2n,H_2e)和A₃＝(E_3x,E_3n,H_3e)，建立关于低水平驾驶员的驾驶行为评估的综合云模型A＝(E_x,E_n,H_e)，具体过程如下：

(4-1)假设E_1x≤E_2x，如果|E_1x-E_2x|＜|3(E_1n+E_2n)|，则新云模型A′＝(E′_x,E′_n,H′_e)中的三个表征参数可按以下公式计算：

如果|E_1x-E_2x|≥|3(E_1n+E_2n)|，则新的云模型A′用A₁和A₂两个云模型表示，即如果

则A′＝A₁∪A₂＝A₂。反之，如果

则A′＝A₁∪A₂＝A₁。

(4-2)重复上述步骤(4-1)，通过云模型A′和A₃中的三个表征参数，得到关于菜鸟的驾驶行为评估的综合云模型A＝(E_x,E_n,H_e)。

(5)通过正向云发生器对驾驶状态云模型刻画，具体步骤如下：

(5-1)输入步骤(4)得到的期望值E_x、熵E_n和超熵H_e，并给定云滴数N；

(5-2)产生一个均值为E_x，标准差为H_e的正态随机数

(5-3)产生一个均值为E_n，标准差为

的正态随机数x，令x是定性概念的一次具体量化值；

(5-4)计算

令y是x的确定度；

(5-5)重复上述步骤(5-2)至步骤(5-4)，直至产生N个云滴。

(5-6)输出N个关于赛车手的云滴在数域空间的定量位置和定性概念的确定度，即(x,y)。最后通过软件刻画关于低驾驶水平的评估云图A^*。

(6)重复上述步骤(1)至步(5)，分别刻画关于正常驾驶员和高水平驾驶员的驾驶行为的评估云图B^*和C^*。

进一步地，上述步骤S17：将该待评估驾驶员的水平评估云图与所述不同驾驶水平对应的评估云图进行比较，确定该待评估驾驶员的驾驶水平，包括：

具体来说，记录一位未知水平的驾驶员的实时驾驶数据，得到不同驾驶状态中的不同评估指标的平均值，刻画未知水平的驾驶员的评估云图X^*。将评估云图X^*分别与评估云图A^*、B^*和C^*进行比较，其中，评估云图A^*、B^*和C^*分别表示低驾驶水平、正常驾驶水平和高驾驶水平。如果评估云图X^*与评估云图A^*相似度大于与评估云图B^*和C^*的相似度，则可判断该驾驶员的驾驶水平是低驾驶水平；如果评估云图X^*与评估云图B^*相似度大于与评估云图A^*和C^*的相似度，则可判断该驾驶员的驾驶水平是正常驾驶水平；同理，如果评估云图X^*与评估云图C^*相似度大于与评估云图A^*和B^*的相似度，则可判断该驾驶员的驾驶水平是高驾驶水平。

上述车辆驾驶方法的形式化表示方法中，通过云模型分析大量的实验数据，有效地实现在驾驶行为判断过程中定性与定量之间不确定性的转换，使智能车具有与驾驶员相同的驾驶习性。通过驾驶脑对周围环境的认知，生成驾驶态势图，直观地分析了智能车周边路权大小及其变化情况，为智能车提供了更加可靠的驾驶模式。通过优化的赛贝尔曲线拟合法，获得一条光滑并且符合智能车转向结构的二次规划路径，为实现寻的驾驶提供良好的基础。

具体来说：上述步骤S2：基于公共数据库中的时序数据，利用贝塞尔曲线拟合生成二次规划路径，包括如下细分步骤：

按照时序查找所述公共数据库中的关键数据点；

具体为：根据公共数据库所提供的时序数据，分析智能车与前方车辆的车距、路权大小以及变化趋势，形成决策从而确定驾驶行为。按时序寻找公共数据库中的关键数据点，利用优化的贝塞尔曲线拟合算法将一个个关键数据点平滑拟合成曲率变化的弯曲线，以形成待行驶的二次规划路径。本实施例采用贝塞尔曲线拟合法，所生成的二次规划路径的曲率变化连续，而且曲率半径跳动范围不大，符合车辆实际运动的路径要求。

根据关键点和控制点，确定二次规划路径的步骤如下：

如图5所示，给定的相邻时域关键数据点P₀、P₁和控制点P₀ ^*，依次用线段连接；

在线段P₀P₀ ^*上任选一个点D,计算点D到线段起点P₀的距离P₀D与线段P₀P^*总长的比例α＝P₀D:P₀P₀ ^*；

根据比例α，从线段P₀ ^*P₁上找出对应点E，满P₀D:P₀P₀ ^*＝P₀ ^*E:P₀ ^*P₁＝α；

连接点D和点E，从新线段DE中寻找具有相同比例α的点F,满足DF:DE＝P₀D:P₀P₀ ^*＝P₀ ^*E:P₀ ^*P₁＝α；

让选取的点D在线段P₀P₀ ^*上从起点P₀移动到控制点P^*，则点F的轨迹Q₀(t)用参数表示即第一贝塞尔曲线方程组为：

其中P₀＝(x₀,y₀)，P₁＝(x₁,y₁)，

Q₀(t)＝(x′₀,y′₀)和t∈[0,1]。

优化地，本实施例中利用贝塞尔曲线拟合复杂曲线的拟合方法，即把复杂曲线分解为多段较为简单的贝塞尔曲线，再将多段简单的贝塞尔曲线首末拼接起来形成整体曲线。在拼接点处需要满足曲线的光滑性，优化过程如下：

将前一条曲线的终点P₁作为后一条曲线的起点，下一个关键数据点P₂作为后一条曲线的终点，P₁ ^*为后一条曲线的控制点则下一条曲线的轨迹Q₁(t)即第二贝塞尔曲线方程组是：

其中P₂＝(x₂,y₂)，

Q₁(t)＝(x′₁,y′₁)和t∈[0,1]。

为了保证曲线在拼接点的光滑性，必须满足在拼接点的导数是相等的，即

上述第一贝塞尔曲线方程组在参数t＝1的导数是：

第二贝塞尔曲线方程组在参数t＝0的导数是：

因此，要满足

则

和

即

重复上述步骤，直至将所有关键数据点拟合完毕，生成一条待行驶的二次规划路径。上述所拟合后的弯曲线是曲率连续光滑变化，保证了二次规划路径的平滑性。

优选地，在拟合得到二次规划路径后，还包括：

具体来说，上述步骤S3：根据二次规划路径与车辆正前向所形成的航向角，实现车辆的寻的驾驶，包括：

控制车辆行驶使控制角为零，实现车辆的寻的驾驶。

本实施例通过将二次轨迹路径和智能车正前向所形成的航向角信息，输出到控制模块，完成踩油门、踩刹车以及打方向等基本的驾驶动作，保证智能车能按照符号要求的速度沿着二次轨迹路径行驶，同时与周围车辆保持一定的车距，实现智能车寻的驾驶。

需要说明的是，对于不同的驾驶行为，驾驶水平高(赛车手)的占据路权越小，更容易缓解交通拥堵的压力。相反，驾驶水平低(菜鸟)的占据路权越大，更容易增加交通拥堵的压力。如图6至图11所示：

a)巡线驾驶中的寻的驾驶，车道前方百米内无车时，车辆按设定轨迹、车速行驶，不得超过。此时的“的”为二次规划路径和智能车正前向形成的航向角即控制角，如图6所示。

b)跟驰驾驶中的寻的驾驶，前车离本车百米之内，两侧车道均不满足换道条件，本车处于跟车状态。你快我快，你慢我慢，你停我停。此时的“的”为前车与本车连线和智能车正前向形成的航向角即控制角，如图7所示。

c)换道驾驶中的寻的驾驶，本车道内车间距较小，且没有变大趋势，路权受限，超出容忍；邻车道的车间距较大，且没有变小趋势，路权允许；则从换道窗口中确定换道路径，执行换道。此时的“的”为二次规划路径和智能车正前向形成的航向角即控制角，如图8所示。

d)路口驾驶中的寻的驾驶，此时的“的”为二次规划路径和智能车正前向形成的航向角即控制角，如图9所示。

e)泊车中的寻的驾驶，此时的“的”为二次规划路径和智能车正前向形成的航向角即控制角，如图10所示。

f)超车中的寻的驾驶，满足换道驾驶的要求，且在换车道存在能够加速行驶的路段为超车提供足够的距离，当本车在水平方向完全超越前车时，重新行驶回原来的车道。此时的“的”为二次路径规路径和智能车正前向形成的航向角即控制角，如图11所示。

本申请将人类驾驶水平和驾驶行为赋予智能车辆，并对其形式化，基于驾驶态势图进行自主决策生成精细的二次规划路径，实现从起点到终点的寻的驾驶。这种设计方法可以处理驾驶水平的不确定性，模拟不同驾驶水平的驾驶员的驾驶方式，能更为真实地研究实时交通中遇到的问题和处理办法。同时，通过不同驾驶水平的驾驶员对道路资源的占有情况及其对拥堵的影响，可以研究路权与城市交通拥堵的关系，从而最大程度地解决城市交通拥堵问题。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。