Kinematic analysis and fault-tolerant trajectory planning of space manipulator under a single joint failure

A space manipulator plays an important role in spacecraft capturing, repairing, maintenance, and so on. However, the harsh space environment will cause its joints fail to work. For a non-redundant manipulator, single joint locked failure will cause it to lose one degree of freedom (DOF), hence reducing its movement ability. In this paper, the key problems related to the fault-tolerant including kinematics, workspace, and trajectory planning of a non-redundant space manipulator under single joint failure are handled. First, the analytical inverse kinematics equations are derived for the 5-DOF manipulator formed by locking the failure joint of the original 6-DOF manipulator. Then, the reachable end-effector pose (position and orientation) is determined. Further, we define the missions can be completed by the 5-DOF manipulator. According to the constraints of the on-orbital mission, we determine the grasp envelope required for the end-effector. Combining the manipulability of the manipulator and the performance of its end-effector, a fault tolerance parameter is defined and a planning method is proposed to generate the reasonable trajectory, based on which the 5-DOF manipulator can complete the desired tasks. Finally, typical cases are simulated and the simulation results verify the proposed method.


Background
SPACE manipulators are expected to be widely used in various space missions, including constructing large space structures, removing orbit debris, and repairing malfunctioned satellite [1][2][3]. However, a space robot in harsh space environment is prone to failure, resulting in decreased performance of it. It is extremely difficult, even impossible, to repair or replace these malfunctioning devices on orbit. Therefore, fault tolerance is critical for space manipulators. Because each joint is generally controlled and driven independently, we can deal with the problem respectively when a joint fails. There are mainly two types of joint failure: locked [4,5] and free-swinging [5,6] failures. The former refers to a malfunctioning joint that is constrained mechanically, whereas with the latter, actuator torque is lost, and the joint revolves according to the coupling torques of the other joints. In fact, the most common joint failures are joint locked failures. So this paper focuses on the fault-tolerant planning under single joint locked failures. This failure covers two cases: active locking, in which a joint can be locked by fail-safe brakes, and passive locking, in which a joint is locked unexpectedly due to mechanical failure. For previous works [7,8], the scholars studied the fault-tolerant planning and control methods for redundant manipulators. The redundancy is used to compensate for the motions of the failed joint to continue the designed tasks. However, for a non-redundant manipulator, such as a 6-DOF (degree of freedom) space manipulator, these methods can not be directly applied.
When a joint is locked, a 6-DOF manipulator will decrease to a 5-DOF manipulator, which has insufficient degrees of freedoms to make its end-effector freely move in 3D space. The kinematics, workspace, and trajectory planning are much more different from the original 6-DOF manipulator. There are two types of methods to solve the inverse kinematics of such manipulator with single locked joint: One is to directly solve the 5-DOF manipulator; the other is to construct a new 6-DOF manipulator with a virtual joint. For the former, there are various methods [9,10] to obtain the inverse solution of the manipulator. The methods based on vector algebra and linear transform [11][12][13] are always used to solve the inverse solution of a manipulator with insufficient degrees of freedoms. Such manipulator can not attain some given pose (attitude and position), i.e., there will be no any rational inverse solutions for some desired pose. Correspondingly, there will be no rational trajectory to achieve the desired pose. If single variable arc tangent function [14,15] is used in solving the joint angles, one solution of arc tangent function may be lost.
On the other hand, a virtual joint can be added to construct a new 6-DOF manipulator. Li Xiaotang [16] assumed that the robot grabs bar-like object, so that an attitude parameter corresponding to the gripper rotating around the centerline of rods is not considered. The attitude parameter was regarded as a virtual rotary joint. Then a numerical method was used to solve the inverse kinematics equations. Similarly, Masayuki Shimizu [17] and Zhang Chengkun [18] also constituted a new 6R robot from the 5-DOF manipulator by using a virtual rotary joint. The inverse solution can be derived according to the kinematics of 6-DOF manipulator. However, these papers did not deeply analyze the fault tolerance at any configuration by considering arbitrary joint which is locked.
To take full advantage of space robot under a single joint failure, this paper carries out the workspace analysis for any joint which is fail to work. The bivariate tangent function is used to solve the joint variables, and the quadrant can be determined by the symbolic of independent variable. Then, the analytical inverse solution of the space manipulator under a single joint failure is obtained. By introducing the concept of fault tolerance angle, the 5-DOF robot can complete more tasks at various fault conditions under different fault tolerance requirements. Finally, the Cartesian linear trajectory planning simulation under the 2nd joint failure is performed. The simulation results show the effectiveness of the algorithm.

Kinematics modeling and workspace analysis
According to the structure of the robot, the DH method is used to establish the coordinate system of each link; it is shown in Fig. 1. The corresponding DH parameters are listed in Table 1. Maintop Carlo method is used to obtain the normal workspace which is shown as Fig. 2. Without loss of generality, joint 2 is regarded as the fault joint, and its workspace is shown in Fig. 3; obviously, its workspace becomes a subspace of normal manipulator's workspace

Inverse kinematics
The main existing inverse solution method for space robot includes analytical method, geometric method, numerical methods and neural network method. Generally, the inverse solution of joint angles is calculated in the range of 360°, and if a single variable arc tangent function is used in solving joint angle, an arc tangent function of the solution may be lost. So the analytical method and bivariate arc tangent function are adopted to solve the inverse solution of space robot in this paper.

Analytical inverse kinematics of a 5-DOF manipulator
For adjusting the structural parameters of the failure joint and the adjacent joints, the rotation matrix of failure joint is still remained in the kinematic equations, and the current angle of failure joint locked is substituted into kinematic equations. For different failure joints and location, only the corresponding joint variables in the kinematics should be converted to a constant which is currently locked rotation angle. Then the normal analytical solution algorithm is used to solve the equations. This method can improve the solvability of insufficient DOF manipulator and achieve its trajectory planning; this will make sure 6-DOF manipulator still accomplishes most tasks in case of a single joint failure.
If the pose of end-effector has been given and θ 2 is known, the value of θ 1 , θ 3 , θ 4 , θ 5 , θ 6 can be determined as follows The corresponding elements (3,4) in both sides of matrix Eq. (1) are equal:

Fig. 1 Coordinate system of the manipulator
The expression of θ 1 can be solved as follows: or, The elements (1,4), (2,4) of the left and right sides of (1) are respectively equal: The expression of θ 3 can be solved as follows: The left and right sides' elements (1,3), (2,3) of (1) are respectively equal: Then the expression of θ 5 can be solved as follows: The both sides' elements (3, 3) of (1) are respectively equal: The expression of θ 4 can be solved as follows: The elements (3, 1), (3, 2) of the both sides of (1) are respectively equal: The expression of θ 6 can be solved as follows:   It can be seen from the above derivation process of inverse kinematics. When the second joint is failure, θ 1 , θ 3 , θ 4 , θ 5 , θ 6 can also be obtained from the formula corresponding to the value of θ 2 . Similarly, the analytical inverse solution of other joint failure can also be analyzed.

Analytical inverse kinematics of a 6-DOF manipulator
In order to avoid the possibility of the solution being lost during the inverse solutions of the manipulator joint angles process, bivariate arctangent function is used in this paper, and then the quadrant of the joint angle can be determined by the argument symbolic. If the pose of endeffector has been given, namely n, o, a, p is known, the value of θ 1 , θ 2 , θ 3 , θ 4 , θ 5 , θ 6 can be determined. This paper gives a solution results directly.
The solutions of joint θ 1 , θ 2 , θ 3 which determine the position are as follows: The solutions of joint θ 4 , θ 5 , θ 6 which determine the attitude are as follows:

Fault-tolerant planning based on the inverse solution of 5-DOF manipulator
In practice, when a joint of the 6-DOF manipulator fails, the system will lock the joint and keeps it in the current angle. In this way, the 6-DOF manipulator becomes a 5-DOF manipulator, which also names the insufficient DOF manipulator. Actually, it is vital for the insufficient DOF manipulator to complete the expected task accurately by the effective artificially motion control and trajectory planning. For example, a 6-DOF manipulator in aerospace cannot be put to work anymore when a failure happened in one of its joint. But if we use the control system with a new inverse solution algorithm instead of the inverse solution algorithm, in spite of the fact that the manipulator has become the insufficient DOF manipulator, the manipulator can still reach the most pose in its original workspace, and this means that the robot can still put to work and complete the most planning tasks which will improve the capacitive and availability of the entire spacecraft system. When there is a single joint failure in a 6-DOF manipulator, its related workspace will be reduced accordingly, as shown in Fig. 3. Therefore, the manipulator is still able to complete part of its task based on its ability to work. As for the tasks that a 5-DOF manipulator with the fault can complete, we can carry out the trajectory planning with conventional planning methods, including getting the inverse solution in accordance with "Analytical inverse kinematics of a 5-DOF manipulator" of formula 5-DOF and calculate the angles about the movable joint.

Fault-tolerant planning based on the inverse solution of 6-DOF manipulator The introduction of fault-tolerant method
The insufficient DOF manipulator can only reach part of the position and attitude in their original workspace, so that the inverse solution of the desired position and attitude may not exist. Because not all the location and attitude are solvable, the unrealizable attitudes of manipulator will lead to the failure of expected tasks. In most cases, the vector algebra, linear transformation, and other methods are used to get the inverse solution about the position of the insufficient DOF manipulator. But if there is no inverse solution of a certain posture when using the ordinary inverse solution algorithm, the manipulator trajectory planning cannot be achieved and the insufficient DOF manipulator with a failure joint will not able to complete the task. Therefore, it is full of high research value and practical value to study inverse solution algorithm about the position of 6-DOF manipulator with fault-tolerant performance.
When the manipulator intends to grab the rod or other similar objects, we will not restrict the attitude parameters if the gripper rotates around bar's centerline. Then the rotation variables can be regarded as a virtual unknown rotary joint which is shown in Fig. 4, so inverse position problem of the insufficient DOF manipulator can be converted into an inverse solution problem. Due to the change in structural parameters and the uncertainty of the joints and location of the fault, the inverse solution program must meet the requirements of the uncertain fault state. In this paper, we traversed all the value of the rotation angle around the X-axis and obtained overall relationship diagram between the rotation angle around X-axis and the corresponding joint with a failure in order to select the proper angle for the planning values.
The algorithmic process is as follows: 1. When a single joint fails, the desired joint angle of each joint can still be substituted into the kinematic equations in order to calculate the target point relative to the base coordinate system of the position and attitude matrix e 0 T; 2. The matrix e 0 T is multiplied with the homogeneous transformation matrix of the rotational angle γ which rotates around X-axis of the tool coordinate system, we obtain e  6. After determining each desired angle, the trajectory will be re-planned in accordance with the analytic solution of inverse kinematics of the 5-DOF to complete scheduled tasks.
The algorithm takes advantage of the normal arm inverse kinematics, so that we don't need to re-identify the parameters and reconstruction of the manipulator even when the joint fails. After obtaining the graph, we can consider the actual fault condition about the fault tolerance and the size of angle in the fault joint. As a result, when the target matrix rotates around the X-axis in coordinate system with the angle γ, the insufficient DOF manipulator can still be able to complete the scheduled tasks due to the substitute motion by other joints. But if a joint failure occurs in some less flexible operating space, the value of angle γ calculated by this method will be relatively large, which leads to the fact that the manipulator will fail to complete scheduled tasks with the solution of the six joint angle values, because the corresponding position and posture of the manipulator may cause a collision between the end-effector and the target object.

The analysis of fault tolerance under a single joint failure
We can carry out the work about the mission planning under the condition that the fault angle of joint 2 is 60°. Also we set the initial movement joint q0 = [0, 60, 0, 0, 0, 0]°, while the terminate movement joint: qf = [5, 60, 35, 10, −120, 90]°; the initial speed: qv0 = [0, 0, 0, 0, 0, 0]; the termination rate: qvf = [0, 0, 0, 0, 0, 0]; from Fig. 5, it can be seen that when the rotation angle rotating around the X-axis changes in the range of ±180°, fault compensation range of joint 2 is 48° to 123°. As shown in Fig. 6, when the Fig. 5 Overall relationship between the angle rotating about the X-axis and joint 2 diagram angle which is the movement attitude of the end-effector rotates about the X-axis, the failure compensation range of the joint 2 is 59.2° to 60.8°.
It is assumed that a failure occurs in the second joint of the space robot when its angle is 59.4° and we can get γ = −1.45 according to the fault-tolerant algorithm. At this point the six joint angle values obtained for the target point and performing the tasks are qf = [5.18, 59.40, 35.51, 9.89, −121.40, 89.76]°. It means that when the failure occurs in the second joint, in order to approach the target, the space robot will need other joints' substitute motion. When the end-effector reaches the desired position, the rotation angle will be equal to −1.45° in this case. Figures 7 and 8

The trajectory planning and simulation about fault tolerance under the typical tasks
Based on the Simulink, the SimMechanics is an interdisciplinary research and analysis environment for the controller and the target system. Acting as an intuitive and effective modeling and analysis tools for the multibody dynamic mechanical systems and control systems, the SimMechanics completes all its work in the Simulink environment. In this paper, ProE5.0 and MatlabR2010b jointly establish a simulation systems as shown in Fig. 9, so as to carry out simulation. Taking the linear motion in Cartesian space for example, if we know the starts and ends coordinates A(x a , y a , z a ),C(x c , y c , z c ) of the line in the space and the interpolation number N, then we get:   Fig. 11. The change in the attitude and position of the end-effector is shown in Fig. 12. Therefore, we can get the tracking error of the end position that is less than 2° from Fig. 13, which means that the position eventually reaches our expectation. The speed of the attitude and position of the end-effector are shown in Fig. 14. Applying the PD control to each joint according to the linear motion in the Cartesian space of the end-effector, we obtain the control force of each joint as shown in Fig. 15.   We carry out the task planning of the fault tolerance in the condition that the joint 2 fails (similar to the situation in other joints) when the manipulator is working if it cannot achieve the desired angle of 60°. When the joint fails at 52.0547°, as we can see in Fig. 5, the posture tolerance that the end-effector rotates around the X-axis is −10°. So the actual posture can be obtained: After the introduction of fault tolerance angle, the actual configuration of manipulator can be determined at this time. This indicates that when the manipulator has not yet reached the desired second joint of 60°, it is necessary for the other joints to do some replacement motion so that the end-effector of the robot can get closed to the target ultimately and work with the posture that the original desired position rotates around the X-axis with −10°. According to the actual location of the fault of the joint, we can re-plan the end of the gesture to carry out the task in the case of without affecting the arrest. By solving it can be found that when the joints fail at 52.0547°, the fault tolerance angle is −10°, which can be substituted by the movement through other joints, and the manipulator still is able to complete the task.  The initial and final states are as shown in this section in Fig. 16. Linear motion planning cycling position is 250 ms. The angle variation of each joint is shown in Fig. 17. The change in the attitude and position of the end-effector is shown in Fig. 18. Therefore, we can get the tracking error of the end position that is less than 2° from Fig. 19, which means that the position eventually reaches our expectation. The speed of the attitude and position of the end-effector is shown in Fig. 20. Applying the PD control to each joint according to the linear motion in the Cartesian space of the end-effector, we obtain the control force of each joint as shown in Fig. 21.

Conclusion
This paper conducts a research on fault tolerance trajectory planning for a 6-DOF space manipulator under a single joint failure, and the workspace of the manipulator for pre-or post-failure is analyzed based on the actual situation of joint failure. The analytical inverse kinematics equation of the 5-DOF formed from the 6-DOF manipulator with locking single joint is derived to ensure the completion of part tasks in the workspace. In order to further represent the ability of the manipulator under a single joint failure, the concept of fault-tolerant angle is introduced. Then the relationship between the fault tolerance angle and the locked joint is established. Based on it, the best choice of desired angle of each joint which is most suitable for specific task can be determined. This paper provided an effective way to maximize the ability of a space manipulator under a single joint failure.
Authors' contributions ZM and LH have jointly carried out the research on section II, section III, and section IV, and ZM revised and completed all the figures. Finally, WX and ZM have jointly written section I and section V. All authors read and approved the final manuscript.