CN106249616A - 一种在轨服务机械臂动力学建模方法和系统 - Google Patents

Abstract

本发明公开一种在轨服务机械臂动力学建模方法，该方法包含：定义机械臂系统的系统坐标系，以及矢量参数和速度参数；确定系统的广义偏速度，并得出该广义偏速度对应的广义惯性力和广义主动力；广义主动力和广义惯性力带入凯恩方程，得到机械臂系统中所有节臂的动力学方程；机械臂系统中所有节臂的动力学方程进行简化，合并成整个机械臂系统的动力学模型。本发明只需要获得广义主动力和广义惯性力，不需要考虑个物体之间的约束力，也不需要计算系统的动能和势能，所以该方法非常简便，克服了其它建模方法计算量大、物理意义不直观、建模复杂度高的缺点。

Description

一种在轨服务机械臂动力学建模方法和系统

技术领域

本发明涉及一种多自由度系统的动力学建模方法，具体涉及一种基于凯恩方程在轨服务多臂多自由度机械臂系统的动力学建模方法和系统。

背景技术

随着空间技术的进步，全球每年发射的卫星和航天器的数量在不断增加，随之而来的是空间工作的多样性和复杂性迅速增长，对舱外活动的需求也日益增加，如对在轨航天器的抓捕、维修和燃料补充，外空间环境的科学实验，对空间垃圾的清理等等。过去这些任务主要由宇航员完成，但这些活动受到很多限制：首先，宇航员的力量与工作范围有限，对于大尺寸的航天设备的安装无能为力；其次，由于氧气供应和外太空辐射等含有危险的原因，宇航员的舱外连续工作时间有限；另外，由于太空微重力、高真空、强辐射和微小行星体的存在，在太空环境下从事各种作业，对宇航员来说是十分困难和危险的；同时为支持宇航员的舱外活动，需要庞大而复杂的环境控制系统和生命保障系统，不可避免的带来巨大的费用支出。为了解决这些问题，在轨服务机械臂系统应运而生。而一种好的在轨服务机械臂动力学建模方法是机械臂结束的核心技术之一。

本质上，所有动力学建模的方法都是等效的。然而，不同的方法的使用难易程度是不同的。目前，多体系统动力学建模的主要方法有3种，即牛顿－欧拉方法、Lagrange方法和Kane方法。牛顿－欧拉方法基本上能解决全部的多体动力学建模问题，但是计算效率很低。牛顿－欧拉方法需要考虑多体系统的每个个体所受到的力和力矩之间的平衡关系，其中包括各个个体之间的相互关联的力和约束力，所以它的效率很低。Lagrange方法不需要虑多体系统的各个个体之间的相互关联的力和约束力，但是它最大的缺点是需要对能量函数进行微分。对于自由度较少的多体系统，计算量不算大，但是对于自由度较多的多体系统，它的计算量和计算效率是一个不容忽略的问题。

相对于牛顿－欧拉方法和Lagrange方法，Kane方法具有它们所没有的优势。因为Kane方法采用广义力的概念，所以可以忽略多体系统的各个个体之间的相互关联的力和约束力。因为Kane方法没有使用能量方程，所以对方程的微分对计算量的影响不大。因此，针对在轨服务机械臂系统的计算量比牛顿－欧拉方法和Lagrange方法要小的多。使用Kane方法能大大提高计算效率，缩短计算时间。

发明内容

本发明提供一种在轨服务机械臂动力学建模方法，具有系统模型计算量小的特点，达到系统运行实时性的目的。

为实现上述目的，本发明提供一种在轨服务机械臂动力学建模方法，其特点是，该方法包含：

S1、定义机械臂系统的系统坐标系，以及矢量参数和速度参数；

S2、确定系统的广义偏速度，并得出该广义偏速度对应的广义惯性力和广义主动力；

S3、广义主动力和广义惯性力带入凯恩方程，得到机械臂系统中所有节臂的动力学方程；

S4、机械臂系统中所有节臂的动力学方程进行简化，合并成整个机械臂系统的动力学模型。

上述定义机械臂系统的系统坐标系包含：

惯性坐标系SI：原点OI位于地球中心，ZI轴沿地球指向北极，XI轴指向春分点，YI轴满足右手定则；

轨道坐标系So：原点Oo位于系统质心，zo轴指向地心，yo轴指向轨道负法线方向，xo轴与其它两轴构成右手坐标系；

中心体连体坐标系Sb：原点Ob位于复杂连接体中心体的质心，xb、yb和zb与中心体惯性主轴平行，且满足右手定则；当中心体相对于轨道坐标系的三个方位角为零时，Sb坐标系与So系一致；

机械臂坐标系Sji(i＝1，2，…M；j＝1，2，…Ni)：设系统有i个机械臂，对应第i个机械臂有Ni节，定义第i个机械臂中第j节臂的机械臂坐标系为：原点位于第i个机械臂系统中的第j节机械臂与其内接体的铰链o_j ⁱ处，坐标系各轴与第j节机械臂固连。

上述定义矢量参数包含：中心体连体坐标系原点在惯性坐标系中的位置矢量中心体质心在中心体连体坐标系内的位置矢量中心体质心在惯性坐标系内的位置矢量第i组机械臂中第一节机械臂在中心体上的铰接点在中心体连体坐标系中的位置矢量第i组机械臂中第j(j＝2,3,…,Ni)节机械臂与第j-1节机械臂的连接点在第j-1节机械臂的连体坐标系中的位置矢量第i组机械臂中第j节机械臂的质心在其自身连体坐标系中的位置矢量第i组机械臂中第j节机械臂的质心在惯性坐标系中的位置矢量

上述定义速度参数包含：中心体连体系相对惯性系的角速度矢量第i组机械臂中第j节机械臂相对其自身连体坐标系的角速度矢量第i组机械臂中第j节机械臂相对惯性坐标系的角速度矢量

上述S2包含：

分析机械臂系统，选定广义偏速度；

根据广义偏速度，计算出该广义偏速度下的广义惯性力和广义主动力；广义惯性力包含：中心体惯性力和第i组机械臂第j节臂的惯性力。

上述中心体惯性力包含：

中心体第k(k>2)阶广义惯性力如式(1)：

$f_{Ik,B}^{*}=-{\∫}_B({\stackrel{\→}{a}}_B\·{\stackrel{\→}{G}}_{k,B})dm=0,k>2---\left(1\right)$

其中，为B体的绝对加速度，为第k阶广义偏速度；

上述第i组机械臂第j节臂的惯性力包含：

第i组机械臂第j节臂的第2+N*(i-1)+l阶广义惯性力如式(2)：

$\begin{array}{c}{f}_{In,j}^{*i}=-{\∫}_{i,j}({\stackrel{\→}{a}}_j^i\·{\stackrel{\→}{G}}_{n,j}^i)dm\\ =-{\∫}_{i,j}({\tilde{r}}_{l,j}^{iT}+A_{lj}^{i_1}{\tilde{r}}_j^{iT}{A}_{jl}^i)e_l^{iT}\·\left[\begin{array}{c}{e}_I^T\stackrel{\·\·}{R}-e_b^T({\tilde{r}}_{b,j}^i+A_{bj}^i{\tilde{r}}_j^i{A}_{jb}^i){\stackrel{\·}{\mathrm{\ω}}}_b-\underset{k=1}{\overset{j}{\Σ}}{e_k^i}^T({\tilde{r}}_{k,j}^i+A_{kj}^i{\tilde{r}}_j^i{A}_{jk}^i){\stackrel{\·}{\mathrm{\ω}}}_k^i\\ -e_b^T{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_{b,1}^i{\mathrm{\ω}}_b-\underset{k=1}{\overset{j-1}{\Σ}}{e_k^i}^T{\widetilde{\mathrm{\Ω}}}_k^i{\tilde{r}}_{k,k+1}^i{\mathrm{\Ω}}_k^i-{e_j^i}^T{\widetilde{\mathrm{\Ω}}}_j^i{\tilde{r}}_j^i{\mathrm{\Ω}}_j^i\\ -e_1^{iT}({\tilde{r}}_{1,j}^i+A_{1j}^i{\tilde{r}}_j^i{A}_{j1}^i)A_{1b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i\\ -\underset{k=2}{\overset{j}{\Σ}}{e_k^i}^T({\tilde{r}}_{k,j}^i+A_{kj}^i{\tilde{r}}_j^i{A}_{jk}^i)A_{k,k-1}^i{\widetilde{\mathrm{\Ω}}}_{k-1}^i{A}_{k-1,k}^i{\mathrm{\Ω}}_k^i\end{array}\right]dm\\ (n=2+N*(i-1)+l,l\≤j)\end{array}---\left(2\right)$

其中，为第j节臂绝对加速度，为j节机械臂上第n阶广义偏速度，上式中所有e都为连体基，其下标表示为在哪个坐标系下，上标表示为第几组，T表示转置；A表示方向余弦矩阵，下标表示为后者对前者的方向余弦矩阵，上标表示在哪个坐标系下；表示位置的叉乘矩阵，下标为后一点相对前一点，上标为在哪个坐标系下；ω表示角速度，下标表示是那个体；Ω为关节旋转角速度，下标表示哪个体，上标表示相对那个坐标系。

上述S3包含：

把广义主动力和广义惯性力带入凯恩方程，得到N*M+2个力学平衡方程，包含：中心体平动方程、中心体转动方程和第i组机械臂第j节臂的转动方程(i＝1，2，…M；j＝1，2，…N)：

中心体平动方程包含：

中心体平动方程对应于第一阶广义速度如式(3)：

$\begin{array}{c}m\stackrel{\·\·}{R}-A_{Ib}{\tilde{S}}_{ot}{\stackrel{\·}{\mathrm{\ω}}}_b-\underset{i=1}{\overset{3}{\Σ}}\underset{j=1}{\overset{7}{\Σ}}{A}_{Ij}^i{\tilde{S}}_{jt}^i{\stackrel{\·}{\mathrm{\ω}}}_j^i-A_{Ib}{\widetilde{\mathrm{\ω}}}_b{\tilde{S}}_b^{*}{\mathrm{\ω}}_b-\underset{i=1}{\overset{3}{\Σ}}\underset{j=1}{\overset{7}{\Σ}}{A}_{Ij}^i{\widetilde{\mathrm{\Ω}}}_j^i{\tilde{S}}_j^{i*}{\mathrm{\Ω}}_j^i\\ -\underset{i=1}{\overset{3}{\Σ}}\left(A_{I1}^i{\tilde{S}}_{1t}^i{A}_{1b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i+\underset{j=2}{\overset{7}{\Σ}}{A}_{Ij}^i{\tilde{S}}_{jt}^i{A}_{j,j-1}^i{\widetilde{\mathrm{\Ω}}}_{j-1}^i{A}_{j-1,j}^i{\mathrm{\Ω}}_j^i\right)=F\end{array}---\left(3\right);$

中心体转动方程包含：

中心体转动方程对应于第二阶广义速度ω_b，如式(4)：

$\begin{array}{c}-{\tilde{S}}_{ot}^T{A}_{bI}\stackrel{\·\·}{R}+I_{ot}{\stackrel{\·}{\mathrm{\ω}}}_b+\underset{i=1}{\overset{3}{\Σ}}\underset{j=1}{\overset{7}{\Σ}}{A}_{bj}^i{I}_{j\_o}^i{\stackrel{\·}{\mathrm{\ω}}}_j^i+I_{\mathrm{\ω}}{\mathrm{\ω}}_b+\underset{i=1}{\overset{3}{\Σ}}\underset{j=1}{\overset{7}{\Σ}}{A}_{bj}^i{I}_{\mathrm{\Ω}j\_o}^i{\mathrm{\Ω}}_j^i\\ +\underset{i=1}{\overset{3}{\Σ}}\left(A_{b1}^i{I}_{1\_o}^i{A}_{1b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i+\underset{j=2}{\overset{7}{\Σ}}{A}_{bj}^i{I}_{j\_o}^i{A}_{j,j-1}^i{\widetilde{\mathrm{\Ω}}}_{j-1}^i{A}_{j-1,j}^i{\mathrm{\Ω}}_j^i\right)=T\end{array}---\left(4\right);$

第i组机械臂第N节臂的转动方程包含：

第i组机械臂第N节臂的转动方程对应于第(2+N*(i-1)+N)阶广义速度如式(5)

$\begin{array}{c}-{{\tilde{S}}_{Nt}^i}^T{A}_{NI}^i\stackrel{\·\·}{R}+{I_{N\_o}^i}^T{A}_{Nb}^i{\stackrel{\·}{\mathrm{\ω}}}_b+\underset{j=1}{\overset{N-1}{\Σ}}{I_{N\_j}^i}^T{A}_{Nj}^i{\stackrel{\·}{\mathrm{\ω}}}_j^i+I_{Nt}^i{\stackrel{\·}{\mathrm{\ω}}}_N^i+{{\tilde{S}}_{Nt}^i}^T{A}_{Nb}^i{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_{b,1}^i{\mathrm{\ω}}_b\\ +\underset{j=1}{\overset{N-1}{\Σ}}{A}_{Nj}^i{I}_{{\mathrm{\Ω}}_{j\_}N}^i{\mathrm{\Ω}}_j^i+I_{{\mathrm{\Ω}}_N}^i{\mathrm{\Ω}}_N^i+{I_{N\_1}^i}^T{A}_{Nb}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i\\ +\underset{j=2}{\overset{N-1}{\Σ}}{I_{N\_j}^i}^T{A}_{N,j-1}^i{\widetilde{\mathrm{\Ω}}}_{j-1}^i{A}_{j-1,j}^i{\mathrm{\Ω}}_j^i+I_{Nt}^i{A}_{N(N-1)}^i{\widetilde{\mathrm{\Ω}}}_{(N-1)}^i{A}_{(N-1)N}^i{\mathrm{\Ω}}_N^i=T_N^i\end{array}---\left(5\right).$

上述S4中，经整理后的机械臂系统的动力学方程如式(6)：

$\underset{~}{M}\underset{~}{\stackrel{\·}{Y}}=\underset{~}{Q}-{\underset{~}{Q}}_N---\left(6\right)$

式中为系统的质量阵，表达式如式(7)：

$\underset{~}{M}=\left[\begin{array}{cccccccc}{\mathrm{mE}}_3& -A_{Ib}{\tilde{S}}_t& -A_{I1}^1{S}_{1t}^1\mathrm{...}& -A_{I7}^1{S}_{Nt}^1& -A_{I1}^2{S}_{1t}^2\mathrm{...}& -A_{IN}^2{S}_{Nt}^2\mathrm{...}& -A_{I1}^M{S}_{1t}^M\mathrm{...}& -A_{IN}^M{S}_{7N}^M\\ & I_t& A_{b1}^1{I}_{1\_o}^1\mathrm{...}& A_{bN}^1{I}_{N\_o}^1& A_{b1}^2{I}_{1\_o}^2\mathrm{...}& A_{bN}^2{I}_{N\_o}^2\mathrm{...}& A_{b1}^M{I}_{1\_o}^M\mathrm{...}& A_{bN}^M{I}_{N\_o}^M\\ & & I_{1t}^1\mathrm{...}& A_{1N}^1{I}_{N\_1}^1& 0& 0& 0& 0\\ \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}\\ & & & I_{Nt}^1& 0& 0& 0& 0\\ & & & & I_{1t}^2\mathrm{...}& A_{1N}^2{I}_{N\_1}^2& 0& 0\\ \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}\\ & & & & & I_{Nt}^2& 0& 0\\ & & & & & & I_{1t}^n\mathrm{...}& A_{1N}^M{I}_{N\_1}^M\\ \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}\\ & & & & & & & I_{Nt}^M\end{array}\right]---\left(7\right)$

质量阵为对称正定阵，因此只给出了对角线及以上的元素；

为系统的广义速度列阵，表达式如式(8)：

$\underset{~}{Y}={\left[\begin{array}{cccccccccccc}{\stackrel{\·}{R}}^T& {\mathrm{\ω}}_b^T& {\mathrm{\ω}}_1^{1T}& \mathrm{...}& {\mathrm{\ω}}_N^{1T}& {\mathrm{\ω}}_1^{2T}& \mathrm{...}& {\mathrm{\ω}}_N^{2T}& \mathrm{...}& {\mathrm{\ω}}_1^{MT}& \mathrm{...}& {\mathrm{\ω}}_N^{MT}\end{array}\right]}^T---\left(8\right)$

为广义力列阵，表达式如式(9)：

$\underset{~}{Q}={\left[\begin{array}{cccccccccccc}{F}^T& T^T& T_1^{1T}& \mathrm{...}& T_N^{1T}& T_1^{2T}& \mathrm{...}& T_N^{2T}& \mathrm{...}& T_1^{MT}& \mathrm{...}& T_N^{MT}\end{array}\right]}^T---\left(9\right).$

上述建模方法用于航天器本体及搭载在航天器本体上的N个多自由度机械臂的联合机械臂系统的动力学建模。

一种在轨服务机械臂动力学建模系统，其特点是，该系统包含：

坐标系定义模块，其定义机械臂系统的系统坐标系，以及矢量参数和速度参数；

广义惯性力和广义主动力计算模块，其接收系统坐标系、矢量参数和速度参数，确定系统的广义偏速度，并得出该广义偏速度对应的广义惯性力和广义主动力；

动力学方程建立模块，其接收广义惯性力和广义主动力计算模块带入凯恩方程，得到机械臂系统中所有节臂的动力学方程；

动力学模型建立模块，其接收机械臂系统中所有节臂的动力学方程并进行简化，合并成整个机械臂系统的动力学模型。

本发明一种在轨服务机械臂动力学建模方法和系统和现有技术相比，其优点在于，本发明只需要获得广义主动力和广义惯性力，不需要考虑个物体之间的约束力，也不需要计算系统的动能和势能，所以该方法非常简便，克服了其它建模方法计算量大、物理意义不直观、建模复杂度高的缺点。

附图说明

图1为本发明一种在轨服务机械臂动力学建模方法的流程图；

图2为在轨服务机械臂系统的构型图。

具体实施方式

以下结合附图，进一步说明本发明的具体实施例。

如图1所示，本发明公开了一种采用凯恩原理在轨服务机械臂动力学建模方法，该方法以航天器本体及搭载在航天器本体上的N个多自由度机械臂的联合机械臂系统动力学建模的前提条件为基础，该方法具体包含以下步骤：

S1、如图2所示，定义机械臂系统的系统坐标系，以及矢量参数和速度参数。

系统坐标系包含：

惯性坐标系S_I(O_IX_IY_IZ_I)：原点O_I位于地球中心，Z_I轴沿地球指向北极，X_I轴指向春分点，Y_I轴满足右手定则。

轨道坐标系S_o(O_ox_oy_oz_o)：原点O_o位于系统质心，z_o轴指向地心，y_o轴指向轨道负法线方向，x_o轴与其它两轴构成右手坐标系。

中心体连体坐标系S_b(O_bx_by_bz_b)：原点O_b位于复杂连接体中心体的质心，x_b、y_b和z_b与中心体惯性主轴平行，且满足右手定则；当中心体相对于轨道坐标系的三个方位角为零时，S_b坐标系与S_o系一致。

机械臂坐标系S_ji(o_jix_jiy_jiz_ji)((i＝1，2，…M；j＝1，2，…Ni)：设系统有i个机械臂，对应第i个机械臂有Ni节，定义第i个机械臂中第j节臂的机械臂坐标系为：原点位于第i个机械臂系统中的第j节机械臂与其内接体的铰链oji处，坐标系各轴与第j节机械臂固连。

定义的矢量参数包含：中心体连体坐标系原点在惯性坐标系中的位置矢量中心体质心在中心体连体坐标系内的位置矢量中心体质心在惯性坐标系内的位置矢量第i组机械臂中第一节机械臂在中心体上的铰接点在中心体连体坐标系中的位置矢量第i组机械臂中第j(j＝2,3,…,Ni)节机械臂与第j-1节机械臂的连接点在第j-1节机械臂的连体坐标系中的位置矢量第i组机械臂中第j节机械臂的质心在其自身连体坐标系中的位置矢量第i组机械臂中第j节机械臂的质心在惯性坐标系中的位置矢量

定义的速度参数包含：中心体连体系相对惯性系的角速度矢量第i组机械臂中第j节机械臂相对其自身连体坐标系的角速度矢量第i组机械臂中第j节机械臂相对惯性坐标系的角速度矢量

S2、选择系统广义偏速度，并计算出所选择的偏速度下对应的广义主动力和广义惯性力。

首先，对系统进行分析，选择适当的广义偏速度。偏速度包括：中心体偏速度向量和各个机械臂各节臂的的偏速度向量。然后，根据所选择的广义偏速度，计算出所选择的偏速度下对应的广义主动力和广义惯性力。

S2.1、选择广义偏速度：

表1、中心体偏速度向量

表2第i组机械臂第j节臂的偏速度向量

S2.2、计算广义惯性力。

1)中心体惯性力：

a)中心体第1阶广义惯性力如下：

$\begin{array}{c}{f}_{I1,B}^{*}=-{\∫}_B({\stackrel{\→}{a}}_B\·{\stackrel{\→}{G}}_{1,B})dm\\ =-{\∫}_B{E}_3{e}_I\[e_I^T\stackrel{\·\·}{R}-e_b^T{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_b{\mathrm{\ω}}_b-e_b^T{\tilde{r}}_b{\stackrel{\·}{\mathrm{\ω}}}_b\]dm\\ =-{\∫}_B\[\stackrel{\·\·}{R}-A_{Ib}{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_b{\mathrm{\ω}}_b-A_{Ib}{\tilde{r}}_b{\stackrel{\·}{\mathrm{\ω}}}_b\]dm\\ =-m_B\stackrel{\·\·}{R}+A_{Ib}{\widetilde{\mathrm{\ω}}}_b{\tilde{S}}_b{\mathrm{\ω}}_b+A_{Ib}{\tilde{S}}_b{\stackrel{\·}{\mathrm{\ω}}}_b\end{array}$

其中：S_b＝∫_Br_bdm。

b)中心体第2阶广义惯性力如下：

$\begin{array}{c}{f}_{I2,B}^{*}=-{\∫}_B({\stackrel{\→}{a}}_B\·{\stackrel{\→}{G}}_{2,B})dm\\ =-{\∫}_B-{\tilde{r}}_b^T{e}_b\·\[e_I^T\stackrel{\·\·}{R}-e_b^T{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_b{\mathrm{\ω}}_b-e_b^T{\tilde{r}}_b{\stackrel{\·}{\mathrm{\ω}}}_b\]dm\\ ={\∫}_B{\tilde{r}}_b^T\[A_{Ib}\stackrel{\·\·}{R}-{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_b{\mathrm{\ω}}_b-{\tilde{r}}_b{\stackrel{\·}{\mathrm{\ω}}}_b\]dm\\ ={\tilde{S}}_b^T{A}_{Ib}\stackrel{\·\·}{R}-J_{\mathrm{\ω}b}{\mathrm{\ω}}_b-J_b{\stackrel{\·}{\mathrm{\ω}}}_b\end{array}$

其中：

c)中心体第k(k>2)阶广义惯性力如式(1)：

$f_{Ik,B}^{*}=-{\∫}_B({\stackrel{\→}{a}}_B\·{\stackrel{\→}{G}}_{k,B})dm=0,k>2---\left(1\right)$

其中，为B体的绝对加速度，为第k阶广义偏速度。

2)第i组机械臂第j节臂的惯性力：

a)第i组机械臂第j节臂的第1阶广义惯性力如下：

$\begin{array}{c}{f}_{I1,j}^{*i}=-{\∫}_{i,j}({\stackrel{\→}{a}}_j^i\·{\stackrel{\→}{G}}_{1,j}^i)dm\\ =-\left[\begin{array}{c}{m}_j^i\stackrel{\·\·}{R}-A_{Ib}(m_j^i{\tilde{r}}_{b,j}^i+A_{bj}^i{\tilde{S}}_j^i{A}_{jb}^i){\stackrel{\·}{\mathrm{\ω}}}_b-\underset{k=1}{\overset{j}{\Σ}}{A}_{Ik}^i(m_j^i{\tilde{r}}_{k,j}^i+A_{kj}^i{\tilde{S}}_j^i{A}_{jk}^i){\stackrel{\·}{\mathrm{\ω}}}_k^i\\ -m_j^i{A}_{Ib}{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_{b,1}^i{\mathrm{\ω}}_b-\underset{k=1}{\overset{j-1}{\Σ}}{m}_j^i{A}_{Ik}^i{\widetilde{\mathrm{\Ω}}}_k^i{\tilde{r}}_{k,k+1}^i{\mathrm{\Ω}}_k^i-A_{Ij}^i{\widetilde{\mathrm{\Ω}}}_j^i{\tilde{S}}_j^i{\mathrm{\Ω}}_j^i\\ -A_{I1}^i(m_j^i{\tilde{r}}_{1,j}^i+A_{1j}^i{\tilde{S}}_j^i{A}_{j1}^i)A_{1b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i\\ -\underset{k=2}{\overset{j}{\Σ}}{A}_{Ik}^i(m_j^i{\tilde{r}}_{k,j}^i+A_{kj}^i{\tilde{S}}_j^i{A}_{jk}^i)A_{k,k-1}^i{\widetilde{\mathrm{\Ω}}}_{k-1}^i{A}_{k-1,k}^i{\mathrm{\Ω}}_k^i\end{array}\right]\end{array}$

第i组机械臂第j节臂的第2阶广义惯性力如下：

$\begin{array}{c}{f}_{I2,j}^{*i}=-{\∫}_{i,j}({\stackrel{\→}{a}}_j^i\·{\stackrel{\→}{G}}_{2,j}^i)dm\\ =-{\∫}_{i,j}({\tilde{r}}_{b,j}^{iT}+A_{bj}^i{\tilde{r}}_j^{iT}{A}_{jb}^i)\·\left[\begin{array}{c}{A}_{bI}\stackrel{\·\·}{R}-({\tilde{r}}_{b,j}^i+A_{bj}^i{\tilde{r}}_j^i{A}_{jb}^i){\stackrel{\·}{\mathrm{\ω}}}_b-\underset{k=1}{\overset{j}{\Σ}}{A}_{bk}^i({\tilde{r}}_{k,j}^i+A_{kj}^i{\tilde{r}}_j^i{A}_{jk}^i){\stackrel{\·}{\mathrm{\ω}}}_k^i\\ -{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_{b,1}^i{\mathrm{\ω}}_b-\underset{k=1}{\overset{j-1}{\Σ}}{A}_{bk}^i{\widetilde{\mathrm{\Ω}}}_k^i{\tilde{r}}_{k,k+1}^i{\mathrm{\Ω}}_k^i-A_{bj}^i{\widetilde{\mathrm{\Ω}}}_j^i{\tilde{r}}_j^i{\mathrm{\Ω}}_j^i\\ -A_{b1}^i({\tilde{r}}_{1,j}^i+A_{1j}^i{\tilde{r}}_j^i{A}_{j1}^i)A_{1b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i\\ -\underset{k=2}{\overset{j}{\Σ}}{A}_{bk}^i({\tilde{r}}_{k,j}^i+A_{kj}^i{\tilde{r}}_j^i{A}_{jk}^i)A_{k,k-1}^i{\widetilde{\mathrm{\Ω}}}_{k-1}^i{A}_{k-1,k}^i{\mathrm{\Ω}}_k^i\end{array}\right]dm\end{array}$

第i组机械臂第j节臂的第2+N*(i-1)+l阶广义惯性力如式(2)：

$\begin{array}{c}{f}_{In,j}^{*i}=-{\∫}_{i,j}({\stackrel{\→}{a}}_j^i\·{\stackrel{\→}{G}}_{n,j}^i)dm\\ =-{\∫}_{i,j}({\tilde{r}}_{l,j}^{iT}+A_{lj}^{i_1}{\tilde{r}}_j^{iT}{A}_{jl}^i)e_l^{iT}\·\left[\begin{array}{c}{e}_I^T\stackrel{\·\·}{R}-e_b^T({\tilde{r}}_{b,j}^i+A_{bj}^i{\tilde{r}}_j^i{A}_{jb}^i){\stackrel{\·}{\mathrm{\ω}}}_b-\underset{k=1}{\overset{j}{\Σ}}{e_k^i}^T({\tilde{r}}_{k,j}^i+A_{kj}^i{\tilde{r}}_j^i{A}_{jk}^i){\stackrel{\·}{\mathrm{\ω}}}_k^i\\ -e_b^T{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_{b,1}^i{\mathrm{\ω}}_b-\underset{k=1}{\overset{j-1}{\Σ}}{e_k^i}^T{\widetilde{\mathrm{\Ω}}}_k^i{\tilde{r}}_{k,k+1}^i{\mathrm{\Ω}}_k^i-{e_j^i}^T{\widetilde{\mathrm{\Ω}}}_j^i{\tilde{r}}_j^i{\mathrm{\Ω}}_j^i\\ -e_1^{iT}({\tilde{r}}_{1,j}^i+A_{1j}^i{\tilde{r}}_j^i{A}_{j1}^i)A_{1b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i\\ -\underset{k=2}{\overset{j}{\Σ}}{e_k^i}^T({\tilde{r}}_{k,j}^i+A_{kj}^i{\tilde{r}}_j^i{A}_{jk}^i)A_{k,k-1}^i{\widetilde{\mathrm{\Ω}}}_{k-1}^i{A}_{k-1,k}^i{\mathrm{\Ω}}_k^i\end{array}\right]dm\\ (n=2+N*(i-1)+l,l\≤j)\end{array}---\left(2\right)$

其中，为第j节臂绝对加速度，为j节机械臂上第n阶广义偏速度，上式中所有e都为连体基，其下标表示为在哪个坐标系下，上标表示为第几组，T表示转置；A表示方向余弦矩阵，下标表示为后者对前者的方向余弦矩阵，上标表示在哪个坐标系下；表示位置的叉乘矩阵，下标为后一点相对前一点，上标为在哪个坐标系下；ω表示角速度，下标表示是那个体；Ω为关节旋转角速度，下标表示哪个体，上标表示相对那个坐标系。

S2.3、计算系统广义主动力，如表3。

表3广义主动力

S3、把广义主动力和广义惯性力带入凯恩方程，得到机械臂系统中所有节臂的动力学方程，具体得到N*M+2个力学平衡方程，包含：中心体平动方程、中心体转动方程和第i组机械臂第j节臂的转动方程(i＝1，2，…M；j＝1，2，…N)。

1)中心体平动方程包含：

中心体平动方程对应于第一阶广义速度如式(3)：

$\begin{array}{c}m\stackrel{\·\·}{R}-A_{Ib}{\tilde{S}}_{ot}{\stackrel{\·}{\mathrm{\ω}}}_b-\underset{i=1}{\overset{3}{\Σ}}\underset{j=1}{\overset{7}{\Σ}}{A}_{Ij}^i{\tilde{S}}_{jt}^i{\stackrel{\·}{\mathrm{\ω}}}_j^i-A_{Ib}{\widetilde{\mathrm{\ω}}}_b{\tilde{S}}_b^{*}{\mathrm{\ω}}_b-\underset{i=1}{\overset{3}{\Σ}}\underset{j=1}{\overset{7}{\Σ}}{A}_{Ij}^i{\widetilde{\mathrm{\Ω}}}_j^i{\tilde{S}}_j^{i*}{\mathrm{\Ω}}_j^i\\ -\underset{i=1}{\overset{3}{\Σ}}\left(A_{I1}^i{\tilde{S}}_{1t}^i{A}_{1b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i+\underset{j=2}{\overset{7}{\Σ}}{A}_{Ij}^i{\tilde{S}}_{jt}^i{A}_{j,j-1}^i{\widetilde{\mathrm{\Ω}}}_{j-1}^i{A}_{j-1,j}^i{\mathrm{\Ω}}_j^i\right)=F\end{array}---\left(3\right);$

2)中心体转动方程包含：

中心体转动方程对应于第二阶广义速度ω_b，如式(4)：

$\begin{array}{c}-{\tilde{S}}_{ot}^T{A}_{bI}\stackrel{\·\·}{R}+I_{ot}{\stackrel{\·}{\mathrm{\ω}}}_b+\underset{i=1}{\overset{3}{\Σ}}\underset{j=1}{\overset{7}{\Σ}}{A}_{bj}^i{I}_{j\_o}^i{\stackrel{\·}{\mathrm{\ω}}}_j^i+I_{\mathrm{\ω}}{\mathrm{\ω}}_b+\underset{i=1}{\overset{3}{\Σ}}\underset{j=1}{\overset{7}{\Σ}}{A}_{bj}^i{I}_{\mathrm{\Ω}j\_o}^i{\mathrm{\Ω}}_j^i\\ +\underset{i=1}{\overset{3}{\Σ}}\left(A_{b1}^i{I}_{1\_o}^i{A}_{1b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i+\underset{j=2}{\overset{7}{\Σ}}{A}_{bj}^i{I}_{j\_o}^i{A}_{j,j-1}^i{\widetilde{\mathrm{\Ω}}}_{j-1}^i{A}_{j-1,j}^i{\mathrm{\Ω}}_j^i\right)=T\end{array}---\left(4\right);$

3)第i组机械臂第1节臂的转动方程包含：

第i组机械臂第1节臂的转动方程对应于第(2+N*(i-1)+1)阶广义速度因此有：

$\begin{array}{c}-{{\tilde{S}}_{1t}^i}^T{A}_{1I}^i\stackrel{\·\·}{R}+{I_{1\_o}^i}^T{A}_{1b}^i{\stackrel{\·}{\mathrm{\ω}}}_b+I_{1t}^i{\stackrel{\·}{\mathrm{\ω}}}_1^i+\underset{j=2}{\overset{7}{\Σ}}{A}_{1j}^i{I}_{j\_1}^i{\stackrel{\·}{\mathrm{\ω}}}_j^i+{{\tilde{S}}_{1t}^i}^T{A}_{1b}^i{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_{b,1}^i{\mathrm{\ω}}_b+I_{{\mathrm{\Ω}}_1}^i{\mathrm{\Ω}}_1^i\\ +\underset{j=2}{\overset{7}{\Σ}}{A}_{1j}^i{I}_{{\mathrm{\Ω}}_j\_1}^i{\mathrm{\Ω}}_j^i+I_{1t}^i{A}_{1b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i+\underset{j=2}{\overset{7}{\Σ}}{A}_{1j}^i{I}_{j\_1}^i{A}_{j,j-1}^i{\widetilde{\mathrm{\Ω}}}_{j-1}^i{A}_{j-1,j}^i{\mathrm{\Ω}}_j^i=T_1^i\end{array};$

4)第i组机械臂第2节臂的转动方程包含：

第i组机械臂第2节臂的转动方程对应于第(2+N*(i-1)+2)阶广义速度因此有：

$\begin{array}{c}-{{\tilde{S}}_{2t}^i}^T{A}_{2I}^i\stackrel{\·\·}{R}+{I_{2\_o}^i}^T{A}_{2b}^i{\stackrel{\·}{\mathrm{\ω}}}_b+{I_{2\_1}^i}^T{A}_{21}^i{\stackrel{\·}{\mathrm{\ω}}}_1^i+I_{2t}^i{\stackrel{\·}{\mathrm{\ω}}}_2^i+\underset{j=3}{\overset{7}{\Σ}}{A}_{2j}^i{I}_{j\_2}^i{\stackrel{\·}{\mathrm{\ω}}}_j^i\\ +{{\tilde{S}}_{2t}^i}^T{A}_{2b}^i{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_{b,2}^i{\mathrm{\ω}}_b+A_{21}^i{I}_{{\mathrm{\Ω}}_{1\_}2}^i{\mathrm{\Ω}}_1^i+I_{{\mathrm{\Ω}}_2}^i{\mathrm{\Ω}}_2^i+\underset{j=3}{\overset{7}{\Σ}}{A}_{2j}^i{I}_{{\mathrm{\Ω}}_j\_2}^i{\mathrm{\Ω}}_j^i\\ +{I_{2\_1}^i}^T{A}_{2b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b,1}^i{\mathrm{\Ω}}_1^i+I_{2t}^i{A}_{21}^i{\widetilde{\mathrm{\Ω}}}_1^i{A}_{12}^i{\mathrm{\Ω}}_2^i+\underset{j=3}{\overset{7}{\Σ}}{A}_{2j}^i{I}_{j\_2}^i{A}_{j,j-1}^i{\widetilde{\mathrm{\Ω}}}_{j-1}^i{A}_{j-1,j}^i{\mathrm{\Ω}}_j^i=T_2^i\end{array};$

5)第i组机械臂第3节臂的转动方程包含

第i组机械臂第3节臂的转动方程对应于第(2+N*(i-1)+3)阶广义速度因此有：

$\begin{array}{c}-{{\tilde{S}}_{3t}^i}^T{A}_{3I}^i\stackrel{\·\·}{R}+{I_{3\_o}^i}^T{A}_{3b}^i{\stackrel{\·}{\mathrm{\ω}}}_b+\underset{j=1}{\overset{2}{\Σ}}{I_{3\_j}^i}^T{A}_{3j}^i{\stackrel{\·}{\mathrm{\ω}}}_j^i+I_{3t}^i{\stackrel{\·}{\mathrm{\ω}}}_3^i+\underset{j=4}{\overset{7}{\Σ}}{A}_{3j}^i{I}_{j\_3}^i{\stackrel{\·}{\mathrm{\ω}}}_j^i\\ +{{\tilde{S}}_{3t}^i}^T{A}_{3b}^i{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_{b,1}^i{\mathrm{\ω}}_b+\underset{j=1}{\overset{2}{\Σ}}{A}_{3j}^i{I}_{{\mathrm{\Ω}}_{j\_}3}^i{\mathrm{\Ω}}_j^i+I_{{\mathrm{\Ω}}_3}^i{\mathrm{\Ω}}_3^i+\underset{j=4}{\overset{7}{\Σ}}{A}_{3j}^i{I}_{{\mathrm{\Ω}}_j\_3}^i{\mathrm{\Ω}}_j^i\\ +{I_{3\_1}^i}^T{A}_{3b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i+{I_{3\_2}^i}^T{A}_{31}^i{\widetilde{\mathrm{\Ω}}}_1^i{A}_{12}^i{\mathrm{\Ω}}_2^i\\ +I_{3t}^i{A}_{32}^i{\widetilde{\mathrm{\Ω}}}_2^i{A}_{23}^i{\mathrm{\Ω}}_3^i+\underset{j=4}{\overset{7}{\Σ}}{A}_{3j}^i{I}_{j\_3}^i{A}_{j,j-1}^i{\widetilde{\mathrm{\Ω}}}_{j-1}^i{A}_{j-1,j}^i{\mathrm{\Ω}}_j^i=T_3^i\\ \mathrm{......}\\ \mathrm{......}\end{array};$

6)第i组机械臂第N-1节臂的转动方程包含：

第i组机械臂第N-1节臂的转动方程对应于第(2+N*(i-1)+N-1)阶广义速度因此有：

$\begin{array}{c}-{{\tilde{S}}_{(N-1)t}^i}^T{A}_{(N-1)I}^i\stackrel{\·\·}{R}+{I_{(N-1)\_o}^i}^T{A}_{(N-1)b}^i{\stackrel{\·}{\mathrm{\ω}}}_b+\underset{j=1}{\overset{N-2}{\Σ}}{I_{(N-1)\_j}^i}^T{A}_{(N-1)j}^i{\stackrel{\·}{\mathrm{\ω}}}_j^i+I_{(N-1)t}^i{\stackrel{\·}{\mathrm{\ω}}}_{(N-1)}^i\\ +A_{(N-1)N}^i{I}_{N\_(N-1)}^i{\stackrel{\·}{\mathrm{\ω}}}_N^i+{{\tilde{S}}_{(N-1)t}^i}^T{A}_{(N-1)b}^i{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_{b,1}^i{\mathrm{\ω}}_b+\underset{j=1}{\overset{N-2}{\Σ}}{A}_{(N-1)j}^i{I}_{{\mathrm{\Ω}}_{j\_}(N-1)}^i{\mathrm{\Ω}}_j^i+\\ I_{{\mathrm{\Ω}}_{(N-1)}}^i{\mathrm{\Ω}}_{(N-1)}^i+A_{(N-1)N}^i{I}_{{\mathrm{\Ω}}_N\_(N-1)}^i{\mathrm{\Ω}}_{(N-1)}^i+{I_{(N-1)\_1}^i}^T{A}_{(N-1)b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i+\\ \underset{j=2}{\overset{N-2}{\Σ}}{I_{(N-1)\_j}^i}^T{A}_{(N-1),j-1}^i{\widetilde{\mathrm{\Ω}}}_{j-1}^i{A}_{j-1,j}^i{\mathrm{\Ω}}_j^i+I_{(N-1)t}^i{A}_{(N-1)(N-2)}^i{\widetilde{\mathrm{\Ω}}}_{(N-2)}^i{A}_{(N-2)(N-1)}^i{\mathrm{\Ω}}_{(N-1)}^i\\ +A_{(N-1)N}^i{I}_{N\_(N-1)}^i{A}_{N(N-1)}^i{\widetilde{\mathrm{\Ω}}}_{(N-1)}^i{A}_{(N-1)N}^i{\mathrm{\Ω}}_N^i=T_{(N-1)}^i\end{array};$

7)第i组机械臂第N节臂的转动方程包含：

第i组机械臂第N节臂的转动方程对应于第(2+N*(i-1)+N)阶广义速度如式(5)

$\begin{array}{c}-{{\tilde{S}}_{Nt}^i}^T{A}_{NI}^i\stackrel{\·\·}{R}+{I_{N\_o}^i}^T{A}_{Nb}^i{\stackrel{\·}{\mathrm{\ω}}}_b+\underset{j=1}{\overset{N-1}{\Σ}}{I_{N\_j}^i}^T{A}_{Nj}^i{\stackrel{\·}{\mathrm{\ω}}}_j^i+I_{Nt}^i{\stackrel{\·}{\mathrm{\ω}}}_N^i+{{\tilde{S}}_{Nt}^i}^T{A}_{Nb}^i{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_{b,1}^i{\mathrm{\ω}}_b\\ +\underset{j=1}{\overset{N-1}{\Σ}}{A}_{Nj}^i{I}_{{\mathrm{\Ω}}_{j\_}N}^i{\mathrm{\Ω}}_j^i+I_{{\mathrm{\Ω}}_N}^i{\mathrm{\Ω}}_N^i+{I_{N\_1}^i}^T{A}_{Nb}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i\\ +\underset{j=2}{\overset{N-1}{\Σ}}{I_{N\_j}^i}^T{A}_{N,j-1}^i{\widetilde{\mathrm{\Ω}}}_{j-1}^i{A}_{j-1,j}^i{\mathrm{\Ω}}_j^i+I_{Nt}^i{A}_{N(N-1)}^i{\widetilde{\mathrm{\Ω}}}_{(N-1)}^i{A}_{(N-1)N}^i{\mathrm{\Ω}}_N^i=T_N^i\end{array}---\left(5\right)$

S4、机械臂系统中所有节臂的动力学方程进行简化，合并成整个机械臂系统的动力学模型。

对S3计算得到的方程，经整理后的机械臂系统的动力学方程如式(6)：

$\underset{~}{M}\underset{~}{\stackrel{\·}{Y}}=\underset{~}{Q}-{\underset{~}{Q}}_N---\left(6\right);$

式中为系统的质量阵，表达式如式(7)：

$\underset{~}{M}=\left[\begin{array}{cccccccc}{\mathrm{mE}}_3& -A_{Ib}{\tilde{S}}_t& -A_{I1}^1{S}_{1t}^1\mathrm{...}& -A_{I7}^1{S}_{Nt}^1& -A_{I1}^2{S}_{1t}^2\mathrm{...}& -A_{IN}^2{S}_{Nt}^2\mathrm{...}& -A_{I1}^M{S}_{1t}^M\mathrm{...}& -A_{IN}^M{S}_{7N}^M\\ & I_t& A_{b1}^1{I}_{1\_o}^1\mathrm{...}& A_{bN}^1{I}_{N\_o}^1& A_{b1}^2{I}_{1\_o}^2\mathrm{...}& A_{bN}^2{I}_{N\_o}^2\mathrm{...}& A_{b1}^M{I}_{1\_o}^M\mathrm{...}& A_{bN}^M{I}_{N\_o}^M\\ & & I_{1t}^1\mathrm{...}& A_{1N}^1{I}_{N\_1}^1& 0& 0& 0& 0\\ \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}\\ & & & I_{Nt}^1& 0& 0& 0& 0\\ & & & & I_{1t}^2\mathrm{...}& A_{1N}^2{I}_{N\_1}^2& 0& 0\\ \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}\\ & & & & & I_{Nt}^2& 0& 0\\ & & & & & & I_{1t}^n\mathrm{...}& A_{1N}^M{I}_{N\_1}^M\\ \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}\\ & & & & & & & I_{Nt}^M\end{array}\right]---\left(7\right)$

质量阵为对称正定阵，因此只给出了对角线及以上的元素；

为系统的广义速度列阵，表达式如式(8)：

$\underset{~}{Y}={\left[\begin{array}{cccccccccccc}{\stackrel{\·}{R}}^T& {\mathrm{\ω}}_b^T& {\mathrm{\ω}}_1^{1T}& \mathrm{...}& {\mathrm{\ω}}_N^{1T}& {\mathrm{\ω}}_1^{2T}& \mathrm{...}& {\mathrm{\ω}}_N^{2T}& \mathrm{...}& {\mathrm{\ω}}_1^{MT}& \mathrm{...}& {\mathrm{\ω}}_N^{MT}\end{array}\right]}^T---\left(8\right);$

为广义力列阵，表达式如式(9)：

$\underset{~}{Q}={\left[\begin{array}{cccccccccccc}{F}^T& T^T& T_1^{1T}& \mathrm{...}& T_N^{1T}& T_1^{2T}& \mathrm{...}& T_N^{2T}& \mathrm{...}& T_1^{MT}& \mathrm{...}& T_N^{MT}\end{array}\right]}^T---\left(9\right);$

为非线性项列阵，为高阶小量，可忽略。

本发明还公开了一种在轨服务机械臂动力学建模系统，该系统包含：坐标系定义模块、连接坐标系定义模块的广义惯性力和广义主动力计算模块、连接广义惯性力和广义主动力计算模块的动力学方程建立模块、连接动力学方程建立模块的动力学模型建立模块。

坐标系定义模块用于定义机械臂系统的系统坐标系，以及矢量参数和速度参数。

广义惯性力和广义主动力计算模块接收系统坐标系、矢量参数和速度参数，用于确定系统的广义偏速度，并得出该广义偏速度对应的广义惯性力和广义主动力；

动力学方程建立模块接收广义惯性力和广义主动力计算模块带入凯恩方程，得到机械臂系统中所有节臂的动力学方程。

动力学模型建立模块接收机械臂系统中所有节臂的动力学方程并进行简化，合并成整个机械臂系统的动力学模型。

尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。

Claims (10)

1.一种在轨服务机械臂动力学建模方法，其特征在于，该方法包含：

S1、定义机械臂系统的系统坐标系，以及矢量参数和速度参数；

S2、确定系统的广义偏速度，并得出该广义偏速度对应的广义惯性力和广义主动力；

S3、广义主动力和广义惯性力带入凯恩方程，得到机械臂系统中所有节臂的动力学方程；

S4、机械臂系统中所有节臂的动力学方程进行简化，合并成整个机械臂系统的动力学模型。

2.如权利要求1所述的在轨服务机械臂动力学建模方法，其特征在于，所述定义机械臂系统的系统坐标系包含：

惯性坐标系S_I：原点O_I位于地球中心，Z_I轴沿地球指向北极，X_I轴指向春分点，Y_I轴满足右手定则；

轨道坐标系S_o：原点O_o位于系统质心，z_o轴指向地心，y_o轴指向轨道负法线方向，x_o轴与其它两轴构成右手坐标系；

中心体连体坐标系S_b：原点O_b位于复杂连接体中心体的质心，x_b、y_b和z_b与中心体惯性主轴平行，且满足右手定则；当中心体相对于轨道坐标系的三个方位角为零时，S_b坐标系与S_o系一致；

机械臂坐标系S_ji(i＝1，2，…M；j＝1，2，…N_i)：设系统有i个机械臂，对应第i个机械臂有N_i节，定义第i个机械臂中第j节臂的机械臂坐标系为：原点位于第i个机械臂系统中的第j节机械臂与其内接体的铰链o_j ⁱ处，坐标系各轴与第j节机械臂固连。

3.如权利要求2所述的在轨服务机械臂动力学建模方法，其特征在于，所述定义矢量参数包含：中心体连体坐标系原点在惯性坐标系中的位置矢量中心体质心在中心体连体坐标系内的位置矢量中心体质心在惯性坐标系内的位置矢量第i组机械臂中第一节机械臂在中心体上的铰接点在中心体连体坐标系中的位置矢量第i组机械臂中第j(j＝2,3,…,Ni)节机械臂与第j-1节机械臂的连接点在第j-1节机械臂的连体坐标系中的位置矢量第i组机械臂中第j节机械臂的质心在其自身连体坐标系中的位置矢量第i组机械臂中第j节机械臂的质心在惯性坐标系中的位置矢量

4.如权利要求2所述的在轨服务机械臂动力学建模方法，其特征在于，所述定义速度参数包含：中心体连体系相对惯性系的角速度矢量第i组机械臂中第j节机械臂相对其自身连体坐标系的角速度矢量第i组机械臂中第j节机械臂相对惯性坐标系的角速度矢量

5.如权利要求2所述的在轨服务机械臂动力学建模方法，其特征在于，所述S2包含：

分析机械臂系统，选定广义偏速度；

根据广义偏速度，计算出该广义偏速度下的广义惯性力和广义主动力；广义惯性力包含：中心体惯性力和第i组机械臂第j节臂的惯性力。

6.如权利要求5所述的在轨服务机械臂动力学建模方法，其特征在于，所述中心体惯性力包含：

中心体第k(k>2)阶广义惯性力如式(1)：

$f_{Ik,B}^{*}=-{\∫}_B({\stackrel{\→}{a}}_B\·{\stackrel{\→}{G}}_{k,B})dm=0,k>2---\left(1\right)$

其中，为B体的绝对加速度，为第k阶广义偏速度；

所述第i组机械臂第j节臂的惯性力包含：

第i组机械臂第j节臂的第2+N*(i-1)+l阶广义惯性力如式(2)：

$\begin{array}{c}{f}_{In,j}^{*i}=-{\∫}_{i,j}({\stackrel{\→}{a}}_j^i\·{\stackrel{\→}{G}}_{n,j}^{i}dm)\\ =-{\∫}_{i,j}({\tilde{r}}_{l,j}^{iT}+A_{lj}^{i_1}{\tilde{r}}_j^{iT}{A}_{jl}^i)e_l^{iT}\·\left[\begin{array}{c}{e}_I^T\stackrel{\·\·}{R}-e_b^T({\tilde{r}}_{b,j}^i+A_{bj}^i{\tilde{r}}_j^i{A}_{jb}^i){\stackrel{\·}{\mathrm{\ω}}}_b-\underset{k=1}{\overset{j}{\Σ}}{e_k^i}^T({\tilde{r}}_{k,j}^i+A_{kj}^i{\tilde{r}}_j^i{A}_{jk}^i){\stackrel{\·}{\mathrm{\ω}}}_k^i\\ -e_b^T{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_{b,1}^i{\mathrm{\ω}}_b-\underset{k=1}{\overset{j-1}{\Σ}}{e_k^i}^T{\widetilde{\mathrm{\Ω}}}_k^i{\tilde{r}}_{k,k+1}^i{\mathrm{\Ω}}_k^i-{e_j^i}^T{\widetilde{\mathrm{\Ω}}}_j^i{r}_j^i{\mathrm{\Ω}}_j^i\\ -e_1^{iT}({\tilde{r}}_{1,j}^i+A_{1j}^i{\tilde{r}}_j^i{A}_{j1}^i)A_{1b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i\\ -\underset{k=2}{\overset{j}{\Σ}}{e_k^i}^T({\tilde{r}}_{k,j}^i+A_{kj}^i{\tilde{r}}_j^i{A}_{jk}^i)A_{k,k-1}^i{\widetilde{\mathrm{\Ω}}}_{k-1}^i{A}_{k-1,k}^i{\mathrm{\Ω}}_k^i\end{array}\right]dm\\ (n=2+N*(i-1)+l,l\≤j)\end{array}---\left(2\right)$

其中，为第j节臂绝对加速度，为j节机械臂上第n阶广义偏速度，上式中所有e都为连体基，其下标表示为在哪个坐标系下，上标表示为第几组，T表示转置；A表示方向余弦矩阵，下标表示为后者对前者的方向余弦矩阵，上标表示在哪个坐标系下；表示位置的叉乘矩阵，下标为后一点相对前一点，上标为在哪个坐标系下；ω表示角速度，下标表示是那个体；Ω为关节旋转角速度，下标表示哪个体，上标表示相对那个坐标系。

7.如权利要求6所述的在轨服务机械臂动力学建模方法，其特征在于，所述S3包含：

把广义主动力和广义惯性力带入凯恩方程，得到N*M+2个力学平衡方程，包含：中心体平动方程、中心体转动方程和第i组机械臂第j节臂的转动方程(i＝1，2，…M；j＝1，2，…N)：

中心体平动方程包含：

中心体平动方程对应于第一阶广义速度如式(3)：

$\begin{array}{c}m\stackrel{\·\·}{R}-A_{Ib}{\tilde{S}}_{ot}{\stackrel{\·}{\mathrm{\ω}}}_b-\underset{i=1}{\overset{3}{\Σ}}\underset{j=1}{\overset{7}{\Σ}}{A}_{Ij}^i{\tilde{S}}_{jt}^i{\stackrel{\·}{\mathrm{\ω}}}_j^i-A_{Ib}{\widetilde{\mathrm{\ω}}}_b{\tilde{S}}_b^{*}{\mathrm{\ω}}_b-\underset{i=1}{\overset{3}{\Σ}}\underset{j=1}{\overset{7}{\Σ}}{A}_{Ij}^i{\widetilde{\mathrm{\Ω}}}_j^i{\tilde{S}}_j^{i*}{\mathrm{\Ω}}_j^i\\ -\underset{i=1}{\overset{3}{\Σ}}(A_{I1}^i{\tilde{S}}_{1t}^i{A}_{1b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i+\underset{j=2}{\overset{7}{\Σ}}{A}_{Ij}^i{\tilde{S}}_{jt}^i{A}_{j,j-1}^i{\widetilde{\mathrm{\Ω}}}_{j-1}^i{\mathrm{\Ω}}_j^i)=F\end{array}---\left(3\right);$

中心体转动方程包含：

中心体转动方程对应于第二阶广义速度ω_b，如式(4)：

$\begin{array}{c}-{\tilde{S}}_{ot}^T{A}_{bI}\stackrel{\·\·}{R}+I_{ot}{\stackrel{\·}{\mathrm{\ω}}}_b+\underset{i=1}{\overset{3}{\Σ}}\underset{j=1}{\overset{7}{\Σ}}{A}_{bj}^i{I}_{j\_o}^i{\stackrel{\·}{\mathrm{\ω}}}_j^i+I_{\mathrm{\ω}}{\mathrm{\ω}}_b+\underset{i=1}{\overset{3}{\Σ}}\underset{j=1}{\overset{7}{\Σ}}{A}_{bj}^i{I}_{\mathrm{\Ω}I\_o}^i{\mathrm{\Ω}}_j^i\\ +\underset{i=1}{\overset{3}{\Σ}}(A_{b1}^i{I}_{1\_o}^i{A}_{1b}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i+\underset{j=2}{\overset{7}{\Σ}}{A}_{bj}^i{I}_{j\_o}^i{A}_{j,j-1}^i{\widetilde{\mathrm{\Ω}}}_{j-1}^i{A}_{j-1,j}^i{\mathrm{\Ω}}_j^i)=T\end{array}---\left(4\right);$

第i组机械臂第N节臂的转动方程包含：

第i组机械臂第N节臂的转动方程对应于第(2+N*(i-1)+N)阶广义速度如式(5)

$\begin{array}{c}-{{\tilde{S}}_{Nt}^i}^T{A}_{NI}^i\stackrel{\·\·}{R}+{I_{N\_o}^i}^T{A}_{Nb}^i{\stackrel{\·}{\mathrm{\ω}}}_b+\underset{j=1}{\overset{N-1}{\Σ}}{I_{N\_j}^i}^T{A}_{Nj}^i{\stackrel{\·}{\mathrm{\ω}}}_j^i+I_{Nt}^i{\stackrel{\·}{\mathrm{\ω}}}_N^i+{{\tilde{S}}_{Nt}^i}^T{A}_{Nb}^i{\widetilde{\mathrm{\ω}}}_b{\tilde{r}}_{b,1}^i{\mathrm{\ω}}_b\\ +\underset{j=1}{\overset{N-1}{\Σ}}{A}_{Nj}^i{I}_{{\mathrm{\Ω}}_{j\_}N}^i{\mathrm{\Ω}}_j^i+I_{{\mathrm{\Ω}}_N}^i{\mathrm{\Ω}}_N^i+{I_{N\_1}^i}^T{A}_{Nb}^i{\widetilde{\mathrm{\ω}}}_b{A}_{b1}^i{\mathrm{\Ω}}_1^i\\ +\underset{j=2}{\overset{N-1}{\Σ}}{I_{N\_j}^i}^T{A}_{N,j-1}^i{\mathrm{\Ω}}_{j-1}^i{A}_{j-1,j}^i{\mathrm{\Ω}}_j^i+I_{Nt}^i{A}_{N(N-1)}^i{\widetilde{\mathrm{\Ω}}}_{(N-1)}^i{A}_{(N-1)N}^i{\mathrm{\Ω}}_N^i=T_N^i\end{array}---\left(5\right).$

8.如权利要求7所述的在轨服务机械臂动力学建模方法，其特征在于，所述S4中，经整理后的机械臂系统的动力学方程如式(6)：

$\underset{~}{M}\underset{~}{\stackrel{\·}{Y}}=\underset{~}{Q}-{\underset{~}{Q}}_N---\left(6\right)$

式中为系统的质量阵，表达式如式(7)：

$\underset{~}{M}=\left[\begin{array}{cccccccc}{\mathrm{mE}}_3& -A_{Ib}{\tilde{S}}_t& -A_{I1}^1{S}_{1t}^1\mathrm{...}& -A_{I7}^1{S}_{Nt}^1& -A_{I1}^2{S}_{Nt}^2\mathrm{...}& -A_{IN}^2{S}_{Nt}^2\mathrm{...}& -A_{I1}^M{S}_{1t}^M\mathrm{...}& -A_{IN}^M{S}_{7N}^M\\ & I_t& A_{b1}^1{I}_{1\_o}^1\mathrm{...}& A_{bN}^1{I}_{N\_o}^1& A_{b1}^2{I}_{1\_o}^2\mathrm{...}& A_{bN}^2{I}_{N\_o}^2\mathrm{...}& A_{b1}^M{I}_{1\_o}^M\mathrm{...}& A_{bN}^M{I}_{N\_o}^M\\ & & I_{1t}^1\mathrm{...}& A_{1N}^1{I}_{N\_1}^1& 0& 0& 0& 0\\ \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}\\ & & & I_{Nt}^1& 0& 0& 0& 0\\ & & & & I_{1t}^2\mathrm{...}& A_{1N}^2{I}_{N\_1}^2& 0& 0\\ \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}\\ & & & & & I_{Nt}^2& 0& 0\\ & & & & & & I_{1t}^n\mathrm{...}& A_{1N}^M{I}_{N\_1}^M\\ \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}& \mathrm{...}\\ & & & & & & & I_{Nt}^M\end{array}\right]---\left(7\right)$

质量阵为对称正定阵，因此只给出了对角线及以上的元素；

为系统的广义速度列阵，表达式如式(8)：

$\underset{~}{Y}={\left[\begin{array}{cccccccccccc}{\stackrel{\·}{R}}^T& {\mathrm{\ω}}_b^T& {\mathrm{\ω}}_1^{1T}& \mathrm{...}& {\mathrm{\ω}}_N^{1T}& {\mathrm{\ω}}_1^{2T}& \mathrm{...}& {\mathrm{\ω}}_N^{2T}& \mathrm{...}& {\mathrm{\ω}}_1^{MT}& \mathrm{...}& {\mathrm{\ω}}_N^{MT}\end{array}\right]}^T---\left(8\right)$

为广义力列阵，表达式如式(9)：

$\underset{~}{Q}={\left[\begin{array}{cccccccccccc}{F}^T& T^T& T_1^{1T}& \mathrm{...}& T_N^{1T}& T_1^{2T}& \mathrm{...}& T_N^{2T}& \mathrm{...}& T_1^{MT}& \mathrm{...}& T_N^{MT}\end{array}\right]}^T---\left(9\right).$

9.如权利要求1所述的在轨服务机械臂动力学建模方法，其特征在于，所述建模方法用于航天器本体及搭载在航天器本体上的N个多自由度机械臂的联合机械臂系统的动力学建模。

10.一种在轨服务机械臂动力学建模系统，其特征在于，该系统包含：

坐标系定义模块，其定义机械臂系统的系统坐标系，以及矢量参数和速度参数；

广义惯性力和广义主动力计算模块，其接收系统坐标系、矢量参数和速度参数，确定系统的广义偏速度，并得出该广义偏速度对应的广义惯性力和广义主动力；

动力学方程建立模块，其接收广义惯性力和广义主动力计算模块带入凯恩方程，得到机械臂系统中所有节臂的动力学方程；

动力学模型建立模块，其接收机械臂系统中所有节臂的动力学方程并进行简化，合并成整个机械臂系统的动力学模型。