Skip to main content

Full text of "Fault tolerant kinematic control of hyper-redundant manipulators"

See other formats


AIAA-94-1295-CP 

FAULT TOLERANT KINEMATIC CONTROL OF 
HYPER-REDUNDANT MANIPULATORS 

Nazareth S. Bedrossian 1 

The Charles Stark Draper Laboratory, Inc. 

2200 Space Park Drive, Suite 210 

Houston, TX 77058 

e-mail: naz@mickey-csdl.jsc.nasa.gov 



Abstract 

This paper investigates the fault-tolerant control of 
hyper-redundant spatial manipulators. The standard re- 
solved rate control law using the pseudoinverse is modi- 
fied to account for joint failures. To combat the problem 
of extremely high joint velocity solutions generated near 
singular configurations by the pseudoinverse, the singu- 
larity robust inverse is employed. A method to com- 
pute an optimal scale factor for the robust inverse is 
derived. Simulation results of this approach applied to 
an 11 DOF manipulator are presented which verify the 
validity of this approach. 

1 Introduction 

Recent advances in the field of robotics has resulted 
in redundant manipulators gaining increased attention 
due to advantages over conventional manipulators such 
as increased dexterity. Hyper-redundant manipula- 
tors represent the next step in manipulator evolution. 
These manipulators posses a large number of Degrees- 
Of-Freedom (DOF). This paper investigates the kine- 
matic control of hyper-redundant spatial manipulators 
with particular emphasis on fault-tolerant control. 

The redundant structure of such manipulators en- 
dows them with many desirable properties, such as 
fault- tolerant features. The redundancy can be ex- 
ploited to seamlessly complete end-effector tasks in 
the face of single or multiple joint motor failures. A 
rate-inverse kinematic control algorithm utilizing the 
pseudoinverse is presented that is insensitive to arbi- 
trary joint motor failures. Another feature of redun- 
dant manipulators is the possibility of optimal joint- 
motion coordination. However, redundant manipula- 
tors are plagued by the presence of singular configura- 



tions. In these configurations first order motion in a 
certain end-effector direction is not possible, and are 
associated with loss of manipulator Jacobian matrix 
rank. Standard kinematic control algorithms fail at or 
near singular configurations. The singularity problem 
is also considered by implementing a singularity robust 
kinematic control algorithm that is insensitive to joint 
failures. The robustness is determined by a scaling fac- 
tor. An optimal method to pick the scaling factor is 
also derived such that end-effector tracking accuracy is 
sacrificed in order to not violate joint velocity limits. 
Simulation results of this approach applied to an 11 
DOF spatial manipulators are presented which verify 
the validity of the proposed methodology. 

2 Manipulator Kinematics 

In this section, a brief review of manipulator spatial 
kinematics is presented. For an n-link, serial, open- 
loop spatial manipulator, denote the space of joint co- 
ordinates by q 6 Q — T" . The corresponding end- 
effector position and attitude is denoted by x 6 X = 
H 3 x 50(3). For a redundant manipulator, n > 6. The 
end-effector motion kinematics are given by: 



x = 



P 



= J(q) q 



1 Member Technical Staff, Control & Decision Systems. 
'Copyright ©1993 by the American Institute of Aeronautics 
and Astronautics, Inc. All rights reserved 



Here, p denotes the end-effector position, ui is the end- 
effector angular velocity expressed in an inertial Carte- 
sian reference frame, and J(q) is the 6x n "constructed" 
Jacobian transformation matrix. 

All manipulators posses singular configurations in- 
side their workspace. These configurations, q" , are 
manifest when the Jacobian matrix loses full rank, 
i.e. rank(J) < dim(X). In terms of the Singular 
Value Decomposition (SVD) of the Jacobian matrix, 
J = UHV T , this corresponds to at least one singular 
value <Tj = 0. The corresponding singular direction U, 



830 



(the i-th column of U) is the direction in which end- 
effector motion to first order is instantaneously impos- 
sible. At a singular configuration, a solution for the 
joint rates cannot be constructed from the first order 
approximation of the forward kinematics. Hence, kine- 
matic control algorithms that rely on inversion of the 
Jacobian matrix in one form or the other generate ex- 
tremely large joint rate solutions near or at a singular 
configuration. 

3 Inverse Rate Kinematics 

Manipulator tasks are described in end-effector 
space, X, while actuator commands are in joint-space, 
Q. For this reason, an inverse kinematic algorithm is re- 
quired to generate joint-angle commands from the end- 
effector commands. The inversion can be carried out 
at different differential levels of the forward kinematics. 
In this paper only first order inversion or Inverse Rate 
Kinematics (IRK) will be considered. Kinematic con- 
trol algorithms that use this approach are also known 
as Resolved Rate Control (RRC) algorithms. 

For inverse rate kinematics, the end-effector velocity 
profile along the desired trajectory is specified. At each 
increment, the joint rates are computed by "inverting" 
the Jacobian matrix evaluated at the current configu- 
ration. These joint rates are then integrated (usually 
via Euler integration) to generate the next set of joint 
angles. Assuming the current configuration is nonsin- 
gular, the general form of the solution for joint rates 



Qk 



= J^(qk) ik + v 



where the notation jt denotes a generalized inverse and 
v is the instantaneous self motion (or null motion), i.e. 
J(lk) v = Omxi- For nonredundant manipulators jt = 
J~ l and v = 0„ xl . 

For a redundant manipulator J f = J T [j J T ] , 
known as the pseudoinverse of J, and v ^ nx i- The 
pseudoinverse originates from a 2-norm joint rate min- 
imization problem. Specifically it is obtained as the 
solution to the following quadratic cost optimization 
problem: 



min 0.5 q q 
i 

subject to J(q)q = & 



(1) 



In the redundant case, there are n— 6 degrees of freedom 
in the nullspace of the Jacobian matrix that can be 
assigned arbitrarily. In both cases, the joint angles are 
then obtained from: 

qk+i = qk + 4k At 



4 Resolved Rate Law For Failed Joints 

The solution to the quadratic optimization problem 
in the previous section assumed that all the joints were 
active or had not failed. In this section a modified ver- 
sion of the pseudoinverse which accounts for joint fail- 
ures is derived. Assuming joint i fails, this implies that 
?i = 0. The incidence of a failed joint can be expressed 
as an additional constraint of the form: 

M = mx i 

The m x n constraint matrix, A, is a zero matrix with 
A(i,j) = 1 for the j-th failed joint with a total number 
of failures equal to m. To illustrate this, for a 9- link ma- 
nipulator with joints 4 and 7 failed, the corresponding 
A matrix has the form: 



A = 



000100000 
000000100 



To derive the modified pseudoinverse to account for 
failed joints, the quadratic optimization problem (2) is 
reformulated as: 

min 0.5 q T q (2) 



subject to J(q)q — i 
subject to Aq = mx i 

The solution of this new optimization problem (2) is, 



q = BJ T [jBJ T ] 



t i-i 



(3) 



where: 



5 = 



l nxn -A T (AA T )- 1 A] 



Comparing this solution with the one for all joints free, 
it is seen that by setting A = mxrj the pseudoinverse 
solution is obtained. 

5 Robust RRC For Failed Joints 

The standard IRK solution utilizing the pseudoin- 
verse fails at or near singular configurations due to ex- 
tremely large joint rate solutions. It has been shown 
[2] that the pseudoinverse does not generate singular- 
ity free trajectories. Therefore, the pseudoinverse ap- 
proach does not posses any singularity avoidance prop- 
erties. Since solving (2) can drive the system to a singu- 
lar configuration, one approach to alleviate this prob- 
lem is to relax the equality trajectory constraint. 

The Singularity Robust Inverse (SRI) (or damped 
least squares) proposed in [6], and [8] as an alternative 
to the pseudoinverse accomplishes a tradeoff between 



831 



accuracy and feasibility of solution. Near a singular 
configuration the joint rates remain finite and bounded 
in exchange for a build-up in tracking error. The robust 
resolved rate law is obtained from the solution to the 
constrained minimization problem, 



mm 
i 



vhere: 



W = 



0.5 e T W e 



x- J(q)q 

q 

Wi 6x „ 

0„x6 W 2 



(4) 



The weighting matrices, W\ (6 x 6) and W 2 (nxn), are 
arbitrary. Assuming W\ = 16x6 and W 2 = k l nxn , the 
solution to (4) is given by [6]: 



q = n= [J T J + kI] l J 1 



(5) 



In the above, J* is the Singularity Robust Inverse 
(SRI). An alternative but equivalent expression for J* 
is [6]: 

q = n = J T [J J T +kI] 1 x (6) 

Setting the scaling factor k — in (11), it is seen that 
J* reduces to the standard pseudoinverse J*. The er- 
ror vector e represents the tradeoff between accuracy 
of solution (expressed by e(l)) and feasibility of solu- 
tion (expressed by e(2)). The tradeoff parameter is the 
scaling factor, k, which is the degree of freedom in the 
formulation of the SRI control law. In the following 
section, an optimal method to chose the scaling factor 
is presented. 

The standard formulation of the SRI assumes that 
all joints are free. To derive the modified SRI to ac- 
count for failed joints, the optimization problem (4) is 
reformulated as: 



mm 



0.5e T W e 



(7) 



subject to Aq = mx i 



The solution to this new optimization problem (7) is 
given by, 



q = p-' 



where: 



I -A 1 



[AP~ l A T 



AP~ 



J T Wi x 



(8) 



P = J T Wi J + W 2 



It will be assumed that W\ = 16x6 and W 2 = k l„ xri . 
It is noted that the first term in the right hand side of 
(8) is just the standard SRI solution, while the second 
term accounts for the effect of the failed joints. 



5.1 Determining the Scaling Factor 

The robustness properties or the SR-Inverse are de- 
termined by the scaling factor k, which expresses the 
tradeoff between the exactness and feasibility of solu- 
tion. Depending on the particular emphasis of an ap- 
plication, several methods for choosing the scaling fac- 
tor have appeared in the literature [6], [8], [4]. These 
methods adjust the scaling factor as a function of some 
Jacobian based metric such as the minimum singular 
value or the manipulability measure. Hence, the scal- 
ing factor is adjusted based on the proximity of the 
manipulator to a singular configuration. 

Another approach is to chose the scaling factor such 
that an appropriate norm of the joint rates does not 
exceed a predetermined threshold [3], [5]. Such an ap- 
proach directly takes into account the maximum allow- 
able joint velocity constraints while minimizing the end- 
effector deviation from the desired trajectory. When- 
ever the pseudoinverse (or modified pseudoinverse in 
the case of failed joints) does not violate the joint ve- 
locity threshold (i.e. the solution is feasible) scaling is 
not required and k = 0. In this case the SRI solution 
is not required. When the pseudoinverse solution vio- 
lates the velocity norm threshold (i.e. the solution is 
infeasible) the SRI can be used to generate a feasible 
solution which minimizes the deviation from the speci- 
fied end-effector trajectory if the choice of scaling factor 
satisfies: 



J T [ J J T + k I ] 1 x \\ m < q„ 



(9) 



In (9), the notation || • || m denotes the vector m-norm. 
Hence, the problem of generating joint rates that do 
not violate a rate limit is posed as follows: 



if || J^x \\ m < q max k — else solve 
|| J T [JJ T + K/] _1 z|| m = q max (10) 



Obtaining a closed form solution to (10) is not practi- 
cally possible as it is a nonlinear problem. An iterative 
approach to solve (10) for m = 2 has been presented 
in [3] and [5]. However, an approximate closed form 
solution can be obtained which is described in the fol- 
lowing. 

Using the singular value decomposition (SVD) of the 
Jacobian matrix, J = UT,V T , (10) can be written in 
the form, 



= II V E f U 1 



= 9r, 



(11) 



832 



where: 



E* = 



a\ + k 











02 





g\ + K 











"6 






To simplify (11), the joint velocity norm can be upper- 
bounded using induced matrix norms [7]: 

II Q IU < II V |U || Et || im || U T x || m (12) 

In (12), the notation || • ||i m denotes the induced (m) 
norm corresponding to the vector norm || • || m . Using 
(12), a conservative relation involving the joint velocity 
norm threshold is: 



|| V \\ im || E* |U || U T X \\ m < q„ 
Solving for £t f rom (13): 

II s f IU < 



II v |U II U T x ||„ 



(13) 



(14) 



The relation (14) is the key to solving for the scaling 
factor in order to enforce the joint velocity norm lim- 
its. Typically, joint velocity thresholds are specified as 
either a 2-norm or oo-norm constraint. For a 2-norm 
rate limit, the induced 2-norm of E* is [7], 



II S f ||i2 = [ A max ( EtV ) ] 



1/2 



where: 

^max ( E* SM = Maximum eigenvalue of E* E* 

For an oo-norm rate limit, the induced oo-norm of E* 
is [7]: 

IIE'IU = max£ |E^| 

Due to the special structure of E* and the fact that 
Ej j > 0, it is easy to show that in both cases its induced 
norm reduces to: 

II ^ ||„ = || Et \\ ioo = max -p— 

In applications, usually the maximum absolute joint 
velocity is limited instead of the quadratic velocity 



norm. Assuming that maximum absolute velocity limit 
is the same for all joints, (14) reduces to: 



max 



<rf + K 



< 



II V \\ ioo || U-r i || c 



(15) 



For a fixed value of the scaling factor k, as a singular 
value <?i approaches zero, the expression ^/(cr? + k) 
increases until it reaches a maximum value when <Ti = 
■y/w and then decreases to zero. Therefore, 



1 



max 



flf + K ~ 2-y/K 



Hence, a conservative solution for the scaling factor that 
will satisfy (15) is: 



k > 



II V llico II U T i lie 

^ Qmax 



(16) 



In actual implementation, the equality is used in com- 
puting the scaling factor. Similarly, if the joint velocity 
threshold is expressed in terms of a 2-norm, the solution 
for the scaling factor is given by: 



k > 



II V 11.2 II U T i h 

*• Qmax 



6 Numerical Simulations 

In this section, simulation results of the techniques 
described in this paper applied to an 11 DOF spatial 
manipulator are presented. This is a special modular 
manipulator developed at NASA/JSC [1] with a 15 foot 
reach. Its architecture is described by the joint se- 
quence RPR-PR-PR-PR-RPR where R denotes a roll 
joint and P denotes a pitch joint. 

The first simulation example is used to illus- 
trate the fault-tolerant resolved rate law using the 
modified pseudoinverse (3). The initial config- 
uration of the manipulator is given by qg = 
[45, -60, 0, -45, 0, 45, 90, 45, 0, 0, 0, 0]. The ini- 
tial configuration is shown in Figure 1. The end-effector 
command is x = [ -5.5, -2.5, 1.5, 0, 0, ] with units 
of in/sec for the translational component and deg/sec 
for the rotational component. In the following figures, 
the end-effector position is displayed as, solid line for 
x-axis, dashed line for y-axis, and dotted line for z-axis. 
The end-effector attitude is given as a Pitch, Yaw, Roll 
(PYR) Euler angle sequence, with solid line for pitch, 
dashed line for yaw, dotted line for roll. For all joints 
free throughout the simulation, the results for the stan- 
dard pseudoinverse are shown in Figures 2 and 3. Fig- 
ure 2 shows the end-effector position and attitude while 



833 



Figure 3 shows the joint angle trajectory. Now consider 
the case when joints 1,6, and 7 have failed throughout 
the simulation. The results for the modified pseudoin- 
verse (3) are shown in Figures 4 and 5. Figure 4 shows 
the end-effector trajectory which is identical to the all 
joints free case. Figure 5 shows the joint angle trajec- 
tory. It is seen that joints 1,6, and 7 do not move as 
would be expected. 

To illustrate the SRI with joint velocity thresh- 
olds, consider a maneuver of moving the end-effector 
in the a;-direction. The initial configuration is go = 
[0, -50, 0, -30, 0, 70, 0, 30, 0, 0, 0]. The end- 
effector command is x = [-5, 0, 0, 0, 0, 0]. First 
consider the case when all joints are free throughout 
the simulation. The results of using the standard pseu- 
doinverse are shown in Figures 6 and 7. Figure 6 shows 
the position and attitude history of the end-effector, 
while Figure 7 shows the joint rate profile. It is seen 
that the maximum joint rate is less than 6 deg/sec. 

Now consider the case when joints 2,4, and 7 have 
failed throughout the simulation. The results of using 
the modified pseudoinverse, (3), are shown in Figures 8 
and 9. The end-effector trajectory shown in Figure 8 is 
seen to deviate somewhat from the desired trajectory. 
This is due to the very high joint rate solution (up to 
400 deg/sec) generated by (3) shown in Figure 9, and 
the particular integration approach used to generate 
the end-effector trajectory. The results of using the SRI 
with a maximum absolute joint rate limit of 50 deg/sec, 
i.e. || q || 10o < 50 deg/sec, are shown in Figures 10 to 
12. The end-effector trajectory is shown in Figure 10 
where the deviation from the desired trajectory appears 
in the x and z-axes and pitch angle. The main effect 
of using the SRI is the reduction in system response in 
the commanded direction. From Figure 10 it is seen 
that at the end of the simulation the manipulator has 
only moved halfway in the x-direction. The joint angle 
trajectory is shown in Figure 11 from which it is seen 
that the joint rate limit is not exceeded. Finally, Figure 
12 shows the history of the SRI scale factor. It is seen 
that the SRI solution is activated at approximately 2 
seconds. 

7 Conclusion 

This paper has addressed the fault tolerant kine- 
matic control of hyper-redundant manipulators. The 
standard resolved rate control law utilizing the pseu- 
doinverse was modified to account for joint failures. 
However, pseudoinverse based control laws fail at or 
near singular configurations due to extremely high joint 
rate solutions. To generate feasible joint motions near 
singular configurations, the Singularity Robust Inverse 
(SRI) instead of the pseudoinverse was employed. The 



standard formulation of the SRI was modified so as to 
account for the possibility of failed joints. As the ro- 
bustness properties of the SRI are determined by the 
choice of scaling factor, an optimal method to pick the 
scaling factor was presented. Numerical simulations 
were presented utilizing an 11 DOF spatial manipulator 
to illustrate the proposed solution methodologies. The 
simulation results verified the validity of the proposed 
methods. 

References 

[1] Ambrose, R., "Space Modular Manipulators", 
NASA/JSC presentation SMM 93-13, March 1993. 

[2] Baillieul, J., Hollerbach, J., and Brockett, R., 
"Programming and Control of Kinematically Re- 
dundant Manipulators" , 23rd IEEE Conf. Dec. & 
Cont, 1984, pp. 768-774. 

[3] Deo, A. S., and Walker, I. D., "Robot Subtask Per- 
formance with Singularity Robustness using Op- 
timal Damped Least-Squares", 1992 IEEE Int. 
Conf. Rob. & Aut., pp. 434-441. 

[4] Egeland, O., Sagli, J. R., Spangelo, I., and Chi- 
averini, S., "A Damped Least-Squares Solution to 
Redundancy Resolution" , 1991 IEEE Int. Conf. 
Rob. & Aut, pp. 945-950. 

[5] Maciejewski, A. A., and Klein, C. A., "The Singu- 
lar Value Decomposition : Computation and Ap- 
plication to Robotics", Int. J. Rob. Res., Vol. 8, 
No. 6, 1989, pp. 63-79. 

[6] Nakamura, Y., and Hanafusa, H., "Inverse Kine- 
matic Solutions With Singularity Robustness for 
Robot Manipulator Control" , ASME Journal Dy- 
namic Systems, Measurement and Control, Vol. 
108, Sept. 1986, pp. 163-171. 

[7] Vidyasagar, M., Nonlinear Systems Analysis, 
Prentice-Hall, Inc., 1978. 

[8] Wampler, C. W., "Manipulator Inverse Kinematic 
Solutions Based on Damped Least-Squares So- 
lutions", IEEE Transactions on Systems, Man, 
and Cybernetics, Vol. SMC-16, No. 1, Jan- 
uary/February 1986, pp. 93-101. 



834 




■ .V: 




Figure 1: Modular manipulator in initial configuration 



Figure 4: Pseudoinverse solution for all joints free: 
End-effector position and attitude 




Nout«o U-, 2— . 3-^4-. 5-x. fc*. 7.". 8-a 9-, 10— , tl— .) 




Figure 2: Pseudoinverse solution for all joints free: 
End-effector position and attitude 



NoBtioo{l-.2— . 3«,4— , s™.fa*.7^. «-o.»-. 10—. 11-.) 




Figure 5: Pseudoinverse solution for all joints free: 
Joint angles 




10 15 

Time (sec) 



Figure 3: Pseudoinverse solution for all joints free: 
Joint angles 



Figure 6: Pseudoinverse solution for all joints free: 
End-effector position and attitude 



835 



Notation {1— . 2=-, 3=:, 4=-., 5«x. 6=+. 7=*. 8=0, 9--, 10—-. 11=-.} 



****** 



-^^ 



oh33mm« 



*♦**. 



**H, 



a* 



tn iii KimriK mil HHIIlri 



10 15 
Time (sec) 



20 



25 



1 15 ° 
J 100 

£ 50 



as: i 



- 400 



200 



10 



15 



Time (sec) 



20 



^_^_^ 






- 










5 


10 15 
Time (sec) 


20 


25 










- 



25 



Figure 7: Pseudoinverse solution for all joints free: 
Joint rates 



Figure 10: SR-Inverse solution for 3 failed joints: 
End-effector position and attitude 




o 400 



200 



10 15 

Time (sec) 



i I l 
1 I i 



J li_ 



10 15 

Time (sec) 



20 



25 



Notation {!=-, 2=-, 3=:, 4=-., 5=x, 6=+, 7-*. 8=0, 9=-, 10=-, 11=-.} 



■s 



'*"•""'"»«""""*■»» 



' H"Muuw 



aamt'iiiiiiiiiiiMtwwi 



% 



10 15 

Time (sec) 



20 



25 



Figure 8: Pseudoinverse solution for 3 failed joints: 
End-effector position and attitude 



Figure 11: SR-Inverse solution for 3 failed joints: Joint 
rates 



600 



Notation {1=-, 2=--, 3=:, 4=-., 5=x, 6=+, 7=*, 8=0, 9=-, 10—-. 11=-.} 




10 15 

Time (sec) 




Figure 9: Pseudoinverse solution for 3 failed joints: Figure 12: SR-Inverse solution for 3 failed joints: Scale 

Joint rates Factor 



836