The Precollision Trajectory Planning of Redundant Space Manipulator for Capture Task

Abstract

Aiming at on-orbit capture task, a scheme for precollision trajectory planning of redundant space manipulator is proposed in this paper. The minimized collision impulsive force and the correct capture pose of the end-effector can be achieved at the same time by the scheme. At the contact point, the configuration of the manipulator is optimized in a new way to minimize the impulsive force without affecting the correct capture pose. After obtaining the optimal configuration, particle swarm optimization (PSO) algorithm is employed to solve the trajectory planning from the initial point to the desired point (desired configuration and correct capture pose are both contained). Firstly, the joint trajectory is parameterized using sinusoidal functions with the arguments of high-order polynomials. Then the objective function is defined according to the end-effector pose requirements. Finally, the PSO method is used to search for the unknown parameters to obtain the appropriate precollision joint trajectory. The proposed method can satisfy the whole capture process of redundant space manipulator before collision, and simulation results of a 7-dof space manipulator verify the effectiveness.

1. Introduction

The on-orbit capture operation of free-floating objects by a space robot has become an important mission in recent years. It contains the following phases: target chasing control phase, impact phase between the target and robotic hand, and relieve and suppress control phase of tumbling motion. During different phases, it has different requirements. In the first phase, the trajectory tracing or optimization sometimes is needed. In the second phase, the precision of the robot hand pose at the contact point needs to be guaranteed. And due to control and sensing errors, there remain certain amounts of relative velocities between the robot hand and the contact point of the target. Thus at the contact moment, both suffer impulsive force at the contact point, which may damage the target or the manipulator. Therefore, impulsive force minimization is needed to ensure the successful completion of the task. In the third phase, the most important requirement is the stable control of the whole system.

So far, some studies on contact and impact of manipulators have been carried out for the requirements in the first and the second phase. Asada [1] evaluated the amount of resulting impact impulse using the inverse inertia matrix of the robot and the target for the ground-based robots. Yoshikawa and Yamada [2] considered the effects of the servo stiffness and the desired joint rates for the manipulator control and used the Laplace transformation to estimate the contact impulse. Zheng and Hemami [3] discussed the collision effects on the motions of two coordinating terrestrial robots and a general impact dynamics formulation is built. This formulation is adopted in [4] for a two-dimensional planar robot and it has been shown that the impulsive force can be minimized by controlling the configuration of the robot manipulator. The main drawback of [3, 4] is that only the relative translational velocities between two contacting bodies are considered. Yoshida et al. [5] introduced the concept of “extended inertial tensor” and modeled the contact dynamics formulation. On this basis, the target and the robot hand velocities before and after contact can be calculated without measuring the contact impulsive force. In order to figure out the condition minimizing the impulse, the relationship of the impulse with collision directions and link postures is discussed by means of the Impulse Index and Impulse Ellipsoid in [6, 7]. Wee and Walker [8, 9] presented a new model of space robots which takes into account external applied forces, and then a strategy for achieving both the primary objective of trajectory planning in Cartesian space and the secondary objective of impact minimization through configuration space planning is presented. Huang et al. [10] used the momentum and the angular momentum theory to deduce the relationship between the velocity variations of the robot base and the target. And he proposed a genetic algorithm (GA) based method to search the optimal configuration or approach trajectory to minimize the impact [11, 12]. Pei-Chao and Zhao-wei [13] introduced “straight arm capture” concept and a method of configuration planning based on momentum conservation is proposed. The configuration can minimize the disturbance of the robot base, but the derivation will be very complex for the six-dimensional space problem.

In this paper, in order to minimize the impulsive force caused by collision and obtain the correct capture pose required by the specific task at the same time, a scheme for precollision trajectory planning of redundant space manipulator is proposed for on-orbit capture task. At the contact point, the configuration of the manipulator is optimized in its null space to minimize the impulsive force. On this basis, PSO algorithm is employed to solve the trajectory planning from the initial point to the desired point to obtain the correct capture pose. Finally, the proposed method is applied to a 7-dof free-floating space manipulator, and the simulation results verify the effectiveness.

2. Contact Dynamics Model of Free-Floating Space Manipulator

2.1. Collision Assumption and Symbols

The following assumptions are made in modeling contact dynamics at the moment of contact.

1. The duration of contact is so short that the interaction forces act instantaneously.

2. The changes in position and orientation during impact are negligible and effects of other forces except the impact force can be disregarded.

3. From the above assumptions, the inertia term of dynamic equation is dominant and other terms are less important.

4. Impulsive forces as well as moments are induced on an act-react principle at the single-point of contact.

As for a free-floating space manipulator which is composed of n + 1 parts, adjacent parts are connected with revolute joints as shown in Figure 1. Define the symbols (all expressed in inertial frame) as follows.

• n: Joint number;

• m_k: Mass of link k, mass of the base if k = 0;

• M: Total mass of the system;

• x_b: Pose of space manipulator base;

• x_e: Pose of space manipulator end-effector;

• q: Joint angle, $q=\{q_{\mathrm{1}},q_{\mathrm{2}},\dots q_n\}$

• F _b : External applied force/torque on base;

• F _e : External applied force/torque on end-effector;

• τ _m : Joint torque, τ _m = {τ₁,τ₂,…τ _n };

• J _b : Jacobian matrix of the base;

• J _m : Jacobian matrix of the manipulator;

• c _b ,c _m : Velocity dependent nonlinear term for the base and the manipulator, respectively;

• I _k : Inertial tensor of link k with respect to its mass center;

• r_0g: Vector from the base center to total mass center of the system;

• r_0k: Vector from the base center to the mass center of link k;

• $r_{\mathrm{co}}^{}$ : Vector from the target mass center to the contact point;

• E: Identity matrix;

• If r = [a,b,c], then $r^{\times }=\left[\begin{array}{ccc}\mathrm{0}& -c& b\\ c& \mathrm{0}& -a\\ -b& a& \mathrm{0}\end{array}\right]$ .

Figure 1:

A general model of free-floating space manipulator.

2.2. The Modeling of Contact Dynamics

We know that the general formulation of free-floating space manipulator dynamics is

$H\left[\begin{array}{c}{\ddot{x}}_b\\ \ddot{q}\end{array}\right]+\left[\begin{array}{c}{c}_b\\ c_m\end{array}\right]=\left[\begin{array}{c}{F}_b\\ {\tau }_m\end{array}\right]+\left[\begin{array}{c}{J}_b^T\\ J_m^T\end{array}\right]F_e,$

(1)

where $H=\left[\begin{array}{cc}{H}_{\mathrm{1}}& H_{\mathrm{2}}\\ H_{\mathrm{2}}^T& H_q\end{array}\right]$ the specific meaning of the variables can be seen in [14]. For free- floating manipulator, its propulsion system is shut down, so the external applied force/torque on base is zero, namely, F _b = [0,0,0,0,0,0] ^T .

Separate (1), we can obtain the following equation:

$\begin{array}{c}{H}_{\mathrm{1}}{\ddot{x}}_b+H_{\mathrm{2}}\ddot{q}+c_b=J_b^T{F}_e,\\ H_{\mathrm{2}}^T{\ddot{x}}_b+H_q\ddot{q}+c_m={\tau }_m+J_m^T{F}_e.\end{array}$

(2)

Eliminate the base acceleration term ${\ddot{x}}_b$ :

$H^{\ast }\ddot{q}+c^{\ast }\left(q,\dot{q}\right)={\tau }_m+{J^{\ast }}^T{F}_e,$

(3)

where H^* = (H _q -H₂ ^T H₁⁻¹H₂) is the generalized inertial tensor, $c^{*}(q,\dot{q})=(c_m-H_{\mathrm{2}}^T{H}_{\mathrm{1}}^{-\mathrm{1}}{c}_b)$ is the velocity-dependent term, and $J^{*}=(J_m-J_b{H}_{\mathrm{1}}^{-\mathrm{1}}{H}_{\mathrm{2}})$ is called the generalized Jacobian matrix [14].

According to the collision assumptions above, the Coriolis force, centrifugal force, and the velocity-dependent term can be neglected at the contact time. The integral during infinitesimal time interval δt is

$H^{\ast }\delta \dot{q}={J^{\ast }}^T\mathrm{\Phi },$

(4)

where $\mathrm{\Phi }={\int }_{t_{\mathrm{0}}}^{t_{\mathrm{0}}+dt}{F}_{e}dt$ is the impulsive force applied on the end-effector and $\delta \dot{q}={\dot{q}}_{t_0+\delta t}-{\dot{q}}_{t_0}$ is the instantaneously change in joint angle velocity, and it can be calculated as

$\delta \dot{q}={H^{\ast }}^{-\mathrm{1}}{J^{\ast }}^T\mathrm{\Phi }.$

(5)

As we know from the kinematics relationship (the initial momentum and angular momentum are assumed to be zero)

${\dot{x}}_e=J^{\ast }\dot{q}.$

(6)

Hence, the instantaneously change in spatial velocity of the end-effector is

$\delta {\dot{x}}_e=J^{\ast }\delta \dot{q}=J^{\ast }{H^{\ast }}^{-\mathrm{1}}{J^{\ast }}^T\mathrm{\Phi }.$

(7)

For a single-point contact, the impulsive force Φ is equal to the product of a scalar magnitude γ and its direction vector N, namely, Φ = Nγ. Substitute it into (7):

$\delta {\dot{x}}_e=DN\gamma ,$

(8)

where D = J^*H^*−1J^*T is called the Jacobian inertia.

A whole collision can be divided into two phases: the compression phase and the restitution phase. At the end of the compression phase, the velocity projection of end-effector is equal to that of contact point on the surface of the target along the collision direction N. We define v_e1 and v_e2 as the velocities of robot end-effector and the contact point of target just before contact, respectively; δv_e1 and δv_e2 are the corresponding changes. Thus, we can get the following equation:

$N^T\left(v_{e\mathrm{2}}+\delta v_{e\mathrm{2}}-v_{e\mathrm{1}}-\delta v_{e\mathrm{1}}\right)=\mathrm{0}.$

(9)

Let -Nγ ^c represent the impulsive force applied on the robot end-effector during the compression phase, and the impulsive force applied on the target is Nγ ^c obviously. Then using the results of (8)

$\delta v_{e\mathrm{1}}=-D_{\mathrm{1}}N{\gamma }^c,\hspace{1em}\hspace{1em}\delta v_{e\mathrm{2}}=D_{\mathrm{2}}N{\gamma }^c,$

(10)

where D1 is the Jacobian inertia of manipulator and $D_{\mathrm{2}}=\left[\begin{array}{cc}E& -r_{\mathrm{co}}^{\times }\end{array}\right]{\left[\begin{array}{cc}{m}_{o}E& \mathrm{0}\\ \mathrm{0}& I_{\mathrm{2}}\end{array}\right]}^{-\mathrm{1}}{\left[\begin{array}{cc}E& -r_{\mathrm{co}}^{\times }\end{array}\right]}^T$ when the target is a single rigid body.

Substitute (10) into (9), the impulsive force of the compression phase can be obtained:

${\gamma }^c=-\frac{N^T\left(v_{e\mathrm{2}}-v_{e\mathrm{1}}\right)}{N^T\left(D_{\mathrm{2}}+D_{\mathrm{1}}\right)N}.$

(11)

Note that γ ^c >0 since it is necessary that N ^T (v_e2-v_e1)<0, for the collision objects have to contact first. Poisson model is employed during the collision; the impulsive force of the restitute phase is γ ^r = eγ ^c ; 0 ≤ e ≤ 1 is the coefficient of restitute. Therefore, the impulsive force of the whole collision phase is

$\gamma =-\left(\mathrm{1}+e\right)\frac{N^T\left(v_{e\mathrm{2}}-v_{e\mathrm{1}}\right)}{N^T\left(D_{\mathrm{2}}+D_{\mathrm{1}}\right)N}.$

(12)

3. The Optimization of Manipulator Configuration at the Contact Point

Non-minimum-norm solutions to (6) based on Jacobian pseudoinverse can be written in the general form [15]:

$\dot{q}={J^{\ast }}^{†}{\dot{x}}_e+\left(E-{J^{\ast }}^{†}{J}^{\ast }\right)k\phi ,$

(13)

where J^*† = J^*T(J^*J^*T)⁻¹ is the pseudoinverse of Jacobian matrix J^*, (E-J^*†J^*) is called the null space, k is the gain coefficient, which can be a constant or a function, and φ is an arbitrary vector. By observing that null-space velocities produce a change in the configuration of the manipulator without affecting its velocity at the end-effector, it can be exploited to achieve additional goals; here, we mean the impulsive minimization.

From (12), we can see that the impulsive force is affected by the collision direction N, relative velocity v_e12 = v_e1-v_e2, and the robot configuration D1. When N and v_e12 are determined, the impulsive force can be minimized by optimizing its configuration. Assume g = N ^T (D₂ + D₁)N, the impulsive force is minimum when g is maximum. Set $\phi =\nabla g=\{\partial g/\partial q_{\mathrm{1}},\partial g/\partial q_{\mathrm{2}},\dots ,\partial g/\partial q_n\}$ in (13), and if we want the value of g as large as possible, k needs to be positive.

In order to make the end-effector keep the correct capture pose when adjusting its configuration for minimizing the contact impulsive force, set ${\dot{x}}_e=\mathrm{0}$ in (13), so we can get

$\dot{q}=\left(E-{J^{\ast }}^{†}{J}^{\ast }\right)k\phi .$

(14)

To make the initial joint angular velocities zero and run stably, we design the transition function k as follows:

$k=\{\begin{array}{cc}0,\hfill & \mathrm{0}\le t\le T_R,\hfill \\ \sin (\tau -\frac{\pi }{\mathrm{2}})+\mathrm{1},\hfill & T_R<t\le T_m,\hfill \\ 2,\hfill & T_m<t\le T_Z,\hfill \end{array}$

(15)

where $\tau =-(\pi (t-T_R))/(T_m-T_R)$ T _R ,T _m are the start and the end time of the transition curves as shown in Figure 2.

Figure 2:

Curve of transition function k.

The specified steps for optimization are as follows.

1. Set the initial joint angles q0 and get the corresponding value of g, called g0; the initial joint angle velocities are set to be zero.

2. Optimize the joint angle velocities for maximizing g by (14); update the joint angles and the g value by $q_i=q_{i-\mathrm{1}}+{\dot{q}}_{i-\mathrm{1}}\delta t$ and g _i = N ^T (D₂ + D₁)N, separately.

3. Compare the current gvalue g_i with the previous one; if g _i >g_i–1, the optimization goes on, namely, loop to step 2.

4. Obtain the best configuration q_d when g achieves its maximum.

The manipulator configuration will be optimized until the value of g achieves its maximum and remains unchanged. Through the method above, we can obtain the best configuration, which can satisfy the impulse force minimization (g maximum) and the capture pose of the end-effector correction.

4. The Precollision Trajectory Planning Based on PSO

We have obtained the best configuration at the contact point in Section 3; the following problem is how to plan the trajectory from the initial configuration to the desired configuration, and the correct capture pose of the end-effector needs to be achieved at the same time.

For a free-floating space manipulator, it exhibits nonholonomic characteristic due to the dynamic coupling between space manipulator and its base. Therefore, the planning and control of space manipulators have some additional problems beyond those on earth. We decide to employ the PSO algorithm to complete the precollision trajectory planning.

4.1. A Brief Review of PSO

Particle swarm optimization (PSO), a stochastic optimization method based on the simulation of the social behavior of bird flocks, was originally developed by Kennedy and Eberhart [16, 17]. It is similar to a genetic algorithm in that the system is initialized with a population of random solutions. It is unlike a genetic algorithm, however, in that each potential solution is also assigned a randomized velocity, and the potential solutions, called particles, are then “flown” through hyperspace. Each particle keeps track of its coordinates in hyperspace which are associated with the best solution it has achieved so far. The value is called pbest. Another “best” value, which is also tracked, is the overall best value, and its location, obtained so far by any particle in the population. This location is called gbest.

The particle flies in the search space at a certain speed and direction. According to its own flying experience and its companion's flying experience, it will change the velocity toward its pbest and gbest locations at each time step. Acceleration is weighted by a random term, with separate random numbers being generated for acceleration toward pbest and gbest. In a d-dimension space, the velocity and location of particle i are updated by the following equations:

$\begin{array}{c}{\upsilon }_i^d\left(k+\mathrm{1}\right)=w{\upsilon }_i^d\left(k\right)+c_{\mathrm{1}}\mathit{r}\mathit{a}\mathit{n}\mathit{d}\left(\right)\left(p_i^d-x_i^d\left(k\right)\right)+c_{\mathrm{2}}\mathit{r}\mathit{a}\mathit{n}\mathit{d}\left(\right)\left(p_g^d-x_i^d\left(k\right)\right),\\ x_i^d\left(k+\mathrm{1}\right)=x_i^d\left(k\right)+{\upsilon }_i^d\left(k\right),\end{array}$

(16)

where the acceleration constants c1 and c2 represent the weighting of the stochastic acceleration terms that pull each particle toward pbest and gbest positions. Low values allow particles to roam far from target regions before being tugged back, while high values result in abrupt movement toward, or past, target regions. c1 and c2 each equal to 2.0 is suitable for almost all applications. rand () is a random number uniformly distributed in the range [0, 1]. The inertia weight w plays the role of balancing the global and local search. It can be a positive constant or a positive linear or nonlinear function of time. p _i ^d is the best previous position of the ith particle and p _g ^d is the swarm's best position.

4.2. The Solution Based on PSO

One of the reasons that PSO is attractive is that there are very few parameters to adjust. It has been used for space robot path planning in [18–20]. The process for implementing the PSO is as follows.

1. Initialize a population (array) of particles with random positions and velocities on d-dimension in the problem space.

2. For each particle, evaluate the desired optimization fitness function in d variables.

3. Compare particle's fitness evaluation with particle's pbest. If current value is better than pbest, then set pbest value equal to the current value and the pbest location equal to the current location in d-dimension space.

4. Compare fitness evaluation with the population's overall previous best. If current value is better than gbest, then reset gbest to the current particle's array index and value.

5. Change the velocity and position of the particle according to (16).

6. Loop to step 2 until a criterion is met, usually a sufficient good fitness or a maximum number of iterations (generations).

The flowchart of PSO is simply shown in Figure 3.

Figure 3:

The flowchart of PSO.

Before optimization, the particles and objective function need to be determined. The joint variables are parameterized to be the particles, and the optimized results can be directly used to adjust the manipulator configuration. When the manipulator moves to the contact point, as mentioned above, it needs to be at the best configuration, and its end-effector needs to have the correct capture pose, so the objective function should contain all the information.

4.3. The Particles of PSO

As said above, the joint variables are parameterized to be the particles. First, the initial and final joint angles, angular velocities, and angular accelerations are expected to satisfy the following requirements:

$\begin{array}{c}\theta \left(t_{\mathrm{0}}\right)={\mathrm{\Theta }}_{\mathrm{0}},\hspace{1em}\hspace{1em}\theta \left(t_f\right)={\mathrm{\Theta }}_d,\hspace{1em}\hspace{1em}\dot{\theta }\left(t_{\mathrm{0}}\right)=\mathrm{0},\\ \ddot{\theta }\left(t_{\mathrm{0}}\right)=\mathrm{0},\hspace{1em}\hspace{1em}\dot{\theta }\left(t_f\right)=\mathrm{0},\hspace{1em}\hspace{1em}\ddot{\theta }\left(t_f\right)=\mathrm{0},\end{array}$

(17)

where t₀,t _f are the initial and the final time and Θ₀,Θ _d are the initial and the desired final angles, respectively. Furthermore, the path planning solution has to satisfy the mechanical constraints of manipulator

${\theta }_{i\min }\le {\theta }_i\left(t\right)\le {\theta }_{i\max },$

(18)

where θ _imax , θ _imin are the lower and upper permitted angles of the ith joint. The polynomial functions are usually used to obtain smooth joint motion, and considering the joint angle limits, we parameterized the joint trajectory by a sinusoidal function, whose argument is a seventh-order polynomial

${\theta }_i\left(t\right)={\mathrm{\Delta }}_{i\mathrm{1}}\sin (a_{i\mathrm{7}}{t}^{\mathrm{7}}+a_{i\mathrm{6}}{t}^{\mathrm{6}}+a_{i\mathrm{5}}{t}^{\mathrm{5}}+a_{i\mathrm{4}}{t}^{\mathrm{4}}+a_{i\mathrm{3}}{t}^{\mathrm{3}}+a_{i\mathrm{2}}{t}^{\mathrm{2}}+a_{i\mathrm{1}}t+a_{i\mathrm{0}})+{\mathrm{\Delta }}_{i\mathrm{2}},$

(19)

where a_i7,a_i6,…, a_i0 are the coefficients of the polynomial, i = 1,2…, n is the ith joint, ${\mathrm{\Delta }}_{i\mathrm{1}}=({\theta }_{i\max }-{\theta }_{i\min })/2$ , ${\mathrm{\Delta }}_{i\mathrm{2}}=({\theta }_{i\max }+{\theta }_{i\min })/2$ .

The joint velocity and joint acceleration can be obtained by the derivate and second-derivate of (19). Considering (17), the following results are found:

$\begin{array}{l}{a}_{i\mathrm{0}}=\arcsin \left(\frac{{\theta }_{i\mathrm{0}}-{\mathrm{\Delta }}_{i\mathrm{2}}}{{\mathrm{\Delta }}_{i\mathrm{1}}}\right),\\ a_{i\mathrm{1}}=a_{i\mathrm{2}}=\mathrm{0},\\ a_{i\mathrm{3}}=-(\phantom{\left[\arcsin \left(\frac{{\theta }_{id}-{\mathrm{\Delta }}_{i\mathrm{2}}}{{\mathrm{\Delta }}_{i\mathrm{1}}}\right)-\arcsin \left(\frac{{\theta }_{i\mathrm{0}}-{\mathrm{\Delta }}_{i\mathrm{2}}}{{\mathrm{\Delta }}_{i\mathrm{1}}}\right)\right]}\mathrm{3}{a}_{i\mathrm{7}}{t}_f^{\mathrm{7}}+a_{i\mathrm{6}}{t}_f^{\mathrm{6}}-\mathrm{10}\times \left[\arcsin \left(\frac{{\theta }_{id}-{\mathrm{\Delta }}_{i\mathrm{2}}}{{\mathrm{\Delta }}_{i\mathrm{1}}}\right)-\arcsin \left(\frac{{\theta }_{i\mathrm{0}}-{\mathrm{\Delta }}_{i\mathrm{2}}}{{\mathrm{\Delta }}_{i\mathrm{1}}}\right)\right])\times {\left(t_f^{\mathrm{3}}\right)}^{-1},\\ a_{i\mathrm{4}}=(\mathrm{8}{a}_{i\mathrm{7}}{t}_f^{\mathrm{7}}+\mathrm{3}{a}_{i\mathrm{6}}{t}_f^{\mathrm{6}}-\mathrm{15}\times \left[\arcsin \left(\frac{{\theta }_{id}-{\mathrm{\Delta }}_{i\mathrm{2}}}{{\mathrm{\Delta }}_{i\mathrm{1}}}\right)-\arcsin \left(\frac{{\theta }_{i\mathrm{0}}-{\mathrm{\Delta }}_{i\mathrm{2}}}{{\mathrm{\Delta }}_{i\mathrm{1}}}\right)\right]){\times \left(t_f^{\mathrm{4}}\right)}^{-1},\\ a_{i\mathrm{5}}=-(\mathrm{6}{a}_{i\mathrm{7}}{t}_f^{\mathrm{7}}+\mathrm{3}{a}_{i\mathrm{6}}{t}_f^{\mathrm{6}}-\mathrm{6}\times \left[\arcsin \left(\frac{{\theta }_{id}-{\mathrm{\Delta }}_{i\mathrm{2}}}{{\mathrm{\Delta }}_{i\mathrm{1}}}\right)-\arcsin \left(\frac{{\theta }_{i\mathrm{0}}-{\mathrm{\Delta }}_{i\mathrm{2}}}{{\mathrm{\Delta }}_{i\mathrm{1}}}\right)\right]){\times \left(t_f^{\mathrm{5}}\right)}^{-1},\end{array}$

(20)

where θ_i0, θ _id are the initial and the desired angle of the ith joint. After parameterization, only two parameters (a_i6,a_i7) are included in each joint function. Thereby, let a = [a₁₆,a₁₇,a₂₆,a₂₇,…, a_n6,a_n7] be the particles of PSO.

4.4. The Objective Function of PSO

The best configuration of space manipulator has been achieved in (17); the correct capture pose can be realized through the following method.

The position of the end-effector is determined by velocity integral method, namely, $P_e(t)={\int }_{\mathrm{0}}^{t_f}{J}_{ve}\dot{\theta }dt$ , where J _ve is the Jacobian matrix of linear velocity.

The attitude of the end-effector is represented by quaternion. The unit quaternion is a popular nonsingular four-parameter representation [21], which has several advantages over the conventional usage of direction cosines and Euler angles. First, a mechanical system that involves rotation between various coordinate systems does not degenerate for any orientation. Second, the computational cost of using the quaternion is less.

A quaternion Q is defined as

$\mathit{Q}=\eta +q_{\mathrm{1}}\overrightarrow{\mathit{i}}+q_{\mathrm{2}}\overrightarrow{\mathit{j}}+q_{\mathrm{3}}\overrightarrow{\mathit{k}}.$

(21)

And it is constrained by η² + q₁² + q₂² + q₃² = 1.

According to [22, 23] and combining the kinematics knowledge, the variation rate of Q is given by

$\dot{\mathit{Q}}=\frac{\mathrm{1}}{\mathrm{2}}\left[\begin{array}{c}-{\mathbf{q}}^T\\ \eta E-{\mathbf{q}}^{\times }\end{array}\right]J_{\omega e}\dot{\theta },$

(22)

where J_ωe is the Jacobian matrix of angular velocity.

For two coordinate systems, their attitudes being represented by [η₁,q₁] and [η₂,q₂], the relative attitude is given by δη and δq:

$\begin{array}{c}\delta \eta ={\eta }_{\mathrm{1}}{\eta }_{\mathrm{2}}+{\mathbf{q}}_{\mathrm{1}}^T{\mathbf{q}}_{\mathrm{2}},\\ \delta \mathbf{q}={\eta }_{\mathrm{1}}{\mathbf{q}}_{\mathrm{2}}-{\eta }_{\mathrm{2}}{\mathbf{q}}_{\mathrm{1}}-{\mathbf{q}}_{\mathrm{1}}^{\times }{\mathbf{q}}_{\mathrm{2}}.\end{array}$

(23)

When the two frames coincide

$\begin{array}{c}\delta \eta =\mathrm{1},\\ \delta \mathbf{q}=\mathrm{0}.\end{array}$

(24)

It's worth noting that δq = 0 implies δη = 1.

Suppose $\{{\eta }_{ef},{\mathbf{q}}_{ef}\},\{{\eta }_{ed},{\mathbf{q}}_{ed}\}$ are the actual and desired attitude of the end-effector and P _ef ,P _ed are the actual and desired position of the end-effector. And the pose deviations are

$\begin{array}{c}\delta {\mathbf{q}}_e={\eta }_{ef}{\mathbf{q}}_{ed}-{\eta }_{ed}{\mathbf{q}}_{ef}-{\mathbf{q}}_{ef}^{\times }{\mathbf{q}}_{ed},\\ \delta P_e=P_{ed}-P_{ef}=P_{ed}-{\int }_{\mathrm{0}}^{t_f}{J}_{ve}\dot{\theta }dt.\end{array}$

(25)

Thus, the objective function of PSO can be designed as

$f=\mathit{W}\mathit{\alpha },$

(26)

where $\mathit{\alpha }={[\parallel \delta {\mathbf{q}}_e\parallel \parallel \delta P_e\parallel ]}^T$ , $\parallel \mathit{x}\parallel ={\mathit{x}}^T\mathit{x}$ , $\mathbf{W}=\left[\begin{array}{cc}{w}_{\mathrm{1}}& w_{\mathrm{2}}\end{array}\right]$ is the weight matrix, which can balance the convergence result by setting its value.

5. Simulation Study

5.1. The Studied Robot System

The studied space manipulator is composed of the base and a 7-dof manipulator. Set a = k = 0.6 m, b = e = f = h = 0.5 m, c = d = 5 m. Its joint frames according to DH method are shown is Figure 4 and the relative parameters are listed in Tables 1 and 2.

Table 1:

DH parameters of space manipulator.

Link i	θ i	di (m)	ai–1 (m)	αi–1
1	thetas;10	0.6	0	90°
2	θ2(90)	0.5	0	−90°
3	θ3(0)	0.5	5	0
4	θ4(0)	0.5	5	0
5	θ5(0)	0.5	0	90°
6	θ6(−90)	0.5	0	−90°
7	θ7(0)	0.6	0	0

Table 2:

Dynamic parameters of space manipulator.

	Base	Link 1	Link 2	Link 3	Link 4	Link 5	Link 6	Link 7
mass/(kg)	25000	42.5	42.5	70	70	42.5	42.5	42.5
P i /(m)	0	0	−0.25	2.5	2.5	0	0	0
	0	−0.25	0	0	0	0	0	0
	0	0.6	0.5	0.5	0.5	0.25	0.25	0.3
I i /(kg·m2)
I xx	8000	0.89	0.05	0.09	0.09	0.89	0.89	1.28
I yy	8000	0.05	0.89	145.83	145.83	0.89	0.89	1.28
I zz	8000	0.89	0.89	145.83	145.83	0.05	0.05	0.05

Figure 4:

A 7-dof free-floating space manipulator.

The initial joint angle is set as Θ₀ = g[−50°, −170°, 150°, −60°, 130°, 170°, 0°] and the final desired pose of the end-effector is [9.60 m, 0.00 m, 3.00 m, −1.00 rad, −0.50 rad, −2.00 rad]. Assume the target is a rigid sphere, whose mass is m _o = 30kg and radius is R = 0.3 m. The inertial tensor is a diagonal matrix, whose elements are I _xx = I _yy = I _zz = 1.08 kgm². Vector from target mass center to contact point is r _co = [0.07, −0.26, −0.13], and the contact direction is N = [0.30, −0.30, 0.91].

5.2. The Configuration Optimization at the Contact Point

First, we can adopt any method to obtain a group of joint angles that are suitable for the capture pose. For example, we planned the end-effector of the manipulator to trace a straight line from the initial point to the desired point, and the velocity is planned by a symmetric trapezium curve with parabolic transition to make the acceleration change stably. Considering the limit of joint velocity, we assume that the whole planning time T _A = 50 s, the acceleration time T _s = 5 s, and the control cycle δT = 50 ms. And [−84.5°, −188.7°, 116.8°, −37.4°, 164.7°, 187.5°, −41.2°] is a suitable manipulator configuration at the contact time. The value of g function is 0.1468. Thereafter, regard this group of joint angles as the initial angles and use the method in Section 3 to optimize the configuration; the curve of g-value versus time is as shown in Figure 5.

Figure 5:

g-value versus time during configuration optimization.

We can see that after about 80's, the value of g function approaches a constant value 0.1882, which is also the maximum value. And comparing with the g-value before optimization 0.1468, the amount of increase is 28.2%, which means that the configuration optimization minimizes the impulsive force greatly. In the process of optimization, the change of the end-effector pose is shown in Figures 6 and 7; the changes of joint angles and joint angle velocities are shown in Figures 8 and 9.

Figure 6:

The position of the end-effector during the configuration optimization.

Figure 7:

The attitude of the end-effector during the configuration optimization.

Figure 8:

The changes of joint angles during the configuration optimization.

Figure 9:

The changes of joint angle velocities during the configuration optimization.

From these figures, we know that in the process of configuration optimization, the end-effector stays at the correct capture pose. And the joint angle velocities all approach zero at the end time, which means that it has achieved the best configuration. This group of joint angles is [−92.6°, −119.8°, 100.3°, −22.5°, 196.6°, 211.3°, 28.8°].

5.3. The Precollision Trajectory Planning Based on PSO

Set the limits of the joint angles as

$\begin{array}{c}{\theta }_{\max }=\left[\mathrm{16}{\mathrm{0}}^{\circ },\mathrm{20}{\mathrm{0}}^{\circ },\mathrm{18}{\mathrm{0}}^{\circ },\mathrm{17}{\mathrm{0}}^{\circ },\mathrm{22}{\mathrm{0}}^{\circ },\mathrm{25}{\mathrm{0}}^{\circ },\mathrm{18}{\mathrm{0}}^{\circ }\right]\\ {\theta }_{\min }=\left[-\mathrm{16}{\mathrm{0}}^{\circ },-\mathrm{22}{\mathrm{0}}^{\circ },-\mathrm{18}{\mathrm{0}}^{\circ },-\mathrm{11}{\mathrm{0}}^{\circ },-\mathrm{18}{\mathrm{0}}^{\circ },-\mathrm{20}{\mathrm{0}}^{\circ },-\mathrm{8}{\mathrm{0}}^{\circ }\right]\end{array}$

(27)

c₁ = c₂ = 2.0, w₁ = w₂ = 50. The population size is 24. After 200 iterations we can get the optimal particle: 1.8e–08 × [0.22, 0.14, −2.56, 1.31, 1.64, 0.20, 5.41, −0.28, −4.40, 4.56, −4.30, −1.58, −5.60, 6.83]. And the value of objective function f is 0.0134, which means the convergence result is satisfactory. The convergence process is shown as Figure 10. Substitute the optimal particle into (19), we can obtain the changes of the joint angle and joint angle velocity in Figures 11 and 12.

Figure 10:

The variation of the global best fitness evaluation.

Figure 11:

The joint angles from initial point to the desired point.

Figure 12:

The joint angle velocities from initial point to the desired point.

In light of the figures, it can be seen that both the joint angle curves and the joint angle velocity curves are smooth and steady, which means this method can be applied to actual operation. And the final pose of the end-effector is [9.59 m, −0.01 m, 3.00 m, −1.00 rad, −0.49 rad, −2.00 rad]; the deviations are acceptable. As for the effect for the impulsive force minimization, through comparison of g value between the before and after optimization in Section 5.2, it proves that the proposed method minimizes the impulsive force greatly.

6. Conclusions

The on-orbit capture operation of free-floating objects by a space robot has become an important mission in recent years. Through the analysis of task characteristics, a scheme for precollision trajectory planning of redundant space manipulator is proposed in this paper. This scheme can obtain the minimized collision impulsive force and the correct capture pose at the same time. In the simulation experiment, the 28.2% increase of g verifies the effectiveness of minimizing the impulsive force and the joint angles; joint angular velocities as well as the the capture pose obtained by the PSO algorithm demonstrate the feasibility in actual application of space manipulator.

Conflict of Interests

The authors declare that there is no conflict of interests regarding the publication of this paper.