CN110412877A - 一种基于nsp算法的舰载机甲板路径规划最优控制方法 - Google Patents

Abstract

本发明公开了一种基于NSP算法的舰载机甲板路径规划最优控制方法，属于舰载机的自动化与最优控制技术领域。在对舰载机甲板路径规划及其最优控制问题进行分析的基础上，打破原有方法不能兼顾路径规划与控制问题且所得路径难以满足末端约束的不足，结合舰载机滑行的运动约束、障碍约束、控制变量和状态变量约束以及末端约束，建立舰载机甲板的路径规划最优控制模型，并基于Newton迭代法、辛算法以及伪谱法，提出NSP算法对最优控制模型进行快速求解，有效地解决了舰载机甲板的精确路径规划与控制问题，同时提高了计算效率和精度，所得到的路径可严格满足末端约束。本发明能够为舰载机甲板路径规划与控制问题提供合理的解决方案。

Description

一种基于NSP算法的舰载机甲板路径规划最优控制方法

技术领域

本发明属于舰载机甲板路径规划技术领域，特别是涉及一种基于NSP算法的舰载机甲板路径规划最优控制方法。

背景技术

舰载机在甲板上的路径规划最优控制技术主要包含对场景进行建模、路径规划以及最优控制问题，在设计路径规划最优控制方法时，需要兼顾实时性和精确度这两个方面。

目前，在舰载机甲板路径规划问题上，常用的方法主要包括：基于节点的搜索算法、势场法、基于伪谱法的最优控制方法这3类。但各个系统均存在各种不足，其中基于节点的搜索算法和势场法的主要缺陷有：

(1)在进行路径规划时，只考虑如何避障并以最短距离问题，并未考虑时间-能量最优问题；

(2)未考虑路径规划的最优控制问题；

(3)对起点和终点的姿态信息考虑不全面；

(4)一般均未考虑非完整运动约束、控制约束的条件；

而基于伪谱法的最优控制方法主要存在求解效率低、精度不高，以及在配点较多的情况下易发生“维数灾难”的缺点。

当前，工程应用上迫切需要一种能兼顾计算精度和效率的、具有良好适用性的舰载机甲板路径规划最优控制算法。

发明内容

为了解决上述技术问题，本发明提出了一种基于NSP算法的舰载机甲板路径规划最优控制方法，其能兼顾计算精度和效率、具有良好的适用性。

本发明所采用的技术方案是：根据舰载机在甲板上路径规划问题中的运动约束、障碍约束以及时间—能量最优的Bolza型目标函数，建立舰载机在甲板上的路径规划最优控制模型，并基于Newton迭代法、辛算法中的第三类生成函数以及伪谱法各自的优势建立NSP算法对模型进行求解，以快速获取最小时间—能量所对应的路径、控制律以及运动时间，有效地解决了舰载机在甲板上的精确路径规划与最优控制问题。

作为优选，将舰载机前轮的转向角变量的正切值u₁和平移加速度u₂作为控制变量，将2个位置坐标(x,y)、方向变量θ以及平移速度变量V作为状态变量，构建的舰载机在甲板上的运动约束模型为：

其中，边值条件为X(t₀)和X(t_f)。

并采用下列公式来描述障碍物约束

考虑到要尽可能的保证运动时间短、运动速度更加平稳，以及需要尽可能的使舰载机在甲板上少转弯的因素，可认为目标函数为一个时间—能量最优的Bolza型代价目标函数。将系统的控制变量约束、状态变量约束以及公式(2)统一描述为h(X(t),U(t),t)≤0，则可构建舰载机甲板的路径规划最优控制模型为：

根据公式(1～3)，并基于Newton迭代法、第三类生成函数的保辛伪谱算法的NSP算法，对终端时间t_F以及对应的最优轨迹进行求解，可得出最优轨迹对应的最优解，具体的计算步骤为：

S1：初始化公式(3)中的末端时间初始化牛顿迭代法的迭代精度ρ；采用拟线性化技术，可得到运动学约束拟线性化之后的形式为：

状态变量的个数记为ns，k＝0,1,2,…，(·)^[k]表示变量(·)在第k次迭代中所得到的计算结果，参照对运动约束的处理方式，将不等式约束h≤0进行拟线性化处理，得到线性化处理之后的形式为：

通过引入非负补偿向量α，使得：

C^[k]X^[k+1]+D^[k]U^[k+1]+V^[k]+α^[k]＝0 (6)

由于末端时间固定，则可将公式(3)中带约束的时间-能量最优控制问题转化为一个带约束且末端状态、末端时间固定的能量最优控制问题，具体可描述为：

为了表示方便，下文中省略表示迭代次数的上标符，对于上述最优控制模型，通过引入Lagrange算子λ以及参数乘子向量β，可使得原问题变为无约束问题，且参数乘子向量满足α^Tβ＝0，β≥0，则目标函数可表示为：

其中，Hamiltonian函数为:

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

Hamiltonian正则方程为：

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

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

S3：结合X^[k]、U^[k]和λ^[k]，采用基于第三类的保辛伪谱算法计算出公式(7)的解X^{[k +1]}、U^[k+1]和λ^[k+1]，具体计算步骤如下：

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

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

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

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

其中，为第j区间的LGL权重，其表达为：

由于第三类生成函数只是关于和的函数，因此可将其视为独立变量，而在其它插值点处均可取驻值得到：

且则在和处有：

公式(15～16)可统一表示为：

而又可表示为如下形式：

则可得到：

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

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

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

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

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

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

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

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

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

其中，系数矩阵K为对称的稀疏矩阵，其具体表达式为：

常数矩阵为：

其中，变量为：

其次，考虑到边界条件的影响，还需要对相关矩阵进行如下修改：

(1).将K、ξ和γ矩阵的第ns(N¹+1)+1行到ns(N¹+2)行全部替换成0，再将K第ns(N¹+1)+1行到ns(N¹+2)行且第ns(N¹+1)+1列到ns(N¹+2)列部分用单位矩阵替换，r矩阵的第ns(N¹+1)+1行到ns(N¹+2)行替换成X₀；

(2).对于终端姿态固定的问题，将K、ξ和γ矩阵的第最后ns行全部替换成0，再将K最后ns行最后ns列部分用单位矩阵替换，r矩阵的最后ns行替换成X_f；对于部分终端姿态自由的问题，如X_f(3)＝free，则将K、ξ和γ矩阵的第行全部替换成0，再将K的行列部分用单位矩阵替换，r矩阵的行替换成0。

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

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

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

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

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

S5：根据在下的能量最优控制问题对应的最优解，得出在最优轨迹末端的Hamiltonian函数H(t_f)，若满足|H(t_f)+wk|≤ρ，则S4中最优轨迹对应的状态变量、控制变量以及末端时间即为公式(3)的最优解；反之，需根据末端的横截条件H(t_f)+wk＝0，采用Newton迭代法进行迭代，以得出迭代出新的末端时间返回S1。

本发明的方法首先用连续函数对环境中的障碍物进行建模，然后根据舰载机的构造和运动特性建立了其运动学模型，考虑到要尽可能的保证运动时间短、控制更加平稳，将目标函数视为一个时间—能量最优的Bolza型代价目标函数，进而构建了舰载机在甲板上的路径规划最优控制模型。所提出的路径规划模型较其它类型的路径规划模型/方法，既可严格满足终端约束条件，也可将控制与规划问题统筹考虑，且可从理论上确保得到的结果最优，还能在确保控制平稳的前提下，得到使舰载机从起点到终点耗时最短的路径，其结果可直接应用到实际工程中。为了对模型进行求解，引入拟线性化技术并结合最优控制理论将原最优控制问题转化为Hamiltonian系统的两点边值问题，并提出了NSP算法对其进行求解，该方法首先假设终端时间固定，基于第三类生成函数、辛理论和伪谱法提出了保辛伪谱算法，以对时间确定的能量最优控制问题进行求解；然后针对终端时间可变的情况，根据终端的横截条件采用牛顿迭代法和保辛伪谱法对终端时间进行迭代，从而实现了对舰载机甲板路径规划最优控制模型进行求解。所提出的NSP算法较其它最优控制求解算法，可以以更高的精度对时间不确定的最优控制问题进行求解，稳定性更高、对初始猜测解敏感度更低、计算效率更高，收敛速度更快，不易发生“维数灾难”问题。

本发明相对于现有技术，能以更高的精度和效率对路径最优控制问题进行求解，在配点较多的情况下不会发生“维数灾难”，可以快速的实现舰载机甲板路径规划，可得出满足各种约束关系且平滑的路径，且将路径规划问题与控制问题统筹考虑，可直接得出对应的控制率，具有很强的可操作性和可行性，便于实际应用。

附图说明

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

图2：为本发明的动态障碍物环境下的最优路径图。

图3：为本发明的转角随时间变化关系图。

图4：为本发明的速度随时间变化关系图。

图5：为本发明的控制变量u₁随时间变化关系图。

图6：为本发明的控制变量u₂随时间变化关系图。

具体实施方式

首先，对障碍物环境进行建模。假设一共存在7个障碍物，障碍物7为多边形，a＝34m，b＝18m，其它障碍物均为圆形，具体参数见下表。

表1障碍物参数表

障碍物	半径(米)	横坐标位置	纵坐标位置
				1	8	30	20
2	8	65	85
				3	8	65	30
4	8	40	72
				5	8	90	50
6	8	-30+0.003t<sup>2</sup>	20+0.5t
				7	-----	80	0

其次，根据舰载机的外形特征，明确相关参数。如半径为r＝10m，安全距离为dist＝2m，初始状态为X₀＝(0 0 60° 0)^T，终点处的状态为X_f＝(100 100 0° 0)^T，L＝7m。状态变量和控制变量的约束范围为：V_min＝0m/s，V_max＝1m/s，u_1min＝-1，u_1max＝1，u_2min＝-1m/s²，u_2max＝1m/s²，wk＝0.2。

然后，对NSP算法中相关参数进行设置。如通过先将整个区间分为10段，再将每段内采用6维Legendre多项式进行插值，从而将整个时间区间分为60段，共61个离散点。收敛指标ε为10^-4，ρ为10^-3。

接下来，对初始条件进行赋值。如边值条件X^[0]为任意可行路径对应的状态变量，U^[0]和λ^[0]可取任意值(此处均取1)，起始运动时间为154s。

最后，根据图1中的计算流程，采用NSP算法进行计算。根据上述已知条件，可得到图2中所示的舰载机在甲板上的运动环境和最优路径，图3中的转角随时间变化关系，图4中的速度随时间变化关系，图5中的控制变量u₁随时间变化关系，以及图6中控制变量u₂随时间变化关系。可得到最小目标函数值为J_min＝34.2500，最优时间t_F＝151.5197s，具体的计算时间为伪谱法的7.58％。

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

Claims (4)

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

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

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

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

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

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

2.如权利要求1所述的一种基于NSP算法的舰载机甲板路径规划最优控制方法，其特征在于，步骤1中对舰载机与障碍物的避碰约束进行建模的具体方法为：舰载机的运动环境为一个二维空间；以舰载机的尺寸为基准，将环境中障碍物的尺寸向外扩展，将舰载机视为一个质点；障碍物数目有限，舰载机和障碍物的避碰约束采用下列连续函数进行描述：

其中，(x,y)和(x_oi,y_oi)分别表示舰载机和障碍物几何中心的位置坐标，a_i、b_i、p_i分别为第i个障碍物的长轴、短轴、外形参数，当p_i＝1时障碍物为菱形，当p_i＝2时障碍物为圆形，当p_i＞2时障碍物近似为矩形。

3.如权利要求1或2所述的一种基于NSP算法的舰载机甲板路径规划最优控制方法，其特征在于，步骤1中对舰载机滑行的运动学约束进行建模的具体方法为：假设舰载机在甲板上的运动只产生滚动摩擦，不发生滑动，具体描述为：

其中，θ为舰载机的方位角，V为舰载机的速度，u₁为舰载机转向角的正切值，u₂为舰载机的加速度，L为沿舰载机轴线方向上前轮与后轮之间的垂直距离。

4.如权利要求1所述的一种基于NSP算法的舰载机甲板路径规划最优控制方法，其特征在于，步骤3中基于第三类生成函数的辛算法和LGL伪谱法的保辛伪谱算法具体为：

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

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

其中，Hamiltonian函数为:

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

Hamiltonian正则方程为：

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

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

步骤3-3，结合X^[k]、U^[k]和λ^[k]，计算出公式(4)的解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＝1 2 … 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 (14)

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

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

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

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

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

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

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

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

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

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

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。