Path Planning of Free-Floating Robot in Cartesian Space Using Direct Kinematics

Abstract

Dynamic singularities make it difficult to plan the Cartesian path of free-floating robot. In order to avoid its effect, the direct kinematic equations are used for path planning in the paper. Here, the joint position, rate and acceleration are bounded. Firstly, the joint trajectories are parameterized by polynomial or sinusoidal functions. And the two parametric functions are compared in details. It is the first contribution of the paper that polynomial functions can be used when the joint angles are limited(In the similar work of other researchers, only sinusoidla functions could be used). Secondly, the joint functions are normalized and the system of equations about the parameters is established by integrating the differential kinematics equations. Normalization is another contribution of the paper. After normalization, the boundary of the parameters is determined beforehand, and the general criterion to assign the initial guess of the unknown parameters is supplied. The criterion is independent on the planning conditions such as the total time t_f. Finally, the parametes are solved by the iterative Newtonian method. Modification of t_f may not result in the recalculation of the parameters. Simulation results verify the path planning 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 the robotic manipulators pose additional problems beyond those on earth, due to the dynamic coupling between space manipulators and their spacecraft. Free-floating systems exhibit nonholonomic 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.

There are many literatures dealing with the problem of the path planning for free-floating space robot. Z. Vafa and S. Dubowsky (Vafa, Z. & Dubowsky, S. 1990) proposed cyclic motion of the manipulator joints to change the vehicle orientation. However, this scheme has to assume small cyclic motion to neglect the nonlinearity of order greater than two, and it requires many cycles to make even a small change of vehicle orientation. Y. Nakamura and R. Mukherjee (Nakamura, Y. & Mukherjee, R. 1991) discussed the nonholonomic characteristics of free-floating space manipulators, and employed “bidirectional approach” for path generation aiming at controlling both the manipulator configuration and the spacecraft orientation. Although of much success, the control law failed to achieve some desired orientations and smooth feedbacks didn't exist. T. Mineta et al. planned the trajectory of a space manipulator using a neural network (Mineta, T., J. Qiu and Tani, J. 1997). K. Yamada (Yamada, K. 1993) used the variational method to find a closed trajectory of manipulator joints that generates an arbitrary change of satellite attitude. T. Suzuki and Y. Nakamura (Suzuki T. and Nakamura Y. 1996) extended Yamada's method to obtained the optimal spiral motion, which is considered an approximation of an arbitrary given 9-dimensional trajectory of the end-effector pose and the satellite orientation. The “spiral motion” planning method is a good attempt, but its convergence is affected by the singular point. C. Fernandes et al (Fernandes, C., Gumits, L., and Li, Z.X. 1994) developed a near-optimal path planning approach for controlling the spacecraft attitude and manipulator posture at the same time using internal motion. The approach didn't assume any specific structure of a system and can be automated computationally, but it needs symbolic manipulation software to obtain the Jacobians required by the algorithm. M. Torres and S. Dubowskys (Dubowsky S., Torres M. 1991) presented a technique called the Enhanced Disturbance Map (EDM) to plan space manipulator motions that will result in relatively low spacecraft disturbances. But the EDM is not easy to attain, especially for the space robotic system of higher degree of freedom. K. Yoshida et al. (Yoshida, K., Hashizume, K. and Abiko, S. 2001) addressed the Zero Reaction Maneuver (ZRM) concept, but the existence of ZRM is very limited for a 6 DOF manipulator. K. Yoshida et al. (Yoshida, K., Kurazume, R. and Umetani, Y. 1991) proposed that one of the manipulators of a free-flying system should move to compensate for the disturbance created by the other. It is a useful method but will augment the cost of the project and the complexity of the cpntrol algorithm.

Path planning of free-floating robot in Cartesian space belongs to the problem of “Inertially-Referenced End-Point Motion Control”(Papadopoulos, E. and Dubowsky, S. 1991). In the case of inertial manipulation, such as capturing a free object in space, some control methods based on Generalized Jacobian Matrix (GJM) can be used. These methods include resolved rate, resolved acceleration or transposed Jacobian control technology. But the algorithms that use a Jacobian inverse, such as the resolved rate or resolved acceleration control algorithms, fail at dynamic singularities. Ones that use a pseudo inverse Jacobian or a Jacobian transpose will likely follow the available direction, but they may result in large errors.

Dynamic singularities are path dependent (Papadopoulos, E. and Dubowsky, S. 1993). Based on the calculation of PIW (Path Independent Workspaces) and PDW (Path Dependent Workspace), E. Papadopoulos (Papadopoulos, E. 1992) proposed a planning algorithm that avoids dynamic singularities, and it permits the manipulator's end-effector to move from any reachable workspace location to any other. But Papadopoulos only took a planar 2-link robot as the example. In fact, the calculation of PIW and PDW of a space manipulator of higher degree of freedom is very complicated. S. Pandey (Pandey, S. and Agrawal, S. 1997) proposed a method called “mode summation” to plan the Cartesian path of a free-floating system with prismatic joints. R. Lampariello et al. (Lampariello, R. and Deutrich, K. 1999) applied the method to a system with rotational joints. But they used sinusoidal functions to satisfy the limits of the joint variables, since they thaught polynomial functions could not attain the aim. The method to solve the problem of path planning, which is an iterative method, requires to assign the initial values of the unknown parameters. R. Lampariello et al. developed a criterion to do so. The criterion depends on certain condition, such as the planning time t_f. Therefore, it needs recalculating if the condition changes. Moreover, they didn't propose a general method to take the limit of joint rate and acceleration in the planning problem. In this paper, the method is improved to tackle these problems. Polynomial and sinusoidal functions can be used in the case that the joint position, rate and accelaration are bounded. Furthermore, the boundary of the parameters can be determined beforehand and a general criteria is proposed to assign the initial values of the parameters. Modification of t_f can't result in the recalculation of the parameters.

This paper is organized as follows: Section two derives the kinematic equations of space robotic system. Then, the problem of Cartisian path planning is discussed in Section three. In Section four, the joint functions are parameterized and normalized 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. Modelling of Free-Floating Robotic System

Major research topics on space robot dynamic and control were collected by Xu and Kanade(Xu, Y. S. and Kanade, T. 1992). 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.

Fig. 1.

General Model of a Single Arm Space Robot

Some symbols and variables are defined as follows:

Σ _I : the inertia frame;

Σ _E : the frame of the end-effector;

Σ _i : the frame of B_i;

C_i(i = 0, …, n): the CM (center of mass) of B_i

S_i, b_i ∈R³: the position vectors from J_i to C_i and C_i to J_i+1 respectively;

ψ _b , ψ _e ∈ R³: the attitude angle of the base and the end-effector respectively, expressed in terms of x-y-z Euler angles, i.e. ${\mathit{\psi }}_b=[{\alpha }_b,{\beta }_b,{\gamma }_b{]}^T,{\mathit{\psi }}_e=[{\alpha }_e,{\beta }_e,{\gamma }_e{]}^T$ .

r_i ∈ R³: the position vectors of C_i;

p_e ∈ R³: the position vectors of the end-effector;

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

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

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

The direct kinematics of space robot system is in the form as follows(Umetani, Y. and Yoshida, K. 1989):

$${\mathit{X}}_{\mathit{e}}=\left[\begin{array}{c}{\mathit{p}}_e\\ {\mathbf{\Psi }}_e\end{array}\right]=[x_e,y_e,z_e,{\alpha }_e,{\beta }_e,{\gamma }_e{]}^T=\mathit{f}({\mathbf{\Psi }}_b,\mathbf{\Theta }\mathbf{)}$$

(1)

where, X_e is the pose (position and orientation) of the end-effector. For an n joint manipulator, the system of equations (1) has n equations but n+3 unknowns, so it can not be solved. It is easy to deduce from this result that the path planning method which makes use of the non-differential form of the kinematical equations of the system is not suitable for free-floating systems.

The differential form of the kinematical equations of space robot system is:

$$\left[\begin{array}{c}{\mathit{v}}_e\\ {\mathit{\omega }}_e\end{array}\right]={\mathit{J}}_{\mathit{s}}({\mathbf{\Psi }}_b,\mathbf{\Theta })\left[\begin{array}{c}{\mathit{v}}_b\\ {\mathit{\omega }}_b\end{array}\right]+{\mathit{J}}_{\mathit{m}}({\mathbf{\Psi }}_b,\mathbf{\Theta })\dot{\mathbf{\Theta }}$$

(2)

where ${\mathit{J}}_{\mathit{s}}(\mathit{\psi }b,\mathbf{\Theta })\in {\mathit{R}}^{6\times 6}$ and ${\mathit{J}}_{\mathit{m}}(\mathit{\psi }b,\mathbf{\Theta })\in {\mathit{R}}^{6\times \mathrm{n}}$ are Jacobian matrices for a satellite dependent part and a manipulator dependant part respectively. Since there are no external forces and torques acted on the free-floating system, with the assumption made of linear and angular momentum initially zero, the system momentum keeps zero according to the conservation law, i.e.:

$${\mathit{H}}_{\mathit{b}\mathit{b}}\left[\begin{array}{c}{\mathit{v}}_b\\ {\mathit{\omega }}_b\end{array}\right]+{\mathit{H}}_{\mathit{b}\mathit{m}}\dot{\mathbf{\Theta }}=0$$

(3)

where H_bb∈R^6×6 is the inertia matrix of the base and H_bm∈R^6×n is the coupling inertia matrix. From equation (3), v_b and ωb are solved as:

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

(4)

where, J_vb and J_wb are the Jacobian matrices relating $\dot{\mathbf{\Theta }}$ to v_b and to w_b respectively. Substituting (4) to (2), the equation is yielded:

$$\left[\begin{array}{c}{\mathit{v}}_e\\ {\mathit{\omega }}_e\end{array}\right]=[{\mathit{J}}_{\mathit{m}}-{\mathit{J}}_{\mathit{s}}{\mathit{H}}_{bb}^{-1}{\mathit{H}}_{\mathit{b}\mathit{m}}]\dot{\mathbf{\Theta }}={\mathit{J}}^{\ast }({\mathbf{\Psi }}_b,\mathbf{\Theta })\dot{\mathbf{\Theta }}$$

(5)

The matrix ${\mathit{J}}^{\ast }({\mathit{\psi }}_b,\mathbf{\Theta })\in {\mathit{R}}^{6\times 6}$ is termed Generalized Jacobian Matrix (GJM), which represents the reaction dynamics of free-floating system.

When the orientation is expressed in x-y-z Euler angles, the relationship between the resolute angle velocity and the Euler angle velocity is given by:

$${}^{\mathit{I}}\mathit{\omega }=\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]=\left[\begin{array}{ccc}1& 0& s_{\beta }\\ 0& c_{\alpha }& -s_{\alpha }{c}_{\beta }\\ 0& s_{\alpha }& c_{\alpha }{c}_{\beta }\end{array}\right]\left[\begin{array}{c}\dot{\alpha }\\ \dot{\beta }\\ \dot{\gamma }\end{array}\right]=\mathit{N}\dot{\mathbf{\Psi }}$$

(6)

where, $c_{\alpha }=cos(\alpha ),c_{\beta }=cos(\beta ),s_{\alpha }=sin(\alpha ),s_{\beta }=sin(\beta )$ . N is the transformation matrix mapping the Euler angle velocity into resolute angle velocity. N_b and N_e are the transformation matrices for the base and the end-effector respectively. That is to say

$${\dot{\mathbf{\Psi }}}_b{\mathit{N}}_b^{-1}{\mathit{\omega }}_b={\mathit{N}}_b^{-1}{\mathit{J}}_{\omega b}\dot{\mathbf{\Theta }}$$

(7)

$$\dot{{\mathbf{\Psi }}_e}={\mathit{N}}_e^{-1}{\mathit{\omega }}_{\mathit{e}}$$

(8)

Therefore, the differential kinematical equation of the free-floating system becomes:

$${\dot{\mathit{X}}}_{\mathit{e}}=\left[\begin{array}{c}{\mathit{v}}_{\mathit{e}}\\ \dot{{\mathbf{\Psi }}_{\mathit{e}}}\end{array}\right]=\left[\begin{array}{cc}{\mathit{I}}_{\mathbf{3}}& {\mathit{O}}_{\mathbf{3}}\\ {\mathit{O}}_{\mathbf{3}}& {\mathit{N}}_{\mathit{e}}^{-\mathbf{1}}\end{array}\right]\left[\begin{array}{c}{\mathit{v}}_{\mathit{e}}\\ {\mathit{\omega }}_{\mathit{e}}\end{array}\right]={\mathit{K}}_e{\mathit{J}}^{\ast }({\mathbf{\Psi }}_0,\mathbf{\Theta })\dot{\mathbf{\Theta }}$$

(9)

where, I₃ is a 3×3 identity matrix and O₃ is a 3×3 zero matrix. K_e is the composite matrix, i. e.

$${\mathit{K}}_e=\left[\begin{array}{cc}{\mathit{I}}_{\mathbf{3}}& {\mathit{O}}_{\mathbf{3}}\\ {\mathit{O}}_{\mathbf{3}}& {\mathit{N}}_{\mathit{e}}^{-\mathbf{1}}\end{array}\right]$$

(10)

3. Problem of the Path Planning

3.1 Description of the Problem

Point-to-point path planning in Cartesian space for free-floating space robot is studied in the paper, i. e., joint path is planned to make the end-effeor attain the desired pose X_ed. The following equations are obtained by integrating the equation (7) and (9):

$${\mathbf{\Psi }}_b(t)={\int }_0^t{\mathit{N}}_b^{-1}{\mathit{J}}_{\omega b}\dot{\mathbf{\Theta }}dt$$

(11)

$${\mathit{X}}_{\mathit{e}}(t)={\int }_0^t{\dot{\mathit{X}}}_{\mathit{e}}dt={\int }_0^t{\mathit{K}}_{\mathit{e}}{\mathit{J}}^{\ast }({\mathbf{\Psi }}_b,\mathbf{\Theta })\dot{\mathbf{\Theta }}dt$$

(12)

The integral can not be gotten analytically, but it can be calculated numerically. Then the problem of the path planning is expressed as follows:

$$\begin{array}{ll}\text{To determine}:& {\theta }_i(t)=f_i(t),i=1,2,\dots ,n\\ \text{To make}:& {\mathit{X}}_e(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}$$

(13)

and the inequality constraints:

$${\theta }_{i\mathrm{\_}min}\le {\theta }_i(t)\le {\theta }_{i\mathrm{\_}max}$$

(14)

$${\dot{\theta }}_{i\mathrm{\_}min}\le {\dot{\theta }}_i(t)\le {\dot{\theta }}_{i\mathrm{\_}max}$$

(15)

$${\ddot{\theta }}_{i\mathrm{\_}min}\le {\ddot{\theta }}_i(t)\le {\ddot{\theta }}_{i\mathrm{\_}max}$$

(16)

In order to solve the problem of the path planning, the joint functions are parameterized here. There are two methods for the parameterization: one is polynomial parameterization, the other is sinusoidal parameterization. Each has its own advantages, which will be discussed.

3.2 Parameterization Using Polynomial Functions

S. Pandey et al. used polynomial functions to parameterize the manipulator joints when there are no limits on the manipulator joints(Pandey, S. and Agrawal, S. 1997). The degree of the function must satisfy the constraints on the position, velocity and acceleration of the joints. According to the condition of (13), a polynomial functions of fifth degree can be used for the parameterization of i^th joint, i. e.

$$\begin{array}{l}{\theta }_i(t)=a_{i5}{t}^5+a_{i4}{t}^4+a_{i3}{t}^3+a_{i2}{t}^2+a_{i1}{t}^2+a_{i1}t+a_{i0}\\ i=1,2,\cdots ,6\end{array}$$

(17)

where, 0<t<t_f, and a_i0 ˜ a_i5 are the coefficients of the polynomial. Applying the equality constraints of Equation (13) to Equation (17), the following results are found:

$$a_{i0}={\theta }_{i0}$$

(18)

$$a_{i1}=a_{i2}=0$$

(19)

$$a_{i3}=\frac{5}{3}{a}_{i5}{t}_t^2$$

(20)

$$a_{i4}=\frac{5}{2}{a}_{i5}{t}_t$$

(21)

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}_t^2{t}^3)+{\theta }_{i0}$$

(22)

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

(23)

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

(24)

Therefore, there is only a parameter a _i5 in each joint function after parameterization. If n=6, let a=[a₁₅,a₂₅,…,a₆₅]. Then the difference between X_ed and X_ef is the function of a too, i.e.

$$\mathit{F}(\mathit{a})={\mathit{X}}_{\mathit{e}\mathit{d}}-{\mathit{X}}_{\mathit{e}f}={\mathit{X}}_{\mathit{e}\mathit{d}}-{\int }_0^{t_f}{\mathit{K}}_e{\mathit{J}}^{\ast }({\mathbf{\Psi }}_0,\mathbf{\Theta })dt$$

(25)

The path planning problem is reduced to the resolution of the system of nonlinear equations as follows:

$$\mathit{F}(\mathit{a})=0$$

(26)

when the parameter a is solved, the joint path is determined.

3.3 Parameterization Using Sinusoidal Functions

The joint positions can be limited between a maximum and a minimum value, for reasons of mechanical or operational nature. That is to say, the joint functions must satisfy the constraints of (14). Since sinusoidal functions are bounded, they are used here to parameterize the joint angle functions, which is similar with the methods of S. Pandey's and R. Lampariello's(Pandey, S. and Agrawal, S. 1997; Lampariello, R. and Deutrich, K. 1999). The arguments of the sinusoidal functions are polynomial functions. The parameterized joint functions are

$$\begin{array}{c}{\theta }_i={\mathrm{\Delta }}_{i1}sin(a_{i5}{t}^5+a_{i4}{t}^4+a_{i3}{t}^3+a_{i2}{t}^2+a_{i1}t+a_{i0})+{\mathrm{\Delta }}_{i2}\\ i=1,2,\dots ,6\end{array}$$

(27)

where, 0≤t≤t_f and a_i0 ˜ a_i5 are the coefficients of the polynomial. Δ _i1 and Δ _i2 are defined as

$${\mathrm{\Delta }}_{i1}=({\theta }_{imax}-{\theta }_{imin})/2,{\mathrm{\Delta }}_{i2}=({\theta }_{imax}+{\theta }_{imin})/2$$

(28)

where, Θ _imin and Θ _imax are the minimum and maximum of i^th joint. Applying (13) to (27), the coefficents can be solved:

$$a_{i0}=si{n}^{-1}\left(\frac{{\theta }_{i0}-{\mathrm{\Delta }}_{i2}}{{\mathrm{\Delta }}_{i1}}\right)$$

(29)

$$a_{i1}=a_{i2}=0$$

(30)

$$a_{i3}=\frac{5}{3}{a}_{i5}{t}_f^2$$

(31)

$$a_{i4}=-\frac{5}{2}{a}_{i5}{t}_f$$

(32)

Therefore, the joint position, velocity and acceleration functions are shown as the following equations:

$${\theta }_i={\mathrm{\Delta }}_{i1}sin(a_{i5}(t^5-\frac{5}{2}{t}_f{t}^4+\frac{5}{3}{t}_f^2{t}^3)+si{n}^{-1}\left(\frac{{\theta }_{i0}-{\mathrm{\Delta }}_{i2}}{{\mathrm{\Delta }}_{i1}}\right))+{\mathrm{\Delta }}_{i2}$$

(33)

$${\dot{\theta }}_i=\frac{d{\theta }_i}{dt}={\mathrm{\Delta }}_{i1}cos\left[\begin{array}{l}{a}_{i5}(t^5-\frac{5}{2}{t}_f{t}^4+\frac{5}{3}{t}_f^2{t}^3)\\ +si{n}^{-1}\left(\frac{{\theta }_{i0}-{\mathrm{\Delta }}_{i2}}{{\mathrm{\Delta }}_{i1}}\right)\end{array}\right]$$

(34)

$$\begin{array}{rl}& [a_{i5}(5t^4-10t_f{t}^3+5t_f^2{t}^2)]\\ & {\ddot{\theta }}_i=\frac{d^2{\theta }_i}{d{t}^2}=-{\mathrm{\Delta }}_{i1}sin\left[\begin{array}{l}{a}_{i5}(t^5-\frac{5}{2}{t}_f{t}^4+\frac{5}{3}{t}_f^2{t}^3)\\ +si{n}^{-1}\left(\frac{{\theta }_{i0}-{\mathrm{\Delta }}_{i2}}{{\mathrm{\Delta }}_{i1}}\right)\end{array}\right]\\ & [a_{i5}(5t^4-10t_f{t}^3+5t_f^2{t}^2)]\\ & \hspace{0.17em}+{\mathrm{\Delta }}_{i1}cos\left[\begin{array}{l}{a}_{i5}(t^5-\frac{5}{2}{t}_f{t}^4+\frac{5}{3}{t}_f^2{t}^3)\\ +si{n}^{-1}\left(\frac{{\theta }_{i0}-{\mathrm{\Delta }}_{i2}}{{\mathrm{\Delta }}_{i1}}\right)\end{array}\right]\\ & [a_{i5}(20t^3-30t_f{t}^2+10t_f^{2}t)]\end{array}$$

(35)

There is only a parameter a_i5 in each joint function after parameterization, too. For the case of n=6, let a=[a₁₅,a₂₅,…,a₆₅]. Then Equations (25) and (26) are also given.

4. Normalization and Solution

R. Lampariello (1999) and S. Pandey (1997) used sinusoidal functions to paraterize the joint functions when the constrints on joint angles were considered. And the unknown parameters were solved by iterative Newtonian method. R. Lampariello (1999) proposed a criteria for assigning the initial guess of the unknown parameters. But the initial guess is dependent on the total time t_f, therefore, it is not of generality.

Here we proposed a modified method to tackle these problems. Some work is reported on the cofference RA 2005 (Xu, W. F., Liang, B., Ti, C. et al. 2005). With the method, both polynomial and sinusoidal functions can be used to plan the joint motion for the case that joint position, rate and acceleration are limited. And a more practicable criteria is developed to assign the initial values of the unknown parameters, which guarantees that the path solved is the preferable path when there are more than one path, and that the parameters converge even if the manipulator needs a long range manoeuvres. The key of the method is the normalization of the joint functions.

4.1 Normalization of the Polynomial Functions

Let $\tau =\frac{t}{t_f}$ , i.e. $t=\tau t_f(\tau \in [0,1])$ (τ∈[0,1]), then the joint functions become as follows(τ is the normanized time):

$$\begin{array}{rl}{\theta }_i& \hspace{0.17em}=a_{i5}(t^5-\frac{5}{2}{t}_f{t}^4+\frac{5}{3}{t}_f^2{t}^3)+{\theta }_{i0}\\ & \hspace{0.17em}=a_{i5}{t_f}^5({\tau }^5-\frac{5}{2}{\tau }^4+\frac{5}{3}{\tau }^3)+{\theta }_{i0}\\ & \hspace{0.17em}=a_{i5}({\tau }^5-\frac{5}{2}{\tau }^4+\frac{5}{3}{\tau }^3)+{\theta }_{i0}\\ & \hspace{0.17em}={\hat{a}}_{i5}f(\tau )+{\theta }_{i0}\end{array}$$

(36)

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

$$f(\tau )={\tau }^5-\frac{5}{2}{\tau }^4+\frac{5}{3}{\tau }^3$$

(37)

Similarly, the joint rate and acceleration function with respect to the normized time τ are as follows:

$${\dot{\hat{\theta }}}_i=\frac{d\theta }{d\tau }=t_f{\dot{\theta }}_i={\hat{a}}_{i5}{f}^{\mathrm{\prime }}(\tau )+{\theta }_{i0}$$

(38)

$${\ddot{\hat{\theta }}}_i=\frac{d^2{\theta }_i}{d{\tau }^2}=t_f^2{\ddot{\theta }}_i={\hat{a}}_{i5}{f}^{\mathrm{\prime }\mathrm{\prime }}(\tau )$$

(39)

The integrals and the corresponding system of nonlinear equations are

$${\mathbf{\Psi }}_b(\tau )={\int }_0^{\tau }{\mathit{N}}_b^{-1}{\mathit{J}}_{\omega b}\dot{\mathbf{\Theta }}d\tau$$

(40)

$${\mathit{X}}_{\mathit{e}}(\tau )={\int }_0^t{\mathit{K}}_e{\mathit{J}}^{\ast }({\mathbf{\Psi }}_b,\mathbf{\Theta })\dot{\mathbf{\Theta }}dt={\int }_0^{\tau }{\mathit{K}}_e{\mathit{J}}^{\ast }({\mathbf{\Psi }}_b,\mathbf{\Theta })\dot{\hat{\mathbf{\Theta }}}d\tau$$

(41)

$$\mathit{G}(\hat{\mathit{a}})={\mathit{X}}_{\mathit{e}\mathit{d}}-{\int }_0^1{\mathit{K}}_e{\mathit{J}}^{\ast }({\mathbf{\Psi }}_b,\mathbf{\Theta })\dot{\hat{\mathrm{\Theta }}}d\tau =0$$

(42)

Then the path planning problem becomes the problem to solve the parameter $\hat{\alpha }$ from (42). By normalization of the joint functions, the initial guess and the boundary of $\hat{\alpha }$ can be defined beforehand, which will be described in details.

The curves of f(τ) and its first and second derivatives are shown as Fig. 2.

Fig.2

f(τ) and Its First and Second Derivatives

It is obvious that f(τ) increases monotonously between [0,1]. The bounds of f(τ) and its first and second derivatives are calculated as follows:

$$f(\tau )\le f(1)=\frac{1}{6},\tau \in [0,1]$$

(43)

$$f^{\mathrm{\prime }}(\tau )\le f^{\mathrm{\prime }}(\frac{1}{2})=\frac{5}{16},\tau \in [0,1]$$

(44)

$$|f^{\mathrm{\prime }\mathrm{\prime }}(\tau )|\le f^{\mathrm{\prime }\mathrm{\prime }}(\frac{1}{2}\pm \frac{\sqrt{3}}{6})=0.9623,\tau \in [0,1]$$

(45)

According to Equations (14), (36) and (43), the parameter ${\hat{\alpha }}_{i5}$ must be within the domain as follows, which satifies the kinematics constraint of i^th joint:

$${\hat{a}}_{i5}\in [6({\theta }_{i\mathrm{\_}min}-{\theta }_{i0}),6({\theta }_{i\mathrm{\_}max}-{\theta }_{i0})],i=1,\cdots ,n$$

(46)

Inequalities (46) can be written in the vector form as $\hat{\mathit{a}}\in [{\hat{\mathit{a}}}_{min},{\hat{\mathit{a}}}_{max}]$ . The initial guess and the final convergence of â must fall in the boundary. If â converges to a value beyond the domain, it is necessary to re-assign the initial value of â and solve the system of nonlinear equations again. The process above continues till â converges to a value within the domain.

Moreover, t_f can be used to limit joint rate and acceleration. According to Equations (15), (16), (38), (39), (44) and (45), the total time of the path planning is determined according to the following inequalities.

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

(47)

$$t_f^2\ge \left|{\ddot{\hat{\theta }}}_{imax}\right|/{\ddot{\theta }}_{imax}$$

(48)

When the solved parameter ${\hat{a}}_{i5}$ and t_f satisfy (46)˜(48) the paths which satisfy the limits on the joint angle, angle velocity and acceleration are determined.

4.2 Normalization of the Sinusoidal Functions

Similaly, sinusoidal joint functions can be normalized. Let $\tau =\frac{t}{t_f}$ , i.e. $t=\tau t_f(\tau \in [0,1])$ , then the joint functions become as follows (In the following equations ${\hat{a}}_{i5}=a_{i5}{t}_f^5$ ):

$${\theta }_i={\mathrm{\Delta }}_{i1}sin(a_{a5}^{\mathrm{\prime }}({\tau }^5-\frac{5}{2}{\tau }^4+\frac{5}{3}{\tau }^3)+si{n}^{-1}\left(\frac{{\theta }_{i0}-{\mathrm{\Delta }}_{i2}}{{\mathrm{\Delta }}_{i1}}\right))+{\mathrm{\Delta }}_{i2}$$

(49)

$${\dot{\hat{\theta }}}_i=\frac{d{\theta }_i}{d\tau }={\mathrm{\Delta }}_{i1}cos\left[\begin{array}{l}{a}_{i5}^{\mathrm{\prime }}(t^5-\frac{5}{2}{\tau }_4{t}^4+\frac{5}{3}{\tau }^3)\\ +si{n}^{-1}\left(\frac{{\theta }_{i0}-{\mathrm{\Delta }}_{i2}}{{\mathrm{\Delta }}_{i1}}\right)\end{array}\right]$$

(50)

$$\begin{array}{rl}& [a_{i5}^{\mathrm{\prime }}(5{\tau }^4-10{\tau }^3+5{\tau }^2)]\\ & {\ddot{\hat{\theta }}}_i=\frac{d^2{\theta }_i}{d{\tau }^2}=-{\mathrm{\Delta }}_{i1}sin\left[\begin{array}{l}{a}_{i5}^{\mathrm{\prime }}({\tau }^5-\frac{5}{2}{\tau }^4+\frac{5}{3}{\tau }^3)\\ +si{n}^{-1}\left(\frac{{\theta }_{i0}-{\mathrm{\Delta }}_{i2}}{{\mathrm{\Delta }}_{i1}}\right)\end{array}\right]\\ & [a_{i5}^{\mathrm{\prime }}(5{\tau }^4-10{\tau }^3+5{\tau }^2)]\\ & \hspace{0.17em}+{\mathrm{\Delta }}_{i1}cos\left[\begin{array}{l}{a}_{i5}^{\mathrm{\prime }}({\tau }^5-\frac{5}{2}{\tau }^4+\frac{5}{3}{\tau }^3)\\ +si{n}^{-1}\left(\frac{{\theta }_{i0}-{\mathrm{\Delta }}_{i2}}{{\mathrm{\Delta }}_{i1}}\right)\end{array}\right]\\ & [a_{i5}^{\mathrm{\prime }}(20{\tau }^3-30{\tau }^2+10\tau )]\end{array}$$

(51)

The joint function with respect to time for different parameter values are shown as fig.3 (assume that θ _i ∈[−140°, 170°]). Different from the case of polynominal functions, the joint functions are highly nonlinear with respect to the parameter a. And the joint angle is always within the upper and lower bounds.

Fig.3

Joint Function with respect to time for different values of the parameter a_i5

4.3 Solution to the Path planning Problem

The system of the equations with respect to the unknown parameters can be solved with an iterative Newtonian method. The algorithm is as follows:

Initialization:

$\begin{array}{l}k=1,{\hat{\mathit{a}}}_k={\hat{\mathit{a}}}_0,\epsilon ={10}^{-8},X_{e0}=[r_{x0},r_{y0},r_{z0},{\alpha }_{e0},{\beta }_{e0},{\gamma }_{e0}{]}^T,\\ X_{ed}=[r_{xd},r_{yd},r_{zd},{\alpha }_{ed},{\beta }_{ed},{\gamma }_{ed}{]}^T\end{array}$ ;

X_ef is gotten by integrating the Equation (41) and if $\parallel \mathit{G}({\hat{\mathit{a}}}_k)={\mathit{X}}_{ed}-{\mathit{X}}_{ef}\parallel \le \epsilon$ , go to 4); else update the parameters as follows:

$${\mathit{G}}^{\mathrm{\prime }}({\hat{\mathit{a}}}_k)=\left[\begin{array}{ccc}\frac{\mathrm{\partial }{g}_1}{\mathrm{\partial }{\hat{a}}_{15}}& \cdots & \frac{\mathrm{\partial }{g}_1}{\mathrm{\partial }{\hat{a}}_{65}}\\ ⋮& & ⋮\\ \frac{\mathrm{\partial }{g}_6}{\mathrm{\partial }{\hat{a}}_{15}}& \cdots & \frac{\mathrm{\partial }{g}_6}{\mathrm{\partial }{\hat{a}}_{65}}\end{array}\right]$$

(52)

$$\mathrm{\Delta }{\hat{\mathit{a}}}_k=-{[{\mathit{G}}^{\mathrm{\prime }}({\hat{\mathit{a}}}_k)]}^{-1}\mathit{G}({\hat{\mathit{a}}}_k)$$

(53)

$${\hat{\mathit{a}}}_{k+1}=-{\hat{\mathit{a}}}_k+\mathrm{\Delta }{\hat{\mathit{a}}}_k$$

(54)

k=k+1, go to 2);

if ${\hat{\mathit{a}}}_k\in [{\hat{\mathit{a}}}_{min},{\hat{\mathit{a}}}_{max}]$ , go to 5); else, reset ${\hat{\mathit{a}}}_0$ (to vary the sign of ${\hat{a}}_{15}\sim {\hat{a}}_{35}$ cyclically in the similar way proposed by R. Lampariello) and go to 2);

Output the parameter ${\hat{\mathit{a}}}_k$ and determine t_f according to Inequalities (47) and (48), then the parameter $\mathit{a}={\hat{\mathit{a}}}_k/t_f^5$ and the joint functions between [0, t_f] are determined, which satisfy the limits on joint position, joint velocity and acceleration.

The whole processs is shown as Fig.4

Fig.4

The Flow-Chart of the Algorithm

4.4 Criteria for the Initial Parameters Guess

The final pose of the end-effector X_ef is a hypersurface of the parameter â. Fig.5 indicates the variation of X_ef with the parameter ${\hat{a}}_{15}({\hat{a}}_{25}=\cdots ={\hat{a}}_{65}=1)$ for the case of polynominal paramerization (the calculation is based on the space robotic model of Section 5). The initial pose of the end-effector is X_e0=[0.0093,−0.4284,1.6340,−π/4,0, π/2] ^T . It shows that if ${\hat{a}}_{15}\in (0,1)$ or (−10, 0), the function lies in the linearity area and it guarantees that the iterative process won't get stuck in a sink. Other parameters are analyzed in a similar way. Therefore, the initial value of â can be set as ${\hat{\mathit{\alpha }}}_0=[1,1,1,1,1,1]$ .

Fig.5

Pose of the End-effector vs Parameter a₁₅

For the case of sinusoidal parameterization, the variation of X_ef with the parameter can be analyzed in the same way as the case of polynominal parameterization. It shows that when ${\hat{a}}_{15}\in (0,9)$ or (−9, 0), the function lies in the linearity area and it guarantees that the iterative process won't get stuck in a sink. Another citeria of the initial parameters guess for sinusoidal parameterized functions is that the final joint angle with respect to the parameters must be in the linear section as Fig.6, which assures that the joint will not oscillate. The approximately linear section is a_i5 ∈[−8, 8]. Therefore, the initial value of â can be set as ${\hat{\mathit{\alpha }}}_0=[1,1,1,1,1,1]$ .

Fig.6

Final Value with respect to the parameter a_i5

4.5 Advantages of the Path Planning Method

There are some advantages of the path planning method proposed in the paper:

(1)

Not only sinusoidal functions but also polynomial functions can be used to plan the Cartesian path when the limits on joint angles, rates, accelerations are considerd.

(2)

The boundary of the parameters can be defined beforehand. Therefore, the resolution algorithm only needs to search the parameter withen the boundary, which simplifies the algorithm.

(3)

A general criteria is supplied to assign the initial guess of the unknown parameters, which is independent on the planning conditions such as the time t_f.

(4)

A general method is proposed to take the constraint on joint angles, rates and accelerations in the solution to the problem of the path planning.

5. Simulation Study

5.1 Space Robotic System

A space robot system is composed of a spacecraft, a 6 DOF manipulator mounted on the spacecraft and an object as a target to be captured by the manipulator, as Fig.7. The body-fixed frames of the space robotic system are defined by Denavit-Hartenberg convention, as Fig.8. The base frame, which is fixed on the satellite, and the i^th body frame are represented by Σ₀ and Σ _i respectively. The Denavit-Hartenberg parameters of the manipulator are given in Tab.1. And Tab.2 shows the CM position vector ${}^i{\mathit{s}}_i$ and inertia matrix ${}^i{\mathit{I}}_i$ . Here, ${}^i{\mathit{s}}_i$ and ${}^i{\mathit{I}}_i$ are expressed in Σ _i . Sat and B_i stand for the satellite and the ith body respectively).

Fig.7

A Space Robotic System

Fig.8

The D-H Coordinate System

Table.1

D-H Parameters of the Manipulator

i	θ i	α i	ai	di	[θi_min, θ i_max	$\hat{\mathit{a}}\in [{\hat{\mathit{a}}}_{min},{\hat{\mathit{a}}}_{max}]$
	/rad	/rad	/m	/m	/rad	(polynomial case)
1	π/2	π/2	0	0.3	[−2.79,2.79]	[−7.3, 26.18]
2	0	0	0.6	0.3	[−π/4, 3.93]	[−6.63, 21.65]
3	−π2	−π/2	0	−0.3	[−3.93, π/4]	[−22.73, 5.55]
4	0	π/2	0	0.4	[−1.92,2.97]	[−11.52, 17.80]
5	0	−π/2	0	0	[−1.75, 1.75]	[−14.11, 6.84]
6	0	0	0	0.45	[−4.64,4.64]	[−46.71, 9.01]

Table.2

The CM and Mass Properties of Each Body

		B0	B1	B2	B3	B4	B5	B6
mass/kg		400	15	10	12	8	5	2
iSi/m		0	0	−0.41	0	0	0	0
		0.32	−0.15	0	−0.15	−0.2	0	0
		−0.55	0	−0.06	0	0	0.03	−0.16
iIi / kg·m2	Ixx	30	0.15	0.21	0.11	0.12	0.033	0.05
	Iyy	30	0.075	2.27	0.03	0.02	0.026	0.05
	Izz	30	0.15	2.09	0.11	0.12	0.017	0.02
	Ixy	0	0	0	0	0	0	0
	Ixz	0	0	−0.46	0	0	0	0
	Iyz	0	0	0	0	0	0	0

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

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

(55)

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

(56)

5.2 Case of Polynomial Parameterization

For the point-to-point manoeuvre as Fig.7, the initial and desired pose of the end-effector are as follows:

$$\begin{array}{l}{X}_{\mathit{e}\mathbf{0}}=[0.0093,-0.4284,1.6340,-\pi /4,0,\pi /2{]}^T\\ {\mathit{X}}_{\mathit{e}\mathit{d}}=[0.0295,0.0588,1.4317,-1.5789,0.0446,1.5166{]}^T\end{array}$$

The initial values of the manipulator joints and the satellite attitude are Θ₀ =[π/2,0.32, −0.14, 0, 0.6, π] ^T ) and ψ _b (0)=[0,0,0]^T respectively. According to the criterion proposed above, the initial guess of the parameter $\hat{\mathit{\alpha }}$ is set as ${\hat{\mathit{\alpha }}}_0=[1,1,1,1,1,1]$ . The method proposed above is used to solve the point-to-point path planning problem. The parameter finally converges to (The detailed process is shown as fig.4):

$${\hat{\mathit{\alpha }}}^{\ast }=[-0.2980,8.2583,-2.5868,-0.1089,1.2420,-0.2450]$$

The convergence error is

$${1.0}^{-9}\times [-0.0594,-0.0112,0.0691,-0.0468,-0.1255,0.1496]$$

Then the total time t_f must satisfy the following inequalities (according to (47) ˜ (48)):

$$t_f\ge \frac{5}{16}|{\hat{a}}_{i5}|/{\dot{\theta }}_{imax}=\frac{5}{16}|{\hat{a}}_{25}|/{\dot{\theta }}_{2\mathrm{\_}max}=29.5729$$

(57)

$$t_f\ge \sqrt{0.9623|{\hat{a}}_{i5}|/{\ddot{\theta }}_{imax}}=\sqrt{0.9623|{\hat{a}}_{25}|/{\ddot{\theta }}_{2max}}=30.1771$$

(58)

At last, t_f is chosen as t_f=30.2. According to the simulation results, the planned joint angles and the corresponding attitude change of the base are shown as Fig.9. And the joint rate and acceleration vary as Fig.10 and Fig.11 respectively. They are within their bounds. In order to verify the planned path, the dynamic model of the whole system is created by ADAMS–a powerful mechanical motion simulation software (MSC software. 2005). The joint data (θ₁˜θ₆) are imported to ADAMS, and the dynamic simulation is done. The final state of the simulation is shown as Fig.12. It can be seen that the pose of the end-effector attains the desired pose although the orientation of the base changes largely.

Fig.9

The Joint Angles and the Attitude of the Satellite

Fig.10

The Manipulator Joint Angle Velocities

Fig.11

The Manipulator Joint Angle Accelerations

Fig.12

The State of the System after Point-to-Point Manoeuvre

5.3 Case of Sinusoidal Parameterization

The simulation condition is the same as that of the case of sinusoidal parameterization. According to the criterion, the initial value of the parameter $\hat{\mathit{\alpha }}$ is set as ${\hat{\mathit{\alpha }}}_0=[1,1,1,1,1,1]$ . The parameter is solved by the method proposed above. The parameter finally converges to:

$${\hat{\mathit{\alpha }}}^{\ast }=[0.5583,-0.2044,0.3776,-1.3454,-1.4671,-2.8541]$$

The convergence error is

$${1.0}^{-7}\times [0.0011,-0.0000,0.0004,-0.2758,-0.2257,0.4072]$$

The total time is determined by the following inequalities

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

(59)

$$t_f\ge \sqrt{{\ddot{\hat{\theta }}}_{imax}/{\ddot{\theta }}_{imax}}=\sqrt{979.9649/0.5}=44.2711$$

(60)

Therefore, the total time is choosen as t_f=62s. According to the simulation results, the variation of the manipulator joint angles and the base attitude are shown as Fig.13. And the joint rate and acceleration vary as Fig.14 and Fig.15 respectively.

Fig.13

Joint Angles and Satellite Attitude

Fig.14

The Manipulator Joint Angle Velocities

Fig.15

The Manipulator Joint Angle Accelerations

6. Conclusion and Discussion

Direct kinematic equations are used to plan the Cartesin path of free-floating robot, which is a good way to avoid the effect of dynamic singularies. The key of the method is the normalization of the parameterized functions of the manipulator joints. Polynomial and sinusoidal parameterized functions have their own advantages. Sinusoidal functions are used to limit joint angles direcly, but the convergence time is longer than that of the case of polynomial parameterization. After normalization, polynomial parameterization is more effective than sinusoidal parameterization since the constraints are easy to satisfied. So we suggest to use polynomial function to parameterize the joint angles. Moreover, it is worth pointing out that Euler angles are not globally singularity-free representation of the attitude, so the unit quaternion shouled be used when the variation of attitude is possibly larger that π/2.

The shortcoming of the method is that the convergence time is long. Furtherover, there are different paths to reach the desired pose of the end-effector because of the nonholonomic redundancy of the free-floating system. Therefore, the future research will focus on optimal plan based on some cost functions. The path planning for attitude variation using the manipulator joints is another interest. At the same time, we will try to improving the efficiency of the algorithm.