CN113485397B - 一种基于多项式规划的航天器姿态机动路径规划方法 - Google Patents

Abstract

本发明涉及一种基于多项式规划的航天器姿态机动路径规划方法，给出包括航天器姿态运动学模型，动力学模型，饱和约束，姿态禁区约束，姿态强制区约束，初末状态约束以及组合性能函数的航天器姿态机动路径规划模型；对上述模型进行离散化，得到多项式规划模型，再对其进行转化，得到具有非凸二次约束的非齐次二次规划模型；将非齐次二次规划模型转化为齐次模型，并转化为带有秩约束的半定松弛模型；通过罚函数方法将秩约束转化到性能函数中，得到带有秩惩罚的半定规划模型；给出逐次迭代算法，得到航天器姿态姿态机动的最优路径。本发明不需要提供初始机动方案，且性能函数可以按照需求选择为时间最优，能量最优，角速度最优，得到的姿态机动路径为最优机动路径。

Description

一种基于多项式规划的航天器姿态机动路径规划方法

技术领域

本发明涉及航天器姿态机动技术领域，尤其涉及一种基于多项式规划的航天器姿态机动路径规划方法。

背景技术

随着航天技术的发展，空间任务也越来越复杂，航天器姿态控制系统是实现空间任务的关键系统，需要具有较高的精度和可靠性，故其发展也面临着巨大的的挑战。对于传统的无约束航天器姿态机动问题，常见的控制方法有：鲁棒控制、滑模控制、自适应控制、李雅普诺夫直接法，以及各类方法的混合控制方法。但是在实际任务中存在多种约束，对于带约束的航天器姿态机动问题，常见的解决方法有滑模控制、模型预测控制、势函数等。

本发明以航天器姿态系统为载体，考虑航天器姿态机动过程中存在的多种约束，研究基于多项式规划方法的航天器姿态机动路径规划问题，该方法可以得到航天器姿态机动的最优路径。

发明内容

本发明技术解决问题：克服现有技术的不足，基于多项式规划的航天器姿态机动路径规划方法，考虑了多种约束，包括姿态运动学和动力学约束，角速度和控制力矩饱和约束，姿态禁止区和强制区约束以及初末状态约束；所设计的性能函数为考虑时间，角速度和能量的综合性能函数，得到的结果为最优解，且便于增加约束的数目和种类。

本发明技术解决方案：一种基于多项式规划的航天器姿态机动路径规划方法，包括如下步骤：

S1：将四元数的航天器姿态的运动学和动力学模型，航天器姿态的饱和约束、姿态禁区约束、姿态强制区约束以及初末状态约束的数学模型，及综合了时间、力矩和角速度的组合性能函数复合，得到航天器姿态机动的数学模型；

S2：对步骤S1中得到的航天器姿态机动的数学模型进行离散化，得到多项式规划模型，再采用变量扩充的方法得到具有非凸二次约束的非齐次二次规划模型；

S3：将步骤S2中得到的具有非凸二次约束的非齐次二次规划模型转化为齐次形式，再采用半定松弛的方法将非凸二次约束进行转化，得到带有秩约束的半定规划模型；

S4：采用罚函数方法将步骤S3中得到的带有秩约束的半定规划模型中的秩约束转化到性能函数中，得到带有秩惩罚的半定规划模型；

S5：采用逐次迭代的求解策略求解步骤S4中得到的带有秩惩罚的半定规划模型，最终得到航天器姿态机动的最优路径。

所述步骤S1具体实现为：

(1)基于四元数的航天器姿态的运动学和动力学模型如下：

基于四元数的航天器姿态运动学模型为：

四元数：

q＝[q₁,q₂,q₃,q₄]^T∈R⁴ (2)

且四元数的二范数为1，且该范数为1的约束被包含在基于四元数的航天器姿态运动学模型中，范数为1的约束可以描述为如下形式：

航天器本体系相对地心惯性系的角速度向量为ω＝[ω₁,ω₂,ω₃]^T∈R³，ω₁,ω₂,ω₃分别为航天器本体系相对地心惯性系的三轴角速度，其中

航天器的姿态动力学模型为：

其中为向量ω＝[ω₁,ω₂,ω₃]^T∈R³的斜对称矩阵；表示航天器相对于本体系的主轴转动惯量矩阵，J₁,J₂,J₃分别表示航天器相对于本体轴x,y,z的转动惯量，u＝[u₁,u₂,u₃]^T∈R³表示航天器在三轴方向的控制输入力矩向量，u₁,u₂,u₃分别表示航天器在x,y,z方向的控制输入力矩。

(2)饱和约束数学模型为：

航天器执行机构的输出存在上限，故存在控制力矩饱和约束：

|u|≤u_max (5)

其中u_max为执行机构能输出的力矩的最大值。

此外，航天器测量元件存在测量上限，故航天器的转动角速度也存在饱和约束：

|ω|≤ω_max (6)

其中ω_max为航天器测量元件所能测量的最大角速度。

(3)姿态禁区约束和姿态强制区约束的数学模型为：

航天器携带的载荷中存在光敏和热敏元件，不能指向太阳光，则这类元件所在的视轴不能进入指定区域，即姿态禁区约束：

其中为地心惯性系下的约束单位矢量，在姿态禁区约束中指向规避物体，/>表示航天器本体系下航天器的视轴单位矢量，β_m为约束矢量与视轴矢量之间允许存在的最小夹角，被称为约束角；/>为航天器本体系到惯性系的坐标转换矩阵。

经过化简后得到姿态禁区约束的数学模型为：

q^TM_fq≤0 (8)

其中

同理，姿态强制区的数学模型为：

q^TM_mq≥0 (9)

其中

β_M为约束矢量与视轴矢量之间允许存在的最大夹角。

(4)初末状态约束的数学模型为：

其中，t₀表示初始时刻，t_f表示姿态机动至期望姿态的时刻；ω₀表示航天器的初始角速度，ω_f表示航天器的末端角速度；q₀表示航天器的初始姿态，q_f表示航天器的期望姿态。

(5)综合了时间、力矩和角速度的组合性能函数，其数学描述为：

其中第一项表示机动时间最优，第二项表示消耗能量最优，第三项表示角速度最优。

航天器姿态机动的数学模型为以上(1)-(5)的五个模型之和。

所述步骤2中，

(1)对于步骤1中的航天器姿态机动的数学模型进行离散化处理：

采用梯形离散的方法，首先对公式(1)进行离散得到：

其中N表示离散节点的数目，k表示变量的第k个离散节点，Δt表示离散时间步长。

采用同样的方法对公式(4)进行离散得到：

对公式(3)进行离散处理得到：

对(5)和(6)进行离散处理得到：

|u(k)|≤u_max， k＝1...N (15)

|ω(k)|≤ω_max， k＝1...N (16)

对公式(8)和(9)进行离散处理得到：

q(k)^TM_fq(k)≤0， k＝1...N (17)

q(k)^TM_mq(k)≥0， k＝1...N (18)

公式(10)转化为：

组合性能函数的公式(11)转化为：

其中J′为离散后的性能函数，N表示所设置的离散时间节点数目，表示离散时间步长，q(k)为离散后的四元数，ω(k)为离散后的角速度，u(k)为离散后的控制力矩。

(2)对多项式规划模型进行转化：

为了将多项式规划模型转化为二次规划模型，引入如下新变量：

则公式(12)和(13)变为：

公式(20)变为：

向量z(k)＝[u(k)^T,u′(k)^T,ω(k)^T,ω′(k)^T,q(k)^T]^T，则离散后的模型的状态变量表示为：

z＝[z(0)^T,...,z(k)^T,...,z(N)^T]^T∈R^m (25)

则多项式规划模型转化为下面的具有非凸二次约束的非齐次二次规划模型：

其中M_J∈R^m×m表示性能函数的系数矩阵，M_Ei∈R^m×m,p_Ei∈R^m,q_Ei∈R表示等式约束的系数矩阵，M_Ij∈R^m×m,p_Ij∈R^m,q_Ij∈R表示不等式约束的系数矩阵，M_J,M_Ei,M_Ij为对称矩阵，r表示等式约束的数目，s表示不等式约束的数目。

所述步骤S3中，

(1)二次规划的一般模型：

经过步骤S2，航天器姿态机动模型已经转化为具有非凸二次约束的非齐次二次规划模型(26)，给出二次规划的一般形式为：

一般形式的性能函数：

一般形式的约束：

其中x∈R^m为列向量,Q₀,Q_Ei,Q_Ij∈R^m×m为对称矩阵，b₀,b_Ei,b_Ij∈R^m为列向量，c_Ei,c_Ej∈R为常数；若一般形式的性能函数(27)或一般形式的约束(28)(29)为线性则对称矩阵Q_l,l＝0,Ei,Ij为零矩阵，若向量b_l,l＝0,Ei,Ej为零向量，则一般形式的性能函数或约束为齐次方程。

(2)将一般的二次规划模型转化为齐次的二次规划模型：

齐次形式的性能函数：

齐次形式的约束：

α²＝1 (33)

其中α为常变量，且定义 为新的系数矩阵。

(3)采用半定松弛法对齐次的二次规划模型进行松弛，得到半定规划模型：

给出松弛处理后的性能函数：

J₀＝mintr(Q'₀X) (34)

松弛处理后的约束：

tr(Q'_EiX)＝c_Ei, i＝1,...,p+1 (35)

tr(Q'_IjX)≤c_Ij, j＝1,...,q (36)

其中tr(·)代表矩阵的迹，X∈Rⁿ(n＝m+1)为新引入的变量，且等式约束(33)被包含进了约束(35)中，为第p+1个等式约束。

由于松弛处理引入的等式约束为非凸且非线性约束，其等价于：

X≥0 (38)

rank(X)＝1 (39)

其中rank(·)代表矩阵的秩，(·)≥0代表矩阵为半正定矩阵。

通过上述转化，齐次的二次规划模型转化为了带有秩约束的半定规划模型。

所述步骤S4中，

由秩约束(39)仍为非凸约束，采用罚函数的方法对步骤S3中得到的半定规划模型进行转化：

引入秩惩罚后的性能函数：

J″＝mintr(Q'₀X)+γ·rank(X) (40)

引入秩惩罚后的约束：

tr(Q'_EiX)＝c_Ei, i＝1,...,p+1 (41)

tr(Q'_IjX)≤c_Ij, j＝1,...,q (42)

X≥0 (43)

其中γ为惩罚系数，其范围为[100，+∞)。

对于半正定矩阵，其特征值大于零或等于0，其秩为其大于零的特征值的数目。

引入函数ρ(z)＝1-e^-zσ，其中σ为极小的常数，取值范围为(0，0_.5]，称为替代系数；当z＝0时，ρ(z)＝0；当z＞0时，ρ(z)＝1，则矩阵X的秩近似地表示为其中λ_i,i＝1...n为矩阵X的特征值。

rank′(X)仍为非凸函数，其梯度为：

其中U∈R^n×n为矩阵X的特征向量所组成的矩阵；

由于其非凸性，得到：

rank′(X)≤rank′(X_k)+tr(rank′(X_k)·(X-X_k))＝r(X,X_k) (45)

其中X_k为第k次迭代的解；

故通过罚函数进行转化后的性能函数为：

J″′＝mintr(Q'₀X)+γ·r(X,X_k) (46)

带有秩约束的半定规划模型被转化为了带有秩惩罚的半定规划模型。

所述步骤S5中具体实现如下：

根据上述步骤，首先将航天器姿态机动的二次规划模型(26)按照步骤S4进行转化，得到两个模型：

模型1：J″′＝mintr(Q'₀X)+γ·tr(X)

tr(Q'_EiX)＝c_Ei,i＝1,...,p+1，tr(Q'_IjX)≤c_Ij,j＝1,...,q，X≥0

模型2：J″′＝mintr(Q'₀X)+γ·r(X,X_k)

tr(Q'_EiX)＝c_Ei,i＝1,...,p+1，tr(Q'_IjX)≤c_Ij,j＝1,...,q，X≥0

给出如下参数：初始惩罚系数γ＝γ₀；初始替代系数σ＝σ₀；迭代停止参数1ζ₁，取值范围为[0.001，0.05]；迭代停止参数2ζ₂，取值范围为[0.001，0.05]；迭代停止参数3κ₁，取值范围为[4，8]；迭代停止参数4κ₂，数值为2；迭代停止参数5ε，数值为0.01；迭代停止参数1是为了保证精度又使得求解时间不过长，参数5是在解决航天器姿态机动问题时应该达到的精度，参数3和4也均是仿真中测试得到的适合该求解方法的参数；

首先求解模型1,得到航天器姿态机动的初始路径；接着求解模型2，得到其解，判断这个解与上一个解的差的F范数，直到其小于所设置的迭代停止参数1ζ₁，若没有小于ζ₁则再次求解模型2，进行循环；设置σ＝σ/κ₁，再次进行上述循环，并比较前后两次循环得到的解的差的F范数，直到其小于所设置的迭代停止参数2ζ₂；完成上述两组循环后，求解所得到的矩阵X的特征值，判断其第二大的特征值是否小于迭代停止参数5ε，如果小于该值则解为最终解，如果不小于该值，则增大惩罚系数使得γ＝κ₂·γ，再次进行循环，直到得到最终解，即航天器姿态机动的最优路径。

本发明与现有技术相比的有益效果在于：

(1)大部分现有的姿态机动方法仅能得到可行解，现有方法得到的是可行解，而本发明的方法可以得到的是最优解且无需提供初始路径。

(2)大部分现有方法即使能加入性能函数也大都只能是单一形式的，而本发明的性能函数形式多样，即可以设置为时间最优、能量最优、角速度最优，可以按照需求选择。

(3)大部分现有方法难以增加约束的种类和个数，而本发明所采用的航天器姿态机动路径规划方法，便于约束的扩展。

附图说明

图1为本发明实施例的基于多项式规划的航天器姿态机动路径规划方法流程图；

图2为本发明实施例的航天器姿态机动路径规划示意图；

图3为本发明是实施的航天器姿态机动路径规划三维路径示意图；

图4为本发明是实施的航天器姿态机动路径规划二维路径示意图；

图5为本发明是实施的航天器姿态机动路径规划四元数示意图；

图6为本发明是实施的航天器姿态机动路径规划控制力矩示意图；

图7为本发明是实施的航天器姿态机动路径规划角速度示意图。

具体实施方式

下面将结合附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

如图1所示，本发明的方法具体包括如下步骤：

S1：将四元数的航天器姿态的运动学和动力学模型，航天器姿态的饱和约束、姿态禁区约束、姿态强制区约束以及初末状态约束的数学模型，及综合了时间、力矩和角速度的组合性能函数复合，得到航天器姿态机动的数学模型。具体过程如下：

(1)航天器姿态的运动学和动力学模型：

航天器姿态描述了航天器本体坐标系和地心惯性系之间的旋转关系，本方法采用单位四元数来描述航天器姿态，首先给出四元数的定义：

q＝[q₁,q₂,q₃,q₄]^T∈R⁴ (1)

特别地，四元数的二范数为1，且该范数为1的约束被包含在基于四元数的航天器姿态运动学模型中，范数为1的约束可以描述为如下形式：

定义航天器本体系相对地心惯性系的角速度为ω＝[ω₁,ω₂,ω₃]^T∈R³，则基于四元数的航天器姿态运动学模型为：

航天器本体系相对地心惯性系的角速度向量为ω＝[ω₁,ω₂,ω₃]^T∈R³，ω₁,ω₂,ω₃分别为航天器本体系相对地心惯性系的三轴角速度，其中

航天器的姿态动力学模型为：

其中为向量ω＝[ω₁,ω₂,ω₃]^T∈R³的斜对称矩阵；表示航天器相对于本体系的主轴转动惯量矩阵，J₁,J₂,J₃分别表示航天器相对于本体轴x,y,z的转动惯量；u＝[u₁,u₂,u₃]^T∈R³表示航天器在三轴方向的控制输入力矩向量，u₁,u₂,u₃分别表示航天器在x,y,z方向的控制输入力矩。

(2)饱和约束的数学模型为：

由于航天器执行机构的输出存在上限，故存在控制力矩饱和：

|u|≤u_max (5)

其中u_max为执行机构能输出的力矩的最大值。

除此之外，航天器的测量元件存在测量上限，故航天器的转动角速度也存在饱和：

|ω|≤ω_max (6)

其中ω_max为航天器测量元件所能测量的最大角速度；

(3)姿态禁区约束和强制区约束：

由于航天器携带的载荷中可能存在光敏和热敏元件，不能指向太阳光，则这类元件所在的视轴不能进入指定区域，即姿态禁区约束：

其中为地心惯性系下的约束单位矢量，在姿态禁区约束中指向规避物体，/>表示航天器本体系下航天器的视轴单位矢量。β_m为约束矢量与视轴矢量之间允许存在的最小夹角，被称为约束角。/>为航天器本体系到惯性系的坐标转换矩阵。

经过化简后可以得到姿态禁区约束的数学模型为：

q^TM_fq≤0 (8)

其中

姿态强制区是指航天器的视轴必须保持在约束区之内，例如航天器所携带的太阳能板必须对准太阳。与姿态禁区的数学模型类似，下面给出了姿态强制区的数学模型：

q^TM_mq≥0 (9)

其中β_M为约束矢量与视轴矢量之间允许存在的最大夹角。

(4)航天器姿态的初末状态约束为：

其中，t₀表示初始时刻，t_f表示姿态机动至期望姿态的时刻；ω₀表示航天器的初始角速度，ω_f表示航天器的末端角速度；q₀表示航天器的初始姿态，q_f表示航天器的期望姿态。

(5)性能函数为组合性能函数，其数学描述为：

其中第一项表示机动时间最优，第二项表示消耗能量最优，第三项表示角速度最优，所得到的性能函数为其组合形式。

航天器姿态机动的数学模型为以上五个模型之和。

S2：对步骤1中得到的航天器姿态机动的数学模型进行离散化，得到多项式规划模型，再采用变量扩充的方法得到具有非凸二次约束的非齐次二次规划模型。具体过程如下：

(1)对于步骤S1中的航天器姿态机动模型进行离散处理：

本发明采用梯形离散的方法，首先对公式(3)进行离散得到：

其中N表示离散节点的数目，k表示变量的第k个离散节点，Δt表示离散时间步长。采用同样的方法对公式(4)进行离散得到：

对公式(2)进行离散处理得到：

对饱和约束(5)和(6)进行离散处理得到：

|u(k)|≤u_max， k＝1...N (15)

|ω(k)|≤ω_max，k＝1...N (16)

对姿态禁区约束(8)和姿态强制区约束(9)进行离散处理得到：

q(k)^TM_fq(k)≤0， k＝1...N (17)

q(k)^TM_mq(k)≥0， k＝1...N (18)

初末状态约束(10)转化为：

性能函数(11)转化为下面的形式：

其中J′为离散后的性能函数，N表示所设置的离散时间节点数目，表示离散时间步长，q(k)为离散后的四元数，ω(k)为离散后的角速度，u(k)为离散后的控制力矩。

经过离散处理，得到了多项式规划模型；

(2)对多项式规划模型进行转化：

为了将多项式规划模型转化为二次规划模型，引入如下新变量：

则公式(12)和(13)变为：

公式(20)变为：

定义向量z(k)＝[u(k)^T,u′(k)^T,ω(k)^T,ω′(k)^T,q(k)^T]^T，则离散后模型的状态变量可以表示为：

z＝[z(0)^T,...,z(k)^T,...,z(N)^T]^T∈R^m (25)

则多项式规划模型转化为下面的二次规划模型：

其中M_J∈R^m×m表示性能函数的系数矩阵，M_Ei∈R^m×m,p_Ei∈R^m,q_Ei∈R表示等式约束的系数矩阵，M_Ij∈R^m×m,p_Ij∈R^m,q_Ij∈R表示不等式约束的系数矩阵，特别地，M_J,M_Ei,M_Ij为对称矩阵，r表示等式约束的数目，s表示不等式约束的数目。

特别地，该模型为具有非凸二次约束的非齐次二次规划模型。

S3：将步骤2中得到的具有非凸二次约束的非齐次二次规划模型转化为齐次形式，再采用半定松弛的方法将非凸二次约束进行转化，得到带有秩约束的半定规划模型。具体过程如下：

(1)二次规划的一般模型：

经过步骤S2，航天器姿态机动模型已经转化为具有非凸二次约束的非齐次二次规划模型(26)，给出二次规划的一般形式为：

一般形式的性能函数：

一般形式的约束：

其中x∈R^m为列向量,Q₀,Q_Ei,Q_Ij∈R^m×m为对称矩阵，b₀,b_Ei,b_Ij∈R^m为列向量，c_Ei,c_Ej∈R为常数。若一般形式的性能函数(27)或一般形式的约束(28)(29)为线性则对称矩阵Q_l,l＝0,Ei,Ij为零矩阵，若向量b_l,l＝0,Ei,Ej为零向量，则一般形式的性能函数或约束为齐次方程；

(2)将一般的二次规划模型转化为齐次的二次规划模型：

齐次形式的性能函数：

齐次形式的约束：

α²＝1 (33)

其中α为常变量。且定义 为新的系数矩阵。

(3)采用半定松弛法对齐次的二次规划模型进行松弛，得到半定规划模型：

给出松弛处理后的性能函数：

J₀＝mintr(Q'₀X) (34)

松弛处理后的约束：

tr(Q'_EiX)＝c_Ei, i＝1,...,p+1 (35)

tr(Q'_IjX)≤c_Ij, j＝1,...,q (36)

其中tr(·)代表矩阵的迹，X∈Rⁿ(n＝m+1)为新引入的变量。且等式约束(33)被包含进了约束(35)中，为第p+1个等式约束。

由于松弛处理引入的约束为非凸且非线性约束，其等价于：

X≥0 (38)

rank(X)＝1 (39)

其中rank(·)代表矩阵的秩，(·)≥0代表矩阵为半正定矩阵。

通过上述转化，齐次的二次规划模型转化为了带有秩约束的半定规划模型。

S4：采用罚函数方法将步骤S3中得到的带有秩约束的半定规划模型中的秩约束转化到性能函数中，得到带有秩惩罚的半定规划模型。具体实现如下：

由于秩约束(39)仍为非凸约束，采用罚函数的方法对步骤S3中得到的半定规划模型进行转化：

引入秩惩罚后的性能函数：

J″＝mintr(Q'₀X)+γ·rank(X) (40)

引入秩惩罚后的约束：

tr(Q'_EiX)＝c_Ei, i＝1,...,p+1 (41)

tr(Q'_IjX)≤c_Ij, j＝1,...,q (42)

X≥0 (43)

其中γ为惩罚系数，其范围为[100，+∞)。

对于半正定矩阵，其特征值大于零或等于0，其秩为其大于零的特征值的数目。

引入函数ρ(z)＝1-e^-z/σ，其中σ为极小的常数，取值范围为(0，0_.5]，在本发明中称为替代系数。当z＝0时，ρ(z)＝0；当z＞0时，ρ(z)＝1。则矩阵X的秩可以近似地表示为其中λ_i,i＝1...n为矩阵X的特征值。

但由于rank′(X)仍为非凸函数，其梯度为：

其中U∈R^n×n为矩阵X的特征向量所组成的矩阵。

由于其非凸性，可以得到：

rank′(X)≤rank′(X_k)+tr(rank′(X_k)·(X-X_k))＝r(X,X_k) (45)

其中X_k为第k次迭代的解。

通过在性能函数中引入秩惩罚项r(X,X_k)，将性能函数转化为下面的形式：

J″′＝mintr(Q'₀X)+γ·r(X,X_k) (46)

通过上述转化，带有秩约束的半定规划模型被转化为了带有秩惩罚的半定规划模型。

S5：采用逐次迭代的求解策略求解步骤S4中得到的带有秩惩罚的半定规划模型，最终得到航天器姿态机动的最优路径。具体实现如下：

根据上述步骤，首先给出两个模型：

模型1：J″′＝mintr(Q'₀X)+γ·tr(X)

tr(Q_i'X)≤c_i,i＝1,...p，tr(Q'_jX)＝c_j,j＝1,...q,q+1，X≥0

模型2：J″′＝mintr(Q'₀X)+γ·r(X,X_k)

tr(Q_i'X)≤c_i,i＝1,...p，tr(Q'_jX)＝c_j,j＝1,...q,q+1，X≥0

给出如下参数：初始惩罚系数γ＝γ₀；初始替代系数σ＝σ₀；迭代停止参数1ζ₁，取值范围为[0.001，0.05]；迭代停止参数2ζ₂，取值范围为[0.001，0.05]；迭代停止参数3κ₁，取值范围为[4，8]；迭代停止参数4κ₂，数值为2；迭代停止参数5ε，数值为0.01。

首先求解模型1,得到航天器姿态机动的初始路径；接着求解模型2，得到其解，判断该解与上一次得到解的差的F范数，直到其小于所设置的迭代停止参数1ζ₁，若未小于ζ₁则再次求解模型2，进行循环；减小替代系数使得矩阵的近似秩与矩阵的实际秩更为接近，即设置σ＝σ/κ₁，再次进行上述循环，并比较前后两次循环得到的解的差的F范数，直到其小于所设置的迭代停止参数2ζ₂。完成上述两组循环后，求解所得到的矩阵X的特征值，判断其第二大的特征值是否小于迭代停止参数5ε，如果小于该值则得到的解就为最终解，如果不小于该值，则增大惩罚系数，使得γ＝κ₂·γ，再次进行循环，直到得到最终解，即航天器姿态机动的最优路径。

本发明中所有变量上标“·”都是该变量的导数，除非该变量的导数有实际物理含义。

下面以考虑了三个姿态禁区，一个姿态强制区的航天器姿态机动场景为例，所选用的性能函数为能量最优。定义姿态机动时间为60s，航天器的转动惯量矩阵为离散时间节点N＝40。姿态禁区的约束向量和约束角分别为：w₁＝[0；-1；0]，θ₁＝40deg；w₂＝[0；0.8192；0.5736],θ₂＝30deg；w₃＝[-0.1220；-0.1397；-0.9827],θ₃＝10deg，姿态强制区的约束向量和约束角为：w₄＝[-0.8138；0.5483；-0.1926]，θ₄＝55deg。航天器的初始姿态为[0.81744；0.51592；-0.11618；-0.22831]，期望姿态为[0.27536；-0.50637；-0.78252；-0.23542]。控制力矩饱和为u_max＝0.3，角速度饱和为ω_max＝0.3；姿态禁止向量为[0；1；0]，姿态强制向量为[0；0；1]。

图2表示的是航天器姿态机动路径规划任务的示意图，图3为航天器的三维机动示意图，图4为航天器的二维示意图，可以看出航天器的姿态禁止向量轴避开了三个姿态禁区，航天器的强制向量轴始终在姿态强制区内。图5为航天器的四元数示意图，可以看出航天器从初始姿态到达了期望姿态，说明本发明提出的方法可以完成姿态机动任务，到达期望姿态。图6为航天器的三轴力矩，可以看出其数值小于所设置的上限，说明本发明提出的方法满足控制力矩饱和约束。图7为航天器的三轴角速度，其数值也小于所设置的角速度上限，说明本发明提出的方法满足角速度饱和约束。

综上，本发明所提出的基于多项式规划的航天器姿态机动路径规划方法可以使得航天器机动到期望姿态的同时满足各类约束，且所得到的路径为最优路径。

Claims (4)

1.一种基于多项式规划的航天器姿态机动路径规划方法，其特征在于，包括如下步骤：

S1：将四元数的航天器姿态的运动学和动力学模型，航天器姿态的饱和约束、姿态禁区约束、姿态强制区约束以及初末状态约束的数学模型，及综合了时间、力矩和角速度的组合性能函数复合，得到航天器姿态机动的数学模型；

S2：对步骤S1中得到的航天器姿态机动的数学模型进行离散化，得到多项式规划模型，再采用变量扩充的方法得到具有非凸二次约束的非齐次二次规划模型；

S3：将步骤S2中得到的具有非凸二次约束的非齐次二次规划模型转化为齐次形式，再采用半定松弛的方法将非凸二次约束进行转化，得到带有秩约束的半定规划模型；

S4：采用罚函数方法将步骤S3中得到的带有秩约束的半定规划模型中的秩约束转化到性能函数中，得到带有秩惩罚的半定规划模型；

S5：采用逐次迭代的求解策略求解步骤S4中得到的带有秩惩罚的半定规划模型，最终得到航天器姿态机动的最优路径；

所述步骤S1具体实现为：

(1)基于四元数的航天器姿态的运动学和动力学模型如下：

基于四元数的航天器姿态运动学模型为：

四元数：

q＝[q₁,q₂,q₃,q₄]^T∈R⁴ (2)

且四元数的二范数为1，且该范数为1的约束被包含在基于四元数的航天器姿态运动学模型中，范数为1的约束可以描述为如下形式：

航天器本体系相对地心惯性系的角速度向量为ω＝[ω₁,ω₂,ω₃]^T∈R³，ω₁,ω₂,ω₃分别为航天器本体系相对地心惯性系的三轴角速度，其中

航天器的姿态动力学模型为：

其中为向量ω＝[ω₁,ω₂,ω₃]^T∈R³的斜对称矩阵；表示航天器相对于本体系的主轴转动惯量矩阵，J₁,J₂,J₃分别表示航天器相对于本体轴x,y,z的转动惯量，u＝[u₁,u₂,u₃]^T∈R³表示航天器在三轴方向的控制输入力矩向量，u₁,u₂,u₃分别表示航天器在x,y,z方向的控制输入力矩；

(2)饱和约束数学模型为：

航天器执行机构的输出存在上限，故存在控制力矩饱和约束：

|u|≤u_max (5)

其中u_max为执行机构能输出的力矩的最大值；

此外，航天器测量元件存在测量上限，故航天器的转动角速度也存在饱和约束：

|ω|≤ω_max (6)

其中ω_max为航天器测量元件所能测量的最大角速度；

(3)姿态禁区约束和姿态强制区约束的数学模型为：

航天器携带的载荷中存在光敏和热敏元件，不能指向太阳光，则这类元件所在的视轴不能进入指定区域，即姿态禁区约束：

其中为地心惯性系下的约束单位矢量，在姿态禁区约束中指向规避物体，/>表示航天器本体系下航天器的视轴单位矢量，β_m为约束矢量与视轴矢量之间允许存在的最小夹角，被称为约束角；/>为航天器本体系到惯性系的坐标转换矩阵；

经过化简后得到姿态禁区约束的数学模型为：

q^TM_fq≤0 (8)

其中

同理，姿态强制区的数学模型为：

q^TM_mq≥0 (9)

其中β_M为约束矢量与视轴矢量之间允许存在的最大夹角；

(4)初末状态约束的数学模型为：

其中，t₀表示初始时刻，t_f表示姿态机动至期望姿态的时刻；ω₀表示航天器的初始角速度，ω_f表示航天器的末端角速度；q₀表示航天器的初始姿态，q_f表示航天器的期望姿态；

(5)综合了时间、力矩和角速度的组合性能函数，其数学描述为：

其中第一项表示机动时间最优，第二项表示消耗能量最优，第三项表示角速度最优；

航天器姿态机动的数学模型为以上(1)-(5)的五个模型之和；

所述步骤S2具体实现如下：

(1)对于步骤S1中的航天器姿态机动的数学模型进行离散化处理：

采用梯形离散的方法，首先对公式(1)进行离散得到：

其中N表示离散节点的数目，k表示变量的第k个离散节点，△t表示离散时间步长；

采用同样的方法对公式(4)进行离散得到：

对公式(3)进行离散处理得到：

对(5)和(6)进行离散处理得到：

|u(k)|≤u_max，k＝1...N (15)

|ω(k)|≤ω_max，k＝1...N (16)

对公式(8)和(9)进行离散处理得到：

q(k)^TM_fq(k)≤0， k＝1...N (17)

q(k)^TM_mq(k)≥0，k＝1...N (18)

公式(10)转化为：

组合性能函数的公式(11)转化为：

其中J′为离散后的性能函数，N表示所设置的离散时间节点数目，表示离散时间步长，q(k)为离散后的四元数，ω(k)为离散后的角速度，u(k)为离散后的控制力矩；

(2)对多项式规划模型进行转化：

为了将多项式规划模型转化为二次规划模型，引入如下新变量：

则公式(12)和(13)变为：

公式(20)变为：

向量z(k)＝[u(k)^T,u′(k)^T,ω(k)^T,ω′(k)^T,q(k)^T]^T，则离散后的模型的状态变量表示为：

z＝[z(0)^T,...,z(k)^T,...,z(N)^T]^T∈R^m (25)

则多项式规划模型转化为下面的具有非凸二次约束的非齐次二次规划模型：

其中M_J∈R^m×m表示性能函数的系数矩阵，M_Ei∈R^m×m,p_Ei∈R^m,q_Ei∈R表示等式约束的系数矩阵，M_Ij∈R^m×m,p_Ij∈R^m,q_Ij∈R表示不等式约束的系数矩阵，M_J,M_Ei,M_Ij为对称矩阵，r表示等式约束的数目，s表示不等式约束的数目。

2.根据权利要求1所述的方法，其特征在于：所述步骤S3中，

(1)二次规划的一般模型：

经过步骤S2，航天器姿态机动模型已经转化为具有非凸二次约束的非齐次二次规划模型(26)，给出二次规划的一般形式为：

一般形式的性能函数：

一般形式的约束：

其中x∈R^m为列向量,Q₀,Q_Ei,Q_Ij∈R^m×m为对称矩阵，b₀,b_Ei,b_Ij∈R^m为列向量，c_Ei,c_Ej∈R为常数；若一般形式的性能函数(27)或一般形式的约束(28)(29)为线性则对称矩阵Q_l,l＝0,Ei,Ij为零矩阵，若向量b_l,l＝0,Ei,Ej为零向量，则一般形式的性能函数或约束为齐次方程；

(2)将一般的二次规划模型转化为齐次的二次规划模型：

齐次形式的性能函数：

齐次形式的约束：

α²＝1 (33)

其中α为常变量，且定义 为新的系数矩阵；

(3)采用半定松弛法对齐次的二次规划模型进行松弛，得到半定规划模型：

给出松弛处理后的性能函数：

J₀＝mintr(Q'₀X) (34)

松弛处理后的约束：

tr(Q'_EiX)＝c_Ei,i＝1,...,p+1 (35)

tr(Q'_IjX)≤c_Ij,j＝1,...,q (36)

其中tr(·)代表矩阵的迹，X∈Rⁿ(n＝m+1)为新引入的变量，且等式约束(33)被包含进了约束(35)中，为第p+1个等式约束；

由于松弛处理引入的等式约束为非凸且非线性约束，其等价于：

X≥0 (38)

rank(X)＝1 (39)

其中rank(·)代表矩阵的秩，(·)≥0代表矩阵为半正定矩阵；

通过上述转化，齐次的二次规划模型转化为了带有秩约束的半定规划模型。

3.根据权利要求1所述的方法，其特征在于，所述步骤S4中，

由秩约束(39)仍为非凸约束，采用罚函数的方法对步骤S3中得到的半定规划模型进行转化：

引入秩惩罚后的性能函数：

J″＝mintr(Q'₀X)+γ·rank(X) (40)

引入秩惩罚后的约束：

tr(Q'_EiX)＝c_Ei,i＝1,...,p+1 (41)

tr(Q'_IjX)≤c_Ij,j＝1,...,q (42)

X≥0 (43)

其中γ为惩罚系数，其范围为[100，+∞)；

对于半正定矩阵，其特征值大于零或等于0，其秩为其大于零的特征值的数目；

引入函数ρ(z)＝1-e^-zσ，其中σ为极小的常数，取值范围为(0，0.5]，称为替代系数；当z＝0时，ρ(z)＝0；当z>0时，ρ(z)＝1，则矩阵X的秩近似地表示为其中λ_i,i＝1...n为矩阵X的特征值；

rank′(X)仍为非凸函数，其梯度为：

其中U∈R^n×n为矩阵X的特征向量所组成的矩阵；

由于其非凸性，得到：

rank′(X)≤rank′(X_k)+tr(rank′(X_k)·(X-X_k))＝r(X,X_k) (45)

其中X_k为第k次迭代的解；

故通过罚函数进行转化后的性能函数为：

J″′＝mintr(Q'₀X)+γ·r(X,X_k) (46)

带有秩约束的半定规划模型被转化为了带有秩惩罚的半定规划模型。

4.根据权利要求1所述的方法，其特征在于，所述步骤S5中具体实现如下：

根据上述步骤，首先将航天器姿态机动的二次规划模型(26)按照步骤S4进行转化，得到两个模型：

模型1：J″′＝mintr(Q'₀X)+γ·tr(X)

tr(Q'_EiX)＝c_Ei,i＝1,...,p+1，tr(Q'_IjX)≤c_Ij,j＝1,...,q，X≥0

模型2：J″′＝min tr(Q'₀X)+γ·r(X,X_k)

tr(Q'_EiX)＝c_Ei,i＝1,...,p+1，tr(Q'_IjX)≤c_Ij,j＝1,...,q，X≥0

给出如下参数：初始惩罚系数γ＝γ₀；初始替代系数σ＝σ₀；迭代停止参数1ζ₁，取值范围为[0.001，0.05]；迭代停止参数2ζ₂，取值范围为[0.001，0.05]；迭代停止参数3κ₁，取值范围为[4，8]；迭代停止参数4κ₂，数值为2；迭代停止参数5ε，数值为0.01；

首先求解模型1,得到航天器姿态机动的初始路径；接着求解模型2，得到其解，判断这个解与上一个解的差的F范数，直到其小于所设置的迭代停止参数1ζ₁，若没有小于ζ₁则再次求解模型2，进行循环；设置σ＝σ/κ₁，再次进行上述循环，并比较前后两次循环得到的解的差的F范数，直到其小于所设置的迭代停止参数2ζ₂；完成上述两组循环后，求解所得到的矩阵X的特征值，判断其第二大的特征值是否小于迭代停止参数5ε，如果小于该值则解为最终解，如果不小于该值，则增大惩罚系数使得γ＝κ₂·γ，再次进行循环，直到得到最终解，即航天器姿态机动的最优路径。