CN112319477A - 一种用于无人驾驶的决策规划方法 - Google Patents

Abstract

一种用于无人驾驶的决策规划方法，包括：根据各个障碍车的行驶数据，计算每辆障碍车属于各个车道的概率；计算无人驾驶的主车在不同的加速度下，与各个障碍车发生碰撞的概率，以确定每辆障碍车在不同的速度策略下的风险值，所述速度策略包括减速、匀速以及加速；根据所述每辆障碍车属于各个车道的概率，以及每辆障碍车在不同的速度策略下的风险值，确定所述主车的行为，所述主车的行为包括变更车道与调整速度策略。本发明提供的决策规划方法可以提高行为决策的可靠性。

Description

一种用于无人驾驶的决策规划方法

技术领域

本发明涉及无人驾驶技术领域，特别涉及一种用于无人驾驶的决策规划方法。

背景技术

无人驾驶汽车和智能交通系统被视为处理交通拥堵和事故的可行方式。为了确保无人驾驶车辆安全可靠地行驶，决策规划算法在无人驾驶汽车系统中扮演着重要角色。

但是，现有的决策规划算法并没有考虑无人驾驶车辆的加速度因素，很难获得准确的行为决策。因此，如何提高行为决策的可靠性，成为本领域一项亟待解决的技术问题。

发明内容

本发明的目的是提供一种用于无人驾驶的决策规划方法，以提高行为决策的可靠性。

为达到上述目的，本发明实施例提供一种用于无人驾驶的决策规划方法，包括：

根据各个障碍车的行驶数据，计算每辆障碍车属于各个车道的概率；

计算无人驾驶的主车在不同的加速度下，与各个障碍车发生碰撞的概率，以确定每辆障碍车在不同的速度策略下的风险值，所述速度策略包括减速、匀速以及加速；

根据所述每辆障碍车属于各个车道的概率，以及每辆障碍车在不同的速度策略下的风险值，确定所述主车的行为，所述行为包括变更车道与调整速度策略。

在一个实施例中，所述计算无人驾驶的主车在不同的加速度下，与各个障碍车发生碰撞的概率，以确定所述主车在不同的速度策略下的风险值，包括：

计算加速度值属于各种速度策略的概率；

计算所述主车在不同的加速度下，与各个障碍车发生碰撞的概率；

根据所述加速度值属于各种速度策略的概率，以及所述主车在不同的加速度下，与各个障碍车发生碰撞的概率，确定每辆障碍车在不同的速度策略下的风险值。

在一个实施例中，所述方法还包括：

根据所述主车的行为，确定所述主车的期望位置；

根据所述期望位置，选取N个变换车道时刻，以生成N条参考横向位置曲线，N＞1；

生成与每条参考横向位置曲线对应的参考速度曲线；

根据所述N条参考横向位置曲线以及与每条参考横向位置曲线对应的参考速度曲线，生成N个备选轨迹；

从所述N个备选轨迹中，选取所述主车在每个时刻与所有障碍车之间的距离都大于预定阈值的轨迹；

从选取的轨迹中选取成本函数最小的轨迹作为最优轨迹。

由以上本发明提供的技术方案可见，本发明提供的方法至少具有以下有益效果：

本发明设计分层框架完成行为决策与轨迹规划问题，其中，在行为决策层引入加速度要素，更精细地刻画了对各种行为的风险评估，从而提高行为决策的可靠性。另外，在轨迹规划方面，采用多项式与模型预测规划相结合的方法处理规划问题，兼顾了路径的平滑性和速度变化的快速性，得到的行驶轨迹更安全、更平滑、更高效，可以满足结构化道路下无人驾驶车辆高速行驶的需求。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的用于无人驾驶的决策规划方法的流程图；

图2是从笛卡尔坐标系转换到道路曲线坐标系的示意图；

图3是障碍车占据车道的场景示意图；

图4(a)是障碍车与主车的示意图；图4(b)是在图4(a)的场景下得到的主车在不同加速度值时的风险值；图4(c)是在图4(a)的场景下得到的主车采取不同速度策略的风险值；

图5是九种行为的风险评估示意图；

图6是本发明实施例提供的行为规划与轨迹规划的整体框架图；

图7(a)是主车在不同变道时刻下，向左变道的参考横向位置曲线；图7(b)是主车在不同变道时刻下，向右变道的参考横向位置曲线；

图8(a)是主车在匀速策略下，不同的变道时刻的速度曲线；图8(b)是主车在减速策略下，不同的变道时刻的速度曲线；图8(c)是主车在加速策略下，不同的变道时刻的速度曲线；

图9(a)是在一个具体的实施例中，主车在t＝0秒时的轨迹规划图；图9(b)是t＝0秒时的九种行为的风险值；

图10(a)是在一个具体的实施例中，主车在t＝32秒时的轨迹规划仿图；图10(b)是t＝32秒时的九种行为的风险值；

图11(a)是在一个具体的实施例中，主车在t＝41秒时的轨迹规划图；图11(b)是t＝41秒时的九种行为的风险值；

图12是在一个具体的实施例中，主车在t＝0秒到t＝100秒的风险值变化曲线；

图13是在一个具体的实施例中，主车在t＝0秒到t＝100秒的横向位置与速度的变化曲线。

具体实施方式

下面将结合附图和具体实施方式，对本发明的技术方案作详细说明，应理解这些实施方式仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落入本发明所附权利要求限定的范围内。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本发明的说明书中所使用的术语只是为了描述具体的实施方式的目的，不是旨在于限制本发明。

参考图1所示，为本发明实施例提供的用于无人驾驶的决策规划方法的流程图，可以包括以下步骤：

S1：根据各个障碍车的行驶数据，计算每辆障碍车属于各个车道的概率。

具体的，参考图2所示，预先通过公式(1)至公式(5)，将障碍车以及无人驾驶的主车的行驶数据从笛卡尔坐标系转换到道路曲线坐标系：

其中，(X_r，Y_r)通过以下公式计算得到：

s.t. Y＝F(X) (7)

(X_P，Y_P)为笛卡尔坐标系下的坐标，(s_P，n_P)为道路曲线坐标系下的坐标，(X_r，Y_r)是位于道路参考线上且距离(X_P，Y_P)最近的参考点，ψ_P为道路曲线坐标系下的航向角，

为笛卡尔坐标系下的航向角，v_P为道路曲线坐标系下的线速度，

为笛卡尔坐标系下的线速度，ω_P为道路曲线坐标系下的角速度，

为笛卡尔坐标系下的角速度，ψ_r和κ_r分别是参考点(X_r，V_r)处的切线方向和曲率，Y＝F(X)表示已知的道路参考线方程。

具体的，根据下面的公式(8)至公式(14)计算得到每辆障碍车属于各个车道的概率：

m∈{L⁺，L，M，R，R^-}

其中，

W为车道宽度，n_Obs为障碍车在道路曲线坐标系下的n坐标值，n_ego为主车在道路曲线坐标系下的n坐标值，Obs为障碍车

组成的集合，m为车道集合，L⁺表示在左侧车道以外的车道，L表示主车所在车道的左侧车道，M为主车的当前车道，R为主车所在车道的右侧车道，R^-表示在右侧车道以外的车道，

是向上取整函数，σ_m为高斯分布标准差。

具体的，上面的公式(8)至公式(14)中的条件概率可根据下面的条件概率计算公式(15)和公式(16)进行求解。

公式(15)和公式(16)中的x为变量，σ_x是标准差，状态集合χ＝{X₁，X₂，...，X_N}，每个状态X_i对应一个可能重叠或不重叠的区间

由公式(15)和公式(16)可以计算出变量x条件下状态X_i的条件概率。

例如，在一个具体的实施例中，主车和三辆障碍车的当前位置如图3所示，根据主车以及三辆障碍车的行驶数据，利用步骤S1的方法可以计算得到三辆障碍车的属于各个车道的概率，具体的，如下面的表1所示：

	障碍车1	障碍车2	障碍车3
				L<sup>+</sup>	0.156	0.000	0.000
L	0.844	0.000	0.000
				M	0.000	0.411	0.000
R	0.000	0.589	0.008
				R<sup>-</sup>	0.000	0.000	0.992

表1

S2：计算无人驾驶的主车在不同的加速度下，与各个障碍车发生碰撞的概率，以确定每辆障碍车在不同的速度策略下的风险值，所述速度策略包括减速、匀速以及加速。

具体的，步骤S2可以包括子步骤S21至S23：

S21：计算加速度值属于各种速度策略的概率。

S22：计算所述主车在不同的加速度下，与各个障碍车发生碰撞的概率。

S23：根据所述加速度值属于各种速度策略的概率，以及所述主车在不同的加速度下，与各个障碍车发生碰撞的概率，确定每辆障碍车在不同的速度策略下的风险值。

具体的，在步骤S21中，可以根据下面的公式(17)至公式(20)计算加速度值属于各种速度策略的概率。其中，公式(17)至公式(20)中的条件概率同样根据上面的条件概率计算公式(15)和公式(16)进行求解。

A∈{A^-，A⁰，A⁺}

其中，

A为速度策略集合，A^-为减速策略，A⁰为匀速策略，A⁺为加速策略，a为一系列离散的加速度值

组成的集合，a_dec是用于区分减速策略与匀速策略的阈值，a_acc是用于区分匀速策略与加速策略的阈值。

具体的，在步骤S22中，可以根据以下公式(21)至公式(24)计算所述主车在不同的加速度下，与各个障碍车发生碰撞的概率。其中，公式(21)至公式(23)中的条件概率同样根据上面的条件概率计算公式(15)和公式(16)进行求解。

S∈{Safe，Danger}

其中，

上面公式(24)中的加速碰撞时间TTC_a可以通过以下公式(25)至公式(32)进行计算。

当加速度a＝0，加速碰撞时间TTC_a为：

当加速度a≠0，加速碰撞时间TTC_a为：

其中，

Δ：＝(v_Obs-v_ego)²+2a(s_Obs-s) (28)

s_Obs为障碍车在道路曲线坐标系下的s坐标值，s_ego为主车在道路曲线坐标系下的s坐标值，v_Obs为障碍车的速度，v_ego为主车的速度，a为一系列离散的加速度值

组成的集合，TTC_a为加速碰撞时间，v_max和v_min是考虑车辆运动约束和道路规则而确定的速度上界与下界，S为危险程度集合，

是用于区分危险和安全的阈值。

具体的，在步骤S23中，可以根据以下公式(33)与公式(34)计算每辆障碍车在不同的速度策略下的风险值：

p(A，S＝Danger|Obs)＝1-p(A，S＝Safe|Obs) (34)

A∈{A^-，A⁰，A⁺}

a∈{a₁，a₂，...，a_Na}

S∈{Safe，Danger}

其中，a为一系列离散的加速度值

组成的集合，Obs为障碍车

组成的集合，A为速度策略集合，A^-为减速策略，A⁰为匀速策略，A⁺为加速策略，S为危险程度集合。

例如，在一个具体的实施例中，主车和两辆障碍车的当前位置如图4(a)所示。利用上述步骤S22提供的方法，计算得到主车在不同的加速度下，与各个障碍车发生碰撞的概率，如图4(b)所示。利用上述步骤S23提供的方法，计算得到每辆障碍车在不同的速度策略下的风险值，如图4(c)所示。

S3：根据所述每辆障碍车属于各个车道的概率，以及每辆障碍车在不同的速度策略下的风险值，确定所述主车的行为，行为包括变更车道与调整速度策略。

具体的，通过以下公式(35)和公式(36)计算所述主车的各种行为对应的风险值，并选取风险值最小的行为作为所述主车的行为。

其中，

S∈{Safe，Danger}

B∈{LKCV，LKDE，LKAC，LCCV，LCDE，LCAC，RCCV，RCDE，RCAC}

B为九种行为组成的集合，LKCV为匀速车道保持，LKDE为减速车道保持，LKAC为加速车道保持，LCCV为匀速向左变道，LCDE为减速向左变道，LCAC为加速向左变道，RCCV为匀速向右变道，RCDE为减速向右变道，RCAC为加速向右变道，B^*为确定的所述主车的行为，P_lc为换道惩罚因子，P_ac为加速惩罚因子。

换道惩罚因子P_lc用于限制主车频繁换道，当主车保持当前车道行驶时，该因子取值为0；当主车发生变道行为时，该因子的取值可以为正值，例如取值为0.2作为惩罚。加速惩罚因子P_ac用于限制主车频繁加减速，当主车匀速行驶时，该因子取值为0；当主车发生加速或减速时，该因子取值的取值可以为正值，例如取值为0.1作为惩罚。

例如，在一个具体的实施例中，主车和三辆障碍车的当前位置如图5所示。利用步骤S3提供的计算方法，可以计算出九种行为产生危险的概率。根据图5中的表格可以看出，减速车道保持行为(LKDE)的风险值最低，因此，可以确定主车的行为是减速车道保持。

此外，参考图6所示，在确定所述主车的行为之后，所述方法还可以包括以下步骤S4至S9。

S4：根据所述主车的行为，确定所述主车的期望位置。

S5：根据所述期望位置，选取N个变换车道时刻，以生成N条参考横向位置曲线，其中N＞1。

S6：生成与每条参考横向位置曲线对应的参考速度曲线。

S7：根据所述N条参考横向位置曲线以及与每条参考横向位置曲线对应的参考速度曲线，生成N个备选轨迹。

S8：从所述N个备选轨迹中，选取所述主车在每个时刻与所有障碍车之间的距离都大于预定阈值的轨迹。

S9：从选取的轨迹中选取成本函数最小的轨迹作为最优轨迹。

具体的，在步骤S6中，可以利用以下公式(37)至公式(43)得到参考横向位置曲线：

n(t)＝α₀+α₁t+α₂t²+α₃t³+α₄t⁴+α₅t⁵ (37)

n(0)＝α₀＝n₀ (38)

n(T_n)＝α₀+α₁T_n+α₂T_n ²+α₃T_n ³+α₄T_n ⁴+α₅T_n ⁵＝n_e (41)

其中，n₀为所述主车在道路曲线坐标系下的初始位置的n坐标，n_e为所述主车在道路曲线坐标系下的期望位置的n坐标，T_n为变换车道时刻，α₀，α₁，α₂，α₃，α₄，α₅分别为五次多项式的系数，t为时间。

例如，在一个具体的实施例中，主车分别在第3秒至第10秒向左变道，得到的参考横向位置曲线如图7(a)所示；主车分别在第3秒至第10秒向右变道，得到的参考横向位置曲线如图7(b)所示。

具体的，在步骤S7中，可以利用以下公式(44)至公式(49)得到所述参考速度曲线。

首先，建立纵向运动的二阶模型。状态为纵向位置和速度[s，v_s]^T，控制量为加速度a，步长为h的离散化模型如下：

考虑到纵向规划中既要关注安全性，即与障碍车的距离，也要关注平滑性和效率，即最大速度和加速度大小，因此，定义如下加权型的成本函数并使用优化算法求解。

其中，s为所述主车的纵向位置，v_s为所述主车的速度，s_Obs为障碍车在道路曲线坐标系下的s坐标值，a为加速度，h为步长，J为总代价值，

为k时刻所述主车距离障碍车最小距离产生的代价，

为k时刻所述主车加速度产生的代价；

为k时刻所述主车的速度与期望速度v_exp间偏差产生的代价；w₁，w₂与w₃为权重系数，S_r，A_r与V_r为归一化参数，δ_s(k)为k时刻所述主车距离所有与主车在同一车道内的障碍车的纵向距离的最小值，v_s(k)为k时刻主车的速度，a_s(k)为k时刻主车的加速度。

例如，在一个具体的实施例中，基于图7(a)和图7(b)所示的参考横向位置曲线，生成对应变道时刻的参考速度曲线。参考图8(a)所示，是主车在匀速策略下，不同的变道时刻的速度曲线；图8(b)是主车在减速策略下，不同的变道时刻的速度曲线；图8(c)是主车在加速策略下，不同的变道时刻的速度曲线。

具体的，在步骤S8中，通过以下公式(50)确定主车在每个时刻与所有障碍车之间的距离都大于预定阈值。用1表示安全，即主车在每个时刻与所有障碍车之间的距离都大于预定阈值，用0表示不安全。

其中，s_Obs为障碍车在道路曲线坐标系下的s坐标，s_ego为主车在道路曲线坐标系下的s坐标，n_Obs为障碍车在道路曲线坐标系下的n坐标，n_ego为主车在道路曲线坐标系下的n坐标，Obs为障碍车

组成的集合，D_safe为预定阈值。

具体的，在步骤S9中，通过以下的公式(51)至公式(54)定义成本函数。

其中，J_Opt为总代价值，T_p是规划轨迹的时间跨度，J^v为主车速度与最大速度v_max间偏差产生的代价；J^a为主车加速度产生的代价；J^ω为主车转弯角速度产生的代价；u₁，u₂，u₃为权重系数，a_max为主车的最大加速度限制，ω_max为主车的最大转向角速度限制。上述优化问题中包含非线性，难以得到数值解，可以通过梯度下降法进行数值求解。

下面通过一个具体的实施例，来进一步说明本发明的技术方案。

具体的，仿真实验的总时长为100s，仿真步长为0.1s，仿真中限制了车速不得超过100km/h。图9(a)、图10(a)和图11(a)分别展示了在第0秒，第32秒和第41秒时的仿真实验场景。在所有的场景示意图中，黑色方框表示主车，灰色方框表示障碍车；点划线表示规划得到的主车的行驶轨迹。

参考图9(a)所示，在第0s时，由于主车的当前车道的前方有行驶缓慢的障碍车，且当前车道的右侧车道障碍车较多，所以主车向左变道是比较理想的行为。参考图9(b)所示的九种行为的风险值，减速向左变道(LCDE)为最优的行为，所以规划结果为向左变道的参考轨迹。

参考图10(a)所示，在第32s时，主车行驶在最左侧车道且当前车道和右侧车道的障碍车距离主车较近，所以应当保持当前车道行驶。参考图10(b)所示的九种行为的风险值，减速车道保持(LKDE)为最优的行为，所以故规划结果仍沿当前车道。

参考图11(a)所示，在第41s时，主车的正前方车辆较多，而主车位于最左侧车道，所以向右变道更理想。参考图11(b)所示的九种行为的风险值，匀速向右变道(RCCV)可以获得较小的风险。

图12描述了整个仿真过程中风险的变化，可以看到在整个仿真过程中风险值都保持在0.5以下，大部分时候都保持在0.2以下。从两条曲线的对比可以看到，在确定主车的行为时，通常选取最低风险值的行为，但是也存在不相等的情况，这主要是加速惩罚因子和变道惩罚因子的作用结果。

图13描述了横向位置和速度的变化情况。速度的频繁变化和平均值在70km/h以上，证明了轨迹规划的方法满足快速性和高效性，可以满足结构化道路下无人驾驶车辆高速行驶的需求。

本说明书中的上述各个实施方式均采用递进的方式描述，各个实施方式之间相同相似部分相互参照即可，每个实施方式重点说明的都是与其他实施方式不同之处。

以上所述仅为本发明的几个实施方式，虽然本发明所揭露的实施方式如上，但所述内容只是为了便于理解本发明的技术方案而采用的实施方式，并非用于限定本发明。任何本发明所属技术领域的技术人员，在不脱离本发明所揭露的精神和范围的前提下，可以在实施方式的形式上及细节上作任何的修改与变化，但本发明的专利保护范围，仍须以所附权利要求书所界定的范围为准。

Claims (10)

1.一种用于无人驾驶的决策规划方法，其特征在于，包括：

根据各个障碍车的行驶数据，计算每辆障碍车属于各个车道的概率；

计算无人驾驶的主车在不同的加速度下，与各个障碍车发生碰撞的概率，以确定每辆障碍车在不同的速度策略下的风险值，所述速度策略包括减速、匀速以及加速；

根据所述每辆障碍车属于各个车道的概率，以及每辆障碍车在不同的速度策略下的风险值，确定所述主车的行为，所述主车的行为包括变更车道与调整速度策略。

2.根据权利要求1所述的方法，其特征在于，所述计算无人驾驶的主车在不同的加速度下，与各个障碍车发生碰撞的概率，以确定所述主车在不同的速度策略下的风险值，包括：

计算加速度值属于各种速度策略的概率；

计算所述主车在不同的加速度下，与各个障碍车发生碰撞的概率；

根据所述加速度值属于各种速度策略的概率，以及所述主车在不同的加速度下，与各个障碍车发生碰撞的概率，确定每辆障碍车在不同的速度策略下的风险值。

3.根据权利要求2所述的方法，其特征在于，根据以下公式计算加速度值属于各种速度策略的概率：

A∈{A^-，A⁰，A⁺}

其中，

A为速度策略集合，A^-为减速策略，A⁰为匀速策略，A⁺为加速策略，a为一系列离散的加速度值

组成的集合，a_dec是用于区分减速策略与匀速策略的阈值，a_acc是用于区分匀速策略与加速策略的阈值。

4.根据权利要求2所述的方法，其特征在于，根据以下公式计算所述主车在不同的加速度下，与各个障碍车发生碰撞的概率：

S∈{Safe，Danger}

其中，

加速碰撞时间TTC_a通过以下公式计算：

当加速度a＝0，加速碰撞时间TTC_a为：

当加速度a≠0，加速碰撞时间TTC_a为：

其中，

Δ：＝(v_Obs-v_ego)²+2a(s_Obs-s)

s_Obs为障碍车在道路曲线坐标系下的s坐标值，s_ego为主车在道路曲线坐标系下的s坐标值，v_Obs为障碍车的速度，v_ego为主车的速度，a为一系列离散的加速度值

组成的集合，TTC_a为加速碰撞时间，v_max和v_min是考虑车辆运动约束和道路规则而确定的速度上界与下界，S为危险程度集合，

是用于区分危险和安全的阈值。

5.根据权利要求2所述的方法，其特征在于，根据以下公式计算每辆障碍车在不同的速度策略下的风险值：

p(A，S＝Danger|Obs)＝1-p(A，S＝Safe|Obs)

A∈{A^-，A⁰，A⁺}

a∈{a₁，a₂，...，a_Na}

S∈{Safe，Danger}

其中，a为一系列离散的加速度值

组成的集合，Obs为障碍车

组成的集合，A为速度策略集合，A^-为减速策略，A⁰为匀速策略，A⁺为加速策略，S为危险程度集合。

6.根据权利要求1所述的方法，其特征在于，通过以下公式计算所述主车的各种行为对应的风险值，并选取风险值最小的行为作为所述主车的行为：

其中，

S∈{Safe，Danger}

B∈{LKCV，LKDE，LKAC，LCCV，LCDE，LCAC，RCCV，RCDE，RCAC}

P_lc为换道惩罚因子，P_ac为加速惩罚因子，B为九种行为组成的集合，LKCV为匀速车道保持，LKDE为减速车道保持，LKAC为加速车道保持，LCCV为匀速向左变道，LCDE为减速向左变道，LCAC为加速向左变道，RCCV为匀速向右变道，RCDE为减速向右变道，RCAC为加速向右变道，B^*为确定的所述主车的行为。

7.根据权利要求1所述的方法，其特征在于，在确定所述主车的行为之后，所述方法还包括：

根据所述主车的行为，确定所述主车的期望位置；

根据所述期望位置，选取N个变换车道时刻，以生成N条参考横向位置曲线，N＞1；

生成与每条参考横向位置曲线对应的参考速度曲线；

根据所述N条参考横向位置曲线以及与每条参考横向位置曲线对应的参考速度曲线，生成N个备选轨迹；

从所述N个备选轨迹中，选取所述主车在每个时刻与所有障碍车之间的距离都大于预定阈值的轨迹；

从选取的轨迹中选取成本函数最小的轨迹作为最优轨迹。

8.根据权利要求7所述的方法，其特征在于，利用以下公式得到所述参考横向位置曲线：

n(0)＝α₀＝n₀

其中，n₀为所述主车在道路曲线坐标系下的初始位置的n坐标，n_e为所述主车在道路曲线坐标系下的期望位置的n坐标，T_n为变换车道时刻，α₀，α₁，α₂，α₃，α₄，α₅分别为五次多项式的系数，t为时间。

9.根据权利要求7所述的方法，其特征在于，利用以下公式得到所述参考速度曲线：

其中，s为所述主车的纵向位置，s_Obs为障碍车在道路曲线坐标系下的s坐标值，a为加速度，h为步长，J为总代价值，

为k时刻所述主车距离障碍车最小距离产生的代价，

为k时刻所述主车加速度产生的代价；

为k时刻所述主车的速度与期望速度v_exp间偏差产生的代价；w₁，w₂与w₃为权重系数，S_r，A_r与V_r为归一化参数，δ_s(k)为k时刻所述主车距离所有与主车在同一车道内的障碍车的纵向距离的最小值，v_s(k)为k时刻所述主车的速度，α_s(k)为k时刻所述主车的加速度。

10.根据权利要求7所述的方法，其特征在于，所述成本函数为：

其中，J_Opt为总代价值，T_p是规划轨迹的时间跨度，J^v为主车速度与最大速度v_max间偏差产生的代价；J^a为主车加速度产生的代价；J^ω为主车转弯角速度产生的代价；u₁，u₂，u₃为权重系数，a_max为主车的最大加速度限制，ω_max为主车的最大转向角速度限制。

Images