CN103455707A - 基于凸优化技术的有限推力航天器自主交会轨迹规划方法 - Google Patents

Abstract

本发明提供了一种基于凸优化技术的有限推力航天器自主交会轨迹规划方法，首先建立描述两航天器相对运动的数学模型，然后执行自主交会轨迹规划算法，最后考虑自主交会的有限推力约束。本发明通过凸优化技术，对自主交会轨迹规划问题，进行优化求解，保证了全局最优性，并通过高效的求解方法，使其成为一种具有良好实时性的轨迹规划算法。

Description

基于凸优化技术的有限推力航天器自主交会轨迹规划方法

技术领域

本发明涉及一种有限推力航天器自主交会轨迹规划方法。

背景技术

近十几年里，以电推进、离子推进、光帆推进等为代表的连续推力推进技术逐渐发展了起来，这类推进系统与大推力化学火箭相比，具有大比冲、长寿命、消耗燃料少等特点的，并且可以提高有效载荷，应用前景十分广泛，比如，可以先利用大推力化学火箭发动机将航天器送入停泊轨道，然后由连续推力推进系统接替工作，完成后续任务。有限推力作用下的轨迹优化问题逐渐成为各国轨道设计工作者的研究重点。

有限推力变轨可以分为两种：一种是连续推力变轨，即变轨过程中推力一直作用；另外一种是中间带有无推力滑行段的变轨，需要发动机多次点火启动。在实际工程中，发动机的推力都是有限的。当发动机推力较小时，继续使用脉冲转移方法将使误差增大。因此，研究有限推力作用下航天器轨道优化设计方法是非常必要的。

目前的航天器自主交会轨迹规划方法，缺少将凸优化技术应用于航天器轨迹规划的研究。同时，目前现有的其他轨迹规划算法，往往计算量较大，计算时间长，在星上计算能力有限的情况下，难以进行实时解算，进而难以得到实际应用。

总的来说，当前还未能提出一种实时有效的基于凸优化技术的有限推力自主交会轨迹规划方法。因此有必要建立航天器的相对运动方程，设计基于凸优化技术的有限推力轨迹规划方法以实现航天器的自主交会轨迹规划。

发明内容

为了克服现有技术的不足，本发明提供一种基于凸优化技术的有限推力航天器自主交会轨迹规划方法，保证了全局最优性，具有良好实时性。

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

步骤一、使用HCW方程来描述两个航天器的近距离相对运动：

$\left\{\begin{array}{c}\stackrel{..}{x}-2n\stackrel{.}{y}-3n^{2}x=\frac{F_x}{m}\\ \stackrel{..}{y}+2n\stackrel{.}{x}=\frac{F_y}{m}\\ \stackrel{..}{z}+n^{2}z=\frac{F_z}{m}\end{array}\right.---\left(1\right)$

其中，m和n分别代表目标航天器的质量和轨道角速度，(F_x,F_y,F_z)^T为轨道控制力，

代表在轨道坐标系下三个方向轴上的相对位置和相对速度，X为径向位置矢量，Y为速度方向，Z方向与轨道面垂直，跟X，Y成右手坐标系；将该动力学方程写成如下的状态空间形式：

$\stackrel{.}{X}=\mathrm{AX}+B---\left(2\right)$

其中，U＝(F_x/m,F_y/m,F_z/m)^T为控制向量，A为系统矩阵，B为控制输入矩阵，矩阵A和B可分别表示如下：

$A=\left[\begin{array}{cccccc}0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\\ {3n}^2& 0& 0& 0& 2n& 0\\ 0& 0& 0& -2n& 0& 0\\ 0& 0& {-n}^2& 0& 0& 0\end{array}\right]$ $B=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

将该连续方程进行离散化，则离散化后的状态方程为

X_k+1=A_dX_k+B_dU_k     (3)

其中，X_k+1为k+1时刻系统的状态，X_k和U_k分别为k时刻的离散状态和控制输入；离散化之后的系统矩阵A_d和控制输入矩阵B_d如下

$A_d=\left[\begin{array}{cccccc}4-3C& 0& 0& \frac{S}{n}& \frac{2(1-C)}{n}& 0\\ 6(S-\mathrm{nT})& 1& 0& -\frac{2(1-C)}{n}& \frac{4S-3\mathrm{nT}}{n}& 0\\ 0& 0& C& 0& 0& \frac{S}{n}\\ 3\mathrm{nS}& 0& 0& C& 2S& 0\\ -6n(1-C)& 0& 0& -2S& 4C-3& 0\\ 0& 0& -\mathrm{nS}& 0& 0& C\end{array}\right]$ $B_d=\left[\begin{array}{ccc}\frac{1-C}{n^2}& \frac{2\mathrm{nT}-2S}{n^2}& 0\\ \frac{2(S-\mathrm{nT})}{n^2}& -\frac{{3T}^2}{2}+4\frac{1-C}{n^2}& 0\\ 0& 0& \frac{1-C}{n^2}\\ \frac{S}{n}& 2\frac{1-C}{n}& 0\\ \frac{2(C-1)}{n}& -3T+4\frac{S}{n}& 0\\ 0& 0& \frac{S}{n}\end{array}\right]$

其中，S＝sin(nT)，C＝cos(nT)，T为采样时间间隔；

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

U_k＝U_k-1+ΔU_k     (4)

其中Δ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(5\right)$

首先将整个轨迹规划时间分成N个时间段，这些时间段之间的节点处时间为：

t₀＜t₁＜t₂＜...＜t_k＜...＜t_N-1＜t_N     (6)

其中，t_N就是终端时间t_f，在轨迹规划过程中的控制量可以表示为

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

定义U_c和ΔU_c分别为控制输入向量和控制增量向量，则有如下的表达式

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

基于以上定义和状态方程表达式(3)，则可以得到如下的状态方程表达式为

X(k+j)＝A^jX(k)+[A^j-1 A^j-2 ... I]BU_c     (9)

定义

和

分别为实际状态向量和参考状态向量，即

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

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

X(k+j)＝φX(k)+ΓU(k-1)+G_yΔU_c     (11)

其中

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

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

$J_k=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\left|\right|X(k+j)-X_{\mathrm{ref}}(k+j)\left|\right|}_Q^2+\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\left|\right|\mathrm{\ΔU}(k+j)\left|\right|}_R^2---\left(12\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(13\right)$

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

$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---\left(14\right)$

$=\mathrm{\Δ}{U}_c^T[G_y^{T}Q{G}_y+R]\mathrm{\Δ}{U}_c-2E^{T}Q{G}_y\mathrm{\Δ}{U}_c^T+E^T\mathrm{QE}$

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

$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(15\right)$

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

步骤三、每一时刻的轨道控制力约束为

U^min≤U(k+i)≤U^max,(i＝0,...,N-1)     (16)

可以将控制向量表达成

U_c＝MΔU_c+FU(k-1)     (17)

其中

$F=\left[\begin{array}{c}I\\ I\\ \·\\ \·\\ \·\\ I\end{array}\right]$ $U_c^{\max }=\left[\begin{array}{c}{U}^{\max }\\ U^{\max }\\ \·\\ \·\\ \·\\ U^{\max }\end{array}\right]$ $U_c^{\min }=\left[\begin{array}{c}{U}^{\min }\\ U^{\min }\\ \·\\ \·\\ \·\\ U^{\min }\end{array}\right]$

将方程(17)代入方程(16)，然后不等式约束可以变为

$\left[\begin{array}{c}M\\ -M\end{array}\right]\mathrm{\Δ}{U}_c\left(k\right)\≤\left[\begin{array}{c}{U}_c^{\max }-\mathrm{FU}(k-1)\\ -U_c^{\min }+\mathrm{FU}(k-1)\end{array}\right]---\left(18\right).$

本发明的有益效果是：凸优化方法具有如下优点：第一，局部最优就是全局最优；第二，求解复杂的凸优化问题可以转化为求解较简单的对偶问题，甚至在满足强对偶的条件(KKT条件)下，可直接得到原优化问题的最优解；第三，求解方法简单有效,耗时短。所以，本发明通过凸优化技术，对自主交会轨迹规划问题，进行优化求解，保证了全局最优性，并通过高效的求解方法，使其成为一种具有良好实时性的轨迹规划算法。本文正是基于凸优化技术设计了有限推力航天器自主交会轨迹规划方法。

附图说明

图1是相对位置变化示意图；

图2是相对速度变化示意图；

图3是控制力变化示意图。

具体实施方式

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

一种基于凸优化技术的有限推力轨迹规划方法，其具体步骤包括：

步骤一、建立描述两航天器相对运动的数学模型

这里，使用HCW方程来描述两个航天器的近距离相对运动：

$\left\{\begin{array}{c}\stackrel{..}{x}-2n\stackrel{.}{y}-3n^{2}x=\frac{F_x}{m}\\ \stackrel{..}{y}+2n\stackrel{.}{x}=\frac{F_y}{m}\\ \stackrel{..}{z}+n^{2}z=\frac{F_z}{m}\end{array}\right.---\left(1\right)$

其中，m和n分别代表目标航天器的质量和轨道角速度，(F_x,F_y,F_z)^T为轨道控制力，

代表在轨道坐标系下的相对位置和相对速度。正如图1所示，X为径向位置矢量，Y为速度方向，Z方向与轨道面垂直，跟X，Y成右手坐标系。将该动力学方程写成如下的状态空间形式:

$\stackrel{.}{X}=A_{d}X+B_{d}U---\left(2\right)$

其中U＝(F_x/m,F_y/m,F_z/m)^T.将该连续方程，进行离散化，则离散化后的状态方程为

X_k+1=A_dX_k+B_dU_k     (3)

其中，X_k和U_k分别为k时刻的离散状态和控制输入。每一时刻的状态矩阵A_d和控制输入矩阵B_d可以表达为

$A_d=\left[\begin{array}{cccccc}4-3C& 0& 0& \frac{S}{n}& \frac{2(1-C)}{n}& 0\\ 6(S-\mathrm{nT})& 1& 0& -\frac{2(1-C)}{n}& \frac{4S-3\mathrm{nT}}{n}& 0\\ 0& 0& C& 0& 0& \frac{S}{n}\\ 3\mathrm{nS}& 0& 0& C& 2S& 0\\ -6n(1-C)& 0& 0& -2S& 4C-3& 0\\ 0& 0& -\mathrm{nS}& 0& 0& C\end{array}\right]$ $B_d=\left[\begin{array}{ccc}\frac{1-C}{n^2}& \frac{2\mathrm{nT}-2S}{n^2}& 0\\ \frac{2(S-\mathrm{nT})}{n^2}& -\frac{{3T}^2}{2}+4\frac{1-C}{n^2}& 0\\ 0& 0& \frac{1-C}{n^2}\\ \frac{S}{n}& 2\frac{1-C}{n}& 0\\ \frac{2(C-1)}{n}& -3T+4\frac{S}{n}& 0\\ 0& 0& \frac{S}{n}\end{array}\right]$

其中，S＝sin(nT)，C＝cos(nT)。

步骤二、自主交会轨迹规划算法

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

U_k＝U_k-1+ΔU_k

                          (4)

将上式带入状态方程，可得到

$\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(5\right)$

首先将整个轨迹规划时间分成N个时间段，这些时间段之间的节点处时间为：

t₀＜t₁＜t₂＜...＜t_k＜...＜t_N-1＜t_N     (6)

其中，t_N就是终端时间t_f。因此，在轨迹规划过程中的控制量可以表示为

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

定义U_c和ΔU_c分别为控制输入向量和控制增量向量，则有如下的表达式

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

基于以上定义和状态方程表达式(3)，则可以得到如下的状态方程表达式为

X(k+j)＝A^jX(k)+[A^j-1 A^j-2 ... I]BU_c     (9)

定义

和分别为状态向量和参考状态向量，即

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

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

X(k+j)＝φX(k)+ΓU(k-1)+G_yΔU_c     (11)

其中

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

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

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

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

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

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

$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---\left(14\right)$

$=\mathrm{\Δ}{U}_c^T[G_y^{T}Q{G}_y+R]\mathrm{\Δ}{U}_c-2E^{T}Q{G}_y\mathrm{\Δ}{U}_c^T+E^T\mathrm{QE}$

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

$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(15\right)$

其中

至此，已将自主交会的轨迹规划问题转换为一个标准的凸优化问题，可使用凸优化问题的求解方法对该轨迹规划问题进行求解。

步骤三、自主交会的有限推力约束

航天器轨迹规划必须要考虑到推力器的实际工作能力，因此每一时刻的轨道控制力约束为

U^min≤U(k+i)≤U^max,(i＝0,...,N-1)     (16)

因此，可以将控制向量表达成

U_c＝MΔU_c+FU(k-1)     (17)

其中

$F=\left[\begin{array}{c}I\\ I\\ \·\\ \·\\ \·\\ I\end{array}\right]$ $U_c^{\max }=\left[\begin{array}{c}{U}^{\max }\\ U^{\max }\\ \·\\ \·\\ \·\\ U^{\max }\end{array}\right]$ $U_c^{\min }=\left[\begin{array}{c}{U}^{\min }\\ U^{\min }\\ \·\\ \·\\ \·\\ U^{\min }\end{array}\right]$

将方程(17)代入方程(16),然后不等式约束可以变为

$\left[\begin{array}{c}M\\ -M\end{array}\right]\mathrm{\Δ}{U}_c\left(k\right)\≤\left[\begin{array}{c}{U}_c^{\max }-\mathrm{FU}(k-1)\\ -U_c^{\min }+\mathrm{FU}(k-1)\end{array}\right]---\left(18\right)$

本发明方法的实例验证：

1）初始相对位置为[-180,-220,-50]^Tm，初始角速度[0.5,0.6,0.7]^Tm/s；

2）追踪航天器和目标航天器的矢量分别为400.0Kg，800.0Kg；

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

4）仿真时间为40s，步长0.10s。

Claims (1)

1.一种基于凸优化技术的有限推力航天器自主交会轨迹规划方法，其特征在于包括下述步骤：

步骤一、使用HCW方程来描述两个航天器的近距离相对运动：

$\left\{\begin{array}{c}\stackrel{..}{x}-2n\stackrel{.}{y}-3n^{2}x=\frac{F_x}{m}\\ \stackrel{..}{y}+2n\stackrel{.}{x}=\frac{F_y}{m}\\ \stackrel{..}{z}+n^{2}z=\frac{F_z}{m}\end{array}\right.---\left(1\right)$

其中，m和n分别代表目标航天器的质量和轨道角速度，(F_x,F_y,F_z)^T为轨道控制力，

代表在轨道坐标系下三个方向轴上的相对位置和相对速度，X为径向位置矢量，Y为速度方向，Z方向与轨道面垂直，跟X，Y成右手坐标系；将该动力学方程写成如下的状态空间形式：

$\stackrel{.}{X}=\mathrm{AX}+B---\left(2\right)$

其中，U＝(F_x/m,F_y/m,F_z/m)^T为控制向量，A为系统矩阵，B为控制输入矩阵，矩阵A和B可分别表示如下：

$A=\left[\begin{array}{cccccc}0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\\ {3n}^2& 0& 0& 0& 2n& 0\\ 0& 0& 0& -2n& 0& 0\\ 0& 0& {-n}^2& 0& 0& 0\end{array}\right]$ $B=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

将该连续方程进行离散化，则离散化后的状态方程为

X_k+1=A_dX_k+B_dU_k     (3)

其中，X_k+1为k+1时刻系统的状态，X_k和U_k分别为k时刻的离散状态和控制输入；离散化之后的系统矩阵A_d和控制输入矩阵B_d如下

$A_d=\left[\begin{array}{cccccc}4-3C& 0& 0& \frac{S}{n}& \frac{2(1-C)}{n}& 0\\ 6(S-\mathrm{nT})& 1& 0& -\frac{2(1-C)}{n}& \frac{4S-3\mathrm{nT}}{n}& 0\\ 0& 0& C& 0& 0& \frac{S}{n}\\ 3\mathrm{nS}& 0& 0& C& 2S& 0\\ -6n(1-C)& 0& 0& -2S& 4C-3& 0\\ 0& 0& -\mathrm{nS}& 0& 0& C\end{array}\right]$ $B_d=\left[\begin{array}{ccc}\frac{1-C}{n^2}& \frac{2\mathrm{nT}-2S}{n^2}& 0\\ \frac{2(S-\mathrm{nT})}{n^2}& -\frac{{3T}^2}{2}+4\frac{1-C}{n^2}& 0\\ 0& 0& \frac{1-C}{n^2}\\ \frac{S}{n}& 2\frac{1-C}{n}& 0\\ \frac{2(C-1)}{n}& -3T+4\frac{S}{n}& 0\\ 0& 0& \frac{S}{n}\end{array}\right]$

其中，S＝sin(nT)，C＝cos(nT)，T为采样时间间隔；

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

U_k＝U_k-1+ΔU_k     (4)

其中Δ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(5\right)$

首先将整个轨迹规划时间分成N个时间段，这些时间段之间的节点处时间为：

t₀＜t₁＜t₂＜...＜t_k＜...＜t_N-1＜t_N    (6)

其中，t_N就是终端时间t_f，在轨迹规划过程中的控制量可以表示为

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

定义U_c和ΔU_c分别为控制输入向量和控制增量向量，则有如下的表达式

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

基于以上定义和状态方程表达式(3)，则可以得到如下的状态方程表达式为

X(k+j)＝A^jX(k)+[A^j-1 A^j-2 ... I]BU_c     (9)

定义

和

分别为实际状态向量和参考状态向量，即

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

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

X(k+j)＝φX(k)+ΓU(k-1)+G_yΔU_c     (11)

其中

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

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

$J_k=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\left|\right|X(k+j)-X_{\mathrm{ref}}(k+j)\left|\right|}_Q^2+\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\left|\right|\mathrm{\ΔU}(k+j)\left|\right|}_R^2---\left(12\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(13\right)$

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

$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---\left(14\right)$

$=\mathrm{\Δ}{U}_c^T[G_y^{T}Q{G}_y+R]\mathrm{\Δ}{U}_c-2E^{T}Q{G}_y\mathrm{\Δ}{U}_c^T+E^T\mathrm{QE}$

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

$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(15\right)$

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

步骤三、每一时刻的轨道控制力约束为

U^min≤U(k+i)≤U^max,(i＝0,...,N-1)     (16)

可以将控制向量表达成

U_c＝MΔU_c+FU(k-1)     (17)

其中

$F=\left[\begin{array}{c}I\\ I\\ \·\\ \·\\ \·\\ I\end{array}\right]$ $U_c^{\max }=\left[\begin{array}{c}{U}^{\max }\\ U^{\max }\\ \·\\ \·\\ \·\\ U^{\max }\end{array}\right]$ $U_c^{\min }=\left[\begin{array}{c}{U}^{\min }\\ U^{\min }\\ \·\\ \·\\ \·\\ U^{\min }\end{array}\right]$

将方程(17)代入方程(16)，然后不等式约束可以变为

$\left[\begin{array}{c}M\\ -M\end{array}\right]\mathrm{\Δ}{U}_c\left(k\right)\≤\left[\begin{array}{c}{U}_c^{\max }-\mathrm{FU}(k-1)\\ -U_c^{\min }+\mathrm{FU}(k-1)\end{array}\right]---\left(18\right).$

Images