A Novel Multi-Objective Trajectory Planning Method for Robots Based on the Multi-Objective Particle Swarm Optimization Algorithm

Abstract

The three performance indexes of the space robot, travel time, energy consumption, and smoothness, are the key to its important role in space exploration. Therefore, this paper proposes a multi-objective trajectory planning method for robots. Firstly, the kinematics and dynamics of the Puma560 robot are analyzed to lay the foundation for trajectory planning. Secondly, the joint space trajectory of the robot is constructed with fifth-order B-spline functions, realizing the continuous position, velocity, acceleration, and jerk of each joint. Then, the improved multi-objective particle swarm optimization (MOPSO) algorithm is used to optimize the trajectory, and the distribution uniformity, convergence, and diversity of the obtained Pareto front are good. The improved MOPSO algorithm can realize the optimization between multiple objectives and obtain the trajectory that meets the actual engineering requirements. Finally, this paper implements the visualization of the robot’s joints moving according to the optimal trajectory.

1. Introduction

With the rapid development of science and technology in today’s world, humankind’s space exploration has shown unprecedented momentum, and space technology has gradually risen to the focus of public attention. Among them, space robots have become indispensable to space activities due to their powerful functions and high adaptability to the space environment. A series of challenging tasks, such as assembling satellite parts, capturing space targets, and monitoring alien spacecraft, cannot be realized without the support of space robots. These tasks and harsh working environments also set higher requirements for space robots’ travel time, energy consumption, and smoothness.

The primary method to improve each performance index is to design and plan the robot’s trajectory rationally. Trajectory planning can be performed both in Cartesian space and in joint space. The latter plans the trajectory of each joint of the robot, which has a small amount of calculation and enables the real-time control of the robot. Trajectory planning can usually be divided into two steps. The first step is to interpolate between given path points using interpolation algorithms to obtain a trajectory-time sequence. The second step is to optimize the trajectory in terms of single or multiple performance indexes within the constraints of the kinematics and dynamics of the robot [1].

The mainstream interpolation algorithms in joint space are polynomial interpolation and spline curve interpolation, and the former is mainly used in early research [2,3,4,5]. To obtain the robot’s trajectory, Ref. [6] used a cubic polynomial to connect the path points. This method is simple to calculate, but the acceleration curve obtained is not continuous, the smoothness could be better, and it tends to cause rigid impacts. Ref. [7] used quintic polynomial interpolation to ensure the continuity of acceleration, but it increased the amount of calculation, and there was still a problem of easy distortion. Compared to the fifth-degree polynomial, the seventh-degree polynomial adds constraints to the jerk at the start and termination points, realizing the continuity of the jerk. However, the eight boundary conditions increase the difficulty of the solution, and the high-order polynomial interpolation may cause the Runge phenomenon [8]. With the deepening of research, some scholars have applied spline curves to the trajectory planning of robots [9,10,11,12,13,14]. Ref. [15] used the fifth-order B-spline curves to interpolate the joint space trajectory, which realized the continuity of the jerk and set the velocity and acceleration at the start and stop time to be 0. When interpolating with seventh-order B-spline curves, it is possible to specify the acceleration at the start and stop time, but the calculation process is complicated [1].

Trajectory optimization mostly takes a single performance index as the optimization objective. Robot efficiency, the shortest time required by the robot to perform a task, was the earliest goal of trajectory planning [16,17,18,19]. Ref. [20] constructed the trajectory with a quintic polynomial and reduced the robot’s travel time by 75.35% through the improved MOPSO algorithm under the constraints of each joint’s angles, velocities, and accelerations. The time-optimal trajectory often leads to a large impact on the robot, affecting its motion accuracy and shortening the service life of the robot structure. Many scholars have solved this problem by optimizing the jerk. Ref. [7] effectively increased the smoothness of the robot by optimizing the maximum value of joint jerks. Ref. [21] combined the PSO algorithm with K-means clustering to achieve a fast solution for joint trajectories with minimal shocks. Energy consumption optimization is also an important issue for robots working in unique environments such as oceans, deserts, and space [22,23,24,25]. Ref. [26] obtained a parameterized dynamic robot model through identification experiments and used a sequential quadratic programming solver to minimize a mechanical energy-based cost function under consideration of physical constraints. These three performance indexes all play an important role in the motion of the space robot, and the single optimization objective ignores the intricate balance between them. Therefore, there are studies on the comprehensive optimization of multiple objectives [27,28,29]. Ref. [30] realized the comprehensive optimization of time, energy, and smoothness by a differential evolution algorithm. Ref. [31] used the NSGA-II algorithm to optimize the same three objectives and obtained Pareto optimal solution sets, thus obtaining the high-order continuous optimal trajectories.

There are few studies on multi-objective optimization problems, so this paper proposes a trajectory planning method that can make the robot’s travel time, energy consumption, and smoothness achieve the integrated optimal state when performing the task. In this paper, continuous and smooth joint space trajectories are constructed using fifth-order B-spline functions, which also realize the specification of the velocity and acceleration of the robot at the start/stop moment. The trajectories are then optimized using the improved MOPSO algorithm to obtain the Pareto optimal solution sets, from which suitable solutions are selected according to practical needs.

The rest of the paper is organized as follows. Section 2 analyzes the kinematics and dynamics of the Puma 560 robot manufactured by Unimation, USA. Section 3 uses fifth-order B-spline curves to construct the joint space trajectories of the robot, builds a mathematical model for the multi-objective optimization problem based on Section 2, and solves the model with the improved MOPSO algorithm. Section 4 performs simulation experiments on multi-objective trajectory planning. Section 5 summarizes this article.

2. Kinematics and Dynamics Analysis

2.1. Kinematics Analysis

This section gives the kinematics model of the Puma560 robot and analyzes its forward and inverse kinematics. The MDH (Modified Denavit–Hartenberg) coordinate system [32] shown in Figure 1 is established on the Puma560 robot with six rotary joints. The link parameters from the MATLAB R2020b Robot Toolbox are shown in

Table 1. Link parameters of the Puma560 robot.

Link i	${\mathit{\alpha }}_{\mathit{i}-1}$ (rad)	${\mathit{a}}_{\mathit{i}-1}$ (m)	${\mathit{d}}_{\mathit{i}}$ (m)	${\mathit{\theta }}_{\mathit{i}}$ (rad)
1	0	0	0	${\theta }_1$
2	−1.5708	0	0.2435	${\theta }_2$
3	0	0.4318	−0.0934	${\theta }_3$
4	1.5708	−0.0203	0.4331	${\theta }_4$
5	−1.5708	0	0	${\theta }_5$
6	1.5708	0	0	${\theta }_6$

.

The simulation model of the Puma560 robot is established by using the Robot Toolbox in MATLAB, as shown in Figure 2.

2.1.1. Forward Kinematics Analysis

Forward kinematics analysis refers to obtaining the end-effector pose relative to the base according to the angle of each robot joint. The transformation matrix between neighboring links, that is, the coordinate system {i} relative to the coordinate system {i − 1}, can be represented by

$${}_i{}^{i-1}T=\left[\begin{array}{cccc}\begin{array}{c}\cos {\theta }_i\\ \sin {\theta }_i\cos {\alpha }_{i-1}\\ \sin {\theta }_i\sin {\alpha }_{i-1}\\ 0\end{array}& \begin{array}{c}-\sin {\theta }_i\\ \cos {\theta }_i\cos {\alpha }_{i-1}\\ \cos {\theta }_i\sin {\alpha }_{i-1}\\ 0\end{array}& \begin{array}{c}0\\ -\sin {\alpha }_{i-1}\\ \cos {\alpha }_{i-1}\\ 0\end{array}& \begin{array}{c}{a}_{i-1}\\ -\sin {\alpha }_{i-1}{d}_i\\ \cos {\alpha }_{i-1}{d}_i\\ 1\end{array}\end{array}\right]$$

(1)

Substituting the parameters in Table 1 into (1), the six homogeneous transformation matrixes of the Puma560 robot can be obtained by

$$\begin{array}{c}{}_1{}^{0}T=\left[\begin{array}{cccc}\cos {\theta }_1& -\sin {\theta }_1& 0& 0\\ \sin {\theta }_1& \cos {\theta }_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right] {}_2{}^{1}T=\left[\begin{array}{cccc}\cos {\theta }_2& -\sin {\theta }_2& 0& 0\\ 0& 0& 1& d_2\\ -\sin {\theta }_2& -\cos {\theta }_2& 0& 0\\ 0& 0& 0& 1\end{array}\right] {}_3{}^{2}T=\left[\begin{array}{cccc}\cos {\theta }_3& -\sin {\theta }_3& 0& a_2\\ \sin {\theta }_3& \cos {\theta }_3& 0& 0\\ 0& 0& 1& d_3\\ 0& 0& 0& 1\end{array}\right]\\ {}_4{}^{3}T=\left[\begin{array}{cccc}\cos {\theta }_4& -\sin {\theta }_4& 0& a_3\\ 0& 0& -1& -d_4\\ \sin {\theta }_4& \cos {\theta }_4& 0& 0\\ 0& 0& 0& 1\end{array}\right] {}_5{}^{4}T=\left[\begin{array}{cccc}\cos {\theta }_5& -\sin {\theta }_5& 0& 0\\ 0& 0& 1& 0\\ -\sin {\theta }_5& -\cos {\theta }_5& 0& 0\\ 0& 0& 0& 1\end{array}\right] {}_6{}^{5}T=\left[\begin{array}{cccc}\cos {\theta }_6& -\sin {\theta }_6& 0& 0\\ 0& 0& -1& 0\\ \sin {\theta }_6& \cos {\theta }_6& 0& 0\\ 0& 0& 0& 1\end{array}\right].\end{array}$$

(2)

The transformation matrixes in (2) can be multiplied together to find the transformation matrix of the end-effector coordinate system {6} relative to the base coordinate system {0}

$${}_6{}^{0}T={}_1{}^{0}T{}_2{}^{1}T{}_3{}^{2}T{}_4{}^{3}T{}_5{}^{4}T{}_6{}^{5}T=\left[\begin{array}{cccc}\begin{array}{l}{r}_{11}\\ r_{21}\\ r_{31}\\ 0\end{array}& \begin{array}{l}{r}_{12}\\ r_{22}\\ r_{32}\\ 0\end{array}& \begin{array}{l}{r}_{13}\\ r_{23}\\ r_{33}\\ 0\end{array}& \begin{array}{l}{p}_x\\ p_y\\ p_z\\ 1\end{array}\end{array}\right]$$

(3)

where $p_x, p_y, p_z$ represent the position of the end-effector, and $r_{11}, r_{12}, r_{13}, r_{21}, r_{22}, r_{23}, r_{31}, r_{32}, r_{33}$ represent the orientation of the end-effector. These 12 elements are calculated by the joint angles.

2.1.2. Inverse Kinematics Analysis

Inverse kinematics analysis refers to the inverse solution of the angle of each joint by using the end-effector pose relative to the base. Analyzing the structural characteristics of the Puma560 robot, it is easy to know that its last three axes intersect at one point, and the six joints of the robot are all rotary joints. So, the Pieper method [33] can solve the inverse kinematics of the Puma560 robot. When $\sin \left({\theta }_5\right)\ne 0$, the joint angles can be obtained by

$${\theta }_1=\mathrm{A}\tan 2\left({\displaystyle \frac{g_{1}y-g_{2}x}{g_1^2+g_2^2}},{\displaystyle \frac{g_{1}x+g_{2}y}{g_1^2+g_2^2}}\right)$$

(4)

$${\theta }_2=\mathrm{A}\tan 2\left({\displaystyle \frac{-z}{{\rho }_2}},\pm \sqrt{1-{\displaystyle \frac{z^2}{{\rho }_2^2}}}\right)-\mathrm{A}\tan 2\left(f_2,f_1\right)$$

(5)

$${\theta }_3=\mathrm{A}\tan 2\left({\displaystyle \frac{C-r}{{\rho }_3}},\pm \sqrt{1-{\displaystyle \frac{{\left(C-r\right)}^2}{{\rho }_3^2}}}\right)-\mathrm{A}\tan 2\left(a_3,d_4\right)$$

(6)

$${\theta }_4=\mathrm{A}\tan 2\left({\displaystyle \frac{x_{33}}{\sin ({\theta }_5)}},{\displaystyle \frac{x_{13}}{\sin ({\theta }_5)}}\right)$$

(7)

$${\theta }_5=\mathrm{A}\tan 2\left(\pm \sqrt{x_{21}^2+x_{22}^2},-x_{23}\right)$$

(8)

$${\theta }_6=\mathrm{A}\tan 2\left({\displaystyle \frac{-x_{22}}{\sin ({\theta }_5)}},{\displaystyle \frac{x_{21}}{\sin ({\theta }_5)}}\right)$$

(9)

where $\mathrm{A}\tan 2$ is predefined in many programming language libraries; its function is to judge the quadrant of the angle according to the positive and negative of x and y while calculating ${\tan }^{-1}\left({\displaystyle \frac{y}{x}}\right)$. Unknown in (4)–(9), such as $g_1, g_2$, are the intermediate quantities in the derivation process of the Pieper method, which are defined as

$$f_1=a_2+a_3{c}_3+d_4{s}_3, f_2=a_3{s}_3-d_4{c}_3, f_3=d_3$$

(10)

$$g_1=f_1{c}_2-f_2{s}_2, g_2=f_3+d_2, g_3=-f_1{s}_2-f_2{c}_2$$

(11)

$$r=f_1^2+f_2^2+f_3^2+d_2^2+2f_3{d}_2, C=r-\left(a_2^2+a_3^2+d_4^2+d_2^2+d_3^2+2d_2{d}_3\right)$$

(12)

$$x=\left(f_1{c}_2-f_2{s}_2\right)c_1-\left(f_3+d_2\right)s_1, y=\left(f_1{c}_2-f_2{s}_2\right)s_1+\left(f_3+d_2\right)c_1, z=-f_1{s}_2-f_2{c}_2$$

(13)

$${\rho }_2=\sqrt{f_1^2+f_2^2}, {\rho }_3=2a_2\sqrt{a_3^2+d_4^2}$$

(14)

$${\varphi }_2=\mathrm{A}\tan 2\left(f_2,f_1\right), {\varphi }_3=\mathrm{A}\tan 2\left(a_3,d_4\right)$$

(15)

$$x_{13}=r_{13}{c}_{23}{c}_1-r_{33}{s}_{23}+r_{23}{c}_{23}{s}_1, x_{21}=-r_{31}{c}_{23}-r_{11}{c}_1{s}_{23}-r_{21}{s}_1{s}_{23}$$

(16)

$$x_{22}=-r_{32}{c}_{23}-r_{12}{c}_1{s}_{23}-r_{22}{s}_1{s}_{23}, x_{23}=-r_{33}{c}_{23}-r_{13}{c}_1{s}_{23}-r_{23}{s}_1{s}_{23}, x_{33}=r_{23}{c}_1-r_{13}{s}_1$$

(17)

where $c_i=\cos {\theta }_i, s_i=\sin {\theta }_i, s_{ij}=\sin \left({\theta }_i+{\theta }_j\right), c_{ij}=\cos \left({\theta }_i+{\theta }_j\right),$ $i,j=\left[1,2,3,4,5,6\right]$, and $i\ne j$.

If $\sin \left({\theta }_5\right)\ne 0$ and ${\theta }_5=0$, then the solutions of ${\theta }_1,{\theta }_2,{\theta }_3$ do not change, and ${\theta }_4,{\theta }_5,{\theta }_6$ become

$${\theta }_4=0$$

(18)

$${\theta }_5=0$$

(19)

$${\theta }_6=\mathrm{A}\tan 2\left(-x_{12},x_{11}\right)$$

(20)

where $x_{11}=r_{11}{c}_{23}{c}_1-r_{31}{s}_{23}+r_{21}{c}_{23}{s}_1, x_{12}=r_{12}{c}_{23}{c}_1-r_{32}{s}_{23}+r_{22}{c}_{23}{s}_1$.

If $\sin \left({\theta }_5\right)\ne 0$ and ${\theta }_5=\mathsf{\pi }$, then the solutions of ${\theta }_1,{\theta }_2,{\theta }_3$ do not change, and ${\theta }_4,{\theta }_5,{\theta }_6$ become

$${\theta }_4=0$$

(21)

$${\theta }_5=\mathsf{\pi }$$

(22)

$${\theta }_6=\mathrm{A}\tan 2\left(x_{12},-x_{11}\right)$$

(23)

2.2. Dynamics Analysis

The iterative Newton–Euler dynamics algorithm [34] is computationally efficient, suitable for real-time control, and commonly used for modeling robot dynamics. The algorithm is composed of two parts. First, link velocities and accelerations are iteratively calculated from the base. Second, starting from the end-effector, the force and torque of each link are calculated in reverse. The specific iterative calculation process is as follows.

Outward iterations: $i:0\to n_l-1$

$${}^{i+1}w_{i+1}={}_i{}^{i+1}R^i{w}_i+{\dot{\theta }}_{i+1}{}^{i+1}\widehat{Z}_{i+1}$$

(24)

$${}^{i+1}\dot{w}_{i+1}={}_i{}^{i+1}R^i{\dot{w}}_i+{}_i{}^{i+1}R^i{w}_i\times {\dot{\theta }}_{i+1}{}^{i+1}\widehat{Z}_{i+1}+{\ddot{\theta }}_{i+1}{}^{i+1}\widehat{Z}_{i+1}$$

(25)

$${}^{i+1}\dot{v}_{i+1}={}_i{}^{i+1}R\left[{}^i\dot{w}_i\times {}^{i}P_{i+1}+{}^{i}w_i\times \left({}^{i}w_i\times {}^{i}P_{i+1}\right)+{}^i\dot{v}_i\right]$$

(26)

$${}^i\dot{v}_{C_i}={}^i\dot{w}_i\times {}^{i}P_{C_i}+{}^{i}w_i\times \left({}^{i}w_i\times {}^{i}P_{C_i}\right)+{}^i\dot{v}_i$$

(27)

$$F_i=m{\dot{v}}_{C_i}$$

(28)

$$N_i={}^{C_i}I{\dot{w}}_i+w_i\times {}^{C_i}I{w}_i$$

(29)

where $n_l$ is the number of links, ${}^{i+1}\widehat{Z}_{i+1}$ is the unit vector of the coordinate system {i + 1} on the Z axis, $F_i$ and $N_i$ are, respectively, the inertia force and torque acting on the mass center of the link i.

Inward iterations: $i:n_l\to 1$

$${}^{i}f_i={}_{i+1}{}^{i}R^{i+1}{f}_{i+1}+{}^{i}F_i$$

(30)

$${}^{i}n_i={}^{i}N_i+{}_{i+1}{}^{i}R^{i+1}{n}_{i+1}+{}^{i}P_{C_i}\times {}^{i}F_i+{}^{i}P_{i+1}\times {}_{i+1}{}^{i}R^{i+1}{f}_{i+1}$$

(31)

$${\tau }_i={}^{i}n_i^T{}^i\widehat{Z}_i$$

(32)

where ${}^{i}f_i, {}^{i}n_i$ are, respectively, the force and torque acting on the link i, and ${\tau }_i$ is the driving force of the joint motor.

3. Multi-Objective Trajectory Planning

3.1. Construction of Joint Space Trajectory

A kth-degree B-spline curve [35] is defined by

$$p\left(u\right)={\displaystyle \sum _{i=0}^n{d}_i}{N}_{i,k}\left(u\right)$$

(33)

where $d_i$ is the control point, n+1 is its number, $p\left(u\right)$ is the path point at node u, $N_{i,k}\left(u\right)$ is the kth-degree B-spline basis function, and its specific definition is

$$\left\{\begin{array}{c}{N}_{i,0}\left(u\right)=\left\{\begin{array}{c}1, u_i\le u<u_{i+1}\hfill \\ 0, \mathrm{others}\hfill \end{array}\right.\hfill \\ N_{i,k}\left(u\right)={\displaystyle \frac{u-u_i}{u_{i+k}-u_i}}{N}_{i,k-1}\left(u\right)+{\displaystyle \frac{u_{i+k+1}-u}{u_{i+k+1}-u_{i+1}}}{N}_{i+1,k-1}\left(u\right)\hfill \\ {\displaystyle \frac{0}{0}}=0.\hfill \end{array}\right.$$

(34)

The interval of $N_{i,k}\left(u\right)$, $u\in \left[u_i,u_{i+k+1}\right]$, contains $k+1$ node intervals. It can be seen from (34) that for any node $u\in \left[u_i,u_{i+k+1}\right]$ on the parameter axis, there are only up to $k+1$ nonzero basis functions $N_{r,k}\left(u\right)\left(r=i-k,i-k+1,\dots ,i\right)$. This is the local support property of the B-spline curve. Therefore, the B-spline curve can also be expressed as

$$p\left(u\right)={\displaystyle \sum _{r=i-k}^i{d}_r{N}_{r,k}\left(u\right)}$$

(35)

In this paper, the joint space trajectory of the robot is obtained by fifth-order B-spline curve interpolation, so $k=5$. Assuming that the position-time series of a certain joint is $P=\left(p_j,t_j\right),j=0,1,\dots ,m$, then the node vector is $U=\left[u_0,u_1,\dots ,u_{m+2k}\right]$, and $n=m+k-1$.

In order to make the B-spline curve pass through the first and end position points of the joint, the node repetition degree of these two positions needs to be defined as

$$u_0=u_1=\dots =u_5=0$$

(36)

$$u_{m+5}=u_{n+6}=\dots =u_{m+10}=1$$

(37)

Moreover, the accumulative chord length parameterization method normalizes the remaining $m-1$ inner nodes

$$u_i=u_{i-1}+{\displaystyle \frac{\left|\Delta t_{i-6}\right|}{{\displaystyle \sum _{r=0}^{m-1}\left|\Delta t_r\right|}}},i=6,7,\dots ,m+4$$

(38)

The $n+1$ equations are needed to solve $n+1$ control points, where $m+1$ equations can be given by

$$p\left(u_{i+5}\right)={\displaystyle \sum _{r=i}^{i+5}{d}_r{N}_{r,5}\left(u_{i+5}\right)}=p_i, u_{i+5}\in \left[u_5,u_{m+5}\right],i=0,1,\dots ,m$$

(39)

Additional conditions determine the other $k-1$ equations. For the fifth-order B-spline curve, specifying the velocity and acceleration of the joint at the start and end points can add four additional equations

$$\left\{\begin{array}{l}{p}^{\prime }\left(u\right){|}_{u=u_5}=v_s, p^{\prime }\left(u\right){|}_{u=u_{m+5}}=v_e\\ p^{″}\left(u\right){|}_{u=u_5}=a_s, p^{″}\left(u\right){|}_{u=u_{m+5}}=a_e\end{array}\right.$$

(40)

where $p^{\prime }\left(u\right), p^{″}\left(u\right)$ are, respectively, the first and second derivatives of the B-spline curve, representing the velocity and acceleration of the joint. The deBoor–Cox recurrence formula can calculate the lth derivative of the B-spline curve

$$\left\{\begin{array}{c}{p}^l\left(u\right)={\displaystyle \sum _{r=i-k+l}^i{d}_r^l{N}_{r,k-l}\left(u\right), u_i<u<u_{i+1}}\hfill \\ d_r^l=\left\{\begin{array}{c}{d}_j, l=0\\ \left(k+1-l\right)\left(d_r^{l-1}-d_{r-1}^{l-1}\right)/\left(u_{r+k+1-l}-u_r\right), l=1,2,\dots ,r.\end{array}\right.\hfill \end{array}\right.$$

(41)

Expression (39) is combined with (40) to obtain

$$A_{n}d=p$$

(42)

where $d={\left[d_0,d_1,\dots ,d_{n-1},d_n\right]}^T, p={\left[p_0,p_1,\dots ,p_m,v_s,v_e,a_s,a_e\right]}^T$, and the coefficient matrix is

$$A_n=\left[\begin{array}{ccccccccc}1& & & & & & & & \\ & N_{1,5}(u_6)& N_{2,5}(u_6)& \cdots & N_{5,5}(u_6)& & & & \\ & & N_{2,5}(u_7)& N_{3,5}(u_7)& \cdots & N_{6,5}(u_7)& & & \\ & & \ddots & & & \ddots & & & \\ & & & N_{m-2,5}(u_{m+3})& N_{m-1,5}(u_{m+3})& \cdots & N_{m+2,5}(u_{m+3})& & \\ & & & & N_{m-1,5}(u_{m+4})& N_{m,5}(u_{m+4})& \cdots & N_{m+3,5}(u_{m+4})& \\ c_{s1}& c_{s2}& & & & & & & 1\\ & & & & & & & c_{e1}& c_{e2}\\ a_{s1}& a_{s2}& & & & & & & \\ & & & & & & a_{e1}& a_{e2}& a_{e3}\end{array}\right]$$

Some parameters in the coefficient matrix are defined by

$$\begin{array}{c}{c}_{s1}=-5/\left(u_6-u_1\right)\\ c_{s2}=5/\left(u_6-u_1\right)\\ c_{e1}=-5/\left(u_{m+9}-u_{m+4}\right)\\ c_{e2}=5/\left(u_{m+9}-u_{m+4}\right)\\ a_{s1}=20/\left[\left(u_6-u_2\right)\left(u_6-u_1\right)\right]\\ a_{s2}=-20\left\{1/\left[\left(u_6-u_2\right)\left(u_6-u_1\right)\right]+1/\left[\left(u_6-u_2\right)\left(u_7-u_2\right)\right]\right\}\\ a_{s3}=20/\left[\left(u_6-u_2\right)\left(u_7-u_2\right)\right]\\ a_{e1}=20/\left[\left(u_{m+8}-u_{m+4}\right)\left(u_{m+8}-u_{m+3}\right)\right]\\ a_{e2}=-20\left\{1/\left[\left(u_{m+8}-u_{m+4}\right)\left(u_{m+8}-u_{m+3}\right)\right]+1/\left[\left(u_{m+8}-u_{m+4}\right)\left(u_{m+9}-u_{m+4}\right)\right]\right\}\\ a_{e3}=20/\left[\left(u_{m+8}-u_{m+4}\right)\left(u_{m+9}-u_{m+4}\right)\right].\end{array}$$

(43)

From (42), all the control points can be obtained by

$$d=A_n^{-1}p$$

(44)

Bringing the control points back to (35), the angular displacement curve of each robot joint can be obtained. Then, the angular velocity, angular acceleration, and angular jerk curves of each joint are obtained by (41).

3.2. Establishing the Multi-Objective Optimization Model

This model consists of objective functions and constraint conditions. Firstly, the specific expressions of travel time, energy consumption, and smoothness are shown in (45)–(47). It is assumed that the trajectory of each joint of the Puma560 is divided into m segments, which means that each joint is assigned m + 1 angle values in turn.

The total travel time of the robot is the sum of the travel time of each trajectory, so its expression is

$$S_1=T={\displaystyle \sum _{j=1}^m\Delta t_j={\displaystyle \sum _{j=1}^m\left(t_j-t_{j-1}\right)}}$$

(45)

where $t_{j-1}, t_j, \Delta t_j$ are, respectively, the starting time, ending time, and travel time of the jth trajectory, and $T$ is the total travel time.

The average accelerations of joints represent the energy consumption

$$S_2={\displaystyle \sum _{i=1}^6\sqrt{\frac{1}{T}{\displaystyle \underset{0}{\overset{T}{\int }}{\ddot{\theta }}_i^{2}dt}}}$$

(46)

where ${\ddot{\theta }}_i$ is the acceleration of the ith joint.

The average jerks of joints measure the smoothness

$$S_3={\displaystyle \sum _{i=1}^6\sqrt{\frac{1}{T}{\displaystyle \underset{0}{\overset{T}{\int }}{\stackrel{⃛}{\theta }}_i^{2}dt}}}$$

(47)

where ${\stackrel{⃛}{\theta }}_i$ is the jerk of the ith joint.

Constraints of kinematics and dynamics of the robot need to be considered when the robot performs tasks. Kinematic constraints mainly include the limitation of joint angle, velocity, and acceleration. Dynamic constraints mostly refer to the restriction of the joint torque. Therefore, the multi-objective optimization model can be established as

$$\begin{array}{l}\mathrm{Minimize}\\ S={\left[S_1,S_2,S_3\right]}^T\\ \mathrm{Subject}\mathrm{to}\\ g_{i,1}={\theta }_i^{\min }-\min \left({\theta }_i\left(t\right)\right)\le 0\\ g_{i,2}=\max \left({\theta }_i\left(t\right)\right)-{\theta }_i^{\max }\le 0\\ g_{i,3}={\dot{\theta }}_i^{\min }-\min \left({\dot{\theta }}_i\left(t\right)\right)\le 0\\ g_{i,4}=\max \left({\dot{\theta }}_i\left(t\right)\right)-{\dot{\theta }}_i^{\max }\le 0\\ g_{i,5}={\ddot{\theta }}_i^{\min }-\min \left({\ddot{\theta }}_i\left(t\right)\right)\le 0\\ g_{i,6}=\max \left({\ddot{\theta }}_i\left(t\right)\right)-{\ddot{\theta }}_i^{\max }\le 0\\ g_{i,7}={\tau }_i^{\min }-\min \left({\tau }_i\left(t\right)\right)\le 0\\ g_{i,8}=\max \left({\tau }_i\left(t\right)\right)-{\tau }_i^{\max }\le 0.\end{array}$$

(48)

where ${\theta }_i\left(t\right), {\dot{\theta }}_i\left(t\right), {\ddot{\theta }}_i\left(t\right), {\tau }_i\left(t\right)$ are, respectively, the angle, velocity, acceleration, and driving torque of the ith joint at t time, and ${\theta }_i^{\max }, {\theta }_i^{\min }, {\dot{\theta }}_i^{\max }, {\dot{\theta }}_i^{\min }, {\ddot{\theta }}_i^{\max }, {\ddot{\theta }}_i^{\min }, {\tau }_i^{\max }, {\tau }_i^{\min }$ are, respectively, the upper and lower limits of the angle, velocity, acceleration, and driving torque of the ith joint.

3.3. Solving the Multi-Objective Optimization Model

The three optimization objectives conflict with each other, and there is a complex balance between them, so they cannot achieve the best solution simultaneously. When the improved MOPSO algorithm is used to solve the previous model, the result is no longer a single optimal solution, but an optimal solution set called the Pareto optimal solution set [36]. There is no good or bad solution in the Pareto optimal solution set, and the appropriate solution can be selected according to the actual engineering needs.

The MOPSO algorithm [37] originated from the study of bird foraging behavior and has the advantages of fast convergence speed, strong global search capability, and a wide range of application. In this algorithm, each particle’s position represents a solution to the problem. Under the influence of the individual optimal position and group optimal position, particles update their velocity and position

$$v_{id}^{k+1}=w{v}_{id}^k+c_1{r}_1\left(p_{id,pbest}^k-x_{id}^k\right)+c_2{r}_2\left(p_{d,gbest}^k-x_{id}^k\right)$$

(49)

$$x_{id}^{k+1}=x_{id}^k+v_{id}^{k+1}$$

(50)

where $w$ is the inertia weight, $c_1$ and $c_2$ are the individual and group learning factors, $r_1$ and $r_2$ are random numbers in the range of [0,1], $k$ is the current iteration number, $d$ is the vector’s dimension number, $x_{id}^k$ and $v_{id}^k$ are the position and velocity of particle i, and $p_{id,pbest}^k$ and $p_{d,gbest}^k$ are the optimal position of individual and group.

Furthermore, the algorithm determines the dominance relationship between particles by comparing the objective function values of particles. To better manage and preserve the non-dominated solution, it is saved to the external archive.

The traditional MOPSO algorithm determines the global leader and deletes the redundant particles in the external archive by random selection. The convergence, distribution uniformity, and accuracy of the Pareto front are not good. When facing complex problems, the algorithm easily falls into the local optimum.

In order to solve these problems, this paper uses the adaptive grid technology and roulette strategy to change the selection of the global leader and redundant particles in the external archive, applies the adaptive mutation technique to the position of particles, and makes the inertia weight, individual, and group learning factors change nonlinearly with the iterations number. The workflow flow chart of the improved MOPSO algorithm is show in Figure 3.

The combination of the adaptive grid technology and roulette strategy enables the algorithm to select particles in a suitable sub-grid. For each iteration, the maximum value $f_{i\max }$ and minimum value $f_{i\min }$ of the ith objective function on all particles are found as the upper and lower bounds of the initial grid. In order to cover the boundary particles, the range of the grid is expanded according to

$$L{B}_i=f_{i\min }-\alpha \left(f_{i\max }-f_{i\min }\right),U{B}_i=f_{i\max }+\alpha \left(f_{i\max }-f_{i\min }\right)$$

(51)

where $L{B}_i,U{B}_i$ are the new upper and lower bounds of the grid, and $\alpha$ is the expansion ratio.

The number of grids for each dimension is set to $n_d$, and the grid is equally divided into $n_d^e$ sub-grids, where $e$ is the number of objective function. Figure 4 shows the general process of adaptive grid technology.

Suppose that there are $n_s$ sub-grids containing particles, and the number of particles in the ith sub-grid is $N_i$. When selecting the global leader of particles, the probability that the ith sub-grid is selected is

$$P_{i,1}={\displaystyle \frac{e^{-\beta N_i}}{e^{-\beta N_1}+e^{-\beta N_2}+\cdots +e^{-\beta N_b}}}$$

(52)

where $i=1,2,\dots ,n_s$, and $\beta$ is a non-negative leader selection pressure parameter. It can be seen from (52) that the global leader of particles is more likely to come from sub-grids with fewer particles, which encourages the algorithm to explore areas that have been searched less before and increase the diversity of solutions.

When the number of particles in the external archive is greater than the set number, it is necessary to delete the redundant particles. At this time, the probability that the ith sub-grid is selected is

$$P_{i,2}={\displaystyle \frac{e^{\sigma N_i}}{e^{\sigma N_1}+e^{\sigma N_2}+\cdots +e^{\sigma N_b}}}$$

(53)

where $\sigma$ is a non-negative delete selection pressure parameter. It can be seen from (63) that the deleted redundant particles are more likely to come from the sub-grids with more particles, promoting the uniform distribution of the solutions.

Expressions (52)–(53) are the individual selection probabilities in the roulette strategy, and the cumulative probabilities of each grid are defined as

$$Q_{i,1}={\displaystyle \sum _{j=1}^i{P}_{j,1}},Q_{i,2}={\displaystyle \sum _{j=1}^i{P}_{j,2}}.$$

(54)

The individual selection strategy is to generate a random number $r\in \left[0,1\right]$, compare it with $Q_{i,1}$ or $Q_{i,2}$, find the first cumulative probability exceeding $r$, and select its sub-grid.

When the particle’s velocity and position are updated, the particle’s position is adaptively mutated. The mutation rate in the kth iteration is

$$p{m}_k={\left(1-{\displaystyle \frac{k-1}{M-1}}\right)}^{{\displaystyle \frac{1}{h}}}$$

(55)

where $k$ is the current iteration number, $M$ is the maximum iteration number, and h is a given constant.

The mutation step of the particle i is defined as

$$\Delta x_i^k=p{m}_k\cdot \left(x_{\max }-x_{\min }\right)$$

(56)

where $x_{\max },x_{\min }$ are the limit values of particle position. When $p{m}_k$ decreases nonlinearly with the increase in the iterations number, $\Delta x_i^k$ also decreases.

Now, it is randomly specified that the dth dimensional component $x_{id}^k$ of the position vector is mutated. The upper and lower bounds of the range of mutation are

$$lb=x_{id}^k-\Delta x_i^k,ub=x_{id}^k+\Delta x_i^k$$

(57)

A random number is generated in the continuous distribution of $lb$ and $ub$ as the value of the position vector after mutation in this dimension. This makes the algorithm jump out of the local optimum in the early stage and converge to the global optimal solution better in the later stage.

The relationship between the inertia weight and the number of iterations is

$$w=w_{\max }-\left(w_{\max }-w_{\min }\right){\left({\displaystyle \frac{k}{M}}\right)}^2$$

(58)

where $w_{\max },w_{\min }$ are the upper and lower limits of $w$. As the number of iterations $k$ increases, the inertia weight decreases nonlinearly.

The individual and group learning factors are defined as

$$c_1=c_{1s}+\left(c_{1e}-c_{1s}\right)\sin \left({\displaystyle \frac{\mathsf{\pi }k}{2M}}\right),c_2=c_{2s}+\left(c_{2e}-c_{2s}\right)\sin \left({\displaystyle \frac{\mathsf{\pi }k}{2M}}\right)$$

(59)

where $c_{1s}$ and $c_{2s}$ are, respectively, the initial values of $c_1$ and $c_2$, and $c_{1e}$ and $c_{2e}$ are, respectively, the final values of $c_1$ and $c_2$. As the number of iterations increases, $c_1$ decreases from large to small, and $c_2$ is the opposite.

These give the particle a strong global search capability at the beginning of the algorithm iterations to avoid falling into local optimum and a strong local search capability at the later stages to improve convergence accuracy.

4. Simulation

In this section, the multi-objective trajectory planning simulation of the Puma560 robot is carried out in MATLAB. The constraints of each joint are shown in

Table 2. Kinematic and dynamic constraints.

Constraints	Joint 1	Joint 2	Joint 3	Joint 4	Joint 5	Joint 6
Angle/(rad)	3.100	3.100	3.100	3.100	3.100	3.100
$\mathrm{Velocity}/(\mathrm{rad}\cdot {\mathrm{s}}^{-1}$)	0.876	0.876	1.598	0.876	0.926	0.926
$\mathrm{Acceleration}/(\mathrm{r}\mathrm{a}\mathrm{d}\cdot {\mathrm{s}}^{-2}$)	0.725	0.725	2.378	0.725	1.450	1.450
$\mathrm{Torque}/(\mathrm{N}\cdot \mathrm{m}$)	44.940	44.940	8.866	44.940	0.050	0.050

.

The target captured by the robot is a small ball moving at a constant speed of 0.5 m/s along the Z-axis, and its trajectory is known. Six key positions of each joint in the process of catching the ball are given, as shown in

Table 3. Position sequence of each joint.

Node	Joint 1/(rad)	Joint 2/(rad)	Joint 3/(rad)	Joint 4/(rad)	Joint 5/(rad)	Joint 6/(rad)
1	0.5821	−0.3805	−0.8168	0.6283	−0.9390	0.2531
2	0.4829	−0.3735	−0.7981	0.6299	−0.9245	0.2621
3	0.0383	−0.1212	0.0608	0.4289	−0.4005	0.6502
4	−0.5872	0.1770	1.0702	0.1995	0.2189	1.1091
5	−1.0317	0.3890	1.7877	0.0364	0.6592	1.4352
6	−1.1310	0.4363	1.9478	0	0.7547	1.5080

.

The MOPSO algorithm takes the time interval of each trajectory $\Delta t_j$ as the decision variable, which is in the range of [0.75, 7]. The population size and the maximum iteration number of the traditional and improved MOPSO algorithms are both 200, and the size of the Pareto optimal solution set is 100. In addition, this paper sets $n_d$ in the improved MOPSO algorithm to 5, $\beta$ to 2, and $\sigma$ to 2.

The Pareto front obtained by the traditional MOPSO algorithm is shown in Figure 5a. It falls into the local optimum, and the distribution and convergence of the Pareto front are also poor. The Pareto front obtained by the improved MOPSO algorithm is shown in Figure 5b. It jumps out of the local optimum, and the convergence and distribution are significantly improved, which proves the effectiveness of the improved MOPSO algorithm.

Four points are taken on the Pareto front, which, from top to bottom, are A, B, C, and D. The closer to A, the shorter the travel time, the more the energy consumption, and the greater the jerk; the closer to D, the less the energy consumption, the smaller the jerk, and the longer the travel time. It follows that smoothness is positively correlated with energy consumption, while they are negatively correlated with travel time. The values of the three objective functions for A, B, C, and D are shown in

Table 4. The partial optimum solution.

Solution	Travel Time/($\mathit{s}$)	$\mathbf{Energy}\mathbf{Consummation}/(\mathbf{r}\mathbf{a}\mathbf{d}\cdot {\mathbf{s}}^{-2}$)	$\mathbf{Jerk}/(\mathbf{r}\mathbf{a}\mathbf{d}\cdot {\mathbf{s}}^{-3}$)
A	3.7566	3.2251	8.2585
B	4.8760	1.6688	3.0157
C	9.0883	0.4932	0.4656
D	39.5825	0.0286	0.0069

.

Taking B and C points as examples, the travel time of point B is 4.8760 s, 46.35% less than that of point C. The energy consumption of point C is 0.4932 $\mathrm{rad}\cdot {\mathrm{s}}^{-2}$, 70.45% lower than that of point B. The jerk of point C is 0.4932 $\mathrm{rad}\cdot {\mathrm{s}}^{-3}$, 70.45% lower than that of point B.

C is selected as the actual solution of the project, and its time series is [0, 1.1990, 3.6445, 5.3612, 7.2749, 9.0883]. As shown in Figure 6, the curves of joint angles, velocities, accelerations, and jerks varying with time can be obtained by interpolating fifth-order B-spline curves.

A dominant solution E is randomly selected outside the Pareto optimal solution set as the time series [0, 1.3, 2.4, 5.3, 8.4, 10.4] before the trajectory optimization. Under this time series, the three performance indexes of the robot are shown in

Table 5. Comparison before and after optimization.

Solution	Travel Time/($\mathbf{s}$)	$\mathbf{Energy}\mathbf{Consumption}/(\mathbf{r}\mathbf{a}\mathbf{d}\cdot {\mathbf{s}}^{-2}$)	$\mathbf{Jerk}/(\mathbf{r}\mathbf{a}\mathbf{d}\cdot {\mathbf{s}}^{-3})$
C	9.0883	0.4932	0.4656
E	10.4000	1.1457	1.8733

.

According to the data in Table 5, the travel time of point C is 12.61% higher than that of E, the energy consumption is increased by 56.95%, and the jerk is increased by 75.15%. The three objective function values of point C are better than the results before optimization, improving the robot’s comprehensive performance.

Taking robot joint 2 as an example, Figure 7 shows that the trajectories after optimization are smoother and more continuous than before optimization, especially the curves of joint velocity, acceleration, and jerk.

An optimal solution is randomly selected in the Pareto optimal solution set, called D, and its time series is [0, 1.7255, 4.5821, 6.1810, 7.7798, 10.0000]. The motion of the Puma560 robot under this solution can be visualized by the Robot Toolbox of MATLAB, as shown in Figure 8.

5. Conclusions

This paper investigates a trajectory planning method for a robot which enables it to reach a comprehensive optimal state of travel time, energy consumption, and smoothness when executing a task. In order to fully understand the kinematics and dynamics characteristics of the robot and lay a solid theoretical foundation for follow-up research, this paper first deduces the position and orientation of the end-effector relative to the base and uses the Pieper method to calculate the closed solutions of the inverse kinematics. Finally, the dynamic model of the robot is established by the iterative Newton–Euler dynamics algorithm.

The joint space trajectory of the Puma560 robot is constructed using fifth-order B-spline curves, which has the advantages of continuous jerk and zero velocity and acceleration at the start/stop time. Then, the improved MOPSO algorithm is used to optimize the trajectory of the robot with the time interval between the path points as the decision variable. The convergence and distribution of the Pareto front are good, and the different solutions in the Pareto optimal solution set correspond to different engineering needs. In addition, by comparing the robot’s travel time, energy consumption, and smoothness before and after optimization, it can be seen that its three performances have improved. This paper also visualizes the robot movement according to the planned trajectory in the Robot Toolbox of MATLAB.