IROS2019国际学术会议论文集 1646

Agile Standing up Control of Humanoids Energy based Reactive Contact Wrench Optimization with Strict Dynamic Consistency Yisoo Lee Nikos Tsagarakis and Jinoh Lee Abstract This paper presents a dynamic whole body control method for humanoids to render agile and stable standing up motion based on energy concepts First to cope with the standing up problem with multiple contacts in hierarchical tasks an enhanced operational space based whole body control WBC framework is proposed which offers optimal torque resolutions guaranteeing strict dynamic consistency with in equality constraints formulated by quadratic programming Second agile standing up control strategy with dynamic push and rise actions is newly developed based on the notion of the total energy The optimal pushing wrenches at contacts are computed to obtain suffi cient energy to accelerate the center of mass CoM of the robot as quick as possible and the total energy is then controlled to attain rapid rise up motion and to stabilize the body of the robot Consequently the robot can effectively and actively stand up to recover from a certain pose in which cannot be accomplished by any quasi static motion The proposed method is numerically experimented and validated with dynamic parameters from the real humanoid COMAN fulfi lling different types of standing up actions I INTRODUCTION A recent trend in robotics research encourages to deploy legged robots into the real world and practical applications such as disaster responses elderly care and hazardous envi ronment maneuvering become more highlighted Particularly for humanoid robots it is of great importance to overcome inevitable failure scenarios such as falling over due to its conceptual challenges of bipedal structure and the robots are compulsory to autonomously recover by itself and to continue the operation accomplishing their tasks without human intervention In this regard a standing up action is one of the chal lenging tasks to render versatility and practicality into hu manoids because the robot can often face diverse situations necessitating stand up such from crouching kneeling sitting prone lying down or even leaning postures during the task executions or as post failure recovery procedures There have been pioneering works 1 3 that the humanoid robot is controlled to imitate a human like standing up routine where the center of mass CoM projected on the ground is controlled to move into the supporting polygon and contact transition motion is consecutively created 4 This is an intuitive and simple method yet motions are generally slow and specifi c joints such as knee and waist demand high torques 5 This work was supported by European Unions Horizon 2020 research and innovation programme under grant agreement No 779963 EUROBENCH and No 644727 CogIMon AuthorsarewiththeDepartmentofAdvancedRobotics Isti tutoItalianodiTecnologia Genoa16163 Italy yisoo lee nikos tsagarakis jinoh lee iit it To mitigate heuristic and laborious motion planning of the standing up there have been interesting approaches based on machine learning which are for example applied to the under actuated three link robot with a fi xed base 6 or small sized humanoid robots 7 8 There also have been researches to emulate dynamic postural movements demonstrated by humans from analysis of motion capture data and reaction forces of human standing up performance where more dynamical motions such for sit to standing 9 11 and roll and rise 12 are successfully demonstrated in numerical simulations and impressive experiment scenarios Nevertheless a large number of off line training is generally required for successful learning results and there exists a risk of failure in obtaining optimal data sets from real experiments due to its high dimensional action space of the humanoid robot Besides the dynamics of fl oating base humanoids with multiple contacts and high kinematic redundancies are often disregarded which is important to render generic standing up actions respecting diverse dy namic constraints In this paper we focus on a whole body control WBC strategy to bestow agile and dynamic standing up action into humanoids exploiting optimal reactive force generation which assesses the multi contact multi body dynamics of the fl oating base robot under realistic physical constraints Foremost taking a deeper look into the standing up control problem one can refi ne it as control of the ground projected position of the center of mass CoM driving into the bound ary of the support polygon of the feet as well as guaranteeing its suffi cient height from the ground However the CoM initially located out of the foot support polygons e g sitting or lying postures cannot be physically propelled unless any reaction wrenches are added by supplemental contact points over those of feet such as pushing forces by hands Therefore control of the CoM task with multi contact wrenches and their state transitions are crucial to create effective whole body standing up motion In this light we develop a standing up control method rendering agile entire body motion exploiting the optimiza tion of reactive contact wrenches such as pushing forces at hands Mainly we strive to build following two contributions 1 a proposition of an enhanced WBC framework providing optimal torques under certain equality and inequality con straints as well as guaranteeing strict dynamic consistency in hierarchical tasks and 2 a standing up controller based on energy control concepts encompassing pushing quick rising up and stabilizing phases To effectively compute the torques for the dynamic whole 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 IEEE4652 body motion of the humanoid with its multiple contacts executing several hierarchical tasks there have been notice able progresses based on inverse dynamics control and op erational space formalism 13 18 Moreover formulating the inverse dynamics problem as a quadratic program QP optimization 19 22 has been established to cope with the full multi body dynamics and contact interactions along with inequality constraints In this paper we draw particular attention to the operational space based WBC OS WBC framework 14 23 24 This offers strict dynamic con sistency among multiple interaction tasks which is essential in the standing up problem as aforementioned whereas inequality constraints cannot be explicitly taken into account Accordingly we develop an enhanced OS WBC framework which is formulated as a QP to compute optimal torques un der both equality and inequality constraints strictly assuring the dynamic consistency Within the new OS WBC framework we propose an energy based control strategy attaining agile and versatile standing up capability to recover from generic unstable postures with supplemental interaction forces pushing dy namic rising up and stabilizing This intends that the robot can quickly and stably obtain suffi cient energy to control the dynamic CoM motion by generating maximum pushing wrenches with consideration of dynamically consistent con straints of the multi contacted body such as friction cones and the center of pressure CoP II A NEWWHOLE BODY CONTROL FRAMEWORK OF DYNAMICALLYCONSISTENTOPTIMIZATION As a basis to create holistic standing up actions the robot is controlled via the OS WBC framework 14 23 24 In this section the conventional one is fi rst reviewed in brief and an enhanced method is proposed to deal with diverse constraints by amalgamating a QP optimization approach A Review of the OS WBC Framework A fl oating base robot which has n k 6 DoFs k joints and c DoFs contact can be described as the following rigid body dynamics equation A q b g JT cFc b 1 where q Rnis the joint angle vector including virtual joints of the fl oating base A Rn nis the inertia matrix b Rnis the Coriolis centrifugal force vector and g Rn is the gravity force vector b R6is the virtual joint torque vector that is attached from inertial frame to the robot base frame Rkis the actual joint torque vector while Jc Rc n is the contact Jacobian matrix defi ned by xc Jc q xc Rcis the vector of contact positions and orientations and Fc Rcis the contact wrench vector The torque solution controlling operational space tasks with rigid contact constraints i e xc xc 0 is given as follows 14 24 t eJTF e NT o 2 eJT x p e NT o 3 where x and F are the vectors denoting reference ac celeration and the reference force in the operational space respectively f JTdenotes the dynamically consistent inverse of JTSTgiven by the weighted pseudo inverse solution with the weighting matrix W SA 1 I JT c JT c ST which minimizes the acceleration energy where denotes the dynamically consistent inverse J is the Jacobian matrix defi ned as x J q with given operational space coordinate x S is the matrix to select the actuated joints p are the operational space vectors of the Coriolis centrifugal force and gravity force respectively while is the contact constraint projected inertia matrix in the operational space e NT is the null space projection matrix defi ned as e NT I eJTeJT and 0is the arbitrary torque vector Considering a PD type controller to achieve task x one can design the operational space acceleration reference as x kp x x kv x 4 where kpand kvare control gains Note that since the OS WBC torque 3 only copes with equality contact constraints it is crucial to determine the proper contact wrench distribution torque c Regarding this authors in 24 present that the contact wrenches of c 6 DoFs can be modifi ed to distribute them exploiting the redundancies in W when c 6 i e dynamically consistent contact null space without any interference among the tasks The contact wrench distribution torque can be obtained with following equation c v2eVF c 5 where v2 Rk c 6 is the part of a unitary matrix without interferences to its singular values which is de composed from W by the singular value decomposition e V R c 6 c 6 is dynamically consistent inverse of Sc JT cSTv2 and Sc R c 6 cis the selection matrix for contact wrench components to be modifi ed by the reference contact wrench vector F c R c 6 Given x and F c as control commands the actual joint control torque can be composed as follows t gc c 6 eJT x e NT o gc v2eVF c 7 where gcis the torque vector to compensate gravity and Coriolis centrifugal forces Note that and p in 3 are compensated by gcin this equation In addition the contact wrench in 1 represented as a function of can be calculated as Fc JT cS T c pc 8 where c pcare vectors of the Coriolis centrifugal force and the gravity force projected on the contact space respectively B Proposition of an OS WBC framework of Dynamically Consistent Optimization In the conventional OS WBC framework control com mands x and F c are determined in the consecutive manner 4653 since cis calculated after tis obtained Accordingly certain x cannot guarantee feasible solutions of F cto satisfy inequality constraints such as CoP friction cones and joint torque limits To determine optimal task references assuring inequality constraints as well as the dynamic consistency among the hierarchical tasks we propose a new QP based OS WBC framework This indeed offers optimized control commands for acceleration xoptand contact wrench Fopt c The QP problem for OS WBC is formulated as follows min xopt Fopt c m X i 1 T iWi i 9 where m 4 Wi denotes a weighting matrix defi ned as W1 W2 W3 W4 and the objective is to fi nd the vector igiven as 1 xopt x 2 fc 3 1 4 mc 10 11 12 13 where fcis the vector of contact forces 1is the reference joint torque vector in the previous sampling period and mc is the vector of contact moments The cost function 9 is minimized subject to eJT xopt e NT o gc v2eVFopt c 14 Fc JT cS T e JT xopt e NT o gc Fopt c c pc 15 0 R c1 0 Pc1Rc1 Rcn0 PcnRcn Fopt c QFopt c 16 min max 17 fc z j f2 c x j f2 c y j 1 2 s fc z j mc z j mc y j fc z j xmin p xmax p mc x j fc z j ymin p ymax p j 1 cn 18 where Q R6 cis the grasp map matrix Rcj R3 3is the rotation matrix of the j th contact with respect to arbitrary coordinates in which the resultant contact wrench will be expressed Pcj R3 3is the skew symmetric matrix of the distance vector from the arbitrary coordinate to the location of j th contact min maxare lower and upper limits of the joint torque respectively s is the static friction coeffi cient xmin p xmax p ymin p ymax p are the lower and upper boundaries of the CoP in x and y axis respectively ymax p is the upper boundary of the CoP in y axis cnis the total number of contact link and fc jis the contact force at the j th contact The subscript x and y denote the tangential directions at the contact and that of z denotes the normal direction to the contact plane while mc jis the contact moment at the j th contact along direction x y z axes Fig 1 Three phases for standing up motion control x y O a b c Fig 2 a The global coordinates for standing up control b the inverted pendulum model in xz plane with a constant distance between pivot point and CoM of the pendulum and c the linear inverted pendulum model regulating the height of the CoM This optimization targets that the difference between xopt and x is minimized by 10 while the magnitude of linear forces at contacts 11 the amount of joint torque changes 12 and the magnitude of contact moment 13 are minimized With the two equality condition in 14 15 which are derived from 7 8 the calculated results follow the laws of the contact consistent operational space control scheme The term related to cin 14 provides the torque solution that does not interfere in the operational space tasks Note that for the given operational space tasks and robot states required resultant reaction wrenches from the contacts should have a unique solution i e QFc Therefore the constraint 16 guarantees that the resultant contact wrench is not affected by Fopt c The overall torque commands are limited by the inequality constrains in 17 and contact conditions related to CoP and friction cone are refl ected by 18 Finally whole body joint control torques through the dynamically consistent optimization are determined by re placing control commands x to xoptand F c to Fopt c in 7 III AGILESTANDING UPCONTROL To create and control the agile standing up motion of the humanoid this paper considers three phases of the humanoid pushing rising and stabilizing as illustrated in Fig 1 The global coordinate frame O is set at the center point of the feet as depicted in Fig 2 a where its x axis is aligned with the vector from the origin to the ground projected CoM while the z axis is parallel to the gravity yet opposite upward direction By redirecting the y axis one can then express the 3D standing up motion in xz plane In the following subsections we propose an energy based standing up control strategy governing three phases 4654 A Pushing Phase In this phase the robot uses contact wenches to recover the body posture as quickly as possible from initial pose whose CoM is out of the support polygons e g leaning sitting or prone postures More specifi cally the robot demands maximum acceleration of the CoM regarded as the inverted pendulum motion to obtain energy with pushing wrenches at contact locations e g hands The required energy for the greatest possible CoM accel eration can be determined in the inverted pendulum model in xz plane as depicted in Fig 2 b whose equation of the motion without any control input is expressed as follows IG mglsin 19 where IGis the moment of inertia of the pendulum m is the mass of the pendulum g is the gravitational acceleration l is the constant length between the CoM and its pivot point and is the pivot angle The system energy is then written as E IG 2 2 mgl cos 1 20 Note that in 20 for convenience the potential energy is regarded as zero at the upright standing position and accordingly the energy of the initial posture is negative 25 To ensure a static upright standing position of the pendu lum the total energy of the system in the given fi nite time should be controlled by pushing wrenches to be the same as that of the standing position that is 0 and E 0 When E becomes zero no additional energy injection is requested Therefore the pushing phase terminates if E 0 during the standing up control From the above observation of the energy we develop whole body pushing control of the full humanoid incor porating with the new OS WBC framework introduced in Section II B Hands and feet are generally considered as contact links to compose contact Jacobian matrix in the whole body controller The number of contacts cn may vary depending on the confi guration of the robot whereas at least contacts at one hand and one foot must be guaranteed to generate feasible pushing motion in this phase The operational space task vector x includes the position of the CoM xm xmymzm T and other tasks such as the trunk orientation the corresponding reference x is then defi ned accordingly The components x mand z mof x have to be chosen as large constant values One then can obtain maximized acceleration xopt m and zopt m according to the objective function of the QP optimization 10 Whereas the rest of the acceleration components is determined by feedback control 4 to track the assigned x To control the CoM with the maximal acceleration emulat ing the motion of the inverted pendulum in the QP formula tion in Section II B a scalar variable 5with the weighting value W5and an additional constraint are introduced for the standing up as follows x m tan ctrl z m 21 where ctrl is used to defi ne a reference direction of the CoM in x z plane is slack variable then we set 5 to be minimized and W5is the same level as W3 To regulate the distance of the CoM position of the robot lm px2 m z2m as that of the inverted pendulum model l ctrlis controlled by the following equation ctrl k0 p l lm k 0 v lm 2 22 where k0 p and k0 v are control gains Note that the