A 6-DOF robot-time optimal trajectory planning based on an improved genetic algorithm

Abstract

By using quintic polynomial function to interpolate several given points of each joint of the robot, the mathematical expressions of each joint variable of the robot with time are established. In addition, to improve the search algorithm performance crossover operator and mutation operator of the genetic algorithm are improved in cosine form. Furthermore, the improved adaptive genetic algorithm is applied to optimize the time interval of interpolation points of each joint, so as to realize time optimal trajectory planning. Moreover, MATLAB simulation is carried out, and the results show that the method proposed in this paper reduces the running time of the robot tasks. Meanwhile, the curves of angle position, velocity and acceleration of each joint are smooth enough, which ensure accomplish its tasks in a stable and efficient way.

Background

Robot trajectory planning usually refers to track points given several expectations and target pose, and timely adjust the rotation angle of each joint of the robot to the end effector at a prescribed trajectory followed by each point to eventually reach the target point. The trajectory planning in joint space is simpler and convenient than that of Cartesian space. Therefore, several fixed points which located at the end of several robotic arms are usually given. Then, these track points for the robot are computed by using the inverse kinematics so as to convert it from Cartesian space to joint coordinate space. Next, the track points are used to carry out interpolation operation using various spline functions, polynomial functions or other forms of curves, and the expressions about the time of each joint variable for the robot are obtained. In addition, in light of the mechanical characteristics of the robot, the speed and acceleration of each joint should be limited to the allowable range. Therefore, it is necessary to optimize the speed and acceleration of each joint arm, not only to ensure the smooth operation of the joint arm but also to reduce the wear and impact to prolong the working life of the robot.

The method of optimal trajectory planning generally includes time optimal trajectory planning [1,2,3], energy minimum trajectory planning [3, 4] and impact minimum trajectory planning [5], or multi-objective trajectory optimization combining these estimation schemes. Among them, the optimal trajectory planning with the robot running time as main consideration is favored by many scholars. In recent years, many researchers have made some achievements in the field of robot trajectory planning. Tohfeh and Fakharian [6] constructed a function expression for the omnidirectional robot’s energy dissipation by combining obstacle avoidance performance, and the optimization problem was transformed into a parameter minimization problem by the polynomial interpolation method, which provided a more effective way for the study of robot obstacle avoidance. However, due to the complexity of this method, there is a certain degree of difficulty in practice. Bende [7] studied a method of modeling underwater robot based on bond graph theory and optimized the model parameters with the genetic algorithm to obtain the optimized trajectory of the robot. Experiments were carried to show the significance of this method. Zhu and Liu [8] applied the seventh-order B-spline curve for the interpolation operation of robot’s articulation arm trajectory and applied the sequential quadratic programming for the trajectory planning, which achieved the optimal planning goal and the angular displacement, velocity and acceleration curve of each joint of the robot are smoother.

In this paper, from the point of view of robot running time, the trajectory of robot joint is planned by the quintic polynomial interpolation under the premise of smooth operation of the manipulator, and the time interval of trajectory interpolation point is optimized by the improved adaptive genetic algorithm, so that the robot accomplishes task time as short as possible.

Model

In this paper, we consider PUMA560 robot as the research object, which belongs to a conventional six-arm-type robot. Its first three joints are used to determine the position of the robot end effector, and the latter three joints are used to determine its attitude. In order to make it easier to analyze the motion of articulation arm of the robot, it is necessary to establish the kinematic equation of the robot. The link pole coordinate system is established by D-H method, and through the translation, rotation and other coordinate transformation to establish the relationship between the coordinate system. The link pole coordinate system is shown in Fig. 1.

$$ \begin{aligned} {}^{0}T_{1} = \left[ {\begin{array}{*{20}c} {c\theta_{1} } & { - s\theta_{1} } & 0 & 0 \\ {s\theta_{1} } & {c\theta_{1} } & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} } \right]\quad {}^{1}T_{2} = \left[ {\begin{array}{*{20}c} {c\theta_{2} } & { - s\theta_{2} } & 0 & 0 \\ 0 & 0 & 1 & {d_{2} } \\ { - s\theta_{2} } & { - c\theta_{1} } & 0 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} } \right] \hfill \\ {}^{2}T_{3} = \left[ {\begin{array}{*{20}c} {c\theta_{3} } & { - s\theta_{3} } & 0 & {a_{2} } \\ {s\theta_{3} } & {c\theta_{3} } & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} } \right]\quad {}^{3}T_{4} = \left[ {\begin{array}{*{20}c} {c\theta_{4} } & { - s\theta_{4} } & 0 & {a_{3} } \\ 0 & 0 & 1 & {d_{4} } \\ { - s\theta_{4} } & { - c\theta_{4} } & 0 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} } \right] \hfill \\ {}^{4}T_{5} = \left[ {\begin{array}{*{20}c} {c\theta_{5} } & { - s\theta_{5} } & 0 & 0 \\ 0 & 0 & { - 1} & 0 \\ {s\theta_{5} } & {c\theta_{5} } & 0 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} } \right]\quad {}^{5}T_{6} = \left[ {\begin{array}{*{20}c} {c\theta_{6} } & { - s\theta_{6} } & 0 & 0 \\ 0 & 0 & 1 & 0 \\ { - s\theta_{6} } & { - c\theta_{6} } & 0 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} } \right] \hfill \\ \end{aligned} $$

(1)

Fig. 1

The link pole coordinate system of PUMA560 robot according to the link pole coordinate system of Fig. 1, the transformation matrix of each link can be obtained as follows

Multiplying the above transformation matrice in turn, the comprehensive transformation matrix of the PUMA560 robot is obtained:

$$ \begin{aligned} {}^{0}T_{6} & = {}^{0}T_{1} (\theta {}_{1})^{1} T_{2} (\theta {}_{2})^{2} T_{3} (\theta {}_{3})^{3} T_{4} (\theta {}_{4})^{4} T_{5} (\theta {}_{5})^{5} T_{6} (\theta {}_{6}) \\ & = \left[ {\begin{array}{*{20}c} {n_{x} } & {o_{x} } & {a_{x} } & {p_{x} } \\ {n_{y} } & {o_{y} } & {a_{y} } & {p_{y} } \\ {n_{z} } & {o_{z} } & {a_{z} } & {p_{z} } \\ 0 & 0 & 0 & 1 \\ \end{array} } \right] \\ \end{aligned} $$

(2)

where the third-order sub-matrix $\left[ {\begin{array}{*{20}c} {n_{x} } & {o_{x} } & {a_{x} } \\ {n_{y} } & {o_{y} } & {a_{y} } \\ {n_{z} } & {o_{z} } & {a_{z} } \\ \end{array} } \right]$ represents the posture of the end effector relative to the base system {0}, and $\left[ {\begin{array}{*{20}c} {p_{x} } \\ {p_{y} } \\ {p_{z} } \\ \end{array} } \right]$ represents its position relative to the base system {0}.

Trajectory planning analysis

The number of desired track points of the robot end effector in the Cartesian space is transformed into the corresponding joint variables by the kinematic inverse operation, and then, track points of the joint space are interpolated to obtain the joint variables of the robot a function of time-varying expression [9]. In this paper, the quintic polynomial is used to interpolate trajectory points in the robot joint space.

Denote by $\theta (t)$ the joint angle. Assume $\theta (t_{0} )$ = $\theta_{0}$, $\theta (t_{f} )$ = $\theta_{f}$. Obviously, there are several quintic polynomial curves satisfying the above conditions, as shown in Fig. 2.

Fig. 2

Quintic polynomial function of the same starting point and end point

It is now necessary to find a smooth curve with starting point $\theta_{0}$ and ending point $\theta_{f}$. In addition, the expression of the quintic polynomial is:

$$ \theta (t) = a_{0} + a_{1} t + a_{2} t^{2} + a_{3} t^{3} + a_{4} t^{4} + a_{5} t^{5} $$

(3)

At the start and the end points, the displacement constraint, speed constraint and acceleration constraint are expressed in (4)–(6), respectively:

$$ \left\{ {\begin{array}{*{20}l} {\theta (0) = \theta_{0} } \hfill \\ {\theta (t_{f} ) = \theta_{f} } \hfill \\ \end{array} } \right. $$

(4)

$$ \left\{ {\begin{array}{*{20}l} {\theta^{{\prime }} (0) = \theta_{0}^{{\prime }} } \hfill \\ {\theta^{{\prime }} (t_{f} ) = \theta_{f}^{{\prime }} } \hfill \\ \end{array} } \right. $$

(5)

$$ \left\{ {\begin{array}{*{20}l} {\theta^{{\prime \prime }} (0) = \theta_{0}^{{\prime \prime }} } \hfill \\ {\theta^{{\prime \prime }} (t_{f} ) = \theta_{f}^{{\prime \prime }} } \hfill \\ \end{array} } \right. $$

(6)

Deriving Eq. (3), the velocity expression of the robot’s trajectory is obtained as:

$$ \theta^{{\prime }} (t) = a_{1} + 2a_{2} t + 3a_{3} t^{2} + 4a_{4} t^{3} + 5a_{5} t^{4} $$

(7)

Similarly, by means of the second derivative of t in formula Eq. (6), we can get the acceleration function as follows:

$$ \theta^{\prime \prime } (t) = 2a_{2} + 6a_{3} t + 12a_{4} t^{2} + 20a_{5} t^{3} $$

(8)

Combining Eqs. (4), (5) and (6), we obtain the coefficients of the quintic polynomial as follows:

$$ \left\{ {\begin{array}{*{20}l} {a_{0} = \theta_{0} } \hfill \\ {a_{1} = \theta_{0}^{{\prime }} } \hfill \\ {a_{2} = \frac{{\theta_{0}^{{\prime \prime }} }}{2}} \hfill \\ {a_{3} = \frac{{20\theta_{f} - 20\theta_{0} - (12\theta_{0}^{{\prime }} + 8\theta_{f}^{{\prime }} )t_{f} - (3\theta_{0}^{{\prime \prime }} - \theta_{f}^{{\prime \prime }} )t_{f}^{2} }}{{2t_{f}^{3} }}} \hfill \\ {a_{4} = \frac{{30\theta_{f} - 30\theta_{0} + (16\theta_{0}^{{\prime }} + 14\theta_{f}^{{\prime }} )t_{f} + (3\theta_{0}^{{\prime \prime }} - 2\theta_{f}^{{\prime \prime }} )t_{f}^{2} }}{{2t_{f}^{4} }}} \hfill \\ {a_{5} = \frac{{12\theta_{f} - 12\theta_{0} - (6\theta_{0}^{{\prime }} + 6\theta_{f}^{{\prime }} )t_{f} - (\theta_{0}^{{\prime \prime }} - \theta_{f}^{{\prime \prime }} )t_{f}^{2} }}{{2t_{f}^{5} }}} \hfill \\ \end{array} } \right. $$

(9)

Introducing the above factors into Eq. (3), the robot trajectory equation of the quintic polynomial can be derived.

Improvement in genetic algorithm

The basic knowledge of the kinematics of the robot and the basic principle of the interpolation of the trajectory points of the joint space by the quintic polynomial have been introduced in the preceding narrative. On this basis, it is necessary to optimize the time interval of the interpolation point of the track point by using the genetic algorithm to realize the optimal planning of the robot time and further make the angular displacement, velocity and acceleration curve of each joint movement smoother.

The genetic algorithm, which is an artificial intelligence optimization algorithm for simulating the genetic and evolutionary processes in nature [10,11,12], owns the characteristics of simplicity and robustness, and starts from the parallel solution of the problem (rather than a single solution). Therefore, it not only has excellent global optimization but also is used in the optimization process of practical problems [13].

However, the crossover operator and mutation operator of a simple genetic algorithm are invariant in the process of algorithm implementation and do not satisfy the dynamic requirement of biological evolution. To this end, Ren and San [14] proposed an adaptive genetic algorithm according to the individual fitness of different dynamic adjustment of crossover probability and mutation probability. The adjustment of the crossover and mutation probabilities is shown in Eqs. (10) and (11), respectively:

$$ p_{c} = \left\{ {\begin{array}{*{20}l} {p_{c1} - \frac{{(p_{c1} - p_{c2} )(f^{{\prime }} - f_{\text{avg}} )}}{{f_{\max} - f_{\text{avg}} }},} \hfill & {f^{{\prime }} \ge f_{\text{avg}} } \hfill \\ {p_{c1} ,} \hfill & {f^{{\prime }} < f_{\text{avg}} } \hfill \\ \end{array} } \right. $$

(10)

$$ p_{m} = \left\{ {\begin{array}{*{20}l} {p_{m1} - \frac{{(p_{m1} - p_{m2} )(f^{{\prime }} - f_{\text{avg}} )}}{{f_{\max} - f_{\text{avg}} }},} \hfill & {f \ge f_{\text{avg}} } \hfill \\ {p_{m1} ,} \hfill & {f^{{\prime }} < f_{\text{avg}} } \hfill \\ \end{array} } \right. $$

(11)

where $p_{c1} = 0.9$, $p_{c2} = 0.6$, $p_{m1} = 0.1$, $p_{m2} = 0.01$ $f^{{\prime }}$ is the parent of the larger fitness value; $f_{\text{avg}}$ is the average fitness value in the population; $f_{\max}$ is the fitness value of the largest individual in the current population.

Equation (10) shows that a larger fixed crossover probability is given when the fitness value of the cross parent is small. And the greater the fitness of the two chromosomes, the smaller the crossover probability is. The adjustment of the mutation probability in Eq. (11) is consistent with the crossover probability.

It is noted that the above method has two drawbacks. Firstly, the crossover probability and the mutation probability are fixed for the parent chromosomes whose fitness is lower than the average fitness of the population. Secondly, the adjustment of crossover and mutation probability is linear, and it can not meet the objective of population evolution. To overcome the two drawbacks, this paper presents an adaptive genetic algorithm that uses a cosine function to adjust crossover probability and mutation probability. Adjusting Eqs. (10) and (11) as follows:

$$ p_{c} = \left\{ {\begin{array}{*{20}l} {\frac{{p_{c0} + p_{c\,{{\rm min}} } }}{2} + \frac{{p_{c0} - p_{c\,{{\rm min}} } }}{2}\cos \left( {\frac{{f - f_{\text{avg}} }}{{f_{\max} - f_{\text{avg}} }}\pi } \right),} \hfill & {f \ge f_{\text{avg}} } \hfill \\ {\frac{{p_{c0} + p_{c \max} }}{2} + \frac{{p_{c0} - p_{c \max } }}{2}\cos \left( {\frac{{f_{\text{avg}} - f}}{{f_{\text{avg}} }}\pi } \right),} \hfill & {f < f_{\text{avg}} } \hfill \\ \end{array} } \right. $$

(12)

$$ p_{m} = \left\{ {\begin{array}{*{20}l} {\frac{{p_{m0} + p_{m\,{{\rm min}} } }}{2} + \frac{{p_{m0} - p_{m\,{{\rm min}} } }}{2}\cos \left( {\frac{{f^{\prime } - f_{\text{avg}} }}{{f_{\max } - f_{\text{avg}} }}\pi } \right),} \hfill & {f^{\prime } \ge f_{\text{avg}} } \hfill \\ {\frac{{p_{m0} + p_{m \max } }}{2} + \frac{{p_{m0} - p_{m \max } }}{2}\cos \left( {\frac{{f_{\text{avg}} - f^{\prime } }}{{f_{\text{avg}} }}\pi } \right),} \hfill & {f^{\prime } < f_{\text{avg}} } \hfill \\ \end{array} } \right. $$

(13)

where $p_{c\,{{\rm min}} }$ and $P_{c \max }$ are the minimum and the maximum probability, respectively. $p_{c\,{{\rm min}} } \le p_{c0} \le p_{c \max }$ is a crossover probability; $p_{m\,{{\rm min}} }$ and $p_{m \max }$ denote the smallest and largest mutation probabilities, respectively, and $p_{c\,{{\rm min}} } \le p_{m0} \le p_{c \max }$ is a mutation probabilities; $f_{ \max }$ is the fitness of the best individual in the population and $f_{\text{avg}}$ is the average fitness; $f$ is the greater fitness of crossed parent; $f^{{\prime }}$ is the fitness of the individual performing the mutation.

For the crossover probability adjustment method in Eq. (12), $p_{c0}$ represents a crossover probability for the average fitness of the population, and the size of $p_{c0}$ can be determined based on the required problem and the algorithm optimization process. If $p_{c0}$ is larger, it means raising the crossover probability of the individual in the population and promoting the change in the individual gene pattern from a wide range. If x is small, the opposite is true. Therefore, in order to improve the optimization performance of the algorithm, we need to adjust the value of $p_{c0}$ to balance the global optimization ability and local optimization ability of the algorithm.

According to the formula, the crossover probability and mutation probability of the improved algorithm can be approximately calculated, which are automatically adjusted according to the individual fitness in the population, as shown in Figs. 3 and 4.

Fig. 3

Crossover probability adjustment curve of IAGA

Fig. 4

Mutation probability adjustment curve of IAGA

The adaptive genetic algorithm is used to adjust crossover operator and mutation operator by the cosine function. The crossover probability and mutation probability are adjusted nonlinearly according to the fitness of the population. The algorithm flow is shown in Fig. 5.

Fig. 5

Cosine genetic algorithm evolution process

Combining the improved algorithm with trajectory planning

Problem description

Assume that the robot performs an action with its end effector passing n points (including the start and the end points). N points can be converted into n corresponding joint variables of the joint space by the inverse kinematics of the robot, that is produced n − 1 time segment with length of $T_{i} (i = 1,2,3, \ldots ,n - 1)$, $T_{i} = t_{i + 1} - t_{i}$, where x represents the moment when the robot end effector reaches the i-th path point. The total time is:

$$ T = T_{1} + T_{2} + \cdot \cdot \cdot + T_{i - 1} = \sum\limits_{i = 1}^{n - 1} {T_{i} } $$

(14)

where T is the total time of the robot movement which is the objective function of the problem; $T_{i} {\kern 1pt} {\kern 1pt} {\kern 1pt} (i = 1,2,3 \ldots ,n - 1)$ is the time interval of the joint variable; the constraint is the maximum angular velocity, acceleration and jerk of the joints of the robot. Therefore, the problem of time optimal trajectory planning for robot is described as follows:

• (1) Objective function:

  $$ {{\rm min}} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} T = \sum\limits_{i = 1}^{n - 1} {T_{i} } $$

  (15)

• (2) constraint condition:

  • (A) angular velocity

    $$ \left\{ {\begin{array}{*{20}l} {\forall t \in [t_{i} ,t_{i + 1} ]} \hfill \\ {|\theta_{i}^{{\prime }} (t)| \le \theta^{{\prime }}_{ \max } } \hfill \\ \end{array} } \right. $$

    (16)

    where $\theta_{ \max }^{{\prime }}$ is the maximum angular velocity allowed by the arm joint.

  • (B) acceleration

    $$ \left\{ {\begin{array}{*{20}l} {\forall t \in [t_{i} ,t_{i + 1} ]} \hfill \\ {|\theta^{{\prime \prime }}_{i} (t)|\; \le \theta^{{\prime \prime }}_{ \max } } \hfill \\ \end{array} } \right. $$

    (17)

    where $\theta_{ \max }^{{\prime \prime }}$ is the maximum angular acceleration value allowed by the robot joint.

  • (C) jerk

    $$ \left\{ {\begin{array}{*{20}l} {\forall t \in [t_{i} ,t_{i + 1} ]} \hfill \\ {|\theta^{{\prime \prime }}_{i} (t)|\; \le \theta^{{\prime \prime }}_{ \max } } \hfill \\ \end{array} } \right. $$

    (18)

    where $\theta_{ \max }^{{{\prime \prime \prime }}}$ is the maximum jerk value allowed for the arm joint.

Time-optimal simulation

According to the parameters of the PUMA560 robot, we can get the constraints of the robot joints (see Table 1).

Table 1 The constraints of the robot joints

The improved algorithm is coded based on the real coding, and the parameters are selected as follows: population size M = 80; maximum crossover probability $P_{c1} = 0.9$, minimum value $P_{c2} = 0.4$, $P_{c0} = 0.7$; maximum mutation probability $P_{m1} = 0.1$, minimum value $P_{m2} = 0.01$, $P_{m0} = 0.7$; Evolutional generation G = 100. A MATLAB program for the optimal trajectory planning of the first three joints of PUMA560 is written by combining the quintic polynomial interpolation trajectory [15, 16]. In the optimization process, the trajectory of the robot joint is composed of the seven-segment polynomial curve, and its optimization precision is 0.001 s. The results are shown in Table 2.

Table 2 Optimization Results

It can be seen from Table 2 that after the optimization of the running time of the robot, the time taken by the robot to reach the target point is obviously reduced under the constraint of the joint angular velocity, acceleration and jerk of the robot. For the first three joints, the total time of the original run is 28 s. After optimization of this method, it is shortened to 13.729, 14.381 and 14.648 s, which was at least 47.7% shorter than 28 s. Compared with several literatures that use PUMA560 as the object for MATLAB simulation, the same constraints apply. The reference literature [17] describes the method of interpolating the trajectory of the robot with seventh-order B-spline curves and optimizing the robot trajectory using the genetic algorithm. After optimization, the time is shortened from the original 20–15.620 s, which is shortened by 21.9%. In reference literature [18], a cubic B-spline curve is used to interpolate the robot motion trajectory, and then an improved genetic algorithm based on the crossover operator and mutation operator adjusted with evolutionary algebraic average fitness is used to perform the time optimal trajectory planning for the robot motion trajectory. The time after optimization was shortened from the original 20–13.072 s, a 34.6% reduction. It can be seen that the improved genetic algorithm, which the crossover operator and the mutation operator in the general adaptive genetic algorithm are adjusted cosine, based on quintic polynomial interpolation described in this paper satisfies the goal of the shortest time trajectory planning.

Simulation and analysis of smoothness of joint operation

The optimization of the robot’s running time is shown in the previous section, and then, we simulate the angular displacement, velocity and acceleration curves of the first three joints of the robot.

From Figs. 6, 7 and 8, it can be seen that the angular displacement, velocity and acceleration curves are smooth and can reduce shock and impact of the robot arm and ensure smooth operation of the robot.

Fig. 6

Kinematic curve of joint 1

Fig. 7

Kinematic curve of joint 2

Fig. 8

Kinematic curve of joint 3

Conclusions

Based on the PUMA560 robot model, this paper briefly describes the method of locating the trajectory with the quintic polynomial interpolation in the joint space. Then, the crossover operator and the mutation operator in the general adaptive genetic algorithm are adjusted cosine to improve the performance of the algorithm. Using the improved algorithm to optimize the interpolation time of the robot trajectory, the simulations show that the running time of each joint of the robot has been greatly reduced. The angular displacement, velocity and acceleration curve of the joint operation shows that the smoothness of the robot is better and the oscillations and shocks of the manipulator can be reduced. In conclusion, the method of this paper realizes the optimal trajectory planning target for robot time.