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

摘要

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

主权项

一种空间绳系机器人的通用动力学模型的建立方法，其特征在于，包括如下步骤：步骤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\le s\le s_P\left(t\right)\\ s_P\left(t\right)\le s\le s_M\left(t\right)\\ s_M\left(t\right)\le s\le L\end{array}\right.---\left(1\right)$长度为s_P的空间系绳存放在平台中，长度为L‑s_M的空间系绳存放在操作机器人中，剩余的长度为s_M‑s_P的空间系绳释放在平台和操作机器人之间；空间系绳的轴向张力表示为：$\left\{\begin{array}{c}N=EA(ϵ+\alpha \stackrel{·}{ϵ})\\ n=N\tau \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}{\int }}\left\{\underset{i=1}{\overset{n_P}{\Sigma }}\right\{{\mathrm{\delta R}}_P^{iT}[-m_P^i{\stackrel{··}{R}}_P^i-\underset{k=1}{\overset{n_P^C}{\Sigma }}{\left(\frac{\partial C_P^k}{\partial R_P^i}\right)}^T{\lambda }_P^k+{\left(\frac{\partial C_{CP}}{\partial R_P^i}\right)}^T{\lambda }_{CP}+F_P^i+F_{Pg}^i]\\ +{\mathrm{\delta \Lambda }}_P^{iT}[-4L_P^{iT}{J}_P^i{L}_P^i{\stackrel{··}{\Lambda }}_P^i-8{\stackrel{·}{L}}_P^{iT}{J}_P^i{L}_P^i{\stackrel{·}{\Lambda }}_P^i-\underset{k=1}{\overset{n_P^C}{\Sigma }}{\left(\frac{\partial C_P^k}{\partial {\Lambda }_P^i}\right)}^T{\lambda }_P^k-2{\Lambda }_P^i{\lambda }_{P\Lambda }^i\\ +{\left(\frac{\partial C_{CP}}{\partial {\Lambda }_P^i}\right)}^T{\lambda }_{CP}+Q_P^i+Q_{Pg}^i\left]\right\}\}dt\end{array}$$\begin{array}{c}{G}_M=\underset{t_0}{\overset{t_1}{\int }}\left\{\underset{i=1}{\overset{n_M}{\Sigma }}\right\{{\mathrm{\delta R}}_M^{iT}[-m_M^i{\stackrel{··}{R}}_M^i-\underset{k=1}{\overset{n_M^C}{\Sigma }}{\left(\frac{\partial C_M^k}{\partial R_M^i}\right)}^T{\lambda }_M^k+{\left(\frac{\partial C_{CM}}{\partial R_M^i}\right)}^T{\lambda }_{CM}+F_M^i+F_{Mg}^i]\\ +{\mathrm{\delta \Lambda }}_M^{iT}[-4L_M^{iT}{J}_M^i{L}_M^i{\stackrel{··}{\Lambda }}_M^i-8{\stackrel{·}{L}}_M^{iT}{J}_M^i{L}_M^i{\stackrel{·}{\Lambda }}_M^i-\underset{k=1}{\overset{n_M^C}{\Sigma }}{\left(\frac{\partial C_M^k}{\partial {\Lambda }_M^i}\right)}^T{\lambda }_M^k-2{\Lambda }_M^i{\lambda }_{M\Lambda }^i\\ +{\left(\frac{\partial C_{CM}}{\partial {\Lambda }_M^i}\right)}^T{\lambda }_{CM}+Q_M^i+Q_{Mg}^i\left]\right\}\}dt\end{array}$$\begin{array}{c}{G}_T=\underset{t_0}{\overset{t_1}{\int }}\left\{{\mathrm{\delta R}}_{CP}^T\right\{-{\mathrm{\rho s}}_P[{\stackrel{··}{R}}_{CP}+▿\Phi \left(R_{CP}\right)]-\rho {\stackrel{·}{s}}_P^2\eta \left(s_P\right)\tau \left(s_P\right)-{\lambda }_{CP}-{\lambda }_{PT}+F_{CP}\}\\ +\underset{s_P}{\overset{s_M}{\int }}{\mathrm{\delta R}}^T[-\rho (\stackrel{··}{R}+▿\Phi )+F_T+\frac{\partial n}{\partial s}]ds+{\mathrm{\delta R}}_{CM}^T\{-\rho (L-s_M)[{\stackrel{··}{R}}_{CM}+▿\Phi \left(R_{CM}\right)]\\ +\rho {\stackrel{·}{s}}_M^2\eta \left(s_M\right)\tau \left(s_M\right)-{\lambda }_{CM}-{\lambda }_{MT}+F_{CM}\}+{\mathrm{\delta R}}^T\left(s_P\right)[{\lambda }_{PT}+n\left(s_P\right)]\\ +{\mathrm{\delta R}}^T\left(s_M\right)[{\lambda }_{MT}-n\left(s_M\right)]\}dt\end{array}$$G_{DR}=\underset{t_0}{\overset{t_1}{\int }}\left\{\right[N_{PD}+N_P-N\left(s_P\right)]\eta \left(s_P\right){\mathrm{\delta s}}_P+[-N_{MD}-N_M+N\left(s_M\right)\left]\eta \left(s_M\right){\mathrm{\delta s}}_M\right\}dt$$\begin{array}{c}{G}_C=\underset{t_0}{\overset{t_1}{\int }}\{-{\mathrm{\delta \lambda }}_P^{kT}{C}_p^k-{\mathrm{\delta \lambda }}_M^{kT}{C}_M^k-{\mathrm{\delta \lambda }}_{CP}^T[R_{CP}-C_{CP}(R_p^i,{\Lambda }_p^i)]\\ -{\mathrm{\delta \lambda }}_{CM}^T[R_{CM}-C_{CM}(R_M^i,{\Lambda }_M^i)]+{\mathrm{\delta \lambda }}_{PT}^T[R_{CP}-R\left(s_P\right)]\\ +{\mathrm{\delta \lambda }}_{MT}^T[R_{CM}-R\left(s_M\right)]+\underset{i=1}{\overset{n_P}{\Sigma }}{\mathrm{\delta \lambda }}_{P\Lambda }^i[{\Lambda }_p^{iT}{\Lambda }_p^i-1]+\underset{i=1}{\overset{n_M}{\Sigma }}{\mathrm{\delta \lambda }}_{M\Lambda }^i[{\Lambda }_M^{iT}{\Lambda }_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}{\int }}\left\{\underset{i=1}{\overset{n_P}{\Sigma }}\right\{{\mathrm{\delta r}}_P^{iT}[-m_P^i{\stackrel{··}{r}}_P^i-m_P^i{\bar{\omega }}^2{A}_f{r}_P^i-\underset{k=1}{\overset{n_P^C}{\Sigma }}{\left(\frac{\partial C_P^k}{\partial r_P^i}\right)}^T{\lambda }_P^k+{\left(\frac{\partial C_{CP}}{\partial r_P^i}\right)}^T{\lambda }_{CP}+F_P^i]\\ +{\mathrm{\delta \Lambda }}_P^{iT}[-4L_P^{iT}{J}_P^i{L}_P^i{\stackrel{··}{\Lambda }}_P^i-8{\stackrel{·}{L}}_P^{iT}{J}_P^i{L}_P^i{\stackrel{·}{\Lambda }}_P^i+6{\bar{\omega }}^2{L}_P^{iT}[i_P^i\times (I_P^i·i_P^i)]-\underset{k=1}{\overset{n_P^C}{\Sigma }}{\left(\frac{\partial C_P^k}{\partial {\Lambda }_P^i}\right)}^T{\lambda }_P^k\\ -2{\Lambda }_P^i{\lambda }_{P\Lambda }^i+{\left(\frac{\partial C_{CP}}{\partial {\Lambda }_P^i}\right)}^T{\lambda }_{CP}+Q_P^i\left]\right\}\}dt\end{array}$$\begin{array}{c}{G}_M^o=\underset{t_0}{\overset{t_1}{\int }}\left\{\underset{i=1}{\overset{n_M}{\Sigma }}\right\{{\mathrm{\delta R}}_M^{iT}[-m_M^i{\stackrel{··}{r}}_M^i-m_M^i{\bar{\omega }}^2{A}_f{r}_M^i-\underset{k=1}{\overset{n_M^C}{\Sigma }}{\left(\frac{\partial C_M^k}{\partial R_M^i}\right)}^T{\lambda }_M^k+{\left(\frac{\partial C_{CM}}{\partial R_M^i}\right)}^T{\lambda }_{CM}+F_M^i]\\ +{\mathrm{\delta \Lambda }}_M^{iT}[-4L_M^{iT}{J}_M^i{L}_M^i{\stackrel{··}{\Lambda }}_M^i-8{\stackrel{·}{L}}_M^{iT}{J}_M^i{L}_M^i{\stackrel{·}{\Lambda }}_M^i+6{\bar{\omega }}^2{L}_M^{iT}[i_M^i\times (I_M^i·i_M^i)]-\underset{k=1}{\overset{n_M^C}{\Sigma }}{\left(\frac{\partial C_M^k}{\partial {\Lambda }_M^i}\right)}^T{\lambda }_M^k\\ -2{\Lambda }_M^i{\lambda }_{M\Lambda }^i+{\left(\frac{\partial C_{CM}}{\partial {\Lambda }_M^i}\right)}^T{\lambda }_{CM}+Q_M^i\left]\right\}\}dt\end{array}$$\begin{array}{c}{G}_T^o=\underset{t_0}{\overset{t_1}{\int }}\left\{{\mathrm{\delta r}}_{CP}^T\right[-{\mathrm{\rho s}}_P({\stackrel{··}{r}}_{CP}+{\bar{\omega }}^2{A}_f{r}_{CP})-\rho {\stackrel{·}{s}}_P^2\eta \left(s_P\right)\tau \left(s_P\right)-{\lambda }_{CP}-{\lambda }_{PT}+F_{CP}]\\ +\underset{s_P}{\overset{s_M}{\int }}{\mathrm{\delta r}}^T[-\rho (\stackrel{··}{r}+{\bar{\omega }}^2{A}_{f}r)+F_T+\frac{\partial n}{\partial s}]ds+{\mathrm{\delta r}}_{CM}^T[-\rho (L-s_M)({\stackrel{··}{r}}_{CM}+{\bar{\omega }}^2{A}_f{r}_{CM})\\ +\rho {\stackrel{·}{s}}_M^2\eta \left(s_M\right)\tau \left(s_M\right)-{\lambda }_{CM}-{\lambda }_{MT}+F_{CM}\}+{\mathrm{\delta r}}^T\left(s_P\right)[{\lambda }_{PT}+n\left(s_P\right)]\\ +{\mathrm{\delta r}}^T\left(s_M\right)[{\lambda }_{MT}-n\left(s_M\right)]\}dt\end{array}$$G_{DR}^o=\underset{t_0}{\overset{t_1}{\int }}\left\{\right[N_{PD}+N_P-N\left(s_P\right)]\eta \left(s_P\right){\mathrm{\delta s}}_P+[-N_{MD}-N_M+N\left(s_M\right)\left]\eta \left(s_M\right){\mathrm{\delta s}}_M\right\}dt$$\begin{array}{c}{G}_C^o=\underset{t_0}{\overset{t_1}{\int }}\{-{\mathrm{\delta \lambda }}_P^{kT}{C}_p^k(r_p^i,{\Lambda }_p^i)-{\mathrm{\delta \lambda }}_M^{kT}{C}_M^k(r_M^i,{\Lambda }_M^i)-{\mathrm{\delta \lambda }}_{CP}^T[r_{CP}-C_{CP}(r_p^i,{\Lambda }_p^i)]\\ -{\mathrm{\delta \lambda }}_{CM}^T[r_{CM}-C_{CM}(r_M^i,{\Lambda }_M^i)]+{\mathrm{\delta \lambda }}_{PT}^T[r_{CP}-r\left(s_P\right)]+{\mathrm{\delta \lambda }}_{MT}^T[r_{CM}-r\left(s_M\right)]\\ +\underset{i=1}{\overset{n_P}{\Sigma }}{\mathrm{\delta \lambda }}_{P\Lambda }^i[{\Lambda }_p^{iT}{\Lambda }_p^i-1]+\underset{i=1}{\overset{n_M}{\Sigma }}{\mathrm{\delta \lambda }}_{M\Lambda }^i[{\Lambda }_M^{iT}{\Lambda }_M^i-1]\}dt\end{array};$其中，设分别是空间平台和操作机器人各刚体在轨道惯性系下的坐标；表示轨道平均角速度，表示地心惯性系矢径的单位方向向量在本体坐标系内的坐标；f为真近点角；步骤5中，在离散化之前，引入归一化自然坐标$\bar{s}=\frac{s-s_P}{s_M-s_P}\in [0,1]$将采用描述的位置矢量记为利用n+1个节点将积分区间[0,1]分为n段，每段上选取插值函数为：于是第i段空间系绳上点的位置向量近似满足：$\bar{r}(\bar{s},t)\approx \left[\begin{array}{cc}1-\bar{s}& \bar{s}\end{array}\right]\left[\begin{array}{c}{\bar{r}}_{i-1}^n\left(t\right)\\ r_i^n\left(t\right)\end{array}\right]{\bar{r}}^{\prime }(\bar{s},t)\approx {\bar{r}}_i\left(t\right)-{\bar{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{\bar{\omega }}^2{A}_f^i{r}_P^i+\underset{k=1}{\overset{n_P^C}{\Sigma }}{\left(\frac{\partial C_P^k}{\partial r_P^i}\right)}^T{\lambda }_P^k-{\left(\frac{\partial C_{CP}}{\partial r_P^i}\right)}^T{\lambda }_{CP}=F_P^i\\ 4L_P^{iT}{J}_P^i{L}_P^i{\stackrel{··}{\Lambda }}_P^i+8{\stackrel{·}{L}}_P^{iT}{J}_P^i{L}_P^i{\stackrel{·}{\Lambda }}_P^i-6{\bar{\omega }}^2{L}_P^{iT}[i_P^i\times (I_P^i·i_P^i)]\\ +\underset{k=1}{\overset{n_P^C}{\Sigma }}{\left(\frac{\partial C_P^k}{\partial {\Lambda }_P^i}\right)}^T{\lambda }_P^k+2{\Lambda }_P^i{\lambda }_{P\Lambda }^i-{\left(\frac{\partial C_{CP}}{\partial {\Lambda }_P^i}\right)}^T{\lambda }_{CP}=Q_P^i\\ \begin{array}{cc}{C}_P^k(r_P^i,n_P^i)=0& {\Lambda }_p^{iT}{\Lambda }_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{\bar{\omega }}^2{A}_f{r}_M^i+\underset{k=1}{\overset{n_M^C}{\Sigma }}{\left(\frac{\partial C_M^k}{\partial R_M^i}\right)}^T{\lambda }_M^k-{\left(\frac{\partial C_{CM}}{\partial R_M^i}\right)}^T{\lambda }_{CM}=F_M^i\\ 4L_M^{iT}{J}_M^i{L}_M^i{\stackrel{··}{\Lambda }}_M^i+8{\stackrel{·}{L}}_M^{iT}{J}_M^i{L}_M^i{\stackrel{·}{\Lambda }}_M^i-6{\bar{\omega }}^2{L}_M^{iT}[i_M^i\times (I_M^i·i_M^i)]\\ +\underset{k=1}{\overset{n_M^C}{\Sigma }}{\left(\frac{\partial C_M^k}{\partial {\Lambda }_M^i}\right)}^T{\lambda }_M^k+2{\Lambda }_M^i{\lambda }_{M\Lambda }^i-{\left(\frac{\partial C_{CM}}{\partial {\Lambda }_M^i}\right)}^T{\lambda }_{CM}=Q_M^i\\ \begin{array}{cc}{C}_M^k(r_M^i,{\Lambda }_M^i)=0& {\Lambda }_M^{iT}{\Lambda }_M^i-1=0\end{array}\end{array}\right.---\left(13\right)$(3)存放空间系绳的动力学方程及约束方程$\left\{\begin{array}{c}{\mathrm{\rho s}}_P({\stackrel{··}{r}}_{CP}+{\bar{\omega }}^2{A}_f{r}_{CP})+\rho {\stackrel{·}{s}}_P^2\eta \left(s_P\right)\tau \left(s_P\right)+{\lambda }_{CP}+{\lambda }_{PT}=F_{CP}\\ \rho (L-s_M)({\stackrel{··}{r}}_M+{\bar{\omega }}^2{A}_f{r}_{CM})-\rho {\stackrel{·}{s}}_M^2\eta \left(s_M\right)\tau \left(s_M\right)+{\lambda }_{CM}+{\lambda }_{MT}=F_{CM}\\ \begin{array}{cc}{r}_{CP}-C_{CP}(r_p^i,{\Lambda }_p^i)=0& r_{CM}-C_{CM}(r_M^i,{\Lambda }_M^i)=0\end{array}\end{array}\right.---\left(14\right)$(4)释放空间系绳的动力学方程及约束方程$\left\{\begin{array}{c}\rho l[M_1{\stackrel{··}{\bar{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{\bar{r}}^n+{\bar{\omega }}^2{M}_1{\hat{A}}_f{\bar{r}}^n]-B\left[\begin{array}{c}{\lambda }_{PT}\\ {\lambda }_{MT}\end{array}\right]=F_T^n+M_3{\bar{n}}^n\\ {\bar{n}}_i^n=\frac{EA}{l}\left[\right|{\bar{r}}_i^{\prime }|-l+\alpha (\frac{{\bar{r}}_i^{\prime }·{\stackrel{·}{\stackrel{‾}{r}}}_i^{\prime }}{\left|{\bar{r}}_i^{\prime }\right|}-\frac{\stackrel{·}{l}}{l}|{\bar{r}}_i^{\prime }\left|\right)]\frac{{\bar{r}}_i^{\prime }}{\left|{\bar{r}}_i^{\prime }\right|}\\ \begin{array}{cc}{r}_{CP}-{\bar{r}}_0^n=0& r_{CM}-{\bar{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\le j\le 3\\ \frac{2}{3n}& i=j,4\le j\le n-3\\ \frac{1}{3n}& i=j,n-2\le j\le n\\ \frac{1}{6n}& i=j-3,4\le j\le n\\ \frac{1}{6n}& i=j+3,1\le j\le n-3\\ 0& others\end{array}\right.,M_2(i,j)=\left\{\begin{array}{cc}\frac{1}{6n}-\frac{1}{2}& i=j,1\le i\le 3\\ -\frac{1}{6n}& i=j,4\le i\le n-3\\ \frac{1}{6n}& i=j,n-2\le i\le n\\ \frac{1}{2}-(\frac{k}{2}-\frac{1}{3})\frac{1}{n}& i+3=j,3k-2\le i\le 3k,1\le k\le n\\ \frac{1}{2}-(\frac{i}{2}-\frac{1}{3})\frac{1}{n}& i=j+3,3k-2\le i\le 3k,1\le k\le n\\ 0& others\end{array}\right.,$