一种基于优先级的非完整自主体甲板协同调运路径规划方法

技术领域

本发明属于非完整自主体甲板协同调运路径规划技术领域，涉及一种基于优先级的非完整自主体甲板协同调运路径规划方法。

背景技术

航母自从问世以来，就成为一个国家军事力量和水平的重要象征。其作战能力一方面取决于自身的性能，另一方面取决于其出动回收能力。作为贯穿出动回收过程的重要环节，调运作业旨在使被调运对象高效、安全地到达起飞站位、保障点、停机位等位置以完成飞行或保障任务，其所针对的对象主要包括舰载特种车辆、自主体、无杆牵引系统、有杆牵引系统这4大类。虽然这4类运动体结构各异，但均是受非完整约束的系统，因此均可被视为非完整自主体。而如何在狭小的甲板空间及复杂作业环境下实现非完整多自主体之间的有效协同调运，以确保各阶段作业协调、安全、高效，是提高航母出动回收能力的关键问题。

相较于单个自主体的轨迹规划问题，多自主体的协同轨迹规划问题复杂度更高，尤其是需要考虑其非线性动力学/运动学、控制以及状态约束时，其规划就会变得更加困难，而狭小、复杂的甲板环境会使求解难度进一步增加，有时甚至难以求解。当参与规划的自主体均为同构、数量较少、求解的质量要求较低、允许长时间的离线计算、甲板环境复杂度较低、只存在静态障碍物、末端时间固定、忽略控制约束、非线性约束可线性化时，多自主体的协同调运轨迹规划将容易被实现。但是在实际应用中，这些前提条件难以同时满足，且往往需要以较高的求解性能(包括高计算效率、高求解精度)对多自主体的协同轨迹规划问题进行求解。而当存在多异构自主体协同作业时，由于不同结构的运动学/动力学约束不相同，将导致其协同轨迹规划问题比多同构体协同轨迹规划问题更加复杂。

目前，关于多自主体在甲板上协同调运轨迹规划的研究还较少，现有的研究主要是采用粒子群等仿生算法来实现多架自主体协同滑行的路径规划，这些研究成果虽然可以为本项目的研究提供参考，但其结果却难以严格满足运动学/动力学、终端、控制等约束。而无人机、无人车、机器人等领域的协同轨迹规划问题已被广泛研究，因此可对其进行借鉴。这些领域中，目前最常用的方法主要有：人工势场算法、数学规划方法、人工智能算法、交互避碰策略、优先级策略等。这些方法可解决多自主体轨迹规划问题，但却难以兼顾计算效率与精度。

多自主体的协同轨迹规划方法可分为集中式和分布式。集中式的解决方案是从全局角度来进行轨迹规划，其规划结果为一个全局的解。与此相反，分布式方法是基于多个主体来实现多自主体的协同轨迹规划。

在集中式方法中，顶层决策单元可接收所有自主体的状态，并将所有自主体视为一个整体的系统，由顶层决策单元进行整体规划并为单个自主体提供控制输入，通常采用混合整数规划、顺序凸规划、伪谱法等。

在分布式方法中，每个自主体都是通过自己来获得其他自主体的状态信息从而进行自主决策的。因此，每个自主体都能自主决策并为自己提供控制输入，而不用考虑其他自主体的动力学和控制输入。然而，在复杂动态环境下的多自主体分布式局部轨迹规划问题仍面临诸多挑战：首先，通过假设静止状态来获得的多自主体轨迹不能确保在实际动态环境中的安全性；其次，由于每个自主体均单独使用一个轨迹规划器，一个自主体的不合理运动可能会导致碰撞从而使整个系统陷入混乱；最后，由于缺少工作空间和其他自主体的先验知识，末端时间和总距离等性能指标均不容易实现最优化。

而非完整自主体在甲板上的调运往往要求自主体以预定角度准确的到达目标位置，即其运动轨迹均必须严格满足末端约束，而现有的集中式/分布式方法大多都不能以较高的计算效率和精度实现这一目标。因此，需针对舰载自主体协同调运轨迹规划的特点，设计高效、鲁棒的求解方法来实现求解。

综上所述，目前，工程应用上迫切需要一种能兼顾计算精度和效率的、具有良好适用性的非完整自主体协同调运路径规划算法。

发明内容

为了解决上述技术问题，本发明提出一种基于优先级的非完整自主体甲板协同调运路径规划方法，其能兼顾计算精度和效率、具有良好的适用性。

本发明所采用的技术方案是：

一种基于优先级的非完整自主体甲板协同调运路径规划方法，该方法首先，将所有参与调运任务的自主体开始调运时间均预设为0s；然后，对于某一自主体，根据由其位置、角度、速度所构成的起始状态、末端状态，同时将其余参与调运任务的自主体均视为一直停在起始位置上的静态障碍物，采用最优控制方法离线计算出该自主体调运的离线路径，并采用该方法依次计算出各个自主体的离线路径；最后，采用穷举法列出所有的优先级排序方案，针对每一种排序方案，分别基于各个自主体的离线路径以及优先级方法优化出各自主体的实际调运时间、所有自主体完成调运作业所需耗费的总时间，从所有排序方案中选择耗费总时间最短的作为最优方案，各自主体以最优方案中优化出的实际滑出时间作为出发时间，并沿着各自的离线路径进行调运。包括如下步骤：

步骤1，确定需调运的非完整自主体数量N，将所有自主体的开始滑出时间均预设为0s；

步骤2，基于各自主体的起始状态、末端状态，采用最优控制方法分别计算出各自主体的离线路径，得出由各自主体横坐标、纵坐标、角度及时间所构成的4维路径数据。其中，在采用最优控制方法对某个自主体离线路径进行计算时，其余自主体均被视为一直停在起始位置上的静态障碍物。采用最优控制方法计算离线路径的具体步骤为：

步骤2-1：建立非完整自主体调运的运动学方程

建立描述非完整自主体调运的状态空间x，并确定控制变量u，从而建立如下的非完整自主体调运运动学方程：

其中，t为时间变量；

步骤2-2：确定非完整自主体调运过程中的状态和控制约束

将非完整自主体调运过程中对状态变量和控制变量施加的约束表示为如下不等式形式：

C(x,u,t)≤0 (2)

步骤2-3：建立避障约束模型

非完整自主体和障碍物的避碰约束均采用下列连续函数进行描述：

其中，(x

当p

H(x,t)≤0 (4)

步骤2-4：确定单个非完整自主体调运轨迹规划的边界条件

定义非完整自主体开始调运时刻和终端时刻分别为t

x(t

步骤2-5：建立单个非完整自主体调运的最优控制模型

根据公式(1)、(2)、(3)及公式(4)，建立如下的时间-能量最优控制问题：

其中，w为终端时间权重因子；

步骤2-6：最优控制模型的求解

根据公式(6)，采用最优控制算法对其进行求解，优化出最小末端时间及对应的4维路径数据。

步骤3，采用穷举法列出所有的优先级排序方案，针对每一种排序方案，分别基于优先级方法优化出各自主体的实际开始调运时间、所有自主体完成调运作业所需耗费的总时间，最后从所有排序方案中选择耗费总时间最短的方案作为最优方案，各自主体以最优方案中优化出的实际开始调运时间作为出发时间，并沿着各自的离线路径进行调运。

基于优先级方法优化各自主体实际开始调运时间、所有自主体完成调运作业所需耗费总时间的具体步骤为：

步骤3-1：生成优先级排序方案。将N个待调运自主体分别编号为1、2、3、…、N，对1、2、3、…、N这N个数进行全排列，生成M(M≥1,M∈N

步骤3-2：令i＝1，确定排序方案P

步骤3-3：计算排序方案P

步骤3-3-1：自主体P

步骤3-3-2：将自主体P

步骤3-3-3：令k＝1，并将自主体P

步骤3-3-4：将自主体P

步骤3-3-5：自主体P

步骤3-3-6：将

步骤3-3-7：若k＜j，k＝k+1，转入步骤3-3-3；否则，转入步骤3-3-8。

步骤3-3-8：确定自主体P

步骤3-3-9：若j＜N，令j＝j+1，转入步骤3-3-2；否则，转入步骤3-4。

步骤3-4：对于排序方案P

步骤3-5：若i＜M，i＝i+1，转入步骤3-2；否则，取所有排序方案完成调运任务所需耗费总时间的最小值所对应排序方案作为最优方案，并按照该最优方案中各自主体对应的实际开始调运时间出发，并沿着步骤2中得到的调运轨迹进行调运。

进一步的，所述的最优控制算法包括伪谱法、保辛伪谱算法等。

本发明相对于现有技术，其有益效果为：

本发明能以更高的精度和效率对多个非完整自主体的协同路径规划问题进行求解，可以快速的实现多同构/异构自主体在甲板上的协同路径规划，得出满足各种约束关系且平滑的路径，具有很强的可操作性和可行性，便于实际应用。

附图说明

图1为本发明的计算流程图。

图2为本发明的飞机A1、A2、A3调运轨迹图。

图3为本发明的飞机A1的位置和角度随时间的变化图；图3(a)为飞机A1后轮中心位置横坐标随时间的变化图，图3(b)为飞机A1后轮中心位置纵坐标随时间的变化图，图3(c)为飞机A1纵轴与横坐标夹角随时间的变化图。

图4为本发明的飞机A2的位置和角度随时间的变化图；图4(a)为飞机A2后轮中心位置横坐标随时间的变化图，图4(b)为飞机A2后轮中心位置纵坐标随时间的变化图，图4(c)为飞机A2纵轴与横坐标夹角随时间的变化图。

图5为本发明的飞机A3的位置和角度随时间的变化图；图5(a)为飞机A3后轮中心位置横坐标随时间的变化图，图5(b)为飞机A3后轮中心位置纵坐标随时间的变化图，图5(c)为飞机A3纵轴与横坐标夹角随时间的变化图。

具体实施方式

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

在某一次任务中，需要优先出动2架飞机(A1和A2)，这两架飞机均已保障完毕且可以直接滑至对应位置后待命起飞，然后一架飞机(A3)采用有杆牵引的方式从原停机位调运至某保障点附近的停机位进行保障。其中，飞机A1从(44m，138m)以初始角度90°、初始速度0m/s滑行至(192m，185m)、末端角度0°、末端速度0m/s；飞机A2从(44m，188m)以初始角度-90°、初始速度0m/s滑行至(192m，152m)、末端角度0°、末端速度0m/s；飞机A3从(155m，138m)以初始角度90°、初始速度0m/s被牵引至(44m，138m)、末端角度90°、末端速度0m/s。在该环境中，存在一个长34m、宽20m的静态矩形障碍物，其几何中心位于(102m，138m)处。该实施例中基于优先级的非完整自主体甲板协同调运路径规划方法包括以下步骤：

根据步骤1，确定需调运的非完整自主体数量N＝3，将所有自主体的开始滑出时间均预设为0s。

根据步骤2，基于3个自主体的起始状态、末端状态，采用最优控制方法中的伪谱法分别计算出各自主体的离线路径，得出3个自主体横坐标、纵坐标、角度及时间所构成的4维路径数据。在采用伪谱法对某个自主体离线路径进行计算时，其余自主体均被视为一直停在起始位置上的静态障碍物，采用伪谱法计算离线路径的具体步骤为：

根据步骤2-1，对于飞机A1或A2建立如下的非完整自主体调运运动学方程：

其中，t为时间变量，上标(*)

对于A3，建立飞机A3对应牵引系统的调运运动学方程为：

其中，状态变量为

根据步骤2-2，飞机A1和A2滑行过程中的状态-控制约束为：

飞机A3对应牵引飞机系统的状态-控制约束为：

根据步骤2-3，非完整自主体和障碍物的避碰约束，采用下列连续函数进行描述：

其中，(x

则对于飞机A1，其障碍物包括静态矩形障碍物、位于起始位置或终端位置处的飞机A2、在起始位置处A3对应的牵引系统，则其与所有障碍物的避碰约束表示为如下形式：

对于飞机A2，其障碍物包括静态矩形障碍物、位于起始位置或终端位置处的飞机A1、在起始位置处A3对应的牵引飞机系统，则其与所有障碍物的避碰约束表示为如下形式：

对于飞机A3对应的牵引系统，其障碍物包括静态矩形障碍物、位于起始位置或终端位置处的飞机A2、可能在终端位置处的A1，则其与所有障碍物的避碰约束可表示为如下形式：

根据步骤2-4，记非完整自主体开始调运时刻和终端时刻分别为t

则飞机A1的边界条件为：

x

飞机A2的边界条件为：

x

飞机A3对应牵引系统的边界条件为：

x

根据步骤2-5，可建立飞机A1的调运最优控制模型为：

建立飞机A2的调运最优控制模型为：

建立飞机A3对应牵引系统的调运最优控制模型为：

根据步骤2-6，采用最优控制算法中常用的伪谱法分别对飞机A1、飞机A2、飞机A3对应牵引系统的调运最优控制模型进行求解，可得到飞机A1、飞机A2、飞机A3调运的最小末端时间分别为169.71s、164.96s、165.36s，对应的4维路径数据分别为附图2、附图3、附图4、附图5所示。

根据步骤3，采用穷举法列出所有的优先级排序方案，针对每一种排序方案，分别基于优先级方法优化出各自主体的实际开始调运时间、所有自主体完成调运作业所需耗费的总时间，最后从所有排序方案中选择耗费总时间最短的方案作为最优方案，各自主体以最优方案中优化出的实际开始调运时间作为出发时间，并沿着各自的离线路径进行调运。

则根据步骤3-1，将3个待调运自主体分别编号为1、2、3，对1、2、3这3个数进行排列，生成2种排列方案，其可构成排序方案集合P＝{P

根据步骤3-2，首先需确定排序方案P

根据步骤3-3，计算排序方案P

根据步骤3-3-1，自主体P

根据步骤3-3-2，将自主体P

根据步骤3-3-3，将自主体P

根据步骤3-3-4，将自主体P

根据步骤3-3-5，自主体P

根据步骤3-3-6，将

根据步骤3-3-7，转入步骤3-3-8。

根据步骤3-3-8，可确定自主体P

根据步骤3-3-9：令j＝3，转入步骤3-3-2，可得到

根据步骤3-4，对于排序方案P

根据步骤3-5，由于i＜2，i＝i+1，转入步骤3-2，可得到方案P

故按照该最优方案，自主体A1在13.82s时、自主体A2在0.00s时、自主体A3在129.29s时分别沿着步骤2中得到的调运轨迹进行调运，自主体A1完成调运时间为183.53s、A2为186.24s、A3为294.65s。

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