Select the base coordinate system and attach the coordinate systems to the links.

The forward kinematics problem is Calculating the matrix T6

KINEMATIC PARAMETER (DENAVIT HARTENBERG)

Θi :Angle of rotation about the Z axis
α : Angle of rotation of the Z axis around the X axis (positive counterclockwise)
ai: Distance between axes Z
di: Distance between axes Z
GENERAL MATRIX
An=Rot(z,θ)Trans (0, 0, d) Trans(a,0,0) Rot(x,α)
cosθ -sinθcosα sinθsinα a cosθ
sinθ cosθcosα -cosθsinα a sinθ
An = 0 sinα cosα d
0 0 0 1

Using MatLab to calculate the Matrix T6
A1= [cosd(t1) 0 sind(t1) 0;sind(t1) 0 -cosd(t1) 0;0 1 0 0;0 0 0 1];
A2= [cosd(t2) -sind(t2) 0 cosd(t2)*a2 ;sind(t2) cosd(t2) 0 sind(t2)*a2; 0 0 1 0;0 0 0 1];
A3= [cosd(t3) -sind(t3) 0 cosd(t3)*a3;sind(t3) cosd(t3) 0 sind(t3)*a3 ;0 1 0 0;0 0 0 1];
A4= [cosd(t4) 0 -sind(t4) cosd(t4)*a4;sind(t4) 0 cosd(t4) sind(t4)*a4;0 -1 0 0;0 0 0 1];
A5= [cosd(t5) 0 sind(t5) 0; sind(t5) 0 -cosd(t5) 0;0 1 0 0;0 0 0 1];
A6= [cosd(t6) -sind(t6) 0 0;sind(t6) cosd(t6) 0 0;0 0 1 0;0 0 0 1];
Inverse Kinematics: The inverse kinematics problem is to calculate the rotations around the joint axis
θ1 = θ1 + 1800
θ2 = arctg2((C3a3 + a2)p’y – S3a3p’x , (C3a3 + a2)p’x + S3a3p’y )
θ3 = arctg2(S3 , C3)
θ4 = θ234 – θ3 – θ2
θ5 = arctg2(C234(C1ax + S1ay) + S234az , S1ax – C1ay)
θ6 = arctg2(S6 , C6)
Leave a Reply