Abstract
Capturing and stabilizing tumbling targets using dual-arm space robots are very crucial to on-orbit servicing task. However, it is still very challenging due to the complex dynamics coupling and closed-chain constraints between the manipulators, the base, and the target. In this paper, a kinodynamic trajectory optimization method is proposed to generate the motion of a dual-arm space robot for stabilizing the captured tumbling target, which is formulated and solved as a nonlinear programming problem using direct collocation. Instead of optimizing the trajectory of each joint with the dynamics model of space robot, this method optimizes the trajectory of the tumbling target while considering the kinematics and dynamics constraints between the two arms and the target simultaneously. The objective function of the optimization is defined as weighted detumbling time, base disturbance, and manipulability, in order to avoid singularity and save the energy of space robot for further manipulation. Several physical simulations are carried out to validate the proposed method.
1. Introduction
With growing scientific research and commercial applications in space, more and more malfunctioning satellites and space debris are occupying precious orbital resources which will bring a great threat to the safety of on-orbit spacecrafts [1, 2]. In order to utilize or remove them, space robots have been studied and developed for many years [3]. For capturing a space target, the space robotic system mainly adopts the following two capturing methods, i.e., stiff-connection capturing and flexible-connection capturing [4]. The flexible-connection capturing method using tethered flying net [5] or flying gripper [6] can deal with a variety of targets even without any requirement on rendezvous and docking. This method also allows a long capture distance between the target and the servicing spacecraft and a broad range of size and shape of the target object. However, it has limitations on dexterous manipulation of the captured target, such as on-orbit maintenance and space assembly. Therefore, the stiff-connection capturing method using robotic arms [7] will still be a promising method for on-orbit servicing of noncooperative space target.
Moosavian and Papadopoulos summarize the modeling, planning, and control methods for free-floating space robots [8]. In order to solve the dynamics coupling between the base and the manipulator, the generalized Jacobian matrix is proposed for single-arm space robots [9]. Zhang et al. design an efficient decoupling controller based on the time-delay estimation (TDE) and the supertwisting control (STC), which can linearize the nonlinear dynamics of space robot and drive the state variables to converge to the equilibrium point robustly [10]. Compared with single-arm space robot, multiarm robotic system can perform more dexterous, flexible, and complex tasks [11, 12]. Yoshida et al. designs a resolved motion-rate coordinated control method for dual-arm space robot in which one of the manipulators tracks the desired trajectory while the other maintains the orientation of the satellite base. However, these methods can not be used for multiarm coordinated planning of space robots. Similar to relative Jacobian matrix [13] for terrestrial dual-arm robots, a generalized relative Jacobian matrix is proposed for multiarm space robots which can easily plan and control the relative motion between the arms while considering the dynamics coupling between the arms and the base [14]. For multiarm space robots, the base can be kept to be inertially stable during multiarm coordinated manipulation by reactionless motion planning [15]. However, most of these methods with Jacobian projection can be only used for nonconstrained quasistatic planning and control problem.
One of the most challenging on-orbit servicing tasks for space robots is to capture a space target with nonzero momentum. During the capture phase, several components need to be considered, including path planning to capture the tumbling target, hybrid control of the motion and the contact force for the end-effectors, coordinated control of the base attitude, and parameter identification of the tumbling target [16]. Considering the grasping force limitation, parameter uncertainty of the target and arbitrary detumbling motion, a time-optimal control method [17] is proposed for free-floating space robots stabilizing a tumbling target. However, the limitation of this method is the parameterized end-effector velocity. For capturing and stabilizing a tumbling space target with uncertain dynamics, Aghili [18] proposes an optimal motion planning scheme which will generate the end-effector trajectories for both pre- and postcapture phases. Further, a two-layer optimization is proposed to yield both end-effector forces and contact locations for cooperative manipulation of an on-orbit passive objects [19]. For the postcontact phase of capturing a tumbling target in space, Zhang et al. [20] present a control scheme and parameter identification technique for postcapture stabilization of unknown tumbling target, in which the manipulator’s motion is used to compensate torque limitation. A detumbling strategy is also proposed to minimize the detumbling time and control torques, in which the target’s trajectory is represented by quartic Bézier curves and the optimal solution is found by adaptive particle swarm optimization algorithm [21]. Joint-velocity limits are further considered in the detumbling and stabilization manipulation [22]. In order to limit the target attitude motion as well as interaction torque at the grasping point, a time-optimal control problem (OCP) is formulated and solved using the calculus of variations method with a highly accurate solution [23]. Taking advantage of the coupling between dynamics of translational and rotational systems, Aghili proposes an optimal controller which can damp out both translational and rotational motions collaboratively and simultaneously [24].
During postcapture manipulation, the optimal detumbling motion of space robot should be generated to reduce the momentum of the tumbling target with minimal base disturbance, while satisfying equality and inequality constraints simultaneously. A purely kinematic trajectory optimization method is proposed to manipulate the in-grasp object with relaxed-rigidity constraints [25]. However, it can not be used for heavy object manipulation with nonnegligible dynamics. Recently, a nonlinear trajectory optimization method is proposed to generate the trajectory for approaching the tumbling target during precontact phase [26] and solved by direct collocation method [27]. Similarly, neither the object dynamics nor interaction between the object and the space robot is considered for this precontact trajectory optimization. However, the closed-chain constraints and coupling dynamics between the object and space robot make the postcapture manipulation much more challenging. In this paper, we will formulate the postcapture manipulation as a trajectory optimization problem in which the base disturbance will be minimized. Betts et al. [28] reviewed the numerical methods for trajectory optimization and discussed the direct and indirect methods. In this paper, we will adopt the direct collocation method to solve the trajectory optimization problem of postcapture manipulation.
Kinodynamic motion planning [29] is first proposed to solve motion planning problem subject to simultaneous kinematics and dynamics constraints. In this paper, a kinodynamic trajectory optimization framework is proposed for generating dual-arm detumbling motion while satisfying the closed-chain kinodynamic constraints between the object and dual-arm space robot. The main contributions of this paper are as follows: (1) a kinodynamic trajectory optimization framework is proposed to minimize the base disturbance of dual-arm space robot for postcapture manipulation of tumbling target; (2) the base disturbance of dual-arm space robot during detumbling manipulation is derived as a function of the position vector of the tumbling target and the total detumbling force exerted on the tumbling target, without calculating dual-arm operational forces, respectively; (3) instead of optimizing the trajectory for each single joint of space robot, the optimal detumbling motion of dual-arm space robot is generated from the optimal trajectory of the tumbling target according to closed-chain kinodynamic constraints.
The remainder of this paper is organized as follows. In Section 2, the dynamics modelling of dual-arm space robot and the tumbling target is presented. In Section 3, the kinodynamic trajectory optimization framework for postcapture manipulation is introduced, and the detailed formulation is presented. In Section 4, the proposed method is verified and compared through physical simulations with different objective functions and initial conditions. Finally, the conclusion and future work are presented in Section 5.
2. Modelling of Dual-Arm Space Robots and Tumbling Target
2.1. Modelling of Dual-Arm Space Robots
As shown in Figure 1(a), the dual-arm space robotic system consists of a satellite base and two central-symmetrically mounted manipulators. The initial and final states of dual-arm space robot and tumbling target during detumbling manipulation are shown in Figure 1(b). The degrees of freedom (DOF) of manipulator Arm-k is denoted by . In this paper, each arm is an S-R-S (spherical-revolute-spherical) 7 DOF redundant manipulator. Moreover, any two adjacent joints are perpendicular without offset. All the variables in Figure 1 are defined in Table 1. The reference coordinate system of Arm-k is the same as the coordinate system of the base of Arm-k. The center of mass (CoM) coordinate system of each body has the same orientation with the coordinate system of each joint . The reference coordinate system of each variable and its corresponding derivative is denoted by the left superscript in the rest of this paper. Unless specified, all the variables are expressed in the inertial coordinate system “” which is omitted for simplification.
(a)
(b)
As shown in Figure 1, the end-effector velocity of each manipulator can be derived as follows: where and are the Jacobian matrices related to the base and the manipulator, respectively, which can be calculated by the following equations: where is the vector from the CoM of the base to the end-effector of Arm-k, ; is the skew symmetric matrix of , i.e.,
The linear and angular momentums of dual-arm space robots can be expressed as follows: where
For the free-floating space robots, the linear and angular momentums are conserved as the environmental force caused by solar pressure, air drag, and microgravity is negligible. The whole system satisfies the following holonomic and nonholonomic constraints:
From (6), the following relationship between the base’s motion and arms’ motion can be obtained:
where
Substituting (7) into (1) yields the end-effector velocity of each manipulator:
Therefore, the generalized Jacobian matrix of dual-arm space robots can be derived as where , , , , and is the coupling Jacobian matrix between the base and the manipulator. The derivation details can be found in [14].
2.2. Motion Equation of Tumbling Target
Assuming that the target is tumbling with an initial velocity, its inertia parameters and are known or can be estimated during the precontact phase and contact phase [30, 31]. Therefore, the motion equation of the target which is captured by the two arms of space robot can be expressed in the base frame as where is the inertia matrix, is the Coriolis and centrifugal force, and is the total force exerted on the target; it also refers to the detumbling force of the target in this paper; is the operational force of Arm-k; are the external force exerted on the target and the corresponding grasp matrix, respectively. The external force caused by solar pressure, air drag, and microgravity is order of magnitude less than the operational forces exerted by the manipulator’s end-effector, and hence, is negligible. The inertia matrix and the Coriolis and centrifugal force can be obtained as follows:
On the other hand, when the end-effectors of dual-arm space robot and the grasping points of the target are connected, the dual-arm space robot and the target form a closed-chain constraint. Considering the postcapture manipulation, the end-effectors of the two arms will be fixed with the grasping points. Therefore, we can have the following position-level constraint: where represents the homogeneous transformation matrix (HTM) of the target with respect to the base, represents the HTM of the reference coordinate system of Arm-k with respect to the base, represents the HTM of the end-effector with respect to the reference coordinate system of Arm-k, and represents the HTM of the target with respect to the grasp coordinate system of Arm-k.
Therefore, given the desired velocity of the target with respect to the base frame of dual-arm space robot, the corresponding velocity of the end-effector of Arm-k can be obtained as where is the position vector from the CoM of target to the grasping point; is the grasp matrix [32] of Arm-k.
Furthermore, by differentiating (15), the acceleration constraint can be obtained as
The kinematics of each manipulator of dual-arm space robot can be written as where is the rotation matrix from the base frame to the reference (base) frame of Arm-k, which is a constant matrix as the base of Arm-k is fixed with the satellite base.
Given the desired velocity and acceleration of the end-effector, the desired joint velocity and acceleration can be obtained directly by the inverse kinematics of the manipulator: where is the Moore-Penrose pseudoinverse of ; for each 7 DOF redundant manipulator of dual-arm space robot, the Moore-Penrose pseudoinverse is used to obtain the least-square solution of differential kinematics with minimum norm.
2.3. Base Disturbance Caused by Detumbling Manipulation
During the detumbling manipulation of dual-arm space robot, the base disturbance resulted from the operational forces of two arms to detumble the tumbling target is analyzed in this section. The dynamic constraints between the two arms and the target are shown in (12), where is equal to zero. Therefore, given the desired motion of the target, the operational forces of the two arms can be obtained. However, there is no unique solution for (12). Many existed algorithms can be used to solve this problem, for example, the master-slave or shared force control proposed in [33] and the optimal distribution method which minimized the squared operational forces proposed in [32].
As the main purpose in this paper is to minimize the detumbling time and base disturbance caused by the detumbling maneuver during the postcapture phase, we only consider the total disturbance force exerted on the base. Therefore, the total disturbance force exerted on the base which is caused by the operational forces of two arms is calculated as follows: where is the position vector from the CoM of base to the end-effector of Arm-k and is the position vector from the CoM of base to the CoM of target.
Combining equations (12) and (19) and the external force on the target is equal to zero, then we can have:
It can be seen from (20) that the disturbance force exerted on the base is related to both the position vector and the detumbling force of the target. The disturbance force should be minimized in order to decrease the base disturbance. Therefore, in the kinodynamic trajectory optimization method presented in the Section 3, the base disturbance force will be minimized as an objective function.
3. Kinodynamic Trajectory Optimization for Detumbling Manipulation
Generally speaking, the process of capturing a tumbling target in space can be decomposed into three phases: the precontact, contact, and postcontact phases. However, the precontact and contact phases are not in the scope of this paper. In order to study the kinodynamic trajectory optimization problem for the postcontact phase, the following assumptions are presented: (1)In the precontact phase, the two arms can reach the grasping point by generalized relative Jacobian [14] or reactionless motion planning method [11](2)In the contact phase, the two arms and the target can form a stable connection for further manipulation [30, 34](3)For a tumbling target, the initial velocity and inertia parameters can be estimated during the precontact phase [30] and the postcontact phase [31]
In this section, the kinodynamic trajectory optimization problem for stabilizing a tumbling target in the postcapture phase will be formulated, in which only the trajectory of the target is optimized while the detumbling motion of two arms is generated from the optimal trajectory of the target by considering the kinematic and dynamic constraints between the target and the two arms. This trajectory optimization problem is transformed into a nonlinear programming problem (NLP) by the direct collocation method [27]. Then, the solution of NLP can be found by the NLP solver fmincon in the Optimization Toolbox of MATLAB. The kinodynamic trajectory optimization algorithm is developed based on open-source trajectory optimization library OptimTraj [35]. The detailed formulation of the kinodynamic trajectory optimization method is shown in the following sections.
3.1. Kinodynamic Trajectory Optimization Framework
For postcontact/capture phase, the space robot servicer and target will form a closed-chain constraint. In order to stabilize the tumbling target in the postcapture phase, the kinodynamic trajectory optimization framework is proposed for generating detumbling motion of dual-arm space robot. Given the initial conditions of the target and the dual-arm space robot, the deceleration trajectory of the tumbling target should be optimized to minimize the detumbling time and base disturbance and avoid singularity of dual-arm space robot, while the detumbling motions of dual-arm space robot can be generated according to the closed-chain kinodynamic constraints between the two arms and the target. The framework of kinodynamic trajectory optimization is shown in Figure 2.
3.2. System Dynamics and Decision Variables
To solve the trajectory optimization problem of the tumbling target, we use direct collocation to discrete the continuous trajectory. For each collocation point, we define the following state variable and control variable as follows: where , , and are the pose, velocity, and acceleration of the target object; the states of two adjacent collocation points are constrained by the following dynamics equation of the object: where and . The attitude in is represented by the -- Euler angles; the angular velocity in the dynamic equation is represented by .
In order to deal with the nonholonomic property of the angular velocity, the transformation between the Euler angle rate and the angular velocity is derived as follows: where is the matrix which projects the Euler angle rate to the angular velocity ; are the abbreviations of sin(), cos(), sin(), and cos().
Therefore, we can obtain the following equation:
Furthermore, we can have the following equation by differentiating (23):
3.3. Constraints of the Trajectory Optimization
For deaccelerate the tumbling target, we can specify the initial and final states of the tumbling target as follows: where and are the initial and final (desired) states of the target, respectively.
Additionally, in order to ensure that the target is within the workspace of the dual-arm space robot, we use box bounds to approximate them in this paper. The state limits of the target object in the trajectory optimization are introduced as follows: where are the minimum and maximum poses of the target and are the minimum and maximum velocities of the target.
During the detumbling manipulation, the force magnitude of each manipulator applied to the target is constrained as follows: where is the maximum force of Arm-k. Through equation (28), the optimal trajectory generation of the target is decoupled from the dynamics of space robot [21]. Additionally, as the proposed kinodynamic trajectory optimization method can not handle the time-variant constraints, the corresponding joint torque can be guaranteed to be below its limit by setting strict end-effector force/torque limit. Therefore, the prescribed maximum end-effector force (28) of each manipulator is designed to guarantee joint torque limits of space robot during manipulation.
In addition to the above explicit constraints, the implicit constraints are also included in the kinodynamic trajectory optimization. As shown in Section 2.3, the base disturbance is calculated according to the kinematic and dynamic closed-chain constraints between two arms and the target.
3.4. Objective Function of the Trajectory Optimization
For space robot, the attitude of the base is generally required to keep fixed with respect to the sun and the earth for communication and observation purposes. However, the fuel of thrusters for attitude control is very limited and mainly reserved for orbital maneuvers. Therefore, the trajectory optimization problem of postcapture phase is formulated to minimize the detumbling time and base disturbance force, i.e., minimize the energy consumption during the whole detumbling manipulation. For the dual-arm space robot, the base disturbance mainly comes from the operational forces of two arms for detumbling the tumbling target as shown in Section 2.3.
Furthermore, the inverse kinematic equation of dual-arm space robot can be obtained from (10):
The singularity of dual-arm space robot occurs if the generalized Jacobian matrix is not full ranked. In order to avoid the singularity, we added another weighted function into the objective function of trajectory optimization. This function is the negative manipulability of dual-arm space robot based on the generalized Jacobian matrix, which is defined as follows:
By minimizing (maximizing the manipulability), dual-arm space robot can keep away from the singular configuration. Therefore, the objective function of this trajectory optimization for stabilizing the tumbling target can be written as the following (equality constraints (22) and (26) and inequality constraints (27) and (28)): where is the detumbling time and is the disturbance force of the base during the detumbling manipulation. Among the three items in the objective function, the base disturbance force and manipulability are much more important than the detumbling time; , , and are the weights to trade off the detumbling time, base disturbance, and manipulability. Unless specified, , , and are set to 1 in this paper.
4. Simulation Study
In order to verify the kinodynamic trajectory optimization method proposed in this paper, simulation studies with different objective functions and initial conditions are carried out. In Section 4.1, the proposed kinodynamic trajectory optimization framework is used to minimize the detumbling time and the base disturbance of dual-arm space robot during detumbling manipulation. In Section 4.2, the proposed method is used to deal with a general case of stabilizing the tumbling target while considering the singularity avoidance.
For detumbling manipulation in postcapture phase, the two arms have formed a stable connection with the target. Without loss of generality, the initial configuration of the dual-arm space robot and the target in the simulation study are shown in Figure 3. The initial state of Arm-l and Arm-r are set to and , respectively. The mass and inertia parameters of the base, each manipulator, and the tumbling target are shown in Table 2. Unless specified, the length and angle units are and , respectively. In order to evaluate the results generated from trajectory optimization, the base disturbance metric is defined as follows: where and are the position and orientation disturbance of the base and and are the corresponding weights for position disturbance and orientation disturbance.
4.1. Minimal Detumbling Time and Base Disturbance
In this section, the objective function of the trajectory optimization is set to minimize the detumbling time and the base disturbance caused by the detumbling force of two arms. The kinodynamic trajectory optimization is carried out with the dynamic equation of the tumbling target while the corresponding motions and forces of the two arms are generated by the kinematic and dynamic constraints between the two arms and the target. The constraints for the trajectory optimization are listed in Table 3. The initial guesses for control variable are set to zero.
For space robots, the attitude stabilization is much more important than position as the specific attitude is required to guarantee communication and solar energy utilization. Without loss of generality, the weight coefficients for base disturbance force and base disturbance torque in the objective function are set to 0 and 1 correspondingly. The generated trajectory for stabilizing a moving target is performed in the gravity-free simulation environment as shown in Figure 4.
The position and linear velocity trajectories of the target are shown in Figure 5. The corresponding position and attitude disturbances of the base of dual-arm space robot during the manipulation are shown in Figure 6. This result is compared with the optimization result of minimizing the detumbling time and the detumbling force at the end-effector of each manipulator [21]. As shown in Figure 6, the attitude disturbances of the base resulted from these two different objective functions are [-0.09, -0.34, -0.79] and [-0.12, -0.38, -0.85], respectively. It should be noted that minimizing detumbling force is not equivalent to minimizing the base disturbance. The trajectories of the tumbling target and corresponding disturbance metrics in the above-mentioned two different cases are shown in Figure 7. It can be seen that the trajectory which minimizes the base disturbance is different from the one minimizing detumbling force and consequently has smaller base disturbance.
In order to show the capability and robustness of the proposed kinodynamic trajectory optimization method, we carry out several simulations in which the mass and inertia ratio between the target and the base of dual-arm space robot is set to 1 while the uncertainties in mass and inertia parameters of the target are also considered as shown in Table 4. The optimal trajectories corresponding to different mass and inertia of the target are shown in Figure 8, which can be generated from the proposed kinodynamic trajectory optimization. However, in order to show the robustness of this method, dual-arm space robot only adopted the optimal trajectory (solid line in Figure 8) where the mass and inertia of the target and base are both set to 100 kg and [20, 20, 20] kg.m2. Considering 0 (solid line), 10% (dashed line), and 20% (dash-dotted line) mass and inertia uncertainties of the target, the corresponding base disturbance force is shown in Figure 9. It can be seen that for the same optimal trajectory of the tumbling target, the smaller mass and inertia parameters will result in a smaller base disturbance. Therefore, considering the detumbling manipulation of space target with initial mass and inertia uncertainty, we can choose the maximum value for mass and inertia in the optimization to get a conservative detumbling solution and increase the robustness of the optimal solution.
Furthermore, we consider different initial velocities in the trajectory optimization where the mass and inertia of the target and base are both set to 100 kg and [20, 20, 20] kg.m2. For different initial linear velocities [0.20, 0.10. 0.0], [0.22, 0.11, 0.0], and [0.24, 0.12, 0.0], the optimal trajectories generated from the trajectory optimization are shown in Figure 10(a). The corresponding base disturbance metric is shown in Figure 10(b). It can be seen that for the same tumbling target, the larger initial velocity will result in a larger base disturbance.
4.2. A General Case for Singularity Avoidance
For stabilizing the tumbling target, a general case is considered in which the objective function is the same as Section 4.1 and the constraints condition is shown in Table 3. However, in order to verify the singularity avoidance capacity of the trajectory optimization framework, the initial velocity of the target is set to [0.15 0.10 0 0.05 0.04 0.03], which may cause the singularity of dual-arm space robot because of the initial linear velocity and angular velocity.
The simulation results of trajectory optimization without singularity avoidance (i.e., ) are shown in Figure 11. It can be seen that the singularity happens around 8 s. The joint angular velocities under singularity and manipulability of dual-arm space robot are shown in Figure 12, from which it can be seen that the singularity happens twice at 5.6 s and 8.4 s, respectively. On the other hand, the simulation results of trajectory optimization with singularity avoidance are shown in Figure 13. The optimal trajectories for the velocity of the target are shown in Figure 14. The corresponding base linear and angular velocity are shown in Figure 15. It can be seen that the dual-arm space will move with the tumbling target during the detumbling manipulation as the base is not actively controlled. The corresponding joint angular velocities and manipulability of dual-arm space robot are shown in Figure 16. The simulation results show that dual-arm space robot can keep away from the singular configuration by maximizing the manipulability. Therefore, the singularity avoidance problem of dual-arm space robot can also be solved in the kinodynamic trajectory optimization framework.
(a)
(b)
(a)
(b)
(a)
(b)
(a)
(b)
5. Conclusion
In order to stabilize the tumbling target to a desired pose for further maintenance and manipulation, the kinodynamic trajectory optimization method is proposed for postcapture phase in this paper. Instead of minimizing the detumbling time and detumbling force, the objective function is formulated to minimize the detumbling time and base disturbance caused by the dual-arm detumbling force of dual-arm space robot. To verify the proposed method, several physical simulations with different initial conditions and objective functions are carried out. The results show that the trajectory generated from the proposed kinodynamic trajectory optimization method which minimizes the base disturbance force can result in smaller base disturbance than other objective functions and avoid singularities of dual-arm space robot. Therefore, the energy of dual-arm space robot can be saved for further manipulation. The proposed kinodynamic trajectory optimization method can be used to plan the trajectory of space robots for on-orbit manipulation. For implementing the proposed method on real space robot system, warm start is needed to decrease the computation time. The multiple capturing phases, including precontact, contact, and postcontact phases, will be also considered into the whole trajectory optimization in future work.
Data Availability
Data is available on request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work was supported by the National Key R&D Program of China (Grant No. 2018YFB1304600), the Shenzhen Excellent Scientific and Technological Innovation Talent Training Project (Grant No. RCJC20200714114436040), and the Basic Research Program of Shenzhen (Grant No. JSGG20200103103401723). This work was also partially supported by the Shenzhen Municipal Basic Research Project for Natural Science Foundation (Grant No. JCYJ20190806143408992) and the Guangdong Basic and Applied Basic Research Foundation (Grant No. 2019A1515110680).