design of a compliant bipedal walking controller for the...

7
Design of a Compliant Bipedal Walking Controller for the DARPA Robotics Challenge Michael A. Hopkins, Robert J. Griffin, Alexander Leonessa * , Brian Y. Lattimer and Tomonari Furukawa Abstract— This paper provides an overview of the bipedal walking controller implemented on ESCHER, a new torque- controlled humanoid designed by Virginia Tech to compete in the DARPA Robotics Challenge (DRC). The robot’s compliant control approach relies on an optimization-based inverse dy- namics solver proposed in a previous publication. This work presents two unique features to improve stability on soft and uncertain terrain, developed in preparation for the dirt track and stairs task at the DRC Finals. First, a step adjustment algorithm is introduced to modify the swing foot position based on the divergent component of motion (DCM) error. Second, a simple heuristic is introduced to improve stability on compliant surfaces such as dirt and grass by modifying the design of the center of pressure (CoP) trajectory. The proposed approach is validated through DRC-related experiments demonstrating the robot’s ability to climb stairs and traverse soft terrain. I. I NTRODUCTION As bipeds, humanoid robots have the potential to achieve excellent mobility in natural and man-made environments, ideally matching the robustness and versatility of humans. The DARPA Robotic Challenge (DRC) helped bring a new generation of bipedal walking controllers to real hardware systems. Nevertheless, the high risk of failure at the competi- tion indicates that a number of challenges still remain before bipeds can reliably negotiate difficult real-world scenarios. In [1], the authors presented an optimization-based whole- body controller enabling the THOR humanoid to walk on flat terrain using series elastic actuation. In preparation for the DRC, this approach was ported to ESCHER, a new 1.8 m tall humanoid developed at Virginia Tech (see Fig. 1). This paper provides an overview of the final bipedal walking controller used at competition, including a discussion of design considerations related to the dirt track and stairs task. The presented controls approach is an iteration of a previ- ously published design which relies on divergent component of motion (DCM) tracking to stabilize the centroidal dynam- ics [1–3]. Two unique features are introduced to improve stability on soft and uncertain terrain. First, a new step adjustment algorithm is proposed to modify the swing foot trajectory during the single support phase using a simple heuristic based on the instantaneous DCM tracking error. Second, a simple, yet surprisingly effective modification to the nominal center of pressure (CoP) trajectory is introduced to reduce tracking errors on soft terrain. The proposed approach is verified through a variety of walking and push re- covery experiments conducted using the ESCHER humanoid. The authors are with the Terrestrial Robotics, Engineering & Controls Laboratory (TREC) at Virginia Tech. (e-mail: robert.griffi[email protected]) * This material is based upon work supported by (while serving at) the National Science Foundation. Fig. 1. Left: THOR humanoid walking on the Ex-USS Shadwell as part of the ONR Shipboard Autonomous Firefighting Robot (SAFFiR) program (photo courtesy of Virginia Tech / Logan Wallace). Right: Team VALOR’s ESCHER humanoid walking on the dirt track at the DARPA Robotics Challenge Finals (photo courtesy of DARPA). II. RELATED WORK A number of top-performing DRC teams developed com- pliant whole-body control strategies to enable the Atlas humanoid, supplied by Boston Dynamics, to traverse the final course [4–6]. As in [7–10], these approaches compute admis- sible joint torque and acceleration setpoints for whole-body control by solving an efficient convex optimization based on the rigid body dynamics. Feng et al. developed a hybrid inverse dynamics and inverse kinematics solver to enable the Atlas robot to walk on uneven terrain [4]. Kuindersma et al. stabilized the walking gait by minimizing an LQR cost function based on the zero moment point (ZMP) [5]. Johnson et al. and Koolen et al. tracked an instantaneous capture point (ICP) trajectory using an approach similar to the DCM-based controller presented in this paper [6, 11]. While several authors have proposed methods to improve walking on soft terrain including grass and sand [12, 13], experimental results are limited. Online adjustment of the target foothold position is a well-known approach to increase stability in response to external disturbances and unmodeled terrain features. A number of publications have presented reactive stepping strategies based on the linear inverted pendulum dynamics [14–16]. Englsberger et al. proposed a method to determine the upcoming foothold position given the desired DCM position at the end of the next step [17]. Unlike the method presented here, the authors computed the step adjustment based on the extrapolated DCM position at the time of heel-strike. 2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids) November 3 - 5, 2015, Seoul, Korea 978-1-4799-6885-5/15/$31.00 ©2015 IEEE 831

Upload: others

Post on 26-Mar-2020

2 views

Category:

Documents


0 download

TRANSCRIPT

Page 1: Design of a Compliant Bipedal Walking Controller for the ...web.tuat.ac.jp/~gvlab/ronbun/ReadingGroupHCR/Design... · Design of a Compliant Bipedal Walking Controller for the DARPA

Design of a Compliant Bipedal Walking Controller forthe DARPA Robotics Challenge

Michael A. Hopkins, Robert J. Griffin, Alexander Leonessa*, Brian Y. Lattimer and Tomonari Furukawa

Abstract— This paper provides an overview of the bipedalwalking controller implemented on ESCHER, a new torque-controlled humanoid designed by Virginia Tech to compete inthe DARPA Robotics Challenge (DRC). The robot’s compliantcontrol approach relies on an optimization-based inverse dy-namics solver proposed in a previous publication. This workpresents two unique features to improve stability on soft anduncertain terrain, developed in preparation for the dirt trackand stairs task at the DRC Finals. First, a step adjustmentalgorithm is introduced to modify the swing foot position basedon the divergent component of motion (DCM) error. Second, asimple heuristic is introduced to improve stability on compliantsurfaces such as dirt and grass by modifying the design of thecenter of pressure (CoP) trajectory. The proposed approach isvalidated through DRC-related experiments demonstrating therobot’s ability to climb stairs and traverse soft terrain.

I. INTRODUCTION

As bipeds, humanoid robots have the potential to achieveexcellent mobility in natural and man-made environments,ideally matching the robustness and versatility of humans.The DARPA Robotic Challenge (DRC) helped bring a newgeneration of bipedal walking controllers to real hardwaresystems. Nevertheless, the high risk of failure at the competi-tion indicates that a number of challenges still remain beforebipeds can reliably negotiate difficult real-world scenarios.

In [1], the authors presented an optimization-based whole-body controller enabling the THOR humanoid to walk onflat terrain using series elastic actuation. In preparation forthe DRC, this approach was ported to ESCHER, a new 1.8m tall humanoid developed at Virginia Tech (see Fig. 1).This paper provides an overview of the final bipedal walkingcontroller used at competition, including a discussion ofdesign considerations related to the dirt track and stairs task.

The presented controls approach is an iteration of a previ-ously published design which relies on divergent componentof motion (DCM) tracking to stabilize the centroidal dynam-ics [1–3]. Two unique features are introduced to improvestability on soft and uncertain terrain. First, a new stepadjustment algorithm is proposed to modify the swing foottrajectory during the single support phase using a simpleheuristic based on the instantaneous DCM tracking error.Second, a simple, yet surprisingly effective modification tothe nominal center of pressure (CoP) trajectory is introducedto reduce tracking errors on soft terrain. The proposedapproach is verified through a variety of walking and push re-covery experiments conducted using the ESCHER humanoid.

The authors are with the Terrestrial Robotics, Engineering & ControlsLaboratory (TREC) at Virginia Tech. (e-mail: [email protected])

*This material is based upon work supported by (while serving at) theNational Science Foundation.

Fig. 1. Left: THOR humanoid walking on the Ex-USS Shadwell as partof the ONR Shipboard Autonomous Firefighting Robot (SAFFiR) program(photo courtesy of Virginia Tech / Logan Wallace). Right: Team VALOR’sESCHER humanoid walking on the dirt track at the DARPA RoboticsChallenge Finals (photo courtesy of DARPA).

II. RELATED WORK

A number of top-performing DRC teams developed com-pliant whole-body control strategies to enable the Atlashumanoid, supplied by Boston Dynamics, to traverse the finalcourse [4–6]. As in [7–10], these approaches compute admis-sible joint torque and acceleration setpoints for whole-bodycontrol by solving an efficient convex optimization basedon the rigid body dynamics. Feng et al. developed a hybridinverse dynamics and inverse kinematics solver to enablethe Atlas robot to walk on uneven terrain [4]. Kuindersma etal. stabilized the walking gait by minimizing an LQR costfunction based on the zero moment point (ZMP) [5]. Johnsonet al. and Koolen et al. tracked an instantaneous capture point(ICP) trajectory using an approach similar to the DCM-basedcontroller presented in this paper [6, 11].

While several authors have proposed methods to improvewalking on soft terrain including grass and sand [12, 13],experimental results are limited. Online adjustment of thetarget foothold position is a well-known approach to increasestability in response to external disturbances and unmodeledterrain features. A number of publications have presentedreactive stepping strategies based on the linear invertedpendulum dynamics [14–16]. Englsberger et al. proposed amethod to determine the upcoming foothold position giventhe desired DCM position at the end of the next step [17].Unlike the method presented here, the authors computed thestep adjustment based on the extrapolated DCM position atthe time of heel-strike.

2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids) November 3 - 5, 2015, Seoul, Korea

978-1-4799-6885-5/15/$31.00 ©2015 IEEE 831

Page 2: Design of a Compliant Bipedal Walking Controller for the ...web.tuat.ac.jp/~gvlab/ronbun/ReadingGroupHCR/Design... · Design of a Compliant Bipedal Walking Controller for the DARPA

III. DESIGN AND MODELING

A. Hardware DesignIn preparation for the 2013 DRC Trials, Virginia Tech

developed the THOR humanoid, a 63 kg prototype platformfeaturing a compliant lower body with human-like range ofmotion [18]. The increased autonomy and run-time require-ments of the DRC Finals spurred a design iteration leading tothe development of ESCHER, the Electric Series CompliantHumanoid for Emergency Response. At 77 kg, the reviseddesign features a new upper body with 11 DoF manipulatorsfrom HDT Global and onboard power and computing forextended operation. The hip pitch and knee assemblies werealso upgraded to achieve the necessary output torque forlocomotion on uneven terrain.

Like its predecessor, ESCHER features custom linearseries elastic actuators, enabling compliant control of all 12degrees of freedom (DoF) in the lower body [19]. A titaniumleaf spring is mounted in series with the actuator load to in-crease shock tolerance and decrease mechanical impedance.In-line load cell sensors provide direct force measurementsto enable high-performance torque and impedance control ofthe hip, knee, and ankle joints.

B. Dynamic ModelsAs in [4–6], the presented controls approach employs a

rigid body model to approximate the whole-body dynamicsof an articulated humanoid. Assuming each joint behaves likean ideal torque source, the floating-base rigid body equationsof motion are given by[

]= Hq + C−

∑JTc f c, (1)

where τ ∈ Rn is the vector of joint torques, q ∈ Rn+6

is the vector of generalized coordinates, and f c ∈ R3 isthe vector of external forces acting at each contact point,c = 1 . . . N , with corresponding Jacobian, Jc. Here Hand C represent the joint-space inertia matrix and vectorof centrifugal, Coriolis, and gravity torques, respectively.As in [9], approximate no-slip conditions are expressed byconstraining the acceleration of each contact point using theequality, Jcq + Jcq = 0.

The controls approach presented in Section IV makesextensive use of the divergent component of motion (DCM)to stabilize the centroidal dynamics of the rigid body systemduring walking. The DCM represents a linear transformationof the CoM state that partitions the second-order linearinverted pendulum dynamics into coupled first-order sys-tems [17, 20]. Appropriate planning of the DCM can beused to ensure that the CoM is stabilizable following oneor more steps. Assuming a constant CoM height, the closelyrelated ICP defines the two-dimensional point at which thecentroidal moment pivot (CMP) must be placed for the CoMto come to a complete rest [14], whereas the DCM definesthe three-dimensional point at which the CoM converges.

Using the time-varying formulation presented in [2], theDCM is defined as

ξ = x +1

ωx, (2)

Fig. 2. Left: DCM dynamics and external forces acting on an articulatedhumanoid. Right: Lateral step adjustment based on the estimated DCM error.Here r∗foot,r and rfoot,r are the nominal and adjusted swing foot positions.

where x ∈ R3 is the CoM position and ω(t) > 0 is the time-varying natural frequency of the transformed CoM dynamics,x = ω (ξ − x). The coupled first-order DCM dynamics aregiven by

ξ =

(ω − ω

ω

)(ξ − rvrp) , (3)

where rvrp ∈ R3 represents the virtual repellent point (VRP)[2, 17]. Given (ω − ω/ω) > 0, the VRP repels the DCM ata rate proportional to its distance; by definition, this three-dimensional point maps the position of the CoM to the totallinear momentum rate of change acting on the system,

l = m(ω2 − ω)(x− rvrp). (4)

Note that the linear momentum rate of change is related tothe net contact force via Newton’s second law, i.e. l = mx =∑

f c + fg , where fg = [ 0, 0, −mg ]T is the force of gravity

and m is the total mass of the robot.The various reference points related to the DCM dynamics

are illustrated in Fig. 2, including the enhanced centroidalmoment pivot (eCMP) which maps the CoM position to thenet contact force,

∑f c = m(ω2 − ω)(x − recmp) [2, 17].

The eCMP is typically collocated with the center of pressure(CoP) in the robot’s base of support to ensure that theline of action passes through the CoM. This results in anet horizontal angular momentum rate of change of zero.Note that the VRP lies directly above the eCMP with avertical offset of ∆zvrp = g/(ω2 − ω), determined by thegravitational constant and natural frequency [2]. Thus, ifω = 0, the natural frequency is given by ω =

√g

∆zvrp.

IV. CONTROLS APPROACH

Figure 3 includes a high-level block diagram of the bipedalwalking controller developed for the ESCHER humanoid.The presented controls approach is an extension of the com-pliant locomotion framework described in [1]. This sectionprovides a summary of the overall design including a newstep adjustment algorithm based on the DCM dynamics.

832

Page 3: Design of a Compliant Bipedal Walking Controller for the ...web.tuat.ac.jp/~gvlab/ronbun/ReadingGroupHCR/Design... · Design of a Compliant Bipedal Walking Controller for the DARPA

Fig. 3. High-level block diagram of ESCHER’s stepping controller. Note that some control paths have been omitted to improve readability.

A. State Machine

Single-step and multi-step behaviors are implemented us-ing the finite state machine depicted on the far left of Fig. 3which transitions between various contact phases includingdouble support, toe-off, single support, and heel-strike. Indouble support, both feet are assumed to be in contact withthe surface. If an ankle or knee pitch limit is encountered bythe predefined swing leg prior to single support, the robottransitions to a reactive toe-off state to compensate for theleg range of motion limits. Once the heel is unloaded, theswing foot rotates about the toe contact points, shifting theankle pitch away from the soft limit. During single support,the swing foot travels to a new foothold position. After apredetermined duration, the state machine transitions to heel-strike, at which point the swing foot is lowered at a constantvelocity until it contacts the ground and settles into a newfoothold pose. When the foot is sufficiently loaded, the statemachine transitions back to double support.

B. Task-Space Planning

The step controller obtains desired foothold poses andstep durations from a footstep queue that is continuouslypopulated by a high-level planner. As illustrated in Fig. 3,several mid-level planners are defined to compute task-spacereference trajectories for the swing foot, pelvis, and DCM atthe beginning of each double support phase.

The DCM reference trajectory is generated using the real-time numeric planner described in [2]. First, a nominal CoPand eCMP trajectory is computed from the desired footholdposes to ensure no-tip conditions during each support phase.Second, the time-varying natural frequency, ω(t), is derivedfrom the desired vertical CoM and eCMP trajectories. Third,an initial DCM reference trajectory is generated via reverse-time integration of the DCM dynamics (3). Finally, theplanner solves a time-varying LQR optimization to refine thetrajectory over a short preview window, thereby eliminatingany initial discontinuities due to replanning. Although morecomputationally intensive than the analytical DCM plannersproposed by Englsberger et al. [17, 21], the proposed ap-proach permits generic CoP and vertical CoM trajectories.

The pelvis rotation and swing foot pose, Rpelvis,r(t),Rfoot,r(t), and r∗foot,r(t), are generated using piecewiseminimum jerk trajectories that interpolate between inter-mediate waypoints determined from the initial and finalfoothold poses. The upper body joint trajectories, qr(t), aredefined by an internal planner that manages arm swingingduring stepping. During manipulation, these trajectories aredetermined by an external planner that manages end-effectorpositioning and collision avoidance for tasks such as graspingand carrying an object.

C. Task-Space Control

Task-space position and velocity errors are regulated usinga set of feedback controllers that command desired task-space forces and accelerations. Table I lists the proportionaland derivative gains used to compute desired momentumrates of change, pelvis and swing foot accelerations, andupper body joint accelerations for the experiments in SectionVI. To maintain rotation invariance, x and y-axis gainsare represented in pelvis yaw coordinates. The computedmotion tasks are tracked using the quadratic program (QP)formulation described in Section IV-E.

The desired linear momentum rate of change is derivedusing a DCM tracking controller designed to stabilize thecentroidal dynamics. This VRP-based controller uses PIfeedback to regulate the estimated DCM error as describedin [2]. The VRP setpoint is mapped to a desired linearmomentum rate of change, ld, using (4). Proportional-integralgains of 3 m/m and 1 m/m · s were selected for theexperiments in Section VI. To prevent windup, the integralaction is disabled during single support and heel-strike.

The desired angular momentum rate of change is givenby kd = 0. This reflects the assumption that the eCMPand CoP are collocated during walking, in which case, thehorizontal moment about the CoM is equal to zero. Notethat the robot’s angular momentum is not directly regulatedusing feedback control. The proposed approach relies onCartesian and joint-space PID controllers to track the pelvis,swing foot, and upper body trajectories and prevent excessiveangular momentum during walking.

833

Page 4: Design of a Compliant Bipedal Walking Controller for the ...web.tuat.ac.jp/~gvlab/ronbun/ReadingGroupHCR/Design... · Design of a Compliant Bipedal Walking Controller for the DARPA

D. Step Adjustment

In the event of a disturbance, the momentum controllerwill attempt to stabilize the DCM by shifting the eCMP andCoP away from the nominal reference position. This caninduce a significant moment about the CoM whenever theeCMP leaves the base of support. Due to the robot’s limitedrange of motion, it is not always possible to sustain thecorresponding angular momentum rates of change requiredto prevent a fall, especially during the single support phase.In such scenarios, a fall can often be avoided by adjustingthe target foothold position to modify the base of support.

The proposed controller employs a simple heuristic to im-plement real-time step adjustment in response to significantdisturbances. As illustrated in Fig. 3, a feedforward pathis included to adjust the nominal swing foot trajectory inresponse to the DCM tracking error. The reference positionis given by

rfoot,r = projection(r∗foot,r + deadband (ξ − ξr)

), (5)

where r∗foot,r is the nominal swing foot position, ξ is theestimated DCM position, and ξr is the reference DCMposition. The deadband function implements a neutral zonein the DCM error signal to maintain precise foothold trackingin the event of small disturbances. The vertical DCM error iseliminated to avoid early or late heel-strike events. The pro-jection function projects the horizontal swing foot positiononto a circular sector which excludes the base of support.This region is computed as a function of the current stancefoot pose to prevent self-collision and range of motion issues.

Figure 2 illustrates an ideal lateral step adjustment inresponse to a large DCM error. Note that the offset betweenthe final DCM and swing foot position is equivalent to theoffset between the reference DCM and swing foot position(assuming zero deadband). Intuitively, the step adjustmentalgorithm shifts the swing foot in the direction of the esti-mated DCM position to ensure that the DCM will lie insidethe base of support at the time of heel-strike. The futurefoothold positions are specified relative to the current supportfoot position. As a result, horizontal tracking errors canaccumulate with each footstep. This is an inherent limitationof the proposed approach. The responsibility to correct forthe corresponding drift is tasked to the high-level footstepplanner. In scenarios requiring precise foothold tracking, thestep adjustment can be minimized by increasing the DCMerror deadband or increasing the DCM controller stiffness.

E. Inverse Dynamics (QP Formulation)

Given desired task-space forces and accelerations at eachtime step, an inverse dynamics module is used to computeoptimal joint accelerations, q, and generalized contact forces,ρ =

[ρT

1 . . . ρTN

]T, by minimizing a quadratic cost

function in the form,

minq,ρ

∥∥∥Cb

(b− Jq− Jq

)∥∥∥2

+ λq‖q‖2 + λρ‖ρ‖2. (6)

Here b represents the vector of desired motion tasks, Jrepresents the corresponding matrix of task-space Jacobians,

TABLE IWHOLE-BODY CONTROLLER WEIGHTS AND GAINS1

Motion Task Units Weight P-Gain D-Gainld N 5, 5, 12 - -kd Nm 5, 5, 0 0 0ωpelvis,d rad/s2 100, 100, 100 70, 70, 30 30, 30, 15ωfoot,d rad/s2 500, 500, 500 100, 100, 75 5, 5, 5rfoot,d m/s2 1e3, 1e3, 1e3 50, 50, 100 35, 35, 35rcontact,d m/s2 1e5, 1e5, 1e5 0, 0, 0 0, 0, 0qarm,d rad/s2 15 45 10qwaist,d rad/s2 100 40 20

Qb = CTbCb represents the task weighting matrix, and λq

and λρ define the regularization parameters. The QP includeslinear Newton-Euler and Coulomb friction constraints inaddition to joint position and torque limits to ensure dynamicfeasibility of the optimized setpoints [1]. Table I lists theexperimentally selected weights for each motion task.

F. Low-Level Control

Joint torque setpoints are derived from the optimized jointaccelerations and contact forces via the rigid body equationsof motion (1). As illustrated in Fig. 3, joint velocity setpointsare obtained from the optimized joint accelerations using aleaky integrator described in [1]. The computed torque andvelocity setpoints are relayed to embedded motor controllersat a rate of 150 Hz. The update rate is currently limitedby the robot’s state estimator, which computes velocity esti-mates using a decoupled Kalman filter design. Upper bodytrajectories are tracked using a high-gain velocity controllerfor each DoF, while lower body trajectories are tracked usinga low-gain impedance controller updated at 2 kHz [3].

V. DESIGN CONSIDERATIONS FOR THE DRCThis section presents implementation details and lessons

learned in adapting ESCHER’s walking controller to two ofthe challenges presented by the DRC Finals: the dirt trackand the stairs task.

A. Dirt Track

As ESCHER was unable to attempt the driving task atthe DRC, the robot was required to traverse the 200 ftdirt track on foot. Because the QP formulation presentedin Section IV-E assumes a rigid contact model, soft terrainsuch as dirt and grass can introduce significant unmodeleddynamics during walking. Although low-impedance controlof the lower body provides a certain degree of robustness tosurface compliance, poor DCM tracking can lead to tippingas the CoP deviates to the edge of the support polygon. Inparticular, lateral tipping about the outside edge of the footcan occur when the momentum controller fails to preventovershoot of the DCM trajectory near the end of the doublesupport phase. This failure mode is particularly dangerous, asrange of motion and self-collision risks often prevent properstep adjustment during single support.

1Cartesian weights and gains are specified for the x, y, and z axes.2These weights are decreased to (2.5, 5, 1) during the single support

phase to reduce oscillations in the sagittal plane.

834

Page 5: Design of a Compliant Bipedal Walking Controller for the ...web.tuat.ac.jp/~gvlab/ronbun/ReadingGroupHCR/Design... · Design of a Compliant Bipedal Walking Controller for the DARPA

Fig. 4. The nominal CoP trajectory is shifted towards the inside of thesupport foot to improve stability on soft terrain and reduce lateral motionof the CoM during walking.

During the course of development, the authors found thata simple modification to the CoP reference trajectory couldsignificantly improve performance on soft and uncertainterrain. In the bipedal walking literature, the CoP trajectoryis often defined to pass through the center of each supportfoot. A notable exception is [22], where the authors shift theCoP towards the big toe during single support to reduce theamount of hip sway. A similar approach is proposed hereto improve stability on compliant terrain. As illustrated inFig. 4, a lateral offset is introduced to shift the CoP towardthe inside of the support foot. This decreases the lateralmotion of the CoM similar to narrowing the step width, yetwithout the additional risk of self-collision. By increasingthe nominal distance of the CoP to the outer edge of thefoot, additional horizontal torque is available to correct forDCM overshoot. This decreases the risk of outward tipping.In the event of inward tipping, the controller can still regainstability through an appropriate step adjustment.

B. Stairs Task

The DRC course featured an industrial stairway and roughterrain area designed to test the robot’s mobility in structuredand unstructured environments. The team chose to focusdevelopment efforts on the stairs task rather than the roughterrain, due to initial range of motion issues encounteredwhile stepping down on the cinder blocks. Although the pro-posed controls approach was designed to support locomotionon uneven terrain, a number of difficulties arose during thecourse of testing due to the risk of collision between thesupport leg and upper stair when stepping up.

In order to increase the available workspace, a half-stepstrategy was adopted, permitting the robot to place the centerof the support foot on the lip of the stair. As illustrated in Fig.5, the rear contact points were shifted to the middle of thesole in order to appropriately constrain the CoP trajectory.To prevent knee and shin collisions, a soft position limitwas enforced on the ankle pitch joint corresponding to themeasured angle of collision. Using this approach, the inversedynamics optimization was able to appropriately adjust theankle pitch to avoid collision during stepping.

By reducing the available ankle range of motion, carefuldesign of the nominal CoM height trajectory was requiredto enable the robot to safely step onto the stair. Fig. 5includes the vertical CoM and DCM reference trajectoriescorresponding to a 23 cm (9 in) step. Note that the CoMbegins to accelerate at the beginning of the double supportphase. Raising the CoM height early in the step cycle tends

Fig. 5. Left: A half-step strategy is implemented to increase the availablecollision-free workspace of the support leg when ascending stairs. Right:During ascension, vertical CoM acceleration initiates at the beginning of thedouble support phase (shaded in gray), reducing the required knee torque.

to rotate the support ankle away from the soft position limit.This has the added benefit of straightening the support knee,resulting in lower knee torques. As the CoM height increases,the length of the swing leg also increases. As a result,significant toe-off rotation is required to allow the edge ofthe foot to remain in contact with the stair prior to lift-off.

VI. EXPERIMENTAL RESULTS

This section presents experimental results for the ESCHERhumanoid demonstrating the walking controller’s ability tonegotiate each of the challenges discussed in the previoussection. Identical control parameters are used in each exper-iment (refer to Table I), with the exception of the stairs taskwhich required stiffer tracking of the z-axis DCM trajectory.

A. Soft and Uncertain Terrain

Images of ESCHER walking on a various indoor andoutdoor terrain including grass, gravel, dirt, and unmodeledblocks can be seen in Fig. 6. Compliant control of the lowerbody enables accurate tracking of the desired ground reactionforces, allowing the robot to adapt naturally to uncertainterrain features. Figure 7 includes the desired and estimatedhorizontal CoP, VRP, and DCM trajectories corresponding totwo separate walking experiments conducted on 3.5 cm pileheight artificial turf (refer to Fig. 6-a).

In the first experiment, the desired CoP waypoints areshifted by 2 cm towards in the inside of the foot as describedin Section V-A. In the second experiment, the CoP waypointsare defined at the center of the support foot. In each case, thedesired footstep plan consists of eight forward steps with astride length of 0.15 m, step duration of 2 s, double support

Fig. 6. ESCHER walking on various terrain including grass, gravel, and3.8 cm blocks. In each case, the controller assumes a rigid contact surface,and has no knowledge of height variations or surface compliance.

835

Page 6: Design of a Compliant Bipedal Walking Controller for the ...web.tuat.ac.jp/~gvlab/ronbun/ReadingGroupHCR/Design... · Design of a Compliant Bipedal Walking Controller for the DARPA

Fig. 7. Desired and estimated DCM, VRP, and CoP trajectories whilewalking on artificial turf with a 2 s step duration. In the top plot, the CoPtrajectory is shifted inward by 2 cm to improve tracking performance. Inthe bottom plot, the CoP passes through the center of the foot.

time of 0.6 s and single support time of 1.4 s. The resultsdemonstrate that the lateral CoP offset can significantlyreduce oscillations in the VRP, DCM, and CoP trajectoriesresulting from unmodeled surface compliance. Note that,without the offset, the estimated CoP approaches the edgeof the foot on multiple occasions, indicating that the robotis approaching a tipping point.

B. Uneven Terrain

Figure 8 includes images of the robot stepping up a flightof industrial stairs of the same dimensions as those at theDRC Finals course (9in x 11in). Image 2a, 2b, 4a and 4bwere captured during toe-off, as the robot pitched the rightankle to extend the effective length of the swing leg. In thisexperiment, the heel of the left foot was positioned 10 cmfrom the lip of the stair. With 60% of the sole in contact,this configuration provided a slight safety margin to preventthe foot from rocking given the limited base of support.

The support bar on the knee clears the upper stair byapproximately 2 cm using the methods described in SectionV-B. A minimum position limit of –0.6 radians was selectedfor the ankle pitch joint in order to prevent collision. Forthis experiment, the proportional gain of the DCM controllerwas increased from 3 m/m to 5 m/m in the vertical axis,while the vertical ld weighting was increased from 1 to 5 toimprove tracking (refer to Table I).

C. Step Adjustment

In a final experiment, ESCHER was subjected to externaldisturbance forces while walking forward on flat terrain. Fig-ure 10 includes the lateral CoM, VRP, and DCM trajectoriesover the course of eight steps. The figure marks the desiredhorizontal position of the swing foot at the time of heel-strike to illustrate the corresponding step adjustments. As inthe compliant terrain experiments, each footstep was selectedwith a 2 s step duration and 0.15 m stride length to emulate

Fig. 8. Time-lapse of ESCHER stepping up 9in x 11in industrial stairs,made to specification of the DRC Finals.

Fig. 9. Time-lapse of a lateral step adjustment in response to an externaldisturbance applied to the hip (corresponding to Fig. 10 at the 10 s mark).

Fig. 10. Lateral DCM, VRP, and CoM trajectories during step adjustmentexperiment. The desired swing foot position at the time of each heel strikeis noted by the triangle markers (blue for nominal, yellow for adjustedreference). The two lateral pushes occur at approximately 6 and 10 seconds.

a moderate walking speed. Two disturbance impulses ofincreasing magnitude were applied at approximately 6 and10 seconds. Figure 9 includes a time lapse of the robotrecovering from the large push applied to the left hip usingthe proposed step adjustment strategy (5).

As illustrated in Fig. 10, each push results in a significantdeviation of the estimated DCM from the nominal refer-ence trajectory. The DCM error transients appearing in theundisturbed steps occur primarily during heel-strike and arequickly corrected by the momentum controller. The DCMerror transient following the first push is within the deadbandof the step adjustment algorithm. Thus, the desired footholdposition remains unchanged, as the momentum controller isable to eliminate the error by shifting the eCMP. The secondpush, however, results in an error that exceeds the deadband,triggering a step adjustment of 6.8 cm in the same direction

836

Page 7: Design of a Compliant Bipedal Walking Controller for the ...web.tuat.ac.jp/~gvlab/ronbun/ReadingGroupHCR/Design... · Design of a Compliant Bipedal Walking Controller for the DARPA

as the disturbance force. The modified base of support allowsthe robot to apply the necessary restoring forces to stabilizethe DCM following heel-strike. Note that the DCM trackingquality is recovered within one step.

VII. CONCLUSION

The experimental results demonstrated the effectivenessof the proposed controls approach for locomotion on softand uneven terrain. By shifting the nominal CoP trajectorytowards the inside of the foot, the controller was able toreduce lateral sway and improve tracking performance in thepresence of unknown surface features. This simple modifi-cation was critical to the robot’s ability to walk on outdoorsurfaces including dirt, gravel, and grass. The proposedstep adjustment algorithm was shown to increase controllerrobustness in the presence of external disturbances. Theresults motivate the use of simple heuristics to modify theswing foot position in response to the estimated DCM error.

The proposed walking controller was used during the DRCFinals without modification. On the first day, the robot fellwhen traversing the 200 ft dirt track due to a minor hardwarecomplication. On the second day, the robot was able to walkfrom the starting line to the door, verifying the reliability ofthe proposed approach on compliant terrain. Additional ex-periments were performed using identical control parameterson a variety of terrain to test the reliability of the approach.

Although unable to attempt the stairs task at the DRCFinals, ESCHER successfully demonstrated the proposedapproach outside of competition. The use of a half-step strat-egy significantly increased the available workspace. Whencombined with careful vertical CoM planning, a soft positionlimit on the ankle joint allowed the robot to naturally avoidcollisions when stepping up. Future research efforts willfocus on alternative methods for real-time step adjustmentusing model predictive control. By including the step adjust-ment parameters in the dynamic planner it may be possibleto improve robustness in the face of large perturbations andlimit horizontal drift during walking.

ACKNOWLEDGMENTS

This material is supported by ONR through grant N00014-11-1-0074 and by DARPA through grant N65236-12-1-1002.We would like to thank Team VALOR and the many studentswho contributed to the development of ESCHER.

REFERENCES[1] M. A. Hopkins, D. W. Hong, and A. Leonessa, “Compliant locomo-

tion using whole-body control and Divergent Component of Motiontracking,” in Robotics and Automation (ICRA), IEEE InternationalConference on, May 2015.

[2] M. A. Hopkins, D. W. Hong, and A. Leonessa, “Humanoid locomotionon uneven terrain using the time-varying Divergent Component of Mo-tion,” in Humanoid Robots (Humanoids), 14th IEEE-RAS InternationalConference on, Nov 2014.

[3] M. A. Hopkins, S. A. Ressler, D. F. Lahr, D. W. Hong, andA. Leonessa, “Embedded joint-space control of a series elastic hu-manoid,” in Intelligent Robots and Systems (IROS), IEEE/RSJ Inter-national Conference on, Hamburg, Germany, October 2015.

[4] S. Feng, E. Whitman, X. Xinjilefu, and C. G. Atkeson, “Optimization-based full body control for the DARPA Robotics Challenge,” Journalof Field Robotics, vol. 32, no. 2, pp. 293–312, 2015.

[5] S. Kuindersma, F. Permenter, and R. Tedrake, “An efficiently solvablequadratic program for stabilizing dynamic locomotion,” in Roboticsand Automation (ICRA), IEEE International Conference on, May2014.

[6] M. Johnson, B. Shrewsbury, S. Bertrand, T. Wu, D. Duran, M. Floyd,P. Abeles, D. Stephen, N. Mertins, A. Lesman et al., “Team IHMC’slessons learned from the DARPA Robotics Challenge Trials,” Journalof Field Robotics, vol. 32, no. 2, pp. 192–208, 2015.

[7] B. Stephens and C. Atkeson, “Dynamic balance force control forcompliant humanoid robots,” in Intelligent Robots and Systems (IROS),IEEE/RSJ International Conference on, Oct 2010, pp. 1248–1255.

[8] S.-H. Lee and A. Goswami, “Ground reaction force control at eachfoot: A momentum-based humanoid balance controller for non-leveland non-stationary ground,” in Intelligent Robots and Systems (IROS),IEEE/RSJ International Conference on, Oct 2010, pp. 3157–3162.

[9] A. Herzog, L. Righetti, F. Grimminger, P. Pastor, and S. Schaal, “Bal-ancing experiments on a torque-controlled humanoid with hierarchicalinverse dynamics,” in Intelligent Robots and Systems (IROS 2014),2014 IEEE/RSJ International Conference on, Sept 2014, pp. 981–988.

[10] P. M. Wensing and D. E. Orin, “Generation of dynamic humanoidbehaviors through task-space control with conic optimization,” inRobotics and Automation (ICRA), IEEE International Conference on,May 2013, pp. 3103–3109.

[11] T. Koolen, J. Smith, G. Thomas, S. Bertrand, J. Carff, N. Mertins,D. Stephen, P. Abeles, J. Englsberger, S. McCrory, J. van Egmond,M. Griffioen, M. Floyd, S. Kobus, N. Manor, S. Alsheikh, D. Duran,L. Bunch, E. Morphis, L. Colasanto, K.-L. Ho Hoang, B. Layton,P. Neuhaus, M. Johnson, and J. Pratt, “Summary of team IHMC’svirtual robotics challenge entry,” in Humanoid Robots (Humanoids),13th IEEE-RAS International Conference on, Oct 2013.

[12] K. Hashimoto, H. jin Kang, M. Nakamura, E. Falotico, H. ok Lim,A. Takanishi, C. Laschi, P. Dario, and A. Berthoz, “Realization ofbiped walking on soft ground with stabilization control based on gaitanalysis,” in Intelligent Robots and Systems (IROS), 2012 IEEE/RSJInternational Conference on, Oct 2012, pp. 2064–2069.

[13] Y. Zheng, H. Wang, S. Li, Y. Liu, D. Orin, K. Sohn, Y. Jun, and P. Oh,“Humanoid robots walking on grass, sands and rocks,” in Technologiesfor Practical Robot Applications (TePRA), 2013 IEEE InternationalConference on, April 2013, pp. 1–6.

[14] J. Pratt, J. Carff, S. Drakunov, and A. Goswami, “Capture Point:A Step toward Humanoid Push Recovery,” in Humanoid Robots(Humanoids), 6th IEEE-RAS International Conference on, Dec 2006,pp. 200–207.

[15] 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 Intelligent Robots and Systems(IROS), IEEE/RSJ International Conference on, Sept 2008, pp. 1121–1126.

[16] J. Urata, K. Nshiwaki, Y. Nakanishi, K. Okada, S. Kagami, andM. Inaba, “Online decision of foot placement using singular lq previewregulation,” in Humanoid Robots (Humanoids), 2011 11th IEEE-RASInternational Conference on, Oct 2011, pp. 13–18.

[17] J. Englsberger, C. Ott, and A. Albu-Schaffer, “Three-dimensionalbipedal walking control based on divergent component of motion,”Robotics, IEEE Transactions on, vol. 31, no. 2, pp. 355–368, April2015.

[18] B. Lee, “Design of a humanoid robot for disaster response,” Master’sthesis, Virginia Polytechnic Institute and State University, April 2014.

[19] C. Knabe, B. Lee, V. Orekhov, and D. Hong, “Design of a compact,lightweight, electromechanical linear series elastic actuator,” in ASMEInternational Design Engineering Technical Conference, 2014.

[20] T. Takenaka, T. Matsumoto, and T. Yoshiike, “Real time motiongeneration and control for biped robot -1st report: Walking gait patterngeneration,” in Intelligent Robots and Systems (IROS), IEEE/RSJInternational Conference on, Oct 2009, pp. 1084–1091.

[21] J. Englsberger, T. Koolen, S. Betrand, J. Pratt, C. Ott, and A. Albu-Schaffer, “Trajectory generation for continuous leg forces duringdouble support and heel-to-toe shift based on divergent componentof motion,” in Intelligent Robots and Systems (IROS), IEEE/RSJInternational Conference on, Sept 2014.

[22] K. Harada, K. Miura, M. Morisawa, K. Kaneko, S. Nakaoka, F. Kane-hiro, T. Tsuji, and S. Kajita, “Toward human-like walking patterngenerator,” in Intelligent Robots and Systems (IROS), IEEE/RSJ In-ternational Conference on, Oct 2009, pp. 1071–1077.

837