Robot's inverse kinematics

CONTENT:
1. Introduction
1.1. Forward kinematics of robot – purpose
1.2. Inverse kinematics of robot – purpose
1.3 Static analysis of robot - purpose
2. Forward kinematics of robot
2.1. Project of robot
2.2. Denavit-Hartenberg notation
2.3. Calculation of trajectory in MATLAB program
2.4. Succeeding robot’s positions for changes of configuration variables
3. Inverse kinematics of robot
3.1. General form of transformation matrix
3.2. Transformation matrixes for consecutive coordinates systems
3.3. General form of setpoint matrix
3.4. Comparison of setpoint matrix and transformation matrix
3.5. Calculation of configuration variables
3.6. Calculation of trajectory in program MATLAB
4. Static analysis of robot

1. Introduction

Project shown here is a complete solution of robot’s inverse kinematics. Subject of the project is an example robot. To make possible calculation of inverse kinematics, firstly, the forward kinematics must be calculated. Additionally, beside forward and inverse kinematics of robot, there are drawn trajectories of robot’ motion. Trajectory was calculated in MATLAB environment. A complete source code for simulation in MATLAB is placed in this project.
Known: θi(t), di(t)
Unknown: x0(t), y0(t), z0(t)
In brief forward kinematics can be presented in one line by notation:
i(t), di(t)} → {x0(t), y0(t), z0(t)}

1.1. Forward kinematics of robot – purpose

Forward kinematics can be described in brief as a case when angles and shifts of robot’s elements are known. As a result of forward kinematics we want to obtain position defined in the last coordinates system (tool coordinate system) in the base coordinates system.

1.1. Inverse kinematics of robot – purpose

Inverse kinematics can be described in brief as a case when position of tool in the base coordinate system is known and as a result, angles and shifts of robot’s elements must be calculated. Known: x0(t), y0(t), z0(t)
Unknown: θi(t), di(t)
In brief inverse kinematics can be presented in one line by notation:
{x0(t), y0(t), z0(t)} → {θi(t), di(t)}

1.3. Static analysis of robot - purpose

In this project static analysis of robot is also performed. Performing of static analysis will give an answer what forces and torques have to be produced by robot’s drives to keep its construction in equilibrium during resting. Note that real robot has brakes on drives which are responsible for keeping it in balance during resting, however, knowledge about static load is important. Moreover, as a result of static analysis required strength of robot’s construction will be known. Please note that it is only static analysis. During normal work of robot dynamic loads will appear. Analysis of dynamics load is performed in another project which is only about robot’s dynamics. Project about robot’s dynamics may be found on website dynamics of robot.

2. Forward kinematics of robot

Forward kinematics will be solved first for an example robot.

2.1. Project of robot

Considered robot has four angular drives and one linear drive.

Model of robot

2.2. Denavit-Hartenberg notation

Table with Denavit-Hartenberg parameters.

Model of robot

2.3. Calculation of trajectory in MATLAB program

Trajectory was calculated in matlab program. To do this, firstly a function for calculation transformation matrixes was created. Function takes as input parameters Denavit-Hartenberg parameters.
function[T]=ma_trans(alfa,a,d,theta)
cA=cos(alfa);
sA=sin(alfa);
cQ=cos(theta);
sQ=sin(theta);
T=[ cQ -sQ 0 a ;
sQ*cA cQ*cA -sA -sA*d ;
sQ*sA cQ*sA cA cA*d ;
0 0 0 1 ] ;

Changes of configuration variables during forward kinematics

Y axes of graphs, which are shown above, are described with different symbols. Following configuration variables are assigned to these symbols:
q_1(t) → θ1(t)
q_2 (t) → θ2(t)
q_3 (t) → θ3(t)
d_4 (t) → d4(t)
q_5 (t) → θ5(t)

Motion of robot in XY plane for set configuration variables

2.4. Succeeding robot’s positions for changes of configuration variables

Projection of trajectory in XY plane
Robot's initial position - time t0
Robot's position after time t1
Robot's position after time t2
Robot's position after time t3
Robot's position after time t4
Robot's position after time t5

3. Inverse kinematics of robot

In this elaboration robot’s inverse kinematics will be designated only to the third coordinates system. Reader may use calculations from this project to calculate inverse kinematics with consideration of all robot’s coordinates systems.
Purpose of inverse kinematics is to find angles and shifts of robot’s elements for Cartesian position which is defined in base coordinates system. As it is written above inverse kinematics will be calculated only to the third coordinates system.

3.1. General form of transformation matrix

Matrix i-1iT describes transformation from coordinates system „i” to coordinates system „i-1”

General form of transformation matrix

Abbreviated symbols of trigonometric functions are applied in matrix above.

Applied abbreviations of trigonometric functions

3.2. Transformation matrixes for consecutive coordinates systems

Transformation matrix between coordinates systems „1” and „0”

Transformation matrix T10

Where: S1=sinθ1, C1=cosθ1

Transformation matrix between coordinates systems „2” and „1”

Transformation matrix T21

Where: S2=sinθ2, C2=cosθ2

Transformation matrix between coordinates systems „3” and „2”

Transformation matrix T32

Where: S3=sinθ3, C3=cosθ3

If consecutive transformation matrixes between coordinates systems are known then it is possible to calculate transformation matrix between base’s coordinates system and the last coordinates system.

Transformation matrix T32

After consecutive multiply operations between matrixes the result below is obtained:

Multiply transformation matrices

Obtained matrix describes by symbolic notation rotations and shifts between third coordinates system and base’s coordinates system.

3.3.General form of setpoint matrix

General form of setpoint matrix.

General form of setpoint matrix

3.4. Comparison of setpoint matrix and transformation matrix

In robot’s model, which is the subject of this elaboration, motion is in two dimensions. Matrix 03TD in the general case for motion in two dimensions looks as follows:

General form of setpoint matrix for two dimensional motion

Matrix 03TD is called a setpoint matrix. Matrix 03TD has to be compared with obtained earlier matrix 03T to make possible expression angles and shifts as functions of cartesian coordinates relative to base coordinates system. Through a comparison it is understood the comparison of indexes of these matrixes and equate them to each other. As a result the following system of equations is obtained:

Comparison of matrices
Comparison of matrices

Two first equations are simplifying after application of trigonometric reduction formulas; thereby, as a result is obtained:

Comparison of matrices

Subsequently notation of equations for X_0 and Y_0 is changed as follows:

Solving the system of equations

Mentioned earlier equations are raised square root and are added by sides:

Solving the system of equations

Expressions, which are inside equations, are multiplied:

Solving the system of equations

Relation is used:

Used relation

Substitutions are applied:

Applied substitutions

The first equation is written after taking into account substitutions:

Thus:

Where:

3.5. Calculation of configuration variables

Expression for θ1(t) is obtained by inserting relations, which were calculated above, into general formula:

Remaining variables are calculated in analogously way:

Designation of the second configuration variable → θ2(t)

Designation of the third configuration variable → θ3(t)

At this point, all expressions for configuration variables in function of cartesian coordinates are known. It is essential to designate domains of functions. Without any issues it is easy to note that lengths of arms have to bigger than nil.
L1>0; L2>0;L4>0
Obviously it is not an end of functions domains designations. Remaining domains designation is left to reader. Various positions of robot has to considered during designation of domains (range of angles values) because of possible collisions.

3.6. Calculation of trajectory in program MATLAB