The Tele-operation of the Humanoid Robot -Whole Body Operation for Humanoid Robots in Contact with Environment- Hitoshi Hasunuma, Kensuke Harada, and Hirohisa Hirukawa System Technology Development Center, Kawasaki Heavy Industries, Ltd. 1-1, Kawasaki-cho, Akashi, 673-8666, Japan Humanoid Research Group, Intelligent System Institute, National Institute of Advanced Industrial Science and Technology, 1-1-1, Umezono, Tsukuba, 305-8568, Japan Abstract We are developing remote control technologies for humanoid robots. The advantage of the remote control is that tele-operated humanoid robots can deal with various tasks according to the situation in real environment. Our goal is to develop the control system to execute the tele-operation of the humanoid robot easily. We developed whole body operation methods of the humanoid robot that works in environment with some obstacles. In this paper, we describe the developed method and experimental results using the tele-operation system of the humanoid robot HRP-3P. We discuss about the robot autonomy to perform the operation without difficulty. I. INTRODUCTION A humanoid robot has the same shape as humans and has substantial advantage that it can work in the same environment as humans do without any previous modifications. For this reason, humanoid robots are expected to be the next generation of robotic systems that can be used in the coexistent field with humans. However, present applications of humanoid robots are limited to the field for research, education and entertainment. This restriction is caused by not only the less motion ability of humanoid robots than humans, but also the less recognition ability that is not enough for task execution in various situations. Since the recognition ability of humans would be utilized in the remote control for robots, tele-operated humanoid robots can deal with various tasks according to the situation in the real environment. Currently, the New Energy and Industrial technology Development Organization (NEDO) in Japan is supporting the research and development of basic technologies for humanoid robots that work in real environment that started in 2002. As one of technologies, we are developing remote control technologies for humanoid robots. The humanoid robot has some characteristics different from conventional fixed-base manipulators. The humanoid robot has many degrees of freedom (DOF). The control algorithm of the robot has to satisfy the severe balance constraint because of the small supporting area of the robot. There are the geometrical and dynamical differences between humanoid robots and humans. For these reasons, it is difficult to control the whole body motion of the humanoid robot only by the command from Fig. 1. Autonomous-remote hybrid control system the human operator. We are trying to develop an autonomousremote hybrid control system whose configuration is shown in Fig.1. This control method makes it possible to execute the tele-operation easily with the operator s command whose DOF are less than those of the robot. We have proposed a basic method of the autonomousremote hybrid control system [1] and described the whole body operation that the workspace of the operated arm was extended with step motion autonomously [2]. We developed the whole body operation of the humanoid robot manipulating objects in environment with some obstacles. Especially, we supposed the situation that the object lies in the depth of the table [3]. In this case, the humanoid robot has to contact with the environment to achieve the task. In this paper, we describe about the developed method and experimental results using the real humanoid robot teleoperated by the remote control cockpit we developed. Finally, we discuss about the robot autonomy to assist the operator when the humanoid robot works in complicated environment. II. THE WHOLE BODY TELE-OPERATION METHOD FOR THE HUMANOID ROBOT A. Problems of the Remote Control for Humanoid Robots In this research, we suppose that the vision information is provided only from cameras on the robot even in unknown environment. When the operator of the robot recognizes the state of the environment around the robot through the vision information, he/she can form the work plan. To achieve the work plan, it is important to realize the desired body motion of the humanoid robot. In the case of humans, there are various kinds of work plans in the same 1-4244-0200-X/06/$20.00 2006 IEEE 333 HUMANOIDS 06
situation. For example, in the strategy to move the hand forward, we stretch the arm, bend the waist forward, twist the waist and the upper body, or move the foot and the waist forward. This is because the human body has more DOF than those required for the task execution. These motions are chosen considering the task strategy, the task environment and his/her ability. In this research, this whole body motion strategy to achieve the work plan is called the Operator s- Intention. It is difficult to realize the Operator s-intention only by the motion command from the remote operator. This is because the humanoid robot has many DOF and there exists geometrical and dynamical differences between the humanoid robot and the human operator. On the other hand, the whole body motion of the human consists of two motions. The first one is the conscious motion that controls the important part for the task execution, and the other motion is the sub-conscious motion, for example, to keep balance and extend the workspace of the arm. In this research, the framework of generating the human motion is applied to the whole body operation method for humanoid robots. The whole body motion of the humanoid robot is realized by combining the conscious motion controlled by the operator and the sub-conscious motion controlled by the robot autonomy. In the control system, various kinds of whole body operation methods are provided in advance. The operator selects the method according to the Operator s-intention, and controls only the conscious motion through the remote control system. B. Overview of the Proposed Method We consider the humanoid robot as the group of subsystems so that the robot is divided into some parts. The goal for the desired motion of the robot is given to each subsystem. When the goal of each subsystem is achieved to some extent, the whole body motion is realized. In this method, it is difficult to achieve the global optimum condition of the robot, but the proposed method achieves the local optimum condition and it works fairly well. Each subsystem is controlled by the operator or the robot autonomy. When the motion of a subsystem depends on the conscious motion, the subsystem is controlled by the operator. And the other subsystems are controlled by the robot autonomy, whose motions depend on the subconscious motion. The Operator s-intention can be accomplished by combining tasks of subsystems. In the proposed method, the humanoid robot is considered as shown in Fig.2 where the robot is composed of some subsystems(head, arm, body, and leg). Fig.3 shows the example algorithm of the proposed method. At first, the representative points of each subsystem are defined. The position of the eye, the hand, the shoulder, and the waist are defined as a representative point of the head part, the arm part, the body part, and the leg part respectively. These representative points are joining points between neighboring subsystems or end points of the link. The po- Fig. 2. Subsystems of a humanoid robot sition of the foot that is in contact with the ground is also defined as the representative point of the leg part. The basic coordinate of the robot is set on the ground under the initial position of the waist. With respect to the basic coordinate, the reference position of each representative point will be defined as described in this subsection. The reference position of each representative point is defined as the optimum position that is evaluated for the task execution of each subsystem and is calculated independently. When the representative point reaches the reference position, we consider that the task of the subsystem is achieved. As neighboring subsystems are linked with each other, the hierarchical structure is organized, and the structure consists of the foot, the waist, the shoulder, and the hand. The foot is the lower-class and the hand is the highest. Since the reference position is given independently, every representative point may not reach its reference position at the same time according to the restrictions due to the positional relations between neighboring subsystems. To solve such a contradiction, the reference position is converted to the achievable position so that the most important purpose of some subsystem will be achieved as much as possible, and the less important ones will be considered only within the limitation of the task conversion. Each of the converted position will be transferred into a target position that is the command value to the humanoid robot controller. The target position is given as the achievable position considering the restriction of the higher-class on the basis of the target position of lower-class in the hierarchical structure of subsystems. After completing all of the target position determination, the target angles of each axis are calculated by the inverse kinematics of the robot link structure. The proposed method can be applied to various kinds of whole body operation methods by combining some target tasks of subsystems. A whole body operation method can be changed to another one by changing the target task and parameters of its subsystem. Note that whole body operation methods have to be preencoded in advance. The task of each subsystem can be designed by the human programmer from the observation of the human motion because the humanoid robot has the humanlike configuration of the body. 334
Fig. 3. Example Algorithm of the proposed method C. Related Work There are a few attempts to generate the online motion of the humanoid robot using the framework based on observation of human conscious and subconscious motion generations; mobile manipulation [4], the whole body reaching method for extending its workspace [5], the whole body walking motion based on the hand position [6]. In their method, strategies to generate the whole body motion are defined as a set of algorithms to realize the specified behavior. In order to realize another behavior, the strategy would be changed. Neo proposed the whole body operation method with the workspace extension autonomy that utilized body parts are changed according to the hand position [7], [8]. Their method can be also used in the case that the operator controls the other part such as the leg or the waist consciously changing by switches. However, another method to generate the whole body motion in the hand operation was not mentioned. On the other hand, in the proposed method, the strategy to realize Operator s-intention is due to the combination of tasks in each subsystem. Another strategy can be realized only by changing some parts of target tasks in each subsystem. Furthermore, by preparing the state flow in which different kinds of strategy changes, the complex Operator s-intention combining some kinds of whole body operation methods would be realized. For example, in the hand manipulation task to pick up an object, when the tele-operated robot stretches its arm to reach the object, the upper body bends to extend the workspace of the arm. When the hand does not reach the object using the whole body operation, the target task of the leg subsystem would be changed from keeping balance to walking forward and the whole body operation method is transferred from one to another continuously. III. THE WHOLE BODY OPERATION FOR THE HUMANOID ROBOT IN CONTACT WITH ENVIRONMENT A. Whole body motions to realize strategies In this research, we have developed the whole body operation method in the task environment with a flat floor and no obstacle. In this paper, we describe the operation method in the environment with some obstacles. We supposed three situations in the task of manipulating an object described below. First, small difference in level lies between the object and the robot. Second, the object lies in the depth of the table from the robot. Third, another object lies before the object. When the humanoid robot does not reach the object because of some obstacles in the environment, the robot has to contact with the environment to achieve the task. The effective strategy to achieve the task is described as follows. In the first case, the robot changes its standing position shown in Fig.4. The operator controls the foot position to go up the difference in level and the leg subsystem controls the waist position to keep its balance. In the second case, the robot extends the workspace of the arm using the obstacle shown in Fig.5. The operator controls the hand position to contact with the table and to manipulate the object. The leg subsystem controls the waist position to keep its balance with two legs and one arm. In the third case, the robot removes the obstructive object shown in Fig.6. The operator controls the hand position to lift up the object. The leg subsystem controls the waist position to keep its balance using external force from the hand and controls the foot position to move its position according to the operator s command. We developed whole body operation methods for the humanoid robot to realize each strategy described above. In this section, we focus on the second case and show the whole body operation method to manipulate the object keeping balance in contact with the environment. In this method, we consider the robot motion as quasi-static motion in which the robot keeps the static balance with the position of the center of the gravity(cog). B. Whole body operation to manipulate the object supporting with two legs and one arm The outline of the motion is described as follows. The robot puts the hand on the table, and moves forward the other hand to handle the target object. The upper body of the robot goes forward allowing the contact between the supporting hand and the table. 335
Fig. 4. Strategy to go up the difference in level [9]. By considering the fill six dimensional force/moment, we can determine whether or not the robot can maintain the contact with the environment in any cases while it is impossible by using the Zero Moment Point(ZMP). Let f G and τ G be the three dimensional vectors of the force and moment, respectively, applied to the robot. Let also P and L be the three dimensional vectors of the linear and angular momentum, respectively. The relationship among them can be given by Fig. 5. Strategy to manipulate the object in the depth of the table supporting with two legs and one arm In the first phase, the leg subsystem controls the reference position of the waist so that the position of the robot CoG is located within the supporting area of the robot. When the robot stands with two legs, the area that consists of two foot prints is considered as the supporting area of the robot. Since the object is far from the robot, the robot cannot reach the target object in the standing posture with two legs. In order to reach the object, the supporting area of the robot should be extended so that the position of the waist goes forward. When the hand touches the table and supports the body of the robot, the supporting area of the robot is extended. The operator controls the hand for support to touch the table and the body subsystem bends the upper body to enlarge the workspace of the arm. In the second phase, the operator controls the other hand to handle the target object. The body subsystem controls the reference position of the shoulder to extend the workspace of the arm. The leg subsystem controls the reference position of the robot CoG according to the velocity of the shoulder. When the hand contacts with the environment, it becomes difficult to determine whether or not the robot keeps balance due to the reason shown in the next subsection. We consider the position where the maximum force applied at the support hand as the maximum position of the robot CoG. The leg subsystem controls the reference position of the robot CoG between the initial position and the maximum position. C. Balance of the robot We first consider the balance of the robot taking the six dimensional force/moment applied to the robot into account f G = Ṗ + Mg (1) τ G = p G ( Ṗ + Mg) L (2) where M and p G denote the total mass and the CoG vector, respectively, of the robot. We can also express the force/moment applied to the robot by using the force/moment applied at each contact point. We obtain f C = τ C = K f k (3) k=1 K ((p k p Z ) f k + τ k ) (4) k=1 where p Z and p k denotes the position vector of the center of the supporting area and the position vector of the each contact point, respectively. The internal force is the component of contact forces without affecting the linear/angular momenta of the robot and can be expressed by using eqs.(3) and (4) as φ = K U k f k + V k τ k (5) k=1 where φ denotes the parameter vector which dimension is same as that of the internal force. By using eqs.(3), (4) and (5), we can obtain the forcef k and the moment τ k (k =1,,K) at each contact point uniquely as a function of f C, τ C and φ. Moreover, if f C and τ C satisfy the following relationship under f C = f G and τ C = τ G, the robot maintains contact with the environment. By approximating the friction cone using the L faced polyhedral convex cone, we can obtain Fig. 6. Strategy to remove the object with walk motion (v l k) T f k 0, l =1,,L (6) (e i k) T τ k (e r k) T (r l k f k ), i =1,,m (7) n T k τ k ν k n T k f k, (8) where n l k, vl k, ν k, r l k and ei k denote the normal vector of the environment, the face vector of the polyhedral cone, the coefficient of torsional friction, the position vector of the edge of the contact area, and its direction vector, respectively. 336
A. Experimental System IV. EXPERIMENT The experimental system consists of the humanoid robot HRP-3P and the remote control cockpit shown in Fig.7. HRP-3P has 1600 mm height, 664mm width, 363 mm depth and the weight of 65 kg including batteries. It consists of a total of 36 DOF; seven DOF for each arm and six DOF for each leg, two DOF for a head and waist, three DOF for each hand. Four force/torque sensors are equipped in the tip of the arms and the legs, and one acceleration sensor and one inclination sensor is equipped inside the body, three cameras for vision recognition and two small cameras for tele-operation are equipped inside the head. The remote control cockpit we developed consists of the following control devices; two master arms (six DOF each) for hand operation, two joysticks (three DOF each) for continuous input, one master foot (two DOF) for auxiliary operation, a touch panel display for control state transition, a naked-eye three-dimension (3D) display for vision information. In the developed system, the operator recognizes the state of the environment from vision information through the 3D display, and selects the whole body operation method using graphical user interface on the touch panel display. The operator s conscious motion commands are input by control devices. The master arm can be used to control the position/orientation of the hand or the foot. The joystick can be used to control the velocity of each part or the walking motion. Using the master foot, some kinds of auxiliary operation can be used: the head control, the hand control, the elbow control, and so on. These operations are controlled by the pedal of the master foot on the floor. The cockpit communicates with HRP-3P using the wireless LAN system. The updating cycle for the data communication was less than 50 msec in the experiment. The mode command and the motion command are transferred from the cockpit to the control system of HRP-3P. The mode command changes the mode of the whole body operation method to another one. The present operation method and the sensor information of HRP-3P are transferred to the cockpit. The motor command values for the robot hardware are generated by the plug-in software of OpenHRP [10] that is the control software system of HRP-3P. The tele-operation plugin software generates target angles of each joint, the target position of ZMP, and target posture angles. These target angles are corrected by the stabilization plug-in software so that the measured ZMP and posture angles follow their target values. B. Experimental Results Developed methods were confirmed in the experiment. Fig.8 shows the experimental result in the operation to go up the small difference in level. The robot could go up and down the difference in level by the foot control of the operator. The height of the difference was 150 mm. Fig.9 shows the experimental results in the operation to remove the long pipe with walk motion. The weight of the pipe Fig. 7. Fig. 8. HRP-3P and the remote control cockpit Going up the difference in level was 1.4 kg and the robot could walk forward/backward/rightleft carrying the pipe according to the operator s command. Fig.10 shows the state flow of operation methods to control the hand in contact with the table. In this strategy, the operator has to instruct the translation of the method between different tasks of the leg subsystem. We considered the trigger switch of the other operation device as the support switch for translation. Fig.11 shows the experimental result when the right hand went forward to handle the object supporting with the left arm and both legs. The height of the table was 700 mm from the floor level, and the object was located on about 600 mm depth from the edge of the table. The position of the waist was moved about 170 mm forward from the initial position, and the robot could reach the target object. In the experiment, the maximum force at the support hand was 100 N, and the contact point of the foot was set on Fig. 9. Walking forward carrying the pipe 337
Fig. 12. Trajectory of the reference CoG Fig. 10. Fig. 11. State flow of manipulation in contact with the table Right hand operation supporting with the left hand the toe. Fig.12 shows the trajectory of the reference CoG and the supporting area of the robot. The leg subsystem controlled the position of the waist so that the robot CoG followed the reference CoG of the robot. When HRP-3P stands with two legs and the robot CoG goes out of the supporting area with two legs, HRP-3P falls down. The supporting hand could extend the supporting area of HRP-3P. Although our formulation can be applied for keeping the dynamical balance, we assumed the quasi-static condition where the linear/angular acceleration of the robot is small enough. In these experiments, the tele-operated HRP-3P could realize the whole body motion to accomplish the task in the environment with some obstacles. In the developed system, the operator can change the operation method according to the situation and the tele-operated humanoid robot can support various situations by combining some kinds of operation methods. In order to reduce the load of the operator, it is important to develop the interface that the operator can select the suitable operation method easily. V. ROBOT AUTONOMY TO ASSIST THE OPERATOR In the operation of such a complicated whole body motion that contacts with environment, many kinds of restriction affect the robot motion. It is difficult for the operator to recognize the restriction of the robot motion during the operation. Then, the operator usually controls the humanoid robot very carefully not to break it. In the experiment shown above, the applied force at the support hand was set in advance and the value of the force was within the range of force/torque sensor on the wrist. Due to the limitation, the operator could control the robot without considering the degree of the force applied at the support hand. The operator, however, did not recognize the limitation of the workspace of the arm and the waist. If the robot autonomy assists the operator to recognize the limitation of the robot motion, the operator will control the robot more easily. In this section, we consider implementing several autonomous functions as the robot autonomy to assist the operator. First, we consider calculation of the margin for the robot CoG to keep balance in contact with environment. Second, we also consider the position of the robot CoG avoiding extremely large force/torque applied at both hands. Third, we consider calculation of the margin to reach the limit of the range of each joint. A. The margin of the robot CoG for keeping balance In the previous section, we described the method to calculate the robot CoG keeping balance in contact with environment. Now, we obtain the margin for the position of the CoG. Let ṗ G be the velocity of the CoG. By substituting p G = p G + kṗ G into the eq.(2) in the previous section, we can calculate the limit of the scalar parameter k for maintaining the contact with environment. B. The margin avoiding large force/torque at the hand 6 axis force/torque sensors are attached at the wrists and ankles of HRP-3P. The force/torque sensor at the wrists, however, cannot support large force applied by the environment. We apply the constraints on the position of the CoG avoiding large force applied at the wrists. The constraint is applied in a similar fashion as in the previous section. 338
We use eqs.(1), (2), (3) and (4). Here, we apply the following constraints on these equations. f k min f k f k max (9) τ k min τ k τ k max (10) For simplicity, we just focus on the vertical component of force and assume the balance of force. We obtain the margin for the position of CoG just same as the method as explained in the previous subsection. C. The margin to reach the limitation of the joint angle Let ξ k be the linear/angular velocity at the tip of the arms and legs. Let also ξ b and θ be the linear/angular velocity at the base of the arms and legs, the velocity of each axis of the arms and legs, respectively. The velocity at the tip of the arms and legs can be given by ξ k = J bk ξb + J qk θ (11) Let J bk,j qk be the Jacobian matrix of each link structure. For simplicity, let us consider the case where we fix the position of the waist and change the position of the hand. In this case, the displacement of the hand position is limited by J qk (θ min θ) Δξ k J qk (θ max θ) (12) While we use the approximation in this equation, this approximation becomes exact when the joint angle is close to its limit. Also, if we consider obtaining the constraint on the position of the CoG, we assumed that the displacement of the waist is same as that of the CoG. Finally, we show how to apply these constraints on the real robot. We can control the position of the hands and CoG separately [11]. If we consider controlling the position of the CoG, the constraint on the position of the CoG can be obtained by using the above equations. If the margin of the CoG position is calculated as k max,the repulsive force f applied to the master arm can be obtained as the potential function approach [12]. Let K s be the gain matrix. ( 1 f = K s kmax 2 1 ) k0 2 (13) The margin of the joint angle can be estimated using eq.(13). If the angle of a joint is close to its limit, the repulse force acting on the master arm becomes larger. VI. CONCLUSION Our final target is to develop the remote control system for the humanoid robot that can complete given tasks even when the robot is put on unknown environment. We aimed to realize the remote control of the humanoid robot that has a large number of DOF. It is, however, difficult for the human operator to control all DOF simultaneously. In order to reduce the load of the operator, we introduced the control method that generates the whole body motion by less operated DOF than all DOF of the robot. In this paper, we described the whole body operation method to control the humanoid robot in the environment with some obstacles. Finally, the efficiency was confirmed in the experiment using the real humanoid robot and the remote control cockpit we developed. In the proposed method, an operation method can be changed to another method by changing the task and parameters of the subsystem. In addition, we explained the robot autonomy to assist the operator in realizing the restriction of the robot motion. Currently, we are implementing several autonomous functions for assistance of the operator to the robot controller. In the future, we will develop more operation methods for the humanoid robot and examine the method to estimate operation methods suitable for the Operator s-intention in the remote control cockpit system. ACKNOWLEDGMENT The authors thank New Energy and Industrial Technology Development Organization (NEDO) and The Ministry of Economy, Trade and Industry (METI) for their entrusting development of this project and the members cooperating in the project for their constructive support. REFERENCES [1] H. Hasunuma, et al, The Development of the Autonomous-Remote Hybrid Control System for the Humanoid Robot, Proc. Int. Conf. Machine Automation, pp. 285-290, 2004. [2] H. Hasunuma and K. Nakashima, The Tele-operation of the Humanoid Robot Workspace Extension of the Arm with Step Motion, Proc. IEEE- RAS/RSJ Int. Conf. Humanoid Robots, pp. 245-252, 2005. [3] K. Harada, S. Kajita, K. Kaneko, and H. Hirukawa, Dynamics and Balance of a Humanoid Robot during Manipulation Tasks, IEEE Trans. on Robotics, 2006(To appear). [4] T. Takubo, K.Inoue, K.Sakata, Y. Mae, and T. Arai, Mobile Manipulation of Humanoid Robots: Control Method for CoM Position with External Force, Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, pp. 1180-1185, 2004. [5] M.Kuga, et al, Efficient Generation of Whole-Body Reaching Motions for Humanoid Robots, Proc. JSME Conf. Robotics and Mechatronics, p.2p1-1f-f7, 2003. [6] Y. Fukumoto, K. Nishiwaki, M. Inaba, and H. Inoue, Hand-Centered Whole-Body Motion Control for a Humanoid Robot, Proc. Int. Conf. Intelligent Robots and Systems, pp. 1186-1191, 2004. [7] N.E. Sian, K. Yokoi, S. Kajita, and K. Tanie, Whole Body Teleoperation of a Humanoid Robot Integrating Operator s Intention and Robot s Autonomy. an Experimental Verification, Proc. of IEEE/RSJ International Conf. on Intelligent Robots and Systems, pp. 1651-1656, 2003. [8] E. S. Neo, K. Yokoi, S. Kajita, and K. Tanie, A Framework for Remote Execution of Whole Body Motions for Humanoid Robots, Proc. IEEE- RAS/RSJ Int. Conf. Humanoid Robots, 2004. [9] H. Hirukawa, S. Hattori, K. Harada, S. Kajita, K. Kaneko, F. Kanehiro, K. Fujiwara, and M. Morisawa, A Universal Stability Criterion of the Foot Contact of Legged Robots Adios ZMP, Proc. of IEEE Int. Conf. on Robotics and Automation, pp., 2006. [10] H. Hirukawa et al, OpenHRP: Open Architecture Humanoid Robot Platform, Proc. Int. Symp. on Robotics Research, 2001. [11] S. Kajita et al: Resolved Momentum Control: Humanoid Motion Planning based on the Linear and Angular Momentum, Proc. of 2003 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp. 1644-1650, 2003. [12] L. Sentis and O. Khatib, Synthesis of Whole-Body Behaviors through Hierarchical Control of Behavioral Primitives, Int. J. of Humanoid Robotics, vol. 2, no. 4, pp. 505-518, 2005. 339