# Trajectory Optimization for Velocity Jumps Reduction considering the Unexpectedness Characteristics of Space Manipulator Joint-Locked Failure

Authors: Qingxuan Jia; Tong Li; Gang Chen; Hanxu Sun; Jian Zhang
Journal: International Journal of Aerospace Engineering
 (2016)

---

## Abstract

Aiming at reducing joint velocity jumps caused by an unexpected joint-locked failure during space manipulator on-orbit operations without shutting down manipulator, trajectory optimization strategy considering the unexpectedness characteristics of joint-locked failure is proposed in the paper, which can achieve velocity jumps reduction in both operation space and joint space simultaneously. In the strategy, velocity in operation space concerning task completion directly is treated as equality constraints, and velocity in joint space concerning motion performance is treated as objective function. Global compensation vector which consists of coefficient, gradient of manipulability, and orthogonal matrix of null space is constructed to minimize the objective function. For each particular failure time, unique optimal coefficient can be obtained when the objective function is minimal. As a basis, a method for optimal coefficient function fitting is proposed based on a priori failure information (possible failure time and the corresponding optimal coefficient) to guarantee the unexpectedness characteristics of joint-locked failure. Simulations are implemented to validate the efficiency of trajectory optimization strategy in reducing velocity jumps in both joint space and operation space. And the feasibility of coefficient function is also verified in reducing velocity jump no matter when joint-locked failure occurs.


---

## Body

## 1. Introduction

Space manipulator [1, 2] is a kind of complex electromechanical system, which plays an important role in space station maintenance, rendezvous and docking, and space exploration. During its long-time service on orbit, affected by radiation, energetic particle, and high-low temperature alternating of space environment, many kinds of malfunctions are inevitable, such as link deformation, performance degradation, and joint failure [3, 4]. Joint as a precision unit, integrating with machinery, communications, and automatic control technology, is the core component of space manipulator to achieve safe and smooth operation. Joint failure may bring about performance degradation of space manipulator and lead to bad effect on task completion.For general manipulator on the ground, when joint fails during manipulator motion, the manipulator can be shut down to maintain or replace the failure joint. However, if shutting down space manipulator once joint failure occurs, the inertia force acting on the manipulator will cause serious impact force on the end-effector and joints, as well as cause instability of the base. When joint fails especially during load operations such as space cabin docking [5] and astronauts assistant [6], the bigger mass and inertia will cause huger impact, which may exceed the rated torque and cause serious hurt on the manipulator parts and operating objects. On the other hand, when trying to execute task continuously after joint failure, control strategy should be adjusted in real time. As a result, velocity jumps will be caused in joint space and operation space, which will also generate impact on manipulator structure. Thus, it is of great significance to smooth motion parameters when joint failure occurs during on-orbit operations no matter whether space manipulator will be shut down or execute task continuously.Joint failure can be divided into joint-locked [7] and free swing [8] according to whether joint can be operated or not after failure occurs. In free swing state, failure joint moves freely, making the manipulator out of control. In joint-locked state, failure joint is locked at the angle of failure time. Although the degree of freedom (DOF) of space manipulator is reduced, the healthy joints can move normally, and the task can be executed continuously when joint-locked failure occurs. Thus in order to avoid the impact force caused by shutting down space manipulator after joint-locked failure, the paper is devoted into trajectory optimization research to achieve task completion and motion parameters smoothness (velocity jumps reduction especially) without shutting down manipulator.In order to achieve task completion after manipulator failure, scholars concentrate on the following researches. First, in order to obtain the failure information such as failure time and failure joint, fault detection, fault isolation, and fault identification [9–12] are researched. Second, abilities of manipulator are analyzed in failure condition such as fault-tolerant workspace [13, 14] and dexterity [15, 16]. Based on these researches above, pseudoinverse method [17], self-motion manifold method [18], quadratic programming algorithm [19], and configuration optimization [20, 21] are applied to achieve task completion after failure occurs. Meanwhile, constraints are analyzed during fault tolerance to guarantee the practical application. Ralph and Pai [22] analyzed the minimal constraints necessary in fault tolerance, Jamisola Jr. et al. [23] took environment obstacle constraints into account, and Xie et al. [24] introduced the singularity constraints into the fault-tolerant method.In the mentioned fault-tolerant researches, some of them shut down manipulator after failures; then fault-tolerant method is introduced to achieve task completion. However, it is not suitable for space manipulator fault-tolerance. The others execute task via fault-tolerant method in real time. Unfortunately, when guaranteeing velocity and position in operation space for task completion, velocity jumps are brought about into joint space. Scholars try to reduce the velocity jumps; Abdi et al. [25] concentrated on the minimum velocity jump in operation space and proposed an optimization strategy for joint velocity redistribution. Meanwhile, the velocity jump in joint space is reduced with the application of least square method and matrix perturbation method [26]. Jing and Cheng [27] achieved velocity jump reduction in joint space by optimizing the initial configuration of a dual-arm manipulator. In the authors’ former work [28], velocity jumps in joint space were reduced by constructing compensation vectors based on the gradient of manipulability. Further, the coefficient form of compensation vector was discussed to achieve better reduction performance.However, simultaneous reduction for velocity jumps in operation space and joint space is not achieved. What is more, velocity jumps reduction in the operation space may even increase the velocity jumps in joint space. Since velocity jumps in operation space are concerned with task completion and velocity jumps in joint space are concerned with joint structure safety, it is of equal importance to achieve velocity jumps reduction in the two spaces to guarantee task completion and motion safety. According to the self-motion characteristics of space manipulator, a set of velocities in operation space corresponds to multiple sets of joint velocities. So when the velocity in operation space is fixed, joint velocity can be adjusted to find an optimal value minimizing the jumps. On the basis, the trajectory optimization strategy can be formed by treating velocity in operation space as constraints and treating velocity in joint space as optimization objective.Besides, the mentioned fault-tolerant methods depend on failure information; only after the failure condition is confirmed can they begin to work. However, in practical application, joint-locked failure occurs unexpectedly. It needs time to obtain failure information with fault detection method, so time delay is inventible between failure discovery and fault-tolerant method application, which makes fault-tolerant performance worse than the ideal condition. Although controllers are designed based on neural networks to achieve real-time fault tolerance [29–31], the adaptation of controllers for rapid response and stable operation is not enough for on-orbit application. Therefore, in order to avoid time delay problem of fault detection method and to guarantee the unexpectedness characteristics of joint-locked failure, global compensation for joint velocity during the entire path planning is proposed in the paper, based on which the trajectory optimization is not relevant to the failure information and can handle an unexpected joint-locked failure.In conclusion, the trajectory optimization strategy proposed in the paper aims at dealing with three problems. First, the task completion on-orbit is guaranteed without shutting down space manipulator, and the parameters of space manipulator particularly the velocity jumps should be smoothed after joint-locked failure occurs. Second, the reduction for velocity jumps in both operation space and joint space should be achieved simultaneously. It can be achieved by considering velocity in operation space as equality constraints and considering velocity in joint space as optimization objective in the strategy. Third, the avoidance of time delay problem and the unexpectedness characteristics of joint-locked failure should be handled. For this purpose, a global compensation vector is constructed for velocity compensation over the entire path planning task, which makes the optimization strategy irrelevant with the real-time failure information acquisition. In addition, with global compensation for joint velocity, a unique coefficient which makes objective function minimal can be obtained related to a particular failure time. Taking it as a priori failure information, the law of optimal coefficient related to arbitrary failure time is concluded. A coefficient function is obtained to deal with velocity jump reduction caused by unexpected joint-locked failure.The presented paper is organized as follows: in Section2, velocity jumps in operation space and joint space caused by joint-locked failure are analyzed. The mathematical expression of trajectory optimization strategy is derived, during which velocity jumps in operation space are expressed as equality constraint and velocity jumps in joint space are expressed as objective function. In Section 3, global compensation vector for joint velocity is constructed, and the solving method is proposed to obtain the optimal coefficient of global compensation when joint-locked failure occurs at a particular time. Then considering the unexpectedness characteristics of joint-locked failure, coefficient of global compensation vector is fitted in function form based on a priori failure information. In Section 4, simulations are implemented to validate the effectiveness of trajectory optimization strategy in reducing the velocity jump in both joint space and operation space when joint-locked failure occurs at arbitrary time. The last part is conclusion.


## 2. Mathematical Expression of Trajectory Optimization Strategy

Joints are the key parts of space manipulator operations. When joint-locked failure occurs, it will bring about velocity jumps in operation space and joint space, which would tremendously reduce the operability of space manipulator, especially the ability of pose-reaching and task-completing. In this section, trajectory optimization is proposed considering simultaneous velocity jumps reduction in two spaces. Based on the analysis of velocity jumps, the constraints and objective of trajectory optimization are derived.Trajectory optimization is discussed on a typical space manipulator path planning task, which starts at timet0 and ends at time te. The control interval is represented as Δt, and the execution time of path planning t can be represented as t=t0+sΔt, 1≤s≤te-t0/Δt, s∈N+.


### 2.1. Analysis on Velocity Jumps in Operation Space and Joint Space

For an degree space manipulator, when joints are healthy during path planning task, the Jacobian matrix and joint velocity vector at time t(t0≤t≤te) can be expressed as(1)Jqt=j1qt,…,jk-1qt,jkqt,jk+1qt,…,jnqt∈Rr×n,q˙t=q˙1t,…,q˙k-1t,q˙kt,q˙k+1t,…,q˙ntT∈Rn×1,t0≤t≤te.The Jacobian matrixJ is the function of joint angle vector q(t) at current time t. And r represents the dimension of operation space. ji(i∈[1,n]) represents the ith column of Jacobian matrix.When joint-locked failure occurs in jointk at time tf, the dimension of joint space degenerates to n-1. Correspondingly, the Jacobian matrix degenerates to Jk, which is named as reduced Jacobian matrix [32]. The reduced Jacobian matrix and joint velocity vector at time t(tf<t≤te) can be expressed as(2)Jkqkt=j1kqkt,…,jk-1kqkt,jk+1kqkt,…,jnkqkt∈Rr×n-1,q˙kt=q˙1kt,…,q˙k-1kt,q˙k+1kt,…,q˙nktT∈Rn-1×1,tf<t≤te.Correspondingly,jik(i∈[1,n]∩i≠k) represents the ith column of reduced Jacobian matrix. And qk(t) represents the reduced joint angle vector. After joint k fails at time tf, the velocity of failure joints turns to 0, and velocity of healthy joints keeps constant. Thus, at time t(tf<t≤te), it has(3)q˙it=q˙ikt,jiqt≠jikqkt,tf<t≤te,i∈1,n∩i≠k.Then the velocity jump vector in joint space at timet(tf<t≤te) can be expressed as(4)Δq˙t=0⋯q˙kt⋯0T.q
˙
k
(
t
) represents the nominal velocity of kth joint when the joint is healthy. On the basis, the velocity jump in operation space Δx˙(t) caused by joint-locked failure is expressed as(5)Δx˙t=Jq˙t-Jkq˙kt.According to (5), it will lead to accumulative pose (position and orientation) deviation ΔP in operation space:(6)ΔPt=∫tftΔx˙tdt=∫tftJq˙t-Jkq˙ktdt.When joint failure occurs unexpectedly during path planning task, an average velocity deviation in the operation space can be obtained:(7)Δx˙-t=ΔPtte-tf=1te-tf∫tfteJq˙t-Jkq˙ktdt.Equation (7) can be used to evaluate the influence of velocity deviation caused by an unexpected joint-locked failure, while, for a general path planning task, the pose accuracy should fulfill accuracy threshold at each time to guarantee task completion. Set the threshold value as l; when ΔP(t)>l, the task cannot be completed. Therefore, the velocity jump in operation space should be eliminated preferentially to guarantee task completion.Generally, velocity jump in operation space can be reduced by compensation for healthy joint. When compensation vectorΔq˙k(t)(t∈(tf,te]) is introduced, (5) is turned into(8)Δx˙t=Jq˙t-Jkq˙kt+Δq˙kt.When the velocity deviation in operation space is completely eliminated, namely,Δx˙(t)=0, the compensation vector for joint velocity can be expressed as(9)Δq˙kt=Jk†Jq˙t-q˙kt.J
k
† represents the pseudoinverse of the reduced Jacobian matrix. By given the compensation Δq˙k(t) as (9), the velocity jump in operation space can be completely eliminated. The contribution of failure joint is redistributed to healthy joints. However, additional velocity jumps are introduced into healthy joints. The velocity compensation Δq˙k(t) may be too large for safety threshold of joint velocity and the impact caused by compensation may damage the joint structure, leading to terribly concatenate failure of joints. Thus, it is of equal importance to decrease the velocity jumps in operation space and joint space.


### 2.2. Constraints and Objective of Trajectory Optimization

In order to achieve simultaneous velocity jumps reduction in both operation space and joint space, the velocity in operation space is considered as equality constraint, which is preferentially guaranteed for task completion. And joint velocity is used to construct the objective function, which is adjusted to achieve joint velocity jumps minimal. The constraints and objective function of trajectory optimization can be expressed as follows.


#### 2.2.1. Constraints

Assume only position accuracy in operation space is considered during path planning task. The actual value of linear velocity in operation space should be in accordance with the nominal value; namely,(10)h1t=veat-vedt.v
e
a
,
v
e
d
∈
R
3
×
1 represent the actual and nominal value of the linear velocity in the operation space. Correspondingly, the position in operation space should be tracked as the nominal value:(11)h2t=Peat-Pedt=∫t0tveatdt-∫t0tvedtdt.P
e
a
,
P
e
d
∈
R
3
×
1 represent the actual and nominal position in operation space, respectively. Equations (10) and (11) are named as task constraints. When task constraints are guaranteed, it has hi(t)=03×1 (i=1,2; 03×1∈R3×1).Besides, the parameters of space manipulator should fulfill the thresholds in path planning. Constraints of joint velocity and joint angular acceleration are taken into account:(12)g1,jt=q˙jt-q˙jmax,g2,jt=q¨jt-q¨jmax=q˙jt+Δt-q˙jtΔt-q¨jmax,wherein j=1,…,n represents the number of joints and q˙jmax and q¨jmax represent the threshold value of joint velocity and angular acceleration of the jth joint, respectively. When (12) is fulfilled, it has gi,j(t)≤0i=1,2.


#### 2.2.2. Objective Function

The objective function is constructed as the joint velocity deviation at failure timetf. Since the dimension of joint velocity is reduced after joint failure, in order to guarantee the dimension, q˙~k is defined based on q˙k:(13)q˙~k=q˙1k,…,q˙k-1k,0,q˙k+1k,…,q˙nkT∈Rn×1.Then the objective function can be expressed as(14)f=Δq˙~ktf-Δq˙tf-Δt.Based on the analysis above, the mathematical expression of trajectory optimization strategy is derived as(15)minfs.t.gi,jt≤0i=1,2;j=1,…,7hkt=03×1k=1,2;03×1∈R3×1.


## 2.1. Analysis on Velocity Jumps in Operation Space and Joint Space

For an degree space manipulator, when joints are healthy during path planning task, the Jacobian matrix and joint velocity vector at time t(t0≤t≤te) can be expressed as(1)Jqt=j1qt,…,jk-1qt,jkqt,jk+1qt,…,jnqt∈Rr×n,q˙t=q˙1t,…,q˙k-1t,q˙kt,q˙k+1t,…,q˙ntT∈Rn×1,t0≤t≤te.The Jacobian matrixJ is the function of joint angle vector q(t) at current time t. And r represents the dimension of operation space. ji(i∈[1,n]) represents the ith column of Jacobian matrix.When joint-locked failure occurs in jointk at time tf, the dimension of joint space degenerates to n-1. Correspondingly, the Jacobian matrix degenerates to Jk, which is named as reduced Jacobian matrix [32]. The reduced Jacobian matrix and joint velocity vector at time t(tf<t≤te) can be expressed as(2)Jkqkt=j1kqkt,…,jk-1kqkt,jk+1kqkt,…,jnkqkt∈Rr×n-1,q˙kt=q˙1kt,…,q˙k-1kt,q˙k+1kt,…,q˙nktT∈Rn-1×1,tf<t≤te.Correspondingly,jik(i∈[1,n]∩i≠k) represents the ith column of reduced Jacobian matrix. And qk(t) represents the reduced joint angle vector. After joint k fails at time tf, the velocity of failure joints turns to 0, and velocity of healthy joints keeps constant. Thus, at time t(tf<t≤te), it has(3)q˙it=q˙ikt,jiqt≠jikqkt,tf<t≤te,i∈1,n∩i≠k.Then the velocity jump vector in joint space at timet(tf<t≤te) can be expressed as(4)Δq˙t=0⋯q˙kt⋯0T.q
˙
k
(
t
) represents the nominal velocity of kth joint when the joint is healthy. On the basis, the velocity jump in operation space Δx˙(t) caused by joint-locked failure is expressed as(5)Δx˙t=Jq˙t-Jkq˙kt.According to (5), it will lead to accumulative pose (position and orientation) deviation ΔP in operation space:(6)ΔPt=∫tftΔx˙tdt=∫tftJq˙t-Jkq˙ktdt.When joint failure occurs unexpectedly during path planning task, an average velocity deviation in the operation space can be obtained:(7)Δx˙-t=ΔPtte-tf=1te-tf∫tfteJq˙t-Jkq˙ktdt.Equation (7) can be used to evaluate the influence of velocity deviation caused by an unexpected joint-locked failure, while, for a general path planning task, the pose accuracy should fulfill accuracy threshold at each time to guarantee task completion. Set the threshold value as l; when ΔP(t)>l, the task cannot be completed. Therefore, the velocity jump in operation space should be eliminated preferentially to guarantee task completion.Generally, velocity jump in operation space can be reduced by compensation for healthy joint. When compensation vectorΔq˙k(t)(t∈(tf,te]) is introduced, (5) is turned into(8)Δx˙t=Jq˙t-Jkq˙kt+Δq˙kt.When the velocity deviation in operation space is completely eliminated, namely,Δx˙(t)=0, the compensation vector for joint velocity can be expressed as(9)Δq˙kt=Jk†Jq˙t-q˙kt.J
k
† represents the pseudoinverse of the reduced Jacobian matrix. By given the compensation Δq˙k(t) as (9), the velocity jump in operation space can be completely eliminated. The contribution of failure joint is redistributed to healthy joints. However, additional velocity jumps are introduced into healthy joints. The velocity compensation Δq˙k(t) may be too large for safety threshold of joint velocity and the impact caused by compensation may damage the joint structure, leading to terribly concatenate failure of joints. Thus, it is of equal importance to decrease the velocity jumps in operation space and joint space.


## 2.2. Constraints and Objective of Trajectory Optimization

In order to achieve simultaneous velocity jumps reduction in both operation space and joint space, the velocity in operation space is considered as equality constraint, which is preferentially guaranteed for task completion. And joint velocity is used to construct the objective function, which is adjusted to achieve joint velocity jumps minimal. The constraints and objective function of trajectory optimization can be expressed as follows.


### 2.2.1. Constraints

Assume only position accuracy in operation space is considered during path planning task. The actual value of linear velocity in operation space should be in accordance with the nominal value; namely,(10)h1t=veat-vedt.v
e
a
,
v
e
d
∈
R
3
×
1 represent the actual and nominal value of the linear velocity in the operation space. Correspondingly, the position in operation space should be tracked as the nominal value:(11)h2t=Peat-Pedt=∫t0tveatdt-∫t0tvedtdt.P
e
a
,
P
e
d
∈
R
3
×
1 represent the actual and nominal position in operation space, respectively. Equations (10) and (11) are named as task constraints. When task constraints are guaranteed, it has hi(t)=03×1 (i=1,2; 03×1∈R3×1).Besides, the parameters of space manipulator should fulfill the thresholds in path planning. Constraints of joint velocity and joint angular acceleration are taken into account:(12)g1,jt=q˙jt-q˙jmax,g2,jt=q¨jt-q¨jmax=q˙jt+Δt-q˙jtΔt-q¨jmax,wherein j=1,…,n represents the number of joints and q˙jmax and q¨jmax represent the threshold value of joint velocity and angular acceleration of the jth joint, respectively. When (12) is fulfilled, it has gi,j(t)≤0i=1,2.


### 2.2.2. Objective Function

The objective function is constructed as the joint velocity deviation at failure timetf. Since the dimension of joint velocity is reduced after joint failure, in order to guarantee the dimension, q˙~k is defined based on q˙k:(13)q˙~k=q˙1k,…,q˙k-1k,0,q˙k+1k,…,q˙nkT∈Rn×1.Then the objective function can be expressed as(14)f=Δq˙~ktf-Δq˙tf-Δt.Based on the analysis above, the mathematical expression of trajectory optimization strategy is derived as(15)minfs.t.gi,jt≤0i=1,2;j=1,…,7hkt=03×1k=1,2;03×1∈R3×1.


## 2.2.1. Constraints

Assume only position accuracy in operation space is considered during path planning task. The actual value of linear velocity in operation space should be in accordance with the nominal value; namely,(10)h1t=veat-vedt.v
e
a
,
v
e
d
∈
R
3
×
1 represent the actual and nominal value of the linear velocity in the operation space. Correspondingly, the position in operation space should be tracked as the nominal value:(11)h2t=Peat-Pedt=∫t0tveatdt-∫t0tvedtdt.P
e
a
,
P
e
d
∈
R
3
×
1 represent the actual and nominal position in operation space, respectively. Equations (10) and (11) are named as task constraints. When task constraints are guaranteed, it has hi(t)=03×1 (i=1,2; 03×1∈R3×1).Besides, the parameters of space manipulator should fulfill the thresholds in path planning. Constraints of joint velocity and joint angular acceleration are taken into account:(12)g1,jt=q˙jt-q˙jmax,g2,jt=q¨jt-q¨jmax=q˙jt+Δt-q˙jtΔt-q¨jmax,wherein j=1,…,n represents the number of joints and q˙jmax and q¨jmax represent the threshold value of joint velocity and angular acceleration of the jth joint, respectively. When (12) is fulfilled, it has gi,j(t)≤0i=1,2.


## 2.2.2. Objective Function

The objective function is constructed as the joint velocity deviation at failure timetf. Since the dimension of joint velocity is reduced after joint failure, in order to guarantee the dimension, q˙~k is defined based on q˙k:(13)q˙~k=q˙1k,…,q˙k-1k,0,q˙k+1k,…,q˙nkT∈Rn×1.Then the objective function can be expressed as(14)f=Δq˙~ktf-Δq˙tf-Δt.Based on the analysis above, the mathematical expression of trajectory optimization strategy is derived as(15)minfs.t.gi,jt≤0i=1,2;j=1,…,7hkt=03×1k=1,2;03×1∈R3×1.


## 3. Solution for Trajectory Optimization considering Unexpectedness Characteristics of Joint-Locked Failure

In this section, trajectory optimization when joint fails at a particular time is solved firstly. A global compensation vector for joint velocity is constructed based on the orthogonal basis of Jacobian matrix null space. Then the optimal coefficient of compensation vector is obtained when the objective function is minimal. Since global compensation vector compensates joint velocity in each control interval ignoring the failure time, time delay problem can be efficiently avoided. Taking the failure time and corresponding optimal coefficient as a priori failure information, the law of optimal coefficient related to arbitrary failure time is concluded, and a method for coefficient function fitting is proposed based on a priori failure information to guarantee the unexpectedness characteristics of joint-locked failure.


### 3.1. Global Compensation Vector for Joint Velocity

In order to construct the global compensation vector, the orthogonal basis of Jacobian matrix null spaceI-J†J is used to represent the self-motion characteristics of space manipulator. Manipulability [33] w=det⁡JJT is introduced into the vector, whose gradient can be used to represent the velocity compensation weight of each joint. When joint-locked failure occurs, it is reduced as wk=det⁡JkJTk. Besides, a constant coefficient ck is used to adjust the velocity compensation rate for each joint. Then for each time, it has(16a)ut=ckI-J†J∇wqtt0≤t<tf,(16b)ukt=ckI-Jk†Jk∇wkqkttf≤t≤te.In (16a) and (16b), u(t)∈Rn×1 and uk(t)∈R(n-1)×1 are used to compensate joint velocity at each time in path planning. Before failure occurs, precompensation is implemented with the vector of (16a). After failure occurs, joint velocity is compensated according to (16b). Namely, joint velocity at each time t corresponds to a vector u(t) or uk(t). Then the global compensation vector for joint velocity over the entire task is constructed:(17)U=ut0,ut0+Δt,…,utf-Δt,uktf,…,ukte.With the compensation ofU, joint velocity can be expressed as q˙=J†x˙+U, which is detailed in(18)q˙t+Δt=J†x˙t+ckI-J†J∇wqtt0≤t<tf,q˙kt+Δt=Jk†x˙t+ckI-Jk†Jk∇wkqkttf≤t≤te.Joint angle at timet+Δt can be obtained as(19)qt+Δt=qt+q˙t+ΔtΔt,qkt+Δt=qkt+q˙kt+ΔtΔt.By substitutingq(t+Δt) and qk(t+Δt) into (16a) and (16b), u(t+Δt) and uk(t+Δt) can be obtained. The recurrence relation between u, q˙, and q is generated, which is shown in Figure 1.Figure 1
The recurrence relation betweenu, q˙, and q.Since the dimension of velocity is reduced after joint failure, similar to (13), u~k(t) is defined as the same dimension as u(t), whose kth element equals 0. Then, according to (18), the objective function can be turned into(20)fU=J†kx˙tf-Jx˙tf-Δt+u~ktf-utf-Δt.For a particular path planning task, when failure time is certain, the global compensation vectorU will change with coefficient ck. Thus the objective function can be optimized to achieve joint velocity jump minimal by finding the optimal ck.


### 3.2. Calculation for the Optimal Coefficientck

In order to find the optimalck, the gradient of reduced manipulability in global compensation vector should be calculated firstly. It has(21)∇wkqk=∂wk∂qk=∂wk∂q1,…,∂wk∂qi,…,∂wk∂qnTi=1,…,n,i≠k,wherein the partial derivative of the reduced manipulability to the ith joint angle can be expressed as(22)∂wk∂qi=∂detJkJTk∂qi=∂det⁡JkJTk/∂qi2·wk.MakeJkp=JkJTk, and Jkpi expresses the ith row of Jkp. Then(23)∂det⁡Jkp∂qi=∑j=1ndet⁡Jkp1,…,∂Jkpj∂qi,…,Jkpn,wherein ∂Jkpj/∂qi=∂Jkp/∂qij represents the jth row of ∂Jkp/∂qi∈Rr×r and r is the dimension of operation space. Then(24)∂Jkp∂qi=∂JkJTk∂qi=∂Jk∂qiJTk+Jk∂Jk∂qiT.In (24), ∂Jk/∂qi∈R(n-1)×r. Substitute (24) into (23); the partial derivative of reduced manipulability to joint angle qi can be obtained:(25)∂wk∂qi=∑j=1ndet⁡Jkp1,…,∂Jk/∂qiJTk+Jk∂Jk/∂qiTj,…,Jkpn2·wk.Substitute (25) into (21); the gradient of the reduced manipulability can be obtained. The gradient of manipulability can be obtained in a similar way. Then, by given ck, the value of global compensation vector U can be calculated.Before searching for the optimalck, its feasible region should be preferentially determined. According to constraints (10) and (11), the feasible region for ck can be defined as(26)ck∈ck∣PeaUkc-Pedx≤lx∩PeaUkc-Pedy≤ly∩PeaUkc-Pedz≤lz,wherein the subscripts x,y,z represent the x,y,z component of vector (Pea(U(ck))-Ped).Equation (23) means the x,y,z component of deviation between actual position and desired position should fulfill the threshold value of position accuracy l simultaneously, where l is expressed as l=[lx,ly,lz]T. In the feasible region, the optimal ck can be obtained as follows:(a)
According to the constraints, obtain the feasible region ofck∈ckmin,ckmax.(b)
Set the initial value ofck=ckmin and interval value Δck>0, and calculate the global compensation vector U(ck).(c)
Calculate the joint velocity and acceleration compensated withU(ck).(d)
Confirm whether joint velocity and acceleration guarantee the constraints (12). If the constraints are not guaranteed, the corresponding ck is abandoned. Else the value of objective function is obtained f(ck). ck,fck is stored in set K.(e)
ck=ck+Δck. If ck≤ckmax, turn to step (c). Else select the minimal value of fck in set K, and the corresponding ck is the optimal value.The steps are also detailed as shown in Figure2.Figure 2
Process of finding the optimalck when joint fails at a particular time.


### 3.3. The Method of Fitting Optimal Coefficient Function

With the analysis above, the optimalck can be obtained to minimize the objective function. However, during practical application, the accurate failure time is not easy to be obtained. In this section, the trajectory optimization is developed considering the unexpectedness characteristics of joint-locked failure when failure information is unknown. And a method is proposed to derive the law of optimal coefficient for unexpected joint failure.Since failure time is unexpected in practical application, the objective function is changed as the maximal value of joint velocity deviation between adjacent control cycles:(27)f=maxt⁡Δq˙t,t=t0+Δt,t0+2Δt,…,te-Δt,te,wherein Δq˙(t) represents the joint velocity deviation, which is expressed as(28)Δq˙t=q˙t-q˙t-Δt,t<tfq˙~kt-q˙t-Δt,t=tfq˙~kt-q˙~kt-Δt,t>tf.According to the analysis in Sections3.1 and 3.2, a unique ck can be obtained to minimize the objective function when joint-locked failure occurs at particular time tf. Thus, when the failure time is unknown, a function of operation time ck(t) can be constructed to guarantee the arbitrary failure time. Then (18) can be turned into(29)q˙t+Δt=J†x˙t+cktI-J†J∇wqtt0≤t<tf,q˙kt+Δt=Jk†x˙t+cktI-Jk†Jk∇wkqkttf≤t≤te.As a basis, the trajectory optimization strategy considering the unexpectedness characteristics of joint-locked failure can be expressed as(30)findfunctioncktminf=maxt⁡Δq˙tt=t0+Δt,t0+2Δt,…,t0+sΔt,…,tes.t.gi,jt≤0i=1,2;j=1,…,7hkt=03×1k=1,2;03×1∈R3×1.In order to solve the optimization strategy and obtain the coefficient functionck(t), a method of fitting the coefficient function based on polynomial fitting using a priori failure information is proposed. Define the polynomial function as follows:(31)ckt=∑i=0sai·ti.a
0
,
a
1
,
…
,
a
s
T
∈
R
(
s
+
1
)
×
1 represents the coefficient of polynomial, s represents the order of polynomial, and t represents the task execution time. Generally, joint velocity at the initial time t0 and the end time te should be zero during path planning; namely, q˙(t0)=0 and q˙(te)=0. According to q˙=J†x˙+U, function ck(t) should guarantee the boundary condition:(32)ckt0=0,ckte=0.In order to obtain the polynomial coefficienta0,a1,…,asT∈R(s+1)×1, a priori failure information is used to fit function (31). The a priori failure information includes failure time, failure joint angle, and the corresponding optimal coefficient ck. When failure time is certain, the failure joint angle can be theoretically calculated via manipulator path planning algorithm, and the optimal ck can be obtained based on the analysis in Section 3.2. In this way, a priori failure information can be obtained by supposing possible failure time in path planning. Then numbers of failure time tsam=(tf1,tf2,…,tfm) and the corresponding optimal coefficient cksam=(ck1,ck2,…,ckm) can be obtained as samples for function fitting.The samples consist of failure timetsam=(tf1,tf2,…,tfm) and the corresponding optimal coefficient cksam=(ck1,ck2,…,ckm). The ith sample can be represented as tfi,cki. tfi and cki are the ith element of tsam and cksam, which can also be represented as tsami and cksami, respectively. Thus, the ith sample can be represented as tsami,cksami.Define the evaluation criterion of function fittinge as the sum of square for deviations between samples and fitting function value, which has the expression of(33)e=∑i=1mcksami-cktsami2.The threshold of evaluation criterion is defined asec. When e≤ec, function ck(t) has a high fitting quality, and the fitting result is acceptable, while e>ec means more samples should be selected or polynomial order should be increased for better fitting. In conclusion, the process of fitting function ck(t) is shown in Figure 3.Figure 3
Calculation process of functionck(t).By considering the unexpectedness characteristics of joint-locked failure, a method is proposed based on a priori failure information for optimal coefficient function fitting. So when accurate joint failure information cannot be acquired in practical application, global compensation vector based onck(t) can be introduced to achieve trajectory optimization, with which velocity jumps can be efficiently reduced no matter when joint-locked failure occurs.


## 3.1. Global Compensation Vector for Joint Velocity

In order to construct the global compensation vector, the orthogonal basis of Jacobian matrix null spaceI-J†J is used to represent the self-motion characteristics of space manipulator. Manipulability [33] w=det⁡JJT is introduced into the vector, whose gradient can be used to represent the velocity compensation weight of each joint. When joint-locked failure occurs, it is reduced as wk=det⁡JkJTk. Besides, a constant coefficient ck is used to adjust the velocity compensation rate for each joint. Then for each time, it has(16a)ut=ckI-J†J∇wqtt0≤t<tf,(16b)ukt=ckI-Jk†Jk∇wkqkttf≤t≤te.In (16a) and (16b), u(t)∈Rn×1 and uk(t)∈R(n-1)×1 are used to compensate joint velocity at each time in path planning. Before failure occurs, precompensation is implemented with the vector of (16a). After failure occurs, joint velocity is compensated according to (16b). Namely, joint velocity at each time t corresponds to a vector u(t) or uk(t). Then the global compensation vector for joint velocity over the entire task is constructed:(17)U=ut0,ut0+Δt,…,utf-Δt,uktf,…,ukte.With the compensation ofU, joint velocity can be expressed as q˙=J†x˙+U, which is detailed in(18)q˙t+Δt=J†x˙t+ckI-J†J∇wqtt0≤t<tf,q˙kt+Δt=Jk†x˙t+ckI-Jk†Jk∇wkqkttf≤t≤te.Joint angle at timet+Δt can be obtained as(19)qt+Δt=qt+q˙t+ΔtΔt,qkt+Δt=qkt+q˙kt+ΔtΔt.By substitutingq(t+Δt) and qk(t+Δt) into (16a) and (16b), u(t+Δt) and uk(t+Δt) can be obtained. The recurrence relation between u, q˙, and q is generated, which is shown in Figure 1.Figure 1
The recurrence relation betweenu, q˙, and q.Since the dimension of velocity is reduced after joint failure, similar to (13), u~k(t) is defined as the same dimension as u(t), whose kth element equals 0. Then, according to (18), the objective function can be turned into(20)fU=J†kx˙tf-Jx˙tf-Δt+u~ktf-utf-Δt.For a particular path planning task, when failure time is certain, the global compensation vectorU will change with coefficient ck. Thus the objective function can be optimized to achieve joint velocity jump minimal by finding the optimal ck.


## 3.2. Calculation for the Optimal Coefficientck

In order to find the optimalck, the gradient of reduced manipulability in global compensation vector should be calculated firstly. It has(21)∇wkqk=∂wk∂qk=∂wk∂q1,…,∂wk∂qi,…,∂wk∂qnTi=1,…,n,i≠k,wherein the partial derivative of the reduced manipulability to the ith joint angle can be expressed as(22)∂wk∂qi=∂detJkJTk∂qi=∂det⁡JkJTk/∂qi2·wk.MakeJkp=JkJTk, and Jkpi expresses the ith row of Jkp. Then(23)∂det⁡Jkp∂qi=∑j=1ndet⁡Jkp1,…,∂Jkpj∂qi,…,Jkpn,wherein ∂Jkpj/∂qi=∂Jkp/∂qij represents the jth row of ∂Jkp/∂qi∈Rr×r and r is the dimension of operation space. Then(24)∂Jkp∂qi=∂JkJTk∂qi=∂Jk∂qiJTk+Jk∂Jk∂qiT.In (24), ∂Jk/∂qi∈R(n-1)×r. Substitute (24) into (23); the partial derivative of reduced manipulability to joint angle qi can be obtained:(25)∂wk∂qi=∑j=1ndet⁡Jkp1,…,∂Jk/∂qiJTk+Jk∂Jk/∂qiTj,…,Jkpn2·wk.Substitute (25) into (21); the gradient of the reduced manipulability can be obtained. The gradient of manipulability can be obtained in a similar way. Then, by given ck, the value of global compensation vector U can be calculated.Before searching for the optimalck, its feasible region should be preferentially determined. According to constraints (10) and (11), the feasible region for ck can be defined as(26)ck∈ck∣PeaUkc-Pedx≤lx∩PeaUkc-Pedy≤ly∩PeaUkc-Pedz≤lz,wherein the subscripts x,y,z represent the x,y,z component of vector (Pea(U(ck))-Ped).Equation (23) means the x,y,z component of deviation between actual position and desired position should fulfill the threshold value of position accuracy l simultaneously, where l is expressed as l=[lx,ly,lz]T. In the feasible region, the optimal ck can be obtained as follows:(a)
According to the constraints, obtain the feasible region ofck∈ckmin,ckmax.(b)
Set the initial value ofck=ckmin and interval value Δck>0, and calculate the global compensation vector U(ck).(c)
Calculate the joint velocity and acceleration compensated withU(ck).(d)
Confirm whether joint velocity and acceleration guarantee the constraints (12). If the constraints are not guaranteed, the corresponding ck is abandoned. Else the value of objective function is obtained f(ck). ck,fck is stored in set K.(e)
ck=ck+Δck. If ck≤ckmax, turn to step (c). Else select the minimal value of fck in set K, and the corresponding ck is the optimal value.The steps are also detailed as shown in Figure2.Figure 2
Process of finding the optimalck when joint fails at a particular time.


## 3.3. The Method of Fitting Optimal Coefficient Function

With the analysis above, the optimalck can be obtained to minimize the objective function. However, during practical application, the accurate failure time is not easy to be obtained. In this section, the trajectory optimization is developed considering the unexpectedness characteristics of joint-locked failure when failure information is unknown. And a method is proposed to derive the law of optimal coefficient for unexpected joint failure.Since failure time is unexpected in practical application, the objective function is changed as the maximal value of joint velocity deviation between adjacent control cycles:(27)f=maxt⁡Δq˙t,t=t0+Δt,t0+2Δt,…,te-Δt,te,wherein Δq˙(t) represents the joint velocity deviation, which is expressed as(28)Δq˙t=q˙t-q˙t-Δt,t<tfq˙~kt-q˙t-Δt,t=tfq˙~kt-q˙~kt-Δt,t>tf.According to the analysis in Sections3.1 and 3.2, a unique ck can be obtained to minimize the objective function when joint-locked failure occurs at particular time tf. Thus, when the failure time is unknown, a function of operation time ck(t) can be constructed to guarantee the arbitrary failure time. Then (18) can be turned into(29)q˙t+Δt=J†x˙t+cktI-J†J∇wqtt0≤t<tf,q˙kt+Δt=Jk†x˙t+cktI-Jk†Jk∇wkqkttf≤t≤te.As a basis, the trajectory optimization strategy considering the unexpectedness characteristics of joint-locked failure can be expressed as(30)findfunctioncktminf=maxt⁡Δq˙tt=t0+Δt,t0+2Δt,…,t0+sΔt,…,tes.t.gi,jt≤0i=1,2;j=1,…,7hkt=03×1k=1,2;03×1∈R3×1.In order to solve the optimization strategy and obtain the coefficient functionck(t), a method of fitting the coefficient function based on polynomial fitting using a priori failure information is proposed. Define the polynomial function as follows:(31)ckt=∑i=0sai·ti.a
0
,
a
1
,
…
,
a
s
T
∈
R
(
s
+
1
)
×
1 represents the coefficient of polynomial, s represents the order of polynomial, and t represents the task execution time. Generally, joint velocity at the initial time t0 and the end time te should be zero during path planning; namely, q˙(t0)=0 and q˙(te)=0. According to q˙=J†x˙+U, function ck(t) should guarantee the boundary condition:(32)ckt0=0,ckte=0.In order to obtain the polynomial coefficienta0,a1,…,asT∈R(s+1)×1, a priori failure information is used to fit function (31). The a priori failure information includes failure time, failure joint angle, and the corresponding optimal coefficient ck. When failure time is certain, the failure joint angle can be theoretically calculated via manipulator path planning algorithm, and the optimal ck can be obtained based on the analysis in Section 3.2. In this way, a priori failure information can be obtained by supposing possible failure time in path planning. Then numbers of failure time tsam=(tf1,tf2,…,tfm) and the corresponding optimal coefficient cksam=(ck1,ck2,…,ckm) can be obtained as samples for function fitting.The samples consist of failure timetsam=(tf1,tf2,…,tfm) and the corresponding optimal coefficient cksam=(ck1,ck2,…,ckm). The ith sample can be represented as tfi,cki. tfi and cki are the ith element of tsam and cksam, which can also be represented as tsami and cksami, respectively. Thus, the ith sample can be represented as tsami,cksami.Define the evaluation criterion of function fittinge as the sum of square for deviations between samples and fitting function value, which has the expression of(33)e=∑i=1mcksami-cktsami2.The threshold of evaluation criterion is defined asec. When e≤ec, function ck(t) has a high fitting quality, and the fitting result is acceptable, while e>ec means more samples should be selected or polynomial order should be increased for better fitting. In conclusion, the process of fitting function ck(t) is shown in Figure 3.Figure 3
Calculation process of functionck(t).By considering the unexpectedness characteristics of joint-locked failure, a method is proposed based on a priori failure information for optimal coefficient function fitting. So when accurate joint failure information cannot be acquired in practical application, global compensation vector based onck(t) can be introduced to achieve trajectory optimization, with which velocity jumps can be efficiently reduced no matter when joint-locked failure occurs.


## 4. Simulation Experiments

### 4.1. Research Object

An SSRMS [2] type space manipulator who has 7 DOF is used to carry out simulation experiments. The coordinate systems are shown in Figure 4, and the DH parameters are listed in Table 1. Path planning task mentioned in the paper concerns the position accuracy in the operation space of space manipulator only.Table 1
DH parameters of space manipulator.
i
θ
i (°)
d
i (m)
a
i
-
1 (m)
α
i
-
1 (°)
1
0
0.6
0
90
2
90
0.5
0
-
90
3
0
0.5
5
0
4
0
0.5
5
0
5
0
0.5
0
90
6
-
90
0.5
0
-
90
7
0
0.6
0
0Figure 4
Coordinate systems of 7 DOF space manipulator.


### 4.2. Velocity Jump Reduction When Joint-Locked Failure Occurs at a Particular Time

In a particular path planning task, the initial configuration of space manipulator is set as[-50,-170,150,-60,130,170,0](°) and the target pose is set as [9.6,0,3,-1,-0.5,-2]. The other parameters of path planning task are listed in Table 2.Table 2
Parameters for path planning task.
Task cycle
Control interval
Failure joint
Failure time
Position threshold
20 s
0.05 s
2nd  joint
10 s
l= (0.01 m, 0.01 m, 0.01 m)According to Sections3.1 and 3.2, under the position threshold of l, the feasible region of ck is calculated as ck∈[0,0.36]. Then set the initial value of ck as 0, and carry out search algorithm with interval Δck=0.01 in the feasible region. The value of objective function f changing with ck is shown in Figure 5. When ck=0.25, the objective function is minimal; fmin=0.0035rad/s.Figure 5
Value of objective function changes withck.Whenck=0.25, the joint velocities are shown in Figure 6. Figure 6(a) represents the velocity curve of joint 2 (failure joint), and Figure 6(b) represents the velocity curve of joint 3 (healthy joint). It is obvious that the joint velocity jump is significantly reduced after global compensation. In detail, the value of velocity jumps, acceleration, and jump reduction rate of each joint is listed in Table 3.Table 3
Analysis of velocity jump and acceleration with compensation for each joint.
Jointi
Velocity jump before compensation (rad/s)
Acceleration before compensation (rad/s2)
Velocity jump after compensation (rad/s)
Acceleration after compensation (rad/s2)
Reduction rate of velocity jump
1
7.891
×
1
0
-
3
1.578
×
1
0
-
1
1.476
×
1
0
-
3
2.952
×
1
0
-
2
81.29
%
2
1.940
×
1
0
-
2
3.880
×
1
0
-
1
2.584
×
1
0
-
3
5.168
×
1
0
-
2
86.68
%
3
3.682
×
1
0
-
3
7.364
×
1
0
-
2
1.185
×
1
0
-
3
2.370
×
1
0
-
2
67.82
%
4
5.114
×
1
0
-
4
1.023
×
1
0
-
2
3.323
×
1
0
-
4
6.646
×
1
0
-
3
35.02
%
5
2.774
×
1
0
-
4
5.548
×
1
0
-
3
6.454
×
1
0
-
4
1.291
×
1
0
-
2
-
57.62
%
6
6.048
×
1
0
-
4
1.210
×
1
0
-
2
1.259
×
1
0
-
4
2.518
×
1
0
-
3
79.18
%
7
0
0
0
0
0
Objective function
2.128
×
1
0
-
2
4.256
×
1
0
-
1
3.287
×
1
0
-
3
5.673
×
1
0
-
2
84.55
%Figure 6
Joint velocity compensated whenck=0.25.
(a)
Joint 2
(b)
Joint 3In Table3, velocity jump of each joint is reduced significantly except joint 5. Since objective function reflects the accumulation of velocity jump of all the joints, when objective function is minimal, it does not mean the velocity jump of each joint is minimal. The velocity jumps of some joints may increase, such as joint 5 in Table 3. In a former paper of the author [28], the coefficient is changed into diagonal matrix form to achieve velocity jump minimal for each joint. However, it brings about higher computation cost and more complex solution process. With constant coefficient of ck, the norm of velocity jumps is reduced 84.55%, which shows great efficiency of the global compensation vector constructed with constant coefficient in reducing joint velocity jumps. Although jumps of some joints (joint 5) increase, the velocity jump is very small and can be accepted in practical application.Besides, the position deviation in operation space whenck=0.25 is obtained in Figure 7. With the compensation in joint velocity at each control interval, the position deviation in operation space is accumulated. The maximum deviation is less than 4×10-3m, which is much smaller than the length of manipulator (101 order of magnitude). During the 20 s’ path planning task, the velocity jump in operation space is about 10-4m/s order of magnitude, which can be basically ignored. In conclusion, the velocity jumps in operation space can be mostly eliminated, while the velocity jump in joint space can be reduced to minimal when joint-locked failure occurs at a particular time.Figure 7
Position deviations in operation space after velocity reduction.


### 4.3. Coefficient Function Fitting for Unexpected Joint-Locked Failure

Define the initial configuration of space manipulator path planning task as[-61.5146-176.3220145.8516-60.7288129.9836169.13760](°), and set the target pose as [9.6m, 0m, 3m, -1rad, -0.5rad, -2rad]. The velocity in operation space is planned by trapezoidal planning with parabola transition. Task cycle is 12 s, acceleration time is 2 s, and control interval is 50 ms. Joint-locked failure is assumed to occur in joint 2.Since joint-locked failure time is unexpected, a priori failure information is used to fit the coefficient function. A priori failure information includes possible failure time, failure joint angle, and the corresponding optimalck. During the mentioned path planning task, 40 moments are selected uniformly as possible failure time. According to (18) and (19), the joint angle at failure time can be obtained. Correspondingly, the optimal ck related to each moment can be obtained, which is shown in Figure 8. Regardless of the samples ck=0, the other samples can be used to fit a five-order polynomial function when meeting the threshold of fitting ec=10-4:(34)ct=-0.0005t5+0.0175t4-0.2341t3+1.532t2-5.0034t+6.9496.Figure 8
Samples and fitted polynomial function ofck(t).Considering the samplesck=0, the coefficient function for an unexpected joint-locked failure can be expressed as(35)ckt=00≤t<ts1ctts1≤t≤ts20ts2≤t<te.t
s
1 and ts2 are segment points. The criterion to determine the segment points is defined as follows: the values of objective function compensated with ck=ck(ts) and ck=0 are the same; namely,(36)fck=ckts-fck=0∈ε.ε represents infinitesimal. However, for each possible failure moment t, there is a feasible region for the related coefficient ckt∈cktmin,cktmax. If ts selected according to (36) guarantees c(ts)∉cktsmin,cktsmax, it should be abandoned. Instead of it, the segment points are set as ts=ts∣c(t)=cktsmaxorc(t)=cktsmin,ts∈R+.Accordingly, two segment points can be obtained asts1=1.1s, ts2=10.9s. Then (35) can be expressed as(37)ckt=00≤t<1.1-0.0005t5+0.0175t4-0.2341t3+1.532t2-5.0034t+6.94961.1≤t≤10.9010.9≤t<12.Based on (37), the compensation vector of each control interval u(ck(t)) can be obtained, with which the velocity jump can be reduced no matter when joint-locked failure occurs.


### 4.4. Verification of the Coefficient Function Related to Unexpected Joint-Locked Failure

In order to verify the efficiency of the optimal coefficient function (37), assume joint-locked failure occurs at each control interval; namely, failure occurs at 0.05s,0.1s,0.15s,…,12s. Via the velocity compensation vector based on (37), the velocity jumps related to each failure time are shown in Figure 9.Figure 9
Joint velocity jump comparison along the task cycle.At most failure time, the joint velocity jump can be significantly reduced. The average velocity jump equals4.9e-3rad/s after compensation. Compared with the average value of 0.0172rad/s before compensation, velocity jump is reduced 71.5% averagely. Especially in period of ts3–ts4, the performance of jump reduction is remarkable, and the velocity jump is reduced below 5e-3rad/s.Before velocity jump reduction, during a control intervalΔt=0.05s, joint acceleration needs to change 0.344rad/s2 averagely when joint-locked failure occurs. In order to afford the acceleration, huge joint torque is needed. When the safe threshold of joint acceleration is set as 0.15rad/s2 to protect joint torque not exceeding the rated torque, the change of velocity cannot be achieved and huge impact will be affected on joint motor. After velocity jump reduction, the requirement for joint acceleration is reduced to 0.098rad/s2 averagely, which fulfills the safe threshold and the impact on the joint can be eliminated. In this way, the proposed trajectory strategy is of great efficiency in smoothing the velocity parameter and guaranteeing motion safety.During periods of0–ts1 and ts2–te, the reduction performance of velocity jump is worse than other periods. According to (37), the compensation coefficient in this period equals 0, which means no compensation is introduced. So when joint-locked failure occurs in this period, the velocity jump cannot be reduced with the proposed method. However, the velocity jump in this period is below 0.01rad/s(0.57°/s), which is acceptable for space manipulator in most applications.Periods oft0–ts3 and ts4–te are almost consistent with the acceleration stage and deceleration stage of path planning. Velocity in operation space changes quickly in the two stages, making the velocity compensation insufficient for jump reduction. As a result, the reduction performance is not ideal in these periods. Period ts3–ts4 corresponds to the uniform motion stage of path planning, the velocity in operation space during this stage is stable, and the velocity jump reduction is significant. It can be concluded that velocity changing in operation space at the failure time can influence the performance of velocity jump reduction.In conclusion, the coefficient function obtained based on a priori failure information is verified efficiently in reducing the velocity jumps caused by an unexpected joint-locked failure. And velocity changing in operation space at the failure time can influence the reduction performance. The limitation of the method is that when joint-locked failure occurs at the initial and end period of path planning, the velocity jump cannot be reduced. However, velocity jumps in these periods are always small, which can be ignored in space manipulator applications.


## 4.1. Research Object

An SSRMS [2] type space manipulator who has 7 DOF is used to carry out simulation experiments. The coordinate systems are shown in Figure 4, and the DH parameters are listed in Table 1. Path planning task mentioned in the paper concerns the position accuracy in the operation space of space manipulator only.Table 1
DH parameters of space manipulator.
i
θ
i (°)
d
i (m)
a
i
-
1 (m)
α
i
-
1 (°)
1
0
0.6
0
90
2
90
0.5
0
-
90
3
0
0.5
5
0
4
0
0.5
5
0
5
0
0.5
0
90
6
-
90
0.5
0
-
90
7
0
0.6
0
0Figure 4
Coordinate systems of 7 DOF space manipulator.


## 4.2. Velocity Jump Reduction When Joint-Locked Failure Occurs at a Particular Time

In a particular path planning task, the initial configuration of space manipulator is set as[-50,-170,150,-60,130,170,0](°) and the target pose is set as [9.6,0,3,-1,-0.5,-2]. The other parameters of path planning task are listed in Table 2.Table 2
Parameters for path planning task.
Task cycle
Control interval
Failure joint
Failure time
Position threshold
20 s
0.05 s
2nd  joint
10 s
l= (0.01 m, 0.01 m, 0.01 m)According to Sections3.1 and 3.2, under the position threshold of l, the feasible region of ck is calculated as ck∈[0,0.36]. Then set the initial value of ck as 0, and carry out search algorithm with interval Δck=0.01 in the feasible region. The value of objective function f changing with ck is shown in Figure 5. When ck=0.25, the objective function is minimal; fmin=0.0035rad/s.Figure 5
Value of objective function changes withck.Whenck=0.25, the joint velocities are shown in Figure 6. Figure 6(a) represents the velocity curve of joint 2 (failure joint), and Figure 6(b) represents the velocity curve of joint 3 (healthy joint). It is obvious that the joint velocity jump is significantly reduced after global compensation. In detail, the value of velocity jumps, acceleration, and jump reduction rate of each joint is listed in Table 3.Table 3
Analysis of velocity jump and acceleration with compensation for each joint.
Jointi
Velocity jump before compensation (rad/s)
Acceleration before compensation (rad/s2)
Velocity jump after compensation (rad/s)
Acceleration after compensation (rad/s2)
Reduction rate of velocity jump
1
7.891
×
1
0
-
3
1.578
×
1
0
-
1
1.476
×
1
0
-
3
2.952
×
1
0
-
2
81.29
%
2
1.940
×
1
0
-
2
3.880
×
1
0
-
1
2.584
×
1
0
-
3
5.168
×
1
0
-
2
86.68
%
3
3.682
×
1
0
-
3
7.364
×
1
0
-
2
1.185
×
1
0
-
3
2.370
×
1
0
-
2
67.82
%
4
5.114
×
1
0
-
4
1.023
×
1
0
-
2
3.323
×
1
0
-
4
6.646
×
1
0
-
3
35.02
%
5
2.774
×
1
0
-
4
5.548
×
1
0
-
3
6.454
×
1
0
-
4
1.291
×
1
0
-
2
-
57.62
%
6
6.048
×
1
0
-
4
1.210
×
1
0
-
2
1.259
×
1
0
-
4
2.518
×
1
0
-
3
79.18
%
7
0
0
0
0
0
Objective function
2.128
×
1
0
-
2
4.256
×
1
0
-
1
3.287
×
1
0
-
3
5.673
×
1
0
-
2
84.55
%Figure 6
Joint velocity compensated whenck=0.25.
(a)
Joint 2
(b)
Joint 3In Table3, velocity jump of each joint is reduced significantly except joint 5. Since objective function reflects the accumulation of velocity jump of all the joints, when objective function is minimal, it does not mean the velocity jump of each joint is minimal. The velocity jumps of some joints may increase, such as joint 5 in Table 3. In a former paper of the author [28], the coefficient is changed into diagonal matrix form to achieve velocity jump minimal for each joint. However, it brings about higher computation cost and more complex solution process. With constant coefficient of ck, the norm of velocity jumps is reduced 84.55%, which shows great efficiency of the global compensation vector constructed with constant coefficient in reducing joint velocity jumps. Although jumps of some joints (joint 5) increase, the velocity jump is very small and can be accepted in practical application.Besides, the position deviation in operation space whenck=0.25 is obtained in Figure 7. With the compensation in joint velocity at each control interval, the position deviation in operation space is accumulated. The maximum deviation is less than 4×10-3m, which is much smaller than the length of manipulator (101 order of magnitude). During the 20 s’ path planning task, the velocity jump in operation space is about 10-4m/s order of magnitude, which can be basically ignored. In conclusion, the velocity jumps in operation space can be mostly eliminated, while the velocity jump in joint space can be reduced to minimal when joint-locked failure occurs at a particular time.Figure 7
Position deviations in operation space after velocity reduction.


## 4.3. Coefficient Function Fitting for Unexpected Joint-Locked Failure

Define the initial configuration of space manipulator path planning task as[-61.5146-176.3220145.8516-60.7288129.9836169.13760](°), and set the target pose as [9.6m, 0m, 3m, -1rad, -0.5rad, -2rad]. The velocity in operation space is planned by trapezoidal planning with parabola transition. Task cycle is 12 s, acceleration time is 2 s, and control interval is 50 ms. Joint-locked failure is assumed to occur in joint 2.Since joint-locked failure time is unexpected, a priori failure information is used to fit the coefficient function. A priori failure information includes possible failure time, failure joint angle, and the corresponding optimalck. During the mentioned path planning task, 40 moments are selected uniformly as possible failure time. According to (18) and (19), the joint angle at failure time can be obtained. Correspondingly, the optimal ck related to each moment can be obtained, which is shown in Figure 8. Regardless of the samples ck=0, the other samples can be used to fit a five-order polynomial function when meeting the threshold of fitting ec=10-4:(34)ct=-0.0005t5+0.0175t4-0.2341t3+1.532t2-5.0034t+6.9496.Figure 8
Samples and fitted polynomial function ofck(t).Considering the samplesck=0, the coefficient function for an unexpected joint-locked failure can be expressed as(35)ckt=00≤t<ts1ctts1≤t≤ts20ts2≤t<te.t
s
1 and ts2 are segment points. The criterion to determine the segment points is defined as follows: the values of objective function compensated with ck=ck(ts) and ck=0 are the same; namely,(36)fck=ckts-fck=0∈ε.ε represents infinitesimal. However, for each possible failure moment t, there is a feasible region for the related coefficient ckt∈cktmin,cktmax. If ts selected according to (36) guarantees c(ts)∉cktsmin,cktsmax, it should be abandoned. Instead of it, the segment points are set as ts=ts∣c(t)=cktsmaxorc(t)=cktsmin,ts∈R+.Accordingly, two segment points can be obtained asts1=1.1s, ts2=10.9s. Then (35) can be expressed as(37)ckt=00≤t<1.1-0.0005t5+0.0175t4-0.2341t3+1.532t2-5.0034t+6.94961.1≤t≤10.9010.9≤t<12.Based on (37), the compensation vector of each control interval u(ck(t)) can be obtained, with which the velocity jump can be reduced no matter when joint-locked failure occurs.


## 4.4. Verification of the Coefficient Function Related to Unexpected Joint-Locked Failure

In order to verify the efficiency of the optimal coefficient function (37), assume joint-locked failure occurs at each control interval; namely, failure occurs at 0.05s,0.1s,0.15s,…,12s. Via the velocity compensation vector based on (37), the velocity jumps related to each failure time are shown in Figure 9.Figure 9
Joint velocity jump comparison along the task cycle.At most failure time, the joint velocity jump can be significantly reduced. The average velocity jump equals4.9e-3rad/s after compensation. Compared with the average value of 0.0172rad/s before compensation, velocity jump is reduced 71.5% averagely. Especially in period of ts3–ts4, the performance of jump reduction is remarkable, and the velocity jump is reduced below 5e-3rad/s.Before velocity jump reduction, during a control intervalΔt=0.05s, joint acceleration needs to change 0.344rad/s2 averagely when joint-locked failure occurs. In order to afford the acceleration, huge joint torque is needed. When the safe threshold of joint acceleration is set as 0.15rad/s2 to protect joint torque not exceeding the rated torque, the change of velocity cannot be achieved and huge impact will be affected on joint motor. After velocity jump reduction, the requirement for joint acceleration is reduced to 0.098rad/s2 averagely, which fulfills the safe threshold and the impact on the joint can be eliminated. In this way, the proposed trajectory strategy is of great efficiency in smoothing the velocity parameter and guaranteeing motion safety.During periods of0–ts1 and ts2–te, the reduction performance of velocity jump is worse than other periods. According to (37), the compensation coefficient in this period equals 0, which means no compensation is introduced. So when joint-locked failure occurs in this period, the velocity jump cannot be reduced with the proposed method. However, the velocity jump in this period is below 0.01rad/s(0.57°/s), which is acceptable for space manipulator in most applications.Periods oft0–ts3 and ts4–te are almost consistent with the acceleration stage and deceleration stage of path planning. Velocity in operation space changes quickly in the two stages, making the velocity compensation insufficient for jump reduction. As a result, the reduction performance is not ideal in these periods. Period ts3–ts4 corresponds to the uniform motion stage of path planning, the velocity in operation space during this stage is stable, and the velocity jump reduction is significant. It can be concluded that velocity changing in operation space at the failure time can influence the performance of velocity jump reduction.In conclusion, the coefficient function obtained based on a priori failure information is verified efficiently in reducing the velocity jumps caused by an unexpected joint-locked failure. And velocity changing in operation space at the failure time can influence the reduction performance. The limitation of the method is that when joint-locked failure occurs at the initial and end period of path planning, the velocity jump cannot be reduced. However, velocity jumps in these periods are always small, which can be ignored in space manipulator applications.


## 5. Conclusion

With the proposed trajectory optimization strategy in the paper, the on-orbit operations can be executed to completion without shutting down the space manipulator when joint-locked failure occurs unexpectedly. The velocity jumps in both joint space and operation space caused by joint-locked failure are simultaneously reduced, and the impact caused on space manipulator structure can be eliminated. As a result, the safety of space manipulator and related parts can be guaranteed. The core of the strategy is the construction of global compensation vector for joint velocity, which consists of the gradient of reduced manipulability, orthogonal matrix of null space, and coefficient. The strategy can deal with velocity jumps reduction problem caused by joint-locked failure at either a particular time or an unexpected time. For joint-locked failure at a particular time, the optimal coefficient of compensation vector can be obtained to achieve velocity jump minimal. For joint-locked failure at an unexpected time, a priori failure information is obtained by assuming the possible failure time and calculating the corresponding optimal coefficient. Then the law of coefficient is concluded for trajectory optimization. As a result, coefficient is turned into function form and a function fitting method based on a priori failure information is proposed. With simulations, the efficiency of the method is verified in reducing the velocity jumps no matter when joint-locked failure occurs.For further study, the effect of the initial configuration of manipulator on the velocity jump reduction performance should be discussed in-depth. Besides, the velocity in operation space is planned by the trapezoidal planning with parabola transition in the paper. However, the reduction performance in the acceleration and deceleration stage is not ideal compared with the uniform motion stage, which is considered to be the limitation of the velocity planning method in operation space. By changing the planning way of velocity in operation space, the reduction performance for velocity jump is worthy to be researched. In addition, the paper concentrates on space manipulator and single joint failure. For arbitrary serial manipulator, the trajectory optimization strategy is adaptable, which can also be developed for multiple joint failure tolerance.



---

*Source: 7819540-2016-02-21.xml*