Simple Landmark Localization on a Three-Layer Mobile RobotArchitecture

Nathaniel Fairfield

April 30, 2001

Abstract

This paper describes a mobile robot system designed toexplore and map an area such as is encountered in urbansearch and rescue simulations. The robot uses homo-geneous artificial landmarks for localization as it buildsmetric and topological maps, determining landmark dis-tance and bearing with groundplane calculations from asingle camera and using Kalman filtering techniques toperform localization. A three-layer robot control archi-tecture is discussed which combines the features of a re-active navigation system and a symbolic planner as wellas a middle layer which consists of a set of information-gathering and motor schema. When implemented on aMagellan II mobile robot, the localization technique andthe three-layer architecture correctly localized the robotwhile exploring and mapping.

1 INTRODUCTION

Reactive navigation systems have been shown to behighly effective at quickly and reliably achieving short-term navigational goals [7]. The rapid feedback fromthe environment dilutes sensory noise [18]. Unfortu-nately, most reactive systems lack a “big picture” ofthe world and a systematic way to achieve the robot’sgoals (eg “Go to the supermarket and turn right to get tothe bookstore”). Reactive systems are too easily trappedinto repetitive behavior in a dead end because they lackmemory, or state [18]. On the other hand, state-basedrobots are very good at implementing plans because theyhave sets of rules or requirements which must be metin order to transition between states. But the problemwith state-based navigation systems is that they are of-ten clumsy and delayed in their responses to dynamicstimuli[12].

A robust robot system would therefore combine theshort-term navigational skills of a reactive system withthe long-term planning skills of a state-based system.For the sake of the robot designer, it would be ideal tobe able to use a simple, fast, and safe reactive navigationsystem and at the same time write large-scale state plansusing abstract symbolic information. In order for thisto take place, there must be some middle layer whichcan take the raw sensor information presented by therobot’s hardware and extract features and symbols fromit for the high-level state planner. An equally importantrole of the middle layer is to translate the state planner’sabstract goals into short-term navigational goals whichcan be carried out by the reactive navigator.

This paper discusses the various layers of informationand control flow in a general-purpose exploration andmapping robot. Briefly, these layers must accomplishthe tasks of Sensors, Robot Motion, Mapping, Local-ization, and Exploration. These tasks are conceptuallydistinct and hierarchical in that information flows fromSensors to Exploration and control flows the other way,but in actual implementation the lines between themmust be blurred. For a robot to autonomously exploreand map, all of these tasks must be closely integrated.

Also presented is a vision-based landmark localiza-tion scheme in which small green rings that can easilybe deployed from a mobile robot platform are droppedas the robot explores. By keeping a record of the drop-points for each ring, the robot can use later sightings ofthe rings to localize itself.

Section 2 discusses the challenges relevant to eachlayer mentioned above and talks about related workand standard approaches to these challenges. Section3 presents the solutions used in this paper and their im-plementation on a mobile robot. The results of severalexperiments are in Section 4, and a discussion of their

1

significance in Section 5.

2 BACKGROUND

2.1 Sensors

One of the great challenges facing roboticists is thepaucity of sensor information available on a mobilerobot platform. A standard research robot may have six-teen sonar range sensors arranged in a ring around thebody of the robot so that it can detect the distance toobstacles in sixteen different directions. Unfortunately,sonar range sensors are notoriously noisy, often return-ing both false positive readings and also false negatives.False negatives are especially common when the robotis near a very smooth flat plane, such as a metal door,which may act as a mirror for the sound pings. Po-laroid sonars emit a wide cone of sound (about 55 de-grees wide) and so have poor angular resolution. Theeffective range of sonars begins at about 50 cm and endsaround five meters. Sonars cannot measure shorter dis-tances because the same electronic element must act asa speaker (to emit the ping) and as a microphone to de-tect the returning echo and there is a certain amount oftime required for the element to stabilize after emittingthe ping before it can reliably detect the echo.

Because of this long minimum range, sonar sensorsare often complimented by a ring of infrared range de-tectors in order to fill in the ring of blind space nearthe robot. Infrared detectors measure the amount of in-frared light reflected by nearby objects. They work wellon most surfaces except mirrors, but must be calibrateddaily for good results. Infrared detectors are also highlysensitive to sunlight and incandescent lighting (both ofwhich are powerful sources of infrared light) and somust be used in enclosed spaces with stable lighting.The range of most infrared sensors is approximately fivecentimeters to half a meter. This makes them an excel-lent match for the sonar sensors, and together with a ringof bump sensors, which detect actually physical contactwith the robot’s body, the sensor arrays can provide con-tinuous range detection.

Although visual navigation can be achieved usingoptical flow [17], vision-based mobile robot naviga-tion usually focuses on the detection of landmarks, ei-ther natural or artificial [6]. There have been attemptsat Swarthmore College to use vision to gather moregeneric range information for mapping purposes by

sampling the floor color, visually determining the edgeswhere obstacles meet the floor, and calculating the dis-tance to those obstacles using ground-plane trigonome-try similar to that described below for the visual local-ization of landmarks. In general, however, landmark-based systems attempt to triangulate from several dif-ferent landmarks (usually three) in order to accuratelydetermine the robot’s location [17].

Knowing the relative strengths and weaknesses of thedifferent sensors, we can intelligently combine, or fuse,their information in order to get a more trustworthy re-sult. A simple technique, used by the evidence gridsdescribed below, collects many readings over time andthen combines the sensor information probabilisticallyand trusts that a coherent pattern will emerge [18].

2.2 Robot Architecture

The basic problems of any autonomous robot navigationsystem are path planning and obstacle avoidance. If thepath planning is done properly by upper levels of thenavigation systems, any path can be reduced to a seriesof goal points. The task of the lowest layer of a naviga-tion system is to get the robot from point A to point B,preferably by traveling in a straight line. On the otherhand, if there are obstacles on the way, the bottom layermust make some effort to get around the obstacle with-out endangering the safety of the robot. After a certainamount of time has passed in the attempt to surmountthe obstacle, the bottom layer could send an alert to theplanner, or the planner itself could notice the delay andchose an alternative route.

The way that the robot delegates and achieves thesetasks depends on the control architecture. The robot canbe governed by a state machine that watches for a partic-ular set of inputs and internal state conditions to becometrue before shifting the robot along to the next state andsending new control signals. This is the stereotypicalbehavior exhibited by manufacturing robots which op-erate under strict programmed sequences of movement.They tend to move in straight lines, then pause and ro-tate slowly before heading off in another straight line.They move slowly and deliberately, with a high degreeof accuracy. The three basic elements of this kind of ar-chitecture are Sense, Plan, and Act [12](see Figure 2).In the sensing stage, the robot gathers raw sensor dataand uses it to construct a world model. The planningstage takes this model and a pre-defined goal and gen-

2

erates a plan to achieve the goal. Finally, the executionstate takes the plan, which may be expressed in high-level or symbolic terms, and translates it into actions,such as robot motion. While this type of control archi-tecture is perfectly suited for industrial robots that workin constrained, engineered environments, its clearly nota functional approach for the less-than-ideal real world.

The opposite extreme to this Sense-Plan-Actparadigm is the “reactive” paradigm. One of firstreactive control architectures was described in 1986by Rodney Brooks [7]. Brooks’ subsumption archi-tecture combines a set of simple reactive modulesin a subsumption hierarchy (see Figure 1). Each ofthe modules responds in a simple way to a given setof inputs, and then there is an arbitration processbetween modules that determines which module or setof modules actually gets to determine the response ofthe robot. This makes the robot very reactive and thecumulative knowledge of all of the modules workingtogether means that the robot can display emergentbehavior in response to complex stimuli.

The problem with the subsumption architecture is thatall of the modules are very simple, and they are all al-ways activated. This makes designing complex behaviorvery difficult, since there is never any central decision-maker which can drastically modify the behavior of therobot in accordance with its current task [20]. Subsump-tion architectures also run into problems with noisy sen-sors because they do not have memory and hence cannotdo temporal analysis of sensor readings to compensatefor noisy or intermittent data [12].

Over the past decade, a hybrid architecture theoryhas begun to emerge. More specifically, several authorshave tried to theoretically justify the interesting findingthat almost all new mobile robot architectures use threedifferent layers or levels. The theory proposed by Gat[12] states that the tasks of a robot control system areclearly divided into three types. The part of the systemwhich needs to be reactive, the part which depends onthe past, and the part which tries to make predictionsabout the future. In addition, [8] proposes three coreconcepts which must be kept in mind while designinga robot architecture, namely modularity, hierarchicallyorganized action selection, and a parallel environmentmonitoring system.

Both the Sense-Plan-Act and Reactive approacheshave relative strengths and weaknesses. The absolute-trackers do make metrically accurate maps and often

Avoid Obstacle

Explore

Find Door

Left Motor −43Right Motor 54

Sensor data

Arbitrator

Figure 1: A reactive architecture

support complex path-planning strategies. The reactiverobots move smoothly and quickly and investigate allof the corners and nooks, but in general it is difficult toget them to exhibit complex behavior. Of course, mostnavigational approaches lie somewhere between thesetwo extremes and try to tap into the advantageous as-pects of each. In the case of the Saphira architecture[21], all of the outputs from the behaviors are fed intoa central controller which combines them into a singlecontrol signal, weighing each behaviors contribution byits “degree of applicability to the current situation.” Inthe case presented here, the robot uses a modified ver-sion of the subsumption architecture in which severalsimple modules (called schema or behaviors) observeinputs and then output recommendations for motor con-trol [1]. These schema reside in an intermediate level,and feed into the bottom or navigation level, which isultimately responsible for the motion of the robot.

2.3 Mapping

Although the sensor information available to the mo-bile robot may be noisy and inaccurate, the computercontroller can collect sensor data over long periods oftime and mine that data for reliable information. In or-der to avoid the necessity of storing all of the accumu-lated sensor data as raw readings, the robot can estimatehow much confidence it should have in a particular read-ing and then integrate the new information into a mapwith corresponding certainty. In most cases, the robotdoesn’t have much confidence in a single reading, butmultiple similar readings can accumulate. In general,the robot is trying to collect evidence about the occu-pancy of the space around it (i.e., is the space empty andtherefore passable by the robot, or is the space occupied

3

Turn left 90Go forward

Plan

Act Left Motor −50Right Motor 50

Sense

Sensor Data

Figure 2: A Sense-Plan-Act architecture.

and therefore impassable), so the space into which thesensor readings are entered is called an evidence grid.In a two-dimensional evidence grid space is divided upinto small squares, each of which contains the currentprobability of occupation for that particular square [18].This occupation evidence is gradually accumulated overtime using the available sensor array. All of the squaresare initialized with a value of 0.5 (which is representedas grey) and as the robot collects sensor information andenters it into the grid, empty squares become zero andoccupied squares become one [18] (see Figure 8, blackis zero occupancy, white is completely occupied).

Excellent evidence grids can be constructed usinghighly accurate (and highly expensive) laser range-finders. Often, however, the primary sensors used tocollect occupation evidence are Polaroid sonar range-finders. These sonars emit a broad cone of sound andmeasure the amount of time that passes until the ma-jor echo returns, as described above. While notoriouslynoisy (both in terms of the accuracy of their data and theracket they produce), sonars have the advantage of beingcheap. The evidence grid approach allows us to accumu-late many sensor readings over time so that eventuallysomething very close to reality emerges even from thenoisy sonar data.

The coarseness of sonar sensors makes it difficult topinpoint the location of a specific object but yields ex-cellent information about empty space. In a standard ev-idence grid implementation, a sonar ping increases theprobability of occupancy of grid cells in an arc at the

sensed range and decreases the probability of grid cellsbetween the sensor and the sensed range. Since sonarsmore reliably detect objects in the center of the soundcone, the grid cells along the axis of the cone are ad-justed more than the cells at the periphery [23].

A nice feature of evidence grids is that the geometryof the evidence grid corresponds directly to the metri-cal geometry of the world, providing a basis estimate ofthe robot’s location for the purposes of localization andnavigation [25]. A disadvantage of evidence grids is thatthey require relatively large amounts of space in mem-ory, are processor-intensive to update, and do not lendthemselves naturally to symbolic planning [11].

Topological maps represent the world by graphs:nodes connected by edges. Notable features of theworld, such as doorways or landmarks, make up thenodes of the graph and the edges are filled in as therobot determines that there is a direct path betweentwo nodes. The robot determines its position by com-paring its current sensor feature vector with the sen-sor vectors associated with each node on the graph andchoosing the closest match[16]. Topological maps canbe constructed from evidence grid representations us-ing Voronoi diagrams[25], or also learned directly fromodometric data, which is used to determine the relationsbetween connected nodes [24].

The problem with constructing these maps is that itrequires the integration of many different sensor read-ings over time, preferably readings that have been col-lected while the robot was moving around from placeto place. In order to be sure that the readings all lineup, the robot must keep very careful track of its currentlocation. This is the problem of localization.

2.4 Localization

Clearly there are several possible approaches to figuringout an agent’s current location. Humans, for the mostpart, rely on vision to determine exactly where they arein a room. Just close your eyes and try to walk outof the room and down the hall. For the first few stepsyou’re pretty sure of your location and the location ofthat pesky trash-can, but very quickly you become un-certain.

On the other hand, blind people handle this sort of3D tracking very well. This is because they have devel-oped their ability to judge distance traveled, as well astheir ability to remember a highly detailed three dimen-

4

sional map of the room and its contents. In robot terms,this is known as dead-reckoning or odometry-based lo-calization. By precisely recording the movements ofits wheels using wheel encoders, a robot can determinehow far it has moved (translated) and how much it hasturned (rotated). There are several common sources oferror inherent in these measurements, however. The firstis wheel-slippage. As the robot moves around and/orcontacts objects, its inevitable that the wheels will slipslightly. If the robot is going in a straight line at the time,this slippage is not too serious, but if the robot is mak-ing even a small turn then the small angular error willbecome a serious translation error as the robot movesaway. Second, the robot is only calculating a rough dis-crete approximation of a smooth curve. The amount oferror resulting from this approximation depends on thefrequency of the calculation and the resolution of thewheel encoders.

There have been a broad range of approaches to theproblem of how to maintain accurate position informa-tion about a mobile agent. The most obvious (and ex-pensive) include perfectionist techniques which try touse absolute coordinates generated by a differential GPSor some other high-accuracy tracking system to localizethe robot precisely at all times. Other approaches re-quire parallel or perpendicular walls with low amountsof clutter which can be easily recognized and orientedto [25]. Under ideal circumstances these systems workquite well, but the idea is to get out of the lab and intothe less-than-ideal world. Of course at the other endof the spectrum are the robots which don’t even con-struct maps at all but instead are reactive to the presentinput and make no attempt to remember past states. Butsince mapping is clearly a priority under most condi-tions, such systems will be passed over.

Many approaches depend ona priori knowledge ofthe environment, usually in the form of a hand-craftedCAD map [13]. One approach which uses either apre-programmed map or carefully generated long-termmaps (see [27]) for localization is to construct short-term sonar maps, or scan maps [13] which are gatheredquickly enough that there isn’t much odometry errorand so are metrically consistent. The short-term mapsare then transformed to best fit the existing long-termmap, and the inverse of the transformation is appliedto the robot’s localization odometry. This approach iscomputationally intensive and is difficult to adapt to adynamic environment, though good results have been

achieved (for example [11, 27]) . An advantage of thistype of mapping is that it does allow fairly continu-ous localization in that the robot’s odometry is correctedregularly and frequently, keeping individual correctionssmall [23].

A Markov model can be used to relate proximatenodes in the topological map so that the robot can dis-ambiguate two locations with near-identical feature vec-tors based on history [13, 24]. Markov localization im-plements the common-sense intuition that changes inrobot location, due either to robot motion or to local-ization corrections, must be fairly small and only to ar-eas roughly adjacent to the current position estimation.Especially in cases where localization is completely de-pendent on visual landmarks (and odometry informationis unavailable or ignored), the robot must have someway to decide whether a transition from one place to an-other is statistically likely [24]. By using Markov local-ization, robot systems can actually localize themselveswithin a known map without knowing their starting po-sition [13].

The only other sensor commonly available on the av-erage robot is a video camera. The camera is able tovisually locate landmarks relative to the robot’s posi-tion and use the landmarks’ stored positions to derivethe robot’s current location. Ideally, the robot should beable to pick out naturally occurring landmarks and usethem to correct its odometry. Unfortunately, it is verydifficult to get a robot to wisely choose its own land-marks, robustly recognize them from any angle, and ac-curately estimate their position. On the other hand, thereexist excellent real-time algorithms for detecting customartificial landmarks, such as the self-similar patterns de-scribed in [22]. Artificial landmarks can be constructedto be easy to identify and to yield extra coded data, suchas a unique id number. Such approaches have two draw-backs. First, the patterns used are easily occluded (ex-cept in the case where the landmarks are placed on theceiling, such as [3]), and second they are too large andcomplex to be deployed autonomously. In other words,their use is restricted to engineered environments wherehuman handlers can lay out landmarks for the robot.This is clearly a problem in any situation where humanplanners cannot be present to place the landmarks, suchas an urban search and rescue situation.

Other approaches to landmarks have assumed thatmultiple landmarks, usually three, can be observed si-multaneously and used to triangulate the robot’s posi-

5

tion in linear time [4]. This approach only requires thatthe bearing to the three landmarks be known and not anydistance information, though it does need a hand-mademetric map of the area with marked landmarks. Con-structing maps using a triangulation approach is a verycumbersome process (though it obviously works; car-tographers used to do everything that way!).

The landmarks proposed in [6] are recognized by areal-time algorithm and can be positioned at many dif-ferent orientations and angles. This is due to the self-similar nature of the patterns used, and is a substantialimprovement on other strategies, which have involvedmounting landmarks on the ceiling and using layers ofdi-chromatic disks. Non-real-time algorithms or specialhardware were necessary to process these landmarks.Regardless, this entire type of artificial landmark is com-plex to identify and too large to be deployed by a mobilerobot, though they do often encode additional informa-tion (such as the angle of the landmark or a unique idnumber) which can be very useful.

Crowley [10] uses binocular vision to calculate angleand distance to a landmark, or just angle, or just dis-tance, and uses Kalman techniques for localization. Thebinocular vision approach obviously has great promise,but in the interim, monocular vision is more commonand much less complex.

2.5 Exploration

The first priority for a robot in an urban search and res-cue operation is to physically explore as much as pos-sible. All other goals, such as victim identification orpackage delivery are interesting and important but sec-ondary. If the robot can completely explore an area, it islikely that it will be able to get information back out tohuman rescuers, who can analyze it far more effectively.Of course, all the exploration is useless if the robot can’tget out again and report its findings.

The dual priorities of urban search and rescue–exploration and reporting–work together to place sev-eral constraints on the methodology which the robotuses to explore. First of all, it is obvious that the robotmust have some sort of internal representation of thearea which it has explored. The robot can use this in-ternal map to aid its exploration algorithm. On the otherhand, its important that the internal representation be insome form which can easily be interpreted by humans(or at least convertible to such a form).

The simplest type of exploration is a random wander.As long as the wandering is truly random, the robot willeventually explore all of an area. Though naive, the ran-dom exploration technique can be quite effective [19].

The goal of frontier-based exploration is to gain newinformation about the world by moving to the bound-aries between mapped open space and unknown greyareas[27]. In the case of evidence grids, the frontiersare the places where open or unoccupied space is adja-cent to ambiguous space. As the robot approaches suchfrontiers, the new sensor information will either pushthe frontier back or reveal the presence of an obstacle.Frontier exploration has been implemented in past mo-bile robot projects, but for the results of this paper thesimpler wander technique was used.

3 IMPLEMENTATION

3.1 The Magellan II

The Real World Interfaces (RWI) Magellan II is a smallround robot with symmetrically opposed wheels whichallow it to rotate on its axis (see Figure 4). The basicsensor array consists of three rings of 16 bump (con-tact), sonar, and IR sensors mounted around the sides ofthe robot (see Figure 3). In addition the Magellan II usedin this paper has a Canon VC-C4 pan-tilt-zoom camera.The on-board computer is a Pentium III running RedHatLinux, which communicates with the robot’s rFlex con-troller over a 9600 baud serial line. We could connectto the robot over a wireless ethernet connection and for-ward XWindows sessions. This greatly facilitated de-bugging and run-time monitoring of the robot’s state.All code was written in C.

3.2 Mage

For the purposes of this research, RWI’s proprietarycontrol software was found to lack clear documenta-tion and to be unnecessarily complex and so a customlow-level software library was developed at SwarthmoreCollege. This library, called Mage, communicates di-rectly with the rFlex robot controller with over a serialline. The rFlex accepts a simple set of motor controlcommands and is also responsible for transmitting thesensor data of the robot back over the serial line. Mostof the serial communication protocol was extracted ordeduced from some example code that RWI provided

6

Figure 3: The Magellan II sensor arrays. Figure fromthe Magellan II Users Manual.

for updating the firmware on the rFlex. RWI also sentcode snippets demonstrating the serial interface to theIR sensors, which allowed the activation and reading ofthe IR range values. To control the Canon pan-tilt-zoomcamera, Canon provided documentation for their serialcontrol protocol so that the camera could be controlledover a separate serial line.

The library developed at Swarthmore College to in-terface with the Magellan’s rFlex controller is an alter-native to RWI’s Mobility, which is a C++ and CORBA-based interface (and in general a more sophisticated ap-proach). Mage was designed to be as simple as possible,with a small set of commands for setting motor controls,turning sensor arrays on or off, and other basic tasks.

For each of the sensor types, the rFlex can be set toreport on request or to report continuously. The rFlexcan also report other information upon request, such asthe current translation (distance traveled) and rotation(amount turned) in wheel encoder ticks (which can beconverted tox and y coordinates in millimeters, andθ orientation in thousandths of radian, see Figure 5).Under normal operation Mage set the sonar, IR, andbumper data to stream mode and then ran a separatethread which read new data from the serial line, re-quested new translation and rotation information, up-dated the odometry, and sent any new motor commands.A similar thread controlled the independent operation ofthe Canon PTZ camera.

In order to allow upper levels of code to have instantaccess to the robot’s current state, all sensor informa-tion was parsed into a state vector as it was receivedover the serial line from the rFlex. New readings sim-ply overwrote old information so that the state vector

Figure 4: The Magellan II with axially mounted camera.The robot is about half a meter wide.

always contained the closest approximation possible tothe current state of the robot. Mage provided simple yetcomplete functionality (it can boast more features thanMobility itself), and was an excellent base for the rest ofthe control architecture.1

3.3 Modular Architecture

During the summer of 2000, the robotics team atSwarthmore College developed a flexible modular ar-chitecture for the AAAI mobile robot competition [19].The core of the design was a large shared memory whichstored only symbolic information to be exchanged be-tween modules. The set of modules developed includeda navigation module, a vision module, a speech module,a voice-recognition module, a communications mod-ule (for inter-robot communication), and various debug-ging and monitoring modules. In addition, there was astate module which had the sole prerogative of sendingcommands to the other modules. Each module wouldmine its particular source of information and post time-stamped symbolic data to the shared memory. For in-

1Mage 0.1 is available athttp://www.sccs.swarthmore.edu/˜than/mage . It has been tested with the Magellan Pro andMagellan II, but it should work on any robot which uses the rFlexcontroller.

7

y

x

0

Figure 5: The robot’s current position is represented bythe tripletx, y andθ, which determine its displacementin thex andy axis, and its rotation angle.

stance, if the vision module saw one of the other robots,it would post the sighting to the shared memory alongwith a bearing estimate. When the communicationsmodule noticed that the other robot had been sighted, itwould send wireless ethernet packets to the other robotsnotifying them of the event. This modular architectureproved to be very flexible since each module could op-erate independently and also because each module hasfull access to the entire state of the robot.

The control architecture used during the summer of2000 was a simple two layer design. The bottom layer,called the navigation module, had at its core a simplereactive system much like the one described in this pa-per, but it also had a lot of specialized code for goal-points, tracking, and other specific behavior. The toplayer, which was part of the state module, would setflags in the shared memory to change the behavior ofthe navigation module. Unfortunately, this did not re-sult in a clear distinction between the tasks of the var-ious layers. The navigation module knew how to per-form complex behaviors like going to a goal-point, butat the same time it relied heavily on specific translationand rotation commands from the state module for gen-eral motion (though, as always, the reactive core keptthe robot away from most obstacles).

Conceptually, there are clear lines of specializationwhich can be drawn around the top and bottom lay-ers of a robot control architecture, though the middleis more muddled. In some sense, a purely reactive nav-igation system is a coherent whole, as is a purely sym-bolic planner. This was the approach taken in this paper.

The bottom layer was whittled down to the bare reactivefunctionality, and the top layer became the location forpurely symbolic decisions. All of the remaining func-tionality was placed in a new middle layer.

The robot has three different navigation control lev-els (implemented in the three layers described above)which can loosely be classified according to Gat [12]:the bottom level is composed of stateless sensor-basedalgorithms, the middle level contains behaviors that con-tain memory about the past, and the top level attemptsto make decisions, or “make predictions about the fu-ture” (see Figure 6). The first level is simply reactive,using the sonar and IR data (and in extreme cases thebumpers) to keep the robot reasonably free from danger.The top level is state-based and makes path-planning de-cisions and controls the general behavior of the robotbased on symbolic information extracted from the rawsensor data. The level in between, the middle level,plays a mediating role between the two other levels. Thetask domains of these three levels are quite clearly de-lineated, with the middle layer responsible for the inter-esting problems of transforming raw data into symbolsand symbols back into raw data. The bottom layer actslike a stateless reactive agent, the top acts like a clas-sical AI planner, and the middle layer somehow has totranslate between these two viewpoints and produce afunctioning system.

A short note is in order about another module in-cluded in the robot architecture; the vision module. Thismodule was also designed during the summer of 2000and was designed to address the problem of how to runmultiple recognition “processors” in a flexible fashion.Basically, the vision module has at its disposal a largenumber of “processors”, each of which recognizes aspecific feature or object in the image. In order to bal-ance the load on the robot’s processor, the vision moduleprobabilistically selects certain of these “processors” toexecute during each run cycle. The probabilities of each“processor” can be adjusted online by the state module,depending on what the robot is looking for.

3.4 Control Paradigm

The control paradigm is a combination of subsumptionand schema-based architectures. In all cases, the bot-tom layer in the hierarchy, for example the navigationmodule, has full and final discretion about what is ac-tually sent to the robot’s motors. The top layer controls

8

Wander

Motor Commands Sensor readings

World

Reactive Navigation System

Goto Goal Bored Map

Symbolic Deliberator Symbolic Information

Bottom Layer

Middle Layer

Top Layer

Mage

Smart Sensors

Figure 6: A diagram of the information flow between thevarious layers. Information flows upwards, and controlflows downwards.

the middle layer by specifying the priority of each ofthe various schemata. For example, if the state mod-ule wants the robot to sit still and look for landmarks,it simply gives the vision module’s landmark schema ahigh priority and turns off almost all of the middle mod-ule’s schemata, and then the middle module will cor-respondingly shut down the navigation module’s avoid-ance schema and bring the robot to a halt. This meansthat the state module never directly controls the specificparameters of robot’s motion, but instead allows the ac-tive schemata to reach an equilibrium which determinesthe global behavior. This approach is very similar to,and to a certain extent based on, Konolige’s Saphira ar-chitecture [15], and Arkin’s schema-based architecture[1].

3.5 The Bottom Level

The basic premise of the bottom level is that the worldis a scary, scary place that wants to hurt nice robots. Thebottom level is determined that nothing bad is going tohappen to this robot. Unfortunately, the bottom leveldoesn’t have the choice to sit in one place (as it wouldlike), but rather it has to try to obey the translation androtation biases handed down by the middle level. Tokeep from running into things, the bottom level uses theraw data from the sonar/IR sensor arrays. The three sen-

sors arrays of the Magellan II compliment each othernicely because the sonars have a max range of aroundthree meters and a minimum range of thirty centimeters,while the IRs have a maximum range of about thirtycentimeters and a minimum range of around five cen-timeters, which is about when the bump sensors are trig-gered. To navigate and avoid obstacles, the bottom layergroups the sensors into four more reliable meta-sensors(see Figure 7). Each of these meta-sensors exerts a nega-tive force on the robot as an obstacle approaches, push-ing the robot safely away or even causing it to reversedirection. This approach is very similar to the classicBraitenberg vehicle [5]. In testing conditions, the onlyserious challenges to this sensor scheme are chair legs,low flat obstacles (like books) and stairs. The chair legsare so narrow that none of the sensors can detect themreliably (except the bump sensors). Books, stairs, andother flat obstacles actually lay below the sensor planeand are also difficult to detect.

The bottom layer interacts directly with Mage, theinterface to the robot’s rFlex control hardware. Thebottom layer incorporates a very simple reactive objectavoidance schema, which is tuned to keep the robot fromrunning into anything too hard, regardless of the com-mands that the navigation module is receiving from theupper levels of the navigation hierarchy. The reactivesystem is the simplest design imaginable, although itproved very robust and flexible; to navigate through acrowd of people it used the same method that it usedin an urban search-and-rescue simulation (with slightlydifferent parameters). The robot decided how muchto translate (Tout) and rotate (Rout) based on translate(Tin) and rotate (Rin) biases fed in from another layer,the distances reported by four meta sensors, which areput intoDfront, Dbehind, Dright andDleft and on twosimple lines of code:

Tout = Tin − saDfront + sbDbehind

Rout = Rin − scDright + sdDleft

where thesa,b,c,d were scale factors which changedthe sensitivity of the robot to nearby objects. The mini-mum distances to front, back, left, and right, were foundby taking the minimum of the set of five sonars thatfaced in the appropriate direction. The robot resortedto this simple approach because individual sonar read-ings were too noisy to be informative but as a groupthey were fairly trustworthy, since reactions based on

9

Figure 7: The sensor clusters used in reactive naviga-tion: Dfront, Dbehind, Dright andDleft.

false-positives were quickly corrected (which is one ofthe advantages of a reactive navigation system). Giv-ing the Translate a slight a forward bias made the robotwander. To go to a goal point, the robot calculated theTranslation and Rotation bias required to push the robottowards the goal point. This approach proved to be veryrobust as long as the biases did not exceed the maximumrepulsion of obstacles.

3.6 The Middle Level

The middle layer is related to the subsumption architec-ture described by Brooks in [7], but is divided up intosmall sub-behaviors, called schema [2], which are prior-itized by a classical state machine to provide a smoothtransition between states within behavior-space.

The schema in the middle layer can be prioritized (oreven shut off) by the top layer or state machine, whichallows the top layer to drastically modify the overall be-havior of the robot without actually controlling any pa-rameters directly. The middle layer also processed theraw sensor data into a form which is usable by the sym-bolic logic of the top layer (see below for a more in-depth discussion of the robot architecture and the func-tions of each of the three layers).

This approach is loosely modeled on naturally occur-ring biological system. Taken with a large grain of salt,its possible to see how the bottom layer is essentiallya set of reflexes which keep the organism from colli-sion or other immediate disaster. The middle layer has

slightly more specialized schema which could be com-pared to the learned motor tasks of animals, such aswalking or picking up an object. The middle layer alsorefines information from the level of raw sensor data tothe more advanced sub-symbolic feature vectors (whichare in turn converted to symbols by the top layer). Usingthese symbols (such as ’door’), the top layer makes far-reaching decisions about the robot’s goals and modifiesthe behavior of the robot in such a way as to bring aboutthose goals.

The middle layer or middle module interprets highlevel commands from the top layer, passing them downto the bottom layer. A very important role of the middlelayer is its temporal behavioral tweaking. While the vi-sion module and the navigation module are responsiblefor mining features from the raw data of the camera andsonars/IRs respectively, the middle layer watches howthis data changes over time and modifies the robot’s be-havior accordingly. For example, if the top layer de-cided that the robot needed to go through a wall and thebottom layer balked, it was the task of the middle layerto resolve the situation by either forcing the bottom layerahead or by trying to get around the obstacle.

The middle layer implements several processes whichare run concurrently, very similar to Arkin’s schemadesign [2]. Each process contributes to the robot’soverall behavior, including robot motion and data-gathering. Adaptable data-gathering is an implementa-tion of schema which allows the same flexibility andcomplexity reflected in schema-based behavior. Al-though the vision module is not the focus of this paper, ittoo used a scheduler to allow the state module to specifyhigh-priority vision tasks which were run concurrently[19].

3.6.1 Data-gathering Schema

Landmark Schema The actual image-capturing andprocessing was done by a separate model, developed atSwarthmore during the summer of 2000 for the AAAImobile robot competition. The vision module was alsoschema-based in that there were a large set of varioustypes of detectors which could be prioritized dependingon the current task of the robot. Only the ring-detectorwas used during this project (see section 3.9.1 for anexplanation of how the images were analyzed for rings).All information gathered by these detectors was time-stamped and stored in the shared-memory space.

The landmark schema used this time-stamped vision

10

data to determine what sort of odometry correctionswould be necessary to remove any differences betweenthe perceived position of the rings and their known po-sition on the map.

Mapping Schema The mapping schema would peri-odically take all raw range sensor data, as it had been en-tered into the state vector by Mage, and enter it into theevidence grid. In order to avoid false readings, the sonarreadings over three meters or under thirty centimeterswere discarded. Likewise, the infrared sensor readingswere accepted only between thirty and five centimeters.Each pair of sonar and infrared sensors was combinedto provide a continuous range sensor, and if the readingwas not outside the range of accepted values the newdata was entered into the evidence grid (see below fordetails of the evidence grid implementation).

Path-laying Schema The robot constructed a topo-logical map dynamically as it explored. The approachcan basically be described as dragging a tight string be-hind the robot, and placing nodes at the points where thestring changes directions at a corner. This allowed therobot to plan paths very easily, since the path plannercan assume that there is a straight path between eachpair of connected nodes. The algorithmic method bywhich the path-laying schema constructed the topolog-ical map is discussed below. The path-laying schemaisn’t all that time-critical, so in general it was given alow priority.

3.6.2 Control Schema

Drop Landmark Schema This very simple schemadetermined when the robot should drop a landmark. Al-though there are many different complex criteria whichcould be used to decide when and where to drop a land-mark, the simple expedient of measuring the distanceto all the landmarks and dropping a landmark when theshortest distance was greater than three meters turnedout to be quite usable and sufficient for the testing con-ditions. Every time a landmark was dropped, the robotstored the location in a landmark list. These knownlandmark positions were then used by the LandmarkSchema (see above) to correct the robot’s odometry.

Wander Schema Wandering was accomplished bygiving a weak forward bias to the reactive layer. In the

process of obeying this bias, the robot wandered aroundthe room with basic Braitenberg vehicle behavior [5],avoiding obstacles.

Goalpoint Schema If the top layer wished to directthe robot to a goalpoint, it would simply tell the middlelayer which node of the topological map it wished to goto. The middle layer then needed to periodically calcu-late the biases necessary to push the reactive navigationsystem towards that goal point. Since the robot cannotgo sideways, it was fairly important that it be orientedtowards the goal point. For this reason, the rotationalbiases generated by the goalpoint schema were moder-ately strong. On the other hand, the translational biaseswere kept fairly weak to keep the robot from collidingwith something in its eagerness to push towards the goal.The goalpoint schema only needed to be run at a low fre-quency in order to direct the robot to the goal. Becauseall nodes in the topological map known to be connectedby straight paths (see section 3.8.2), the robot could af-ford to use a very simple goalpoint algorith.

Boredom schema With the reactive navigation func-tion used by the bottom layer, it was possible for therobot to reach a state of equilibrium in which the inter-nal driving force exerted by the wander schema or someother motor schema could be perfectly balanced by therepulsive biases caused by the sensors (corners in par-ticular are prone to this sort of situation). In order toget out of these situations, the middle layer containeda schema which monitored the amount of displacementover time and triggered simple escape biases if the robotappeared to be stuck in repetitive actions or stalled. Ifthe total translation over the past ten seconds fell be-low ten centimeters, the boredom schema would beginto exert strong turning biases, either to the left or to theright. Since the Magellan is round, this is a simple andeffective way to get out of a corner.

3.6.3 Schema Scheduling

All these schema were selectively run and prioritized bythe top level. For example, during the runs performedfor this paper, the top layer used the following code toconfigure the middle layer:

robot->mid.schema[SCH_PATH] = 1000;robot->mid.schema[SCH_WANDER] = 1000;

11

robot->mid.schema[SCH_STALLED] = 2000;robot->mid.schema[SCH_MAP] = 300;robot->mid.schema[SCH_LM] = 200;robot->mid.schema[SCH_DROPLM] = 2000;

Robot is a pointer to the shared memory structurewhich contains sub-structures for each of the majormodules (the bottom level was called nav, the middlelevel mid, the top level state, and the vision module vi-sion). In the shared memory associated with the mid-dle level, there is an array called schema which con-tains the execution frequency of each schema in mil-liseconds. Clearly, if all of the schema are set to run allthe time, the middle level will become swamped and be-gin to degrade the performance of other modules, suchas vision and state. In the code snippet given above,PATH, or path-finder schema, is set to run once per sec-ond. DROPLM, which is the schema which watches tosee when another landmark should be deployed, is notvery time critical and is only run every two seconds. TheLandmark schema, LM, is run most often to reduce theamount of time between a sighting of a landmark by theVision module, and the corresponding odometry correc-tion.

3.7 The Top Level

The top level is currently the simplest of the three, al-though once the two lower levels reach a satisfactorylevel of development, all further work will be concen-trated on the top level. That is, all of the nitty-grittywork of the lower levels is designed to make changes tothe top layer as easy and as intuitive as possible. Forexample, in order to begin a random wander with map-ping and localization, all the top level has to do is tellthe middle level to turn on the wander, mapping, andlocalization schema. The wander schema automaticallydirects the bottom level to wander around, the mappingschema collects data and constructs maps, and the local-ization schema manages the placement, recognition, andodometry corrections allowed by the visually localizedlandmarks. The top level uses Dijkstra’s algorithm onthe dynamically constructed topological map in order toplan paths and determine intermediate nodes, which arethen passed on to the middle level’s Goalpoint schema[9].

3.8 Mapping

3.8.1 Evidence Grid

To construct the evidence grid, the robot used a versionof the original code used by Moravec in [18] which hadbeen placed in a wrapper by Bruce Maxwell. This codeuses a simple model of each sonar ping (see Figure 9)in which the center of the sound cone, or wedge in the2-d plane, is weighted more heavily than the edges. Inother words, the center is more likely to cause an echoif something is there, so if there is no echo we are morecertain that there is nothing there. Unfortunately, calcu-lating this weighting gradient proved to be computation-ally expensive considering the large number of readingsbeing entered (remember that the Map schema ran overthree times a second and entered up to sixteen readingseach time). Due to the volume of readings, it was foundthat removing the gradient calculation did not adverselyaffect the quality of the maps while cutting the runningtime of the Map schema in half.

As each range sensor value is entered in to the evi-dence grid at the appropriate location and orientation, amap of the environment emerges (see Figure 8). A cellsize of five centimeters per pixel was used during the ex-periments, which allowed large maps (up to thirty me-ters square) to be constructed, while retaining sufficientaccuracy to allow the robot to perceive navigationallysignificant details.

One of the most effective measurements of the occu-pation of a cell in the evidence grid is the robot itself.Any space that the robot currently occupies is almostcertainly empty. For this reason, the current locationof the robot was entered into the grid as a robot-shapedblob of certainty. Since otherwise the robot was hesitantto commit on the complete emptiness of other areas forwhich it only had sonar data, this dark path of robot lo-cations helped to trace the route that the robot traversedduring a particular test.

3.8.2 Topological Maps

In our case, the landmarks were differentiated from thenodes of the topological map. The topological map wasused only for navigational purposes and contained onlyimplicit information about the world.

The robot started out at a given location in the ev-idence grid, say +200+200 in a 400x400 map and ac-cordingly placed a node in its topological graph (which

12

Figure 8: The evidence grid after only the first few sonarreadings have been entered. Note that the robot movedslightly.

was just a list of nodes). As it explored its environ-ment, the information collected from the sonar sensorswas entered into the evidence grid, gradually “darken-ing” empty areas and “whitening” occupied areas. Sev-eral times per second, as the robot explored, it updatedits topological map. In order to determine if there was adirect path in between its current location and the orig-inal node at timeTj , the robot traced a straight line onthe evidence grid from its current location to the origi-nal node, and observed the occupancy state of each pointalong that line. Thus, when the robot went around a cor-ner or any other obstruction, it could detect that therewas no longer a direct path. This portion of the algo-rithm runs in constant time to the number of nodes inthe topological map, since the robot only traces a line toone node.

If there was no direct path to the current node, therobot then searched for a new node in the graph withwhich to associate itself by attempting to trace linesfrom all nodes to its current location. If it succeeded,then it checked to see if the new node connected with theold node (see Figure 10). If not, or if the robot could findno node with which to associate itself, a new node wasinserted into the graph at the robot’s location atTj−1

(at which time there was a direct path to the old node)

Figure 9: A model showing how a sonar ping wouldaffect the entries in an evidence grid.

and tested for connectedness with all the other nodesin the graph using the same line-tracing technique. Af-ter being properly linked up within the graph, the newnode became the robot’s new base node, and it contin-ued mapping, always checking back to see if there was adirect line from it to its new base node. Both the searchfor a new node, and the connectivity test of a new nodetake order of n time, where n is the number of nodes inthe topological map.

By repeating this process as the robot explored theenvironment, the robot dynamically constructed a topo-logical map which was as connected as allowed by theinformation stored in the evidence grid. This comple-mentary approach allowed the quick construction of afunctionally useful topological map and its use for pathplanning while also maintaining the metrically usefulevidence grid. Of course, some possible connectionscould be left out because the algorithm did not updatethe connectedness of the entire topological map, whichwould be computationally expensive. On the whole, theapproach worked very well because it exploited the nat-ural path-finding behavior of the robot as it wanderedthrough an area.

13

Figure 10: A map of the Sproul Computer Science labshowing the topological nodes and the edges connectingthem (which have been enhanced for added visibility).Similar topological maps are also visible on the maps ofHicks Hall.

3.9 Localization

As the robot moves around the odometry steadily accu-mulates error. Over the course of a few minutes, therobot’s current dead-reckoning estimate may be com-pletely inaccurate (Figure 11). Without the use of a GPSor some other external localization scheme, the robotmust attempt to correct its odometry using features ofits environment. As discussed above, most visual lo-calization schemes either use groups of landmarks (usu-ally three) with known positions to uniquely localize therobot, or use complex landmarks which encode informa-tion such as unique landmark number and orientation.

3.9.1 Visual Identification

The approach described here uses the camera to visu-ally identify and locate small green plastic rings thathave been placed as landmarks. All of the landmarksare identical and contain no encoded information. Thebright shade of green for the rings was selected becauseit is a fairly unique color in most environments, although

0

200

400

600

800

1000

1200

1400

1600

1800

0 1 2 3 4 5

Mill

imet

ers

Minutes

Figure 11: Odometry error over the course of five min-utes. Notice that error doesn’t necessarily increasemonotonically; in an enclosed area odometry may driftback.

in the robot lab where some of the test runs were per-formed, there were certain ethernet cables with exactlythe same shade of green. In order to avoid false identifi-cations, the robot ignored a green object if its estimatedposition was more than a certain distance away fromthe expected position of a landmark. This naive tacticwas very effective at weeding out false positives, butupon occasions when the robot became seriously disori-ented could cause it to ignore real landmarks. The sizeof the rings made it feasible to design an automatic de-ployment mechanism, and in fact a simple prototype ofsuch a mechanism was constructed and tested, althoughit was not used in the final tests due to time constraints.The rings were deployed as the robot explored accord-ing to the Drop Landmark schema described above.

The first step in calculating the position of the ringrelative to the robot was to locate the ring in the image.Because of the unique color of the rings, the image wasthresholded for bright green and then a two-pass seg-mentation algorithm was used to find the centroids ofgreen blobs of made up of at least five pixels. A min-imum of five pixels was chosen because under certainlighting conditions specularities may create tiny spots ofgreen (or any other color). The minimum size thresholdalso automatically discarded images which contained norings at all. The segmentation algorithm returned thecoordinates(x, y) of the blobs in the image. Due to the

14

Figure 12: A robot’s eye view of several of the ringswhich were used as landmarks. During an experimentalrun, they are deployed about three meters apart.

deliberate spacing between landmarks (usually aroundthree meters), it was assumed that only one landmarkwould be visible at a time, so only one blob (the largest)was used for localization purposes. Because the hori-zontalφ and verticalχ fields of view of the camera andthe horizontalτ and verticalυ dimensions of the imagewere known (see Figure 14), the horizontal andα andverticalβ angles of the ring relative to the camera ori-entation could be calculated as follows:

α = x− τ

2× φ

τ2

β = y − υ

2× χ

υ2

Since the height of the cameraH and its angles of dec-lination δ and rotationρ and the robot’s global positionx, y and orientationθ are known (see Figure 13), the per-ceived distance to the ring relative to the robot’s currentposition is calculated as

distance = tan(β + δ)×H

ringY = x+ sin(α+ ρ+ θ)× distance

ringX = x+ cos(α+ ρ+ θ)× distance

Unfortunately, the position estimates yielded by thisprocess were only as accurate as the images capturedby the camera allowed, and in order to maintain a high

y

0

H

x

ρ

δ

Figure 13: A ring’s position is calculated relative to therobot’s position and orientation.

Image

φ

τ

υ

Projectionχ

α

β

Ring

y

x

Figure 14: The ring is projected onto the image plane ofthe camera.

frame-rate the images were captured at a fairly low res-olution (160x120 pixels). This meant that for distantrings, the difference of a few pixels in the image couldresult in the displacement of tens of centimeters of ourposition estimate. On the other hand, distant landmarkshad smaller centroids, so the angular estimation wasmuch more consistent, although not necessarily moreaccurate (see Figure 16).

After having calculated the perceived position of thering, the robot searched through the list of known ringpositions and found the stored location with the smallestEuclidean distance from the perceived ring location. Ifthe closest match was further than a threshold, in thiscase somewhat arbitrarily chosen to be a meter, the ring

15

identification was discarded as false.If a good match was found, the problem remained of

how the robot’s current position should be corrected totake this information into account. In effect there arethree pieces of relevant information: the known ring po-sition, the perceived distance to the ring, and the robot’scurrent location. Each of the three is subject to error.The known ring position may be inaccurate because ofodometry error before the ring was deployed. There islittle to be done about the error of the stored ring loca-tion. The perceived distance may be inaccurate becausethe camera has limited resolution and may have pickeda bad centroid to judge the distance to the landmark (re-member that at a distance of two meters, a difference ofa few pixels can result in an error of several centime-ters). Multiple observations of a ring can increase theoverall trustworthiness of the perceived ring location.Finally, the robot’s estimate of its current location maybe erroneous due to the odometry errors that accumulateover time (see Figure 11).

Since the overall error of our three pieces of locationinformation increases gradually (due to the odometry er-ror), it seemed reasonable to attempt to make the mini-mum correction possible. To find the minimum correc-tion, the slope of a line from the current robot position tothe known ring position was calculated. Then the pointwas found on the circle around the known ring positionwith a radius of the perceived distance to the ring that in-tersected this line. That point was the robot’s best guessfor its current location. These corrections were then ap-plied to the robot’sx, y andθ odometry (see Figure 15).

A problem discovered during test runs was that if allsightings of landmarks were used for localization sig-nificant drift occurred. This was because there wereoften tens of sightings with little or no movement onthe part of the robot, which effectively removes one ofthe sources of information discussed above: the odome-try. Because of slight miscalibrations and nonlinearities,these multiple sightings tended to cause the robot to driftaround the landmark in a circle with a radius of the dis-tance between the robot and the landmark. To reducethis effect, sightings of the same landmark were onlyused for localization after one second had elapsed sincethe last localization.

3.9.2 Kalman Filtering

The landmark position yielded by process describedabove needed to be used to correct the odometry of the

known ring location

new position and orientation

known distance

old position and orientation estimate

perceived ring location

Figure 15: When the robot sees a ring, it corrects itsposition information to take into account where it knowsthe ring to be.

robot. Unfortunately, the visual estimates of headingand distance, from which the positions of the landmarkwas calculated were far from accurate. Variations inlighting conditions, ring orientation (or robot orienta-tion relative to the ring), and the fluctuations commonin all frame-grabbers made ring identification tricky andfar from precise.

The simplest approach is to completely trust the vi-sual landmark estimation and to jump to the nearestpoint and orientation on the circle defined by the dis-tance to the landmark as determined visually (see Figure15). This approach is very simple and quick, but as thevisual readings are noisy, a certain amount of jumpingaround is inevitable.

A more sophisticated approach using a Kalman filtertakes into account the expected error in the visual esti-mate of landmark position. The Kalman filter also fac-tors in the information we already have from odometry,weighing it by the estimated accuracy of the encoderswhich decays over time. Because the robot only hasthese two sources of information and the model of therobot’s position is the same as the odometry, a very sim-plified version of the Kalman filter could be used (fora good explanation of the Kalman filter, see [26]). Inthe first implementation of the Kalman filter, the robot’sodometry was corrected by a fraction of the amount sug-gested by each positive landmark identification. Thisfraction is called the Kalman gain and was experimen-tally tuned to reflect the average error in the visual iden-

16

tification. Briefly,

K =P

P +R,

whereK is the Kalman gain,P the accumulated odom-etry error, andR the sensor error. The new positionxtis calculated from the old positionxt−1 by

xt = xt−1 +K(z),

wherez is the correction calculated from the landmark.2

Several variations of these parameters were used dur-ing testing. In the simple “jump” technique,P = 1 andK = 1. In the first implementation used during testing,P = 1 andK = a constant (usually around 0.3). In thesecond implementation,P = 1 andK is scaled depend-ing on the expected error of the visual landmark identi-fication as predicted by its distance (in other words,Kdecreased with the distance of the landmark).

In a more complex implementation more true to theprinciples of the Kalman filter,P would increase arith-metically over time in accordance with accumulatedodometry error (such a test was not run). Anotheruntested approach might use a combination of thesetechniques. Instead of setting the Kalman gain as a sin-gle function of the distance of the landmark, it could beseparated intox, y andθ components. The Kalman gainof thex, y component would decrease with distance, buttheθ component would increase with the distance of thelandmark. This would take advantage of the fact thatorientation estimates are better with further landmarks,but position estimates are better with close landmarks.

4 RESULTS

All of the tests were run in the hallways of Swarth-more College’s Hicks Hall (see Figure 17), except forthe topological map (see Figure 10) which was of the themain computer lab in Sproul when it was fairly emptyand the robot was allowed to explore that area. Whilethe halls of Hicks were fairly clear, the rooms in Sproulhad almost no clear walls, being surrounded with rowsof desks, chairs, tables, etc. Another very obvious char-acteristic of the maps in Hicks is that the walls weresomewhat specular and so the evidence grids tended to

2Crowley [10] describes a mathematical model used to estimateuncertainty in position and orientation, and also a good overview ofthe use of Kalman filters in robot navigation.

Figure 16: Amount of variation in the visual estimationof landmark position relative to the distance of the land-mark from from the camera.

be affected by false echos, which created artifacts be-hind walls.

Figure 18 shows a plan of the area mapped in Hicks.In the tests, the robot was started in the upper right cor-ner area and then made several passes along the corri-dor and into the other room. Note the three open doorsin the hallway. Depending on the time of day, differ-ent doors were alternatively open or closed–not all mapswere constructed with the same configuration of doors.

As a base case, several maps were constructed withno localization. The first, Figure 19, is a map of thearea created in one pass (i.e., the robot traveled all theway down the hall and then back again in one pass). Thenext map, Figure 20, is the result of three passes withoutlocalization. In addition to the overall darkening of themap due to more sonar readings (and false echos), theeffects of the accumulation of odometry error is clearlyobvious in the unaligned wall lines and gradual counter-clockwise shift.

The next set of maps (see Figures 21, 22, 23, and 24)were made with values ofKx,y,θ ranging from 0.1 to 0.5(all maps were made with three passes). These maps aremuch better than the maps with no localization, but theyclearly suffer from the spurious echos and false read-ings of the sonars. Regardless, for low values ofKx,y

andKθ the robot was correctly recognizing the land-marks and localizing itself, as is demonstrated by thefact the the walls of the hallway were super-imposed onsuccessive passes. However, asKθ grew past 0.2, therobot began to over-correct to the point where in Figure

17

Figure 17: This picture shows the robot in the hallway ofHicks Hall. The robot is heading down the long hallway,facing “west” in the maps.

24 withKx,y = 0.5 andKθ = 0.5, the robot becamecompletely lost and the map is as bad or worse than themaps constructed with no localization at all.

The last pair of maps show the results of varyingKx,y,θ with the distance of the landmark. In Figure25, the Kalman gain increased with distance and var-ied from 0.3 to 0.7, while in Figure 26, the Kalman gaindecreased with distance and varied from 0.4 to 0.1. Con-sidering that high-values ofKθ were seen to be poten-tially disorienting, the map in Figure 25 with increas-ing Kalman gain is remarkably good, and yet the de-creasing Kalman gain approach appears to yield a bettermap, perhaps due to its lower values overall. Both mapshave a noisy center hallway compared to the better of themaps constructed with static Kalman gains, for exampleFigure 21.

Figure 29 shows a graph of the magnitudes of the cor-rections made as each landmark identification (labeledon the x-axis) was used to correct the robots localiza-tion. In almost all cases the magnitude of the correc-tions decreased with multiple observations of the samelandmark. This shows that the robot was settling into anoptimal position for each landmark.

Figure 27 shows the relationship of the magnitude ofthe odometry correction and the amount of time sincethe last correction. Although it might be expected that

Figure 18: The floor plan for the part of Hicks Hallwhich was used during mapping tests, with the five land-mark locations labeled. The hallway is about ten meterslong.

Figure 19: A map withK = 0 (no localization) afterone pass (forth and back).

the correction would increase as the time interval in-creased (because odometry error accumulates over time)this graph does not show a correlation. Note that all ofthe data points along the left-hand side are correctionsmade immediately after the one-second wait period be-tween localizations had elapsed.

Figure 28 shows the correction to the robot’s headinglabeled with the first sighting of each landmark. It isinteresting to note that landmarks zero and four, whichare located in the small rooms off the hallway (see Fig-ure 18), correct the heading in an opposite direction thanthe landmarks in the hall (one through three). This il-lustrates the importance of making sure that the land-marks are not in a straight line. The fact that the sum of

18

Figure 20: A map withK = 0 (no localization) afterthree passes.

Figure 21: A map withKx,y = 0.3 andKθ = 0.1 afterthree passes.

the corrections is negative indicates that there is a fainttendency for the odometry to drift counter-clockwise,which can be seen in Figure 20, where no localizationwas used.

5 DISCUSSION

The discussion is divided into two parts, each of whichexamines an element of the robot system described inthis paper. The first section discusses the results of thelandmark localization scheme, and also takes a look atthe problems encountered with this approach and fur-ther work which would improve the performance of thesystem. The second section discusses the three-layer ar-chitecture in the same fashion.

Figure 22: A map withKx,y = 0.3 andKθ = 0.3 afterthree passes.

Figure 23: A map withKx,y = 0.5 andKθ = 0.3 afterthree passes.

5.1 Landmark Localization

The simple landmark technique yielded satisfactory re-sults and provided frequent small corrections to theodometry which allowed the robot to correct odome-try error and maintain correct localization. In turn, thisallowed the robot to construct accurate evidence gridmaps and topological navigation maps. The Kalmangain was found to be optimal somewhere in between 0.1and 0.3.

Identical landmarks were easy to deploy and identify,and because of the deliberate spatial distance it was verydifficult to misidentify a landmark. At the beginning ofthis project, the green rings were thought of as merely asa starting point for a more generic system, which woulduse naturally occurring landmarks. Many of the tech-niques developed for artificial landmarks could easilybe adapted for use with a system which picked out itsown landmarks from “interesting” visual features.

Regardless, the simple type of landmark used in this

19

Figure 24: A map withKx,y = 0.5 andKθ = 0.5 afterthree passes.

Figure 25: A map withKx,y andKθ increasing withthe distance of the landmark, after three passes. BothKalman gains varied equally from 0.3 with the nearestlandmark to 0.7 with the furthest.

experiment, which is small and simple to deploy is anexcellent intermediary between a system which useshighly engineered landmarks (like the self-similar pat-terns [22]) and a system which dynamically identifieslandmarks, a much more difficult proposition.

5.1.1 Problems

The landmarks we used are easily moved, either by thirdparty agents within the environment or even the robotitself. In the actual tests, the markers were taped downbecause the rooms were often in use by other people. Asimple improvement would be to use flat or disc-shapedlandmarks which would also present a more uniformsurface (and hence color) to the robot, increasing the ro-

Figure 26: A map withKx,y andKθ decreasing withthe distance of the landmark, after three passes. BothKalman gains varied equally from 0.4 with the nearestlandmark to 0.1 with the furthest.

bustness of the recognition algorithm and the accuracyof the position estimation and reducing the probabilitythat the landmark would be disturbed.

When a landmark was finally sighted after the robothad not sighted a landmark for a relatively long periodof time, the localization corrections were occasionallylarge because of the accumulated odometry error. Thiscaused abrupt skips and other artifacts in the evidencegrid map. Although landmark sightings were fairly fre-quent, the odometry error wasn’t corrected continuouslyand so any small errors that crept in between sightingswere be incorporated into the sensor readings as theywere entered into the evidence grid.

5.1.2 Further Work

It would have been good to methodically and rigorouslycalculate the best Kalman gain factors, as well as theproper ratio in relation to distance. The values used forthe odometry error were also largely guesses and couldalso have been refined. On the other hand, even withrough estimates of these values, the robot was able touse the landmarks to localize itself.

Originally, the landmarks were not designed to pro-vide localization for mapping. Rather, they were sup-posed to be used by the robot to find its way back out ofcomplex urban search and rescue situations. While thecurrent system obviously keeps track of which landmarkthe robot is currently looking at, it doesn’t have any wayto deal with getting lost. In other words the system can-not recover from large odometry error (the sort of thingthat is common enough in urban search and rescue situ-

20

Figure 27: This graph shows the magnitude of the lo-calization correction and the elapsed time since the lastcorrection. Note that all of the points along the left sideare corrections that occurred one second after a previouslocalization using the same landmark.

ations). A dual Markov / Kalman approach seems to bewell suited to the problem of recovering the robot’s po-sition from multiple sightings after a catastrophic odom-etry error.

5.2 Three-layer Architecture

The three layers worked very well together. The stateand navigation modules became much simpler and fo-cused when the intermediate functions were moved intothe middle module. The state module became purelysymbolic, and the navigation module purely reactive.Nevertheless, the state module was able to effectivelycontrol the robot’s behavior by tweaking the executionfrequencies of the middle module’s schema. In a sense,these schema were split into a set of functions whichextracted information from raw data and a set of func-tions which transformed abstract state commands intolow-level plans.

5.2.1 Problems

Under circumstances where a precise sequence of ac-tions need to be performed, a more direct method ofcontrol from the top level might be preferable to the un-

Figure 28: This graph shows a running total of the cor-rection to the robot’s heading (θ) labeled with the firstsightings of each landmark. This data comes from thedescending Kalman gain experiment (see Figure 26).

certain behavior resulting from several schema. This iscertainly not very difficult to implement (one could sim-ply design a schema with parameters which could becontrolled by the top level), but such back-doors pose aproblem to the methodology of schema-based behavior.

Currently the timing values for the various schema areeducated guesswork and not based on any theoretical orempirical optimization scheme. More tests would needto be run to determine the effects of variations in thesetimings, for example how often sonar data should be en-tered into the evidence grid by the mapping schema. Inthe case of the mapping schema, a drastic decrease infrequency would certainly impair its functionality whilea higher frequency would not necessarily yield better re-sults either, since it might just be entering the same datafrom the sonars twice. In general, however, the schemawere designed to be robust and functional within a broadfrequency range.

During tests, it was found that updating the evidencegrid on a regular basis (for example twice a second) con-sumed the greater part of the processing resources ofthe robot. In order to alleviate this situation, the evi-dence grid update function was moved into its own in-dependent thread, though it still conceptually resided inthe middle level and its frequency of execution was stillcontrolled by the parameters set by the state module.

21

Figure 29: This graph shows the magnitude of correc-tion in millimeters versus the landmark identified.

5.2.2 Further Work

In order to have a truly generic and versatile control sys-tem, several new schema would need to be developedto deal more intelligently with exploration and othermore specialized tasks. To make the schema schedulermore advanced, it should be able to specify not just themean run rate, but also the standard deviation in the rate,which could be used to ensure that time-critical schemawere promptly executed.

There are also interesting questions which could beinvestigated about the difference between the temporalinterleaving of schema, which is the primitive methodcurrently used, and the summation and weighing of mul-tiple simultaneous outputs. This question arises fromthe fact that the schema used in this paper are not as-sumed to be constantly executing; some are only runonce every few seconds. This is a contrast to the stan-dard schema model in which all currently active schemaare run at each time interval [2]. In the extreme casein which a schema only runs every few seconds, howshould its outputs be integrated into the global output?The most obvious possibility would be to decay schemaoutputs over time, as their relevance to the current situ-ation decreases.

6 CONCLUSIONS

The three-layer architecture used to implement the map-ping and navigation system divided the problem up into

independent subproblems which could then be workedon individually. Because of the focus of this paper, thebottom level and the top level were kept simple and con-ceptually clean while most of the work was concentratedon the data-gathering and transparent localization of themiddle layer. For a paper that used a similar architec-ture but focused more on the intricacies of navigation,see Jones [14].

In addition, the mobile robot implementation hasshown the feasibility of transparent localization by fus-ing information from sparse landmarks and odometricdata using a Kalman filter. Although most values of theKalman gain yielded an improvement over an odometry-only approach, a range from 0.1 to 0.3 was found to bebest for accurate mapping.

The system described here demonstrates successfulsolutions to many of the problems related to an UrbanSearch and Rescue sitation. It explores, maps, localizes,and constructs a topological map which can be used forquick and easy navigation.

Acknowledgments

This paper would not have been possible without the ad-vice of Bruce Maxwell, Gil Jones’ probing questions,and the excellent comments of Martin Krafft and An-drew Stout. Many thanks.

References

[1] R. Arkin and T. Balch. Aura: principles and prac-tice in review.Journal of Experimental and Theo-retical Artificial Intelligence, 1997.

[2] R. C. Arkin, K. Ward, T. Balch, T.R. Collins, A.M.Henshawand D.C. MacKenzie, E. Nitz, and D. Ro-driguez. Buzz: An instantiation of a schema-basedreactive robotic system. InProc. of the Int. Conf.on Intelligent Autononmous Systems, pages 418–427, 1993.

[3] C. Becker, J. Salas, K. Tokusei, and J. Latombe.Reliable navigation using landmarks. InProceed-ings of thee IEEE International Conference onRobotics and Automation, pages 401–406, June1995.

22

[4] M. Betke and K. Gurvits. Mobile robot local-ization using landmarks. InProceedings of theIEEE International Conference on Robotics andAutomation, volume 2, pages 135–142, 1994.

[5] V. Braitenberg.Vehicles: Experiments in SyntheticPsychology. MIT Press, Cambridge, MA, USA,1984.

[6] A. Briggs, D. Scharsctein, and S. Abbott. Reli-able mobile robot navigation from unreliable vi-sual cues. InFourth International Workshop on Al-gorithmic Foundations of Robatics. WAFR, 2000.

[7] R. A. Brooks. A robust layered control system fora mobile robot.IEEE Journal of Robotics and Au-tomation, RA-2(1):14–23, 1986.

[8] J. Bryson. Cross-paradigm analysis of autonomousagent architecture.J. of Experimental and Theoret-ical Artificial Intelligence, 12(2):165–190, 2000.

[9] T. Cormen, C. Leiserson, and R. Rivest.Introduc-tion to Algorithms. MIT Press, Cambridge, MA,USA, 1996.

[10] J. Crowley. Mathematical foundations of navi-gation an perception for an autonomous mobilerobot. In L. Dorst, editor, Reasoning with Uncer-tainty in Robotics. Springer Verlag, 1996.

[11] D. Fox, W. Burgard, and S. Thrun. Probabilisticmethods for mobile robot mapping. InProceed-ings of the IJCAI-99 Workshop on Adaptive SpatialRepresentations of Dynamic Environments. IJCAI,1999.

[12] E. Gat. On three-layer architectures. In D. Ko-rtenkamp, R. P. Bonnasso, and R. Murphy, ed-itors, Artificial Intelligence and Mobile Robots.MIT/AAAI Press, 1997. RR n3552 26 Laugier,Fraichard, Garnier, Paromtchik and Scheuer, 1997.

[13] J. Gutmann, W. Burgard, D. Fox, and K. Kono-lige. An experimental comparison of localiza-tion methods. InProc. of the IEEE/RSJ Interna-tional Conference on Intelligent Robots and Sys-tems (IROS’98), 1998.

[14] E. G. Jones. HYSTE: A HYbrid System forThorough Exploration. Unpublished undergradu-ate thesis, 2001.

[15] K. Konolige, K. Myers, E. Ruspini, and A. Saf-fiotti. The saphira architecture: a design for au-tonomy. ournal of Experimental and TheoreticalArtificial Intelligence, 9:215–235, 1997.

[16] B. Kuipers and Y. Byun. A robot exploration andmapping strategy based on a semantic hierarchyof spatial representations. Technical Report AI90-120, 1, 1990.

[17] C. Madsen and C. Andersen. Optimal landmarkselection for triangulation of robot position.J.Robotics and Autonomous Systems, 13(4):277–292, 1998.

[18] M. Martin and H. Moravec. Robot evidence grids.Technical Report CMU-RI-TR-96-06, RoboticsInstitute, Carnegie Mellon University, Pittsburgh,PA, March 1996.

[19] B. Maxwell, L. Meeden, N. S. Addo, P. Dickson,N. Fairfield, N. Johnson, E. G. Jones, S. Kim,P. Malla, M. Murphy, B. Rutter, and E. Silk.Reaper: A reflexive architecture for perceptiveagents.AI Magazine, 2001.

[20] J. K. Rosenblatt and D. Payton. A fine-grainedalternative to the subsumption architecture for mo-bile robot control. InProc IEEE/INNS Int’l JointConf on Neural Networks, page 65ff, 1989.

[21] A. Saffiotti, E. H. Ruspini, and K. Konolige.Blending reactivity and goal-directedness in afuzzy controller. InProc. FUZZ-IEEE ’93, IEEEComputer Press, pages 134–139, 1993.

[22] D. Scharstein and A. Briggs. Real-time recogni-tion of self-similar landmarks. Technical report,Middlebury College, 1999.

[23] A. Schultz, W. Adams, and B. Yamauchi. Integrat-ing exploration, localization, navigation and plan-ning with a common representation.AutonomousRobots, 6(3), June 2000.

[24] H. Shatkay and L. Kaelbling. Learning topologicalmaps with weak local odometric information. InIJCAI (2), pages 920–929, 1997.

[25] S. Thrun and A. Bucken. Integrating grid-basedand topological maps for mobile robot navigation.

23

In Proceedings of the Thirteenth National Con-ference on Artificial Intelligence. AAAI, August1996.

[26] G. Welch and G. Bishop. An introduction to thekalman filter. Technical Report TR 95-041, Uni-versity of North Carolina at Chapel Hill, ChapelHill, NC, 1999.

[27] B. Yamauchi, A. Schultz, and W. Adams. Mo-bile robot exploration and map building with con-tinuous localization. InProceedings of the 1998IEEE International Conference on Robotics andAutomation. IEEE, May 1998.

24