6 DOF ROBOTIC MANIPULATOR MATHEMATICS AND MODELS

Author: Erwin D. López Zapata

Figure 1. Configuration of the 6 DOF Robotic manipulator.

FORWARD KINEMATICS

The Forwards Kinematics model will be defined to generate a function able to calculate the effector’s
position vector with the values of the 6 motors rotation values. As a first step we will define the
mathematical relationships between the different joints position vectors and the robot configuration by
the Denavit-Hartenberg matrices.

We can define the function DH as the Denavit-Hartenberg matrix as:

cos 𝜃− sin 𝜃 cos ∝ sin 𝜃 sin ∝ 𝑎 cos 𝜃
𝐷𝐻(𝜃, 𝑑, 𝑎, ∝) = [ sin 𝜃 cos 𝜃 cos ∝ − cos 𝜃 sin ∝ 𝑎 sin 𝜃 ]
0 sin ∝ cos ∝ 𝑑
0 0 0 1
So, the transformations matrices will be defined as:

𝐴01 (𝑞1 ) = 𝐷𝐻(𝑞1 , 𝐿1 , 0, 𝜋⁄2)

𝐴12 (𝑞2 ) = 𝐷𝐻(𝑞2 , 0, 𝐿2 , 0)

𝐴23 (𝑞3 ) = 𝐷𝐻(𝑞3 , 0, 𝐿3 , 0)
𝐴34 (𝑞4 ) = 𝐷𝐻(0,0, 𝐿4 , 𝑞4 )
𝐴45 (𝑞5 ) = 𝐷𝐻(𝑞5 , 0, 𝐿5 , 0)
𝐴56 (𝑞6 ) = 𝐷𝐻(0,0, 𝐿6 , 𝑞6 )
Then, the function that calculates the transformation matrix of the effector from the 6 motors rotation
values will be defined as:

𝑇06 (𝑞1 , 𝑞2 , 𝑞3 , 𝑞4 , 𝑞5 , 𝑞6 ) = 𝑇0 𝐴01 (𝑞1 )𝐴12 (𝑞2 )𝐴23 (𝑞3 )𝐴34 (𝑞4 )𝐴45 (𝑞5 )𝐴56 (𝑞6 )
From this transformation matrix we can get the cartesian position vector and the orientation vector
(considering the Yaw, Pitch and Roll orientations) as it is defined below:
 [𝑥; 𝑦; 𝑧] = 𝑇06 |1:3,4

𝑇06 |3,2 𝑇06 |3,2

[tan−1 ( ) ; sin−1 𝑇06 |3,1 ; tan−1 ( )] , 𝑇06 |3,2 ≠ 0^𝑇06 |3,2 ≠ 0
𝑇06 |3,3 𝑇06 |3,3
[𝜑; 𝜃; ∅] =
𝜋 𝑇06 |2,3
[0; ; tan−1 ( )] , 𝑇06 |3,2 = 𝑇06 |3,2 = 0
{ 2 𝑇06 |1,3

Finally, we can define the effector’s position vector as the vector 𝑥⃗ = [𝑥; 𝑦; 𝑧; 𝜑; 𝜃; ∅], which could be
calculated from the matrix 𝑇06 and therefore from the 6 motors rotation values. The information
architecture of the signals processing is shown in the figure 2.

Figure 2. Forward Kinematics model’s signal processing architecture.

INVERSE KINEMATICS

We have two possible approaches for calculating the Inverse Kinematics model. The first is the geometric
approach, which is usually limited by the need of identifying work regions for all the motors (situation that
allows different solutions according to the selected work regions) and by the complexity of the geometric
calculations in a 6 DOF manipulator.

The second is the iterative method, which optimize the differential equation that defines the relationship
between the vectors 𝑞⃗ and 𝑥⃗. For this reason, this method finds only one solution which would change
according to the selected hyperparameter value of the speed of optimization.

We chose the second approach, so the needed differential equation was defined as:

𝐽𝑑𝑞⃗ = 𝑑𝑥⃗
Where J is the Jacobian matrix and is defined as:
𝜕𝑥 𝜕𝑥 𝜕𝑥
⋯
𝜕𝑞1 𝜕𝑞2 𝜕𝑞6
𝜕𝑦 𝜕𝑦 𝜕𝑦
𝐽 = 𝜕𝑞1 ⋯
𝜕𝑞2 𝜕𝑞6
⋮ ⋮ ⋱ ⋮
𝜕∅ 𝜕∅ 𝜕∅
⋯
[𝜕𝑞1 𝜕𝑞2 𝜕𝑞6 ]
In order to optimize the differential equation, we define the loss S as:

𝑆 = (𝑑𝑥⃗ − 𝐽𝑑𝑞⃗)𝑇 (𝑑𝑥⃗ − 𝐽𝑑𝑞⃗)

Then expanding the value of S is:

𝑆 = (𝑑𝑥⃗)𝑇 (𝑑𝑥⃗) − (𝑑𝑞⃗)𝑇 (𝐽𝑇 𝑑𝑥⃗) − (𝑑𝑥⃗)𝑇 𝐽𝑑𝑞⃗ + (𝑑𝑞⃗)𝑇 (𝐽𝑇 𝐽)(𝑑𝑞⃗)
Since in the Inverse Kinematics model we attempt to calculate the necessary values of 𝑞⃗ to reach a
stablished 𝑥⃗, we will minimize the value of S by modifying the values of 𝑑𝑞⃗, therefore we will define the
optimization as:
𝜕𝑆
=0 , 𝑗 = 1, 2, . . . ,6
𝜕𝑑𝑞𝑗

After replacing the value of S in this optimization equation it defines the next equation:

(𝐽𝑇 𝐽)𝑑𝑞⃗ = 𝐽𝑇 𝑑𝑥⃗

Therefore, the optimization equation will be:

𝑑𝑞⃗ = (𝐽𝑇 𝐽)−1 𝐽𝑇 𝑑𝑥⃗

Where 𝐽+ = (𝐽𝑇 𝐽)−1 𝐽𝑇 is called the pseudo-inverse of the Jacobian matrix, because it transforms the first
differential equation in this differential equation:

𝑑𝑞⃗ = 𝐽+ 𝑑𝑥⃗
Then, to calculate the necessary values of 𝑞⃗, we define the goal vector as 𝑥⃗𝑆𝑃 and make an iterative
process defined as follows:

∆𝑞⃗ 𝑛 = 𝛼𝐽+ (𝑥⃗𝑆𝑃 − 𝑥⃗0𝑛 )

𝑞⃗ 𝑛+1 = 𝑞⃗ 𝑛 + ∆𝑞⃗𝑛
Where 𝛼 is the hyperparameter of the Inverse Kinematics model that stablishes the speed of the
optimization.

The only problem with this approach is the calculation of the Jacobian matrix, since it requires analytical
solutions of all the matrix elements. However, with the Forward Kinematics model we can indirectly
calculate an approximation of this matrix. In the figure 3 it is shown how we can use this model to calculate
the approximated values of the partial differential values of the 𝑥⃗ vector with respect to the differential
value of the vector 𝑞⃗.

𝜕𝑥⃗
Figure 3. Approximation of the partial derivative vector 𝜕𝑞 using the Forward Kinematics model.
1
Consequently, we can calculate the approximated Jacobian matrix by repeating the process shown in
the figure 3 with the variations of all the motors rotation values. So, we will obtain:

∆𝑥⃗𝑞1 ∆𝑥⃗𝑞2 ∆𝑥⃗𝑞6

𝐽≈[ ⋯ ]
‖∆𝑞⃗1 ‖ ‖∆𝑞⃗2 ‖ ‖∆𝑞⃗6 ‖

Finally, we can apply the iterative equations defined above to update the values of the vector 𝑞⃗ until the
system reaches the desired vector 𝑥⃗. The information architecture of the signals processing is shown in
the figure 4.

Figure 4. Inverse Kinematics model’s signal processing architecture.

PLANNED INVERSE KINEMATICS

As long as the chose approach for the inverse kinematics modeling is based in an iterative process and is
defined as a multivariable control loop (figure 4), we can modify this model to allow the system to find a
path to both avoid an obstacle and reach the desire vector 𝑥⃗. For this purpose, we will use as background
the potential field path planning model to develop a Planned Inverse Kinematics.

Figure 5. Representation of the distances between the joints of the robot and an obstacle.
We have defined 6 repulsive forces from the 6 distances showed in figure 5. Each of these distances
considers a point corresponding to one of the robot joints’ extreme and the point of the position of the
obstacle (figure 5).

𝑑𝑗 = ‖𝑝𝑗 − 𝑝𝑜𝑏𝑠 ‖

Then, we define the repulsive force of each distance as:

2
1 1 1
𝑛𝑗 ( − ) , 𝑑𝑗 ≤ 𝜌
𝐹𝑗 = { 2 𝑑𝑗 𝜌
0 , 𝑑𝑗 > 𝜌

Accordingly, the partial derivate of the repulsive force respect with the xyz coordinates (without
considering orientations) is:

1 1 (𝑝𝑗 − 𝑝𝑜𝑏𝑠 )
𝑑𝐹𝑗 𝑛𝑗 ( − ) , 𝑑𝑗 ≤ 𝜌
={ 𝜌 𝑑𝑗 𝑑𝑗 3
𝑑𝑥⃗
⃗0⃗ , 𝑑𝑗 > 𝜌

We have also defined the partial Jacobian matrices for the different 6 points of the robot as:
𝜕𝑥𝑗 𝜕𝑥𝑗 𝜕𝑥𝑗
𝜕𝑞1 𝜕𝑞2 𝜕𝑞6
𝑇
𝑑𝐹𝑗 𝜕𝑦𝑗 𝜕𝑦𝑗 𝜕𝑦𝑗
𝐽𝐹𝑞(𝑗) =( ) ⋯
𝑑𝑥⃗ 𝜕𝑞1 𝜕𝑞2 𝜕𝑞6
𝜕𝑧𝑗 𝜕𝑧𝑗 𝜕𝑧𝑗
[𝜕𝑞1 𝜕𝑞2 𝜕𝑞6 ]
In order to calculate these partial Jacobian matrices, we can proceed on a similar way to the method for
calculate the Jacobian matrix for the inverse kinematics model, which was described above. So, we have:
𝑇
𝑑𝐹𝑗 ∆𝑥⃗𝑗𝑞1 ∆𝑥⃗𝑗𝑞2 ∆𝑥⃗𝑗𝑞6
𝐽𝐹𝑞(𝑗) ≈( ) [ ⋯ ]
𝑑𝑥⃗ ‖∆𝑞⃗1 ‖ ‖∆𝑞⃗2 ‖ ‖∆𝑞⃗6 ‖

The reader can note that the next equation is true according to the definition of the repulsive forces:
6

∆𝑞⃗𝑟𝑒𝑝 = − ∑ 𝐽𝐹𝑞(𝑗)
𝑗=1

Where the vector ∆𝑞⃗𝑟𝑒𝑝 define the necessary values to change in the 6 motors rotation values so as to
the current position of the robot don’t collide with the obstacle.

Finally, we have defined as the attraction model the Inverse Kinematics model. Therefore, the iterative
process will be defined by the rule:

𝑞⃗ 𝑛+1 = 𝑞⃗ 𝑛 + ∆𝑞⃗ 𝑛 + ∆𝑞⃗𝑟𝑒𝑝

𝑛

Then, the information architecture of the signals processing will be the showed in the figure 6.
Figure 6. Planned Inverse Kinematics model’s signal processing architecture.