DE-based Algorithm for Solving the Inverse Kinematics on a Robotic Arm Manipulators

DE algorithm method for Solving the inverse kinematics is a fundamental problem in the robotics field especially in controlling industrial robots to move following a pre-defined trajectory. In this paper, we proposed to use a meta-heuristic optimization algorithm, namely Differential Evolution (DE), to solve the Inverse Kinematic (IK) problem for a five-degree-of-freedom (DOF) articulated robot. By considering joints’ angles as continuous variables, the process of solving the Inverse Kinematic problem for the robot with the optimal algorithm has been significantly improved in terms of accuracy, execution time, standard deviation (STD) and the number of iterations needed as well. The algorithm has been applied to solve the Inverse Kinematic problem on the 5-Degree of Freedom 5R robot model. The simulation results for three case studies showed that our method can solve the inverse kinematics problem efficiently not only for minimum errors but also for smooth value of the joints’ variables.


Introduction
Solving the inverse kinematics is an essential step in the field of robotics [1] for controlling the robot following a pre-defined trajectory. The results after solving the IK problem are joints' values that would be used to control the robot's hand completing desired tasks. Solving IK problem is one of the most critical issues in robotics before considering bigger problems such as motion planning, dynamic analysis, and control [2]. For articulated robots, IK problem is often far more complicated than forward kinetics problems. The reason for the complexity of IK problem lies in nonlinear formulas and geometric properties between the working space and the joint space. Besides, they are difficulty in the singularity and non-unique solution property of those formulas. Solutions of mathematical equations for IK problems are sometimes not suitable for real solutions. There are several methods to solve the IK problem for an articulated robot such as geometric and iterative method. The geometric method based on geometric and trigonometric relationships to solve, while the iterative method is the method that often requires the inversion of a Jacobian matrix to solve the IK problem. These methods may have some disadvantages mentioned above. In addition to those existing methods for solving the IK problems, there are a new trend of studies solving IK problems for articulated robots by applying meta-heuristic optimization algorithms. The primary purpose of an optimal algorithm is to minimize or maximize an objective function, and some constraints to get the optimal solution for a given problem. Meta-heuristic optimization algorithms were applied to solve the IK problem as endpoints are in form of a single point or a whole trajectory [3]. The simulation results showed that the Particle swarm optimization (PSO) algorithm could solve the IK problem effectively. In [4], a Modified Artificial Bee Colony (ABC) was proposed and used to solve the IK problem of a 5 DOF robot arm manipulator. The simulations showed that PSO gave better results compared to the K-ABC in solving the helical path for the 5-DOF robot. The PSO algorithm has been used in various studies to solve the IK problem of industrial robots. In [5], five variants of PSO were used to solve the IK problem of the 2-DOF double link system using forward kinematic formulas. Simulation results showed that inertia weight PSO performed better than the four other variants. However, the study just applied for a low DOF case. In [6], the authors applied PSO to solve the 6DOF of IK problems. The result showed the effectiveness of applying the metaheuristic optimization methods to IK. However, the limit of this research is only solving IK problems for a single point (pose). In [7], the authors used a method of simulation techniques to determine the robot kinematics motion systems. In the paper [8], the author compared four different heuristic optimization algorithms for the IK solutions of a real 4-DOF serial robot manipulator. Four optimal algorithms were used in the paper: Genetic Algorithm (GA), PSO, Quantum Particle Swarm (Q-PSO), and Gravitational Search Algorithm (GSA). The robot was moved in the workspace in two different scenarios. The study shows that GA and GSA were found to be unsuitable for solving the IK problem. Besides, the study demonstrated that PSO and QPSO algorithms gave acceptance results in terms of the execution time and convenience of the solution. However, the study focused on optimal parameters such as execution time, the number of generations, and distance error without considering the value of joints' variables as the robot moves following a pre-defined trajectory. This can lead to infisible solutions that waste search resources. The reason is that joints' variables for continuous endpoints are dramatically changed. Therefore, in reality, it is difficult for the control algorithm to meet this requirement. The reason of this phenomenon is the multi solutions feature of the IK for articulated robots. At the same time, the optimal algorithm search is randomly generated in the robot workspace. In this research, we proposed the new method for solving the IK problem for a 5-DOF manipulator by using the DE algorithm. The DE algorithm was used to solve the IK problem where the end-effectors points move along a given spiral path. The objective function is amount of total energy that the robot needs to move the robot's hand from the current point to the next point of the orbit with the constraints of distance error's endpoints as well as the smallest number of moving joints. When applying these objective functions and the limit range for joints' variables, they ensure the continuity, reducing the search domain to speed up the convergence speed. The rest of the paper is divided into the following sections: -Section 2 describes the background on Kinematic problem and the review of DE. -Section 3, the proposed algorithms for path learning, and the case studies are presented. -Section 4, the results after applying the algorithm are shown and compared. -Finally, conclusions and development directions are outlined in section 5.

Kinematic problem
In the forward kinematics stage, the position and orientation of the end-effector will be calculated for a set of as given value of the angle revolute joint and transmit prismatic joints. In this study, a serial redundant robot manipulator was used to evaluate the algorithm. The robot model and coordinate systems were shown in the Fig.1. As in the figure, the 5-DOF serial robot manipulator is type 5R (R: Revolute). The standard Denavit-Hartenberg (DH) method is used in this research to find the homogeneous transformation matrix [9] [10]. Using the DH parameters in table 1, the homogeneous transformation matrix is calculated as in Eq. (1) and Eq. (2). The Eq. (3) is used to find the position and orientation of the end.
Where S and C denote the sine and cosine functions. The Homogeneous transformation matrix is calculated as in Eq. (2) The position (pose) and orientation of the end-effector: C q * l *C q + l *C q + l *C q + l *Sq Sq * l *C q + l *C q + l *C q + l *Sq l + l *Sq + l *Sq + l *Sq -l *C q Where: S and C denote the sine and cosine functions, li the length of each robot link l=[l1 l2 l3 l4 l5], q the joint variables q=[q1 q2 q3 q4 q5], qij is the sum of qi and qj, q23=q2+q3, and q234=q2+q3+q4. When solving the IK problem for a trajectory, with the endpoint position of each point on the trajectory, we need to determine a matching set of the value of joints' variables qi. However, according to the formula in Eq. (4), the number of equations will be less than the number of variables (three equations and five variables); this makes it very difficult to find a unique and exact match solution. In this research, we propose the new method that we will use the DE algorithm with search domain improvement to find joints' variable values to ensure continuity as well as improve the optimization process.

Working space of the robot
The range of all joints' variables is given as in Table 2 and the working spaces of the robot are described as in the Fig.2.

DE algorithm
To find the solution to the constrained optimization problems in this paper, we will apply a metaheuristic, namely DE algorithms. The reason why we chose DE is that this algorithm is the evolutionary-based method that can balance the local search and global search. Therefore, DE can provide suitable solutions. DE is initially developed by Storn and Price [11] based on the theory of evolution.
The DE algorithm applied the principle of mutation, crossover, and selection of human populations to solve problems. The core idea behind this algorithm is that the more fitness, the more change to survive of individuals in the social collection. The generation will repeat until the terminal condition is met.

Proposed algorithm and objective function
As explained above, the drawback of many studies when using optimization algorithms to solve the IK problem is to focus only on the optimal running process such as computer time, the error distance, etc ... Nevertheless, they have not yet considered the continuity of the joints' variable values found. This research will focus on improving the continuity of these values by limiting the initial initialization domain of particles. Since then, the program can achieve the dual goal of increasing calculation speed, accuracy, and ensuring continuity for the value of joints' variables. The objective function is the total amount of energy that the robot spends to move end-effector from point j to point j + 1 as shown in the Eq. (5), while the constrain will be the error distance, as shown in the Eq. (6), and finally, the limit of joint angle variables as in Eq. (7).
Objective function: Constraints: C q * l *C q + l *C q + l *C q + l *Sq 0 Sq * l *C q + l *C q + l *C q + l *Sq 0 l + l *Sq + l *Sq + l *Sq -l *C q 0 Where:  i the moment that causes the movement of each joint i from point j to point j+1 on the trajectory (xE, yE, zE) is the endpoint coordinates on the trajectory.
To solve the non-linear constrained optimization problem above Eq. (5,6,7), we will apply the DE algorithm to combine with the penalty method in this research. The proposed new method is shown in Fig. 3. In this algorithm, firstly, the robot hand moves from any position to the first point of the trajectory. With this first point, the initialization values for the particles are randomly selected in the full range of motion of joints. After that, the forward kinematic model is used to determine the endeffector coordinate, and finally, the objective function and some constrained functions are calculated. The primary operator of DE will repeat until the terminal condition reached, and the optimum value of the joints angle for the first point is determined. The algorithm will move to the next end-effector, in this step some parameters such as the initial angle joint q0i and the boundary need to be updated, this process repeats for all end-effector points on the trajectory. One the algorithm finishes, the optimum solution set of joint angle values is determined.

Case study
The manipulator is required to move the end-effector following a specific trajectory. The path was selected as a spiral trajectory shown in Fig.4 Where: (xE, yE, zE) is the coordinates of the endpoints on the trajectory. In this study, the program will run in the following cases:

Case 1: The method used in this case (conventional method), as shown below:
The boundary of all joint variables is set to a full range of link movement as in Eq. (9).
The particle initialization domain is fixed for all points on the trajectory as in Eq. (10).
Where j=1…n, n is the number of endpoints that belong to the trajectory, i=1…5 is the joint variable's index.

Case 2: In this case ((proposed method 1)), we focus on reducing the total amount of energy that the robot spends to move end-effector from point j to point j + 1
-The boundary of each joint's variables is limited to the workspace of each joint same as case 1.
-The particle initialization domain is around the previous joint variable, as in Eq. (12). By proposed this method, the energy consumption can be reduced in comparison with case 1.
Where j=1…n, n is the number of endpoints that belong to the trajectory, i=1,…,5 is the joint variable's index. q is the fixed value of the angle.

Case 3: In this case (proposed method 2), we control the boundary of all variables adaptively.
The purpose of this work is to control the continuity of the joint's values during the movement of the robot manipulator follow the trajectory.
-The boundary of each variable is limited to the previous trajectory's points, as shown in Eq. (9).
Where j=1…n, n is the number of endpoints that belong to the trajectory, i=1…5 is the joint variable's index.
-The particle initialization domain is updated for all points on the trajectory as in Eq. (12). The purpose of solving case 2 and case 3 is to compare the proposed solution with the conventional method (case 1) when applying DE to solve IK problems. In particular, the difference between case 2 and case 3 is that the initial points in the trajectory were generated fix and dynamically, respectively.

Discusion on the Simulation Results for the Case Studies
In order to compare the proposed methods with the conventional method in this research, we will use the DE algorithm of scheme DE/best/2. The algorithm will repeat the main operator until the distance errors equal to or less than the specific accuracy of 1.0E-3. In Table 3, F and CR are weight factors and Crossover ratio, respectively. NP is the population size. Iter_max is the maximum number of iterations. The three case studies were programed on MATLAB R2019a with the same condition of paramter on Window10 operating system with hardware configuration as Intel® Core™ i7-7700CPU@ 3.6GHz. In the case 3 the boundary of each joint's variables is set to /4 (rad). The results of the appling DE for all cases are given in Fig. 5 to Fig.7 and Table 4. It can be seen that the DE algorithm produced appropriate solutions for all of the points belonging to the trajectory in all cases. However, As seen in the Table 4 and Fig.6 and Fig.7 the avarage energy comsumption, execution CPU time, iteration in case 1 is much more than the other cases. The main reason for this was that in the case 2 and case 3 we update the values for all points on the trajectory as in Eq. (12), by proposed this techneque, the energy consumption can be reduced dramaticlely. More specifically, as seen in the Fig.5 the case 3 achieved the solution of the of joint's values continuity. While the case 1 and case 2 got the solution the joints' value changed dramatically. Among these angular values, only the value of the q5 joint variable has a sudden change. The reason for this phenomenon is that q5 joints do not affect the endpoint position accuracy shown in Eq. (4).  Table 4 reports the results of the average error, the standard deviation of the error (STD), the average iteration, and the average CPU execution time and energy comsumption of each case. As in the table, the proposed algorithm performs better results with the average error, the average iteration, and the average execution CPU time.

Conclusions
In this research, we proposed a new method for solving the Inverse Kinematics of 5 DOF-serial manipulator robots by using DE and proposed new methods. The 5-DOF robot was applied to move in the spiral trajectory in three different cases for validating the new proposed algorithm. The simulation results were compared in the three-case study by using the MATLAB robotics toolbox. In case 1 (convention method), 100 equidistant continuous points were generated in the path, and the DE solved the problem. In case 2 (proposed method), the boundary of each joint's variables is limited to the previous trajectory's points, and the initialization domain is fixed as same as in case 1. In case 3 (proposed method), the boundary of each joint's variables is limited to the previous trajectory's points, and the particle initialization domain is around the previous joint variable. The simulation results showed that the proposed method (case 2 and case 3) could give proper solutions for the inverse kinematics problem for a path in all aspects such as accuracy, computer time, especially the continuity of joints angle for all end-effector points. This proposed algorithm can be used for controlling the robot effectively because the robot can avoid the sudden change in the value of joints angle during the moment of a robot. In future work, this algorithm could be extended to solving more complex path in which the robot not only to move following a path but also the robot has to avoid collision with obstacles during the moving.