CN112061114A - 一种基于自适应伪谱法的自主泊车系统最优路径控制方法 - Google Patents

Abstract

本发明公开了一种基于自适应伪谱法的自主泊车系统最优路径控制方法，首先建立自主泊车系统时间能量最优控制模型，并提出自适应伪谱法对其控制求解；然后通过自适应更换时间网格区间和高斯节点，使得自适应伪谱法求解得到的最优位移与期望位移在实际工业容许误差之内，输出最优位移。本发明具有高效、稳定、计算精度高等优点。

Description

一种基于自适应伪谱法的自主泊车系统最优路径控制方法

技术领域

本发明涉及自主泊车路径控制的技术领域，尤其涉及到一种基于自适应伪谱法的自主泊车系统最优路径控制方法。

背景技术

随着高科技领域的深入发展，人们对生活水平的要求提高和对美好交通出行的需要日益增长，推动了智能科学的研究的发展，而自主泊车系统为智能科学研究的重要分支之一。

和人类驾驶员相比，自主泊车系统能够更加全面的掌握即时路况并及时对汽车巡航状态进行调整，从而避免了人为失误造成的交通事故和伤亡，其发展与推广在保障行车安全、提高通行效率、建设智慧交通体系等方面具有重要意义。

在自动驾驶技术水平不断向着更高层级跃迁的渐进发展过程中，各层级技术均存在不足，即便是已实现产业化的辅助驾驶系统仍存在着继续提升的潜力。在自动驾驶车辆的诸多核心技术模块中，运动规划负责生成车辆的局部行驶轨迹，是体现驾驶智慧水平的关键环节。但目前，对于自主泊车系统的系统模型，还没有一种高效、稳定的最优控制方法对其控制求解。

发明内容

本发明的目的在于克服现有技术的不足，提供一种基于自适应伪谱法的自主泊车系统最优路径控制方法。

为实现上述目的，本发明所提供的技术方案为：

一种基于自适应伪谱法的自主泊车系统最优路径控制方法，包括以下步骤：

S1、对泊车系统进行数学建模，得到自主泊车系统时间能量最优控制模型；

S2、初始化时间网格，在每个时间网格S_k上，将泊车系统控制模型转换成标准Bolza最优控制问题；

S3、根据步骤S2所得标准Bolza最优控制问题，采用自适应伪谱法对其求数值解；

S4、在自主泊车过程中，把数值解和解析解的绝对值之差所容许的最大误差设为ε，通过判断求出全局的初始化网格数值解和解析解的绝对值之差是否小于ε，若满足，则得到最优路径规划，输出最优路径规划；若不满足，通过自适应的更换时间网格区间和高斯节点，直到满足全局的数值解和解析解的绝对值之差小于ε。

进一步地，所述步骤S1对泊车系统进行数学建模的具体过程如下：

由前轮转向的四轮车辆设计时符合车辆转向几何原理，得车辆的运动学微分方程为：

方程(1)为状态约束方程，并简记为：

其中，

为状态变量，U＝(a,ω)为控制变量，L为车辆长度，w为车辆宽度，L_W为前后轴中心间距，v为后轴中心速度，

为等效前轮转角，a为后轴中心加速度，θ为车辆朝向与全局坐标系x轴夹角；

对于车辆来说，其运动受到其固有的物理限制，车辆的前轮转角和其角速度具有一个限定的范围，车辆的加速性能和制动性能受到许多因素的影响，所以车辆会受到一定的物理约束为：

方程(2)称为边界约束方程，并简记为：

C(X，U，t)≤0；

上式中，v_max为最大的后轴中心速度，a_max为最大的后轴中心加速度，

为最大等效前轮转角，ω为控制变量，ω_max为最大控制变量；

为了要求在整个车辆泊车过程所花的时间和能量最少，设计性能指标函数为：

上式中，σ为泊车时间的权重，t₀为泊车开始时间，t_f为泊车停止时间，

为泊车过程消耗的能量。

进一步地，所述步骤S2的具体过程如下：

首先初始化网格空间[t₀,t_f]，把[t₀,t_f]网格化为K个网格区间S_k＝[t_k,t_k+1]组成，k＝1,…,K，即

在每一个网格空间S_k上，作如下等量代换：

把本来的泊车系统的函数区间[t₀,t_f]映射到区间[-1,1]上；

通过以上等量代换(6)，把原问题变成标准Bolza最优控制问题：

约束条件为：

进一步地，所述步骤S3的具体过程如下：

在每个时间网格S_k内，

把状态变量散化：

把控制变量离散化：

其中，

表示高斯节点且满足：

-1＝τ₀＜τ₁＜τ₂＜τ₃＜...＜τ_N＜τ_N+1＝1；

对离散后的状态变量求导：

对末端状态变量离散：

其中X₀为初始状态变量，ω_i为高斯权重；

对性能指标函数进行离散化：

在自适应伪谱法下，经过(7)-(11)的离散化，标准Bolza最优控制问题被转化为以下非线性规划问题：

约束条件为：

C(X(τ_i),U(τ_i),τ_i)≤0；

然后使用稀疏非线性优化软件包求解该非线性规划问题。

进一步地，所述步骤S4的具体过程如下：

在初始化网格区间，采用自适应伪谱法把其转化为非线性规划问题，并求解该非线性规划问题得到的全局数值解X后，通过判断数值解X与解析解X_des是否满足：

|X-X_des|＜ε (12)

若满足(12)，计算终止，输出数值解X，得到泊车系统最优路径；

若不满足(12)，通过判断每个网格区间的数值解是否满足(12)式，找出不满足(12)式的网格区间通过更换高斯节点数为N+1，计算非线性规划问题，而满足(12)式的网格区间通过更换高斯节点数为N-1，计算非线性规划问题；

再次通过判断是否满足(12)式：

若满足(12)，终止计算，得到泊车系统最优路径；

若不满足(12)，则通过判断每个网格区间的数值解是否满足(12)，把满足(12)的所有网格区间记为集合A，把不满足(12)的所有网格区间记为集合B；

在集合A中判断任意的两个网格区间是否为相邻网格，如果是，则合并网格；如果不是，则减少高斯节点数；

在集合B中判断每个小网格区间是否为光滑网格，如果是，则增加高斯节点数；如果不是，则以光滑点拆分网格；

通过以上对初始化网格的处理，初始化网格进行了更换网格化，基于该新网格区间，再次把标准Bolza最优控制问题转化为非线性规划问题；求解非线性规划问题得到新的数值解，判断是否满足(12)，若满足，输出最优路径；若不满足，回到以上重新更换网格区间步骤，如此重复，直到满足全局最优解，终止计算，输出最优路径。

与现有技术相比，本方案原理及优点如下：

本方案首次建立了自主泊车系统时间能量最优控制模型，并提出自适应伪谱法对其控制求解；通过自适应更换时间网格区间和高斯节点，使得自适应伪谱法求解得到的最优位移与期望位移在实际工业容许误差之内，输出最优位移。

本方案具有高效、稳定、计算精度高等优点。

附图说明

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

图1为车辆运动示意图；

图2为自适应伪谱法的流程图。

具体实施方式

下面结合具体实施例对本发明作进一步说明：

本实施例所述的一种基于自适应伪谱法的自主泊车系统最优路径控制方法，包括以下步骤：

S1、对泊车系统进行数学建模，得到自主泊车系统时间能量最优控制模型，具体过程如下：

如图1所示，由前轮转向的四轮车辆设计时符合车辆转向几何原理，得车辆的运动学微分方程为：

方程(1)为状态约束方程，并简记为：

其中，

为状态变量，U＝(a,ω)为控制变量，L为车辆长度，w为车辆宽度，L_W为前后轴中心间距，v为后轴中心速度，

为等效前轮转角，a为后轴中心加速度，θ为车辆朝向与全局坐标系x轴夹角；

对于车辆来说，其运动受到其固有的物理限制，车辆的前轮转角和其角速度具有一个限定的范围，车辆的加速性能和制动性能受到许多因素的影响，所以车辆会受到一定的物理约束为：

方程(2)称为边界约束方程，并简记为：

C(X，U，t)≤0；

上式中，v_max为最大的后轴中心速度，a_max为最大的后轴中心加速度，

为最大等效前轮转角，ω为控制变量，ω_max为最大控制变量；

为了要求在整个车辆泊车过程所花的时间和能量最少，设计性能指标函数为：

上式中，σ为泊车时间的权重，t₀为泊车开始时间，t_f为泊车停止时间，

为泊车过程消耗的能量。

S2、初始化时间网格，在每个时间网格S_k上，将泊车系统控制模型转换成标准Bolza最优控制问题；

本步骤中，首先初始化网格空间[t₀,t_f]，把[t₀,t_f]网格化为K个网格区间S_k＝[t_k,t_k+1]组成，k＝1,…,K，即

在每一个网格空间S_k上，作如下等量代换：

把本来的泊车系统的函数区间[t₀,t_f]映射到区间[-1,1]上；

通过以上等量代换(6)，把原问题变成标准Bolza最优控制问题：

约束条件为：

S3、根据步骤S2所得标准Bolza最优控制问题，采用自适应伪谱法对其求数值解；求解过程如下：

在每个时间网格S_k内，

把状态变量散化：

把控制变量离散化：

其中，

表示高斯节点且满足：

-1＝τ₀＜τ₁＜τ₂＜τ₃＜...＜τ_N＜τ_N+1＝1；

对离散后的状态变量求导：

对末端状态变量离散：

其中X₀为初始状态变量，ω_i为高斯权重；

对性能指标函数进行离散化：

在自适应伪谱法下，经过(7)-(11)的离散化，标准Bolza最优控制问题被转化为以下非线性规划问题(NLP)：

约束条件为：

C(X(τ_i),U(τ_i),τ_i)≤0；

然后使用稀疏非线性优化软件包求解该非线性规划问题。

S4、如图2所示，在初始化网格区间，采用自适应伪谱法把其转化为非线性规划问题，并求解该非线性规划问题得到的全局数值解X后，通过判断数值解X与解析解X_des是否满足：

|X-X_des|＜ε (12)

若满足(12)，计算终止，输出数值解X，得到泊车系统最优路径；

若不满足(12)，通过判断每个网格区间的数值解是否满足(12)式，找出不满足(12)式的网格区间通过更换高斯节点数为N+1，计算非线性规划问题，而满足(12)式的网格区间通过更换高斯节点数为N-1，计算非线性规划问题；

再次通过判断是否满足(12)式：

若满足(12)，终止计算，得到泊车系统最优路径；

若不满足(12)，则通过判断每个网格区间的数值解是否满足(12)，把满足(12)的所有网格区间记为集合A，把不满足(12)的所有网格区间记为集合B；

在集合A中判断任意的两个网格区间是否为相邻网格，如果是，则合并网格；如果不是，则减少高斯节点数；

在集合B中判断每个小网格区间是否为光滑网格，如果是，则增加高斯节点数；如果不是，则以光滑点拆分网格；

通过以上对初始化网格的处理，初始化网格进行了更换网格化，基于该新网格区间，再次把标准Bolza最优控制问题转化为非线性规划问题；求解非线性规划问题得到新的数值解，判断是否满足(12)，若满足，输出最优路径；若不满足，回到以上重新更换网格区间步骤，如此重复，直到满足全局最优解，终止计算，输出最优路径。

以上所述之实施例子只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (5)

1.一种基于自适应伪谱法的自主泊车系统最优路径控制方法，其特征在于，包括以下步骤：

S1、对泊车系统进行数学建模，得到自主泊车系统时间能量最优控制模型；

S2、初始化时间网格，在每个时间网格S_k上，将泊车系统控制模型转换成标准Bolza最优控制问题；

S3、根据步骤S2所得标准Bolza最优控制问题，采用自适应伪谱法对其求数值解；

S4、在自主泊车过程中，把数值解和解析解的绝对值之差所容许的最大误差设为ε，通过判断求出全局的初始化网格数值解和解析解的绝对值之差是否小于ε，若满足，则得到最优路径规划，输出最优路径规划；若不满足，通过自适应的更换时间网格区间和高斯节点，直到满足全局的数值解和解析解的绝对值之差小于ε。

2.根据权利要求1所述的一种基于自适应伪谱法的自主泊车系统最优路径控制方法，其特征在于，所述步骤S1对泊车系统进行数学建模的具体过程如下：

由前轮转向的四轮车辆设计时符合车辆转向几何原理，得车辆的运动学微分方程为：

方程(1)为状态约束方程，并简记为：

其中，

为状态变量，U＝(a,ω)为控制变量，L为车辆长度，w为车辆宽度，L_W为前后轴中心间距，v为后轴中心速度，

为等效前轮转角，a为后轴中心加速度，θ为车辆朝向与全局坐标系x轴夹角；

对于车辆来说，其运动受到其固有的物理限制，车辆的前轮转角和其角速度具有一个限定的范围，车辆的加速性能和制动性能受到许多因素的影响，所以车辆会受到一定的物理约束为：

方程(2)称为边界约束方程，并简记为：

C(X，U，t)≤0；

上式中，v_max为最大的后轴中心速度，a_max为最大的后轴中心加速度，

为最大等效前轮转角，ω为控制变量，ω_max为最大控制变量；

为了要求在整个车辆泊车过程所花的时间和能量最少，设计性能指标函数为：

上式中，σ为泊车时间的权重，t₀为泊车开始时间，t_f为泊车停止时间，

为泊车过程消耗的能量。

3.根据权利要求2所述的一种基于自适应伪谱法的自主泊车系统最优路径控制方法，其特征在于，所述步骤S2的具体过程如下：

首先初始化网格空间[t₀,t_f]，把[t₀,t_f]网格化为K个网格区间S_k＝[t_k,t_k+1]组成，k＝1,…,K，即

在每一个网格空间S_k上，作如下等量代换：

把本来的泊车系统的函数区间[t₀,t_f]映射到区间[-1,1]上；

通过以上等量代换(6)，把原问题变成标准Bolza最优控制问题：

约束条件为：

4.根据权利要求3所述的一种基于自适应伪谱法的自主泊车系统最优路径控制方法，其特征在于，所述步骤S3的具体过程如下：

在每个时间网格S_k内，

把状态变量散化：

把控制变量离散化：

其中，

1≤i≤N；

表示高斯节点且满足：

-1＝τ₀<τ₁<τ₂<τ₃<...<τ_N<τ_N+1＝1；

对离散后的状态变量求导：

对末端状态变量离散：

其中X₀为初始状态变量，ω_i为高斯权重；

对性能指标函数进行离散化：

在自适应伪谱法下，经过(7)-(11)的离散化，标准Bolza最优控制问题被转化为以下非线性规划问题：

约束条件为：

C(X(τ_i),U(τ_i),τ_i)≤0；

然后使用稀疏非线性优化软件包求解该非线性规划问题。

5.根据权利要求4所述的一种基于自适应伪谱法的自主泊车系统最优路径控制方法，其特征在于，所述步骤S4的具体过程如下：

在初始化网格区间，采用自适应伪谱法把其转化为非线性规划问题，并求解该非线性规划问题得到的全局数值解X后，通过判断数值解X与解析解X_des是否满足：

|X-X_des|<ε (12)

若满足(12)，计算终止，输出数值解X，得到泊车系统最优路径；

若不满足(12)，通过判断每个网格区间的数值解是否满足(12)式，找出不满足(12)式的网格区间通过更换高斯节点数为N+1，计算非线性规划问题，而满足(12)式的网格区间通过更换高斯节点数为N-1，计算非线性规划问题；

再次通过判断是否满足(12)式：

若满足(12)，终止计算，得到泊车系统最优路径；

若不满足(12)，则通过判断每个网格区间的数值解是否满足(12)，把满足(12)的所有网格区间记为集合A，把不满足(12)的所有网格区间记为集合B；

在集合A中判断任意的两个网格区间是否为相邻网格，如果是，则合并网格；如果不是，则减少高斯节点数；

在集合B中判断每个小网格区间是否为光滑网格，如果是，则增加高斯节点数；如果不是，则以光滑点拆分网格；

通过以上对初始化网格的处理，初始化网格进行了更换网格化，基于该新网格区间，再次把标准Bolza最优控制问题转化为非线性规划问题；求解非线性规划问题得到新的数值解，判断是否满足(12)，若满足，输出最优路径；若不满足，回到以上重新更换网格区间步骤，如此重复，直到满足全局最优解，终止计算，输出最优路径。

Images