CN112947074A - 一种基于抽取虚拟飞机策略的有杆牵引飞机系统轨迹规划方法 - Google Patents

Abstract

一种基于抽取虚拟飞机策略的有杆牵引飞机系统轨迹规划方法，属于有杆牵引飞机系统的自动化与控制技术领域。首先，将有杆牵引飞机系统中的飞机部分转换为虚拟飞机系统，其可以同时实现正向和倒车运动；然后，基于该虚拟飞机系统建立其轨迹规划的最优控制模型，采用最优控制算法进行解算，得到虚拟飞机系统的最优轨迹及对应控制变量；最后，使有杆牵引飞机系统中的飞机跟踪虚拟飞机系统轨迹，建立最优跟踪控制模型，采用最优控制方法进行求解得到有杆牵引飞机系统轨迹及对应的控制律。本发明能够有效地解决有杆牵引飞机系统的精确轨迹规划与控制难题，同时提高了计算效率和鲁棒性；能够为有杆牵引飞机系统轨迹规划与控制问题提供合理的解决方案。

Description

一种基于抽取虚拟飞机策略的有杆牵引飞机系统轨迹规划 方法

技术领域

本发明属于有杆牵引飞机系统的自动化与控制技术领域，涉及一种基于抽取虚拟飞机策略的有杆牵引飞机系统轨迹规划方法。

背景技术

有杆牵引飞机系统轨迹规划技术主要包含对场景进行建模、路径规划以及控制问题，在设计轨迹规划方法时，需要兼顾效率、精度以及工程许用性这3方面。

目前，针对杆牵引飞机系统的轨迹规划技术已经发展出各种各样的方法，包括启发式算法、最优控制方法等。在现有的方法中，启发式算法具有计算效率高、易于实现等优点，但这类算法仍存在如下不足：

(1)在进行轨迹规划时，未考虑有杆牵引系统的控制问题，从而使所得到的结果可能不符合工程实际；

(2)难以考虑有杆牵引飞机系统的运动学约束或者动力学约束；

(3)难以严格满足终端约束条件；

(4)计算精度和效率难以均衡。

最优控制方法以其简洁的数学公式和统一的框架而备受关注，该类算法可将有杆牵引飞机系统的运动学约束或者动力学约束、终端约束条件考虑其中，可有效兼顾轨迹规划问题和控制问题，所得到的结果具有较高的工程许用性，但是由于其强非线性特点以及常常存在大规模的非凸约束，可能会超出标准数值求解器的处理能力以至于难以求解。这类算法存在如下问题：

(1)由于有杆牵引飞机系统运动学或者动力学模型非线性强，求解规模大，导致整体计算效率低，在配点较多的情况下易发生“维数灾难”；

(2)在采用这类算法进行有杆牵引飞机系统轨迹规划时难收敛，通常需要设计合理的初始猜测解。

当前，工程应用上迫切需要一种能兼顾计算精度和效率的、具有良好适用性的有杆牵引飞机系统轨迹规划算法。

发明内容

为了解决上述技术问题，本发明提出了一种用于有杆牵引飞机系统轨迹规划问题，且能兼顾效率、精度及工程许用性的、具有良好适用性的方法。

为了达到上述目的，本发明所采用的技术方案是：

一种基于抽取虚拟飞机策略的有杆牵引飞机系统轨迹规划方法，首先，将有杆牵引飞机系统中的飞机部分抽象成一个虚拟的飞机系统，其可以同时实现正向和倒车这两种运动；然后，基于该虚拟飞机系统建立其轨迹规划的最优控制模型，并采用最优控制算法进行解算，从而得到虚拟飞机系统的最优轨迹及对应控制变量；最后，使有杆牵引飞机系统中的飞机跟踪虚拟飞机系统轨迹，从而基于有杆牵引飞机系统的运动学模型建立最优跟踪控制模型，并采用最优控制方法进行求解以得到有杆牵引飞机系统轨迹及对应的控制律。

作为优选，典型的有杆牵引飞机系统如图1所示。对于飞机和牵引车后轮的中点，用P₁(x₁,y₁)和P₄(x₄,y₄)来表示它们的位置，它们的速度分别定义为v₁和v₃。飞机和牵引机的铰链点位置分别表示为P₂(x₂,y₂)和P₃(x₃,y₃)。变量θ₁，θ₂和θ₃分别是飞机、牵引杆和牵引车的方位角。然后，将两个夹角定义为β₁＝θ₂-θ₁和β₂＝θ₃-θ₂，其中，β₁表示飞机与牵引杆之间的纵向夹角，β₂表示牵引杆与牵引车之间的纵向夹角；牵引车前轮转向角表示为α；L₁、L₂、M、L₃分别是

的长度。整个牵引系统用圆心为O₁(x₁+D₁ cos θ₁,y₁+D₁ sinθ₁)、半径为R₁的特征圆表示，其中，D₁表示有杆牵引飞机系统的特征圆圆心位置到飞机后轮中心位置之间的距离。

根据速度分解可得：

为实现有杆牵引飞机系统的轨迹规划，可采取如下方法进行轨迹规划，包括以下步骤。

步骤1：将原系统中的飞机部分抽取出来，并将其转换为结构更简单的虚拟飞机系统，牵引系统由虚拟飞机系统代替。由于原系统中飞机部分在牵引车和牵引杆的共同作用下可实现转弯、倒车、正向运动，本专利中所构造的虚拟飞机系统是在不改变飞机运动特点的前提下将其单独抽取出来，故仍认为虚拟飞机系统具备转弯、倒车、正向运动的功能。

步骤2：通过建立虚拟飞机运动学模型、确定避碰约束、状态-控制约束，进一步建立虚拟飞机系统的轨迹规划最优控制模型，并采用最优控制方法中较为常用的伪谱法对虚拟飞机系统的轨迹规划最优控制模型进行求解，实现对所构建虚拟飞机系统的轨迹规划与控制。

步骤2-1：虚拟飞机系统的运动学建模

令X_V＝(x₁,y₁,θ₁,v₁)^T为状态变量，

为控制变量，t表示时间变量；

为虚拟系统的运动学模型，则其可表示为：

步骤2-2：避碰约束建模

令x_oi和y_oi为第i(i∈N⁺)个障碍物的几何中心位置，a_i和b_i为第i个障碍物的宽和高，

为其形状描述参数，dist为安全缓冲距离，dist≥0m。则当虚拟飞机系统满足如下关系时可实现避碰：

步骤2-3：确定轨迹规划阶段的状态-控制约束

令

和

分别为v₁在轨迹规划阶段的最小值和最大值，

和

分别为

的最小值和最大值，

和

分别为

的最小值和最大值。则虚拟飞机系统的状态-控制约束为：

步骤2-4：建立虚拟飞机系统的轨迹规划最优控制模型

以虚拟飞机系统的加速度和转向角速度作为控制变量，通过最小化控制变量以减少突然加减速和转弯，并最小化末端时间来缩短调运时间以提高调运效率，故目标函数可以选择为“时间-能量”最优类型，并建立如下所示的轨迹规划最优控制模型：

式中，J_V为目标函数的数值；t₀和t_f为起始时间和末端时间；X_V0和X_Vf为边界条件；R_V是正定矩阵；h_V≤0为不等式约束，其是由公式(3)和公式(4)组成。

步骤2-5：采用伪谱法对虚拟飞机系统的轨迹规划最优控制模型进行求解

对于如公式(5)所示的轨迹规划最优控制模型，可采用伪谱法进行求解，从而可得到虚拟飞机系统最优轨迹以及对应的状态变量和控制变量，此处将对应控制变量和状态变量表示分别为(x_1r,y_1r,θ_1r,v_1r)^T和

步骤3：为根据虚拟飞机系统轨迹规划结果得出有杆牵引飞机系统的轨迹与控制量，可使有杆牵引飞机系统中的飞机尽量沿虚拟飞机系统的最优轨迹进行运动，从而反推出有杆牵引飞机系统的实际轨迹和控制。

步骤3-1：有杆牵引飞机系统的运动学建模

令状态变量X＝(x₁,y₁,θ₁,β₁,β₂,α,v₁)^T，控制向量U＝(u₁,u₂)^T，

表示牵引车前轮转向角的角速度，

表示牵引系统中飞机的加速度；M表示牵引杆与牵引车铰接点到牵引车后轮中心位置距离。则有杆牵引飞机系统的运动学模型

为：

步骤3-2：确定跟踪误差约束

由于实际轨迹不能与标准轨迹完全重合，实际轨迹与标准轨迹之间不可避免地存在偏差，故可结合公式(3)对偏差的范围进行约束，以确保偏差不大于dist，则：

(x₁-x_1r)²+(y₁-y_1r)²≤(dist)² (7)

步骤3-3：确定跟踪阶段的状态-控制约束

令v_1min和v_1max分别为v₁在轨迹跟踪阶段的最小值和最大值，β_1min和β_1max分别为β₁的最小值和最大值，β_2min和β_2max分别为β₂的最小值和最大值，α_min和α_max分别为α的最小值和最大值，u_1min和u_1max分别为u₁的最小值和最大值，u_2min和u_2max分别为u₂的最小值和最大值。则有杆牵引系统控制变量、v₁、β₁和β₂应满足如下状态-控制约束：

步骤3-4：建立有杆牵引系统的最优跟踪控制模型

通过使有杆牵引飞机系统跟踪虚拟飞机系统的最优轨迹，从而反推出有杆牵引飞机系统的实际轨迹和控制，故可建立如下最优跟踪控制模型：

其中，X₀和X_f为边界条件；R是正定矩阵；参考轨迹X_r＝(x_1r,y_1r,θ_1r,β_1r,0,0,v_1r)^T；β_1r可根据步骤2中最优轨迹对应控制变量得到，即

P为权重矩阵，h≤0由公式(7)和公式(8)组成。

步骤3-5：对最优跟踪控制模型进行求解

对于公式(9)，可采用最优控制方法中常用的保辛伪谱算法进行求解，能够得到最优轨迹对应的状态变量和控制变量，从而实现对有杆牵引飞机系统的轨迹规划与控制。

相对于现有技术，本发明有益效果为：本发明能以更高的精度、效率和稳定性对有杆牵引飞机系统轨迹规划问题进行求解，可有效解决基于有杆牵引系统运动学模型进行轨迹规划时易发散、不收敛等难题。本发明基于有杆牵引系统的运动学模型，将轨迹规划问题与控制问题统筹考虑，不仅可得到满足状态和控制约束关系的平滑轨迹，还可得到最优轨迹对应的控制律，所得结论可直接应用于工程实际，具有更好的工程许用性，且具有很强的可操作性和可行性，便于实际应用。

附图说明

图1为本发明的有杆牵引飞机系统示意图。

图2为本发明的虚拟飞机系统示意图。

图3为本发明的虚拟飞机系统运动轨迹图。

图4为本发明的虚拟飞机系统速度变化图。

图5为本发明的有杆牵引飞机系统与虚拟飞机系统的运动轨迹误差图。

图6为本发明的牵引车速度和转向角变化图。

图7为本发明的有杆牵引飞机系统控制变化图。

图8为本发明的系统流程图。

具体实施方式

以下结合具体实施例对本发明做进一步说明。

对于某一待调运的有杆牵引飞机系统，采用半径为12m的圆形来描述(包含dist＝1m的缓冲距离)其轮廓，其几何参数为L₁＝6m,L₂＝4m,L₃＝2m,M＝1m,D₁＝4m,边界状态为X₀＝(90m,50m,90°,0°,0°,0°,0m/s)^T，X_f＝(195m,50m,90°,free,free,free,0m/s)^T。

调运环境中一共存在3个障碍物，障碍物1的位置为(70m，50m)，采用宽为10m、高15m的矩形描述；障碍物2的位置为(125m，50m)，宽40m、高20m；障碍物3的位置为(175m，95m)，采用宽50m、高15m的矩形描述。

根据步骤1，首先将有杆牵引飞机系统中的飞机部分抽取出来，并将其作为结构更简单的虚拟飞机系统。

根据步骤2，通过建立虚拟飞机运动学模型、确定避碰约束、状态-控制约束，进一步建立虚拟飞机系统的轨迹规划最优控制模型，并采用最优控制方法中较为常用的伪谱法对虚拟飞机系统的轨迹规划最优控制模型进行求解，实现对所构建虚拟飞机系统的轨迹规划与控制。

根据步骤2-1，可得虚拟飞机系统运动学模型为：

根据步骤2-2以及3个障碍物的外形参数，可将避碰约束描述为：

本案例中，

故根据步骤2-3可将轨迹规划阶段的状态-控制约束描述为：

根据步骤2-4，以虚拟飞机系统的加速度和转向角速度作为控制变量，通过最小化控制变量以减少突然加减速和转弯，并最小化末端时间来缩短调运时间以提高调运效率，建立虚拟飞机系统的轨迹规划最优控制模型。本案例中，t₀＝0s，X_V(t₀)＝(90m,50m,90°,0m/s)^T，X_V(t_f)＝(195m,50m,90°,0m/s)^T，公式(5)中R为单位矩阵，则虚拟飞机系统的轨迹规划最优控制模型为：

根据步骤2-5，采用伪谱法对轨迹规划最优控制模型进行求解，可得到虚拟系统的运动轨迹如图3所示，对应的速度变量如图4所示。

根据步骤3，使有杆牵引飞机系统跟踪虚拟飞机系统的最优轨迹，从而反推出有杆牵引飞机系统的实际轨迹和控制。

根据步骤3-1，可得到有杆牵引飞机系统的运动学模型为

根据步骤3-2，由于实际轨迹不能与标准轨迹完全重合，实际轨迹与标准轨迹之间不可避免地存在偏差，故可结合公式(3)对偏差的范围进行约束，以确保偏差不大于1m，则跟踪误差约束为：

(x₁-x_1r)²+(y₁-y_1r)²≤1

根据步骤3-3，有杆牵引飞机系统控制量、v₁、β₁和β₂应该在某一安全范围之内。此处v_{1 min}＝-1.6m/s、v_{1 max}＝1.6m/s、β_{1 min}＝-0.95、β _1max＝0.95、β_{2 min}＝-0.95、β_{2 max}＝0.95、α_min＝-0.95、α_max＝0.95、u_{1 min}＝-2.00、u_{1 max}＝2.00、u_{2 min}＝-2.00、u_{2 max}＝2.00，故状态-控制约束为：

根据步骤3-4，使有杆牵引飞机系统尽量沿虚拟飞机系统的最优轨迹进行运动，故可建立如公式(9)所示的最优跟踪控制模型。此处，X₀＝(90m,50m,90°,0°,0°,0°,0m/s)^T，X_f＝(195m,50m,90°,free,free,free,0m/s)^T，R是单位矩阵，参考轨迹X_r＝(x_1r,y_1r,θ_1r,β_1r,0,0,v_1r)^T，β_1r可根据步骤2中最优轨迹对应控制变量得到，即

P＝diag([1,1,1,0.1,0.1,0.1,0.1])，则有杆牵引飞机系统的最优跟踪控制模型为：

根据步骤3-5，采用最优控制方法中较为常用的保辛伪谱算法对最优跟踪控制模型进行求解，可得到描述有杆牵引飞机系统的实际轨迹，从而得到有杆牵引飞机系统与虚拟飞机系统的运动轨迹误差如图5所示，牵引车的速度和转向角如图6所示，有杆牵引飞机系统控制变量如图7所示，从而实现对有杆牵引飞机系统的轨迹规划与控制。

本发明在对有杆牵引飞机系统结构、运动学、轨迹规划及控制问题进行深入分析的基础上，打破原有方法难以兼顾轨迹规划与系统运动学、控制问题的不足，将有杆牵引飞机系统中的飞机部分抽象成虚拟飞机系统并建立虚拟系统轨迹规划的最优控制模型，通过采用最优控制算法进行求解以得到虚拟飞机系统的最优轨迹及控制量，然后使有杆牵引飞机系统跟踪虚拟飞机系统轨迹并建立最优跟踪控制模型，最后采用最优控制方法进行求解得到有杆牵引飞机系统轨迹及控制律，能够有效地解决了有杆牵引飞机系统的精确轨迹规划与控制难题，同时提高了计算效率和鲁棒性。

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

Claims (4)

1.一种基于抽取虚拟飞机策略的有杆牵引飞机系统轨迹规划方法，其特征在于，包括以下步骤：

首先，将有杆牵引飞机系统中的飞机部分抽取为虚拟飞机系统，其中，虚拟飞机系统可以同时实现转弯、正向和倒运动的功能；

然后，基于该虚拟飞机系统的运动学模型、确定避碰约束、状态-控制约束建立虚拟飞机系统轨迹规划的最优控制模型，并采用最优控制算法进行解算，得到虚拟飞机系统的最优轨迹及对应控制变量；

最后，将虚拟飞机系统最优轨迹作为标准轨迹，使有杆牵引飞机系统中的飞机跟踪虚拟飞机系统轨迹，并基于有杆牵引飞机系统的运动学模型建立最优跟踪控制模型，采用最优控制方法对最优跟踪控制模型进行求解，从而反推出有杆牵引飞机系统的实际轨迹及对应的控制律。

2.根据权利要求1所述的一种基于抽取虚拟飞机策略的有杆牵引飞机系统轨迹规划方法，其特征在于，包括以下具体步骤：

步骤1：将原系统中的飞机部分转换为虚拟飞机系统，牵引系统由虚拟飞机系统代替；

步骤2：建立虚拟飞机系统的轨迹规划最优控制模型，采用最优控制方法进行求解，实现对所构建虚拟飞机系统的轨迹规划与控制；具体如下：

步骤2-1：虚拟飞机系统的运动学建模

令X_V＝(x₁,y₁,θ₁,v₁)^T为状态变量；

为控制变量，

β₁为飞机前轮转向角；

为飞机加速度；t表示时间变量；(x₁,y₁)为飞机后轮中点位置坐标；θ₁为飞机方位角；v₁为飞机速度；L₁为飞机前后轮距；

为虚拟系统的运动学模型，则其可表示为：

步骤2-2：避碰约束建模

令(x_oi，y_oi)为第i(i∈N⁺)个障碍物的几何中心位置，a_i和b_i为第i个障碍物的宽和高，

为其形状描述参数，dist为安全缓冲距离，dist≥0m；则当虚拟飞机系统满足如下关系时可实现避碰：

步骤2-3：确定轨迹规划阶段的状态-控制约束

令

和

分别为v₁在轨迹规划阶段的最小值和最大值，

和

分别为

的最小值和最大值，

和

分别为

的最小值和最大值；则虚拟飞机系统的状态-控制约束为：

步骤2-4：建立虚拟飞机系统的轨迹规划最优控制模型

以虚拟飞机系统的加速度和转向角速度作为控制变量，通过最小化控制变量以减少突然加减速和转弯，并最小化末端时间来缩短调运时间以提高调运效率，故目标函数可选择为“时间-能量”最优类型，并建立如下所示的虚拟飞机系统的轨迹规划最优控制模型：

式中，J_V为目标函数得值；t₀和t_f为起始时间和末端时间；X_V0和X_Vf为边界条件；R_V是正定矩阵；h_V≤0为不等式约束，其是由公式(3)和公式(4)组成的不等式约束；

步骤2-5：采用最优控制方法对轨迹规划最优控制模型进行求解，得到虚拟飞机系统最优轨迹以及对应的状态变量和控制变量，其中，控制变量和状态变量表示分别为(x_1r,y_1r,θ_1r,v_1r)^T和

步骤3：得出有杆牵引飞机系统的实际轨迹及对应的控制律，具体步骤如下：

步骤3-1：有杆牵引飞机系统的运动学建模

令状态变量X＝(x₁,y₁,θ₁,β₁,β₂,α,v₁)^T；控制向量U＝(u₁,u₂)^T；

表示牵引车前轮转向角的角速度；

表示牵引系统中飞机的加速度；β₂为牵引杆与牵引车之间的夹角，α为牵引车前轮转向角，L₂为牵引杆的长度，M为牵引杆与牵引车铰接点到牵引车后轮中心位置距离，L₃为牵引车前后轮距；则有杆牵引飞机系统的运动学模型

为：

步骤3-2：确定跟踪误差约束

实际轨迹与标准轨迹之间存在偏差，结合公式(3)对偏差的范围进行约束，以确保偏差不大于dist，则：

(x₁-x_1r)²+(y₁-y_1r)²≤(dist)² (7)

步骤3-3：确定跟踪阶段的状态-控制约束

令v_1min和v_1max分别为v₁在轨迹跟踪阶段的最小值和最大值，β_1min和β_1max分别为β₁的最小值和最大值，β_2min和β_2max分别为β₂的最小值和最大值，α_min和α_max分别为α的最小值和最大值，u_1min和u_1max分别为u₁的最小值和最大值，u_2min和u_2max分别为u₂的最小值和最大值；则有杆牵引系统控制变量、v₁、β₁和β₂应满足如下状态-控制约束：

步骤3-4：建立有杆牵引系统的最优跟踪控制模型

通过使有杆牵引飞机系统跟踪虚拟飞机系统的最优轨迹，从而反推出有杆牵引飞机系统的实际轨迹和控制，故可建立如下最优跟踪控制模型：

其中，X₀和X_f为边界条件；R是正定矩阵；参考轨迹X_r＝(x_1r,y_1r,θ_1r,β_1r,0,0,v_1r)^T；β_1r可根据步骤2中最优轨迹对应控制变量得到，即

P为权重矩阵，h≤0由公式(7)和公式(8)组成的不等式约束；

步骤3-5：对最优跟踪控制模型进行求解

对于公式(9)，采用最优控制方法进行求解，能够得到最优轨迹对应的状态变量和控制变量，最终实现对有杆牵引飞机系统的轨迹规划与控制。

3.根据权利要求2所述的一种基于抽取虚拟飞机策略的有杆牵引飞机系统轨迹规划方法，其特征在于，所述步骤2-5中采用的最优控制方法中为伪谱法。

4.根据权利要求2所述的一种基于抽取虚拟飞机策略的有杆牵引飞机系统轨迹规划方法，其特征在于，所述步骤3-5中采用的最优控制方法中为保辛伪谱算法。

Images