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.

Keywords

Space robot path planning on-orbit servicing particle swarm optimization

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{x}}_{b} \in R^{6}$ : The linear velocity and angular velocity of B₀, i.e. ${\dot{x}}_{b} = {[v_{0}^{T}, ω_{0}^{T}]}^{T}$ ;

${\dot{x}}_{e} \in R^{6}$ : The linear velocity and angular velocity of the end-effector, i.e. ${\dot{x}}_{e} = {[v_{e}^{T}, ω_{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

p_{e} = r_{0} + b_{0} + \sum_{k = 1}^{n} (a_{k} + b_{k})

(1)

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

v_{e} = {\dot{p}}_{0} = v_{0} + ω_{0} \times (p_{e} - r_{0}) + \sum_{k = 1}^{n} [k_{k} \times (p_{e} - p_{k})] {\dot{θ}}_{k}

(2)

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

ω_{e} = ω_{0} + \sum_{k = 1}^{n} k_{k} {\dot{θ}}_{k}

(3)

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

[\begin{matrix} v_{e} \\ ω_{e} \end{matrix}] = J_{b} [\begin{matrix} v_{0} \\ ω_{0} \end{matrix}] + J_{m} \dot{Θ}

(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

H_{b} {\dot{x}}_{b} + H_{b m} \dot{Θ} = 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{x}}_{b}$ can be solved as

{\dot{x}}_{b} = - {H_{b}}^{- 1} H_{b m} \dot{Θ} = J_{b m} \dot{Θ} \Leftrightarrow [\begin{matrix} v_{0} \\ ω_{0} \end{matrix}] = [\begin{matrix} J_{b m_v} \\ J_{b m_ω} \end{matrix}] \dot{Θ}

(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{x}}_{e} = [\begin{matrix} v_{e} \\ ω_{e} \end{matrix}] = J_{g} (Ψ_{0}, Θ, m_{i}, I_{i}) \dot{Θ} = [\begin{matrix} J_{g_v} \\ J_{g_ω} \end{matrix}] \dot{Θ}

(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{Θ} = {J_{g}}^{- 1} {\dot{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{Ψ}$ and ω is

ω = [\begin{matrix} 0 & - s_{α} & c_{α} c_{β} \\ 0 & c_{α} & s_{α} c_{β} \\ 1 & 0 & - s_{β} \end{matrix}] \dot{Ψ} = J_{E u l e r} \dot{Ψ}

(8)

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

\dot{Ψ} = (J_{E u l e r})^{- 1} ∙ ω

(9)

However, when β = ±π/2, J _Euler is singular, i.e. infinite rotational rates ( $\dot{α}, \dot{β}, \dot{γ}$ ) 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{aligned} Q & = η + q_{1} \vec{i} + q_{2} \vec{j} + q_{3} \vec{k} = η + q {[η q_{1} q_{2} q_{3}]}^{T} \\ = \cos \frac{ψ}{2} + r ∙ s i n \frac{ψ}{2} \end{aligned}

(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

η^{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

[\begin{matrix} \dot{η} \\ \dot{q} \end{matrix}] = \frac{1}{2} [\begin{matrix} - q^{T} \\ η I - \tilde{q} \end{matrix}] ω

(12)

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

\tilde{q} = [\begin{matrix} 0 & - q_{3} & q_{2} \\ q_{3} & 0 & - q_{1} \\ - q_{2} & q_{1} & 0 \end{matrix}]

(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{cases} δ η = η_{1} η_{2} + {q_{1}}^{T} q_{2} \\ δ q = η_{1} q_{2} - η_{2} q_{1} - {\tilde{q}}_{1} q_{2} \end{cases}

(14)

When the two frames coincide,

{\begin{cases} δ η = 1 \\ δ q = 0 \end{cases}

(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 ₁	B ₂	B ₃	B ₄	B ₅	B ₆
Mass/kg		400	6	5	5	4	3	2
ⁱ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
ⁱ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/kg.m²	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:

X_{e} = [\begin{matrix} Q_{e} \\ p_{e} \end{matrix}] = {[η_{e}, q_{e 1}, q_{e 2}, q_{e 3}, p_{e x}, p_{e y}, p_{e y}, p_{e z}]}^{T} = X_{e} (Q_{0}, Θ)

(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{aligned} T o d e t e r m i n e : θ_{i} (t) f_{i} (t), i = 1, 2, \dots, n \\ T o m a k e : X_{e} (t_{0}) = X_{e 0}, X_{e} (t_{f}) = X_{e d} \end{aligned}

subject to the equality constraints:

{\begin{cases} θ_{i} (0) = θ_{i 0}, {\dot{θ}}_{i} (0) = 0, {\ddot{θ}}_{i} (0) = 0, \\ {\dot{θ}}_{i} (t_{f}) = {\ddot{θ}}_{i} (t)_{f} = 0 \end{cases}

(17)

Q_{0} (0) = Q_{00}

(18)

and the inequality constraints:

{\begin{cases} θ_{i_m i n} \leq θ_{i} (t) \leq θ_{i_m a x}, {\dot{θ}}_{i_m i n} \leq {\dot{θ}}_{i} (t) \leq {\dot{θ}}_{i_m a x} \\ {\ddot{θ}}_{i_m i n} \leq {\ddot{θ}}_{i} (t) \leq {\ddot{θ}}_{i_m a x} \end{cases}

(19)

where, $[θ_{i_m i n}, θ_{i_m a x}], [{\dot{θ}}_{i_m i n}, {\dot{θ}}_{i_m a x}]$ and $[{\ddot{θ}}_{i_m i n}, {\ddot{θ}}_{i_m a x}]$ 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.

∥ X_{e d} - X_{e f} ∥ \to 0

(20)

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

{\dot{Q}}_{0} = [\begin{matrix} {\dot{η}}_{0} \\ {\dot{q}}_{0} \end{matrix}] = \frac{1}{2} [\begin{matrix} - {q_{0}}^{T} \\ η_{0} I - {\tilde{q}}_{0} \end{matrix}] J_{b m_ω} \dot{Θ}

(21)

{\dot{Q}}_{e} = [\begin{matrix} {\dot{η}}_{e} \\ {\dot{q}}_{e} \end{matrix}] = \frac{1}{2} [\begin{matrix} - {q_{e}}^{T} \\ η_{e} I - {\tilde{q}}_{e} \end{matrix}] J_{g_ω} \dot{Θ}

(22)

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

Q_{0} (t) = \int_{0}^{t} \frac{1}{2} [\begin{matrix} - {q_{0}}^{T} \\ η_{0} I - {\tilde{q}}_{0} \end{matrix}] J_{b m_ω} \dot{Θ} d t

(23)

Q_{e} (t) = \int_{0}^{t} \frac{1}{2} [\begin{matrix} - {q_{e}}^{T} \\ η_{e} I - {\tilde{q}}_{e} \end{matrix}] J_{g_ω} \dot{Θ} d t

(24)

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

δ q_{e} = η_{e f} q_{e d} - η_{e d} q_{e f} - {\tilde{q}}_{e f} q_{e d}

(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)):

P_{e} (t) = \int_{0}^{t} {\dot{v}}_{e} d t = \int_{0}^{t} J_{g_v} \dot{Θ} d t

(26)

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

δ P_{e} = P_{e d} - P_{e f} = P_{e d} - \int_{0}^{t} J_{g_v} \dot{Θ} d t

(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:

θ_{i} (t) = \sum_{p = 0}^{5} a_{i p} t^{p}, (i = 1, \dots, 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{cases} a_{i 0} = θ_{i 0}, \\ a_{i 1} = a_{i 2} = 0 \\ a_{i 3} = \frac{5}{3} a_{i 5} t_{f}^{2}, \\ a_{i 4} = - \frac{5}{2} a_{i 5} t_{f} \end{cases}

(29)

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

θ_{i} = a_{i 5} (t^{5} - \frac{5}{2} t_{f} t^{4} + \frac{5}{3} t_{f}^{2} t^{3}) + θ_{i 0}

(30)

{\dot{θ}}_{i} = \frac{d θ_{i}}{d t} = a_{i 5} (5 t^{4} - 10 t_{f} t^{3} + 5 t_{f}^{2} t^{2})

(31)

{\ddot{θ}}_{i} = \frac{d^{2} θ_{i}}{d t^{2}} = a_{i 5} (20 t^{3} - 30 t_{f} t^{2} + 10 t_{f}^{2} t)

(32)

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

a = {[a_{15}, a_{25}, \dots, a_{n 5}]}^{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):

θ_{i} = {\hat{a}}_{i 5} (τ^{5} - \frac{5}{2} τ^{4} + \frac{5}{3} τ^{3}) + θ_{i 0}

(34)

{\dot{\hat{θ}}}_{i} = \frac{d θ_{i}}{d τ} = {\hat{a}}_{i 5} (5 τ^{4} - 10 τ^{3} + 5 τ^{2}) = t_{f} {\dot{θ}}_{i}

(35)

{\ddot{\hat{θ}}}_{i} = \frac{d^{2} θ_{i}}{d τ^{2}} = {\hat{a}}_{i 5} (20 τ^{3} - 30 τ^{2} + 10 τ) = t_{f}^{2} {\dot{θ}}_{i}

(36)

where,

{\hat{a}}_{i 5} = a_{i 5} t_{f}^{5}

(37)

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

Q_{0} (τ) = \int_{0}^{τ} \frac{1}{2} [\begin{matrix} - {q_{0}}^{T} \\ η_{0} I - {\tilde{q}}_{0} \end{matrix}] J_{b m_ω} \dot{\hat{Θ}} d τ

(38)

P_{e} (τ) = \int_{0}^{τ} J_{g_v} \dot{\hat{Θ}} d τ

(39)

Q_{e} (τ) = \int_{0}^{τ} \frac{1}{2} [\begin{matrix} - {q_{e}}^{T} \\ η_{e} I - {\tilde{q}}_{e} \end{matrix}] J_{g_ω} \dot{\hat{Θ}} d τ

(40)

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

G (\hat{a}) = [\begin{matrix} δ q_{e} \\ δ P_{e} \end{matrix}] \in 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{a}) = \frac{∥ δ q_{e} ∥}{k_{q}} + \frac{∥ δ p_{e} ∥}{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{a}}^{*}$ . When $f ({\hat{a}}^{*}) \leq 1$ , the problem is solved, and ${\hat{a}}^{*}$ 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

X_{i} = (x_{i 1}, x_{i 2}, \dots, x_{i n})

(43)

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

P_{i} = (p_{i 1}, p_{i 2}, \dots, p_{i n})

(44)

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

P_{g} = (p_{g 1}, p_{g 2}, \dots, p_{g n})

(45)

The velocity of particle i is

V_{i} = (v_{i 1}, v_{i 2}, \dots, v_{i n})

(46)

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

\begin{aligned} v_{i d} & = w ∙ v_{i d} + c_{1} ∙ r a n d () ∙ (p_{i d} - x_{i d}) \\ + c_{2} ∙ R a n d () ∙ (p_{g d} - x_{i d}) \end{aligned}

(47)

x_{i d} = x_{i d} + v_{i d}

(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

X_{i} = {\hat{a}}^{i} = ({\hat{a}}_{15}^{i}, {\hat{a}}_{25}^{i}, \dots, {\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{a}}^{1} (k), {\hat{a}}^{2} (k), \dots, {\hat{a}}^{n_{p}} (k)}, 1 \leq k \leq N_{m a x}

(50)

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

F_{i} (k) = f ({\hat{a}}^{i} (k)), i = 1, 2, \dots, 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_b e s t} = {\begin{cases} F_{i} (k), if k = 1 \\ F_{i} (k), else if F_{i} (k) ≺ F_{i_b e s t} \\ F_{i_b e s t}, else \end{cases}

(52)

P_{i} = {\begin{cases} {\hat{a}}^{i} (k), if k = 1 \\ {\hat{a}}^{i} (k), else if F_{i} (k) ≺ F_{i_b e s t} \\ P_{i}, else \end{cases}

(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_b e s t} = {\begin{cases} min_{1 \leq i \leq n_{p}} (F_{i} (k)), if k = 1 \\ min_{1 \leq i \leq n_{p}} (F_{i} (k)), else if min_{1 \leq i \leq n_{p}} (F_{i} (k)) ≺ F_{g_b e s t} \\ F_{g_b e s t}, else \end{cases}

(54)

P_{g} = {\begin{cases} {\hat{a}}^{i_m i n} (k), if k = 1 \\ {\hat{a}}^{i_m i n} (k), else if \underset{1 \leq i \leq n_{p}}{m i n} (F_{i} (k)) ≺ F_{g_b e s t} \\ P_{g}, else \end{cases}

(55)

where, ${\hat{a}}^{i_m i n} (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{θ}}_{i} | \leq {\dot{θ}}_{i_m a x} = 5^{\circ} / s = 0.0873 r a d / s

(56)

| {\ddot{θ}}_{i} | \leq {\ddot{θ}}_{i_m a x} = {0.5}^{\circ} / s^{2} = 0.0873 r a d / s^{2}

(57)

The initial and desired poses of the end-effector are

X_{e 0} = [\begin{array}{l} [0.0000, 0.9848, 0.0000, 0.1736]^{T} \\ [1.4644, - 0.0137, 0.4602]^{T} \end{array}]

(58)

X_{e d} = [\begin{array}{l} [- 0.0085, 0.9988, 0.0096, 0.0472]^{T} \\ [1.3501, - 0.0137, 0.1401]^{T} \end{array}]

(59)

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

Θ_{0} = {[0, {47.72}^{\circ}, - {93.91}^{\circ}, 0, - {23.82}^{\circ}, {0.00}^{\circ}]}^{T}

(60)

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} = ∥ r ∙ s i n \frac{ψ}{2} ∥ = ∥ r ∥ ∙ s i n \frac{ψ}{2} = s i n (\frac{1}{2} \times \frac{π}{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}}_{i 5} \in [- 5, 5]

(64)

The parameters for the algorithms are:

V_{m a x} = 2

(65)

n_{p} = 24, N_{m a x} = 1000

(66)

c_{1} = c_{2} = 2

(67)

w_{m a x} = 0.9, w_{m i n} = 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{cases} {\hat{a}}^{*} = [- 0.0438 - 3.4567 1.2781 - 0.1517 - 0.3410 - 0.2711] \\ f ({\hat{a}}^{*}) = 0.0859 \end{cases}

(69)

Fig. 6.

The Variation of the Global Best Fitness Evaluation

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

t_{f} \geq | {\dot{\hat{θ}}}_{i m a x} | / {\dot{θ}}_{i m a x} = 12.3784

(70)

t_{f} \geq \sqrt{{\ddot{\hat{θ}}}_{i m a x} / {\ddot{θ}}_{i m a x}} = 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.

Footnotes

7.

We would like to thank Dr. P. F. Huang and Dr. L. T. Li for their valuable suggestion on the paper. This work is supported by National Nature Science Foundation of China (No.60775049).

References

Angeles

(2006). Fundamentals of Robotic Mechanical Systems: Theory, Methods, and Algorithms, Springer-Verlag, New York, USA.

Jack

C. K. C.

(1992). Quaternion Kinematic and Dynamic Differential Equations, IEEE Transactions On Robotics and Automatic, Vol.8, No.1, pp. 53–64.

Eberhart

R. C.

and Kennedy

(1995). A new optimizer using particle swarm theory, in Proc. the sixth international symposium on micro machine and human science, IEEE service center, Piscataway, NJ, Nagoya, Japan, pp. 39–43.

Huang

P. F.

Y. S.

and Liang

(2006). Tracking Trajectory Planning of Space Manipulator for Capturing Operation, International Journal of Advanced Robotic Systems, Vol.3, No.3, pp. 211–218.

Huang

P. F.

and Xu

Y. S.

(2006). PSO-Based Time-Optimal Trajectory Planning for Space Robot with Dynamic Constraints, in Proc. IEEE International Conference on Robotics and Biomimetics, Kunming, China, pp. 1402–1407.

Huang

P. F.

Y. S.

and Liang

(2006). Minimum-torque Path Planning of Space Robots using Genetic Algorithms, International Journal of Robotics and Automation, Vol.21, No.3.

Kennedy

and Eberhart

R. C.

(1995). Particle Swam Optimization, in Proc. IEEE International Conference on Neural Networks, Path, Australia, IEEE Service Center, Piscataway, NJ, N, pp. 1942–1948.

Lampariello

and Deutrich

(1999). Simplified Path Planning for Free-Floating Robots, Internal Report DLR 515-99-04.

Moosavian

S. A.

and Papadopoulos

(1997). Control of Space Free-Flyers Using Modified Transpose Jacobian Algorithm, in Proc. IEEE/RSJ International Conference on Intelligent Robots and Systems, Grenoble, France, pp. 1500–1505.

10.

Moosavian

S. A.

and Papadopoulos

(2007). Modified Transpose Jacobian control of Robotic systems, Automatica, Vol.43, No.7, pp. 1226–1233.

11.

Moosavian

S. A.

and Papadopoulos

(2007). Free-flying robots in space: an overview of dynamics modeling, planning and control, Robotica (to appear).

12.

Nakamura

and Mukherjee

(1991). Nonholonomic path planning of space robots via a bidirectional approach, IEEE Transactions on Robotics and Automation, Vol.7, No.4, pp. 500–514.

13.

Pandey

and Agrawal

(1997). Path Planning of Free-Floating Prismatic-Jointed Manipulators, Multibody System Dynamics, Vol.1, pp. 127–140.

14.

Papadopoulos

and Dubowsky

(1993). Dynamic Singularities in the Control of Free-Floating Space Manipulators, ASME Journal of Dynamic Systems, Measurement and Control, Vol.115, No.1, pp. 44–52.

15.

Papadopoulos

Tortopidis

and Nanos

(2005). Smooth Planning for Free-floating Space Robots Using Polynomials, in Proc. IEEE International Conference on Robotics and Automation, Barcelona, Spain, pp. 4272–4277.

16.

Papadopoulos

(1992). Path Planning for Space Manipulators Exhibiting Nonholonomic Behavior, in Proc. the International Conference on Intelligent Robots and Systems, Raleigh, North Carolina, pp. 669–675.

17.

Shi

and Eberhart

R. C.

(1998). A modified particle swarm optimizer, in Proc. IEEE International Conference on Evolutionary Computation, IEEE Press, Piscataway, NJ, pp. 69–73.

18.

Takahashi

Yamakita

and Hyon

(2006). An Optimization Approach for Underactuated Running Robot, in Proc. SICE-ICASE International Joint Conference, Bexco, Busan, Korea, pp. 3505–3510.

19.

Tortopidis

and Papadopoulos

(2007). On point-to-point motion planning for underactuated space manipulator systems, Robotics and Autonomous Systems, Vol.55, No.2, pp. 122–13.

20.

Traira

Sagara

and Katoh

(2000). Digital adaptive control of space robot manipulators using transpose of generalized Jacobian matrix, in Proc. the IEEE/RSJ International Conference on Intelligent Robots and Systems, pp.1553–1558.

21.

Umetani

and Yoshida

(1989). Resolved Motion Rate Control of Space Manipulators with Generalized Jacobian Matrix, IEEE Transactions on Robotics and Automation, Vol.5, No.3, pp. 303–314.

22.

W. F.

Liu

Liang

Y. S.

Qiang

W. Y.

(2008). Non-holonomic Path Planning of a Free-Floating Space Robotic System Using Genetic Algorithms, RSJ Advanced Robotics, Vol. 22, pp.451–476.

23.

W. F.

Liang

Y. S.

and Qiang

W. Y.

(2007). Path Planning of Free-Floating Robot in Cartesin Space Using Direct Kinematics, International Journal of Advanced Robotic System, Vol.4, No.1, pp. 17–26.

24.

Y. S.

and Kanade

(1992). Space Robotics: Dynamics and Control, Kluwer Aca-demic Publishers.

25.

Yoshida

(2003). Engineering Test Satellite VII flight experiments for space robot dynamics and control: Theories on laboratory test beds ten years ago, now in orbit, International Journal of Robotics Research, Vol.22, No.5, pp. 321–335.

26.

Yuan

J. S. C.

(1988). Closed loop manipulator control using quaternion feedback, IEEE Journal of Robotics and Automation, Vol.4, No.4, pp. 434–440.