CN114261399A - 一种冰雪路面下智能驾驶汽车决策规划方法 - Google Patents

Abstract

本发明涉及一种冰雪路面下智能驾驶汽车决策规划方法,其主要包括以下步骤：步骤一：考虑冰雪环境下换道安全距离和驾驶员不满意度累计设计一种分层状态机的决策方法；步骤二：基于冰雪环境改进混合A^*规划算法规划出预期路径；步骤三：采用三次样条插值法对生成的路径进行优化,规划出平滑轨迹。

Description

一种冰雪路面下智能驾驶汽车决策规划方法

技术领域

本发明涉及一种人性化先进辅助驾驶系统(ADAS)的设计，特别是关于一种基于环境的智能驾驶汽车决策规划方法。

背景技术

冰雪环境长期存在于我国东北,西北等广袤的地域中，寒冷地区的车辆长期存在于冰雪环境这种特殊工况中。在东北和西北等地区，冬季漫长，气温低，降雪量大，在清除积雪之前，汽车驾驶员的驾驶条件非常差。在对道路进行清雪后，也会留下一些残雪。在冰雪环境的作用下，交通运行环境恶劣，路面附着系数仅为正常路面的1/8～1/4，驾驶员的驾驶舒适性变差，易产生较大的精神压力和疲惫等不适感。在冰雪环境下，交通运行效率和行车安全性低于非冰雪路面，邢恩辉等人通过对哈尔滨市公路交通数据的分析，得出了冰雪环境下车辆的平均行驶速度要低于非冰雪路面20％。这些因素导致了很多安全隐患，邱琳等人通过对美国1967-2005年交通事故的数据分析，发现冰雪环境下的事故率明显高于雨水，雾等不利天气，这些行驶的不利因素也使得冰雪环境下的智能驾驶的研究更有意义。

智能驾驶中的决策规划模块是衡量和评价智能驾驶汽车智能化的核心的指标之一，它的主要任务是在接收到传感器的各种感知信息之后，对当前环境作出分析，然后对底层控制模块下达指令。基于规则的决策方法是一种经典方法，最早在2004年及之后的历届DAPPA无人驾驶挑战赛中被广泛应用。麻省理工大学的Talos决策系统采用串联状态机结构，该自动驾驶系统分为定位与导航、障碍物检测、车道线检测、路标识别，可行驶区域地图构建、运动规划、运动控制等模块规划模块采用快速探索随机树算法生成可行路径，该系统的优点是可以有效应对先验信息较差的高度动态环境，缺点是采用串联结构，场景覆盖不完全。为了克服上述缺点斯坦福Junior无人车首次提出采用并联式结构状态机，以模块方式处理每个状态，整个系统可以快速灵活地响应输入。然而在复杂条件下，由于遍历状态的增加，算法结构变得庞大，使得状态之间的划分和解决状态冲突变得困难。而并联结构适用于更复杂的工况。与串联结构相比，并联结构具有场景遍历范围更广、复杂功能组合更容易实现、模块化和可扩展性更好等优点。缺点是场景缺乏深度，并且环境中的细微变化会导致决策错误。有学者结合了串联式和并联式结构的优点，陈永尚提出了一种将遵守交通法规和避让交通参与物解耦的分层决策方法，先根据交规选取可以采用的动作，形成一个候选行为集，再综合碰撞风险，交通规则等因素，采用隐马尔可夫模型推测他车驾驶员意图，行驶效率，宏观推荐路径等评估方法综合决策出一个最优行为，再根据该行为与当前区域和目标区域来进行路径规划，并给出推荐车速。

智能驾驶汽车规划方法主要分为基于采样的规划和基于搜索的规划。基于的采样的方法存在求解复杂度高的问题，并且在一些边界场景采样效率低。Zhang Yu等人应用了混合A*算法和共轭梯度下降路径平滑器用于通过以动力学扩展四个节点模式来生成平滑路径，相对于A*来说引入了运动学模型从而使得图得每条边也就是生成得出的轨迹更加平滑，其缺点是缺乏避障模块并且未考虑周车障碍物。Jorge Godoy等人采用基于路径最优决策方法设计了一种局部路径规划模块，该模块与全局路径规划模块交互，并具有生成平滑拟合轨迹，规避静态和动态障碍物等功能，其缺点是存在时间复杂度过高的问题。

发明内容

在以往的智能驾驶汽车决策规划研究中，很少出现针对冰雪混合路面这一特殊工况的方法，本发明的目的是提供一种针对冰雪环境的决策规划方法。

为实现上述目的，本发明采取以下技术方案：

一种冰雪路面下智能驾驶汽车决策规划方法，主要涉及的步骤如下:

步骤一：考虑冰雪环境下换道安全距离和驾驶员不满意度累计设计一种分层状态机的决策架构；

步骤二：基于冰雪环境改进混合A^*规划算法规划出预期路径，

步骤三：采用三次样条插值法对规划出的预期路径进行优化,从而得出平滑轨迹；

2.设计一种分层状态机的决策架构包括以下几个步骤:

1)设计一种状态机决策模型，该模型分为两层，上层分为两种状态分别为，正常行驶，路口。下层分为三种状态,分别为车道保持、准备换道、换道、停止/等待。两层状态机的状态储存在一个键值对内,表明车辆当前所处状态，公式(1)给出了状态机的状态集合，

(正常行驶，路口，车道保持，准备换道，换道，停止/等待} (1)

决策模型的状态转移关系具体如下：

上层：正常行驶状态在驶入路口时转换为路口状态，路口状态在驶出路口后转换为正常行驶状态；

下层：停止状态在启动后转换为车道保持状态，当不满意度达到阈值转换为准备换道状态，准备换道状态下根据当前是否需要换道、是否满足安全距离和是否会发生碰撞进行判断，当条件满足时由准备换道状态转换为换道状态(包括左换道和右换道)；车辆到达目标车道后或者轨迹规划失败、车辆无法到达目标车道，由换道状态转换为车道保持状态，车道保持状态下达到终止条件制动时进入停止/等待状态；

2)考虑冰雪环境下，由于冰雪路面附着条件差，存在低速前车等因素，车辆的行驶的连续性和速度无法满足驾驶员期望，驾驶员的不满意度会逐渐累计从而产生超车意图，得出驾驶员不满意度累计公式(2)。

其中，D(k)为此时的不满意度；D(k-1)为上一时间点的不满意度；v_pre为期望车速；v_des为当前行驶车速，θ为行驶连续性加权系数，Count_avg为平均每公里停车次数，T为采样时间。

3)考虑换道或超车的安全性，能否换道或超车，取决于相邻车道上各交通参与者之间的前后位置即换道安全距离。考虑冰雪环境下安全制动距离，可以分为制动作用距离和制动行驶距离，前车间隔距离，如公式(3)。

其中，s为安全制动距离，s₀为制动后前后距离之差，a_bmax为制动减速度的最大值，s₀为前后车距离之差，t₁,t₂为制动时间，v为自车速度。

3.基于冰雪环境改进混合A^*规划算法包括以下几个步骤：

1)冰雪路面和正常路面混合的工况下，将A^*算法2D搜索扩展到4D空间[x,y,θ,D]，其中θ是本车的航向，D表示车辆是前进还是后退；对规划模型输入冰雪路面的位置信息和周车轨迹预测信息，如果搜索出的节点被冰雪覆盖，则该节点的代价会减小；如果混合A^*搜索出的节点与当前节点之间的轨迹与周车预测轨迹重合，则这条轨迹上的累计代价会减小。

2)如图3所示，自车与他车距离大于自车与前车安全距离并且满足自车与他车车辆速度期望，触发换道条件，自车通过混合A^*算法搜索出两条轨迹a,b。由于轨迹b节点会经过冰雪路面，所以总体代价会变小，所以规划模块会输出a轨迹到控制模块。如图4所示，自车触发换道条件，搜索出两条轨迹,a,b,而由于轨迹a，b在某个时间段上会与自车右侧的他车预测出的轨迹产生重合，即发生碰撞，所以a，b两条轨迹都不能选用，将会向状态机模块返回规划失败的状态，状态机则转移至车道保持状态。

4.采用三次样条插值法对生成的路径进行优化包括以下几个步骤：

1)将路径分割成若干部分，每个小部分用插值得到不同的函数，用这些插值出的函数代表整体曲线。

2)为每个部分构造三次方程,即S_i(x)＝a_i+b_i(x-x_i)+c_i(x-x_i)²+d_i(x-x_i)³。

其中，S_i(x)为分段插值函数(i＝0,1,…,n)，a_i,b_i,c_i,d_i为插值区间上的未知数，x_i为插值点。

3)让方程S_i(x)满足四个插值条件。条件一，函数必须穿过所有已知节点。条件二，在节点处0阶导数必须连续。条件三，除了第一个节点和最后一个节点之外的所有节点处1阶连续。条件四，除了第一节点和最后一个节点处2阶连续。

本发明由于采取以上技术方案，相比于现有方法，具有以下优点：

1.本发明设计了一种分层状态机架构，将上下层状态解耦，上层为场景，下层为场景内的子状态，上层状态机作为基类，下层状态机作为派生类，该框架具有方便扩展的优点，当需要应对新的场景或者更细致处理场景任务时，可以在原有的框架中方便的添加新状态，对原有各种状态的影响较小，采用层次状态机又避免了需要设定的状态转移条件的数目。

2.本发明涉及的分层状态机架构中，引入了冰雪环境下的约束如不满意度累计换道机制和行驶安全距离，增加了决策规划模型在冰雪环境行驶下的安全性和舒适性。

3.本发明基于冰雪环境改进混合A^*规划算法规划出预期路径并采用三次样条插值法优化轨迹，该算法引入冰雪路面的信息和周车轨迹预测信息，考虑了周车行为和冰雪混合路面下的特殊工况，从而规划出一条安全和相对平滑的轨迹。

附图说明

图1是分层状态机决策模型示意图

图2是冰雪混合路面示意图

图3是轨迹规划场景1

图4是轨迹规划场景2

图5是轨迹规划示意图

图6是设定的仿真环境

图7是仿真器设定参数

图8是跟驰工况下自车和前车的纵向位移

图9是跟驰工况下自车和前车的纵向位移差

图10是跟驰工况下自车速度曲线

图11是跟驰工况下自车加速度曲线

图12是超车工况下换道状态值

图13是超车工况下自车速度曲线

图14是超车工况下自车加速度曲线

具体实施方式

以下进一步说明本发明的具体内容及其实施方式。

本发明涉及一种针对冰雪路面下的智能驾驶汽车的决策规划方法，其主要包括以下步骤：

步骤一：考虑冰雪环境下换道安全距离和驾驶员不满意度累计设计一种分层状态机的决策方法；步骤二：基于冰雪环境改进混合A^*规划算法生成路径；步骤三：采用三次样条插值法对生成的路径进行优化，规划出平滑轨迹。

1基于规则的状态机决策模型建立

本发明决策模型采用分层状态机，上层分为两种状态分别为，正常行驶和路口，下层分为四种状态，分别为车道保持、准备换道、换道和停止/等待，状态机模型如图1。状态之间的转移由自车与前车在冰雪环境下的安全距离,自车在冰雪路面上的期望速度,与规划模块能否规划出合理的轨迹所决定。

{正常行驶，路口，车道保持，准备换道，换道，停止/等待} (1)

决策模型的状态转移关系具体如下：

上层：正常行驶状态在驶入路口时转换为路口状态，路口状态在驶出路口后转换为正常行驶状态；

下层：停止状态在启动后转换为车道保持状态，当不满意度达到阈值转换为准备换道状态，准备换道状态下根据当前是否需要换道、是否满足安全距离和是否会发生碰撞进行判断，当条件满足时由准备换道状态转换为换道状态(包括左换道和右换道)；车辆到达目标车道后或者轨迹规划失败、车辆无法到达目标车道，则由换道状态转换为车道保持状态，车道保持状态下达到终止条件制动时进入停止/等待状态。

本发明中的分层状态机架构，将上下层状态解耦。上层为场景，下层为场景内的子状态；上层状态机作为基类，下层状态机作为派生类。该框架具有方便扩展的优点，当需要应对新的场景或者更细致处理场景任务时，可以在原有的框架中方便的添加新状态，对原有各种状态的影响较小，采用层次状态机又避免了需要设定的状态转移条件的数目。所涉及的分层状态机架构中，引入了冰雪环境下的约束如不满意度累计换道机制和行驶安全距离，增加了决策规划模型在冰雪环境行驶下的安全性和舒适性。

1.1不满意度累积

考虑冰雪环境下，由于冰雪路面附着条件差,存在低速前车等因素，车辆的行驶的连续性和速度无法满足驾驶员期望，当汽车在冰雪路面行驶时，驾驶员不会因为突然加速或减速而有任何变道的意图。由于冰雪路面附着条件差，车辆的速度不会太高，因此车辆在冰雪路面变道的意愿较高。大多是由于前车长期低速行驶，使后车乘员得不到速度的满足。对车速的累计不满程度设置在上层的任务文件中或通过其他方式设置，设置了对行驶过程中车辆速度的期望值。但是在实际驾驶过程中，车辆会以非常慢的速度跟随前车。确保行车安全，但未达到预期行车速度。随着时间的推移，车速不满的累积程度会越来越高。驾驶员的不满意度会逐渐累计从而产生超车意图，得出驾驶员不满意度累计公式(2)。

其中,D(k)为当前时刻的不满意度；D(k-1)为上一时刻的不满意度；v_pre为期望车速；v_des为当前行驶车速，θ为行驶连续性加权系数，Count_avg为平均每公里停车次数，T为采样周期。

1.2自车与前车安全距离

车辆跟前车的最小安全距离是指当车辆在道路上行驶时，由于前车突然出现的情况而发生紧急停车。车辆可以在有效安全距离内制动，而不会造成交通事故。目标车辆与车道前方车辆的行驶距离应超过最小安全距离，以防止发生危险。最小安全距离可从制动距离获得。根据对汽车制动性能的分析，距离与汽车的行驶安全性有着直接的关系。从整个制动过程来看，其包括两部分：制动作用时间的行驶距离和连续制动的行驶距离。

其中，a_bmax为制动减速度的最大值，s₀为前后车距离之差，t₁,t₂为制动时间，v为自车速度。

根据前述自车与前车安全距离，与车辆速度期望公式的建立，可以得出状态机模型的转移方法。

2轨迹规划模型建立

规划模型建立的思路是首先采用改进混合A^*算法确定车辆的未来位置，并采用三次样条插值法的轨迹规划模型拟合出一条曲线供控制器进行解算。

2.1混合A^*规划方法

混合A^*算法分为两个阶段,第一个阶段其实是对传统的A^*算法进行改进,与之不同的是，混合A^*是在连续坐标系下进行启发式搜索，并且能够保证生成的轨迹满足车辆非完整性约束，但算法运行过程中该路径并不一定是全局最优的，尽管如此，这条路径是在全局最优解的“附近”。传统A^*与混合A^*两种算法都是基于网格世界的,A^*是赋予每个网格的中心点相应的损失并且算法只访问这些中心点,而混合A^*是先在这些网格中挑选满足车辆3D连续状态的点，并将损失赋值给这些点。不同于图搜索算法A^*，混合A^*考虑运动学模型，将A^*算法2D搜索扩展到4D空间[x,y,θ,D]，其中θ是本车的航向，D表示车辆是前进还是后退。在图中该搜索方法由六个搜索方向组成，圆弧半径为车辆的最小转弯半径如图5。

2.2针对特定工况的轨迹规划

本发明针对冰雪路面和正常路面混合的工况如图2进行轨迹规划模型的建立。传统的混合A^*方法没有考虑到冰雪路面与周车的轨迹，本发明对经典混合A^*算法进行改进，对规划模型输入冰雪路面的位置信息和周车轨迹预测信息。如果搜索出的节点被冰雪覆盖，则该节点的代价会减小。如果混合A^*搜索出的节点与当前节点之间的轨迹与周车预测轨迹重合，则这条轨迹上的累计代价会减小。

如图3所示,自车与他车距离大于自车与前车安全距离并且满足自车与他车车辆速度期望，触发换道条件,自车通过混合A^*算法搜索出两条轨迹,分别为轨迹a,轨迹b。由于轨迹b节点会经过冰雪路面，所以总体代价会变小，所以规划模块会输出a轨迹到控制模块。如图4所示，自车触发换道条件，搜索出两条轨迹,a,b,而由于轨迹a，b在某个时间段上会与自车右侧的他车预测出的轨迹产生重合，既发生碰撞，所以a，b两条轨迹都不能选用，将会向状态机模块返回规划失败的状态，状态机则保持跟驰。

2.3三次样条采样法

由于混合A^*规划出的路径不平滑，部分路径[x,y]采用基于采用三次样条插值法的轨迹规划模型输出。对智能驾驶汽车进行轨迹规划。根据车辆的初始状态和目标状态，规划换道轨迹，使车辆在指定的时间到达目标位置。

采用三次样条插值法对生成的路径进行优化包括以下几个步骤：

1)将路径分割成若干部分，每个小部分用插值得到不同的函数，用这些插值出的函数代表整体曲线。

2)为每个部分构造三次方程,如式(4)。

S_i(x)＝a_i+b_i(x-x_i)+c_i(x-x_i)²+d_i(x-x_i)³ (4)

其中，S_i(x)为分段插值函数(i＝0,1,…,n)，a_i,b_i,c_i,d_i为插值区间上的未知数，x_i为插值点。

3)让方程S_i(x)满足四个插值条件。条件一，函数必须穿过所有已知节点。条件二，在节点处0阶导数必须连续。条件三，除了第一个节点和最后一个节点之外的所有节点处1阶连续。条件四，除了第一节点和最后一个节点处2阶连续。

需要找出4n个方程来求解4n个未知数,首先由于所有点必须满足插值条件S(x_i)＝y_i(i＝0,1,…,n),除了两个端点，所有n-1个内部点的每个点都满足S_i(x_i+1)＝y_i+1,前后两个分段三次方程，则有2(n-1)个方程，再加上两个端点分别满足第一个和最后一个三次方程，则总共有2n个方程；其次n-1个内部点的一阶导数应该是连续的，即在第i区间的末点和第i+1区间的起点是同一个点，它们的一阶导数应该也相等，则有n-1个方程，另外内部点的二阶导数也要连续，即现在总共有4n-2个方程，其他两个方程由通过边界条件可得。

边界条件采用自然边界条件，指定端点的二阶导数为0，即S”(x₀)＝S”(x_n)＝0。将插值点带入插值函数，可得出a_i＝y_i，用h_i＝x_i+1-x_i表示步长，由S_i(x_i+1)＝y_i+1可得出式(5)

由S′_i(x_i+1)＝S′_i+1(x_i+1)可以得出式(6)。

S′_i+1(x_i+1)＝b_i+1+2c_i(x_i+1-x_i+1)+3d_i(x_i+1-x_i+1)²＝b_i+1 (6)

由S″_i(x_i+1)＝S″_i+1(x_i+1)可推出2c_i+6h_id_i＝2c_i+1。设m_i＝S”_i(x_i)＝2c_i则得出式(7)。

将a_i,c_i,d_i带入到式(6)中，可得式(8)。

在自然边界条件时，m₀＝0，m_n＝0。由式(8)可以得出一个以m为未知数的线性方程组如式(9)。

左侧的系数矩阵为严格对角占优矩阵，故线性方程组有唯一解，将方程组解带入式(4)得：

本发明基于冰雪环境改进混合A^*规划算法规划出预期路径并采用三次样条插值法优化轨迹，该算法引入冰雪路面的信息和周车轨迹预测信息，考虑了周车行为和冰雪混合路面下的特殊工况，从而规划出一条安全和相对平滑的轨迹。

3观测数据采集与筛选

3.1模拟驾驶仿真实验设计

本发明以CarMaker车辆动力学仿真软件为基础搭建的模拟驾驶器作为实验平台，设计模拟驾驶实验，采用多种工况对决策规划算法进行验证，采集驾驶行为、车辆状态以及道路环境信息。该模拟驾驶平台的主要软硬件构成如下所示：

模拟驾驶器实物为，罗技G27套件包含油门踏板、制动踏板、离合踏板、档把以及方向盘等与实车相同的驾驶位结构。罗技G27方向盘提供了力反馈技术，为驾驶员提供路感信息，该反馈力比例系数的大小也可以进行设置。通过CarMaker主界面中的IPGRoad模块可以自定义场景，本发明通过IPGRoad建立了直行-环岛-直行工况。在CarMaker软件的CIT中对车辆参数进行设定,如图7所示，通过该界面可对车辆相关参数以及行驶策略仿真进度进行控制，车辆选择为AudiR8，轮胎等参数选择默认值，由于实验设定仅由驾驶员驾驶，所以这里将载荷设定为60kg。实验车道为三车道城市道路，随机在车道上插入冰雪路面，路面摩擦系数为0.28。路况如图6所示，浅灰色路面为冰雪覆盖路面。

3.1.1跟驰工况

前车以30km/h行驶，并设置为定速巡航，前车位于本车前方15m，自车从静止起步跟驰前车，期望速度设定为30km/h，并保持自车与前车距离,自车从静止加速到行驶速度为30km/h,并控制自车速度稳定。

自车的决策为状态机决定,在跟驰过程中，自车保持定速巡航。图8为自车与前车的纵向位移。前车从静止加速到以30km/h速度行驶，图10为自车的速度曲线。

由公式(3)可以计算出为满足安全要求跟驰距离需要在20m以上。由图9可知,前车与自车的行驶距离先缩小后增大,最后满足跟驰安全距离。加速度大小如图11,最小为-3.9,最大为1.0，满足舒适性要求和安全性要求。

3.1.2超车工况

设置两辆前方车辆以20km/h定速巡航，相距50m，自车与前车之间相距50m，由自车决策规划模块决定是否选择换道，当不满意度累计达到设定值则产生换道意图，自车进行换道决策并进行路径规划。在行驶过程中,前车不满意度逐渐积累达到阈值,状态机由车道保持切换为换道状态如图12，自车速度如图13，自车加速度如图14。最小为-4.3,最大为2.0，满足舒适性要求和安全性要求。

Claims (5)

1.一种冰雪路面下智能驾驶汽车决策规划方法，主要涉及的步骤如下:

步骤一：考虑冰雪环境下换道安全距离和驾驶员不满意度累计设计一种分层状态机的决策架构；

步骤二：基于冰雪环境改进混合A^*规划算法规划出预期路径，

步骤三：采用三次样条插值法对规划出的预期路径进行优化,从而得出平滑轨迹；

步骤一种所述的状态机决策模型，该模型分为两层，上层分为两种状态分别为，正常行驶和路口，下层分为四种状态,分别为车道保持、准备换道、换道和停止/等待；两层状态机的状态储存在一个键值对内,表明车辆当前所处状态，公式(1)给出了状态机的状态集合，

(正常行驶，路口，车道保持，准备换道，换道，停止/等待} (1)

决策模型的状态转移关系具体如下：

上层：正常行驶状态在驶入路口时转换为路口状态，路口状态在驶出路口后转换为正常行驶状态；

下层：停止状态在启动后转换为车道保持状态，当不满意度达到阈值转换为准备换道状态，准备换道状态下根据当前是否需要换道、是否满足安全距离和是否会发生碰撞进行判断，当条件满足时由准备换道状态转换为换道状态；车辆到达目标车道后或者轨迹规划失败、车辆无法到达目标车道，由换道状态转换为车道保持状态，车道保持状态下达到终止条件制动时进入停止状态；

步骤二中所述的基于冰雪环境改进混合A^*规划算法是在冰雪路面和正常路面混合的工况下，将A^*算法2D搜索扩展到4D空间[x,y,θ,D]，其中θ是本车的航向，D表示车辆是前进还是后退；对规划模型输入冰雪路面的位置信息和周车轨迹预测信息，如果搜索出的节点被冰雪覆盖，则该节点的代价会减小；如果混合A^*搜索出的节点与当前节点之间的轨迹与周车预测轨迹重合，则这条轨迹上的累计代价会减小。

2.根据权利要求1所述的冰雪路面下智能驾驶汽车决策规划方法，其特征在于，驾驶员不满意度累计方法具体如下：

驾驶员不满意度累计公式(2)；

其中，D(k)为此时的不满意度；D(k-1)为上一时间点的不满意度；v_pre为期望车速；v_des为当前行驶车速，θ为行驶连续性加权系数，Count_avg为平均每公里停车次数，T为采样时间。

3.根据权利要求1所述的冰雪路面下智能驾驶汽车决策规划方法，其特征在于，所述的换道安全距离，可以分为制动作用距离和制动行驶距离，前车间隔距离，如公式(3)，

其中，s为安全制动距离，s₀为制动后前后距离之差，a_bmax为制动减速度的最大值，s₀为前后车距离之差，t₁,t₂为制动时间，v为自车速度。

4.根据权利要求1所述的冰雪路面下智能驾驶汽车决策规划方法，其特征在于，步骤二基于冰雪环境改进混合A^*规划算法规划出预期路径的具体方法如下：

自车与他车距离大于自车与前车安全距离并且满足自车与他车车辆速度期望，触发换道条件，自车通过混合A^*算法搜索出两条轨迹a,b；由于轨迹b节点会经过冰雪路面，所以总体代价会变小，所以规划模块会输出a轨迹到控制模块；自车触发换道条件，搜索出两条轨迹,a,b,而由于轨迹a，b在某个时间段上会与自车右侧的他车预测出的轨迹产生重合，即发生碰撞，所以a，b两条轨迹都不能选用，将会向状态机模块返回规划失败的状态，状态机则转移至车道保持状态。

5.根据权利要求1所述的冰雪路面下智能驾驶汽车决策规划方法，其特征在于，步骤三：采用三次样条插值法对规划出的预期路径进行优化,从而得出平滑轨迹的方法具体如下：

1)将路径分割成若干部分，每个小部分用插值得到不同的函数，用这些插值出的函数代表整体曲线；

2)为每个部分构造三次方程,即S_i(x)＝a_i+b_i(x-x_i)+c_i(x-x_i)²+d_i(x-x_i)³；

其中，S_i(x)为分段插值函数(i＝0,1,…,n)，a_i,b_i,c_i,d_i为插值区间上的未知数，x_i为插值点；

3)让方程S_i(x)满足四个插值条件；条件一，函数必须穿过所有已知节点；条件二，在节点处0阶导数必须连续；条件三，除了第一个节点和最后一个节点之外的所有节点处1阶连续；条件四，除了第一节点和最后一个节点处2阶连续。

Images