R01AN5644EJ0100 Rev.1.00
Page 45 of 62
Jan 22, 2021
Reference Guide for a 2-Axis Robot Arm with 2-Phase Stepping
Motors Incorporating Resolvers
RX24T, RX72M,
RAA3064002GFP/RAA3064003GFP
2.4.3 Joint Angle Calculation
Calculate the angle of each joint from the tip coordinates of the arm by using successive Jacobian
calculations.
•
Successive Jacobian calculations
Calculate the tip coordinates (x, y) from
𝜃𝜃
1
and
𝜃𝜃
2
.
x =
𝐿𝐿
1
sin
𝜃𝜃
1
+
𝐿𝐿
2
sin (
𝜃𝜃
1
+
𝜃𝜃
2
)
y =
𝐿𝐿
1
cos
𝜃𝜃
1
+
𝐿𝐿
2
cos (
𝜃𝜃
1
+
𝜃𝜃
2
)
Differentiate them by time.
𝑥𝑥̇
= {
𝐿𝐿
1
cos
𝜃𝜃
1
+
𝐿𝐿
2
cos (
𝜃𝜃
1
+
𝜃𝜃
2
)}
𝜃𝜃
1
̇
+
𝐿𝐿
2
cos (
𝜃𝜃
1
+
𝜃𝜃
2
)
𝜃𝜃
2
̇
𝑦𝑦̇
= {
𝐿𝐿
1
sin
𝜃𝜃
1
+
𝐿𝐿
2
sin (
𝜃𝜃
1
+
𝜃𝜃
2
)}
𝜃𝜃
1
̇
+
𝐿𝐿
2
sin (
𝜃𝜃
1
+
𝜃𝜃
2
)
𝜃𝜃
2
̇
Change them into a matrix.
�𝑥𝑥̇𝑦𝑦̇�
=
� 𝐿𝐿
1
cos
𝜃𝜃
1
+
𝐿𝐿
2
cos(
𝜃𝜃
1
+
𝜃𝜃
2
)
𝐿𝐿
2
cos(
𝜃𝜃
1
+
𝜃𝜃
2
)
−𝐿𝐿
1
sin
𝜃𝜃
1
− 𝐿𝐿
2
sin(
𝜃𝜃
1
+
𝜃𝜃
2
)
−𝐿𝐿
2
sin(
𝜃𝜃
1
+
𝜃𝜃
2
)
� �𝜃𝜃
1
̇
𝜃𝜃
2
̇ �
Change the matrix into a form to obtain
𝜃𝜃
1
and
𝜃𝜃
2
, and differentiate both sides by time.
�
𝑑𝑑𝜃𝜃
1
𝑑𝑑𝑡𝑡
𝑑𝑑𝜃𝜃
2
𝑑𝑑𝑡𝑡
�
=
� 𝐿𝐿
1
cos
𝜃𝜃
1𝑖𝑖
+
𝐿𝐿
2
cos(
𝜃𝜃
1𝑖𝑖
+
𝜃𝜃
2𝑖𝑖
)
𝐿𝐿
2
cos(
𝜃𝜃
1𝑖𝑖
+
𝜃𝜃
2𝑖𝑖
)
−𝐿𝐿
1
sin
𝜃𝜃
1𝑖𝑖
− 𝐿𝐿
2
sin(
𝜃𝜃
1𝑖𝑖
+
𝜃𝜃
2𝑖𝑖
)
−𝐿𝐿
2
sin(
𝜃𝜃
1𝑖𝑖
+
𝜃𝜃
2𝑖𝑖
)
�
−1
�
𝑑𝑑𝑋𝑋
𝑖𝑖
𝑑𝑑𝑡𝑡
𝑑𝑑𝑌𝑌
𝑖𝑖
𝑑𝑑𝑡𝑡
�
𝑑𝑑𝑋𝑋
𝑖𝑖
=
𝑋𝑋
𝑖𝑖
− 𝑋𝑋
𝑖𝑖−1
and
𝑑𝑑𝑌𝑌
𝑖𝑖
=
𝑌𝑌
𝑖𝑖
− 𝑌𝑌
𝑖𝑖−1
are assumed.
Add the difference of the angle to the joint angle in the previous cycle and assume the result as the
current joint angle.
�𝜃𝜃
1𝑖𝑖
𝜃𝜃
2𝑖𝑖
�
=
�𝜃𝜃
1𝑖𝑖−1
𝜃𝜃
2𝑖𝑖−1
�
+
� 𝐿𝐿
1
cos
𝜃𝜃
1𝑖𝑖−1
+
𝐿𝐿
2
cos(
𝜃𝜃
1𝑖𝑖−1
+
𝜃𝜃
2𝑖𝑖−1
)
𝐿𝐿
2
cos(
𝜃𝜃
1𝑖𝑖−1
+
𝜃𝜃
2𝑖𝑖−1
)
−𝐿𝐿
1
sin
𝜃𝜃
1𝑖𝑖−1
− 𝐿𝐿
2
sin(
𝜃𝜃
1𝑖𝑖−1
+
𝜃𝜃
2𝑖𝑖−1
)
−𝐿𝐿
2
sin(
𝜃𝜃
1𝑖𝑖−1
+
𝜃𝜃
2𝑖𝑖−1
)
�
−1
�
𝑑𝑑𝑋𝑋
𝑖𝑖
𝑑𝑑𝑡𝑡
𝑑𝑑𝑌𝑌
𝑖𝑖
𝑑𝑑𝑡𝑡
�
L
2
L
1
θ
1
θ
2
P(x,y)
X
Y
