為在結構上減小機械臂的體積, 同時還減小電機噪音、輻射等不良因素對上肢功能障礙患者的影響, 本文提出了上肢康復機器人的中央驅動式傳動結構。利用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自由度(包含腕關節屈/伸運動和前臂的內旋/外旋運動)來實現。
