[发明专利]一种基于NSP算法的舰载机甲板路径规划最优控制方法

权利要求书

1.一种基于NSP算法的舰载机甲板路径规划最优控制方法，其特征在于，包括如下步骤：

步骤1，根据路径规划任务确定由舰载机起始位置和角度以及速度所构成的起始状态X₀、由舰载机末端位置和角度以及速度所构成的末端状态X_f、开始滑行的时间t₀，对舰载机与障碍物的避碰约束和舰载机滑行的运动学约束进行建模，从而建立舰载机在甲板上路径规划的最优控制模型，具体如下：

其中，t_f为末端时间，wk为滑行时间的权重调节因子，J为目标函数，R为控制变量权重矩阵，U为控制变量，X为状态变量，为运动学约束，X₀为初始状态，X_f为末端状态，X(t₀)表示初始时刻的状态变量，X(t_f)表示末端时刻的状态变量，h≤0为舰载机的速度约束、加速度约束、转向角约束以及障碍物的避碰约束所构成的不等式约束；

步骤2，初始化末端时间初始化牛顿迭代法的迭代精度；

步骤3，采用一种基于第三类生成函数的辛算法和LGL伪谱法的保辛伪谱算法进行求解，得出在下的最优轨迹，进一步得到在最优轨迹末端的Hamiltonian函数H(t_f)；其中，采用一种基于第三类生成函数的辛算法和LGL伪谱法的保辛伪谱算法进行求解的步骤如下：

步骤3-1：采用拟线性化方法，将舰载机的运动学约束和不等式约束进行拟线性化，并引入非负补偿向量α，可将步骤2中的能量最优控制问题描述为：

其中，状态变量的个数记为ns，k＝0,1,2,...，(·)^[k]表示变量(·)在第k次迭代中所得到的计算结果。为了表示方便，下文中省略表示迭代次数的上标符，对于上述最优控制模型，通过引入Lagrange算子λ以及参数乘子向量β，可使得原问题变为无约束问题，且参数乘子向量满足α^Tβ＝0，β≥0，则目标函数可表示为：

其中，λ为Lagrange算子，Hamiltonian函数为:

若使目标函数J最小，需要使系统同时满足控制方程和Hamiltonian正则方程，则控制方程为：

Hamiltonian正则方程为：

由于哈密顿函数是关于状态变量、控制变量、拉格朗日乘子向量、参数乘子向量和补偿向量的函数，而由控制方程可将控制变量用拉格朗日乘子向量和参数乘子向量来进行表示，则哈密顿函数为状态变量、拉格朗日乘子向量、参数乘子向量和补偿向量这4个自变量的函数；

步骤3-2：初始化迭代参数k＝0，设置控制变量、控制变量和协变量的初始猜测解分别为X^[0]、U^[0]和λ^[0]；

步骤3-3：结合X^[k]、U^[k]和λ^[k]，计算出公式(2 )的解X^[k+1]、U^[k+1]和λ^[k+1]，具体计算步骤如下：

步骤3-3-1：将时间区间T＝[t₀ t_f]离散化处理成P个区间，第j个区间为T^j＝[t_j-1,t_j]，j＝12…P，并将第j个区间采用N^j维的Legendre多项式L^j(τ)进行插值，即LGL配点个数为N^j，则X、λ、β和α可表示为：

且当j＝2,3,…,P时，有

步骤3-3-2：通过对状态表达式进行微分，可得到：

其中，为j区间的伪谱微分矩阵，则由第三类生成函数可得：/

其中，为第j区间的LGL权重；由于第三类生成函数只是关于/和/的函数，因此可将其视为独立变量，而在其它插值点处均可取驻值，则可得到：

且

而根据又可得到：

上式中，变量为σ^j和β^j可表示为：

将σ^j的系数矩阵记为K^j，则有：

K^jσ^j+ξ^jβ^j+γ^j＝r^j (12)

由控制方程可得到U＝g(X,λ,β)，则可将边界约束等式进行进一步整理为：

C^jX^j-H^jλ^j-M^jβ^j+V^j+α^j＝0 (13)

其中，j＝1 2 … P，X^j，λ^j，β^j对应的系数以及分别为：/

进一步整理可得到单个区间内的形式为：

其中，Γ^j＝[-H^j,C^j]；

步骤3-3-3：将单个区间得到的结果进行组装，以得到整个区间的求解形式为：

其中，系数矩阵K为对称的稀疏矩阵，γ、r、α为常数矩阵，且

进一步可得状态变量和协变量为：

σ＝-K^-1ξβ-K^-1(γ-r) (17)

从而可得到最优控制下的解，并将其代入到边界约束等式中，可得到如下关系：

Y＝ΓK^-1ξ+M，q＝ΓK^-1(γ-r)-V，由于该方程变量β和方程右端α的值均为未知，且二者满足正交关系，可以视其为一个标准的线性互补问题，采用Lemke方法对其进行求解即可得到β和α的值，进一步得到在该次迭代中的解X^[k+1]、U^[k+1]和λ^[k+1]；

步骤3-4：判断是否满足收敛条件|(X^[k+1]-X^[k])/X^[k+1]|≤ε，其中ε为计算精度，若满足该收敛条件，则停止迭代，并将本次迭代所得到的解作为步骤2中能量最优控制问题的最优解；反之，令k＝k+1，返回步骤3-3；

步骤4：初始化迭代收敛精度ρ，若满足|H(t_f)+wk|≤ρ，则步骤3中最优轨迹对应的状态变量、控制变量以及末端时间即为公式(1)的最优解；反之，需根据末端的横截条件H(t_f)+wk＝0，采用Newton迭代法进行迭代，以得出迭代出新的末端时间返回步骤2。