拟人机器人一直是仿生机器人领域的研究热点，受到了众多学者的重视^[1-2]. 目前，美国处于领先水平，如美国波士顿公司研制的PETMAN机器人已经应用在防化服的测试领域，其机械臂机构主要采用串并混联机构，承载能力较强，在工程方面取得了较好的效果. 我国研制的拟人机器人仅能演示一些基本的类人动作，且负载较小、控制实时性较差，究其原因主要是在结构方面采用串联机构，导致运动惯性较大、累积误差及承载能力较小等问题.

相对于串联机构，并联机构具有承载能力强、运动惯性小、动态特性好等优点^[3]，有效地弥补了串联机构的不足之处，但并联机构存在工作空间较小的缺点，而混联机构兼备上述2种机构的优点^[4-5]. 非冗余自由度机构往往存在奇异位形，对机构及驱动器有较大的冲击，冗余自由度机构可以避开奇异位形、有较好的刚度特性和动态性能^[6-8]. 因此，冗余混联机构受到许多学者的广泛关注^[9-10]. 目前，常见的混联机构动力学建模方法有拉格朗日法^[11]、牛顿-欧拉法^[12]、凯恩法^[13]、虚功原理法^[14]等，Moosavian等^[15]针对由平面并联机构与Puma型机构串联组成的混联轮式移动机器人，结合牛顿-欧拉和拉格朗日法建立其动力学模型；Wang等^[16]采用虚功原理法建立（2-UPS+U）R混联轮式机械腿的动力学模型，利用拉格朗日方程求解驱动力矩并对其进行数值仿真分析，该结果可以用于机械臂的控制分析和机构的设计开发；Sangveraphunsiri等^[17]采用拉格朗日法建立基于H-4并联机构的五自由度混联机械臂的动力学模型，通过ADAMS仿真验证动力学模型的正确性，并将动力学模型用于实时反馈控制. 上述工作均以运动构件较少的混联机构为研究目标，采用虚功原理和拉格朗日法推导出形式简洁的动力学模型；在系统运动构件较多的情况下，其求解过程较复杂.

本研究提出七自由度冗余串并混联拟人机械臂机构，在各关节坐标系下采用矢量法分析各关节构件的速度和加速度，建立机械臂机构的雅克比矩阵；在各关节坐标系下利用牛顿-欧拉法建立力平衡方程，利用小变形原理建立肩关节协调补充方程，联立方程组求解出机械臂机构动力学全解，基于动力学模型，计算末端轨迹为直线、抛物线和圆形时各关节的驱动力矩/力，并通过仿真验证机械臂动力学模型的正确性，为该机构的控制策略提供重要依据.

混联拟人机械臂三维模型如图1所示. 肩关节由5R二自由度并联机构组成，可以实现人体肩关节的前屈和后伸、外展和内敛运动；上臂处转动副由谐波减速器构成，可以实现人体肩关节的内旋和外旋运动；前臂处由一对锥齿轮组成，可以完成人体肘关节的上屈和下伸运动；腕关节由3 UPS/S三自由度并联机构组成，可以实现人体腕关节的桡屈和尺屈、背伸和屈曲、外旋和内旋运动. 将上述结构通过并-串-串-并形式组成七自由度冗余混联拟人机械臂.

拟人机械臂机构简图如图2所示，分别构建各关节坐标系. 肩关节静坐标系O_s-X_sY_sZ_s，动坐标系O_s-x_sy_ss_s，肩关节静坐标系和动坐标系的坐标原点均与肩关节动平台转动中心O_s重合，Z_s轴垂直于基座，方向向上，Y_s轴平行于向量 $\overrightarrow{O_{\rm s}A_2} $，X_s轴符合右手螺旋定则；z_s轴平行于向量 $\overrightarrow {O_{\rm s}C_1} $，x_s轴平行于向量 $\overrightarrow {O_{\rm s}C_2 }$，y_s轴符合右手螺旋定则；在初始状态，肩关节动坐标系与静坐标系重合. 上臂动坐标系O_m-x_my_mz_m，z_m轴平行于上臂旋转轴，方向向上，x_m轴平行于向量 $\overrightarrow {O_{\rm s}C_2} $，y_m轴符合右手螺旋定则. 前臂动坐标系O_e-x_ey_ez_e，z_e轴平行于前臂旋转轴，y_e轴平行于向量 $\overrightarrow {EF} $， x_e轴符合右手螺旋定则. 腕关节静坐标系O_w-X_wY_wZ_w，动坐标系O_w-x_wy_wz_w，腕关节静坐标系和动坐标系的坐标原点均与中央球铰的旋转中心点O_w重合，Z_w轴垂直于U₁U₂U₃平面，方向向下，X_w轴平行于向量 $\overrightarrow{{{O}}_{\rm{w}}^{'}{{{U}}_1}}$，Y_w轴符合右手螺旋定则， ${O_{\rm{w}}^{'}}$为点O_w在U₁U₂U₃平面的投影点；z_w轴垂直于S₁S₂S₃平面，方向向下，x_w轴平行于向量 $\overrightarrow {O_{\rm w}S_1} $，y_w轴符合右手螺旋定则；在初始状态，腕关节动坐标系与静坐标系重合. 肩关节静坐标系为机械臂的基坐标系.

上臂坐标系原点O_m在肩关节动坐标系的位置坐标 ${{{p}}_{\rm{m}}} = {{{[}}{\rm{0,0,- }}{r_{\rm{1}}}{{]}}^{\rm{T}}}$，前臂坐标系原点O_e在上臂动坐标系的位置坐标 ${{{p}}_{\rm{e}}} = {{{[}}{\rm{0,0, - }}{r_{\rm{2}}}{{]}}^{\rm{T}}}$，腕关节坐标系原点O_w在前臂动坐标系的位置坐标 ${{{p}}_{\rm{w}}} = {{{[}}{\rm{0,}}{r_{\rm{3}}}{\rm{,0}}{{]}}^{\rm{T}}}$，机械臂末端手掌中心点H在腕关节动坐标系的位置坐标 ${{{p}}_{\rm{h}}} = {{{[}}{\rm{0,0,}}{r_{\rm{4}}}{{]}}^{\rm{T}}}$. 采用Denavit-Hartenberg（D-H）法构建相邻关节之间的位姿转换矩阵：

(1) $\left. \begin{array}{l} {{{A}}_{\rm{1}}}\! =\! \left[ {\begin{array}{*{20}{c}} {{{{R}}_{\rm{s}}}}&\!{\bf{0}} \\ {\bf{0}}\!&\!\!{\rm{1}} \end{array}} \right],{{{A}}_{\rm{2}}} \!=\! \left[ {\begin{array}{*{20}{c}} {{{{R}}_{\rm{m}}}}&{{{{p}}_{\rm{m}}}} \\ {\bf{0}}\!&\!\!{\rm{1}} \end{array}} \right],{{{A}}_{\rm{3}}} \!=\! \left[ {\begin{array}{*{20}{c}} {{{{R}}_{\rm{e}}}}&{{{{p}}_{\rm{e}}}} \\ {\bf{0}}\!&\!\!{\rm{1}} \end{array}} \right], \\ {{{A}}_{\rm{4}}} \!=\! \left[ {\begin{array}{*{20}{c}} {{{{I}}_{\rm{1}}}}&\!{{{{p}}_{\rm{w}}}} \\ {\bf{0}}\!&\!\!{\rm{1}} \end{array}} \right],{{{A}}_{\rm{5}}}\! =\! \left[ {\begin{array}{*{20}{c}} {{{{R}}_{\rm{w}}}}&{\bf{0}} \\ {\bf{0}}\!&\!\!{\rm{1}} \end{array}} \right],{{{A}}_{\rm{6}}} \!=\! \left[ {\begin{array}{*{20}{c}} {{{{I}}_{\rm{2}}}}&{{{{p}}_{\rm{h}}}} \\ {\bf{0}}\!&\!\!{\rm{1}} \end{array}} \right], \\ \end{array} \right\}$

(2) $\left. \begin{array}{l} {{{R}}_{\rm{s}}} = {{R}}(y,{\beta _{\rm{s}}}){{R}}(x,{\gamma _{\rm{s}}}) ,\\ {{{R}}_{\rm{m}}} = {{R}}(z,{\theta _{\rm{3}}}) ,\\ {{{R}}_{\rm{e}}} = {{R}}(y,{\rm{9}}{{\rm{0}}^{\rm{o}}}){{R}}(z,{\rm{ - 9}}{{\rm{0}}^{\rm{o}}}){{R}}(z,{\theta _{\rm{4}}}) ,\\ {{{I}}_{\rm{1}}} = {{R}}(z,{\rm{18}}{{\rm{0}}^{\rm{o}}}){{R}}(x,{\rm{9}}{{\rm{0}}^{\rm{o}}}) ,\\ {{{R}}_{\rm{w}}} = {{R}}(z,{\alpha _{\rm{w}}}){{R}}(y,{\beta _{\rm{w}}}){{R}}(x,{\gamma _{\rm{w}}}), \\ {{{I}}_{\rm{2}}} = {{I}}. \\ \end{array} \right\}$

式中： ${\beta _{\rm{s}}}$、 ${\gamma _{\rm{s}}}$为肩关节动平台绕 ${y_{\rm{s}}}$、 ${x_{\rm{s}}}$轴的旋转角度，θ₃为上臂处驱动器的旋转角度，θ₄为前臂驱动器的旋转角度， ${\alpha _{\rm{w}}}$、 ${\beta _{\rm{w}}}$、 ${\gamma _{\rm{w}}}$为腕关节动平台绕 ${z_{\rm{w}}}$、 ${y_{\rm{w}}}$、 ${x_{\rm{w}}}$轴的旋转角度，I为单位矩阵.

手掌坐标系原点与点H重合，坐标轴与腕关节动坐标轴平行，将上述位姿变换矩阵依次相乘，可以得到机械臂手掌相对于基坐标系的位姿变换矩阵：

(3) ${}^{\rm{0}}{{{T}}_{\rm{h}}} = {{{A}}_{\rm{1}}}{{{A}}_{\rm{2}}}{{{A}}_{\rm{3}}}{{{A}}_{\rm{4}}}{{{A}}_{\rm{5}}}{{{A}}_{\rm{6}}} = \left[ {\begin{array}{*{20}{c}} ^0{{{{R}}_{\rm{h}}}}&^0{{{{P}}_{\rm{h}}}} \\ {\bf{0}}&{\rm{1}} \end{array}} \right].$

式中： $^0{{{R}}_{\rm{h}}}$、 $^0{{{P}}_{\rm{h}}}$为末端手掌相对于基坐标系的姿态矩阵和位置向量.

腕关节动坐标系相对于机械臂基坐标系的位姿变换矩阵为

(4) ${}^{\rm{0}}{{{T}}_{\rm{w}}} = {{{A}}_{\rm{1}}}{{{A}}_{\rm{2}}}{{{A}}_{\rm{3}}}{{{A}}_{\rm{4}}}{{{A}}_{\rm{5}}} = \left[ {\begin{array}{*{20}{c}} {{}^{\rm{0}}{{{R}}_{\rm{w}}}}&{{}^{\rm{0}}{{{P}}_{\rm{w}}}} \\ {\bf{0}}&{\rm{1}} \end{array}} \right].$

式中： ${}^{\rm{0}}{{{R}}_{\rm{w}}}$、 ${}^{\rm{0}}{{{P}}_{\rm{w}}}$为腕关节相对于基坐标系的姿态矩阵和位置向量.

(5) ${}^{\rm{0}} {{{T}}_{\rm{w}}} = {}^{\rm{0}}{{{T}}_{\rm{h}}}{{A}}_{\rm{6}}^ {{\rm{ - 1}}}.$

由式（5）可以求出腕关节坐标系原点在基坐标下的位置矢量 ${}^{\rm{0}}{{{P}}_{\rm{w}}}$，根据余弦定理可以求解前臂驱动器的旋转角度θ₄：

(6) ${\theta _{\rm{4}}} = {\text{π}} - {\rm{arc}}\cos \;\Bigg(\frac{{{{({{{r}}_{\rm{1}}} + {{{r}}_{\rm{2}}})}^{\rm{2}}}{\rm{ +}} r_{\rm{3}}^{\rm{2}}{\rm{ - }}p_{\rm{w}}^{\rm{2}}}}{{{\rm{2}}({{{r}}_{\rm{1}}}{\rm{ + }}{{{r}}_{\rm{2}}}){{{r}}_{\rm{3}}}}}\Bigg).$

式中： ${p_{\rm{w}}} = \sqrt {p_{{\rm{w\_}}x}^{\rm{2}} + p_{{\rm{w\_}}y}^{\rm{2}} + p_{{\rm{w\_}}z}^{\rm{2}}}$.

腕关节坐标系原点相对于基坐标系的位置向量可以表示为

(7) ${}^{\rm{0}}{{{P}}_{\rm{w}}}{\rm{ = }}\left[ {\begin{array}{*{20}{c}} {{p_{{\rm{w\_}}x}}} \\ {{p_{{\rm{w\_}}y}}} \\ {{p_{{\rm{w\_}}z}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{\rm{ - }}{r_{\rm{3}}}({s_{{\theta _{\rm{4}}}}}({c_{\beta _{\rm{s}}}}{s_{{\theta _{\rm{3}}}}}{\rm{ - }}{s_{\beta _{\rm{s}}}}{c_{{\theta _{\rm{3}}}}}{s_{\gamma _{\rm{s}}}}){\rm{ + }}{c_{\gamma _{\rm{s}}}}{s_{\beta _{\rm{s}}}}{c_{{\theta _{\rm{4}}}}}){\rm{ - }}{r_{\rm{1}}}{c_{\gamma _{\rm{s}}}}{s_{\beta _{\rm{s}}}}{\rm{ - }}{r_{\rm{2}}}{c_{\gamma _{\rm{s}}}}{s_{\beta _{\rm{s}}}}} \\ {{r_{\rm{3}}}({c_{{\theta _{\rm{4}}}}}{s_{\gamma _{\rm{s}}}}{\rm{ + }}{c_{\gamma _{\rm{s}}}}{c_{{\theta _{\rm{3}}}}}{s_{{\theta _{\rm{4}}}}}){\rm{ + }}{r_{\rm{1}}}{s_{\gamma _{\rm{s}}}}{\rm{ + }}{r_{\rm{2}}}{s_{\gamma _{\rm{s}}}}} \\ {{r_{\rm{3}}}({s_{{\theta _{\rm{4}}}}}({s_{\beta _{\rm{s}}}}{s_{{\theta _{\rm{3}}}}}{\rm{ + }}{c_{\beta _{\rm{s}}}}{c_{{\theta _{\rm{3}}}}}{s_{\gamma _{\rm{s}}}}){\rm{ - }}{c_{\gamma _{\rm{s}}}}{c_{\beta _{\rm{s}}}}{c_{{\theta _{\rm{4}}}}}){\rm{ - }}{r_{\rm{1}}}{c_{\gamma _{\rm{s}}}}{c_{\beta _{\rm{s}}}}{\rm{ - }}{r_{\rm{2}}}{c_{\gamma _{\rm{s}}}}{c_{\beta _{\rm{s}}}}} \end{array}} \right].$

式中：s_j=sin j，c_j=cos j.

由于机构存在冗余自由度，末端手掌位置反解存在无数解，须给定一个输入参数值，才可以求出各个关节输入值的唯一解. 假设θ₃为给定值，由式（7）可以得到

(8) $\left. \begin{array}{l} {\beta _{\rm{s}}}{\rm{ = 2arctan}}\;\left( {\dfrac{{{\rm{ - }}{B_{\rm{2}}}{\rm{ - }}\sqrt {A_{\rm{2}}^{\rm{2}}{\rm{ + }}B_{\rm{2}}^{\rm{2}}{\rm{ - }}C_{\rm{2}}^{\rm{2}}} }}{{{A_{\rm{2}}}{\rm{ - }}{{\rm{C}}_2}}}} \right), \\ {\gamma _{\rm{s}}}{\rm{ = 2arctan}}\;\left( {\dfrac{{{B_{\rm{1}}}{\rm{ + }}\sqrt {B_{\rm{1}}^{\rm{2}}{\rm{ - 4}}{A_{\rm{1}}}{C_{\rm{1}}}} }}{{{\rm{2}}{A_{\rm{1}}}}}} \right). \\ \end{array} \right\}$

式中：

$ \left. \begin{array}{l} {A_{\rm{1}}}{\rm{ = }} - {p_{{\rm{w\_}}y}} - {r_{\rm{3}}}{c_{{\theta _{\rm{3}}}}}{c_{{\theta _{\rm{4}}}}} ,\\ {B_{\rm{1}}}{\rm{ = }} - {\rm{2}}{{{r}}_{\rm{1}}} - {\rm{2}}{{{r}}_{\rm{2}}} - {\rm{2}}{{{r}}_{\rm{3}}}{c_{{\theta _{\rm{4}}}}} ,\\ {C_{\rm{1}}}{\rm{ = }} - {p_{{\rm{w\_}}y}} + {r_{\rm{3}}}{c_{{\theta _{\rm{3}}}}}{c_{{\theta _{\rm{4}}}}} ,\\ \end{array}\!\!\! \right\}\!\!\!\! \;\;\left. \begin{array}{l} {A_{\rm{2}}}{\rm{ = }}{r_{\rm{3}}}{s_{{\theta _{\rm{3}}}}}{s_{{\theta _{\rm{4}}}}} ,\\ {B_{\rm{2}}}{\rm{ = }}{r_{\rm{3}}}{c_{{\theta _{\rm{3}}}}}{s_{{\theta _{\rm{4}}}}}{{\rm{s}}_{{\gamma _{\rm s}}}} - {r_{\rm{3}}}{c_{{\theta _{\rm{4}}}}}{{{c}}_{\gamma _{\rm{s}}}} \\ \;\;\;\;\;\; {\rm{ }} - {r_{\rm{1}}}{{{c}}_{\gamma _{\rm{s}}}} - {r_{\rm{2}}}{{{c}}_{\gamma _{\rm{s}}}}, \\ {C_{\rm{2}}}{\rm{ = }}{p_{{\rm{w}}\_x}}. \\ \end{array} \!\!\!\right\} $

将肩关节动平台输出角度 ${\beta _{\rm{s}}}$和 ${\gamma _{\rm{s}}}$代入肩关节位置反解^[18]，即可求出肩关节驱动器的输入角度.

腕关节静坐标系相对于基坐标系的姿态矩阵 ${}^{\rm{0}}{{R}}_{\rm{w}}^{'} = {{{R}}_{\rm{s}}}{{{R}}_{\rm{m}}}{{{R}}_{\rm{e}}}{{{I}}_{\rm{1}}}$，腕关节动坐标系相对于基坐标系的姿态矩阵 ${}^{\rm{0}}{{{R}}_{\rm{w}}} = {}^{\rm{0}}{{R}}_{\rm{w}}^{'} {{{R}}_{\rm{w}}}$. 则有

(9) ${{{R}}_{\rm{w}}} = {}^{\rm{0}}{{{R}}_{\rm{w}}}{}^{\rm{0}}{{R}}{_{\rm{w}}^{'{\rm{ - 1}}}} = {}^{\rm{0}}{{{R}}_{\rm{w}}}{}^{\rm{0}}{{R}}{_{\rm{w}}^{'{\rm{T}}}} = {{W}}.$

根据式（9）两边对应元素相等，可以求得腕关节动平台的输出角度 ${\alpha _{\rm{w}}}$、 ${\beta _{\rm{w}}}$、 ${\gamma _{\rm{w}}}$，将其代入腕关节位置反解^[19]，即可求得腕关节驱动器的输入位移.

欧拉角速度并非机构动平台相对于静坐标系的角速度，肩关节动平台的欧拉角速度转换成笛卡尔角速度矢量为

(10) ${{\bf{\omega }}_{\rm{s}}} = \left[ {\begin{array}{*{20}{c}} {{c_{\beta _{\rm{s}}}}}&{\rm{0}} \\ {\rm{0}}&{\rm{1}} \\ {{\rm{ - }}{s_{\beta _{\rm{s}}}}}&{\rm{0}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{{\dot \gamma }_{\rm{s}}}} \\ {{{\dot \beta }_{\rm{s}}}} \end{array}} \right] = {{{G}}_{\rm{s}}}\left[ {\begin{array}{*{20}{c}} {{{\dot \gamma }_{\rm{s}}}} \\ {{{\dot \beta }_{\rm{s}}}} \end{array}} \right].$

肩关节动平台质心的角速度 $^{\rm{s}}{{{\omega }}_{\rm{s}}} $、角加速度 ${\rm{ }}{}^{\rm{s}}{{{\varepsilon }}_{\rm{s}}} $和线加速度 $^{\rm{s}}{{{a}}_{{\rm{sc}}}} $在肩关节静坐标下分别为

(11) $\left. \begin{array}{l} {}^{\rm{s}}{{{\omega }}_{\rm{s}}} = {{{\omega }}_{\rm{s}}},{\rm{ }}{}^{\rm{s}}{{{\varepsilon }}_{\rm{s}}} = {{\dot{ \omega }}_{\rm{s}}},{\rm{ }}{}^{\rm{s}}{{{a}}_{\rm{s}}} = {}^{\rm{0}}{{\dot{ v}}_{\rm{0}}} ,\\ {}^{\rm{s}}{{{a}}_{{\rm{sc}}}} = {}^{\rm{s}}{{{\varepsilon }}_{\rm{s}}} \times {{{C}}_{\rm{s}}} + {}^{\rm{s}}{{{\omega }}_{\rm{s}}} \times ({}^{\rm{s}}{{{\omega }}_{\rm{s}}} \times {{{C}}_{\rm{s}}}) + {}^{\rm{s}}{{{a}}_{\rm{s}}} .\\ \end{array} \right\}$

式中： ${}^{\rm{s}}{{{a}}_{\rm{s}}}$为基座的线加速度； ${{{C}}_{\rm{s}}}$为肩关节动平台质心的位置矢量； ${{{C}}_{\rm{s}}}={{R}}_{\rm s} \cdot [0,0,d_{\rm{s}}]^{\rm{T}}$，d_s为动平台质心到坐标原点的距离； $ {^0{{v}}_{\rm{0}}}=[0,0,-g]^{\rm{T}}$， $ ^0{\dot{ v}}_0$为考虑重力时基座的线加速度.

上臂杆质心的角速度 $^{\rm{m}}{{{\omega }}_{\rm{m}}} $、角加速度 $^{\rm{m}}{{{\varepsilon }}_{\rm{m}}} $和线加速度 $^{\rm{m}}{{{a}}_{{\rm{mc}}}} $在上臂动坐标系下分别为

(12) $\left. \begin{aligned} &{}^{\rm{m}}{{{\omega }}_{\rm{m}}} = {}_{\rm{s}}^{\rm{m}}{{R}}{}^{\rm{s}}{{{\omega }}_{\rm{s}}} + {{\dot \theta }_{\rm{3}}}{[ {\rm{0}},\;{\rm{0}},\;{\rm{1}} ]^{\rm{T}}}, \\ &{}^{\rm{m}}{{{\varepsilon }}_{\rm{m}}} = {}_{\rm{s}}^{\rm{m}}{{R}}{}^{\rm{s}}{{{\varepsilon }}_{\rm{s}}} + {}_{\rm{s}}^{\rm{m}}{{R}}{}^{\rm{s}}{{{\omega }}_{\rm{s}}} \times {{\dot \theta }_{\rm{3}}}{[ {\rm{0}},\;{\rm{0}},\;{\rm{1}} ]^{\rm{T}}} + \\ &\quad{{\ddot \theta }_{\rm{3}}}{[ {\rm{0}},\;{\rm{0}},\;{\rm{1}} ]^{\rm{T}}} ,\\ &{}^{\rm{m}}{{{a}}_{\rm{m}}} = {}_{\rm{s}}^{\rm{m}}{{R}}[{}^{\rm{s}}{{{\varepsilon }}_{\rm{s}}} \times {}^{\rm{s}}{{{p}}_{\rm{m}}} + {}^{\rm{s}}{{{\omega }}_{\rm{s}}} \times ({}^{\rm{s}}{{{\omega }}_{\rm{s}}} \times {}^{\rm{s}}{{{p}}_{\rm{m}}}) + {}^{\rm{s}}{{{a}}_{\rm{s}}}] ,\\ &{}^{\rm{m}}{{{a}}_{{\rm{mc}}}} = {}^{\rm{m}}{{{\varepsilon }}_{\rm{m}}} \times {{{p}}_{{\rm{mc}}}} + {}^{\rm{m}}{{{\omega }}_{\rm{m}}} \times ({}^{\rm{m}}{{{\omega }}_{\rm{m}}} \times {{{p}}_{{\rm{mc}}}}) + {}^{\rm{m}}{{{a}}_{\rm{m}}} .\\ \end{aligned} \right\}$

式中： ${}^{\rm{m}}{{{a}}_{\rm{m}}}$为上臂杆与肩关节连接处点D的线加速度； ${}_{\rm{s}}^{\rm{m}}{{R}} = {({{{R}}_{\rm{s}}}{{{R}}_{\rm{m}}})^{\rm{T}}}$； ${{{p}}_{{\rm{mc}}}} = {[{\rm{0}},\;{\rm{0}},\;{ - {d_{\rm{m}}}} ]^{\rm{T}}}$， ${d_{\rm{m}}}$为上臂质心到坐标原点的距离； ${}^{\rm{s}}{{{p}}_{\rm{m}}}$为上臂坐标原点在肩关节静坐标系下的位置矢量， ${}^{\rm{s}}{{{p}}_{\rm{m}}} = {{{R}}_{\rm{s}}}{{{p}}_{\rm{m}}}$.

前臂杆质心的角速度 $^{\rm{e}}{{{\omega }}_{\rm{e}}} $、角加速度 $^{\rm{e}}{{{\varepsilon }}_{\rm{e}}} $和线加速度 $^{\rm{e}}{{{a}}_{{\rm{ec}}}} $在前臂动坐标系下分别为

(13) $\left. \begin{array}{l} {}^{\rm{e}}{{{\omega }}_{\rm{e}}} = {}_{\rm{m}}^{\rm{e}}{{R}}{}^{\rm{m}}{{{\omega }}_{\rm{m}}} + {{\dot \theta }_{\rm{4}}}{[ {\rm{0}},\;{\rm{0}},\;{\rm{1}} ]^{\rm{T}}} ,\\ {}^{\rm{e}}{{{\varepsilon }}_{\rm{e}}} = {}_{\rm{m}}^{\rm{e}}{{R}}{}^{\rm{m}}{{{\varepsilon }}_{\rm{m}}} + {}_{\rm{m}}^{\rm{e}}{{R}}{}^{\rm{m}}{{{\omega }}_{\rm{m}}} \times {{\dot \theta }_{\rm{4}}}{[ {\rm{0}},\;{\rm{0}},\;{\rm{1}} ]^{\rm{T}}} + \\ \quad{{\ddot \theta }_{\rm{4}}}{[ {\rm{0}},\;{\rm{0}},\;{\rm{1}} ]^{\rm{T}}} ,\\ {}^{\rm{e}}{{{a}}_{\rm{e}}} = {}_{\rm{s}}^{\rm{m}}{{R}}[{}^{\rm{m}}{{{\varepsilon }}_{\rm{m}}} \times {{{p}}_{\rm{e}}} + {}^{\rm{m}}{{{\omega }}_{\rm{m}}} \times ({}^{\rm{m}}{{{\omega }}_{\rm{m}}} \times {{{p}}_{\rm{e}}}) + {}^{\rm{m}}{{{a}}_{\rm{m}}}] ,\\ {}^{\rm{e}}{{{a}}_{{\rm{ec}}}} = {}^{\rm{e}}{{{\varepsilon }}_{\rm{e}}} \times {{{p}}_{{\rm{ec}}}} + {}^{\rm{e}}{{{\omega }}_{\rm{e}}} \times ({}^{\rm{e}}{{{\omega }}_{\rm{e}}} \times {{{p}}_{{\rm{ec}}}}) + {}^{\rm{e}}{{{a}}_{\rm{e}}} .\\ \end{array} \right\}$

式中： ${}^{\rm{e}}{{{a}}_{\rm{e}}}$为前臂杆与上臂杆连接处点E的线加速度； ${}_{\rm{m}}^{\rm{e}}{{R}} = {{R}}_{\rm{e}}^{\rm{T}}$； ${{{p}}_{{\rm{ec}}}} = {[ {\rm{0}},\;{{d_{\rm{e}}}},\;{\rm{0}} ]^{\rm{T}}}$，d_e为前臂质心到坐标原点的距离.

腕关节动平台的欧拉角速度转换成笛卡尔角速度矢量为

(14) ${{\bf{\omega }}_{\rm{w}}} = \left[ {\begin{array}{*{20}{c}} {{c_{\alpha \_{\rm{w}}}}{c_{\beta \_{\rm{w}}}}}&{{\rm{ - }}{s_{\alpha \_{\rm{w}}}}}&{\rm{0}} \\ {{s_{\alpha \_{\rm{w}}}}{c_{\beta \_{\rm{w}}}}}&{{c_{\alpha \_{\rm{w}}}}}&{\rm{0}} \\ {{\rm{ - }}{s_{\beta \_{\rm{w}}}}}&{\rm{0}}&{\rm{1}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{{\dot \gamma }_{\rm{w}}}} \\ {{{\dot \beta }_{\rm{w}}}} \\ {{{\dot \alpha }_{\rm{w}}}} \end{array}} \right] = {{{G}}_{\rm{w}}}\left[ {\begin{array}{*{20}{c}} {{{\dot \gamma }_{\rm{w}}}} \\ {{{\dot \beta }_{\rm{w}}}} \\ {{{\dot \alpha }_{\rm{w}}}} \end{array}} \right].$

腕关节动平台的角速度 $^{\rm{w}}{{{\omega }}_{\rm{w}}} $、角加速度 $^{\rm{w}}{{{\varepsilon }}_{\rm{w}}} $和线加速度 $^{\rm{w}}{{{a}}_{{\rm{wc}}}} $在腕关节静坐标系下分别为

(15) $\left. \begin{array}{l} {}^{\rm{w}}{{{\omega }}_{\rm{w}}} = {}_{\rm{e}}^{\rm{w}}{{R}}{}^{\rm{e}}{{{\omega }}_{\rm{e}}} + {{{\omega }}_{\rm{w}}} ,\\ {}^{\rm{w}}{{{\varepsilon }}_{\rm{w}}} = {}_{\rm{e}}^{\rm{w}}{{R}}{}^{\rm{e}}{{{\varepsilon }}_{\rm{e}}} + {}_{\rm{e}}^{\rm{w}}{{R}}{}^{\rm{e}}{{{\omega }}_{\rm{e}}} \times {{{\omega }}_{\rm{w}}} + {{\dot{ \omega }}_{\rm{w}}} ,\\ {}^{\rm{w}}{{{a}}_{\rm{w}}} = {}_{\rm{e}}^{\rm{w}}{{R}}[{}^{\rm{e}}{{{\varepsilon }}_{\rm{e}}} \times {{{p}}_{\rm{w}}} + {}^{\rm{e}}{{{\omega }}_{\rm{e}}} \times ({}^{\rm{e}}{{{\omega }}_{\rm{e}}} \times {{{p}}_{\rm{w}}}) + {}^{\rm{e}}{{{a}}_{\rm{e}}}] ,\\ {}^{\rm{w}}{{{a}}_{{\rm{wc}}}} = {}^{\rm{w}}{{{\varepsilon }}_{\rm{w}}} \times {{{p}}_{{\rm{wc}}}} + {}^{\rm{w}}{{{\omega }}_{\rm{w}}} \times ({}^{\rm{w}}{{{\omega }}_{\rm{w}}} \times {{{p}}_{{\rm{wc}}}}) + {}^{\rm{w}}{{{a}}_{\rm{w}}} .\\ \end{array} \right\}$

式中： ${}^{\rm{w}}{{{a}}_{\rm{w}}}$为腕关节定平台的线加速度； $_{\rm{e}}^{\rm{w}}{{R}}={{I}}_1^{\rm{T}}$； ${{{p}}_{{\rm{wc}}}}$为腕关节动平台质心在腕关节静坐标系下的位置矢量， ${{{p}}_{{\rm{wc}}}} = {[{\rm{0,\;0,\;0}}]^{\rm{T}}}$.

腕关节UPS支链由胡克铰、缸体、推杆和球铰构成，缸体质心在腕关节静坐标系下的位置矢量 $^{\rm{w}}{{{l}}_{{\rm{u}}i}} $、角速度 $^{\rm{w}}{{{\omega }}_{{\rm{u}}i}} $、角加速度 $^{\rm{w}}{{{\varepsilon }}_{{\rm{u}}i}} $和线加速度 $^{\rm{w}}{{{a}}_{{\rm{u}}i}} $分别为

(16) $\left. \begin{aligned} &{}^{\rm{w}}{{{l}}_{{\rm{u}}i}} = {l_{\rm a}}{{{n}}_i}{\rm{ }},{\rm{ }}{{{\omega }}_{{\rm{u}}i}} = {{{H}}_{{\rm{w}}i}}{{\omega} _{\rm{w}}}, \\ &{}^{\rm{w}}{{{\omega }}_{{\rm{u}}i}} = {}_{\rm{e}}^{\rm{w}}{{R}}{}^{\rm{e}}{{{\omega }}_{\rm{e}}} + {{{\omega }}_{{\rm{u}}i}},{\rm{ }}{{\dot{ \omega }}_{{\rm{u}}i}} = ({{{n}}_i} \times {{{a}}_{{{S}}i}} - {\rm{2}}{{\dot l}_i}{{{\omega }}_{{\rm{u}}i}})/{l_i}, \\ &{}^{\rm{w}}{{{\varepsilon }}_{{\rm{u}}i}} = {}_{\rm{e}}^{\rm{w}}{{R}}{}^{\rm{e}}{{{\varepsilon }}_{\rm{e}}} + {}_{\rm{e}}^{\rm{w}}{{R}}{}^{\rm{e}}{{{\omega }}_{\rm{e}}} \times {{{\omega }}_{{\rm{u}}i}} + {{\dot{ \omega }}_{{\rm{u}}i}} ,\\ &{{{a}}_{{\rm{u}}i}} = {}_{\rm{e}}^{\rm{w}}{{R}}[{}^{\rm{e}}{{{\varepsilon }}_{\rm{e}}} \times {}^{\rm{e}}{{{U}}_i} + {}^{\rm{e}}{{{\omega }}_{\rm{e}}} \times ({}^{\rm{e}}{{{\omega }}_{\rm{e}}} \times {}^{\rm{e}}{{{U}}_i}) + {}^{\rm{e}}{{{a}}_{\rm{e}}}] ,\\ &{}^{\rm{w}}{{{a}}_{{\rm{u}}i}} = {}^{\rm{w}}{{{\varepsilon }}_{{\rm{u}}i}} \times {}^{\rm{w}}{{{l}}_{{\rm{u}}i}} + {}^{\rm{w}}{{{\omega }}_{{\rm{u}}i}} \times ({}^{\rm{w}}{{{\omega }}_{{\rm{u}}i}} \times {}^{\rm{w}}{{{l}}_{{\rm{u}}i}}) + {{{a}}_{{\rm{u}}i}} .\\ \end{aligned} \right\}$

式中：i表示腕关节各支链，i=1,2,3； ${{{\omega }}_{{\rm{u}}i}}$、 ${{\dot{ \omega }}_{{\rm{u}}i}}$、 ${{{a}}_{{\rm{u}}i}}$为腕关节动平台传递到缸体的角速度、角加速度和线加速度； ${{{H}}_{{\rm{w}}i}} = ({{n}}_i^{\rm{T}}{{{S}}_i}{{{E}}_{\rm{3}}} - {{{S}}_i}{{n}}_i^{\rm{T}})/{l_i},$ $ {{n}}_i$为支链UPS的单位方向矢量， $ {{S}}_i$为球铰中心点的位置坐标，E₃为3阶单位矩阵，l_i为球铰和胡克铰中心的距离；l_a为缸体质心到胡克铰中心的距离； ${{{l}}_{{\rm{u}}i}}$为缸体质心的位置矢量； $^{\rm{e}}{{{U}}_{i}}$为胡克铰中心在前臂坐标系下的位置矢量.

推杆质心在基坐标系下的位置矢量和线加速度为

(17) $\left. \begin{array}{l} {}^{\rm{w}}{{{l}}_{{\rm{l}}i}} = \left( {{l_{{i}}} - {l_{\rm{b}}}} \right){{{n}}_i} ,\\ {}^{\rm{w}}{{{a}}_{{\rm{l}}i}} = {}^{\rm{w}}{{{\omega }}_{{\rm{u}}i}} \times \left( {{}^{\rm{w}}{{{\omega }}_{{\rm{u}}i}} \times ({}^{\rm{w}}{{{l}}_{{\rm{l}}i}} - {}^{\rm{w}}{{{l}}_{{\rm{u}}i}})} \right) + {}^{\rm{w}}{{{a}}_{{\rm{u}}i}} + \\ \;\;\;\;\;\;\;\;\;\;{{\ddot l}_i}{{{n}}_i} + {}^{\rm{w}}{{{\varepsilon }}_{{\rm{u}}i}} \times ({}^{\rm{w}}{{{l}}_{{\rm{l}}i}} - {}^{\rm{w}}{{{l}}_{{\rm{u}}i}}) + {\rm{2}}{{\dot l}_i}{}^{\rm{w}}{{{\omega }}_{{\rm{u}}i}} \times {{{n}}_i} .\\ \end{array} \right\}$

式中：l_b为推杆质心到球铰中心的距离， $ {\dot l_i}$为UPS支链的驱动线速度值， $ {\ddot l_i}$为支链的驱动线加速度值，l_li为推杆质心的位置矢量.

另外，有

(18) $\left. \begin{array}{l} [ {{{\dot l}_{\rm{1}}}},\;{{{\dot l}_{\rm{2}}}},\;{{{\dot l}_{\rm{3}}}} ] = {{J}}_{\rm w}^{{\rm{ - 1}}} {{{\omega }}_{\rm w}}, \\ {{\ddot l}_i} = {{{a}}_{Si}} {{{n}}_i} + l{}_i{{{\omega }}_{{\rm u}i}}{{{\omega }}_{{\rm u}i}}, \\ {{{a}}_{Si}} = {{\dot{ \omega }}_{\rm w}} \times {{{S}}_i} + {{{\omega }}_{\rm w}} \times ({{{\omega }}_{\rm w}} \times {{{S}}_i}). \\ \end{array} \right\}$

式中： ${{J}}_{\rm{w}}^{{\rm{ - 1}}} = {[\begin{array}{*{20}{c}} {{{{S}}_{\rm{1}}} \times {{{n}}_{\rm{1}}}},&{{{{S}}_{\rm{2}}} \times {{{n}}_{\rm{2}}}},&{{{{S}}_{\rm{3}}} \times {{{n}}_{\rm{3}}}} \end{array}]^{\rm{T}}}$为腕关节逆雅克比矩阵.

由式（3）可知机械臂手掌位置H与各驱动器输入的关系式 $ {{H}} = {{f}}\left( {{\theta }} \right)$，通过微分可以得到拟人机械臂手掌中心点速度与各关节驱动速度的关系表达式 $ {{V}} = {{J}}\left( {{\theta }} \right){\dot{ \theta }}$，V为机械臂手掌中心点H的速度， ${{V}} = {\left[ {{{v}},{{\omega }}} \right]^{\rm{T}}}$.

由于拟人机械臂的每个关节都会对手掌点H的运动产生影响，则手掌速度等于每个关节传递到手掌速度的叠加：

(19) $ {{V}} = {{{J}}_1}({{\theta }})\dot {\theta} + {{{J}}_2}({{\theta }})\dot {\theta} + {{{J}}_3}({{\theta }})\dot {\theta} + {{{J}}_4}({{\theta }})\dot {{\theta}} . $

拟人机械臂的雅克比矩阵可以表示为

(20) $ {{J}}({{\theta }}) = \left[ {{{{J}}_1}({{\theta }}),{{{J}}_2}({{\theta }}),{{{J}}_3}({{\theta }}),{{{J}}_4}({{\theta }})} \right]. $

式中： ${{J}}_i({{\theta }})\;(i=1,2,3,4)$为关节速度传递到手掌速度的关系矩阵.

肩关节速度传递到末端手掌速度^[20-21]表达式为

(21) $\left. \begin{split}\left[ {\begin{array}{*{20}{l}} {{}^{\rm{h}}{{{v}}_{\rm{s}}}} \\ {}^{\rm{h}}{{\omega }}_{\rm{s}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{}_{\rm{h}}^{\rm{s}}{{{R}}^{\rm{T}}}}&{{\rm{ - }}{}_{\rm{h}}^{\rm{s}}{{{R}}^{\rm{T}}}{{{S}}_{\rm{s}}}(p)} \\ {\bf{0}}&{{}_{\rm{h}}^{\rm{s}}{{{R}}^{\rm{T}}}} \end{array}} \right], \\ \left[ {\begin{array}{*{20}{c}} {\bf{0}} \\ {{{R}}_{\rm{s}}^{\rm{T}}{{{G}}_{\rm{s}}}{{{J}}_{\rm{s}}}\left[ {\begin{array}{*{20}{c}} {{{\dot \theta }_{\rm{1}}}} \\ {{{\dot \theta }_{\rm{2}}}} \end{array}} \right]} \end{array}} \right] = {{{J}}_{\rm{1}}}\left[ {\begin{array}{*{20}{l}} {{{\dot \theta }_{\rm{1}}}} \\ {{{\dot \theta }_{\rm{2}}}} \end{array}} \right].\end{split}\right\}$

式中： ${}_{\rm{h}}^{\rm{s}}{{R}}$为肩关节静坐标系相对手掌坐标系的旋转矩阵， ${{{S}}_{\rm{s}}}(p)$为 ${}^{\rm{s}}{{{P}}_{\rm{h}}}$的反对称矩阵， ${{{J}}_{\rm{s}}}$为肩关节的雅克比矩阵.

同样地，可以推导出上臂、前臂和腕关节的速度传递雅克比矩阵J₂、J₃、J₄，故机械臂速度反解表达式为

(22) $\left[ {\begin{array}{*{20}{c}} {{}^{\rm{h}}{{v}}} \\ {{}^{\rm{h}}{{\omega }}} \end{array}} \right] = \left[ { {{{{J}}_{\rm{1}}}},\;{{{{J}}_{\rm{2}}}},\;{{{{J}}_{\rm{3}}}} ,\;{{{{J}}_{\rm{4}}}} } \right]{{\theta }} = {{{J}}_{\rm{h}}}{{\theta }},$

(23) $ {{J}} = \left[ {\begin{array}{*{20}{c}} {_{\rm{h}}^{\rm{s}}{{R}}}&{{0}}\\ {{0}}&{_{\rm{h}}^{\rm{s}}{{R}}} \end{array}} \right]{{{J}}_{\rm{h}}}. $

式中：J为基坐标下的拟人机械臂的雅克比矩阵， ${{{J}}_{\rm{h}}} $为手掌坐标系下的雅克比矩阵.

在腕关节静坐标系下，由式（15）~（17）可知腕关节各杆件质心的速度和加速度，利用牛顿-欧拉法分析腕关节所需驱动力^[22].

前臂与腕关节定平台固连，这两部分的运动性能一致，因此，在计算前臂的质量和转动惯量时将两部分合并. 通过分析腕关节动平台和UPS支链的受力推导出前臂对腕关节的作用力^eF_we和力矩^eM_we，进而分析前臂受力情况，假设腕关节传递到前臂力的作用点在前臂与腕关节定平台连接点F处，腕关节受力如图3所示.

腕关节动平台和UPS支链的牛顿-欧拉方程在腕关节静坐标系下为

(24) ${}^{\rm{w}}{{{F}}_{{\rm{ew}}}} = {m_{{\rm{wm}}}}{}^{\rm{w}}{{{a}}_{\rm{w}}} + \sum\limits_{i = {\rm{1}}}^{\rm{3}} {\left( {{m_{\rm{l}}}{}^{\rm{w}}{{{a}}_{{\rm{l}}i}} + {m_{\rm{u}}}{}^{\rm{w}}{{{a}}_{{\rm{u}}i}}} \right)} - {}^{\rm{w}}{{{F}}_{\rm{ex}}},$

(25) $\begin{split} &\displaystyle\sum\limits_{i = {\rm{1}}}^{\rm{3}} {}^{\rm{w}}{{{{l}}_{{\rm{l}}i}} \times {m_{\rm{l}}}{}^{\rm{w}}{{{a}}_{{\rm l}i}} + } \displaystyle\sum\limits_{i = {\rm{1}}}^{\rm{3}} {}^{\rm{w}}{{{{l}}_{{\rm{u}}i}} \times {m_{\rm{u}}}{}^{\rm{w}}{{{a}}_{{\rm{u}}i}}} + {}^{\rm{w}}{{{p}}_{{}_F}} \times {}^{\rm{w}}{{{F}}_{{\rm{ew}}}} + {}^{\rm{w}}{{{p}}_{\rm{h}}} \times\\ &\quad{}^{\rm{w}}{{{F}}_{{\rm{ex}}}} + {}^{\rm{w}}{{{M}}_{{\rm{ew}}}} + {}^{\rm{w}}{{{M}}_{{\rm{ex}}}} = {}^{\rm{w}}{{{I}}_{\rm{w}}}{}^{\rm{w}} {{{\varepsilon }}_{\rm{w}}} + {}^{\rm{w}}{{{\omega }}_{\rm{w}}} \times \left( {{}^{\rm{w}}{{{I}}_{\rm{w}}} {}^{\rm{w}}{{{\omega }}_{\rm{w}}}} \right) + \\ &\quad\displaystyle\sum\limits_{i = {\rm{1}}}^{\rm{3}} {\left( {{}^{\rm{w}}{{{I}}_{{\rm l}i}} + {}^{\rm{w}}{{{I}}_{{\rm{u}}i}}} \right) {}^{\rm{w}} {{{\varepsilon }}_{{{\rm l}i}}} + {}^{\rm{w}}{{{\omega }}_{{\rm{u}}i}} \times [\left( {{}^{\rm{w}}{{{I}}_{{\rm l}i}} + {}^{\rm{w}}{{{I}}_{{\rm{u}}i}}} \right)} {}^{\rm{w}} {{{\omega }}_{{\rm u}i}}]. \end{split} $

式中： ${}^{\rm{w}}{{{p}}_{{}_F}}$为点F在腕关节静坐标的位置矢量， ${}^{\rm{w}}{{{p}}_{{}_F}} = {[\begin{array}{*{20}{c}} {\rm{0}},{\rm{0}},{ - {r_{{\rm{w3}}}}} \end{array}]^{\rm{T}}}$； ${m_{{\rm{wm}}}}$、m_l、m_u为腕关节动平台、推杆、缸体的质量； ${}^{\rm{w}}{{{I}}_{\rm{w}}}$、 ${}^{\rm{w}}{{{I}}_{{\rm{l}}i}}$、 ${}^{\rm{w}}{{{I}}_{{\rm{u}}i}}$为腕关节动平台、推杆和缸体相对腕关节静坐标的惯量矩阵； ${}^{\rm{w}}{{{F}}_{{\rm{ew}}}}$、 ${}^{\rm{w}}{{{M}}_{{\rm{ew}}}}$为腕关节静坐标下前臂对腕关节的作用力和力矩； ${}^{\rm{w}}{{{F}}_{{\rm{ex}}}} = {}_{\rm{s}}^{\rm{w}}{{R}} {{{F}}_{{\rm{ex}}}}$、 ${}^{\rm{w}}{{{M}}_{{\rm{ex}}}}$为腕关节静坐标下的外力和外力矩； ${}^{\rm w}{\varepsilon}_{{\rm l}i}={}^{\rm w}{\varepsilon}_{{\rm u}i} $.

当外力和外力矩已知时，由式（24）、（25）可以计算出 ${}^{\rm{w}}{{{F}}_{{\rm{ew}}}}$、 ${}^{\rm{w}}{{{M}}_{{\rm{ew}}}}$，腕关节对前臂的力和力矩在前臂动坐标系下为 ${}^{\rm{e}}{{{F}}_{{\rm{we}}}} = - {{{I}}_{\rm{1}}}{}^{\rm{w}}{{{F}}_{{\rm{ew}}}}$， ${}^{\rm{e}}{{{M}}_{{\rm{we}}}} =$ $ - {{{I}}_{\rm{1}}}{}^{\rm{w}}{{{M}}_{{\rm{ew}}}} $.

前臂受到腕关节、上臂及自身重力作用，其动力学方程为

(26) ${}^{\rm{e}}{{{F}}_{{\rm{me}}}} + {}^{\rm{e}}{{{F}}_{{\rm{we}}}} = {m_{\rm{e}}}{}^{\rm{e}}{{{a}}_{{\rm{ec}}}},$

(27) $\begin{array}{l} ({{{p}}_{\rm{e}}} - {{{p}}_{{\rm{ec}}}}) \times {}^{\rm{e}}{{{F}}_{{\rm{we}}}} - {{{p}}_{{\rm{ec}}}} \times {}^{\rm{e}}{{{F}}_{{\rm{me}}}} + {}^{\rm{e}}{{{M}}_{{\rm{we}}}} + {}^{\rm{e}}{{{M}}_{{\rm{me}}}}= \\ \;\;\;\;\;\;{\rm{ }} {{{I}}_{\rm{e}}}{}^{\rm{e}}{{{\varepsilon }}_{\rm{e}}} +{}^{\rm{e}} {{{\omega }}_{\rm{e}}} \times \left( {{{{I}}_{\rm{e}}} {}^{\rm{e}}{{{\omega }}_{\rm{e}}}} \right) .\\ \end{array} $

式中：I_e为前臂在其动坐标下的转动惯量，^eF_me、^eM_me为前臂动坐标系下上臂对前臂的作用力和力矩， m_e为前臂的质量. 由式（26）、（27）可得 ${}^{\rm{e}}{{{M}}_{{\rm{me}}}}$， ${}^{\rm{e}}{{{M}}_{{\rm{me}}}}$投影在z_e轴等于前臂所需驱动力矩：

(28) ${\tau _{\rm{4}}} = {}^{\rm{e}}{{{M}}_{{\rm{me}}}} \cdot {[ {\rm{0}},\;{\rm{0}},\;{\rm{1}} ]^{\rm{T}}}.$

上臂对前臂的作用力和力矩在上臂动坐标系下为 ${}^{\rm{m}}{{{F}}_{{\rm{em}}}} \!=\! - {{{R}}_{\rm{e}}}{}^{\rm{e}}{{{F}}_{{\rm{me}}}}$， ${}^{\rm{m}}{{{M}}_{{\rm{em}}}} \!=\! - {{{R}}_{\rm{e}}}{}^{\rm{e}}{{{M}}_{{\rm{me}}}}$，上臂受到肩关节动平台、前臂及自身重力作用，其动力学方程为

(29) ${}^{\rm{m}}{{{F}}_{{\rm{sm}}}} + {}^{\rm{m}}{{{F}}_{{\rm{em}}}} = {m_{\rm{m}}}{}^{\rm{m}}{{{a}}_{{\rm{mc}}}},$

(30) $\begin{split} ({{{p}}_{\rm{m}}} - {{{p}}_{{\rm{mc}}}}){ \times ^{\rm{m}}}{{{F}}_{{\rm{em}}}} - {{{p}}_{{\rm{mc}}}} \times {}^{\rm{m}}{{{F}}_{{\rm{sm}}}} + {}^{\rm{m}}{{{M}}_{{\rm{em}}}} + {}^{\rm{m}}{{{M}}_{{\rm{sm}}}} \\ \;\; = {{{I}}_{\rm{m}}}{}^{\rm{m}}{{{\varepsilon }}_{\rm{m}}} + {}^{\rm{m}}{{{\omega }}_{\rm{m}}} \times \left( {{{{I}}_{\rm{m}}} {}^{\rm{m}}{{{\omega }}_{\rm{m}}}} \right) . \end{split} $

式中： ${{{I}}_{\rm{m}}}$为上臂相对其动坐标的惯量矩阵， ^mF_sm为上臂动坐标系下肩关节动平台对上臂的作用力， m_e为前臂的质量. 由式（29）、（30）求得肩关节动平台对前臂的力矩 ${}^{\rm{m}}{{{M}}_{{\rm{sm}}}}$， ${}^{\rm{m}}{{{M}}_{{\rm{sm}}}}$投影在z_m轴等于上臂所需驱动力矩：

(31) ${\tau _{\rm{3}}} = {}^{\rm{m}}{{{M}}_{{\rm{sm}}}} \cdot {[ {\rm{0}},\;{\rm{0}},\;{\rm{1}} ]^{\rm{T}}}.$

肩关节动平台受到上臂的作用力 ${}^{\rm{s}}{{{F}}_{{\rm{ms}}}}$和力矩 ${}^{\rm{s}}{{{M}}_{{\rm{ms}}}}$，受到杆A₂C₂作用力F_c1和力矩M_c1、杆B₁C₁的作用力F_c2和力矩M_c2以及自身的重力，如图4所示.

肩关节动平台动力学方程为

(32) ${{{F}}_{c1}} + {{{F}}_{c2}} + {}^{\rm{s}}{{{F}}_{{\rm{ms}}}} = {m_{\rm{s}}}{}^{\rm{s}}{{{a}}_{{\rm{sc}}}},$

(33) $\begin{array}{l} ({{D}} - {{{C}}_{\rm{s}}}) \times {}^{\rm{s}}{{{F}}_{{\rm{ms}}}} + ({{{C}}_{\rm{2}}} - {{{C}}_{\rm{s}}}) \times {{{F}}_{c{\rm{2}}}} + ({{{C}}_{\rm{1}}} - {{{C}}_{\rm{s}}}) \times {{{F}}_{c{\rm{1}}}}+ \\ \quad {}^{\rm{s}}{{{M}}_{{\rm{ms}}}} + {{{M}}_{c{\rm{1}}}} + {{{M}}_{c{\rm{2}}}} = {}^{\rm{s}}{{{I}}_{\rm{s}}}{}^{\rm{s}}{{{\varepsilon }}_{\rm{s}}} + {{{\omega }}_{\rm{s}}} \times \left( {{}^{\rm{s}}{{{I}}_{\rm{s}}} {}^{\rm{s}} {{{\omega }}_{\rm{s}}}} \right). \\[-10pt] \end{array} $

式中： ${}^{\rm{s}}{{{F}}_{{\rm{ms}}}} = - {{{R}}_{\rm{s}}}{{{R}}_{\rm{m}}}{}^{\rm{m}}{{{F}}_{{\rm{sm}}}}$， ${}^{\rm{s}}{{{M}}_{{\rm{ms}}}} = - {{{R}}_{\rm{s}}}{{{R}}_{\rm{m}}}{}^{\rm{m}}{{{M}}_{{\rm{sm}}}}$， ${}^{\rm{s}}{{{I}}_{\rm{s}}} = {{{R}}_{\rm{s}}}{{{I}}_{\rm{s}}}{{R}}_{\rm{s}}^{\rm{T}}$， ${m_{\rm{s}}}$为动平台质量， ${}^{\rm{s}}{{{I}}_{\rm{s}}}$为动平台相对肩关节静坐标系的惯量矩阵，D、C₁、C₂、 ${{{C}}_{\rm{s}}}$均为肩关节静坐标系下的位置坐标，F_c1、M_c1和F_c2、M_c2分别为杆B₁C₁和杆B₂C₂对肩关节动平台的作用力和力矩.

基于牛顿-欧拉法建立肩关节杆件力平衡方程，计算过程同上臂和前臂，不再赘述. 为了解决肩关节力平衡方程数少于未知数个数的问题，考虑杆件弹性，利用小变形理论建立变形协调方程^[23]，联立力平衡方程和变形协调方程建立肩关节动力学模型.

为了便于描述杆件变形，引入新的坐标系{B₁}，y轴沿OB₁方向，z轴垂直于OA₁B₁平面，x轴符合右手螺旋定则，相对于肩关节静坐标系的旋转矩阵 ${{{R}}_{{\rm{s1}}}} = [{{O}}{{{A}}_{\rm{1}}} \times {\rm{(}}{{O}}{{{B}}_{\rm{1}}} \times {{O}}{{{A}}_{\rm{1}}}{\rm{) }}{{O}}{{{A}}_{\rm{1}}}{\rm{ }}{{O}}{{{B}}_{\rm{1}}} \times $ ${{O}}{{{A}}_{\rm{1}}}] $.

由于杆件轴向拉伸和剪切变形相对于弯曲和扭转变形较小，因此忽略不计. 根据5R球面并联机构的结构特点，2条支链变形前后对应球心始终交于一点，采用积分法求解出2条支链弯曲和扭转组合变形引起的球心位移，从而建立补充方程.

杆件A₁B₁在A₁处存在6个约束，将其简化为固定端，在B₁处受到动平台的力F_b1和力矩M_b1，将其转化到坐标系{B₁}，如图5所示.

杆A₁B₁在任意断面θ处的力矩为

(34) $\left. \begin{aligned} &{M_{{\rm{1}}{\rm l}}} = {F_{b{\rm{1}}y}}{r_{{\rm{s1}}}}\sin \;({\alpha _{\rm{1}}} - \theta ) -\\ &\quad\quad\;\;\;{F_{b{\rm{1}}x}}{r_{{\rm s}{\rm{1}}}}({\rm{1}} - \cos\;({\alpha _{\rm{1}}} - \theta )) + {M_{b{\rm{1}}z}}, \\ &{M_{{\rm{1}}{\rm l}w}} = - ({M_{b{\rm{1}}x}} + {F_{b{\rm{1}}z}}{r_{{\rm s}{\rm{1}}}})\sin \;({\alpha _{\rm{1}}} - \theta ), \\ &{M_{{\rm{1}}{\rm l}t}}={M_{b{\rm{1}}x}}\cos\;({\alpha_{\rm{1}}}-\theta)-\\ &\quad\quad\;\;\;{F_{b{\rm{1}}z}}{r_{{\rm s}{\rm{1}}}}({\rm{1}}-\cos\;({\alpha_{\rm{1}}}-\theta)). \end{aligned}\right\}\\$

式中：r_s1为杆件A₁B₁的半径，α₁为轴线OA₁和轴线OB₁之间的夹角，F_b1、M_b1在坐标系{B₁}的分量为[F_b1x, F_b1y, F_b1z]=R_s1F_b1，[M_b1x, M_b1y, M_b1z]=R_s1M_b1.

1）弯矩 ${M_{{\rm{1}}{\rm l}}}$引起平面弯曲变形，转角和挠度分别为

(35) $\left. \begin{array}{l} {\theta _{z{\rm{1}}}} = \int {\dfrac{{{M_{{\rm{1}}{\rm l}}}}}{{E{I_z}}}{\rm{d}}s + {C_{\rm{1}}}} ,\\ {v_{{\rm H}{\rm{1}}}} = \int {{\theta _{z{\rm{1}}}}{\rm{d}}s + {D_{\rm{1}}}}. \\ \end{array} \right\}$

式中：EI_z为抗弯刚度，E为材料弹性模量，I_z为横断面对中性轴的惯性矩，矩形横截面I_z=hb³/12；C₁、D₁为积分常数. 当θ=0时，θ_z1=0，可以得到积分常数C₁；当θ=0时，v_H1=0，可以得到积分常数D₁.

2）弯矩 ${M_{{\rm{1}}{\rm l}w}}$引起平面弯曲变形，转角和挠度分别为

(36) $\left. \begin{array}{l} {\theta _{y{\rm{1}}}} = \int {\dfrac{{{M_{{\rm{1}}{\rm l}w}}}}{{E{I_y}}}{\rm{d}}s + {C_{\rm{2}}}}, \\ {v_{{\rm V}{\rm{1}}}} = \int {{\theta _{y{\rm{1}}}}{\rm{d}}s + {D_{\rm{2}}}} .\\ \end{array} \right\}$

式中：I_y为横断面对中性轴的惯性矩，矩形横截面I_y=h³b/12；C₂、D₂为积分常数. 当θ=0时，θ_y1=0，可以得到积分常数C₂；当θ=0时，v_V1=0，可以得到积分常数D₂.

3）扭矩 ${M_{{\rm{1}}{\rm l}t}}$引起平面扭转变形，扭转角为

(37) ${\varphi _{\rm{1}}} = \int {\frac{{{M_{{\rm{1}}{\rm l}t}}}}{{G{I_t}}}} {\rm{d}}s.$

式中：GI_t为杆件的抗扭刚度，G为杆件的切变模量，I_t=βhb³；β为与h/b相关的系数.

当θ=α₁时，得到弯矩和扭矩组合变形产生的叠加位移，对应球心点的线位移在肩关节静坐标系的表达式为

(38) $\Delta {{{o}}_{{\rm{11}}}} = {{{R}}_{{\rm{s1}}}} \cdot {\left[ {\begin{array}{*{20}{c}} { - {r_{{\rm s}{\rm{1}}}}{\theta _{z{\rm{1}}}}},{{v_{H{\rm{1}}}}},{{r_{{\rm{s1}}}}{\varphi _{\rm{1}}} - {v_{{\rm V}{\rm{1}}}}} \end{array}} \right]^{\rm{T}}}.$

利用达朗贝尔原理在杆件A₁B₁质心处添加惯性力和重力，将动力学转化为静力学问题，如图6所示. 将惯性力和重力转化到坐标系{B₁}，分析过程与在端点处一致，最后求得当θ=α₁/2时，在肩关节静坐标系对应球心点的线位移 $ \Delta {{{o}}_{{\rm{12}}}}$.

杆B₁C₁通过旋转副与杆件A₁B₁连接，其下端有5个约束，可以简化为单旋转副，如图7所示. 引入新的坐标系{C₁}，y轴沿OC₁方向，z轴垂直于OB₁C₁平面，x轴符合右手螺旋定则，相对肩关节静坐标系的旋转矩阵为

${{{R}}_{{\rm{s2}}}} = [\overrightarrow{{O}{{A}_{\rm{1}}}} \times {\rm{(}}\overrightarrow{{O}{{C}_{\rm{1}}}} \times {\overrightarrow{{OB}_{\rm{1}}}}{\rm{) }}{\overrightarrow{{OA}_{\rm{1}}}}{\overrightarrow{{OC}_{\rm{1}}}} \times{\overrightarrow {{OB}_{\rm{1}}}}].$

杆B₁C₁在任意断面θ处的力矩为

(39) $\left. \begin{array}{l} {M_{{\rm{2}}{\rm l}}} = {F_{c{\rm{1}}x}}{r_{{\rm{s2}}}}(\cos\;({\alpha _{\rm{2}}} - \theta ) - {\rm{1}}) +\\ \quad\quad\;\;\;{F_{c{\rm{1}}y}}{r_{{\rm{s2}}}}\sin\; ({\alpha _{\rm{2}}} - \theta ) + {M_{c{\rm{1}}z}} ,\\ {M_{{\rm{2}}{\rm l}w}} = - ({M_{c{\rm{1}}x}} + {F_{c{\rm{1}}z}}{r_{{\rm{s2}}}})\sin \;({\alpha _{\rm{2}}} - \theta ) \equiv {\rm{0}}, \\ {M_{{\rm{2}}{\rm l}t}} \!=\! ({M_{c{\rm{1}}x}} \!+\! {F_{c{\rm{1}}z}}{r_{{\rm{s2}}}})\cos\;({\alpha _{\rm{2}}} \!-\! \theta ) \!-\! {F_{c{\rm{1}}z}}{r_{{\rm{s2}}}} \!=\! -\! {F_{c{\rm{1}}z}}{r_{{\rm{s2}}}}. \\ \end{array} \!\!\!\! \right\}$

利用4.3.1节的方法分析杆B₁C₁端点处力和力矩引起的变形，弯矩 ${M_{{\rm{2}}{\rm l}}}$引起平面的弯曲变形，其转角和挠度分别为θ_z2、v_H2，弯矩 ${M_{{\rm{2}}{\rm l}w}}$引起的平面弯曲变形，其转角和挠度均为0，扭矩 ${M_{{\rm{2}}{\rm l}t}}$引起的平面扭转变形，其扭转角为φ₂.

根据叠加原理可以得到对应球心点的线位移在肩关节静坐标系的表达式为

(40) $ \Delta {{{o}}_{13}} = {{{R}}_{\rm{s2}}} \cdot {\left[ { - {r_{\rm{s2}}}{\theta _{z1}}, {v_{{\rm H}1}}, {r_{\rm{s2}}}{\varphi _2}} \right]^{\rm{T}}}. $

利用4.3.1节的方法分析杆B₁C₁的惯性力和重力在肩关节静坐标系使球心点产生的线位移 $ \Delta {{{o}}_{14}}$.

支链A₁B₁C₁变形对应动平台球心的线位移为

(41) $ \Delta {{{o}}_1} = \Delta {{{o}}_{11}} + \Delta {{{o}}_{12}} + \Delta {{{o}}_{13}} + \Delta {{{o}}_{14}}. $

杆B₂C₂的分析过程与杆A₁B₁完全一致，在肩关节静坐标下的位置偏移量为 $\Delta {{{o}}_{2}}$.

考虑整个机构，动平台比各支链杆尺寸大得多，变形远小于杆件的变形，故假定动平台为刚体，每个支链对应上平台的球心O点，变形后仍交与一点 $ O'$，则每个支链对应的中心线位移量相等.

(42) $ \Delta {{{o}}_1} = \Delta {{{o}}_2}. $

根据变形协调分析得到3个补充方程，联立24个力平衡方程，可以得到肩关节各杆件间27个未知力.

肩关节的驱动力矩表达式为

(43) $\left. \begin{array}{l} {\tau _{\rm{1}}} = {{{M}}_{a{\rm{1}}}} \cdot {{O}}{{{A}}_{\rm{1}}}, \\ {\tau _{\rm{2}}} = {{{M}}_{a{\rm{2}}}} \cdot {{O}}{{{A}}_{\rm{2}}}.\\ \end{array} \right\}$

式中：M_a1为A₁处的力矩，M_a2为A₂处的力矩，OA₁、OA₂为单位方向向量.

牛顿-欧拉法构建动力学模型是消除各杆件间的内力，建立输入与输出之间的动力学关系，整理式（28）、（31）和（44），化简可得机械臂各关节的驱动力和力矩：

(44) $ {{\tau}} = {\left[ {\begin{array}{*{20}{l}} {{\tau _1}},{{\tau _2}},{{\tau _3}},{{\tau _4}},{{F_1}},{{F_2}},{{F_3}} \end{array}} \right]^{\rm{T}}}. $

式中：F₁、F₂、F₃为腕关节驱动力.

根据人体手臂基本尺寸给出拟人机械臂的长度参数，肩关节支链杆件的材料选用45号钢，其他部件的材料均选用铝合金，机械臂机构的惯量参数、质量参数和长度参数如表1、2、3所示. 表中，m_mc、m_ec、m_s1、m_s2分别为上臂、前臂、杆A₁B₁、杆B₁C₁的质量.

假设机械臂手掌点H处负载重物10 kg，则在腕关节处的外力 ${}^{\rm{w}}{{{F}}_{\rm{ex}}} = {}_{\rm{s}}^{\rm{w}}{{R}} \cdot {[\begin{array}{*{20}{c}} {\rm{0}},{\rm{0}},{{\rm{100}}} \end{array}]^{\rm{T}}}$，外力矩 ${}^{\rm{w}}{{{M}}_{\rm{ex}}} = \left( {{{{R}}_{\rm{w}}} {{{p}}_{\rm{h}}}} \right) \times {}^{\rm{w}}{{{F}}_{\rm{ex}}}$. 给定上臂驱动器角度的时间函数 ${\theta _3} = {\text{π}} {t^5}/729 - 5{\text{π}} {t^4}/486 + 5{\text{π}} {t^3}/243$. 机械臂手掌点H的初始位置 $ {{q}}_0=[0,0,-0.830]^{\rm{T}}$，终点位置 ${{q}}_{\rm f}=[-0.02,0.02,-0.77]^{\rm{T}}$，姿态保持不变，设定运动时间T=3 s. 机械臂末端的基本轨迹有直线、曲线和圆3种轨迹^[24].

当机械臂末端轨迹为直线时，采用三次多项式规划末端轨迹，在初始和终止时刻，末端速度均为零，其轨迹可由下列关系式确定：

(45) $\left. \begin{array}{l} {q_x}(t) = {a_{\rm{3}}}{t^{\rm{3}}} + {a_{\rm{2}}}{t^{\rm{2}}} + {a_{\rm{1}}}t + {a_{\rm{0}}} ,\\ {q_y}(t) = - {{\dot q}_x}(t), \\ {q_z}(t) = - {\rm{3}}{{\dot q}_x}(t) - {\rm{0}}{\rm{.83}} .\\ \end{array} \right\}$

式中，a₀、a₁、a₂、a₃为多项式系数.

当机械臂末端轨迹为抛物线时，其轨迹可由下列关系式确定：

(46) $ \left. \begin{aligned} q_x'(t) = - \frac{{{l_{0{\rm{f}}}}}}2 {\left(1 - \frac2{T}t\right)^2},\\ q_y'(t) = \frac{{{l_{0{\rm{f}}}}}}2\left(1 - \frac2{T}t\right),\\ q_z'(t) = 0,\\ [{q_x}(t),{q_y}(t),\;\;{q_z}(t),1] = {{A}} [q_x'(t),q_y'(t),\;\;q_z'(t),1]. \end{aligned} \right\}$

式中：l_0f为指定的起点与终点的距离，矩阵A为抛物线所在坐标系转换到基坐标系的齐次旋转矩阵.

当机械臂末端轨迹为四分之一圆形时，其轨迹可由下列关系式确定：

(47) $\left. \begin{array}{l} {q_x}(t) = {c_x} + r\Bigg[{{{u}}_x}\cos \;\Bigg(\dfrac{{\text{π}} }{{{{2T}}}}t\Bigg) + {k_x}\sin \;\Bigg(\dfrac{{\text{π}} }{{{{2T}}}}t\Bigg)\Bigg] ,\\ {q_y}(t) = {c_y} + r\Bigg[{{{u}}_y}\cos \;\Bigg(\dfrac{{\text{π}} }{{{{2T}}}}t\Bigg) + {k_y}\sin \;\Bigg(\dfrac{{\text{π}} }{{{{2T}}}}t\Bigg)\Bigg] ,\\ {q_z}(t) = {c_z} + r\Bigg[{{{u}}_z}\cos\; \Bigg(\dfrac{{\text{π}} }{{{{2T}}}}t\Bigg) + {k_z}\sin\; \Bigg(\dfrac{{\text{π}} }{{{{2T}}}}t\Bigg)\Bigg]. \\ \end{array} \right\}$

式中：c_x、c_y、c_z为空间圆的圆心坐标；r为圆的半径；假设圆所在平面的法向量为e，u为圆所在平面上的单位向量，且与e相互正交，k为与e和u都正交的单位向量.

基于机械臂运动学模型和末端轨迹，将表1、2、3中的数据代入式（45），利用MATLAB编程得到各关节驱动器的力矩/力曲线，并将机械臂的三维模型导入ADAMS软件中，设定材料属性和约束条件，仿真时间设定为3 s，进行仿真验证，得到3种轨迹下机械臂各驱动力矩/力曲线的理论值和仿真值，如图8所示. 图中，LT、LS为直线轨迹下的理论值和仿真值，PT、PS为抛物线轨迹下的理论值和仿真值，CT、CS为圆形轨迹下的理论值和仿真值. 统计图8中3种轨迹下机械臂各关节驱动力矩/力的最大误差结果，如表4所示. 可以看出，当机械臂末端轨迹为直线、抛物线和圆形时，各关节驱动力矩/力的最大误差分别为2.98%、2.85%、2.35%和2.31%. 理论值与仿真值基本一致，验证了动力学模型的正确性^[25-26].

提出新型七自由度冗余串并混联拟人机械臂机构，可以完成人体手臂的一系列运动，具有结构简单、运动灵活和运动惯性小的优点. 根据运动和力的传递特性，提出混联机构的动力学建模方法，采用矢量法分别在各关节坐标系下分析各关节杆件的速度、加速度和受力情况，基于牛顿-欧拉法建立机械臂的力平衡方程，并利用小变形叠加原理建立肩关节机构的变形协调补充方程，联立补充方程建立机械臂的动力学全解模型. 在末端轨迹为直线、抛物线和圆形的情况下，各关节力矩/力理论值与仿真值的最大偏差分别为2.98%、2.85%、2.35%和2.31%，验证了机械臂动力学模型的正确性. 该研究为此冗余混联拟人机械臂的控制策略奠定了理论基础.

本研究求解的机械臂动力学模型较为复杂，难以实现实时控制，后续将简化动力学模型以设计控制系统.