The Cartesian Path Planning of Free-Floating Space Robot using Particle Swarm Optimization

Abstract

The Cartesian path planning of free-floating space robot is much more complex than that of fixed-based manipulators, since the end-effector pose (position and orientation) is path dependent, and the position-level kinematic equations can not be used to determine the joint angles. In this paper, a method based on particle swarm optimization (PSO) is proposed to solve this problem. Firstly, we parameterize the joint trajectory using polynomial functions, and then normalize the parameterized trajectory. Secondly, the Cartesian path planning is transformed to an optimization problem by integrating the differential kinematic equations. The object function is defined according to the accuracy requirement, and it is the function of the parameters to be defined. Finally, we use the Particle Swarm Optimization (PSO) algorithm to search the unknown parameters. The approach has the following traits: 1) The limits on joint angles, rates and accelerations are included in the planning algorithm; 2) There exist not any kinematic and dynamic singularities, since only the direct kinematic equations are used; 3) The attitude singularities do not exist, for the orientation is represented by quaternion; 4) The optimization algorithm is not affected by the initial parameters. Simulation results verify the proposed method.

1. Introduction

Robotic systems are expected to play an increasingly important role in the future space activity. One broad area of application is in the servicing, construction, and maintenance of satellites and large space structures in orbit. However, the planning and control of space robot pose additional problems beyond those on earth, due to the dynamic coupling between space manipulators and their spacecraft (Nakamura & Mukherjee, 1991; Xu et al., 2006; Huang et al., 2006). Free-floating systems (Both the attitude and position of the base are not controlled) exhibit non-holonomic behavior, which results from the non-integrability of the angular momentum. The free-floating mode is very important, since it can save the fuel of the spacecraft and avoid collision. So, many scholars have been studing the planning and control of free-floating space robot. Huang et al presented a minimum-torque path-planning scheme in joint space (Huang et al., 2006). For inertial manipulation, such as capturing a free object, some methods based on Generalized Jacobian Matrix (GJM) are proposed (Umetani & Yoshida, 1989; Moosavian & Papadopoulos, 1997; Moosavian & Papadopoulos, 2007; Traira et al., 2000). However, the algorithms that use a Jacobian inverse, such as the resolved rate or resolved acceleration control algorithms, fail at dynamic singularities. Ones that use the pseudo inverse or transpose of the Jacobian will likely follow the available direction, but they may result in large errors.

Dynamic singularities are path dependent (Papadopoulos & Dubowsky, 1993). Based on the calculation of PIW (Path Independent Workspaces) and PDW (Path Dependent Workspace), Papadopoulos (Papadopoulos, 1992) proposed a planning algorithm that avoids dynamic singularities, and permits the manipulator's end-effector to move from any reachable workspace location to any other. But the PIW and PDW's calculation for a space manipulator of higher degree of freedom is very complicated. Papadopoulos and Tortopidis also developed a path planning methodology that allows simultaneous manipulator end-point and spacecraft attitude control using manipulator actuators only (Papadopoulos et al., 2005; Tortopidis & Papadopoulos, 2007). Pandey (Pandey & Agrawal, S., 1997) proposed a method called “mode summation” to plan the Cartesian path of a free-floating system with prismatic joints. Lampariello et al. (Lampariello & Deutrich, 1999) applied the method to a system with rotational joints. But they didn't propose a general method to limit the joint accelerations. They also did not develop a general criterion to assign the initial values of the unknown parameters. The authors (Xu et al., 2007) improved the mxethod to tackle these problems, by normalizing the joint functions. However, there are still some shortcomings of the method. Firstly, the essential of the method is a Newton iterative algorithm, which will converges to local minimum. Secondly, the initial parameters are very important for the convergence speed. If they are assigned close to the solutions, the convergence speed is quadratic. Otherwise, the algorithm converges very slowly. Thirdly, the representation of the body attitude (including the base attitude and the end-effector attitude), which is described by Euler angles, will meets with singular orientations (i.e. the Jacobian that maps differential changes in the representation to the angular velocity is singular for some orientations). In this paper, an approach, which is the further research of the previous work (Xu et al., 2007), is proposed to plan the Cartesian plan of free-floating space robotic system. The system is composed of a 6 DOF manipulator and a satellite. The method solves the shortcomings of the old algorithm.

This paper is organized as follows: Section two derives the kinematic equations of space robotic system. Then, the problem of Cartesian path planning is discussed in Section three. In Section four, the joint functions are parameterized and normalized, and the PSO algorithm is used to solve the path planning problem. In section five, the simulation results are showed. Section six is for conclusion and discussion. The last section is the acknowledgement.

2. Modeling of Space Robotic System

2.1 Symbols and Variables

Major research achievements on space robot were collected by Xu and Kanade (Xu & Kanade, 1992), and were also reviewed by Moosavian and Papadopoulos recently (Moosavian & Papadopoulos, 2007).

Here, we model the space robotic system using Lagrangian method. Fig.1 shows a general model of a space robot system with a single manipulator arm, which is regarded as a n+1 serial link system connected with n degree of freedom active joints. B₀ denotes the satellite main body, B _i (i = 1, …, n) denotes the i ^th link of the manipulator, and J _i is the joint which connects B _i-1 and B _i . In order to conveniently discuss, some symbols and variables are defined as follows (The following vectors are described in the inertia frame, if not pointed out specially):

Fig. 1.

General Model of a Single Arm Space Robot

Σ _I , Σ _E : the inertia frame and the end-effector frame, respectively;

Σ _i (i = 0, …, n): the body fixed frame of B _i

C _i (i = 0, …, n): the position of the B _i 's centroid

a _i , b _i ∈ R ³(i = 0, …, n): the position vectors from J _i to C _i and C _i to J_i+1 respectively;

r _i ∈ R ³(i = 1, …, n): the position vector of C _i ;

p _e ∈ R ³(i = 1, …, n): the position vector of the end-effector;

r _g ∈ R ³:: the position vector of the system's CM;

p _i ∈ R ³(i = 1, …, n): the position vector of J _i ;

k _i (i = 1, …, n): the unit vector representing the rotation direction of J _i ;

Q _b , Q _e ∈ R ⁴: the unit quaternion representations of the base and the end-effector orientations, respectively.

${\dot{\mathit{x}}}_{\mathit{b}}\in {\mathit{R}}^6$ : The linear velocity and angular velocity of B₀, i.e. ${\dot{\mathit{x}}}_{\mathit{b}}={[{\mathit{v}}_0^T,{\mathit{\omega }}_0^T]}^T$ ;

${\dot{\mathit{x}}}_e\in {\mathit{R}}^6$ : The linear velocity and angular velocity of the end-effector, i.e. ${\dot{\mathit{x}}}_e={[{\mathit{v}}_e^T,{\mathit{\omega }}_e^T]}^T$

Θ = [θ₁, …, θ _n ] ∈ R ⁿ : the joint angle vector;

m _i (i = 0, …, n): the mass of B _i ;

M: total mass of the system, i.e. $M=\sum _{i=0}^n{m}_i$

I _i(i = 0, …, n): the inertia matrix of B _i

E: 3 × 3 identity matrix;

2.2 Kinematic Equations of Space Robotic System

From Fig. 1, the position vector of the end-effector is

$${\mathit{p}}_e={\mathit{r}}_0+{\mathit{b}}_0+\sum _{k=1}^n({\mathit{a}}_k+{\mathit{b}}_k)$$

(1)

Differentiating it with respect to time, a relationship between end-effector linear velocity and joint velocity is obtained, i.e.

$${\mathit{v}}_e={\dot{\mathit{p}}}_0={\mathit{v}}_0+{\mathit{\omega }}_0\times ({\mathit{p}}_e-{\mathit{r}}_0)+\sum _{k=1}^n[{\mathit{k}}_k\times ({\mathit{p}}_e-{\mathit{p}}_k)]{\dot{\theta }}_k$$

(2)

On the other hand, a relationship between end-effector angular velocity and joint velocity is expressed with

$${\mathit{\omega }}_e={\mathit{\omega }}_0+\sum _{k=1}^n{\mathit{k}}_k{\dot{\theta }}_k$$

(3)

Then, the differential kinematic equation can be determined according to (2) and (3), i.e.:

$$\left[\begin{array}{c}{\mathit{v}}_e\\ {\mathit{\omega }}_e\end{array}\right]={\mathit{J}}_b\left[\begin{array}{c}{\mathit{v}}_0\\ {\mathit{\omega }}_0\end{array}\right]+{\mathit{J}}_m\dot{\mathbf{\Theta }}$$

(4)

where, J _b and J _m are the Jacobian matrixes dependent on the base and the manipulator, respectively(see Ref. (Yoshida, 2003)). Since no external forces and torques acting on the free-floating system, the linear and angular momentums are conserved. With the assumption that their initial values are zeros, the momentum conservation equations of the free-floating space robot are

$${\mathit{H}}_b{\dot{\mathit{x}}}_b+{\mathit{H}}_{bm}\dot{\mathbf{\Theta }}=0$$

(5)

The matrixes H _b and H _bm , whose expressions can be found in Ref. (Yoshida, 2003), are the inertia matrix of the base and coupling inertia matrix, respectively. Then ${\dot{\mathit{x}}}_b$ can be solved as

$${\dot{\mathit{x}}}_b=-{{\mathit{H}}_b}^{-1}{\mathit{H}}_{bm}\dot{\mathbf{\Theta }}={\mathit{J}}_{bm}\dot{\mathbf{\Theta }}\iff \left[\begin{array}{c}{\mathit{v}}_0\\ {\mathit{\omega }}_0\end{array}\right]=\left[\begin{array}{c}{\mathit{J}}_{bm\mathrm{\_}v}\\ {\mathit{J}}_{bm\mathrm{\_}\omega }\end{array}\right]\dot{\mathbf{\Theta }}$$

(6)

The matrix J _bm is base-manipulator Jacobian matrix and J _{bm_v} and J _{bm_ω} are its sub matrixes. Substituting (6) to (4), the kinematic equations of free-floating space robotic system are given as

$${\dot{\mathit{x}}}_e=\left[\begin{array}{c}{\mathit{v}}_e\\ {\mathit{\omega }}_e\end{array}\right]={\mathit{J}}_g({\mathbf{\Psi }}_0,\mathbf{\Theta },m_i,{\mathit{I}}_i)\dot{\mathbf{\Theta }}=\left[\begin{array}{c}{\mathit{J}}_{g\mathrm{\_}v}\\ {\mathit{J}}_{g\mathrm{\_}\omega }\end{array}\right]\dot{\mathbf{\Theta }}$$

(7)

where, J _g (Ψ₀, Θ, m _i , I _i ) is the Generalized Jacobian Matrix (Umetani & Yoshida, 1989). If the GJM is of full rank, the joint rate can be calculated by $\dot{\mathbf{\Theta }}={{\mathit{J}}_g}^{-1}{\dot{\mathit{x}}}_e$ . Unfortunately, in most of the workspace, the singularities of J_g can not be avoided. Since J _g is the functions of the dynamic parameters of each body, its singularities are called dynamic singularities (Papadopoulos & Dubowsky, 1993). Different from the kinematic singularities (The singularities of traditional Jacobian are called kinematic singularities for the difference) of fixed-based manipulators, the dynamic singularities in Cartesian space are dependent on the path, and can not be determined beforehand. The characteristic complicate the Cartesian path planning of space robot.

2.3 Quaternion Representation of Body Attitude

Euler angles are the common minimal representation of attitude. However there are always singular orientations. For example, if the attitude is represented by the Z-Y-X Euler angles Ψ= [α, β, γ], the relationship between $\dot{\mathbf{\Psi }}$ and ω is

$$\mathit{\omega }=\left[\begin{array}{ccc}0& -s_{\alpha }& c_{\alpha }{c}_{\beta }\\ 0& c_{\alpha }& s_{\alpha }{c}_{\beta }\\ 1& 0& -s_{\beta }\end{array}\right]\dot{\mathbf{\Psi }}={\mathit{J}}_{Euler}\dot{\mathbf{\Psi }}$$

(8)

When J _Euler is non-singular, $\dot{\mathbf{\Psi }}$ can be calculated according to the angular velocity ω, i. e.

$$\dot{\mathbf{\Psi }}=({\mathit{J}}_{Euler}{)}^{-1}\bullet \mathit{\omega }$$

(9)

However, when β = ±π/2, J _Euler is singular, i.e. infinite rotational rates ( $\dot{\alpha },\dot{\beta },\dot{\gamma }$ ) are required to produce a finite rigid-body angular velocity. This is called orientation singularity or attitude singularity.

The unit quaternion is a popular nonsingular four-parameter representation(Jack, 1992). A quaternion Q is defined as

$$\begin{array}{rl}\mathit{Q}& =\eta +q_1\overrightarrow{i}+q_2\overrightarrow{j}+q_3\overrightarrow{k}=\eta +\mathit{q}{\left[\eta q_1{q}_2{q}_3\right]}^T\\ & =\cos \frac{\psi }{2}+\mathit{r}\bullet sin\frac{\psi }{2}\end{array}$$

(10)

where, r and Ψ are, respectively, the unit vector and the rotation angle of an equivalent angle/axis description. Notice that, the scalar part and the vector part are constrained by

$${\eta }^2+{q_1}^2+{q_2}^2+{q_3}^2=1$$

(11)

It is well known that {η, q} and {-η, -q} represent the same orientation. In this paper, Ψ is bounded by (−π, π], so η≥0. The relationship between the time derivative of the quaternion and the angular velocity is established by

$$\left[\begin{array}{c}\dot{\eta }\\ \dot{\mathit{q}}\end{array}\right]=\frac{1}{2}\left[\begin{array}{c}-{\mathit{q}}^T\\ \eta \mathit{I}-\tilde{\mathit{q}}\end{array}\right]\mathit{\omega }$$

(12)

where, $\tilde{\mathit{q}}$ is defined as

$$\tilde{\mathit{q}}=\left[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}\right]$$

(13)

For two coordinate systems, if their orientations are represented by {η, q₁} and {η₂, q₂} respectively, the relative attitude is given by {Δη, Δq}, where (Yuan, 1988)

$$\{\begin{array}{l}\delta \eta ={\eta }_1{\eta }_2+{{\mathit{q}}_1}^T{\mathit{q}}_2\\ \delta \mathit{q}={\eta }_1{\mathit{q}}_2-{\eta }_2{\mathit{q}}_1-{\tilde{\mathit{q}}}_1{\mathit{q}}_2\end{array}$$

(14)

When the two frames coincide,

$$\{\begin{array}{l}\delta \eta =1\\ \delta \mathit{q}=0\end{array}$$

(15)

Furthermore, through the normality condition (11), δq=0 implies δ, η=±1. Therefore, it can be concluded that the two frames coincide if and only if δq=0.

2.4 The Space Robotic System Studied in the Paper

The space robotic system studied in the paper is shown as Fig. 2. It is composed of a carrier spacecraft (called Space Base or Base), a PUMA-type manipulator (called Space Manipulator) and a target satellite (called Target to be captured. The frames fixed on the multi-body system are defined as Fig. 3 (when the joint angles are all zeros), where Z _i is the direction of J _i and the link lengths are denoted by L₁∼L₇. Table I lists the dimensions and mass properties of the bodies (Sat and B _i stand for the satellite and the i^th body, respectively). The link vectors ⁱ a _i , ⁱ b _i and the inertial matrix ⁱ I _i are expressed in Σ _i .

Fig. 2.

The space robotic system studied in the paper

Fig. 3.

The body-fixed frames of the space robotic system

Table 1.

The Mass Properties of the Space Robotic System

		Sat	B 1	B 2	B 3	B 4	B 5	B 6
Mass/kg		400	6	5	5	4	3	2
i a i /m			0	0.2702	0	0	0	0
			0	0	0	0	−0.0338	0
			0.15	−0.2513	0.15	−0.35	0	0.0750
i b i /m		0.3570	0	0.5598	0	0	0	0
		−0.0095	0	0	0	0	−0.0662	0
		0.419	0.15	−0.0487	0.15	−0.35	0	0.1595
i I i /kg.m2	I xx	30	0.15	0.0926	0.105	0.2498	0.0330	0.052
	I yy	28	0.15	0.9053	0.105	0.2498	0.0172	0.052
	I zz	32	0.075	0.8451	0.0294	0.0196	0.0260	0.022
	I xy	0.26	0	0	0	0	0	0
	I xz	0.37	0	0.1315	0	0	0	0
	I yz	−0.29	0	0	0	0	0	0

3. Problem of the Cartesian Path Planning

3.1 Description of the Problem

Point-to-point path planning in Cartesian space for free-floating space robot is studied here, i.e., the joint path is planned to make the end-effeor attain the desired pose. Then, the system state is composed of the attitude (quartenion) and position of the end-effector:

$${\mathit{X}}_{\mathit{e}}=\left[\begin{array}{c}{\mathit{Q}}_e\\ {\mathit{p}}_e\end{array}\right]={[{\eta }_e,q_{e1},q_{e2},q_{e3},p_{ex},p_{ey},p_{ey},p_{ez}]}^T={\mathit{X}}_{\mathit{e}}({\mathit{Q}}_0,\mathbf{\Theta })$$

(16)

Equation (16) shows that, the end-effector is the function of the base attitude and the joint angles. Correspondingly, the initial, final and desired poses of the end-effector are written as X _e0, X _ef and X _ed respectively. Then the problem of the path planning is expressed as follows:

$$\begin{array}{rl}& Todetermine:{\theta }_i(t)f_{\mathrm{i}}(t),i=1,2,\dots ,n\\ & Tomake:{\mathit{X}}_e(t_0)={\mathit{X}}_{e0},{\mathit{X}}_e(t_f)={\mathit{X}}_{ed}\end{array}$$

subject to the equality constraints:

$$\{\begin{array}{l}{\theta }_i(0)={\theta }_{i0},{\dot{\theta }}_i(0)=0,{\ddot{\theta }}_i(0)=0,\\ {\dot{\theta }}_i(t_f)={\ddot{\theta }}_i(t{)}_f=0\end{array}$$

(17)

$${\mathit{Q}}_0(0)={\mathit{Q}}_{00}$$

(18)

and the inequality constraints:

$$\{\begin{array}{l}{\theta }_{i\mathrm{\_}min}\le {\theta }_i(t)\le {\theta }_{i\mathrm{\_}max},{\dot{\theta }}_{i\mathrm{\_}min}\le {\dot{\theta }}_i(t)\le {\dot{\theta }}_{i\mathrm{\_}max}\\ {\ddot{\theta }}_{i\mathrm{\_}min}\le {\ddot{\theta }}_i(t)\le {\ddot{\theta }}_{i\mathrm{\_}max}\end{array}$$

(19)

where, $[{\theta }_{i\mathrm{\_}min},{\theta }_{i\mathrm{\_}max}],[{\dot{\theta }}_{i\mathrm{\_}min},{\dot{\theta }}_{i\mathrm{\_}max}]$ and $[{\ddot{\theta }}_{i\mathrm{\_}min},{\ddot{\theta }}_{i\mathrm{\_}max}]$ are the bounds of i^th joint position, velocity and acceleration, respectively. The aim of path planning is actually to make the difference between the final state and desired state approaches zero, i.e.

$$\parallel {\mathit{X}}_{ed}-{\mathit{X}}_{ef}\parallel \to 0$$

(20)

According to Equations (6), (7) and (12), the following results are given

$${\dot{\mathit{Q}}}_0=\left[\begin{array}{c}{\dot{\eta }}_0\\ {\dot{\mathit{q}}}_0\end{array}\right]=\frac{1}{2}\left[\begin{array}{c}-{{\mathit{q}}_0}^T\\ {\eta }_0\mathit{I}-{\tilde{\mathit{q}}}_0\end{array}\right]{\mathit{J}}_{bm\mathrm{\_}\omega }\dot{\mathbf{\Theta }}$$

(21)

$${\dot{\mathit{Q}}}_e=\left[\begin{array}{c}{\dot{\eta }}_e\\ {\dot{\mathit{q}}}_e\end{array}\right]=\frac{1}{2}\left[\begin{array}{c}-{{\mathit{q}}_e}^T\\ {\eta }_e\mathit{I}-{\tilde{\mathit{q}}}_e\end{array}\right]{\mathit{J}}_{g\mathrm{\_}\omega }\dot{\mathbf{\Theta }}$$

(22)

Therefore, the base attitude and end-effector attitude at time t are obtained by the integrals:

$${\mathit{Q}}_0(t)={\int }_0^t\frac{1}{2}\left[\begin{array}{c}-{{\mathit{q}}_0}^T\\ {\eta }_0\mathit{I}-{\tilde{\mathit{q}}}_0\end{array}\right]{\mathit{J}}_{bm\mathrm{\_}\omega }\dot{\mathbf{\Theta }}dt$$

(23)

$${\mathit{Q}}_e(t)={\int }_0^t\frac{1}{2}\left[\begin{array}{c}-{{\mathit{q}}_e}^T\\ {\eta }_e\mathit{I}-{\tilde{\mathit{q}}}_e\end{array}\right]{\mathit{J}}_{g\mathrm{\_}\omega }\dot{\mathbf{\Theta }}dt$$

(24)

Then, From Equation (14), the orientation difference of end-effector at time t _f is:

$$\delta {\mathit{q}}_e={\eta }_{ef}{\mathit{q}}_{ed}-{\eta }_{ed}{\mathit{q}}_{ef}-{\tilde{\mathit{q}}}_{ef}{\mathit{q}}_{ed}$$

(25)

where, {η_ef, q _ef } and {η _ed , q _ed } are the quaternion representations of the final and desired values of the end-effector orientation. On the other hand, the end-effector position is given by the integral (see Equation (7)):

$${\mathit{P}}_e(t)={\int }_0^t{\dot{\mathit{v}}}_{e}dt={\int }_0^t{\mathit{J}}_{g\mathrm{\_}v}\dot{\mathbf{\Theta }}dt$$

(26)

And the position difference of end-effector at time t _f is:

$$\delta {\mathit{P}}_e={\mathit{P}}_{ed}-{\mathit{P}}_{ef}={\mathit{P}}_{ed}-{\int }_0^t{\mathit{J}}_{g\mathrm{\_}v}\dot{\mathbf{\Theta }}dt$$

(27)

3.2 Parameterization and Normalization of Joint Functions

The polynomial functions are usually used to get smooth joint motion(Angeles, 2006). Here we parameterize the joint angle by 5^th order polynomial functions:

$${\theta }_i(t)=\sum _{p=0}^5{a}_{ip}{t}^p,(i=1,\cdots ,6)$$

(28)

where, 0≥t≥t _f , and a_i0∼a_i5 are the coefficients of the polynomial. Applying the constraints of Equation (17) to Equation (28), the results are:

$$\{\begin{array}{l}{a}_{i0}={\theta }_{i0},\\ a_{i1}=a_{i2}=0\\ a_{i3}=\frac{5}{3}{a}_{i5}{t}_f^2,\\ a_{i4}=-\frac{5}{2}{a}_{i5}{t}_f\end{array}$$

(29)

Correspondingly, the joint position, velocity and acceleration functions are given by:

$${\theta }_i=a_{i5}(t^5-\frac{5}{2}{t}_f{t}^4+\frac{5}{3}{t}_f^2{t}^3)+{\theta }_{i0}$$

(30)

$${\dot{\theta }}_i=\frac{d{\theta }_i}{dt}=a_{i5}(5t^4-10t_f{t}^3+5t_f^2{t}^2)$$

(31)

$${\ddot{\theta }}_i=\frac{d^2{\theta }_i}{d{t}^2}=a_{i5}(20t^3-30t_f{t}^2+10t_f^{2}t)$$

(32)

From (30)∼(32), there is only a parameter a_i5 in each joint function after parameterization. Let

$$\mathit{a}={[a_{15},a_{25},\cdots ,a_{n5}]}^T$$

(33)

when the parameter a is solved, the joint path is determined. Let τ=t/t _f , i.e. t=τ/t _f (τ∈[0,1]), the joint functions become as follows(τ is the normalized time):

$${\theta }_i={\hat{a}}_{i5}({\tau }^5-\frac{5}{2}{\tau }^4+\frac{5}{3}{\tau }^3)+{\theta }_{i0}$$

(34)

$${\dot{\hat{\theta }}}_i=\frac{d{\theta }_i}{d\tau }={\hat{a}}_{i5}(5{\tau }^4-10{\tau }^3+5{\tau }^2)=t_f{\dot{\theta }}_i$$

(35)

$${\ddot{\hat{\theta }}}_i=\frac{d^2{\theta }_i}{d{\tau }^2}={\hat{a}}_{i5}(20{\tau }^3-30{\tau }^2+10\tau )=t_f^2{\dot{\theta }}_i$$

(36)

where,

$${\hat{a}}_{i5}=a_{i5}{t}_f^5$$

(37)

Correspondinly, the integrals (23), (24) and (26) become

$${\mathit{Q}}_0(\tau )={\int }_0^{\tau }\frac{1}{2}\left[\begin{array}{c}-{{\mathit{q}}_0}^T\\ {\eta }_0\mathbf{I}-{\tilde{\mathit{q}}}_0\end{array}\right]{\mathit{J}}_{bm\mathrm{\_}\omega }\dot{\hat{\mathbf{\Theta }}}d\tau$$

(38)

$${\mathit{P}}_e(\tau )={\int }_0^{\tau }{\mathit{J}}_{g\mathrm{\_}v}\dot{\hat{\mathbf{\Theta }}}d\tau$$

(39)

$${\mathit{Q}}_e(\tau )={\int }_0^{\tau }\frac{1}{2}\left[\begin{array}{c}-{{\mathit{q}}_e}^T\\ {\eta }_e\mathbf{I}-{\tilde{\mathit{q}}}_e\end{array}\right]{\mathit{J}}_{g\mathrm{\_}\omega }\dot{\hat{\mathbf{\Theta }}}d\tau$$

(40)

And, known from (25) and (27), the end-effector pose error at t _f is the function of the parameters $\hat{\mathit{a}}$ after normalization, i. e.

$$\mathit{G}(\hat{\mathit{a}})=\left[\begin{array}{c}\delta {\mathit{q}}_e\\ \delta {\mathit{P}}_e\end{array}\right]\in {\mathit{R}}^6$$

(41)

According to (20) and (41), the planning problem can be transformed to an optimization problem, whose object function is defined according to the accuracy requirement. A good choice is

$$f(\hat{\mathit{a}})=\frac{\parallel \delta {\mathit{q}}_e\parallel }{k_q}+\frac{\parallel \delta {\mathit{p}}_e\parallel }{k_p}$$

(42)

where, the norm ‖x‖ = x ^T x, and, k _q and k _p are the weight parameters of attitude error (represented by quaternion) and position error, respectively. Then, the planning problem is reduced to the searching of the optimized parameter ${\hat{\mathit{a}}}^{\ast }$ . When $f({\hat{\mathit{a}}}^{\ast })\le 1$ , the problem is solved, and ${\hat{\mathit{a}}}^{\ast }$ determines the planned path. The calculation process of the object function is shown as Fig. 4.

Fig. 4.

Object function Calculation

4. Solution to the Path planning Problem Based on Particle Swarm Optimization

4.1 Review of Particle Swarm Optimization

Particle Swarm Optimization (PSO), which resembles a school of flying birds, was originally developed by Kennedy and Eberhart to efficiently find optimal or near-optimal solutions in large search spaces (Kennedy & Eberhart, 1995; Eberhart & Kennedy, 1995). In a particle swarm optimizer, instead of using genetic operators, these individuals are “evolved” by cooperation and competition among the individuals themselves through generations. Each individual, named as a “particle”, adjusts its flying according to its own flying experience and its companions' flying experience. It represents a potential solution to a problem, and is treated as a point in a n-dimensional space. The i^th particle is described as

$${\mathit{X}}_i=(x_{i1},x_{i2},\cdots ,x_{in})$$

(43)

The best previous position (the position giving the best fitness value) of any particle is recorded and written as

$${\mathit{P}}_i=(p_{i1},p_{i2},\cdots ,p_{in})$$

(44)

and the best particle among all the particles in the population is denoted by

$${\mathit{P}}_g=(p_{g1},p_{g2},\cdots ,p_{gn})$$

(45)

The velocity of particle i is

$${\mathit{V}}_i=(v_{i1},v_{i2},\cdots ,v_{in})$$

(46)

Then, the particles are manipulated according to the following equations (Shi and Eberhart, 1998):

$$\begin{array}{rl}{v}_{id}& =w\bullet v_{id}+c_1\bullet rand()\bullet (p_{id}-x_{id})\\ & +c_2\bullet Rand()\bullet (p_{gd}-x_{id})\end{array}$$

(47)

$$x_{id}=x_{id}+v_{id}$$

(48)

where, c₁ and c₂ are two positive constants, called the cognitive and social parameter respectively; rand() and Rand() are two random functions in the range [0,1]. The inertia weight w plays the role of balancing the global search and local search. It can be a positive constant or even a positive linear or nonlinear function of time. The particle swarm optimizer has been found to be robust and fast in solving nonlinear, non-differentiable, multi-modal problems.

4.2 The Solution Based on PSO

One of the reasons that particle swarm optimization is attractive is that there are very few parameters to adjust. It has been used for the path planning of free-floating space robot (Huang & Xu, 2006) and underactuated runing robot (Takahashi et al., 2006). Here, the optimization problem of (42) is solved using the PSO algorithm too. Firstly, the i^th particle is defined as

$${\mathit{X}}_i={\hat{\mathit{a}}}^i=({\hat{a}}_{15}^i,{\hat{a}}_{25}^i,\cdots ,{\hat{a}}_{65}^i)$$

(49)

The population size is assumed to be n _p , and the maximum number of iterations (epochs) to train is N _max . The iteration number is denoted by k. Then, the optimization procedure is as follows:

Set k=1, and initialize a population (array) of particles with random positions and velocities on n dimensions(here, n=6) in the problem space:

$$\{{\hat{\mathit{a}}}^1(k),{\hat{\mathit{a}}}^2(k),\cdots ,{\hat{\mathit{a}}}^{n_p}(k)\},1\le k\le N_{max}$$

(50)

The fitness function of each particle F _i is calculated by (42), i.e.

$$F_i(k)=f({\hat{\mathit{a}}}^i(k)),i=1,2,\cdots ,n_p$$

(51)

To calculate the best previous fitness evaluation of each particle F _{i_best} and the best previous position P _i :

$$F_{i\mathrm{\_}best}=\{\begin{array}{l}{F}_i(k),\text{if\hspace{0.17em}}k=1\\ F_i(k),\text{\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}else if\hspace{0.17em}}{F}_i(k)\prec F_{i\mathrm{\_}best}\\ F_{i\mathrm{\_}best},\text{\hspace{0.17em}\hspace{0.17em}else}\end{array}$$

(52)

$${\mathit{P}}_i=\{\begin{array}{l}{\hat{\mathit{a}}}^i(k),\text{if\hspace{0.17em}}k=1\\ {\hat{\mathit{a}}}^i(k),\text{\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}else if\hspace{0.17em}}{F}_i(k)\prec F_{i\mathrm{\_}best}\\ {\mathit{P}}_i,\text{else}\end{array}$$

(53)

4. To calculate the best fitness evaluation F _{g_best} and the best particle P _g among all the particles in the population:

$$F_{g\mathrm{\_}best}=\{\begin{array}{l}\underset{1\le i\le n_p}{min}(F_i(k)),\text{\hspace{0.17em}if\hspace{0.17em}}k=1\\ \underset{1\le i\le n_p}{min}(F_i(k)),\text{\hspace{0.17em}else if\hspace{0.17em}}\underset{1\le i\le n_p}{min}(F_i(k))\prec F_{g\mathrm{\_}best}\\ F_{g\mathrm{\_}best},\text{\hspace{0.17em}else}\end{array}$$

(54)

$${\mathit{P}}_g=\{\begin{array}{l}{\hat{\mathit{a}}}^{i\mathrm{\_}min}(k),\text{if\hspace{0.17em}}k=1\\ {\hat{\mathit{a}}}^{i\mathrm{\_}min}(k),\text{\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}else if\hspace{0.17em}}\underset{1\le i\le n_p}{min}(F_i(k))\prec F_{g\mathrm{\_}best}\\ {\mathit{P}}_g,\text{else}\end{array}$$

(55)

where, ${\hat{\mathit{a}}}^{i\mathrm{\_}min}(k)$ is the best particle of i^th iteration.

Change the velocity and position of the particle according to equations (47) and (48), respectively:

Loop to step 2) until a criterion is met, usually a sufficiently good fitness or a maximum number of iterations (N _max ).

The whole process of the algorithm is shown as Fig.5. Particles' velocities on each dimension are limited to a maximum velocity V_max. If the sum of accelerations cause the velocity on that dimension to exceed V_max, which is a parameter specified by the user, then it is assigned as V_max. Therefore, V_max is an important parameter. It determines the resolution, or fineness, with which regions between the present position and the target (best so far) position are searched. If V_max is too high, particles might fly past good solutions. If it is too small, on the other hand, particles may not explore sufficiently beyond locally good regions. In fact, they could become trapped in local optima, unable to move far enough to reach a better position in the problem space.

Fig. 5.

The Flow-Chart of the Algorithm

5. Simulation Study

Not loss of generality, the limits on the joint velocity and acceleration are assumed to be:

$$|{\dot{\theta }}_i|\le {\dot{\theta }}_{i\mathrm{\_}max}=5^{\circ }/\mathrm{s}=0.0873rad/s$$

(56)

$$|{\ddot{\theta }}_i|\le {\ddot{\theta }}_{i\mathrm{\_}max}={0.5}^{\circ }/{\mathrm{s}}^2=0.0873rad/s^2$$

(57)

The initial and desired poses of the end-effector are

$${\mathit{X}}_{e0}=\left[\begin{array}{l}[0.0000,0.9848,0.0000,0.1736{]}^T\\ [1.4644,-0.0137,0.4602{]}^T\end{array}\right]$$

(58)

$${\mathit{X}}_{ed}=\left[\begin{array}{l}[-0.0085,0.9988,0.0096,0.0472{]}^T\\ [1.3501,-0.0137,0.1401{]}^T\end{array}\right]$$

(59)

The initial values of joint angles (Θ₀) and the attitude quaternion of the base Q ₀₀ are

$${\mathbf{\Theta }}_0={[0,{47.72}^{\circ },-{93.91}^{\circ },0,-{23.82}^{\circ },{0.00}^{\circ }]}^T$$

(60)

$${\mathit{Q}}_{00}={[1,0,0,0]}^T$$

(61)

The object function is defined as (42). If the admitted orientation error and position error are 1° and 5 mm respectively, k _q and k _p are determined as

$$k_q=\parallel \mathit{r}\bullet sin\frac{\psi }{2}\parallel =\parallel \mathit{r}\parallel \bullet sin\frac{\psi }{2}=sin(\frac{1}{2}\times \frac{\pi }{180})=0.0087$$

(62)

$$k_p=5\times {10}^{-3}=0.005$$

(63)

After the normalization, the bound of â_i5 can be set (Xu et al., 2007)

$${\hat{a}}_{i5}\in [-5,5]$$

(64)

The parameters for the algorithms are:

$$V_{max}=2$$

(65)

$$n_p=24,N_{max}=1000$$

(66)

$$c_1=c_2=2$$

(67)

$$w_{max}=0.9,w_{min}=0.4$$

(68)

The inertia weight w is decreased linearly from w_max to w_min during a run. The converged parameters and the corresponding object function are (The convergence process is shown as Fig. 6.)

$$\{\begin{array}{l}{\hat{\mathit{a}}}^{\ast }=[-0.0438-3.45671.2781-0.1517-0.3410-0.2711]\\ f({\hat{\mathit{a}}}^{\ast })=0.0859\end{array}$$

(69)

Fig. 6.

The Variation of the Global Best Fitness Evaluation

The total time is determined by (Xu et al., 2007)

$$t_f\ge \left|{\dot{\hat{\theta }}}_{imax}\right|/{\dot{\theta }}_{imax}=12.3784$$

(70)

$$t_f\ge \sqrt{{\ddot{\hat{\theta }}}_{imax}/{\ddot{\theta }}_{imax}}=19.5237$$

(71)

Therefore, it is chosen as t _f =20s. According to the simulation results, the planned joint angles are shown as Fig. 7. And the joint rate and acceleration vary as Fig. 8 and Fig. 9 respectively. These trajectories are very smooth and suitable for the control of the manipulator. The curves of the end-effector orientation and the position are shown as Fig. 10 and Fig. 11. Both show that, the end-effector moves from the initial pose to the desired pose. The corresponding attitude of the base changes as Fig. 12.

Fig. 7.

The Planned Joint Angles

Fig. 8.

The Curves of Joint Rates during the maneuver

Fig. 9.

The Curves of Joint Accelerations during the maneuver

Fig. 10.

The Curves of the End-effector Attitude Quaternion

Fig. 11.

The Curves of the End-effector Position

Fig. 12.

The Variation of the Base Attitude (represented by Quaternion)

6. Conclusion and Discussion

For free-floating space robotic system, the end-effector pose is not only dependent on the current joint angles, but also on the history of each joint path. Therefore, the position-level kinematic equation can not be used to plan the Cartesian path, although it is often used by the fixed-based manipulators. In this paper, we propose an approach to plan the Cartesian point-to-point path. After parameterization and normalization of joint trajectories, the planning problem is transformed to an optimization problem, and the PSO algorithm is used to search the parameters to be determined. It is more applicable, since the constraints on the joint angles, rates and accelerations are included into the planning method, and the planned path is very smooth to control the manipulator. Furthermore, the kinematic and dynamic singularities do not affect the algorithm, because only the direct kinematic equations are used. Another advantage is that the convergence of the algorithm is not affected by the attitude singularity, for the orientation error is represented by quaternion. And the initial parametesr have little effect on its convegence.

It is worth pointing out that the reason of parameterization using monotonic functions is for the possible shorter manoeuvre, not especially for the PSO. If the desired poses is infeasible (such as the poses beyond the workspace), the PSO will search the parameters generating the minimum differences between the final poses and the desired poses. The shortcomings of the algorithms are: 1) The accumulated error in the numerical integration will affect the accuracies of the position and attitude of the end-effector; and 2) the computational cost might be large and the convergence time will be long. The future work will focus on the improving of the algorithm.