CN104049637A - 一种空间绳系机器人三轴主动姿态控制方法 - Google Patents

Abstract

本发明提出了一种空间绳系机器人三轴主动姿态控制方法，根据空间绳系机器人的轨道动力学特性计算空间绳系机器人在平衡状态空间系绳应具有的标称张力，根据空间绳系机器人姿态运动学与动力学方程确定控制力矩变量，建立空间绳系机器人姿态动力学的状态方程和输出方程并根据反馈线性化控制规律计算出三个方向的姿态控制力矩，根据控制力矩与空间系绳连接点位置、反作用轮转速之间的关系得出相应的空间系绳连接点位置及反作用轮的转动角速度。该方法根据目标的姿态利用空间系绳及反作用轮对空间绳系机器人的姿态实施三轴姿态协调控制，适用于空间绳系机器人位于空间平台与地心连线时对空间绳系机器人的三轴姿态控制。

Description

一种空间绳系机器人三轴主动姿态控制方法

技术领域

本发明涉及航天飞行器控制技术领域，具体为一种空间绳系机器人三轴主动姿态控制方法。 

背景技术

空间绳系机器人是一种新型的空间机器人，主要用于捕获空间目标并进行在轨维修、在轨组装等任务。空间绳系机器人的一般架构为“空间平台-空间系绳-空间绳系机器人”，空间平台通过空间系绳释放空间绳系机器人，空间绳系机器人逼近空间目标、抓捕；空间绳系机器人上自带操作机械臂及末端操作手用于执行捕获目标及在轨服务任务。空间绳系机器人的空间绳系机器人在抓捕捕获目标前，必须根据目标状态的变化调整自身三轴姿态以便准确地确定抓捕位置进行抓捕。 

传统的卫星或空间机器人的姿态控制系统主要分为被动姿态控制稳定系统和主动姿态控制系统；被动姿态稳定控制系统主要利用重力梯度或卫星的自旋特性实现被动姿态稳定控制，传统的主动姿态控制系统主要包括喷气三轴控制系统、飞轮三轴姿态控制系统及磁力矩器三轴稳定系统。对于卫星的姿态控制方法，申请号：201310036385.2的专利公开了一种卫星轨道和姿态控制方法，该方法适用于在轨喷气过程有大干扰力矩的卫星，在确定轨道干扰力矩的前提下提出了姿态喷气控制方式和角动量交换控制的方法；授权公告号：CN100451898C的专利提出一种微小卫星的姿态控制方法及系统，该发明采用以磁力矩器主动磁控为主，结合重力梯度杆与动量轮偏置稳定的控制系统作为卫星姿态稳定平台；授权公告号：CN101554926B公开了一种航天器姿态控制系统及方法，该发明所述的控制系统仅有一个偏置动量轮、一套三轴磁力矩器及存载算法的姿态控制器，该方法包括速率阻尼控制步骤、初始捕获控制步骤及稳定控制步骤；授权公告号：CN101934863B公开了一种基于磁力矩器和飞轮的卫星姿态全方位控制方法，该发明涉及一种利用磁力矩器和飞轮完成卫星入轨阶段全方位姿态控制方法，解决了现有卫星姿态全方位控制技术可靠性低、寿命短的问题。可见传统卫星姿态控制系统基本采用传统执行机构（如推力器、飞轮、磁力矩器等） 来实现卫星的三轴姿态稳定控制。 

空间绳系机器人作为一种新型的空间机器人，空间系绳的存在导致其结构不同于传统的卫星或空间机器人，因此其姿态控制器的设计也与传统的卫星或空间机器人不同。对于空间绳系机器人姿态控制系统，申请号：201210540460.1公开了一种可移动系绳点的空间绳系机器人的逼近姿态协调控制方法，该方法设计了系绳控制机构，通过改变系绳点的位置改变拉力的方向产生需要的姿态控制力矩；申请号：201310018221.7公开了一种空间绳系机器人目标抓捕后复合体姿态协调控制方法，该方法引进了系绳拉力，设计了系绳拉力和推力器的协调姿态控制方法。但这两个专利对于协调姿态控制方法的实现需要对系绳上拉力保持恒定或进行控制，较难实现。 

发明内容

技术方案 

本发明的目的在于克服姿态协调控制过程中的系绳拉力控制问题，针对空间绳系机器人的姿态控制问题，提出一种该类机器人的三轴姿态控制方法，该方法的特点是根据目标的姿态利用空间系绳及反作用轮对空间绳系机器人的姿态实施三轴姿态协调控制，该方法结合了被动姿态控制方法和主动姿态控制策略，空间系绳中的张力被动产生，无需控制，该方法适用于空间绳系机器人位于空间平台与地心连线时对空间绳系机器人的三轴姿态控制，该方法的主要步骤包括根据空间绳系机器人的轨道动力学特性计算空间绳系机器人在平衡状态（空间绳系机器人位于空间平台与地心连线上）空间系绳应具有的标称张力，根据空间绳系机器人姿态运动学与动力学方程确定控制力矩变量，建立空间绳系机器人姿态动力学的状态方程和输出方程并根据反馈线性化控制规律计算出三个方向的姿态控制力矩，根据控制力矩与空间系绳连接点位置、反作用轮转速之间的关系得出相应的空间系绳连接点位置及反作用轮的转动角速度。 

本发明的技术方案为： 

所述一种空间绳系机器人三轴主动姿态控制方法，其特征在于：采用以下步骤： 

步骤1：计算空间绳系机器人三个方向的姿态控制力矩u_x、u_y及u_z： 

$\left\{\begin{array}{c}{u}_x=-I_x(-\frac{I_r}{I_y}{x}_7{x}_4+(1-n_x)\mathrm{\Ω}{x}_6-n_x{\mathrm{\Ω}}^2{x}_1+\frac{I_r}{I_x}\mathrm{\Ω}{x}_7)+v_x\\ u_y=-I_y(\frac{I_r}{I_y}{x}_7{x}_2-\frac{I_r}{I_y}\mathrm{\Ω}{x}_7{x}_5)+v_y\\ u_z=-I_z((n_z-1)\mathrm{\Ω}{x}_2-n_z{\mathrm{\Ω}}^2{x}_5+\frac{\tan x_3}{\cos x_1{I}_z}{u}_x-\frac{\tan x_1}{I_z}{u}_y)+v_z\end{array}\right.$

其中x₁＝φ、x₃＝θ、x₅＝ψ、x₇＝Ω_r，φ、θ及ψ为空间绳系机器人的三轴姿态角，I_x、I_y、I_z分别为空间绳系机器人转动惯量，Ω_r为反作用轮转动角速度，I_r为反作用轮的转动惯量，n_x和n_z为系数，Ω为空间绳系机器人的轨道角速度，  $v_x={\stackrel{\·\·}{\mathrm{\φ}}}_d-k_1(\mathrm{\φ}-{\mathrm{\φ}}_d)-k_2(\stackrel{\·}{\mathrm{\φ}}-{\stackrel{\·}{\mathrm{\φ}}}_d),v_y={\stackrel{\·\·}{\mathrm{\θ}}}_d-k_3(\mathrm{\θ}-{\mathrm{\θ}}_d)-k_4(\stackrel{\·}{\mathrm{\θ}}-{\stackrel{\·}{\mathrm{\θ}}}_d)$ 及 $v_z={\stackrel{\·\·}{\mathrm{\ψ}}}_d-k_5(\mathrm{\ψ}-{\mathrm{\ψ}}_d)-k_6(\stackrel{\·}{\mathrm{\ψ}}-{\stackrel{\·}{\mathrm{\ψ}}}_d),$ φ_d、θ_d及ψ_d分别为空间绳系机器人的指令滚转角、指令俯仰角及指令偏航角，k₁、k₂、k₃、k₄、k₅及k₆为正值常数； 

步骤2：根据步骤1得到的空间绳系机器人三个方向的姿态控制力矩u_x、u_y及u_z，由公式 

$\left\{\begin{array}{c}{M}_x=u_x+M_{\mathrm{dx}}=-\sin \mathrm{\φ}\cos \mathrm{\θT}{z}_L+\cos \mathrm{\φ}\cos \mathrm{\θT}{y}_L+M_{\mathrm{dx}}\\ M_y=u_y+M_{\mathrm{dy}}=-\sin \mathrm{\θT}{z}_L-\cos \mathrm{\φ}\cos \mathrm{\θT}{x}_L+M_{\mathrm{dy}}\\ M_z=-I_r{\stackrel{\·}{\mathrm{\Ω}}}_r+\frac{\tan \mathrm{\θ}}{\cos \mathrm{\φ}}{u}_x-\tan \mathrm{\φ}{u}_y+M_{\mathrm{dz}}\\ T=T_0-m_r\stackrel{\·\·}{l}\\ \stackrel{\·\·}{l}=|(-\cos \mathrm{\φ}\cos \mathrm{\θ}{y}_L+\sin \mathrm{\φ}\cos \mathrm{\θ}{z}_L)\stackrel{\·\·}{\mathrm{\φ}}+(\cos \mathrm{\θ}{x}_L+\sin \mathrm{\φ}\sin \mathrm{\θ}{y}_L+\cos \mathrm{\φ}\sin \mathrm{\θ}{z}_L)\stackrel{\·\·}{\mathrm{\θ}}\\ +\sin \mathrm{\θ}{\stackrel{\·\·}{x}}_L-\sin \mathrm{\φ}\cos \mathrm{\θ}{\stackrel{\·\·}{y}}_L+(-\sin \mathrm{\θ}{x}_L+\sin \mathrm{\φ}\cos \mathrm{\θ}{y}_L+\cos \mathrm{\φ}\cos \mathrm{\θ}{z}_L){\stackrel{\·}{\mathrm{\θ}}}^2\\ +(\sin \mathrm{\φ}\cos \mathrm{\θ}{y}_L+\cos \mathrm{\φ}\cos \mathrm{\θ}{z}_L){\stackrel{\·}{\mathrm{\φ}}}^2+2\cos \mathrm{\θ}\stackrel{\·}{\mathrm{\θ}}{\stackrel{\·}{x}}_L-2\cos \mathrm{\φ}\cos \mathrm{\θ}{\stackrel{\·}{y}}_L\stackrel{\·}{\mathrm{\φ}}\\ +(2\cos \mathrm{\φ}\sin \mathrm{\θ}{y}_L-2\sin \mathrm{\φ}\sin \mathrm{\θ}{z}_L+2\sin \mathrm{\φ}\sin \mathrm{\θ})\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\θ}}+2\sin \mathrm{\φ}\sin \mathrm{\θ}{\stackrel{\·}{y}}_L\stackrel{\·}{\mathrm{\θ}}|\end{array}\right.$

得到空间系绳连接点位置x_L、y_L及反作用轮的转动角速度Ω_r实现空间绳系机器人三轴姿态的主动控制；其中M_dx、M_dy和M_dz分别为空间绳系机器人三个轴向的干扰力矩，M_x、M_y和M_z为空间绳系机器人所受到的力矩，T为空间系绳上的张力，T₀为空间绳系机器人在平衡状态时，空间系绳应具有的标称张力，T₀＝3Ω²m_rlm_r空间绳系机器人的质量，l为空间系绳的长度；所述平衡状态指空间绳系机器人位于空间平台与地心连线上。 

有益效果 

本发明提出的空间绳系机器人三轴姿态控制方法，根据目标的姿态利用空间系绳及反作用轮对空间绳系机器人的姿态实施三轴姿态协调控制，该方法结合了被动姿态 控制方法和主动姿态控制策略，空间系绳中的张力被动产生，无需控制，该方法适用于空间绳系机器人位于空间平台与地心连线时对空间绳系机器人的三轴姿态控制。 

附图说明

图1为本发明所应用的空间绳系机器人结构及各部分相对位置图。 

图2为本发明所应用空间绳系机器人的空间系绳在空间绳系机器人上连接点移动示意图。 

图3为本发明所应用空间绳系机器人轨道面内姿态变化时空间系绳长度变化图。 

图4为本发明控制流程图。 

其中，1表示空间平台，2表示空间系绳，3表示空间绳系机器人，4表示空间平台运动轨道，5表示空间绳系机器人运动轨道，6表示空间系绳在空间绳系机器人上的连接点，7表示空间系绳连接点移动平面 

图1中oxyz为空间绳系机器人轨道坐标系，o_cx_cy_cz_c为空间绳系机器人轨道坐标系，空间平台与空间绳系机器人的连线（空间系绳）方向通过地心。 

图2中o_c′x_c′y_c′z_c′为空间绳系机器人本体坐标系，反作用轮安装在空间绳系机器人的o_c′z_c′轴上，空间系绳在空间绳系机器人上的连接点6可以在空间绳系机器人连接点移动平面7上移动。 

图3中θ表示空间绳系机器人在其运行轨道面内俯仰姿态角，O表示初始空间系绳连接点位置，L表示在控制过程中空间系绳连接点位置，空间系绳名义长度变化量可以表示为|LL′|-|OO_c′|。 

具体实施方式

下面结合附图对本发明的实施方式进行说明，本实施例中空间绳系机器人位于空间平台和地心连线上。 

在分析过程中，首先由空间绳系机器人的轨道动力学特性给出空间绳系机器人在平衡状态（空间绳系机器人位于空间平台与地心连线上）空间系绳应具有的标称张力T₀； 

在空间绳系机器人轨道坐标系oxyz中，空间绳系机器人的运动可以由Hill方程 （式(1)～(3)）表示： 

$\stackrel{\·\·}{x}-2\mathrm{\Ω}\stackrel{\·}{z}=F_x/m_r---\left(1\right)$

$\stackrel{\·\·}{y}+{\mathrm{\Ω}}^{2}y=F_y/m_r---\left(2\right)$

$\stackrel{\·\·}{z}+2\mathrm{\Ω}\stackrel{\·}{x}-3{\mathrm{\Ω}}^{2}z=F_z/m_r---\left(3\right)$

式(1)～(3)中x、y及z分别为空间绳系机器人在空间绳系机器人轨道坐标系oxyz（x轴为目标轨道切线方向，z轴沿地球半径方向指向地心，y轴垂直与x轴和z轴，并且满足右手定则）中的位置，及分别为空间绳系机器人在空间绳系机器人轨道坐标系oxyz中的速度，及分别为空间绳系机器人在空间绳系机器人轨道坐标系oxyz中的加速度，m_r为空间绳系机器人的质量，F_x、F_y及F_z分别为空间绳系机器人上所受的空间绳系机器人轨道坐标系oxyz下的三个方向的外力，Ω为空间绳系机器人的轨道角速度，可以由式(4)所表示。 

$\mathrm{\Ω}=\sqrt{\mathrm{\μ}/{R_0}^3}---\left(4\right)$

式(4)中μ＝3.986005×10¹⁴m³/s²，R₀为空间绳系机器人的轨道半径。 

空间绳系机器人的平衡状态为：x＝y＝0，z＝±l（l为空间系绳的长度），及在这种情况下由式(1)～(3)可以得出： 

F_x＝0,F_y＝0,F_z＝T₀≈3Ω²m_rl (5) 

从而得到空间绳系机器人在平衡状态时空间系绳上的张力T₀。 

其次根据空间绳系机器人姿态运动学与动力学方程确定控制力矩变量： 

设空间绳系机器人轨道坐标系为o_cx_cy_cz_c，本体坐标系为o_c′x_c′y_c′z_c′，o_cx_cz_c为空间绳系机器人运行的轨道平面，坐标轴o_cx_c指向空间绳系机器人轨道切向方向，坐标轴o_cz_c指向地心，空间系绳张力在空间绳系机器人轨道坐标系中可以表示为系绳连接点L在空间绳系机器人本体坐标系o_c′x_c′y_c′z_c′中的位置可以表示为 则空间系绳张力产生的对空间绳系机器人的控制力矩Q_T可以表示为： 

式(6)中 为空间绳系机器人轨道坐标系o_cx_cy_cz_c到本体坐标系o_c′x_c′y_c′z_c′的转化矩阵，可以表示为： 

$C_o^b=\begin{array}{}\end{array}\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]---\left(7\right)$

式(7)中φ为空间绳系机器人绕o_cx_c旋转的滚转姿态角，θ为绕o_cy_c旋转的俯仰姿态角，ψ为绕o_cz_c旋转的偏航姿态角。则式(6)可以写为： 

$Q_T=\left[\begin{array}{c}-\sin \mathrm{\φ}\cos \mathrm{\θT}{z}_L+\cos \mathrm{\φ}\cos \mathrm{\θT}{y}_L\\ -\sin \mathrm{\θT}{z}_L-\cos \mathrm{\φ}\cos \mathrm{\θT}{x}_L\\ \sin \mathrm{\θT}{y}_L+\sin \mathrm{\φ}\cos \mathrm{\θT}{x}_L\end{array}\right]---\left(8\right)$

根据欧拉转动顺序“3-2-1”，将空间绳系机器人的空间旋转角速度ω在本体坐标系o_c′x_c′y_c′z_c′下的分量ω_x、ω_y及ω_z用旋转欧拉角φ、θ及ψ表示得到空间绳系机器人的姿态运动学方程为： 

$\stackrel{\·}{\mathrm{\φ}}-\sin \mathrm{\θ}\stackrel{\·}{\mathrm{\ψ}}={\mathrm{\ω}}_x---\left(9\right)$

$\cos \mathrm{\φ}\stackrel{\·}{\mathrm{\θ}}+\sin \mathrm{\φ}\cos \mathrm{\θ}\stackrel{\·}{\mathrm{\ψ}}={\mathrm{\ω}}_y---\left(10\right)$

$\cos \mathrm{\φ}\cos \mathrm{\θ}\stackrel{\·}{\mathrm{\ψ}}-\sin \mathrm{\φ}\stackrel{\·}{\mathrm{\θ}}={\mathrm{\ω}}_z---\left(11\right)$

设空间绳系机器人的动量矩为H，M为空间绳系机器人所受到的外力矩（包括控制力矩和干扰力矩），根据动量矩定理，满足以下关系式： 

$M=\frac{\mathrm{dH}}{\mathrm{dt}}=\stackrel{\·}{H}+{\mathrm{\ω}}^{\×}H---\left(12\right)$

式(12)中 $M=\left[\begin{array}{ccc}{M}_x& M_y& M_z\end{array}\right],H=\left[\begin{array}{ccc}{h}_x& h_y& h_z\end{array}\right],{\mathrm{\ω}}^{\×}=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right].$

设空间绳系机器人的惯量矩阵 $I=\left[\begin{array}{ccc}{I}_x& -I_{\mathrm{xy}}& -I_{\mathrm{xz}}\\ -I_{\mathrm{xy}}& I_y& -I_{\mathrm{yz}}\\ -I_{\mathrm{xz}}& -I_{\mathrm{yz}}& I_z\end{array}\right],$ 其中I_x、I_y、I_z分别为空间绳系机器人绕坐标轴o_c′x_c′、o_c′y_c′及o_c′z_c′的转动惯量；I_xy、I_yz、I_xz为惯量积；它们可以分别表示为： 

$\begin{array}{ccc}{I}_x={\∫}_0^{m_r}(y^2+z^2)\mathrm{dm}& I_y={\∫}_0^{m_r}(x^2+z^2)\mathrm{dm}& I_z={\∫}_0^{m_r}(y^2+x^2)\mathrm{dm}\end{array}---\left(13\right)-$

$\begin{array}{ccc}{I}_{\mathrm{xy}}={\∫}_0^{m_r}\left(\mathrm{xy}\right)\mathrm{dm}& I_{\mathrm{yz}}={\∫}_0^{m_r}\left(\mathrm{yz}\right)\mathrm{dm}& I_{\mathrm{xz}}={\∫}_0^{m_r}\left(\mathrm{xz}\right)\mathrm{dm}\end{array}---\left(14\right)$

若选取o_c′x_c′、o_c′y_c′及o_c′z_c′为空间绳系机器人的主轴量轴，则I_xy＝0、I_yz＝0、I_xz＝0， 此时空间绳系机器人的动量矩可以表示为： 

$\left\{\begin{array}{c}{h}_x=I_x{\mathrm{\ω}}_x\\ h_y=I_y{\mathrm{\ω}}_y\\ h_z=I_z{\mathrm{\ω}}_z+I_r{\mathrm{\Ω}}_r\end{array}\right.---\left(15\right)$

式(15)中I_r为安装在o_c′z_c′轴上反作用轮的转动惯量，Ω_r为反作用轮转动角速度。 

将式(15)代入式(12)即可得到空间绳系机器人的姿态动力学方程： 

$\left\{\begin{array}{c}{M}_x=I_x{\stackrel{\·}{\mathrm{\ω}}}_x+{\mathrm{\ω}}_y{\mathrm{\ω}}_z(I_z-I_y)+{\mathrm{\ω}}_y{I}_r{\mathrm{\Ω}}_r\\ M_y=I_y{\stackrel{\·}{\mathrm{\ω}}}_y+{\mathrm{\ω}}_x{\mathrm{\ω}}_z(I_x-I_z)-{\mathrm{\ω}}_x{I}_r{\mathrm{\Ω}}_r\\ M_z=I_z{\stackrel{\·}{\mathrm{\ω}}}_z+I_r{\stackrel{\·}{\mathrm{\Ω}}}_r+{\mathrm{\ω}}_x{\mathrm{\ω}}_y(I_y-I_x)\end{array}\right.---\left(16\right)$

根据空间绳系机器人复合运动关系，空间旋转角速度ω等于空间绳系机器人本体坐标系o_c′x_c′y_c′z_c′相对于空间绳系机器人质心轨道坐标系o_cx_cy_cz_c的旋转角速度ω_r与质心轨道坐标系o_cx_cy_cz_c相对于惯性坐标系的牵连角速度矢量ω_e之和。 

ω＝ω_r+ω_e   (17) 

将式(17)投影至空间绳系机器人本体坐标系o_c′x_c′y_c′z_c′上有： 

$\mathrm{\ω}={\mathrm{\ω}}_r+C_o^b{\mathrm{\ω}}_e---\left(18\right)$

式(18)中 ${\mathrm{\ω}}_r={\left[\begin{array}{ccc}\stackrel{\·}{\mathrm{\φ}}& \stackrel{\·}{\mathrm{\θ}}& \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]}^T,$ 为空间绳系机器人轨道坐标系到本体坐标系的转化矩阵，ω_e＝[0 -Ω 0]^T。 

在小角度假设条件下，即：|φ|1rad、|θ|1rad和|ψ|1rad，式(18)可以写为： 

$\mathrm{\ω}=\left[\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}\\ \stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]+\left[\begin{array}{ccc}1& \mathrm{\ψ}& \mathrm{\θ}\\ -\mathrm{\ψ}& 1& \mathrm{\φ}\\ \mathrm{\θ}& -\mathrm{\φ}& 1\end{array}\right]\left[\begin{array}{c}0\\ -\mathrm{\Ω}\\ 0\end{array}\right]---\left(19\right)$

将式(19)代入式(16)并忽略二次项即得空间绳系机器人姿态动力学方程的最终表达形式： 

$\stackrel{\·\·}{\mathrm{\φ}}+\frac{I_r}{I_x}{\mathrm{\Ω}}_r\stackrel{\·}{\mathrm{\θ}}-(1-n_x)\mathrm{\Ω}\stackrel{\·}{\mathrm{\ψ}}+n_x{\mathrm{\Ω}}^2\mathrm{\φ}=\frac{I_r}{I_x}\mathrm{\Ω}{\mathrm{\Ω}}_r+\frac{M_x}{I_x}---\left(20\right)$

$\stackrel{\·\·}{\mathrm{\θ}}-\frac{I_r}{I_y}{\mathrm{\Ω}}_r\stackrel{\·}{\mathrm{\φ}}+\frac{I_r}{I_y}\mathrm{\Ω}{\mathrm{\Ω}}_r\mathrm{\ψ}=\frac{M_y}{I_y}---\left(21\right)$

$\stackrel{\·\·}{\mathrm{\ψ}}+(1-n_z)\mathrm{\Ω}\stackrel{\·}{\mathrm{\φ}}+n_z{\mathrm{\Ω}}^2\mathrm{\ψ}=-\frac{I_r}{I_z}{\stackrel{\·}{\mathrm{\Ω}}}_r+\frac{M_{\mathrm{dz}}}{I_z}---\left(22\right)$

式(20)～(22)中M_x＝Q_Tx+M_dx、M_y＝Q_Ty+M_dy及其中M_dx、M_dy及M_dz分别为空间绳系机器人三个轴向的干扰力矩，而Q_Tx、Q_Ty及Q_Tz分别确定为空间绳系机器人三个轴向力矩控制变量。那么在实际控制中就需要直接计算空间绳系机器人三个轴向力矩控制变量。 

所以建立空间绳系机器人姿态动力学的状态方程和输出方程并根据反馈线性化控制规律计算出三个方向的姿态控制力矩： 

将式(20)～(22)中相应变量写为状态变量的形式即设：x₁＝φ、x₃＝θ、x₅＝ψ、x₇＝Ω_r及u＝[u_x u_y u_z]^T，其中u_x＝Q_Tx、u_y＝Q_Ty及u_z＝Q_Tz，则空间绳系机器人姿态动力学方程用状态方程形式表示为： 

${\stackrel{\·}{x}}_1=x_2---\left(23\right)$

${\stackrel{\·}{x}}_2=-\frac{I_r}{I_x}{x}_7{x}_4+(1-n_x)\mathrm{\Ω}{x}_6-n_x{\mathrm{\Ω}}^2{x}_1+\frac{I_r}{I_x}\mathrm{\Ω}{x}_7+\frac{u_x}{I_x}+\frac{M_{\mathrm{dx}}}{I_x}---\left(24\right)$

${\stackrel{\·}{x}}_3=x_4---\left(25\right)$

${\stackrel{\·}{x}}_4=\frac{I_r}{I_y}{x}_7{x}_2-\frac{I_r}{I_y}\mathrm{\Ω}{x}_7{x}_5+\frac{u_y}{I_y}+\frac{M_{\mathrm{dy}}}{I_y}---\left(26\right)$

${\stackrel{\·}{x}}_5=x_6---\left(27\right)$

${\stackrel{\·}{x}}_6=(n_z-1)\mathrm{\Ω}{x}_2-n_z{\mathrm{\Ω}}^2{x}_5+\frac{\tan x_3}{\cos x_1{I}_z}{u}_x-\frac{\tan x_1}{I_z}{u}_y+\frac{u_z}{I_z}+\frac{M_{\mathrm{dz}}}{I_z}---\left(28\right)$

${\stackrel{\·}{x}}_7=-\frac{u_z}{I_r}---\left(29\right)$

设状态变量X＝[x₁ x₂ x₃ x₄ x₅ x₆ x₇]^T，系统输出变量Y＝[φ θ ψ]^T，则系统状态方程和输出方程可以分别写为： 

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{Bu}+d---\left(30\right)$

Y＝GX (31) 

由于该系统为非线性系统，所以式(30)及(31)中矩阵A不是唯一的，这里给出一个矩阵A的数学表达： 

$A=\left[\begin{array}{ccccccc}0& 1& 0& 0& 0& 0& 0\\ -n_x{\mathrm{\Ω}}^2{x}_1& 0& 0& -\frac{I_r}{I_x}{x}_7& 0& (1-n_x)\mathrm{\Ω}& \frac{I_r}{I_x}\mathrm{\Ω}\\ 0& 0& 0& 1& 0& 0& 0\\ 0& \frac{I_r}{I_y}{x}_7& 0& 0& -\frac{I_r}{I_y}\mathrm{\Ω}{x}_7& 0& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& (n_z-1)\mathrm{\Ω}& 0& 0& -n_z{\mathrm{\Ω}}^2& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\end{array}\right]---\left(32\right)$

式(30)及(31)中，B及G是唯一确定的，d为系统的干扰，它们可以表示为： 

$B=\left[\begin{array}{ccc}0& 0& 0\\ \frac{1}{I_x}& 0& 0\\ 0& 0& 0\\ 0& \frac{1}{I_y}& 0\\ 0& 0& 0\\ \frac{\tan x_3}{\cos x_1{I}_z}& -\frac{\tan x_1}{I_z}& \frac{1}{I_z}\\ 0& 0& -\frac{1}{I_r}\end{array}\right]G=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0\end{array}\right]d=\left[\begin{array}{c}0\\ \frac{M_{\mathrm{dx}}}{I_x}\\ 0\\ \frac{M_{\mathrm{dy}}}{I_y}\\ 0\\ \frac{M_{\mathrm{dz}}}{I_z}\\ 0\end{array}\right]---\left(33\right)$

对输出变量Y＝[φ θ ψ]^T求二阶时间导数并结合空间绳系机器人姿态动力学方程得： 

$\stackrel{\·\·}{Y}=\left[\begin{array}{c}\stackrel{\·\·}{\mathrm{\φ}}\\ \stackrel{\·\·}{\mathrm{\θ}}\\ \stackrel{\·\·}{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{c}-\frac{I_r}{I_x}{x}_7{x}_4+(1-n_x)\mathrm{\Ω}{x}_6-n_x{\mathrm{\Ω}}^2{x}_1+\frac{I_r}{I_x}\mathrm{\Ω}{x}_7+\frac{u_x}{I_x}+\frac{M_{\mathrm{dx}}}{I_x}\\ \frac{I_r}{I_y}{x}_7{x}_2-\frac{I_r}{I_y}\mathrm{\Ω}{x}_7{x}_5+\frac{u_y}{I_y}+\frac{M_{\mathrm{dy}}}{I_y}\\ (n_z-1)\mathrm{\Ω}{x}_2-n_z{\mathrm{\Ω}}^2{x}_5+\frac{\tan x_3}{\cos x_1{I}_z}{u}_x-\frac{\tan x_1}{I_z}{u}_y+\frac{u_z}{I_z}+\frac{M_{\mathrm{dz}}}{I_z}\end{array}\right]---\left(34\right)$

利用“输入—输出”反馈线性化的思想设计控制律： 

$u_x=-I_x(-\frac{I_r}{I_x}{x}_7{x}_4+(1-n_x)\mathrm{\Ω}{x}_6-n_x{\mathrm{\Ω}}^2{x}_1+\frac{I_r}{I_x}\mathrm{\Ω}{x}_7)+v_x---\left(35\right)$

$u_y=-I_y(\frac{I_r}{I_y}{x}_7{x}_2-\frac{I_r}{I_y}\mathrm{\Ω}{x}_7{x}_5)+v_y---\left(36\right)$

$u_y=-I_z((n_z-1)\mathrm{\Ω}{x}_2-n_z{\mathrm{\Ω}}^2{x}_5+\frac{\tan x_3}{\cos x_1{I}_z}{u}_x-\frac{\tan x_1}{I_z}{u}_y)+v_z---\left(37\right)$

式(35)～(37)所设计的控制律中 $v_x={\stackrel{\·\·}{\mathrm{\φ}}}_d-k_1(\mathrm{\φ}-{\mathrm{\φ}}_d)-k_2(\stackrel{\·}{\mathrm{\φ}}-{\stackrel{\·}{\mathrm{\φ}}}_d),v_y={\stackrel{\·\·}{\mathrm{\θ}}}_d-k_3(\mathrm{\θ}-{\mathrm{\θ}}_d)-k_4(\stackrel{\·}{\mathrm{\θ}}-{\stackrel{\·}{\mathrm{\θ}}}_d)$ 及 其中φ_d、θ_d及ψ_d分别为空间绳系机器人的指令滚转角、指令俯仰角及指令偏航角，及分别为空间绳系机器人的指令滚转角速度、指令俯 仰角速度及指令偏航角速度。 

将所设计的控制律式(35)～(37)代入式(34)中得： 

${\stackrel{\·\·}{e}}_{\mathrm{\φ}}+k_2{\stackrel{\·}{e}}_{\mathrm{\φ}}+k_1{e}_{\mathrm{\φ}}=\frac{M_{\mathrm{dx}}}{I_x}---\left(38\right)$

${\stackrel{\·\·}{e}}_{\mathrm{\θ}}+k_4{\stackrel{\·}{e}}_{\mathrm{\θ}}+k_3{e}_{\mathrm{\θ}}=\frac{M_{\mathrm{dy}}}{I_y}---\left(39\right)$

${\stackrel{\·\·}{e}}_{\mathrm{\ψ}}+k_6{\stackrel{\·}{e}}_{\mathrm{\ψ}}+k_5{e}_{\mathrm{\ψ}}=\frac{M_{\mathrm{dz}}}{I_z}---\left(40\right)$

式(38)～(40)中e_φ＝φ-φ_d，e_θ＝θ-θ_d，e_ψ＝ψ-ψ_d；k₁、k₂、k₃、k₄、k₅及k₆为正常数。 

式(35)～(37)所设计的控制律中所计算出的u_x、u_y及u_z即为空间绳系机器人三个轴向的姿态控制力矩。 

在得到空间绳系机器人三个轴向的姿态控制力矩后，要转化为实际的控制量，即空间系绳连接点位置、反作用轮转速，所以下面根据控制力矩与空间系绳连接点位置、反作用轮转速之间的关系得出相应的空间系绳连接点位置及反作用轮的转动角速度： 

由式(8)及空间绳系机器人姿态动力学方程式(20)～(22)得出空间绳系机器人所受的外力矩为： 

M_x＝u_x+M_dx＝-sinφcosθTz_L+cosφcosθTy_L+M_dx (41) 

M_y＝u_y+M_dy＝-sinθTz_L-cosφcosθTx_L+M_dy (42) 

$M_z=-I_r{\stackrel{\·}{\mathrm{\Ω}}}_r+\frac{\tan \mathrm{\θ}}{\cos \mathrm{\φ}}{u}_x-\tan \mathrm{\φ}{u}_y+M_{\mathrm{dz}}---\left(43\right)$

x_L、y_L及z_L分别为系绳连接点在o_c′x_c′、o_c′y_c′及o_c′z_c′轴向的移动量，一般情况下，空间系绳在空间绳系机器人上的连接点只能在一个平面内移动（即朝两个方向移动），另外一个方向的移动量是固定不变的，如果设z_L是固定不变的，则系绳连接点在o_c′x_c′y_c′z_c′上的坐标在o_cx_cy_cz_c中可以表示为： 

$\begin{array}{c}{A}_b=C_b_o^T{}_o{[x_L,y_L,z_L]}^T=\\ \left[\begin{array}{c}\left(\cos \mathrm{\θ}\cos \mathrm{\ψ}\right)x_L+(\sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ})y_L+(\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ})z_L\\ \left(\cos \mathrm{\θ}\sin \mathrm{\ψ}\right)x_L+(\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ})y_L+(\cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ})z_L\\ -\sin \mathrm{\θ}{x}_L+\sin \mathrm{\φ}\cos \mathrm{\θ}{y}_L+\cos \mathrm{\φ}\cos \mathrm{\θ}{z}_L\end{array}\right]\end{array}---\left(44\right)$

由于空间系绳在空间绳系机器人上连接点的移动及空间绳系机器人姿态的变化可以导致空间系绳的名义长度（空间平台质心到空间绳系机器人质心长度）由|z_L|变为|-sinθx_L+sinφcosθy_L+co_sφcosθz_L|，设空间系绳的名义长度为l，则空间系绳上的张力可以表示为： 

$T=T_0-m_r\stackrel{\·\·}{l}---\left(45\right)$

式(45)中为空间系绳名义长度变化加速度，对|-sinθx_L+sinφcosθy_L+cosφcosθz_L|求二阶导数得到： 

$\begin{array}{c}\stackrel{\·\·}{l}=|(-\cos \mathrm{\φ}\cos \mathrm{\θ}{y}_L+\sin \mathrm{\φ}\cos \mathrm{\θ}{z}_L)\stackrel{\·\·}{\mathrm{\φ}}+(\cos \mathrm{\θ}{x}_L+\sin \mathrm{\φ}\sin \mathrm{\θ}{y}_L+\cos \mathrm{\φ}\sin \mathrm{\θ}{z}_L)\stackrel{\·\·}{\mathrm{\θ}}\\ +\sin \mathrm{\θ}{\stackrel{\·\·}{x}}_L-\sin \mathrm{\φ}\cos {\mathrm{\θ}\stackrel{\·\·}{y}}_L+(-\sin \mathrm{\θ}{x}_L+\sin \mathrm{\φ}\cos \mathrm{\θ}{y}_L+\cos \mathrm{\φ}\cos \mathrm{\θ}{z}_L){\stackrel{\·}{\mathrm{\θ}}}^2\\ +(\sin \mathrm{\φ}\cos \mathrm{\θ}{y}_L+\cos \mathrm{\φ}\cos \mathrm{\θ}{z}_L){\stackrel{\·}{\mathrm{\φ}}}^2+2\cos \mathrm{\θ}\stackrel{\·}{\mathrm{\θ}}{\stackrel{\·}{x}}_L-2\cos \mathrm{\φ}\cos \mathrm{\θ}{\stackrel{\·}{y}}_L\stackrel{\·}{\mathrm{\φ}}\\ +(2\cos \mathrm{\φ}\sin \mathrm{\θ}{y}_L-2\sin \mathrm{\φ}\sin \mathrm{\θ}{z}_L+2\sin \mathrm{\φ}\sin \mathrm{\θ})\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\θ}}+2\sin \mathrm{\φ}\sin \mathrm{\θ}{\stackrel{\·}{y}}_L\stackrel{\·}{\mathrm{\θ}}|\end{array}---\left(46\right)$

在控制过程中可以实时测量得到空间绳系机器人姿态角，姿态角速度可以通过对姿态角的差分得到，姿态角加速度可以通过对姿态角速度的差分得到，因此由式(41)～(43)、式(45)及(46)可以得到关于x_L、y_L及Ω_r的微分方程，解此微分方程即得x_L、y_L及Ω_r。 

Claims (1)

1.一种空间绳系机器人三轴主动姿态控制方法，其特征在于：采用以下步骤：

步骤1：计算空间绳系机器人三个方向的姿态控制力矩u_x、u_y及u_z：

$\left\{\begin{array}{c}{u}_x=-I_x(-\frac{I_r}{I_y}{x}_7{x}_4+(1-n_x)\mathrm{\Ω}{x}_6-n_x{\mathrm{\Ω}}^2{x}_1+\frac{I_r}{I_x}\mathrm{\Ω}{x}_7)+v_x\\ u_y=-I_y(\frac{I_r}{I_y}{x}_7{x}_2-\frac{I_r}{I_y}\mathrm{\Ω}{x}_7{x}_5)+v_y\\ u_z=-I_z((n_z-1)\mathrm{\Ω}{x}_2-n_z{\mathrm{\Ω}}^2{x}_5+\frac{\tan x_3}{\cos x_1{I}_z}{u}_x-\frac{\tan x_1}{I_z}{u}_y)+v_z\end{array}\right.$

其中x₁＝φ、x₃＝θ、x₅＝ψ、x₇＝Ω_r，φ、θ及ψ为空间绳系机器人的三轴姿态角，I_x、I_y、I_z分别为空间绳系机器人转动惯量，Ω_r为反作用轮转动角速度，I_r为反作用轮的转动惯量，n_x和n_z为系数，Ω为空间绳系机器人的轨道角速度， $v_x={\stackrel{\·\·}{\mathrm{\φ}}}_d-k_1(\mathrm{\φ}-{\mathrm{\φ}}_d)-k_2(\stackrel{\·}{\mathrm{\φ}}-{\stackrel{\·}{\mathrm{\φ}}}_d),v_y={\stackrel{\·\·}{\mathrm{\θ}}}_d-k_3(\mathrm{\θ}-{\mathrm{\θ}}_d)-k_4(\stackrel{\·}{\mathrm{\θ}}-{\stackrel{\·}{\mathrm{\θ}}}_d)$ 及 $v_z={\stackrel{\·\·}{\mathrm{\ψ}}}_d-k_5(\mathrm{\ψ}-{\mathrm{\ψ}}_d)-k_6(\stackrel{\·}{\mathrm{\ψ}}-{\stackrel{\·}{\mathrm{\ψ}}}_d),$ φ_d、θ_d及ψ_d分别为空间绳系机器人的指令滚转角、指令俯仰角及指令偏航角，k₁、k₂、k₃、k₄、k₅及k₆为正值常数；

步骤2：根据步骤1得到的空间绳系机器人三个方向的姿态控制力矩u_x、u_y及u_z，由公式

$\left\{\begin{array}{c}{M}_x=u_x+M_{\mathrm{dx}}=-\sin \mathrm{\φ}\cos \mathrm{\θT}{z}_L+\cos \mathrm{\φ}\cos \mathrm{\θT}{y}_L+M_{\mathrm{dx}}\\ M_y=u_y+M_{\mathrm{dy}}=-\sin \mathrm{\θT}{z}_L-\cos \mathrm{\φ}\cos \mathrm{\θT}{x}_L+M_{\mathrm{dy}}\\ M_z=-I_r{\stackrel{\·}{\mathrm{\Ω}}}_r+\frac{\tan \mathrm{\θ}}{\cos \mathrm{\φ}}{u}_x-\tan \mathrm{\φ}{u}_y+M_{\mathrm{dz}}\\ T=T_0-m_r\stackrel{\·\·}{l}\\ \stackrel{\·\·}{l}=|(-\cos \mathrm{\φ}\cos \mathrm{\θ}{y}_L+\sin \mathrm{\φ}\cos \mathrm{\θ}{z}_L)\stackrel{\·\·}{\mathrm{\φ}}+(\cos \mathrm{\θ}{x}_L+\sin \mathrm{\φ}\sin \mathrm{\θ}{y}_L+\cos \mathrm{\φ}\sin \mathrm{\θ}{z}_L)\stackrel{\·\·}{\mathrm{\θ}}\\ +\sin \mathrm{\θ}{\stackrel{\·\·}{x}}_L-\sin \mathrm{\φ}\cos \mathrm{\θ}{\stackrel{\·\·}{y}}_L+(-\sin \mathrm{\θ}{x}_L+\sin \mathrm{\φ}\cos \mathrm{\θ}{y}_L+\cos \mathrm{\φ}\cos \mathrm{\θ}{z}_L){\stackrel{\·}{\mathrm{\θ}}}^2\\ +(\sin \mathrm{\φ}\cos \mathrm{\θ}{y}_L+\cos \mathrm{\φ}\cos \mathrm{\θ}{z}_L){\stackrel{\·}{\mathrm{\φ}}}^2+2\cos \mathrm{\θ}\stackrel{\·}{\mathrm{\θ}}{\stackrel{\·}{x}}_L-2\cos \mathrm{\φ}\cos \mathrm{\θ}{\stackrel{\·}{y}}_L\stackrel{\·}{\mathrm{\φ}}\\ +(2\cos \mathrm{\φ}\sin \mathrm{\θ}{y}_L-2\sin \mathrm{\φ}\sin \mathrm{\θ}{z}_L+2\sin \mathrm{\φ}\sin \mathrm{\θ})\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\θ}}+2\sin \mathrm{\φ}\sin \mathrm{\θ}{\stackrel{\·}{y}}_L\stackrel{\·}{\mathrm{\θ}}|\end{array}\right.$

得到空间系绳连接点位置x_L、y_L及反作用轮的转动角速度Ω_r实现空间绳系机器人三轴姿态的主动控制；其中M_dx、M_dy和M_dz分别为空间绳系机器人三个轴向的干扰力矩，M_x、M_y和M_z为空间绳系机器人所受到的力矩，T为空间系绳上的张力，T₀为空间绳系机器人在平衡状态时，空间系绳应具有的标称张力，T₀＝3Ω²m_rlm_r空间绳系机器人的质量，l为空间系绳的长度；所述平衡状态指空间绳系机器人位于空间平台与地心连线上。