CN108717265B - 一种基于控制变量参数化的无人飞行器巡航跟踪控制系统及控制方法 - Google Patents

Abstract

本发明公开了一种基于控制变量参数化的无人飞行器巡航跟踪控制系统及控制方法，无人飞行器在巡航空域飞行，飞行器巡航高度传感器、飞行器速度传感器、飞行器航道倾角传感器开启，获得飞行器巡航高度偏差、速度和飞行器航道倾角信息，飞行器MCU根据设定的巡航高度偏差、速度和飞行器航道倾角要求自动执行内部控制变量参数化优化算法，得到使无人飞行器在指定时间内到达设定巡航轨迹的控制策略，飞行器MCU将获得的控制策略转换为控制指令发送给飞行器推力控制模块和俯仰角控制模块执行。本发明能够根据无人飞行器不同的巡航高度偏差、速度和飞行器航道倾角状态快速地得到优化控制策略，使无人飞行器在设定时间内平稳到达指定的巡航轨迹状态。

Description

一种基于控制变量参数化的无人飞行器巡航跟踪控制系统及 控制方法

技术领域

本发明涉及无人飞行器巡航跟踪控制领域，主要是一种基于控制变量参数化算法的无人飞行器巡航跟踪控制系统及控制方法。能够根据无人飞行器的不同巡航高度偏差、速度和飞行器航道倾角信息快速获取飞行器推力和俯仰角控制策略，使飞行器在设定调节时间内平稳到达指定的飞行轨迹状态。

背景技术

随着科技的进步，无人飞行器已经逐步应用于航空侦查、航空测绘、电力巡线、森林防火等领域，无人飞行器已经成为世界航空航天领域一个极其重要的发展方向，研究和发展无人飞行器控制系统对进一步提升其飞行品质和扩展其应用领域具有非常重要的意义。

通常，无人飞行器到达巡航空域后，为了保证飞行安全，飞行器的速度、航迹倾角和飞行高度需要限制在“安全气动包络线”之内，因此，巡航跟踪控制是保证无人飞行器完成飞行任务的核心关键技术之一。然而，由于无人飞行器系统非线性、多变量、时变以及多输入输出的特性，传统的经典控制方法并不能完全满足无人飞行器日益增长的高品质控制要求，研究高品质的无人飞行器跟踪控制系统已成为当前国内外研究的热点和前沿。

发明内容

为了提高无人飞行器的巡航跟踪控制品质，使飞行器在设定调节时间内平稳到达指定的飞行轨迹状态，本发明提供了一种基于控制变量参数化方法的无人飞行器巡航跟踪控制系统。

首先，在设定调节时间内飞行的无人飞行器巡航跟踪过程动力学模型可以用如下式子描述：

x(t₀)＝x₀

其中，t表示时间，x(t)表示无人飞行器速度、飞行器航道倾角和飞行器巡航高度偏差组成的状态向量，

是x(t)的一阶导数；u(t)表示无人飞行器推力和俯仰角变量组成的控制向量，

是u(t)的一阶导数；f(u(t),x(t),t)是无人飞行器速度、飞行器航道倾角和飞行器巡航高度偏差动力学方程组成的微分方程组；t₀表示无人飞行器进行巡航跟踪控制的起始时刻，x₀为t₀时刻的状态向量。要使飞行器在设定调节时间内平稳到达指定的飞行轨迹状态，即要求飞行器调节过程状态变量偏差最小，则该无人飞行器的巡航跟踪问题可以描述为：

x(t₀)＝x₀

x(t_f)＝x_tf

g_i(u(t),x(t),t)＝0,i＝1,2,...,m_e

g_j(u(t),x(t),t)≤0,j＝m_e+1,...,m

t₀≤t≤t_f

其中t表示时间，x(t)表示无人飞行器速度、飞行器航道倾角和飞行器巡航高度偏差组成的状态向量，

是x(t)的一阶导数；u(t)表示无人飞行器推力(u₁(t))和俯仰角(u₂(t))组成的控制向量，

是u(t)的一阶导数；f(u(t),x(t),t)是无人飞行器速度、飞行器航道倾角和飞行器巡航高度偏差动力学方程组成的微分方程组；t₀表示无人飞行器进行巡航跟踪控制的起始时刻，x₀为t₀时刻的状态向量；t_f表示无人飞行器进行巡航跟踪控制的终止时刻，

为t_f时刻需要到达的巡航跟踪目标状态向量；J(u(t),x(t),t)表示无人飞行器轨迹优化的目标函数；L₀(u(t),x(t),t)表示飞行器调节过程状态变量偏差函数；g_i(u(t),x(t),t)＝0,i＝1,2,...,m_e是无人飞行器调节过程的等式约束条件函数；g_j(u(t),x(t),t)≤0,j＝m_e+1,...,m是无人飞行器调节过程的不等式约束条件；m_e表示等式约束个数，m表示约束条件总的个数，

分别表示无人飞行器推力和俯仰角调节量的下限值，

分别表示无人飞行器推力和俯仰角调节量的上限值；无人飞行器的巡航跟踪调节时间为t₀时刻到t_f时刻。分析可知，该问题本质上为最优控制问题

本发明解决其技术问题所采用的技术方案是：在无人飞行器MCU中集成了控制变量参数化优化方法，能够根据无人飞行器的不同巡航高度偏差、飞行速度和飞行器航道倾角信息给出飞行器推力和俯仰角控制策略，使飞行器在设定调节时间内平稳到达指定的飞行轨迹状态。

具体地，本发明的系统包括动力学模型、飞行器性能约束条件、优化目标设置模块，无人飞行器MCU模块，飞行器巡航高度传感器，飞行器速度传感器，飞行器航道倾角传感器，飞行器巡航高度偏差、速度、飞行器航道倾角、飞行器调节时间设置模块，飞行器推力控制模块和飞行器俯仰角控制模块；其中所述动力学模型、飞行器性能约束条件、优化目标设置模块用于设定飞行器动力学模型函数、飞行器性能约束条件函数和飞行器推力、俯仰角优化目标参数。

所述飞行器巡航高度传感器用于获取飞行器当前的巡航高度偏差，并输入无人飞行器MCU模块。

所述飞行器速度传感器用于获取飞行器当前的速度，并输入无人飞行器MCU模块。

所述飞行器航道倾角传感器用于获取飞行器当前的航道倾角，并输入无人飞行器MCU模块。

所述飞行器巡航高度偏差、速度、飞行器航道倾角、飞行器调节时间设置模块用于设定飞行器巡航高度偏差、速度、飞行器航道倾角和飞行器调节时间；

所述无人飞行器MCU模块用于根据设定的飞行器巡航高度偏差、速度、飞行器航道倾角和飞行器调节时间，以及当前的巡航高度偏差、当前的速度和当前的航道倾角，采用控制变量参数化优化算法得到使无人飞行器在设定调节时间范围内平稳到达指定飞行轨迹状态的推力和俯仰角控制量，并分别发送给飞行器推力控制模块和飞行器俯仰角控制模块。

所述飞行器推力控制模块用于控制飞行器飞行；所述飞行器俯仰角控制模块用于控制飞行器的俯仰角。

本发明还提供了一种控制方法，步骤如下：

步骤A1)：将所述MCU安装在某型无人飞行器上，通过动力学模型、飞行器性能约束条件、优化目标设置模块设定飞行器动力学模型函数、飞行器性能约束条件函数和飞行器推力、俯仰角优化目标参数；

步骤A2)：无人飞行器到达指定飞行空域后，开启飞行器巡航高度传感器、飞行器速度传感器和飞行器航道倾角传感器，分别获得飞行器当前的巡航高度偏差、当前的速度和当前的飞行器航道倾角；

步骤A3)：无人飞行器MCU根据飞行器巡航高度偏差、速度、飞行器航道倾角、飞行器调节时间设置模块中设定的巡航高度、飞行速度、飞行器航道倾角和调节时间，采用控制变量参数化优化算法，得到使无人飞行器在设定调节时间范围内平稳到达指定飞行轨迹状态的推力和俯仰角控制量；

步骤A4)：无人飞行器MCU将步骤A3)所述推力和俯仰角控制量分别输出至飞行器推力控制模块和飞行器俯仰角控制模块。

所述的无人飞行器MCU自动产生飞行器推力和俯仰角控制指令的控制变量参数化优化算法运行步骤如下：

步骤B1)：无人飞行器到达巡航空域后，飞行器巡航高度传感器、飞行器速度传感器和飞行器航道倾角传感器开启，信息采集模块获取无人飞行器当前的巡航高度偏差、飞行速度和飞行器航道倾角状态信息；

步骤B2)：初始化模块开始运行，输入信息采集模块获得的信息，设置调节时间优化过程的离散段数、飞行器推力和俯仰角变量组成的控制向量的初始猜测值u⁽⁰⁾(t)，设定优化精度要求tol，将迭代次数k置零；

步骤B3)：通过ODE求解模块获取本次迭代的状态信息x^(k)(t)和目标函数值J^(k)(x(t),u(t),t)；

步骤B4)：通过梯度求解模块24获取本次迭代目标函数梯度信息dJ^(k)(u(t),x(t),t)和约束条件梯度信息dg^(k)(u(t),x(t),t)；当k＝0时跳过步骤B5)和B6)，直接执行步骤B7)；

步骤B5)：NLP问题求解模块25运行，通过NLP收敛性判断模块进行收敛性判断，如果本次迭代得到的目标函数值J^(k)(u(t),x(t),t)与上一次迭代的目标函数值J^(k-1)(u(t),x(t),t)的绝对值之差小于精度tol，则判断收敛性满足，并将本次迭代的控制策略转换为无人飞行器的推力和俯仰角控制指令输出至控制策略输出模块26；如果本次迭代得到的目标函数值J^(k)(u(t),x(t),t)与上一次迭代的目标函数值J^(k-1)(u(t),x(t),t)的绝对值之差大于精度tol，则判断收敛性不满足，继续执行步骤B6)；

步骤B6)：用u^(k)(t),J^(k)(u(t),x(t),t),dJ^(k)(u(t),x(t),t),dg^(k)(u(t),x(t),t)的值覆盖上一次迭代u^(k-1)(t),J^(k-1)(u(t),x(t),t),dJ^(k-1)(u(t),x(t),t),dg^(k-1)(u(t),x(t),t)的值，并将迭代次数k加1；

步骤B7)：NLP问题求解模块25利用在步骤B3)和B4)中获得的目标函数值和梯度信息，求解寻优方向和寻优步长，并进行寻优修正，获得比上一次迭代推力和俯仰角控制策略组成的控制向量u^(k-1)(t)更优的新的推力和俯仰角控制策略组成的控制向量u^(k)(t)。该步骤执行完成后再次跳转至步骤B3)，直至NLP收敛性判断模块满足为止；得到的最优解作为控制策略输入控制策略输出模块26。

所述的ODE快速求解模块，采用的是四级五阶龙格库塔方法，求解公式为：

K₁＝f[u^(k)(t),x^(k)(t_i),t_i]

K₂＝f[u^(k)(t),x^(k)(t_i)+K₁h/2,t_i+h/2]

K₃＝f[u^(k)(t),x^(k)(t_i)+K₂h/2,t_i+h/2]

K₄＝f[u^(k)(t),x^(k)(t_i)+K₃h,t_i+h]

x^(k)(t_i+h)＝x^(k)(t_i)+h(K₁+2K₂+2K₃+K₄)/6

其中，t表示时间，t_i表示龙格库塔方法选择的积分时刻，h为积分步长，x^(k)(t_i)表示无人飞行器在第k次迭代中第t_i时刻的飞行状态信息(包括飞行器巡航高度偏差、飞行器速度、飞行器航道倾角)，f(·)是描述无人飞行器动力学方程组的函数，K₁、K₂、K₃、K₄分别表示龙格库塔法积分过程中的4个节点的函数值。u^(k)(t)表示推力和俯仰角控制策略组成的控制向量。

所述的梯度求解模块，采用灵敏度轨迹方程法：

步骤C1)：定义第k次迭代的灵敏度轨迹方程Γ^(k)(t)为：

Γ^(k)(t)的求解公式为：

其中，t表示时间，

表示第k次迭代中灵敏度轨迹方程对于时间t的导数，f(u^(k)(t),x^(k)(t),t)是描述无人飞行器状态微分方程的函数，Γ^(k)(t₀)表示灵敏度轨迹方程在第k次迭代时的初始时刻状态值，x₀表示无人飞行器状态微分方程函数的初始时刻状态值。

步骤C2)：采用四级五阶龙格库塔方法求解灵敏度轨迹方程Γ^(k)(t)在各积分时刻的值，求解公式为：

Q₁＝S[u^(k)(t),x^(k)(t_i),t_i]

Q₂＝S[u^(k)(t),x^(k)(t_i)+Q₁h/2,t_i+h/2]

Q₃＝S[u^(k)(t),x^(k)(t_i)+Q₂h/2,t_i+h/2]

Q₄＝S[u^(k)(t),x^(k)(t_i)+Q₃h,t_i+h]

Γ^(k)(t_i+h)＝Γ^(k)(t_i)+h(Q₁+2Q₂+2Q₃+Q₄)/6

其中，t表示时间，t_i表示龙格库塔方法选择的控制过程中某一时间点，h为积分步长，x^(k)(t_i)表示无人飞行器在第k次迭代中第t_i时刻的状态信息(包括飞行器巡航高度偏差、飞行器速度、飞行器航道倾角)，S(·)是描述灵敏度方程的函数，Q₁、Q₂、Q₃、Q₄分别表示龙格库塔法积分过程中的4个节点的函数值。

步骤C3)：根据得到的无人飞行器状态信息x^(k)(t)和灵敏度轨迹方程Γ^(k)(t)，求解目标函数的梯度信息dJ^(k)(u(t),x(t),t)：

其中，J^(k)(u(t),x(t),t)表示第k次迭代的目标函数，dJ^(k)(u(t),x(t),t)表示第k次迭代的目标函数关于控制向量u^(k)(t)的梯度信息。

步骤C4)：根据得到的无人飞行器状态信息x^(k)(t)和灵敏度轨迹方程Γ^(k)(t)，求解约束条件的梯度信息dg^(k)(u(t),x(t),t)：

dg^(k)(u(t),x(t),t)＝[dg₁ ^(k) … dg_j ^(k)],j＝1,2,...,m_e,m_e+1,...,m

其中，g_j ^(k)(u(t),x(t),t)表示第j个约束条件第k次迭代的函数，dg_j ^(k)(u(t),x(t),t)表示第j个约束条件关于控制向量u^(k)(t)的第k次迭代梯度信息，m_e表示等式约束个数，m表示约束条件总的个数。

所述的NLP求解模块，采用如下步骤实现：

步骤D1)：将飞行器推力和俯仰角变量组成的控制向量u^(k-1)(t)作为向量空间中的某个点，记作P₁，P₁对应的目标函数值就是J^(k-1)(u(t),x(t),t)；

步骤D2)：从点P₁出发，根据选用的NLP算法，构造向量空间中的一个寻优方向d^(k-1)和步长α^(k-1)；

步骤D3)：通过式u^(k)(t)＝u^(k-1)(t)+α^(k-1)d^(k-1)构造向量空间中对应u^(k)(t)的另外一个点P₂，使得P₂对应的目标函数值J^(k)(u(t),x(t),t)比J^(k-1)(u(t),x(t),t)更优。

步骤D4)：采用寻优校正u^(k)(t)，得到校正后的点

记为点P₃，同时令

使得P₃对应的目标函数值

比J^(k)(u(t)，x(t)，t)更优；

步骤D5)：如果本次迭代的目标函数值

与上一次迭代的目标函数值J^(k-1)(u(t),x(t),t)的绝对值之差小于精度tol，则判断收敛性满足，令

同时将本次迭代得到的控制策略u^(k)(t)输出至控制策略输出模块26；如果收敛性不满足，迭代次数k增加1，将u^(k)(t)设置为向量空间点P₁，继续执行步骤D2)。

本发明的有益效果主要表现在：根据无人飞行器的不同巡航高度偏差、速度和飞行器航道倾角信息快速获取飞行器推力和俯仰角控制策略，使飞行器在设定调节时间内平稳到达指定的巡航轨迹状态，提高无人飞行器的巡航跟踪控制水平。

附图说明

图1是本发明的结构示意图；

图2是本发明无人飞行器MCU内部模块结构图；

图3是实施例1的无人飞行器推力控制曲线图；

图4是实施例1的无人飞行器俯仰角控制曲线图；

图5是实施例1的无人飞行器速度曲线图；

图6是实施例1的无人飞行器航迹倾角曲线图；

图7是实施例1的无人飞行器巡航高度偏差曲线图。

具体实施方式

所述巡航跟踪控制系统如图1所示，包括动力学模型、飞行器性能约束条件、优化目标设置模块11，无人飞行器MCU模块12，飞行器巡航高度传感器13，飞行器速度传感器14，飞行器航道倾角传感器15，飞行器巡航高度偏差、速度、飞行器航道倾角、飞行器调节时间设置模块16，飞行器推力控制模块17，飞行器俯仰角控制模块18，所述系统内的各组成部分均由控制器内数据总线连接。

所述的无人飞行器MCU部分如图2所示，包括信息采集模块21、初始化模块22、常微分方程组(Ordinary Differential Equations，简称ODE)求解模块23、梯度求解模块24、非线性规划(Non-linear Programming，简称NLP)问题求解模块25、控制策略输出模块26。其中，信息采集模块包括飞行器初始巡航高度偏差和飞行速度采集、飞行器初始飞行器航道倾角采集、飞行器巡航高度偏差和速度设定采集、飞行器航道倾角设定采集、飞行器的气动系数模型函数和性能约束条件函数以及优化目标参数采集五个子模块；NLP求解模块包括寻优方向求解、寻优步长求解、寻优修正、NLP收敛性判断四个子模块。

实施例1

某型号无人飞行器，其动力学模型如下：

其中，x₁(t)表示飞行器的飞行速度，x₂(t)表示飞行器的航道倾角，x₃(t)表示飞行器的巡航高度偏差，u₁(t)表示飞行器的推力，u₂(t)表示飞行器的俯仰角，x(t)是由x₁(t)、x₂(t)和x₃(t)组成的状态向量(x(t)＝[x₁(t)x₂(t)x₃(t)]^T)，

是x(t)的一阶导数，控制向量为u(t)＝[u₁(t)u₂(t)]^T。为了保证飞行器的飞行安全，飞行器在巡航空域的性能约束限制为：

92≤x₁(t)≤170(m/s)，-20≤x₂(t)≤25(°)，-150≤x₃(t)≤150(m)

60000≤u₁(t)≤125000(N)，-20≤u₂(t)≤25(°)

无人飞行器到达指定巡航空域后，飞行器巡航高度传感器、飞行器速度传感器和飞行器航道倾角传感器和MCU均已开启。信息采集模块立即采集飞行器动力学性能约束条件函数，当前时刻的巡航高度偏差、飞行速度和飞行器航道倾角信息，设当前初始时刻t₀＝0s，巡航高度传感器传入MCU后得到偏差为x₃(t₀)＝-120(m)，速度传感器传入MCU的速度为x₁(t₀)＝150(m/s)，飞行器航道倾角传感器传入MCU的飞行器航道倾角为x₂(t₀)＝20(°)；飞行器的调节时间限制为100s，即调节终值时间为t_f＝100(s)；终止时刻无人飞行器需要满足的巡航轨迹要求为

无人飞行器的巡航跟踪要求是根据无人飞行器当前巡航高度偏差、速度和飞行器航道倾角信息快速获取飞行器推力和俯仰角控制策略，使飞行器在设定调节时间内平稳到达指定的巡航轨迹状态。因此，结合飞行器的三维空间运动方程、气动系数模型、飞行器性能约束条件函数和控制目标，整理得到该问题的数学模型如下：

92≤x₁(t)≤170(m/s)

-20≤x₂(t)≤25(°)

-150≤x₃(t)≤150(m)

60000≤u₁(t)≤125000(N)

-20≤u₂(t)≤25(°)

x₁(t₀)＝150(m/s),x₂(t₀)＝20(°),x₃(t₀)＝-120(m)

t₀＝0(s),t_f＝100(s)

其中，J(u(t),x(t),t)表示无人飞行器巡航跟踪问题的优化目标函数。可以看出问题是一个典型的最优控制问题。为了便于表述，采用F(u(t),x(t),t)表示无人飞行器动力学微分方程组数学模型，即：

采用g(u(t),x(t),t)表示无人飞行器状态变量约束条件，为：

所述无人飞行器巡航跟踪控制系统自动产生推力和俯仰角控制指令的控制变量参数化算法如图2所示，其运行步骤如下：

步骤B1)：无人飞行器到达巡航空域后，飞行器巡航高度传感器、飞行器速度传感器和飞行器航道倾角传感器开启，设当前初始时刻t₀＝0s，信息采集模块21采集无人飞行器的性能约束条件函数和控制目标函数J(u(t),x(t),t)，获取t₀时刻无人飞行器速度x₁(t₀)＝150(m/s)，飞行器航道倾角x₂(t₀)＝20(°)，巡航高度偏差x₃(t₀)＝-120(m)，设定飞行器的调节时间限制为100s，终止时刻无人飞行器巡航轨迹要求

步骤B2)：初始化模块22开始运行，输入信息采集模块21获得的信息，设置调节时间优化过程的离散段数为20、控制向量的初始猜测值为u⁽⁰⁾(t)＝0.5，设定优化精度要求tol＝10^-4，将迭代次数k置零；

步骤B3)：通过ODE快速求解模块23获取本次迭代的状态信息x^(k)(t)和目标函数值J^(k)(u(t),x(t),t)；

步骤B4)：通过梯度求解模块24获取本次迭代目标函数梯度信息dJ^(k)(u(t),x(t),t)和约束条件梯度信息dg^(k)(u(t),x(t),t)；当k＝0时跳过步骤B5)和步骤B6)，直接执行步骤B7)；

步骤B5)：NLP问题求解模块25运行，通过NLP收敛性判断模块进行收敛性判断，如果本次迭代得到的目标函数值J^(k)(u(t),x(t),t)与上一次迭代的目标函数值J^(k-1)(u(t),x(t),t)的绝对值之差小于精度tol＝10^-4，则判断收敛性满足，并将本次迭代的控制策略转换为无人飞行器的推力和俯仰角控制指令输出至控制策略输出模块26；如果本次迭代得到的目标函数值J^(k)(u(t),x(t),t)与上一次迭代的目标函数值J^(k-1)(u(t),x(t),t)的绝对值之差大于精度tol，则判断收敛性不满足，继续执行步骤B6)；

步骤B6)：用u^(k)(t),J^(k)(u(t),x(t),t),dJ^(k)(u(t),x(t),t),dg^(k)(u(t),x(t),t)的值覆盖上一次迭代u^(k-1)(t),J^(k-1)(u(t),x(t),t),dJ^(k-1)(u(t),x(t),t),dg^(k-1)(u(t),x(t),t)的值，并将迭代次数k加1；

步骤B7)：NLP问题求解模块25利用在步骤B3)和B4)中获得的目标函数值和梯度信息，求解寻优方向和寻优步长，并进行寻优修正，获得比上一次迭代推力和俯仰角控制策略组成的控制向量u^(k-1)(t)更优的新的推力和俯仰角控制策略组成的控制向量u^(k)(t)。该步骤执行完成后再次跳转至步骤B3)，直至NLP收敛性判断模块满足为止；得到的最优解作为控制策略输入控制策略输出模块26。

所述的ODE快速求解模块，采用的是四级五阶龙格库塔方法，求解公式为：

K₁＝F[u^(k)(t),x^(k)(t_i),t_i]

K₂＝F[u^(k)(t),x^(k)(t_i)+K₁h/2,t_i+h/2]

K₃＝F[u^(k)(t),x^(k)(t_i)+K₂h/2,t_i+h/2]

K₄＝F[u^(k)(t),x^(k)(t_i)+K₃h,t_i+h]

x^(k)(t_i+h)＝x^(k)(t_i)+h(K₁+2K₂+2K₃+K₄)/6

其中，t表示时间，t_i表示龙格库塔方法选择的积分时刻，h为积分步长，x^(k)(t_i)表示无人飞行器在第k次迭代中第t_i时刻的飞行状态信息(包括飞行器巡航高度偏差、飞行器速度、飞行器航道倾角)，F(·)是无人飞行器动力学微分方程组数学模型，K₁、K₂、K₃、K₄分别表示龙格库塔法积分过程中的4个节点的函数值。

所述的梯度求解模块，采用灵敏度轨迹方程法：

步骤C1)：定义第k次迭代的灵敏度轨迹方程Γ^(k)(t)为：

Γ^(k)(t)的求解公式为：

其中，t表示时间，

表示第k次迭代中灵敏度轨迹方程对于时间t的导数，F(u^(k)(t),x^(k)(t),t)是无人飞行器动力学微分方程组数学模型，Γ^(k)(t₀)表示灵敏度轨迹方程在第k次迭代时的初始时刻状态值，x₀表示无人飞行器状态微分方程函数的初始时刻状态值。

步骤C2)：采用四级五阶龙格库塔方法求解灵敏度轨迹方程Γ^(k)(t)在各积分时刻的值，求解公式为：

Q₁＝S[u^(k)(t),x^(k)(t_i),t_i]

Q₂＝S[u^(k)(t),x^(k)(t_i)+Q₁h/2,t_i+h/2]

Q₃＝S[u^(k)(t),x^(k)(t_i)+Q₂h/2,t_i+h/2]

Q₄＝S[u^(k)(t),x^(k)(t_i)+Q₃h,t_i+h]

Γ^(k)(t_i+h)＝Γ^(k)(t_i)+h(Q₁+2Q₂+2Q₃+Q₄)/6

其中，t表示时间，t_i表示龙格库塔方法选择的控制过程中某一时间点，h为积分步长，x^(k)(t_i)表示无人飞行器在第k次迭代中第t_i时刻的状态信息(包括飞行器巡航高度偏差、飞行器速度、飞行器航道倾角)，S(·)是描述灵敏度方程的函数，Q₁、Q₂、Q₃、Q₄分别表示龙格库塔法积分过程中的4个节点的函数值。

步骤C3)：根据得到的无人飞行器状态信息x^(k)(t)和灵敏度轨迹方程Γ^(k)(t)，求解目标函数的梯度信息dJ^(k)(u(t),x(t),t)：

其中，J^(k)(u(t),x(t),t)表示第k次迭代的目标函数，dJ^(k)(u(t),x(t),t)表示第k次迭代的目标函数关于控制向量u(t)的梯度信息。

步骤C4)：根据得到的无人飞行器状态信息x^(k)(t)和灵敏度轨迹方程Γ^(k)(t)，求解约束条件的梯度信息dg^(k)(u(t),x(t),t)：

dg^(k)(u(t),x(t),t)＝[dg₁ ^(k) … dg_j ^(k)],j＝1,2,...,6

其中，g_j ^(k)(u(t),x(t),t)表示第j个约束条件第k次迭代的函数，dg_j ^(k)(u(t),x(t),t)表示第j个约束条件关于控制向量u(t)的第k次迭代梯度信息。

所述的NLP求解模块，采用如下步骤实现：

步骤D1)：将飞行器推力和俯仰角变量组成的控制向量u^(k-1)(t)作为向量空间中的某个点，记作P₁，P₁对应的目标函数值就是J^(k-1)(u(t),x(t),t)；

步骤D2)：从点P₁出发，根据选用的NLP算法，构造向量空间中的一个寻优方向d^(k-1)和步长α^(k-1)；

步骤D3)：通过式u^(k)(t)＝u^(k-1)(t)+α^(k-1)d^(k-1)构造向量空间中对应u^(k)(t)的另外一个点P₂，使得P₂对应的目标函数值J^(k)(u(t),x(t),t)比J^(k-1)(u(t),x(t),t)更优。

步骤D4)：采用寻优校正u^(k)(t)，得到校正后的点

记为点P₃，同时令

使得P₃对应的目标函数值

比J^(k)(u(t),x(t),t)更优；

步骤D5)：如果本次迭代的目标函数值

与上一次迭代的目标函数值J^(k-1)(u(t),x(t),t)的绝对值之差小于精度tol＝10^-4，则判断收敛性满足，令

同时将本次迭代得到的控制策略u^(k)(t)输出至控制策略输出模块26；如果收敛性不满足，迭代次数k增加1，将u^(k)(t)设置为向量空间点P₁，继续执行步骤D2)。

最后，飞行器MCU将获得的巡航跟踪推力和俯仰角控制量作为指令输出到控制策略输出模块，转换为控制指令发送给推力和俯仰角控制模块，完成巡航跟踪的执行。

图3～图7给出了实施例1中飞行器MCU执行内部控制变量参数化优化算法得到的无人飞行器推力、俯仰角控制曲线，以及在该推力和俯仰角控制策略下的无人飞行器速度、航迹倾角、巡航高度偏差曲线。可以看出，针对实施例1，控制变量参数化优化算法得到的无人飞行器推力控制量维持在6×10⁴N附近，如图3所示；而俯仰角控制量则主要在前40秒进行调节，之后趋于平稳，如图4所示。采用图3和图4所示的无人飞行器推力和俯仰角控制策略后，飞行器的速度由150m/s调整到目标设定的140m/s，如图5所示；同时，飞行器航迹倾角由20度降低到优化设定目标的0度，如图6所示；此外，飞行器巡航高度偏差由-120米调整到0米，如图7所示。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims (5)

1.一种基于控制变量参数化的无人飞行器巡航跟踪控制系统，其特征在于：包括动力学模型、飞行器性能约束条件、优化目标设置模块(11)，无人飞行器MCU模块(12)，飞行器巡航高度传感器(13)，飞行器速度传感器(14)，飞行器航道倾角传感器(15)，飞行器巡航高度偏差、速度、飞行器航道倾角、飞行器调节时间设置模块(16)，飞行器推力控制模块(17)和飞行器俯仰角控制模块(18)；其中所述动力学模型、飞行器性能约束条件、优化目标设置模块(11)用于设定飞行器动力学模型函数、飞行器性能约束条件函数和飞行器推力、俯仰角优化目标参数；

所述飞行器巡航高度传感器(13)用于获取飞行器当前的巡航高度偏差，并输入无人飞行器MCU模块(12)；

所述飞行器速度传感器(14)用于获取飞行器当前的速度，并输入无人飞行器MCU模块(12)；

所述飞行器航道倾角传感器(15)用于获取飞行器当前的航道倾角，并输入无人飞行器MCU模块(12)；

所述飞行器巡航高度偏差、速度、飞行器航道倾角、飞行器调节时间设置模块(16)用于设定飞行器巡航高度偏差、速度、飞行器航道倾角值和飞行器调节时间；

所述无人飞行器MCU模块(12)用于根据设定的飞行器巡航高度偏差、速度、飞行器航道倾角和飞行器调节时间，以及当前的巡航高度偏差、当前的速度和的航道倾角，采用控制变量参数化优化算法得到使无人飞行器在设定调节时间范围内平稳到达指定飞行轨迹状态的推力和俯仰角控制量，并分别发送给飞行器推力控制模块(17)和飞行器俯仰角控制模块(18)；

所述飞行器推力控制模块(17)用于控制飞行器飞行推力；

所述飞行器俯仰角控制模块(18)用于控制飞行器的俯仰角；

所述无人飞行器MCU模块(12)包括信息采集模块(21)、初始化模块(22)、ODE求解模块(23)、梯度求解模块(24)、NLP问题求解模块(25)和控制策略输出模块(26)；

所述信息采集模块(21)用于采集无人飞行器当前的巡航高度偏差、飞行速度和飞行器航道倾角状态信息；

所述初始化模块(22)用于设置由调节时间优化过程的离散段数、飞行器推力和俯仰角变量组成的控制向量初始猜测值u⁽⁰⁾(t)，设置优化精度要求tol，将迭代次数k置零；

所述ODE求解模块(23)根据无人飞行器动力学方程获取本次迭代的状态信息x^(k)(t)和目标函数值J^(k)(u(t),x(t),t)；

所述梯度求解模块(24)根据状态信息x^(k)(t)和灵敏度轨迹方程Γ^(k)(t)获取本次迭代目标函数梯度信息dJ^(k)(u(t),x(t),t)和约束条件梯度信息dg^(k)(u(t),x(t),t)；

所述NLP问题求解模块(25)进行收敛性判断，如果本次迭代得到的目标函数值J^(k)(u(t),x(t),t)与上一次迭代目标函数值J^(k-1)(u(t),x(t),t)的绝对值之差小于精度tol，则判断收敛性满足，并将本次迭代的控制策略转换为无人飞行器的推力和俯仰角控制指令输出至控制策略输出模块(26)；如果本次迭代得到的目标函数值J^(k)(u(t),x(t),t)与上一次迭代的目标函数值J^(k-1)(u(t),x(t),t)的绝对值之差大于精度tol，则判断收敛性不满足。

2.一种基于控制变量参数化的无人飞行器巡航跟踪控制方法，其特征在于，包括以下步骤：

步骤A1)：通过动力学模型、飞行器性能约束条件、优化目标设置模块(11)设定飞行器动力学模型函数、飞行器性能约束条件函数和飞行器推力、俯仰角优化目标参数；

步骤A2)：无人飞行器到达指定飞行空域后，开启飞行器巡航高度传感器(13)、飞行器速度传感器(14)和飞行器航道倾角传感器(15)，分别获得飞行器当前的巡航高度偏差、当前的速度和当前的飞行器航道倾角；

步骤A3)：无人飞行器MCU(12)根据飞行器巡航高度偏差、速度、飞行器航道倾角、飞行器调节时间设置模块(16)中设定的巡航高度偏差、飞行速度、飞行器航道倾角和调节时间，采用控制变量参数化优化算法，得到使无人飞行器在设定调节时间范围内平稳到达指定飞行轨迹状态的推力和俯仰角控制量；

所述控制变量参数化优化算法包括以下步骤：

步骤B1)：信息采集模块(21)采集无人飞行器当前的巡航高度偏差、飞行速度和飞行器航道倾角状态信息；

步骤B2)：初始化模块(22)设置由调节时间优化过程的离散段数、飞行器推力和俯仰角变量组成的控制向量的初始猜测值u⁽⁰⁾(t)，设置优化精度要求tol，将迭代次数k置零；

步骤B3)：ODE求解模块(23)根据无人飞行器动力学方程获取本次迭代的状态信息x^(k)(t)和目标函数值J^(k)(u(t),x(t),t)；

步骤B4)：梯度求解模块(24)根据状态信息x^(k)(t)和灵敏度轨迹方程Γ^(k)(t)获取本次迭代目标函数梯度信息dJ^(k)(u(t),x(t),t)和约束条件梯度信息dg^(k)(u(t),x(t),t)，当k＝0时跳过步骤B5)和B6)，直接执行步骤B7)；

步骤B5)：NLP问题求解模块(25)进行收敛性判断，如果本次迭代得到的目标函数值J^(k)(u(t),x(t),t)与上一次迭代的目标函数值J^(k-1)(u(t),x(t),t)的绝对值之差小于精度tol，则判断收敛性满足，并将本次迭代的控制策略转换为无人飞行器的推力和俯仰角控制指令输出至控制策略输出模块(26)；如果本次迭代得到的目标函数值J^(k)(u(t),x(t),t)与上一次迭代的目标函数值J^(k-1)(u(t),x(t),t)的绝对值之差大于精度tol，则判断收敛性不满足，继续执行步骤B6)；

步骤B6)：用u^(k)(t),J^(k)(u(t),x(t),t),dJ^(k)(u(t),x(t),t),dg^(k)(u(t),x(t),t)的值覆盖上一次迭代u^(k-1)(t),J^(k-1)(u(t),x(t),t),dJ^(k-1)(u(t),x(t),t),dg^(k-1)(u(t),x(t),t)的值，并将迭代次数k加1；

步骤B7)：利用在步骤B3)和B4)中获得的目标函数值和梯度信息，求解寻优方向和寻优步长，并进行寻优修正，获得比上一次迭代推力和俯仰角控制策略组成的控制向量u^(k-1)(t)更优的新的推力和俯仰角控制策略组成的控制向量u^(k)(t)；该步骤执行完成后再次跳转至步骤B3)，直至NLP收敛性判断模块满足为止；得到的最优解作为控制策略输入控制策略输出模块(26)；

步骤A4)：无人飞行器MCU(12)将步骤A3)所述推力和俯仰角控制量分别输出至飞行器推力控制模块(17)和飞行器俯仰角控制模块(18)。

3.根据权利要求2所述一种基于控制变量参数化的无人飞行器巡航跟踪控制方法，其特征在于：所述步骤B3)中ODE快速求解采用的是四级五阶龙格库塔方法，求解公式为：

K₁＝f[u^(k)(t),x^(k)(t_i),t_i]

K₂＝f[u^(k)(t),x^(k)(t_i)+K₁h/2,t_i+h/2]

K₃＝f[u^(k)(t),x^(k)(t_i)+K₂h/2,t_i+h/2]

K₄＝f[u^(k)(t),x^(k)(t_i)+K₃h,t_i+h]

x^(k)(t_i+h)＝x^(k)(t_i)+h(K₁+2K₂+2K₃+K₄)/6

其中，t表示时间，t_i表示龙格库塔方法选择的积分时刻，h为积分步长，x^(k)(t_i)表示无人飞行器在第k次迭代中第t_i时刻的飞行状态信息，f(·)表示描述无人飞行器动力学方程组的函数，K₁、K₂、K₃、K₄分别表示龙格库塔法积分过程中的4个节点的函数值，u^(k)(t)表示推力和俯仰角控制策略组成的控制向量。

4.根据权利要求3所述一种基于控制变量参数化的无人飞行器巡航跟踪控制方法，其特征在于：所述梯度求解模块采用灵敏度轨迹方程法，具体为：

步骤C1)：定义第k次迭代的灵敏度轨迹方程Γ^(k)(t)为：

Γ^(k)(t)的求解公式为：

其中，t表示时间，

表示第k次迭代中灵敏度轨迹方程对于时间t的导数，f(u^(k)(t),x^(k)(t),t)是描述无人飞行器状态微分方程的函数，Γ^(k)(t₀)表示灵敏度轨迹方程在第k次迭代时的初始时刻状态值，x₀表示无人飞行器状态微分方程函数的初始时刻状态值；

步骤C2)：采用四级五阶龙格库塔方法求解灵敏度轨迹方程Γ^(k)(t)在各积分时刻的值，求解公式为：

Q₁＝S[u^(k)(t),x^(k)(t_i),t_i]

Q₂＝S[u^(k)(t),x^(k)(t_i)+Q₁h/2,t_i+h/2]

Q₃＝S[u^(k)(t),x^(k)(t_i)+Q₂h/2,t_i+h/2]

Q₄＝S[u^(k)(t),x^(k)(t_i)+Q₃h,t_i+h]

Γ^(k)(t_i+h)＝Γ^(k)(t_i)+h(Q₁+2Q₂+2Q₃+Q₄)/6

其中，t表示时间，t_i表示龙格库塔方法选择的控制过程中某一时间点，h为积分步长，x^(k)(t_i)表示无人飞行器在第k次迭代中第t_i时刻的状态信息，S(·)是描述灵敏度轨迹方程的函数，Q₁、Q₂、Q₃、Q₄分别表示龙格库塔法积分过程中的4个节点的函数值；

步骤C3)：根据得到的无人飞行器状态信息x^(k)(t)和灵敏度轨迹方程Γ^(k)(t)，求解目标函数的梯度信息dJ^(k)(u(t),x(t),t)：

其中，J^(k)(u(t),x(t),t)表示第k次迭代的目标函数，dJ^(k)(u(t),x(t),t)表示第k次迭代的目标函数关于控制向量u^(k)(t)的梯度信息；

步骤C4)：根据得到的无人飞行器状态信息x^(k)(t)和灵敏度轨迹方程Γ^(k)(t)，求解约束条件的梯度信息dg^(k)(u(t),x(t),t)：

dg^(k)(u(t),x(t),t)＝[dg₁ ^(k) … dg_j ^(k)],j＝1,2,...,m_e,m_e+1,...,m

其中，g_j ^(k)(u(t),x(t),t)表示第j个约束条件第k次迭代的函数，dg_j ^(k)(u(t),x(t),t)表示第j个约束条件关于控制向量u^(k)(t)的第k次迭代梯度信息，m_e表示等式约束个数，m表示约束条件总的个数。

5.根据权利要求2所述一种基于控制变量参数化的无人飞行器巡航跟踪控制方法，其特征在于：所述NLP问题求解模块(25)进行收敛性判断的步骤为：

步骤D1)：将飞行器推力和俯仰角变量组成的控制向量u^(k-1)(t)作为向量空间中的某个点，记作P₁，P₁对应的目标函数值就是J^(k-1)(u(t),x(t),t)；

步骤D2)：从点P₁出发，构造向量空间中的一个寻优方向d^(k-1)和步长α^(k-1)；

步骤D3)：通过式u^(k)(t)＝u^(k-1)(t)+α^(k-1)d^(k-1)构造向量空间中对应u^(k)(t)的另外一个点P₂，使得P₂对应的目标函数值J^(k)(u(t),x(t),t)比J^(k-1)(u(t),x(t),t)更优；

步骤D4)：采用寻优校正u^(k)(t)，得到校正后的点

记为点P₃，同时令

使得P₃对应的目标函数值

比J^(k)(u(t),x(t),t)更优；

步骤D5)：如果本次迭代的目标函数值

与上一次迭代的目标函数值

的绝对值之差小于精度

则判断收敛性满足，令

同时将本次迭代得到的控制向量u^(k)(t)输出至控制策略输出模块(26)；如果收敛性不满足，迭代次数k增加1，将u^(k)(t)设置为向量空间点P₁，继续执行步骤D2)。

Images