Path-Dependent Gaze Control for Obstacle Avoidancein Vision Guided Humanoid Walking

Javier F. Seara, Klaus H. Strobl, and Gunther Schmidt

Institute of Automatic Control EngineeringTechnische Universitat Munchen, 80290 Munich, Germany

Javier.Fernandez.Seara@ei.tum.de

Abstract— This article presents a novel gaze control stra-tegy for obstacle avoidance in the context of vision guidedhumanoid walking. The generic strategy is based on themaximization of the predicted visual information. For in-formation/uncertainty managementa new hybrid formulationof an Extended Kalman Filteris employed. The performanceresulting from this view direction control scheme shows thedependence of the intelligent gazing behavior on the pre-planned local path.

I. INTRODUCTION

The achievement of the high level of coordinationrequired between vision and locomotion has become amajor topic in current research into humanoid robotics.Most of the groups working in this field, seem to haveaccepted several antrophomorphic characteristics. Theseinclude biped structures and the presence of some kindof active perception system, for example based on stereovision [1], [2]. Starting with these premises and withthe goal of increasing the degree of independence, orrather autonomy, of the robot, some intelligent meansof controlling perception become necessary in order toguarantee that the humanoid is provided with the highestpossible amount of task-dependent relevant information.

This article is organized as follows: In SectionII theproblem of gaze control in vision guided walking isdefined, and the requirements on the gaze controller arestated. SectionIII gives an overview of the informationmanagement mechanism used. This represents the startingpoint for the predictive view direction control schemepresented in SectionIV. Section V demonstrates withsimulations the applicability of this approach. The articlepresents in SectionVI an outlook on possible improve-ments that can be obtained by the proposed strategy.

II. PROBLEM DEFINITION

For a vision guided walking machine to execute agiven locomotion task, the guidance system must pro-vide appropriate step sequences which should allow therobot to navigate and reach the goal position taking intoaccount the environmental situation. In case of a visionguided humanoid, the information about the environmentis provided by an active vision system. The acquisition and

xC

zCSC

xF

z

y

S

F

F

F

Cy

RR

R

R

Fig. 1 Biped robot with camera reference frameSC and foot reference

frameSFR

management of this information can be accomplished by agaze control scheme selecting the orientation of the sensor,such that the relevant objects of the scene are kept withinthe sensor’s limited field of view. This is also denoted asthe “where to look next?”task.

In our research we consider aninformation managementscheme, which registrates and administers the uncertain-ties associated with the different processes involved inthe perception based locomotion [3], [4]. This descriptionof the accumulated information together with the uncer-tainties in the measurements can provide a basis for thetask-oriented gathering of the relevant information. Sinceuncertainty must be redefined in the context of bipedwalking, we will refer to it with the notionIncertitudeν. We defineIncertitudeas a measure of the uncertaintyrelated to the total amount of reliable knowledge requiredfor the successful fulfillment of a given task. This canbe interpreted as a task-dependent mapping of the variousuncertainties. We define theInformation Contentof a viewsituation as a measure of the degree to which perceptionunder these terms is capable of reducingIncertitude.

III. COUPLED HYBRIDEKF FORINFORMATIONMANAGEMENT

In [3] we presented aninformation managementschemefor active gaze control in the context of vision guidedhumanoid walking. The proposed biologically inspired

first approach to gaze control was developed for obstacleavoidance tasks and is based on the maximization of short-range predicted visual information. The quantification ofthe information required a stochastic model of both therobot motion and the perception system. In this work,a visual perception model for a stereo-camera pair wasconsidered. Its capability of measuring distances is used,and both the reconstruction formulation and the errorrepresentation have been discussed in detail in [4].

xFL

yFL

xs

ys

xFR

yFR

SFL

s

SFR

s+1

xs

ys

s

s

Estimated Foot Pose

Real Foot Pose

Fig. 2 Step errors of a biped in dead-reckoning.

The biped robot model considered was a simplifiedgeneral model inspired by real walking machines [5]. Therobot model comprises kinematic transformations betweendifferent reference coordinate frames.CT FR,L

describesthe transformation between the camera reference frameSC and the current foot frameSFR,L

, cf. Fig. 1. Thisframe changes at the beginning of each step, e.g.Ss

FL

to Ss+1FR

in Fig. 2, and remains attached to the currentlystanding foot for the step duration. This reflects thediscrete character of the biped locomotion and leads toa modeling of the walking process taking into account thedead-reckoning step errors, see Fig.2. Both, the relativeposition of the next foot and its orientation are considereduncertain with errors∆xs,∆ys, and∆θs.

The information/uncertainty management, i.e. relation-ship between system state estimation and the activemeasurements, employed acoupled (considering cross-covariances)hybrid (reflecting the discontinuous charac-ter of biped walking)Extended(copes with nonlinearsystems)Kalman Filter. With the process modeling forwalking explained earlier, thestate transition functionf for the new filter formulation can be represented asfollows:

xk+1 = f (xk,uk+1,wk) == xk (1− γk+1) + fs (xk,wk) γk+1 , (1)

where γk+1 is a binary variable representing the currentcontrol vector:γk+1 = 0 indicates no change in the coor-dinate frame, whereasγk+1 = 1 means an instantaneouschange in the coordinate frame. Consequently, the functionfs is defined as the transformation of thestate vectorxwhen the standing foot changes. On the other hand, anappropriate measurement equation must be chosen basedboth on the stereo-vision system properties and the objectfeatures detected by it, refer to [4] for details.

IV. GAZE CONTROL STRATEGY FOROBSTACLE AVOIDANCE

Walking in an environment with obstacles requires areliable, predictive classification and pose estimation of allobstacles in the walking trail. As discussed in SectionII ,an active vision system needs a view direction controlscheme to determine where to direct the sensor so that therelevant objects in the scene are within its field of view.A simple reactive controller neglecting the delay due tonon-zero image acquisition and processing times seemstherefore inappropriate for an active vision application,which requires not only a reactive component but alsoa purposive task-dependent active behavior. Predictionproves to be an appropriate method to improve perfor-mance of such a system.

A. Predictive Gaze Control Strategy

With the concepts ofInformation Contentand In-certitude defined earlier, we present a predictive path-dependent gaze control strategy for an active vision systembased on the maximization of the predicted visual informa-tion. The approach is as follows: Using the accumulatedavailable information about the scene (for example a3D-map, i.e. positionF xi of object i relative to thecurrent foot frameS s

F and its covariance matrixCij),the current observer location0x0 – which forms togetherwith F xi the state vectorx, present motion parameters,and the current task (which determines the relationshipνi = f(x,Cij , task)), the gaze controller predicts the(quasi-)optimal pose of the visual sensor for a futureview situation Ω? (predicted variables are denoted by and optimal ones by index?). The optimal pose, i.e.position and orientation, but commonly and without lossof generality only orientation (view direction), is foundby selecting the state of maximumpredicted InformationContent, ICmax, among a set of predicted states. This canbe expressed by:

Ω? = arg maxΩ

N∑i=1

ICi(Ω, 0x0, F xi,Cij , νi), (2)

subject to Ωmin ≤ Ω ≤ Ωmax and (3)

r(Ω) = 0 , (4)

where Eq. (3) are upper and lower bounds for the activevision system parameters and Eq. (4) guarantees a steadygaze behavior without abrupt view direction changes. Thelatter would disturb the perception process. Eq. (4) takesinto consideration the observer’s system dynamics andlimits the performance (e.g. speed) of saccades.

B. Incertitude Definition: Path-dependence

Next, a formulation ofIncertituderelated exclusively toan obstacle avoidance task is presented, see SectionII . Itis obvious that the existence of obstacles to be avoideddepends on which path the biped robot is to follow.

This fact must be taken into consideration by the gazecontroller. Therefore, a definition ofIncertituderelated toan obstacle avoidance must include the planned local path– assumed to be given – in order to optimize the resultinggaze behavior. The main task considered here is collisionavoidance with obstacles present in the environment. Thisimplies that the relevance of an obstacle for the gazecontrol depends, in this case, on the risk of collisionthat this object may represent during robot locomotion.In the following some concepts related to this aspect arediscussed in more detail.Critical Points for Collision

One of the main objectives of safe locomotion isavoiding collisions. Given a path through a scenario withdifferent objects, there exist several points on this path inwhich the danger of colliding with some obstacle is greaterthan in others. These points are namedcritical points forcollision and are defined here as the points on the pathwhere the distancedi between the standing footSF andthe Obj i has a local minimum – taking into account thediscrete character of the walking process. These points andtheir critical distances– when the robot passes by for thej-th time at the instantcij , cf. Fig.3 – can be characterizedby the positionF x

cij

i of the objecti relative to the footframeS

cij

F and its covariance matrixC cij

ii .The existence and location of thesecritical pointsmust

be estimated in time, i.e. before the humanoid arrivesthere, in order to adapt its behavior for coping with them.The estimation of both the positionF x

cij

i and the positioncovariance matrixC

cij

ii of the objecti at the critical instantcij rests on the propagation of the system state; see theleft-hand column in Fig.4. Estimation can be achieved bylinearizing of the system equations, Eq. (1). Informationinputs for these estimates are past measurements (in steps and previous).IncertitudeDefinition for Obstacle Avoidance

Based on the presented aspects, a definition of obstacleavoidanceIncertitudeis proposed corresponding with theconcept ofProbability of Collision. This Incertitude isnow ν s

ij = Pcij

i , i.e. the predicted probability of collisionwith the objecti at cij , wherecij represents a descriptorfor a critical point. This gives an idea of thesecuritybypassing the objecti the j-th time. ν s|s

ij should reflect theuncertainty in the position of the objecti at the instantcij

from F xcij |si andC

cij |sii – i.e. for the obstacle avoidance

tasks we haveν s|sij = f

(F x

cij |si , C

cij |sii

). At cij , the

critical parameters are the predicted object distancedi

and its corresponding uncertainty represented byCdi–

which can be easily obtained fromCcij |sii . With both

the predicteddistancedi and the statistical deviationσdi,

the probabilityPcij

i of collision with the object can beestimated as follows:

Pi =∫ 0

−∞

1σdi

√2π

e

−(x−di)2

2σ2di dx (5)

Pi can be calculated either numerically or by meansof tables. But in this case, being only interested in thereduction of these values, and not in their evaluation,it may be replaced byPi ∼ σdi/di, thus simplifyingsubstantially the calculation efforts.

Obj iSF

s SFs+1

d iSF

cij

Field of view Gaussiandistribution

Fig. 3 Definition of the Probability of collisionPi.

Gaze Control Specific Task Definition

Given such a situation described by a pre-plannedlocal path, severalcritical points for collision, the presentknowledge about the environment and the characteristicsof the walking machine, the task of the gaze controlcan be defined. The goal consists in minimizing the riskof collision, i.e. the obstacle avoidanceIncertitudesνij

in order to gather enough information either for passingsafely by the object or planning alternative paths aroundit. This definition holds also in the case of complicatedpaths, since multiple critical points may appear for eachobject.

C. Prediction Algorithm and Gaze Optimization

The objective is then, as explained in SectionIV-A , todefine the optimal camera orientationΩ? =

pan, tilt

that maximizes the predicted acquiredIC in the nextmeasurement process ins+1 for the obstacle avoidancetask. This can be expressed as a maximum reduction ofthe predicted Incertitudes, i.e.

Ωs+1

? = arg maxΩ

Nobj∑i=1

Nci∑j=1

ICs+1

ij (Ω, x s, C s, νs+1|sij ) =

= arg maxΩ

Nobj∑i=1

Nci∑j=1

(ν

s+1|sij − ν

s+1|s+1ij

)=

= arg minΩ

Nobj∑i=1

Nci∑j=1

νs+1|s+1ij (6)

where Nobj is the number of objects present andNci

the estimated number ofcritical points per object.ICs+1

ij

is the predicted acquiredIC about the objects’ rele-vance in thecritical points cij , and equals the differenceν

s+1|sij − ν

s+1|s+1ij . The first term represents the predicted

Probability of Collisionwith the present knowledge, seeleft column in Fig. 4. The second, this probability if ameasurement is taken under the conditionsΩ in the next

steps+1, as indicated in the right column in Fig.4. Sinceall the Incertitudesνij represent the same – predictedprobabilities of collision, these can be compared directly.All the reductions have the same significance, althoughsome will be reduced more than others, as expected.This is one of the most favorable characteristics of theproposed definition of obstacle avoidanceIncertitude incomparison with [3]. Thus, one can assert that the strategy

SFs

SFs+1

SFs+2

SF

c

SFs+1

SFs+2

SFc

Current state s

With measurementin next step s+1

d d i

ij ij

Obj i

Criticalcollision

point

i

x , CiFs

iis

Fig. 4 Definition of obstacle avoidanceIncertitutewith (right) and

without (left) measurement in the next step.

is to behave in such a way that the uncertainty in theobjects position at the key instants of time –criticalpoints – is minimized, thus doing at each point in timethe best that the accumulated information allows. Futuremeasurements are obviously not taken into account owingto the enormous increase in the alternatives this wouldimply.

D. Practical Considerations

Next, we shall consider some factors involved in theway the information gathering effects the gaze behavior re-

sulting from the strategy presented. The specific influenceof each of these factors cannot be treated independently,but as an interaction among them. This can be resumedin the influence of four parameters which determine howthe IC varies related to the measurement process.

The first factor is the amount of uncertainty introducedto the state estimation in the walking process until therobot reaches the critical point. This depends on howimportant the dead-reckoning errors are, cf. Fig.2. Inthe case where this uncertainty is considered constant –which implies a uniformity in the stepping process – it canbe expressed as the number of steps (cij − s) left untilarriving at objecti. If a normal step length is assumedfor the prediction process, then this measure representsthe current relative distance between object and robot. Alarge distance implies that the information from measuringthis object now will be deteriorated to a greater extent tillthis critical point is actually reached.

Another factor is the predicted critical distance to theobject when passing it bydi. It is determined by the pre-planned local path to be followed. Large distances meanlow Probability of Collision, see Fig.3. The precision inthe measurement process [4], and the number of measure-ments per step are also factors influencing the informationgathering strategy. The higher the precision or the greaternumber of measurements, the higher the gathered amountof information and, therefore, the possible reduction inthe uncertainties. The information accumulation increaseswith high quality perception, meaning that the interest ofthe object in the next step will be lower.

The last factor is the present amount of informationaccumulated about object position, represented inC s

ii.A large present uncertainty implies a high reduction ifa measurement takes place, i.e. perception under theseconditions is worthy. On the one hand, obviously, thelarger is the present uncertainty, the larger the presentIncertitude ν

s+1|sij . On the other hand, after a precise

measurement ats + 1, the Incertitude νs+1|s+1ij will be

similar regardless of past information. Thus, the difference(ν s+1|s

ij −νs+1|s+1ij ) will be greater if the present uncertainty

is also high.

V. IMPLEMENTATION AND SIMULATION

To test the validity of the proposed approach, simulationexperiments were carried out in which a given local pathwith stochastic deviations in dead-reckoning, was gener-ated for the robot by driving its motion model with Gaus-sian noises: This simulates a polygonal path composedof straight line segments and the robot’s noisy motionover it. The goal of the gaze control is to gather as muchpath-dependent information about the objects’ position aspossible. The effects of this goal-oriented perception andthe functioning of theinformation managementschemeare described next.

0 1 2 3 4 5 6

-1

-0.5

0

0.5

1

1.5

2

x / m

y / m

Goal region

Start region

Obj 1

Obj 3

Obj 2

Obj 4 Obj 5

Real path Estimated path

Planned path

View directionStep #6

#6

#7

#10

#13

#44

#25

Real foot position Estimated foot position

Field of viewStep #6

Fig. 5 Simulated scenario with various obstacles, walking motion, measurement process, and intelligent gaze behavior.

A. Simulation Layout

The robot is commanded to perform position-basednavigation along a given path (Planned pathfrom Startregion to Goal region, see Fig.5) as described above,specified by a sequence of step parameters. The robot isdriven in a U-shaped scenario. The desired local path hasan approximately open rectangular form with side-lengths6×3 m in the vicinity of an irregular distribution of featurepoints – representing obstacles – of which it is able tomake simulated noisy stereo visual measurements. Someof these obstacles, seeObj 3, are deliberately located onthe given planned local path and should be avoided – formore details on this topic see [5].

In this simulation, we assume a rather simple walkingmotion resulting in a continuous constant head movementin a finite 3D-space with objects (Obj 1, 2, 3, 4, and 5,marked by squares), cf. Fig.5. The robot starts walkingstraight ahead inx-direction with constant velocity, andperforms steps translating its foot frameSFR,L

accordingto [xS ± yS 0 ]T where “+” is used to change fromthe right to the left foot, and “-” viceversa. Assumedvalues arexS = 0.3 m and yS = 0.2 m. The parameterθS describing discrete changes in orientation is variedafter some steps in order to follow the planned local path.However, robot locomotion causes errors performing stepsin both, next foot position and orientation, cf. Fig.2. Inthis simulation three sources of white Gaussian noise havebeen considered:σ∆xS

=0.005 m, σ∆yS=0.005 m, and

σ∆θS=3. Errors in the kinematic chain modeling have

not been taken into account. The active vision system takesan image of the scenario every 0.02m as the robot moves.After a sequence of measurements (14 in this case, markedwith crosses), a step is performed, i.e. the foot frameSFR,L

changes to the other foot. Assuming the measuringprocess takes place in camera frame-rate, this correspondswith a walking velocity of0.6 m/s, acceptable for currentbipeds. For this measurement process a Gaussian non-homogeneous (foveated) pixel noise with a standard devi-ation depending on the projecting point has been assumed[6]. The active perception system has only two rotationaldegrees of freedom:pan and tilt , i.e. Ω =

pan, tilt

.

The gaze control system makes a prediction once per stepand selects then the new view direction.B. Simulation Results

Fig. 5 shows the results of this simulation. Planned, real,and the estimated robot head motion and feet positionsin the world frame are depicted. The operation of theinformation managementscheme is shown by means of theestimated head motion and feet locations – by means ofthe90% confidence ellipses. The resulting gaze behavior isindicated in each step with arrows. ForStep#6, besidesthe arrow, the field of view on the ground is depictedin order to indicate the range of a measurement. Themeasurements ofObj 1 and Obj 3 corresponding to thisstep are highlighted. The position uncertainties ofObj 1and Obj 3 at this point are shown in detail in Fig.6by means of the90% confidence ellipsoids (their groundintersection). The latter are calculated14 times per step

taking into account the different measurements; they aredepicted with crosses. Note the clearly larger first ellip-soids – hard to see in the figures – they are consequenceof the errors in the step parameters defined above.

This simulation also demonstrates how the objects arefocused with respect to the presence ofcritical points forcollision. For example forObj 1, among others, steps#7and#13 are critical andStep#10 for Obj 2. Fig.5 showsthat immediately before reaching thesecritical points,these objects become more relevant; in further steps,this behavior can be also observed. At the last criticalpoint of the scenario, atStep#44, no more obstaclesare task-relevant, thus the view direction remains in itshome position. From this point on, the functioning ofthe information managementscheme can be observed. Noextra information is gathered by the perception systemanymore and therefore the quality of estimation of therobots position, through objects’ perception, is gettingworse. AtStep#25 another feature of the gaze controllercan be observed. Here the view direction remains in thehome position since the physical constraints of the activevision system cannot be violated, as stated in Eq. (3).

1.52 1.56 1.6

-1.42

-1.4

-1.38

-1.36

x / m0.5 0.52

-1.08

-1.07

-1.06

-1.05

x / m

y / m

#6 Obj 1 #6 Obj 3

MeasurementsObj 3

Fig. 6 Reduction of position uncertainty ofObj 1 andObj 3 in foot

reference frameSF at Step #6.

It is worth noting that the same obstacle can get a veryhigh relevance in two or more different situations, i.e.the robot does not “forget” an obstacle, in case it canbecome dangerous later on or can still contribute usefulinformation if observed from another perspective. Fixationon only one feature implies that robot angular deviationsare hardly estimated and consequently, a correction of theother obstacles position estimations can not be performed.

From a practical point of view some of the proceduresdescribed here might not be totally appropriate for imple-mentation on a real robot. Although the optimal behaviorwould be achieved applying the method presented in ageneric formulation, i.e. with an unlimited number ofcritical points and a large planning horizon, very similarbehavior has been achieved by limiting these parameters,with a ten-steps planning horizon in the presented sim-ulation. With a limited planning horizon, the behavior

obtained is fairly equivalent to the one presented withoutlimitation in the prediction and the computation timedecreases orderly.

VI. CONCLUSIONS AND FUTURE WORK

In this article we have presented an approach to gazecontrol integrating active information gathering and localpath following requirements for obstacle avoidance. Thisnovel, biologically inspired predictive gaze control strat-egy for active vision systems rests on two new concepts:Information Contentand Incertitude. A redefinition ofthe Incertitude in the context of vision guided humanoidobstacle avoidance during walking allows the inclusion ofthe planned local path-dependence. The general strategyis based on the maximization of the predicted visualinformation. For theinformation/uncertainty managementa coupled hybrid Extended Kalman Filteris employed.The performance resulting from this view direction controlscheme shows the dependence of the intelligent gazingbehavior on the planned local path.

In order to fulfill the requirements of autonomous loco-motion, these proposed conceptions for obstacle avoidancemust be combined with other strategies related to concur-rent navigational task, e.g. self-localization or exploration.Some work in this direction will be presented elsewhere.

Acknowledgments: This work was supported in partby the German Research Foundation (DFG) within the“Autonomous Walking” Priority Research Program.

VII. REFERENCES

[1] K. Nishiwaki, S. Kagami, J. J. Kuffner, K. Okada, Y. Ku-niyoshi, M. Inaba, and H. Inoue, “Online humanoid loco-motion control by using 3d vision information,” inProcs. ofthe I8th Int. Symp. on Exp. Robotics, Sant’Angelo d’Ischia,Italy, July 2002. 1

[2] K. Yokoi, F. Kanehiro, K. Kaneko, K. Fujiwara, S. Kajita,and H. Hirukawa, “Experimental study of biped locomotionof humanoid robot HRP-1s,” inProcs. of the I8th Int. Symp.on Exp. Robotics, Sant’Angelo d’Ischia, Italy, July 2002.1

[3] J. F. Seara, K. H. Strobl, and G. Schmidt, “Information man-agement for gaze control in vision guided biped walking,”in Procs. of the IEEE/RSJ Int. Conf. on Intelligent Robotsand Systems IROS, Lausanne, Switzerland, 2002, pp. 31–36.1, 4

[4] O. Lorch, J. F. Seara, K. H. Strobl, U. D. Hanebeck, andG. Schmidt, “Perception Errors in Vision Guided Walking:Analysis, Modeling, and Filtering,” inProcs. of the IEEE Int.Conf. on Robotics and Automation, Washington DC, USA,2002, pp. 2048–2053.1, 2, 4

[5] O. Lorch, A. Albert, J. Denk, M. Gerecke, R. Cupec, J. F.Seara, W. Gerth, and G. Schmidt, “Experiments in vision-guided biped walking,” inProcs. of the IEEE/RSJ Int.Conf. on Intelligent Robots and Systems IROS, Lausanne,Switzerland, 2002, pp. 2484–2490.2, 5

[6] L. Matthies and S. A. Shafer, “Error modeling in stereonavigation,” IEEE Journal of Robotics and Automation, vol.3, no. 3, June 1987.5