The path planning of space manipulator based on Gauss–Newton iteration method

Abstract

A path planning algorithm based on Gauss–Newton iteration method is proposed for the motion path of space manipulators. The algorithm of space manipulator path planning consists of two optimization aspects, the optimal joint motion target and the trajectory of joint motion, which make the grippers at the end of the space manipulators reach the expected target. First, the inverse solution of the desired state and the joint optimum state of the grippers is supposed to be the minimum value problem, and the Gauss–Newton iterative algorithm is designed to solve the problem. Then, according to the optimal joint target, the sinusoidal trajectory is designed. The path planning solution is transformed into the optimal parameter solution. Based on the optimal sinusoidal trajectory parameters, the path planning of the space manipulator is achieved. Simulation results indicate the correctness and effectiveness of the promoted method.

Introduction

With the development of space techniques, space missions are becoming increasingly diverse and complex. The on-orbit maintenance, assembly, recovery, and reuse demand quantities of research for space manipulator desperately. Failure repair, space debris cleaning, probe sampling, and other on-orbit missions complicate the environment of space manipulator motion. ¹ For ensuring the autonomous rendezvousing and capturing the target when space manipulators are under the complex environments, path planning during the space manipulator motion process is great significance.

There are plenty of achievements to the path planning of space manipulator research. L Radavelli et al., ² Ben-Horin and Shoham, ³ and Akyar ⁴ analyzed the two modeling methods based on D-H (Denavit–Hartenberg) and dual-quaternion and then compared the main performance of the two dynamic models, which was lack of derivation of the dynamic models for manipulators. S Xu et al., ⁵ Dimitrov and Yoshida, ⁶ and Yoshida et al. ⁷ researched the continuous path tracking of the gripper through Null Space Induction method and achieved the inverse dynamic solution of space manipulators which was only adapted to 6-degree-of-freedom (6-DOF) space manipulators. The singularity problem of the dynamic model based on Lagrange factor was solved by M Wang et al. ⁸ through combining the dynamic equations and the particle swarm optimization algorithm. This method obtained the optimal solution of joint motion path with constraints and solved the irreversibility of Jacobi matrix through quaternion. However, the complex penalty function of particle swarm made the calculation more complicated. Nedungadi and Kazerounian ⁹ replaced Jacobi matrix optimization algorithm by the pseudo inverse algorithm of the minimum joint torque, which was not able to adapt to the large motions. R Lampariello et al., ¹⁰ Xu et al., ¹¹ and Huang et al. ¹² proposed a sequential quadratic planning algorithm for the manipulator grippers and solved the space manipulator path planning problem as an optimization problem. The existing research works on path planning of the space manipulator are mainly based on the combination of intelligent optimization algorithm and path parameters. However, plenty priori data are needed in the optimizing process and manipulator motion cost is not considered at the stage of path parameterizing.

In this article, a new method is proposed for path planning of manipulators in two-dimensional (2D), which is based on the D-H model of space manipulators and divides the path planning into two categories: the optimal joint motion target planning and the joint motion path planning. First, according to the expected position of the manipulator gripper, we get the reverse solution of moving target position of the manipulator joint. The redundancy of the manipulator leads to various solutions in the process of inverse solution, so we change the dynamic inverse solution to minimum problem based on the cost function according to the joint mass, and the optimal motion target of the joint is obtained through the Gauss–Newton iterative algorithm; then, design the sinusoidal joint motion path according to the motion target and program the optimal parameters of the sinusoidal path function using the optimization theory. Finally, the path planning algorithm for manipulators in 2D is achieved.

The outline of this article is as follows: in the second part, the dynamic model of the space manipulator based on the D-H method is introduced, and the position description equation of the grippers is derived. In the third part, the path planning of the manipulator in 2D space is illustrated, and the method to select the appropriate evaluation function according to the motion characteristics of the space manipulator is presented. In the fourth part, the optimal motion planning algorithm and the joint motion path planning algorithm of the space manipulator are designed. In the fifth part, the path planning results for the 6-DOF space manipulator with six joints are simulated using the path planning algorithm designed here. The sixth part is the conclusion.

The dynamic model of space manipulator based on the D-H method

The 2D space manipulator model is shown in Figure 1. The manipulator is composed of a satellite base, n joints, and connecting rods. Defining $L_i$ as the joint, where $i=0,1,2,\dots ,n$ , $L_0$ represents the joint between the satellite base and the first articulation point. $L_n$ represents the gripper at the end of the space manipulator. For dynamic modeling and the coordinate description among the manipulator joints, according to the dynamic modeling method of space manipulator based on D-H method in Radavelli et al., ² the right hand coordinate system of manipulator joints $\sum _i$ is created by taking the joint center as the center of circle, the connecting rod as axis, and the axis of the joint rotation as the axis. Figure 2 shows the schematic diagrams of the two joint coordinate systems.

Figure 1.

Schematic diagram of two-dimensional space manipulator.

Figure 2.

The diagrammatic sketch of joint coordinate system of the two joints.

In Figure 2, $a_i$ is the distance between the coordinate system $\sum _{i-1}$ and $\sum _i$ along the axis $x_{i-1}$ direction, that is, the length $i-1$ of the joint $L_i$ ; ${\theta }_i$ is the angle of $x_{i-1}$ axis and $x_i$ axis rotating around $z_i$ axis. For the manipulator in 2D space researched in this article, the transformation from axis $\sum _i$ to axis $\sum _{i-1}$ can be achieved by rotating ${\theta }_i$ around $O_i{z}_i$ and translating $a_i$ along $O_i{x}_i$ . According to the principle of homogeneous transformation matrix describing the coordinate transformation, the transformational matrix of coordinate system $\sum _i$ transforming to coordinate $\sum _{i-1}$ is as follows

$$\begin{array}{c}{}^{i-1}{\mathit{h}}_i={\mathit{T}}_x\left(a_i\right){\mathit{R}}_z\left({\theta }_i\right)\hfill \\ =\left[\begin{array}{cccc}\cos {\theta }_i\hfill & -\sin {\theta }_i\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{\sin {\theta }_i}& \cos {\theta }_i\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 1\hfill \end{array}\right]·\left[\begin{array}{cccc}1\hfill & 0\hfill & 0\hfill & a_i\hfill \\ \multicolumn{1}{c}{0}& 1\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 1\hfill \end{array}\right]\hfill \\ =\left[\begin{array}{cccc}\cos {\theta }_i\hfill & -\sin {\theta }_i\hfill & 0\hfill & a_i\cos {\theta }_i\hfill \\ \multicolumn{1}{c}{\sin {\theta }_i}& \cos {\theta }_i\hfill & 0\hfill & a_i\sin {\theta }_i\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 1\hfill \end{array}\right]\hfill \end{array}$$

(1)

Similarly, the homogeneous coordinate transformation matrix of coordinate system $\sum _n$ coordinating with the 2D gripper at the end of the manipulator transformed to the coordinate system $\sum _0$ coordinating with the satellite base is given as

$$\begin{array}{c}{}^0{\mathit{h}}_n={}^0{\mathit{h}}_1{}^1{\mathit{h}}_2\cdots {}^{n-1}{\mathit{h}}_n\hfill \\ =\left[\begin{array}{cccc}\cos \left(\sum _{i=1}^n{\theta }_i\right)\hfill & -\sin \left(\sum _{i=1}^n{\theta }_i\right)\hfill & 0\hfill & \sum _{i=1}^n\left(a_i\cos \left(\sum _{j=1}^i{\theta }_j\right)\right)\hfill \\ \multicolumn{1}{c}{\sin \left(\sum _{i=1}^n{\theta }_i\right)}& \cos \left(\sum _{i=1}^n{\theta }_i\right)\hfill & 0\hfill & \sum _{i=1}^n\left(a_i\sin \left(\sum _{j=1}^i{\theta }_j\right)\right)\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 1\hfill \end{array}\right]\hfill \end{array}$$

(2)

In archive ² based on the D-H method of dynamic modeling, the 2D dynamic equation of the space manipulator is established as

$${}^0{\mathit{q}}_n={}^0{\mathit{h}}_n{}^n{\mathit{q}}_n$$

(3)

where ${}^0{\mathit{q}}_n$ is the homogeneous description of the position coordinates of the gripper at the end of the manipulator in the coordinate system coordinating with the satellite base in 2D space; ${}^n{\mathit{q}}_n$ is the homogeneous description of the position coordinates of the gripper in the coordinate system coordinating with the gripper; by the coordinate system definition rules ${}^n{\mathit{q}}_n=[\begin{array}{cccc}0\hfill & 0\hfill & 0\hfill & 1\hfill \end{array}{]}^{\mathrm{T}}$ .

According to the schematic diagram of manipulator in 2D space, $a_i=L_i$ . The position coordinates of the manipulator gripper can be obtained by substituting equation (2) into equation (3)

$${}^0{\mathit{q}}_n=\left[\begin{array}{c}\sum _{i=1}^n\left(L_i\cos \left(\sum _{j=1}^i{\theta }_j\right)\right)\hfill \\ \multicolumn{1}{c}{\sum _{i=1}^n\left(L_i\sin \left(\sum _{j=1}^i{\theta }_j\right)\right)}\\ \multicolumn{1}{c}{0}\\ \multicolumn{1}{c}{1}\end{array}\right]$$

(4)

The path planning of the space manipulator

Issue description

To realize the reachability of the target position of the space manipulator, first, motion target of each joint of manipulator is programmed according to the target position. Second, the motion path of each joint is programmed according to the motion target of each joint. The variable superscript s, f, and d are defined, respectively, to represent the initial, final, and expected value. ${}^0{\mathit{q}}_n^f$ represents the description of gripper final position coordinates in the coordinate system coordinating with the satellite base. The relative deviation between the final position and the expected position of the manipulator gripper is obtained from equation (4)

$$\delta {}^0{\mathit{q}}_n={}^0{\mathit{q}}_n^f-{}^0{\mathit{q}}_n^d$$

(5)

Solving the problem of each joint of space manipulator motion target in 2D space is equivalent with the optimal solution which satisfies equality and inequality constraints

$$\{\begin{array}{c}{}^0{\mathit{q}}_n^f\left({\mathbf{\theta }}^d\right)={}^0{\mathit{q}}_n^d\hfill \\ \multicolumn{1}{c}{{\mathbf{\theta }}_{\min }\le {\mathbf{\theta }}^d\le {\mathbf{\theta }}_{\max }}\end{array}$$

(6)

where $\mathbf{\theta }=[\begin{array}{cccc}{\theta }_1\hfill & {\theta }_2\hfill & \cdots \hfill & {\theta }_n\hfill \end{array}{]}^{\mathrm{T}}$ . The expected motion target of each joint is to ensure the manipulator gripper to get the target position, that is, $\delta {}^0{\mathit{q}}_n\to 0$ .

After determining expected motion targets of each joint, in order to ensure the efficient and accurate movement of the manipulator toward the target, path planning in the process of manipulator is needed. According to the programmed joint expected movement target, the relative deviation between the final position and the expected position of the joint is

$$\delta \mathbf{\theta }={\mathbf{\theta }}^f-{\mathbf{\theta }}^d$$

(7)

The solution of joint motion path problem is to make a law of motion that satisfies the following constraints

$$\{\begin{array}{c}\mathbf{\theta }\left(t^s\right)={\mathbf{\theta }}^s,\mathbf{\theta }\left(t^f\right)={\mathbf{\theta }}^d\hfill \\ \multicolumn{1}{c}{\stackrel{·}{\mathbf{\theta }}\left(t^s\right)=\stackrel{··}{\mathbf{\theta }}\left(t^s\right)=0,\stackrel{·}{\mathbf{\theta }}\left(t^f\right)=\stackrel{··}{\mathbf{\theta }}\left(t^f\right)=0}\\ \multicolumn{1}{c}{{\mathbf{\theta }}_{\min }\le \mathbf{\theta }\le {\mathbf{\theta }}_{\max },{\stackrel{·}{\mathbf{\theta }}}_{\min }\le \stackrel{·}{\mathbf{\theta }}\le {\stackrel{·}{\mathbf{\theta }}}_{\max },{\stackrel{··}{\mathbf{\theta }}}_{\min }\le \stackrel{··}{\mathbf{\theta }}\le {\stackrel{··}{\mathbf{\theta }}}_{\max }}\end{array}$$

(8)

Each joint motion path should ensure the joint final movement state to reach the expected target as possible, that is, $\delta \mathbf{\theta }\to 0$ . There has no crash in the motor process of the robot arm’s joint. For the purpose of avoiding manipulator system’s collision problem, this article limits the movement of the joint which is ${\mathbf{\theta }}_{\min }\le \mathbf{\theta }\le {\mathbf{\theta }}_{\max }$ .

Cost function of the path planning

Redundant DOFs are often used for ensuring the on-orbit mission by space manipulator. Therefore, in the process of path planning of the space manipulator, the reachability of manipulator grippers should be taken into account. And the cost function should be established on the basis of the manipulator motion characteristics. For example, the force or torque required to drive the joint needs to be considered in the mission process of space manipulator. In accordance with the principle of minimum torque required for general motion, the cost function of optimal joint movement target is as follows

$$J={\left(\sum _{i=1}^n{({\theta }_i^f-{\theta }_i^s)}^2\right)}^{\frac{1}{2}}$$

(9)

Different joint has different moment of inertia, so it requires different torques when rotating the same angle. By the formula of moment of inertia $I=\sum _i{m}_i{r}_i^2$ , where $m_i$ is the mass of a mass element of rigid body and $r_i$ is the distance between the mass element and spindle. Moment of inertia of each joint is relative to the joint mass because each joint of manipulator system uses the same material which means the mass of manipulator system is equally distributed. Based on the mass of manipulator joints, the cost weights of different joint motions are set. The weighted cost functions of optimal joint movement target are as follows

$$J={\left(\sum _{i=1}^n{w}_i{({\theta }_i^f-{\theta }_i^s)}^2\right)}^{\frac{1}{2}}$$

(10)

$$w_i=\frac{\sum _{j=i}^n{m}_j}{\sum _{j=1}^n{m}_j}$$

(11)

where $m_j$ is the mass of joint j.

Considering both path planning description and the cost function

$$\{\begin{array}{c}\underset{{\mathbf{\theta }}^d}{\min }J\left({\mathbf{\theta }}^d\right)\hfill \\ \multicolumn{1}{c}{s.t.g_1\left({\mathbf{\theta }}^d\right)\ge 0,h_1\left({\mathbf{\theta }}^d\right)=0}\end{array}$$

(12)

where $g_1({\mathbf{\theta }}^d)$ and $h_1({\mathbf{\theta }}^d)$ separately represent the inequality and equality constraints of equation (5).

Planning algorithm

Optimal motion planning of joints

The optimal motion target planning of joints is mainly studied in this section. By analyzing mathematical planning problems (equation (13)), equation $h_1({\mathbf{\theta }}^d)=0$ is equivalent to $\{h_1({\mathbf{\theta }}^d)\ge 0,-h_1({\mathbf{\theta }}^d)\ge 0\}$ . Therefore, substituting equation (4) into equation (12), it can be obtained that

$$\{\begin{array}{c}\underset{{\mathbf{\theta }}^d}{\min }J\left({\mathbf{\theta }}^d\right)\hfill \\ \multicolumn{1}{c}{g_1^1\left({\mathbf{\theta }}^d\right)={\mathbf{\theta }}^d-{\mathbf{\theta }}_{\min }\ge 0}\\ \multicolumn{1}{c}{g_1^2\left({\mathbf{\theta }}^d\right)={\mathbf{\theta }}_{max}-{\mathbf{\theta }}^d\ge 0}\\ \multicolumn{1}{c}{g_1^3\left({\mathbf{\theta }}^d\right)=\sum _{i=1}^n\left(L_i\cos \left(\sum _{j=1}^i{\theta }_j\right)\right)\ge 0}\\ \multicolumn{1}{c}{g_1^4\left({\mathbf{\theta }}^d\right)=-\sum _{i=1}^n\left(L_i\cos \left(\sum _{j=1}^i{\theta }_j\right)\right)\ge 0}\\ \multicolumn{1}{c}{g_1^5\left({\mathbf{\theta }}^d\right)=\sum _{i=1}^n\left(L_i\sin \left(\sum _{j=1}^i{\theta }_j\right)\right)\ge 0}\\ \multicolumn{1}{c}{g_1^6\left({\mathbf{\theta }}^d\right)=-\sum _{i=1}^n\left(L_i\sin \left(\sum _{j=1}^i{\theta }_j\right)\right)\ge 0}\end{array}$$

(13)

So, the mathematical planning (equation (12)) can be transformed to an extreme value problem

$$\underset{{\mathbf{\theta }}^d\in R}{min}J\left({\mathbf{\theta }}^d\right),R=\{{\mathbf{\theta }}^d\in S|g_1^i\left({\mathbf{\theta }}^d\right)\ge 0,i=1,2,\dots ,6\}$$

(14)

Theorem 1

Considering the extreme value problem (equation (14)), $J({\mathbf{\theta }}^d)$ is lower semi-continuous real function of $\Theta \supseteq S$ , constraint set R ≠ Ø, and R is bounded closed set, there is an optimal solution of the extreme value $\underset{{\mathbf{\theta }}^d\in R}{\min }J({\mathbf{\theta }}^d)$ . ¹³

Proof

For R ≠ Ø, define $\overline{J}=\underset{{\mathbf{\theta }}^d\in R}{\inf }J({\mathbf{\theta }}^d)$ , so $-\infty \le \overline{J}\le +\infty$ . Take monotonic decrease series $\{J_k\}$ and $\underset{k\to \infty }{lim}{J}_k=\overline{J}$ . Hereby

$$J_k=\{\begin{array}{cc}\overline{J}+\frac{J_1-\overline{J}}{k}\hfill & \overline{J}>-\infty \hfill \\ \multicolumn{1}{c}{-\mathrm{ak}}& \overline{J}=-\infty \hfill \end{array}$$

where $J_1-\overline{J}>0$ , $a>0$ .

From the definition of the lower bound, it is shown that there are $\{{\mathbf{\theta }}^{\mathrm{dk}}\}$ and ${\theta }^{\mathrm{dk}}\in R$ which satisfy

$$J\left({\mathbf{\theta }}^{\mathrm{dk}}\right)<J_k,k=1,2,\dots$$

For $\{{\mathbf{\theta }}^{\mathrm{dk}}\}$ bounded point series, accumulation point ${\overline{\mathbf{\theta }}}^d\in R$ definitely exists. With generality, supposing that

$$\underset{k\to \infty }{lim}{\mathbf{\theta }}^{\mathrm{dk}}={\overline{\mathbf{\theta }}}^d$$

It can be proved that $J({\overline{\mathbf{\theta }}}^d)\le J_k,k=1,2,\dots$ . Actually, if $k_0,(k_0\ge 1)$ , then

$$J\left({\mathbf{\theta }}^d\right)>J_{k_0}$$

From lower semi-continuity of $J({\mathbf{\theta }}^d)$ , for $\epsilon =(J({\overline{\mathbf{\theta }}}^d)-J_{k_0})/2>0$ , there is $\delta >0$ , when ${\mathbf{\theta }}^d\in N_{\delta }({\overline{\mathbf{\theta }}}^d)$ , $N_{\delta }({\overline{\mathbf{\theta }}}^d)$ is

$$J\left({\mathbf{\theta }}^d\right)>J\left({\overline{\mathbf{\theta }}}^d\right)-\epsilon =\frac{J\left({\overline{\mathbf{\theta }}}^d\right)+J_{k_0}}{2}>J_{k_0}$$

For $K>k_0$ exist in $\underset{k\to \infty }{lim}{\mathbf{\theta }}^{\mathrm{dk}}={\overline{\mathbf{\theta }}}^d$ , when $k>K$ , ${\mathbf{\theta }}^{\mathrm{dk}}\in N_{\delta }({\overline{\mathbf{\theta }}}^d)$ , so

$$J\left({\mathbf{\theta }}^{\mathrm{dk}}\right)>J_{k_0}\ge J_k>J\left({\mathbf{\theta }}^{\mathrm{dk}}\right)$$

The former equation is in contradiction.

Therefore, ${\overline{\mathbf{\theta }}}^d\in R$ so that $\overline{J}\le J({\overline{\mathbf{\theta }}}^d)\le \underset{k\to \infty }{\lim }{J}_k=\overline{J}$ . It can be obtained that $\overline{J}=J({\overline{\mathbf{\theta }}}^d)\ne -\infty$ , ${\overline{\mathbf{\theta }}}^d$ is the optimal solution of $\underset{{\mathbf{\theta }}^d\in R}{\min }J({\mathbf{\theta }}^d)$ .

Suppose $f_i({\mathbf{\theta }}^d)=\sqrt{w_i}({\mathit{A}}_i{\mathbf{\theta }}^d+{\mathit{B}}_i)$ , where

$$\begin{array}{c}{\mathit{A}}_i=\left[\begin{array}{cccc}{a}_{i1}\hfill & 0\hfill & \cdots \hfill & 0\hfill \\ \multicolumn{1}{c}{0}& a_{i2}\hfill & \cdots \hfill & 0\hfill \\ \multicolumn{1}{c}{⋮}& ⋮\hfill & \ddots \hfill & ⋮\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & \cdots \hfill & a_{\mathrm{in}}\hfill \end{array}\right],a_{\mathrm{ij}}=\{\begin{array}{cc}1\hfill & i=j\hfill \\ \multicolumn{1}{c}{0}& i\ne j\hfill \end{array}\hfill \\ {\mathit{B}}_i=\left[\begin{array}{c}{b}_{i1}\hfill \\ \multicolumn{1}{c}{b_{i2}}\\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{b_{\mathrm{in}}}\end{array}\right],b_{\mathrm{ij}}=\{\begin{array}{cc}-{\theta }_i^s\hfill & i=j\hfill \\ \multicolumn{1}{c}{0}& i\ne j\hfill \end{array}\hfill \end{array}$$

Extreme value equation (14) can be transformed to

$$\{\begin{array}{c}\underset{{\mathbf{\theta }}^d\in R}{\min }f\left({\mathbf{\theta }}^d\right)\hfill \\ \multicolumn{1}{c}{f\left({\mathbf{\theta }}^d\right)=\sum _{i=1}^n{f}_i{\left({\mathbf{\theta }}^d\right)}^2}\\ \multicolumn{1}{c}{F\left({\mathbf{\theta }}^d\right)={[f_1\left({\mathbf{\theta }}^d\right),\dots ,f_n\left({\mathbf{\theta }}^d\right)]}^{\mathrm{T}}}\\ \multicolumn{1}{c}{R=\{{\mathbf{\theta }}^d|g_1\left({\mathbf{\theta }}^d\right)={[g_1^1\left({\mathbf{\theta }}^d\right),\dots ,g_1^6\left({\mathbf{\theta }}^d\right)]}^{\mathrm{T}}\ge 0\}}\end{array}$$

(15)

From analyzing, $f_i({\mathbf{\theta }}^d)(i=1,2,\dots ,n)$ and $-g_1^j({\mathbf{\theta }}^d)(i=1,2,\dots ,6)$ are first-order continuous functions.

Suppose that existing ${\mathbf{\theta }}^{d0}\in R$ , which makes $L_0=\{{\mathbf{\theta }}^d|{\mathbf{\theta }}^d\in R,f({\mathbf{\theta }}^d)\le f({\mathbf{\theta }}^{d0})\}$ bounded, and ${\mathbf{\theta }}^{d*}\in {\mathit{E}}_n$ , which makes $g_1({\mathbf{\theta }}^{d*})\ge 0$ .

Estimating $c>0$ that can make $L_0\subseteq \{{\mathbf{\theta }}^d||{\mathbf{\theta }}_i^d|\le c,i=1,2,\dots ,n\}$ , we suppose $\delta >0$ , where

$$\begin{array}{c}{\mathit{F}}_{{\mathbf{\theta }}^d}\left({\mathbf{\theta }}^d\right)={[f_{1{\mathbf{\theta }}^d}\left({\mathbf{\theta }}^d\right),f_{2{\mathbf{\theta }}^d}\left({\mathbf{\theta }}^d\right),\dots ,f_{n{\mathbf{\theta }}^d}\left({\mathbf{\theta }}^d\right)]}^{\mathrm{T}}\hfill \\ {\mathit{f}}_{i{\mathbf{\theta }}^d}\left({\mathbf{\theta }}^d\right)=[\frac{\partial f_i\left({\mathbf{\theta }}^d\right)}{\partial {\theta }_1^d},\frac{\partial f_i\left({\mathbf{\theta }}^d\right)}{\partial {\theta }_2^d},\dots ,\frac{\partial f_i\left({\mathbf{\theta }}^d\right)}{\partial {\theta }_n^d}]\hfill \\ E_k=E({\mathbf{\theta }}^{\mathrm{dk}},\delta )=\{j|0\le g_1^j\left({\mathbf{\theta }}^{\mathrm{dk}}\right)<\delta ,1\le j\le 6\}=\{j_1,\dots ,j_s\}\hfill \\ E({\mathbf{\theta }}^{\mathrm{dk}},0)=\{j|g_1^j\left({\mathbf{\theta }}^{\mathrm{dk}}\right)=0,1\le j\le j_6\}\hfill \\ F_k=F({\mathbf{\theta }}^{\mathrm{dk}},\delta )=\{j|g_1^j\left({\mathbf{\theta }}^{\mathrm{dk}}\right)\ge \delta ,1\le j\le 6\}=\{j_{s+1},\dots ,6\}\hfill \\ g_{E_k}\left({\mathbf{\theta }}^d\right)={[g_1^{j_1}\left({\mathbf{\theta }}^d\right),\dots ,g_1^{j_s}\left({\mathbf{\theta }}^d\right)]}^{\mathrm{T}}\hfill \\ g_{F_k}\left({\mathbf{\theta }}^d\right)={[g_1^{j_{s+1}}\left({\mathbf{\theta }}^d\right),\dots ,g_1^{j_6}\left({\mathbf{\theta }}^d\right)]}^{\mathrm{T}}\hfill \\ \mathit{e}={[1,\dots ,1]}^{\mathrm{T}}\hfill \end{array}$$

The following iteration method is designed to get the optimal solution of equation (15).

Take the allowable error of the optimal solution $\epsilon >0$ and possibly the minimum allowable error of constrained boundary $\delta >0$ ; take matrix $\mathit{P}$ , $\mathit{P}$ as the matrix of $n\times n$ and ${\mathit{P}}^{\mathrm{T}}\mathit{P}$ is positive definite:^14,15

• 1. Take the initial value point ${\mathbf{\theta }}^{d0}\in R$ , $k:0$ .

• 2. Suppose ${\mathbf{\theta }}^{\mathrm{dk}}$ is given. If $g_1({\mathbf{\theta }}^{\mathrm{dk}})>0$ , get the optimal solution ${\phi }^k$ of $\underset{\in E_n}{\min }\{||F({\mathbf{\theta }}^{\mathrm{dk}})+F_{{\mathbf{\theta }}^d}({\mathbf{\theta }}^{\mathrm{dk}})(\varphi -{\mathbf{\theta }}^{\mathrm{dk}})||+||\mathit{P}(\varphi -{\mathbf{\theta }}^{\mathrm{dk}})|{|}^2\}$ ; if $0\le g_{E_k}({\mathbf{\theta }}^{\mathrm{dk}})<\delta e,g_{F_k}({\mathbf{\theta }}^{\mathrm{dk}})\ge \delta e$ , then

$$\{\begin{array}{c}\underset{\in R_k}{\min }\{{\Vert F\left({\mathbf{\theta }}^{\mathrm{dk}}\right)+F_{{\mathbf{\theta }}^d}\left({\mathbf{\theta }}^{\mathrm{dk}}\right)(\varphi -{\mathbf{\theta }}^{\mathrm{dk}})\Vert }^2+{\Vert \mathit{P}(\varphi -{\mathbf{\theta }}^{\mathrm{dk}})\Vert }^2\}\hfill \\ \multicolumn{1}{c}{R_k=\{|g_{E_k}\left({\mathbf{\theta }}^{\mathrm{dk}}\right)(\varphi -{\mathbf{\theta }}^{\mathrm{dk}})\ge -F{\left({\mathbf{\theta }}^{\mathrm{dk}}\right)}^{\mathrm{T}}{F}_{{\theta }^d}\left({\mathbf{\theta }}^d\right)(\varphi -{\mathbf{\theta }}^{\mathrm{dk}})e,\left|{}_i\right|\le c,i=1,\dots ,n\}}\end{array}$$

Get the optimal solution ${\phi }_k$ . If $||{\varphi }^k-{\mathbf{\theta }}^{\mathrm{dk}}||<\epsilon$ , iteration suspends, ${\theta }^{\mathrm{dk}}$ is the approximate solution; otherwise, proceed to step 3.

• 3. If $||{\varphi }^k-{\mathbf{\theta }}^{\mathrm{dk}}||\ge \epsilon$ , get the optimal solution ${\lambda }^k$ of $\min \{||F({\mathbf{\theta }}^{\mathrm{dk}}+\lambda ({\varphi }^k-{\mathbf{\theta }}^{\mathrm{dk}}))|{|}^2|{\mathbf{\theta }}^{\mathrm{dk}}+\lambda ({\varphi }^k-{\mathbf{\theta }}^{\mathrm{dk}})\in R|\}$ . Make ${\mathbf{\theta }}^{d(k+1)}={\mathbf{\theta }}^{\mathrm{dk}}+{\lambda }^k({\varphi }^k-{\mathbf{\theta }}^{\mathrm{dk}})$ ; $k:k+1$ ; proceed to step 2.

Path planning of the joint motion

For realizing joint path planning, according to the angular velocity change of joint motion, the joint motion path is divided into three phases: acceleration, uniform, and deceleration phases.^16–18 The angular acceleration in the process of acceleration phase of joint motion is ${\mathit{A}}_1\sin (\tilde{\mathbf{\omega }}t+{\varphi }_1)$ , 0 in the uniform phase, and ${\mathit{A}}_2\sin (\tilde{\mathbf{\omega }}t+{\varphi }_2)$ in the deceleration phase. Through the above three phases of joint movement changes, eventually the manipulator joint stabilizes at the target position.

The angular acceleration expression of sinusoidal angular acceleration joint motion path is defined as

$$\{\begin{array}{cc}{a}_{1\max }\sin \left(\frac{\pi }{T_1}t\right)\hfill & 0\le t\le T_1\hfill \\ \multicolumn{1}{c}{0}& T_1<t<T_1+T_2\hfill \\ \multicolumn{1}{c}{-a_{2\max }\sin \left(\frac{\pi }{T_3}(t-T_1-T_2)\right)}& T_1+T_2\le t\le T_1+T_2+T_3\hfill \end{array}$$

(16)

Integrating the above angular acceleration, the angular velocity expression of sinusoidal angular acceleration joint motion path is as follows

$$\{\begin{array}{cc}-\frac{T_1}{\pi }{a}_{1\max }\cos \left(\frac{\pi }{T_1}t\right)+\frac{T_1}{\pi }{a}_{1\max }\hfill & 0\le t\le T\hfill \\ \multicolumn{1}{c}{\frac{2T_1}{\pi }{a}_{1\max }}& T_1<t<T_1+T_2\hfill \\ \multicolumn{1}{c}{\frac{2T_1}{\pi }{a}_{1\max }-\frac{T_3}{\pi }{a}_{2\max }(1-\cos \left(\frac{\pi }{T_3}(t-T_1-T_2)\right))}& T_1+T_2\le t\le T_1+T_2+T_3\hfill \end{array}$$

(17)

Integrating the above angular acceleration, the angular position expression of sinusoidal angular acceleration joint motion path is as follows

$$\{\begin{array}{cc}\frac{T_1}{\pi }{a}_{1\max }t-\frac{T_1^2}{{\pi }^2}{a}_{1\max }\sin \left(\frac{\pi }{T_1}t\right)\hfill & 0\le t\le T\hfill \\ \multicolumn{1}{c}{\frac{2T_1}{\pi }{a}_{1\max }t-\frac{T_1^2}{\pi }{a}_{1\max }}& T_1<t<T_1+T_2\hfill \\ \multicolumn{1}{c}{\frac{T_1}{\pi }{a}_{1\max }(2t-T_1)-\frac{T_3}{\pi }{a}_{2\max }(t-\frac{T_3}{\pi }\sin \left(\frac{\pi }{T_3}(t-T_1-T_2)\right)-T_1-T_2)}& T_1+T_2\le t\le T_1+T_2+T_3\hfill \end{array}$$

(18)

Three phases in expressions (16)–(18), respectively, represent acceleration phase $t\in [0,T_1)$ , uniform phase $t\in [T_1,T_1+T_2)$ , and deceleration phase $t\in [T_1+T_2,T_1+T_2+T_3)$ . In expressions, $T_1$ , $T_2$ , and $T_3$ are the durations of acceleration, uniform, and deceleration phases; $a_{1\max }$ and $a_{2\max }$ are the maximum accelerated velocities of acceleration phase and deceleration phase; optimal parameters are selected using optimization algorithm for joint path planning of manipulators. The following steps are taken in planning the optimal parameter of the joint:

• 1. Confirming $a_{1\max }$ and $a_{2\max }$

It is considered that $a_{1\max }=a_{2\max }=a_{\max }$ , where $a_{\max }$ is the maximum angular acceleration of manipulator joints.

• 2. Confirming $T_1$ and $T_3$

From expressions (8), (16)–(18), it is known that

$$T_1=T_3=\frac{\pi }{2}·\frac{{\nu }_{\max }}{a_{\max }}$$

where ${\nu }_{\max }$ is the maximum angular velocity of manipulator joints.

• 3. Revising $a_{1\max }$ and $a_{2\max }$

When the joint movement angle is relatively small, adapting the maximum joint angular acceleration will make the joint angle motion over large. So, $a_{1\max }$ and $a_{2\max }$ are revised by $T_1$ and $T_3$

$$a_{1\max }=a_{2\max }=\frac{\pi }{2}·\frac{|{\mathbf{\theta }}^d-{\mathbf{\theta }}^s|}{T_1^2}$$

• 4. Confirming $T_2$

$$T_2=\frac{1}{2}(T_1+(|{\mathbf{\theta }}^d-{\mathbf{\theta }}^s|+\frac{T_3^2}{\pi }{a}_{1\max })\frac{\pi }{T_1{a}_{2\max }})$$

Simulations

The 6-DOF manipulator with six joints in 2D space displayed in Figure 1 is used in simulation to verify the algorithm of the path planning of space manipulator based on Gauss–Newton iteration method. Table 1 shows the mass and length of each joint of the space manipulator. In the process of path planning simulation, first, the designed algorithm of joint optimal motion planning is adopted to program the target angle of each joint motion and then based on the optimal joint moving target, make path planning of the joint motion in accordance with the joint motion path planning algorithm. Parameter settings of the path planning algorithm for the space manipulator are as follows

$$\begin{array}{c}{}^0{\mathit{q}}_n^s={\left[\begin{array}{cccc}2.2265\hfill & 1.8948\hfill & 0\hfill & 1\hfill \end{array}\right]}^{\mathrm{T}}\hfill \\ {}^0{\mathit{q}}_n^d={\left[\begin{array}{cccc}2.5568\hfill & 0.3110\hfill & 0\hfill & 1\hfill \end{array}\right]}^{\mathrm{T}}\hfill \\ {\mathbf{\theta }}^s={\left[\begin{array}{cccccc}-60\circ \hfill & 0\hfill & 45\circ \hfill & 0\hfill & -15\circ \hfill & 0\hfill \end{array}\right]}^{\mathrm{T}}\hfill \\ \mathbf{\theta }\in [-70\circ ,70\circ ]\hfill \\ a_{\max }={\left[\begin{array}{cccccc}0.002\hfill & 0.004\hfill & 0.01\hfill & 0.012\hfill & 0.02\hfill & 0.024\hfill \end{array}\right]}^{\mathrm{T}}\hfill \\ {\nu }_{\max }={\left[\begin{array}{cccccc}0.02\hfill & 0.04\hfill & 0.1\hfill & 0.12\hfill & 0.2\hfill & 0.24\hfill \end{array}\right]}^{\mathrm{T}}\hfill \\ \delta =\frac{\pi }{180},\epsilon =0.0005\hfill \end{array}$$

Table 1.

Parameters of space manipulator.

	Base	Joint 1	Joint 2	Joint 3	Joint 4	Joint 5	Joint 6
$m(\mathrm{kg})$	200	4	8	2	6	2	2
$L(\mathrm{m})$	1.084	0.281	1.365	0.174	0.967	0.174	0.174

Results are found by the optimal joint moving object planning algorithm. The coordinate is ${}^0{\mathit{q}}_6^f=[\begin{array}{cccc}2.5571\hfill & 0.3109\hfill & 0\hfill & 1\hfill \end{array}{]}^{\mathrm{T}}$ according to the manipulator gripper in 2D space. Angle motion of each joint is ${\mathbf{\theta }}^f=[\begin{array}{cccccc}-42\circ \hfill & 21\circ \hfill & 61\circ \hfill & 10\circ \hfill & -22\circ \hfill & -10\circ \hfill \end{array}{]}^{\mathrm{T}}$ , where the value of cost function is $J({\mathbf{\theta }}^f)=2.5331$ . According to the optimal joint motion target, the path planning is proceeded by the path planning algorithm of joints. Results are displayed in Figures 3 and 4.

Figure 3.

Trajectory of space manipulator joint planning: (a) joint 1, (b) joint 2, (c) joint 3, (d) joint 4, (e) joint 5, and (f) joint 6.

Figure 4.

Planned trajectory of gripper: (a) position change curve of gripper and (b) motion trajectory of gripper.

Figures 3 and 4 show it takes about 32 s to reach the target position using the algorithm. The trajectory of joint motion indicates the planned path is steadier without great vibration. The algorithm can ensure the gripper reach the target position fast and steadily.

For further verifying the performance of the path planning algorithm of the space manipulator, simulations for stretching of the space manipulator are made. Figure 5 shows the schematic diagram of the manipulator in retraction; Figure 6 shows the schematic diagram of the manipulator in complete expansion.

Figure 5.

The schematic diagram of the manipulator in retraction.

Figure 6.

The schematic diagram of the manipulator in complete expansion.

The unexpanded position coordinate of the manipulator is

$${}^0{\mathit{q}}_6^p={\left[\begin{array}{cccc}0.4672\hfill & -0.2627\hfill & 0\hfill & 1\hfill \end{array}\right]}^{\mathrm{T}}$$

The unexpanded angles of the manipulator joints are

$${\mathbf{\theta }}^p={\left[\begin{array}{cccccc}70\circ \hfill & -70\circ \hfill & -70\circ \hfill & -70\circ \hfill & -70\circ \hfill & -70\circ \hfill \end{array}\right]}^{\mathrm{T}}$$

The expanded position coordinate of the manipulator is

$${}^0{\mathit{q}}_6^u={\left[\begin{array}{cccc}2.8540\hfill & 0\hfill & 0\hfill & 1\hfill \end{array}\right]}^{\mathrm{T}}$$

The expanded angles of the manipulator joints are

$${\mathbf{\theta }}^u={\left[\begin{array}{cccccc}0\circ \hfill & 0\circ \hfill & 0\circ \hfill & 0\circ \hfill & 0\circ \hfill & 0\circ \hfill \end{array}\right]}^{\mathrm{T}}$$

Other simulation parameters are referred below. The path planning algorithm in this article is adopted in path planning of the space manipulator in expansion and retraction. Figures 7 and 8 show simulation results for expansion, and Figures 9 and 10 show simulation results for retraction.

Figure 7.

Joint planning trajectory of space manipulator: (a) joint 1, (b) joint 2, (c) joint 3, (d) joint 4, (e) joint 5, and (f) joint 6.

Figure 8.

The trajectory of the gripper in expansion: (a) position change curve of gripper and (b) motion trajectory of gripper.

Figure 9.

The planning trajectory of the manipulator joints in retraction: (a) joint 1, (b) joint 2, (c) joint 3, (d) joint 4, (e) joint 5, and (f) joint 6.

Figure 10.

The planning trajectory of the gripper in retraction: (a) position change curve of gripper and (b) motion trajectory of gripper.

Simulation planning results show that the angular velocity of joint 1 and joint 2 can reach the maximum value in the motion and finish stretch mission rapidly within 80 s at the maximum angular velocity; joints 3, 4, 5, and 6 complete the motion mission without reaching the maximum speed of the joint. The simulation results indicate the path planning algorithm can simultaneously ensure the rapidity, stability, and accuracy in the manipulator motion completing process.

Conclusion

A path planning algorithm based on Gauss–Newton iteration method is designed for the path planning of space manipulators to ensure the reachability of the gripper target position:

1. The dynamic inverse solution of motion from the gripper to the redundant joints is transformed into minimal value problem with constraints and proves the existence of the optimal solution of the extreme value problem. Furthermore, the optimal motion target of the manipulator is obtained using the iterative calculation.

2. The joint motion path is set as a sinusoidal motion; the joint motion path planning is transferred to an optimal parameter problem. Therefore, the optimal joint motion path and further the path planning of the gripper are obtained.

3. Compared with the conventional searching algorithm, the path planning of space manipulator is based on Gauss–Newton iteration method and D-H model. It combines the optimal joint motion target and optimal joint motion path, meanwhile ensures the optimal path and the planning accuracy of grippers at the end of the manipulators.