Bài tập lớn robotics - Nguyễn Thị Phương Anh - 2018 3683 PDF

Preview

Summary

Download Bài tập lớn robotics - Nguyễn Thị Phương Anh - 2018 3683 PDF

Description

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN CƠ KHÍ **********

BÀI TẬP LỚN ROBOTICS

Giáo viên hướng dẫn

: PGS.Ts Phan Bùi Khôi

Sinh viên thực hiện

:

Nguyễn Thị Phương Anh: 20183683

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

MÔ HÌNH 1

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

Bảng D-H: 1 2 3

𝜃𝑖 0 𝜃2 𝜃3

𝑑𝑖 𝑑1 0 0

𝑎𝑖 𝑎1 𝑎2 𝑎3

𝛼𝑖 0 90° 0

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

Cơ cấu gồm n = 3 khâu động và k = 3 khớp gồm một khớp tịnh tiến và hai khớp quay (một bậc tự do). Bậc tự do của cơ cấu 𝑓 = 𝜆 ∗ (𝑛 − 𝑘) + ∑𝑓𝑖 + 𝑓𝑒 − 𝑓𝑐 . Trong đó 𝜆 = 6, 𝑛 = 3, 𝑘 = 3 bậc tự do chuyển động của mỗi khớp là 1.

→ 𝑓 = 3.

HỆ PHƯƠNG TRÌNH ĐỘNG HỌC CỦA MÔ HÌNH: Dựa vào bảng D-H:

Ta có: 0𝐴3 = 0𝐴1 ∗ 1𝐴2 ∗ 2𝐴3

0𝐴

1 0 𝑎10 10 0 0 0 1 𝑑1 1 =(

) 𝑐𝜃2 0 𝑠𝜃2 0 0𝑠𝜃02 01 −𝑐𝜃2 → 0𝐴2 = ( 0 1 0

𝑠𝜃2 0 −𝑐𝜃2 𝑎2𝑠𝜃2 𝑐𝜃20 01 𝑠𝜃2 0 𝑎2𝑐𝜃20

1𝐴 = ( 2 𝑎2𝑐𝜃2 + 𝑎1 𝑎2𝑠𝜃2 𝑑1 )

0 1 ng cuối: Chuyển độ0ng 0của điểm tác độ

𝑟𝐸 = 𝑟3 = [𝑥3 𝑦3 𝑧3 ]𝑇

0

0

0

)

0

1

𝑣3𝑥 = −(𝑎3 𝑠𝜃2 𝑐𝜃3 + 𝑎2 𝑠𝜃2 )𝜃2󰇗 −𝑎3𝑐𝜃2 𝑠𝜃3 𝜃3󰇗 𝑣3𝑦 = (𝑎3𝑐𝜃2 𝑐𝜃3 + 𝑎2 𝑐𝜃2 )𝜃2󰇗 −𝑎3𝑠𝜃2 𝑠𝜃3 𝜃3󰇗 𝑣3𝑧 = 𝑑󰇗1 − 𝑎3 𝑐𝜃3 𝜃3󰇗

−𝑐𝛽𝑠𝜂 −𝑠𝛼𝑠𝛽𝑠𝜂 + 𝑐𝛼𝑠𝜂 𝑐𝛼𝑠𝛽𝑠𝜂 + 𝑠𝛼𝑐𝜂

𝜂 = 𝜃3

0

𝑣3 = 𝑟3󰇗

𝑧3 = 𝑑1 + 𝑎3𝑠𝜃3

→ 𝛽 = 𝜃2

3

Tính toán vận tốc của khâu cuối:

𝑦3 = 𝑎3 𝑠𝜃2 𝑐𝜃3 + 𝑎2𝑠𝜃2

𝑐𝛽𝑐𝜂 (𝑠𝛼𝑠𝛽𝑐𝜂 + 𝑐𝛼𝑠𝜂 𝑠𝛼𝑠𝜂 − 𝑐𝛼𝑠𝛽𝑐𝜂

= ( 0

1𝑠𝜃𝑐𝜃 −𝑐𝜃2 𝑎 𝑐𝜃𝑎3𝑐𝜃 𝑠𝜃2 𝑐𝜃3 + 𝑎2𝑠𝜃2 2 𝑐𝜃3 −𝑠𝜃2 𝑠𝜃3 𝑠𝜃 𝑐𝜃 2 3 −𝑐𝜃2 𝑠𝜃3 2 3 2 3 + 𝑎2 𝑐𝜃2 + 𝑎1 ) 𝑠𝜃3 𝑐𝜃3 0 𝑑1 + 𝑎3𝑠𝜃3 → 0𝐴3 = ( 0 0 0 1

𝑥3 = 𝑎3𝑐𝜃2 𝑐𝜃3 + 𝑎2𝑐𝜃2 + 𝑎1

Góc Cardan:

2𝐴

𝑠𝜃3 𝑐𝜃3 0 𝑎3𝑠𝜃3 0 0 1 0 𝑐𝜃3 −𝑠𝜃3 0 𝑎3𝑐𝜃3 )

𝑠𝛽 𝑐𝜃2 𝑐𝜃3 −𝑐𝜃2 𝑠𝜃3 𝑠𝜃2 −𝑠𝛼𝑐𝛽 ) = ( 𝑠𝜃2 𝑐𝜃3 −𝑠𝜃2 𝑠𝜃3 −𝑐𝜃2 ) 𝑐𝛼𝑐𝛽 𝑠𝜃3 𝑐𝜃3 0

𝛼 = 90°

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. Khâu 2:

𝑐𝜃2

−𝑠𝜃2

0 𝑠𝜃2

Ta có: 0𝑅2 = (𝑠𝜃2 0 −𝑐𝜃2 ) → ω  = 0𝑅󰇗2 ∗ 0𝑅 2T = 𝜃󰇗2 ( 𝑐𝜃2 Từ đó ta có:

𝜔2𝑥 = 0

0

1

0

𝜔2𝑦 = 0

𝜔2𝑧 = 𝜃󰇗2

Khâu 3:

→ 𝑐𝜃2 𝑐𝜃3

−𝑐𝜃2 𝑠𝜃3 𝑐𝜃3

0

−𝑠𝜃2 𝑐𝜃3 𝜃󰇗2 − 𝑐𝜃2 𝑠𝜃3 𝜃󰇗3 ω  = 0𝑅󰇗 3 ∗ 0𝑅 3T = ( 𝑐𝜃2 𝑐𝜃3 𝜃󰇗2 − 𝑠𝜃2 𝑠𝜃3 𝜃󰇗3 𝑐𝜃3 𝜃󰇗3 = (

0 𝜃󰇗2 −𝑐𝜃2 𝜃󰇗3

−𝜃󰇗2

𝑎2𝑦 = 0

𝑎2𝑧 = 𝜃󰇘2

𝑠𝜃2

Ta có: 0𝑅3 = (𝑠𝜃2 𝑐𝜃3 −𝑠𝜃2 𝑠𝜃3 −𝑐𝜃2 ) 𝑠𝜃3

𝑎2𝑥 = 0

0

0 𝑠𝜃2 𝑐𝜃2 0 𝑠𝜃2 0 −1 0 0 −𝑐𝜃2 ) ∗ ( 𝑠𝜃2 0 −𝑐𝜃2 ) = 𝜃󰇗2*( 1 0 0) 0 0 0 1 0 0 1 0

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

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

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

Từ đó ta có:

𝜔3𝑥 = 𝑠𝜃2 𝜃󰇗3

𝜔3𝑦 = 𝑐𝜃2 𝜃󰇗3

→

𝜔3𝑧 = 𝜃󰇗2

2 𝑎3𝑥 = 𝑐𝜃2 𝜃󰇗3 + 𝑠𝜃2 𝜃󰇘3

𝑎3𝑦 = 𝑐𝜃2 𝜃󰇗3

𝑎3𝑧 = 𝜃󰇘2

TÍNH TOÁN LỰC TÁC D ỤNG LÊN CÁC KHÂU CỦA MÔ HÌNH: Sơ đồ lực tác dụng lên các khâu: 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𝑟𝑐𝑖𝑖 ∗ 0𝑃𝑖 − 0𝑟𝑖−1

0𝑀

𝑖,𝑖−1

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

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

0

2𝑟 2

1

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

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

0 𝑠𝜃2 0 −𝑐𝜃2

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

−𝑎 𝑐𝜃 −𝑎02 −𝑎 2𝑠𝜃 2) )= ( 2 2 )∗ ( 0 0 0 −𝑎3 𝑐𝜃2 𝑐𝜃3 −𝑐𝜃2 𝑠𝜃3 𝑠𝜃2 −𝑎3 −𝑠𝜃2 𝑠𝜃3 −𝑐𝜃2 ) ∗ ( 0 ) = (−𝑎3 𝑠𝜃2 𝑐𝜃3) 0 −𝑎3 𝑠𝜃3 𝑐𝜃3 0

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

0𝑟 1

𝑐1

0𝑟 2

𝑐2

0𝑟 2

𝑐3

= [−

=[

𝑎1 2

; 0; 0]T

−𝑎2𝑐𝜃2 2

=[

;

−𝑎2𝑠𝜃2

;

−𝑎3𝑐𝜃2𝑐𝜃3 2

2

; 0]T

−𝑎2 𝑠𝜃2𝑐𝜃3 2

;

−𝑎3𝑠𝜃3 T ] 2

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

= [00 − 𝑃1 ]T

0𝑃

= [00−𝑃3 ]T

1

0𝑃

2

2

= [00−𝑃2 ]T

𝐹𝐸 = [𝐹𝑥 𝐹𝑦 𝐹𝑧 ]T

𝑀𝐸 = [𝑀𝑥 𝑀𝑦 𝑀𝑧 ]T

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

0

0 0 𝑑1 0 𝑎1 ) = (−𝑑1 0 −𝑎1 0

0 0𝑟 2 1 = ( 0 𝑎2 𝑠𝜃2

0 −𝑎2 𝑠𝜃2 0 𝑎2 𝑐𝜃2 ) −𝑎2 𝑐𝜃2 0

0 0𝑟 3 −𝑎 = ( 3 𝑠𝜃3 2 𝑎2 𝑠𝜃2 𝑐𝜃3

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

Tính toán lực tác động lên các khâu: Khâu cuối:

𝐹𝐸 = [𝐹𝑥 𝐹𝑦 𝐹𝑧 ]T

Tọa độ khối tâm: 0𝑟

1 𝑐1

0𝑟 2

𝑐2

0𝑟 3

𝑐3

0 0 0 𝑎1 = (0 0 2) −𝑎1 0 0

= =

2

0

0

𝑎2 𝑠𝜃2

(

(

2

0

−𝑎3𝑠𝜃3

0

−𝑎2𝑠𝜃2 2 𝑎2 𝑐𝜃2 2

−𝑎2 𝑐𝜃2

2 𝑎2𝑠𝜃2𝑐𝜃3 2

0 2

𝑎3 𝑠𝜃3 2

0

−𝑎3𝑐𝜃2𝑐𝜃3 2

0

)

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

0

)

𝑀𝐸 = [𝑀𝑥 𝑀𝑦 𝑀𝑧 ]T

𝐹𝑥 𝐹3,2 = 0𝐹𝐸 − 0𝑃3 = ( 𝐹𝑦

𝐹𝑦𝑥 ) 00 ) − ( ) = (𝐹𝑧 + 𝑃3 −𝑃3 𝐹𝑧 ∗ 0𝑃3 − 0𝑟23 ∗ 0𝐹3,2

0

𝑀3,2 = 0𝑀𝐸 − 0𝑟𝑐33

0

𝑀𝑥 → 0𝑀3,2 = (𝑀𝑦 ) − 𝑀𝑧 =

Khâu 2: 𝑀3,2 =

𝐹3,2 = (

(

−𝑎3 𝑠𝜃3 2 𝑎2𝑠𝜃2𝑐𝜃3 ( 2

𝑎2 𝑠𝜃2 𝑐𝜃3

2 𝑎3 𝑐𝜃2𝑐𝜃3

𝑀𝑦 −

𝑎3𝑠𝜃3

−𝑎2 𝑠𝜃2 𝑐𝜃3

−𝑎3 𝑐𝜃2 𝑐𝜃3

0

2

0 2

2 𝑎3 𝑐𝜃2 𝑐𝜃3 2

𝑀𝑥 +

𝑃3 − 𝐹𝑦 𝑎3 𝑠𝜃3 +𝑎2 𝑠𝜃2𝑐𝜃3𝐹𝑧

𝑃3 − 𝐹𝑦 𝑎3 𝑠𝜃3 +𝑎2 𝑠𝜃2𝑐𝜃3 𝐹𝑧

𝑎2 𝑠𝜃2 𝑐𝜃3

2 𝑎3 𝑐𝜃2 𝑐𝜃3

𝑃3 + 𝑎3 𝑠𝜃3𝐹𝑥 − 𝑎3 𝑐𝜃2𝑐𝜃3𝐹𝑧 2 𝑀𝑧 − 𝑎2 𝑠𝜃2𝑐𝜃3 𝐹𝑥 + 𝑎3 𝑐𝜃2𝑐𝜃3𝐹𝑦 )

𝐹𝑥 𝐹𝑦 ) 𝐹𝑧 + 𝑃3

𝐹2,1 = 0𝐹3,2 − 0𝑃2 = (

0

𝐹𝑥 𝐹𝑥 0 𝐹𝑦 ) − ( 0 ) = ( 𝐹𝑦 ) −𝑃2 𝐹𝑧 + 𝑃3 + 𝑃2 𝐹𝑧 + 𝑃3

𝑀2,1 = 0𝑀3,2 − 0𝑟𝑐22 ∗ 0𝑃2 − 0𝑟12 ∗ 0𝐹2,1

0

=

=

𝑀𝑥 +

𝑎2 𝑠𝜃2 𝑐𝜃3

2 𝑎 𝑐𝜃 𝑐𝜃 𝑀𝑦 − 3 2 3 2

(

(

)

𝑃3 − 𝐹𝑦 𝑎3 𝑠𝜃3 +𝑎2 𝑠𝜃2 𝑐𝜃3 𝐹𝑧

𝑃3 + 𝑎3 𝑠𝜃3𝐹𝑥 − 𝑎3 𝑐𝜃2𝑐𝜃3𝐹𝑧 − 𝑀𝑧 − 𝑎2 𝑠𝜃2 𝑐𝜃3 𝐹𝑥 + 𝑎3 𝑐𝜃2𝑐𝜃3𝐹𝑦 )

𝑀𝑥 + (

𝑀𝑦 − (

2

(

0

𝑎2 𝑠𝜃2 2

0

0

−𝑎2 𝑐𝜃2 2

𝑀2,1 =

𝑀𝑥 + (

𝑎2 𝑠𝜃2 𝑐𝜃3

2 𝑎3 𝑐𝜃2𝑐𝜃3

𝐹1,0 = 0𝐹2,1 − 0𝑃1 = (

)

0 0 ∗ ( 0 ) −( 0 −𝑃2 𝑎2 𝑠𝜃2

𝑃2 𝑎2 𝑠𝜃2

2 𝑃2 𝑎2 𝑐𝜃2 2

)

+𝑎2 𝑠𝜃2 )𝑃3 − 𝐹𝑦 𝑎3 𝑠𝜃3+(𝑎2𝑠𝜃2 𝑐𝜃3 + 𝑎2 𝑠𝜃2) 𝐹𝑧 +

𝑃2 𝑎2𝑠𝜃2

+ 𝑎2 𝑐𝜃2 ) 𝑃3 + 𝑎3 𝑠𝜃3 𝐹𝑥 − (𝑎3 𝑐𝜃2𝑐𝜃3 + 𝑎2 𝑐𝜃2)𝐹𝑧 − 2 𝑀𝑧 − (𝑎2 𝑠𝜃2𝑐𝜃3 +𝑎2 𝑠𝜃2)𝐹𝑥 + (𝑎3𝑐𝜃2𝑐𝜃3 + 𝑎2 𝑐𝜃2 )𝐹𝑦

𝐹𝑥 𝐹𝑦 ) 𝐹2,1 = ( 𝐹𝑧 + 𝑃3 + 𝑃2

𝐹𝑥 𝐹𝑥 0 𝐹𝑦 𝐹𝑦 )− ( 0 )=( ) −𝑃1 𝐹𝑧 + 𝑃3 + 𝑃2 𝐹𝑧 + 𝑃3 + 𝑃2 + 𝑃1

𝑀1,0 = 0𝑀2,1 − 0𝑟𝑐11 ∗ 0𝑃1 − 0𝑟11 ∗ 0𝐹1,0 =

0

0

+ 𝑎2 𝑐𝜃2 ) 𝑃3 + 𝑎3 𝑠𝜃3 𝐹𝑥 − (𝑎3 𝑐𝜃2 𝑐𝜃3 + 𝑎2 𝑐𝜃2 )𝐹𝑧 −

0

0

2 𝑎2 𝑐𝜃2 2

𝑀𝑧 − (𝑎2 𝑠𝜃2 𝑐𝜃3 +𝑎2 𝑠𝜃2 )𝐹𝑥 + (𝑎3 𝑐𝜃2 𝑐𝜃3 + 𝑎2 𝑐𝜃2 )𝐹𝑦

𝑀𝑦 − (

(

−𝑎2 𝑠𝜃2

+𝑎2 𝑠𝜃2 )𝑃3 − 𝐹𝑦 𝑎3 𝑠𝜃3 +(𝑎2 𝑠𝜃2 𝑐𝜃3 + 𝑎2 𝑠𝜃2 ) 𝐹𝑧 +

𝑎2 𝑠𝜃2 𝑐𝜃3

2 𝑎3 𝑐𝜃2 𝑐𝜃3

0

Khâu 1:

0

𝐹𝑥 −𝑎2 𝑠𝜃2𝑐𝜃3 𝑎3 𝑠𝜃3 0 𝑎3 𝑐𝜃2 𝑐𝜃3 ) ∗ ( 𝐹𝑦 ) 0 𝐹𝑧 + 𝑃3 −𝑎3 𝑐𝜃2𝑐𝜃3

0 ( −𝑎3 𝑠𝜃3 𝑎2 𝑠𝜃2𝑐𝜃3

0 ∗ ( 0 )− −𝑃3

𝑃3 + 𝑎3 𝑠𝜃3 𝐹𝑥 − 𝑎3 𝑐𝜃2 𝑐𝜃3 𝐹𝑧 2 𝑀𝑧 − 𝑎2 𝑠𝜃2𝑐𝜃3𝐹𝑥 + 𝑎3 𝑐𝜃2 𝑐𝜃3𝐹𝑦 )

𝑀𝑦 −

0

0

(

𝑀𝑥 +

0

2 𝑃2 𝑎2𝑐𝜃2 2

)

𝐹𝑥 0 −𝑎2 𝑠𝜃2 𝐹𝑦 0 𝑎2 𝑐𝜃2 ) ∗ ( ) −𝑎2 𝑐𝜃2 0 𝐹𝑧 + 𝑃3 + 𝑃2

𝑀𝑥 + (

𝑀𝑦 − (

(

=

𝑎2 𝑠𝜃2 𝑐𝜃3

2 𝑎3 𝑐𝜃2 𝑐𝜃3

+𝑎2 𝑠𝜃2)𝑃3 − 𝐹𝑦 𝑎3 𝑠𝜃3 +(𝑎2 𝑠𝜃2𝑐𝜃3 + 𝑎2 𝑠𝜃2) 𝐹𝑧 +

𝑃2 𝑎2 𝑠𝜃2

+ 𝑎2 𝑐𝜃2 ) 𝑃3 + 𝑎3 𝑠𝜃3𝐹𝑥 − (𝑎3 𝑐𝜃2𝑐𝜃3 + 𝑎2 𝑐𝜃2)𝐹𝑧 − 𝑀𝑧 − (𝑎2 𝑠𝜃2𝑐𝜃3+𝑎2 𝑠𝜃2)𝐹𝑥 + (𝑎3 𝑐𝜃2𝑐𝜃3 + 𝑎2 𝑐𝜃2)𝐹𝑦 2

𝑀𝑥 + (

𝑎 𝑐𝜃 𝑐𝜃 𝑀𝑦 − ( 3 2 3 2

(

𝑎2 𝑠𝜃2 𝑐𝜃3 2

0 0 𝑎0 0 0 1 0 1 𝑑10 𝑎01 𝐹𝐹𝑥𝑦 0 −𝑑 ) 2 0 − (0 −𝑎1 )− ( ) ∗ ( 𝐹𝑧 + 𝑃3 + 𝑃2 + 𝑃1 ∗ −𝑃 0 −𝑎1 0 1 ) 0

2 𝑃2 𝑎2 𝑐𝜃2 2

2

+𝑎2 𝑠𝜃2)𝑃3 − 𝐹𝑦 (𝑎3 𝑠𝜃3 + 𝑑1) +(𝑎2 𝑠𝜃2𝑐𝜃3 + 𝑎2 𝑠𝜃2)𝐹𝑧 +2 2

𝑃 𝑎2 𝑠𝜃2

+ 𝑎2 𝑐𝜃2 + 𝑎1 ) 𝑃3 + (𝑎3 𝑠𝜃3 + 𝑑1)𝐹𝑥 − (𝑎3 𝑐𝜃2𝑐𝜃3 + 𝑎2 𝑐𝜃2 + 𝑎1 )𝐹𝑧 − 𝑃2 ( 2 +2 𝑎1 ) − 𝑃1 𝑎1 2 2 𝑀𝑧 − (𝑎2 𝑠𝜃2𝑐𝜃3+𝑎2 𝑠𝜃2)𝐹𝑥 + (𝑎3 𝑐𝜃2𝑐𝜃3 + 𝑎2 𝑐𝜃2 +𝑎1 )𝐹𝑦 ) 𝑎 𝑐𝜃

KHẢO SÁT ĐỘNG HỌC: Các Tensor quán tính: c1Θ

𝑐1

0 0

=

(0

0

𝑚1 𝑎12 12

0

0

0

𝑚1 𝑎1 2 12 )

c2Θ

𝑐2

0

0

0

=

𝑚2 𝑎22 12

(0

0

0

0

c3Θ

𝑐3

𝑚2 𝑎2 2 12 )

=

0 0

(0

0

𝑚3 𝑎32 12

0

0

0

𝑚3 𝑎3 2 12 )

Các ma trận Jacobi tịnh tiến:

Các thành phần q =[ 𝑑1 , 𝜃2 , 𝜃3 ] Khâu 1 0𝑟

𝑐1

𝑎1 2

= (0) 𝑑1

Khâu 2: 2𝐴

𝑐2

=

→ 0𝐴𝑐2 0𝑟

𝑐2

=

3𝐴

𝑐3

0 0 0 = (0 0 0) 1 0 0

0𝐴

𝑐𝜃2 0 𝑠𝜃2 𝑠𝜃 0 −𝑐𝜃2 = ( 2 0 1 0 0 0 0

𝑇1

1 0 0 −

0 1 0 0 0 1 (0 0 0

𝑎2

0 0 1

2

2

)

𝑐𝜃2 0 𝑠𝜃2 𝑎2𝑐𝜃2 + 𝑎1 𝑎2𝑠𝜃2 ) ∗ 0 2 = 𝐴2 ∗ 𝐴𝑐2 = ( 𝑠𝜃2 0 −𝑐𝜃2 0 1 0 𝑑1 0 0 0 1 𝑎2 𝑐𝜃2 + 𝑎1 2 ( 𝑎2𝑠𝜃2 )

Khâu 3:

0𝐽

→ 0𝐽𝑇2

2

𝑑1

1 0 0 −

= 0 1 0 0 0 1 (0 0 0

𝑎3

0 0 1

1 0 0 (0

2

)

0𝐴

3

0 0 − 2 1 0 0 0 1 0 0 0 1 𝑎

0 = ( 0 1

2

)

−𝑎2𝑠𝜃2 2 𝑎2𝑐𝜃2

0

2

=

𝑎2 𝑐𝜃2 + 𝑎1 𝑎2 𝑠𝜃2 ) 𝑑1 1

𝑐𝜃2

𝑠𝜃2

0 𝑠𝜃2

0 −𝑐𝜃2

0 1 ( 0 0

0 ) 0 0

𝑐𝜃2 𝑐𝜃3 −𝑐𝜃2 𝑠𝜃3 𝑠𝜃 𝑐𝜃 −𝑠𝜃2 𝑠𝜃3 = ( 2 3 𝑠𝜃3 𝑐𝜃3 0 0

0 0

𝑎2 𝑐𝜃2 + 𝑎1 2 𝑎2 𝑠𝜃2 2

𝑑1 1

)

𝑠𝜃2 𝑎3 𝑐𝜃2 𝑐𝜃3 + 𝑎2 𝑐𝜃2 + 𝑎1 −𝑐𝜃2 𝑎3 𝑠𝜃2 𝑐𝜃3 + 𝑎2 𝑠𝜃2 ) 0 𝑑1 + 𝑎3 𝑠𝜃3 0 1

→ 0𝐴𝑐3

𝑠𝜃2𝑐𝜃 𝑐𝜃3 −𝑐𝜃 −𝑠𝜃2𝑠𝜃 𝑠𝜃3 −𝑐𝜃 𝑎3 𝑠𝜃 2 𝑐𝜃3 + 𝑎2 𝑠𝜃2 𝑐𝜃 𝑠𝜃2 2 𝑎3 𝑐𝜃 2 3 2 3 2 𝑐𝜃3 + 𝑎2 𝑐𝜃2 + 𝑎1 𝑠𝜃3 𝑐𝜃3 0 𝑑1 + 𝑎3 𝑠𝜃3 )∗ = 0𝐴3 ∗ 3𝐴𝑐3 = ( 0 0 0 1 𝑎3 𝑐𝜃2𝑐𝜃3 𝑐𝜃2 𝑐𝜃3 −𝑐𝜃2 𝑠𝜃3 𝑠𝜃2 + 𝑎2 𝑐𝜃2 + 𝑎1

=

→ 0𝑟𝑐3 =

(

𝑠𝜃2 𝑐𝜃3 𝑠𝜃3 0

𝑎3𝑐𝜃2𝑐𝜃3

(

−𝑠𝜃2 𝑠𝜃3 0

0

+ 𝑎2 𝑐𝜃2 + 𝑎1

2 𝑎3𝑠𝜃2𝑐𝜃3 2

𝑐𝜃3

−𝑐𝜃2

𝑑1 +

+ 𝑎2 𝑠𝜃2 𝑎3 𝑠𝜃3

Các ma trận jacobi quay: 0𝑅

1

2

𝑑1 +

+ 𝑎2 𝑠𝜃2

𝑎3𝑠𝜃3 2

1

→ 0𝐽𝑇3 =

)

2

Khâu 1:

0

2 𝑎3 𝑠𝜃2𝑐𝜃3

0

0

1 (

)

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

0

+ 𝑎2 𝑐𝜃2

0 0 0 → c1𝜔 1𝑟 = 0𝑅1𝑇 ∗ 0𝑅󰇗1 = ( 0 0 0) 0 0 0

1 0 0 = (0 1 0) 0 0 1

− 𝑎2 𝑠𝜃2

1 0 0 (0

0 1 0 0

0 − 3 0 0 1 0 0 1 𝑎

2

)

−𝑎3𝑐𝜃2𝑠𝜃3

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

)

0 0 0 0 → 𝜔1𝑟 = ( 0) → 𝐽𝑅1 = (0 0 0) 0 0 0 0

Khâu 2: 0𝑅

2

𝑐𝜃2 0 𝑠𝜃2 = (𝑠𝜃2 0 −𝑐𝜃2 ) 0 1 0

𝑐𝜃2 →c2𝜔  2𝑟 = 0𝑅2𝑇 ∗ 0𝑅󰇗2 = ( 0 𝑠𝜃2

𝑠𝜃2 0 0 0 1 −𝑠𝜃2 0 𝑐𝜃2 0 1 ) ∗ ( 𝑐𝜃2 0 𝑠𝜃2) ∗ 𝜃󰇗2 = 𝜃󰇗2 ( 0 0 ) 0 −𝑐𝜃2 0 −1 0 0 0 0 0

Khâu 3

𝑠𝜃2 −𝑐𝜃2) 0

0 0 0 0 → 𝜔𝑟2 = (𝜃󰇗2 ) → 𝐽𝑅2 = (0 1 0) 0 0 0 0 0𝑅

3

𝑐𝜃2 𝑐𝜃3 = (𝑠𝜃2 𝑐𝜃3 𝑠𝜃3

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

𝑐𝜃2 𝑐𝜃3

𝑠𝜃2 𝑐𝜃3

𝑠𝜃3

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

→c3𝜔 𝑟3 = 0𝑅𝑇3 ∗ 0𝑅󰇗3 = ( −𝑐𝜃2 𝑠𝜃3 −𝑠𝜃2 𝑠𝜃3 𝑐𝜃3 ) ∗ ( 𝑐𝜃2 𝑐𝜃3 𝜃󰇗2 − 𝑠𝜃2 𝑠𝜃3 𝜃󰇗3 𝑠𝜃2

−𝑐𝜃2

0

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

𝑠𝜃3 𝜃󰇗 2 0 𝑠𝜃3 0 → 𝜔𝑟3 = (𝑐𝜃3 𝜃󰇗 2 ) → 𝐽𝑅3 = (0 𝑐𝜃3 )0 0 0 1 𝜃󰇗 3

𝑐𝜃3 𝜃󰇗3

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

𝑐𝜃2 𝜃󰇗2 𝑠𝜃2 𝜃󰇗2 ) 0

3

∑ 𝑀𝑖 (𝑞 ) Ma trận khối lượng: 𝑀 (𝑞) = 𝑖=1 Khâu 1:

𝑀1 (𝑞) = 0𝐽𝑇1 𝑇 ∗ 𝑚1 ∗ 0𝐽𝑇1 + 0𝐽𝑅1 𝑇 ∗

c1Θ

𝑐1

∗ 0𝐽𝑅1

0

0 0 1 0 0 0 0 0 0 = 𝑚1 ∗ (0 0 0) ∗ ( 0 0 0) + ( 0 0 0) ∗ 0 0 0 1 0 0 0 0 0

1 0 0 = 𝑚1 ∗ (0 0 0) 0 0 0

Khâu 2:

𝑀2 (𝑞) = 0𝐽𝑇2 𝑇 ∗ 𝑚1 ∗ 0𝐽𝑇2 + 0𝐽𝑅2 𝑇 ∗

0𝐽

𝑇2

0𝐽

𝑅2

𝑇

𝑇

∗ 𝑚2 ∗ 0𝐽𝑇2 = 𝑚2 ∗ ( ∗

c2Θ

𝑐2

∗

0𝐽

𝑅2

1

→ 𝑀2 (𝑞) = ( 0 0

Khâu 3:

0

0

3

0

0

2

2

0

0

0

𝑀3 (𝑞) = 0𝐽𝑇3 𝑇 ∗ 𝑚1 ∗ 0𝐽𝑇3 + 0𝐽𝑅3 𝑇 ∗ 𝐽𝑇3𝑇 ∗ 𝑚3 ∗ 0𝐽𝑇3 = 𝑚3 ∗ (

0

= 𝑚3 ∗ 0𝐽

𝑅3

𝑇

∗

Θ𝑐3 ∗ 𝐽𝑅3

c3

0

0 = (𝑠𝜃3 0

0 1 ) ∗ ( 0 0 0 1

0

𝑐3

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

(−

0

0

c3Θ

0

1

∗ 0𝐽𝑅2

𝑚2 𝑎22 12

(0

)0

0

𝑐2

𝑎2𝑐𝜃2

−𝑎2 𝑠𝜃2

0 0 0 = (0 1 0) ∗ 0 0 0

𝑚2 𝑎22

c2Θ

0

𝑎3 𝑐𝜃2 𝑐𝜃3

𝑎3 𝑐𝜃3 2

2

2

)

0 + 𝑎2𝑐𝜃2

2 −𝑎3 𝑠𝜃2 𝑠𝜃3

0

𝑎

0

12

2

(23𝑐𝜃3 + 𝑎2)

0

0

0

−

2

𝑎3 𝑐𝜃3 2

0

𝑎23 4

12

(0

2 𝑎2 𝑐𝜃2

𝑚2 𝑎2

∗ 0𝐽𝑅3

0

−𝑎2 𝑠𝜃2

0

0

𝑚1 𝑎12

)

0

0

0

𝑚1 𝑎1 2 12 )

0 0 0 ∗ (0 0 0) 0 0 0

1 0 0 0 𝑎2 )0= 𝑚2 ∗ ( 0 2 )0 4 0 0 0 0

0 0 0 0 0 0 2 ∗ (0 1 0) = ( 0 𝑚2 𝑎2 ) 0 12 0 0 0 0 0 0

1

0 ) ∗

𝑎3 𝑐𝜃3 2

0

(

0

1

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

0

−𝑎3 𝑐𝜃2 𝑠𝜃3

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

)

0 0 0 0 0 0 0 𝑠𝜃3 0 0 𝑠𝜃3 0 0 0 𝑚3 𝑠𝜃3 𝑎3 2 𝑚3 𝑎32 0 0 0 ) ∗ ( 0 𝑐𝜃 ) 0 ) ∗ ( 0 𝑐𝜃3 0 )= ( 12 12 𝑐𝜃3 0) ∗ ( 3 0 𝑚3 𝑎3 2 𝑚3 𝑎3 2 0 0 1 0 1 0 0 1 0 0 0 0 12 12 0

= 𝑚3 ∗ ( 0 0

0

𝑠𝜃3 𝑐𝜃3 𝑎32 12

0

0 0 )

𝑎3

2

12

→ 𝑀3(𝑞) = 𝑚3 ∗

(−

= 𝑚3 ∗

1

0

2

( 3𝑐𝜃3 + 𝑎2) 2 𝑎

𝑎3 𝑐𝜃3 2

(−

0

1

0

0

−

𝑎3 𝑐𝜃3 2

2

𝑎3 𝑐𝜃3 2

0

𝑎32

)

4

2

( 𝑐𝜃3 + 𝑎2) +

0

𝑎3

0

2 𝑠𝜃3 𝑐𝜃3 𝑎 3

12

12

−

𝑚3 𝑎3 𝑐𝜃3

0

𝑎3 𝑐𝜃3

0

0

𝑎3

2

𝑎3

2

)

𝑚1 + 𝑚2 + 𝑚3 2

0

𝑠𝜃3 𝑐𝜃3 𝑎32

+ 𝑚3 ∗ (0

12

→ 𝑀(𝑞) = 𝑀1 (𝑞) + 𝑀2(𝑞) + 𝑀3(𝑞) = 0 −

0

𝑚3 (

𝑎3

2
...