Báo Cáo Đồ Án Cánh Tay Robot Để Mở Tay Nắm Cửa.pdf

Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (1.23 MB, 24 trang )

Trang 1<div class="page_container" data-page="1">

TRƯƠNG ĐAI HOC BACH KHOA HA NÔI

VIÊN ĐIÊN- ĐIÊN TƯ

BAO CAO ĐÔ AN II

ĐÊ TAI: CANH TAY ROBOT ĐÊ MƠ TAY NĂM CƯA

Giang viên hương dân: PGS.TS Ph m Thai  Thuc Anh

Chuyên ng nh: Taư đông hoa công nghiêp Thanh viên : Nguyên Manh Hiêu 20191835 Nguyên Tiên Hưng 20192212

Nguyên H u T ng 20192158 ưu Nguyên Ph Vinh 20192174 u

Ha Nôi, 6/2/2023

</div>Trang 2<div class="page_container" data-page="2">

MỤC L C Ụ

I. YÊU CẦU BÀI TOÁN VÊ CANH TAY ROBOT

1. Yêu cầu thiêt kê ... …3

2. Giơi thiê u v cê anh tay robot 4 bâ c tư do………3

3. Ứng dụng trong công nghiệp ... …5

2. Tinh toan ma trâ n………8

IV. TINH TOAN MA TRÂN JACOBY………9

1. Cac bước tính tốn ma tr n Jacoby theo ậ JH………...9

2. Tinh toan ma trâ n Jacoby……… 11

V. ĐÔNG L C HƯ OC……….13

1. Đô ng năng………..13

2. Thê năng ……….14

3. T nh toi an đô ng lưc ho c ………15

VI. MÔ PHONG TRÊN MATLAB/ SIMULINK……….……21

</div>Trang 3<div class="page_container" data-page="3">

I. YÊU CÂU BAI TOAN VÊ CANH TAY ROBOT

1. Yêu câu thiêt kê

- Thiêt kê tay robot co thê mơ đươc tay năm cưa loa i tron va loa i tay năm cai vơi cach mơ khoa la xoay ngươ c hoă c thuâ n chiêu kim đông hô ô khoa

-Pha m vi hoa t đô ng : 1m-1,5m -Cân nă ng : 3kg-5kg

-Số bậc tự do cần thiết là 4

Chọn thiết kế Robot 4 bậc tự do với 3 khớp quay và 1 khớp tịnh tiến: 2 khớp quay loại R

1 khớp quay loại T 1 khớp tịnh tiến

- Ha n ch : Chê i mơ đươc cac ô khoa không bi  r ri et do lưc không đu khoe

2. Giơi thiê u vê canh tay Robot 4 bâ c tư do

</div>Trang 4<div class="page_container" data-page="4">

Hi nh 1. Canh tay robot

Canh tay Robot có khớp nối loại là robot vô cùng phổ biến. Và mỗi khi nhắcđến Robot chúng ta sẽ lập tức nghĩ ngay đến chúng. Giống như các nhà máy CNC, Robot khớp nối được phân loại theo số điểm quay hoặc số trục mà chúng có. Phổ biến nhất là robot khớp trục. 4 Ngồi cịn ra có các loại robot 6 tru c 7 và trục cũng rất phổ biến trên thị trường.

Tính linh hoạt, khéo léo và khả năng tiếp cận khiến robot có khớp nối là sựlựa chọn phù hợp lýtưởng cho các nhiệm vụ thực hiện trên các mặt phẳngkhông song song, chẳng hạn như chỉnh sửa máy móc.

Robot có khớp nối cũng có thể dễ dàng tiếp cận khoang máy công cụ và dưới các vật cản để tiếp cận phơi. Hoặc thậm chí xung quanh vật cản, trong trường hợp này robot 7 là trục.

Ưu điểm

Các khớp nối kín và ống bảo vệ cho phép Robot có khớp nối hoạt động tốt trong môi trường sạch cũng như môi trường bẩn. Khả năng năng lắp cánh tay Robot 4 bậc tự do này trên bất kỳ bề mặt nào. ( Ví dụ: trần nhà, đườngray trượt). Điều này cho phép doanh nghiệp có nhiều sự lựa chọn khi làm việc.

Tuy nhiên, tinh sự tế và hiện đại của robot có khớp nối này sẽ đi kèm với giá thành cao. Do đó, robot này cao hơn so với cac loa i robot có trọng tảitương tự trên thị trường.

</div>Trang 5<div class="page_container" data-page="5">

Khuyết điểm

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

3. Ưng du ng công nghiê p

Canh tay robot bâ c 4 đươc sư du ng phô biên trong công nghiê p đê: - Di chuyên link kiê n, chi ti t mê ay

- Thao l p link kiă ê n - Han ma ch

Trong dư an nay, chung ta se dung robot bâ c 4 nay đê mơ tay năm cưa

II. ĐÔNG HOC THUÂN VI  TRI

</div>Trang 6<div class="page_container" data-page="6">

𝜽i là góc giữa 2 đường vng góc nói trên. Biến khớp: Nếu khớp động i là kh p quay thì ớ 𝜽i là biến khớp. Nếu khớp động i là tịnh tiến thì di là biến khớp.

Ma trâ n biên đôi to a đô  tông quat la

Do vâ y ta s te inh đươc b n ma trô â n A1 , A2 , A , A4 3

</div>Trang 7<div class="page_container" data-page="7">

0 0 0 1]

</div>Trang 8<div class="page_container" data-page="8">

vị trí mong muốn của tay trong không gian. Tức là xác định ma trận các biến khớp Q q q1, ,...,2 qnT từ vị trí của khâu tác động cuối đã biết, giả sử T có 0

n o a p

Ta có các phương pháp giải vị trí các động học ngược vị trí: Phép đảo vị trí: áp dụng cho các robot có hai bậc tự do Phép đảo hướng: áp d ng cho phép quay c nh ụ ố đị Phương pháp phân ly biến:

] 𝑉𝑇 = 𝐴1−1𝑇4

</div>Trang 9<div class="page_container" data-page="9">

{𝑠𝑖𝑛𝜃3𝑐𝑜𝑠𝜃3= −𝑎𝑥𝑠𝑖𝑛𝜃1= 𝑎𝑥𝑐𝑜𝑠𝜃1+ 𝑎𝑦𝑠𝑖𝑛𝜃1+ 𝑎𝑦𝑐𝑜𝑠𝜃1=> 𝜃3= 𝑎𝑡𝑎𝑛2 𝑠𝑖𝑛𝜃( 3, 𝑐𝑜𝑠𝜃 ) 3{𝑠𝑖𝑛𝜃4= 𝑛𝑧

𝑐𝑜𝑠𝜃4= 𝑜𝑧 => 𝜃4= 𝑎𝑡𝑎𝑛2 𝑠𝑖𝑛𝜃( 4, 𝑐𝑜𝑠𝜃4) 𝑑2= 𝑝𝑥𝑐𝑜𝑠𝜃1+ 𝑝𝑦𝑠𝑖𝑛𝜃1

IV. TINH TOAN MA TRÂN JABOBY

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

1. Các bước tính tốn ma trận Jacoby theo HJ

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

</div>Trang 10<div class="page_container" data-page="10">

Khi i1 là khớp quay, biến khớp i1

Sử dụng ma trận inT

p n p n pp

o p o pp a p a p

(3)

Khi i1 là khớp trượt, biến khớp 𝑑𝑖+1Sử dụng ma trận i

𝜕𝐻𝑝𝑥𝜕𝜃𝑖+1= 𝑖𝑛𝑧

</div>Trang 11<div class="page_container" data-page="11">

𝜕𝐻𝑝𝑦𝜕𝜃𝑖+1= 𝑖𝑜𝑧𝜕𝐻𝑝𝑧

𝜕𝜃𝑖+1= 𝑖𝑎𝑧 (4) 𝜕𝐻𝛷𝑥

𝜕𝛷𝑖+1= 0 𝜕𝐻𝛷𝑦𝜕𝛷𝑖+1= 0 𝜕𝐻𝛷𝑧𝜕𝛷𝑖+1= 0

Bước 3: Tính tốn ma tr n J ậ

d C C 2 4 3

d S C 2 3

d S

C

z

</div>Trang 12<div class="page_container" data-page="12">

x

y

d S S

p

2 343H

d S C

xS C 3

yC 4

3H

</div>Trang 13<div class="page_container" data-page="13">

Trọng tâm:

𝑃𝑐1= ⌊𝑙1𝑐𝑜𝑠𝜃𝑙1𝑠𝑖𝑛𝜃11

0 ⌋ → 𝑃󰇗𝑐1= ⌊

Suy ra:

𝐾1=12 𝑚1((−𝑙1𝑠𝑖𝑛𝜃1𝜃󰇗1) + (𝑙2 1𝑐𝑜𝑠𝜃1𝜃󰇗1) ) +2 12 𝐼𝑧𝑧1𝜃󰇗1=12 (𝑚1𝑙1+ 𝐼𝑧𝑧1)𝜃󰇗1 Thanh nối thứ 2:

2− 𝑙1→ 𝑃󰇗𝑐1=12⌊𝑑󰇗2𝑐𝑜𝑠𝜃𝑑󰇗2𝑠𝑖𝑛𝜃11+ (2𝑙1− (2𝑙1+ 𝑑2)𝑐𝑜𝑠𝜃+ 𝑑2)𝑠𝑖𝑛𝜃1𝜃󰇗1𝜃󰇗110

⌋

Suy ra:

𝐾2=18 𝑚2𝑑󰇗2+18 (𝑚2(2𝑙1+ 𝑑2)2+ 𝐼𝑧𝑧2)𝜃󰇗1 Thanh nối thứ 3:

Trọng tâm:

𝑃𝑐3= ⌊𝑑2𝑐𝑜𝑠𝜃1𝑑2𝑠𝑖𝑛𝜃1+ 𝑙3𝑐𝑜𝑠𝜃13+ 𝑙3𝑠𝑖𝑛𝜃13

𝑑󰇗2𝑐𝑜𝑠𝜃1− 𝑑2𝑠𝑖𝑛𝜃1𝜃󰇗1− 𝑙 𝑠𝑖𝑛𝜃313𝜃󰇗13𝑑󰇗2𝑠𝑖𝑛𝜃1+ 𝑑2𝑐𝑜𝑠𝜃1𝜃󰇗1+ 𝑙 𝑐𝑜𝑠𝜃313𝜃󰇗13

Suy ra:

𝐾3=12 𝑚3𝑑󰇗2+12 (𝑚3𝑙3+ 𝐼𝑧𝑧3)𝜃󰇗132 +12 𝑚3𝑑2𝜃󰇗1+ 𝑚3𝑙3(𝑑2𝑐𝑜𝑠𝜃3𝜃󰇗1− 𝑑󰇗2𝑠𝑖𝑛𝜃 )𝜃3 󰇗13

Thanh nối th 4 ứ

Trọng tâm:

</div>Trang 14<div class="page_container" data-page="14">

𝐾4=12 𝑚4𝑑󰇗2+12 (𝑚4(𝑙4+ 2𝑙3)2+ 𝐼𝑧𝑧4)𝜃󰇗132 +12 𝑚4𝑑2𝜃󰇗1+ 𝑚4(𝑙4+ 2𝑙3)(𝑑2𝑐𝑜𝑠𝜃3𝜃󰇗1− 𝑑󰇗2𝑠𝑖𝑛𝜃3)𝜃󰇗13+12 𝐼𝑥𝑥4𝜃󰇗4

𝑃2= 𝑚2𝑔(2𝑙1+ 𝑙2)𝑠𝑖𝑛𝜃1= 𝑚2𝑔(𝑙1+𝑑22 )𝑠𝑖𝑛𝜃1 Thanh nối thứ 3:

𝑃3= 𝑚3𝑔(𝑑2𝑠𝑖𝑛𝜃1+ 𝑙3𝑠𝑖𝑛𝜃13) Thanh nối thứ 4:

1) − 𝜕𝐿𝜕𝜃1

</div>Trang 15<div class="page_container" data-page="15">

𝜕𝜃󰇗1= (𝑚1𝑙1+ 𝐼𝑧𝑧1)𝜃󰇗1+14(𝑚2(2𝑙1+ 𝑑2)2+ 𝐼𝑧𝑧2)𝜃󰇗1+ (𝑚3𝑙3+ 𝐼𝑧𝑧3)𝜃󰇗13+𝑚3𝑑2𝜃󰇗1+ 2𝑚 𝑙3 3𝑑2𝑐𝑜𝑠𝜃3𝜃󰇗1+ 𝑚 𝑙 (𝑑 𝑐𝑜𝑠𝜃 𝜃3 3 2 3 󰇗3− 𝑑󰇗2𝑠𝑖𝑛𝜃 ) +3 (𝑚4(𝑙4+2𝑙3)2+ 𝐼𝑧𝑧4)𝜃󰇗13+ 𝑚4𝑑2𝜃󰇗1+ 2𝑚4(𝑙 + 2𝑙4 3)𝑑2𝑐𝑜𝑠𝜃3𝜃󰇗1+ 𝑚4(𝑙 +42𝑙3)(𝑑2𝑐𝑜𝑠𝜃 𝜃3 󰇗3− 𝑑󰇗2𝑠𝑖𝑛𝜃3)

Đặt: 𝑙 + 2𝑙 = 𝑙4334,. Suy ra: 𝜕𝐿

𝜕𝜃󰇗1= (𝑚1𝑙1+ 𝑚3𝑙3+ 𝑚4𝑙342 + 𝐼𝑧𝑧1+41𝐼𝑧𝑧2+ 𝐼𝑧𝑧3+ 𝐼𝑧𝑧4) 𝜃󰇗1+ (𝑚3𝑙3+𝐼𝑧𝑧3+ 𝑚4𝑙342 )𝜃󰇗3+41𝑚2(2𝑙1+ 𝑑2)2𝜃󰇗1+ (𝑚 + 𝑚 )𝑑342𝜃󰇗1+ (𝑚3𝑙 +3𝑚4𝑙34)(2𝑑 𝑐𝑜𝑠𝜃23𝜃󰇗1+ 𝑑 𝑐𝑜𝑠𝜃2 3𝜃󰇗3− 𝑑󰇗2𝑠𝑖𝑛𝜃3)

𝑑𝑡(𝜕𝜃󰇗𝜕𝐿1) = (𝑚1𝑙1+ 𝑚3𝑙3+ 𝑚4𝑙342 + 𝐼𝑧𝑧1+14𝐼𝑧𝑧2+ 𝐼𝑧𝑧3+ 𝐼𝑧𝑧4) 𝜃󰇘1+(𝑚3𝑙3+ 𝐼𝑧𝑧3+ 𝑚4𝑙342)𝜃󰇘3+14𝑚2(2𝑙1+ 𝑑2)2𝜃󰇘1+12𝑚2(2𝑙1+ 𝑑2)𝑑󰇗2𝜃󰇗1+𝑚34𝑑2𝜃󰇘1+ 2𝑚34𝑑2𝑑󰇗2𝜃󰇗1+ (𝑚 𝑙 + 𝑚 𝑙 )(2𝑑 𝑐𝑜𝑠𝜃3 3 4 34 2 3𝜃󰇘1−

2𝑑 𝑠𝑖𝑛𝜃2 3𝜃󰇗1𝜃󰇗3+ 2𝑐𝑜𝑠𝜃3𝜃󰇗1𝑑󰇗2+ 𝑑 𝑐𝑜𝑠𝜃 𝜃23 󰇘3− 𝑑 𝑠𝑖𝑛𝜃2 3𝜃󰇗3+ 𝑐𝑜𝑠𝜃3𝜃󰇗3𝑑󰇗2−𝑐𝑜𝑠𝜃3𝜃󰇗3𝑑󰇗2− 𝑠𝑖𝑛𝜃3𝑑󰇘2)

= (𝑚1𝑙1+ 𝑚3𝑙3+ 𝑚4𝑙342 + 𝐼𝑧𝑧1+14 𝐼𝑧𝑧2+ 𝐼𝑧𝑧3+ 𝐼𝑧𝑧4+14 𝑚2(2𝑙1+ 𝑑2)2+ 𝑚34𝑑2+ 2(𝑚3 3𝑙 + 𝑚 𝑙4 34)𝑑2𝑐𝑜𝑠𝜃 )𝜃3 󰇘1

+ (𝑚3𝑙3+ 𝐼𝑧𝑧3+ 𝑚4𝑙342 + (𝑚3𝑙3+ 𝑚4𝑙34)𝑑2𝑐𝑜𝑠𝜃3)𝜃󰇘3− (𝑚3𝑙 + 𝑚 𝑙3 4 34)𝑠𝑖𝑛𝜃3𝑑󰇘2+ (12 𝑚2(2𝑙1+ 𝑑2) + 2𝑚34𝑑2) 𝑑󰇗2𝜃󰇗1+ (𝑚 𝑙 + 𝑚 𝑙 )(−2𝑑 𝑠𝑖𝑛𝜃3 34 3423𝜃󰇗1𝜃󰇗3+ 2𝑐𝑜𝑠𝜃 𝜃3 󰇗1𝑑󰇗2− 𝑑 𝑠𝑖𝑛𝜃2 3𝜃󰇗3) 𝜕𝐿

𝜕𝜃1= − (𝑚 𝑔𝑙1 1𝑐𝑜𝑠𝜃1+ 𝑚2𝑔(𝑙 +1 𝑑2

2)𝑐𝑜𝑠𝜃1+ 𝑚3𝑔(𝑑2𝑐𝑜𝑠𝜃1+𝑙 𝑐𝑜𝑠𝜃313) + 𝑚4𝑔(𝑑2𝑐𝑜𝑠𝜃1+ 𝑙34𝑐𝑜𝑠𝜃13)

</div>Trang 16<div class="page_container" data-page="16">

= − ((𝑚1𝑙1+ 𝑚2 (𝑙1+𝑑22 ) + 𝑚3𝑑2+ 𝑚4𝑑2) 𝑐𝑜𝑠𝜃1+ (𝑚3𝑙 + 𝑚 𝑙34 34)𝑐𝑜𝑠𝜃13) 𝑔

Suy ra:

𝑭 = (𝒇𝟏 𝟏𝟏+𝟏𝟒 𝒎𝟐(𝟐𝒍𝟏+ 𝒅𝟐)𝟐+ 𝒎𝟑𝟒𝒅𝟐+ 𝟐𝒇𝒎𝒍𝟑𝟒𝒄𝒐𝒔𝜽𝟑) 𝜽󰇘𝟏+ (𝒇𝟏𝟐+ 𝒇𝒎𝒍𝟑𝟒 𝟐𝒅 𝒄𝒐𝒔𝜽𝟑)𝜽󰇘𝟑− 𝒇𝒎𝒍𝟑𝟒𝒔𝒊𝒏𝜽 𝒅𝟑 󰇘𝟐

+ 𝒇𝒎𝒍𝟑𝟒(−𝟐𝒅 𝒔𝒊𝒏𝜽 𝜽𝟐 𝟑 󰇗𝟏𝜽󰇗𝟑+ 𝟐𝒄𝒐𝒔𝜽 𝜽𝟑 󰇗𝟏𝒅󰇗𝟐− 𝒅 𝒔𝒊𝒏𝜽 𝜽𝟐 𝟑 󰇗𝟑𝟐)+ ((𝒎 𝒍 + 𝒎𝟏 𝟏 𝟐 (𝒍𝟏+𝒅𝟐 ) + 𝒎𝟐 𝟑𝒅𝟐+ 𝒎𝟒𝒅𝟐) 𝒄𝒐𝒔𝜽𝟏+ 𝒇𝒎𝒍𝟑𝟒𝒄 𝜽 )𝒈𝒐𝒔 𝟏𝟑

Với:

𝑓11= 𝑚1𝑙1+ 𝑚3𝑙3+ 𝑚4𝑙342 + 𝐼𝑧𝑧1+14 𝐼𝑧𝑧2+ 𝐼𝑧𝑧3+ 𝐼𝑧𝑧4𝑓12= 𝑚3𝑙3+ 𝐼𝑧𝑧3+ 𝑚4𝑙342

𝑓𝑚𝑙34= 𝑚3𝑙3+ 𝑚 𝑙 434

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

𝐹2=𝑑𝑡 (𝑑 𝜕𝐿𝜕𝑑󰇗2) −

𝜕𝐿𝜕𝑑2

</div>Trang 17<div class="page_container" data-page="17">

𝜕𝑑󰇗2=14𝑚2𝑑󰇗2+ 𝑚 𝑑3 󰇗2− 𝑚 𝑙 𝑠𝑖𝑛𝜃 𝜃3 3 3 󰇗13+ 𝑚 𝑑4 󰇗2− 𝑚 𝑙 𝑠𝑖𝑛𝜃4 34 3𝜃󰇗13 = (14 𝑚2+ 𝑚34) 𝑑󰇗2− 𝑓𝑚𝑙34𝑠𝑖𝑛𝜃3𝜃󰇗13

𝑑𝑡(𝜕𝑑󰇗𝜕𝐿2) = (14𝑚2+ 𝑚34) 𝑑󰇘2− 𝑓𝑚𝑙34(𝑐𝑜𝑠𝜃3𝜃󰇗3𝜃󰇗13+ 𝑠𝑖𝑛𝜃 𝜃3 󰇘13) 𝜕𝐿

𝜕𝑑2=14(2𝑙1+ 𝑑2)𝜃󰇗1+ 𝑚3𝑑2𝜃󰇗1+ 𝑚3𝑙3𝑐𝑜𝑠𝜃3𝜃󰇗1𝜃󰇗13+ 𝑚4𝑑2𝜃󰇗1+𝑚4𝑙34𝑐𝑜𝑠𝜃 𝜃3 󰇗1𝜃󰇗13− (1

2𝑚2𝑠𝑖𝑛𝜃1+ 𝑚3𝑠𝑖𝑛𝜃1+ 𝑚4𝑠𝑖𝑛𝜃1) 𝑔 = (14 (2𝑙1+ 𝑑2) + 𝑚34𝑑2+ 𝑓𝑚𝑙34𝑐𝑜𝑠𝜃3) 𝜃󰇗1+ 𝑓𝑚𝑙34𝑐𝑜𝑠𝜃3𝜃󰇗1𝜃󰇗3

− (12 𝑚2+ 𝑚34) 𝑠𝑖𝑛𝜃1𝑔 Suy ra:

𝑭 = −𝒇𝟐𝒎𝒍𝟑𝟒𝒔𝒊𝒏𝜽 𝜽𝟑 󰇘𝟏+ (𝟏

𝟒 𝒎𝟐+ 𝒎𝟑𝟒) 𝒅󰇘𝟐− 𝒇𝒎𝒍𝟑𝟒𝒔𝒊𝒏𝜽 𝜽𝟑 󰇘𝟑− 𝟐𝒇𝒎𝒍𝟑𝟒𝒄𝒐𝒔𝜽 𝜽𝟑 󰇗𝟑𝜽󰇗𝟏− 𝟐𝒇𝒎𝒍𝟑𝟒𝒄𝒐𝒔𝜽 𝜽𝟑 󰇗𝟑− (𝟏𝟒 (𝟐𝒍𝟏+ 𝒅𝟐) + 𝒎𝟑𝟒𝒅𝟐+ 𝒇𝒎𝒍𝟑𝟒𝒄𝒐𝒔𝜽𝟑) 𝜽󰇗𝟏+ (𝟏𝟐 𝒎𝟐+ 𝒎𝟑𝟒) 𝒔𝒊𝒏𝜽𝟏𝒈

3.3. Khớp 3 là kh p quay: ớ𝜏3=𝑑𝑡 (𝑑 𝜕𝜃󰇗𝜕𝐿

3) −𝜕𝜃3𝜕𝐿𝜕𝐿

𝜕𝜃󰇗3= (𝑚3𝑙3+ 𝐼𝑧𝑧3+ 𝑚4(𝑙4+ 2𝑙3)2+ 𝐼𝑧𝑧4)𝜃󰇗13+ 𝑚 𝑙 (𝑑 𝑐𝑜𝑠𝜃 𝜃3 323 󰇗1−𝑑󰇗2𝑠𝑖𝑛𝜃 ) + 𝑚 𝑙 (𝑑 𝑐𝑜𝑠𝜃34 3423𝜃󰇗1− 𝑑󰇗2𝑠𝑖𝑛𝜃 )3

= (𝑚3𝑙3+ 𝐼𝑧𝑧3+ 𝑚4(𝑙4+ 2𝑙3)2+ 𝐼𝑧𝑧4)𝜃󰇗13+ 𝑓𝑚𝑙34(𝑑2𝑐𝑜𝑠𝜃3𝜃󰇗1− 𝑑󰇗2𝑠𝑖𝑛𝜃3) Với: 𝑓33= 𝑚3𝑙3+ 𝐼𝑧𝑧3+ 𝑚4(𝑙4+ 2𝑙3)2+ 𝐼𝑧𝑧4

</div>Trang 18<div class="page_container" data-page="18">

𝑑𝑡(𝜕𝜃󰇗𝜕𝐿3) = 𝑓33𝜃󰇘13+ 𝑓𝑚𝑙34(𝑑2𝑐𝑜𝑠𝜃3𝜃󰇘1− 𝑑 𝑠𝑖𝑛𝜃2 3𝜃󰇗1𝜃󰇗3+ 𝑐𝑜𝑠𝜃3𝜃󰇗1𝑑󰇗2−𝑠𝑖𝑛𝜃3𝑑󰇘2− 𝑐𝑜𝑠𝜃3𝜃󰇗3𝑑󰇗2)

𝜕𝜃3= −𝑚 𝑙 𝑔𝑐𝑜𝑠𝜃3 313− 𝑚4𝑙 𝑔𝑐𝑜𝑠𝜃34 13= −𝑓𝑚𝑙34𝑐𝑜𝑠𝜃13𝑔 Suy ra:

𝝉 = 𝒇 𝜽𝟑 𝟑𝟑󰇘𝟏𝟑+ 𝒇𝒎𝒍𝟑𝟒(𝒅 𝒄𝒐𝒔𝜽 𝜽𝟐 𝟑 󰇘𝟏− 𝒅 𝒔𝒊𝒏𝜽 𝜽𝟐 𝟑 󰇗𝟏𝜽󰇗𝟑+ 𝒄𝒐𝒔𝜽 𝜽𝟑 󰇗𝟏𝒅󰇗𝟐− 𝒔𝒊𝒏𝜽 𝒅𝟑 󰇘𝟐− 𝒄𝒐𝒔𝜽 𝜽𝟑 󰇗𝟑𝒅󰇗𝟐) + 𝒇𝒎𝒍𝟑𝟒𝒄𝒐𝒔𝜽𝟏𝟑𝒈

= (𝒇 + 𝒇𝟑𝟑 𝒎𝒍𝟑𝟒 𝟐𝒅 𝒄𝒐𝒔𝜽𝟑)𝜽󰇘𝟏− 𝒇𝒎𝒍𝟑𝟒𝒔𝒊𝒏𝜽 𝒅𝟑󰇘𝟐+ 𝒇 𝜽𝟑𝟑󰇘𝟑− 𝒇𝒎𝒍𝟑𝟒 𝟐𝒅 𝒔𝒊𝒏𝜽 𝜽𝟑󰇗𝟏𝜽󰇗𝟑+ 𝒇𝒎𝒍𝟑𝟒𝒄𝒐𝒔𝜽 𝜽𝟑 󰇗𝟏𝒅󰇗𝟐− 𝒇𝒎𝒍𝟑𝟒𝒄𝒐𝒔𝜽 𝜽𝟑 󰇗𝟑𝒅󰇗𝟐+ 𝒇𝒎𝒍𝟑𝟒𝒄𝒐𝒔𝜽𝟏𝟑𝒈

3.4. Khớp 4 là kh p quay: ớ

𝜏4=𝑑𝑡 (𝑑 𝜕𝐿𝜕𝜃󰇗4) −𝜕𝜃4𝜕𝐿𝜕𝐿

𝜕𝜃󰇗4= 𝐼𝑥𝑥4𝜃󰇗4𝑑𝑑𝑡(𝜕𝜃󰇗𝜕𝐿

4) = 𝐼𝑥𝑥4𝜃󰇘4𝜕𝐿

</div>Trang 19<div class="page_container" data-page="19">

o 𝐻21= −𝑓𝑚𝑙34𝑠𝑖𝑛𝜃3o 𝐻22=14𝑚2+ 𝑚34

o 𝐻41= 𝐻42= 𝐻43= 0 o 𝐻44= 𝐼𝑥𝑥4

𝑉 = 𝑣[ 1 2, 𝑣 , 𝑣3, 𝑣4]𝑇

o 𝑣1= (12𝑚2(2𝑙1+ 𝑑2) + 2𝑚34𝑑2) 𝑑󰇗2𝜃󰇗1+ 𝑓𝑚𝑙34(−2𝑑2𝑠𝑖𝑛𝜃3𝜃󰇗1𝜃󰇗3+2𝑐𝑜𝑠𝜃3𝜃󰇗1𝑑󰇗2− 𝑑 𝑠𝑖𝑛𝜃2 3𝜃󰇗3)

o 𝑣 = −2𝑓2𝑚𝑙34𝑐𝑜𝑠𝜃3𝜃󰇗3𝜃󰇗1− 2𝑓𝑚𝑙34𝑐𝑜𝑠𝜃3𝜃󰇗3− (14(2𝑙1+ 𝑑2) +𝑚34𝑑2+ 𝑓𝑚𝑙34𝑐𝑜𝑠𝜃3) 𝜃󰇗1

o 𝑣 = −𝑓3𝑚𝑙34𝑑2𝑠𝑖𝑛𝜃3𝜃󰇗1𝜃󰇗3+ 𝑓𝑚𝑙34𝑐𝑜𝑠𝜃3𝜃󰇗1𝑑󰇗2− 𝑓𝑚𝑙34𝑐𝑜𝑠𝜃3𝜃󰇗3𝑑󰇗2o 𝑣4= 0

𝐺(𝑄) =[

(𝑚 𝑙1 1+ 𝑚2 (𝑙1+𝑑2

2) + 𝑚3𝑑2+ 𝑚4𝑑2) 𝑐𝑜𝑠𝜃1+ 𝑓𝑚𝑙34𝑐𝑜𝑠𝜃13(12𝑚2+ 𝑚34) 𝑠𝑖𝑛𝜃1

Trong đó:

𝑙 = 𝑙 + 2𝑙3443𝑚34= 𝑚 + 𝑚34

𝑓11= 𝑚1𝑙1+ 𝑚3𝑙3+ 𝑚4𝑙342 + 𝐼𝑧𝑧1+14𝐼𝑧𝑧2+ 𝐼𝑧𝑧3+ 𝐼𝑧𝑧4𝑓12= 𝑚3𝑙3+ 𝐼𝑧𝑧3+ 𝑚4𝑙234

𝑓33= 𝑚3𝑙3+ 𝐼𝑧𝑧3+ 𝑚4𝑙342 + 𝐼𝑧𝑧4𝑓𝑚𝑙34= 𝑚3𝑙3+ 𝑚 𝑙 434

</div>Trang 20<div class="page_container" data-page="20">

VI. MƠ PHONG TRÊN MATLAB 1. Bơ  điêu khiên

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

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

</div>Trang 21<div class="page_container" data-page="21">

𝐾𝑝= ⌊

0 20 0 00

0 00 20 00 3

⌋ ; 𝐾𝑣= ⌊

0 15 0 00

0 00 0 0.110 0⌋ Kết qu mô ph ng: ả ỏ

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

</div>Trang 22<div class="page_container" data-page="22">

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

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

</div>Trang 23<div class="page_container" data-page="23">

+) Giá trị X:

+) Giá tr Y: ị

</div>Trang 24<div class="page_container" data-page="24">

3. Nh n xét: ậ

- Giá trị góc quay và giá tr t nh ti n sau mơ hình tốn h c robot bám sát ị ị ế ọtheo giá trị đặt

- Thời gian đáp ứng bị trễ đi 1-3s.

- kh p 2 giá tr t nh tiỞ ớ ị ị ến có độ quá điều chỉnh nh kho ng 4.4%. ỏ ả- Tọa độ theo trục X có độ quá điều ch nh khoỉ ảng 5,26%, độ quá điều ch nh ỉtheo tr c Y nhụ ỏ.