Chương 3 XÂY DỰNG MÔ HÌNH TOÁN HỌC
3.2. Sơ đồ động học cánh tay Robot 3 bậc tự do
3.2.1. Thông số tay máy:
Phạm vi đề tài tập trung vào việc nghiên cứu điều khiển cánh tay robot 3 bậc chuyển động quay (QQQ).
Cánh tay máy 3 bậc tự do mô phỏng trong luận văn với các thông số dựa theo robot công nghiệp IRB 120. IRB 120 là một trong những robot công nghiệp 6 trục thế hệ mới nhất của ABB, với trọng lượng robot 25 kg, trọng tải 3 kg, được thiết kế dành riêng cho các ngành sản xuất sử dụng tự động hóa dựa trên robot linh hoạt (Các thông số của robot IRB 120 được thể hiện trong datasheet tài liệu [29]).
Do yêu cầu của luận văn, nên trong luận văn này chỉ xây dựng robot 3 bậc tự do.
Các thông số và hình dạng của tay máy có một số thay đổi khi mô phỏng nhưng sẽ không làm ảnh hưởng đến kết cấu và đáp ứng yêu cầu luận văn. Dưới đây là bảng thông số của tay máy:
Thông số Ký hiệu Giá trị Đơn vị tính
Bán kính khâu 1 R 60 mm
Chiều dài khâu 1 (tính từ đế máy đến khớp vai) l1 250 mm
Chiều dài khâu 2 (khớp vai đến khớp khuỷu tay) l2 240 mm Chiều dài khâu 3 (khớp khuỷu tay đến cần gắp) l3 360 mm
Khối lượng khâu 1 m1 15 kg
Khối lượng khâu 2 m2 6 kg
Khối lượng khâu 3 m3 4 kg
Gia tốc trọng trường g g 9,81 m/s2
Bảng 3.1 Thông số tay máy 3 bậc tự do 3.2.2. Động học thuận của cánh tay Robot 3 DOF:
3.2.2.1. Chọn hệ tọa độ cơ sở và gắn hệ tọa độ mở rộng lên các khâu:
- Số bậc tự do (DoF): 3
- Ta giả định vị trí ban đầu và chọn gốc tọa độ O0 của robot như hình 3.1 các trục z đặt cùng phương với trục khớp.
- Gắn các hệ tọa độ như hình 3.1.
Hình 3.1 Các hệ tọa độ của tay máy 3 bậc 3.2.2.2. Lập bảng thông số D-H (Denavit Hartenberg):
Dựa vào các thông số có ở bảng 3.1 và hình 3.1 đã gắn các trục tọa độ ta lập được bảng thông số D-H như sau:
Khâu ai i di i
1 0 900 l1 1*
2 l2 0 0 2*
3 l3 0 0 3*
Bảng 3.2 Thông số D-H
Trong đó: ai: Chiều dài của khâu i (Khoảng cách giữa 2 trục z).
i: Góc xoắn của khâu i (Góc hợp giữa 2 trục z).
di: Khoảng cách giữa hai khâu (Khoảng cách giữa 2 trục x).
i: Góc quay giữa 2 khâu (Góc hợp giữa 2 trục x).
3.2.2.3. Xác định ma trận đặc trưng cho các khâu:
Sau khi có được các thông số và lập được bảng thông số D-H ta thành lập các ma trận đặc trưng cho từng khâu của tay máy (gọi là ma trận T). Ma trận T là ma trận mô tả hướng và vị trí của hệ tọa độ gắn trên 2 khớp.
Ma trận đặc trưng 𝑖+1𝑖𝑇 có dạng:
𝑖+1𝑖𝑇 = [
cos 𝜃 − sin 𝜃 cos 𝛼 sin 𝜃 sin 𝛼 𝑎 cos 𝜃 sin 𝜃 cos 𝜃 cos 𝛼 − cos 𝜃 sin 𝛼 𝑎 sin 𝜃
0 sin 𝛼 cos 𝛼 𝑑 0 0 0 1
] (3.1)
- Ma trận đặc trưng của khâu 1:
𝑇 = [
cos 𝜃1 0 sin 𝜃1 0 sin 𝜃1 0 − cos 𝜃1 0 0 1 0 𝑙1 0 0 0 1
1 ]
0 (3.2)
- Ma trận đặc trưng của khâu 2:
𝑇 = [
cos 𝜃2 − sin 𝜃2 0 𝑙2cos 𝜃2 sin 𝜃2 cos 𝜃2 0 𝑙2sin 𝜃2
0 0 1 0 0 0 0 1
2 ]
1 (3.3)
- Ma trận đặc trưng của khâu 3:
𝑇 = [
cos 𝜃3 − sin 𝜃3 0 𝑙3cos 𝜃3 sin 𝜃3 cos 𝜃3 0 𝑙3sin 𝜃3
0 0 1 0 0 0 0 1
3 ]
2 (3.4)
3.2.1.4. Tính các ma trận biến đổi thuần nhất T:
Ma trận biến đổi thuần nhất T là ma trận vector cuối, mô tả hướng và vị trí của hệ tọa độ gắn lên khâu chấp hành cuối so với hệ tọa độ gốc. Ma trận biến đổi thuần nhất T có dạng:
𝑇 = 𝑇 ∗ 𝑇21 ∗ 𝑇 = [
𝑟11 𝑟12 𝑟13 𝑝𝑥 𝑟21 𝑟22 𝑟23 𝑝𝑦 𝑟31 𝑟32 𝑟33 𝑝𝑧 0 0 0 1
3 ]
1 2 3 0
0 (3.5)
Vậy nhân các ma trận (3.2), (3.3), (3.4) lại với nhau ta sẽ tính được tọa độ đầu mút như sau:
𝑝𝑥 = 𝑙2∗ 𝑐𝑜𝑠(𝜃1) ∗ 𝑐𝑜𝑠(𝜃2) + 𝑙3∗ 𝑐𝑜𝑠(𝜃1) ∗ 𝑐𝑜𝑠(𝜃2) ∗ 𝑐𝑜𝑠(𝜃3) − 𝑙3∗ 𝑐𝑜𝑠(𝜃1)
∗ 𝑠𝑖𝑛(𝜃2) ∗ 𝑠𝑖𝑛(𝜃3)
𝑝𝑦 = 𝑙2∗ 𝑐𝑜𝑠(𝜃2) ∗ 𝑠𝑖𝑛(𝜃1) + 𝑙3∗ 𝑐𝑜𝑠(𝜃2) ∗ 𝑐𝑜𝑠(𝜃3) ∗ 𝑠𝑖𝑛(𝜃1) − 𝑙3∗ 𝑠𝑖𝑛(𝜃1)
∗ 𝑠𝑖𝑛(𝜃2) ∗ 𝑠𝑖𝑛(𝜃3)
𝑝𝑧 = 𝑙1 + 𝑙2∗ 𝑠𝑖𝑛(𝜃2) + 𝑙3∗ 𝑐𝑜𝑠(𝜃2) ∗ 𝑠𝑖𝑛(𝜃3) + 𝑙3∗ 𝑐𝑜𝑠(𝜃3) ∗ 𝑠𝑖𝑛(𝜃2)
3.2.3. Động học nghịch của cánh tay Robot 3 DOF:
Để tính động học nghịch của tay robot ta dựa vào phương pháp “giải bài toán động học ngược của robot Elbow” (Tài liệu [2] – Robot công nghiệp). Hệ phương trình biến đổi thuần nhất đã được xác định ở trên:
𝑇 = 𝑇 ∗ 𝑇21 ∗ 𝑇 = [
𝑟11 𝑟12 𝑟13 𝑝𝑥 𝑟21 𝑟22 𝑟23 𝑝𝑦 𝑟31 𝑟32 𝑟33 𝑝𝑧 0 0 0 1
3 ]
1 2 3 0 0
Nhân 2 vế cho ma trận nghịch đảo 10𝑇−1. Ta có:
𝑇 10 −1∗ 𝑇 = 𝑇30 10 −1∗ 𝑇 ∗ 𝑇 ∗ 𝑇10 21 32 (3.6)
⇔ [
∗ ∗ ∗ 𝑛1
∗ ∗ ∗ 𝑛2
∗ ∗ ∗ 𝑛3 0 0 0 1
] = [
∗ ∗ ∗ 𝑚1
∗ ∗ ∗ 𝑚2
∗ ∗ ∗ 𝑚3 0 0 0 1
] (3.7)
Ta có:
{
𝑛1 = 𝑝𝑥 ∗ cos(𝜃1) + 𝑝𝑦∗ sin(𝜃1) 𝑛2 = 𝑝𝑧 − 𝑙1 𝑛3 = 𝑝𝑥∗ sin(𝜃1) − 𝑝𝑦∗ cos(𝜃1) {
𝑚1 = 𝑙3∗ 𝑐𝑜𝑠(𝜃2 + 𝜃3) + 𝑙2∗ 𝑐𝑜𝑠(𝜃2) 𝑚2 = 𝑙3∗ 𝑠𝑖𝑛(𝜃2 + 𝜃3) + 𝑙2∗ 𝑠𝑖𝑛(𝜃2) 𝑚3 = 0 - Tính 𝜃1:
⟹ 𝑛3 = 𝑚3 ⇔ 𝑝𝑥 ∗ sin(𝜃1) − 𝑝𝑦∗ cos(𝜃1) = 0 ⟹ sin(𝜃1) cos(𝜃1) =𝑝𝑥
𝑝𝑦 ⟹ tan(𝜃1) = 𝑝𝑥 𝑝𝑦
⟹ 𝜃1 = 𝑎 tan 2( 𝑝𝑦, 𝑝𝑥) (3.8)
- Tính 𝜃3: {𝑛1 = 𝑚1
𝑛2 = 𝑚2 ⇔ {𝑝𝑥∗ cos(𝜃1) + 𝑝𝑦∗ sin(𝜃1) = 𝑙3∗ 𝑐𝑜𝑠(𝜃2 + 𝜃3) + 𝑙2∗ 𝑐𝑜𝑠(𝜃2) 𝑝𝑧 − 𝑙1 = 𝑙3∗ 𝑠𝑖𝑛(𝜃2 + 𝜃3) + 𝑙2∗ 𝑠𝑖𝑛(𝜃2) Đặt: {𝑛𝑥 = 𝑝𝑥 ∗ cos(𝜃1) + 𝑝𝑦∗ sin(𝜃1)
𝑛𝑦 = 𝑝𝑧− 𝑙1
⟹ {𝑛1 = 𝑚1
𝑛2 = 𝑚2 ⇔ {𝑛𝑥 = 𝑙3∗ 𝑐𝑜𝑠(𝜃2 + 𝜃3) + 𝑙2∗ 𝑐𝑜𝑠(𝜃2) 𝑛𝑦 = 𝑙3∗ 𝑠𝑖𝑛(𝜃2 + 𝜃3) + 𝑙2∗ 𝑠𝑖𝑛(𝜃2)
⟹ {(𝑛𝑥)2 = (𝑙3∗ 𝑐𝑜𝑠(𝜃2 + 𝜃3) + 𝑙2∗ 𝑐𝑜𝑠(𝜃2))2 (𝑛𝑦)2 = (𝑙3∗ 𝑠𝑖𝑛(𝜃2 + 𝜃3) + 𝑙2∗ 𝑠𝑖𝑛(𝜃2))2
⟹ {(𝑛𝑥)2 = 𝑙32∗ (𝑐𝑜𝑠(𝜃2 + 𝜃3))2 + 𝑙22∗ (𝑐𝑜𝑠(𝜃2))2+ 2 ∗ 𝑙3∗ 𝑙2∗ 𝑐𝑜𝑠(𝜃2) ∗ 𝑐𝑜𝑠(𝜃2 + 𝜃3) (𝑛𝑦)2 = 𝑙32∗ (𝑠𝑖𝑛(𝜃2 + 𝜃3))2+ 𝑙22∗ (𝑠𝑖𝑛(𝜃2))2+ 2 ∗ 𝑙3∗ 𝑙2∗ 𝑠𝑖𝑛(𝜃2) ∗ 𝑠𝑖𝑛(𝜃2 + 𝜃3)
⟹ (𝑛𝑥)2+ (𝑛𝑦)2
= 𝑙32∗ ((cos(𝜃2 + 𝜃3))2+ (𝑠𝑖𝑛(𝜃2 + 𝜃3))2) + 𝑙22
∗ ((𝑐𝑜𝑠(𝜃2))2+ (𝑠𝑖𝑛(𝜃2))2) + 2 ∗ 𝑙3∗ 𝑙2∗ ( 𝑐𝑜𝑠(𝜃2) ∗ 𝑐𝑜𝑠(𝜃2 + 𝜃3) + 𝑠𝑖𝑛(𝜃2) ∗ 𝑠𝑖𝑛(𝜃2 + 𝜃3))
⟹ (𝑛𝑥)2+ (𝑛𝑦)2 = 𝑙32+ 𝑙22+ 2 ∗ 𝑙3∗ 𝑙2∗ 𝑐𝑜𝑠(𝜃3)
⟹ 𝑐𝑜𝑠(𝜃3) =(𝑛𝑥)2+ (𝑛𝑦)2− 𝑙32− 𝑙22
2 ∗ 𝑙3∗ 𝑙2 ⟹ sin(𝜃3) = ±√1 − (𝑐𝑜𝑠(𝜃3))2
⟹ tan(𝜃3) = sin(𝜃cos(𝜃3)
3)⟹ 𝜃3 = 𝑎 tan 2( sin 𝜃3, cos 𝜃3) (3.9) - Tính 𝜃2:
Ta có: {𝑛𝑥 = 𝑙3∗ 𝑐𝑜𝑠(𝜃2 + 𝜃3) + 𝑙2∗ 𝑐𝑜𝑠(𝜃2) 𝑛𝑦 = 𝑙3∗ 𝑠𝑖𝑛(𝜃2 + 𝜃3) + 𝑙2∗ 𝑠𝑖𝑛(𝜃2)
⟹ {𝑛𝑥 = 𝑙3∗ (cos(𝜃2) ∗ cos(𝜃3) − sin(𝜃2) ∗ sin(𝜃3)) + 𝑙2∗ 𝑐𝑜𝑠(𝜃2) 𝑛𝑦 = 𝑙3∗ (cos(𝜃2) ∗ sin(𝜃3) + sin(𝜃2) ∗ cos(𝜃3)) + 𝑙2∗ 𝑠𝑖𝑛(𝜃2)
⟹ {𝑛𝑥 = cos(𝜃2) ∗ (𝑙3∗ cos(𝜃3) + 𝑙2) − 𝑙3∗ sin(𝜃2) ∗ sin(𝜃3) 𝑛𝑦 = sin(𝜃2) ∗ (𝑙3∗ cos(𝜃3) + 𝑙2) + 𝑙3∗ cos(𝜃2) ∗ sin(𝜃3)
⟹ {
cos(𝜃2) =𝑛𝑥+ 𝑙3∗ sin(𝜃2) ∗ sin(𝜃3) 𝑙3∗ cos(𝜃3) + 𝑙2 sin(𝜃2) = 𝑛𝑦− 𝑙3∗ cos(𝜃2) ∗ sin(𝜃3)
𝑙3∗ cos(𝜃3) + 𝑙2
⟹ cos(𝜃2) = 𝑛𝑥 + 𝑙3∗ sin(𝜃3) ∗𝑛𝑦− 𝑙3∗ cos(𝜃2) ∗ sin(𝜃3) 𝑙3∗ cos(𝜃3) + 𝑙2 𝑙3∗ cos(𝜃3) + 𝑙2
⟹ cos(𝜃2) = 𝑛𝑥 ∗ (𝑙3∗ cos(𝜃3) + 𝑙2) + 𝑙3∗ sin(𝜃3) ∗ (𝑛𝑦− 𝑙3∗ cos(𝜃2) ∗ sin(𝜃3)) (𝑙3∗ cos(𝜃3) + 𝑙2)2
⟹ cos(𝜃2)
=𝑛𝑥∗ (𝑙3∗ cos(𝜃3) + 𝑙2) + 𝑙3∗ sin(𝜃3) ∗ 𝑛𝑦− 𝑙32∗ cos(𝜃2) ∗ (sin(𝜃3))2 (𝑙3∗ cos(𝜃3) + 𝑙2)2
⟹ cos(𝜃2) = 𝑛𝑥 ∗ (𝑙3∗ cos(𝜃3) + 𝑙2) + 𝑙3∗ sin(𝜃3) ∗ 𝑛𝑦 (𝑙3∗ cos(𝜃3) + 𝑙2)2
− 𝑙32∗ (sin(𝜃3))2
(𝑙3∗ cos(𝜃3) + 𝑙2)2∗ cos(𝜃2)
⟹ cos(𝜃2) [1 + 𝑙32∗ (sin(𝜃3))2
(𝑙3∗ cos(𝜃3) + 𝑙2)2] = 𝑛𝑥 ∗ (𝑙3∗ cos(𝜃3) + 𝑙2) + 𝑙3∗ sin(𝜃3) ∗ 𝑛𝑦 (𝑙3∗ cos(𝜃3) + 𝑙2)2
⟹ cos(𝜃2) [(𝑙3∗ cos(𝜃3) + 𝑙2)2+ 𝑙32∗ (sin(𝜃3))2 (𝑙3∗ cos(𝜃3) + 𝑙2)2 ]
=𝑛𝑥∗ (𝑙3∗ cos(𝜃3) + 𝑙2) + 𝑙3∗ sin(𝜃3) ∗ 𝑛𝑦 (𝑙3∗ cos(𝜃3) + 𝑙2)2
⟹ cos(𝜃2) = 𝑛𝑥 ∗ (𝑙3∗ cos(𝜃3) + 𝑙2) + 𝑙3∗ sin(𝜃3) ∗ 𝑛𝑦 (𝑙3∗ cos(𝜃3) + 𝑙2)2+ 𝑙32∗ (sin(𝜃3))2 Tương tự cho sin(𝜃2):
⟹ {
cos(𝜃2) =𝑛𝑥+ 𝑙3∗ sin(𝜃2) ∗ sin(𝜃3) 𝑙3∗ cos(𝜃3) + 𝑙2 sin(𝜃2) = 𝑛𝑦− 𝑙3∗ cos(𝜃2) ∗ sin(𝜃3)
𝑙3∗ cos(𝜃3) + 𝑙2
⟹ sin(𝜃2) =𝑛𝑦− 𝑙3∗ sin(𝜃3) ∗ 𝑛𝑥 + 𝑙3∗ sin(𝜃2) ∗ sin(𝜃3) 𝑙3∗ cos(𝜃3) + 𝑙2 𝑙3∗ cos(𝜃3) + 𝑙2
⟹ sin(𝜃2) =𝑛𝑦∗ (𝑙3 ∗ cos(𝜃3) + 𝑙2) − 𝑙3∗ sin(𝜃3) ∗ 𝑛𝑥 (𝑙3∗ cos(𝜃3) + 𝑙2)2+ 𝑙32∗ (sin(𝜃3))2
⟹ tan(𝜃2) = sin(𝜃2)
cos(𝜃2)=𝑛𝑦∗ (𝑙3∗ cos(𝜃3) + 𝑙2) − 𝑙3∗ sin(𝜃3) ∗ 𝑛𝑥 𝑛𝑥∗ (𝑙3∗ cos(𝜃3) + 𝑙2) + 𝑙3∗ sin(𝜃3) ∗ 𝑛𝑦
⟹ 𝜃2 = 𝑎 tan 2( 𝐴, 𝐵) (3.10)
Với:
𝐴 = 𝑛𝑦∗ (𝑙3∗ cos(𝜃3) + 𝑙2) − 𝑙3∗ sin(𝜃3) ∗ 𝑛𝑥; 𝐵 = 𝑛𝑥 ∗ (𝑙3∗ cos(𝜃3) + 𝑙2) + 𝑙3∗ sin(𝜃3) ∗ 𝑛𝑦; 3.2.4. Thành lập phương trình động lực học của tay máy 3 DOF:
Xuất phát từ phương pháp động lực học cho hệ cơ học tổng quát. Phương trình chuyển động Lagrange thiết lập cho một cơ hệ được cho bởi:
𝑑 𝑑𝑡
𝜕𝐿
𝜕𝜃̇−𝜕𝐿
𝜕𝜃 = 𝜏
Trong đó là vector biểu diễn các tọa độ suy rộng của các khâu của tay máy, là vector biểu diễn các lực suy rộng của các khâu của tay máy và hàm Lagrange là sự chênh lệch giữa động năng và thế năng của cơ hệ, với:
L = K – P
Các thông số động lực học của tay máy được cho bởi hình 3.2, với trọng tâm được đặt ở đầu mút của từng khớp (bỏ qua khối lượng của tải trọng).
Hình 3.2 Sơ đồ thông số động lực của tay máy
* Động năng và thế năng của tay máy 3 bậc:
K = K1 + K2 + K3; P = P1 + P2 + P3; Đặt:
{𝑠1 = sin 𝜃1
𝑐1 = cos 𝜃1; {𝑠2 = sin 𝜃2
𝑐2 = cos 𝜃2; {𝑠3 = sin 𝜃3 𝑐3 = cos 𝜃3 {𝑠23 = sin(𝜃2+ 𝜃3)
𝑐23 = cos(𝜃2+ 𝜃3)
+ Khâu 1:
Khi khâu 1 xoay một góc 𝜃1 thì toàn bộ robot sẽ xoay cùng một góc 𝜃1, do khớp xoay khâu 1 đặt trên trục quay của khâu 1 nên ta có động năng khâu 1:
𝐾1 =1
2𝑚1𝑣112 +1
2𝑚2𝑣122 +1
2𝑚3𝑣132 (3.11)
𝑃1 = 𝑚1𝑔𝑙1 (3.12)
Ta có:
- 𝑣112 = 𝑅2𝜃̇12 - 𝑣122 = 𝑥̇122 + 𝑦̇122
{𝑥12 = 𝑙12𝑐1
𝑦12 = 𝑙12𝑠1 ⟹ {𝑥̇2 = −𝑙12𝜃̇1𝑠1 𝑦̇2 = 𝑙12𝜃̇1𝑐1
⇒ 𝑣122 = 𝑙122 𝜃̇12 = 𝑙22𝑐22𝜃̇12 - 𝑣132 = 𝑥̇132 + 𝑦̇132
{𝑥12 = 𝑙13𝑐1
𝑦13 = 𝑙13𝑠1 ⟹ {𝑥̇13 = −𝑙13𝜃̇1𝑠1 𝑦̇13= 𝑙13𝜃̇1𝑐1
⇒ 𝑣132 = 𝑙132 𝜃̇12 = (𝑙2𝑐2+ 𝑙3𝑐23)2𝜃̇12 𝐾1 =1
2𝑚1𝑅2𝜃̇12+1
2𝑚2𝑙22𝑐22𝜃̇12+1
2𝑚3𝑙22𝑐22𝜃̇12+1
2𝑚3𝑙32𝑐232 𝜃̇12+ 𝑚3𝑙2𝑙3𝑐2𝑐23𝜃̇12 + Khâu 2:
{ 𝑥2 = 𝑙2𝑐2
𝑦2 = 𝑙1+ 𝑙2𝑠2 ⟹ {𝑥̇2 = −𝑙2𝜃̇2𝑠2 𝑦̇2 = 𝑙2𝜃̇2𝑐2 𝐾2 =1
2𝑚2𝑣22 =1
2𝑚2(𝑥̇22+ 𝑦̇22) =1
2𝑚2𝑙22𝜃̇22 (3.13) 𝑃2 = 𝑚2𝑔𝑦2 = 𝑚2𝑔(𝑙1+ 𝑙2𝑠2) = 𝑚2𝑔𝑙1+ 𝑚2𝑔𝑙2𝑠2 (3.14)
+ Khâu 3:
{ 𝑥3 = 𝑙2𝑐2+ 𝑙3𝑐23 𝑦3 = 𝑙1+ 𝑙2𝑠2+ 𝑙3𝑠23
⟹ {𝑥̇3 = −𝑙2𝜃̇2𝑠2− 𝑙3(𝜃̇2+ 𝜃̇3)𝑠23 𝑦̇3 = 𝑙2𝜃̇2𝑐2+ 𝑙3(𝜃̇2+ 𝜃̇3)𝑐23
𝑣32 = 𝑥̇32+ 𝑦̇32 = 𝑙22𝜃̇22 + 𝑙32(𝜃̇2+ 𝜃̇3)2+ 2𝑙2𝑙3(𝜃̇22+ 𝜃̇2𝜃̇3)𝑐3 𝐾3 =1
2𝑚3𝑣32 =1
2𝑚3𝑙22𝜃̇22+1
2𝑚3𝑙32(𝜃̇2+ 𝜃̇3)2+ 𝑚3𝑙2𝑙3(𝜃̇22+ 𝜃̇2𝜃̇3)𝑐3 (3.15) 𝑃3 = 𝑚3𝑔𝑦3 = 𝑚3𝑔[𝑙1+ 𝑙2𝑠2 + 𝑙3𝑠23] (3.16) 3.2.5. Mô tả toán học tay máy 3 DOF bằng phương trình vi phân:
3.2.5.1. Thành lập hàm Lagrange:
Hàm Lagrange cho tay máy là:
𝐿 = 𝐾 − 𝑃 = 𝐾1+ 𝐾2+ 𝐾3− 𝑃1− 𝑃2− 𝑃3
=1
2𝑚1𝑅2𝜃̇12+1
2𝑚2𝑙22𝑐22𝜃̇12+1
2𝑚3𝑙22𝑐22𝜃̇12+1
2𝑚3𝑙32𝑐232 𝜃̇12 + 𝑚3𝑙2𝑙3𝑐2𝑐23𝜃̇12+1
2𝑚2𝑙22𝜃̇22+1
2𝑚3𝑙22𝜃̇22+1
2𝑚3𝑙32(𝜃̇2+ 𝜃̇3)2 + 𝑚3𝑙2𝑙3(𝜃̇22+ 𝜃̇2𝜃̇3) cos 𝜃3− 𝑚1𝑔𝑙1− 𝑚2𝑔𝑙1− 𝑚2𝑔𝑙2sin 𝜃2
− 𝑚3𝑔[𝑙1+ 𝑙2sin 𝜃2+ 𝑙3sin(𝜃2+ 𝜃3)]
(3.17) Vậy:
𝐿 =1
2𝑚1𝑅2𝜃̇12+1
2𝑚1𝑅2𝜃̇121
2𝑚1𝑅2𝜃̇121
2𝑚1𝑅2𝜃̇121
2𝑚1𝑅2𝜃̇121
2(𝑚2+ 𝑚3)𝑙22𝑐22𝜃̇12 +1
2𝑚3𝑙32𝑐232 𝜃̇12+ 𝑚3𝑙2𝑙3𝑐2𝑐23𝜃̇12+1
2(𝑚2+ 𝑚3)𝑙22𝜃̇22 +1
2𝑚3𝑙32(𝜃̇2+ 𝜃̇3)2+ 𝑚3𝑙2𝑙3(𝜃̇22+ 𝜃̇2𝜃̇3) cos 𝜃3
− (𝑚1+ 𝑚2+ 𝑚3)𝑔𝑙1− (𝑚2+ 𝑚3)𝑔𝑙2sin 𝜃2− 𝑚3𝑔𝑙3sin(𝜃2+ 𝜃3)
Các biểu thức cần tính được xác định dưới đây:
+ Khâu 1:
𝜕𝐿
𝜕𝜃̇1 = 𝑚1𝑅2𝜃̇1+ (𝑚2+ 𝑚3)𝑙22𝑐22𝜃̇1+ 𝑚3𝑙32𝑐232 𝜃̇1+ 2𝑚3𝑙2𝑙3𝑐2𝑐23𝜃̇1 (3.18)
𝑑 𝑑𝑡
𝜕𝐿
𝜕𝜃̇1= 𝑚1𝑅2𝜃̈1+ (𝑚2+ 𝑚3)𝑙22𝑐22𝜃̈1+ 𝑚3𝑙32𝑐232 𝜃̈1+ 2𝑚3𝑙2𝑙3𝑐2𝑐23𝜃̈1− 2(𝑚2+ 𝑚3)𝑙22𝑠2𝑐2𝜃̇1𝜃̇2− 2𝑚3𝑙32𝑠23𝑐23𝜃̇1(𝜃̇2+ 𝜃̇3) − 2𝑚3𝑙2𝑙3𝑠2𝑐23𝜃̇1𝜃̇2−
2𝑚3𝑙2𝑙3𝑐2𝑠23𝜃̇1(𝜃̇2+ 𝜃̇3) (3.19)
𝜕𝐿
𝜕𝜃1 = 0 (3.20)
+ Khâu 2:
𝜕𝐿
𝜕𝜃̇2 = (𝑚2+ 𝑚3)𝑙22𝜃̇2+ 𝑚3𝑙32(𝜃̇2+ 𝜃̇3) + 𝑚3𝑙2𝑙3(2𝜃̇2+ 𝜃̇3) cos 𝜃3 (3.21)
𝑑 𝑑𝑡
𝜕𝐿
𝜕𝜃̇2 = (𝑚2+ 𝑚3)𝑙22𝜃̈2+ 𝑚3𝑙32(𝜃̈2+ 𝜃̈3) + 𝑚3𝑙2𝑙3(2𝜃̈2+ 𝜃̈3) cos 𝜃3− 𝑚3𝑙2𝑙3(2𝜃̇2𝜃̇3+ 𝜃̇32) sin 𝜃3 (3.22)
𝜕𝐿
𝜕𝜃2 = −(𝑚2+ 𝑚3)𝑙22𝑠2𝑐2𝜃̇12− 𝑚3𝑙32𝑠23𝑐23𝜃̇12− 𝑚3𝑙2𝑙3𝑠2𝑐23𝜃̇12−
𝑚3𝑙2𝑙3𝑐2𝑠23𝜃̇12 − (𝑚2+ 𝑚3)𝑔𝑙2cos 𝜃2− 𝑚3𝑔𝑙3cos(𝜃2+ 𝜃3) (3.23) + Khâu 3:
𝜕𝐿
𝜕𝜃̇3 = 𝑚3𝑙32(𝜃̇2+ 𝜃̇3) + 𝑚3𝑙2𝑙3𝜃̇2cos 𝜃3 (3.24)
𝑑 𝑑𝑡
𝜕𝐿
𝜕𝜃̇3 = 𝑚3𝑙32(𝜃̈2+ 𝜃̈3) + 𝑚3𝑙2𝑙3𝜃̈2cos 𝜃3− 𝑚3𝑙2𝑙3𝜃̇2𝜃̇3sin 𝜃3 (3.25)
𝜕𝐿
𝜕𝜃3 = −𝑚3𝑙32𝑠23𝑐23𝜃̇12− 𝑚3𝑙2𝑙3𝑐2𝑠23𝜃̇12− 𝑚3𝑙2𝑙3(𝜃̇22+ 𝜃̇2𝜃̇3)𝑠3− 𝑚3𝑔𝑙3𝑐23 (3.26) 3.2.5.2. Mô tả hệ bằng phương trình Lagrange bậc hai:
Phương trình chuyển động của cơ hệ tay máy được cho bởi hệ 3 phương trình vi phân:
𝑑 𝑑𝑡
𝜕𝐿
𝜕𝜃̇−𝜕𝐿
𝜕𝜃 = 𝜏 𝜏1 = 𝑑
𝑑𝑡
𝜕𝐿
𝜕𝜃̇1− 𝜕𝐿
𝜕𝜃1= 𝑚1𝑅2𝜃̈1+ (𝑚2+ 𝑚3)𝑙22𝑐22𝜃̈1+ 𝑚3𝑙32𝑐232 𝜃̈1+
2𝑚3𝑙2𝑙3𝑐2𝑐23𝜃̈1− 2(𝑚2+ 𝑚3)𝑙22𝑠2𝑐2𝜃̇1𝜃̇2− 2𝑚3𝑙32𝑠23𝑐23𝜃̇1(𝜃̇2+ 𝜃̇3) − 2𝑚3𝑙2𝑙3𝑠2𝑐23𝜃̇1𝜃̇2− 2𝑚3𝑙2𝑙3𝑐2𝑠23𝜃̇1(𝜃̇2+ 𝜃̇3)
(3.27) 𝜏2= 𝑑
𝑑𝑡
𝜕𝐿
𝜕𝜃̇2− 𝜕𝐿
𝜕𝜃2 = [(𝑚2+ 𝑚3)𝑙22+ 𝑚3𝑙32+ 2𝑚3𝑙2𝑙3𝑐3]𝜃̈2+ [𝑚3𝑙32+ 𝑚3𝑙2𝑙3𝑐3]𝜃̈3− 𝑚3𝑙2𝑙3𝑠3(2𝜃̇2𝜃̇3+ 𝜃̇32) + (𝑚2+ 𝑚3)𝑙22𝑠2𝑐2𝜃̇12+
𝑚3𝑙32𝑠23𝑐23𝜃̇12+ 𝑚3𝑙2𝑙3𝑠2𝑐23𝜃̇12+ 𝑚3𝑙2𝑙3𝑐2𝑠23𝜃̇12+ (𝑚2+ 𝑚3)𝑔𝑙2𝑐2+ 𝑚3𝑔𝑙3𝑐23
(3.28) 𝜏3= 𝑑
𝑑𝑡
𝜕𝐿
𝜕𝜃̇3− 𝜕𝐿
𝜕𝜃3 = [𝑚3𝑙32+ 𝑚3𝑙2𝑙3𝑐3]𝜃̈2+ 𝑚3𝑙32𝜃̈3+ 𝑚3𝑙32𝑠23𝑐23𝜃̇12+ 𝑚3𝑙2𝑙3𝑐2𝑠23𝜃̇12+ 𝑚3𝑙2𝑙3𝑠3𝜃̇22+ 𝑚3𝑔𝑙3𝑐23 (3.29) Dưới dạng ma trận, phương trình chuyển động hay phương trình động lực học của hệ cơ tay máy có thể được biểu diễn chung dưới dạng sau:
𝑀̂(𝜃)𝜃̈ + 𝐶̂(𝜃, 𝜃̇)𝜃̇ + 𝐺̂(𝜃) = 𝜏
Với:
𝑀̂(𝜃) = (𝑚11 0 0
0 𝑚22 𝑚32
𝑚023
𝑚33) ; 𝐶̂(𝜃, 𝜃̇) = (𝐶11 𝐶21 𝐶31
𝐶12 𝐶22 𝐶32
𝐶13 𝐶23 𝐶33
) ; 𝐺̂(𝜃) = (𝑔1 𝑔2 𝑔3
)
Vậy ta có:
(𝑚11 0 0
0 𝑚22 𝑚32
𝑚023 𝑚33) (𝜃̈1
𝜃̈2 𝜃̈3
) + (𝐶11 𝐶21 𝐶31
𝐶12 𝐶22 𝐶32
𝐶13 𝐶23 𝐶33
) (𝜃̇1 𝜃̇2 𝜃̇3
) + (𝑔1 𝑔2 𝑔3
) = (𝜏1 𝜏2 𝜏3
)
Trong đó:
𝑚11 = 𝑚1𝑅2𝜃̈1+ 𝑚3𝑙32𝑐232 + 2𝑚3𝑙2𝑙3𝑐2𝑐23+ (𝑚2+ 𝑚3)𝑙22𝑐22. 𝑚22 = (𝑚2+ 𝑚3)𝑙22+ 2𝑚3𝑙2𝑙3cos 𝜃3+ 𝑚3𝑙32.
𝑚23 = 𝑚32 = 𝑚3𝑙32+ 𝑚3𝑙2𝑙3cos 𝜃3. 𝑚33 = 𝑚3𝑙32.
{
𝐶11= −(𝑚2+ 𝑚3)𝑙22𝑠2𝑐2𝜃̇2− 𝑚3𝑙32𝑠23𝑐23(𝜃̇2+ 𝜃̇3) − 𝑚3𝑙2𝑙3𝑠2𝑐23𝜃̇2− 𝑚3𝑙2𝑙3𝑐2𝑠23(𝜃̇2+ 𝜃̇3) 𝐶12= −(𝑚2+ 𝑚3)𝑙22𝑠2𝑐2𝜃̇1− 𝑚3𝑙32𝑠23𝑐23𝜃̇1− 𝑚3𝑙2𝑙3𝑠2𝑐23𝜃̇1− 𝑚3𝑙2𝑙3𝑐2𝑠23𝜃̇1 𝐶13 = −𝑚3𝑙32𝑠23𝑐23𝜃̇1− 𝑚3𝑙2𝑙3𝑐2𝑠23𝜃̇1 {
𝐶21 = (𝑚2+ 𝑚3)𝑙22𝑠2𝑐2𝜃̇1+ 𝑚3𝑙32𝑠23𝑐23𝜃̇1+ 𝑚3𝑙2𝑙3𝑠2𝑐23𝜃̇1+ 𝑚3𝑙2𝑙3𝑐2𝑠23𝜃̇1 𝐶22= −𝑚3𝑙2𝑙3𝑠3𝜃̇3 𝐶23 = −𝑚3𝑙2𝑙3𝑠3𝜃̇3− 𝑚3𝑙2𝑙3𝑠3𝜃̇2 {
𝐶31= 𝑚3𝑙32𝑠23𝑐23𝜃̇1+ 𝑚3𝑙2𝑙3𝑐2𝑠23𝜃̇1 𝐶32= 𝑚3𝑙2𝑙3𝑠3𝜃̇2 𝐶33 = 0 {