ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ

PHẦN 1: THIẾT KẾ MÔ HÌNH 3D ROBOT RRR

Sử dụng phần mền solidwork để thiết kế mô hình 3D của robot
RRR

1.thiết kế các khâu:

Khâ u đế:

Khâ u 1:
Khâ u 2:

Khâ u 3:
2.Các chi tiết khác

Bá nh ră ng trụ c bá nh ră n

Nắ p bộ truyền bá nh ră ng

Xylanh khí nén

3.các bộ truyền động:

Khớ p 1:

Bộ truyền bá nh ră ng

Khớ p 2:

Bộ truyền bá nh ră ng
Khớ p 3:

Bộ truyền bá nh ră ng

Tay kẹp:

Sử dụ ng xylanh khí nén

 PHẦN 2: TÍNH TOÁN ĐỘNH HỌC THUẬN VÀ NGƯỢC ROBOT

Cấ u hình robot RRR

1.bảng tham số DH

a.Xây dựng hệ tọa độ DH:

 Hệ tọ a độ O0 x0 y 0 z 0:

Gố c tọ a độ đặ t tạ i tâ m củ a khớ p độ ng thứ nhấ t

Trụ c O0 z 0 dọ c theo hướ ng củ a trụ c khớ p độ ng thứ nhấ t,hướ ng từ dướ i lên

Trụ c O0 x0 có phương vuô ng gó c vớ i O z 0 hướ ng từ ngoà i vao trong như hình

vẽ

Trụ c O0 y 0 xác định theo quy tắ c tam diện thuậ n

 Hệ tọ a độ O1 x 1 y 1 z 1:

Gố c tọ a độ đặ t tạ i tâ m củ a khớ p độ ng thứ 2
Trụ c O1 z 1 phương dọ c theo trụ c khớ p độ ng thứ 2 hướ ng từ trong ra ngoà i
như hình vẽ

Trụ c O1 x 1 có phương nằ m trên đườ ng vuô ng gó c chung củ a trụ c O0 z 0 và O1 z 1

hướ ng từ O0 tớ i O1

Trụ c O1 y 1 xá c định theo quy tắ c tam diện thuậ n

 Hệ tọ a độ O2 x 2 y 2 z 2:

Gố c tọ a độ đặ t tạ i tâ m củ a khớ p độ ng thứ 3

Trụ c O2 z 2 phương dọ c theo trụ c khớ p độ ng thứ 2 hướ ng từ trong ra ngoà i

như hình vẽ

Trụ c O2 x 2 có phương nằ m trên đườ ng vuô ng gó c chung củ a trụ c O1 z 1 và O2 z 2

hướ ng từ O1 tớ i O2

Trụ c O2 y2 xác định theo quy tắ c tam diện thuậ n

 Hệ tọ a độ O3 x 3 y 3 z 3:

Gố c tọ a độ đặ t tạ i tâ m củ a tay kẹp

Trụ c O3 z 3 phương dọ c theo trụ c khớ p độ ng thứ 2 hướ ng từ trong ra ngoà i

như hình vẽ

Trụ c O3 x 3 có phương nằ m trên đườ ng vuô ng gó c chung củ a trụ c O2 z 2 và O3 z 3

hướ ng từ O2 tớ i O3

Trụ c O3 y 3 xá c định theo quy tắ c tam diện thuậ n

b.cách xác định các tham số động học DH

vị trí củ a hệ tọ a độ khớ p (Oxyz)i đố i vớ i hệ tọ a độ khớ p (Oxyz)i−1 đượ c xác

định bở i 4 tham số θi , d i, α i, a i như sau:
θi : gó c quay quanh trụ c z i−1 để trụ c x i−1 trù ng vớ i trụ c x ' i( x i /¿ x ' i )

d i : dịch chuyển tịnh tiế dọ c trụ c z i−1 để gố c tọ a độ Oi−1 chuyể đến O ' i là giao
điểm củ a trụ c x i và trụ c z i−1
a i : dịch chuyển dọ c trụ c x i để điểm O ' i chuyển đến điểm O i

α i : gó c quay quanh trụ c x i sao cho trụ c z ' i−1( z ' i−1 /¿ z i−1) trù ng vớ i trụ c z i
c.bảng tham số động học DH

di θi ai αi
1 l1 q1 0 90ᶱ
2 0 q2 l2 0
3 0 q2 l3 0

 Cá c ma trậ n truyền:

Ma trậ n truyền khâ u i so vớ i khâ u i-1:

Ma trậ n truyền khâ u 2 và 3 so vớ i khâ u 0:

2.bài toán động học thuận

Cá c thong số đầ u và o :q1, q2, q3

Thô ng số cầ n xá c định: điểm tá c độ ng cuố i P=( Px, Py,Pz),hướ ng củ a khâ u

cuố i
  Tọ a độ điểm tá c độ ng cuố i:

 Hướ ng củ a khâ u thao tá c có ma trậ n cosin chỉ hướ ng:

3.bài toán động học

ngược

Thô ng số đầ u và o là tọ a độ điểm tá c độ ng cuố i: P=( Px; Py; Pz) và hướ ng

khâ u cuố i

Thô ng số cầ n xá c định:cá c biến khớ p (q1; q2; q3)

Ta có ma trậ n cấ u hình củ a khâ u thao tá c:

Trong đó a 14, a 24, a 34 là cá c tọ a độ củ a điểm thao tá c E:

a 14
= ,a 24= , a 34=

Cò n a ijlà cá c phầ n tử củ a ma trậ cosin chỉ hướ ng củ a khâ u thao tá c.

Từ bà i toá n độ ng họ c thuậ n ta có cá c ma trậ n:

 (2)

cho cá c phầ n tử (1,4) và (2,4) củ a hai ma trậ n (1) và (3) bằ ng nhau ta đươc:

Từ phương trình ma trậ n (3) ta suy ra ;

trong đó  :

= (8)

(9)

Mặ t khá c ta có  :

(10)

So sá nh các phầ n tử ở cá c ô (1,4) , (2,4 ) và (3,4) củ a cá c ma trậ n (9) và (10)

ta đượ c :
 (11)

(12)

(13)

Bình phương hai vế củ a phương trình (11) và (12) rồ i cộ ng lạ i ta đượ c :

(14)

Từ đó suy ra :

(15)

Kết hợ p (13) và (15) ta có  :

So sá nh các phầ n tử (3,1) và (3,2) củ a hai ma trậ n (1) và (2 ) ta đượ c :

4.tính vận tốc và gia tốc điểm tác động cuối

vậ n tố c dà i và gia tố c dà i củ a điểm tá c độ ng cuố i đượ c suy ra từ đạ o hà m

củ a véc tơ tọ a độ r (0)
E :

 Vậ n tố c củ a điểm tá c độ ng cuố i:
  gia tố c củ a điểm tá c độ ng cuố i:

5.vận tốc góc và gia tốc góc các khâu

 cá c ma trậ n cosin chỉ hướ ng củ a cá c khau so vớ i R0:

khâ u 1:

khâ u 2:

khâ u 3:

 Vậ n tố c gó c cá c khâ u:

tính vậ n tố c gó c khâ u thứ i củ a robot dự a trên cô ng thứ c tính vậ n tố c gó c

củ a vậ t rắ n thong qua ma trậ n cosin chỉ hướ ng củ a nó :
 (sử dụ ng phầ n mềm maple)

Khâ u 1:

Khâ u 2:

Khâ u 3:

 gia tố c gó c cá c khâ u:

khâ u 1:

khâ u 2:
khâ u 3:

PHẦN 3:ĐỘNG LỰC HỌC CỦA ROBOT

Dạng ma trận của phương trình lagrange II:

Độ ng nă ng củ a robot có dạ ng:

Trong đó = ,

Phương trình lagrange II có dạ ng :

Sử dụ ng định lý đạ o hà m riêng theo vecto tích củ a hai ma trậ n ta có :

(1)

Trong đó :

(2)

Mặ t khá c:

(3)
Thay (2) và (3) và o (1) ta đượ c:

, (4)

Tính toá n tương tự ta có :

(5)

Thay (4) và (5) và o phương trình lagrange II ta đượ c:

Đặ t ta có :

(6)

Theo định lý đạ o hà m toà n phầ n và đạ o hà m riêng ta có :

thay và o (6) ta đượ c:

Ma trậ n ly tâ m và coriolis có dạ ng:

Khi đó phương trình vi phâ n chuyển đô ng củ a cá c khâ u :

1.bảng tham số động lực học của robot

khâ u Vị trí trọ ng tâ m Khố i Ma trậ n mô men quá n tính

xc yc zc lượ n I xx I yy I zz I xy I yz I zx
g
1 0 −l C 1 0 m1 I 1x I1 y I 1z 0 0 0
2 −l C 2 0 0 m2 I 2x I2y I 2z 0 0 0
3 −l C 3 0 0 m3 I 3x I3y I 3z 0 0 0

2.ma trận Jacobi của các khâu

Từ cấ u tạ o hình họ c củ a robot ta xá c định đượ c tọ a độ khố i tâ m củ a cá c

khâ u so vớ i hệ tọ a độ cố định  R0 :

a.các ma trận Jacobi tịnh tiến của các khâu

b.ma trận Jacobi quay của các khâu :

3.ma trận khối lượng suy rộng của robot:

Trong đó :

4.Ma trận ly tâm và coriolis :

Sử dụ ng phầ n mềm Maple nhâ n ra ta đượ c :

5.Thế năng của robot

Từ đó suy ra :

6.phuơng trình vi phân chuyển động của các khâu

Thế các biểu thứ c và o phương trình Lagrange loạ i hai :

Ta nhậ n đượ c hệ phương trình vi phâ n chuyển độ ng củ a robot ba khâ u

trong khô ng gian :

Khâ u 1 :
Khâ u 2 :

Khâ u 3 :

PHẦN 4: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CỦA ROBOT

1.thiết kế quỹ đạo trong không gian khớp :

Là xác định đườ ng biểu diễn củ a vị trí gó c khớ p(gó c quay củ a khớ p vớ i
khớ p quay và độ di chuyển củ a khớ p đố i vớ i khớ p tịnh tiến) theo thờ i gian
khi di chuyển từ vị trí ban đầ u q 0(A) tớ i vị trí cuố i q c(B) trong thờ i gian t cvớ i
q là biến khớ p tổ ng quá t. Quỹ đạ o di chuyển củ a khớ p giữ a hai vị trí sẽ thỏ a
mã n 4 điều kiện : vị trí ban đầ u và vị trí cuố i cù ng, tố c độ tạ i vị trí ban đầ u
và tố c độ tạ i vị trí cuố i cù ng. Do đó đa thứ c bậ c 3 sẽ phù hợ p vớ i quỹ đạ o
củ a khớ p robot:
q i(t)=a 0 i+ a1 i t +a2 i t 2 +a 3i t 3

giả thiết vậ n tố c tạ i điểm đầ u và điểm cuố i bằ ng 0:

q̇ i 0=0 ; q̇ ic=0

Ta có hệ phương trình:
Giả i hệ ta đượ c cá c hệ số củ a phương trình quỹ đạ o.

Bài toán cụ thể:

cho điểm A(90; 0; 55) và điểm B(0; 43.30; 120). Thờ i gian chuyển độ ng
t c =5( s)

Từ bà i toá n ngượ c ta xác định đượ c các thong số biến khớ p:

và

hệ số củ a phương trình quỹ đạ o củ a khớ p 1:

=0 =10.8 =-1.44
a 01 a 11 a 21 a 31
=0

Phương trình độ ng họ c củ a khâ u 1:

Đườ ng biểu diễn vị trí, vậ n tố c và gia tố c củ a khớ p 1 thao tá c theo thờ i gian

 Hệ số củ a phương trình quỹ đạ o củ a khớ p 2:

a 02 =¿0 a 12=0 a 22=3.6 a 32=−0.48

Phương trình độ ng họ c củ a khâ u 2:

Đườ ng biểu diễn vị trí, vậ n tố c và gia tố c củ a khớ p 2 thao tá c theo thờ i gian

 Hệ số củ a phương trình quỹ đạ o củ a khớ p cuố i:

a 03 =¿0 a 13=¿0 a 23=¿7.2 a 33=−0.96

Phương trình độ ng họ c củ a khâ u :

Đườ ng biểu diễn vị trí, vậ n tố c và gia tố c củ a khớ p 3 thao tá c theo thờ i gian.

2.thiết kế quỹ đạo trong không gian làm việc

Tay robot di chuyển từ vi trí A( x A ; y A ; z A) trong khô ng gian là m việc tớ i vị trí

B( x B ; y B ; z B ) theo mộ t đườ ng thẳ ng. ta có phương trình đườ ng thẳ ng AB có
dạ ng sau:

Cho robot chuyển độ ng trong thờ i gian 5(s) đi từ vị trí A(90; 0; 55) tớ vị trí
B(0; 43.3; 120). Tạ i vị trí A các khớ p q 1=0, q 2=0, q 3=0

Phương trình đườ ng thẳ ng AB:

 PHẦN 5: ĐIỀU KHIỂN CHUYỂN ĐỘNG ROBOT
1.Hệ thống điều khiển phản hồi trong không gian khớp

Chứ c nă ng củ a hệ thố ng điều khiển là tạ o ra mộ t momen cầ n thiết để truyền

độ ng khớ p robot đả m bả o khớ p robot bá m theo vị trí đặ t

Sử dụ ng cấ u trú c bộ điều khiển có dạ ng tỉ lệ -tích phâ n- đạ o hà m (PID):

Trong đó :

:là sai số vị trí củ a khớ p robot

: là sai số tố c độ củ a khớ p robot

=diag( K P 1, K P 2,…, K Pn) :ma trậ n đườ ng chéo các hệ số khuếch đạ i

từ ng khớ p riêng biệt
K D=diag( K D 1, K D 2,…., K Dn):ma trậ n đườ ng chéo cá c hệ số đạ o hà m
từ ng khớ p riêng biệt
K I =diag( K I 1 , K I 2,…. K ¿): ma trậ n đườ ng chéo cá c hệ số tích phâ n từ ng
khớ p riêng biệt
Hệ thống điều khiển với cấu trúc điều khiển trên ổn định tuyệt đối toàn cục.
thực vậy :

Hệ tay má y vớ i phương trình độ ng lưc họ c có dạ ng:

Luậ t điều khiển có dạ ng:

Vớ i giả thiết bỏ qua thà nh phầ n củ a momen trọ ng lự c. ta nhậ n đượ c hệ điều

khiển vò ng kín:

Xét hà m lyapunov có dạ ng như sau:

Ta có M là các ma trậ n dương. Vậ y V luô n khô ng â m. Dạ o hà m V vớ i

ε =q d−q ta có  :
 =

Thà nh phầ n vecto đượ c viết ở dạ ng ma trậ n :

Trong đó có quan hệ vớ i M(q) như sau :

Vớ i S là ma trậ n đố i xứ ng lệch nên =0 do vậ y :

Nên : ≤0

Bấ t đẳ ng thứ c cho thấ y hệ thố ng ổ n định tuyệt đố i.

2.Các thao tác thực hiện mô phỏng

Khố i lượ ng củ a cá c khâ u :

Khâ u đế m0=10 kg, khâ u 1m1=8 kg, khâ u 2 m2=3 kg, khâ u 3 m3=1.5 kg

Phương trình độ ng lự c họ c robot có dạ ng :

Luậ n điều khiển có dạ ng sau :

Vớ i quỹ đạ o củ a cá c khớ p đã thiết lậ p trong phầ n thiết lậ p quỹ đạ o. đả m

bả o điểm tá c độ ng cuố i đi từ điểm A(90; 0; 55) tớ i điểm B(0; 43.30; 120)
trong thờ i gian 5(s).

Cá c hệ số khuếch đạ i tỉ lệ, tích phâ n và đạ o hà m đượ c lự a chọ n như sau :

 ,  ,

Ta có mô hình simulink củ a robot :

Mô hình bộ điều khiển PID :

Trong đó  :

q d , v d là tín hiệu quỹ đạ o đặ t và vậ n tố c đặ t và o cá c khớ p củ a robot đươc

thiết lậ p trong phầ n thiết lậ p quỹ đạ o

q, v là cá c tín quỹ đọ a và vậ n tố c phả n hồ i lạ i từ robot(quỹ đạ o thự c)

ở đâ y ta sử dụ ng cá c khố i:

+khố i Gain dù ng để khếch đạ i cá c tín hiệu đầ u và o

+khố i mux chậ p cá c tín hiệu đơn thà nh cá c tín hiệu tổ ng hợ p và tá ch thà nh
nhiều tín hiệu

+khố i input, ouput đầ u và o và ra củ a tín hiệu

+khố i hà m biểu diễn mộ t hà m toá n họ c khi có tín hiệu đi và o là cá c biến và

tín hiệu đầ u ra là giá trị củ a hà m
q d 1 =(10.8*u(1)^2-1.44*u(1)^3)*pi/180 ; q d 2=(3.6*u(1)^2-
0.48*u(1)^3)*pi/180
q d 3=(7.2*u(1)^2-0.96*u(1)^3)*pi/180

v d 1=(21.6*u(1)-4.32*u(1)^2)*pi/180; v d 2=(7.2*u(1)-1.44*u(1)^2)*pi/180

v d 3=(14.4*u(1)-2.88*u(1)^2)*pi/180

+ khố i scope hiển thị tín hiệu đầ u ra cả u quá trình mô phỏ ng theo thờ i gian.

+ khố i thờ i gian đặ t giá trị bằ ng 5(s)

3.kết quả mô phỏng

Sau khi mô phỏ ng ta đượ c kết quả là đồ thị củ a cá c đườ ng đặ c tính củ a cá c

biến:

Mô men củ a cá c khớ p theo thờ i gian

 Vị trí cá c khớ p theo thờ i gian

Vậ n tố c cá c khớ p theo thờ i gian

Sai số vị trí khớ p theo thờ i gian