6-DoF Robot Elbow – A Tutorial in MATLAB and Simulink

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

Comments

Leave a Reply

Your email address will not be published. Required fields are marked *