Multiobjective preimpact trajectory planning of space manipulator for self-assembling a heavy payload

To assemble a heavy payload to the spacecraft (free-floating base), the present study proposes a scheme of multiobjective trajectory planning for preimpact motion of redundant space manipulator (mounted on the base). Force impulse for self-assembly is derived as the function of joint angles/velocities, base pose, and impact direction. The trajectory planning problem is formulated as multiobjective optimization to minimize force impulse, base attitude disturbance, and energy consumption in the load-carrying process. A two-stage trajectory planning algorithm is proposed. To be specific, at the first stage, multiple desired configurations at the contact point are generated by position-level inverse kinematics with Newton–Raphson iterative method. At the second stage, joint trajectories satisfying joint angle limits and desired motion of the payload are parameterized by coefficients of sinusoidal polynomial functions. Multiobjective particle swarm optimization algorithm is adopted to solve the problem of multiobjective trajectory planning, and screening process is conducted to reserve nondominated solutions in limits of joint torques. The algorithm is implemented to a seven degrees of freedom space manipulator, and the effectiveness of the proposed method is verified by simulation results.


Introduction
Space manipulators are increasingly critical to future on-orbit servicing, 1 especially to capture, assemble, and maintain large-space structures. 2 After capturing a heavy payload, the space manipulator mounted on the spacecraft base is employed to assemble payload to the base. In the preassembly operation, space manipulator should carry the payload from capture pose (including position and orientation) to preassembly pose. However, for the dynamic coupling effect between space manipulator and spacecraft, 3 attitude disturbance of the base induced by heavy payload should be considered in the motion planning process. Existing studies considering payload constraints were conducted for ground manipulators. [4][5][6] For on-orbit load-carrying operation, a multiobjective optimization-based approach was proposed to achieve the maximum-payload trajectory planning of free-floating space manipulator (FFSM). 7 Dalla and Pathak proposed a method for multiple space manipulators to perform a docking task in the control of base disturbance. 8 However, impact problem has not been considered in the mentioned studies.
For space manipulators, the effect exerted by impact force should be analyzed. Yoshida et al. divided the capture mission into three phases and proposed extended generalized inertia tensor to formulate collision dynamics. 9 Chen et al. explored the impact dynamics and the motion after impact. 10 Huang et al. derived the force and momenta transmission from the end-effector to the base under impact between the end-effector and the target. 11,12 For the trajectory planning problem involving impact force influence, Wee et al. developed a joint space planning strategy that achieves both trajectory tracking and impact minimization. 13 Zhang et al. investigated a desired configuration at the contact point; subsequently, they proposed a scheme of preimpact trajectory planning to achieve minimized base attitude disturbance caused by impact, 14 whereas the results were achieved by depending on a specified base pose. In the present study, base disturbance in preimpact motion is not considered, which cannot be neglected when carrying a heavy payload. Cong and Zhang analyzed a dual-arm space manipulator and proposed the "straight arm capture" concept, which is not suitable for self-assembly. 15 However, the mentioned studies were conducted for capture tasks, in which impact influences on space manipulator were exerted by an external force, and impact force displayed the only relationship to the configuration of the manipulator. For impact process of self-assembly, the collision between the base and the payload should be considered as internal force. Base suffers directly impact the assembly mechanism and the disturbance force transmitted from payload to the base.
When carrying a heavy payload, the force impulse can be extremely large, which may damage payload, spacecraft, and manipulator. Large disturbance to the base may take place for the nonholonomic characteristics, 16 which is not expected since the requirements of communication and observation. Moreover, for large mass and inertia tensor of payload, energy consumption involved in load-carrying operation can significantly increase. Given task constraints and limited capacity of space manipulator, multiple objectives and constraints should be considered. Misra and Bai and Luo et al. adopted quadratic programming method to optimize base disturbance and avoid collision. 17,18 Similar method was employed for local optimization of reaction torque and joint torque simultaneously. 19 The taskpriority-based null-space trajectory planning problems were studied for redundant space manipulator. [20][21][22] Moreover, evolutionary algorithms were adopted to perform optimization, such as multiswarm particle swarm optimization (PSO), 16 differential evolution algorithm, 23 and genetic algorithm. 24 In the mentioned studies, trajectory planning was altered to a single-objective problem, and weighting coefficient exists in cost functions to determine the weight of each objective. Wang et al. explored the application of PSO to trajectory planning and discussed nondominated solutions. 25 The Pareto Front was obtained by mapping the solutions of single-objective optimization to objective space, multiobjective evolutionary computation and dominance relations were uninvolved. Multiobjective optimization was introduced for mobile and industrial manipulators, 26,27 which also applies to trajectory planning of space manipulator.
In the present study, a scheme of multiobjective preimpact trajectory planning is proposed for self-assembly task of space manipulator when carrying a heavy payload. The rest of this study is organized as follows: The second section analyzes the force impulse for self-assembly task. In the third section, how to formulate trajectory planning as a multiobjective constrained optimization problem is discussed. The fourth section illustrates the two-stage algorithm that combines of solving multiple configurations and multiobjective evolutionary computation. The fifth section presents the simulation results of a seven degrees of freedom (7-DOF) space manipulator. The conclusions of the work are presented in the sixth section.

Modeling of free-floating space manipulator for self-assembling a heavy payload
Mathematical model of space manipulator with a payload Figure 1 shows the general model of a free-floating space manipulator that consists of a base, a nÀDOF revolutejointed manipulator, and a payload. The symbols and variables applied for modeling of the system in this section are listed in Table 1.
The payload is attached to the end-effector with rigid connection; thus, link 1 and payload can be considered a single composite rigid body. Mass and inertia tensor of the composite body can be expressed as where m c denotes the mass of the combined body; I L and n I L represent inertial tensors of payload in S L and S n , respectively; n R L is the rotation matrix of S L with respect to S n ; and E 3 is the third-order unit matrix.
Next, the dynamics equation of space manipulator can be formulated as where the inertia and Jacobian matrixes are calculated considering payload parameters by equation (1). Under the free-floating condition, the total momentum of the system is conserved, as expressed by The initial momentum is assumed as L 0 ¼ 0, and then, the motion relationship of the base and the manipulator is expressed as where J bm ¼ ÀH À1 b H bm is coupling Jacobian matrix. For the coupling motion of the manipulator and the base, trajectory planning of joints displays an association with the stability of the base1.
Substitute equation (4) to the following general kinematics equation of FFSM where J float ¼ J m À J b H À1 b H bm denotes the generalized Jacobian matrix. With respect to equations (4) and (5), motion of the payload and the base is decided by joint rotation. With appropriate planning of joint trajectories, the assembly of a heavy payload and stability of the base can be simultaneously achieved.

Force impulse analysis for self-assembly
At the contact point, the following collision assumptions are made to develop the force impulse function: 14 1. The interaction duration is significantly short, so the contact force acts instantaneously. 2. The pose changes of payload in the impact process are negligible, and the effects of other forces except for the contact force can be ignored. 3. Based on the mentioned assumptions, the inertia term of dynamic equation is dominant and other terms are disregarded. 4. Impulsive forces and moments are induced on an act-react principle at the single-point of contact.
Next, equation (2) can be reformulated as (6), and the definition is made; it can be therefore expressed as The matrixes in equation (7) are listed as follows When ignoring centrifugal force and Coriolis force, the inertia term of dynamics equation is dominant. Equation (7) can be simplified as Define F T as the contact force, force impulse is formulated as At the contact point, the contact force acts payload and base are assumed as F T and ÀF T , respectively. At the impact time t c , it can be equivalently considered that F L ðt c Þ¼F T and F b ðt c Þ¼ À F T . Then, we have The integral formula above is taken in a very short time dt, and d _ q m ¼ _ q m ðt 0 þ dtÞ À _ q m ðt 0 Þ is substituted into the integral. Then, it yields Pose of the base in inertial frame x L Pose of payload in inertial frame q m ¼ ½q 1 ;q 2 ; Á Á Á ;q n T , Joint angle vector Nonlinear terms of manipulator FFSM: free-floating space manipulator.

So that
where J c ¼ ðJ T float À J T bm Þ. The integral of equation (5) in dt is taken as 6 and h denote collision direction and force impulse, respectively). It yields According to equation (4), the motion of the base is calculated by Substituting equation (14) into the integral of equation (16), it easily yields The collision consists of the stages of compression and rebound. At the end of the compression stage, the relative velocity of contact surface between the base and the payload is zero, which is expressed as Substituting equations (15) to (17) into equation (18), the force impulse in the self-assembly process is defined as Since the relative motion of the base and the payload is attributed to the joint rotation, the given relative velocity of self-assemble task can be described as where q m ðt c Þ and _ q m ðt c Þ denote joint angles and velocities at the impact time, respectively. Take Poisson model in collision and set the restitution coefficient as e, the total impulse is considered as We can establish the force impulse function as where q m ðt c Þ, n, and D _ x are determined according to the task. However, due to the dynamics coupling effect, x b ðt c Þ relies on the history motion of the joints.

Multiobjective optimization problem for trajectory planning
The purpose of this study is to obtain appropriate joint trajectories, which accomplish self-assembly operation of a heavy payload and maintain stability of the whole system. Hence, trajectory planning problem exhibits the following characteristics: 1. While the payload pose reaches the contact point, the relative motion of payload and base should satisfy the assembly requirements. 2. Impulse force should be minimized to avoid damaging any part of the multibody system. 3. Due to load-carrying capacity constraint of space manipulator, disturbance of the base, driving torque constraints of the joints, and total energy of the system should be considered. 7

Multiobjective optimization problem statement of preimpact trajectory planning
Given the mentioned analysis, preimpact trajectory planning of FFSM can be solved as a constrained multiobjective optimization problem (MOP) as follows where l denotes the decision vector that expresses joint trajectories; y ¼ ½f 1 ; f 2 ; Á Á Á ; f M indicates the M-dimensional objective vector, each element of which represents an objective function; g and h are equality and inequality constrained conditions, respectively. The initial constraints of space manipulator are defined as where t 0 represents the initial time and q ini is initial joint angle vector.
The final constraints at the contact point are written as where x des and _ x des , respectively, represent the desired pose and velocity of the payload in base frame.
Joint angle constraints for t r 2 ½t 0 ; t f where ½q imin ; q imax denotes the limit of joint i.
Joint torque constraints are where t imin and t imax , respectively, denote the lower and upper torque limitations of joint i.

Definition of cost functions
To maintain stability of the space manipulator system, force impact, attitude disturbance of the base, and total energy involved in preimpact motion should be simultaneously minimized. Thus, the following three cost functions are established.

Force impulse
According to equation (22), force impulse is affected by the motion of space manipulator. To ensure the safety of the system under self-assembly collision, the cost function below is considered 2. Attitude disturbance of the base Attitude disturbance of the base should be limited as impacted by the requirements of communication and observation. 23 When carrying a heavy payload, the range of base movement could be extended; accordingly, more fuel consumption can be achieved for the maintenance of its attitude. The objective function to minimize base disturbance is given by where jj Á jj is the norm of a vector.

Energy of the whole system
For the considerable mass and inertia tensor of the payload, the total energy involved in the motion may significantly increase. It is noteworthy that given the confliction between multiple objectives in solving MOP, energy may increase as well when f 1 or f 2 is optimized. Thus, the energy minimizing cost function is defined as

Parameterization of joint trajectories
To obtain smooth and continuous trajectories of joint angles, velocities, and accelerations, sinusoidal polynomial function is adopted to parameterize joint trajectories in the present study.
The trajectories are defined as where l ij denotes the coefficient of the polynomial, With sinusoidal polynomial functions, smooth and continuous trajectories of joint motions and joint angle constraints expressed in equation (26) can be achieved. For a given self-assembly task, the values of q ini , x des , and _ x des are already determined, and the following conditions have to be met where _ q des can be easily obtained by velocity-level inverse kinematics in base frame In the base frame, any position-level inverse kinematics method is adopted to obtain q des suitable for self-assembly pose x des ; subsequently, q ini and q des in equations (32) and (33) are substituted into equation (31) where the symbols are calculated as follows Given l ¼ ½l 16 ; l 17 ; l 26 ; l 27 ; Á Á Á; l i6 ; l i7 2 R 2nÂ1 as the decision vector, joint trajectories are determined, and equation (26) is satisfied.
Preimpact trajectory planning of free-floating space manipulator using multiobjective particle swarm optimization Brief review of multiobjective particle swarm optimization Multiobjective particle swarm optimization (MOPSO) algorithm refers to an evolutionary technology based on swarm intelligence that simulates social behavior. Given the advantages of fast convergence, less computing resource occupancy, and excellent diversity, MOPSO algorithm is adopted to solve the trajectory optimization problem.
Pareto dominance is incorporated into a conventional single-objective PSO algorithm to process the problems with several cost functions in MOPSO. 28 This algorithm employs an external repository of particles as guidance to lead the evolution of the population toward the Pareto Front.
In accordance with the multiobjective optimization theory, when the following Pareto dominance principle is satisfied, l 0 dominates l 1 ðl 0 1 l 1 Þ The solution satisfying equation (36) is called a nondominated solution. Therefore, different from PSO, in the velocity formula of particle's flight, the best position that all the particles ever had (g best ) is replaced by a value taken from repository (R½h). Subsequently, the velocity formula is written as where P½k and V ½k, respectively, denote position and velocity of particle k; ¼ 2=ðj2 À À ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ð À 4Þ p jÞ represents the constriction factor; ¼ c 1 þ c 2 , while c 1 and c 2 refer to the learning factor that regulates the step size of the particles flying to p best (the best position a particle has ever explored) and g best ; r 1 and r 2 indicate independent random numbers in the range ½0; 1.

Trajectory planning algorithm
Equation (22) indicates that the final configuration of space manipulator is capable of affecting the force impulse. Moreover, joint trajectories can vary significantly with different final states of the manipulator, which affects base disturbance and energy consumption. For a redundant space manipulator, multiple configurations should be considered in the trajectory planning process when payload reaches the contact point. 29 Initialize the population ,generate position and velocity of each particle.

No
Compute particles' speed using Eq(37) and update their positions. Maintain the particles within the search space in case the go beyond their boundaries.
Store the positions of particles that represent non-dominated vectors (according to Eq.(36)) in the repository.
Evaluate particles and add currently non-dominated locations into the repository. Maintain the repository using adaptive grid method.
Update particles' memory when current position gets better.
Obtain the set of non-dominated solutions Maximum iteration number? For joint vector , calculate kinematics to obtain its corresponding pose matrix of the payload in base frame. q Calculate differential motion matrix using Eq.(39).
Obtain differential motion matrix .  In the present study, Newton-Raphson iterative method 30 is employed to compute q des , and MOPSO is adopted for optimizing calculation. To obtain the applicable motion of space manipulator without violating the constraints, the preimpact trajectory planning algorithm is proposed as the following two stages.
Stage I: Obtain a set of desired configurations with Newton-Raphson iteration.
T r is defined as the corresponding pose matrix of the payload in S B for any given joint vector q r . Differential motion from T r to desired pose T des is written as where D L is differential motion matrix, as expressed below where dx, dy, and dz denote the differential translation operators; dx, dy, and dz represent the differential rotation operators. Subsequently, the differential motion vector D¼ ½ dx dy dz dx dy dz is obtained. According to equation (33), joint differential motion vector is calculated as Figure 3 shows the Newton-Raphson iteration. Execute iterative computation until the maximum iteration N is reached, corresponding desire preimpact configuration for the given joint vector can be obtained if jjdqjj x, where x is a given sufficiently small number.
A set of random joint vectors q r f g is generated, the corresponding set of configurations can be yielded by complying with Newton-Raphson iteration. The elements that do not satisfy equation (26) are eliminated, and then, n p types of configurations with the smallest value of jjq des À q ini jj are selected. Lastly, q des f g 2 R iÂn p consisting of the selected configurations is determined.
Stage II: Solve nondominated solutions for trajectory planning problem using MOPSO.
Step 1 Initialize the population: (a) Generate a population containing n p particles and restrict the search range as ½l min ; l max in all directions of the 2n-dimensional decision space. Then, assign each particle a joint vector in q des f g.  Link (b) A random decision vector l is generated in the search range and then substituted to equation (31) to calculate q, _ q , and € q . Execute kinematics and dynamics calculation (where spatial operator algebra method is applied to reduce computational complexity) determine the values of joint torques.
(c) If equation (27) is satisfied, l is stored as the initial position of the particle; otherwise, loop to (b) until a desired decision vector is determined. (d) If the number of stored decision vectors reaches n p , define initial velocities of all the particles as zero.
Step 2 Initialize external repository: (a) The maximum allowable number of the repository is limited to n R .   (b) Substitute kinematics and dynamics calculation results of the respective particle into equations (28) to (30) to determine the objective vector of particle k: y 0 k ¼ ½f 0 1 ðkÞ; f 0 2 ðkÞ; f 0 3 ðkÞ. (c) Store nondominated decision vector in R 0 according to equation (36). (d) Initialize the best position particle k has experienced as p 0 best ½k ¼ P 0 ½k.
Step 3 Execute evolutionary computations: (a) Calculate particles' velocity using equation (37) and update the position of the respective particle. According to dynamics calculation results, only new solutions that satisfy joint torque constraints are reserved in each iteration. (b) Add these reserved solutions into the repository via a similar solution procedure in (b) and (c) of step 2, nondominated solutions can be acquired to guide the evolutionary process of the population. (c) The set of nondominated solutions is finally yielded, while the maximum number of iteration is reached. An appropriate solution can be taken based on the meanings of the particle's position in objective space, according to the task priority or practical engineering requirements.

Seven degrees of freedom space manipulator
As shown in Figure 4, a 7DOF redundant space manipulator is studied in the present study. D-H coordinate system is established, and the relevant parameters are listed in Table 2. Table 3 lists the dynamics parameters, in which only the elements of diagonal matrixes are given:

Results and discussions
The pose of the base at initial time is zero in inertia frame; the initial joint angles are set to q ini ¼ ½70 ; À 100 ; À100 ; À 40 ; 20 ; À 90 ; 20 ; the desired pose of assembly mechanism when payload reaches the contact point is x des ¼ ½0 m; À6:5 m; À2 m; 0 ; 0 ; À180 in base frame; the desired self-assembly velocity in base frame is _ x des ¼ ½0; 0:1 m=s; 0; 0; 0; 0; and the total time for The bold values are used to highlight the positions of the maximum and minimum values. preimpact motion of space manipulator is t f ¼ 200s. The upper and lower limits of joint iði ¼ 7; 6; Á Á Á ; 1Þ are q imin ¼ À270 and q imax ¼ 270 , and the limits of joint torques reach t imin ¼ À500N Á m and t imax ¼ 500N Á m. At stage I of the simulation, the relevant parameters in Newton-Raphson iteration are set to x ¼ 0:0001 and N ¼ 10; 000. We generate 5500 uniformly distributed random configurations and corresponding desired joint vectors are solved using iteration equation. q des f g 2 R 7Â200 is selected according to the value of jjq des À q ini jj. For stage II, the specific parameters of MOPSO are set: the range of values for 14 decision variables refers to ½l min ; l max ¼ ½À10 Â 10 À16 ; þ 10 Â 10 À16 ; n P ¼ 200; n R ¼ 1000; and c 1 ¼ c 2 ¼ 2:05. The yielded 200 joint vectors in stage I are assigned for the particles. After 200 iterations of evolutionary computation, Figure 5 shows the distribution of all nondominated solutions in objective space. It is seen that all the solutions distribute on a surface, which can be approximately regarded as the Pareto Front. Execute dynamics calculation for each nondominated solution, and the maximum absolute value of joint torques does not exceed 499.6272 N m, revealing that all the solutions satisfy equation (27).
The optimal values of three cost functions in each generation are shown in Figures 6 to 8, demonstrating high convergence effects. The number of particles in external repository is elevated, as shown in Figure 9. n R is reached at 115th iteration and the subsequent fluctuation of the curve indicates the maintenance of repository. The extreme values of cost functions are listed in Table 4,  , thereby demonstrating the necessity of preimpact trajectory planning. As revealed from the listed objective vectors, the six extreme solutions do not dominate each other, and the relationships between three cost functions are conflicting. The mentioned results indicate that while minimizing one of the cost functions, the values of other cost functions can be significantly elevated, which are even unacceptable. Thus, by no means, an optimal solution can be found with respect to all of the three objectives. In the present study, the priority of the objectives is assessed as an example for picking out a proper solution. We assume that the avoidance of system damage and base instability should take precedence over reducing of energy consumption. When considering f 1 and f 2 having higher priority than f 3 , mapping the three-dimensional Pareto Front to objective space consists of f 1 and f 2 , and dominated solutions are eliminated according to equation (36). The obtained two-dimensional Pareto Front is shown in Figure 10. Nondominated solutions A and B are selected to illustrate the effectiveness of the proposed algorithm. Figure 11 depicts the joint trajectories, and the manipulator moves in the limits of joint angles. The desired joint angles listed in Table 5 are reached for each solution, demonstrating that the self-assembly pose of payload is satisfied at the impact time. Figure 12 shows the velocity of the payload in base frame for both solutions, and the desired assembly velocity _ x des ¼ ½0; 0:1 m=s; 0; 0; 0; 0 is satisfied. All the requirements are met. Figure 13 shows the variations of base attitude (represented by Z-Y-X Euler angles). Given the kinematics calculation, the final pose of the base in inertia frame is . By contrast, l A is considered a proper solution for the given trajectory planning problem. As shown in Figures 11 and 13, for the appropriate planning of joint trajectories, base attitude changes are smooth and continuous. Since optimization of base attitude disturbance is considered in the trajectory planning, final attitude values of the base are acceptable. Conclusively, the obtained nondominated solution is applicable.

Conclusion
Given the significance of on-orbit assembling a heavy payload to free-floating base with a space manipulator, the present study proposes a scheme of multiobjective preimpact trajectory planning. The force impulse in the self-assembly process is formulated as a function of joint angles, joint velocities, base pose, and collision direction. As revealed from such function, force impulse is determined by the current state of the system at the contact point as well as the history motion of the space manipulator. The joint trajectories are built by coefficients of sinusoidal polynomial functions. Inverse kinematics is adopted to transmit constraints, so pose and velocity requirements for self-assembly are satisfied. Based on redundancy of space manipulator, multiple configurations are obtained as the input conditions for trajectory planning. The trajectory planning algorithm based on MOPSO minimizes force impulse, base disturbance, and energy cost simultaneously. Only solutions that satisfy joint torque constraints are reserved for both initialization and evolution. Each of the yielded nondominated solutions is of explicit physical significance in objective space, which provides convenience to pick out an appropriate solution.
The proposed trajectory planning method is capable of generating multiple, feasible, and effective preimpact motion schemes of space manipulator for selection. Accordingly, the on-orbit self-assembly of heavy payload and the spacecraft base is endowed with promising engineering application prospect. However, only preimpact process is considered in this study. Disturbance of the base and impact of the joint may cause discontinuous and instable motion of space manipulator system after assembly collision, which is to be studied in our future works.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding
The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported by the National Nature Science Foundation