Model-Based Predictive Bipedal Walking Stabilization

Robert Wittmann, Arne-Christoph Hildebrandt, Daniel Wahrmann, Felix Sygulla, Daniel Rixenand Thomas Buschmann1

Abstract— A well known strategy in bipedal locomotion toprevent falling in the presence of large disturbances is to modifydrastically future motion. This is an important capability of awalking control system in order to bring humanoid robots fromcontrolled laboratory conditions to real environment situations.This paper presents a predictive stabilization method whichmodifies planned center of mass and foot trajectories dependingon the current state of the robot. It uses a nonlinear predictionmodel [1] and applies a conjugate gradient method to solvethe resulting optimization problem in real-time. Furthermore,the method is integrated in the walking control system ofour bipedal robot LOLA. Simulation results demonstrate theeffectiveness and the advantages of the proposed method.

I. INTRODUCTION

There are several critical tasks that have to be solvedif humanoid robots should operate in arbitrary unknownenvironments. In order to avoid falling and colliding withthe environment, the walking system has to be flexible androbust. This means that it can quickly adapt to new andunknown situations. Therefore it has to first perceive theenvironment, detect obstacles and surfaces and adapt itsmotion in order to avoid collisions [2], [3]. Second, it hasto be able to react to unknown disturbances which resultfrom perception errors, unknown external forces or unde-tected environment properties (e.g. soft ground). For largedisturbances this often requires a method to perform not onlylocal modifications but also an adaptation of future motion.This paper deals with the second task and presents a methodto adapt the robot’s ideal planned motion according to sensordata. In addition to our previously published method [4]which modifies swing foot trajectories we propose a methodfor modifying the horizontal motion (x and y, cf. frame ofreference in Fig. 1) of the center of mass (CoM) in this paper.

The paper is organized as follows: Section II gives anoverview of related work. A description of the problemand the walking control system of our biped is given inSection III. The walking stabilization algorithm is describedin Section IV. Section V presents first results obtained fromthe proposed method and Section VI concludes the paper.

II. RELATED WORK

There are many different methods for bipedal walkingstabilization. This section starts with two control frameworksthat include local modifications as well as an adaptation offuture motion. A feedback control framework for the bipedHRP2 is presented in [5], [6]. It is based on a continuous re-calculation of the walking pattern with a moderate frequency

1Institute of Applied Mechanics, Technische Universitat Munchen, 85748Garching, Germany. E-mail: robert.wittmann@tum.de

x y

z

Joint DoFHead 2Shoulder 2Elbow 1Pelvis 2Hip 3Knee 1Ankle 2Toe 1Total 24

Fig. 1: Photograph and kinematic structure of LOLA.

and local adaption of the trajectories with a high frequency.The current state is used as initial value for the trajectoryplanning problem which is formulated as a preview controlof the linear inverted pendulum model (LIPM). Ref. [7],[8] present a stabilizer of the humanoid robot ASIMO. Themain feedback variable is the absolute inclination of theupper body which is treated as horizontal displacement errorof the CoM and is used to calculate a reaction moment.This reaction moment aims to restore an upright posture.The regulation of the moment is then distributed on threecontrol strategies which are a ground reaction force control,model ZMP control and foot landing position control. Awalking controller that is based on the full robot model wasdeveloped in [9] for the biped robot JOHNNIE. The authorsused a feedback-linearization technique in order to imposea linear behavior for the tracking errors. Buschmann etal. [10] presents a stabilizer that first modifies desired contactforces and torques and second applies a hybrid position/forcecontrol which generates local taskspace modifications. Twosimilar approaches for a whole-body motion controller whichalso considers long term stability based on the LIPM arepresented in [11], [12]. The stabilizer solves a quadraticprogramming problem for the overall multibody dynamicsat each time step which considers e.g. dynamic constraintsof the contact forces and joint torques.

One strategy that consists of adaptations of future motionis the sensor based calculation of next foot steps. Forthe stabilizing step length modifications there are severalapproaches that apply heuristics or linear models [13], [14],[15]. The capture point introduced by [13] is an often appliedmethod for footstep placement and bipedal walking control

[16]. Online model predictive control methods to calculatea CoM trajectory and optimize the next footsteps using theLIPM are presented in [17], [18], [19]. The authors of [17]formulate the optimal control problem for the pendulum in adifferent way by choosing the time derivative of the CoPas input and setting the weight of the input in the costfunction to zero. This allows to give an explicit solution ofthe problem and to compute more than hundred iterationsof the optimization in each control cycle. To our knowledgethis is the only work that includes an additional step timeoptimization.

Even though these approaches present convincing resultsin experiments we believe that methods using an optimiza-tion together with more accurate models are necessary toimprove the overall robustness of bipedal walking robots inarbitrary unknown situations. Our model directly resolves theunilateral contacts and has two passive DoFs. This offers newpossibilities for motion generation in rough terrain situationsor under large disturbances as will be discussed later.

III. SYSTEM OVERVIEW AND PROBLEM DESCRIPTION

This section gives a short overview of the dynamics ofbipedal locomotion and describes the bipedal robot LOLAand its walking control system. This information will be usedin the next sections to state the main goal of our proposedbipedal walking stabilization.

A. Problem DescriptionBipedal robots represent a class of mechanical systems

with different challenging properties for planning and con-trol. Beside their nonlinear multi-body dynamics with manydegrees of freedom (DoFs) these robots are, in contrast toindustrial manipulators, not fixed to the environment. Thisis a necessary property as bipedal locomotion requires asystem that can change periodically the contact states of thefeet between open and closed. Splitting the overall DoFsq

R

2 Rn of the robot into the free floating base located atthe torso q

T

2 R6 and the joints q

J

2 Rn�6 the partitioned[20] equations of motion (EoM) of the robot yieldM

TT

M

TJ

M

JT

M

JJ

� ¨

q

T

¨

q

J

�+

h

T

h

J

�=

0⌧

�+

J

T

�,T

J

T

�,J

�⇤. (1)

The parts of the mass matrix are denoted by M

ij

, hi

are thenonlinear vectors of Coriolis, centrifugal and gravitationalforces and ⌧ 2 Rn�6 are the joint torques. The contactforces and moments ⇤ 2 R12 are projected with the matricesJ

�,T

,J�,J

to the directions of q

T

and q

J

respectively.With ⌧ being the system input, the q

T

can not be directlycontrolled and the robot described by (1) is under-actuated.Physically feasible ground forces are limited because of theunilateral contact, resulting in inequality constraints. Theyhave to be accounted for in order to generate a feasiblemotion. Examples of such motion generation methods canbe found in [21], [22]. But even for perfect tracking (of q

J

)of a feasible motion there are always deviations from theideal planned state (especially q

T

). Reasons are modelingerrors, a finite compliance in the contacts between robot and

the ground and other external disturbances. One strategy tocontrol q

T

is to modify the contact forces ⇤ by changingthe joint trajectories q

J

. There were many different methodsdeveloped in order to achieve this modification, e.g. changingthe robot’s CoM motion [7], total angular momentum, footorientation [23] or horizontal foot positions [4].The following part gives an overview of the walking controlsystem of LOLA. Referring to the problem description thewalking pattern generation and the control strategy aredescribed.

B. System Overview

The experimental platform for this research is the bipedrobot LOLA shown in Fig. 1. It weights 60 kg, its total heightis 1.8m and has 24 electrically actuated joints. The jointconfiguration is depicted on the right hand side of Fig. 1. Therobot is equipped with an Inertial Measurement Unit (IMU)at the upper body and 6-axis force-torque sensors (FTS)located at each foot. The IMU consists of three fiber-opticgyroscopes and three MEMS accelerometers. The systemincluds internal sensor fusion algorithms which providesaccurate and drift-free measurements for the absolute orien-tation and rotation rate. Those quantities are converted intoabsolute inclination '

m

and inclination rate ˙

'

m

which arethe angles between the upper body and the gravity vector.Details of the mechatronic design can be found in [24].The control system has a hierarchical design shown in Fig.2. User commands via joystick or step parameter input aretranslated into an ideal step sequence which is then used inthe trajectory planning. The generated ideal walking patternin task–space w

id

consists of the center of mass (CoM)trajectory, upper body orientation, the pose of both feetand the toe angles. The task–space trajectories are usedas set points in the local stabilization. It is based on ahybrid force–position control that performs modificationsin task–space depending on sensor data. IMU and FTSmeasurements are used to modify the commanded stance footorientation and vertical position. Finally, desired joint anglesq

d

and velocities ˙

q

d

are calculated by inverse kinematics(redundancy resolution is done by automatic supervisorycontrol originally formulated by Liegois [25]). These arethen passed on to the local joint controllers. Following adecentralized concept, the local joint feedback loops run ondistributed controllers at high sampling rates (50 µs current,100 µs velocity and position), while the central control runsat 1 ms sampling rate on an embedded system. A detailedoverview of the walking controller is provided in [26].

In this work we present a method to modify the horizontalCoM motion as well as next foot positions in a model predic-tive control scheme. The main input is the estimated upperbody inclination which is a subset of the unactuated DoFsq

T

. The algorithm has to consider constraints for the CoMmotion due to the unilateral contact forces which themselvesdepend again on the foot positions. The method extendsour previously published algorithm [4] for optimizing nextfootsteps by an optimization of the robot’s horizontal CoMtrajectories.

User commands

Ideal Walking PatternGeneration

Predictive Bipedal WalkingStabilization

Local Stabilization &Inverse Kinematics

Position ControlledRobot

w

id

, ˙

w

id

w

mod

, ˙

w

mod

q

d

, ˙qd

1 msSensorData

'

m

, ˙

'

m

⇤

20 ms

Fig. 2: Overview of the walking control system.

IV. PREDICTIVE BIPEDAL WALKING STABILIZATION

A. Method Overview

The main goal of the presented method is to stabilizethe absolute inclination of the robot w.r.t. the ground. It isincluded in the unactuated DoF of (1). The measurementdata of the IMU ('

m

, ˙

'

m

) is filtered in a state observer[27] in order to estimate the absolute inclination in x-and y-direction ('

x

, ˙'x

, 'y

, ˙'y

). Following this approachall disturbances that are not related to joint tracking errorshave to be visible in the inclination errors. For rough terrainlocomotion, pushes and modeling errors, this turned out tobe sufficient information. Other state variables that could beused for feedback are the remaining part of q

T

or the statesof all contacts at the feet.The predictive stabilizer modifies ideal horizontal CoMtrajectories, and the overall swing foot trajectories (x, y, zposition and orientation about x- and y-axis �c

x

,�cy

1)producing the modified task–space motion w

mod

in Fig. 2.The horizontal CoM motion and final swing foot positionare optimized while the vertical swing foot location and�c

x

,�cy

are chosen such that early or late contact areavoided and the swing foot touches the ground horizontally(cf. [28], [4]). We choose to use a dynamic model of thebiped that includes the mentioned unactuated DoFs of q

T

naturally as well as the CoM and swing foot motion. Thisenables constructing a method to relate the optimizationvariables with the control goal. In the following the trajec-tory modification for the CoM acceleration is denoted byux

(t), uy

(t) in x- and y-direction and the modification ofthe next final swing foot location by �L

x

,�Ly

. A planarmodel is used and the problem is solved in a decoupled wayfor both directions. Consequently all relations shown beloware only described for one direction and can be easily appliedfor sagittal and lateral motion.

The goal of the presented stabilizer can be formulated

1refer to Fig. 1 for the global coordinate system definition

mathematically as a minimization of a quadratic cost function

J = �

ˆ

x

T

f

S

x

�

ˆ

x

f

+

tfZ

t0

�

ˆ

x

T

Q�

ˆ

x+Ru2 dt (2)

for a certain time horizon t 2 [t0, tf ]. The state error �

ˆ

x

is the difference between ideal and predicted state of themodel and will be defined in the next part. Note that thecost function weights S

x

,Q and R will be parameterized tomainly minimize the absolute inclination error over a certaintime horizon which is included in the state variable ˆ

x.

B. Dynamic Prediction Model

4r

f1

r

f1 = r

f1,id +4r

f1

r

b,id

r

b

= r

b,id

+4r

b

'x

z

T-System

mf

mb

,⇥yyz

T

zI

xI

xT

Fig. 3: Prediction model with three point masses, upper bodymodification �r

b

and swing foot modification �rf1.

The model proposed in [1] is motivated by the observationthat a more accurate model for stiff position controlledbipedal robots has to include the unactuated DoFs betweenrobot and ground. Therefore it has to consider the unilateraland compliant contacts. Starting from the full multi-bodymodel of a bipedal robot (1) the following simplifications aredone. The planned CoM and foot trajectories are assumedto be perfectly tracked in the robot’s planning frame ofreference (FoR) which rotates with the upper body (T-system). Inertia effects are approximately included via threepoint masses. The unactuated DoFs are the inclination '

x

in x-direction and the vertical translation z (horizontal trans-lation is neglected). They describe the transformation froman inertial (index I, x

I

, zI

) to a FoR rotating with theupper body of the robot (index T, x

T

, zT

). Furthermore,the trajectories for the robot’s upper body

T

rb

and the feetT

rf1, T

rf2 correspond to the ideal planned trajectories from

the walking pattern generation in the T-system. The upperbody has the mass m

b

and the inertia ⇥

yy

. Each foot has apoint mass m

f

and one point contact located at the middleof the foot. Those masses are not fixed to each other andfollow known trajectories which means that they are timevarying but no DoF. The contacts are linear spring-damperpairs (stiffness k

c

, damping dc

) with the values identifiedfrom the real robot’s rubber sole. They act only in z

I

-direction and have to be considered as unilateral in order tosimulate a walking cycle that necessarily requires switchingof the contact states between open and closed. This enables

the model to predict a divergence of 'x

. For a given foottrajectory

T

r

fi

(t) = [xfi

(t), 0, zfi

(t)]T and state vectorq = [z,'

x

]

T , the vertical contact distance is calculated

�zi

(q) = z � xfi

(t) sin'x

+ zfi

(t) cos'x

(3)

During integration (3) has to be compared with a neutralspring length z0 for each contact in order to set the corre-sponding contact active or inactive. With the inertial positionof upper body and feet obtained by a translation (z) androtation about the y-axis (A

y

(�'x

))

I

r

j

(q) = [0 , 0 , z]T +A

y

(�'x

)

T

r

j

, 8j 2 [b, f1, f2] (4)

the overall equations of motion of the model in Fig. 3 canbe stated in the form

M

p

(q, t) ¨q + h

p

(q, ˙q, t) = �p

(q, ˙q, t) + T

s

(5)

where M

p

is the mass matrix of the prediction model, hp

the nonlinear terms and �p

is the sum of all active contactsN

c

projected on the model’s DoFs

�

p

(q, ˙q, t) =X

i2Nc

@�zi

@q(�k

c,i

�zi

� dc,i

�zi

). (6)

T

s

= [0, sat(�Kp

�'x

� Kd

�'x

)]

T describes the effectof the local stabilization term for the model and includesan approximation of the rotational stiffness for finite sizedfeet. The gains K

p

and Kd

are manually tuned for a goodapproximation. Using the two–DoF–model ensures that aprediction can be done in real-time for a sufficient long timehorizon. Predicting 0.8 s takes approx. 4 · 10�5 s using anexplicit Euler integrator with a fixed time step of 3 ms. 2

The model considers the overall modified motion dueto the parameter �L

x

and the trajectory ux

(t) which isdescribed in the following. The footstep modification isused to calculate a quintic polynomial that begins at currentposition and ends and �L

x

. This is shown in Fig. 3 wherethe polynomial �

T

r

f1(�Lx

) is added to the ideal trajectoryT

r

f1,id via

T

r

f1 =

T

r

f1,id +�

T

r

f1 (�Lx

) (7)

which includes a modification of the swing foot height too(for inclination compensation). CoM modifications are addedthe same way

T

r

b

=

T

r

b,id

+�

T

r

b

=

2

4T

xb,id

0

T

zb,id

3

5+

2

4�

T

xb

0

0

3

5 (8)

with the difference that the overall trajectory is modified.We choose to define our input to be the upper body accel-eration u

x

= �

T

xb

in order to ensure C1-continuity of thegenerated trajectory. The overall set of first order differential

2The onboard computer runs the RT-OS QNX 6.6 and is equipped withan Intel core i7-4770@3.4 Ghz and 8Gb Ram

equations including a double integrator for ux

can be writtenas

˙

x =

d

dt

2

664

q

˙

q

�

T

xb

�

T

xb

3

775 =

2

664

˙

q

M

�1p

[�

p

+ T

s

� h

p

]

�

T

xb

ux

3

775

= f (

ˆ

x, t,�Lx

, ux

) .

(9)

The state error in (2) will be defined for our implementationas �

ˆ

x =

ˆ

x � [0, 0, 0, 0, �Lx

, 0]T . This model does notonly improve prediction accuracy when compared to theLIPM, it also has two additional advantages:

1) The inclination error which is used as feedback variableis not contained in the commanded trajectories w whichavoids discontinuities in �x

b

(t). This is often a problemin generated trajectories together with real-time modelpredictive control.

2) As the contact state is directly resolved and no ZMP-like planning is performed the method can be appliedfor situations that violate the assumptions of flat footgaits and horizontal ground.

The next section deals with the real-time minimization of (2)submitted to (9) for �L

x

, ux

.

C. Real-Time Solution by Conjugate Gradient Algorithm

The resulting dynamic optimization problem will besolved with the conjugate gradient method which was orig-inally proposed by [29]. The author of [30] extended themethod for optimal control problems with additional freeparameters. The conjugate gradient provides better resultsthan using only the gradient and is at the same time compu-tationally not as expensive as second order algorithms [31]with better stability properties. Additionally it is often usedfor real-time applications as it can be aborted after eachiteration providing a suboptimal but feasible solution (whichis not necessarily the fact for e.g. collocation methods [32]).Finding the optimal solution u(t) = u

x

(t) and p = �Lx

forthe cost function defined in (2) with the model (9) can bestated as mathematical problem as follows

min

p,u(t)J = min

p,u(t)

s(ˆx(t

f

),p) +

tfZ

t0

h(ˆx,p,u, t)dt�

(10)

s.t. ˙

x(t) = f(

ˆ

x,p,u(t), t), ˆ

x(t0) = ˆ

x0 (11)u(t) 2 U, p 2 P (12)

If the final state ˆ

x

f

is free, the application of Pontryagin’sMinimum Principle results in a decoupled boundary valueproblem (see Appendix, (24) and (25)). It is solved with theconjugate gradient method iteratively by integrating the stateˆ

x from t0 to tf

forward and the costate backward in time.Afterward an update of the control input u(t) and parameterp for the (k+1)-th iteration is generated

u

k+1(t) = u

k

(t) + ↵k

s

k

u

(t) (13)

p

k+1= p

k

+ ↵k

s

k

p

. (14)

The conjugate gradient directions s

k

u

(t) and s

k

p

are deter-mined by the Fletcher-Reeves adaptation

s

k

u

(t) = �gk

u

(t) + �k

u

s

k�1u

(t), �k

u

=

�g

k

u

, gk

u

��g

k�1u

, gk�1u

� (15)

s

k

p

= �gk

p

+ �k

p

s

k�1p

, �k

p

=

(g

k

p

)

T

g

k

p

(g

k�1p

)

T

g

k�1p

(16)

with (g

k

u

, gk

u

) =

Rtf

t0g

k

u

(t), gk

u

(t) d t using the gradients

g

k

u

(t) =@H

@u(17)

and (from Appendix relation (27))

g

k

p

=

@s

@p

����t0

+

tfZ

t0

@H

@pdt (18)

for the unconstrained case. They have to be replaced bya projection pointing into the normal cone of U ,P if theboundary of the valid regions is reached. The factor ↵k iscomputed from the one-dimensional line search problem

↵k min↵

J(uk

+ ↵sku

,pk

+ ↵skp

) (19)

which we solve approximately using the backtrackingmethod. The overall procedure is summarized in Algo-rithm 1.

Algorithm 1 Conjugate Gradient Method

1: Determine ˆ

x

a

2: initialize u0x

(t) u⇤x,previous

(t)3: initialize �L0

x

4: k 0

5: repeat6: solve ˆ

x

k

(u

k,pk, t) from (11)7: solve k

(u

k,pk, ˆxk, t) from (25)8: compute s

k

u

, skp

from (15) and (16)9: solve line search (19)

10: update u

k+1,pk+1 with (13) and (14)11: k k + 1

12: until (converged or k > max iterations)13: end

D. System Integration DetailsThe algorithm requires an initial value ˆ

x

a

and an initialsolution for u

0(t), p0. The former is obtained by feeding

the IMU-measurements ('m

, ˙

'

m

) into a state observer. Theobserver is similar to the one presented in [27] but uses thenonlinear model (5). It is based on an extended kalman filterwith a compensation for model errors and external distur-bances. The overall predictive bipedal walking stabilizationis called every 20 ms in the walking control system. For allexperiments the optimization horizon T

opt

= te

� ta

is set tothe time of one step which we choose as 0.8 s for this paper.The initial solution for the control trajectory u0

x

uses the

solution from previous optimization ux,previous

following areceeding horizon approach

u0x

(⌧) = ux,previous

(⌧ � 20ms) (20)

The value �L0x

is computed from the predicted CoM positionerror �x

c

(t1) and velocity error �xc

(t1) at the end ofcurrent step (t1) by the heuristic motivated by the divergentcomponent of motion of the LIPM

�L0x

= k1

⇣�x

c

(t1) +pzc

/g�xc

(t1)⌘. (21)

The overall planned CoM height is described by zc

andk1 2 [0, 1] is a manually tuned factor.At the same time the predicted state in x- and y-directionat time t1 is used in order to compute modification valuesfor the final foot rotation (�c

x

,�cy

) and height (�z) asdescribed in [4]. This helps to compensate for expectedinclination errors.

All mentioned variables can be summarized in the vector� = [�L

x

,�Ly

,�cx

,�cy

,�z] which is used to calculatea set of fifth-order polynomials for the task-space modifica-tions of the legs

�w

legs

(t) = f(t,�) (22)

continuously when new values � are generated. The CoMmodification �w

com

(t) is computed by integrating the ac-celerations u

x

(t), uy

(t) in both directions twice. Using onlythe computed accelerations avoids the problem of generatingdiscontinuous trajectories �w

com

and �

˙

w

com

. A similarstrategy for online generated task-space velocities is usedin [33]. The final modified task-space position for a timeinstance t

k

is computed via

w

mod

(tk

) = w

id

(tk

) +

�w

legs

(tk

)

�w

com

(tk

)

�(23)

and analogous for the task-space velocity ˙

w

mod

(tk

). Theoverall computational time for the whole predictive stabiliza-tion algorithm is below 3.5 ms for one direction. The maxi-mum number of iterations is set to three for all experimentsshown below.

V. RESULTS

A. Simulation with DisturbancesThe proposed method was implemented in the control

system of LOLA and tested in our simulation environment. Itconsiders compliant contacts as well as the motor dynamicsand the joint control loops. The robot is stepping in placewhile it is pushed at the upper body with a force acting inx- and y-direction. The unknown disturbance force is shownin Fig. 4. The plot also shows the resulting inclination errorsof the simulated robot and compares it with the result whenthe predictive stabilizer is turned off. It can be seen that theabsolute inclination of the robot does not diverge with thepredictive stabilizer.

The total CoM modification trajectory and the result fromsingle optimizations are depicted in Fig. 5. At the beginningof the disturbance the method requires several calls in

0

0.1

0.2

4 5 6 7

0

100

200

'[rad]

F[N

]

t [s]

4'm,x

4'm,y

Fext,x

Fext,y

4'm,x

(off)

4'm,y

(off)

Fig. 4: Disturbance forces and inclination errors of thesimulated robot with the proposed stabilization method andfor the method turned off.

order to adapt to the unknown situation. The correspondingtrajectories for the x- and y-positions of the feet can beseen in Fig. 6. The effect of choosing different predictionhorizons T

opt

is underlined in Fig. 7. The same simulation asdescribed above is performed with three different horizons(0.72 s, 0.8 s, 0.96 s) and it can be seen that the stabilizerperforms better with higher prediction horizons.

VI. CONCLUSION AND OUTLOOK

A new predictive method to determine a stabilizing motionadaptation of the robot is presented in this work. It usesthe upper body inclination and inclination rate in order tooptimize CoM and foot trajectories. A previously proposednonlinear model of the biped robot is used in a modelpredictive optimization. The resulting problem is solved witha conjugate gradient algorithm that ensures computationaltimes that allow an application in real-time. Results fromour simulation environment are presented where the methodis integrated into the walking control system of the bipedalrobot LOLA. Next steps include finishing testing the methodon the real robot which performs very close to the simulatedrobot. The planned experiments are walking while the robotreceives external pushes and walking over uneven terrain.

APPENDIX

The conditions from Pontryagin’s Minimum Principle forthe problem (10) to (12) can be derived by stating theaugmented cost function

¯J =s(y(tf

),p) +

tfZ

t0

h(y,p,u, t) + T

(f(y,p,u, t)� ˙

y)dt

+

T

0 (y0 � y(t0))

with the Lagrange multipliers (t) and 0. Using the Hamil-tonian H(y,p,u, t, ) = h(y,p,u, t)+ T

f(y,p,u, t) thefirst variation is

�J =@s

@y

����tf

�y(tf ) +@s

@p

����tf

�p+ � T0 (y0 � y(t0)) + 0 · 0

+

tfZ

t0

@H

@y�z +

@H

@p�p+

@H

@u�u+

@H

@ � � � T y � T �y dt.

0

0.1

0.2

4 5 6 7

4xb

[m

]

t [s]

single

total

Fig. 5: CoM modification trajectories in simulation for singleoptimizations (single) and the overall executed trajectory(total).

0

0.1

0.2

0.3

4 5 6 7

4xi

[m

]

t [s]

0

0.1

0.2

4 5 6 7

4y i

[m

]

Right leg Left leg CoM

Fig. 6: Modification trajectories for the legs and the CoM inx- and y-direction.

Integrating by partsR

T � ˙y and rearranging leads to

�J =

@s

@y

����tf

� (tf )T!�y(tf ) + � T

0 (y0 � y(t0))

+

tfZ

t0

✓@H

@y+

T◆�y +

✓@H

@ � yT

◆� +

@H

@u�u dt

+@s

@p

����tf

�p+

tfZ

t0

@H

@p�p dt

The conditions for the optimal solution are finally

@s

@y

����tf

� (tf

)

T

= 0 y0 � y(t0) = 0 (24)

@H

@y+

˙

T

= 0

@H

@ � ˙

y

T

= 0 (25)

@H

@u�u = 0 (26)

0

0.1

0.2

4 5 6 7

4'x

[rad]

t [s]

Topt

= 0.72 s

Topt

= 0.80 s

Topt

= 0.96 s

Fig. 7: Resulting upper body inclination error of the bipedfor three different time horizons in the optimization.

0

@ @s

@p

����tf

+

tfZ

t0

@H

@pdt

1

A �p = 0. (27)

These are the same conditions as for an optimal controlproblem with free final state and fixed final time with theadditional condition (27) for the free parameters p.

ACKNOWLEDGMENT

This work is supported by the Deutsche Forschungsge-meinschaft (project BU 2736/1-1).

REFERENCES

[1] R. Wittmann, A.-C. Hildebrandt, A. Ewald, and T. Buschmann, “AnEstimation Model for Footstep Modifications of Biped Robots,” inIEEE/RSJ International Conference on Intelligent Robots and Systems.IEEE, sep 2014, pp. 2572–2578.

[2] A.-C. Hildebrandt, D. Wahrmann, R. Wittmann, D. Rixen, andT. Buschmann, “Real-Time Pattern Generation Among Obstacles forBiped Robots,” in IEEE/RSJ International Conference on IntelligentRobots and Systems. IEEE, 2015, pp. 2780–2786.

[3] D. Wahrmann, A.-C. Hildebrandt, R. Wittmann, D. Rixen, andT. Buschmann, “Fast Object Approximation for Real-Time 3D Ob-stacle Avoidance with Biped Robots,” IEEE International Conferenceon Advanced Intelligent Mechatronics., 2016.

[4] R. Wittmann, A.-C. Hildebrandt, D. Wahrmann, D. Rixen, andT. Buschmann, “Real-Time Nonlinear Model Predictive Footstep Op-timization for Biped Robots,” in IEEE-RAS International Conferenceon Humanoid Robots, 2015, pp. 711–717.

[5] K. Nishiwaki and S. Kagami, “Online Walking Control System forHumanoids with Short Cycle Pattern Generation,” The InternationalJournal of Robotics Research, vol. 28, no. 6, pp. 729–742, may 2009.

[6] ——, “Frequent walking pattern generation that uses estimated ac-tual posture for robust walking control,” in IEEE/RAS InternationalConference on Humanoid Robots. IEEE, dec 2009, pp. 535–541.

[7] K. Hirai, M. Hirose, Y. Haikawa, and T. Takenaka, “The developmentof Honda humanoid robot,” in IEEE International Conference onRobotics and Automation, vol. 2. IEEE, 1998, pp. 1321–1326.

[8] T. Takenaka, T. Matsumoto, T. Yoshiike, T. Hasegawa, S. Shirokura,H. Kaneko, and A. Orita, “Real time motion generation and controlfor biped robot -4th report: Integrated balance control,” in IEEE/RSJInternational Conference on Intelligent Robots and Systems. IEEE,oct 2009, pp. 1601–1608.

[9] K. Loffler, M. Gienger, and F. Pfeiffer, “Model based control of a bipedrobot,” in 7th International Workshop on Advanced Motion Control.IEEE, 2002, pp. 443–448.

[10] T. Buschmann, “Simulation and Control of Biped Walking Robots,”Ph.D. dissertation, 2010.

[11] S. Kuindersma, F. Permenter, and R. Tedrake, “An efficiently solvablequadratic program for stabilizing dynamic locomotion,” in IEEEInternational Conference on Robotics and Automation. IEEE, may2014, pp. 2589–2594.

[12] A. Sherikov, D. Dimitrov, and P.-b. Wieber, “Whole body motion con-troller with long-term balance constraints,” in IEEE/RAS InternationalConference on Humanoid Robots. IEEE, 2014, pp. 444–450.

[13] J. Pratt, J. Carff, S. Drakunov, and A. Goswami, “Capture Point: AStep toward Humanoid Push Recovery,” in IEEE/RAS InternationalConference on Humanoid Robots. IEEE, dec 2006, pp. 200–207.

[14] J. Rebula, F. Canas, J. Pratt, and A. Goswami, “Learning CapturePoints for humanoid push recovery,” in IEEE/RAS International Con-ference on Humanoid Robots. IEEE, nov 2007, pp. 65–72.

[15] J. Hodgins and M. Raibert, “Adjusting step length for rough terrainlocomotion,” IEEE Transactions on Robotics and Automation, vol. 7,no. 3, pp. 289–298, jun 1991.

[16] 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.

[17] J. Urata, K. Nishiwaki, Y. Nakanishi, K. Okada, S. Kagami, andM. Inaba, “Online decision of foot placement using singular LQ pre-view regulation,” in IEEE-RAS International Conference on HumanoidRobots. IEEE, oct 2011, pp. 13–18.

[18] R. Tajima, D. Honda, and K. Suga, “Fast running experiments involv-ing a humanoid robot,” in IEEE International Conference on Roboticsand Automation. IEEE, may 2009, pp. 1571–1576.

[19] P.-B. Wieber, “Trajectory Free Linear Model Predictive Control forStable Walking in the Presence of Strong Perturbations,” in IEEE-RASInternational Conference on Humanoid Robots. IEEE, dec 2006, pp.137–142.

[20] Y. Fujimoto, S. Obata, and A. Kawamura, “Robust biped walkingwith active interaction control between foot and ground,” in IEEEInternational Conference on Robotics and Automation, vol. 3, no. May.IEEE, 1998, pp. 2030–2035.

[21] T. Buschmann, S. Lohmeier, M. Bachmayer, H. Ulbrich, and F. Pfeif-fer, “A collocation method for real-time walking pattern generation,”in IEEE/RAS International Conference on Humanoid Robots. IEEE,nov 2007, pp. 1–6.

[22] S. Kajita, O. Matsumoto, and M. Saigo, “Real-time 3D walkingpattern generation for a biped robot with telescopic legs,” in IEEEInternational Conference on Robotics and Automation, vol. 3. IEEE,2001, pp. 2299–2306.

[23] T. Buschmann, S. Lohmeier, and H. Ulbrich, “Biped walking controlbased on hybrid position/force control,” in IEEE/RSJ InternationalConference on Intelligent Robots and Systems. IEEE, oct 2009, pp.3019–3024.

[24] S. Lohmeier, “System design and control of anthropomorphic walkingrobot LOLA,” IEEE/ASME Transactions on Mechatronics, vol. 14,no. 6, pp. 658–666, 2009.

[25] A. Liegeois, “Automatic Supervisory Control of the Configuration andBehavior of Multibody Mechanisms,” IEEE Transactions on Systems,Man, and Cybernetics, vol. 7, no. 12, pp. 868–871, 1977.

[26] T. Buschmann, V. Favot, S. Lohmeier, M. Schwienbacher, and H. Ul-brich, “Experiments in fast biped walking,” in IEEE InternationalConference on Mechatronics. IEEE, apr 2011, pp. 863–868.

[27] R. Wittmann, A.-C. Hildebrandt, D. Wahrmann, T. Buschmann, andD. Rixen, “State Estimation for Biped Robots Using Multibody Dy-namics,” in IEEE/RSJ International Conference on Intelligent Robotsand Systems. IEEE, 2015, pp. 2166–2172.

[28] K. Nishiwaki and S. Kagami, “Walking control on uneven terrain withshort cycle pattern generation,” in IEEE/RAS International Conferenceon Humanoid Robots. IEEE, nov 2007, pp. 447–453.

[29] L. Lasdon, S. Mitter, and A. Waren, “The conjugate gradient methodfor optimal control problems,” IEEE Transactions on Automatic Con-trol, vol. 12, no. 2, pp. 132–138, apr 1967.

[30] M. Bocek, “Conjugate gradient algorithm for optimal control problemswith parameters,” Kybernetika, vol. 16, no. 5, pp. 454–461, 1980.

[31] Y. Tassa, T. Erez, and E. Todorov, “Synthesis and stabilization of com-plex behaviors through online trajectory optimization,” 2012 IEEE/RSJInternational Conference on Intelligent Robots and Systems, pp. 4906–4913, oct 2012.

[32] J. Nocedal and S. J. Wright, “Numerical Optimization,” pp. 1–651,2004.

[33] K. Nishiwaki, J. Chestnutt, and S. Kagami, “Autonomous Navigationof a Humanoid Robot over Unknown Rough Terrain using a LaserRange Sensor,” The International Journal of Robotics Research, 2012.