Positive kinematics solution of robot based on DH parameter method

Positive kinematics solution of robot based on DH parameter method

Two Conventions for the establishment of a coordinate system using the DH parameter method:

( 1 ) x_i and Z_ (i-1) Vertical

( 2 ) intersection of X_i and Z_ (i-1)

The second transformation matrix of the coordinate system I and the coordinate system i-1 is:

                  A is a distance of two z-axis, and D is the distance of two x-axis.

                 alpha and Theta's Square conventions are:

here are three examples:

A, planar two-DOF robot

clear ;
CLC;
Syms theta1 alpha1 A1 D1 theta2 alpha2 A2 D2 a B theta D;
A1=[cos (theta1),-sin (theta1) *cos (alpha1), sin (theta1) *sin (ALPHA1), A1*cos (THETA1), ....
       sin (theta1), cos (theta1) *cos (ALPHA1),-cos (theta1) *sin (ALPHA1), A1*sin (THETA1);
       0,sin (ALPHA1), cos (ALPHA1), D1;
       0,0,0,1];
A2=[cos (THETA2),-sin (THETA2) *cos (alpha2), sin (theta2) *sin (ALPHA2), A2*cos (THETA2);
       sin (THETA2), cos (THETA2) *cos (ALPHA2),-cos (THETA2) *sin (ALPHA2), A2*sin (THETA2);
       0,sin (ALPHA2), cos (ALPHA2), D2;
       0,0,0,1];

L=SQRT (a^2+b^2);
Beta=atan (b/a);

A1=l;
Alpha1=sym (0);
D1=sym (0);
Theta1=theta;

A2=d;
Alpha2=sym (0);
D2=sym (0);
Theta2=-beta;

t=eval_r(A1*A2)

The kinematic positive solution is obtained:

B, planar three-DOF robot

Clear
CLC
Syms theta1 alpha1 A1 D1 theta2 alpha2 A2 D2 theta3 alpha3 A3 D3 D;
A1=[cos (THETA1),-sin (theta1) *cos (alpha1), sin (theta1) *sin (ALPHA1), A1*cos (THETA1);
Sin (theta1), cos (theta1) *cos (ALPHA1),-cos (theta1) *sin (ALPHA1), A1*sin (THETA1);
0,sin (ALPHA1), cos (ALPHA1), D1;
0,0,0,1];
A2=[cos (THETA2),-sin (THETA2) *cos (alpha2), sin (theta2) *sin (ALPHA2), A2*cos (THETA2);
Sin (theta2), cos (THETA2) *cos (ALPHA2),-cos (THETA2) *sin (ALPHA2), A2*sin (THETA2);
0,sin (ALPHA2), cos (ALPHA2), D2;
0,0,0,1];
A3=[cos (THETA3),-sin (THETA3) *cos (ALPHA3), sin (theta3) *sin (ALPHA3), A3*cos (THETA3);
Sin (theta3), cos (theta3) *cos (ALPHA3),-cos (THETA3) *sin (ALPHA3), A3*sin (THETA3);
0,sin (ALPHA3), cos (ALPHA3), D3;
0,0,0,1];

Alpha1=sym (0);
D1=sym (0);

A2=d;
Alpha2=sym (0);
D2=sym (0);
Theta2=sym (0);

Alpha3=sym (0);
D3=sym (0);
THETA3=-THETA3;

t=eval_r(A1*A2*A3)

The kinematic positive solution is obtained:

C, six DOF robot

Clear
CLC
Syms theta1 alpha1 A1 D1 theta2 alpha2 A2 D2 theta3 alpha3 A3 D3 ...
Theta4 alpha4 a4 d4 theta5 alpha5 a5 d5 theta6 alpha6 a6 d6;
A1=[cos (THETA1),-sin (theta1) *cos (alpha1), sin (theta1) *sin (ALPHA1), A1*cos (THETA1);
Sin (theta1), cos (theta1) *cos (ALPHA1),-cos (theta1) *sin (ALPHA1), A1*sin (THETA1);
0,sin (ALPHA1), cos (ALPHA1), D1;
0,0,0,1];
A2=[cos (THETA2),-sin (THETA2) *cos (alpha2), sin (theta2) *sin (ALPHA2), A2*cos (THETA2);
Sin (theta2), cos (THETA2) *cos (ALPHA2),-cos (THETA2) *sin (ALPHA2), A2*sin (THETA2);
0,sin (ALPHA2), cos (ALPHA2), D2;
0,0,0,1];
A3=[cos (THETA3),-sin (THETA3) *cos (ALPHA3), sin (theta3) *sin (ALPHA3), A3*cos (THETA3);
Sin (theta3), cos (theta3) *cos (ALPHA3),-cos (THETA3) *sin (ALPHA3), A3*sin (THETA3);
0,sin (ALPHA3), cos (ALPHA3), D3;
0,0,0,1];
A4=[cos (THETA4),-sin (THETA4) *cos (alpha4), sin (theta4) *sin (ALPHA4), A4*cos (THETA4);
Sin (theta4), cos (THETA4) *cos (ALPHA4),-cos (THETA4) *sin (ALPHA4), A4*sin (THETA4);
0,sin (ALPHA4), cos (ALPHA4), D4;
0,0,0,1];
A5=[cos (THETA5),-sin (THETA5) *cos (ALPHA5), sin (theta5) *sin (ALPHA5), A5*cos (THETA5);
Sin (theta5), cos (THETA5) *cos (ALPHA5),-cos (THETA5) *sin (ALPHA5), A5*sin (THETA5);
0,sin (ALPHA5), cos (ALPHA5), D5;
0,0,0,1];
A6=[cos (THETA6),-sin (THETA6) *cos (alpha6), sin (theta6) *sin (ALPHA6), A6*cos (THETA6);
Sin (theta6), cos (THETA6) *cos (ALPHA6),-cos (THETA6) *sin (ALPHA6), A6*sin (THETA6);
0,sin (ALPHA6), cos (ALPHA6), D6;
0,0,0,1];

A1=sym (0);
Alpha1=sym (-PI/2);

Alpha2=sym (0);
D2=sym (0);

A3=sym (0);
Alpha3=sym (-PI/2);
D3=sym (0);

A4=sym (0);
Alpha4=sym (-PI/2);
D4=sym (0);

A5=sym (0);
Alpha5=sym (PI/2);
D5=sym (0);

A6=sym (0);
Alpha6=sym (0);

t=simplify (eval_r (A1*A2*A3*A4*A5*A6))

Note: The above eval_r is eval (do not know why, after saving" eval "becomes" Eval_r ")

from:http://blog.sina.com.cn/u/2707887295

Positive kinematics solution of robot based on DH parameter method