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 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 : Nguyễn Thị Phương Anh: 20183683 download by : skknchat@gmail.com MỤC LỤC Ni dung MƠ HÌNH BIỂU DIỄN HỆ TỌA ĐỘ KHỚP Bảng D-H: SỐ BẬC TỰ DO CỦA MƠ HÌNH ROBOT: HỆ PHƯƠNG TRÌNH ĐỘNG HỌC CỦA MƠ HÌNH: PHƯƠNG TRÌNH VI PHÂN CHUYỂN ĐỘNG CỦA MƠ HÌNH: TÍNH TỐN LỰC TÁC DỤNG LÊN CÁC KHÂU CỦA MƠ HÌNH: KHẢO SÁT ĐỘNG HỌC: 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 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 TỐ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 download by : skknchat@gmail.com MƠ HÌNH BIỂU DIỄN HỆ TỌA ĐỘ KHỚP 𝜃𝑖 𝜃2 𝜃3 Bảng D-H: 𝑑𝑖 𝑑1 0 𝑎𝑖 𝑎1 𝑎2 𝑎3 𝛼𝑖 90° SỐ BẬC TỰ DO CỦA MÔ HÌNH ROBOT: - Cơ cấu gồm n = khâu động k = khớp gồm khớp tịnh tiến hai khớp quay (một bậc tự do) Bậc tự cấu 𝑓 = 𝜆 ∗ (𝑛 − 𝑘) + ∑𝑓𝑖 + 𝑓𝑒 − 𝑓𝑐 Trong 𝜆 = 6, 𝑛 = 3, 𝑘 = bậc tự chuyển động khớp → 𝑓 = 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 download by : skknchat@gmail.com 0𝐴 𝑎10 10 0 𝑑1 =( ) 𝑐𝜃2 𝑠𝜃2 0𝑠𝜃02 01 −𝑐𝜃2 → 0𝐴2 = ( 𝑠𝜃2 −𝑐𝜃2 𝑎2𝑠𝜃2 𝑐𝜃20 01 𝑠𝜃2 𝑎2𝑐𝜃20 1𝐴 = ( 𝑎2𝑐𝜃2 + 𝑎1 𝑎2𝑠𝜃2 𝑑1 ) ng cuối: Chuyển độ0ng 0của điểm tác độ 𝑟𝐸 = 𝑟3 = [𝑥3 𝑦3 𝑧3 ]𝑇 0 ) 𝑣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 𝑣3 = 𝑟3󰇗 𝑧3 = 𝑑1 + 𝑎3𝑠𝜃3 → 𝛽 = 𝜃2 Tính tốn vận tốc khâu cuối: 𝑦3 = 𝑎3 𝑠𝜃2 𝑐𝜃3 + 𝑎2𝑠𝜃2 𝑐𝛽𝑐𝜂 (𝑠𝛼𝑠𝛽𝑐𝜂 + 𝑐𝛼𝑠𝜂 𝑠𝛼𝑠𝜂 − 𝑐𝛼𝑠𝛽𝑐𝜂 = ( 1𝑠𝜃𝑐𝜃 −𝑐𝜃2 𝑎 𝑐𝜃𝑎3𝑐𝜃 𝑠𝜃2 𝑐𝜃3 + 𝑎2𝑠𝜃2 𝑐𝜃3 −𝑠𝜃2 𝑠𝜃3 𝑠𝜃 𝑐𝜃 −𝑐𝜃2 𝑠𝜃3 3 + 𝑎2 𝑐𝜃2 + 𝑎1 ) 𝑠𝜃3 𝑐𝜃3 𝑑1 + 𝑎3𝑠𝜃3 → 0𝐴3 = ( 0 𝑥3 = 𝑎3𝑐𝜃2 𝑐𝜃3 + 𝑎2𝑐𝜃2 + 𝑎1 Góc Cardan: 2𝐴 𝑠𝜃3 𝑐𝜃3 𝑎3𝑠𝜃3 0 𝑐𝜃3 −𝑠𝜃3 𝑎3𝑐𝜃3 ) 𝑠𝛽 𝑐𝜃2 𝑐𝜃3 −𝑐𝜃2 𝑠𝜃3 𝑠𝜃2 −𝑠𝛼𝑐𝛽 ) = ( 𝑠𝜃2 𝑐𝜃3 −𝑠𝜃2 𝑠𝜃3 −𝑐𝜃2 ) 𝑐𝛼𝑐𝛽 𝑠𝜃3 𝑐𝜃3 𝛼 = 90° PHƯƠNG TRÌNH VI PHÂN CHUYỂN ĐỘNG CỦA MƠ HÌNH: Tính tốn vận tốc góc gia tốc góc cho khâu Khâu 2: 𝑐𝜃2 −𝑠𝜃2 𝑠𝜃2 Ta có: 0𝑅2 = (𝑠𝜃2 −𝑐𝜃2 ) → ω  = 0𝑅󰇗2 ∗ 0𝑅 2T = 𝜃󰇗2 ( 𝑐𝜃2 Từ ta có: 𝜔2𝑥 = 0 𝜔2𝑦 = 𝜔2𝑧 = 𝜃󰇗2 Khâu 3: → 𝑐𝜃2 𝑐𝜃3 −𝑐𝜃2 𝑠𝜃3 𝑐𝜃3 −𝑠𝜃2 𝑐𝜃3 𝜃󰇗2 − 𝑐𝜃2 𝑠𝜃3 𝜃󰇗3 ω  = 0𝑅󰇗 ∗ 0𝑅 3T = ( 𝑐𝜃2 𝑐𝜃3 𝜃󰇗2 − 𝑠𝜃2 𝑠𝜃3 𝜃󰇗3 𝑐𝜃3 𝜃󰇗3 = ( 𝜃󰇗2 −𝑐𝜃2 𝜃󰇗3 −𝜃󰇗2 𝑎2𝑦 = 𝑎2𝑧 = 𝜃󰇘2 𝑠𝜃2 Ta có: 0𝑅3 = (𝑠𝜃2 𝑐𝜃3 −𝑠𝜃2 𝑠𝜃3 −𝑐𝜃2 ) 𝑠𝜃3 𝑎2𝑥 = 0 𝑠𝜃2 𝑐𝜃2 𝑠𝜃2 −1 0 −𝑐𝜃2 ) ∗ ( 𝑠𝜃2 −𝑐𝜃2 ) = 𝜃󰇗2*( 0) 0 0 𝑠𝜃2 𝑠𝜃3 𝜃󰇗2 − 𝑐𝜃2 𝑐𝜃3 𝜃󰇗3 −𝑐𝜃2 𝑠𝜃3 𝜃󰇗2 − −𝑠𝜃2 𝑐𝜃3 𝜃󰇗3 −𝑠𝜃3 𝜃󰇗3 𝑐𝜃2 𝜃󰇗3 −𝑠𝜃2 𝜃󰇗3 ) 𝑠𝜃2 𝜃󰇗3 𝑐𝜃2 𝑐𝜃3 −𝑐𝜃2 𝑠𝜃3 𝑠𝜃2 𝑐𝜃2 𝜃󰇗2 𝑠𝜃 𝑐𝜃 𝑠𝜃2 𝜃󰇗2 ) ∗ ( −𝑠𝜃2 𝑠𝜃3 −𝑐𝜃2 ) 𝑠𝜃3 𝑐𝜃3 0 download by : skknchat@gmail.com Từ ta có: 𝜔3𝑥 = 𝑠𝜃2 𝜃󰇗3 𝜔3𝑦 = 𝑐𝜃2 𝜃󰇗3 → 𝜔3𝑧 = 𝜃󰇗2 𝑎3𝑥 = 𝑐𝜃2 𝜃󰇗3 + 𝑠𝜃2 𝜃󰇘3 𝑎3𝑦 = 𝑐𝜃2 𝜃󰇗3 𝑎3𝑧 = 𝜃󰇘2 TÍNH TỐ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 khâu: Khâu 1: Khâu 2: Khâu 3: Áp dụng hệ phương trình đệ quy tính tốn lực khớp 0𝐹 𝑖,𝑖−1 = 0𝐹𝑖+1,𝑖 − 0𝑃𝑖 0𝑀 𝑖+1,𝑖 𝑖 ∗ 0𝐹𝑖,𝑖−1 − 0𝑟𝑐𝑖𝑖 ∗ 0𝑃𝑖 − 0𝑟𝑖−1 download by : skknchat@gmail.com 0𝑀 𝑖,𝑖−1 𝑖 ∗ 0𝐹𝑖,𝑖−1 − 0𝑟𝑐𝑖𝑖 ∗ 0𝑃𝑖 = 0𝑀𝑖+1,𝑖 − 0𝑟𝑖−1 Các vecto tọa độ khớp biểu diễn hệ tọa độ sở: 0𝑟1 2𝑟 = [−𝑎1 ; 0; −𝑑1 ]T 𝑐𝜃2 = [−𝑎2 ; 0; 0]T → 0𝑟12 = 0𝑅2 ∗ 2𝑟12 = (𝑠𝜃2 𝑠𝜃2 −𝑐𝜃2 𝑐𝜃2 𝑐𝜃3 3𝑟 T 3 = [−𝑎3 ; 0; 0] → 𝑟2 = 𝑅 ∗ 𝑟3 = ( 𝑠𝜃2 𝑐𝜃3 𝑠𝜃3 −𝑎 𝑐𝜃 −𝑎02 −𝑎 2𝑠𝜃 2) )= ( 2 )∗ ( 0 −𝑎3 𝑐𝜃2 𝑐𝜃3 −𝑐𝜃2 𝑠𝜃3 𝑠𝜃2 −𝑎3 −𝑠𝜃2 𝑠𝜃3 −𝑐𝜃2 ) ∗ ( ) = (−𝑎3 𝑠𝜃2 𝑐𝜃3) −𝑎3 𝑠𝜃3 𝑐𝜃3 Các vecto khối tâm biểu diễn hệ tọa độ sở: 0𝑟 𝑐1 0𝑟 𝑐2 0𝑟 𝑐3 = [− =[ 𝑎1 ; 0; 0]T −𝑎2𝑐𝜃2 =[ ; −𝑎2𝑠𝜃2 ; −𝑎3𝑐𝜃2𝑐𝜃3 2 ; 0]T −𝑎2 𝑠𝜃2𝑐𝜃3 ; −𝑎3𝑠𝜃3 T ] Biểu diễn lực hệ tọa độ sở: 0𝑃 = [00 − 𝑃1 ]T 0𝑃 = [00−𝑃3 ]T 0𝑃 2 = [00−𝑃2 ]T 𝐹𝐸 = [𝐹𝑥 𝐹𝑦 𝐹𝑧 ]T 𝑀𝐸 = [𝑀𝑥 𝑀𝑦 𝑀𝑧 ]T Thiết lập ma trận tọa độ khớp khối tâm: Tọa độ khớp: 0𝑟 0 𝑑1 𝑎1 ) = (−𝑑1 −𝑎1 0 0𝑟 = ( 𝑎2 𝑠𝜃2 −𝑎2 𝑠𝜃2 𝑎2 𝑐𝜃2 ) −𝑎2 𝑐𝜃2 0 0𝑟 −𝑎 = ( 𝑠𝜃3 𝑎2 𝑠𝜃2 𝑐𝜃3 𝑎3 𝑠𝜃3 −𝑎2 𝑠𝜃2 𝑐𝜃3 𝑎3 𝑐𝜃2 𝑐𝜃3 ) −𝑎3 𝑐𝜃2 𝑐𝜃3 Tính tốn lực tác động lên khâu: Tọa độ khối tâm: 0𝑟 𝑐1 0𝑟 𝑐2 0𝑟 𝑐3 0 𝑎1 = (0 2) −𝑎1 0 = = 0 𝑎2 𝑠𝜃2 ( ( −𝑎3𝑠𝜃3 −𝑎2𝑠𝜃2 𝑎2 𝑐𝜃2 −𝑎2 𝑐𝜃2 𝑎2𝑠𝜃2𝑐𝜃3 2 𝑎3 𝑠𝜃3 −𝑎3𝑐𝜃2𝑐𝜃3 Khâu cuối: 𝐹𝐸 = [𝐹𝑥 𝐹𝑦 𝐹𝑧 ]T download by : skknchat@gmail.com ) −𝑎2𝑠𝜃2𝑐𝜃3 𝑎3𝑐𝜃2𝑐𝜃3 ) 𝑀𝐸 = [𝑀𝑥 𝑀𝑦 𝑀𝑧 ]T 𝐹𝑥 𝐹3,2 = 0𝐹𝐸 − 0𝑃3 = ( 𝐹𝑦 𝐹𝑦𝑥 ) 00 ) − ( ) = (𝐹𝑧 + 𝑃3 −𝑃3 𝐹𝑧 ∗ 0𝑃3 − 0𝑟23 ∗ 0𝐹3,2 𝑀3,2 = 0𝑀𝐸 − 0𝑟𝑐33 𝑀𝑥 → 0𝑀3,2 = (𝑀𝑦 ) − 𝑀𝑧 = Khâu 2: 𝑀3,2 = 𝐹3,2 = ( ( −𝑎3 𝑠𝜃3 𝑎2𝑠𝜃2𝑐𝜃3 ( 𝑎2 𝑠𝜃2 𝑐𝜃3 𝑎3 𝑐𝜃2𝑐𝜃3 𝑀𝑦 − 𝑎3𝑠𝜃3 −𝑎2 𝑠𝜃2 𝑐𝜃3 −𝑎3 𝑐𝜃2 𝑐𝜃3 2 𝑎3 𝑐𝜃2 𝑐𝜃3 𝑀𝑥 + 𝑃3 − 𝐹𝑦 𝑎3 𝑠𝜃3 +𝑎2 𝑠𝜃2𝑐𝜃3𝐹𝑧 𝑃3 − 𝐹𝑦 𝑎3 𝑠𝜃3 +𝑎2 𝑠𝜃2𝑐𝜃3 𝐹𝑧 𝑎2 𝑠𝜃2 𝑐𝜃3 𝑎3 𝑐𝜃2 𝑐𝜃3 𝑃3 + 𝑎3 𝑠𝜃3𝐹𝑥 − 𝑎3 𝑐𝜃2𝑐𝜃3𝐹𝑧 𝑀𝑧 − 𝑎2 𝑠𝜃2𝑐𝜃3 𝐹𝑥 + 𝑎3 𝑐𝜃2𝑐𝜃3𝐹𝑦 ) 𝐹𝑥 𝐹𝑦 ) 𝐹𝑧 + 𝑃3 𝐹2,1 = 0𝐹3,2 − 0𝑃2 = ( 𝐹𝑥 𝐹𝑥 𝐹𝑦 ) − ( ) = ( 𝐹𝑦 ) −𝑃2 𝐹𝑧 + 𝑃3 + 𝑃2 𝐹𝑧 + 𝑃3 𝑀2,1 = 0𝑀3,2 − 0𝑟𝑐22 ∗ 0𝑃2 − 0𝑟12 ∗ 0𝐹2,1 = = 𝑀𝑥 + 𝑎2 𝑠𝜃2 𝑐𝜃3 𝑎 𝑐𝜃 𝑐𝜃 𝑀𝑦 − 3 ( ( ) 𝑃3 − 𝐹𝑦 𝑎3 𝑠𝜃3 +𝑎2 𝑠𝜃2 𝑐𝜃3 𝐹𝑧 𝑃3 + 𝑎3 𝑠𝜃3𝐹𝑥 − 𝑎3 𝑐𝜃2𝑐𝜃3𝐹𝑧 − 𝑀𝑧 − 𝑎2 𝑠𝜃2 𝑐𝜃3 𝐹𝑥 + 𝑎3 𝑐𝜃2𝑐𝜃3𝐹𝑦 ) 𝑀𝑥 + ( 𝑀𝑦 − ( ( 𝑎2 𝑠𝜃2 0 −𝑎2 𝑐𝜃2 𝑀2,1 = 𝑎2 𝑐𝜃2 𝑀𝑥 + ( 𝑎2 𝑠𝜃2 𝑐𝜃3 𝑎3 𝑐𝜃2𝑐𝜃3 ) 0 ∗ ( ) −( −𝑃2 𝑎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 𝑠𝜃2 +𝑎2 𝑠𝜃2 )𝑃3 − 𝐹𝑦 𝑎3 𝑠𝜃3 +(𝑎2 𝑠𝜃2 𝑐𝜃3 + 𝑎2 𝑠𝜃2 ) 𝐹𝑧 + 𝑎2 𝑠𝜃2 𝑐𝜃3 𝑎3 𝑐𝜃2 𝑐𝜃3 Khâu 1: 𝐹𝑥 −𝑎2 𝑠𝜃2𝑐𝜃3 𝑎3 𝑠𝜃3 𝑎3 𝑐𝜃2 𝑐𝜃3 ) ∗ ( 𝐹𝑦 ) 𝐹𝑧 + 𝑃3 −𝑎3 𝑐𝜃2𝑐𝜃3 ( −𝑎3 𝑠𝜃3 𝑎2 𝑠𝜃2𝑐𝜃3 ∗ ( )− −𝑃3 𝑃3 + 𝑎3 𝑠𝜃3 𝐹𝑥 − 𝑎3 𝑐𝜃2 𝑐𝜃3 𝐹𝑧 𝑀𝑧 − 𝑎2 𝑠𝜃2𝑐𝜃3𝐹𝑥 + 𝑎3 𝑐𝜃2 𝑐𝜃3𝐹𝑦 ) 𝑀𝑦 − 0 ( 𝑀𝑥 + 𝑃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𝑐𝜃3 +𝑎2 𝑠𝜃2)𝐹𝑥 + (𝑎3𝑐𝜃2𝑐𝜃3 + 𝑎2 𝑐𝜃2 )𝐹𝑦 𝐹𝑥 𝐹𝑦 ) 𝐹2,1 = ( 𝐹𝑧 + 𝑃3 + 𝑃2 𝐹𝑥 −𝑎2 𝑠𝜃2 𝐹𝑦 𝑎2 𝑐𝜃2 ) ∗ ( ) −𝑎2 𝑐𝜃2 𝐹𝑧 + 𝑃3 + 𝑃2 𝑃2 𝑎2𝑐𝜃2 ) 𝐹1,0 = 0𝐹2,1 − 0𝑃1 = ( 𝐹𝑥 𝐹𝑥 𝐹𝑦 𝐹𝑦 )− ( )=( ) −𝑃1 𝐹𝑧 + 𝑃3 + 𝑃2 𝐹𝑧 + 𝑃3 + 𝑃2 + 𝑃1 𝑀1,0 = 0𝑀2,1 − 0𝑟𝑐11 ∗ 0𝑃1 − 0𝑟11 ∗ 0𝐹1,0 = download by : skknchat@gmail.com 𝑀𝑥 + ( 𝑀𝑦 − ( ( = 𝑎2 𝑠𝜃2 𝑐𝜃3 𝑎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)𝐹𝑦 𝑀𝑥 + ( 𝑎 𝑐𝜃 𝑐𝜃 𝑀𝑦 − ( 3 ( 𝑎2 𝑠𝜃2 𝑐𝜃3 0 𝑎0 0 1 𝑑10 𝑎01 𝐹𝐹𝑥𝑦 −𝑑 ) − (0 −𝑎1 )− ( ) ∗ ( 𝐹𝑧 + 𝑃3 + 𝑃2 + 𝑃1 ∗ −𝑃 −𝑎1 ) 𝑃2 𝑎2 𝑐𝜃2 2 +𝑎2 𝑠𝜃2)𝑃3 − 𝐹𝑦 (𝑎3 𝑠𝜃3 + 𝑑1) +(𝑎2 𝑠𝜃2𝑐𝜃3 + 𝑎2 𝑠𝜃2)𝐹𝑧 +2 𝑃 𝑎2 𝑠𝜃2 + 𝑎2 𝑐𝜃2 + 𝑎1 ) 𝑃3 + (𝑎3 𝑠𝜃3 + 𝑑1)𝐹𝑥 − (𝑎3 𝑐𝜃2𝑐𝜃3 + 𝑎2 𝑐𝜃2 + 𝑎1 )𝐹𝑧 − 𝑃2 ( +2 𝑎1 ) − 𝑃1 𝑎1 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 𝑚1 𝑎12 12 0 𝑚1 𝑎1 12 ) c2Θ 𝑐2 0 = 𝑚2 𝑎22 12 (0 0 c3Θ 𝑐3 𝑚2 𝑎2 12 ) = 0 (0 𝑚3 𝑎32 12 0 𝑚3 𝑎3 12 ) Các ma trận Jacobi tịnh tiến: Các thành phần q =[ 𝑑1 , 𝜃2 , 𝜃3 ] Khâu 0𝑟 𝑐1 𝑎1 = (0) 𝑑1 Khâu 2: 2𝐴 𝑐2 = → 0𝐴𝑐2 0𝑟 𝑐2 = 3𝐴 𝑐3 0 = (0 0) 0 0𝐴 𝑐𝜃2 𝑠𝜃2 𝑠𝜃 −𝑐𝜃2 = ( 0 0 𝑇1 0 − 0 (0 0 𝑎2 0 2 ) 𝑐𝜃2 𝑠𝜃2 𝑎2𝑐𝜃2 + 𝑎1 𝑎2𝑠𝜃2 ) ∗ = 𝐴2 ∗ 𝐴𝑐2 = ( 𝑠𝜃2 −𝑐𝜃2 𝑑1 0 𝑎2 𝑐𝜃2 + 𝑎1 ( 𝑎2𝑠𝜃2 ) Khâu 3: 0𝐽 → 0𝐽𝑇2 𝑑1 0 − = 0 (0 0 𝑎3 0 1 0 (0 ) 0𝐴 0 − 0 0 𝑎 = ( ) −𝑎2𝑠𝜃2 𝑎2𝑐𝜃2 = 𝑎2 𝑐𝜃2 + 𝑎1 𝑎2 𝑠𝜃2 ) 𝑑1 𝑐𝜃2 𝑠𝜃2 𝑠𝜃2 −𝑐𝜃2 ( 0 ) 0 𝑐𝜃2 𝑐𝜃3 −𝑐𝜃2 𝑠𝜃3 𝑠𝜃 𝑐𝜃 −𝑠𝜃2 𝑠𝜃3 = ( 𝑠𝜃3 𝑐𝜃3 0 0 𝑎2 𝑐𝜃2 + 𝑎1 𝑎2 𝑠𝜃2 𝑑1 ) 𝑠𝜃2 𝑎3 𝑐𝜃2 𝑐𝜃3 + 𝑎2 𝑐𝜃2 + 𝑎1 −𝑐𝜃2 𝑎3 𝑠𝜃2 𝑐𝜃3 + 𝑎2 𝑠𝜃2 ) 𝑑1 + 𝑎3 𝑠𝜃3 download by : skknchat@gmail.com → 0𝐴𝑐3 𝑠𝜃2𝑐𝜃 𝑐𝜃3 −𝑐𝜃 −𝑠𝜃2𝑠𝜃 𝑠𝜃3 −𝑐𝜃 𝑎3 𝑠𝜃 𝑐𝜃3 + 𝑎2 𝑠𝜃2 𝑐𝜃 𝑠𝜃2 𝑎3 𝑐𝜃 3 𝑐𝜃3 + 𝑎2 𝑐𝜃2 + 𝑎1 𝑠𝜃3 𝑐𝜃3 𝑑1 + 𝑎3 𝑠𝜃3 )∗ = 0𝐴3 ∗ 3𝐴𝑐3 = ( 0 𝑎3 𝑐𝜃2𝑐𝜃3 𝑐𝜃2 𝑐𝜃3 −𝑐𝜃2 𝑠𝜃3 𝑠𝜃2 + 𝑎2 𝑐𝜃2 + 𝑎1 = → 0𝑟𝑐3 = ( 𝑠𝜃2 𝑐𝜃3 𝑠𝜃3 𝑎3𝑐𝜃2𝑐𝜃3 ( −𝑠𝜃2 𝑠𝜃3 0 + 𝑎2 𝑐𝜃2 + 𝑎1 𝑎3𝑠𝜃2𝑐𝜃3 𝑐𝜃3 −𝑐𝜃2 𝑑1 + + 𝑎2 𝑠𝜃2 𝑎3 𝑠𝜃3 Các ma trận jacobi quay: 0𝑅 𝑑1 + + 𝑎2 𝑠𝜃2 𝑎3𝑠𝜃3 → 0𝐽𝑇3 = ) Khâu 1: 𝑎3 𝑠𝜃2𝑐𝜃3 0 ( ) −𝑎3 𝑠𝜃2𝑐𝜃3 𝑎3 𝑐𝜃2𝑐𝜃3 + 𝑎2 𝑐𝜃2 0 → c1𝜔 1𝑟 = 0𝑅1𝑇 ∗ 0𝑅󰇗1 = ( 0 0) 0 0 = (0 0) 0 − 𝑎2 𝑠𝜃2 0 (0 0 − 0 0 𝑎 ) −𝑎3𝑐𝜃2𝑠𝜃3 −𝑎3𝑠𝜃2𝑠𝜃3 𝑎3 𝑐𝜃3 ) 0 0 → 𝜔1𝑟 = ( 0) → 𝐽𝑅1 = (0 0) 0 0 Khâu 2: 0𝑅 𝑐𝜃2 𝑠𝜃2 = (𝑠𝜃2 −𝑐𝜃2 ) 𝑐𝜃2 →c2𝜔  2𝑟 = 0𝑅2𝑇 ∗ 0𝑅󰇗2 = ( 𝑠𝜃2 𝑠𝜃2 0 −𝑠𝜃2 𝑐𝜃2 ) ∗ ( 𝑐𝜃2 𝑠𝜃2) ∗ 𝜃󰇗2 = 𝜃󰇗2 ( 0 ) −𝑐𝜃2 −1 0 0 Khâu 𝑠𝜃2 −𝑐𝜃2) 0 0 → 𝜔𝑟2 = (𝜃󰇗2 ) → 𝐽𝑅2 = (0 0) 0 0 0𝑅 𝑐𝜃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 −𝜃󰇗3 𝑐𝜃3 𝜃󰇗2 = ( 𝜃󰇗3 −𝑠𝜃3 𝜃󰇗2 ) −𝑐𝜃3 𝜃󰇗2 𝑠𝜃3 𝜃󰇗2 𝑐𝜃3 𝜃󰇗3 𝑠𝜃2 𝑠𝜃3 𝜃󰇗2 − 𝑐𝜃2 𝑐𝜃3 𝜃󰇗3 −𝑐𝜃2 𝑠𝜃3 𝜃󰇗2 − 𝑠𝜃2 𝑐𝜃3 𝜃󰇗3 −𝑠𝜃3 𝜃󰇗3 𝑠𝜃3 𝜃󰇗 𝑠𝜃3 → 𝜔𝑟3 = (𝑐𝜃3 𝜃󰇗 ) → 𝐽𝑅3 = (0 𝑐𝜃3 )0 0 𝜃󰇗 download by : skknchat@gmail.com 𝑐𝜃2 𝜃󰇗2 𝑠𝜃2 𝜃󰇗2 ) ∑ 𝑀𝑖 (𝑞 ) 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 0 = 𝑚1 ∗ (0 0) ∗ ( 0 0) + ( 0 0) ∗ 0 0 0 0 = 𝑚1 ∗ (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 → 𝑀2 (𝑞) = ( 0 Khâu 3: 0 0 2 0 𝑀3 (𝑞) = 0𝐽𝑇3 𝑇 ∗ 𝑚1 ∗ 0𝐽𝑇3 + 0𝐽𝑅3 𝑇 ∗ 𝐽𝑇3𝑇 ∗ 𝑚3 ∗ 0𝐽𝑇3 = 𝑚3 ∗ ( = 𝑚3 ∗ 0𝐽 𝑅3 𝑇 ∗ Θ𝑐3 ∗ 𝐽𝑅3 c3 0 = (𝑠𝜃3 0 ) ∗ ( 0 𝑐3 −𝑎3 𝑠𝜃2 𝑐𝜃3 − 𝑎2𝑠𝜃2 −𝑎3 𝑐𝜃2 𝑠𝜃3 (− 0 c3Θ ∗ 0𝐽𝑅2 𝑚2 𝑎22 12 (0 )0 𝑐2 𝑎2𝑐𝜃2 −𝑎2 𝑠𝜃2 0 = (0 0) ∗ 0 𝑚2 𝑎22 c2Θ 𝑎3 𝑐𝜃2 𝑐𝜃3 𝑎3 𝑐𝜃3 2 ) + 𝑎2𝑐𝜃2 −𝑎3 𝑠𝜃2 𝑠𝜃3 𝑎 12 (23𝑐𝜃3 + 𝑎2) 0 − 𝑎3 𝑐𝜃3 𝑎23 12 (0 𝑎2 𝑐𝜃2 𝑚2 𝑎2 ∗ 0𝐽𝑅3 −𝑎2 𝑠𝜃2 0 𝑚1 𝑎12 0 𝑚1 𝑎1 12 ) 0 ∗ (0 0) 0 0 𝑎2 )0= 𝑚2 ∗ ( )0 0 0 0 0 0 ∗ (0 0) = ( 𝑚2 𝑎2 ) 12 0 0 0 ) ∗ 𝑎3 𝑐𝜃3 ( −𝑎3 𝑠𝜃2 𝑐𝜃3 − 𝑎2𝑠𝜃2 𝑎3 𝑐𝜃2 𝑐𝜃3 + 𝑎2 𝑐𝜃2 ) −𝑎3 𝑐𝜃2 𝑠𝜃3 −𝑎3 𝑠𝜃2 𝑠𝜃3 𝑎3 𝑐𝜃3 ) 0 0 0 𝑠𝜃3 0 𝑠𝜃3 0 𝑚3 𝑠𝜃3 𝑎3 𝑚3 𝑎32 0 ) ∗ ( 𝑐𝜃 ) ) ∗ ( 𝑐𝜃3 )= ( 12 12 𝑐𝜃3 0) ∗ ( 𝑚3 𝑎3 𝑚3 𝑎3 0 1 0 0 0 12 12 = 𝑚3 ∗ ( 0 𝑠𝜃3 𝑐𝜃3 𝑎32 12 0 ) 𝑎3 12 download by : skknchat@gmail.com MÔ PHỎNG ĐIỀU KHIỂN BẰNG MATLAB SIMULINK download by : skknchat@gmail.com MƠ HÌN HÌNH H2 z0 a1,m1 q2 a2,m2 F q1 x0 O 𝜃𝑖 𝜃2 𝑑𝑖 𝑑1 𝑎𝑖 𝑎1 𝑎2 𝛼𝑖 90° SỐ BẬC TỰ DO CỦA MƠ HÌNH ROBOT: - Cơ cấu gồm n = khâu động k = khớp gồm khớp tịnh tiến khớp quay (một bậc tự do) Bậc tự cấu 𝑓 = 𝜆 ∗ (𝑛 − 𝑘) + ∑𝑓𝑖 + 𝑓𝑒 − 𝑓𝑐 Trong 𝜆 = 3, 𝑛 = 2, 𝑘 = bậc tự chuyển động khớp tịnh tiến khớp quay → 𝑓 = CÁC MA TRẬN D-H: 0A 1 = ( 0 0 𝑎1 −1 ) 𝑑1 0 𝑐𝜃2 → 0𝐴2 = 0𝐴1 ∗ 1𝐴2 = ( 𝑠𝜃2 1A −𝑠𝜃2 𝑎2 𝑐𝜃2 + 𝑎1 −1 ) 𝑐𝜃2 𝑑1 + 𝑎2 𝑠𝜃2 0 𝑐𝜃2 −𝑠𝜃2 𝑎2 𝑐𝜃2 𝑠𝜃2 𝑐𝜃2 𝑎2 𝑠𝜃2 ) = ( 0 0 0 Chuyển động điểm cuối khâu thao tác: 𝑥2 = 𝑦2 = 𝑧2 = 𝑎2 𝑐𝜃2 + 𝑎1 𝑑1 + 𝑎2 𝑠𝜃2 Vận tốc điểm cuối khâu thao tác: download by : skknchat@gmail.com 𝑣3𝑥 𝑣3𝑦 = = 󰇗 𝑎2−𝑎 𝑐𝜃2 𝑠𝜃 𝜃2󰇗 2+𝜃𝑑 󰇗1 𝑣3𝑧 = Góc Cardan: 𝑐𝛽𝑐𝜂 (𝑠𝛼𝑠𝛽𝑐𝜂 + 𝑐𝛼𝑠𝜂 𝑠𝛼𝑠𝜂 − 𝑐𝛼𝑠𝛽𝑐𝜂 → 𝛽= −𝑐𝛽𝑠𝜂 −𝑠𝛼𝑠𝛽𝑠𝜂 + 𝑐𝛼𝑠𝜂 𝑐𝛼𝑠𝛽𝑠𝜂 + 𝑠𝛼𝑐𝜂 𝜂 = 𝜃2 𝑠𝛽 −𝑠𝛼𝑐𝛽 𝑐𝛼𝑐𝛽 𝑐𝜃2 ) = ( 𝑠𝜃2 −𝑠𝜃2 0 −1 ) 𝑐𝜃2 𝛼 = 90° PHƯƠNG TRÌNH VI PHÂN CHUYỂN ĐỘNG CỦA MƠ HÌNH: Khâu 1: 0𝑅 1 0 = (0 −1 ) 0 0 → ω  = 0𝑅󰇗 ∗ 0𝑅1T = (0 0) 0 Từ ta có: 𝜔2𝑥 = 𝜔2𝑦 = → 𝜔2𝑧 = Khâu 2: 0𝑅 1𝑅 𝑐𝜃2 (𝑠𝜃2 Từ ta có: 𝜔3𝑥 = 𝜔3𝑦 = −𝜃2󰇗 𝜔3𝑧 = 𝑎2𝑦 = 𝑎2𝑧 = 𝑐𝜃2 −𝑠𝜃2 0 −1 ) → 0𝑅2 = 0𝑅1 ∗ 1𝑅2 = ( 𝑠𝜃2 𝑐𝜃2 0 = (0 −1 ) 2= 𝑎2𝑥 = −𝑠𝜃2 𝑐𝜃2 ) 0 0 −1 → ω  = 0𝑅󰇗2 ∗ 0𝑅2T = 𝜃2󰇗 ∗ ( 0 ) 0 → 𝑎3𝑥 = 󰇘 𝑎3𝑦 = −𝜃 𝑎3𝑧 = TÍNH TỐN LỰC TÁC D ỤNG LÊN CÁC KHÂU CỦA MƠ HÌNH: Khâu 1: download by : skknchat@gmail.com Khâu 2: Áp dụng hệ phương trình đệ quy tính toán lực khớp 0𝐹 𝑖,𝑖−1 0𝑀 𝑖,𝑖−1 = 0𝐹𝑖+1,𝑖 + 0𝑃𝑖 0𝑀 𝑖+1,𝑖 𝑖 − 0𝑟𝑖−1 ∗ 0𝐹𝑖,𝑖−1 − 0𝑟𝑐𝑖𝑖 ∗ 0𝑃𝑖 𝑖 = 0𝑀𝑖+1,𝑖 − 0𝑟𝑖−1 ∗ 0𝐹𝑖,𝑖−1 − 0𝑟𝑐𝑖𝑖 ∗ 0𝑃𝑖 Các vecto tọa độ khớp biểu diễn hệ tọa độ sở: 0𝑟1 2𝑟 = [−𝑎1 ; 0; −𝑑1 ]T 𝑐𝜃2 = [−𝑎2 ; 0; 0]T → 0𝑟12 = 0𝑅2 ∗ 2𝑟12 = ( 𝑠𝜃2 −𝑠𝜃2 −𝑎2 −𝑎2 𝑐𝜃2 −1 ) ∗ ( ) = ( ) 𝑐𝜃2 0 −𝑎2 𝑠𝜃2 Các vecto khối tâm biểu diễn hệ tọa độ sở: 0𝑟 𝑐1 = [− 𝑎1 ; 0; 0]T download by : skknchat@gmail.com 0𝑟 =[ −𝑎2𝑐𝜃2 𝑐2 ; 0; −𝑎2𝑠𝜃2 T ] Biểu diễn lực hệ tọa độ sở: 0𝑃 0𝑃 = [00 − 𝑃1 ]T 𝐹2 = [𝐹𝑥 𝐹𝑦 𝐹𝑧 ]T = [00−𝑃2 ]T 𝑀2 = [𝑀𝑥 𝑀𝑦 𝑀𝑧 ]T Thiết lập ma trận tọa độ khớp khối tâm: Tọa độ khớp: T ọa độ khối tâm: 0𝑟 0𝑟 0 𝑑1 𝑎1 ) = (−𝑑1 −𝑎1 𝑐1 0 𝑎2 𝑠𝜃2 0𝑟 𝑎2 𝑐𝜃2 ) = (−𝑎2 𝑠𝜃2 −𝑎2 𝑐𝜃2 0𝑟 𝑐2 0 𝑎1 0 = ( ) −𝑎1 0 = Tính tốn lực tác động lên khâu: − ( 𝑎2𝑠𝜃2 𝑎2 𝑠𝜃2 −𝑎2𝑐𝜃2 𝑎2 𝑐𝜃2 ) Khâu cuối; 𝐹𝐸 = [𝐹𝑥 , 0, 𝐹𝑧 ]T 𝑀𝐸 = [0, 𝑀𝑦 , 0]T 0𝐹 2,1 𝐹𝑥 𝐹𝑥 = 0𝐹𝐸 − 0𝑃2 = − = 𝐹𝑧 𝑃2 + 𝐹𝑧 −𝑃2 → 0𝑀2,1 = (𝑀𝑦 ) − − ( Khâu 1: 2,1 0𝐹 2,1 𝑎2𝑠𝜃2 𝑎2 𝑠𝜃2 −𝑎2 𝑐𝜃2 2,1 𝑎2𝑐𝜃2 ) ∗ ( 0) − −𝑃2 = ( 𝑀𝑦 + 𝑃2 𝑎2𝑐𝜃2 + 𝐹𝑥 𝑎2 𝑠𝜃2 − (𝐹𝑧 + 𝑃2 )𝑎2 𝑐𝜃) 2 0𝑀 0𝑀 = 0𝑀𝐸 − 0𝑟𝑐22 ∗ 0𝑃2 − 0𝑟12 ∗ 0𝐹2,1 𝐹𝑥 𝑎2 𝑠𝜃2 0 𝑎2 𝑐𝜃2 ) ∗ ( 𝐹𝑦 ) (−𝑎2 𝑠𝜃2 −𝑎2 𝑐𝜃2 𝐹𝑧 + 𝑃2 0 = (𝑀𝑦 + 𝑃2 𝑎2𝑐𝜃2 + 𝐹𝑥 𝑎2 𝑠𝜃2 − (𝐹𝑧 + 𝑃2 )𝑎2 𝑐𝜃) 2 𝐹𝑥 = ( ) 𝐹𝑧 + 𝑃2 download by : skknchat@gmail.com 0𝐹 1,0 𝐹 0𝑥 = 0𝐹2,1 − 0𝑃1 = ( 0 )− ( + 0𝑟𝑃012 ∗ 0𝐹1,0 −𝑃1 𝑀1,0 = 0𝑀2,1 − 0𝑟𝑐11 ∗ 0𝑃𝐹1𝑧 − = (𝑀𝑦 + 𝑃2 𝐹𝑥 ) ) = (𝐹𝑧 + 𝑃2 + 𝑃1 𝑀𝑥 − 𝐹𝑦 𝑎2 𝑠𝜃2 𝑑1 0 𝑎10 0 0 ) ∗ ( ) − (−𝑑1 + 𝐹𝑥 𝑎2 𝑠𝜃2 − (𝐹𝑧 + 𝑃2 )𝑎2 𝑐𝜃) − ( −𝑎1 −𝑎1 −𝑃1 0 𝑀𝑧 + 𝐹𝑦 𝑎2 𝑐𝜃2 𝑎2 𝑐𝜃2 𝑎1 = (𝑀𝑦 + 𝑃2 𝑎2𝑐𝜃2 + 𝐹𝑥 (𝑎2𝑠𝜃2 + 𝑑1 ) − (𝐹𝑧 + 𝑃2)(𝑎2𝑐𝜃2 + 𝑎1 ) − 𝑃) 2 0 𝐹𝑥 𝑎1) ∗ ( ) 𝐹𝑧 + 𝑃2 + 𝑃1 KHẢO SÁT ĐỘNG HỌC: Các Tensor quán tính: c1Θ = 𝑐1 0 (0 𝑚1 𝑎12 12 0 𝑚1 𝑎1 12 c2Θ 𝑐2 ) = 0 (0 𝑚2 𝑎22 12 0 𝑚2 𝑎2 12 ) Các ma trận Jacobi tịnh tiến: Các thành phần q =[ 𝑑1 , 𝜃2 ] Khâu 𝑎1 𝑟𝑐1 = ( ) 𝑑1 0𝐽 𝑇2 Khâu 2: 2𝐴 0𝐴 0𝑟 𝑐2 𝑐2 = 0 − 0 (0 0 ∗ 2𝐴𝑐2 𝑐𝜃2 = ( 𝑠𝜃2 𝑎2 𝑐𝜃2 = ( 𝑎2 𝑎2 𝑠𝜃2 + 𝑎1 0 ) + 𝑑1 ) −𝑠𝜃2 𝑎2 𝑐𝜃2 + 𝑎1 −1 ) ∗ 𝑐𝜃2 𝑑1 + 𝑎2 𝑠𝜃2 0 Các ma trận jacobi quay: 0 = (0 ) 𝑐𝜃2 −𝑠𝜃2 𝑎2 𝑐𝜃2 + 𝑎1 −1 0𝐴 = ( ) 𝑠𝜃2 𝑐𝜃2 𝑑1 + 𝑎2 𝑠𝜃2 0 1 0 − 0 (0 0 𝑎2 0 ) 𝑐𝜃2 𝑠𝜃2 = 0𝐽 𝑇2 ( −𝑠𝜃2 0 −1 𝑐𝜃2 0 0 − 2 ) = (0 𝑎2 𝑐𝜃2 𝑎 𝑠𝜃 Khâu 1: download by : skknchat@gmail.com 𝑎2 𝑐𝜃2 𝑎2 𝑠𝜃2 + 𝑎1 + 𝑑1 ) 0𝑅 01 00 −10 ) = ( 1 c1𝜔 𝑟 0 = 0𝑅1𝑇 ∗ 0𝑅󰇗1 = (0 0) 0 0 0 𝜔1𝑟 = (0) → 𝐽𝑅1 = (0 0) 0 Khâu 2: 0𝑅 𝑐𝜃2 = (0 𝑠𝜃2 c2𝜔 𝑟 −𝑠𝜃2 0 −1 ) 𝑐𝜃2 𝑐𝜃2 = 0𝑅2𝑇 ∗ 0𝑅󰇗2 = (−𝑠𝜃2 0 0 𝜔𝑟2 = ( ) → 𝐽𝑅2 = (0 0) 𝜃󰇗2 −𝑠𝜃2 𝑠𝜃2 𝑐𝜃2 ) ∗ ( 𝑐𝜃2 −1 −𝑐𝜃2 0 −1 0 ) ∗ 𝜃󰇗2 = 𝜃󰇗2 (1 0) −𝑠𝜃2 0 0 ∑ 𝑀𝑖 (𝑞 ) Ma trận khối lượng: 𝑀 (𝑞) = 3𝑖=1 Khâu 1: 𝑀1 (𝑞) = 0𝐽𝑇1 𝑇 ∗ 𝑚2 ∗ 0𝐽𝑇1 + 0𝐽𝑅1 𝑇 ∗ c1Θ 𝑐1 ∗ 0𝐽𝑅1 0 0 0 0 1) ∗ ( = 𝑚1 ∗ ( 0) + ( 0 0) ∗ 0 0 0 = 𝑚1 ∗ ( ) 0 Khâu 2: 𝑀2 (𝑞) = 0𝐽𝑇2 𝑇 ∗ 𝑚2 ∗ 0𝐽𝑇2 + 0𝐽𝑅2 𝑇 ∗ 0𝐽 𝑇2 0𝐽 𝑅2 𝑇 𝑇 ∗ 𝑚2 ∗ 0𝐽𝑇2 = 𝑚2 ∗ ( 𝑎2 𝑠𝜃2 − ∗ c2Θ 𝑐2 0 ∗ 0𝐽𝑅2 = ( ) ∗ 0 1 → 𝑀2 (𝑞) = 𝑚2 ∗ 𝑎(𝑐𝜃 2 0 𝑐2 (0 ∗ 0𝐽𝑅2 12 0 𝑚2 𝑑2 12 ) 0 ∗ (0 0) 0 0 − 22 𝑎2 𝑐𝜃2 ) ∗ (0 ) = 𝑚2 ∗ ( 𝑎2 𝑐𝜃2 𝑎2𝑐𝜃2 2 0 (0 𝑚2 𝑎22 12 𝑎 𝑠𝜃 0 𝑚2 𝑎2 12 2 ) 𝑎2𝑐𝜃2 𝑎22 0 0 ∗ (0 0) = ( 𝑚2 𝑎2) 12 0 ) + 𝑚2 ∗ ( 𝑎22 ) = 𝑚 ∗ ( 𝑎 𝑐𝜃 12 2 𝑎2 𝑐𝜃2 𝑎22 c2Θ 0 𝑚2 𝑑22 ) 𝑎2 𝑐𝜃2 𝑎22 ) download by : skknchat@gmail.com 1𝑎2𝑐𝜃2 𝑀(𝑞) = 𝑀1 (𝑞) + 𝑀2 (𝑞 ) = 𝑚1 ∗ ( ) + 𝑚2 ∗ ( 0 Lực quán tính coriolis lực quán tính li tâm: 𝑐1 𝐶(𝑞, 𝑞󰇗 )𝑞󰇗 =𝑐 𝑐1 = ∑ 2𝑘,𝑙=1 (𝑘; 𝑙; 1)𝑞󰇗 𝑘 𝑞󰇗 𝑙 (1,1,1) = + 𝜕𝑚11 𝜕𝑚11 𝜕𝑞1 (1,2,1) = + 𝜕𝑚11 𝜕𝑚21 𝜕𝑞2 (2,1,1) = 𝑐1 = − 𝜕𝑞1 + 𝜕𝑚21 𝜕𝑚11 𝜕𝑞1 (2,2,1) = 𝜕𝑞1 𝜕𝑞2 + 𝜕𝑚21 𝜕𝑚21 𝜕𝑞2 𝑚2𝑠𝜃2 𝜕𝑞2 − − − − ∗ 𝑞󰇗 22 = − 𝜕𝑚11 𝜕𝑞1 𝜕𝑚12 𝜕𝑞1 𝜕𝑚21 𝜕𝑞1 𝜕𝑚22 𝜕𝑞1 𝑚2 𝑠𝜃2 Tính lực thế: 𝑎2 𝑐𝜃22 𝑎2 𝑚21 + 𝑚2 )= ( 𝑎 𝑎𝑐𝜃 𝑚𝑚2 2 2 ) 𝑎2 𝑐𝜃2 3 𝑘,𝑙=1 (𝑘; 𝑙; 2)𝑞󰇗 𝑘 𝑞󰇗 𝑙 𝑐2 = ∑ (1,1,2) = 𝜕𝑞 12+ = (1,2,2) = = (2,1,2) = = 𝜕𝑚 ∗ 𝜃󰇗22 𝑐2 = − 𝜕𝑞1 + 𝜕𝑚12 𝜕𝑞2 + 𝜕𝑚22 𝜕𝑞1 (2,2,2) =𝜕𝑞 22+ = −𝑚2 𝑠𝜃2 𝜕𝑚12 𝜕𝑚 𝑚2𝑠𝜃2 2 𝜕𝑚22 𝜕𝑞1 𝜕𝑚12 𝜕𝑞2 𝜕𝑚22 𝜕𝑞2 − 𝜕𝑚11 − 𝜕𝑚21 − − ∗ 𝑞󰇗 22 = − 𝜕𝑞2 𝜕𝑚12 𝜕𝑞2 𝜕𝑞2 𝜕𝑚22 𝜕𝑞2 𝑚2 𝑠𝜃2 Π = Π1 +Π2 Π1 = 𝑚1 𝑔𝑑1 Π2 = 𝑚 ∗ 𝑔 ∗ ( 𝑎2 𝑠𝜃2 + 𝑑1 ) Π = (𝑚1 + 𝑚2 )𝑔𝑑1 + 𝑚2 𝑔 𝜕Π 𝐺1 = 𝜕𝑞 = 𝐺2 = 𝜕Π 𝜕𝑞2 = 𝜕Π 𝜕𝑑1 𝜕Π 𝜕𝜃2 𝑎2 𝑠𝜃2 = (𝑚1 + 𝑚2 )𝑔 = 𝑚2𝑔 𝑎2𝑐𝜃2 Tính lực suy rộng lực khơng thế: − 2 0𝐽 ) 𝑇2 = ( 𝑎2𝑐𝜃2 𝑎 𝑠𝜃 0𝐽 𝑅2 0 = 0 𝑇 = ( → 𝐽𝑇2 𝑇1 𝑄1 = 𝐽𝑇1 𝑇2𝐹2 + 𝐽𝑅2 𝑀2 = 0 ∗ 𝐹𝑥 + 0 ∗ 𝐹𝑧 − 𝑎2 𝑠𝜃2 → 𝐽𝑅2𝑇 = 0 0 0 𝑎2𝑐𝜃2 ) 𝑀𝑦 = 𝐹𝑧 𝐹𝑥 𝑎 𝑠𝜃 ∗ + 0 ∗ 𝑀𝑦 = 𝐹𝑧 𝑎2𝑐𝜃2 − 𝐹𝑥 2 2 𝐹𝑧 Lập phương trình vi phân chuyển động mơ hình robot: 𝑎 𝑠𝜃 𝑇2 𝑇2 𝑄2 = 𝐽𝑇2 𝐹2 + 𝐽𝑅2 𝑀2 = − 2 𝑎2 𝑐𝜃2 download by : skknchat@gmail.com = = = = −𝑚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 đường thẳng không gian thao tác: Phương trình đường thẳng từ 𝐴(𝑥0 , 𝑦0 ) đến 𝐵(𝑥𝑒 , 𝑦𝑒 ): 𝑥−𝑥0 𝑥𝑒 −𝑥0 = 𝑦−𝑦0 𝑦𝑒 −𝑦0 Thỏa mãn Tại 𝐴: (𝑡0 , 𝑥0 , 𝑦0 , 𝑥󰇗0 , 𝑦󰇗 ) Tại 𝐵: (𝑡𝑒 , 𝑥𝑒 , 𝑦𝑒 , 𝑥󰇗 𝑒 , 𝑦󰇗𝑒 ) Khi để thỏa mãn quan hệ vận tốc đầu cuối ta chọn phương án dịch chuyển cho 𝑥, 𝑦, 𝑧 la đa thức bậc thời gian 𝑇𝑎 𝑐ó: 𝑥(𝑡) = 𝑎10 + 𝑎11 𝑡 + 𝑎12 𝑡 + 𝑎13 𝑡 𝑥󰇗 (𝑡) = 𝑎11 + 2𝑎12 𝑡 + 3𝑎13𝑡 𝑥(𝑡0 ) = 𝑎10 + 𝑎11𝑡0 + 𝑎12 𝑡0 + 𝑎13 𝑡0 = 𝑥0 𝑥󰇗 (𝑡0 ) = 𝑎11 + 2𝑎12𝑡0 + 3𝑎13 𝑡0 = 𝑥󰇗 𝑦(𝑡) = 𝑎20 + 𝑎21 𝑡 + 𝑎22 𝑡 + 𝑎23 𝑡 Tìm tham số 𝑥(𝑡) theo điều kiện: 𝑥(𝑡𝑒 ) = 𝑎10 + 𝑎11𝑡𝑒 + 𝑎12𝑡𝑒 + 𝑎13 𝑡𝑒 = 𝑥𝑒 Viết lại hệ phương trình dạng ma trận : 1 → 0 ( 𝑦󰇗 (𝑡) = 𝑎21 + 2𝑎22 𝑡 + 3𝑎23 𝑡 𝑥󰇗 (𝑡𝑒 ) = 𝑎11 + 2𝑎12 𝑡𝑒 + 3𝑎13 𝑡𝑒 = 𝑥󰇗 𝑒 𝑡0 𝑡0 𝑡0 𝑎10 𝑥0 𝑎11 𝑥𝑒 𝑡𝑒 𝑡𝑒 𝑡𝑒 ∗ (𝑎 ) = (𝑥󰇗 ) ↔ 𝐴 ∗ 𝑎 = 𝑋 12 2𝑡0 3𝑡0 𝑥󰇗𝑒 𝑎13 2𝑡𝑒 3𝑡𝑒 ) Ma trận mở rộng ma trận 𝐴: download by : skknchat@gmail.com 1 𝐴󰆹 = 0 ( 𝑡0 𝑡0 𝑡0 𝑥0 𝑡𝑒 𝑡𝑒 𝑡𝑒 𝑥𝑒 2𝑡0 3𝑡0 𝑥󰇗0 2𝑡𝑒 3𝑡𝑒 𝑥󰇗 𝑒 𝑡0 𝑡0 → (0 𝑡0 + 𝑡𝑒 0 0 ↔ 𝐴󰆹 = ) Ta có: 𝑥−𝑥0 𝑥𝑒 −𝑥0 = ( 0 13 𝑥0 𝑥𝑒 −𝑥0 𝑡𝑒 −𝑡0 3𝑡𝑒 +2𝑡 0) −2𝑡 𝑒 +𝑡0) 𝑥󰇗0 (𝑡 )2 + (𝑥𝑒 − 𝑥0 ) − (2𝑡0 (𝑡𝑒 −𝑡0)3 𝑒 −𝑡0 𝑥󰇗 𝑒 +𝑥󰇗 2(𝑥𝑒 −𝑥0 ) 𝑦−𝑦0 𝑦𝑒 −𝑦0 (𝑡𝑒 −𝑡0)2 → 𝑦 = 𝑦0 + − (𝑡𝑒 −𝑡0)3 𝑦𝑒 −𝑦0 𝑥𝑒 −𝑥0 𝑡0 𝑡0 + 𝑡0 𝑡𝑒 + 𝑡𝑒 (0 𝑡0 𝑎𝑎 1011 𝑡0 + 𝑡0 𝑡𝑒 + 𝑡𝑒 )= (2𝑡0 + 𝑡𝑒 ) ) ∗ ( 𝑎12 𝑎 𝑇ừ 𝑔𝑖ả𝑖 𝑛𝑔ượ𝑐 ℎệ để 𝑡ì𝑚 𝑐á𝑐 𝑡ℎ𝑎𝑚 𝑠ố: 𝑎10 𝑎11 (𝑎 ) = 12 𝑎13 𝑡0 𝑡0 𝑡0 + 𝑡𝑒 𝑥󰇗 𝑡0−𝑡𝑒 𝑥󰇗 𝑒 +𝑥󰇗 (2𝑡0 + 𝑡𝑒 ) −(𝑡0+𝑡𝑒 ) 𝑥0 𝑥𝑒 −𝑥0 𝑥 𝑥𝑒 −𝑥00 𝑡𝑒 −𝑡0 𝑥󰇗 𝑥𝑒 −𝑥0 𝑡0−𝑡𝑒 𝑥󰇗 𝑒 −𝑥󰇗 (𝑡𝑒 −𝑡0)2 𝑥󰇗 𝑥 −𝑥 − − (𝑡𝑒 −𝑡0)2) 2(𝑡𝑒 −𝑡0 ) 𝑡0−𝑡𝑒 𝑒 𝑡𝑒 −𝑡0 𝑥𝑒 −𝑥0 + (𝑡𝑒 −𝑡0)2 2(𝑥𝑒 −𝑥0) ((𝑡𝑒−𝑡0)2 − (𝑡𝑒−𝑡0)3) + 𝑡𝑒 ) 𝑥󰇗 𝑒 (𝑡𝑒 −𝑡0)2 + (c ) (𝑥 − 𝑥0 ) THIẾT KẾ MƠ HÌNH ĐIỀU KHIỂN ROBOT THEO LUẬT ĐIỀU KHIỂN PD: Sơ đồ khối hệ thống điều khiển động lực học ngược vịng ngồi tuyến tín PD: 𝑞 = [𝑑1 ; 𝜃2 ]𝑇 𝑀(𝑞)𝑞󰇘 + 𝐶 (𝑞, 𝑞󰇗 )𝑞󰇗 + 𝐺(𝑞) + 𝑄 = 𝑈 MÔ PHỎNG SIMULINK: download by : skknchat@gmail.com download by : skknchat@gmail.com Code RobotDynamic: function ddq = RobotDynamic(t) %Các tham số động học m1 = 15; m2 = 7; a1 = 0.5; a2 = 0.5; g =9.81; Fx = 10; Fz = 10; My = 10; %Vecto tọa độ suy rộng (tọa độ khớp) q = t(1:2); %Vecto dao ham tọa độ suy rộng (tọa độ khớp) dq = t(3:4); %Vecto lực suy rộng ứng với tọa độ suy rộng(tọa độ khớp) U = t(5:6); %Load = t(10); %m3 = load + m3; %Ma m11 m12 m21 m22 M = trận khối lượng = m1 + m2; = m2*a2*cos(q(2))/2; = m2*a2*cos(q(2))/2; = m2*a2*a2/3; [m11,m12;m21,m22]; %Lực quán tính coriolis quán tính ly tâm psi1= -m2*sin(q(2))*dq(2)/2; psi2= -m2*sin(q(2))*dq(2)/2; Psi = [psi1;psi2]; %Lực suy rộng lực g1 = (m1 + m2)*g; download by : skknchat@gmail.com g2 = m2*g*a2*cos(q(2))/2; G =[g1;g2]; %Lực suy rộng lực không Q1 = Fz; Q2 = Fz*a2*cos(q(2))/2-Fx*a2*sin(q(2))/2; Q = [Q1;Q2]; %Gia tốc suy rộng ddq = inv(M)*([U(1);U(2)]-Psi-G-Q); -Code Inversemodel: function T = InverseModel(t) %Các tham số động học m1 = 15; m2 = 7; a1 = 0.5; a2 = 0.5; g =9.81; Fx = 10; Fz = 10; My = 10; %Vecto tọa độ suy rộng (tọa độ khớp) q = t(1:2); %Vecto dao ham tọa độ suy rộng (tọa độ khớp) dq = t(3:4); %Vecto lực suy rộng ứng với tọa độ suy rộng(tọa độ khớp) u = t(5:6); %Load = t(10); %m3 = load + m3; %Ma m11 m12 m21 m22 M = trận khối lượng = m1 + m2; = m2*a2*cos(q(2))/2; = m2*a2*cos(q(2))/2; = m2*a2*a2/3; [m11,m12;m21,m22]; %Lực quán tính coriolis quán tính ly tâm psi1= -m2*sin(q(2))*dq(2)/2; psi2= -m2*sin(q(2))*dq(2)/2; Psi = [psi1;psi2]; %Lực suy rộng cuae lự g1 = (m1 + m2)*g; g2 = m2*g*a2*cos(q(2))/2; download by : skknchat@gmail.com G =[g1;g2]; %Lực suy rộng lực không Q1 = Fz; Q2 = Fz*a2*cos(q(2))/2-Fx*a2*sin(q(2))/2; Q = [Q1;Q2]; %Lực dẫn động khớp T = M*[u(1);u(2)]+Psi+G+Q; Kết mô phỏng: Với tham số động học đầu vào: m1 = 15; m2 = 7; a1 = 0.5; a2 = 0.5; g =9.81; Fx = 10; Fz = 10; My = 10; download by : skknchat@gmail.com download by : skknchat@gmail.com ... =