Zhiqiang Xu, Xiangxin Zeng, Jinghui Li and Zhaoyan Jia

(Norinco Group Air Ammunition Research Institute Co., Ltd., Harbin 150030, China)

Abstract: For the problem of free-floating space robot (FFSR) that the motion of manipulator will cause a large disturbance to the attitude of satellite, a path planning method based on hp-adaptive Gauss pseudospectral method (hp-AGPM) is proposed in this paper. In this method, the minimum reaction torque acting on satellite is taken as the objective function, and the number of segments and the order of polynomial in each segment are determined adaptively to improve the accuracy and the efficiency of the solution. At the same time, the theoretical convergence of the designed method is innovatively proved to ensure that the solution of the discretized nonlinear programming (NLP) problem is the optimal solution to the original optimal problem. The simulation results of a planar two degree-of-freedom (2-DOF) space manipulator show that the proposed path planning method is more effective than the resolved acceleration control (RAC) method and the control variable parameterization (CVP) method, and is better than other pseudospectral methods both in computation speed and the number of collocation points.

Keywords: free-floating space robot; hp-adaptive Gauss pseudospectral method; path planning; nonlinear programming

0 Introduction

With the increasingly fierce competition in space technology around the world, the FFSRs are playing an increasingly important role in the field of on-orbit service (OOS), such as transporting payloads, recovering and releasing satellites, maintaining orbit spacecraft and capturing space debris[1-3]. Because the FFSR is in the condition of momentum conservation, its dynamics and kinematics are strongly coupled, which makes the motion path planning of FFSR more complicated[4-5]. In other words, the FFSR system obeys the law of conservation of momentum in space environment, so the motion of the manipulator will cause a large attitude disturbance to the satellite[6-7]. The disturbance of the satellite attitude will have an effect on such tasks as the communications and the earth-observation[8-9]. Meanwhile, it will also bring a huge challenge to the attitude control system. Therefore, it is of great significance to study the path planning of the FFSR with minimal attitude disturbance to the satellite in order to complete the service task of OOS.

A variety of optimization methods and path planning strategies have been developed for the path planning problem of the FFSR. Rybus[10]presented a path planning scheme based on bidirectional rapidly-exploring random trees (RRT) algorithm, and the purpose is to make the desired change of satellite attitude. Okubo et al.[11]designed a path planning method using enhanced disturbance map (EDM) method that reduces attitude disturbance of the base satellite. Flores-Abad et al.[12-13]worked on path planning of the FFSR to capture a space non-cooperative rolling target. Wang et al.[14]investigated the path planning of the kinematically redundant FFSR based on the Partible Swarm Optimization (PSO) strategy.

The path planning problem of the FFSR is usually described as the optimal control problem (OCP), and the methods of solving OCP are generally classified into two kinds: the indirect method and the direct method[15]. In the indirect method, the OCP is transformed into the Hamilton boundary value problem, and then the Hamilton boundary value problem is solved by the maximum principle. The indirect method has the advantage of solving the optimization problem with high accuracy, but it has small convergence domain and complicated solution process, and it is inefficient to solve the OCP with path constraints. In contrast, the direct method has the advantages of wide convergence domain and low initial value estimation. It does not need to guess the values of the initial covariant variables accurately, uses the discretization method to transform the OCP into the NLP, and then optimizes the performance index by the numerical method. The Gauss pseudospectral method (GPM) is the most well-developed pseudospectral methods in solving NLP problems. As a branch of direct method, the GPM develops extremely rapidly in recent years, and applies widely in complex OCP, such as aircraft trajectory optimization[15], launch vehicle trajectory optimization[16-17], spacecraft formation reconfiguration[18]and space robot path planning[19].

Compared with the traditional direct method, the GPM has the characteristics of fast convergence. However, when the OCP is non-smooth, the p-method has a slow convergence rate, and even cannot solve the OCP which requires high accuracy. The h-method may require using a large number of segments to achieve an acceptable error tolerance. Therefore, Darby et al.[20-21]proposed an hp-AGPM to get the solution of the OCP. In this method, a two-tiered strategy is used to determine the number of segments and the order of the polynomial, and the purpose is to achieve a specified solution accuracy in each segment.

This paper focus on the path planning of the FFSR with minimum attitude disturbance of the satellite by using the hp-AGPM and analyses the convergence of the proposed method. The remainder of this paper is organized as follows. In Section 1, the general kinematics and dynamics models of the FFSR are outlined. Then the path planning problem of the minimum reaction torque of the satellite is established in the form of OCP. In Section 2, the theories and associated equations in the hp-AGPM are introduced. In Section 3, the convergence analysis of hp-AGPM is provided. In Section 4, the numerical simulations of a simplified planar 2-DOF FFSR are shown to validate the effectiveness of the proposed method. Conclusions are drawn in Section 5.

1 Path Planning Model

1.1 Kinematics and Dynamics Model

This paper makes the following assumptions about the FFSR system: 1) the FFSR system consists of a rigid satellite and n rigid manipulator links; 2) each joint has only one rotational degree-of-freedom; 3) in the process of the capture maneuver, the attitude and position of the satellite is not controlled.

The kinematics and the dynamics models of the FFSR system are established in the inertial reference coordinate systemoxIyIzI, as shown in Fig.1.The position vector of the manipulator’s end-effector is described as

(1)

wherereandr0are the position vector of the end-effector and the centroid of satellite, respectively;l0andli(i=1,2,…,n) are the position vector of the first joint with respect to the centroid of satellite and the linki, respectively.

Fig.1 Configuration of the FFSR

For ease of illustration, the planar 2-DOF FFSR model is used to verify the design method, which is described in Fig. 2. Definere=[xe,ye]Tandr0=[x0,y0]T, the kinematics equations of the 2-DOF FFSR are as follows:

(2)

whereq0is the angle of satellite andqi(i=1,2) is the joint angle of jointi.

The linear velocity vector of the end-effector inoxIyIzIis obtained by the derivative of time of Eq.(2).

(3)

Fig. 2 Configuration of the 2-DOF FFSR

In deep space environment, the gravity and other external forces have little influence on the FFSR, so they are neglected in the process of analysis. Therefore, the momentum of the FFSR system are conserved. Suppose the generalized coordinateq=[q0,q1,q2]T.The dynamics equation of the FFSR system is derived as follows according to the Lagrange equation:

(4)

1.2 Bolza Optimal Control Model of Path Planning Problem

In this paper, the purpose of path planning of the end-effector is to obtain a special trajectory from a given initial position to the target capturing position, and minimize reaction torque of the satellite at the same time. Furthermore, the total reaction torqueurcaused by the manipulator motion at the satellite is defined as

(5)

wheremiandviare the mass and the translational velocity of linki, respectively.

To find the set of optimal control torques, the objective function of OCP is chosen as

(6)

wheret0andtfare the initial and the final moment of the manipulator motion, respectively.

(7)

where

The OCP of manipulator motion can be described as

(8)

Subject to

whereq0andqfare the vectors of joint angles in the initial and final moment, respectively;xminandxmaxare the lower boundary and the upper boundary of the state variable, respectively;uminandumaxare the lower boundary and the upper boundary of the control variable, respectively.

The time interval of Bolza OCP isτ∈[-1,1], so the time intervalt∈[t0,tf] is necessary to be transformed toτ∈[-1,1] via the below affine transformation.

(9)

Then, the objective function described by Eq.(6) can be changed to

(10)

The dynamics equation Eq.(7) is given in terms ofτas follows:

(11)

T7he OCP is transformed to the following Bolza form:

(12)

Subject to

2 Optimal Motion Planning

2.1 Discretization Based on Gauss Pseudospectral Method

The GPM is a discretization method approximating the state and control trajectories using interpolating polynomials. There are (N+1) discrete points in the GPM which are composed of the initial pointτ0=-1 and theNLG points, where the LG points are the zeroes of Nth-order Legendre polynomial. The state variables and control variables are approximated at the discrete points as

(13)

whereU(τ) andX(τ) are the mappings of the control variableuand the state variablexon the time intervalτ∈[-1,1], respectively;Li(τ) is a basis function of Lagrange polynomials and given by

(14)

Solving Eq.(11) by Gauss quadrature formula, there is

(15)

The terminal state of system is approximated by

(16)

whereωiis the Gauss weights.

The path constraints at each discrete point is approximated by

(17)

Then, Eq.(10) is approximated as the following function by using an LG quadrature.

(18)

In this way, the optimization problem of the minimum reaction torque of the base satellite is discretized into a NLP problem.

2.2 hp-AGPM Method

The hp-AGPM is a method in which the segments number and the polynomial order in each segment are determined adaptively. It can provide accurate approximation for solving Bolza OCP.

It is assumed that t∈[t0,tf] of the manipulator motion is divided into S segments, and the corresponding time interval of segmentsis [ts-1,ts]， wheres∈{1,…,S},t0

In segments, the time interval is transformed to the intervalτ∈[-1,1] by the following affine transformation:

(19)

Then, the OCP Eq.(12) is approximated to the following NLP function:

(20)

Subject to

(21)

Let peak-to-average ratersbe the metric to determine whether to increase the collocation points in segmentsor subdivide the segments, and it is calculated as

(22)

The iterative procedure of the hp-AGPM is as follows:

1)Initialize the parameters and chooseMcollocation points.

2)The Bolza OCP is discretized on the collocation points, then the OCP is converted to a discrete NLP problem.

3)Solve the discrete NLP problem.

4)Determine whether the constraints of boundary, path and dynamic in each segment are satisfied to the maximum allowable toleranceε. For all constraints are out of the given tolerance, proceed to Step 5), or terminate the iteration.

6)Increase the number of collocation points, i.e., increase the order of the Lagrange interpolation polynomial, then return to Step 3).

7)Divide the segment into multiple segments, then return to Step 3).

The iterative flow chart of the hp-AGPM is presented as follows.

Fig. 3 The iterative flow chart of hp-AGPM

3 Convergence Analysis for hp-AGPM

This section provides the proof of the convergence theorem for the designed hp-AGPM. To analyze the convergence of hp-AGPM, the following assumptions are employed:

Assumption1: The continuous optimal problem Eq. (12) has a local optimal solution (x*,u*)∈Cl+1(Rn)×L∞(Rm) forl≥3. There is an open set Γ?Rn×Rmandρ>0 satisfying the following function.

Βρ(x*(t),u*(t))?Γor allt∈[-1,1]

(23)

whereBρ(×) is the closed ball set whose radius isρa(bǔ)nd centers at ×,Ckis the collection of real-valuedktimes continuously differential functions on the interval [-1,+1], andL∞denotes the space of essentially bounded functions.

Furthermore, there exits associated covariateλ*∈Cl+1(Rn) andμ*∈Rn, which satisfies the following equations (Pontryagin’s minimum principle):

λ*(-1)=μ*

(24)

(25)

0=?uH(x*(t),u*(t),λ*(t))

(26)

whereHis the Hamiltonian and its specific expression is

H(x*,u*,λ*)=〈λ*,f(x*,u*)〉

(27)

where 〈ζ,ξ〉 is the inner product ofζandξ.

Assumption2: For someα>0, the smallest eigenvalues of the following matrices are greater than or equal toα:

V=?xxJ(x*(t))

(28)

(29)

where

Q(t)=?xxH(x*(t),u*(t),λ*(t))

S(t)=?uxH(x*(t),u*(t),λ*(t))

R(t)=?uuH(x*(t),u*(t),λ*(t))

The vector sequencesX*(s),U*(s)andλ*(s)are defined as

(30)

where 1≤i≤N.

If the following theorem is established, the method designed is convergent.

(31)

whereSis the number of segments,his the length between adjacent collocation points,Nis the number of collocation points on each segment,lis the continuous differentiable order of the state variable, andcis a constant.

Proof: Letχbe a Banach space andγbe a linear normed space. The norm in both spaces are described by ‖ · ‖. In order to prove Theorem 1, several lemmas presented in Ref. [23] are introduced first.

Lemma1: If Assumptions 1-3 holds, for eachε>0, there isr>0 such that for allθ∈Br(θ*),

‖?T(θ)-?T(θ*)‖≤ε

(32)

where ‖ · ‖ is the matrix norm induced by theL∞norm onχandγ, andris independent ofN.

Lemma2: If Assumptions 1-3 hold, ‖?T(θ)-1‖ is bounded by a constant independent ofNandK.

If Lemma 1 and Lemma 2 hold, the following proposition exists according Ref. [24]:

Proposition1: Let Τ:χ→γ.Tis continuously Fréchet differentiable inBr(θ*) for someθ*∈χandr>0. Ifεμ<1, whereμ=‖?T(θ*)-1‖ and ‖T(θ*)‖≤(1-με)r/μ, there is a uniqueθ∈Br(θ*) that makesT(θ)=0. Moreover, the following estimation inequality holds:

(33)

and 1≤s≤S. Letθ*=(X*,U*,λ*), where the tuples are similar toθ. The norm is the discreteL∞norm given by

‖θ‖=‖(X,U,λ)‖∞=

max{‖X‖∞,‖U‖∞,‖λ‖∞}

(34)

The mappingTis given as follows:

(35)

Lemma3: If Assumption 1 holds, there is a constantcindependent ofNandS.Thus

(36)

The proof of Lemma 3 is given in Ref.[23].

4 Simulation Example

In this part, the planar 2-DOF FFSR presented in Section 1 is used to verify the designed hp-AGPM, which is widely used in the verification of the new method. It is sufficient to comprehensively illustrate the proposed path planning method and to demonstrate its capabilities.

The configuration of the 2-DOF FFSR is shown in Fig. 2 and its mass and geometrical properties are listed in Table 1.

Table 1 Parameters of the 2-DOF FFSR

To evaluate performance of the proposed hp-AGPM, the obtained results were compared with the results of RAC and CVP methods presented in Refs.[25-26]. The results are shown in Fig.4 to Fig.6.

Fig.4 Trajectory of the end-effector

Fig. 4 presents the comparisons of the position trajectories of the end-effector among the three methods. From the figure, we can see that all three methods can realize the motion of the end-effector from the initial position to final desired position. The trajectory of the end-effector generated by the RAC method is a straight line, and the trajectories generated by CVP method and hp-AGPM are curves.

Fig.5 Attitude curves of the satellite body

Fig.6 Reaction torque at the satellite body

Fig. 5 denotes the comparisons of the time histories of the satellite body’s attitude angle among the three methods. As expected, the motion of manipulator will cause the attitude of satellite to change.From Fig. 5, we can see that the maximum attitude angle change of the satellite is equal to 0.0524 rad by using hp-AGPM, and the attitude angle of the satellite body for the CVP method and the RAC method are 0.1026 rad and 0.1237 rad respectively at the final time. Thus in the considered case hp-AGPM allowed reduction of the attitude angle of satellite by 48.93% and 57.64% from that of CVP method and RAC method, respectively. The validity of the proposed method in this paper is verified.

Fig. 6 illustrates the comparisons of the time histories of reaction torques applied to the satellite among the three methods. It can be seen that the reaction torque applied to the satellite using hp-AGPM is smooth, and there is no sudden change in the reaction torque during the motion.

In order to verify the optimality of the path, the obtained control variables are interpolated by the cubic spline interpolation and substituted into the space robot dynamics model. The obtained results are taken as the actual state, as shown by the dotted line in Fig.7 to Fig.14.

Fig. 7 Configuration change of the space robot

Fig. 8 Attitude displacement of the satellite body for hp-AGPM method

Positions of the satellite and manipulator during the maneuver with planned trajectory are presented in Fig. 7, and the result shows that the satellite attitude can be influenced by the motion of the manipulator. The time histories of satellite attitude are presented in Fig. 8. The angles and angular velocities of manipulator joints are shown in Fig. 9 and Fig. 10, respectively. In order to clearly see the differences of the angles and angular velocities between the hp-AGPM and the ODE45 method (Matlab function), the error curves are presented in Fig. 11 and Fig. 12. The results of Fig. 11 and Fig. 12 show that the relative errors of the angles and angular velocities of manipulator joints are small, and the optimality of the hp-AGPM is verified. State approximation points on various grids are shown in Fig. 13. The hp-AGPM can guarantee that it will be converged to an acceptable solution by 7 iterations. According to Pontryagin’s maximum principle, the optimal Hamiltonian for this problem is constant (because the final time is constant) and Fig.14 shows that the Hamiltonian is in excellent agreement with this theoretical result.

Fig. 9 Angles of manipulator joints

Fig. 10 Velocities of manipulator joints

Fig.11 Errors of the joint angles

Fig.12 Errors of the joint angular velocities

Fig.13 State approximation points on various grids

Fig.14 Hamiltonian vs time

Table 2 shows the comparisons of performance of the algorithms using the h-method, p-method and the hp-AGPM. Through simulation, it was found that the p-method was never possible to obtain a solution for the toleranceε≤10-3, so the h-method and hp-AGPM method were compared and analyzed.

Table 2 Results comparison of different pseudospectral methods

It can be seen from Table 2 that the CPU time and number of collocation points are similar forε≥10-4. As the accuracy tolerance is tightened, the advantages of the hp-AGPM are gradually shown. The h-method requires more collocation points to achieve an acceptable tolerance, which significantly increased the computational cost when compared with the hp-AGPM. The hp-AGPM has great advantages in both the solving speed and the number of collocation points.

5 Conclusions

In this paper, a path planning scheme of FFSR based on the hp-AGPM is proposed. The optimization objective of path planning is the minimum reaction torque of the satellite at terminal time. A planar 2-DOF FFSR is taken as an example to verify the proposed method. The simulation results show that the hp-AGPM can program an optimal trajectory of the end-effector of space robot in 10.6 s for the toleranceε=10-6. It can be seen that the planned state variables and control variables are continuous and smooth and meet the path constraints.

The performances of hp-AGPM, CVP method and RAC method have been compared numerically with same simulation conditions. It has been shown that the hp-AGPM do significantly reduce the attitude disturbance for satellite at terminal time and provide a smooth trajectory of reaction torque at satellite body.

The hp-AGPM can automatically divide the segment or increase the number of collocation points. Compared with the p-method and h-method, the hp-AGPM has fewer collocation points and faster convergence speed, which is more suitable for solving the path planning problem of FFSR.