CN113625714B - 一种基于并行求解的自动驾驶运动规划方法 - Google Patents

Abstract

本发明涉及一种基于并行求解的自动驾驶运动规划方法，属于自动驾驶技术领域。该方法包括：S1：构建运动规划问题，并对其离散化；S2：通过引入全局变量和松弛变量，将离散化的运动规划问题转换为并行运动规划问题；S3：针对构建的并行运动规划问题，采用分层交替方向乘子法进行数值求解。本发明方法大大降低了自动驾驶运动规划问题的优化维度和约束规模，简化了问题难度，同时通过将单一车载处理器的运载负荷分摊到多个并行处理器上，显著提高了自动驾驶运动规划问题的求解实时性。

Description

一种基于并行求解的自动驾驶运动规划方法

技术领域

本发明属于自动驾驶技术领域，涉及一种基于并行求解的自动驾驶运动规划方法。

背景技术

运动规划系统是自动驾驶系统中的关键技术之一，能够根据路况信息、驾驶员出行目的以及全局地图数据库规划出自起始点至终点的一条无碰撞、可通行的最优行驶路径。

依据有无模型结构，运动规划方法可分为无模型方法以及基于模型的优化方法。无模型方法主要有基于规则的方法、人工势场法、采样法等。其中，基于规则的方法不能处理规则之外的新场景，人工势场法缺少对车辆及环境的推理预测，采样法面临自由空间搜索的维数灾问题，机器学习方法的可解释性差。在基于模型的优化方法中，基于非线性模型预测控制(Nonlinear Model Predictive Control,NMPC)的运动规划能够考虑车辆动力学等非线性约束，在滚动时域内可实现安全性、舒适性等多性能目标优化，因此成为解决高等级自动驾驶运动规划面临的问题的一条重要途径。

然而，随着交通场景变复杂，NMPC运动规划中优化指标以及非线性约束的数量也随之增加，尤其是非凸的避障约束的增加，这大大增加了运动规划问题求解的复杂度。并且，当前NMPC运动规划的求解方法通常以车载单核处理器进行求解，由于单核处理器计算能力有限，这使得NMPC运动规划的实时求解变得更加困难。

发明内容

有鉴于此，本发明的目的在于提供一种基于并行求解的自动驾驶运动规划方法，针对非线性预测控制(NMPC)架构的运动规划问题优化维度较高、约束规模较大的问题，通过一致性优化解耦与约束松弛设计分布式并行运动规划问题框架以降低问题规模；同时通过将单核车载处理器的计算负荷分摊到多个车载处理器上，实现并行计算，进而得到了一种高效的分布式并行运动规划方法。

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

一种基于并行求解的自动驾驶运动规划方法，具体包括以下步骤：

S1：构建运动规划问题，并对其离散化；

S2：通过引入全局变量和松弛变量，将步骤S1离散化的运动规划问题转换为并行运动规划问题；

S3：针对步骤S2构建的并行运动规划问题，采用分层交替方向乘子法进行数值求解。

进一步，步骤S1中，构建的运动规划问题的表达式为：

满足约束条件：

x₀＝x(0)

h_o(x,u)≥0,o＝0,…,M

其中，J为基于车辆状态的指标函数，F(x,u)为车辆状态量与车辆控制量的指标函数，G(Δu)为车辆状态量变化率指标函数，x为车辆状态量(如x＝[v_x,v_y,ω]^T，v_x表示车辆纵向速度，v_y表示车辆横向速度，ω表示车辆横摆角速度)，u为车辆控制量(如u＝[F_T,δ_f]^T，F_T表示纵向驱动力，δ_f表示前轮转角)，Δu为车辆控制量变化率，用来抑制不同任务切换过程中过大的纵横向控制冲击度，保证车辆行驶安全性；T为预测时域，x₀为车辆初始状态约束，为车辆状态方程约束，h_o(x,u)为车辆周围障碍物的避障约束，M为障碍物数量，o为障碍物编号。

进一步，步步骤S1中，为实现并行运动规划，对步骤S1构建的运动规划问题进行离散化，离散化后的表达式为：

满足约束条件：

s.t.g_i(x_i,u_i)＝0,i＝0,…,N

h_i(x_i,u_i,u_i-1)<0,i＝0,…,N

其中，J_i(x_i,u_i,u_i-1)＝F_i(x_i,u_i)+G_i(Δu_i),F_i(x_i,u_i)为第i个插值点的车辆状态量与控制量的指标函数，G_i(Δu_i)为反应第i个插值点的车辆控制量变化率指标函数，N为离散点个数，x_i为第i个插值点的车辆状态量(如x_i＝[v_x,v_y,ω]_i ^T)，u_i为第i个插值点的车辆控制量(如u_i＝[F_T,δ_f]_i ^T)，Δu_i＝u_i-u_i-1为第i个插值点的车辆控制量变化率，g_i(x_i,u_i)为包含第i个插值点的车辆状态模型约束以及车辆初始状态约束的等式约束集合，h_i(x_i,u_i,u_i-1)为第i个插值点的车辆周围障碍物的避障约束组成的不等式约束集合。

进一步，步步骤S2中，为保证运动规划过程的全局收敛性，通过引入全局变量z解耦优化变量x_i,u_i之间的耦合关系，并引入松弛变量将步骤S1离散化的运动规划问题转换为并行运动规划问题，表达式为：

满足约束条件：

其中，α为对偶变量，为二次型惩罚项，β为二次惩罚项系数，/>χ＝[χ₀ ^T,…,χ_N ^T]^T为解耦后的阶段优化变量，z＝[x₀ ^T,…,x_N ^T,u₀ ^T,…,u_N ^T]^T表示由所有车辆状态量x_i和车辆控制量u_i组成的全局变量，P_i(χ_i)为由第i个等式约束g_i(χ_i)和第i个不等式约束h_i(χ_i)组成的惩罚函数，σ_i为其对应的惩罚因子，A为关联矩阵；

其中，χ和A的具体表达式为：

其中，n_x为车辆状态量x_i的维度，n_u为车辆控制量u_i的维度。

进一步，步步骤S3中，采用分层交替方向乘子法求解步骤S2构建的并行运动规划问题，具体包括以下步骤：

S31：迭代变量及参数初始化，对并行求解过程的初始变量及所有参数进行初始化，包括：原始变量初值χ⁰、全局变量初值z⁰以及松弛变量初值对偶变量初值α¹，罚参数β¹>0以及/>对偶变量α边界/>松弛变量衰减比例σ，惩罚参数放大比例γ，内层迭代终止的精度阈值/>外层迭代终止精度阈值{ε₁,ε₂,ε₃}，外层最大迭代次数k_max，内层最大迭代次数l_max；

S32：当外层迭代终止精度条件不满足或外层迭代次数k<k_max时，依次执行S33和S34；

S33：内层并行求解：并行更新原始变量i＝0,…,N，然后依次更新全局变量z^k,l+1、松弛变量/>和拉格朗日乘子λ^k,l+1；

S34：外层求解：依据松弛变量更新对偶变量α^k+1与二次惩罚项参数β^k+1，其公式为：

其中，为对偶变量上下界，用于保证其有界，γ>1为二次惩罚项参数β的尺度参数，衰减系数σ∈(0,1)；

S35：根据设定的收敛判定条件，设定内层迭代精度阈值与外层迭代精度阈值{ε₁,ε₂,ε₃}，外层最大迭代次数k_max，内层最大迭代次数l_max，若内层与外层迭代精度均满足收敛阈值或达到最大迭代次数，则算法停止，否则转到S33。

进一步，步所述步骤S33具体包括以下步骤：

S331：并行求解原始变量i＝0,…,N，其公式为：

S332：依据原始变量χ^k,l+1求解全局变量z^k,l+1，其公式为：

S333：依据原始变量χ^k,l+1和全局变量z^k,l+1求解松弛变量其公式为：

S334：依据原始变量χ^k,l+1、全局变量z^k,l+1、松弛变量求解拉格朗日乘子λ^k,l+1，其公式为：

其中，λ为拉格朗日乘子，ρ为二次惩罚项系数，上标k表示第k次外层迭代，上标l表示第l次内层迭代，为增广拉格朗日函数。

本发明的有益效果在于：本发明方法大大降低了将自动驾驶运动规划问题的优化维度和约束规模，简化了问题难度，同时通过将单一车载处理器的运载负荷分摊到多个并行处理器上，显著提高了自动驾驶运动规划问题的求解实时性。

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

附图说明

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

图1为本发明自动驾驶运动规划问题的并行求解方法整体流程图；

图2为基于全局变量一致性优化的耦合变量解耦示意图；

图3为并行求解过程示意图；

图4为本发明方法与传统方法的问题维度和约束数量对比结果示意图。

具体实施方式

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

请参阅图1～图4，本发明优选了一种自动驾驶运动规划问题的并行求解方法，其整体流程如图1所示，包含如下步骤：

S1：构建运动规划问题，并对其离散化；

步骤S1中构建的运动规划问题形式为：

满足约束条件：

x₀＝x(0)

h_o(x,u)≥0,o＝0,…,M

其中，J为基于车辆状态的指标函数，F(x,u)为车辆状态量与车辆控制量的指标函数，G(Δu)为车辆状态量变化率指标函数，x为车辆状态量(如x＝[v_x,v_y,ω]^T，v_x表示车辆纵向速度，v_y表示车辆横向速度，ω表示车辆横摆角速度)，u为车辆控制量(如u＝[F_T,δ_f]^T，F_T表示纵向驱动力，δ_f表示前轮转角)，Δu为车辆控制量变化率，用来抑制不同任务切换过程中过大的纵横向控制冲击度，保证车辆行驶安全性，T为预测时域。x₀为车辆初始状态约束，为车辆状态方程约束，h_o(x,u)为车辆周围障碍物的避障约束，M为障碍物数量，o为障碍物编号。

运动规划问题离散化：为实现并行运动规划，对步骤S1构建的运动规划问题进行离散化处理，将其构造成如下形式：

满足约束条件：

s.t.g_i(x_i,u_i)＝0,i＝0,…,N

h_i(x_i,u_i,u_i-1)<0,i＝0,…,N

满足约束条件：

s.t.g_i(x_i,u_i)＝0,i＝0,…,N

h_i(x_i,u_i,u_i-1)<0,i＝0,…,N

其中，J_i(x_i,u_i,u_i-1)＝F_i(x_i,u_i)+G_i(Δu_i),F_i(x_i,u_i)为第i个插值点的车辆状态量与控制量的指标函数，G_i(Δu_i)为反应第i个插值点的车辆控制量变化率指标函数，N为离散点个数，x_i为第i个插值点的车辆状态量(如x_i＝[v_x,v_y,ω]_i ^T)，u_i为第i个插值点的车辆控制量(如u_i＝[F_T,δ_f]_i ^T)，Δu_i＝u_i-u_i-1为第i个插值点的车辆控制量变化率，g_i(x_i,u_i)为包含第i个插值点的车辆状态模型约束以及车辆初始状态约束的等式约束集合，h_i(x_i,u_i,u_i-1)为第i个插值点的车辆周围障碍物的避障约束组成的不等式约束集合。

S2：通过引入全局变量和松弛变量构建并行运动规划问题；

为保证运动规划过程的全局收敛性，通过引入全局变量z解耦优化变量x_i,u_i之间的耦合关系，其过程如图2所示，并引入松弛变量将S1所述的离散化运动规划问题转换为如下形式：

满足约束条件：

其中，α为对偶变量，为二次型惩罚项，β为二次惩罚项系数，/>χ＝[χ₀ ^T,…,χ_N ^T]^T为解耦后的阶段优化变量，z＝[x₀ ^T,…,x_N ^T,u₀ ^T,…,u_N ^T]^T表示由所有车辆状态量x_i和车辆控制量u_i组成的全局变量，P_i(χ_i)为由第i个等式约束g_i(χ_i)和第i个不等式约束h_i(χ_i)组成的惩罚函数，σ_i为其对应的惩罚因子，A为关联矩阵。

其中，χ和A的具体形式为：

n_x为车辆状态量x_i的维度，n_u为车辆控制量u_i的维度。

S3：针对S2中构建的并行运动规划问题，采用分层交替方向乘子法进行数值求解。

基于分层交替方向乘子法并行求解过程S3如图3所示，包含如下过程：

S31：迭代变量及参数初始化，对并行求解过程的初始变量及所有参数进行初始化，如原始变量初值χ⁰、全局变量初值z⁰以及松弛变量初值对偶变量初值α¹，罚参数β¹>0以及/>对偶变量α边界/>松弛变量衰减比例σ，惩罚参数放大比例γ，内层迭代终止的精度阈值/>外层迭代终止精度阈值{ε₁,ε₂,ε₃}，外层最大迭代次数k_max，内层最大迭代次数l_max；

S32：当外层迭代终止精度条件不满足或外层迭代次数k<k_max时，依次执行S33和S34；

S33：内层并行求解：当内层迭代终止精度条件不满足或内层迭代次数l<l_max，则并行更新原始变量i＝0,…,N、然后依次更新全局变量z^k,l+1、松弛变量/>和拉格朗日乘子λ^k,l+1，具体步骤为：

S331：并行求解原始变量i＝0,…,N，其公式为：

S332：依据原始变量χ^k,l+1求解全局变量z^k,l+1，其公式为：

S333：依据原始变量χ^k,l+1和全局变量z^k,l+1求解松弛变量其公式为：

S334：依据原始变量χ^k,l+1、全局变量z^k,l+1、松弛变量求解拉格朗日乘子λ^k,l+1，其公式为：

其中，λ为拉格朗日乘子，ρ为二次惩罚项系数，上标k表示第k次外层迭代，上标l表示第l次内层迭代，为增广拉格朗日函数。

S34：外层求解：依据S33中更新的松弛变量的更新结果，更新对偶变量α^k+1与二次惩罚项参数β^k+1，其公式为：

其中，为对偶变量上下界，用于保证其有界，γ>1为二次惩罚项参数β的尺度参数，衰减系数σ∈(0,1)。

S35：根据设定的收敛判定条件，设定内层迭代精度阈值与外层迭代精度阈值{ε₁,ε₂,ε₃}，外层最大迭代次数k_max，内层最大迭代次数l_max，若内层与外层迭代精度均满足收敛阈值或达到最大迭代次数，则算法停止，否则转到S33。

本实施例的并行求解优化参数设置如下表1所示：

表1优化参数表

对比实验：

如图4所示，本发明采用的并行求解方法与传统求解方法，在优化变量维度和约束数量上，都获得了很大的降低，可以看出，本发明采用的并行求解方法大大简化了问题求解难度和计算量，从而提高了自动驾驶运动规划问题的求解实时性。

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

Claims (2)

1.一种基于并行求解的自动驾驶运动规划方法，其特征在于，该方法具体包括以下步骤：

S1：构建运动规划问题，并对其离散化；

S2：通过引入全局变量和松弛变量，将步骤S1离散化的运动规划问题转换为并行运动规划问题；

S3：针对步骤S2构建的并行运动规划问题，采用分层交替方向乘子法进行数值求解；

步骤S1中，构建的运动规划问题的表达式为：

满足约束条件：

x₀＝x(0)

h_o(x,u)≥0，o＝0，...，M

其中，J为基于车辆状态的指标函数，F(x,u)为车辆状态量与车辆控制量的指标函数，G(Δu)为车辆状态量变化率指标函数，x为车辆状态量，u为车辆控制量，Δu为车辆控制量变化率，T为预测时域，x₀为车辆初始状态约束，为车辆状态方程约束，h_o(x,u)为车辆周围障碍物的避障约束，M为障碍物数量，o为障碍物编号；

步骤S1中，对步骤S1构建的运动规划问题进行离散化，离散化后的表达式为：

满足约束条件：

s.t.g_i(x_i，u_i)＝0，i＝0，...，N

h_i(x_i，u_i，u_i-1)＜0，i＝0，...，N

其中，J_i(x_i，u_i，u_i-1)＝F_i(x_i，u_i)+G_i(Δu_i)，F_i(x_i，u_i)为第i个插值点的车辆状态量与控制量的指标函数，G_i(Δu_i)为反应第i个插值点的车辆控制量变化率指标函数，N为离散点个数，x_i为第i个插值点的车辆状态量，u_i为第i个插值点的车辆控制量，Δu_i＝u_i-u_i-1为第i个插值点的车辆控制量变化率，g_i(x_i，u_i)为包含第i个插值点的车辆状态模型约束以及车辆初始状态约束的等式约束集合，h_i(x_i，u_i，u_i-1)为第i个插值点的车辆周围障碍物的避障约束组成的不等式约束集合；

步骤S2中，通过引入全局变量z解耦优化变量x_i，u_i之间的耦合关系，并引入松弛变量将步骤S1离散化的运动规划问题转换为并行运动规划问题，表达式为：

满足约束条件：

其中，α为对偶变量，为二次型惩罚项，β为二次惩罚项系数，/>χ＝[χ₀ ^T,…,χ_N ^T]^T为解耦后的阶段优化变量，z＝[x₀ ^T,…,x_N ^T,u₀ ^T,…,u_N ^T]^T表示由所有车辆状态量x_i和车辆控制量u_i组成的全局变量，P_i(χ_i)为由第i个等式约束g_i(χ_i)和第i个不等式约束h_i(χ_i)组成的惩罚函数，σ_i为其对应的惩罚因子，a为关联矩阵；

其中，χ和A的具体表达式为：

其中，n_x为车辆状态量x_i的维度，n_u为车辆控制量u_i的维度；

步骤S3中，采用分层交替方向乘子法求解步骤S2构建的并行运动规划问题，具体包括以下步骤：

S31：迭代变量及参数初始化，对并行求解过程的初始变量及所有参数进行初始化，包括：原始变量初值χ⁰、全局变量初值z⁰以及松弛变量初值对偶变量初值α¹，罚参数β¹>0以及/>对偶变量α边界/>松弛变量衰减比例σ，惩罚参数放大比例γ，内层迭代终止的精度阈值/>外层迭代终止精度阈值{ε₁,ε₂,ε₃}，外层最大迭代次数k_max，内层最大迭代次数l_max；

S32：当外层迭代终止精度条件不满足或外层迭代次数k<k_max时，依次执行S33和S34；

S33：内层并行求解：并行更新原始变量然后依次更新全局变量z^{k ,l+1}、松弛变量/>和拉格朗日乘子λ^k,l+1；

S34：外层求解：依据松弛变量更新对偶变量α^k+1与二次惩罚项参数β^k+1，其公式为：

其中，为对偶变量上下界，用于保证其有界，γ>1为二次惩罚项参数β的尺度参数，衰减系数σ∈(0,1)；

S35：根据设定的收敛判定条件，设定内层迭代精度阈值与外层迭代精度阈值{ε₁,ε₂,ε₃}，外层最大迭代次数k_max，内层最大迭代次数l_max，若内层与外层迭代精度均满足收敛阈值或达到最大迭代次数，则算法停止，否则转到S33。

2.根据权利要求1所述的基于并行求解的自动驾驶运动规划方法，其特征在于，所述步骤S33具体包括以下步骤：

S331：并行求解原始变量其公式为：

S332：依据原始变量χ^k,l+1求解全局变量z^k,l+1，其公式为：

S333：依据原始变量χ^k,l+1和全局变量z^k,l+1求解松弛变量其公式为：

S334：依据原始变量χ^k,l+1、全局变量z^k,l+1、松弛变量求解拉格朗日乘子λ^k,l+1，其公式为：

其中，λ为拉格朗日乘子，ρ为二次惩罚项系数，上标k表示第k次外层迭代，上标l表示第l次内层迭代，为增广拉格朗日函数。