Robot motion planning for the humanoid robot HOAP-3

Transcription

1 Robot motion planning for the humanoid robot HOAP-3 Xochitl Hernandez V., Abraham Sánchez L. and Pablo Camarillo R. Computer Science Department Benemérita Universidad Autónoma de Puebla Puebla, Pue. - México hevexo3@hotmail.com, asanchez@cs.buap.mx, p.camarillor@gmail.com Abstract In the field of robotics there is a great interest in developing strategies and algorithms to reproduce humanlike behavior. Programming a humanoid robot to walk is a challenging problem. Traditional approaches rely heavily on prior knowledge of the robot s physical parameters to devise sophisticated control algorithms for generating a stable gait. We present two approaches to solve the motion planning problem for humanoid robots, one of them is based on a classic robot motion planning idea and the other is a novel idea based on the learning human behavior by imitation. practice and converge towards a uniform exploration of the search space. In the second proposed approach, we showed as a humanoid robot can learn to walk directly by imitating a human gait obtained from motion capture. The training using human motion capture is an intuitive and flexible approach to program a robot, but the direct use of the data results in dynamically unstable motions. We propose an approach to tractable imitation-based learning in humanoids. I. INTRODUCTION Motion planning is one of the key capabilities for autonomous humanoid robots. Previous researches have focused on weight balancing, collision detection, and gait generation. The motion for a humanoid robot to achieve a given goal is typically very complex because of the degrees of freedom involved and the contact constraint that needs to be maintained. Due to the curse of dimensionality, developing practical motion planning algorithms for humanoid robots is a daunting task. Humanoid robots such as HOAP- 3 (Figure 1) have 28 or more degrees of freedom. The problem is further complicated by the fact that humanoid robots must be controlled very carefully in order to maintain overall static and dynamic stability. These constraints severely restrict the set of allowable configurations and prohibit the direct application of existing sampling-based motion planning techniques [3], [5]. Recently, these kind of algorithms that account for system dynamics have been developed for 2D and 3D rigid bodies with state spaces of up to twelve dimensions. These methods have yet to be applied to complex articulated models such as humanoid robots. It has been suggested to limit the active body degrees of freedom for humanoid robot motion planning and balance control, though these ideas are still under development [4], [1]. Our first approach is to adapt a variation of the randomized planner described in [5] to compute full-body motions for humanoid robots that are both dynamically-stable and collision-free. This planner and its variants utilize Rapidlyexploring Random Trees (RRTs) combined with a simple greedy heuristic that aggressively tries to connect two search trees, one from the initial configuration and the other from the goal. These methods have been shown to be efficient in Figure 1. The virtual and the real dynamic humanoid robot Hoap-3. The paper is organized as follows. Some important details of the humanoid robot Hoap-3 are presented in section II. Section III presents the motion planning problem oriented to humanoid robots, in this section we detail the two proposed approaches. Some simulation results are presented and discussed in Section IV, and finally Section V gives some concluding remarks and future works. II. HOAP-3 Robotics research activities have been very active in developing robots, particularly humanoid robots, which can coexist with human beings. Robots that can perform a variety of work in collaboration with human beings are expected to come into being in the near future. Fujitsu Laboratories Ltd. has successfully developed its Hoap-1, Hoap-2 and Hoap-3 miniature robots, which can be utilized in a variety of research and development applications such as development of bipedal walking motion control algorithms and working robot motion algorithms in combination with the basic simulation software, research on human-to-robot communication interfaces, etc. 60

2 HOAP-3, measuring 60 cm tall (23.6 inches) and weighing 8.8 kg (19 lbs), builds on HOAP-2 s ease of use and mobility by adding functions that improve its ability to interact with its environment. The HOAP-3 can be controlled with a personal computer (either wired or wireless) and uses a 1.1GHz Pentium M processor that runs on RT-Linux. This robot has a total of 28 joints in two arms of 6 flexibility, two legs of 6 flexibility, and the head of 3 flexibility and the body part of 1 flexibility, see Figure 2. It is rotation flexibility altogether and a leg, an arm, and the waist are serial links. Figure 2. Kinematics structure of Hoap-3 humanoid robot. The geometric model represents each part of HOAP-3 robot by a fixed mesh of few triangles, as depicted in Figure 1. These meshes are rigidly attached to coordinate frames hierarchically organized in a tree. The root of the tree is the coordinate frame attached to the hips, and represents the global translation and orientation of the model. Each subsequent node in the tree represents the three-dimensional rigid transformation between the node and its parent. This representation is normally called a skeleton or kinematic chain (Figure 2). Each node, together with its corresponding attached body part is called a bone. Each bone is allowed to rotate but not translate with respect to its parent around one or more axes. Thus, at a particular time instant t, the pose of the skeleton can be described by Φ(t) = (R (t), s (t), ϕ (t) ), where R (t) and s (t) are the global orientation and translation of the root node, and ϕ (t) is the set of relative rotations between successive children. For upper-body motion, it is assumed that only ϕ needs to be updated. III. MOTION PLANNING Motion planning is one of the fundamental problems in mobile robot navigation. It has been shown long before that the problem of moving an object through space is PSPACEhard with a time complexity exponential in the degrees of freedom of the object. In mobile robots the degrees of freedom are usually small (three or less) which opens the application of a range of search techniques specifically tailored to the problem. Humanoid robots while being mobile allow for more than the usual three degrees of freedom. Their feet can be placed with a greater choice and the change of body posture allows to overcome obstacles where wheeled robots fail in passing through. The higher degrees of freedom can be tackled, e.g. by sampling-based approaches such as probabilistic roadmaps [3] or RRTs [5]. However, if one can employ a non-probabilistic approach by finding a suitable approximation then the solution can be seen preferable due to its deterministic nature. One possible approximation is to model the robot as a cylinder or by its convex hull which allows for efficient collision checking. Humanoid robots are non-holonomic, i.e. they cannot turn in place without requiring additional space. The trajectory of the body center describes a curve similar to a car-like robot, although the turn radius is generally quite small. The environment in which the humanoid robot moves is W. R is a collection of p links L i (i = 1,..., p) organized in a kinematic hierarchy with Cartesian frames F i attached to each link. We denote the position of the center of mass c i of link L i relative to F i (see Figure 2). A configuration or pose of the robot R is denoted by the set P = {T 1, T 2,..., T p }, of p relative transformations for each of the links L i as defined by the frame F i relative to its parent link s frame. The base or root link transformation T 1 is defined relative to some world Cartesian frame F world. A configuration is denoted by q C (C, is the configuration space), a vector of n real numbers specifying values for each generalized coordinates of R. One can define the C-obstacle region CB C as the set of all configurations q C where one or more of the links of R intersect with another link of R. The open subset C\CB is denoted by C free, and it represents the space of collision free configurations in C of the robot R. Let FORWARD(q) be a forward kinematics function mapping values of q to a particular pose P. FORWARD(q) can be used to compute the global transformation G i of a given link frame F i relative to the world frame F world. Let X (q) be a vector relative to F world representing the global position of the center of mass of R while in the configuration q. A configuration q is statically stable if the projection of X (q) along the gravity vector g lies within the area of support SP (i.e., the convex hull of all points of contact between R and the support surface in W.) Let C stable C be the subset of statically stable configurations of R. Let 61

3 C valid = C stable C free denote the subset of configurations that are both collision free and statically stable postures of the robot R. C valid is called the set of valid configurations. Any statically-stable trajectory can be transformed into a dynamically-stable trajectory by arbitrarily slowing down the motion [2], [10] A zero moment point (ZMP) is a key concept of dynamic balance maintenance [11]. It is the point where the moment induced by the ground reaction becomes zero. When we consider static balance, the condition of equilibrium is that the projection of the center of mass (CM) is inside the supporting area by the feet. If it goes out of the area, the static balance is broken and the human begins to fall down. However, when we consider dynamic balance, the human does not lose his or her control unless the ZMP is inside the support area, even if the projection of the CM goes out to the area. If the ZMP is on the boundary of the area, the human begins to fall down. Contrary to the projection of the CM in static balance, the ZMP stays on the boundary during falling down and never gets out of the area (Figure 3). Figure 3. ZMP vs. projection of the CM. A. A classical robot motion planning approach Most real-world motion planning problems have dynamic, as well as static constraints. Early randomized motion planning algorithms did not take into account these dynamic constraints. These algorithms generated a solution path in the configuration space, and then determined control inputs to steer along the path. Kinodynamic motion planning eliminates this second step by incorporating dynamic constraints into the global planner, but in doing so, it doubles the dimensionality of the motion planning problem. RRRTs are a class of randomized algorithms that can be used both for systems involving kinodynamic constraints or not [6]. The RRT-Connect algorithm is a single-shot planning algorithm that attempts to find a path by connecting two growing RRT s rooted at the initial and goal configurations. The idea of the algorithm is illustrated in Figure 4. T a and T b denote two RRT s rooted at the initial configuration, q init and the goal configuration, q goal, respectively. The algorithm starts by letting T a explore outward with the Figure 4. The RRT-Connect algorithm. EXTEND operation. This operation first samples a point (q rand ) in the configuration space and then finds the nearest configuration (q near ) to this sampled point. We extend q near to a new configuration q new by some distance ε toward q rand. Then we try to connect a nearest node of T b to q new by a CONNECT operation, which is usually a straight-line connection. If the connection succeeds, then we have found a path connecting q init and q goal. Otherwise, we will switch the roles of T a and T b and let T b perform the EXTEND operation and T a perform the CONNECT operation. The process repeats until a path is found or a maximal number of trials have been reached. The planner described in the previous paragraph performs its search in C. However, it needs to use a balance compensation method to enforce dynamic constraints imposed upon the ZMP trajectory, the modifications to the algorithm are the following: The new configuration in the EXTEND operation first generates a goal configuration q targetl by making an incremental step motion towards q from q near as before. However, q new is generated by filtering the straight-line path connecting q near and q target through the dynamic balance compensator. This creates an incremental dynamically-stable trajectory from q near towards q target. The filter simulation terminates when the configuration converges or after a preset time limit is exceeded. If no collision occurs prior to termination, the most recent configuration output by the filter becomes q new and is added to the tree T. In this way, we are guaranteed that q new C valid. Rather than picking a purely random configuration q rand C at every planning iteration, we pick a random configuration that also happens to correspond to a statically stable posture of the robot, i.e., q rand C stable. The modified EXTEND operation is showed in Figure 5. The main planning loop involves performing a simple iteration in which each step attempts to extend the RRT by adding a new vertex that is biased by a randomly selected stable configuration. EXTEND selects the nearest vertex already in the RRT to the given sample configuration, q. The function 62

4 NEW CONFIG makes a dynamically-stable motion toward q as outlined previously using some fixed incremental distance ε to generate q target. Three situations can occur: Reached, in which q is directly added to the RRT, Advanced, in which a new vertex q new q is added to the RRT; Trapped, in which no new vertex is added due to the inability of the balance compensator to generate an incremental trajectory that lies in C valid. One of the key differences between RRT CONNECT STABLE and the classic RRT- Connect is that instead of uniformly sampling C and growing trees that lie entirely in C free, it attempts to uniformly sample C stable and grow trees that lie within C valid. Figure 6. The learning human behavior by imitation idea. Figure 5. The modified EXTEND processus. We require a method of generating random statically stable postures (random point samples of C stable ). For our implementation, a set of N samples of C stable is generated as a preprocessing step; this computation is specific to the robot Hoap-3 and support-leg configuration, and need only be performed once. The collection of stable postures is saved to a file and loaded into memory when the planner is initialized. B. learning human behavior by imitation Humanoid robots are electro-mechanical systems resembling the morphology of the human body. Consequently, it is very much desired to reproduce ordinary motions like humans. Great efforts have been made already to construct especially walking robots to study bipedal locomotion. The approach to achieve stable gait acquisition in humanoid robots via imitation is depicted in Figure 6. First, a motion capture system transforms Cartesian position of markers attached to the human body to joint angles based on kinematic relationships between the human and robot bodies. Human and HOAP-3 bodies are very different in shape and proportions (see Figure 7). Once the Cartesian coordinates of human head and hands are obtained, it is necessary to normalize these data to HOAP-3 dimensions. Although there are many different normalization techniques [8], for the proposed upper-body imitation system we scale the tracked end-points by a constant factor that takes into account the difference in size between human and robot. Results show that this rough approximation to a normalization process is enough to satisfy the requirements of this purpose. Then, we employ dimensionality reduction to represent posture information in a compact low-dimensional subspace. Optimization of whole-body robot dynamics to match human motion can be performed in the low dimensional space. Particular classes of motion such as walking or kicking are intrinsically low-dimensional. We can apply the well known method of principal components analysis (PCA) to parameterize the low-dimensional motion subspace [7], [9]. The result of linear PCA can be thought of as two linear operators C and C 1 which map from high to low and low to high dimensional spaces respectively. We use a straightforward, standard linear PCA method to map between the low and high-dimensional posture spaces. Vectors in the high-dimensional space are mapped to the low-dimensional space by multiplication with the transformation matrix C. Figure 7. Skeleton hierarchy for the human body and the hoap-3. For the accomplishment of applications in the real robot, we considered that the sensory feedback data can be recorded from the robot during motion and a causal relationship between actions in the low dimensional posture space and the expected sensory feedback is learned. This learned sensory-motor mapping allows humanoid motion 63

5 dynamics to be optimized. An inverse mapping from the reduced space back to the original joint space is then used to generate optimized motion on the robot. We will present results demonstrating that the proposed approach allows a humanoid robot to learn to walk based solely on human motion capture without the need for a detailed physical model of the robot. Recorded kinematic data of human movements were analyzed in order to find geometrical relations among various joint angles characterizing the instantaneous postures. A simplified human body representation leads to dynamics of an underactuated mechanical system with parameters based on anthropometric data. Motion planning for humanoid robots of similar structure can be carried out by considering solutions of reduced dynamics obtained by imposing the virtual holonomic constraints that are found in human movements. IV. SIMULATION RESULTS Planners were implemented on an Intel c Pentium IV processor-based PC running at 2.6 GHz with 2 GB RAM, using C# and OpenGL. An important feature of our system is the use of ODE as a physics engine, and in addition as collision checker. ODE is an open source, high performance library for simulating rigid body dynamics. It has advanced joint types and integrated collision detection with friction. This section presents some preliminary experiments. We have implemented the prototype planners that run within a graphical simulation environment using a dynamic model of the Hoap-3 humanoid robot (28-DOF). Through a graphical user interface, an user can position individual joints or use inverse kinematics to specify body postures. The filter function can be run interactively to ensure that the goal configuration is statically-stable. In Figure 8, the virtual environment of an office is showed; this scene contains furniture within it. It is desired that the robot moves from one desk located in a corner to another desk in the opposite corner. The following scene shows a cafeteria with tables and small decorations; the robot is placed in one of the tables of the scene and requires to plan a collision free trajectory to arrive until the bar. We proposed a motion generation system for the humanoid robot Hoap-3. The motion generated by our system is smooth and stable in the balance. Our system performs various and unrestricted motions for humanoid robots without hard problem in dynamics. Recorded kinematic data of human movements were analyzed in order to find geometrical relations among various joint angles characterizing the instantaneous postures. For motions like sitting down and rising from a chair, the very complex structure of the human body can be reasonably simplified to just three links. Assuming that the feet are fixed parallel to the ground, we can take the first link to Figure 8. Dynamically-stable planned trajectory for a walking motion. be the combined lower legs, the second link to be the combined upper legs and the third link to represent the upper body including arms and head. Of course, we know that humans are fully actuated during the considered motions since the feet act as bases flat on the ground. However, it is reasonable to believe that, although the whole body constitution balances about the ankles, stabilization is mostly done without using torques of the ankle joints, but via synchronization of the other joints. The walking gait on the real robot is not as stable as the results in the simulator because of differences in frictional forces between the simulator and the floor. We expect an improvement in performance when learning is performed directly on the real robot. We note that the learned motion is indeed dynamic and not quasi-static motion because there are only two postures in our walking gait that can be considered statically stable, namely, the two postures in the walking cycle when the two feet of the robot contact the ground. The remaining postures in the walking gait are not 64

6 statically stable as the gait has a fairly fast walking speed. is an important learning mechanism in many biological systems including humans. It is easy to recover kinematic information from human motion using, for example, motion capture, but imitating the motion with stable robot dynamics is a challenging research problem. Traditional model-based approaches based on zero-moment point or the inverted pendulum model require a highly accurate model of robot dynamics and the environment in order to achieve a stable walking gait. REFERENCES [1] K. Hauser, T. Bretl, and J. C. Latombe, Using motion primitives in probabilistic sample-based planning for humanoid robots, In proceedings of the Workshop on the Algorithmic Foundations of Robotics (WAFR), [2] S. Kagami, F. Kanehiro, Y. Tamiya, M. Inaba and H. Inoue, Autobalancer: An online dynamic balance compensation scheme for humanoid robots, Robotics: The Algorithmic Perspective, Workshop on Algorithmic Foundations of Robotics, A K Peters, [3] L. E. Kavraki, P. Svestka, J. C. Latombe and M. H. Overmars, Probabilistic roadmaps for path planning in high-dimensional configuration spaces, IEEE Transactions on Robotics and Automation, Vol. 12, No. 4, pp , [4] J. Kuffner, K. Nishiwaki, S. Kagami, M. Inaba and H. Inoue, Motion planning for humanoid robots, International Symposium of Robotics Research (ISRR03), [5] S. M. LaValle and J. J. Kuffner, Rapidly-exploring random trees: Progress and prospects, Proc. Workshop on the Algorithmic Foundations on Robotics, Figure 9. Dynamically-stable planned trajectory for a walking motion. V. CONCLUSION Humanoid robots are still a young technology with many research challenges. Only few humanoid robots are currently commercially available, often at high costs. Physical prototypes of robots are needed to investigate the complex interactions between robots and humans and to integrate and validate research results from the different research fields involved in humanoid robotics. The development of a humanoid robot platform according to a special target system at the beginning of a research project is often considered a time consuming hindrance. We presented in this paper two algorithms for computing dynamically-stable collision-free motions for humanoid robots given full-body posture goals. Balance constraints are imposed upon incremental search motions computed by a randomized planner in order to maintain the overall dynamic stability of the computed trajectories. Imitation [6] S. M. LaValle and J. J. Kuffner, Randomized kinodynamic planning, The International Journal of Robotics Research, Vol. 20, No. 5, pp , [7] S. Ra, G. Park, C. Kim and B. You, PCA-based genetic operator for evolving movements of humanoid robot, IEEE World Congress on Computational Intelligence, pp , [8] A. Safonova, N. Pollard and J. K. Hodgins, Optimizing human motion for the control of a Humanoid Robot, 2nd International Symposium on Adaptive Motion of Animals and Machines, [9] A. Safonova, J. K. Hodgins and N. S. Pollard, Synthesizing physically realistic human motion in low-dimensional, behavior-specific spaces, ACM SIGGRAPH, pp , [10] T. Sugihara and Y. Nakamura, Whole-boy cooperative balancing of humanoid robot using COG Jacobian, Proc. of the IEEE Int. Conf. on Intelligent Robots and Systems, pp , [11] M. Vukobratović and D. Juricic, Contribution to the synthesis of biped gait, IEEE Transaction on Biomedical Engineering, Vol. 16, No. 1, pp. 1-6,