機械手臂機器人動能

剛體上某一質點 i，質量大小為 dm，其位置向量為𝒓_𝒊，在此瞬間的速 度為𝒗_𝒊，則質點的動能可表示為𝑇_𝑖 =¹

2(𝑑𝑚)𝒗_𝒊^𝟐。整個剛體的動能可以積分 的方式推導而得[37]，即

T_𝑖 =1 2∫ 𝒗_𝒊^𝟐

𝑚_𝑖 (𝑑𝑚) (4 − 25)

同理，機械手臂機器人的動能

T = 1

2∫ 𝒓_𝟏^𝑇𝒓_𝟏𝑑𝑚₁+1

2∫ 𝒓_𝟐^𝑇𝒓_𝟐𝑑𝑚₂ (4 − 26)

在推導的過程中，需要應用到一些運算技巧來簡化算式[34-37-38]，如下 𝐒_𝐢= ∫ 𝝆_𝒊𝑑𝑚_𝑖 = 𝑚_𝑖𝒅_𝒊 (4 − 27)

- 45 -

其中𝑑_𝑖是質心位置向量

𝑹^𝑻𝑵_𝒊𝑹= 𝑰

𝑵𝒊 (4 − 28)

其中 I 為單位矩陣

𝒂 ̃ 𝒃 = 𝒂 × 𝒃 = −𝒃 × 𝒂 = −𝒃̃𝒂 (4 − 29)

∂

∂𝑹𝒂^𝑻𝑹𝒃 = 𝒃̃𝑹^𝑻𝒂 (4 − 30)

𝒂̃𝒃̃^𝑻+ 𝒃̃𝒂̃ + 𝒂̃𝒃̃ = 𝟎 (4 − 31)

故動能

T = 1

2𝝎_𝟏^𝑻𝑰_𝟏𝝎_𝟏+1

2𝒎_𝟐𝝎_𝟏^𝑻𝑳̃_𝟏^𝑻𝑳̃_𝟏𝝎_𝟏+ 𝑳_𝟏^𝑻𝝎_𝟏^𝑻_𝟐^𝟏𝑹𝝎̃ 𝑺_{𝟐 𝟐} +1

2𝝎_𝟐^𝑻𝑰_𝟐𝝎_𝟐 (4 − 32)

由(4-29)式，動能 T 可得 T =1

2𝝎_𝟏^𝑻𝑰_𝟏𝝎_𝟏+1

2𝒎_𝟐𝝎_𝟏^𝑻𝑳̃_𝟏^𝑻𝑳̃_𝟏𝝎_𝟏+ 𝝎_𝟏^𝑻𝑳_𝟏^𝑻_𝟐^𝟏𝑹𝑺̃𝝎_{𝟐 𝟐} +1

2𝝎_𝟐^𝑻𝑰_𝟐𝝎_𝟐 (4 − 33)

利用純量轉置其值不變，可改變上式第三項得

- 46 -

T = 1

2𝝎_𝟏^𝑻𝑰_𝟏𝝎_𝟏+1

2𝒎_𝟐𝝎_𝟏^𝑻𝑳̃_𝟏^𝑻𝑳̃_𝟏𝝎_𝟏+ 𝝎_𝟐^𝑻𝑺̃_𝟐^𝑻_𝟐^{𝟏 𝑻}𝑹 𝑳̃𝝎_{𝟏 𝟏}+1

2𝝎_𝟐^𝑻𝑰_𝟐𝝎_𝟐 (4 − 34)

又_𝟐^𝟏𝑹= 𝑹^𝑵_𝟏 ^𝑻𝑵_𝟐𝑹，再改變上式第三項得

T =1

2𝝎_𝟏^𝑻𝑰_𝟏𝝎_𝟏+1

2𝒎_𝟐𝝎_𝟏^𝑻𝑳̃_𝟏^𝑻𝑳̃_𝟏𝝎_𝟏 +𝑺_𝟐^𝑻𝝎̃_𝟐^{𝑻𝑵 𝑻}_𝟐𝑹 ^𝑵_𝟏𝑹𝝎̃ 𝑳_{𝟏 𝟏}+1

2𝝎_𝟐^𝑻𝑰_𝟐𝝎_𝟐 (4 − 35)

(4-33)式對𝝎_𝟏作偏微分，得

∂T

∂𝝎_𝟏 = 𝑰_𝟏𝝎_𝟏+ 𝒎_𝟏𝑳̃_𝟏^𝑻𝑳̃𝝎_{𝟏 𝟏}+ 𝑳̃_𝟏^𝑻_𝟐^𝟏𝑹𝑺̃𝝎_{𝟐 𝟐} (4 − 36)

再對時間微分，得

𝑑 𝑑𝑡( ∂T

∂𝝎_𝟏) = 𝑰_𝟏𝝎̇_𝟏+ 𝒎_𝟏𝑳̃_𝟏^𝑻𝑳̃𝝎̇_{𝟏 𝟏}+ 𝑳̃_𝟏^𝑻_𝟐^𝟏𝑹̇𝑺̃𝝎_{𝟐 𝟐}+ 𝑳̃_𝟏^𝑻_𝟐^𝟏𝑹𝑺̃𝝎̇_{𝟐 𝟐} (4 − 37)

(4-34)式對𝝎_𝟐作偏微分，得

∂T

∂𝝎_𝟐 = 𝑰_𝟐𝝎_𝟐+ 𝑺̃_𝟐^𝑻_𝟐^𝟏𝑹𝑳̃𝝎_{𝟏 𝟏} (4 − 38)

再對時間微分，得 𝑑

𝑑𝑡( ∂T

∂𝝎_𝟐) = 𝑰_𝟐𝝎̇_𝟐+ 𝑺̃_𝟐^𝑻_𝟐^𝟏𝑹̇𝑳̃𝝎_{𝟏 𝟏}+ 𝑺̃_𝟐^𝑻_𝟐^𝟏𝑹𝑳̃𝝎̇_{𝟏 𝟏} (4 − 39)

- 47 -

應用(4-30)式，(4-35)、(4-32)式分別對^𝑵_𝟏𝑹、^𝑵_𝟐𝑹作偏微分，得

∂T

∂ 𝑹^𝑵_𝟏 = 𝝎̃ 𝑳̃ 𝑹_{𝟏 𝟏}^{𝑵 𝑻}_𝟏 ^𝑵_𝟐𝑹𝝎̃ 𝑺_{𝟐 𝟐} (4 − 40)

∂T

∂ 𝑹^𝑵_𝟐 = 𝝎̃ 𝑳̃ 𝑹_{𝟐 𝟐}^{𝑵 𝑻}_𝟐 ^𝑵_𝟏𝑹𝝎̃ 𝑳_{𝟏 𝟏} (4 − 41) 4.3.2 機械手臂機器人位能

以慣性參考座標 XNYNZN作為量測位能的基準，重力加速度 g 向上為 正。對機械手臂機器人作位能的推導，即

𝑈 = ∫ −𝒈^𝑻𝒓_𝟏𝑑𝑚₁+ ∫ −𝒈^𝑻𝒓_𝟐𝑑𝑚₂

= −𝒈^𝑻𝑵_𝟏𝑹𝑺_𝟏− 𝑚₂𝒈^𝑻𝑵_𝟏𝑹𝑳_𝟏 − 𝒈^𝑻𝑵_𝟐𝑹𝟐 (4 − 42)

應用(4-30)式，上式分別對^𝑵_𝟏𝑹、^𝑵_𝟐𝑹作偏微分，得

∂U

∂ 𝑹^𝑵_𝟏 = −𝑺̃ 𝑹_𝟏^{𝑵 𝑻}_𝟏 𝒈 − 𝒎_𝟐𝑳̃ 𝑹_𝟏^{𝑵 𝑻}_𝟏 𝒈 (4 − 43)

∂U

∂ 𝑹^𝑵_𝟐 = −𝑺̃ 𝑹_𝟐^{𝑵 𝑻}_𝟐 𝒈 (4 − 44)

4.3.3 虛功與反向動力學

緊接著探討準座標與慣性參考座標間一些重要的關係式，以利其後計 算的運用:

𝝎_𝒊 = _𝒊−𝟏^𝒊𝑹𝝎_𝒊−𝟏+ 𝑫_𝒊𝜼_𝒊̇ = _𝒊−𝟏^𝒊𝑹𝝎_𝒊−𝟏+ 𝛀_𝒊 (4 − 45) 所以由上式，得

𝛀_𝒊 = 𝝎_𝒊 −_𝒊−𝟏^𝒊𝑹𝝎_𝒊−𝟏 = 𝑫_𝒊𝜼_𝒊̇ (4 − 46)

- 48 -

則可得

δ𝜶_𝒊 = δ𝜷_𝒊−_𝒊−𝟏^𝒊𝑹𝝎_𝒊−𝟏 = 𝑫_𝒊𝜹𝜼_𝒊 (4 − 47)

其中，𝝎_𝒊是是連桿 i 對其參考座標的準角速度；𝑫_𝒊是連桿 i 相對於絕對座 標 的 準 速 度 與 尤 拉 角 轉 動 間 的 速 度 轉 換 矩 陣 (velocity transformation matrix)；𝜼_𝒊̇ 是連桿 i 相對於絕對座標的尤拉角角速度；𝛀_𝒊是連桿 i 相對於 絕對座標的準角速度；δ𝜶_𝒊是連桿 i 相對於絕對座標的準角位移；δ𝜷_𝒊是連 桿 i 對其參考座標的準角位移；𝜹𝜼_𝒊是連桿 i 相對於絕對座標的尤拉角角位 移。

(4-45)式對時間微分求角加速度，得

𝝎̇_𝒊 = _𝒊−𝟏^𝒊𝑹𝝎̇_𝒊−𝟏+ 𝑫̃_𝒊𝜼_𝒊̇ _𝒊−𝟏^𝒊𝑹𝝎_𝒊−𝟏+ 𝑫_𝒊𝜼_𝒊̈ + 𝑫̇ 𝜼_{𝒊 𝒊}̇ (4 − 48)

由(4-45)(4-46)(4-48)式，可得下列結果

𝛀_𝟐 = 𝜃₄̇ 𝒁_𝟐 (4 − 49)

𝝎_𝟐 = 𝑹_𝟏^𝟐 𝝎_𝟏+ 𝜃₄̇ 𝒁_𝟐 (4 − 50)

𝝎̇_𝟐 = 𝑹_𝟏^𝟐 𝝎̇_𝟏+ 𝜃̃ 𝑹₄̇ 𝒁_𝟐𝟏^𝟐 𝝎_𝟏+ 𝜃̈₄𝒁_𝟐 (4 − 51) 其中，𝒁_𝟐為沿著Z₂軸的單位向量。

由(4-47)式，可得

- 49 -

δ𝜶_𝟏 = δ𝜷_𝟏 = 𝑫_𝟏𝛿𝜼_𝟏 即 𝛿𝜼_𝟏 = 𝑫_𝟏^−𝟏δ𝜷_𝟏

(4 − 52) δ𝜶_𝟐 = δ𝜷_𝟐− 𝑹_𝟏^𝟐 δ𝜷_𝟏 = 𝑫_𝟐𝛿𝜼_𝟐 (4 − 53)

即 𝛿𝜼_𝟐 = 𝑫_𝟐^−𝟏δ𝜶_𝟐 = 𝑫_𝟐^−𝟏(δ𝜷_𝟐 − 𝑹_𝟏^𝟐 δ𝜷_𝟏) (4 − 54)

接下來，有了以上的關係式及動能、位能的數學式與以準座標建立的 Lagrangian 方程式，即(4-2)(4-3)式。我們就可以正式推導機械手臂機器人 的反向動力方程式:

首先將(4-36)(4-37)(4-40)(4-42)式代入(4-2)式，得

(𝑰_𝟏+ 𝑚₂𝑳̃_𝟏^𝑻𝑳̃) 𝝎̇_{𝟏 𝟏}+ (𝑳̃_𝟏^𝑻_𝟐^𝟏𝑹𝑺̃_𝟐) 𝝎̇_𝟐 + (𝝎̃ 𝑰_{𝟏 𝟏}+ 𝑚₂𝝎̃ 𝑳_𝟏̃_𝟏^𝑻𝑳̃) 𝝎_{𝟏 𝟏} +(−𝑺̃ 𝑹_𝟏^{𝑵 𝑻}_𝟏 𝒈 − 𝑚₂𝑳̃ 𝑹_𝟏^{𝑵 𝑻}_𝟏 𝒈) = 𝑸_𝟏+ 𝑭_𝑪𝟏 (4 − 55)

再將(4-51)式代入上式，得

(𝑰_𝟏+ 𝑚₂𝑳̃_𝟏^𝑻𝑳̃ + 𝑳_𝟏 ̃_𝟏^𝑻_𝟐^𝟏𝑹𝑺̃_𝟐𝟏^𝟐𝑹) 𝝎̇_𝟏

+ (𝑳̃_𝟏^𝑻_𝟐^𝟏𝑹𝑺̃𝜃_𝟐̃ 𝑹₄̇ 𝒁_𝟐𝟏^𝟐 + 𝝎̃ 𝑰_{𝟏 𝟏}+ 𝑚₂𝝎̃ 𝑳_𝟏̃_𝟏^𝑻𝑳̃) 𝝎_{𝟏 𝟏} + 𝑳̃_𝟏^𝑻_𝟐^𝟏𝑹𝑺̃𝒁_{𝟐 𝟐}𝜃̈₄+ (−𝑺̃ 𝑹_𝟏^{𝑵 𝑻}_𝟏 𝒈 − 𝑳̃ 𝑹_𝟏^{𝑵 𝑻}_𝟏 𝑾_𝟐)

= 𝑸_𝟏+ 𝑭_𝑪𝟏 (4 − 56) 接著，將(4-38)(4-39)(4-41)(4-43)式代入(4-3)式，並以技巧(4-31)式對算式 作化簡，可得

𝑺̃ 𝑹_𝟐𝟐^{𝟏 𝑻}𝑳̃𝝎̇_{𝟏 𝟏}+ 𝑰_𝟐𝝎̇_𝟐+ 𝝎̃ 𝑰_{𝟐 𝟐}𝝎_𝟐− 𝑺̃ 𝑹_𝟐^{𝑵 𝑻}_𝟐 𝒈 = 𝑸_𝟐+ 𝑭_𝑪𝟐 (4 − 57)

- 50 -

再將(4-50)(4-51)式代入上式，得

𝑺̃_𝟐^𝑻_𝟐^{𝟏 𝑻}𝑹 𝑳̃𝝎̇_{𝟏 𝟏}+ 𝑰_𝟐𝟏^𝟐𝑹𝝎̇_𝟏 + 𝑰_𝟐𝜃̃ 𝑹₄̇ 𝒁_𝟐𝟏^𝟐 𝝎_𝟏 + 𝑰_𝟐𝒁_𝟐𝜃̈₄+ 𝝎̃ 𝑰_{𝟐 𝟐𝟏}^𝟐𝑹𝝎_𝟏+ 𝝎̃ 𝑰_{𝟐 𝟐}𝒁_𝟐𝜃₄̇ +(−𝑺̃ 𝑹_𝟐^{𝑵 𝑻}_𝟐 𝒈) = 𝑸_𝟐+ 𝑭_𝑪𝟐 (4 − 58)

結合(4-56)(4-58)式，可得

[𝑰_𝟏+ 𝑚₂𝑳̃_𝟏^𝑻𝑳̃ + 𝑳_𝟏 ̃_𝟏^𝑻_𝟐^𝟏𝑹𝑺̃_𝟐𝟏^𝟐𝑹 𝑳̃_𝟏^𝑻_𝟐^𝟏𝑹𝑺̃𝒁_{𝟐 𝟐} 𝑺̃_𝟐^𝑻_𝟐^{𝟏 𝑻}𝑹 𝑳̃ + 𝑰_{𝟏 𝟐𝟏}^𝟐𝑹 𝑰_𝟐𝒁_𝟐 ] [𝝎̇_𝟏

𝜃̈₄]

+ [𝑳̃_𝟏^𝑻_𝟐^𝟏𝑹𝑺̃𝜃_𝟐̃ 𝑹₄̇ 𝒁_𝟐𝟏^𝟐 + 𝝎̃ 𝑰_{𝟏 𝟏}+ 𝑚₂𝝎̃ 𝑳_𝟏̃_𝟏^𝑻𝑳̃_𝟏 0

𝑰_𝟐𝜃̃ 𝑹₄̇ 𝒁_𝟐𝟏^𝟐 + 𝝎̃ 𝑰_{𝟐 𝟐𝟏}^𝟐𝑹 𝝎̃ 𝑰_{𝟐 𝟐}𝒁_𝟐] [𝝎_𝟏 𝜃₄̇ ] + [−𝑺̃ 𝑹_𝟏^{𝑵 𝑻}_𝟏 𝒈 − 𝑳̃ 𝑹_𝟏^{𝑵 𝑻}_𝟏 𝑾_𝟐

−𝑺̃ 𝑹_𝟐^{𝑵 𝑻}_𝟐 𝒈 ] = [𝑸_𝟏+ 𝑭_𝑪𝟏

𝑸_𝟐+ 𝑭_𝑪𝟐] (4 − 59)

最後，上式所得的結果即為機器手臂機器人的動力方程式，以下列形式簡 化表示:

𝑴𝜣̈ + 𝑯𝜣̇ + 𝑮 = 𝝉 (4 − 60)

其中，𝑴 = [𝑴_𝟏𝟏 𝑴_𝟏𝟐 𝑴_𝟐𝟏 𝑴_𝟐𝟐]

𝑴_𝟏𝟏 = 𝑰_𝟏+ 𝑚₂𝑳̃_𝟏^𝑻𝑳̃ + 𝑳_𝟏 ̃_𝟏^𝑻_𝟐^𝟏𝑹𝑺̃_𝟐𝟏^𝟐𝑹

𝑴_𝟏𝟐 = 𝑳̃_𝟏^𝑻_𝟐^𝟏𝑹𝑺̃𝒁_{𝟐 𝟐}

𝑴_𝟐𝟏 = 𝑺̃_𝟐^𝑻_𝟐^{𝟏 𝑻}𝑹 𝑳̃ + 𝑰_{𝟏 𝟐𝟏}^𝟐𝑹

𝑴_𝟐𝟐 = 𝑰_𝟐𝒁_𝟐

𝑯 = [𝑯_𝟏𝟏 𝑯_𝟏𝟐 𝑯_𝟐𝟏 𝑯_𝟐𝟐]

- 51 -

𝑯_𝟏𝟏 = 𝑳̃_𝟏^𝑻_𝟐^𝟏𝑹𝑺̃𝜃_𝟐̃ 𝑹₄̇ 𝒁_𝟐𝟏^𝟐 + 𝝎̃ 𝑰_{𝟏 𝟏} + 𝑚₂𝝎̃ 𝑳_𝟏̃_𝟏^𝑻𝑳̃ _𝟏 𝑯_𝟏𝟐 = 𝟎

𝑯_𝟐𝟏 = 𝑰_𝟐𝜃̃ 𝑹₄̇ 𝒁_𝟐𝟏^𝟐 + 𝝎̃ 𝑰_{𝟐 𝟐𝟏}^𝟐𝑹

𝑯_𝟐𝟐 = 𝝎̃ 𝑰_{𝟐 𝟐}𝒁_𝟐

𝑮 = [𝑮_𝟏 𝑮_𝟐]

𝝉 = [𝑸_𝟏+ 𝑭_𝑪𝟏 𝑸_𝟐+ 𝑭_𝑪𝟐]

𝜣̈ = [𝝎̇_𝟏 𝜃̈₄]

𝜣̇ = [𝝎_𝟏 𝜃₄̇ ]

(4-60)式所得的機器手臂機器人的動力方程式還不夠清楚表示軸承力 矩與轉動角度間的關係，還需要做更進一步的化簡。根據達朗伯特原理 (d’Alembert’s principle)，在一個動力系統中，拘束力所作的總虛功會等於 零，即

𝛅𝑾_𝑪 = 𝑭_𝑪𝟏^𝑻𝜹𝜷_𝟏+ 𝑭_𝑪𝟐^𝑻𝜹𝜷_𝟐 = 𝟎 (4 − 61)

由(4-60)式，可得

𝑭_𝑪𝟏 = 𝑴_𝟏𝟏𝝎̇_𝟏+ 𝑴_𝟏𝟐𝜽̈_𝟒+ 𝑯_𝟏𝟏𝝎_𝟏+ 𝑮_𝟏 − 𝑸_𝟏 (4 − 62) 𝑭_𝑪𝟐 = 𝑴_𝟐𝟏𝝎̇_𝟏+ 𝑴_𝟐𝟐𝜽̈_𝟒+ 𝑯_𝟐𝟏𝝎_𝟏+ 𝑯_𝟐𝟐𝜃̇₄+ 𝑮_𝟐 − 𝑸_𝟐 (4 − 63)

- 52 -

而角速度與尤拉角之間的關係為

𝝎_𝟏 = 𝑪_𝟏𝜸̇_𝟏 (4 − 64) 𝝎̇_𝟏 = 𝑪̇_𝟏𝜸̇_𝟏+ 𝑪_𝟏𝜸̈_𝟏 (4 − 65)

將(4-64)(4-65)式分別代入(4-61)(4-62)式，得

𝑭_𝑪𝟏 = 𝑴_𝟏𝟏𝑪_𝟏𝜸̈_𝟏+ 𝑴_𝟏𝟐𝜽̈_𝟒+ (𝑴_𝟏𝟏𝑪̇_𝟏+ 𝑯_𝟏𝟏𝑪_𝟏)𝜸̇_𝟏+ 𝑮_𝟏− 𝑸_𝟏 (4 − 66)

𝑭_𝑪𝟐 = 𝑴_𝟐𝟏𝑪_𝟏𝜸̈_𝟏+ 𝑴_𝟐𝟐𝜃̈₄+ (𝑴_𝟐𝟏𝑪̇_𝟏 + 𝑯_𝟐𝟏𝑪_𝟏)𝜸̇_𝟏+ 𝑯_𝟐𝟐𝜃̇₄+ 𝑮_𝟐− 𝑸_𝟐 (4 − 67)

又由(4-50)(4-63)式可得

δ𝜷_𝟏 = 𝑪_𝟏𝛿𝜸_𝟏 (4 − 68)

δ𝜷_𝟐 = 𝑹_𝟏^𝟐 δ𝜷_𝟏+ 𝒁_𝟐𝛿𝜃₄ (4 − 69)

其中，𝑪_𝟏是連桿 1 相對於參考座標的準速度與尤拉角速度間的速度轉換矩 陣；𝛿𝜃₄是手肘的轉動角位移。

將 (4-66)(4-67)(4-68)(4-69)式代入到(4-61)式，可得

- 53 -

𝛅𝑾_𝑪 = {𝑪_𝟏^𝑻(𝑴_𝟏𝟏+ 𝑹_𝟐^𝟏 𝑴_𝟐𝟏)𝑪_𝟏𝜸̈_𝟏+ 𝑪_𝟏^𝑻(𝑴_𝟏𝟐+ 𝑹_𝟐^𝟏 𝑴_𝟐𝟐)𝜃̈₄

+ [𝑪_𝟏^𝑻(𝑴_𝟏𝟏 + 𝑹_𝟐^𝟏 𝑴_𝟐𝟏)𝑪̇_𝟏+ 𝑪_𝟏^𝑻(𝑯_𝟏𝟏+ 𝑹_𝟐^𝟏 𝑯_𝟐𝟏)𝑪_𝟏]𝜸̇_𝟏

+ 𝑪_𝟏^𝑻_𝟐^𝟏𝑹𝑯_𝟐𝟏𝜃̇₄+ 𝑪_𝟏^𝑻(𝑮_𝟏+ 𝑹_𝟐^𝟏 𝑮_𝟐) − 𝑪_𝟏^𝑻(𝑸_𝟏+ 𝑹_𝟐^𝟏 𝑸_𝟐)}^𝑇𝛿𝜸_𝟏 + {𝒁_𝟐^𝑻𝑴_𝟏𝟏𝑪_𝟏𝜸̈_𝟏+ 𝒁_𝟐^𝑻𝑴_𝟐𝟐𝜃̈₄+ 𝒁_𝟐^𝑻(𝑴_𝟐𝟏𝑪̇_𝟏+ 𝑯_𝟐𝟏𝑪_𝟏)𝜸̇_𝟏 + 𝒁_𝟐^𝑻𝑯_𝟐𝟐𝜃̇₄+ 𝒁_𝟐^𝑻𝑮_𝟐− 𝒁_𝟐^𝑻𝑸_𝟐}^𝑇𝛿𝜃₄ = 0 (4 − 70)

將上式簡明表示為下列型式:

𝛅𝑾_𝑪 = 𝑲_𝑪𝟏𝛿𝜸_𝟏+ 𝐾_𝐶2𝛿𝜃₄ = 0 (4 − 71) 其中，

𝑲_𝑪𝟏= {𝑪_𝟏^𝑻(𝑴_𝟏𝟏+ 𝑹_𝟐^𝟏 𝑴_𝟐𝟏)𝑪_𝟏𝜸̈_𝟏+ 𝑪_𝟏^𝑻(𝑴_𝟏𝟐+ 𝑹_𝟐^𝟏 𝑴_𝟐𝟐)𝜃̈₄ + [𝑪_𝟏^𝑻(𝑴_𝟏𝟏+ 𝑹_𝟐^𝟏 𝑴_𝟐𝟏)𝑪̇_𝟏 + 𝑪_𝟏^𝑻(𝑯_𝟏𝟏+ 𝑹_𝟐^𝟏 𝑯_𝟐𝟏)𝑪_𝟏]𝜸̇_𝟏 + 𝑪_𝟏^𝑻_𝟐^𝟏𝑹𝑯_𝟐𝟏𝜃̇₄+ 𝑪_𝟏^𝑻(𝑮_𝟏+ 𝑹_𝟐^𝟏 𝑮_𝟐) − 𝑪_𝟏^𝑻(𝑸_𝟏+ 𝑹_𝟐^𝟏 𝑸_𝟐)}^𝑇

(4 − 72)

𝐾_𝐶2 = {𝒁_𝟐^𝑻𝑴_𝟏𝟏𝑪_𝟏𝜸̈_𝟏+ 𝒁_𝟐^𝑻𝑴_𝟐𝟐𝜃̈₄+ 𝒁_𝟐^𝑻(𝑴_𝟐𝟏𝑪̇_𝟏+ 𝑯_𝟐𝟏𝑪_𝟏)𝜸̇_𝟏+ 𝒁_𝟐^𝑻𝑯_𝟐𝟐𝜃̇₄ + 𝒁_𝟐^𝑻𝑮_𝟐− 𝒁_𝟐^𝑻𝑸_𝟐}^𝑇 (4 − 73)

在𝛿𝜸_𝟏與𝛿𝜃₄不完全為零的情況下，欲使(4-71)式等於零， 𝑲_𝑪𝟏與 𝐾_𝐶2必 須為零，(4-71)式等於零才會成立，故

𝑲_𝑪𝟏 = 0 (4 − 74) 𝐾_𝐶2 = 0 (4 − 75) 聯立(4-74)(4-75)式，可得

- 54 -

[𝑪_𝟏^𝑻(𝑴_𝟏𝟏 + 𝑹_𝟐^𝟏 𝑴_𝟐𝟏)𝑪_𝟏 𝑪_𝟏^𝑻(𝑴_𝟏𝟐 + 𝑹_𝟐^𝟏 𝑴_𝟐𝟐) 𝒁_𝟐^𝑻𝑴_𝟏𝟏𝑪_𝟏𝜸̈_𝟏 𝒁_𝟐^𝑻𝑴_𝟐𝟐 ] [𝜸̈_𝟏

𝜃̈₄]

+ [[𝑪_𝟏^𝑻(𝑴_𝟏𝟏 + 𝑹_𝟐^𝟏 𝑴_𝟐𝟏)𝑪̇_𝟏+ 𝑪_𝟏^𝑻(𝑯_𝟏𝟏+ 𝑹_𝟐^𝟏 𝑯_𝟐𝟏)𝑪_𝟏] 𝑪_𝟏^𝑻_𝟐^𝟏𝑹𝑯_𝟐𝟏 𝒁_𝟐^𝑻(𝑴_𝟐𝟏𝑪̇_𝟏+ 𝑯_𝟐𝟏𝑪_𝟏) 𝒁_𝟐^𝑻𝑯_𝟐𝟐 ] [𝜸̇_𝟏

𝜃̇₄]

+ [𝑪_𝟏^𝑻(𝑮_𝟏+ 𝑹_𝟐^𝟏 𝑮_𝟐) 𝒁_𝟐^𝑻𝑮_𝟐 ]

= [𝑪_𝟏^𝑻(𝑸_𝟏+ 𝑹_𝟐^𝟏 𝑸_𝟐)

𝒁_𝟐^𝑻𝑸_𝟐 ] (4 − 76)

充氣時，人工肌肉氣壓缸拉動鋼索，使關節處的軸承作動，產生了力 矩。應用虛功原理，計算這些力矩所做的虛功:

δ𝑾_𝑷 = 𝝉_𝟏^𝑻𝜹𝜼_𝟏+ 𝝉_𝟐^𝑻𝜹𝜼_𝟐 = 𝝉_𝟏^𝑻𝜹𝜼_𝟏+ 𝜏_𝑒𝛿𝜃₄

= 𝑸_𝟏^𝑻𝛿𝜷_𝟏+ 𝑸_𝟐^𝑻𝜹𝜷_𝟐 (4 − 77) 𝑸_𝟏 = 𝑫_𝟏^−𝑻𝝉_𝟏− 𝑹_𝟏^𝟐 𝑫_𝟐^−𝑻𝝉_𝟒 (4 − 78) 𝑸_𝟐 = 𝑫_𝟐^−𝑻𝝉_𝟒 (4 − 79)

其中，𝛅𝑾_𝑷為四個人工肌肉氣壓缸產生的力矩所作的虛功；𝝉_𝟏是肩膀處 三個軸承的致動致力矩向量；𝝉_𝟐是手肘處軸承的力矩向量；𝜏_𝑒是手肘處軸 承以𝒁_𝟐為轉軸的致動力矩。

將(4-68)(4-69)式代入(4-77)式，可得

δ𝑾_𝑷 = (𝑸_𝟏^𝑻𝑫_𝟏+ 𝑸_𝟐^𝑻_𝟏^𝟐𝑹𝑫_𝟏)𝜹𝜼_𝟏 + 𝑸_𝟐^𝑻𝑫_𝟐𝜹𝜼_𝟐 (4 − 80) 而𝑫_𝟏 = 𝑪_𝟏，𝑫_𝟐 = 𝒁_𝟐

- 55 -

則上式可得

δ𝑾_𝑷 = (𝑸_𝟏^𝑻𝑪_𝟏+ 𝑸_𝟐^𝑻_𝟏^𝟐𝑹𝑪_𝟏)𝜹𝜼_𝟏+ 𝑸_𝟐^𝑻𝒁_𝟐𝜹𝜼_𝟐 = 𝝉_𝟏^𝑻𝜹𝜼_𝟏 + 𝜏_𝑒𝛿𝜃₄ 即

𝝉_𝟏 = 𝑪_𝟏^𝑻(𝑸_𝟏+ 𝑹_𝟐^𝟏 𝑸_𝟐) (4 − 81) 𝝉_𝒆= 𝒁_𝟐^𝑻𝑸_𝟐 (4 − 82)

所以(4-76)式可寫成

[𝑪_𝟏^𝑻(𝑴_𝟏𝟏+ 𝑹_𝟐^𝟏 𝑴_𝟐𝟏)𝑪_𝟏 𝑪_𝟏^𝑻(𝑴_𝟏𝟐+ 𝑹_𝟐^𝟏 𝑴_𝟐𝟐) 𝒁_𝟐^𝑻𝑴_𝟏𝟏𝑪_𝟏𝜸̈_𝟏 𝒁_𝟐^𝑻𝑴_𝟐𝟐 ] [𝜸̈_𝟏

𝜃̈₄]

+ [[𝑪_𝟏^𝑻(𝑴_𝟏𝟏+ 𝑹_𝟐^𝟏 𝑴_𝟐𝟏)𝑪̇_𝟏+ 𝑪_𝟏^𝑻(𝑯_𝟏𝟏+ 𝑹_𝟐^𝟏 𝑯_𝟐𝟏)𝑪_𝟏] 𝑪_𝟏^𝑻_𝟐^𝟏𝑹𝑯_𝟐𝟏 𝒁_𝟐^𝑻(𝑴_𝟐𝟏𝑪̇_𝟏+ 𝑯_𝟐𝟏𝑪_𝟏) 𝒁_𝟐^𝑻𝑯_𝟐𝟐 ] [𝜸̇_𝟏

𝜃̇₄]

+ [𝑪_𝟏^𝑻(𝑮_𝟏+ 𝑹_𝟐^𝟏 𝑮_𝟐)

𝒁_𝟐^𝑻𝑮_𝟐 ] = [ 𝝉_𝟏

𝜏_𝑒] (4 − 83)

將上式以簡明的型式表示為

𝑴_𝑷𝜽̈_𝑷+ 𝑯_𝑷𝜽̇_𝑷+ 𝑮_𝑷 = 𝝉_𝑷 (4 − 84) 其中，

𝑴_𝑷 = [𝑪_𝟏^𝑻(𝑴_𝟏𝟏+ 𝑹_𝟐^𝟏 𝑴_𝟐𝟏)𝑪_𝟏 𝑪_𝟏^𝑻(𝑴_𝟏𝟐+ 𝑹_𝟐^𝟏 𝑴_𝟐𝟐) 𝒁_𝟐^𝑻𝑴_𝟏𝟏𝑪_𝟏𝜸̈_𝟏 𝒁_𝟐^𝑻𝑴_𝟐𝟐 ] 𝑯_𝑷 =

[[𝑪_𝟏^𝑻(𝑴_𝟏𝟏 + 𝑹_𝟐^𝟏 𝑴_𝟐𝟏)𝑪̇_𝟏+ 𝑪_𝟏^𝑻(𝑯_𝟏𝟏+ 𝑹_𝟐^𝟏 𝑯_𝟐𝟏)𝑪_𝟏] 𝑪_𝟏^𝑻_𝟐^𝟏𝑹𝑯_𝟐𝟏 𝒁_𝟐^𝑻(𝑴_𝟐𝟏𝑪̇_𝟏+ 𝑯_𝟐𝟏𝑪_𝟏) 𝒁_𝟐^𝑻𝑯_𝟐𝟐 ]

𝑮_𝑷 = [𝑪_𝟏^𝑻(𝑮_𝟏+ 𝑹_𝟐^𝟏 𝑮_𝟐) 𝒁_𝟐^𝑻𝑮_𝟐 ]

- 56 -

𝜽̈_𝑷 = [𝜸̈_𝟏

𝜃̈₄] 𝜽̇_𝑷 = [𝜸̇_𝟏 𝜃̇₄]

(4-84)式所得的結果即為機器手臂機器人的反向動力學方程式，且清 楚表示軸承力矩與轉動角度間的關係，並有其物理意義如下[30]:

𝑴_𝑷

: 代表機器手臂機器人的慣性向量

𝑯_𝑷

: 代表機器手臂機器人的柯氏力及離心力所產生的向量

𝑮_𝑷

: 代表重力所產生的向量

𝝉_𝑷

: 人工肌肉氣壓缸拉動鋼索，作動於各部位軸承所需的力矩大小

- 57 -

第五章 復健姿勢模擬結果與討論

5.1 人工肌肉氣壓缸之動態模擬

根據第三章所建立的人工肌肉氣壓缸負載動態數學模型，並使用 Matlab

Simulink 進行模擬運算，得到人工肌肉氣壓缸之動態模擬結果如下:

圖 5-1 人工肌肉氣壓缸充氣時之輸出力響應圖

圖 5-2 人工肌肉氣壓缸充氣時之位移響應圖

0 0.5 1 1.5

0 100 200 300 400 500

時間t(s)

輸出力F(N)

0 0.5 1 1.5

-0.02 0 0.02 0.04 0.06 0.08 0.1

時間t(s)

位移x(m)

- 58 -

- 59 -

5.2 復健姿勢模擬

本論文所建立的機械手臂機器人具有 4 個自由度，包含肩膀的 3 個自由 度及手肘的 1 個自由度。對應到基本的復健姿勢與手臂運動，此 4 個自由度 可完成的動作有:

(1) 上臂的外展及內收 (2) 上臂的屈曲及伸展 (3) 上臂的內轉及外轉 (4) 前臂的屈曲及伸展

在此章節，本論文針對上述動作(1)、(2)、(4)進行模擬，包含順向運動 學、反向運動學、反向動力學的模擬。上臂的外展及內收動作是採取一開始 上肢自然放置，再向側邊平舉進行外展的動作。內收為由平舉後的外展姿勢，

回復自然放置的動作；上臂的屈曲及伸展是採取一開始上肢自然放置，再向 前方平舉進行屈曲的動作。伸展為由平舉後的屈曲姿勢，回復自然放置的動 作；前臂的屈曲及伸展是採取一開始上肢自然放置，前臂以手肘為軸向上舉 至與上臂垂直的屈曲動作。伸展為前臂與上臂垂直的姿勢，回復自然放置的 動作。至於上臂的內轉及外轉將以固定的角度配合其他動作來進行模擬運動，

在此處假設

𝜃

₃固定為零，即第四章所述之軌跡規則 1。

5.2.1 MATLB - Robotics toolbox

1996 年，Peter I. Corke 在 Matlab 上開發了一個在機器人學的應用上具 有相當便利性的工具[44]。結合 DENAVIT-HARTENBERG 標記方式，以此 為基礎建立機器手臂的模型(圖 5-5)，並可對其進行軌跡的規劃、求解順向 運動學及反向運動學。且可產生一個機械手臂的控制介面(圖 5-6)，對建立

- 60 -

- 61 -

theta: 連桿的旋轉角度 d: 連桿的橫位移長度

sigma: 0 代表旋轉關節，1 代表移動關節 (2) 機器手臂模型的建立

Name=robot ( { L{i} L{i+1}… } ) Name: 機器手臂模型的名稱 (3) 點到點軌跡的建立

T1 = transl(x, y, z) T2 = transl(u, v, w) T = ctraj(T1, T2, length) T1: 起始點

T2: 目標點 T: 齊性轉換矩陣 (4) 正向運動學的建立

T = fkine(Name, q) q: 關節角度 (5) 反向運動學的建立

q = ikine(Name, T) (6) 模擬介面及模型的建立

drivebot(Name)

- 62 -

5.2.2 順向運動學模擬結果

圖 5-7 上臂的外展及內收軌跡

圖 5-8 上臂的屈曲及伸展軌跡

圖 5-9 前臂的屈曲及伸展軌跡

- 63 -

- 64 -

- 65 -

- 66 -

5.2.4 反向動力學模擬結果

本論文使用易於表達矩陣的 MATLB 程式撰寫，並根據第四章節所得 的結果為程式的運算理論，以求解機器手臂機器人的反向動力學。而想要 求解反向動力學，必須有一定的計算過程，圖 5-16 即列出程式的數學運 算流程，給予相對的復健動作的順向運動學結果，經過計算，最後條列出 所需要的關節力矩大小。

定義基本參數

計算基本數值

計算轉換矩陣

計算轉換矩陣延伸 數值

計算所需之反對稱 矩陣

定義運算矩陣

延伸運算矩陣

給定動力方程式

得到各角度轉動 所需力矩大小

圖 5-16 反向動力學程式數學運算流程圖

- 67 -

- 68 -

- 69 -

觀察圖 5-17、圖 5-19，因為此兩種情況上臂的復健動作相當類似，均為 舉起上臂的動作，所以所需的力矩大小相同，且隨著時間的增加，上臂所處 的位能增加，所需的力矩大小也跟著遞增。同樣，圖 5-21 為單純舉起前臂 的動作，根據時間的增加，前臂所處的位能增加，所需的力矩大小也跟著遞 增，但相對於舉起上臂時會包含前臂的重量，僅舉起前臂所需的力矩較小。

而圖 5-18、圖 5-20，均為放下上臂的動作，且隨著時間的增加，上臂所所處 的位能減少，所需的力矩大小也跟著遞減。圖 5-22 則為放下前臂的動作，

隨著時間的增加，前臂所處的位能減少，所需的力矩大小也跟著遞減。

- 70 -

第六章 結論與未來發展方向

本論文已開發完成一個不同設計的外骨架機器人，不但有獨特的關節 機構設計，以軸承取代轉軸，且採用可大幅減輕整體重量的人工肌肉氣壓 缸作為致動器。同時，建立外骨架機器人各組成元件的數學模型。推導出 外骨架機器人的順向運動學、反向運動學及應用準座標系統推導反向動力 學，並進行復健動作的模擬運算，可得到外骨架機器人的運動軌跡、關節 角度變化與轉動關節所需的力矩大小。

未來發展方向:

 機械結構尺寸需量身打造，或者需再設計可動結構，以符合不同的穿 戴者。

 機械手臂機器人可運動至任何所能達到的位置，但是在某些位置時，

有無法轉向其他方向的情形，這必須在規劃軌跡時加以考量。

 實際穿載測試

外骨架機器人未來儼然會成為人類所需依賴的工具之一，其實際的應 用，不僅僅用於人體肢臂的復健，未來年齡的增延，老年化的族群驟增，

但老年人肌肉卻日益衰弱，外骨架機器人亦可作為動力輔助的裝置，幫助 完成日常所需的動作。但這些理想化的構想，尚須研究者的不懈努力，期 許未來真能有實踐的一日。

- 71 -

參考文獻

[1] 廖婉彣、林光華、謝好葳、莊覓玲、吳菁宜、林克忠，「機器輔助療 法 於 中 風 復 健 之 成 效 - 隨 機 控 制 試 驗 之 系 統 回 顧 」， FJPT, 35(2):126-138, 2010

[2] Zunt, Dominik. “Who did actually invent the word "robot" and what does it mean? ” The Karel Čapek website. Retrieved 2007-09-11

[3] 德林股份有限公司，http://www.tlco.com.tw

[4] Joel C. Perry, Jacob Rosen, “Upper-Limb Powered Exoskeleton Design”, IEEE/ASME TRANSACTIONS ON MECHATRONICS, VOL. 12, NO.

4, pp.408-417, AUGUST 2007

[5] Ruihua Wei, Mike Perez, Ben Shepard, Edward Koeneman, James Koeneman, and Jiping He, “RUPERT: An Exoskeleton Robot for Assisting Rehabilitation of Arm Functions”, Virtual Rehabilitation IEEE, Vancouver, BC, pp. 163 – 167, Aug. 2008

[6] R. A. R. C. Gopura and Kazuo Kiguchi, “Mechanical Designs of Active Upper-Limb Exoskeleton Robots State-of-the-Art and Design Difficulties”, IEEE 11th International Conference on Rehabilitation Robotics, Japan, pp. 178-187, 2009

[7] R. A. R. C. Gopura, Kazuo Kiguchi, D. S. V. Bandara, “A Brief Review on Upper Extremity Robotic Exoskeleton Systems”, 2011 6th International Conference on Industrial and Information Systems(ICIIS ), Kandy ,pp.346-351, Aug. 2011

[8] S. Moubarak, M. T. Pham, R. Moreau and T. Redarce, “Gravity compensation of an upper extremity exoskeleton”, 32nd Annual International Conference of the IEEE EMBS Buenos Aires, Argentina,

- 72 -

pp.4489-4493, August 31 - September 4, 2010

[9] Thunyanoot Prasertsakul, Teerapong Sookjit, and Warakorn Charoensuk,

“Design of Exoskeleton Arm For Enhancing Human Limb Movement”, Proceedings of the 2011 IEEE International Conference on Robotics and Biomimetics, Karon Beach, Phuket, pp.2199-2203, December 7-11, 2011

[10] G.R. Johnson and M.A. Buckley, “Development of a new Motorised Upper Limp Orthotic System (MULOS)”, In Proceedings of the Rehabilitation Engineering Society of North America. Pittsburgh, PA, pp.

399–401, 1997.

[11] S.S. Marchese, M.A. Buckley, R. Valleggi and G.R. Johnson, “An optimized design of an active orthosis for the shouder—an iterative approach”, International Conference on RehabilitationRobotics, Stanford, CA, 1997.

[12] A. Yardley, G. Parrini, D. Carus and J. Thorpe, “Development of an upper limb orthoticexercise system,”In International Conference on Rehabilitation Robotics, Stanford, CA, 1997.

[13] C.G. Burgar, P.S. Lum, P.C. Shor and H.F. Machiel Van der Loos,

“Development of robots for rehabilitation therapy: The Palo Alto VA/Stanford experience,”Journal of Rehabilitation Research and Development, 37(6), p663 , 2000.

[14] T. Nef, M. Mihelj, G. Colombo and R. Riener, “ARMin-Robot for Rehabilitation of the Upper Extremities”, in Proc. IEEE Int. Conf. on Robotics and Automat., Orlando, USA, pp. 3152-3157, 2006.

[15] K. Nagai, et al., “Development of an 8 dof robotic orthosis for assisting

- 73 -

human upper limb motion”, In Proceedings of the IEEE International Conference on Robotics and Automation, Leuven, vol.4, pp. 3486-3491, 16-20 May1998.

[16] Julius Klein, Steve Spencer, J. Allington, James E. Bobrow, and David J.

Reinkensmeyer, “Optimization of a Parallel Shoulder Mechanism to Achieve a High-Force, Low-Mass, Robotic-Arm Exoskeleton”, IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 4, pp.710-715, AUGUST 2010

[17] C.-P. Chou and B. Hannaford, “Measurement and modeling of McKibben pneumatic artificial muscles”, in Proc. IEEE Robotics and Automation, vol.12(1), pp. 90–102. , 1996.

[18] Hiroshi Kobayashi and Hirokazu Nozaki, “Development of Support System for Forward Tilting of the Upper Body”, Proceedings of 2008 IEEE International Conference on Mechatronics and Automation, Takamatsu , pp.352-356, Aug. 2008

[19] Takamitsu Aida, Hirokazu Nozaki and Hiroshi Kobayashi,

“Development of Muscle Suit and Application to Factory Laborers”, Mechatronics and Automation, 2009. ICMA 2009. International

“Development of Muscle Suit and Application to Factory Laborers”, Mechatronics and Automation, 2009. ICMA 2009. International

在文檔中 McKibben 人工肌肉上肢外骨架機器人之設計 (頁 56-0)