CN113386795B - 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统 - Google Patents

Abstract

本发明提供了一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统，方法包括下列步骤：步骤一、由道路信息采样给定车辆当前位置及给定的目标位置点；步骤二、通过Frenet坐标系对轨迹计算进行简化，利用多项式插值出车辆未来的遍历轨迹对应的路径，通过汽车的初始状态与目标状态求解轨迹集合；步骤三、对上述轨迹集合进行成本代价评估并按最小原则排序，根据排序结果依次进行是否满足约束条件的检查，直至确定同时满足各约束条件的最优轨迹；步骤四、获得最优轨迹后下发行车命令用于车辆运动控制。本发明将复杂的非线性轨迹规划问题简化，降低了轨迹规划的难度，规划出光滑、平顺的轨迹，令车辆行驶更加平稳。

Description

一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策 系统

技术领域

本发明涉及一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统。

背景技术

随机科技的不断发展，目前国内外很多知名公司都投入了大量的高精尖人员和技术来发展高水平的智能驾驶技术。决策系统与轨迹规划算法作为车辆自动驾驶最核心的模块之一，越来越受到研究者的关注。通常车辆的决策系统主要是根据传感器感知车辆周围的环境信息，如动态障碍物、静态障碍物、车辆位置、车辆速度、车道边界信息等。系统根据这些信息先规划出一条安全、合理、可行的行车轨迹，并将轨迹信息发送给执行模块，从而控制车辆的加减速、航向角等，实现自动驾驶功能。目前市面上出现的自动驾驶车辆、大多能够感知静态障碍物，而事实上道路上的信息千变万化，对更为复杂场景下的动态障碍物难以应对。

目前轨迹规划方法分为全局轨迹规划与局部轨迹规划。通常全局路径规划已经事先给定于汽车，但汽车在行驶过程中会受到各种各样的外界干扰。因此，除了全局路径规划外，还需要局部轨迹规划(二次规划)，以应对复杂突变的环境。目前，局部轨迹规划方法有：A*算法、RRT(Rapidly-exploring Random Tree，快速搜索随机树)算法等，这类图搜索方法通过随机采样的方式实现对环境的逐步随机探索，能够快速得到局部轨迹的同时还能避免陷入局部最优。然而由于其探索过程具备随机性，因此导致这种方法的规划结果难以预测，在实际工业应用中存在一定的不可控性。概率地图方法、人工势场等，需要不断搜索自由空间(无障碍空间)，需要对每次规划都执行一次，因此会造成极大的时间和资源的浪费。采样差值法的主要思想是，通过密集采样得到一系列的汽车状态，针对每一种状态通过多项式差值、样条曲线拟合差值等手段规划得到轨迹集合，并从中选取最优轨迹。但是不合适的差值方法容易导致汽车的运动学及动力学参数不连续，乘坐体验感欠佳。随着计算机技术不但的发展，人工智能方法也逐渐应用于自动驾驶，不过人工智能的计算对汽车硬件要求较高，同时人工智能技术对样本的需求较大，当样本数量不够时，最终输出的结果将对行驶轨迹有较大的影响因此，智能驾驶系统如何规划出一条能能够应对复杂环境的可靠轨迹尤为重要。

自动驾驶的另一个难点在于汽车位置、姿态与道路的相对关系的建立。通常描述二者之间关系的方法有：全局笛卡尔坐标系、里程坐标系及Frenet坐标系。全局坐标系规划全局路径相对容易，但对于局部道路感知变化难以应对；里程坐标系由于受到里程长度的影响，道路拟合的难度会逐渐变得复杂。

发明内容

本发明的目的在于提供一种自动驾驶车辆智能决策及局部轨迹规划方法，以解决现有技术中对复杂环境下进行轨迹规划存在局部道路复杂难以实现，不可控性高并且计算量大，耗时长的技术问题。

所述的一种自动驾驶车辆智能决策及局部轨迹规划方法，包括下列步骤，

步骤一、由道路信息采样给定车辆当前位置及给定的目标位置点；

步骤二、通过Frenet坐标系对轨迹计算进行简化，利用多项式插值出车辆未来的遍历轨迹对应的路径，通过汽车的初始状态与目标状态求解轨迹集合；

步骤三、对上述轨迹集合进行成本代价评估并按最小原则排序，根据排序结果依次进行是否满足约束条件的检查，不能同时满足所有约束条件时则按排序取下一轨迹重复检查，直至确定同时满足各约束条件的最优轨迹；

步骤四、获得最优轨迹后对其进行Frenet坐标系到笛卡尔坐标的转换，然后将转换结果下发行车命令用于车辆运动控制。

优选的，所述步骤二具体包括：给定车辆当前位置及给定的目标位置点利用6个约束，使用5次多项式进行拟合，具体如下：

若已知t₀时刻的状态[f(t₀),f'(t₀),f”(t₀)]^T及t₁时刻的状态[f(t₁),f'(t₁),f”(t₁)]^T，将已知条件代入上式并用矩阵的形式表达线性方程组如下：

MA＝F

其中：

A＝[a₀ a₁ a₂ a₃ a₄ a₅]^T

F＝[f(t₀)，f′(t₀)，f″(t₀)，f(t₁)，f′(t₁)，f″(t₁)]^T

确定车辆的初始状态d(t₀)＝[d₀，d′₀，d″₀]^T及车辆的目标状态d(t₁)＝[d₁，0，0]^T，令t₀＝0；确定车辆纵向的初始状态s(t₀)＝[s₀，s′₀，s″₀]^T及车辆的目标状态s(t₁)＝[s₁，s′₁，s″₁]^T，令t₀＝0，通过上式便可求解线性方程；

由上述多项式插值计算分别得到对应的横向轨迹集合{d_i(t)|i＝0，1，2...}和纵向轨迹集合{s_i(t)|i＝0，1，2...}，二者结合考虑时间离散{ΔTt₀，t₀+ΔT，t₀+2ΔT，...，t₁}得到横、纵合成轨迹集合{d_i，j，s_i，j|i，j＝0，1，2...}。

优选的，所述步骤二中，目标状态根据场景变化包括下列方法确定目标状态s(t₁)：

巡航：对于横向目标状态，使用本车的横向速度和更新步长N，计算目标状态的横向偏移，其中横向速度和加速度设置为零，对于纵向，终端速度设置为道路限制速度，终端加速度设置为零，纵向位置不受限制，指定为Nan，最终生成在给定时间范围内与道路速度平滑匹配的轨迹：

其中，

表示目标纵向速度；d_explane表示横向位移，其基本更新方法为：d_explane＝v₀+0.5×d²t(i)，i＝1，2，...，N；其中，v₀表示初始速度；

变道：首先确定本车辆的当前车道，然后检查相邻车道是否存在，对于每个现有车道，目标状态的定义方式与巡航控制行为相同，将终端速度设置为当前速度：

其中，

表示车辆目标速度；d_desLane表示车辆横向目标位置；

跟车：首先确定本车当前车道，根据设定的前车时间范围，预测前车的未来状态，并查找与本车占用同一车道的所有车辆，同时确定哪个是最接近本车的车辆，如果前车不存在，则该功能不会返回任何内容，通过前车的位置和速度，将终端纵向位置减少一定的安全距离，计算出自车辆的目标状态：

其中，s_lead-l_safety表示前车与本车的纵向间隔距离；

表示前车纵向速度，作为本车的纵向目标速度；d_lead表示前车的横向位移，作为本车的横向目标位移；

表示前车的横向速度，作为本车的横向目标速度。

优选的，所述步骤三中的约束条件包括，

位置条件：需要保证车辆始终在车道之内，如果目标位置超出道路边界，车辆将会冲出道路发生危险，位置条件可以表示为：d(s)＜d_rode，其中：d_rode表示道路边界位置；

速度条件：车辆在道路上行驶时，速度v_car不能超过车辆本身的速度阈值v_max，同时在路上也会有道路限速，因此速度条件可以表示为：v_car＜v_max；

加速度条件：车辆的行驶的加速度a_car不能超过加速度的上限a_max，即：a_car＜a_max；

曲率条件：轨迹曲率k_x反应了车辆的转弯效果，若曲率k_x过大，车辆速度过快，容易导致车辆侧翻，曲率可以表示为：k_x＜k_max，k_max为曲率阈值；

碰撞条件：假设以障碍物为中心的安全距离是r_block，车身外接圆的半径是r_car，则车辆与障碍物的碰撞限制可以表示为：

其中(x_x，y_x)为车辆自身的坐标，(x_block，y_block)为障碍物的坐标。

优选的，所述步骤三中的约束条件包括，

乘坐舒适度：转化为对车辆加加速度的评估，即在初始状态时刻t₀到目标状态时刻t₁的加加速度积分，横向加加速度的评估J_d和纵向加加速度的评估J_s以及总的乘坐舒适度可以表示为：

J_comfort＝K_dJ_d+K_sJ_s

其中，K_d、K_s分别表示横向加加速度的评估J_d和纵向加加速度的评估J_s的加权系数；

车辆安全性：车辆安全性评估用车辆碰撞时间t_TTC来描述，当碰撞时间大于碰撞阈值时间，则车辆是安全的，并且碰撞时间越长车辆越安全，车辆安全评估值表示为：

其中：ε是一个正数值，为了避免t_TTC≈0时产生奇异值，v_x为车辆自身的速度，v_block障碍物的速度，由此得到车辆碰撞时间t_TTC的算式如下：

车辆行驶效率：效率的评估由初始状态时刻t₀到目标状态时刻t₁车辆纵向速度与目标速度差值的积分表示：

其中，v_target为目标速度,

为由初始状态时间t₀到目标状态时刻t₁车辆纵向速度。

本发明还提供了一种自动驾驶车辆智能决策系统，采用如前所述的一种自动驾驶车辆智能决策及局部轨迹规划方法，所述自动驾驶车辆智能决策系统包括，

环境感知模块，用于感知车辆及车辆周围环境信息；

智能决策与规划模块，用于结合车辆及周围环境、驾驶员命令等决策车辆如何运动下发行车命令，还承担处理并传递信息的任务；

执行系统模块，用于根据下发的行车命令完成车辆的运行控制。

优选的，所述智能决策与规划模块包括智能决策模块和轨迹规划模块；

所述智能决策模块与环境感知模块、所述轨迹规划模块通过CAN总线进行通讯，所述执行系统模块包括控制各个执行系统的车辆控制模块，所述车辆控制模块与智能决策模块通过CAN总线进行通讯；

轨迹规划模块，进行局部轨迹规划，通过汽车的初始状态与目标状态，考虑汽车的动力学与运动学约束及环境影响，求解满足约束条件的轨迹集合，并在集合中根据成本代价条件选择最合适的轨迹，确定最优轨迹后据此下达相应的行车命令。

本发明的技术效果：本方案通过建立Frenet坐标系与笛卡尔坐标系的相对关系，可以将复杂的非线性轨迹规划问题简化，并通过基于该坐标系进行的多项式插值规划方法进一步降低了轨迹规划的难度，更深层次地降低了对硬件计算能力的要求。

通过多项式插值规划的方法，可以规划出光滑、平顺的轨迹，宏观上看，汽车横纵向的行驶更加平稳。通过对速度、加速度、加加速度建立代价函数，可以筛选出当前状态下的最优轨迹，并能根据需要将道路边界、速度条件、加速度条件，以及乘坐舒适度，车辆安全性等多个约束条件考虑到最优轨迹的选择过程中，满足使用中的不同需要，确保车辆安全的同时改善乘坐舒适度。

附图说明

图1为本发明一种自动驾驶车辆智能决策及局部轨迹规划方法的流程图。

图2为本发明一种自动驾驶车辆智能决策系统的系统示意图。

具体实施方式

下面对照附图，通过对实施例的描述，对本发明具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。

如图1-2所示，本发明提供了一种自动驾驶车辆智能决策及局部轨迹规划方法，包括下列步骤：

步骤一、由道路信息采样给定车辆当前位置及给定的目标位置点；

步骤二、通过Frenet坐标系对轨迹计算进行简化，利用多项式插值出车辆未来的遍历轨迹对应的路径，通过汽车的初始状态与目标状态求解轨迹集合；

具体为：给定车辆当前位置及给定的目标位置点利用6个约束，使用5次多项式进行拟合，具体如下：

若已知t₀时刻的状态[f(t₀)，f′(t₀)，f″(t₀)]^T及t₁时刻的状态[f(t₁)，f′(t₁)，f″(t₁)]^T，将已知条件代入上式并用矩阵的形式表达线性方程组如下：

MA＝F

其中：

A＝[a₀ a₁ a₂ a₃ a₄ a₅]^T

F＝[f(t₀)，f′(t₀)，f″(t₀)，f(t₁)，f′(t₁)，f″(t₁)]^T

该方法预先建立建立Frenet坐标系与笛卡尔坐标的转化关系，本步骤中采用Frenet坐标系分别实现横向轨迹规划和纵向轨迹规划，并综合二者建立横、纵合成轨迹集合。其中横向轨迹规划主要的作用是控制车辆及时变道或避障，但无论如何控制车辆，对于车辆的目标位置我们始终希望：车辆与车道保持平行的状态。纵向轨迹规划主要面向的场景有定速巡航、变道塞车、跟车等。利用Frenet坐标系简化后横向初始配置为

横向终端配置为

纵向初始配置为

纵向终端配置为

因此，可以确定车辆的初始状态d(t₀)＝[d₀，d′₀，d″₀]^T及车辆的目标状态d(t₁)＝[d₁，0，0]^T，同时为了计算方便，令t₀＝0。

与横向轨迹规划类似，当确定了车辆纵向的初始状态s(t₀)＝[s₀，s′₀，s″₀]^T及车辆的目标状态s(t₁)＝[s₁，s′₁，s″₁]^T，同时为了计算方便，令t₀＝0，通过上式便可求解线性方程。根据不同的场景，其目标状态s(t₁)也不同，具体分析如下。

巡航：对于横向目标状态，使用本车的横向速度和更新步长N，计算目标状态的横向偏移，其中横向速度和加速度设置为零，对于纵向，终端速度设置为道路限制速度，终端加速度设置为零，纵向位置不受限制，指定为Nan，最终生成在给定时间范围内与道路速度平滑匹配的轨迹：

其中，

表示目标纵向速度；d_explane表示横向位移，其基本更新方法为：d_explane＝v₀+0.5×d²t(i)，i＝1，2，...，N；其中，v₀表示初始速度。

变道：首先确定本车辆的当前车道，然后检查相邻车道是否存在，对于每个现有车道，目标状态的定义方式与巡航控制行为相同，将终端速度设置为当前速度：

其中，

表示车辆目标速度；d_desLane表示车辆横向目标位置。

跟车：首先确定本车当前车道，根据设定的前车时间范围，预测前车的未来状态，并查找与本车占用同一车道的所有车辆，同时确定哪个是最接近本车的车辆，如果前车不存在，则该功能不会返回任何内容，通过前车的位置和速度，将终端纵向位置减少一定的安全距离，计算出自车辆的目标状态：

其中，s_lead-l_safety表示前车与本车的纵向间隔距离；

表示前车纵向速度，作为本车的纵向目标速度；d_lead表示前车的横向位移，作为本车的横向目标位移；

表示前车的横向速度，作为本车的横向目标速度。

由上述方式计算分别得到对应的横向轨迹集合{d_i(t)|i＝0，1，2...}和纵向轨迹集合{s_i(t)|i＝0，1，2...}。二者结合考虑时间离散{ΔT|t₀，t₀+ΔT，t₀+2ΔT，...，t₁}得到横、纵合成轨迹集合{d_i，j，s_i，j|i，j＝0.1.2...}。

步骤三、对上述轨迹集合进行成本代价评估并按最小原则排序，根据排序结果依次进行是否满足约束条件的检查，不能同时满足所有约束条件时则按排序取下一轨迹重复检查，直至确定同时满足各约束条件的最优轨迹。

成本代价C的评估公式为：{cost(i)＝sort(C_i)|i＝0，1，2，...，N}。

一般情况下，约束条件的检查包括进行边界约束、车辆自身运动参数校核和碰撞检测，具体过程为：选择排序结果的第一条轨迹进行车辆运动参数校核，根据校核结果确定是否为最优轨迹，不满足校核条件则是该轨迹为次优轨迹，否则对该轨迹进行碰撞检测，满足碰撞检测则将该轨迹作为最优轨迹，否则将该轨迹作为次优轨迹，当车辆运动参数校核或碰撞检测得到轨迹为次优轨迹时依次从排序后的轨迹集合中选择下一轨迹重复进行车辆运动参数校核。车辆运动参数校核包括运动学校核、加速度校核、曲率校核和速度校核。碰撞检测主要为检测与障碍物之间的安全距离是否满足要求，边界约束为确定车辆位置是否在道路边界内。

上述校核和检测均为了保证最优轨迹满足相应约束条件，这些约束条件包括：位置条件、速度条件、加速度条件、碰撞条件，具体如下。

位置条件：需要保证车辆始终在车道之内，如果目标位置超出道路边界，车辆将会冲出道路发生危险。位置条件可以表示为：d(s)＜d_rode，其中：d_rode表示道路边界位置。

速度条件：车辆在道路上行驶时，速度v_car不能超过车辆本身的速度阈值v_max，同时在路上也会有道路限速。因此速度条件可以表示为：v_car＜v_max。

加速度条件：车辆的行驶的加速度a_car不能超过加速度的上限a_max，即：a_car＜a_max。

曲率条件：轨迹曲率k_x反应了车辆的转弯效果，若曲率k_x过大，车辆速度过快，容易导致车辆侧翻。曲率可以表示为：k_x＜k_max，k_max为曲率阈值。

碰撞条件。车辆在行驶过程中难免会遇到各种各样的路况，如车辆、行人、路障等。对车辆来说，这些道路上的物体可以全部认为障碍物。所谓碰撞限制，就是汽车规划的路径不能与障碍物发生安全距离的干涉。假设以障碍物为中心的安全距离是r_block，车身外接圆的半径是r_car，则车辆与障碍物的碰撞限制可以表示为：

其中(x_x，y_x)为车辆自身的坐标，(x_block，y_block)为障碍物的坐标。

进一步地，所述轨迹规划单元在规划时还可以考虑车辆的乘坐舒适度、安全性及车辆行驶效率，从而将上述影响因素增加为约束条件。

乘坐舒适度：车辆的加加速度反应了汽车加速度的变化率，宏观上乘客最直观的感受就是乘坐舒适度。若加加速度过大，车辆会急加速或急减速甚至有顿挫感。乘坐舒适度评估可以转化为对车辆加加速度的评估，即在初始状态时刻t₀到目标状态时刻t₁的加加速度积分。横向加加速度的评估J_d和纵向加加速度的评估J_s以及总的乘坐舒适度可以表示为：

J_comfort＝K_dJ_d+K_sJ_s

其中，K_d、K_s分别表示横向加加速度的评估J_d和纵向加加速度的评估J_s的加权系数；

车辆安全性：车辆安全性评估可以用车辆碰撞时间t_TTC来描述。当碰撞时间大于碰撞阈值时间，则车辆是安全的，并且碰撞时间越长车辆越安全。车辆安全评估值可以表示为：

其中：ε是一个正数值，为了避免t_TTC≈0时产生奇异值，v_x为车辆自身的速度，v_block障碍物的速度，由此得到车辆碰撞时间t_TTC的算式如下：

车辆行驶效率：效率的评估由初始状态时间t₀到目标状态时刻t₁车辆纵向速度与目标速度差值的积分表示：

其中，v_target为目标速度,

为由初始状态时间t₀到目标状态时刻t₁车辆纵向速度。

步骤四、获得最优轨迹后对其进行Frenet坐标系到笛卡尔坐标的转换，然后将转换结果用于车辆运动控制。

当决策模块根据上述方法做出决策并下发行车命令时，控制模块将根据与决策模块下发的车辆目标位置、车辆转角等数据控制电机电流大小、方向盘转角等。当遇到突发情况时决策模块下发急停命令，车辆控制模块将停止执行行车命令，并且对车辆紧急制动。

本发明还提供了一种自动驾驶车辆智能决策系统，具有局部轨迹规划功能，包括：环境感知模块、智能决策与规划模块和执行系统模块。其中：

环境感知模块，该模块用于感知车辆及车辆周围环境信息，包括：车辆速度、车辆位置、道路上静态障碍物位置及动态障碍物位置速度等。

所述环境感知模块包括车辆上的GPS定位传感器、激光雷达定位、超声波雷达测速雷达等装置。

智能决策与规划模块，该模块用于结合车辆及周围环境、驾驶员命令等决策车辆如何运动，包括车辆行驶、停止、急停等；同时该模块还承担处理并传递其他模块信息的任务，在完成行为决策后进一步进行轨迹规划，具体通过其中的轨迹规划模块实现，因此可将该模块进一步包括智能决策模块和轨迹规划模块。

所述智能决策模块相当于系统中枢，环境感知模块与智能决策模块通过CAN总线进行通讯，轨迹规划模块与智能决策模块通过CAN总线进行通讯，车辆控制模块与智能决策模块通过CAN总线进行通讯。

轨迹规划模块，通常全局路径规划已经事先给定于汽车，但汽车在行驶过程中会受到各种各样的外界干扰。因此，除了全局路径规划外，还需要局部轨迹规划(二次规划)，以应对复杂突变的环境。该模块将通过汽车的初始状态与目标状态，考虑汽车的动力学与运动学约束及环境影响(如障碍物碰撞等)，求解满足约束条件的轨迹集合，并在集合中根据成本代价条件选择最合适的轨迹。确定最优轨迹后据此下达相应的横纵向控制指令即行车命令。

执行系统模块，用于根据下发的行车命令完成车辆的运行控制，由车辆控制模块与受其控制的转向系统、制动系统、动力系统等执行系统一同构成。

车辆控制模块，该模块主要执行由决策模块下发的行车命令。当决策模块下发行车命令时，控制模块将根据与决策模块下发的车辆目标位置、车辆转角等数据控制电机电流大小、方向盘转角等。当遇到突发情况时决策模块下发急停命令，车辆控制模块将停止执行行车命令，并且对车辆紧急制动。进一步地，所述轨迹规划单元在规划时考虑车辆的边界条件及约束。

转向系统、制动系统、动力系统则为现有车辆的常规技术，在此不做赘述。

本决策系统在工作时，环境感知单元先利用各种传感器来获取自动驾驶车辆周围的运动信息，并将这些信息传输到车载计算机内进行实时决，具体决策时，利用“遍历轨迹+最优搜索法”来进行轨迹规划，该轨迹既包含了自动驾驶车辆的路径信息，又包含了速度信息，从而很好的适应实际复杂工况下的决策规划。得到轨迹信息后，车辆的控制单元会利用相应的控制算法将轨迹信息转化为控制器直接输入的控制量信息，并将这些控制量输入到主动转向电机、制动及油门踏板位移电机上，从而实现车辆的自动驾驶。

上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的发明构思和技术方案进行的各种非实质性的改进，或未经改进将本发明构思和技术方案直接应用于其它场合的，均在本发明保护范围之内。

Claims (6)

1.一种自动驾驶车辆智能决策及局部轨迹规划方法，其特征在于：

步骤一、由道路信息采样给定车辆当前位置及给定的目标位置点；

步骤二、通过Frenet坐标系对轨迹计算进行简化，利用多项式插值出车辆未来的遍历轨迹对应的路径，通过汽车的初始状态与目标状态求解轨迹集合；

所述步骤二中，采用Frenet坐标系分别实现横向轨迹规划和纵向轨迹规划，并综合二者建立横、纵合成轨迹集合；利用Frenet坐标系简化后确定车辆的初始状态d(t₀)＝[d₀，d′₀，d″₀]^T及车辆的目标状态d(t₁)＝[d₁，0，0]^T；确定车辆纵向的初始状态s(t₀)＝[s₀，s′₀，s″₀]^T及车辆的目标状态s(t₁)＝[s₁，s′₁，s″₁]^T；

目标状态根据场景变化包括下列方法确定目标状态s(t₁)：

巡航：对于横向目标状态，使用本车的横向速度和更新步长N，计算目标状态的横向偏移，其中横向速度和加速度设置为零，对于纵向，终端速度设置为道路限制速度，终端加速度设置为零，纵向位置不受限制，指定为Nan，最终生成在给定时间范围内与道路速度平滑匹配的轨迹：

其中，

表示目标纵向速度；d_explane表示横向位移，其基本更新方法为：d_explane＝v₀+0.5×d²t(i)，i＝1，2，...，N；其中，v₀表示初始速度；

变道：首先确定本车辆的当前车道，然后检查相邻车道是否存在，对于每个现有车道，目标状态的定义方式与巡航控制行为相同，将终端速度设置为当前速度：

其中，

表示车辆目标速度；d_desLane表示车辆横向目标位置；

跟车：首先确定本车当前车道，根据设定的前车时间范围，预测前车的未来状态，并查找与本车占用同一车道的所有车辆，同时确定哪个是最接近本车的车辆，如果前车不存在，则该功能不会返回任何内容，通过前车的位置和速度，将终端纵向位置减少一定的安全距离，计算出自车辆的目标状态：

其中，s_lead-l_safety表示前车与本车的纵向间隔距离；

表示前车纵向速度，作为本车的纵向目标速度；d_lead表示前车的横向位移，作为本车的横向目标位移；

表示前车的横向速度，作为本车的横向目标速度；

步骤三、对上述轨迹集合进行成本代价评估并按最小原则排序，根据排序结果依次进行是否满足约束条件的检查，不能同时满足所有约束条件时则按排序取下一轨迹重复检查，直至确定同时满足各约束条件的最优轨迹；

步骤四、获得最优轨迹后对其进行Frenet坐标系到笛卡尔坐标的转换，然后将转换结果下发行车命令用于车辆运动控制。

2.根据权利要求1所述的一种自动驾驶车辆智能决策及局部轨迹规划方法，其特征在于：所述步骤二具体包括：给定车辆当前位置及给定的目标位置点利用6个约束，使用5次多项式进行拟合，具体如下：

若已知t₀时刻的状态[f(t₀)，f′(t₀)，f″(t₀)]^T及t₁时刻的状态[f(t₁)，f′(t₁)，f″(t₁)]^T，将已知条件代入上式并用矩阵的形式表达线性方程组如下：

MA＝F

其中：

A＝[a₀ a₁ a₂ a₃ a₄ a₅]^T

F＝[f(t₀)，f′(t₀)，f″(t₀)，f(t₁)，f′(t₁)，f″(t₁)]^T

确定车辆的初始状态d(t₀)＝[d₀，d′₀，d″₀]^T及车辆的目标状态d(t₁)＝[d₁，0，0]^T，令t₀＝0；确定车辆纵向的初始状态s(t₀)＝[s₀，s′₀，s″₀]^T及车辆的目标状态s(t₁)＝[s₁，s′₁，s″₁]^T，令t₀＝0，通过上式便可求解线性方程；

由上述多项式插值计算分别得到对应的横向轨迹集合{d_i(t)|i＝0，1，2...}和纵向轨迹集合{s_i(t)|i＝0，1，2...}，二者结合考虑时间离散{ΔT|t₀，t₀+ΔT，t₀+2ΔT，...，t₁}得到横、纵合成轨迹集合{d_i，j，s_i，j|i，j＝0，1，2...}。

3.根据权利要求1所述的一种自动驾驶车辆智能决策及局部轨迹规划方法，其特征在于：所述步骤三中的约束条件包括，

位置条件：需要保证车辆始终在车道之内，如果目标位置超出道路边界，车辆将会冲出道路发生危险，位置条件可以表示为：d(s)＜d_rode，其中：d_rode表示道路边界位置；

速度条件：车辆在道路上行驶时，速度v_car不能超过车辆本身的速度阈值v_max，同时在路上也会有道路限速，因此速度条件可以表示为：v_car＜v_max；

加速度条件：车辆的行驶的加速度a_car不能超过加速度的上限a_max，即：a_car＜a_max；

曲率条件：轨迹曲率k_x反应了车辆的转弯效果，若曲率k_x过大，车辆速度过快，容易导致车辆侧翻，曲率可以表示为：k_x＜k_max，k_max为曲率阈值；

碰撞条件：假设以障碍物为中心的安全距离是r_block，车身外接圆的半径是r_car，则车辆与障碍物的碰撞限制可以表示为：

其中(x_x，y_x)为车辆自身的坐标，(x_block，y_block)为障碍物的坐标。

4.根据权利要求1所述的一种自动驾驶车辆智能决策及局部轨迹规划方法，其特征在于：所述步骤三中的约束条件包括，

乘坐舒适度：转化为对车辆加加速度的评估，即在初始状态时刻t₀到目标状态时刻t₁的加加速度积分，横向加加速度的评估J_d和纵向加加速度的评估J_s以及总的乘坐舒适度可以表示为：

J_comfort＝K_dJ_d+K_sJ_s

其中，K_d、K_s分别表示横向加加速度的评估J_d和纵向加加速度的评估J_s的加权系数；

车辆安全性：车辆安全性评估用车辆碰撞时间t_TTC来描述，当碰撞时间大于碰撞阈值时间，则车辆是安全的，并且碰撞时间越长车辆越安全，车辆安全评估值表示为：

其中：ε是一个正数值，为了避免t_TTC≈0时产生奇异值，v_x为车辆自身的速度，v_block障碍物的速度，由此得到车辆碰撞时间t_TTC的算式如下：

车辆行驶效率：效率的评估由初始状态时刻t₀到目标状态时刻t₁车辆纵向速度与目标速度差值的积分表示：

其中，v_target为目标速度，

为由初始状态时间t₀到目标状态时刻t₁车辆纵向速度。

5.一种自动驾驶车辆智能决策系统，其特征在于：采用如权利要求1-4中任一所述的一种自动驾驶车辆智能决策及局部轨迹规划方法，所述自动驾驶车辆智能决策系统包括，

环境感知模块，用于感知车辆及车辆周围环境信息；

智能决策与规划模块，用于结合车辆及周围环境、驾驶员命令等决策车辆如何运动下发行车命令，还承担处理并传递信息的任务；

执行系统模块，用于根据下发的行车命令完成车辆的运行控制。

6.根据权利要求5所述的一种自动驾驶车辆智能决策系统，其特征在于：所述智能决策与规划模块包括智能决策模块和轨迹规划模块；

所述智能决策模块与环境感知模块、所述轨迹规划模块通过CAN总线进行通讯，所述执行系统模块包括控制各个执行系统的车辆控制模块，所述车辆控制模块与智能决策模块通过CAN总线进行通讯；

轨迹规划模块，进行局部轨迹规划，通过汽车的初始状态与目标状态，考虑汽车的动力学与运动学约束及环境影响，求解满足约束条件的轨迹集合，并在集合中根据成本代价条件选择最合适的轨迹，确定最优轨迹后据此下达相应的行车命令。

Images