CN103955611B - 一种空间绳系机器人的通用动力学模型的建立方法 - Google Patents

Abstract

本发明提供一种能够适应不同任务以及不同结构的空间绳系机器人的建模需求，简化建模工作，提高建模和解算效率的空间绳系机器人的通用动力学模型的建立方法；其包括如下步骤：步骤1，确定空间绳系机器人的拓扑结构和结构参数，空间绳系机器人的拓扑结构包括空间系绳，以及通过空间系绳连接且均为多刚体结构的空间平台和操作机器人；步骤2，建立空间系绳的动力学模型；步骤3，基于Hamilton原理并结合空间系绳的动力学模型，建立地心惯性系下空间绳系机器人的动力学模型；步骤4，将地心惯性系下空间绳系机器人的动力学模型转换至轨道惯性系下；步骤5，利用有限元法离散化系统模型，建立空间绳系机器人的通用动力学模型。

Description

一种空间绳系机器人的通用动力学模型的建立方法

技术领域

本发明涉及空间绳系机器人在轨服务领域，具体为一种空间绳系机器人的通用动力学模型的建立方法。

背景技术

空间绳系机器人是一种新型的空间机器人系统，由于其灵活、安全、成本低等特点，将广泛应用于失效卫星救护、空间垃圾清理、在轨维修、在轨组装等任务。空间绳系机器人的一般架构为“空间平台+空间系绳+操作机器人”，空间平台通过空间系绳释放操作机器人，操作机器人逼近空间目标实施抓捕；并利用操作机器人上自带操作机械臂及末端操作手执行在轨服务任务。作为一种复杂的刚柔组合系统，其动力学建模是一个巨大的挑战。针对这类系统建模，目前的常用模型按照复杂度可分为哑铃模型、弹簧-质量模型与多单元模型三类。最为复杂的多单元模型虽然很好的反映了空间系绳的质量、弹性、张力等特性，但将两端绑体近似为质点或单刚体结构，并不适合空间绳系机器人。因此，针对这种复杂空间绳系机器人的建模问题，需要在多单元模型的基础上，考虑两端绑体的多刚体构型，建立空间绳系机器人动力学模型。另外，尽管空间绳系机器人具有类似的结构，但针对不同的任务，空间绳系机器人的结构仍有所不同。例如，操作手的配置及自由度需针对任务设计。这给空间绳系机器人的研究带来极大的动力学建模工作量。因此，亟需建立一种空间绳系机器人的通用动力学模型，并设计一种通用解算方法，为空间绳系机器人的研究奠定基础。

发明内容

本发明解决的问题在于提供一种能够适应不同任务以及不同结构的空间绳系机器人的建模需求，简化建模工作，提高建模和解算效率的空间绳系机器人的通用动力学模型的建立方法。

本发明是通过以下技术方案来实现：

一种空间绳系机器人的通用动力学模型的建立方法，其包括如下步骤：步骤1，确定空间绳系机器人的拓扑结构和结构参数，空间绳系机器人的拓扑结构包括空间系绳，以及通过空间系绳连接且均为多刚体结构的空间平台和操作机器人；步骤2，建立空间系绳的动力学模型；步骤3，基于Hamilton原理并结合空间系绳的动力学模型，建立地心惯性系下空间绳系机器人的动力学模型；步骤4，将地心惯性系下空间绳系机器人的动力学模型转换至轨道惯性系下；步骤5，利用有限元法离散化系统模型，建立空间绳系机器人的通用动力学模型。

优选的，步骤1中确定的拓扑结构参数包括，定义地心惯性坐标系为OX_ωY_ωZ_ω；空间平台由n_P个刚体连接而成，第i个刚体的质心为P_i，质量为本体系下惯量为相对惯性坐标系的欧拉四元数为操作机器人由n_M个刚体连接而成，第i个刚体的质心为M_i，质量为本体系下惯量为相对惯性坐标系的欧拉四元数为空间系绳连接空间平台和操作机器人，且两端均能收放；点C_P表示空间平台与空间系绳之间的连接点，点C_M表示末端操作机器人与空间系绳之间的连接点。

进一步，步骤2中建立空间系绳的动力学模型时，引入自然坐标s表示空间系绳未变形时，空间系绳上一点与某一端点间的绳段长度；选取空间系绳存放在平台中的一端为自然坐标起点，并规定s_P(t)表示C_P点处空间系绳自然坐标，s_M(t)表示C_M点处空间系绳自然坐标；总长为L的空间系绳表示为：

长度为s_P的空间系绳存放在平台中，长度为s_M的空间系绳存放在操作机器人中，剩余的长度为s_M-s_P的空间系绳释放在平台和操作机器人之间；

空间系绳的轴向张力表示为：

式中，n表示空间系绳轴向张力矢量，N表示轴向张力的大小，E为空间系绳杨氏模量，A为空间系绳截面积，ε表示空间系绳上某一点的应变，τ表示空间系绳上某一点的切向量，α为系数。

进一步，空间系绳轴向张力包括弹性力N_C=EAε和粘弹性体轴向阻尼力两部分。

进一步，步骤3中建立地心惯性系下空间绳系机器人的动力学模型为：

G_P+G_M+G_T+G_DR+G_C=0 （8）

式中，

其中，分别表示对应约束条件的拉格朗日乘子；和分别表示作用在空间平台和末端操作机构上的万有引力，和分别表示作用在空间平台和末端操作机构上的广义重力梯度力矩，N_PD和N_MD表示释放回收机构中由于空间系绳释放造成的附加阻尼力。

进一步，步骤3中建立地心惯性系下空间绳系机器人的动力学模型时引入Carnot能量损失项来对空间系绳释放与回收过程中系统的能量方程进行修正。

进一步，步骤4中，将空间绳系机器人的动力学模型从地心惯性系转换到轨道惯性系：

式中，

进一步，步骤5中，在离散化之前，引入归一化自然坐标s，

将采用描述的位置矢量记为利用n+1个节点将积分区间[0,1]分为n段，每段上选取插值函数为：

于是第i段空间系绳上点的位置向量近似满足：

进一步，步骤5中，将轨道惯性系下的模型离散化后得到空间绳系机器人的通用动力学模型为：

(1)空间平台动力学方程及约束方程

(2)操作机器人位姿动力学方程及约束方程

(3)存放空间系绳的动力学方程及约束方程

(4)释放空间系绳的动力学方程及约束方程

(5)释放/回收机构动力学方程

式中，和为3(n+1)维的向量，为3n维的向量，它们满足：

M₁、M₂、为3(n+1)×3(n+1)维的矩阵，M₃为3(n+1)×3n维的矩阵，它们满足：

与现在技术相比，本发明具有以下有益的技术效果：

本发明通过在通用空间绳系机器人的拓扑结构上，考虑空间系绳两端绑体的结构特点，将两端绑体确定为多刚体结构的复杂刚柔组合体系统，先建立空间系绳的动力学模型，然后根据Hamilton原理建立地心惯性系下的动力学模型，再通过转换至轨道惯性系下，降低由于轨道半径远大于释放长度而引起的计算误差；最后通过有限元法进行离散处理，使其模型建立过程中的计算能够继续进行，建立得到面向仿真分析的通用动力学模型；完整的反映了两端绑体的特性及空间系绳的张力、质量、弹性等特征，能够适应于各种不同任务，不同结构的空间绳系机器人的动力学建模，为空间绳系机器人的研究奠定了基础。

进一步的，由于在空间系绳的释放/回收建模时，一般假设存放的空间系绳完全松弛并忽略其体积，因为忽略了存放空间系绳的体积和运动速度，所以在空间系绳释放与回收过程中，空间系绳微元在释放/回收机构出口处会被瞬间加速或减速，这一质量流动的过程仅仅满足动量守恒，并不满足机械能守恒，会给系统的动力学建模带来困难，对此通过引入Carnot能量损失项来对空间系绳释放与回收过程中系统的能量方程进行修正，保证建模的正确性。

进一步的，通过在离散化之前，引入归一化自然坐标s，消除变化的空间系绳长度的影响。

附图说明

图1是本发明所述的空间绳系机器人的拓扑结构图。

图中：1为空间平台，2为空间系绳，3为操作机器人。

具体实施方式

下面结合具体的实施例对本发明做进一步的详细说明，所述是对本发明的解释而不是限定。

本发明一种空间绳系机器人的通用动力学模型的建立方法，针对空间绳系机器人这一复杂的刚柔组合体系统建立一种通用的动力学模型，为空间绳系机器人的研究奠定基础。

本发明具体通过以下步骤实现：

步骤1，确定空间绳系机器人的拓扑结构和结构参数。

如图1所示，空间绳系机器人采用一种通用的拓扑结构。空间平台1和操作机器人3均为多刚体结构。定义地心惯性坐标系OX_ωY_ωZ_ω。空间平台1由n_P个刚体连接而成，第i个刚体的质心为P_i，质量为本体系下惯量为相对惯性坐标系的欧拉四元数为操作机器人3由n_M个刚体连接而成，第i个刚体的质心为M_i，质量为本体系下惯量为相对惯性坐标系的欧拉四元数为空间系绳2连接空间平台1和操作机器人3，且两端均能收放。点C_P表示空间平台与空间系绳之间的连接点，点C_M表示操作机器人与空间系绳之间的连接点。

步骤2，建立空间系绳的动力学模型。

为了便于描述，引入自然坐标s表示空间系绳未变形时，空间系绳上一点与某一端点间的绳段长度。选取空间系绳存放在平台中的一端为自然坐标起点，并规定s_P(t)表示C_P点处空间系绳自然坐标，s_M(t)表示C_M点处空间系绳自然坐标。于是总长为L的空间系绳分为三段：

长度为s_P的空间系绳存放在平台中，长度为s_M的空间系绳存放在操作机器人中，剩余的长度为s_M-s_P的空间系绳释放在平台和操作机器人之间。

空间系绳的轴向张力表示为：

式中，n表示空间系绳轴向张力矢量，N表示轴向张力的大小，E为空间系绳杨氏模量，A为空间系绳截面积，ε表示空间系绳上某一点的应变，τ表示空间系绳上某一点的切向量，α为系数。可以看出，空间系绳轴向张力包括弹性力N_C=EAε和粘弹性体轴向阻尼力两部分。

在空间系绳的释放/回收建模时，一般假设存放的空间系绳完全松弛并忽略其体积。由于忽略了存放空间系绳的体积和运动速度，因此在空间系绳释放与回收过程中，空间系绳微元在释放/回收机构的出口处会被瞬间加速或减速，这一质量流动的过程仅仅满足动量守恒，并不满足机械能守恒，会给系统的动力学建模带来困难。对此，引入Carnot能量损失项来对空间系绳释放与回收过程中系统的能量方程进行修正，保证建模的正确性。

步骤3，基于Hamilton原理并结合空间系绳的动力学模型，建立地心惯性系下空间绳系机器人的动力学模型。

对于任何动力学力学系统，均满足如下形式的Hamilton原理：

式中，T表示系统动能，V表示系统的势能，λ表示拉格朗日乘子，C表示系统的约束方程，δW表示非保守外力所做的虚功。

针对图1所示的空间绳系机器人，系统动能

式中，ρ为空间系绳线密度，R表示地心惯性系下的位置矢量。矩阵和为四元素的转换矩阵。

系统势能V满足：

式中，和分别表示平台和操作机器人中刚体的万有引力势能，而引力势能项Φ和弹性势能项Π分别满足：

其中，μ_e表示地球的引力常数，η为空间系绳某一点处的伸长量，

系统中非保守力做的功为：

式中，F_CP、F_CM和F_T分别表示作用在平台、操作机构及空间系绳上非保守外力，广义力矩和满足：

其中，和分别表示作用在平台和操作机构非保守外力形成的机动力矩。Carnot能量损失项CELT_P和CELT_M满足：

另外，空间绳系机器人系统还需要满足约束方程：

在式（7）中，第一组为平台和操作机构上各刚体连接带来的约束，第二组表示四元数模为1的约束，第三组表示平台与操作机构上的CP点，CM点应与实际CP点，CM点位置重合带来的约束，第四组表示空间系绳上的CP点，CM点应与实际CP点，CM点位置重合带来的约束。

化简，可得地心惯性系下，空间绳系机器人的动力学模型：

G_P+G_M+G_T+G_DR+G_C=0 (8)

式中，

其中，λ_CP,λ_CM,λ_PT,λ_MT分别表示对应约束条件的拉格朗日乘子。和分别表示作用在空间平台和末端操作机构上的万有引力，它满足：

和分别表示作用在空间平台和末端操作机构上的广义重力梯度力矩，它满足：

式中，d(R)表示质点与刚体质心之间的距离。另外，N_PD和N_MD表示释放回收机构中由于空间系绳释放造成的附加阻尼力，它们满足：

式中，和分别表示平台释放和回收空间系绳的情况，和分别表示末端操作机构释放和回收空间系绳的情况。

步骤4，将地心惯性系下空间绳系机器人的动力学模型转换至轨道惯性系下；降低由于轨道半径远大于释放长度而引起的计算误差。

由于释放长度远小于轨道半径，释放/回收操作对系统轨道影响极小，直接利用式(8)进行求解会造成数值计算上较大的误差。为了克服这一问题，在空间绳系机器人系统的附近选取一点o，建立轨道惯性系。其原点o绕地球按开普勒轨道运动，和空间绳系机器人距离保持在20km以下，且运动不受机器人影响，其坐标轴与地心惯性系保持平行。

设分别是空间平台和操作机器人各刚体在轨道惯性系下的坐标。表示轨道平均角速度，表示地心惯性系矢径的单位方向向量在本体坐标系内的坐标，f为真近点角。

将系统动力学模型从地心惯性系转换到轨道惯性系：

式中，

步骤5，利用有限元法离散化系统模型，建立空间绳系机器人的通用动力学模型。

由于方程(9)中存在描述空间系绳运动的积分项，不能直接继续求解，需要采用有限单元法进行离散处理。在离散化之前，引入归一化自然坐标消除变化的空间系绳长度的影响。

为了便于区分，将采用描述的位置矢量记为利用n+1个节点将积分区间[0,1]分为n段，每段上选取插值函数为：

于是第i段空间系绳上点的位置向量近似满足：

空间绳系机器人动力学方程离散化为：

(1)空间平台动力学方程及约束方程

(2)操作机器人位姿动力学方程及约束方程

(3)存放空间系绳的动力学方程及约束方程

(4)释放空间系绳的动力学方程及约束方程

(5)释放/回收机构动力学方程

式中，和为3(n+1)维的向量，为3n维的向量，它们满足：

M₁、M₂、为3(n+1)×3(n+1)维的矩阵，M₃为3(n+1)×3n维的矩阵，它们满足：

Claims (1)

1.一种空间绳系机器人的通用动力学模型的建立方法，其特征在于，包括如下步骤：

步骤1，确定空间绳系机器人的拓扑结构和结构参数，空间绳系机器人的拓扑结构包括空间系绳(2)，以及通过空间系绳(2)连接且均为多刚体结构的空间平台(1)和操作机器人(3)；

步骤2，建立空间系绳的动力学模型；

步骤3，基于Hamilton原理并结合空间系绳的动力学模型，建立地心惯性系下空间绳系机器人的动力学模型；

步骤4，将地心惯性系下空间绳系机器人的动力学模型转换至轨道惯性系下；

步骤5，利用有限元法离散化系统模型，建立空间绳系机器人的通用动力学模型；

步骤1中确定的拓扑结构参数包括，定义地心惯性坐标系为OX_ωY_ωZ_ω；空间平台由n_P个刚体连接而成，第i个刚体的质心为P_i，质量为本体系下惯量为相对惯性坐标系的欧拉四元数为操作机器人由n_M个刚体连接而成，第i个刚体的质心为M_i，质量为本体系下惯量为相对惯性坐标系的欧拉四元数为空间系绳连接空间平台和操作机器人，且两端均能收放；点C_P表示空间平台与空间系绳之间的连接点，点C_M表示末端操作机器人与空间系绳之间的连接点；

步骤2中建立空间系绳的动力学模型时，引入自然坐标s表示空间系绳未变形时，空间系绳上一点与某一端点间的绳段长度；选取空间系绳存放在平台中的一端为自然坐标起点，并规定s_P(t)表示C_P点处空间系绳自然坐标，s_M(t)表示C_M点处空间系绳自然坐标；总长为L的空间系绳表示为：

$\left\{\begin{array}{c}0\≤s\≤s_P\left(t\right)\\ s_P\left(t\right)\≤s\≤s_M\left(t\right)\\ s_M\left(t\right)\≤s\≤L\end{array}\right.---\left(1\right)$

长度为s_P的空间系绳存放在平台中，长度为L-s_M的空间系绳存放在操作机器人中，剩余的长度为s_M-s_P的空间系绳释放在平台和操作机器人之间；

空间系绳的轴向张力表示为：

$\left\{\begin{array}{c}N=EA(\mathrm{\ϵ}+\mathrm{\α}\stackrel{\·}{\mathrm{\ϵ}})\\ n=N\mathrm{\τ}\end{array}\right.---\left(2\right)$

式中，n表示空间系绳轴向张力矢量，N表示轴向张力的大小，E为空间系绳杨氏模量，A为空间系绳截面积，ε表示空间系绳上某一点的应变，τ表示空间系绳上某一点的切向量，α为系数；

空间系绳轴向张力包括弹性力N_C＝EAε和粘弹性体轴向阻尼力两部分；

步骤3中建立地心惯性系下空间绳系机器人的动力学模型为：

G_P+G_M+G_T+G_DR+G_C＝0 (8)

式中，

$\begin{array}{c}{G}_P=\underset{t_0}{\overset{t_1}{\∫}}\left\{\underset{i=1}{\overset{n_P}{\Σ}}\right\{{\mathrm{\δR}}_P^{iT}\[-m_P^i{\stackrel{\·\·}{R}}_P^i-\underset{k=1}{\overset{n_P^C}{\Σ}}{\left(\frac{\∂C_P^k}{\∂R_P^i}\right)}^T{\mathrm{\λ}}_P^k+{\left(\frac{\∂C_{CP}}{\∂R_P^i}\right)}^T{\mathrm{\λ}}_{CP}+F_P^i+F_{Pg}^i\]\\ +{\mathrm{\δ\Λ}}_P^{iT}\[-4L_P^{iT}{J}_P^i{L}_P^i{\stackrel{\·\·}{\mathrm{\Λ}}}_P^i-8{\stackrel{\·}{L}}_P^{iT}{J}_P^i{L}_P^i{\stackrel{\·}{\mathrm{\Λ}}}_P^i-\underset{k=1}{\overset{n_P^C}{\Σ}}{\left(\frac{\∂C_P^k}{\∂{\mathrm{\Λ}}_P^i}\right)}^T{\mathrm{\λ}}_P^k-2{\mathrm{\Λ}}_P^i{\mathrm{\λ}}_{P\mathrm{\Λ}}^i\\ +{\left(\frac{\∂C_{CP}}{\∂{\mathrm{\Λ}}_P^i}\right)}^T{\mathrm{\λ}}_{CP}+Q_P^i+Q_{Pg}^i\]\left\}\right\}dt\end{array}$

$\begin{array}{c}{G}_M=\underset{t_0}{\overset{t_1}{\∫}}\left\{\underset{i=1}{\overset{n_M}{\Σ}}\right\{{\mathrm{\δR}}_M^{iT}\[-m_M^i{\stackrel{\·\·}{R}}_M^i-\underset{k=1}{\overset{n_M^C}{\Σ}}{\left(\frac{\∂C_M^k}{\∂R_M^i}\right)}^T{\mathrm{\λ}}_M^k+{\left(\frac{\∂C_{CM}}{\∂R_M^i}\right)}^T{\mathrm{\λ}}_{CM}+F_M^i+F_{Mg}^i\]\\ +{\mathrm{\δ\Λ}}_M^{iT}\[-4L_M^{iT}{J}_M^i{L}_M^i{\stackrel{\·\·}{\mathrm{\Λ}}}_M^i-8{\stackrel{\·}{L}}_M^{iT}{J}_M^i{L}_M^i{\stackrel{\·}{\mathrm{\Λ}}}_M^i-\underset{k=1}{\overset{n_M^C}{\Σ}}{\left(\frac{\∂C_M^k}{\∂{\mathrm{\Λ}}_M^i}\right)}^T{\mathrm{\λ}}_M^k-2{\mathrm{\Λ}}_M^i{\mathrm{\λ}}_{M\mathrm{\Λ}}^i\\ +{\left(\frac{\∂C_{CM}}{\∂{\mathrm{\Λ}}_M^i}\right)}^T{\mathrm{\λ}}_{CM}+Q_M^i+Q_{Mg}^i\]\left\}\right\}dt\end{array}$

$\begin{array}{c}{G}_T=\underset{t_0}{\overset{t_1}{\∫}}\left\{{\mathrm{\δR}}_{CP}^T\right\{-{\mathrm{\ρs}}_P\[{\stackrel{\·\·}{R}}_{CP}+\▿\mathrm{\Φ}\left(R_{CP}\right)\]-\mathrm{\ρ}{\stackrel{\·}{s}}_P^2\mathrm{\η}\left(s_P\right)\mathrm{\τ}\left(s_P\right)-{\mathrm{\λ}}_{CP}-{\mathrm{\λ}}_{PT}+F_{CP}\}\\ +\underset{s_P}{\overset{s_M}{\∫}}{\mathrm{\δR}}^T\[-\mathrm{\ρ}(\stackrel{\·\·}{R}+\▿\mathrm{\Φ})+F_T+\frac{\∂n}{\∂s}\]ds+{\mathrm{\δR}}_{CM}^T\{-\mathrm{\ρ}(L-s_M)\[{\stackrel{\·\·}{R}}_{CM}+\▿\mathrm{\Φ}\left(R_{CM}\right)\]\\ +\mathrm{\ρ}{\stackrel{\·}{s}}_M^2\mathrm{\η}\left(s_M\right)\mathrm{\τ}\left(s_M\right)-{\mathrm{\λ}}_{CM}-{\mathrm{\λ}}_{MT}+F_{CM}\}+{\mathrm{\δR}}^T\left(s_P\right)\[{\mathrm{\λ}}_{PT}+n\left(s_P\right)\]\\ +{\mathrm{\δR}}^T\left(s_M\right)\[{\mathrm{\λ}}_{MT}-n\left(s_M\right)\]\}dt\end{array}$

$G_{DR}=\underset{t_0}{\overset{t_1}{\∫}}\{\[N_{PD}+N_P-N\left(s_P\right)\]\mathrm{\η}\left(s_P\right){\mathrm{\δs}}_P+\[-N_{MD}-N_M+N\left(s_M\right)\]\mathrm{\η}\left(s_M\right){\mathrm{\δs}}_M\}dt$

$\begin{array}{c}{G}_C=\underset{t_0}{\overset{t_1}{\∫}}\{-{\mathrm{\δ\λ}}_P^{kT}{C}_p^k-{\mathrm{\δ\λ}}_M^{kT}{C}_M^k-{\mathrm{\δ\λ}}_{CP}^T\[R_{CP}-C_{CP}(R_p^i,{\mathrm{\Λ}}_p^i)\]\\ -{\mathrm{\δ\λ}}_{CM}^T\[R_{CM}-C_{CM}(R_M^i,{\mathrm{\Λ}}_M^i)\]+{\mathrm{\δ\λ}}_{PT}^T\[R_{CP}-R\left(s_P\right)\]\\ +{\mathrm{\δ\λ}}_{MT}^T\[R_{CM}-R\left(s_M\right)\]+\underset{i=1}{\overset{n_P}{\Σ}}{\mathrm{\δ\λ}}_{P\mathrm{\Λ}}^i\[{\mathrm{\Λ}}_p^{iT}{\mathrm{\Λ}}_p^i-1\]+\underset{i=1}{\overset{n_M}{\Σ}}{\mathrm{\δ\λ}}_{M\mathrm{\Λ}}^i\[{\mathrm{\Λ}}_M^{iT}{\mathrm{\Λ}}_M^i-1\]\}dt\end{array}$

其中，λ_CP,λ_CM,λ_PT,λ_MT分别表示对应约束条件的拉格朗日乘子；和分别表示作用在空间平台和末端操作机构上的万有引力，和分别表示作用在空间平台和末端操作机构上的广义重力梯度力矩，N_PD和N_MD表示释放回收机构中由于空间系绳释放造成的附加阻尼力，ρ为空间系绳线密度，R表示地心惯性系下的位置矢量，矩阵和为四元素的转换矩阵，Φ为引力势能项，η为空间系绳某一点处的伸长量，F_CP、F_CM和F_T分别表示作用在平台、操作机构及空间系绳上非保守外力，和为广义力矩；

步骤3中建立地心惯性系下空间绳系机器人的动力学模型时引入Carnot能量损失项来对空间系绳释放与回收过程中系统的能量方程进行修正；

步骤4中，将空间绳系机器人的动力学模型从地心惯性系转换到轨道惯性系：

$G_P^o+G_M^o+G_T^o+G_{DR}^o+G_C^o=0---\left(9\right)$

式中，

$\begin{array}{c}{G}_P^o=\underset{t_0}{\overset{t_1}{\∫}}\left\{\underset{i=1}{\overset{n_P}{\Σ}}\right\{{\mathrm{\δr}}_P^{iT}\[-m_P^i{\stackrel{\·\·}{r}}_P^i-m_P^i{\stackrel{\‾}{\mathrm{\ω}}}^2{A}_f{r}_P^i-\underset{k=1}{\overset{n_P^C}{\Σ}}{\left(\frac{\∂C_P^k}{\∂r_P^i}\right)}^T{\mathrm{\λ}}_P^k+{\left(\frac{\∂C_{CP}}{\∂r_P^i}\right)}^T{\mathrm{\λ}}_{CP}+F_P^i\]\\ +{\mathrm{\δ\Λ}}_P^{iT}\[-4L_P^{iT}{J}_P^i{L}_P^i{\stackrel{\·\·}{\mathrm{\Λ}}}_P^i-8{\stackrel{\·}{L}}_P^{iT}{J}_P^i{L}_P^i{\stackrel{\·}{\mathrm{\Λ}}}_P^i+6{\stackrel{\‾}{\mathrm{\ω}}}^2{L}_P^{iT}\[i_P^i\×(I_P^i\·i_P^i)\]-\underset{k=1}{\overset{n_P^C}{\Σ}}{\left(\frac{\∂C_P^k}{\∂{\mathrm{\Λ}}_P^i}\right)}^T{\mathrm{\λ}}_P^k\\ -2{\mathrm{\Λ}}_P^i{\mathrm{\λ}}_{P\mathrm{\Λ}}^i+{\left(\frac{\∂C_{CP}}{\∂{\mathrm{\Λ}}_P^i}\right)}^T{\mathrm{\λ}}_{CP}+Q_P^i\]\left\}\right\}dt\end{array}$

$\begin{array}{c}{G}_M^o=\underset{t_0}{\overset{t_1}{\∫}}\left\{\underset{i=1}{\overset{n_M}{\Σ}}\right\{{\mathrm{\δR}}_M^{iT}\[-m_M^i{\stackrel{\·\·}{r}}_M^i-m_M^i{\stackrel{\‾}{\mathrm{\ω}}}^2{A}_f{r}_M^i-\underset{k=1}{\overset{n_M^C}{\Σ}}{\left(\frac{\∂C_M^k}{\∂R_M^i}\right)}^T{\mathrm{\λ}}_M^k+{\left(\frac{\∂C_{CM}}{\∂R_M^i}\right)}^T{\mathrm{\λ}}_{CM}+F_M^i\]\\ +{\mathrm{\δ\Λ}}_M^{iT}\[-4L_M^{iT}{J}_M^i{L}_M^i{\stackrel{\·\·}{\mathrm{\Λ}}}_M^i-8{\stackrel{\·}{L}}_M^{iT}{J}_M^i{L}_M^i{\stackrel{\·}{\mathrm{\Λ}}}_M^i+6{\stackrel{\‾}{\mathrm{\ω}}}^2{L}_M^{iT}\[i_M^i\×(I_M^i\·i_M^i)\]-\underset{k=1}{\overset{n_M^C}{\Σ}}{\left(\frac{\∂C_M^k}{\∂{\mathrm{\Λ}}_M^i}\right)}^T{\mathrm{\λ}}_M^k\\ -2{\mathrm{\Λ}}_M^i{\mathrm{\λ}}_{M\mathrm{\Λ}}^i+{\left(\frac{\∂C_{CM}}{\∂{\mathrm{\Λ}}_M^i}\right)}^T{\mathrm{\λ}}_{CM}+Q_M^i\]\left\}\right\}dt\end{array}$

$\begin{array}{c}{G}_T^o=\underset{t_0}{\overset{t_1}{\∫}}\{{\mathrm{\δr}}_{CP}^T\[-{\mathrm{\ρs}}_P({\stackrel{\·\·}{r}}_{CP}+{\stackrel{\‾}{\mathrm{\ω}}}^2{A}_f{r}_{CP})-\mathrm{\ρ}{\stackrel{\·}{s}}_P^2\mathrm{\η}\left(s_P\right)\mathrm{\τ}\left(s_P\right)-{\mathrm{\λ}}_{CP}-{\mathrm{\λ}}_{PT}+F_{CP}\]\\ +\underset{s_P}{\overset{s_M}{\∫}}{\mathrm{\δr}}^T\[-\mathrm{\ρ}(\stackrel{\·\·}{r}+{\stackrel{\‾}{\mathrm{\ω}}}^2{A}_{f}r)+F_T+\frac{\∂n}{\∂s}\]ds+{\mathrm{\δr}}_{CM}^T\[-\mathrm{\ρ}(L-s_M)({\stackrel{\·\·}{r}}_{CM}+{\stackrel{\‾}{\mathrm{\ω}}}^2{A}_f{r}_{CM})\\ +\mathrm{\ρ}{\stackrel{\·}{s}}_M^2\mathrm{\η}\left(s_M\right)\mathrm{\τ}\left(s_M\right)-{\mathrm{\λ}}_{CM}-{\mathrm{\λ}}_{MT}+F_{CM}\}+{\mathrm{\δr}}^T\left(s_P\right)\[{\mathrm{\λ}}_{PT}+n\left(s_P\right)\]\\ +{\mathrm{\δr}}^T\left(s_M\right)\[{\mathrm{\λ}}_{MT}-n\left(s_M\right)\]\}dt\end{array}$

$G_{DR}^o=\underset{t_0}{\overset{t_1}{\∫}}\{\[N_{PD}+N_P-N\left(s_P\right)\]\mathrm{\η}\left(s_P\right){\mathrm{\δs}}_P+\[-N_{MD}-N_M+N\left(s_M\right)\]\mathrm{\η}\left(s_M\right){\mathrm{\δs}}_M\}dt$

$\begin{array}{c}{G}_C^o=\underset{t_0}{\overset{t_1}{\∫}}\{-{\mathrm{\δ\λ}}_P^{kT}{C}_p^k(r_p^i,{\mathrm{\Λ}}_p^i)-{\mathrm{\δ\λ}}_M^{kT}{C}_M^k(r_M^i,{\mathrm{\Λ}}_M^i)-{\mathrm{\δ\λ}}_{CP}^T\[r_{CP}-C_{CP}(r_p^i,{\mathrm{\Λ}}_p^i)\]\\ -{\mathrm{\δ\λ}}_{CM}^T\[r_{CM}-C_{CM}(r_M^i,{\mathrm{\Λ}}_M^i)\]+{\mathrm{\δ\λ}}_{PT}^T\[r_{CP}-r\left(s_P\right)\]+{\mathrm{\δ\λ}}_{MT}^T\[r_{CM}-r\left(s_M\right)\]\\ +\underset{i=1}{\overset{n_P}{\Σ}}{\mathrm{\δ\λ}}_{P\mathrm{\Λ}}^i\[{\mathrm{\Λ}}_p^{iT}{\mathrm{\Λ}}_p^i-1\]+\underset{i=1}{\overset{n_M}{\Σ}}{\mathrm{\δ\λ}}_{M\mathrm{\Λ}}^i\[{\mathrm{\Λ}}_M^{iT}{\mathrm{\Λ}}_M^i-1\]\}dt\end{array};$

其中，设分别是空间平台和操作机器人各刚体在轨道惯性系下的坐标；表示轨道平均角速度，表示地心惯性系矢径的单位方向向量在本体坐标系内的坐标；f为真近点角；

步骤5中，在离散化之前，引入归一化自然坐标

$\stackrel{\‾}{s}=\frac{s-s_P}{s_M-s_P}\∈\[0,1\]$

将采用描述的位置矢量记为利用n+1个节点将积分区间[0,1]分为n段，每段上选取插值函数为：

于是第i段空间系绳上点的位置向量近似满足：

$\stackrel{\‾}{r}(\stackrel{\‾}{s},t)\≈\left[\begin{array}{cc}1-\stackrel{\‾}{s}& \stackrel{\‾}{s}\end{array}\right]\left[\begin{array}{c}{\stackrel{\‾}{r}}_{i-1}^n\left(t\right)\\ r_i^n\left(t\right)\end{array}\right]{\stackrel{\‾}{r}}^{\′}(\stackrel{\‾}{s},t)\≈{\stackrel{\‾}{r}}_i\left(t\right)-{\stackrel{\‾}{r}}_{i-1}\left(t\right)---\left(11\right);$

步骤5中，将轨道惯性系下的模型离散化后得到空间绳系机器人的通用动力学模型为：

(1)空间平台动力学方程及约束方程

$\left\{\begin{array}{c}{m}_P^i{\stackrel{\·\·}{r}}_P^i+m_P^i{\stackrel{\‾}{\mathrm{\ω}}}^2{A}_f^i{r}_P^i+\underset{k=1}{\overset{n_P^C}{\Σ}}{\left(\frac{\∂C_P^k}{\∂r_P^i}\right)}^T{\mathrm{\λ}}_P^k-{\left(\frac{\∂C_{CP}}{\∂r_P^i}\right)}^T{\mathrm{\λ}}_{CP}=F_P^i\\ 4L_P^{iT}{J}_P^i{L}_P^i{\stackrel{\·\·}{\mathrm{\Λ}}}_P^i+8{\stackrel{\·}{L}}_P^{iT}{J}_P^i{L}_P^i{\stackrel{\·}{\mathrm{\Λ}}}_P^i-6{\stackrel{\‾}{\mathrm{\ω}}}^2{L}_P^{iT}\[i_P^i\×(I_P^i\·i_P^i)\]\\ +\underset{k=1}{\overset{n_P^C}{\Σ}}{\left(\frac{\∂C_P^k}{\∂{\mathrm{\Λ}}_P^i}\right)}^T{\mathrm{\λ}}_P^k+2{\mathrm{\Λ}}_P^i{\mathrm{\λ}}_{P\mathrm{\Λ}}^i-{\left(\frac{\∂C_{CP}}{\∂{\mathrm{\Λ}}_P^i}\right)}^T{\mathrm{\λ}}_{CP}=Q_P^i\\ \begin{array}{cc}{C}_P^k(r_P^i,n_P^i)=0& {\mathrm{\Λ}}_p^{iT}{\mathrm{\Λ}}_p^i-1=0\end{array}\end{array}\right.---\left(12\right)$

(2)操作机器人位姿动力学方程及约束方程

$\left\{\begin{array}{c}{m}_M^i{\stackrel{\·\·}{R}}_M^i+m_M^i{\stackrel{\‾}{\mathrm{\ω}}}^2{A}_f{r}_M^i+\underset{k=1}{\overset{n_M^C}{\Σ}}{\left(\frac{\∂C_M^k}{\∂R_M^i}\right)}^T{\mathrm{\λ}}_M^k-{\left(\frac{\∂C_{CM}}{\∂R_M^i}\right)}^T{\mathrm{\λ}}_{CM}=F_M^i\\ 4L_M^{iT}{J}_M^i{L}_M^i{\stackrel{\·\·}{\mathrm{\Λ}}}_M^i+8{\stackrel{\·}{L}}_M^{iT}{J}_M^i{L}_M^i{\stackrel{\·}{\mathrm{\Λ}}}_M^i-6{\stackrel{\‾}{\mathrm{\ω}}}^2{L}_M^{iT}\[i_M^i\×(I_M^i\·i_M^i)\]\\ +\underset{k=1}{\overset{n_M^C}{\Σ}}{\left(\frac{\∂C_M^k}{\∂{\mathrm{\Λ}}_M^i}\right)}^T{\mathrm{\λ}}_M^k+2{\mathrm{\Λ}}_M^i{\mathrm{\λ}}_{M\mathrm{\Λ}}^i-{\left(\frac{\∂C_{CM}}{\∂{\mathrm{\Λ}}_M^i}\right)}^T{\mathrm{\λ}}_{CM}=Q_M^i\\ \begin{array}{cc}{C}_M^k(r_M^i,{\mathrm{\Λ}}_M^i)=0& {\mathrm{\Λ}}_M^{iT}{\mathrm{\Λ}}_M^i-1=0\end{array}\end{array}\right.---\left(13\right)$

(3)存放空间系绳的动力学方程及约束方程

$\left\{\begin{array}{c}{\mathrm{\ρs}}_P({\stackrel{\·\·}{r}}_{CP}+{\stackrel{\‾}{\mathrm{\ω}}}^2{A}_f{r}_{CP})+\mathrm{\ρ}{\stackrel{\·}{s}}_P^2\mathrm{\η}\left(s_P\right)\mathrm{\τ}\left(s_P\right)+{\mathrm{\λ}}_{CP}+{\mathrm{\λ}}_{PT}=F_{CP}\\ \mathrm{\ρ}(L-s_M)({\stackrel{\·\·}{r}}_M+{\stackrel{\‾}{\mathrm{\ω}}}^2{A}_f{r}_{CM})-\mathrm{\ρ}{\stackrel{\·}{s}}_M^2\mathrm{\η}\left(s_M\right)\mathrm{\τ}\left(s_M\right)+{\mathrm{\λ}}_{CM}+{\mathrm{\λ}}_{MT}=F_{CM}\\ \begin{array}{cc}{r}_{CP}-C_{CP}(r_p^i,{\mathrm{\Λ}}_p^i)=0& r_{CM}-C_{CM}(r_M^i,{\mathrm{\Λ}}_M^i)=0\end{array}\end{array}\right.---\left(14\right)$

(4)释放空间系绳的动力学方程及约束方程

$\left\{\begin{array}{c}\mathrm{\ρ}l\[M_1{\stackrel{\·\·}{\stackrel{\‾}{r}}}^n+2\frac{\stackrel{\·}{l}}{l}{M}_2{\stackrel{\·}{\stackrel{\‾}{r}}}^n+(\frac{\stackrel{\·\·}{l}}{l}-\frac{2{\stackrel{\·}{l}}^2}{l^2})M_2{\stackrel{\‾}{r}}^n+{\stackrel{\‾}{\mathrm{\ω}}}^2{M}_1{\hat{A}}_f{\stackrel{\‾}{r}}^n\]-B\left[\begin{array}{c}{\mathrm{\λ}}_{PT}\\ {\mathrm{\λ}}_{MT}\end{array}\right]=F_T^n+M_3{\stackrel{\‾}{n}}^n\\ {\stackrel{\‾}{n}}_i^n=\frac{EA}{l}\[\left|{\stackrel{\‾}{r}}_i^{\′}\right|-l+\mathrm{\α}(\frac{{\stackrel{\‾}{r}}_i^{\′}\·{\stackrel{\·}{\stackrel{\‾}{r}}}_i^{\′}}{\left|{\stackrel{\‾}{r}}_i^{\′}\right|}-\frac{\stackrel{\·}{l}}{l}|{\stackrel{\‾}{r}}_i^{\′}\left|\right)\]\frac{{\stackrel{\‾}{r}}_i^{\′}}{\left|{\stackrel{\‾}{r}}_i^{\′}\right|}\\ \begin{array}{cc}{r}_{CP}-{\stackrel{\‾}{r}}_0^n=0& r_{CM}-{\stackrel{\‾}{r}}_n^n=0\end{array}\end{array}\right.---\left(15\right)$

(5)释放/回收机构动力学方程

$\left\{\begin{array}{c}{N}_{PD}+N_P-N_1^n=0\\ -N_{MD}-N_M+N_n^n=0\end{array}\right.---\left(16\right)$

式中，和为3(n+1)维的向量，为3n维的向量，它们满足：

M₁、M₂、为3(n+1)×3(n+1)维的矩阵，M₃为3(n+1)×3n维的矩阵，它们满足：

$M_1(i,j)=\left\{\begin{array}{cc}\frac{1}{3n}& i=j,1\≤j\≤3\\ \frac{2}{3n}& i=j,4\≤j\≤n-3\\ \frac{1}{3n}& i=j,n-2\≤j\≤n\\ \frac{1}{6n}& i=j-3,4\≤j\≤n\\ \frac{1}{6n}& i=j+3,1\≤j\≤n-3\\ 0& others\end{array}\right.,M_2(i,j)=\left\{\begin{array}{cc}\frac{1}{6n}-\frac{1}{2}& i=j,1\≤i\≤3\\ -\frac{1}{6n}& i=j,4\≤i\≤n-3\\ \frac{1}{6n}& i=j,n-2\≤i\≤n\\ \frac{1}{2}-(\frac{k}{2}-\frac{1}{3})\frac{1}{n}& i+3=j,3k-2\≤i\≤3k,1\≤k\≤n\\ \frac{1}{2}-(\frac{i}{2}-\frac{1}{3})\frac{1}{n}& i=j+3,3k-2\≤i\≤3k,1\≤k\≤n\\ 0& others\end{array}\right.,$