**4. PSO and GA applications in robotic**

In this section some well-known problems in Robotic were solved with heuristic search algorithms. The first problem is the inverse kinematics problem for a nearly PUMA robot with 6 DOF, and the second problem is the path planning problem for mobile robots.

#### **4.1 The inverse kinematics problem for PUMA robot**

Robot kinematics refers to robot motions without consideration of the forces. The forward kinematics is finding the robot's end effector's position and orientation using the given robot parameters and the joint's variables. Also, the inverse kinematics is about finding the robot's joints variables while the robot's parameters and the desired position of the end effector are given (Kucuk & Bingul, 2006). The solution of the robot's forward kinematics is always possible and unique. On the other hand, generally, the inverse kinematics problem has several solutions (Kucuk & Bingul, 2006). The most known and used method for solving robot kinematics problems is Denavit-Hartenberg method. In this method several parameters, namely DH parameters, are defined according to robot's parameters and the replaced coordinate systems as given in Figure 8. These parameters are used to define transformations between the coordinate systems using 4x4 transformation matrices. The multiplication of all the matrices of the robot gives the final robot's end effectors' position and orientation (Kucuk & Bingul, 2006).

The selected robot for this example is a nearly PUMA type robot. Different from the general PUMA robot, the robot used in this example was equipped with an offset wrist. The inverse kinematics of the robot equipped with such a wrist is quite complicated and can be computationally cumbersome (Kucuk & Bingul, 2005). In this example, this problem was solved with GA and PSO. The robot's link parameters and coordinate systems replacements can be seen in Figure 9. Furthermore, according to Figure 9, the DH parameters of the robot can be defined as given in Table 1. Using the DH parameters, the forward kinematics of the robot can be solved easily using robot's transformation matrices. A transformation matrix is a 4x4 matrix and has the form as it is seen in equation 25.

According to the law of motion, particles try to move towards the heavier objects. The heavier masses represent good solutions and they move slowly for the exploitation. The

In this section some well-known problems in Robotic were solved with heuristic search algorithms. The first problem is the inverse kinematics problem for a nearly PUMA robot

Robot kinematics refers to robot motions without consideration of the forces. The forward kinematics is finding the robot's end effector's position and orientation using the given robot parameters and the joint's variables. Also, the inverse kinematics is about finding the robot's joints variables while the robot's parameters and the desired position of the end effector are given (Kucuk & Bingul, 2006). The solution of the robot's forward kinematics is always possible and unique. On the other hand, generally, the inverse kinematics problem has several solutions (Kucuk & Bingul, 2006). The most known and used method for solving robot kinematics problems is Denavit-Hartenberg method. In this method several parameters, namely DH parameters, are defined according to robot's parameters and the replaced coordinate systems as given in Figure 8. These parameters are used to define transformations between the coordinate systems using 4x4 transformation matrices. The multiplication of all the matrices of the robot gives the final robot's end effectors' position

The selected robot for this example is a nearly PUMA type robot. Different from the general PUMA robot, the robot used in this example was equipped with an offset wrist. The inverse kinematics of the robot equipped with such a wrist is quite complicated and can be computationally cumbersome (Kucuk & Bingul, 2005). In this example, this problem was solved with GA and PSO. The robot's link parameters and coordinate systems replacements can be seen in Figure 9. Furthermore, according to Figure 9, the DH parameters of the robot can be defined as given in Table 1. Using the DH parameters, the forward kinematics of the robot can be solved easily using robot's transformation matrices. A transformation matrix is

with 6 DOF, and the second problem is the path planning problem for mobile robots.

pseudo code of GSA is given in Figure 7.

Fig. 7. The pseudo codes of GSA

**4. PSO and GA applications in robotic** 

and orientation (Kucuk & Bingul, 2006).

**4.1 The inverse kinematics problem for PUMA robot** 

a 4x4 matrix and has the form as it is seen in equation 25.

d dd x (t 1) x (t) v (t) i ii (24)

Fig. 9. The nearly Puma robot's link parameters and coordinate systems replacement


Table 1. The DH parameters of the robot

Heuristic Optimization Algorithms in Robotics 325

The individual that offer the smallest distance is the most convenient candidate of the

**Oi** is object function value, **p** is penalty constant and **di** is the Euler distance calculated by

The problem was solved firstly using GA. The features of the GA was defined as follows: The gene coding type was determined as real-value coding and the selection type was determined as roulette wheel technique while the mutation and the crossover types were determined as one-point crossover and one-point mutation. The GA starts with defining the needed parameters, like crossover and mutation rates, number of individuals, number of iterations (stopping criteria). Then the initial population is defined randomly in the ranges of the constraints and the object and the fitness values of this population are calculated. In each iteration, the elitism, crossover and mutation operations are performed. The new population is generated after these operations and it masked to comply with the range of the constraints. And finally, the new generation's object and fitness values are calculated and the iterations proceeds until to reach the stopping criteria. In the end of the algorithm, the

solution. The object function for the problem can be formulated like in equation 29.

results are presented. The flowchart of the algorithm can be seen in Figure 11.

equation 28, for i'th individual.

Fig. 11. The flowchart of the GA

2 22

d xx yy zz ii i i ddd (28)

<sup>2</sup> O pd i i (29)

Where, T and R are 4x4 transformation and 3x3 rotation matrices between two consecutive links respectively while P is 3x1 position vector. Detailed information about defining this matrices and vectors can be found in (Kucuk & Bingul, 2006). The transformation matrices of the robot are as follows:

$$\begin{aligned} \;^0\_1T &= \begin{bmatrix} \mathcal{C}\theta\_1 & -\mathcal{S}\theta\_1 & 0 & 0\\ \mathcal{S}\theta\_1 & \mathcal{C}\theta\_1 & 0 & 0\\ 0 & 0 & 1 & h\\ 0 & 0 & 0 & 1 \end{bmatrix} \;^1\_2T = \begin{bmatrix} \mathcal{C}\theta\_2 & -\mathcal{S}\theta\_2 & 0 & 0\\ 0 & 0 & 1 & d\_1\\ -\mathcal{S}\theta\_2 & -\mathcal{C}\theta\_2 & 0 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \;^2\_3T = \begin{bmatrix} \mathcal{C}\theta\_3 & -\mathcal{S}\theta\_3 & 0 & l\_1\\ \mathcal{S}\theta\_3 & \mathcal{C}\theta\_3 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \\\;^3\_4T &= \begin{bmatrix} \mathcal{C}\theta\_4 & -\mathcal{S}\theta\_4 & 0 & l\_2\\ \mathcal{S}\theta\_4 & \mathcal{C}\theta\_4 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \;^4\_5T = \begin{bmatrix} \mathcal{C}\theta\_5 & -\mathcal{S}\theta\_5 & 0 & l\_0\\ 0 & 0 & 1 & 0\\ -\mathcal{S}\theta\_5 & -\mathcal{C}\theta\_5 & 0 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned} \tag{26}$$

In the above matrices *Ci* and *Si* (i=1,2,…,6) indicate cos *<sup>i</sup>* and sin *<sup>i</sup>* respectively. The forward kinematics of the robot is the forward multiplication of these matrices given by equation 27. The end effector's position in 3D space is defined with the last column of the result matrix.

$$T\_6^0 T = \_1^0 T \; \_2^1 T \; \_3^2 T \; \_4^3 T \; \_5^4 T \; \_6^5 T = T = \begin{bmatrix} R & P \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{27}$$

In this example, the inverse kinematics of the robot was solved using the most general type of GA and PSO. Before starting to solve the problem with an optimization algorithm, it is needed to define the problem, exactly. The problem presented in this example is finding the robot joint angle values while the robot's parameters and the end effector's position are given. According to problem it can be seen that one individual of the population should have six joint's constraints. They are 1 , <sup>2</sup> , <sup>3</sup> , <sup>4</sup> , 5 and <sup>6</sup> . So, an individual can be a 1x6 vector. Each component of the individual is for one of the joint's constraints. The range of the each constraint can be defined separately. However, for the sake of simplicity the ranges of the joints were defined between [-pi, pi]. A sample individual can be seen in Figure 10.

#### Fig. 10. A sample individual

The most important part of the solution is the object function, since the fitness value of an individual can be defined with the help of this function. In the example, the object function was defined using the desired position values of the end effector of the robot and the obtained position values from individuals using forward kinematic equations of the robot. The Euler distance between the desired position of the end-effector and the results obtained from an individual can be defined as follows.

Let the desired position of the end effector be *P x d ddd y z* and the obtained position knowledge using an individual be *P x i i ii y z* . The Euler distance between these two points in 3D space can be obtained as given in equation 28.

Where, T and R are 4x4 transformation and 3x3 rotation matrices between two consecutive links respectively while P is 3x1 position vector. Detailed information about defining this matrices and vectors can be found in (Kucuk & Bingul, 2006). The transformation matrices

1 1 2 2 33 1

*C S C S CS l*

 

00 0 0 1 0 0 0 01 0 0 0 0 10

 

 

 

*i* (i=1,2,…,6) indicate cos

forward kinematics of the robot is the forward multiplication of these matrices given by equation 27. The end effector's position in 3D space is defined with the last column of the

6 1 23456 0001

*T TTTTTT T*

In this example, the inverse kinematics of the robot was solved using the most general type of GA and PSO. Before starting to solve the problem with an optimization algorithm, it is needed to define the problem, exactly. The problem presented in this example is finding the robot joint angle values while the robot's parameters and the end effector's position are given. According to problem it can be seen that one individual of the population should have six joint's

component of the individual is for one of the joint's constraints. The range of the each constraint can be defined separately. However, for the sake of simplicity the ranges of the

The most important part of the solution is the object function, since the fitness value of an individual can be defined with the help of this function. In the example, the object function was defined using the desired position values of the end effector of the robot and the obtained position values from individuals using forward kinematic equations of the robot. The Euler distance between the desired position of the end-effector and the results obtained

Let the desired position of the end effector be *P x d ddd y z* and the obtained position knowledge using an individual be *P x i i ii y z* . The Euler distance between these two

joints were defined between [-pi, pi]. A sample individual can be seen in Figure 10.

0 0 01 0 0 0 1 0 0 01

0 0 0 0 0

6 5 6 6

*T*

0 0 01 0 0 0 1

*<sup>i</sup>* and sin

*R P*

0 0 0 0

*S C*

*C S*

 

 

0 0

*l*

*<sup>i</sup>* respectively. The

(27)

(26)

6 6 5 4

001

 

<sup>6</sup> . So, an individual can be a 1x6 vector. Each

 

2 2

01 2 1 1 1 33

*S C d SC*

5

*S C*

12 3

*TT T h SC*

44 2 5 5 3

*CS l C S l*

0 0 12345

*i* and *S*

0 0 00 0 0 10

of the robot are as follows:

In the above matrices *C*

result matrix.

constraints. They are

Fig. 10. A sample individual

3 4 4 4 4 5

 

*S C*

0 0 10 0 0 01

1 , <sup>2</sup> , <sup>3</sup> , <sup>4</sup> , 5 and

from an individual can be defined as follows.

points in 3D space can be obtained as given in equation 28.

*T T*

 

 

 

$$\mathbf{d}\_{\mathbf{i}} = \sqrt{\left(\mathbf{x\_{d}} - \mathbf{x\_{i}}\right)^{2} + \left(\mathbf{y\_{d}} - \mathbf{y\_{i}}\right)^{2} + \left(\mathbf{z\_{d}} - \mathbf{z\_{i}}\right)^{2}}\tag{28}$$

The individual that offer the smallest distance is the most convenient candidate of the solution. The object function for the problem can be formulated like in equation 29.

$$\mathbf{O}\_{\mathbf{i}} = \mathbf{p} \mathbf{d}\_{\mathbf{i}}^{\mathcal{D}} \tag{29}$$

**Oi** is object function value, **p** is penalty constant and **di** is the Euler distance calculated by equation 28, for i'th individual.

The problem was solved firstly using GA. The features of the GA was defined as follows: The gene coding type was determined as real-value coding and the selection type was determined as roulette wheel technique while the mutation and the crossover types were determined as one-point crossover and one-point mutation. The GA starts with defining the needed parameters, like crossover and mutation rates, number of individuals, number of iterations (stopping criteria). Then the initial population is defined randomly in the ranges of the constraints and the object and the fitness values of this population are calculated. In each iteration, the elitism, crossover and mutation operations are performed. The new population is generated after these operations and it masked to comply with the range of the constraints. And finally, the new generation's object and fitness values are calculated and the iterations proceeds until to reach the stopping criteria. In the end of the algorithm, the results are presented. The flowchart of the algorithm can be seen in Figure 11.

Fig. 11. The flowchart of the GA

Heuristic Optimization Algorithms in Robotics 327

The same problem was solved with PSO algorithm and the solutions were compared with GA. The flowchart of PSO algorithm that was used to solve the problem can be seen in

Figure 13 and, the parameters defined for PSO can be seen in Table 3.

ponent

Social

[0 0 0] [10 5 60] 50 2000 2 2 10 0.4 0.4 h=30; d1=5;

and the red is the found solution [-1.3725 -2.5852 1.8546 -0.9442 0.9538 0].

Component

Penalty

When the algorithm was run, the constraints were found in radian as [-1.3725 -2.5852 1.8546 -0.9442 0.9538 0] and the coordinates of the end effector calculated using these constraints were [9.9979 4.9941 60.0125]. According to these results it can be seen that PSO is found as it serves more convenient solution than GA's solution. In the Figure 14 the two solutions were depicted the blue one is the sample solution, [0 -pi/2 pi/2 –pi/2 pi/2 0]

The convenience of an optimization algorithm to a problem differs from one problem to another and it can only be determined by doing several trials and comparisons. There is no specific algorithm to achieve the best solution for all optimization problems. The comparisons are mainly related to the algorithm's final object function values and the execution time of the algorithm. In the final part of the example a comparison between GA

Min. inertia

wei

ght

Max. inertia

wei

ght

Link Lengths

l1=l2=l3=l4=10

Zero Position

Thetas

[0 0 0 0 0 0]

Fig. 13. The flowchart of the PSO algorithm

World Coordinates Goal Point Number of Population Number of Generation Cognitive Com

Table 3. The defined parameters for PSO

The numerical example given below is simply the realization of the algorithm. The robot's first link was replaced in the [0 0 0] coordinates in 3D space and the other link's coordinates were defined according to these coordinates. The aim of the example is to find the robot's joint angles (constraints) that are needed to replace the end-effector of the robot in the goal coordinates. The parameters used in the algorithms were given in Table 2. According to defined parameters, the zero position of the robot's end effector was calculated using zero thetas as [30 15 30]. The solution is the smallest Euler distance between the goal coordinates, [10 5 60], and the each of the individuals. One of the real solutions for this goal coordinates is [0 -pi/2 pi/2 –pi/2 pi/2 0]. The algorithm continues until the stopping criterion is met. In this study, the generation number is the stopping criterion. After running the GA, the constraints' values were found as [-1.0278 -0.5997 -0.6820 -0.8940 0.9335 0] as radian. The coordinates of the end effector can be calculated using these constraints as [9.8143 4.9310 60.0652]. In Figure 12, the two solutions were depicted, the blue one is the sample solution, [0 -pi/2 pi/2 –pi/2 pi/2 0] and the red is the found solution [-1.4238 -1.9043 -0.7396 1.5808 0.9328 0]. The error between the desired and the found end effector's position was [0.1857 0.0690 -0.0652] units.


Table 2. The defined parameters for GA

In this example the most general type of GA was used to solve the problem. It is obvious that more acceptable solutions can be found using different techniques and/or object functions in GA.

Fig. 12. The sample solution and the solution found with GA

The numerical example given below is simply the realization of the algorithm. The robot's first link was replaced in the [0 0 0] coordinates in 3D space and the other link's coordinates were defined according to these coordinates. The aim of the example is to find the robot's joint angles (constraints) that are needed to replace the end-effector of the robot in the goal coordinates. The parameters used in the algorithms were given in Table 2. According to defined parameters, the zero position of the robot's end effector was calculated using zero thetas as [30 15 30]. The solution is the smallest Euler distance between the goal coordinates, [10 5 60], and the each of the individuals. One of the real solutions for this goal coordinates is [0 -pi/2 pi/2 –pi/2 pi/2 0]. The algorithm continues until the stopping criterion is met. In this study, the generation number is the stopping criterion. After running the GA, the constraints' values were found as [-1.0278 -0.5997 -0.6820 -0.8940 0.9335 0] as radian. The coordinates of the end effector can be calculated using these constraints as [9.8143 4.9310 60.0652]. In Figure 12, the two solutions were depicted, the blue one is the sample solution, [0 -pi/2 pi/2 –pi/2 pi/2 0] and the red is the found solution [-1.4238 -1.9043 -0.7396 1.5808 0.9328 0]. The error between the desired and the found end effector's position was [0.1857

0.0690 -0.0652] units.

World Coordinates Goal Point Number of

functions in GA.

Population

Table 2. The defined parameters for GA

Number of

Fig. 12. The sample solution and the solution found with GA

Generation

Mutation

[0 0 0] [10 5 60] 50 2000 0.1 0.9 10 h=30; d1=5;

Rate

Crossover

In this example the most general type of GA was used to solve the problem. It is obvious that more acceptable solutions can be found using different techniques and/or object

Rate

Penalty

Link Lengths

l1=l2=l3=l4=10 [0 0 0 0 0 0]

Zero Position

Thetas

The same problem was solved with PSO algorithm and the solutions were compared with GA. The flowchart of PSO algorithm that was used to solve the problem can be seen in Figure 13 and, the parameters defined for PSO can be seen in Table 3.

Fig. 13. The flowchart of the PSO algorithm


Table 3. The defined parameters for PSO

When the algorithm was run, the constraints were found in radian as [-1.3725 -2.5852 1.8546 -0.9442 0.9538 0] and the coordinates of the end effector calculated using these constraints were [9.9979 4.9941 60.0125]. According to these results it can be seen that PSO is found as it serves more convenient solution than GA's solution. In the Figure 14 the two solutions were depicted the blue one is the sample solution, [0 -pi/2 pi/2 –pi/2 pi/2 0] and the red is the found solution [-1.3725 -2.5852 1.8546 -0.9442 0.9538 0].

The convenience of an optimization algorithm to a problem differs from one problem to another and it can only be determined by doing several trials and comparisons. There is no specific algorithm to achieve the best solution for all optimization problems. The comparisons are mainly related to the algorithm's final object function values and the execution time of the algorithm. In the final part of the example a comparison between GA

Heuristic Optimization Algorithms in Robotics 329

outperforms PSO. The second criterion is for the execution times of the algorithms. The obtained results, in seconds, were presented in Figure 16. In the Figure, it is obvious that the elapsed time of the PSO is much less than the GA's. As a result, it can be said that PSO produced better results than GA in terms of both final object function's values and the

The second example is about path planning for mobile robots. Path planning is one of the most studied topics related to mobile robots. It works for finding the shortest path between the start and goal points while avoiding the collisions with any obstacles. In the example,

Path planning can be classified in two categories, global and local path planning. The global type path planning is made for a static and completely known environment while the local path planning is required if the environment is dynamic (Sedighi et al., 2004). In this

Fig. 15. The object function values of the two algorithms

Fig. 16. The execution times of the two algorithms

**4.2 Path planning problem for mobile robots** 

this problem was solved using GA and PSO.

execution times.

Fig. 14. The sample solution and the found solution using PSO

and PSO for the inverse kinematics problem of the 6 DOF nearly puma robot was presented. For each problem, 100 runs were simulated using the same parameters. The final object function's values and execution times of the two algorithms were presented in a comparative manner using graphs and Tables. The defined parameters for both of the algorithms can be seen in Table 4.


Table 4. The parameters for GA and PSO for the inverse kinematic problem

Each algorithm was executed under the same condition and on the same computer, and according to the two criterions; the comparison was made between the two algorithms. The first criterion is about the algorithms' final object function values presented in Figure 15. The Figure indicates that both metaheuristic algorithms found the nearly optimum solutions to the problem. On the other hand it can be seen that PSO had more acceptable results than GA. In almost all executions PSO find the exact solutions. However, in 6 executions GA

and PSO for the inverse kinematics problem of the 6 DOF nearly puma robot was presented. For each problem, 100 runs were simulated using the same parameters. The final object function's values and execution times of the two algorithms were presented in a comparative manner using graphs and Tables. The defined parameters for both of the

Each algorithm was executed under the same condition and on the same computer, and according to the two criterions; the comparison was made between the two algorithms. The first criterion is about the algorithms' final object function values presented in Figure 15. The Figure indicates that both metaheuristic algorithms found the nearly optimum solutions to the problem. On the other hand it can be seen that PSO had more acceptable results than GA. In almost all executions PSO find the exact solutions. However, in 6 executions GA

Fig. 14. The sample solution and the found solution using PSO

**Parameters GA PSO**  Number of Individuals 100 100 Number of Iterations 1000 1000 Penalty 10 10 World Coordinates [0 0 0] [0 0 0] Goal Coordinates [10 5 60] [10 5 60] Gene Code Type Real Values Real Values

Mutation Rate 0.2 \*\*\* Crossover Rate 0.9 \*\*\* Selection Type R. Wheel \*\*\* Mutation Type One Point \*\*\* c1 \*\* 2 c2 \*\* 2 Wmin \*\* 0.4 Wmax \*\* 0.9 Table 4. The parameters for GA and PSO for the inverse kinematic problem

algorithms can be seen in Table 4.

Fig. 15. The object function values of the two algorithms

outperforms PSO. The second criterion is for the execution times of the algorithms. The obtained results, in seconds, were presented in Figure 16. In the Figure, it is obvious that the elapsed time of the PSO is much less than the GA's. As a result, it can be said that PSO produced better results than GA in terms of both final object function's values and the execution times.

Fig. 16. The execution times of the two algorithms
