Dong Luc Hoc

Đồ án tốt nghiệp

Động lực lực học và và điều khiển khiển rôbôt rôbôt Scara Scara

CHƯƠNG III

ĐỘNG LỰC HỌC VÀ ĐIỀU KHIỂN ROBOT SCARA 1. CƠ SỞ LÝ THUYẾT 1.1. Biểu thức động năng và thế năng của rôbôt công nghiệp xác định từ các ma trận Denavit – Hartenberg 1.1.1. Biểu thức động năng Xét mô hình rôbôt có n bậc tự do, ma trận Denav trận Denavit it – Harten Hartenberg berg chuyển chuyển toạ độ một điểm từ hệ qui chiếu R i về hệ qui chiếu R 0 như sau:  0 A i pi  Ti = Di =  0 1   

(3.1) zi-1

Khâu i xi-1

Ci yi-1 yi

zi Oi

xi

z0

x0

Khâu n

y0

Hình 3.1 Gọi sCi là véc tơ vị trí trọng tâm của khâu thứ i đối với hệ toạ độ khớp đặt tại khớp thứ i. Khi đó vị trí trọng tâm khâu thứ i đối với hệ toạ độ cơ sở được xác định theo công thức sau r Ci = pi + sCi

(3.2)

Viết dưới dạng ma trận ta có 84



rCi = pi + Ai sC( ii)

(3.3)

Suy ra vận tốc trọng tâm khâu thứ i vCi = r&Ci = p&i + A&i sC(ii)

(3.4)

Toán tử sóng biểu diễn vận tốc góc khâu thứ i đối với hệ toạ độ cơ sở, theo công thức (2.1) trang 118, [1] ω%i( 0) = ω%i = A&i A iT

(3.5)

 0 −ωiz −ω iy    ω%i =  ωiz 0 −ω ix  ω iy ω ix 0 

(3.6)

Suy ra vận tốc góc khâu thứ i trong hệ toạ độ cơ sở ωi = ωix ωiy ω iz 

T

(3.7)

Biểu thức động năng của khâu thứ i 1 1 Ti = mi vTCi vω Ci +I ω 2 2

T i i

(3.8)

i

Trong đó mi : khối lượng khâu thứ i ICi : ma trận của tenxơ quán tính của khâu tứ i đối với khối tâm Ci của nó ở trong hệ toạ độ R 0 Biểu thức động năng của hệ n vật rắn n

1 T = Ti = 2 i =1

∑

n

1 n m v vω +I ω 2 i =1 i =1

∑

T i Ci Ci

∑

T i i

i

(3.9)

1.1.2. Biểu thức thế năng Biểu thức thế năng của khâu thứ i Π i = mi gT rCi

(3.10)

Trong đó g =  g x

gy

g z 

T

(3.11)

85



rCi = pi + Ai sC( ii)

(3.3)

Suy ra vận tốc trọng tâm khâu thứ i vCi = r&Ci = p&i + A&i sC(ii)

(3.4)

Toán tử sóng biểu diễn vận tốc góc khâu thứ i đối với hệ toạ độ cơ sở, theo công thức (2.1) trang 118, [1] ω%i( 0) = ω%i = A&i A iT

(3.5)

 0 −ωiz −ω iy    ω%i =  ωiz 0 −ω ix  ω iy ω ix 0 

(3.6)

Suy ra vận tốc góc khâu thứ i trong hệ toạ độ cơ sở ωi = ωix ωiy ω iz 

T

(3.7)

Biểu thức động năng của khâu thứ i 1 1 Ti = mi vTCi vω Ci +I ω 2 2

T i i

(3.8)

i

Trong đó mi : khối lượng khâu thứ i ICi : ma trận của tenxơ quán tính của khâu tứ i đối với khối tâm Ci của nó ở trong hệ toạ độ R 0 Biểu thức động năng của hệ n vật rắn n

1 T = Ti = 2 i =1

∑

n

1 n m v vω +I ω 2 i =1 i =1

∑

T i Ci Ci

∑

T i i

i

(3.9)

1.1.2. Biểu thức thế năng Biểu thức thế năng của khâu thứ i Π i = mi gT rCi

(3.10)

Trong đó g =  g x

gy

g z 

T

(3.11)

85



Với gx, gy, gz là các thành phần của véctơ gia tốc trọng trường so với hệ toạ độ cơ sở. Nếu hướng của gia tốc trọng trường trùng z0 thì gz = g, còn gx = gy = 0 . Biểu thức tổng thế năng của hệ n khâu n

n

i =1

i =1

Π = ∑ Π i = ∑ mi gT rCi

(3.12)

1.2. Thiết lập phương trình vi phân chuyển động của rôbôt công nghiệp bằng phương pháp Lagrange loại hai Oi

Ci Khâu i

On Khâu n P

z0

O0

x0

Khớp 1 y0

Hình 3.2 Xét một hệ gồm n bậc tự do, ta sẽ dựa vào khái niệm ma trận Jacobi , thiết lập dạng thức Lagrange loại hai cho hệ nhiều vật hôlônôm. hôlônôm. Ký hiệu các toạ độ suy rộng tối thiểu là q = [q1, …, qn]T

(3.13)

Vị trí của vật rắn B i được xác định bởi vị trí của điểm định vị Ci và ma trận côsin chỉ hướng Ai của nó. rci = rci(q,t) ; Ai = Ai(q,t)

(3.14)

Để đơn giản, sau đây ta chỉ xét hệ hôlônôm giữ và dừng. Khi đó ta sẽ có rci = rci(q) ; Ai = Ai(q)

(3.15) 86


Động lực học và điều khiển rôbôt Scara

Trạng thái vận tốc của vật rắn B i được xác định bởi vận tốc khối tâm và vận tốc góc của nó. vCi =

d rCi ∂rCi q = dt ∂q

ω%i = A&i AT i

;

(3.16)

Ma trận Jacôbi tịnh tiến và ma trận Jacôbi quay như sau JTi =

∂rCi ∂q

J Ri =

;

∂ω i ∂q&

(3.17)

Công thức vận tốc khối tâm vật rắn và vận tốc góc của nó có dạng v Ci = J Ti ( q ) q&

ω i = J Ri ( q ) q&

;

(3.18)

Như phần trên ta có biểu thức động năng của khâu thứ i như sau 1 1 Ti = 2 mi vTCi vω Ci +I ω 2

T i i

(3.19)

i

Biểu thức tổng động năng của hệ n khâu như sau n

1 T = Ti = 2 i =1

∑

n

1 n m v vω +I ω 2 i =1 i =1

∑

∑

T i Ci Ci

T i i

(3.20)

i

Với Ii là ma trận tenxơ quán tính của vật rắn thứ i đối với khối tâm C i của nó trong hệ qui chiếu R 0. Trong thực tế ta có thể tính Ii theo công thức sau: Ii = AiIi(i)AiT n

Việc tính biểu thức n

∑ω i =1

∑ i =1

(3.21)

ωT iI i ωi có thể được thay bằng tính

n

∑ω A I i =1

T i

(i ) i i

AT i ωi hay

( i )T (i ) i i

I ωi(i ) . Trong đó, Ii(i) là ma trận của tenxơ quán tính khối của vật rắn thứ

i đối với khối tâm C i của nó ở trong hệ qui chiếu động C ixiyizi gắn liền vào vật rắn Bi, còn ωi(i ) là véc tơ đại số của

ω i

trong hệ qui chiếu C ixiyizi .

Kết hợp giữa (3.18) và (3.20), biểu thức động năng của hệ có thể được tính theo công thức sau

87


Động lực học và điều khiển rôbôt Scara n

1 T= 2

1 mi ( JTi q&) J Ti q&+ 2 i =1

∑

T

n

∑( J i =1

T

q ) Ii J Ri q

Ri &

1 Tn  T = 2 q&  ( mi JTiT JTi + J TRi Ii J Ri )  q  i =1 

∑

hay

(3.22) (3.23)

Ta đưa vào ký hiệu M(q) =

n

∑( m J

T i Ti Ti

i =1

J + J TRi Ii J Ri )

(3.24)

Biểu thức (3.24) gọi là ma trận khối lượng suy rộng. Khi đó biểu thức động năng của hệ vật rắn có dạng như sau 1 T = 2 q&T M ( q ) q

(3.25)

 m11 (q ) ... m1n (q )    M(q) =  ... ... ...  mn1 (q) ... mnn (q )

(3.26)

 q1   q&1   q&&1  q =    ; q&=  M ; q&&=  M q n   q&n  q&&n 

(3.27)

Trong đó

Biểu thức động năng (3.25) có thể viết lại như sau 1 T= 2

n

n

∑∑ m

jk

j

(q)q&& j qk

k

(3.28)

Đạo hàm biểu thức động năng (3.28) theo các vận tốc suy rộng q i và các toạ độ suy rộng qi ta sẽ có ∂T n & = mij q j ∂q&i ∑ j =1

;

∂T 1 n n ∂m jk   q j q k = q ∂qi 2 ∑∑ ∂ j =1 k =1 i

(3.29)

Từ đó suy ra n n ∂m d  ∂T  n ij     m q q j q k = + ij j dt  ∂q i  q ∂ j =1 k =1 k  j =1

∑

∑∑

(3.30)

Thế các biểu thức trên vào phương trình Lagrange loại 2 88



d  ∂T  ∂T ∂Π   − = − + Qinp  dt  ∂q i  ∂qi ∂qi

(i = 1,...,n)

(3.31)

ta có n

∑ j =1

mij q&&j +

 ∂mij 1 ∂m jk  q + g ( q ) = τ i ( t ) (i = 1,...,n) −   q&& ∑∑ 2 ∂qi  j k i j =1 k =1  ∂qk n

n

(3.32)

Trong đó g i ( q ) =

∂Π ∂qi

là các lực có thế

Qinp = −QRi ( q, q&) + τ i ( t ) là các lực không thế tác dụng vào khâu thứ i Q Ri ( q, q&) là lực suy rộng của các lực hao tán tác dụng vào khâu i τ i ( t )

lực suy rộng của các lực phát động tác dụng vào khâu thứ i

Đưa vào ký hiệu hijk ( q ) =

∂mij 1 ∂m jk 1  ∂mij ∂mik ∂mkj  − =  + −  ∂qk 2 ∂qi 2  ∂qk ∂q j ∂qi  

(3.33)

thì phương trình (3.32) trở thành n

∑ j =1

& mij q& j +

n

n

∑∑ h j =1 k =1

ijk

( q ) q&& j qk + gi ( q ) = τ i ( t )

(i = 1,...,n)

(3.34)

Ký hiệu n

∑h

cij ( q, q&) =

k =1

q

ijk & k

(3.35)

thì ta có phương trình sau n

∑ j =1

& mij q& j +

n

n

∑∑ c ( q, q&) q& + g ( q ) = τ ( t ) j =1 k =1

ij

j

i

i

(i = 1,...,n)

(3.36)

Dùng ký hiệu ma trận M(q) = [mij(q)] C ( q, q&) = [cij( q, q )]

Hệ phương trình vi phân chuyển động được viết lại như sau

89



M(q) q& + C ( q, q&) q + g(q) = τ(t)

(3.37)

1.3. Giải bài toán động lực học thuận của rôbôt công nghiệp Bài toán đặt ra là: khi cho biết các lực phát động τ i(t) tác dụng vào các khớp, các thông số động học của rôbôt đã biết trước, ta cần tìm qui luật chuyển động của các khớp tương ứng. Giả thiết là ta đã biết điều kiện đầu của bài toán: q ( 0 ) = q0 ; q&( 0 ) = q&0 , giải hệ phương trình (3.37) bằng phương pháp số ta sẽ tìm

được qui luật chuyển động của các khớp q(t) trong khoảng thời gian [0,T]. Hệ phương trình (3.37) có thể được viết lại như sau −1 &= Mτ ( Cq q& g− ) − &

(3.38)

Đây là hệ phương vi phân cấp hai. Ta có thể giải trực tiếp hệ phương trình trên bằng phương pháp Runge-Kutta-Nyström như sau −1 &= f (t ,q ,q &) = Mτ ( Cq q& g− ) − &

(3.39)

Với điều kiện đầu: q ( 0 ) = q0 ; q&( 0 ) = q&0

(3.40)

Nystöm đưa ra công thức tính như sau h qi +1 = qi + hq&i + ( k1 + k 2 + k 3 ) 3

(3.41)

1 q&i +1 = q&i + ( k1 + 2 k2 + 2k 3 + k 4 ) 3

(3.42)

Bốn vectơ hàm k i được xác định là giá trị của f ( t , q, q&) tại bốn vị trí trung gian trong khoảng tích phân [t i,ti + h] của mỗi bước tính. h k1 = f ( t i , qi , q&i ) 2

(3.43)

h h h k 2 = f  t i + , qi + q&i , q&i + k1   2  2 2 

(3.44)

90



h  h h h  k 3 = f  t i + , qi + q&i + k1 , q&i + k 2  2  2 2 4 

(3.45)

h k 4 = f ( ti + h, qi + hq&i + hk 3 , q&i + 2k 3 ) 2

(3.46)

Chú ý: ta có thể biến đổi hệ n phương trình vi phân cấp 2 (3.39) về hệ 2n phương trình vi phân cấp một rồi giải bằng phương pháp Runge-Kutta cấp 4. 1.4. Giải bài toán động lực học ngược của rôbôt công nghiệp Bài toán động lực học ngược có ý nghĩa rất quan trọng trong lập trình và điều khiển rôbôt. Có hai bài toán là: bài toán động lực học ngược trong không gian khớp và bài toán động lực học ngược trong không gian thao tác. Việc giải bài toán động lực học ngược trong không gian thao tác sẽ khó khăn hơn, nhưng rất gần với yêu cầu thực tế đặt ra.

( t ) q1 ( t ) , ...,ĐLH q nNGƯỢC Bài toán ĐLH ngược trong không gian khớp

( t ) x1 ( t ) , ... , x ĐHmNGƯỢC

q1 ĐLH NGƯỢC

( t ) , ... , q n ( t )

Bài toán ĐLH ngược trong không gian thao tác Khi đã biết được qui luật thay đổi của

qi ( t )

ta đạo hàm cấp 1, cấp 2 chúng để

tìm các vận tốc khớp q i ( t ) và gia tốc khớp qi ( t ) tương ứng rồi thay vào hệ phương trình (3.37) ta sẽ tìm được các lực phát động tác dụng vào các khớp. 1.5. Thuật toán điều khiển trượt rôbôt công nghiệp Nhiệm vụ của bài toán điều khiển là tìm qui luật của lực/mômen do các động cơ điện tạo ra tác dụng lên các khâu để đảm bảo rôbôt chuyển động theo qui luật qd(t) cho trước, nhằm thực hiện một nhiệm vụ nào đó. Trên cơ sở chuyển động mong muốn qd(t) được định nghĩa trước và chuyển động hiện tại của rôbôt được đo bởi các cảm biến đặt tại các khớp, bộ điều khiển có nhiệm vụ 91



đưa ra các lực/mômen cần thiết. Các lực/mômen này tác động làm cho rôbôt thực hiện chuyển động mong muốn một cách ổn định và chính xác. Sơ đồ khối của một bộ điều khiển cho rôbôt có dạng như hình vẽ 3.3 . d Bộ điều khiển

qd(t)

τ

+

Rôbôt

q(t)

Hình 3.3. Sơ đồ điều khiển của rôbôt Để có được luật điều khiển đáp ứng các yêu cầu vừa nêu, thông thường luật điều khiển dựa trên động lực học ngược được sử dụng. Với luật điều khiển này lực/mômen của bộ phận dẫn động được tính như sau: τ = M ( q ) ν + C ( q, q&) q&+ g ( q )

(3.47)

Trong đó & &− q &d ) − K p ( q − q d ) ν = q& d − Kd ( q

(3.48)

Thay (3.47) và (3.48) vào phương trình vi phân chuyển động của rôbôt (3.49) M ( q ) q&&+ C ( q, q&) q&+ g ( qτ) =

(3.49)

&− q & & &− q &d ) + K p ( q − q d )  = 0 M ( q ) ( q& d ) + Kd ( q 

(3.50)

ta được Do ma trận khối lượng M(q) là ma trận xác định dương, nên từ hệ thức (3.50) ta có ( q&&− q&&d ) + K d ( q&− q&d ) + K p ( q − q d ) = 0

(3.51)

Nếu đặt e(t) = q(t) – qd(t) là sai số giữa chuyển động thực của rôbôt và chuyển động mong muốn, ta có

92



= K d e&+ K pe

& e&

(3.52)

Các ma trận K d và K p cần phải chọn sao cho nghiệm e(t) của phương trình vi phân (3.52) tiến tới không khi thời gian t tăng đủ lớn. Nếu chọn K d và K p là các ma trận đường chéo thì phương trình (3.52) tương đương với n phương trình vi phân cấp hai độc lập: = kdi e&i + k pi ei = 0

& e& i

(i=1,…,n)

(3.53)

để thuận tiện cho việc chọn thông số ta đặt k pi = ω i2

k di = 2ζω i

,

Thông thường ta chọn cản tới hạn quá không cần thiết. Giá trị cơ, nếu

ω i

ω i

(3.54) ζ = 1

, để tránh những chuyển động vượt

phụ thuộc vào công suất/mômen cực đại động

nhỏ thì thời gian chuyển tiếp lớn, ngược lại nếu

ω i

lớn thì thời gian

chuyển tiếp nhỏ. Khi áp dụng luật điều khiển (3.47) thì ta cần phải biết chính xác các tham số hệ có mặt trong các đại lượng M(q), C ( q, q&) và g(q) như khối lượng, vị trí khối tâm, mômen quán tính khối,… Việc xác định các tham số này không đơn giản, đặc biệt khi rôbôt đã được lắp đặt sẵn. Để tránh những khó khăn này, ở đây một phương pháp điều khiển bền vững được khai thác sử dụng. Nếu đặt s là sai số suy rộng được định nghĩa bởi &( t ) + s = eΛe

(3.55)

( t )

Trong đó Λ = diag { λ1 , λ2 ,..., λn } , λi > 0

(3.56)

e = q ( t ) + q d ( t )

(3.57)

Rõ ràng khi si = 0 thì ei = e ( 0 ) e−λ t sẽ tiến về 0 khi thời gian t tăng đủ lớn. Bây i

giờ ta tìm luật điều khiển để đảm bảo si = 0 hoặc si tiến về 0. &d ( t ) − Đặt q&r = qΛe

( t ) ta có

93



& s = q&( t ) − q&r ( t ) ==> s&= q&&( t ) − q& r ( t )

(3.58)

Để tìm luật điều khiển ta chọn hàm Lyapunov như sau 1 & V = sT M ( q) s 2

(3.59)

Đạo hàm V theo thời gian t ta được &= sT M ( q ) s&+ V

1 2

T & s M ( q) s

(3.60)

Từ (3.49) và chú ý đến (3.58) ta có &= C− q ( q , q &) & M ( q ) sτ g− q ( )M − q ( q ) &&r ( t )d−

(3.61)

Cũng từ (3.58) ta suy ra q&( t ) = s + q&r ( t ) , nên ta có C ( q, q&) q&= C ( q, q&) s + C ( q, q&) q&r

Cuối cùng ta nhận được &= C− q ( q , s&) M M ( q ) sτ − q ( q ) &&rC− q (q , q&) &r g− q ( )d−

(3.62)

Thay hệ thức (3.62) vào hệ thức (3.60) nhận được & &) &r g− q ( )d− s+ V &= sτT  M− q ( q ) & rC− q (q , q 

T

M1 &   2 q ( s ) C− q (q , &s) 

(3.63)

1  Lưu ý rằng sT  2 M&( q ) s − C ( q, q&)  s = 0  

Từ (3.63) ta chọn luật điều khiển như sau ˆ ˆ ( q ) q& ˆ ( q ) − K pd s − K s sgn ( s ) & &) q &r + g τ=M r + C ( q, q

(3.64)

Ở đây, sgn(s) = [sgn(s 1), sgn(s2), …, sgn(sn)]T , K pd và K s là các ma trận thực đối xứng xác định dương, K pd = K Tpd > 0 , K s = K T s > 0. Để đơn giản ta chọn hai ma trận này có dạng đường chéo 22 11 22 nn K pd = diag { k 11pd , k pd ,..., k nn pd } và K s = diag { ks , k s ,..., k s }

Với cách chọn (3.64) hệ thức (3.63) trở thành % &) q %( q ) & &r + g % V &= −sT K pd s − sT K s sgn ( s ) − sT d − sT  M q& ( q )  r + C ( q, q

94


Động lực học và điều khiển rôbôt Scara n

n

= −∑ K s − ∑ K i =1

( ii ) 2 pd i

i =1

( ii ) s

si −

n

n

∑ s ( d + ρ ) ≤ − ∑ K i =1

i

i

i

i =1

( ii ) 2 pd i

s −

n

∑( K i =1

( ii ) pd

− di + ρ i ) si

Với các sai lệch giữa mô hình thực sử dụng trong (3.49) và các thông số mô hình sử dụng trong bộ điều khiển (3.64) như sau ˆ ( q) , %( q ) = M ( q ) − M M C%( q, q&) = C ( q, q&) − Cˆ ( q, q&) , g%( q ) = g ( q ) − gˆ ( q ) ,

Và ρ i = M%( q ) q&&r + C%( q, q&) q&r + g%( q ) i Như thế đảm bảo V &≤ 0 thì ta phải chọn các phần tử của K s sao cho K s( ii ) − d i + ρ i > 0 hay K s( ii ) > d i + ρ i > 0

2. THIẾT LẬP PHƯƠNG TRÌNH VI PHÂN CHUYỂN ĐỘNG

95



Hình 3.4 Ta qui ước các thông số của bài toán động lực học robot SCARA như sau: Khối lượng của các khâu 1, 2, 3, 4 tương ứng là m1, m2, m3, m4 . Các mômen quán tính chính của các khâu trong hệ qui chiếu gắn liền vào các khâu là I ix , Iiy , Iiz ( i = 1, 2, 3, 4). Sau đây ta sẽ thiết lập phương trình vi phân chuyển động của robot SCARA 4 bậc tự do loại RRTR bằng phương pháp Lagrange loại 2. Chọn toạ độ suy rộng là: T T q = [θ 1 ,θ 2 , d 3 ,θ 4 ] = [q1 , q 2 , q3 , q 4 ]

Dưới đây là bảng mô tả vị trí trọng tâm, khối lượng và mômen quán tính khối của từng khâu của robot như sau: Vị trí trọng tâm (so với gốc toạ độ gắn Khâu trên mỗi khớp) xC yC zC 1 -(a1-L1) 0 - d1 2 -(a2-L2) 0 0 3 0 0 - L3 4 0 0 - L4

Khối lượng m1 m2 m3 m4

Mômen quán tính khối từng khâu (Tính đối với hệ toạ đặt tại trọng tâm từng khâu và song song với toạ độ khớp) Ixx Iyy Izz Ixy Ixz Izx I1x I1y I1z 0 0 0 I2x I2y I2z 0 0 0 I3x I3y I3z 0 0 0 I4x I4y I4z 0 0 0

Trong phần tính toán động học thuận ta đã tính được các ma trận DH của các khâu đối với hệ qui chiếu R 0 như sau: C 1 − S 1  S C 1 1 D1 =  0 0 0 0 

0 a1C 1  0 a1 S 1  1 d 1   0 1 

96



C 12 S 12 0 a 2 C 12 + a1C 1   S − C 0 a S + a S  12 12 2 12 1 1 D2 =  0  0 d 1 −1 0  0 0 1   C12 S12 0 a2C12 + a1C1   S −C 0 a S + a S  D3 =  012 012 −1 2 d12− q 1 1  1 3 0  0 0 1  

D4 =

C12− 4 S  12−4  0   0

S12−4

−C12− 4 0 0

0

a2 C12 + a1C1 

0

a2 S12 + a1 S1

  −1 d1 − q3 − d 4   0 1 

Từ các ma trận Di ( i = 1, 2, 3, 4) ta dễ dàng xác định được vị trí khối tâm của các khâu tương ứng trong hệ toạ độ R 0. Vị trí khối tâm khâu thứ nhất a1C 1  C 1 − S 1 0 − (a1 − L1 )  L1C 1         rC1 =  a1 S 1  +  S 1 C 1 0  0  =  L1 S 1   d 1   0 0 1  − d 1   0 

Đạo hàm biểu thức trên theo thời gian ở trong hệ qui chiếu cố định ta xác định được vận tốc khối tâm khâu thứ nhất.  − L1q1 sin q1  vC1 = r&C 1 =  L1q1 cos q1    0

Ma trận Jacobi tịnh tiến của khâu thứ nhất − S 1 L1 0 0 0 Jt1 =  C 1 L1 0 0 0  0 0 0 0

Vị trí khối tâm khâu thứ hai

97



a1C 1 + a 2 C 12  C 12 S 12 0  − (a 2 − L2 )      0 rC2 =  a1 S 1 + a 2 S 12  +  S 12 − C 12 0       0  d 1 0 0 − 1  a1C 1 + L2 C 12  rC2 =  a1 S 1 + L2 S 12    d 1

Vận tốc khối tâm khâu thứ hai −a1q&1S1 − L2 q&12S12  vC2 = r&C 2 =  a1q&1C1 + L2 q&12C12    0

Ma trận Jacobi tịnh tiến của khâu thứ hai − a1 S 1 − L2 S 12 − L2 S 12 0 0 Jt2 =  a1C 1 + L2 C 12 L2C 12 0 0  0 0 0 0

Vị trí khối tâm khâu thứ ba a1C 1 + a 2 C 12  C 12 S 12 0   0  rC3 =  a1 S 1 + a 2 S 12  +  S 12 − C 12 0   0   d 1 − q 3   0 0 − 1 − L3  a1C 1 + a 2 C 12    rC3 =  a1 S 1 + a 2 S 12   d 1 − q3 + L3 

Vận tốc khối tâm khâu thứ ba − a1q&1S1 − a2 q&12S12    vC3 = r&C 3 =  a1q&1C1 + a2 q&12C12    −q&3

Ma trận Jacobi tịnh tiến của khâu thứ ba − a1 S 1 − L2 S 12 − L2 S 12 0 0   Jt3 =  a1C 1 + L2 C 12 L2 C 12 0 0  0 0 − 1 0

Vị trí khối tâm khâu thứ tư 98



a1C 1 + a 2 C 12  C 124 S 124 0   0       rC4 =  a1 S 1 + a 2 S 12  +  S 124 − C 124 0   0   d 1 − q3 − d 4   0 0 − 1 − L4   a1C 1 + a 2 C 12  rC4 =  a1 S 1 + a 2 S 12  d 1 − q3 − d 4 + L4 

Vận tốc khối tâm khâu thứ tư −a1q&1S1 − a2 q&12S12  vC4 = r&C 4 =  a1q&1C1 + a2 q&12C12    − q&3

Ma trận Jacobi tịnh tiến của khâu thứ tư − a1 S 1 − L2 S 12 − L2 S 12 0 0 Jt4 =  a1C 1 + L2 C 12 L2 C 12 0 0  0 0 − 1 0

Cũng từ các ma trận Di ( i = 1, 2, 3, 4) ta dễ dàng xác định được các ma trận côsin chỉ hướng của các khâu. Ma trận côsin chỉ hướng của khâu thứ nhất C 1 − S 1 0 A1 =  S 1 C 1 0  0 0 1

Ma trận côsin chỉ hướng của khâu thứ hai C 12 S 12 0  A2 =  S 12 − C 12 0   0 0 − 1

Ma trận côsin chỉ hướng của khâu thứ ba C 12 S 12 0  A3 =  S 12 − C 12 0   0 0 − 1

Ma trận côsin chỉ hướng của khâu thứ tư 99



C12− 4 S 12− 4 0    A4 =  S12−4 −C 12− 4 0   0 0 −1

Toán tử sóng của véc tơ vận tốc góc khâu thứ nhất xác định theo công thức (4.30) trong [1] là  − S1q&1 −C1q&1 0 C1 − S1 0  ω%1 = A&1A1T =  C1q&1 − S1q&1 0  S1 C1 0  0 0 0   0 0 1  0 − q&1 0  ω%1 =  q&1 0 0   0 0 0

Suy ra vận tốc góc khâu thứ nhất trong hệ toạ độ R 0 là ω1 = [ 0 0 q 1 ] T Ma trận Jacobi quay của khâu thứ nhất 0 0 0 0  Jr1 = 0 0 0 0 1 0 0 0

Toán tử sóng của véc tơ vận tốc góc khâu thứ hai  − S12 q&1 − S12 q&2 C12 q&1 + C12 q&2 0   C12 S12 0  ω%2 = A&2 AT 2 =  C12 q&1 + C12 q&2 S12q&1 + S12q&2 0   S12 − C12 0   0 0 0   0 0 −1 −q&1 − q&2 0   0  %2 =  q&1 + q&2 0 0 ω    0 0 0 

Suy ra vận tốc góc khâu thứ hai trong hệ toạ độ R 0 là ω2 = [ 0 0 q 1 + q 2 ] T Ma trận Jacobi quay của khâu thứ hai

100



0 0 0 0    Jr2 = 0 0 0 0 1 1 0 0

Toán tử sóng của véc tơ vận tốc góc khâu thứ ba  − S12 q&1 − S12 q&2 C12 q&1 + C12 q&2 0 C12 S12 0  ω%3 = A&3 AT 3 =  C12 q&1 + C12 q&2 S12q&1 + S12q&2 0  S12 − C12 0   0 0 0  0 0 −1 −q&1 − q&2 0  0  %3 =  q&1 + q&2 0 0 ω    0 0 0

Suy ra vận tốc góc khâu thứ ba trong hệ toạ độ R 0 là ω3 = [ 0 0 q 1 + q 2 ] T Ma trận Jacobi quay của khâu thứ ba 0 0 0 0  Jr3 = 0 0 0 0 1 1 0 0

Toán tử sóng của véc tơ vận tốc góc khâu thứ tư  − S12− 4 q&1 − S12 −4 q&2 + S12 −4 q&4 C12 −4 q&1 +C12 −4q&2 −C12 −4 q&4 0  C12 & AT =  C q& + C q& −C q& S q& +S q& −S q& 0  S %4 = A ω 4 4 12 − 4 2 12 −4 4 12 −4 1 12 −4 2 12 −4 4  12− 4 1   12  0 0 0   0

4− 4 −

S12 4−

0 

−C 12 4− 0  0 − 1 

0 −q&1 − q&2 + q&4 0   ω%4 =  q&1 + q&2 − q&4 0 0   0 0 0 

Suy ra vận tốc góc khâu thứ tư trong hệ toạ độ R 0 là ω4 = [ 0 0 q 1 + q 2 − q 4 ] T Ma trận Jacobi quay của khâu thứ tư

101



0 0 0 0    Jr4 = 0 0 0 0  1 1 0 − 1

Các ma trận mômen quán tính của các khâu, tính đối với các hệ toạ độ đi qua khối tâm tương ứng và có các trục song song với các trục của hệ toạ độ R i Đối với khâu thứ nhất  I 1 x 0 0    I1 =  0 I 1 y 0   0 0 I 1 z 

Đối với khâu thứ hai  I 2 x 0 0    I2 =  0 I 2 y 0   0 0 I 2 z 

Đối với khâu thứ ba  I 3 x 0 0    I3 =  0 I 3 y 0   0 0 I 3 z 

Đối với khâu thứ tư  I 4 x 0 0    I4 =  0 I 4 y 0   0 0 I 4 z 

Ma trận khối lượng xác định theo công thức (4.35) trang 208 trong [1], ta có m11 = m1L12 + m2(a12 + 2L2a1cosq2 + L22) + m3(a12 + 2a1a2cosq2 + a22) + m4(a12 + 2a1a2cosq2 + a22) + I1z + I2z + I3z + I4z m12 = m2(L2a1cosq2 + L22) + m3(a22 + a2a1cosq2) + m4(a22 + a2a1cosq2) + I2z + I3z + I4z m13 = m31 = 0 m14 = m41 = - I4z

102



m21 = m2(L2a1cosq2 + L22) + m3(a22 + a2a1cosq2) + m4(a22 + a2a1cosq2) + I2z + I3z + I4z m22 = m2L22 + m3a22 + m4a22 + I2z + I3z + I4z m23 = m32 = 0 m24 = m42 = - I4z m33 = m3 + m4 m34 = m43 = 0 m44 = I4z Ma trận C ( q, q&) xác định theo công thức (3.35) trong phần lý thuyết, ta có c11 = (- m2L2a1sinq2 - m3a2a1sinq2 - m4a2a1sinq2) q 2 c12 = (- m2L2a1sinq2 - m3a2a1sinq2 - m4a2a1sinq2) q 1 + (- m2L2a1sinq2 m3a2a1sinq2 - m4a2a1sinq2) q 2 c13 = c14 = 0 c21 = (m2L2a1sinq2 + m3a2a1sinq2 + m4a2a1sinq2) q 1 c22 = c23 = c24 = 0 c31 = c32 = c33 = c34 = 0 c41 = c42 = c43 = c44 = 0 Biểu thức thế năng của hệ có dạng: Π = [0 0 g]{m 1rC1 + m2rC2 + m3rC3 + m4rC4 } Π = m2.d1.g + m3.(d1 – q3 + L3).g + m4.(d1 – q3 – d4 + L4).g Đạo hàm biểu thức Π theo q i ta sẽ thu được các lực suy rộng có thế như sau 0     0 g(q) = − m g − m g  3 4   0  

Nếu đưa vào thành phần lực cản nhớt ở các khớp động thì hàm hao tán có dạng: 103



1 Ф = 2 .( b12 q 1 + b22 q 2 + b32 q 3 + b42 q 4 )

Đạo hàm biểu thức Ф theo q i ta sẽ thu được các lực cản nhớt (là thành phần lực không thế) như sau:  b1q1  b q  2 2 Lc = b q  3 3 b q   4 4

Gọi τi là các lực (nếu nó đặt tại khớp tịnh tiến) hoặc ngẫu lực (nếu nó đặt tại khớp quay) trực tiếp làm cho khớp chuyển động. Như vậy ta ký hiệu véc tơ lực động cơ tạo nên dịch chuyển ở các khớp đọng là Lt . Lt =

τ 1  τ   2 τ 3    τ 4 

Sau khi thay vào phương trình (3.37) ở phần lý thuyết ta thu được hệ phương trình vi phân chuyển động của robot như sau: m11 q1 + m12 q2 + m13 q3 + m14 q4 - 2(m2L2a1sinq2 + m3a2a1sinq2 + m4a2a1sinq2) q 1q 2 - (m2L2a1sinq2 + m3a2a1sinq2 + m4a2a1sinq2) q 22 + b1q 1 = τ1

m21 q1 + m22 q2 + m23 q3 + m24 q4 + (m2L2a1sinq2 + m3a2a1sinq2 + m4a2a1sinq2) q 1

2

+ b2 q 2 = τ2 m31 q1 + m32 q2 + m33 q3 + m24 q4 - (m3 + m4)g + b3 q 3 = τ3 m41 q1 + m42 q2 + m43 q3 + m44 q4 + b4 q 4 = τ4 Nếu viết dưới dạng ma trận thì ta thu được hệ phương trình như sau M ( q ) q& + C ( q , q&) q + g(q) + Lc = Lt

(3.47)

3. GIẢI BÀI TOÁN ĐỘNG LỰC HỌC THUẬN Phương trình vi phân chuyển động của rôbôt SCARA, đã tìm được ở trên. 104



M ( q ) q& + C ( q , q&) q + g(q) + Lc = Lt

(3.48)

Khi đã biết các tham số động lực của rôbôt và các lực phát động từ động cơ tác động vào các khớp động là L*t, ta cần giải hệ phương trình (3.48) để tìm các giá trị qi của các khớp. Giải hệ (3.48) bằng phương pháp số, cụ thể là phương pháp Runge–Kutta–Nyström, với các điều kiện đầu

q (0) = q 0 ; q&(0) = q&0

ta sẽ tìm được luật chuyển động của các khớp q(t) trong khoảng thời gian [0 , T]. Các bước thực hiện giống như đã trình bày trong phần lý thuyết ở đầu chương. Sau đây là kết quả mô phỏng số ứng với các thông số động học và động lực học cụ thể. T

 π π π π  Lt =  2sin(π t + ) 2.5sin  π t +  − 20 + 1.5sin  π t +  4sin  π t +   12 4 6 3      q0 = [ −0,35 0 0,15 0,5] q&0 = [ 0

0 0 0]

T

T

Các thông số động học: a1 = 0,425 m ; a2 = 0,375 m ; d1 = 0,16 m ;

d4 = 0,275 m;

L1 = 0,2125 m ; L2 = 0,1875 m ; L3 = 0,15 m ;

L4 = 0,1 m ;

Các thông số động lực học: I1x = 0 ;

I1y = 0,821 ; I1z = 0,821 (kg.m2)

I2x = 0 ;

I2y = 0,136 ; I2z = 0,136 ;

I3x = 0 ;

I3y = 0

I4x = 0 ;

I4y = 0 ;

I4z = 0,3181 ;

g = 9,81 m/s 2

b1 = 0,5 ;

b2 = 0,5 ;

b3 = 0,5 ;

b4 = 0,5 ;

m3 = 1,5 kg ;

m4 = 0,5 kg ;

; I3z = 0,008 ;

m1 = 10 kg ; m2 = 5 kg ;

Kết quả mô phỏng số trong khoảng thời gian [0 , 2,5] giây như sau:

105



106



107



4. GIẢI BÀI TOÁN ĐỘNG LỰC HỌC NGƯỢC Cho bàn kẹp di chuyển theo qui luật sau, xét trong khoảng thời gian T = 4 s   5π t   0,4 0,15.cos +  6         x p    5π t    y   0,35 + 0,05.sin  6      x(t) =  z p  =   3π t   p  ϕ   −0,415 − 0,1.sin     4       t    − 32 sin  2π   3  −e    

Sau khi giải bài toán động học ngược, ta sẽ tìm được q(t), đạo hàm q(t) theo t ta sẽ tìm được vận tốc, gia tốc tương ứng của các biến khớp. Thay vào phương trình vi phân chuyển động (3.48) ta sẽ tìm được Lt . Tiến hành mô phỏng số ta được:

108



109



110



111



112



5. ĐIỀU KHIỂN RÔBÔT SCARA Ở phần trên ta đã thiết lập được hệ phương trình vi phân chuyển động của rôbôt với các ma trận như sau Ma trận khối lượng M(q) = [mij(q)] với (i,j = 1..4) có các thành phần như sau m11 = m1L12 + m2(a12 + 2L2a1cosq2 + L22) + m3(a12 + 2a1a2cosq2 + a22) + m4(a12 + 2a1a2cosq2 + a22) + I1z + I2z + I3z + I4z m12 = m2(L2a1cosq2 + L22) + m3(a22 + a2a1cosq2) + m4(a22 + a2a1cosq2) + I2z + I3z + I4z m13 = m31 = 0 m14 = m41 = - I4z m21 = m2(L2a1cosq2 + L22) + m3(a22 + a2a1cosq2) + m4(a22 + a2a1cosq2) + I2z + I3z + I4z m22 = m2L22 + m3a22 + m4a22 + I2z + I3z + I4z m23 = m32 = 0 m24 = m42 = - I4z m33 = m3 + m4 m34 = m43 = 0 113



m44 = I4z Ma trận C ( q, q&) = [cij ( q, q&) ] với (i,j = 1..4) có các thành phần như sau c11 = (- m2L2a1sinq2 - m3a2a1sinq2 - m4a2a1sinq2) q 2 c12 = (- m2L2a1sinq2 - m3a2a1sinq2 - m4a2a1sinq2) q 1 + (- m2L2a1sinq2 m3a2a1sinq2 - m4a2a1sinq2) q 2 c13 = c14 = 0 c21 = (m2L2a1sinq2 + m3a2a1sinq2 + m4a2a1sinq2) q 1 c22 = c23 = c24 = 0 c31 = c32 = c33 = c34 = 0 c41 = c42 = c43 = c44 = 0 Ma trận g(q) như sau T g(q) = [ 0 0 −m3 g − m4 g 0]

Mômen/lực động cơ tác động vào các khớp động τ = [ τ 1 τ 2 τ 3 τ 4 ]

T

Để thực hiện việc mô phỏng số ta chọn trước chuyển động mong muốn của rôbôt trong toạ độ khớp như sau q1d = 2cos(2t)

;

q2d = 2sin(2t) ;

q3d = 0,3 + 0,1sin(2t) ;

q4d = sin(3t) ;

Tính các vận tốc, gia tốc biến khớp q&1d = −4sin ( 2t )

;

q&& 1d = − 8cos ( 2t )

q&2 d = 4cos ( 2t )

;

& q& 2 d = −8sin ( 2t )

q&3d = 0, 2cos ( 2t )

;

& q& 3 d = −0, 4sin ( 2t )

q&4 d = 3cos ( 3t )

;

& q& 4 d = −9sin ( 3t )

Sử dụng phần mềm maple ta có kết quả mô phỏng cho hai trường hợp có nhiễu và không có nhiễu như sau

114



Trường hợp 1: không có nhiễu tác dụng lên hệ , d = 0, và các thông số của hệ được xác định chính xác. Trong trường hợp này, các thông số của bộ điều khiển được chọn như sau Λ = diag { 20, 20, 20, 20} ; K p = diag { 30, 30, 30, 30} ; K s = diag { 50, 50, 50, 50}

Đồ thị các toạ độ khớp q1 , q2 , q3 , q4 theo thời gian (nét liền - chuyển động thực, nét đứt - chuyển động mong muốn)

115



Đồ thị s1 , s2 , s3 , s4 theo thời gian Từ các hình vẽ trên ta thấy rằng chỉ sau gần 1 s thì chuyển động của rôbôt đã đạt được tới chuyển động mà ta mong muốn. Các sai số suy rộng là khá nhỏ. Trường hợp 2: có nhiễu d(t) tác dụng lên hệ , và các thông số của hệ được xác định ước lượng với sai số 10%. Trong trường hợp này, các thông số của bộ điều khiển được chọn như sau Λ = diag { 5, 5, 5, 5}

; K p = diag { 30, 30, 30, 30}

K s = diag { 50, 50, 50, 50}

116



Đồ thị các toạ độ khớp q1 , q2 , q3 , q4 theo thời gian (nét liền - chuyển động thực, nét đứt - chuyển động mong muốn)

Đồ thị s1 , s2 , s3 , s4 theo thời gian 6. CHƯƠNG TRÌNH MAPLE VÀ MATLAB 6.1. Sử dụng chương trình Maple thiết lập phương trình vi phân chuyển động cho mô hình rôbôt SCARA > restart; with(linalg): > # Ki hieu cac toa do suy rong,van toc suy rong va gia toc suy rong cua bien khop n:=4: q

:=matrix(4,1,[q1,q2,q3,q4]);

117


Động lực học và điều khiển rôbôt Scara éq1 ù ê ú êq2 ú q := ê ú êq3 ú ê ê ú ú ëq4 û

> qd :=matrix(4,1,[qd1,qd2,qd3,qd4]); éqd1 ù ê ú êqd2 ú ú qd := ê êqd3 ú ê ú ê ú ëqd4 û

> qdd:=matrix(4,1,[qdd1,qdd2,qdd3,qdd4]); éqdd1 ê êqdd2 qdd := ê êqdd3 ê ê ëqdd4

> # Xac dinh cac matran cosin chi huong Ai cua cac khau tu cac matran Denavit-Hatenberg > A1:=matrix(3,3,[cos(q1),sin(q1),0,sin(q1),cos(q1),0,0,0,1]); écos( q1 ) K sin ( q1 ) ê A1 := ê ê sin ( q1 ) cos ( q1 ) ê 0 0 ë

0 0 1

> A2:=matrix(3,3,[cos(q1+q2),sin(q1+q2),0,sin(q1+q2),cos(q1+q2),0,0,0,-1]); 0 ù éco s ( q1 C q2 ) sin ( q1 C q2 ) ê ú ê A2 := êsin ( q1 C q2 ) K cos( q1 C q2 ) 0 ú ú ê ú 0 0 K 1 û ë

> A3:=matrix(3,3,[cos(q1+q2),sin(q1+q2),0,sin(q1+q2),cos(q1+q2),0,0,0,-1]);

118


Động lực học và điều khiển rôbôt Scara éco s ( q1 C q2 ) sin ( q1 C q2 ) 0 ù ê ú ê A3 := êsin ( q1 C q2 ) K cos( q1 C q2 ) 0 ú ú ê ú 0 0 K 1 û ë

> A4:=matrix(3,3,[cos(q1+q2-q4),sin(q1+q2q4),0,sin(q1+q2-q4),-cos(q1+q2-q4),0,0,0,-1]); 0 ù éc o s ( q1 C q2 K q4 ) sin ( q1 C q2 K q4 ) ê ú ê A4 := ê sin ( q1 C q2 K q4 ) K cos( q1 C q2 K q4 ) 0 ú ú ê ú 0 0 K 1 û ë

> # Tinh toa do khoi tam cac khau trong he toa do dong rC11:=matrix(3,1,[-(a1-L1),0,-d1]); éK a1 C L1 ù ê ú ê ú rC11 := ê 0 ú ê ú ë K d1 û

> rC22:=matrix(3,1,[-(a2-L2),0,0]); éK a2 C L2 ù ê ú ê ú rC22 := ê 0 ú ê ú 0 ë û

> rC33:=matrix(3,1,[0,0,-L3]); é 0 ê rC33 := ê ê 0 ê ëK L3

> rC44:=matrix(3,1,[0,0,-L4]); é 0 ê rC44 := ê ê 0 ê ëK L4

> # Vi tri goc toa do cua cac he qui chieu R i trong he qui chieu R0 > r1:=matrix(3,1,[a1*cos(q1),a1*sin(q1),d1]);

119


Động lực học và điều khiển rôbôt Scara éa1 cos ( q1 ) ù ê ú ê r1 := êa1 sin ( q1 ) ú ú ê ú d1 ë û

>r2:=matrix(3,1, [a2*cos(q1+q2)+a1*cos(q1),a2*sin(q1+q2)+a1*sin(q1),d1]); éa2 c o s ( q1 C q2 ) C a1 cos( q1 ) ù ê ú ê r2 := ê a2 sin ( q1 C q2 ) C a1 sin ( q1 ) ú ú ê ú d1 ë û

>r3:=matrix(3,1, [a2*cos(q1+q2)+a1*cos(q1),a2*sin(q1+q2)+a1*sin(q1),d1q3]); éa2 c o s ( q1 C q2 ) C a1 cos( q1 ) ù ê ú ê r3 := ê a2 sin ( q1 C q2 ) C a1 sin ( q1 ) ú ú ê ú d1 K q3 ë û

>r4:=matrix(3,1, [a2*cos(q1+q2)+a1*cos(q1),a2*sin(q1+q2)+a1*sin(q1),d1q3-d4]); éa2 c o s ( q1 C q2 ) C a1 cos( q1 ) ù ê ú ê r4 := ê a2 sin ( q1 C q2 ) C a1 sin ( q1 ) ú ú ê ú d1 K q3 K d4 ë û

> # Tinh vi tri khoi tam cua cac khau trong he qui chieu R0 > rC1:=simplify(evalm(r1+A1&*rC11)); éc o s ( q1 ) L1 ù ê ú ê rC1 := êsin ( q1 ) L1 ú ú ê ú 0 ë û

> rC2:=simplify(evalm(r2+A2&*rC22));

120


Động lực học và điều khiển rôbôt Scara éa1 c o s ( q1 ) C co s ( q1 C q2 ) L2 ê rC2 := ê ê a1 sin ( q1 ) C sin ( q1 C q2 ) L2 ê d1 ë

> rC3:=simplify(evalm(r3+A3&*rC33)); éa2 c o s ( q1 C q2 ) C a1 c o s ( q1 ) ê rC3 := ê ê a2 sin ( q1 C q2 ) C a1 sin ( q1 ) ê d1 K q3 C L3 ë

> rC4:=simplify(evalm(r4+A4&*rC44)); éa2 c o s ( q1 C q2 ) C a1 c o s ( q1 ) ê rC4 := ê ê a2 sin ( q1 C q2 ) C a1 sin ( q1 ) ê d1 K q3 K d4 C L4 ë

> # Tinh cac matran jacobian tinh tien cua cac khau > JacobiTinhTien:=proc(rC::matrix) local Jt,i,j; Jt:=matrix(3,n,0): for i from 1 by 1 to 3 do for j from 1 by 1 to n do Jt[i,j]:=diff(rC[i,1],q[j,1]); od;od; return(evalm(Jt)); end: > Jt1:=JacobiTinhTien(rC1); éK sin ( q1 ) L1 ê Jt1 := ê ê cos( q1 ) L1 ê 0 ë

0

0

0

0

0

0

0

0

0

> Jt2:=JacobiTinhTien(rC2);

121



éK a1 sin ( q1 ) K sin ( q1 C q2 ) L2 K sin ( q1 C q2 ) L2 ê Jt2 := ê ê a1 c o s ( q1 ) C co s ( q1 C q2 ) L2 co s ( q1 C q2 ) L2 ê 0 0 ë

0

0

0

0

0

0

> Jt3:=JacobiTinhTien(rC3); éK a2 sin ( q1 C q2 ) K a1 sin ( q1 ) K a2 sin ( q1 C q2 ) 0 ê 0 Jt3 := ê ê a2 c o s ( q1 C q2 ) C a1 c o s ( q1 ) a2 cos( q1 C q2 ) ê 0 0 K 1 ë

0 ù

ú 0 ú ú ú 0 û

> Jt4:=JacobiTinhTien(rC4); éK a2 sin ( q1 C q2 ) K a1 sin ( q1 ) K a2 sin ( q1 C q2 ) 0 ê 0 Jt4 := ê ê a2 c o s ( q1 C q2 ) C a1 c o s ( q1 ) a2 cos( q1 C q2 ) ê K 1 0 0 ë

0 ù

ú 0 ú ú ú 0 û

> # Tinh cac vector van toc goc cua cac khau > Tinh_omega:=proc(A) local Ad,i,j,k,omega,omega_song; Ad:=matrix(3,3,0): for i from 1 by 1 to 3 do for j from 1 by 1 to 3 do for k from 1 by 1 to n do Ad[i,j]:=Ad[i,j]+ diff(A[i,j],q[k,1])*qd[k,1]; od; od; od; omega_song:=simplify(evalm(Ad&*transpose(A))); omega:=vector([-omega_song[2,3],omega_song[1,3],omega_song[1,2]]); return(evalm(omega)); end: > omega1:=Tinh_omega(A1);

122


Động lực học và điều khiển rôbôt Scara u 1 := [ 0

0

qd1 ]

> omega2:=Tinh_omega(A2); u 2 := [ 0

0

qd1 C qd2 ]

0

qd1 C qd2 ]



0

qd1 C qd2 K qd4 ]

> # Tinh cac matran jacobian quay cua cac khau > JacobiQuay:=proc(omega) local Jr,i,j; Jr:=matrix(3,n,0); for i from 1 by 1 to 3 do for j from 1 by 1 to n do Jr[i,j]:=coeff(omega[i],qd[j,1]); od; od; return(evalm(Jr)); end: > Jr1:=JacobiQuay(omega1); é0 ê Jr1 := ê ê0 ê ë1

0 ù

0

0

0

0

0

0

ú 0 ú ú ú 0 û

0

0

0 ù

0

0

1

0

> Jr2:=JacobiQuay(omega2); é0 ê Jr2 := ê ê0 ê ë1

ú 0 ú ú ú 0 û

> Jr3:=JacobiQuay(omega3);

123


Động lực học và điều khiển rôbôt Scara é0 ê Jr3 := ê ê0 ê ë1

0 ù

0

0

0

0

1

0

ú 0 ú ú ú 0 û

0

0

0

0

0

0

1

0

K 1

> Jr4:=JacobiQuay(omega4); é0 ê Jr4 := ê ê0 ê ë1

> # Momen quan tinh khoi cua cac khau > I1:=diag(I1x,I1y,I1z); é I1x 0 0 ê I1 := ê ê 0 I1y 0 ê 0 I1z ë0

> I2:=diag(I2x,I2y,I2z); é I2x 0 0 ê I2 := ê ê 0 I2y 0 ê 0 I2z ë0

> I3:=diag(I3x,I3y,I3z); 0 é I3x 0 ê I3 := ê ê 0 I3y 0 ê 0 I3z ë0

> I4:=diag(I4x,I4y,I4z); é I4x 0 0 ê I4 := ê ê 0 I4y 0 ê 0 I4z ë0

> # Thu tuc xac dinh matran khoi luong M KL:=proc(m::scalar,Ii::matrix,A::matrix,Jr::matrix,Jt::m atrix) local M;

124



M:=evalm(transpose(Jt)&*scalarmul(Jt,m) + transpose(Jr)&*A&*Ii&*transpose(A)&*Jr); return(M); end: M1:=map(combine,matrix(KL(m1,I1,A1,Jr1,Jt1)),trig): M2:=map(combine,matrix(KL(m2,I2,A2,Jr2,Jt2)),trig): M3:=map(combine,matrix(KL(m3,I3,A3,Jr3,Jt3)),trig): M4:=map(combine,matrix(KL(m4,I4,A4,Jr4,Jt4)),trig): > M:=simplify(evalm(M1+M2+M3+M4)): save M,"matrankhoiluong.txt": # Cac phan tu cua ma tran khoi luong for i from 1 by 1 to n do for j from 1 by 1 to n do print(m[i,j]=evalm(M[i,j])); od; od; m1,

1

= L12 m1 C I1z C m2 a12 C 2 m2 a1 L2 cos ( q2 ) C m2 L22 C I2z

C m3 a22 C 2 m3 a2 a1 cos ( q2 ) C m3 a12 C I3z C m4 a22 C 2 m4 a2 a1 cos ( q2 ) C m4 a12 C I4z m1,

2

= m2 a1 L2 cos ( q2 ) C m2 L22 C I2z C m3 a22

C m3 a2 a1 cos ( q2 ) C I3z C m4 a22 C m4 a2 a1 cos ( q2 ) C I4z m1,

3

=0

m1,

4

= K

m2,

1

= m2 a1 L2 cos ( q2 ) C m2 L22 C I2z C m3 a22

C m3 a2 a1 cos ( q2 ) C I3z C m4 a22 C m4 a2 a1 cos ( q2 ) C I4z m2,

2

= m2 L2 2 C I2z C m3 a22 C I3z C m4 a22 C

m2,

3

=0

m2,

4

= K

125

Đồ án tốt nghiệp m3,

1

=0

m3,

2

=0

m3,

3

= m3 C m4

m3,

4

=0

m4,

1

= K

m4,

2

= K

m4,

3

=0

m4,

4


=

> # Thu tuc tinh matran C[q,qd] tu matran M MatC:=proc(M::matrix,q::matrix,qd::matrix,n::integer) local C,i,j,k,x,y,h; for i from 1 by 1 to n do for j from 1 by 1 to n do for k from 1 by 1 to n do h[i,j,k]:=1/2*(diff(M[i,j],q[k,1]) + diff(M[i,k],q[j,1]) - diff(M[k,j],q[i,1])); od; od; od; C:=matrix(n,n,0): for x from 1 by 1 to n do for y from 1 by 1 to n do C[x,y]:=sum(h[x,y,z]*qd[z,1],z=1..n); od; od; return(evalm(C)); end:

126



> C:=MatC(M,q,qd,n);save C,"matran_C.txt": é ê ê C := ê[ ( K m2 a1 L2 sin ( q2 ) K m3 a2 a1 sin ( q2 ) K m4 a2 a1 sin ( q2 ) ) qd2 , ê ê ê ë ( K m2 a1 L2 sin ( q2 ) K m3 a2 a1 sin ( q2 ) K m4 a2 a1 sin ( q2 ) ) qd1 C ( K m2 a1 L2 sin ( q2 ) K m3 a2 a1 sin ( q2 ) K m4 a2 a1 sin ( q2 ) ) qd2 , 0, 0], [ ( m2 a1 L2 sin ( q2 ) C m3 a2 a1 sin ( q2 ) C m4 a2 a1 sin ( q2 ) ) qd1 , 0, 0, ù ú ú 0], [0, 0, 0, 0], [0, 0, 0, 0] ú ú ú ú û

> # Bieu thuc the nang cua he PI:=m1*rC1[3,1]*g + m2*rC2[3,1]*g + m3*rC3[3,1]*g + m4*rC4[3,1]*g ; P := m2 d1 g C m3 ( d1 K q3 C L3 ) g C m4 ( d1 K q3 K d4 C L4 ) g

> # Tinh cac luc trong truong(luc co the) tac dung vao cac khau > G:=matrix(n,1,0): for i from 1 by 1 to n do G[i,1]:=diff(PI,q[i,1]); od: print(G=evalm(G)); save G,"matran_G.txt": 0 é ù ê ú ê ú 0 ê ú G= êK m3 g K m4 g ú ê ú ê ú 0 ë û

> # Bieu thuc ham hao tan co dang Phi:=1/2*(b1*qd1^2+b2*qd2^2+b3*qd3^2+b4*qd4^2);

127


Động lực học và điều khiển rôbôt Scara F :=

1 1 1 1 b1 qd1 2 C b2 qd2 2 C b3 qd3 2 C b4 qd4 2 2 2 2 2

> # Tinh cac luc can nhot tac dung vao cac khop dong cua robot Lc:=matrix(n,1,0): for i from 1 by 1 to n do Lc[i,1]:=diff(Phi,qd[i,1]); od: print(Lc=evalm(Lc)); save Lc,"matran_Lc.txt": éb1 ê êb2 Lc = ê êb3 ê ê ëb4

qd1 qd2 qd3 qd4

> # Cac luc phat dong cua dong co o cac khop Lt:=matrix(n,1,[tau[1],tau[2],tau[3],tau[4]]); save Lt,"matran_Lt.txt": ét 1 ê êt 2 Lt := ê ê t ê 3 ê ët 4

> # Cac phuong trinh vi phan chuyen dong thu duoc vetrai:=map(convert,evalm(M&*qdd + C&*qd + G + Lc),rational): save vetrai,"VetraiPT.txt": > vetrai[1,1]=Lt[1,1];

128



( L12 m1 C I1z C m2 a1 2 C 2 m2 a1 L2 cos( q2 ) C m2 L22 C I2z C m3 a2 2 C 2 m3 a2 a1 cos( q2 ) C m3 a1 2 C I3z C m4 a2 2 C 2 m4 a2 a1 c o s ( q2 ) C m4 a1 2 C I4z ) qdd1 C ( m2 a1 L2 cos( q2 ) C m2 L22 C I2z C m3 a2 2 C m3 a2 a1 cos( q2 ) C I3z C m4 a2 2 C m4 a2 a1 cos( q2 ) C I4z ) qdd2 K I4z qdd4 C ( K m2 a1 L2 sin ( q2 ) K m3 a2 a1 sin ( q2 ) K m4 a2 a1 sin ( q2 ) ) qd2 qd1 C ( ( K m2 a1 L2 sin ( q2 ) K m3 a2 a1 sin ( q2 ) K m4 a2 a1 sin ( q2 ) ) qd1 C ( K m2 a1 L2 sin ( q2 ) K m3 a2 a1 sin ( q2 ) K m4 a2 a1 sin ( q2 ) ) qd2 ) qd2 C b1 qd1 = t 1

> vetrai[2,1]=Lt[2,1]; ( m2 a1 L2 cos( q2 ) C m2 L22 C I2z C m3 a2 2 C m3 a2 a1 cos( q2 ) C I3z C m4 a2 2 C m4 a2 a1 cos( q2 ) C I4z ) qdd1 C ( m2 L22 C I2z C m3 a2 2 C I3z C m4 a2 2 C I4z ) qdd2 K I4z qdd4 C ( m2 a1 L2 sin ( q2 ) C m3 a2 a1 sin ( q2 ) C m4 a2 a1 sin ( q2 ) ) qd1 2 C b2 qd2 = t 2

> vetrai[3,1]=Lt[3,1]; ( m3 C m4 ) qdd3 K m3 g K m4 g C b3 qd3 = t 3

> vetrai[4,1]=Lt[4,1];

K I4z qdd1 K I4z qdd2 C I4z qdd4 C b4 qd4 = t 4

6.2. Giải bài toán động lực học thuận cho rôbôt SCARA > restart; with(linalg): with(plots): > read "matrankhoiluong.txt": read "matran_C.txt": G:=vector([0,0,-m3*g-m4*g,0]): Lc:=vector([b1*qd1,b2*qd2,b3*qd3,b4*qd4]): read "Chogiatridau.txt": Lt:=evalm(Lt); q0:=vector([q10,q20,q30,q40]); qd:=vector([qd1,qd2,qd3,qd4]):

129



qd0:=vector([qd10,qd20,qd30,qd40]); qdd:=vector([qdd1,qdd2,qdd3,qdd4]):

0

é ë

1

0 1 0

1 1

1 1 p , 2.5 sin p t C p , 12 4 1 1 ù K 20 C 1.5 sin p t C p , 4 sin p t C p ]ú 6 3 û

Lt := ê[2 sin p t C

0

q0 := [ K .35

0

0.15 0.5 ]

qd 0 := [ 0

0

0

0 ]

> # Nhap cac thong so dong hoc a1:=0.425:

a2:=0.375:

L1:=a1/2:

L2:=a2/2:

L3:=0.15:

L4:=0.1:

d1:=0.16:

d4:=0.275:

# don vi la met

> # Nhap vao cac thong so dong luc hoc cua robot SCARA (ADEPTONE-XL) I1x:=0:

I1y:=0.821:

I1z:=0.821 :

I2x:=0:

I2y:=0.136:

I2z:=0.136 :

I3x:=0:

I3y:=0:

I3z:=0.008 :

I4x:=0:

I4y:=0:

I4z:=0.3181:

# Don vi la kgm^2

b1:=0.5: b2:=0.5:

b3:=0.5:

b4:=0.5:

m1:=10:

m3:=1.5:

m4:=0.5:

m2:=5:

g:=9.81:

> f:=proc(ti,qi,qdi) local f; f:=simplify(inverse(M)&*(Lt-C&*qd-Lc-G)); return(subs(t=ti,q1=qi[1],q2=qi[2],q3=qi[3],q4=qi[4],qd1 =qdi[1],qd2=qdi[2],qd3=qdi[3],qd4=qdi[4],evalm(f))); end: > K:=200: T:=2.5:

130



h:= T/K; i:=0; t0:=0; qi:=evalm(q0); qdi:=evalm(qd0); h := 0.01250000000 i := 0

t0 := 0

qi := [ K .35

0

0.15 0.5 ]

qdi := [ 0

0

0

0 ]

> q1t:=array(1..K): q2t:=array(1..K): q3t:=array(1..K): q4t:=array(1..K): qd1t:=array(1..K): qd2t:=array(1..K): qd3t:=array(1..K): qd4t:=array(1..K): tg:=array(1..K): > while i < K do ti:=t0+i*h: k1:=simplify(scalarmul(f(ti,qi,qdi),h/2)); k2:=simplify(scalarmul(f(ti+h/2,evalm(qi+scalarmul(qdi,h /2)+scalarmul(k1,h/4)),evalm(qdi+k1)),h/2)); k3:=simplify(scalarmul(f(ti+h/2,evalm(qi+scalarmul(qdi,h /2)+scalarmul(k1,h/4)),evalm(qdi+k2)),h/2));

131



k4:=simplify(scalarmul(f(ti+h,evalm(qi+scalarmul(qdi,h) +scalarmul(k3,h)),evalm((qdi+scalarmul(k3,2)))),h/2)); i:=i+1; q1t[i]:=qi[1]; q2t[i]:=qi[2]; q3t[i]:=qi[3]; q4t[i]:=qi[4]; qd1t[i]:=qdi[1]; qd2t[i]:=qdi[2]; qd3t[i]:=qdi[3]; qd4t[i]:=qdi[4]; tg[i]:=ti; qi:=simplify(evalm(qi+scalarmul(qdi,h) +scalarmul((k1+k2+k3),h/3))); qdi:=simplify(evalm(qdi+scalarmul((k1+2*k2+2*k3+k4),1/3) )); end do: > plot(Lt[1],t=0..T,title="momen tac dong vao khop 1"); plot(Lt[2],t=0..T,title="momen tac dong vao khop 2"); plot(Lt[3],t=0..T,title="Luc tac dong vao khop 3"); plot(Lt[4],t=0..T,title="momen tac dong vao khop 4");

132



> PLOT(CURVES([seq([tg[j],q1t[j]], j=1..K )]),TITLE("Do thi q1(t) tim bang PPS")); PLOT(CURVES([seq([tg[j],qd1t[j]], j=1..K )]),TITLE("Do thi qd1(t) tim bang PPS"));

133



> PLOT(CURVES([seq([tg[j],q2t[j]], j=1..K )]),TITLE("Do thi q2(t) tim bang PPS")); PLOT(CURVES([seq([tg[j],qd2t[j]],

j=1..K

)]),TITLE("Do

thi qd2(t) tim bang PPS"));


j=1..K

)]),TITLE("Do


134




j=1..K

)]),TITLE("Do


6.3. Giải bài toán động lực học ngược cho rôbôt SCARA > restart; with(linalg): with(plots): > # cho biet vi tri ban kep

135



read "Vitribankep.txt": x:=matrix(4,1,[xp,yp,zp,phi]);

0 1 0 1 0 1

5 é 0.4 C 0.15 cos p t ê 6 ê ê 5 p t ê 0.35C 0.05 sin 6 ê x := ê êK 0.415K 0.1 sin 3 p t 4 ê ê 3 2 ê K p t sin ê 2 3 K e ë

0

0

11

> # Nhap cac thong so dong hoc a1:=0.425; # don vi la met a2:=0.375; L1:=a1/2; L2:=a2/2; L3:=0.15; L4:=0.1; d1:=0.16; d4:=0.275; T:=4: a1 := 0.425

a2 := 0.375

L1 := 0.2125000000

L2 := 0.1875000000

L3 := 0.15

L4 := 0.1

d1 := 0.16

136


Động lực học và điều khiển rôbôt Scara d4 := 0.275

> # Thay vao bai toan dong hoc nguoc q2t:=simplify(arccos((((x[1,1])^2+(x[2,1])^2-a1*a1a2*a2)/(2*a1*a2)))): q1t:=arctan(-a2*sin(q2t)*x[1,1]+(a1+a2*cos(q2t))*x[2,1], (a1+a2*cos(q2t))*x[1,1]+a2*sin(q2t)*x[2,1]): q3t:=x[3,1]-d1-d4: q4t:=evalf(simplify(Pi*ln(abs(x[4,1])))): > # Tinh van toc suy rong,gia toc suy rong cac khop qd1t:=diff(q1t,t): qdd1t:=diff(qd1t,t): plot(q1t,t=0..T,color=red,title="Do thi q1t"); plot(qd1t,t=0..T,color=red,title="Do thi qd1t"); plot(qdd1t,t=0..T,color=red,title="Do thi qdd1t"); > qd2t:=diff(q2t,t): qdd2t:=diff(qd2t,t): plot(q2t,t=0..T,color=red,title="Do thi q2t"); plot(qd2t,t=0..T,color=red,title="Do thi qd2t"); plot(qdd2t,t=0..T,color=red,title="Do thi qdd2t"); > qd3t:=diff(q3t,t): qdd3t:=diff(qd3t,t): plot(q3t,t=0..T,color=red,title="Do thi q3t"); plot(qd3t,t=0..T,color=red,title="Do thi qd3t"); plot(qdd3t,t=0..T,color=red,title="Do thi qdd3t"); > qd4t:=diff(q4t,t): qdd4t:=diff(qd4t,t): plot(q4t,t=0..T,color=red,title="Do thi q4t"); plot(qd4t,t=0..T,color=red,title="Do thi qd4t"); plot(qdd4t,t=0..T,color=red,title="Do thi qdd4t");

137



138



139



> # Nhap vao cac thong so dong luc hoc cua robot SCARA (ADEPTONE-XL) I1x:=0.0006:

I1y:=0.0165:

I1z:=0.0165 :

# Don vi la

kgm^2 I2x:=0.0003:

I2y:=0.0072

:

I2z:=0.0074

:

140



I3x:=0.0008:

I3y:=0.0008

I4x:=0.000037:

I4y:=0.000037:

b1:=0: m1:=1.85:

b2:=0:

b3:=0:

m2:=1.24:

:

I3z:=0.00001

:

I4z:=0.00000074: b4:=0: m3:=0.25:

m4:=0.035:

g:=9.81: > # Doc vao phuong trinh vi phan chuyen dong cua robot SCARA (ADEPTONE-XL) read "VetraiPT.txt": Lt:=evalm(vetrai): q1:=q1t:

qd1:=qd1t:

qdd2:=qdd2t:q3:=q3t:

qdd1:=qdd1t: qd3:=qd3t:

q2:=q2t: qdd3:=qdd3t:

qd2:=qd2t: q4:=q4t:

qd4:=qd4t: qdd4:=qdd4t: > # Ve do thi luc phat dong cua dong co phu thuoc thoi gian plot(Lt[1,1],t=0..T,color=red,title="Do thi tau[1]"); plot(Lt[2,1],t=0..T,color=red,title="Do thi tau[2]"); plot(Lt[3,1],t=0..T,color=red,title="Do thi tau[3]"); plot(Lt[4,1],t=0..T,color=red,title="Do thi tau[4]");

141



6.4. Chương trình matlab điều khiển rôbôt Scara 6.4.1. Chương trình matlab function outputs=DK_truot(inputs) % Dat lai bien chi cac gia tri dau vao qd1 = inputs(1); qd2 = inputs(2); qd3 = inputs(3); qd4 = inputs(4); qd1_dot = inputs(5); qd2_dot = inputs(6); qd3_dot = inputs(7); qd4_dot = inputs(8); qd1_2dot = inputs(9); qd2_2dot = inputs(10); qd3_2dot = inputs(11); qd4_2dot = inputs(12); q1 = inputs(13); q2 = inputs(14); q3 = inputs(15); q4 = inputs(16); q1_dot = inputs(17);

142



q2_dot = inputs(18); q3_dot = inputs(19); q4_dot = inputs(20); q_dot=[q1_dot;q2_dot;q3_dot;q4_dot]; qd_dot=[qd1_dot;qd2_dot;qd3_dot;qd4_dot]; qd_2dot = [qd1_2dot; qd2_2dot;qd3_2dot; qd4_2dot]; e = [q1 - qd1;q2 - qd2;q3 - qd3;q4 - qd4]; e_dot = q_dot - qd_dot; %================================================== m1 = 10.0; m2 = 5.0; m3 = 1.5; m4 = 0.5; % Khoi luong 4 khau I1z = 0.821; I2z = 0.136; I3z = 0.008; I4z = 0.3181;

% Moment

quan tinh a1 = 0.425; a2 = 0.375; % Chieu dai cua 2 khau L1 = 0.2125; L2 = 0.1875 ; L3 = 0.15; L4 = 0.1; % Vi tri khoi tam cua cac khau g=9.81; %================================================== %Sai so tham so bo dieu khien c=1.1; m1=c*m1;m2=c*m2;m3=c*m3;m4=c*m4; I1z=c*I1z;I2z=c*I2z;I3z=c*I3z;I4z=c*I4z; a1=c*a1;a2=c*a2; L1=c*L1;L2=c*L2;L3=c*L3;L4=c*L4; %================================================== lambda=diag([5,5,5,5]); s=lambda*e+e_dot; qr_dot= -lambda*e+qd_dot; qr_2dot= -lambda*e_dot+qd_2dot; %================================================== M(1,1)

=

I1z+I2z+I3z+I4z+

m1*L1^2

+

m2*(a1^2+L2^2+2*a1*L2*cos(q2))+

143



m3*(a1^2+a2^2+2*a1*a2*cos(q2))+ m4*(a1^2+a2^2+2*a1*a2*cos(q2)); M(1,2) = I2z+I3z+I4z+ m2*(L2^2 + a1*L2*cos(q2))+ m3*(a2^2 + a1*a2*cos(q2))+ m4*(a2^2 + a1*a2*cos(q2)); M(1,3) = 0; M(1,4) = -I4z; M(2,1) = M(1,2); M(2,2) = I2z+I3z+I4z+ m2*L2^2 + m3*a2^2 + m4*a2^2; M(2,3) = 0; M(2,4) = -I4z; M(3,1) = 0; M(3,2) = 0; M(3,3) = m3 + m4; M(3,4) = 0; M(4,1) = -I4z; M(4,2) = -I4z; M(4,3) = 0; M(4,4) = I4z; C(1,1)

=

(-m2*a1*L2*sin(q2)-

m3*a1*a2*sin(q2)-

m4*a1*a2*sin(q2))*q2_dot; C(1,2)

=

(-m2*a1*L2*sin(q2)-

m3*a1*a2*sin(q2)-

m4*a1*a2*sin(q2))*(q1_dot + q2_dot); C(1,3) = 0; C(1,4) = 0; C(2,1)

=

(m2*a1*L2*sin(q2)+

m3*a1*a2*sin(q2)+

m4*a1*a2*sin(q2))*q1_dot; C(2,2) = 0; C(2,3) = 0; C(2,4) = 0; C(3,1) = 0; C(3,2) = 0; C(3,3) = 0;

144



C(3,4) = 0; C(4,1) = 0; C(4,2) = 0; C(4,3) = 0; C(4,4) = 0; G(1) = 0; G(2) = 0; G(3) = -m3-m4 ; G(4) = 0; G = g*G; G = G'; Kp = diag([30,30,30,30]); Ks = diag([50,50,50,50]); outputs = M*qr_2dot+C*qr_dot+G-Ks*sign(s)-Kp*s;

function outputs=model(inputs) % Dat lai bien chi cac gia tri dau vao t1 = inputs(1); t2 = inputs(2); t3 = inputs(3); t4 = inputs(4); q1 = inputs(5); q2 = inputs(6); q3 = inputs(7); q4 = inputs(8); q1_dot = inputs(9); q2_dot = inputs(10); q3_dot = inputs(11); q4_dot = inputs(12); noise(1) = inputs(13); noise(2) = inputs(14); noise(3) = inputs(15);

145



noise(4) = inputs(16); noise=noise'; q_dot=[q1_dot;q2_dot;q3_dot;q4_dot]; % Cac tham so cua he %================================================== m1 = 10.0; m2 = 5.0; m3 = 1.5; m4 = 0.5;

% Khoi luong cua 4

khau I1z = 0.821; I2z = 0.136; I3z = 0.008; I4z = 0.3181;

% Moment

quan tinh a1 = 0.425; a2 = 0.375; % Chieu dai cua 2 khau L1 = 0.2125; L2 = 0.1875 ; L3 = 0.15; L4 = 0.1; % Vi tri khoi tam cua cac khau g=9.81; %================================================== M(1,1)

=

I1z+I2z+I3z+I4z+

m1*L1^2

+

m2*(a1^2+L2^2+2*a1*L2*cos(q2))+ m3*(a1^2+a2^2+2*a1*a2*cos(q2))+ m4*(a1^2+a2^2+2*a1*a2*cos(q2)); M(1,2) = I2z+I3z+I4z+ m2*(L2^2 + a1*L2*cos(q2))+ m3*(a2^2 + a1*a2*cos(q2))+ m4*(a2^2 + a1*a2*cos(q2)); M(1,3) = 0; M(1,4) = -I4z; M(2,1) = M(1,2); M(2,2) = I2z+I3z+I4z+ m2*L2^2 + m3*a2^2 + m4*a2^2; M(2,3) = 0; M(2,4) = -I4z; M(3,1) = 0; M(3,2) = 0; M(3,3) = m3 + m4; M(3,4) = 0; M(4,1) = -I4z; M(4,2) = -I4z;

146



M(4,3) = 0; M(4,4) = I4z; C(1,1)

=

(-m2*a1*L2*sin(q2)-

m3*a1*a2*sin(q2)-

m4*a1*a2*sin(q2))*q2_dot; C(1,2)

=

(-m2*a1*L2*sin(q2)-

m3*a1*a2*sin(q2)-

m4*a1*a2*sin(q2))*(q1_dot + q2_dot); C(1,3) = 0; C(1,4) = 0; C(2,1)

=

(m2*a1*L2*sin(q2)+

m3*a1*a2*sin(q2)+

m4*a1*a2*sin(q2))*q1_dot; C(2,2) = 0; C(2,3) = 0; C(2,4) = 0; C(3,1) = 0; C(3,2) = 0; C(3,3) = 0; C(3,4) = 0; C(4,1) = 0; C(4,2) = 0; C(4,3) = 0; C(4,4) = 0; G(1) = 0; G(2) = 0; G(3) = -m3-m4 ; G(4) = 0; G = g*G; G = G'; T=[t1;t2;t3;t4]; outputs = inv(M)*(T-noise-C*q_dot-G);

function outputs=Hams(inputs) % Dat lai bien chi cac gia tri dau vao

147



qd1 = inputs(1); qd2 = inputs(2); qd3 = inputs(3); qd4 = inputs(4); qd1_dot = inputs(5); qd2_dot = inputs(6); qd3_dot = inputs(7); qd4_dot = inputs(8); qd1_2dot = inputs(9); qd2_2dot = inputs(10); qd3_2dot = inputs(11); qd4_2dot = inputs(12); q1 = inputs(13); q2 = inputs(14); q3 = inputs(15); q4 = inputs(16); q1_dot = inputs(17); q2_dot = inputs(18); q3_dot = inputs(19); q4_dot = inputs(20); q_dot=[q1_dot;q2_dot;q3_dot;q4_dot]; qd_dot=[qd1_dot;qd2_dot;qd3_dot;qd4_dot]; qd_2dot = [qd1_2dot; qd2_2dot;qd3_2dot; qd4_2dot]; e = [q1 - qd1;q2 - qd2;q3 - qd3;q4 - qd4]; e_dot = q_dot - qd_dot; lambda=diag([5,5,5,5]); %================================================== outputs = e_dot+lambda*e;

% Chuong trình ve do thi >> figure(1); hold on;

148



grid on; plot(q1(:,1),q1(:,2), 'k--'); plot(q1(:,1),q1(:,3), 'k'); legend('q_1d','q_1',4); xlabel('Thoi gian(s)'); ylabel('Do thi mo phong q1 va q1d (rad)' ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(2); hold on; grid on; plot(q2(:,1),q2(:,2), 'k--'); plot(q2(:,1),q2(:,3), 'k'); legend('q_2d','q_2',4); xlabel('Thoi gian(s)'); ylabel('Do thi mo phong q2 va q2d (rad)' ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(3); hold on; grid on; plot(q3(:,1),q3(:,2), 'k--'); plot(q3(:,1),q3(:,3), 'k'); legend('q_3d','q_3',4); xlabel('Thoi gian(s)'); ylabel('Do thi mo phong q3 va q3d (rad)' ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(4); hold on; grid on; plot(q4(:,1),q4(:,2), 'k--'); plot(q4(:,1),q4(:,3), 'k'); legend('q_4d','q_4',4); xlabel('Thoi gian(s)');

149



ylabel('Do thi mo phong q4 va q4d (rad)' ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% >> figure(1); hold on; grid on; plot(s1(:,1),s1(:,2), 'k'); legend('s_1',4); xlabel('Thoi gian(s)'); ylabel('Do thi mo phong s1' ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% >> figure(2); hold on; grid on; plot(s2(:,1),s2(:,2), 'k'); legend('s_2',4); xlabel('Thoi gian(s)'); ylabel('Do thi mo phong s2' ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% >> figure(3); hold on; grid on; plot(s3(:,1),s3(:,2), 'k'); legend('s_3',4); xlabel('Thoi gian(s)'); ylabel('Do thi mo phong s3' ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% >> figure(4); hold on; grid on; plot(s4(:,1),s4(:,2), 'k'); legend('s_4',4); xlabel('Thoi gian(s)');

150



ylabel('Do thi mo phong s4' ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% >> figure(1); hold on; grid on; plot(d1(:,1),d1(:,2), 'k'); legend('d_1',4); xlabel('Thoi gian(s)'); ylabel('Do thi mo phong nhieu d1' ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(2); hold on; grid on; plot(d2(:,1),d2(:,2), 'k'); legend('d_2',4); xlabel('Thoi gian(s)'); ylabel('Do thi mo phong nhieu d2' ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(3); hold on; grid on; plot(d3(:,1),d3(:,2), 'k'); legend('d_3',4); xlabel('Thoi gian(s)'); ylabel('Do thi mo phong nhieu d3' ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(4); hold on; grid on; plot(d4(:,1),d4(:,2), 'k'); legend('d_4',4);

151



6.4.1. Sơ đồ Simulink

152

Dong Luc Hoc

Recommend Documents