基于二阶锥优化的多约束下航天器姿态机动路径规划方法

本发明属于航天器姿态机动领域，具体来说，涉及一种基于二阶锥优化的多约束下航天器姿态机动路径规划方法。

背景技术：

随着航天技术的发展，空间任务也越来越复杂，航天器姿态控制系统是实现空间任务的关键系统，需要具有较高的精度和可靠性，故其发展也面临着巨大的的挑战。例如热带降雨测量任务卫星，凌日系外行星勘测卫星，锁眼系列侦察卫星，高分系列卫星，以及其他用于完成各类空间任务的卫星。为了完成着这些任务，姿态控制系统需要具有较好的性能和较高的可靠性。

航天器姿态机动指在轨航天器从一个姿态转动到另一个姿态的过程。对于传统的无约束航天器姿态机动问题，常见的控制方法有：鲁棒控制、滑模控制、自适应控制、李雅普诺夫直接法，以及各类方法的混合控制方法。但是在航天器实际完成空间任务的过程中，必须考虑存在的各类约束，例如力矩饱和约束，姿态禁止区约束。对于多约束下航天器姿态机动的路径规划问题，也已经有了一定的研究，常见的方法有势函数法、a^*算法、模型预测控制算法等等。但是这些方法难以保证最优性，限制了其应用范围。

这些方法无法同时考虑本发明中考虑的多种约束，并且无法保证得到的路径为最优路径。

技术实现要素：

本发明技术解决问题：克服现有技术的不足，提供一种基于二阶锥优化的多约束下航天器姿态机动路径规划方法，同时考虑动力学与运动学约束，角速度饱和约束，力矩饱和约束和姿态禁区约束，且所得到的结果为全局最优路径；同时本发明求解速度快，计算效率高，也便于增加约束的数目和种类。

本发明技术解决方案：一种基于二阶锥优化的多约束下航天器姿态机动路径规划方法，包括如下步骤：

s1：利用基于欧拉角的航天器姿态机动模型，采用变量代换的方法将所述模型进行变量转化得到转化后的模型，由此引入新的等式约束且因为变量代换带来新的等式约束，再对转化后的模型进行线性化得到线性模型；

s2：基于步骤s1中得到的线性模型，确定初末状态约束，饱和约束，姿态禁区约束，置信区间约束的数学模型，从而得到多约束下航天器姿态机动的数学模型；

s3：将步骤s1中因为变量代换带来的新的等式约束进行松弛处理为不等式约束，将s2中的姿态禁区约束数学模型进行凸化处理，再通过确定不等式约束避免奇异；

s4：设计代价函数保证步骤s3中的松弛处理和凸化处理不改变航天器姿态机动的路径，得到最终转化后的数学模型；

s5：将步骤s4中得到的数学模型进行离散化，得到能够利用二阶锥优化方法求解的数学模型，再利用二阶锥优化方法进行迭代求解得到航天器姿态机动路径。

所述步骤s1具体实现为：

(1)基于欧拉角的航天器姿态机动模型包括基于欧拉角的航天器姿态机动运动学模型和刚性航天器的姿态动力学模型。其中：

基于欧拉角的航天器姿态机动运动学模型：

其中，表示滚转角，θ表示俯仰角，ψ表示偏航角，ωx,ωy,ωz为转速在航天器本体系各个坐标轴上的分量；

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

其中，ix,iy,iz为航天器绕各个坐标轴的转动惯量，mx,my,mz为执行机构提供的力矩在航天器本体系各个坐标轴上的分量；

(2)线性化得到线性模型

对(1)式进行变量代换：

其中u21＝sinθ,u22＝cosθ,u31＝sinψ,u32＝cosψ；

则在转化后的模型中，状态变量为x＝[u12u22u32ωxωyωz]，控制输入为ν＝[u11u21u31mxmymz],令(x^k,ν^k)代表第k次迭代的解，对方程(2),(3)进行线性化，得到下面的线性模型：

其中b22＝(-u31ωx-u32ωy)^k，b23＝(-u21ωx)^k，c2＝(2u21u31ωx+2u21u32ωy)^k，

由变量代换引起的约束如下：

a为6*6系统矩阵，b为6*6输入矩阵，c为6*1常数矩阵，a12,a13,a14,a15,a23,a24,a25,a32,a33,a34,a35,a36,a45,a46,a54,a56,a64,a65,b11,b13,b22,b23,b32,b33,b44,b55,b66,c1,c2,c3,c4,c5,c6为通过上述公式计算出的具体数值。

所述步骤2中，

(1)航天器的初末状态约束模型为：

其中，t0表示初始时刻，tf表示机动结束时刻，x0表示航天器初始姿态和初始角速度，xf表示航天器末端姿态和末端角速度，ν0表示初始控制输入，νf表示末端控制输入；

(2)饱和约束数学模型包括角速度饱和约束的数学模型和力矩饱和约束的数学模型，其中：

角速度饱和约束的数学模型如下：

-ωmax≤ωj≤ωmax,j＝x,y,z(7)

其中，ωmax为角速度的最大值。

力矩饱和约束的数学模型如下：

-mmax≤mj≤mmax,j＝x,y,z(8)

其中，mmax为力矩的最大值；

(3)如果敏感元件装在航天器z轴，姿态禁区约束的数学模型为：

x0u21-y0u11u22+z0u12u22≤cosβ(9)

其中，(x0,y0,z0)为姿态禁区圆锥轴与单位球的交点在惯性坐标系下的坐标，β为姿态禁区圆锥半角；

u21＝sinθ,u22＝cosθ,u31＝sinψ,u32＝cosψ；

(4)为了使得到的航天器姿态机动路径更为准确，设置置信区间约束模型如下：

其中为极小的正常数，即0.01～0.02；

多约束下航天器姿态机动的数学模型包括上述四个模型之和。

所述步骤s3中，

(1)凸化处理如下：

将由于变量代换带来的等式约束(5)进行松弛处理，得到下面的不等式约束：

对于姿态禁区约束(9)，通过泰勒展开进行凸化处理，得到：

为了确保航天器能够绕过姿态禁区，对凸化后的姿态禁区约束(11)进行如下处理：

其中δ为较小的正常数，0.1～0.5；

(2)松弛处理如下：

为了保证航天器能够找到可行的路径，根据l1罚函数方法，在姿态禁区约束(13)中引入松弛变量μ1≥0,μ2≥0：

由于采用欧拉角描述航天器姿态运动时存在奇异，即当cosθ＝0，为了避免奇异，进行如下处理：

-sin(π/2-σ)≤u21≤sin(π/2-σ)(15)

其中σ为极小的正常数，即0.01～0.02。

所述步骤s4中，

(1)设计的代价函数如下：

其中，ε1,ε2,ε3,ε4为表示权重的非负系数，在0.1～100取值；

(2)转化后的模型p1由代价函数和约束组成，且要在满足约束的条件下使得代价函数最小：

其中代价函数为：

约束为：

-ωmax≤ωj≤ωmax,j＝x,y,z(7)

-mmax≤mj≤mmax,j＝x,y,z(8)

μ1≥0,μ2≥0(14-1)

-sin(π/2-σ)≤u21≤sin(π/2-σ)(15)

所述步骤s5中，得到的数学模型进行离散化，得到能够利用二阶锥优化方法的求解的数学模型具体实现如下：

(1)离散后的数学模型包括代价函数和约束，且要在满足约束的条件下使得代价函数最小：

代价函数为：

约束为：

x1＝x0,xn+1＝xf,ν1＝ν0,νn+1＝νf(19)

-ωmax≤ωxi≤ωmax,-ωmax≤ωyi≤ωmax,-ωmax≤ωzi≤ωmax(20)

-mmax≤mxi≤mmax,-mmax≤myi≤mmax,-mmax≤mzi≤mmax(21)

μ1i≥0,μ2i≥0(25)

-sin(π/2-σ)≤u21i≤sin(π/2-σ)(26)

i＝1,...,n(27)

其中，离散时间步长为n+1为所设置的离散时间点，上标k表示迭代次数，下标i表示第i个离散点；

(2)基于二阶锥优化的逐次迭代策略：首先当k＝1时，在不考虑姿态禁区的情况下采用pd控制器得到航天器的第一条机动路径；当k＝2时，将中的代入k-1条路径中对应的值，求解得到第k条路径；当k≥3时，将中的代入k-1条路径中对应的值，并设置置信区间ζ为极小值，即0.01～0.02，求解得到第k条路径，计算两条路径之间的误差并与设定值1进行对比，计算代价函数中松弛变量和的绝对值并与设定值2进行对比，如果上述两个对比结果均小于设定值则停止迭代；将k与所设定的最大迭代次数进行对比，若大于最大迭代次数则停止迭代，最终得到航天器姿态机动的路径。

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

(1)本发明中所设计的基于二阶锥优化的航天器姿态机动路径规划方法，可以给出基于所设计策略和代价函数的最优解，且不仅可以直接给出所有的状态量，还可以给出控制输入，避免了单独设计控制律。

(2)本发明所采用的航天器姿态机动路径规划方法，计算效率相较于其他路径规划方法明显较高，并且便于增加约束的数目，也便于增加约束的种类。

附图说明

图1为本发明实施例的基于二阶锥优化的多约束下航天器姿态机动路径规划方法流程图；

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

图3为本发明实施例路径规划方法的航天器姿态机动三维路径图；

图4为本发明实施例路径规划方法的航天器姿态机动二维视图；

图5为本发明实施例路径规划方法的控制力矩示意图；

图6为本发明实施例路径规划方法的角速度示意图。

具体实施方式

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

如图1所示，本发肯实施例的基于二阶锥优化的多约束航天器姿态机动路径规划方法，包括如下步骤：

s1：利用基于欧拉角的航天器姿态机动模型，采用变量代换的方法将所述模型进行变量转化得到转化后的模型，由此引入新的等式约束且因为变量代换带来新的等式约束，再对转化后的模型进行线性化得到线性模型。具体过程如下：

(1)基于欧拉角的航天器姿态机动模型包括基于欧拉角的航天器姿态机动运动学模型和刚性航天器的姿态动力学模型。其中：

基于欧拉角的航天器姿态机动运动学模型:

其中，表示滚转角，θ表示俯仰角，ψ表示偏航角，ωx,ωy,ωz为转速在航天器本体系各个坐标轴上的分量。

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

其中，ix,iy,iz为航天器绕各个坐标轴的转动惯量，mx,my,mz为执行机构提供的力矩在航天器本体系各个坐标轴上的分量。

(2)线性化得到线性模型：

对(1)式进行变量代换：

其中u21＝sinθ,u22＝cosθ,u31＝sinψ,u32＝cosψ。

则在转化后的模型中，状态变量为x＝[u12u22u32ωxωyωz]，控制输入为ν＝[u11u21u31mxmymz],令(x^k,ν^k)代表第k次迭代的解，则利用泰勒展开对模型进行线性化，得到：

其中b22＝(-u31ωx-u32ωy)^k，b23＝(-u21ωx)^k，c2＝(2u21u31ωx+2u21u32ωy)^k，

由变量代换引起的约束如下：

a为6*6系统矩阵，b为6*6输入矩阵，c为6*1常数矩阵，a12,a13,a14,a15,a23,a24,a25,a32,a33,a34,a35,a36,a45,a46,a54,a56,a64,a65,b11,b13,b22,b23,b32,b33,b44,b55,b66,c1,c2,c3,c4,c5,c6为通过上述公式计算出的具体数值。

s2：基于步骤s1中得到的线性模型，确定初末状态约束，饱和约束，姿态禁区约束，置信区间约束的数学模型，从而得到多约束下航天器姿态机动的数学模型。具体过程如下：

(1)航天器的初末状态约束模型为：

其中，t0表示初始时刻，tf表示机动结束时刻，x0表示航天器初始姿态和初始角速度，xf表示航天器末端姿态和末端角速度，ν0表示初始控制输入，νf表示末端控制输入。

(2)饱和约束数学模型包括角速度饱和约束的数学模型和力矩饱和约束的数学模型，其中：

航天器转动角速度饱和约束数学模型为：

-ωmax≤ωj≤ωmax,j＝x,y,z(7)

其中，ωmax为角速度的最大值。

控制力矩饱和约束数学模型为：

-mmax≤mj≤mmax,j＝x,y,z(8)

其中，mmax为力矩的最大值。

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

本体系到惯性系的坐标转换矩阵为：

其中c代表cos，s代表sin。

如果敏感元件装在航天器z轴，即其在航天器本体系下的坐标为[0,0,1]^t，则利用坐标转换矩阵将其转换到惯性后的坐标为即[u21,-u11u22,u12u22]^t。

则姿态禁区的数学模型为：

x0u21-y0u11u22+z0u12u22≤cosβ(9)

其中，(x0,y0,z0)为姿态禁区圆锥轴与单位球的交点在惯性坐标系下的坐标，β为姿态禁区圆锥半角。

(4)为了使得到的航天器姿态机动路径更为准确，设置置信区间约束模型如下：

多约束下航天器姿态机动的数学模型包括上述四个模型之和。

s3：将步骤s1中因为变量代换带来的新的等式约束进行松弛处理为不等式约束，将s2中的姿态禁区约束数学模型进行凸化处理，再通过确定不等式约束避免奇异。具体过程如下：

(1)凸化处理如下：二阶锥优化方法要求所有的等式约束均为线性形式，不等式约束为线性或二阶锥形式，故将约束(5)松弛处理为不等式约束：

将约束(9)通过泰勒展开进行凸化处理：

为了确保航天器能够绕过姿态禁区，对(11)进行如下处理：

其中δ为较小的正常数，0.1～0.5；

(2)松弛处理如下：

为了保证能够找到可行的航天器姿态机动路径，根据l1罚函数方法，可以在约束(12)中引入松弛变量μ1≥0,μ2≥0：

μ1≥0,μ2≥0(14-1)

由于采用欧拉角描述航天器姿态运动时存在奇异，即当cosθ＝0，为了避免奇异，进行如下处理：

-sin(π/2-σ)≤u21≤sin(π/2-σ)(15)

其中σ为极小的正常数，即0.01～0.02。

s4：设计代价函数保证步骤s3中的松弛处理和凸化处理不改变航天器姿态机动的路径，得到最终转化后的数学模型。

(1)设计的代价函数如下：

其中，ε1,ε2,ε3,ε4为表示权重的非负系数，在0.1～100取值；

(2)转化后的模型p1由代价函数和约束组成，且要在满足约束的条件下使得代价函数最小：

其中代价函数为：

约束为：

-ωmax≤ωj≤ωmax,j＝x,y,z(7)

-mmax≤mj≤mmax,j＝x,y,z(8)

μ1≥0,μ2≥0(14-1)

-sin(π/2-σ)≤u21≤sin(π/2-σ)(15)。

(3)等价性证明如下：

假设约束(11)为不起作用约束，即只取不等号，这可以通过设置的值来实现。

给出p的哈密顿方程如下：

p的拉格朗日方程为：

a)非平凡条件；

b)共态微分方程：

c)静止条件：

d)互补松弛条件：

λ10≥0,λ10μ1＝0(16-25)

λ11≥0,λ11μ2＝0(16-26)

如果存在时间间隔[ta,tb]∈[t0,tf]，使得约束(11)为不起作用约束，则根据(16-22)～(16-24)可得：

λ7＝λ8＝λ9＝0(16-29)

由(16-16)～(16-18)，可以得到：

由(16-19)～(16-21)，可以得到：

由于非负松弛变量μ1,μ2的引入，可以得到：

λ12＝0(16-32)

将上述公式带入共态微分方程和静止条件可得：

[p0p1p2p3p4p5p6]＝0(16-33)

上述分析表明[p0p1p2p3p4p5p6]＝0，与非平凡条件矛盾，故(11)为起作用约束。

s5：将得到的数学模型进行离散化，得到能够利用二阶锥优化方法的求解的数学模型。具体实现如下：

(1)离散后的数学模型包括代价函数和约束，且要在满足约束的条件下使得代价函数最小：

代价函数为：

约束为：

x1＝x0,xn+1＝xf,ν1＝ν0,νn+1＝νf(19)

-ωmax≤ωxi≤ωmax,-ωmax≤ωyi≤ωmax,-ωmax≤ωzi≤ωmax(20)

-mmax≤mxi≤mmax,-mmax≤myi≤mmax,-mmax≤mzi≤mmax(21)

μ1i≥0,μ2i≥0(25)

-sin(π/2-σ)≤u21i≤sin(π/2-σ)(26)

i＝1,...,n(27)

其中，离散时间步长为n+1为所设置的离散时间点，上标k表示迭代次数，下标i表示第i个离散点。

(2)基于二阶锥优化的逐次迭代策略：首先当k＝1时，在不考虑姿态禁区的情况下采用pd控制器得到航天器的第一条机动路径；当k＝2时，将中的代入k-1条路径中对应的值，求解得到第k条路径；当k≥3时，将中的代入k-1条路径中对应的值，并设置置信区间为极小值，即0.01～0.02，求解得到第k条路径，计算两条路径之间的误差并与设定值1进行对比，计算代价函数中松弛变量和的绝对值并与设定值2进行对比，如果上述两个对比结果均小于设定值则停止迭代；将k与所设定的最大迭代次数进行对比，若大于最大迭代次数则停止迭代，最终得到航天器姿态机动的路径。

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

下面以某航天器姿态机动路径规划场景为例，说明本发明所提出的方法的有效性。定义航天器的初始姿态指向为(-72.3646,0.0344,27.3817)deg，目标姿态指向为(0,0,0)deg，航天器机动时间为150s，场景中姿态禁区矢量为(-0.06898；0.3616；0.9298)和(0.2915；0.6194；0.7290)，姿态禁区圆锥的半锥角为11deg和15deg，航天器的角速度最大值为0.1rad/s，控制力矩的最大值为3n.m。在该方法下得到的航天器姿态机动三维路径如图3所示，其中由点划线组成的圆形为三维视图下的姿态禁区，虚线为三维初始机动路径，实线为本方法所求出的三维航天器姿态机动路径。二维视图如图4所示，其中点划线组成的椭圆表示二维视图下的姿态禁区，虚线为二维初始机动路径，实线为本方法所求出的二维航天器姿态机动路径。航天器的控制力矩mx,my,my如图5所示，角速度ωx,ωy,ωz如图6所示，其数值均小于所设置的上限值，上述结果说明本发明所设计的基于二阶锥优化方法的航天器姿态机动路径满足姿态禁区约束以及饱和约束。除此之外(11)也为起作用约束，即取等号，说明转化后模型与原模型等价。

由上述可以看出本发明所提出的基于二阶锥优化的航天器姿态机动路径规划方法有效的给出航天器的机动路径并满足所规定的约束，所得到的结果为全局最优路径；除此之外还可以直接给出航天器的控制输入，避免了额外设计跟踪控制律；而且计算效率明显高于其他路径规划方法，易于增加约束的数目。

对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以对本发明的实施例做出若干变型和改进，这些都属于本发明的保护范围。