IEEE/RSJ Intern. Conf. on Intelligent Robots Systems IROS 23), USA, Oct. 27-3, 23 Human-like Motion of a Humanoid Robot Arm Based on a Closed-Form Solution of the Inverse Kinematics Problem T. Asfour R. Dillmann Industrial Applications of Informatics Microsystems IAIM) Computer Science Department, University of Karlsruhe Haid-und-Neu-Str. 7, 763 Karlsruhe Germany Abstract Humanoid robotics is a new challenging field. To cooperate with human beings, humanoid robots not only have to feature human-like form structure but, more importantly, they must possess human-like characteristics regarding motion, communication intelligence. In this paper, we propose an algorithm for solving the inverse kinematics problem associated with the redundant robot arm of the humanoid robot ARMAR. The formulation of the problem is based on the decomposition of the workspace of the arm on the analytical description of the redundancy of the arm. The solution obtained is characterized by its accuracy low cost of computation. The algorithm is enhanced in order to generate human-like manipulation motions from object trajectories. I. INTRODUCTION From the mechanical point of view, a robot arm imitating the human arm motions should be kinematically redundant like the human arm. According to [], the human arm mechanism is composed of seven degrees of freedom DOF) from the shoulder to the wrist. Because there is kinematic redundancy, an infinite number of joint angle trajectories leads to the same end-effector trajectory. Kinematics redundancy can be used to avoid joint limits [2], obstacles [3], singular configurations [4], as well as to provide a fault tolerant operation [5] or to optimize the robot arm dynamics [6]. Stard methods that deal with redundancy can be divided in two global local methodes [7]. The former require prior information about the entire Cartesian space trajectory of the endeffector to generate, usually iteratively, a set of joint angle trajectories with global optimality. Therefore, global control schemes are computationally epensive not suitable for real-time implementation. On the other h, local methods use only the instantaneous position of the end-effector to perform local optimization in real time. Therefore, local control schemes are required in case of cooperation tasks between humans humanoid robots. The use of redundancy for the generation of humanlike robot arm motions has been already proposed in the literature a variety of hypothetical cost functions has been suggested to eplain principles of the human arm movements [8]. In [9], for instance, the kinematic redundancy is used to achieve human-like joint motions Fig.. The humanoid robot ARMAR of a robot arm during the writing task. Recently, human motion capture has been used for the generation of kinematics models describing human humanoid robot motions []. This method has been successfully applied to the generation of human-like motions of our humanoid robot ARMAR []. However, the applicability of this method to generate human-like manipulation motions is not yet clear from the literature. In this paper we consider the problem of generating human-like motions from the kinematics point of view. We rely on a hypothesis from neurophysiology present its on-line application for generating human-like motions of a redundant humanoid robot arm. The paper is organized as follows. Section II describes the kinematics of the 7- DOF arm of the humanoid robot ARMAR. The description of the arm redundancy the algorithm for solving the inverse kinematics problem is given in section III. Section IV presents the approach we use to generate human-like motions. Finally section V summarizes the results concludes the paper.
II. THE KINEMATICS MODEL OF THE ARM Figure 2 shows the reference link coordinate systems of the 7-DOF arm using the Denavit-Hartenberg convention. The values of the kinematic parameters are listed in table I, where l s, l f are the link lengths of shoulder, upperarm forearm, respectively. They correspond to the home position pictured in the link coordinate diagram of figure 2. The Denavit-Hartenberg convention z 3 z z y y 3 l s l o 2 y 2 z 2 Fig. 2. 4 y 4 y 3 z 4 z 5 5 y 5 l h y 6 TCP Coordinate systems of the arm z 6 6 z 7 o) y 7 a) 7 n) allows the construction of the forward kinematics function by composing the coordinate transformations into one homogeneous transformation matri. The description of the coordinate transformation between frame i frame i is given by the homogeneous transformation matri c θi c αi s θi s αi s θi a i c θi A i = s θi c αi c θi s αi c θi a i s θi s αi c αi d i, ) where s ηi c ηi are sinη i cosη i, respectively. Having defined a frame for each link, the coordinate transformation describing the position orientation of the end-effector with respect to the base frame is given by T tcp θ) = 7 i= Ai θ i ) ) = TABLE I n o a p DENAVIT-HARTENBERG PARAMETERS OF THE ARM i θ i α i a i [mm] d i [mm] range θ 9 l s 9 9 2 9 9 9 9 3 θ 3 + 9 9 23 9 4 θ 4 9 45 5 θ 5 9 l f 35 6 θ 6 + 9 9 45 45 7 θ 7 9 l h 45 45 ), 2) where θ is the 7 ) vector of the joint variables, n, o a are the unit vectors of the frame attached to the end-effector, p is the position vector of the origin of this frame with respect to the origin of the reference frame,y,z ) figure 2). III. THE REDUNDANCY OF THE ARM The robot arm redundancy can be described by the rotation of the center of the elbow joint about the ais, that passes through the wrist the origin of the base frame,y,z ). The feasible positions of the elbow around this ais are defined by a curve. This curve can be derived from the fact that the ending point of the upper arm describes an ellipsoid centered in the origin of the base coordinate system,y,z ) that the starting point of the forearm describes a sphere centered on the wrist w,y w,z w ), which corresponds to the origin of the coordinate system 6,y 6,z 6 ) in figure 2. Since these two points have to coincide, the redundancy curve results from the intersection of the ellipsoid the sphere. Figure 3 shows the ellipsoid, which is given by l s + l o ) 2 + y l s + l o ) 2 + z l o ) 2 =. 3) the sphere with the radius l f whose center is given by the position vector w of the wrist w = w,y w,z w ) T = p l h n, 4) where p is the position vector of the end-effector, l h is the distance between the wrist n the normal vector of the frame attached to the end-effector figure 2). For a given position orientation of the end-effector all possible positions of the elbow e,y e,z e ) can be determined based on the derived description of the arm redundancy. Once having the elbow position orientation both the origin the unit vectors of the coordinate system 4 )), the remaining joint angles are then easy to determine. Fig. 3. z y l s l f l h y 6 z 6 6 Workspace of the upperarm forearm TCP z 7 o) y 7 a) 7 n)
The description of the complete curve resulting from the intersection of the ellipsoid the sphere mentioned above is analytically difficult. To make this task mathematically tractable, we first determine the intersection between the ellipsoid the sphere at z = z e. The redundancy curve of the arm can eventually be obtained by combining all such intersections for all values of z e in the interval [z min e,z ma e ], where z min e z ma e are the minimal maimal values of the z-coordinate of the elbow for a given position orientation of the end-effector. A. Determination of the elbow position orientation Let z e [z min e e ] be the z-coordinate of an elbow position for a given position orientation of the endeffector. The intersection of the ellipsoid with the plane z = z e results in a circle. The radius r the center M of this circle figure 4) are given by,z ma r = l s + l 2 u z 2 e 5) M =,,z e ) T 6) The intersection of the sphere with the plane z = z e results in a circle. The radius r 2 the center M 2 of this circle figure 5) are given by r 2 = l s + l 2 f dz2 e, dz e = z e z w 7) M 2 = w,y w,z e ) T 8) The two points e e 2 provided by the intersection of the circles represent two possible positions of the elbow. In addition, the unit vectors of the frame 4 ) attached to the elbow must be established. The vector z 4 lies along the forearm points to the wrist. The vector y 4 is perpendicular to the upperarm r u ) forearm r f ). The vector 4 completes the right-hed coordinate frame. Fig. 4. z y M r s l s l o r r o r u l h r h TCP z e dz e The Ellipsoid the plane z = z e z = z e z Fig. 5. y rs l s M 2 r 2 l o r o ze z = z e r u l h r TCP h dz e The Sphere the plane z = z e The above criteria can be formally epressed as: z 4 = r u r u y 4 = z 4 r o z 4 r o 4 = y 4 z 4 The vectors r u r o can be epressed using the vector w from equation 4), the elbow position vectors e,2 the position vector of the shoulder r s : z 4 = w e i w e i 9) y 4 = z 4 r s e i ) i =,2 ) z 4 r s e i ) 4 = y 4 z 4 ) Having determined the position the unit vectors of the frame attached to the elbow, the two possible positions of the elbow can be epressed by the homogeneous transformation matri ) T elbow = 4 y 4 z 4 e i i =,2 2) which provides the position orientation of the 4 ) with respect to the base frame. In order to get z min e z ma e, we consider the necessary but not sufficient condition, which a point of intersection of the circles mentioned above must satisfy 2 w + y 2 w r + r 2 3) substitute r from equation 5) r 2 from equation 7) in equation 3) solve for z.
B. Solution of the inverse kinematics problem The task of solving the inverse kinematics problem results in the determination of the arm joint angles for a given position orientation of the end effector. The relationship between the elbow the reference base frame is given by equation 2). Furthermore, this relationship can be epressed as T elbow = A A 2 A 3 A 4 4) n o a p n y o y a y p y = n z o z a z p z Note that the epression of the position orientation of the frame 4 ) depends only on the first four joint variables θ,,θ 3, which are responsible for the position of the wrist. The position orientation of the end-effector relative to the elbow is given by T = A 4 A 3 A 2 A T elbow 5) = A 5 A 6 A 7 = o a p y o y a y p y z o z a z p z Hence, the inverse kinematics problem can be decomposed into two smaller dimension subproblems: the inverse kinematics problem given by equation 4) the inverse kinematics problem given by equation 5). The solution starts with the matri equation A T } {{ elbow = A } 2 A 3 A 4 6) } {{ } L R To simplify the notation s i c i are sinθ i cosθ i, respectively. The notation Wi, j) denotes the element of the ith row jth column of the matri W. The equation L 3,4) = R 3,4) of the elements of the matrices L R can be used to solve for θ. This yields: θ,2 = atan2 ± p y, ± p ) 7) The equations L,4) = R,4) L 2,4) = R 2,4) of the matrices L R provide the solution for : = atan2 p z, c p + s p y l s) 8) The solution for the joint angles θ 3 can be obtained from the following matri equation: A 2 A T } {{ elbow } = A 3 A } {{ } 4 9) L 2 R 2 The equations L 2,2) = R 2,2) L 2 2,2) = R 2 2,2) of the elements of the matrices L 2 R 2 provide θ 3 as follows: θ 3 = atan2 s o + c o y, s 2 c o s 2 s o y c 2 o z ) 2) The equations L 2 3,) = R 2 3,) L 2 3,3) = R 2 3,3) of the elements of the matrices L 2 R 2 provide θ 4 as follows: θ 4 = atan2 c 2 c n + c 2 s n y s 2 n z, c 2 c a + c 2 s a y s 2 a z 2) Since the joint angles θ,,θ 3 are already known, closed-form epressions for the joint variables θ 5, θ 6 θ 7 can be obtained from the matri equation: A 5 T } {{ } = A 6 A } {{ } 7 22) L 3 R 3 The equation L 3 3,2) = R3,2) of the elements of the matrices L 3 R 3 can be used to solve for θ 5. This yields: θ 5,2 = atan2 ±o y, ±o ) 23) The equations L 3,2) = R 3,2) L 3 2,2) = R 3 2,2) of the elements of the matrices L 3 R 3 provide θ 6 as follows: θ 6 = atan2 o z, c 5 o s 5 o y) 24) Solving the equations L 3 3,) = R 3 3,) L 3 3,3) = R 3 3,3) for the joint angle θ 7 then yields: θ 7 = atan2 s 5 + c 5 y, s 5 a c 5 a y ) 25) In the follwoing section we describe how to select a human-like arm configuration out of the infinite number of distinct possible ones. IV. HUMAN-LIKE ROBOT ARM MOTION The underlying idea for the generation of human-like robot arm motions is based on the results in [2] [3]. Soechting Flers have shown that arm movements are planned in shoulder-centered spherical coordinates suggest a sensorimotor transformation model that maps the wrist position on a natural arm posture using a set of representation parameters q u e,q f e,qu y q f y, which are the upperarm elevation, the forearm elevation, the upperarm yaw the forearm yaw, respectively figure 7). The relation between these parameters the spherical coordinates of the wrist position r,φ χ is given by q u e qe f q u y qy f = 4. +. r +.9 φ = 39.4 +.54 r.6 φ = 3.2 +.86 χ +. φ =. +.8 χ.35 φ 26)
z z y Anterior χ q f y q u y Lateral q f y θ q u y y φ Base θ 3 q u e r q u e l u q f e l f q f e θ 4 Fig. 6. Parameters for the human arm posture Fig. 7. Parameters for the humanoid arm posture. Once these parameters are obtained for a given position of the wrist frame, they are mapped on the joint angles of the shoulder elbow θ,, θ 3, of the robot arm. Note that the position of the wrist is completely determined by these angles. The joint angles θ depend only on the upperarm elevation q o e forearm yaw qe f. θ = qu y 27) = π q u e arccos l s cosq u e) ) 28) The joint angle θ 4 can be determined from the scalar product of l f. This yields: θ 4 < lu, l f > ) = ±arccos 29) l f = ±arccos cos ) sinq u y) sinqe f ) sinq y f )+ cos ) cosq u y ) sinq f e ) cosq f y ) sin ) cosq f e ) ) The difference z e z w ) of the wrist elbow z- coordinate can be determined using the direct kinematics the arm representation parameters. Thus, the joint angle θ 3 can be computed as follows: θ 3 sinθ2 ) cosθ = arcsin 4 ) + cosqe f ) ) cos ) sinθ 4 ) 3) Since the sensorimotor transformation model is only an approimation, the wrist position given by the determined angles θ,, θ 3 does not eactly concide with the target one. However, the eact position orientation of the end-effector can be reached by applying the inverse kinematics algorithm presented in section III-B, which allows calculating the remaining joint angles θ 5, θ 6 θ 7. In case one of these angles violates its limit, we correct this by changing iteratively the elbow position until either a feasible solution is found or all possible positions of the elbow in the interval [z min e,z ma e ] are ehausted. V. RESULTS AND CONCLUSIONS The generation of human-like manipulation motions has been implemented tested successfully for the 7-DOF arm of the humanoid robot ARMAR. The result is a real-time control scheme that eliminates a major part of unnatural arm configurations during the eecution of manipulation tasks. Figure 8 shows an eample of the desired dushed line) generated solid line) position trajectories in the Cartesian space during the eecution of a manipulation task operation. The deviations are compensated through the inverse kinematics algorithm with minimal modifications of the joint angle trajectories determined according to section IV. The presented approach does not consider the dynamics of the robot arm. This would be necessary to generate realistic velocity distribution for manipulation motions. A limitation of the proposed way to generate human-like motions is the simple kinematic structure of the robot arm. In the case of comple manipulation tasks, the eact position orientation of the h can be obtained only through significantly large modifications of the joints θ,, θ 3. However, the resulting configuration ist not guaranteed to be human-like. Therefore, a humanoid robot arm with a higher degree of redundancy is envisaged.
4 [mm] 2 y [mm] -2-2 5 5 2-4 5 5 2 z [mm] 2-2 Error [mm] 3 5 Δ Δy Δz -4 5 5 2-5 5 5 2 Fig. 8. Time history of the position trajectories. VI. ACKNOWLEDGMENTS This work has been performed in the framework of the german humanoid robotics program SFB 588 funded by the Deutsche Forschungsgemeinschaft DFG). VII. REFERENCES [] M. Benati, S. Gaglio, P. Morasso, V. Tagliassco, R. Zaccaria. Anthropomorphic Robotics: I. Representing Mechanical Compleity. Biological Cybernetics, 38:25 4, 98. [2] A. Ligeois. Automatic Supervisory Control of the Configuration Behavior of Multibody Mechanism. IEEE Transaction on Systems, Man Cybernetics, 977. [3] A.A. Maciejewski C.A. Klein. Obstacle Avoidance for Kinematically Redundant Manipulators in Dynamically Varying Environments. The International Journal of Robotics Research, 43):9 7, 985. [4] Y. Nakamura H. Hanafusa. Inverse Kinematics Solution with Singularity Robustness for Robot Manipulator Control. ASME Journal of Mechanisms, Transmissions, Automation, Design, 8:63 7, 986. [5] K.N. Groom, A.A. Maciejewski, V. Balakrishnan. Real-Time Failure-Tolerant Control of Kinematically Redundant Manipulators. The International Conference on Robotics Automation, pages 2595 26, 997. [6] J.M. Hollerbach K.C. Suh. Redundancy Resolution of Manipulators through Torque Optimization. In IEEE International Conference on Robotics Automation ICRA), volume 4, pages 38 36, 987. [7] Y. Nakamura. Advanced Robotics: Redundancy Optimization. Addison Wesley Publishing, 99. [8] H. Cruse, E. Wischmeyer, M. Brüwer, P. Brockfeld, A. Dress. On the Cost Funktions for the Control of the Human Arm Movement. Biological Cybernetics, 62:59 528, 99. [9] V. Potkonjak, M. Popovic, M. Lazarevic, J. Sinanovic. Redundancy Problem in Writing: From Human to Anthropomorphic Robot Arm. IEEE Transactions on Systems, Man Cybernetics Part B: Cybernetics), 286):79 84, 998. [] A. Ude, C. Man, M. Riley, C.G. Atkeson. Automatic Generation of Kinematic Models for the Conversion of Human Motion Capture Data into Humanoid Robot Motion. In Proceeding of the First IEEE-RAS International Conference on Humanoid Robots, pages 2223 2228, Boston, 2. [] T. Asfour, A. Ude, K. Berns, R. Dillmann. Control of ARMAR for the Realization of Anthropomorphic Motion Patterns. In The 2nd IEEE- RAS International Conference on Humanoid Robots HUMANOIDS 2), Tokyo, Japan, 2. [2] J. F. Soechting M. Flers. Sensorimotor Representations for Pointing to Targets in Three- Dimensional Space. Journal of Neurophysiology, 622):582 594, 989. [3] J. F. Soechting M. Flers. Errors in Pointing are Due to Approimations in Targets in Sensorimotor Transformations. Journal of Neurophysiology, 622):595 68, 989.