1 Introduction

With the development of technology, people’s lives are becoming more and more convenient, especially in industrial production, where highly repetitive labor has been gradually replaced by industrial robots [1]. As an automation device, industrial robots combine technologies such as mechanics, control and programming and have the advantages of being programmable and working with high stability and efficiency. When the initial industrial robots used robotic arms to carry goods, their routes were set manually and the robotic arms worked in cycles according to the set routes [2], which were relatively simple but too rigid to cope with unexpected situations, and the manually planned routes might be not the best routes for robotic arm operation because of unstable joint changes during route changes. With the development of cybernetics and programming, the route planning of robotic arms has become more intelligent, and a suitable route connecting the starting and end points can be planned using intelligent algorithms [3]. Prianto et al. [4] proposed a soft actor-critic-based route planning algorithm and found through experiments that the performance of the algorithm was better than the current results. Wall et al. [5] proposed a new path planning algorithm to perform the high-speed computation to deal with fast-moving obstacles and found that the algorithm could calculate a safe path faster for the robotic arm. This paper briefly introduced the motion model and trajectory planning method of robotic arms. The motion trajectory of the robotic arm was optimized by the particle swarm algorithm improved by the genetic algorithm. The simulation experiments were conducted for the trajectory planning of the robot arm. The improved particle swarm algorithm was compared with the genetic and particle swarm algorithms.

2 Robotic arm motion model of robots

Figure 1 shows a schematic diagram of a robot arm with six-degree-of-freedom, and every node serves as an origin in the linkage coordinate system. There are six rotatable nodes in the robot arm [6]; then, there are seven origin points in the linkage coordinate system, where origin 0 is the part connecting the base to the first node. Table 1 shows the D–H parameters of the robotic arm in the linkage coordinate system [7]. \(\alpha_{i - 1}\) represents the angle required by \(Z_{i - 1}\) to rotate to the \(Z_{i}\) axis around the \(X_{i - 1}\) axis, which is used for determining the direction of the \(Z_{i}\) axis for node \(i\), \(a_{i - 1}\) represents the straight-line distance between the \(Z_{i - 1}\) axis and the \(Z_{i}\) axis along the \(X_{i - 1}\) axis, \(d_{i}\) represents the straight-line distance between the \(X_{i - 1}\) axis and the \(X_{i}\) axis along the \(X_{i - 1}\) axis, and \(\theta_{i}\) is the angle required by the \(X_{i - 1}\) axis to rotate to the \(Z_{i - 1}\) axis along the \(X_{i - 1}\) axis, i.e., the joint angle of node \(i\) rotating around the \(Z_{i}\) axis. The conversion formula of the fixed coordinates and the linkage coordinates is:

$$ \left\{ \begin{gathered} {}_{i}^{i - 1} T = \left[ {\begin{array}{*{20}c} {\cos \theta_{i} } & { - \sin \theta_{i} } & 0 & {a_{i - 1} } \\ {\cos \alpha_{i - 1} \sin \theta_{i} } & {\cos \alpha_{i - 1} \cos \theta_{i} } & { - \sin \alpha_{i - 1} } & { - d_{i} \sin \alpha_{i - 1} } \\ {\sin \alpha_{i - 1} \sin \theta_{i} } & {\sin \alpha_{i - 1} \cos \theta_{i} } & {\cos \alpha_{i - 1} } & {d_{i} \cos \alpha_{i - 1} } \\ 0 & 0 & 0 & 1 \\ \end{array} } \right] \hfill \\ {}_{6}^{0} T = {}_{1}^{0} T{}_{2}^{1} T{}_{3}^{2} T{}_{4}^{3} T{}_{5}^{4} T{}_{6}^{5} T \hfill \\ \end{gathered} \right.. $$
(1)
Fig. 1
figure 1

Schematic diagram of a robotic arm with six-degree-of-freedom and its linkage coordinate system

Full size image
Table 1 D–H parameters of the robotic arm
Full size table

Among the four D–H parameters used in the above conversion expression matrix, only joint angle \(\theta_{i}\) is a variable. The remaining three D–H parameters are determined by the structure of the robotic arm, and the three parameters are constants for the same robotic arm [8].

3 Trajectory motion control algorithm

3.1 Joint space planning method

Planning the robotic arm trajectory is to move the end of the arm from the initial position to the set position by changing the joint angle. To keep the robotic arm stable along the planned path, interpolation calculation for intermediate transition points between the starting and end points is needed to fit the smooth route curve.

The Cartesian spatial planning method directly solves the spatial trajectory of the end of the robotic arm. The obtained trajectory is intuitive, but the computation required to calculate the joint angles corresponding to the spatial point location of the trajectory in real-time is huge, especially in the robotic arm whose joint has a high degree of freedom. The joint spatial planning method performs interpolation calculations on the joint angles corresponding to the starting and ending points. Although the trajectory obtained from the planning is not intuitive, the computation is much smaller. The goal of this paper is to optimize the running time of the robotic arm trajectory movement, so the joint space planning method is finally chosen to design the motion trajectory.

In this paper, the joint space planning method is chosen to plan the robotic arm trajectory. The general process of the joint space planning method [9] is: ① calculate the joint angles when the end of the robotic arm is at the starting point, transition point and end point; ② make interpolation calculations on the joint angles of two adjacent points and fit to get the function relationship between joint angle and time in the two adjacent points. In the first step, the joint angles are obtained through the inverse operation of Eq. (1); in the second step, the trajectory is fitted by polynomial interpolation [10].

To improve the stability of the path and reduce the computational effort, avoiding the Runge’s phenomenon should be avoided. This paper combines the cubic and quintic polynomial interpolation methods, and the corresponding formulas are:

$$ \left\{ \begin{gathered} \theta_{1} (t) = a_{10} + a_{11} t + a_{12} t^{2} + a_{13} t^{3} \hfill \\ \theta_{2} (t) = a_{20} + a_{21} t + a_{22} t^{2} + a_{23} t^{3} + a_{24} t^{4} + a_{25} t^{5} \hfill \\ \theta_{3} (t) = a_{30} + a_{31} t + a_{32} t^{2} + a_{33} t^{3} \hfill \\ a = \left[ {\begin{array}{*{20}c} {a_{13} } & {a_{12} } & {a_{11} } & {a_{10} } & {a_{25} } & {a_{24} } & {a_{23} } & {a_{22} } & {a_{21} } & {a_{20} } & {a_{33} } & {a_{32} } & {a_{31} } & {a_{30} } \\ \end{array} } \right] = D^{ - 1} m \hfill \\ m = \left[ {\begin{array}{*{20}c} 0 & 0 & 0 & 0 & 0 & 0 & {M_{3} } & 0 & 0 & {M_{0} } & 0 & 0 & {M_{2} } & {M_{1} } \\ \end{array} } \right] \hfill \\ D = \left[ {\begin{array}{*{20}c} {D_{11} } & {D_{12} } & {D_{13} } \\ 0 & 0 & {D_{23} } \\ {D_{31} } & 0 & {D_{33} } \\ \end{array} } \right] \hfill \\ \end{gathered} \right., $$
(2)
$$ \begin{array}{*{20}c} {D_{11} = \left[ {\begin{array}{*{20}c} {t_{1}^{3} } & {t_{1}^{2} } & {t_{1} } & 1 & 0 \\ {3t_{1}^{2} } & {2t_{1} } & 1 & 0 & 0 \\ {6t_{1} } & 2 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & {t_{2}^{5} } \\ 0 & 0 & 0 & 0 & {5t_{2}^{4} } \\ 0 & 0 & 0 & 0 & {20t_{2}^{3} } \\ \end{array} } \right]} & {D_{12} = \left[ {\begin{array}{*{20}c} 0 & 0 & 0 & 0 & { - 1} \\ 0 & 0 & 0 & { - 1} & 0 \\ 0 & 0 & { - 2} & 0 & 0 \\ {t_{2}^{4} } & {t_{2}^{3} } & {t_{2}^{2} } & {t_{2} } & 1 \\ {4t_{2}^{3} } & {3t_{2}^{2} } & {2t_{2} } & 1 & 0 \\ {12t_{2}^{2} } & {6t_{2} } & 2 & 0 & 0 \\ \end{array} } \right]} & {D_{13} = \left[ {\begin{array}{*{20}c} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & { - 1} \\ 0 & 0 & { - 1} & 0 \\ 0 & { - 2} & 0 & 0 \\ \end{array} } \right]} \\ {D_{23} = \left[ {\begin{array}{*{20}c} 0 & {t_{3}^{3} } & {t_{3}^{2} } & {t_{3} } & 1 \\ 0 & {3t_{3}^{2} } & {2t_{3} } & 1 & 0 \\ 0 & {6t_{3} } & 2 & 0 & 0 \\ \end{array} } \right]} & {D_{31} = \left[ {\begin{array}{*{20}c} 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ \end{array} } \right]} & {D_{33} = \left[ {\begin{array}{*{20}c} 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 \\ 1 & 0 & 0 & 0 & 0 \\ \end{array} } \right]} \\ \end{array} , $$
(3)

where \(\theta_{1} (t)\), \(\theta_{2} (t)\) and \(\theta_{3} (t)\) are the joint angles in the first, second and third sections of route, \(t\) is the time, \(t_{1}\) is the time required for the first section of route, \(t_{2}\) is the time required for the second section of route, \(t_{3}\) is the time required for the third section of route, \(a\) is the matrix of the unknown coefficients, \(D\) is the matrix of the equation set of the path constraints, and \(m\) is the four known insertion points of the three sections of route.

3.2 Particle swarm optimization-based trajectory optimization

In this paper, the joint space planning method divides the route into three sections, so there are four route points in a route. Using Eqs. (2) and (3), the corresponding planned route can be obtained as long as the four route points and the movement time are known. The route fitted by the polynomial interpolation method is smooth enough, so the optimization of the trajectory of the robotic arm in this paper mainly focuses on the running time, i.e., to make the robotic arm move from the starting point to the end point as fast as possible while maintaining stability.

There are various algorithms for optimizing motion trajectories in space, and this paper chooses the particle swarm optimization (PSO) algorithm [11]. The PSO algorithm imitates bird foraging in nature. In this algorithm, a population containing a plural number of particles is generated first. The position coordinates of every particle in the population in the search space is a candidate solution, i.e., a population is a set of candidate solutions. The fitness value of every particle in the population is calculated. Taking the best particle position as the target, the particle population moves in the search space. “The judgment of the current and historical optimal position of the particle—particle population movement” is repeated until the particle population converges at a certain position. The coordinates of this position is the optimal solution.

This paper uses the joint space planning method to plan the end movement trajectory of the robotic arm, which means that the movement of the end of the robotic arm is achieved by controlling the change of the joint angle. The starting and ending positions of the end of the robotic arm can obtain the angle of every joint angle through conversing Eq. (1). The movement of the end can be regarded as the change of the joint angle from the angle corresponding to the starting position to the angle corresponding to the end position. Since the end can have different paths from the starting point to the end, the changing process of the joint angle is also different.

To make the trajectory smoother and more stable, this paper chooses polynomial interpolation to fit the joint angle change curve with time and divides the process from the starting point to the end point into three segments. The first and third segments are fitted with cubic polynomial interpolation, and the second segment is fitted with quintic polynomial interpolation. In this three-segment polynomial interpolation method, in addition to the starting point and the end point, two additional insertion points need to be inserted, and the corresponding joint angles can be obtained using Eq. (1).

Based on the starting point, the end point, and the joint angles corresponding to the two insertion points, polynomial interpolation can be performed in combination with Eqs. (3) and (4), with the final goal of obtaining the coefficient matrix \(a\) of every joint angle. In this paper, besides using the interpolation method to obtain a smooth moving trajectory, it is also necessary to make the moving time of the trajectory as small as possible so as to improve the efficiency of the robotic arm.

When using the PSO algorithm to optimize the trajectory of joint angle change, instead of gradually fitting coefficient \(a\), the coordinates of the particles can be used as coefficient \(a\) and thus substituted into the polynomial, followed by the calculation of the consumed time. The time consumption is minimized during the population iteration. However, a robotic arm often has plural joint angles, and the trajectory polynomial of every joint angle requires a coefficient matrix \(a\), which leads to a high dimensionality of particles within the PSO population and increases the search difficulty. The plural joint angle changes of the robotic arm are simultaneous, which means that the movement time of the robotic arm does not need to distinguish the joint angle; therefore, it searches by taking the movement time spent in the three segments of paths as the particle coordinates, and the time represented by the particles are substituted into Eqs. (3) and (4) to calculate the coefficient of every joint angle polynomial in the iteration process. The goal of the particle iteration process is still to minimize the total time, but to ensure the stability of the moving trajectory, after the polynomial function of the trajectory is obtained based on the consumed time, it is necessary to calculate the running speed of joint angles, and the particles exceeding the limit cannot be used as the optimal particle even if the total time is minimal.

Compared with the genetic algorithm, the PSO algorithm is simpler in principle and implementation, but in the process of searching for the optimal solution, the particles may fall into the locally optimal solution. To make the particle population jump out of the locally optimal solution, this paper introduces the concept of crossover and mutation from the genetic algorithm, so that the particles in the PSO population perform crossover and mutation operations after the iteration of position and velocity. The randomness of the two operations is used to avoid the particles falling into the locally optimal solution as much as possible.

Figure 2 shows the basic process of optimizing the joint angle trajectory of the robotic arm using an improved PSO algorithm, and the specific steps are as follows.

  1. ➀

    Every particle in the population represents a planning scheme, and the coordinates of the particle in the search space are the scheme content. The time required for the first, second and third sections of the route is taken as the coordinate axes of the three-dimensional search space. The initialization of the population will randomly generate particles at different positions under the number of population sizes.

  2. ➁

    Whether particles corresponding to the time required for the three sections of the route in the population can make matrix \(D\) singular is determined. Positions of particles that will make matrix \(D\) singular are adjusted, i.e., the time required for the three sections of route represented by the particles is adjusted.

  3. ➂

    The time required for the three sections of route represented by the particles are substituted into Eqs. (2) and (3) to calculate the coefficient of the three-segment polynomial function for every joint angle trajectory.

  4. ➃

    The maximum running speed of the joint angle in the path is calculated according to the three-segment polynomial function of every joint angle trajectory to determine whether the running speed exceeds the maximum running speed allowed by the joint angle. If not, the next step is performed; if it does, the particles exceeding the limit are processed by regular PSO iterations to make the joint angle speed decrease.

  5. ➄

    The fitness of the particles within the population is calculated, i.e., the total time required for the three sections of the route, and the particles are sorted in descending order of fitness.

  6. ➅

    The position and velocity of the particles within the population are updated [12] using the following equation:

    $$ \left\{ \begin{gathered} v_{i} (t + 1) = \varpi v_{i} (t) + c_{1} r_{1} (P_{i} (t) - x_{i} (t)) + c_{2} r_{2} (G_{g} (t) - x_{i} (t)) \hfill \\ x_{i} (t + 1) = x_{i} (t) + v_{i} (t + 1) \hfill \\ \end{gathered} \right., $$
    (4)

    where \(v_{i} (t + 1)\) and \(x_{i} (t + 1)\) are the velocity and position of particle \(i\) after one iteration, \(v_{i} (t)\) and \(x_{i} (t)\) are the velocity and position of particle \(i\) before the iteration, \(\varpi\) is the inertia weight of the particle, \(c_{1}\) and \(c_{2}\) are learning factors, \(r_{1}\) and \(r_{2}\) are random numbers between 0 and 1, \(P_{i} (t)\) is the individual historical optimal point of particle \(i\), and \(G_{g} (t)\) is the historical optimal point of particle swarm.

  7. ➆

    The three coordinate values of particles in the search space are regarded as three gene fragments. The single-point crossover [13] is used in this paper, which means exchanging the fragment of the same gene locus in two randomly selected particles according to the crossover probability. The mutation operation is also used, which means changing one gene fragment in a randomly selected particle according to the mutation probability.

  8. ➇

    If the termination condition is satisfied, then the optimal scheme in the population is output; if not, step ② is repeated until the termination condition is satisfied.

Fig. 2
figure 2

Basic flow of joint space trajectory planning under an improved particle swarm optimization algorithm

Full size image

4 Simulation experiments

4.1 Experimental setup

The robotic arm used in the simulation experiment was a six-degree-of-freedom robotic arm, whose basic structure is shown in Fig. 1. The D–H parameters of the robotic arm are shown in Table 2. In Table 2, \(\theta_{i}\) represents the rotation angle of joint angle \(i\), whose value depends on the end position of the arm. The planned joint space trajectory was fitted by the three-segment polynomial interpolation method, so four interpolation points were needed [14], which were starting point \(M_{0}\), transition points \(M_{1}\) and \(M_{2}\), and end point \(M_{3}\). The joint angles corresponding to the four points are shown in Table 2.

Table 2 D–H parameters of the robotic arm
Full size table

To further demonstrate the performance of the improved PSO algorithm, this paper compared it with the traditional PSO algorithm and the genetic algorithm. The basic process of the traditional PSO algorithm was the process after removing the crossover and mutation steps in Fig. 2, and its relevant parameters were the same as those of the improved PSO algorithm except the crossover and mutation probabilities.

4.2 Experimental results

Figure 3 shows the convergence curves when optimizing the joint angle space trajectory of the robotic arm using the genetic algorithm, the traditional PSO algorithm and the improved PSO algorithm, and the fitness function is the total time required for the three sections of the route. It was seen from Fig. 3 that after optimization by the three algorithms, the total movement time of the robotic arm was subsequently reduced, and the improved PSO algorithm iterated the minimum number of times and had the lowest fitness before stability.

Fig. 3
figure 3

Convergence curves of the three algorithms when optimizing the trajectory

Full size image

Due to space limitation, this paper only shows the changes of angle, angular velocity and angular acceleration of joint angle 1 within the planned time, as shown in Fig. 4. Figure 4 shows that the route obtained by the genetic algorithm took 2.50 s, the route obtained by the PSO algorithm took 2.00 s, and the route obtained by the improved PSO algorithm took 1.00 s. It was seen from Fig. 4 that the changes in the joint angle and angular velocity were continuous and smooth under the routes planned by the three algorithms, and the quintic polynomial interpolation method was used to plan the intermediate route, which avoided the problem of sudden changes in angular acceleration in the planning of the cubic polynomial interpolation method and improved the joint stability of the robotic arm.

Fig. 4
figure 4

Changes in angle, angular velocity and angular acceleration of joint 1 under three algorithms

Full size image

The computation time of the three optimization algorithms is shown in Fig. 5. The genetic algorithm took 3.2 s, the PSO algorithm took 2.8 s, and the improved PSO algorithm took 1.3 s. It was seen from Fig. 5 that the genetic algorithm consumed the most time in computation, followed by the PSO algorithm and the improved PSO algorithm. The reason for the above result is as follows. Although the genetic algorithm used the fitness function as the guiding target when iterating on the population, individuals in the population were randomly adjusted according to the crossover and mutation probabilities, and the fitness function played more of a screening role. The PSO algorithm used the fitness function to select the optimal individuals when adjusting the individuals so that the other individuals were adjusted toward the optimal individuals, which had a more precise direction; therefore, it obtained the optimal solution faster than the genetic algorithm. As the PSO algorithm aimed at adjusting the particles to make them optimal, the optimal individual would affect the whole algorithm; thus, to avoid the optimal individual be a local optimum, the improved PSO algorithm used the crossover and mutation operations in the genetic algorithm to adjust the individuals, thus jumping out of the locally optimal solution, accelerating the convergence and improving the computational speed.

Fig. 5
figure 5

Computational time of the three algorithms

Full size image

5 Conclusion

This paper briefly introduced the motion model of the robotic arm and the trajectory planning method. The trajectory of the robotic arm was optimized using the genetic algorithm-improved PSO algorithm; simulation experiments were conducted on the trajectory planning of the robotic arm; the improved algorithm was compared with the genetic algorithm and the PSO algorithm. The genetic algorithm iterated about 330 times before stability and had the highest fitness; the PSO algorithm iterated about 300 times before stability and had the second-highest fitness; the improved PSO algorithm iterated about 200 times before stability and had the third-highest fitness. The paths planned by the genetic algorithm, the PSO algorithm and the improved PSO algorithm took 2.50 s, 2.00 s and 1.00 s, respectively, and the changes in the angle, angular and angular acceleration were continuous and smooth. The genetic algorithm consumed 3.2 s in computing, the PSO algorithm consumed 2.8 s, and the improved PSO algorithm consumed 1.3 s.

The limitation of this paper is that only the improved PSO algorithm was compared with the traditional PSO algorithm and GA, the effectiveness of the improved PSO algorithm was initially verified, and the improved PSO algorithm was more suitable for trajectory planning with fixed targets. Therefore, the future research direction is to compare the improved PSO algorithm with other optimization algorithms and try to realize trajectory planning with real-time changing targets.