Table of Contents
Advertisement
Reference Guide for a 2-Axis Robot Arm with 2-Phase Stepping
Motors Incorporating Resolvers
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
Y
θ
2
θ
1
L
1
Calculate the tip coordinates (x, y) from ����
x = ����
sin ����
+ ����
1
1
y = ����
cos ����
+ ����
1
1
���� ̇ = {����
cos ����
Differentiate them by time.
1
1
���� ̇ = {����
sin ����
1
1
����
cos ����
���� ̇
Change them into a matrix.
�
� = �
���� ̇
1
−����
sin ����
1
Change the matrix into a form to obtain ����
��������
1
����
cos ����
��������
�
� = �
1
��������
−����
2
1
��������
��������
= ����
− ����
and ��������
����
����
����−1
Add the difference of the angle to the joint angle in the previous cycle and assume the result as the
current joint angle.
����
����
�
� = �
� + �
1����
1����−1
����
����
2����
2����−1
R01AN5644EJ0100 Rev.1.00
Jan 22, 2021
P(x,y)
L
2
X
sin (����
+ ����
)
2
1
2
cos (����
+ ����
)
2
1
2
̇ + ����
+ ����
cos (����
+ ����
)}����
2
1
2
1
̇ + ����
+ ����
sin (����
+ ����
)}����
2
1
2
1
+ ����
cos ( ����
+ ����
)
1
2
1
2
sin ( ����
) −����
− ����
+ ����
1
2
1
2
cos ( ����
)
+ ����
+ ����
1����
2
1����
2����
sin ����
− ����
sin ( ����
+ ����
1����
2
1����
2����
= ����
− ����
����
����
����−1
are assumed.
cos ( ����
����
cos ����
+ ����
1
1����−1
2
sin ( ����
−����
sin ����
− ����
1
1����−1
2
and ����
1
2
.
̇
cos (����
+ ����
) ����
2
1
2
2
̇
sin (����
+ ����
) ����
2
1
2
2
̇
����
cos ( ����
+ ����
)
����
� �
�
2
1
2
1
sin ( ����
)
̇
+ ����
����
2
1
2
2
and ����
1
2
, and differentiate both sides by time.
cos ( ����
)
����
+ ����
−1
�
2
1����
2����
) −����
sin ( ����
+ ����
)
2
1����
2����
)
cos ( ����
+ ����
����
1����−1
2����−1
2
) −����
sin ( ����
+ ����
1����−1
2����−1
2
RX24T, RX72M,
RAA3064002GFP/RAA3064003GFP
��������
����
��������
�
�
��������
����
��������
��������
)
����
+ ����
��������
−1
�
�
1����−1
2����−1
)
��������
+ ����
����
1����−1
2����−1
��������
�
Page 45 of 62
Advertisement
Table of Contents
