The optimization of global fault tolerant trajectory for redundant manipulator based on self-motion

. The redundancy feature of manipulators provides the possibility for the fault tolerant trajectory planning. Aiming at the completion of the specific task, an algorithm of global fault tolerant trajectory optimization for redundant manipulator based on the self-motion is proposed in this paper. Firstly, inverse kinematics equation of single redundancy manipulator based on self-motion variable and null-space velocity array of Jacobian are analyzed. Secondly, the mathematical description of fault tolerance criteria of the configuration of manipulator is established and the fault tolerance configuration group of manipulator is obtained by using iteration traversal under the fault tolerance criteria. Then, considering the joint limits and minimum the energy consumption as the optimization target, the global fault tolerant joint trajectory is achieved. Finally, simulation for 7 degree of freedom (DOF) manipulator is performed, by which the effectiveness of the algorithm is validated.


Introduction
Redundancy manipulators are usually applied for tasks in hazardous environments, such as the nuclear disposal, outer-space explorations and deep-sea missions [1].During its long-time service, many kinds of malfunctions are inevitable, such as joint delay, performance degradation and joint complete failure.For the special trajectory tracking task, joint faults will influence the task completion seriously without effective fault-tolerant control measures [2].The optimization of global fault tolerant trajectory for redundant manipulator is very meaningful.
The redundancy feature of manipulators makes its joint configuration move free in a certain rules without changing the posture of end-effector and this process is called self-motion, which provides the possibility for the global trajectory optimization [3] This idea was first proposed in [4], where the null-space component of a redundant manipulator was used to locally minimize the kinematic fault tolerance measure.However, due to the local nature of the fault tolerance measure, fault tolerance could not be guaranteed globally.In [5], the self-motion manifolds of redundant manipulator was introduced to represent the global features of the kinematic and in [6] the self-motion topology with joint limits was further refined, but both of them do not take the joint failure into consider.In [7], the global fault tolerant trajectory planning problem was discussed and a redundancy resolution algorithm for single joint failure using the selfmotion topology was proposed.Nevertheless, it only focus on the 3 degree of freedom(DOF) planar manipulator and the 4-DOF spatial manipulator, for more degree of freedom such as 7-DOF manipulator, the inverse kinematic and the analysis of self-motion manifolds is very difficult and the computation of the global fault tolerant trajectory planning will be huge.
An optimal method of joint angle sequence for redundant manipulator aiming at the point to point path planning task was proposed based on the particle-swarm optimization algorithm and real code genetic algorithm in [8].In [9], a new method of deciding the self-motion variable of redundant manipulator was proposed by analyzing the null-space speed array of Jacobian and selfmotion variable in the view of inverse kinematic in velocity level.The above contents provide a new idea for the global fault tolerant trajectory optimization of redundant manipulator.The paper is organized as follow: In section 2, The algorithm of global fault tolerant trajectory optimization is discussed.In section 3, the optimization goals and the implementation process of the fault tolerant trajectory optimization are analyzed in detail.In section 4, simulations on 7-DOF manipulator are performed to validate the effectiveness of the proposed trajectory optimization method.The last section is conclusion.

The algorithm of global fault tolerant trajectory optimization
In this section, the inverse kinematic function based on the self-motion variable, which makes achieving different configurations possible, is analyzed.In order to get the group of global fault tolerant trajectory, two fault tolerance criteria of the configuration derive from the Cartesian space and joint space is proposed.

Definitions of global fault tolerant trajectory optimization
The trajectory of joint space, (t) n q T , is fault tolerant to single joint failure with respect to the task of following the Cartesian path (t) m p R , only if for every joint and every instant t , there is an alternative trajectory (t, j, t ) e q , satisfy: 1.Both ( Where, j is the number of the error joint and t e in the error time, (t)  q is the fault tolerant trajectory maps onto (t) p under the forward kinematics and (t, j, t ) e q is the alternate trajectory after the joint error at t e .(t ) e q and (t , j, t ) e e q are the joint sequence at error time t e for the fault tolerant trajectory and the alternate trajectory separately.
(t ) j e q and (t, j, t ) j e q represent the joint sequence of joint j.

The inverse kinematic function based on the self-motion variable
The forward kinematics function of manipulator is expressed as equation ( 1): ( ) (1) x R is the pose for the end of the manipulator in Cartesian space, and n q R is the joint angle sequence of the manipulator, m and n represent operation dimension and joint space dimension of the manipulator, respectively.For redundant manipulator, each redundant DOF corresponds to a unique self-motion.Therefore, the forward kinematics function based on selfmotion variable of the manipulator with r redundant can be implicitly summarized as: θ R is the self-motion variable of the redundant manipulator.In generally, explicit inverse solution does not exist in equation (2).Therefore, the fault-tolerant trajectory is difficult to be achieved by using the self-motion topological manifold.But for the 1redundancy manipulator, the inverse kinematic in velocity level based on the self-motion variable can be described as equation ( 3) by introducing the null-space speed array of Jacobian.† e s Wherein, is velocity of the end-effector, J is the Jacobian matrix of manipulator, † J is the Pseudo inverse of J , s T is self-motion variable, which is correspond to joint velocity in dimension, the self-motion of the redundant manipulator is reflected through changing the value of s T , i Z (J) is null-space velocity array of Jacobian matrix, which could be calculated by formula (4): Wherein, i J is the sub-matrix by removing elements of column i from J .Readers unfamiliar with the equation are referred to [8] for a detailed overview.Then, according to equation ( 3) and ( 4), the group of joint space configuration which satisfied the request of end track and end velocity can be obtained by choosing different selfmotion variables.

Fault tolerance criteria of the configuration for redundant manipulator
It is obviously that the fault tolerance criteria is necessary to determine whether the configurations of manipulator corresponding to different specified self-motion variables have the global fault tolerance features.Through the above analysis, two fault tolerance criteria of the configuration derive from the Cartesian space and joint space is proposed to get the group of global fault tolerant trajectory.
If the trajectory of joint space, (t) n q T , is global fault tolerant for all single joint failure, it should obey the follow criteria: 1.The Cartesian path, (t) m p R , of end-effector should located in the fault tolerant workspace of the redundant manipulator, as described in equation ( 5): Wherein, W i is the reduced workspace of the manipulator for the immobilized joint i and W f is the fault tolerant workspace for each joint.It has been discussed in detail in [10].
2. When the configuration of redundancy manipulator is (t)   n q R , the Jacobi matrix is full rank.Under the current manipulator configuration, for the ith joint of manipulator, the necessary condition of no fault tolerance is the ith element of null-space velocity array Z(J) is zero.
Necessary proof if the current configuration q has no fault tolerant capability, then i J must be singular, therefore, it must exist non-zero null-space Vector, assume that the unit null-space velocity array 1 R V is: It is obvious that: If a zero element is added to the i th element of velocity array, a length of n array is get, that is: [ , ,..., ,0, ,..., ] It must be the null-space velocity array of J , therefore, it must be that: , V is the unique unit null-space velocity array of J , then the ith element of J matrix null-space velocity array is must be zero.
dimensions vector of J matrix null-space velocity array without the ith element, therefore, it has that: dimensions array, therefore, V is the null-space array of i J , ( ) , it shows that when the ith joint is locked, the manipulator is singular, that is, the ith joint is not fault tolerance.
The above criteria is a standard for selecting fault tolerant configurations group in a group of joint configurations satisfying the end trajectory sequence, that is the standard of selecting the corresponding self-motion variables, the process of optimizing the performance index and trajectory optimization is described in detail below.

The process of fault tolerant trajectory optimization
Through the analysis of section 2, the selection of selfmotion variables cannot be too large, otherwise it will cause the jump of the manipulator joint velocity and a great impact torque, and make the system unstable.Therefore, it need to set the upper and lower limits of its value.When the global fault tolerance trajectory that select from the range of self-motion variables is not unique, in order to get the best performance global fault tolerant joint trajectory, set the secondary optimization objective is necessary.In this paper, the joint angle limit and the energy consumption, which are described as equation (11), are used as the optimization goals.min max H q q q H min q (11) Wherein, max q and min q is the upper and lower limits of joint angle, n is total sampling point through the operating period of manipulator.Then, the implementation of global fault tolerant trajectory optimization based on self-motion variable is as follows: 1.For a given manipulator end-effector trajectory and task parameters, judge that if it satisfies the first fault tolerant criterion or not.If it is not satisfy, then it has no fault tolerant to the joint failure, otherwise, the terminal velocity of each sampling time is obtained by using the path planning method.
2. Select the upper and lower limit of self-motion variable traversal T smax , and T smin , and set the length of the iteration step for self-motion variables; 3. For a given self-motion variable, by using formula (3) and ( 4) to obtain the manipulator joint velocity in each sampling time, and by using linear theory, the joint angle of every moment is obtained.4. By using the secondary fault tolerance criteria, judge that if the manipulator joint configuration in each sampling time has global fault tolerance capability or not, if not, eliminate the corresponding self-motion variables, otherwise, reserve the corresponding self-motion variable.5. Complete the iterative process of self-motion variables, and get all the self-motion variables with global fault tolerance, that is, using these variables in the formula (3) to carry out the inverse velocity solution, the manipulator joint space trajectory with the global fault tolerance capability can be obtained.
6. Use the optimization objectives in formula (11), choose the best one in joint motion trajectory with global fault tolerance capability.

Simulation
This paper takes 7-DOF manipulator as the simulation model.The coordinate system of the modular robot is shown in Figure 1, while the DH parameters are listed in Table 1.Develop the end-effector trajectory tracking task in Cartesian space, the initial configuration is T [ 50 , 70 50 -60 30 70 0 ] ini q q q q q q q q , the desired pose is T [9.6m,0m,3m, 1rad, 0.5rad, 2rad] des x ,which has been proved to satisfy the first fault tolerance criteria by using the straight path planning method of the endeffector in previous .The task cycle is 20s and the control cycle is 50ms.The feasible region of s θ is calculated as , and set the iteration step length of s θ is 0.01, the joint limits for every joint is (t) [ 150 ,150 ]   q q q .The trajectory of the end-effector is shown Figure 2. , the global fault tolerant joint trajectory is achieved by using the algorithm and the optimization goal proposed in the paper.In order to check the fault tolerant to joint failure when any joint at any time failed.Set the 4th joint locked at 5s and 2th joint locked at 10s separately.Before the failure, the joint angle sequence are caculated from the equation ( 3) and ( 4) with 0.25 s θ , and after the joint failure , the column element corresponding to the error joint error of J is canceled [11].The simulations result is show in Figure 3 and Figure 4.As we can seen in Figure 3 and Figure 4 the solid line is the global fault tolerant joint trajectory.Before the joint failure, the manipulator follows the fault tolerant trajectory.When the joint fails and is immobilized, the manipulator then deviates from the fault tolerant trajectory and follows the alternate trajectory(dotted line), which is caculete by cancleing the column element corresponding to the error joint error of J .Many simulations with a wide varity of failure times and failing joints resulted in a maxmium positional error of

Conclusion
In this paper, an algorithm of global fault tolerant trajectory optimization for redundant manipulator based on the self-motion has been presented.The joint trajectory mapping onto the special end-effector trajectory is achieved through the algorithm, it guarantees fault tolerance on the global scale(for every joint fail at any time of the task period) and satisfies the secondary optimization goals such as the joint limit and the minimum energy consumption.The algorithm consists of three main parts.In the first part, the inverse kinematics equation of single redundancy manipulator based on selfmotion variable and null-space velocity array of Jacobian 02015-p.4 are analyzed and the group of joint space configurations which satisfies the request of end-effector trajectory tracking can be obtained by choosing different selfmotion variables.In the second part the mathematical description of fault tolerance criteria of the configuration for redundant manipulator is established and considering the joint limits and minimum the energy consumption the secondary optimization target is established in the third part.Finally, simulation for 7 degree of freedom (DOF) manipulator is performed to verify the effectiveness of the algorithm.

DOI: 10
.1051/ C Owned by the authors, published by EDP Sciences,

Figure 2 .
Figure 2. The trajectory of the end-effector When

Figure 3 .Figure 4 .
Figure 3.The global fault tolerant joint trajectory from q1-q7 compared with the one when the 4th joint failure at 5s analysis above, the effectiveness of global fault tolerant trajectory optimization for redundant manipulator is validated.

Table 1 .
DH parameters of modular robot