空间机器人运动学分析

An Analytical Algorithm with Minimum Joint Velocity Jump for Redundant Robots in the Presence of Locked-Joint Failures Zhao Jing, and Li Qian Abstract— The joint velocity jump for redundant robots in the presence of locked-joint failures is discussed in this paper. First, the analytical formula of the optimal joint velocity with minimum jump is derived, and its specific expressions for both all joint failure and certain single joint failure are presented. Then, the jump difference between the minimum jump solution and the least-norm velocity solution is mathematically analyzed, and the influence factors on this difference are also discussed. Based on this formula, a new fault tolerant algorithm with the minimum jump is proposed. Finally simulation examples are implemented with a planar 3R robot and a 4R spatial robot, and an experimental study is also done. Study results indicate that the new algorithm proposed in this paper is well suited for real time implementation, and can further reduce the joint velocity jump thereby improving the motion stability of redundant robots in fault tolerant operations. Also, the fewer the possible failed joints are, the more obvious the effect of this new algorithm is. I. INTRODUCTION HEN robots perform a task in harsh and/or remote environments, they are subject to actuator and sensor failures. For these cases, it is quite difficult or impossible for us to repair the failures on the spot without delays. Therefore, the tolerance to failures is essential for the robots to carry out such tasks as space exploration [1], underwater exploration [2] and hazardous material disposal [3]. The so-called fault tolerance means that a robot can still continue the desired tasks in the presence of failures. The failure modes of a robot include locked-joint, free-swinging joint and following-motion joint [4], where the locked-joint is one of the most common modes. The locked-joint failure will be discussed in this article. This failure covers two cases: one is active locking, where a joint can be locked by fail-safe brakes when a robot is capable of detecting its failure in advance; the other is passive locking, where a joint is locked unexpectedly due to mechanical failures. Since redundant robots have “extra” degree-of-freedom (DOF) which can compensate the motions of failed joints to continue the desired tasks after failures, they are often used in fault tolerant operations [5]. When any joint of a redundant robot with one redundancy fails and is locked, the robot will degrade to a new robot with different structural parameters that is called reduced robot here. Obviously, the performance of the robot will inevitably degrade. The evaluation on the performance of a reduced robot is one of significant problems. The minimum singular value of the reduced Jacobian matrix and the reduced manipulability of the robot are two of common fault tolerant indexes, which can be used to evaluate the dexterity of the reduced robot at the instant of locking failed joint [5]-[7]. The volume of the fault tolerant workspace and the centrality index that describes the positional relation between the fault tolerant workspace and the operational task are the other two fault tolerant indexes, which are often used to quantify the dexterity of the reduced robot during post-failure operations [8]-[10]. Besides the dexterity of reduced robots, the joint velocity jump at the instant of locking failed joint is another critical issue that should have been studied [11]. At this instant the reduced robot will replace the healthy robot to continue the desired task. To exactly follow the desired trajectory of the end-effector, the velocities and torques of surviving joints in the reduced robot will produce a jump, which inevitably reduces the accuracy of the end-effector trajectory. This problem was addressed to some extent. Reference [12] proposed a fault tolerant control algorithm based on the minimum singular value of the reduced Jacobian matrix. This algorithm can avoid the singularity of the reduced robot to ease the joint velocity jump. Nevertheless, research results show that the jump inherently results from the structural difference between the healthy robot and the reduced robot, and so it occurs even if the reduced robot is not in singular configurations [13]. The support of this work by National Natural Science Foundation of China (50775002) and Funding Project for Academic Human Resources Development in Institutions in Higher Learning under the Jurisdiction of Beijing Municipality is greatly appreciated. Zhao Jing and Li Qian are with the College of Mechanical Engineering and Applied Electronics Technology, Beijing University of Technology, Beijing 100022, China (e-mail: zhaojing@bjut.edu.cn; leeqian020112@yahoo.com). A fault tolerant algorithm with minimum joint velocity jump was proposed [13], [14]. However, its computation is heavy, and not suitable for real time control. The remainder of this paper is organized as follows. Section II defines the joint velocity jump. In section III, the analytical formula of the optimal joint velocity with minimum jump is derived, and its specific expressions for both all joint failure and certain single joint failure are presented. Besides some problems related to this formula including a comparison with the least-norm velocity solution, singularity and a new fault tolerant planning algorithm are also discussed in this section. Simulation examples of a planar 3R robot and a spatial 4R robot are presented in Section IV. An experimental study is done in section V. Finally, the conclusions are given in Section VI. II. DEFINITION OF THE JOINT VELOCITY JUMP Assuming that a robot has n DOF and m absolute parameters of the end-effector. When one of robot’s joints fails and is locked, the robot will change into a reduced robot W 2008 IEEE International Conference on Robotics and Automation Pasadena, CA, USA, May 19-23, 2008 978-1-4244-1647-9/08/$25.00 ©2008 IEEE. 1987 and the velocity of surviving joints will also be redistributed due to the failed and locked joint. In this way, the difference of joint velocity between the healthy robot and the reduced robot may occur. The difference is defined as joint velocity jump (JVJ) and formulated as follows [13]. jj i j i θθλ && −= (1) where j is the joint velocity of the healthy robot, and j i is the joint velocity of the reduced robot. Subscript “j” ranges from 1 to n, which denotes all joints; superscript “i” denotes the failed joints. When , θ& θ& ij = jji θλ &= since the failed joint is locked, i.e. . It is evident that the JVJ depends on not only the joint velocity of the healthy robot but also that of the reduced robot. The smaller j i is, the smaller the JVJ is, and namely, the failed joint has smaller influence on the kinematical properties of a robot. This means that the operational accuracy of the robot at the instant of failures is higher. Researches show that the excessive JVJ will result in vibration and jerk to a robot, which does reduce its operational accuracy at the instant of failures [13],[14]. 0=jiθ& λ In (1), j i is decided by the reduced Jacobian matrix, which is given by the original Jacobian matrix with its i-th column removed. Thus, the reduced Jacobian matrix can be expressed: . In the case of redundant robots with one redundancy, the can be calculated by θ& J [ n1i1-i1 ,,,,, jjjjJ i LL += ] j iθ& XJ i i && 1−=θ (2) where 1×∈ mRX& is the velocity vector of the end-effector, and is the inverse of the reduced Jacobian. )1(1 −×− ∈ nmi RJ III. ANALYTICAL FORMULA OF THE JOINT VELOCITY AND RELATED PROBLEMS A. Derivation of the Formula Equation (1) indicates that the JVJ depends on the difference of joint velocity between the healthy robot and the reduced robot. For the redundant robot with one redundancy, when any joint of the robot fails and is locked, the joint velocities of the reduced robot are uniquely determined by the desired trajectory of the end-effector. For this reason, one can optimize the joint velocities of the healthy robot through the joint self-motion to make it get close to that of the reduced robots as much as possible. This optimal motion planning can be stated as: on condition that the end-effector’s motion is satisfied, optimize the joint velocities of the redundant robot to minimize the square sum of the JVJ. Assuming that any redundant joint of a robot is possible to fail, the optimal motion planning can be formulated as Min )()( 2 1Z 'T' 1 θθθθ &&&& −−= ∑ = ii n i (3) Subject to (4) θ&& JX = By solving this optimization problem, a general analytical formula of the joint velocity that minimizes the JVJ of the redundant robot can be obtained as '( )( n i i 1 1 n )θ θ+ = = + − ∑& &&+J X I J J (5) where is the Moore-Penrose generalized inverse of , mnRJJJJ ×−+ ∈= 1TT )( J I nnR ×∈ is identity matrix, and “n” is the numbers of possible failed joints. i-1 1 i 1 n 'θ&i i i i T[ , , ,0, , , ]iθ θ θ θ= & & & &L L 1×∈ nR+ is the extended joint velocity vector of the reduced robot, whose non-zero components can be calculated by (2). Equation (5) shows that the optimal joint velocity with minimum JVJ can be written in the standard pattern of the gradient projection algorithm [15]. The specific expression of the sum of the extended joint velocity vector depends on the numbers of possible failed joints. For a robot with one degree of redundancy, if all joints are possible to fail, that is, on the condition that all single joint failure is considered, the sum of the extended joint velocity vector of the reduced robot can be calculated by n i i 1= ∑ &'θ ⎥⎥ ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎢ ⎣ ⎡ ++ ⎥⎥ ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎢ ⎣ ⎡ + ⎥⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎣ ⎡ = 0 0 0 2 1 2 1 2 1 2 1 M & & K & M & & M & θ θ θ θ θ θ n n nn (6) If only joint k is possible to fail and is locked, (6) then becomes n i i 1= ∑ &'θ Ti-[ , , ,0, , , ]k k k k1 1 i 1 nθ θ θ θ+= & & & &L L (7) In this case, assuming that joint “n” is locked, an easy derivation will arrive at [ ] ⎥⎦ ⎤⎢⎣ ⎡=⎥⎦ ⎤⎢⎣ ⎡−⎥⎦ ⎤⎢⎣ ⎡+= −− + − + 0 0 0 111 XJXJjJJXJXJ nnnnn &&&&&θ (8) This result indicates that, on the condition that only one of joints is possible to fail and is locked, to minimize the JVJ, the joint should be locked in advance. In this way, the joint velocities of the original robot are equal to those of the reduced robot, and so the JVJ keeps zero throughout. Obviously, this is only an extreme case. For most cases, all the joints, at least some of joints, are possible to fail. Hence, the joint velocity is usually not equal to zero. B. Comparison with the Least-norm Velocity Solution The least-norm velocity solution can operate a robot with the minimum joint velocity in square sum sense. Generally, the larger the velocity of the failed joint is, the larger the JVJ of the robot is. This implies that except the optimal joint velocity solution with minimum JVJ based on (5), the least-norm velocity solution is better than other solutions with respect to the JVJ index. It is therefore a natural question: What is the difference of the JVJ between the least-norm solution and the minimum JVJ solution? And, what are the key influence factors on the difference? They will be answered in this section. When a redundant robot operates in terms of the least-norm solution and the minimum JVJ solution respectively, 1988 assuming that all single joint failure is considered, their square sum of the JVJ can be respectively expressed as =LNλ ∑ = −n i LN i 1 2' θθ && (9) =OPTλ ∑ = −n i OPT i 1 2' θθ && (10) where • is 2 norm of joint velocity vector, LN is least-norm joint velocity solution, and is optimal solution with minimum JVJ. &θ OPTθ& From (6), the minimum JVJ solution can be written HLNOPT θθθ &&& += (11) where is a homogenous solution belonging to the null space, and orthogonal to . Hθ& LNθ& Expanding (11) and collecting terms lead to )(2 n 111 1 1 22 ∑∑∑ ∑∑ === = = −+= n i j i n j LN n j n j n i j i LNLN jj θθθθλ &&&& (12) In similar manner, substituting (11) into (10), and noting the orthogonality of and , we obtain Hθ& LNθ& ∑ ∑ ∑∑∑ ∑ = = = == = − +−= n j n i j i LN n j n i j i n j n j HLNOPT j jj 1 1 1 1 2 1 1 22 )(2 )(n θθ θθθλ && &&& (13) Thus, the difference δ in the square sum of the JVJ between the two solutions can be expressed as 2 1 n j n LN OPT H j δ λ λ θ = = − = ∑ & (14) Evidently, (14) is always greater than zero. This shows that in the case of the jump index the minimum JVJ solution does outperform the least-norm solution; and the difference depends on the homogenous solution of the joint velocity for a redundant robot and its numbers of possible failed joints. Generally, the fewer the possible failed joints are, the greater the difference is (noting that consists of 1/n). H It is worth explaining that the least-norm solution and the minimum JVJ solution all belong to a local solution and the comparison of joint velocities between the two solutions is also based on a specific joint configuration. Therefore, above conclusion is exactly true only when the two solutions have the same joint configuration. Nevertheless, the end-effector of a robot is often required to follow a continuous trajectory. At this time, if the two solutions are respectively used to generate a family of joint trajectories, even at the same instant, because the robot’s joint configuration is different, (14) is not satisfied, i.e. θ& δ is not always greater than zero. C. Singularity of A Reduced Robot When one of robot’s joints fails and is locked, the robot will change to a reduced robot with fewer DOF, and its D-H (Denavit-Hartenberg) parameters will also vary with the position where the failed joint is locked. Since robot failures may occur at random anywhere over its entire range of motion, all the reduced robots at any instant of possible failures must be non-singular and more dexterous. The minimum singular value and the reduced manipulability based on the reduced Jacobian matrix are commonly used to quantify the dexterity of a redundant robot at the instant of failures [5], [6]. These indexes have a dimension relative to length unit. To obtain a dimensionless index, the condition number of a robot can be introduced [16]. Therefore, we now define the reduced condition number of the Jacobian (RCN) as a new fault tolerant index, and sign it with ik 1 /i ik riσ σ= (15) where, i1σ and riσ are the maximum and minimum singular values of the reduced Jacobian matrix iJ respectively. Obviously, ∞<≤ ik1 . The smaller the i is, the stronger the dexterity of the reduced robot with locked joint “i” is, which means the robot has stronger fault tolerant ability at the instant of failures. When i =1, the reduced robot is isotropic and the most dexterous. If the fault tolerance of more than one joint is considered, the square sum of the dexterity for every joint can be taken, which is consistent with the JVJ index discussed above. Using the gradient of the RCN to replace the sum of the extended joint velocity vector in (5), we can obtain a fault tolerant algorithm with minimum dexterity. k k In most cases, although (5) can minimize the JVJ, it can not guarantee the reduced robot to avoid singular configurations. For a given task, the joint trajectories of a robot are related to the initial postures of its end-effector. Different initial postures correspond to different solution domains of the joint motion. Therefore, properly choosing the initial postures can avoid the singular configurations of a robot while minimizing the JVJ. D. A Fault Tolerant Planning Algorithm with the JVJ The calculation procedures of this algorithm are as follows: (i) Arbitrarily choose the initial position of the end-effector, and let the end-effector’s velocity be zero. Determine the optimal initial configuration with minimum RCN through the joint self-motion in the null space [17]. (ii) Determine the joint trajectories under this initial condition, and calculate the RCN at any instant. (iii) Compare the calculated RCN with a specified threshold value. If the RCN is greater than the threshold value, the calculation ends; otherwise, return to step 1 until the requirement is satisfied. It is indicated that when (5) is adopted to minimize the JVJ in the fault tolerant motion planning, the singular configurations can be avoided by adjusting the end-effector’s initial position. Since our main interests in this paper are the JVJ problem of a robot but not its singularity problem, and the approach to choose the initial position to avoid the singularity of the robot was discussed in [13], this new algorithm does not include how to determine an optimal initial position for the robot. IV. SIMULATION EXAMPLES A. Planar 3R Robot Fig. 1 is a planar 3R robot. For positional tasks it is a redundant robot with one degree of redundancy. The three 1989 algorithms including the JVJ algorithm (JVJA), the least norm velocity algorithm (LNVA) and the maximum dexterity algorithm (MDA) will be respectively implemented with this robot for the following two cases: all joint failures and two joint failures. And the comparisons of these algorithms on the JVJ and the RCN indexes will also be made. The simulation conditions are as follows. The three links of the robot are identical and each is 0.5 m long. Assume the velocity of its end-effector , simulation time t=1.0 s and calculation time dt=0.001 s. m/s t t T)]2cos(2.0),2sin(2.0[ ππππ=X& 1) All Joint Fault Tolerance: Fig. 2(a) and (b) are the simulation results obtained respectively by the JVJA, the LNVA and the MDA, where the initial position of the end-effector , and the two indexes, m ]0,35.0[ T=X λ and k are in their square sum sense. It is seen that on the joint velocity jump the JVJA is the best while the MDA is the poorest; on the dexterity, it is quite reverse. The two indexes of the LNVA are in the middle of the three algorithms. It is noted that the solutions may cross in quality at a certain point: for instance, the MDA solution in Fig. 2(b) grows over the ones with JVJA and LNVA at t=0.85. This is due to the local characteristics of these algorithms as motioned above. Next, we change the initial position of the end-effector and let . Similar simulation results are presented in Fig. 3(a) and (b). It is shown that the initial positions of the end-effector affect the absolute values of the JVJ and the RCN of these algorithms, but almost do not affect the relative values of the two indexes. Generally, the two indexes of these algorithms conflict with each other, i.e., adjusting the initial position of the end-effector can improve one index, meanwhile it will worse the other. In addition, for the all joint fault tolerance of a robot, no matter where the initial position is, the LNVA is always close to the JVJA on the jump index, and the closing extent is related to the D-H parameters of the robot. m ]0,4.0[ T=X 2) Two Joint Fault Tolerance: If only joints 2 and 3 are possible to fail, and the initial position of the end-effector is taken as , we can obtain the simulation results shown in Fig. 4(a) and (b). These results indicate for the jump index the optimal effect of the JVJA in this case is more obvious than in all joint fault tolerance; but for the RCN, there is no big difference among the th