Inverse Kinematics SCARA

1 Tính toán

Quỹ đạo điểm cuối

x(t) = a + r cos ωt
y(t) = b + r sin ωt (1)

trong đó (a, b) là tọa độ tâm vòng tròn.

Góc đo trong thí nghiệm là

!
y(t)
θS (t) = arctan − θQ
x(t)
! !
y(t) x2(t) + y 2 (t) + L21 − L22
= arctan − arccos p (2)
x(t) 2L1 x2(t) + y 2 (t)

Đặt
y(t)
φ1 (t) =
x(t)
x2(t) + y 2 (t) + L21 − L22
φ2 (t) = p (3)
2L1 x2 (t) + y 2 (t)

Ngày 23 tháng 12 năm 2017

Tính

ẏ(t)x(t) − ẋ(t)y(t)
φ̇1 (t) =
x2(t)
(ÿ(t)x(t) − ẍ(t)y(t))x(t) − 2ẋ(t)(ẏ(t)x(t) − ẋ(t)y(t))
φ̈1 (t) = (4)
x3(t)

Biểu diễn

1 L21 − L22 1
q
φ2 (t) = x (t) + y (t) +
2 2 p (5)
2L1 2L1 x2(t) + y 2 (t)

Tính

1 x(t)ẋ(t) + y(t)ẏ(t) L21 − L22 2 3

φ̇2 (t) = p − (x (t) + y 2 (t))− 2 (x(t)ẋ(t) + y(t)ẏ(t))
2L1 x2(t) + y 2 (t) 2L1
1 
2 2 − 21 2 2 2 2 − 23

= (x (t) + y (t)) − (L1 − L2 )(x (t) + y (t)) (x(t)ẋ(t) + y(t)ẏ(t))
2L1
(6)

Do đó
!
1 3(L21 − L22 ) (x(t)ẋ(t) + y(t)ẏ(t))2
φ̈2 (t) = −1 3
2L1 x2(t) + y 2 (t) (x2(t) + y 2 (t)) 2
!
2 2
1 L1 − L2 ẋ2(t) + ẏ 2 (t) + x(t)ẍ(t) + y(t)ÿ(t)
+ 1− 2 p (7)
2L1 x (t) + y 2 (t) x2(t) + y 2 (t)

Công thức

d 1
arctan x =
dx 1 + x2
d 1
arccos x = − √ (8)
dx 1 − x2

Vận tốc góc

φ̇1 (t) φ̇2(t)

θ̇S (t) = + (9)
1 + φ21 (t)
p
1 − φ22 (t)

2
Gia tốc góc

φ̈1 (t)(1 + φ21 (t)) − 2φ̇21 (t)φ1(t)

θ̈S (t) =
(1 + φ21 (t))2
p −2φ2(t)φ̇2(t)
φ̈2 (t) 1 − φ22 (t) − p
2 1 − φ22 (t)
+
1 − φ22 (t)
φ̈1 (t)(1 + φ21 (t)) − 2φ̇21 (t)φ1(t) φ̈2 (t)(1 − φ22 (t)) + φ2 (t)φ̇2(t)
= + 3 (10)
(1 + φ21 (t))2 (1 − φ22 (t)) 2

2 Thí nghiệm

Chọn các tham số a, b, r, ω

Tính

x(t) = a + r cos ωt
y(t) = b + r sin ωt
ẋ(t) = −rω sin ωt
ẏ(t) = rω cos ωt
ẍ(t) = −rω 2 cos ωt
ÿ(t) = −rω 2 sin ωt (11)

Tính

y(t)
φ1 (t) =
x(t)
ẏ(t)x(t) − ẋ(t)y(t)
φ̇1 (t) =
x2(t)
(ÿ(t)x(t) − ẍ(t)y(t))x(t) − 2ẋ(t)(ẏ(t)x(t) − ẋ(t)y(t))
φ̈1 (t) = (12)
x3(t)

3
Tính
x2(t) + y 2 (t) + L21 − L22
φ2 (t) = p
2L1 x2(t) + y 2 (t)
!
2 2
1 L − L2 x(t)ẋ(t) + y(t)ẏ(t)
φ̇2 (t) = 1− 2 1 p
2L1 x (t) + y 2 (t) x2(t) + y 2 (t)
!
1 3(L21 − L22 ) (x(t)ẋ(t) + y(t)ẏ(t))2
φ̈2 (t) = −1 3
2L1 x2(t) + y 2 (t) (x2(t) + y 2 (t)) 2
!
2 2
1 L1 − L2 ẋ2(t) + ẏ 2 (t) + x(t)ẍ(t) + y(t)ÿ(t)
+ 1− 2 p (13)
2L1 x (t) + y 2 (t) x2(t) + y 2 (t)

Tính
φ̇1 (t) φ̇2 (t)
θ̇S (t) = 2 +p
1 + φ1 (t) 1 − φ22 (t)
φ̈1 (t)(1 + φ21 (t)) − 2φ̇21 (t)φ1(t) φ̈2 (t)(1 − φ22 (t)) + φ2 (t)φ̇2(t)
θ̈S (t) = + 3 (14)
(1 + φ21 (t))2 (1 − φ22 (t)) 2

So sánh (14) với kết quả đo từ IMU và encoder.