Evolutionary multi-objective trajectory optimization for a redundant robot in Cartesian space considering obstacle avoidance

A method of end-effector trajectory planning in Cartesian space based on multi-objective optimization is proposed in this paper to solve the collision problem during the motion of the redundant manipulator. First, a cosine polynomial function is used to interpolate the trajectory of the end effector, enabling it to reach the desired pose at a specific time. Then, the joint trajectory of the manipulator is solved by inverse kinematics, and the null space term is introduced as the joint limit constraint in the inverse kinematics equation. During the operation of the manipulator, the collision detection algorithm is employed to calculate the distance between the obstacle and each arm in real time. Finally, a multi-objective, multi-optimization model of trajectory that considers the obstacle avoidance, joint velocity, joint jerk and energy consumption is established and optimized with a multi-objective particle swarm optimization algorithm. The simulation results demonstrate that the proposed method can effectively accomplish the trajectory planning task and avoid obstacles; the joint trajectories obtained are smooth and meet the limit constraints; the joint jerk and energy consumption are well suppressed.


Introduction
In many of today's vastly complex and difficult manufacturing applications, manipulators may have more degrees of freedom (DOFs) than essential due to the need of executing intended complicated jobs like human arms. Specifically, the end effector of redundant manipulator should satisfy a desired trajectory while its body avoids colliding with obstacles in the environment, contributing to finishing the job. The extremely crucial obstacle avoidance problem is that robotic manipulators are required to move from an initial pose to a specified final objective without colliding with any obstacles in the workspace.
To solve the trajectory planning problem considering obstacle avoidance, many researchers have focused on different methods. For example, Ma and Liang (2020) proposed an obstacle avoidance algorithm for a 6-DOF manipulator based on a sixth-degree polynomial, with limitations to the redundant manipulator. For the obstacle-avoiding motion of the end effector of the redundant robot, Shen et al. (2014) presented an obstacle avoidance algorithm based on the transition between the primary and the secondary tasks to acquire a smooth and continuous trajectory. Dalla and Pathak (2017) put forward a methodology that a limited number of joints are actuated and a curve-constrained link trajectory tracking is developed to control hyper-redundant space robots. However, these are only for planar robots. Shi et al. (2019) constructed a visual servoing system, which obtained the image errors by the ORB feature points and constructed a virtual repulsion torque to avoid obstacles.
In addition to the above methods, more researchers handle the obstacle avoidance problem by introducing the concept of potential field to construct the artificial potential field. Safeea et al. (2020) redefined the obstacle avoidance problem as a second-order optimization problem and constructed the potential function that is minimized by Newton's method to realize real-time trajectory calculation in joint space. It lacks the intuitiveness and efficiency compared with Carte-sian space trajectory planning. Wang et al. (2018) realized trajectory planning and obstacle avoidance through the improved artificial potential field method with the consideration of both position and posture of manipulator end effectors. Park et al. (2020) employed a Jacobian matrix-based numerical method and an improved potential field method to achieve path planning and obstacle avoidance, both in redundant and non-redundant conditions.
The above-mentioned trajectory planning methods focus more on obstacle avoidance than kinematics or dynamics optimization. Nowadays, an optimized path should be found for robot motion regarding minimum time (Bourbonnais et al., 2015), minimum distance (Salaris et al., 2010), and minimum energy (Arakelian et al., 2011) given the robot optimization problem in obstacle avoidance planning. This suggests that multiple objective functions should be simultaneously optimized to obtain a suitable trajectory. Based on the above articles, some criteria for evaluating manipulator trajectory can be obtained: 1. Trajectories need to be predictable, accurate, and effective to both compute and execute.
2. Trajectories need to ensure that the joints of the robotic arm do not appear singular.
3. Trajectories should be found for a manipulator with respect to minimum time, minimum distance, and minimum energy.
Many researchers used an evolutionary algorithm to address multi-objective planning problems considering obstacle avoidance. For example, Saravanan et al. (2008) optimized two evolutionary algorithms of an elite non-dominant sorting genetic algorithm (NSGA-II) and differential evolution (DE) to handle the multi-objective problem and achieved the predetermined goal in the PUMA560 robot. Han et al. (2013) realized the obstacle avoidance path planning optimization of the robot arm in the environment with single or multiple obstacles based on genetic algorithms with execution time, space distance, and trajectory length as the optimization parameters. However, most of the above researchers have weighted each constraint as an objective function, making the result very dependent on the setting of the weight factor. Some researchers transform multi-objective problems into quadratic programming (QP) problems. Chen and Zhang (2017) proposed a hybrid multi-objective scheme including the specified primary task for the end effector, obstacle avoidance, joint-physical limits avoidance, and repetitive motion of redundant robot manipulators and transformed it into a dynamic quadratic program (DQP) problem. Zhang et al. (2020) studied two schemes of hybrid end-effector pose-maintaining and obstacle-limits avoidance (PM-OLA) for redundant robot motion planning based on QP framework to solve the end-effector drift problem in the planar task. Other researchers obtained the Pareto front in multiobjective problems, as well as the non-dominant solution. Guigue et al. (2010) employed the discrete dynamic programming (DDP) approximation method to obtain an approximate solution of the Pareto optimal set to handle multiobjective robotic trajectory planning problems. Marcos et al. (2012) combined a closed-loop pseudo-inverse method with a multi-objective genetic algorithm to control the joint position and choose a solution from the Pareto solution set. Although these researchers solved the multi-objective problem, they did not add energy consumption constraint to the multi-objective problem for obtaining a superior solution with more energy-saving or less joint jerk.
Besides, many researchers have improved the particle swarm optimization (PSO) algorithm to acquire the optimized trajectory of the robotic arm. Cao et al. (2016) adopted a hybrid algorithm combing crossover and mutation particle swarm optimization (CMPSO) with the interior point method (IPM) to determine the minimum-length motion path of robot end effector and the best movement posture of the robot arm. Lin (2014) proposed a k-means clustering method based on PSO to address the near-optimal solution of the minimum-jerk joint trajectory, making it easy to obtain the optimized trajectory control law. Kucuk (2017) developed an optimal trajectory generation algorithm (OTGA) for generating smooth motion trajectories with minimum time with the PSO algorithm. Under the diversification of robot optimization goals, some researchers used a multi-objective particle swarm optimization (MOPSO) algorithm (Coello Coello and Lechuga, 2002) to solve multi-objective optimization problems. For instance, Yan et al. (2020) presented multiobjective configuration optimization for the maximum maneuverability and minimum base disturbance in the precontact stage of dual-arm space robot by MOPSO. Liu and Zhang (2021) proposed an improved external archives self-searching multi-objective particle swarm optimization (EASS-MOPSO) algorithm to handle the multi-objective trajectory optimization problem.
In this paper, regarding obstacle avoidance, the change rate of joint torque, joint angular velocity, and energy consumption are minimized using the path planning method based on a multi-objective optimization algorithm. The main contributions of this paper are highlighted as follows.
1. A cosine polynomial function is used to interpolate the trajectory of the end effector, enabling it to reach the desired pose at a specific time, and the null space term is introduced as the joint limit constraint in the inverse kinematics equation.
2. During the operation of the manipulator, a collision detection algorithm is employed to calculate the distance between the obstacle and each arm in real time.
3. The trajectory optimization model of redundant manipulator was established to consider obstacle avoidance, joint velocity, joint jerk and energy consumption, and the optimized trajectories were obtained by a multiobjective particle swarm optimization algorithm.
The remaining paper is organized as follows. In Sect. 2, the end trajectory of the robotic arm is interpolated using the cosine fifth-degree polynomial, allowing the end to reach the specified pose at the specified time. Then, the joint trajectory of the robotic arm is reversed through inverse kinematics. Section 3 presents the collision detection algorithm used in this paper. Afterward, the multi-objective optimization problem equations are described in Sect. 4. The obstacle avoidance trajectory planning based on multi-objective optimization is introduced in Sect. 5. Simulation results are analyzed in Sect. 6. Finally, conclusions are drawn in Sect. 7.

Trajectory parameterization of end effector in Cartesian space
A five-order cosine polynomial is adopted to describe the trajectory to ensure the position, orientation, velocity, and acceleration of the end effector change smoothly and continuously. With this method, the motion of the end effector follows a helical curve in Cartesian space (Zghal et al., 1990). The pose of the end effector is taken to compose a six-dimensional generalized coordinate vector X e = [X 1 (t), . . ., X 6 (t)] T as the status variable of the system. The trajectory is parameterized as where X j 0 denotes the initial pose of end effector, and λ, T and ϕ are trajectory parameters, j = 1, . . ., 6. By taking the differential of the equation above, the generalized velocity and acceleration of the end effector can be expressed aṡ where K j (t) can be described as follows: The boundary conditions of the trajectory planning problem are where t 0 and t f represent the initial and final time, respectively; X j f refers to the desired pose of end effector. By substituting Eq. (4) into Eqs. (1) and (2), the following relationship can be obtained: where ψ and D j can be described as According to Eq. (5), polynomial coefficients can be determined with parameters T j and ϕ j . Then trajectories of end effector that satisfy the constraints can be obtained with the following parameters vector:

Joint motion planning for redundant manipulator
The subject of this paper is a n-DOF redundant manipulator. Assuming the dimension of task space is m(m ≤ 6), a redundant manipulator satisfies n > m. The joint velocity vector of redundant manipulator iṡ where x e ∈ R m indicates the pose of end effector in Cartesian space; q ∈ R n denotes joint angle vector; J ∈ R n×m represents the Jacobian matrix of manipulator; I ∈ R n×n refer to unit matrix;˙ ∈ R n stands for an arbitrary n-dimensional vector; (I−J + J) ∈ R n×n designates the null space projection matrix; J + ∈ R n×m means the generalized inverse of the Jacobian matrix, which is defined by For redundant manipulators, a specific trajectory of end effector can be obtained by multiple joint trajectories. Hence, the joint trajectory should be selected to ensure that the motion of the manipulator follows specific constraints. Null space (Zghal et al., 1990) term is introduced in this paper to enable the joint motion to satisfy joint limit constraints. The joint limit constraint function is established as where q i donates the angle of joint i; q imax and q imin represent upper and lower limits of joint i, respectively. According to the gradient projection method, the joint angle limit constraints are resolved by substituting k∇H (q) foṙ in Eq. (8) aṡ where the gradient vector ∇H (q) is given by ∇H (q) reaches minimum value when q i = (q imin +q imax )/2; ∇H (q) approaches to infinity when q i = q imin or q i = q imax .

Collision detection algorithm
A collision detection algorithm should be established to achieve obstacle avoidance. A redundant manipulator is explored in this paper. Most of the links are like cuboids or cylinders. Moreover, obstacles are irregular geometric objects. The collision detection between the manipulator and obstacles is extremely complicated. Thus, sphere and cylindrical envelopes are applied in this paper to describe the manipulator links, joints, and obstacles.

The description of space geometry relation
Considering that obstacles in three-dimensional space generally have irregular geometric shapes and various kinds, it is difficult to extract their boundary contours. Thus, regular body envelopment is adopted in this paper to simplify the obstacle model, so as to simplify the complexity of obstacle modeling. For example, the cylindrical envelopment model and sphere envelopment model are employed for slender rod and rounded objects, respectively. This modeling method expands the range of obstacles while guaranteeing the reliability of the collision detection algorithm. The obstacle uses a sphere envelope, as illustrated in Fig. 1. The radius of the sphere is r S . The cylindrical envelope is used for the links of the manipulator while the sphere envelope and cylindrical envelope are adopted to describe the joint. In this paper, a 7-DOF redundant manipulator (Fig. 5) is studied. As exhibited in Fig. 2. The two typical cases are described as follows. 1. For case I, when axes of adjacent links are vertical, straight connecting links (link i − 1 and link i) are enveloped by cylinders whose axes are coplanar. The intersection points P i of cylindrical axes are centers of joint envelope spheres. The radius of the cylinder for the link i is r i ; the radius of the sphere for the joint i is Hence, the distance between links and obstacles can be substituted as the distance from a line segment to a point in 3-dimensional space.
2. For case II, when axes of adjacent links are parallel, joint i and link i − 1 are enveloped in the same way as case I considering the joint with offset. Hence, r j i ≥ r 2 joint + r 2 i−1 , where r joint indicates the radius of envelope cylinder for joint i. Two cylinders whose axes are coplanar and a sphere whose center is located in the intersection point (P i ) of the axes are applied to envelop it.

Collision detection algorithm
The space geometry (Guan et al., 2015) relations between manipulator's links and obstacle are divided into three classes, as exhibited in Fig. 3. Whether the links and obstacle collide depends on d i , it denotes the minimum distance between the obstacle and manipulator's link/joint. If d i > 0, the obstacle is avoided; otherwise, it collides with the manipulator. Therefore, the collision detection problem between cylinder and sphere can be reduced to the calculation of the shortest distance between a point and line segment in 3D space.
Assume the length of link i is L i . Centers of the bottom are P i and P i+1 . A vertical line of segment P i P i+1 is drawn through P 0 , and P vi indicates the point of intersection to the cylindrical axis.
Based on the space geometry relation, the collision detection algorithm (Cheng et al., 1993) to achieve obstacle  avoidance path planning for redundant manipulators is summarized as follows.
Step 1. Based on forwarding kinematics, the coordinates P i of all nodes and joints are calculated with the current configuration and geometrical sizes of the manipulator.
The relative translation and rotation between link i coordinate and link i − 1 coordinate can be described with a homogeneous transformation matrix T i i−1 . The kinematics equation of n degree of freedom manipulators can be expressed as Step 2. P i P vi is defined as the projection vector of P i P 0 on P i P i+1 , and the scale factor λ is calculated: The three-dimensional coordinates of the perpendicular foot P vi can be obtained with P vi = P i + λ(P i+1 − P i ).
Step 3. The minimal distance from the links/joints to the obstacles is calculated. There are three possible positions of perpendicular foot P vi related to P i P i+1 : -For class I, P vi is on P i P i+1 when ≤ λ ≤ 1.
-For class II, P vi is on the reverse extension of P i P i+1 when λ < 0.
-For class III, P vi is on the extension of P i P i+1 when λ > 1.
Therefore, the minimum distance between P 0 and P i P i+1 is Step 4. If min{d 1 , d 2 , . . ., d n } < 0, a collision occurs. Then, the collision information is outputted. According to the collision condition, obstacles can be avoided if min{d 1 , d 2 , . . ., d n } > 0.

Description of multi-objective trajectory planning problem
In this study, minimization of total energy, joint jerks, and avoidance of obstacles are considered together to build a multi-criterion cost function. Singularity involved in null space and joint limits is considered a constraint.

Establishment of objective functions
1. For a given trajectory of the end effector, the smaller the joint velocity is, the less likely the joint angle mutation occurs. Therefore, the optimization of joint angular ve-locity has the effect of avoiding singularity. Meanwhile, the null space term is used to constrain the joint limit when solving inverse kinematics. Since the collision detection algorithm is also the kinematic solution of the manipulator in essence, the obstacle avoidance term is taken as a penalty function in this paper to fuse with the joint angular velocity optimization objective. The specific expression is where obt denotes obstacle avoidance term; λ 1 and λ 2 are scale factors.
Combining with Eq. (11), the kinematic optimization term can be expressed as Due to the scale factors, joint angular velocity is the major term when obstacles are avoided. Besides, the objective function is larger when the links failed to avoid the obstacle. This is convenient for choosing feasible solutions in the process of trajectory optimization.
2. For redundant manipulators, the frequent changes in the joint jerk would cause a great impact. Reducing the joint jerk can indirectly limit the change rate of joint torque and thus make the movement of the manipulator more stable. This effectively reduces the execution error of the manipulator and resonance caused by the movement and wear on the joint mechanism. The jerk is the third derivative of the joint angle regarding time, and its optimization objective function is 3. Total energy consumption may significantly increase when the manipulator is trying to find an obstacle avoidance trajectory with a small joint jerk. This may result in frequent acceleration and deceleration, which should also be avoided in the manipulator's motion. The optimization objective function is where τ i denotes the torque of joint i. Joint torque is calculated by the following dynamics equations (Saramago and Junior, 2000). where C ij k = ∂D ij ∂q k − ∂D j k 2∂q i ; n j =1 D ij (q)q j indicates the acceleration term; D ij represents inertial system matrix. n j =1 n k=1 C ij k (q)q jqk stands for quadratic velocity term; C ij k refers to Coriolis and centripetal forces matrix; g i (q) designates a configuration-dependent term that depends on the gravity.

Description of multi-objective trajectory planning problem
The trajectory of the manipulator's end effector is only decided by (T j ϕ j ) according to Eqs. (3) and (5). Different trajectories can be obtained by changing the values of (T j ϕ j ) in the cosine polynomial when initial conditions are given. Hence, T j and ϕ j are selected as the decision variables in the following optimization. The range of them is defined as the feasible region: ∀ξ ∈ : T min ≤ T j ≤ T max ∧ ϕ min ≤ ϕ j ≤ ϕ max , j = 1, 2, . . ., 6.
For redundant manipulators, the core of the multi-objective trajectory planning problem is to determine the decision vector ξ that will optimize the following objective vector: ξ is defined in Eq. (7), which generates the trajectory of the end effector in Cartesian space. The objective vector F = [f 1 , f 2 , f 3 ] T is composed of the objective functions' values. The constraints in the optimization problem include the initial and final.
During the trajectory planning process, the joint limit constraints are q i (t) is angle of joint i at time t.

Trajectory planning based on multi-objective optimization
It is difficult to find a global minimum solution for multiobjective optimization problems (MOPs). In MOP (Liu et al., 2015), a vector ξ dominates ξ * (ξ ξ * ) when the following Pareto dominance principle is satisfied.
The solution satisfying Eq. (24) is defined as a nondominated solution of Eq. (22). Respecting the given multi-objective trajectory optimization problem, the Pareto optimal solution set S is defined as The mapping of S is a three-dimensional surface called Pareto front. Therefore, discovering as many non-dominated solutions as possible to cover all features of MOP is the key to solving the proposed trajectory planning problem. MOPSO algorithm is adopted to solve the multi-objective trajectory optimization problem. This algorithm employs an external repository of particles as guidance to guide the evolution of the whole population toward the Pareto front. Only non-dominated solutions can be stored in the repository according to the following process. The velocity formula of particle's flight consists of not only the position and velocity of the current particle (P [k], V [k]) but also the best position that all the particles ever had in the repository (R[h]): represents the constriction factor; φ = C 1 + C 2 , where C 1 and C 2 refer to the learning factors regulating the step size of the particles flying to P best (the best position a particle has ever explored) and the best position that all the particles ever had, respectively; R 1 and R 2 indicate independent random numbers in the range [0, 1]. Combining with the trajectory planning algorithm and MOPSO mentioned above, the obstacle avoidance trajectory optimization can be concluded as follows.
1. The population containing n P particles is initialized, and the range of decision variables is defined as [T min , T max ] and [ϕ min , ϕ max ]. Decision variables ξ are randomly generated, and the trajectory of the end effector is calculated by Eq. (1).
2. The joint trajectories are solved according to inverse kinematics, and then the values of three objective functions are obtained following kinematics and dynamics calculation results.
3. The repository is initialized, and its maximum allowable number is restricted to n R . Next, the positions of particles that represent non-dominated decision vectors in the initial population are stored. Then, the best positions that the particles have had are initialized.
4. While the maximum number of I max has not been reached, evolutionary computations are performed to update and maintain the repository. The velocities of particles are calculated using Eq. (26), and the position of each particle is updated. Besides, particles that failed to avoid the obstacle are eliminated. The reserved solutions are added into the repository according to the process as shown in Fig. 4. The loop counter is incremented until the maximum number is reached.
5. The non-dominated solutions are obtained from the repository. The particles constitute the set of nondominant solution S in this paper.

Simulation results and analysis
In this paper, a 7-DOF redundant manipulator is adopted to simulate the approached algorithm. The parameters are exhibited in Fig. 5. Mass parameters are provided in Table 1 ], [λ min2 , λ max2 ] = [−π, +π ], n P = 1000; n R = 1500; C 1 = C 2 = 2.05. Figure 6 illustrates the minimum value of three objective functions during the iteration, which demonstrates the excellent astringency and stability of the proposed algorithm. The Pareto front is exhibited in Fig. 7. The particles are divided into two categories owing to the penalty function. Red particles satisfy the obstacle avoidance constraint while blue ones do not. Although the screening process can be executed to remove all blue solutions in each iteration, they are reserved to demonstrate the effectiveness of trajectory optimization for obstacle avoidance in this paper. According to the obstacle avoidance constraint, all the remaining non-dominant 50 Y. Liu et al.: Evolutionary multi-objective trajectory optimization for a redundant robot  solutions successfully avoid the obstacle after the particles that fail to avoid the obstacle are removed. This reflects that the proposed algorithm has a good performance in obstacle avoidance.
The first objective function is merely adopted to avoid singularity and obstacles. It will not affect the other physical   trajectories significantly vary, verifying the necessity of trajectory optimization.
Given no optimal solution to the multi-objective optimization problem, all the solutions are non-dominant. In this paper, the non-dominant solutions corresponding to the minimum values of f 2 and f 3 are selected for comparative analysis. It is assumed that the non-dominant solutions are S 2 and S 3 . The decision vectors and objective vectors of S 2 and S 3 are presented in Table 2.
The minimum distances d i between the center of obstacles and the manipulator's links are illustrated in Fig. 8. The minimum distances for S 2 and S 3 are 0.0597 and 0.0014 m, respectively. The trajectory obtained in S 2 and S 3 successfully avoided the obstacle. The configuration change of the manipulator and its geometric relationship with the obstacle are displayed in Fig. 9. The position and orientation change in the end effector are shown in Fig. 10. The manipulator reached the desired pose with different changing processes.
The change in joint angular over time is exhibited in Fig. 11. Since the joint angular velocity is one of the optimization objectives, no mutation of joint angle occurs. The change in joint angle is smooth. Moreover, the variation range of joint angle is also inhibited with the suppression of joint angular velocity. For solution S 3 , the joint angle of link i is still suppressed due to the null-space term, though it approaches the limitation at 24 s. Figure 12 provides the change in joint angular velocities. For S 3 , the joint angular velocity changes abruptly when the joint angle approaches the joint limit, forcing the joint angle of the manipulator to return to the range of the joint limit. This also validates the effectiveness of the null-space term as the joint angle constraint in this paper. The shaking process endured for 2 s and finally stabilized. Besides, lower energy cost always demands higher joint jerk, according to Fig. 7. It takes more energy to achieve a smooth angular velocity change, which reduces the impact on the joint mechanisms for redundant manipulators.

Conclusions
In this paper, a method of obstacle avoidance trajectory planning based on a multi-objective optimization algorithm in Cartesian space is proposed to avoid obstacles during the redundant manipulator's motion. On the premise that the end effector reaches the desired position and orientation at a specific time, obstacle avoidance can be achieved successfully, and joint jerk and energy consumption can also be inhibited. The results are described as follows.
1. The cosine polynomial function was employed to realize the task requirements of the end effector reaching the desired pose at a specific time. Then, the inverse kinematics and null space inverse solution were introduced to meet the joint angle limit constraint.
2. The multinomial weighted objective function is established in a combination of joint velocity and obstacle avoidance function. By optimizing the proposed function, the manipulator avoids not only the obstacle, but also the joint singularity.
3. Multi-objective particle swarm optimization was performed to optimize joint jerk and energy consumption simultaneously. The clear physical meaning of the objective functions is beneficial for the selection of nondominant solutions.
The trajectory optimization method proposed in this paper possesses the strong flexibility and diversity. Obstacle avoidance trajectories can be obtained by inputting the initial and expected pose of the end effector. Several non-dominant solutions can be provided for selection. Therefore, this method has an acceptable prospect in engineering applications. However, the collision detection in this paper was implemented based on static obstacles, with all known environment variables. Thus, how to realize dynamic obstacle avoidance in an unknown environment needs to be investigated in the future. Data availability. The data are available upon request from the corresponding author.
Author contributions. YL, XL, PJ, and ZD planned the campaign; PJ, ZD, BS, and ZW wrote code and performed the simulations; XL, XH, YL, and ZW analyzed the data; XL, BS, XH, and PJ wrote the manuscript draft; YL, XL, ZW, and ZD reviewed and edited the manuscript.
Competing interests. The contact author has declared that neither they nor their co-authors have any competing interests.

Disclaimer.
Publisher's note: Copernicus Publications remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.
Financial support. This research has been supported by the National Natural Science Foundation of China (grant no. 52175013) and the Fundamental Research Funds for the Central Universities (grant no. JZ2020HGTB0034).
Review statement. This paper was edited by Zi Bin and reviewed by three anonymous referees.