T 6= (2.2) ổng quát, ma trận 6 có thể biểu diễn gọn hơn như sau :
3.2.3.Động học ngược của Scorbot ER-4u
Bài toán động học ngược được xây dựng để tính góc quay của các khớp khi biết toạ độ điểm cuối và hướng dịch chuyển của robot. Đối với Scorbot ER-4u ta có 5 thông số trong toạ độ Đề-các-tơ là x, y, z, roll (β), pitch (γ). Để xác định gốc quay của các khớp cần phải xây dựng ma trận chuyển đổi từ 5 thông số trong toạ độ Đề- các-tơ.
Ma trận quay được tạo ra chỉ phụ thuộc vào roll, pitch và yaw của cánh tay robot.
Đối với Scorbot ER-4u không có yaw, nhưng có sự quay của khớp 1. Do đó tính toán cho yaw như sau:
α = θ1 = atan2(x,y) (3.16)
Đầu tiên ta quay khớp K5 đang ở {5} một góc -β quanh trục của nó, sau đó quay khớp ở vị trí mới {5’} một góc γ quanh trục nguyên lý y’ của nó, sau cùng ta quay khớp này ở vị trí mới {5’’} một góc α quanh trục nguyên lý z’’ của nó.
Ma trận quay = Rx(-β)*Ry(γ)*Rz(α)
=
= (3.17)
Ta xoay ma trận một góc 900 quanh trục y:
Ry(-900)= = (3.18)
Nhân phương trình (3.18) với (3.17), ta có ma trận quay:
= (3.19)
T = (3.20)So sánh ma trận chuyển đổi trong biểu thức (3.7) với biểu thức (3.17) ta suy ra: So sánh ma trận chuyển đổi trong biểu thức (3.7) với biểu thức (3.17) ta suy ra:
θ1 = α ; θ234 = β ; θ5 =y (3.21)
Để tìm θ2, θ3, θ4 ta dùng phương pháp hình học như hình 3.10
Hình 3.11: Động học ngược bằng phương pháp hình học Để tìm θ2, θ3 và θ4 ta có tọa độ X, Y, Z trong không gian Đề-các-tơ
X1 = và Y1 = Z
Ta có pitch của cơ cấu chấp hành cuối θ234 = β, từ đó ta tính được điểm X2, Y2
như sau: X2 = X1 – d5cosθ234 Y2 = Y1 + d5sinθ234 Khoảng cách X3 và Y3 có thể được tính bằng cách: X3 = X2 – a1 Y3 = Y2
Áp dụng định lí hàm số cos cho tam giác ABC ta tính được: cosθ3 = (X32 + Y32 – a22 – a32) / 2a2a3
θ3 = a tan2(± , cosθ3) (3.22) Từ hình 3.10 ta tính được θ2 = -ϕ – δ hay
θ2 = -a tan2(Y3 , X3) – a tan2(a3sinθ3 , a2 + cosθ3) (3.23) Cuối cùng ta có được:
θ4 = θ234 – θ2 - θ3 (3.24)
Các biểu thức trên cho động học ngược của Scorbot ER-4u sẽ được sử dụng để tính toán các góc quay theo toạ độ điểm cuối và hướng quay của robot, sử dụng cho lập trình điều khiển và mô phỏng Scorbot ER-4u.
Chương 4: Thiết kế chế tạo bộ điều khiển cho Scorbot ER-4u 4.1. Giới thiệu bộ điều khiển lập trình CJ1M của hãng Omron
Hình 4.1: Giới thiệu bộ điều khiển lập trình CJ1M của hãng Omron