Bài tập lớn : Xây dựng cấu trúc và thiết lập hệ phương trình động học của robot,

 ứ i-1 có dạng:ng d ng ph n m m Maple vẽ quỹ đ o c a đi m tác đ ng cu iụng phần mềm Maple vẽ quỹ đạo của điểm tác động cuối ầu vào :q1, q2, q3,q4 ềm Maple vẽ quỹ đạo của điểm tác động c

BÀI T P L N: ROBOTICS ẬP LỚN: ROBOTICS ỚN: ROBOTICS

PH N I: XÂY D NG C U TRÚC VÀ THI T L P H PH ẦN I: XÂY DỰNG CẤU TRÚC VÀ THIẾT LẬP HỆ PHƯƠNG TRÌNH ỰNG CẤU TRÚC VÀ THIẾT LẬP HỆ PHƯƠNG TRÌNH ẤU TRÚC VÀ THIẾT LẬP HỆ PHƯƠNG TRÌNH ẾT LẬP HỆ PHƯƠNG TRÌNH ẬP LỚN: ROBOTICS Ệ PHƯƠNG TRÌNH ƯƠNG TRÌNH NG TRÌNH

Đ NG H C C A ROBOT ỘNG HỌC CỦA ROBOT ỌC CỦA ROBOT ỦA ROBOT

1.xây d ng c u trúc: ựng cấu trúc: ấu trúc:

C u trúc 3D:ấu trúc 3D:

2 thi t l p h ph ết lập hệ phương trình động học của robot: ập hệ phương trình động học của robot: ệ phương trình động học của robot: ương trình động học của robot: ng trình đ ng h c c a robot: ộng học của robot: ọc của robot: ủa robot:

Quy ướn Môn :Roboticsc vi t t t: ết tắt: ắt: C i  cos ; i S i  sin ; i C12  cos(  1   2 );S12  sin(  1   2 );

Các ma tr n v trí và hập Lớn Môn :Robotics ị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1: ướn Môn :Roboticsng c a khâu c a khâu th i so v i khâu th i-1:ủa khâu i so với khâu thứ i-1 có dạng: ủa khâu i so với khâu thứ i-1 có dạng: ứ i-1 có dạng: ớn Môn :Robotics ứ i-1 có dạng:

S S A

H ph ệ phương trình động học robot: ương trình động học robot: ng trình đ ng h c robot: ộng học robot: ọc robot:

Đi m tác đ ng cu i đểm tác động cuối được mô tả bởi ma trận: ộng cuối được mô tả bởi ma trận: ối được mô tả bởi ma trận: ược mô tả bởi ma trận:c mô t b i ma tr n:ả bởi ma trận: ởi ma trận: ập Lớn Môn :Robotics 0 0 0 1

x y z x z y

PH N II:GI I BÀI TOÁN Đ NG H C ẦN I: XÂY DỰNG CẤU TRÚC VÀ THIẾT LẬP HỆ PHƯƠNG TRÌNH ẢI BÀI TOÁN ĐỘNG HỌC ỘNG HỌC CỦA ROBOT ỌC CỦA ROBOT

1.bài toán đ ng h c thu n: ộng học của robot: ọc của robot: ập hệ phương trình động học của robot:

Các thông s đ u vào :q1, q2, q3,q4ối được mô tả bởi ma trận: ầu vào :q1, q2, q3,q4

Thông s c n xác đ nh: đi m tác đ ng cu i P=( Px, Py,Pz),hối được mô tả bởi ma trận: ầu vào :q1, q2, q3,q4 ị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1: ểm tác động cuối được mô tả bởi ma trận: ộng cuối được mô tả bởi ma trận: ối được mô tả bởi ma trận: ướn Môn :Roboticsng c a khâu ủa khâu i so với khâu thứ i-1 có dạng:

cu iối được mô tả bởi ma trận:

 T a đ đi m tác đ ng cu i:ọa độ điểm tác động cuối: ộng cuối được mô tả bởi ma trận: ểm tác động cuối được mô tả bởi ma trận: ộng cuối được mô tả bởi ma trận: ối được mô tả bởi ma trận:

a.xây d ng quy lu t chuy n đ ng c a các khâu c a robot: ựng quy luật chuyển động của các khâu của robot: ật chuyển động của các khâu của robot: ển động của các khâu của robot: ộng học robot: ủa các khâu của robot: ủa các khâu của robot:

gi s robot chuy n t v trí ban đ u ả bởi ma trận: ử robot chuyển từ vị trí ban đầu ểm tác động cuối được mô tả bởi ma trận: ừ vị trí ban đầu ị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1: ầu vào :q1, q2, q3,q4 q0(A) t i v trí cu i ớn Môn :Robotics ị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1: ối được mô tả bởi ma trận: q c(B) trong th i ời gian t c.quỹ đ o c a kh p robot theo đa th c b c 3:ạng: ủa khâu i so với khâu thứ i-1 có dạng: ớn Môn :Robotics ứ i-1 có dạng: ập Lớn Môn :Robotics

gi thi t v n t c t i đi m đ u và đi m cu i b ng 0: ả bởi ma trận: ết tắt: ập Lớn Môn :Robotics ối được mô tả bởi ma trận: ạng: ểm tác động cuối được mô tả bởi ma trận: ầu vào :q1, q2, q3,q4 ểm tác động cuối được mô tả bởi ma trận: ối được mô tả bởi ma trận: ằng 0: q´i 0=0 ; q´ic= 0

cho đi m A(700; 0; 240) và đi m B(175; 153.11; 140) ểm tác động cuối được mô tả bởi ma trận: ểm tác động cuối được mô tả bởi ma trận: Th i gian chuy n ời ểm tác động cuối được mô tả bởi ma trận:

đ ng ộng cuối được mô tả bởi ma trận: t c=5(s)

T bài toán ngừ vị trí ban đầu ược mô tả bởi ma trận:c ta xác đ nh đị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1: ược mô tả bởi ma trận:c các thong s bi n kh p:ối được mô tả bởi ma trận: ết tắt: ớn Môn :Robotics

60 30 100 0

B B B B

q q q q

V i:ớn Môn :Robotics a03= ¿0 a13=0 a23=12 a33=−1.6

q t  t  t

Khâu 4:không chuy n đ ng.ểm tác động cuối được mô tả bởi ma trận: ộng cuối được mô tả bởi ma trận:

b.tính v n t c đi m tác đ ng cu i và v n t c góc c a khâu thao tác: ật chuyển động của các khâu của robot: ố DH: ển động của các khâu của robot: ộng học robot: ố DH: ật chuyển động của các khâu của robot: ố DH: ủa các khâu của robot:

 V n t c c a đi m tác đ ng cu i:ập Lớn Môn :Robotics ối được mô tả bởi ma trận: ủa khâu i so với khâu thứ i-1 có dạng: ểm tác động cuối được mô tả bởi ma trận: ộng cuối được mô tả bởi ma trận: ối được mô tả bởi ma trận:

0

R E E

 V n t c góc c a các khâu:ập Lớn Môn :Robotics ối được mô tả bởi ma trận: ủa khâu i so với khâu thứ i-1 có dạng:

Ma tr n cosin ch hập Lớn Môn :Robotics ỉ hướng: ướn Môn :Roboticsng c a các khâu so v i R0:ủa khâu i so với khâu thứ i-1 có dạng: ớn Môn :Robotics

1 1

1

00

S

C S

12

3 12

00

C

S C

 ứ i-1 có dạng:ng d ng ph n m m Maple vẽ quỹ đ o c a đi m tác đ ng cu iụng phần mềm Maple vẽ quỹ đạo của điểm tác động cuối ầu vào :q1, q2, q3,q4 ềm Maple vẽ quỹ đạo của điểm tác động cuối ạng: ủa khâu i so với khâu thứ i-1 có dạng: ểm tác động cuối được mô tả bởi ma trận: ộng cuối được mô tả bởi ma trận: ối được mô tả bởi ma trận:

Khởi động học của robot:ng chương trình động học của robot:ng trình và khai báo thư việ phương trình động học của robot:n sử dụ thể:ng

Tính ma trập hệ phương trình động học của robot:n Denavit-Hartenbergcác khâu

Thết lập hệ phương trình động học của robot: bảng thông số DH cho mỗi khâu vào ma trập hệ phương trình động học của robot:n H tổng quát

Đ th zp theo th i gian:ồ thị xp theo thời gian: ị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1: ời

>

Tọa độ khâu điểm tác động cuối:

2.bài toán đ ng h c ng ộng học của robot: ọc của robot: ược c

a.xây d ng quy lu t chuy n đ ng c a khâu cu i: ựng quy luật chuyển động của các khâu của robot: ật chuyển động của các khâu của robot: ển động của các khâu của robot: ộng học robot: ủa các khâu của robot: ố DH:

đi m tác đ ng cu i chuy n đ ng theo đểm tác động cuối được mô tả bởi ma trận: ộng cuối được mô tả bởi ma trận: ối được mô tả bởi ma trận: ểm tác động cuối được mô tả bởi ma trận: ộng cuối được mô tả bởi ma trận: ười ng xo n c:ắt: ối được mô tả bởi ma trận:

b.gi i bài toán đ ng h c ng ảng tham số DH: ộng học robot: ọc robot: ược: c:

ch n góc cardan làm góc đ nh họa độ điểm tác động cuối: ị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1: ướn Môn :Roboticsng cho khâu thao tác:

ma tr n cardan:ập Lớn Môn :Robotics

px py Cd

Ta có : n xC C12 4 S S12 4 C124  cos 

Suy ra :q4    q1  q4

c ng d ng ph n m m Matlap, Maple đ tính toán và mô ph ng s ứng dụng phần mềm Matlap, Maple để tính toán và mô phỏng số ụng phần mềm Matlap, Maple để tính toán và mô phỏng số ần mềm Matlap, Maple để tính toán và mô phỏng số ềm Matlap, Maple để tính toán và mô phỏng số ển động của các khâu của robot: ỏng số ố DH: :

PH N III ẦN I: XÂY DỰNG CẤU TRÚC VÀ THIẾT LẬP HỆ PHƯƠNG TRÌNH : TÍNH TOÁN TĨNH H C ỌC CỦA ROBOT 1.tính l c và momen d n đ ng t i các kh p ựng cấu trúc: ẫn động tại các khớp ộng học của robot: ại các khớp ớp

-Đối với khớp tịnh tiến lực kích động tác dụng dọc theo trục khớp thứ i:(giả sử lực masat là không đáng kể)

1 1

1

l C

l S h

2 0 0

2 12

2

0

c c

0

3 3

c

l l

c

l l

: là ma tr n coriolisập Lớn Môn :Robotics

 : là th năng c a robotết tắt: ủa khâu i so với khâu thứ i-1 có dạng:

2.B ng tham s đ ng l c h c c a robot ả ố ộng học của robot: ựng cấu trúc: ọc của robot: ủa robot:

khâu V trí tr ng tâmị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1: ọa độ điểm tác động cuối: Kh i lược mô tả bởi ma trận:ối được mô tả bởi ma trận:n

3.ma tr n Jacobi c a các khâu ập hệ phương trình động học của robot: ủa robot:

T c u t o hình h c c a robot ta xác đ nh đừ vị trí ban đầu ấu trúc 3D: ạng: ọa độ điểm tác động cuối: ủa khâu i so với khâu thứ i-1 có dạng: ị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1: ược mô tả bởi ma trận: ọa độ điểm tác động cuối:c t a đ kh i tâm c a các ộng cuối được mô tả bởi ma trận: ối được mô tả bởi ma trận: ủa khâu i so với khâu thứ i-1 có dạng:khâu so v i h t a đ c đ nhớn Môn :Robotics ệ phương trình: ọa độ điểm tác động cuối: ộng cuối được mô tả bởi ma trận: ối được mô tả bởi ma trận: ị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1: R0:

Từ đó suy ra :

0 0

7.ph ương trình động học của robot: ng trình vi phân chuy n đ ng c a các khâu ể: ộng học của robot: ủa robot:

Th các bi u th c vào phết tắt: ểm tác động cuối được mô tả bởi ma trận: ứ i-1 có dạng: ương trình:ng trình Lagrange lo i haiạng: :

PH N V: CH N LU T ĐI U KHI N ẦN I: XÂY DỰNG CẤU TRÚC VÀ THIẾT LẬP HỆ PHƯƠNG TRÌNH ỌC CỦA ROBOT ẬP LỚN: ROBOTICS ỀU KHIỂN ỂN

1.Hệ thống điều khiển phản hồi trong không gian khớp

Chức năng của hệ thống điều khiển là tạo ra một momen cần thiết để truyền động khớ p robot đảm bảo khớp robot bám theo vị trí đặt

Sử dụng cấu trúc bộ điều khiển có dạng tỉ lệ - đạo hàm (PD):

Hàm V Lbiểu thị tổng năng lượng của hệ thống robot Thành phần

1 ( 2

T p

K

) tỷ lệ năng lượng đầu vào và thành phần

q Hq , (3) được rút gọn dạng:

Thay phương trình động lực học dạng tổng quát (1) vào phương trình (4) ,với

Bất đẳng thức (7) cho thấy rằng hệ thống ổn định tuyệt đối

Quỹ đ o chuy n đ ng c a các khâu:ạng: ểm tác động cuối được mô tả bởi ma trận: ộng cuối được mô tả bởi ma trận: ủa khâu i so với khâu thứ i-1 có dạng:

Khâu 1:q t1( ) 7.2  t2 0.96 t3

Khâu 2:q t2( ) 3.6  t2 0.48 t3

Khâu 3:q t3( ) 12  t2 1.6 t3

Khâu 4:không chuy n đ ng.ểm tác động cuối được mô tả bởi ma trận: ộng cuối được mô tả bởi ma trận:

2.Các thao tác th c hi n mô ph ng ựng cấu trúc: ệ phương trình động học của robot: ỏng

Các h s khu ch đ i t l và đ o hàm đệ phương trình: ối được mô tả bởi ma trận: ết tắt: ạng: ỉ hướng: ệ phương trình: ạng: ược mô tả bởi ma trận: ực:c l a ch n nh sauọa độ điểm tác động cuối: ư :

q, v là các tín quỹ đ a và v n t c ph n h i l i t robot(quỹ đ o th c)ọa độ điểm tác động cuối: ập Lớn Môn :Robotics ối được mô tả bởi ma trận: ả bởi ma trận: ồ thị xp theo thời gian: ạng: ừ vị trí ban đầu ạng: ực:

đây ta s d ng các kh i:

ởi ma trận: ử robot chuyển từ vị trí ban đầu ụng phần mềm Maple vẽ quỹ đạo của điểm tác động cuối ối được mô tả bởi ma trận:

+kh i Gain dùng đ kh ch đ i các tín hi u đ u vàoối được mô tả bởi ma trận: ểm tác động cuối được mô tả bởi ma trận: ết tắt: ạng: ệ phương trình: ầu vào :q1, q2, q3,q4

+kh i mux ch p các tín hi u đ n thành các tín hi u t ng h p và tách thànhối được mô tả bởi ma trận: ập Lớn Môn :Robotics ệ phương trình: ơng trình: ệ phương trình: ổng bình phương của hai vế: ợc mô tả bởi ma trận:nhi u tín hi uềm Maple vẽ quỹ đạo của điểm tác động cuối ệ phương trình:

+kh i input, ouput đ u vào và ra c a tín hi uối được mô tả bởi ma trận: ầu vào :q1, q2, q3,q4 ủa khâu i so với khâu thứ i-1 có dạng: ệ phương trình:

+kh i hàm bi u di n m t hàm toán h c khi có tín hi u đi vào là các bi n vàối được mô tả bởi ma trận: ểm tác động cuối được mô tả bởi ma trận: ễn một hàm toán học khi có tín hiệu đi vào là các biến và ộng cuối được mô tả bởi ma trận: ọa độ điểm tác động cuối: ệ phương trình: ết tắt: tín hi u đ u ra là giá tr c a hàmệ phương trình: ầu vào :q1, q2, q3,q4 ị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1: ủa khâu i so với khâu thứ i-1 có dạng:

Mô hình b đi u khi n PDộng cuối được mô tả bởi ma trận: ềm Maple vẽ quỹ đạo của điểm tác động cuối ểm tác động cuối được mô tả bởi ma trận: :

3.k t qu mô ph ng ết lập hệ phương trình động học của robot: ả ỏng

Sau khi mô ph ng ta đỏng ta được kết quả là đồ thị của các đường đặc tính của các ược mô tả bởi ma trận:c k t qu là đ th c a các đết tắt: ả bởi ma trận: ồ thị xp theo thời gian: ị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1: ủa khâu i so với khâu thứ i-1 có dạng: ười ng đ c tính c a cácặt và vận tốc đặt vào các khớp của robot đươc ủa khâu i so với khâu thứ i-1 có dạng:

bi n:ết tắt:

Mô men c a các kh p theo th i gianủa khâu i so với khâu thứ i-1 có dạng: ớn Môn :Robotics ời

V trí các kh p theo th i gianị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1: ớn Môn :Robotics ời

Sai s v trí kh p theo th i gianối được mô tả bởi ma trận: ị trí và hướng của khâu của khâu thứ i so với khâu thứ i-1: ớn Môn :Robotics ời

TÀI LIỆU THAM KHẢO

1.Bài giảng Robotics-PGS.TS.Phan Bùi Khôi

2.cơ sở robot công nghiệp-GS.TSKH.Nguyễn Văn Khang

3.matlap và simulink – Nguyễn Phùng Quang

4.Phần help của phần mềm Maple

5.Phần help của phần mềm Matlab