Keywords

1 Introduction

Inverse kinematics [1] is the cornerstone for articulated robots in all daily life applications because all the rest of the robotic processes depends on its output. Articulated arm robot moves by giving joint input variables to the actuators [2], and accordingly, the tip of the arm moves in Cartesian space. This is called forward kinematic, and it is a straightforward operation that does not need serious computations or optimizations. We deal with the motion in Cartesian space because that what is desired for most of the application while the movement in the joint space still in the shadow. The most difficult process is when the inputs are the Cartesian coordinates, and the desired is to find the corresponding joint variables. This is called inverse kinematics which is what we usually deal with most of the robotic applications. In the automotive industry, robots follow a specific trajectory of Cartesian points to do some achievement like welding, cutting, grinding, painting, etc. [3]. Thus, inverse kinematics maps the motion of the tooltip (or just the extreme tip of the arm) from Cartesian space where the tip swims to the joint space where the actuators perform. It is worth mentioning that for simple topology robots, inverse kinematics can be simple and solved by many methods like geometric or analytical solutions. For a complicated high degree of freedom robots, the process will be hard or even impossible to be solved by traditional methods [4]. In this work, we are presenting the formulation of the objective function for inverse kinematics to be solved by any of the optimization algorithms. Dynamic annealed optimization algorithm (DDAO) [5], specifically, proposed to find the inverse solutions for robot arms. What is interesting in this algorithm is that it is independent of the population size, and that makes DDAO perfect for embedded systems applications as we will express in the respective sections.

2 DDAO

Dynamic differential optimization algorithm (DDAO) is a physically inspired optimization algorithm that mimics the process of production dual-phase steel. The mathematical model of the algorithm is expressed as follows:

$$ S^{k} \, = \,\left( {Sc_{i} \, - \,Sc_{j} } \right)\, + \,Sr.f $$
(1)
$$ f = \left\{ {\begin{array}{*{20}c} 1 & {if \, rem(iteration,2) = 1} \\ {random \, [0,1]} & {if \, rem(iteration,2) = 0} \\ \end{array} } \right., $$
(2)

where rem is the remonder after division on 2, we suggest the same procedure depending on the probability formula described by SA algorithm

$$ P = e^{{\frac{ - \Delta E}{T}}} , $$
(3)
$$ \Delta E = \frac{{Cost(S^{k} ) - Cost(S_{L} )}}{{Cost(S_{L} )}}, $$
(4)

where Sk is a new solution proposed for the iteration number (k), k = 1…n where n is the number of iterations, and Sci and Scj, are randomly chosen solutions from the population with random (i) and (j) indices. Sr is a randomly generated solution within the search space of the problem out of the population. P is the probability of accepting a new solution, \( \Delta E \) is the difference between the objective value of the proposed solution from Eq. (1) and the objective value of the solution SL, which is a solution of index L in the population, L = 1,…, population size. T is the temperature variable, which should start with high value and be updated during iterations constantly to a lower value. The proposed solution can be accepted if P > random number \( \in \left[ {0,1} \right] \). At the beginning of the search, T starts with high value; consequently, P will be close to one, according to Eq. (3). This means that a wide range of random numbers can be less that one and the solution will be selected. At the low value of T, the probability P will be close to zero; according to Eq. (3), this means that a very narrow range of random numbers could be less than P and the solution is less likely to be selected. The pseudocode illustrated below.

Initialize population Xi (i = 1,2,…,n) Initialize parameter T, cooling rate Calculate the cost of each solution Xb= The best solution While (t < Max iteration) Initialize sub-population S Calculate the cost of the sub-population Sort sub-population Sr= Best solution in sub-population Choose two random solutions Xm and Xn from population Calculate Sk from Eq. (1) Sort population X foreach solution in population X if there is an improvement Xi= Sk otherwise, replace the worst solution in population X using Eqs. (3) and (4) endif endfor Update Xb T = T*cooling rate t = t+1 endwhile return Xb

DDAO has a unique characteristic which is that it is independent of population size, this means that it uses the minimum size of the RAM when considering the population size of three individuals. Of course, other algorithms like particle swarm optimization [6], genetic algorithm [7], grey wolf optimization [8]. These algorithms can be used by setting population size as minimum as possible and what is the best algorithm for the inverse kinematic problem is left to the reader for future works.

3 Practical Example

In this study, LabVolt 5150 robot manipulator [9] is used to apply the proposed methodology of calculating the inverse kinematics using optimization algorithms. It is a 5 DOF manipulator; its rotational axes are base, shoulder, elbow, pitch and roller rotation, the manipulator equipped with a griper and all the revolute joints actuated by fife stepper motors. Figure 1 shows this robot manipulator, Fig. 2 expresses the configuration pace of the robot, and Fig. 3 reveals the frame assignment. According to the model shown in Fig. 2, the spatial parameters are estimated for each link, as shown in Table 1.

Fig. 1.
figure 1

Lab-volt 5150 manipulator

Full size image
Fig. 2.
figure 2

Operative ranges and specifications of lab-volt 5150

Full size image
Fig. 3.
figure 3

Operative ranges and specifications of lab-volt 5150

Full size image
Table 1. Spatial parameters of the lab-volt 5150 manipulator
Full size table

By using Denvit-Hartenberg convention [10], the homogenous transformation matrix HTM for the links are calculated as follows:

$$ H_{1}^{0} = \left[ {\begin{array}{*{20}c} {C_{1} } & 0 & {S_{1} } & 0 \\ {S_{1} } & 0 & { - C_{1} } & 0 \\ 0 & 1 & 0 & {d_{1} } \\ 0 & 0 & 0 & 1 \\ \end{array} } \right] $$
(5)
$$ H_{2}^{1} = \left[ {\begin{array}{*{20}c} {C_{2} } & { - S_{2} } & 0 & {a_{2} .C_{2} } \\ {S_{2} } & {C_{2} } & 0 & {a_{2} .S_{2} } \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} } \right] $$
(6)
$$ H_{3}^{2} = \left[ {\begin{array}{*{20}c} {C_{3} } & { - S_{3} } & 0 & {a_{3} .C_{3} } \\ {S_{3} } & {C_{3} } & 0 & {a_{3} .S_{3} } \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} } \right] $$
(7)
$$ H_{4}^{3} = \left[ {\begin{array}{*{20}c} {C_{4} } & 0 & {S_{4} } & 0 \\ {S_{4} } & 0 & { - C_{4} } & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} } \right] $$
(8)
$$ H_{5}^{4} = \left[ {\begin{array}{*{20}c} {C_{5} } & { - S_{5} } & 0 & 0 \\ {S_{5} } & {C_{5} } & 0 & 0 \\ 0 & 0 & 1 & {d_{5} } \\ 0 & 0 & 0 & 1 \\ \end{array} } \right] $$
(9)
$$ H = H_{1}^{0} \times H_{2}^{1} \times H_{3}^{2} \times H_{4}^{3} \times H_{5}^{4} $$
(10)

equation (10) is \( 4 \times 4 \) matrix holds the orientation and position vector of the end-effector with respect to the base frame, as revealed in Eq. (11) and Eq. (12).

$$ R_{5}^{0} = \left[ {\begin{array}{*{20}c} {S_{1} .S_{5} + C_{1} .C_{5} .C_{234} } & {S_{1} .C_{5} - C_{1} .S_{5} .C_{234} } & {C_{1} .S_{234} } \\ { - C_{1} .S_{5} + S_{1} .C_{5} .C_{234} } & {C_{1} .S_{5} + S_{1} .C_{5} .C_{234} } & {S_{1} .S_{234} } \\ {C_{5} .S_{234} } & {S_{5} .S_{234} } & { - C_{234} } \\ \end{array} } \right] $$
(11)
$$ P_{5}^{0} = \left[ {\begin{array}{*{20}c} {x = d_{5} C_{1} S_{234} + a_{2} C_{1} C_{2} + a_{3} C_{1} C_{23} } \\ {y = d_{5} S_{1} S_{234} + a_{2} C_{2} S_{1} + a_{3} C_{23} } \\ {z = - d_{5} C_{234} + a_{2} S_{2} + d_{1} + a_{3} S_{23} } \\ \end{array} } \right] $$
(12)

4 Objective Function

This is the problem of finding the joint variables from the given position and orientation of the end-effector. While forward kinematics is detecting the position and orientation of the end effector from the given set of joint variables, inverse kinematics is the inverse operation, but it is somewhat complicated.

$$ \left[ {\begin{array}{*{20}c} {\theta_{1} } \\ {\theta_{2} } \\ \vdots \\ {\theta_{n} } \\ \end{array} } \right]\begin{array}{*{20}c} \Rightarrow \\ \Leftarrow \\ \end{array} \left[ {\begin{array}{*{20}c} {r_{11} } & {r_{12} } & {r_{13} } & x \\ {r_{21} } & {r_{22} } & {r_{23} } & y \\ {r_{31} } & {r_{32} } & {r_{33} } & z \\ 0 & 0 & 0 & 1 \\ \end{array} } \right] $$
(13)

While forward equations are a straightforward process, we will rely on these equations to establish the objective function for the inverse problem.

Here we looking for an optimum set of joint variables that can lead to the minimum of a cost function, the only thing to do is developing cost function for the inverse Kinematic. Consider Fig. 4, for a specific robot configuration, the current position vector of the end-effector can be represented by the distance from the base of the end-effector of the manipulator while the desired position vector represents the task point. Obviously, if the difference between these two vectors is zero, then the tooltip will be in the right position at the task point, and this is the objective function f of the inverse problem

Fig. 4.
figure 4

Representation of the objective function for inverse kinematic problem

Full size image
$$ f = \left\| {Ci - De} \right\| $$
(14)

Where Ci denotes the instantaneous position vector, and De is the desired position vector. In other words, Eq. (14) is the function that has to be minimized as much as possible, and it just the distance between the end-effector and task point.

$$ f = \sqrt {(x_{Ci} - x_{t} )^{2} + (y_{Ci} - y_{t} )^{2} + (z_{Ci} - z_{t} )^{2} } $$
(15)

Where t refers to the task point coordinates which is given for the inverse kinematic problem.

If Eq. (15) has been used alone as an objective function, we may get the end-effector in the task point but with many choices of orientations.

5 The Procedure of the Objective Function

In this section, we shall model the objective function for the inverse kinematic of any robot manipulator. Figure 5 shows a schematic diagram for the inverse problem; it is more descriptive to explain the procedure by a set of notes as follows.

Fig. 5.
figure 5

Objective function scheme for inverse kinematic problem

Full size image
  1. 1.

    Optimization algorithm sent the candidate solution, which is a set of possible joint variables, to the cost function to evaluate its fitness.

  2. 2.

    Cost function contains the desire task point coordinates; it sends the possible solution to Forward function to get x, y, and z coordinates of the tooltip.

  3. 3.

    Forward function contains all the forward kinematic equations of the robot arm, by substituting the candidate solution to that equations we can get the overall homogenous transformation matrix by a repeated call for HTM function. The output of the Forward function is the position vector of the total transformation matrix.

  4. 4.

    Cost function will receive the position vector and apply Eq. (15) to the candidate vector and the desired task position vector. The result is the fitness of the solution that will be back to the main optimization algorithm.

This process is constant for all types of manipulators; the only thing to change is the forward kinematic equations and the task position vector. It is worth mentioning that not all optimization methods can return a guaranteed solution; this depends on the efficiency of the algorithm itself.

The implementation of this methodology has a great significance on robot autonation, consequently, facilate and increase the productivity especially in aumotive engineeering. The automotive enginnering has wide applications for robots where they can be used in many cases and many operations [11, 12].

6 Implementation of the Objective Function

The problem for LabVolt 5150 consists of five variables with lower and upper limits shown in Table 1, for a given point in space \( v \in R^{3} \) the objective is finding the best corresponding joint angles that drive the end-effector to that point. From optimization algorithm, the candidate solution (sol) is transferred to the cost function (cost):

figure a

For each candidate solution, we have to calculate the corresponding forward kinematic to find the position vector in Eq. (12) to be used in Eq. (15) to estimate the objective. Thus, the candidate solution v is transferred to the forward kinematic equations function (Forward). In this function, the spatial parameters are defined for each link considering data in Table 1:

figure b

For general usage, we have developed separated function (HTM) to return the homogenous transformation matrix described by Denavit matrix.

figure c

The source code described above is general, one can solve the inverse kinematic of any robot just by replacing the proper spatial parameters and define them in function (Forward). According to the number of links, less or more Denavit matrices can be estimated H_6,…, H_n. The function (HTM) is still valid for all types of robot manipulators without changes.

7 Conclusion

Optimization algorithms are proposed to find the solution for the inverse kinematic problem for robots of any type by optimizing the minimization objective function. The proposed optimization algorithm is dynamic differential annealed optimization, which is simple, fast, and uses low space of the memory of host machines or target devices. A practical example of 5DOF revolute joints manipulator, LabVolt 5150, was considered for the inverse problem. The described methodology is quite simple and can simplify the hard problem of inverse kinematic greatly and make it simple, straightforward operation. The proposed DDAO does not promise a perfect solution, and many other optimization algorithms should be tested to find a solution for the inverse problem, and that is proposed for future works.