ĐỘNG HỌC ROBOT
Xây dựng cấu trúc robot
Hình 1 : Mô hình Robot khảo sát
Bậc tự do của mô hình khảo sát:
2 download by : skknchat@gmail.com https://khuonghust.wordpress.com/ f=6.(3-3)+3.1=3
Robot có 2 khớp quay và 1 khớp tịnh tiến
Hình 2 : Đặt hệ trục tọa độ theo phương pháp DH 1.1.3 B ộ thông s ố Denavit-Hartenbeg
Từ mô hình và hệ trục tọa độ ở trên ta xây dựng được bảng thông số Danavit- Hartenbeg như sau:
- Các thông số được biển diễn như trên hình vẽ
- Các biến khớp tương ứng là là 1 , d 2 ,3
Các ma trận biến đổi tọa độ thuần nhất Denavit-Hartenbeg dựa vào bộ thông số trên là:
3 download by : skknchat@gmail.com cos( ) sin( ) 0 a cos( )
Bài toán động học thuận
1.2.1 Tìm v ị trí điể m thao tác bi ể u di ễ n theo các t ọa độ kh ớ p, hướ ng c ủ a khâu thao tác
Ta có các ma trận biến đổi thuần nhất biểu diễn vị trí và hướng của hệ tọa độ khâu thứ i đối với hệ tọa độ cơ sở O o x o y o z o là: c(1 ) s( 1) 0 a1 c( 1)
Từ các ma trận trên ta xác định được:
➢ Ma trận biểu diễn vị trí điểm thao tác biểu diễn theo các tọa độ khớp
4 download by : skknchat@gmail.com
Hướng của hệ tọa độ khâu thác tác so với hệ tọa độ cơ sở được xác định thông qua các góc Roll, Pitch và Yaw Các ký hiệu Roll, Pitch, Yaw cùng với các phép quay tương ứng đóng vai trò quan trọng trong việc xác định sự định hướng của hệ tọa độ này.
Hình 3 : Mô tả các phép quay
RPY Ma trận quay biểu diễn phép quay RPY: cos cos sin coscos sin sin sin sincos sin cos
Để tính toán các góc RPY, cần so sánh với ma trận quay của khâu cuối, cụ thể là ma trận có dạng c(13) s(13) 0.
Giải các hệ phương trình tìm được các góc như sau:
1.2.2 V ậ n t ốc điểm tác độ ng cu ố i E và v ậ n t ố c góc các khâu
➢ Xác định vận tốc điểm tác động cuối E.
Vận tốc của điểm tác động cuối E được xác định thông qua đạo hàm theo thời gian của ma trận định vị, từ đó cho ra ma trận biểu diễn vận tốc tuyệt đối trong hệ cơ sở.
5 download by : skknchat@gmail.com a 3 (
➢ Xác định vận tốc góc các khâu.
- Các ma trận quay biểu diễn hướng của hệ tọa độ khâu i so với hệ tọa độ cơ sởO o x o y o z o c(1 ) s(1) 0 c( 1 ) s(1 ) 0 c( 13) s(13 ) 0
- Đạo hàm các ma trận quay:
- Chuyển vị các ma trận quay: c(1) s( 1) 0 c(1)
- Các ma trận sóng tương ứng:
6 download by : skknchat@gmail.com
Vận tốc góc của các khâu trong hệ cố định là:
1.2.3 Tính gia t ốc điểm tác độ ng cu ố i Tính gia t ố c góc khâu thao tác
➢ Xác định gia tốc điểm tác động cuối:
Gia tốc của điểm tác động cuối E được xác định thông qua việc đạo hàm ma trận biểu diễn vận tốc của nó theo thời gian trong hệ cơ sở Kết quả là ma trận biểu diễn gia tốc của điểm tác động cuối E trong cùng hệ cơ sở.
➢ Xác định gia tốc góc khâu thao tác
Gia tốc góc của khâu thao tác được tính bằng cách đạo hàm ma trận biểu diễn vận tốc góc trong hệ tọa độ cơ sở Kết quả là ma trận biểu diễn gia tốc góc của khâu thao tác cũng trong hệ tọa độ cơ sở.
Bài toán động học ngược
7 download by : skknchat@gmail.com q [q 1 ,q 2 ,q 3 ] T q
1.3.1 Tính các t ọa độ kh ớ p
Ta có: xE a1 c( 1 ) a3c( 13) 0 rE yE a1 s(1 ) a3 s(13 ) z d d
Rút ra các hệ phương trình sau: a 1cos 1 a3.cos(1 3) xE (1)
- q 1 1 , q 2 d 2 , q 3 3 là các biến khớp, các nghiệm cần giải
- a1 ,a3 ,d1 , x E , y E , zE là các số liệu cho trước
Bình phương 2 vế của 2 phương trình (1) và (2) sau đó cộng 2 phương trình lại ta được phương trình sau: a 1 2a 3 2 2a 1a3[cos 1 cos(1 3) sin 1 sin(1 3)] xE 2 y E 2 a1 2 a3 2 2a1a3 cos 3 xE 2 y E 2 cos 3 x E 2 y E 2 a 1 2 a 3 2
Biến đổi phương trình (1) (2) về dạng sau:
8 download by : skknchat@gmail.com a cosa (cos cossin sin
1 1 3 1 3 1 a1 sin 1 a3 (sin 1 cos 3 cos1 sin 3 ) yE cos 1.(a 1 a 3 cos 3) a3 sin 1 sin 3 xE a3 cos 3 ) a 3 cos 1 sin 3 sin 1.(a 1 x (a a cos ) y a cos1 E 1 2 3 2 3 E 3 a 1 a 3 2a 1 a 3 cos 3 sin y
1 3 y (a a cos ) x a q 1 arctan E 1 3 3 E 3 x E (a 1 a3 cos 3 ) y Ea3 sin 3
Vậy ta xác định được các tọa độ khớp như sau: y (a a cos q 1 arctan E 1 3 xE (a1 a3 cos 3 ) yEa3 sin 3 zE d1 q 2 x E
1.3.2 V ậ n t ố c dài c ủa các khâu tương ứ ng v ớ i các kh ớ p t ị nh ti ế n và v ậ n t ố c góc ứ ng v ớ i các kh ớ p quay
Ta có các phương trình động học sau: f 1 a1 co s 1 a 3.cos( 1 3 ) xE 0 a1 s in 1 a 3 sin( 1 3) y E 0 f 2 f
0 Áp dụng công thức q để tính vận tốc các biến khớp: a 3s( 13)
9 download by : skknchat@gmail.com f
➢ Khâu 2 ứng với khớp tịnh tiến nên có vận tốc dài q
➢ Khâu 1 và 3 ứng với khớp quay nên có vận gốc góc tương ứng lần lượt là: q s( )y
1.3.3 Gia t ố c dài c ủa các khâu tương ứ ng v ớ i các kh ớ p t ị nh ti ế n và gia t ố c góc ứ
10 download by : skknchat@gmail.com
TĨNH HỌC ROBOT
Mô hình khảo sát bài toán tĩnh học
Hình 4 : Mô hình khảo sát tĩnh học
Robot Cho trước các thông số:
➢ Coi các thanh là thanh đồng chất, tiết diện ngang không đáng kể, khối lượng các khâu là m 1 , m 2 , m 3
Cho lực tác dụng từ robot lên môi trường F, M:
Tính toán bài toán tĩnh học
➢ Biễu diễn các vector i ri i
1 và i rci i trong hệ tọa độ các khâu
11 download by : skknchat@gmail.com
➢ Tính lực và momen của khâu 2 tác dụng lên khâu 3
Hệ phương trình cân bằng dạng ma trận khảo sát trong hệ tọa độ cơ sở:
➢ Tính lực và momen của khâu 1 tác dụng lên khâu 2
Hệ phương trình cân bằng dạng ma trận khảo sát trong hệ tọa độ cơ sở:
12 download by : skknchat@gmail.com
➢ Tính lực và momen của khâu 0 tác dụng lên khâu 1
Hệ phương trình cân bằng dạng ma trận khảo sát trong hệ tọa độ cơ sở:
13 download by : skknchat@gmail.com
14 download by : skknchat@gmail.com
THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG
Thiết kế quỹ đạo trong không gian khớp
Cho 2 điểm A, B bất kì trong không gian làm việc, biết tọa độ điểm thao tác (x E , y E , z E ) và hướng của khâu thao tác Thiết kế quỹ đạo chuyển động bất kì từ A đến B, chỉ quan tâm đến các điều kiện về vị trí, vận tốc gia tốc tại điểm đầu và điểm cuối.
Chọn quỹ đạo thiết kế là đa thức bậc 3 theo thời gian có dạng: q (t) a i b t c t 2 d t 3 với i=1 3 ứng với 3 biến khớp tương ứng với ,d
Ta được hệ phương trình quỹ đạo chuyển động: q (t) a b t c t 2 d t 3
Yêu cầu thời gian robot đi từ A đến B trong t(s) và vận tốc tại 2 điểm A, B bằng 0.
Từ đó ta có: i (0) q i (t) q q q a bi i ci d i qi (A) ai qi (B) a i bi t ci t 2 d i t 3
2 t 2[qi (B) qi (A)] t 3 Cho các thông số của robot: a1 0.2,a 3 0.2,d1 0.2
Giả sử Robot cần dịch chuyển từ điểm A(-0.2932, 0.2250, 0.3) tới điểm
B(0.3, - 0.1732, 0.35) trong thời gian t=5(s) Giải động học ngược ta được
Từ đó ta tính được các hệ số của phương trình quỹ đạo cho khâu 1:
15 download by : skknchat@gmail.com a 1 2.0944
Tương tự ta tính được q1 (t) 2.0944 0t 0.3770t 2 0.0503t 3
Sử dụng matlab để vẽ đồ thị:
Hình 5 : Đồ thị góc khớp 1
16 download by : skknchat@gmail.com
Thiết kế quỹ đạo trong không gian thao tác
Thiết kế quỹ đạo cho điểm tác động cuối từ điểm A đến điểm B với AB là đường kính Sử dụng phương trình tròn trong không gian, mặt phẳng giữa hai điểm A(x0, y0, 0) và B(xe, ye, 0) sẽ được xác định dựa trên đường kính AB.
Phương trình đường tròn (x xi ) 2 (y yi ) 2 R 2
Phương trình viết dưới dạng tham số như sau: x xi R cos( (t))
R sin( (t)) y y i z 0 Để thỏa mãn điều kiện về vận tốc thì tính toán hàm (t) là hàm bậc 3 theo thời gian:
17 download by : skknchat@gmail.com s(0) A(x 0 , y 0 , 0)
Thỏa mãn các điều kiện: 0 s(te ) B(x e , y e , 0) v(t e ) ve 0
Từ điều kiện trên tìm được các hệ số a0 ,a1 ,a2 ,a3 như sau: a
Giả sử cần tạo quỹ đạo chuyển động từ điểm A(0.7, 0) đến B(-0.7, 0) trong thời gian te=5(s).Ta tính được các giá trị sau: a0 1.5708
Sử dụng Matlab vẽ được đồ thị:
18 download by : skknchat@gmail.com
Hình 8 : Đồ thị x(t) và vx(t)
Hình 9 : Đồ thị y(t) và vy(t)
19 download by : skknchat@gmail.com https://khuonghust.wordpress.com/
