PARTICLE SWARM OPTIMIZATION FOR INVERSE KINEMATICS SOLUTION AND TRAJECTORY PLANNING OF 7-DOF AND 8-DOF ROBOT MANIPULATORS BASED ON UNIT QUATERNION REPRESENTATION

Finding the inverse kinematic solution of a serial manipulator has always attracted the attention of optimization enthu-siasts, as the solution space is highly nonlinear and, depending on the number of degrees of freedom, has multiple solutions. In the literature, one can find several proposed solutions using heuristic techniques; however, for highly re - dundant manipulators, e.g., seven or more, the discussions focused on minimizing the positional error. In this paper, a metaheuristic approach is presented to solve not only the inverse kinematics of a 7 and 8 DOF manipulators but the proposed algorithm is used to find the robot’s poses for trajectory planning where the robot is required to meet the desired position and orientation based on quaternion representation of each point along the path. The metaheuristic approach used in this paper is particle swarm optimization (PSO), where the unit quaternion is used in the objective function to find the orientation error. The results prove that the use of the unit quaternion representation improved the performance of the algorithm and that our approach can be used not only for individual poses but for trajectory planning.


INTRODUCTION
The inverse kinematics (IK) problem is one of the most studied topics in robotics research. The problem consists of finding the joint angles for a desired position and orientation for the end-effector. Geometric, numerical, and algebraic methods have been used to solve the IK, however, these get more complex as the degrees of freedom (DOF) increase. Also, the emergence of redundant manipulators with seven or more DOF further complicates the problem since there are multiple solutions and postures that the robot can have for a specific pose of the end-effector; therefore, paving the way for researchers to develop and implement new solutions. Metaheuristics approaches, or in other words, search algorithms have gained traction and interest to solve the IK for redundant manipulators. [1], introduced a quantum-behaved particle swarm algorithm to minimize the position error of a seven DOF manipulator. This algorithm guarantees global convergence by utilizing a wave function instead of position and velocity variables. On the other hand, [2] proposed a combination of (PSO) and real coded genetic algorithm (RCGA) to solve the IK for a seven DOF manipulator. The hybrid algorithm was composed of all the basic operators of a genetic algorithm, such as mutation and crossover. [3], also proposed the (ABC) algorithm to minimize the position error for a seven DOF robot. ABC was able to minimize the position error better than the standard PSO but had the same execution time. [4], introduced a PSO algorithm for a six DOF manipulator that solved the IK for position and orientation in less than 16 iterations. [5], introduced the chaotic and parallelized artificial bee colony algorithm (CPABC), a modified version of the ABC algorithm to solve the IK for position and orientation for a seven DOF manipulator. CPABC outperformed the standard ABC algorithm by providing more stable solutions. [6], for a seven DOF robot, used joint parameterization to turn the problem into a one-dimensional problem. To find the solution for the lone joint angles, a combination of GA and PSO was used. To avoid local minima, [7], introduced a mutating PSO algorithm for a six DOF robot that detects if the algorithm is stuck in local minima with four new variables. Also, [8] utilized the firefly algorithm to solve for the position of a seven DOF robot, which had better performance in terms of error but not execution time. [9], developed an adaptive PSO algorithm that adapts the acceleration and inertia parameters to avoid getting stuck in a local optimum. Lastly, [10] utilizes PSO to find the desired position and orientation of a 6 DOF robot using a weighted RMSE objective function. Based on previous related work, the IK solution for 7 DOF robots focused on minimizing the position error but not the orientation error. For real applications, having a

Original Scientific Paper
Javier Alexis Abdor-Sierra, et al. -Particle swarm optimization for inverse kinematics solution and trajectory planning of 7-DOF and 8-DOF robot manipulators based on unit quarternion representation highly redundant robot reach the desired position with any orientation is not feasible and does not utilize its full potential. Also, most of the works besides [2] focus on reaching a certain pose or random poses but do not show if the approach is limited to a couple of poses or if it can be applied for trajectory planning. Furthermore, works based on minimizing the position and orientation error is studied on robot manipulators with 6 or less DOF using the rotation matrix in the objective function to minimize the rotation error. Therefore, the objective of this paper is to present a novel approach based on PSO and unit quaternions to solve the IK for 7 and 8 DOF robot manipulators. This approach will minimize not only the position error but the orientation error for a desired position and orientation of the end-effector. The unit quaternion plays a key role since the number of parameters for the orientation is reduced from nine to four, which has not been proposed in previous works along with metaheuristic approaches. In addition, our approach extends its application to not only solving for individual poses but for trajectory planning where the robot must describe a smooth trajectory along or between waypoints.

PARTICLE SWARM OPTIMIZATION
The particle swarm optimization (PSO) is a metaheuristic algorithm presented by [11], that is inspired by the social behavior of animals, most notably bird flocks and fish shoaling. In the wild, swarms of these animals work together to find resources for survival by communicating with each other and sharing information. This same principle is also applied to solving non-linear engineering problems, where a set of candidate solutions look through a search space, and with each iteration, a best new solution is obtained. Some advantages of PSO over other metaheuristic approaches are that it has few parameters to adjust and it has a fast convergence rate. To illustrate PSO, a swarm with n number of particles is initialized, where each particle is a possible solution to the optimization problem. Each particle in the swarm has a position and velocity vector that is updated every iteration with the following equations.
As shown in equation (1), the velocity vector is made up of parameters w, r 1 , r 2 , C 1 , and C 2 . Parameter w is the inertia constant that affects the particle's exploration, it is usually initialized at 1 and it decreases as the iterations increase to reduce exploration in the search space while the algorithm gets close to the optimal solution. On the other hand, parameters r 1 and r 2 are random uniform values from [0,1] that helps the algorithm avoid premature convergence. Lastly, C 1 and C 2 are acceleration coefficients that determine the influence of the local best position and the global best position, these are usually set to two [12]. Furthermore, as seen in equation (1), the difference between the particles best position P ij and the current position X ij is calculated and helps the particle stay close to its best position if it gets distant from it. Also, the difference between the global best position g j and the current position X ij is calculated, which attracts all the particles to that position.

KINEMATICS ANALYSIS
The robots used for this study are the Motoman SIA20D and Motoman SDA20D, both from the Japanese manufacturer Yaskawa. On one hand, the Motoman SIA20D is a seven DOF robot and the SDA20D is a fifteen DOF robot that has two SIA20D arms with a turntable. For this study, we will be working with the right arm and the turntable to have an eight DOF robot, both robots are shown in figure 1.

Figure 1: Robots used in this study
To solve the IK, the forward kinematics (FK) solution needs to be obtained. FK is the problem of finding the position and orientation of the end-effector with respect to the base given the joint angles. There are various methods to solve the FK, the most common being the Denavit-Hartenberg convention (DH). The DH convention relates two consecutive joints with the transformation matrix given in (3).
sgn s -a n -s -a + = sgn s -a n -s -a + sgn s -a n -s -a + for each joint are in degrees. While all the joint limits are the same for the robot arm, we consider θ 1 , the turntable angle for the SDA20D.
For each joint, the DH parameters are substituted in the matrix (3), resulting in seven and eight matrices for each respective robot. These matrices are multiplied in order by joints to obtain 0 A 7 and 0 A 8 which provides the position and orientation of the end-effector relative to the base of the robot. The resulting matrix is in the form: where n x , n y , n z , s x , s y , s z , a x , a y , a z denotes the orientation of the end-effector with respect to the base of the robot and p x , p y , p z denotes the position of the end-effector with respect to the base of the robot. In addition, since our approach makes use of unit quaternions, we convert the rotation parameters of the matrix (4) to a unit quaternion. A unit quaternion is another form to represent the orientation of a rigid body and provides an advantage over rotation matrices by having only four parameters. A quaternion is represented with a 4-tuple (q 0 , q 1 , q 2 , q 3 ) as: where η is the scalar part and ϵ=[ϵ x ϵ y ϵ z ] T is the vector part. Since the orientation is represented by a 3x3 matrix, we extract 6 from 4 and compute a unit quaternion from a matrix as follows: where Finally, by finding the homogenous transformation matrix (4) and converting the rotation matrix to a unit quaternion, we will be able to obtain the position and orientation of the end-effector given the set of the joint angles.

OPTIMIZATION APPROACH
As previously mentioned, the objective of this paper is to find the IK solution for 7 or 8 DOF robots, that is, find the joint angles for a desired position and orientation of the end-effector. The joint angles are obtained from the optimization algorithm which are then used to find the position and orientation using the FK. Therefore, we get two matrices, the desired matrix and the matrix that is being evaluated. Both matrices are expressed in the form of matrix (4) and are used to find the position and orientation error. To start, we first compute the position error by finding the Euclidean distance between the two positions using the following equation: where subscript d is the desired position and subscript c is the current position that is being evaluated. Next, we obtain the orientation error by converting both rotation matrices to a unit quaternion. Notice that by doing this conversion, the rotation is now represented by four parameters instead of nine which simplifies the problem significantly. Once we obtain the unit quaternion, we separate the orientation error into two parts, the scalar part, and the vector part. For the scalar part, we take the absolute difference of the scalar part of the desired orientation with the scalar part of the current orientation.
As for the vector part, we compute the magnitude of the vector part of the desired orientation with the vector part of the current orientation as follows: Next, we add the scalar and vector error and multiply it by 200 to give it weight in the cost function with respect to the position error, this weight was chosen experimentally. Finally, the total orientation error can be obtained with equation (13).
Lastly, the objective function to minimize the position and orientation error is expressed in equation (14) and the optimization problem can be described as: where, A d is the desired homogenous matrix and A c is the homogenous matrix that is being evaluated, obtained with the FK and output joint angles given by the algorithm.

PSO implementation for solving the inverse kinematics
The PSO algorithm was implemented in Matlab© on a personal laptop with an Intel Core i5, 2.40 GHz processor, and 8 GB of RAM. For the IK solution, each particle in the swarm is made up of a seven or eight-dimensional vector, which is initialized randomly within the joint limits shown in tables 1 and 2. The parameters set for the algorithm are the following: w=1, C 1 =C 2 =2, max gener-ations=300, swarm size=50, and velocity limiter=0.009. For the parameters r 1 and r 2 , these change every generation to a number from 0 to 1. Also, while the generations increase in the algorithm, we decrease w by 0.01 to minimize the exploration of each particle. Even though the values for w, C 1 , and C 2 are close to what is recommended in the literature, other values were manually tuned until achieving the desired performance. Furthermore, a trajectory from point A to point B is made up of intermediate poses of the tool plate, therefore, in the algorithm, we take the solution from the previous pose and insert it to the new swarm for the new pose, similar to the elitism operation from a genetic algorithm. This will help maintain the robot in the same posture throughout the trajectory and decrease the execution time. In addition, we limit the velocity vector by taking the difference between the maximum and the minimum angles for each joint and multiply it by 0.009, this will also help the algorithm stay close to the previous solution during the trajectory while improving the execution time. Finally, after updating the new position, we apply lower and upper bound limits to make sure that the solution stays within the robot's joint limits, the pseudo-code is shown in algorithm 1.

SIMULATION AND RESULTS
To validate the performance of the proposed algorithm, two square trajectories for each robot were generated. For the 7 DOF robot, each side of the square measures 250 mm with 25 intermediate points, meaning the whole trajectory is made up of 100 poses, the same type of trajectory is applied for the 8 DOF robot. The trajectory and initial configuration of the 7 DOF robot are shown in figure 2 and for the 8 DOF robot, it is shown in figure 3. As seen in figure 2 the robot needs to complete the trajectory outlined in red while maintaining its initial orientation. The results of the simulation for the 7 DOF robot can be seen in figure 4, where we selected ten poses from the trajectory, and it shows the robot maintaining the orientation of its end-effector and its posture throughout the trajectory. The same procedure was done with the 8 DOF robot, as seen in figure 5, ten poses were selected from the trajectory and it kept its initial orientation and posture for the whole trajectory. To validate the precision of the algorithm, 30 separate trajectory runs were made for both robots to obtain the execution time for the whole trajectory, the average orientation and position error of all 100 poses in the trajectory, and the average generations it takes to solve the IK for one pose, this is presented in tables 3 and 5. Moreover, tables 4 and 6 list the statistical analysis for each robot where we show the best and worst run, the average of all 30 runs combined, and the standard deviation.    In the case of the orientation error, both robots have an average error of 1.88E-03 and 2.61E-03 which can be considered zero. Additionally, the average position error for the 7 DOF robot is 8.79E-03 mm and the 8 DOF robot is 8.40E-03 mm which can be minimized even further by sacrificing execution time, however, since the repeatability factor given by the manufacturer is 0.1 mm, our solution is already well below that value. Moreover, the standard deviation data from tables 4 and 6 proves that the algorithm is finely tuned since the variation in the position and orientation error of all 30 runs is close to zero. In the case of the execution time, for both robots, the variation is around two seconds which is a good indication that the algorithm is consistent. Finally, to verify that the robot joints move smoothly throughout the trajectory and that there are not any large movements between each pose, figures 6 and 7 show the graph of the position of the joints during the trajectory. As it can be observed, the joints move from pose to pose smoothly not generating any large movements between poses that can affect a real robot.    On the other hand, to validate the effectiveness of using the unit quaternion, we tested the same algorithm 30 times for both robots but using the rotation matrix in the objective function. That is, to calculate the rotation error we take the absolute difference between every parameter of the desired matrix and the matrix that is being evaluated. Then we take the sum of all nine errors and multiply it by 60, this weight was also obtained experimentally . The results of all 30 runs are shown in table 7 for the 7 DOF robot and table 9 for the 8 DOF robot, the statistical analysis is presented in tables 8 and 10 for each respective robot. As it can be seen, for both robots the rotation and position error are similar to the unit quaternion approach. However, considering the average execution time, it significantly increased with the use of the rotation matrix. For the 7 DOF robot, the average execution time went up by 35 seconds which is an 89% increase. On the other hand, the average execution went up by 54 seconds for the 8 DOF robot, a 113% increase. With these results, it is obvious that the use of the unit quaternion in the objective function significantly decreases the execution time compared to the results given using the rotation matrix while also minimizing the desired orientation and position error.

CONCLUSIONS
In this paper, we presented a novel approach by employing PSO and unit quaternions to minimize both, the position and orientation errors to iteratively solve the IK problem of 7 and 8 DOF robot manipulators to describe the desired trajectory. First, the D-H parameters were obtained for each robot to solve the forward kinematics     error with the unit quaternion. The advantage of the objective function is that the unit quaternion simplifies the problem by having only four parameters to represent the orientation as opposed to the rotation matrix which has nine. To test the PSO algorithm, a simulation of two square trajectories were generated for each robot with 100 desired poses. The results show great performance as the average execution time for the trajectory for the 7 DOF robot was 39.52 seconds or 0.39 seconds per pose. Furthermore, the average rotation error was 1.88 E-03 which can be considered zero and the average position error is 8.79E-03 mm. On the other hand, the average execution time for the 8 DOF robot for a trajectory is 47.56 seconds or 0.47 seconds per pose, with an average rotation and position error of 2.61E-03 and 8.40E-03 mm for the latter. Also, based on the statistical analysis for both robots it shows that the algorithm is stable and well optimized. To show the advantage of using the unit quaternion representation, the same trajectories were generated but using the rotation matrix in the objective function. The results showed a similar average position and orientation error as with the quaternion approach, but the average execution time increased by 89% for the 7 DOF robot and 113% for the 8 DOF robot. Hence, making our approach feasible for applications like trajectory planning with redundant robots, which due to their dexterity provides flexibility in avoiding workspace obstacles.