# 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