Thiết kế và chế tạo robot Delta.pdf

ĐẠI HỌC ĐÀ NẴNG
TRƯỜNG ĐẠI HỌC BÁCH KHOA
KHOA CƠ KHÍ
ĐỒ ÁN TỐT NGHIỆP
ĐỀ TÀI:
THIẾT KẾ VÀ CHẾ TẠO ROBOT DELTA
Người hướng dẫn: TS. LÊ HOÀI NAM
Sinh viên thực hiện: LÊ TẤN VINH
NGUYỄN VIẾT LONG
Đà Nẵng, 2019

TÓM TẮT ĐỒ ÁN TỐT NGHIỆP
Tên đề tài: Thiết kế và chế tạo robot Delta
Sinh viên thực hiện: Lê Tấn Vinh Lớp: 15CDT1 MSSV: 101150197
Nguyễn Viết Long Lớp: 15CDT1 MSSV: 101150173
Giảng viên hướng dẫn: TS. Lê Hoài Nam
Giảng viên duyệt: PGS.TS. Trần Xuân Tùy
 Nhu cầu thực tế của đề tài
Ngày nay, robot đang chiếm một ví trí quan trọng trong các dây chuyền sản xuất tự
động. Với những ưu điểm vượt trội về tốc độ làm việc của robot Delta thì việc nghiên
cứu, thiết kế và chế tạo chúng đang thực sự là một nhu cầu cấp thiết. Đặc điểm của
robot Delta là với những yêu khác nhau về vùng làm việc thì bên cạnh đó cũng có
những bộ thông số khác nhau về kích thước của robot. Trong khuôn khổ của đề tài này
sẽ trình bày về quá trình nghiên cứu và thiết kế robot đi từ vùng làm việc yêu cầu đến
khi hoàn thiện sản phẩm cuối cùng, là một mẫu robot có đủ khả năng ứng dụng thực tế
trong công nghiệp. Có thể được sử dụng trong các dây chuyền phân loại sản phẩm trên
các băng tải động.
 Nội dung đề tài đã thực hiện
Mô hình: 1, Bản vẽ: 5, Thuyết minh gồm: 62 trang.
 Kết quả đạt được
Phần lí thuyết:
 Tìm hiểu chung về các loại robot, biết được ưu và nhược điểm của mỗi loại.
 Nắm được phần động học của robot Delta, tính toán động lực học, vùng làm
việc của robot Delta.
 Tính chọn và tìm hiểu về động cơ, driver điều khiển động cơ, thuật toán
điều khiển chúng.
Phần thiết kế thực tế:
 Viết chương trình giải bài toán động học, động lực học và mô phỏng robot
Delta.
 Thiết kế 3D và 2D các chi tiết robot Delta.
 Thiết kế tủ điện và các bộ phận điều khiển quá trình hoạt động của robot.
Sinh viên thực hiện Sinh viên thực hiện
Lê Tấn Vinh Nguyễn Viết Long
DUT.LRCC

ĐẠI HỌC ĐÀ NẴNG
TRƯỜNG ĐẠIHỌC BÁCHKHOA
KHOA CƠKHÍ
CỘNG HÒA XÃ HÔI CHỦ NGHĨA VIỆT NAM
Độc lập - Tự do - Hạnh phúc
NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP
Sinh viên thực hiện: Lê Tấn Vinh Lớp: 15CDT1 MSSV: 101150197
Nguyễn Viết Long Lớp: 15CDT1 MSSV: 101140138
Tên đề tài: Thiết kế và chế tạo robot Delta
Nội dung chính
a. Phần nghiên cứu chung.
 Nghiên cứu tổng quan về robot Delta.
 Tìm hiểu về động học, động lực học của robot Delta.
 Xây dựng hệ thống cơ khí cho đề tài.
 Thiết kế hệ thống điện và tủ điện.
b. Phần nghiên cứu riêng
STT Sinh viên thực hiện Nội dung
1 Lê Tấn Vinh  Lập trình mô phỏng chuyển động robot Delta
trên Matlab và Solidwork.
 Thiết lập bản vẽ chế tạo các chi tiết của robot
Delta.
2 Nguyễn Viết Long  Thiết Kế 3D các chi tiết của robot Delta.
 Nghiên cứu các phương pháp điều khiển cho
robot.
Ngày giao đồ án: / /
Ngày hoàn thành đồ án: / /
Giáo viên hướng dẫn: TS. Lê Hoài Nam
Đà Nẵng, ngày tháng năm 2019
Trưởng Bộ môn Cơ Điện tử Giảng viên hướng dẫn
TS. Võ Như Thành TS. Lê Hoài Nam
DUT.LRCC

Thiết kế và chế tạo robot Delta
SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS. Lê Hoài Nam Trang i
Lời nói đầu
Trong sự nghiệp công nghiệp hóa và hiện đại hóa đất nước, tự động hóa ngày càng
đóng một vai trò quan trọng. Với tốc độ phát triển như hiện nay chúng ta không chỉ cần
một lượng lao động khổng lồ mà còn đòi hỏi có trình độ, chất lượng tay nghề, kỹ thuật lao
động và thiết bị sản xuất. Mức độ phát triển của khoa học kỹ thuật ngày càng cao thì vấn
đề tự động hoá ngày càng được chú trọng.
Trong những năm gần đây, robot đóng vai trò rất quan trọng trong hoạt động kinh tế,
môi trường và xã hội. Nó là một cột mốc quan trọng đánh dấu sự phát triển vượt bậc của
toàn nhân loại. Nó giúp cho người lao động an toàn hơn, nâng cao hiệu quả sản xuất nhằm
đẩy mạnh phát triển nền kinh tế và một phần nó còn làm giảm rác thải và ô nhiễm trong
các nhà máy công nghiệp.
Kết hợp xu thế phát triển của thời đại cũng những kiến thức quý báu được thầy cô
truyền đạt qua 5 năm học tại trường. Nhóm tác giả quyết định lựa chọn đề tài “Thiết kế và
chế tạo Robot Delta” nhằm đáp ứng nhu cầu trong các dây chuyển sản suất ở các nhà máy
công nghiệp sao cho hệ thống là tối ưu nhất.
Trong thời gian làm đồ án, được sự chỉ bảo tận tình của thầy TS. Lê Hoài Nam cùng sự
hỗ trợ từ công ty Điện tử RP và phía sau là sự giúp đỡ to lớn từ gia đình, bạn bè. Nhóm
tác giả đã thực sự cố gắng hoàn thành đề tài một cách tốt nhất. Tuy nhiên, với kiến thức
còn hạn chế, kinh nghiệm, kĩ năng còn thiếu, mặc dù cố gắng nhiều nhưng bên cạnh đó
vẫn còn nhiều thiếu sót cần bổ sung, hoàn thiện hơn. Mong thầy cô thông cảm và góp ý để
đề tài của nhóm hoàn thiện hơn, có thể phát triển và được ứng dụng trong thời gian sắp
tới.
Một lần nữa nhóm xin cảm ơn gia đình, người thân và bạn bè đã động viên, giúp đỡ
trong suốt quá trình thực hiện đồ án này.
Đặc biệt, nhóm tác giả xin chân thành cảm ơn các Thầy, Cô giáo trong khoa Cơ khí –
Trường Đại học Bách khoa – Đại học Đà Nẵng, đặc biệt là các Thầy đã trực tiếp hướng
dẫn nhóm trong đề tài tốt nghiệp này.
DUT.LRCC

SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS. Lê Hoài Nam Trang ii
LỜI CAM ĐOAN
Chúng em xin cam đoan đồ án tuân thủ đầy đủ các quy định về liêm chính học thuật.
Không bịa đặt, đưa ra thông tin sai lệch so với nguồn trích dẫn.
Không ngụy tạo số liệu trong quá trình khảo sát, thí nghiệm, thực hành, thực tập hoặc
hoạt động học thuật khác.
Không sử dụng các hình thức gian dối trong việc trình bày, thể hiện các hoạt động học
thuật hoặc kết quả từ quá trình học thuật của mình.
Không đạo văn, sử dụng từ ngữ, cách diễn đạt của người khác như thể là của mình,
trình bày, sao chép, dịch đoạn, hoặc nêu ý tưởng của người khác mà không trích dẫn.
Không tự đạo văn, sử dụng lại thông tin nghiên cứu của mình mà không có trình dẫn
hoặc phân mảnh thông tin về kết quả nghiên cứu của mình để công bố trên nhiều ấn
phẩm.
Sinh viên thực hiện Sinh viên thực hiện
Lê Tấn Vinh Nguyễn Viết Long
DUT.LRCC

SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS. Lê Hoài Nam Trang iii
MỤC LỤC
Tóm tắt đồ án tốt nghiệp ........................................................................................................
Nhiệm vụ đồ án tốt nghiệp.....................................................................................................
Lời nói đầu ........................................................................................................................... i
Lời cam đoan....................................................................................................................... ii
Mục lục............................................................................................................................... iii
Danh sach các bản biểu, hình vẽ..........................................................................................v
Chương 1. Tổng quan về đề tài............................................................................................1
1.1. Giới thiệu chung ........................................................................................................... 1
1.2. Giới thiệu về robot Delta.............................................................................................. 2
1.2.1. Phân loại .....................................................................................................................3
1.2.2. Ưu điểm và nhược điểm của robot Delta ...................................................................4
1.3. Mục tiêu đề ra.............................................................................................................. 5
Chương 2 Cơ sở lí thuyết .....................................................................................................7
2.1. Bài toán động học......................................................................................................... 7
2.1.1. Bậc tự do...................................................................................................................10
2.1.2.Xây dựng phương trình động học..............................................................................11
2.2. Động học nghịch......................................................................................................... 13
2.2.1. Phương pháp giải tích:..............................................................................................13
2.2.2. Phương pháp số Newton-rashson:............................................................................15
2.3. Động học thuận........................................................................................................... 17
2.4. Bài toán động lực học................................................................................................. 23
2.4.1. Xác định vận tốc khối tâm và vận tốc góc các khâu: ...............................................23
2.4.2. Xác định phương trình Lagrande dạng nhân tử cho hệ p vật rắn.............................25
2.4.3. Tính toán các ma trận ...............................................................................................26
2.4.4. Giải bài toán động lực học ngược bằng cách biến đổi về các tọa độ suy rộng: .......30
2.5. Khảo sát vùng làm việc .............................................................................................. 32
Chương 3 Thiết kế hệ thống...............................................................................................36
3.1. Sơ đồ tổng quan.......................................................................................................... 36
3.1.1. Sơ đồ tổng quát hệ thống..........................................................................................36
3.1.2. Lưu đồ thuật toán......................................................................................................37
3.2. Thiết kế hệ thống cơ khí............................................................................................. 37
DUT.LRCC

SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS. Lê Hoài Nam Trang iv
3.2.1. Tính chọn bộ thông số cơ bản của Robot Delta: ......................................................39
3.2.2. Tính chọn động cơ....................................................................................................41
3.2.3. Thiết kế cơ khí các chi tiết robot Delta ....................................................................45
a. Tấm đế cố định ...............................................................................................................45
b. Gá động cơ......................................................................................................................46
c. Bạc lót.............................................................................................................................47
d. Tấm đế di động...............................................................................................................47
e. Cánh tay chủ động ..........................................................................................................48
f. Cánh tay bị động .............................................................................................................48
3.3. Thiết kế hệ thống điện – điện tử................................................................................. 50
3.3.1. Thiết kế hệ thống tủ điện..........................................................................................50
a. Cách bố trí tủ...................................................................................................................50
b. Contactor(MS)................................................................................................................51
c. Bộ lọc nguồn lọc nhiễu (Noise Filter) ............................................................................51
3.3.2. Thiết kế mạch điện tử...............................................................................................53
a. Driver điều khiển động cơ ..............................................................................................53
b. Mạch điều khiển .............................................................................................................54
3.4. Thiết kế hệ thống khí nén ........................................................................................... 56
Chương 4 Kết quả nghiên cứu và kết luận.........................................................................58
4.1. Kết quả nghiên cứu..................................................................................................... 58
4.2. Đánh giá:..................................................................................................................... 60
4.3. Kết luận....................................................................................................................... 61
4.4. Hướng phát triển......................................................................................................... 62
Phụ lục....................................................................................................................................
DUT.LRCC

SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS. Lê Hoài Nam Trang v
DANH SÁCH CÁC BẢNG BIỂU, HÌNH VẼ
Hình 1.1 Robot chuỗi của hãng Yaskawa............................................................................1
Hình 1. 2 Robot Delta ..........................................................................................................2
Hình 1. 3 Robot Delta ứng dụng trong dây chuyền đóng gói sản phẩm..............................3
Hình 1. 4 Robot Delta kiểu khớp xoay ................................................................................3
Hình 1. 5 Robot Delta kiểu khớp trượt ................................................................................4
Hình 1. 6 Tốc độ của Robot .................................................................................................5
Hình 1. 7 Mục tiêu hướng đến đề tài trong tương lai...........................................................6
Hình 2. 1 Thông số hình học................................................................................................8
Hình 2. 1 Tám nhiệm khả dĩ của phương trình động học ngược .......................................14
Hình 2. 2 Sơ đồ động học thuận.........................................................................................18
Hình 2. 3 Các hệ tọa độ địa phương gắn cứng vào các khâu chủ động .............................23
Hình 2. 6 Vùng làm việc có dạng ring torus ......................................................................32
Hình 2. 7 Vùng làm việc có dạng horn torus .....................................................................33
Hình 2. 8 Vùng làm việc có dạng spindle torus.................................................................33
Hình 2. 9 Vùng làm việc của mỗi cánh tay........................................................................34
Hình 2. 10 Mười vùng làm việc .........................................................................................35
Hình 3. 1 Sơ đồ tổng quan về hệ thống..............................................................................36
Hình 3. 2 Lưu đồ thuật toán ...............................................................................................37
Hình 3. 3 Sơ đồ hệ thống thiết kế cơ khí............................................................................38
Hình 3. 5 Vùng làm việc được chọn ..................................................................................41
Hình 3. 6 Mô hình 3D vùng làm việc ................................................................................41
Hình 3. 7 Vị trí cánh tay đạt momen tĩnh lớn nhất ............................................................42
Hình 3. 8 Đồ thị mô momen dẫn động tại z = -0.1 m .......................................................43
Hình 3. 9 Đồ thị mô momen dẫn động tại z = -0.34 m ......................................................44
Hình 3. 10 Đồ thị mô momen dẫn động tại z = -0,45 ........................................................45
Hình 3. 11 Tấm đế cố định.................................................................................................46
Hình 3. 12 Gá động cơ .......................................................................................................46
Hình 3. 13 Bạc lót ..............................................................................................................47
Hình 3. 14 Tấm đế di động.................................................................................................47
DUT.LRCC

SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS. Lê Hoài Nam Trang vi
Hình 3. 15 Cánh tay chủ động............................................................................................48
Hình 3. 16 Cánh tay bị động ..............................................................................................49
Hình 3. 17 Mô hình 3D Solid Works robot Delta..............................................................49
Hình 3. 18 Sơ đồ hệ thống điện- điện tử ............................................................................50
Hình 3. 19 Contactor..........................................................................................................51
Hình 3. 20 Sóng hài............................................................................................................52
Hình 3. 21 Tác dụng của lọc 3P và 1P...............................................................................53
Hình 3. 22 AC Servo Driver MDDKT5540.......................................................................53
Hình 3. 23 Sơ đồ chân kết nối Driver MDDKT5540.........................................................54
Hình 3. 24 Arduino Mega 2560 .........................................................................................55
Hình 3. 25 Sơ đồ chân board mega 2560...........................................................................55
Hình 3. 26 Hệ thống khí nén ..............................................................................................56
Hình 4. 1 Hoàn thiện sản phẩm..........................................................................................58
Hình 4. 2 Giao diện điều khiển ..........................................................................................59
Hình 4. 3 Giao diện mô phỏng trên Matlab/Simulink........................................................60
Bảng 2. 1 Bảng kí hiệu hình học..........................................................................................8
Bảng 3. 1 Vùng IIf .............................................................................................................39
Bảng 3. 2 Vùng IIIb ...........................................................................................................39
Bảng 3. 3 Các thông số kích thước được chọn. .................................................................39
Bảng 3. 4 Các thông số kích thước sau khi hiệu chỉnh......................................................40
Bảng 3.5 Các thông số cơ bản động cơ AC Servo Panasonic............................................45
Bảng 3. 5 Thông số kỹ thuật của Board Arduino Mega ....................................................56
Bảng 4. 1 Thông số chính ..................................................................................................59
DUT.LRCC

SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS. Lê Hoài Nam Trang 1
Chương 1. Tổng quan về đề tài
1.1. Giới thiệu chung
Hiện nay, phân loại sản phẩm là một công đoạn được sử dụng rất nhiều trong thực
tế sản xuất. Khi dùng sức người, công việc này đòi hỏi sự tập trung cao và có tính lặp
đi lặp lại. Mặt khác, có những yêu cầu phân loại sản phẩm dựa trên nhiều đặc tính khác
nhau như màu sắc, hình dáng, khối lượng hay những yêu cầu kĩ thuật rất nhỏ mà mắt
thường khó có thể nhận ra. Điều này ảnh hưởng trực tiếp đến năng suất và chất lượng
sản phẩm. Nhận biết những điều này khi còn đang ngồi trên ghế Nhà trường, nhóm đã
bắt tay vào nghiên cứu hệ thống phân loại sản phẩm. Đi kèm với dây chuyền phân loại
sản phẩm là robot công nghiệp có thể thay con người làm những công việc mang tính
lặp đi lặp lại nhàm chán với một năng suất vượt trội và độ tin cậy cao. Robot công
nghiệp được chia làm hai loại chủ yếu là robot chuỗi và robot song song.
Hình 1.1 Robot chuỗi của hãng Yaskawa
Robot chuỗi có độ linh hoạt cao, không gian làm việc rộng. Tuy nhiên, các động cơ
dẫn động thường được gắn trên khâu động nên quán tính lớn, độ cứng vững không cao.
Ngược lại, robot song song tuy có không gian làm việc bị hạn chế, xuất hiện các điểm
kì dị làm cho robot thiếu hoặc thừa bậc tự do nhưng lại có điểm mạnh là độ cứng vững
cao do sự ràng buộc giữa các khâu của chuỗi động học kín, khớp truyền động là cố
DUT.LRCC

định. Đặc biệt hơn là nó có thể thực hiện được các chuyển động với vận tốc cao mà
không sợ bị hạn chế về mặt quán tính.
Từ khi xuất hiện lần đầu tiên vào năm 1947 với cơ cấu Hexapod do tiến sỹ Eric phát
minh ra [1] đến nay robot song song đã có được chặng đường phát triển khá dài với
nhiều thành tựu nối bật. Robot song song cũng có rất nhiều loại, từ hai bậc tự do cho
đến sáu bậc tự do. Chính vì ưu điểm về độ cứng vững và gia công tốc độ cao mà robot
song song ngày càng được nhiều nhà khoa học nghiên cứu để đưa vào công nghiệp
thay thế cho hệ thống máy công cụ.
1.2. Giới thiệu về robot Delta
Robot song song kiểu Delta (từ đây trở về sau gọi tắt là robot Delta) lần đầu tiên
được phát minh bởi giáo sư Reymond Clavel vào năm 1985 [2][3], đến nay đã có rất
nhiều hãng sản xuất robot nổi tiếng đã chế tạo thành công và đưa vào ứng dụng thực
tiễn như Fanuc, ABB, Bosch Packaging…
Robot Delta sử dụng các cơ cấu hình bình hành (parallelogram) và một tấm đế di
động (moving platform) có ba bậc tự do tịnh tiến so với tấm đế cố định (base).
Hình 1. 2 Robot Delta
Do tính ưu việt của Robot song song nên ngày càng thu hút được nhiều nhà khoa
học nghiên cứu, đồng thời cũng được ứng dụng ngày càng rộng rãi vào nhiều lĩnh vực
khác nhau như nghành vật lý, cơ khí, y tế, quân sự…
Một số hình ảnh về ứng dụng của robot Delta
DUT.LRCC

Hình 1. 3 Robot Delta ứng dụng trong dây chuyền đóng gói sản phẩm
1.2.1. Phân loại
Dựa theo đặc tính của khớp ta phân loại robot Delta theo 2 dạng chính:
 Robot Delta kiểu ba khớp xoay: Loại robot này ban đầu được ứng dụng để gắp
và thả các thanh sôcôla từ băng chuyền vào trong hộp để đóng gói. Sau đó, nó
được ứng dụng nhiều trong các dây chuyền sản xuất thực phẩm và trong y học .
Hình 1. 4 Robot Delta kiểu khớp xoay
 Robot Delta kiểu ba khớp trượt: Loại robot này được sử dụng nhiều trong các
loại máy in 3D
DUT.LRCC

Hình 1. 5 Robot Delta kiểu khớp trượt
1.2.2. Ưu điểm và nhược điểm của robot Delta
Ưu điểm :
 Độ cứng vững cao do kết cấu hình học của chúng: Tất cả các lực tác động
đồng thời được chia sẻ cho tất cả các chân, cấu trúc động học một cách đặc
biệt của các khớp liên kết cho phép chuyển tất cả các lực tác dụng thành các
lực kéo/nén của các chân.
 Có thế thực hiện được các thao tác phức tạp và hoạt động với độ chính xác
cao với cấu trúc song song, sai số chỉ phụ thuộc vào sai số dọc trục của các
cụm cơ cấu chân riêng lẻ và các sai số không bị tích lũy.
 Có thể thiết kế ở các kích thước khác nhau.
 Đơn giản hóa các cơ cấu máy và giảm số lượng phần tử do các chân và khớp
nối
 được thiết kế sẵn thành các cụm chi tiết tiêu chuẩn.
 Đặc biệt có ưu điểm vượt trội về mặt tốc độ so với Robot chuỗi.
DUT.LRCC

Hình 1. 6 Tốc độ của Robot
Nhược điểm:
 Khoảng không gian làm việc nhỏ và thiết kế khó khăn.
 Việc giải các bài toán động học, động lực học phức tạp.
 Có nhiều điểm suy biến (kỳ dị) trong không gian làm việc.
1.3. Mục tiêu đề ra
Để thiết kế được mẫu robot mang tiêu chuẩn công nghiệp và có thể hoạt động ổn
định trong sản xuất, đề tài cần xác định đúng các mục tiêu đề ra ban đầu. Đầu tiên là
thiết kế phải mang tính cứng vững cao, chịu được tải trọng và rung động, cũng như
cường đồ làm việc liên tục. Bên cạnh đó, thuật toán điều khiển và xử lý ảnh phải được
tối ưu để có thể giải quyết được bài toán gắp vật chính xác trên băng tải động. Yêu cầu
cuối cùng là về độ hoàn thiện và thẩm mỹ của robot, những yếu tố này phải được đảm
bảo trong quá trình thiết kế và chế tạo, dưới sự hỗ trợ của máy CNC.
DUT.LRCC

Hình 1. 7 Mục tiêu hướng đến đề tài trong tương lai
DUT.LRCC

Chương 2 Cơ sở lí thuyết
2.1. Bài toán động học
Robot delta kiểu ba khớp quay gồm ba cánh tay giống nhau RUU [3] duy trì sự
song song giữa tấm đế cố định và tấm đế di động có gắn khâu chấp hành cuối. Các
khớp quay trên cùng được dẫn động (kí hiệu bởi từ gạch chân R – Revolute) bởi các cơ
cấu chấp hành tạo chuyển động quay (động cơ) gắn trên tấm đế cố định. Các biến khớp
là 𝜃𝑖 với 𝑖 = 1,2,3 như trong hình dưới, chiều dương của 𝜃𝑖 được xác định theo qui tắc
bàn tay phải. Góc 𝜃𝑖 = 0 được xác định khi khâu dẫn động nằm trên mặt phẳng nằm
ngang. Cơ cấu hình bình hành gồm bốn thanh trong ba khâu bên dưới đảm bảo chuyển
động tịnh tiến. Các khớp các-đăng (U – Universal) được tạo bởi ba khớp quay R không
nằm cùng một chỗ (non-collocated) (gồm hai song song và một vuông góc, ta có sáu
khớp các-đăng như vậy)
Robot delta ba bậc tự do có khả năng điều khiển tấm đế di động di chuyển tịnh tiến
theo các phương xyz trong vùng làm việc của nó. Xét ba chuỗi RUU giống nhau, ba
chuỗi này có cấu trúc giống chân người với các điểm 𝐵𝑖 với 𝑖 = 1,2,3 là các khớp hông
(hip), các điểm 𝐴𝑖 với 𝑖 = 1,2,3 là các khớp gối (knee), các điểm 𝑃𝑖 với 𝑖 = 1,2,3 là
các mắt cá chân (ankle). Chiều dài cạnh tam giác đều tấm đế cố định là 𝑠𝐵 và chiều dài
cạnh tam giác đều tấm đế di động là 𝑠𝑃. Tam giác đều tấm đế di động ngược với tam
giác đều tấm đế cố định theo hướng không đổi. Hình dạng tam giác đều của tấm di
chuyển được nghịch đảo với hình tam giác tấm đế cố định theo phương không đổi.
Các thông số hình học của tấm đế cố định và tấm đế di động (chứa khâu chấp hành
cuối) được thể hiện trong hình 2.2 với các kí hiệu và ý nghĩa được tóm tắt bằng bảng
dưới.
DUT.LRCC

Hình 2. 1 Thông số hình học
Bảng 2. 1 Bảng kí hiệu hình học
Ký hiệu Ý nghĩa
𝑃𝑖 𝑖 = 1,2,3 điểm nối giữa cánh tay hình bình hành và tấm đế di động
𝑠𝐵 chiều dài cạnh tam giác đều tấm đế cố định
𝑤𝐵 khoảng cách từ tâm 𝑂 đến cạnh của tấm đế cố định
𝑢𝐵 khoảng cách từ tâm 𝑂 đến đỉnh của tấm đế cố định
𝑠𝑃 chiều dài cạnh tam giác đều tấm đế di động
𝑤𝑃 khoảng cách từ tâm 𝑃 đến cạnh của tấm đế di động
𝑢𝑃 khoảng cách từ tâm 𝑃 đến đỉnh 𝑃𝑖 (𝑖 = 1,2,3) của tấm đế di động
𝐿 chiều dài cánh tay 𝐵𝑖𝐴𝑖 (𝑖 = 1,2,3)
𝑙 chiều dài của mỗi cánh tay hình bình hành
ℎ chiều rộng của mỗi cánh tay hình bình hành
DUT.LRCC

Hệ toạ độ {𝐵} gắn với tấm đế cố định và có gốc tọa độ là tâm của tam giác đều tấm
đế cố định. Tương tự, hệ tọa độ {𝑃} gắn vào tấm đế di động và có gốc tọa độ là tâm của
tam giác đều tấm đế di động. Hai hệ tọa độ {𝐵} và {𝑃} luôn luôn cùng phương do đó
ma trận quay [ 𝑅
𝑃
𝐵 ] = 𝐼3. Các biến khớp là Θ = [𝜃1 𝜃2 𝜃3]𝑇
, và biến thể hiện vị trí
của khâu chấp hành cuối 𝑃 trong hệ tọa độ {𝐵} đang xem xét là 𝑂𝑃
⃗⃗⃗⃗⃗ /𝐵 = [𝑥 𝑦 𝑧]𝑇
.
Thiết kế có sự đối xứng cao với ba cánh tay trên có chiều dài 𝐿 và ba cánh tay dưới có
chiều dài 𝑙 (Hình 2.2).
Ta nhận thấy rằng các khớp quay 𝐵𝑖 cố định trong hệ tọa độ {𝐵} và các khớp các-
đăng 𝑃𝑖 cố định trong hệ tọa độ {𝑃}.
Ta có:
𝑂𝐵1
⃗⃗⃗⃗⃗⃗⃗
/𝐵
= [
0
−𝑤𝐵
0
] (2.1)
𝑂𝐵2
⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
=
[
√3
2
𝑤𝐵
1
2
𝑤𝐵
0 ]
(2.2)
𝑂𝐵3
⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
=
[
−
√3
2
𝑤𝐵
1
2
𝑤𝐵
0 ]
(2.3)
𝑃𝑃1
⃗⃗⃗⃗⃗⃗⃗
/𝑃
= [
0
−𝑢𝑃
0
] (2.4)
𝑃𝑃2
⃗⃗⃗⃗⃗⃗⃗
/𝑃
= [
𝑠𝑃
2
𝑤𝑃
0
] (2.5)
𝑃𝑃3
⃗⃗⃗⃗⃗⃗⃗
/𝐵
= [
−
𝑠𝑃
2
𝑤𝑃
0
] (2.6)
DUT.LRCC

Tọa độ các đỉnh tam giác đều tấm đế cố định:
𝑂𝑏1
⃗⃗⃗⃗⃗⃗⃗
/𝐵
= [
𝑠𝐵
2
−𝑤𝐵
0
] (2.7)
𝑂𝑏2
⃗⃗⃗⃗⃗⃗⃗
/𝐵
= [
0
𝑢𝐵
0
] (2.8)
𝑂𝑏3
⃗⃗⃗⃗⃗⃗⃗
/𝐵
= [
−
𝑠𝐵
2
𝑤𝐵
0
] (2.9)
Với:
𝑤𝐵 =
√3
6
𝑠𝐵 𝑢𝐵 =
√3
3
𝑠𝐵 𝑤𝑃 =
√3
6
𝑠𝑃 𝑢𝑃 =
√3
3
𝑠𝑃
2.1.1. Bậc tự do
Phần này chứng minh robot Delta có 3 bậc tự do. Sử dụng công thức tính bậc tự
không gian của Kutzbach, ta có:
𝑀 = 6(𝑁 − 1) − 5𝐽1 − 5𝐽2 − 3𝐽3 (2.10)
Với:
𝑀 là số bậc tự do (dof – degrees-of-freedom)
𝑁 là số khâu, kể cả đế
𝐽1 là các khớp có một bậc tự do (khớp quay hoặc khớp trượt)
𝐽2 là các khớp có hai bậc tự do (khớp các-đăng)
𝐽3 là các khớp có ba bậc tự do (khớp cầu)
Đối với robot delta kiểu ba khớp quay, ta có: 𝑁 = 17, 𝐽1 = 21, 𝐽2 = 0, 𝐽3 = 0. Do
đó: 𝑀 = 6(17 − 1) − 5(21) − 4(0) − 3(0) = −9 bậc tự do.
Như thường lệ, công thức Kutzbach cho ta kết quả sai vì kết quả rõ ràng phải là 3
bậc tự do. Kết quả sai này cho ta dự đoán rằng robot delta là một cấu trúc siêu tĩnh.
Điều này là sai.
DUT.LRCC

Thực tế thì công thức Kutzbach không sử dụng được cho cấu trúc đặc biệt – trong
trường hợp của robot delta, đó là ba cơ cấu hình bình hành. Nếu ta bỏ bớt đi một cạnh
dài ở mỗi cơ cấu hình bình hành (tức là bỏ bớt đi hai khớp quay ở mỗi cơ cấu hình bình
hành) thì robot vẫn hoạt động y hết về mặt động học như robot delta ban đầu. Với
trường hợp tương đương này, công thức Kutzbach cho ta: 𝑁 = 14, 𝐽1 = 15, 𝐽2 = 0,
𝐽3 = 0 và 𝑀 = 6(14 − 1) − 5(15) − 4(0) − 3(0) = 3 bậc tự do.
Đây là kết quả đúng. Có một cách khác để tính số bậc tự do của robot delta, đó là
thay ba cơ cấu hình bình hành bằng ba khâu đơn (tạm gọi là khâu ảo). Trong trường
hợp này, ta vẫn phải xem ở hai đầu của khâu ảo này là các khớp các-đăng. Công thức
Kutzbach cho trường hợp này vẫn cho ta kết quả tương tự: 𝑁 = 8, 𝐽1 = 3, 𝐽2 = 6, 𝐽3 =
0 và 𝑀 = 6(8 − 1) − 5(3) − 4(6) − 3(0) = 3 bậc tự do
2.1.2.Xây dựng phương trình động học
Mục đích của việc xây dựng các phương trình động học là để nắm rõ và sâu hơn khi
viết phương trình điều khiển và mô phỏng robot .
Từ sơ đồ động học ở Hình 2.1, ta có thể biểu diễn tọa độ của khâu chấp hành cuối
(điểm 𝑃) trong hệ tọa độ {𝐵} gắn với khâu chấp hành cuối:
𝑂𝐵𝑖
⃗⃗⃗⃗⃗⃗⃗
/𝐵𝑠
+ 𝐵𝑖𝐴𝑖
⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
+ 𝐴𝑖𝑃𝑖
⃗⃗⃗⃗⃗⃗⃗
/𝐵
= 𝑂𝑃
⃗⃗⃗⃗⃗ /𝐵 + [ 𝑅
𝑃
𝐵 ]. 𝑃𝑃𝑖
⃗⃗⃗⃗⃗⃗
/𝑃
= 𝑂𝑃
⃗⃗⃗⃗⃗ /𝐵 + 𝑃𝑃𝑖
⃗⃗⃗⃗⃗⃗
/𝑃 (2.11)
Vì hướng của hệ tọa độ {𝐵} và {𝑃} là đồng nhất nên ma trận xoay [ 𝑅
𝑃
𝐵 ] = 𝐼3. Ngoài
ra, cấu trúc của robot là đối xứng với độ dài cạnh các hình bình hành 𝑙, ta có thể viết
lại:
𝑙 = ‖𝐴𝑖𝑃𝑖
⃗⃗⃗⃗⃗⃗⃗
/𝐵
‖ = ‖𝑂𝑃
⃗⃗⃗⃗⃗ /𝐵 + 𝑃𝑃𝑖
⃗⃗⃗⃗⃗⃗
/𝑃
− 𝑂𝐵𝑖
⃗⃗⃗⃗⃗⃗⃗
/𝐵
− 𝐵𝑖𝐴𝑖
⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
‖ (2.12)
Để thuận tiện cho việc tính toán, ta bình phương hai vế của phương trình (2.12).
Việc này giúp ta tránh được phép tính căn bậc hai khi tính độ dài của véctơ 𝐴𝑖𝑃𝑖
⃗⃗⃗⃗⃗⃗⃗
/𝐵
:
𝑙2
= ‖𝐴𝑖𝑃𝑖
⃗⃗⃗⃗⃗⃗⃗
/𝐵
‖
2
= ‖𝑂𝑃
⃗⃗⃗⃗⃗ /𝐵 + 𝑃𝑃𝑖
⃗⃗⃗⃗⃗⃗
/𝑃
− 𝑂𝐵𝑖
⃗⃗⃗⃗⃗⃗⃗
/𝐵
− 𝐵𝑖𝐴𝑖
⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
‖
2
(2.13)
Nhắc lại, biến thể hiện vị trí của khâu chấp hành cuối P trong hệ tọa độ Descartes
{𝐵} đang xem xét là 𝑂𝑃
⃗⃗⃗⃗⃗ /𝐵 = [𝑥 𝑦 𝑧]𝑇
. Các véctơ vị trí của các điểm 𝐵𝑖 và 𝑃𝑖 được
cho ở các phương trình từ (2.1) đến (2.6). Véctơ 𝐵𝑖𝐴𝑖
⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
phụ thuộc vào các biến khớp
𝜃 = [𝜃1 𝜃2 𝜃3]𝑇
:
DUT.LRCC

𝐵1𝐴1
⃗⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
= [
0
−𝐿 cos 𝜃1
−𝐿 sin 𝜃1
] (2.14)
𝐵2𝐴2
⃗⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
=
[
√3
2
𝐿 cos 𝜃2
1
2
𝐿 cos 𝜃2
−𝐿 sin 𝜃2 ]
(2.15)
𝐵3𝐴3
⃗⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
=
[
−
√3
2
𝐿 cos 𝜃3
1
2
𝐿 cos 𝜃3
−𝐿 sin 𝜃3 ]
(2.16)
Thay các phương trình từ (2.1) đến (2.6) và (2.14) đến (2.16) vào phương trình
(2.11), với 𝑖 = 1,2,3, ta có:
𝐴1𝑃1
⃗⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
= [
𝑥 + 0 − 0 − 0
𝑦 − 𝑢𝑃 + 𝑤𝐵 + 𝐿 cos 𝜃1
𝑧 + 𝐿 sin 𝜃1
] = [
𝑥
𝑦 + 𝑎 + 𝐿 cos 𝜃1
𝑧 + 𝐿 sin 𝜃1
] (2.17)
𝐴2𝑃2
⃗⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
=
[
𝑥 +
𝑠𝑃
2
−
√3
2
𝑤𝐵 −
√3
2
𝐿 cos 𝜃2
𝑦 + 𝑤𝑃 −
1
2
𝑤𝐵 −
1
2
𝐿 cos 𝜃2
𝑧 + 𝐿 sin 𝜃2 ]
=
[
𝑥 + 𝑏 −
√3
2
𝐿 cos 𝜃2
𝑦 + 𝑐 −
1
2
𝐿 cos 𝜃2
(2.18)
𝐴3𝑃3
⃗⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
=
[
𝑥 −
𝑠𝑃
2
+
√3
2
𝑤𝐵 +
√3
2
𝐿 cos 𝜃3
𝑦 + 𝑤𝑃 −
1
2
𝑤𝐵 −
1
2
𝐿 cos 𝜃3
=
[
𝑥 − 𝑏 +
√3
2
𝐿 cos 𝜃3
𝑦 + 𝑐 −
1
2
𝐿 cos 𝜃3
(2.19)
Với:
𝑎 = −𝑢𝑃 + 𝑤𝐵 𝑏 =
𝑠𝑃
2
−
√3
2
𝑤𝐵
𝑐 = 𝑤𝑃 −
𝑤𝐵
2
DUT.LRCC

Từ các phương trình (2.13), (2.17), (2.18), (2.19), ta có hệ phương trình động học
robot delta kiểu ba khớp quay:
𝑥2
+ (𝑦 + 𝑎 + 𝐿 cos 𝜃1)2
+ (𝑧 + 𝐿 sin 𝜃1)2
= 𝑙2 (2.20)
(𝑥 + 𝑏 −
√3
2
𝐿 cos 𝜃2)
2
+ (𝑦 + 𝑐 −
1
2
𝐿 cos 𝜃2)
2
+ (𝑧 + 𝐿 sin 𝜃2)2
= 𝑙2 (2.21)
(𝑥 − 𝑏 +
√3
2
𝐿 cos 𝜃3)
2
+ (𝑦 + 𝑐 −
1
2
𝐿 cos 𝜃3)
2
+ (𝑧 + 𝐿 sin 𝜃3)2
= 𝑙2 (2.22)
2.2. Động học nghịch
Bài toán động học ngược của robot delta kiểu ba khớp quay được phát biểu: Biết tọa
độ của khâu chấp hành cuối 𝑂𝑃
⃗⃗⃗⃗⃗ /𝐵 = [𝑥 𝑦 𝑧]𝑇
, xác định giá trị của các biến khớp
𝜃 = {𝜃1, 𝜃2, 𝜃3}𝑇
.
2.2.1. Phương pháp giải tích:
Nghiệm bài toán động học ngược robot song song không phức tạp, là các nghiệm
không tầm thường (khác 0) nhưng có thể giải được bằng phương pháp giải tích. Nhìn
vào sơ đồ động học ở Hình 2.1, bài toán động học ngược có thể được giải một cách độc
lập cho mỗi cánh tay RUU. Về mặt hình học, nghiệm bài toán động học ngược của mỗi
cánh tay RUU là giao điểm giữa đường tròn (𝐵𝑖, 𝐿) và mặt cầu (𝑃𝑖, 𝑙). Ta cũng có thể
giải bài toán này bằng phương pháp lượng giác.
Tuy nhiên, trước mắt ta cần tìm nghiệm giải tích của nó. Bằng một số biến đổi đơn
giản, các phương trình động học (2.20), (2.21), (2.22) có thể được viết lại dưới dạng:
𝐸𝑖 cos 𝜃𝑖 + 𝐹𝑖 sin 𝜃𝑖 + 𝐺𝑖 = 0 (2.23)
Với:
𝐸1 = 2𝐿(𝑦 + 𝑎)
𝐹1 = 2𝑧𝐿
𝐺1 = 𝑥2
+ 𝑦2
+ 𝑧2
+ 𝑎2
+ 𝐿2
+ 2𝑦𝑎 − 𝑙2
𝐸2 = −𝐿(√3(𝑥 + 𝑏) + 𝑦 + 𝑐)
𝐹2 = 2𝑧𝐿
𝐺2 = 𝑥2
+ 𝑦2
+ 𝑧2
+ 𝑏2
+ 𝑐2
+ 𝐿2
+ 2(𝑥𝑏 + 𝑦𝑐) − 𝑙2
(2.24)
DUT.LRCC

𝐸3 = 𝐿(√3(𝑥 − 𝑏) − 𝑦 − 𝑐)
𝐹3 = 2𝑧𝐿
𝐺3 = 𝑥2
+ 𝑦2
+ 𝑧2
+ 𝑏2
+ 𝑐2
+ 𝐿2
+ 2(−𝑥𝑏 + 𝑦𝑐) − 𝑙2
Phương trình (2.23) là phương trình động học thường thấy trong các loại robot và
cơ cấu, được giải bằng cách dùng công thức tang góc chia đôi (tangent half-angle
substitution).
Nếu ta đặt 𝑡𝑖 = tan
𝜃𝑖
2
, khi đó: cos 𝜃𝑖 =
1−𝑡𝑖
2
1+𝑡𝑖
2 và sin 𝜃𝑖 =
2𝑡𝑖
1+𝑡𝑖
2. Thay vào phương
trình (2.23), sau một vài phép biến đổi đơn giản, ta có :
(𝐺𝑖 − 𝐸𝑖)𝑡𝑖
2
+ (2𝐹𝑖)𝑡𝑖 + (𝐺𝑖 + 𝐸𝑖) = 0 (2.25)
Nghiệm của phương trình bậc hai trên là:
𝑡𝑖1,2
=
−𝐹𝑖 ± √𝐸𝑖
2
+ 𝐹𝑖
2
− 𝐺𝑖
2
𝐺𝑖 − 𝐸𝑖
(2.26)
Để tìm 𝜃𝑖, ta nghịch đảo công thức góc tang chia đôi:
𝜃𝑖 = 2 tan−1
(𝑡𝑖) (2.27)
Hai nghiệm 𝜃𝑖 tương ứng với dấu ± trong biểu thức (2.27). Cả hai nghiệm đều đúng
bởi vì hai vị trí của mỗi cánh tay RUU tương ứng với giá trị của góc 𝜃𝑖 đều hợp lệ. Như
vậy, ta có tổng cộng tám nghiệm khả dĩ (𝐶2
1
× 𝐶2
1
× 𝐶2
1
). Thông thường, chỉ một
nghiệm mà ở đó tất cả các khớp các-đăng 𝐴𝑖 của robot delta quay ra thay vì quay vào
trong sẽ được chọn (Hình 2.3a).
Hình 2. 1 Tám nhiệm khả dĩ của phương trình động học ngược
DUT.LRCC

2.2.2. Phương pháp số Newton-Raphson:
Việc giải bài toán động học ngược bằng phép giải tích cho ta kết quả nhanh, tuy
nhiên ta cần phải chọn nghiệm trong bộ tám nghiệm khả dĩ bởi hệ phương trình động
học liên quan đến các hàm lượng giác. Để tránh vấn đề này ta có thể dùng các phương
pháp số.
Từ phương trình (2.29,[1]) tính được vận tốc khớp suy rộng:
𝒒̇ = −𝑱𝑞
−1
𝑱𝑥ẋ (2.28)
Từ phương trình (2.30,[1]) tính được gia tốc khớp suy rộng:
𝒒̈ = −𝑱𝑞
−1
(𝑱𝑞
̇ 𝒒̇ + 𝑱𝑥
̇ 𝒙̇ + 𝑱𝑥𝒙̈) (2.29)
Giả sử, ta đã biết được quy luật chuyển động của khâu thao tác 𝑥(𝑡), ta sẽ xác định
được 𝒒(𝑡), 𝒒̇ (𝑡), 𝒒̈ (𝑡) tại thời điểm khảo sát.
Xét khoảng thời gian làm việc từ 𝑡 = 0[𝑠] đến 𝑡 = 𝑇[𝑠]. Chia khoảng thời gian
[0, 𝑇] làm 𝑁 khoảng bằng nhau. Vậy thời gian mỗi khoảng là:
∆𝑡 =
𝑇
𝑁
(2.30)
Hai thời điểm khảo sát liên tiếp nhau có quan hệ:
𝑡𝑘+1 = 𝑡𝑘 + ∆𝑡 với 𝑘 = 0 ÷ (𝑁 − 1) (2.31)
Áp dụng khai triển Taylor hàm véctơ 𝒒(𝑡) ở lân cận giá trị 𝑡 = 𝑡𝑘, ta được:
𝒒(𝑡𝑘+1) = 𝒒(𝑡𝑘 + ∆𝑡) = 𝒒(𝑡𝑘) + 𝒒̇ (𝑡𝑘)∆𝑡 +
1
2
𝒒̈ (𝑡𝑘)(∆𝑡)2 (2.32)
Thay biểu thức và bỏ qua các vô cùng bé bậc lớn hơn hoặc bằng 2, ta được:
𝒒(𝑡𝑘+1) = 𝒒(𝑡𝑘) + 𝒒̇ (𝑡𝑘)∆𝑡 ≈ 𝒒(𝑡𝑘) − 𝑱𝑞
−1
(𝒒(𝑡𝑘))𝑱𝑥(𝒒(𝑡𝑘))𝒙̇ (𝑡𝑘)∆𝑡 (2.33)
Để thuận tiện, ta dùng các kí hiệu rút gọn:
𝒒𝑘 = 𝒒(𝑡𝑘), 𝒒̇ 𝑘 = 𝒒̇ (𝑡𝑘), 𝒒̈ 𝑘 = 𝒒̈ (𝑡𝑘)
𝒙𝑘 = 𝒙(𝑡𝑘), 𝒙̇𝑘 = 𝒙̇ (𝑡𝑘), 𝒙̈ 𝑘 = 𝒙̈(𝑡𝑘)
Việc xác định 𝒒(𝑡) tại thời điểm 𝑡 = 𝑡𝑘+1 được thực hiện qua hai bước:
Bước 1: Hiệu chỉnh gia lượng các tọa độ khớp tại thời điểm 𝑡0
DUT.LRCC

Đầu tiên ta xác định véctơ gần đúng ban đầu 𝒒
̃0 của véctơ 𝒒0 bằng phương pháp vẽ
hoặc bằng thực nghiệm.
Ta có:
𝒒0 = 𝒒
̃0 + ∆𝒒0 (2.34)
Áp dụng khai triển Taylor đến bậc 1 ta được:
𝒇(𝒒0, 𝒙0) = 𝒇(𝒒
̃0 + ∆𝒒0, 𝒙0) ≈ 𝒇(𝒒
̃0, 𝒙0) +
𝜕𝒇
𝜕𝒒
(𝒒
̃0, 𝒙0)∆𝒒0 (2.35)
Suy ra:
𝒇(𝒒
̃0, 𝒙0) + 𝑱𝑞(𝒒
̃0, 𝒙0)∆𝒒0 ≈ 0 (2.36)
Từ đó:
∆𝒒𝟎 = −𝑱𝒒
−𝟏
(𝒒
̃𝟎, 𝒙𝟎)𝒇(𝒒
̃𝟎, 𝒙𝟎) (2.37)
Sau đó lấy:
𝒒
̃0 = 𝒒
̃0 + ∆𝒒0 (2.38)
Nếu ‖∆𝒒0‖ ≥ 𝜀 (với 𝜀 là tham số dương bé cho trước) thì ta lại thế (2.38) vào
(2.37) cho đến khi ‖∆𝒒0‖ < 𝜀. Kết quả ta được:
𝒒0 = 𝒒
̃0 (2.39)
Sau khi tìm được xấp xỉ của 𝒒0 với sai số 𝜀 cho trước, ta thay vào các công thức
(2.28) và (2.29) để tìm 𝒒̇ 0 và 𝒒̈ 0.
Bước 2: Hiệu chỉnh gia lượng véctơ tọa độ suy rộng tại thời điểm 𝑡𝑘+1
Giả sử đã biết 𝒒𝑘, 𝒒̇ 𝑘, 𝒒̈ 𝑘, ta cần tìm giá trị 𝒒(𝑡) tại thời điểm 𝑡𝑘+1 = 𝑡𝑘 + ∆t.
Trước hết, ta lấy gần đúng 𝒒
̃𝑘+1 bằng xấp xỉ theo (2.33):
𝒒
̃𝑘+1 = 𝒒𝑘 + 𝒒̇ 𝑘∆𝑡 (2.40)
Sau đó xác định véctơ chính xác hơn của 𝒒
̃𝑘+1 theo công thức hiệu chỉnh:
𝒒𝑘+1 = 𝒒
̃𝑘+1 + ∆𝒒𝑘+1 (2.41)
Tương tự Bước 1 ta cũng có xấp xỉ:
∆𝒒𝑘+1 = −𝑱𝑞
−1
(𝒒
̃𝑘+1, 𝒙𝑘+1)𝒇(𝒒
̃𝑘+1, 𝒙𝑘+1) (2.42)
Sau đó lấy:
DUT.LRCC

𝒒
̃𝑘+1 = 𝒒
̃𝑘+1 + ∆𝒒𝑘+1 (2.43)
Nếu ‖∆𝒒𝑘+1‖ ≥ 𝜀 thì thay (2.43) vào (2.42) cho đến khi ‖∆𝒒𝑘+1‖ < 𝜀. Kết quả ta
được:
𝒒𝑘+1 = 𝒒
̃𝑘+1 (2.44)
Sau khi tìm được 𝒒𝑘+1, thay vào công thức (2.28) và (2.29) tìm được 𝒒̇ 𝑘+1 và 𝒒̈ 𝑘+1.
2.3. Động học thuận
Bài toán động học thuận của robot Delta kiểu ba khớp quay được phát biểu: Biết giá
trị của các biến khớp khớp 𝜃 = [𝜃1 𝜃2 𝜃3]𝑇
, xác định tọa độ của khâu chấp hành
cuối 𝑃
𝐵
𝑃 = {𝑥, 𝑦, 𝑧}𝑇
. 𝑂𝑃
⃗⃗⃗⃗⃗ /𝐵 = [𝑥 𝑦 𝑧]𝑇
. Bài toán động học thuận cho robot song
song nói chung rất khó để giải, vì chúng ta cần phải tìm nghiệm của hệ ba phương trình
đại số phi tuyến (2.20), (2.21), (2.22). Hệ này cho ta nhiều nghiệm hợp lệ.
Phương pháp hình học
Với 𝜃 = [𝜃1 𝜃2 𝜃3]𝑇
đã biết, ta có thể xác định được tọa độ ba khớp các-đăng 𝐴𝑖
trong hệ tọa độ {𝐵} theo công thức: 𝑂𝐴𝑖
⃗⃗⃗⃗⃗⃗⃗
/𝐵
= 𝑂𝐵𝑖
⃗⃗⃗⃗⃗⃗⃗
/𝐵
+ 𝐵𝑖𝐴𝑖
⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
với 𝑖 = 1,2,3:
𝑂𝐴1
⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
= [
0
−𝑤𝐵 − 𝐿 cos 𝜃1
−𝐿 sin 𝜃1
] (2.45)
𝑂𝐴2
⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
=
[
√3
2
(𝑤𝐵 + 𝐿 cos 𝜃2)
1
2
−𝐿 sin 𝜃2 ]
(2.46)
𝑂𝐴3
⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
=
[
−
√3
2
1
2
−𝐿 sin 𝜃3 ]
(2.47)
Từ giản đồ động học thuận robot ở Hình 2.5 và biết rằng hướng của tấm đế di động
là không đổi (luôn luôn nằm ngang) với [ 𝑅
𝑃
𝐵 ] = 𝐼3, ta có thể định nghĩa ba tâm mặt cầu
ảo 𝑂𝐴𝑖𝑣
⃗⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
= 𝑂𝐴𝑖
⃗⃗⃗⃗⃗⃗⃗
/𝐵
− 𝑃𝑃𝑖
⃗⃗⃗⃗⃗⃗
/𝑃
với 𝑖 = 1,2,3:
DUT.LRCC

Nghiệm bài toán động học thuận (điểm 𝑃) là giao điểm của ba mặt cầu (𝐴1𝑣, 𝑙),
(𝐴1𝑣, 𝑙) và (𝐴1𝑣, 𝑙).
Hình 2. 2 Sơ đồ động học thuận
𝑂𝐴1𝑣
⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
= [
0
−𝑤𝐵 − 𝐿 cos 𝜃1 + 𝑢𝑃
−𝐿 sin 𝜃1
] (2.48)
𝑂𝐴2𝑣
⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
=
[
√3
2
(𝑤𝐵 + 𝐿 cos 𝜃2) −
𝑠𝑃
2
1
2
(𝑤𝐵 + 𝐿 cos 𝜃2) − 𝑤𝑃
−𝐿 sin 𝜃2 ]
(2.49)
𝑂𝐴3𝑣
⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗
/𝐵
=
[
−
√3
2
(𝑤𝐵 + 𝐿 cos 𝜃3) +
𝑠𝑃
2
1
2
(𝑤𝐵 + 𝐿 cos 𝜃3) − 𝑤𝑃
−𝐿 sin 𝜃3 ]
(2.50)
DUT.LRCC

Bây giờ chúng ta sẽ tìm các nghiệm là giao điểm của ba mặt cầu. Cách giải này
được áp dụng cho nhiều loại robot cáp treo (cable-suspended robot) và các loại robot
song song khác đã được đăng trước đó. Giả thuyết có ba mặt cầu
(𝒄1, 𝑟1), (𝒄2, 𝑟2), (𝒄3, 𝑟3). Tâm xác định bằng các véctơ: 𝒄1 = {𝑥1, 𝑦1, 𝑧1}𝑇
, 𝒄2 =
{𝑥2, 𝑦2, 𝑧2}𝑇
, 𝒄3 = {𝑥3, 𝑦3, 𝑧3}𝑇
, bán kính 𝑟1, 𝑟2, 𝑟3 đã biết (tọa độ véctơ các tâm phải
được thể hiện trên cùng một hệ tọa độ, gốc {0}; kết quả trên cùng một hệ tọa độ).
Phương trình ba mặt cầu:
(𝑥 − 𝑥1)2
+ (𝑦 − 𝑦1)2
+ (𝑧 − 𝑧1)2
= 𝑟1
2
(2.5
1)
(𝑥 − 𝑥2)2
+ (𝑦 − 𝑦2)2
+ (𝑧 − 𝑧2)2
= 𝑟2
2
(𝑥 − 𝑥3)2
+ (𝑦 − 𝑦3)2
+ (𝑧 − 𝑧3)2
= 𝑟3
2
Các phương trình trên đều là các phương trình phi tuyến với ba biến chưa biết x,y,z.
Nghiệm bài toán sẽ xác định tọa độ của điểm giao nhau 𝑷 = {𝑥 𝑦 𝑧}𝑇
. Bằng cách khai
triển các phương trình (2.34) sau đó kết hợp lại theo dạng 𝑥 = 𝑓(𝑦), 𝑧 = 𝑓(𝑦); sau đó
thay thế các hàm này vào một trong những phương trình mặt cầu thuần túy ta được
phương trình bậc 2 theo 𝑦, giải phương trình này được hai nghiệm. Sau đó lại sử dụng
các phương trình 𝑥 = 𝑓(𝑦), 𝑧 = 𝑓(𝑦) để xác định các biến 𝑥, 𝑧.
Đầu tiên ta khai triển bậc hai vế bên trái các phương trình (2.34). Sau đó trừ phương
trình ba cho một và ba cho hai ta được:
𝑎11𝑥 + 𝑎12𝑦 + 𝑎13𝑧 = 𝑏1 (2.52)
𝑎21𝑥 + 𝑎22𝑦 + 𝑎23𝑧 = 𝑏2 (2.53)
Với:
𝑎11 = 2(𝑥3 − 𝑥1) 𝑎21 = 2(𝑥3 − 𝑥2)
𝑎12 = 2(𝑦3 − 𝑦1) 𝑎22 = 2(𝑦3 − 𝑦2)
𝑎13 = 2(𝑧3 − 𝑧1) 𝑎23 = 2(𝑧3 − 𝑧2)
𝑏1 = 𝑟1
2
− 𝑟3
2
− 𝑥1
2
− 𝑦1
2
− 𝑧1
2
+ 𝑥3
2
+ 𝑦3
2
+ 𝑧3
2
𝑏2 = 𝑟2
2
− 𝑟3
2
− 𝑥2
2
− 𝑦2
2
− 𝑧2
2
+ 𝑥3
2
+ 𝑦3
2
+ 𝑧3
2
Tìm z từ (2.35), (2.36):
𝑧 =
𝑏1
𝑎13
−
𝑎11
𝑎13
𝑥 −
𝑎12
𝑎13
𝑦 (2.54)
DUT.LRCC

𝑧 =
𝑏2
𝑎23
−
𝑎21
𝑎23
𝑥 −
𝑎22
𝑎23
𝑦 (2.55)
Trừ phương trình (2.54) cho (2.55) và đưa về phương trình x=f(y):
𝑥 = 𝑓(𝑦) = 𝑎4𝑦 + 𝑎5 (2.56)
Với:
𝑎4 = −
𝑎2
𝑎1
𝑎5 = −
𝑎3
𝑎1
𝑎1 =
𝑎11
𝑎13
−
𝑎21
𝑎23
𝑎2 =
𝑎12
𝑎13
−
𝑎22
𝑎23
𝑎3 =
𝑏2
𝑎23
−
𝑏1
𝑎13
Thay phương trình (2.56) vào phương trình (2.55) đưa về phương trình z=f(y):
𝑧 = 𝑓(𝑦) = 𝑎6𝑦 + 𝑎7 (2.57)
Với:
𝑎6 =
−𝑎21𝑎4 − 𝑎22
𝑎23
𝑎7 =
𝑏2 − 𝑎21𝑎5
𝑎23
Thay phương trình (2.56) và (27) vào phương trình đầu tiên của hệ phương trình
(2.52) ta được phương trình theo y:
𝑎𝑦2
+ 𝑏𝑦 + 𝑐 = 0 (2.58)
Với:
𝑎 = 𝑎4
2
+ 1 + 𝑎6
2
𝑏 = 2𝑎4(𝑎5 − 𝑥1) − 2𝑦1 + 2𝑎6(𝑎7 − 𝑧1)
𝑐 = 𝑎5(𝑎5 − 2𝑥1) + 𝑎7(𝑎7 − 2𝑧1) + 𝑥1
2
+ 𝑦1
2
+ 𝑧1
2
− 𝑟1
2
Giải phương trình này được hai nghiệm y:
DUT.LRCC

𝑦± =
−𝑏 ± √𝑏2 − 4𝑎𝑐
2𝑎
(2.59)
Thay hai giá trị của y là 𝑦+ và 𝑦− vào các phương trình (2.56) và (2.57) ta tìm được
x và z:
𝑥± = 𝑎4𝑦± + 𝑎5 (2.60)
𝑧± = 𝑎6𝑦± + 𝑎7 (2.61)
Thông thường có hai nghiệm, một nghiệm âm và một nghiệm dương. Hiển nhiên,
các nghiệm tương ứng + và – không thể bị switched:
{𝑥+ 𝑦+ 𝑧+}𝑇
{𝑥− 𝑦− 𝑧−}𝑇
(2.62)
 Nghiệm ảo:
Thuật toán tìm giao điểm ba mặt cầu có thể cho ra nhiều nghiệm ảo. Xuất hiện
nghiệm ảo khi biểu thức 𝑏2
− 4𝑎𝑐 có giá trị âm; khi đó có hai nghiệm ảo 𝑦±, điều này
có nghĩa là không có giao điểm nào giữa ba mặt cầu. Nếu trường hợp này xuất hiện
trong phần lập trình, sẽ có một góc tại khớp gây lỗi hoặc mô hình hóa lỗi, bởi vì phần
lập trình nên đảm bảo robot có thể lắp rắp được.
Một trường hợp đặc biệt xuất hiện khi biểu thức 𝑏2
− 4𝑎𝑐 bằng 0. Khi đó, cả hai
nghiệm suy biến thành một nghiệm đơn, có 2 mặt cầu sẽ tiếp xúc nhau tại một điểm, và
mặt cầu thứ ba sẽ đi qua điểm này.
 Điểm kỳ dị:
Trong toán học, các điểm kỳ dị xuất hiện khi chia cho 0, nhưng không gây cản trở
cho phần lập trình (không làm mất hoặc tăng số bậc tự do). Mục này phân tích các
điểm kỳ dị trong thuật toán tìm giao điểm của ba mặt cầu đã trình bày ở trước. Từ việc
phân tích và giải để tìm ra y, sẽ xuất hiện một tập các điểm kỳ dị.
Xét thuật toán đã được giới thiệu từ các phương trình trước ta tìm được 4 điều kiện
sinh ra các điểm kỳ dị từ các biểu thức chia cho 0:
Điều kiện xác định các điểm kỳ dị:
𝑎13 = 0
𝑎23 = 0 (2.63)
𝑎1 = 0
𝑎4
2
+ 1 + 𝑎6
2
= 0
DUT.LRCC

 Hai điều kiện đầu:
𝑎13 = 2(𝑧3 − 𝑧1) = 0 (2.64)
𝑎23 = 2(𝑧3 − 𝑧2) = 0 (2.65)
Trường hợp này tương ứng với tâm của ba mặt cầu có cùng độ cao z: 𝑧1 = 𝑧2 = 𝑧3.
Do đó, thông thường khi giao điểm của ba mặt cầu mà tâm có cùng độ cao z thì thường
xuất hiện điểm kỳ dị. Một cách giải khác được giới thiệu ở phụ lục B có thể khắc phục
vấn đề này.
 Điều kiện thứ ba:
𝑎1 =
𝑎11
𝑎13
−
𝑎21
𝑎23
= 0 (2.66)
Hay:
𝑥3 − 𝑥1
𝑧3 − 𝑧1
=
𝑥3 − 𝑥2
𝑧3 − 𝑧2
(2.67)
Điều kiện này được thỏa mãn khi tâm của ba mặt cầu cùng nằm trên một đường
thẳng thuộc mặt phẳng XZ. Thông thường điều kiện thứ ba sinh ra các điểm kỳ dị nằm
trên biên của vùng làm việc dùng được và như vậy sẽ không gây ra vấn đề gì đến việc
thực thi của phần mềm nếu hệ thống được thiết kế tuân theo các giới hạn của vùng làm
việc.
 Điều kiện thứ 4:
𝑎4
2
+ 1 + 𝑎6
2
= 0
Hay:
𝑎4
2
+ 𝑎6
2
= −1
Điều kiện này không thể thỏa mãn nếu 𝑎4 và 𝑎6 là các số thực, do đó điều kiện thứ
tư không ảnh hưởng đến việc thực thi của phần mềm.
Đa nghiệm:
Thuật toán tìm giao điểm của ba mặt cầu thông thường cho ra hai nghiệm đúng
riêng biệt (tương ứng dấu ±). Thông thường chỉ một nghiệm phù hợp, được xác định
phụ thuộc kết cấu của Robot Delta có thể lắp rắp được.
DUT.LRCC

2.4. Bài toán động lực học
2.4.1. Xác định vận tốc khối tâm và vận tốc góc các khâu:
Ngoài hệ tọa độ cố định {𝐵} gắn với tấm đế cố định, ta cần thiết lập thêm các hệ tọa
độ địa phương gắn cứng vào các khâu chủ động và bị động để xác định vị trí các khâu
so với hệ {𝐵}.
Đầu tiên, ta cần xác định vị trí các khớp chủ động so với hệ tọa độ gốc. Ta gắn ba
hệ toạ độ cố định {𝐵𝑖} = 𝐵𝑖𝑥𝑖𝑦𝑖𝑧𝑖 với 𝑖 = 1,2,3 bằng cách biến đổi hệ tọa độ gốc qua
hai phép: tịnh tiến gốc 𝑂 về 𝐵𝑖 và xoay quanh trục 𝑧𝐵 một góc 𝛼𝑖 (với 𝛼1 =
−90°, 𝛼2 = 30°, 𝛼3 = 150°).
Hình 2. 3 Các hệ tọa độ địa phương gắn cứng vào các khâu chủ động
Ma trận cosin chỉ hướng của hệ {𝐵𝑖} so với {𝐵}:
𝐴𝑍(𝛼𝑖) = [
cos𝛼𝑖 −𝑠𝑖𝑛𝛼𝑖 0
𝑠𝑖𝑛𝛼𝑖 𝑐𝑜𝑠𝛼𝑖 0
0 0 1
]
Gọi 𝐶1𝑖 là khối tâm của khâu 𝐵𝑖𝐴𝑖. Ta gắn cứng hệ tọa độ động {𝐶1𝑖} = 𝐶1𝑖𝑋1𝑖𝑌1𝑖𝑍1𝑖
vào khâu 𝐵𝑖𝐴𝑖 sao cho 𝐵𝑖𝐴𝑖 luôn nằm trên trục 𝑋1𝑖. {𝐶1𝑖} được xác định bằng cách biến
đổi hệ {𝐵𝑖} bằng 2 phép: tịnh tiến về gốc 𝐶1𝑖 và quay quanh trục 𝑌𝑖 một góc 𝜃𝑖. Ta có:
DUT.LRCC

𝑪
𝐵𝑖
1𝑖 =
[
𝐿
2
𝑐𝑜𝑠𝜃𝑖
0
−
𝐿
2
𝑠𝑖𝑛𝜃𝑖]
𝑪
𝐵
1𝑖 = 𝐵1
𝐵
+ 𝐴𝑍(𝛼𝑖) 𝑪
𝐵𝑖
1𝑖
= [
𝑤𝑏. 𝑐𝑜𝑠𝛼𝑖
𝑤𝑏. 𝑠𝑖𝑛𝛼𝑖
0
] + [
cos𝛼𝑖 −𝑠𝑖𝑛𝛼𝑖 0
𝑠𝑖𝑛𝛼𝑖 𝑐𝑜𝑠𝛼𝑖 0
0 0 1
]
[
𝐿
2
𝑐𝑜𝑠𝜃𝑖
0
−
𝐿
2
𝑠𝑖𝑛𝜃𝑖]
=
[
𝑤𝑏𝑐𝑜𝑠𝛼𝑖 +
𝐿
2
𝑐𝑜𝑠𝛼𝑖𝑐𝑜𝑠𝜃𝑖
𝑤𝑏𝑠𝑖𝑛𝛼𝑖 +
𝐿
2
𝑠𝑖𝑛𝛼𝑖𝑐𝑜𝑠𝜃𝑖
−
𝐿
2
𝑠𝑖𝑛𝜃𝑖 ]
𝑪
𝑩
11 =
[
0
−𝑤𝐵 −
𝐿
2
cos 𝜃1
−
𝐿
2
sin 𝜃1 ]
𝑪
𝑩
12 =
[
√3
2
(𝑤𝐵 +
𝐿
2
cos 𝜃2)
1
2
(𝑤𝐵 +
𝐿
2
cos 𝜃2)
−
𝐿
2
sin 𝜃2 ]
𝑪
𝑩
13 =
[
−
√3
2
(𝑤𝐵 +
𝐿
2
cos 𝜃3)
1
2
(𝑤𝐵 +
𝐿
2
cos 𝜃3)
−
𝐿
2
sin 𝜃3 ]
Vì khối lượng các thanh hình bình hành thường nhỏ hơn các khâu còn lại nên để
đơn giản hóa quá trình tính toán, ta quy khối lượng khâu bị động 𝐴𝑖𝑃𝑖 về hai đầu khớp,
khối lượng sẽ tập trung tại 𝐶2𝑎𝑖 ≡ 𝐴𝑖, 𝐶2𝑏𝑖 ≡ 𝑃𝑖. Với việc quy khối lượng về hai đầu
khớp, ta bỏ qua vận tốc góc và tenxơ quán tính của khâu bi động khiến bài toán đơn
giản hơn rất nhiều.
Tọa độ các véctơ 𝑪
𝐵
2𝑎𝑖 và 𝑪
𝐵
2𝑏𝑖 với 𝑖 = 1,2,3:
DUT.LRCC

𝑪
𝐵
2𝑎𝑖 = 𝐵1
𝐵
+ 𝐴𝑍(𝛼𝑖) 𝑪
𝐵𝑖
2𝑎𝑖
𝑪
𝑩
2𝑎1 = [
0
−𝑤𝐵 − 𝐿 cos 𝜃1
−𝐿 sin 𝜃1
]
𝑪
𝑩
2𝑎2 =
[
√3
2
1
2
−𝐿 sin 𝜃2 ]
𝑪
𝑩
2𝑎3 =
[
−
√3
2
1
2
−𝐿 sin 𝜃3 ]
𝑪
𝐵
2𝑏𝑖 = 𝑷
𝐵
+ 𝑷𝑖
𝑃
𝑪
𝑩
2𝑏1 = 𝑷
𝐵
+ 𝑷1
𝑃
= [
𝑥
𝑦 − 𝑢𝑝
𝑧
]
𝑪
𝑩
2𝑏2 = 𝑷
𝐵
+ 𝑷2 = [
𝑥 +
𝑠𝑝
2
𝑦 + 𝑤𝑝
𝑧
]
𝑃
𝑪
𝑩
2𝑏3 = 𝑷
𝐵
+ 𝑷3 = [
𝑥 −
𝑠𝑝
2
𝑦 + 𝑤𝑝
𝑧
]
𝑃
Vận tốc góc khâu 𝐵𝑖𝐴𝑖 trong hệ {𝐶1𝑖}:
𝝎1 = [
0
𝜃̇1
0
]
𝐶11 , 𝝎2 = [
0
𝜃̇2
0
]
𝐶12 , 𝝎3 = [
0
𝜃̇3
0
]
𝐶13
2.4.2. Xác định phương trình Lagrande dạng nhân tử cho hệ p vật rắn
𝐌(𝐬)𝐬̈ + 𝐂(𝐬, 𝐬̇)𝐬̇ + 𝐠(𝐬) + 𝐉𝑠
𝑇(𝐬)𝛌 = 𝐐𝑛𝑝 (2.68)
Trong đó: 𝐌(𝐬) là ma trận khối lượng suy rộng 6 × 6
𝐂(𝐬, 𝐬̇) là ma trận quán tính và Coriolis 6 × 6
DUT.LRCC

𝐂(𝐬, 𝐬̇) =
𝑑𝐌(𝐬)
𝑑𝑡
−
1
2
(
𝜕(𝐌(𝐬)𝐬̇)
𝜕𝐬
)
𝑇
𝐠(𝐬) là ma trận do trọng trường gây ra 6 × 1
𝐠(𝐬) = (
𝜕𝛱
𝜕𝐬
)
𝑇
𝐉𝑠 là ma trận jacobian của tọa độ suy rộng đầy đủ 3 × 6
𝛌 là vector các nhân tử Lagrande 3 × 1
𝐐𝑛𝑝
vector chứa lực suy rộng của các lực không thế 6 × 1
2.4.3. Tính toán các ma trận
 Ma trận khối lượng M(s)
Coi khối lượng khâu chủ động và khâu bị động ở cả ba chân bằng nhau. Gọi khối
lượng của khâu chủ động và bị động lần lượt là 𝒎𝟏, 𝒎𝟐. Gọi khối lượng tấm đế di
động là 𝒎𝑷. Các khâu chủ động xét đến tenxơ quán tính đối với khối tâm 𝑪𝟏𝒊 trong hệ
{𝑪𝟏𝒊}, để đơn giản ta đã bỏ qua tenxơ quán tính khâu bị động.
𝐈11
𝐶11 = 𝐈12
𝐶12 = 𝐈13
𝐶13 = [
𝐼𝑥 0 0
0 𝐼𝑦 0
0 0 𝐼𝑧
]
𝐼𝑥 = 0; 𝐼𝑦 = 𝐼𝑧 =
𝑚1𝐿2
12
Các ma trận Jacobian tịnh tiến và xoay:
𝐉𝑇11
=
𝜕𝐂11
𝜕𝐬
=
[
0 0 0 0 0 0
1
2
𝐿𝑠𝑖𝑛𝜃1 0 0 0 0 0
−
1
2
𝐿𝑐𝑜𝑠𝜃1 0 0 0 0 0]
𝐉𝑇12
=
𝜕𝐂12
𝜕𝐬
=
[
0 −
√3
4
𝐿𝑠𝑖𝑛𝜃2 0 0 0 0
0 −
1
4
𝐿𝑠𝑖𝑛𝜃2 0 0 0 0
0 −
1
2
𝐿𝑐𝑜𝑠𝜃2 0 0 0 0]
DUT.LRCC

𝐉𝑇13
=
𝜕𝐂13
𝜕𝐬
=
[
0 0
√3
4
𝐿𝑠𝑖𝑛𝜃3 0 0 0
0 0 −
1
4
0 0 −
1
2
𝐿𝑐𝑜𝑠𝜃3 0 0 0]
𝐉𝑇2𝑎1
=
𝜕𝐂2𝑎1
𝜕𝐬
= [
0 0 0 0 0 0
𝐿𝑠𝑖𝑛𝜃1 0 0 0 0 0
−𝐿𝑐𝑜𝑠𝜃1 0 0 0 0 0
]
𝐉𝑇2𝑎2
=
𝜕𝐂2𝑎2
𝜕𝐬
=
[
0 −
√3
2
𝐿𝑠𝑖𝑛𝜃2 0 0 0 0
0 −
1
2
𝐿𝑠𝑖𝑛𝜃2 0 0 0 0
0 −𝐿𝑐𝑜𝑠𝜃2 0 0 0 0]
𝐉𝑇2𝑎3
=
𝜕𝐂2𝑎3
𝜕𝐬
=
[
0 0
√3
2
0 0 −
1
2
0 0 −𝐿𝑐𝑜𝑠𝜃3 0 0 0]
𝐉𝑇2𝑏1
= 𝐉𝑇2𝑏2
= 𝐉𝑇2𝑏3
= 𝐉𝑇𝑃
= [
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1
]
𝐉𝑅11
11
=
𝜕
𝐶11
𝛚11
𝜕𝐬̇
= [
0 0 0 0 0 0
1 0 0 0 0 0
0 0 0 0 0 0
]
𝐉𝑅12
12
= [
0 0 0 0 0 0
0 1 0 0 0 0
0 0 0 0 0 0
]
𝐉𝑅12
12
= [
0 0 0 0 0 0
0 0 1 0 0 0
0 0 0 0 0 0
]
DUT.LRCC

𝐌(𝐬) = ∑(𝑚𝑘𝐉𝑇𝑘
𝑇
𝐉𝑇𝑘
+ 𝐉𝑅𝑘
𝑇
𝐈𝑘𝐉𝑅𝑘
)
𝑝
𝑘=1
= ∑ (𝑚1𝐉𝑇1𝑖
𝑇
𝐉𝑇𝐶1𝑖
+
1
2
𝑚2(𝐉𝑇2𝑎𝑖
𝑇
𝐉𝑇𝐶2𝑎𝑖
+ 𝐉𝑇2𝑏𝑖
𝑇
𝐉𝑇2𝑏𝑖
)
3
𝑖=1
+ 𝐉𝑅1𝑖
(1𝑖)𝑇
𝐈1𝑖
(1𝑖)
𝐉𝑅1𝑖
(1𝑖)
) + 𝑚𝑃𝐉𝑇𝑃
𝑇
𝐉𝑇𝑃
(2.69)
Thay vào (2.52), ta được một ma trận chỉ có các phần tử trên đường chéo,các phần
tử còn lại bằng 0.
𝑚11 = 𝑚22 = 𝑚33 = 𝐼𝑦 +
𝐿2
4
(𝑚1 + 2𝑚2)
𝑚44 = 𝑚55 = 𝑚66 =
3
2
𝑚2 + 𝑚𝑃
 Ma trận quán tính và Coriolis :
𝐂(𝐬, 𝐬̇) =
𝑑𝐌(𝐬)
𝑑𝑡
−
1
2
(
𝜕(𝐌(𝐬)𝐬̇)
𝜕𝐬
)
𝑇
𝐂(𝐬, 𝐬̇) = 𝟎
 Ma trận 𝐠(𝐬)
Thế năng của robot Delta:
𝛱 = − ∑ [
1
2
𝑚1𝑔𝐿𝑠𝑖𝑛𝜃𝑖 +
1
2
𝑚2𝑔𝐿𝑠𝑖𝑛𝜃𝑖] + (𝑚𝑃 +
3
2
𝑚2)𝑔𝑧
3
𝑖=1
𝐠(𝐬) = (
𝜕𝛱
𝜕𝐬
)
𝑇
=
[
−
1
2
(𝑚1𝑔𝐿 + 𝑚2𝑔𝐿)cos𝜃1
−
1
2
−
1
2
0
0
(
3
2
𝑚2 + 𝑚𝑃) 𝑔 ]
DUT.LRCC

 Ma trận Jacobian các tọa độ suy rộng đầy đủ
𝐉𝑠 =
𝜕𝐟
𝜕𝐬
= [𝐉q 𝐉x]
𝑱𝑞 = [
𝐽𝑞11 0 0
0 𝐽𝑞22 0
0 0 𝐽𝑞33
]
𝑱𝑞11 = 2𝐿[𝑧 𝑐𝑜𝑠 𝜃1 − (𝑦 + 𝑎) 𝑠𝑖𝑛 𝜃1]
𝑱𝑞22 = 2𝐿[𝑧 𝑐𝑜𝑠 𝜃2 + (
√3
2
𝑥 +
1
2
𝑦 +
√3
2
𝑏 +
1
2
𝑐) 𝑠𝑖𝑛 𝜃2]
𝑱𝑞33 = 2𝐿[𝑧 𝑐𝑜𝑠 𝜃3 + (
−√3
2
𝑥 +
1
2
𝑦 +
√3
2
𝑏 +
1
2
𝑐) 𝑠𝑖𝑛 𝜃3]
𝑱𝑥(𝑠) =
𝜕𝒇
𝜕𝒙
= [𝑱𝑥1 𝑱𝑥2 𝑱𝑥3]
𝑱𝑥1 =
[
2𝑥
2 (𝑥 −
√3
2
𝐿 𝑐𝑜𝑠 𝜃2 + 𝑏)
2 (𝑥 +
√3
2
𝐿 𝑐𝑜𝑠 𝜃3 − 𝑏)
]
𝑱𝑥2 =
[
2(𝑦 + 𝐿 𝑐𝑜𝑠 𝜃1 + 𝑎)
2(𝑦 −
𝐿
2
𝑐𝑜𝑠 𝜃2 + 𝑐)
2(𝑦 −
𝐿
2
𝑐𝑜𝑠 𝜃3 + 𝑐)]
𝑱𝑥3 = [
2(𝑧 + 𝐿 𝑠𝑖𝑛 𝜃1)
]
Với:
𝑎 = −𝑢𝑃 + 𝑤𝐵 𝑏 =
𝑠𝑃
2
−
√3
2
𝑤𝐵
𝑐 = 𝑤𝑃 −
𝑤𝐵
2
 Ma trận các lực không thế suy rộng 𝐐𝒏𝒑
DUT.LRCC

Các moment dẫn động đặt tại các khớp chủ động 𝜏1, 𝜏2, 𝜏3.
𝝉
𝐵1
1 = [
0
𝜏1
0
] , 𝝉
𝐵2
2 = [
0
𝜏2
0
] , 𝝉
𝐵3
3 = [
0
𝜏3
0
]
Áp dụng công thức (0.13,[1]) ta được:
𝐐𝑛𝑝
= [𝜏1 𝜏2 𝜏3 0 0 0]𝑇
2.4.4. Giải bài toán động lực học ngược bằng cách biến đổi về tọa độ suy rộng:
Ta tiến hành bằng cách khử các tọa độ suy rộng phụ thuộc và các nhân tử Lagrange,
sau đó biến đổi hệ phương trình vi phân đại số về hệ phương trình vi phân thường với
số phương trình bằng số bậc tự do của hệ.
Nhắc lại các phương trình liên kết:
𝐟(𝐬) = 𝐟(𝐪, 𝐱) = 𝟎, 𝐟 ∈ ℝ𝟑
, 𝐪 ∈ ℝ𝟑
, 𝐱 ∈ ℝ𝟑
Đạo hàm ta được:
𝐟̇(𝐬) = 𝐉𝐬(𝐬)𝐬̇ = 𝐉𝐪(𝐪, 𝐱)𝐪̇ + 𝐉𝐱(𝐪, 𝐱)𝐱̇ = 𝟎 (2.70)
Trong đó 𝐉𝐪, 𝐉𝐱 đều là các ma trận vuông 3x3 đã xác định ở phần động học.
Giả sử 𝐉𝐱 là ma trận không suy biến, det 𝐉𝐱 ≠ 𝟎. Từ (2.70), ta suy ra:
𝐱̇ = −𝐉𝒙
−𝟏
𝐉𝐪𝐪̇ (2.71)
Nếu ta đưa vào phương trình đồng nhất:
𝐪̇ = 𝐄𝐪̇ (2.72)
với 𝐄 là ma trận vuông cấp 3.
Thì các phương trình (2.71), (2.72) có thể viết gộp lại:
𝐬̇ = [
𝐪̇
𝐱̇
] = [
𝐄
−𝐉𝒙
−𝟏
𝐉𝐪
] 𝐪̇ (2.73)
Nếu ta đưa vào kí hiệu:
R(s) = 𝐑(𝐪, 𝐱) = [
𝐄
−𝐉𝒙
−𝟏
𝐉𝐪
]
thì phương trình (2.56) được viết lại:
𝐬̇ = 𝐑(𝐪, 𝐱)𝐪̇ (2.74)
Thay vào phương trình (2.53) ta được:
𝐉𝐬(𝐪, 𝐱)𝐑(𝐪, 𝐱)𝐪̇ = 𝟎
DUT.LRCC

Do 𝐪̇ gồm các biến độc lập tuyến tính, nên:
𝐉𝐬(𝐪, 𝐱)𝐑(𝐪, 𝐱) = 𝟎
Hay:
𝐑𝑻(𝐪, 𝐱)𝐉𝒔
𝑻
(𝐪, 𝐱) = 𝟎 (2.75)
Từ phương trình Lagrande dạng nhân tử:
𝐌(𝐬)𝐬̈ + 𝐠(𝐬) + 𝐉𝒔
𝑻(𝐬)𝛌 = 𝛕 (2.76)
Nhân hai vế với ma trận 𝐑𝑻
ta được:
𝐑𝑻
𝐌𝐬̈ + 𝐑𝑻
𝐠 + 𝐑𝑻
𝐉𝒔
𝑻(𝐬)𝛌 = 𝐑𝑻
𝛕 (2.77)
Thay (2.58) vào (2.60) ta được:
𝐑𝑻
𝐌𝐬̈ + 𝐑𝑻
𝐠 = 𝐑𝑻
𝛕 (2.78)
Đạo hàm theo thời gian biểu thức (2.54) ta có:
𝐬̈ = 𝐑𝐪̈ + 𝐑̇ 𝐪̇ (2.79)
Thay (2.62) vào (2.61) ta được:
𝐑𝑻
𝐌𝐑𝐪̈ + 𝐑𝑻
𝐌𝐑̇ 𝐪̇ + 𝐑𝑻
𝐠 = 𝐑𝑻
𝛕 (2.80)
Nếu ta đưa về các ký hiệu:
𝐑𝑻
𝛕 = 𝛕𝒂
𝐑𝑻
𝐌𝐑 = 𝐌𝒂
𝐑𝑻
𝐌𝐑 = 𝐂𝒂
𝐑𝑻
𝐠 = 𝐠𝒂
Khi đó, phương trình (2.63) được viết lại:
𝐌𝒂𝐪̈ + 𝐂𝒂𝐪̇ + 𝐠𝒂 = 𝛕𝒂 (2.81)
Vector 𝛕𝒂 chính là nghiệm của bài toán động lực học ngược
 Các bước giải bài toán động lực học theo phương pháp trên:
 Bước 1: Giải bài toán động học ngược. Cho biết x(t) và f(q,x)=0.
Từ đó tính được 𝒒(𝑡), 𝒒̇ (𝑡), 𝒒̈ (𝑡).
 Bước 2: Xác định các ma trận R(s), RT(s), M(s), g(s).
𝐑(𝐬) = [
𝐄
−𝐉𝒙
−𝟏
𝐉𝐪
]
DUT.LRCC

Suy ra:
𝑹𝑻(𝐬) = [ 𝑬 , (−𝐉𝒙
−𝟏)𝑻
𝑱𝒒
𝑻
]
 Bước 3 : Tính các mô men (lực) các khâu dẫn theo công thức (2.64)
2.5. Khảo sát vùng làm việc
Mục đích của việc khảo sát vùng làm việc để biết được ảnh hưởng của các thông số
hình học của Robot Delta đến khoảng không gian làm việc của nó như thế nào rồi từ đó
suy ngược lại thiết kế ngược.
Ta thấy rằng vùng làm việc của mỗi cánh tay RUU là vùng bao của một mặt cầu
𝑠𝑖(𝐴𝑖, 𝑙) có tâm 𝐴𝑖 trượt trên một đường tròn 𝑐𝑖(𝐵𝑖, 𝐿). Tọa độ các điểm 𝐴𝑖 đã được xác
định tại các phương trình (2.48), (2.49) và (2.50) với 𝜃𝑖 ∈ [𝜃𝑖𝑚𝑖𝑛, 𝜃𝑖𝑚𝑎𝑥], trong đó
[𝜃𝑖𝑚𝑖𝑛, 𝜃𝑖𝑚𝑎𝑥] là khoảng hoạt động trên thực tế của mỗi khớp quay 𝐵𝑖.
Vùng bao này [5] được mô tả như sau:
 Nếu 𝐿 > 𝑙: Vùng bao 𝑡1 là hình xuyến có dạng ring torus (Hình 2.6);
 Nếu 𝐿 = 𝑙: Vùng bao 𝑡2 là hình xuyến có dạng horn torus (Hình 2.7);
 Nếu 𝐿 < 𝑙: Vùng bao 𝑡3 là hình xuyến có dạng spindle torus, bao gồm một vùng
lõi trống bên trong (Hình 2.8).
Hình 2. 6 Vùng làm việc có dạng ring torus
DUT.LRCC

Hình 2. 7 Vùng làm việc có dạng horn torus
Hình 2. 8 Vùng làm việc có dạng spindle torus
Ta chỉ mới tìm được vùng làm việc của mỗi cánh tay RUU (tức là vùng bao của các
điểm 𝑃𝑖). Điều ta cần tìm là vùng làm việc của khâu chấp hành cuối thể hiện bởi điểm
𝑃.
Tấm đế di động được xem là một vật rắn, chỉ có các chuyển động tịnh tiến theo các
trục 𝑥, 𝑦, 𝑧. Véctơ 𝑃𝑖𝑃
⃗⃗⃗⃗⃗ có phương và độ lớn không đổi trong hệ tọa độ {𝐵}. Do đó,
DUT.LRCC

vùng làm việc của điểm 𝑃 (kí hiệu là 𝑡𝑖𝑣) chính là vùng làm việc 𝑡𝑖 của các điểm 𝑃𝑖 tịnh
tiến theo véctơ 𝑃𝑖𝑃
⃗⃗⃗⃗⃗ (Hình 2.9).
Hình 2. 9 Vùng làm việc của mỗi cánh tay
Vùng làm việc của robot Delta kiểu ba khớp quay là giao điểm của ba vùng bao
hình xuyến 𝑡𝑖𝑣 (với 𝑖 = 1,2,3). Vùng làm việc này cũng chính là vùng bao tập nghiệm
của hệ phương trình động học thuận. Do cấu trúc của robot Delta mà chỉ có phần giao
nhau tương ứng với 𝑧 âm (hay nói cách khác, nằm dưới tấm đế cố định) sẽ được chọn.
Phương trình động học có thể được viết dưới dạng:
(𝑥 − 𝑥𝑖)2
+ (𝑦 − 𝑦𝑖)2
+ (𝑧𝑃 − 𝑧𝑖)2
= 𝑙2 (2.82)
Với 𝑥𝑖, 𝑦𝑖, 𝑧𝑖 là tọa độ của điểm ảo 𝐴𝑖𝑣. Phương trình (2.82) là phương trình của 3
mặt cầu tâm 𝐴𝑖𝑣 và bán kính là 𝑙.
Vì tâm mặt cầu nội tiếp nằm trên 𝑧𝐵 nên phương trình (2.82) được viết lại:
(𝑥)2
+ (𝑦)2
+ (𝑧 − 𝑧𝑖)2
= 𝑙2
với 𝑖 = 1,2,3 (2.83)
Nếu có một điểm mà tại đó 𝑧1 = 𝑧2 = 𝑧3 = 𝑧𝑖𝑛𝑡 thì ba hình xuyến giao nhau tại một
mặt cầu 𝑠′𝑖𝑛𝑡 tâm là 𝐴′𝑖𝑛𝑡(0,0, 𝑧𝑖𝑛𝑡) và bán kính là 𝑟𝑖𝑛𝑡 = 𝑙. Ta sẽ xác định mặt cầu
này.
DUT.LRCC

Thay 𝑥𝑖 = 𝑦𝑖 = 0 vào biểu thức xác định tọa độ các tâm ảo 𝐴𝑖𝑣 đã xác định ở phần
động lực học. Ta thu được các phương trình:
{
𝑐𝑜𝑠𝜃1 =
−𝑅3
𝐿
𝑐𝑜𝑠𝜃2 =
−𝑅3
𝐿
𝑐𝑜𝑠𝜃3 =
−𝑅3
𝐿
Các phương trình này có nghiệm với điều kiện 𝐿 ≥ |𝑅3|
Suy ra 𝜃1 = 𝜃2 = 𝜃3, khi đó 𝑧1 = 𝑧2 = 𝑧3 = −𝐿𝑠𝑖𝑛𝜃𝑖 = 𝑧𝑖𝑛𝑡. Như vậy ba hình
xuyến luôn giao nhau tại một mặt cầu nội tiếp nếu các hình xuyến thỏa mãn điều kiện
𝑅1 = 𝐿 ≥ |𝑅3|. Mặt cầu nội tiếp 𝑠′𝑖𝑛𝑡 sẽ bao gồm các đường tròn nội tiếp 𝑐𝑠𝑖𝑛𝑡 có
bán kính 𝑟𝑐𝑠𝑖𝑛𝑡 = √𝑙2 − (𝑧𝑃 − 𝑧𝑖𝑛𝑡)2 tại mặt cắt ngang 𝑧𝑃.
Khi đã xác định được ba hình xuyến luôn giao nhau tại một mặt cầu nội tiếp thì việc
tiếp theo đó là chọn một trong mười vùng làm việc trong tài liệu tham khảo [8].
Hình 2. 10 Mười vùng làm việc
Việc phân tích chi tiết được nêu ra trong [4] dựa vào các thông số
𝑟1 =
𝐿
𝐷
𝑟2 =
𝑙
𝐷
𝑟3 =
𝑅3
𝐷
𝐷 =
𝐿 + 𝑙 + |𝑅3|
3
Tìm mối liên hệ giữa chúng sau đó tìm ra phương án thiết kế dựa theo các thông số cho
trước.
DUT.LRCC

Chương 3 Thiết kế hệ thống
3.1. Sơ đồ tổng quan
3.1.1. Sơ đồ tổng quát hệ thống
Hình 3. 1 Sơ đồ tổng quan về hệ thống
Nguyên tắc hoạt động:
Khi hệ thống hoạt động robot Delta quay về vị trí gốc (chạm CTHT) rồi đến vị trí
set điểm home(1, 2, 1 = 0). Sau đó băng tải hoạt động mang sản phẩm đến, camera
chụp ảnh gửi dữ liệu về máy tính để tiến hành xử lý phân loại màu sắc và hình dáng
của sản phẩm, từ đó gửi thông tin đến adruino để xuất xung điều khiển động cơ và van
thực hiện quá trình phân loại sản phẩm.
DUT.LRCC

3.1.2. Lưu đồ thuật toán
Hình 3. 2 Lưu đồ thuật toán
3.2. Thiết kế hệ thống cơ khí
Yêu cầu đề ra: Hệ thống cơ khí cần đảm bảo sự linh hoạt, có tải trọng nhỏ nhất có
thể nhưng vẫn đảm bảo vững chắc trong khi làm việc.
Sơ đồ khối quá trình thiết kế cơ khí
DUT.LRCC

Hình 3. 3 Sơ đồ hệ thống thiết kế cơ khí
Từ vùng làm việc yêu cầu ta xác định các thông số cơ bản của robot Delta,với các
thông số trên tiến hành thiết kế 3d các chi tiết sao cho tối ưu nhất.
Sau đó tiến hành gia công các chi tiết, rồi lắp ráp, thử nghiệm. Nếu robot hoạt động
không ổn định thì quay lại bước thiết kế 3D để kiểm tra lỗi.
DUT.LRCC

3.2.1. Tính chọn bộ thông số cơ bản của robot Delta:
Với vùng làm việc yêu cầu của robot Delta là hình trụ có rc = 225 mm, h = 250 mm,
z1 = 100 mm. Ta tính toán các thông số của robot Delta trong bảng 3.1 và 3.2.
𝑟2 𝑟1 𝑟3 𝐷 𝐿 𝑙 𝑅3 𝑧ỉ𝑛𝑡 𝑙𝑡
1,1 0,95 0,95 800 800 880 720 -349 224
1,2 0.95 0,85 320 304 384 272 -136 114
1,3 1,0 0,7 267 267 347 283 -190 176
1,4 1,05 0,65 228 210 320 126 -204 186
Bảng 3. 1 Vùng IIf
1,5 1.1 0,4 200 220 300 80 -205 187
1,6 1,13 0,25 178 201 281 48 -122 183
1,7 1,3 0 200 260 340 0 -260 214
1,8 1,05 0,15 107 112 192 16 -110 51
Bảng 3. 2 Vùng IIIb
Chọn các thông số kích thước robot Delta sao cho 𝑙 phải gần với giá trị 𝑙𝑡 tối ưu.
Bảng các thông số kích thước được chọn:
1,6 1,15 0,25 178 201 281 48 -122 183
Bảng 3. 3 Các thông số kích thước được chọn.
Hiệu chỉnh các thông số kích thước robot Delta để phù hợp với thực tế của thiết
kế(kích thước phù hợp với máy CNC, giảm sai số trong khâu thiết kế gia công).
Bảng các thông số kích thước sau khi hiệu chỉnh:
DUT.LRCC

𝑟2 𝑟1 𝑟3 𝐷 𝐿 𝑙 𝑅3 𝑧ỉ𝑛𝑡
1,58 1,13 0,29 177 200 280 50 -190
Bảng 3. 4 Các thông số kích thước sau khi hiệu chỉnh.
Khảo sát vùng làm việc thực tế của robot Delta sau khi hiệu chỉnh:
Với robot Delta có vùng làm việc thuộc vùng IIIb, tiết diện vùng làm việc gồm 3
dạng:
Gọi zp là mặt cắt ngang vùng làm việc.
 Phần 1 : 𝑧𝑃 ∈ (𝑧𝑃1, 𝑧𝑚𝑎𝑥]
 Phần 2 : 𝑧𝑃 ∈ [𝑧𝑚𝑖𝑛, 𝑧𝑃0)
 Phần 3 : 𝑧𝑃 ∈ [𝑧0, 𝑧𝑃1]
Vì vùng làm việc ở phần 1 và phần 2 nhỏ hơn rất nhiều so với phần 3 nên ta chỉ xét
vùng làm việc của robot Delta ở phần 3.
Bán kính đường tròn nội tiếp lớn nhất:
𝑟𝑖𝑛𝑡𝑚𝑎𝑥 = √𝑙2 − (𝑧𝑃 − 𝑧𝑖𝑛𝑡)2 , 𝑧𝑃 ∈ [𝑧𝑃0, 𝑧𝑃1]
Với |zp(i+1) - zp(i)| = 0.0039 m (i = (1:100)) ta xác định được vùng làm việc robot
Delta như hình 3.4.
Hình 3. 4 Vùng làm việc robot Delta
DUT.LRCC

Từ đó ta chọn được vùng làm việc với các kích thước như hình 3.5, vùng làm việc
này hoàn toàn phù hợp với thiết kế ban đầu.
Hình 3. 5 Vùng làm việc được chọn
Hình 3. 6 Mô hình 3D vùng làm việc
3.2.2. Tính chọn động cơ
 Tính momen tĩnh lớn nhất:
DUT.LRCC

Hình 3. 7 Vị trí cánh tay đạt momen tĩnh lớn nhất
Ta có : Cánh tay B1A1 có quỹ tích là đường tròn (B1; B1A1)
Mà momen của B1A1 tại B1 = m1 l.cos φ1
⟹ Momen M lớn nhất đạt được khi B1A1 vuông góc xx’.
Cánh tay A1P1 có quỹ tích là đường tròn (A1; A1P1)
Tấm đế di động luôn song song với tấm đế cố định (P1P2 // B1B2 )
Mặt khác, tâm P của tấm đế di động ( cơ cấu chấp hành) chỉ chuyển động trong
vùng làm việc của Robot ( hình vẽ)
⟹ Vị trí tối đa mà P đạt được là giao điểm của đường giới hạn vùng làm việc robot
và tấm đế di động.
⟹ Ta xác định được Pmax , tại đó mô men tại B1 là lớn nhất.
Xét momen tại B1 :
MB1 = 0.5m1g.B1A1 + m2g(B1A1 – (A1P1. cosφ)/2) + m3g(rc - B1O)
= 0.5 m1g.L + m2g(L – (l. cos φ)/2) + m3g(rc – wb)
DUT.LRCC

Với
 rc = 0.225 m : bán kính đường tròn đáy vùng làm việc hình trụ
 L = 0.2 m : Chiều dài cánh tay chủ động.
 l = 0.28 m : Chiều dài cánh tay bị động.
 m1 = mL + mb = 0.642 kg : Khối lượng cánh tay chủ động
 m2 = ml + 4mkc + mkn = 0.5 kg : Khối lượng cánh tay bị động
 m3 =(mtải + mP)/3 = (2+0.5)/3 = 5/6 kg : Khối lượng tấm đế di động và tải.
 mkc: Khối lượng mỗi khớp cầu
 mkn: Khối lượng vít cấy
 mtải: Khối lượng tải
 mP: Khối lượng tấm đế di động
Ta có momen tĩnh tại B1:
MB1 = 0.5.0,642.9,81.0,2 + 0,43.9,81.(0,2-(0,28.cos(85))/2) + (
5
6
).9,81.(0.225 – 0.1)
= 2.45 N.m
 Xác định momen động lớn nhất:
Giả sử mô phỏng chuyển động của p trong 2 chu kỳ với quỹ đạo chuyển động:
+ P
x 0.225cos(2 t)
  (m);
+ P
y 0.225sin(2 t)
  (m);
+ P
z 0.1
  (m);
Hình 3. 8 Đồ thị mô momen dẫn động tại z = -0.1 m
Momen dẫn động lớn nhất:
DUT.LRCC

M = 5.7716 N.m
Với :
+ P
x 0.225cos(2 t)
  (m);
+ P
y 0.225sin(2 t)
  (m);
+ P
z 0.34
  (m);
Hình 3. 9 Đồ thị mô momen dẫn động tại z = -0.34 m
M = 6.5585 N.m
Với:
+ P
x 0.1cos(2 t)
  (m);
+ P
y 0.1sin(2 t)
  (m);
+ P
z 0.45
  (m);
DUT.LRCC

Hình 3. 10 Đồ thị mô momen dẫn động tại z = -0,45
M = 1.5406 N.m
Giá trị momen dẫn động phụ thuộc vào gia tốc suy rộng các khớp chủ động,giá trị
lớn nhất của momen dẫn động đạt được khi gia tốc lớn nhất.
Từ đó ta xác định được momen dẫn động lớn nhất trong vùng làm việc là
M = 6.5585 N.m
Chọn momen động cơ :
Mđc ≥ 1,2M = 1,2.6.5585 = 7,8702 N.m
Từ các thông số trên chọn động cơ AC Servo Panasonic với các thông số như trong
bảng 3.5.
Mã
Công suất
(KW)
Điện áp đầu
vào (V)
Tốc độ cực
đại(rev/min)
Mômen xoắn
cực đại(N.m)
MSME102G1G 1 200 177 9,55
Bảng 3. 5 Các thông số cơ bản động cơ AC Servo Panasonic
3.2.3. Thiết kế cơ khí các chi tiết robot Delta
a. Tấm đế cố định
Tấm đế cố định phải đảm bảo độ cứng vững để chịu tải trọng của toàn bộ robot
Delta.
DUT.LRCC

Vì kích thước lớn nên chia làm 3 phần để thuận tiện trong việc gia công cnc, tiết
kiệm chi phí nhưng vẫn đảm bảo yêu cầu đặt ra.
 Vật liệu: Nhôm nguyên khối dạng tấm
 Kích thước: Sb= 346,4 mm
 Bản vẽ (1)
Hình 3. 11 Tấm đế cố định
b. Gá động cơ
 Vật liệu: Nhôm nguyên khối
 Kích thước:
 Bản vẽ (1)
Hình 3. 12 Gá động cơ
DUT.LRCC

c. Bạc lót
 Vật liệu: Thép
 Kích thước:
 Bản vẽ (1)
Hình 3. 13 Bạc lót
d. Tấm đế di động
Tấm đế di động được thiết kế sao cho khối lượng nhỏ nhất nhưng vẫn đảm bảo
vững chắc trong quá trình hoạt động. Trên đó được khoan các lỗ để gá cơ cấu chấp
hành cuối.
 Vật liệu: Nhôm nguyên khối dạng tấm
 Kích thước: Sp= 86,6 mm
 Bản vẽ (1)
Hình 3. 14 Tấm đế di động
DUT.LRCC

e. Cánh tay chủ động
Cánh tay chủ động là chi tiết chịu momen xoắn trực tiếp từ động cơ, với yêu cầu
đó thì cánh tay chủ động phải được thiết kế sao cho đảm bảo không bị phá hủy trong
quá trình hoạt động.Từ quá trình kiểm nghiệm lực cũng như kế thừa từ những
phiên bản trước, nhóm đã thiết kế cánh tay đạt được những yêu cầu trên.
 Vật liệu: Nhôm nguyên khối
 Kích thước: L= 200 mm
 Bản vẽ (1)
Hình 3. 15 Cánh tay chủ động
f. Cánh tay bị động
Cánh tay bị động được làm từ carbon fiber, đảm bảo cứng vững về kết cấu, giảm
khối lượng cho robot .
 Vật liệu: Carbon Fiber
 Kích thước: l= 280 mm
 Bản vẽ (1)
DUT.LRCC

Hình 3. 16 Cánh tay bị động
Hình 3. 17 Mô hình 3D Solid Works robot Delta
DUT.LRCC

3.3. Thiết kế hệ thống điện – điện tử
Hình 3. 18 Sơ đồ hệ thống điện- điện tử
Sơ đồ khối gồm có 5 khối hoạt động chính:
 Khối nguồn cung cấp nguồn cho động cơ và bộ điều khiển trung tâm.
 Khối điều khiển động cơ gồm 3 driver điều khiển động cơ
 Khối điều khiển trung tâm
 Khối thu nhận dữ liệu
 Khối xử lí dữ liệu
3.3.1. Thiết kế hệ thống tủ điện
a. Cách bố trí tủ
 Nhóm thiết bị điều khiển phải được thành từng nhóm tránh hiện tượng nhiễu
khi đặt cùng nhóm động lực.
 Nhóm khí cụ điện đóng cắt đặt cùng một hàng phía dưới (Aptomat,
Contactor,).
DUT.LRCC

 Aptomat tổng đặt ở trung tâm hoặc ở trên cùng bên trái tủ điện sao cho dễ
vận hành nhất.
b. Contactor(MS)
Hình 3. 19 Contactor
Contactor là khí cụ điện dùng để đóng, ngắt thường xuyên các mạch điện động lực,
từ xa, bằng tay (qua hệ thống nút bấm) hoặc tự động, được điều khiển để cung cấp
nguồn cho một thiết bị công suất tải lớn: Máy Lạnh lớn, động cơ kéo tải lớn... Khác với
Rơle nguồn điều khiển là một chiều điện áp thấp. Contactor nguồn điều khiển là loại
xoay chiều điện áp cao. Việc đóng cắt contactor có tiếp điểm có thể được thực hiện
bằng nam châm điện, thủy lực hay khí nén. Thông thường ta gặp loại đóng cắt bằng
nam châm điện.
Trong công nghiệp, Contactor được sử dụng để điều khiển vận hành các động cơ
hay thiết bị điện, để an toàn khi vận hành, dễ điều khiển cho người sử dụng bằng cách
nhấn nút mà không cần đến chip xử lý nào cả. Đây là một giải pháp tự động hóa bằng
phương pháp cơ điện. Phương pháp này không xử lý những quá trình phức tạp nhưng
nó đơn giản và ổn định cao dễ sửa chữa và không cần chuyên môn cao.
c. Bộ lọc nguồn lọc nhiễu (Noise Filter)
Bộ lọc các xung điến áp gây nhiễu các thiết bị. Sử dụng cho các bộ nguồn xung,
máy hàn, máy tính công nghiệp, Driver Servo…
Có tác dụng tạo ra nguồn điện ổn định, trong hệ thống điện từ nhà máy, các xí
nghiệp, tòa nhà đến hệ thống điện gia đình. Có rất nhiều các yếu tố gây ra nhiễu nguồn
điện, trong nội bộ, hoặc từ bên ngoài. Đặc biệt là các máy cao tần, máy hàn, biến tần,
DUT.LRCC

máy biến thế, động cơ điện, bộ chỉnh lưu, nắn dòng, các loại máy cắt tốc độ cao, các
loại motor, trong quá trình khởi động và hoạt động các thiết bị này sẽ tạo ra sóng mang
(sóng hài, harmonic) quay ngược lại hệ thống xung quanh, làm cho biên độ hình sin
của nguồn điện bị thay đổi liên tục hay tạo ra các xung điện áp.
Hình 3. 20 Sóng hài
Khi tín hiệu nguồn hiệu dụng và biên độ giao động điện của các thiết bị thay đổi sẽ
gây ra các hiện tượng:
 Làm tăng thêm độ phát nhiệt của các thiết bị.
 Ảnh hưởng đến tuổi thọ và khả năng truyền tải.
 Ảnh hưởng đến các thiết bị bảo vệ, gây ra những tác động sai đối với các CB,
cầu chì, relay bảo vệ, ảnh hưởng đến các thiết bị đo đếm, cảm biến áp
suất, cảm biến siêu âm…
 Ảnh hưởng đến các thiết bị truyền thông.
DUT.LRCC

Hình 3. 21 Tác dụng của lọc 3P và 1P
3.3.2. Thiết kế mạch điện tử
a. Driver điều khiển động cơ
Panasonic AC servo driver: MDDKT5540
Hình 3. 22 AC Servo Driver MDDKT5540
Thông số kỹ thuật:
 Điện áp hoạt động: 1 pha, 200 – 240 V
DUT.LRCC

 Dòng điện lớn nhất: 50A
 Độ phân giải: 20-bit
 Các chế độ điều khiển: Điều khiển vị trí, điều khiển momen, điều khiển tốc
độ,…
 Chuẩn giao tiếp: RS232, RS485, USB.
 Nhà sản xuất: Panasonic.
Sơ đồ chân kết nối :
Hình 3. 23 Sơ đồ chân kết nối Driver MDDKT5540
b. Mạch điều khiển
Chọn loại vi điều khiển: Mega 2560
DUT.LRCC

Hình 3. 24 Arduino Mega 2560
Sơ đồ chân của Arduino mega 2560
Hình 3. 25 Sơ đồ chân board mega 2560
DUT.LRCC

Vi điều khiển ATmega2560
Điện áp hoạt động 5VDC
Nguồn ngoài(jack dc) 7-12VDC
Xung clock 16Mhz
Số chân digital I/O 54
Số chân PWM 15
Số chan Analog 16
Giao tiếp UART 4 Bộ
Giao tiếp SPI 1 Bộ
Giao tiếp I2C 1 Bộ
Ngắt ngoài 6
Flash
256Kb
(8Kb dùng cho Bootloader)
SRAM 8Kb
EEPROM 4Kb
Bảng 3. 6 Thông số kỹ thuật của Board Arduino Mega
3.4. Thiết kế hệ thống khí nén
Hình 3. 26 Hệ thống khí nén
Sử dụng máy khí nén chứa rất nhiều tạp chất và hóa chất. Đặc biệt trong không khí
còn có dầu tổng hợp chứa các dung môi hữu cơ, muối hoặc khí ăn mòn. Chúng có thể
gây hư hại làm ăn mòn trong các xilanh. Điều này làm ảnh hưởng đến quá trình hút vật
của đầu công tác.
DUT.LRCC

Nếu bơm khí nén chứa nhiều chất thải hoặc bột Cacbon. Nó có thể dính vào bộ
phận tạo chân không hoặc bên trong van solenoid và thậm chí có thể dính vào công tắc
áp suất làm giảm hiệu suất hoặc lỗi hệ thống khí.
 Máy nén khí: bộ phận tạo ra nguồn khí cung cấp cho hệ thống
 Bộ phận làm khô không khí: Đây là một trong những bộ phận quan trọng
nhất của hệ thống khí nén. Bất kể ở đâu, ở vùng nhiệt đới hay sa mạc, không
khí trong khí quyển đều chứa một lượng hơi nước nhất định. Khi hơi ẩm
không giữ được trạng thái hơi nó sẽ chuyển sang trạng thái lỏng làm ảnh
hưởng đến quá trình hoạt động nói chung và hoạt động lọc tiếp theo đó nói
riêng.
 Bộ lọc không khí: Sau khi làm khô không khí, một lượng tạp chất lớn sẽ
vượt qua bộ phận làm khô. Chức năng của bộ lọc không khí là lọc các tạp
chất này rồi thải ra ngoài.
 Bộ lọc và tách không khí sau khi lọc cặn bẩn thì còn lại một số hóa chất chứa
dung môi hữu cơ, muối, chất ăn mòn… Nhiệm vụ của bộ lọc này là loại bỏ
những chất này để cho ra hệ không khí sạch.
 Bộ van điều áp: để điều chỉnh áp suất cho hệ thống. Van hiển thị có thể hiển
thị bằng kim hoặc đồng hồ điện tử.
 Van điện từ: để điều khiển luồng hay đóng mở khí
 Bộ chuyển đổi dòng khí: Bộ phận này sẽ chuyển đổi khí nén thành chân
không
 Bộ phận công tác: Có rất nhiều bộ phận công tác như hút, tay gắp…
DUT.LRCC

Chương 4 Kết quả nghiên cứu và kết luận
4.1. Kết quả nghiên cứu
Sau một thời gian nghiên cứu, thiết kế và lắp ráp, nhóm đã hoàn thiện đề tài và đạt
được những yêu cầu đặt ra từ đầu:
 Về tổng thể:
Hình 4. 1 Hoàn thiện sản phẩm
DUT.LRCC

 Các thông số kĩ thuật chính
Stt Đặc tính Kí hiệu Thông số
1 Chiều cao tổng 1800 mm
2 Chiều rộng tổng 1000 mm
3 Cánh tay trên L 200 mm
4 Cánh tay dưới l 280 mm
5 Tấm đế cố định Sb 364,4 mm
6 Tấm đế di động Sp 86,6 mm
7 Vùng làm việc rc x h 225 x 300 mm
Bảng 4. 1 Thông số chính
 Về giao diện điều khiển:
Hình 4. 2 Giao diện điều khiển
DUT.LRCC

 Về giao diện mô phỏng:
Hình 4. 3 Giao diện mô phỏng trên Matlab/Simulink
 Tủ điện điều khiển:
 Có đầy đủ các thành phần bảo vệ nguồn và bảo vệ các driver cho động cơ.
 Các thiết bị điện được bố trí một cách hợp lí.
 Có các thành phần lọc nhiễu cho Driver động cơ.
4.2. Đánh giá:
 Phần cơ khí
Robot Delta hoạt động tốt, ít gặp sự cố. Kiểu dáng được thiết kế chắc chắn song bên
cạnh đó vẫn còn nhiểu điểm cần khắc phục như các khớp cầu mua sẵn bị hạn chế góc
quay dẫn đến vùng làm việc trên thực tế của robot bị thu hẹp đáng kể.
 Phần xử lí ảnh
Thuật toán xử lý ảnh đưa vào hợp lý, khiến cho robot hoạt động ổn định. Camera bắt
vật chính xác và đã trả về đúng tọa độ vật trên băng tải động. Nhưng bên cạnh đó cần
nhiều vấn đề cần nâng cấp, cải tiến là sử dụng camera công nghiệp để tăng tốc độ và
chất lượng hình ảnh, giúp quá trình xử lý chính xác hơn, nâng cấp vùng xử lí ảnh được
rộng hơn để tăng được tốc độ làm việc của băng tải và tốc độ làm việc của robot.
 Phần mô phỏng điều khiển
DUT.LRCC

Thiết kế và chế tạo robot Delta.pdf

Thiết kế và chế tạo robot Delta.pdf

Recommended

Recommended

More Related Content

What's hot

What's hot (20)

Similar to Thiết kế và chế tạo robot Delta.pdf

Similar to Thiết kế và chế tạo robot Delta.pdf (20)

More from Man_Ebook

More from Man_Ebook (20)

Recently uploaded

Recently uploaded (20)

Thiết kế và chế tạo robot Delta.pdf