Đề tài tính toán Động học thuận, Động học nghịch, quy hoạch quỹ Đạo và mô phỏng robot bằng phần mềm simscape

M6 ta Robot trén Solidwork Canh tay robot 3 bac ty do là một hệ thong co khi được thiết kế để mô phỏng hoạt động của cánh tay con người, với khả năng di chuyên và điều chỉnh vị trí trong

BỘ GIÁO DỤC & ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỎ CHÍ MINH

KHOA ĐIỆN - ĐIỆN TỬ

BO MON TU DONG DIEU KHIEN

#A^#

BẢO CÁO

MÔN HỌC: THỰC TẬP KỸ THUẬT ROBOT

ĐÈ TÀI: TÍNH TOÁN ĐỘNG HỌC THUẬN, ĐỘNG

HỌC NGHỊCH, QUY HOẠCH QUÝ ĐẠO VÀ MÔ

PHONG ROBOT BANG PHAN MEM SIMSCAPE

GVHD: Trần Đức Thiện

SVTH: Phạm Gia Khương-21151120 Huỳnh Minh Thông-21151483

Tp Hỗ Chí Minh tháng II năm 2024

MỤC LỤC

11 Mô tảvề Robot 22 v22 2 1n 221122211 rde 5

1.2 Trinh bay đặt trục theo modified DH L0 2222121211211 1122112151 8tr rryy 7

CHƯƠNG 3: XUẤT BẢN VẼ 2D CHO CAC KHAU CUA ROBOT VA CHUYEN

ĐỌÓI SOLIDWORKS SANG SIMSCAPE HH HH HH HH 15

3.1 Xuất bản vẽ 2D 2c: 222 2221112111102211112211122111.1211121112011211 1e de 15

3.2 Cách Simscape từ SolidWorks sang MATLAB Simulink - 5555 s +25 s3 18 CHƯƠNG 4: QUY HOẠCH QUỸ ĐẠO CHO ROBOIT - 55s 5cs se sesesse 21 4.1 Giới thiệu về quy hoạch quỹ đạo - 5s tt E1 11112121111 2 211 ng ng run 21 4.2 Quy hoạch quỹ đạo ở không gian khớp và không gian làm viỆc - 21 4.3 Phuong phap quy hoach quy dao bang ham da thite cccccccccscescsssseeeseeeteeeeeereees 22 4.4 Quy hoạch quỹ đạo theo đường thang ccccccccccccecsscsscsscseseesessessesrsesevsvsnssevseseeees 24 4.5 Quy hoạch quỹ đạo theo hình tam g1ác c0 21 1222121211121 11 1251121111118 1 1E key 25

CHUONG 5: MO PHONG KIEM CHỨNG e cccsserrxerrrrrrrierrrree 27

5.1 Kiém chimg góc quay và m6 phong Simscape c ccccccsscsscsessessesessessseetevsseseseseeeeees 27 5.2 Mô phỏng và đánh giá động học thuận 2 222121211122 11211122281 eg 28 5.2.1 Mô phỏng trên phần mềm Matlab - SE SE EEE2E1 1211211111111 re 28

5.2.2 Đánh giá kết quả của động học thuận trên Matlab 5 c2 eects 29

5.3 Mô phỏng và kiêm chứng kết quả động học nghịch 5 5c ccnn serrrere 31 5.3.1 Mô phỏng trên phần mềm Matlab - 2S SE SEEE2E1 1121121111111 1x 31 5.3.2 Đánh giá kết quả của động học nghịch trên Matlab 55c sctsE cxercre 33 5.4 Kiểm chứng kết hợp Động học thuận, Động học nghịch và mô phỏng Simscape 37

5.4.2 Mô phỏng kiêm chứng tư thế robot với các bộ nghiệm se: 38 5.5 Kiểm chứng quy hoạch quỹ đạo bằng phần mềm Simscape 2S s2 4] 5.5.1 Vẽ không gian làm việc của robot ccc cece cccnseteeeeseesssessseesseeeeensnas 4] 5.5.2 Quy hoạch quỹ đạo đường thăng 5 Sàn TỰ 212 re 43 5.5.3 Quy hoạch quỹ đạo tam gi1ác 0 2121211121112 1112121511111 11011101111 50

6.1 Kết quả đạt được - - cv TH 12111112111 121 1 ng HH HH ga 59

6.2 Harn nha ẻ s9

6.3 Hướng phát triỂn - 5+ S1 1 E 112111111221 1110121 12112111 n1 ray, 59

TÀI LIỆU THAM KHAO .ccsssssessssssessssssessssssessssssesssssscsssssncsssssussasescsseeseeceaceacencassenes 60

PHỤ LỤC HÌNH ẢNH

Hình 1 Mô tả Robot trên SolidWOrĂK ác 2c 111111211121 01 111110111101 11 1111101111118 triệt 6 Hình 2 Hệ tọa độ trên Solidwork - cee n TT TT k SH ng ng 11k 1k kh nu Hy xu 8 Hinh 3 Thiét lap don vi do trén phan mềm Solid WorkS cccccccsssscesesecesesesvsvsvesesesvecsesesees 15

Hình 4 Bản vẽ 2D khâu đỀ c 22t HH Hee ưng l6

Hình 5 Bản vẽ 2D link Ì 2 22 221222211 12121 1111535111121 111 1111111112011 01 1 1 HH ru 16 Hinh 6 Bản vẽ 2D link 2 2 221221223211 19121 111115351 11121 111111111111 11 01 1T HH, 17 Hình 7 Bản vẽ 2D của End EÍẤecfOr c1 11211211121 111 1111111111111 111111 111118118 ray 17

Hình 8 Click chọn Simscape tại cửa số Add-Ïns 222 H2 E21 E55 E2 nhe re 18 Hinh 9 File simulink m6 phong hé cánh tay robot 3 bậc 2c 2c S222 ree 19

Hình 10 Chính sửa giá trị gia tốc trọng trƯờng - s22 12222221112 reg 19

Hình 11 Chỉnh sửa các khối Revolute -.-:-: 55c: 22 tt tEtrrHHrrrrrrrrrrie 20

Hình 12 File mô phỏng Simscape đề kiểm chứng các gÓc quay s-5cccc ren 20 Hình 13 Quy hoạch chuyên động 55 St E11 111112222211 12111 re 21 Hình 14 Tư thế robot trong trường hợp Ì -:-scscEx E221 11 112.2 1x Etctrrerre 27 Hình 15 Tu thé robot trong trường hợp 2 - 5s sc E1 121121 11 11 121 tre 27 Hình 16 Tư thế robot trong trường hợp 3 :- c1 E221 11 121 tra 28

Hình 17 Mô phỏng kiểm tra động học thuận trên Matlab - 5-52 2e s2Ec xe rree 28

Hinh 18 Mô hình robot trong trường hợp Ì 2 2221211222222 11151 1H e 29 Hình 19 Kết quả tọa độ thu được trong trường hợp Ì 5c 2s tt rườn 30 Hình 20 Mô hình robot trong trường hợp 2 - 1 2 22211121222 21 11151 tre He 30 Hình 21 Kết quả tọa độ thu được trong trường hợp 2 5c ng HH rờn 31

Hình 22 Mô phỏng kiểm tra động học nghịch trên Miatlab c c2 222cc se 31

Hình 23 Kết quả mô phỏng của bộ nghiệm số l với trường hợp Ì sccs¿ 33 Hình 24 Kết quả mô phỏng của bộ nghiệm số l với trường hợp 2 5-c-csccscs¿ 34 Hình 25 Kết quả mô phỏng của bộ nghiệm số 2 với trường hợp Ì sccs¿ 34 Hình 26 Kết quả mô phỏng của bộ nghiệm số 2 với trường hợp 2 5-c-csccscs 35 Hình 27 Kết quả mô phỏng của bộ nghiệm số 3 với trường hợp Ì sccs¿ 35 Hình 28 Kết quả mô phỏng của bộ nghiệm số 3 với trường hợp 2 -. c-csccscs¿ 36 Hình 29 Kết quả mô phỏng của bộ nghiệm số 4 với trường hợp Ì sccs¿ 36 Hinh 30 Két qua m6 phong cua bé nghiém s6 4 voi truong hop 2.00 eeeeeeeeeeee 37

Hình 31 File mô phỏng kiểm chứng trên Matlab Simulinkk 2 2+ 2s2cz2 set 37

Hình 32 Kết quả mô phỏng các bộ nghiệm trong trường hợp Ì - -cccc sec 38 Hinh 33 Két qua mé phong cac b6 nghiém trong trong hop 2 0 ccccceccseeeceeeeeeeeeeeeeees 39 Hình 34 Kết quả mô phỏng các bộ nghiệm trong trường hợp 3 5c cc sec 40 Hình 35 Không gian làm việc của cánh tay robOI - ác c1 v1.1 11H 1v H141 111k, 42 Hình 36 Quỹ đạo đường thăng đi qua 2 điểm trong không gian làm việc của robot 43 Hình 37 Mô phỏng quy hoạch quỹ đạo đường thăng trên MATLAB Simulink 44 Hình 38 Robot thực hiện quy hoạch quỹ đạo đường thăng - 2-5 tt nen 46

Mô phỏng quy hoạch quỹ đạo tam giác - c1 211121 vàn Hye 31 Robot thực hiện quy hoạch quỹ đạo tam g146 0 000 cece ccc cece ects eetesetseeentees 34

VỊ trí theo trục x sau 5 chu kỳ quy hoạch quỹ đạo tam giác - 52-55: 55 vận toc theo truc x sau 5 chu ky quy hoach quy dao tam gia cece 55

VỊ trí theo trục y sau 5 chu kỳ quy hoạch quỹ dao tam giác - 552755: 56 Vận tốc theo truc y sau 5 chu kỳ quy hoạch quỹ đạo tam giác -sssc¿ 56

VỊ trí theo trục z sau 5 chu ky quy hoạch quỹ đạo tam giác 57 Vận tốc theo trục z sau 5 chu kỳ quy hoạch quỹ đạo tam giác -csc¿ 57

PHU LUC BANG

Bang 1 Théng số và ký hiệu của Robot

Bảng 2 Bảng DH của hệ cánh tay Robot 2 12212122122 1H T1 kho

CHUONG 111Equation Section 1 BONG HOC THUAN

1.1 M6 ta vé Robot

Hinh 1 M6 ta Robot trén Solidwork Canh tay robot 3 bac ty do là một hệ thong co khi được thiết kế để mô phỏng hoạt

động của cánh tay con người, với khả năng di chuyên và điều chỉnh vị trí trong không gian

ba chiêu Hệ thông này bao gôm ba khớp xoay va ba doan lién két, cho phép nó thực hiện các thao tác phức tạp như câm năm, di chuyên, hoặc lặp ráp

Cau tạo của cánh tay bao gồm ba đoạn liên kết chính:

- Lự Là đoạn liên kết đầu tiên, kết nối từ điểm gốc đến khớp xoay thứ nhất, thường đóng vai trò như phần "vai" của robot

- _ Lạ: Là đoạn nối giữa khớp xoay thứ nhất và khớp xoay thứ hai, tương tự như phần

"cánh tay trên”

- Lz Là đoạn cuối, kéo đài từ khớp xoay thứ hai đến điểm cuối (thường là dụng cụ gắn kèm, như kẹp hoặc đầu hàn), tương ứng với phần "cánh tay dưới"

Ba khớp xoay của cánh tay cho phép điều chỉnh vị trí và góc độ của các đoạn liên kết, tạo nên sự linh hoạt trong việc thao tác trong không gian ba chiều Đây là một câu trúc phô biến trong các ứng dụng như sản xuât, nghiên cứu, hoặc đào tạo về robot

Nhờ sự linh hoạt của các khớp và các đoạn liên kết, cánh tay robot có thể thực hiện các chuyên động tính vị, cho phép điêm đâu cuôi đạt được vị trí và định hướng mong muôn trong không gian ba chiêu Đê vận hành chính xác, việc hiệu rõ các nguyên lý điêu khiên động học của hệ thông là vô cùng quan trọng

Điều này bao gồm:

- Động học thuận: Dùng đề tính toán vị trí và hướng của điểm đầu cuối dựa trên các

góc và vị trí hiện tại của các khớp

- _ Động học nghịch: Giúp xác định các góc cần thiết ở mỗi khớp để điểm đầu cuối đạt

đến vị trí và định hướng mục tiều

1.2 Trình bày đặt trục theo modified DH

Quy tắc đặt trục robot theo phương pháp Denavit — Hartenberg Convention:

+ Bước l1: Gán trục cơ sở: Trục {0} sẽ được gán cho liên kết {0} Chọn trục z¿ đọc theo trục z¡ khi biến khớp đầu tiên bằng 0 Sử dụng quy ước này, chúng ta có a¿=0 và œạ=0, Đảm bảo rằng dụ = 0 nếu là khớp quay và 9,=0 nếu là khớp lăng trụ

+ Bước 2: Xác định các Links: Các Links liên kết được đặt tên theo số liên kết mà chúng

được gắn vào Xác định khớp: Trục z của khung ƒ¡}, được gọi là z¡, trùng với trục khớp 1 Liên kết ¡ có hai trục khớp, Z; và Z;.¡ Trục z, duoc gan cho khớp 1 và Z;,;được gán cho khớp (+1)

+ Bước 3: Xác định pháp tuyến chung giữa trục Z; và z;,,hoặc điểm giao nhau Gốc của khung ƒ¡} nằm tại nơi pháp tuyến chung (,) gặp trucz;

+ Bước 4: Gán trục z; dọc theo trục khớp thứ {1}

+ Bước 5: Gán trục x; đọc theo đường pháp tuyên chung theo hướng từ trục z, đến trục Z¡„, Trong trường hợp a,=9, x;, là đường pháp tuyến chung với mặt phăng của trục Z; và Z¡„ + Bước 6: Trục y, được đặt dựa trên quy tắc bàn tay phải

+ Bước 7: Gán trục cuối (trục EE): Nếu khớp n là khớp quay, hướng của x„ được chọn đọc theo hướng củax,„_, khi 9,=0và gốc của khung ƒn} được chọn sao cho dn = 0 Nếu khớp n

là khớp lăng trụ, hướng của xn được chọn sao cho „=0 và gốc của khung ƒn} được chọn tại giao điểm của X„_¡ với Z„ sao cho d,=0

+ Bước 8: Xác định các thông số trong bảng DH:

a,_, là khoảng cách đo từ trục z,_, đến z, dọc theo trục x,_

œ,_, là góc giữa trục z,_, đên z, dọc theo trục x,_

d, là khoảng cách từ x,_, đến x, đọc theo trục z

0, là góc giữa x._, đến x, đọc theo trục z,„

L; Chiéu dai link thir 2 100 mm L; Chiéu dai link thir 3 70 mm

Bảng Ì Thông số và ký hiệu của Robot

1.4 Tinh cdc ma tran chuyén déi

- _ Công thức ma trận chuyén déi đồng nhất của robot:

Ta có ma trận chuyên đôi giữa hệ trục tọa độ [i— 1} sang hệ trục tọa độ [i } là :

cos ở - sing 0 a

¬ JIn.€os COS COS - SING -sing ad

/ “| SIn4.SIn €os# SIn COS, cosa a

MERGEFORMAT (.)

- - Ma trận chuyền đổi đồng nhất của Robot:

Ta có ma trận chuyên đôi giữa hệ trục tọa độ |0} sang hệ trục tọa d6 {1} la:

|cosự -sInø 00 sing, cosg, U 0

313\* MERGEFORMAT (.)

10

Ta có ma trận chuyển đổi giữa hệ trục tọa độ |} sang hệ trục tọa độ (2) là :

CHUONG 292Equation Section 2: DONG HOC NGHICH

2.1 Cách tiếp cận tính toán

Trong động học robot, bài toán động học nghịch là bài toán tìm các giá trị của

biến khớp khi biết trước vị trí và hướng của bộ phận cuối của robot Bải toán này

thường phức tạp hơn so với bài toán động học thuận do tính phi tuyến và đa nghiệm của nó Có 2 phương pháp để giải bài toán động học nghịch, bao gồm: phương pháp hình học và phương pháp đại số Ở đây nhóm đã áp dụng phương pháp đại số để giải Bằng việc sử dụng các phép biến đổi đại số, các công thức lượng giác, phép đồng nhất hệ số ta có thể tính toán được các biến khớp q1, q2, q3 đưới dạng tổng quát

2.2 Các lời giải bài toán động học nghịch

- Ta viét lai ma tran “T„„ dưới dạng như sau:

10210\* MERGEFORMAT (.)

Ta có:

«®

- Nhân cả 2 về của đẳng thức với ma trận nghịch đảo ta được:

11211\* MERGEFORMAT ()

12

12212\* MERGEFORMAT ()

- _ Từ đó ta có các phương trỉnh tương đương như sau:

“ Tinh

- Bién d6i phuong trình (2.4) va (2.6) bằng các phương trình lượng giác ta được:

16216\* MERGEFORMAT (.)

17217\* MERGEFORMAT (.)

- Tu (2.7) va (2.8) ta co hé phuong trinh:

- Từ đó, ta tính được l nghiệm

14

18218\* MERGEFORMAT (.)

Kết luận: bằng phương pháp đại số ta thu được 4 bộ nghiệm của các biến khớp q, q; ¢

lần lượt như sau:

CHƯƠNG 3: XUẤT BẢN VẼ 2D CHO CÁC KHẨU CUA ROBOT VA CHUYEN

DOI SOLIDWORKS SANG SIMSCAPE

3.1 Xuất bản vẽ 2D

Để xuất bản vẽ 2D cho một khâu bất ky ta tạo một file Drawing trên phân mềm SolidWorks, sau đó chọn khô giấy có kích thước phù hợp Tiến hành thiết lập đơn vị đo là

millimeter tai g6c phai man hình

Hinh 3 Thiét lap don vi do trén phan mém SolidWorks

Đề xuất được bản vẽ 2D ta chon Drawing— Model View — Browser sau đó lựa chọn file 3D của khâu mong muốn và đưa các hình chiếu của chúng ra ngoài, cần lưu ý chọn tỷ

lệ bản vẽ cho phù hợp

s* Khâu đề:

16

khớp và cánh tay của robot được xây dựng Nó có vai trò là điểm gốc hoặc "gốc tọa độ" của robot, nơi mà toàn bộ các tính toán về vị trí và chuyển động được thực hiện

Khâu đề thường chứa động cơ, hệ thống điều khiến, và các bộ phận cần thiết để đảm bảo robot có thê đi chuyên ôn định Từ khâu đề này, các phần khác của robot sẽ chuyên động liên kết với nhau qua các khớp nối, tạo nên chuyển động tổng thể mà robot thực hiện khi

hoạt động

Hình 4 Bản vẽ 2D khâu để

s% Link l:

Link 1 là link liên kết giữa khâu để và link 2 Cá hai liên kết này đều là khớp xoay Link

này thực hiện chuyển động quay với phương song song với mặt phằng của đề

s%% Link 2:

Hinh 6 Ban vé 2D link 2 Link 2 là link liên két gitra link 1 va End Effector, ca 2 liên kết này đều là khớp xoay

18

s%* End Effector:

End effector là phần cuối của cánh tay robot, trực tiếp tương tác với môi trường bên

ngoài đề thực hiện các tác vụ cụ thể End effector có thê là một công cụ, dụng cụ, hoặc

thiết bị mà robot dùng đề thao tác, gap, di chuyên, hoặc xử lý các vật thẻ

Hinh 7 Ban vé 2D cua End Effector

19

3.2 Cach Simscape tir SolidWorks sang MATLAB Simulink

Đề mô phỏng robot bằng Simscape ta cần thực hiện 4 bước được trình bày sau đây:

® Bước l: Cài đặt Simscape Multibody Link cho MATLAB

Truy cập theo đường link sau:

https://www.mathworks.com/campaigns/offerings/download_smlink_confirmation.html? elqsid=1546655391846&potential_use=student

Tái Simscape phù hợp với phiên bán MATLAB đang sử dụng Chạy MATLAB bằng quyền quản trị viên Tại cửa số Command Window của MATLAB gõ lệnh install_addon(‘smlink-r2022b-win64.zip’), sau khi cài đặt xong gõ tiếp câu lệnh

smlink_linksw đề tiến hành liên kết voi phan mém SolidWorks

® Bước 2: Add Simscape cho SolidWorks

Vao Tools— Add-Ins — Simscape Multibody Link

Hinh 8 Click chon Simscape tai cira s6 Add-Ins

20

s - Bước 3: Xuất File XML cho mô hình robot 3 bậc

Vao Tools— Simscape Multibody Link — Export —Simscape Multibody Chon thu mục lưu file và xác nhận

© Bước 4: Sau khi đã xuất ñle thành công, nhập lệnh smimport(*#”) vào cửa số Command của MATLAB (trong đó * là tên file XML đã lưu trước đó) ta thu được một file

Simulink mô phỏng cho hệ cánh tay robot 3 bậc mà ta da vé trén SolidWorks

Hinh 9 File simulink m6 phong hệ cánh tay robot 3 bậc

¢ Thiết lập các thông số mô phỏng

Nhấn tô hợp phím Ctrl + E để mở hép thoai Model Settings, tai muc Solver selection

chon Fixed step va chon fixed step size la 0.01 Tiép dén trong Block Parameters: MechanismConfiguration ta chinh stra gia tri cua gia toc trong trường

Hình 10 Chỉnh sửa giá trị gia toc trong trường

Trong khối Revolute, ta chọn vào mục Actuation để chỉnh sửa Torque va Motion Tai muc

Sensing ta tich chon Position

21

Hình 11 Chỉnh sửa các khối Revolute Thêm các khối để nhập giá trị góc quay ta thu được ñle mô phỏng Simscape hoàn chỉnh, tiễn hành chạy mô phỏng đề kiểm chứng kết quả

Hình 12 File mô phỏng Simscape đề kiêm chứng các góc quay

22

CHUONG 4194Equation Section 4: QUY HOACH QUY DAO CHO ROBOT 4.1 Giới thiệu về quy hoạch quỹ đạo

- Quy hoạch quỹ đạo là một phần nhỏ của một vấn đề tổng thể như là dẫn đường

(navigation) hay quy hoạch chuyên động (Motion planning) Hệ thống cấp bậc điển hình cho quy hoạch chuyên động như sau:

® Task planning — Thiết kế một vài mục tiêu (Ví dụ như nhặt đồ vật trược mặt

bạn lên, gắp vật, thả vật, hàn, )

¢ Path planning — Dua ra duong dan kha thi từ điểm đầu đến điểm cuối Một

đường dẫn luôn bao gồm tập hợp của các điểm tham chiếu

¢ Trajectory planning — Đưa ra lịch trình theo thời gian làm sao để di chuyên

theo đường dẫn mà có tính rang buộc ví dụ như vị trí, vận tốc, gia tốc

©_ Trajectory following — Khi kế hoạch đã được lên thì cần một hệ thống điều khiển để cho kế hoạch có thể đi chuyên đầy đủ và chính xác nhất

Hình 13 Quy hoạch chuyên động

4.2 Quy hoạch quỹ đạo ở không gian khớp và không gian làm việc

- Quy hoạch quỹ đạo ở không gian làm việc là các điểm tham chiếu và nội suy năm

trên trục tọa độ Descartes (vị trí và hướng) của một vị trí cụ thê trên cánh tay robot

(thường là khâu cuối — end effector)

23

- Quy hoạch quỹ đạo ở không gian khớp là các điểm tham chiếu và nội suy trực tiếp

trên các vị trí khớp Cả 2 quỹ đạo ở các không gian khác nhau

- Đối với mỗi không gian khác nhau sẽ có những ưu nhược điểm riêng, ưu nhược điểm của mỗi không gian

® Không gian làm việc:

-_ Ưu điểm: + Chuyên động có thể dự đoán

+ Có khả năng tránh chướng ngại vật và va chạm tốt hơn

- Nhược điểm: + Thực hiện chậm hơn

+ Chuyên động của cơ cầu truyền động không trơn tru và khó

xác định hơn

® Không gian khớp:

- Ưuđiểm: + Tốc độ thực hiện nhanh hơn

+ Chuyển động của cơ cầu truyền động trơn tru và dễ xác định

hơn

-_ Nhược điểm: Các điểm trung gian không được đảm bảo tuân theo các giới

hạn chung hoặc va chạm

4.3 Phương pháp quy hoạch quỹ đạo bằng hàm đa thức

Trong phương pháp quy hoạch quỹ đạo bằng hàm đa thức, đa thức bậc 3 và bậc 5

là hai loại đa thức được sử dụng để quy hoạch quỹ đạo Trong đề tài này nhóm sử dụng đa thức bậc 3 để quy hoạch quỹ đạo cho cánh tay robot 3 bậc

Các điêu kiện ràng buộc:

¢ Tai diém bat dau: 20420\

* MERGEFORMAT (.)

24

4.4 Quy hoạch quỹ đạo theo đường thắng

Ta cần quy hoạch quỹ đạo cho robot di chuyên từ điểm A đến điểm B với thời

gian là t; rồi từ điểm B về điểm A với thời gian là t; theo hàm đa thức bậc ba Toàn

bộ chu kỳ được lặp lại 5 lần trước khi dừng lại tại điểm A

Lưu đồ giải thuật: gọi t là thời gian thực tế, Ty là tông thời gian chạy của 5 chu

kỳ Ta có thể tính toán được thời gian hiện tại trong chu kỳ thuộc khoảng quy hoạch

quỹ đạo nào bằng phép chia lấy dư của t cho thời gian I chu kỳ là 2t; Từ đây ta tiễn

26

hành quy hoạch quỹ đạo với mỗi khoảng thời gian tương ứng và sẽ đừng lại tại điểm

A khi thời gian thực tế bằng với tổng thời gian 5 chu ky

Dựa vào lưu đỗ giải thuật và phương pháp quy hoạch quỹ đạo bằng hàm đa thức bậc 3 nhóm tiến hành mô phỏng quy hoạch quỹ đạo trên Matlab Simulink

4.5 Quy hoạch quỹ đạo theo hình tam giác

Ta cần quy hoạch quỹ đạo cho robot di chuyên từ điểm A đến điểm B với thời

gian là t;, từ điểm B đến điểm C với thời gian là t, và từ điểm C về điểm A cũng với

thời gian là t; theo hàm đa thức bậc ba Toản bộ chu ky được lặp lại 5 lần trước khi

dừng lại tại điểm A Lưu đồ giải thuật được trình bảy tương tự như quỹ đạo đường

thang

Goi t là thời gian thực tế, Ty là tổng thời gian chạy của 5 chu kỳ Ta có thê tính toán được thời gian hiện tại trong chu kỷ thuộc khoảng quy hoạch quỹ đạo nảo bằng

phép chia lấy dư của t cho thời gian 1 chu kỳ là 3t, Từ đây ta tiến hành quy hoạch

quỹ đạo với mỗi khoảng thời gian tương ứng và sẽ dừng lại tại điểm A khi thời gian thực tế bằng với tổng thời gian 5 chu kỳ

Dựa vào lưu đỗ giải thuật và phương pháp quy hoạch quỹ đạo bằng hàm đa thức bậc 3 nhóm tiến hành mô phỏng quy hoạch quỹ đạo trên Matlab Simulink

28

CHUONG 5: MO PHONG KIEM CHUNG 5.1 Kiém ching géc quay va mô phỏng Simscape

¢ Truong hop 1: q,=90 q,=0 q;=

29

Hinh 14 Tue thể robot trong trường hợp 1

Nhận xét: Tư thế robot ở điểm EE hoàn toàn đúng với góc quay mà ta mô phỏng s% Trường hợp 2: q,=0 q;=90, q,=Ý

Hinh 15 Tue thé robot trong trường hợp 2

Nhận xét: Tư thế robot ở điểm EE hoàn toàn đúng với góc quay mà ta mô phỏng

% Trường hợp 3: q,=0 q;=0, q;=9

30

Hinh 16 Tue thé robot trong trường hợp 3

Nhận xét: Tư thế robot ở điểm EE hoàn toàn đúng với góc quay mà ta mô phỏng Kết luận: Bằng việc thử từng trường hợp thì chúng ta nhận thấy robot quay đúng với góc quay mả ta mong muốn

5.2 Mô phỏng và đánh giá động học thuận

5.2.1 Mô phỏng trên phần mềm Matlab

Hình 17 Mô phỏng kiểm tra động học thuận trên Matlab

e - Chương trình trong khối kiểm tra động học thuận DHT trong khối Matlab Function

31