随着当代射电天文学的发展，国际社会对于射电天文望远镜的观测精度要求也逐年提高。中国500 m口径球面射电望远镜(five-hundred-meter aperture spherical radio telescope，FAST)^[1-4]提出使用6根绳索牵引重30 t的馈源舱追踪馈源，本质是大跨度索驱动并联机器人结构；同时，馈源舱内装有Stewart平台，由上、下平台和连接两平台的6根伸缩杆组成，通过调整6根伸缩杆的长度控制两平台的相对位姿，可以对追踪馈源的误差进行补偿。通过绳索牵引和Stewart平台的二级调节，FAST最终可以实现毫米级别的馈源动态追踪能力。

目前，这种索驱动并联机器人与Stewart平台结合的动力学研究较少，同时有各自的局限性：Qiu等^[5]对一种FAST模型进行了绳索的准静力学分析，但并未涉及动力学控制；Lu等^[6]通过简化Stewart平台，将绳索视为直线，通过绳索PID(proportion integral differential)算法完成了早期FAST实验模型的简易控制；针对相同的模型，Ren等^[7]则通过将Stewart平台的伸缩杆和绳索假设为弹簧阻尼结构，完成该模型PID控制的研究；后来，Sun等^[8]通过全站仪激光定位技术，通过Stewart平台PID控制降低了部分FAST馈源误差。随后，Duan等^[9]针对另一种FAST实验模型，通过引入Stewart平台动力学方程反推伸缩杆电机的电信号表达式，并以此为依据对整体进行PID控制，忽略绳索的具体情况并将其抽象入控制参数中。这些研究都通过PID来规避直接对绳索或者Stewart平台建立动力学模型，控制精度主要取决于PID参数的迭代，缺乏原理性的解释。汤奥斐等^[10]通过对绳索有限元建模，在忽略Stewart平台影响的情况下完成了动力学仿真；Shao等^[11]的研究进一步舍弃PID，通过分别推导Stewart平台动力学模型和将绳索视为直线弹簧阻尼模型，两者联立求解完成了FAST模型风振相关的动力学研究。虽然该研究没有PID，但大跨度绳索的大部分物理特性被等效入弹簧阻尼参数中，同时也未针对Stewart平台高频率补偿对系统的动力学影响进行讨论。

针对这些局限性，本文提出一种结合绳索悬链线模型^[12]和Stewart平台在非惯性系下逆解模型^[13-14]的索驱动并联机器人纯理论动力学模型，旨在尽量摆脱迭代参数依赖，从原理上说明Stewart平台补偿误差的可行性，同时探讨系统耦合振动的本质，为这类刚柔结合机构的控制算法提供重要理论基础。本文通过Newton-Euler方程推导Stewart平台的动力学逆解方程，再与精确绳索模型联立，对馈源舱整体的动力学方程数值求解。最后，基于此模型提出一套FAST索驱动并联机器人的动力学仿真算法，并通过数值仿真讨论该算法的合理性。

1 主要机构几何关系及坐标系建立

FAST主要由馈源支撑系统和主动反射面系统构成，馈源支撑系统通过6座高塔牵引6根绳索控制中央馈源舱的移动和姿态对馈源实现追踪，如图 1所示。其中，以反射面下顶点为原点^GO，原点朝T[1]塔的方向为^Gx轴方向，垂直地面向上为^Gz轴方向，^Gy轴垂直于^GO-^Gx^Gz平面，建立全局坐标G系；以馈源舱上的绳索锚固点A[i]平面中心为原点^CO，^Cy轴为由锚固点A[5]指向原点^CO方向，^Cz轴垂直于馈源舱锚固点A[i]平面向上，^Cx轴垂直于^CO-^Cy^Cz平面，建立与馈源舱绑定的局部坐标C系，如图 2所示。

馈源舱的3个锚固点A[i]均匀分布在半径r_A的圆上，圆心即馈源舱局部坐标C系原点^CO；而6座塔T[i]均匀分布在半径r_T的圆上，每座塔高H，每2座塔与1个锚固点通过绳索相连。

以锚固点A[i]为原点为每条绳索建立局部坐标A系，为方便，要求这些坐标系始终与全局坐标系G保持平行，如图 3所示。在各绳索局部坐标A系中，设绳索下端坐标在原点^AO[i]，而绳索上端坐标为(^AX[i], ^AY[i], ^AZ[i])，则可以得到以下几何关系：

$\left[\begin{array}{c} { }\ ^A X[i] \\ { }\ ^A Y[i] \\ { }\ ^A Z[i] \end{array}\right]=\left[\begin{array}{c} r_T \cos \left((i-1) \frac{{\rm{ \mathsf{ π}}}}{3}\right) \\ r_T \sin \left((i-1) \frac{{\rm{ \mathsf{ π}}}}{3}\right) \\ H \end{array}\right]-\left(\begin{array}{l} { }\ ^G \boldsymbol{R}_C{ }\ ^C \boldsymbol{r}_A[i] \\ +{ }^G \boldsymbol{t}_C \end{array}\right), $

(1)

${ }\ ^C \boldsymbol{r}_A[i]=\left[\begin{array}{c} r_A \cos \left(\frac{{\rm{ \mathsf{ π}}}}{6}+\lfloor(i-1) / 2\rfloor \frac{2 {\rm{ \mathsf{ π}}}}{3}\right) \\ r_A \sin \left(\frac{{\rm{ \mathsf{ π}}}}{6}+\lfloor(i-1) / 2\rfloor \frac{2 {\rm{ \mathsf{ π}}}}{3}\right) \\ 0 \end{array}\right].$

(2)

其中：[i]为对应第i座高塔的第i根绳索，^GR_C为馈源舱局部坐标C系相对全局坐标G系的旋转矩阵，^Cr_A[i]为第i根绳索相连的锚固点在C系下的坐标，^Gt_C为馈源舱局部坐标C系原点^CO在全局坐标G系下的位置向量。

馈源舱内的Stewart平台由上、下平台和中间6根伸缩杆构成，如图 4所示。其中，上平台与伸缩杆之间的锚固点B[i]分布在半径r_B的圆上，短边长度记为$ \overline {B[1]B[6]} $；下平台与伸缩杆之间的锚固点P[i]分布在半径r_P的圆上，短边长度记为$ \overline{P[1] P[2]}$。以上平台锚固点B[i]平面中心为原点^BO，^By轴垂直于上平台锚固点B[1]与B[2]连成的直线，^Bz轴垂直于上平台锚固点B[i]平面向上，^Bx轴垂直于^BO-^By^Bz平面，建立与上平台绑定的局部坐标B系；同理，通过下平台锚固点P[i]亦可以建立与下平台绑定的局部坐标P系。则可以通过几何关系确定，上下平台锚固点在各自坐标系中的位置，分别记为^BB[i]和^PP[i]。因此，每根伸缩杆下端相对上端在全局坐标G系下的位置向量可以表示为

${ }\ ^G \boldsymbol{S}[i]={ }\ ^G \boldsymbol{R}_B\left({ }^B \boldsymbol{R}_P{ }\ ^P \boldsymbol{P}[i]+{ }^B \boldsymbol{t}_P-{ }\ ^B \boldsymbol{B}[i]\right) .$

(3)

其中：[i]为第i根伸缩杆，^GR_B为上平台坐标B系相对全局坐标G系的旋转矩阵，^BR_P为下平台坐标P系相对上平台坐标B系的旋转矩阵，^Bt_P为P系原点^PO在B系下的位置向量。

同时，根据式(3)可以得到伸缩杆长和单位方向向量分别为：

$L[i]=\left|{ }\ ^G \boldsymbol{S}[i]\right|, $

(4)

${ }\ ^G \boldsymbol{s}[i]={ }\ ^G \boldsymbol{S}[i] / L[i] .$

(5)

2 绳索模型建立

考虑到FAST馈源支撑系统在Stewart平台的精密补偿运动下，馈源舱整体振动范围很小；同时，为兼顾实时控制的高速计算需要，决定在准静态的情况下，建立精确的绳索悬链线模型进行求解。

在各绳索的局部坐标A系下，设绳索下端三轴方向的受力分别为^GF_x[i]、^GF_y[i]和^GF_z[i]，如图 3所示。在绳索长度为p[i]处截断，K[i]为该点处的索力值，s[i]为这段绳索未受力作用时的长度，称为绳索原长。根据平衡方程可得

$K[i] \frac{\mathrm{d} x[i]}{\mathrm{d} p[i]}+{ }^G F_x[i]=0,$

(6)

$K[i] \frac{\mathrm{d} y[i]}{\mathrm{d} p[i]}+{ }^G F_y[i]=0,$

(7)

$K[i] \frac{\mathrm{d} z[i]}{\mathrm{d} p[i]}+{ }^G F_z[i]+\rho g s[i]=0, $

(8)

$K[i]=\sqrt{{ }\ ^G F_x[i]^2+{ }^G F_y[i]^2+\left({ }^G F_z[i]+\rho g s[i]\right)^2} .$

(9)

其中：ρ为绳索未受力作用时的线密度，g为重力加速度。又根据弹性力学有以下关系：

$K[i]=E A\left(\frac{\mathrm{d} p[i]}{\mathrm{d} s[i]}-1\right).$

(10)

其中EA为绳索弹性模量与截面积的乘积。通过将p[i]消元后，关于s[i]积分，结合式(1)代入绳索两端坐标的边界条件，可以求得

$\begin{gathered} { }\ ^A X[i]=-\frac{{ }\ ^G F_x[i]}{E A} s_0[i]- \\ \frac{{ }\ ^G F_x[i]}{\rho g}\left(\begin{array}{c} \operatorname{arcsh}\left(\frac{{ }\ ^G F_z[i]+\rho g s_0[i]}{\sqrt{{ }\ ^G F_x[i]^2+{ }^G F_y[i]^2}}\right) \\ -\operatorname{arcsh}\left(\frac{{ }\ ^G F_z[i]}{\sqrt{{ }\ ^G F_x[i]^2+{ }^G F_y[i]^2}}\right) \end{array}\right) . \end{gathered}$

(11)

$\left.\begin{array}{c} { }\ ^A Y[i]=-\frac{{ }\ ^G F_y[i]}{E A} s_0[i]- \\ \frac{{ }\ ^G F_y[i]}{\rho g}\left(\begin{array}{l} \operatorname{arcsh}\left(\frac{{ }\ ^G F_z[i]+\rho g s_0[i]}{\sqrt{{ }\ ^G F_x[i]^2+{ }^G F_y[i]^2}}\right) \\ -\operatorname{arcsh}\left(\frac{{ }\ ^G F_z[i]}{\sqrt{{ }\ ^G F_x[i]^2+{ }^G F_y[i]^2}}\right) \end{array}\right) \end{array}\right) .$

(12)

$\begin{gathered} { }\ ^A Z[i]=-\frac{{ }\ ^G F_z[i]}{E A} s_0[i]-\frac{\rho g}{2 E A} s_0[i]^2- \\ \frac{1}{\rho g}\left(\begin{array}{l} \sqrt{\left.\left.{ }\ ^G F_x[i]\right]^2+{ }^G F_y[i]\right]^2+\left({ }^G F_z[i]+\rho g s_0[i]\right)^2} \\ -\sqrt{{ }\ ^G F_x[i]^2+{ }^G F_y[i]^2+{ }^G F_z[i]^2} \end{array}\right) . \end{gathered}$

(13)

式(11)—(13)中，已知^GR_C和^Gt_C时，未知变量有绳索下端三轴方向受力^GF_x[i]、^GF_y[i]、^GF_z[i]和原长s₀[i]，需要额外补充方程才可以迭代求解。

3 Stewart平台模型建立

在非惯性系下Stewart平台的逆解动力学模型是本文的讨论重点之一，核心目标是通过下平台的运动方程推导整体耦合振动的非惯性坐标系中6根伸缩杆对上平台产生的反作用力，为后一步迭代做基础。其推导较为复杂，本节将作简明叙述。

首先，将伸缩杆视为整体，关于上平台锚固点B[i]，结合式(3)可以列出伸缩杆的Newton-Euler方程：

$\begin{gathered} { }\ ^G \boldsymbol{F}_U[i]+{ }^G \boldsymbol{F}_D[i]= \\ m_U\left({ }^G \boldsymbol{a}_B[i]+{ }^G \boldsymbol{a}_U[i]-\boldsymbol{g}\right)+ \\ m_D\left({ }^G \boldsymbol{a}_B[i]+{ }^G \boldsymbol{a}_D[i]-\boldsymbol{g}\right) . \end{gathered}$

(14)

$\begin{gathered}^G\mathit{\boldsymbol{S}}[i]{ \times ^G}{\mathit{\boldsymbol{F}}_D}[i] + {M_{UD}}{[i]^G}\mathit{\boldsymbol{s}}[i] = \\ m_U{ }\ ^G \boldsymbol{r}_U[i] \times\left({ }^G \boldsymbol{a}_B[i]+{ }^G \boldsymbol{a}_U[i]-\boldsymbol{g}\right)+ \\ m_D{ }\ ^G \boldsymbol{r}_D[i] \times\left({ }^G \boldsymbol{a}_B[i]+{ }^G \boldsymbol{a}_D[i]-\boldsymbol{g}\right)+ \\ \quad\left({ }^G \boldsymbol{I}_U[i]+{ }^G \boldsymbol{I}_D[i]\right){ }\ ^G \boldsymbol{A}[i]+ \\ { }\ ^G \boldsymbol{W}[i] \times\left(\left({ }^G \boldsymbol{I}_U[i]+{ }^G \boldsymbol{I}_D[i]\right){ }\ ^G \boldsymbol{W}[i]\right) . \end{gathered}$

(15)

其中：^GF_U[i]和^GF_D[i]分别为上平台锚固点B[i]和下平台锚固点P[i]对伸缩杆产生的作用力，M_UD[i]^Gs[i]为上下平台阻止伸缩杆产生轴向转动的力矩，^Ga_B[i]为上平台锚固点B[i]在全局坐标下的加速度；m_U和m_D分别为伸缩杆上下两部分的质量，而^GI_U[i]和^GI_D[i]、^Gr_U[i]和^Gr_D[i]以及^Ga_D[i]和^Ga_D[i]分别为其在全局坐标下关于锚固点B[i]的惯量矩阵、质心相对位置以及质心相对加速度，^GW[i]和^GA[i]则分别为伸缩杆在全局坐标系下的旋转角速度和角加速度。

结合式(4)和(5)，用单位方向向量^Gs[i]乘式(15)后化简可以得到

${ }\ ^G \boldsymbol{F}_D[i]=F_S[i]{ }\ ^G \boldsymbol{s}[i]+\boldsymbol{c}[i], $

(16)

$F_S[i]={ }\ ^G \boldsymbol{F}_D[i] \times{ }\ ^G \boldsymbol{s}[i], $

(17)

$\boldsymbol{c}[i]=-{ }\ ^G \boldsymbol{s}[i] \times \boldsymbol{D}[i] / L[i], $

(18)

$\begin{gathered} \boldsymbol{D}[i]=m_U{ }\ ^G \boldsymbol{r}_U[i] \times\left({ }^G \boldsymbol{a}_B[i]+{ }^G \boldsymbol{a}_U[i]-\boldsymbol{g}\right)+ \\ m_D{ }\ ^G \boldsymbol{r}_D[i] \times\left({ }^G \boldsymbol{a}_B[i]+{ }^G \boldsymbol{a}_D[i]-\boldsymbol{g}\right)+ \\ \left({ }^G \boldsymbol{I}_U[i]+{ }^G \boldsymbol{I}_D[i]\right){ }\ ^G \boldsymbol{A}[i]+ \\ { }\ ^G \boldsymbol{W}[i] \times\left(\left({ }^G \boldsymbol{I}_U[i]+{ }^G \boldsymbol{I}_D[i]\right){ }\ ^G \boldsymbol{W}[i]\right) . \end{gathered}$

(19)

其中F_S[i]为作用力^GF_D[i]在伸缩杆轴向上的投影。

同理，已知下平台坐标P系下各锚固点坐标^PP[i]的情况下，关于P系原点^PO可以列出下平台的Newton-Euler方程：

$-\sum\limits_{i=1}^6{ }\ ^G \boldsymbol{F}_D[i]=m_P\left({ }^G \boldsymbol{a}_P+{ }^G \boldsymbol{a}_{P 0}-\boldsymbol{g}\right),$

(20)

$\begin{gathered} -\sum\limits_{i=1}^6\left(\left({ }^G \boldsymbol{R}_B{ }\ ^B \boldsymbol{R}_P{ }\ ^P \boldsymbol{P}[i]\right) \times{ }\ ^G \boldsymbol{F}_D[i]\right)= \\ { }\ ^G \boldsymbol{I}_P{ }\ ^G \boldsymbol{\varepsilon}_P+{ }^G \boldsymbol{\omega}_P \times\left({ }^G \boldsymbol{I}_P{ }\ ^G \boldsymbol{\omega}_P\right)+ \\ m_P{ }\ ^G \boldsymbol{r}_{P 0} \times\left({ }^G \boldsymbol{a}_P+{ }^G \boldsymbol{a}_{P 0}-\boldsymbol{g}\right) . \end{gathered}$

(21)

其中：m_P为下平台的质量，^Ga_P为P系原点^PO在全局坐标下的加速度，^Gω_P和^Gε_P分别为下平台的旋转角速度和角加速度，^GI_P、^Gr_P0和^Ga_P0分别为全局坐标下平台相对P系原点^PO的惯量矩阵、质心相对位置和质心相对加速度。

将式(16)代入式(20)和(21)并化简可得

$\boldsymbol{N} \cdot \boldsymbol{F}_S=\boldsymbol{n}.$

(22)

其中，有

$\mathit{\boldsymbol{N}} = \left[ \begin{array}{l} \ ^G{\mathit{\boldsymbol{s}}}[1]{\quad ^G}{\mathit{\boldsymbol{s}}}[2]{\quad ^G}{\mathit{\boldsymbol{s}}}[3]{\quad ^G}{\mathit{\boldsymbol{s}}}[4]{\quad ^G}{\mathit{\boldsymbol{s}}}[5]{\quad ^G}{\mathit{\boldsymbol{s}}}[6]\\ {\mathit{\boldsymbol{j}}}[1]\quad\quad {\mathit{\boldsymbol{j}}}[2]\quad\quad {\mathit{\boldsymbol{j}}}[3]\quad\quad {\mathit{\boldsymbol{j}}}[4]\quad \quad {\mathit{\boldsymbol{j}}}[5]\quad {\mathit{\boldsymbol{j}}}[6] \end{array} \right], $

(23)

$\boldsymbol{j}[i]=\left({ }^G \boldsymbol{R}_B{ }\ ^B \boldsymbol{R}_P{ }\ ^P \boldsymbol{P}[i]\right) \times{ }\ ^G \boldsymbol{s}[i], $

(24)

$\boldsymbol{F}_S=\left[F_S[1] \ \ F_S[2] \ \ F_S[3]\ \ F_S[4]\ \ F_S[5] \ \ F_S[6]\right]^{\mathrm{T}}, $

(25)

$\boldsymbol{n}=\left[\begin{array}{l} \boldsymbol{n}_1 \\ \boldsymbol{n}_2 \end{array}\right], $

(26)

$\boldsymbol{n}_1=-\sum\limits_{i=1}^6 \boldsymbol{c}[i]-m_P\left({ }^G \boldsymbol{a}_P+{ }^G \boldsymbol{a}_{P 0}-\boldsymbol{g}\right), $

(27)

$\begin{aligned} \boldsymbol{n}_2 &=-m_P{ }\ ^G \boldsymbol{r}_{P 0} \times\left({ }^G \boldsymbol{a}_P+{ }^G \boldsymbol{a}_{P 0}-\boldsymbol{g}\right)-\\ &\left({ }^G \boldsymbol{I}_P{ }\ ^G \boldsymbol{\varepsilon}_P+{ }^G \boldsymbol{\omega}_P \times\left({ }^G \boldsymbol{I}_P{ }\ ^G \boldsymbol{\omega}_P\right)\right)+\\ & \sum\limits_{i=1}^6\left(\left({ }^G \boldsymbol{R}_B{ }\ ^B \boldsymbol{R}_P{ }\ ^P \boldsymbol{P}[i]\right) \times \boldsymbol{c}[i]\right) . \end{aligned}$

(28)

综上，根据式(22)即可求得重要的中间变量F_S[i]。将式(16)再代入式(14)，可以上平台锚固点B[i]对伸缩杆作用力的表达式

$\begin{gathered} { }\ ^G \boldsymbol{F}_U[i]=m_U\left({ }^G \boldsymbol{a}_B[i]+{ }^G \boldsymbol{a}_U[i]-\boldsymbol{g}\right)+ \\ m_D\left({ }^G \boldsymbol{a}_B[i]+{ }^G \boldsymbol{a}_D[i]-\boldsymbol{g}\right)- \\ F_S[i]{ }\ ^G \boldsymbol{s}[i]-\boldsymbol{c}[i] . \end{gathered}$

(29)

整理可知式(29)本质是关于上下平台姿态变量的函数，分别为^GR_B、^Gω_B、^Gε_B、$ ^G \ddot{\boldsymbol{t}}_B$和^BR_P、^Bω_P、^Bε_P、^Bt_P、$ ^B \dot{\boldsymbol{t}}_P$、$ ^B \ddot{\boldsymbol{t}}_P$，以及^Ga_P0，代表上平台B系在全局坐标下的旋转角、角速度、角加速度和位移加速度，下平台P系在B系下的旋转角、角速度、角加速度和位移、速度、加速度，以及下平台质心相对P系原点的加速度。

4 模型的中间变量与参数取值

参考图 4与式(3)，可以写出伸缩杆下端相对上端在B系下的位置向量

${ }\ ^B \boldsymbol{S}[i]={ }\ ^B \boldsymbol{R}_P{ }\ ^P \boldsymbol{P}[i]+{ }^B \boldsymbol{t}_P-{ }\ ^B \boldsymbol{B}[i] .$

(30)

对式(30)关于时间求导，可知伸缩杆下端相对上端在B系下的速度和加速度分别为：

${ }\ ^B \dot{\boldsymbol{S}}[i]={ }\ ^B \boldsymbol{\omega}_P \times\left({ }^B \boldsymbol{R}_P{ }\ ^P \boldsymbol{P}[i]\right)+{ }^B \dot{\boldsymbol{t}}_P, $

(31)

$\begin{gathered} { }\ ^B \ddot{\boldsymbol{S}}[i]={ }\ ^B \boldsymbol{\varepsilon}_P \times\left({ }^B \boldsymbol{R}_P{ }\ ^P \boldsymbol{P}[i]\right)+ \\ { }\ ^B \boldsymbol{\omega}_P \times\left({ }^B \boldsymbol{\omega}_P \times\left({ }^B \boldsymbol{R}_P{ }\ ^P \boldsymbol{P}[i]\right)\right)+{ }^B \ddot{\boldsymbol{t}}_P . \end{gathered}$

(32)

同理，对式(3)关于时间求导可知伸缩杆下端相对上端在全局坐标下的速度和加速度为：

${ }\ ^G \dot{\boldsymbol{S}}[i]={ }\ ^G \boldsymbol{\omega}_B \times{ }\ ^G \boldsymbol{S}[i]+{ }^G \boldsymbol{R}_B{ }\ ^B \dot{\boldsymbol{S}}[i], $

(33)

$\begin{gathered} { }\ ^G \ddot{S}[i]={ }\ ^G \boldsymbol{\varepsilon}_B \times{ }\ ^G \boldsymbol{S}[i]+{ }^G \boldsymbol{\omega}_B \times{ }\ ^G \dot{\boldsymbol{S}}[i]+ \\ { }\ ^G \boldsymbol{\omega}_B \times\left({ }^G \boldsymbol{R}_B{ }\ ^B \dot{\boldsymbol{S}}[i]\right)+{ }^G \boldsymbol{R}_B{ }\ ^B \ddot{\boldsymbol{S}}[i] . \end{gathered}$

(34)

对伸缩杆建立与之绑定的Cartesian坐标系，设为L系。其原点^LO[i]为上平台锚固点B[i]，如图 4所示，并且3个正交轴单位向量在全局坐标系下分别为：

${ }\ ^G \boldsymbol{x}_L[i]={ }\ ^G \boldsymbol{s}[i], $

(35)

${ }\ ^G \boldsymbol{y}_L[i]={ }\ ^G \boldsymbol{R}_B \frac{{ }\ ^B \boldsymbol{S}[i] \times{ }\ ^B \boldsymbol{B}[i]}{\left|{ }\ ^B \boldsymbol{S}[i] \times{ }\ ^B \boldsymbol{B}[i]\right|}, $

(36)

${ }\ ^G \boldsymbol{z}_L[i]={ }\ ^G \boldsymbol{x}_L[i] \times{ }\ ^G \boldsymbol{y}_L[i] .$

(37)

则全局坐标G系与伸缩杆L系间的旋转矩阵为

${ }\ ^G \boldsymbol{R}_L[i]=\left[{ }\ ^G \boldsymbol{x}_L[i] \quad{ }\ ^G \boldsymbol{y}_L[i] \quad{ }\ ^G \boldsymbol{z}_L[i]\right] .$

(38)

以下为第3节中其他需要补充的中间变量：

${ }\ ^G \boldsymbol{r}_U[i]={ }\ ^G \boldsymbol{R}_L[i]^L \boldsymbol{r}_{U 0}[i], $

(39)

${ }\ ^G \boldsymbol{r}_D[i]={ }\ ^G \boldsymbol{R}_L[i]{ }\ ^L \boldsymbol{r}_{D 0}[i]+{ }^G \boldsymbol{S}[i] .$

(40)

其中^Lr_U0[i]和^Lr_D0[i]分别为在L系下伸缩杆上部分的质心距上平台锚固点B[i]和下部分的质心距下平台锚固点P[i]的位置向量。

${ }\ ^G \boldsymbol{I}_U[i]={ }\ ^G \boldsymbol{R}_L[i]^L \boldsymbol{I}_U[i]{ }\ ^G \boldsymbol{R}_L[i]^{\mathrm{T}}, $

(41)

${ }\ ^G \boldsymbol{I}_D[i]={ }\ ^G \boldsymbol{R}_L[i]{ }\ ^L \boldsymbol{I}_D[i]{ }\ ^G \boldsymbol{R}_L[i]^{\mathrm{T}}, $

(42)

${ }\ ^G \boldsymbol{I}_P=\left({ }^G \boldsymbol{R}_C{ }\ ^C \boldsymbol{R}_B{ }\ ^B \boldsymbol{R}_P\right){ }\ ^P \boldsymbol{I}_P\left({ }^G \boldsymbol{R}_C{ }\ ^C \boldsymbol{R}_B{ }\ ^B \boldsymbol{R}_P\right)^{\mathrm{T}} .$

(43)

其中：^LI_U[i]和^LI_D[i]为L系下伸缩杆上、下部分关于锚固点B[i]的惯量矩阵，^PI_P为下平台在P系下相对原点^PO的惯量矩阵。

$\dot{L}[i]={ }\ ^G \boldsymbol{s}[i] \times{ }\ ^G \dot{\boldsymbol{S}}[i], $

(44)

${ }\ ^G \boldsymbol{W}[i]={ }\ ^G \boldsymbol{s}[i] \times{ }\ ^G \dot{\boldsymbol{S}}[i] / L[i], $

(45)

$\ddot{L}[i]={ }\ ^G \boldsymbol{s}[i] \times{ }\ ^G \ddot{\boldsymbol{S}}[i]+L[i]{ }\ ^G \boldsymbol{W}[i] \times{ }\ ^G \boldsymbol{W}[i], $

(46)

${ }\ ^G \boldsymbol{A}[i]=\left({ }^G \boldsymbol{s}[i] \times{ }\ ^G \ddot{\boldsymbol{S}}[i]-2 \dot{L}[i]{ }\ ^G \boldsymbol{W}[i]\right) / L[i].$

(47)

其中$ \dot{L}[i]$和$ \ddot{L}[i]$分别代表伸缩杆长度变化的速度和加速度。

$\begin{gathered} { }\ ^G \boldsymbol{a}_B[i]={ }\ ^G \boldsymbol{\varepsilon}_B \times\left({ }^G \boldsymbol{R}_B{ }\ ^B \boldsymbol{B}[i]\right)+{ }^G \ddot{\boldsymbol{t}}_B+ \\ { }\ ^G \boldsymbol{\omega}_B \times\left({ }^G \boldsymbol{\omega}_B \times\left({ }^G \boldsymbol{R}_B{ }\ ^B \boldsymbol{B}[i]\right)\right), \end{gathered}$

(48)

$\begin{aligned} &{ }\ ^G \boldsymbol{a}_U[i]={ }\ ^G \boldsymbol{A}[i] \times{ }\ ^G \boldsymbol{r}_U[i]+ \\ &{ }\ ^G \boldsymbol{W}[i] \times\left({ }^G \boldsymbol{W}[i] \times{ }\ ^G \boldsymbol{r}_U[i]\right), \end{aligned}$

(49)

$\begin{gathered} { }\ ^G \boldsymbol{a}_D[i]={ }\ ^G \boldsymbol{A}[i] \times{ }\ ^G \boldsymbol{r}_D[i]+ \\ { }\ ^G \boldsymbol{W}[i] \times\left({ }^G \boldsymbol{W}[i] \times{ }\ ^G \boldsymbol{r}_D[i]\right)+ \\ \ddot{L}[i]^G \boldsymbol{s}[i]+2 \dot{L}[i]^G \boldsymbol{W}[i] \times{ }\ ^G \boldsymbol{s}[i], \end{gathered}$

(50)

${ }\ ^G \boldsymbol{\omega}_P={ }\ ^G \boldsymbol{\omega}_B+{ }^G \boldsymbol{R}_B{ }\ ^B \boldsymbol{\omega}_P, $

(51)

${ }\ ^G \boldsymbol{\varepsilon}_P={ }\ ^G \boldsymbol{\varepsilon}_B+{ }^G \boldsymbol{\omega}_B \times\left({ }^G \boldsymbol{R}_B{ }\ ^B \boldsymbol{\omega}_P\right)+{ }^G \boldsymbol{R}_B{ }\ ^B \boldsymbol{\varepsilon}_P, $

(52)

$\begin{aligned} { }\ ^G \boldsymbol{a}_P=&{ }\ ^G \boldsymbol{\varepsilon}_B \times\left({ }^G \boldsymbol{R}_B{ }\ ^B \boldsymbol{t}_P\right)+{ }^G \boldsymbol{\omega}_B \times\left({ }^G \boldsymbol{\omega}_B \times\left({ }^G \boldsymbol{R}_B{ }\ ^B \boldsymbol{t}_P\right)\right)+\\ & 2^G \boldsymbol{\omega}_B \times\left({ }^G \boldsymbol{R}_B{ }\ ^B \boldsymbol{t}_P\right)+{ }^G \boldsymbol{R}_B{ }\ ^B \ddot{\boldsymbol{t}}_P+{ }^G \ddot{\boldsymbol{t}}_B . \end{aligned}$

(53)

式(48)—(53)为伸缩杆和下平台的相关(角)加速度表达式，其中^Bω_p为下平台P系在B系下的旋转角速度。

表 1中列出了本文计算所需的相关参数取值。

5 模型迭代算法

根据2节中式(11)—(13)的分析，可以将绳索模型的下端索力函数记为

$\begin{aligned} &{ }\ ^G \boldsymbol{F}_C[i]=\left[\begin{array}{lll} { }\ ^G F_x[i] & { }\ ^G F_y[i] & \left.{ }\ ^G F_z[i]\right]^{\mathrm{T}}= \end{array}\right.\\ &{ }\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ^G \boldsymbol{F}_C[i]\left({ }^G \boldsymbol{R}_C, { }\ ^G \boldsymbol{t}_C, s_0[i]\right) . \end{aligned}$

(54)

同理，根据3节中式(29)，可得上平台锚固点B[i]对伸缩杆的作用力函数可以写为

$\begin{gathered} { }\ ^G \boldsymbol{F}_U[i]= \\ { }\ ^G \boldsymbol{F}_U[i]\left(\begin{array}{c} { }\ ^G \boldsymbol{R}_B, { }\ ^G \boldsymbol{\omega}_B, { }\ ^G \boldsymbol{\varepsilon}_B, { }\ ^G \ddot{\boldsymbol{t}}_B, { }\ ^G \boldsymbol{a}_{P 0}, \\ { }\ ^B \boldsymbol{R}_P, { }\ ^B \boldsymbol{\omega}_P, { }\ ^B \boldsymbol{\varepsilon}_P, { }\ ^B \boldsymbol{t}_P, { }\ ^B \dot{\boldsymbol{t}}_P, { }\ ^B \ddot{\boldsymbol{t}}_P \end{array}\right). \end{gathered}$

(55)

又因为馈源舱与Stewart上平台通过刚体相连可视为整体，则

$\begin{gathered} { }\ ^G \boldsymbol{F}_U[i]= \\ { }\ ^G \boldsymbol{F}_U[i]\left(\begin{array}{c} { }\ ^G \boldsymbol{R}_C{ }\ ^C \boldsymbol{R}_B, { }\ ^G \boldsymbol{\omega}_C, { }\ ^G \boldsymbol{\varepsilon}_C, { }\ ^G \ddot{\boldsymbol{t}}_C, { }\ ^G \boldsymbol{a}_{P 0}, \\ { }\ ^B \boldsymbol{R}_P, { }\ ^B \boldsymbol{\omega}_P, { }\ ^B \boldsymbol{\varepsilon}_P, { }\ ^B \boldsymbol{t}_P, { }\ ^B \boldsymbol{t}_P, { }\ ^B \ddot{\boldsymbol{t}}_P \end{array}\right) . \end{gathered}$

(56)

其中：^Gω_C=^Gω_B、^Gε_C=^Gε_B和$ ^G{\ddot t_C}{ = ^G}{\ddot t_B}$，分别为馈源舱整体的旋转角速度、角加速度和位移加速度；^CR_B为上平台B系相对馈源舱C系的旋转矩阵，为常量。

联立式(54)和(56)，对馈源舱与Stewart上平台整体关于C系原点^CO求Newton-Euler方程，

$-\sum\limits_{i=1}^6\left({ }^G \boldsymbol{F}_C[i]+{ }^G \boldsymbol{F}_U[i]\right)=m_{C B}\left({ }^G \ddot{\boldsymbol{i}}_C+{ }^G \boldsymbol{a}_{C B}-\boldsymbol{g}\right) .$

(57)

$\begin{gathered} \left.-\sum\limits_{i=1}^6\left({ }^G \boldsymbol{R}_C{ }\ ^C \boldsymbol{r}_A[i]\right) \times{ }\ ^G \boldsymbol{F}_C[i]\right)- \\ \sum\limits_{i=1}^6\left({ }^G \boldsymbol{R}_C\left({ }^C \boldsymbol{R}_B{ }\ ^B \boldsymbol{B}[i]+{ }^C \boldsymbol{t}_B\right) \times{ }\ ^G \boldsymbol{F}_U[i]\right)= \\ { }\ ^G \boldsymbol{I}_{C B}{ }\ ^G \boldsymbol{\varepsilon}_C+{ }^G \boldsymbol{\omega}_C \times\left({ }^G \boldsymbol{I}_{C B}{ }\ ^G \boldsymbol{\omega}_C\right)+ \\ m_{C B}\left({ }^G \boldsymbol{R}_C{ }\ ^C \boldsymbol{r}_{C B 0}\right) \times\left({ }^G \ddot{\boldsymbol{t}}_C+{ }^G \boldsymbol{a}_{C B}-\boldsymbol{g}\right). \end{gathered}$

(58)

$\begin{aligned} &{ }\ ^G \boldsymbol{a}_{C B}={ }\ ^G \boldsymbol{\varepsilon}_C \times\left({ }^G \boldsymbol{R}_C{ }\ ^C \boldsymbol{r}_{C B 0}\right)+ \\ &{ }\ ^G \boldsymbol{\omega}_C \times\left({ }^G \boldsymbol{\omega}_C \times\left({ }^G \boldsymbol{R}_C{ }\ ^C \boldsymbol{r}_{C B 0}\right)\right). \end{aligned}$

(59)

其中：^Ct_B为C系下B系原点^BO的位置向量，m_CB、^Cr_CB0和^Ga_CB分别为馈源舱与Stewart上平台整体的质量、C系下的质心位置和G系下相对C系原点^CO的加速度，且有

${ }\ ^G \boldsymbol{I}_{C B}={ }\ ^G \boldsymbol{R}_C{ }\ ^C \boldsymbol{I}_{C B}{ }\ ^G \boldsymbol{R}_C^{\mathrm{T}} \text {. }$

(60)

其中^CI_CB为C系下馈源舱和Stewart上平台整体关于C系原点的惯量矩阵，为常量。

至此，FAST动力学模型的迭代函数已推导完毕，具体迭代计算流程如图 5所示，其中t为时间，T为仿真时长，Δt为时间步长。

6 仿真结果与分析

将5节中的迭代算法基于MATLAB和Simulink编程进行仿真，取馈源舱在工作空间中央时，即有初始静态坐标

${ }\ ^G \boldsymbol{t}_C(0)=\left[\begin{array}{lll} 0 & 0 & 140 \mathrm{~m} \end{array}\right]^{\mathrm{T}} \text {. }$

(61)

在该点时，旋转矩阵^GR_C的初始值和^CR_B均为单位矩阵；同时下平台在x轴方向相对上平台以0.01 m为幅值作5 Hz的正弦运动来模拟现实中不断矫正误差来回运动的情况，即B系下平台的运动轨迹为

${ }\ ^B \boldsymbol{x}_P(t)=\left[\begin{array}{lll} 0.01 \mathrm{~m} \times \sin (10 {\rm{ \mathsf{ π}}} t) & 0 & 0 \end{array}\right]^{\mathrm{T}}, $

(62)

${ }\ ^B \boldsymbol{t}_P(t)={ }\ ^B \boldsymbol{x}_P(t)+\left[\begin{array}{lll} 0 & 0 & h \end{array}\right]^{\mathrm{T}}.$

(63)

其中h是下平台相对上平台的高度差。于是，可得C系下平台的运动轨迹为

${ }\ ^C \boldsymbol{t}_P(t)={ }\ ^C \boldsymbol{R}_B{ }\ ^B \boldsymbol{t}_P(t)+{ }^C \boldsymbol{t}_B \text {. }$

(64)

在此情况下，下平台的运动会导致系统整体振动，从而带动下平台一起偏移原来计划运动的位置，则下平台实际在全局坐标下相对初始位置的运动轨迹表达式为

$\begin{gathered} { }\ ^G \boldsymbol{x}_P(t)={ }\ ^G \boldsymbol{t}_C(t)+{ }^G \boldsymbol{R}_C(t){ }\ ^C \boldsymbol{t}_P(t)- \\ \left({ }^G \boldsymbol{t}_C(0)+{ }^G \boldsymbol{R}_C(0){ }\ ^C \boldsymbol{t}_P(0)\right). \end{gathered}$

(65)

图 6为式(65)仿真计算得出的下平台全局实际运动轨迹，相比原定的下平台运动轨迹，实际运动偏移了不少，将其与式(62)的最初计划轨迹做差可得

$\Delta{ }\ ^G \boldsymbol{x}_P={ }\ ^G \boldsymbol{x}_P(t)-{ }\ ^G \boldsymbol{R}_C(0){ }\ ^C \boldsymbol{R}_B{ }\ ^B \boldsymbol{x}_P(t) .$

(66)

即下平台运动带来的额外系统误差，如图 7所示。

又如图 5中介绍，仿真过程中的每个时间节点都会求解馈源舱整体的振动(角)加速度，来源于Stewart下平台运动带来的耦合振动。对该(角)加速度积分可以得到系统的振动位移和角度，如图 8和图 9所示。通过计算，发现系统的振动位移与额外系统误差显示出很强相关性，三轴方向分别为0.971 7、0.991 5和0.972 0，标准差分别为3.147 3× 10^-4 m、9.601 5×10^－6 m和1.900 2×10^－6 m。

通过相同方法，在多个位置计算得到的相关性均在0.97以上，表 2列出了几个典型位置的标准差和误差幅值。因为额外误差只会来自于系统的耦合振动位移和角度，考虑到误差幅值的量级，可以认为额外误差主要来自于系统的耦合振动位移，只有小部分来自转动角度，即产生标准差的原因。

从这些算例中还可得，Stewart平台在以5 Hz补偿0.01 m误差的同时还会带来额外最大约0.003 m的误差，这既说明通过Stewart补偿误差的有效作用，又为进一步提高FAST精度控制提供了参考。日后该理论模型通过实验数据进行参数迭代，利用提前仿真计算额外补偿误差的优势，可实现PID等高精度的实时动力学主动补偿控制算法。

7 结论

本文针对一种应用于FAST的索驱动并联机器人进行了动力学建模与仿真分析，通过对上端绳索和下端Stewart平台分别建模，后联立方程对中间结构的Newton-Euler方程迭代来求解馈源舱整体的振动。基于该算法仿真，可以得出结论：1) Stewart平台从原理上能有效补偿下平台的姿态误差；2) Stewart下平台频繁的误差补偿运动会带来额外误差；3) 该额外误差主要来自于系统的耦合振动位移而非转动。本文提出的索驱动并联机器人与Stewart平台结合的动力学迭代算法，为FAST和其他类似结构进一步提高控制精度提供了参考，并有望实现实时动力学主动补偿控制算法。