CN114644018A - 一种基于博弈论的自动驾驶车辆人车交互决策规划方法 - Google Patents

Abstract

本发明涉及一种基于博弈论的自动驾驶车辆人车交互决策规划方法，属于自动驾驶汽车技术领域。该方法包括：S1：构建基于博弈论的人车交互模型，具体包括：构建车辆和行人的行为概率预测模型、车辆和行人成本矩阵；基于累积前景理论，根据车辆和行人的行为概率以及成本矩阵，计算车辆和行人行为的收益；引入协作系数，改进人车交互模型；基于改进的人车交互模型进行行人协作值推理；对人车交互模型进行验证；S2：结合实时交通信息的自动驾驶汽车协作控制。本发明能实现自动驾驶汽车针对不同合作程度行人的协作性控制，提高自动驾驶汽车的在无信号灯人行横道的通行效率与安全性。

Description

一种基于博弈论的自动驾驶车辆人车交互决策规划方法

技术领域

本发明属于自动驾驶汽车技术领域，涉及一种基于博弈论的自动驾驶车辆人车交互决策规划方法。

背景技术

自动驾驶的初衷是给人们带来安全、高效、便捷的出行。在未来，混合交通将是常态，交通参与者包括自动驾驶车辆、人类驾驶车辆、以及行人等等。对于独立的单个交通参与者，其行为对其余交通参与者来说充满着不确定性。此外，各个交通参与者之间也存在强耦合的交互作用，分别影响相互间的行为，这给自动驾驶汽车的安全性带来了巨大的挑战。

一方面，现有的自动驾驶交互决策研究中，常采用保守的方法来保证安全性，而牺牲了交通效率。此外，有的研究将隐性的信息透明化，或者基于决策双方完全理智的决策假设，忽略了不同交通参与者的独立个性，这可能导致潜在的问题。另一方面，现有的人车交互研究聚焦于探索影响两者间交互的因素，为未来自动驾驶汽车的实现提供参考数据，人车交互尚未纳入自动驾驶汽车决策制定中。对人车交互而言，行人往往更易受到伤害，并产生严重后果。人车交互作用是自动驾驶车辆决策中不可或缺的考虑因素。

因此，目前亟需一种能将自动驾驶汽车与行人的交互考虑进去的决策方法，使得自动驾驶车辆能够与不同合作程度的行人进行交互并产生协作性动作，提升在复杂交互环境下的行驶安全性和交通效率。

发明内容

有鉴于此，本发明的目的在于提供一种基于博弈论的自动驾驶车辆人车交互决策规划方法，使得自动驾驶车辆能够与不同合作程度的行人进行交互并产生协作性动作，提升在复杂交互环境下的行驶安全性和交通效率。

为达到上述目的，本发明提供如下技术方案：

一种基于博弈论的自动驾驶车辆人车交互决策规划方法，包括以下步骤：

S1：构建基于博弈论的人车交互模型，具体包括：

S11：构建车辆和行人的行为概率预测模型；

S12：构建车辆和行人成本矩阵；

S13：基于累积前景理论，根据车辆和行人的行为概率以及成本矩阵，计算车辆和行人通行和让行行为的收益；

S14：引入协作系数，改进人车交互模型；

S15：基于改进的人车交互模型进行行人协作值推理；

S16：对步骤S15的人车交互模型进行验证：通过与实际交互数据比较，调整风险成本和延误成本的设定，保证模型的交互准确性；

S2：基于步骤S16验证后的人车交互模型，结合实时交通信息来实现自动驾驶汽车协作控制。

进一步，步骤S11具体包括以下步骤：

S111：在信号灯的人行横道采集车辆和行人的交互信息，包括：车辆速度、行人速度、车辆与行人的纵向相对距离；

S112：建立二元逻辑回归预测模型，表达式为：

其中，z_i是影响车辆或行人行为的变量，

表示变量z_i的影响程度，φ为常量，U为影响车辆或行人行为变量的线性组合，m表示选取的影响车辆或行人行为变量的数量；二进制值y＝0表示车辆或行人的通行行为，y＝1表示让行行为；

S113：通过步骤S111采集到的交互信息，采用极大似然法求解预测模型参数。

进一步，步骤S12具体包括：从风险和延误两方面评估车辆和行人动作，基于车辆和行人不同行为组合构建成本矩阵。

进一步，步骤S13具体包括以下步骤：

S131：根据步骤S11预测模型获得的车辆和行人动作概率，计算权重函数，表达式为：

其中，w⁺(p_i)和w^-(p_i)为单位区间内严格单调递增的权重函数，满足w⁺(0)＝w^-(0)＝0,w⁺(1)＝w^-(1)＝1，p_i为不同动作获得成本x_i的概率，γ和δ为常量参数；

S132：根据步骤S12获得的成本矩阵，计算成本价值，成本价值v(x)计算函数如下：

其中，α、β和λ为常量参数，x为当前动作组合下车辆或行人的成本；

S133：计算车辆和行人的通行和让行行为的收益，表达式为：

其中，V_veh,cross为车辆通行收益，V_veh,yield为车辆让行收益，V_ped,cross为行人通行收益，V_ped,yield为行人让行收益，p₀、p₁、p₂、p₃分别表示车辆通行、车辆让行、行人通行、行人让行的概率，a₀₀、a₀₁、a₁₀、a₁₁分别表示车辆通行且行人通行、车辆通行且行人让行、车辆让行且行人通行、车辆让行且行人让行动作组合下的车辆成本，b₀₀、b₁₀、b₁₀、b₁₁分别表示车辆通行且行人通行、车辆通行且行人让行、车辆让行且行人通行、车辆让行且行人让行动作组合下的行人成本。

进一步，步骤S14具体包括以下步骤：

S141：设定协作系数η为：

其中，q表示行人的协作值，其范围为[0,1]；默认η＝1表示协作值为0.5的行人。

S142：计算行人协作值为q的行人通行前景值，表达式为：

V(q,a)＝η·V_ped,cross

其中，V(q,a)表示行人协作值为q的行人通行前景值，V_ped,cross为行人通行收益。

进一步，步骤S15具体包括以下步骤：

S151：自动驾驶车辆首先将行人协作值视为0.5，即改进交互模型协作因子为1，根据Stackelberg博弈原理，比较前景值大小，预测行人下一采样时间的动作；

S152：在下一时刻，自动驾驶车辆观测行人的真实动作；

S153：根据不等式求解行人的协作值；具体包括：当预测动作a_predicted为通行，真实动作a_real为让行，行人相比默认值更加保守，根据下式计算协作值：

V(q₁,a_predicted)>V_ped,yield≥V(q₂,a_real)

相反，当预测动作a_predicted为让行，真实动作a_real为通行，行人相比默认值更加激进，根据下式计算协作值：

V(q₁,a_predicted)≤V_ped,yield<V(q₂,a_real)

根据以上步骤，可以在预测行人动作和实际动作矛盾情况下对行人协作值进行上下限估计。

进一步，步骤S2具体包括以下步骤：

S21：在观测范围内，实时获取本车和行人的运动信息；

S22：交互及行人协作性推理，根据步骤S1构建的人车交互模型与步骤S21获取的实时交通信息，获得与行人的实时交互信息；

S23：根据步骤S22获得的交互信息，求解车辆的最优加速度序列；

S24：在t时刻执行车辆最优加速度序列的第一个动作u_t+1；

S25：在t+1时刻更新环境状态信息，重复执行以上步骤，直至车辆成功通过人行横道。

进一步，步骤S23具体包括以下步骤：

S231：根据步骤S22获得的实时交互信息，确定自动驾驶车辆的收益函数Φ(x_t,u_t,q)，表达式为：

Φ(x_t,u_t,q)＝Φ_task(x_t,u_t,q)+Φ_action(u_t,q)

其中：

其中，Φ_task(x_t,u_t,q)为任务项，Φ_action(u_t,q)为动作项，x为自动驾驶车辆状态，u为车辆控制加速度，q为行人的作值，H表示预测时域的步长；sgm(cross)表示当车辆选择通行时，值为1，否则为0；同理sgm(yield)表示当车辆选择让行时，值为1，否则为0；w_v、w_u、w_u2以及w_Δu均为权重参数，任务项中v_desired为车辆的理想速度，可根据道路限速选取；u_desired为车辆的理想减速度；

S232：当车辆的动作为让行时，计算车辆理想减速度；

车辆的理想减速度为标准减速度沿当前距离到最小安全距离的线性化：

κ＝(d_obs-d_veh(t))/(d_obs-d_safe(t))^2q

u_desired＝κ·u_init(t)

其中，u_init(t)为t时刻能使车辆刚好停在人行横道边界的标准减速度，d_safe(t)为t时刻确保不发生碰撞的最小安全距离，v_veh(t)为t时刻车辆的速度，d_veh(t)为t时刻车辆距人行横道边界的距离，u_max为车辆的最大制动减速度，d_obs为观测范围，κ为线性化因子；

S233：确定约束条件：

1)纵向速度v_t+h+1约束条件：0≤v_t+h+1≤v_max，v_max表示车辆纵向最大速度；

2)纵向加速度u_t+h约束条件：u_min≤u_t+h≤u_max，u_min、u_max分别表示车辆纵向最小和最大加速度；

3)车辆动力学f(x_t+h,u_t+h)约束条件：x_t+h+1＝f(x_t+h,u_t+h)；

S234：根据收益函数和约束条件构成非线性优化问题，并用模型预测控制优化求解得到自动驾驶汽车在预测时域的最优加速度序列。

本发明的有益效果在于：

1)本发明设计的基于博弈论的人车交互决策模型，能使自动驾驶车辆能与不同协作程度的行人进行交互，并具有推理行人协作程度的能力。

2)本发明设计的自动驾驶汽车人车交互协作规划控制方法，能够在无信号灯人行横道针对不同协作程度的行人进行自适应的协作控制，有利于提高交通行驶效率和行驶安全性。

本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。

附图说明

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：

图1为本发明方法整体的逻辑结构图；

图2为人车交互博弈模型结构示意图；

图3为自动驾驶车辆与行人的交互示意图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

请参阅图1～图3，本发明提供了一种基于博弈论的自动驾驶汽车人车交互决策规划方法。考虑到现实交通环境中车辆与行人间的交互行为，以及不同行人具有不同的协作程度，基于博弈理论提供了一种车辆与行人的交互博弈模型，用于自动驾驶与行人进行交互决策，并推理行人的协作程度。同时，考虑到自动驾驶汽车的行驶效率与安全性的要求，提出一种行驶效率更高的自动驾驶汽车协作规划控制方法。

如图2所示，构建一种基于博弈论的人车交互模型，具体包括以下步骤：

S11：构建车辆和行人的通行和让行行为概率预测模型。具体过程如下：

S111：在信号灯的人行横道采集车辆和行人的交互信息；交互信息包括：车辆速度、行人速度、车辆与行人的纵向相对距离。

S112：建立二元逻辑回归预测模型：

其中，z_i是影响车辆或行人行为的变量，

表示变量z_i的影响程度，φ为常量，U为影响车辆或行人行为变量的线性组合，m表示选取的影响车辆或行人行为变量的数量，二进制值y＝0表示车辆或行人的通行行为，y＝1表示让行行为。

S113：通过步骤S111采集到的状态信息数据，可根据极大似然法求解预测模型参数。

S12：构建车辆和行人成本矩阵，车辆和行人的成本从风险和延误两个方面衡量。具体如下：

S121：风险成本衡量车辆或行人当前行为的风险值，其取值示例如表1所示。

表1车辆行人风险成本

S122：延误成本衡量车辆或行人当前行为的时间代价，其取值示例如表2所示。

表2车辆行人延误成本

等待时间	t≤5s	5<t≤10s	t>5s
				车辆延误成本R<sub>veh,delay</sub>	-1	-2	-3
行人延误成本R<sub>ped,delay</sub>	-1	-3	-5

S123：根据车辆和行人成本设定，构建车辆行人成本矩阵，对应成本矩阵示例如表3所示。

表3车辆行人成本矩阵

S13：基于累积前景理论，根据行人和车辆的动作概率以及成本矩阵，计算行人和车辆通行和让行行为的收益。具体如下：

S131：根据步骤S11的预测模型获得行人和车辆动作概率，计算权重函数：

其中，w⁺(p_i)和w^-(p_i)为单位区间内严格单调递增的权重函数，满足w⁺(0)＝w^-(0)＝0,w⁺(1)＝w^-(1)＝1，p_i为不同动作获得成本x_i的概率，γ和δ为常量参数。

S132：根据步骤S12获得的成本矩阵，计算成本价值，成本价值计算函数如下：

其中，α、β和λ为常量参数，x为当前动作组合下车辆或行人的成本。

S133：计算车辆和行人的通行和让行行为的前景值：

其中，其中，V_veh,cross为车辆通行收益，V_veh,yield为车辆让行收益，V_ped,cross为行人通行收益，V_ped,yield为行人让行收益，p₀、p₁、p₂、p₃分别表示车辆通行、车辆让行、行人通行、行人让行的概率，a₀₀、a₀₁、a₁₀、a₁₁分别表示车辆通行且行人通行、车辆通行且行人让行、车辆让行且行人通行、车辆让行且行人让行动作组合下的车辆成本，b₀₀、b₁₀、b₁₀、b₁₁分别表示车辆通行且行人通行、车辆通行且行人让行、车辆让行且行人通行、车辆让行且行人让行动作组合下的行人成本。

S14：引入协作系数，改进人车交互模型；具体如下：

S141：设定协作系数η为：

其中，q为行人的协作值，其范围为[0,1]。默认η＝1表示协作值为0.5的行人。

S142：计算行人协作值为q的行人通行前景值：

V(q,a)＝η·V_ped,cross

其中，V(q,a)表示行人协作值为q的行人通行前景值。

根据以上步骤，交互模型被拓展到不同协作程度的行人身上。

S15：基于拓展交互模型进行行人协作性推理，如图3所示，具体过程如下：

S151：自动驾驶车辆首先将行人协作值视为0.5，即拓展交互模型协作因子为1，根据Stackelberg博弈原理，比较前景值大小，预测行人下一采样时间的动作。

S152：在下一时刻，自动驾驶车辆观测行人的真实动作。

S153：根据不等式求解行人的协作值。具体地，当预测动作a_predicted为通行，真实动作a_real为让行，行人相比默认值更加保守，可根据下式计算协作值：

V(q₁,a_predicted)>V_ped,yield≥V(q₂,a_real)

相反，当预测动作a_predicted为让行，真实动作a_real为通行，行人相比默认值更加激进，可根据下式计算协作值：

V(q₁,a_predicted)≤V_ped,yield<V(q₂,a_real)

根据以上步骤，可以在预测行人动作和实际动作矛盾情况下对行人协作值进行上下限估计。

S16：对步骤S15的人车交互模型进行验证，通过与实际交互数据比较，调整风险成本和延误成本的设定，保证模型的交互准确性。本发明中的风险和延误成本设定并不一定与表1和表2中的设定相同，需要根据实际情况调整。

一种基于交互模型的自动驾驶汽车协作控制方法，具体包括以下步骤：

S21：在观测范围内，实时获取行人的位置、速度和加速度信息。

S22：根据前面构建的基于博弈论的人车交互模型，获得与行人的交互信息，交互信息包括：行人的动作，行人的协作性。

S23：根据步骤S22获得的交互信息，求解车辆的最优加速度序列，具体包括以下步骤：

S231：根据步骤S22获得的交互信息，确定自动驾驶车辆的收益函数：

Φ(x_t,u_t,q)＝Φ_task(x_t,u_t,q)+Φ_action(u_t,q)

其中：

其中，Φ_task(x_t,u_t,q)为任务项，Φ_action(u_t,q)为动作项，x为自动驾驶车辆状态，u为车辆控制加速度，q为行人的协作值，H表示预测时域的步长，sgm(cross)表示当车辆选择通行时，值为1，否则为0；同理sgm(yield)表示当车辆选择让行时，值为1，否则为0，w_v、w_u、w_u2以及w_Δu均为权重参数，任务项中v_desired为车辆的理想速度，可根据道路限速选取；u_desired为车辆的理想减速度。

S232：当车辆的动作为让行时，计算车辆理想减速度。车辆的理想减速度为标准减速度沿当前距离到最小安全距离的线性化：

κ＝(d_obs-d_veh(t))/(d_obs-d_safe(t))^2q

u_desired＝κ·u_init(t)

其中，u_init(t)为t时刻能使车辆刚好停在人行横道边界的标准减速度，d_safe(t)为t时刻确保不发生碰撞的最小安全距离，v_veh(t)为t时刻车辆的速度，d_veh(t)为t时刻车辆距人行横道边界的距离，u_max为车辆的最大制动减速度，d_obs为观测范围，κ为线性化因子。

S233：确定如下约束条件：

1)纵向速度v_t+h+1约束条件：0≤v_t+h+1≤v_max，v_max表示车辆纵向最大速度；

2)纵向加速度u_t+h约束条件：u_min≤u_t+h≤u_max，u_min，u_max分别表示车辆纵向最小和最大加速度；

3)车辆动力学f(x_t+h,u_t+h)：x_t+h+1＝f(x_t+h,u_t+h)，动力学约束。

S234：根据该收益函数和约束构成的非线性优化问题，可用模型预测控制优化求解得到自动驾驶汽车在预测时域的最优加速度序列。

S24：在t时刻执行车辆最优加速度序列的第一个动作u_t+1。

S25：在t+1时刻更新环境状态信息，重复执行以上步骤，直至车辆成功通过人行横道。

本发明通过利用基于博弈论的交互模型，使得自动驾驶汽车具备与行人交互并对行人协作性进行推理的能力，同时利用该交互信息，实现了自动驾驶汽车针对不同合作程度行人的协作性控制，提高了自动驾驶汽车的在无信号灯人行横道的通行效率与安全性。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (8)

1.一种基于博弈论的自动驾驶车辆人车交互决策规划方法，其特征在于，该方法包括以下步骤：

S1：构建基于博弈论的人车交互模型，具体包括：

S11：构建车辆和行人的行为概率预测模型；

S12：构建车辆和行人成本矩阵；

S13：基于累积前景理论，根据车辆和行人的行为概率以及成本矩阵，计算车辆和行人行为的收益；

S14：引入协作系数，改进人车交互模型；

S15：基于改进的人车交互模型进行行人协作值推理；

S16：对步骤S15的人车交互模型进行验证：通过与实际交互数据比较，调整风险成本和延误成本的设定，保证模型的交互准确性；

S2：基于步骤S16验证后的人车交互模型，结合实时交通信息来实现自动驾驶汽车协作控制。

2.根据权利要求1所述的自动驾驶车辆人车交互决策规划方法，其特征在于，步骤S11具体包括以下步骤：

S111：在信号灯的人行横道采集车辆和行人的交互信息，包括：车辆速度、行人速度、车辆与行人的纵向相对距离；

S112：建立二元逻辑回归预测模型，表达式为：

其中，z_i是影响车辆或行人行为的变量，

表示变量z_i的影响程度，φ为常量，U为影响车辆或行人行为变量的线性组合，m表示选取的影响车辆或行人行为变量的数量；二进制值y＝0表示车辆或行人的通行行为，y＝1表示让行行为；

S113：通过步骤S111采集到的交互信息，采用极大似然法求解预测模型参数。

3.根据权利要求1所述的自动驾驶车辆人车交互决策规划方法，其特征在于，步骤S12具体包括：从风险和延误两方面评估车辆和行人动作，基于车辆和行人不同行为组合构建成本矩阵。

4.根据权利要求1所述的自动驾驶车辆人车交互决策规划方法，其特征在于，步骤S13具体包括以下步骤：

S131：根据步骤S11预测模型获得的车辆和行人动作概率，计算权重函数，表达式为：

其中，w⁺(p_i)和w^-(p_i)为单位区间内严格单调递增的权重函数，满足w⁺(0)＝w^-(0)＝0,w⁺(1)＝w^-(1)＝1，p_i为不同动作获得成本x_i的概率，γ和δ为常量参数；

S132：根据步骤S12获得的成本矩阵，计算成本价值，成本价值v(x)计算函数如下：

其中，α、β和λ为常量参数，x为当前动作组合下车辆或行人的成本；

S133：计算车辆和行人的通行和让行行为的收益，表达式为：

其中，V_veh,cross为车辆通行收益，V_veh,yield为车辆让行收益，V_ped,cross为行人通行收益，V_ped,yield为行人让行收益，p₀、p₁、p₂、p₃分别表示车辆通行、车辆让行、行人通行、行人让行的概率，a₀₀、a₀₁、a₁₀、a₁₁分别表示车辆通行且行人通行、车辆通行且行人让行、车辆让行且行人通行、车辆让行且行人让行动作组合下的车辆成本，b₀₀、b₁₀、b₁₀、b₁₁分别表示车辆通行且行人通行、车辆通行且行人让行、车辆让行且行人通行、车辆让行且行人让行动作组合下的行人成本。

5.根据权利要求1所述的自动驾驶车辆人车交互决策规划方法，其特征在于，步骤S14具体包括以下步骤：

S141：设定协作系数η为：

其中，q为行人的协作值，其范围为[0,1]；

S142：计算行人协作值为q的行人通行前景值，表达式为：

V(q,a)＝η·V_ped,cross

其中，V(q,a)表示行人协作值为q的行人通行前景值，V_ped,cross为行人通行收益。

6.根据权利要求5所述的自动驾驶车辆人车交互决策规划方法，其特征在于，步骤S15具体包括以下步骤：

S151：自动驾驶车辆首先将行人协作值视为0.5，即改进交互模型协作因子为1，根据Stackelberg博弈原理，比较前景值大小，预测行人下一采样时间的动作；

S152：在下一时刻，自动驾驶车辆观测行人的真实动作；

S153：根据不等式求解行人的协作值；具体包括：当预测动作a_predicted为通行，真实动作a_real为让行，行人相比默认值更加保守，根据下式计算协作值：

V(q₁,a_predicted)>V_ped,yield≥V(q₂,a_real)

相反，当预测动作a_predicted为让行，真实动作a_real为通行，行人相比默认值更加激进，根据下式计算协作值：

V(q₁,a_predicted)≤V_ped,yield<V(q₂,a_real)。

7.根据权利要求1所述的自动驾驶车辆人车交互决策规划方法，其特征在于，步骤S2具体包括以下步骤：

S21：在观测范围内，实时获取本车和行人的运动信息；

S22：交互及行人协作性推理，根据步骤S1构建的人车交互模型与步骤S21获取的实时交通信息，获得与行人的实时交互信息；

S23：根据步骤S22获得的交互信息，求解车辆的最优加速度序列；

S24：在t时刻执行车辆最优加速度序列的第一个动作u_t+1；

S25：在t+1时刻更新环境状态信息，重复执行以上步骤，直至车辆成功通过人行横道。

8.根据权利要求7所述的自动驾驶车辆人车交互决策规划方法，其特征在于，步骤S23具体包括以下步骤：

S231：根据步骤S22获得的实时交互信息，确定自动驾驶车辆的收益函数Φ(x_t,u_t,q)，表达式为：

Φ(x_t,u_t,q)＝Φ_task(x_t,u_t,q)+Φ_action(u_t,q)

其中：

其中，Φ_task(x_t,u_t,q)为任务项，Φ_action(u_t,q)为动作项，x为自动驾驶车辆状态，u为车辆控制加速度，q为行人的协作值，H表示预测时域的步长；sgm(cross)表示当车辆选择通行时，值为1，否则为0；同理sgm(yield)表示当车辆选择让行时，值为1，否则为0；w_v、w_u、

以及w_Δu均为权重参数，任务项中v_desired为车辆的理想速度；u_desired为车辆的理想减速度；

S232：当车辆的动作为让行时，计算车辆理想减速度；

车辆的理想减速度为标准减速度沿当前距离到最小安全距离的线性化：

κ＝(d_obs-d_veh(t))/(d_obs-d_safe(t))^2q

u_desired＝κ·u_init(t)

其中，u_init(t)为t时刻能使车辆刚好停在人行横道边界的标准减速度，d_safe(t)为t时刻确保不发生碰撞的最小安全距离，v_veh(t)为t时刻车辆的速度，d_veh(t)为t时刻车辆距人行横道边界的距离，u_max为车辆的最大制动减速度，d_obs为观测范围，κ为线性化因子；

S233：确定约束条件：

1)纵向速度v_t+h+1约束条件：0≤v_t+h+1≤v_max，v_max表示车辆纵向最大速度；

2)纵向加速度u_t+h约束条件：u_min≤u_t+h≤u_max，u_min，u_max分别表示车辆纵向最小和最大加速度；

3)车辆动力学f(x_t+h,u_t+h)约束条件：x_t+h+1＝f(x_t+h,u_t+h)；

S234：根据收益函数和约束条件构成非线性优化问题，并用模型预测控制优化求解得到自动驾驶汽车在预测时域的最优加速度序列。

Images