10
Local Path Planning along Bush Tracks using Vision David Fernandez and Raymond A Jarvis Intelligent Robotics Research Centre, Monash University, Clayton, VIC 3800, Australia [email protected] Abstract This paper investigates the problem of au- tonomous mobile robot navigation in bush en- vironments. Such environments form part of the workspace of a number of groups, such as rural search-and-rescue and bush fire-fighting crews, who could greatly benefit from helper robots. In particular, given the existence of many tracks through bush areas, the ability to autonomously drive along such tracks would help ensure robot safety and efficiency of travel, as a track by def- inition represents a passageway with a high like- lihood of being clear. The novelty of the work presented comes from the sole use of passive vi- sion to find obstacles and locate tracks, allowing a safe local path to be determined. 1 Introduction In bush environments, where often under-manned personnel including bush firefighters and search-and- rescue teams work and could greatly benefit from robotic assistance, the task of navigation is often very difficult. Because such environments are the workplace of a number of groups, they invariably contain access tracks – whether vehicular dirt roads or smaller walk- ing paths – that can and should be made use of when- ever possible, as, by definition, they represent a rel- atively clear passageway between locations that have been determined to be of interest. While cross-country traversal may at times be necessary, the probability of mission success will be greater if tracks are utilised. Additionally, many if not most of the larger paths and tracks in National and State Parks throughout Australia have already been mapped with these maps being available either commercially [Feathertop Map- ping Services, 2006] or through government agencies [Parks Victoria, 2007]. Accordingly, it is a relatively easy task to find a high-level path between two points of interest that makes use of such tracks. This leaves the problem of how a robot may be guided safely and efficiently along a given track, once it has been chosen. While a number of different sensor system could be used to perform autonomous or semi-autonomous navi- gation along such tracks, passive vision offers a number of benefits. Firstly, the equipment is cheap, perhaps the cheapest of the sensors most likely to be useful (with sonar and lasers being the two main competi- tors). Given the typically stretched resources the rele- vant organisations (for example the CFA and SES), low cost is important. Secondly, the natural world is visu- ally rich, meaning there is a wealth of visual informa- tion that has the potential to be exploited. Finally, the ability to make use of human perception and insight into the problem makes vision an attractive choice in such an unstructured and variable environment. The work presented in this paper aims to tackle a particular element of the wider navigation problem. Specifically we investigate the coupled problems of firstly assessing the traversability of the local terrain and secondly finding a short-term path, relative to the robot, that takes the robot along a track while avoid- ing obstacles. That is, the path determined must be efficient, by staying on the track when possible, and it must be safe, by avoiding any form of obstacle that it is likely to encounter. We choose to attempt this using passive vision alone, motivated by the reasons stated earlier. The solution presented uses stereo vi- sion to find obstacles, colour image processing to char- acterise and extract the track surface, and simple path- planning techniques to combine these two classes of in- formation and determine a safe path along the track. 2 Related Work The work presented in this paper formed part of the author’s doctoral dissertation [Fernandez, 2007] and extends and improves upon earlier work presented in [Fernandez and Price, 2005]. Accordingly, a thor- ough review of related works can be found in those publications. In summary, we note that the most closely related work is found in road following projects. Sealed urban roads have been the primary target of au- tonomous road-followers (as indicated by [Bertozzi et al., 2000; Bertozzi et al., 2002]), where assumptions of consistent geometry and visible road markings can and have been used, often with great success. In compar- atively degraded bush environments, where structure is minimal at best, such assumptions simply do not hold. When poorly structured roads have been the fo-

Local Path Planning along Bush Tracks using Vision · rescue teams work and could greatly benefit from robotic assistance, the task of navigation is often very ... ping Services,

  • Upload
    others

  • View
    1

  • Download
    0

Embed Size (px)

Citation preview

Page 1: Local Path Planning along Bush Tracks using Vision · rescue teams work and could greatly benefit from robotic assistance, the task of navigation is often very ... ping Services,

Local Path Planning along Bush Tracks using Vision

David Fernandez and Raymond A JarvisIntelligent Robotics Research Centre,

Monash University, Clayton, VIC 3800, [email protected]

Abstract

This paper investigates the problem of au-

tonomous mobile robot navigation in bush en-

vironments. Such environments form part of the

workspace of a number of groups, such as rural

search-and-rescue and bush fire-fighting crews,

who could greatly benefit from helper robots. In

particular, given the existence of many tracks

through bush areas, the ability to autonomously

drive along such tracks would help ensure robot

safety and efficiency of travel, as a track by def-

inition represents a passageway with a high like-

lihood of being clear. The novelty of the work

presented comes from the sole use of passive vi-

sion to find obstacles and locate tracks, allowing

a safe local path to be determined.

1 Introduction

In bush environments, where often under-mannedpersonnel including bush firefighters and search-and-rescue teams work and could greatly benefit fromrobotic assistance, the task of navigation is often verydifficult. Because such environments are the workplaceof a number of groups, they invariably contain accesstracks – whether vehicular dirt roads or smaller walk-ing paths – that can and should be made use of when-ever possible, as, by definition, they represent a rel-atively clear passageway between locations that havebeen determined to be of interest. While cross-countrytraversal may at times be necessary, the probability ofmission success will be greater if tracks are utilised.

Additionally, many if not most of the larger pathsand tracks in National and State Parks throughoutAustralia have already been mapped with these mapsbeing available either commercially [Feathertop Map-ping Services, 2006] or through government agencies[Parks Victoria, 2007]. Accordingly, it is a relativelyeasy task to find a high-level path between two pointsof interest that makes use of such tracks. This leavesthe problem of how a robot may be guided safely andefficiently along a given track, once it has been chosen.

While a number of different sensor system could beused to perform autonomous or semi-autonomous navi-gation along such tracks, passive vision offers a number

of benefits. Firstly, the equipment is cheap, perhapsthe cheapest of the sensors most likely to be useful(with sonar and lasers being the two main competi-tors). Given the typically stretched resources the rele-vant organisations (for example the CFA and SES), lowcost is important. Secondly, the natural world is visu-ally rich, meaning there is a wealth of visual informa-tion that has the potential to be exploited. Finally, theability to make use of human perception and insightinto the problem makes vision an attractive choice insuch an unstructured and variable environment.

The work presented in this paper aims to tackle aparticular element of the wider navigation problem.Specifically we investigate the coupled problems offirstly assessing the traversability of the local terrainand secondly finding a short-term path, relative to therobot, that takes the robot along a track while avoid-ing obstacles. That is, the path determined must beefficient, by staying on the track when possible, andit must be safe, by avoiding any form of obstacle thatit is likely to encounter. We choose to attempt thisusing passive vision alone, motivated by the reasonsstated earlier. The solution presented uses stereo vi-sion to find obstacles, colour image processing to char-acterise and extract the track surface, and simple path-planning techniques to combine these two classes of in-formation and determine a safe path along the track.

2 Related Work

The work presented in this paper formed part of theauthor’s doctoral dissertation [Fernandez, 2007] andextends and improves upon earlier work presented in[Fernandez and Price, 2005]. Accordingly, a thor-ough review of related works can be found in thosepublications. In summary, we note that the mostclosely related work is found in road following projects.Sealed urban roads have been the primary target of au-tonomous road-followers (as indicated by [Bertozzi etal., 2000; Bertozzi et al., 2002]), where assumptions ofconsistent geometry and visible road markings can andhave been used, often with great success. In compar-atively degraded bush environments, where structureis minimal at best, such assumptions simply do nothold. When poorly structured roads have been the fo-

Page 2: Local Path Planning along Bush Tracks using Vision · rescue teams work and could greatly benefit from robotic assistance, the task of navigation is often very ... ping Services,

cus, colour vision is the most common sensing modal-ity [Thorpe et al., 1991; Crisman and Thorpe, 1991;Fernandez and Casals, 1997; Ghurchian et al., 2002]though work has been done where colour vision is com-bined with other sensors, such as lasers [Rasmussen,2002]. In most cases, the inherent difference in colourof road and non-road regions, typically the result ofa difference in surface materials, is exploited, allow-ing road-surface regions to be characterised and henceextracted.

3 A System-level Overview

The task of autonomously navigating along bush tracksis broken down into three sub-problems: determiningthe presence and location of obstacles, determining thelocation of the track, and determining a local paththat will safely guide the robot, staying on the trackas much as possible while avoiding obstacles.

Here obstacles are defined as any region of space thatcould serve to impede or hinder the robot’s motion. Assuch, we are not necessarily considering distinct or dis-crete objects; a particularly bumpy and uneven patchof ground is as much of an obstacle as a fallen treeand both should be avoided to minimise the chance ofbecoming stuck or being damaged. This classificationof the environment, the “obstacle map”, describes theareas we wish to avoid to ensure safety.

As described in earlier work [Fernandez and Price,2005; Fernandez, 2007], the track is characterised anddefined in terms of its colour-space statistics, followingthe assumption that the track surface is visually dif-ferent to the surrounding environment, primarily be-cause of a difference in surface materials (for example,brown dirt or grey gravel compared to green surround-ing foliage). Using a dynamic description of the trackthat adapts to the current terrain, the environment isclassified based on its similarity to the current trackdescription. This second classification of the environ-ment, the “road map”, describes the areas we wish totraverse over in order to afford an efficient path.

The combination of these two maps gives rise towhat we call the “amenability map”, a description ofthe terrain’s amenability to safe traversal, given thedesire to stay on tracks where possible while avoid-ing obstacles. This map, along with a set of geomet-ric constraints, is then used to determine a local paththat begins at the robot and ends on the track somedistance away, avoiding obstacles and staying on thetrack along its trajectory.

4 Obstacle Space

The obstacle detection system uses a calibrated stereo-camera-pair, positioned to give a view of the terrain infront of the robot that provides a depth-map of the ter-rain. We define an obstacle as any region of space thatdeviates from an assumed ground plane. This allowsboth positive obstacles – those that extend above the

Figure 1: Sampled ground region

Figure 2: Ground normal constraint cone

ground such as rocks and fallen branches – and neg-ative obstacles – those that extend below the groundsuch as potholes and ruts – to be detected equally. Theground plane is itself dynamically defined, that is, it isdetermined from the current depth-map, allowing lo-cal variations in the terrain, such as slight inclines ordeclines, to be accommodated.

The fitted ground plane is described by a (unit) nor-mal vector Np and an anchor point Ap. Any three(non-collinear) points pa, pb and pc can be used todefine the plane as follows:

Np =(pb − pa)× (pc − pa)‖(pb − pa)× (pc − pa)‖

(1)

and

Ap =pa + pb + pc

3(2)

A window of depth-map points is sampled and theRANSAC algorithm [Fischler and Bolles, 1981] is usedto find and fit a plane to that region, assuming thatmost of what is visible in the sampled region is clearground. The windows is shown in Figure 1, superim-posed on the raw stereo image for clarity.

To ensure only sensible planes are used a normal con-straint is included which is simply an upper thresholdon the allowed angle between a recovered normal and acalibrated normal, where the angle between two (unit)normal vectors is defined by

θ = Cos−1(Ncal · Nest) (3)

Essentially, the calibrated normal and the thresholdangle, θnorm describe a cone (shown in Figure 2) andfor a plane to be considered viable its normal must liewithin this cone.

Creation of the obstacle map consists of a mappingof the absolute height of terrain points compared to

Page 3: Local Path Planning along Bush Tracks using Vision · rescue teams work and could greatly benefit from robotic assistance, the task of navigation is often very ... ping Services,

Figure 3: Obstacle mapping function

the ground. Using the minimum distance, h, betweenthe point p and the plane:

h = (p−Ap) · Np (4)

we then define the ground divergence dg of a point as

dg = |h| (5)

The ground-based obstacle likelihood Og, ranging from0 (clear ground) to 1 (highest likelihood of an obstacle)is defined as

Og(dg) =

0 , dg < dfloor

12 −

12 cos

(π(dg−dfloor)dceil−dfloor

), dfloor ≤ dg < dceil

1 , dg ≥ dceil

(6)where dfloor is a minimum divergence, below whichpoints are assumed to be safe and hence given azero obstacle score, and dceil is a maximum di-vergence, above which divergences carry maximumpenalty (greatest likelihood of an obstacle). Figure 3depicts this relationship graphically.

The specific form of this mapping function is notcritical. The key elements are an increase in likelihoodof an obstacle with increasing divergence from planarand a maximum allowable divergence. The idea is that,above this threshold, it no longer matters how muchof divergence there is, it is simply enough to assignthe highest likelihood of there being an impediment tomotion. The divergence thresholds used are dfloor =25, dceil = 100, empirically determined.

As a final point it should be noted that the defaultmap state is one of high obstacle likelihood, that is, apessimistic approach that errs on the side of caution.Accordingly, if the stereo-processing yields no infor-mation (typically because no correspondence match ofhigh enough quality is found), we assume maximumlikelihood.

5 Road Space

The road is defined by a set of dynamically extractedcolour-space descriptors. An image region that is as-sumed to contain the road is repeatedly sampled andcolour-space statistics are retrieved. If the retrieveddescriptor differs enough from the descriptors alreadyin the set, indicating the surface has changed, the newdescriptor is added to the set and the descriptor withthe lowest “utility” – a measure of usefulness related to

the age of the descriptor and its ability to consistentlydescribe the road – is discarded. In this manner, thesystem can readily adapt to changes in the road surfacebut still maintains an efficiently small set of descrip-tors. Each descriptor in the set is then used as thekernel of a colour filter, allowing each pixel to be givena likelihood of being part of the road based upon itssimilarity to the stored descriptors.

With variations in lighting being one of the largerproblems faced outdoors – especially mixtures ofstrong sunlight and dark shadows – a colour repre-sentation that separates brightness or intensity fromthe “colour” information was used. The specific trans-form from a standard red, green, blue triplet pRGB =[r, g, b]T to a so-called XYI triplet pXY I = [x, y, i]T isgiven as follows: x

yi

=

16 (3 +

√3) − 1

6 (3−√

3) − 1√3

− 16 (3−

√3) 1

6 (3 +√

3) − 1√3

1√3

1√3

1√3

r

gb

(7)

with conventional normalisation to give in inputsr, g, b ∈ [0, 1] and transformed values x, y, i ∈ [0, 1].

Using the pixels within the sampling region, a modi-fied K-means algorithm [Lloyd, 1982] is used to extractat most K clusters, each giving a mean c = [cx, cy, ci]and a standard deviation S = [Sx, Sy, Si]. The road-sampling window contains Ntotal samples (pixels) thatare distributed among K clusters (the number of clus-ters actually found and which is equal to or less thanthe K clusters requested). With a uniform distributionof samples, we would expect a quota of Ntotal

Ksamples

to be assigned to each cluster. A useful measure ofconfidence is the ratio of samples actually assigned toa cluster against this “uniform-assignment” quota, de-fined as follows:

ζ =N · KNtotal

(8)

It is a description of how much data was used to pro-duce a descriptor compared to an even spread of sam-ples. Each road descriptor is then defined by a clustermean c and standard deviation S, the number of sam-ples within the cluster N and a confidence measure ζ.When used as a filter kernel, the total number of hitshtotal (the total number of pixels associated with thisdescriptor) and the descriptor’s age t (the number ofprocessing iterations since the descriptors creation) arealso stored.

To determine the likelihood that a pixel in the imageis part of the road, we then perform a filtering oper-ation, comparing each pixel against all of the storedroad descriptors. For a given pixel of colour [x, y, i],we define its score ρ against a filter using tolerances of∆ = [∆x,∆y,∆i] as follows:

d =

√(x− cx

∆xSx

)2

+(

y − cy

∆ySy

)2

+(

i− ci

∆iSi

)2

(9)

Page 4: Local Path Planning along Bush Tracks using Vision · rescue teams work and could greatly benefit from robotic assistance, the task of navigation is often very ... ping Services,

and

ρ =

{minimum

(1, (1− d) · ζ

), d ≤ 1

0 , otherwise(10)

where ζ is the road descriptor confidence, used as aweighting factor. Note also that the score ρ is limitedto the range [0.0, 1.0]. Typical tolerances, empiricallydetermined, are ∆x = 3.0, ∆y = 3.0 and ∆i = 4.0,reflecting the desired greater tolerance to variations inintensity compared with variations in colour. Withthe use of multiple descriptors and hence multiple fil-ters, we simply assign to each pixel the highest scoreit receives, incrementing the hit-count htotal of the as-sociated filter in the process. We now have a way ofdetermining the likelihood of a pixel belonging to theroad, based on its similarity to a sampled road section.

In order to adapt to the current environment andchanges in the appearance of the road (for example,changes that occur when moving through shadows orfrom dirt onto gravel), the system samples the roadin order to create a new colour filter. However, formuch of the time, we can expect there to be only smallchanges in the appearance, essentially caused by thespecific patch of road that is being sampled. To avoidthe inefficient creation of new filters that do not actu-ally help (because the colours they allow are largely al-ready covered by existing filters), we introduce a meansof comparing a new descriptor against those of the ex-isting filters.

For two descriptors and a tolerance ∆ =[∆x,∆y,∆i] we first define the following quantities:

x0,min = c0 −∆xS0

x0,max = c0 + ∆xS0

x1,min = c1 −∆xS1

x1,max = c1 + ∆xS1

Xmin = maximum(x0,min , x1,min )Xmax = minimum(x0,max , x1,max )

We then define the overlap of the two filters in theX channel as follows:

X0,overlap =Xmax −Xmin

2∆xS0

with Y0,overlap and I0,overlap defined similarly for theremaining two colour channels. These measures de-scribe the amount of overlap between two filters ineach colour channel, normalised against the spread ofthe first filter. (It should be noted that this method ofcalculating overlap implicitly treats the filters as de-scribing a cuboid, not ellipsoid, region of colour-space,and so is a simplified approximation to the true over-lap.) A positive overlap indicates that there is a sharedregion in the relevant channel, while a zero or negativeoverlap indicates no common coverage. We then definethe similarity of the two filters, referenced against the

first, as follows:

s0 = minimum( X0,overlap, Y0,overlap, I0,overlap )(11)

Two filters that share a common region in colour space(under the cuboid approximation) will give a positivesimilarity, while two that do not (even if there is over-lap in one or two channels) will give a zero or negativesimilarity. A new filter is only created if the similar-ity thus defined is less than an empirically determinedthreshold (a threshold of 0.9 is used), meaning the re-gion in colour space covered by the new filter is noteffectively covered by an existing filter.

Filters are held in a list that is sorted based on thefilter’s utility uF , defined as follows:

uF =htotal

1 + t(12)

with htotal and t being the total hit count and age ofthe filter respectively. The utility is an indication ofhow useful, on average, a filter has been, with regardsto its ability to consistently describe the road.

When a new filter is created (because the new statis-tics are different enough from all existing filters), thelist of filters is first sorted by filter utility, the leastuseful filter is discarded and the new filter is added tothe list. In this way, we provide adaptability to cur-rent conditions (through the creation of new filters)and a manageable temporal accumulation of knowl-edge (through a fixed-size set of filters that is modifiedas time passes).

6 Finding an Amenable Path

We now have available two environmental maps, onedescribing the likelihood of an area being part of anobstacle and another describing the likelihood of anarea belonging to the road. While at first glance itmight seem that these two maps are simply inversionsof one-another, this is not necessarily the case. Forexample, a rut or a particularly bumpy and unevenpatch of the track would both most likely still look likethe track and hence be given a high likelihood of beingpart of the track. According to our earlier definitionof what constitutes an obstacle, however, these regionsare unfavourable and should be avoided, and would begiven a moderate to high likelihood of being part ofan obstacle. In another case, we may wish – or need– to deviate off the track, perhaps to move around alarge obstacle (such as a fallen tree), in which casewe still wish to stay where the ground is obstacle-free,but not necessarily stay on the track. As such, the twolikelihood maps are independent but compatible. Thecombination of these maps describes the environmentin terms of overall amenability to traversal.

In the physical configuration used, the camera usedfor finding the track is not either of the stereo camerasand as such the road map and obstacle map are in dif-ferent frames of reference. Image registration through

Page 5: Local Path Planning along Bush Tracks using Vision · rescue teams work and could greatly benefit from robotic assistance, the task of navigation is often very ... ping Services,

(a) Raw stereo im-age (left)

(b) Obstacle space (c) Obstaclesspace, reprojected

(d) Raw road im-age

(e) Road space (f) Road space, re-projected

Figure 4: Re-projection of obstacle and road spacesonto virtual camera

re-projection onto a virtual camera (that has a birds-eye view), using data obtained during initial systemcalibration, is therefore the first step in combining thetwo maps. The results of such a re-projection areshown in Figure 4. In these and similar figures, highlikelihoods are shown as light regions and low likeli-hoods are shown as dark regions.

The likelihood of a location having feature f (wheref is either obstacle or road), is denoted as Lf and is inthe normalised range [0, 1]. However, we wish to pro-vide a means by which the relative importance of eachfeature can be adjusted. Typically, we wish to placea slightly higher importance on avoiding obstacles, astraversal into an obstacle is likely to present a greaterimpediment to motion (and present a greater risk ofmission failure) than simply straying off the road tem-porarily. However, under certain circumstances (forexample, if we are required to travel cross-country, thatis, off the road), we would wish to ignore the road in-formation. Each feature is therefore assigned a sim-ple weight, denoted γf , and the overall amenability totraversal, Atrav, is given by

Atrav = γobsLobs + γroadLroad (13)

Accordingly, γobs will be negative and γroad will bepositive. An amenability below zero then indicates ter-rain we should strongly avoid, zero indicates somewhatneutrality with regards to traversal, and positive indi-cates the terrain we most strongly wish to traverse.Applying Eqn 13 to each point in the re-projectedlikelihood maps produces an overall amenability map,which we can then use to deduce a safe path. Figure 5shows an obstacle and a road likelihood map, and theresultant amenability map.

The amenability map is in essence a finely-grainedoccupancy grid where the value of each cell is the like-lihood of that cell being safely traversable. This isconsidered to be both a feasible and effective represen-tation because the map is only intended to be kept for

(a) Obstacle-space (b) Road-space (c) Amenability

Figure 5: Obstacle and road likelihood maps and theresultant amenability map

a short time and used locally, and so problems associ-ated with scaling (and the corresponding increases instorage and processing requirements) are not likely tooccur.

Having determined for each point in the map anamenability to traversal, we are now in a position tofind potential paths, compare them, and determinethe best candidate, which then determines where therobot’s next waypoint will lie. This process consistsof three steps: finding contiguous regions in the map,linking them to form potential candidates, and fittinga model to each candidate to determine its suitability.

Traditionally, maps based on occupancy-grids aretypically coupled with planners based on the distance-transform or potential-field concepts, where the pathfrom known starting location (that is, the current po-sition) to known destination is incrementally built, forexample, by performing a steepest descent operationusing the distance transform or by iteratively deter-mining the resultant “force” being applied to the robotby the attraction of a goal and the repulsion of obsta-cles in a potential field. In either case, the destinationmust be known (whether provided or determined) be-fore a path can be planned. In the context of follow-ing dirt roads or attempting straight-line cross-countrytraversal, these intermediate goals must be determinedfrom an examination of the locally produced amenabil-ity map, applying the high-level goal of either followingthe road or maintaining a bearing, to find a destina-tion that lies within safe (that is, amenable) space andis in the appropriate direction (either the direction ofthe road or the desired bearing), but is located somedistance away. The tasks of determining an intermedi-ate destination and finding a path to that destinationare therefore tightly coupled. Accordingly, a differentapproach to finding a path is adopted where no specificdestination is used. Instead some simple geometric andenvironmental assumptions are made which allow thegeneration of multiple paths which can then be com-pared to find what is considered the best option.

6.1 Sliced Segments

With our underlying goal being to drive along a road,the assumption is made that the road we wish to fol-low, and hence the path we will wish to take, flowsgenerally away from the robot in approximately thedirection it is facing. This assumption, which will typ-

Page 6: Local Path Planning along Bush Tracks using Vision · rescue teams work and could greatly benefit from robotic assistance, the task of navigation is often very ... ping Services,

(a) All segments (b) Merged and culled seg-ments

Figure 6: Segments: before and after merging andculling

ically be valid whenever the robot is on the road andeither already travelling along it or at least facing thedirection it wishes to travel, allows the process of deter-mining the path and intermediate goal to be simplified.

Following this assumption, the map of amenable ter-rain is separated into slices: horizontal regions that lieperpendicular to the direction the robot is facing. Wethen segment the map into contiguous regions withineach slice, looking for what should be safe regions ofterrain that may be separated by potentially unsafeareas. Segments are described by their centre-of-mass,total mass and area, where member pixels contributeaccording to their amenability (that is, the total massis the sum of all contributing pixels’ amenabilities, andthe centre-of-mass is the amenability-weighted meanposition of contributing pixels). Only segments abovea minimum size (in both area and mass) are consideredfurther. Within a slice, segments that are separated byless than a maximum distance are merged to allow forsmall discontinuities that should not impede motion.For example, a small tuft of green grass in the mid-dle of a brown dirt road would not impede motion,but would likely be considered to not belong to theroad, given the difference in appearance. Additionally,starting closest to the robot, the adjacency of segmentsbetween slices is examined. A gap between segmentsis not allowed, and any segment that is not directlyadjacent to another segment that is closer to the robotis culled.

Figure 6 shows an example of a segmented amenabil-ity map. In Figure 6(a), all segments are shown, eachdescribed by a bounding rectangle and a circle at itscentre of mass. Figure 6(b) shows the merged andculled segments, described similarly. As can be seen,many segments in the upper part of the image that areclearly not part of the road have been removed due totheir separation from the main road body. Along themain road body, a number of small segments can alsoseen to have been merged with horizontally adjacentsegments when the separation is very small.

6.2 The Graph of Good GroundTo maintain a safe path, we wish, as much as possible,to stay on or in the regions we know to be safe andamenable to motion. Other than the assumption that

Figure 7: The path graph

the road flows generally away from the robot, we donot know where it lies. It may veer to the left, it mayveer to the right, it might fork or it may be interruptedby obstacles. To accommodate this, we create a graphfrom the derived segments. The graph is rooted at therobot, and extends away, linking increasingly distantsegments that touch but are within adjacent slices. Inthe case of a fork, for example, we will have a singlesegment in the slice at the base of the fork but two, hor-izontally separated segments in the next slice, wherethe road has forked. The segment at the base will thenbe linked separately to the segments in both arms ofthe fork, and so on. Similarly, an obstacle in the mid-dle of the road will produce a graph that forks aroundthe obstacle and recombines beyond it. The centre-of-mass of each included segment forms a node in thegraph, such that the graph flows between points ofmaximum amenability (following the assumption thatthe centre-of-mass is the most amenable point within asegment). By construction, the graph is directed andacyclic, as each new node that is added may be thechild of multiple existing nodes, but is the parent ofnone.

This graph represents the set of potential short-termpaths that conform to our requirement of staying onsafe terrain. The adjacency requirement means a robotmoving along the graph should never travel over unsafeterrain.

Figure 7 is an example path graph, showing pathsegments (as previously) as the resultant graph thatconnects the segment centres-of-mass.

6.3 From a Graph to a Trunk:Evaluating Potential Paths

The graph that we have created describes, in general,a whole series of potential paths, any of which may betaken. We must therefore reduce this graph to a singletrunk that represents the desired path. To do this, weneed a way of comparing potential paths against each-other, according to some metric, allowing them to beranked and a most favourable candidate to be chosen.

As much as possible, for both simplicity and effi-ciency, we would like the robot to take a smooth paththat takes the robot as far along the road as possi-ble. By smooth we mean a path that will require onlysmall steering changes, reducing the chance of errorsassociated with large or rapid changes in the direc-tion of the robot. The desire for a relatively long path

Page 7: Local Path Planning along Bush Tracks using Vision · rescue teams work and could greatly benefit from robotic assistance, the task of navigation is often very ... ping Services,

trades off the ability to react to environmental changesagainst the efficiency of needing many (computation-ally) costly scanning and planning operations. In thebush, it is assumed that the physical environment islargely static. While wind can cause rapid motion ofbranches and shifting clouds can quickly change light-ing and shadows, for example, these changes do nothave a significant effect – if any effect at all – on whereit is safe to travel. Moving obstacles are also unlikelyto be encountered, largely because human and vehic-ular traffic is very low in these areas. Those obstaclesthat are likely to be encountered – animals, such askangaroos – typically move faster than the robot couldrealistically react to, and so an attempt to accommo-date them would lead to ineffective planning.

We would also like the robot to travel where it is con-sidered safest, meaning traversing a path that is highlyamenable (according to obstacle and road detection)and that provides physical safety by being wide andoffering large buffers on either side of the robot.

To find a potential path, we begin at the root of thegraph and work outwards, performing a breadth-firsttraversal. At each stage, the current node is appendedto a list of nodes that describes the path taken to reachthe current node. It is this list of nodes (where eachnode is a segment in the amenability map) that is usedto fit a path model for evaluation. A limit is placed onthe total number of paths allowed to be evaluated, inorder to place an approximate bound on the process-ing time required when the amenability map is poorlyconstructed and produces a dense, highly-connectedgraph. By using a breadth-first traversal, we favourshort-distance, wide-area exploration of paths insteadof long-distance, narrow-area exploration, as would beobtained with a limited depth-first traversal.

To create a smooth path, a small-window smoothingoperation is first performed on the centres-of-mass ofthe contributing segments. This is primarily done toremove large jumps between adjacent segment centres-of-mass that typically occur when a narrow segmentappears adjacent to a wide one. For example, atthe point of splitting, a fork in the road will producetwo segments that will each be approximately half thewidth of the preceding segment. This is depicted inFigure 8, where segment centres-of-mass are shown ascircles and the trace of contributing segments is shownas a solid line. In Figure 8(a) the trace undergoesa sharp change in direction at the point of forking,while in Figure 8(b), with smoothing, the change indirection is much less extreme. In this usage, smooth-ing serves to avoid potential problems associated withrapid changes in robot direction and the potentiallydangerous situation of cutting corners.

We choose to model a potential path as a simplesecond-order polynomial, reflecting the desire for apath that can accommodate road curvature yet stillproduces a smooth trajectory. In fitting the polyno-mial to a potential path we use the error – the differ-

(a) Un-smoothed (b) Smoothed

Figure 8: Original and smoothed potential path

ence between the model and the contributing nodes’locations – as an indication of how smooth that pathis (compared to the idealised polynomial path).

There is one other factor that needs to be consideredwhen selecting a final path: direction. In some situa-tions, most notably when facing a fork in the road, adecision needs to be made (typically by a higher-level,global path-planner) as to which fork to take. At othertimes, the robot may be traversing cross-country, andneeds to maintain a given bearing, again provided by ahigher-level planner. In the more general case, we willsimply wish to maintain a consistent trajectory andprovide some immunity to instabilities and vacillationthat may occur when the choice of path is not distinct.To allow for these types of control, we introduce the no-tion of path bearing. In evaluating a path, in additionto the second-order polynomial, a first-order polyno-mial is fitted, and the gradient of this model is usedto determine the overall bearing of the path, relativeto the robot. We then have a form of control with thebearing error, equal to the absolute difference betweenthe path bearing and the desired bearing. The desiredbearing is either given by a global-planner or simplyequal to the previous bearing in order to produce atemporally consistent choice of path.

For a given potential path, we have a number ofdefining characteristics. The total amenability A isthe sum of all contributing segment masses. The dis-placement d is the straight-line distance between thefirst and last contributing segments. The error ε isthe difference between (smoothed) segment centres-of-mass and the fitted model. The minimum buffer b isthe minimum distance between the fitted model andhorizontal segment boundaries. The average width wis the average width of contributing segments. Thebearing error θ is the absolute difference between theapproximate path bearing and the desired bearing.

The overall fitness of this path is defined by:

f = cAA + cdd + cεε + cbb + cww + cθθ (14)

where {cA, cd, cε, cb, cw, cθ} are weighting coefficientsthat describe the relative importance of each trait.Typically we use cA = 1.5, cd = 2.0, cε = −0.5,cb = 1.5, cw = 1.0 and cθ = −1.0. With these val-ues, we favour a highly amenable, long path with largebuffering, and weakly penalise paths with a large fit-

Page 8: Local Path Planning along Bush Tracks using Vision · rescue teams work and could greatly benefit from robotic assistance, the task of navigation is often very ... ping Services,

(a) Trunk path (b) Fitted model

Figure 9: The path trunk

Figure 10: Road-following result 1

ting error, indicating a path that is not smooth, andpaths that deviate from the desired bearing. Standardbehaviour also simply sets the desired bearing to theprevious bearing in order to provide a path that istemporally smooth.

7 Experimental Results

In this section a selection of local path-planning resultswill be presented in order to give an overview of thesystem’s performance.

In the results that follow, the raw centre image, re-projected obstacle likelihood map, reprojected roadlikelihood map, amenability map (overlaid with seg-ments, graph, trunk and fitted model) and reprojectedcentre image (overlaid with fitted model) are shown.

Figure 10 shows a fairly simple scene where the roadveers off to the right, with the only potentially trou-blesome element being the alternating strong shadowsand bright regions. The road map shows, however,that these potentially problematic lighting conditions

Figure 11: Road-following result 2

are handled comfortably. There is a patch to the left ofthe road that is incorrectly considered to be part of theroad, however, because of the brightly-lit leaf-litter’sresemblance to the road surface. The ground is quiteflat and the obstacle map reflects this, though thereare some falsely classified obstacle regions in the dis-tance. The amenability map accordingly shows thesenon-existent obstacles as regions to avoid. However,the overall shape of the safe region on the amenabilitymap is a good representation of the environment, andthe resultant path is a smooth curve to the right thatfollows the centre of the road.

In Figure 11 a large rock is situated on the left sideof the road surface, creating a clear obstacle. The ob-stacle map shows a safe and traversable environmentexcept for the presence of the rock. In spite of therebeing a fairly small change in appearance between thesandy dirt road and the dry surrounds, the road mapshows that the road surface has been very well ex-tracted. The amenability map shows the terrain aheadto be quite safe ahead except for the large rock. Thepath chosen aims to veer around the rock and to theright while largely staying on the road.

In Figure 12 there is a very subtle obstacle infring-ing upon an otherwise clear road. There is a featheryoverhanging branch that droops from above over theright side of the road. The colour of the branch is verysimilar to that of parts of the road making it essentiallyinvisible in the road likelihood map (unlike some earlyobstacles, that had distinctly different appearances tothe road). However, it is well detected by the stereosystem and appears as a clear obstacle on the obstacle

Page 9: Local Path Planning along Bush Tracks using Vision · rescue teams work and could greatly benefit from robotic assistance, the task of navigation is often very ... ping Services,

Figure 12: Road-following result 3

map. The chosen path, while stopping slightly shortdue to false-positive obstacles in the middle-ground,safely aims to guide the robot around and to the leftof the branch, the safer of two alternatives (there is apotential path around and to the right, but the unob-structed road is wider to the left).

Some fallen branches lie on the left of the road inFigure 13. They are not at all distinctly visible onthe road map, which does otherwise extract the roadvery clearly. The obstacle map, though, does show thepresence of obstacles to the left on otherwise flat andclear ground. The generated path veers around andto the right of the branches, stopping short becauseof sparse stereo information in the distance and smallregions of false-positives.

The next set of results show scenes of cross-countrytraversal. Accordingly, no road likelihood map isshown, and the amenability map is produced usingonly the obstacle likelihood map.

In Figure 7 the robot is facing short, step-like rise inthe terrain with the ground quite flat on both sides stepbut at different levels. The obstacle map clearly showsthe higher ground to be unsafe while the foregroundand region to the right is flat and so considered safe.The path produced veers to the right, aiming to steerthe robot away from the step and guide it parallel toit.

The scene in Figure 7 is quite cluttered on the leftwith wispy branches and a large tuft of grass. There isa fairly clear path around the shrubbery to the right,and this is firstly shown in the obstacle and amenabil-ity maps and secondly taken advantage of by the path-

Figure 13: Road-following result 4

Figure 14: Cross-country result 1

Page 10: Local Path Planning along Bush Tracks using Vision · rescue teams work and could greatly benefit from robotic assistance, the task of navigation is often very ... ping Services,

Figure 15: Cross-country result 2

planner.

8 Discussion

The results section above has shown the system’s abil-ity to evaluate the terrain under varied conditions andconsistently determine which regions are likely to be-long to the track and to contain obstacles and there-fore which regions are likely to be amenable to safetraversal. Following this, the determination of an ef-ficient local path that serves to take the robot alongthe track while avoiding obstacles is shown. Obstaclesboth obvious – such as large rocks and embankments– and more subtle – including overhanging and fallenbranches – are detected, while extraction of the trackis shown to be resilient to adverse conditions includingstrong sunlight and shadows and situations where thetrack is only slightly different in appearance to the sur-rounds. What has not been shown here is the dynamicbehaviour of the system, in particular the possibilityof the system, under certain circumstances, to enter adetrimental positive feedback loop. If the robot wan-ders off the track for too long, perhaps in order toavoid a large obstacle, it is possible for the historyof track descriptors (Section 5) to be filled completelywith “off-road” descriptors, causing the system to thentry to stay off the track. This is largely a result of thecurrent system obtaining the obstacle and road mapsindependently. Future work aims solve this problem byusing the obstacle map to guide the extraction of roaddescriptors: only regions free from obstacles would beused to characterise the road and so the likelihood ofobtaining and retaining off-road descriptors is signifi-cantly reduced.

9 Conclusions

In this paper we presented a navigation subsystem thataims to autonomously guide a robot along a roughbush track. The aim is to determine where the trackis, where any obstacles may lie, and then choose an ap-propriate path that stays on the track where possible

while still avoiding obstacles. The novelty of the ap-proach presented, in part, lies with the fact that this isdone using only passive vision, making it a cheap andsimple (in terms of equipment) solution that could beused on robots aimed to assist SES or CFA workers,for instance, who often find themselves undermannedand in life-threatening situations where helper-robotscould be of great use.

References[Bertozzi et al., 2000] M. Bertozzi, A. Broggi, and A. Fas-

cioli. Vision-based intelligent vehicles: State of the artand perspectives. Robotics and Autonomous Systems,32(1):1–16, Jul 2000.

[Bertozzi et al., 2002] M. Bertozzi, A. Broggi, M. Cellario,A. Fascioli, P. Lombardi, and M. Porta. Artificial visionin road vehicles. Proceedings of the IEEE, 90:1258–1271,Jul 2002.

[Crisman and Thorpe, 1991] J. Crisman and C. Thorpe.Unscarf-a color vision system for the detection of un-structured roads. In IEEE International Conference onRobotics and Automation, volume 3, pages 2496–2501,Apr 1991.

[Feathertop Mapping Services, 2006] Feathertop Map-ping Services. Feathertop and rooftop maps.http://www.feathertopmapping.biz/, 2006. Visited2006.

[Fernandez and Casals, 1997] J. Fernandez and A. Casals.Autonomous navigation in ill-structured outdoor envi-ronment. In IEEE/RSJ International Conference on In-telligent Robots and Systems, volume 1, pages 395–400,1997.

[Fernandez and Price, 2005] D. Fernandez and A. Price.Visual detection and tracking of poorly structured dirtroads. In 12th International Conference on AdvancedRobotics, pages 553–560, 2005.

[Fernandez, 2007] D. Fernandez. Visual Navigation onRough Tracks in Bush Environments. PhD thesis, Intelli-gent Robotics Research Centre, Department of Electricaland Computer Systems Engineering, Monash University,Australia, 2007.

[Fischler and Bolles, 1981] M. A. Fischler and R. C.Bolles. Random sample consensus: a paradigm formodel fitting with applications to image analysis andautomated cartography. Communications of the ACM,24(6):381–395, 1981.

[Ghurchian et al., 2002] R. Ghurchian, T. Takahashi,Z. Wang, and E. Nakano. On robot self-navigation inoutdoor environments by color image processing. In7th International Conference on Control, Automation,Robotics and Vision, volume 2, pages 625–630, Dec 2002.

[Lloyd, 1982] S. Lloyd. Least squares quantization in pcm.IEEE Transactions on Information Theory, 28:129–137,1982.

[Parks Victoria, 2007] Parks Victoria. Parkweb, 2007. Vis-ited 2007.

[Rasmussen, 2002] C. Rasmussen. Combining laser range,color, and texture cues for autonomous road following.In IEEE International Conference on Robotics and Au-tomation, volume 4, pages 4320–4325, 2002.

[Thorpe et al., 1991] C. Thorpe, M. Herbert, T. Kanade,and S. Shafer. Toward autonomous driving: the cmunavlab. i. perception. IEEE Expert, 6(4):31–42, 1991.