[发明专利]一种基于路径优选的飞行器绕多禁飞区轨迹优化方法

权利要求书

1.一种基于路径优选的飞行器绕多禁飞区轨迹优化方法，其特征在于：其具体步骤如下：

步骤一、建立路径优选问题模型；

首先，基于各禁飞区的相对位置设计虚拟路径点，对每个禁飞区分别建立上、下两个节点，坐标为圆心±半径×调整系数，作为路径的决策点或拐点，反映是从禁飞区上方还是下方绕过；调整系数的选择根据实际情况制定；

然后，建立有向图模型，选择能串行依次绕过的禁飞区放入一个列表中，所有可能的串行线形成一个多维列表；连接起点与每一个列表中障碍上下的节点，目标点之间的连线，相邻两障碍上下的节点之间进行交叉穿行，形成所有的有向边，删除重复边后完成有向图建模；

使用深度优先遍历方法对从起点到终点的所有路径进行遍历，得到飞行器可行的所有绕飞路径；

步骤二、路径选择与规划；

对每条路径进行路径点跟踪制导，基于飞行器的运动模型规划现有路径；飞行器的动力学模型如下：

其中，r为地球中心到飞行器重心的径向距离，θ和分别为对应的经度和纬度，V为飞行器相对于地球的速度，γ为航迹角，ψ为航向角，定义为速度向量在当地水平面的投影与正北方向的夹角，顺时针方向旋转为正，飞行器通过改变其横向加速度a来改变其运动方向，ω为地球旋转角速度；

设飞行器需要经过N个路径点，根据飞行器与第i个路径点的相对几何关系，其运动学模型如下：

其中，s_i和ψ_LOSi表示相对大圆距离和视线角，Δψ_i表示航向误差；

设N个路径点是按其相应到达时间t_f,i增加排序的，即t_f,it_f,i+1；在飞行器接近路径点时，到达时间近似为：

通过参考相关文献，利用零脱靶量转换以及施瓦兹不等式扩展引理，得到存在N个拉格朗日乘子λ_i,i＝1,2,…,N，使得横向加速度a最优，且：

其中，拉格朗日乘子向量λ＝[λ₁,λ₂,…,λ_N]^T解析求解，该制导律本质上是一种改进的自适应比例导引律，且普适性强；

使用控制指令a自动规划每个路径，并根据凸优化需求选择路径评价指标；这样通过指标排序则得到性能指标最好的一条路径，并将该路径作为后续轨迹优化的初始参考轨迹；

步骤三、建立禁飞区规避轨迹优化的凸优化问题；

将非凸非线性模型的运动模型和过程约束在参考轨迹处线性化，将问题转化为一系列凸子问题，通过内点法进行求解，在多项式时间内获得最优解，且每个子问题的解能够确保收敛到极小值；

首先，建立数学模型；在动力学处理的过程中忽略地球自转，令ψ＝π/2-ψ，将r、V、t、m分别用R₀、m₀进行归一化操作，R₀为地球半径，g₀为海平面处重力加速度，m₀为标称质量，得到归一化之后的运动方程如下所示：

其中σ为倾侧角，L和D分别为升力和阻力，T为推力，其表达式如下所示：

其中ρ(h)为大气密度，它是海拔高度h的函数；S_r为发动机参考面积，m为飞行器质量；α为攻角，M为马赫数，s为燃油当量比，C_L和C_D分别为升力系数和阻力系数，C_T为推力系数；其中，ρ、C_L、C_D、声速均采用多项式拟合；

飞行过程除了状态量约束、控制量约束、终端约束外，禁飞区不等式约束如下：

其中θ_C、为圆形禁飞区圆心坐标，d为禁飞区半径；

然后，对该轨迹规划问题进行凸化；原动力学方程写为其中u＝[α,σ,s]，在参考轨迹处泰勒展开，得到：

其中，为动力学右端对状态量的雅可比矩阵，为动力学右端对控制量的雅可比矩阵；

同样，对禁飞区约束进行泰勒展开，综合得到：

为防止线性化带来的人为不可行，在性能指标中加入邻近项；

然后，将连续的最优控制问题转化为离散的参数优化问题；取离散点数为K，间隔为Δt，离散点t_k，k＝1,2,…,K；将控制输入参数化为样本点之间的线性函数：

结合上述处理，离散的凸规划子问题表示如下：

其中J为性能指标，为临近项，k为离散点序列号，k＝1,2,…,K，K为离散点数；为飞行器的状态量，u＝[α,σ,s]为控制量；A、B、C、Z为系数矩阵；为状态量初值，u_k＝[α_k,σ_k,s_k]，为经纬度的终端约束，D为状态量和控制量的定义域；为参考轨迹，θ_C、为圆形禁飞区圆心坐标，d为禁飞区半径；

步骤四、基于优选路径设计初始猜想，采用模型补偿方法迭代求解凸优化问题；

采用“CVX”算法包求解上述凸优化问题，求解器选为SDPT3，在设计初始猜想时，绕飞策略采用优选路径得到的最优路径的策略；在之后的序列迭代过程中，将上一次优化结果作为本次线性化的参考轨迹，以此对线性化模型进行补偿，增加外环迭代收敛速度；当求解结果满足收敛条件ε取0.0001时，停止求解。