为在结构上减小机械臂的体积, 同时还减小电机噪音、辐射等不良因素对上肢功能障碍患者的影响, 本文提出了上肢康复机器人的中央驱动式传动结构。利用Denavit-Hartenberg (D-H)表示法对中央驱动式上肢康复机器人进行运动学建模, 得到了其正、逆运动学方程。使用SolidWorks三维建模软件进行三维建模, 并让其在预设康复轨迹下进行运动仿真实验, 得到了各关节的角度-时间曲线及机械臂手柄的位置-时间曲线。实验结果表明, 机械臂手柄能在设定的康复轨迹下运动, 验证了中央驱动式结构设计的合理性。同时, 由机械臂手柄位置信息通过逆运动学方程计算得到的各关节的角度-时间曲线与从实验中得到的曲线最大误差不到3°, 验证了所得运动学方程的有效性。
引用本文： 易金花, 喻洪流, 张颖, 胡鑫, 石萍. 中央驱动式上肢康复机器人运动学建模与分析. 生物医学工程学杂志, 2015, 32(6): 1196-1201. doi: 10.7507/1001-5515.20150212 复制
0 引言
据统计估算，我国40岁以上脑卒中患者高达1 036万人[1]，每年新发病人数在150万以上，在脑卒中众多的后遗症中，以偏瘫的发生率最高，而偏瘫的康复中又以上肢功能的康复最为困难。上肢功能的恢复对患者的日常生活能力影响较大[2]，是目前康复治疗中难于解决的问题之一。医学理论和临床医学证明，除手术、药物治疗和电刺激治疗外，科学的康复训练对脑卒中偏瘫患者的肢体功能康复起着无比重要的作用[3-4]。同时，尽早介入康复训练不仅能够有效维持关节活动度、防止关节挛缩，而且能够明显提高患者的最终康复程度[5]。
目前，机器人技术已广泛应用于康复领域中，不仅大大提高了康复训练的效率[6-8]，同时也降低了患者以及临床医护人员的负担。然而，现阶段国内外设计的具有三维空间训练功能的电动上肢康复训练机械装置，多是将驱动电机安装在需要运动的上肢关节处，如瑞士苏黎世大学研制的ARMIN和ARMEO公司最新研发的一款上肢康复机器ARMEO@POWER。这类设计不仅在结构上大大增加了机械臂的体积，造成了上肢功能障碍患者的心理压力，而且也增加了电机噪音[9-11]与电机辐射等因素对他们造成的不良影响。针对上述问题，本文设计了一款机械臂体积小巧、动力驱动系统集中设计，并能将电机噪音、辐射等不良影响进行屏蔽的三自由度中央驱动式上肢康复机器人。
1 结构设计
本文的整体结构设计方案是：通过轴套及直齿轮，将中央集中式的三种动力源互无干扰地平行传出，并通过连杆与锥齿轮进行动力的换向，最终将动力输出到上肢的肘关节与肩关节处，完成运动所需的肘关节一个自由度(曲/伸)和肩关节两个自由度(曲/伸、内收/外展)的结构设计。中央驱动式上肢康复机器人运动结构简图如图 1所示。
图1
中央驱动式上肢康复机器人结构简图
Figure1.
Structural diagram of the central driven robot
由图 1可以看到，运动所需的肘关节一自由度和肩关节两自由度运动动力均从动力输出系统中平行输出，在结构上大大减小了机械臂的体积。同时，将电机、减速箱等动力输出装置和控制系统都集成在设备底座内进行屏蔽处理，完成了对电机产生的噪音、辐射等不良因素的有效控制。
2 运动学建模
运动模型的建立是控制系统设计的基础，对于本文研究的中央驱动式上肢康复机器人，建立运动学模型主要是为了研究关节活动的角度与机械臂手柄的空间位置之间的关系，为设计出有效的康复训练模式以实现上肢功能障碍患者的上肢康复提供理论依据。
2.1 正运动学方程求解
中央驱动式上肢康复机器人属于关节式机器人，肩关节两自由度由两个转动关节实现，肘关节一个自由度由一个转动关节实现，这三个自由度能确定机械臂手柄在三维空间的任意位置。本文采用D-H表示法对机器人建模，首先为每个关节指定一个本地的参考坐标系，其次对每个关节指定一个z轴和x轴。在得到中央驱动式上肢康复机器人的连杆坐标系之后，根据一系列相邻坐标系之间的变换，从机器人基座开始，直至变换到机器人机械臂手柄。这种变换的关系可由四个其次变换来描述，此关系式表示为
| $\begin{gathered} {A_i}=Rot\left({z, {\theta _i}} \right)Trans\left({0, 0, {d_i}} \right)Trans\left({{a_{i-1}}, 0, 0} \right) \hfill \\ Rot\left({x, {a_{i-1}}} \right) \hfill \\ \end{gathered} $ |
其中θi为绕zi-1轴的旋转角，di为在zi-1轴上两条相邻公垂线之间的距离，ai-1为每一条公垂线的长度，αi-1为两相邻z轴之间的夹角。
展开上式得：
| $\begin{gathered} {A_i}=\left[\begin{gathered} c{\theta _i}\; \;\; -s{\theta _i}\; \;0\;\; 0 \hfill \\ s{\theta _i}\; \;\; \;\; c{\theta _i}\; \;0\;\; \;0 \hfill \\ \; 0\;\; \;\; \;\; \;0\;\; \;\; 1\;\; \;0 \hfill \\ \; 0\;\; \;\; \;\; \;0\;\; \;\; 0\;\; \;1 \hfill \\ \end{gathered} \right] \times \left[\begin{gathered} 1\;0\;0\;0 \hfill \\ 0\;1\;0\;0 \hfill \\ 0\;0\;1\;{d_i} \hfill \\ 0\;0\;0\;1 \hfill \\ \end{gathered} \right] \times \hfill \\ \left[\begin{gathered} 1\;0\;0\;{a_{i-1}} \hfill \\ 0\;1\;0\;0 \hfill \\ 0\;0\;1\;0 \hfill \\ 0\;0\;0\;1 \hfill \\ \end{gathered} \right] \times \left[\begin{gathered} 1\;\; \;\; \;0\;\; \;\; \;\; \;0\;\; \;\; \;\; 0 \hfill \\ 0\;c{\alpha _{i-1}}\; \;-s{\alpha _{i-1}}\; \;0 \hfill \\ 0\;s{\alpha _{i-1}}\; \;-c{\alpha _{i-1}}\; \;0 \hfill \\ 0\;\; \;\; 0\;\; \;\; \;\; \;\; 0\;\; \;\; \;\; 1 \hfill \\ \end{gathered} \right] \hfill \\=\left[\begin{gathered} c{\theta _i}\; \;\; -s{\theta _i}c{\alpha _{i-1}}\; \;s{\theta _i}s{\alpha _{i-1}}\; \;{\alpha _{i-1}}c{\theta _i} \hfill \\ s{\theta _i}\; \;\; \;\; c{\theta _i}c{\alpha _{i-1}}\; \;-c{\theta _i}s{\alpha _{i-1}}\; {\alpha _{i-1}}s{\theta _i} \hfill \\ \; 0\;\; \;\; \;\; \;s{\alpha _{i-1}}\; \;\; \;\; \;\; \;\; c{\alpha _{i-1}}\; \;\; \;\; \;\; {d_i} \hfill \\ \; 0\;\; \;\; \;\; \;0\;\; \;\; \;\; \;\; \;\; \;\; \;\; 0\;\; \;\; \;\; \;\; \;\; \;\; \;1 \hfill \\ \end{gathered} \right] \hfill \\ \end{gathered} $ |
得到机器人基座与机械臂手柄之间的总变换用T矩阵表示为
| $^0T={A_1}{A_2}{A_3}{A_4} \cdots {A_n}, $ |
其中n指关节数，对于一个三自由度的机器人而言，有三个A矩阵。
本文以中央驱动式上肢康复机器人的机械结构为研究对象，得到的连杆坐标系如图 2所示。得到的连杆和关节参数见表 1。
图2
中央驱动式上肢康复机器人的连杆坐标系
Figure2.
Coordinate system of central driven connecting rod upper limb rehabilitation robot
表1
中央驱动式上肢康复机器人连杆-关节参数表
Table1.
D-H parameter table
在图 2中，l表示关节之间的距离。
根据式(2)和表 1可求得各连杆变换矩阵如下：
| $ \begin{gathered} {A_1}=\left[\begin{gathered} c{\theta _1}\; \;\; \;0\;\; \;\; s{\theta _1}\; \;\; \;0 \hfill \\ s{\theta _1}\; \;\; \;0\;\; \;-c{\theta _1}\; \;0 \hfill \\ 0\;\; \;\; \;\; \;1\;\; \;\; \;\; 0\;\; \;\; \;{l_1} \hfill \\ 0\;\; \;\; \;\; \;0\;\; \;\; \;\; 0\;\; \;\; \;1 \hfill \\ \end{gathered} \right] \hfill \\ {A_2}=\left[\begin{gathered} c{\theta _2}\; \;-s{\theta _2}\; \;0\;\; \;{l_2}c{\theta _2} \hfill \\ s{\theta _1}\; \;\; c{\theta _2}\; \;\; \;0\;\; \;{l_2}s{\theta _2} \hfill \\ 0\;\; \;\; \;\; \;0\;\; \;\; \;\; 1\;\; \;\; \;0 \hfill \\ 0\;\; \;\; \;\; \;0\;\; \;\; \;\; 0\;\; \;\; \;1 \hfill \\ \end{gathered} \right] \hfill \\ {A_3}=\left[\begin{gathered} c{\theta _3}\; \;-s{\theta _3}\; \;0\;\; \;{l_3}c{\theta _3} \hfill \\ s{\theta _3}\; \;\; c{\theta _3}\; \;\; \;0\;\; \;{l_3}s{\theta _3} \hfill \\ 0\;\; \;\; \;\; \;0\;\; \;\; \;\; 1\;\; \;\; \;0 \hfill \\ 0\;\; \;\; \;\; \;0\;\; \;\; \;\; 0\;\; \;\; \;1 \hfill \\ \end{gathered} \right] \hfill \\ \end{gathered} $ |
于是，根据式(3)可求出中央驱动式上肢康复机器人的T变换矩阵，即：
| $\begin{gathered} ^0{T_3}={A_1}{A_2}{A_3}=\left[\begin{gathered} {n_x}\; \;{o_x}\; \;{a_x}\; \;{p_x} \hfill \\ {n_y}\; \;{o_y}\; \;{a_y}\; \;{p_y} \hfill \\ {n_z}\; \;{o_z}\; \;{a_z}\; \;{p_z} \hfill \\ 0\;\; \;\; 0\;\; \;\; 0\;\; \;\; \;1 \hfill \\ \end{gathered} \right] \hfill \\=\left[\begin{gathered} c{\theta _1}c{\theta _{23}}\; \;\; -c{\theta _1}s{\theta _{23}}\; \;\; s{\theta _1}\; \;\; {l_3}c{\theta _1}c{\theta _{23}} + {l_2}c{\theta _1}c{\theta _2} \hfill \\ s{\theta _1}c{\theta _{23}}\; \;\; -s{\theta _1}s{\theta _{23}}\; \;-c{\theta _1}\; \;\; {l_3}s{\theta _1}c{\theta _{23}} + {l_2}s{\theta _1}c{\theta _2} \hfill \\ \; \;\; \;s{\theta _{23}}\; \;\; \;\; \;\; \;c{\theta _{23}}\; \;\; \;\; \;\; 0\;\; \;\; \;\; {l_3}s{\theta _{23}} + {l_2}s{\theta _2} + {l_1} \hfill \\ \; \;\; \;\; \;0\;\; \;\; \;\; \;\; \;\; \;\; 0\;\; \;\; \;\; \;\; \;0\;\; \;\; \;\; \;\; \;\; \;\; \;\; \;\; 1 \hfill \\ \end{gathered} \right] \hfill \\ \end{gathered} $ |
进而得到中央驱动式上肢康复机器人的正运动学方程有：
| $\left. \begin{gathered} {n_x}=c{\theta _1}c{\theta _{23}} \hfill \\ {n_y}=s{\theta _1}c{\theta _{23}} \hfill \\ {n_z}=s{\theta _{23}} \hfill \\ {o_x}=-c{\theta _1}s{\theta _{23}} \hfill \\ {o_y}=-s{\theta _1}s{\theta _{23}} \hfill \\ {o_z}=c{\theta _{23}} \hfill \\ {a_x}=s{\theta _1} \hfill \\ {a_y}=-c{\theta _1} \hfill \\ {a_z}=0 \hfill \\ {p_x}={l_3}c{\theta _1}c{\theta _{23}} + {l_2}c{\theta _1}c{\theta _2} \hfill \\ {p_y}={l_3}s{\theta _1}{\theta _{23}} + {l_2}s{\theta _1}c{\theta _2} \hfill \\ {p_z}={l_3}s{\theta _{23}} + {l_2}s{\theta _2} + {l_1} \hfill \\ \end{gathered} \right\}$ |
根据计算出来的正运动学方程，在已知各关节运动角度的情况下，可以对机器人机械臂手柄的位置进行求解。
2.2 逆运动学方程求解
要根据机器人机械臂手柄的位置推导出各关节的运动角度，为求正运动学方程的逆解问题，求解过程如下：
根据式(5)中的10、11项，即：
| $\left\{ \begin{gathered} {p_x}={l_3}c{\theta _1}c{\theta _{23}} + {l_2}c{\theta _1}c{\theta _2} \hfill \\ {p_y}={l_3}s{\theta _1}c{\theta _{23}} + {l_2}s{\theta _1}c{\theta _2} \hfill \\ \end{gathered} \right.$ |
可以得到:θ1=arctan()和θ1=θ1±180°，使计算得到的角度值满足θ1。
根据式(5)中的10、11、12项，有：
| $\left\{ \begin{gathered} {p_x}={l_3}c{\theta _1}c{\theta _{23}} + {l_2}c{\theta _1}c{\theta _2} \hfill \\ {p_y}={l_3}s{\theta _1}c{\theta _{23}} + {l_2}s{\theta _1}c{\theta _2} \hfill \\ \end{gathered} \right.$ |
将式(7)等号的左、右分别平方相加得到：
| $\begin{gathered} p_x^2 + p_y^2 + {\left({p_z^2-{l_1}} \right)^2}={\left({{l_3}c{\theta _1}c{\theta _{23}} + {l_2}c{\theta _1}c{\theta _2}} \right)^2} + \hfill \\ {\left({{l_3}s{\theta _1}c{\theta _{23}} + {l_2}s{\theta _1}c{\theta _2}} \right)^2} + {\left({{l_3}s{\theta _{23}} + {l_2}s{\theta _2}} \right)^2} \hfill \\ \end{gathered} $ |
整理得到：
| $l_3^2 + l_2^2 + 2{l_2}{l_3}c{\theta _3}=p_x^2 + p_y^2 + {\left({p_z^2-{l_1}} \right)^2}$ |
即得:
| $ {\theta _3}=\arccos \left({\frac{{p_x^2 + p_y^2 + {{\left({p_z^2-{l_1}} \right)}^2}-l_3^2-l_2^2}}{{2{l_2}{l_3}}}} \right) $ |
将式(7)等号的左、右两边分别作[12]处理，得到：
| $\frac{{{p_x}-{l_1}}}{{\sqrt {p_x^2 + p_y^2} }}=\frac{{{l_3}s{\theta _{23}} + {l_2}s{\theta _2}}}{{{l_3}c{\theta _{23}} + {l_2}c{\theta _2}}}$ |
令，则：
| $s{\theta _2}\left({{l_3}c{\theta _3} + {l_2} + k{l_3}s{\theta _3}} \right)=c{\theta _2}\left({k{l_3}c{\theta _3}-{l_3}s{\theta _3} + k{l_2}} \right)$ |
即得：和θ2=θ2±180°，使计算得到的角度值满足。
通过以上计算，根据机器人机械臂手柄的位置，得到的三个关节的角度值分别为：
| $\left\{ \begin{gathered} {\theta _1}={\text{arctan}}\left({\frac{{{p_y}}}{{{p_x}}}} \right), 和{\theta _1}={\theta _1} + {180^ \circ } \hfill \\ {\theta _2}=\arctan \left({\frac{{k\left({{l_3}c{\theta _3} + {l_2}} \right)-{l_3}s{\theta _3}}}{{{l_3}c{\theta _3} + {l_2} + k{l_3}s{\theta _3}}}} \right), 和{\theta _2}={\theta _2} + {180^ \circ }, 其中k=\frac{{{p_z}-{l_1}}}{{\sqrt {p_x^2 + p_y^2} }}=k \hfill \\ {\theta _3}=\arccos \left({\frac{{p_x^2 + p_y^2 + {{\left({p_z^2-{l_1}} \right)}^2}-l_3^2-l_2^2}}{{2{l_2}{l_3}}}} \right) \hfill \\ \end{gathered} \right.$ |
方程组(12)给出了中央驱动式上肢康复机器人在任何期望位置时所需要的关节值。
3 运动学仿真分析
在上肢康复控制系统中，为使机器人机械臂手柄按照合理的康复运动轨迹运动，就要利用上文求得的逆运动学方程计算得到各关节的运动角度，进而控制驱动电机驱动各关节运动，实现上肢功能障碍患者的被动康复训练。
为验证结构设计的合理性以及逆运动方程的有效性，本文在SolidWorks建模软件下对中央驱动式上肢康复机器人进行三维建模，利用SolidWorks建模软件中的Motion模块[13-14]，对上肢康复机器人机械臂手柄进行预设三维康复运动轨迹的仿真实验。由于运动轨迹一定，可以直接得到运动过程中机器人机械臂手柄的位置信息，即各时刻的px、py、pz值，和3个关节的角度信号。预设三维康复运动轨迹的起始、中间和停止位置如图 3所示。
图3
康复训练运动轨迹三维图
(a)起始位置；(b)中间位置；(c)停止位置
Figure3. Three-dimensional maps of rehabilitation route(a) starting position; (b) middle position; (c) stop position
通过SolidWorks建模得到中央驱动式上肢康复机器人的模型，已知l1=306 mm, l2=452.36 mm, l3=374.95 mm。在机器人进行运动仿真时，根据各时刻机器人机械臂手柄的px、py、pz的位置大小和式(12)，得到各时刻θ1、θ2、θ3的大小。将求出的θ1、θ2、θ3大小与在运动仿真中测量得到的各关节角度大小进行比较，结果如图 4所示。
图4
计算得到的与测量得到的机器人各关节角度对比图
(a)
(a)
从图 4可以看到，θ1的运动范围在-120°~20°之间，满足的活动范围；θ2的运动范围在160°~175°之间，满足的活动范围；θ3的运动范围在5°~55°之间，满足的活动范围。由图 4(a)、(c)可以看出，通过逆运动学方程计算得到的θ1、θ3与通过SolidWorks直接得到的各关节角度基本相同，运动角度-时间曲线拟合度好。由式(13)可知，θ2是在θ3的基础上计算得到，因此会有累积误差，但从图 4(b)可以看出，θ2计算得到的角度值与测量得到的角度值最大误差不超过3°。
4 结论
(1)本文利用轴套及锥齿轮结构，设计了一款中央驱动式上肢康复机器人。不仅能减小机械臂的体积，还能减小电机噪音、辐射等不良因素对上肢功能障碍患者的影响。
(2)利用D-H表示法进行运动学建模，得到了中央驱动式上肢康复机器人的正逆运动学方程。在对机械臂手柄预设康复轨迹路线的情况下，完成了中央驱动式上肢康复机器人的运动仿真。实验得到的各关节角度值完全在设定范围之内，验证了中央驱动式上肢康复机器人结构设计的合理性。将通过逆运动学方程计算得到的各关节角度-时间曲线与仿真中测量得到的角度-时间曲线进行比较，误差不超过3°，验证了逆运动学方程的有效性。
(3)中央驱动式上肢康复机器人的正逆运动学方程与运动学仿真实验，为进一步控制中央驱动式上肢康复机器人提供了理论与实验依据。
(4)本文将在此研究的基础上，继续设计利用套筒结构实现大臂和前臂的长短可调来更好地满足不同患者需求。同时，为帮助上肢功能障碍患者更好地进行上肢康复，本文还将增加患者的手部训练，将通过设计腕关节2自由度(包含腕关节屈/伸运动和前臂的内旋/外旋运动)来实现。
0 引言
据统计估算，我国40岁以上脑卒中患者高达1 036万人[1]，每年新发病人数在150万以上，在脑卒中众多的后遗症中，以偏瘫的发生率最高，而偏瘫的康复中又以上肢功能的康复最为困难。上肢功能的恢复对患者的日常生活能力影响较大[2]，是目前康复治疗中难于解决的问题之一。医学理论和临床医学证明，除手术、药物治疗和电刺激治疗外，科学的康复训练对脑卒中偏瘫患者的肢体功能康复起着无比重要的作用[3-4]。同时，尽早介入康复训练不仅能够有效维持关节活动度、防止关节挛缩，而且能够明显提高患者的最终康复程度[5]。
目前，机器人技术已广泛应用于康复领域中，不仅大大提高了康复训练的效率[6-8]，同时也降低了患者以及临床医护人员的负担。然而，现阶段国内外设计的具有三维空间训练功能的电动上肢康复训练机械装置，多是将驱动电机安装在需要运动的上肢关节处，如瑞士苏黎世大学研制的ARMIN和ARMEO公司最新研发的一款上肢康复机器ARMEO@POWER。这类设计不仅在结构上大大增加了机械臂的体积，造成了上肢功能障碍患者的心理压力，而且也增加了电机噪音[9-11]与电机辐射等因素对他们造成的不良影响。针对上述问题，本文设计了一款机械臂体积小巧、动力驱动系统集中设计，并能将电机噪音、辐射等不良影响进行屏蔽的三自由度中央驱动式上肢康复机器人。
1 结构设计
本文的整体结构设计方案是：通过轴套及直齿轮，将中央集中式的三种动力源互无干扰地平行传出，并通过连杆与锥齿轮进行动力的换向，最终将动力输出到上肢的肘关节与肩关节处，完成运动所需的肘关节一个自由度(曲/伸)和肩关节两个自由度(曲/伸、内收/外展)的结构设计。中央驱动式上肢康复机器人运动结构简图如图 1所示。
图1
中央驱动式上肢康复机器人结构简图
Figure1.
Structural diagram of the central driven robot
由图 1可以看到，运动所需的肘关节一自由度和肩关节两自由度运动动力均从动力输出系统中平行输出，在结构上大大减小了机械臂的体积。同时，将电机、减速箱等动力输出装置和控制系统都集成在设备底座内进行屏蔽处理，完成了对电机产生的噪音、辐射等不良因素的有效控制。
2 运动学建模
运动模型的建立是控制系统设计的基础，对于本文研究的中央驱动式上肢康复机器人，建立运动学模型主要是为了研究关节活动的角度与机械臂手柄的空间位置之间的关系，为设计出有效的康复训练模式以实现上肢功能障碍患者的上肢康复提供理论依据。
2.1 正运动学方程求解
中央驱动式上肢康复机器人属于关节式机器人，肩关节两自由度由两个转动关节实现，肘关节一个自由度由一个转动关节实现，这三个自由度能确定机械臂手柄在三维空间的任意位置。本文采用D-H表示法对机器人建模，首先为每个关节指定一个本地的参考坐标系，其次对每个关节指定一个z轴和x轴。在得到中央驱动式上肢康复机器人的连杆坐标系之后，根据一系列相邻坐标系之间的变换，从机器人基座开始，直至变换到机器人机械臂手柄。这种变换的关系可由四个其次变换来描述，此关系式表示为
| $\begin{gathered} {A_i}=Rot\left({z, {\theta _i}} \right)Trans\left({0, 0, {d_i}} \right)Trans\left({{a_{i-1}}, 0, 0} \right) \hfill \\ Rot\left({x, {a_{i-1}}} \right) \hfill \\ \end{gathered} $ |
其中θi为绕zi-1轴的旋转角，di为在zi-1轴上两条相邻公垂线之间的距离，ai-1为每一条公垂线的长度，αi-1为两相邻z轴之间的夹角。
展开上式得：
| $\begin{gathered} {A_i}=\left[\begin{gathered} c{\theta _i}\; \;\; -s{\theta _i}\; \;0\;\; 0 \hfill \\ s{\theta _i}\; \;\; \;\; c{\theta _i}\; \;0\;\; \;0 \hfill \\ \; 0\;\; \;\; \;\; \;0\;\; \;\; 1\;\; \;0 \hfill \\ \; 0\;\; \;\; \;\; \;0\;\; \;\; 0\;\; \;1 \hfill \\ \end{gathered} \right] \times \left[\begin{gathered} 1\;0\;0\;0 \hfill \\ 0\;1\;0\;0 \hfill \\ 0\;0\;1\;{d_i} \hfill \\ 0\;0\;0\;1 \hfill \\ \end{gathered} \right] \times \hfill \\ \left[\begin{gathered} 1\;0\;0\;{a_{i-1}} \hfill \\ 0\;1\;0\;0 \hfill \\ 0\;0\;1\;0 \hfill \\ 0\;0\;0\;1 \hfill \\ \end{gathered} \right] \times \left[\begin{gathered} 1\;\; \;\; \;0\;\; \;\; \;\; \;0\;\; \;\; \;\; 0 \hfill \\ 0\;c{\alpha _{i-1}}\; \;-s{\alpha _{i-1}}\; \;0 \hfill \\ 0\;s{\alpha _{i-1}}\; \;-c{\alpha _{i-1}}\; \;0 \hfill \\ 0\;\; \;\; 0\;\; \;\; \;\; \;\; 0\;\; \;\; \;\; 1 \hfill \\ \end{gathered} \right] \hfill \\=\left[\begin{gathered} c{\theta _i}\; \;\; -s{\theta _i}c{\alpha _{i-1}}\; \;s{\theta _i}s{\alpha _{i-1}}\; \;{\alpha _{i-1}}c{\theta _i} \hfill \\ s{\theta _i}\; \;\; \;\; c{\theta _i}c{\alpha _{i-1}}\; \;-c{\theta _i}s{\alpha _{i-1}}\; {\alpha _{i-1}}s{\theta _i} \hfill \\ \; 0\;\; \;\; \;\; \;s{\alpha _{i-1}}\; \;\; \;\; \;\; \;\; c{\alpha _{i-1}}\; \;\; \;\; \;\; {d_i} \hfill \\ \; 0\;\; \;\; \;\; \;0\;\; \;\; \;\; \;\; \;\; \;\; \;\; 0\;\; \;\; \;\; \;\; \;\; \;\; \;1 \hfill \\ \end{gathered} \right] \hfill \\ \end{gathered} $ |
得到机器人基座与机械臂手柄之间的总变换用T矩阵表示为
| $^0T={A_1}{A_2}{A_3}{A_4} \cdots {A_n}, $ |
其中n指关节数，对于一个三自由度的机器人而言，有三个A矩阵。
本文以中央驱动式上肢康复机器人的机械结构为研究对象，得到的连杆坐标系如图 2所示。得到的连杆和关节参数见表 1。
图2
中央驱动式上肢康复机器人的连杆坐标系
Figure2.
Coordinate system of central driven connecting rod upper limb rehabilitation robot
表1
中央驱动式上肢康复机器人连杆-关节参数表
Table1.
D-H parameter table
在图 2中，l表示关节之间的距离。
根据式(2)和表 1可求得各连杆变换矩阵如下：
| $ \begin{gathered} {A_1}=\left[\begin{gathered} c{\theta _1}\; \;\; \;0\;\; \;\; s{\theta _1}\; \;\; \;0 \hfill \\ s{\theta _1}\; \;\; \;0\;\; \;-c{\theta _1}\; \;0 \hfill \\ 0\;\; \;\; \;\; \;1\;\; \;\; \;\; 0\;\; \;\; \;{l_1} \hfill \\ 0\;\; \;\; \;\; \;0\;\; \;\; \;\; 0\;\; \;\; \;1 \hfill \\ \end{gathered} \right] \hfill \\ {A_2}=\left[\begin{gathered} c{\theta _2}\; \;-s{\theta _2}\; \;0\;\; \;{l_2}c{\theta _2} \hfill \\ s{\theta _1}\; \;\; c{\theta _2}\; \;\; \;0\;\; \;{l_2}s{\theta _2} \hfill \\ 0\;\; \;\; \;\; \;0\;\; \;\; \;\; 1\;\; \;\; \;0 \hfill \\ 0\;\; \;\; \;\; \;0\;\; \;\; \;\; 0\;\; \;\; \;1 \hfill \\ \end{gathered} \right] \hfill \\ {A_3}=\left[\begin{gathered} c{\theta _3}\; \;-s{\theta _3}\; \;0\;\; \;{l_3}c{\theta _3} \hfill \\ s{\theta _3}\; \;\; c{\theta _3}\; \;\; \;0\;\; \;{l_3}s{\theta _3} \hfill \\ 0\;\; \;\; \;\; \;0\;\; \;\; \;\; 1\;\; \;\; \;0 \hfill \\ 0\;\; \;\; \;\; \;0\;\; \;\; \;\; 0\;\; \;\; \;1 \hfill \\ \end{gathered} \right] \hfill \\ \end{gathered} $ |
于是，根据式(3)可求出中央驱动式上肢康复机器人的T变换矩阵，即：
| $\begin{gathered} ^0{T_3}={A_1}{A_2}{A_3}=\left[\begin{gathered} {n_x}\; \;{o_x}\; \;{a_x}\; \;{p_x} \hfill \\ {n_y}\; \;{o_y}\; \;{a_y}\; \;{p_y} \hfill \\ {n_z}\; \;{o_z}\; \;{a_z}\; \;{p_z} \hfill \\ 0\;\; \;\; 0\;\; \;\; 0\;\; \;\; \;1 \hfill \\ \end{gathered} \right] \hfill \\=\left[\begin{gathered} c{\theta _1}c{\theta _{23}}\; \;\; -c{\theta _1}s{\theta _{23}}\; \;\; s{\theta _1}\; \;\; {l_3}c{\theta _1}c{\theta _{23}} + {l_2}c{\theta _1}c{\theta _2} \hfill \\ s{\theta _1}c{\theta _{23}}\; \;\; -s{\theta _1}s{\theta _{23}}\; \;-c{\theta _1}\; \;\; {l_3}s{\theta _1}c{\theta _{23}} + {l_2}s{\theta _1}c{\theta _2} \hfill \\ \; \;\; \;s{\theta _{23}}\; \;\; \;\; \;\; \;c{\theta _{23}}\; \;\; \;\; \;\; 0\;\; \;\; \;\; {l_3}s{\theta _{23}} + {l_2}s{\theta _2} + {l_1} \hfill \\ \; \;\; \;\; \;0\;\; \;\; \;\; \;\; \;\; \;\; 0\;\; \;\; \;\; \;\; \;0\;\; \;\; \;\; \;\; \;\; \;\; \;\; \;\; 1 \hfill \\ \end{gathered} \right] \hfill \\ \end{gathered} $ |
进而得到中央驱动式上肢康复机器人的正运动学方程有：
| $\left. \begin{gathered} {n_x}=c{\theta _1}c{\theta _{23}} \hfill \\ {n_y}=s{\theta _1}c{\theta _{23}} \hfill \\ {n_z}=s{\theta _{23}} \hfill \\ {o_x}=-c{\theta _1}s{\theta _{23}} \hfill \\ {o_y}=-s{\theta _1}s{\theta _{23}} \hfill \\ {o_z}=c{\theta _{23}} \hfill \\ {a_x}=s{\theta _1} \hfill \\ {a_y}=-c{\theta _1} \hfill \\ {a_z}=0 \hfill \\ {p_x}={l_3}c{\theta _1}c{\theta _{23}} + {l_2}c{\theta _1}c{\theta _2} \hfill \\ {p_y}={l_3}s{\theta _1}{\theta _{23}} + {l_2}s{\theta _1}c{\theta _2} \hfill \\ {p_z}={l_3}s{\theta _{23}} + {l_2}s{\theta _2} + {l_1} \hfill \\ \end{gathered} \right\}$ |
根据计算出来的正运动学方程，在已知各关节运动角度的情况下，可以对机器人机械臂手柄的位置进行求解。
2.2 逆运动学方程求解
要根据机器人机械臂手柄的位置推导出各关节的运动角度，为求正运动学方程的逆解问题，求解过程如下：
根据式(5)中的10、11项，即：
| $\left\{ \begin{gathered} {p_x}={l_3}c{\theta _1}c{\theta _{23}} + {l_2}c{\theta _1}c{\theta _2} \hfill \\ {p_y}={l_3}s{\theta _1}c{\theta _{23}} + {l_2}s{\theta _1}c{\theta _2} \hfill \\ \end{gathered} \right.$ |
可以得到:θ1=arctan()和θ1=θ1±180°，使计算得到的角度值满足θ1。
根据式(5)中的10、11、12项，有：
| $\left\{ \begin{gathered} {p_x}={l_3}c{\theta _1}c{\theta _{23}} + {l_2}c{\theta _1}c{\theta _2} \hfill \\ {p_y}={l_3}s{\theta _1}c{\theta _{23}} + {l_2}s{\theta _1}c{\theta _2} \hfill \\ \end{gathered} \right.$ |
将式(7)等号的左、右分别平方相加得到：
| $\begin{gathered} p_x^2 + p_y^2 + {\left({p_z^2-{l_1}} \right)^2}={\left({{l_3}c{\theta _1}c{\theta _{23}} + {l_2}c{\theta _1}c{\theta _2}} \right)^2} + \hfill \\ {\left({{l_3}s{\theta _1}c{\theta _{23}} + {l_2}s{\theta _1}c{\theta _2}} \right)^2} + {\left({{l_3}s{\theta _{23}} + {l_2}s{\theta _2}} \right)^2} \hfill \\ \end{gathered} $ |
整理得到：
| $l_3^2 + l_2^2 + 2{l_2}{l_3}c{\theta _3}=p_x^2 + p_y^2 + {\left({p_z^2-{l_1}} \right)^2}$ |
即得:
| $ {\theta _3}=\arccos \left({\frac{{p_x^2 + p_y^2 + {{\left({p_z^2-{l_1}} \right)}^2}-l_3^2-l_2^2}}{{2{l_2}{l_3}}}} \right) $ |
将式(7)等号的左、右两边分别作[12]处理，得到：
| $\frac{{{p_x}-{l_1}}}{{\sqrt {p_x^2 + p_y^2} }}=\frac{{{l_3}s{\theta _{23}} + {l_2}s{\theta _2}}}{{{l_3}c{\theta _{23}} + {l_2}c{\theta _2}}}$ |
令，则：
| $s{\theta _2}\left({{l_3}c{\theta _3} + {l_2} + k{l_3}s{\theta _3}} \right)=c{\theta _2}\left({k{l_3}c{\theta _3}-{l_3}s{\theta _3} + k{l_2}} \right)$ |
即得：和θ2=θ2±180°，使计算得到的角度值满足。
通过以上计算，根据机器人机械臂手柄的位置，得到的三个关节的角度值分别为：
| $\left\{ \begin{gathered} {\theta _1}={\text{arctan}}\left({\frac{{{p_y}}}{{{p_x}}}} \right), 和{\theta _1}={\theta _1} + {180^ \circ } \hfill \\ {\theta _2}=\arctan \left({\frac{{k\left({{l_3}c{\theta _3} + {l_2}} \right)-{l_3}s{\theta _3}}}{{{l_3}c{\theta _3} + {l_2} + k{l_3}s{\theta _3}}}} \right), 和{\theta _2}={\theta _2} + {180^ \circ }, 其中k=\frac{{{p_z}-{l_1}}}{{\sqrt {p_x^2 + p_y^2} }}=k \hfill \\ {\theta _3}=\arccos \left({\frac{{p_x^2 + p_y^2 + {{\left({p_z^2-{l_1}} \right)}^2}-l_3^2-l_2^2}}{{2{l_2}{l_3}}}} \right) \hfill \\ \end{gathered} \right.$ |
方程组(12)给出了中央驱动式上肢康复机器人在任何期望位置时所需要的关节值。
3 运动学仿真分析
在上肢康复控制系统中，为使机器人机械臂手柄按照合理的康复运动轨迹运动，就要利用上文求得的逆运动学方程计算得到各关节的运动角度，进而控制驱动电机驱动各关节运动，实现上肢功能障碍患者的被动康复训练。
为验证结构设计的合理性以及逆运动方程的有效性，本文在SolidWorks建模软件下对中央驱动式上肢康复机器人进行三维建模，利用SolidWorks建模软件中的Motion模块[13-14]，对上肢康复机器人机械臂手柄进行预设三维康复运动轨迹的仿真实验。由于运动轨迹一定，可以直接得到运动过程中机器人机械臂手柄的位置信息，即各时刻的px、py、pz值，和3个关节的角度信号。预设三维康复运动轨迹的起始、中间和停止位置如图 3所示。
图3
康复训练运动轨迹三维图
(a)起始位置；(b)中间位置；(c)停止位置
Figure3. Three-dimensional maps of rehabilitation route(a) starting position; (b) middle position; (c) stop position
通过SolidWorks建模得到中央驱动式上肢康复机器人的模型，已知l1=306 mm, l2=452.36 mm, l3=374.95 mm。在机器人进行运动仿真时，根据各时刻机器人机械臂手柄的px、py、pz的位置大小和式(12)，得到各时刻θ1、θ2、θ3的大小。将求出的θ1、θ2、θ3大小与在运动仿真中测量得到的各关节角度大小进行比较，结果如图 4所示。
图4
计算得到的与测量得到的机器人各关节角度对比图
(a)
(a)
从图 4可以看到，θ1的运动范围在-120°~20°之间，满足的活动范围；θ2的运动范围在160°~175°之间，满足的活动范围；θ3的运动范围在5°~55°之间，满足的活动范围。由图 4(a)、(c)可以看出，通过逆运动学方程计算得到的θ1、θ3与通过SolidWorks直接得到的各关节角度基本相同，运动角度-时间曲线拟合度好。由式(13)可知，θ2是在θ3的基础上计算得到，因此会有累积误差，但从图 4(b)可以看出，θ2计算得到的角度值与测量得到的角度值最大误差不超过3°。
4 结论
(1)本文利用轴套及锥齿轮结构，设计了一款中央驱动式上肢康复机器人。不仅能减小机械臂的体积，还能减小电机噪音、辐射等不良因素对上肢功能障碍患者的影响。
(2)利用D-H表示法进行运动学建模，得到了中央驱动式上肢康复机器人的正逆运动学方程。在对机械臂手柄预设康复轨迹路线的情况下，完成了中央驱动式上肢康复机器人的运动仿真。实验得到的各关节角度值完全在设定范围之内，验证了中央驱动式上肢康复机器人结构设计的合理性。将通过逆运动学方程计算得到的各关节角度-时间曲线与仿真中测量得到的角度-时间曲线进行比较，误差不超过3°，验证了逆运动学方程的有效性。
(3)中央驱动式上肢康复机器人的正逆运动学方程与运动学仿真实验，为进一步控制中央驱动式上肢康复机器人提供了理论与实验依据。
(4)本文将在此研究的基础上，继续设计利用套筒结构实现大臂和前臂的长短可调来更好地满足不同患者需求。同时，为帮助上肢功能障碍患者更好地进行上肢康复，本文还将增加患者的手部训练，将通过设计腕关节2自由度(包含腕关节屈/伸运动和前臂的内旋/外旋运动)来实现。
