Trajectory Planning with Pose Feedback for a Dual-Arm Space Robot

In order to obtain high precision path tracking for a dual-arm space robot, a trajectory planning method with pose feedback is proposed to be introduced into the design process in this paper. Firstly, pose error kinematic models are derived from the related kinematics and desired pose command for the end-effector and the base, respectively. On this basis, trajectory planning with pose feedback is proposed from a control perspective. Theoretical analyses show that the proposed trajectory planning algorithm can guarantee that pose error converges to zero exponentially for both the end-effector and the base when the robot is out of singular configuration. Compared with the existing algorithms, the proposed algorithm can lead to higher precision path tracking for the end-effector. Furthermore, the algorithm renders the system good anti-interference property for the base. Simulation results demonstrate the effectiveness of the proposed trajectory planning algorithm.


Introduction
Space robots have become a hotspot in the development of space technology.They have been applied to many areas, including capturing the abandoned satellites, servicing aircraft equipment, and removing junk from the satellite orbit.Compared with a single-arm space robot, a dual-arm robot can perform more complex missions and has better flexibility and dexterity.Many countries are paying more and more attention to the development of space robot, which can improve the efficiency of space exploration [1][2][3][4][5].
For continuous path tracking, kinematic singularity is an inherent characteristic of the mechanical arm, and it can lead to infinite joint angular velocity which cannot be achievable in practice.To deal with this problem, many researchers proposed the singularity avoidance method which ensures the continuity and finiteness of the joint angular velocity around singular points [6].A commonly used approach is damped least-squares (DLS) method [7], but it sacrifices the pose precision in all directions.On this basis, Chiaverini proposed a method in which only the minimum singular value is needed to introduce the damping coefficient [8].Buss and Kim presented selectively damped least-squares (SDLS), which adjusts the damping coefficient separately for each singular vector [9].Pechev used a control method against singularity by minimizing the difference between desired and real Cartesian velocity [10].The above inverse kinematic solution at velocity level only has a good performance on tracking velocity while the Jacobian matrix is nonsingular.The introduced damping coefficient results in the pose error in the case of singularity.The error is cumulated before the robot leaves the singularity region.However, if the trajectory planning is based on velocity level kinematics, the error does not vanish after the singularity is handled.This disadvantage might lead to the failure of the robot in finishing the required task.
In microgravity environment of space, due to the dynamic interaction between base and arms, the movement of arms might lead to disturbance on the base.The disturbance also occurs when the base is disturbed by external impact, such as when the propulsion system fails which causes chaotic jet, or the satellite has a collision with other space objects.The disturbance might make the base deviate from the desired pose.This can result in the failure of on-orbit operation tasks.Therefore, it is important to keep the base stabilized when the space robot is performing a task; consequently this problem receives extensive research interest [11].Nakamura and Mukherjee proposed a bidirectional method of trajectory planning which can control mechanical arm configuration as well as the base attitude [12].Matunaga et al. presented a method of rotational motion-damper [13], by which the coupling angular momentum on the base can be gradually removed to keep the base fixed.A coordinated planning method is proposed by Huang et al., in which the balance arm is used to reject the effect of the mission arm on the base [14].Because the above methods use inverse kinematics solution at velocity level, the base pose error does not vanish when the balance arm crosses a singularity region.In order to solve the unavoidable singularity problem for those methods based on differential kinematic equation, Xu et al. [15] gave a method based on position-level kinematic equation to avoid the singularity problem of the balance arm, and there are not any special constraints of the balance arm.Yan et al. [16] used Xu's method successfully to other different configurations of space robot.However, the singularity problem of mission arm still exists and this method does not work when the base is disturbed by the external impact.
In this paper, in order to reduce the pose error which exists in the above method, pose feedback is introduced into the design process to obtain high precision path tracking.The design procedure is carried out from a control perspective.To this end, pose error kinematic models are derived based on the velocity level kinematics and the desired tracking path.Then the joint angular velocities are regarded as the control of the pose error kinematic system.On this basis, the trajectory planning with pose feedback is accomplished both for the end-effector and the base by solving the corresponding control problem.
This paper is organized as follows.Section 2 derives the kinematic model of a dual-arm space robotic system.Section 3 addresses trajectory planning algorithm for the end-effector and the base.Some simulations are presented in Section 4 to demonstrate the effectiveness of the proposed algorithm.Conclusions follow in Section 5.

System Modeling
The dual-arm space robotic system in this paper has the following four parts: a satellite base, a mission arm, a balance arm, and a set of reaction wheels.The mission arm has six DOFs (degree of freedom) and is used to complete a task, such as tracking or capturing a target object.The balance arm has three DOFs and is employed to keep the base centroid fixed.The reaction wheels serve to stabilize the attitude of the base.Figure 1 shows the model of the dual-arm space robot system.For convenience, some symbols are defined as follows:  = , : labels of the mission arm and the balance arm, respectively, Σ  : the inertia frame, Σ  : the end-effector frames of arm-, The satellite base

Velocity Kinematics of the Dual-Arm Space Robot.
The differential kinematic equation of the end-effector can be described as [15] where is the velocity of the base, J 0 is the Jacobian matrix of the base, J  is the Jacobian matrix of the mission arm, and θ  is the joint angular velocity of mission arm.The linear and angular momentum conservation equations of the system can be described as the following: where P, L are the linear and angular momentum of the system, respectively, E is a third-order unit matrix, M is the total mass of the system, r  is the system centroid, r 0 is the position vector from the system's centroid to the base's centroid, r is a cross-product operator [1], θ  is the joint angular velocity of balance arm,   is the velocity vector of the reaction wheels, I  is the inertia matrix, and J   and H   denote the coupling inertia matrix of arm-.Equations ( 2) and ( 3) can be combined to get the following form: where , Thus, the velocity of the base can be determined according to (4): that is,

Pose Error Kinematics of the Dual-Arm Space Robot.
The position error e  of the end-effector can be defined as where p and p  represent the real value and the desired one of the position of the end-effector, respectively.Taking the derivative of e  with respect to time, we get where v  is linear velocity vector of the end-effector.The attitude error can be calculated by the following formula: where q  = [ 0  1  2  3 ]  = [ 0 q V ]  is the attitude error quaternion of the end-effector, q is the real attitude quaternion, and q  is the desired attitude quaternion.
The attitude error can also be reformulated by using Modified Rodrigues Parameters (MRP) as The shadow set of MRP [1] is defined as It is clear from (11) that ‖e  ‖ 2 > 1 if ‖e  ‖ 2 < 1.Thus, one can always restrict the magnitude of MRP vector from exceeding unity by switching to the shadow MRP when needed; thus, the singularity can be avoided when MRP is used to represent the attitude error of the end-effector [1].
Using MRP, the kinematic equation of attitude error can be described as where R ∈  3×3 is the rotational matrix of the real attitude relative to the desired attitude, w  is angular velocity vector of the end-effector, and G(e  ) is given by and it has the following property: Now the pose error is defined as e = [e  e  ]  ; combining (8) and ( 12) yields where , where V  is used to denote the desired velocity of the endeffector.Substituting (1) into (15), the pose error kinematic equation of the end-effector can be obtained as For the trajectory planning of the balance arm, the desired velocity of the base is zero which means the base is fixed.Let e 0 denote the pose error of the base, using (6), perform the similar derivation as that done for the pose error kinematics of the end-effector, and the pose error kinematic equation of the base can be expressed as

Trajectory Planning for the Dual-Arm Space Robot System
As is known, when the mission arm crosses a singular configuration, the tracking error will occur by using the previous singularity avoidance methods.As was mentioned at the beginning of this paper, most of the existing methods are based on the velocity kinematics; if the trajectory planning is carried out based on the velocity level kinematics, the velocity error will vanish when the robot goes out of the singular configuration.But the pose error will not change.There are methods which solve the singularity problem by carrying out the trajectory planning based on position-level kinematics; however, this method becomes invalid for some emergency cases in which the base's centroid pose changes sharply.Due to the above reasons, instead of the methods which are based on the velocity level kinematics, we carry out the trajectory planning on the basis of the pose error kinematics given by ( 17) and (18).Firstly, trajectory planning is performed for the mission arm based on the system given by (17).The joint angular velocity of the mission arm is seen as control; thus, the trajectory planning problem is transformed as a control problem.Then the pose error of the end-effector is introduced in the course of trajectory planning for the mission arm such that a high precision tracking is obtained for the end-effector.Once the trajectory planning is finished, θ  can be seen as constant vector, and θ  and   can be seen as the control vector of the system given by (18).Thus, we can also regard the trajectory planning problem of the balance arm as a control problem.Then a controller is designed to employ the base pose feedback to keep the base fixed with high precision.

Trajectory Planning for the Mission Arm.
In this subsection, trajectory planning is carried out for the mission arm based on the system given by (17).The goal of the control synthesis is to make the closed loop system respond according to the follow equation: where k  can be chosen as a diagonal matrix, in which the diagonal elements are positive real numbers.It is easy to see that this system is exponentially stable, and satisfactory response speed can be obtained by proper choice of the diagonal elements of k  .In order to achieve the above-mentioned design objective, from ( 17) and (19) we get It can be obtained from ( 14) and ( 16) that J  is invertible.If J  is full rank, then the joint angular velocity of mission arm can be chosen as When J  is singular, the improved DLS method [17] can be used to deal with the singularity.In this case, the joint angular velocity of the mission arm can be chosen as where    is the singular value of J  , k   , and u   which are the corresponding singular vectors and    is the damping coefficient which is adjusted according to the following formula: where  is a threshold and is used to determine whether the mission arm encounters singularity and   can be chosen as a constant.It can be obtained that   is the maximum value of    .

Trajectory
Planning for the Balance Arm.Now, we perform the trajectory planning for the balance arm based on the system given by (18).The control objective is to make the closed loop system exhibit a similar response as that given by (19); namely, the closed loop system is to be synthesized according to where k 0 can be chosen according to the desired convergence speed of the base pose error.Based on this design goal, combining (18) with (24), we obtain the result after simplification: Substituting ( 5) into (25), we have This equation can be decomposed into the following two equations: If J   is invertible, it can be obtained from (27a) that the joint angular velocity of the balance arm can be computed by When J   is not full rank, the singularity can be handled by employing a similar method as for the trajectory planning of mission arm.Now θ  is known, thus from (28), the angular velocity of the reaction wheel can be obtained as follows:

Simulations
The trajectory planning of the dual-arm space robot is simulated to demonstrate the effectiveness of the proposed algorithms.The space robotic system is composed of a satellite base, a 6-DOF mission arm, a 3-DOF positon balance arm, and a three-axis attitude balance reaction wheel.The arms are installed symmetrically on the base: that is   = 0 ∘ ,   = 180 ∘ .The mass properties of satellite base, mission arm, and balance arm are shown in Table 1.The inertia moments of reaction wheels are   =   =   = 1.5 kg⋅m.

Simulations For Path Tracking When Crossing Singularity
Region.In this subsection, in order to compare the tracking precision of the proposed method with the previous method, a command trajectory of end-effector is assigned such that the mission arm crosses a singularity region.The comparison simulations are carried out for the proposed method and Xu's method [15].The initial linear and angular momentums of the system are assumed as P 0 = 0, L 0 = 0, respectively.The pose  of the base centroid and that of the end-effector are assigned the following initial values, respectively: The desired path of end-effector is shown as in Figure 2.
The simulation results are given by Figures 3 and 4. The position error of the end-effector is illustrated in Figure 3.The attitude error of the end-effector can be seen in Figure 4.It can be observed that the end-effector has observable pose error between  = 2.95 s and  = 8.12 s.The underlying reason is that the mission arm has entered and remained in the singularity region during this period.For comparison, the related position vector is transformed into a scalar by using 3-axis superposition, and the attitude error is repressed as rotation angle.Thus, both the position error and the attitude error can be quantized by a scalar.As can be seen, when the proposed method is used, the maximum position error of the end-effector is 4.521 × 10 −4 m, and the maximum attitude error is 1.39 ∘ , and these errors approximately converge to zero when  = 12.65 s.However, the maximum position error is 3.501 × 10 −3 m, and the maximum attitude error is 1.86 ∘ when Xu's approach is used.And these errors still exist after the mission arm goes out of the singular configuration.The linear velocity and angular velocity of the base are plotted in Figure 5.This figure shows that the base is stabilized.

Simulation of Anti-Interference for the Base.
To demonstrate the disturbance rejection property of the proposed method against external impact on the base, a disturbance is exerted on the base at  = 6 s.The velocities of the disturbed base are shown in Figure 6.
The simulation results of the position errors and attitude errors of the base are shown in Figures 7 and 8, respectively.
As can be observed, when the proposed method is employed, the maximum position error of the end-effector is 1.9 × 10 −3 m, the maximum attitude error is 0.113 ∘ , and the errors converge to zero rapidly.However, Xu's approach leads to maximum position error of 4 × 10 −3 m and a maximum attitude error of 0.126 ∘ , and worst of all, the pose error still exists after robot leaves the singularity region.This comparative analysis shows that the proposed algorithm can accommodate external impact on the base better than Xu's approach.
From the simulation results, it is clear that the proposed method can lead to higher tracking precision of the end-effector comparing with Xu's one when the space robot crosses a singularity region.Furthermore, good antiinterference property for the base can be obtained by using the proposed algorithm.

Conclusion
In this paper, a new trajectory planning method for a dualarm space robot is proposed by using pose feedback from   Then the joint angular velocities are seen as the control of the error kinematic system; thus, high precision trajectory planning is obtained by controller design for the related error kinematic system.Compared with the previous method which cannot solve the problem of the related pose error, the proposed method can guarantee that the related pose errors tend to zero exponentially.The simulation results show that the trajectory planning approach is effective not only for eliminating the pose error resulting from singularity, but also for resisting the disturbance of the base.Furthermore, there are not any requirements for the configuration and mass properties of the robot in this method.Therefore, this method has good engineering practicability.

Figure 1 :
Figure 1: The model of the dual-arm space robotic system.

Σ
: the geometry reference of the base,  0 ,  0 : the base satellite and its center of mass,   ,   : DOF of arm- and arm-, respectively: in this paper,   = 6 and   = 3,    ,    ( = 1, . . .,   ): th link and joint of arm-, respectively,    : the position of    's center of mass, a   , b   : position vectors from    to    and    to   +1 , respectively, b  0 : the position vectors from  0 to   1 ,   : the rotation angle from vector b  0 to the -axis of Σ  , v 0 , w 0 : the linear and angular velocities of the base, respectively, v  , w  : the linear and angular velocities of the endeffector, respectively.

Figure 2 :Figure 3 :
Figure 2: The desired position and attitude tracking path of the end-effector.

Figure 4 :
Figure 4: The variation of the end-effector's attitude errors.

Figure 5 :
Figure 5: The linear and angular velocities of the base.

Figure 6 :
Figure 6: The linear and angular velocities of the disturbed base.

Figure 7 :
Figure 7: The variation of the base's positon errors.

Figure 8 :
Figure 8: The variation of the base's attitude errors.

Table 1 :
The mass properties of satellite base, mission arm, and balance arm.