Embedded Files
The Aim of the project was to design a symbolic algorithm in Matlab that that derives the complete equations of motion of a swayer robot arm given the homogenous transformations from base frame to the end effector following DH convention.
Step 1) Forward kinematics
The forward kinematics of this robot are calculated using RH convention with the assumption of CCW rotation as +.The table of the complete DH parameters from base frame (0) to the wrist joint (frame 7).
The table of parameters shown above are derived based on the current (image) position of the robot. The algorithm goes through each of the parameters per link drives the Homogenous transformation matrices based on the z*d*a*x rule. Each transformation is then multiplied by the previous transformation and the first transformation is assumed as 4 by 4 identity.
Step 2) Angular and linear jacobians
To derive the equation of motions it is necessary to derive the angular and linear jacobians the COM of each link with all other frames. The COM position are assumed to be given with respect to the consecutive frame e.g. the position vector of the COM for link 1 is given with respect to frame 1. Therefore in the next section of the algorithm the position vector for al 7 COMs are calculated w.r.t the base frame (0), this is done by Pi=T0-i*PCOM where i represents each link. The angular jacobians are 3 by 7 matrix, the derivation is based on the assumption that all links up to the specific COM effect the angular velocity of that COM therefore all z component of the rotation of all links up the frame after the specific COM e.g.
JwCOM[6]=[z0 z1 z2 z3 z4 z5 0] where all components of the jacobian are 3 dimensional vectors.
The process of driving the linear velocity jacobians is through:
JvCOM [i,k] =Zk X PCOM - Pk (i, k <= n and n=7)
For every iteration of i, k will iterate from 1 to n. This will result in 7 different linear Jacobians where each jacobian is a 3 by 7 matrix. The combination of the both jacobians is:
[[Jvcom[i] ... Jvcom[n]];[JwCOM[i] ... JwCOM[n]]]
Step 3) Inertial matrix and kinetic energy
After deriving the angular and linear velocity jacobians it is necessary to derive the inertial matrix. To do so it is required know the inertia tensor of each link with respect to the base frame. Any design software like Solidworks can provide this data or a combination of parallel axis theorem and steiner theorem can be used to derive the inertia matrix. To derive the D matrix we use:
where the Jw and Ji are now known and the masses of each link mi is assumed to be given and by knowing the inertia tensor with respect to the inertial frame we can derive the symmetric inertial matrix D(q).
deriving the kinetic energy is trivial and is obtained through
Where all the values are known and q(dot) which is the rotational velocity of the motors.
Step 4) Deriving potential energy
In this step we derive the potential energies using the equation.
Where mi is the link masses g is the gravitational vector and Ci is the position of each centre of mass with respect to the base frame.
Step 5) Coriolis matrix
We derive the coriolis forces by using the symmetric matrix D and the Christoffel symbols. The general equation for deriving Christoffel symbols is:
Where d(index) is an element of the inertial matrix D and q(dot) is the joint velocity.
Step 6) Deriving the equations of motion
We use the equation:
where the coriolis matrix C is a 7 by 1 matrix.
The overall equation of motion can the be derived as:
The Matlab code for this derivation process can be found in the link below. Due to the high DOF nature of the robot the symbolic derivation is very long and should be evaluated element by element however the results are accurate.
Page updated
Google Sites
Report abuse