# Trajectory Planning Based on Screw Theory with Consideration of the Optimal Berth Position for a Space Robot

Authors: Yong Wang; Ying Liao; Kejie Gong
Journal: International Journal of Aerospace Engineering
 (2019)

---

## Abstract

Trajectory planning is a prerequisite for the tracking control of a free-floating space robot. There are usually multiple planning objectives, such as the pose of the end-effector and the base attitude. In efforts to achieve these goals, joint variables are often taken as exclusive operable parameters, while the berth position is neglected. This paper provides a novel trajectory planning strategy that considers the berth position by applying screw theory and an optimization method. First, kinematic equations at the position level are established on the basis of the product of exponential formula and the conservation of the linear momentum of the system. Then, generalized Jacobian matrices of the base and end-effector are derived separately. According to the differential relationship, an ordinary differential equation for the base attitude is established, and it is solved by the modified Euler method. With these sufficient and necessary preconditions, a parametric optimization strategy is proposed for two trajectory planning cases: zero attitude disturbance and attitude adjustment of the base. First, the berth position is transformed into the desired position of the end-effector, and its constraints are described. Joint variables are parameterized using a sinusoidal function combined with a five-order polynomial function. Then, objective functions are constructed. Finally, a genetic algorithm with a modified mutation operator is used to solve this optimization problem. The optimal berth position and optimized trajectory are obtained synchronously. The simulation of a planar dual-link space robot demonstrates that the proposed strategy is feasible, concise, and efficient.


---

## Body

## 1. Introduction

On-orbit servicing (OOS) [1] technology has been attracting increasing research and investment from space institutions worldwide for its potential to mitigate on-orbit failures [2] and maintain and extend outer-space projects. Space robots are one of the most successful and extensively used means to achieve these goals, as demonstrated by several notable deployments and experimental missions, such as Space Station Remote Manipulator System (SSRMS) [3], Experimental Test Satellite VII (ETS-VII) [4], Front-end Robotics Enabling Near-term Demonstration (FREND) [5], and China’s intelligent space robots [6]. The concept of cooperative small robotic servicers to handle large passive objects has also been studied in recent years [7–9]. Possible applications of space robots include inspection, assembly, maintenance, repair, and refueling [10]. When executing these tasks, the system often works in one of three modes: free-flying mode (in which the position and attitude of the base are both controllable) [11], free-translation mode (in which only the base attitude is controllable), and free-floating mode [12] (in which the base is completely free).Trajectory planning—a term that is often used interchangeably withpath planning, despite their technical differences—is crucial for implementing a space mission. Many research papers have addressed the trajectory planning and control issues of the free-floating space robot because it consumes relatively little fuel and avoids potential collisions that can result from excess control [13]. The distinctive feature of a free-floating space robotic system is that it is nonholonomic because the angular momentum equation cannot be integrated. This feature has been used to develop some helpful planning strategies, known as nonholonomic trajectory planning. Vafa and Dubowsky [14] proposed the self-correcting motion method, which adjusts the base attitude by adding small cyclic motions to the joint motions. They also developed another method called the Disturbance Map (DM) [15]. The DM method builds a relationship between each point in the joint space and the actual movement of the base, and then the trajectory of the joints that causes the minimum disturbance to the base can be selected. Dubowsky and Torres [16, 17] improved the DM method to minimize spacecraft attitude disturbance. They called the improved method the Enhanced Disturbance Map (EDM), which can also be used to minimize the fuel consumption for attitude control. Fernandes et al. [18] were inspired by the falling cat problem and developed a near-optimal motion planning method. Papadopoulos et al. [19, 20] proposed a path planning methodology that allows for simultaneous control of the end-effector and the base attitude. In this approach, polynomials are employed to obtain a smooth and continuous trajectory. Xu et al. [21] developed a coordinated trajectory planning method to stabilize the base attitude and centroid position by a balance arm.In recent years, optimization algorithms have been introduced in studies that aim to achieve optimal trajectories for free-floating space robots. Huang et al. [22] provided an approach based on a Genetic Algorithm (GA) to find a global minimum-jerk trajectory of a space manipulator in the joint space. Xu et al. [23] employed sinusoidal functions, whose arguments are seven-order polynomials, and a GA to realize the base attitude and attain the desired states of the joints. They also proposed a similar planning strategy in Cartesian space and searched for the optimal parameters using the Particle Swarm Optimization (PSO) algorithm [24]. PSO was also used in the studies in [25, 26]. Rybus and Seweryn [27] employed the Rapidly-exploring Random Trees (RRT) algorithm for the trajectory planning of a satellite-manipulator system, and joint limits and obstacles were taken into account. Luo et al. [28] proposed a novel trajectory planning framework with a task-priority handling strategy for a space robot. A Quadratic Programming (QP) procedure was applied to solve the problem.In the above studies, the parameterization and optimization approaches have been successfully applied to solve trajectory planning problems. Nevertheless, the parameters to be optimized are often exclusively chosen as coefficients of joint trajectories that are represented by continuous functions. Before executing the mission, the base (spacecraft) hovers at a proper position (called the berth position) relative to the target to avoid unexpected collision and change in the work mode. The variation in the berth position markedly affects the desired trajectories of the joints; however, it seems to have been frequently omitted from trajectory planning studies. Zhang and Liu [29] provided a new viewpoint that utilizes the base berth position as an optimizable parameter. A unified motion planning strategy based on the forward kinematics and parametrical optimization method was developed. The objective function was simplified by proposing the concept of grasping area and using a penalty function. Finally, PSO was employed to determine the parameters. It was demonstrated that the DOF requirement of the manipulator could be reduced by taking the berth position into account. This innovative idea has great potential to diversify the objectives of a single mission. However, only the desired position and attitude of the end-effector are satisfied. Furthermore, the berth position is not optimized directly; instead, it is calculated from the optimal results of the joint variables according to a geometric relationship. This point is worth further exploring.Another point of current research that can be expanded is the kinematical theory. Kinematic equations constitute the objective function, which directly affects the efficiency of a given trajectory planning strategy. Because space robots differ from ground-based robots, researchers have developed new kinematical theories for space robots, including the Virtual Manipulator (VM) method [14], Generalized Jacobian Matrix (GJM) [30], and Dynamically Equivalent Manipulator (DEM) [31]. GJM is widely used in control of space robots [32, 33]. However, when applied to trajectory planning, the VM and DEM methods focus on the transformation of the model, and the transformation process significantly increases the workload. Furthermore, deriving the GJM with body-fixed vectors makes the programming difficult. Besides, integration is needed when calculating the displacement of joints and the end-effector at the position level, as Zhang and Liu did in the autonomous trajectory planner [34].Screw theory is an efficient alternative to the kinematic analysis of a rigid body and its mechanism. It can establish a global description of the rigid body and avoid singularities [35]. It has been applied to research on the ground-based robot [36] and parallel manipulator [37, 38]. The unified description of translation and rotation confers the advantages of conciseness and efficiency to screw theory. Rocha et al. [39] compared the screw-based method with the Denavit-Hartenberg method [40] in detail and concluded that the former has advantages over the D-H approach, although the latter is more commonly used. Nevertheless, few studies on space robots using screw theory are available in the current literature. Liu and Wu [41] developed a dynamic model of a space robot system on the basis of screw theory and Kane’s equation. Liu and Wu [42] provided a simple and efficient method to derive the Jacobian matrix of a free-floating branching robot system by dividing it into two parts: the ground-robot with a fixed base and a locked-joint robot with a floating base. The authors of this paper studied kinematic problems at position level base on screw theory [43]; a part of these works will be applied in this research.This paper proposes a novel trajectory planning strategy based on screw theory. The proposed approach considers the berth position and aims to improve calculation efficiency. First, the kinematic model of the free-floating space robot is built using screw theory, and the model includes the position and differential level. The base attitude is calculated using the modified Euler method. Next, we construct the optimization problem by transforming the berth position and parameterizing the joint variables. Then, the planning strategy is provided in detail. A simulation of a planar dual-link space robot is presented, and Adams simulation results are provided to verify the proposed method. A nomenclature is given to clarify important variables and abbreviations.


## 2. Kinematic Modeling Using Screw Theory

### 2.1. Model of the Space Robotic System

A space robot (also referred to as a space manipulator) is a very complicated system, which can be simplified if only kinematics is considered. Figure1 provides a general model of the space robotic system, which consists of the base (spacecraft), arm(s), and the End-Effector (EE). The arm contains several links and joints. The following assumptions are made to build a concise and accurate kinematic model.
1.
The position and orientation of the base is not controlled, and there are no external forces applied to the system, which means that the system is in free-floating mode
2.
The initial linear and angular momentum of the system are both zero
3.
The arm is a single chain connected to the base, and the parallel manipulator is not considered
4.
All links and joints are rigid
5.
All joints are revolute, and all rotation angles are limited between−180∘ and 180∘Figure 1
Model of the space robot system.


### 2.2. Kinematic Equations at the Position Level

According to the product of exponentials (POE) formula, the final pose of a centroid-fixed frame of theith link with respect to the base-fixed frame can be defined as follows:
(1)gCiC0=eθ1ξ^1eθ2ξ^2⋯eθiξ^igCiC00=RC0ipC0i01,i=1,2,⋯,n,where ξi=ωi,ri×ωiT denotes the twist coordinates of ξi^.Then, the final pose of the end-effector with respect to the base-fixed frame can be determined by(2)gEC0=eθ1ξ^1eθ2ξ^2⋯eθnξ^ngEC00=RC0EpC0E01.The final pose of the base and the centroid-fixed frame of theith link with respect to the inertial frame can be expressed as follows:
(3)gCiI=RIipIi01,i=0,1,⋯,n,where RI0 is determined by the orientation of the base. If we define the attitude angles (roll, pitch, yaw) of the base as Ψ=δ,β,γ, then
(4)R0I=cβcγsδsβcγ−cδsγcδsβcγ+sδsγcβsγsδsβsγ+cδcγcδsβsγ−sδcγ−sβsδcβcδcβ,where c denotes cos and sdenotes sin.In terms of the transformation rule of rigid motion, the following equation holds:(5)gCiI=gC0IgCiC0=RI0pI001RC0ipC0i01=RI0RC0iRI0pC0i+pI001.Thus,(6)RIi=RI0RC0i,pIi=RI0pC0i+pI0.Then, the final pose of the end-effector with respect to the inertial frame can be derived as follows:(7)gEI=gC0IgEC0=RI0pI001RC0EpC0E01=RI0RC0ERI0pC0E+pI001.In the inertial frame, the centroid formula of the system is written as follows:(8)m0pI0+∑i=1nmipIi=MpIC.Substituting Equation (6) into Equation (8) yields
(9)pI0=pIC−RI0M∑i=1nmipC0i=pIC−RI0pM,where pM=1/M∑i=0nmipiC0. In particular, pCI=0 when the centroid of the system is chosen as the initial point of the inertial frame.Substituting Equation (9) into Equation (7) yields
(10)gEI=RI0RC0ERI0pC0E−pM+pIC01,i=1,2,⋯,n.


### 2.3. The Generalized Jacobian Matrix

Angular momentum of the base and theith link with respect to the inertial frame can be defined as follows:
(11)Li=ICiωiB+mipIi×ṗIi,where ωiB is the body angular velocity of the ith link. The body velocity of the base and the ith link is defined as follows:
(12)VCiIB=gCiI−1ġCiIV=ωIiBvIiB=RIiTṘIiVRIiTṗIi.Similarly, the spatial velocity is specified as follows:(13)VCiIS=ġCiIgCiI−1V=ωIiSvIiS=ṘIiRIiTVṘIiIRiTpIi+ṗIi.The similarity transformation relationship between the body and spatial velocity is specified as follows:(14)VCiIS=AdgCiIVCiIBorVCiIB=AdgCiI−1VCiIS,where AdgCiI corresponds to the adjoint transformation of gCiI, and
(15)AdgCiI=RCiI0p^IiRCiIRCiI.Then,Li can be derived as follows:
(16)Li=ICimip^IiRIiωIiBRIiTṗIi=ICimip^IiRIiAdgCiI−1VCiIS=ICiRIiT−mip^Iip^Iimip^IiVCiIS=KiVCiIS,where KiICiRIiT−mip^Iip^Iimip^Ii, which can be defined as the generalized inertia matrix.In terms of the transformation rule of spatial velocities, the following equation holds:(17)VCiIS=VC0IS+AdgC0IVCiC0S.From Equations (16) and (17), the angular momentum of the system can be defined by
(18)L=∑i=1nKiVC0IS+AdgC0IVCiC0S+K0VC0IS=K~BVC0IS+K~NJNθ̇,where K~B=∑i=0nKi∈ℝ3×6, K~N=K1AdgC0IK2AdC0Ig⋯KnAdgC0I∈ℝ3×6n, and JN=J1J2⋯JnT∈ℝ6n×n, Ji=ξ^1′,ξ^2′,⋯ξ^i′,0,⋯0T∈ℝn×6.The angular momentum of the system is conserved. Without loss of generality, it is considered to be zero. Thus,(19)VC0IS=−K~B+K~NJNθ̇=JBθ̇,where K~B+ is the pseudoinverse matrix of K~B. JB=−K~B+K~NJN, which is defined as the generalized Jacobian matrix of the base.Furthermore,(20)VEIS=VC0IS+AdgC0IVEC0S=JB+AdgC0IJnθ̇=JEθ̇,where JE=JB+AdgC0IJn, which is defined as the generalized Jacobian matrix of the end-effector.


### 2.4. Attitude Calculation of the Base Using the Generalized Jacobian Matrix

Equation (19) can be rewritten as follows:
(21)VC0IS=ωI0SvI0S=JBθ̇=JBωJBvθ̇,where JBω,JBv∈ℝ3×n is the submatrix of JB related to orientation and position, respectively. According to the deductions in Subsections 2.2 and 2.3, it is apparent that JBω is a function that depends on Ψ. Thus,
(22)ωI0S=JBωΨθ̇.On the other hand, according to Equations (4) and (13),
(23)ωI0S=ṘI0RI0TV=0−sγcγcβ0cγsγcβ10−sβδ̇β̇γ̇=JEulerΨΨ,where
(24)JEuler=0−sγcγcβ0cγsγcβ10−sβ.Substituting Equation (23) into Equation (22) yields
(25)Ψ̇=JEuler−1ΨJBωΨθ̇=FΨ,θ̇,t.Ifθ̇ is given, then Equation (25) is an ordinary differential equation about Ψ, which can be solved by the modified Euler method. If the initial state of Ψ (Ψt0) is known, then
(26)Ψ¯k=Ψk−1+hFΨk−1,θ̇k−1,tk−1,Ψk=Ψk−1+h2FΨk−1,θ̇k−1,tk−1+FΨ¯k,θ̇k,tk,Ψ0=Ψt0,θ̇k=θ̇tk,where h is the time step.


## 2.1. Model of the Space Robotic System

A space robot (also referred to as a space manipulator) is a very complicated system, which can be simplified if only kinematics is considered. Figure1 provides a general model of the space robotic system, which consists of the base (spacecraft), arm(s), and the End-Effector (EE). The arm contains several links and joints. The following assumptions are made to build a concise and accurate kinematic model.
1.
The position and orientation of the base is not controlled, and there are no external forces applied to the system, which means that the system is in free-floating mode
2.
The initial linear and angular momentum of the system are both zero
3.
The arm is a single chain connected to the base, and the parallel manipulator is not considered
4.
All links and joints are rigid
5.
All joints are revolute, and all rotation angles are limited between−180∘ and 180∘Figure 1
Model of the space robot system.


## 2.2. Kinematic Equations at the Position Level

According to the product of exponentials (POE) formula, the final pose of a centroid-fixed frame of theith link with respect to the base-fixed frame can be defined as follows:
(1)gCiC0=eθ1ξ^1eθ2ξ^2⋯eθiξ^igCiC00=RC0ipC0i01,i=1,2,⋯,n,where ξi=ωi,ri×ωiT denotes the twist coordinates of ξi^.Then, the final pose of the end-effector with respect to the base-fixed frame can be determined by(2)gEC0=eθ1ξ^1eθ2ξ^2⋯eθnξ^ngEC00=RC0EpC0E01.The final pose of the base and the centroid-fixed frame of theith link with respect to the inertial frame can be expressed as follows:
(3)gCiI=RIipIi01,i=0,1,⋯,n,where RI0 is determined by the orientation of the base. If we define the attitude angles (roll, pitch, yaw) of the base as Ψ=δ,β,γ, then
(4)R0I=cβcγsδsβcγ−cδsγcδsβcγ+sδsγcβsγsδsβsγ+cδcγcδsβsγ−sδcγ−sβsδcβcδcβ,where c denotes cos and sdenotes sin.In terms of the transformation rule of rigid motion, the following equation holds:(5)gCiI=gC0IgCiC0=RI0pI001RC0ipC0i01=RI0RC0iRI0pC0i+pI001.Thus,(6)RIi=RI0RC0i,pIi=RI0pC0i+pI0.Then, the final pose of the end-effector with respect to the inertial frame can be derived as follows:(7)gEI=gC0IgEC0=RI0pI001RC0EpC0E01=RI0RC0ERI0pC0E+pI001.In the inertial frame, the centroid formula of the system is written as follows:(8)m0pI0+∑i=1nmipIi=MpIC.Substituting Equation (6) into Equation (8) yields
(9)pI0=pIC−RI0M∑i=1nmipC0i=pIC−RI0pM,where pM=1/M∑i=0nmipiC0. In particular, pCI=0 when the centroid of the system is chosen as the initial point of the inertial frame.Substituting Equation (9) into Equation (7) yields
(10)gEI=RI0RC0ERI0pC0E−pM+pIC01,i=1,2,⋯,n.


## 2.3. The Generalized Jacobian Matrix

Angular momentum of the base and theith link with respect to the inertial frame can be defined as follows:
(11)Li=ICiωiB+mipIi×ṗIi,where ωiB is the body angular velocity of the ith link. The body velocity of the base and the ith link is defined as follows:
(12)VCiIB=gCiI−1ġCiIV=ωIiBvIiB=RIiTṘIiVRIiTṗIi.Similarly, the spatial velocity is specified as follows:(13)VCiIS=ġCiIgCiI−1V=ωIiSvIiS=ṘIiRIiTVṘIiIRiTpIi+ṗIi.The similarity transformation relationship between the body and spatial velocity is specified as follows:(14)VCiIS=AdgCiIVCiIBorVCiIB=AdgCiI−1VCiIS,where AdgCiI corresponds to the adjoint transformation of gCiI, and
(15)AdgCiI=RCiI0p^IiRCiIRCiI.Then,Li can be derived as follows:
(16)Li=ICimip^IiRIiωIiBRIiTṗIi=ICimip^IiRIiAdgCiI−1VCiIS=ICiRIiT−mip^Iip^Iimip^IiVCiIS=KiVCiIS,where KiICiRIiT−mip^Iip^Iimip^Ii, which can be defined as the generalized inertia matrix.In terms of the transformation rule of spatial velocities, the following equation holds:(17)VCiIS=VC0IS+AdgC0IVCiC0S.From Equations (16) and (17), the angular momentum of the system can be defined by
(18)L=∑i=1nKiVC0IS+AdgC0IVCiC0S+K0VC0IS=K~BVC0IS+K~NJNθ̇,where K~B=∑i=0nKi∈ℝ3×6, K~N=K1AdgC0IK2AdC0Ig⋯KnAdgC0I∈ℝ3×6n, and JN=J1J2⋯JnT∈ℝ6n×n, Ji=ξ^1′,ξ^2′,⋯ξ^i′,0,⋯0T∈ℝn×6.The angular momentum of the system is conserved. Without loss of generality, it is considered to be zero. Thus,(19)VC0IS=−K~B+K~NJNθ̇=JBθ̇,where K~B+ is the pseudoinverse matrix of K~B. JB=−K~B+K~NJN, which is defined as the generalized Jacobian matrix of the base.Furthermore,(20)VEIS=VC0IS+AdgC0IVEC0S=JB+AdgC0IJnθ̇=JEθ̇,where JE=JB+AdgC0IJn, which is defined as the generalized Jacobian matrix of the end-effector.


## 2.4. Attitude Calculation of the Base Using the Generalized Jacobian Matrix

Equation (19) can be rewritten as follows:
(21)VC0IS=ωI0SvI0S=JBθ̇=JBωJBvθ̇,where JBω,JBv∈ℝ3×n is the submatrix of JB related to orientation and position, respectively. According to the deductions in Subsections 2.2 and 2.3, it is apparent that JBω is a function that depends on Ψ. Thus,
(22)ωI0S=JBωΨθ̇.On the other hand, according to Equations (4) and (13),
(23)ωI0S=ṘI0RI0TV=0−sγcγcβ0cγsγcβ10−sβδ̇β̇γ̇=JEulerΨΨ,where
(24)JEuler=0−sγcγcβ0cγsγcβ10−sβ.Substituting Equation (23) into Equation (22) yields
(25)Ψ̇=JEuler−1ΨJBωΨθ̇=FΨ,θ̇,t.Ifθ̇ is given, then Equation (25) is an ordinary differential equation about Ψ, which can be solved by the modified Euler method. If the initial state of Ψ (Ψt0) is known, then
(26)Ψ¯k=Ψk−1+hFΨk−1,θ̇k−1,tk−1,Ψk=Ψk−1+h2FΨk−1,θ̇k−1,tk−1+FΨ¯k,θ̇k,tk,Ψ0=Ψt0,θ̇k=θ̇tk,where h is the time step.


## 3. Problem Construction

By considering berth position, we can achieve an extra goal while the EE reaches the desired pose. A relatively simple case is planning a trajectory with zero attitude disturbance to the base. Another case is the adjustment of the attitude of the base to the desired orientation simultaneously. Both trajectory planning problems can be converted to parametric optimization problems. The problems can be described clearly by first performing the following preparations.


### 3.1. Transformation of the Berth Position

The problem can be simplified by disregarding the movement of the target. Thus, the berth position can be directly expressed as the vectorpItb from the target’s centroid to the base’s centroid, which is the variable we aim to optimize. With respect to the target-centroid-fixed inertial frame ∑TI, the desired position of the end-effector pIte is constant, while the centroid of the system varies with pItb because of the conservation of linear momentum. Conversely, the problems become complex if they are analyzed in ∑TI. Given that the berth position has no effect on the kinematic relationship, and the centroid of the system will be immobile once the berth position is determined, kinematic equations can be made much more concise when expressed in the system-centroid-fixed inertial frame ∑SI. In fact, if the berth position is expressed with respect to ∑SI, then the kinematic equations contain no variables of the berth position. Only the target’s centroid, as well as the desired position of the EE, varies with pItb, which means that the berth position is transformed into the desired position of the EE. The transformation process is shown in Figure 2. It must be clarified that the transformation is performed at the initial time, and all the involved vectors are independent of time.Figure 2
Transformation of the berth position.According to Equation (9), the position of the base’s centroid with respect to ∑SI is as follows:
(27)pI00=−pM0=−1M∑j=0nmjpC0j0.Then, the position of the target’s centroid with respect to∑SI can be derived as follows:
(28)pIt=pI00−pItb=−pM0−pItB.Then, the desired position of the EE with respect to∑SI can be derived as follows:
(29)pIde=pIt+pIte=pIte−pM0−pItb.Further, the desired orientation of the EE should also be transformed:(30)REId=RTIRPTREPd,where REPd is the desired rotation matrix from the capture point to the EE, RPT is the rotation matrix of the capture point with respect to ∑TI, and RTI is the rotation matrix of ∑TI with respect to ∑SI.Next, constraints ofpItb need to be specified.
(1)
Safe distance, which is designed to avoid unexpected collision while the space robot system is docking, is expressed as follows:
(31)pItb>Ps,where Ps is the threshold value of safe distance.
(2)
In workspace, which is designed to guarantee that the capture point is reachable by the EE, is expressed as follows:
(32)pIde<Rw,where Rw is the radius of the system’s guaranteed workspace. In terms of the inequality property of the norm,
(33)pItb=pIte−pM0−pIde<Rw+pIte+pM0.
(3)
Proper location, which is designed to prevent the system from docking at the opposite side of the capture point. Considering the situation in Figure 2,
(34)pItbx<pItex,where pItbx and pItex are projections of pItb and pIte along the x axis, respectively.


### 3.2. Parameterization of Joint Variables

Trajectory planning in the joint space requires that the joint variables be parameterized first. Many methods have been developed to obtain a feasible and smooth trajectory. Since these variables are bounded, we chose a concise and effective approach—a sinusoidal function combined with five-order polynomial functions—to parameterize the joint angles, angle velocities, and angular accelerations.(35)θi=li1sinA+li2,θi̇=li1cosA·Ȧ,θi¨=−li1sinA·Ȧ2+li1cosA·A,¨where A=ai5t5+ai4t4+ai3t3+ai2t2+ai1t+ai0, and li1,li2,aijj=0,1,⋯,5 are undetermined coefficients.Furthermore, the constraints of these variables are specified as follows.(1)
Equality constraints, including the initial and final states of the joint angles, angle velocities, and angle accelerations, are defined by
(36)θ0=0,θ̇0=θ̇tf=0,θ¨0=θ¨tf=0,where tf is the final time. Substituting Equation (35) into Equation (36) yields
(37)ai0=−arcsinli2li1,ai1=ai2=0,ai3=53tf2ai5,ai4=−52tfai5.Then, Equation (35) can be rewritten as follows:
(38)θi=li1sinA~+li2,θi̇=li1cosA~·A~̇,θi¨=−li1sinA~·A~̇2+li1cosA~·A,~¨where A~=ai5t5−5/2tft4+5/3tf2t3−arcsinli2/li1.
(2)
Inequality constraints, including limits of joint angles, angle velocities, and angular accelerations, are expressed as follows:
(39)θmin≤θ≤θmax.(40)θ̇min≤θ̇≤θ̇max.(41)θ¨min≤θ¨≤θ¨max.According to Equations (38) and (39), li1 and li2 can be determined as follows:
(42)l1=θmin+θmax2,l2=θmax+θmin2,where l1=li1,l2=li2.According to Equations (38), (40), and (41), the boundary limits of ai5 can be determined by
(43)ai5min=maxθ̇iminli1M1,θ¨iminli1M12+M2,ai5max=minθ̇imaxli1M1,θ¨imaxli1M12+M2,where M1=max5t4−10tft3+5tf2t2,t∈0,tf, and M2=max20t3−30tft2+10tf2t,t∈0,tf. Let a5=ai5,a5min=ai5min,a5max=ai5max.


### 3.3. Objective Function

According to Subsection3.1, the desired pose of the EE can be determined by
(44)gEId=REIdpIde01.For the trajectory planning of zero attitude disturbance to the base, the following equality should be satisfied:(45)ωC0IS=JBωθ̇≡0.Thus, the objective function for this case can be defined as follows:(46)Jzd=loggEId−1gEI+∫0tfJBωθ̇dt,where log() means logarithm mapping of a matrix.For the trajectory planning of the attitude adjustment of the base, the following equality should be satisfied:(47)Ψf=Ψd.Thus, the objective function for this case can be defined as follows:(48)Jαα=loggEId−1gEI+Ψf−Ψd.


## 3.1. Transformation of the Berth Position

The problem can be simplified by disregarding the movement of the target. Thus, the berth position can be directly expressed as the vectorpItb from the target’s centroid to the base’s centroid, which is the variable we aim to optimize. With respect to the target-centroid-fixed inertial frame ∑TI, the desired position of the end-effector pIte is constant, while the centroid of the system varies with pItb because of the conservation of linear momentum. Conversely, the problems become complex if they are analyzed in ∑TI. Given that the berth position has no effect on the kinematic relationship, and the centroid of the system will be immobile once the berth position is determined, kinematic equations can be made much more concise when expressed in the system-centroid-fixed inertial frame ∑SI. In fact, if the berth position is expressed with respect to ∑SI, then the kinematic equations contain no variables of the berth position. Only the target’s centroid, as well as the desired position of the EE, varies with pItb, which means that the berth position is transformed into the desired position of the EE. The transformation process is shown in Figure 2. It must be clarified that the transformation is performed at the initial time, and all the involved vectors are independent of time.Figure 2
Transformation of the berth position.According to Equation (9), the position of the base’s centroid with respect to ∑SI is as follows:
(27)pI00=−pM0=−1M∑j=0nmjpC0j0.Then, the position of the target’s centroid with respect to∑SI can be derived as follows:
(28)pIt=pI00−pItb=−pM0−pItB.Then, the desired position of the EE with respect to∑SI can be derived as follows:
(29)pIde=pIt+pIte=pIte−pM0−pItb.Further, the desired orientation of the EE should also be transformed:(30)REId=RTIRPTREPd,where REPd is the desired rotation matrix from the capture point to the EE, RPT is the rotation matrix of the capture point with respect to ∑TI, and RTI is the rotation matrix of ∑TI with respect to ∑SI.Next, constraints ofpItb need to be specified.
(1)
Safe distance, which is designed to avoid unexpected collision while the space robot system is docking, is expressed as follows:
(31)pItb>Ps,where Ps is the threshold value of safe distance.
(2)
In workspace, which is designed to guarantee that the capture point is reachable by the EE, is expressed as follows:
(32)pIde<Rw,where Rw is the radius of the system’s guaranteed workspace. In terms of the inequality property of the norm,
(33)pItb=pIte−pM0−pIde<Rw+pIte+pM0.
(3)
Proper location, which is designed to prevent the system from docking at the opposite side of the capture point. Considering the situation in Figure 2,
(34)pItbx<pItex,where pItbx and pItex are projections of pItb and pIte along the x axis, respectively.


## 3.2. Parameterization of Joint Variables

Trajectory planning in the joint space requires that the joint variables be parameterized first. Many methods have been developed to obtain a feasible and smooth trajectory. Since these variables are bounded, we chose a concise and effective approach—a sinusoidal function combined with five-order polynomial functions—to parameterize the joint angles, angle velocities, and angular accelerations.(35)θi=li1sinA+li2,θi̇=li1cosA·Ȧ,θi¨=−li1sinA·Ȧ2+li1cosA·A,¨where A=ai5t5+ai4t4+ai3t3+ai2t2+ai1t+ai0, and li1,li2,aijj=0,1,⋯,5 are undetermined coefficients.Furthermore, the constraints of these variables are specified as follows.(1)
Equality constraints, including the initial and final states of the joint angles, angle velocities, and angle accelerations, are defined by
(36)θ0=0,θ̇0=θ̇tf=0,θ¨0=θ¨tf=0,where tf is the final time. Substituting Equation (35) into Equation (36) yields
(37)ai0=−arcsinli2li1,ai1=ai2=0,ai3=53tf2ai5,ai4=−52tfai5.Then, Equation (35) can be rewritten as follows:
(38)θi=li1sinA~+li2,θi̇=li1cosA~·A~̇,θi¨=−li1sinA~·A~̇2+li1cosA~·A,~¨where A~=ai5t5−5/2tft4+5/3tf2t3−arcsinli2/li1.
(2)
Inequality constraints, including limits of joint angles, angle velocities, and angular accelerations, are expressed as follows:
(39)θmin≤θ≤θmax.(40)θ̇min≤θ̇≤θ̇max.(41)θ¨min≤θ¨≤θ¨max.According to Equations (38) and (39), li1 and li2 can be determined as follows:
(42)l1=θmin+θmax2,l2=θmax+θmin2,where l1=li1,l2=li2.According to Equations (38), (40), and (41), the boundary limits of ai5 can be determined by
(43)ai5min=maxθ̇iminli1M1,θ¨iminli1M12+M2,ai5max=minθ̇imaxli1M1,θ¨imaxli1M12+M2,where M1=max5t4−10tft3+5tf2t2,t∈0,tf, and M2=max20t3−30tft2+10tf2t,t∈0,tf. Let a5=ai5,a5min=ai5min,a5max=ai5max.


## 3.3. Objective Function

According to Subsection3.1, the desired pose of the EE can be determined by
(44)gEId=REIdpIde01.For the trajectory planning of zero attitude disturbance to the base, the following equality should be satisfied:(45)ωC0IS=JBωθ̇≡0.Thus, the objective function for this case can be defined as follows:(46)Jzd=loggEId−1gEI+∫0tfJBωθ̇dt,where log() means logarithm mapping of a matrix.For the trajectory planning of the attitude adjustment of the base, the following equality should be satisfied:(47)Ψf=Ψd.Thus, the objective function for this case can be defined as follows:(48)Jαα=loggEId−1gEI+Ψf−Ψd.


## 4. Trajectory Planning Strategy with Consideration of the Optimal Berth Position

With the conclusion of Section3, trajectory planning problems using a parametric optimization method can be described as follows:
(49)minJzdα5,pItborJααα5,pItbst.α5min≤α5≤α5maxPs<pItb<Rw+pIte+pM0pItbx<pItex,where the coefficients α5and berth position pItb are variables to be optimized, and X=α5,pItb.A genetic algorithm is employed to solve this minimization problem because of its fast and stochastic searching ability. Here, the designation and procedure of the trajectory planning strategy is sketched. A flowchart is shown in Figure3.
Step 1.
Coding. Choose real-number coding, and vectorX is the individual
Step 2.
Fitness function. Let the fitness functionfX equal the objective function Jzd or Jaa. The smaller the value of fX, the better the individual
Step 3.
Initialization. Set the genetic algorithm parameters, including the population sizens, maximum generation number ng, crossover fraction pc, mutation fraction pm, and stopping criterion ε. Then, generate the first generation randomly in terms of constraints
Step 4.
Fitness calculation. Fitness functions for the two cases consist of two parts. Here, the calculation process is detailed as follows:(a)
CalculategCiC0, gEC0 by substituting Equation (38) into Equations (1) and (2), respectively. Thus, RC0i, pC0i is known
(b)
DenoteRI0, JEuler by substituting undetermined Ψ into Equations (4) and (23), respectively
(c)
DenotepI0 by substituting RI0, pC0i into Equation (9), where pIC=0
(d)
Denote the generalized Jacobian matrix of the baseJB by substituting gC0I, gCiC0 into Equation (19)
(e)
Calculate the final attitude of the baseΨ by Equation (26)
(f)
CalculateJBω by using Step 4 and Equation (21)
(g)
CalculateloggEId−1gEI, where gEI can be calculated by Equation (10), and gEId can be calculated by Equations (29), (30), and (44)Figure 3
Flowchart of the provided trajectory planning strategy.From the above,fX can be achieved. Then, rank individuals according to their fitness values and judge whether the minimum fitness value is less than ε. If yes, go to Step 7; otherwise, go to the next step.
Step 5.
Reproduction. Generate the next generation by the selection, crossover, and mutation operatorSelection. Use the Roulette Wheel Selection method, in which the area of each individual depends on its rankCrossover. Choose the scatter crossover operator; a random binary vector selects the genes of the parents, which then combine to form the childMutation. Modify the classical Gaussian mutation method, which is usually used for unconstrained problems. The Gaussian mutation method adds random numbers from a Gaussian distribution in terms ofμk,σk to the parent, and σk shrinks as the generations increase. The method has two disadvantages: the first is that the genes of the child might exceed the boundary, and the second is that the process might slow down to find the optimal solution when performing the local search because σk only varies with the generation. In view of these shortcomings, we offer a modified Gaussian mutation method. The determination of σk is defined as follows:
(50)σk=Xbound1−kngiffbest≥1,σk=Xbound1−kng0.5expfbest−0.5if0.05<fbest<1,σk=Xbound1−kng0.8expfbest−0.8iffbest≤0.05,where Xbound=Xmax−Xmin, k is current generation number, and fbest is the smallest fitness value of the current population. Furthermore, correction is made when the genes of the child exceed the boundary:
(51)Xi=Xmaxi−rXmaxi−XminiifXi>Xmaxi,Xi=Xmini+rXmaxi−XminiifXi<Xmini,where Xi is the ith gene of the child and r is a random number between 0,1.
Step 6.
Stopping judgment. Judge whether the number of the current generation exceedsng. If yes, go to Step 7; otherwise, go to Step 4
Step 7.
Trajectory calculation. Obtain the population containing the best individual. Calculate the trajectory of each joint usingai5 of the best individual. Thus, the planning concludes


## 5. Simulation Examples

Trajectory planning of a planar dual-link space robot was performed with MATLAB. To verify the effectiveness of our strategy and illustrate berth position and the moving process of optimized joint trajectory, we built a corresponding model by Adams. The optimal results of each calculation were substituted into the Adams model, and Adams simulations were run. The simulation model in the initial configuration is shown in Figure4. The CPU clock of the computer used is 3.5 GHz.Figure 4
Simulation model of the planar dual-link space robot in the initial configuration.The detailed initial and inertia parameters of the model are as follows.ω1=ω2=0,0,1T,r1=1.2,0.7,0T,r2=2.09,−0.85,0T.Then,ξ1=ω1,r1×ω1T=0,0,1,0.7,−1.2,0T,ξ2=ω2,r2×ω2T=0,−1,0,−0.85,−2.09,0T.Next,m0=400,m1=18,m2=24;Iz0=260,Iz1=6,Iz2=7.8;
(52)gC1B0=1.68Rz−60°−0.14001,gC2B0=2.65Rz60°0.12001,gEB0=3.0Rz60°0.72001,where
(53)Rzϕ=cosϕ−sinϕ0sinϕcosϕ0001is the direction cosine matrix along z axis. According to Equation (27), pM0=−0.212,0,0T and pIte=−0.65,0,0T, Ps=3.60, Rw=4.69, so constraints of pItb can be given as 3.60<pItb<5.55 and pItbx<−0.65. Constraints of the angular variables are as follows.
(54)θmin=−75,−165T°,θmax=75,165T°,θ̇min=−30,−35T°/s,θ̇max=30,35T°/s,θ¨min=−15,−20T°/s2,θ¨max=15,20T°/s2..According to Equations (42) and (43), α5min=−2.002,−0.801T×10−4,α5max=2.002,0.801T×10−4,l1=75,165T,l2=0,0T, where tf=10.0s.The desired orientation of the EE is set atREPd=Rz−90°, and the desired attitude of the base is set as Ψd=0,0,−7.5° for attitude adjustment. The parameters of the genetic algorithm are set at ns=45,ng=300,pc=0.83,pm=0.08,ε=0.001. The results of the simulation using MATLAB programs are as follows.For the planning case of zero attitude disturbance to the base, the iteration stops when the maximum generation number is reached. The variation in the mean fitness and best fitness of each generation is shown in Figure5(a). The optimal individual is Xbest=−5.227,−0.010,0.316,−0.380, and the corresponding fitness is fXbest=0.0144. The disturbance to the base attitude is tolerable, and the maximum amplitudes of the base’s angle and angular velocity are 0.216° and 0.080°/s, respectively. The optimized trajectories of the joints and the base attitude are shown in Figures 6 and 7, respectively. Figure 8 shows the optimized berth position and motion process simulated by Adams, with the dotted lines denoting the initial configuration. It can be observed that the EE reaches the capture point precisely, and the base attitude is nearly changeless. The trajectory of the EE is also sketched. The calculation time for each simulation is about 7.5 min.Figure 5
Variation in the mean fitness and best fitness of each generation.
(a)
(b)Figure 6
Optimized trajectories of joints for the planning case of zero attitude disturbance.Figure 7
Optimized trajectory of the base for the planning case of zero attitude disturbance.Figure 8
Optimized berth position and motion process for the planning case of zero attitude disturbance.For the planning case of attitude adjustment of the base, iteration stops at the297th generation as the stopping criterion ε is satisfied. Variation in the mean fitness and the best fitness of each generation is shown in Figure 5(b). The optimal individual is Xbest=−5.422,−0.401,0.622,−0.473, and the corresponding fitness is fXbest=0.00087. The final attitude of the base achieves the desired adjustment, Ψf=0,0,−7.514∘, and the corresponding error is 0.19% compared with Ψd. The optimized trajectories of the joints and the base attitude are shown in Figures 9 and 10, respectively. Figure 11 shows the optimized berth position and motion process simulated by Adams, and it shows that the EE again reaches the capture point precisely. The trajectory of the EE is also sketched. The calculation time for each simulation is about 8 min.Figure 9
Optimized trajectories of joints for the planning case of attitude adjustment.Figure 10
Optimized trajectory of the base for the planning case of attitude adjustment.Figure 11
Optimized berth position and motion process for the planning case of attitude adjustment.


## 6. Conclusion

In this paper, several innovations are presented to improve the approach to the trajectory planning of a free-floating space robot. First, there are two useful techniques to improve the calculation efficiency: systematical deduction of the generalized Jacobian matrix using screw theory and the calculation of the base attitude by the modified Euler method without integration. Second, the berth position is optimized with the joint variables by the transformation between the target-centroid-fixed and system-centroid-fixed inertial frame. The extra goals proposed are all achieved by considering the berth position, while the EE reaches the desired position and orientation. More importantly, the simulations of example missions are completed by using only two joints. Figure5 shows that the presented optimization method converges quickly and stably, thereby demonstrating that the proposed objective function is of good quality. Furthermore, the optimized trajectories of both the joints and the EE are feasible and smooth.



---

*Source: 1034705-2019-10-29.xml*