IROS2019国际学术会议论文集0670

Whole-Body Control of Humanoid Robot in 3D Multi-Contact under Contact Wrench Constraints Including Joint Load Reduction with Self-Collision and Internal Wrench Distribution Naoki Hiraoka, Masaki Murooka, Hideaki Ito, Iori Yanokura, Kei Okada and Masayuki Inaba AbstractIn this paper, we propose an approach for online whole-body control of position-controlled humanoid robot with 3D multi-contact to cope with contact wrench constraints and joint overload. In our method, robots are controlled under contact wrench constraints with three features: 1) internal wrench control to reduce joint load and prolong the time in which the high-load postures can be maintained 2) feasible utilization of self-collision to reduce joint load by turning off joint servo gains 3) handling degenerated degree of freedom by solving a quadratic optimization problem integrating wrench distribution and inverse kinematics in which internal wrench is controlled only in controllable directions. With our methods, HRP2-JSKNTS could pick up an object under a desk with squatting with the back of the upper leg on the back of the lower leg without sliding at the right arm. We also evaluated the effectiveness of our control to reduce joint load with another experiment. I. INTRODUCTION Humanoid robots are made to imitate the joint structure of humans. One reason for this is to operate in humans living environments. Humans living environments are designed to suit for the humans shape and the way of supporting their bodies, so humanoid robots need to support their bodies in the way humans do. Bending knee joints to reach objects near the ground, making hands contact with the environment to support its body, and squatting with self-collision between the upper leg and the lower leg to reduce load is an example of such postures (Fig. 1). Such 3D multi-contact postures need caution against contact wrench constraints and joint overload. Since the normal force working at vertical planes is relatively small, when robots support their bodies with hands contacting with vertical planes, the contact points are easy to slide and this may cause the robots to fall. When robots support their bodies with relatively low power joints (e.g. arm joints) or in postures with long moment arms (e.g. bending knee posture), the load of joint motors is large so motor temperature will increase and this may cause motors to overheat. In this paper, we propose an approach to cope with these problems for position-controlled humanoid robots. A. Joint Load Reduction To prevent motor overheat, some approaches to reduce motor load are proposed by previous studies. Adaptive N.Hiraoka,M.Murooka,H.Ito,I.Yanokura,K.Okadaand M.InabaarewithDepartmentofMechano-Informatics,TheUni- versity of Tokyo, 7-3-1 Hongo, Bunkyo-ku, Tokyo 113-8656, Japan hiraokajsk.imi.i.u-tokyo.ac.jp Fig. 1. HRP2-JSKNTS supports body with the right hand, and self-collision between the upper leg and the lower leg. torque limitation 1 can prevent motors from reaching fatal temperature. The limit of adaptive torque limitation is the lack of feasibility of whole-body balance. To guarantee both whole-body balance and motor overheat prevention, online optimization of position of the centroid based on joint motor temperature is proposed 2. This method balances joint load and prolongs the time in which robots can maintain the posture. In this paper, we propose two other approaches for joint load reduction. One of our approaches focuses on internal wrench instead of position of the centroid 2. Whole-body multi-contact postures have closed loops and internal wrench is available to balance joint load. The other is the utilization of self-collision. Convention- ally, self-collision is recognized as something to be avoided. However, self-collision have the potential for joint load reduction because the contact points between colliding links can support the mass of the robot. Fig. 1 shows an example of the utilization of self-collision: a humanoid robot squats with the back of the upper leg on the back of the lower leg, and load of knee joints is reduced. 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Macau, China, November 4-8, 2019 978-1-7281-4003-2/19/$31.00 2019 IEEE3860 B. Contact Wrench Constraints There are many surveys of offl ine planners to generate whole-body motions under contact wrench constraints. For online controllers, online 3D multi-contact control under contact wrench constraints is achieved for torque- controlled robots 34. Online controllers under contact wrench constraints for position-controlled robots are diffi cult because position- controlled robots cannot control force/torque directly. (1) is the equation of motion of a fl oating link system like humanoid robots. M q + b = ? O6 ? + JTw.(1) q = qT v qT jT is the sequence of the velocity of fl oating link virtual joint and velocity of the robots joints. M is mass matrix. b is Coriolis force and gravity force. = 1.nTis the sequence of joint torque. J = JT 1 .JT mT is jacobians of the position and rotation of each contact points, and w = wT 1 .wT mT is the sequence of contact wrench of each contact points. The problem of position- controlled robots is that and w on the right-hand side of (1) have redundancy so and w are not determined uniquely by the commanded joint angles q. Even if q is calculated by any motion planners or controllers based on (1) with a feasible reference combination of and w, this redundancy may cause actual contact wrench w to violate contact wrench constraints or cause actual joint torque /Gamma to violate min-max limits. To control contact wrench of position-controlled robots, modifying command positions of contacting end-effectors compliantly is used in previous studies 5678. In this paper, we use this compliant control to cope with the redun- dancy of and w. We propose a method which combines this compliant control and wrench distribution control derived from (1). Our method can optimize both contact wrench, joint torque, and joint angles. Our method can work when robots utilize self-collision and degree of freedom degenerates. This is because our method searches optimal wrench distribution by only using controllable directions9. C. Our Approach In this paper, we assume that the reference trajectory of joint angles, torques and wrenches is given by some motion planner under constraints of contact wrench and joint overload. We explain our online 3D multi-contact control method for position-controlled robots to balance joint load and meet contact wrench constraints in section II, and the method of utilizing self-collision in section III. We show the results of two experiments which show the effectiveness of our method in section IV. II. ONLINE3D MULTI-CONTACTCONTROLMETHOD FORPOSITION-CONTROLLEDROBOT In our method, we solve both inverse kinematics and optimal contact wrench / joint torque distribution in single quadratic programming in each control loop. The resul- tant optimal joint angles are commanded to the position- controlled robot in each control loop. TABLE I NOTATIONS INTHISPAPER Notation Defi nition nnumber of joints mnumber of contacting end-effectors eefnumber of end-effectors dttime period of each control loop iangle of joint i qjoint angles Rn+6 itorque of joint i joint torques Rn fiforce of end-effector i R3 nimoment of end-effector i R3 wcontact wrenches R6m pintpositions and rotations of non-contacting end-effectors R6(eefm) psuppositions and rotations of contacting end-effectors R6m esuperrors between pc supand prsup R6m cposition of centroid R3 Jjacobian Wweight matrix Sselect matrix Dmatrix of feedback gains mct,imaximum continuous torque of joint i amount of modifi cation in a control loop r reference value planed offl ine aactual value ttarget value for optimization ccommand value A. Quadratic Problem of Inverse Kinematics and Optimal contact wrench / joint torque Distribution in Controllable Directions The quadratic objective function of our method is min qc kmax i (| a i + c i mct,i |)k2 Wl+ k a + ck2 W +kwa+ wck2 Ww+ kS(p t sup p c sup)k 2 Wpsup +kct cck2 Wc+ kp t int p c intk 2 Wpint +kqt qck2 Wq. (2) Constraints are wc= SD( 1 dtJsupq c + 1 T esup).(3) ? O6 c ? + JTwc= 0.(4) S(wa+ wc) ContactConstraints.(5) min5 a+ c5 max.(6) qmin5 qc dt 5 qmax.(7) qmin5 qc prev+ q c 5 qmax.(8) The next command joint angles qcare determined by the equation qc= qc prev+ q c. (9) ais the sequence of actual joint torque and wais the sequence of actual contact wrench, which can be measured in each control loop. Note that the optimization variable is only joint angles qc. Joint torques and contact wrenches 3861 are not included in the optimization variable, and they are linearly depending on joint angles qcby (3)(4). We explain this QP in the following sections. B. Combination of Compliant Control and Internal Wrench Distribution Control In internal wrench control, since position-controlled hu- manoid robots cannot control joint torque and contact wrench directly, we realize commanded wrench for each end-effector compliantly by Damping Control 10. Damping Control is described in following equations (10)(11)(12)(13). d dtei,P =D1 P (fa i fc i) 1 TP ei,P.(10) Pc i =Pr i + Rr iei,P. (11) d dtei,R =D1 E (na i nc i) 1 TR ei,R.(12) Rc i =Rr iRrpy(ei,R). (13) In (10), ei,P is spatial position modifi cation of end-effector i from the reference position in the local frame. fa i is actual force and fc i is command force in the local frame. D1 P is a feedback gain and TPis a time constant to retrieve neutral points. This modifi cation is applied by (11) and command position Pc i is computed. In (12), ei,Ris Euler angles which represent rotational posture modifi cation of end-effector i in the local frame. na i is actual torque and nc i is command torque in the local frame. D1 E is a feedback gain and TEis a time constant to retrieve neutral points. This modifi cation is applied by (13) and command orientation Rc i is computed. Rrpycalculates a rotation matrix with given roll-pitch-yaw angles. The relationship between qcand wcin (3) is obtained from (10)(12). Matrix S is select matrix and described later. Jsupis jacobian of position and rotation of actually contact- ing end-effectors. esup is previous modifi cations of actually contacting end-effectors (esup= Rr 1 i (pc supprev prsup). (4) is to avoid confl ict between compliant control and other controls (e.g. centroid position control). This equation is corresponding to the right-hand side of (1), and restricts space of contact wrench and joint torque controlled by compliant control to the redundant space of (1). With the presence of (4), compliant control can be used to control only internal wrench. This is the difference between this approach and those of previous researches 68. We have to point out that (4) is not correct in dynamical motion. qc is projected to the amount of modifi cation of contact wrench wc by (3) and to the amount of modifi cation of joint torque cby (4), so wcand care restricted within joint movable directions. As a result, optimal wrench distribution without violating geometric solvability can be achieved. C. Internal Wrench Control for Joint Load Reduction In this section, we explain our method of reducing excess internal wrench to balance joint load for motor overheat prevention. Balancing joint load is executed by the fi rst and second terms of (2) (kmaxi(| a i+ c i mct,i |)k2 Wl+ k a + ck2 W). Our strategy of internal wrench control for joint load reduction is: 1) The length of time in which robots can maintain the posture without overheat should be maximized. 2) Posterior to 1, the less maximum continuous torque 11 a joint can exert, the less joint load should be distributed. First, we explain maximum continuous torque 11. Max- imum continuous torque is the maximum joint torque which the joint can exert permanently without motor core temper- ature reaching the maximum operation temperature Tmax. Maximum continuous torque can be calculated from Two Resistor Thermal Model 12. Two Resistor Thermal Model is expressed by (14). C1 dT1 dt =Qin T1 T2 R1 .(14) C2 dT2 dt = T1 T2 R1 T2 Tair R2 . T1is motor core temperature and T2is motor housing temperature. Tairis constant air temperature. C1and C2 represent thermal capacitance of the motor core and the motor housing. R1and R2represent the thermal resistance between the motor core and the motor housing, and between the motor housing and the air. Qinis input calorie. Qin= Re( K )2.(15) Reis electric resistance of the motor coil, and Kis torque constant of the joint. is the joint torque. From (14)(15), maximum continuous torque mctis cal- culated to satisfy (16). Re(mct K )2= Tmax Tair R1+ R2 .(16) We used the parameters on data-sheets as Tmax, C1, C2, R1, R2, Re, K. We assume that the proportion of joint torque to maximum continuous torque mctis related to the length of time in which the joint can maintain the posture without overheat. With this assumption, minimizing the maximum proportion among all joints will lead to maximizing the length of time in which robots can maintain the posture without overheat. The fi rst term of (2) (kmaxi(| a i+ c i mct,i |)k2 Wl) represents this. We weight joint torques in cost function according to maximum continuous torque. W,i= 1 mct,i2 .(17) The second term of (2) (ka+ ck2 W) represents this. D. Contact Wrench Constraints: Realizing Plane Contact The constraint (5) is to meet contact constraints. The third term of (2) (kwa+wck2 Ww) is to reduce friction force and to keep the center of pressure close to the middle of contact plane of the end-effector to have the margin of contact wrench constraints. 3862 The fourth term of (2) (kS(pt sup pc sup)k2Wpsup) controls the posture of end-effectors that are expected to meet plane contact with the external world but actually meet line contact. Line contacting end-effectors need to be modifi ed their posture to meet the plane contact. pt supis the sequence of target position modifi cation of each contacting end-effectors. Kinematics errors of the external world and the robot model cause the positions of each contact point to get out of planned position. As a result, end-effectors of the robot whose surface plane is planned to contact with the external world may not contact actually or may meet only the line contact instead of the plane contact. These situations make the robot unstable and there is a risk of a fall. If the end-effector is meeting plane contact, the corresponding component of pt supis zero. But if the end-effector actually has line contact, the corresponding component of pt sup becomes the motion to rotate toward the plane contact. Assuming Z-axis is the normal direction of the contact plane, this algorithm can be described as (18). c xi = ? vrx(if Ca y,i upperthre) (18) c yi = ? vry(if Ca x,i upperthre) vry(if Ca x,i 0. If the position of center of pressure is out of threshold, the end-effector is considered to have line contact and commanded to rotate toward plane contact. Since position of center of pressure cannot be controlled along the line contact direction, we introduce select matrix S to handle line contact. S selects forces of XYZ direction and torques of Z direction. In addition, S selects torques of XY direction only if the actual position of center of pressure is inside threshold in that direction, which means plane contact along that direction. pc sup is the sequence of the amount of modifi cation of end-effectors in a control loop and described as pc sup= SJsupq c. (19) E. Other Terms and Constraints The fi fth term of (2) (kctcck2 Wc) controls the position of centroid. Kinematics error of the external world and robot model causes the positions of centroid to deviate from the planned stable position. The purpose of the fi fth term of (2) is to prevent this and to keep the centroid close to the planned stable position. ccis expressed as cc= Jcomqc.(20) Jcomis centroid Jacobian. ctis the target centroid velocity obtained from (21). ct dt = k(cr ca) + ct dt .(21) cr is the reference position of centroid planned offl ine and ca is the actual position of centroid. k is a constant gain. This control law is simple and doesnt care the acceleration of cen- troid, so this controller doesnt function as quick stabilization or elimination of vibration. However, it is enough because our target motion is relatively slow and stable because of multi-contact supports. The sixth term of (2) (kpt int pc intk2Wpint) controls the positions and rotations of actually non-contacting end- effectors. pc int is expressed as pc int= Jintq c. (22) Jintis jacobian of position and rotation of actually non- contacting end-effectors. pt int is the target velocity of those end-effectors. pt int is usually determined by the impedance control Algorithm, but if end-effectors planned to contact with the external world are not actually in contact, components of pt int corresponding to those end-effectors are determined by (23). pc zi dt = vz(if fa z,i 0. If fa z,i is under the threshold, the end-effector i is considered not to contact and commanded to move toward contact direction -Z. The seventh term of (2) (kqt qck2 Wq) controls the redundancy of other terms. qtis the velocity following reference angles qr. The constraint (6) is to keep joint torque inside hardware constant limits. (7)(8) is joint velocity and position limita- tions. F. Prioritization of tasks To prioritize the above tasks, we confi gure the weight of objective function (2) to Wl W,Ww Wpsup,W cxy Wpint W cz Wq. (24) The reason that W cz is in the low priority is that Z-axis position of centroid doesnt affect the balancing of robots in quasi-static motions.