Đồ Án Môn Học: Trường Đại Học Bách Khoa Hà Nội Viện Cơ Khí

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
VIỆN CƠ KHÍ
ĐỒ ÁN MÔN HỌC
ĐỀ TÀI:
THIẾT KẾ VÀ ĐIỀU KHIỂN ROBOT BA BẬC TỰ DO
Giảng viên hướng dẫn: TS. Nguyễn Kiên Trung
Sinh viên thực hiện: Bùi Văn Mạnh
MSSV : 20152383
Hà Nội, 06/2017
Đồ Án Thiết Kế Hệ Thống Cơ điện tử GVHD:TS.Nguyễn Kiên Trung
LỜI MỞ ĐẦU
Robot công nghiệp là lĩnh vực được nghiên cứu và phát triển mạnh trên thế
giới nhưng ở nước ta, lĩnh vực này còn mới mẻ. Ngày nay trong quá trình sản xuất
hiện đại, hầu hết các quy trình được thực hiện bằng máy chuyên dùng. Các
phương pháp này đã làm giảm rõ rệt chi phí sản xuất các sản phẩm công nghiệp
phù hợp với đa số người tiêu dùng.
Tuy nhiên, mỗi máy công cụ được thiết kế để thực hiện nguyên công cho
trước, mỗi khi cần thay đổi kiểu mẫu sản phẩm, thì toàn bộ dây chuyền sản xuất
phải được cải tạo lại. Việc sửa đổi rất tốn kém. Chính những lý do đó đã thúc đẩy
sự phát triển của phươnng pháp sản xuất tiên tiến, đó là chế tạo ra các loại robot
được đưa vào quy trình chế tạo để thực hiện nhiều nguyên công như: chuyển tải
vật tư và các thiết bị trong các dây truyền hiện đại, hàn điểm, sơn phun và lắp ráp
trong công nghiệp ô tô. Đây là kiểu tự động hóa linh hoạt và mang lại hiệu quả
kinh tế cao, rất cần thiết trong hoàn cảnh đất nước ta hiện nay đang tiến tới công
nghiệp hóa hiện đại hóa toàn diện.
Robot công nghiệp được ứng dụng rộng rãi trong các dây truyền sản xuất
hiện đại và có sự linh hoạt cao. Vì vậy việc nghiên cứu và chế tạo robot là rất cần
thiết và vô cùng quan trọng .
Chúng em xin chân thành cảm ơn sự hướng dẫn tâm huyết của thầy
TS.Nguyễn Kiên Trung– Bộ môn Công nghệ chế tạo máy - Viện cơ khí – Đại học
Bách khoa Hà Nội đã tận tình chỉ bảo, giúp đỡ chúng em trong suốt thời gian hoàn
thành đồ án này.
Do kiến thức còn hạn hẹp và thời gian thực hiện không nhiều nên trong quá
trình thực hiện sẽ không tránh khỏi những thiếu sót. Đề tài rất mong được sự góp
ý của quý thầy cô.
Em xin chân thành cảm ơn!
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 2

MỤC LỤC
LỜI MỞ ĐẦU .............................................................................................................................. 2

CHƯƠNG 1:PHÂN TÍCH VÀ LỰA CHỌN CẤU TRÚC ĐỀ TÀI .......................................................... 5
I. Tổng quan về đề tài ............................................................................................................. 5
1) Khái niệm và nguyên lý hoạt động của Robot ................................................................. 5
2) Đối tượng thao tác, dạng thao tác ................................................................................. 6
II. Phân tích các thông số kỹ thuật Robot ................................................................................. 6
1) Các thành phần kết cấu của Robot..................................................................................... 6
2) Thông số kỹ thuật ............................................................................................................... 7
3) Phân tích nguyên lý hoạt động ........................................................................................... 8
CHƯƠNG 2: Tính toán quỹ đạo chuyển động ........................................................................... 10
1) Bảng D-H .................................................................................................................................... 10
2) Thiết lập các phương trình động học Robot ............................................................................ 11
3) Bài toán động học thuận................................................................................................... 13
4) Bài toán động học ngược .................................................................................................. 18
CHƯƠNG 3: Bài toán tĩnh học .................................................................................................. 21
1) Tính toán lý thuyết .......................................................................................................... 21
2) Thay số liệu thực tế . ....................................................................................................... 26
CHƯƠNG 4: Bài toán động lực học........................................................................................... 28
1) Cơ sở lý thuyết .......................................................................................................................... 28
2) Thiết lập phương trình động học ............................................................................................. 30
CHƯƠNG 5: Kiểm nghiệm bền ................................................................................................. 39
CHƯƠNG 6: Thiết kế quỹ đạo chuyển động của Robot ............................................................. 41
1) Khái niệm, yêu cầu và nhiệm vụ thiết kế quỹ đạo................................................................... 41
2) Không gian làm việc của Robot ................................................................................................ 41
3) Thiết kế quỹ đạo trong không gian khớp ................................................................................. 41

CHƯƠNG 7: Xây dựng mô hình toán và điều khiển động cơ ..................................................... 48

I. Giới thiệu chung ................................................................................................................ 48
1) Động cơ Bước là gì ? .................................................................................................... 48
2) Hoạt động ................................................................................................................... 48
3) Ứng dụng .................................................................................................................... 48
II. Điều khiển động cơ bước ................................................................................................. 49
1)Giới thiệu phương pháp điều khiển tần số , băm xung cho động cơ step ...................... 49
2) Nguyên lý .................................................................................................................... 50
3) Thiết kế bộ điều khiển PID và mô phỏng trên MATLAB ................................................ 52
CHƯƠNG 8: Thiết kế hệ thống điều khiển bám quỹ đạo Robot................................................. 61
1) Cơ sở lý thuyết .......................................................................................................................... 61
2) Mô phỏng bằng phần mềm Matlab-simulink .......................................................................... 64
PHỤ LỤC .................................................................................................................................. 70
TÀI LIỆU THAM KHẢO .............................................................................................................. 76

CHƯƠNG 1 Phân tích và lựa chọn kết cấu

đề tài
I. Tổng quan về đề tài .

1) Khái niệm và nguyên lý hoạt động của Robot
Thuật ngữ “Robot” đã được sử dụng lần đầu tiên bởi Karel Capek trong vở
kịch của ông Rosum’s Universal Robots được xuất bản vào năm 1921. Co lẽ đó là
một gợi ý ban đầu cho các nhà sáng chế kỹ thuật về việc sáng chế nhưng cơ cấu,
máy móc bắt chước các hoạt động của con người.
Về mặt kỹ thuật, những robot công nghiệp ngày nay có nguồn gốc từ hai
lĩnh vực kỹ thuật ra đời sớm hơn đó là các cơ cấu điều khiển từ xa (Teleoperator)
và các máy công cụ điều khiển số (NC-Numerially Controoled Machine Toll).
 Robot là thiết bị linh hoạt phục vụ con người
– Có hình dạng (cơ cấu) gần giống tay người
– Có khả năng thao tác tự động
– Có khả năng bắt chước thao tác con người.

 Các bộ phận chính của cánh tay robot
– Cơ cấu chấp hành
– Hệ thống dẫn động
– Hệ thống điều khiển theo chương trình có khả năng lập trình linh
hoạt
– Hệ thống thông tin giám sát.
• Tay máy hoạt động được nhờ các động cơ thông qua các bộ truyền như:
hộp giảm tốc bánh răng, bánh răng - thanh răng, trục vít, khí nén, …
• Kết cấu tay robot vững chắc, hoạt động tin cậy, dễ dàng đạt đến vị trí của
vật thể được tác động.
2) Một số ứng dụng của robot
- Lắp ráp và thử nghiệm các chi tiết khác nhau, lắp ráp các linh kiện
điện tử
- Kiểm tra bảng PC, tự động làm sạch các thành phần mạch
- Đóng gói sản phẩm
- Ứng dụng tự động trong y tế

II. PHÂN TÍCH CÁC THÔNG SỐ KỸ THUẬT CỦA ROBOT

1) Các thành phần kết cấu của Robot
Các khâu chính
Tay robot có 3 bậc tự do, thiết kế cơ khí dạng 3 khớp xoay.
 Thân robot: Là khâu cố định,đặt thẳng đứng giữ robot cố định khi làm
việc, gắn với khâu động 1 qua khớp xoay 1 với trục z01 thẳng đứng
 Khâu 1: khâu dẫn động nằm ngang vuông góc với trục thẳng đứng trong
suốt quá trình làm việc của robot,có khả năng quay xung quanh trục Z0 qua
khớp xoay 1 .
 Khâu 2: Khâu động có khả năng xoay trong mặt phẳng chứa trục Z0 .
 Khâu 3: Giống như khâu 2 , có khả năng quay trong mặt phẳng chứa trục
Z0 .
Hệ dẫn động
 Khớp xoay 1 : hệ bánh răng(hộp giảm tốc), truyền động đai…
 Khớp xoay 2 , 3 : truyền động bánh răng vi sai ăn khớp .
 Động cơ truyền động : Servo , Step ....
 Sử dụng hộp giảm tốc , biến tần hoặc PWM .... để điều khiển tốc độ .
Hệ điều khiển
 Dùng biến tần kết nối với PLC .

 Dùng các mạch nguyên lý như mạch băm xung PMW ...
 Dùng các Driver để điều khiển : WHD24....
2) Thông số kỹ thuật
L1 = 240 mm .
L2 = 350mm .

L3 = 325 mm .
Chọn:
3
- θ1=[0; ]
- θ2=[0; ]
- θ3=[0; ]
Hình 2: Các thông số kỹ thuật của Robot 3 DOF RRR
3) Phân tích nguyên lý hoạt động .
Mô hình 3D của robot :
Khâu 0: Khâu cố định.
Khâu 1: Khâu động quay quanh trục thẳng đứng.
Khâu 2: Khâu động quay quanh trục nằm ngang vuông góc với khâu 1.
Khâu 3: Khâu thao tác quay quanh trục nằm ngang vuông góc khâu .

Mô hình 3D của robot

CHƯƠNG 2 Tính toán quỹ đạo chuyển

động
Khảo sát động học thuận, khảo sát động học ngược
1) Bảng D-H
Joint θi di ai αi
1 θ1 d1 0 90◦
2 θ2 0 a2 0◦
3 θ3 0 a3 0◦
Bảng 1: Các tham số Denavit-Hartenberg
Trong đó :

 θ1 , θ2 , θ3 là các biến , góc quay theo thời gian của các khớp .
 d1 , a2 , a3 là các góa trị hằng số , là độ dài của các khâu đã chọn ở bên
trên :
 d1 =240 mm (L1).
 a2 =350 mm (L2).
 a3 = 325mm (L3).
Ta có dạng tổng quát của ma trận Denavit – Hartenberg cho các khâu :
−
i-1 −
Ai = (2.0)
0
0 0 1
Ta có các ma trận DH:
⎡cθ1 0 sθ1 0 ⎤
⎢ sθ 0 − cθ1 0 ⎥
0
A1= ⎢ 1 ⎥ (2.1)
0 1 0
⎢ ⎥
⎣ 0 0 0 1⎦
⎡cθ2 −sθ2 0 a2cθ2 ⎤

⎢ sθ cθ2 0 a2sθ2 ⎥
1
A2= ⎢ 2 ⎥ (2.2)
0 0 1 0 ⎥
⎢
⎣ 0 0 0 1 ⎦
⎡cθ3 −sθ3 0 a3cθ3 ⎤

⎢ sθ cθ3 0 a3sθ3 ⎥
2
A3= ⎢ 3 ⎥(2.3)
0 0 1 0 ⎥
⎢
⎣ 0 0 0 1 ⎦
Trong đó cθ1=cosθ1 ; sθ1=sinθ1 tương tự đối với các góc θ3.
2) Thiết lập các phương trình động học RoBot .

Từ các ma trận (2.1) và (2.2) ta xác định được ma trận biến đổi tọa độ
thuần nhất của hệ Ox3y3z3 so với hệ cố định Ox0y0z0 là :

− s1
0 − −
A2=0A1*1A2= (2.4)
0 +
0 0 0 1
− s1 +
− − + 0 0
0 R3 r3
A3=0A2*2A3= = (2.5)
0 + + 0T 1
0 0 0 1
Ma trận 0A3 cho ta biết hướng và vị trí khâu thao tác trong hệ tọa độ cố định
hay nói cách khác là vị trí của điểm tác động cuối và hướng của hệ tọa độ động
gắn vào khâu tại điểm tác động cuối trong hệ cố định . Vì thế nó còn được biểu
diễn thông qua các biến khớp qi .
Khi đó ma trận (2.8) được kí hiệu thành : 0A3(q) .
Sử dụng các góc Cardan xác định hướng của vật rắn . Gọi [xE ,yE ,zE , , , ]
là giá trị mô tả trục tiếp vị trí và hướng của điểm tác động cuối E so với hệ cố
định .Do các tọa độ đều là hàm của thời gian nên ta biểu diễn :
0
R 0r3
0
A3 = T3 (2.6)
0 1
Với :
0
 RE là ma trận Cardan .
−
0 + − + −
Rn = (2.7)
− + +
0
 rE là vec tơ mô tả vị trí của điểm tác động cuối trong hệ tọa độ
Ox0y0z0 .
0
rE = [ xE (t) , yE (t) , zE (t) ]T (2.8)
 0T = [ 0 , 0 , 0 ] .
Dó đó ma trận 0 A4 (q) biểu diễn vị trí của khâu thao tác trong hệ tọa độ cố
định thông qua biến khớp qi . Còn ma trận 0AE (t) cũng mô tả vị trí và hướng
của khâu thao tác thông qua hệ tọa độ của khâu thao tác . Và ở đây ta chọn là
các goác Cardan .
Từ đó ta có phương trình động học Robot có dạng :

0 A3 (q) = 0AE (t) . ( 2.9)

Từ các hệ thức (2.7) , (2.8) ,(2.9),(2.10) ta xây dựng được hệ 6 phương trình
độc lập như sau :
= ( )[1,4] − ( )[1,4] = + − =0
⎧
⎪ = ( )[2,4] − ( )[2,4] = + − =0
⎪
= ( )[3,4] − ( )[3,4] = a3 + a2s2 + − =0
⎨ = ( )[1,1] − ( )[1,1] = −c ⋅s =0
⎪
⎪ = ( )[2,2] − ( )[2,2] = − + s ⋅ s .s − . = 0
⎩ = ( )[3,3] − ( )[3,3] = −c ⋅ c = 0
3) Bài toán động học thuận
Bài toán động hoc thuận : q = [ , , 3 ]T , ̇ = [ ̇ , ̇ , ̇ ]T .
Ta cần xác định tọa độ điểm tác động cuối E =[xE , yE , zE] và hướng của hệ
cuối trong hệ cơ sở .
Từ hệ các phương trình độc lập bên trên ta có điểm tác động cuối :
= +
= +
= a3 + a2s2 +
+
0
Suy ra rE(t) = + (2.10)
a3 + a2s2 +
Và :
Ta có hướng của khâu thao tác cuối được biểu diễn bởi ma trận :
− s1
0
RE = − − (2.11)
0 0 0
Từ đó ta có thể tính được vận tốc và gia tốc khâu thao tác :
• Vận tốc điểm thao tác cuối :
−( s1 + a1 ) ̇ − ( c1 + a1 ) ̇ − ̇
vE = 0 ̇ (t) = ( c1 + a1 ) ̇ − ( s1 + a1 ) ̇ − ̇ (2.12)
(a3c23 + a2c2) ̇ + a3c23 ̇
Mà VE = JTE. ̇ nên ta có ma trận Jacobi tịnh tiến :

−( s1 + a1 ) −( c1 + a1 ) −
JTE = ( c1 + a1 ) −( s1 + a1 ) − (2.13)
0 (a3c23 + a2c2) a3c23
• Gia tốc điểm thao tác cuối

aE = 0 ̇ = J̇ . ̇ + JTE . ̈
Các biến khớp θ1,θ2, 3 là các biến phụ thuộc thời gian t.
Theo yêu cầu đề bài góc quay các khớp : 135˚ x 270˚ 270˚ .
Ta chọn quy luật chuyển động cho các khớp:
3 3
- θ1= ⇒ ̇ =
- θ2= . ⇒ ̇ = ;
- θ3= . ⇒ ̇ = ;
Chọn:
3
- θ1=[0; ]
- θ2=[0; ]
- θ3=[0; ]
Đơn vị của góc quay là rad và khoảng tịnh tiến là mm.
Số liệu đề bài cho chiều dài các trục d1=240;a2=350;a3=325.
*Tính vận tốc góc các khâu trong hệ tọa độ cơ sở:
Cần tìm được:
 Ma trận cosin chỉ hướng của từng khâu

 Ma trận chuyển vị của các khâu
 Đạo hàm của ma trận cosin chỉ hướng
 Tìm được ma trận sóng ω = Ṙ. R
Từ các ma trận 0A1, 0A2, 0A3, 0A4 đã tìm được ta tìm được các ma trận cosin
chỉ hướng sau:

0
0
R1= 0 − (2.15)
0 1 0
− s1
0
R2= − − (2.16)
0
− s1
0
R3= − − (2.17)
0
Tính đạo hàm của các ma trận cosin chỉ hướng:
− θ̇ 0 θ̇
0 ̇
R1 = θ̇ 0 θ ̇ (2.18)
0 0 0
− ̇ − ̇ ̇ − ̇ θ̇
Ṙ =
0 ̇ − ̇ − ̇ − ̇ θ ̇ (2.19)
̇ − ̇ 0
− ̇ − ( ̇ + ̇ ) ̇ − ( ̇ + ̇ ) θ̇
R3̇ =
0 ̇ − ( ̇ + ̇ ) − ̇ − ( ̇ + ̇ ) θ ̇ (2.20)
( ̇ + ̇ ) − ( ̇ + ̇ ) 0
Từ đó tìm được các ma trận sóng:
0 −θ ̇ 0
0̇ 0 T
ω = R1 R1 = θ ̇ 0 0 (2.21)
0 0 0
0 −θ ̇ − ̇
ω =0R ̇ 0R2T = θ̇ 0 ̇ (2.22)
̇ − ̇ 0
0 −θ ̇ − ( ̇ + ̇ )
ω = θ̇ 0 ( ̇ + ̇ )
( ̇ + ̇ ) − ( ̇ + ̇ ) 0

Từ ma trận sóng ta suy ra các ma trận vận tốc góc:
0 0
ω = 0 = 30 (2.23)
θ̇
3
− ̇ ⎡− sin( )⎤
̇ ⎢ 3 ⎥
ω = = ⎢ cos( ) ⎥(2.24)
θ̇ ⎢ 3 ⎥
⎣ ⎦
3
− sin( )⎤
− ( ̇ + ̇ ) ⎡
⎢ 3 ⎥
ω = ( ̇ + ̇ ) = ⎢ cos( )⎥
θ̇ ⎢ 3 ⎥
⎣ ⎦
Tính gia tốc góc của từng khâu:

0
= ̇ = 0 (2.25)
0
3
− cos( ) = +
= ̇ = − = +
) (2.26)
3
sin(
= a3 + a2s2 +
0
3
− cos( )
= ̇ = − sin(3 )
0
Thay các giá trị , , và các thông số d1=240;a2=350;a3=325 ta được biểu
thức của xE , yE , zE :
3 2 3
 XE =325cos( )cos( )+ 350cos( )cos( ) .
3 2 3
 YE =325sin( )cos( )+ 350sin( )cos( ) .
2
 ZE =325sin( )+ 350sin( ) + 240 .

Đồ thị xE theo thời gian t
Đồ thị yE theo thời gian t

Đồ thị zE theo thời gian t
Không gian thao tác của robot
4) Bài toán động học ngược

Ta biết trước tọa độ của khâu thao tác,yêu cầu cần tính ra được các thông
số tọa độ khớp.Thực tế đây là bài toán thường gặp,vì khi robot làm việc thực tế

trong công nghiệp thường có vùng làm việc xác định.Nhiệm vụ là tính toán thiết
kế cách chuyển động của khớp để thỏa mãn yêu cầu làm việc.
Thường giải bài toán này bằng 2 phương pháp:
_Phương pháp số.
_Phương pháp giải tích.
Nhóm chọn giải bằng phương pháp giải tích vì phạm vi đề bài nhỏ.
Nghiệm của bài toán động học ngược robot là nghiệm của hệ phương trình:
− s1 +
− - c1 + =
0 a3s23 + a2s2+
0 0 0 1 0 0 0 1
Với R= và E= [ ] và hệ 6 phương trình :
= ( )[1,4] − ( )[1,4] = + − =0
⎧
⎪ = ( )[2,4] − ( )[2,4] = + − =0
⎪
= ( )[3,4] − ( )[3,4] = a3 + a2s2 + − =0
⎨ = ( )[1,1] − ( )[1,1] = −c ⋅s =0
⎪
⎪ = ( )[2,2] − ( )[2,2] = − + s ⋅ s .s − . = 0
⎩ = ( )[3,3] − ( )[3,3] = −c ⋅ c = 0
Ta có hệ phương trình sau
+ = (∗)
 + = (∗∗)
a3 + a2s2 + = (∗∗∗)
Ta bình phương (*) và (**) cộng 2 vế với nhau :
( + ) = xE2+yE2
 + =± +

+ =± +
 (****)
a3 + a2s2 = −
Ta lại bình phương 2 phương trình của hệ trên rồi cộng lại với nhau
( )
 =
 =± 1−
 = arctan2( , )
Thế s3 , c3 vào (****) ta được :
( + ) + a3 =± +
a3 +( + a2) = −
Đây là hệ phương trình bậc nhất 2 ẩn sθ , cθ và đặt
(( + ) = A ; a3 = B ta có:
⎧ −A(± + ) + B( − )
⎪ =
A −B
⎨ ± + − .
⎪ =
⎩
=> θ = arctan2( , )
Thế θ và vào (*) và (**) ta suy ra :
=
=>
=
=> = arctan2(s1 ,c1) .

CHƯƠNG 3 Bài toán tĩnh học

1) Tính toán lý thuyết.
Tính các phản lực,ngẫu lực tác dụng lên các khớp của robot để robot đạt trạng
thái cân bằng khi không chuyển động.
Gồm các lực
ROi= iFi+1,i - iPi - iFi,i-1 (1)
MOi = iMi+1,i - i i
Fi,i-1 -i i
Pi - iMi,i-1 (2)
Trong đó (chiếu trong hệ tọa độ thứ i) :
 iFi,i-1 : lực tác dụng từ khâu i-1 sang khâu i tại Oi-1 .
 iFi+1,i : lực tác dụng từ khâu I sang khâu i+1 tại Oi .
 iM1,i-1 : momen lực tác dụng từ khâu i-1 sang khâu i .
 iMi+1,i : momen lực tác dụng từ khâu i sang khâu i+1 .
 -iFi+1,i : lực tác dụng từ khâu i+1 sang khâu i tại Oi .
 -iMi+1,i : momen lực tác dụng từ khâu i+1 sang khâu i .
 ⃗ : vector có gốc tại Oi,mút tại Oi-1 .
 ⃗ : vector có gốc tại Oi,mút tại Ci .
Để đạt điều kiện cân bằng thì :
ROi= iFi+1,i - iPi - iFi,i-1 = 0 (3)
MOi = iMi+1,i - i i
Fi,i-1 -i i
Pi - iMi,i-1 = 0 (4)
Trong đó các ma trận sóng ( ma trận đối xứng lệch ) của các vecto định vị điểm
đặt lực :
0 −izi-1 i
yi-1 0 −izci iyci
i
= i
zi-1 0 −ixi-1 ; = izci 0 −ixci ;
−iyi-1 i
xi-1 0 −iyci ixci 0

Từ (3),(4) ta có :
i
Fi,i-1 = iFi+1,i - iPi (5)
i
Mi,i-1 = iMi+1,i - ii i
Fi,i-1 -i i
Pi (6)
0 − s1
0
R1= 0 − ; 0R2= − − ; 0R3=
0 1 0 0
− s1
− − .
0
Ta xét bài toán tĩnh học trong hệ tọa độ cơ sở.

a) Phương trình lực tác dụng tại khớp O3 của khâu 3 là :
0
F3,2 = 0F4,3 - 0P3
0
M3,2 = 0M4,3 - 0 0
F2,3 -0 0
P3
Giả sử các thanh đồng chất, khối tâm nằm ở chính giữa các thanh .
0
0
 g3 = 0 ;
−
0
0 0 0
 P3=m3 g3 = ;
−
− − − −
 3
r = 0 0
→ r = R3 r = 0 3 − − 0 = − ;
0 0 0 −
0 −
0
→ = − 0 ;
− 0

− − ⎡− ⎤
 3
r = 0 0
→ r 0 3
= R3 r = − −
0 =⎢− ⎥
0 ⎢ ⎥
0 0 ⎣ − ⎦
;
⎡ 0 − ⎤
→0
=⎢− 0 ⎥;
⎢ ⎥
⎣ − 0 ⎦
0
0
 F4,3= 0 ;
0
0
 M4,3=0;
Thay các đại lượng trên vào phương trình ta có:
0 0 0
0 0 0
F3,2 = 0 - = ;
0 −
0 − 0
0 0
M3,2 = - − 0 -
− 0
⎡ 0 − ⎤ 0
⎢− 0 ⎥ 0 = − ;
⎢ ⎥ −
⎣ − 0 ⎦ 0
b) khâu 2 .
0
F2,1 = 0F3,2 - 0P2
0
M2,1 = 0M3,2 - 0 0
F1,2 -0 0
P2
0
0
 g2 = 0 ;
−

0
0 0 0
 P2=m**2 g2 = ;
−
− − s1 − −
2
 r = 0 0
→ r = R2 r = 0 2 − − 0 =− ;
0 0 0 0
0 0 −
→0 = 0 0 ;
− 0
− s1
 2
r = → r 0 0
= R2 r 2
= − − = ;
0 0
0 0 0 0
⎡ 0 0 ⎤
→0 =⎢ 0 0 ⎥;
⎢ ⎥
⎣ 0 ⎦
0
0 0
 F3,2= ;
0
 M3,2= − ;
0
Thay các đại lượng trên vào phương trình ta có:
0 0 0
0 0 0 0
F2,1 = - = ;
− +

0 0 − 0
0 0 0 0
M2,1 = − - -
− 0 +
0
⎡ 0 0 ⎤ 0
⎢ 0 0 ⎥ 0 =
⎢ ⎥ −
⎣ 0 ⎦
+ +
− − − ;
0
c) Khâu 1.
0
F1,0= 0F2,1 - 0P1
0
M1,0 = 0M2,1 - 0 0
F1,0 -0 0
P1
0
0
 g1 = 0 ;
−
0
0 0 0
 P1=m1 g1 = ;
−
0 − s1 0
 1
r = − 0
→ r = R1 r = 0 1 − − − = ;
0 0 0 −
0
→0 = − 0 − ;
− 0
0 − s1 0 ⎡ ⎤
1 − − ⎢ ⎥
 r = → 0r = 0R1 1r = =⎢ ⎥;
0 0 0 ⎢ ⎥
⎣ ⎦

⎡ 0 ⎤
⎢ ⎥
→0 =⎢ 0 − ⎥;
⎢ ⎥
⎣− 0 ⎦
0
0 0
 F2,1= ;
+
+ +
0
 M2,1= − − − ;
0
Thay các đại h ta có:
0 0 0
0 0 0 0
F1,0 = - = ;
+ − ( + + )
+ +
0
M1,0= − − − -
0
0 0
− 0 − 0 -
− 0 ( + + )
⎡ 0 ⎤ 0
⎢ ⎥ 0
⎢ 0 − ⎥ ;
⎢ ⎥ −
⎣− 0 ⎦
Suy ra :
0
 M1,0 x =( + − ) +( − )
+( − )
0
 M1,0 y = (− − + ) + (− + )
+( − ) g.
0
 M1,0 z = 0

2) Thay số liệu thực tế .
Khâu ai (mm) di (mm) Khối lượng mi

(kg)
1 0 240 24,79
2 350 0 7.5
3 325 0 8
*Lấy θ1 = 0 , θ2 = = 45˚ , a2 = 350 , a3 = 325 , d1 = 240
Ta có:
0 0
_0F3,2 = 0 (N), 0M3,2 = 0 (Nm) .
80 0
0 0
_0F2,1 = 0 (N), M2,1= −28,53 (Nm) .
0
155 0
0 0
_0F1,0 = 0 (N) , 0M1,0=
−28,53 (Nm) .
402,9 0

CHƯƠNG 4 Bài toán động lực học

1) Cơ sở lý thuyết
Ở đây nghiên cứu về phân tích động lực học bao gồm :
 Động lực học thuận (direct dynamics) khảo sát tính toán các đại lượng
đặc trưng cho chuyển động dưới tác dụng của lực đã xác định .
 Động lực học ngược ( inverse dynamics) khảo sát tính toán các lực dẫn
động để robot thực hiện được chuyển động thao tác theo quy luật đã xác
định .
 Thiết lập phương trình Lagrange cho hệ nhiều vật rắn .
 Vị trí mỗi vật Bi trong hệ quy chiếu cơ sở R0 (Ox0y0z0) được xác định bởi
vector xác định khối tâm và ma trận cosin chỉ hướng rci=rci(q) và
Ri=Ri(q) .
 Trạng thái vận tốc của vật rắn Bi được xác định bởi vận tốc khối tâm vci
và vận tốc góc của nó ωi .
̇ ; ῶ =Ṙ . T .
drci ∂rci
Với v = =
ci i i i
∂
Tính được các ma trận Jacobi :

∂rci ∂
JTi= và JRi= .
∂ ∂ ̇
Vận tốc khối tâm vật rắn và vận tốc góc :
vci= JTi. ̇ ; = JRi. ̇ (*) .
Động năng của hệ gồm n vật rắn :
T= ∑ (mivciTvci + ωiT iωi) (**) .
Trong đó I là ma trận tenxo quán tính của vật rắn thứ i với khối tâm Ci của
nó trong hệ tọa độ cơ sở .
Thay (*) vào (**) ta có :
T= ∑ [mi (JTi. )̇T JTi. ̇ + (JRi. ̇ )T i JRi. ̇] = ̇T∑ (mi JTiTJTi + JRiT iJRi) ̇ .

Đặt M(q)= ∑ (mi JTiTJTi + JRiT iJRi) ta có :
T= ̇ T M(q) ̇ trong đó :
m11(q) … m1n(q) q1
M(q)= ⋮ ⋱ ⋮ ; q= ⋮ ;
mn1(q) … mnn(q) qn
=>T= ∑ ∑ mij(q1,....,qn) ̇ q̇ j
Thế năng của hệ :

n
T
∏= m g r
i=1
i ci trong đó gT =[gx gy gz]T
Phương trình Lagrange loại 2 :

∂T ∂T ∂∏
̇ − =− + Qi + Ui với i=1->n
∂qi ∂qi ∂qi
 qi-Tọa độ suy rộng thứ i .

 Qi- Lực suy rộng của các lực không thế ứng với tọa độ suy rộng qi .
 Ui-Lực/momen điều khiển ứng với tọa độ suy rộng qi .
Dạng ma trận của phương trình Lagrange loại 2 :
M(q) ̈ + C(q, ̇ ) ̇ + G(q) + Q = U
G(q)= [gj(q)]nx1
∂∏
gj(q)=
∂qj
C(q, ̇ ) ̇ =[cj(q, ̇ ) ̇ ]nx1

∂mkj ∂mlj ∂mkl
cj(q, ̇ ) ̇ =∑ , (k,l;j) qk̇ q̇ l = ( + − )qk̇ q̇ l .
∂ql ∂qk ∂qj

∂mkj ∂mlj ∂mkl

Kí hiệu (k,l;j) = ( + − ).
∂ql ∂qk ∂qj
2) Thiết lập phương trình động học

Các tọa độ suy rộng là :
[q1 q2 q3 q4 ]T=[θ1 , θ2 , 3 ]T
Từ phần mềm ta suy ra được tọa độ trọng tâm theo hệ tọa độ khâu :
Khâu Vị trí trọng tâm so với tọa độ Khối Momen quán tính khối từng khâu
gắn trên khâu lượng
xGi yGi ZGi Ixx Iyy Izz Ixy Ixz Iyz
1 0 87 I1x I1y I1z 0 0 0
2 0 0 12.5 I2x I2y I2z 0 0 0
3 0 0 14 I3x I3y I3z 0 0 0
Bảng 2:Thông số vị trí khối tâm,khối lượng và momen quán tính của các khâu
Với khâu 1 : xc1= -82.33 ; yc1 = -172,1 , zc1 ≈ 0 .
Khâu 2 :xc2 = -175,5 ; yc2 = zc2 ≈ 0 .
Khâu 3 :xc3 = -152.15 ; yc3 =0 ; zc3 =0 .
Coi các khâu được chế tạo bởi vật rắn đồng chất,đối xứng nên Ixy,Ixz,Iyz=0 .
a. Vị trí và hướng của hệ tọa độ Cixciycizci có gốc là ci biểu diễn trong hệ tọa
độ cơ sở xác định bởi ma trận :
i ( ) ( )
0
A = 0 A . Aci = (5.1)
Trong đó :
 0
A – ma trận biến đổi tọa độ thuần nhất của từ khâu i sang khâu 0 .
i
 Aci – ma trận biến đổi tọa độ thuần nhất của hệ tọa độ Cixciycizci biểu
diễn trong khâu i .
Từ công thứ (5.1) ta có :

⎡cθ1 0 sθ1 0 ⎤ 1 0 0 c1 0 c1
⎢
0Ac1 = sθ1
0 − cθ1 0 ⎥ 0 1 0 0 −
⎢ 0 ⎥ 0 = s1 s1 ;
1 0 0 1 0 0 1 0 +
⎢ ⎥
⎣ 0 0 0 1⎦ 0 0 0 1 0 0 0 1
− s1 1 0 0
0Ac2
− − 0 1 0 0
= =
0 + 0 0 1 0
0 0 0 1 0 0 0 1
− s1 +
− − +
0 + +
0 0 0 1
− s1 + 1 0 0
0Ac3
− − + 0 1 0 0
= =
0 + + 0 0 1 0
0 0 0 1 0 0 0 1
− s1 + +
− − + +
0 + + +
0 0 0 1
*Ma trận tenxo quán tính của khâu i đối với khối tâm Ci trong hệ tọa độ động
gắn với khâu i .
Các giá trị a2 = 350 , a3 =325 , d1 = 240 .
Xét khâu 1:
Hệ tọa độ động C1x1y1z1
J c1xx J c1xy J c1xz  I1x 0 0

 
1
c1  J c1yx J c1yy J c1yz    0 I1y 0
 
 J c1zx 
J c1zy J c1zz   0 0 I1z 

Tọa độ của khối tâm C1 :

- 0rc1=[
c1 ; s1 ; + ]T
Vận tốc khối tâm :
(− ) ̇1
 vc1 = 0 ̇ = ( ) ̇1 ;
0
Xác định ma trận Jacobi tịnh tiến
− 0 0 − 0
 JTc1= 0 0 => JTc1T= 0 0 0;
0 0 0 0 0 0
Lại có:
0 0 − ̇
ω = Rc10 T0 ̇
Rc1 = 0 => wc1 = [ 0 , ̇ ,0] ;
0 0
̇ 0 0
=>Ma trận Jacobi quay :

0 0 0 0 0 0
 JRc1= 0 1 0 => JRc1 = 0 1 0 ;
T
0 0 0 0 0 0
+Khâu 2
Ma trận ten xơ quán tính của khâu đối với khối tâm C2 trong hệ tọa độ động
C2xC2yC2zC2 .

 
2
Θc2 =  J c2yx J c2yy J c2yz  =  0 I2y 0
 
 J c2zx J c2zy J c2zz   0 0 I2z 

+Tọa độ của khối tâm c2
 0rc2=[ + , + , + + ]T
Đặt L = ( + ).
Suy ra : 0rc2 = .
+
+Vận tốc khối tâm c2 :

̇ − − ̇
.
 vc2= 0 rc2 = ̇ − ̇ ;
̇
+Ma trận Jacobi tịnh tiến
− − 0
 JTc2= − 0 ;
0 0
− 0
 =>JTc2 = −
T − ;
0 0 0
+Ma trận Jacobi quay :
Có :
0 −θ ̇ ̇
ω =0Rc2T 0Rc2̇ = θ̇ 0 − ̇ => wc2 = [ ̇ , ̇ , θ̇ ]
− ̇ ̇ 0
0 0 0
=>JRc2= 0 0 ; JRc2T= 0 0 1 ;
0 1 0 0 0 0
+Khâu 3
Ma trận tenxo quán tính của khâu đối với khối tâm C3 trong hệ tọa độ C3x3y3z3 .

 
3
Θc3 =  J c3yx J c3yy J c3yz  =  0 I3y 0
 
 J c3zx J c3zy J c3zz   0 0 I3z 

+Tọa độ của khối tâm C3 :
[( + ) + ]
- = [(
0rc3 + ) + ]
( + ) + +
Đặt k1 = [(x + a )c + a c ] ; k2 =(x + a )s + a s ; k3 = (x + a )s ;
k4 =(x + a )c .

Suy ra : 0rc3 = .
+
+Vận tốc khối tâm :
.
− θ̇ − ̇ − ̇
- vc3= 0 rc3 = ̇ − ̇ − ̇ ;
̇ + ̇
+Ma trận tịnh tiến Jacobi
− − −
- JTc3= − − ;
0
− 0
- => JTc3T= − − ;
− −
+Ma trận quay Jacobi .
Ta có :
0 −θ ̇ − ̇ ̇
̇
ω =0Rc3T 0 Rc3 = θ ̇ + ̇ ̇
0 −
− ̇ ̇ 0
=> wc3 = [ ̇ , ̇ , θ̇ + ̇ ]
0 0 0
- JRc3= 0 0 ;=> JRc3T= 0 0 1 ;
0 1 1 0 0 1
b. Ma trận khối lượng .
M(q)= [ ( + )] .
 M1(q)= + .

− 0 − 0 0 0 0 0 1 0 0
=m1 0 0 0 0 0 +0 1 0 0 1 0
0 0 0 0 0 0 0 0 0 0 0 1
0 0 0
0 1 0
0 0 0
0 0
= 0 1 0 ;
0 0 0
 M2(q)= + .
− 0 − − 0
=m2 − − − 0
0 0 0 0 0
0 0 0 0 0
+ 0 0 1 0 0 0 0 =
0 0 0 0 0 0 1 0
c m + + 0 0
0 s m +I 0
0 0 0
 M3(q)= +
− 0 − − −
=m3 − − − − +
− − 0
0 0 0 0 0
0 0 1 0 0 0 0
0 0 1 0 0 0 1 1
=
+ + 0 0
0 ( + ) + ( + ) +
0 ( + ) + +
 M(q)=M1(q)+M2(q)+M3(q) .
c. Sau khi xác định được M(q) ta có biểu thức động năng .
Dạng ma trận :
M(q) = ;
Trong đó :
 = + + + + + +
.
 = +( + ) + + .
 =( + ) + .
 =( + ) + .
 =2 + .
 m12=m21 = = =0.
Với L = ( + ) ; k1 = [(x + a )c + a c ] ; k2 =(x + a )s + a s ; k3 =
(x + a )s ; k4 =(x + a )c .
d. Xác định ma trận C(q, ̇ ) ̇ =[c1 c2 c3 ]T
Với cj = ,
( , , ) ̇ ̇ ta có :
c1= +2 +2 − − −
c2=2 + +2 +2 +2 − −
c3=2 +2 +2 +2 + − −
Thay số vào ta được :
 c1=[−2 −2 ] ̇ − ̇ − ̇ −
k1k3c1m3 ̇ .
 c2= ̇ +[ +2 −2 ] ̇ +
[2 −2 ] ̇3 + ̇ +[ ( + ) +
( − ) ] ̇
3− [− − ] ̇ −2 ̇
 c3= ̇ +[ ( + ) + ( − ) ] ̇
2+
4 ̇ +2 ̇ − 3
̇ −[2
3 −2 ] ̇
3
 ..
=>C  q, q  q = ;
 
Xác định biểu thức thế năng :

= (q) =migzci .
g =[0 –g 0]T
− =( + )
=> − =( + )
− =( + )
G(q)=[gj(q)]3x1

 g1(q)= =0
q1

 g2(q)= =L g .
q2

 g3(q)= = m3g .
q3
0
=> G(q)= g .
m3g
Tính các lực suy rộng của các lực không thế.Robot gắp vật bỏ qua ảnh hưởng
của ngoại lực,lực ma sát cản trở chuyển động thì lực suy rộng chỉ có trọng lực
của phôi tác dụng lên khâu 3,hướng thẳng đứng chiều hướng xuống dưới
Q=[Q1 Q2 Q3]T với

Qj= ∑ ( + )
Do F= 0 và M = 0 nên ta có :
=>Q=0
Vậy ta có phương trinh dưới dạng ma trận
.. . .
M  q q +C q,q  q +G(q)+Q=U
 
Từ đó giải được hệ phương trình đại số :
 ( + + +1 + + ) ̈ + [−2 −
1
2 ] ̇ − ̇ − ̇ − k1k3c1m3 ̇ = U1 .
 ( + + + + ) ̈ +( + +
) ̈ + ̇ +[ +2 −2 ] ̇ +
[2 −2 ] ̇
3 + ̇ +[ ( + ) +

( − ) ] ̇
3− [− − ] ̇ −2 ̇ +
g = U2 .
 ( + + ) ̈ + (2 + ) ̈ +
̇ +[ ( + ) + ( − ) ] ̇ 2+ 4 ̇ +
2 ̇ − ̇
3 3 −[2 −2 ] ̇ 3 + m3g = U3 .
Với L = ( + ) ; k1 = [(x + a )c + a c ] ; k2 =(x + a )s + a s ; k3 =
(x + a )s ; k4 =(x + a )c .

CHƯƠNG 5 Kiểm nghiệm bền
Vật liệu của robot là thép có [ ] = 6. 10
Tiến hành mô phỏng đặt lực và momen vào các khâu của robot sử dụng phần
mềm solidwork ta thu được kết quả ứng suất tác dụng vào các khâu như sau:
 Ứng suất lớn nhất tác dụng lên khâu 1 là: 9.103 .
 Ứng suất lớn nhất tác dụng lên khâu 2 là: 9,2.104 .

 Ứng suất lớn nhất tác dụng lên khâu 3 là: 1,9.104 .
Ứng suất lớn nhất tác dụng vào các khâu là 9,2. 10 < [ ] = 6. 10 => Các
khâu đủ bền .

CHƯƠNG 6 Thiết kế quỹ đạo chuyển

động của Robot
1) Khái niệm , yêu cầu và nhiệm vụ thiết kế quỹ đạo .
Việc xác định quỹ đạo chuyển động của khâu thao tác của Robot theo các
tiêu chuẩn công nghệ được hiểu là thiết kế quỹ đạo . Mục tiêu là tạo ra bộ tham
số đầu vào cho hệ thống điều khiển để đảm bảo Robot thực hiện quỹ đjao đã
được lập trình .
Có hai kỹ thuật thiết kế quỹ đjao chính là :
 Thiết kế quỹ đạo trong không gian khớp .
 Thiết kế quỹ đạo trong không gian thao tác .
Yêu cầu bài toán :thiết kế quỹ đạo chuyển động trong không gian thao tác với
luật chuyển động là hàm bậc 3 theo theo biến thời gian t
2) Không gian làm việc của Robot .
Từ các thông số của Robot :
 Chiều dài các khâu là : L1 = 240mm ; L2 = 350 mm ; l3 = 325 mm .
 Giới hạn các góc khớp :
q1 [0; 32 ] rad .
q2 [0 ; 23 ] rad.
q3 [0; 23 ]rad.
3) Thiết kế quỹ đạo trong không gian khớp .
Trong trường hợp chuyển động từ điểm tới điểm , người ta có thể quan tâm
hoặc không quan tâm đến việc khâu thao tác đi qua một số điểm trung gian .
Khi đó kỹ thuật thiết kế quỹ đạo trong không gian khớp hay được sử dụng .
thiết kế quỹ đạo trong không gian khớp đòi hỏi phải xây dựng được các hàm
phụ thuộc thời gian của các biến khớp và các đjao hàm bậc nhất , bậc hai của
nó . Qua đó mô tả được chuyển động cần thiết của Robot .
a) Đề bài :
Mô tả : Ban đầu, từ vị trí gắp vật ,robot di chuyển vị trí khâu cuối tịnh tiến
theo trục z đi lên 1 đoạn. Tiếp theo robot đưa khâu thao tác đi theo nửa cung

tròn để sang vị trí đặt phôi. Cuối cùng,robot hạ điểm thao tác theo trục z đến vị
trí đặt mong muốn rồi thả vật.
Chọn phương án dịch chuyển các khâu sao cho các tọa độ khớp qk (k
= , …)là các đa thức bậc 3 của thời gian như sau :
= + + +
̇ = + +
̈ = +
Gọi qi0,qie lần lượt là vị trí đầu và vị trí cuối của quỹ đạo .
ti là thời gian dịch chuyển từ vị trí đầu đến vị trí cuối của đoạn quỹ đạo .
te là tổng thời gian chuyển động từ vị trí O sau đó quay về vị trí O .
Vận tốc điểm đầu và điểm cuối quỹ đạo chọn bằng 0 .
- Áp dụng công thức để tính các hệ số trong pt (1) cho từng đoạn quỹ đạo
chuyển động:
ai0 = qi0
ai1 = 0
( )
ai2 =
( )
ai3 =
+ Phương trình vận tốc cho từng đoạn quỹ đạo: ̇ = +2 +3 .
+ Phương trình gia tốc cho từng đoạn quỹ đạo: ̈ = 2 +6 .
b) Ví dụ cụ thể .
Quá trình chuyển động gồm 3 giai đoạn:
+ Giai đoạn 1: robot đi từ A(300,400,200) tới điểm B(300,400,500) với quỹ
đạo là đường thẳng .

+ Giai đoạn 2: robot đi từ B(300,400,500) tới điểm C(300,-400,500) với quỹ

đạo là đường tròn tâm H(100,0,500) ,bán kính R = √200 + 400 = 200 5
(mm) .
+ Giai đoạn 3:robot di từ C(300,-400,500) tới D(300,-400,200) với quỹ đạo là
đường thẳng .
Thiết kế quỹ đạo chuyển động trong không gian thao tác :
 Giai đoạn 1: robot đi từ A(300,400,200) tới điểm B(300,400,500) với quỹ
đạo là đường thẳng .
• Ta có quy luật : = + + + .
• Chọn thời gian di chuyển : t1 = 2s .
• Quãng đường di chuyển: q = 300 .
• Điểm đầu A(300,400,200).
• Điểm cuối B(300,400,500).
Suy ra q10 = 200 , q1e = 500 .
Từ đó ta có :

= 200
⎧ =0
⎪
3(500 − 200)
= = 225
⎨ 2
⎪ 2(200 − 500)
⎩ = = −75
2
Vậy nên :
= + −
̇ = −
̈ = −
Đồ thị quỹ đạo , vận tốc , gia tốc theo thời gian gđ 1
 Giai đoạn 2: robot đi từ B(300,400,500) tới điểm C(300,-400,500) với quỹ
đạo là đường tròn tâm H(100,0,500) ,bán kính R = √200 + 400 = 200 5
(mm) .
• Ta có quy luật : = + + + .
• Quãng đường di chuyển(độ dài cung): q = 2r.arctan (2) .
• Điểm đầu B(300,400,500) .
• Điểm cuối C(300,-400,500) .

Suy ra q10 = r.arctan(2) , q1e = - r.arctan(2) .
Từ đó ta có :
= 4456
⎧ =0
⎪
3(−4456 − 4456)
= = −6684
⎨ 2
⎪ 2(4456 + 4456)
⎩ = = 2228
2
Vậy nên :
= − +
̇ = − +
̈ =− +

 Giai đoạn 3:robot di từ C(300,-400,500) tới D(300,-400,200) với quỹ đạo là
đường thẳng .
• Ta có quy luật : = + + + .
• Quãng đường di chuyển: q = 300 .
• Điểm đầu C(300,-400,500) .
• Điểm cuối D(300,-400,200) .
Suy ra q10 = 500 , q1e = 200 .
Từ đó ta có :
= 500
⎧ =0
⎪
3(200 − 500)
= = −225
⎨ 2
⎪ 2(500 − 200)
⎩ = = 75
2
Vậy nên :

= − +
̇ = − +
̈ =− +

CHƯƠNG 7 Xây dựng mô hình toán

và khảo sát điều khiển động cơ
I. Giới thiệu chung
1. Động cơ bước là gì?
Động cơ bước là một loại động cơ điện có nguyên lý và ứng dụng khác biệt
với đa số các động cơ điện thông thường. Chúng thực chất là một động cơ đồng
bộ dùng để biến đổi các tín hiệu điều khiển dưới dạng các xung điện rời rạc kế
tiếp nhau thành các chuyển động góc quay hoặc các chuyển động của rôto có
khả năng cố định rôto vào các vị trí cần thiết.
Về cấu tạo, động cơ bước có thể được coi là tổng hợp của hai loại động cơ:
Động cơ một chiều không tiếp xúc và động cơ đồng bộ giảm tốc công suất nhỏ.
2. Hoạt động
Động cơ bước không quay theo cơ chế thông thường, chúng quay theo
từng bước nên có độ chính xác rất cao về mặt điều khiển học. Chúng làm việc
nhờ các bộ chuyển mạch điện tử đưa các tín hiệu điều khiển vào stato theo thứ
tự và một tần số nhất định. Tổng số góc quay của rôto tương ứng với số lần
chuyển mạch, cũng như chiều quay và tốc độ quay của rôto phụ thuộc vào thứ
tự chuyển đổi và tần số chuyển đổi.
3. Ứng dụng
Trong điều khiển chuyển động kỹ thuật số, động cơ bước là một cơ cấu
chấp hành đặc biệt hữu hiệu bởi nó có thể thực hiện trung thành các lệnh đưa
ra dưới dạng số.

Động cơ bước được ứng dụng nhiều trong ngành Tự động hoá, chúng được
ứng dụng trong các thiết bị cần điều khiển chính xác. Ví dụ: Điều khiển robot,
điều khiển tiêu cự trong các hệ quang học, điều khiển định vị trong các hệ
quan trắc, điểu khiển bắt, bám mục tiêu trong các khí tài quan sát, điều khiển
lập trình trong các thiết bị gia công cắt gọt, điều khiển các cơ cấu lái phương và
chiều trong máy bay...
Trong công nghệ máy tính, động cơ bước được sử dụng cho các loại ổ đĩa
cứng, ổ đĩa mềm, máy in...
II. Điều khiển động cơ bước
Trong phạm vi đồ án này, nhóm điều khiển động cơ bước dựa trên
phương pháp “Điều chế độ rộng xung”
1. Giới thiệu phương pháp điều khiển tần số , băm xung cho động cơ step .
Điều chế độ rộng xung (tiếng Anh: Pulse-width modulation (PWM)),

hay Điều chế thời gian xung (tiếng Anh: pulse-duration modulation (PDM)), là
một kỹ thuật điều chế được sử dụng để mã hóa một thông điệp thành một tín
hiệu xung. Mặc dù kỹ thuật điều chế này có thể được sử dụng để mã hóa thông
tin để truyền tải, việc sử dụng chính của nó là cho phép điều khiển nguồn điện
cung cấp cho các thiết bị điện, đặc biệt là để tải quán tính như động cơ. Ngoài
ra, PWM là một trong hai thuật toán chính được sử dụng trong bộ sạc pin
quang điện năng lượng mặt trời, thuật toán kia là giám sát điểm công suất cực
đại.
Giá trị trung bình của điện áp (và dòng điện) cung cấp cho tải được kiểm
soát bằng cách thay đổi việc đóng cắt giữa nguồn và tải tắt với tốc độ rất
nhanh. Thời gian đóng càng lâu so với thời gian cắt, thì tổng công suất cung cấp
cho tải càng cao.

Tần số đóng cắt PWM phải cao hơn nhiều so với tần số ảnh hưởng đến tải
(các thiết bị sử dụng điện), để dạng sóng cuối cùng được đưa tới tải phải càng
mịn càng tốt. Tốc độ (hoặc tần số) mà tại đó các nguồn cấp phải đóng cắt có
thể rất khác nhau tùy thuộc vào tải và ứng dụng, ví dụ
Việc đóng cắt phải được thực hiện nhiều lần một phút đối với một bếp
điện; 120 Hz trong một bộ dimmer đèn sợi đốt; giữa một vài kiloHertz (kHz),
cho đến hàng chục kHz trong một bộ điều khiển động cơ; và khoảng vài chục
hoặc vài trăm kHz trong các bộ khuếch đại âm thanh và các bộ nguồn máy tính.
Thuật ngữ chu kỳ làm việc mô tả tỷ lệ giữa thời gian “bật” với thời gian điều
chỉnh hoặc “chu kỳ” làm việc; chu kỳ làm việc thấp tương ứng với công suất
thấp, bởi vì nguồn điện bị cắt trong phần lớn thời gian điều chỉnh. Chu kỳ làm
việc được thể hiện theo phần trăm, 100% là bật hoàn toàn.
Ưu điểm chính của PWM đó là tổn hao công suất trên các thiết bị đóng cắt
(Chuyển mạch) rất thấp. Khi khóa chuyển mạch tắt thì không có dòng điện nào
đi qua, và khi bật thì nguồn sẽ được đưa sang phụ tải, thì hầu như không có
sụt áp trên thiết bị chuyển mạch. Tổn hao công suất, là tích của điện áp và
dòng điện, do đó trong cả hai trường hợp gần như bằng không. PWM cũng
hoạt động tốt với điều khiển kỹ thuật số, mà vì tính chất bật/tắt, ta có thể dễ
dàng thiết lập chu kỳ làm việc cần thiết.
PWM cũng đã được sử dụng trong một số hệ thống truyền thông, trong đó
chu kỳ làm việc của nó được sử dụng để truyền tải thông tin qua một kênh
truyền thông.
2. Nguyên lý
Điều chế độ rộng xung sử dụng một sóng xung hình chữ nhật có độ rộng
được điều chế dẫn đến sự biến thiên của giá trị trung bình của dạng sóng. Nếu
chúng ta xem xét một sóng xung ( ), với chu kỳ , giá trị thấp , giá trị
cao , và chu kỳ làm việc D (xem hình 1), giá trị trung bình của dạng sóng
đó được cho bởi:

Hình 1: một sóng xung, thể hiện các định nghĩa của , và D.
1
= ( ) .
Vì ( ) là một sóng xung, giá trị của nó là trong khoảng < <
. , và trong khoảng . < < . Biểu thức trên trở thành:
1
= +
1
= ( . . + (1 − ) )
= . + (1 − ) .
Biểu thức sau này có thể khá đơn giản trong nhiều trường hợp trong đó =
0 khi = . . Từ đó, rõ ràng là giá trị trung bình của tín hiệu ( ) trực tiếp
phụ thuộc vào chu kỳ làm việc D.
Cách đơn giản nhất để tạo ra một tín hiệu PWM là phương pháp giao thoa,
chỉ yêu cầu cần có một sóng răng cưa hoặc sóng tam giác (dễ dàng tạo ra bằng
cách sử dụng một bộ tạo dao động đơn giản) và một mạch so sánh. Khi giá trị của
tín hiệu tham chiếu (tín hiệu đặt) (sóng sin màu đỏ trong hình 2) lớn hơn sóng
điều biến (màu xanh lam), thì tín hiệu PWM (màu đỏ tía) sẽ ở trạng thái cao, nếu
không thì ở trạng thái thấp.

Hình 2: Một phương pháp đơn giản để tạo ra mạch xung PWM tương
ứng với một tín hiệu cho trước là PWM giao thoa: tín hiệu (ở đây là sóng sin
màu đỏ) được so sánh với dạng sóng răng cưa (màu xanh lam). Khi tín hiệu
sau là nhỏ hơn tín hiệu trước, tín hiệu PWM (màu đỏ tía) sẽ ở trạng thái cao
(1). Nếu không thì nó ở trạng thái thấp (0).
3. Thiết kế bộ điều khiển PID và mô phỏng trên MATLAB

a) Mô hình toán học:
Mô hình động cơ bước nam châm vĩnh cửu (PMS)
Phần này cung cấp một dẫn xuất ngắn gọn của một mô hình phi tuyến của
động cơ bước 2 pha PM.
Phương trình động lực học của trục rotor được đưa ra bởi:
= − − (1)

trong đó: – mô men quán tính của trục rotor
F – hệ số ma sát nhớt
– mô men tải
Phương trình (1) tạo cơ sở cho mô hình động lực học chung của rotor động
cơ bước PM. Do đó với động cơ PM có P cặp cực rôto và hai pha ( ) tại 0 và ( /
2), các phương trình không gian trạng thái sau có thể được suy ra:
= (− sin + cos )− −
=− + sin
+ (2)
=− − cos −
trong đó: là gia tốc góc của tải
là đạo hàm dòng điện qua cuôn dây a
là đạo hàm dòng điện qua cuôn dây b
Phương trình xét trục nối đàn hồi:
Xem xét trường hợp khi tải được kết nối với động cơ qua một trục cứng dài
có độ cứng k, thì quán tính của động cơ sẽ là và quán tính tải sẽ là như trong
hình 3.

Hình 3 - Sơ đồ nối trục đàn hồi
Trong trường hợp này, phương trình tải trọng và trục đàn hồi sẽ là:
= (3)
1
= [− ( − )− ( − )]
Một kỹ thuật điều khiển cổ điển cho PMSM dựa trên biến đổi vectơ (u) và
(i) được biểu diễn trong hệ quy chiếu gắn với stato cố định (a, b) thành các vectơ
biểu diễn trong một khung (d, q) xoay dọc theo vectơ lực dẫn động giả tưởng sao
cho:
cos sin
=
−sin cos
Phương trình trạng thái (2) thể hiện dưới dạng dòng điện và điện áp trong
tọa độ xoay (d, q) trở thành:
1
= − − − [ ( − )+ ( − )]
=− + .
+ (4)
=− − . − +

b) Mô phỏng với MATLAB Simulink

Trong MATLAB, chạy file Thong_so_mo_hinh_hoa_Stepper_motor.m
chứa các thông số của động cơ bước.
% momen quan tinh cua dong co

Jm = 0.08; % kg.m^2
% he so can nhot cua khop noi truc

B = 3; % Nms/rad
% dien cam cua cuon day

L = 3; % Henry
% dien tro cua dong co

R = 3; % Omh
% so cap cuc
P = 6; % khong thu nguyen
% he so cua dong co
Km = 2; % Nm/rad
% he so can nhot cua truc dong co

F = 0.01; % Nms/rad
% momen tai
TL = 5; % Nm
k = 0.9;
Tiến hành mô phỏng hệ phương trình (4) với Simulink
Trong Simulink tạo sơ đồ khối như sau:

Hình 4 – Sơ đồ Simulink mô phỏng động cơ bước
Hình 5 – Khối Subsystem “Dong co buoc“
Chạy mô phỏng và Tune bộ điều khiển PID, ta nhận được kết quả như sau:

Hình 6 – Tham số PID điều khiển vận tốc góc
Hình 7 - Kết quả đáp ứng vận tốc góc
Hình 8 – Tham số PID điều khiển vị trí góc quay

Hình 9 - Kết quả đáp ứng vị trí góc quay
Nhận xét: Đánh giá bộ điều khiển
Từ kết quả đáp ứng nhận được trên các đồ thị, ta rút ra một số nhận xét:
+ Bộ điều khiển vị trí góc quay tương đối đạt yêu cầu khi độ
quá điều chỉnh và thời gian đáp ứng nhỏ
+ Bộ điều khiển vận tốc góc chưa đạt yêu cầu do độ quá điều
chỉnh rất lớn. Do vậy, bộ điều khiển này phải cần tiếp tục nghiên cứu
và khắc phục
c) Điều khiển động cơ bước thực tế với vi điều khiển:
Phần cứng:
- Arduino UNO
- Driver điều khiển động cơ A4988
- Breadboard
- Nguồn cấp 12V (dòng điện tối thiểu 1A)

- Dây nối
Kết nối driver, động cơ bước vào Arduino như sau:
Code Arduino tham khảo

// Run a A4998 Stepstick from an Arduino UNO.
// Paul Hurley Aug 2015 - http://www.instructables.com/id/Drive-
a-Stepper-Motor-with-an-Arduino-and-a-A4988-/
int x;
#define BAUD (9600)
void setup()
{

Serial.begin(BAUD);
pinMode(6,OUTPUT); // Enable pin - chân khởi động - nối vào
GND sẽ giúp ta bật động cơ bước, nối vô VCC động cơ bước được
thả ra. Nôm na: GND = servo.attach, VCC = servo.detach
pinMode(5,OUTPUT); // Step pin
pinMode(4,OUTPUT); // Dir - pin
digitalWrite(6,LOW); // Set Enable low
}
void loop()
{
digitalWrite(6,LOW); // Đặt Enable ở trạng thái LOW
digitalWrite(4,HIGH); // Đặt Dir ở trạng thái HIGH
Serial.println("Cho chay 200 steps (1 vong)");
for(x = 0; x < 200; x++) // Cho chay 1 vong
{
digitalWrite(5,HIGH); // Output high
delay(10); // chờ
digitalWrite(5,LOW); // Output low
delay(100); // chờ
}
Serial.println("Pause");
delay(1000); // dừng 1 s rồi quay tiếp
}

CHƯƠNG 8 Thiết kế hệ thống điều

khiển bám quỹ đạo Robot
1) Cơ sở lý thuyết .
Ở các phần trước nhóm đã tính toán, xác định quy luật biến thiên của các
biến khớp theo thời gian, tương ứng với quỹ đạo công tác của robot theo yêu
cầu. Phần này sẽ trình bày việc điều khiển robot sao cho chúng có thể thực
hiện được đúng các chuyển động mong muốn.
Phương pháp điều khiển tuyến tính chỉ thích hợp với các hệ điều khiển
được mô hình hóa bởi các phương trình vi phân tuyến tính. Tuy nhiên trong
phần Động lực học robot chúng ta đã nhận thấy, hệ phương trình động lực
của chúng ta là các phương trình vi phân phi tuyến, do vậy các biện pháp xấp
xỉ sẽ được sử dụng để phù hợp với yêu cầu của bài toán điều khiển tuyến tính.
Xuất phát trực tiếp từ hệ phương trình vi phân chuyển động đã được
nghiên cứu trong phần Động lực học hệ robot. Phương pháp điều khiển áp
dụng là phương pháp điều khiển lực (mô men) thường được sử dụng để điều
khiển cho mô hình của nhóm.
Mục tiêu của bài toán điều khiển là làm sao cho robot bám theo quỹ đạo
đã được thiết kế. Các phần tử dẫn động làm việc theo cách nhận lệnh điều
khiển và sinh ra lực (mô men). Mô men đầu ra sẽ được sử dụng để tính toán
mô men mong muốn tiếp theo.
Vấn đề cốt lõi của việc thiết kế bộ điều khiển robot là làm thế nào để bảo
đảm rằng bộ điều khiển được thiết kế sẽ đáp ứng tốt các yêu cầu làm việc cho
trước. Tiêu chí cơ bản quan trọng nhất là hệ phải đảm bảo ổn định. Nghĩa là
đảm bảo thời gian quá độ, độ quá điều chỉnh và sai số quỹ đạo đủ nhỏ theo
yêu cầu đặt ra cho dù hệ có phải chịu tác động của một số nhiễu trong suốt
quá trình làm việc.
Trước hết ta có mô hình toán học của hệ thống đã được xây dựng từ các
phần trước là hệ phương trình vi phân động học:
M(q) ̈ + C(q, ̇ , ̈ ) + G(q) – Q = u

Trong đó vế trái của phương trình là mô hình với các tham số của robot, vế
phải là momen mà bộ điều khiển tác động lên robot. Momen này được sinh
ratừ bộ điều khiển với giá trị thỏa mãn yêu cầu đã nêu ở trên.
Chúng ta phải xét xem cấu trúc của bộ điều khiển như thế nào thì có thể
đáp ứng mục tiêu thiết kế. trược hết chúng ta chia bộ điều khiển thành hai
phần: một phần dựa trên mô hình và một phần dựa trên phản hồi.
u= +
Trong đó α, β là các hàm được chọn lựa sao cho nếu τ với tư cách là đầu
vào mới của hệ, thì hệ trở thành hệ khối lượng đơn vị.
Với cấu trúc này của luật điều khiển, phương trình của hệ trở thành:
M(q) ̈ + C(q, ̇ , ̈ ) + G(q) – Q = +
Với mục tiêu biến hệ thành hệ khối lượng đơn vị với u là thành phần đầu
vào thì α, β có thể được chọn:
= ( )
= ( , ̇ , ̈) + ( ) –
Sau khi chọn được α, β thì phương trình còn lại là:
̈ =u
Chúng ta còn lại tham số cuối cùng cần chọn để thu được hệ khối lượng
đơn vị :
= ̈ + ( − )+ ( − )
Trong đó các giá trị đầu vào là quy luật vị trí, quy luật vận tốc, quy luật gia
tốc mong muốn đã được tính toán trong phần thiết kế quỹ đạo:
( )
̇ ( )
̈ ( )

Và các hệ số Kd , Kp là các ma trận đường chéo vuông cấp n (n là số tham

số động học của mô hình robot), phần sau chúng ta sẽ xem xét chi tiết việc
chọn các hệ số Kd , Kp này theo các điều kiện ràng buộc và mục tiêu bài toán.
Ta tìm được biểu thức đặc trưng cho hệ điều khiển vòng kín robot:
̈+ ̇+ e=0
Với:
= −
̇ = ̇ − ̇
̈ = ̈ − ̈
Hệ phương trình trên bao gồm các phương trình độc lập, các ma trận
Kd , Kp là các ma trận đường chéo, do vậy phương trình này có thể viết riêng
cho từng khớp:
̈+ ̇+ =0
Theo lí thuyết về dao động kĩ thuật, khi nhìn vào phương trình trên ta có
thể nhận xét đây là phương trình dao động tự do có cản, ứng xử của hệ
thống phụ thuộc vào hệ số kvi và kpi. Trong đó mục tiêu của chúng ta là đưa
đáp ứng của hệ càng sát với giá trị mong muốn càng tốt, khoảng thời gian đạt
được đáp ứng càng nhanh càng tốt. Do đó, giá trị nghiệm hướng tới là
trường hợp nghiệm kép, giá trị thực, và trường hợp này gọi là trường hợp tới
hạn khi k d  2 k P .
Từ nội dung trên ta có thể xây dựng được sơ đồ khối của mô hình hệ thống
như sau:

Từ sơ đồ trên có thể nhận thấy rằng chúng ta đang điều khiển hệ thống
trong không gian khớp theo thời gian. Tín hiệu đặt và tín hiệu phản hồi đều
tương ứng với từng khớp (vị trí, vận tốc,gia tốc).
2) Mô phỏng bằng phần mềm Matlab- simulink .

Từ cơ sơ lí thuyết ở trên ta tiến hành xây dựng chương trình mô phỏng điều
khiển hệ thống trên Toolbox Simulink trong phần mêm Matlab :

 Khối đầu vào :
 Khối PD :
Với Kp=[625 625 625]
Kd=[50 50 50]
 Khối Robot :

 Kết quả nhận được :

• q và q_dat :

• dq và dq_dat :


Đáp ứng của quỹ đạo đã thiết kế ở chương 6 :

Phụ lục
%CODE TINH LUC DAN DONG U

function u = dieukhien_phituyen(u1)
%thong so dau vao
m=3; %[kg]
g=9.81; %[m/s2]
n=3; %so bac tu do
q=u1(1:n);q1=q(1);q2=q(2);q3=q(3);
dq=u1(n+1:2*n);dq1=dq(1);dq2=dq(2);dq3=dq(3);
ddq=u1(2*n+1:3*n);ddq1=ddq(1);ddq2=ddq(2);ddq3=ddq(3);
% khau 1
m1=15.52; %[kg]
I1x=.13; %[kg m2]
I1y=.18; %[kg m2]
I1z=.14; %[kg m2]
a1=.17; %[m]
L1=.24; %[m]
%Khau 2
m2=6.44; %[kg]
I2x=0.01; %[kg m2]
I2y=0.09; %[kg m2]
I2z=0.1; %[kg m2]
a2=.18; %[m]
L2=0.350; %[m]
%khau 3
m3=6.64; %[kg]
I3x=.01; %[kg m2]
I3y=.08; %[kg m2]
I3z=0.09; %[kg m2]
a3=.15; %[m]
L3=0.325; %[m]
%MA TRAN KHOI LUONG M

m11=I1y + I3y*cos(q2 + q3)^2 + I3x*sin(q2 + q3)^2 + I2y*cos(q2)^2 +
I2x*sin(q2)^2 + m3*sin(q1)^2*(L3*cos(q2 + q3) - a3*cos(q2 + q3) +
L2*cos(q2))^2 + m3*cos(q1)^2*(L3*cos(q2 + q3) ...
- a3*cos(q2 + q3) + L2*cos(q2))^2 + m2*cos(q2)^2*sin(q1)^2*(L2 - a2)^2 +
m2*cos(q1)^2*cos(q2)^2*(L2 - a2)^2;
m12=0;m13=0;m21=0;m31=0;
m22= I2z + I3z + L2^2*m2 + L2^2*m3 + L3^2*m3 + a2^2*m2 + a3^2*m3 - 2*L2*a2*m2
- 2*L3*a3*m3 + 2*L2*L3*m3*cos(q3) - 2*L2*a3*m3*cos(q3);
m23=m3*L3^2 - 2*m3*L3*a3 + L2*m3*cos(q3)*L3 + m3*a3^2 - L2*m3*cos(q3)*a3 +
I3z;
m32=m23;
m33= m3*L3^2 - 2*m3*L3*a3 + m3*a3^2 + I3z;
M= [m11,m12,m13;m21,m22,m23;m31,m32,m33];

%MA TRAN C
c11=(I2x*dq2*sin(2*q2))/2 - (I2y*dq2*sin(2*q2))/2 + (I3x*dq2*sin(2*q2 +
2*q3))/2 + (I3x*dq3*sin(2*q2 + 2*q3))/2 - (I3y*dq2*sin(2*q2 + 2*q3))/2 -
(I3y*dq3*sin(2*q2 + 2*q3))/2 ...
- (L2^2*dq2*m2*sin(2*q2))/2 - (L2^2*dq2*m3*sin(2*q2))/2 -
(a2^2*dq2*m2*sin(2*q2))/2 - (L3^2*dq2*m3*sin(2*q2 + 2*q3))/2 -
(L3^2*dq3*m3*sin(2*q2 + 2*q3))/2 - (a3^2*dq2*m3*sin(2*q2 + 2*q3))/2 ...
- (a3^2*dq3*m3*sin(2*q2 + 2*q3))/2 + L2*a2*dq2*m2*sin(2*q2) +
L3*a3*dq2*m3*sin(2*q2 + 2*q3) + L3*a3*dq3*m3*sin(2*q2 + 2*q3) -
(L2*L3*dq3*m3*sin(q3))/2 + (L2*a3*dq3*m3*sin(q3))/2 ...
- L2*L3*dq2*m3*sin(2*q2 + q3) - (L2*L3*dq3*m3*sin(2*q2 + q3))/2 +
L2*a3*dq2*m3*sin(2*q2 + q3) + (L2*a3*dq3*m3*sin(2*q2 + q3))/2;
c12=-(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) - I2x*sin(2*q2) +
I2y*sin(2*q2) + a3^2*m3*sin(2*q2 + 2*q3) + L2^2*m2*sin(2*q2) +
L2^2*m3*sin(2*q2) + a2^2*m2*sin(2*q2)...
+ L3^2*m3*sin(2*q2 + 2*q3) + 2*L2*L3*m3*sin(2*q2 + q3) -
2*L2*a3*m3*sin(2*q2 + q3) - 2*L2*a2*m2*sin(2*q2) - 2*L3*a3*m3*sin(2*q2 +
2*q3)))/2;
c13=-(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) + a3^2*m3*sin(2*q2 +
2*q3) + L3^2*m3*sin(2*q2 + 2*q3) + L2*L3*m3*sin(q3)...
- L2*a3*m3*sin(q3) + L2*L3*m3*sin(2*q2 + q3) - L2*a3*m3*sin(2*q2 + q3) -
2*L3*a3*m3*sin(2*q2 + 2*q3)))/2;
c21=(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) - I2x*sin(2*q2) +
L2^2*m3*sin(2*q2) ...
+ a2^2*m2*sin(2*q2) + L3^2*m3*sin(2*q2 + 2*q3) + 2*L2*L3*m3*sin(2*q2 +
q3) - 2*L2*a3*m3*sin(2*q2 + q3) - 2*L2*a2*m2*sin(2*q2) - 2*L3*a3*m3*sin(2*q2
+ 2*q3)))/2;
c22=-L2*dq3*m3*sin(q3)*(L3 - a3);
c23=-L2*m3*sin(q3)*(dq2 + dq3)*(L3 - a3);
c31=(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) + a3^2*m3*sin(2*q2 +
2*q3) + L3^2*m3*sin(2*q2 + 2*q3) ...
+ L2*L3*m3*sin(q3) - L2*a3*m3*sin(q3) + L2*L3*m3*sin(2*q2 + q3) -
L2*a3*m3*sin(2*q2 + q3) - 2*L3*a3*m3*sin(2*q2 + 2*q3)))/2;
c32=L2*dq2*m3*sin(q3)*(L3 - a3);
c33=0;
C=[c11,c12,c13;c21,c22,c23;c31,c32,c33];
%MA TRAN LUC SUY RONG CO THE G

G=[0;g*m3*(cos(q2 + q3)*(L3 - a3) + L2*cos(q2)) + g*m2*cos(q2)*(L2 -
a2);g*m3*cos(q2 + q3)*(L3 - a3)];
%LUC KO THE
Q=[ 0;-m*g*(L3*cos(q2 + q3) + L2*cos(q2));-m*g* L3*cos(q2 + q3)];
% PHUONG TRINH VI PHAN

u=M*ddq+C*dq+G-Q;

% CODE TINH GIA TOC KHOP DDQ

function ddq = robot_model(u)
%thong so dau vao
m=3; %[kg]
g=9.81; %[m/s2]
n=3; %so bac tU do
% Thong so tinh toan
q=u(1:n);q1=q(1);q2=q(2);q3=q(3);
dq=u(n+1:2*n);dq1=dq(1);dq2=dq(2);dq3=dq(3);
U=[u(2*n+1);u(2*n+2);u(2*n+3)];
% khau 1
m1=15.52; %[kg]
I1x=.13; %[kg m2]
I1y=.18; %[kg m2]
I1z=.14; %[kg m2]
a1=.17; %[m]
L1=.24; %[m]
%Khau 2
m2=6.44; %[kg]
I2x=0.01; %[kg m2]
I2y=0.09; %[kg m2]
I2z=0.1; %[kg m2]
a2=.18; %[m]
L2=0.350; %[m]
%khau 3
m3=6.64; %[kg]
I3x=.01; %[kg m2]
I3y=.08; %[kg m2]
I3z=0.09; %[kg m2]
a3=.15; %[m]
L3=0.325; %[m]
%MA TRAN KHOI LUONG M

m11=I1y + I3y*cos(q2 + q3)^2 + I3x*sin(q2 + q3)^2 + I2y*cos(q2)^2 +
I2x*sin(q2)^2 + m3*sin(q1)^2*(L3*cos(q2 + q3) - a3*cos(q2 + q3) +
L2*cos(q2))^2 + m3*cos(q1)^2*(L3*cos(q2 + q3) ...
- a3*cos(q2 + q3) + L2*cos(q2))^2 + m2*cos(q2)^2*sin(q1)^2*(L2 - a2)^2 +
m2*cos(q1)^2*cos(q2)^2*(L2 - a2)^2;
m12=0;
m13=0;
m21=0;
m22= I2z + I3z + L2^2*m2 + L2^2*m3 + L3^2*m3 + a2^2*m2 + a3^2*m3 - 2*L2*a2*m2
- 2*L3*a3*m3 + 2*L2*L3*m3*cos(q3) - 2*L2*a3*m3*cos(q3);
m23=m3*L3^2 - 2*m3*L3*a3 + L2*m3*cos(q3)*L3 + m3*a3^2 - L2*m3*cos(q3)*a3 +
I3z;
m31=0;
m32=m23;
m33= m3*L3^2 - 2*m3*L3*a3 + m3*a3^2 + I3z;
M= [m11,m12,m13;m21,m22,m23;m31,m32,m33];
%MA TRAN C
c11=(I2x*dq2*sin(2*q2))/2 - (I2y*dq2*sin(2*q2))/2 + (I3x*dq2*sin(2*q2 +
2*q3))/2 + (I3x*dq3*sin(2*q2 + 2*q3))/2 - (I3y*dq2*sin(2*q2 + 2*q3))/2 -
(I3y*dq3*sin(2*q2 + 2*q3))/2 ...

- (L2^2*dq2*m2*sin(2*q2))/2 - (L2^2*dq2*m3*sin(2*q2))/2 -
(a2^2*dq2*m2*sin(2*q2))/2 - (L3^2*dq2*m3*sin(2*q2 + 2*q3))/2 -
(L3^2*dq3*m3*sin(2*q2 + 2*q3))/2 - (a3^2*dq2*m3*sin(2*q2 + 2*q3))/2 ...
- (a3^2*dq3*m3*sin(2*q2 + 2*q3))/2 + L2*a2*dq2*m2*sin(2*q2) +
L3*a3*dq2*m3*sin(2*q2 + 2*q3) + L3*a3*dq3*m3*sin(2*q2 + 2*q3) -
(L2*L3*dq3*m3*sin(q3))/2 + (L2*a3*dq3*m3*sin(q3))/2 ...
- L2*L3*dq2*m3*sin(2*q2 + q3) - (L2*L3*dq3*m3*sin(2*q2 + q3))/2 +
L2*a3*dq2*m3*sin(2*q2 + q3) + (L2*a3*dq3*m3*sin(2*q2 + q3))/2;
c12=-(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) - I2x*sin(2*q2) +
L2^2*m3*sin(2*q2) + a2^2*m2*sin(2*q2)...
+ L3^2*m3*sin(2*q2 + 2*q3) + 2*L2*L3*m3*sin(2*q2 + q3) -
2*L2*a3*m3*sin(2*q2 + q3) - 2*L2*a2*m2*sin(2*q2) - 2*L3*a3*m3*sin(2*q2 +
2*q3)))/2;
c13=-(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) + a3^2*m3*sin(2*q2 +
2*q3) + L3^2*m3*sin(2*q2 + 2*q3) + L2*L3*m3*sin(q3)...
- L2*a3*m3*sin(q3) + L2*L3*m3*sin(2*q2 + q3) - L2*a3*m3*sin(2*q2 + q3) -
2*L3*a3*m3*sin(2*q2 + 2*q3)))/2;
c21=(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) - I2x*sin(2*q2) +
L2^2*m3*sin(2*q2) ...
+ a2^2*m2*sin(2*q2) + L3^2*m3*sin(2*q2 + 2*q3) + 2*L2*L3*m3*sin(2*q2 +
q3) - 2*L2*a3*m3*sin(2*q2 + q3) - 2*L2*a2*m2*sin(2*q2) - 2*L3*a3*m3*sin(2*q2
+ 2*q3)))/2;
c22=-L2*dq3*m3*sin(q3)*(L3 - a3);
c23=-L2*m3*sin(q3)*(dq2 + dq3)*(L3 - a3);
c31=(dq1*(I3y*sin(2*q2 + 2*q3) - I3x*sin(2*q2 + 2*q3) + a3^2*m3*sin(2*q2 +
2*q3) + L3^2*m3*sin(2*q2 + 2*q3) ...
+ L2*L3*m3*sin(q3) - L2*a3*m3*sin(q3) + L2*L3*m3*sin(2*q2 + q3) -
L2*a3*m3*sin(2*q2 + q3) - 2*L3*a3*m3*sin(2*q2 + 2*q3)))/2;
c32=L2*dq2*m3*sin(q3)*(L3 - a3);
c33=0;
C=[c11,c12,c13;c21,c22,c23;c31,c32,c33];
%MA TRAN LUC SUY RONG CO THE G

G=[0;
g*m3*(cos(q2 + q3)*(L3 - a3) + L2*cos(q2)) + g*m2*cos(q2)*(L2 - a2);
g*m3*cos(q2 + q3)*(L3 - a3)];
%LUC KO THE
Q=[ 0;
-m*g*(L3*cos(q2 + q3) + L2*cos(q2));
-m*g* L3*cos(q2 + q3)];
% PHUONG TRINH VI PHAN

ddq=M\(U-G-C*dq+Q);

%CODE LAP TRINH QUY DAO

function [x,y,z]= fcn(t)
%#codegen
r=sqrt(0.2^2+0.4^2);
if (t<=2)
x=0.3;
% dx=0;
% ddx=0;
y=0.4;
% dy=0;
% ddy=0;
t0=0;
tf=2;
a=tim_he_so_bac3(t0,tf,0.2,0.5,0,0);
a0=a(1);a1=a(2);a2=a(3);a3=a(4);
z=a0+a1*t+a2*t^2+a3*t^3;
% dz=a1+a2*2*t+a3*3*t^2;
% ddz=2*a2+6*a3*t;
elseif ((t>2)&&(t<=4))
t0=2;
tf=4;
a=tim_he_so_bac3(t0,tf,atan(2)*r,-atan(2)*r,0,0);
a0=a(1);a1=a(2);a2=a(3);a3=a(4);
x=0.1+r*cos((a0+a1*t+a2*t^2+a3*t^3)/r);
% dx=-(a1+a2*2*t+a3*3*t^2)*sin((a0+a1*t+a2*t^2+a3*t^3)/r);
% ddx=-(2*a2+6*a3*t)*sin((a0+a1*t+a2*t^2+a3*t^3)/r)...
% -(a1+a2*2*t+a3*3*t^2)^2/r*cos((a0+a1*t+a2*t^2+a3*t^3)/r);
y=r*sin((a0+a1*t+a2*t^2+a3*t^3)/r);
% dy=(a1+a2*2*t+a3*3*t^2)*cos((a0+a1*t+a2*t^2+a3*t^3)/r);
% ddy=(2*a2+6*a3*t)*cos((a0+a1*t+a2*t^2+a3*t^3)/r)...
% -(a1+a2*2*t+a3*3*t^2)^2/r*sin((a0+a1*t+a2*t^2+a3*t^3)/r);
z=0.5;
% dz=0;
% ddz=0;
else
x=.3;
% dx=0;
% ddx=0;
y=-.4;
% dy=0;
% ddy=0;
t0=4;
tf=6;
a=tim_he_so_bac3(t0,tf,.5,.2,0,0);
a0=a(1);a1=a(2);a2=a(3);a3=a(4);
z=a0+a1*t+a2*t^2+a3*t^3;
% dz=a1+a2*2*t+a3*3*t^2;
% ddz=2*a2+6*a3*t;

end
%CODE DONG HOC NGUOC

function [q1,q2,q3]= fcn(x,y,z)
%#codegen
L1=.2400;
L2=.3500;
L3=.32500;
r=sqrt(x^2+y^2);
cq3=(r^2+(z-L1)^2-L2^2-L3^2)/(2*L2*L3);
sq3=sqrt(1-cq3^2);
q1=atan2(y,x);
q2=atan2(z-L1,r)-atan2(L3*sq3,L2+L3*cq3);
q3=atan2(sq3,cq3);
%CODE DONG HOC THUAN

function [x,y,z] = fcn(q1,q2,q3)
%#codegen
L1=.2400;
L2=.3500;
L3=.32500;
x=cos(q1)*(L3*cos(q2 + q3) + L2*cos(q2));
y=sin(q1)*(L3*cos(q2 + q3) + L2*cos(q2));
z= L1 + L3*sin(q2 + q3) + L2*sin(q2);
%%%%%% CODE PHỤ %%%%%%

%CODE TIM HE SO BAC 3 CUA LUAT CHUYEN DONG HAM BAC 3
function a = tim_he_so_bac3(t0,tf,s0,sf,ds0,dsf )
M=[1 t0 t0^2 t0^3;
0 1 2*t0 3*t0^2;
1 tf tf^2 tf^3;
0 1 2*tf 3*tf^2];
s=[s0;ds0;sf;dsf];
a=M\s;

TÀI LIỆU THAM KHẢO
1. Wikipedia
https://vi.wikipedia.org/wiki/%C4%90%E1%BB%99ng_c%C6%A1_b%C6%B0%E1%
BB%9Bc (Động cơ bước)
https://vi.wikipedia.org/wiki/%C4%90i%E1%BB%81u_ch%E1%BA%BF_%C4%91%E
1%BB%99_r%E1%BB%99ng_xung (Điều chế độ rộng xung)
2. arduino.vn
http://arduino.vn/bai-viet/685-huong-dan-dieu-khien-dong-co-buoc
3. PGS.TS Phan Bùi Khôi . Bài giảng Robotic. Trường ĐH Bách Khoa Hà Nội
2014.
4. Tài liệu trên internet.
5. GS.TSKH Nguyễn Văn Khang . Cơ sở Robot công nghiệp . Nhà xuất bản
Khoa học và Kỹ thuật, Hà Nội 2011.

Đồ Án Môn Học: Trường Đại Học Bách Khoa Hà Nội Viện Cơ Khí

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Đồ Án Môn Học: Trường Đại Học Bách Khoa Hà Nội Viện Cơ Khí

Uploaded by

Copyright:

Available Formats

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

Giảng viên hướng dẫn: TS. Nguyễn Kiên Trung

Sinh viên thực hiện: Bùi Văn Mạnh

Em xin chân thành cảm ơn!

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 2

LỜI MỞ ĐẦU .............................................................................................................................. 2

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 3

CHƯƠNG 7: Xây dựng mô hình toán và điều khiển động cơ ..................................................... 48

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 4

CHƯƠNG 1 Phân tích và lựa chọn kết cấu

I. Tổng quan về đề tài .

 Robot là thiết bị linh hoạt phục vụ con người

– Có hình dạng (cơ cấu) gần giống tay người

– Có khả năng thao tác tự động

– Có khả năng bắt chước thao tác con người.

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 5

 Các bộ phận chính của cánh tay robot

– Cơ cấu chấp hành

– Hệ thống dẫn động

– Hệ thống thông tin giám sát.

2) Một số ứng dụng của robot

- Đóng gói sản phẩm

- Ứng dụng tự động trong y tế

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 6

II. PHÂN TÍCH CÁC THÔNG SỐ KỸ THUẬT CỦA ROBOT

Tay robot có 3 bậc tự do, thiết kế cơ khí dạng 3 khớp xoay.

 Khớp xoay 1 : hệ bánh răng(hộp giảm tốc), truyền động đai…

 Khớp xoay 2 , 3 : truyền động bánh răng vi sai ăn khớp .

 Động cơ truyền động : Servo , Step ....

 Dùng biến tần kết nối với PLC .

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 7

Hình 2: Các thông số kỹ thuật của Robot 3 DOF RRR

3) Phân tích nguyên lý hoạt động .

Mô hình 3D của robot :

Khâu 0: Khâu cố định.

Khâu 1: Khâu động quay quanh trục thẳng đứng.

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 8

Mô hình 3D của robot

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 9

CHƯƠNG 2 Tính toán quỹ đạo chuyển

Bảng 1: Các tham số Denavit-Hartenberg

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 10

Ta có các ma trận DH:

⎡cθ2 −sθ2 0 a2cθ2 ⎤

⎡cθ3 −sθ3 0 a3cθ3 ⎤

Trong đó cθ1=cosθ1 ; sθ1=sinθ1 tương tự đối với các góc θ3.

2) Thiết lập các phương trình động học RoBot .

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 11

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 12

0 A3 (q) = 0AE (t) . ( 2.9)

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 13

• Gia tốc điểm thao tác cuối

Cần tìm được:

 Ma trận cosin chỉ hướng của từng khâu

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 14

Tính đạo hàm của các ma trận cosin chỉ hướng:

Từ đó tìm được các ma trận sóng:

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 15

Từ ma trận sóng ta suy ra các ma trận vận tốc góc:

Tính gia tốc góc của từng khâu:

Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 16

Đồ thị xE theo thời gian t

Đồ thị yE theo thời gian t