CN103412485A - 基于滚动优化策略的刚体航天器姿态机动路径规划方法 - Google Patents

Abstract

本发明提供了一种基于滚动优化策略的刚体航天器姿态机动路径规划方法，首先建立刚体航天器的姿态动力学模型，然后执行姿态自主规划的预测控制算法，最后使用滚动优化策略对问题进行求解。本发明可以考虑系统的动态变化，同时将问题转换为凸优化问题，求解速度快，可以快速得到系统的优化解。

Description

基于滚动优化策略的刚体航天器姿态机动路径规划方法

技术领域

本发明涉及一种刚体航天器姿态机动路径规划方法。

背景技术

航天工程属于复杂大系统工程，在轨运行的航天器都承担一定的探测、开发和利用空间的任务，为了完成这些任务，对航天器姿态控制提出了各种严格要求。作为航天器正常运行并完成各种飞行任务的重要保障系统之一，航天器的姿态控制直接影响着航天器的工作性能和在轨寿命，设计高性能的姿态控制系统对于确保航天器成功应用至关重要。随着航天器探测空间的拓展，现代航天器对自主性要求的提高，特别在深空探测领域，长距离的通信长延迟与安全性和实时性之间的矛盾对航天器的自主性要求越来越高。因此，姿态机动的星上自主规划的意义更加突出，航天器必须能在轨自主完成满足此类约束的姿态机动控制过程。

航天器在轨运行期间需要进行大量的姿态机动来完成预定的观测等定向任务。在进行姿态机动路径规划时，执行机构的作用力矩的有界约束也对姿态机动路径规划造成影响。地面规划虽能通过大量计算获得机动过程的最优解，但是需要时刻保持航天器与地面站的通讯，实时性较差。另一方面，地面规划的最优化方法要占用大量的计算资源，需要针对星上计算资源条件，开发能够获得可行解乃至次优解的星上算法。

目前的航天器的姿态机动路径规划算法，缺少对燃料消耗最优性的研究，同时现有的星上自主机动规划算法，往往计算负载较大，在星上计算能力有限的情况下，难以得到实际应用。

总的来说，当前还未能提出一种实时有效的刚体航天器姿态机动规划算法。

发明内容

为了克服现有技术的不足，本发明提供一种基于滚动优化策略的姿态自主机动路径规划方法，以实现航天器的星上自主姿态机动控制。

本发明解决其技术问题所采用的技术方案包括以下步骤：

步骤一、在航天器本体坐标系中，选用欧拉刚体动力学方程建立刚体航天器姿态动力学模型：

扩展形式为

其中，上标Υ代表刚体航天器，

是航天器转动惯量矩阵，航天器转动角速度和所施加的控制力矩在本体坐标系下的表达式为

使用四元数来描述航天器的姿态运动，则航天器的姿态表示为

其中，

为四元数的标量部分，为四元数的矢量部分；

航天器姿态运动学方程为

定义状态变量

联立方程(1)和方程(5)，将非线性姿态动力学方程转换成如下的状态空间形式：

具体为

将该连续方程进行离散化，选取

作为k+1时刻的离散化变量，则离散化后的状态方程为

其中，

为离散化后的状态矩阵，

为离散化后的控制输入矩阵；

步骤二、将控制变量进行增量式处理，得到

U_k＝U_k-1+ΔU_k                               (9)

其中ΔU_k为从第k-1步到第k步的控制增量，将上式带入状态方程，得到

$\left[\begin{array}{c}{X}_{k+1}\\ U_k\end{array}\right]=\left[\begin{array}{cc}{A}_k& B_k\\ 0& I\end{array}\right]\left[\begin{array}{c}{X}_k\\ U_{k-1}\end{array}\right]+\left[\begin{array}{c}{B}_K\\ I\end{array}\right]\mathrm{\Δ}{U}_k---\left(10\right)$

定义N和N_c分别为预测时域和控制时域，在控制时域N_c中，控制量表示为

$U(k+i|k)=\underset{i=0}{\overset{N_c-1}{\mathrm{\Σ}}}\mathrm{\ΔU}(k+i|k)+U(k-1)---\left(11\right)$

定义U_c(k)和ΔU_c(k)分别为k时刻的预测控制输入向量和控制增量向量，则有

$U_c\left(k\right)=\left[\begin{array}{c}U\left(k\right|k)\\ \·\\ \·\\ \·\\ U(k+N_c-1|k)\end{array}\right]$   $\mathrm{\Δ}{U}_c\left(k\right)=\left[\begin{array}{c}\mathrm{\ΔU}\left(k\right|k)\\ \·\\ \·\\ \·\\ \mathrm{\ΔU}(k+N_c-1|k)\end{array}\right]---\left(12\right)$

基于以上定义和状态方程表达式(8)，得到如下的在预测时域内的状态方程表达式

X(k+j|k)＝A^jX(k)+[A^j-1   A^j-2   …   I]BU_c(k)     (13)

定义

和分别为k时刻的预测状态向量和预测参考状态向量，即

$X_c^p\left(k\right)=\left[\begin{array}{c}X(k+1|k)\\ \·\\ \·\\ \·\\ X(k+N|k)\end{array}\right]$   $X_c^{\mathrm{ref}}\left(k\right)=\left[\begin{array}{c}{X}_{\mathrm{ref}}(k+1|k)\\ \·\\ \·\\ \·\\ X_{\mathrm{ref}}(k+N|k)\end{array}\right]---\left(14\right)$

将方程(14)代入状态方程(13)当中，得到

$X_c^p\left(k\right)=\mathrm{\φX}\left(k\right)+\mathrm{\ΓU}(k-1)+G_y\mathrm{\Δ}{U}_c\left(k\right)---\left(15\right)$

其中

为状态列向量， $G_y=\left[\begin{array}{ccc}\underset{i=0}{\overset{j-1}{\mathrm{\Σ}}}{A}^{i}B& \·\·\·& \underset{i=0}{\overset{j-N_c}{\mathrm{\Σ}}}{A}^{i}B\end{array}\right],$ $\mathrm{\Γ}=\underset{i=0}{\overset{j-1}{\mathrm{\Σ}}}{A}^{i}B,$ φ＝A^j；

在预测时域当中的每一时刻，选定如下的目标函数：

$J_k=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\left|\right|X(k+j|k)-X_{\mathrm{ref}}(k+j|k)\left|\right|}_Q^2+\underset{i=0}{\overset{N_c-1}{\mathrm{\Σ}}}{\left|\right|\mathrm{\Δ}U(k+i|k)\left|\right|}_R^2---\left(16\right)$

其中，Q和R分别是状态权重矩阵和控制权重矩阵，X(k+j)和X_ref(k+j),j＝1,...,N分别为每一时刻的实际状态向量和参考状态向量；定义E为辅助变量，

$E=X_c^{\mathrm{ref}}-\mathrm{\φX}\left(k\right)-\mathrm{\ΓU}(k-1)---\left(17\right)$

将E代入目标函数，对目标函数做如下转换

$\begin{array}{c}{J}_k={\left|\right|G_y\mathrm{\Δ}{U}_c-E\left|\right|}_Q^2+{\left|\right|\mathrm{\Δ}{U}_c\left|\right|}_R^2\\ =[\mathrm{\Δ}{U}_c{G}_y^T-E^T]Q[G_y\mathrm{\Δ}{U}_c-E]+\mathrm{\Δ}{U}_c^T\mathrm{R\Δ}{U}_c\\ =\mathrm{\Δ}{U}_c^T[G_y^{T}Q{G}_y+R]\mathrm{\Δ}{U}_c-2E^T{\mathrm{QG}}_y\mathrm{\Δ}{U}_c^T+E^T\mathrm{QE}\end{array}---\left(18\right)$

最终，将目标函数转换为如下的形式

$J_k=\frac{1}{2}\mathrm{\Δ}{U}_c^T\left(k\right)\mathrm{H\Δ}{U}_c\left(k\right)+f^T\mathrm{\Δ}{U}_c\left(k\right)+\mathrm{const}---\left(19\right)$

其中 $H=2(G_y^{T}Q{G}_y+R),f=-2G_y^T\mathrm{QE}\left(k\right);$

步骤三、使用滚动优化策略进行求解，通过在每个时刻计算一个开环优化问题，在控制时域内得到一个优化解的序列，然后将优化计算得到的控制向量序列中的第一个控制变量施加到动力学模型中，进而得到新的状态变量，从而使循环迭代计算。

本发明的有益效果是：滚动时域策略是一种滚动的、有限时域内的优化算法，使用系统的动力学模型来预测未来时刻的系统输出和状态，在满足控制约束和状态约束的情况下，在有限时域内得到系统的优化解。滚动优化策略在每一个采样时刻，将系统的当前状态作为初始条件，计算在有限控制时域内系统的未来状态和系统输出，并将该控制序列的第一项作用于系统的动力学模型。在下一采样时刻，再用新的飞行器状态，迭代求解上述优化问题。该方法可以考虑系统的动态变化，同时将问题转换为凸优化问题，求解速度快，可以快速得到系统的优化解。本文正是基于此方法设计了基于滚动时域策略的刚体航天器姿态机动路径优化算法。

附图说明

图1是航天器姿态四元数变化曲线示意图。

图2是航天器姿态角速度变化曲线示意图。

图3是航天器控制力矩变化曲线示意图。

图4是本发明的方法流程图。

具体实施方式

下面结合附图和实施例对本发明进一步说明，本发明包括但不仅限于下述实施例。

如图4所示，一种基于滚动优化策略的刚体航天器姿态机动路径规划算法，其具体步骤包括：

步骤一、建立刚体航天器的姿态动力学模型

在航天器本体坐标系中，选用欧拉刚体动力学方程建立刚体航天器姿态动力学模型：

扩展形式为

其中，上标Υ代表刚体航天器，是航天器转动惯量矩阵，航天器转动角速度和所施加的控制力矩在本体坐标系下的表达式为

使用四元数来描述航天器的姿态运动，则航天器的姿态可表示为

其中，

为四元数的标量部分，

为四元数的矢量部分。航天器姿态运动学方程为

由此可见，航天器姿态动力学和运动学方程，均是高度非线性的数学模型。定义状态变量

联立方程(1)和方程(5)，将该非线性姿态动力学方程转换成如下的状态空间形式：

具体为

将该连续方程，进行离散化，选取

作为k+1时刻的离散化变量，则离散化后的状态方程为

其中，

为离散化后的状态矩阵，

为离散化后的控制输入矩阵。

步骤二、姿态自主规划的预测控制算法

将控制变量进行增量式处理，可得到

                     U_k＝U_k-1+ΔU_k

            (9)

其中ΔU_k为从第k-1步到第k步的控制增量，将上式带入状态方程，可得到

$\left[\begin{array}{c}{X}_{k+1}\\ U_k\end{array}\right]=\left[\begin{array}{cc}{A}_k& B_k\\ 0& I\end{array}\right]\left[\begin{array}{c}{X}_k\\ U_{k-1}\end{array}\right]+\left[\begin{array}{c}{B}_K\\ I\end{array}\right]\mathrm{\Δ}{U}_k---\left(10\right)$

定义N和N_c分别为预测时域和控制时域。因此，在控制时域N_c中，控制量可表示为

$U(k+i|k)=\underset{i=0}{\overset{N_c-1}{\mathrm{\Σ}}}\mathrm{\ΔU}(k+i|k)+U(k-1)---\left(11\right)$

定义U_c(k)和ΔU_c(k)分别为k时刻的预测控制输入向量和控制增量向量，则有

$U_c\left(k\right)=\left[\begin{array}{c}U\left(k\right|k)\\ \·\\ \·\\ \·\\ U(k+N_c-1|k)\end{array}\right]$   $\mathrm{\Δ}{U}_c\left(k\right)=\left[\begin{array}{c}\mathrm{\ΔU}\left(k\right|k)\\ \·\\ \·\\ \·\\ \mathrm{\ΔU}(k+N_c-1|k)\end{array}\right]---\left(12\right)$

基于以上定义和状态方程表达式(8)，则可以得到如下的在预测时域内的状态方程表达式为

X(k+j|k)＝A^jX(k)+[A^j-1A^j-2…I]BU_c(k)    (13)

定义和

分别为k时刻的预测状态向量和预测参考状态向量，即

$X_c^p\left(k\right)=\left[\begin{array}{c}X(k+1|k)\\ \·\\ \·\\ \·\\ X(k+N|k)\end{array}\right]$    $X_c^{\mathrm{ref}}\left(k\right)=\left[\begin{array}{c}{X}_{\mathrm{ref}}(k+1|k)\\ \·\\ \·\\ \·\\ X_{\mathrm{ref}}(k+N|k)\end{array}\right]---\left(14\right)$

将方程(14)代入状态方程(13)当中，可以得到

$X_c^p\left(k\right)=\mathrm{\φX}\left(k\right)+\mathrm{\ΓU}(k-1)+G_y\mathrm{\Δ}{U}_c\left(k\right)---\left(15\right)$

其中

为状态列向量， $G_y=\left[\begin{array}{ccc}\underset{i=0}{\overset{j-1}{\mathrm{\Σ}}}{A}^{i}B& \·\·\·& \underset{i=0}{\overset{j-N_c}{\mathrm{\Σ}}}{A}^{i}B\end{array}\right],$ $\mathrm{\Γ}=\underset{i=0}{\overset{j-1}{\mathrm{\Σ}}}{A}^{i}B,$ φ＝A^j。

在时域当中的每一时刻，选定如下的目标函数用以使系统状态收敛到参考状态值，同时减少控制过程中的燃料消耗:

$J_k=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\left|\right|X(k+j|k)-X_{\mathrm{ref}}(k+j|k)\left|\right|}_Q^2+\underset{i=0}{\overset{N_c-1}{\mathrm{\Σ}}}{\left|\right|\mathrm{\Δ}U(k+i|k)\left|\right|}_R^2---\left(16\right)$

其中，Q和R分别是状态权重矩阵和控制权重矩阵，X(k+j)和X_ref(k+j),j＝1,...,N分别为每一时刻的实际状态向量和参考状态向量。定义E为辅助变量，E的计算公式如下

$E=X_c^{\mathrm{ref}}-\mathrm{\φX}\left(k\right)-\mathrm{\ΓU}(k-1)---\left(17\right)$

因此，将E代入目标函数，可对目标函数做如下转换

$\begin{array}{c}{J}_k={\left|\right|G_y\mathrm{\Δ}{U}_c-E\left|\right|}_Q^2+{\left|\right|\mathrm{\Δ}{U}_c\left|\right|}_R^2\\ =[\mathrm{\Δ}{U}_c{G}_y^T-E^T]Q[G_y\mathrm{\Δ}{U}_c-E]+\mathrm{\Δ}{U}_c^T\mathrm{R\Δ}{U}_c\\ =\mathrm{\Δ}{U}_c^T[G_y^{T}Q{G}_y+R]\mathrm{\Δ}{U}_c-2E^T{\mathrm{QG}}_y\mathrm{\Δ}{U}_c^T+E^T\mathrm{QE}\end{array}---\left(18\right)$

最终，将目标函数转换为如下的形式

$J_k=\frac{1}{2}\mathrm{\Δ}{U}_c^T\left(k\right)\mathrm{H\Δ}{U}_c\left(k\right)+f^T\mathrm{\Δ}{U}_c\left(k\right)+\mathrm{const}---\left(19\right)$

其中

至此，已将刚体航天器姿态机动路径优化问题转化为一个标准的二次型规划问题，可使用Matlab软件当中的quadprog函数对该轨迹规划问题进行求解。

步骤三、滚动优化策略

当使用滚动优化策略对问题进行求解时，通过在每个时刻计算一个开环优化问题，在控制时域内得到一个优化解的序列，然后将优化计算得到的控制向量序列中的第一个控制变量施加到动力学模型中，进而得到新的状态变量，从而使循环迭代计算。

本发明方法的实例验证：

1）初始四元数为q＝[-0.2,0.3,-0.5,0.7874]^T，初始角速度ω＝[-0.2,0.1,0.3]^Trad/s；

2）航天器转动惯量矩阵为J＝diag([722,876,720])kg·m²

3）权重矩阵分别选择为Q＝45×I_{N·state_num}，

4）控制时域N_c，预测时域N分别取为5和10；

5）仿真时间为80s，步长0.10s；

6）理想角速度变化曲线为

ω_x＝0.2sin(2πt/90)

ω_y＝-0.3sin(2πt/120)

ω_z＝0.1sin(2πt/120)

由图1可以看到，在预测控制作用下，航天器的姿态四元数成功跟踪上了理想四元数的变化趋势；由图2可以看到，航天器的初始角速度跟理想角速度存在一定偏差，在预测控制作用下，航天器的角速度在不到10s时间内追踪上了理想角速度的变化趋势；由图3可以看到，施加在航天器上的控制力矩的幅值在航天器正常的输出范围内，同时控制力矩在前5s内迅速衰减，以后保持在0值附近很小范围内波动。

Claims (1)

1.一种基于滚动优化策略的刚体航天器姿态机动路径规划方法，其特征在于包括下述步骤：

步骤一、在航天器本体坐标系中，选用欧拉刚体动力学方程建立刚体航天器姿态动力学模型：

扩展形式为

其中，上标Υ代表刚体航天器，

是航天器转动惯量矩阵，航天器转动角速度和所施加的控制力矩在本体坐标系下的表达式为 使用四元数来描述航天器的姿态运动，则航天器的姿态表示为

其中，

为四元数的标量部分，

为四元数的矢量部分；

航天器姿态运动学方程为

定义状态变量联立方程(1)和方程(5)，将非线性姿态动力学方程转换成如下的状态空间形式：

具体为

将该连续方程进行离散化，选取

作为k+1时刻的离散化变量，则离散化后的状态方程为

其中，为离散化后的状态矩阵，

为离散化后的控制输入矩阵；

步骤二、将控制变量进行增量式处理，得到

U_k＝U_k-1+ΔU_k                               (9)

其中ΔU_k为从第k-1步到第k步的控制增量，将上式带入状态方程，得到

$\left[\begin{array}{c}{X}_{k+1}\\ U_k\end{array}\right]=\left[\begin{array}{cc}{A}_k& B_k\\ 0& I\end{array}\right]\left[\begin{array}{c}{X}_k\\ U_{k-1}\end{array}\right]+\left[\begin{array}{c}{B}_K\\ I\end{array}\right]\mathrm{\Δ}{U}_k---\left(10\right)$

定义N和N_c分别为预测时域和控制时域，在控制时域N_c中，控制量表示为

$U(k+i|k)=\underset{i=0}{\overset{N_c-1}{\mathrm{\Σ}}}\mathrm{\ΔU}(k+i|k)+U(k-1)---\left(11\right)$

定义U_c(k)和ΔU_c(k)分别为k时刻的预测控制输入向量和控制增量向量，则有

$U_c\left(k\right)=\left[\begin{array}{c}U\left(k\right|k)\\ \·\\ \·\\ \·\\ U(k+N_c-1|k)\end{array}\right]$     $\mathrm{\Δ}{U}_c\left(k\right)=\left[\begin{array}{c}\mathrm{\ΔU}\left(k\right|k)\\ \·\\ \·\\ \·\\ \mathrm{\ΔU}(k+N_c-1|k)\end{array}\right]---\left(12\right)$

基于以上定义和状态方程表达式(8)，得到如下的在预测时域内的状态方程表达式

X(k+j|k)＝A^jX(k)+[A^j-1   A^j-2   …   I]BU_c(k)  (13)

定义

和分别为k时刻的预测状态向量和预测参考状态向量，即

$X_c^p\left(k\right)=\left[\begin{array}{c}X(k+1|k)\\ \·\\ \·\\ \·\\ X(k+N|k)\end{array}\right]$    $X_c^{\mathrm{ref}}\left(k\right)=\left[\begin{array}{c}{X}_{\mathrm{ref}}(k+1|k)\\ \·\\ \·\\ \·\\ X_{\mathrm{ref}}(k+N|k)\end{array}\right]---\left(14\right)$

将方程(14)代入状态方程(13)当中，得到

$X_c^p\left(k\right)=\mathrm{\φX}\left(k\right)+\mathrm{\ΓU}(k-1)+G_y\mathrm{\Δ}{U}_c\left(k\right)---\left(15\right)$

其中

为状态列向量， $G_y=\left[\begin{array}{ccc}\underset{i=0}{\overset{j-1}{\mathrm{\Σ}}}{A}^{i}B& \·\·\·& \underset{i=0}{\overset{j-N_c}{\mathrm{\Σ}}}{A}^{i}B\end{array}\right],$ $\mathrm{\Γ}=\underset{i=0}{\overset{j-1}{\mathrm{\Σ}}}{A}^{i}B,\mathrm{\φ}=A^j;$

在预测时域当中的每一时刻，选定如下的目标函数：

$J_k=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\left|\right|X(k+j|k)-X_{\mathrm{ref}}(k+j|k)\left|\right|}_Q^2+\underset{i=0}{\overset{N_c-1}{\mathrm{\Σ}}}{\left|\right|\mathrm{\Δ}U(k+i|k)\left|\right|}_R^2---\left(16\right)$

其中，Q和R分别是状态权重矩阵和控制权重矩阵，X(k+j)和X_ref(k+j),j＝1,...,N分别为每一时刻的实际状态向量和参考状态向量；定义E为辅助变量，

$E=X_c^{\mathrm{ref}}-\mathrm{\φX}\left(k\right)-\mathrm{\ΓU}(k-1)---\left(17\right)$

将E代入目标函数，对目标函数做如下转换

$\begin{array}{c}{J}_k={\left|\right|G_y\mathrm{\Δ}{U}_c-E\left|\right|}_Q^2+{\left|\right|\mathrm{\Δ}{U}_c\left|\right|}_R^2\\ =[\mathrm{\Δ}{U}_c{G}_y^T-E^T]Q[G_y\mathrm{\Δ}{U}_c-E]+\mathrm{\Δ}{U}_c^T\mathrm{R\Δ}{U}_c\\ =\mathrm{\Δ}{U}_c^T[G_y^{T}Q{G}_y+R]\mathrm{\Δ}{U}_c-2E^T{\mathrm{QG}}_y\mathrm{\Δ}{U}_c^T+E^T\mathrm{QE}\end{array}---\left(18\right)$

最终，将目标函数转换为如下的形式

$J_k=\frac{1}{2}\mathrm{\Δ}{U}_c^T\left(k\right)\mathrm{H\Δ}{U}_c\left(k\right)+f^T\mathrm{\Δ}{U}_c\left(k\right)+\mathrm{const}---\left(19\right)$

其中 $H=2(G_y^{T}Q{G}_y+R),f=-2G_y^T\mathrm{QE}\left(k\right);$

步骤三、使用滚动优化策略进行求解，通过在每个时刻计算一个开环优化问题，在控制时域内得到一个优化解的序列，然后将优化计算得到的控制向量序列中的第一个控制变量施加到动力学模型中，进而得到新的状态变量，从而使循环迭代计算。

Images