Redundancy Resolution of a 7 DOF Haptic Interface considering Collision and Singularity Avoidance

Transcription

1 28 IEEE/RSJ International Conference on Intelligent Robots and Systems Acropolis Convention Center Nice, France, Sept, 22-26, 28 Redundancy Resolution of a 7 DOF Haptic Interface considering Collision and Singularity Avoidance Yuta Komoguchi and Ken ichi Yano Gifu University Dept. of Human and Information Systems Yanagido 1-1, Gifu Japan k31322@edu.gifu-u.ac.jp, yano-kn@cc.gifu-u.ac.jp Angelika Peer and Martin Buss Technische Universität München Institute of Automatic Control Engineering Theresienstrasse 9, München, Germany {angelika.peer, mb}@tum.de Abstract This paper presents a new redundancy resolution approach based on the gradient projection method, which allows to avoid man-machine and machine-machine collisions as well as singularities of a redundant haptic interface. On this account cost functions for singularity and collision avoidance are formulated. Hereby the collision avoidance cost function is derived from a kinematic analysis of the human arm. Appropriate weighting factors are used to guarantee a simultaneous optimization of all defined side criteria. The performance of the presented method is analyzed by simulation. I. INTRODUCTION In [1] a haptic interface with 7 degrees of freedom (DOF), called VISHARD7, has been developed, see Fig. 1 and 2. To be able to use this device as haptic interface a variety of controllers and inverse kinematics were proposed in the past, see [2]. Since the haptic interface is redundant, no unique redundancy resolution exists. In order to solve this non-uniqueness additional side criteria for collision and singularity avoidance can be formulated without affecting the position and orientation of the end-effector. Since the kinematical design of VISHARD7 implies a singular configuration on the yaw axis, prior research was focused on using redundancy for singularity avoidance only. While the workspace around the yaw axis can be significantly enlarged by using this method, collisions with the human operator and between linkages of the device might occur. In order to guarantee a safe interaction with the haptic interface this problem must be solved. A well known solution to avoid collisions is to detect the impact of the robot with the environment and to reduce the impact force by control. This can be achieved by mounting force sensors not only at the end-effector, but on the whole arm [3], [4], which results in an expensive and complex construction. Kosuge proposes passive motion control to detect collisions. His method is based on the comparison of the actual input torque to the manipulator and the reference input torque, which is calculated by a model of the manipulator dynamics. Using this method impacts can be deducted without sensors, however this method is restricted to collision avoidance only [5]. Kaneko et al. proposed a collision avoidance method using a collision Jacobian, which relates the distance to obstacles to the end-effector velocity [6]. If links are close to obstacles, the end-effector velocity is Fig. 1. VISHARD7 Fig. 2. Mobile haptic interface and human operator modified to avoid collisions. This method however changes the end-effector position and orientation, which should not be affected by the collision avoidance function. Seto et al. [7] finally realized collision avoidance of a 7 DOF anthropomorphic manipulator by setting elastic elements around the linkages. If the linkages approach each other, reaction forces are generated with these elastic elements. However this method focused only on collision avoidance. This work proposes a new redundancy resolution scheme, which is able to simultaneously optimize multiple side criteria. Since in [2] singularity avoidance was solved at the velocity level, the side criterion for collision avoidance is also formulated at this level. Further, in order to control both side criteria properly, they are normalized and multiplied with weighting coefficients. While normalization is necessary to prevent one of the side criteria of being excluded, the weighting coefficients make the redundancy resolution more effective. This paper is structured as follows: The following section II starts with a review of the prior implemented redundancy resolution schemes based on singularity avoidance. Section III deals with the detection of man-machine and machine-machine collisions. Then section IV formulates a side criterion for collisions resulting from translational movements of the end-effector and section V expands the presented method for the optimization of multiple criteria. Finally in section VI simulation results of the proposed method are presented and discussed in section VII /8/$ IEEE. 3513

2 II. PREVIOUS REDUNDANCY RESOLUTION The haptic interface VISHARD7 has 7 DOF and is designed in such a way that translational and rotational motions can be decoupled by using an adequate inverse kinematics. Thus joint four, see Fig. 3, is used to realize this decoupling and to avoid singular configurations in the wrist. In [2] redundancy was solved by using two different kind of inverse kinematics: an inverse function and a partitioned inverse kinematic solution. The former one controls the orientation by using Euler-angles, while the fourth joint was used to decouple translational and rotational motions. But since the 6th joint has a singular configuration at ±π/2 and a certain distance to these configurations has to be maintained, rotations around the yaw axis were limited to ± π/3. To overcome this drawback and to enlarge the rotational workspace of the device a weighted Pseudoinverse control [8] was applied q = J ω +[I J J]k H, (1) where [I J J] projects an arbitrary joint velocity vector k H onto the nullspace of J. Hereby k H can be used to optimize a side criterion H(q), which is a scalar function of the joint angles. In order to avoid singularities one of the manipulability indices m = m(j) reported in [9] can be used as a performance criterion. In [2] another, rather simple performance criterion H was proposed: H = q6 2 πq 6 with k<. (2) It tries to keep the 6th joint fixed to π/2, which is the position farthest away from the wrist singular configuration. It does not only require less peak acceleration of joint 4, but also turned out to be simpler to understand and predict by the operator. On the one hand this algorithm allows the avoidance of singular configurations and extends the rotational workspace around the yaw axis, but on the other hand it causes a couple of man-machine and machinemachine collisions, which are depicted in Fig. 4 and Fig. 5. The former one is a collision between the fourth link and the human operator. The latter one presents a collision between link 1 and links 4, 5 and 6 of the device. III. COLLISION DETECTION As already mentioned in the prior section one of the main reasons for man-machine collisions is the implemented algorithm for redundancy resolution based on singularity avoidance. On this account a new method is proposed which takes into account these possible collisions and allows a simultaneous optimization of two side criteria, namely singularity and obstacle avoidance. Since collisions may not only result from rotational movements, but also translational movements may cause them, an additional side criterion based on the calculation of an escape velocity and the transposed Jacobian is introduced. But in a first step collisions between man-machine and machine-machine have to be detected. On this account the minimum distance between them has to be calculated. At the beginning, the most serious collision between link 1 and 4 is considered. q 1 z N q 7 l 7 q 6 q 2 q 3 q 4 x N Fig. 4. Fig. 5. z E,B l 6 x E,B P EE l 2 l 3 l 4h Fig. 3. Kinematic model of VISHARD7 Man-machine collision (left: overview, right: zoom) Machine-machine collision (left: overview, right: zoom) A. Machine-machine collision detection First of all a potential collision risk has to be detected. This can be realized by analyzing the horizontal position of the end-effector P EE = x 2 EE + y2 EE and taking into account the length of the fourth link l 4h =.33, which is responsible for possible collisions. If P EE > l 4h there is no collision risk. But if P EE <l 4h there is collision risk, and the minimum distance between link 4 and the obstacle has to be calculated. Hereby allows not only collision detection, but also the observation of possible collisions beforehand. Introducing an additional scaling parameter s [...1], which lies on the fourth link, the position of the collision point, see Fig. 6, can be calculated as follows [ ] [ ] xcp l2 C = 2 + l 3 C 23 + sl 4h C 234 (3) y cp l 2 S 2 l 3 S 23 sl 4h S 234 q 5 l 5 l 4v where l 2, l 3 and l 4h are constant link lengths and the fourth link is multiplied with the scaling variable s. Hereby can be determined by taking the norm of this vector. The additional parameter s results from the following 3514

3 geometrical equations: P 1 P EE + s P 4 P 5 = P 1 P cp, (4) P 4 P 5 P 1 P cp =, (5) where P 4 P 5 means a vector in direction of the fourth link and P 1 P cp a vector pointing from the first link to the collision point as shown in Fig. 6. As can be seen P 1 P cp and P 4 P 5 are perpendicular and thus the scalar product of them is zero. Finally given (4) and (5), s can be derived as follows operator s chest and his/her arm θ h is given as follows ( ) P op θ h =cos 1 EE,x, (7) P op P EE where P op P EE is a vector from P op to P EE and P op EE,x is the x-coordinate of the end-effector position expressed in the {op} coordinate system. Thus the position P h, as depicted in Fig. 8, can be determined as follows P h = P EE +(l 4h sin(θ h ),l 4h cos(θ h )). (8) P 1 P EE P 1 P cp (= ) P 4 P 5 P 1 P cp P 1 P EE End-effector Fig. 7. Minimum distance between link 4 and first joint Fig. 6. Minimum distance between link when lies not on link 4 4 and first joint P 1 P EE P 4 P 5 s = P. (6) 4 P 5 2 In the case the control point lies not on link 4 as shown in Fig. 7, the nearest edge is used. B. Man-machine collision detection Man-machine collision detection is very difficult since the collision point is not any more fixed as for machine-machine collision. For a detailed detection of collisions additional sensors, e.g. wearable motion capture sensors, are necessary. However, such advanced tracking systems are typically not available when using a haptic interface. On this account a very simple kinematic model of the human arm is proposed and a side criterion for collision avoidance is formulated by using this model. This is allowed when the following two conditions are satisfied: Firstly the device and the human operator face each other as shown in Fig. 2. Secondly the distance between human operator and platform is kept constant. Both can be assured by control, see [1]. Thus the human operator can be considered to be an almost static object. In the following collisions between the human arm and the tip of link 4 are considered, see Fig. 8. The resulting collision point is then called P h. To determine for man-machine collision analogously to the method presented for machinemachine collision, the collision point is exchanged by P h. To finally calculate the position of P h an additional frame {op}, which is placed at the shoulder of the human arm is introduced, see Fig. 8. Its position in the base frame {N} is assumed to be (.7, ). The angle between the human y N x N P4P5 = l 4h P EE θ h P op P EE P h y op θ h P op x op P op EE,x Fig. 8. Minimum distance between human arm and link 4 Finally given the following geometrical equations P h P EE + s P 4 P 5 = P h P cp, (9) P 4 P 5 P h P cp =, (1) the parameter s for man-machine collision avoidance is as follows P h P EE P 4 P 5 s h = P (11) 4 P 5 2 and using (3) the minimum distance between man and robot link can be determined. Since the first joint is a linear axis and the human grasps the handle of the device always in the same position the height does not play a role in this analysis. IV. SIDE CRITERION FOR COLLISION AVOIDANCE Many studies have been reported on collision/obstacle avoidance for redundant manipulators. A very common approach is based on the optimization of objective functions using the self-motion of the device while not affecting the position of the end-effector. Typical forms for such an objective function employ the minimum distance between manipulator and obstacles: d =min p m p o, (12) where p m and p o are positions of manipulator and obstacle, respectively. Once the minimum distance is obtained, there are several different ways to define objective functions for collision avoidance. The simplest objective function is obtained by setting H(q) = (13) 3515

4 to be maximized with positive k [11], [12]. Or one may also use the squareimum distance as an objective function [13]. Another type of objective function for collision avoidance has been formulated using the reciprocal of the minimum distance H(q) =d 1 min (14) which should be minimized with negative k. From the optimization point of view, it is clear that, for one obstacle case and positive, the maximization of is equivalent to the minimization of the reciprocal of : max (q) min d 1 min (q); for >. (15) However there are several shortcomings of these conventional methods found by Lee [14]. He showed that the methods sometimes cannot make right decisions when multiple obstacles are considered and chattering problems might occur. Thus he proposed an alternative method, which avoids collisions by introducing an escape velocity. Since this escape velocity is mapped into the joint space by using the Jacobian transpose, no calculation of the gradient of the minimum distance function is needed, which results in a computationally simple algorithm. The principle of the algorithm is as follows: Given a collision point, an escape velocity ẋ esc is calculated, which pushes this point towards the border of the forbidden area, see Fig. 9. If the forbidden area is described by a radius d and is given by the collision detection algorithm the escape velocity can be calculated as follows: ẋ esc = 1 d d, (16) whereby the equation is multiplied with 1/d for normalization. If the minimum distance to obstacles becomes small, ẋ esc becomes large and the collision is avoided. Fig. 9. d x robot forbidden area workspace ẋ esc Calculation of escape velocity To control the device, ẋ esc must be mapped from Cartesian into joint space. This is done by multiplying it with the transposed Jacobian J T 42 R 4 2, which is expressed for as follows y l 2 S 2 l 3 S 23 l 4h S 234 l 2 C 2 + l 3 C 23 + l 4h C 234 l 3 S 23 l 4h S 234 l 3 C 23 + l 4h C 234 sl 4h S 234 sl 4h C 234 Thus q esc is given with (17) q esc = J T 42ẋ esc. (18) This joint space escape velocity q esc is used as additional side criterion H CA for collision avoidance, such that collisions resulting from translational movements can be omitted. V. REDUNDANCY RESOLUTION FOR MULTIPLE CRITERIA To optimize all defined criteria simultaneously, normalization and multiplying weighting coefficients is essential as reported in [15]. If one of the side criterion s magnitude is too large, it can be exclusive without normalization. Thus all criteria have to be normalized. On this account (1) can be reformulated by using adequate weighing coefficients C i : n q = J ω +[I J J] kc i H i, (19) i=1 where H i mean the normalized side criteria. The side criterion for collision avoidance H CA calculated by (18) can be used directly without normalization. The side criterion for singularity avoidance H SA can be rewritten as follows H SA = q2 6 πq 6. π (2) Hereby the weighting coefficient C i is a function of the minimum distance C CA ( )= for d M < for d m < <d M, (21) 1 for <d m d m d M d m where d M and d m are upper and lower boundaries of the weighting coefficient given with.15 and.5 m respectively. Given the volume of the linear axis with m(w H D), d m has been set to.5 m supposing W/2 and a bit margin. Since C SA decreases when C CA increases, the following relationship holds C SA =1 C CA, (22) which is shown in Fig. 1 and is mapped into the workspace of the device in Fig. 11. The same relationship also holds for C SA for man-machine collision avoidance. VI. SIMULATION RESULTS Evaluation of the proposed redundancy resolution scheme with multiple criteria is examined by simulation. In a first step the results of the man-machine collision avoidance, then of the machine-machine collision avoidance are presented. As will be shown collisions can be clearly avoided. 3516

5 C i 1 C SA C CA y A d M d m Fig. 11. Weighting coefficients Fig. 1. Relationship of weighting mapped into the workspace of the coefficients device x B A. Man-machine collision To evaluate the implemented algorithms a counterclockwise rotation around the yaw axis of about -115 deg is used as a reference signal for the low level position controller. Fig. 12 and Fig. 13-A and B show the results by using the former (1) and the new proposed redundancy resolution scheme (19). While in Fig. 12-left the rotation about the yaw axis is carried out only by the fourth joint, in Fig. 12-right joint six is used to perform parts of the rotation such that the overall rotation around the yaw axis is given by the sum of sixth and fourth joint. C D elements of yaw rot. [deg] q 4 q 6 sum elements of yaw rot. [deg] q 4-1 q 6 sum Fig. 12. Man-machine collision (left: former redundancy resolutionscheme, right: proposed redundancy resolution scheme) In addition some simulations have been carried out as depicted in Fig. 13-C, D. Fig. 13-C shows a case where the human arm is far away from the robot. In this case H SA is penalized later, since there is a lot of space between human arm and robot. On the other hand Fig. 13-D shows a case where there is not enough space and thus H SA is penalized earlier. B. Machine-machine collisions In machine-machine collision, there are a couple of collisions to be considered: rotational and translational collisions. Former type of collisions occur when rotating around the yaw axis, latter type of collisions occur when the end-effector moves in translational direction even without any rotary motion. 1) Rotational collisions: These collisions are caused by a rotation around the yaw axis of the end-effector. As a benchmark example the first axis is assumed to be located between the sixth and fourth link of the device as depicted in Fig and a rotation of 2 deg about the yaw axis is commanded. The upper part of Fig. 14 shows the result with simultaneous singularity and collision avoidance. While in the beginning (step 1,2) the rotation around the yaw axis is realized mainly by joint 4, joint 6 takes over when the Fig. 13. Man-machine collision simulation (A: with former redundancy resolution, B: with proposed redundancy resolution (θ h = 9), C: with proposed redundancy resolution (θ h > 9), D: with proposed redundancy resolution (θ h < 9) danger of collision increases (step 3,4,5). On the other hand, when only singularity avoidance is considered, as shown in the lower part of Fig. 14, collision arises as depicted in Fig (white arrow). Fig. 14. Machine-machine collision avoidance in case of rotational movement(up: singularity and collision avoidance, low: only singularity avoidance) 2) Translational collisions: This kind of collisions occur when moving the end-effector in translational direction as depicted in Fig. 15 to 18. As an example a translational motion with.1 m/s in y direction is used as reference signal. When passing by the obstacle the weighting coefficient C CA increases, while C SA decreases. As shown in Fig. 17, collisions can be avoided and after having passed the obstacle, the singularity avoidance becomes active again. Fig. 19 shows the behavior of the device during simulation. 3517

6 Gradient of side criteria [deg/s] H_SA H_CA weight function C_SA C_CA minimum distance [m] Fig. 15. H i edundant joint vel.[deg/s] Fig Weighting coefficients Fig. 17. Fig. 18. [deg/s] Redundant joint vel. VII. CONCLUSION In this paper collision detection algorithms for manmachine and machine-machine collisions were presented. For machine-machine collision detection, the minimum distance between the linkages can be calculated exactly due to the fixed kinematics of the device. For man-machine collision detection, the distance between the human operator and robot cannot be determined exactly, but approximated by using a simple model of the human arm. To enable machine-machine collision avoidance, a side criterion has been formulated, which generates an escape velocity. This is mapped into joint space by using the transposed Jacobian and the null space projector. To simultaneously optimize multiple side criteria a normalization of the different criteria was carried out and weighting coefficients were designed. Finally, to examine the proposed method, simulations were performed. The obtained results indicate a good collision avoidance performance. Future work consists in carrying out experiments and evaluating the proposed method using the real hardware setup. ACKNOWLEDGMENTS This work is supported in part by German Research Foundation (DFG) within the collaborative research center SFB453 High-Fidelity Telepresence and Teleaction. REFERENCES [1] A. Peer, Y. Komoguchi, and M. Buss, Towards a mobile haptic interface for bimanual manipulations, in Proceedings of the IEEE Int. Conf. on Intelligent Robots and Systems, 27. [2] Y. Komoguchi, A. Peer, and M. Buss, Control and performance evaluation of a new redundant haptic interface, in Proceedings of the Society of Instrument and Control Engineers (SICE) Annual Conference, 27. [3] S. Iwata, H. Sugano, whole-body haptic interface for human symbiotic robots, IEEE Inter. Conf. on Robotics and Automation, vol. 2, no. 5, pp. pp.81 87, 22. [4] D. Gandhi and E. Cervera, Sensor covering of a robot arm for collision avoidance, in IEEE International Conference on Systems, Man and Cybernetics, vol. 5, 23, pp [5] K. K. Morinaga. S, Whole body compliant motion control system based on collision detection algorithm, in JSME Conference on Robotics and Mechatronics, 24, pp. 2P 1 L2 26. Fig. 19. Machine-machine collision avoidance in case of translational movement [6] H. Kaneko, T. Arai, K. Inoue, and Y. Mae, Real-time obstacle avoidance for robot arm using collision jacobian, in International Conference Intelligent Robots and Systems, 1999, pp [7] F. Seto, K. Kosuge, R. Suda, and Y. Hirata, Real-time control of self-collision avoidance for mobile manipulator using robe, in Japan Society of Mechanical Engnieering, Robotics and Mechatronics Conference 23, 23. [8] A. Liegeois, Automatic supervisory control of the configurationand behavior of multibody mechanisms, IEEE Transactions on Systems, Man, and Cybernetics, vol. 7, no. 12, pp , [9] M. Ueberle, N. Mock, and M. Buss, Design, control, and evaluation of a hyper-redundant haptic interface, in Advances in Telerobotics: Human Interfaces, Control, and Applications, R. Aracil, C. Balaguer, M. Buss, M. Ferre, and C. Melchiorri, Eds. Springer, STAR series, 27. [1] A. Peer, T. Schauß, U. Unterhinninghofen, and M. Buss, A Mobile Haptic Interface for Bimanual Manipulations in Extended Remote/Virtual Environments, in Robotics Research Trends. Nova Publishers, 28. [11] Z. Y. Guo and T. C. Hsia, Joint trajectory generation for redundant robots in an environmentwith obstacles, IEEE Inter. Conf. on Robotics and Automation, pp , 199. [12] D. Wang and Y. Hamam, Optimal trajectory planning of manipulators with collision detection and avoidance, Int. J. Robotics Research, vol. 11, no. 5, pp , [13] J. Chen, J. Liu, W. Lee, and T. Liang, On-line multi-criteria based collision-free posture generation of redundant manipulator in constrained workspace, Robotica, vol. 2, pp , 22. [14] K. Lee and M. Buss, Multiple obstacles avoidance for kinematically redundant manipulators using jacobian transpose method, in Proceedings of the Society of Instrument and Control Engineers (SICE) Annual Conference, 27. [15], Redundancy resolution with multiple criteria, in Proceedings of the International Conference of Intelligent Robots and Systems,