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

VIỆN ĐIỆN- ĐIỆN TỬ

BÁO CÁO ĐỒ ÁN II
ĐỀ TÀI: ĐIỀU KHIỂN CÁNH TAY ROBOT ĐỂ MỞ TAY NẮM CỬA

Giảng viên hướng dẫn: PGS.TS Nguyễn Phạm Thục Anh

Chuyên ngành: Tự động hóa công nghiệp
Sinh viên: Nguyễn Mạnh Hiếu
MSSV: 20191835

Hà Nội, 6/2/2023

 MỤC LỤC
LỜI NÓI ĐẦU...............................................................................................................2
CHƯƠNG I: TỔNG QUAN VỀ CÁNH TAY ROBOT..............................................3
1. Tổng quan về cánh tay Robot:.....................................................................3
1.1. Giới thiệu:.......................................................................................................3
1.2. Phân loại cánh tay Robot:................................................................................3
2. Các bài toán điều khiển cánh tay Robot:....................................................4
CHƯƠNG 2: YÊU CẦU BÀI TOÁN VỀ CÁNH TAY ROBOT................................5
1. Yêu cầu thiết kế:............................................................................................5
2. Giới thiệu về cánh tay Robot 4 bậc tự do....................................................5
CHƯƠNG 3: TÍNH TOÁN ĐỘNG HỌC TAY MÁY................................................7
Phần 1: Động học thuận vị trí..................................................................................7
Phần 2: Bài toán Động học ngược..........................................................................10
Phần 3: Tính toán Ma trận Jacoby........................................................................12
Phần 4: Thiết kế quỹ đạo........................................................................................17
Phần 5: Động lực học..............................................................................................17
CHƯƠNG 4: MÔ PHỎNG TRÊN SIMULINK........................................................26
1. Động học thuận:..........................................................................................26
2. Động học nghịch:.........................................................................................27
3. Thiết kế quỹ đạo:.........................................................................................27
4. Khối mô hình toán học Robot:...................................................................28
5. Bộ điều khiển...............................................................................................29
6. Mô phỏng.....................................................................................................29
7. Nhận xét sau mô phỏng:.............................................................................34
CHƯƠNG 5: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN...........................................35
1. Kết luận........................................................................................................35
2. Hướng phát triển.........................................................................................35
TÀI LIỆU THAM KHẢO...........................................................................................36

1
 LỜI NÓI ĐẦU

Ngày nay cùng với sự phát triển không ngừng của khoa học kĩ thuật thì sự phát
triển của lĩnh vực cơ điện tử cũng phát triển và được coi là một trong những ngành mũi
nhọn trong quá trình hiện đại hóa và công nghiệp hóa đất nước. Sản phẩm đặc trưng của
cơ điện tử là ROBOT và CNC.

Trong khuôn khổ nội dung môn học Kĩ thuật robot em được nghiên cứu một
trong hai sản phẩm đặc trưng của cơ điện tử đó là robot, cụ thể trong báo cáo này em
thiết kế và mô phỏng bộ điều khiển của robot bốn bậc tự do RTRR. Robot này là robot
dung trong việc giải cứu thoát hiểm khẩn cấp trong những điều kiện con người khó hoạt
động ví dụ như là môi trường xảy ra hỏa hoạn.

Qua đồ án cùng với sự giúp đỡ và tài liệu mà cô Thục Anh cung cấp, em đã bước
đầu làm quen với việc tính toán robot: Về cấu trúc động học của robot, cơ sở lý thuyết
tính toán, thiết kế quỹ đạo, thiết kế bộ điều khiển mô phỏng hoạt động của robot.

Với kiến thức còn nhiều hạn chế nên khó tránh khỏi báo cáo còn nhiều thiết sót,
mong các thầy cô chỉ bảo thêm cho em.

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

2
 CHƯƠNG I: TỔNG QUAN VỀ CÁNH TAY ROBOT
1. Tổng quan về cánh tay Robot:

1.1. Giới thiệu:

Cánh tay Robot là cỗ máy chuyên phục vụ vào quá trình sản xuất công nghiệp và
được vận hành bởi con người. Chúng có ưu điểm là thiết kế linh hoạt dễ thao tác với
tác dụng hoàn thiện những phần chi tiết nhỏ nhất cho sản phẩm.
Cánh tay robot hoạt động rất linh hoạt với cơ chế khớp ngón như bàn tay con
người. Chính những ưu điểm nổi bật mà những cỗ máy cơ khí này ngày càng được
ứng dụng trong các lĩnh vực công nghiệp khác nhau. Cỗ máy có thể thay thế bàn tay
con người trong việc lắp ráp, sửa chữa hay thay thế các linh phụ kiện nhỏ trong các
nhà máy.
Cánh tay robot là bước tiến tuyệt vời trong quy trình tự động hóa sản xuất. Không
chỉ phục vụ quá trình lắp ráp sản phẩm mà còn góp phần xử lý những công việc phức
tạp trong sản xuất.

1.2. Phân loại cánh tay Robot:

Hiện nay, trên thị trường có rất nhiều loại cánh tay Robot với nhiều đặc điểm khác
nhau, phù hợp với nhiều công việc khác nhau. Dưới đây là 1 số loại cánh tay Robot:
 Robot giàn: Loại máy này có ba khớp lăng trụ ứng dụng trong việc lắp đặt
những linh kiện hay xử lý những công cụ máy.
 Robot hình trụ: Được ứng dụng trong những hoạt động lắp ráp hay xử lý
máy móc công cụ, hàn điểm hay xử lý ở các máy Diecasting. Trong cấu tạo,
chúng có một trục để tạo nên hệ tọa độ hình trụ.
 SCARA robot: Thiết kế hai khớp quay song song hoạt động trên một mặt
phẳng. Chúng được ứng dụng trong những hoạt động lắp ráp hay xử lý các
chi tiết máy.
 Robot nhân tạo: Được mô phỏng theo chuyển động con người, thiết kế bàn
tay và ngón cái chuyển động độc lập với nhau. Chính sự linh hoạt mà cho
phép chúng tham gia vào việc lắp ráp và xử lý các loại linh kiện điện tử hay
công nghệ xe siêu vi.
 Robot khớp nối: Thường thiết kế với ba khớp quay. Đối với những loại máy
phổ biến là cánh tay robot 3 bậc, 4 bậc… Theo đó, cấu trúc bao gồm hai
nhánh đơn giản đến những hệ thống có 10 hoặc nhiều khớp tương tác hơn.
Loại cánh tay robot khớp nối thường được trang bị cho những phương tiện gồm
động cơ điện để ứng dụng trong những hoạt động lắp rán, hàn hồ quang, hàn khí hay

3
phun sơn. Với tính ứng dụng cao vì thế, robot khớp nối được nhiều xí nghiệp sản xuất
lớn sử dụng bởi tính hiện đại, năng suất cao với tính linh hoạt.

2. Các bài toán điều khiển cánh tay Robot:

Động học tay máy là bài toán nghiên cứu chuyển động của tay máy mà không quan
tâm đến tính chất về lực tương tác cũng như về khối lượng mà chỉ xét đến cấu trúc
hình học của tay máy.
Động học tay máy gồm hai bài toán cơ bản: động học thuận vị trí và động học đảo
vị trí.
- Động học thuận là xác định vị trí của khâu tác động cuối của Robot khi biết các
biến khớp của Robot.
- Động học ngược là xác định góc quay của các khớp khi biết vị trí khâu tác
động cuối của robot.

4
CHƯƠNG 2: YÊU CẦU BÀI TOÁN VỀ CÁNH TAY ROBOT
1. Yêu cầu thiết kế:
- Thiết kế tay robot có thể mở được tay nắm cửa loại tròn và loại tay nắm cài với
cách mở khóa là xoay ngược hoặc thuận chiều kim đồng hồ ổ khóa

- Phạm vi hoạt động : 1m-1,5m

- Cân nặng : 3kg-5kg
- Số bậc tự do cần thiết là 4

Chọn thiết kế Robot 4 bậc tự do với 3 khớp quay và 1 khớp tịnh tiến: 
 2 khớp quay loại R 
 1 khớp quay loại T 
 1 khớp tịnh tiến 
- Hạn chế: Chỉ mở được các ổ khóa không bị rỉ rét do lực không đủ khỏe

2. Giới thiệu về cánh tay Robot 4 bậc tự do

Hình 1. Cánh tay robot

5
 Cánh tay Robot có khớp nối là loại robot vô cùng phổ biến. Và mỗi khi nhắc đến
Robot chúng ta sẽ lập tức nghĩ ngay đến chúng. Giống như các nhà máy CNC,
Robot khớp nối được phân loại theo số điểm quay hoặc số trục mà chúng có. Phổ
biến nhất là robot khớp 4 trục. Ngoài ra còn có các loại robot 6 trục và 7 trục cũng
rất phổ biến trên thị trường. 
Tính linh hoạt, khéo léo và khả năng tiếp cận khiến robot có khớp nối là sự lựa
chọn phù hợp lý tưởng cho các nhiệm vụ thực hiện trên các mặt phẳng không song
song, chẳng hạn như chỉnh sửa máy móc. 
Robot có khớp nối cũng có thể dễ dàng tiếp cận khoang máy công cụ và dưới các
vật cản để tiếp cận phôi. Hoặc thậm chí xung quanh vật cản, trong trường hợp này
là robot 7 trục.

Ưu điểm
Các khớp nối kín và ống bảo vệ cho phép Robot có khớp nối hoạt động tốt trong
môi trường sạch cũng như môi trường bẩn. Khả năng năng lắp cánh tay Robot 4 bậc
tự do này trên bất kỳ bề mặt nào. ( Ví dụ: trần nhà, đường ray trượt). Điều này cho
phép doanh nghiệp có nhiều sự lựa chọn khi làm việc.
Tuy nhiên, sự tinh tế và hiện đại của robot có khớp nối này sẽ đi kèm với giá
thành cao. Do đó, robot này cao hơn so với các loại robot có trọng tải tương tự trên
thị trường.

Khuyết điểm
Và khuyết điểm của dòng Robot này là: Chúng sẽ không phù hợp với các ứng
dụng tốc độ rất cao. Do chúng có hệ thống chuyển động học phức tạp với nhiều chi
tiết bộ phận.

6
 CHƯƠNG 3: TÍNH TOÁN ĐỘNG HỌC TAY MÁY

Phần 1: Động học thuận vị trí.

Hình 2. Mô hình động học

Đây là bước quan trọng cho việc thiết kế robot, từ đó có thể giải bài
toán điều khiển robot theo các quỹ đạo. Từ đây ta mới có đủ các thông
số để điều khiển robot theo một quỹ đạo cho trước hoặc với lực cho
trước ta thu được một quỹ đạo chuyển động nhất định.
1. Xác định các tham số động học D-H. 

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ố như sau: θi , di , ai , α i
 ai là độ dài đường vuông góc chung giữa 2 trục khớp động i+1 và i 
 α i là góc chéo giữa 2 trục khớp động i+1 và i 

7
  di là khoảng cách đo dọc trục khớp động i kể từ đường vuông góc chung
giữa trục khớp động i+1 và trục khớp động i tới đường vuông góc chung giữa
trục khớp động i và trục khớp động i -1 
 θ i là góc giữa 2 đường vuông góc nói trên. Biến khớp: 

Thanh(i) ai αi θi di
1 0 90
0 0
90 +θ 1 0
2 0 −90
0
0 d2
3 0 90
0
θ3 0
4 0 0 θ4 0

Hình 3 . Bảng thông số D-H

2. Tính toán các ma trận Ai

Ma trận biến đổi tọa độ tổng quát là

Do vậy ta sẽ tính được bốn ma trận A1 , A2 , A3 , A4

[ ]
−sin θ1 0 cos θ 1 0
cos θ1 0 sin θ1 0
A1=
0 1 0 0
0 0 0 1

[ ]
1 0 0 0
0 0 1 0
A 2=
0 −1 0 d 2
0 0 0 1

8
 [ ]
cos θ3 0 sin θ3 0
sin θ3 0 −cos θ3 0
A3 =
0 1 0 0
0 0 0 1

[ ]
cos θ4 −sin θ4 0 0
sinθ 4 cos θ4 0 0
A 4=
0 0 1 0
0 0 0 1

Từ các ma trận biến đổi giữa các trục tọa độ của các khớp, ta sẽ xác định được
ma trận vecto cuối, mô tả hướng và vị trí của khâu chấp hành cuối so với hệ tọa độ
gốc.
Sử dụng công cụ matlab để nhập và nhân các ma trận. Ta thu được ma trận

[ ]
−sin θ13 cos θ4 sin θ13 sin θ4 cos θ 13 d 2 cos θ 1
cos θ13 cos θ 4 −cos θ 13 sin θ4 sin θ13 d 2 sin θ 1
T 04= A 1 A2 A 3 A 4 =
sinθ 4 cos θ4 0 0
0 0 0 1

9
Phần 2: Bài toán Động học ngược
1. Sơ lược
Động học ngược vị trí để xác định giá trị các biến khớp tương ứng với vị trí mong
muốn của tay trong không gian. Tức là xác định ma trận các biến khớp

từ vị trí của khâu tác động cuối đã biết, giả sử có giá trị:

Ta có các phương pháp giải vị trí các động học ngược vị trí:
 Phép đảo vị trí: áp dụng cho các robot có hai bậc tự do
 Phép đảo hướng: áp dụng cho phép quay cố định
 Phương pháp phân ly biến: áp dụng cho các robot có số bậc tự
do ≥3
Do vậy ,ta sẽ sử dụng phương pháp phân ly biến để tính toán động học ngược
vị trí cho robot.

2. Tính toán ma trận

[ ]
nx ox ax px
0 n o a py
Ta có T 4= A 1 A2 A 3 A 4 = n y o y ay pz
z z z
0 0 0 1

[ ]
nx sin θ1+ n y cos θ1 −o x sin θ1 +o y cos θ1 −a x sinθ 1+ a y cos θ1 − p x sinθ 1+ p y cos θ 1
nz oz az pz
VT =A−1 0
1 T 4=
nx cos θ1 +n y sin θ1 o x cos θ 1+ o y sinθ 1 a x cos θ1 +a y sin θ1 p x cos θ1 + p y sinθ 1
0 0 0 1

[ ]
cos θ 3 cos θ 4 −cos θ3 sinθ 4 sin θ3 0
sin θ 4 cos θ4 0 0
VP= A 2 A3 A 4 =
−sin θ3 cos θ 4 sinθ 3 cos θ 4 cos θ3 d 2
0 0 0 1

10
 Cho VT =VP, ta thấy:
− p x sin θ1 + p y cos θ1=0 =>θ1=atan 2 ( p y , p x )

{ cos θ3=a x cos θ1 + a y sin θ 1

sin θ3=−ax sin θ1 +a y cos θ1
=> θ3 =atan 2 ( sin θ3 , cos θ3 )

{sin θ4 =n z
cos θ4 =o z
=> θ 4=atan 2 ( sinθ 4 , cos θ4 )

d 2= p x cos θ1+ p y sin θ 1

11
Phần 3: Tính toán Ma trận Jacoby

Nhiệm vụ yêu cầu Robot điều khiển được hướng và vị trí thì vận tốc được điều
khiển trên quỹ đạo hoạt động của nó. Ma trận Jacoby là một trong những đặc tính
quan trọng của tau máy và là công cụ toán học cần thiết dành cho việc phân tích
và điều khiển chức năng cũng như vận hành Robot.

Các bước tính toán ma trận Jacoby theo

Bằng cách đạo hàm các phương trình vị trí của Robot, ta có thể tính được
ma trận Jacoby theo thuật toán gồm 3 bước:

Bước 1: Xác định ma trận

(1)

Bước 2: Xác định ma trận J-H

(2)

12
 Khi là khớp quay, biến khớp
Sử dụng ma trận

(3)
Khi là khớp trượt, biến khớp d i+ 1
Sử dụng ma trận
H H H
∂ px i ∂ py i ∂ pz i
¿ nz ¿ oz ¿ az
∂ θi +1 ∂θ i+1 ∂ θi+1
H H H
∂ Φx ∂ Φy ∂ Φz
(4) =0 =0 =0
∂Φ i+1 ∂ Φi+ 1 ∂Φ i+1

Bước 3: Tính toán ma trận J

(5)

Tính toán Ma trận Jacoby

[ ]
−S 1+3 C 4 S 1+3 S 4 C1+3 d 2 C 1
C C −C 1+3 S 4 S 1+3 d 2 S 1
T 04= 1+3 4 (6)
S4 C4 0 0
0 0 0 1

13
  Khớp 1 là khớp xoay:

[ ]
C 3 C4 −C 3 S4 S3 0
S4 C4 0 0
T 14= (7)
−S 3 C 4 C4 C3 d 2
0 0 0 1

 Khớp 2 là khớp trượt:

[ ]
C3 S 4 S 3 C 4 −C3 S4 d 2 S3
S4 0 C4 0
T 24= (8)
−S 3 C 4 −C 3 C4 d 2 +d 2 C 3
0 0 0 1

 Khớp 3 là khớp xoay:

14
 [ ]
C 4 −S 4 0 0
3 S C4 0 0
T 4= 4 (9)
0 0 1 0
0 0 0 1

 Khớp 4 là khớp xoay:

15
=

16
Phần 4: Thiết kế quỹ đạo
- Thiết kế quỹ đạo của từng khớp biến thiên theo dạng hàm
bậc 3 : q ( t )=at 3 +b t 2+ ct+ d
- Chọn thời gian quá độ là t c =2 s
- Khi t c < 2s thì q ( t )=at 3 +b t 2+ ct+ d
- Khi t c ≥ 2s thì q(t) =q d ( giá trị đặt của q)
- Tính toán hàm bậc 3:
+ Chọn q(t =0) = d = 0 (lúc đầu cá khớp ở vị trí ban đầu)
+ Vận tốc tại thời điểm ban đầu của khớp bằng 0
 q̇ ( t =0 )=3 a .02 +2 b .0+c=0 => c = 0
+ Khi q(t =2) = 8a + 4b +2c = q d
+ Vận tốc tại thời điểm ban đầu của khớp bằng 0
 q̇ ( t=2 ) =3 a .22+ 2b .2+c=0
- Giải các phương trình ta được hàm q(t) code vào matlab.
Phần 5: Động lực học

1. Động năng
1 T 1 T
K i= Pci mi P ci + ωi I i ωi
2 2
 Thanh nối thứ nhất:
Trọng tâm:
l 1 cos θ 1 −l 1 sin θ1 θ̇1
Pc 1=⌊ l 1 sin θ1 ⌋ → Ṗc 1=⌊ l1 cos θ1 θ̇1 ⌋
0 0
Suy ra:
1 1 1
K 1= m1 ( (−l 1 sin θ1 θ̇ 1) + ( l 1 cos θ 1 θ̇1 ) ) + I zz 1 θ̇1= ( m1 l 1 + I zz 1) θ̇ 1
2 2 2 2 2
2 2 2

 Thanh nối thứ 2:

Trọng tâm:
(2 l 1+l 2 )cos θ 1 l̇ cos θ 1−(2 l ¿ ¿ 1+l 2)sin θ1 θ̇ 1 ¿ l̇ 2 sin θ1 +(2l 1 +l 2)cos θ 1 θ̇1
Pc 2=⌊ ⌋ → Ṗc 2=⌊ 2 ⌋
(2 l ¿ ¿ 1+l 2 )sinθ 1 ¿ 0 0

17
 Với
d2 1 ḋ cos θ1−(2l ¿ ¿ 1+d 2 )sin θ 1 θ̇1 ¿ ḋ 2 sin θ1 +(2l 1 +d 2 )cos θ 1 θ̇1
l 2= −l 1 → Ṗ c1= ⌊ 2 ⌋
2 2 0

Suy ra:
1 2 1
K 2= m2 ḋ 2 + ( m2 ( 2 l1 + d2 ) + I zz 2 ) θ̇1
2 2
8 8

 Thanh nối thứ 3:

Trọng tâm:
d 2 cos θ1+ l 3 cos θ 13 ḋ 2 cos θ1−d 2 sin θ1 θ̇ 1−l 3 sin θ 13 θ̇13
Pc 3=⌊ d 2 sin θ1+ l 3 sin θ13 ⌋ → Ṗc 3=⌊ ḋ 2 sin θ1+ d 2 cos θ 1 θ̇ 1+l 3 cos θ13 θ̇13 ⌋
0 0

Suy ra:
1 2 1 1
K 3= m3 ḋ 2+ ( m3 l 3 + I zz 3 ) θ̇13 + m3 d 2 θ̇1 +m3 l 3 (d 2 cos θ3 θ̇1− ḋ2 sin θ3 ) θ̇13
2 2 2 2
2 2 2

 Thanh nối thứ 4

Trọng tâm:
Pc 4 =⌊ d 2 cos θ1 +(2l ¿ ¿ 3+l 4 )cos θ13 ¿ d 2 sin θ1 +(2l 3 +l ¿ ¿ 4) sin θ13 ¿ 0 ⌋ → Ṗ c 4=⌊ ḋ 2 cos θ1−d 2 sin θ1 θ̇ 1−(l 4+ 2l ¿ ¿

Suy ra:
1 1 1 1
K 4 = m4 ḋ 22+ ( m4 ( l 4 +2 l 3 )2 + I zz 4 ) θ̇13
2
+ m4 d 22 θ̇ 21+ m 4 ( l 4 +2 l3 ) ( d 2 cos θ 3 θ̇1− ḋ 2 sin θ3 ) θ̇13 + I xx 4 θ̇24
2 2 2 2

2. Thế năng

Pi=−mi [ 0 ,−1 , 0 ] g Pci

 Thanh nối thứ nhất:
P1=m1 g l 1 sinθ 1

 Thanh nối thứ 2:

d2
P2=m2 g (2l ¿ ¿ 1+l 2 )sin θ1=m2 g(l ¿ ¿ 1+ )sin θ1 ¿ ¿
2

18
  Thanh nối thứ 3:
P3=m3 g(d ¿ ¿ 2 sin θ1+ l 3 sin θ13 )¿

 Thanh nối thứ 4:

P4 =m4 g ¿¿

3. Tính toán động lực học:

4
L=K −P=∑ K i−Pi
i=1

3.1.Khớp 1 là khớp quay:

M 1=
d ∂L
( ) −
∂L
dt ∂ θ̇1 ∂ θ1


∂L 1
=( m 1 l 1 + I zz 1 ) θ̇1 + ( m 2 ( 2l 1 +d 2 ) + I zz 2 ) θ̇1 + ( m3 l 3+ I zz3 ) θ̇ 13+m3 d 2 θ̇1 +2 m3 l 3 d 2 cos θ3 θ̇ 1+ m3 l 3 (d ¿ ¿ 2 co
2 2 2 2

∂ θ̇ 1 4

Đặt: l4 +2 l3=l34 ,. Suy ra:


∂L
∂ θ̇ 1 ( 2 2 1
4 ) 2 2 1
= m1 l 1 +m 3 l 3+ m 4 l 34 + I zz 1 + I zz 2+ I zz3 + I zz 4 θ̇1 + ( m3 l 3+ I zz3 + m 4 l 34 ) θ̇3 + m 2 ( 2l 1 +d 2 ) θ̇ 1+(m ¿ ¿ 3+m
2
4
2


d ∂L
( )(
dt ∂ θ̇1
2 2 2 1
4 ) 2 2 1
4
1
= m 1 l 1 +m3 l 3 +m 4 l 34 + I zz 1 + I zz 2+ I zz3 + I zz 4 θ̈1 + ( m3 l3 + I zz3 +m4 l 34 ) θ̈3 + m2 ( 2 l 1 +d 2 ) θ̈ 1+ m2 (
2
2

( 1
4
1
)
¿ m1 l 21+ m3 l 23 +m4 l 234+ I zz1 + I zz 2 + I zz 3+ I zz 4 + m 2 ( 2 l 1+ d 2 )2+ m34 d 22 +2 ( m3 l 3 +m 4 l 34 ) d 2 cos θ 3 θ̈1 + ( m3 l 23+ I zz3 +
4

∂L
 =−¿
∂θ 1

((
¿− m1 l 1 +m2 l 1+ ( ) d2
2 )
+ m3 d 2+ m4 d 2 cos θ1 + ( m3 l 3+ m4 l 34 ) cos θ 13 g
)
19
Suy ra:

( 1 2
) ( 1
M 1= f 11 + m2 ( 2 l 1+ d 2 ) + m34 d 22 +2 f ml 34 cos θ3 θ̈ 1+ ( f 12 +f ml34 d 2 cos θ3 ) θ̈3−f ml 34 sin θ3 d̈ 2+ m2 ( 2 l 1+ d 2 ) +2 m34 d
4 2

Với:
2 2 2 1
f 11=m 1 l 1+ m3 l 3 +m4 l 34+ I zz 1+ I zz 2 + I zz 3 + I zz 4
4

f 12=m3 l 23 + I zz 3 +m4 l 234

f ml 34=m3 l 3 +m 4 l 34

3.2.

20
 3.3.Khớp 2 là khớp tịnh tiến:

M 2=
( )
d ∂L
−
∂L
dt ∂ ḋ 2 ∂ d 2

∂L 1
 = m ḋ + m ḋ −m3 l 3 sin θ3 θ̇13 +m4 ḋ 2−m4 l 34 sinθ 3 θ̇ 13
∂ ḋ 2 4 2 2 3 2

¿ ( 14 m +m )ḋ −f
2 34 2 ml 34 sin θ3 θ̇ 13

 ( )(
d ∂L
dt ∂ ḋ 2
1
)
= m2+ m34 d̈ 2−f ml34 ( cos θ3 θ̇3 θ̇ 13 + sinθ 3 θ̈ 13)
4


∂L 1 1
(
= ( 2l +d ) θ̇2+ m d θ̇2 +m l cos θ3 θ̇1 θ̇ 13+ m4 d2 θ̇21 +m 4 l 34 cos θ3 θ̇ 1 θ̇13 − m2 sin θ1 +m3 sin θ1+ m4 si
∂ d2 4 1 2 1 3 2 1 3 3 2

¿ ( 14 ( 2l +d )+m
1 2 34 )
d 2+ f ml 34 cos θ3 θ̇ 21+ f ml 34 cos θ3 θ̇1 θ̇ 3− ( 12 m +m )sin θ g
2 34 1

Suy ra:

M 2=−f ml 34 sin θ 3 θ̈1 + ( 14 m + m ) d̈ −f

2 34 2 ml34 ( 14 ( 2l +d )+m
sin θ3 θ̈3−2 f ml 34 cos θ 3 θ̇3 θ̇1−2 f ml 34 cos θ3 θ̇ 23− 1 2 34 d 2+ f m

3.4.

21
 3.5.Khớp 3 là khớp quay:

M 3=
( )
d ∂L
−
∂L
dt ∂ θ̇3 ∂θ3


∂L
=(m3 l 3+ I zz3 +m4 ( l 4 + 2l 3 ) + I zz 4 ) θ̇ 13+ m3 l 3 ( d 2 cos θ 3 θ̇1 −ḋ 2 sin θ 3) + m 4 l34 ( d2 cos θ3 θ̇1− ḋ 2 sin θ3 )
2 2

∂ θ̇ 3
¿ ( m3 l 3+ I zz 3+ m4 ( l 4 +2l 3 ) + I zz 4 ) θ̇13 +f ml34 ( d 2 cos θ3 θ̇1− ḋ2 sin θ3 )
2 2

2 2
Với: f 33=m 3 l 3 + I zz 3 +m 4 ( l 4 +2 l3 ) + I zz 4

d ∂L
( )
dt ∂ θ̇3
=f 33 θ̈13 + f ml 34 ( d2 cos θ3 θ̈1−d 2 sin θ3 θ̇1 θ̇ 3+ cos θ3 θ̇1 ḋ 2−sin θ3 d̈ 2−cos θ3 θ̇3 ḋ 2)

∂L
 =−m3 l 3 gcos θ13−m 4 l 34 gcos θ 13 =−f ml 34 cos θ 13 g
∂θ 3

Suy ra:
M 3=f 33 θ̈13 + f ml 34 ( d 2 cos θ 3 θ̈ 1−d 2 sin θ 3 θ̇1 θ̇3 +cos θ 3 θ̇1 ḋ 2−sin θ3 d̈ 2−cos θ 3 θ̇ 3 ḋ 2 ) + f ml 34 cos θ13 g

¿ ( f 33+ f ml 34 d 2 cos θ 3 ) θ̈ 1−f ml 34 sin θ3 d̈ 2 +f 33 θ̈3−f ml 34 d 2 sin θ3 θ̇1 θ̇3 +f ml34 cos θ 3 θ̇1 ḋ 2−f ml 34 cos θ3 θ̇ 3 ḋ2 + f ml 34 cos θ13

22
 3.6. Khớp 4 là khớp quay:
M 4=
( )
d ∂L
−
∂L
dt ∂ θ̇ 4 ∂θ 4

∂L
 =I xx 4 θ̇ 4
∂ θ̇ 4


d ∂L
( )
dt ∂ θ̇4
=I xx 4 θ̈ 4

∂L
 =0
∂θ 4
Suy ra:
M 4 =I xx 4 θ̈4

Có hệ phương trình động lực học: M =H ( Q ) Q̈+V ( Q , Q̇ )+ G ( Q ) g

Với:
T
 M =[ M 1 , M 2 , M 3 , M 4 ]
T
 Q= [θ 1 , d 2 ,θ 3 , θ 4 ]
 H ( Q )=[ H ij ] (i, j=1→ 4 )
1 2 2
o H 11=f 11+ m 2 ( 2l 1 +d 2 ) +m34 d2 +2 f ml34 cos θ 3
4
o H 12 =−f ml 34 sin θ 3
o H 13 =f 12+ f ml 34 d 2 cos θ 3
o H 14=0
o H 21 =−f ml 34 sin θ 3
1
o H 22= m 2+ m34
4
o H 23=−f ml 34 sinθ 3
o H 24=0
o H 31=f 33+ f ml 34 d 2 cos θ 3
o H 32=−f ml 34 sin θ 3
o H 33=f 33
o H 34=0
o H 41=H 42= H 43=0
o H 44 =I xx 4
T
 V = [ v 1 , v 2 , v3 , v 4 ]

23
 o
v1 = ( 12 m ( 2l +d )+2 m d ) ḋ θ̇ + f
2 1 2 34 2 2 1 ml 34 (−2 d2 sin θ3 θ̇1 θ̇ 3+ 2cos θ 3 θ̇1 ḋ 2−d2 sin θ3 θ̇23 )
1
(
o v 2=−2 f ml 34 cos θ3 θ̇3 θ̇1−2 f ml 34 cos θ3 θ̇23− ( 2 l1+ d 2 )+ m34 d 2 + f ml 34 cos θ 3 θ̇21
4 )
o v3 =−f ml 34 d 2 sin θ3 θ̇ 1 θ̇3 + f ml 34 cos θ3 θ̇1 ḋ2 −f ml 34 cos θ3 θ̇3 ḋ 2
o v 4=0

[ ( )
]
( )
m1 l 1 +m2 l 1+
d2
2
+ m3 d 2 +m4 d 2 cos θ1 + f ml 34 cos θ13

 G(Q)=
( 12 m + m ) sinθ
2 34 1

f ml 34 cos θ13
0

Trong đó:
 l 34 =l 4 + 2l 3
 m34=m3 +m4
1
 f 11=m 1 l 21+ m3 l 23 +m4 l 234+ I zz 1+ I zz 2 + I zz 3 + I zz 4
4
2 2
 f 12 =m3 l 3 + I zz 3 +m 4 l 34
 f 33=m3 l 23 + I zz 3 +m4 l 234+ I zz 4
 f ml 34=m3 l 3 +m 4 l 34

24
 CHƯƠNG 4: MÔ PHỎNG TRÊN SIMULINK
1. Động học thuận:

- Code:

25
2. Động học nghịch:

3. Thiết kế quỹ đạo:

26
4. Khối mô hình toán học Robot:

27
5. Bộ điều khiển

d q(t)
q(t)
M

Q d (t) Bộ điều robot


khiển

Hình 4 : Sơ đồ điều khiển robot

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

M =H ( Q ) Q̈+V ( Q , Q̇ )+ G ( Q ) g

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

M =Kp . (Qd−Q˙) −Kd . Q̇+G (Q)

6. Mô phỏng
a) Khảo sát quỹ đạo, giá trị đầu ra sau bộ mô hình toán học robot và
giá trị sau khâu đông học thuận khi đặt các giá trị đặt trực tiếp không
thông qua bộ biến đổi nghịch
Áp dụng thuật toán điều khiển PD bù trọng trường
Thông số:
Khối lượng các thanh nối:
m 1=5 kg ; m2=4 kg ; m3=3 kg ; m4=1 kg

Chiều dài các thanh nối:

l 1=0.6 m ; l 2 max ¿ 0.5 m ;l 3 =0.4 m; l 1=0.1m

Giá trị đặt:

28
 θ1=0.55 rad ;d 2=0.45 m; θ3=0.3 rad ; θ 4=0.4 rad

Ma trận điều khiển:

50 0 0 0 40 0 0 0
K p =⌊ 0 20 0 0 ⌋ ;K v =⌊ 0 15 0 0 ⌋
0 0 20 0 0 0 10 0
0 0 0 3 0 0 0 0.1

Kết quả mô phỏng:

- Giá trị đặt sau bộ thiết kế quỹ đạo:

29
- Giá trị tính toán sau bộ mô hình toán học robot:

- Giá trị tọa độ vị trí của cánh tay sau khâu động học thuận:
+) Giá trị X:

30
 +) Giá trị Y:

b) Khảo sát giá trị tọa độ đặt trước bộ động học nghịch và độ bám của tọa độ
sau bộ động học thuận

31
- Giá trị X sau bộ động học thuận:

- Giá trị Y sau bộ động học thuận:

32
7. Nhận xét sau mô phỏng:
- Giá trị góc quay và giá trị tịnh tiến sau mô hình toán học robot bám
sát theo giá trị đặt
- Thời gian đáp ứng bị trễ đi 1-3s.
- Ở khớp 2 giá trị tịnh tiến có độ quá điều chỉnh nhỏ khoảng 4.4%.
- Tọa độ theo trục X có độ quá điều chỉnh khoảng 5,26%, độ quá điều
chỉnh theo trục Y nhỏ.

33
 CHƯƠNG 5: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN
1. Kết luận
Đồ án của chúng em đã đạt được yêu cầu bài toán đề ra:
- Tính toán và mô phỏng lại hoạt động của cánh tay Robot trên Matlab
- Kết quả mô phỏng ổn định
Bên cạnh đó, do thời gian nghiên cứu và đầu tư có hạn nên chúng em còn một
số vấn đề như:
- Chưa thể tìm hiểu, so sánh kết quả mô phỏng trên Matlab với kết quả mô
phỏng bằng Multibody Simscape
- Mô hình dùng để mô phỏng chưa chuẩn xác so với mô hình mẫu
- Khi xảy ra sai lệch, cánh tay khó có thể trở lại vị trí ban đầu.

2. Hướng phát triển

Sau khi đã tính toán lý thuyết và có kết quả mô phỏng bằng Matlab, chúng em
sẽ tiến hành tìm hiểu thêm về Mutibody Simscape để so sánh 2 kết quả mô
phỏng. Từ 2 kết quả mô phỏng đó, chúng em sẽ xây dựng lên một mô hình tay
máy nhỏ và thiết kế bộ điều khiển thực tế.

34
 TÀI LIỆU THAM KHẢO

- Sách ROBOT công nghiệp – Trịnh Quang Vinh, Nguyễn Đăng Bình,
Phạm Thành Long

- Sách MATLAB & SIMULINK cho kĩ sư – Nguyễn Quang Hoàng

35