BÀI t p l n ROBOTICS số b c t DO c a mô HÌNH ROBOT

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI... 13 THIẾT KẾ MÔ HÌNH ĐIỀU KHI N ROBOT THEOLUỂ ẬT ĐIỀU KHIỂN PD ...

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

MỤC LỤC

Ni dung

MÔ HÌNH 1 3

BIỂU DI N H TỄ Ệ ỌA ĐỘ KHỚP 3

Bảng D- H: 3

SỐ BẬC TỰ DO CỦA MÔ HÌNH ROBOT: 3

HỆ PHƯƠNG TRÌNH ĐỘNG HỌC CỦA MÔ HÌNH: 3

PHƯƠNG TRÌNH VI PHÂN CHUYỂN ĐỘNG C A MÔ HÌNH: Ủ 4

TÍNH TOÁN L C TÁC D NG LÊN CÁC KHÂU C A MÔ HÌNH: Ự Ụ Ủ 5

KHẢO SÁT ĐỘNG HỌC: 8

THIẾT KẾ, VẼ ĐỒ TH QUỸ Ị ĐẠO CHUYỂN ĐỘNG C A KHÂU THAO TÁC: Ủ 13

THIẾT KẾ MÔ HÌNH ĐIỀU KHI N ROBOT THEOLUỂ ẬT ĐIỀU KHIỂN PD 15

MÔ PHỎNG ĐIỀU KHI N B NG MATLAB SIMULINK Ể Ằ 16

MÔ HÌNH 2 17

SỐ BẬC TỰ DO CỦA MÔ HÌNH ROBOT: 17

CÁC MA TR N D- Ậ H: 17

PHƯƠNG TRÌNH VI PHÂN CHUYỂN ĐỘNG C A MÔ HÌNH: Ủ 18

TÍNH TOÁN L C TÁC D NG LÊN CÁC KHÂU C A MÔ HÌNH: Ự Ụ Ủ 18

KHẢO SÁT ĐỘNG HỌC: 21

THIẾT KẾ, VẼ ĐỒ TH QUỸ Ị ĐẠO CHUYỂN ĐỘNG C A KHÂU THAO TÁC: Ủ 24

THIẾT KẾ MÔ HÌNH ĐIỀU KHI N ROBOT THEO LUỂ ẬT ĐIỀU KHIỂN PD: 25

PHƯƠNG TRÌNH VI PHÂN CHUYỂN ĐỘNG CỦA MÔ HÌNH:

Tính toán v n t c góc và gia t c góc cho khâu 2 và 3 ậ ố ố

𝑐𝜃 3𝜃󰇗3 −𝑠𝜃 3𝜃󰇗3 0 ) ∗ (

𝑐𝜃 𝑐𝜃 2 3 −𝑐𝜃 2𝑠𝜃3 𝑠𝜃2

𝑠𝜃 𝑐𝜃2 3 −𝑠𝜃2𝑠𝜃3 −𝑐𝜃2

𝑠𝜃 3 𝑐𝜃 3 0 ) = ( 𝜃󰇗02 −𝜃󰇗20 −𝑠𝜃2𝜃󰇗3 𝑐𝜃2𝜃󰇗3

−𝑐𝜃2𝜃󰇗3 𝑠𝜃 𝜃 󰇗3 0 )

TÍNH TOÁN L C TÁC D NG LÊN CÁC KHÂU C A MÔ HÌNH: Ự Ụ Ủ

Khâu 1:

Khâu 2:

Khâu 3:

Áp d ng hụ ệ phương trình đệ quy tính toán l c trong các khự ớp

0𝐹𝑖,𝑖−1= 𝐹0 𝑖+1,𝑖− 0𝑃𝑖 0𝑀𝑖+1,𝑖− 0𝑟𝑖−1𝑖 ∗ 𝐹0 𝑖,𝑖−1− 0𝑟𝑐𝑖𝑖 ∗ 0𝑃𝑖

0𝑀𝑖,𝑖−1= 0𝑀𝑖+1,𝑖− 0𝑟𝑖−1𝑖 ∗ 𝐹0

𝑖,𝑖−1− 𝑟0

𝑐𝑖𝑖 ∗ 𝑃0

𝑖 Các vecto tọa độ khớp bi u di n trong h tể ễ ệ ọa độ cơ sở:

0𝑃1= [00 − 𝑃1]T

0𝑃2= [00−𝑃2]T 𝐹𝐸= [𝐹𝑥𝐹𝑦𝐹𝑧]T

0𝑃2= [00−𝑃3]T 𝑀𝐸 = [𝑀𝑥𝑀𝑦𝑀𝑧]T

Thiết l p các ma trậ ận tọa độ khớp và khối tâm:

Tọa độ khớp: Tọa độ khối tâm:

0 𝑎3 𝑠𝜃3

2

−𝑎 2 𝑠𝜃2𝑐𝜃32

𝑐𝜃2 0 𝑠𝜃2 𝑎2𝑐𝜃22 + 𝑎1 𝑠𝜃2 0 −𝑐𝜃2 𝑎2 𝑠𝜃2

0) → 𝐽𝑅1= (

0 0 0

0 0 0

0 0 0) Khâu 2:

−𝑎 𝑠𝜃3 2𝑐𝜃3

2 − 𝑎2𝑠𝜃2 𝑎 𝑐𝜃 𝑐𝜃3 2 32 + 𝑎2𝑐𝜃2 0

−𝑎 𝑐𝜃 𝑠𝜃3 2 32

−𝑎 3 𝑠𝜃2𝑠𝜃32

𝑞 = 𝑑[ 1; 𝜃2; 𝜃3]𝑇

𝑀(𝑞)𝑞󰇘 + 𝐶 𝑞, 𝑞󰇗 𝑞󰇗 + 𝐺( ) (𝑞) + 𝑄 = 𝑈

THIẾT KẾ, VẼ ĐỒ THỊ QUỸ ĐẠO CHUY ỂN ĐỘ NG C A KHÂU THAO TÁC: Ủ

Quỹ đạo chuyển động là đường thẳng trong không gian thao tác:

Ma trận mở r ng c a ma trộ ủ ận 𝐴:

𝑥 −𝑥 𝑒 0

𝑡𝑒−𝑡0𝑥󰇗0

𝑡0−𝑡𝑒+ 𝑥𝑒 −𝑥 0

(𝑡 −𝑡 )𝑒 0 2𝑥󰇗𝑒+𝑥󰇗0

Sơ đồ khối hệ thống điều khiển động lực học ngược vòng ngoài tuyến tín PD:

𝑞 = 𝑑[ 1; 𝜃2; 𝜃3]𝑇

𝑀(𝑞)𝑞󰇘 + 𝐶 𝑞, 𝑞󰇗 𝑞󰇗 + 𝐺( ) (𝑞) + 𝑄 = 𝑈

MÔ PH ỎNG ĐIỀ U KHI N B NG MATLAB SIMULINK Ể Ằ

- B c t do cậ ự ủa cơ cấu 𝑓 = 𝜆 ∗ 𝑛 − 𝑘 + ∑𝑓( ) 𝑖+ 𝑓𝑒− 𝑓𝑐

- Trong đó 𝜆 = 3, 𝑛 = 2, 𝑘 = 2 b c t do chuyậ ự ển động của khớp t nh ti n là 1 và kh p quay ị ế ớ

0𝑟0 = [−𝑎1; 0; −𝑑 ]1 T

2𝑟1 = [−𝑎2; 0; 0]T →0𝑟1 =0𝑅2 ∗ 2𝑟1 = (𝑐𝜃02 −𝑠𝜃0 −12 0

𝑠𝜃2 𝑐𝜃2 0) ∗ (

−𝑎20

0 ) = (

−𝑎2𝑐𝜃20

−𝑎 𝑠𝜃2 2) Các vecto kh i tâm bi u di n trong h tố ể ễ ệ ọa độ cơ sở:

0𝑟𝑐11 = [−𝑎1

2 ; 0; 0]T

0𝑟𝑐22 = [−𝑎2 𝑐𝜃 2

2 ; 0;−𝑎2 𝑠𝜃2

2 ]T Biểu di n các l c trong h tễ ự ệ ọa đ cơ sởộ :

0𝑃1= [00 − 𝑃1]T 𝐹2= [𝐹𝑥𝐹𝑦𝐹𝑧]T

0𝑃2= [00−𝑃2]T 𝑀2 = [𝑀𝑥𝑀𝑦𝑀𝑧]T

Thiết l p các ma trậ ận tọa độ khớp và khối tâm:

Tọa độ khớp: Tọa độ khối tâm:

Khâu 1 :

1 𝑎2 𝑐𝜃22

) = 𝑚2∗ ( 1

𝑎 2 𝑐𝜃22

𝑎 𝑐𝜃2 22

𝑎24

𝑎 𝑐𝜃2 22

𝑎24

𝑎23

)

𝑀(𝑞) = 𝑀1(𝑞) + 𝑀2( )𝑞 = 𝑚1∗ (1 0

0 0 ) + 𝑚2∗ (1

𝑎 2 𝑐𝜃 2 2

𝑎 𝑐𝜃2 22

𝑞 = 𝑑[ 1; 𝜃2]𝑇

𝑀(𝑞)𝑞󰇘 + 𝐶 𝑞, 𝑞󰇗 𝑞󰇗 + 𝐺( ) (𝑞) + 𝑄 = 𝑈

THIẾT K Ế, VẼ ĐỒ THỊ QUỸ ĐẠO CHUY ỂN ĐỘ NG C A KHÂU THAO TÁC: Ủ

Quỹ đạo chuyển động là đường thẳng trong không gian thao tác:

Ma trận mở r ng c a ma trộ ủ ận 𝐴:

𝑥 −𝑥 𝑒 0

𝑡𝑒−𝑡0𝑥󰇗0

𝑡0−𝑡𝑒+ 𝑥𝑒 −𝑥 0

(𝑡 −𝑡 )𝑒 0 2𝑥󰇗𝑒+𝑥󰇗0

Sơ đồ khối hệ thống điều khiển động lực học ngược vòng ngoài tuyến tín PD:

𝑞 = 𝑑[ 1; 𝜃2]𝑇

𝑀(𝑞)𝑞󰇘 + 𝐶 𝑞, 𝑞󰇗 𝑞󰇗 + 𝐺( ) (𝑞) + 𝑄 = 𝑈

MÔ PH NG SIMULINK: Ỏ