Nhom4 ThietKeRobot MaLop132841 LapRapSanPhamMach

ĐẠI HỌC BÁCH KHOA HÀ NỘI
TRƯỜNG CƠ KHÍ
Khoa Cơ điện tử
***************
BÁO CÁO
THIẾT KẾ ROBOT
Đề xuất dự án và thực hiện tính toán thiết kế
mô hình robot ứng dụng trong lắp ráp sản phẩm
Giảng viên hướng dẫn: PGS.TS. Phan Bùi Khôi
Họ và tên Mã số sinh viên Lớp

Trần Minh Hiếu 20184447 CĐT.02.K63
Phương Đức Trung 20184640 CĐT.02.K63
1
LỜI NÓI ĐẦU
Robot là một ng ành kỹ thuật bao gồm thiết kế, chế tạo, vận hành, và ứng dụng
robot, cũng như các hệ thống máy tính để điều khiển, phản hồi tín hiệu cảm biến, và xử lý
thông tin của chúng. Những công nghệ này liên hệ với các máy móc tự động dùng để thay
thế con người trong những môi trường độc hại hoặc trong các quá trình sản xuất.
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.
Đặc biệt trong công cuộc công nghiệp hóa hiện đại hóa, có thể nói rằng Robot mang
tới cho cuộc sống con người một cuộc sống mới, một cách trải nghiệm cuộc sống và đôi
khi còn là người bạn. Những hãng Robot từ các nước nổi tiếng trên thế giới từ Đức, Nhật
bản, Nga, Mỹ ngày một khẳng định sự hiện diện của Robot là phần không thiếu trong cuộc
sống hiện nay và tương lai của phía trước. Nó xuất hiện ở tất cả các lĩnh vực từ khoa học
vĩ mô cho tới vi mô và ngày một đa dạng.
Trong khuôn khổ môn học Thiết kế Robot với đề tài thiết kế Robot ứng dụng trong
lắp ráp sản phẩm, nhóm tin tưởng rằng với những kết quả có được từ việc tìm hiểu và tính
toán trong bài tiểu luận này sẽ là bước đệm quan trọng cho việc phát triển nhiều hơn nữa
những ý tưởng trong tương lai về tính toán và thiết kế các loại Robot công nghiệp.
Nhóm xin gửi lời cảm ơn tới PGS.TS. Phan Bùi Khôi vì đã luôn tận tình giúp đỡ và
hướng dẫn chúng em trong quá trình trao đổi ở các buổi học. Những góp ý, sửa chữa của
thầy sẽ phần nào giúp nhóm tự tin hơn trong cách thức tiếp cận với nền công nghiệp hiện
nay bởi mặc dù đã có những sự chuẩn bị của nhóm hoặc cũng có thể kiến thức nhóm mang
đến trong bài tiểu luận này con sai sót và chưa đúng. Nhóm rất mong có được sự bổ sung,
sửa chữa đó, chúng em chân thành cảm ơn và chúc Thầy sức khoẻ!
Chúng em xin chân thành cảm ơn.
Hà Nội, tháng 8 năm 2022
Nhóm sinh viên thực hiện
Nhóm 4
1
DANH SÁCH THÀNH VIÊN NHÓM 4 VÀ PHÂN CÔNG NHIỆM VỤ
Nhiệm vụ chính được

TT MSSV Họ và tên Xếp loại Chữ ký
phân công
Phân tích lựa chọn cấu trúc
robot.
Thiết kế 3D mô hình robot,
kiểm bền, tính toán tĩnh học.
1 20184447 Trần Minh Hiếu Tốt
Thiết kế quỹ đạo robot.
Thiết kế hệ thống điều khiển
và mô phỏng.
Giải các bài toán động học,

động lực học.
Thiết kế 3D mô hình robot,
tính toán tĩnh học.
2 20184640 Phương Đức Trung Thiết kế hệ thống dẫn động Tốt
robot.
Thiết kế hệ thống điều khiển
và mô phỏng.
2
MỤC LỤC
LỜI NÓI ĐẦU ..................................................................................................................... 1
1. PHÂN TÍCH VÀ LỰA CHỌN CẤU TRÚC .................................................................. 5
1.1. Phân tích mục đích ứng dụng robot .......................................................................... 5
1.2. Phân tích yêu cầu kỹ thuật thao tác .......................................................................... 6
1.3. Xác định các đặc trưng kỹ thuật ............................................................................. 10
1.4. Các phương án thiết kế cấu trúc robot, cấu trúc các khâu khớp, phân tích, chọn
phương án thực hiện. ..................................................................................................... 11
1.5. Thông số kỹ thuật: robot thiết kế, đối tượng và hệ thống thao tác. ........................ 13
2. THIẾT KẾ 3D MÔ HÌNH ROBOT .............................................................................. 13
2.1. Thiết kế 3D ............................................................................................................. 13
2.2. Lập bản vẽ 2D......................................................................................................... 14
2.3. Lập hồ sơ kỹ thuật .................................................................................................. 15
2.4. Xác định các thông số đặc trưng hình học - khối lượng......................................... 15
3. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG .................................................................... 29
3.1. Khảo sát động học thuận, khảo sát động học ngược. ............................................. 29
3.2. Thiết kế quỹ đạo chuyển động của robot theo mục đích ứng dụng........................ 40
4. PHÂN TÍCH TRẠNG THÁI (TĨNH) YÊU CẦU LỰC/MOMEN ĐỘNG CƠ LỚN
NHẤT ................................................................................................................................ 76
4.1. Cơ sở lí thuyết......................................................................................................... 76
4.2. Giải bài toán cụ thể ................................................................................................. 77
4.3. Tính toán lực và momen lớn nhất ở trạng thái tĩnh ................................................ 82
5. TÍNH TOÁN ĐỘNG LỰC HỌC .................................................................................. 84
5.1 Thiết lập phương trình vi phân chuyển động của robot bằng các phương pháp đã học.
....................................................................................................................................... 84
5.2 Tính động lực học ngược theo quy luật chuyển động được khảo sát ở câu 3.c. ..... 94
6. THIẾT KẾ HỆ DẪN ĐỘNG ROBOT ........................................................................ 100
6.1 Lựa chọn động cơ cho khâu 1, khâu 2 và khâu 4 .................................................. 104
6.2 Thiết kế hệ dẫn động cho khâu 3 ........................................................................... 106
6.4 Thiết kế 3D và kiểm nghiệm bền các khâu của robot ........................................... 114
3
7. THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN LỰC............................................................. 115
7.1 Chọn luật điều khiển phù hợp, thiết kế mô hình điều khiển.................................. 115
7.2 Mô phỏng bằng Matlab. ........................................................................................ 118
7.3 Mô phỏng bằng Simscape Multibody.................................................................... 131
8.THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN BIẾN KHỚP VỚI ĐỘNG CƠ DC SERVO. 135
8.1 Mô hình hóa cho một trục chuyển động, xác định hàm truyền ............................. 135
8.2 Xây dựng mô hình hệ thống điều khiển cho toàn bộ rô bốt .................................. 145
8.3. Sử dụng Matlab – Simulink để mô phỏng động lực học cho một trục chuyển động,
và cho toàn bộ rô bốt ................................................................................................... 148
8.4. Bộ điều điều khiển PID ........................................................................................ 151
8.5. Chọn các hệ số cho hàm điều khiển ..................................................................... 153
8.6 Kết quả ................................................................................................................... 156
KẾT LUẬN ..................................................................................................................... 160
TÀI LIỆU THAM KHẢO ............................................................................................... 161
PHỤ LỤC HÌNH ẢNH.................................................................................................... 162
4
1. PHÂN TÍCH VÀ LỰA CHỌN CẤU TRÚC
1.1. Phân tích mục đích ứng dụng robot
Mục tiêu ứng dụng robot công nghiệp nhằm góp phần nâng cao năng suất dây
chuyền công nghệ, giảm giá thành, nâng cao chất lượng và khả năng cạnh tranh của sản
phẩm, đồng thời cải thiện điều kiện lao động. Đạt được các mục tiêu trên là nhờ vào khả
năng của robot như: làm việc không biết mệt mỏi, làm việc được ở các môi trường mà con
người không thể làm được… Hiện nay, ở các công xưởng lớn trên thế giới, việc sản xuất
tự động đã được ứng dụng rất rộng rãi, tuy nhiên một số khâu trong sản xuất vẫn chưa được
tự động hóa mà vẫn cần đến sức lao động của con người.
Hình 1. Sử dụng robot trong sản xuất công nghiệp

Trong quy trình sản xuất lắp ráp điện thoại, việc lắp mạch là một công việc tuy
không nặng nhưng cần độ chính xác cao. Vì vậy, để nâng cao tốc độ sản xuất lắp ráp, giảm
thiểu sai sót trong các khâu tiếp theo, nhóm xin đề xuất bố trí thêm robot lắp ráp mạch vào
điện thoại theo đúng yêu cầu cần độ chính xác cao.
Hình 2. Sản xuất mạch điện thoại
5
1.2. Phân tích yêu cầu kỹ thuật thao tác
a. Đối tượng thao tác, dạng thao tác.
Mạch điện thoại có đặc điểm là mỏng, kích thước không quá lớn, cần độ chính xác
cao. Đường dẫn trên mạch rất nhỏ tương đương kích thước sợi tóc, các lỗ khoan cũng rất
nhỏ. Bề mặt tương tối phẳng do là mạch sử dụng trong điện thoại smartphone có độ mỏng
lớn (5~7mm) nên thường sử dụng các linh kiện nhỏ và phân bố sao cho bề mặt phẳng chiểm
ít không gian nhất có thể. Mật độ các linh kiện khá dày nên khi thao tác cần nhẹ nhàng
tránh va chạm. Không được dùng lực lớn tác động lên các linh kiện này.
Thông số kĩ thuật của mạch:
- Giả định mạch có dạng tấm mỏng với kích thước 160x80x3 mm.
- Khối lượng tối đa 50 gram.
- Mạch được đặt nằm trên băng chuyền.
Hình 3. Đối tượng thao tác: Mạch điện thoại

Do mạch có khối lượng nhỏ, dạng tấm, nên ta sử dụng giác hút gắn với khâu thao
tác của robot để nhấc và lắp ráp về đúng vị trí yêu cầu. Cơ cấu hút chân không sẽ tránh
được các nguy cơ gây hỏng hóc cho mạch. Nếu sử dụng khâu gắp là khâu kẹp thì có thể
khiến linh kiện bị biến dạng, hỏng hóc, ngoài ra do mạch nằm trên mặt băng chuyền nên
độ cao của mạch chỉ là 3 mm, khó có cơ cấu nào kẹp gắp chính xác được đối tượng này.
6
Hình 4. Sử dụng giác hút cho khâu thao tác
Hình 5 Đối tượng thao tác: Vỏ điện thoại

Thông số kĩ thuật của vỏ điện thoại:
- Giả định vỏ điện thoại với kích thước 165x85x8 mm.
- Khối lượng tối đa 500 gram.
- Vỏ được đặt nằm ngay ngắn trên băng chuyền.
b. Phân tích yêu cầu về vị trí.

Mỗi đối tượng thao tác trên băng chuyền số 1 sẽ có vị trí và các hướng ngẫu nhiên
cần phải được gắp và lắp ráp vào vỏ điện thoại theo hướng quy định vào khay đặt. Khoảng
cách giữa các bo mạch khảng 150 mm. Sau khi gắp được các mạch hoàn thiện, các mạch
này sẽ được gắp ra khỏi băng chuyền và lắp ráp vào vỏ điện thoại trên băng chuyền số 2
để vận chuyển tiếp đến công đoạn tiếp theo.
7
Hình 5. Sơ đồ bố trí
Hình 6. Các kích thước khác

Các bo mạch trên băng chuyền 1 được đặt ngẫu nhiên, có thể song song hoặc không
song song với băng chuyền. Robot sẽ gặp mạch từ băng chuyền 1 sang băng chuyền 2, hệ
thống camera xử lý ảnh sẽ xác định góc xoay của mạch để robot có thể xác định góc xoay
để lắp ráp mạch vào vỏ điện thoại đã được sắp xếp song song cách đều nhau.
8
Đối với yêu cầu của bài toán lắp ráp sản phẩm chúng ta đang thực hiện nhiệm vụ là
gắp vật ở vị trí có hướng bất kì từ băng tải 1 sang băng tải 2 và lắp ráp theo đúng vị trí và
hướng của vỏ điện thoại.
Các bo mạch được sắp xếp nằm trên mặt băng chuyền nên việc tác động bằng cần
gạt, tay kẹp thông thường là rất khó do kích thước của chiều cao mạch chỉ là 3mm và nếu
tác động lực quá lớn thì sẽ khiến mạch biến dạng, hỏng hóc. Nên phương án tối ưu để thao
tác di chuyển các bo mạch là sử dụng tay kẹp hút khí nén, vì trọng lượng của mạch nhẹ, ít
tác động lực tới bo mạch nên tránh được hỏng hóc, tốc độ làm việc cũng rất nhanh, mạch
có nhiều bề mặt phẳng để giác hút dễ dàng thực hiện việc nâng và giữ vật trên không trong
quá trình di chuyển.
c. Yêu cầu về hướng của khâu thao tác.

Đặc điểm của đối tượng thao tác là các bo mạch điện là nằm trên mặt băng chuyền,
các mạch này có mặt cùng nằm ngửa lên trên và không thể đặt theo bất kì một phương nào
khác. Bề mặt phẳng để sử dụng cơ cấu hút chân không để kẹp giữ và di chuyển vật sang
băng chuyền khác.
Do đó khâu thao tác sẽ có hướng vuông góc với mặt bo mạch hay chính là mặt băng
chuyền, đồng thời phải có bậc tự do quay để xoay lại vật thể trong trường hợp vật thể không
được sắp xếp đúng hướng song song với băng chuyền.
d. Yêu cầu về không gian thao tác.

Từ sơ đồ bố trí robot ta xác định được khoảng không gian làm việc để thực hiên yêu
cầu của bài toán là dạng hình phần tư của trụ.
Hình 7. Không gian thao tác của Robot Scara
9
Do đó khi lựa chọn robot thì phải xác định không gian làm việc của robot phải chứa
được không gian yêu cầu thao tác. Có thể chọn không gian hình trụ, hoặc hình hộp là tối
ưu nhất. Không gian hình cầu không phù hợp với bài toán này do khó có thể bao hết không
gian làm việc, trong trường hợp bao hết cũng rất khó thao tác và khiến kích thước robot trở
lên rất cồng kềnh.
Hình 8. Không gian thao tác của các loại robot khác
e. Yêu cầu về vận tốc, gia tốc khi thao tác.
Đối tượng làm việc có kích thước nhỏ, mỏng, khối lượng rất nhẹ cùng với cơ cấu
chọn để gắp vật là cơ cấu hút chân không nên vận tốc thao tác có thể đảy lên cao được. Gia
tốc và vận tốc của quá trình làm việc sẽ khá cao giúp tăng năng suất lắp ráp, nhất là đối
tượng nhỏ nhẹ và dễ sắp xếp như mạch điện. Mạch điện khi sắp xếp không cần quan tâm
đến các hiện tượng như đổ, lăn hay rơi chỉ cần thả đúng vị trí và xoay đúng hướng.
Thời gian thao tác có thể dưới 1 giây cho 1 quá trình gắp mạch và lắp ráp 1 vật thể.
1.3. Xác định các đặc trưng kỹ thuật

a. Số bậc tự do cần thiết.
Trong không gian để di chuyển giữa hai vị trí bất kì cần ít nhất 3 bậc tự do. Trong
bài toán lắp ráp mạch thì cần phải xoay lại hướng của mạch nên cần thêm 1 bậc tự do xoay
nữa. Nên yêu cầu cảu robot là cần 4 bậc tự do.
10
b. Vùng làm việc có thể với tới của robot.
Tầm với của robot phải lớn hơn khoảng cách tới băng chuyền xa nhất là 500 mm,
không gian làm việc là hình phần tư của trụ.
c. Yêu cầu về tải trọng.

Đối tượng làm việc là mạch điện tử có kích thước nhỏ và khối lượng không đáng
kể.
1.4. Các phương án thiết kế cấu trúc robot, cấu trúc các khâu khớp, phân tích, chọn phương
án thực hiện.
Như đã phân tích ở trên, với Robot thực hiện lắp ráp mạch cần tối thiểu 4 bậc tự do.
Các phương án thiết kế đáp ứng được các yêu cầu về hướng và vị trí của đối tượng:
Kết cấu 4 bậc tự do:
+ Robot song song:
Hình 9. Mô hình Robot song song

- Ưu điểm: Kết cấu nhẹ, cứng vững cao, các chuyển động hầu như không tạo ra
lực quán tính, tốc độ làm việc cực cao.
- Nhược điểm: độ linh hoạt thấp.
+ Robot dạng chuỗi:
- Ưu điểm: Độ linh hoạt cao
- Nhược điểm: độ cứng vững chưa cao, chuyển động tạo quán tính lớn.
11
Hình 10. Mô hình Robot dạng chuỗi
Trong đề tài này, chúng em lựa chọn mô hình dạng chuỗi thứ 3 cho robot do trọng
lượng đối tượng thao tác rất nhỏ, và mô hình này cũng thỏa mãn các yêu cầu về không gian
thao tác đã đề ra ở bên trên.
12
1.5. Thông số kỹ thuật: robot thiết kế, đối tượng và hệ thống thao tác.
THÔNG SỐ KỸ THUẬT
Số trục 4
Tầm với (mm) 700
Chiều dài khâu 1 (mm) 350
Chiều dài khâu 2 (mm) 350
Hành trình trục Z (mm) 400
Đối tượng: mạch kích thước 160x80x3 mm, nặng 50g
2. THIẾT KẾ 3D MÔ HÌNH ROBOT

2.1. Thiết kế 3D
Hình 11. Mô hình 3D của đề tài
13
2.2. Lập bản vẽ 2D
Hình 12. Hình chiếu đứng của Robot
Hình 13. Hình chiếu bằng của Robot
14
2.3. Lập hồ sơ kỹ thuật
2.4. Xác định các thông số đặc trưng hình học - khối lượng
Thông số đặc trưng hình học:
Hình 14. Các kích thước hình học của mô hình
d1 = 600(mm)
d = 100(mm)
 2
d 4 = 50( mm)
a = 350(mm)
 1
a2 = 350(mm)
Vật liệu, khối lượng, momen quán tính các khâu
2.4.1 Khâu 0
• Vật liệu
Khâu 0 được chọn vật liệu phù hợp để đủ đối trọng với các khâu còn lại vì khâu 0
đóng vai trò là chân đế không tham gia vào các thông số động học. Vật liệu được chọn cho
khâu không là thép cacbon 1023 có đặc tính cơ học như sau:
15
Hình 15. Đặc tính cơ học của thép cacbon 1023
• Khối lượng, momen quán tính khâu 0
Hình 16. Khâu 0
16
Hình 17. Khối lượng, momen quán tính khâu 0
2.4.2. Khâu 1 và khâu 2 và khâu 4
• Vật liệu
Vật liệu lần này được chọn cho khâu 1,khâu 2 và khâu 4 là hợp kim nhôm AA5454
là hợp kim trong họ nhôm-magie.
Hình 18. Thành phần hóa học của hợp kim nhôm
17
Hình 19. Đặc tính cơ học được xuất từ Solidwork
Hình 20. Khâu 1
18
Hình 21. Khối lượng, momen quán tính của khâu 1
Từ các thông số xuất ra từ solidwork ta thu được:
Khối lượng:
m1 = 7.23(kg )
Vị trị khối tâm:
Lc1 = 0.14(m)
Momen quán tính khối (với các trục tọa độ là trục quán tính chính trung tâm):
I1zz = 0.2(kg.m 2 )
I1 yy = 0.18(kg.m 2 )
I1xx = 0.03(kg .m 2 )
19
Hình 22. Khâu 2
20
Khối lượng:
m2 = 7.16(kg )
Lc2 = 0.15(m)
I 2 zz = 0.19(kg .m 2 )
I 2 yy = 0.18(kg .m 2 )
I 2 xx = 0.02(kg .m 2 )
Hình 24. Khâu 4
21
Khối lượng:
m2 = 0.04(kg )
Lc4 = 0.01(m)
Momen quán tính khối (với các trục tọa độ là trục quán tính chính trung tâm)
rất nhỏ và có thể được bỏ qua.
22
2.4.3. Khâu 3
• Vật liệu
Vật liệu được chọn cho khâu 3 là một loại hợp kim cứng của thép có các đặc tính
cơ học như sau:
Hình 26. Đặc tính cơ học của thép hợp kim đã chọn
23
Hình 27. Khâu 3
Hình 28. Khối lượng, momen quán tính khâu 3
24
Khối lượng:
m3 = 2.11(kg )
I 3 zz = 0.00163(kg.m 2 )
I 3 yy = 0.056(kg.m2 )
I 3 xx = 0.056(kg.m 2 )
2.4.4. Tính momen quán tính cho các khớp trên hệ thống Robot
• Khớp 1
Hình 29. Mô hình tính toán momen quán tính cho khớp 1
25
Hình 30. Kết quả tính momen quán tính cho khớp 1
• Khớp 2
Hình 31. Mô hình tính toán momen quán tính cho khớp 2
26
Hình 32. Kết quả tính momen quán tính cho khớp 2
• Khớp 3 và khớp 4
Kết quả tính momen cho khớp 3 và 4 đạt kết quả nhỏ có thể được bỏ qua như thể
hiện trong hình dưới đây:
Hình 33. Mô hình tính toán momen quán tính cho khớp 3 và khớp 4
27
Hình 34. Kết quả tính momen quán tính cho khớp 3 và 4
28
3. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG
3.1. Khảo sát động học thuận, khảo sát động học ngược.
• Tính toán động học thuận:
Hình 35. Sơ đồ động học của Robot
Khâu  d  a
1 q1 d1 0 a1
2 q2 d2 0 a2
3 0 q3 0 0
4 q4 d4 0 0
29
d1 = 600(mm)
d = 100(mm)
 2
Tham số động học: d 4 = 50( mm)
a = 350(mm)
 1
a2 = 350(mm)
q =  q1 , q2 , q3 , q4 
T
Biến khớp:
q1 = ( −165  165 ) ; q2 = ( −147  147 ) ; q3 = ( 0  400mm ) ; q4 = ( 0  360 )

• Các ma trận biến đổi tọa độ thuần nhất
Theo cách thiết lập hệ tọa độ và bảng thông số động học D – H như trên ta có các
ma trận D – H sau đây:
cos(q1 ) − sin(q1 ) 0 a1 cos(q1 )  cos(q 2 ) − sin(q 2 ) 0 a2 cos(q 2 ) 

 sin(q ) cos(q )   sin(q ) cos(q )
0 a1 sin(q1 )  1 0 a2 sin(q 2 ) 
0
A1 =  1 1
; A2 =  2 2
 0 0 1 d1   0 0 1 d2 
   
 0 0 0 1   0 0 0 1 
1 0 0 0 cos(q 4 ) − sin(q 4 ) 0 0
0 1 0 0   sin(q ) cos(q ) 0 0 
2
A3 =  ; 3
A4 =  4 4
;
0 0 1 q3   0 0 1 d4 
   
0 0 0 1  0 0 0 1
cos ( q1 + q2 ) − sin ( q1 + q2 ) 0 a1 cos ( q1 ) + a2 cos ( q1 + q2 ) 

 
sin ( q1 + q2 ) cos ( q1 + q2 ) 0 a1 sin ( q1 ) + a2 sin ( q1 + q2 )  ;
0
A2 = 0 A1 .1 A2 = 
 0 0 1 d1 + d 2 
 
 0 0 0 1 
cos ( q1 + q2 ) − sin ( q1 + q2 ) 0 a1 cos ( q1 ) + a2 cos ( q1 + q2 ) 

 
sin ( q1 + q2 ) cos ( q1 + q2 ) 0 a1 sin ( q1 ) + a2 sin ( q1 + q2 )  ;
0
A3 = 0 A2 2 A3 = 
 0 0 1 d1 + d 2 + q3 
 
 0 0 0 1 
30
cos ( q1 + q2 + q4 ) − sin ( q1 + q2 + q4 ) 0 a1 cos ( q1 ) + a2 cos ( q1 + q2 ) 
 
sin ( q1 + q2 + q4 ) cos ( q1 + q2 + q4 ) 0 a1 sin ( q1 ) + a2 sin ( q1 + q2 ) 
0
A4 = A3 . A4 = 
0 3
 0 0 1 d1 + d 2 + q3 + d 4 
 
 0 0 0 1 
Đặt: s1 = sin(q1), c1 = cos(q1)
s12 = sin(q1+q2), c12 = cos(q1+q2)
s124 = sin(q1+q2 + q4), c124 = cos(q1+q2 +q4)
Khi đó ta được:
c1 −s1 0 a1c1  c12 − s12 0 a1c1 + a2 c12 

  
s c1 0 a1s1  s c12 0 a1 s1 + a2 s12 
0
A1 =  1 ; o
A2 =  12 ;
0 0 1 0  0 0 1 d1 + d 2 
   
 0 0 0 1 
 0 0 0 1 
c12 − s12 0 a1c1 + a2 c12  c124 − s124 0 a1c1 + a2 c12 


s c12 0 a1 s1 + a2 s12  
s c124 0 a1 s1 + a2 s12 
0
A3 =  12 ; 0
A4 =  124
0 0 1 d1 + d 2 + q3   0 0 1 d1 + d 2 + q3 + d 4 
   
0 0 0 1   0 0 0 1 
Phương trình động học thuận của robot:
𝑥 = 350cos(𝑞1 + q2 ) + 350cos(𝑞1 )
{ 𝑦 = 350sin(𝑞1 + q2 ) + 350sin(𝑞1 ) (𝑚𝑚)
𝑧 = 750 − 𝑞3
3.1.1. Thiết lập phương trình động học cho robot

a. Thiết lập ma trận trạng thái khâu thao tác theo tọa độ thao tác:
Lấy gốc tọa độ thiết lập trên khâu thao tác 04 trùng với E. Sử dụng góc Cardan xác
định hướng vật rắn, gọi p là vectơ tọa độ định vị khâu thao tác.
p = [xE, yE, zE, α, β, η]T
Trong đó:
31
+ 3 thành phần đầu biểu diễn vị trí điểm tác động cuối thuộc khâu thao tác:
o
rE = [xE , y E , z E ]T
+3 thành phần cuối biểu diễn hướng của khâu thao tác:
 c11 ( ,  , ) c12 ( ,  , ) c13 ( ,  , ) 
o
RE ( ,  , ) == c21 ( ,  , ) c22 ( ,  , ) c23 ( ,  , ) 
 c31 ( ,  , ) c32 ( ,  , ) c33 ( ,  , ) 
 cos  cos − cos  sin  sin  


=  sin  sin  cos + cos  sin  − sin  sin  sin + cos  cos − sin  cos  
 − cos  sin  cos + sin  sin  cos  sin  sin  + sin  cos cos  cos  
Khi đó ma trận biến đổi tọa độ thuần nhất của khâu cuối biểu diễn bởi tọa độ khâu
thao tác là:
 c11 ( ,  , ) c12 ( ,  , ) c13 ( ,  , ) x E 


 RE ( ,  , )
o o
rE ( xE , yE , zE )   c21 ( ,  , ) c22 ( ,  , ) c23 ( ,  , ) y E 
o
AE ( p) =   ==
 0T 1   c31 ( ,  , ) c32 ( ,  , ) c33 ( ,  , ) z E 
 
 0 0 0 1
b. Thiết lập ma trận trạng thái khâu thao tác theo cấu trúc động học
Ma trận biến đổi tọa độ thuần nhất oA4 biểu diễn trang thái khâu thao tác có thể xác
định được từ cấu trúc động học của robot.
Ta có:
c124 − s124 0 a1c1 + a2 c12 

s c124 0 a1 s1 + a2 s12 
0
A4 ( q ) =  124 .
 0 0 1 d1 + d 2 + q3 + d 4 
 
 0 0 0 1 
Ma trận được đưa về dạng như sau:
 o Rn ( q ) o
rn ( q ) 
o
An (q) =  T 
 0 1 
T
Trong đó q =  q1 , q2 , q3 , q4 
32
 c11 (q) c12 (q) c13 (q) 
o
Rn = c21 (q) c22 (q) c23 (q) 
 c31 (q) c32 (q) c33 (q) 
rn =  x(q), y (q), z (q) 

o T
Phương trình động học robot nhận được trong dạng ma trận như sau:
0
A4(q) = 0A4(p)
 o Rn ( q ) o
rn ( q )   o RE ( ,  , ) o
rE ( xE , yE , z E ) 
 T = 
 0 1   0T 1 
Từ đó phương trình động học trong dạng ma trận có dạng khai triển như sau:
 c11 (q ) c12 (q ) c13 (q ) x(q )   c11 ( ,  , ) c12 ( ,  , ) c13 ( ,  , ) x E 

c (q) c (q) c (q) y (q )  c21 ( ,  , ) c22 ( ,  , ) c23 ( ,  , ) y E 
 21 22 23
=
 c31 (q ) c32 (q ) c33 (q ) z (q )  c31 ( ,  , ) c32 ( ,  , ) c33 ( ,  , ) z E 
   
 0 0 0 1   0 0 0 1
3.1.2. Giải bài toán động học thuận

Đối với bài toán động học thuận, vị trí, vận tốc, gia tốc các biến khớp xem như đã
biết, cần tìm vị trí, vận tốc, gia tốc của khâu thao tác đối với hệ tọa độ cố định.
Vị trí của khâu thao tác đối với hệ tọa độ cố định O0 x0 y0 z0 được xác định bởi các
tọa độ thao tác gồm các tọa độ định vị điểm tác động cuối và hướng của khâu thao tác.
- Xác định các tọa độ định vị điểm tác động cuối

Các tọa độ định vị điểm tác động cuối được xác định bằng cách so sánh các phần tử
ở hai vế của hệ phương trình động học dạng ma trận, ta được:
 xE = x(q)  xE = a1c1 + a2 c12

 
 yE = y (q ) =>  yE = a1 s1 + a2 s12
 z = z (q) z = d + d + q + d
 E  E 1 2 3 4
33
- Xác định hướng của khâu thao tác
Sử dụng ma trận xác định hướng của khâu thao tác là ma trận Cardan, so sánh hai
vế của phương trình động học, ta có:
 cos  cos − cos  sin  sin   c124 − s124 0 

 sin  sin  cos + cos  sin  − sin  sin  sin  + cos  cos − sin  cos   =  s 
   124 c124 0 
 − cos  sin  cos + sin  sin  cos  sin  sin  + sin  cos cos  cos    0 0 1 
Hướng của khâu thao tác có thể xác định bằng cách đối chiếu 3 phần tử không cùng
hàng (cột) giữa hai ma trận quay:
 c11 ( ,  , ) = c11 (q)

=  c13 ( ,  , ) = c13 (q)
c ( ,  , ) = c (q)
 23 23
cos  sin  = cos(q1 + q 2 + q 4 )


= sin  = 0
− sin  cos  = 0


sin  = 0
  = 0

= sin  = 0 =>   = 0 (I)
  = q + q + q
sin  = − s124 = s  1 2 4
 cos 
124
Hệ (I) chính là hướng của khâu thao tác cuối biểu diến theo 3 góc Cardan.
- Vận tốc điểm cuối, vận tốc góc các khâu
+ Vận tốc điểm tác động cuối
Vận tốc được xác định: 0vE = o rE = JTE .q
Với:
34
 −a1 s1 − a2 s12 −a2 s12 0 0
J TE =  a1c1 + a2 c12 a2 c12 0 0 
 0 0 1 0 
được gọi là ma trận Jacobi tịnh tiến của khâu tác động cuối.
q =  q1 q2 q4 
T
q3
là vận tốc các biến khớp.
Thay vào công thức trên nhận được:
q 
 − a1 s1 − a2 s12 −a2 s12 0 0   1   −(a1 s1 + a2 s12 )q1 − a2 s12 q2 
vE =  a1c1 + a2 c12 0 0   2  =  (a1c1 + a2 c12 )q1 + a2 c12 q2  (1.6)
q
a2 c12
q   
 0 0 1 0   3   q3 
 q4 
+ Vận tốc góc khâu thao tác:
Từ ma trận
c124 − s124 0 a1c1 + a2c12 


s c124 0 a1s1 + a2 s12 
0
A4 =  124
 0 0 1 d1 + d 2 − q3 − d 4 
 
 0 0 0 1 
c124 − s124 0  c124 s124 0

=>  R4 =  s124
o
c124 0  , R4 =  − s124
 o T
c124 0  ,
 0 0 1   0 0 1 
 − s124 −c124 0
o
R4 = (q1 + q2 + q4 )  c124 − s124 0 
 0 0 0 
,
Khi đó:
35
 − s124 −c124 0  c124 s124 0
o
4 = o R4 o R4T = (q1 + q2 + q4 )  c124 − s124 0   − s124 c124 0
 
 0 0 0   0 0 1 
 0 −(q1 + q2 + q4 ) 0 

=  q1 + q2 + q4 0 0 
 0 0 0 
Lại có
 0 −w 4z w 4y 
 
o
w 4 = oR 4 . oR 4T =  w 4z 0 w 4x 
 −w w 4x 0 
 4y 
 0 
 
=> 4 = 4 x 4 y 4 z  =  0 0 q1 + q2 + q4 
T
=
o T
0
 
 q1 + q2 + q4 
3.1.3. Giải bài toán động học ngược

Bài toán động học ngược có ý nghĩa rất quan trọng trong lập trình và điều khiển
chuyển động của robot. Bởi vì trong thực tế thường cần điều khiển robot sao cho bàn kẹp
(khâu thao tác) di chuyển tới các vị trí nhất định trong không gian thao tác theo một quy
luật nào đó. Đối với bài toán động học ngược, quy luật chuyển động của khâu thao tác (các
tọa độ định vị) đã biết, cần xác định các tọa độ khớp (biến khớp).
3.1.4. Động học ngược vị trí
Bài toán động học ngược có thể giải bằng nhiều phương pháp khác nhau. Ở đây
nhóm xin trình bày phương pháp phổ biến nhất là Phương pháp giải tích, ngoài ra còn một
số phương pháp giải khác như phương pháp số Newton-Raphson. Trong nhiều trường hợp,
đặc biệt là khi robot có số bậc tự do lớn, việc giải bằng giải tích sẽ rất khó khăn, sử dụng
phương pháp Newton-Raphson kết hợp Matlab hoặc Maple sẽ dễ dàng hơn rất nhiều.
Với động học ngược vị trí cho robot 4 bậc tự do, ở trường hợp này là Robot lắp ráp.
Đầu vào cần xác định đó là  xE , yE , z E , 
36
+ Phương pháp giải tích:
Từ phương trình động học ta có:
 xE = a2 c12 + a1c1 (1)


 yE = a2 s12 + a1s1 (2)
z = d + d + q + d
 E 1 2 3 4 (3)
Với hướng của khâu tác động cuối  = q1 + q2 + q4 (4)
Bình phương hai vế của phương trình (1) và (2) rồi cộng vế tương ứng ta được:
xE2 + yE2 = a12 + a22 + 2a1a2c2
 xE2 + yE2 − a12 − a22

c 2 =
 2a1a2

s 2 =  1 − c 2
2
 q2 = a tan 2(s 2 ,c 2 )
Khai triển c12 và s12 ta được:
 xE = a 2cos(q1 ).cos(q 2 ) − a2sin(q1 ).sin(q 2 ) + a1cos(q1 )


 yE = a2sin(q1 ).cos(q 2 ) + a2 cos(q1 ).sin(q 2 ) + a1 sin(q1 )
 x = (a 2c2 + a1 )c1 − a2 s2 s1
 E
 yE = a2 s2c1 + (a 2c2 + a1 )s1
Giải hệ phương trình đại số tuyến tính (nghiệm c1 và s1) này ta được:
a 2c2 + a1 −a2 s2
= = a12 + a22 + 2a1a2c2 = xE2 + yE2
a2 s2 a 2c2 + a1
xE −a2 s2
1 = = a1 xE + a2 (x E c2 + y E s2 )
yE a 2c2 + a1
37
a 2c2 + a1 xE
2 = = a1 yE + a2 (y E c2 − xE s2 )
a2 s2 yE
 1 a1 xE + a2 (x E c2 + y E s2 )
 c = =
 xE2 + yE2
1


s =  2 = a1 yE + a2 (y E c2 − xE s2 )
 1  xE2 + yE2
 q1 = a tan 2(s1 ,c1 )
Từ (3) => q3 = z E − ( d1 + d 2 + d 4 )
Từ (4) => q4 =  − q1 − q2
Vậy giải bài toán động học ngược vị trí bằng phương pháp giải tích ta thu được kết
quả các biến khớp như sau:
  a1 yE + a2 (y E c2 + xE s2 ) a1 xE + a2 (x E c2 + y E s2 ) 
q1 = a tan 2  , 
  xE2 + yE2 xE2 + yE2 

q2 = a tan 2(  1 − c2 2 , c 2 )

q3 = z E − ( d1 + d 2 + d 4 )
q =  − q − q
 4 1 2
3.1.5. Động học ngược vận tốc, gia tốc

Với động học ngược vận tốc gia tốc cho robot 4 bậc tự do.
 p =  xE , yE , z E , T


Đầu vào cần xác định đó là  p =  xE , yE , z E , 
T

p =  x E , y E , z E , 
T


Đầu ra cần tìm: Ví trí (đã xác định ở động học ngược vị trí)
qT = [q1 ,q 2 , q 3 ,q 4 ]T
qT = [q1 ,q 2 , q 3 ,q 4 ]T
Ta có từ phương trình động học ta có hệ:
38
 f1 
f 
f =  2 = 0
 f3 
 
 f4 
 f1 = a2 c12 + a1c1 − x E
f =a s +as − y

Với  2 2 12 1 1 E
 f 3 = (d1 + d 2 + q3 + d 4 ) − z E
 f 4 = (q1 + q2 + q4 ) − 
Vận tốc biến khớp:

q = J q−1 J p p ;
Gia tốc biến khớp:

q = J q−1 ( − J q q + J p p + J p p ) ;
 f1 f1 f1 f1 

 q q2 q3 q4 
 1 
 f 2 f 2 f 2 f 2   − a1 s1 − a2 s12 −a2 s12 0 0
   ac +a c
q q2 q3 q4  a2 c12 0 0 
Với J q =  1 = J q =  1 1 2 12
;
 f3 f3 f3 f3   0 0 1 0
   
 q1 q2 q3 q4   1 1 0 1
 f f 4 f 4 f 4 
 4 
 q1 q2 q3 q4 
 − a1c1 − a2 c12 − a2 c12 0 0

 −a s − a s −a2 s12 0 0 
= J q =  1 1 2 12
 0 0 0 0
 
 0 0 0 0
39
 f1 f1 f1 f1 
 p p2 p3 p4 
 1 
 f 2 f 2 f 2 f 2  1 0 0 0
  0 1
p1 p2 p3 p4  0 0 
Jp = − => J p = − 
 f3 f3 f3 f3  0 0 1 0
   
 p1 p2 p3 p4  0 0 0 1
 f f 4 f 4 f 4 
 4 
 p1 p2 p3 p4 
3.2. Thiết kế quỹ đạo chuyển động của robot theo mục đích ứng dụng.
3.2.1. Xác định không gian làm việc của Robot
Cho giới hạn chuyển động của các khâu tại các khớp:
Tham số động học:
d1 = 600(mm)
d = 100(mm)
 2
d 4 = 50( mm)
a = 350(mm)
 1
a2 = 350(mm)
Biến khớp:
q =  q1 , q2 , q3 , q4 
T
q1 = ( −165  165 ) ; q2 = ( −147  147 ) ; q3 = ( 0  400mm ) ; q4 = 360o
Phương trình động học thuận của robot:
 x = 350cos(q1 + q 2 ) + 350cos(q1 )

 y = 350sin(q1 + q 2 ) + 350sin(q1 ) (mm)
 z = 750 − q3

40
Hình 36. Mô phỏng không gian làm việc trong Matlab
Hình 37. Không gian làm việc của Robot
41
Khảo sát quy luật chuyển động trong không gian khớp:
Quỹ đạo hướng đến là thực hiện di chuyển giữa hai điểm trong vùng làm việc của
Robot trong thời gian te=3s. Trong phần này nhóm sử dụng hai điểm:
Điểm A (0; 0,4 ; 0,4) và điểm B (0,4 ; 0 ; 0,6) . Hướng của khâu thao tác trong
suốt quá trình chuyển động thỏa mãn:

 = 0  = 0
Tại A:   = 0 tại B: 
  = 0
 = 0 
 
 =
 2
Giải động học ngược ta xác định được các biến khớp tại A và B lần lượt như sau:
q1 A = 0.61( Rad ) q1B = 0.96( Rad )
q = 1.93( Rad ) q = −1.93( Rad )
 2A  2B
 
q3 A = −0.35(m) q3 B = −0.15(m)
q4 A = −2.53( Rad ) q4 B = 2.54( Rad )
Quỹ đạo qi biến thiên theo bậc 3 đáp ứng về mặt tốc độ và góc khớp trong thời gian
te. Tổng quát ta có:
q(t ) = a0 + a1t + a2t 2 + a3t 3

q(t ) = a1 + 2a2t + 3a3t
2
Với:
a0 = qi (t0 )
qi (0) = qiM a = 0
q (0) = q = 0  1
 i
=> a2 = 3(qi (te ) −2 qi (t0 ))
iM

qi (te ) = qiN  te
qi (te ) = qiN = 0  −2(qi (te ) − qi (t0 ))
a3 =
 te3
Dựa trên bài toán tổng quát, nhóm tìm được quy luật chuyển động của các biến khớp
lần lượt như sau:
42
q1 = 0.61 + 0.117t 2 − 0.026t 3
q2 = 1.93 − 1.28t 2 + 0.28t 3
1 2 2 3
q3 = −0.35 + t − t
15 135
q4 = −2.53 + 1.69t 2 − 0.38t 3
Đồ thị quy luật các khâu:
Chương trình matlab:
%bien khop
t = [0:0.01:3];
q1 = 0.61+0.0117*t.^2-0.026*t.^3;
q2 = 1.93-1.28*t.^2+0.28*t.^3;
q3 = -0.35+1/15*t.^2-2/135*t.^3;
q4 = -2.53+1.69*t.^2-0.38*t.^3;
plot(t,q1,'k', t,q2,'r', t,q3,'b', t,q4,'g')
grid on
xlabel('t[s]'),ylabel('Rad')
legend('q1(rad)','q2(rad)','q3(rad)','q4(rad)')
%van toc moi bien khop

t = [0:0.01:3];
dq1 = 2*0.0117*t-3*0.026*t.^2;
dq2 = -2*1.28*t+3*0.28*t.^2;
dq3 = 2*1/15*t-3*2/135*t.^2;
dq4 = 2*1.69*t-3*0.38*t.^2;
plot(t,dq1,'k', t,dq2,'r', t,dq3,'b', t,dq4,'g')
grid on
legend('dq1(rad/s)','dq2(rad/s)','dq3(rad/s)','dq4(rad/s)')
%gia toc moi bien khop

t = [0:0.01:3];
ddq1 = 2*0.0117-2*3*0.026*t;
ddq2 = -2*1.28+2*3*0.28*t;
ddq3 = 2*1/15-2*3*2/135*t;
ddq4 = 2*1.69-2*3*0.38*t;
plot(t,ddq1,'k', t,ddq2,'r', t,ddq3,'b', t,ddq4,'g')
grid on
43
legend('ddq1(rad/s^2)','ddq2(rad/s^2)','ddq3(rad/s^2)','ddq
4(rad/s^2)')
Kết quả:
Hình 38. Đồ thị biến khớp
Hình 39. Đồ thị vận tốc góc
44
Hình 40. Đồ thị gia tốc góc
Với đối tượng lắp ráp được chọn, để thực hiện được các thao tác lắp ráp, nhóm đề
xuất quỹ đạo như sau:
Hình 41. Mô tả quỹ đạo chuyển động điểm thao tác
45
Hình 42. Mô tả không gian thao tác trong Matlab
Hình 43. Mô tả không gian thao tác và không gian làm việc
Quá trình lắp ráp bao gồm phát hiện, gắp và lắp mạch từ băng truyền 1 sang băng
truyền 2 gồm các giai đoạn sau:
- Phát hiện ra vị trí A( x A ; y A ;0.4) mạch bên băng truyền 1.
- Di chuyển đầu hút từ S0 ra A '( x A ; y A ;0.6) trong 2 giây.
46
- Di chuyển từ A '( x A ; y A ;0.6) xuống A( x A ; y A ;0.4) 2 giây và dừng lại 1 giây
để thực hiện gắp mạch.
- Di chuyển từ A lên A’ trong 2 giây.
- Di chuyển từ A’ sang B’ trong 2 giây.
- Di chuyển từ B’ xuống B (vị trí thả mạch) trong 2 giây và dừng 1 giây để
thả mạch.
- Di chuyển từ B về B’ trong 2 giây.
- Di chuyển từ B’ về A mới để thực hiện chu trình tiếp theo.
Trong quá trình thao tác để có thể thực hiện được thao tác gắp và lắp ráp thì
chuyển động của Robot phải đảm bảo không có va chạm với thành bên của mạch Vì
vậy, để có thể vào vị trí gắp mạch và đưa mạch trên băng truyền thì hướng tiếp cận
của tay kẹp robot theo quỹ đạo chuyển động tương ứng phải có phương vuông góc
với mặt phẳng băng chuyền. Do đó quỹ đạo AA’ và BB’ là đường thẳng, quỹ đạo
A’B’, SA’ không bắt buộc, ở đây nhóm chọn là đường thẳng.
Để khảo sát quỹ đạo làm việc của bài toán đặt ra, nhóm đã chọn 1 tọa độ cho
A để khảo sát:
Tọa độ các điểm:
S (0.1 ;0.6 ;0.6) là điểm khởi động của robot.
A’(0 ; 0.5 ;0.6)
A (0 ; 0.5 ;0.4)
B (0.5 ; 0 ;0.4)
B’ (0.5 ; 0 ; 0.6)
Quỹ SA’ A’A Dừng gắp AA’ A’B’ B’B Dừng thả BB’
đạo mạch mạch
Thời 0.4 0.4 0.2 0.4 0.6 0.4 0.2 0.4
gian(s)
47
Để thiết kế được quỹ đạo cho Robot, người ta thường dùng phương pháp xấp
xỉ đa thức bậc n hoặc dạng phối hợp. Trong nội dung báo cáo này nhóm sử dụng quỹ
đạo theo đa thức bậc 3. Vì dạng này vừa đơn giản trong thiết kế đồng thời thỏa mãn
được các yêu cầu như: Đảm bảo tính liên tục của vận tốc -> quỹ đạo không giật cục,
gấp khúc, gây sốc trong quá trình hoạt động của robot.
3.2.2. Thiết kế quỹ đạo trong không gian khớp
Trong không gian khớp, quỹ đạo chuyển động nhận được bằng cách gán giá
trị đầu/cuối, có thể là trung gian cho các biến khớp, tương ứng với quy luật chuyển
động mong muốn.
Quỹ đạo đa thức là quỹ đạo thường được áp dụng bởi cách tính toán đơn giản,
trong bài toán này chúng em xin đề xuất điều khiển quỹ đạo bậc 3 trong không gian
khớp.
Quỹ đạo qi biến thiên theo bậc 3 đáp ứng về mặt tốc độ và góc khớp trong thời
gian te. Tổng quát ta có:
q(t ) = a0 + a1t + a2t 2 + a3t 3

q(t ) = a1 + 2a2t + 3a3t
2
Với:
a0 = qi (t0 )
qi (0) = qiM a = 0
q (0) = q = 0  1
 i
=> a2 = 3(qi (te ) −2 qi (t0 ))
iM

qi (te ) = qiN  te
qi (te ) = qiN = 0  −2(qi (te ) − qi (t0 ))
a3 =
 te3
Tính giá trị biến khớp tại các vị trí mà robot đã được xác định trước là S, A,
A’, B, B’.
48
Hình 44. Khối động học ngược vị trí
% Tinh toan thiet ke Robot:Nhom 4
% Robot lap rap mach-Robot 4 bac tu do-ket cau Scara
% Chuong trinh tinh dong hoc nguoc vi tri
%==========================================================
================
function q = DHN_vitri(p)
% a1=0.400; a2=0.30;d4=0.050;l3=0.125;d1=0.7;d2=0.1;
% m1=16;m2=12;m3=8;m4=6;%kg
% g=9.81; %m/s^2
a1=0.35; a2=0.35; d4=0.05; %l3=0.125;

d1=0.7;d2=0.05;
m1=10;m2=12;m3=6;m4=2;%kg
g=9.81; %m/s^2
% Dau vao vi tri--------------------------------------------

---------------
xk=[p(1) p(4) p(7) p(10) p(13)];yk=[p(2) p(5) p(8) p(11)
p(14)];zk=[p(3) p(6) p(9) p(12) p(15)];
q1 = [0 0 0 0 0]; q2 = [0 0 0 0 0]; q3 = [0 0 0 0 0]; q4 = [0
0 0 0 0];
49
% % Dau vao huong-------------------------------------------
-----------------
eta=[0 0 0 pi/2 pi/2];
% alpha=0;beta=0;
%Tinh dong hoc nguoc vi tri---------------------------------
---------------
for i = 1:5
c2=(xk(i)^2+yk(i)^2-a1^2-a2^2)/(2*a1*a2);
s2=sqrt(1-c2^2);
q2(i)=atan2(s2,c2);
c1=((a2*c2+a1)*xk(i)+a2*s2*yk(i))/(xk(i)^2+yk(i)^2);
s1=((a2*c2+a1)*yk(i)-a2*s2*xk(i))/(xk(i)^2+yk(i)^2);
q1(i)=atan2(s1,c1);
q3(i)=zk(i)-d4-d1-d2;
q4(i)=eta(i)-q1(i)-q2(i);
end
q = [q1(1) q1(2) q1(3) q1(4) q1(5) q2(1) q2(2) q2(3) q2(4)
q2(5) q3(1) q3(2) q3(3) q3(4) q3(5) q4(1) q4(2) q4(3) q4(4)
q4(5)]
Sau khi xác định các giá trị biến khớp thì xây dựng quỹ đạo bậc 3:
Hình 45. Khối quỹ đạo lắp ráp

function [q,dq,ddq] = QD_laprap_codung(t)
StartPoint=[0.8880 1.0354 -0.6000 -1.9233];

50
% 4 Diem lap rap
A1=[0.7956 1.5504 -0.4000 -2.3460]; % Diem A
A2=[0.7956 1.5504 -0.2000 -2.3460]; % Diem A'
B1=[-0.7752 1.5504 -0.4000 0.7956]; % Diem B
B2=[-0.7752 1.5504 -0.2000 0.7956]; % Diem B'
%
% StartPoint=[0.1 0.6 0.2];
% % 4 Diem lap rap
% A1=[0 0.5 0.1]; % Diem A
% A2=[0 0.5 0.2]; % Diem A'
% B1=[0.5 0 0.1]; % Diem B
% B2=[0.5 0 0.2]; % Diem B'
% Thoi gian cac doan quy dao SA'-A'A-AA'-A'B'-B'B---------

T=[0.4 0.4 0.4 0.6 0.4 0.4]; % Don vi:s
% Thoi gian dung de gap vat va th? vat -------------------
Td =0.2; % Don vi:s
%================Nhap cac diem quy dao====================

if (t<=T(1))&&(t>=0)
A=StartPoint; % Quy dao S-A'
B=A2;
Te=T(1);t_in = t - 0;
q1 = A(1) + 3*(B(1)-A(1))*(t_in/Te)^2 - 2*(B(1)-

A(1))*(t_in/Te)^3;
dq1 = 6*(B(1)-A(1))*t_in*(1/Te)^2 - 6*(B(1)-
A(1))*t_in^2*(1/Te)^3;
ddq1 = 6*(B(1)-A(1))*(1/Te)^2 - 12*(B(1)-A(1))*Te*(1/Te)^3;
q2 = A(2) + 3*(B(2)-A(2))*(t_in/Te)^2 - 2*(B(2)-
A(2))*(t_in/Te)^3;
dq2 = 6*(B(2)-A(2))*t_in*(1/Te)^2 - 6*(B(2)-
A(2))*t_in^2*(1/Te)^3;
ddq2 = 6*(B(2)-A(2))*(1/Te)^2 - 12*(B(2)-A(2))*Te*(1/Te)^3;
q3 = A(3) + 3*(B(3)-A(3))*(t_in/Te)^2 - 2*(B(3)-
A(3))*(t_in/Te)^3;
dq3 = 6*(B(3)-A(3))*t_in*(1/Te)^2 - 6*(B(3)-
A(3))*t_in^2*(1/Te)^3;
ddq3 = 6*(B(3)-A(3))*(1/Te)^2 - 12*(B(3)-A(3))*Te*(1/Te)^3;
q4 = A(4) + 3*(B(4)-A(4))*(t_in/Te)^2 - 2*(B(4)-
A(4))*(t_in/Te)^3;
51
dq4 = 6*(B(4)-A(4))*t_in*(1/Te)^2 - 6*(B(4)-
A(4))*t_in^2*(1/Te)^3;
ddq4 = 6*(B(4)-A(4))*(1/Te)^2 - 12*(B(4)-A(4))*Te*(1/Te)^3;
q = [q1;q2;q3;q4];
dq = [dq1;dq2;dq3;dq4];
ddq = [ddq1;ddq2;ddq3;ddq4];
% xA=A(1);yA=A(2);zA=A(3);
% xB=B(1);yB=B(2);zB=B(3);
% xE=xA+3*(xB-xA)/(Te^2)*(t-t_in)^2-2*(xB-xA)/(Te^3)*(t-
t_in)^3;
% yE=(yB-yA)/(xB-xA)*xE+(yA*xB-yB*xA)/(xB-xA);
% zE=(zB-zA)/(xB-xA)*xE+(zA*xB-zB*xA)/(xB-xA);
% vxE=2*3*(xB-xA)/(Te^2)*(t-t_in)-2*3*(xB-xA)/(Te^3)*(t-
t_in)^2;
% vyE=(yB-yA)/(xB-xA)*vxE;
% vzE=(zB-zA)/(xB-xA)*vxE;
% axE=2*3*(xB-xA)/(Te^2)-2*3*2*(xB-xA)/(Te^3)*(t-t_in);
% ayE=(yB-yA)/(xB-xA)*axE;
% azE=(zB-zA)/(xB-xA)*axE;
elseif (t<=T(1)+T(2))&&(t>=T(1))
A=A2; % Quy dao A'-A
B=A1;
Te=T(2);t_in=t - T(1);
q1 = A(1) + 3*(B(1)-A(1))*(t_in/Te)^2 - 2*(B(1)-

A(1))*(t_in/Te)^3;
dq1 = 6*(B(1)-A(1))*t_in*(1/Te)^2 - 6*(B(1)-
A(1))*t_in^2*(1/Te)^3;
ddq1 = 6*(B(1)-A(1))*(1/Te)^2 - 12*(B(1)-A(1))*Te*(1/Te)^3;
q2 = A(2) + 3*(B(2)-A(2))*(t_in/Te)^2 - 2*(B(2)-
A(2))*(t_in/Te)^3;
dq2 = 6*(B(2)-A(2))*t_in*(1/Te)^2 - 6*(B(2)-
A(2))*t_in^2*(1/Te)^3;
ddq2 = 6*(B(2)-A(2))*(1/Te)^2 - 12*(B(2)-A(2))*Te*(1/Te)^3;
q3 = A(3) + 3*(B(3)-A(3))*(t_in/Te)^2 - 2*(B(3)-
A(3))*(t_in/Te)^3;
dq3 = 6*(B(3)-A(3))*t_in*(1/Te)^2 - 6*(B(3)-
A(3))*t_in^2*(1/Te)^3;
ddq3 = 6*(B(3)-A(3))*(1/Te)^2 - 12*(B(3)-A(3))*Te*(1/Te)^3;
q4 = A(4) + 3*(B(4)-A(4))*(t_in/Te)^2 - 2*(B(4)-
A(4))*(t_in/Te)^3;
dq4 = 6*(B(4)-A(4))*t_in*(1/Te)^2 - 6*(B(4)-
A(4))*t_in^2*(1/Te)^3;
52
ddq4 = 6*(B(4)-A(4))*(1/Te)^2 - 12*(B(4)-A(4))*Te*(1/Te)^3;
q = [q1;q2;q3;q4];
% Dung de gap vat

elseif (t<=T(1)+T(2)+Td)&&(t>=T(1)+T(2))
A=A1;B=A1; % Quy dao A-A: Dung tai diem A
Te=Td;t_in=t-T(1)-T(2);
q1 = A(1) + 3*(B(1)-A(1))*(t_in/Te)^2 - 2*(B(1)-

A(1))*(t_in/Te)^3;
dq1 = 6*(B(1)-A(1))*t_in*(1/Te)^2 - 6*(B(1)-
A(1))*t_in^2*(1/Te)^3;
ddq1 = 6*(B(1)-A(1))*(1/Te)^2 - 12*(B(1)-A(1))*Te*(1/Te)^3;
q2 = A(2) + 3*(B(2)-A(2))*(t_in/Te)^2 - 2*(B(2)-
A(2))*(t_in/Te)^3;
dq2 = 6*(B(2)-A(2))*t_in*(1/Te)^2 - 6*(B(2)-
A(2))*t_in^2*(1/Te)^3;
ddq2 = 6*(B(2)-A(2))*(1/Te)^2 - 12*(B(2)-A(2))*Te*(1/Te)^3;
q3 = A(3) + 3*(B(3)-A(3))*(t_in/Te)^2 - 2*(B(3)-
A(3))*(t_in/Te)^3;
dq3 = 6*(B(3)-A(3))*t_in*(1/Te)^2 - 6*(B(3)-
A(3))*t_in^2*(1/Te)^3;
ddq3 = 6*(B(3)-A(3))*(1/Te)^2 - 12*(B(3)-A(3))*Te*(1/Te)^3;
q4 = A(4) + 3*(B(4)-A(4))*(t_in/Te)^2 - 2*(B(4)-
A(4))*(t_in/Te)^3;
dq4 = 6*(B(4)-A(4))*t_in*(1/Te)^2 - 6*(B(4)-
A(4))*t_in^2*(1/Te)^3;
ddq4 = 6*(B(4)-A(4))*(1/Te)^2 - 12*(B(4)-A(4))*Te*(1/Te)^3;
q = [q1;q2;q3;q4];
%
elseif (t<=T(1)+T(2)+T(3)+Td)&&(t>=T(1)+T(2)+Td)
A=A1; %Quy dao A-A'
B=A2;
Te=T(3);t_in=t-(T(1)+T(2)+Td);
q1 = A(1) + 3*(B(1)-A(1))*(t_in/Te)^2 - 2*(B(1)-

A(1))*(t_in/Te)^3;
dq1 = 6*(B(1)-A(1))*t_in*(1/Te)^2 - 6*(B(1)-
A(1))*t_in^2*(1/Te)^3;
53
ddq1 = 6*(B(1)-A(1))*(1/Te)^2 - 12*(B(1)-A(1))*Te*(1/Te)^3;
q2 = A(2) + 3*(B(2)-A(2))*(t_in/Te)^2 - 2*(B(2)-
A(2))*(t_in/Te)^3;
dq2 = 6*(B(2)-A(2))*t_in*(1/Te)^2 - 6*(B(2)-
A(2))*t_in^2*(1/Te)^3;
ddq2 = 6*(B(2)-A(2))*(1/Te)^2 - 12*(B(2)-A(2))*Te*(1/Te)^3;
q3 = A(3) + 3*(B(3)-A(3))*(t_in/Te)^2 - 2*(B(3)-
A(3))*(t_in/Te)^3;
dq3 = 6*(B(3)-A(3))*t_in*(1/Te)^2 - 6*(B(3)-
A(3))*t_in^2*(1/Te)^3;
ddq3 = 6*(B(3)-A(3))*(1/Te)^2 - 12*(B(3)-A(3))*Te*(1/Te)^3;
q4 = A(4) + 3*(B(4)-A(4))*(t_in/Te)^2 - 2*(B(4)-
A(4))*(t_in/Te)^3;
dq4 = 6*(B(4)-A(4))*t_in*(1/Te)^2 - 6*(B(4)-
A(4))*t_in^2*(1/Te)^3;
ddq4 = 6*(B(4)-A(4))*(1/Te)^2 - 12*(B(4)-A(4))*Te*(1/Te)^3;
q = [q1;q2;q3;q4];
elseif (t<=T(1)+T(2)+T(3)+T(4)+Td)&&(t>=T(1)+T(2)+T(3)+Td)
A=A2; % Quy dao A'-B'
B=B2;
Te=T(4);t_in=t-(T(1)+T(2)+T(3)+Td);
q1 = A(1) + 3*(B(1)-A(1))*(t_in/Te)^2 - 2*(B(1)-

A(1))*(t_in/Te)^3;
dq1 = 6*(B(1)-A(1))*t_in*(1/Te)^2 - 6*(B(1)-
A(1))*t_in^2*(1/Te)^3;
ddq1 = 6*(B(1)-A(1))*(1/Te)^2 - 12*(B(1)-A(1))*Te*(1/Te)^3;
q2 = A(2) + 3*(B(2)-A(2))*(t_in/Te)^2 - 2*(B(2)-
A(2))*(t_in/Te)^3;
dq2 = 6*(B(2)-A(2))*t_in*(1/Te)^2 - 6*(B(2)-
A(2))*t_in^2*(1/Te)^3;
ddq2 = 6*(B(2)-A(2))*(1/Te)^2 - 12*(B(2)-A(2))*Te*(1/Te)^3;
q3 = A(3) + 3*(B(3)-A(3))*(t_in/Te)^2 - 2*(B(3)-
A(3))*(t_in/Te)^3;
dq3 = 6*(B(3)-A(3))*t_in*(1/Te)^2 - 6*(B(3)-
A(3))*t_in^2*(1/Te)^3;
ddq3 = 6*(B(3)-A(3))*(1/Te)^2 - 12*(B(3)-A(3))*Te*(1/Te)^3;
q4 = A(4) + 3*(B(4)-A(4))*(t_in/Te)^2 - 2*(B(4)-
A(4))*(t_in/Te)^3;
dq4 = 6*(B(4)-A(4))*t_in*(1/Te)^2 - 6*(B(4)-
A(4))*t_in^2*(1/Te)^3;
54
ddq4 = 6*(B(4)-A(4))*(1/Te)^2 - 12*(B(4)-A(4))*Te*(1/Te)^3;
q = [q1;q2;q3;q4];
elseif (t<=T(1)+T(2)+T(3)+T(4)+T(5)+Td) &&

(t>=T(1)+T(2)+T(3)+T(4)+Td)
A=B2; % Quy dao B'-B
B=B1;
Te=T(5);t_in=t-(T(1)+T(2)+T(3)+T(4)+Td);
q1 = A(1) + 3*(B(1)-A(1))*(t_in/Te)^2 - 2*(B(1)-

A(1))*(t_in/Te)^3;
dq1 = 6*(B(1)-A(1))*t_in*(1/Te)^2 - 6*(B(1)-
A(1))*t_in^2*(1/Te)^3;
ddq1 = 6*(B(1)-A(1))*(1/Te)^2 - 12*(B(1)-A(1))*Te*(1/Te)^3;
q2 = A(2) + 3*(B(2)-A(2))*(t_in/Te)^2 - 2*(B(2)-
A(2))*(t_in/Te)^3;
dq2 = 6*(B(2)-A(2))*t_in*(1/Te)^2 - 6*(B(2)-
A(2))*t_in^2*(1/Te)^3;
ddq2 = 6*(B(2)-A(2))*(1/Te)^2 - 12*(B(2)-A(2))*Te*(1/Te)^3;
q3 = A(3) + 3*(B(3)-A(3))*(t_in/Te)^2 - 2*(B(3)-
A(3))*(t_in/Te)^3;
dq3 = 6*(B(3)-A(3))*t_in*(1/Te)^2 - 6*(B(3)-
A(3))*t_in^2*(1/Te)^3;
ddq3 = 6*(B(3)-A(3))*(1/Te)^2 - 12*(B(3)-A(3))*Te*(1/Te)^3;
q4 = A(4) + 3*(B(4)-A(4))*(t_in/Te)^2 - 2*(B(4)-
A(4))*(t_in/Te)^3;
dq4 = 6*(B(4)-A(4))*t_in*(1/Te)^2 - 6*(B(4)-
A(4))*t_in^2*(1/Te)^3;
ddq4 = 6*(B(4)-A(4))*(1/Te)^2 - 12*(B(4)-A(4))*Te*(1/Te)^3;
q = [q1;q2;q3;q4];
% Dung de tha vat

elseif (t<=T(1)+T(2)+T(3)+T(4)+T(5)+2*Td) &&
(t>=T(1)+T(2)+T(3)+T(4)+T(5)+Td)
A=B1; B=B1; % Quy dao B-B: Dung tai diem B
Te=Td;t_in=t-(T(1)+T(2)+T(3)+T(4)+T(5)+Td);
q1 = A(1) + 3*(B(1)-A(1))*(t_in/Te)^2 - 2*(B(1)-

A(1))*(t_in/Te)^3;
55
dq1 = 6*(B(1)-A(1))*t_in*(1/Te)^2 - 6*(B(1)-
A(1))*t_in^2*(1/Te)^3;
ddq1 = 6*(B(1)-A(1))*(1/Te)^2 - 12*(B(1)-A(1))*Te*(1/Te)^3;
q2 = A(2) + 3*(B(2)-A(2))*(t_in/Te)^2 - 2*(B(2)-
A(2))*(t_in/Te)^3;
dq2 = 6*(B(2)-A(2))*t_in*(1/Te)^2 - 6*(B(2)-
A(2))*t_in^2*(1/Te)^3;
ddq2 = 6*(B(2)-A(2))*(1/Te)^2 - 12*(B(2)-A(2))*Te*(1/Te)^3;
q3 = A(3) + 3*(B(3)-A(3))*(t_in/Te)^2 - 2*(B(3)-
A(3))*(t_in/Te)^3;
dq3 = 6*(B(3)-A(3))*t_in*(1/Te)^2 - 6*(B(3)-
A(3))*t_in^2*(1/Te)^3;
ddq3 = 6*(B(3)-A(3))*(1/Te)^2 - 12*(B(3)-A(3))*Te*(1/Te)^3;
q4 = A(4) + 3*(B(4)-A(4))*(t_in/Te)^2 - 2*(B(4)-
A(4))*(t_in/Te)^3;
dq4 = 6*(B(4)-A(4))*t_in*(1/Te)^2 - 6*(B(4)-
A(4))*t_in^2*(1/Te)^3;
ddq4 = 6*(B(4)-A(4))*(1/Te)^2 - 12*(B(4)-A(4))*Te*(1/Te)^3;
q = [q1;q2;q3;q4];
elseif (t<=T(1)+T(2)+T(3)+T(4)+T(5)+T(6)+2*Td) &&

(t>=T(1)+T(2)+T(3)+T(4)+T(5)+2*Td)
A=B1; % Quy dao B-B'
B=B2;
Te=T(6);t_in=t - (T(1)+T(2)+T(3)+T(4)+T(5)+2*Td);
q1 = A(1) + 3*(B(1)-A(1))*(t_in/Te)^2 - 2*(B(1)-

A(1))*(t_in/Te)^3;
dq1 = 6*(B(1)-A(1))*t_in*(1/Te)^2 - 6*(B(1)-
A(1))*t_in^2*(1/Te)^3;
ddq1 = 6*(B(1)-A(1))*(1/Te)^2 - 12*(B(1)-A(1))*Te*(1/Te)^3;
q2 = A(2) + 3*(B(2)-A(2))*(t_in/Te)^2 - 2*(B(2)-
A(2))*(t_in/Te)^3;
dq2 = 6*(B(2)-A(2))*t_in*(1/Te)^2 - 6*(B(2)-
A(2))*t_in^2*(1/Te)^3;
ddq2 = 6*(B(2)-A(2))*(1/Te)^2 - 12*(B(2)-A(2))*Te*(1/Te)^3;
q3 = A(3) + 3*(B(3)-A(3))*(t_in/Te)^2 - 2*(B(3)-
A(3))*(t_in/Te)^3;
dq3 = 6*(B(3)-A(3))*t_in*(1/Te)^2 - 6*(B(3)-
A(3))*t_in^2*(1/Te)^3;
ddq3 = 6*(B(3)-A(3))*(1/Te)^2 - 12*(B(3)-A(3))*Te*(1/Te)^3;
56
q4 = A(4) + 3*(B(4)-A(4))*(t_in/Te)^2 - 2*(B(4)-
A(4))*(t_in/Te)^3;
dq4 = 6*(B(4)-A(4))*t_in*(1/Te)^2 - 6*(B(4)-
A(4))*t_in^2*(1/Te)^3;
ddq4 = 6*(B(4)-A(4))*(1/Te)^2 - 12*(B(4)-A(4))*Te*(1/Te)^3;
q = [q1;q2;q3;q4];
else
q = [0;0;0;0];
dq = [0;0;0;0];
ddq = [0;0;0;0];
end
end
Kết quả quỹ đạo thu được:
Hình 46. Giá trị biến khớp
57
Hình 47. Giá trị vận tốc
Hình 48. Giá trị gia tốc
58
3.2.3. Thiết kế quỹ đạo trong không gian thao tác, quỹ đạo đường thẳng
Tổng quát: ta có phương trình đường thẳng đi qua 2 điểm M (x o , yo , z o ) và
N (x e , ye , z e ) là:
x − x0 y − y0 z − z0
= =
xe − x0 ye − y0 ze − z0
Độ dời dịch chuyển của điểm thao tác E theo phương x,y,z là đa thức bậc 3
theo thời gian:

 x = a0 + a1t + a2t + a3t
2 3

 x = a1 + 2a2t + 3a3t
2

ye − y0 y x −y x
y= x+ 0 e e 0
xe − x0 xe − x0
ze − z0 z x −z x
z= x+ 0 e e 0
xe − x0 xe − x0
Với thời gian đặt cho quỹ đạo là te(s) để đi từ M đến N theo một đường thẳng.
Với điều kiện đầu:
 xE (0) = xo
 x (0) = x = 0
 E o

 xE (t e ) = xe
 xE (t e ) = xe = 0
a0 = x0

a1 = 0
 3(x e − x0 )
Ta xác định được các hệ số như sau: a2 =
 te2
 −2(x e − x0 )
a3 =
 te3
+ Hướng của khâu thao tác:

Ngoài việc quỹ đạo chuyển động phải bám theo vị trí đã thiết lập thì hướng
của khâu thao tác phải thỏa mãn điều kiện lắp ráp của đối tượng. Bảng mạch gắp từ
dây chuyền 2 sẽ phải được quay góc 90o.
59
Hướng của khâu thao tác được xác định theo 3 góc Cardan như đã trình bày ở
phần phương trình động học:
 = 0

 = 0
 = q + q + q
 1 2 4
Theo quỹ đạo đã thiết lập thì trên đoạn SA’ và A’A: ŋ = 0, góc ŋ sẽ thay đổi trong
đoạn quỹ đạo A’B’ từ 0 đến 90o, trên đoạn B’B: ŋ = 90o.
Góc ŋ cũng được xây dựng theo dạng bậc 3, với vận tốc đầu và vận tốc cuối đều
bằng 0: thực hiện quay từ ŋo = 0 đến ŋ1 = 90o = π/2.
 = b0 + b1t + b2t + b3t
 2 3

 = b1 + 2b2t + 3b3t
2

b0 = 0

b1 = 0
 3( −  )
=> b2 = e 2 0
 te
 −2(e − 0 )
b3 =
 te3
Từ bài toán xây dựng tổng quát quỹ đạo thỏa mãn vị trí và tư thế của khâu thao tác
như trên. Bằng cách sử dụng phần mềm Matlab-Simulink, nhóm đưa ra phương án tối giản
hóa quá trình tính toán: Chỉ cần cung cấp các tham số đầu vào là tọa độ các điểm, hướng
yêu cầu của khâu thao tác, thời gian thực hiện, phần mềm sẽ trả về quy luật chuyển động
theo yêu cầu:
Chương trình tính toán:
function [E,vE,aE,Eta_angle] = QD_laprap_codung(t)
% StartPoint=[0.68 0.01 0.65];

% % 4 Diem lap rap
% A1=[0.5 0.05 0.4]; % Diem A
% A2=[0.5 0.05 0.6]; % Diem A'
60
% B1=[0.2 0.6 0.405]; % Diem B
% B2=[0.2 0.6 0.6]; % Diem B'
StartPoint=[0.1 0.6 0.6];

% 4 Diem lap rap
A1=[0 0.5 0.4]; % Diem A
A2=[0 0.5 0.6]; % Diem A'
B1=[0.5 0 0.4]; % Diem B
B2=[0.5 0 0.6]; % Diem B'
% Huong quy dao khau thao tac-----------------------------

eta0=0;
eta1=pi/2;
% Thoi gian cac doan quy dao SA'-A'A-AA'-A'B'-B'B---------
T=[0.4 0.4 0.4 0.6 0.4 0.4]; % Don vi:s
% Thoi gian dung de gap vat va th? vat -------------------
Td =0.2; % Don vi:s
%================Nhap cac diem quy dao====================

if (t<=T(1))&&(t>=0)
A=StartPoint; % Quy dao S-A'
B=A2;
Te=T(1);t_in=0;
xA=A(1);yA=A(2);zA=A(3);
xB=B(1);yB=B(2);zB=B(3);
xE=xA+3*(xB-xA)/(Te^2)*(t-t_in)^2-2*(xB-xA)/(Te^3)*(t-
t_in)^3;
yE=(yB-yA)/(xB-xA)*xE+(yA*xB-yB*xA)/(xB-xA);
zE=(zB-zA)/(xB-xA)*xE+(zA*xB-zB*xA)/(xB-xA);
vxE=2*3*(xB-xA)/(Te^2)*(t-t_in)-2*3*(xB-xA)/(Te^3)*(t-
t_in)^2;
vyE=(yB-yA)/(xB-xA)*vxE;
vzE=(zB-zA)/(xB-xA)*vxE;
axE=2*3*(xB-xA)/(Te^2)-2*3*2*(xB-xA)/(Te^3)*(t-t_in);
ayE=(yB-yA)/(xB-xA)*axE;
azE=(zB-zA)/(xB-xA)*axE;
eta=eta0;
etad=0;
etadd=0;
E=[xE;yE;zE];
vE=[vxE;vyE;vzE];
aE=[axE;ayE;azE];
Eta_angle=[eta;etad;etadd];
61
elseif (t<=T(1)+T(2))&&(t>=T(1))
A=A2; % Quy dao A'-A
B=A1;
Te=T(2);t_in=T(1);
xA=A(1);yA=A(2);zA=A(3);
xB=B(1);yB=B(2);zB=B(3);
zE=zA+3*(zB-zA)/(Te^2)*(t-t_in)^2-2*(zB-zA)/(Te^3)*(t-
t_in)^3;
xE=(xB-xA)/(zB-zA)*zE+(xA*zB-xB*zA)/(zB-zA);
yE=(yB-yA)/(zB-zA)*zE+(yA*zB-yB*zA)/(zB-zA);
vzE=2*3*(zB-zA)/(Te^2)*(t-t_in)-2*3*(zB-zA)/(Te^3)*(t-
t_in)^2;
vxE=(xB-xA)/(zB-zA)*vzE;
vyE=(yB-yA)/(zB-zA)*vzE;
azE=2*3*(zB-zA)/(Te^2)-2*3*2*(zB-zA)/(Te^3)*(t-t_in);
axE=(xB-xA)/(zB-zA)*azE;
ayE=(yB-yA)/(zB-zA)*azE;
eta=eta0;
etad=0;
etadd=0;
E=[xE;yE;zE];
vE=[vxE;vyE;vzE];
aE=[axE;ayE;azE];
% Dung de gap vat

elseif (t<=T(1)+T(2)+Td)&&(t>=T(1)+T(2))
A=A1; % Quy dao A-A: Dung tai diem A
xA=A(1);yA=A(2);zA=A(3);
zE=zA;
xE=xA;
yE=yA;
vzE=0;
vxE=0;
vyE=0;
azE=0;
axE=0;
ayE=0;
eta=eta0;
etad=0;
etadd=0;
E=[xE;yE;zE];
vE=[vxE;vyE;vzE];
62
aE=[axE;ayE;azE];
%
elseif (t<=T(1)+T(2)+T(3)+Td)&&(t>=T(1)+T(2)+Td)
A=A1; %Quy dao A-A'
B=A2;
Te=T(3);t_in=T(1)+T(2)+Td;
xA=A(1);yA=A(2);zA=A(3);
xB=B(1);yB=B(2);zB=B(3);
t_in)^3;
t_in)^2;
eta=eta0;
etad=0;etadd=0;
E=[xE;yE;zE];
vE=[vxE;vyE;vzE];
aE=[axE;ayE;azE];
elseif (t<=T(1)+T(2)+T(3)+T(4)+Td)&&(t>=T(1)+T(2)+T(3)+Td)
A=A2; % Quy dao A'-B'
B=B2;
Te=T(4);t_in=T(1)+T(2)+T(3)+Td;
xA=A(1);yA=A(2);zA=A(3);
xB=B(1);yB=B(2);zB=B(3);
xE=xA+3*(xB-xA)/(Te^2)*(t-t_in)^2-2*(xB-xA)/(Te^3)*(t-
t_in)^3;
yE=(yB-yA)/(xB-xA)*xE+(yA*xB-yB*xA)/(xB-xA);
zE=(zB-zA)/(xB-xA)*xE+(zA*xB-zB*xA)/(xB-xA);
vxE=2*3*(xB-xA)/(Te^2)*(t-t_in)-2*3*(xB-xA)/(Te^3)*(t-
t_in)^2;
vyE=(yB-yA)/(xB-xA)*vxE;
vzE=(zB-zA)/(xB-xA)*vxE;
axE=2*3*(xB-xA)/(Te^2)-2*3*2*(xB-xA)/(Te^3)*(t-t_in);
63
ayE=(yB-yA)/(xB-xA)*axE;
azE=(zB-zA)/(xB-xA)*axE;
eta=eta0+3*(eta1-eta0)/(Te^2)*(t-t_in)^2-2*(eta1-
eta0)/(Te^3)*(t-t_in)^3;
etad=2*3*(eta1-eta0)/(Te^2)*(t-t_in)-2*3*(eta1-
eta0)/(Te^3)*(t-t_in)^2;
etadd=2*3*(eta1-eta0)/(Te^2)-2*3*2*(eta1-eta0)/(Te^3)*(t-
t_in);
E=[xE;yE;zE];
vE=[vxE;vyE;vzE];
aE=[axE;ayE;azE];
elseif (t<=T(1)+T(2)+T(3)+T(4)+T(5)+Td) &&

(t>=T(1)+T(2)+T(3)+T(4)+Td)
A=B2; % Quy dao B'-B
B=B1;
Te=T(5);t_in=T(1)+T(2)+T(3)+T(4)+Td;
xA=A(1);yA=A(2);zA=A(3);
xB=B(1);yB=B(2);zB=B(3);
t_in)^3;
t_in)^2;
eta=eta1;
etad=0;etadd=0;
E=[xE;yE;zE];
vE=[vxE;vyE;vzE];
aE=[axE;ayE;azE];
% Dung de tha vat

elseif (t<=T(1)+T(2)+T(3)+T(4)+T(5)+2*Td) &&
(t>=T(1)+T(2)+T(3)+T(4)+T(5)+Td)
A=B1; % Quy dao B-B: Dung tai diem B
xA=A(1);yA=A(2);zA=A(3);
zE=zA;
64
xE=xA;
yE=yA;
vzE=0;
vxE=0;
vyE=0;
azE=0;
axE=0;
ayE=0;
eta=eta0;
etad=0;
etadd=0;
E=[xE;yE;zE];
vE=[vxE;vyE;vzE];
aE=[axE;ayE;azE];
elseif (t<=T(1)+T(2)+T(3)+T(4)+T(5)+T(6)+2*Td) &&

(t>=T(1)+T(2)+T(3)+T(4)+T(5)+2*Td)
A=B1; % Quy dao B-B'
B=B2;
Te=T(6);t_in=T(1)+T(2)+T(3)+T(4)+T(5)+2*Td;
xA=A(1);yA=A(2);zA=A(3);
xB=B(1);yB=B(2);zB=B(3);
t_in)^3;
t_in)^2;
eta=eta1;
etad=0;etadd=0;
E=[xE;yE;zE];
vE=[vxE;vyE;vzE];
aE=[axE;ayE;azE];
else
E=[0;0;0];
65
vE=[0;0;0];
aE=[0;0;0];
Eta_angle=[0;0;0];
end
end
66
Kết hợp hai chương trình trên với Simulink:
Hình 49. Simulink

Kết quả:
Trong không gian thao tác:
67
68
69
Trong không gian khớp:
Biến khớp:
70
71
Vận tốc biến khớp:
72
73
Gia tốc biến khớp:
74
75
4. PHÂN TÍCH TRẠNG THÁI (TĨNH) YÊU CẦU LỰC/MOMEN ĐỘNG CƠ LỚN
NHẤT
4.1. Cơ sở lí thuyết
Phương pháp khảo sát theo điều kiện cân bằng:
 Roi' = 0

 M oi = 0
 Roi' = Fi ,i −1 − Fi +1,i + Pi = 0

 M oi = M i ,i −1 − M i +1,i + ri −1  Fi ,i −1 + rci  Pi = 0
i i
Trong dạng ma trận:
 k Roi' = k Fi ,i −1 − k Fi +1,i + k Pi = 0
k
 M oi = M i ,i −1 − M i +1,i + ri −1. Fi ,i −1 + rci . Pi = 0
k k k i k k i k
Các ma trận đối xứng lệch:
 0 − k zi −1 k yi −1   0 − k zci k
yci 
   
ri −1 =  k zi −1
k i
0 − k xi −1  , k rcii =  k zci 0 − k xci 
 − k yi −1 k xi −1 0   − k yci k
xci 0 
 
Có thể sử dụng các ma trận quay để biểu diễn các vector định vị điểm tác dụng lực
0 i 0 i
trong các hệ quy chiếu khác nhau. Các vecto r , r biểu diễn trong hệ tọa độ cơ sở và
i −1 ci
được xác định:


 ri −1 = Ri ri −1
0 i 0 i i
0 i 0 i i
 rci = Ri rci

Hệ phương trình cân bằng lực trong hệ tọa độ khâu i:
 k Fi ,i −1 = k Fi +1,i − k Pi
k
 M i ,i −1 = M i +1,i − ri −1. Fi ,i −1 − rci . Pi
k k i k k i k
76
Trong hệ tọa độ cơ sở:
 0 Fi ,i −1 = 0 Fi +1,i −0 Pi
0
 M i ,i −1 = M i +1,i − ri −1. Fi ,i −1 − rci . Pi
0 0 i 0 0 i 0
4.2. Giải bài toán cụ thể
Hình 50. Hệ tọa độ khảo sát tĩnh học

Lực tác dụng từ Robot lên đối tượng công nghệ F, M:
T T
0
F5,4 =  0 Fx 0
Fy 0
Fz  , 0 M5,4 = 0 0 0
M z 
Coi các khâu là những thanh đồng chất tiết diện ngang không đáng kể.
Vị trí tương đối của gốc hệ tọa độ i rii−1 và vị trí trọng tâm của các khâu i rcii được xác
định như sau:
 −a
T

r =  −a 1 0 0 , r =  1 0 0 
1 1 T 1 1
0 c1
 2 
 −a
T

r =  −a 2 0 0 , r =  2
2 2 T 2 2
1 c2 0 0
 2 
T
 d 
r = 0 0 d3 
T
3 3
2 , r = 0 0 3 
3 3
c3
 2
77
T
 d 
r = 0 0 d 4 
T
4 4
3 , r = 0 0 4 
4 4
c4
 2
Vecto gia tốc trọng trường và trọng lực các khâu biểu diễn trong hệ tọa độ cơ sở:
g i =  0 0 −g 
0 T
Pi = mi g i =  0 0 −mi g 
0 T
Áp dụng hệ phương trình đệ quy để tính toán lực trong các khớp:
 0 Fi,i −1 = 0 Fi +1,i − 0 Pi
0
 M i,i −1 = M i +1,i − ri −1 Fi,i −1 − rci Pi
0 0 i 0 0 i 0
Ta tính các vecto 0 rii−1 , 0 rcii sau đó thiết lập các ma trận đối xứng lệch 0 rii−1 , 0 rcii từ các
vecto này.
+ Khâu 4:
Biểu diễn 0 r34 , 0 rc44 trong hệ tọa độ cơ sở:
cos(1 + 2 + 4 ) − sin(1 + 2 + 4 ) 0   0   0 
r3 = 0 R 4 . 4 r34 =  sin(1 + 2 + 4 ) cos(1 + 2 + 4 ) 0  .  0  =  0 
0 4
 0 0 1  d 4  d 4 
   
cos(1 + 2 + 4 ) − sin(1 + 2 + 4 ) 0   0   0 
   
rc4 = 0 R 4 . 4 rc44 =  sin(1 + 2 + 4 ) cos(1 + 2 + 4 ) 0  .  0  =  0 
0 4
 0 0 1   d 4   d 4 
   
2 2
Ma trận đối xứng lệch:
 d4 
0 − 0
−d 4 2
0 0  
r3 = d 4 
0  , rc4 =  4 0
0 4 0 4 d
0 0
2 
 0 0  0
0
 0 0 
 
Hệ phương trình đệ quy:
78
 0 F4,3 = 0 F5,4 − 0 P4
0
 M 4,3 = M 5,4 − r3 F4,3 − rc4 P4
0 0 40 0 4 0
  Fx 
0  
 F4,3 =  Fy 

  Fz + m 4g 
= 
  d 4 Fy 
 M =  −d F 
0
 4,3  4 x 

  M z 
Momen do động cơ sinh ra: U 4 = M z

+ Khâu 3:
cos(1 + 2 ) − sin(1 + 2 ) 0   0   0 
r2 = 0 R 3 . 3 r23 =  sin(1 + 2 ) cos(1 + 2 ) 0  .  0  =  0 
0 3
 0 0 1  d 3   d 3 
   
cos(1 + 2 ) − sin(1 + 2 ) 0   0   0 
   
rc3 = R 3 . rc3 =  sin(1 + 2 ) cos(1 + 2 ) 0  .  0  =  0 
0 3 0 3 3
 0 0 1   d 3   d 3 
   
2 2

 d3 
0 − 0
−d 3 2
0 0  
r2 = d 3 
0  , rc3 =  3 0
0 3 0 3 d
0 0
2 
 0 0  0
0
 0 0 
 
 0 F3,2 = 0 F4,3 − 0 P3
0
 M 3,2 = M 4,3 − r2 F3,2 − rc3 P3
0 0 30 0 3 0
79
  Fx 
0  
 F3,2 =  Fy 

 
 Fz + m3g + m 4g 
= 
  (d 3 + d 4 )Fy 
 M =  −(d + d )F 
0
 3,2  3 4 x

  Mz 
Lực do động cơ sinh ra: U 3 = Fz + m3g + m 4g

+Khâu 2:
cos(1 + 2 ) − sin(1 + 2 ) 0   −a 2   −a 2 cos(1 + 2 ) 

r1 = 0 R 2 . 2 r12 =  sin(1 + 2 ) cos(1 + 2 ) 0  .  0  =  −a 2 sin(1 + 2 ) 
0 2
 0 0 1   0   0 
 −a cos(1 + 2 ) 
 −a 2   2 
cos(1 + 2 ) − sin(1 + 2 ) 0   2   2

     −a 2 sin(1 + 2 ) 
rc2 = R 2 . rc2 =  sin(1 + 2 ) cos(1 + 2 ) 0  .  0  =
0 2 0 2 2
 2 
 0 0 1   0   
   0 
   

 0 0 −a 2 sin(1 + 2 ) 
0 2 
r = 0 0 a 2 cos(1 + 2 ) 
1
a 2 sin(1 + 2 ) −a 2 cos(1 + 2 ) 0 
 −a 2 sin(1 + 2 ) 
 0 0 
2
 
a 2 cos(1 + 2 ) 
rc2 = 
0 2
0 0
 2 
 a sin( +  ) −a 2 cos(1 + 2 ) 
 2 1 2
0 
 2 2 
80
 0 F2,1 = 0 F3,2 − 0 P2
0
 M 2,1 = M 3,2 − r1 F2,1 − rc2 P2
0 0 20 0 2 0
  Fx 
0  
 F2,1 =  Fy 
  Fz + m 4g + m3g + m 2g 


  m 2g 
=   (d 3 + d 4 )Fy + a 2 sin( 1 +  2 )( + m 3 g + m 4 g + Fz ) 
2
  
 0 M =  −(d + d )F − a cos( +  )( m 2g + m g + m g + F ) 
 2,1  3 4 x 2 1 2
2
3 4 z

  M z − a 2 sin(1 + 2 )Fx + a 2 cos(1 + 2 )Fy 
  

 
 
Momen do động cơ sinh ra: U 2 = M z − a 2 sin(1 + 2 )Fx + a 2 cos(1 + 2 )Fy
+ Khâu 1:
cos 1 − sin 1 0   −a1   −a1 cos 1 

r0 = 0 R1.1 r01 =  sin 1 cos 1 0  .  0  =  −a1 sin 1 
0 1
 0 0 1   0   0 
 −a cos 1 
 −a1   1 
cos 1 − sin 1 0  2   2

   −a1 sin 1 
rc1 = 0 R 1.1 rc11 =  sin 1 cos 1
0 1 
0 .  0  =
 2 
 0 0 1   0   
   0 
   

 −a1 sin 1 
 0 0
2 
 0 0 −a1 sin 1   
  a1 cos 1 
r = 0 a1 cos 1  , rc1 =  0
0 1 0 1
0 0
0
 2 
a1 sin 1 −a1 cos 1 0   a sin  
 1 1 −a1 cos 1 
0
 2 2 
81
 0 F1,0 = 0 F2,1 − 0 P1
0
 M1,0 = M 2,1 − r0 F1,0 − rc1 P1
0 0 10 0 1 0
  Fx 
0  
 F1,0 =  Fy 
  Fz + m 4g + m3g + m 2g + m1g 

  m 2g m1g 
=   (d 3 + d 4 )Fy + a 2 sin(1 + 2 )( 2 + m 3g + m 4g + Fz ) + a1 sin 1 ( 2 + m 2g + m 3g + m 4g + Fz ) 
  
 0 M =  −(d + d )F − a cos( +  )( m 2g + m g + m g + F ) − a cos  ( m1g + m g + m g + m g + F ) 
 1,0  3 4 x 2 1 2
2
3 4 z 1 1
2
2 3 4 z

  M z − a 2 sin(1 + 2 )Fx + a 2 cos(1 + 2 )Fy − a1 sin 1Fx + a1 cos 1Fy 
  
  
Momen do động cơ sinh ra

U1 = M z − a 2 sin(1 + 2 )Fx + a 2 cos(1 + 2 )Fy − a1 sin 1Fx + a1 cos 1Fy
4.3. Tính toán lực và momen lớn nhất ở trạng thái tĩnh
Ta tiến hành thay các thông số về khối lượng, lực, momen và kích thước các khâu
gồm:
m1 = 10(kg ), m2 = 12(kg ), m3 = 2(kg ), m4 = 0.5(kg )
0
F5,4 = [0 0 −0.5]T ( N )
0
M 5,4 = [0 0 0]T ( N .m)
a1 = 0.35(m), a2 = 0.35(m), d1 = 0.6(m), d 2 = 0.1(m), d 3max = 0.4(m), d 4 = 0.05(m)
Kết quả thu được là:
+ Khâu 4
  0 
0  
 F4,3 =  0  (N)
  −29.4 

 0
 0 M = 0  (N.m)
 4,3  
 0 
Momen do động cơ sinh ra: U 4 = 0 (N.m)
82
+ Khâu 3
  0 
0  
 F3,2 =  0  (N)
  29.4 

 0
 M =  0  (N.m)
0
 3,2  
  0 
Lực do động cơ sinh ra: U3 = 29.4 (N)
+ Khâu 2
  0 
0  
 F2,1 =  0  (N)
 147.0 

  30.87sin(1 + 2 ) 
 0 M =  −30.87 cos( +  )  (N.m)
 2,1  1 2 
 
 0 
Momen do động cơ sinh ra: U 2 = 0 (N.m)
+ Khâu 1
  0 
0  
 F1,0 =  0 
  245.0 

  30.87sin(1 + 2 ) + 68.6sin 1 
 0 M =  −30.87 cos( +  ) − 68.6cos  
 1,0  1 2 1
 
 0 
Momen do động cơ sinh ra:
U1 = 0 (N.m)
83
5. TÍNH TOÁN ĐỘNG LỰC HỌC
5.1 Thiết lập phương trình vi phân chuyển động của robot bằng các phương pháp đã học.
5.1.1. Tham số động lực học
Hình 51. Thiết lập hệ tọa độ khối tâm

Ta có bảng tham số động lực học của robot như sau:
Vị trí trọng tâm so

với hệ tọa độ gắn Momen quán tính khối và momen tích quán
Khối
trên khâu i tính của khâu i đối với các trục xci, yci, zci
lượng
Khâu
mi
xci yci zci J xx J yy J zz J xy J xz J yz
1 -l1 0 0 m1 J1xx J1yy J1zz J1xy J1xz J1yz

2 -l2 0 0 m2 J2xx J2yy J2zz J2xy J2xz J2yz
3 0 0 l3 m3 J3xx J3yy J3zz J3xy J3xz J3yz
84
4 0 0 l4 m4 J4xx J4yy J4zz J4xy J4xz J4yz
Trong đó với vị trí khối tâm được thiết lập ta xác định được:
Khối lượng các khâu (kg) Thông số động học (m)

m1 m2 m3 m4 a1 a2 d1 d2 d3 d4
10 12 2 0.5 0.35 0.35 0.7 0.05 0.35 0.05
5.1.2. Thiết lập phương trình vi phân chuyển động

Phương trình Lagrange loại 2 áp dụng cho các cơ hệ:
d  T  T 
 − =− + Qi + U i với i = i..n
dt  qi  qi qi
Trong đó:
T là động năng của cả hệ
Π là thế năng của cả hệ
qi là tọa độ suy rông thứ i
Qi là lực suy rộng các lực không thế
ứng với tọa độ suy rộng qi
Trong tính toán thiết kế robot người ta thường sử dụng dạng ma trận của phương
trình Lagrange loại 2 để thuận lợi trong sử dụng các công cụ toán học và tiến hành mô
phỏng trên máy tính, nó có dạng:
M ( q ) q + C ( q, q )q + G (q ) + Q = U
Trong đó:
M là ma trận khối lượng
C là ma trận corilois
Q là vecto lực suy rộng của các lực không thế.
U là vecto lực suy rộng ứng với tọa độ khớp qi
của các lực dẫn động τi của động cơ đặt tại các khớp: U = [ 1 ,  2 ,  3 ,  4 ]
T
85
Với khớp tịnh tiến thì τi là lực Fmi
Với khớp quay thì τi là ngẫu lực có momen Mmi
Ở nội dung báo cáo này, nhóm thiết lập phương trình vi phân chuyển động cho robot
dạng ma trận.
5.1.3 Ma trận khối lượng
Vị trí và hướng của hệ tọa độ Cixciycizci gắn vào khâu thứ i tại khối tâm Ci , so với
R rci (3 x1) 
hệ tọa độ cơ sở được xác định bởi: 0 Aci = 0 Ai i ci =  i (3Tx 3)
 0 1 
Dựa trên hệ tọa độ khối tâm đã thiết lập ta xác định được:
1 0 0 −l1  1 0 0 −l 2 
0 1 0 0  0 1 0 0 
1
AC1 =  ; 2
AC 2 = 
0 0 1 0 0 0 1 0 
   
0 0 0 1 0 0 0 1 
1 0 0 0 1 0 0 0
0 
1 0 0 4 0 1 0 0 
3
AC 3 =  ; AC 4 =  ;
0 0 1 l3  0 0 1 l4 
   
0 0 0 1 0 0 0 1
c1 − s1 0 (a1 − l1 )c1

s c1 0 (a1 − l1 )s1
0 AC1 = A1 AC1 =  1
0 1
;
0 0 1 d1 
 
0 0 0 1 
c 12 − s12 0 a1c1 + (a2 − l2 )c12 
s c12 0 a1 s1 + (a2 − l2 )s12 
0
AC 2 = 0 A2 2 AC 2 =  12 ;
0 0 1 d 2 + d1 
 
0 0 0 1 
c 12 − s12 0 a1c1 + a2 c12 
s c12 0 a1 s1 + a2 s12 
0
AC 3 = A3 AC 3 =  12
0 3
;
0 0 1 l3 + d3 + d 2 + d1 
 
0 0 0 1 
86
c 124 − s124 0 a1c1 + a2 c12 
s c124 0 a1 s1 + a2 s12 
0
AC 4 = A4 AC 4  124
0 4
;
 0 0 1 l4 + d3 + d 2 + d1 
 
 0 0 0 1 
Xét khâu 1:
+ Ma trận ten xơ quán tính của khâu đối với khối tâm C1 trong hệ tọa độ động
 J c1xx J c1xy J c1xz 
 
: c1 =  J c1xy
1
C1 xC1 yC1 zC1 J c1 yy J c1 yz 
 J c1xz J c1 yz J c1zz 

+ Bán kính định vị khối tâm: o rc1 = ( −l1 + a1 ) c1 , ( −l1 + a1 ) s1 ,

T
d1 
− ( −l1 + a1 ) s1q1 
+ Vận tốc khối tâm: vc1 = o rc1 =  ( −l1 + a1 ) c1q1 
 0 
+ Xác định ma trận Jacobian tịnh tiến JT 1 :
 −(−l1 + a1 ) s1 0 0 0   −(−l1 + a1 ) s1 (−l1 + a1 )c1 0 0 

JT 1  
=  (−l1 + a1 )c1 0 0 0  = J T 1 = 
T
0 0 0 0 
 0 0 0 0   0 0 0 0 
+ Xác định ma trận Jacobian quay J R1 :
c1 − s1 0 

Từ o Ac1 => Ma trận quay: Rc1 =  s1 o
c1 0 
 0 0 1 
 c1 s1 0   −q1s1 −q1c1 0 
 
=> R =  − s1 c1 0  ;
o T o
Rc1  q1c1 −q1s1 0 
c1
 0 0 1   0 0 0 
 c1 s1 0  −q1s1 −q1c1 0  0 −q1 0

 = R R c1=  − s1 c1 0  q1c1 −q1s1 0 =  q1 0 0
r
1
o T o
c1
 0 0 1   0 0 0   0 0 0 
87
 1R =  0 0 q1 
T
=> ma trận Jacobian quay J R1
0 0 1
0 0 0 0  0 0 0 
J R1 = 0 0 0 0  = J RT1 = 
0 0 0
1 0 0 0   
0 0 0
Xét khâu 2:
 J c 2 xx J c 2 xy J c 2 xz 
 
: c 2 =  J c 2 xy
2
C2 xC 2 yC 2 zC 2 J c 2 yy J c 2 yz 
 J c 2 xz J c 2 yz J c 2 zz 

 a1c1 + (−l2 + a2 )c12 
 
+ Bán kính định vị khối tâm: rc 2 =  a1 s1 + (−l2 + a2 ) s12 
o
 d 2 + d1 
−a1 s1q1 − ( −l2 + a2 ) s12 (q1 + q2 ) 

+ Vận tốc khối tâm: vc 2 = rc 2 =  a1c1q1 + ( −l2 + a2 ) c12 (q1 + q2 ) 
o
 0 
− ( −l2 + a2 ) s12 − a1 s1 − ( −l2 + a2 ) s12 0 0

 
JT 2 =  ( −l2 + a2 ) c12 + a1c1 ( −l2 + a2 ) c12 0 0
 0 0 0 0 
− ( −l2 + a2 ) s12 − a1 s1 ( −l2 + a2 ) c12 + a1c1 0
 
− ( −l2 + a2 ) s12 ( −l2 + a2 ) c12 0
= J TT 2 =
 0 0 0
 
 0 0 0
88
c12 − s12 0 

Từ o Ac2 => Ma trận quay: Rc 2 =  s12 o
c12 0 
 0 0 1 
 c12 s12 0   −(q1 + q2 ) s12 −(q1 + q2 )c12 0

 
=> R =  − s12 c12 0  ;
o T o
Rc 2 =  (q1 + q2 )c12 −(q1 + q2 ) s12 0 
c2
 0 0 1   0 0 0 
c12 − s12 0  −(q1 + q2 ) s12 −(q1 + q2 )c12 0   0 −q1 − q2 0 

 = R
r
2
o T o
c2 R c 2 =  s12 c12 0  (q1 + q2 )c12 −(q1 + q2 ) s12 0  =  q1 + q2 0 0 
 0 0 1   0 0 0   0 0 0 
 2R =  0 0 q1 + q2 
T
0 0 1
0 0 0 0 0 0 1 
J R2 =  0 0 0 0  = J RT 2 = 
0 0 0
1 1 0 0   
0 0 0
Xét khâu 3:
+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
 J c 3 xx J c 3 xy J c 3 xz 
 
: c 3 =  J c 3 xy
3
C3 xC 3 yC 3 zC 3 J c 3 yy J c 3 yz 
 J c 3 xz J c 3 yz J c 3 zz 

 a1c1 + a2 c12 
+ Bán kính định vị khối tâm: o
rc 3 =  a1 s1 + a2 s12 
l3 + d3 + d 2 + d1 
 −a1s1q1 − a2 s12 (q1 + q2 ) 


+ Vận tốc khối tâm: vc 3 = o rc 3 =  a1c1q1 + a2c12 (q1 + q2 ) 

 −q3 
89
 − a2 s12 − a1s1 a2c12 + a1c1 0
 − a2 s12 − a1s1 − a2 s12 0 0  −a s 0 
=  a2c12 + a1c1 0 0  = J TT3 =  2 12 a2c12
JT 3 a2c12
 0 0 1
 0 0 1 0   
 0 0 0
c12 − s12 0

+ Từ o Ac3 => Ma trận quay: o Rc 3 =  s12 c12 0 
 0 0 1 
 c12 s12 0  −(q1 + q2 ) s12 (q1 + q2 ) c12 0


=> o RcT3 =  − s12 c12 0  ; o
Rc 3 =  (q1 + q2 ) c12 −(q1 + q2 ) s12 0 
 0 0 1   0 0 0 
c12 − s12 0  −(q1 + q2 ) s12 (q1 + q2 )c12 0  0 −q1 − q2 0

 = R
r
3
o T o
c3 R c 3=  s12 c12 0  (q1 + q2 )c12 −(q1 + q2 ) s12 0  =  q1 + q2 0 0 
 0 0 1   0 0 0   0 0 0 
 3R =  0 0 q1 + q2 
T
0 0 1
0 0 0 0 0 0 1 
J R3 = 0 0 0 0  = J RT 3 = 
0 0 0
1 1 0 0   
0 0 0
Xét khâu 4:
 J 4 xx J 4 xy J 4 xz 
 
C4 xC 4 yC 4 zC 4 :
4
C 4 =  J 4 xy J 4 yy J 4 yz 
 J 4 xz J 4 yz J 4 zz 

Bán kính định vị khối tâm:
 a1c1 + a2 c12 
o
rc 4 =  a1 s1 + a2 s12 
l4 + d3 + d 2 + d1 
90
 −a1s1q1 − a2 s12 (q1 + q2 ) 

Vận tốc khối tâm: vc 4 = rc 4 =  a1c1q1 + a2c12 (q1 + q2 ) 
o 
 q3 

 − a2 s12 − a1s1 a2c12 + a1c1 0 
 − a2 s12 − a1s1 −a2 s12 0 0  −a s 0 
=  a2c12 + a1c1  a2c12
JT 4 a2c12 0 0  = J T 4 = 
T 2 12
 0 0 1
 0 0 1 0   
 0 0 0
c124 − s124 0 

Từ o Ac4 => Ma trận quay: o Rc 4 =  s124 c124 0 
 0 0 1 
 c124 s124 0   −(q1 + q2 − q 4 ) s124 −(q1 + q2 − q 4 )c124 0 

  
=> o RcT4 =  − s124 c124 0  ; o Rc 4 =  (q1 + q2 − q 4 )c124 −(q1 + q2 − q 4 ) s124 0 
 0 0 1   0 0 0 
c124 − s124 0  −(q1 + q2 − q 4 ) s124 −(q1 + q2 − q 4 )c124 0

 = R
r
4
o T o
c4 R c 4 =  s124 c124 0  (q1 + q2 − q 4 )c124 −(q1 + q2 − q 4 ) s124 0 
 0 0 1   0 0 0 
 0 −q1 − q2 − q 4 0 

=  =  q1 + q2 + q 4
r
0 0
4
 0 0 0 
 4R =  0 0 q1 + q2 + q4 
T
91
0 0 1
0 0 0 0 0 0 1 
J R4 =  0 0 0 0  = J RT 4 = 
0 0 0
1 1 0 1   
0 0 1
Trong đó M(q) là ma trận khối lượng (Ma trận đối xứng) được xác định bởi:
 m11 (q) m12 (q) m13 (q) m14 (q) 
4  m (q) m (q) m (q) m (q) 
M ( q ) =  ( J Ti mi J Ti + J Ri i J Ri ) =  21
T T 22 23 24 
i =1  m31 (q) m32 (q) m33 (q) m34 (q) 
 
 m41 (q) m42 (q) m43 (q) m44 (q) 
 m11 (q) m12 (q) m13 (q) m14 (q) 

4  m (q) m (q) m (q) m (q) 
M (q ) =  ( J Ti mi J Ti + J Ri Ai I Ci Ai J Ri ) =  21
T T 0 ( i ) 0T 22 23 24 
i =1  m31 (q) m32 (q) m33 (q) m34 (q) 
 
 m41 (q) m42 (q) m43 (q) m44 (q) 
Kết quả tính:

m11 = J 1zz + J 2 zz + J 3zz + J 4 zz + m1 (−2a1l1 + l12 + a12 ) + (m3 + m 4 )(2 a1a 2c 2 + a12 + a 2 2 )
+ m 2 (2 a1a 2c 2 − 2 a1 l2 c 2 + a 2 2 + a12 + l2 2 − 2a2 l 2 )
m22 = J 2 zz + J 3zz + J 4 zz + m2 (−2 a 2 l2 + a 2 2 + l2 2 ) + (m3 + m 4 ) a 2 2
m33 = m3 + m4
m44 = J 4 zz
m12 = m21 = J 2 zz + J 3 zz + J 4 zz + m2 (−2a 2 l2 + l2 2 + a 2 2 − l2 a1c 2 + a1a 2c 2) + ( m3 + m 4 ) (a 2 2 + a1a 2 c 2)

m13 = m31 = m23 = m32 = m34 = m43 = 0
m14 = m41 = m24 = m42 = J 4 zz
5.1.4. Ma trận Corilois

Ma trận đặc trưng cho lực quán tính Coriolis và lực quán tính li tâm C (q, q) xác
định bởi:
 c11 (q) c12 (q) c14 (q) 
c13 (q)
c (q) c22 (q) c23 (q) c24 (q) 
C(q,q) =  21
 c31 (q) c32 (q) c33 (q) c34 (q) 
 
c41 (q) c42 (q) c43 (q) c44 (q) 
92
Các phần tử của ma trận này được xác định theo công thức:
1 4 mij mik m jk
cij (q,q) =  ( + − )q k
2 k =1 qk q j qi
Từ đó tính được:
c11 = −[m 2 a1 ( a2 -l 2 )s2+ m 3a1a2s2+ m 4 a1a2s2] q2
c12 = −[m 2 a1 ( a2 -l 2 )s2+ m 3a2 a1s2+ m 4 a2 a1s2 q1 − [m 2 a1 ( a2 -l 2 )s2+ m 3 a2 a1s2+ m 4 a2 a1s2]q2
c13 = c14 = 0
c 21 = [m 2 a1 ( a2 -l2 ) s2+ m3a2 a1s2+ m 4 a2 a1s2 q1
c 22 = c 23 = c 24 = 0
c31 = c32 = c33 = c34 = 0
c 41 = c 42 = c 43 = c 44 = 0
5.1.5. Thế năng

Chọn gốc thế năng trùng với hệ tọa độ cơ sở:
Khi đó thế năng của robot được xác định:
4
 =  mi o zci g =gm1d1 + gm2 (d1 + d 2 ) + gm3 ( l3 + d1 + d 2 + d3 ) + gm4 (l4 + d1 + d 2 + d3 + d 4 )
i =1
T
  
Vector lực do trọng lực: G (q) =   = (0, 0, − ( m3 + m4 ) g, 0)
T
 qi 
5.1.6. Lực suy rộng của các lực không thế
Trong nội dung đồ án, Robot được coi là mô hình lí tưởng, do đó bỏ qua lực ma sát,
lực cản nhớt. Lực không thế ở đây chỉ bao gồm lực do vật tác động lên khâu thao tác cuối.
Xét trường hợp tổng quát với lực và momen có giá trị:
F 4 = [Fx, Fy, Fz ];
M 4 = [Mx, My, Mz ]
 Q1 
Q 
Q =  2;
 Q3 
 
Q4 
Lực suy rộng được được xác định theo công thức:
93
n
Qi =  (JTj
Ti
Fj + JTiRj M j )
j
Với là trị số ứng với hàng i của ma trận Jacobi JTjT

n
Q1 =  (JTTj1 Fj + JTRj1 M j ) = JTT 14 F4 + JTR14 M 4
j =1
=> Q1 = ( −a2 sin(1 +  2 ) − a1 sin(1 ) ) Fx + ( a2 cos(1 +  2 ) + a1 cos(1 ) ) Fy − M z

n
Q2 =  (JTTj2 Fj + JTRj2 M j ) = JTT 32 F3 + JTR 23 M 3
j =1
=> Q2 := −a2 sin(1 +  2 ) Fx + a2 cos(1 +  2 ) Fy + M z

n
Q3 =  (JTTj3 Fj + JTRj3 M j ) = JTT 33 F3 + JTR33 M 3
j =1
=> Q3 = Fz
n
Q4 =  (JTTj4 Fj + JTRj4 M j ) = JTT 44 F4 + JTR 44 M 4
j =1
=> Q4 = Mz
Tùy vào yêu cầu trong thực tế mà các giá trị F4, M4 được xác định cụ thể và áp
dụng vào điều khiển robot. Ở đây Robot ứng dụng trong lắp ráp mạch điện tử cho điện
thoại , bỏ qua khối lượng của mạch (khối lượng không đáng kể). Do đó Q = [0,0,0,0]T
5.2 Tính động lực học ngược theo quy luật chuyển động được khảo sát ở câu 3.c.
Động lực học thuận: là 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. Cụ thể ở đây là tính toán các đại lượng đặc trưng về vị
trí và hướng của khâu thao tác cùng với đạo hàm của chúng (vận tốc, gia tốc dài, vận tốc
góc, gia tốc góc).
M (q)q + C (q, q)q + G (q) + Q = U
Động lực học ngược: 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.
U = M (q)q + C (q, q)q + G(q) + Q
94
Sử dụng phần mềm Matlab để triển khai simulink và thu được kết quả như sau:
Hình 52. Khối động lực học ngược

Với đầu vào u là: biến khớp q , vận tốc biến khớp dq, và gia tốc biến khớp ddq.
Model inverse_dynamics:
function T = InverseModel(u)
% cac tham so dong hoc, dong luc hoc
m1 = 10; m2 = 12; m3=2; m4 = 0.5;
a1 = 0.35; a2= 0.35; d1 = 0.7; d2 = 0.05; d4=0.05;
g = 9.81;
95
l1 = a1/2; l2 = a2/2;
J11zz = a1^2*m1/12; J22zz = a2^2*m2/12 ;J33zz = 0;J44zz =
d4^2*m4/12;
% Fx=20; Fy=20; Mz=10;F3=40;
% vector toa do suy rong (toa do khop)

q = u(1:4);
% vector dao ham cac toa do suy rong (toa do khop)
dq = u(5:8);
% vector gia t?c
ddq = u(9:12);
% vector luc suy rong ung voi cac toa do suy rong (toa do
khop)
% u = u(7:9);
% Ma tran khoi luong

m11=J11zz + J22zz + J33zz + J44zz + m1*(-2*a1*l1 + l1^2 +
a1^2) + (m3+m4)*(2*a1*a2*cos(q(2))+ a1^2 +a2^2) + m2
*(2*a1*a2*cos(q(2))- 2*a1*l2*cos(q(2))+ a1^2 +a2^2 + l2^2 -
2*a2*l2);
m12 = J22zz + J33zz + J44zz + m2*(-2*a2*l2+l2^2 +a2^2 -
l2*a1*cos(q(2))+a2*a1*cos(q(2)))+(m3+m4)*(a1*a2*cos(q(2))+a
2^2);
m21 = J22zz + J33zz + J44zz + m2*(-2*a2*l2+l2^2 +a2^2 -
l2*a1*cos(q(2))+a2*a1*cos(q(2)))+(m3+m4)*(a1*a2*cos(q(2))+a
2^2);
m22 = J22zz + J33zz + J44zz + m2*(-2*a2*l2 + l2^2 + a2^2)+
(m3 +m4)*a2^2;
m33=m3+m4;
m44 = J44zz;
m13 =0; m31 =0; m23 =0; m32 =0; m34 =0; m43 =0;
m14 =J44zz; m41=J44zz; m24=J44zz; m42=J44zz;
M =
[m11,m12,m13,m14;m21,m22,m23,m24;m31,m32,m33,m34;m41,m42,m4
3,m44];
% Luc qun tinh coriolis va qun tinh li tam

% psi1 = -(a1*a2*dq(2)*sin(q(2))*(2*dq(1) + dq(2))*(m2 +
2*m3))/2;
% psi2 = (a1*a2*dq(1)^2*sin(q(2))*(m2 + 2*m3))/2;
% psi3 = 0;
96
% Psi=[psi1;psi2;psi3];
c11 = -(m2*a1*(a2-l2)*sin(q(2))+m3*a1*a2*sin(q(2))+
m4*a1*a2*sin(q(2)))*dq(2);
c12 = -(m2*a1*(a2-l2)*sin(q(2))+m3*a1*a2*sin(q(2))+
m4*a1*a2*sin(q(2)))*dq(1)-(m2*a1*(a2-
l2)*sin(q(2))+m3*a1*a2*sin(q(2))+
m4*a1*a2*sin(q(2)))*dq(2);
c21 = (m2*a1*(a2-l2)*sin(q(2))+m3*a1*a2*sin(q(2))+
m4*a1*a2*sin(q(2)))*dq(1);
c13 =0; c14 =0;
c23 =0; c24 =0; c22 =0;
c31 =0; c32 =0; c33 =0; c34 =0;
c41 =0; c42 =0; c43 =0; c44 =0;
C =
[c11,c12,c13,c14;c21,c22,c23,c24;c31,c32,c33,c34;c41,c42,c4
3,c44];
% Luc suy rong cua cac luc co the

g1=0;
g2=0;
g3=-(m3+m4)*g;
g4=0;
G = [g1; g2; g3; g4];
% Luc suy rong cua cac luc khong the

%Q1=0.2*dq(1);
%Q2=0.2*dq(2);
Q1=0; Q2=0; Q3=0; Q4=0;
%Q1=0;
%Q2=0;
Q=[Q1;Q2;Q3;Q4];
% Luc dan dong tai cac khop

% T = M*[u(1);u(2);u(3);u(4)]+C+G+Q;
T = M*ddq+C*dq+G+Q;
97
Kết quả:
98
99
6. THIẾT KẾ HỆ DẪN ĐỘNG ROBOT
Từ kết quả của bài toán thiết kế quỹ đạo phù hợp cho robot và bài toán động
lực học ngược ta thu được kết quả:
Thông số vận tốc Giá trị
 1max 5.2 (rad/s)
 2 max 2.8 (rad/s)
v 3max 0.75 (m/s)
 4 max 9 (rad/s)
Giá trị lực Giá trị

U1 62 (N.m)
U2 25 (N.m)
U3 43 (N)
U4 2.5(N.m)
Giới thiệu một số hệ dẫn động hay dùng trong robot công nghiệp:
Có rất nhiều loại hộp giảm tốc được sử dụng trong thiết kế robot công nghiệp, phụ
thuộc vào yêu cầu kỹ thuật, mục đích ứng dụng của robot ta có thể chọn được một hộp
giảm tốc phù hợp. Dưới đây là các hộp giảm tốc thông dụng trong thiết kế robot và đặc
điểm ứng dụng cơ bản của chúng.
• Hộp giảm tốc bánh răng trụ
100
Hình 53. Hộp giảm tốc bánh răng trụ
Hộp giảm tốc bánh răng trụ có hiệu suất truyền không cao, có tỉ số kích thước trên
tỉ số truyền lớn, độ chính xác không cao, tỉ số truyền thấp, trong các robot có yêu cầu kết
cấu nhỏ, độ chính xác cao thường không sử dụng. Tuy nhiên với ưu điểm kết cấu cơ khí
đơn giản, giá thành rẻ và phổ biến trên thị trường có khả năng làm việc với phạm vi vận
tốc và tải trọng rộng nên bộ truyền bánh răng trụ vẫn được dùng phổ biến cho các tay máy
công nghiệp.
• Hộp giảm tốc bánh răng hành tinh
Hình 54. Bộ truyền hành tinh
101
Bộ truyền hành tinh bao gồm các bánh răng trung tâm, cần mang trục, các bánh vệ
tinh. Bộ truyền hành tinh có nhiều ưu điểm như kết cấu đơn giản, nhỏ gọn, độ chính xác
cao, hiệu suất cao, quán tính nhỏ, tùy vào kết cấu mà có dải tỷ số truyền từ một tới hàng
trăm lần.
• Bộ truyền Harmonic
Hình 55. Bộ truyền bánh răng sóng

Bộ truyền bánh răng sóng có nhiều ưu điểm như độ chính xác cơ khí lặp lại cao
momen xoắn truyền lớn, hệ số giảm tốc cao từ 50: 1 cho tới 320: 1, kết cấu đồng trục. Loại
bộ truyền này có kết cấu phức tạp, khó chế tạo nên giá thành cao tuy nhiên vẫn được ứng
dụng rộng.
• Bộ truyền bánh răng con lăn (Bộ truyền với ăn khớp Cycloid)
Hình 56. Bộ truyền bánh răng chốt Cycloid
102
Bộ truyền với ăn khớp Cycloid có nhiều ưu điểm như phù hợp với các bài toán đặt
ra cho các hệ dẫn động có kích thước nhỏ, tỷ số truyền lớn (có thể từ 6 đến 65), kích thước
nhỏ gọn… Tuy nhiên việc ứng dụng bộ truyền bánh răng này vào thực tế còn nhiều hạn
chế do sự phức tạp trong việc xây dựng biên dạng Cycloid và hiệu suất của bộ truyền chưa
cao do chưa khắc phục được ma sát trượt hình thành trong quá trình làm việc
• Truyền động thanh răng bánh răng
Hình 57. Bộ truyền thanh răng bánh răng

Bộ truyền này có ưu điểm: đảm bảo độ chính xác truyền động vì không có sự trượt,
tỉ số truyền ổn định, có thể sắp đặt vị trí tương đối theo ý muốn linh hoạt, kích thước nhỏ
gọn, làm việc trong phạm vi công suất, tốc độ và tỉ số truyền khá rộng. Tuy nhiên bộ truyền
này có nhược điểm: không thực hiện được tỉ số truyền vô cấp, không có khả năng tự bảo
vệ an toàn khi quá tải, có nhiều tiếng ồn khi vận tốc lớn.
103
• Truyền động vít me đai ốc bi
Hình 58. Bộ truyền vít me đai ốc bi

Vít me – đai ốc bi: là một hệ thống chuyền động, được gia công chính xác để biến
đổi chuyển động quay thành chuyển động tịnh tiến theo cơ chế bu lông – đai ốc. Khu vực
tiếp xúc giữa trục vít và đai ốc là một dải bi bằng thép để biến ma sát trượt thành ma sát
lăn, qua đó giúp chuyền động một cách trơn tru và chính xác hơn, hoạt động liên tục bền
bỉ trong thời gian dài.
Hoạt động: Khu vực tiếp xúc giữa vít me bi và đai ốc có 1 đường rãnh (rãnh me)
được lấp đầy bởi những viên bi thép. Khi trục vít xoay, những viên bi lăn tròn trong mối
ren của trục vít và đai ốc. Điều này nhằm giảm ma sát của chúng. Bởi vì các viên bi cuối
cùng sẻ rơi ra ngoài, nên đai óc có 1 đường ống dẫn về (đường hồi bi) để hứng những viên
bi không bị rơi khỏi rãnh của trục vít và đưa chúng trở lại phần đầu của đường bi ở phía
cuối của đai óc. Lực đẩy của đai ốc nhẹ nhàng nhờ chuyển động lăn của những viên bi
cuộn tròn.
6.1 Lựa chọn động cơ cho khâu 1, khâu 2 và khâu 4
Khâu 1, khâu 2 chuyển động quay: chọn dẫn động là hộp giảm tốc.
Chọn giải pháp bố trí động cơ nằm ngang. Riêng khâu 4 chỉnh dùng để định hướng
cho mạch , yêu cầu về momen nhỏ nên bỏ qua việc sử dụng hộp giảm tốc.
 Để thuân cho việc truyền động tại các khớp quay động cơ bố trí nằm ngang mà
trục của khớp theo phương thẳng đứng => chọn bộ truyền bánh răng côn.
104
 1max  5.2(rad/ s)

Vận tốc góc lớn nhất của khâu 1, khâu 2 và khâu 4:  2 max  2.8(rad/ s)

 4 max  9(rad/ s)
 Tốc độ quay lớn nhất của khớp:
 30 30
 n1max =   1max =   5.2 = 49.66(vg / ph)

 30 30
 n2 max =  2 max =  2.8 = 26.74(vg / ph)
  
 30 30
 n4 max =   4 max =   9 = 85.94(vg / ph)

 Chọn tốc độ quay của khớp: n1 = 50 vg/ph, n2 = 30 vg/ph, n4 = 90 vg/ph
Do kết cấu của robot lựa chọn hộp giảm tốc cycloid cho khớp 1 và khớp 2 tỉ số
truyền u = 10.Riêng khâu 4 do chỉ thực hiện công việc định hướng cho mạch nên yêu cầu
về momen nhỏ , có thể bỏ qua hộp giảm tốc
 M 1max = 62 N .m

Momen lớn nhất theo quỹ đạo thiết kế:  M 2 max = 25 N .m
M
 4 max = 0.0025 N .m
 P1max = M 1max .1max = 322.4W


Công suất tại khớp:  P2 max = M 2 max .2 max = 70W
 P = M .
 4 max 4 max 4 max = 0.0225W
Hiệu suất của bộ truyền:  = br .ol . kn

2
Trong đó: Chọn br = 0.95 - hiệu suất bộ giảm tốc cycloid
ol = 0.99 - hiệu suất một cặp ổ lăn (0.99 – 0.995)
 kn = 1 - hiệu suất của khớp nối
=  = br .ol2 .kn = 0.95  0.992  1 = 0.93
105
P1max 322.4
Công suất yêu cầu cho khâu 1: Pyc1 = = = 347W
 0.93
P2 max 70
Công suất yêu cầu cho khâu 2: Pyc 2 = = = 76W
 0.93
Công suất yêu cầu cho khâu 4: Pyc 4 = P4 max =0.0225 W
n yc1 = u.n1 = 50.10 = 500vg / ph


Số vòng quay yêu cầu: n yc 2 = u.n2 = 30.10 = 300vg / ph

n yc 4 = n4 = 90vg / ph
Lựa chọn động cơ thỏa mãn:
 Pdc  Pyc


 ndc  n yc

6.2 Thiết kế hệ dẫn động cho khâu 3

• Lựa chọn vật liệu và profin ren:
Vít, bi: Thép 40CrMn (𝜎𝑐ℎ = 785MPa)
Thép hợp kim kết cấu 40X được sử dụng trong các bộ phận nặng (trục, trục bánh
răng, pít tông, thanh, trục khuỷu và trục cam, vòng, trục chính, chân trôi, bạt, bánh răng,
bu lông, nửa trục, ống lót, ốc vít, v.v.). Thép crom 40X cũng được sử dụng trong các thành
phần rèn, lạnh và dập nóng, và sản phẩm thép được sử dụng trong đường ống, bể chứa và
sản xuất khác.
Đai ốc: Thép chịu lực 100Cr6
Thép chịu lực 100Cr6 là một loại thép đặc biệt có tính năng cưỡng lại quá trình cán,
kéo và giảm độ mỏi của thép. Thép carbon và Crom cao, thép chế tạo cơ khí và một vài
loại thép không gỉ cưỡng lại sự ăn mòn kim loại, đươc sử dụng cho chế tạo vật liệu chịu
lực.
106
• Sơ đồ truyền động:
Hình 59. Sơ đồ truyền động trục vit me-đai ốc

Hành trình: 400mm.
• Tính toán sơ bộ đường kính ren trong d1 của ren theo độ bền kéo
4 1.3  Fa
d1 
   K 
Trong đó: Fa là lực dọc trục d1 là đường kính trong của vít me (mm)
𝜎𝐾 = 980 (𝑀𝑃𝑎)
4.1,3.43
Thay các giá trị vào ta được giá trị của d1 
.980
 d1 ≥ 0.27 𝑚𝑚
Chọn d1= 23 (mm) => d0 = 25 (mm) theo tiêu chuẩn của SKF về vít me đai ốc bi
loại rotating nut.
107
Hình 60. Vít me đai ốc bi loại rotating nut
• Tính các thông số của bộ truyền
Hình 61. Các thông số bộ truyền động vít bi

Đường kính bi: db= (0.08÷0.15) d1=0,1.23 =2,3(mm)
Chọn db = 3 (mm)
Bước vít p = db + (1÷5) =3+2 =5 (mm)
Chọn p= 25 (mm)
Bán kính rãnh lăn: r1 = (0,51÷0,53) db = 0,51.3 = 1,53 (mm)
Khoảng cách từ tâm rãnh đến tâm bi
108
 d   3
c =  r1 − b  cos = 1,62 −  cos45o = 0.08 (mm)
 2  2
Trong đó:  là góc tiếp xúc (45o)
Đường kính vòng tròn qua các tâm bi
Dtb=d1+ 2. (r1-c) = 26,08 (mm)
Đường kính trong của ren đai ốc
D1= Dtb + 2(r1-c) = 29,16 (mm)
Chiều cao làm việc của ren h1
h1= (0.3÷0.35) db= 0,32.3=0.96 (mm)
Chọn h1= 1 (mm)
Đường kính ngoài của vít d và của đai ốc D
d= d1+ 2.h1= 25 (mm)
D =D1- 2.h1=27,16 (mm)
Góc nâng vít γ:
p 25
 =arctg( ) = arctg( ) = 16,97o
.Dtb .26,08
Số bi trong các vòng ren làm việc:
.Dtb .K .26,08.2,3
Zb = −1 = − 1 = 61.82  62 viên
db 3
Với số vòng ren làm việc là K=2,3. Chọn Zb= 62
Số vòng ren làm việc theo chiều cao đai ốc không nên quá 2-2,5 vòng, nếu không
sẽ làm tăng sự phân bố không đều tải trọng cho các vòng ren. Số bi trên các vòng ren làm
việc 𝑍𝑏 = 𝜋𝐷𝑡𝑏 𝐾/𝑑𝑏 − 1 ; 𝑣ớ𝑖 𝐾 − 𝑠ố 𝑣ò𝑛𝑔 𝑟𝑒𝑛 𝑙à𝑚 𝑣𝑖ệ𝑐.
Nếu số bi lớn hơn 65 thì nên giảm bớt bằng cách tăng đường kính bi. (Trích sách
Tính toán thiết kế hệ dẫn động cơ khí tập 1-Trịnh Chất).
109
Khe hở hướng tâm
 = D1 − (2d b + d1) = 29,16 − (2.3 + 23) = 0,16mm
Khe hở tương đối
 0,16
= = = 0,007
d1 23
Góc ma sát lăn thay thế:
2f t 2.0,005
1 = arctg( ) = arctg( ) = 0,035o
d1.sin  23.sin 45o
Hệ số ma sát lăn
f t = 0.005
f t = 0.004...0.006
Hiệu suất lý thuyết biến chuyển động quay thành chuyển động tịnh tiến:
tg tg16,97
= = = 0,99
tg(  + 1) tg(16,97 + 0,035)
Hiệu suất thực tế:
p = 0.9 = 0,891
Từ những thông số trên ta chọn được trục vít me bi với đai ốc xoay theo tiêu chuẩn
của hãng SKF:
• Chọn trục vít me bi theo tiêu chuẩn SKF:
Từ những thông số trên, em chọn vit me bi với trục vit tịnh tiến và đai ốc quay (Ball
screw rotating nut) với đường kính danh nghĩa là 25 mm.
110
Hình 62. Vít me đai ôc bi rotating nut (Ball screw rotating nut)
Hình 63. Bảng thông số theo tiêu chuẩn SKF
Hình 64. Kích thước hình học của trục vít me và đai ốc lựa chọn
111
𝑑0 𝑥𝑃ℎ L 𝐿1 𝐿2 𝐿3 𝐿4 𝐿5 𝐿6 𝐿7 𝐿8 𝐿9
25x20 121 15 12,4 19,9 74 2,9 16,8 12,4 15 15
25x25 126,2 15 12,4 19,9 74 2,9 16,8 12,4 15 15
𝑑0 𝑥𝑃ℎ 𝐷1 𝐷2 𝐷3 𝐷4 𝐷5 𝑔6 𝐷6
25x20 40 72,5 100 133 100 65
25x25 40 72,5 100 133 100 65
𝐷7 𝑅𝑚𝑎𝑥 𝐽1 𝐽2 𝑍1 𝑥𝐻1 𝑍1 𝑥𝐻1 (𝑢𝑑) 𝐻3

48 0,8 116 55 6x∅9 6xM6x20 M6x1
48 0,8 116 55 6x∅9 6xM6x20 M6x1
• Kiểm tra bền
Tính kiểm nghiệm về độ bền
Fa 43
Tải trọng riêng dọc trục: q a = = = 0.096 N
Zbd 2b 62.32.0,8
Với λ = 0,8 – hệ số phân bố tải trọng không đều cho các viên bi.
Từ  = 0,0022 và qa= 1,98 từ đồ thị ta xác định được σmax = 1600 MPa
Hình 65. Đồ thị xác định ứng suất lớn nhất σmax
112
max  max  = 5000MPa  Thỏa mãn độ bền
Yêu cầu: Đối với mặt làm việc của vít và đai ốc đạt HRC 53
Đối với bi đạt HRC 63
• Tính chọn động cơ cho trục vít me

- Khi vận tốc của khâu đạt max thì lực dọc trục lớn nhất lúc đó là
𝐹𝑎 = 43𝑁
- Mô men xoắn cần truyền vào trục vít me:
T3 = Fa .D tb .tan(  + t ) / 2
Với Fa là lực dọc trục vitme, D tb lầ đường kính trung bình của vit me,  là góc
nâng vít, t là góc ma sát lăn thay thế.


 Fa = 43N, D tb = 26,08mm
 p
  = arctg( ) = 16,97
  .D tb
 2f t
t = arctg( ) = 0.035
 d1 .sin 
→ T3  0,168(Nm)
- Tính số vòng quay khâu 3:

60.v
n3 = = 1800(v/ ph)
Z.p
Trong đó v = 750 mm/s, p - bước vít: 25 mm, Z - số mối ren: 1
- Tính công suất động cơ:

T3 .n 3 0,168.1800
P3 = = = 35.54 ( W )
p .9,55 0,891.9,55
113
6.4 Thiết kế 3D và kiểm nghiệm bền các khâu của robot
Hình 66. Kiểm bền các khâu trên phần mềm SolidWorks
114
7. THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN LỰC
7.1 Chọn luật điều khiển phù hợp, thiết kế mô hình điều khiển.
Sau khi nhận được số liệu đầu vào tương ứng với quỹ đạo của khâu tác động cuối
hoặc khớp, hệ thống điều khiển phải điều khiển robot chuyển động theo đúng quỹ đạo đặt
ra.
Hiện nay có nhiều phương pháp điều khiển robot như:
+ Điều khiển tự do và điều khiển có tương tác với đối tượng.
+ Điều khiển phân tán và điều khiển tập trung
+ Điều khiển thích nghi phi tuyến.
+ Điều khiển trượt…
Từ mô hình hệ tay máy xây dựng ta nhận thấy nó là một hệ đa biến và phi tuyến. Ở
báo cáo này nhóm sử dụng phương pháp điều khiển tập trung hay sử dụng mô hình động
lực học để thiết kế điều khiển cho robot.
Hiện nay bộ điều khiển PD được ứng dụng rộng rãi trong điều khiển robot nói riêng
và nhiều hệ thống đa dạng khác. Bộ PD có nhiệm vụ đưa sai lệch e(t) của hệ thống về 0
sao cho quá trình quá độ thỏa mãn các yêu cầu cơ bản về chất lượng:
- Nếu sai lệch e(t) càng lớn thì thông qua khâu khuếch đại (P), tín hiệu u(t) càng lớn.
- Nếu thay đổi sai lệch của e(t) càng lớn thì thông qua thành phần vi phân (D), phản
ứng thích hợp của u(t) sẽ càng nhanh.
Bộ điều khiển PID được mô tả bằng mô hình vào ra:
de(t )
u(t ) = k pe(t ) + TD
dt
115
Hình 67. Mô hình hệ thống sử dụng bộ điều khiển PID
Ta có phương trình động lực học của robot đã xây dựng được từ nội dung trước.
M ( q ) q + C ( q, q ) q + G ( q ) + Q = U
Sử dụng kỹ thuật tuyến tính hóa nhờ bù truyền thẳng hay còn gọi là điều khiển trước
mô men, bởi vì để tạo được quỹ đạo mong muốn thì các lực và mô men cần thiết phải tính
toán trước và được nối vào hệ kỹ thuật cần điều khiển. Khi đó bài toán phi tuyến phức tạp
trở thành bài toán tuyến tính.
Phương trình lực điều khiển được chọn như sau:
U = U ' + 
 = M (q)
Với 
  = C (q, q)q + G(q) + Q
Hình 68. Mô hình đối tượng điều khiển
116
Kết hợp hai mô hình dẫn đến một luật điều khiển tuyến tính: Tín hiệu điều khiển
U’ được xác định
U ' = qd + K v e + K p e
Hình 69. Mô hình hệ thống

Với các hệ số Kp, Kd là các ma trận đường chéo kích thước (nxn):
Kp = diag ( kpi )( 3 x 3) , Kv = diag ( kvi )( 3 x 3)
Trong đó kpi = n 2 , kvi = 2n 2 với  , n là hệ số giảm chấn mong muốn và tần số
riêng cho sai số của khớp thứ i. Điều không mong muốn là tay máy vượt quá giảm chấn
gây ảnh hưởng đến độ chính xác của quỹ đạo mong muốn khi muốn dừng chính xác tại vị
trí của bề mặt công tác nào đó. Cho nên hệ số kd, kp thường được chọn với giảm chấn tới
hạn là 1. Khi đó:
n 2
kvi = 2 kpi ; kpi =
4
ki
Với ωn < ωr /2 để tránh công hưởng, với r = với J là mô men quán tính và ki
J
là mô men quán tính của khâu i. J ở đây thay đổi theo hình dạng tay máy, nên chon giá trị
lớn nhất dùng để tính toán. Việc xác định các giá trị này có thể thực hiện thông qua mô
phỏng với các đầu vào cụ thể về kích thước, vật liệu thực. Trong thực tế các giá trị Kp,
117
Kv thường được chọn theo kinh nghiệm. Các giá trị này đồng thời phải nằm trong
giới hạn vật lí của robot.
7.2 Mô phỏng bằng Matlab.
Hình 70. Mô hình Simulink của hệ thống

- Tín hiệu đầu vào là các file.mat lưu các giá trị biến khớp, vận tốc biến khớp, gia tốc
biến khớp.
118
Giá trị biến khớp và vận tốc biến khớp ban đầu.
- Khối điều khiển PD:
119
- Khối phản hồi phi tuyến:
120
function T = InverseModel(t)
a1=0.35; a2=0.35; d4=0.05; %l3=0.125;
l1=a1/2; l2=a2/2;
d1=0.7;d2=0.05;
m1=10;m2=12;m3=2;m4=0.5;%kg
g=9.81; %m/s^2
% Fx=20; Fy=20; Mz=10;

F3=0;
%========================== Momen quan tinh
khoi==========================
J1xx=0.0374;J1yy=0.2763;J1zz=0.2924; % Khau 1
J2xx=0.0447;J2yy=0.1895;J2zz=0.2151; % Khau 2
J3xx=0.0476;J3yy=0.0476;J3zz=0.0007; % Khau 3
J4xx=0.0001;J4yy=0.0002;J4zz=0.0002; % Khau 4

qd = t(1:4);
dqd = t(5:8);
khop)
121
u = t(9:12);
q1=qd(1);q2=qd(2);q3=qd(3);q4=qd(4);
q1d=dqd(1);q2d=dqd(2);q3d=dqd(3);q4d=dqd(4);
% Ma tran khoi luong

m11=J1zz+J2zz+J3zz+J4zz+m1*(-
2*a1*l1+l1^2+a1^2)+m2*(2*a1*a2*cos(q2)-
2*a1*l2*cos(q2)+a1^2+a2^2+l2^2-
2*a2*l2)+(m3+m4)*(2*a1*a2*cos(q2)+a1^2+a2^2);
m12=J2zz+J3zz+J4zz+m2*(-2*a2*l2++l2^2+a2^2-
l2*a1*cos(q2)+a1*a2*cos(q2))+(m3+m4)*(2*a1*a2*cos(q2)+a2^2)
;
m13=0;
m14=J4zz;
;
m22=J2zz+J3zz+J4zz+m2*(-2*a2*l2+a2^2+l2^2)+(m3+m4)*a2^2;
m23=0;m24=J4zz;
m31=0;m32=0;m33=m3+m4;m34=0;
m41=J4zz;m42=J4zz;m43=0;
m44=J4zz;
M=[m11 m12 m13 m14;m21 m22 m23 m24;m31 m32 m33 m34;m41 m42
m43 m44];
% Luc qun tinh coriolis va qun tinh li tam

c11=-(m2*a1*(a2-
l2)*sin(q2)+m3*a1*a2*sin(q2)+m4*a1*a2*sin(q2))*q2d;
c12=-(m2*a1*(a2-
l2)*sin(q2)+m3*a1*a2*sin(q2)+m4*a1*a2*sin(q2))*q1d-
(m2*a1*(a2-
c13=0;c14=0;
c21=(m2*a1*(a2-
l2)*a2*sin(q2)+m3*a1*a2*sin(q2)+m4*a1*a2*sin(q2))*q1d;
c22=0;
c23=0;c24=0;
c31=0;
c32=0;
c33=0;c34=0;c41=0;c42=0;c43=0;c44=0;
C=[c11 c12 c13 c14;c21 c22 c23 c24;c31 c32 c33 c34;c41 c42
c43 c44];
122
%==========================================================
================

g1=0;
g2=0;
g3=-(m3+m4)*g;
g4=0;
G = [g1; g2; g3; g4];
Q1=0; Q2=0; Q3=0; Q4=0;
Q=[Q1;Q2;Q3;Q4];
% Luc dan dong tai cac khop

T = M*[u(1);u(2);u(3);u(4)]+
C*[dqd(1);dqd(2);dqd(3);dqd(4)]+G+Q;
- Khối Robot:
123
function ddq = RobotDynamic(t)
a1=0.35; a2=0.35; d4=0.05; %l3=0.125;
l1=a1/2; l2=a2/2;
d1=0.7;d2=0.05;
m1=10;m2=12;m3=2;m4=0.5;%kg
g=9.81; %m/s^2
% Fx=20; Fy=20; Mz=10;

F3=0;
%========================== Momen quan tinh

khoi==========================
J1xx=0.0374;J1yy=0.2763;J1zz=0.2924; % Khau 1
J2xx=0.0447;J2yy=0.1895;J2zz=0.2151; % Khau 2
J3xx=0.0476;J3yy=0.0476;J3zz=0.0007; % Khau 3
J4xx=0.0001;J4yy=0.0002;J4zz=0.0002; % Khau 4

qd = t(1:4);
dqd = t(5:8);
khop)
u = t(9:12);
q1=qd(1);q2=qd(2);q3=qd(3);q4=qd(4);
q1d=dqd(1);q2d=dqd(2);q3d=dqd(3);q4d=dqd(4);
124
%========================= Ma tran khoi luong
M============================
m11=J1zz+J2zz+J3zz+J4zz+m1*(-
2*a1*l1+l1^2+a1^2)+m2*(2*a1*a2*cos(q2)-
2*a1*l2*cos(q2)+a1^2+a2^2+l2^2-
2*a2*l2)+(m3+m4)*(2*a1*a2*cos(q2)+a1^2+a2^2);
;
m13=0;
m14=J4zz;
;
m22=J2zz+J3zz+J4zz+m2*(-2*a2*l2+a2^2+l2^2)+(m3+m4)*a2^2;
m23=0;m24=J4zz;
m31=0;m32=0;m33=m3+m4;m34=0;
m41=J4zz;m42=J4zz;m43=0;
m44=J4zz;
M=[m11 m12 m13 m14;m21 m22 m23 m24;m31 m32 m33 m34;m41 m42
m43 m44];
%============================Ma tran
corilois==============================
c11=-(m2*a1*(a2-
c12=-(m2*a1*(a2-
l2)*sin(q2)+m3*a1*a2*sin(q2)+m4*a1*a2*sin(q2))*q1d-
(m2*a1*(a2-
c13=0;c14=0;
c21=(m2*a1*(a2-
l2)*a2*sin(q2)+m3*a1*a2*sin(q2)+m4*a1*a2*sin(q2))*q1d;
c22=0;
c23=0;c24=0;
c31=0;
c32=0;
c33=0;c34=0;c41=0;c42=0;c43=0;c44=0;
C=[c11 c12 c13 c14;c21 c22 c23 c24;c31 c32 c33 c34;c41 c42
c43 c44];
%==========================================================
================
125
g1=0;
g2=0;
g3=-(m3+m4)*g;
g4=0;
G = [g1; g2; g3; g4];
% Luc suy rong cua cac luc khong the

%Q1=0.2*dq(1);
%Q2=0.2*dq(2);
Q1=0; Q2=0; Q3=0; Q4=0;
%Q1=0;
%Q2=0;
Q=[Q1;Q2;Q3;Q4];
% Gia toc suy rong

ddq = inv(M)*([u(1);u(2);u(3);u(4)]-
C*[dqd(1);dqd(2);dqd(3);dqd(4)]-G-Q);
126
Kết quả bám quỹ đạo thu được:
Khớp 1:
Sai lệch khớp 1:
127
Khớp 2:
128
Khớp 3:
129
Khớp 4:
130
7.3 Mô phỏng bằng Simscape Multibody.
Mô hình điều khiển robot Scara 4 bậc từ do chạy theo quỹ đạo
131
Hình 71. Mô hình 3D của Robot trong Matlab
Kết quả điều khiển với bộ điều khiển PID:
132
133
Link video: https://drive.google.com/file/d/1vTJuAtojjLkQRYryXPrrwc6u-
SYRYSb7/view?usp=sharing
134
8.THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN BIẾN KHỚP VỚI ĐỘNG CƠ DC SERVO
8.1 Mô hình hóa cho một trục chuyển động, xác định hàm truyền
Hình 72. Sơ đồ khớp rô bốt hoàn chỉnh bao gồm hộp số
J a là momen quán tính của động cơ
J m là momen quán tính của khớp nối
J l là momen quán tính của khâu gắp trên khớp đó
 m là momen ở trục động cơ

 l là momen tải quán tính
 m là góc quay ở trục động cơ
 s là góc quay ở tải
Bm là hệ số giảm chấn của động cơ
Bl là hệ số giảm chấn của tải
Ta có hệ số giảm tốc:
s
n=
m
Sử dụng nguyên lý D’Alambert , chúng ta có:
135
 l − Bl  s = J l  s
Với:
 s
s = = s là tốc độ góc của tải (Rad / s)
dt
 s s
s = = là gia tốc góc của tải ( Rad / s 2 )
dt 2 dt
Phương trình trên tương đương với:
 l − Bls = J l s
Sử dụng nguyên lý trên cho trục động cơ chính ta được:
 m − n l − Bm  m = ( J a + J m ) m
Kết hợp các phương trình trên ta thu được:
 m = ( J a + J m + n 2 J l ) m + ( Bm + n 2 Bl ) m
Đặt:
J eff = J a + J m + n 2 J l là momen quán tính tác động ở trục động cơ
Beff = Bm + n 2 Bl là hệ số giảm chấn tác dụng ở trục động cơ
Ta thu được:
 m = J eff  m + Beff  m
 m = J eff m + Beff m
• Động cơ điện một chiều
136
Hình 73. Sơ đồ khối của hệ thống truyền động điện cho một khớp Robot
Trong hình vb (t ) là sức phản điện động phần ứng , được biểu diễn theo biểu thức
sau:
vb (t ) = K b  m (t ) (3.1)
vb (t ) = K bm (t )
Với K b hằng số sức phản điện động (V.s / Rad). L và R là độ tự cảm (H) và
điện trở (  ) của phần ứng. Sử dụng định luật Kirchhoff ta thu được biểu thức sau:
di
L + R.i = v(t ) − vb (t ) (3.2)
dt
di
= L + R.i = v(t ) − Kb .m (t )
dt
Với phép biến đổi Laplace ta thu được phương trình trên trong miền tần số:
( L.s + R ).I ( s ) = V ( s ) − K b .W ( s ) (3.3)
Với s là tần số phức (rad / s). Động cơ DC hoạt động trong khoảng tuyến tính
vì thế momen tạo ta tỷ lệ với dòng phần ứng.
 m = K m .i (t ) (3.4)
Trong miền tần số:

Tm = K m .I ( s ) (3.5)
137
Với K m là hằng số momen. Trục động cơ với momen quán tình tác dụng
J eff và hệ số giảm chấn tác động Beff :
 m = J eff m + Beff m (3.6)
Mối liến hệ trong miền tần số qua phép biến đổi Laplace:
Tm = ( J eff .s + Beff ).Wm (3.7)
Từ (3.3) (3.5) (3.7) ta thu được:

W ( s) Km
= (3.8)
V ( s ) J eff Ls + ( Beff L + J eff R ) s + Beff R + K m K b
2
m ( s) Km 1
= . (3.9)
V (s) J eff Ls 2 + ( Beff L + J eff R ) s + Beff R + K m K b s
Đây là hàm truyền với tín hiệu vào là hiệu điện thế cấp vào động cơ DC và tín hiệu
đầu ra là giá trị góc của trục động cơ:
m ( s)
G ( s) = (Rad/Volt) (3.10)
V ( s)
• Hàm truyền khâu 1:

m1 ( s)
G1 ( s) = (Rad/Volt) (3.11)
V1 ( s)
• Hàm truyền khâu 2:

m 2 ( s )
G2 ( s) = (Rad/Volt) (3.12)
V2 ( s)
Riêng với khâu 3 là khâu tịnh tiến nên ta cần phải có thêm một bộ truyền động biến
đổi chuyển động quay thành tịnh tiến.Ở đây ta sử dụng bộ truyền ví me đai ốc bi
138
• Trục vít me đai ốc bi
Tín hiệu vào: Vận tốc góc  (t ) của trục vít me (rad/s)
Tín hiệu ra : Lượng di động y(t) của bàn máy gắn liền với trục vít me đai ốc bi hay
độ dịch chuyển của bàn máy.
Gọi P(m) là bước của vít me ta có quan hệ:
t
P
2 0
y (t ) =  (t )dt (3.11)
Biến đổi Laplace hai về với điều kiện đầu bằng 0 ta có:
P W (s)
Y ( s) = (3.12)
2 s
Ta thu được hàm truyền:
W (s) P 1 Q
= = (3.13)
Y ( s) 2 s s
Với:
P
Q= (3.14)
2
139
• Do đó đối với khâu 3:
W3 ( s) P 1
G3 ( s) = (3.15)
V3 ( s) 2 s
• Các thông số:

Bảng thông số động cơ servo DC được sử dụng trên Scara Robot.
Rated Torque (N.m) 4
Peak current (A) 4.4
Voltage constant (V/rad/s) _ Kb 56/60
Torque coeffient (Nm/A)_Km 0.9
Rotor inertia (kg.m2)_Jm 2,97E-04
R (Ohm) 1.83
L (H) 4.72E-03
Rated speed (rpm) 2500
Bm (Nm/s) 2.58E-04
Bảng thông số tham khảo động cơ servo DC cho khâu 1,2 và 3
Thông số động cơ được chọn cho khâu 4
140
8.1.1 Tính toán hàm truyền
• Khớp 1:
Tính momen quán tính J l1
Momen quán tính J l1 của khớp 1 được xác định là momen quán tính theo trục
z và do khớp 1 phải chịu tác động bao gồm cả momen quán tính của các khâu phía
sau tác dụng lên , sử dụng phần mềm SolidWork ta thu được kết quả như đã trình
bày ở mục 2.4.4
Từ bảng kết quả trên ta thu được J l1 có giá trị bằng 4.71(kg.m 2 )
 −4 1 −3
 J eff = J a1 + J m1 + n1 J l1 = 2,97.10 + 252  4,69 = 7,801.10 ( Kg .m )
2 2

 B = B + n 2 .B = 2,58.10−4 + 1  0,01 = 2,74.10−4 ( Nms / Rad )
 eff m1 1 l1
252
Với các thông số ta đã xác định:

K m = 0.9( Nm / Ampe)
K b = 0.93(V / Rad / s )
J l1 = 4.69( Kg .m 2 )
J a = 2,97.10−4 ( Kg .m 2 )
Bm = 2,58.10 −4 ( Nms / Rad )
Bl = 0.01( Nms / Rad )
R = 1,83(Ohm)
L = 4,72.10−3 ( H )
1
n=
25
B1 ( s) 0,9
 G1 ( s) = = −5 3
( Rad / Volt )
A1 ( s) 3,659.10 s + 0,01419s 2 + 0,8408s
Kiểm tra tính ổn định của G1 ( s ) :
141
➢ Kiểm tra tính ổn định của hệ hở:
+ Điều kiện cần:
Ta thấy các hệ số của đa thức A(s) đều lớn hơn và khác 0. Vậy thỏa mãn điều kiện cần của
tiêu chuẩn Routh.
+ Điều kiện đủ:
Nhận thấy các số hạng thuộc cột đầu tiên của bảng Routh đều dương nên thỏa mãn điều
kiện ổn định. Vậy hệ thống được mô tả bằng phương trình đã cho là ổn định.
➢ Kiểm tra tính ổn định của hệ kín:
Theo tiêu chuẩn Nyquist:
Hình 74. Đồ thị Nyquist của hàm truyền G1 ( s )
Qua đồ thị ta nhận xét rẳng: Theo định lý 2.22 (Nyquist) (Sách ‘Cơ sở lí thuyết điều
khiển tuyến tính’) thì ‘Nếu hàm truyền 𝐺ℎ (𝑠) của hệ hở có m điểm cực không nằm bên trái
trục ảo thì điều kiện cần và đủ để hệ kín ổn định là đồ thị Nyquist của hệ hở bao điểm (-
1+0j) của mặt phẳng phức m lần theo chiều ngược chiều kim đồng hồ’. Ta thấy rằng đồ thị
Nyquist không bao điểm (-1+0j) nên sẽ không có điểm cực nào nằm trên trục ảo hoặc bên
phải trục ảo, cho nên hệ kín sẽ ổn định.
142
Đồ thị Bode:
Đồ thị Bode của hàm truyền G1 ( s )

Nhận xét: Theo chuẩn Bode
Để hệ kín Gk ( s ) ổn định thì đồ thị bode của hệ hở G ( s ) phải thõa mãn đồng
thời cả hai yếu tố là có độ dữ trữ pha và độ dự trữ biên độ lớn hơn 0. Từ đồ thị của
G1 ( s ) cho thấy có độ dự trữ biên độ Gm và độ dự trữ pha Pm đồng thời lớn hơn 0,
vậy hệ kín này ổn định
Các khớp sau được thực hiện một cách tương tự với các số liệu đã có ở các phần
trước
8.1.2 Xây dựng sơ đồ Simulink mô tả cho một khớp của robot

• Cơ sở lý thuyết
Như đã trình bày ở trong phần 3.1.1 ,kết hợp hai định luật D’Alambert và Kirchhoff
ta thu được hệ phương trình sau:
143
d 2 m d m d 2 m 1  d m 
J eff =  m − Beff = =  K mi − Beff 
 dt 
2 2
dt dt dt J eff
di di 1  d 
L = V − Ri − eb = =  − Ri + V − K b m 
dt dt L  dt 
• Sơ đồ Simulink
Hình 75. Sơ đồ simulink cho động cơ servo DC
144
8.2 Xây dựng mô hình hệ thống điều khiển cho toàn bộ rô bốt
• Các bước để xây dựng hệ thống điều khiển
Hình 76. Các bước thực hiện
• Đưa mô hình đã thiết kế trên Cad và SolidWork vào Matlab/SimMechanics
Hình 77. Mô hình 3D sau khi đưa vào Matlab
145
• Xây dựng mô hình điều khiển cho toàn Robot
Hình 78. Mô hình hóa điều khiển cho toàn bộ robot vơi Simulink
Với đầu vào là các biến khớp mong muốn được xây tính toán trong hàm của Matlab
từ một quỹ đạo bất kì theo yêu cầu. Sau đó qua bộ điều khiển , khối động cơ , mô hình 3D
của robot ta thu được đầu ra các biến khớp đáp ứng.
146
Trong đó khối 3D_Model_ScaraRobot được xây dựng dựa trên mô hình 3D trên
Simulink như sau:
Hình 79. Mô hình hóa Robot trên simulink

Với đầu vào là các giá trị biến khớp từ bộ điều khiển , đầu ra là các giá trị các biến
khớp đã thực hiện thu được từ các cảm biến khớp trên mô hình 3D.
147
8.3. Sử dụng Matlab – Simulink để mô phỏng động lực học cho một trục chuyển động, và
cho toàn bộ rô bốt
Sử dụng matlab simulink mô phỏng động lực học cho khớp 1
• Sơ đồ simulink cho khớp 1:
Hình 80. Sơ đồ simulink mô phỏng động lực học cho một khớp 1 sử dụng Servo_DC kết
hợp hộp giảm tốc
148
Lấy tín hiệu đầu vào là hằng số 1(t) ta thu được kết quả
Hình 81. Đồ thị vận tốc theo thời gian trong 0.1s
Hình 82. Đồ thị biến góc theo thời gian trong 0.1s
149
Hình 83. Đồ thị momen xoắn theo thời gian trong 0.1s
Hình 84. Đồ thị cường độ dòng điện theo thời gian trong 0.1s
150
• Sơ đồ Simulink mô phỏng động lực học và điều khiển cho toàn bộ robot
8.4. Bộ điều điều khiển PID
Hình 85. Sơ đồ khối bộ điều khiển PID
Với mục tiêu đưa robot về một vị trí mong muốn ứng với các tọa độ khớp qd
theo yêu cầu quỹ đạo cần thực hiện, ta xét luật điều khiển đơn giản và phổ biến đáp
ứng yêu cầu này. Đó là luật điều khiển PID. Tín hiệu điện điều khiển được tính tỷ lệ
với sai lệch về vị trí và vận tốc của thời điểm hiện tại so với giá trị mong muốn.
Theo luật điều khiển này , tín hiệu điện điều khiển:
t
de(t )
u (t ) = K p .e(t ) + K d . + K I . e(t ) dt (5.1)
dt 0
151
Với e(t ) = qd (t ) − q (t )
Trong đó:
e(t ) là sai lệch giá trị biến khớp mong muốn với giá trị biến khớp đạt được tại thời
điểm t
qd (t ) là giá trị biến khớp mong muốn tại thời điểm t được lấy ra từ yêu cầu của
quỹ đạo mong muốn
q (t ) là giá trị biến khớp thực đạt được tại thời điểm t thu được từ hệ thống phản
hồi đầu ra của hệ thống , cụ thể ở đây là cảm biến đo góc khớp trên robot
• Triển khai trên Matlab
Xây dựng khối controller
Hình 86. Sơ đồ simulink một trục khớp
152
Hình 87. Sơ đồ Simulink khối Controller
8.5. Chọn các hệ số cho hàm điều khiển
Sử dụng công cụ Tune PID của Matlab để chọn được bộ số PID thích hợp cho
từng trục
Hình 88. Block PID controller trên Matlab

Tại mục Select tuning method chọn Tune... để bắt đầu quá trình chọn hệ số PID
153
Hình 89. Cửa sổ PID Tuning
Tại mục Tuning Tools của của sổ tùy chỉnh hai thanh trượt để thu được đồ thị
đáp ứng mong muốn. Kết thúc nhấn vào Update block để nhập hệ số PID tương ứng
với đồ thị mong muốn.
Hình 90. Mục Tuning Tools và Results
154
Kết quả đáp ứng sáu khi đã chỉnh được hệ số PID
Hình 91. Đồ thị đáp ứng biến khớp của trục khớp 1 sau khi đã chỉnh các hệ số PID
Với cách làm như trên ta thu được hệ số PID cho các trục khớp của Robot như
sau:
Khớp Kp Ki Kd
1 669.5 3138 7.108
2 1184 11364 1.181
3 65620 717500 -208.7
4 819.3 79711.3 0.494
155
8.6 Kết quả
Kết quả của hệ cả hệ điều khiển sau khi đã có bộ điều khiển hoàn chỉnh áp
dụng cho quỹ đạo của một chu trình gắp nhả vật được thiết kế ở phần 8.1
Với đường màu đỏ là giá trị mong muốn và đường màu xanh là giá trị đáp ứng
Hình 92. Đồ thị biến khớp mong muốn và đáp ứng 1
Hình 93. Sai số biến khớp 1
156
157
158
Nhận xét:
Hệ điều khiển đã cho ra đồ thị biến khớp đáp ứng gần như bám sát so với đồ
thị mong muốn và có sai số nhỏ, ổn định.
159
KẾT LUẬN
Trên đây là toàn bộ báo cáo của nhóm 4 về đề tài “Thiết kế Robot ứng dụng trong
lắp ráp sản phẩm”. Nhóm đã nghiêm túc thực hiện theo đúng nhiệm vụ được giao. Với
những kiến thức và lập luận dựa trên cơ sở khoa học – kỹ thuật về robot, nhóm đã hoàn
thành những công việc sau:
• Phân tích và lựa chọn cấu trúc robot hợp lý cho đề tài.
• Tiến hành xây dựng bản vẽ mô hình 3D robot và bản vẽ lắp 2D bằng các
phần mềm Solidworks và AutoCAD.
• Khảo động học robot và xây dựng quỹ đạo chuyển động.
• Tính toán các điều kiện về tĩnh học.
• Giải bài toán động lực học, tìm ra phương trình vi phân chuyển động cùng
với đó là lực và momen trên mỗi khâu.
• Thiết kế hệ thống dẫn động cho toàn bộ robot.
• Tính toán và mô phỏng hệ thống điều khiển cho robot trên MATLAB.
160
TÀI LIỆU THAM KHẢO
[1] PGS.TS. Phan Bùi Khôi, Bài giảng Robotics (2009), ĐH Bách khoa Hà nội
[2] Ninh Đức Tốn, Dung sai lắp ghép và kỹ thuật đo
[3] Trịnh Chất, Lê Văn Uyển, Tính toán thiết kế hệ dẫn động cơ khí
[4] Trịnh Chất, Cơ sở thiết kế máy và chi tiết máy
[5] Các website, forum trực tuyến như www.mathworks.com, www.dientuvietnam.net,
http://codientu.org.......
161
PHỤ LỤC HÌNH ẢNH
Hình 1. Sử dụng robot trong sản xuất công nghiệp ............................................................. 5
Hình 2. Sản xuất mạch điện thoại ........................................................................................ 5
Hình 3. Đối tượng thao tác: Mạch điện thoại ...................................................................... 6
Hình 4. Sử dụng giác hút cho khâu thao tác ........................................................................ 7
Hình 5. Sơ đồ bố trí ............................................................................................................. 8
Hình 6. Các kích thước khác ............................................................................................... 8
Hình 7. Không gian thao tác của Robot Scara..................................................................... 9
Hình 8. Không gian thao tác của các loại robot khác ........................................................ 10
Hình 9. Mô hình Robot song song..................................................................................... 11
Hình 10. Mô hình Robot dạng chuỗi ................................................................................. 12
Hình 11. Mô hình 3D của đề tài ........................................................................................ 13
Hình 12. Hình chiếu đứng của Robot ................................................................................ 14
Hình 13. Hình chiếu bằng của Robot ................................................................................ 14
Hình 14. Các kích thước hình học của mô hình ................................................................ 15
Hình 15. Đặc tính cơ học của thép cacbon 1023 ............................................................... 16
Hình 16. Khâu 0................................................................................................................. 16
Hình 17. Khối lượng, momen quán tính khâu 0 ................................................................ 17
Hình 18. Thành phần hóa học của hợp kim nhôm............................................................. 17
Hình 19. Đặc tính cơ học được xuất từ Solidwork ............................................................ 18
Hình 20. Khâu 1................................................................................................................. 18
Hình 21. Khối lượng, momen quán tính của khâu 1 ......................................................... 19
Hình 22. Khâu 2................................................................................................................. 20
Hình 24. Khâu 4................................................................................................................. 21
Hình 26. Đặc tính cơ học của thép hợp kim đã chọn ........................................................ 23
Hình 27. Khâu 3................................................................................................................. 24
Hình 28. Khối lượng, momen quán tính khâu 3 ................................................................ 24
Hình 29. Mô hình tính toán momen quán tính cho khớp 1 ............................................... 25
Hình 30. Kết quả tính momen quán tính cho khớp 1 ........................................................ 26
Hình 31. Mô hình tính toán momen quán tính cho khớp 2 ............................................... 26
Hình 32. Kết quả tính momen quán tính cho khớp 2 ........................................................ 27
Hình 33. Mô hình tính toán momen quán tính cho khớp 3 và khớp 4 .............................. 27
Hình 34. Kết quả tính momen quán tính cho khớp 3 và 4................................................. 28
Hình 35. Sơ đồ động học của Robot .................................................................................. 29
Hình 36. Mô phỏng không gian làm việc trong Matlab .................................................... 41
162
Hình 37. Không gian làm việc của Robot ......................................................................... 41
Hình 38. Đồ thị biến khớp ................................................................................................. 44
Hình 39. Đồ thị vận tốc góc ............................................................................................... 44
Hình 40. Đồ thị gia tốc góc ............................................................................................... 45
Hình 41. Mô tả quỹ đạo chuyển động điểm thao tác ......................................................... 45
Hình 42. Mô tả không gian thao tác trong Matlab ............................................................ 46
Hình 43. Mô tả không gian thao tác và không gian làm việc ............................................ 46
Hình 44. Khối động học ngược vị trí ................................................................................. 49
Hình 45. Khối quỹ đạo lắp ráp .......................................................................................... 50
Hình 46. Giá trị biến khớp ................................................................................................. 57
Hình 47. Giá trị vận tốc ..................................................................................................... 58
Hình 48. Giá trị gia tốc ...................................................................................................... 58
Hình 49. Simulink.............................................................................................................. 67
Hình 50. Hệ tọa độ khảo sát tĩnh học ................................................................................ 77
Hình 51. Thiết lập hệ tọa độ khối tâm ............................................................................... 84
Hình 52. Khối động lực học ngược ................................................................................... 95
Hình 53. Hộp giảm tốc bánh răng trụ .............................................................................. 101
Hình 54. Bộ truyền hành tinh .......................................................................................... 101
Hình 55. Bộ truyền bánh răng sóng ................................................................................. 102
Hình 56. Bộ truyền bánh răng chốt Cycloid .................................................................... 102
Hình 57. Bộ truyền thanh răng bánh răng ....................................................................... 103
Hình 58. Bộ truyền vít me đai ốc bi ................................................................................ 104
Hình 59. Sơ đồ truyền động trục vit me-đai ốc ............................................................... 107
Hình 60. Vít me đai ốc bi loại rotating nut ...................................................................... 108
Hình 61. Các thông số bộ truyền động vít bi ................................................................... 108
Hình 62. Vít me đai ôc bi rotating nut (Ball screw rotating nut) .................................... 111
Hình 63. Bảng thông số theo tiêu chuẩn SKF ................................................................. 111
Hình 64. Kích thước hình học của trục vít me và đai ốc lựa chọn .................................. 111
Hình 65. Đồ thị xác định ứng suất lớn nhất σmax........................................................... 112
Hình 66. Kiểm bền các khâu trên phần mềm SolidWorks .............................................. 114
Hình 67. Mô hình hệ thống sử dụng bộ điều khiển PID.................................................. 116
Hình 68. Mô hình đối tượng điều khiển .......................................................................... 116
Hình 69. Mô hình hệ thống.............................................................................................. 117
Hình 70. Mô hình Simulink của hệ thống ....................................................................... 118
Hình 71. Mô hình 3D của Robot trong Matlab ............................................................... 132
Hình 72. Sơ đồ khớp rô bốt hoàn chỉnh bao gồm hộp số ................................................ 135
Hình 73. Sơ đồ khối của hệ thống truyền động điện cho một khớp Robot ..................... 137
163
Hình 74. Đồ thị Nyquist của hàm truyền G1 ( s ) ............................................................ 142
Hình 75. Sơ đồ simulink cho động cơ servo DC ............................................................. 144
Hình 76. Các bước thực hiện ........................................................................................... 145
Hình 77. Mô hình 3D sau khi đưa vào Matlab ................................................................ 145
Hình 78. Mô hình hóa điều khiển cho toàn bộ robot vơi Simulink ................................. 146
Hình 79. Mô hình hóa Robot trên simulink ..................................................................... 147
Hình 80. Sơ đồ simulink mô phỏng động lực học cho một khớp 1 sử dụng Servo_DC kết
hợp hộp giảm tốc ............................................................................................................. 148
Hình 81. Đồ thị vận tốc theo thời gian trong 0.1s ........................................................... 149
Hình 82. Đồ thị biến góc theo thời gian trong 0.1s ......................................................... 149
Hình 83. Đồ thị momen xoắn theo thời gian trong 0.1s .................................................. 150
Hình 84. Đồ thị cường độ dòng điện theo thời gian trong 0.1s ....................................... 150
Hình 85. Sơ đồ khối bộ điều khiển PID .......................................................................... 151
Hình 86. Sơ đồ simulink một trục khớp .......................................................................... 152
Hình 87. Sơ đồ Simulink khối Controller........................................................................ 153
Hình 88. Block PID controller trên Matlab ..................................................................... 153
Hình 89. Cửa sổ PID Tuning ........................................................................................... 154
Hình 90. Mục Tuning Tools và Results ......................................................................... 154
Hình 91. Đồ thị đáp ứng biến khớp của trục khớp 1 sau khi đã chỉnh các hệ số PID ..... 155
Hình 92. Đồ thị biến khớp mong muốn và đáp ứng 1 ..................................................... 156
Hình 93. Sai số biến khớp 1 ............................................................................................ 156
164

Nhom4 ThietKeRobot MaLop132841 LapRapSanPhamMach

Uploaded by

Document Information

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

Nhom4 ThietKeRobot MaLop132841 LapRapSanPhamMach

Uploaded by

Copyright:

Available Formats

ĐẠI HỌC BÁCH KHOA HÀ NỘI

Họ và tên Mã số sinh viên Lớp

Nhóm sinh viên thực hiện

Nhiệm vụ chính được

Giải các bài toán động học,

Hình 1. Sử dụng robot trong sản xuất công nghiệp

Hình 2. Sản xuất mạch điện thoại

Thông số kĩ thuật của mạch:

- Khối lượng tối đa 50 gram.

- Mạch được đặt nằm trên băng chuyền.

Hình 3. Đối tượng thao tác: Mạch điện thoại

Hình 5 Đối tượng thao tác: Vỏ điện thoại

- Giả định vỏ điện thoại với kích thước 165x85x8 mm.

- Khối lượng tối đa 500 gram.

- Vỏ được đặt nằm ngay ngắn trên băng chuyền.

b. Phân tích yêu cầu về vị trí.

Hình 6. Các kích thước khác

c. Yêu cầu về hướng của khâu thao tác.

d. Yêu cầu về không gian thao tác.

Hình 7. Không gian thao tác của Robot Scara

1.3. Xác định các đặc trưng kỹ thuật

c. Yêu cầu về tải trọng.

Kết cấu 4 bậc tự do:

+ Robot song song:

Hình 9. Mô hình Robot song song

- Nhược điểm: độ linh hoạt thấp.

+ Robot dạng chuỗi:

- Ưu điểm: Độ linh hoạt cao

Đối tượng: mạch kích thước 160x80x3 mm, nặng 50g

2. THIẾT KẾ 3D MÔ HÌNH ROBOT

Hình 11. Mô hình 3D của đề tài

Hình 12. Hình chiếu đứng của Robot

Hình 13. Hình chiếu bằng của Robot

Hình 14. Các kích thước hình học của mô hình

Vật liệu, khối lượng, momen quán tính các khâu

Hình 16. Khâu 0

Hình 20. Khâu 1

Hình 22. Khâu 2

Hình 23. Khối lượng, momen quán tính của khâu 2

• Khối lượng, momen quán tính khâu 4

Hình 24. Khâu 4

Hình 28. Khối lượng, momen quán tính khâu 3

Hình 35. Sơ đồ động học của Robot

q1 = ( −165  165 ) ; q2 = ( −147  147 ) ; q3 = ( 0  400mm ) ; q4 = ( 0  360 )

cos(q1 ) − sin(q1 ) 0 a1 cos(q1 )  cos(q 2 ) − sin(q 2 ) 0 a2 cos(q 2 ) 

cos ( q1 + q2 ) − sin ( q1 + q2 ) 0 a1 cos ( q1 ) + a2 cos ( q1 + q2 ) 

cos ( q1 + q2 ) − sin ( q1 + q2 ) 0 a1 cos ( q1 ) + a2 cos ( q1 + q2 ) 

Đặt: s1 = sin(q1), c1 = cos(q1)

s12 = sin(q1+q2), c12 = cos(q1+q2)

s124 = sin(q1+q2 + q4), c124 = cos(q1+q2 +q4)

c1 −s1 0 a1c1  c12 − s12 0 a1c1 + a2 c12 

c12 − s12 0 a1c1 + a2 c12  c124 − s124 0 a1c1 + a2 c12 

Phương trình động học thuận của robot:

3.1.1. Thiết lập phương trình động học cho robot

 cos  cos − cos  sin  sin  

 c11 ( ,  , ) c12 ( ,  , ) c13 ( ,  , ) x E 

Ma trận được đưa về dạng như sau:

rn =  x(q), y (q), z (q) 

 c11 (q ) c12 (q ) c13 (q ) x(q )   c11 ( ,  , ) c12 ( ,  , ) c13 ( ,  , ) x E 

3.1.2. Giải bài toán động học thuận

- Xác định các tọa độ định vị điểm tác động cuối

 xE = x(q)  xE = a1c1 + a2 c12

 cos  cos − cos  sin  sin   c124 − s124 0 

cos  sin  = cos(q1 + q 2 + q 4 )

q1 = A(1) + 3(B(1)-A(1))(t_in/Te)^2 - 2*(B(1)-

q1 = A(1) + 3(B(1)-A(1))(t_in/Te)^2 - 2*(B(1)-

q1 = A(1) + 3(B(1)-A(1))(t_in/Te)^2 - 2*(B(1)-

q1 = A(1) + 3(B(1)-A(1))(t_in/Te)^2 - 2*(B(1)-

q1 = A(1) + 3(B(1)-A(1))(t_in/Te)^2 - 2*(B(1)-

q1 = A(1) + 3(B(1)-A(1))(t_in/Te)^2 - 2*(B(1)-

q1 = A(1) + 3(B(1)-A(1))(t_in/Te)^2 - 2*(B(1)-

q1 = A(1) + 3(B(1)-A(1))(t_in/Te)^2 - 2*(B(1)-