Nonlinear Model Predictive Control for Robust Bipedal Locomotion: Exploring Angular Momentum and CoM Height Changes
Jiatao Ding, Chengxu Zhou, Songyan Xin, Xiaohui Xiao, Nikos Tsagarakis
NNonlinear Model Predictive Control for Robust Bipedal Locomotion:Exploring Angular Momentum and CoM Height Changes
Jiatao Ding a,b , Chengxu Zhou c , Songyan Xin d , Xiaohui Xiao a ∗ and Nikos Tsagarakis e Abstract — Human beings can utilize multiple balance strate-gies, e.g. step location adjustment and angular momentumadaptation, to maintain balance when walking under dynamicdisturbances. In this work, we propose a novel Nonlinear ModelPredictive Control (NMPC) framework for robust locomotion,with the capabilities of step location adjustment, Center of Mass(CoM) height variation, and angular momentum adaptation.These features are realized by constraining the Zero MomentPoint within the support polygon. By using the nonlinearinverted pendulum plus flywheel model, the effects of upper-body rotation and vertical height motion are considered. As aresult, the NMPC is formulated as a quadratically constrainedquadratic program problem, which is solved fast by sequentialquadratic programming. Using this unified framework, robustwalking patterns that exploit reactive stepping, body inclination,and CoM height variation are generated based on the stateestimation. The adaptability for bipedal walking in multiplescenarios has been demonstrated through simulation studies.
I. I
NTRODUCTION
Humanoid robots have attracted much attention for theircapabilities in accomplishing challenging tasks in real-worldenvironments. With several decades passed, state-of-the-artrobot platforms such as ASIMO [1], Atlas [2], WALK-MAN [3], and CogIMon [4] have been developed for thispurpose. However, due to the complex nonlinear dynamicsof bipedal locomotion over the walking process, enhancingwalking stability, which is among the prerequisites in makinghumanoids practical, still needs further studies. In this paper,inspired by the fact that human beings can make use ofthe redundant Degree of Freedom (DoF) and adopt variousstrategies, such as the ankle, hip, and stepping strategies,to realize balance recovery [5]–[7], we aim to develop aversatile and robust walking pattern generator which canintegrate multiple balance strategies in a unified way.To generate the walking pattern in a time-efficient man-ner, simplified dynamic models have been proposed, amongwhich the Linear Inverted Pendulum Model (LIPM) is widelyused [8]. Using the LIPM, Kajita et al. proposed the previewcontrol for Zero Moment Point (ZMP) tracking [9]. Byadopting a Linear Quadratic Regulator (LQR) scheme, theankle torque was adjusted to modulate the ZMP trajectoryand Center of Mass (CoM) trajectory. As a result, robust a School of Power and Mechanical Engineering, Wuhan University,Wuhan, China. b Shenzhen Institute of Artificial Intelligence and Robotics for Society,ShenZhen, China. c School of Mechanical Engineering, University of Leeds, Leeds, UK. d School of Informatics, The University of Edinburgh, Edinburgh, UK. e Humanoid and Human Centered Mechatronics Research Line, IstitutoItaliano di Tecnologia, Genoa, Italy
Corresponding Author : [email protected] walking can be achieved in the presence of external pertur-bations. Nevertheless, this strategy can neither modulate thestep parameters nor take into consideration the feasibilityconstraints arisen from actuation limitations and environ-mental constraints. To overcome this drawback, Wieber etal. proposed a Model Predictive Control (MPC) algorithm toutilize the ankle strategy [10] and then extended it for adjust-ing step location [11]. Since then, by utilizing the steppingstrategy (step location adjustment), stable walking on theuneven surface, unknown slope, and balance recovery fromexternal pushes have been realized [12]–[15]. However, theabsence of angular momentum adaptation and vertical heightvariation limits the capability against dynamic disturbances.The angular momentum adaptation, which is also calledas the hip strategy, plays a vital role in enhancing walkingrobustness against external disturbances [5]. Based on thegeneral ZMP dynamics which took into account the momentaround the CoM, a Quadratic Programming (QP) algorithmwas employed in [16] to determine the body inclinationstate. Then, by using the simplified Linear Inverted Pendulumplus Flywheel Model (LIPFM), the roll and pitch angularmomentum adaptation were realized by rotating the upper-body [19]. Focusing on the hip strategy, Li et al. proposed anopen-loop control algorithm for rejecting the external pushes[20]. Furthermore, by employing the LIPFM, Aftab et al.adopted a Nonlinear MPC (NMPC) framework [17], andJeong et al. proposed a Quadratic Programming (QP) scheme[21], which can both integrate the ankle, stepping, and hipstrategies. Nevertheless, since the LIPFM still assumes aconstant CoM height plane, above works could not makeuse of the height variation.Vertical height motion also contributes to higher walkingrobustness [5], [22]. Nishiwaki et al. proposed a trajec-tory planning algorithm, which could generate time-varyingheight trajectories [23]. However, this work did not considerthe variation of the ZMP dynamics. The same problem alsoexists in [24]. Englsberger et al. [25] solved the verticalheight motion through generalizing the 3D divergent com-ponent of motion, whereas it could not consider feasibilityconstraints. In term of ZMP dynamics, the nonlinear motionequations of the general inverted pendulum model that werederived in [16], [26], [27] can be used to model the heightvariation. Through limiting the nonlinear part of the ZMPmotion constraint between properly chosen extreme values,Brasseur et al. built a Linear MPC (LMPC) scheme and In some studies such as [17], [18], the roll and pitch angular momentumof upper-body around the CoM are also referred as spin angular momentum. a r X i v : . [ c s . R O ] J a n ealized the real-time generation of 3D walking gait [28].Van Heerden solved the NMPC problem efficiently viaSequential Quadratic Programming (SQP) after formulatingthe optimization problem as a Quadratically ConstrainedQuadratic Program (QCQP) problem [29]. Then, by introduc-ing the concept of stiffness , Caron et al. studied the dynamicsproperty of the so-called Variable-Height Inverted Pendulum(VHIP) in term of capturability analysis. Consequently, thenonlinear feasibility constraints were linearized, and a hierar-chical optimization scheme was proposed for stable walkingon uneven terrains [30].To further enhance the walking robustness, Englsberger etal. proposed a measurement-based tracking controller to inte-grate vertical CoM motion with body rotation [31]. Utilizingthe general Nonlinear Inverted Pendulum plus FlywheelModel (NIPFM) [26], Lack et al. [18] and Shafiee-Ashtianiet al. [32] studied the upper-body rotation and verticalheight variation using the LMPC framework. However, theymerely followed the pre-defined height trajectory. Based onthe work in [30], Guan et al. proposed the Virtual-mass-ellipsoid Inverted Pendulum (VIP) model [33], [34]. Then,they enhanced the intrinsically stable MPC for humanoid gaitgeneration [35] and realized the disturbance rejection duringstairs climbing. Nevertheless, to the best of our knowledge,there is still a lack of research that integrates step locationadjustment, angular momentum adaptation and CoM heightvariation simultaneously.Differing from the above work, we propose an NMPC-based walking pattern generator for robust locomotion. Byconsidering the dynamics effects caused by the change of rolland pitch angular momentum and the CoM height variation,the proposed approach can generate stable walking patternsby merely using walking parameter references. Based onstate feedback, this optimizer is capable of modulating CoMheight trajectory and body inclination angle in real-timeinstead of strictly tracking the pre-defined values.The main contributions can be concluded as follows.Firstly, the manipulation of the ZMP trajectory (ankle strat-egy), the adjustment of step location (stepping strategy), thechange of angular momentum (hip strategy), and the varia-tion of CoM height (height variation strategy) are integratedinto one single NMPC. As a result, a versatile frameworkfor walking pattern generator is proposed, which in turndramatically enhances the robustness in compensation forsevere external disturbances. Secondly, through employingbody rotation and CoM vertical movement, higher adapt-ability in real-world scenarios is also achieved. For example,the robot can pass through the limited space or climb higherstairs by modifying the CoM height and body rotation statein real-time. Thirdly, based on the NIPFM dynamics, thenonlinear constrained optimization can be transformed as aQCQP problem, which is solved fast via the SQP algorithm.The rest of this paper is organized as follows. In SectionII, we briefly review the CoM dynamics and the procedure ofSQP for NMPC solution. In Section III, the QCQP problemis formulated. In IV and V, the inverted pendulum simula-tions and whole-body dynamic simulations are conducted. Finally, in Section VI, we draw conclusions.II. P RELIMINARIES
This section introduces the fundamental knowledge aboutthe NIPFM dynamics, the general MPC framework, theQCQP theory, and the SQP algorithm.
A. NIPFM Dynamics
The LIPM [8], which is a linear approximation of hu-manoid walking dynamics, assumes 1) the robot has alumped mass body; 2) legs are mass-less and telescopic;3) CoM moves in a constant plane. These assumptions,which over-constrain the robot’s motion capabilities, limitthe robot’s performance undergoing external perturbations.In this work, in order to enhance the bipedal mobility,we propose to use the NIPFM to utilize the whole-bodyability, especially the upper-body inclination and verticalCoM height variation, for robust locomotion.As demonstrated in Fig. 1, the NIPFM (referred as VIP in[33], [34]), assumes 1) the flywheel has rotational inertia; 2)legs are mass-less and telescopic; 3) the CoM is located at thehip joint; 4) the CoM moves arbitrarily as long as physicallimits are satisfied. Thus, it can be used to model upper-body rotation and vertical body motion. Using this model, theZMP, which should be restricted inside the robot’s supportpolygon, is computed by [16], [26], [27] p x = c x − c z − d z g + ¨ c z ¨ c x − ˙ L y m ( g + ¨ c z ) , ˙ L y = I y ¨ θ p ,p y = c y − c z − d z g + ¨ c z ¨ c y + ˙ L x m ( g + ¨ c z ) , ˙ L x = I x ¨ θ r ,p z = d z , (1)where [ p x , p y , p z ] T , [ c x , c y , c z ] T and [ d x , d y , d z ] T denote the3D ZMP position, CoM position and supporting foot loca-tion, respectively. [ L x , L y ] T , [ I x , I y ] T and [ θ r , θ p ] T separatelydenote the (roll and pitch) angular momentum, moment ofinertia, and flywheel rotation angle about x - and y - axis ( x -axis pointing to the forward direction in the sagittal plane and y - axis pointing to the left direction in the coronal plane). m is the total mass, and g is the gravitational acceleration. B. MPC State Equation
To build a MPC, the state equation should be first estab-lished. In this work, inspired by the work in [9]–[11], wechoose to use the Euler-integration equation. Specifically,assuming the constant jerk of CoM movement and bodyrotation over the time interval ∆ t , we can compute the nextstate at time t k +1 , ˆ x ( k +1) = A ˆ x ( k ) + B ... x ( k ) , (2)where ˆ x ( k ) = [ x ( k ) , ˙ x ( k ) , ¨ x ( k ) ] T ( x ∈ { c x , c y , c z , θ r , θ p } ) isthe current state, ... x ( k +1) is the expected jerk, A = t ∆ t t , B = ∆ t ∆ t ∆ t . z (a) NIPFM y (b) 3D walking on uneven terrains xz d x c x c z d z θ P mg IF τ hip τ ankle ZMPsupport region Σ w Fig. 1. NIPFM for bipedal walking. (a) The ankle torque helps to manipulate the ZMP and the flywheel rotation models the effect of the change ofangular momentum. Note that the vertical height is changeable. (b) The definition of step parameters when walking on uneven terrains.
Using (2), we can derive relationships between the jerk,its position, velocity and acceleration over the predictionhorizon (of length N h ), X ( k ) = P ps ˆ x ( k ) + P pu ... X ( k ) , ˙ X ( k ) = P vs ˆ x ( k ) + P vu ... X ( k ) , ¨ X ( k ) = P as ˆ x ( k ) + P au ... X ( k ) , (3)where X ( k ) = [ x ( k +1) , ..., x ( k + N h ) ] T ( X ∈{ C x , C y , C z Θ r , Θ p } ) denotes the future state of CoMalong x -, y - and z - axis and the upper-body inclination stateabout x - and y - axis over the prediction horizon, where,e.g. C x ( k ) = [ c x ( k +1) , ..., c x ( k + N h ) ] T . ˙ X ( k ) , ¨ X ( k ) , and ... X ( k ) separately denote the velocity, acceleration and jerk of eachmotion channel, respectively. X ( k ) = [ x ( k +1) , ..., x ( k + N h ) ] T ,and separately. P ps , P pu , P vs , P vu , P as and P au are obtainedby calculating (2) recursively [11]. C. SQP-based QCQP solution
Using the state equation expressed in (2) as the predic-tive model, the MPC problem is built as a QCQP, whenconsidering the feasible constraints, especially the ZMPstability constraint. Specifically, the QCQP can be expressedas follows: min X f ( X ) = X T G X + g T X , s.t. h j ( X ) ≤ , (4) h j ( X ) = X T V j X + v Tj X + σ j , j ∈ { , ..., N c } , where X ∈ (cid:60) N t is the state vector, N c and N t are thenumber of constraints and state variables, respectively. G , V j ∈ (cid:60) N t × N t , g , v j ∈ (cid:60) N t , and σ j ∈ (cid:60) are the parametersthat specify the objective function and constraints.The above nonlinear optimization problem can be solvedby the SQP algorithm [36]. By using the SQP, the solutionof (4) is transformed as solving following problem, min ∆ X
12 ∆ T X ∇ X ( f ( X ))∆ X + ( ∇ X ( f ( X ))) T ∆ X , s.t. ( ∇ X ( h j ( X ))) T ∆ X + h j ( X ) ≤ , (5)where ∇ and ∇ separately denote the first-order and thesecond-order differential operator, respectively. By using (4), we can easily obtain ∇ X ( f ( X )) = 2 G , ∇ X ( f ( X )) = 2 G X + g , ∇ X ( h j ( X ))= 2 V j , ∇ X ( h j ( X )) = 2 V j X + v j . (6)As a result, the QCQP can be transformed as the QP, whichcan be solved by time-efficient algorithms such as the active-set method. The solution of the QP ( ∆ X ∈ (cid:60) N t ) is thenused to update the state variable via X = X + ∆ X . Oncecompleted, the solving process of (5) is repeated with thenew X for N s times until meeting the convergence condition,which will be discussed in detail in Section V-D.III. P ROBLEM F ORMULATION
Considering the NIPFM dynamics and feasibility limita-tions, a NMPC problem is established and solved to generateversatile walking patterns by exploring the step location ad-justment, angular momentum adaptation and vertical heightvariation. This section discusses the objective function andthe feasibility constraints in detail.
A. Objective Function
After defining the reference step location and walkingcycle, the work aims to minimize the tracking errors of CoMpositions, upper-body inclination angles, and step locations.Also, we minimize the velocities and jerks of the CoMmovement and body inclination. Thus, at the k th samplingtime, we have the objective function as follows: f = (cid:88) X (cid:110) α X (cid:107) ˙ X ( k ) (cid:107) + β X (cid:107) X ( k ) − X ref ( k ) (cid:107) + γ X (cid:107) ... X ( k ) (cid:107) (cid:111) + (cid:88) U δ U (cid:107) U ( k ) − U ref ( k ) (cid:107) . (7)where α X , β X , γ X and δ U separately denote the velocity,position tracking, jerk, support position tracking penalties. X ref ( k ) = [ x ref ( k +1) , ..., x ref ( k + N h ) ] T are the reference statesof CoM position and upper-body inclination angles overthe prediction horizon. U ( k ) = [ u ( k, , ..., u ( k,N f ) ] T and U ref ( k ) = [ u ref ( k, , ..., u ref ( k,N f ) ] T are the actual and referencefuture step locations over the prediction horizon ( U ( k ) ∈{ D x , D y , D z } ). [ D x ( k ) , D y ( k ) , D z ( k ) ] T are the actual hori-zontal step locations over the prediction horizon, where, e.g. x ( k ) = [ d x ( k, , ..., d x ( k,N f ) ] T . N f is the number of futurestep locations over the prediction horizon. Note that in thiswork, the actual step height ( D z ( k ) ) is set to be the desiredstep height ( D refz ( k ) ), which is determined by the surfaceheight configuration.Under this objective function, the parameters for (4) canbe calculated by using the predictive model expressed in (2)and (3). More details can be found in Appendix A.During each walking cycle, the reference CoM positionsalong x - and y - axis are set to be the center of the referencestep locations, as explained in Appendix B. By default, thereference roll angles and pitch angles are set to be zeroduring the whole walking process so that the robot keepsupright. Particularly, the reference CoM height is the sum ofthe default inverted pendulum height (denoted by h refz ) andthe reference step height. That is, (cid:40) θ refr ( k + i ) = θ refp ( k + i ) = 0 , i ∈ { , ..., N h } ,c refz ( k + i ) = h refz + d refz ( k + i ) , i ∈ { , ..., N h } , (8)where [ θ refr ( k + i ) , ..., θ refr ( k + N h ) ] T and [ θ refp ( k + i ) , ..., θ refp ( k + N h ) ] T denote the reference roll angles and pitch angles over the pre-diction horizon, h refz is the default inverted pendulum heightdetermined by physical structure, [ c refz ( k + i ) , ..., c refz ( k + N h ) ] T de-note the reference CoM height, [ d refz ( k + i ) , ..., d refz ( k + N h ) ] T de-note the reference step height which is determined by thesurface height configuration. B. Constraints
In order to guarantee the feasibility, this work takes intoaccount the constraints on the ZMP movement, step locationvariation, CoM vertical motion, upper-body inclination, andhip torque output.
1) Constraints on ZMP trajectory:
The ZMP should berestricted inside the support polygon in order to maintainthe balance. Although the support polygon expands whenswitching from single supporting phase to double supportingphase, we only consider the single supporting phase duringthe whole walking process. Since those ZMP movementconstraints applied on single supporting phase are morerestrictive at this stage and the sampling time can be largeenough in the MPC framework [11], this simplification isreasonable. Therefore, at the k th sampling time, taking thesagittal movement for an example, the following constraintneeds to be satisfied: p min x ≤ p x ( k + i ) − d x ( k + i ) ≤ p max x , i ∈ { , ..., N h } , (9)where [ p x ( k +1) , ..., p x ( k + N h ) ] T denote the generated ZMPposition over the prediction horizon along the x - axis, [ d x ( k +1) , ..., d x ( k + N h ) ] T denote the generated step locationover the prediction horizon along the x - axis, p min x and p max x are the lower and upper ZMP boundary along x - axis, whichare determined by the size of supporting foot. In this paper, we use [ d x ( k, , ..., d x ( k,N f ) ] T to denote the future steplocations of different walking cycles falling on the prediction horizon, and [ d x ( k +1) , ..., d x ( k + N h ) ] T to denote step locations at different samplingtimes over the prediction horizon. The ZMP movement in the coronal plane should alsosatisfy the similar constraints. When using CoM dynamicsexpressed in (1), the ZMP constraints are nonlinear inequali-ties. As demonstrated in Appendix B, they can be formulatedas quadratic forms.
2) Constraints on step location variation:
The objectivefunction takes the horizontal step locations as variables thatcan change arbitrarily. However, such modification shouldsatisfy physical limitations, such as the maximal leg stretchlength, motor actuation capability, self-collision avoidanceetc. In this paper, the following limitations are considered.Firstly, due to limitations arisen from the mechanical struc-ture and actuation capability, the range of step parameters,including step length and step width, should be constrained.At present, these limitations are simplified to be followinglinear inequalities (taking the forward movement for anexample): (cid:40) d min x ≤ d x ( k,i ) − ˆ d x ( k ) ≤ d max x , i = 1 ,d min x ≤ d x ( k,i ) − d x ( k,i − ≤ d max x , i ∈ { , ..., N f } , (10)where ˆ d x ( k ) denotes the current supporting foot positionalong the x - axis, d min x and d max x are lower and upperboundaries of step length.Secondly, since the future step location falls into thesame walking cycle is re-computed in each loop, it maychange in real-time. However, the position of swing foot,which is determined by the future step location, cannotchange rapidly due to the joint velocity limitation. In thiswork, for the simplification, rather than constraining theswing foot trajectory, we limit the change of future steplocations corresponding to the same walking cycle generatedby different control loops. Since this constraint is imposed ateach time interval, limiting the next one future step locationis enough. Thus, we have (taking the forward motion for anexample): ˙ d min x ∆ t ≤ d x ( k, − d x ( k − , ≤ ˙ d max x ∆ t, (11)where d x ( k, and d x ( k − , are the next one step positionscorresponding to the same period computed by current andlast optimization loop, ˙ d min x and ˙ d max x are the lower andupper velocity boundaries along the x - axis.Finally, as mentioned above, the actual step height ismerely determined by the surface height configuration. Thus,we have the equality constraint on step height: d z ( k,i ) = d refz ( k,i ) , i ∈ { , ..., N f } , (12)where d refz ( k,i ) is the reference step height determined by thesurface height.
3) Constraints on CoM motion:
According to (1), the ver-tical height variation can be utilized to manipulate the ZMPmovement, but the height variation should be constrained soas to avoid infeasible trajectories which break the kinematicconstraints. The limitation of the CoM vertical motion leadsto the following constraints: h min ≤ c z ( k + i ) − d z ( k + i ) − h ref z ≤ h max , i ∈ { , ..., N h } , (13)here h min and h max are the lower and upper boundaries ofthe vertical height variance, [ d z ( k +1) , ..., d z ( k + N h ) ] T denotethe generated vertical height of the supporting foot over theprediction horizon.Additionally, since the ground only generates unilateralreactive forces, the vertical acceleration should be limited toavoid free fall. That is: ¨ c z ( k + i ) ≥ − g, i ∈ { , ..., N h } . (14)
4) Constraints on body inclination:
The trunk rotation islimited by articulation constraints. When using NIPFM, itcan be constrained by using following bounds (taking theroll angle for an example): θ min r ≤ θ r ( k + i ) ≤ θ max r , i ∈ { , ..., N h } , (15)where θ min r and θ max r are the lower and upper boundaries ofroll angle.
5) Constraints on hip joint torque:
The hip joint torqueshould also be limited. Taking the roll direction for anexample: τ min r ≤ I x ¨ θ r ( k + i ) ≤ τ max r , i ∈ { , ..., N h } , (16)where τ min r and τ max r are the lower and upper limits of rolltorque. IV. I NVERTED P ENDULUM S IMULATIONS
Here, we validate the effectiveness of the proposed al-gorithm, where the bipedal walking in multiple scenarios,including 3D walking across uneven terrains and push recov-ery from external disturbances, is demonstrated. Particularly,this section focuses on the inverted pendulum simulations byusing the physical characteristics of the COMAN humanoidrobot [37]. The robot body has 25 DoFs. The total weightis 31 kg and the total height is 0.95 m. For the NIPFMsimulation, the fixed time interval ∆ t is 0.005 s and thepredictive length N h is 31. That is, the predictive windowis 1.55 s long. For the gait generation, the constant walkingcycle ( T ) is 0.8 s. As a result, the information of the twofuture steps is utilized in each optimization loop. Otheressential parameters for the inverted pendulum simulationsare listed in Appendix C. A. 3D walking across uneven terrains
In this section, the performance of the proposed NMPCframework is tested in a 3D walking scenario where the robotis expected to walk across uneven terrains, including upstairsand downstairs. To further validate the walking adaptability,the step parameters (including step length and step width)variation is also taken into consideration. The reference stepparameters are listed in Table I. The results are illustrated inFig. 2 and Fig. 3.As illustrated in Fig. 2 (a), through restricting the ZMPtrajectory within the allowable region formed by the sup-porting feet, the proposed method is capable of generating3D feasible gait which satisfies the stability constraints.Further observations demonstrate that the NMPC algorithmcan manipulate the ZMP movement to maintain the balance,
TABLE IR
EFERENCE STEP PARAMETERS FOR WALKING ON UNEVENTERRAINS .Param. Cycle 1 2-4 5 6 7 8 9 10-Step length[m] 0.15 0.15 0.15 0.3 0.25 0.15 0.05 0.15Step width[m] 0.145 0.145 0.2 0.14 0.14 0.2 0.145 0.145Step height[m] 0 0.1 0 0 -0.1 -0.1 0 0 x [m] -0.200.2 y [ m ] CoM ZMP Step location x [m] z [ m ] CoM height-refCoM height (b)(a)
Fig. 2. Bipedal gait generation for 3D walking across uneven terrains. (a)Generated horizontal CoM trajectory, ZMP trajectory and step locations.The green blocks represent the supporting foot area. (b) Generated sagittalCoM trajectory. Defining the constant height reference during each walkingcycle, time-varying height trajectory is automatically generated. even when the step length and step length vary dramaticallyfrom the th cycle to the th cycle. Namely, the ankle strategy[17] is utilized by the proposed NMPC framework.As can be seen in Fig. 2 (b), vertical CoM motion isalso employed when walking across the uneven terrains. Inthis case, it should be noted that we only take the referencesurface height as the input and assume the constant pendulumheight ( h refz ) during the whole process. Nevertheless, byusing the proposed NMPC strategy, the time-varying heighttrajectory is automatically generated. In addition, as can beseen in Fig. 3 (a), the robot slightly rotates the upper-body formaintaining stability when stepping upstairs and downstairs,meaning that the angular momentum adaption is integrated.Due to the integration of the vertical body motion, the upper-body inclination is suppressed. As a result, as demonstratedin Fig. 3 (b), the hip torque variation is also suppressed intoa very low level. B. Balance recovery from external disturbances
In this section, we analyse the push recovery capability ofthe proposed NMPC scheme. When walking in free space,human beings tend to take reactive steps to overcome thelarge disturbances. In this paper, considering the upper-bodyrotation and height variation, three strategy combinations,i.e., step location adjustment (strategy 1), step locationadjustment plus upper-body rotation (strategy 2), and steplocation adjustment, upper-body rotation plus vertical heightvariation (strategy 3) are considered. In addition, when theexternal disturbances are smaller or when walking in certain t [s] -4-2024 I n c li na t i on ang l e [ r ad ] -3 Roll angle-refRoll anglePitch angle-refPitch angle t [s] -0.500.5 H i p t o r que [ N m ] Roll torquePitch torque (a)(b)
Fig. 3. Generated body inclination angles (a) and hip torques (b) for 3Dwalking across uneven terrains. Since the CoM height variation is integrated,the generated body rotation angles and hip torques are very low. circumstances where the step location adjustment is not al-lowed, the robot can only make use of the angular momentumadaptation and height variation [5], [33]. Thus, the upper-body rotation plus vertical height variation (strategy 4) isalso discussed here.Using the objective function (7), above four strategycombinations can be activated flexibly by tuning the weightcoefficients. However, in this work, in order to compare therobustness of different strategy combinations in a quantitativemanner, we apply equality constraints (hard constraints) onthe generated step locations, CoM height trajectory, and bodyinclination angles to activate different strategy combinations.In particular, the step location adjustment would not beenabled if following equality constraints are applied (takingthe forward movement for an example): d x ( k,i ) = d refx ( k,i ) , i ∈ { , ..., N f } , (17)where d refx ( k,i ) is the forward step location reference.The upper-body rotation would not be enabled if followingequality constraints are applied (taking the roll rotation foran example): θ r ( k + i ) = θ refr ( k + i ) , i ∈ { , ..., N h } . (18)The vertical height variation would not be enabled iffollowing equality constraints are applied: c z ( k + i ) = c refz ( k + i ) , i ∈ { , ..., N h } . (19)Note that, to improve the compute efficiency in the realapplication, we can apply the above equality constraints (18)and (19) merely on the next one sampling time.
1) Balance recovery when stepping in place:
In thissection, the robot is expected to recover from external pusheswhen stepping in place. Briefly, only the horizontal externalforce along the forward direction was applied at the pelvisat 2 s, lasting 0.1 s. Under the external force push, thewalking patterns generated by four different strategies aredemonstrated in Fig. 4 and Fig. 5. Note that 125 N forward force is applied when using strategy 1, 2 and 3, whereas only80 N forward force is applied when using strategy 4.As illustrated in Fig. 4, when using strategy 1, 2, and3, the ZMP support region is extended by making reactivesteps for disturbance rejection. Taking the strategy 1 as anexample, the robot timely adjusts the step locations whenthe external force is imposed, as can be seen in Fig. 5 (a).Since the external disturbance is large enough, the next steplength is also adjusted. As a result, the robot adjusts the steplocations of the th and th steps (lasting from 2.4 s to 4.0 s),as illustrated in Fig. 4 (a). Furthermore, since the objectivefunction (7) aims to track the global step locations, the steplocation returned to be 0 m again after the th cycle. Thesimilar phenomena can also be found when using strategy 2and strategy 3.Besides, when using strategy 2 where merely the equalityconstraint expressed in (19) is applied, the angular mo-mentum is also utilized. As demonstrated in Fig. 5 (b),the maximal pitch angle increases to be about 0.1 rad.Correspondingly, as can be seen in Fig. 5 (c), the pitch torqueis needed to rotate the upper-body. When using strategy 3,where no constraint in (17), (18) or (19) is applied, thevertical height variation is also utilized to reject the externaldisturbance, as can be seen in Fig. 5 (d). Compared withstrategy 2, the maximal pitch angle decreases to be about0.058 rad. In addition, the step length variation when usingstrategy 2 or strategy 3 is also reduced when compared withstrategy 1. As can be seen in Fig. 5 (a), the maximal steplength of strategy 1 is 0.16 m whereas the step length ofstrategy 2 and strategy 3 are 0.15 m and 0.13 m, respectively.That is to say, when walking in the narrow space with limitedsupport zone, strategy 2 and strategy 3 contributes to highrobustness against external disturbances.As a comparison, strategy 4, where merely the equalityconstraint expressed in (17) is applied, is also tested in thissection. As can be seen in Fig. 4 (d), when the externalpush is imposed, step location adjustment is not utilized.Instead, the ZMP movement is utilized to reject the forceperturbation. At the same time, upper-body rotation andheight variation are both utilized to maintain the balance,as demonstrated in Fig 5 (b) and (d). In this sense, whenwalking in the scenario such as stair climbing where the steplocation adjustment is not allowed, the upper-body rotationand height variation can be employed by the proposedmethod for disturbance rejection, which is similar with thework in [34]. However, it should be mentioned that althoughonly 80 N forward push is imposed when using strategy 4,which is smaller than the 125 N imposed on other strategies,the needed pitch angle and height variation by the strategy4 are larger than those of other three strategy combinations,(see Fig. 5 (b) and (d)), meaning that the stepping strategyplays a key role in rejecting large disturbances.It is also worth mentioning that the ZMP trajectory moveswithin the support region, no matter which strategy combina-tion is used, as can be seen in Fig. 4. That is to say, the anklestrategy is also utilized by the proposed NMPC scheme fordisturbance rejection. t [s] x [ m ] strategy 1 t [s] x [ m ] strategy 2 t [s] x [ m ] strategy 3 t [s] x [ m ] strategy 4 CoMZMPZMP upper boundaryZMP lower boundary (a) (b)(c) (d)
Fig. 4. Forward CoM trajectories, ZMP trajectories and ZMP boundaries generated by four different strategies. The ZMP boundaries are determined bythe adjusted step locations. t [s] S t ep l o c a t i on [ m ] t [s] -0.100.10.2 P i t c h ang l e [ r ad ] t [s] -505 P i t c h t o r que [ N m ] t [s] z [ m ] stategy 1stategy 2stategy 3stategy 4(a) (b)(d)(c) Fig. 5. Forward step location sequences (a), pitch angles (b), pitch torques (c) and CoM height trajectories (d) generated by four different strategies.
2) Balance recovery when walking forward:
In this case,the robot is expected to recover from external pushes whenwalking forward. The horizontal external forces are appliedat the pelvis at 2 s, lasting 0.1 s. The default step lengthis 0.15 m, and the default step width is 0.145 m. Otherparameters are the same as those used in the last section.Particularly, multi-directional external forces are applied tothe robot.Under the same external push (forward 125 N, lateral 75N in this case), the robust walking patterns generated by fourdifferent strategy combinations are demonstrated in Fig. 6.Similar to the result in the last section, when using strategy 1,which has been adopted in other work such as [11], the robotalso adjusts the step locations when the external push occurs.Numerical analysis shows that the step length is modified tobe 0.28 m (note the maximal allowable step length is 0.3m) and the step width is switched from 0.145 m to 0.19 m(note the maximal allowable step width is 0.2 m). To recoverfrom the perturbation, the next step location is also adjusted(from (0.6 m,-0.0725 m) to (0.605 m,0.028 m)). After threesteps, the robot fully recovers from the external push andtracks the reference step locations again. Further observationreveals that the ZMP trajectory goes along the border of the support region at the th and th steps. That is to say, theankle strategy is also employed to keep stability.When using strategy 2, the angular momentum is modu-lated so as to reject external disturbances, as demonstratedin Fig. 7. Due to the rotation of the upper-body, the th step length is modified to be 0.27 m. Besides, the th steplocation returns to be (0.6 m,-0.635 m), which is quite closeto the reference one. That is to say, compared with strategy1, smaller parameters variation is needed to recover from thesame push force, which can be seen from Fig. 6 (a) ∼ (b).By using strategy 3, the CoM height variation is utilized toenhance the robustness. As demonstrated in Fig. 8, when thedisturbance occurs, the CoM moves up so that the excesskinetic energy partly converts into potential energy, whichis similar to the result in the last section. As a result, thesmallest variation of step length and step width is neededto reject the same external disturbance, as can be seen inFig. 6 (c). Furthermore, due to the vertical CoM variation,less body inclination is observed in Fig. 7. Differing from[18] and [32], the optimal time-varying CoM trajectory isgenerated online in this work.As to strategy 4, as illustrated in Fig. 6 (d), Fig. 7 andFig. 8, even though the step location is not adjusted, the ABLE IIM
AXIMAL PUSH FORCES THE ROBOT CAN REJECT WHEN USING FOURDIFFERENT STRATEGIES - INVERTED PENDULUM SIMULATIONS .Force Strategy Strategy 1 Strategy 2 Strategy 3 Strategy 4 F x [N] 139 149 174 144 F y [N] 78 93 112 89 upper-body rotation and height variation contributes to robustwalking. Under the same push force, the height variationand pitch angle variation of strategy 4 are much higher thanthose of other strategies. One interesting thing is that theroll angle fluctuation is dramatically suppressed. We guessit is because that 1) the height variation compensates for thelateral tracking error, 2) the lateral force is not large enough.Further analysis reveals that the integration of multiplereactive strategies contributes to stronger push recoverycapability. As listed in Table II, strategy 1 could only endure139 N forward force ( F x ) and 78 N lateral force ( F y ) , andstrategy 2 could endure 149 N forward force and 93 Nlateral force. By integrating all the three balancing strategies(actually, four strategies are integrated if taking into accountthe ankle strategy), the robot could recover from much largerpushes (174 N forward force and 112 N lateral force). Hence,the improved robustness against external pushes is achieved.Besides, in this case, the simulation result shows that strategy4 contributes to the higher push recovery capability thanstrategy 1.V. W HOLE - BODY D YNAMIC S IMULATION
To further demonstrate the effectiveness of the proposedstrategy, the whole-body dynamic simulations are conductedon the COMAN humanoid robot. In this section, the th polynomial interpolation algorithm is adopted to synthesizethe swing foot trajectory. The control frequency is 200Hz.To reduce the tracking error, the low-level PD controllers forCoM position tracking, upper-body inclination angle trackingare also integrated here. Besides, the landing reductionmethod proposed in [13] is also integrated here. The blockdiagram is illustrated in Fig. 9. A. 3D walking across uneven terrain
In this section, the stair climbing capability is demon-strated. Since the step location is limited by the supportsurface, the robot would not adjust the step locations inreal-time. Thus, strategy 4 is used here to generate a robustwalking pattern.The whole-body dynamic simulations demonstrate that, byintegrating the CoM height adaptation, the robot can walkstably on the 9 cm high stairs (21% of the total leg length),which can be seen in the attached video [38]. On the contrary,when the height variation is not employed, the robot can onlyclimb the 3 cm height stairs.The snapshots of stable walking across 5 cm high stairsare illustrated in Fig. 10. In this case, the robot first climbsup four stairs and then climbs down. The default step length, step width and walking cycle for walking on the flat groundare 0.15 m, 0.0145 m and 0.8 s, respectively. To avoidcollision with stairs, the step length is extended to be 0.3m when climbing stairs. Using the proposed strategy, theestimated and generated trajectories are illustrated in Fig.11.As can be seen from Fig. 11, the proposed frameworkgenerates the feasible 3D CoM trajectory by modulating theCoM height ((Fig. 11 (c)) and body inclination states (Fig.11 (d)) as well. Further observation in Fig.11 (a) and Fig.11(b) demonstrates that the real ZMP trajectory is restrictedwithin the support region during most period. However, dueto the landing impact, the forward ZMP trajectory easily goesbeyond the foot support region during the double supportphase. Besides, when climbing down the stairs, the lateralZMP trajectory may also go beyond the stability region,especially when walking from the th to the th step.Nevertheless, due to the angular momentum adaptation andheight variation, the robot can accomplish this walking taskusing the synthesized gait.Since the step length enlarges dramatically when climbingstairs, the effect of the swing leg mass on the walkingstability cannot be ignored. Consequently, the real CoMheight oscillates up and down during the walking process, ascan be seen in Fig.11 (c). Besides, the earlier landing whenstepping up the stairs and the later landing when steppingdown the stairs cannot be avoid. As a result, there existtracking errors on the horizontal step locations and CoMposition, such as the lateral movement of step locationsdemonstrated in Fig.11 (b). Furthermore, the upper-body firstleans backward when stepping up the stairs and then leansforward when stepping down the stairs, as can be seen inFig.11 (d). Nevertheless, the robot still maintains balancewhen walking on the uneven terrain by using the proposedNMPC algorithm. Since the main task is to walking stablyacross the uneven terrain, we believe the tracking error isacceptable in this case. B. Adaptive walking through the narrow space
In the above sections, the default inverted pendulum height( h refz ) is set to be 0.467 m, and the body inclination angles( θ refr and θ refp ) are set to be zeros during the whole walkingprocess. However, in certain circumstances, the NMPC algo-rithm can be used to generate adaptive gaits by tracking thetime-varying inverted pendulum height reference and bodyinclination angle reference, contributing to high adaptability.One typical scenario is that the robot is required to reduce thebody height and lean forward as well in order to go througha narrow passage with limited height.In the ODE environment, the obstacle is set at the 0.3m in front of the robot and 0.75 m in height. As onesolution, the robot needs to reduce the height by 5 cmwhile rotating the upper-body along the y axis in 0.1 rad.As a comparison, no height variation or body inclination isalso tested. The snapshots of walking motions when usingdifferent references are demonstrated in Fig. 12. The height x [m] -0.100.10.2 y [ m ] strategy 1 x [m] -0.100.10.2 y [ m ] strategy 2 x [m] -0.100.10.2 y [ m ] strategy 3 x [m] -0.100.10.2 y [ m ] strategy 4 CoMZMPFoot location (a) (b)(c) (d)
Fig. 6. Horizontal CoM trajectory, ZMP trajectory and step locations generated by different strategies for rejecting the same push force. The green blocksrepresent supporting foot plane. t [s] -0.100.10.2 I n c li na t i on ang l e [ r ad ] Roll angle (stategy 1)Pitch angle (stategy 1)Roll angle (stategy 2)Pitch angle (stategy 2)Roll angle (stategy 3)Pitch angle (stategy 3)Roll angle (stategy 4)Pitch angle (stategy 4)
Fig. 7. Body inclination angles generated by different strategies whenfaced with the same push force. t [s] z [ m ] CoM height (stategy 1)CoM height (stategy 2)CoM height (stategy 3)CoM height (stategy 4)
Fig. 8. CoM height trajectories generated by different strategies whenfaced with the same push force. trajectories and pitch angles when using different referencesare illustrated in Fig. 13.As can be seen in Fig. 12, by tracking time-varying heightreference and pitch angle reference, the NMPC algorithmgenerates the adaptive gait for walking through the narrowspace. In particular, as illustrated in Fig. 13 (a), the robotfirstly reduces the CoM height and then recovers to thenormal height. The similar motion can also be found inthe pitch rotation. Although there are tracking errors ofthe CoM height and pitch angle, the stable walking isrealized. In contrast, when the height variation or upper-body rotation is not utilized, the robot collides with theobstacle and falls down. That is to say, the hip strategyand the height variation strategy integrated into the proposedNMPC algorithm contributes to adaptive walking in real- world environments.
C. Balance recovery from external pushes
Similar to Section IV-B, we analyze the push recoverycapability when using four different strategies. Firstly, thepush recovery performance when the robot is stepping inplace is evaluated. Then, push recovery performance whenthe robot is walking forward is discussed.
1) Balance recovery when stepping in place:
In this case,the robot is stepping in place when the horizontal externalforces are applied at the pelvis center at 3.6 s, which lasted0.1 s.In this section, we discuss the balance recovery move-ments under forward push in detail. Under different externalpushes, the generated patterns when using four differentstrategy combinations are demonstrated in Fig. 14, and thephysical motions can be seen in the supplementary video.When the external force is not large enough, the robotcan recover from the disturbance by merely making onereactive step. As demonstrated in Fig. 14 (a), the step lengthis modulated to be 0.18 m to extend the support region. As aresult, the excess kinetic energy can be dissipated so that thebody balance is maintained. Due to the coupling property,the lateral CoM is also adjusted.As external impulse increases, angular momentum adap-tation needs to be employed. As demonstrated in Fig. 14 (b),the robot rotates the upper-body around the y axis as wellas modulates step location when 23 Ns external impulse isapplied. Differing from Fig. 14 (a) where no reactive rollangle is generated, the maximal reference roll angle reaches0.08 rad, as can be seen from the ’Pitch angle-ref’ plot in14 (b). Due to the utilization of upper-body rotation, evenwhen faced with the more considerable disturbance, smallerstep location modulation is needed (0.12 m in Fig. 14 (b) vs.0.18 m Fig. 14 (a)).When the external force goes beyond one specific limit,vertical height variation needs to be activated. As demon-strated in Fig. 14 (c), three reactive strategies are all triggered eferences:[ d x , d y , d z ] T , [ θ p , θ r ] T , h z Robot q Inverse Kinematics Body posture tracking control CoM position tracking control
High-level plan Low-lever Control
Update: [ d x , d y , d z ] T ,[ c x , c y , c z ] T , [ θ p , θ r ] T Landing reduction
Correction: [ d x , d y , d z ] T ,[ c x , c y , c z ] T , [ θ p , θ r ] T Nonlinear modelpredictive control
Fig. 9. Algorithm flow for robust locomotion control.Fig. 10. COMAN robot climbs stairs, the stair height is 5 cm (11.5% of whole leg length). t [s] x [ m ] t [s] -0.2-0.100.1 y [ m ] ZMP-real CoM-real CoM-generation ZMP upper boundary ZMP lower boundary t [s] z [ m ] CoM height-realCoM height -generation t [s] -0.06-0.04-0.0200.020.04 I n c li na t i on ang l e [ r ad ] Roll angle-realPitch angle-realRoll angle-genenrationPitch angle-generation (a)(c) (d)(b)
Fig. 11. Estimated and generated trajectories when climbing 5 cm stairs. (a) Forward trajectories. (b) Lateral trajectories. (c) CoM height trajectories. (d)Body inclination angles.
Fig. 12. COMAN robot walks through the narrow space with limited height. The top arrow demonstrates that the robot collides with the obstacle andfalls down because of no usage of height variation or upper-body rotation. The below arrow demonstrates that the robot walks across the narrow crouchby reducing CoM height and rotating upper-body. t [s] z [ m ] Real height (height variation)Real height (no height variation)Generated height (height variation)Generated height (no height variation) t [s] -0.3-0.2-0.100.1 P i t c h ang l e [ r ad ] Real angle (body rotation)Real angle (no body rotation)Generated angle (body rotation)Generated angle (no body rotation) (a) (b)
Fig. 13. Generated and real trajectories for walking through the narrow space. (a) CoM height trajectories. (b) Upper-body pitch angles. (a) strategy 1(b) strategy 2 20Ns23Ns t [s] -0.100.10.2 x [ m ] CoM-refCoM-realLeft foot-refRight foot-ref t [s] -0.100.1
CoM-refCoM-realLeft foot-refRight foot-ref t [s] -0.100.10.2 y [ m ] t [s] z [ m ] CoM-refCoM-real t [s] x [ m ] CoM-refCoM-realLeft foot-refRight foot-ref t [s] -0.100.1
CoM-refCoM-realLeft foot-refRight foot-ref t [s] -0.100.10.2 p [ r ad ] Pitch angle-refPitch angle-real t [s] z [ m ] CoM-refCoM-real y [ m ] (c) strategy 3 26Ns t [s] x [ m ] CoM-refCoM-realLeft foot-refRight foot-ref t [s] -0.100.1 y [ m ] CoM-refCoM-realLeft foot-refRight foot-ref t [s] -0.100.10.2 2 4 6 8 t [s] z [ m ] CoM-refCoM-real (d) strategy 4 t [s] x [ m ] CoM-refCoM-realLeft foot-refRight foot-ref t [s] -0.100.1 y [ m ] CoM-refCoM-realLeft foot-refRight foot-ref t [s] t [s] z [ m ] CoM-refCoM-real
Pitch angle-refPitch angle-realPitch angle-refPitch angle-realPitch angle-refPitch angle-real p [ r ad ] p [ r ad ] p [ r ad ] Fig. 14. Reference and estimated trajectories when recovering from different levels of forward pushes. The snapshots of robot motions are demonstratedon the right side. In this case, when 20Ns external push is imposed, only the step location is modulated (strategy 1), as demonstrated by the green lineararrow in sub-figure (a). As the disturbance increases (23Ns in this case), the body rotation should be integrated (strategy 2), as demonstrated by the greencurved arrow in sub-figure (b). When 26Ns external impulse is imposed, the CoM vertical height variation (strategy 3) is also activated, as demonstrated bythe green vertical arrow in sub-figure (c). As a comparison, merely the body rotation and vertical height variation (strategy 4) are utilized, as demonstratedin sub-figure (d). o as to maintain the balance when 26 Ns external impulseis imposed. Differing from Fig. 14 (a) and 14 (b) where noreactive height variation is employed, the CoM moves upabove 0.51 m after 4 s, as can be seen from the verticalheight trajectory in 14 (c). Then, after 5 s, the CoM heightdrops, leading to higher stability.As a comparison, step location adjustment is not utilizedfor push recovery test (Strategy 4). As demonstrated in Fig.14 (d), when the step location adjustment is not used, therobot leans forward while increasing the body height to rejectthe external perturbation. Due to the weight coefficientssetup, the maximal reference pitch angle is 0.078 rad (theupper boundary for the pitch angle is 0.087 rad) while themaximal height reaches 0.49 m. Compared with Fig. 14 (c),it also takes a longer time period to return to the referenceinclination angle and reference CoM height.
2) Balance recovery when walking forward:
In this case,the robot is walking forward when the horizontal externalforces are applied at the pelvis center at 3.6 s which lasted0.1 s. The default step length is 0.1 m and the default steplength is 0.145 m.Differing from Section IV-B.2 where the forward andlateral pushes are imposed simultaneously, in this section,we focus on the forward push recovery so as to compare theperformances using different strategy combinations. Underdifferent external pushes, the generated patterns when usingfour strategy combinations are illustrated in Fig. 15- 17, andthe animation can be seen in the supplementary video.As demonstrated in Fig. 15 (a), Fig. 16 (a), and Fig.17 (a), where no angular momentum adaptation or heightvariation is employed, the robot can still maintain the bal-ance by adjusting the step locations in following steps. Asdemonstrated in Fig. 15 (a), differing from Section IV-B.2where the robot enlarges the step length immediately afterthe external push is imposed, the swing leg here lands theground in a shorter length at the th step (the step locationis reduced from 0.3 m to 0.29 m). We guess it is becausethat, at the startup of the th step, the estimated CoM alreadyfalls behind the step center, which can be seen in Fig. 15 (a)at about 3.3 s. Besides, there is also a time delay in stateestimation. Thus, the step location is reduced at the th stepeven when the forward force is imposed at 3.6 s. The similarphenomena can also be observed in other sub-figures in Fig.15. Then, since the CoM falls beforehand the step centerat 4.1 s, the step length is enlarged to track the referencestep location (0.4 m in this case). After then, since no bodyrotation or height variation is activated, the step locations inthe following steps are also adjusted. As a result, even underthe smallest external force, the tracking error of the steplocation when using strategy 1 becomes the largest amongthe four strategies.As can be seen in Fig. 15-17, the robot can reject largerpush force when more strategies is integrated. Taking strategy3 as an example, by adjusting the step location, rotating theupper-body and changing the CoM height simultaneously,the robot can recover from the 350 N push force.In particular, when the strategy 4 is employed, only the TABLE IIIM
AXIMAL TOLERANT PUSH FORCES THE ROBOT CAN REJECT UNDERDIFFERENT STRATEGIES - DYNAMIC SIMULATIONS .Param. Strategy Strategy 1 Strategy 2 Strategy 3 Strategy 4 F x [N] 273 335 376 295 F y [N] 160 185 255 166 angular momentum and CoM height are adjusted to rejectthe external disturbance, as demonstrated in 16 (d) andFig. 17 (d). It should be mentioned that, differing from theSection IV-B.2, the CoM height reduces dramatically whenthe external force is imposed. We guess that it is becausethat the external force is too large. By reducing the CoMheight, the moment arm could be shortened so as to reducethe overturning moment caused by the variation of forwardCoM position, considering that there is no possibility of thestep location adjustment, After then, the robot increases theheight and goes back to the normal height. Compared withother sub-figures in Fig. 17, there is the largest fluctuation inCoM height when using strategy 4. Nevertheless, the robotcan maintain balance during the whole walking process.When walking forward, the maximal tolerant push forcesthe robot can reject are listed in Table III. Again, the robotachieves the strongest recovery capability by integrating thestep location adjustment, angular momentum optimization,and vertical height adaptation in one framework. That isto say, the proposed NMPC algorithm contributes to higherrobustness. D. Computation Efficiency
In this paper, the SQP algorithm (5) is used to solve theQCQP (4). Differing from [29] where the N s was set tobe 2 by hand, we propose to use the following terminationfunction: min( F m ) ≤ ε where F m ( n ) = max( ∆ n k ( n ) ) ,n k > N s . (20)where ∆ n k , consisting of the increments of 3D CoM jerk,2D angular jerk and 3D step location, is computed by theQP solver, F m ∈ (cid:60) , n k is the loop count of QP solver.That is to say, there are two ways to terminate the SQPloop. The first way is that we firstly judge the maximalabsolute value of each state channel, including 3D CoM,2D body inclination angle, and 3D step location, over theprediction horizon. Then, we need to compare the minimumamong the maximal values with the ε . The second way isthe judge if the loop number goes beyond the limitation.Therefore, before using the condition expressed in (20), theproper ε and N max should be determined so as to balancethe trade-off between computation cost and optimizationperformance.To determine the ε and N max , the gait pattern generationtests are conducted by using the QP solver provided in theC++ optimization library QuadProg++ to solve the NMPCproblem. In this case, the robot walks with constant step a)(c) t [s] x [ m ] strategy 1 t [s] x [ m ] strategy 2 (b)(d) t [s] x [ m ] strategy 3 t [s] x [ m ] strategy 4 CoM-refCoM-realLeft foot-refRight foot-ref
Fig. 15. Reference and estimated forward trajectories when recovering from different levels of forward pushes: (a) F x = 260 N; (b) F x = 310 N; (c) F x = 350 N; (d) F x = 295 N. (a) t [s] -0.0500.05 p [ r ad ] strategy 1 t [s] -0.0500.050.1 p [ r ad ] strategy 2 (b)(c) (d) t [s] -0.0500.050.1 p [ r ad ] strategy 3 t [s] -0.0500.050.1 p [ r ad ] strategy 4 Pitch angle-refPitch angle-real
Fig. 16. Reference and estimated pitch angles when recovering from different levels of forward pushes. length (0.1 m), step width (0.145 m), and step duration (0.8s). The sampling time dt is 0.1 s and the time duration ofthe prediction horizon is 1.0 s. When using different N s , theoptimization performance of CoM generation is illustratedin Fig. 18 and the statistical result about the minimal ε s andaverage time costs (on a 3.3 GHz quad-core CPU) are listedin Table IV.As can be seen in Fig. 18, especially in the Fig. 18(b), theCoM error decreases as the N s increases. However, when N s becomes larger than 3, the effect is dramatically weakened.The similar rule can be found in the ε , which is listed inTable IV. On the other side, the (average) time cost increases TABLE IVA
VERAGE TIME COSTS AND MINIMAL THRESHOLDS W . R . T THE LOOPNUMBER .Param. N s ε × − × − × − × − × − × − time cost[ms] 1.52 2.64 3.56 4.56 5.52 6.57 linearly with the increase of N s . Thus, using a too largeloop number which is bigger than 3 is not advised. As aresult, for the real application, we can choose the parameters a) (b)(c) (d) t [s] z [ m ] strategy 1 t [s] z [ m ] strategy 2 t [s] z [ m ] strategy 3 z [ m ] strategy 4 CoM-refCoM-real t [s]
Fig. 17. Reference and estimated height trajectories when recovering from different levels of forward pushes. t [s] x [ m ] t [s] -2-101 y [ m ] t [s] -50510 z [ m ] Ns =2 Ns =3 Ns =4 Ns =5 Ns =6 (a) (b) (c) Fig. 18. The CoM errors when using different N s : the benchmark is the generated CoM trajectory when N s = 1 . combination where ε = 5 × − and N s = 3 for QCQPsolution. VI. C ONCLUSION
In this paper, we propose a versatile framework for robustlocomotion. The proposed framework is formulated as aQCQP and solved via the off-the-shelf SQP technique.By using the NIPFM, which can take into considerationthe CoM height variation and angular momentum change, theZMP constraints are formulated in a quadratic form. Throughcombining step movement limitations and other feasibilityconstraints, robust bipedal walking is realized with the capa-bilities of ZMP manipulation, stepping location adjustment,CoM height variation and upper-body rotation. In addition,the proposed framework can generate versatile walking pat-terns by customizing the reference CoM height trajectory orbody inclination angle. Simulation studies demonstrate thatthe robot is able to achieve higher adaptability under multiplereal-world scenarios.We believe a promising future about this framework sinceit can be used to generate the adaptive walking patternsand to maintain balance as well for humanoids in thereal environment. At present, we tune the penalties in theobjective function by hand, which is tedious. Thus, studying the priority of different strategies under realistic scenarioscan be the next focus. A
PPENDIX
For the objective function defined in (7), using (3), the G , g and state X in (4) are expressed as follows: G = diag ( ϑ C x , ϑ C y , ϑ C z , ϑ Θ r , ϑ Θ p , φ D x , φ D y , φ D z ) ,ϑ X = γ X I N h × N h + α X P T vu P vu + β X P T pu P pu ,φ U = δ U I N f × N f , g = ( α C x P T vu P vs + β C x P T pu P ps )ˆ c x ( k ) − β C x P T pu C refx ( k ) ( α C y P T vu P vs + β C y P T pu P ps )ˆ c y ( k ) − β C y P T pu C refy ( k ) ( α C z P T vu P vs + β C z P T pu P ps )ˆ c z ( k ) − β C z P T pu C refz ( k ) ( α Θ r P T vu P vs + β Θ r P T pu P ps )ˆ θ r ( k ) − β Θ r P T pu Θ refr ( k ) ( α Θ p P T vu P vs + β Θ p P T pu P ps )ˆ θ p ( k ) − β Θ p P T pu Θ refp ( k ) − δ D x D refx ( k ) − δ D y D refy ( k ) − δ D z D refz ( k ) , X ( k ) = [ ... C x ( k ) ; ... C y ( k ) ; ... C z ( k ) ; ... Θ r ( k ) ; ... Θ p ( k ) ; D x ( k ) ; D y ( k ) ; D z ( k ) ] , (21)here, diag () is the function that produces a diagonal matrixwith the given parameters in the diagonal positions, X ∈{ C x , C y , C z , Θ r , Θ p } , U ∈ { D x , D y , D z } .All the constraints introduced in Section III-B can beformulated in quadratic form as expressed in (4). In thissection, we present more details about how to express theZMP constraints.At the k th sampling time, defining the selection matrices,the predictive CoM jerk and step locations can be expressedas follows: U ( k ) = S U X ( k ) , S u ∈ (cid:60) N f × N t , ... X ( k ) = S x X ( k ) , S x ∈ (cid:60) N h × N t , ... x ( k + j ) = S j ... X ( k ) , S j ∈ (cid:60) × N h , j ∈ { , ..., N h } . (22)Besides, the step locations over the prediction horizon aregenerated by using the separated locations corresponding tothe following walking cycles, we have following relationship(taking the step location along the x - axis for instance): ¯ D x ( k ) = e c ( k ) ˆ d x ( k ) + E c ( k ) D x ( k ) (23)where, ¯ D x ( k ) = [ d refx ( k +1) , ..., d refx ( k + N h ) ] T consists of the steplocations over the prediction horizon, ˆ d x ( k ) denotes positionof the current supporting foot, the e c ( k ) and E c ( k ) aremapping matrix, with more details can be found in [11].And then, the position of supporting foot is determined by d x ( k + j ) = S j ¯ D x ( k ) , j ∈ { , ..., N h } . (24) quadratic form of ZMP constraints : To be brief, we onlydiscuss the upper boundary. Taking the motion along the x -axis for instance, substitute (1) into (9), we have: ( c x ( k + j ) − d x ( k + j ) − p max x )( g + ¨ c z ( k + j ) ) − ( c z ( k + j ) − d z ( k + j ) )¨ c x ( k + j ) − I y ¨ θ p ( k + j ) /m ≤ (25)Then, by substituting (3), (22) and (24) into (25) andcollecting terms, the quadratic form of ZMP constraints is: V p x ( j ) = m ( S T c x P T pu S Tj S j P au S c z − S T c x P T au S Tj S j P pu S c z − S T D x E T c ( k ) S Tj S j P au S c z ) , (26) v p x ( j ) = m (ˆ c T x ( k ) P T ps S Tj S j P au S c z + ˆ c T z ( k ) P T as S Tj S j P pu S c x + g S j P pu S c x − (ˆ c T z ( k ) P T ps S Tj S j P au S c x + ˆ c T x ( k ) P T as S Tj S j P pu S c z ) + d z ( k + j ) S j P au S c x − (ˆ c T z ( k ) P T as S Tj S j E c ( k ) S D x + ˆ d x ( k ) e T c ( k ) S Tj S j P au S c z ) − g S j E c ( k ) S D x − p max x S j P au S c z ) T − ( I y S j P au S Θ p ) T , (27) σ p x ( j ) = m (ˆ c T x ( k ) P T ps S Tj S j P as ˆ c z ( k ) + g S j P ps ˆ c x ( k ) − ˆ c T x ( k ) P T as S Tj S j P ps ˆ c z ( k ) + ˆ c T x ( k ) P T as S Tj d z ( k + j ) − ˆ d x ( k ) e T c ( k ) S Tj S j P as ˆ c z ( k ) − g S j e c ( k ) ˆ d x ( k ) − p max x S j P as ˆ c z ( k ) − gp max x ) − I y S j P as ˆ θ p ( k ) . (28)The essential parameters for the NIPFM simulations andwhole-body dynamic simulations can be found in Table V. TABLE VB
OUNDARY CONDITIONS FOR FEASIBILITY CONSTRAINTS
ZMP constraints ˙ d min y [m · s − ] -1 p min x [m[ -0.03 ˙ d max y [m · s − ] p max x [m] p min y [m] -0.05 h min [m] -0.15 p max y [m] h max [m] d min x [m] -0.1 θ min r [ rad ] -0.087 d max x [m] θ max r [ rad ] d min y [m] θ min p ( − θ max p )[ rad ] -0.175 d max y [m] ˙ d min x [m · s − ] -1 τ min r ( − τ max r )[N · m] -80 ˙ d max x [m · s − ] τ min p ( − τ max p )[N · m] -80 A CKNOWLEDGMENT
This work is supported by National Natural ScienceFoundation of China (Grant No. 51675385) and EuropeanUnion’s Horizon 2020 robotics program CogIMon (ICT-23-2014, 644727). R
EFERENCES[1] Y. Sakagami, R. Watanabe, C. Aoyama, S. Matsunaga, N. Higaki,and K. Fujimura, “The intelligent asimo: System overview and inte-gration,” in
IEEE/RSJ International Conference on Intelligent Robotsand Systems , 2002, pp. 2478–2483.[2] M. Fallon, S. Kuindersma, S. Karumanchi, M. Antone, T. Schneider,H. Dai, C. P. D’Arpino, R. Deits, M. DiCicco, D. Fourie et al. , “Anarchitecture for online affordance-based perception and whole-bodyplanning,”
Journal of Field Robotics , vol. 32, no. 2, pp. 229–254,2015.[3] N. G. Tsagarakis, D. G. Caldwell, F. Negrello, W. Choi, L. Baccelliere,V. Loc, J. Noorden, L. Muratore, A. Margan, A. Cardellino et al. ,“Walk-man: A high-performance humanoid platform for realistic en-vironments,”
Journal of Field Robotics , vol. 34, no. 7, pp. 1225–1259,2017.[4] C. Zhou and N. Tsagarakis, “On the Comprehensive KinematicsAnalysis of a Humanoid Parallel Ankle Mechanism,”
ASME Journalof Mechanisms and Robotics , 2018.[5] F. B. Horak and L. M. Nashner, “Central programming of postu-ral movements: adaptation to altered support-surface configurations,”
Journal of neurophysiology , vol. 55, no. 6, pp. 1369–1381, 1986.[6] L. M. Nashner and G. McCollum, “The organization of human posturalmovements: a formal basis and experimental synthesis,”
Behavioraland brain sciences , vol. 8, no. 1, pp. 135–150, 1985.[7] M. S. Orendurff, A. D. Segal, G. K. Klute, J. S. Berge et al. , “Theeffect of walking speed on center of mass displacement,”
Journal ofrehabilitation research and development , vol. 41, no. 6A, p. 829, 2004.[8] S. Kajita and K. Tani, “Study of dynamic biped locomotion on ruggedterrain-derivation and application of the linear inverted pendulummode,” in
IEEE International Conference on Robotics and Automation ,1991, pp. 1405–1411.[9] S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi,and H. Hirukawa, “Biped walking pattern generation by using previewcontrol of zero-moment point,” in
IEEE International Conference onRobotics and Automation , 2003, pp. 1620–1626.[10] P.-B. Wieber, “Trajectory free linear model predictive control forstable walking in the presence of strong perturbations,” in
IEEE-RASInternational Conference on Humanoid Robots , 2006, pp. 137–142.[11] H. Diedam, D. Dimitrov, P.-B. Wieber, K. Mombaur, and M. Diehl,“Online walking gait generation with adaptive foot positioning throughlinear model predictive control,” in
IEEE/RSJ International Confer-ence on Intelligent Robots and Systems , 2008, pp. 1121–1126.[12] S. Feng, X. Xinjilefu, C. G. Atkeson, and J. Kim, “Robust dynamicwalking using online foot step optimization,” in
IEEE/RSJ Interna-tional Conference on Intelligent Robots and Systems , 2016, pp. 5373–5378.13] J. Ding, Y. Wang, M. Yang, and X. Xiao, “Walking stabilization controlfor humanoid robots on unknown slope based on walking sequencesadjustment,”
Journal of Intelligent & Robotic Systems , vol. 90, no.3-4, pp. 323–338, 2018.[14] J. A. Castano, Z. Li, C. Zhou, N. Tsagarakis, and D. Caldwell,“Dynamic and reactive walking for humanoid robots based on footplacement control,”
International Journal of Humanoid Robotics ,vol. 13, no. 02, p. 1550041, 2016.[15] T. Kamioka, H. Kaneko, M. Kuroda, C. Tanaka, S. Shirokura,M. Takeda, and T. Yoshiike, “Dynamic gait transition between walk-ing, running and hopping for push recovery,” in
IEEE-RAS Interna-tional Conference on Humanoid Robotics , 2017, pp. 1–8.[16] S. Kudoh, T. Komura, and K. Ikeuchi, “The dynamic postural ad-justment with the quadratic programming method,” in
IEEE/RSJInternational Conference on Intelligent Robots and Systems , vol. 3,2002, pp. 2563–2568.[17] Z. Aftab, T. Robert, and P.-B. Wieber, “Ankle, hip and stepping strate-gies for humanoid balance recovery with a single model predictivecontrol scheme,” in
IEEE-RAS International Conference on HumanoidRobots , 2012, pp. 159–164.[18] J. Lack, “Integrating the effects of angular momentum and changingcenter of mass height in bipedal locomotion planning,” in
IEEE-RASInternational Conference on Humanoid Robots , 2015, pp. 651–656.[19] J. Pratt, J. Carff, S. Drakunov, and A. Goswami, “Capture point:A step toward humanoid push recovery,” in
IEEE-RAS InternationalConference on Humanoid Robots , 2006, pp. 200–207.[20] C. Li, R. Xiong, Q.-g. Zhu, J. Wu, Y.-l. Wang, and Y.-m. Huang,“Push recovery for the standing under-actuated bipedal robot usingthe hip strategy,”
Frontiers of Information Technology & ElectronicEngineering , vol. 16, no. 7, pp. 579–593, 2015.[21] H. Jeong, I. Lee, J. Oh, K. K. Lee, and J.-H. Oh, “A robust walkingcontroller based on online optimization of ankle, hip, and steppingstrategies,”
IEEE Transactions on Robotics , vol. 35, no. 6, pp. 1367–1386, 2019.[22] F. Massaad, T. M. Lejeune, and C. Detrembleur, “The up and downbobbing of human walking: a compromise between muscle work andefficiency,”
The Journal of physiology , vol. 582, no. 2, pp. 789–799,2007.[23] K. Nishiwaki and S. Kagami, “Online design of torso height tra-jectories for walking patterns that takes future kinematic limits intoconsideration,” in
IEEE International Conference on Robotics andAutomation , 2011, pp. 2029–2034.[24] R. Mirjalili, A. Yousefi-Korna, F. A. Shirazi, A. Nikkhah, F. Nazemi,and M. Khadiv, “A whole-body model predictive control schemeincluding external contact forces and com height variations,” in
IEEE-RAS International Conference on Humanoid Robots , 2018, pp. 1–6.[25] J. Englsberger, C. Ott, and A. Albu-Sch¨affer, “Three-dimensionalbipedal walking control based on divergent component of motion,”
IEEE Transactions on Robotics , vol. 31, no. 2, pp. 355–368, 2015.[26] S. Kajita,
Humanoid robots (in Japanese) . Ohmsha, 2005.[27] P.-B. Wieber, R. Tedrake, and S. Kuindersma, “Modeling and controlof legged robots,” in
Springer handbook of robotics . Springer, 2016,pp. 1203–1234.[28] C. Brasseur, A. Sherikov, C. Collette, D. Dimitrov, and P.-B. Wieber,“A robust linear mpc approach to online generation of 3d bipedwalking motion,” in
IEEE-RAS International Conference on HumanoidRobots , 2015, pp. 595–601.[29] K. Van Heerden, “Real-time variable center of mass height trajectoryplanning for humanoids robots,”
IEEE Robotics and AutomationLetters , vol. 2, no. 1, pp. 135–142, 2017.[30] S. Caron, A. Escande, L. Lanari, and B. Mallein, “Capturability-based pattern generation for walking with variable height,”
IEEETransactions on Robotics , 2019.[31] J. Englsberger and C. Ott, “Integration of vertical com motion andangular momentum in an extended capture point tracking controllerfor bipedal walking,” in
IEEE-RAS International Conference on Hu-manoid Robots , 2012, pp. 183–189.[32] M. Shafiee-Ashtiani, A. Yousefi-Koma, and M. Shariat-Panahi, “Ro-bust bipedal locomotion control based on model predictive control anddivergent component of motion,” in
IEEE International Conference onRobotics and Automation , 2017, pp. 3505–3510.[33] K. Guan, K. Yamamoto, and Y. Nakamura, “Push recovery by angularmomentum control during 3d bipedal walking based on virtual-mass-ellipsoid inverted pendulum model,” in
IEEE-RAS 19th InternationalConference on Humanoid Robots , 2019, pp. 120–125. [34] ——, “Virtual-mass-ellipsoid inverted pendulum model and its ap-plications to 3d bipedal locomotion on uneven terrains,” in
IEEE/RSJInternational Conference on Intelligent Robots and Systems , 2019, pp.1401–1406.[35] N. Scianca, M. Cognetti, D. De Simone, L. Lanari, and G. Oriolo,“Intrinsically stable mpc for humanoid gait generation,” in
IEEE-RASInternational Conference on Humanoid Robots , 2016, pp. 601–606.[36] J. Nocedal and S. J. Wright,
Sequential quadratic programming .Springer, 2006.[37] N. G. Tsagarakis, S. Morfey, G. M. Cerda, L. Zhibin, and D. G.Caldwell, “Compliant humanoid coman: Optimal joint stiffness tuningfor modal frequency control,” in
IEEE International Conference onRobotics and Automation , 2013, pp. 673–678.[38] video: https://youtu.be/6CtlS0UASIAvideo: https://youtu.be/6CtlS0UASIA