Fast Online Planning for Bipedal Locomotion via Centroidal Model Predictive Gait Synthesis
FFast Online Planning for Bipedal Locomotion via Centroidal ModelPredictive Gait Synthesis
Yijie Guo , Mingguo Zhao Abstract — The planning of whole-body motion and step timefor bipedal locomotion is constructed as a model predictivecontrol (MPC) problem, in which a sequence of optimizationproblems need to be solved online. While directly solving theseproblems is extremely time-consuming, we propose a predictivegait synthesizer to solve them quickly online. Based on thefull dimensional model, a library of gaits with different speedsand periods is first constructed offline. Then the proposed gaitsynthesizer generates real-time gaits by synthesizing the gaitlibrary based on the online prediction of centroidal dynamics.We prove that the generated gaits are feasible solutions ofthe MPC optimization problems. Thus our proposed gaitsynthesizer works as a fast MPC-style planner to guarantee thefeasibility and stability of the full dimensional robot. Simulationand experimental results on an 8 degrees of freedom (DoF)bipedal robot are provided to show the performance androbustness of this approach for walking and standing.
I. INTRODUCTIONBipedal robots are complex dynamic systems with highdegrees of freedom. Different approaches have been inves-tigated for real-time motion planning for bipedal robots.Classical methods based on reduced-order models have beenwell developed[1], [2], [3], while the workspace of robotsmay be limited and some physical constraints (actuatorbounds, friction cone, etc. ) are not directly considered.Thus, a lot of recent work [4], [5], [6] focused on trajectoryoptimization based on full dimensional models. However,due to the complexity and non-convexity of the formulatednonlinear optimization problems, these methods are not yetready for online implementation.In order to consider full dimensional dynamics/constraintsand avoid online trajectory optimization, gait library basedmethods have been proposed. Through offline trajectoryoptimization based on the full dimensional model, thesemethods first construct a library of periodic or aperiodic gaittrajectories, then choose the appropriate trajectory online.One of the earliest work based on this idea is [7], where gaitswith fixed speeds are designed for a planar underactuatedbiped. This idea was later extended to fully-actuated bipedalrobots in [8], [9]. In these studies, the gait trajectory is chosenaccording to the speed command. The stability of the robotheavily relies on the controller, since the motion planningdoes not change according to robot states. Under largedisturbances, these methods may fail when the controller cannot track the planned motion due to the physical constraintsof actuators or ground reaction forces. To address this issue, Yijie Guo is with Beijing Research Institute of UBTECH Robotics,Beijing, China. [email protected] Mingguo Zhao is with the Department of Automation, Tsinghua Uni-versity, Beijing, China [email protected] a gait updating method is proposed in [10], the gait trajectoryis updated according to the mid-step average speed, thus therobots can handle larger speed perturbations. However, asthe gait is updated only once at the middle of each step, itcan not react in time for disturbances near the beginning orend of each step.In the meantime, current gait library methods generallykeep a constant step time, while adjusting both step locationand step time greatly enlarges the margin of stability, asshown in [11]. In related work [12], [13] on motion planningbased on reduced-order models, step time adjustment hasalso been applied to improve the robustness of robots. It isshown in [14] that a shorter step time results in a largercapturability region. However walking consistently with avery short step time is unnatural and power-consuming, andusers may require a specific step time in some cases. Thisbrings the need for online step time adjustment to meet theuser command under normal circumstances and satisfy thestability requirement under disturbances.
Contributions:
In this paper, we propose a gait synthe-sis approach from an MPC point of view. The proposedgait synthesizer generates real-time gaits by synthesizing amulti-period gait library based on the online prediction ofcentroidal dynamics. This enables fast reactive gait updatingat each sample time, and the step time is adjusted onlineby synthesizing gaits with different periods. This approachprovides a methodology towards full dimensional modelbased real-time motion planning for bipedal locomotion withtheoretical stability guarantee. Simulation and experimentalresults are provided to show that robots can achieve versatileand robust locomotion with this proposed approach.The paper is organized as follows. The MPC problemfor locomotion planning is described in Sec. II. Then themulti-period gait library is constructed in Sec. III. In Sec.IV, the predictive gait synthesizer is proposed and proved tooffer feasible solutions for the MPC optimization problems.Simulation and experimental results on an 8-DoF bipedalrobot are presented in Sec. V. Finally, conclusions are givenin Sec. VI. II. P
ROBLEM D ESCRIPTION
The overall motion planning and control architecture isshown in Fig. 1. The planner generates whole-body motiontrajectories according to the user command and current robotstates, then an operational space controller (OSC) generatesappropriate motor commands to follow these trajectories. Wefocus on the motion planning part in this paper. a r X i v : . [ c s . R O ] F e b ig. 1. The overall motion planning and control architecture. The proposedcentroidal model predictive gait synthesizer is shown in the dashed box. Our proposed gait synthesizer is an MPC style planner,the basic idea is to solve the following optimization problemfor future 2 steps at current time t (assume the right leg isthe stance leg at the i th step):min ϕ ( t ) ,T (cid:90) Tt || τ ϕ ( t ) · dq ϕ ( t ) || dt subject to lim i →∞ | ˙ p + x [ i + 2] − ˙ p + ∗ x | ≤ c lim i →∞ | ˙ p + y [ i + 2] − R ˙ p + ∗ y | ≤ c lim i →∞ | p + x [ i + 2] − p + ∗ x | ≤ c lim i →∞ | p + y [ i + 2] − R p + ∗ y | ≤ c Whole-body dynamics satisfied.Whole-body constraints satisfied. (1)where ϕ ( t ) represents whole-body motion trajectories, T is the step period, τ ϕ ( t ) and dq ϕ ( t ) are the joint torquesand velocities to achieve ϕ ( t ) , p + [ i ] = [ p + x [ i ] , p + y [ i ]] T and ˙ p + [ i ] = [ ˙ p + x [ i ] , ˙ p + y [ i ]] T are the post-impact horizontal CoMposition and velocity relative to the stance foot, p + ∗ =[ p + ∗ x , R p + ∗ y , L p + ∗ y ] T and ˙ p + ∗ = [ ˙ p + ∗ x , R ˙ p + ∗ y , L ˙ p + ∗ y ] T are thedesired values, subscripts x, y indicate the x/y direction,subscripts R, L indicate the right/left leg is the stance leg.The sufficiently small constants c , c , c , c ∈ R + ensurethat ˙ p + and p + are converged to a small bounded setaround the desired values. Thus the solution of (1) guaranteesthe stability of CoM states while satisfying whole-bodydynamics and constraints. After each optimization, ϕ ( t ) forthe next sample time is sent to the controller for tracking.However, directly solving (1) using whole-body trajectoryoptimization is extremely time-consuming. In the followingsections, we show that our proposed gait synthesizer can pro-vide a feasible solution by combining an offline constructedmulti-period gait library and online gait synthesis based oncentroidal dynamics. As shown in Fig. 1, this gait synthesizercan run at 1kHz, the same as the OSC. This fast re-planninggreatly increases the robot robustness to disturbances andenvironmental uncertainties. III. M ULTI - PERIOD G AIT L IBRARY
In this section, the multi-period gait library is first con-structed through trajectory optimization based on the fulldimensional model.
A. Hybrid Model of Walking
The walking process is modeled as a hybrid system,including a single support phase and an instantaneous doublesupport phase. Assuming the right leg is the stance leg, theoverall hybrid model of walking can be written as: D ( q )¨ q + H ( q, ˙ q ) = Bτ + J R ( q ) T f R J R ( q )¨ q + ˙ J R ( q, ˙ q ) ˙ q = 0 ( q, ˙ q ) / ∈ S ˙ q + = ∆( q ) ˙ q − ( q, ˙ q ) ∈ S (2)where S = { ( q, ˙ q ) | p zL ( q ) = 0 , ˙ p zL ( q, ˙ q ) ≤ } .The first two equations in (2) describe the single supportphase dynamics. The first equation describes the floating basedynamics, where q is the vector of generalized coordinatesincluding both floating states and joint states, D ( q ) is themass-inertia matrix, H ( q, ˙ q ) contains the gravity force andcoriolis force, τ is the vector of motor torques, f R is thecontact wrench. B is the motor torque distribution matrix, J R ( q ) is the Jacobian matrix of the contact point. Thesecond equation describes the contact constraint, i.e., theacceleration of the contact point is zero.The third equation in (2) describes the instantaneous dou-ble support phase. When the vertical position of the swing leg p zL ( q ) decreases to 0, i.e. ( q, ˙ q ) ∈ S , the robot enters doublesupport phase. Following the rigid impact model hypothesesand development process in [15], the double support phasecan be modeled as a discrete map between ˙ q − and ˙ q + (thevelocity of the system just before and after impact). Theirrelationship can be described as: (cid:20) D ( q ) − J TL ( q ) J L ( q ) 0 (cid:21) (cid:20) ˙ q + f L (cid:21) = (cid:20) D ( q ) ˙ q − (cid:21) , (3)where f L := (cid:82) t + t − δf L ( t ) dt is the integration of the impulsivecontact force over the impact instant. J L ( q ) is the Jacobianmatrix of the left foot contact point. J L ( q ) has full row rankand D ( q ) is positive definite, thus we can invert the left sideof (3) and project to get the map between ˙ q − and ˙ q + , ˙ q + = ∆( q ) ˙ q − . (4)Note that if the swing foot impact velocity is 0, the impactwill not change the CoM velocity. B. CoM Related Outputs
Each gait in the gait library contains time trajectoriesof the selected quantities to be controlled, these quantitiesare termed as ’outputs’. In existing work, joint angles areusually selected as outputs [7], [10] for direct use of joint-level control. Recently, workspace quantities are also usedfor their intuitive physical meanings [16], which can thenbe combined with an OSC. In this paper, CoM relatedquantities are selected as outputs to support the CoM basedgait synthesis in Sec. IV.pecifically, 10 quantities are selected as outputs, theyare listed in Tab. I. The CoM x and y positions are notstrictly enforced as the stance foot roll and pitch are in factweakly actuated for limited sole area, these two actuationsare used in the controller to regulate the pre-impact CoMvelocity for robots with active feet. For underactuated bipedalrobots, certain quantities are removed from the outputs. Forexample, for a bipedal robot with fully-passive feet, φ foot , θ foot should be removed. TABLE IS
ELECTED O UTPUTS
Torso roll, pitch and yaw φ torso , θ torso , ψ torso Vertical position of the CoM z COM
Swing foot x and y positions x foot relative to the CoM y foot Swing foot vertical position z foot Swing foot roll, pitch and yaw φ foot , θ foot , ψ foot C. Periodic Gaits Optimization
Fig. 2. The pre-impact CoM velocities of periodic sagittal and lateralmovements.
As shown in Fig. 2, for a periodic gait, the sagittal pre-impact CoM velocity ( v − x ) repeats every step, while thelateral pre-impact CoM velocity ( R v − y and L v − y ) repeatsevery two steps. Therefore, in order to cover different pe-riodic sagittal and lateral movements, we need to build agait library with at least four dimensions [ T, v − x , R v − y , L v − y ] .As the development of trajectory optimization tools such asFROST[17] and C-FROST [18], thousands of trajectories canbe optimized simultaneously and efficiently, which makes theconstruction of this gait library possible.Periodic walking gaits with different sagittal, lateral aver-age velocities and different lateral velocity differences areoptimized for multiple periods. The difference of lateralvelocities δv − y = R v − y − L v − y is also constrained as differentcombinations of R v − y and L v − y can realize the same averagelateral velocity. Each optimization problem is performed overtwo steps with the right and left leg being the stance legsuccessively. The cost function used in the optimization isthe sum square of power:Cost = (cid:90) T || τ · dq || dt. (5) Constraints enforced in the optimization are listed in Tab. II.The swing foot impact velocity is constrained to 0 so that v + x = v − x , v + y = v − y . (6) TABLE IIC
ONSTRAINTS USED IN GAIT OPTIMIZATION
Average Sagittal Velcoity ¯ v x,i Average Lateral Velcoity ¯ v y,i Difference of Lateral Velocity δv − y,i Period T i Friction Cone µ = 0 . Mid-step Swing Foot Height . m Swing Foot Impact Velocity (0 , , m/s Joint Position, Velocity and DeterminedTorque Limits by hardware
After these optimizations, we acquire output trajectoriesfor different gaits. All these trajectories are parameterizedwith B´ezier polynomials in terms of the normalized time s = t/T ∈ [0 , . The i th output h id can be represented as h id ( s ) = M +1 (cid:88) j =1 α ( i, j ) M !( j − M − j + 1)! s j − (1 − s ) M − j +1 , (7)where M is the order of the B´ezier polynomial, N is thenumber of outputs, α ∈ R N × ( M +1) is the parameter matrix.Thus, each gait can be represented with a parameter matrixlabeled with its unique [ T, v − x , R v − y , L v − y ] , i.e., α Tv − x , R v − y , L v − y . Remark 1:
A simplified gait library can be constructedwith 2 dimensions [ T, v − x ] , the lateral periodic gait is ap-proximately calculated online using the LIP model [19]: y foot ( T ) = v − y − dσ , (8)where σ = λtanh ( T λ ) , λ = (cid:113) g ¯ z COM , d = λ sech ( T λ ) T σ ¯ v y , T is the current period, ¯ z COM is the average COM height ofcurrent gait. Then y foot ( T ) is the swing foot y-axis impactposition that can keep the lateral movement in a two-stepperiodic gait with average lateral velocity ¯ v y . Remark 2:
Standing motion can be considered as a pe-riodic trajectory with an ∞ period. The posture at thebeginning of a gait with zero average sagittal and lateralvelocity can be directly used for standing.IV. C ENTROIDAL M ODEL P REDICTIVE G AIT S YNTHESIZER
In this section, we propose the MPC style gait synthesizer.It provides a feasible solution of (1) by first predicting thepre-impact CoM states ˙ p − [ i ] , p − [ i ] according to current ˙ p, p and then synthesizing gaits accordingly, as shown in Fig. 3. A. Pre-Impact CoM States Prediction
The pre-impact CoM states ˙ p − , p − are first predictedaccording to current ˙ p, p . The centroidal dynamics of a robot ig. 3. Our proposed gait synthesizer first predicts ˙ p − [ i ] , p − [ i ] , thensynthesizes a gait that drives ˙ p + [ i + 2] , p + [ i + 2] towards ˙ p + ∗ , p + ∗ . can be described as: ¨ z = 1 m f z − g ¨ p = 1 m f p , (9)where z is the vertical position of the CoM, p representsthe horizontal position of the CoM, which can be either xor y axis position, f p and f z are ground reaction forces, m is the total mass, g is the acceleration of gravity. As theswing and stance legs are generally symmetric and the torsois kept upright, the centroidal angular momentum change rateis assumed to be 0, i.e. f p z − f z p = 0 (10)Combine (9) and (10), we can have (cid:20) ¨ z ¨ p (cid:21) = m − pmz (cid:20) f z g (cid:21) , (11)where f z is determined by the PD controller, f z = (¨ z ∗ + k p ( z ∗ − z ) + k d ( ˙ z ∗ − ˙ z ) + g ) m, (12)where z ∗ , ˙ z ∗ , ¨ z ∗ are current CoM vertical reference trajectoryand its derivatives, k p and k d are controller parameters.Rearrange (11) and (12), we can have a nonlinear state spacemodel ˙ X = f predict ( X, z ∗ , ˙ z ∗ , ¨ z ∗ , t ) , (13)where X = [ z, p, ˙ z, ˙ p ] T . Given current state X ( t ) , the pre-impact states can be predicted by numerical integration, X − = X ( T ) = (cid:90) Tt f predict ( X, z ∗ , ˙ z ∗ , ¨ z ∗ , t ) dt. (14)For standing gait, the CoM height is constant, the CoMstates can be predicted using the LIP model. Since T = ∞ ,we want to predict the CoM states in infinite future. If thesupport region is large enough, ˙ p will eventually come to 0,and p will be at the Capture Point when ˙ p = 0 , i.e., p − = p ( ∞ ) = p + ˙ p/ (cid:112) g/z ˙ p − = ˙ p ( ∞ ) = 0 (15)If p ( ∞ ) is outside the support region, the robot can not remain standing eventually. B. Gait Synthesis Algorithm
Assume the gait library has k periods, l sagittal pre-impact velocities and n pair of lateral pre-impact velocities.The set of periods is S T = { T , T , · · · , T k } in descend-ing order. The sets of pre-impact velocities are S v − x = { v − x, , v − x, , · · · , v − x,l } , S R v − y = { R v − y, , R v − y, , · · · , R v − y,n } and S L v − y = { L v − y, , L v − y, , · · · , L v − y,n } in which velocitiesare in ascending order. From now on, the right leg is assumedto be the stance leg, subscripts R and L can be swapped forthe left leg case. The gait synthesis algorithm is summarizedby the pseudo code in Algorithm 1. Algorithm 1
Gait Synthesis Algorithm Input: p , ˙ p , t , j ∗ T , ˙ p + ∗ , i Output: α Initialize:
F lag = 0 for j = j ∗ T , j ∗ T + 1 , · · · , k do T = S T { j } Predict p − [ i ] , ˙ p − [ i ] with (14) or (15) if ( p − [ i ] , ˙ p − [ i ]) ∈ S feasible then F lag = 1 Break; end if end for if F lag == 1 then
Gait Interpolation: ˙ p − y,d [ i ] = R ˙ p + ∗ y + L ˙ p + ∗ y − ˙ p − y [ i ] ,α = G intp ( T, ˙ p − x [ i ] , ˙ p − y [ i ] , ˙ p − y,d [ i ]) . (16) Gait Modification: α (5 , M + 1) = α (5 , M + 1) + k x ( ˙ p − x [ i ] − ˙ p + ∗ x ) ,α (6 , M + 1) = α (6 , M + 1) − k y ( ˙ p − y [ i ] − L ˙ p + ∗ y ) . (17) else Prepare for falling end if
In lines 1-3 the input, output and initialization of thealgorithm are defined. The input includes current robot states p, ˙ p , step time t , the index j ∗ T of the desired period T ∗ ( T ∗ = S T { j ∗ T } ) and the desired post-impact velocities ˙ p + ∗ , i isthe current step number. The output is the synthesized gaitparameter α . The variable F lag is the flag for finding afeasible α .In lines 4-11 the gait period is first determined by travers-ing the period set S T from the j ∗ T -th element, until a feasible T is found. A T is feasible if the predicted ( p − [ i ] , ˙ p − [ i ]) withthis T is in S feasible . The feasible set S feasible is defined as S feasible = { ( p − , ˙ p − ) | p − ∈ S CoM , g foot ( T, ˙ p − ) ∈ S foot } , (18)where g foot ( T, ˙ p − ) calculates the predicted pre-impactswing foot position, S CoM and S foot are feasible regionsof the CoM and swing foot positions, they are designedto satisfy the workspace limit and avoid foot collision. Ifhe predicted ( p − [ i ] , ˙ p − [ i ]) with current T is not feasible,a smaller T is tested as decreasing step time enlarges thefeasible region. Note that this procedure is executed at eachsample time, thus T may be changed anytime during a step.In line 13 the gait parameter α is synthesized if a feasibleperiod T is found. First, the gait library is interpolated bytri-linear interpolation: G intp ( T, ˙ p − x [ i ] , ˙ p − y ][ i ] , ˙ p − y,d [ i ]) =+ α Tv − x,u , R v − y,v , L v − y,w (1 − ξ )(1 − ξ )(1 − ξ )+ α Tv − x,u +1 , R v − y,v , L v − y,w ξ (1 − ξ )(1 − ξ )+ α Tv − x,u , R v − y,v +1 , L v − y,w (1 − ξ ) ξ (1 − ξ )+ α Tv − x,u , R v − y,v , L v − y,w +1 (1 − ξ )(1 − ξ ) ξ + α Tv − x,u +1 , R v − y,v , L v − y,w +1 ξ (1 − ξ ) ξ + α Tv − x,u +1 , R v − y,v +1 , L v − y,w ξ ξ (1 − ξ )+ α Tv − x,u , R v − y,v +1 , L v − y,w +1 (1 − ξ ) ξ ξ + α Tv − x,u +1 , R v − y,v +1 , L v − y,w +1 ξ ξ ξ , (19)if v − x,u ≤ ˙ p − x [ i ] ≤ v − x,u +1 , R v − y,v ≤ ˙ p − y [ i ] ≤ R v − y,v +1 , L v − y,w ≤ ˙ p − y,d [ i ] ≤ L v − y,w +1 , where ξ = ˙ p − x [ i ] − v − x,u v − x,u +1 − v − x,u , ξ = ˙ p − y [ i ] − R v − y,vR v − y,v +1 − R v − y,v , ξ = ˙ p − y,d [ i ] − L v − y,wL v − y,w +1 − L v − y,w , ≤ u < l , ≤ v < n , ≤ w < n . This interpolated gait is approximately aperiodic gait, which can drive ˙ p − x [ i + 1] , ˙ p − y [ i + 1] to a verysmall neighbor of ˙ p − x [ i ] , ˙ p − y,d [ i ] as explained in (22).In line 14 α is partially modified to regulate ˙ p − [ i + 1] towards ˙ p + ∗ with a discrete P-type controller, α (5 , M + 1) and α (6 , M + 1) represent the pre-impact swing foot posi-tions x foot and y foot respectively * . This type of controllerhas been successfully implemented in [10], [19], [20].After a new gait is synthesized, s is updated with the newperiod T by s = s + t s /T , where t s is the sample time.Then whole-body motion trajectories are calculated by (7).In lines 15-17 the robot prepares for falling if no feasiblegait is found. C. Stability Analysis
In this section, we present the proof for stability of p and ˙ p by showing that the generated gait of our gait synthesizeris a feasible solution of (1). First, some definitions are given.The velocity range of the gait library is S v = { ( v − x , R v − y , L v − y ) | v − x, ≤ v − x ≤ v − x,l , R v − y, ≤ R v − y ≤ R v − y,n , L v − y, ≤ L v − y ≤ L v − y,n } . (20)The poincar´e map of the pre-impcat CoM velocities betweensteps are defined as P map , i.e., [ ˙ p − x [ i + 1] , ˙ p − y [ i + 1]] = P map ( α i , ˙ p − x [ i ] , ˙ p − y [ i ]) , (21)where α i is the gait parameter implemented for the i th step.Then two assumptions are given about gait interpolationand modification. * x foot and y foot are the 5 th and 6 th outputs in Tab. I, they areparameterized by the 5 th and 6 th rows of α , and for a B´ezier polynomial,the last coefficient equals to the end value, i.e., the pre-impact value. Assumption 1.
There exist (cid:15) x , (cid:15) y ∈ R + such that, for any [ v − x , R v − y , L v − y ] ∈ S v and α intp = G intp ( T, v − x , R v − y , L v − y ) , [ P xmap , P ymap ] = P map ( α intp , v − x , R v − y ) satisfy: | P xmap − v − x | ≤ (cid:15) x , | P ymap − L v − y | ≤ (cid:15) y . (22) Assumption 2.
There exist δ x , δ y ∈ R − such that, for any [ v − x , R v − y , L v − y ] ∈ S v and α intp = G intp ( T, v − x , R v − y , L v − y ) , [ P xmap , P ymap ] = P map ( α intp , v − x , R v − y ) satisfy: δ x ≤ ∂P xmap ∂α (5 ,M +1) (cid:12)(cid:12)(cid:12) [ α intp ,v − x , R v − y ] < ,δ y ≤ ∂P ymap ∂α (6 ,M +1) (cid:12)(cid:12)(cid:12) [ α intp ,v − x , R v − y ] < . (23)The first assumption is based on the fact that each gait α Tv − x , R v − y , L v − y in the periodic gait library satisfies P map ( α Tv − x , R v − y , L v − y , v − x , R v − y ) = [ v − x , L v − y ] , (24)thus the interpolated gait should approximately satisfy (24)at the in-between velocities with very small errors (cid:15) x and (cid:15) y .They are bounded by the grid size of the gait library, as thegrid size decreases, (cid:15) x and (cid:15) y converge to 0.The second assumption indicates that finite adjustment ofthe footstrike location around α intp makes finite change of ˙ p − in the opposite direction, and the change ratio is bounded.This can be case-checked for any [ v − x , R v − y , L v − y ] ∈ S v through numerical simulation. (cid:4) Proposition 1.
Each element g intp ( T, V ) of G intp ( T, V ) isLipschitz continuous for V ∈ S v , i.e., for all V , V ∈ S v , | g intp ( T, V ) − g intp ( T, V ) | ≤ K || V − V || . (25) where K is the Lipschitz constant, || . || represents 2-norm. The proof is provided in the appendix. (cid:4)
Theorem 1.
The generated gait of our gait synthesis algo-rithm is a feasible solution of (1), if < k x < − δ x , < k y < − δ y . (26) Proof: (Assume the right leg is the stance leg for step i )We prove for the lateral direction first since it is morecomplex, the sagittal direction is similar. First, we show that lim i →∞ | ˙ p + y [ i + 2] − R ˙ p + ∗ y | ≤ c (27)According to (16),(17): ˙ p − y [ i + 1] = P ymap − ∂P ymap ∂α (6 , M + 1) k y ( ˙ p − y [ i ] − L ˙ p + ∗ y ) . (28)Furthermore, (6) indicates that ˙ p + [ i + 2] = ˙ p − [ i + 1] .Thus | ˙ p + y [ i + 2] − R ˙ p + ∗ y | = | ˙ p − y [ i + 1] − R ˙ p + ∗ y | = | P ymap − ˙ p − y,d + (1 + ∂P ymap ∂α (6 ,M +1) k y )( L ˙ p + ∗ y − ˙ p − y [ i ]) | ≤| ∂P ymap ∂α (6 ,M +1) k y | · | ˙ p + y [ i + 1] − L ˙ p + ∗ y | + | P ymap − ˙ p − y,d | , (29)where ˙ p − y,d [ i ] = R ˙ p + ∗ y + L ˙ p + ∗ y − ˙ p − y [ i ] .According to (22),(23) and (26), | P ymap − ˙ p − y,d | ≤ (cid:15) y , | P ymap /∂α (6 , M + 1) k y | ≤ | δ y k y | < , thus | ˙ p + y [ i +2] − R ˙ p + ∗ y | ≤ | δ y k y |·| ˙ p + y [ i +1] − L ˙ p + ∗ y | + (cid:15) y (30)Then lim i →∞ | ˙ p + y [ i + 2] − R ˙ p + ∗ y | ≤ (cid:15) y − | δ y k y | (31)Hence (27) is proved with c = (cid:15) y −| δ y k y | . Similarly, wecan prove lim i →∞ | ˙ p + x [ i + 2] − ˙ p + ∗ x | ≤ c . (32)Next we show that lim i →∞ | p + y [ i + 2] − R p + ∗ y | ≤ c . (33)The pre-impact swing foot position is part of the outputof G intp (as explained in Sec. IV-B * ), noted as g foot . Thepost-impact CoM position equals to the negative value of thepre-imapct swing foot position, i.e., p + [ i + 1] = − p − foot [ i ] .Thus for the desired periodic gait, p + ∗ and ˙ p + ∗ should satisfy R p + ∗ y = − g yfoot ( T, ˙ p + ∗ x , R ˙ p + ∗ y , L ˙ p + ∗ y ) . (34)Consider the i + 1 th step, according to (16),(17), p + y [ i + 2] = − g yfoot ( T, ˙ p − x [ i + 1] , ˙ p − y [ i + 1] , ˙ p − y,d [ i + 1])+ k y ( ˙ p − y [ i + 1] − R ˙ p + ∗ y ) (35)Thus, | p + y [ i + 2] − R p + ∗ y | = | g yfoot ( T, ˙ p − aug [ i + 1]) − k y ( ˙ p − y [ i + 1] − R ˙ p + ∗ y ) − g yfoot ( T, ˙ p + ∗ ) |≤ | g yfoot ( T, ˙ p − aug [ i + 1]) − g yfoot ( T, ˙ p + ∗ ) | + k y | ˙ p − y [ i + 1] − R ˙ p + ∗ y | (36)where ˙ p − aug [ i + 1] = [ ˙ p − x [ i + 1] , ˙ p − y [ i + 1] , ˙ p − y,d [ i + 1]] T .Then, according to (25) and (6) | p + y [ i + 2] − R p + ∗ y |≤ K || ˙ p − aug [ i + 1] − ˙ p + ∗ || + k y | ˙ p − y [ i + 1] − R ˙ p + ∗ y | = K (cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12) ˙ p + x [ i + 2] − ˙ p + ∗ x ˙ p + y [ i + 2] − R ˙ p + ∗ y ˙ p + y [ i + 2] − R ˙ p + ∗ y (cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12) + k y | ˙ p + y [ i + 2] − R ˙ p + ∗ y | (37)According to (32) and (27), we have lim i →∞ | p + y [ i + 2] − R p + ∗ y | ≤ K (cid:113) c + 2 c + k y c (38)Thus (33) is proved with c = K (cid:112) c + 2 c + k y c .Similarly, we can prove that lim i →∞ | p + x [ i + 2] − p + ∗ x | ≤ c . (39)Additionally, the generated gait is synthesized from gaitsoptimized based on the full dimensional model. Thus thegenerated gait of our gait synthesis algorithm is a feasiblesolution of (1) † . (cid:4) † All the constants c , c , c , c are functions of (cid:15) x , (cid:15) y , as (cid:15) x , (cid:15) y decreaseto 0, they also decrease to 0. V. I
MPLEMENTATION R ESULTS
This section presents simulation and experimental resultsof the proposed gait synthesizer. An 8-DoF bipedal robotis used in the simulation. As shown in Fig. 4, it hasfour actuators each leg, they are for hip abduction, hipflexion, knee and ankle respectively. A physical robot is builtaccording to this model with passive ankles, actuated anklesare to be added in the future. This robot is 0.65m high instanding pose and weighs 16 kg.
Fig. 4. Bipedal robots for simulation and experiment.
A. Simulation Results
We first show simulation results on the 8-DoF robotmodel. The gait library is constructed with 3 peri-ods {∞ , . , . } ( s ) and 8 sagittal average velocities {− . , − . , − . , , . , . , . , . } ( m/s ) , the peri-odic gait for the lateral direction is calculated using (8). Thecontroller used in the simulation is a QP-based operationalspace controller, similar to the one used in [21]. The gaitmodification parameters are k x = 0 . , k y = 0 . , samefor all simulations. Fig. 5. The first two figures show the velocities of the robot walkingand standing following the user command T, ˙ p + ∗ x , R ˙ p + ∗ y , L ˙ p + ∗ y . The thirdfigure shows the gait period, T is ∞ in the blank segments, i.e., the robotis standing. The first simulation is walking and standing following theuser command. As shown in Fig. 5, the third figure showshe desired T , which starts from ∞ , then varies among 0.2sand 0.35s, and finally returns to ∞ . Thus, the robot startedfrom standing, then transitioned to walking following thedesired velocity commands, finally returned to standing. Wecan see that the robot followed the desired ˙ p + ∗ x , R ˙ p + ∗ y , L ˙ p + ∗ y very responsively and accurately. TABLE IIIT
HE TIME AND MAGNITUDE OF IMPULSES . Time 4s 8s 12s 16s 20s 24sSagittal 5Ns 7Ns 0 0 9Ns 9NsLateral 0 0 2Ns 4Ns -6Ns 6Ns
Time (s) P e r i od ( s ) Time (s) -0.8-0.6-0.4-0.200.20.40.60.8 V e l o c t i y ( m / s ) Fig. 6. The first figure shows the velocities of the robot under impulsedisturbances. The second figure shows the gait period, T is ∞ in the blanksegments, i.e., the robot is standing. The second simulation is the push-recovery test. The robotstarted from standing. Six impulses with different magnitudeswere applied to the robot in the sagittal and lateral directions.The magnitude and applied time of these impulses are shownin Tab. III. As shown in Fig. 6, for the first and thirdimpulses, the robot recovered to steady state purely by thestanding controller. While for other cases, the robot detectedthat it can not remain standing and automatically took stepswith appropriate periods and then returned to standing. Therobot recovered from instant velocity change up to 0.7m/s inthe sagittal direction and 0.5m/s in the lateral direction.
Time (s) P e r i od ( s ) Time (s) -0.6-0.4-0.200.20.40.6 V e l o c i t y ( m / s ) Fig. 7. The first figure shows the velocities of the robot walking over theuneven terrain blindly. The second figure shows the gait period.
The third simulation is the rough terrain test. As shown in Fig. 8, the robot blindly walked over a rough terrain with15 degree slopes, 5cm stairs and 2-5cm boards. The desiredspeed is 0.4m/s and the nominal period is 0.35s. The velocityand gait period are shown in Fig. 7, we can see that the robotsuccessfully passed this terrain with small velocity variation.
Fig. 8. The left picture shows the rough terrain with 15 degrees slopes,5cm stairs and 2-5cm boards. The right picture shows the robot is hit by a5kg wall ball while stepping in place.
B. Experimental Results
Finally we apply the proposed gait synthesizer to thephysical robot in Fig. 4. The same gait library and controllerwere used in experiments, and the robot achieved stablewalking with maximal speed 0.7m/s. The result of the push-recovery experiment is presented here. As shown in Fig. 8,the robot was hit by a 5kg (one third of its body weight)wall ball in the sagittal direction for 6 times while steppingin place. The sagittal velocity and gait period are shown inFig. 9, we can see the robot recovered from instant velocitychange up to 0.8m/s.
Time (s) -0.4-0.200.20.40.60.8 V e l o c i t y ( m / s ) Time (s) P e r i od ( s ) Fig. 9. The first figure shows the sagittal velocity of the robot during thepush recovery experiment. The second figure shows the gait period.
VI. CONCLUSIONThe results in this paper provide a methodology towardsfull dimensional model based real-time motion planningfor bipedal locomotion with theoretical stability guarantee.We showed that our proposed gait synthesizer can providefeasible solutions for the constructed MPC optimizationproblems quickly online, which leads to fast online planningat 1kHz. The proof of stability is provided by showing thatthe pos-impact CoM states of the robot are stable. Simulationand experimental results showed that, with this proposedpproach, robots can achieve flexible transitions betweenstanding and walking, accurate velocity tracking with differ-ent step periods, robust locomotion under disturbances andpassing uneven terrains.This paper also provides a perspective that offline com-putation of whole-body dynamics and online computationof concentrated dynamics (such as the centroidal dynamics)can be combined to achieve real-time whole-body motionplanning considering whole-body dynamics and constraints.Future work will be extending current work to locomotionplanning with terrain knowledge. While robots can accom-modate simple rough terrains blindly with current method,passing more complex terrains still needs a perceptive plan-ner. Exploring more intelligent methods of combining whole-body dynamics and centroidal dynamics for motion planningis also a future direction.APPENDIXProof of Proposition 1: S v is split into ( l − × ( n − × ( n − subsets S i,j,kv = { V = [ v , v , v ] (cid:12)(cid:12) v − x,i ≤ v ≤ v − x,i +1 , R v − y,j ≤ v ≤ R v − y,j +1 , L v − y,k ≤ v ≤ L v − y,k +1 } . (40)In each subset, every element of G intp ( T, V ) can be writtenas a cubic function according to (19) g intp ( T, V ) = a v v v + a v v + a v v + a v v + a v + a v + a v + a , (41)where a to a are constants. It can be proved that g intp ( T, V ) is Lipschitz continuous in each subset, by show-ing that each of its component is Lipschitz continuous. Thispart is omitted due to the space limit.Additionally, S v is convex according to (20), thus for V , V ∈ S v , the line connecting V , V passes finite numberof subsets of S v and intersects the boundaries of subsets at o points ( V m, , V m, , · · · , V m,o ) and || V − V || = || V − V m, || + || V m, − V m, || + · · · + || V m,o − − V m,o || + || V m,o − V || (42)Let K be the largest Lipschitz constant of all subsets forall g intp . As g intp ( T, V ) is continuous at the boundaries, wecan have | g intp ( T, V ) − g intp ( T, V ) | = | g intp ( T, V ) − g intp ( T, V m, ) + · · · + g intp ( T, V m,o ) − g intp ( T, V ) |≤ | g intp ( T, V ) − g intp ( T, V m, ) | + · · · + | g intp ( T, V m,o ) − g intp ( T, V ) |≤ K || V − V m, || + · · · + K || V m,o − V || . (43)Then according to (42), we can have | g intp ( T, V ) − g intp ( T, V ) | ≤ K || V − V || , which completes the proof.R EFERENCES[1] M. H. Raibert,
Legged robots that balance . MIT press, 1986.[2] S. Kajita, H. Hirukawa, K. Harada, and K. Yokoi,
Introduction tohumanoid robotics . Springer, 2014, vol. 101. [3] 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.[4] H. Dai, A. Valenzuela, and R. Tedrake, “Whole-body motion planningwith centroidal dynamics and full kinematics,” in . IEEE,2014, pp. 295–302.[5] M. Posa, S. Kuindersma, and R. Tedrake, “Optimization and stabiliza-tion of trajectories for constrained dynamical systems,” in . IEEE,2016, pp. 1366–1373.[6] S. Dafarra, G. Romualdi, G. Metta, and D. Pucci, “Whole-body walk-ing generation using contact parametrization: A non-linear trajectoryoptimization approach,” in . IEEE, 2020, pp. 1511–1517.[7] E. R. Westervelt, J. W. Grizzle, and C. C. De Wit, “Switchingand pi control of walking motions of planar biped walkers,”
IEEETransactions on Automatic Control , vol. 48, no. 2, pp. 308–312, 2003.[8] M. J. Powell, A. Hereid, and A. D. Ames, “Speed regulation in 3drobotic walking through motion transitions between human-inspiredpartial hybrid zero dynamics,” in . IEEE, 2013, pp. 4803–4810.[9] V. Murali, A. D. Ames, and E. I. Verriest, “Optimal walking speedtransitions for fully actuated bipedal robots,” in . IEEE, 2019, pp. 6295–6300.[10] X. Da, O. Harib, R. Hartley, B. Griffin, and J. W. Grizzle, “From 2ddesign of underactuated bipedal gaits to 3d implementation: Walkingwith speed tracking,”
IEEE Access , vol. 4, pp. 3469–3478, 2016.[11] S. Feng, “Online hierarchical optimization for humanoid control,”Ph.D. dissertation, Carnegie Mellon University, 2016.[12] H. Wang and M. Zhao, “A robust biped gait controller using steptiming optimization with fixed footprint constraints,” in .IEEE, 2017, pp. 1787–1792.[13] M. Khadiv, A. Herzog, S. A. A. Moosavian, and L. Righetti, “Walkingcontrol based on step timing adaptation,”
IEEE Transactions onRobotics , 2020.[14] T. Koolen, T. De Boer, J. Rebula, A. Goswami, and J. Pratt,“Capturability-based analysis and control of legged locomotion, part 1:Theory and application to three simple gait models,”
The InternationalJournal of Robotics Research , vol. 31, no. 9, pp. 1094–1113, 2012.[15] E. R. Westervelt, J. W. Grizzle, C. Chevallereau, J. H. Choi, andB. Morris,
Feedback control of dynamic bipedal robot locomotion .CRC press, 2018.[16] Y. Gong, R. Hartley, X. Da, A. Hereid, O. Harib, J.-K. Huang, andJ. Grizzle, “Feedback control of a cassie bipedal robot: Walking,standing, and riding a segway,” in . IEEE, 2019, pp. 4559–4566.[17] A. Hereid and A. D. Ames, “Frost: Fast robot optimization andsimulation toolkit,” in . IEEE, 2017, pp. 719–726.[18] A. Hereid, O. Harib, R. Hartley, Y. Gong, and J. W. Grizzle, “Rapidtrajectory optimization using c-frost with illustration on a cassie-seriesdynamic walking biped,” in . IEEE, 2019, pp. 4722–4729.[19] X. Xiong and A. D. Ames, “Orbit characterization, stabilization andcomposition on 3d underactuated bipedal walking via hybrid passivelinear inverted pendulum model,” in . IEEE, 2019,pp. 4644–4651.[20] S. Rezazadeh, C. Hubicki, M. Jones, A. Peekema, J. Van Why,A. Abate, and J. Hurst, “Spring-mass walking with atrias in 3d: Robustgait control spanning zero to 4.3 kph on a heavily underactuatedbipedal robot,” in . ASME, 2015.[21] T. Apgar, P. Clary, K. Green, A. Fern, and J. W. Hurst, “Fast onlinetrajectory optimization for the bipedal robot cassie.” in