R01AN5644EJ0100 Rev.1.00
Page 46 of 62
Jan 22, 2021
Reference Guide for a 2-Axis Robot Arm with 2-Phase Stepping
Motors Incorporating Resolvers
RX24T, RX72M,
RAA3064002GFP/RAA3064003GFP
Obtain the inverse-Jacobian matrix.
� 𝐿𝐿
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
In the inverse-Jacobian matrix, replace the equations with the following contents.
cos
𝜃𝜃
1𝑖𝑖−1
= cos 1
,
cos(
𝜃𝜃
1𝑖𝑖−1
+
𝜃𝜃
2𝑖𝑖−1
) = cos 1
𝑝𝑝
2
,
sin
𝜃𝜃
1𝑖𝑖−1
= sin 1
,
sin
𝜃𝜃
2𝑖𝑖−1
= sin 2
,
sin(
𝜃𝜃
1𝑖𝑖−1
+
𝜃𝜃
2𝑖𝑖−1
) = sin 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
(
𝐿𝐿
1
cos 1 +
𝐿𝐿
2
cos 1
𝑝𝑝
2) × (
−𝐿𝐿
2
sin 1
𝑝𝑝
2)
−
(
𝐿𝐿
2
cos 1
𝑝𝑝
2) × (
−𝐿𝐿
1
sin 1
− 𝐿𝐿
2
sin 1
𝑝𝑝
2)
�
−𝐿𝐿
2
sin 1
𝑝𝑝
2
−𝐿𝐿
2
cos 1
𝑝𝑝
2
−
(
−𝐿𝐿
1
sin 1
− 𝐿𝐿
2
sin 1
𝑝𝑝
2)
𝐿𝐿
1
cos 1 +
𝐿𝐿
2
cos 1
𝑝𝑝
2
�
=
1
(
−𝐿𝐿
1
𝐿𝐿
2
cos 1 sin 1
𝑝𝑝
2) +
�−𝐿𝐿
2
2
cos 1
𝑝𝑝
2 sin 1
𝑝𝑝
2
�
+ (
𝐿𝐿
2
𝐿𝐿
1
sin 1 cos 1
𝑝𝑝
2) +
�𝐿𝐿
2
2
cos 1
𝑝𝑝
2 sin 1
𝑝𝑝
2
�
�
−𝐿𝐿
2
sin 1
𝑝𝑝
2
−𝐿𝐿
2
cos 1
𝑝𝑝
2
𝐿𝐿
1
sin 1 +
𝐿𝐿
2
sin 1
𝑝𝑝
2
𝐿𝐿
1
cos 1 +
𝐿𝐿
2
cos 1
𝑝𝑝
2
�
=
1
(
−𝐿𝐿
1
𝐿𝐿
2
cos 1 sin 1
𝑝𝑝
2) + (
𝐿𝐿
2
𝐿𝐿
1
sin 1 cos 1
𝑝𝑝
2)
�
−𝐿𝐿
2
sin 1
𝑝𝑝
2
−𝐿𝐿
2
cos 1
𝑝𝑝
2
𝐿𝐿
1
sin 1 +
𝐿𝐿
2
sin 1
𝑝𝑝
2
𝐿𝐿
1
cos 1 +
𝐿𝐿
2
cos 1
𝑝𝑝
2
�
=
1
𝐿𝐿
1
𝐿𝐿
2
(sin 1 cos 1
𝑝𝑝
2
−
cos 1 sin 1
𝑝𝑝
2)
�
−𝐿𝐿
2
sin 1
𝑝𝑝
2
−𝐿𝐿
2
cos 1
𝑝𝑝
2
𝐿𝐿
1
sin 1 +
𝐿𝐿
2
sin 1
𝑝𝑝
2
𝐿𝐿
1
cos 1 +
𝐿𝐿
2
cos 1
𝑝𝑝
2
�
From the additive theorem of trigonometric functions:
=
1
−
𝐿𝐿
1
𝐿𝐿
2
𝑠𝑠𝐸𝐸𝐸𝐸
2
�
−𝐿𝐿
2
sin 1
𝑝𝑝
2
−𝐿𝐿
2
cos 1
𝑝𝑝
2
𝐿𝐿
1
sin 1 +
𝐿𝐿
2
sin 1
𝑝𝑝
2
𝐿𝐿
1
cos 1 +
𝐿𝐿
2
cos 1
𝑝𝑝
2
�
Replace part of the equation for obtaining the current joint angle with the above.
�𝜃𝜃
1𝑖𝑖
𝜃𝜃
2𝑖𝑖
�
=
�𝜃𝜃
1𝑖𝑖−1
𝜃𝜃
2𝑖𝑖−1
�
+
1
−𝐿𝐿
1
𝐿𝐿
2
sin
𝜃𝜃
2𝑖𝑖−1
�
−𝐿𝐿
2
𝑠𝑠𝐸𝐸𝐸𝐸
(
𝜃𝜃
1𝑖𝑖−1
+
𝜃𝜃
2𝑖𝑖−1
)
−𝐿𝐿
2
𝑦𝑦𝐸𝐸𝑠𝑠
(
𝜃𝜃
1𝑖𝑖−1
+
𝜃𝜃
2𝑖𝑖−1
)
𝐿𝐿
1
𝑠𝑠𝐸𝐸𝐸𝐸 𝜃𝜃
1𝑖𝑖−1
+
𝐿𝐿
2
𝑠𝑠𝐸𝐸𝐸𝐸
(
𝜃𝜃
1𝑖𝑖−1
+
𝜃𝜃
2𝑖𝑖−1
)
𝐿𝐿
1
𝑦𝑦𝐸𝐸𝑠𝑠 𝜃𝜃
1𝑖𝑖−1
+
𝐿𝐿
2
𝑦𝑦𝐸𝐸𝑠𝑠
(
𝜃𝜃
1𝑖𝑖−1
+
𝜃𝜃
2𝑖𝑖−1
)
� �
𝑑𝑑𝑋𝑋
𝑖𝑖
𝑑𝑑𝑡𝑡
𝑑𝑑𝑌𝑌
𝑖𝑖
𝑑𝑑𝑡𝑡
�
Solve the determinant. The following equations are actually included in the program.
𝜃𝜃
1𝑖𝑖
=
𝜃𝜃
1𝑖𝑖−1
+
1
−𝐿𝐿
1
𝐿𝐿
2
sin
𝜃𝜃
2𝑖𝑖−1
�−𝐿𝐿
2
𝑠𝑠𝐸𝐸𝐸𝐸
(
𝜃𝜃
1𝑖𝑖
+
𝜃𝜃
2𝑖𝑖
) ×
𝑑𝑑𝑋𝑋
𝑖𝑖
𝑑𝑑𝑡𝑡 − 𝐿𝐿
2
𝑦𝑦𝐸𝐸𝑠𝑠
(
𝜃𝜃
1𝑖𝑖
+
𝜃𝜃
2𝑖𝑖
) ×
𝑑𝑑𝑌𝑌
𝑖𝑖
𝑑𝑑𝑡𝑡 �
𝜃𝜃
2𝑖𝑖
=
𝜃𝜃
2𝑖𝑖−1
+
1
−𝐿𝐿
1
𝐿𝐿
2
sin
𝜃𝜃
2𝑖𝑖−1
�
{
𝐿𝐿
1
𝑠𝑠𝐸𝐸𝐸𝐸 𝜃𝜃
1𝑖𝑖−1
+
𝐿𝐿
2
𝑠𝑠𝐸𝐸𝐸𝐸
(
𝜃𝜃
1𝑖𝑖−1
+
𝜃𝜃
2𝑖𝑖−1
)} ×
𝑑𝑑𝑋𝑋
𝑖𝑖
𝑑𝑑𝑡𝑡
+ {
𝐿𝐿
1
𝑦𝑦𝐸𝐸𝑠𝑠 𝜃𝜃
1𝑖𝑖−1
+
𝐿𝐿
2
𝑦𝑦𝐸𝐸𝑠𝑠
(
𝜃𝜃
1𝑖𝑖−1
+
𝜃𝜃
2𝑖𝑖−1
)} ×
𝑑𝑑𝑌𝑌
𝑖𝑖
𝑑𝑑𝑡𝑡 �