Báo Cáo Thiết Kế Robot Final

ĐẠI HỌC BÁCH KHOA HÀ NỘI
TRƯỜNG CƠ KHÍ
KHOA CƠ ĐIỆN TỬ
Báo cáo bài tập lớn

Đề tài: Tính toán thiết kế mô hình robot ứng dụng bốc xếp
kho chứa tự động trong dây chuyền sản xuất linh hoạt FMS
Giảng viên hướng dẫn : TS. GVC. Nguyễn Văn Quyền

Mã học phần : ME4505
Sinh viên thực hiện : Nguyễn Bá Công 20194926
: Nguyễn Văn Đức 20194963
: Đặng Đức Độ 20194948
Hà Nội 8/2023
Tính toán thiết kế Robot GVHD: TS. GVC. Nguyễn Văn Quyền 2
Nhóm 3 đề tài: Tính toán thiết kế mô hình robot ứng dụng bốc xếp
kho chứa tự động trong dây chuyền sản xuất linh hoạt FMS.
111Equation Chapter 1 Section 1Bảng phân công
nhiệm vụ
STT Tên MSSV Nhiệm vụ Đánh giá
1 Nguyễn Bá Công 20194926 Tính động học thuận, 100%
động học ngược vị trí,
động lực học thuận
2 Đặng Đức Độ 20194948 Thiết kế hệ thống điều 100%

khiển, động học ngược
vận tốc, gia tốc.
3 Nguyễn Văn Đức 20194963 Thiết kế quỹ đạo 100%

chuyển động trong
không gian thao tác,
phân tích lựa chọn kết
cấu, động lực học
ngược.
Mục Lục
Lời mở đầu...........................................................................................................................6
CHƯƠNG 1: TỔNG QUAN VỀ ROBOT..........................................................................7
1.1 Khái niệm Robot công nghiệp...................................................................................7
1.2 Phân loại Robot công nghiệp.....................................................................................7
1.2.1 Phân loại theo kết cấu.........................................................................................7
1.2.2 Phân loại theo điều khiển....................................................................................7
1.2.3 Phân loại theo ứng dụng.....................................................................................8
1.3 Ứng Dụng của Robot công nghiệp............................................................................8
CHƯƠNG 2: PHÂN TÍCH LỰA CHỌN KẾT CẤU ROBOT.........................................13
2.1. Phân tích mục đích ứng dụng của Robot................................................................13
2.2. Phân tích yêu cầu kĩ thuật.......................................................................................13
2.2.1. Phân tích về yêu cầu vị trí, hướng của khâu thao tác......................................13
2.2.2. Số bậc tự do cần thiết.......................................................................................14
2.2.3. Không gian làm việc........................................................................................14
2.3. Các phương án thiết kế cấu trúc Robot...................................................................15
2.4. Các đặc trưng kĩ thuật.............................................................................................19
CHƯƠNG 3: TÍNH TOÁN ĐỘNG HỌC.........................................................................20
3.1 Xây dựng bài toán động học của Robot...................................................................20
3.1.1. Cấu trúc động học của robot............................................................................20
3.1.2. Phương pháp Denavit – Hartenberg................................................................20
3.1.3. Xây dựng cấu trúc động học............................................................................22

3.2. Bài toán động học thuận về vị trí............................................................................24
3.3. Bài toán động học ngược về vị trí...........................................................................25
3.4. Bài toán động học vi phân......................................................................................26
3.4.1. Bài toán động học vi phân thuận.....................................................................26
3.4.2. Bài toán động học vi phân ngược....................................................................27
CHƯƠNG 4: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG THEO YÊU CẦU KỸ THUẬT
...........................................................................................................................................29
4.1. Thiết kế quỹ đạo trong không gian thao tác:..........................................................29
4.2. Xây dựng chương trình mô phỏng trên Matlab......................................................31
4.3. Kết quả thiết kế quỹ đạo trong không gian thao tác khi thực hiện chu trình công
việc.................................................................................................................................32
CHƯƠNG 5: TÍNH TOÁN ĐỘNG LỰC HỌC................................................................40
5.1. Thiết lập phương trình vi phân chuyển động của robot..........................................40
5.2. Động năng và ma trận khối lượng M......................................................................40
5.2.1. Tính các tenxơ quán tính.................................................................................40
5.2.2. Tính các ma trận Jacobi...................................................................................41
5.2.3. Động năng và ma trận khối lượng M...............................................................42
5.3. Ma trận quán tính li tâm và gia tốc Coriolit............................................................42
5.4. Thế năng và ma trận G............................................................................................43
5.5. Động lực học ngược................................................................................................43
CHƯƠNG 6: THIẾT KẾ BỘ ĐIỀU KHIỂN VÀ MÔ PHỎNG.......................................44
6.1. Cơ sở lý thuyết........................................................................................................44
6.2. Mô phỏng bằng Simmechanic Toolbox..................................................................47
6.3. Đồ thị kết quả mô phỏng........................................................................................50
PHỤ LỤC..........................................................................................................................53
1. CODE Động học ngược vị trí:...................................................................................53
2. CODE Động học ngược vận tốc, gia tốc:..................................................................54
3. CODE Không gian thao tác:......................................................................................54
4. CODE Tính động lực học ngược:..............................................................................59
5. CODE mô phỏng điều khiển PD:..............................................................................60
TÀI LIỆU THAM KHẢO.................................................................................................62

Lời mở đầu
Hiện nay khoa học kỹ thuật đang phát triển rất nhanh, mang lại những lợi ích cho
con người về tất cả những lĩnh vực trong cuộc sống. Để nâng cao đời sống nhân dân và
hòa nhập với sự phát triển chung của thế giới, Đảng và nhà nước ta đã đề ra những mục
tiêu đưa đất nước đi lên thành một nước công nghiệp hóa hiện đại hóa. Để thực hiện điều
đó thì một trong những ngành cần quan tâm phát triển nhất đó là ngành cơ khí nói chung
và cơ điện tử nói riêng vì nó đóng vai trò quan trọng trong việc sản xuất ra các thiết bị
công cụ (máy móc, robot...) của mọi ngành kinh tế.
Muốn thực hiện việc phát triển ngành cơ khí cần đẩy mạnh đào tạo đội ngũ cán bộ
kĩ thuật có trình độ chuyên môn đáp ứng yêu cầu của công nghệ tiên tiến, công nghệ tự
động hóa theo dây chuyền sản xuất. Đóng góp vào sự phát triển nhanh chóng của nền
khoa học công nghiệp, tự động hóa đóng vai trò vô cùng quan trọng. Vì vậy công nghệ tự
động hóa được đầu tư và phát triển một cách mạnh mẽ.
Trong công nghiệp hiện nay nói chung, việc máy móc tự động dần thay thế con
người đang trở thành xu thế tât yếu. Nhằm tạo ra 1 hệ thống điều khiển cho robot công
nghiệp phục vụ công việc và nghiên cứu và đưa vào thực tiễn giúp nâng cao năng suất lao
động. Chúng em đã thiết kế mô hình robot ứng dụng trong bốc xếp kho hàng. Trong khi
làm bài không tránh khỏi những sai sót. Rất mong được những đóng góp của các thầy và
các bạn để chúng em sửa chữa, khắc phục và làm tốt hơn trong những lần sau.
Cuối cùng chúng em xin trân thành cảm ơn sự chỉ bảo tận tình của thầy cô trong
Bộ Môn Cơ Ứng Dụng và đặc biệt là sự hướng dẫn của thầy ThS. GVC. Nguyễn Văn
Quyền đã giúp em học được rất nhiều điều và hoàn thành môn học này.
CHƯƠNG 1: TỔNG QUAN VỀ ROBOT

1.1 Khái niệm Robot công nghiệp
Theo tiêu chuẩn AFNOR của Pháp. Khái niệm về robot công nghiệp là một cơ cấu
chuyển động tự động có thể lập trình, lặp lại các chương trình, tổng hợp các chương trình
đặt ra trên các trục tọa độ, có khả năng định vị, định hướng, di chuyển các đối tượng vật
chất như chi tiết, đạo cụ, gá lắp theo những hành trình thay đổi đã được chương trình hóa
nhằm thực hiện các nhiệm vụ công nghệ khác nhau.
Theo tiêu chuẩn RIA của Mỹ, Robot institute of America. Khái niệm về robot công
nghiệp là một tay máy vạn năng có thể lặp lại các chương trình, được thiết kế để di
chuyển vật liệu, chi tiết, dụng cụ, hoặc các thiết bị chuyên dùng thông qua các chương
trình chuyển động có thể thay đổi để hoàn thành các nhiệm vụ khác nhau.
Theo tiêu chuẩn TOCT 25686-85 của Nga. Robot công nghiệp là một máy tự
động, được đặt cố định hoặc di động được, liên kết giữa một tay máy và một hệ thống
điều khiển theo chương trình, có thể lặp đi lặp lại để hoàn thành các chức năng vận động
và điều khiển trong quá trình sản xuất.
Do đó, robot công nghiệp có thể được hiểu là những thiết bị tự động linh hoạt.
Thực hiện các chức năng theo chương trình đã được lập trình sẵn. Với đặc điểm có thể
lập trình lại được, robot công nghiệp là thiết bị tự động hóa và ngày càng trở thành bộ
phận không thể thiếu được của các hệ thống sản xuất linh hoạt.
1.2 Phân loại Robot công nghiệp
Ở thời điểm hiện nay, nền công nghiệp phát triển với tốc độ chóng mặt, vì vậy
chúng ta có thể dễ dàng nhận ra sự hiện diện của Robot đặc biệt là ở những nước phát
triển. Trong công nghiệp hiện nay chúng ta có thể phân loại Robot công nghiệp như sau:
1.2.1 Phân loại theo kết cấu.
Theo kết cấu (hay theo hình học), người ta phân Robot thành các loại: đề các, trụ, cầu,
Sacara, kiểu tay người, robot nối tiếp hoặc song song và các dạng khác nữa.
1.2.2 Phân loại theo điều khiển
Có 2 kiểu điều khiển robot: Điều khiển hở và Điều khiển kín.

- Điều khiển hở: dùng truyền động bước (động cơ điện hoặc động cơ thủy lực, khí
nén) mà quãng đường hoặc góc dịch chuyển tỷ lệ với xung điều khiển. Kiểu này
đơn giản nhưng cho độ chính xác thấp.
- Điều khiển kín: (điều khiển kiểu servo) sử dụng tín hiệu phản hồi vị trí để tăng độ
chính xác điều khiển. Có hai kiểu điều khiển servo: Điều khiển điểm-điểm và điều
khiển theo đường (contour).
- Kiểu điều khiển điểm-điểm: phần công tác dịch chuyển từ điểm này đến điểm kia
theo đường thẳng với tốc độ không cao. Kiểu điều khiển này thường được dùng
trên các Robot hàn điểm, vận chuyển, tán đinh và bắn đinh.
- Điều khiển contour: đảm bảo cho phần công tác dịch chuyển theo quỹ đạo bất kì,
với tốc điều khiển được. Có thể gặp kiểu điều khiển này trên các Robot hàn hồ
quang và phun sơn.
1.2.3 Phân loại theo ứng dụng
Cách phân loại này dựa vào ứng dụng của Robot. Robot hàn, robot cắt, robot gắp
sản phẩm, robot sơn, robot lắp ráp, robot pallet, robot cộng tác
1.3 Ứng Dụng của Robot công nghiệp
Robot hàn mig

Robot hàn là robot công nghiệp sử dụng chủ yếu cho nguyên công hàn trong các
dây chuyền sản xuất cơ khí
Những lợi ích khi sử dụng robot hàn
– Có độ chính xác và năng suất cao, nâng cao độ tin cậy của mối hàn. Một khi được lập
trình hợp lý, các robot sẽ tạo ra những mối hàn y như nhau trên các vật hàn cùng kích
thước và quy cách.
– Chuyển động của mỏ hàn được tự động hóa sẽ giảm nguy cơ mắc lỗi trong thao tác, do
vậy giảm phế phẩm và khối lượng công việc phải làm lại. Robot không những làm việc
nhanh hơn mà còn có thể hoạt động liên tục suốt ngày đêm, hiệu quả hơn nhiều so với
một thiết bị hàn tay.
– Quá trình hàn được tự động hóa giải phóng người công nhân khỏi những tác hại khi hàn
do tiếp xúc với bức xạ hồ quang, vẩy hàn nóng chảy, khí độc.
– Tốc độ: một robot có thể hàn nhanh hơn so với một con người, vì đèn hàn luôn nằm
theo hướng tối ưu và di chuyển càng nhanh càng tốt. Ngoài ra, chu kỳ hàn của robot luôn
giữ nguyên. Điều này giúp quá trình hàn dễ dàng hơn và hiệu suất hàn dễ tính toán hơn,
nhờ vậy khách hàng có thể tưởng vào việc nhận được sản phẩm đúng theo thời gian đã
hứa.
– Thời gian xử lý được tiết kiệm theo một cách khác. Do mối hàn robot nói chung là sạch
hơn so với mối hàn của người, nên ít phải thay thế và dọn dẹp hơn. Điều này tiết kiệm
thêm thời gian trong các thao tác đầu ra.
– Chất lượng: Chuyển động nhất quán cũng có nghĩa là chất lượng nhất quán. Với đèn
hàn và các mảnh ghép ở cùng vị trí và di chuyển với cùng tốc độ, chúng sẽ được hàn
giống nhau. Nguy cơ biến dạng không xuất hiện, và do robot không biết mệt mỏi là gì,
nên sản phẩm hàn sẽ giống nhau từ đầu tới cuối. Đặc biệt, với các bộ phận đặc biệt quan
trọng, tự động hóa mang lại cho khách hàng sự tin tưởng vào sự toàn vẹn của mỗi mối
hàn.
Robot Yaskawa Motoman MPL160

Với sự đa dạng trong các dòng sản phẩm Robot gắp nhả bốc dỡ hàng hóa của
Yaskawa có thể cung cấp cho khách hàng các loại robot cũng như hệ thống đi kèm để
phục vụ trong việc tự động hóa các thiết bị máy móc, dây chuyền sản xuất nhằm tối ưu
quy trình sản xuất, nâng cao năng lực cạnh tranh.
Ứng dụng:
 Chúng ta sử dụng robot loại handling hoặc palletizing tùy theo ứng dụng:
 Bốc xếp bao từ dây chuyển sản xuất lên pallet.
 Bốc xếp các loại thùng hàng.
 Di chuyển sản phẩm từ vị trí này sang vị trí khác.
Giải pháp: Thiết kế giải pháp tổng thể: thiết kế băng tải ra hàng, robot, cấp pallet tự
động, di chuyển pallet sang vị trí khác sau khi xếp hàng. Thiết kế tay gắp hàng( bằng khí
nén hay sử dụng servo tùy theo loại sản phẩm cần gắp). Mô phỏng, thiết kế hệ thống.
Cung cấp giải pháp tối ưu cho hoạt động của robot và đảm bảo an toàn khi vận hành.
Mục đích: Việc sử dụng nhân công để bốc xếp hàng hóa gây tốn kém chi phí cũng
như ảnh hưởng đến năng suất lao động về lâu dài. Ngoài ra, sử dụng nhân công cũng tiềm
ẩn nhiều nguy hiểm cho an toàn lao động do phụ thuộc tình trạng sức khỏe, tinh thần
nhân công khi làm việc. Đồng thời đối với các môi trường làm việc nhiều khói bụi, độc
hại như các nhà máy sản xuất xi măng thì việc sử dụng robot sẽ đảm bảo được an toàn lao
động cũng như sức khỏe của người vận hành.
Robot sơn abb

Robot sơn có thể mang lại hiệu quả ứng dụng cao như bất kỳ hệ thống sơn tự động
nào, nếu nó được sử dụng theo đúng phương pháp kỹ thuật. Ngược lại, trong trường hợp
nó được trang bị những thiết bị tốt nhất nhưng kỹ thuật viên không có kỹ năng vận hành
và tích hợp hệ thống, họ vẫn không thể có được hiệu quả ứng dụng và hiệu suất vận hành
cao từ phía robot.
Tận dụng tối đa hiệu suất robot .Tính năng hữu ích của robot sơn là độ chính xác
và khả năng lặp lại. Robot có thể di chuyển từ điểm A đến điểm B với cùng một tốc độ
liên tục. Vì thế, nếu kỹ thuật viên tận dụng được hiệu suất này, hệ thống vận hành của
robot sẽ đạt được năng suất và hiệu quả kinh tế cao ngay cả khi đang làm việc ở môi
trường kém tác động.
Robot yaskawa cho nghành chế biến gỗ

Là robot được thiết kế đặc biệt, dành riêng cho các khâu đóng gói, xếp loại nhiều
pallet, sản phẩm thành phẩm như thùng carton, túi/ bao, hộp, thùng, chai/lọ trong nhiều
lĩnh vực sản xuất công nghiệp khác nhau.
Cấu trúc của robot palettizer đơn giản, rất dễ và thuận tiện trong bảo trì và sửa
chữa. Các bộ phận & phụ kiện của máy được tối giản để hạn chế phát sinh các chi tiết
phải bảo trì.
Hoạt động của robot palettizer rất linh hoạt thậm chí ở trong những không gian
nhỏ hẹn những vẫn đảm bảo hiệu suất cao trong làm việc.
Tất cả các điều khiển vận hành và hoạt động đều được hiển thị trên màn hình hiển
thị của tủ điều khiển, robot có thể vận hành 24h/ ngày liên tục và hiệu năng của robot
bằng nhiều lao động nên tiết kiệm rất nhiều nhiều chi phí nhân công sản xuất trong một
thời gian dài…
CHƯƠNG 2: PHÂN TÍCH LỰA CHỌN KẾT CẤU ROBOT

2.1. Phân tích mục đích ứng dụng của Robot
- Mục đích: Robot ứng dụng trong việc bốc xếp kho hàng, Robot di chuyển đối tượng
thao tác giữa hai dây chuyền công nghệ.
- Mô tả:
 Robot cần hút được đối tượng tại vị trí nhất định.
 Mang đối tượng di chuyển không bị cản đến vị trí đặt.
 Đặt đối tượng đúng vị trí.
 Trở về vị trí hút đối tượng tiếp theo.
 Quá trình lặp lại.
 Đảm bảo thời gian thực hiện các thao tác (chu trình thao tác).
Trong thực tế việc bốc xếp, dịch chuyển hàng trong kho giữa 2 dây chuyền hay từ dây
chuyền xuống xe đẩy là một công việc đơn điệu, nặng nhọc. Nếu sử dụng con người thì
không đáp ứng được về năng suất. Yêu cầu về vị trí gắp thả phải chính xác vì vậy cần
phải có Robot thay con người làm những công việc này.
2.2. Phân tích yêu cầu kĩ thuật
2.2.1. Phân tích về yêu cầu vị trí, hướng của khâu thao tác.
a) Thông số kĩ thuật của đối tượng

Đối tượng thao tác: hàng dạng hộp
Vật liệu: kim loại
Khối lượng: 1kg
Hình dạng của đối tượng

Các thông số về hình dáng, kích thước của phôi được thể hiện như hình vẽ
b) Dạng thao tác:
- Để hút đối tượng từ 2 băng tải có vị trí vuông góc với nhau để thực hiện 1 quy trình sản
xuất tự động
=> Vật có dạng hình hộp chữ nhật, khối lượng không lớn, sử dụng bơm hút chân không
để di chuyển vật.
2.2.2. Số bậc tự do cần thiết
Lô quấn được đặt thẳng đứng theo phương z vì vậy hướng thao tác phải là hướng
thẳng đứng theo phương z và hướng này không thay đổi trong thời gian hoạt động.
=> Thiết kế robot có đủ bậc tự do và đáp ứng sẵn được hướng yêu cầu.
=> Robot tọa độ trụ có tối thiểu 3 bậc tự do.
2.2.3. Không gian làm việc
Kích thước của dây truyền:

Dài: 5000 mm
Rộng: 600 mm
Chiều cao:600 mm
Vị trí của Robot so với dây chuyền:
Nằm giữa thùng đựng hàng và dây chuyền được mô tả như hình vẽ.
Ta chọn điểm đặt robot như sau:
Smax = 1346mm
Smin = 559 mm
2.3. Các phương án thiết kế cấu trúc Robot

a) Một số phương án thiết kế robot 3 bậc tự do
Robot 3 bậc tịnh tiến (T.T.T)
Robot gồm 3 bậc tịnh tiến theo 3 phương X, Y và Z. Do kết cấu nên robot
có độ cứng cơ học theo phương tiếp tuyến lớn, phù hợp với các quá trình vận
chuyển trên một khoảng cách dài. Bên cạnh đó với việc điều khiển các khâu tịnh
tiến xử lý dễ hơn trục quay nên khâu thao tác có thể định vị rất chính xác. Robot
thường được ứng dụng vào trong các quá trình láp ráp, vận chuyển
 Ưu điểm:
+ Do kết cấu đơn giản, robot có độ cứng vững cao.
+ Độ chính xác được đảm bảo trên toàn bộ không gian làm việc
 Nhược điểm:
+ Diện tích chiếm dụng robot lớn
+ Hạn chế về khả năng linh hoạt
Cánh tay robot 3 bậc quay (R.R.R)
Cánh tay gồm 1 khâu quay theo phương Z và 2 khâu quay theo phương X.
3 khâu quay này phối hợp với nhau để tạo chuyển động khâu thao tác trong không
gian.
 Ưu điểm:
+ Robot có độ linh hoạt cao
+ Vùng làm việc rộng so với khối lượng của Robot
 Nhược điểm
+ Kết cấu phức tạp
+ Khó điều khiển đối với những chuyển động thẳng
+ Độ cững vững của Robot giảm dần khi thao tác ở xa robot
Robot 3 bậc quay Scara (R.R.T)
Khâu 1, 2 quay và khâu 3 tịnh tiến theo trục z. Khâu 3 đảm bảo dao luôn
vuông góc với bàn máy. Khâu 1 và 2 đảm bảo bao quát các điểm trên mặt phẳng.
Khâu 3 tịnh tiến theo trục z giúp tay hút chuyển động lên, xuống.
 Ưu điểm:
+ Dễ dàng thực hiện thao tác đối với các đối tượng trên mặt
phẳng
+ Kết cấu đơn giản và linh hoạt
+ Ổn định về mặt cơ học theo phương thẳng đứng
 Nhược điểm:
+ Robot thường chỉ mang tải trọng nhẹ
Qua ưu điểm và nhược điểm của các loại robot trên thì nhóm chúng
em quyết định lựa chọn mô hình cánh tay robot 3 bậc R.R.T (Phương án 3)
để thực hiện đề tài. Đề xuất mô hình robot đáp ứng được các yêu cầu trên:
Robot SCARA được đặt cố định trên nền với những ưu điểm: Kết cấu động
học đơn giản nhưng vẫn đảm bảo yêu cầu kỹ thuật.
Quá trình thao tác của robot:
- Từ vị trí ban đầu, khâu 3 (đầu hút chân không) di chuyển theo phương trục Z tới
vị trí vật và giữ vật.
- Sau đó, robot di chuyển tới 1 trong 2 băng tải để đưa vật về kho tùy theo kích
thước được phân loại bởi cảm biến.
- Khi đến băng tải đưa hàng vào kho, robot di chuyển theo trục Z hạ vật xuống,
nhả kẹp và về vị trí ban đầu
Mô hình lắp đặt robot và các băng tải

2.4. Các đặc trưng kĩ thuật
Hình 2.4 Không Gian làm Việc

Các kích thước phải thỏa mãn điều kiện:
a1 + a2 > smax
|a1 - a2 | < smin
l3 > smax – smin
Ta chọn được kích thước như sau:

a1 = 500mm, -120≤ q1 ≤ 120 (deg)
a2 = 500mm, -120 ≤ q2 ≤ 120 (deg)
l3 = 550mm, 50 ≤ d3 ≤ 550 (mm)
CHƯƠNG 3: TÍNH TOÁN ĐỘNG HỌC

3.1 Xây dựng bài toán động học của Robot
3.1.1. Cấu trúc động học của robot
Robot nói chung và đặc biệt robot dạng tay máy có cấu trúc cơ học là một hệ nhiều vật,
gọi là khâu, liên kết với nhau bởi các khớp.
Khả năng chuyển động của robot được đặc trưng bởi số khả năng chuyển động độc lập
của các khâu, gọi là số bậc tự do của robot. Bậc tự do của robot phụ thuộc vào số khâu,
cấu trúc và phân bố các khớp.
Để tính số bậc tự do ta sử dụng công thức:
k
f =λ (n−k )+ ∑ f i +¿ f c −f ¿ p ¿ (3.1)
i=1 ¿
Trong đó:
 f : Số bậc tự do của cơ cấu.
 λ : Bậc tự do của không gian trong đó tay máy thực hiện chuyển động
 n: Số khâu động của robot
 k: Số khớp của robot
 f i: Số bậc tự do của khớp thứ i
 f c: Số ràng buộc trùng
 f p: Số ràng buộc thừa
Với robot trong đồ án này là robot SCARA với số bậc tự do là: 6(3 - 3) + 3 + 0 + 0 = 3
3.1.2. Phương pháp Denavit – Hartenberg
Các hệ tọa độ
Khâu đế hệ cố định 0: Hệ tọa độ cố định O0 x 0 y 0 z 0

Khâu động: khâu 1,2,3(EE): Hệ tọa độ động O1 x 1 y 1 z 1 ; O2 x 2 y 2 z 2 ; O3 x 3 y 3 z 3
Phép biến đổi thuần nhất
Hình 3.1: Phép biến đổi tọa độ thuần nhất
[ ]
i−1 i−1
i−1 Ri (q) r i (x O , y O , z O )
A i (q )= i i i
0T 1
i−1
Ri (q) : Ma trận 3x3 phép quay biến i-1 thành i
r i−1
i (x O , y O , z O ): Tọa độ gốc Oi trong hệ tọa độ i-1
i i i
Ma trận biến đổi tọa độ thuần nhất Denavit - Hartenberg
[ ]
cos θi −cos α i sin θ i sin α i sin θ i a i cos θ i
sin θi cos α i cos θ i −sin α i cos θi ai sin θ i
Ai =
i−1
0 sin α i cos α i di
0 0 0 1
(3.2)
Ma trận biến đổi tọa độ độ thuần nhất A0n biểu diễn trạng thái khâu thao tác có thể xác
định từ từ cấu trúc động học robot. A0n có thể nhận được bằng cách nhân phải liên tiếp các
ma trận biến đổi tọa độ thuần nhất ứng với các phép dịch chuyển hệ tọa độ từ hệ trục cố
điịnh tới hệ trục tọa độ gắn với EE.
0 0 1 2 n−1
An =A 1 A 2 A 3 … A n
0 0 1 2 n−1
An ( q ) = A1 ( q ) A 2 ( q ) A 3 ( q ) … A n ( q )
[ ]
0 0
0 R n (q) r E (q)
An (q)=
0 1
[ ]
c 11 (q) c 12(q) c13 (q)
0
R (q)= c21 (q) c 22(q) c 23 (q)
n
c31 (q) c 32(q) c33 (q)
T
r E (q )=[ x (q) y (q) z (q ) ]
0
T
x (q)=[ q1 q2 ... q n ]
q i=
{
θ i−Revolute joint
d i−Prismatic joint
[ ]
c 11 (q) c 12(q) c 13 (q) x (q)
0 c (q) c 22(q) c 23 (q) y (q)
An ( q )= 21
c31 (q) c 32(q) c 33 (q) z (q)
0 0 0 1
3.1.3. Xây dựng cấu trúc động học

Hình 3.2: Sơ đồ động học

Bảng tham số D-H
J θi di α ai
1 θ1 0 0 a1
2 θ2 0 180 a2
3 0 d3 0 0
Thay các dữ kiện:

J θi di α ai
1 θ1 0 0 500
2 θ2 0 180 500
3 0 500 0 0
Miền làm việc

Không gian làm việc 3D
Không gian làm việc 2D

3.2. Bài toán động học thuận về vị trí
Sử dụng công thức (3.2) ta thu được các ma trận biến đổi tọa độ thuần nhất:
Đầu tiên ta thiết lập phương trình động học dạng ma trận.
[ ]
cos ( θ 1 ) −sin ( θ 1) 0 500 cos ( θ1 )
A01 = sin ( θ 1) cos ( θ 1 ) 0 500 sin ( θ1 )
0 0 1 0
0 0 0 1
[ ]
cos ( θ 2) sin ( θ 2 ) 0 500 cos ( θ2 )
A12= sin ( θ2 ) −cos ( θ2 ) 0 500 sin ( θ2 )
0 0 −1 0
0 0 0 1
[ ]
1 0 0 0
2 0 1 0 0
A3 =
0 0 1 d3
0 0 0 1
Ma trận biến đổi tọa độ thuần nhất giữa khâu thao tác và khâu cố định:
[ ]
cos ( θ1+ θ2 ) sin ( θ1 +θ2 ) 0 500 cos ( θ1+ θ2 ) +500 cos ( θ 1)
0 0 1 2 0 sin ( θ 1+θ 2 ) −cos ( θ1 +θ2 ) 0 500 sin ( θ1+ θ2 ) +500 sin ( θ1 )
T E =A 1 . A 2 . A3T E =
0 0 −1 −d 3
0 0 0 1
[ ]
cos φ sin φ 0 xE
sin φ −cos φ 0 yE
T 0E =
0 0 −1 zE
0 0 0 1
Thiết lập phương trình động học dạng ma trận:
[ ][ ]
cos φ sin φ 0 xE cos ( θ1 +θ2 ) sin ( θ 1+θ 2) 0 500 cos ( θ1 +θ2 ) +500 cos ( θ1 )
sin φ −cos φ 0 yE sin ( θ1 +θ2 ) −cos ( θ 1+ θ2 ) 0 500 sin ( θ1 +θ2 ) +500 sin ( θ 1 )
=
0 0 −1 zE 0 0 −1 −d 3
0 0 0 1 0 0 0 1
→ Cân bằng các phần tử trong ma trận 2 vế ta có:
{
¿ x E =500 cos ( θ1 +θ2 ) +500 cos ( θ1 )
¿ y E =500 sin ( θ1+ θ2 ) +500 sin ( θ1 )
¿ z E=−d3
¿ φ=θ1 +θ 2
3.3. Bài toán động học ngược về vị trí
{
¿ x E =500 cos ( θ1 +θ2 ) +500 cos ( θ1 ) (1)
¿ y E=500sin ( θ 1+ θ2 ) +500 sin ( θ1 ) (2)
¿ z E =−d 3 (3)
¿ φ=θ1+ θ2 (4 )
(3)→ d 3=−Z E
Lấy phương trình (1) bình phương cộng phương trình (2) bình phương ta thu được:
2 2 2 2
x E + y E −a1 −a 2 =2 a1 . a 2 . cos (¿ θ2) ¿
2 2 2 2
x E + y E −a1 −a2
⇒ θ 2=± arccos(¿ )¿
2. a1 . a 2
Khi đã biết θ2 thay lại vào 2 phương trình (1), (2) để tìm θ1
Ta thu được (1) và (2) là hệ phương trình đại số tuyến tính với 2 biến sin(θ1) và cos(θ1)
[ ][
xE
yE
a cos ( θ2 ) + a1 −a2 sin ( θ 2 )
= 2
a2 sin ( θ2 )
.
][ ]
cos ( θ1 )
a 2 cos ( θ 2 ) +a1 sin ( θ 1)
Giải ra được:
[ ]
cos ( θ1 )
sin ( θ1 )
=¿
⇒ θ 1=arctan2 ¿
Trong vùng làm việc linh hoạt mỗi tọa độ khâu thao tác sẽ có 2 cách đáp ứng tương ứng
với 1 bộ θ1 , θ2.
3.4. Bài toán động học vi phân
3.4.1. Bài toán động học vi phân thuận
Bài toán động học vi phân thuận là bài toán biết q , q̇ , q̈ cần tìm ṗ , p̈ .
Tính vận tốc góc ω E
Ta có ma trận biến đổi tọa độ thuần nhất:
[ ]
cos ( θ1+ θ2 ) sin ( θ1 +θ2 ) 0 500 cos ( θ1+ θ2 ) +500 cos ( θ 1)
0 sin ( θ 1+θ 2 ) −cos ( θ1 +θ2 ) 0 500 sin ( θ1+ θ2 ) +500 sin ( θ1 )
T E=
0 0 −1 −d 3
0 0 0 1
Ta suy ra được ma trận cosin chỉ hướng R0Ebằng việc lấy các phần tử thuộc 3 hàng và 3
cột đầu tiên của ma trận T 0E
[ ]
cos ( θ1 +θ 2) sin ( θ 1+θ 2 ) 0
0
R = sin ( θ1 +θ2 ) −cos ( θ1+ θ2 ) 0
E
0 0 −1
Từ đây ta suy ra được ma trận sóng của vector ~

ω E:
[ ][ ]
0 −θ̇1−θ̇ 2 0 0 −ω z ω y
( )
0
∂ R E dq
. R E ¿ θ̇ 1+ θ̇ 2 0 = ωz −ω x
0 0T 0T
¿ Ṙ . R =
E E . 0 0
∂ q dt
0 0 0 −ω y ω x 0
[ ]
0
0
ωE = 0
θ̇1 + θ̇2
Tính vận tốc dài v E

Từ ma trận T 0E ta thu được ma trận tọa độ điểm tác động cuối r 0E
0
∂ r E dq
vE= . =J TE . q̇
∂ q dt
Tính gia tốc góc ε 0E
Để tính ε 0E ta đạo hàm ω 0E theo thời gian.
[ ]
d ωE
0 0
0
ε =
E = 0
dt
θ̈1 + θ̈2
Tính gia tốc dài a E

Để tính gia tốc dài a E ta đạo hàm v E theo thời gian.
d vE
aE= =J TE . q̈ + J̇ TE . q̇
dt
3.4.2. Bài toán động học vi phân ngược
Bài toán động học vi phân ngược là bài toán biết ṗ , p̈ cần tìm q̇ , q̈ .
Tính q̇
Đầu tiên ta sẽ thiết lập các phương trình liên kết f ( p , q) :
{
¿ 500 cos ( θ1 +θ2 ) +500 cos ( θ1 )−x E=0
¿500 sin ( θ 1+θ 2 ) +500 sin ( θ1 )− y E=0
¿−d 3 −z E=0
¿ θ 1+θ 2−φ=0
Sau đó ta đạo hàm phương trình liên kết theo thời gian ta được:
df ( p , q) ∂ f dp ∂ f dq ∂f −∂ f
= + =0 ⇒ q̇= ṗ
dt ∂ p dt ∂ q dt ∂q ∂p
∂f ∂f
Đặt =J q ,− =J p ta có
∂q ∂p
¿> q̇=J q J p ṗ
−1 (3.3)
[ ]
−a2 sin ( θ 1+θ 2 )−a 1 sin ( θ 1) −a 2 sin ( θ1 +θ 2) 0
∂f
=J q = a 2 cos ( θ1+ θ2 ) +a1 cos ( θ 1 ) a2 cos ( θ 1+θ 2 ) 0
∂q
0 0 −1
[ ]
1 0 0
∂f
=J p= 0 1 0
∂p
0 0 1
Từ đây ta tính được q̇ theo công thức (3.3).
[ ]
cos ( θ1 +θ2 ) ẋ+ sin ( θ1+ θ2 ) ẏ
a1 sin ( θ2 )
q̇= −cos ( θ 1+ θ2 ) a2 ẋ −sin ( θ1 ) a1 ẏ−cos ( θ1 ) a1 ẋ −sin ( θ1 +θ2 ) a2 ẏ
a 1 sin ( θ 2) a2
− ż
Tính q̈ :
Để tính q̈ ta đạo hàm phương trình J q q̇−J p ṗ=0 theo thời gian được:
J q q̈+ J̇ q q̇−J p p̈−J̇ p ṗ=0¿> q̈=J q−1 ( J̇ q q̇−J p p̈− J̇ p ṗ)
(3.4)
Từ đây ta tính được q̈ theo công thức (3.4).
CHƯƠNG 4: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG

THEO YÊU CẦU KỸ THUẬT
4.1. Thiết kế quỹ đạo trong không gian thao tác:
- Xét quy trình thao tác của ROBOT như sau:
 Di chuyển từ vị trí A’ (0.8;0.2; -0.01) đến vị trí A (0.8;0.2; -0.46) – vị trí
thùng hàng trong khoảng 2 giây.
 Dừng tại vị trí A (0.8;0.2; -0.46) khoảng 1 giây để hút thùng hàng.
 Di chuyển từ vị trí A (0.8;0.2; -0.46) đến vị trí A’ (0.8;0.2; -0.01) trong
khoảng 2 giây.
 Di chuyển từ vị trí A’ (0.8;0.2; -0.01) đến vị trí B’ (-0.4; -0.8; -0.01) trong
khoảng 3 giây.
 Di chuyển từ vị trí B’ (-0.4; -0.8; -0.01) đến vị trí B (-0.4; -0.8; -0.26) – vị
trí thả thùng hàng lên băng tải khác trong khoảng 2 giây.
 Dừng tại vị trí B (-0.4; -0.8; -0.26) khoảng 1 giây để thả thùng hàng.
 Di chuyển từ vị trí B (-0.4; -0.8; -0.26) đến vị trí B’ (-0.4; -0.8; -0.01) trong
khoảng 2 giây.
 Di chuyển từ vị trí B’ (-0.4; -0.8; -0.01) đến vị trí A’ (0.8;0.2; -0.01) trong
khoảng 3 giây.
 Kết thúc một chu trình thao tác.
Hình: Chu trình thao tác của ROBOT

Trong quá trình thao tác để có thể thực hiện được thao tác gắ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 thùng hàng trên băng truyền thì hướng tiếp
cận của tay hút 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’, ở đâ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 :
A’ (0.8;0.2; -0.01) là điểm bắt đầu và kết thúc chu trình thao tác
A (0.8;0.2; -0.46)
B’ (-0.4; -0.8; -0.01)
B (-0.4; -0.8; -0.26)
Quỹ A’A Dừng hút AA’ A’B’ B’B Dừng thả BB’ B’A’
đạo vật vật
Thời 2 1 2 3 1.5 1 1.5 2.5
gian(s)
Để 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.
+ 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 và
là :
x−x 0 y − y 0 z−z 0
= =
x e−x 0 y e − y 0 z e−z 0
Độ 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:
{
2 3
x=a 0+ a1 t +a 2 t + a3 t
2
ẋ=a1 +2 a2 t+3 a 3 t
ye− y0 y0 xe− y e x0
y= x+
x e −x 0 x e −x 0
z e −z 0 z 0 x e −z e x 0
z= x+
x e −x 0 x e −x 0
Với thời gian đặt cho quỹ đạo là t e(s) để đi từ M đến N theo một đường
thẳng. Với điều kiện đầu:
¿
Ta xác định được các hệ số như sau: ¿
4.2. Xây dựng chương trình mô phỏng trên Matlab
Nhóm em thiết lập sơ đồ khối simulink như sau:
4.3. Kết quả thiết kế quỹ đạo trong không gian thao tác khi thực hiện chu
trình công việc.
Đồ thị giá trị xE (m)
Đồ thị giá trị yE (m)

Đồ thị giá trị zE (m)
Đồ thị vận tốc thao tác cuối (E) theo các trục toạ độ
Đồ thị gia tốc thao tác cuối (E) theo các trục toạ độ
Đồ thị biến khớp q1 (rad) theo thời gian t

Đồ thị biến khớp q2 (rad) theo thời gian t
Đồ thị biến khớp d3 (m) theo thời gian t

Đồ thị vận tốc khâu 1 (m/s) theo thời gian t (s)

Đồ thị gia tốc khâu 1(m/s^2) theo thời gian t (s)

Đồ thị gia tốc khâu 2 (m/s^2) theo thời gian t (s)
Đồ thị gia tốc khâu 3 (m/s^2) theo thời gian t (s)

Đồ thị Lực dẫn động khâu 1 (N.m) theo thời gian t (s)
Đồ thị lực dẫn động khâu 2 (N.m) theo thời gian t (s)
Đồ thị lực dẫn động khâu 3 (N) theo thời gian t (s)
CHƯƠNG 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
Phương trình Lagrange loại 2:
( )
d ∂T
dt ∂ q̇
−
∂T ∂Π
+
∂ qi ∂ qi
=U
Viết dưới dạng khai triển:

M¿ (5.1)
Trong đó:
M: Ma trận khối lượng
C: Ma trận quán tính Coriolit
U: Ma trận lực điều khiển
∂Π
G: G = ∂ q
i
5.2. Động năng và ma trận khối lượng M
5.2.1. Tính các tenxơ quán tính

Các tenxơ quán tính của 3 khâu so với tục gắn với khối tâm song song với hệ trục của
khâu cũng tương ứng là hệ quán tính chính.
[ ]
0 0 0
1 2
0 m 1 a1 0
Θ 1= 12
1 2
0 0 m 1 a1
12
[ ]
0 0 0
1 2
0 m a 0
Θ 2= 12 2 2
1 2
0 0 m2 a2
12
[ ]
0 0 0
1 2
0 m3 a3 0
Θ 3= 12
1 2
0 0 m 3 a3
12
5.2.2. Tính các ma trận Jacobi
5.2.2.1. Ma trận Jacobi tịnh tiến

0
∂ r Ci
J Ti = (5.2)
∂q
Ma trận Jacobi tịnh tiến khâu 1:
[ ]
−a1
sin ( θ1 ) 0 0
∂r
0 2
J T 1 = C 1 = a1
∂q cos ( θ1 ) 0 0
2
0 0 0
[ ]
−a 2 −a 2
sin ( θ1 +θ2 )−a1 sin ( θ1 ) sin ( θ1 +θ2 ) 0
0
∂ rC 2 2 2
JT 2= = a2 a2
∂q cos ( θ1 +θ2 ) + a1 cos ( θ1 ) cos ( θ1 +θ2 ) 0
2 2
0 0 0

[ ]
−a2 sin ( θ1 +θ2 )−a1 sin ( θ1 ) −a2 sin ( θ1+ θ2 ) 0
∂r C0 3
JT 3= = a2 cos ( θ1 +θ2 ) + a1 cos ( θ1 ) a 2 cos ( θ1 +θ2 ) 0
∂q
0 0 −1
5.2.2.2. Ma trận Jacobi quay

∂ω i
J Ri= (5.3)
∂ q̇
Ma trận Jacobi quay khâu 1:
[ ]
0 0 0
JR1= 0 0 0
1 0 0
[ ]
0 0 0
JR2= 0 0 0
−1 −1 0
[ ]
0 0 0
JR3= 0 0 0
−1 −1 0
5.2.3. Động năng và ma trận khối lượng M
{ }
n
1
T = q̇ T
2
∑ (J TTi mi J Ti+ J TRi Θi' J Ri) q̇ (5.4)
i=1
M¿
M¿
1 2 1 2 2 2 2
m11= m1 a1 + m2 a2 + a1 m3 +2 a1 a2 m3 cos ( θ2 ) +a 1 a 2 m2 cos ( θ2 ) + a1 m2 +m3 a2
3 3
1 2 1 2
m12= m2 a2 + a1 a2 m2 cos ( θ2 ) + m3 a 2 +a 1 a 2 m3 cos ( θ2 )
3 2
1 2 1 2
m21= m2 a2 + a1 a2 m2 cos ( θ 2) + m3 a 2 +a 1 a 2 m3 cos ( θ2 )
3 2
1 2 2
m22= m2 a2 + m3 a 2
3
m33=m3
5.3. Ma trận quán tính li tâm và gia tốc Coriolit
[]
c1
C (q , q̇) q̇= c2 (5.5)
c3
4
Trong đó: c j= ∑ ¿ ¿
k ,l=1
Với: ¿
[]
c1
Suy ra: C (q , q̇) q̇= c 2
0
Trong đó:
1 2
c 1=θ̇ 1 θ̇2 (−2 a1 a2 m3 sin ( θ 2 )−a 1 a 2 m2 sin ( θ2 ) ) + θ̇2 (−2 a1 a2 m3 sin ( θ 2 )−a 1 a 2 m2 sin ( θ 2 ) )
2
1 2
c 2= θ̇1 ( 2 a1 a2 m3 sin ( θ 2) + a1 a2 m2 sin ( θ2 ) )
2
5.4. Thế năng và ma trận G
[ ]
T
∂π
G(q )=
∂ qi
[ ]
0
π=m3 g ( −d 3 +l 3 /2 )G(q )= 0
−m3 g
Từ đây ta thiết lập được phương trình Lagrange

M (q) q̈+ C(q , q̇) q̇+G(q)=U
[ ][ ]
M 11 θ̈1 + M 12 θ̈2 +C 11 M m1
⇔ M 21 θ̈1 + M 22 θ̈2 +C 21 = M m 2
M 33 d̈ 3 +G31 Fm3
5.5. Động lực học ngược

Độ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
Ở phần mô phỏng động lực học ngược này, ta ước chừng khối lượng các khâu của
Robot như sau:
m1=17 kg ; m2=7 kg ; m3=5 kg .
Vẫn xét điểm E di chuyển theo chu trình thao tác để khảo sát sự thay đổi của mômen
điều khiển các khâu 1,2 và lực dẫn động khâu 3.
τ 1 =m11 ¿
τ 2=m21 ¿
τ 3 =m33 ¿
CHƯƠNG 6: THIẾT KẾ BỘ ĐIỀU KHIỂN VÀ MÔ

PHỎNG
6.1. Cơ sở lý thuyết
Ở các phần trước nhóm đã tính toán, xác định uy luật biến thiên của các biến khớp
theo thời gian, tương ứng với quỹ đạo công tác của robot theo yêu cầu. Phần này sẽ trình
bày việc điều khiển robot sao cho chúng có thể thực hiện được đúng các chuyển động
mong muốn.
Phương pháp điều khiển tuyến tính chỉ thích hợp với các hệ điều khiển được mô
hình hóa bởi các phương trình vi phân tuyến tính. Tuy nhiên trong phần Động lực học
robot chúng ta đã nhận thấy, hệ phương trình động lực của chúng ta là các phương trình
vi phân phi tuyến, do vậy các biện pháp xấp xỉ sẽ được sử dụng để phù hợp với yêu cầu
của bài toán điều khiển tuyến tính.
Xuất phát trực tiếp từ hệ phương trình vi phân chuyển động đã được nghiên cứu trong
phần Động lực học hệ robot. Phương pháp điều khiển áp dụng là phương pháp điều khiển
lực (mô men) thường được sử dụng để điều khiển cho mô hình của nhóm.
Mục tiêu của bài toán điều khiển là làm sao cho robot bám theo quỹ đạo đã được thiết kế.
Các phần tử dẫn động làm việc theo cách nhận lệnh điều khiển và sinh ra lực (mô men).
Mô men đầu ra sẽ được sử dụng để tính toán mô men mong muốn tiếp theo.
Vấn đề cốt lõi của việc thiết kế bộ điều khiển robot là làm thế nào để bảo đảm rằng bộ
điều khiển được thiết kế sẽ đáp ứng tốt các yêu cầu làm việc cho trước. Tiêu chí cơ bản
quan trọng nhất là hệ phải đảm bảo ổn định. Nghĩa là đảm bảo thời gian quá độ, độ quá
điều chỉnh và sai số quỹ đạo đủ nhỏ theo yêu cầu đặt ra cho dù hệ có phải chịu tác động
của một số nhiễu trong suốt quá trình làm việc.
Trước hết ta có mô hình toán học của hệ thống đã được xây dựng từ các phần trước là hệ
phương trình vi phân động học:
M (q) q̈+ C(q , q̇) q̇+G(q)=τ
Trong đó vế trái của phương trình là mô hình với các tham số của robot, vế phải là
momen mà bộ đều khiển tác động lên robot. Momen này được sinh ra từ bộ điều khiển
với giá trị thỏa mãn yêu cầu đã nêu ở trên.
Chúng ta phải xét xem cấu trúc của bộ điều khiển như thế nào thì có thể đáp ứng mục tiêu
thiết kế. trược hết chúng ta chia bộ điều khiển thành hai phần: một phần dựa trên mô hình
và một phần dựa trên phản hồi.
'
τ =α τ + β
Trong đó α, β là các hàm được chọn lựa sao cho nếu τ với tư cách là đầu vào mới của hệ,
thì hệ trở thành hệ khối lượng đơn vị.
Với cấu trúc này của luật điều khiển, phương trình của hệ trở thành:
'
M (q) q̈+ C( q , q̇) q̇+G( q)=α τ + β
Với mục tiêu biến hệ thành hệ khối lượng đơn vị với τ là thành phần đầu vào thì α, β có
thể được chọn:
{ α =M (q )
β=C (q , q̇) q̇+G( q)
Sau khi chọn được α, β thì phương trình còn lại là:
'
q̈=τ
Chúng ta còn lại tham số cuối cùng cần chọn để thu được hệ khối lượng đơn vị.
'
τ = q̈d + Kv( q̇d −q̇)+ Kp( qd −q)
Trong đó các giá trị đầu vào là quy luật vị trí, quy luật vận tốc, quy luật gia tốc mong
muốn đã được tính toán trong phần thiết kế quỹ đạo:
{
q d (t)
q̇ d (t)
q̈ d (t)
Và các hệ số Kv, Kp là các ma trận đường chéo vuông cấp n (n là số tham số động học
của mô hình robot), phần sau chúng ta sẽ xem xét chi tiết việc chọn các hệ số Kv, Kp này
theo các điều kiện ràng buộc và mục tiêu bài toán.
Ta tìm được biểu thức đặc trưng cho hệ điều khiển vòng kín robot:
Ë+ Kv Ė + KpE=0
Với:
{
E=qd −q
Ė= q̇d −q̇
Ë= q̈d −q̈
Hệ phương trình trên bao gồm các phương trình độc lập, các ma trận Kv, Kp là các ma
trận đường chéo, do vậy phương trình này có thể viết riêng cho từng khớp:
ë i + k vi ė i+ k p ei=0
i
Theo lí thuyết về dao động kĩ thuật, khi nhìn vào phương trình trên ta có thể nhận xét đây
là phương trình dao động tự do có cản, ứng xử của hệ thống phụ thuộc vào hệ số kv i và
kpi. Trong đó mục tiêu của chúng ta là đưa đáp ứng của hệ càng sát với giá trị mong
muốn càng tốt, khoảng thời gian đạt được đáp ứng càng nhanh càng tốt. Do đó, giá trị
nghiệm hướng tới là trường hợp nghiệm kép, giá trị thực, và trường hợp này gọi là trường
hợp tới hạn khi k d=2 √ k P.
Từ nội dung trên ta có thể xây dựng được sơ đồ khối của mô hình hệ thống như sau:
6.2. Mô phỏng bằng Simmechanic Toolbox

- Từ mô hình 3D mà nhóm đã thiết kế trên phần mềm Solidworks 2017, ta đặt các hệ tọa
độ của từng khâu theo quy tắc DH
- Sau đó ta xuất bản vẽ qua Simulink bằng Simmechanic Toolbox

- Tiến hành cài SimMechanics trên Matlab và Import file 3D vào Matlabđã xuất ra từ
Solidworks
- Sau đó, tiến hành mô phỏng động học với quỹ đạo đã thiết kế:
Mô hình mô phỏng:
6.3. Đồ thị kết quả mô phỏng
Góc khớp theta1
Góc khớp theta2

Biến khớp d3
Sai lệch góc khớp Theta1

Sai lệch góc khớp theta2
Sai lệch biến khớp d3

PHỤ LỤC
Sơ đồ Simulink tính toán động học ngược vị trí, vận tốc, gia tốc:
Sơ dồ Simulink tính toán động lực học ngược, tìm lực dẫn động các khâu:
1. CODE Động học ngược vị trí:

function q = DHN_vitri_diem_E(p)
a1=0.5; a2=0.5;
d1=0.55;
g=9.81; %m/s^2
c2=(p(1)^2+p(2)^2-a1^2-a2^2)/(2*a1*a2);
s2= sqrt((1-c2^2));
q2_E=atan2(s2,c2);
c1=((a2*c2+a1)*p(1)+a2*s2*p(2))/(p(1)^2+p(2)^2);
s1=((a2*c2+a1)*p(2)-a2*s2*p(1))/(p(1)^2+p(2)^2);
q1_E=atan2(s1,c1);
q3_E=-p(3);
q = [q1_E q2_E q3_E];
2. CODE Động học ngược vận tốc, gia tốc:
function y = DHN_vantoc_giatoc(u)
a1=0.5; a2=0.5;
d1=0.55;
m1=10;m2=12;m3=2;%kg
%dau vao la u
% u =
[q_E(1),q_E(2),q_E(3),vE(1),vE(2),vE(3),aE(1),aE(2),aE(3)]
Jq11 = -a1*sin(u(1)) - a2*sin(u(1)+u(2));
Jq12 = -a2*sin(u(1)+u(2));
Jq21 = a1*cos(u(1)) +a2*cos(u(1)+u(2));
Jq22 = a2*cos(u(1)+u(2));
Jq = [Jq11,Jq12,0;
Jq21,Jq22,0;
0,0,-1];
Jq111 = -a1*cos(u(1)) - a2*cos(u(1)+u(2));
Jq122 = -a2*cos(u(1)+u(2));
Jq211 = -a1*sin(u(1)) - a2*sin(u(1)+u(2));
Jq222 = -a2*sin(u(1)+u(2));
J_q_cham = [Jq111, Jq122,0;
Jq211, Jq222,0;
0,0,0];
Jp = [1,0,0;0,1,0;0,0,1];
J_p_cham = [0,0,0;0,0,0;0,0,0];
%van_toc_diem E
dpv = [u(4);u(5);u(6)];
%van_toc_cac_bien_khop
dqv = Jq^-1.*Jp*[u(4);u(5);u(6)];
%gia toc diem E
ddpv = [u(7);u(8);u(9)];
%gia_toc_bien_khop
ddqv = Jq^-1*(-J_q_cham*dqv+J_p_cham*dpv+Jp*ddpv);
%van toc bien khop
y = [dqv(1),dqv(2),dqv(3) , ddqv(1),ddqv(2),ddqv(3)];
3. CODE Không gian thao tác:

function [E,vE,aE] = Kgian_thao_tac(t)
% Diem lap rap
P0=[0.8 0.2 -0.01]; % Diem P0

P1=[0.8 0.2 -0.46]; % Diem P1
P2=[0.8 0.2 -0.01]; % Diem P2
P3=[-0.4 -0.8 -0.01]; % Diem P3
P4=[-0.4 -0.8 -0.26]; % Diem P4
P5=[-0.4 -0.8 -0.01]; % Diem P5
P6=[0.8 0.2 -0.01]; % Diem P6
% Thoi gian cac doan quy dao P0P1-P1P2-P2P3-P3P4-P4P5-

P5P6-----
T=[2 2 3 1.5 1.5 2.5]; % Don vi:s
% Th?i gian d?ng ?? g?p v?t và th? v?t -------------------
Td =1; % Don vi:s
%================Nhap cac diem quy dao====================

if (t<=T(1))&&(t>=0)
A=P0; % Quy dao P0-P1'
B=P1;
Te=T(1);t_in=0;
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;
E=[xE;yE;zE];
vE=[vxE;vyE;vzE];
aE=[axE;ayE;azE];
% Dung de gap vat

elseif (t<=T(1)+Td)&&(t>T(1))
A=P1; % Quy dao P1-P1: Dung tai diem P1
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;
E=[xE;yE;zE];
vE=[vxE;vyE;vzE];
aE=[axE;ayE;azE];
elseif (t<=T(1)+T(2)+Td)&&(t>T(1)+Td)
A=P1; % Quy dao P1-P2
B=P2;
Te=T(2);t_in=T(1)+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;
E=[xE;yE;zE];
vE=[vxE;vyE;vzE];
aE=[axE;ayE;azE];
%
elseif (t<=T(1)+T(2)+T(3)+Td)&&(t>T(1)+T(2)+Td)
A=P2; %Quy dao P2-P3
B=P3;
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);
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;
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)
B=P4;
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);
t_in)^3;
t_in)^2;
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)+2*Td) &&
(t>T(1)+T(2)+T(3)+T(4)+Td)
A=P4; % Quy dao P4-P4: Dung tai diem P4
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;
E=[xE;yE;zE];
vE=[vxE;vyE;vzE];
aE=[axE;ayE;azE];
elseif (t<=T(1)+T(2)+T(3)+T(4)+T(5)+2*Td) &&

(t>T(1)+T(2)+T(3)+T(4)+2*Td)
B=P5;
Te=T(5);t_in=T(1)+T(2)+T(3)+T(4)+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;
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)
B=P6;
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);
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;
E=[xE;yE;zE];
vE=[vxE;vyE;vzE];
aE=[axE;ayE;azE];
else
E=[0;0;0];
vE=[0;0;0];
aE=[0;0;0];
end
end
4. CODE Tính động lực học ngược:

function T = InverseModel(u)
% cac tham so dong hoc, dong luc hoc
m1 = 10; m2 = 12; m3=2;
a1 = 0.45; a2= 0.35; d1 = 0.6;
g = 9.81;
l1 = a1/2; l2 = a2/2;
J11zz = a1^2*m1/12; J22zz = a2^2*m2/12 ;J33zz = 0;
% vector toa do suy rong (toa do khop)
q = u(1:3);
% vector dao ham cac toa do suy rong (toa do khop)
dq = [u(4) ; u(5); u(6)];
% vector gia t?c
ddq = [u(7) ; u(8); u(9)];
% 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 + m1*(-2*a1*l1 + l1^2 + a1^2) +
m3*(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 + m2*(-2*a2*l2+l2^2 +a2^2 -
l2*a1*cos(q(2))+a2*a1*cos(q(2)))+m3*(a1*a2*cos(q(2))+a2^2);
m21 = J22zz + J33zz + m2*(-2*a2*l2+l2^2 +a2^2 -
l2*a1*cos(q(2))+a2*a1*cos(q(2)))+m3*(a1*a2*cos(q(2))+a2^2);
m22 = J22zz + J33zz + m2*(-2*a2*l2 + l2^2 + a2^2)+ m3*a2^2;
m33=m3;
m13 =0; m31 =0; m23 =0; m32 =0;
M = [m11,m12,m13;m21,m22,m23;m31,m32,m33];
% Luc qun tinh coriolis va qun tinh li tam
c11 = -m2*a1*(a2-l2)*sin(q(2))+m3*a1*a2*sin(q(2));
c12 = -m2*a1*(a2-l2)*sin(q(2))+m3*a1*a2*sin(q(2))-m2*a1*(a2-
l2)*sin(q(2))+m3*a1*a2*sin(q(2));
c21 = m2*a1*(a2-l2)*sin(q(2))+m3*a1*a2*sin(q(2));
c13 =0;
c23 =0; c22 =0;
c31 =0; c32 =0; c33 =0;
C = [c11,c12,c13;c21,c22,c23;c31,c32,c33];
% Luc suy rong cua cac luc co the
g1=0;
g2=0;
g3=-m3*g;
G = [g1; g2; g3];

% Luc suy rong cua cac luc khong the
Q1=0; Q2=0; Q3=0;
Q=[Q1;Q2;Q3];
% 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;
5. CODE mô phỏng điều khiển PD:

function U2 = PD_Controller(in)
qd = in(1:3);
qd_dot = in(4:6);
qd_ddot = in(7:9);
q = in(10:12);
q_dot = in(13:15);
%Gan lai
qd2 = [qd(1);qd(2);qd(3)];
q2 = [q(1);q(2);q(3)];
qd_dot2 = [qd_dot(1);qd_dot(2);qd_dot(3)];
q_dot2 = [q_dot(1);q_dot(2);q_dot(3)];
qd_ddot2 = [qd_ddot(1);qd_ddot(2);qd_ddot(3)];
e = qd2 - q2;
de = qd_dot2 - q_dot2;
%Kp,Kd
Kp = diag([400 1500 400]);
Kd = diag([40 100 40]);
U = qd_ddot2 + Kp*e + Kd*de; %qd_ddot2 + Kp*e + Kd*de;

% disp('e = ');disp(e);
% disp('de = ');disp(de);
%Gan cac gia tri can thiet
m1 = 50; %kg
m2 = 40;
m3 = 30;
a1 = 0.9; %met
a2 = 0.6;
g = 9.8; %m/s^2
theta1 = q(1);
theta2 = q(2);
d3 = q(3);
theta1_dot = q_dot(1);
theta2_dot = q_dot(2);
d3_dot = q_dot(3);
%Ma tran khoi luong suy rong M
M = [a1 * a2 * m2 * cos(theta2) + 0.2e1 * a1 * a2 * m3 *
cos(theta2) + a1 ^ 2 * m3 + m1 * a1 ^ 2 / 0.3e1 + a1 ^ 2 * m2 +
m2 * a2 ^ 2 / 0.3e1 + m3 * a2 ^ 2 m2 * a2 ^ 2 / 0.3e1 + a1 * a2 *
m2 * cos(theta2) / 0.2e1 + m3 * a2 ^ 2 + a1 * a2 * m3 *
cos(theta2) 0; m2 * a2 ^ 2 / 0.3e1 + a1 * a2 * m2 * cos(theta2) /
0.2e1 + m3 * a2 ^ 2 + a1 * a2 * m3 * cos(theta2) m2 * a2 ^ 2 /
0.3e1 + m3 * a2 ^ 2 0; 0 0 m3];
%Ma tran quan tinh - Coriolis

C = [-theta2_dot * a1 * a2 * m2 * sin(theta2) / 0.2e1 -
theta2_dot * a1 * a2 * m3 * sin(theta2) -theta1_dot * a1 * a2 *
m2 * sin(theta2) / 0.2e1 - theta1_dot * a1 * a2 * m3 *
sin(theta2) - theta2_dot * a1 * a2 * m2 * sin(theta2) / 0.2e1 -
theta2_dot * a1 * a2 * m3 * sin(theta2) 0; theta1_dot * a1 * a2 *
m2 * sin(theta2) / 0.2e1 + theta1_dot * a1 * a2 * m3 *
sin(theta2) 0 0; 0 0 0];
G = [0; 0; -m3 * g];
U2 = M*U + C*q_dot2 + G;
TÀI LIỆU THAM KHẢO
[1]. PGS.TS Phan Bùi Khôi, Bài giảng Robotic, Trường ĐH Bách Khoa Hà Nội,
2015.
[2]. GS. TSKH Nguyễn Văn Khang, Cơ sở Robot công nghiệp, Nhà xuất bản Khoa
học và Kỹ thuật, Hà Nội 2011.
[3]. GS. TSKH. Nguyễn Thiện Phúc, Robot công nghiệp, Nhà xuất bản Khoa học và
Kỹ thuật, 2004.
[4]. PGS.TS. Trịnh chất, TS. Lê Văn Uyển, Tính toán thiết kế hệ dẫn động cơ khí, NXB
giáo dục Việt Nam

Báo Cáo Thiết Kế Robot Final

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

Báo Cáo Thiết Kế Robot Final

Uploaded by

Copyright:

Available Formats

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

Báo cáo bài tập lớn

Giảng viên hướng dẫn : TS. GVC. Nguyễn Văn Quyền

2 Đặng Đức Độ 20194948 Thiết kế hệ thống điều 100%

3 Nguyễn Văn Đức 20194963 Thiết kế quỹ đạo 100%

CHƯƠNG 1: TỔNG QUAN VỀ ROBOT..........................................................................7

1.1 Khái niệm Robot công nghiệp...................................................................................7

1.2 Phân loại Robot công nghiệp.....................................................................................7

1.2.1 Phân loại theo kết cấu.........................................................................................7

1.2.2 Phân loại theo điều khiển....................................................................................7

1.2.3 Phân loại theo ứng dụng.....................................................................................8

1.3 Ứng Dụng của Robot công nghiệp............................................................................8

CHƯƠNG 2: PHÂN TÍCH LỰA CHỌN KẾT CẤU ROBOT.........................................13

2.1. Phân tích mục đích ứng dụng của Robot................................................................13

2.2. Phân tích yêu cầu kĩ thuật.......................................................................................13

2.2.2. Số bậc tự do cần thiết.......................................................................................14

2.2.3. Không gian làm việc........................................................................................14

2.3. Các phương án thiết kế cấu trúc Robot...................................................................15

2.4. Các đặc trưng kĩ thuật.............................................................................................19

CHƯƠNG 3: TÍNH TOÁN ĐỘNG HỌC.........................................................................20

3.1 Xây dựng bài toán động học của Robot...................................................................20

3.1.1. Cấu trúc động học của robot............................................................................20

3.1.2. Phương pháp Denavit – Hartenberg................................................................20

3.1.3. Xây dựng cấu trúc động học............................................................................22

3.2. Bài toán động học thuận về vị trí............................................................................24

3.3. Bài toán động học ngược về vị trí...........................................................................25

3.4. Bài toán động học vi phân......................................................................................26

3.4.1. Bài toán động học vi phân thuận.....................................................................26

3.4.2. Bài toán động học vi phân ngược....................................................................27

4.1. Thiết kế quỹ đạo trong không gian thao tác:..........................................................29

4.2. Xây dựng chương trình mô phỏng trên Matlab......................................................31

CHƯƠNG 5: TÍNH TOÁN ĐỘNG LỰC HỌC................................................................40

5.2. Động năng và ma trận khối lượng M......................................................................40

5.2.1. Tính các tenxơ quán tính.................................................................................40

5.2.2. Tính các ma trận Jacobi...................................................................................41

5.2.3. Động năng và ma trận khối lượng M...............................................................42

5.3. Ma trận quán tính li tâm và gia tốc Coriolit............................................................42

5.4. Thế năng và ma trận G............................................................................................43

5.5. Động lực học ngược................................................................................................43

CHƯƠNG 6: THIẾT KẾ BỘ ĐIỀU KHIỂN VÀ MÔ PHỎNG.......................................44

6.2. Mô phỏng bằng Simmechanic Toolbox..................................................................47

6.3. Đồ thị kết quả mô phỏng........................................................................................50

1. CODE Động học ngược vị trí:...................................................................................53

2. CODE Động học ngược vận tốc, gia tốc:..................................................................54

3. CODE Không gian thao tác:......................................................................................54

4. CODE Tính động lực học ngược:..............................................................................59

5. CODE mô phỏng điều khiển PD:..............................................................................60

TÀI LIỆU THAM KHẢO.................................................................................................62

CHƯƠNG 1: TỔNG QUAN VỀ ROBOT

1.2.1 Phân loại theo kết cấu.

1.2.2 Phân loại theo điều khiển

Có 2 kiểu điều khiển robot: Điều khiển hở và Điều khiển kín.

1.2.3 Phân loại theo ứng dụng

Robot hàn mig

Robot Yaskawa Motoman MPL160

Robot sơn abb

Robot yaskawa cho nghành chế biến gỗ

CHƯƠNG 2: PHÂN TÍCH LỰA CHỌN KẾT CẤU ROBOT

a) Thông số kĩ thuật của đối tượng

Hình dạng của đối tượng

2.2.2. Số bậc tự do cần thiết

2.2.3. Không gian làm việc

Kích thước của dây truyền:

Vị trí của Robot so với dây chuyền: