Trajectory Planning of 7-DOF Space Manipulator for Minimizing Base Disturbance

Abstract

In the free-floating mode, there is intense dynamic coupling existing between the space manipulator and the base, and the base attitude may change while performing a motion with its manipulator. Therefore, it is necessary to reduce the interference that resulted from the manipulator movement. For planning trajectories of the space manipulator with 7 degrees of freedom (7-DOF), simulated annealing particle swarm optimization (SAPSO) algorithm is presented in the paper. Firstly, kinematics equations are setup. Secondly, the joint functions are parameterized by sinusoidal functions, and the objective function is defined according to the motion constraints of manipulator and accuracy requirements of the base attitude. Finally, SAPSO algorithm is used to search the optimal trajectory. The simulation results verify the proposed method.

1. Introduction

Space robot will play an important role in space exploration with the development of space technology. Since the robotic on-orbit services can handle space rubbishes, which resulted from abandoned satellites, and broaden the valuable resources for new satellites reference [1], a lot of scholars have researched them. In addition, space robots instead of astronauts can help astronauts to avoid being hurt and to improve the benefits of the exploration.

Planning suitable path is the premise of space robots carrying out on-orbit service missions. Because of the dynamic coupling between space manipulator and base, the base pose (position and orientation) changes with the movement of manipulator, at the same time, which in turn affects the end-effector pose. Therefore, compared with the space manipulator with fixed base, path planning of free-floating space manipulator (the base pose is not controlled) is more complex.

There are many literatures dealing with the problem of base pose for space manipulator. A method for the attitude control using enhanced disturbance map (EDM) was proposed by Dubowsky and Torres [2], which reduces the interference to the base attitude, but it needs larger storage space and it is low-efficient. The bidirectional path planning method whose ability is to adjust the configuration of manipulator and the base attitude was addressed by Nakamura and Mukherjee [3], but there are no state variables of system defined in detail. A neural network adaptive control strategy to identify all parts of the system parameters was put forward in reference [4], which has a great value in engineering applications, but it is easy to fall into local minimum and its convergence speed is slow. A multiobjective genetic algorithm (GA) was used by Liu and Huang [5] to search the optimal motion trajectory, though the stability of the base attitude control can be achieved, the multiobjective GA is more complex to realize. The GA and the particle swarm optimization (PSO) algorithm, gravitational search algorithm (GSA) and PSO algorithm, and immune PSO algorithm for trajectory planning were, respectively, presented by Roberge et al. [6], Constantin and Lucian [7], and Wang and Yu [8], because the structure of the manipulator with 7 degrees of freedom (7-DOF) is different from those mentioned in the above literature, and the methods are difficult to control the manipulator presented in this paper. So the paper combines PSO algorithm with simulated annealing algorithm to minimize the base attitude disturbance through planning the joint trajectory.

2. Kinematics Model of Space Manipulator

In this paper, the S-R-S space manipulator with 7-DOF as shown in reference [9] is used for path planning, making its base reach the desired attitude. In the S-R-S structure, the first, second, and third joints are used to compose the shoulder joint, which can be equivalent to a virtual ball joints, and its center is located in the intersection of three revolving axes. The wrist joint is composed of the fifth, sixth, and seventh joint, and the fourth joint is regarded as elbow joint, which is a simple rotary joint. Figure 1 is the structure diagram of S-R-S space manipulator.

Figure 1.

Structure diagram of S-R-S space manipulator

The description about joint information of S-R-S space manipulator can refer to the general model as Figure 2. The D-H parameters of the manipulator with 7-DOF are shown in Table 1. Where, ${\alpha }_{i-1}$ denotes the link corner of manipulator, $a_{i-1}$ is the length of common vertical line from the joint shaft $i-1$ to i, d_i shows the link offset, and θ_i represents the joint angles. The quality characteristics of space manipulator are shown in Table 2.

Figure 2.

Structure diagram of general model

Table 1.

D-H parameters of system

i	${\alpha }_{i-1}$	$a_{i-1}$	di	θi
1	$-\pi /2$	0	d 1	θ 1
2	$\pi /2$	0	0	θ 2
3	$-\pi /2$	0	d 3	θ 3
4	$\pi /2$	0	0	θ 4
5	$-\pi /2$	0	d 5	θ 5
6	$\pi /2$	0	0	θ 6
7	0	0	d 7	θ 7

Table 2.

Quality characteristics of space manipulator

	B 0	B 1	B 2	B 3
$m(kg)$	1146.34	3.692	60.416	3.678
$l(m)$	0.850	0.130	1.500	0.120
$I_{xx}{(\text{kg}\times \mathrm{m}}^2\text{)}$	291.077	0.013	13.556	0.012
$I_{yy}{(\text{kg}\times \mathrm{m}}^2\text{)}$	536.263	0.005	2.274	0.010
$I_{zz}{(\text{kg}\times \mathrm{m}}^2\text{)}$	669.647	0.015	15.756	0.005
	B 4	B 5	B 6	B 7
$m\text{(kg)}$	35.654	3.461	2.624	0.114
$l(m)$	1.240	0.220	0.070	0.010
$I_{xx}{(\text{kg}\times \mathrm{m}}^2\text{)}$	1.235	0.061	0.066	0
$I_{yy}{(\text{kg}\times \mathrm{m}}^2\text{)}$	7.309	0.028	0.004	0
$I_{zz}{(\text{kg}\times \mathrm{m}}^2\text{)}$	6.104	0.035	0.004	0

The comments of the symbols appeared in this paper are shown in Table 3.

Table 3.

The comments of the symbols

B 0	Base of the system
$B_i(i=1,2,\cdots ,7)$	The $i\text{th}$ link
mi	Mass of link i
li	The length of the $i\text{th}$ link
I	Movement inertia of the links
$r_b\in R^3$	Position vector of the center of mass (CM) of base with respect to inertial coordinates
$r_i\in R^3\left(i=1,\mathrm{\dots },7\right)$	Position vector of CM of link i with respect to inertial coordinates
$r_g\in R^3$	Position vector of the system with respect to inertial coordinates
$b_0\in R^3$	Position vector of CM of base to joint 1 with respect to inertial coordinates
$p_i\in R^3$	Position vector of joint i with respect to inertial coordinates
$r_e\in R^3$	Position vector of end-effector with respect to inertial coordinates
$v_0\in R^3$	Linear velocity of base
$v_e\in R^3$	Linear velocity of end-effector
${\omega }_0\in R^3$	Angular velocity of base at certain time
${\omega }_e\in R^3$	Angular velocity of end-effector at certain time
$K_i\in R^3$	The unit direction vector of z-axle of frame
θi	The $i\text{th}$ joint angle
$\Theta \in R^n$	Joint angle vector
M	Mass of system
$r_i$	Radius of the $i\text{th}$ link
$\eta =\text{cos}\left(\psi /2\right)$	Scalar part of quaternion representation
$q=k\text{sin}\left(\psi /2\right)$	Vector parts of quaternion representation

Manipulator with fixed base uses position level kinematics equations for path planning, but the free-floating space manipulator system is a nonholonomic system and its end-effector pose is related not only to the current joint angles but also to the motion history of joints, so the Japanese scholars Umetani and Yoshida [10] proposed generalized Jacobin matrix, which reflected the relation between end-effector velocity and joint angular velocities.

2.1. Kinematics Equations of Space Manipulator

For the space manipulator with 7-DOF, the position vector of the links and end-effector is expressed as

$\begin{array}{l}{r}_i=r_b+b_0+{\displaystyle \sum _{k=1}^{i-1}\left(a_k+b_k\right)}+a_i\\ r_e=r_b+b_0+{\displaystyle \sum _{i=1}^n(p_{i+1}-p_i})\end{array}$

(1)

The links and end-effector velocity can be obtained by differentiating equation (1) with respect to time:

$\begin{array}{l}{v}_i={\stackrel{·}{r}}_i=v_i+{\omega }_0\times \left(r_i-r_b\right)+{\displaystyle \sum _{i=1}^n\left[K_i\times \left(r_i-p_i\right)\right]}{\stackrel{·}{\theta }}_i\\ v_e={\stackrel{·}{r}}_e=v_0+{\omega }_0\times \left(r_e-r_b\right)+{\displaystyle \sum _{i=1}^n\left[K_i\times \left(r_e-p_i\right)\right]}{\stackrel{·}{\theta }}_i\end{array}$

(2)

However, the angular velocity of the end-effector can be expressed as

$\begin{array}{l}{\omega }_i^{}={\omega }_0+{\displaystyle \sum _{k=1}^i{k}_k}{\stackrel{·}{\theta }}_k\\ {\omega }_e^{}={\omega }_0+{\displaystyle \sum _{k=1}^n{k}_k}{\stackrel{·}{\theta }}_k\end{array}$

(3)

Equations (2) and (3) are in matrix form as follows:

$\left[\begin{array}{l}{v}_e\\ {\omega }_e\end{array}\right]=J_b\left[\begin{array}{l}{v}_0\\ {\omega }_0\end{array}\right]+J_m\stackrel{·}{\Theta }$

(4)

where,

$J_b=\left[\begin{array}{cc}E& -{\tilde{r}}_{be}\\ 0& E\end{array}\right]r_{be}=r_e-r_b$

(5)

$J_m=\left[\begin{array}{l}{k}_1\times (r_e-p_1)\dots \dots k_n\times (r_e-p_n)\\ k_1\dots \dots k_n\end{array}\right]$

(6)

$J_b$ denotes a Jacobin matrix associated with the base motion, $J_m$ is a Jacobin matrix bound up with the motion of manipulator, and the expression of $J_m$ is identical with the ordinary Jacobin matrix of corresponding manipulator.

For the free-floating space manipulator, if there is no external force disturbance, it meets the angular momentum and linear momentum conservation law as follows:

$P={\displaystyle \sum _{i=0}^n{m}_i{\stackrel{·}{r}}_i}=0$

(7)

$L={\displaystyle \sum _{i=0}^n\left(I_i{\omega }_i+r_i\times m_i{\stackrel{·}{r}}_i\right)}=0$

(8)

Substituting equations (2) and (3) into equation (8), the system meets the nonholonomic constraint:

$I_b\left[\begin{array}{l}{v}_0\\ {\omega }_0\end{array}\right]+I_{bm}\stackrel{·}{\Theta }=0$

(9)

Therefore,

$\left[\begin{array}{l}{v}_0\\ {\omega }_0\end{array}\right]=-I_b^{-1}{I}_{bm}\stackrel{·}{\Theta }=\left[\begin{array}{l}{J}_{vb}\\ J_{\omega b}\end{array}\right]\stackrel{·}{\Theta }$

(10)

where,

$I_b=\left[\begin{array}{cc}M\cdot E& M\cdot {\tilde{r}}_{bg}^T\\ M\cdot {\tilde{r}}_{bg}& I_M\end{array}\right]r_{bg}=r_g-r_b$

(11)

$I_{bm}=\left[\begin{array}{l}{J}_{TM}\\ J_{M\varphi }\end{array}\right]$

(12)

$I_M=\sum _{i=1}^n(I_i+m_i\cdot {\tilde{r}}_{bi}^T\cdot {\tilde{r}}_{bi}+I_0\in R^{3\times 3}$

$r_{bi}=r_i-r_b$

(13)

$J_{M\phi }={\displaystyle \sum _{i=1}^n(I_i\cdot J_{Ri}+m_i\cdot {\tilde{r}}_{bi}\cdot J_{Ti})}\in R^{3\times n}$

(14)

$J_{TM}={\displaystyle \sum _{i=1}^n(m_i\cdot J_{Ti})}\in R^{3\times n}$

(15)

$J_{Ti}=\left[k_1\times (r_i-p_1),\dots ,k_i\times (r_i-p_i),0,\dots ,0\right]\in R^{3\times n}$

(16)

$J_{Ri}=\left[k_1,k_2,\dots ,k_i,0,\dots ,0\right]\in R^{3\times n}$

(17)

If equation (8) is substituted into equation (4), we can achieve the result:

$\left[\begin{array}{l}{v}_e\\ {\omega }_e\end{array}\right]=\left[{J\Psi }_m-J_b{I}_b^{-1}{I}_{bm}\right]\stackrel{·}{\Theta }=J^{\ast }({\mathrm{\Psi }}_b,\Theta ,m_i,I_i)\stackrel{·}{\Theta }$

(18)

where, $J^{\ast }({\Psi }_b,\Theta ,m_i,I_i)$ denotes the generalized Jacobin matrix of space manipulator, which is the function about base attitude, joint angles of manipulator, and the mass and inertia of rigid body.

2.2. Quaternion Representation of Rigid Posture

Compared with rotation matrix and Euler angles, unit quaternion is the most commonly used quaternion for global attitude representation, which has the following advantages as shown in reference [11]: (1) no singular appeared in the process of operation; (2) representations of velocities and angular displacement become simple; and (3) since the quaternion arithmetic is a linear operation, it will improve the calculation efficiency of the relative relationship between the coordinate systems. The quaternion is defined as follows:

$Q=\eta +q_1\overrightarrow{i}+q_2\overrightarrow{j}+q_3\overrightarrow{k}=\eta +q\in R^4$

(19)

We will differentiate η and $q$ separately

$\left[\begin{array}{l}\stackrel{·}{\eta }\\ \stackrel{·}{q}\end{array}\right]=\left[\begin{array}{l}-\frac{1}{2}\omega q\\ \frac{1}{2}\tilde{\omega }q+\frac{1}{2}\eta \omega \end{array}\right]=\frac{1}{2}\left[\begin{array}{l}-q^T\\ \eta I-\tilde{q}\end{array}\right]\omega$

(20)

Considering $\{{\eta }_{bd},q_{bd}\}$ as the base attitude, $\{{\eta }_{b0},q_{b0}\}$ is the expected base attitude, relative attitude of them should satisfy the following relations:

$\begin{array}{l}\delta {\eta }_b={\eta }_{b0}{\eta }_{bd}+q_{b0}{q}_{bd}\\ \delta q_b={\eta }_{b0}{q}_{bd}-{\eta }_{bd}{q}_{b0}-{\tilde{q}}_{b0}{q}_{bd}\end{array}$

(21)

3. Trajectory Planning in Joint Space

We assume that $X_b$ is the base pose, $Q_b$ denotes the base posture, and $P_b$ means the base position, that is, to say

$X_b=\left[\begin{array}{l}{Q}_b\\ P_b\end{array}\right]\in R^{4\times 1}$

(22)

$Q_b(t)=\frac{1}{2}{\displaystyle {\int }_0^t\left[\begin{array}{c}-q_b^T\\ {\eta }_{b}I-{\tilde{q}}_b\end{array}\right]}{J}_{\omega b}\stackrel{·}{\Theta }dt$

(23)

$X_{b0}$ and $X_{bf}$ are respectively the initial and final pose of base during the time $[0,t_f]$ , and $Q_b(t)$ is the base attitude at time t. In order to reduce the base interference that resulted from joints motion, path planning in this paper is to plan the joint trajectories:

${\theta }_i(t)=f_i(t)i=1,2,\dots ,7$

(24)

For the goal:

$Q_b(0)=Q_{b0}{Q}_b(t_f)\to Q_{bd}$

(25)

where, $Q_{b0}$ is the initial attitude of base, and $Q_{bd}$ is the expected attitude of base.

3.1. Constraints

The joint angles (initial and desired joint angles), velocities, and accelerations should satisfy the following constraints:

$\begin{array}{l}{\theta }_i(0)={\theta }_{i0}\hspace{1em}\hspace{1em}{\stackrel{·}{\theta }}_i(0)={\stackrel{··}{\theta }}_i(0)=0\\ {\stackrel{·}{\theta }}_i(t_f)={\stackrel{··}{\theta }}_i(t_f)=0{\theta }_{i\_\text{min}}\le {\theta }_i(t)\le {\theta }_{i\_\text{max}}\\ |{\stackrel{·}{\theta }}_i(t)|\le {\stackrel{·}{\theta }}_{i\_\text{lim}it}|{\stackrel{··}{\theta }}_i(t)|\le {\stackrel{··}{\theta }}_{i\_\text{lim}it}\end{array}$

(26)

where, $i=1,2,\dots ,7$ , ${\theta }_{i0}$ is the initial joint angles. ${\theta }_{i\_\text{min}}$ and ${\theta }_{i\_\text{max}}$ are respectively the minimum and maximum values of the ith joint angles, ${\stackrel{·}{\theta }}_{i\_\text{lim}it}$ and ${\stackrel{··}{\theta }}_{i\_\text{lim}it}$ are used to indicate the restricted values of angular velocities and accelerations.

3.2. Parameterize the Joint Functions

Because joint functions ${\theta }_i(t)$ must satisfy the constraints in (24) and the sine function constraints the range of joint angles directly, so we use sine function as shown in reference [12] with five order polynomial to parameterize joint functions, the joint functions after parameterization are

${\theta }_i(t)={\mathrm{\Delta }}_{i1}\text{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}$

(27)

where, $i=1,2,\dots ,7$ and $[a_{i1},a_{i2},\dots a_{i5}]$ are the polynomial coefficients. ${\mathrm{\Delta }}_{i1}$ and ${\mathrm{\Delta }}_{i2}$ are defined as follows:

${\mathrm{\Delta }}_{i1}=\frac{{\theta }_{i\_\text{max}}-{\theta }_{i\_\text{min}}}{2}{\mathrm{\Delta }}_{i2}=\frac{{\theta }_{i\_\text{max}}+{\theta }_{i\_\text{min}}}{2}$

(28)

Substituting equation (24) into equation (25), the values of polynomial parameters are derived:

$\begin{array}{l}{a}_{i0}={\text{sin}}^{-1}\left(\frac{{\theta }_{i0}-{\mathrm{\Delta }}_{i2}}{{\mathrm{\Delta }}_{i1}}\right)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)

Therefore, the joint functions, joint angle velocities, and accelerations can be respectively expressed as:

$\begin{array}{l}\theta (t)={\mathrm{\Delta }}_{i1}\text{sin}\left(a_{i5}{t}^5-\frac{5}{2}{a}_{i5}{t}_f{t}^4+\frac{5}{3}{a}_{i5}{t}_f^2{t}^3+{\text{sin}}^{-1}\left(\frac{{\theta }_{i0}-{\mathrm{\Delta }}_{i2}}{{\mathrm{\Delta }}_{i1}}\right)\right)+{\mathrm{\Delta }}_{i2}\\ \hspace{1em}\begin{array}{l}\stackrel{·}{\theta }(t)={\mathrm{\Delta }}_{i1}\text{cos}\left(a_{i5}\left(t^5-\frac{5}{2}{t}_f{t}^4+\frac{5}{3}{t}_f^2{t}^3\right)+{\text{sin}}^{-1}\left(\frac{{\theta }_{i0}-{\mathrm{\Delta }}_{i2}}{{\mathrm{\Delta }}_{i1}}\right)\right)\\ \left(a_{i5}\left(5t^4-10t_f{t}^3+5t_f^2{t}^2\right)\right)\\ \end{array}\end{array}$

$\begin{array}{l}{\stackrel{··}{\theta }}_i(t)=-{\mathrm{\Delta }}_{i1}\text{sin}\left(a_{i5}\left(t^5-\frac{5}{2}{t}_f{t}^4+\frac{5}{3}{t}_f^2{t}^3\right)+{\text{sin}}^{-1}\left(\frac{{\theta }_{i0}-{\mathrm{\Delta }}_{i2}}{{\mathrm{\Delta }}_{i1}}\right)\right)\cdots \\ {\left(a_{i5}\left(5t^4-10t_f{t}^3+5t_f^2{t}^2\right)\right)}^2+\cdots \\ {\mathrm{\Delta }}_{i1}\text{cos}\left(a_{i5}\left(t^5-\frac{5}{2}{t}_f{t}^4+\frac{5}{3}{t}_f^2{t}^3\right)+{\text{sin}}^{-1}\left(\frac{{\theta }_{i0}-{\mathrm{\Delta }}_{i2}}{{\mathrm{\Delta }}_{i1}}\right)\right)\cdots \\ \left(a_{i5}\left(20t^3-30t_f{t}^2+10t_f^{2}t\right)\right)\end{array}$

There is only one unknown parameter $a_{i5}$ in the formula. For the manipulator with 7-DOF, the unknown parameter $a$ can be written as

$a=[a_{15},a_{25},a_{35},a_{45},a_{55}^{},a_{65},a_{75}]$

(30)

If we identify the unknown parameter, the joint trajectories can be determined.

3.3. Objective Function

In order to solve the unknown parameter in Section 3.2 and to reduce the interference to the base attitude, we take $a$ as the independent variable, and the objective function is defined as:

$F(a)=\frac{\Vert \delta q_b\Vert }{K_q}+\frac{F_{\stackrel{·}{\mathrm{\Theta }}}}{K_{\stackrel{·}{\mathrm{\Theta }}}}+\frac{F_{\stackrel{··}{\mathrm{\Theta }}}}{K_{\stackrel{··}{\mathrm{\Theta }}}}$

(31)

where, the norm $\Vert x\Vert =x^{T}x$ , K_q is the weight parameter of attitude error. $K_{\stackrel{·}{\mathrm{\Theta }}}$ and $K_{\stackrel{··}{\mathrm{\Theta }}}$ are respectively the limitation coefficient of joint angular velocities and accelerations. $F_{\stackrel{·}{\mathrm{\Theta }}}$ and $F_{\stackrel{··}{\mathrm{\Theta }}}$ are respectively defined according to the constraints of joint angular velocities and accelerations.

$F_{\stackrel{·}{\mathrm{\Theta }}}=\underset{i}{\text{max}}(J_{{\stackrel{·}{\theta }}_i})F_{\stackrel{··}{\mathrm{\Theta }}}=\underset{i}{\text{max}}(J_{{\stackrel{··}{\theta }}_i})$

(32)

Where, $J_{{\stackrel{·}{\theta }}_i}$ and $J_{{\stackrel{··}{\theta }}_i}$ are defined as

$J_{{\stackrel{·}{\theta }}_i}=\{\begin{array}{l}0if{\stackrel{·}{\theta }}_{i\_\text{max}}\le {\stackrel{·}{\theta }}_{i\_\text{lim}it}\\ \frac{{\stackrel{·}{\theta }}_{i\_\text{max}}-{\stackrel{·}{\theta }}_{i\_\text{lim}it}}{{\stackrel{·}{\theta }}_{i\_\text{lim}it}}else\end{array}$

(33)

$J_{{\stackrel{··}{\theta }}_i}=\{\begin{array}{l}0if{\stackrel{··}{\theta }}_{i\_\text{max}}\le {\stackrel{··}{\theta }}_{i\_\text{lim}it}\\ \frac{{\stackrel{··}{\theta }}_{i\_\text{max}}-{\stackrel{··}{\theta }}_{i\_\text{lim}it}}{{\stackrel{··}{\theta }}_{i\_\text{lim}it}}else\end{array}$

(34)

4. Trajectory Planning Based on SAPSO

4.1. The Basic Principle of SAPSO

PSO algorithm invented by Eberhart and Kennedy is an evolutionary computation technique, and it originates from the prey of birds. In PSO algorithm, the birds are all regarded as particles that have no mass and volume, and we extend them to N dimensional space. The location of particle i in N dimensional space is represented as a vector, and each particle velocity is expressed as a vector too. Same as GA, PSO algorithm is also easy to fall into premature convergence and cannot guarantee the global convergence (reference [13]).

The simulated annealing algorithm with the ability of probabilistic jumping in the search process can avoid falling into local minima effectively. Simulated annealing algorithm not only accepts the good solutions in the annealing process, but also accepts bad solutions with a certain probability affected by the temperature parameter, and the probability descends with the decrease of temperature. Therefore, we combine simulated annealing algorithm with particle swarm algorithm for planning the joint trajectories, and the each particle can be in any position with a certain probability in the feasible search space, which is not easy to fall into local minima and the precision of calculation is higher.

4.2. Steps of Trajectory Planning Based on SAPSO

If we suppose that $p\text{best}$ is the optimal solution found by particle itself and $g\text{best}$ is the global optimal solution found by the whole population, so the steps of path planning based on SAPSO are as follows (reference [13]):

Step 1 Initializing the positions $X_i(0)$ and velocities $V_i(0)$ of particles, where

$\begin{array}{l}{X}_i=a_i=[a_{15}^i,a_{25}^i,a_{35}^i,\dots ,a_{75}^i]\in R^7\\ V_i=[v_1^i,v_2^i,v_3^i,\dots ,v_6^i,v_7^i]\in R^7\end{array}$

(35)

Step 2 Calculating objective function $F_i(a)$ of each particle according to equation (29) and comparing them, then the positions and object values of particles are stored in P_i, and the positions and object values of best individuals among all $pbest$ are stored in P_g.

Step 3 Determine the initial temperature:

$t_0=f(P_g)/\text{ln}5$

(36)

Step 4 Determine the match value of each P_i at current temperature:

$TP(P_i)=\frac{e^{-[f(P_i)-f(P_g)]/t}}{{\displaystyle \sum _{i=1}^N{e}^{-[f(P_i)-f(P_g)]/t}}}$

(37)

Step 5 Selecting the global optimal value $P_g^{\text{'}}$ from all P_i according to the Roulette strategy, then the speed and position of each particle are updated on the basis of the following formula:

$\begin{array}{l}{v}_{i,j}(t+1)=\phi \{v_{i,j}(t)+c_1{r}_1[p_{i,j}-x_{i,j}(t)]+c_2{r}_2[p_{g,j}^{\text{'}}-x_{i,j}(t)]\}\\ x_{i,j}(t+1)=x_{i,j}(t)+v_{i,j}(t+1)\end{array}$

(38)

where,

$\phi =\frac{2}{|2-C-\sqrt{C^2-4C}|}C=c_1+c_2$

Step 6 Calculating the novel objective function of each particle and updating P_i and P_g.

Step 7 Annealing temperature operation: $t_{k+1}=\lambda t_k$ .

Step 8 If the procedure contains the number of iterations, we will stop searching for output results; otherwise turn Step 4.

The flow chart of SAPSO is shown as Figure 3.

Figure 3.

The flow chart of SAPSO

5. Simulations

First of all, we use the five-order polynomial to parameterize the joint function as (27), and getting the joint function only containing an unknown parameter a as (30) according to the constraints of joint angle, angular velocity, and angular acceleration. Then, updating the base pose according to equation (23) and then calculating the objective function $F\left(a\right)$ according to (31), finding the minimum value of the objective function with the optimal algorithm (PSO and SAPAO). At this time, a is identified, and we submit it into (27), then the joint function is also identified.

In order to validate the method proposed in this paper, the free-floating space robot system with 7-DOF manipulator is simulated with MATLAB R2013a.

5.1. Settings of System Parameters

We define the ranges of joint angular velocities and accelerations as follows:

$\begin{array}{l}\left|{\stackrel{·}{\theta }}_i\right|\le {\stackrel{·}{\theta }}_{i\_\text{max}}={60}^{\circ }/s=1.0472rad/s\\ \left|{\stackrel{··}{\theta }}_i\right|\le {\stackrel{··}{\theta }}_{i\_\text{max}}={60}^{\circ }/s^2=1.0472rad/s^2\end{array}$

(39)

The initial and anticipant attitudes of base are defined as follows:

$Q_{b0}={[1,0,0,0]}^T{Q}_{bd}={[1,0,0,0]}^T$

(40)

The weight coefficients of $F(a)$ are defined as:

$\{\begin{array}{l}{K}_q=\text{sin}(\pi /360)\\ K_{\stackrel{·}{\mathrm{\Theta }}}=K_{\stackrel{··}{\mathrm{\Theta }}}=0.01\end{array}$

(41)

5.2. Results of PSO - Only Be Used for Comparison

The parameters of algorithm:

$[N{c}_1{c}_{2}wMD]=[402.052.050.51007]$

(42)

Where, N is the number of particles, c₁ and c₂ denote the learning factors, w is the inertia weight, M means the maximum number of iterations, D is the number of variables. The optimal $a$ and the value of fitness are:

$\{\begin{array}{l}{a}_1=[0.1278-0.0062-0.1228-0.0648\\ 0.0257-0.02800.0784]\times {10}^{-5}\\ F_1=2.8848\times {10}^{-2}\end{array}$

(43)

The base attitude at time $t_f(t_f=30)$ is

$Q_{bf}={[0.9999,-0.0045,0.0092,-0.0121]}^T$

(44)

The absolute error between the base at time t_f and the expected one is

${\epsilon }_1={[{10}^{-4},4.5\times {10}^{-3},9.2\times {10}^{-3},1.21\times {10}^{-2}]}^T$

(45)

Figure 4 shows the base attitude; Figure 5 is the trajectories of seven joints.

Figure 4.

Curves of base attitude obtained by PSO

Figure 5.

Joint trajectories obtained by PSO

5.3. Results of SAPSO

The parameters of algorithm:

$[N{c}_1{c}_{2}lMD]=[402.052.050.51007]$

(46)

Where, N is the number of particles, c₁ and c₂ denote the learning factors, λ is the inertia weight, M means the maximum number of iterations, D is the number of variables. The optimal $a$ and the value of fitness are:

$\{\begin{array}{l}{a}_2=[0.06390.00140.1282-0.1532\\ -0.0012-0.0477-0.0357]\times {10}^{-5}\\ F_2=3.5055\times {10}^{-9}\end{array}$

(47)

The base attitude at time $t_f(t_f=30)$ is

$Q_{bf}={[1.0001,2.62\times {10}^{-6},-1.01\times {10}^{-6},-4.76\times {10}^{-6}]}^T$

(48)

The absolute error between the base attitude and the expected one is

${\epsilon }_2={[5\times {10}^{-5},2.62\times {10}^{-6},1.01\times {10}^{-6},4.76\times {10}^{-6}]}^T$

(49)

Figure 6 is the curve of base attitude. Figure 7 is the planned trajectories of joint, which can affirm that the disturbance of base achieve minimum. Figure 8 is the curve of angular velocities. Figure 9 is the curve of angle acceleration.

Figure 6.

Curves of base attitude obtained by SAPSO

Figure 7.

Joint trajectories obtained by SAPSO

Figure 8.

Curves of joint velocities obtained by SAPSO

Figure 9.

Curves of joint accelerations obtained by SAPSO

5.4. Other Trajectory Planning

The method and algorithm presented in this paper can be also applied to other planning for base attitude, for example, the expected base attitude is different from the initial attitude.

The parameters of manipulator and algorithm and the limitations of joint angle and angular velocities, acceleration and so on are consistent with the above planning. But the initial and anticipant attitudes of base are defined as follows:

$\begin{array}{l}{Q}_{b0}=[1,0,0,0{]}^T\\ Q_{bd}=[0.7745,0.3415,0.4085,0.3415{]}^T\end{array}$

(50)

The optimal value of objective function and the responding a obtained by PSO are shown as follows:

$\{\begin{array}{l}{a}_3=[-0.01690.0646-0.16730.1027\\ 0.0293-0.0953-0.1559]\times {10}^{-5}\\ F_3=31.4715\end{array}$

(51)

The base attitude at time $t_f(t_f=30)$ is

$Q_{bf}={[0.9182,0.2731,0.2452,0.1502]}^T$

(52)

The absolute error between the base attitude at the end of time and the expected one is

${\epsilon }_3={[0.1437,0.0684,0.1633,0.1913]}^T$

(53)

Figure 10 shows the base attitude; Figure 11 is the trajectories of seven joints.

Figure 10.

Curves of base attitude obtained by PSO in the other planning

Figure 11.

Joint trajectories obtained by PSO in the other planning

The optimal value of objective function and the responding a obtained by SAPSO are shown as follows:

$\{\begin{array}{l}{a}_4=[-0.03690.0643-0.06100.1853\\ 0.1471-0.0077-0.0262]\times {10}^{-5}\\ F_4=4.5866\end{array}$

(54)

The base attitude at time $t_f(t_f=30)$ is

$Q_{bf}={[0.8758,0.3182,0.2531,0.2680]}^T$

(55)

The absolute error between the base attitude at the end of time and the expected one is

${\epsilon }_4={[0.1013,0.0233,0.1554,0.0735]}^T$

(56)

Figure 12 shows the base attitude; Figure 13 is the trajectories of seven joints.

Figure 12.

Curves of base attitude obtained by SAPSO in the other planning

Figure 13.

Joint trajectories obtained by SAPSO in the other planning

5.5. Analysis

Comparisons of (43) and (47), (51) and (54) can be seen that the value of objective function obtained by simulated annealing particle swarm algorithm is smaller than particle swarm algorithm. Comparing (45) with (49), (53) with (56), figure 4 with figure 6, figure 10 with figure 12, which show that the algorithm presented in this paper makes the base attitude more closely to the anticipated attitude than PSO in the planning time we have set. Figure 5,7,11 and figure 13 indicate that joint trajectories are smooth, which is convenient to control the manipulator. Besides, this paper has considered the limitation of joint angles, rates, accelerations and the changes of base attitude, which makes the algorithm more practical.

6. Conclusions

In this paper, we combine the forward kinematics with the method that parameterizes the joint trajectories through sine polynomial function and design the objective function according to the accuracy requirement of base attitude, so that the path planning of free floating space manipulator is converted to the optimization of the objective function. Because PSO algorithm has many faults, for example, it has slow convergence and is easy to fall into the local optimization, so this paper combines the particle swarm algorithm with simulated annealing algorithm for path planning, which helps us find the global optimal solution quickly and improve the computational efficiency. The approach and algorithm proposed in this paper can make the base attitude up to the attitude we expected effectively. Besides, the method is also suitable for solving path planning, making the base and end effector achieve the target attitude and position.