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 AbstractThis 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 13 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 1318. Moreover, formulating the inverse-dynamics problem as a quadratic program (QP) optimization 1922 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 forcespushing, 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 Rnnis 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 Rcn 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 NTo,(2) =eJT( x+ + p) + e NTo,(3) where xand Fare 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 = SA1(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= IeJTeJT; 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(c6)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(c6)(c6)is dynamically-consistent inverse of ScJT cSTv2; and Sc R(c6)cis the selection matrix for contact wrench components to be modifi ed by the reference contact wrench vector, F c R(c6). Given xand F c as control commands, the actual joint control torque can be composed as follows: = t+ gc+ c,(6) = eJT x+ e NTo + 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 xand F c are determined in the consecutive manner 4653 since cis calculated after tis obtained. Accordingly, certain xcannot 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 iWii, (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 NTo + gc+ v2eVFopt c ,(14) Fc=JT cS T(e JT xopt+ e NTo+ gc) + Fopt c cpc, (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 R6cis the grasp map matrix, Rcj R33is the rotation matrix of the j-th contact with respect to arbitrary coordinates in which the resultant contact wrench will be expressed, Pcj R33is 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 xis 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 xto 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 humanoidpushing, 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 locationse.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 = IG2/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= xmymzmT) and other tasks such as the trunk orientation; the corresponding reference xis then defi ned accordingly. The components x mand zmof x have to be chosen as lar