CN108107899A - 线性伪谱外太空飞行器能量最优大角度姿态机动控制方法 - Google Patents

Abstract

本发明公开提供了一种线性伪谱外太空飞行器能量最优大角度姿态机动控制方法，包括如下步骤：机动任务初始化、获得姿态机动能量最优基准控制轨迹、修正方法参数设置、按基准控制进行机动、控制更新检查、控制指令修正。该控制方法通过修正基准姿态机动控制量实现满足姿态机动终端约束的能量最优姿态机动；本发明的关键技术是基于线性伪谱模型预测控制思想推导的姿态机动终端状态偏差与控制修正量之间的解析公式；该方法具有计算效率高、求解精度高的特点；线性伪谱控制能够达到与LQR控制近似的终端精度，实现飞行器有限时间内的大角度姿态机动，并且相比于LQR控制消耗更少的能量，同时其控制指令更加平滑，满足姿态机动控制在线使用的实时性。

Description

线性伪谱外太空飞行器能量最优大角度姿态机动控制方法

技术领域

本发明涉及飞行器姿态机动控制领域，更具体的说是涉及一种线性伪谱外太空飞行器能量最优大角度姿态机动控制方法。

背景技术

以动能拦截器为代表的一类外太空飞行器，为了实现打击目标、躲避攻击等任务要求，通常需要具有良好的姿态机动能力。相比于以人造卫星、空间站等为代表的航天器，此类外太空飞行器的姿态机动存在以下特点：(1) 机动角度大、时间短，即需要实现限定时间内的大角度姿态机动；(2)控制方式一般为主动控制，且多采用反作用控制系统(RCS)进行控制；(3)携带燃料有限，要求机动消耗能量尽可能小；(4)由于机动时间较短，通常忽略如重力梯度、太阳光压等扰动项。

外太空飞行器的姿态运动系统是具有强耦合特性的多输入多输出 (MIMO)非线性系统，为了实现飞行器能量最优的大角度姿态机动，目前常用的控制方案有线性二次型控制(LQR)和模型预测控制(MPC)。LQR方法基于一条事先计算得到的基准控制路径，以线性模型近似刻画状态偏差传播过程，再设定关于状态量和控制量的性能指标，用最优控制理论进行求解。 MPC方法是一种基于滚动优化的在线控制策略，通过预测终端状态反推当前时刻的控制修正量，具有对模型要求低、抗干扰性好、鲁棒性强等优点，能够在优化性能指标的同时处理各种约束条件。

尽管LQR方法和MPC方法都能实现飞行器能量最优大角度姿态机动控制，但在实际应用中存在一些问题。LQR方法存在的问题有：(1)LQR求解得到的控制量是无限时域中的最优控制修正量，无法保证终端状态精度在限定时间内能够满足要求，也无法保证控制修正量在限定时间内消耗能量最优； (2)LQR求解的控制修正量仅保证控制修正量消耗的能量最优，而并非修正后的控制量消耗的能量最优；(3)LQR控制的效果取决于Q、R阵的选取，对于不同的姿态机动任务都需要进行选取。MPC方法主要面临的问题为计算过于复杂、消耗计算机资源大，难以保证计算的实时性。

因此，如何提供一种满足高精度要求，且保证姿态机动终端约束的能量最优的大角度姿态机动控制方法是本领域技术人员亟需解决的问题。

发明内容

有鉴于此，本发明提供了一种线性伪谱外太空飞行器能量最优大角度姿态机动控制方法，目的是针对以动能拦截器为代表的外太空飞行器开发一种通过修正基准姿态机动控制量进而保证姿态机动终端约束的能量最优姿态机动控制方法；本发明的关键技术是基于线性伪谱模型预测控制的思想推导姿态机动终端状态偏差与控制修正量之间的解析公式；得益于解析公式的获得，本发明具有计算效率高、求解精度高的特点，适用于姿态机动控制的在线使用。

为了达到上述目的，本发明采用如下技术方案：

一种线性伪谱外太空飞行器能量最优大角度姿态机动控制方法，包括如下步骤：

S1：机动任务初始化：设置姿态机动的任务参数；

S2：获得姿态机动能量最优基准控制轨迹：先利用高斯伪谱法将飞行器能量最优姿态机动最优控制问题转化为非线性规划问题，再利用SNOPT求解所述非线性规划问题，从而获得飞行器能量最优姿态机动轨迹，包括状态量的时间序列控制量的时间序列并将所述飞行器能量最优姿态机动轨迹作为基准控制轨迹；

S3：修正方法参数设置：设置线性伪谱模型预测控制修正方法的节点数 N，控制修正更新检查间隔τ_check，控制修正阈值

S4：按基准控制进行机动：飞行器将按照基准控制轨迹进行姿态机动控制，同时记录当前的时间；若当前时间距上次更新检查时间到达τ_check时，进入步骤S5；若当前时间到达姿态机动限定时间T时，停止控制，完成姿态机动；

S5：控制更新检查：根据当前状态和基准控制，进行一次全程积分，获得当前状态控制不加修正下的终端状态积分预测偏差若终端状态积分预测偏差大于控制修正阈值，即则进入步骤S6进行控制修正，否则返回步骤S4；

S6：控制指令修正：获取终端预测状态量与高斯节点的状态量、控制量之间的关系，根据步骤S5得到的终端状态积分预测偏差计算出控制指令修正量δu(t)，与基准控制轨迹中控制量的时间序列结合得到修正后的控制量并将所述修正后的控制量作为新的基准控制，返回步骤 S4。

优选的，在上述线性伪谱外太空飞行器能量最优大角度姿态机动控制方法中，所述步骤S1中，所述任务参数包括：

姿态机动限定时间T，初始姿态角[γ₀,θ₀,ψ₀]，初始姿态角速度[ω_x0,ω_y0,ω_z0]，终端姿态角[γ_f,θ_f,ψ_f]，终端姿态角速度[ω_1f,ω_2f,ω_3f]，控制幅值约束 [U_1max,U_2max,U_3max]，其中，γ为绕飞行器本体系OX^b轴转动的滚转角，θ为绕飞行 器本体系OY^b轴转动的俯仰角，ψ为绕飞行器本体系OZ^b轴转动的偏航角， U_i(i＝1,2,3)分别为滚转、俯仰、偏航通道的控制力矩，下标₀代表起始时刻，下标 f代表终端时刻，下标_max代表幅值上限。

优选的，在上述线性伪谱外太空飞行器能量最优大角度姿态机动控制方法中，所述步骤S2中：飞行器能量最优大角度姿态机动问题可描述为：寻找时变控制指令u(t)∈R³，使得满足状态空间方程的系统的状态量x在有限时间 t≤T内从初始状态转变为终端状态同时满足过程约束和控制消耗的能量最优。

优选的，在上述线性伪谱外太空飞行器能量最优大角度姿态机动控制方法中，所述步骤S6具体包括如下步骤：

(1)状态偏差转播方程的线性化；

(2)终端偏差与控制修正的解析关系的推导；

(3)控制修正能量最优解析解；

(4)控制基准轨迹的更新。

优选的，在上述线性伪谱外太空飞行器能量最优大角度姿态机动控制方法中，所述步骤(1)中：

具有终端约束的姿态机动非线性动力学方程如下：

规划路径每一时刻的状态量控制量与该时刻实际的状态量x(t)、控制量u(t)的偏差为：

规划的终端状态量与实际的终端状态量x(t_f)之间的偏差为：

δx(t)为状态偏差，δu(t)为控制修正；将动力学方程在规划状态量附近进行泰勒展开，并忽略二阶以上高阶项，得到一组以状态偏差δx(t)为自变量的误差传播动力学方程为：

A(t)∈R^6×6为状态误差传播矩阵，B(t)∈R^6×3为控制误差传播矩阵，具体形式为：

其中，A₁₁∈R^3×3，A₁₂∈R^3×3，A₂₂∈R^3×3，其具体表达式为：

优选的，在上述线性伪谱外太空飞行器能量最优大角度姿态机动控制方法中，所述步骤(2)中：

选取LG节点对状态量和控制量进行离散，将实际机动时间t∈[t₀,t_f]映射到归一化时间τ∈[-1,1]上：

则归一化后的误差传播动力学方程为：

其中，

定义L_N(τ)为N阶拉格朗日插值多项式，τ_i为N阶拉格朗日多项式的根，即LG节点的归一化时刻，状态偏差δx用一组由LG节点τ_i为支撑点所形成的拉格朗日插值多项式基的线性组合进行拟合：

同样的，得到控制量的插值拟合：

通过对状态量求导：

其中，微分逼近矩阵D∈R^N×(N+1)是通过对拉格朗日插值多项式的各个元素分别求导获得的，则微分动力学约束表示为LG节点上各个预测状态偏差和控制修正的函数：

其中，k＝1,2,...,N。

设初始指令修正为零，除去初始点，设LG节点上的误差传播动力学方程预测状态偏差序列和控制修正序列为：

重新组合微分逼近矩阵D，得到预测状态偏差序列和控制修正序列表示的关系式如下：

其中，

上式中，s为状态量个数，矩阵A^*和B^*的表达式如下：

则LG节点上的各个状态量表示为：

终端状态预测偏差通过高斯型积分公式表示为初始状态偏差δx₀和LG 节点状态偏差预测的显式函数关系，则终端状态预测偏差表示为以下形式：

其中，为高斯型积分公式的权函数矩阵，表示为：

得终端状态预测偏差关于初始状态偏差δx₀和控制修正的显式解析表达式形式如下：

其中，K^X∈R^6×6，K^u∈R^6×3N，其具体表达式为：

优选的，在上述线性伪谱的外太空飞行器能量最优大角度姿态机动控制方法中，所述步骤(3)中：

设积分得到的积分预测终端状态为目标终端状态量为则未进行控制修正前的终端状态积分预测偏差为：

对终端状态积分预测偏差进行控制修正，产生反向的终端偏差以抵消原有偏差：

存在多个解，则存在一组解使得以下式子最小：

则即为要求的能量最优控制修正量；将Φ展开：

由于为规划控制，故有：

设t_k(k＝1,2,...,N)为[t₀,t_f]上LG节点所在的时间点，为第i通道在t_k时刻的规划控制，为第i通道在t_k时刻的控制修正，用LG节点上的离散值对全路径积分进行拟合：

其中，w_k为高斯型积分的权函数，求解的问题描述为二次规划问题：

利用成熟算法迭代求得数值解，定义拉格朗日函数L为：

其中，为矩阵K^u的第i行，为的第i项，λ_i为拉格朗日乘子：

经代数运算，得：

其中，为LG节点上的规划控制，λ＝[λ₁ ... λ₆]^T，Ω∈R^3N×3N：

P∈R^6×6，设其第i行j列的元素为p_ij，则有：

其中，为矩阵K^u第i行m列的元素，Ω_mm为矩阵Ω第m行m列的元素；

得到既满足终端约束又保证能量最优的姿态机动控制u^mincost(t)为：

经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种线性伪谱外太空飞行器能量最优大角度姿态机动控制方法，通过修正基准姿态机动控制量进而保证姿态机动终端约束的能量最优姿态机动控制方法；本发明的关键技术是基于线性伪谱模型预测控制的思想推导姿态机动终端状态偏差与控制修正量之间的解析公式；该方法具有计算效率高、求解精度高的特点；线性伪谱控制能够达到与LQR控制近似的终端精度，实现飞行器有限时间内的大角度姿态机动，并且相比于LQR控制消耗更少的能量，同时其控制指令更加平滑，满足姿态机动控制在线使用的实时性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1附图为本发明线性伪谱外太空飞行器能量最优大角度姿态机动控制方法的流程示意图；

图2附图为本发明姿态机动仿真的姿态角示意图；

图3附图为本发明姿态机动仿真的角速度示意图；

图4附图为本发明姿态机动仿真的控制指令示意图；

图5附图为本发明姿态机动蒙特卡罗仿真的姿态角示意图；

图6附图为本发明姿态机动蒙特卡洛仿真的角速度示意图；

图7附图为本发明姿态机动蒙特卡洛仿真终端精度散布图；

图8附图为本发明姿态机动蒙特卡洛仿真能量消耗对比图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

外太空飞行器能量最优大角度姿态机动任务描述如下：飞行器可看作刚体，装有多个能提供连续推力的推进器，使其在俯仰、滚转、偏航等3个通道上都可产生连续但有限的控制力矩。飞行器需要在一定的限制时间内，实现从初始姿态到终端姿态的机动，并使得姿态机动消耗的能量尽可能少。飞行器姿态机动过程中不考虑重力梯度、太阳光压产生的误差，但存在测量噪声和过程噪声。

下面将结合实施例对本发明作进一步的详细说明。

一种线性伪谱外太空飞行器能量最优大角度姿态机动控制方法，具体步骤步骤如下：

S1：机动任务初始化：设置姿态机动的任务参数，包括姿态机动限定时间T，初始姿态角[γ₀,θ₀,ψ₀]，初始姿态角速度[ω₁₀,ω₂₀,ω₃₀]，终端姿态角[γ_f,θ_f,ψ_f]，终端姿态角速度[ω_1f,ω_2f,ω_3f]，控制幅值约束[U_1max,U_2max,U_3max]，其中，γ为绕飞行器本体系OX^b轴转动的滚转角，θ为绕飞行器本体系OY^b轴转动的俯仰角，ψ为绕飞行器本体系OZ^b轴转动的偏航角，U_i(i＝1,2,3)分别为滚转、俯仰、偏航通道的控制力矩，下标0代表起始时刻，下标f代表终端时刻，下标max代表幅值上限。

实际的姿态机动任务可能包含多次姿态机动，故可以设置多个姿态机动初始和结束条件，规划出多条基准控制轨迹，存于飞行器数据库中。

S2：获得姿态机动能量最优基准控制轨迹：先利用高斯伪谱法将飞行器能量最优姿态机动最优控制问题转化为非线性规划问题，再利用SNOPT求解该非线性规划问题，从而获得飞行器能量最优姿态机动轨迹，包括状态量的时间序列控制量的时间序列等，并将其作为基准控制轨迹。飞行器姿态机动的建模如下所示：

1)姿态机动动力学方程建模

采用修正罗德里格斯参数(Modified Rodrigues Parameters，MRPs)描述飞行器的姿态。飞行器的姿态运动学和动力学方程可描述为：

式中，J＝diag(J₁,J₂,J₃)为飞行器转动惯量矩阵；ω＝[ω₁ ω₂ ω₃]^T为飞行器本体系相对于惯性系的姿态角速度矢量；u＝[u₁ u₂ u₃]^T为飞行器的控制力矩矢量； d＝[d₁d₂ d₃]^T为飞行器受到的干扰力矩矢量；σ＝[σ₁ σ₂ σ₃]^T为修正罗德里格斯参数(MRPs)，描述了飞行器的本体系相对于惯性系的姿态；S(·)算子为3×3 的反对称矩阵，对于任意一个向量a＝[a₁ a₂ a₃]^T，有：

G(σ)∈R^3×3为飞行器姿态运动学矩阵，定义为：

飞行器姿态机动过程中，控制力矩存在如下幅值约束：

|u_i|≤U_imax (i＝1,2,3) 公式(4)

式中：U_imax＞0为飞行器第i个推力器的最大输出力矩幅值。

2)姿态机动最优控制问题描述

设状态量x＝[σ₁ σ₂ σ₃ ω₁ ω₂ ω₃]^T，则飞行器姿态运动系统可采用状态空间的形式描述为：

式中，状态转移矩阵可由公式(1)至(3)推出，控制转移矩阵 F_B＝[0_3×3J^-1]^T，此处假设飞行器的姿态和姿态角速度均可被测量，故F_C＝I_6×6， r＝[r₁ r₂ r₃]^T为测量误差，干扰力矩d＝[d₁ d₂ d₃]^T。

飞行器能量最优大角度姿态机动问题可描述为：寻找时变控制指令 u(t)∈R³，使得满足状态空间方程(5)的系统的状态量x在有限时间t≤T内从初始状态转变为终端状态同时满足过程约束和控制消耗的能量最优，即：

此处选取二次型指标作为性能指标，不失一般性，取：

3)姿态机动最优控制问题的求解

由于姿态机动的初始状态和终端状态已知，该最优控制问题是一个两点边值问题，理论上可以利用极大值原理求解，但实际上其解析解难以得到(目前仍未有)，工业上都使用数值解法求解此类最优控制问题。此处采用高斯伪谱法，将姿态机动最优控制问题转化为非线性规划问题，再用SNOPT工具包求解该非线性规划问题，得到姿态机动最优控制轨迹。

S3：修正方法参数设置：设置线性伪谱模型预测控制修正方法的节点数 N，控制修正更新检查间隔τ_check，控制修正阈值

对于飞行器姿态机动控制而言，通常选择节点数N≈8便能达到很高精度；控制修正更新检查间隔τ_check无需太小，因为更新过于频繁可能会导致控制量跳动激烈；控制修正阈值通常取决于终端精度要求以及测量噪声。

S4：基准控制段：飞行器将按照基准控制轨迹进行姿态机动控制，同时记录当前的时间；若当前时间距上次更新检查时间到达τ_check时，进入步骤S5；若当前时间到达姿态机动限定时间T时，停止控制，完成姿态机动。

S5：控制更新检查：根据当前状态和基准控制，进行一次全程积分，获得当前状态控制不加修正下的终端状态积分预测偏差若终端状态积分预测偏差大于控制修正阈值，即则进入步骤S6进行控制修正，否则返回步骤S4。无论控制更新与否，均重新设定上次更新时间为当前时间。

S6：控制指令修正：基于线性伪谱、最优控制理论、变分原理等获取终端状态量与高斯节点的状态量、控制量之间的关系，根据步骤S5得到的终端状态积分预测偏差计算出控制指令修正量δu(t)，与基准控制结合得到修正后的控制量并将其作为新的基准控制，返回步骤S4。

这一步为本发明的关键步骤，这里详细说明控制指令修正的具体实现过程及原理。其具体实现过程可以分为状态偏差转播方程的线性化、终端偏差与控制修正的解析关系的推导、控制修正能量最优解析解的推导、控制基准轨迹的更新四部分。

1)状态偏差转播方程的线性化

一般地，考虑具有终端约束的姿态机动非线性动力学方程如下：

假设已存在一条规划完成的能量最优姿态机动路径，则规划路径每一时刻的状态量控制量与该时刻实际的状态量x(t)、控制量u(t)的偏差定义为：

规划的终端状态量与实际的终端状态量x(t_f)之间的偏差定义为：

称δx(t)为状态偏差，δu(t)为控制修正。将动力学方程在规划状态量附近进行泰勒展开，并忽略二阶以上高阶项，可得一组以状态偏差δx(t)为自变量的误差传播动力学方程为：

对于飞行器姿态动力学系统而言，状态误差传播矩阵A(t)∈R^6×6，控制误差传播矩阵B(t)∈R^6×3，他们的具体形式为：

其中，A₁₁∈R^3×3，A₁₂∈R^3×3，A₂₂∈R^3×3，其具体表达式为：

2)终端偏差与控制修正的解析关系的推导

选取Legendre-Gauss节点(以下称LG节点)对状态量和控制量进行离散，将实际机动时间t∈[t₀,t_f]映射到归一化时间τ∈[-1,1]上：

则归一化后的误差传播动力学方程为：

其中，

定义L_N(τ)为N阶拉格朗日插值多项式，τ_i为N阶拉格朗日多项式的根(即 LG节点的归一化时刻)，则可将误差传播动力学方程中的状态偏差δx用一组由LG节点τ_i为支撑点所形成的拉格朗日插值多项式基的线性组合进行拟合：

同样的，我们得到控制量的插值拟合：

通过对状态量求导可得：

其中，微分逼近矩阵D∈R^N×(N+1)是通过对拉格朗日插值多项式的各个元素分别求导获得的，其具体表达式为：

则微分动力学约束不仅能转化为一组代数约束，并且能够表示为LG节点上各个预测状态偏差和控制修正的函数：

其中，k＝1,2,...,N。

由于控制是连续的，故可设初始指令修正为零。除去初始点，设LG 节点上的误差传播动力学方程预测状态偏差和控制修正序列为：

重新组合微分逼近矩阵D，可得到预测状态偏差序列和控制修正序列表示的关系式如下：

其中，

上式中，s为状态量个数，矩阵A^*和B^*的表达式如下：

则LG节点上的各个状态量可表示为：

在高斯伪谱离散过程中，LG节点并不包括终端节点，但终端状态偏差预测可以通过高斯型积分公式表示为初始状态偏差δx₀和LG节点状态偏差预测的显式函数关系：

其中，w_i是高斯型积分的权函数，可通过数值计算获得，则终端状态偏差预测可表示为以下形式：

其中，为高斯型积分公式的权函数矩阵，可表示为：

最终，可得终端状态偏差预测关于初始状态偏差δx₀和控制修正的显式解析表达式形式如下：

其中，K^X∈R^6×6，K^u∈R^6×3N，其具体表达式为：

至此，我们获得了终端状态偏差关于初始状态偏差和控制修正的解析预测关系式。

3)控制修正能量最优解析解

由于我们在进行全程积分时总是根据当前状态量作为初始状态进行积分，因此初始状态偏差δx₀≡0，式(40)可以简写为：

设积分得到的积分预测终端状态为目标终端状态量为则未进行控制修正前的终端状态积分预测偏差为：

这个终端状态积分预测偏差是我们不希望的，因此需要进行控制修正，使其产生反向的终端偏差以抵消原有偏差：

由于根据公式(43)可知存在多个解(一般取LG 节点数N≈8)，则必然存在一组解使得以下式子最小：

则即为要求的能量最优控制修正量。将Φ展开，有：

由于为规划控制，故有：

设t_k(k＝1,2,...,N)为[t₀,t_f]上LG节点所在的时间点，为第i通道在t_k时刻的规划控制，为第i通道在t_k时刻的控制修正，则根据拉格朗日插值多项式的性质，可用LG节点上的离散值对全路径积分进行拟合：

其中，w_k为前面描述的高斯型积分的权函数，可通过数值计算获得。故求解的问题可描述为一个二次规划问题：

此二次规划问题可利用成熟算法迭代求得数值解，此处给出其解析解。定义拉格朗日函数L为：

其中，为矩阵K^u的第i行，为的第i项，λ_i为拉格朗日乘子，则有：

经代数运算，可得：

其中，为LG节点上的规划控制，λ＝[λ₁ ... λ₆]^T，Ω∈R^3N×3N，其表达式为：

P∈R^6×6，设其第i行j列的元素为p_ij，则有：

其中，为矩阵K^u第i行m列的元素，Ω_mm为矩阵Ω第m行m列的元素。

根据公式(53)至公式(65)即可求出能量最优的LG节点控制修正量从而根据公式(24)插值得到每一时刻的控制修正即可得到既满足终端约束又保证能量最优的姿态机动控制u^mincost(t)为：

4)控制基准轨迹的更新

用上一步计算得到的控制指令u^mincost(t)作为新的控制基准指令替换原数据库中的控制基准指令，从而使得控制指令得以不断地迭代更新的。

算例

仿真中的飞行器模型参数如表1所示：

表格1飞行器模型参数

姿态机动任务的初始条件和终端条件设置如表2：

表格2姿态机动初始和终端条件

其中，γ为绕飞行器本体系OX^b轴转动的滚转角，θ为绕飞行器本体系OY^b轴转动的俯仰角，ψ为绕飞行器本体系OZ^b轴转动的偏航角。飞行器采用Z-Y-X的旋转顺序进行姿态变换。注意到姿态机动前后的角速度均为0，故此姿态机动算例为Rest-to-Rest问题。

设初始姿态角偏差为(单位°)：

[Δγ₀,Δθ₀,Δψ₀]＝[3.6436,4.0508,0.5679]

初始角速度偏差为(单位°/s)：

[Δω₁₀,Δω₂₀,Δω₃₀]＝[2.87,2.11,0.26]

指令更新间隔时间设为：

τ_check＝2s

姿态角控制修正阈值设为0.01，角速度控制修正阈值设为0.005。

为对比控制效果，采用LQR控制方法跟踪基准控制轨迹做同样的姿态机动控制，取Q、R阵为：

干扰力矩(控制噪声)满足正态分布，设为d_i～N(0,0.05)；测量噪声满足正态误差r～N(0,0.005)。仿真结果如表3和图2-图4所示。

表3姿态机动算例仿真结果

从仿真结果能够看出，线性伪谱控制能够达到与LQR控制近似的终端精度，实现飞行器有限时间内的大角度姿态机动，并且相比于LQR控制消耗更少的能量，同时其控制指令更加平滑。

设初始姿态角偏差满足正态分布N(0,10)，初始角速度偏差满足正态分布 N(0,5)，做200次蒙塔卡洛仿真，结果如表4和图5-图8所示。

表4姿态机动蒙特卡洛仿真终端精度

仿真使用的计算机为普通配置PC机：Intel Core i5-4200M处理器，4G内存；软件环境为Windows 7(32位)操作系统以及MATLAB R2013a。仿真中，线性伪谱控制的高斯节点数取为8，进行一次指令更新的计算时间约为 0.05s；LQR控制指令更新一次的计算时间约为0.004s。尽管线性伪谱控制指令修正的计算时间大于LQR控制，但该方法不需要每一时刻都计算更新指令，只有在预测偏差大于设定阈值的情况下才更新，在仿真中，指令更新数基本在1～3次的范围内。从计算消耗时间可以看出，线性伪谱控制方法能够满足弹上计算的实时性要求。

通过图5-图8蒙特卡洛仿真结果可以看出，线性伪谱控制能达到LQR控制近似的精度(线性伪谱控制的精度可以用更新阈值来调节)，而线性伪谱控制相对LQR控制而言能平均减少约10％的能量消耗。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (7)

1.一种线性伪谱外太空飞行器能量最优大角度姿态机动控制方法，其特征在于，包括如下步骤：

S1：机动任务初始化：设置姿态机动的任务参数；

S2：获得姿态机动能量最优基准控制轨迹：先利用高斯伪谱法将飞行器能量最优姿态机动最优控制问题转化为非线性规划问题，再利用SNOPT求解所述非线性规划问题，从而获得飞行器能量最优姿态机动轨迹，包括状态量的时间序列控制量的时间序列并将所述飞行器能量最优姿态机动轨迹作为基准控制轨迹；

S3：修正方法参数设置：设置线性伪谱模型预测控制修正方法的节点数N，控制修正更新检查间隔τ_check，控制修正阈值

S4：按基准控制进行机动：飞行器将按照基准控制轨迹进行姿态机动控制，同时记录当前的时间；若当前时间距上次更新检查时间到达τ_check时，进入步骤S5；若当前时间到达姿态机动限定时间T时，停止控制，完成姿态机动；

S5：控制更新检查：根据当前状态和基准控制，进行一次全程积分，获得当前状态控制不加修正下的终端状态积分预测偏差若终端状态积分预测偏差大于控制修正阈值，即则进入步骤S6进行控制修正，否则返回步骤S4；

S6：控制指令修正：获取终端预测状态量与高斯节点的状态量、控制量之间的关系，根据步骤S5得到的终端状态积分预测偏差计算出控制指令修正量δu(t)，与基准控制轨迹中控制量的时间序列结合得到修正后的控制量并将所述修正后的控制量作为新的基准控制，返回步骤S4。

2.根据权利要求1所述的线性伪谱外太空飞行器能量最优大角度姿态机动控制方法，其特征在于，所述步骤S1中，所述任务参数包括：

姿态机动限定时间T，初始姿态角[γ₀,θ₀,ψ₀]，初始姿态角速度[ω_x0,ω_y0,ω_z0]，终端姿态角[γ_f,θ_f,ψ_f]，终端姿态角速度[ω_1f,ω_2f,ω_3f]，控制幅值约束[U_1max,U_2max,U_3max]，其中，γ为绕飞行器本体系OX^b轴转动的滚转角，θ为绕飞行器本体系OY^b轴转动的俯仰角，ψ为绕飞行器本体系OZ^b轴转动的偏航角，U_i(i＝1,2,3)分别为滚转、俯仰、偏航通道的控制力矩，下标0代表起始时刻，下标f代表终端时刻，下标_max代表幅值上限。

3.根据权利要求1所述的线性伪谱外太空飞行器能量最优大角度姿态机动控制方法，其特征在于，所述步骤S2中：飞行器能量最优大角度姿态机动问题可描述为：寻找时变控制指令u(t)∈R³，使得满足状态空间方程的系统的状态量x在有限时间t≤T内从初始状态转变为终端状态同时满足过程约束和控制消耗的能量最优。

4.根据权利要求1所述的线性伪谱外太空飞行器能量最优大角度姿态机动控制方法，其特征在于，所述步骤S6具体包括如下步骤：

(1)状态偏差转播方程的线性化；

(2)终端预测偏差与控制修正的解析关系的推导；

(3)控制修正能量最优解析解；

(4)控制基准轨迹的更新。

5.根据权利要求4所述的线性伪谱外太空飞行器能量最优大角度姿态机动控制方法，其特征在于，所述步骤(1)中：

具有终端约束的姿态机动非线性动力学方程如下：

规划路径每一时刻的状态量控制量与该时刻实际的状态量x(t)、控制量u(t)的偏差为：

规划的终端状态量与实际的终端状态量x(t_f)之间的偏差为：

δx(t)为状态偏差，δu(t)为控制修正；将动力学方程在规划状态量附近进行泰勒展开，并忽略二阶以上高阶项，得到一组以状态偏差δx(t)为自变量的误差传播动力学方程为：

A(t)∈R^6×6为状态误差传播矩阵，B(t)∈R^6×3为控制误差传播矩阵，具体形式为：

其中，A₁₁∈R^3×3，A₁₂∈R^3×3，A₂₂∈R^3×3，其具体表达式为：

6.根据权利要求4所述的线性伪谱外太空飞行器能量最优大角度姿态机动控制方法，其特征在于，所述步骤(2)中：

选取LG节点对状态量和控制量进行离散，将实际机动时间t∈[t₀,t_f]映射到归一化时间τ∈[-1,1]上：

则归一化后的误差传播动力学方程为：

其中，

定义L_N(τ)为N阶拉格朗日插值多项式，τ_i为N阶拉格朗日多项式的根，即LG节点的归一化时刻，状态偏差δx用一组由LG节点τ_i为支撑点所形成的拉格朗日插值多项式基的线性组合进行拟合：

同样的，得到控制量的插值拟合：

通过对状态量求导：

其中，微分逼近矩阵D∈R^N×(N+1)是通过对拉格朗日插值多项式的各个元素分别求导获得的，则微分动力学约束表示为LG节点上各个预测状态偏差和控制修正的函数：

其中，k＝1,2,...,N。

设初始指令修正为零，除去初始点，设LG节点上的误差传播动力学方程预测状态偏差序列和控制修正序列为：

重新组合微分逼近矩阵D，得到预测状态偏差序列和控制修正序列表示的关系式如下：

其中，

上式中，s为状态量个数，矩阵A^*和B^*的表达式如下：

则LG节点上的各个状态量表示为：

终端状态偏差预测通过高斯型积分公式表示为初始状态偏差δx₀和LG节点状态偏差预测的显式函数关系，则终端状态偏差预测表示为以下形式：

其中，为高斯型积分公式的权函数矩阵，表示为：

得到终端状态预测偏差关于初始状态偏差δx₀和控制修正的显式解析表达式形式如下：

其中，K^X∈R^6×6，K^u∈R^6×3N，其具体表达式为：

7.根据权利要求4所述的线性伪谱外太空飞行器能量最优大角度姿态机动控制方法，其特征在于，所述步骤(3)中：

设积分得到的积分预测终端状态为目标终端状态量为则未进行控制修正前的终端状态积分预测偏差为：

对终端状态积分预测偏差进行控制修正，产生反向的终端偏差以抵消原有偏差：

存在多个解，则存在一组解使得以下式子最小：

则即为要求的能量最优控制修正量；将Φ展开：

由于为规划控制，故有：

设t_k(k＝1,2,...,N)为[t₀,t_f]上LG节点所在的时间点，为第i通道在t_k时刻的规划控制，为第i通道在t_k时刻的控制修正，用LG节点上的离散值对全路径积分进行拟合：

其中，w_k为高斯型积分的权函数，求解的问题描述为二次规划问题：

利用成熟算法迭代求得数值解，定义拉格朗日函数L为：

其中，为矩阵K^u的第i行，为的第i项，λ_i为拉格朗日乘子：

经代数运算，得：

其中，为LG节点上的规划控制，λ＝[λ₁ ... λ₆]^T，Ω∈R^3N×3N：

P∈R^6×6，设其第i行j列的元素为p_ij，则有：

其中，为矩阵K^u第i行m列的元素，Ω_mm为矩阵Ω第m行m列的元素；得到既满足终端约束又保证能量最优的姿态机动控制u^mincost(t)为：