IROS2019国际学术会议论文集 1098

Multi Contact Stabilization of a Humanoid Robot for Realizing Dynamic Contact Transitions on Non coplanar Surfaces Mitsuharu Morisawa 1 Mehdi Benallegue1 Rafael Cisneros1 Iori Kumagai1 Adrien Escande2 Kenji Kaneko1and Fumio Kanehiro1 Abstract This paper focuses on a stabilization control for multi contact motion which enables a humanoid robot to locomote by realizing dynamic contact transitions on non fl at environment In the stabilization process of the multi contact motion the desired Zero Moment Point ZMP is modifi ed by the position of the Divergent Component of Motion DCM error with respect to the 3D Center of Mass CoM motion generated from the force distribution ratio The contact wrench of each end effector is determined by quadratic optimization considering the centroidal dynamics and contact friction con straints so as to satisfy the modifi ed ZMP Each end effector is controlled by optimized force reference through a projection of null space by force distribution ratio We propose a multi contact stabilization framework which can be designed not only to generate 3D CoM motion but also the CoM position esti mation and the optimal force distribution around the reference ZMP in a unifi ed manner from a balance controller by using the force distribution ratio The effectiveness of proposed method is validated by a quadruped locomotion leaning against a vertical wall using the joint position controlled humanoid HRP 5P in a dynamic simulator I INTRODUCTION Recently many works on multi contact locomotion for hu manoid robots have been presented tackling not only bipedal walking but also multi contact locomotion using both of legs and arms 1 5 A multi contact locomotion allowing for various contacts has the potential to reach much more places compared to simple bipedal locomotion This can be used for the following purposes 1 distribution of the joint load 2 extension of the stable region and 3 collision avoidance with the environment The generation of a multi contact locomotion trajectory which considers physical constraints tends to have a large computation cost Therefore previous works have proposed to generate a feasible trajectory offl ine without stabilization control 1 2 an offl ine trajectory with active compliance 3 4 or an online trajectory generation with stabilization control assuming a fl at fl oor in the range of negligible unevenness 5 Compliance at the end effector contributes to reduce internal forces but can t guarantee the stable contact force transition by itself In most multi contact locomotion realized by humanoid robots the robots are mainly capable of supporting its own weight by vertical reaction force such as ladder climbing quadruped locomotion and so on Recently behaviors like placing the hands against a non horizontal fl oor 6 7 1Humanoid Research Group 2Interactive Robotics Research Group Na tional Institute of Advanced Industrial Science and Technology AIST Tsukuba Japan Corresponding author E mail m morisawa aist go jp Fig 1 Dynamic contact force transition on non coplanar surfaces or stabilizing under a contact of a part of body with the environment 8 have been achieved These motions are still slow or not intended for locomotion In order to realize stable and dynamic locomotion for humanoid robots in a real environment it is important to control appropriately the contact force transitions in the presence of modeling errors between the robots and the environment Nagasaka et al proposed a stabilization control framework for contact force transition throughout a future horizon using Model Predictive Control MPC on horizontal plane 9 This framework still induces a large computation cost Therefore we focus on the multi contact locomotion with non coplanar contacts as exemplifi ed in Fig 1 and con struct a stabilization control framework that can be used for real humanoid robots We extend our previous stabi lization controller for biped walking 11 12 to multi contact locomotion By introducing centroidal dynamics into a force distribution that generates a desired contact wrench a stabilization controller can deal with non coplanar surfaces together with an optimization based inverse dynamics which is widely used for joint torque based humanoid robots 14 In Sec II the 3D CoM trajectory generation we developed in 10 is explained Then a multi contact stabilization control framework is proposed in Sec III The performance of the proposed approach is evaluated on a quadruped locomotion leaning against a vertical wall in Sec IV II ONLINE3D COM TRAJECTORYGENERATION FOR MULTI CONTACT 10 A Centroidal dynamics The centroidal dynamics can be rewritten as mI30 m pG I3 pG LG mg mpG g fo no 1 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 IEEE2252 where pG R3is the CoM position m is the total mass of the robot g 0 0 g Tis the gravity vector and LG R3is the angular momentum around the CoM x generates a skew symmetric matrix from a vector x R3 and fo no R3are the reaction force and torque in the world coordinates respectively Let us suppose L contacts between the robot and the environment The contact wrench of the centroidal dynamics becomes fo no L i 1 fi pi fi ni Jt Jr fc 0 Jt nc 2 where fc fT 1 fT L T R3Land nc nT 1 nT L T R3Lare the sets of contact forces and torques of each link and fi R3and ni R3are the i th contact force and torque pi R3is the i th contact position Jt R3 3L and Jr R3 3Lare the contact Jacobians From the upper part of 2 the contact force can be obtained as fc J t m pG g I3L J tJt fint 3 wherefint R3Lrepresentstheinternalforce J t WJT t JtWJ T t 1 istheMoore Penrose pseudoinversematrixwithdiagonalweightW diag wx1 wy1 wz1 wxL wyL wzL R3L 3L The pseudo inverse matrix J t can be analytically calculated as J t 1 L T 4 where i diag wxi L i 1wxi wyi L i 1wyi wzi L i 1wzi 5 diag xi yi zi R3 3 Let us denote ias a force distribution ratio for the CoM motion We represent the force distribution by a third order polynomial function to realize an appropriate force transition From 1 2 and 3 the angular momentum rate of the centroidal dynamics can be expressed as pG L i 1 pi i pG pG L i 1 pi i g m 6 where LG Jr I3L J tJt fint Jtnc 7 B CoM Trajectory with Variable Height We suppose that a time profi le of the vertical height of the CoM is pre designed In this way the equation of the horizontal CoM motion becomes a linear time variant system LTVS By extracting the CoM motion in the x axis in 6 this can be represented as xG g zG zG L i 1 xipzi xG L i 1 zipxi y m g zG 8 where L i 1 xipzi encodes the virtual height via zh zG L i 1 xipzi i e the denominator mentioned above is the pendulum height In order to synchronize the CoM motion with the actual contact with the environment the CoM trajectory must be generated sequentially However this requires to solve a boundary value problem with a large computation cost at every control time To reduce the variation of the input we calculate a trajectory from past contact parameters Instead of the preview window going from the current step 0 to the future step F in 9 we adopt a preview window going from a past step F to the future step F When the future sequence of a contact position are preplanned together with the force transition and also the angular momentum rate the future ZMP reference as an input piwill be given i e L i 1 zipxi y m g zG The CoM state xF xG F xG F Tafter the F th future step is xF F F x F F 1 i F F i 1 Bipi 9 where k j Ak 1Ak 2 Ajif k j I2otherwise k j F Where Ak R2 2and Bk R2are system matrices obtained by discretizing of 8 see 10 for more details Finally the next state of the CoM at current time t can be calculated as xlong t 1 t 1 t F xlong t F t 1 i t F t 1 t i 1 Bt iplong t i 10 The long term trajectory of LTVS can be calculated uniquely without any tuning of parameters A short term trajectory is generated to track a long term trajectory smoothly which may have the discontinuities mainly caused by the change of a contact position or a contact timing We adopt the variations of the CoM position velocity and the equivalent ZMP position from the long term trajectory at time t as state variables for this purpose xt xt xtpt T 11 Finally the CoM motion at next step can be obtained through the closed loop system using extended system of Akand Bk with a feedback gain Ft xt 1 Atxt BtFt xlong t plong t xt 12 2253 Fig 2 Overview of the control framework III MULTI CONTACTSTABILIZATIONCONTROL In this section we extend the stabilization controller for biped walking 11 to multi contact locomotion The role of a stabilization controller is to generate a feasible wrench for stable contact wrench transitions A Overview of Control framework The overview of the proposed control framework is shown in Fig 2 We suppose that the humanoid robot controls its joint angles at a high frequency in an inner loop and that all the contact wrenches can be measured by force sensors A multi contact state machine manages a support phase or a swing phase at each end effector The waist height and orientation are also generated by synchronizing these support phases A 3D CoM motion is generated by collecting the future sequence of contact positions and timings A balance controller outputs the ZMP to be compensated from the DCM error The contact wrench at each limb will be generated by optimal force distribution A damping controller as a force controller tracks the generated wrench through the null space of the contact position The contact position and orientation of the foot and the hand are changed by the damping controller Then the joint angles are derived through prioritized inverse kinematics and sent to the joint servos B CoM position estimation At fi rst the base link orientation in the world frame is estimated by the evolved algorithm of 13 Then the estimated base link position is simply obtained by merging the difference between the estimated and the desired contact position at each limb according to the desired force distri bution ratio wpest B wpdes B N i 1 i wpdes B i w Rdes i pi ci wpest B i w Rest i pi ci 13 N is number of contact link wR i is the estimated or the desired orientation of the end effector wp B i denotes the relative position between the base link and the end effector origin in the world frame pi ciis the relative constant origin from the end effector origin The estimated CoM position in the world frame can be calculated from the precise model of the robot with the kinematic and the dynamic parameters C Balance control In our control framework we apply the PID controller of the DCM 12 to a balance controller Since we can give the height of ZMP arbitrarily we design the DCM controller in x and y axes independently For instance a balance controller in x axis outputs a compensating ZMP from pctrl x zmp k1 des x est x dt k2 des x est x k3 des x est x 14 where x x x is defi ned as the DCM 3 desand est denotes the desired and the the estimated variables k1 k2 k3are control gains which are calculated as k1 k2 k3 gp gp gp gp gp 15 is equivalent to the natural frequency of the inverted pendulum from 8 at the current time gpis a reciprocal of the time constant which is approximated as a fi rst order delay system due to the damping control and time delay of actuators Finally the reference ZMP pref zmp p des zmp p ctrl zmp 16 is sent to the force distribution which calculates the net wrench of each limb pdes zmp can be generated from the third element of the short term state vector in 11 pctrl zmp pctrl x zmp pctrl y zmp 0 Tis the DCM controlller outputs in 14 D Optimal force distribution We suppose that a torque around the reference ZMP in 16 will be controlled by the contact wrench at each limb In order to consider the support region and the contact force limitations we set a contact pair with friction between the humanoid robot and the environment expressed as a set of points as shown in Fig 3 fo no L i 1 Mi j 1 eij1 eijNj pij eij1 pij eijNj ij ij def ij1 ijNj T 0 17 2254 Fig 3 Coordinates system L and Miare the number of contact links and the contact points at i th link respectively Njis the number of edges of the pyramidal approximation of the cone at the j th contact position and we take Nj 4 4 sided pyramid for all contact points pijdenotes the j th contact position of the i th link eijkis the k th ray generating the polyhedral cone Each contact wrench should satisfy the centroidal dynamics and the contact constraint in 1 and 17 We will generate the desired contact wrench so that the torque around the reference ZMP pcan be close to 0 under these constraints This can be formulated as a quadratic programming with a linear inequality constraint arg min xG p wG xdes G xG 2 wp p 2 w 2 18 subject to 1 17 and 0 Where xG pT G L T G T consists of the linear CoM acceleration and the angular momentum rate around the CoM From 2 and 17 the contact torque around the ZMP can be expressed by the contact force along the edges of the friction cone at each contact point p pref zmp fo no Qp Then the optimization problem in 18 can be rewritten as arg min xG wG xdes G xG 2 wp Qp 2 w 2 19 subject to 1 17 and 0 Finally we can obtain the net wrench at each limb fopt i nopt i i 1 L as a part of 17 from the optimal contact force In case of the unilateral contact in the vertical direc tion the drift of the CoM will occur even if the contact wrench at each contact position can be controlled due to measurement and modeling errors In fact in the previous work 11 instead of the desired vertical force the difference of the force was used in a damping controller during the double support phase In 16 the balance control for the biped robot is implemented in the null space of the contact position We also apply a similar approach to the multi contact stabilization From 7 and 8 the projection matrix can be represented by the force distribution ratio Then the reference force fref ctrl fref 1 ctrl T fref L ctrl T Tsent to the damping controller for a position modifi cation of a contact link can be calculated from fref ctrl ferr fopt fmes 20 where is the projection matrix of the internal force at the contact position fopt fopt 1 T fopt L T Tis the optimal contact force at a local frame of each limb which is converted from 19 fmes fmes 1 T fmes L T T is the measured forces From 3 and 4 this matrix becomes I3L J tJt I3 1 1 1 2I3 2 2 L LI3 L 21 that is analytically calculated by means of the force distribu tion ratio Note that a damping controller modifi es the contact limbs in such a way that the amount of movement at the reference ZMP position can be zero through the projection matrix The reference torque ref ctrl ref 1 ctrl T ref L ctrl T T sent to the damping controller to modify an orientation of a contact link can be calculated from ref ctrl opt mes 22 the contact torque error at a local frame between the optimalandthemeasuredtorques Where opt opt 1 T opt L T Tis the optimal contact torque at a local frame of each limb which is also converted from 19 mes mes 1 T mes L T T is the measured torque from the attached force torque sensor Notice that the reference torque is generated without the projection matrix The weight of the CoM acceleration is set to a suffi ciently large value as compared with the one for the contact force The selection of the CoM acceleration as a decision vari able has the effect of avoiding a lack of solution for the optimization problem that would arise near to the boundary of the contact force inequality constraint In the meanwhile this implies that the DCM controller cannot track the desired trajectory Physically meaningful CoM acceleration will help to modify the locomotion trajectory In Sec II the 3D CoM trajectory generation does not take into account the contact constraints When the locomotion parameters i e contact position and timing are changed 12 may generate a ZMP out of the stable region Therefore the ZMP in 11 is overwritten by the generated optimal contact wrench pt xpt y T no y fo z no x fo z T 23 This saturates the generated ZMP within the boundary of the stable region In this way since it is possible for the CoM to diverge it is necessary to monitor whether its DCM is included in the stable region or not and change the locomotion parameters 2255 Fig 4 Block diagram of 2DOF damping controller E Damping control We suppose that the end effector will move until a contact with the environment is detected under a constant velocity to absorb errors of the contact position 10 That is there is an impact at every landing In order to improve the simple damping controller of 11 it was adapted to two degree of freedom 15 PI and P controllers are applied to the wrenche error and to the measurement wrench respectively to generate a velocity an angular velocity of the contacting end effector Let us consider a one dimensional model to simplify the discussion about chacteristics of this controller This block diagram is shown in Fig 4 We assume that the reaction torque is equivalent to the measurement torque and that it is proportional to the relative displacement between the end effector and the contact surface by means of the stiffness of the environment ke qeis also assumed as an environmental disturbance as the same dimension of the modifi ed position qmod mes ke qmod qe 24 The sensitivity of this controller can be expressed as mes qe kes2 s2 ke kp ks s keki 25 When the stiffness of the environment is high 25 becomes mes qe ke s kp ks s s K K ki kp ks 26 The fi rst term on the right side of 26 is the same as the simple damping controller and second term consists on a high pass fi lter The response of the torque control is improved by the high pass fi lter In the actual implementation the vector and the ori entation matrix of the end effector in the cartesian space will be modifi ed from the damping controller by replacing ref fref ctrl ref ctrl mod vmod mod and qmod pmod Rmod vmod kp ki s fref ctrl ksf mes 27 mod kp ki s