CN113581181B - 智能车辆超车轨迹的规划方法 - Google Patents

Abstract

本发明公开了一种智能车辆超车轨迹的规划方法，包括(1)建立智能车辆的超车轨迹候选集；(2)利用评价函数确定所述超车轨迹候选集中每一条候选轨迹的代价值，并选取代价值最小的候选轨迹作为超车轨迹。本发明的规划方法保证了参考轨迹无冲突且风险最小、提高了超车的安全性并改善了驾驶员和乘客的舒适性。

Description

智能车辆超车轨迹的规划方法

技术领域

本发明涉及智能驾驶系统的规划领域，尤其涉及一种智能车辆超车轨迹的规划方法。

背景技术

随着我国汽车数量不断增多，交通工况极其严峻，交通事故频频发生，其中最常见的行为事故就是超车。智能车辆作为一个集环境感知、规划和决策以及控制于一体的现代交通工具平台，最重要的就是保证乘员的安全，但是智能车辆行驶在复杂的环境中，很难准确预测前方车辆的行为，因而智能车辆需要规划出一条可靠的超车轨迹，对提高超车安全性有重要作用。

申请号为201910404975.3的中国专利申请公开了一种无人车自动超车轨迹规划方法，该方法先通过采集智能车辆的运动信息、驾驶员的超车意图和激光雷达得到的道路信息判断是否满足超车条件，然后构建基于三阶的贝塞尔曲线的超车轨迹参数方程，以确定超车轨迹，最后通过满足防侧滑约束、横向位置约束、并行超越时间约束和乘客舒适性约束对轨迹进行评估，筛选出最佳超车轨迹。申请号为201810337901.8的中国专利申请公开了一种结构化道路的智能车辆平滑轨迹规划方法，该方法先通过传感器获得前方道路信息并以碰撞时间作为换道条件，然后检测超车车道的车辆状态、建立局部栅格地图并规划轨迹，最后以路径长短、两车之间的安全距离以及车辆在目标点的状态是否靠近车道中央作为评价指标，选出最优超车轨迹。

然而，虽然上述第一个专利申请以碰撞时间做超车决策，但是在生成超车轨迹时，着重考虑的是车辆动力学、舒适性以及超车的效率，没考虑所生成轨迹的潜在碰撞风险成本，因而导致超车的安全性不高。另外，上述第二个专利申请仅将车辆之间的距离作为轨迹与其它车辆的安全参考，没有对这一距离做潜在的碰撞风险分析，因而超车的安全性也不高。

发明内容

本发明为解决上述现有技术中的问题，提出一种智能车辆超车轨迹的规划方法，该方法保证了超车轨迹无冲突且风险最小、提高了超车的安全性并改善了驾驶员和乘客的舒适性。

为实现上述目的，本发明提供了一种智能车辆超车轨迹的规划方法，包括如下步骤：

建立智能车辆的超车轨迹候选集；

利用评价函数确定所述超车轨迹候选集中每一条候选轨迹的代价值，并选取代价值最小的候选轨迹作为超车轨迹。

进一步地，建立智能车辆的超车轨迹候选集的步骤包括：

(1)使用frenet坐标系，将从智能车辆的当前位置沿道路方向向前预瞄一定距离之后的每个端点的位置分解为纵向位移s′和横向位移d′(k)，其中，s′和d′(k)通过下式计算：

在式(1-1)中，s′为沿着参考线的方向从参考线的起始点到所述端点在参考线上的投影点之间的曲线距离，s为沿着参考线的方向从参考线的起始点到智能车辆的当前位置在参考线上的投影点F之间的曲线距离，Δs为沿着参考线的方向从所述投影点F到所述端点在参考线上的投影点之间的曲线距离，Δs_min为Δs的最小值，v为智能车辆的速度，t_pre为预瞄时间阈值；

式(1-2)为智能车辆正常行驶情况下横向位移的计算公式，在式(1-2)中，d′(k)为每个所述端点到参考线的法向距离，d_w为道路的宽度，N表示道路的宽度被等距离分割成N段且N为正整数，k≤N且k为正整数；

式(1-3)为智能车辆在超车决策下横向位移的计算公式，在式(1-3)中，d′(k)为每个所述端点到参考线的法向距离，k₁为沿与道路正交的方向上的增益系数，k₂为区分不同状态的行驶因子，d_w为道路的宽度，N表示道路的宽度被非等距离分割成N段且N为正整数；

(2)对每个所述端点添加两个第一控制点，第一控制点的纵向位移和横向位移通过式(1-4)计算：

在式(1-4)中，s′_n为沿着参考线的方向从参考线的起始点到第一控制点在参考线上的投影点之间的曲线距离，s′为沿着参考线的方向从参考线的起始点到所述端点在参考线上的投影点之间的曲线距离，k₃为第一控制点之间的距离影响因子，d′_n(k)为每个第一控制点到参考线的法向距离，d′(k)为每个所述端点到参考线的法向距离；

(3)在笛卡尔坐标系中对智能车辆的当前位置处添加两个第二控制点并将智能车辆的当前位置的笛卡尔坐标和第二控制点的笛卡尔坐标转换为frenet坐标，第二控制点的笛卡尔坐标通过式(1-5)计算：

在式(1-5)中，X′_n为第二控制点的横坐标，Y′_n为第二控制点的纵坐标，X为智能车辆的当前位置的横坐标，Y为智能车辆的当前位置的纵坐标，k′₃为第二控制点之间的距离影响因子，

为智能车辆的方位角；

(4)基于智能车辆的当前位置的frenet坐标、两个所述第二控制点的frenet坐标、所述端点的frenet坐标以及两个所述第一控制点的frenet坐标，使用五次样条曲线进行拟合，得到多条超车候选轨迹，从而获得智能车辆的超车轨迹候选集。

优选地，所述参考线为道路中心线。

优选地，按照优先级从高到低的顺序选取碰撞风险成本、车辆动力学成本和路径偏好成本组成评价函数，所述评价函数为：

E＝E_{R_ij}+E_a+E_p (2-1)

在式(2-1)中，E为超车候选轨迹的代价值，E_{R_ij}为超车候选轨迹的碰撞风险成本，E_a为超车候选轨迹的车辆动力学成本，E_p为超车候选轨迹的路径偏好成本。

进一步地，根据式(2-2)计算超车候选轨迹的碰撞风险成本：

在式(2-2)中，E_{R_ij}为超车候选轨迹的碰撞风险成本，r_ij为将超车候选轨迹所需要的空间投影到占据栅格地图后最外侧的网格到障碍物的距离，r_max为距离的最大值。

进一步地，在计算超车候选轨迹的碰撞风险成本之前还包括如下步骤：

(i)碰撞检测：通过超车候选轨迹上的路径点反求相交于该路径点的三个圆的中心，并在三个圆内采样，采样所占据的空间相当于智能车辆行驶在超车候选路径上所需要的空间，也是每个超车候选路径的所需空间，然后将代表超车候选路径上的点和代表障碍物的点投影到占据栅格地图中并将超车候选路径所需空间的地图与障碍物所需空间的地图进行匹配，如果匹配的结果为真，则占据栅格地图中的栅格被路径点和障碍物同时占据，表示超车候选路径与障碍物发生碰撞且不评价该超车候选路径，否则，表示超车候选路径与障碍物不会发生碰撞，并评价该超车候选路径；

(ii)超车检测：将frenet坐标下定义的检查区域和被超车辆均投影到占据栅格地图中，并判断所述检查区域是否被被超车辆占用，如果被占用，表明智能车辆没有完成超车，那么智能车辆保持当前状态且不评价该超车候选路径，否则，在换道超车后回到超车前的车道状态，并评价该超车候选路径。

进一步地，根据下式计算三个圆的中心位置和圆内的采样位置：

在式(2-3)中，X_nc和Y_nc分别为圆的中心点的横坐标和纵坐标，X和Y分别为智能车辆当前位置的横坐标和纵坐标，k_nc为控制每个圆的距离的因子，

为智能车辆的方位角；

在式(2-4)中，X_s(i)和Y_s(i)分别为每个圆内采样点的横坐标和纵坐标，X_nc和Y_nc分别为圆的中心点的横坐标和纵坐标，I为采样点的数量，R为圆的半径。

进一步地，所述检查区域为在frenet坐标系下沿着位于超车候选路径上的智能车辆的侧边划分的矩形区域。

进一步地，根据式(2-5)计算超车候选轨迹的车辆动力学成本：

E_a＝k_a∑P′(m)v² (2-5)

在式(2-5)中，E_a为超车候选轨迹的车辆动力学成本，k_a为增益系数，P′(m)为超车候选轨迹上的路径点的曲率，v为智能车辆的速度。

进一步地，根据式(2-6)计算超车候选轨迹的路径偏好成本：

在式(2-6)中，E_p为超车候选轨迹的路径偏好成本，k_p1、k_p2和k_p3为增益系数，l为超车候选轨迹上的参考路径点与占据栅格地图上标注的全局参考点之间的偏移量，l(m)为超车候选轨迹上每个参考路径点的偏移量，m为参考路径点的个数，R_p1和R_p2为范围阈值。

本发明提供的技术方案带来的有益效果是：

本发明的方法采用评价函数确定每条超车轨迹的代价值并根据代价值的大小选择最佳的超车轨迹，且该评价函数中包括碰撞风险成本、车辆动力学成本和路径偏好成本，因而通过本发明的方法选取的超车轨迹能避免潜在的碰撞风险，从而在智能车辆超车决策中，生成可行性更高的行车轨迹，保证超车轨迹无冲突且风险最小，提高了超车的安全性并改善了驾驶员和乘客的舒适性。

附图说明

图1为本发明实施例提供的智能车辆超车轨迹的规划方法的流程图；

图2为图1中步骤S2的流程图；

图3为frenet坐标系下端点T的位置分解示意图；

图4为添加第一控制点和第二控制点的一个实施例的示意图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案作进一步地详细描述。

如图1所示，本发明实施例提供了一种智能车辆超车轨迹的规划方法，包括如下步骤：

S1：建立智能车辆的超车轨迹候选集；

S2：利用评价函数确定所述超车轨迹候选集中每一条候选轨迹的代价值，并选取代价值最小的候选轨迹作为超车轨迹。

详细地，如图2-图4所示，步骤S1包括如下步骤：

S11：使用frenet坐标系，将从智能车辆的当前位置沿道路方向向前预瞄一定距离之后的每个端点T的位置分解为纵向位移s′和横向位移d′(k)，其中，s′和d′(k)分别通过式(1-1)、式(1-2)和式(1-3)计算：

在式(1-1)中，s′为沿着参考线的方向从参考线的起始点O到端点T在参考线上的投影点之间的曲线距离，s为沿着参考线的方向从参考线的起始点O到智能车辆的当前位置在参考线上的投影点F之间的曲线距离，Δs为沿着参考线的方向从投影点F到端点T在参考线上的投影点之间的曲线距离，Δs_min为Δs的最小值，v为智能车辆的速度，t_pre为预瞄时间阈值；

式(1-2)为智能车辆正常行驶情况下横向位移的计算公式，在式(1-2)中，d′(k)为每个端点到参考线的法向距离，d_w为道路的宽度，N表示道路的宽度被等距离分割成N段且N为正整数，k≤N且k为正整数；

式(1-3)为智能车辆在超车决策下横向位移的计算公式，在式(1-3)中，d′(k)为每个端点到参考线的法向距离，k₁为沿与道路正交的方向上的增益系数，k₂为区分不同状态的行驶因子，d_w为道路的宽度，N表示道路的宽度被非等距离分割成N段且N为正整数；

步骤S12：对每个端点T添加两个第一控制点，第一控制点的纵向位移和横向位移通过式(1-4)计算：

在式(1-4)中，s′_n为沿着参考线的方向从参考线的起始点O到第一控制点在参考线上的投影点之间的曲线距离，s′为沿着参考线的方向从参考线的起始点O到端点T在参考线上的投影点之间的曲线距离，k₃为第一控制点之间的距离影响因子，d′_n(k)为每个第一控制点到参考线的法向距离，d′(k)为每个端点到参考线的法向距离；

步骤S13：在笛卡尔坐标系中对智能车辆的当前位置处添加两个第二控制点，并将智能车辆的当前位置的笛卡尔坐标和第二控制点的笛卡尔坐标转换为frenet坐标，第二控制点的笛卡尔坐标通过式(1-5)计算：

在式(1-5)中，X′_n为第二控制点的横坐标，Y′_n为第二控制点的纵坐标，X为智能车辆的当前位置的横坐标，Y为智能车辆的当前位置的纵坐标，k′₃为第二控制点之间的距离影响因子，

为智能车辆的方位角；

步骤S14：基于智能车辆的当前位置的frenet坐标、两个第二控制点的frenet坐标、端点T的frenet坐标以及两个第一控制点的frenet坐标，使用五次样条曲线进行拟合，得到多条超车候选轨迹，从而获得智能车辆的超车轨迹候选集。

需要说明的是，在步骤S11中，预瞄一定距离(即预瞄距离)与智能车辆的速度和预瞄时间有关，而预瞄时间阈值t_pre是由具体实验确定。通常，参考线为道路中心线。在步骤S12中，第一控制点可添加在端点T处的前面或后面。在步骤S13中，两个第二控制点分别添加在智能车辆的当前位置处的前后。另外，由于在进行一次规划时，智能车辆的当前位置是笛卡尔坐标系下，故直接在笛卡尔坐标系下添加第二控制点。

具体地，在步骤S2中，按照优先级从高到低的顺序选取碰撞风险成本、车辆动力学成本和路径偏好成本组成评价函数，所述评价函数为：

E＝E_{R_ij}+E_a+E_p (2-1)

在式(2-1)中，E为超车候选轨迹的代价值，E_{R_ij}为超车候选轨迹的碰撞风险成本，E_a为超车候选轨迹的车辆动力学成本，E_p为超车候选轨迹的路径偏好成本。

显然，在选取最佳超车轨迹时，选择E最小的轨迹作为待执行轨迹，保证路径规划层生成的参考轨迹无冲突且风险最小。

需要说明的是，在计算超车候选轨迹的碰撞风险成本之前还包括如下步骤：

(i)碰撞检测：通过超车候选轨迹上的路径点反求相交于该路径点的三个圆的中心(使用三圆模型(three-circle model)代替车辆外轮廓，三圆模型可用于求三个圆交点坐标)，并在三个圆内采样，采样所占据的空间相当于智能车辆行驶在超车候选路径上所需要的空间，也是每个超车候选路径的所需空间，然后将代表超车候选路径上的点和代表障碍物的点投影到占据栅格地图中并将超车候选路径所需空间的地图与障碍物所需空间的地图进行匹配，如果匹配的结果为真，则占据栅格地图中的栅格被路径点和障碍物同时占据，表示超车候选路径与障碍物发生碰撞且不评价该超车候选路径，否则，表示超车候选路径与障碍物不会发生碰撞(即只被其中一个占据或者没有被占据)，并评价该超车候选路径；

三个圆的中心位置和圆内的采样位置由式(2-3)和式(2-4)计算：

在式(2-3)中，X_nc和Y_nc分别为圆的中心点的横坐标和纵坐标，X和Y分别为智能车辆当前位置的横坐标和纵坐标，k_nc为控制每个圆的距离的因子，

为智能车辆的方位角；其中，每个圆的距离是指三圆模型中的三个圆的圆心和路径点的距离，也相当于三个圆的半径；

在式(2-4)中，X_s(i)和Y_s(i)分别为每个圆内采样点的横坐标和纵坐标，X_nc和Y_nc分别为圆的中心点的横坐标和纵坐标，I为采样点的数量，R为圆的半径，R乘以i表示采样点和圆心的距离占该圆半径的i/I，其中，i的取值范围为(0，I]，表示第i个采样点；

(ii)超车检测：将frenet坐标下定义的检查区域和被超车辆均投影到占据栅格地图中，并判断所述检查区域是否被被超车辆占用，如果被占用，表明智能车辆没有完成超车，那么智能车辆保持当前状态且不评价该超车候选路径，否则，在换道超车后回到超车前的车道状态，并评价该超车候选路径，其中，检查区域是指在frenet坐标系下沿着位于超车候选路径上的智能车辆的侧边划分的矩形区域，其可作为识别智能车辆是否越过障碍物的基础，且该检查区域的大小根据需求划分，区域越大，回道越晚。

具体地，在步骤S2中，根据式(2-2)计算超车候选轨迹的碰撞风险成本：

在式(2-2)中，E_{R_ij}为超车候选轨迹的碰撞风险成本，r_ij为将超车候选轨迹所需要的空间投影到占据栅格地图后最外侧的网格到障碍物的距离，r_max为距离的最大值，该值由实际实验决定。

在计算超车候选轨迹的碰撞风险成本时，如果E_{R_ij}的值为1，意味着碰撞不可接受。另外，在障碍物周围，风险随着距离的增加而降低，直到超过最大的影响范围。通过在候选路径的投影中叠加每个网格的风险值，可以量化距离的安全值。

在步骤S2中，根据式(2-5)计算超车候选轨迹的车辆动力学成本：

E_a＝k_a∑P′(m)v² (2-5)

在式(2-5)中，E_a为超车候选轨迹的车辆动力学成本，k_a为增益系数，P′(m)为超车候选轨迹上的路径点的曲率，v为智能车辆的速度。

由上可知，在计算超车候选轨迹的车辆动力学成本时，选用的是横向加速度作为评价指标。

在步骤S2中，根据式(2-6)计算超车候选轨迹的路径偏好成本：

在式(2-6)中，E_p为超车候选轨迹的路径偏好成本，k_p1、k_p2和K_p3为增益系数，l为超车候选轨迹上的参考路径点与占据栅格地图上标注的全局参考点之间的偏移量(即参考路径点有多个，l代表这多个参考路径点分别与全局参考点之间的偏移量)，l(m)为超车候选轨迹上每个参考路径点的偏移量，m为参考路径点的个数，R_p1和R_p2为范围阈值，其由实验决定。该公式的含义是，将所有参考路径点与全局参考点之间的偏移量均与R_p1和/或R_p2分别比较，再根据比较的结果选择对应的增益系数k_p1、k_p2、k_p3，最后根据选择的对应增益系数将每个参考路径点的偏移量累加得到超车候选轨迹的路径偏好成本。

在计算路径偏好成本时，使用了路径和道路之间的关系来评估路径偏好成本，路径越靠近参考路径，成本越低。

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

Claims (9)

1.一种智能车辆超车轨迹的规划方法，其特征在于，包括如下步骤：

建立智能车辆的超车轨迹候选集，其包括：

(1)使用frenet坐标系，将从智能车辆的当前位置沿道路方向向前预瞄一定距离之后的每个端点的位置分解为纵向位移S′和横向位移d′(k)，其中，S′和d′(k)通过下式计算：

在式(1-1)中，S′为沿着参考线的方向从参考线的起始点到所述端点在参考线上的投影点之间的曲线距离，S为沿着参考线的方向从参考线的起始点到智能车辆的当前位置在参考线上的投影点F之间的曲线距离，Δs为沿着参考线的方向从所述投影点F到所述端点在参考线上的投影点之间的曲线距离，Δs_min为Δs的最小值，v为智能车辆的速度，t_pre为预瞄时间阈值；

式(1-2)为智能车辆正常行驶情况下横向位移的计算公式，在式(1-2)中，d′(k)为每个所述端点到参考线的法向距离，d_w为道路的宽度，N表示道路的宽度被等距离分割成N段且N为正整数，k≤N且k为正整数；

式(1-3)为智能车辆在超车决策下横向位移的计算公式，在式(1-3)中，d′(k)为每个所述端点到参考线的法向距离，k₁为沿与道路正交的方向上的增益系数，k₂为区分不同状态的行驶因子，d_w为道路的宽度，N表示道路的宽度被非等距离分割成N段且N为正整数；

(2)对每个所述端点添加两个第一控制点，第一控制点的纵向位移和横向位移通过式(1-4)计算：

在式(1-4)中，S′_n为沿着参考线的方向从参考线的起始点到第一控制点在参考线上的投影点之间的曲线距离，S′为沿着参考线的方向从参考线的起始点到所述端点在参考线上的投影点之间的曲线距离，k₃为第一控制点之间的距离影响因子，d′_n(k)为每个第一控制点到参考线的法向距离，d′(k)为每个所述端点到参考线的法向距离；

(3)在笛卡尔坐标系中对智能车辆的当前位置处添加两个第二控制点，并将智能车辆的当前位置的笛卡尔坐标和第二控制点的笛卡尔坐标转换为frenet坐标，第二控制点的笛卡尔坐标通过式(1-5)计算：

在式(1-5)中，X′_n为第二控制点的横坐标，Y′_n为第二控制点的纵坐标，X为智能车辆的当前位置的横坐标，Y为智能车辆的当前位置的纵坐标，k′₃为第二控制点之间的距离影响因子，

为智能车辆的方位角；

(4)基于智能车辆的当前位置的frenet坐标、两个所述第二控制点的frenet坐标、所述端点的frenet坐标以及两个所述第一控制点的frenet坐标，使用五次样条曲线进行拟合，得到多条超车候选轨迹，从而获得智能车辆的超车轨迹候选集；

利用评价函数确定所述超车轨迹候选集中每一条候选轨迹的代价值，并选取代价值最小的候选轨迹作为超车轨迹。

2.如权利要求1所述的智能车辆超车轨迹的规划方法，其特征在于，所述参考线为道路中心线。

3.如权利要求2所述的智能车辆超车轨迹的规划方法，其特征在于：按照优先级从高到低的顺序选取碰撞风险成本、车辆动力学成本和路径偏好成本组成评价函数，所述评价函数为：

E＝E_{R_ij}+E_a+E_p (2-1)

在式(2-1)中，E为超车候选轨迹的代价值，E_{R_ij}为超车候选轨迹的碰撞风险成本，E_a为超车候选轨迹的车辆动力学成本，E_p为超车候选轨迹的路径偏好成本。

4.如权利要求3所述的智能车辆超车轨迹的规划方法，其特征在于，根据式(2-2)计算超车候选轨迹的碰撞风险成本：

在式(2-2)中，E_{R_ij}为超车候选轨迹的碰撞风险成本，r_ij为将超车候选轨迹所需要的空间投影到占据栅格地图后最外侧的网格到障碍物的距离，r_max为距离的最大值。

5.如权利要求4所述的智能车辆超车轨迹的规划方法，其特征在于，在计算超车候选轨迹的碰撞风险成本之前还包括如下步骤：

(i)碰撞检测：通过超车候选轨迹上的路径点反求相交于该路径点的三个圆的中心，并在三个圆内采样，采样所占据的空间相当于智能车辆行驶在超车候选路径上所需要的空间，也是每个超车候选路径的所需空间，然后将代表超车候选路径上的点和代表障碍物的点投影到占据栅格地图中并将超车候选路径所需空间的地图与障碍物所需空间的地图进行匹配，如果匹配的结果为真，则占据栅格地图中的栅格被路径点和障碍物同时占据，表示超车候选路径与障碍物发生碰撞且不评价该超车候选路径，否则，表示超车候选路径与障碍物不会发生碰撞，并评价该超车候选路径；

(ii)超车检测：将frenet坐标下定义的检查区域和被超车辆均投影到占据栅格地图中，并判断所述检查区域是否被被超车辆占用，如果被占用，表明智能车辆没有完成超车，那么智能车辆保持当前状态且不评价该超车候选路径，否则，在换道超车后回到超车前的车道状态，并评价该超车候选路径。

6.如权利要求5所述的智能车辆超车轨迹的规划方法，其特征在于，根据下式计算三个圆的中心位置和圆内的采样位置：

在式(2-3)中，X_nc和Y_nc分别为圆的中心点的横坐标和纵坐标，X和Y分别为智能车辆当前位置的横坐标和纵坐标，k_nc为控制每个圆的距离的因子，

为智能车辆的方位角；

在式(2-4)中，X_s(i)和Y_s(i)分别为每个圆内采样点的横坐标和纵坐标，X_nc和Y_nc分别为圆的中心点的横坐标和纵坐标，I为采样点的数量，R为圆的半径。

7.如权利要求5所述的智能车辆超车轨迹的规划方法，其特征在于，所述检查区域为在frenet坐标系下沿着位于超车候选路径上的智能车辆的侧边划分的矩形区域。

8.如权利要求3所述的智能车辆超车轨迹的规划方法，其特征在于，根据式(2-5)计算超车候选轨迹的车辆动力学成本：

E_a＝k_aΣP′(m)v² (2-5)

在式(2-5)中，E_a为超车候选轨迹的车辆动力学成本，k_a为增益系数，P′(m)为超车候选轨迹上的路径点的曲率，v为智能车辆的速度。

9.如权利要求3所述的智能车辆超车轨迹的规划方法，其特征在于，根据式(2-6)计算超车候选轨迹的路径偏好成本：

在式(2-6)中，E_p为超车候选轨迹的路径偏好成本，k_p1、k_p2和k_p3为增益系数，l为超车候选轨迹上的参考路径点与占据栅格地图上标注的全局参考点之间的偏移量，l(m)为超车候选轨迹上每个参考路径点的所述偏移量，m为参考路径点的个数，R_p1和R_p2为范围阈值。

Images