CN103955225A - 一种适合空间绳系机器人逼近目标过程中的燃料最优位姿协调方法 - Google Patents

Abstract

本发明提供了一种适合空间绳系机器人逼近目标过程中的燃料最优位姿协调方法，步骤1：建立系统的动力学模型；步骤2：系统的动力学模型的离散；步骤3：规划最优轨迹控制信号；步骤4：协调最优控制器通过空间绳系机器人系统真实的状态信号x与步骤3计算出的理想状态信号x^*做差后得到误差信号，将误差信号与时变增益协调系数矩阵K_N相乘得到偏差控制信号，该偏差控制信号与步骤3计算出的理想状态信号x^*相加得到控制信号，以此实现协调控制。

Description

一种适合空间绳系机器人逼近目标过程中的燃料最优位姿协调方法

技术领域：

本发明隶属于航天器控制技术研究领域，涉及到一种低燃料消耗的空间绳系机器人位姿协调控制方法，该控制方法可以应用于空间绳系机器人逼近过程的控制中。

背景技术：

绳系机器人由于其灵活、安全、燃料消耗低等特点，在空间在轨服务中有着广泛的作用，其中对失效卫星救助、太空垃圾清理等进行抓捕是其主要应用。这些任务的实现都有赖于机器人对于目标的精确逼近，因此逼近过程中的有效控制方法是绳系机器人的主要研究内容之一。空间机器人在逼近目标过程中，由于末端机器人的重量非常有限，因此它自身的燃料非常有限，因此有必要利用系绳上和推力器对末端机器人的位置和姿态进行协调控制。但由于系绳上的拉力对于机器人的姿态有着巨大的影响，传统将位置和姿态分开进行控制的方案容易导致末端机器人姿态的不稳定，不施加控制甚至会发生系绳缠绕，因此，需要在绳系机器人逼近过程中对其位置和姿态进行协调的一体化控制。

发明内容：

本发明的目的在于提供一种适合空间绳系机器人逼近目标过程中的协调控制方法，该控制方法可以减少燃料消耗，保证姿态稳定，并且一定程度弥补推力器推力不足等特点。

本发明的目的可以通过以下技术方案来实现：

一种适合空间绳系机器人逼近目标过程中的燃料最优位姿协调方法，包括以下步骤：

步骤1：建立系统的动力学模型

假设空间平台的质量远大于空间系绳和末端机器人的质量，忽略空间系绳和末端机器人运动对于空间平台的影响，空间绳系机器人系统的质心与空间平台的质心重合，且运动在近圆形的开普勒轨道上，另外，忽略空间绳系机器人系统的面外运动和滚转运动，则系统的动力学模型为：

其中，μ表示系绳的线密度，l₀表示释放出的系绳长度，表示系绳上点的机动加速度，表示以归一化坐标描述的位置向量，表示系绳中的张力矢量，表示张力矢量的模，表示系绳上某一点处切向量的模，为归一化坐标，ξ为空间平台的释放/回收机构出口处系绳的自然坐标，N_a表示由于系绳释放和回收造成的附近阻尼力，N_P为作用在系绳上的控制力，m_B和J_B分别表示末端操作机构的质量和转动惯量，F_B和M_B分别表示作用在末端操作机构上的控制力和控制力矩；A_B表示末端操作机构的机动加速度；其中，以系绳存放在空间平台中的那一端为系绳自然坐标计算的起点；

步骤2：系统的动力学模型的离散

步骤3：规划最优轨迹控制信号

性能指标为：其中，d_M表示力臂的长度

空间绳系机器人系统在逼近过程中的动力学约束条件：

空间绳系机器人系统在逼近过程中的边界约束条件：x(0)＝x₀,x(t_f)＝x_f

在满足约束条件的情况下，以性能指标为目标，利用hp自适应伪谱算法求解出最优轨迹(x^*,u^*)；

步骤4：协调最优控制器通过空间绳系机器人系统真实的状态信号x与步骤3计算出的理想状态信号x^*做差后得到误差信号将误差信号与时变增益协调系数矩阵K_N相乘得到偏差控制信号该偏差控制信号与步骤3计算出的理想状态信号x^*相加得到控制信号，以此实现协调控制。

在步骤1中，位置向量满足：满足在步骤1中，N_a满足：

$N_a=\left\{\begin{array}{cc}\mathrm{\μ}{\stackrel{\·}{\mathrm{\ξ}}}^2{\stackrel{\‾}{\mathrm{\η}}}^2\left(0\right)& \stackrel{\·}{\mathrm{\ξ}}\≤0\\ \mathrm{\μ}{\stackrel{\·}{\mathrm{\ξ}}}^2\stackrel{\‾}{\mathrm{\η}}\left(0\right)[\stackrel{\‾}{\mathrm{\η}}\left(0\right)-1]& \stackrel{\·}{\mathrm{\ξ}}>0\end{array}\right..$

在步骤1中，A_B满足：

$A_B=\left[\begin{array}{c}{\stackrel{\·\·}{x}}_B-2\mathrm{\ω}{\stackrel{\·}{z}}_B\\ {\stackrel{\·\·}{z}}_B+2\mathrm{\ω}{\stackrel{\·}{x}}_B-3{\mathrm{\ω}}^2{z}_B\mathrm{}\end{array}\right],$ 其中，ω表示系统运行的轨道角速度。

在步骤1中，满足

其中，加速度分量和满足：

${\stackrel{\‾}{A}}_x=\stackrel{\stackrel{\·\·}{\‾}}{x}+2(1-\stackrel{\‾}{s})\frac{{\stackrel{\·}{l}}_0}{l_0}{\stackrel{\stackrel{\·}{\‾}}{x}}^{\′}+{(1-\stackrel{\‾}{s})}^2\frac{{\stackrel{\·}{l}}_0^2}{l_0^2}{\stackrel{\‾}{x}}^{\′\′}+(1-\stackrel{\‾}{s})\frac{{\stackrel{\·\·}{l}}_0}{l_0}{\stackrel{\‾}{x}}^{\′}-2(1-\stackrel{\‾}{s})\frac{{\stackrel{\·}{l}}_0^2}{l_0^2}{\stackrel{\‾}{x}}^{\′}-2\mathrm{\ω}[\stackrel{\stackrel{\·}{\‾}}{z}+\frac{{\stackrel{\·}{l}}_0}{l_0}(1-\stackrel{\‾}{s}){\stackrel{\‾}{z}}^{\′}]$

${\stackrel{\‾}{A}}_z=\stackrel{\stackrel{\·\·}{\‾}}{z}+2(1-\stackrel{\‾}{s})\frac{{\stackrel{\·}{l}}_0}{l_0}{\stackrel{\stackrel{\·}{\‾}}{z}}^{\′}+{(1-\stackrel{\‾}{s})}^2\frac{{\stackrel{\·}{l}}_0^2}{l_0^2}{\stackrel{\‾}{z}}^{\′\′}+(1-\stackrel{\‾}{s})\frac{{\stackrel{\·\·}{l}}_0}{l_0}{\stackrel{\‾}{z}}^{\′}-2(1-\stackrel{\‾}{s})\frac{{\stackrel{\·}{l}}_0^2}{l_0^2}{\stackrel{\‾}{z}}^{\′}+2\mathrm{\ω}[\stackrel{\stackrel{\·}{\‾}}{x}+\frac{{\stackrel{\·}{l}}_0}{l_0}(1-\stackrel{\‾}{s}){\stackrel{\‾}{x}}^{\′}]-3{\mathrm{\ω}}^2\stackrel{\‾}{z}.$

步骤2的具体方法为：通过线形单元的离散得到空间绳系机器人系统的运动满足：

其中，等效质量m^*和等效转动惯量I^*满足：等效力和等效力矩满足：

考虑到系绳的应变近似为0且张力是均匀分布的，因此，认为系绳的张力满足 $N=\left\{\begin{array}{cc}{N}_P+\mathrm{\&mu;}{\stackrel{\&CenterDot;}{l}}^2& \stackrel{\&CenterDot;}{l}\&GreaterEqual;0\\ N_P& \stackrel{\&CenterDot;}{l}<0\end{array}\right.$

根据系绳的张力满足条件和空间绳系机器人系统的运动所满足的条件得到空间绳系机器人系统的简化模型：其中，状态变量为：控制变量为：u＝[N_P,F_Bx,F_Bz,M_B]^T。

步骤4中的协调最优控制器根据以下方法设计：

空间绳系机器人系统的性能指标为：

空间绳系机器人系统的状态方程和边界条件为：

$\left\{\begin{array}{c}\stackrel{\stackrel{\&CenterDot;}{~}}{x}=A\left(t\right)\tilde{x}+B\left(t\right)\tilde{u}\\ \tilde{x}\left(t_0\right)=x\left(t_0\right)-x^{*}\left(t_0\right)\\ \tilde{x}\left(t_1\right)=0\end{array}\right.,$

引入Hamilton函数： $H=\frac{1}{2}{({\stackrel{\&OverBar;}{u}}^{*}+\tilde{u})}^{T}R({\stackrel{\&OverBar;}{u}}^{*}+\tilde{u})+{\mathrm{\&lambda;}}^T(A\tilde{x}+B\tilde{u}),$

通过变分计算可得协态方程为：

满足 $\&PartialD;H/\&PartialD;u=0$ 的最优控制输入为： $\tilde{u}=-R^{-1}{B}^T\left(t\right)\widetilde{\mathrm{\&lambda;}}-{\stackrel{\&OverBar;}{u}}^{*}$

根据最优性的必要条件可知，最优解满足方程：

$\left\{\begin{array}{c}\stackrel{\stackrel{\&CenterDot;}{~}}{x}=A\left(t\right)\tilde{x}-B\left(t\right)R^{-1}{B}^T\left(t\right)\widetilde{\mathrm{\&lambda;}}-B\left(t\right){\stackrel{\&OverBar;}{u}}^{*}\\ \stackrel{\stackrel{\&CenterDot;}{~}}{\mathrm{\&lambda;}}=-A^T\widetilde{\mathrm{\&lambda;}}\end{array}\right.$

和边界条件： $\left\{\begin{array}{c}\tilde{x}\left(t_0\right)=x\left(t_0\right)-x^{*}\left(t_0\right)\\ \tilde{x}\left(t_1\right)=0\end{array}\right.,$

使用τ代替t并对空间绳系机器人系统满足的状态方程和边界条件以及Hamilton函数进行改性，得到 $\left\{\begin{array}{c}\frac{d\hat{x}}{\mathrm{d\&tau;}}=\frac{t_1-t_0}{2}(A\hat{x}-B{R}^{-1}{B}^T\widehat{\mathrm{\&lambda;}}-B{\hat{u}}^{*})\\ \frac{d\widehat{\mathrm{\&lambda;}}}{\mathrm{d\&tau;}}=-\frac{t_1-t_0}{2}{A}^T\widehat{\mathrm{\&lambda;}}\\ \hat{x}(-1)=\tilde{x}\left(t_0\right)\\ \hat{x}\left(1\right)=0\end{array}\right.,$

式中， $\hat{x}\left(\mathrm{\&tau;}\right)=\tilde{x}\left(t\right),\widehat{\mathrm{\&lambda;}}\left(\mathrm{\&tau;}\right)=\widetilde{\mathrm{\&lambda;}}\left(t\right),{\hat{u}}^{*}\left(\mathrm{\&tau;}\right)={\stackrel{\&OverBar;}{u}}^{*}\left(t\right),$

采用LGL点作为插值离散点，设L_N为区间[-1,1]上的N阶Legendre多项式，P_N＝{τ₀,…,τ_N}为[-1,1]内按递增顺序排列的N阶LGL点集，其中，τ₀＝-1，τ_N＝1，τ₁…τ_N-1为多项式的根，则空间绳系机器人系统的状态变量和协态变量可插值近似为：

$\left\{\begin{array}{c}\hat{x}\left(\mathrm{\&tau;}\right)\&ap;{\hat{x}}^N\left(\mathrm{\&tau;}\right)=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\hat{x}\left({\mathrm{\&tau;}}_l\right){\mathrm{\&phi;}}_l\left(\mathrm{\&tau;}\right)\\ \widehat{\mathrm{\&lambda;}}\left(\mathrm{\&tau;}\right)\&ap;{\widehat{\mathrm{\&lambda;}}}^N\left(\mathrm{\&tau;}\right)=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right){\mathrm{\&phi;}}_l\left(\mathrm{\&tau;}\right)\end{array}\right.,$ 式中， ${\mathrm{\&phi;}}_l\left(\mathrm{\&tau;}\right)=\frac{1}{N(N+1)L_N\left({\mathrm{\&tau;}}_l\right)}\frac{({\mathrm{\&tau;}}^2-1){\stackrel{\&CenterDot;}{L}}_N\left(\mathrm{\&tau;}\right)}{\mathrm{\&tau;}-{\mathrm{\&tau;}}_l},$

同时，在LGL点τ_k处状态变量微分和协态变量微分利用插值多项式近似为：

$\left\{\begin{array}{c}\frac{d\hat{x}\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}\&ap;\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\hat{x}\left({\mathrm{\&tau;}}_l\right)\frac{d{\mathrm{\&phi;}}_l\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}{D}_{\mathrm{kl}}\hat{x}\left({\mathrm{\&tau;}}_l\right)\\ \frac{d\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}\&ap;\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right)\frac{d{\mathrm{\&phi;}}_l\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}{D}_{\mathrm{kl}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right)\end{array}\right.,$

式中，D表示(N+1)×(N+1)维的矩阵，满足：

$D\stackrel{\mathrm{\&Delta;}}{=}\left[D_{\mathrm{kl}}\right]=\left\{\begin{array}{cc}\frac{L_N\left({\mathrm{\&tau;}}_k\right)}{L_N\left({\mathrm{\&tau;}}_l\right)}\&CenterDot;\frac{1}{{\mathrm{\&tau;}}_k-{\mathrm{\&tau;}}_l}& k\&NotEqual;l\\ -\frac{N(N+1)}{4}& k=l=0\\ \frac{N(N+1)}{4}& k=l=N\\ 0& \mathrm{otherwise}\end{array}\right.,$

其中，k＝0和l＝0分别表示微分矩阵D的第一行和第一列，

将公式 $\left\{\begin{array}{c}\frac{d\hat{x}\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}\&ap;\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\hat{x}\left({\mathrm{\&tau;}}_l\right)\frac{d{\mathrm{\&phi;}}_l\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}{D}_{\mathrm{kl}}\hat{x}\left({\mathrm{\&tau;}}_l\right)\\ \frac{d\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}\&ap;\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right)\frac{d{\mathrm{\&phi;}}_l\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}{D}_{\mathrm{kl}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right)\end{array}\right.$ 代入 $\left\{\begin{array}{c}\frac{d\hat{x}}{\mathrm{d\&tau;}}=\frac{t_1-t_0}{2}(A\hat{x}-B{R}^{-1}{B}^T\widehat{\mathrm{\&lambda;}}-B{\hat{u}}^{*})\\ \frac{d\widehat{\mathrm{\&lambda;}}}{\mathrm{d\&tau;}}=-\frac{t_1-t_0}{2}{A}^T\widehat{\mathrm{\&lambda;}}\\ \hat{x}(-1)=\tilde{x}\left(t_0\right)\\ \hat{x}\left(1\right)=0\end{array}\right.,$ 得到 $\left\{\begin{array}{c}{M}_{\mathrm{xx}}X+M_{\mathrm{x\&lambda;}}\mathrm{\&Lambda;}=U\\ M_{\mathrm{\&lambda;\&lambda;}}\mathrm{\&Lambda;}=0\\ \mathrm{PX}=0\end{array}\right.,$ 其中，X、Λ和U为6(N+1)维的列向量，满足：

$X={[{\hat{x}}^T\left({\mathrm{\&tau;}}_0\right),{\hat{x}}^T\left({\mathrm{\&tau;}}_1\right),...,{\hat{x}}^T\left({\mathrm{\&tau;}}_N\right)]}^T,\mathrm{\&Lambda;}={[{\widehat{\mathrm{\&lambda;}}}^T\left({\mathrm{\&tau;}}_0\right),{\widehat{\mathrm{\&lambda;}}}^T\left({\mathrm{\&tau;}}_1\right),...,{\widehat{\mathrm{\&lambda;}}}^T\left({\mathrm{\&tau;}}_N\right)]}^T,$

$U={[{\hat{u}}^{*T}\left({\mathrm{\&tau;}}_0\right),{\hat{u}}^{*T}\left({\mathrm{\&tau;}}_1\right),...,{\hat{u}}^{*T}\left({\mathrm{\&tau;}}_N\right)]}^T$

矩阵M_xx、M_xλ和M_λλ为6(N+1)×6(N+1)维的分块矩阵，其分块满足：

${\left[M_{\mathrm{xx}}\right]}_{\mathrm{ij}}=\left\{\begin{array}{cc}{D}_{\mathrm{ij}}{I}_6& i\&NotEqual;j\\ D_{\mathrm{ij}}{I}_6-\frac{t_1-t_0}{2}A\left({\mathrm{\&tau;}}_i\right)& i=j\end{array}\right.,{\left[M_{\mathrm{xx}}\right]}_{\mathrm{ij}}=\left\{\begin{array}{cc}{0}_{6\&times;6}& i\&NotEqual;j\\ \frac{t_1-t_0}{2}B\left({\mathrm{\&tau;}}_i\right)R^{-1}B{\left({\mathrm{\&tau;}}_i\right)}^T& i=j\end{array}\right.,$

${\left[M_{\mathrm{xx}}\right]}_{\mathrm{ij}}=\left\{\begin{array}{cc}{D}_{\mathrm{ij}}{I}_6& i\&NotEqual;j\\ D_{\mathrm{ij}}{I}_6+\frac{t_1-t_0}{2}{A}^T\left({\mathrm{\&tau;}}_i\right)& i=j\end{array}\right.,$

矩阵P为6×6(N+1)维的矩阵，满足：P＝[0_6×6,…,0_6×6,I₆]，

将公式 $\left\{\begin{array}{c}{M}_{\mathrm{xx}}X+M_{\mathrm{x\&lambda;}}\mathrm{\&Lambda;}=U\\ M_{\mathrm{\&lambda;\&lambda;}}\mathrm{\&Lambda;}=0\\ \mathrm{PX}=0\end{array}\right.$ 写成矩阵形式：VZ＝Y，

其中， $V=\left[\begin{array}{cc}{M}_{\mathrm{xx}}& M_{\mathrm{x\&lambda;}}\\ 0& M_{\mathrm{\&lambda;\&lambda;}}\\ P& 0\end{array}\right],Z=\left[\begin{array}{c}X\\ \mathrm{\&Lambda;}\end{array}\right],Y=\left[\begin{array}{c}U\\ 0_{6(N+1)\&times;1}\\ 0_{6\&times;1}\end{array}\right],$

将矩阵V分成可得其中，V₀和V_e分别表示矩阵V的前6列和其它所有列，列向量Z_e表示向量Z的后12(N+1)行。由(58)式求解Z_e的最小二乘解为： $Z_e={\left(V_e^T{V}_e\right)}^{-1}{V}_e^T[Y-V_0\hat{x}\left({\mathrm{\&tau;}}_0\right)],$

构造中间变量 ${\hat{Z}}_e\stackrel{\mathrm{\&Delta;}}{=}{Z}_e-Z_e{|}_{\hat{x}\left({\mathrm{\&tau;}}_0\right)=0}=-{\left(V_e^T{V}_e\right)}^{-1}{V}_e^T{V}_0\hat{x}\left({\mathrm{\&tau;}}_0\right)$

取出中间变量的6N+1～6N+6行，得到式中，系数矩阵K_λ表示矩阵的6N+1～6N+6行，

定义时变增益协调系数矩阵K_N，令则控制量为：将t₀设置为系统当前运行的时刻t，令

t₁＝min{t₀+10,t_f}，即可得到协调控制器。

与现有技术相比，本发明具有如下积极效果：

1、采用了利用系绳和推力器进行协调控制，节省了燃料消耗。系绳可以提供较大的力，从而产生比机器人自身大得多的位置控制力，再结合自身的推力进行姿态控制，和传统的仅仅利用空间机器人自身推力器进行控制相比，燃料消耗较少。

2、采用了新型的位姿耦合动力学模型来设计协调控制器，给出了一种位姿一体化的协调控制方案，相对传统位姿分离控制的方案，避免了系绳对于机器人姿态的过大影响，保证了系统姿态的稳定。

附图说明

图1为空间绳系机器人系统示意图

图2为空间绳系机器人系统简化示意图

图3为释放/回收机构示意图

图4闭环控制系统结构图

具体实施方式

以下针对附图对本发明做详细阐述：

(1)位姿耦合动力学模型的建立

针对如图1所示的空间绳系机器人系统(包括空间平台1、空间系绳3、以及末端操作机构5)，假设空间平台1的质量远大于空间系绳3和末端机器人(末端机器人与末端操作机构之间是什么关系？是同一个部件吗？如果是，应当统一名称)的质量，从而忽略空间系绳和末端机器人运动对于空间平台1的影响，因而可将空间平台1简化为一个质点，并假设空间绳系机器人系统的质心与空间平台1的质心重合，且运动在近圆形的开普勒轨道上，另外，忽略空间绳系机器人系统的面外运动和滚转运动，从而得到如图2所示的系统简化示意图，图中，Px^oy^oz^o表示平台轨道坐标系，Bx^By^Bz^B表示末端机器人本体坐标系。

引入空间系绳的自然坐标s，它表示系绳未变形时，系绳上某一点与系绳某一端点之间的绳段长度，为了便于计算，本专利取系绳存放在空间平台中的那一端为系绳自然坐标计算的起点，并规定ξ(t)表示空间平台的释放/回收机构出口处系绳的自然坐标(也就是系绳与空间平台的连接点随时间变化的自然坐标)，L表示末端操作机构处系绳的自然坐标，即为系绳的总自然长度。对于空间绳系机器人系统的释放/回收机构，本专利将它简化为如图3所示的结构，空间平台中的系绳在张力N(ξ⁺)和控制力N_P的作用下释放和回收操作。

利用Hamilton原理建立系统的动力学模型为：

式中，μ表示系绳的线密度，l₀表示释放出的系绳长度，即l₀＝L-ξ；δ表示变分运算，表示系绳上一点的速度矢量，表示系绳与末端操作机构的连接点位置的转置，表示平台坐标系的x^o轴与末端操作机构坐标系x^B轴的夹角，I_B表示末端操作机构的转动惯量，表示x^o轴与x^B轴夹角的二阶导数。

表示以归一化坐标描述的位置向量(表示系绳上一点的位置矢量)，而满足：

$\stackrel{\&OverBar;}{s}=\frac{1}{l_0}(s-\mathrm{\&xi;})$

于是，位置向量满足 表示系绳中的张力矢量，表示张力矢量的模，表示系绳上某一点处切向量的模，即N_a表示由于系绳释放和回收造成的附近阻尼力，它满足：

$N_a=\left\{\begin{array}{cc}\mathrm{\&mu;}{\stackrel{\&CenterDot;}{\mathrm{\&xi;}}}^2{\stackrel{\&OverBar;}{\mathrm{\&eta;}}}^2\left(0\right)& \stackrel{\&CenterDot;}{\mathrm{\&xi;}}\&le;0\\ \mathrm{\&mu;}{\stackrel{\&CenterDot;}{\mathrm{\&xi;}}}^2\stackrel{\&OverBar;}{\mathrm{\&eta;}}\left(0\right)[\stackrel{\&OverBar;}{\mathrm{\&eta;}}\left(0\right)-1]& \stackrel{\&CenterDot;}{\mathrm{\&xi;}}>0\end{array}\right.$ (表示系绳上一点的自然坐标表示对时间的一阶导数)

m_B和J_B分别表示末端操作机构的质量和转动惯量，F_B和M_B分别表示作用在末端操作机构上的控制力和控制力矩；A_B表示末端操作机构的机动加速度，它满足：

$A_B=\left[\begin{array}{c}{\stackrel{\&CenterDot;\&CenterDot;}{x}}_B-2\mathrm{\&omega;}{\stackrel{\&CenterDot;}{z}}_B\\ {\stackrel{\&CenterDot;\&CenterDot;}{z}}_B+2\mathrm{\&omega;}{\stackrel{\&CenterDot;}{x}}_B-3{\mathrm{\&omega;}}^2{z}_B\mathrm{}\end{array}\right]$ (z_B分别是末端操作机构B在两个方向的加速度分量、速度分量和在z轴方向上的位置分量)

表示系绳上点的机动加速度，它满足：

其中，ω表示系统运行的轨道角速度，加速度分量和满足：

${\stackrel{\&OverBar;}{A}}_x=\stackrel{\stackrel{\&CenterDot;\&CenterDot;}{\&OverBar;}}{x}+2(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;}{l}}_0}{l_0}{\stackrel{\stackrel{\&CenterDot;}{\&OverBar;}}{x}}^{\&prime;}+{(1-\stackrel{\&OverBar;}{s})}^2\frac{{\stackrel{\&CenterDot;}{l}}_0^2}{l_0^2}{\stackrel{\&OverBar;}{x}}^{\&prime;\&prime;}+(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;\&CenterDot;}{l}}_0}{l_0}{\stackrel{\&OverBar;}{x}}^{\&prime;}-2(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;}{l}}_0^2}{l_0^2}{\stackrel{\&OverBar;}{x}}^{\&prime;}-2\mathrm{\&omega;}[\stackrel{\stackrel{\&CenterDot;}{\&OverBar;}}{z}+\frac{{\stackrel{\&CenterDot;}{l}}_0}{l_0}(1-\stackrel{\&OverBar;}{s}){\stackrel{\&OverBar;}{z}}^{\&prime;}],$

${\stackrel{\&OverBar;}{A}}_z=\stackrel{\stackrel{\&CenterDot;\&CenterDot;}{\&OverBar;}}{z}+2(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;}{l}}_0}{l_0}{\stackrel{\stackrel{\&CenterDot;}{\&OverBar;}}{z}}^{\&prime;}+{(1-\stackrel{\&OverBar;}{s})}^2\frac{{\stackrel{\&CenterDot;}{l}}_0^2}{l_0^2}{\stackrel{\&OverBar;}{z}}^{\&prime;\&prime;}+(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;\&CenterDot;}{l}}_0}{l_0}{\stackrel{\&OverBar;}{z}}^{\&prime;}-2(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;}{l}}_0^2}{l_0^2}{\stackrel{\&OverBar;}{z}}^{\&prime;}+2\mathrm{\&omega;}[\stackrel{\stackrel{\&CenterDot;}{\&OverBar;}}{x}+\frac{{\stackrel{\&CenterDot;}{l}}_0}{l_0}(1-\stackrel{\&OverBar;}{s}){\stackrel{\&OverBar;}{x}}^{\&prime;}]-3{\mathrm{\&omega;}}^2\stackrel{\&OverBar;}{z}.$

其中，分别表示释放出的系绳长度对时间的一阶、二阶导数，表示系绳上的点在系绳坐标下的分量，分别表示系绳上点在两个方向的位置分量对系绳坐标的一阶、二阶偏导数， 分别表示系绳上点在两个方向的速度分量和加速度分量，分别表示系绳上点在两个方向的位置分量对系绳坐标的一阶偏导数再对时间的导数。

位姿耦合动力学模型的离散

进一步假设空间系绳在逼近过程中可近似为一条直线且系绳近似不可伸长，即材料的杨氏模量E→∞，于是系绳上任何一点的空间位置向量可以近似满足：

$(\stackrel{\&OverBar;}{s},t)\&ap;\stackrel{\&OverBar;}{s}{r}_C\left(t\right)---\left(2\right)$

而末端操作机构质心B的位置向量满足：

式中，表示连接点C到末端操作机构B的距离。

将(2)式和(3)式代入方程(1)可得：

式中，x_C和z_C分别是空间系绳与末端操作机构的连接点C在两个方向的坐标分量；和分别空间系绳与末端操作机构的连接点C在两个方向的加速度分量；和分别空间系绳与末端操作机构的连接点C在两个方向的速度分量；n_x、n_z分别张力表示沿两个方向的分量，I_B表示末端操作机构的转动惯量。

系数矩阵满足：

$M=\left[\begin{array}{cc}1/3+m_B/\mathrm{\&mu;l}& 0\\ 0& 1/3+m_B/\mathrm{\&mu;l}\end{array}\right],C_1=\left[\begin{array}{cc}-1/3& 0\\ 0& -1/3\end{array}\right],C_2=2\mathrm{\&omega;}\left[\begin{array}{cc}0& -1/3-m_B/\mathrm{\&mu;l}\\ 1/3+m_B/\mathrm{\&mu;l}& 0\end{array}\right],$

$K_1=C_1,K_2=2\mathrm{\&omega;}\left[\begin{array}{cc}0& 1/3\\ -1/3& 0\end{array}\right],K_3=3{\mathrm{\&omega;}}^2\left[\begin{array}{cc}0& 0\\ 0& -1/3-k\end{array}\right]$

末端操作机构的推力矢量满足：

$\left[\begin{array}{c}{F}_x\\ F_z\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\&alpha;}& \sin \mathrm{\&alpha;}\\ -\sin \mathrm{\&alpha;}& \cos \mathrm{\&alpha;}\end{array}\right]\left[\begin{array}{c}{F}_{\mathrm{Bx}}\\ F_{\mathrm{Bz}}\end{array}\right],$ 其中， $\left[\begin{array}{c}{F}_{\mathrm{Bx}}\\ F_{\mathrm{Bz}}\end{array}\right]$ 为作用在末端操作机构上的控制力在两个方向的分量

耦合项满足：

其中，z_C表示连接点C在z方向的位置分量，表示连接点C在两个方向的速度分量和加速度分量。

由变分的任意性可知，空间绳系机器人系统中系绳与末端操作机构之间的连接点C的运动满足方程：

$\begin{array}{c}-\mathrm{\&mu;}{l}_0\{M\left[\begin{array}{c}{\stackrel{\&CenterDot;\&CenterDot;}{x}}_C\\ {\stackrel{\&CenterDot;\&CenterDot;}{z}}_C\end{array}\right]+(2\frac{{\stackrel{\&CenterDot;}{l}}_0}{l_0}{C}_1+C_2)\left[\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_C\\ {\stackrel{\&CenterDot;}{z}}_C\end{array}\right]-\left[\begin{array}{c}{n}_x\\ n_z\end{array}\right]+\left[\begin{array}{c}{F}_x\\ F_z\end{array}\right]\\ +[(\frac{{\stackrel{\&CenterDot;\&CenterDot;}{l}}_0}{l_0}-2\frac{{\stackrel{\&CenterDot;}{l}}_0^2}{l_0^2})K_1+\frac{{\stackrel{\&CenterDot;}{l}}_0}{l_0}{K}_2+K_3]\left[\begin{array}{c}{x}_C\\ z_C\end{array}\right]\}+\left[\begin{array}{c}{\mathrm{CO}}_x\\ {\mathrm{CO}}_z\end{array}\right]=0\end{array}---\left(5\right)$

末端机器人的姿态运动满足方程：

系绳释放点处的张力满足方程：

$N_a+N_P\stackrel{\&OverBar;}{\mathrm{\&eta;}}\left(0\right)-\stackrel{\&OverBar;}{N}\left(0\right)\stackrel{\&OverBar;}{\mathrm{\&eta;}}\left(0\right)=0---\left(7\right)$

将系绳连接点C的坐标写为极坐标形式有：

$\left[\begin{array}{c}{x}_C\\ z_C\end{array}\right]=\left[\begin{array}{c}l\cos \mathrm{\&theta;}\\ -l\sin \mathrm{\&theta;}\end{array}\right]---\left(8\right)$

式中，l表示空间平台质点P和系绳连接点C之间的距离，即θ表示矢量与x^o轴之间的夹角。另外由于系绳的应变ε→0，故l满足：

l≈l₀    (9)

将(8)式和(9)式代入(5)式和(6)式并整理可得：

式中，为了简化公式(10)，定义质量m^*和定义转动惯量I^*：

$m^{*}=m_B+\frac{1}{3}\mathrm{\&mu;l},$ I^*＝I_R+m_Bd²

定义力定义力矩

为转动角加速度。

对于(7)式，考虑到系绳的应变近似为0且张力是均匀分布的，于是可以认为系绳上的张力满足

$N=\left\{\begin{array}{cc}{N}_P+\mathrm{\&mu;}{\stackrel{\&CenterDot;}{l}}^2& \stackrel{\&CenterDot;}{l}\&GreaterEqual;0\\ N_P& \stackrel{\&CenterDot;}{l}<0\end{array}\right.---\left(11\right)$

联立方程(10)和方程(11)即可得到系统运动的简化模型，于是系统的状态方程可以写为：

$\stackrel{\&CenterDot;}{x}=F(x,u)---\left(12\right)$

式中，状态变量

其中，为x^o轴与x^B轴之间的夹角

控制变量

u＝[N_P,F_Bx,F_Bz,M_B]^T。

(2) 最省工质轨迹的规划

将空间绳系机器人系统最省工质的逼近问题看作最优控制问题：寻找控制变量u(t)∈R⁴，在满足约束条件的情况下，使得消耗的工质达到最少。对于空间绳系机器人系统，由于释放/回收机构消耗的主要是平台所储存的电能，因此在分析系统工质消耗时只需要考虑另外三个控制量，于是其性能指标可以写为：

$J=\frac{1}{2}{\&Integral;}_0^{t_f}(u_2^2+u_3^2+d_M^2{u}_4^2)\mathrm{dt}---\left(13\right)$

式中，d_M表示末端操作机构的力臂的长度。t_f表示末端时间，u₂、u₃、u₄分别表示不同控制量。

在运动约束方面，空间绳系机器人系统在逼近过程中需要满足动力学约束条件：

$\stackrel{\&CenterDot;}{x}=F(x,u)---\left(14\right)$

由于释放机构的性能及推力器的上限等因素的限制，因此系统的控制变量u(t)需要满足约束：

u_min≤u(t)≤u_max(15)

另外，由于初始状态及末端交会条件的限制，因此系统还需要边界约束条件：

x(0)＝x₀,x(t_f)＝x_f(16)

针对式(13)-式(16)所构成的标准非线性最优控制问题，可以采用hp自适应伪谱算法进行求解。

(3)协调控制器的设计

以hp自适应伪谱算法求解的最优轨迹(x^*,u^*)为基准，可将空间绳系机器人系统的实际状态x和控制量u写为：

$\left\{\begin{array}{c}x=x^{*}+\tilde{x}\\ u=u^{*}+K\tilde{u}\end{array}\right.---\left(17\right)$

式中，为真实状态信号与理想控制信号的误差信号，为误差信号经修正后的偏差控制信号，

$\tilde{u}={\left[\begin{array}{ccc}{\tilde{u}}_2& {\tilde{u}}_3& {\tilde{u}}_4\end{array}\right]}^T,K=\left[\begin{array}{c}0\\ I_3\end{array}\right]$

其中，I₃表示3×3的单位矩阵。

任意时间段的最优协调控制问题可表达为：求解偏差控制信号使得控制系统的性能指标最小，

$J={\&Integral;}_{t_0}^{t_1}\frac{1}{2}{({\stackrel{\&OverBar;}{u}}^{*}+\tilde{u})}^{T}R({\stackrel{\&OverBar;}{u}}^{*}+\tilde{u})\mathrm{dt}---\left(18\right)$

式中，

${\stackrel{\&OverBar;}{u}}^{*}={\left[\begin{array}{ccc}{u}_2^{*}& u_3^{*}& u_4^{*}\end{array}\right]}^T,$ R＝diag(1,1,d_M)

同时，满足状态方程和边界条件：

$\left\{\begin{array}{c}\stackrel{\stackrel{\&CenterDot;}{~}}{x}=A\left(t\right)\tilde{x}+B\left(t\right)\tilde{u}\\ \tilde{x}\left(t_0\right)=x\left(t_0\right)-x^{*}\left(t_0\right)\\ \tilde{x}\left(t_1\right)=0\end{array}\right.---\left(19\right)$

式中，为t₀时刻的误差信号，x(t₀)为t₀时刻的真实信号，x^*(t₀)为t₀时刻的理想信号，为t₁时刻的误差信号，A(t)为6×6的矩阵，B(t)为6×3的矩阵，且

$A\left(t\right)=\frac{\&PartialD;F}{\&PartialD;x},B\left(t\right)=\frac{\&PartialD;F}{\&PartialD;\stackrel{\&OverBar;}{u}}$

引入Hamilton函数：

$H=\frac{1}{2}{({\stackrel{\&OverBar;}{u}}^{*}+\tilde{u})}^{T}R({\stackrel{\&OverBar;}{u}}^{*}+\tilde{u})+{\mathrm{\&lambda;}}^T(A\tilde{x}+B\tilde{u})---\left(20\right)$

式中，λ∈R⁶为邻域最优协调控制问题的协态向量。通过变分计算可得协态方程为：

$\stackrel{\stackrel{\&CenterDot;}{~}}{\mathrm{\&lambda;}}=-\frac{\&PartialD;H}{\&PartialD;x}=-A^T\widetilde{\mathrm{\&lambda;}}---\left(21\right)$

以及满足的最优控制输入为：

$\tilde{u}=-R^{-1}{B}^T\left(t\right)\widetilde{\mathrm{\&lambda;}}-{\stackrel{\&OverBar;}{u}}^{*}---\left(22\right)$

根据最优性的必要条件可知，最优解满足方程：

$\left\{\begin{array}{c}\stackrel{\stackrel{\&CenterDot;}{~}}{x}=A\left(t\right)\tilde{x}-B\left(t\right)R^{-1}{B}^T\left(t\right)\widetilde{\mathrm{\&lambda;}}-B\left(t\right){\stackrel{\&OverBar;}{u}}^{*}\\ \stackrel{\stackrel{\&CenterDot;}{~}}{\mathrm{\&lambda;}}=-A^T\widetilde{\mathrm{\&lambda;}}\end{array}\right.---\left(23\right)$

和边界条件

$\left\{\begin{array}{c}\tilde{x}\left(t_0\right)=x\left(t_0\right)-x^{*}\left(t_0\right)\\ \tilde{x}\left(t_1\right)=0\end{array}\right.---\left(24\right)$

引入归一化的时间变量τ，令：

$\mathrm{\&tau;}=\frac{1}{t_1-t_0}[2t-(t_0+t_1)]---\left(25\right)$

使用τ代替t并对(23)、(24)式进行改写可得：

$\left\{\begin{array}{c}\frac{d\hat{x}}{\mathrm{d\&tau;}}=\frac{t_1-t_0}{2}(A\hat{x}-B{R}^{-1}{B}^T\widehat{\mathrm{\&lambda;}}-B{\hat{u}}^{*})\\ \frac{d\widehat{\mathrm{\&lambda;}}}{\mathrm{d\&tau;}}=-\frac{t_1-t_0}{2}{A}^T\widehat{\mathrm{\&lambda;}}\\ \hat{x}(-1)=\tilde{x}\left(t_0\right)\\ \hat{x}\left(1\right)=0\end{array}\right.---\left(26\right)$

式中， $\hat{x}\left(\mathrm{\&tau;}\right)=\tilde{x}\left(t\right),\widehat{\mathrm{\&lambda;}}\left(\mathrm{\&tau;}\right)=\widetilde{\mathrm{\&lambda;}}\left(t\right),{\hat{u}}^{*}\left(\mathrm{\&tau;}\right)={\stackrel{\&OverBar;}{u}}^{*}\left(t\right).$

采用Legendre-Gauss-Lobatto(LGL)点作为插值离散点，设L_N为区间[-1,1]上的N阶Legendre多项式，P_N＝{τ₀,…,τ_N}为[-1,1]内按递增顺序排列的N阶LGL点集，其中，τ₀＝-1，τ_N＝1，τ₁…τ_N-1为多项式的根。于是，系统的状态变量和协态变量可插值近似为：

$\left\{\begin{array}{c}\hat{x}\left(\mathrm{\&tau;}\right)\&ap;{\hat{x}}^N\left(\mathrm{\&tau;}\right)=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\hat{x}\left({\mathrm{\&tau;}}_l\right){\mathrm{\&phi;}}_l\left(\mathrm{\&tau;}\right)\\ \widehat{\mathrm{\&lambda;}}\left(\mathrm{\&tau;}\right)\&ap;{\widehat{\mathrm{\&lambda;}}}^N\left(\mathrm{\&tau;}\right)=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right){\mathrm{\&phi;}}_l\left(\mathrm{\&tau;}\right)\end{array}\right.---\left(27\right)$

式中，

${\mathrm{\&phi;}}_l\left(\mathrm{\&tau;}\right)=\frac{1}{N(N+1)L_N\left({\mathrm{\&tau;}}_l\right)}\frac{({\mathrm{\&tau;}}^2-1){\stackrel{\&CenterDot;}{L}}_N\left(\mathrm{\&tau;}\right)}{\mathrm{\&tau;}-{\mathrm{\&tau;}}_l}$

同时，在LGL点τ_k处状态变量微分和协态变量微分也可以利用插值多项式近似为：

$\left\{\begin{array}{c}\frac{d\hat{x}\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}\&ap;\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\hat{x}\left({\mathrm{\&tau;}}_l\right)\frac{d{\mathrm{\&phi;}}_l\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}{D}_{\mathrm{kl}}\hat{x}\left({\mathrm{\&tau;}}_l\right)\\ \frac{d\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}\&ap;\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right)\frac{d{\mathrm{\&phi;}}_l\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}{D}_{\mathrm{kl}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right)\end{array}\right.---\left(28\right)$

式中，D表示(N+1)×(N+1)维的矩阵，它满足：

$D\stackrel{\mathrm{def}}{=}\left[D_{\mathrm{kl}}\right]=\left\{\begin{array}{cc}\frac{L_N\left({\mathrm{\&tau;}}_k\right)}{L_N\left({\mathrm{\&tau;}}_l\right)}\&CenterDot;\frac{1}{{\mathrm{\&tau;}}_k-{\mathrm{\&tau;}}_l}& k\&NotEqual;l\\ -\frac{N(N+1)}{4}& k=l=0\\ \frac{N(N+1)}{4}& k=l=N\\ 0& \mathrm{otherwise}\end{array}\right.$

其中，k＝0和l＝0分别表示微分矩阵D的第一行和第一列，本文的其它矩阵也采用相同的记法。

将(28)式代入(26)式并进行整理可得：

$\left\{\begin{array}{c}{M}_{\mathrm{xx}}X+M_{\mathrm{x\&lambda;}}\mathrm{\&Lambda;}=U\\ M_{\mathrm{\&lambda;\&lambda;}}\mathrm{\&Lambda;}=0\\ \mathrm{PX}=0\end{array}\right.---\left(29\right)$

其中，X、Λ和U为6(N+1)维的列向量，满足：

$X={[{\hat{x}}^T\left({\mathrm{\&tau;}}_0\right),{\hat{x}}^T\left({\mathrm{\&tau;}}_1\right),...,{\hat{x}}^T\left({\mathrm{\&tau;}}_N\right)]}^T$

$\mathrm{\&Lambda;}={[{\widehat{\mathrm{\&lambda;}}}^T\left({\mathrm{\&tau;}}_0\right),{\widehat{\mathrm{\&lambda;}}}^T\left({\mathrm{\&tau;}}_1\right),...,{\widehat{\mathrm{\&lambda;}}}^T\left({\mathrm{\&tau;}}_N\right)]}^T$

$U={[{\hat{u}}^{*T}\left({\mathrm{\&tau;}}_0\right),{\hat{u}}^{*T}\left({\mathrm{\&tau;}}_1\right),...,{\hat{u}}^{*T}\left({\mathrm{\&tau;}}_N\right)]}^T$

矩阵M_xx、M_xλ和M_λλ为6(N+1)×6(N+1)维的分块矩阵，其分块满足：

${\left[M_{\mathrm{xx}}\right]}_{\mathrm{ij}}=\left\{\begin{array}{cc}{D}_{\mathrm{ij}}{I}_6& i\&NotEqual;j\\ D_{\mathrm{ij}}{I}_6-\frac{t_1-t_0}{2}A\left({\mathrm{\&tau;}}_i\right)& i=j\end{array}\right.$

${\left[M_{\mathrm{xx}}\right]}_{\mathrm{ij}}=\left\{\begin{array}{cc}{0}_{6\&times;6}& i\&NotEqual;j\\ \frac{t_1-t_0}{2}B\left({\mathrm{\&tau;}}_i\right)R^{-1}B{\left({\mathrm{\&tau;}}_i\right)}^T& i=j\end{array}\right.$

${\left[M_{\mathrm{xx}}\right]}_{\mathrm{ij}}=\left\{\begin{array}{cc}{D}_{\mathrm{ij}}{I}_6& i\&NotEqual;j\\ D_{\mathrm{ij}}{I}_6+\frac{t_1-t_0}{2}{A}^T\left({\mathrm{\&tau;}}_i\right)& i=j\end{array}\right.$

矩阵P为6×6(N+1)维的矩阵，满足：

P＝[0_6×6,…,0_6×6,I₆]

将(29)式写为矩阵形式可得：

VZ＝Y(30)

式中，

$V=\left[\begin{array}{cc}{M}_{\mathrm{xx}}& M_{\mathrm{x\&lambda;}}\\ 0& M_{\mathrm{\&lambda;\&lambda;}}\\ P& 0\end{array}\right],Z=\left[\begin{array}{c}X\\ \mathrm{\&Lambda;}\end{array}\right],Y=\left[\begin{array}{c}U\\ 0_{6(N+1)\&times;1}\\ 0_{6\&times;1}\end{array}\right]$

由于已知，于是将矩阵V分成可得：

$V_0\hat{x}\left({\mathrm{\&tau;}}_0\right)+V_e{Z}_e=Y---\left(31\right)$

式中，V₀和V_e分别表示矩阵V的前6列和其它所有列，列向量Z_e表示向量Z的后12(N+1)行。由(31)式求解Z_e的最小二乘解为：

$Z_e={\left(V_e^T{V}_e\right)}^{-1}{V}_e^T[Y-V_0\hat{x}\left({\mathrm{\&tau;}}_0\right)]---\left(32\right)$

构造中间变量

${\hat{Z}}_e\stackrel{\mathrm{def}}{=}{Z}_e-Z_e{|}_{\hat{x}\left({\mathrm{\&tau;}}_0\right)=0}=-{\left(V_e^T{V}_e\right)}^{-1}{V}_e^T{V}_0\hat{x}\left({\mathrm{\&tau;}}_0\right)---\left(33\right)$

取出中间变量的6N+1～6N+6行可得：

$\mathrm{\&lambda;}\left({\mathrm{\&tau;}}_0\right)=-K_{\mathrm{\&lambda;}}\hat{x}\left({\mathrm{\&tau;}}_0\right)---\left(34\right)$

式中，系数矩阵K_λ表示矩阵的6N+1～6N+6行。定义时变增益协调系数矩阵K_N，令：

$K_N\left(t_0\right)\stackrel{\mathrm{def}}{=}{R}^{-1}{B}^T\left(t_0\right)K_{\mathrm{\&lambda;}}---\left(35\right)$

则控制量可以表示为：

$\tilde{u}\left(t_0\right)=K_N\left(t_0\right)\tilde{x}\left(t_0\right)---\left(36\right)$

将t₀设置为系统当前运行的时刻t，令

t₁＝min{t₀+T(t₀),t_f}        (37)

式中，T(t₀)表示滑动窗口的宽度，在实时控制过程中，可以通过调节T来调整系统的性能，从而使系统具有更强的适应性，将(24)式计算得到的偏差控制量代入式(17)，即可得到当前时刻的控制量u(t)，从而得到了如图4所示的协调控制器，其中，hp自适应伪谱算法在开环条件下计算出理想状态信号和对应的理想控制信号，并产生对应的时变增益协调系数矩阵K_N，在闭环条件下，通过真实状态信号与理想状态信号作差可以得到误差信号，误差信号与K_N相乘可以得到偏差控制信号，而偏差控制信号与理想控制信号相加得到控制信号，从而实现闭环协调控制。

实施例：

(1)位姿耦合动力学模型的建立

利用Hamilton原理可以建立系统的动力学模型为：

式中，μ＝0.0045kg/m，L＝300m，m_B＝10kg，J_B＝1kg·m²；F_B和M_B分别表示作用在末端操作机构上的控制力和控制力矩；A_B表示末端操作机构的机动加速度，它满足：

$A_B=\left[\begin{array}{c}{\stackrel{\&CenterDot;\&CenterDot;}{x}}_B-2\mathrm{\&omega;}{\stackrel{\&CenterDot;}{z}}_B\\ {\stackrel{\&CenterDot;\&CenterDot;}{z}}_B+2\mathrm{\&omega;}{\stackrel{\&CenterDot;}{x}}_B-3{\mathrm{\&omega;}}^2{z}_B\mathrm{}\end{array}\right]$

表示系绳上点的机动加速度，它满足：

$\stackrel{\&OverBar;}{A}={[{\stackrel{\&OverBar;}{A}}_x,{\stackrel{\&OverBar;}{A}}_z]}^T$

其中，ω＝0.0011rad/s，加速度分量和满足：

${\stackrel{\&OverBar;}{A}}_x=\stackrel{\stackrel{\&CenterDot;\&CenterDot;}{\&OverBar;}}{x}+2(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;}{l}}_0}{l_0}{\stackrel{\stackrel{\&CenterDot;}{\&OverBar;}}{x}}^{\&prime;}+{(1-\stackrel{\&OverBar;}{s})}^2\frac{{\stackrel{\&CenterDot;}{l}}_0^2}{l_0^2}{\stackrel{\&OverBar;}{x}}^{\&prime;\&prime;}+(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;\&CenterDot;}{l}}_0}{l_0}{\stackrel{\&OverBar;}{x}}^{\&prime;}-2(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;}{l}}_0^2}{l_0^2}{\stackrel{\&OverBar;}{x}}^{\&prime;}-2\mathrm{\&omega;}[\stackrel{\stackrel{\&CenterDot;}{\&OverBar;}}{z}+\frac{{\stackrel{\&CenterDot;}{l}}_0}{l_0}(1-\stackrel{\&OverBar;}{s}){\stackrel{\&OverBar;}{z}}^{\&prime;}],$

${\stackrel{\&OverBar;}{A}}_z=\stackrel{\stackrel{\&CenterDot;\&CenterDot;}{\&OverBar;}}{z}+2(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;}{l}}_0}{l_0}{\stackrel{\stackrel{\&CenterDot;}{\&OverBar;}}{z}}^{\&prime;}+{(1-\stackrel{\&OverBar;}{s})}^2\frac{{\stackrel{\&CenterDot;}{l}}_0^2}{l_0^2}{\stackrel{\&OverBar;}{z}}^{\&prime;\&prime;}+(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;\&CenterDot;}{l}}_0}{l_0}{\stackrel{\&OverBar;}{z}}^{\&prime;}-2(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;}{l}}_0^2}{l_0^2}{\stackrel{\&OverBar;}{z}}^{\&prime;}+2\mathrm{\&omega;}[\stackrel{\stackrel{\&CenterDot;}{\&OverBar;}}{x}+\frac{{\stackrel{\&CenterDot;}{l}}_0}{l_0}(1-\stackrel{\&OverBar;}{s}){\stackrel{\&OverBar;}{x}}^{\&prime;}]-3{\mathrm{\&omega;}}^2\stackrel{\&OverBar;}{z}.$

(2)位姿耦合动力学模型的离散

通过线形单元的离散可以得到空间绳系机器人系统的运动满足方程：

式中，等效质量m^*和等效转动惯量I^*满足：

$m^{*}=m_B+\frac{1}{3}\mathrm{\&mu;l},$ I^*＝I_R+m_Bd²

等效力和等效力矩满足：

考虑到系绳的应变近似为0且张力是均匀分布的，于是可以认为系绳上的张力满足

$N=\left\{\begin{array}{cc}{N}_P+\mathrm{\&mu;}{\stackrel{\&CenterDot;}{l}}^2& \stackrel{\&CenterDot;}{l}\&GreaterEqual;0\\ N_P& \stackrel{\&CenterDot;}{l}<0\end{array}\right.---\left(40\right)$

联立方程(39)和方程(40)即可得到系统运动的简化模型，于是系统的状态方程可以写为：

$\stackrel{\&CenterDot;}{x}=F(x,u)---\left(41\right)$

式中，状态变量

控制变量

u＝[N_P,F_Bx,F_Bz,M_B]^T。

(3)最省工质轨迹的规划

将空间绳系机器人系统最省工质的逼近问题看作最优控制问题：寻找控制变量u(t)∈R⁴，在满足约束条件的情况下，使得消耗的工质达到最少。对于空间绳系机器人系统，由于释放/回收机构消耗的主要是平台所储存的电能，因此在分析系统工质消耗时只需要考虑另外三个控制量，于是其性能指标可以写为：

$J=\frac{1}{2}{\&Integral;}_0^{t_f}(u_2^2+u_3^2+d_M^2{u}_4^2)\mathrm{dt}---\left(42\right)$

式中，d_M表示力臂的长度。在运动约束方面，空间绳系机器人系统在逼近过程中需要满足动力学约束条件：

$\stackrel{\&CenterDot;}{x}=F(x,u)---\left(43\right)$

由于释放机构的性能及推力器的上限等因素的限制，因此系统的控制变量u(t)需要满足约束：

u_min≤u(t)≤u_max(44)

式中：

u_min＝[0,-200,-200,-200]^TmN，u_max＝[500,200,200,200]^TmN。

另外，由于初始状态及末端交会条件的限制，因此系统还需要边界约束条件：

x(0)＝[1,0,0,1,0,0]^T,x(t_f)＝[141.4214,0.7854,0,0,0,0]^T(45)

针对式(42)-式(45)所构成的标准非线性最优控制问题，采用hp自适应伪谱算法进行求解。

(4)协调控制器的设计

任意时间段的最优协调控制问题可表达为：求解偏差控制量u ，使得控制系统的性能指标最小，

$J={\&Integral;}_{t_0}^{t_1}\frac{1}{2}{({\stackrel{\&OverBar;}{u}}^{*}+\tilde{u})}^{T}R({\stackrel{\&OverBar;}{u}}^{*}+\tilde{u})\mathrm{dt}---\left(46\right)$

同时，满足状态方程和边界条件：

$\left\{\begin{array}{c}\stackrel{\stackrel{\&CenterDot;}{~}}{x}=A\left(t\right)\tilde{x}+B\left(t\right)\tilde{u}\\ \tilde{x}\left(t_0\right)=x\left(t_0\right)-x^{*}\left(t_0\right)\\ \tilde{x}\left(t_1\right)=0\end{array}\right.---\left(47\right)$

引入Hamilton函数：

$H=\frac{1}{2}{({\stackrel{\&OverBar;}{u}}^{*}+\tilde{u})}^{T}R({\stackrel{\&OverBar;}{u}}^{*}+\tilde{u})+{\mathrm{\&lambda;}}^T(A\tilde{x}+B\tilde{u})---\left(48\right)$

通过变分计算可得协态方程为：

$\stackrel{\stackrel{\&CenterDot;}{~}}{\mathrm{\&lambda;}}=-\frac{\&PartialD;H}{\&PartialD;x}=-A^T\widetilde{\mathrm{\&lambda;}}---\left(49\right)$

以及满足的最优控制输入为：

$\tilde{u}=-R^{-1}{B}^T\left(t\right)\widetilde{\mathrm{\&lambda;}}-{\stackrel{\&OverBar;}{u}}^{*}---\left(50\right)$

根据最优性的必要条件可知，最优解满足方程：

$\left\{\begin{array}{c}\stackrel{\stackrel{\&CenterDot;}{~}}{x}=A\left(t\right)\tilde{x}-B\left(t\right)R^{-1}{B}^T\left(t\right)\widetilde{\mathrm{\&lambda;}}-B\left(t\right){\stackrel{\&OverBar;}{u}}^{*}\\ \stackrel{\stackrel{\&CenterDot;}{~}}{\mathrm{\&lambda;}}=-A^T\widetilde{\mathrm{\&lambda;}}\end{array}\right.---\left(51\right)$

和边界条件

$\left\{\begin{array}{c}\tilde{x}\left(t_0\right)=x\left(t_0\right)-x^{*}\left(t_0\right)\\ \tilde{x}\left(t_1\right)=0\end{array}\right.---\left(52\right)$

使用τ代替t并对(47)、(48)式进行改写可得：

$\left\{\begin{array}{c}\frac{d\hat{x}}{\mathrm{d\&tau;}}=\frac{t_1-t_0}{2}(A\hat{x}-B{R}^{-1}{B}^T\widehat{\mathrm{\&lambda;}}-B{\hat{u}}^{*})\\ \frac{d\widehat{\mathrm{\&lambda;}}}{\mathrm{d\&tau;}}=-\frac{t_1-t_0}{2}{A}^T\widehat{\mathrm{\&lambda;}}\\ \hat{x}(-1)=\tilde{x}\left(t_0\right)\\ \hat{x}\left(1\right)=0\end{array}\right.---\left(53\right)$

式中， $\hat{x}\left(\mathrm{\&tau;}\right)=\tilde{x}\left(t\right),\widehat{\mathrm{\&lambda;}}\left(\mathrm{\&tau;}\right)=\widetilde{\mathrm{\&lambda;}}\left(t\right),{\hat{u}}^{*}\left(\mathrm{\&tau;}\right)={\stackrel{\&OverBar;}{u}}^{*}\left(t\right).$

采用Legendre-Gauss-Lobatto(LGL)点作为插值离散点，设L_N为区间[-1,1]上的N阶Legendre多项式，P_N＝{τ₀,…,τ_N}为[-1,1]内按递增顺序排列的N阶LGL点集，其中，τ₀＝-1，τ_N＝1，τ₁…τ_N-1为多项式的根。于是，系统的状态变量和协态变量可插值近似为：

$\left\{\begin{array}{c}\hat{x}\left(\mathrm{\&tau;}\right)\&ap;{\hat{x}}^N\left(\mathrm{\&tau;}\right)=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\hat{x}\left({\mathrm{\&tau;}}_l\right){\mathrm{\&phi;}}_l\left(\mathrm{\&tau;}\right)\\ \widehat{\mathrm{\&lambda;}}\left(\mathrm{\&tau;}\right)\&ap;{\widehat{\mathrm{\&lambda;}}}^N\left(\mathrm{\&tau;}\right)=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right){\mathrm{\&phi;}}_l\left(\mathrm{\&tau;}\right)\end{array}\right.---\left(54\right)$

式中，

${\mathrm{\&phi;}}_l\left(\mathrm{\&tau;}\right)=\frac{1}{N(N+1)L_N\left({\mathrm{\&tau;}}_l\right)}\frac{({\mathrm{\&tau;}}^2-1){\stackrel{\&CenterDot;}{L}}_N\left(\mathrm{\&tau;}\right)}{\mathrm{\&tau;}-{\mathrm{\&tau;}}_l}$

同时，在LGL点τ_k处状态变量微分和协态变量微分也可以利用插值多项式近似为：

$\left\{\begin{array}{c}\frac{d\hat{x}\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}\&ap;\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\hat{x}\left({\mathrm{\&tau;}}_l\right)\frac{d{\mathrm{\&phi;}}_l\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}{D}_{\mathrm{kl}}\hat{x}\left({\mathrm{\&tau;}}_l\right)\\ \frac{d\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}\&ap;\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right)\frac{d{\mathrm{\&phi;}}_l\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}{D}_{\mathrm{kl}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right)\end{array}\right.---\left(55\right)$

式中，D表示(N+1)×(N+1)维的矩阵，它满足：

$D\stackrel{\mathrm{\&Delta;}}{=}\left[D_{\mathrm{kl}}\right]=\left\{\begin{array}{cc}\frac{L_N\left({\mathrm{\&tau;}}_k\right)}{L_N\left({\mathrm{\&tau;}}_l\right)}\&CenterDot;\frac{1}{{\mathrm{\&tau;}}_k-{\mathrm{\&tau;}}_l}& k\&NotEqual;l\\ -\frac{N(N+1)}{4}& k=l=0\\ \frac{N(N+1)}{4}& k=l=N\\ 0& \mathrm{otherwise}\end{array}\right.$

其中，k＝0和l＝0分别表示微分矩阵D的第一行和第一列，本文的其它矩阵也采用相同的记法。

将(55)式代入(53)式并进行整理可得：

$\left\{\begin{array}{c}{M}_{\mathrm{xx}}X+M_{\mathrm{x\&lambda;}}\mathrm{\&Lambda;}=U\\ M_{\mathrm{\&lambda;\&lambda;}}\mathrm{\&Lambda;}=0\\ \mathrm{PX}=0\end{array}\right.---\left(56\right)$

其中，X、Λ和U为6(N+1)维的列向量，满足：

$X={[{\hat{x}}^T\left({\mathrm{\&tau;}}_0\right),{\hat{x}}^T\left({\mathrm{\&tau;}}_1\right),...,{\hat{x}}^T\left({\mathrm{\&tau;}}_N\right)]}^T$

$\mathrm{\&Lambda;}={[{\widehat{\mathrm{\&lambda;}}}^T\left({\mathrm{\&tau;}}_0\right),{\widehat{\mathrm{\&lambda;}}}^T\left({\mathrm{\&tau;}}_1\right),...,{\widehat{\mathrm{\&lambda;}}}^T\left({\mathrm{\&tau;}}_N\right)]}^T$

$U={[{\hat{u}}^{*T}\left({\mathrm{\&tau;}}_0\right),{\hat{u}}^{*T}\left({\mathrm{\&tau;}}_1\right),...,{\hat{u}}^{*T}\left({\mathrm{\&tau;}}_N\right)]}^T$

矩阵M_xx、M_xλ和M_λλ为6(N+1)×6(N+1)维的分块矩阵，其分块满足：

${\left[M_{\mathrm{xx}}\right]}_{\mathrm{ij}}=\left\{\begin{array}{cc}{D}_{\mathrm{ij}}{I}_6& i\&NotEqual;j\\ D_{\mathrm{ij}}{I}_6-\frac{t_1-t_0}{2}A\left({\mathrm{\&tau;}}_i\right)& i=j\end{array}\right.$

${\left[M_{\mathrm{xx}}\right]}_{\mathrm{ij}}=\left\{\begin{array}{cc}{0}_{6\&times;6}& i\&NotEqual;j\\ \frac{t_1-t_0}{2}B\left({\mathrm{\&tau;}}_i\right)R^{-1}B{\left({\mathrm{\&tau;}}_i\right)}^T& i=j\end{array}\right.$

${\left[M_{\mathrm{xx}}\right]}_{\mathrm{ij}}=\left\{\begin{array}{cc}{D}_{\mathrm{ij}}{I}_6& i\&NotEqual;j\\ D_{\mathrm{ij}}{I}_6+\frac{t_1-t_0}{2}{A}^T\left({\mathrm{\&tau;}}_i\right)& i=j\end{array}\right.$

矩阵P为6×6(N+1)维的矩阵，满足：

P＝[0_6×6,…,0_6×6,I₆]

将(56)式写为矩阵形式可得：

VZ＝Y    (57)

式中，

$V=\left[\begin{array}{cc}{M}_{\mathrm{xx}}& M_{\mathrm{x\&lambda;}}\\ 0& M_{\mathrm{\&lambda;\&lambda;}}\\ P& 0\end{array}\right],Z=\left[\begin{array}{c}X\\ \mathrm{\&Lambda;}\end{array}\right],Y=\left[\begin{array}{c}U\\ 0_{6(N+1)\&times;1}\\ 0_{6\&times;1}\end{array}\right]$

由于已知，于是将矩阵V分成可得：

$V_0\hat{x}\left({\mathrm{\&tau;}}_0\right)+V_e{Z}_e=Y---\left(58\right)$

式中，V₀和V_e分别表示矩阵V的前6列和其它所有列，列向量Z_e表示向量Z的后12(N+1)行。由(58)式求解Z_e的最小二乘解为：

$Z_e={\left(V_e^T{V}_e\right)}^{-1}{V}_e^T[Y-V_0\hat{x}\left({\mathrm{\&tau;}}_0\right)]---\left(59\right)$

构造中间变量

${\hat{Z}}_e\stackrel{\mathrm{\&Delta;}}{=}{Z}_e-Z_e{|}_{\hat{x}\left({\mathrm{\&tau;}}_0\right)=0}=-{\left(V_e^T{V}_e\right)}^{-1}{V}_e^T{V}_0\hat{x}\left({\mathrm{\&tau;}}_0\right)---\left(60\right)$

取出中间变量的6N+1～6N+6行可得：

$\mathrm{\&lambda;}\left({\mathrm{\&tau;}}_0\right)=-K_{\mathrm{\&lambda;}}\hat{x}\left({\mathrm{\&tau;}}_0\right)---\left(61\right)$

式中，系数矩阵K_λ表示矩阵的6N+1～6N+6行。定义系数矩阵K_N，令：

$K_N\left(t_0\right)\stackrel{\mathrm{\&Delta;}}{=}{R}^{-1}{B}^T\left(t_0\right)K_{\mathrm{\&lambda;}}---\left(62\right)$

则控制量可以表示为：

$\tilde{u}\left(t_0\right)=K_N\left(t_0\right)\tilde{x}\left(t_0\right)---\left(63\right)$

将t₀设置为系统当前运行的时刻t，令

t₁＝min{t₀+10,t_f}(64)

将(63)式计算得到的偏差控制量代入式(17)，即可得到当前时刻的控制量u(t)，从而得到了如图4所示的协调控制器。

Claims (7)

1.一种适合空间绳系机器人逼近目标过程中的燃料最优位姿协调方法，其特征在于：包括以下步骤：

步骤1：建立系统的动力学模型

假设空间平台的质量远大于空间系绳和末端机器人的质量，忽略空间系绳和末端机器人运动对于空间平台的影响，空间绳系机器人系统的质心与空间平台的质心重合，且运动在近圆形的开普勒轨道上，另外，忽略空间绳系机器人系统的面外运动和滚转运动，则系统的动力学模型为：

其中，μ表示系绳的线密度，l₀表示释放出的系绳长度，表示系绳上点的机动加速度，表示以归一化坐标描述的位置向量，表示系绳中的张力矢量，表示张力矢量的模，表示系绳上某一点处切向量的模，为归一化坐标，ξ为空间平台的释放/回收机构出口处系绳的自然坐标，N_a表示由于系绳释放和回收造成的附近阻尼力，N_P为作用在系绳上的控制力，m_B和J_B分别表示末端操作机构的质量和转动惯量，F_B和M_B分别表示作用在末端操作机构上的控制力和控制力矩；A_B表示末端操作机构的机动加速度；其中，以系绳存放在空间平台中的那一端为系绳自然坐标计算的起点；

步骤2：系统的动力学模型的离散

步骤3：规划最优轨迹控制信号

性能指标为：其中，d_M表示力臂的长度空间绳系机器人系统在逼近过程中的动力学约束条件：

空间绳系机器人系统在逼近过程中的边界约束条件：

x(0)＝x₀,x(t_f)＝x_f

在满足约束条件的情况下，以性能指标为目标，利用hp自适应伪谱算法求解出最优轨迹(x^*,u^*)；

步骤4：协调最优控制器通过空间绳系机器人系统真实的状态信号x与步骤3计算出的理想状态信号x^*做差后得到误差信号将误差信号与时变增益协调系数矩阵K_N相乘得到偏差控制信号该偏差控制信号与步骤3计算出的理想状态信号x^*相加得到控制信号，以此实现协调控制。

2.根据权利要求1所述的一种适合空间绳系机器人逼近目标过程中的燃料最优位姿协调方法，其特征在于：在步骤1中，位置向量满足： $\stackrel{\&OverBar;}{r}(\stackrel{\&OverBar;}{s},t)=r(\stackrel{\&OverBar;}{s}{l}_0+\mathrm{\&xi;},t);$ 满足 $\stackrel{\&OverBar;}{\mathrm{\&eta;}}=\left|\right|{\stackrel{\&OverBar;}{r}}^{\&prime;}\left|\right|/l_0.$

3.根据权利要求1所述的一种适合空间绳系机器人逼近目标过程中的燃料最优位姿协调方法，其特征在于：在步骤1中，N_a满足：

$N_a=\left\{\begin{array}{cc}\mathrm{\&mu;}{\stackrel{\&CenterDot;}{\mathrm{\&xi;}}}^2{\stackrel{\&OverBar;}{\mathrm{\&eta;}}}^2\left(0\right)& \stackrel{\&CenterDot;}{\mathrm{\&xi;}}\&le;0\\ \mathrm{\&mu;}{\stackrel{\&CenterDot;}{\mathrm{\&xi;}}}^2\stackrel{\&OverBar;}{\mathrm{\&eta;}}\left(0\right)[\stackrel{\&OverBar;}{\mathrm{\&eta;}}\left(0\right)-1]& \stackrel{\&CenterDot;}{\mathrm{\&xi;}}>0\end{array}\right..$

4.根据权利要求1所述的一种适合空间绳系机器人逼近目标过程中的燃料最优位姿协调方法，其特征在于：在步骤1中，A_B满足： $A_B=\left[\begin{array}{c}{\stackrel{\&CenterDot;\&CenterDot;}{x}}_B-2\mathrm{\&omega;}{\stackrel{\&CenterDot;}{z}}_B\\ {\stackrel{\&CenterDot;\&CenterDot;}{z}}_B+2\mathrm{\&omega;}{\stackrel{\&CenterDot;}{x}}_B-3{\mathrm{\&omega;}}^2{z}_B\mathrm{}\end{array}\right],$ 其中，ω表示系统运行的轨道角速度。

5.根据权利要求1所述的一种适合空间绳系机器人逼近目标过程中的燃料最优位姿协调方法，其特征在于：在步骤1中，满足其中，加速度分量和满足：

${\stackrel{\&OverBar;}{A}}_x=\stackrel{\stackrel{\&CenterDot;\&CenterDot;}{\&OverBar;}}{x}+2(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;}{l}}_0}{l_0}{\stackrel{\stackrel{\&CenterDot;}{\&OverBar;}}{x}}^{\&prime;}+{(1-\stackrel{\&OverBar;}{s})}^2\frac{{\stackrel{\&CenterDot;}{l}}_0^2}{l_0^2}{\stackrel{\&OverBar;}{x}}^{\&prime;\&prime;}+(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;\&CenterDot;}{l}}_0}{l_0}{\stackrel{\&OverBar;}{x}}^{\&prime;}-2(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;}{l}}_0^2}{l_0^2}{\stackrel{\&OverBar;}{x}}^{\&prime;}-2\mathrm{\&omega;}[\stackrel{\stackrel{\&CenterDot;}{\&OverBar;}}{z}+\frac{{\stackrel{\&CenterDot;}{l}}_0}{l_0}(1-\stackrel{\&OverBar;}{s}){\stackrel{\&OverBar;}{z}}^{\&prime;}]$

${\stackrel{\&OverBar;}{A}}_z=\stackrel{\stackrel{\&CenterDot;\&CenterDot;}{\&OverBar;}}{z}+2(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;}{l}}_0}{l_0}{\stackrel{\stackrel{\&CenterDot;}{\&OverBar;}}{z}}^{\&prime;}+{(1-\stackrel{\&OverBar;}{s})}^2\frac{{\stackrel{\&CenterDot;}{l}}_0^2}{l_0^2}{\stackrel{\&OverBar;}{z}}^{\&prime;\&prime;}+(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;\&CenterDot;}{l}}_0}{l_0}{\stackrel{\&OverBar;}{z}}^{\&prime;}-2(1-\stackrel{\&OverBar;}{s})\frac{{\stackrel{\&CenterDot;}{l}}_0^2}{l_0^2}{\stackrel{\&OverBar;}{z}}^{\&prime;}+2\mathrm{\&omega;}[\stackrel{\stackrel{\&CenterDot;}{\&OverBar;}}{x}+\frac{{\stackrel{\&CenterDot;}{l}}_0}{l_0}(1-\stackrel{\&OverBar;}{s}){\stackrel{\&OverBar;}{x}}^{\&prime;}]-3{\mathrm{\&omega;}}^2\stackrel{\&OverBar;}{z}.$

6.根据权利要求1所述的一种适合空间绳系机器人逼近目标过程中的燃料最优位姿协调方法，其特征在于：步骤2的具体方法为：

通过线形单元的离散得到空间绳系机器人系统的运动满足：

其中，等效质量m^*和等效转动惯量I^*满足：I^*＝I_R+m_Bd²，等效力和等效力矩满足：

考虑到系绳的应变近似为0且张力是均匀分布的，因此，认为系绳的张力满足 $N=\left\{\begin{array}{cc}{N}_P+\mathrm{\&mu;}{\stackrel{\&CenterDot;}{l}}^2& \stackrel{\&CenterDot;}{l}\&GreaterEqual;0\\ N_P& \stackrel{\&CenterDot;}{l}<0\end{array}\right.$

根据系绳的张力满足条件和空间绳系机器人系统的运动所满足的条件得到空间绳系机器人系统的简化模型：其中，状态变量为：控制变量为：u＝[N_P,F_Bx,F_Bz,M_B]^T。

7.根据权利要求1所述的一种适合空间绳系机器人逼近目标过程中的燃料最优位姿协调方法，其特征在于：步骤4中的协调最优控制器根据以下方法设计：

空间绳系机器人系统的性能指标为：

空间绳系机器人系统的状态方程和边界条件为：

$\left\{\begin{array}{c}\stackrel{\stackrel{\&CenterDot;}{~}}{x}=A\left(t\right)\tilde{x}+B\left(t\right)\tilde{u}\\ \tilde{x}\left(t_0\right)=x\left(t_0\right)-x^{*}\left(t_0\right)\\ \tilde{x}\left(t_1\right)=0\end{array}\right.;$

引入Hamilton函数： $H=\frac{1}{2}{({\stackrel{\&OverBar;}{u}}^{*}+\tilde{u})}^{T}R({\stackrel{\&OverBar;}{u}}^{*}+\tilde{u})+{\mathrm{\&lambda;}}^T(A\tilde{x}+B\tilde{u}),$

通过变分计算可得协态方程为：

满足 $\&PartialD;H/\&PartialD;u=0$ 的最优控制输入为： $\tilde{u}=-R^{-1}{B}^T\left(t\right)\widetilde{\mathrm{\&lambda;}}-{\stackrel{\&OverBar;}{u}}^{*},$

根据最优性的必要条件可知，最优解满足方程：

$\left\{\begin{array}{c}\stackrel{\stackrel{\&CenterDot;}{~}}{x}=A\left(t\right)\tilde{x}-B\left(t\right)R^{-1}{B}^T\left(t\right)\widetilde{\mathrm{\&lambda;}}-B\left(t\right){\stackrel{\&OverBar;}{u}}^{*}\\ \stackrel{\stackrel{\&CenterDot;}{~}}{\mathrm{\&lambda;}}=-A^T\widetilde{\mathrm{\&lambda;}}\end{array}\right.$

和边界条件： $\left\{\begin{array}{c}\tilde{x}\left(t_0\right)=x\left(t_0\right)-x^{*}\left(t_0\right)\\ \tilde{x}\left(t_1\right)=0\end{array}\right.$

使用τ代替t并对空间绳系机器人系统满足的状态方程和边界条件以及Hamilton函数进行改性，得到 $\left\{\begin{array}{c}\frac{d\hat{x}}{\mathrm{d\&tau;}}=\frac{t_1-t_0}{2}(A\hat{x}-B{R}^{-1}{B}^T\widehat{\mathrm{\&lambda;}}-B{\hat{u}}^{*})\\ \frac{d\widehat{\mathrm{\&lambda;}}}{\mathrm{d\&tau;}}=-\frac{t_1-t_0}{2}{A}^T\widehat{\mathrm{\&lambda;}}\\ \hat{x}(-1)=\tilde{x}\left(t_0\right)\\ \hat{x}\left(1\right)=0\end{array}\right.,$

式中， $\hat{x}\left(\mathrm{\&tau;}\right)=\tilde{x}\left(t\right),\widehat{\mathrm{\&lambda;}}\left(\mathrm{\&tau;}\right)=\widetilde{\mathrm{\&lambda;}}\left(t\right),{\hat{u}}^{*}\left(\mathrm{\&tau;}\right)={\stackrel{\&OverBar;}{u}}^{*}\left(t\right),$

采用LGL点作为插值离散点，设L_N为区间[-1,1]上的N阶Legendre多项式，P_N＝{τ₀,…,τ_N}为[-1,1]内按递增顺序排列的N阶LGL点集，其中，τ₀＝-1，τ_N＝1，τ₁…τ_N-1为多项式的根，则空间绳系机器人系统的状态变量和协态变量可插值近似为：

$\left\{\begin{array}{c}\hat{x}\left(\mathrm{\&tau;}\right)\&ap;{\hat{x}}^N\left(\mathrm{\&tau;}\right)=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\hat{x}\left({\mathrm{\&tau;}}_l\right){\mathrm{\&phi;}}_l\left(\mathrm{\&tau;}\right)\\ \widehat{\mathrm{\&lambda;}}\left(\mathrm{\&tau;}\right)\&ap;{\widehat{\mathrm{\&lambda;}}}^N\left(\mathrm{\&tau;}\right)=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right){\mathrm{\&phi;}}_l\left(\mathrm{\&tau;}\right)\end{array}\right.,$ 式中， ${\mathrm{\&phi;}}_l\left(\mathrm{\&tau;}\right)=\frac{1}{N(N+1)L_N\left({\mathrm{\&tau;}}_l\right)}\frac{({\mathrm{\&tau;}}^2-1){\stackrel{\&CenterDot;}{L}}_N\left(\mathrm{\&tau;}\right)}{\mathrm{\&tau;}-{\mathrm{\&tau;}}_l},$

同时，在LGL点τ_k处状态变量微分和协态变量微分利用插值多项式近似为：

$\left\{\begin{array}{c}\frac{d\hat{x}\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}\&ap;\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\hat{x}\left({\mathrm{\&tau;}}_l\right)\frac{d{\mathrm{\&phi;}}_l\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}{D}_{\mathrm{kl}}\hat{x}\left({\mathrm{\&tau;}}_l\right)\\ \frac{d\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}\&ap;\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right)\frac{d{\mathrm{\&phi;}}_l\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}{D}_{\mathrm{kl}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right)\end{array}\right.,$

式中，D表示(N+1)×(N+1)维的矩阵，满足

$D\stackrel{\mathrm{\&Delta;}}{=}\left[D_{\mathrm{kl}}\right]=\left\{\begin{array}{cc}\frac{L_N\left({\mathrm{\&tau;}}_k\right)}{L_N\left({\mathrm{\&tau;}}_l\right)}\&CenterDot;\frac{1}{{\mathrm{\&tau;}}_k-{\mathrm{\&tau;}}_l}& k\&NotEqual;l\\ -\frac{N(N+1)}{4}& k=l=0\\ \frac{N(N+1)}{4}& k=l=N\\ 0& \mathrm{otherwise}\end{array}\right.,$

其中，k＝0和l＝0分别表示微分矩阵D的第一行和第一列，

将公式 $\left\{\begin{array}{c}\frac{d\hat{x}\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}\&ap;\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\hat{x}\left({\mathrm{\&tau;}}_l\right)\frac{d{\mathrm{\&phi;}}_l\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}{D}_{\mathrm{kl}}\hat{x}\left({\mathrm{\&tau;}}_l\right)\\ \frac{d\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}\&ap;\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right)\frac{d{\mathrm{\&phi;}}_l\left({\mathrm{\&tau;}}_k\right)}{\mathrm{d\&tau;}}=\underset{l=0}{\overset{N}{\mathrm{\&Sigma;}}}{D}_{\mathrm{kl}}\widehat{\mathrm{\&lambda;}}\left({\mathrm{\&tau;}}_l\right)\end{array}\right.$ 代入 $\left\{\begin{array}{c}\frac{d\hat{x}}{\mathrm{d\&tau;}}=\frac{t_1-t_0}{2}(A\hat{x}-B{R}^{-1}{B}^T\widehat{\mathrm{\&lambda;}}-B{\hat{u}}^{*})\\ \frac{d\widehat{\mathrm{\&lambda;}}}{\mathrm{d\&tau;}}=-\frac{t_1-t_0}{2}{A}^T\widehat{\mathrm{\&lambda;}}\\ \hat{x}(-1)=\tilde{x}\left(t_0\right)\\ \hat{x}\left(1\right)=0\end{array}\right.,$ 得到 $\left\{\begin{array}{c}{M}_{\mathrm{xx}}X+M_{\mathrm{x\&lambda;}}\mathrm{\&Lambda;}=U\\ M_{\mathrm{\&lambda;\&lambda;}}\mathrm{\&Lambda;}=0\\ \mathrm{PX}=0\end{array}\right.,$ 其中，X、Λ和U为6(N+1)维的列向量，满足：

$X={[{\hat{x}}^T\left({\mathrm{\&tau;}}_0\right),{\hat{x}}^T\left({\mathrm{\&tau;}}_1\right),...,{\hat{x}}^T\left({\mathrm{\&tau;}}_N\right)]}^T,\mathrm{\&Lambda;}={[{\widehat{\mathrm{\&lambda;}}}^T\left({\mathrm{\&tau;}}_0\right),{\widehat{\mathrm{\&lambda;}}}^T\left({\mathrm{\&tau;}}_1\right),...,{\widehat{\mathrm{\&lambda;}}}^T\left({\mathrm{\&tau;}}_N\right)]}^T,$

$U={[{\hat{u}}^{*T}\left({\mathrm{\&tau;}}_0\right),{\hat{u}}^{*T}\left({\mathrm{\&tau;}}_1\right),...,{\hat{u}}^{*T}\left({\mathrm{\&tau;}}_N\right)]}^T$

矩阵M_xx、M_xλ和M_λλ为6(N+1)×6(N+1)维的分块矩阵，其分块满足：

${\left[M_{\mathrm{xx}}\right]}_{\mathrm{ij}}=\left\{\begin{array}{cc}{D}_{\mathrm{ij}}{I}_6& i\&NotEqual;j\\ D_{\mathrm{ij}}{I}_6-\frac{t_1-t_0}{2}A\left({\mathrm{\&tau;}}_i\right)& i=j\end{array}\right.,{\left[M_{\mathrm{xx}}\right]}_{\mathrm{ij}}=\left\{\begin{array}{cc}{0}_{6\&times;6}& i\&NotEqual;j\\ \frac{t_1-t_0}{2}B\left({\mathrm{\&tau;}}_i\right)R^{-1}B{\left({\mathrm{\&tau;}}_i\right)}^T& i=j\end{array}\right.,$

${\left[M_{\mathrm{xx}}\right]}_{\mathrm{ij}}=\left\{\begin{array}{cc}{D}_{\mathrm{ij}}{I}_6& i\&NotEqual;j\\ D_{\mathrm{ij}}{I}_6+\frac{t_1-t_0}{2}{A}^T\left({\mathrm{\&tau;}}_i\right)& i=j\end{array}\right.,$

矩阵P为6×6(N+1)维的矩阵，满足：P＝[0_6×6,…,0_6×6,I₆]，

将公式 $\left\{\begin{array}{c}{M}_{\mathrm{xx}}X+M_{\mathrm{x\&lambda;}}\mathrm{\&Lambda;}=U\\ M_{\mathrm{\&lambda;\&lambda;}}\mathrm{\&Lambda;}=0\\ \mathrm{PX}=0\end{array}\right.$ 写成矩阵形式：VZ＝Y，

其中， $V=\left[\begin{array}{cc}{M}_{\mathrm{xx}}& M_{\mathrm{x\&lambda;}}\\ 0& M_{\mathrm{\&lambda;\&lambda;}}\\ P& 0\end{array}\right],Z=\left[\begin{array}{c}X\\ \mathrm{\&Lambda;}\end{array}\right],Y=\left[\begin{array}{c}U\\ 0_{6(N+1)\&times;1}\\ 0_{6\&times;1}\end{array}\right],$

将矩阵V分成可得其中，V₀和V_e分别表示矩阵V的前6列和其它所有列，列向量Z_e表示向量Z的后12(N+1)行。由(58)式求解Z_e的最小二乘解为： $Z_e={\left(V_e^T{V}_e\right)}^{-1}{V}_e^T[Y-V_0\hat{x}\left({\mathrm{\&tau;}}_0\right)],$

构造中间变量 ${\hat{Z}}_e\stackrel{\mathrm{\&Delta;}}{=}{Z}_e-Z_e{|}_{\hat{x}\left({\mathrm{\&tau;}}_0\right)=0}=-{\left(V_e^T{V}_e\right)}^{-1}{V}_e^T{V}_0\hat{x}\left({\mathrm{\&tau;}}_0\right)$

取出中间变量的6N+1～6N+6行，得到式中，系数矩阵K_λ表示矩阵的6N+1～6N+6行；

定义时变增益协调系数矩阵K_N，令则控制量为：将t₀设置为系统当前运行的时刻t，令t₁＝min{t₀+10,t_f}，即可得到协调控制器。