CHƯƠNG 3 : PHÂN TÍCH ĐỘNG LỰC HỌC
3.2 Xây dựng mơ hình tốn học
3.2.2 Bài toán động học thuận
Bài toán động học thuận (forward kinematics) giúp ta xác định vị trí của điểm cơng tác khi biết giá trị của các khớp quay 𝜃1𝑖 của các khớp.
Hình 3.7: Sơ đồ bài tốn động học thuận
Có nhiều cách để giải bài tốn động học: hình học giải tích, vector, lập bảng tọa độ D-H, … Phương pháp dùng bảng tọa độ D-H thường được dùng để giải bài tốn động học nhưng khơng phải là phương pháp đặc trưng để giải động học cho robot song song. Bảng tọa độ D-H là phương pháp đặc trưng dùng cho robot nối tiếp. Trong chương này, tác giả dùng phương pháp hình học giải tích kết hợp với phép tốn Trilateration để giải bài toán động học thuận cho robot delta 3 bậc tự do. Phương pháp giải này nhằm rút ngắn quá trình giải nghiệm cho phương trình động học thuận, giảm cơng sức tính tốn so với các phương pháp rút thế hay đặt biến phụ truyền thống.
Đầu tiên, ta tính động học thuận của delta robot dựa vào 3 điểm 𝐵𝑖′ là tâm của 3 hình cầu bán kính 𝐿𝑏, 3 hình cầu này sẽ giao nhau tại 2 điểm, 1 điểm với z dương và 1 điểm với z có giá trị âm.
Hệ trục toạ độ {𝑅} có trục z dương hướng lên, do đó ta có tọa độ điểm giao nhau tại P có giá trị z âm.
𝑖
B’2 B’1
B’3
Điểm giao nhau
Hình 3.8: Ba hình cầu giao nhau tại hai điểm
Theo hình 3.8, tọa độ điểm cơng tác là điểm giao nhau giữa 3 hình cầu tâm 𝐵𝑖′ trong đó hai hình cầu giao nhau tạo thành 1 đường trịn, đường trịn đó giao với hình cầu cịn lại tại 2 điểm. Ta có, tọa độ điểm 𝐵𝑖′của mỗi cánh tay như sau:
𝐵𝑖′= [(𝑟 + 𝐿𝑎 cos(𝜃𝑖1)) 0 𝐿𝑎 sin(𝜃𝑖1)]𝑇
(3.3) Để chuyển tọa độ điểm 𝐵𝑖′ về Base frame {𝑅} ta nhân với ma trận quay 𝑅𝑅𝑖𝑧 với 𝐵′, ta được: 𝑐𝑜𝑠𝛼𝑖 – 𝑠𝑖𝑛𝛼𝑖 0 (𝑟 + 𝐿𝑎 cos(𝜃𝑖1) ) 𝐵′𝑅 i =𝑅 i𝑅𝑧 ∗ 𝐵𝑖′ = [ 𝑠𝑖𝑛𝛼𝑖 𝑐𝑜𝑠𝛼𝑖 0] [ 0 ] 0 0 1 𝐿𝑎 sin(𝜃𝑖1) 𝑐𝑜𝑠𝛼𝑖(𝑟 + 𝐿𝑎 cos(𝜃𝑖1)) = [𝑠𝑖𝑛𝛼𝑖(𝑟 + 𝐿𝑎 cos(𝜃𝑖1))] 𝐿𝑎 sin(𝜃𝑖1) (3.4)
𝑖
Ba hình cầu được tạo ra với bán kính bằng nhau là 𝐿𝑏 và có tâm là 𝐵′.
Phương trình hình cầu: (𝑥 − 𝑥0)2 + (𝑦 − 𝑦0)2 + (𝑧 − 𝑧0)2 = 𝑟2 (3.5) Thay vào ta tìm được hệ tọa độ tâm P của hệ trục tọa độ {P}:
(𝑥 − cos 𝛼1(𝑟 + 𝐿𝑎 cos 𝜃1))2 + (𝑦 − sin 𝛼1(𝑟 + 𝐿𝑎 cos 𝜃1)) + (𝑧 − 𝐿𝑎 sin 𝜃1)2 = 𝐿b 2
(𝑥 − cos 𝛼2 (𝑟 + 𝐿 𝑎 cos 𝜃2 ))2 + (𝑦 − sin 𝛼2 (𝑟 + 𝐿 𝑎 cos 𝜃2))2
+ (𝑧 − 𝐿𝑎 sin 𝜃2)2 = 𝐿b 2
(𝑥 − cos 𝛼3(𝑟 + 𝐿𝑎 cos 𝜃3))2 + (𝑦 − sin 𝛼3(𝑟 + 𝐿𝑎 cos 𝜃3))2 + (𝑧 − 𝐿𝑎 sin 𝜃3)2 = 𝐿b 2
(3.6)
Với giá trị các góc (𝛼1; 𝛼2; 𝛼3) = (00, 1200, 2400)
Tiếp theo, ta đi giải phương trình (3.6) theo biến x, y, z để tìm ra nghiệm cho tọa độ điểm cuối P(x,y,z) của đầu cơng tác khi các góc 𝜃1, 𝜃2, 𝜃3 thay đổi.