Assessing Whole-Body Operational Space Control in a Point-Foot Series Elastic Biped: Balance on Split Terrain and Undirected Walking

4.2 MB·English

by  Donghyun Kim

IEEEJOURNALONXX,VOL.XX,NO.YY,JANUARY14,2015 1
Assessing Whole-Body Operational Space Control
in a Point-Foot Series Elastic Biped:
Balance on Split Terrain and Undirected Walking
Donghyun Kim, Ye Zhao, Gray Thomas, and Luis Sentis
Abstract—In this paper we present advancements in control of operational space accelerations, known contact constraints,
5 and trajectory generation for agile behavior in bipedal robots. and desired internal forces. The internal forces, during multi-
We demonstrate that Whole-Body Operational Space Control
1 contact,correspondtothelinearsubspaceofjointtorquesthat
(WBOSC),developedafewyearsago,iswellsuitedforachieving
0 do not cause accelerations of the robot. The basic contact-
two types of agile behaviors, namely, balancing on a high pitch
2
split terrain and achieving undirected walking on flat terrain. consistent whole-body operational space control structures

n The work presented here is the first implementation of WBOSC were laid out in [2] and then extended in [1] but only
a on a biped robot, and more specifically a biped robot with demonstrated in simulation. This differs from the work of [3],
J serieselasticactuators.Wepresentandanalyzeanewalgorithm
a related strategy using torque controllers to optimize the dis-
3 that dynamically balancespoint foot robots bychoosing footstep
tributionofreactionforces,inthatWBOSCconsidersinternal
1 placements.Dealingwiththenaturallyunstabledynamicsofthese
type of systems is a difficult problem that requires both the forces explicitly, and separately from the operational space

] controller and the trajectory generation algorithm to operate acceleration goals, and places them under feedback control.
O
quickly and efficiently. We put forth a comprehensive develop- ThisfeaturealsoseparatesWBOSCfromthecontrollerof[4],
R ment and integration effort: the design and construction of the which acknowledges the internal forces but specifies reaction
bipedsystemandexperimentalinfrastructure,acustomizationof
. forces and does not impose a feedback law on the internal
s WBOSC for the agile behaviors, and new trajectory generation
c algorithms. Using this custom built controller, we conduct, for component. Implementation of a whole-body controller for
[ first time, an experiment in which a biped robot balances in a quadrupedal robotswith optimizeddistribution of thereaction

1 high pitch split terrain, demonstrating our ability to precisely forces is described in [5]. Hardware experiments on a series
regulate internal forces using force sensing feedback techniques.
v elastic actuated quadruped robot with actuated ankles using
Finally, we demonstrate the stabilizing capabilities of our online
5 quadratic programming to minimize tangential reaction forces
trajectory generation algorithm in the physics-based simulator
5
and through physical experiments with a planarized locomotion are shown in [6]. Approaching the problem from a plan-
8
setup. ning perspective, [7] explores advanced optimization methods
2
0 to solve the multi-objective contact dynamics of graphical
. avatars. Recently this team has begun to test quasi static
1
I. INTRODUCTION contact tasks in small size humanoid robots. The above is just
0
5 Controlling contact with the external world is critical for a short list of whole-body controllers of similar type. There is
1 smooth operation of bipedal robots in cluttered environments, anabundanceofwhole-bodytaskcontrollersforleggedrobots
v: quickly climbing rough surfaces, and safely colliding with that we shall review further below. In view of the available
i objects.Addressingthesegoalsnecessitatesthatweunderstand whole-body controllers, the work presented here is the first
X
the hardware and computational requirements of general agile to implement a whole-body operational space controller on a
r behaviors and that we validate such behavior in physical point foot biped robot, it is the first to show biped balancing
a
systems.Themainobjectiveofthispaperistoadvanceagility on high pitch split terrains, and is the first to use whole-body
by leveraging Whole-Body Operational Space Control [1] operational space control for biped dynamic locomotion.
(WBOSC). We focus on dual contact maneuvers in extreme In essence, the main contributions of this study are: (1)
terrain, and undirected walking. We achieve this by (1) build- creating a bipedal robot infrastructure based on whole-body
ing a hardware infrastructure based on a series elastic point operationalspacecontrol,(2)incorporatingsensor-basedfeed-
footbipedalrobot,(2)leveragingWBOSCtoregulateinternal backcontrollersforinternalforceregulationduringbalancing,
force behavior and achieve dynamic locomotion, (3) devel- (3) introducing a new online trajectory generation algorithm
oping a new trajectory generation algorithm for undirected for undirected walking, and (4) assessing the performance of
walking,(4) testingdynamic locomotionwitha physicsbased whole-body operational space control for balancing on a high
simulation, and (5) conducting various experiments on agility. pitch split terrain and for undirected walking.
Whole-body Operational Space Control is a framework
which returns the joint torques consistent with a desired set II. RELATEDWORK
All authors are with the Department of Mechanical Engineering, The A. Control of Robots with Point Feet
UniversityofTexasatAustin,AustinTX78712-0292
Point-foot biped robots similar to ours have been widely
D.Kim,Y.Zhao,G.Thomas,andL.SentisarewiththeHumanCentered
RoboticsLaboratory usedfordynamiclocomotion[8]–[13]duetotheirmechanical
IEEEJOURNALONXX,VOL.XX,NO.YY,JANUARY14,2015 2
simplicity. Point-foot robots have the potential to accomplish springs,wehavehaddifficultytoquicklyandaccuratelyswing
more agile swing trajectories without the weight of an ankle. theleg,”Asentimentechoedbyourownobservations.Recent
Point feet, however, are more difficult to control than robots studies describe potential solutions for this type of problem
with powered ankles since they lack the ability to apply a with SEA actuators [21], [25]–[27], with passivity based
torque to the ground. Few have managed to walk upright controllersemergingasasolid,ifconservative,approach.[21]
without the help of a constraint mechanism, the two most adds an inner motor velocity loop and incorporates integral
notableexamplesbeingthehydraulicallyactuatedhopperfrom torque action to the controller. More recently, [27] compared
[14] and the biped from [10]. This fundamental difficulty can the stability and performance of various active impedance
still be meaningfully addressed in a planar case, and the vast control approaches based on cascaded SEA controllers.
majority of research on point foot biped robot locomotion
focuses on robots constrained to a plane.
C. Whole-Body Controller Design
Similar to [15] our robot is electrically powered and has
six actuated leg joints. In terms of servo rates, point foot Mostexistingexperimentalapproachesforwhole-bodycon-
biped systems are especially unforgiving, since it is difficult trol are based on optimization methods which were pioneered
to reduce the fundamental time constant for falling over. This by [3]. [4] represents the first implementation of full dynamic
makes sensing systems for point foot bipeds different from based task controllers with contact constraints on a humanoid
those of robots with actuated ankles, such as Atlas [16], robot. The work focuses on push recovery and basic walking.
Valkyrie[17],andSarcos[18]inthattheymustbefocusedon In [28] the implementation of hierarchical inverse dynamics
fast maneuvers. To accommodate their fast dynamics, special algorithms using quadratic program solvers is presented on
caremustbetakentoensurethatthecontrolsystemsforthese a Sarcos biped robot. Experiments include balancing while
robots operate at high frequency and that motion is detected withstandingexternalforces,balancingonamovingplatform,
athighspeed.Relativetotheinfrastructureofotherpointfoot and balancing on a single foot. In [29] a torque-based whole-
robots such as [19] and [13], our setup includes an additional body controller is presented for controlling the Atlas and
overhead motion capture system, which communicates with Valkyriehumanoidrobots.Aquadraticprogramsolverisused
the control computer to allow absolute position feedback. We to minimize momentum rate objective, contact force regula-
do not have sensors on the planarizer’s joints or slider. tion and task acceleration regularization. In [30] whole-body
control with inequality constraints via inverse dynamics and a
quadratic program solver is proposed on the humanoid robot
B. SEA Control
HRP-2. However, the algorithm is used offline to generate
SeriesElasticActuators(SEAs)aredesignedwithanelastic trajectoriesthatarethentrackedbytherobot.In[31]atorque-
element and provide two important benefits over their rigid, based whole-body controller focused on optimization of mul-
or directly connected, counterparts: they offer improved force ticontact wrenches over a period of time corresponding to
control accuracy [20], [21], and a lower output inertia. This center of mass (COM) movement is presented. The approach
high fidelity torque tracking is critical to contact scenarios is, for now, presented in the context of balancing and is only
where the internal multi-contact forces must be maintained shown in simulation. All of these works aim only to control
within a certain range, as is the case when a robot balances humanoids with actuated ankles, and thus do not consider
on steep terrain. Some robots, for example the robots of [19] the under-actuation situations endemic to dynamic point-foot
and [15], use SEAs only for energy storage, and do not take walking. Separately, in [32] optimization methods based on
advantage of their force control capabilities. Hume, on the inverse dynamics projected on the contact null space are used
other hand uses SEAs primarily for their force control and to control the gait of a quadrupedal robot. In contrast to these
inertialbenefitstohandlecollisions.YettheinclusionofSEAs works, ours is the first to implemented an operational space
comes at a cost to performance. Controllers that have been inverse dynamic algorithm in an underactuated bipedal point
designed to approximate the dynamics of a perfect position foot robot. It does not attempt to control the center of mass
source, a perfect torque source [20]–[22], or a second order during locomotion, relying solely on estimation via prismatic
mass-spring-dampersystemallfacethesameproblemintheir invertedpendulumdynamics.Additionally,thepreviousworks
final closed loop system: the dynamics of the reference plant have not implemented sensor-based internal force control nor
are impossible to achieve at the highest frequencies. have they attempted to balance the robots on such extreme
The use of SEAs for biped locomotion was pioneered surfaces.
in [23], which suggests a straightforward PD torque control In particular, between those controllers that perform task
strategy with some model-based feedforward terms. However, space inverse dynamics, there are controllers which calculate
the work does not compensate for static errors. The same internal forces as a byproduct of another optimization and
torque controller was used in the SEA actuated robot [24] to there are those that require the specification of the internal
study push recovery. However, such an application does not forces of contact. Methods which deal with contact forces
require accurate force tracking so much as position control, as generally interested in balance [3], [33], [34] rather than
and thus it is not possible to asses the torque performance in controlled interaction with the environment. For instance
of these actuators. Trajectory following accuracy did appear maintaining a stance between highly sloped surfaces. In their
to be limited by the SEA compliance; In the author’s own work the main objective is to maintain balance while keep-
wordstheystate:“DuetotheuseofSEAswithverycompliant ing the reaction forces within friction cones so contact is
IEEEJOURNALONXX,VOL.XX,NO.YY,JANUARY14,2015 3
maintained. [32] used the reaction force method to keep a
quadrupedonasurfacewithaapproximately40◦ inclinations.
However, until now, no group has approached to problem of
accurately controlling these internal forces through feedback
control.Previousmethodshavefedthedesiredreactionforces
intotheirinversedynamicsroutineasafeed-forwardterm,and
the error between the actual internal force and the achieved
internal force was not used to lower this error through joint
torque. Such feedback would potentially require significant
modification to optimization based method of determining the
feed-forward reaction force.
Whole-Body Operational Space Control [2], is centered
around the idea of achieving operational space impedance
control and controlling the internal forces between supporting Fig.1. HumeRobotKinematics,aswhenattachedtotheplanarizinglink-
contacts.Inparticular,internalforcesarechosensuchthatthey age.Blueschematicsdescribethefloatingbasejoints,whileblackdescribes
thekinematicsofthesixlegjointsandthreeplanarizerjoints.Theplanarizer
aredecoupledfromthemotionoftherobotandcanbedirectly
kinematics are not included in the generalized joint vector, since they are
specified by a high level planner. By using feedback control not part of the robot’s model. The locations of the LED tracking markers
on these internal forces we can achieve higher precision force identified by the PhaseSpace Impulse Motion Capture system are shown as
reddots.
tracking which is less sensitive to modeling errors than it
would if we used feed-forward internal forces.
III. SYSTEMCHARACTERIZATION
A. Hardware Setup
D. Locomotion
Our robot is a teen-size humanoid robot measuring 1.5
Ahybriddynamicalsystemsproblem,point-footlocomotion meters in height and 20 kg in weight. The leg kinematics
in the plane is difficult because single support motion is resemble simplified human kinematics and contain an ad-
under-actuated and thus the system can only be stabilized duction/abduction hip joint followed by a flexion/extension
when the discrete effects are taken into account.1 One of the hip joint followed by flexion/extension knee joint as shown
most successful approaches to this problem comes from [19], in Fig. 1. The lack of ankle joints allows the shank to be
wherein the feedback whole body controller constrains the essentially just a lightweight carbon fiber tube. At the tip of
dynamics to match a model which is predicted by a simu- the shank we have incorporated contact sensors which are
lation to be stable in the discrete sense. Another successful essentially limit switches. The series elastic actuators on all
approach based on hybrid zero dynamics is the line of work six joints are based on a sliding spring carriage connected to
by [35] which utilizes human inspired trajectories to generate the output by steel ropes. The deformation in the springs are
stable periodic locomotion. These formulations are designed directly measured within the carriage assembly. The concept,
toachieveperiodicmotions,asisalsothecaseforotherworks kinematics, and specifications of the robot were proposed
based on Poincare maps [13]. by our team at UT Austin, and the design was executed in
Algorithms such as the capture point [24] and phase space collaboration with Meka Robotics and manufactured by that
planning approaches [36] can be used to stabilize the pendu- company. For fall safety the robot is attached to a trolley
lummodelbymodifyingfootsteplocations.Phasespaceplan- system with a block and tackle system which allows easy
ning extends the linear pendulum model to consider the case lifting and locking at a height. In addition, the robot has a
wherethecenterofmassheightisafunctionofthehorizontal removable planarizer mechanism which constrains the motion
position, the prismatic inverted pendulum model [37]. Thus of the robot to the saggital plane. The pitch of the robot
oneobjectiveofthispaperistoassesstheperformanceofour remains unconstrained, as the planarizer connects to the robot
whole body controller in the task of restricting the dynamics through a set of bearings. Ultimately, pitch, forward motion,
to match those of a prismatic inverted pendulum model. In andverticalmotionareallowed,whilelateralmotion,roll,and
this last conference paper we presented an earlier version of yaw are prohibited.
the trajectory generation algorithm considered in this paper. Fromanelectricalpointofviewtherobotiscontrolledwith
However, no control of internal forces or experiments of any distributeddigitalsignalprocessors,connectedbyanEtherCat
typewereconducted.Asstatedin[38]thereisstilllittlework network to a centralized PC running a real-time RTAI Linux
on aperiodic walking. Our trajectory generation algorithm kernel. This communication system introduces a 1ms delay
achievesaperiodicgaitsbyexploitingasimpletimetovelocity from the linux machine to the actuator DSPs and back. Each
reversalrule.Itisinsomewaysclosetothealgorithmby[39] DSP controls a single actuator, and they do not communicate
butdesignedforbalancingin3Dinsteadofwalkinginthe2D directly with each other. Power is delivered through a tether
plane. from an external source.
A Microstrain 3DM-GX3-25-OEM inertial measurement
unit (IMU) on the robot’s torso measures angular velocity
1Providedwearewillingtoneglecttheextracontrollabilityaffordedbythe
Coriolisandgravityterms. and linear acceleration, which is used in the state estimator.
IEEEJOURNALONXX,VOL.XX,NO.YY,JANUARY14,2015 4
Fig.2. OverallControlDiagram.Thisfigureillustratesthewholebodyoperationalspacecontrolprocess(WBOSC)andthejoint-leveltorquecontrollers.
One of our main contributions comes from the feedback control of internal forces. Note that the gains for the controllers are treated as additional input
parameterstorepresentthegainschedulingweperforminordertoachievethebestpossibleperformancewitheachtask.
Additionally,therobothasafulloverheadPhaseSpaceImpulse C. Series Elastic Actuators
motion capture system which gives it global coordinate infor- The robot came with MEKA’s pre-loaded joint controllers
mationaboutsevenuniquelyidentifiableLEDtrackingdevices based on the passivity torque controller described in [21]
mountedrigidlytothetorso.ThePhaseSpacesystemproduces (shown on the lower right corner of Fig. 2). We kept this
adatastreamat480HzandcommunicatestotheLinuxControl controller’s structure while changing the gains of the feed-
PC via a custom UDP protocol. There is an approximate back controller to enhance the performance of the high level
15msdelayinthedataforfeedbackpurposes.Itaccomplishes controller, and this ultimately entailed reducing the low level
this using a system of sixteen high speed cameras mounted torque gains. In order to tune the torque gains we leveraged
on the ceiling above the robot, and a proprietary software our findings in [25]. In this study we describe a trade-off
package to fuse their readings into a single estimate for the between torque gains and position gains in a distributed
three dimensional position of each marker. On each update, control architecture. Specifically, we explore the observation
the system reports the location of as many of the uniquely that raising the torque controller’s proportional gain limits the
identifiable LED markers as it can see. maximum stable position gains and vice versa. To respond to
this observation we implemented a gain scheduling strategy:
B. End-to-End Controller Architecture
in the joints of the stance leg we lowered the torque gains so
The feedback control system is split into six joint level we could raise the proportional gain and reduce error. In the
controllers and a centralized high level controller (see Fig. 2). joints of the swing leg we raised the torque gains to produce
The purpose of the joint level controllers is to achieve good less friction dominated behavior.
torque tracking given the series elastic actuators. This type
of control architecture falls into the category of a distributed D. Whole Body Operational Space Control
control system which allows the joint controllers to focus on Whole-Body Operational Space Control [1] is a feedback
high speed actuator dynamics while the centralized controller control strategy based on Operational Space Control [40],
doesnotneedtodealwiththisnuance.Yetthefeedbackatthe which extends it to floating base robots in contact with
highlevelisnecessaryinordertocreatethecouplingbetween the environment. It allows the user to specify multiple task
jointsimpliedbyoperationalspaceimpedancetasksaswellas objectives and their impedance in operational space. It ad-
regulating the internal forces between multicontact supports. ditionally subdivides the torques applied to the robot into
IEEEJOURNALONXX,VOL.XX,NO.YY,JANUARY14,2015 5
orthogonal spaces which affect either the motion of the robot y˜k = xk +Akzk where Tk = {xk ∈ R3,Ak ∈ R3×3} is the
i i
or the internal forces which do not. When the user specifies affine transform at time k and the default pattern, z ∈R3, is
i
these internal forces our implementation governs them using essentially just the measurement from some default position,
feedback.Whole-BodyOperationalSpaceControlisexplained but is shifted such that the center of the coordinate system is
in Appendix A. the geometric center of the points. That is the first moment is
At the implementation level, WBOSC worked – provided zero for the pattern: Σ7 e z =0 for j =0,1,2. This affine
i=1 j i
that our latencies were sufficiently small. Achieving a 1 ms transform includes both a linear bias term which represents
latency required some significant software work. We re-wrote translation of the geometric center of the LEDs, and a matrix
the firmware provided by our robot manufacturer in order to termwhichrepresentsrotationaswellasnon-physicalskewing
ensureourcontrolleroperatedwithinareal-timethread,andto and scaling of the pattern. The pattern is specifically designed
incorporateitintoahierarchicalchainstructurewhichensures suchthattheoriginofthepatternframeisthegeometriccenter
minimumlatencyforstackedcontrolsystems.Wealsoreduced of the points. As this model is linear in the affine parameters
the basic computational cost of our WBOSC algorithm by we can solve for the case where all the LEDs are visible as
bypassing recursive dynamics software and instead using a follows
closed form expression to calculate the mass matrix. In order y˜k
to reduce the tracking error, we added an integral term to all θk =(cid:18) xk (cid:19), y˜k = ..1=Rθk, (1)
position tasks, which helps alleviate the friction difficulties vec(Ak)  . 
involved in lowering the torque gains at the DSP level. This y˜7k
also reduces error due to inaccuracy in the gravity estimation
 (cid:124) (cid:124) (cid:124)
term and other steady state errors in our dynamics model. I3×3 e0z1 e1z1 e2z1
(cid:124) (cid:124) (cid:124)
When operating with the robot within a planarizing linkage R=I3.×3 e0.z2 e1.z2 e2.z2, (2)
mechanism, as is the case for the experimental section of this  . . . . 
 . . . . 
paper, we still model the robot as having a floating base. (cid:124) (cid:124) (cid:124)
I e z e z e z
3×3 0 7 1 7 2 7
We then incorporate the additional inertia of this planarizer
as a lumped mass inside the floating base. Thus, in the θk (cid:44)(R(cid:124)R)−1R(cid:124)yˆk, (3)
simp
equations in Appendix A, the generalized coordinate variable
q corresponds to the six degrees of floating base kinematics where (1) describes affine transforms in vector form, and
plus six degrees of freedom corresponding to abduction, hip, demonstrates the linearity of prediction, and (3) defines the
and knee kinematics for the two legs as shown in Fig. 2. affinetransformwhichbestdescribestheobservedLEDvector
Our matrix U corresponds to floating base kinematics. We yˆk as a transform of the pattern. However, we have opted
also added terms to the mass matrix representing the rotor to lowpass this sensor data by adding twelve extra rows of
inertia of the motors, which showed a slight improvement in regularization terms and a diagonal weighting matrix,
performance. Finally, we tuned the task gains experimentally  
I 0 0
3n×3n
using heuristics.
W = 0 λ1I3×3 0 , (4)
0 0 λ I
2 9×9
E. State Estimation and Sensor Fusion to the least squares equation. In addition, we cannot always
The controller needs an estimate of the body orientation assume that all LEDs are visible and we must define a
every millisecond, yet the motion capture system updates at knockoutmatrixKo whichselectstheLEDswhichthesystem
480 Hz, occasionally fails to track a subset of the markers, successfully located:
and has a non-trivial latency of 15 ms. Given that the on e(cid:124) if LED 1 was found
0
board IMU accurately reports the rotational velocity of the e(cid:124) if LED 2 was found
torso with respect to an inertial reference frame, we integrate Ko ∈Rn×7 = 1 .. . (5)
forwards in time to maintain an estimate of the orientation  . 
(cid:124)
while waiting for an update from the overhead positioning e6 if LED 7 was found
system. When such an update arrives, we acknowledge the
Where(K ⊗I )selectsonlyequationsrelatingtoobserved
o 3×3
feedback latency of the sensing system process, and generate
LEDs from the original regressor,
an innovation measurement based on not the most current
(cid:18) (cid:19)
(K ⊗I )R
value of the orientation estimate, but the estimate from 15 R (cid:44) o 3×3 . (6)
r I
controller update periods into the past - i.e. the ratio between 12×12
the 15ms latency of the motion capture system and the 1ms This results in a new estimate of the affine transform,
servo rate. (cid:18)(K ⊗I )yˆk(cid:19)
Calling this time k = t−15, we can setup a least squares θk (cid:44)(R(cid:124)WR )−1R(cid:124)W o 3×3 , (7)
r r r r θ˜(k|k−p)
problemwhichminimizesthedistancebetweenmeasuredLED
position yˆk ∈ R3 and predicted LED position y˜k ∈ R3 for where the integration of the IMU orientation rate data,
i i
i=1,...,n,wherenistypically7,butdecreaseswhenLEDs
b (cid:18) (cid:19)
areblockedorotherwisenon-visible.OurmodelpredictsLED θ˜(b|a)=θa+(cid:88) 03 ∆t, (8)
locationsbasedonanaffinetransformationofadefaultpattern r t=a vec(ωˆItMU×)
IEEEJOURNALONXX,VOL.XX,NO.YY,JANUARY14,2015 6
is the basis for the regularization term setpoints, as shown in
(7).
The regularization adds another term to the objective func-
tion, specifically the squared deviation between each element
oftheaffinetransformrotationmatrixandourestimateofthis
matrixgiventheorientationestimateattimek.Regularization
isalsoappliedtothevectorcomponentoftheaffinetransform,
but the weight λ is so small as to be neglected. The weight
1
λ ,ontheotherhand,representsasignificantinitialcovariance
2
which specifies the tradeoff between old knowledge and new
knowledge. The weights were chosen such that the discrete
time half life of an error in the orientation estimate is one
update if all seven LEDs are visible.
Solvingthisleastsquaresproblemreturnsatransformwhich Fig.3. IllustrationofInternalForcesforVariousRobots.Internalforces
inpointfootrobotscorrespondtotensionsorcompressionsbetweenpairsof
is potentially skewed and scaled as well as rotated, and thus
supportingcontacts.
is not a valid rigid body transform. By finding the closest
quaternion to this rotation matrix we constrain the result to
valid transforms, using the method of [41]. Finally, with the Equation (29)
orientation estimate at time k being the closest quaternion to
theaffinetransform,wecanintegratetheIMUdatafromtimes f =S (cid:16)JT (cid:2)UTτ −b−g(cid:3)+Λ J˙ q˙(cid:17),
ext,dual swing s control s s
k to t to estimate the orientation at time t. The integration of (10)
IMU data continues incrementally while the algorithm waits whereS ∈R3×6selectstheconstraintforcesoftheswing
swing
for the next piece of overhead camera data to arrive, and leg from those of both foot contact constraints and w ∈[0,1]
then it repeats the more complex fusion algorithm. Since the represents the linear ramp.
algorithm performed adequately the first time it was used, no
attempt was made to precisely tune the delay estimate and
IV. FEEDBACK-BASEDINTERNALFORCECONTROL
filter constants.
Internal force behavior corresponds to actuator forces that
produce no net effect on the robot’s motion. A such, internal
F. Contact Transitions
forces correspond to mutually canceling forces and moments
In order to reduce the high speed behavior associated with between pairs or groups of contact branches, i.e. tensions,
a sudden change in joint torques, we have implemented a compressions and reaction moments. For instance, a triped
strategy which acts to smooth out the torque commands when point foot robot has three internal force dimensions while a
the robot transitions between single and dual support. The biped point foot robot has a single internal force dimension
sudden change of torque commands is due to the instanta- as shown in Figure 3.
neousswitchbetweenconstraintsetswithinWBOSC,andour In this context, building a biped robot with excellent torque
method to smooth these torque commands effectively bridges sensing has two advantages: (1) its ability to use low level
the difference between single contact and dual contact. To torque control to overcome actuator friction and achieve
make WBOSC with a single contact constraint produce the greatercontrolbandwidth.Inturn,ourrigidbodyassumptions
result it would with a dual contact constraint we simply add tomodelinternalforcesarelessaffectedbylowlevelactuator
a desired reaction force to the swing foot – the same reaction dynamics; and (2) torque sensors on the robot’s joints permit
force that would be expected of this foot in the dual contact theimplementationofsensor-basedinternalforcefeedbackfor
case. Then, to transition, we decrease this desired reaction accurate tracking.
force linearly with time in the case of foot lifting, or linearly What is interesting about taking a model-based approach
rampitupfromzerointhecaseoffootlanding.Thisrequires is that internal forces are fully controllable by definition as
that we know this reaction force beforehand, so we must they are orthogonal to the robot’s motion. As such, both the
always run the controller with dual contact constraints before robot’smovementanditsinternalforcescanbesimultaneously
the single contact version. When lifting the foot we can use controlled to feasible values. Moreover, in many types of
the previous value of the reaction force, but when landing we contactposes,internalforcesareeasilyidentifiableusingsome
run the controller in dual contact once at the beginning of the physical intuition. For instance, in the triped pose of Figure 3
foot landing transition phase for the sole purpose of acquiring the three feet can generate three virtual tensions between the
this reaction force. pointsofcontact.Thephysicsoftensionforceswereanalyzed
To implement this desired external force in single contact in greater detail using a virtual linkage model in [1].
WBOSC we add the term fext to Equation (35), Internal forces are part of the core functionality of Whole-
F =Λ∗ u +µ∗ +p∗ +f . (9) BodyOperationalSpaceControl.IntheAppendixsection,we
task task task task task ext describe the model-based control structures enabling direct
This force f (cid:44) wf can be extracted from the dual control of internal forces. In particular, the basic torque
ext ext,dual
contactWBOSCaftertheoutputtorqueiscalculatedbasedon structure derived in Equation (43) is written here again for
IEEEJOURNALONXX,VOL.XX,NO.YY,JANUARY14,2015 7
readability, with
(cid:16) (cid:17) (cid:0) (cid:1)
Γint = Ji∗|lT Fint,ref −Fint,{t}+ µi+ pi , (11) St = 1 0 0 , (15)
where Fint,ref is the vector of desired internal forces and xˆT xˆ= ||PPRR−−PPLL||
Fint,{t} corresponds to the mapping of task torques into the Rt =yˆT, yˆ=(−xˆ(2),xˆ(1),0)T , (16)
ianstseurmnapltifoonrctehamtacnoifmomlda.nTdheedatboorqvueeseqaunadtioanctiusalbatsoerdquoens tahree zˆT zˆ=xˆ×yˆ
identical, and that the kinematic and dynamic models are
(cid:0) (cid:1)
∆ = I −I . (17)
exact. Because these premises are never true, to achieve best t 3×3 3×3
results on force regulation or tracking, we propose to employ
where P and P are the position of the right and left feet,
sensor-based feedback control on the internal forces. To our R L
respectively.
knowledge, this is the first time that sensor-based feedback
In order to compute desired internal forces, we suggest
control of internal forces is proposed and achieved in a real
eithertouseheuristicsaswedointhispaper,ortheuseofthe
robot.
virtuallinkagemodelandthemulticontact/graspmatrix,which
Because internal forces are fully controllable, we incorpo-
were proposed in [1]. Those models allow to relate the center
rateasimpleproportionalcontrollerontheinternalforcesinto
of mass and the internal force behavior to the reaction forces.
Equation (43),
A study on the usage of these models is currently beyond the
(cid:16) intendedscopeofthispaper.Comparedtoothermethodsbased
Γint = Ji∗|lT Fint,ref −Fint,{t}+ µi+ pi on optimizing reaction forces, our method uses sensor-based
(cid:17) feedback control on the internal forces to regulate or track a
+K (F −F ) , (12)
F int,ref int,act desired tension reference with good accuracy.
where K is a proportional control gain and F are the
F int,act
actual sensor-based internal forces. In order to obtain these V. TRAJECTORYGENERATION
sensor-based forces, we use the torque sensors on the series
elastic actuators to find the co-states of constraint as per In order to stay upright our robot must constantly re-
Equation (29) and apply a projections W to find internal position its feet, and a mechanism of considerable complexity
int
forces, isrequiredtodecidetheupcomingfootpositionfortheswing
foot. We have developed a method for finding this position
(cid:104) (cid:105)
F (cid:44)W JT(UTτ −b−g)+Λ J˙q˙ , (13) which we refer to as a phase space constant time to velocity
int,act int s sensor s
reversal planner. In every step, when the lifting phase reaches
where τ corresponds to the vector of torques sensed by 80% completion, the planner runs in an online fashion and
sensors
thespringelementineachserieselasticactuator(seeFigure2). returns the next footstep location before the lifting phase ends
Althoughthisinternalforcemappingaboveisdistinguished and the landing phase begins. The operational space set-point
from previous work, due to its sensor-based force feedback trajectory for the swing foot is then defined parametrically
control, this mapping is valid due to the physical fact of based on this desired landing position, with the trajectory
robot redundancy in the multi-contact case. The induced ending once ground contact is sensed. If the planned step is
contact closed loop causes the number of controlled tasks outsidethemechanicallimitsoftherobotthestepsaturatesto
to be smaller than that of actuated joints. Correspondingly, the closest reachable step.
additional DOFs are available to be controlled for more tasks, Themethodweusetochoosethisfootsteplocationoperates
such as internal forces in Equation (13). This mapped internal on a one dimensional model appropriate for planar walking,
forceisconsistentwithcontactconstraintsandcancellationof but, by choosing the x and y components of the footstep
accelerations on the robot’s base or on the actuated joints [1]. location as solutions to the forward and lateral planar walking
More details can be found in the Appendix section. problems, it is extended to 3D walking. We will present first
To calculate internal forces for our bipedal robot, Hume, the planar algorithm used in the experimental section going
we need to define the mapping given in Equation (41) in on to explain our approach to 3D walking.
Appendix A, where W is the matrix representing the map 1) 1D Velocity Reversal: Our planner attempts to stabilize
int
from reaction forces to internal forces. In our case, Hume the robot by causing the center of mass to reverse direction
controls the internal forces between the two feet during every step. Central to this undertaking is the exploitation
dual contact phases. In dual support, the reaction forces are of a simplified model of the robot’s zero-dynamics given
(f , f , f , f , f , f )T, where R, L mean a right specific operational space tasks: the prismatic inverted pen-
Rx Ry Rz Lx Ly Lz
andleftfoot,respectively.Accordingto[1],W consistsofa dulum model, or PIPM. The PIPM considers a point mass
int
selection matrix of tensions, S , a rotation matrix from global constrained to an arbitrary continuous height surface by a
t
frame to the direction parallel to the line between two contact constraint wrench which evaluates to a pure force at both the
points, R , and a differential operator matrix, ∆ , i.e. foot point and the center of mass – that is the model assumes
t t
a point foot produces the reaction force and that the reaction
W =S R ∆ , (14) force points towards the center of mass. The PIPM can be
int t t t
IEEEJOURNALONXX,VOL.XX,NO.YY,JANUARY14,2015 8
Fig.4. ConstantTimetoVelocityReversalAlgorithm.Asshownin(a),weapproximatethedynamicsoftherobotwiththeprismaticinvertedpendulum,
shown in (b). This model predicts the dynamics of the horizontal center of mass position x given the stance foot location xp = and the height surface
z=h(x).Thiscanbeintegratedforwardintimeviathenumericalintegrationprocedureshownin(c).Whentheplannerstartsoperatingitrecordstheinitial
state andintegratesthisstateforwardtodeterminetheswitchingstate .Asshownintimelines(d)and(e),the“Estimatedsequence”oftheplannerhas
ananalogueinthe“Robotstates”ofthestatemachine.Inparticular,theswitchingstate roughlycorrespondstothedualsupportphaseofthewalkingstate
machine.Whenplanningwiththephysicalrobot,theplannercomputesapost-impactstate byapplyingavelocityadjustmenttotheswitchingstate.This
empiricalmeasurecompensatesforwhatappearstobeanearlyconstantdecreaseinvelocityateveryimpact.Thisnewstate representstheplanner’sguess
atthetime,x,andx˙ valuesimmediatelyaftertheswitch.Thegoaloftheplannerisultimatelytostabilizetherobot,butthisisimplementedbychoosingthe
nextfootstepsuchthatvelocityreversest(cid:48) secondsafterthefootswitcheverystep.Forsufficientlysmoothheightsurfaces,therelationshipbetweenthenext
footsteplocationpx andthevelocityx˙ ismonotonic,asshownin(g).Weusethestandardbisectionalgorithmtoidentifythenextfootplacement which
resultsinavelocityreversalstate attimet(cid:48),asshownin(f).
expressed as the differential equation gentle Cartesian trajectory task for swing foot. As illustrated
in Fig. 4, the planner begins calculating the landing location
g+z¨
x¨= z (x−xp). (18) when 80% of the lifting phase is reached in the robot state
machine’s progression. As such, the planner continuously re-
Accounting for z being a function of x, the height surface,
plans in an online fashion to correct for trajectory deviations.
dz dz dx Using the current estimate of the center of mass velocity and
= , (19)
dt dx dt position, it numerically integrates forwards in time to predict
its COM position and velocity when its stance foot and swing
d2z d(cid:16)dz(cid:17)dx dz d(cid:16)dx(cid:17)
= + , (20) foot will switch roles. This time, position, and velocity is
dt2 dt dx dt dxdt dt
known as the switching state, in Fig. 4. Yet this is not
d2z dz the state from which the planner begins predicting the COM
z¨= x˙2+ x¨. (21)
dx2 dx behavior for the upcoming swing. Instead, the planner applies
a subtle modification to the switching state which represents
By plugging Equation (21) into Equation (18), we obtain,
the effect of landing. After applying this model, we arrive at
g+ d2zx˙2+ dzx¨ the “post-impact state”, in Fig. 4. This point represents the
x¨= dx2 dx (x−x ), (22)
z p planner’sbestguessatthecenterofmasspositionandvelocity
immediately after the leg switch.
(cid:16) d2z (cid:17) dz
zx¨= g+ x˙2 (x−x )+(x−x ) x¨, (23)
dx2 p p dx
The implementation of the planner enforces the choice of
g+ d2zx˙2 a value for the reversing time, t(cid:48). This time value remains
x¨= z−(x−dx2x )dz(x−xp) (24) constant for every step, thus the user needs only to specify a
p dx single parameter for the planner to operate. As of now, t(cid:48) is
which now lacks any z¨ term, and can be used in Fig. 4c to manually chosen and as we show in the simulations it is able
integrate forward in time. to stabilize the biped for an arbitrary long number of steps.
This model is a close approximate to the zero dynamics of Theplannerthenfindsandreturnsthefootsteplocationwhich
our robot when a specific set of WBOSC tasks are accurately causes the robot’s COM velocity to reach zero, t(cid:48) seconds
maintained. These tasks are: (1) a center of mass height task, after the foot switch, starting from the post-impact state.
(2) a constant body link attitude, and (3) any sufficiently For each potential footstep location considered, the planner
IEEEJOURNALONXX,VOL.XX,NO.YY,JANUARY14,2015 9
Fig. 5. 3D Stabilization in Simulation. Subfig. (a) displays three steps of walking from forward COM phase space perspective, while (b) displays the
lateral COM phase portrait. In both figures the smaller axes highlight the planned versus actual trajectory for each of the three steps. The steps are shown
in(c)usingthesimulationgraphicsprovidedbytheSrLibMulti-BodyDynamicSimulationEnvironment.Subfig.(d)showsalongertimeperiodofwalking
fromseveralangles,anddemonstratesthegenerallystationarybehavior.
integrates forward in time starting from the post-impact state than 0.1m/s, the planner extends the step duration until the
assuggestedinFig.4f,returningthevelocityaftert(cid:48) seconds. switching state has a y speed of at least this value. When
Thisintegrationcanbeviewedasafunctionmappingfootstep the y phase plane process adjusts the switching time, the x
location to a future velocity, and it is this function over which phase plane process uses this new switching time to find its
we search for a zero crossing via bisection. Since we use switching state. Since the switching time represents the point
bisection, the number of integrations actually performed is at which the support leg changes, it cannot differ between the
verylow,howevertheprocessreliesonthemonotonicityofthe two directions.
relationshipbetweenfootsteplocationandthevelocityaftert(cid:48) Although the y directional impact does not appear to have
secondsofintegration.Iftheheightsurfaceisplanar,thenthis a bias, an x directional impact bias of −0.01m/s appears in
relationship is linear. the simulation results and is included in the planner, though
2) 2D Velocity Reversal for 3D walking: Choosing a foot- it is smaller than that of the experiment.
step for 3D walking is, under certain circumstances, identical Fig. 5 provides the results of our 3D walking algorithm
to choosing the footstep for 2D walking in two orthogonal in simulation, and demonstrates that the strategy stabilizes
directions. We take advantage of this interpretation to extend the velocity of the 3D simulation robot for an arbitrary long
our constant time to velocity reversal planner to 3D, as we number of steps. Note that in step (1) of Fig. 5 the planner
split 3D walking to a forward, x, phase space and a lateral, attempts to reverse velocity in x and yet the velocity remains
leftwards or y, phase space. While we could potentially allow positive after the step has been made. This is due in part to
different t(cid:48) parameters for the two planes, we use t(cid:48) = 0.24 the inaccurate landing of the footstep (1), which was not far
seconds for our particular robot in both cases. This short time enoughforwardtoreversethemotion.Thisisafairlycommon
constant is indicative of the highly dynamic motion we are problem, especially at low speeds when the footsteps are very
attempting,andischosentobeasshortaspossiblesincefaster close together.
stepping allows disturbances to be counteracted sooner.
Since an attempt to cross feet laterally could result in a VI. EXPERIMENTALRESULTSANDASSESSMENT
leg-leg collision, we slightly modify the procedure in order to We present here experimental results supporting the ideas
reducethelikelihoodofsuchanevent.They phasespaceand proposed in previous sections. A planarizing linkage system,
the y component of the footstep location is always computed shown in Fig. 1, constrains motion to the sagittal plane in
before that of x, so that the y phase space is given the all experiments. In the first experiment, Hume stands on
opportunity to modify the time of swing to be used by the two wedges inclined inwards as shown in Fig. 6(a). The
x phase space process. This is accomplished in the process first experiment shows balancing on a split terrain robustly
of computing the switching state in the y phase plane. If handling human interactions. It demonstrates a successful
the y velocity hits a maximum |y˙| limit of 0.65 m/s during implementation of WBOSC in a biped robot with elastic
the integration, then the entire step is shortened such that actuators,andvalidatestheperformanceofinternalforcecon-
the switching point occurs where the integration reaches the trol. In the second experiment, Hume implements a stepping
maximum y speed. If |y˙| at the default switching time is less task with the planarizer’s slider locked in place, allowing
IEEEJOURNALONXX,VOL.XX,NO.YY,JANUARY14,2015 10
Fig.6. EllipticalTrajectoryTrackingandHumanDisturbanceRejectiononHighPitchSplitTerrain:Subfigure(a)showsHumestandingbetweentwo
inclinedwoodenpanelsandtrackingapositiontaskwithitscenterofmass.Thispositionset-pointfollowsaconstantvelocitytrajectoryalonganelliptical
pathshown,alongwiththemeasuredCOMpath,insubfigure(b).InSubfigure(c)theCOMhasadifferentimpedanceinthehorizontalandverticaldirections.
Whentherobotispushedbackwardsitmovesasthoughthecenterofmasswereconnectedtoalow-spring-constantspring,whereaswhentherobotispushed
downwardsitreactsasthoughconnectedtoitssetpointbyafarstifferspring.Duetothefeedbackregulationofinternalforces,thebipeddoesnotfalldown
whendisturbedwithlargeexternalforces.
only vertical motion, approximately, and pitching of the body.
PositionGain
We use the stepping test to validate the contact transition
techniqueintroducedinSec.III-F.Thesteppingtestalsotests Kx Ix Dx
the DSP-level gain scheduling strategy we used. In the final COMx 15.0 0.0 0.0
experiment, we show undirected dynamic walking with the COMz 200.0 30.0 3.0
proposed continuous re-planning method. As a final comment qRy 150.0 15.0 7.0
beforedescribingfurtherdetails,wenotethatinallcontrollers
qRx 250.0 15.0 1.0
used in the experiments we omit the implementation of
Coriolis/centrifugal terms represented by the symbols µ∗ TorqueandInternalForceGain
task
in Equation (35) and µi in Equation (12). KP,τ KI,tau KF
50 0 1.0
A. Balance in a High Pitch Split Terrain
TABLEI
Inthisexperiment,theHumebipedbalancesonahighpitch GainSetfortheInternalForceTest.
terrain composed of two 45◦ wedges angled in towards the
robottocreateaconvexfloorprofile.Asfarasweknow,thisis
thefirsttimeabipedrobothasbeenreportedunderconditions
that strictly require internal force to remain standing. The
robot’s tasks were to maintain a 100N internal force pushing
outwards against the two contact points, a desired impedance
task for the center of mass point, and a desired impedance
taskforthebodyorientation.Bycontrollingthisinternalforce,
Hume was able to avoid slipping while accurately adjusting
its COM position. This experiment is divided into two sub-
experiments: Hume was made to follow a time-varying COM
trajectory which followed an elliptical path in the sagittal
plane, as shown in Fig. 6(b); and Hume was made to hold a
CartesianimpedancetaskontheCOMwhichhadlowstiffness
horizontallyandhighstiffnessverticallyasshowninFig.6(c).
In the second sub-experiment the robot was perturbed by
external disturbances in the form of human pushes. In this
test, xd = [COM , COM , q , q ]T, Fd = [0]
task x z Ry Rx task 4×1
and Fd = 100N, using the notation of Fig. 2. Gains are
int Fig. 7. Deficient Internal Force Control: This figure demonstrates how
summarized in Table I.
Hume falls when the internal force is insufficient to maintain static friction
Due to this feedback strategy, the errors between desired at both feet. In this case the internal force was only 10 N which was not
(black) and actual (red) internal forces are small enough to sufficienttoovercometheeffectofgravitationalforcesonthesurfaces.
achieve a stable pose control. COM tracking performance is