Documenti di Didattica
Documenti di Professioni
Documenti di Cultura
1. Forward Kinematics
1- Identify the joints axes and assign the Z axis pointing along the i-th
joint axis.
2- Identify the common perpendiculars of the joints axes. At the point
where the common perpendicular meets the i-th axis, assign the
link-frame origin.
3- Assign the Xi axis pointing along the common perpendicular.
4- Assign the Yi axis to complete a right-hand coordinate system.
5- Assign frame {0} to match with frame {1} when the first joint
variable is zero.
6- For the last frame {n}, choose an origin location and X direction so
that as many as possible linkage parameters become zero. The last
method is called Right Handed Coordinate System (RHCS).
If the link frames have been attached to the links according to the
above convention, the following definitions of the link parameters are
valid:
αi-1 = the angle from Zi-1 to Zi measured about Xi-1 … (link twist);
ai-1 = the distance from Zi-1 to Zi measured along Xi-1 .. (link length);
(3.1)
Also, Equation 1 shows the general transformation matrix is:
[
i-1 T i = sinθ i cosα i-1 ,
sinθ i sin α i-1 ,
0
cosθ i cosα i-1,
cos θ i sin α i-1 ,
0
sin α i-1
cos α i-1
0
sin α i-1 d i
cos α i-1 d i
1
]
(1)
i i-1 ai-1 di i
1 0 0 0 ❑1
3 0 a3 0 ❑3
4 −90 ° a4 d4 ❑4
5 90 ° 0 0 90 ° +❑5
6 −90 ° 0 0 ❑6
Rewrite equation (1); the general transformation matrix is:
cos θ i -sin θ i 0 a i-1
0
[
i-1 T i = sinθ i cos α i-1,
sinθ i sin α i-1 ,
cos θ i cos α i-1,
cos θ i sin α i-1,
0
sin α i-1
cos α i-1
0
sin α i-1 d i
cos α i-1 d i
1
]
We can formulate the transformation matrix between each two successive
frames as follow:
c1 - s1 0 0 s2 c2 0 a2 c3 - s3 0 a3
[
0 T 1 = s1
0
0
c1
0
0
0
1
0
] [ ] [ ]
0 1T2=
0
1
; 0
c2
0
0
- s2
0
1
0
0
0
0
1
; 2T3= s3
0
0
c3
0
0
0
1
0
0
0
1
c4 - s4 0 a4 - s5 - c5 0 0 c6 - s6 0 0
3T 4=
0
[
- s4
0
0
- c4
0
1
0
0
0
1
] [ ] [ ]
d4
; 4 T 5= 0
c5
0
0
- s5
0
-1
0
0
0
0
1
; 5T6= 0
- s6
0
0
- c6
0
1
0
0
0
0
1
B
TW= 0T6 = 0T1.1T2.2T3.3T4.4T5.5T6 (2)
Where
nx ox ax px
n
BT W =0T6= y
nz
0
[ oy
oz
0
ay
az
0
py
Pz
1
] (3)
Where Px, Py and PZ are the global coordinates indicating the spatial
position of the end-effector. Using MATLAB programming to multiply
the individual matrices.
n z = -c 6 *( c5 * s 23 + c 4 * s 5 *c23 )- s4 * s6 *c23
o y = -s 6 * ( c 5 * s1 * c23 + s 5 * ( c 1 * s4 - c4 * s1 * s23 ) ) - c 6 * (c 1 * c 4 + s 4 *s 1 *s 23 )
a x = -s5 * c1 *c 23 - c5 *( s1 *s 4 + c4 * c1 *s23 )
a y = c5 *(c 1 * s 4 - c 4 *s 1 * s 23 )- s5 *s 1 *c 23
a z = s5 *s 23 - c4 * c5 *c23
P y = a 2 * s 1 + a 4 * s 1 * s23 +d 4 * s 1 * c23 + a 3 * s 1 * s2
P z = a 3 * c 2 + a 4 *c 23 -d 4 * s 23
Where:
cn : cos(θn) , sn : sin(θn)
The last equations can be noted as equation (4) to simplify using it.
These equations give the forward kinematics of the designed arm robot.
Knowing the robot variables (θ1, θ2, θ3, θ4, θ5, θ6) then 0T6 will be identified,
and the position and orientation of the robot wrist relative to the base
frame is known.
Example:
Compute the total transformation matrix of 6DOF robotic arm which
have: θ1 = 45, θ2 = 30, θ3 = 60, θ4 = 20, θ5 = 30, θ6 = 70
Solution
The transformation matrix can be found by substitution in equation
(4) so:
0.2421 0.5704 -0.7849 0.0767
[
0 T 6 = -0.9239
-0.2962
0
-0.1114
0.8138
0
-0.3660
0.5000
0
0.0767
-0.0777
1
]
2. Inverse Kinematics (IK)