Time Parameterization of Humanoid Robot Paths
Total Page:16
File Type:pdf, Size:1020Kb
SUBMITTED TO IEEE TRANS. ROBOT. , VOL. XX, NO. XX, 2010 1 Time Parameterization of Humanoid Robot Paths Wael Suleiman, Fumio Kanehiro, Member, IEEE, Eiichi Yoshida, Member, IEEE, Jean-Paul Laumond, Fellow Member, IEEE, and Andr´eMonin Abstract—This paper proposes a unified optimization frame- work to solve the time parameterization problem of humanoid robot paths. Even though the time parameterization problem is well known in robotics, the application to humanoid robots has not been addressed. This is because of the complexity of the kinematical structure as well as the dynamical motion equation. The main contribution in this paper is to show that the time pa- rameterization of a statically stable path to be transformed into a dynamical stable trajectory within the humanoid robot capacities can be expressed as an optimization problem. Furthermore we propose an efficient method to solve the obtained optimization problem. The proposed method has been successfully validated on the humanoid robot HRP-2 by conducting several experiments. These results have revealed the effectiveness and the robustness (a) Initial configuration (b) Final configuration of the proposed method. Index Terms—Timing; Robot dynamics; Stability criteria; Fig. 1. Dynamically stable motion Optimization methods; Humanoid Robot. these approaches are based on time-optimal control theory [3], I. INTRODUCTION [4], [5], [6], [7]. The automatic generation of motions for robots which are In the framework of mobile robots, the time parameteriza- collision-free motions and at the same time inside the robots tion problem arises also to transform a feasible path into a capacities is one of the most challenging problems. Many feasible trajectory [8], [9]. The main objective, in this case, is researchers have separated this problem into several smaller to reach the goal position as fast as possible. subproblems. For instance, collision-free path planning, time Even though the conventional methods which are based on parameterization of a specified path, feedback control alonga the optimal-control theory have been successfully applied in specified path using vision path planning, etc. In this paper, practice on manipulators and mobile robots, the application of the problem of finding a time parameterization of a given path time optimal control theory in the case of humanoid robot is for a humanoid robot is investigated. however a difficult task. This is because not only the dynamic The time parameterization problem is an old problem in equation of the humanoid robot motion is very complex, robotic research [1]. In order to better understand the objective but also applying time optimal control theory requires the of time parameterization of a path, let us start by defining a calculation of the derivative of the configuration space vector path and a trajectory. of humanoid robot with respect to the parameterized path. A path denotes the locus of points in the joint space, or in Although such calculation can be evaluated from differential the operational space, the robot has to follow in the execution geometry, it is a very difficult task in the case of systems with of the desired motion, and a trajectory is a path on which a large number of degree of freedoms and branched kinematic time law is specified [2]. chains, which is the case of humanoid robot. For that, we pro- Generally speaking, the time parameterization of a path is pose to solve the time parameterization problem numerically the problem of transforming this path into a trajectory which using a finite difference approach. respects the physical limits of the robot, e.g. velocity limits, The remainder of this paper is organized as follows. Section acceleration limits, torque limits, etc. II gives an overview of the dynamic stability notion and the In the research works on manipulators, the time parameter- mathematical formulation of Zero Moment Point (ZMP). The ization problem has the objective of reducing the execution time parameterization problem is formulated as an optimiza- time of the tasks, thereby increasing the productivity. Most of tion problem under constraints and an efficient method to solve it is explained in Section III. Cases studies and experimental W. Suleiman, F. Kanehiro and E. Yoshida are with CNRS-AIST JRL results using the humanoid robot platform HRP-2 are given in (Joint Robotics Laboratory), UMI3218/CRT, National Institute of Ad- Section IV. vanced Industrial Science and Technology (AIST), Tsukuba Central 2, 1- 1-1 Umezono, Tsukuba, Ibaraki, 305-8568 Japan. {wael.suleiman, f-kanehiro, e.yoshida}@aist.go.jp II. DYNAMIC STABILITY AND ZMP: AN OVERVIEW J.-P. Laumond and Andr´e Monin are with LAAS-CNRS, 7 Avenue du Let us start by introducing some definitions: Colonel Roche, 31077 Toulouse, France. {jpl, monin}@laas.fr A part of this work has been done at LAAS-CNRS during the Ph.D. thesis Definition 1: Statically stable trajectory is a trajectory for of W. Suleiman. which the trajectory of the projection of the Center of Mass SUBMITTED TO IEEE TRANS. ROBOT. , VOL. XX, NO. XX, 2010 2 (CoM) of the humanoid robot on the horizontal plane is always where [.]∧ designs the skew operator defined as follows inside of the polygon of support (i.e. the convex hull of all ∧ [.] : ω ∈ R3 → so(3) points of contact between the support foot (feet) and the 0 −ωz ωy ground). ∧ (8) [ω] = ωz 0 −ωx Definition 2: Dynamically stable trajectory is a trajectory for which the trajectory of Zero Moment Point (ZMP) [10] is −ωy ωx 0 always inside of the polygon of support. where so(3) denotes the Lie algebra of SO (3) which is the group of rotation matrices in the Euclidean space. The generation of a statically stable path deals only with the The inverse operator of skew operator can be defined as kinematic constraints of the humanoid robot. It can be obtained follows ∨ by constraining the projection of the Center of Mass (CoM) [.] : Ω ∈ so(3) → R3 on the horizontal plane to be always inside of the polygon of ∨ 0 −ωz ωy support [11], [12], [13], [14]. ∨ [Ω] , ωz 0 −ωx Theoretically, any statically stable trajectory can be trans- −ωy ωx 0 (9) formed into a dynamically stable one by slowing down the humanoid robot’s motion. ωx = ωy Let the ZMP on the horizontal ground be given by the following vector ωz T p = px py (1) and the angular velocity can be calculated using the inverse operator as follows To compute p, one can use the following formula i ∨ n × τ o i dR iT p N ω = R (10) = o (2) dt (f |n) where the operator × and (.|.) refer to the cross and scalar Note that the skew operator and its inverse are linear products respectively, and operators. Finally, ω˙ i denotes the angular acceleration of the ith • N is a constant matrix link. 1 0 0 0 N = (3) Note that R and Ic0 are the rotation matrix associated 0 1 0 to the free-flyer joint (pelvis joint) and its inertia matrix . respectively. ω0 and ω˙ 0 are the angular velocity and the • the vector n is the normal vector on the horizontal ground T angular acceleration of the free-flyer joint respectively. n = 0 0 1 . • The vector f o is the result of the gravity and inertia forces III. TIME PARAMETERIZATION PROBLEM FORMULATION n Generally speaking, the time parameterization problem of o ¨ ci f = Mg − mi X (4) a function f (xt), where t denotes time, consists in finding a i=0 real function in such a way f xS verifies some temporal X St ( t ) where g denotes the acceleration of the gravity (g = constraints. −gn), and M is the total mass of the humanoid robot. Mathematically that means c th The quantities mi, X¨ i are the mass of the i link h(St) ≤ f (xSt ) ≤ l(St) (11) and the acceleration of its center of mass ci respectively. c0 Note that m0, X¨ are, respectively, the mass and the In order to obtain a causal and feasible motion, the function dSt acceleration of the free-flyer joint (pelvis joint) of the St should be a strictly increasing function, that means dt > 0. humanoid robot. Therefore we will express St as the integral of a strictly o o • τ denotes the moment of the force f about the origin positive function sh > 0, as follows of the fixed world frame. The expression of τ o is the t following St = sh dh (12) h=0 n Z ci o ci ci Our objective is to transform the statically stable path into τ = mi X × g − X¨ − L˙ (5) a dynamically stable trajectory by minimizing a specified i=0 X criterion. We will be interested in minimizing cost function where Lci is the angular momentum at the point c i of the form L˙ ci i i i i = R Ici ω˙ − Ici ω × ω (6) tf Ri I and ci are the rotation matrix associated to the J (St)= St + L (qs , q˙s , q¨s , τs ,sh) dh th i f h h h h i link and its inertia matrix respectively. ω is the Zh=0 angular velocity of the ith link can be obtained using tf tf = s dh + L (q , q˙ , q¨ , τ ,s ) dh the following formula h sh sh sh sh h Zh=0 Zh=0 i ∧ dR T T1 T2 ωi = Ri (7) dt (13) | {z } | {z } SUBMITTED TO IEEE TRANS. ROBOT. , VOL. XX, NO. XX, 2010 3 • where qsh , q˙sh , q¨sh and τsh design the configuration vector, Constraint (16) guarantees that the ZMP is inside of the the joint velocity, the joint acceleration and the exerted torque polygon of support. vector respectively.