CHƯƠNG II. GIỚI THIỆU VÀ TÌM HIỂU VỀ ROBOT SCADA

CHƯƠNG II.
GIỚI THIỆU VÀ TÌM HIỂU VỀ ROBOT SCADA
2.1 Giới thiệu chung về động học Robot.
Hệ trục tọa độ và bảng D-H cho 4 khớp của tay máy
Qui ước về cách tr
ình bày như sau:
Cos(θ
23
)=Cosθ
2
.Cosθ
3
-Sinθ
2
.Sinθ
3
Sin(θ
23
)=Sinθ
2
.Cosθ
3
+Cosθ
2
.Sinθ
3
Cos(θ
234
)=Cos(θ
23
).Cosθ

4
– Sin(θ
23
).Sin θ
4
Sin(θ
234
)=Sin(θ
23
).Cosθ
4
+ Cos(θ
23
).Sin θ
4
Ma trận A
i-1
i
mô tả hướng và vị trí của khâu thứ i so với khâu
thứ i-1 (theo mô hình D-H):
Thay thông số 4 khớp vào ma trận chuyển đổi đồng nhất,
Từ 4 ma trận chuyển đổi đồng nhất trên ta tìm ra ma trận
chuyển vị từ hệ tọa độ cuối về hệ tọa độ chuẩn như sau:
Giải xong phần động học thuận cho tay máy. Một khi ma trận
T
0
4
được nhập bởi người dùng thì các thông số trong ma trận T
0
4

là
n
x
, n
y
,…p
z
đã biết. Việc giải bài toán động học ngược về vị trí là
tìm b
ốn thông số khớp θ
2
, θ
3
và θ
4
bằng cách dựa vào ma trận T
0
4
đã biết
2.2. Động học ngược về vị trí cho tay máy .
Đồng nhất ma trận tọa độ điểm Pc = (Px, Py, Pz)
-1
với tọa độ
điểm cuối tay máy
(xc, yc, zc) thì ta có hệ 3 phương trình sau:
Từ (1) va (2) ta tìm ra Tang(θ
1
) = y
c
/x

c
, suy ra θ
1
=
arctang(y
c
/x
c
) (4)
Và ma trận xoay tìm được từ kết cấu của tay máy là:
Đồng nhất ma trận chuyển đổi đồng nhất T
0
4
với ma trận
xoay R để t
ìm mối liên hệ về mặt đại số giữa θ
2
, θ
3
và θ
4
như sau:
Để tay máy hoạt động thì khâu cuối phải có phương thẳng
đứng v
à có chiều hướng xuống mặt phẳng bàn. Vì thế điểm cuối
tay máy có tọa độ so với O
1
là (x
4
, z

4
-d
5
)
Vì O
3
trùng O
4
chiếu theo MP (O
1
x
1
z
1
). Nên vị trí của hệ tọa
độ O
3
so với O
1
là (x
4
, z
4
-d
5
+d
4
).
M
ột khi đã có vị trí hệ tọa độ O

3
so với O
1
thì bây giờ ta giải
nghiệm θ
2
và θ
3
riêng từ khớp thứ hệ tọa độ O
1
đến O
3
để đơn
giản.
Hệ trục tọa độ và bảng D-H cho khớp 2, khớp 3 và khớp 4
của tay máy
Ma trận chuyển đổi tọa độ từ khớp 3 về khớp 2 và từ khớp 4
về khớp 3 là:
Từ 2 ma trận chuyển đổi đồng nhất vừa trên ta tìm ra ma trận
chuyển vị từ hệ tọa độ thứ III về hệ tọa độ thứ I như sau:
Đồng nhất vector vị trí trong ma trận A
2
3
với tọa độ O
3
(x
3
,
y
3

), ta có được:
Tổng bình phương 2 vế của hệ phương trình trên,
Ta có được: (d
3
Cos(θ
23
)+d
2
Cos(θ
2
))
2
+
(d
3
Sin(θ
23
)+d
2
Sin(θ
2
))
2
= x
3
2
+y
3
2
Tương đương với: d

3
2
(Cos(θ
23
)
2
+ Sin(θ
23
)
2
) +
d
2
2
(Cos(θ
2
)
2
+ Sin(θ
2
)
2
) + 2 d
2
d
3
(Cos(θ
23
) Cos(θ
2

)+ Sin(θ
23
)
Sin(θ
2
)) = x
3
2
+y
3
2
Rút gọn biểu thức lại: d
2
2
+d
3
2
+2 d
2
d
3
Cos(θ
2
+ θ
3
– θ
2
) =
x
3

2
+y
3
2
Tách các phần tử khác Cos(θ
3
) sang vế phải thì ta có:
Cos(θ
3
) = (( x
3
2
+y
3
2
) - ( d
2
2
+d
3
2
)) / 2d
2
d
3
(a)
Ở đây θ
3
có 2 nghiệm tương ứng với 2 cấu hình của tay máy có
thể thõa mãn yêu cầu. Vì kết cấu của tay máy nên nghiệm θ

3
âm được chọn.
Vì thế nghiệm θ
3
là: θ
3
= - Arcos[(( x
3
2
+y
3
2
) - ( d
2
2
+d
3
2
)) /
2d
2
d
3
] (b)
Thay (a) vào (1), ta tìm ra θ2 như sau:
Biểu thức (1) được viết lại : d
3
Cos(θ
23
) + d

2
Cos(θ
2
) = x
3