CN117762124B - 一种无杆牵引构型下舰载机甲板调运轨迹规划方法 - Google Patents

Abstract

一种无杆牵引构型下舰载机甲板调运轨迹规划方法，首先，初步构建无杆牵引系统的最优控制问题，根据甲板环境建立栅格地图，并对栅格地图中障碍栅格进行相应扩张；其次，利用改进混合A*算法生成一条粗糙调运路径，估算粗糙调运路径所需的转运时间，并在离散点上对粗糙路径重采样后进行通道构建，对避障条件进行转化。最后，根据通道约束构建新的最优控制问题，进行求解，生成牵引系统的最优控制量及相应轨迹。本发明将牵引车的控制变量映射为舰载机的状态变量，能够保证高效的为最优控制问题提供高质量同伦轨迹，降低面对无杆牵引系统调运任务构建的最优控制问题中避障约束的数量，显著提高求解效率，保证高效的生成一条严格满足舰载机终端位置要求的路径。

Description

一种无杆牵引构型下舰载机甲板调运轨迹规划方法

技术领域

本发明属于舰载武器装备保障技术领域，涉及一种无杆牵引构型下舰载机甲板调运轨迹规划方法。

背景技术

传统甲板调运轨迹依赖于人工进行规划。当使用牵引车对舰载机进行牵引时，由牵引车与舰载机构成的牵引系统本质上为一多体系统，在复杂的甲板环境下，人工方法难以可靠地高效生成调运路径。近年来，基于优化的方法由于其对各类障碍的统一描述与处理能力，在轨迹规划领域获得了广泛的关注。然而甲板通常密集分布着各类障碍(如设备、人员、舰岛等)，且障碍通常不具备规则外形。若使用传统的“1对1”避障描述列式将导致两方面难点：一方面，难以实现对不规则外形障碍的描述；另一方面，由于非线性运动方程的存在导致问题已经具有较高的非凸性，当进一步考虑了较多的障碍时，则难以保证计算收敛性与计算效率。为此，面向优化框架下无杆牵引构型下舰载机甲板调运轨迹规划问题的可靠求解，有必要发展一种高效的避障约束施加方法。

发明内容

为解决上述技术问题，本发明提出一种无杆牵引构型下舰载机甲板调运轨迹规划方法。本发明通过对牵引系统进行运动学建模并进行相关约束分析，从而在最优控制问题框架下求解调运轨迹。为实现高效调运过程中对复杂避障条件的高效处理，引入通道思想，将传统“1对1”的避障描述方法转化为“限制在通道内运动”的描述方法。具体地，首先根据所研究牵引系统的运动特性，构造一种改进的混合A*算法，并采用该算法生成一条粗糙的运动路径。基于该路径，一方面，进行通道的构造以简化避障约束的列式；另一方面，通过重采样实现决策变量的高质量初始化以提高求解速度与鲁棒性。

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

一种无杆牵引构型下舰载机甲板调运轨迹规划方法，第一，根据无杆牵引系统运动学方程、机械约束以及调运需求，初步构建最优控制问题；第二，根据甲板环境建立栅格地图，利用双特征圆对牵引系统进行表示，并对栅格地图中障碍栅格进行相应扩张；第三，根据设定的调运需求，在前述形成的栅格地图上，利用改进的混合A*算法生成一条粗糙调运路径；第四，结合牵引系统机械约束，根据时间最优原则，估算前述粗糙调运路径所需的转运时间；第五，根据运动学方程及前述估算的转运时间，在离散点上对前述粗糙路径进行重采样；第六，根据重采样信息进行通道构建，对前述构造的最优控制问题中的避障条件进行转化；第七，根据通道约束构建新的最优控制问题，并进行求解，生成牵引系统的最优控制量及相应轨迹。

步骤1：根据无杆牵引系统运动学方程、机械约束以及调运需求，初步构建最优控制问题

步骤1-1：建立无杆牵引系统运动学方程

考虑如图2中所示的无杆牵引系统。其中，M_T(x_T,y_T)和M_A(x_A,y_A)分别为牵引车和舰载机的后轴中心点，H为舰载机与牵引车的铰接点。L_T和L_A分别为牵引车和舰载机的轴距，L_H为点M_T与铰接点H间的距离。θ_T和θ_A分别为牵引车和舰载机的朝向角。φ_T和φ_A分别为牵引车和舰载机的前轮转向角，则舰载机与牵引车之间夹角为φ_A＝θ_T-θ_A。记牵引车前轮转向角对应的角速度为ω_T。记牵引车与舰载机的速度分别表示为v_T与v_A，由运动学关系，得到二者关系为根据上述变量及参数定义，牵引系统的运动学方程可以写作如下微分方程形式

在公式(1)中，x_A、y_A、θ_A、θ_T、v_T和φ_T为系统的状态变量，将其记为S，即S＝[x_A,y_A,θ_A,θ_T,v_T,φ_T]^T；a_T和ω_T为系统的控制变量，将其记为C，即C＝[a_T,ω_T]^T。方程中表示变量(·)对于时间的导数。采用动态方程f表示上述运动方程，则公式(1)可表示成如下形式

同时，考虑到实际机械约束，对系统中部分状态/控制变量施加如下箱型约束条件

其中(·)_min和(·)_max分别表示变量(·)的下限和上限。

步骤1-2：将调运需求描述为边界条件

调运位置及姿态需求对应了最优控制问题中的边界条件。记调运过程起始时间为t＝0，记调运过程结束时间为t_f(t_f为需要优化的决策变量)。调运起始端和结束端的边界条件可以分别表示为

其中，(·)₀和(·)_f分别表示变量(·)在起始时刻和终端时刻的构型约束。

步骤1-3：避障约束描述

在调运过程中舰载机需要满足避障条件，将整个甲板所表示区域记为γ，将障碍物区域记为γ_obs，则在甲板的可行区域γ_free可以表示为γ_free＝γ\γ_obs，定义g为牵引系统的从状态空间到构型空间的映射函数，则避碰约束可以描述为

步骤1-4：建立目标函数

建立目标函数J如下

其中P和Q分别为状态变量和控制变量进行惩罚的对角矩阵，w为权重系数。通过调节P、Q以及w来调节相应指标在目标函数中的权重，来保证满足实际调度中的耗时、调运速度、调运稳定性等需求。

步骤1-5：构建最优控制问题，并设定数值求解中离散方式

通过前面的描述，能够获得无杆牵引系统在生成调运轨迹规划问题描述为如下的最优控制问题

最优控制问题数值求解过程中，需要将时间域进行离散，并在节点上施加相关约束条件。不妨考虑将时间域划分为N_OCP个小的时间间隔，对应于(N_OCP+1)个离散节点。时间节点满足如下关系，0＝ρ₀＜ρ₁＜ρ₂＜...＜ρ_k＜...＜ρ_OCP＝1，其中t_k＝ρ_kt_f表示优化后第k个时间节点对应的实际时间。

步骤2：根据甲板环境建立栅格地图，利用双特征圆对牵引系统进行表示，并对栅格地图中障碍栅格进行相应扩张

步骤2-1：栅格地图的构建

为了方便对环境进行信息表示，构建甲板对应的栅格地图。设定栅格地图的分辨率为D_res，则每个栅格表示D_res×D_res区域内的信息。若栅格内包含障碍物，则将该栅格视作障碍物栅格；否则，是为自由栅格。对于一个横坐标上下限分别为X_max和X_min，纵坐标的上下限分别为Y_max和Y_min的区域，可获得一个由i_max×j_max个栅格构成的栅格地图，其中X方向栅格数目为i_max＝ceil(X_max-X_min)/D_res，Y方向栅格数目为j_max＝ceil(X_max-X_min)/D_res，ceil表示向上取整。甲板地图如图3(a)所示，转换后的栅格地图如图3(b)所示。

步骤2-2：舰载机的双特征圆表示及扩张的栅格地图的构建

首先根据图4中舰载机长方形轮廓的长度L以及宽度W设置特征圆的半径为根据特征圆半径R及栅格地图分辨率D_res，将栅格地图中障碍物向外扩张的栅格数设置为N_e＝(ceil(R)/D_res)+λ，其中λ为额外添加的一个安全余量。最终获得如图3(c)所示的扩张后的栅格地图。此时可以将系统的两个特征圆缩聚至对应特征圆的圆心，从而简化路径获取过程中对系统轮廓的表示。若按照图4所示将两个特征圆圆的圆心分别表示为M_Af(x_Af,y_Af)和M_Ar(x_Ar,y_Ar)，L_r表示M_Ar至机尾距离，可以分别得到两个圆心的坐标位置为

{M_Af(x_Af,y_Af)|x_Af＝x_A+(3L/4-L_r)cosθ_A,y_Af＝y_A+(3L/4-L_r)sinθ_A} (9)

{M_Ar(x_Ar,y_Ar)|x_Ar＝x_A+(L/4-L_r)cosθ_A,y_Af＝y_A+(L/4-L_r)sinθ_A} (10)

步骤3：根据设定的调运需求，在前述形成的栅格地图上，利用改进的混合A*算法生成一条粗糙调运路径

步骤3-1：设定混合A*算法中的避障方式

在本专利中若无特别说明，使用(·)^H表示实施混合A*算法过程中涉及的对应变量(·)。

根据步骤2-2中介绍的栅格地图获取方式，将栅格地图中的障碍物栅格记为γ_oe，根据公式(9)和(10)，只要通过混合A*算法获得的舰载机位置对应的特征圆圆心满足且就可以保证混合A*算法获得的路径满足碰撞避免需求。

为防止发生Jackknife现象，在无杆牵引系统的构型下还需要保证φ_A不能够超过其上下限，因此在混合A*算法中将或视作违约行为，同样判定为发生“碰撞”，以保证成功搜索出符合运动学约束的路径。

步骤3-2：设定混合A*算法中的节点扩展方式

混合A*算法中，设定牵引车扩展过程中的控制量为图5中的(v_Tmax,φ_Tmax)，(v_Tmax,0)，(v_Tmax,-φ_Tmax)三种向前扩展模式以及(v_Tmin,φ_Tmax)，(v_Tmin,0)，(v_Tmin,-φ_Tmax)三种向后扩展模式。若使用(·)_c和(·)_p区分混合A*算法中一次扩展结束时获得的当前节点及其父节点相关变量，则可以根据父节点位置和姿态信息以及扩展模式根据运动学公式(1)采用数值积分的方式获得当前节点位置和姿态信息

步骤3-3：设定代价函数的形式

混合A*算法的设计中，需要设定代价函数f_A来引导扩展方向，设置f_A＝g_Ac+h_Ac。g_Ac为搜索至当前节点的代价，将其设置为其中μ_v、μ_d、μ_s分别为对应于行驶距离、变向操作以及转向操作进行相应惩罚的参数。h_Ac为混合A*算法中的启发式函数，表示当前点位置至终点的预估代价，将其设置为其中L_RS表示不考虑碰撞情况下当前节点至终点的reed-shepp曲线长度，L_2DHA表示只考虑二维空间中的位置信息时利用A*算法所获得的当前节点至终点的路径长度。

步骤3-4：混合A*算法的终止条件与粗路径的获取

为保证混合A*算法的搜索效率同时保证舰载机终端位置约束精度，将混合A*算法的扩展终止条件设置为只考虑舰载机位置和朝向角信息落入考虑角度分辨率为D_θ的三维空间的目标网格。若使用N_H表示混合A*算法所获得的除起点外的节点数，通过实施混合A*算法最终可以获得N_H+1个包含无杆牵引系统的位置和姿态信息，记为若其中任意第m个节点的位置和姿态信息使用表示，则4元组δ_m可以具体表示为

步骤4：结合牵引系统机械约束，根据时间最优原则，估算前述粗糙调运路径所需的转运时间

首先为δ_H中任意第k(k＝0,1,…,N_H-1)个节点δ_k定义变量则当D_k(k＞0)与D_k-1符号相反时系统的行驶方向发生变化，此时将δ_k视为一个尖端节点。将尖端节点的数目记为N_C，使用cusp_j(0＜cusp_j＜N_H)记录第j(j＝1,2,…N_C)个尖端节点在δ_H中的位置。定义列表其元素定义为

由此，可以定义δ_H中第n(n＝1,2,...,N_C+1)个片段为δ_seg,n，其由共(seg_n-seg_n-1+1)个节点组成。同时，根据通过舰载机位置信息以及几何关系获得的牵引车位置信息定义片段长度为当对应的时将第n个片段路径的行驶方向记为Dir_n＝1表示该片段路径向前行驶，当时记Dir_n＝-1表示该片段路径向后行驶。设定当Dir_n＝1时， a＝|a_Tmin|；当Dir_n＝-1时， a＝|a_Tmax|，并引入限制变量 以及L_lim＝L_lim1+L_lim2。此时可以估计调运过程在任意第n个片段的运行时间为

基于此，可以获得对终端时间t_f的初步估计t_S为

步骤5：根据运动学方程及前述估算的转运时间，在离散点上对前述粗糙路径进行重采样

本专利中若无特别说明，使用(·)^S表示重采样过程中涉及的对应变量(·)。使用(l＝0,1,…,N_OCP)表示通过重采样获得的对应于第l个采样时刻的相关变量(·)_,l。

根据2.3描述的求解方式，需要提供(N_OCP+1)个节点的初始解，对任意第l个采样点的预估时间为t_l＝t_sρ_l，根据{T_seg,n}判断其所属的片段编号，由此可由插值计算得到 假设在相邻的两个A*节点内，舰载机与牵引车行驶距离之比等于两者速度之比，并依据此假设通过插值点牵引车位置获得相应插值点舰载位置

此外通过向前差分的方式表示并根据运动学关系以及φ_T的上下限获得

对再次进行向前差分并根据ω_T上下限获得

通过重采样可以获得N_OCP+1个带有无杆牵引系统的状态与控制信息的八元组信息将其记为

步骤6：根据重采样信息进行通道构建，对前述构造的最优控制问题中的避障条件进行转化

对任意第l个采样时刻位置(x_Af,l,y_Af,l)通过重采样预估为并根据公式(9)和(10)获得对应预估的特征圆的圆心对应于不同圆心位置构建通道的方式相似，因此只以(x_Af,l,y_Af,l)为例，讲解通道构建过程。首先将对应预估圆心点看作一个0宽0长的矩形并将信息记录为即且每次以相同步长Δ按照上、左、下、右的顺序连续向四个方向扩张，若是新扩张区域(如图6所示)不包含障碍物栅格，则视为扩张成功并更新通道信息若是新扩张区域包含障碍物栅格，则视为扩张失败不记录此次扩张信息，并不再向这个方向进行扩张。直至四个方向均扩张失败获得最终的通道信息。此外为了避免无限扩张设置了一个扩张上限L_max，当任意方向扩张距离超过L_max时，将此次扩张视为失败。分别对N_OCP+1个离散时间节点重复上述通道构建过程可以获得N_OCP+1个如下约束

步骤7：构建新的最优控制问题，并进行求解，生成牵引系统的最优控制量及相应轨迹根据步骤6获得的通道约束可以将最优控制问题重新描述为

最后根据获得的高质量初始解ξ_S与t_S，对最优控制问题进行求解，生成牵引系统的最优控制量及相应轨迹。

本发明的有益效果为：

本发明通过改进混合A*算法将牵引车的控制变量映射为舰载机的状态变量，同时只约束无杆牵引系统中舰载机终端位置，保证高效的为最优控制问题提供一个高质量同伦轨迹，并通过对同伦轨迹重采样获得一个高质量的初始解。根据重采样点通过构建安全通道的方式降低了面对无杆牵引系统调运任务最终构建的最优控制问题中避障约束的数量，使其只与采样点数目有关。结合高质量初解与新的避障约束，显著提高了构建的最优控制问题的求解效率，保证高效的生成一条严格满足舰载机终端位置要求的路径。

附图说明

图1为本发明的流程图。

图2为本发明中舰载机无杆牵引运动学模型示意图。

图3(a)为本发明中初始甲板环境示意图。图3(b)为本发明中转化后的栅格地图示意图。图3(c)为本发明中扩张后的栅格地图示意图。

图4为本发明中双圆表示无杆牵引系统示意图。

图5为本发明中混合A*算法采用的扩展模式示意图。

图6为本发明安全通道构建过程示意图。

图7为本发明实施例中舰载机调运分组示意图。

图8为本发明实施例中调运到目标点的舰载机与牵引车调运轨迹。

图9为本发明实施例中调运到目标点过程中状态与控制量变化曲线。

具体实施方式

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

考虑某舰载战斗机在某航母甲板通过某型牵引车进行调运的轨迹规划问题。舰载机尺寸、牵引车的尺寸以及运动学参数如表1所示，算法相关参数如表2所示。航母甲板的尺寸以及舰载机编队方式如图7所示。实施例展示将舰载机C3调运至弹射器J3的规划过程,对C3进行调度时舰载机C1和C2应该已经完成起飞过程。调运任务起始时刻和终端时刻的构型约束如表3所示。

表1牵引车与舰载机相关参数

表2算法参数设置

表3调运任务起始时刻和终端时刻的构型约束

	xA(m)	yA(m)	θA(deg)	θT(deg)
					初始边界条件	166	41	-81	-81
终端边界条件	214.0	43.0	2.5	2.5

一种无杆牵引构型下舰载机甲板调运轨迹规划方法，包括以下步骤：

步骤1：根据无杆牵引系统运动学方程、机械约束以及调运需求，初步构建最优控制问题步骤1-1：建立无杆牵引系统运动学方程

在无杆牵引系统中设置M_T(x_T,y_T)和M_A(x_A,y_A)分别为牵引车和舰载机的后轴中心点，H为舰载机与牵引车的铰接点。L_T＝1.8和L_A＝8分别为牵引车和舰载机的轴距,L_H＝0.5为点M_T与铰接点H间的距离。θ_T和θ_A分别为牵引车和舰载机的朝向角,，φ_T和φ_A分别为牵引车和舰载机的前轮转向角，则舰载机与牵引车之间夹角定义为φ_A＝θ_T-θ_A。记牵引车前轮转向角对应的角速度为ω_T。记牵引车与舰载机的速度分别表示为v_T与v_A，由运动学关系，得到二者关系为v_A＝v_T(cosφ_A+0.278tanφ_Tsinφ_A)。根据上述变量及实施例中采用的参数定义，牵引系统的运动学方程可以写作如下微分方程形式

在公式(18)中x_A、y_A、θ_A、θ_T、v_T、φ_T为系统中的状态变量，将其记为S，有S＝[x_A,y_A,θ_A,θ_T,v_T,φ_T]^T，a_T、ω_T为系统中的控制变量将其记为C,有C＝[a_T,ω_T]^T，方程中表示变量(·)对于时间的导数。采用动态方程f记录表示上述运动方程，则公式(18)可表示成如下形式

同时，考虑到实际机械约束，对系统中部分状态/控制变量施加如下箱型约束条件

步骤1-2：将调运需求描述为边界条件

调运位置及姿态需求对应了最优控制问题中的边界条件。记调运过程起始时间为t＝0，记调运过程结束时间为t_f(t_f为需要优化的决策变量)。调运起始端和结束端的边界条件可以分别表示为

步骤1-3：避障约束描述

在调运过程中舰载机需要满足避障条件，将整个甲板所表示区域记为γ，将障碍物区域记为γ_obs，则在甲板的可行区域γ_free可以表示为γ_free＝γ\γ_obs，定义g为牵引系统的从状态空间到构型空间的映射函数，则避碰约束可以描述为

步骤1-4：建立目标函数

建立目标函数J如下

其中P＝diag(0,0,0,0,0.1,0.1)、Q＝diag(0.5,10)分别为状态变量和控制变量进行惩罚的对角矩阵，w为权重系数设置为w＝1。

步骤1-5：构建最优控制问题，并设定数值求解中离散方式

通过前面的描述，能够获得整个无杆牵引系统在生成调运轨迹的问题模型，将其描述为一个最优控制问题可以表示为：

最优控制问题数值求解过程中，需要将时间域进行离散，并在节点上施加相关约束条件。对应实施例将时间域划分为N_OCP＝100个小的时间间隔，对应于101个离散时间节点。时间节点满足如下关系，0＝ρ₀＜ρ₁＜ρ₂＜...＜ρ_k＜...＜ρ_OCP＝1，其中t_k＝ρ_kt_f表示优化后第k个时间节点对应的实际时间。

步骤2：根据甲板环境建立栅格地图，利用双特征圆对牵引系统进行表示，并对栅格地图中障碍栅格进行相应扩张

步骤2-1：栅格地图的构建

为了方便对环境进行信息表示，将地图划分为栅格。设定栅格地图的分辨率为D_res＝0.5m，则每个栅格表示0.25m²区域的信息，对应于实际地图中若该栅格内包含障碍物信息即将其视为障碍物栅格使用1表示，若栅格中不包含障碍物信息则使用0表示该栅格。对于一个横坐标上下限分别为X_max＝341、X_min＝-8，纵坐标的上下限分别为Y_max＝87、Y_min＝-8的区域来说可一个获得一个由i_max×j_max个栅格构成的栅格地图。其中i_max＝ceil(X_max-X_min)/D_res＝698，j_max＝ceil(X_max-X_min)/D_res＝190，ceil表示向上取整。

步骤2-2：舰载机的双特征圆表示及扩张的栅格地图的构建

首先根据舰载机长方形轮廓的长度L＝17.3以及宽度W＝8.6，将特征圆的半径设置为设置安全余量λ＝1，根据特征圆半径R，将栅格地图中障碍物向外扩张的栅格数设置为N_e＝(ceil(R)/D_res)+λ＝14，获得扩张后的栅格地图。同时，将两个特征圆圆的圆心分别表示为M_Af(x_Af,y_Af)和M_Ar(x_Ar,y_Ar)，L_r＝-0.975表示M_Ar至机尾距离，可以分别得到两个圆心的坐标位置为

{M_Af(x_Af,y_Af)|x_Af＝x_A+13.95cosθ_A,y_Af＝y_A+13.95sinθ_A} (26)

{M_Ar(x_Ar,y_Ar)|x_Ar＝x_A+5.3cosθ_A,y_Af＝y_A+5.3sinθ_A} (27)

步骤3：根据设定的调运需求，在前述形成的栅格地图上，利用改进的混合A*算法生成一条粗糙调运路径

步骤3-1：混合A*算法中的避障方式

根据步骤2-2中介绍的栅格地图获取方式，将栅格地图中的障碍物栅格记为γ_oe，根据公式(26)和(27)，只要通过混合A*算法获得的舰载机位置对应的特征圆圆心满足且就可以保证混合A*算法获得的路径满足碰撞避免需求。

为防止发生Jackknife现象，在无杆牵引系统的构型下还需要保证φ_A不能够超过其上下限，因此在混合A*算法中将或视作违约行为，同样判定为发生“碰撞”。

步骤3-2：设定混合A*算法中的节点扩展方式

设定混合A*算法实施过程中分别为(v_Tmax,φ_Tmax)，(v_Tmax,0)，(v_Tmax,-φ_Tmax)三种向前搜索模式以及(v_Tmin,φ_Tmax)，(v_Tmin,0)，(v_Tmin,-φ_Tmax)三种向后搜索模式。若使用(·)_c和(·)_p区分混合A*算法中一次扩展结束时获得的当前节点及其父节点相关变量，则可以根据父节点位置和姿态信息以及扩展模式根据运动学公式(18)采用数值积分的方式获得当前节点位置和姿态信息

步骤3-3：设置代价函数形式

混合A*算法中设定代价函数f_A＝g_Ac+h_Ac中，g_Ac为搜索至当前节点的代价，将其设置为其中μ_v＝1、μ_d＝20、μ_s＝5。h_Ac为混合A*算法中的启发式函数，表示当前节点位置至终点的预估代价，将其设置为其中L_RS表示不考虑碰撞情况下当前节点至终点的Reeds-Shepp曲线长度，L_2DHA表示只考虑二维空间中的位置信息时利用A*算法所获得的当前节点至终点的路径长度。

步骤3-4：混合A*算法的终止条件与粗路径的获取

将混合A*算法的扩展终止条件设置为只考虑舰载机位置和朝向角信息落入考虑角度分辨率为D_θ＝12的三维空间的目标网格(428,86,1)。通过应用上述的混合A*算法能获得N_H+1＝52个包含路径信息的节点，记为若其中任意第m个节点的位置和姿态信息使用表示，则4元组δ_m可以具体表示为

步骤4：结合牵引系统机械约束，根据时间最优原则，估算前述粗糙调运路径所需的转运时间

首先，对δ_H中任意第k(k＝0,1,…,N_H-1)个节点δ_k定义变量则当D_k(k＞0)与D_k-1符号相反时系统的行驶方向发生变化，此时将δ_k视为一个尖端节点。尖端节点的数目为N_C＝1，使用cusp_j(0＜cusp_j＜N_H)记录第j(j＝1,2,…N_C)个尖端节点在δ_H中的位置。定义列表其元素定义为

由此，可以定义δ_H中第n(n＝1,2,...,N_C+1)个片段为δ_seg,n，其由共(seg_n-seg_n-1+1)个节点组成。同时，根据通过舰载机位置信息以及几何关系获得的牵引车位置信息定义片段长度为当对应的时记第n个片段路径的行驶方向记为Dir_n＝1，此时设定 a＝|a_Tmin|＝1，当时记Dir_n＝-1，此时设定 a＝|a_Tmax|＝1。引入限制变量以及L_lim＝L_lim1+L_lim2。此时可以估计调运过程在任意第n个片段的运行时间

基于此，可以获得对终端时间t_f的初步估计t_S为

步骤5：根据运动学方程及前述估算的转运时间，在离散点上对前述粗糙路径进行重采样

根据步骤2-3所描述的求解方式，需要为最优控制问题提供101个节点的初始解，根据{T_seg,n}判断其所属的片段编号，由此可由插值计算得到假设在相邻的两个A*节点内，舰载机与牵引车行驶距离之比等于两者速度之比，并依据此假设通过插值点的牵引车位置获得相应插值点的舰载位置

此外通过向前差分的方式表示并根据运动学关系以及φ_T的上下限获得

对再次进行向前差分并根据ω_T上下限获得

通过重采样可以获得101个带有无杆牵引系统的状态与控制信息的八元组信息将其记为

步骤6：根据重采样信息进行通道构建，对前述构造的最优控制问题中的避障条件进行转化，构建新的最优控制问题

对第l个时刻位置(x_Af,l,y_Af,l)通过重采样预估为并根据公式(26)和(27)获得对应预估的特征圆的圆心实际上对应于不同位置不同时刻的圆心构建通道的方式相似，因此只以为例，讲解通道构建过程。首先将对应预估圆心点看作一个0宽0长的矩形并将信息记录为即且每次以相同步长Δ按照上、左、下、右的顺序连续向四个方向扩张，若是新扩张区域不包含障碍物栅格，则视为扩张成功并更新通道信息若是新扩张区域包含障碍物栅格，则视为扩张失败不记录此次扩张信息，并不再向这个方向进行扩张。直至四个方向均扩张失败获得最终的通道信息。此外为了避免无限扩张设置了一个扩张上限L_max，当任意方向扩张距离超过L_max时，将此次扩张视为失败。分别对101个离散时间节点重复上述通道构建过程可以获得101个如下约束

步骤7：构建新的最优控制问题，并进行求解，生成牵引系统的最优控制量及相应轨迹根据步骤6获得的通道约束可以将最优控制问题重新描述为

利用高质量的初始解ξ_S与t_S，对此最优控制问题进行求解，生成牵引系统的最优控制量及相应轨迹。本实施例获得的调运轨迹如图8所示，状态与控制量变化曲线如图9所示。

以上所述实施例仅表达本发明的实施方式，但并不能因此而理解为对本发明专利的范围的限制，应当指出，对于本领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些均属于本发明的保护范围。

Claims (3)

1.一种无杆牵引构型下舰载机甲板调运轨迹规划方法，其特征在于，所述的规划方法步骤如下：

第一，根据无杆牵引系统运动学方程、机械约束以及调运需求，初步构建最优控制问题；第二，根据甲板环境建立栅格地图，利用双特征圆对牵引系统进行表示，并对栅格地图中障碍栅格进行相应扩张；第三，根据设定的调运需求，在栅格地图上，利用改进的混合A*算法生成一条粗糙调运路径；第四，结合牵引系统机械约束，根据时间最优原则，估算粗糙调运路径所需的转运时间；第五，根据运动学方程及前述估算的转运时间，在离散点上对前述粗糙调运路径进行重采样；第六，根据重采样信息进行通道构建，对最优控制问题中的避障条件进行转化；第七，根据通道约束构建新的最优控制问题，并进行求解，生成牵引系统的最优控制量及相应轨迹；包括以下步骤：

步骤1：根据无杆牵引系统运动学方程、机械约束以及调运需求，初步构建最优控制问题，具体如下：

步骤1-1：建立无杆牵引系统运动学方程

无杆牵引系统中，M_T(x_T,y_T)和M_A(x_A,y_A)分别为牵引车和舰载机的后轴中心点，H为舰载机与牵引车的铰接点，x_T和y_T分别牵引车的后轴中心点的横、纵坐标，x_A和y_A分别是舰载机的后轴中心点的横、纵坐标；L_T和L_A分别为牵引车和舰载机的轴距，L_H为点M_T与铰接点H间的距离；θ_T和θ_A分别为牵引车和舰载机的朝向角；φ_T和φ_A分别为牵引车和舰载机的前轮转向角，则舰载机与牵引车之间夹角为φ_A＝θ_T-θ_A；记牵引车前轮转向角对应的角速度为ω_T；记牵引车与舰载机的速度分别表示为v_T与v_A，得到二者关系为v_A＝v_T(L_T cosφ_A+L_H tanφ_Tsinφ_A)/L_T；根据上述变量及参数定义，牵引系统的运动学方程写作如下微分方程形式：

在公式(1)中，x_A、y_A、θ_A、θ_T、v_T和φ_T为系统的状态变量，将其记为S，即S＝[x_A,y_A,θ_A,θ_T,v_T,φ_T]^T；a_T和ω_T为系统的控制变量，将其记为C，即C＝[a_T,ω_T]^T；方程中表示变量(·)对于时间的导数；采用动态方程f表示上述运动学方程，则公式(1)可表示成如下形式：

同时，考虑到实际机械约束，对系统中部分状态/控制变量施加如下箱型约束条件：

其中(·)_min和(·)_max分别表示变量(·)的下限和上限；

步骤1-2：将调运需求描述为边界条件

调运位置及姿态需求对应了最优控制问题中的边界条件；记调运过程起始时间为t＝0，记调运过程结束时间为t_f；调运起始端和结束端的边界条件分别表示为：

其中，(·)₀和(·)_f分别表示变量(·)在起始时刻和结束时刻的构型约束；

步骤1-3：避障约束描述

在调运过程中舰载机需要满足避障条件，将整个甲板所表示区域记为γ，将障碍物区域记为γ_obs，则在甲板的可行区域γ_free表示为γ_free＝γ\γ_obs，定义g为牵引系统的从状态空间到构型空间的映射函数，则避碰约束描述为：

步骤1-4：建立目标函数

建立目标函数J如下

其中，P和Q分别为状态变量和控制变量进行惩罚的对角矩阵，w为权重系数；通过调节P、Q以及w来调节相应指标在目标函数中的权重，来保证满足实际调度中的耗时、调运速度、调运稳定性需求；

步骤1-5：构建最优控制问题，并设定数值求解中离散方式

获得无杆牵引系统在生成调运轨迹规划问题描述为如下的最优控制问题：

步骤2：根据甲板环境建立栅格地图，利用双特征圆对牵引系统进行表示，并对栅格地图中障碍栅格进行相应扩张，具体如下：

步骤2-1：构建栅格地图

设定栅格地图的分辨率为D_res，则每个栅格表示D_res×D_res区域内的信息；若栅格内包含障碍物，则将该栅格视作障碍物栅格；否则，是为自由栅格；对于一个横坐标上下限分别为X_max和X_min，纵坐标的上下限分别为Y_max和Y_min的区域，可获得一个由i_max×j_max个栅格构成的栅格地图，其中X方向栅格数目为i_max＝ceil(X_max-X_min)/D_res，Y方向栅格数目为j_max＝ceil(X_max-X_min)/D_res，ceil表示向上取整；

步骤2-2：舰载机的双特征圆表示及扩张的栅格地图的构建

首先根据舰载机长方形轮廓的长度L以及宽度W设置特征圆的半径为根据特征圆半径R及栅格地图分辨率D_res，将栅格地图中障碍物向外扩张的栅格数设置为N_e＝(ceil(R)/D_res)+λ，其中λ为安全余量，最终获得扩张后的栅格地图；此时将系统的两个特征圆缩聚至对应特征圆的圆心，简化路径获取过程中对系统轮廓的表示，将两个特征圆的圆心分别表示为M_Af(x_Af,y_Af)和M_Ar(x_Ar,y_Ar)，L_r表示M_Ar至机尾距离，分别得到两个圆心的坐标位置为：

{M_Af(x_Af,y_Af)|x_Af＝x_A+(3L/4-L_r)cosθ_A,y_Af＝y_A+(3L/4-L_r)sinθ_A} (9)

{M_Ar(x_Ar,y_Ar)|x_Ar＝x_A+(L/4-L_r)cosθ_A,y_Af＝y_A+(L/4-L_r)sinθ_A} (10)

步骤3：根据设定的调运需求，在栅格地图上，利用改进的混合A*算法生成一条粗糙调运路径，具体如下：

步骤3-1：设定混合A*算法中的避障方式

根据步骤2-2中介绍的栅格地图获取方式，将栅格地图中的障碍物栅格记为γ_oe，根据公式(9)和(10)，只要通过混合A*算法获得的舰载机位置对应的特征圆圆心满足且就能够保证混合A*算法获得的路径满足碰撞避免需求，和分别是舰载机位置的横、纵坐标；

步骤3-2：设定混合A*算法中的节点扩展方式

混合A*算法中，设定牵引车扩展过程中的控制量为(v_Tmax,φ_Tmax)，(v_Tmax,0)，(v_Tmax,-φ_Tmax)三种向前扩展模式以及(v_Tmin,φ_Tmax)，(v_Tmin,0)，(v_Tmin,-φ_Tmax)三种向后扩展模式；若使用(·)_c和(·)_p区分混合A*算法中一次扩展结束时获得的当前节点及其父节点相关变量，则根据父节点位置和姿态信息以及扩展模式根据运动学公式(1)采用数值积分的方式获得当前节点位置和姿态信息

步骤3-3：设定代价函数的形式

混合A*算法的设计中，需要设定代价函数f_A来引导扩展方向，设置f_A＝g_Ac+h_Ac；g_Ac为搜索至当前节点的代价，将其设置为其中μ_v、μ_d、μ_s分别为对应于行驶距离、变向操作以及转向操作进行相应惩罚的参数；h_Ac为混合A*算法中的启发式函数，表示当前点位置至终点的预估代价，将其设置为其中L_RS表示不考虑碰撞情况下当前节点至终点的reed-shepp曲线长度，L_2DHA表示只考虑二维空间中的位置信息时利用A*算法所获得的当前节点至终点的路径长度；

步骤3-4：混合A*算法的终止条件与粗路径的获取

为保证混合A*算法的搜索效率同时保证舰载机终端位置约束精度，将混合A*算法的扩展终止条件设置为只考虑舰载机位置和朝向角信息落入考虑角度分辨率为D_θ的三维空间的目标网格；若使用N_H表示混合A*算法所获得的除起点外的节点数，通过实施混合A*算法最终获得N_H+1个包含无杆牵引系统的位置和姿态信息，记为若其中任意第m个节点的位置和姿态信息使用表示，其中m＝0,1,…N_H，则4元组δ_m具体表示为

步骤4：结合牵引系统机械约束，根据时间最优原则，估算粗糙调运路径所需的转运时间，具体如下：

首先为δ_H中任意第k个节点δ_k定义变量则当D_k与D_k-1符号相反时系统的行驶方向发生变化，此时将δ_k视为一个尖端节点，其中，k＝0,1,…,N_H-1，k＞0；将尖端节点的数目记为N_C，使用cusp_j记录第j个尖端节点在δ_H中的位置，其中，0＜cusp_j＜N_H，j＝1,2,...N_C；定义列表其元素定义为：

由此，定义δ_H中第n个片段为δ_seg,n，其由共(seg_n-seg_n-1+1)个节点组成，其中n＝1,2,...,N_C+1；同时，根据通过舰载机位置信息以及几何关系获得的牵引车位置信息定义片段长度为当对应的时将第n个片段路径的行驶方向记为Dir_n＝1表示该片段路径向前行驶，当时记Dir_n＝-1表示该片段路径向后行驶；设定当Dir_n＝1时， a＝|a_Tmin|；当Dir_n＝-1时， a＝|a_Tmax|，并引入限制变量 以及L_lim＝L_lim1+L_lim2；此时估计调运过程在任意第n个片段的运行时间为：

基于此，获得对终端时间t_f的初步估计t_S为：

步骤5：根据运动学方程及估算的转运时间，在离散点上对粗糙路径进行重采样，具体如下：

根据步骤4描述的求解方式，需要提供(N_OCP+1)个节点的初始解，对任意第l个采样点的预估时间为t_l＝t_sρ_l，根据{T_seg,n}判断其所属的片段编号，由此可由插值计算得到 假设在相邻的两个A*节点内，舰载机与牵引车行驶距离之比等于两者速度之比，并依据此假设通过插值点牵引车位置获得相应插值点舰载位置其中，和分别表示插值点牵引车位置的横、纵坐标，和分别表示插值点舰载位置的横、纵坐标；

此外通过向前差分的方式表示并根据运动学关系以及φ_T的上下限获得

对再次进行向前差分并根据ω_T上下限获得

通过重采样获得N_OCP+1个带有无杆牵引系统的状态与控制信息的八元组信息将其记为

步骤6：根据重采样信息进行通道构建，对构造的最优控制问题中的避障条件进行转化，具体如下：

对任意第l个采样时刻位置(x_Af,l,y_Af,l)通过重采样预估为并根据公式(9)和(10)获得对应预估的特征圆的圆心且不同圆心位置构建通道的方式相似；

分别对N_OCP+1个离散时间节点重复通道构建过程获得N_OCP+1个如下约束：

所述的通道构建过程以(x_Af,l,y_Af,l)进行说明：首先将对应预估圆心点看作一个0宽0长的矩形并将信息记录为即且每次以相同步长Δ按照上、左、下、右的顺序连续向四个方向扩张，若是新扩张区域不包含障碍物栅格，则视为扩张成功并更新通道信息若是新扩张区域包含障碍物栅格，则视为扩张失败不记录此次扩张信息，并不再向这个方向进行扩张；直至四个方向均扩张失败获得最终的通道信息；此外为了避免无限扩张设置了一个扩张上限L_max，当任意方向扩张距离超过L_max时，将此次扩张视为失败；

步骤7：构建新的最优控制问题，并进行求解，生成牵引系统的最优控制量及相应轨迹，具体如下：

根据步骤6获得的通道约束将最优控制问题重新描述为：

最后根据获得的高质量初始解ξ_S与t_S，对最优控制问题进行求解，生成牵引系统的最优控制量及相应轨迹。

2.根据权利要求1所述的一种无杆牵引构型下舰载机甲板调运轨迹规划方法，其特征在于，所述步骤1-5，最优控制问题数值求解过程中，需要将时间域进行离散，并在节点上施加相关约束条件；将时间域划分为N_OCP个小的时间间隔，对应于(N_OCP+1)个离散节点；时间节点满足如下关系，0＝ρ₀＜ρ₁＜ρ₂＜...＜ρ_k＜...＜ρ_OCP＝1，其中t_k＝ρ_kt_f表示优化后第k个时间节点对应的实际时间。

3.根据权利要求1所述的一种无杆牵引构型下舰载机甲板调运轨迹规划方法，其特征在于，所述步骤3-1中，为防止发生Jackknife现象，在无杆牵引系统的构型下还需要保证φ_A不能够超过其上下限，因此在混合A*算法中将或视作违约行为，同样判定为发生“碰撞”，以保证成功搜索出符合运动学约束的路径。