Navigation Algorithms for Autonomous Machines in Off-RoadApplications

Nebot E., Bailey T., Guivant J.

Australian Centre for Field RoboticsDepartment of Mechanical and Mechatronic Engineering

The University of Sydney, NSW 2006, Australianebot/tbailey/guivant/@acfr.usyd.edu.au

Abstract

This work presents some different approaches to the design of navigation systems for off-road terrain. The standardnavigation methods are presented and their limitations in unstructured environments discussed. The concept ofSimultaneous Localization and Map building is presented with outdoor navigation results in a variety of off-roadunstructured environments. A batch data association method is introduced to determine data associations in amanner decoupled from the vehicle pose estimate. Finally, an observation-based dead-reckoning procedure isdemonstrated that produces vehicle motion estimates of significantly better quality than odometry.

1 INTRODUCTIONIn the past, research into the development of unmanned air, underwater and land vehicles has been essentially thedomain of military related organizations. Today, with the availability of accurate and affordable navigation sensorsand the maturity of navigation techniques, it has become possible to extend the application of this research andimplement partial automation of industrial vehicles in areas such as mining, stevedoring and agriculture [1],[2]. Forthese applications to operate with a higher degree of autonomy, it is essential to ensure integrity in terms of beingable to detect and recover from failures.Localisation, the ability to locate a vehicle in space, is a fundamental competence of any Autonomous GuidedVehicle (AGV) system. To achieve any useful purpose, to navigate from one place to another, an AGV must knowwhere it is in relation to its environment. There are well known methods for solving the navigation problem basedon different types of beacon-based techniques [3]. Many industrial outdoor applications make use of standardnavigation loops, which rely on special infrastructure installed in the area of operation. These techniques weredesigned initially for indoor applications but, with the advent of low cost outdoor range and bearing devices (laserand radar), they are also being adopted in outdoor commercial applications [2]. These approaches can be very robustbut require modification of the environment to add the artificial beacon infrastructure.Another localization approach is based on the combination of Global Positioning Systems (GPS) and Inertialsensors. GPS has been widely used in various commercial applications. This particular sensor becomes veryattractive since it can report position with up to 1 cm accuracy in real time. Unfortunately this performance cannotbe maintained all the time in most industrial environments due to satellite availability and multi-path problems [4].In order to add integrity to the navigation system this sensor can be combined with different types of inertialdevices. In particular a six degree of freedom Inertial Measurement Unit (IMU) can be used to predict position andvelocities during GPS outages. A typical GPS-inertial navigation loop is shown in Figure 1. With this approach, theKalman filter estimates the errors in position and velocity using the difference between inertial-indicatedinformation and external sensor information. The inertial high frequency information is fed directly to the outputwithout attenuation while the Kalman Filter provides low frequency corrections to the IMU.

INS (IMU)

Error Model

Kalman Filter

DGPS

_

+

PositionVelocity

Figure 1: INS/GPS Navigation Loop

The position/velocity information derived from the IMU is subtracted from the position/velocity provided by theGPS system. An error propagation model is used to fuse this observation and to estimate errors in position, velocityand attitude. It also corrects various parameters of the inertial measurement unit, such as bias in accelerometers andgyros and platform misalignment angles. These parameters are fed back to the IMU. More information on thisimplementation can be found in [5].Integrity is another very important issue in any commercial autonomous vehicle. A high integrity navigation systemwill not only provide position information but must be able to detect faults and recover from them. The only way toachieve these specifications is by providing information redundancy covering the entire frequency spectrum ofoperation. A navigation architecture with these characteristics will need to be based on a number of independentlocal navigation loops as shown in Figure 2. In each local loop (or navigation node), a local filter generates a localestimate of vehicle state using only local sensors. The partial estimates from each loop are communicated to a dataassociation and assimilation unit. This central unit compares the estimates generated by each loop to obtain faultinformation. Once fault data is eliminated, loop information is combined and returned as additional externalinformation to each navigation loop.

Arbitrationlogic

Loop 1Model

InternalSensor

External Sensor

Loop 2Model

InternalSensor

External Sensor

Estimates

FaultData

Assimilation and DataAssociation Unit

Loop nModel

InternalSensor

External Sensor

Figure 2. Multi-loop Estimation Architecture.

The essential principle encapsulated by this structure is the explicit recognition that all sensors fail at some point intime in a number of (possibly unknown) ways. The structure is designed such that any one failure in any one sensorcan be detected and identified on-line. A failure in any two or more sensors may also be detected but not necessarilyidentified. This architecture is fully described in [6] and was successfully implemented in the automation of thestraddle carrier shown in Figure 3.

Figure 3. Automated Straddle Carrier.

For several reasons the implementation of the aforementioned techniques in off-road terrain applications is difficultand in some cases not possible. Most off-road terrain applications involve large areas of operation. The navigationsystem must be designed to operate reliably without modifying the infrastructure of the environment. This meansthat it would not be feasible to design a localization system based on beacons at known location. Although GPSlooks attractive it may not be a reliable option in environments such as those shown in Figures 4 and 5. In theseenvironments it is very common to have a restricted view of the sky in many areas. In fact GPS position informationmay not be available to the vehicle for a long period of time. This problem is clearly seen in Figure 6 where only theGPS updates with error less than 6 meters are presented. This particular unit is capable of reporting GPS positionwith 2 cm accuracy but, due to lack of satellite availability, the system is not able to operate in this mode. In thistype of application a very high quality inertial unit would be required to localize the vehicle in the area with smallerrors. The cost of this unit will be outside the budget of many industrial applications.The previous approaches are based on the availability of some source of absolute position information, such asbeacons or GPS information. If this information is scare or not available at all, the localization problem can still besolved by building a relative map of the area and localizing with respect to this map simultaneously. It is well knownthat the two problems are correlated and cannot be solved independently [7]. This problem is usually known assimultaneous localization and map building (SLAM) and was originally introduced in [8-9]. During the past fewyears significant progress has been made towards the solution of the SLAM problem. A number of differentapproaches have been presented to address this problem. In [10] a probabilistic approach is presented to solve thelocalization problem or the map-building problem separately when either the map or position of the vehicle(respectively) is known a priori. This approach is based on the approximation of the probability density functionswith samples, also called particles. This idea was originally introduced in [11] as a bootstrap filter but is morecommonly known as the particle filter. This approach has been applied very successfully to a number indoornavigation applications, in particular in [12].One of the most appealing approaches to solve the localization problem in real time is by modeling the environmentand sensors and assuming errors with Gaussian distributions. Then very efficient algorithms, such as Kalman filters,can be used to solve this problem in a very compact and elegant manner [13]. These methods can also be extendedto perform SLAM. One of the main problems of the SLAM algorithms has been the computational requirement, notso much in terms memory but in terms of CPU time requirements. It is well known that the complexity of the SLAMalgorithms can be reduced to the order of N2 [9], N being the number of landmarks in the map. For long durationmissions the number of landmarks will increase and eventually the on-board computer resources will not besufficient to update the map in real time. This problem arises because the full map is correlated. The correlationappears since the observation of a new landmark is obtained with a sensor mounted on the mobile robot andobviously the error will be correlated with the uncertainty in the vehicle location and the other landmarks of themap. This correlation is of fundamental importance for the long-term convergence of the algorithm [7], and needs tobe maintained for the full duration of the mission. The computational issue of this algorithm can be solved bychoosing appropriate local navigation areas and by extending the standard Kalman filter equation to maintain theinformation gained in the local area. This paper presents the formulation required to maintain all the informationgathered in a local area with a cost proportional to the square of the number of landmarks in the area. This

information can then be transferred to the rest of the global map with a cost (at most) similar to full SLAM but inonly one iteration.Consistent performance of the SLAM algorithm is dependent on the correct association of sensed features to mapfeatures. In outdoor applications, and particularly off-road terrain, this presents an important challenge as the vehiclepose estimate, typically used to validate data associations, may possess large uncertainties. In this work we present abatch data association method that uses the simultaneous observation of multiple features to determine dataassociations decoupled from the vehicle pose estimate. This method can also be used to generate very accurate deadreckoning information which becomes necessary when the odometric estimate (based on the kinematic vehiclemodel) suffers from significant slip and other dynamic influences commonly present in off-road applications.

Figure 4 Rugged unsealed forestry roads.

Figure 5 Hilly cattle grazing lands.

-200 -150 -100 -50 0 50 100-100

-50

0

50

100

150

200GPS with error < 6 meters

longitude (m)

latit

ude

(m)

Figure 6 GPS data reported with errors less than 6 meters

Figure 7 Different regions of the Victoria park experiment

This paper is organized as follows. Section 2 presents the Simultaneous Localization and Map Building approachwith the optimization algorithm. Section 3 presents the non-model based dead reckoning evaluation. Finally insection 4 and 5 the experimental results and conclusions are presented.

2 SIMULTANEOUS LOCALIZATION AND MAP BUILDING

Modelling Aspects

When absolute position information is not available it is still possible to use relative observations of the environmentto build a map and navigate with small errors for long periods of time. We called this task SimultaneousLocalization and Map Building (SLAM). The mobile robot is usually equipped with dead reckoning capabilities andan external sensor capable of measuring relative distance of the vehicle to the environment. Such is the case of thevehicle shown in Figure 8. The steering control α, and the speed υc are used with the kinematic model to predict theposition of the vehicle. In this case the external sensor returns range and bearing information to the different featuresBi(i=1..n). This information is obtained with respect to the vehicle coordinates (xl,yl), that is ( ) ( , )z k r β= , where r is

the distance from the beacon to the range sensor, β is the sensor bearing measured with respect to the vehiclecoordinate frame.

��� �

�

�

�

��������

�

�

��

�

���

�

��

Figure 8 Vehicle coordinate system

Considering that the vehicle is controlled through a demanded velocity vc and steering angle α the process modelthat predicts the trajectory of the centre of the back axle is given by [14]:

( )( )( )

cos

sin

tan

c c

c c

c c

x v

y v

v

L

φφ

φ α

⋅ = ⋅ ⋅

(1)

Where L is the distance between wheel axles. The equation that relates the observation with the states is

( )( ) ( )

( )( )

2 2

, ,atan 2

i L i Lir

i i i i LL

i L

x x y yh

z h X x y y yhx x

β πφ

− + − = = = − − + −

(2)

where z is the observation vector, ( ),i ix y are the positions of the features and xL , yL and φL are the vehicle states

defined at the external sensor location. The observation equation can be expressed in short form as

( ) ( ( )) ( )z k h x k kη= + (3)

The noise ( )kη is assumed to be Gaussian, temporally uncorrelated and zero mean. When the vehicle performsSLAM in unknown areas, it will detect new features. Once these features become reliable and stable they areincorporated in to the map becoming part of the state vector. In the general case the vehicle starts at an unknownposition with a given uncertainty and obtains measurements of the environment relative to its position. Thisinformation is used to incrementally build and maintain a navigation map and to localize with respect to this map.The state vector is now given by:

( )( )

3

1 1

, ,

, ,.., ,

L

i

T

L L L L

T Ni n n

xX

x

x x y R

x x y x y R

f

È ˘= Í ˙Î ˚

= Œ

= Œ

(4)

where (x,y,φ)L and (x,y)i are the states of the vehicle and features incorporated into the map. Since this environmentis consider to be static the dynamic model that includes the new states becomes:

( ) ( )( )( ) ( )

1

1

L L

i i

x k f x k

x k x k

+ =

+ =(5)

These models can then be used with a standard EKF to build and maintain a navigation map of the environment andto track the position of the vehicle.

Optimization of SLAM

Under the SLAM framework the size of the state vector is equal to the number of the vehicle states plus twice thenumber of landmarks, that is 2*N+3=M. This is valid when working with point landmarks in 2-D environments. Inmost SLAM applications the number of the vehicle states will be insignificant with respect to the number oflandmarks. The number of landmarks will grow with the area of operation making the standard filter computationimpracticable for on-line applications.

Compressed FilterIn this section we demonstrate that it is not necessary to perform a full SLAM update with each observation whenworking in a local area. This is a fundamental contribution because it reduces the computational requirement of theSLAM to the order of the number of features in the vicinity of the vehicle independent of the size of the global map.A common scenario is having the mobile robot moving in an area and observing features within this area. This maybe due to the vehicle moving slowly or due to an external sensor with high frequency capabilities. Although highfrequency external sensors are highly desired to reduce the position error growth, they will also introduce a highcomputational cost to the SLAM algorithm to process this information. For example a laser sensor can be used atfrequencies of 4 to 30 Hz. Working with full SLAM this will require an update of M states at 30 Hz. In this work wedemonstrate that, while working in a local area observing a number of features, we can preserve all the informationprocessing a SLAM of the order of the number of features in the area. When this area is abandoned then theinformation acquired in this area can be transferred to the rest of the states without loss of information. This willallow incorporating high frequency external information with very low computational cost. It is shown that theglobal map is not required to be updated sequentially at the same rate of the local map.Consider the states divided in 2 groups

, ,

, ,

A

B

A NA

B

N NB A B

XX X R

X

X R X R N N N

È ˘= ŒÍ ˙Î ˚Œ Œ = +

(6)

We can associate the states XA to a local area and the XB as belonging to the rest of the navigation map as shown infigure 9

� �

Figure 9 Local and Global areas

Assume that for a period of time the observations obtained are only related with the states XA. In other words, theobservations do not involve states XB , that is

( ) ( )ˆAh X h X= (7)

Then

( )( ) ( ) ( )

[ ]

,

0

kA BX X k X X k

aA B

h hH H

X X X

h hH

X X

= =

∂ ∂= = =∂ ∂

È ˘∂ ∂= =Í ˙∂ ∂Î ˚

(8)

Considering the zeros of the matrix H the Kalman gain is evaluated as follows

11

1

aa ab

ba bb

TaT aa a

Tbba a

P PP

P P

WP H SW P H S

WP H S

--

-

È ˘= Í ˙Î ˚È ˘◊ ◊ È ˘

= ◊ ◊ = =Í ˙ Í ˙◊ ◊ Î ˚Î ˚

(9)

From these equations it is possible to see that:• The Jacobian matrix Ha has no dependence on the states XB.• The innovation covariance matrix S and Kalman gain Wa are function of Paa and Ha. They do not have any

dependence on Pbb, Pab, Pba and Xb.The change in the covariance matrix can be written

1 ,

aa aa abTT

ab ba ab

Ta a aa

P P PdP W S W

dP P P

H S H P

b mb

b m b-

◊ ◊ ◊È ˘= ◊ ◊ = Í ˙

◊ ◊Î ˚= ◊ ◊ = ◊

(10)

Then the change in covariance matrix after k consecutive updates is:

( ) ( ) ( )

( ) ( ) ( ) ( ) ( )

, 1 , 0

, , 0 , 0 1 , 0

ab k k ab

bb k bb ba k ab

P P

P P P P

f

y-

-

= ◊

= - ◊ ◊(11)

with

( ) ( )( ) ( )( ) ( )( )( ) ( )

1 0

1

1 110

....k k k

kTi i ik

i

I I If m m m

y f b f

-

-

- --=

= - ◊ - ◊ ◊ -

= ◊ ◊Â(12)

The evaluation of the matrices ,k kf y can be done recursive according to:

( ) 1

1 1 1

k k k

Tk k k k k

If m f

y y f b f-

- - -

= - ◊

= + ◊ ◊(13)

with

1 , ,

, , ,

Ta a aa

Na Nak k k k

H S H P

R

b m bf y b m

-

¥

= ◊ ◊ = ◊

Œ(14)

During long term navigation missions, the number Na of states will be in general much smaller than the total number

of states in the global map, that is Na<<Nb<M. The matrixes ,k km b are not full and the calculation of

k kandf y has cost proportional to 2aN . It is noteworthy that Xb, Pab, Pba and Pbb are not needed when the vehicle

is navigating in a local region ‘looking’ only at the states Xa. It will only be required when the rover enter a different

region. The evaluation of Xb, Pbb, Pab and Pba can then be done in only one iteration with full SLAM computationalcost using the compressed expressions.The states Xb can be updated after k update steps using

( ) ( ) ( ), , 0 , 0

11 ,

, , ,

,

kb k b ba

T tk k k a k k k

Na m m Na Nak k k

m Na m ma

X X P

H S z

R z R R

H R S R

q

q q f

q f

--

¥ ¥

¥ ¥

= - ◊

= + ◊ ◊ ◊

Œ Œ Œ

Œ Œ

(15)

Extended Kalman Filter formulation for the Compressed filterIn order to maintain the information gathered in a local area it is necessary to extend the standard EKF formulation.The following equation must be added in the prediction and update stage of the filter

( )

1

1

1

1 1 1

prediction step

update step

k aa k

k k

k k kT

k k k k k

J

I

f fy y

f m fy y f b f

-

-

-

- - -

= ◊ÏÌ =ÓÏ = - ◊Ì = + ◊ ◊Ó

(16)

It can be proved that the computational cost for each ‘compressed’ update will have a cost proportional to Na2. For

more details on the derivation of this algorithm the reader is referred to [15]. When the vehicle makes the transitionto a new area a full update is required to transfer the information acquired in the previous area. The complete updateof the covariance matrix is:

( ) ( ) ( )

( ) ( ) ( ) ( ) ( )

, 1 , 0

, , 0 , 0 1 , 0

ab k k ab

bb k bb ba k ab

P P

P P P P

f

y-

-

= ◊

= - ◊ ◊(17)

The cost of the complete covariance matrix evaluation is approximately 2a bN N◊ . Provided that the vehicle remains

for a period of time in a certain area, the computational saving will be considerable.

Map AdministrationThe next problem to address is the determination of the local areas. One convenient approach consists of dividingthe global map into rectangular regions with side lengths at least equal to the range of the external sensor. When thecar navigates in the region r the compressed filter considers that the active states group, XA includes all the statesrelated to landmarks that belong to region r and its neighbouring regions, as shown in Figure 10 . This implies thatthe local states belong to 9 regions, each of size equal to the range of the external sensor. The vehicle will be able tonavigate inside the region using the compression filter and a full update will only be required when the vehicleleaves the central region r. The method will be extremely efficient in applications with high frequency externalsensors or where the vehicle navigates for long periods of time within local areas.

r

Figure 10 Local area for the compressed algorithm

3 NON MODEL BASED NAVIGATIONA kinematic vehicle model can be unreliable in outdoor environments and particularly in off-road terrain. In suchcases wheel slip and vehicle dynamics can be a predominant factor in many maneuvers making the prediction ofvehicle pose very inaccurate. In some situations this can lead to data association failure and possible divergence ofthe navigation algorithm.

Many navigation techniques require the identification of artificial or natural landmarks in order to update theposition of the vehicle. Data association is the process of relating features observed in the environment to featuresviewed previously or to features in a map. Correct association is essential for mobile robot navigation as it allowsthe robot to determine its location relative to the features it observes. This is a trivial problem when the vehicle ismoving at low velocities in relation to the frequency of the external observations, as new observations will notpresent major differences and the features can be associated in a reliable manner. In most outdoor applications,however, the vehicle can move significant distances before the new set of observations become available. The dataassociation algorithm now requires a model to predict the pose of the vehicle that corresponds to the time stamp ofthe new observation.

Currently feature observations are treated as isolated events and are mapped to a particular tracked feature if thelocation of the observed feature is sufficiently close to the predicted location of the tracked feature. Validation ofthis data association is determined by the error between predicted and observed locations using a threshold bound orobservation gate. The size of this gate is determined as a function of the tracked feature covariance and vehicle posecovariance − typically within a Kalman Filter framework. Any observation that falls outside of a tracked feature'sobservation gate is rejected as an observation of that feature [16]. In the case where an observed feature falls withinthe observation gate of more than one tracked feature, the observed feature must either be rejected entirely or somemore sophisticated method, such as Multiple Hypothesis Tracking [17], may be used to maintain more than onepossible data association.

The problem with traditional data association methods is their reliance on knowledge of the vehicle pose estimate. Ifthe quality of the pose estimate deteriorates, these methods can become quite brittle, particularly in clutteredenvironments. For sensors capable of obtaining batch observations, however, it is possible to constrain dataassociation in a manner decoupled from the pose estimate. Batch observations are a set of observations (i.e., viewedfeatures) sensed simultaneously or within such a short time-span that motion compensation can offset any geometricdistortion. These observations enable data association to be performed on the basis of the relative geometry betweenfeature locations [18]. It is assumed that in each observation there will be noise and dynamic obstacles, which willresult in false features. There will also be valid features that are not common to both data sets due to their differentviewpoints. The aim, therefore, is to find a one-to-one mapping of the features common to both data sets. This isdone by selecting the largest subset of features where the geometric constraints between features are mutuallysatisfied.

The problem of mapping the common features from two feature sets is equivalent to the graph theoretic problem offinding the Maximum Common Subgraph (MCS) between two graphs [19]. In this work, we transform each data setinto a graph by equating features to graph vertices and the geometric relationships between features to graph edges.It is then a matter of solving the MCS problem to obtain the combinatorially optimal common feature mapping. Thegraph-based data association method presented here has several advantages for the feature-tracking problem:

• The matching process depends only on the relative geometry of features observed in a batch and istherefore independent to vehicle pose errors. This also means that the relative pose between the data setscan be determined without previous pose information or tracking.

• A correct mapping between the data sets is guaranteed if the set of features common to both data setsoutnumbers the set of features that generate some coincidental false mapping through geometric symmetry.False mappings through coincidental symmetry is the weakness of this method but the likelihood of failuredecreases greatly with the number of associations found. When this method is applied in conjunction withvehicle pose constraints, false symmetry mappings become extremely unlikely.

• As feature association is performed externally to the (Kalman) filter framework, the filter can simply usethe results of this algorithm to update the corresponding tracked feature estimates without having to dealwith data association.

Data Association Without Tracking: Maximal Common Subgraph

Performing data association between two batch observations as a Maximal Common Subgraph (MCS) problemdecouples the process from the vehicle pose estimate. This algorithm relies on the fact that a set of static featureswill possess invariant inter-feature relationships (e.g., distance between two points or angle between two lines). Notethat the inter-feature relationships are arbitrary and need not be spatial. They may be any invariant property that willapply some constraint between two features. The feature information is characterized as a graph where the featuretypes and their various properties are stored as graph nodes, and the graph edges represent the inter-featurerelationships. Thus, data association between two feature sets can be expressed as the graph-theoretic problem offinding the MCS between two graphs. This results in the maximum subset of features where all the inter-featureconstraints are mutually satisfied. The algorithm has the following steps:

• Feature graph generation.

• Correspondence graph generation.

• Maximal clique search.

The algorithm for converting the MCS search into a maximal clique problem is presented in [20]. Alternative MCSalgorithms include back-track search [21], and neural network search [22].

Feature graph generation

A feature graph is created by defining each feature as a node and the geometric (or otherwise) relationship betweentwo features as a graph edge. To demonstrate the generation of feature graph from geometric relationships, anexample is taken from [19] where the feature types are points and lines and the inter-feature geometric relationshipsare as follows:

• Point to point distance

• Point to line perpendicular distance

• Line to line subtended angle

A geometric representation of a set of features is shown in Figure 11 and its equivalent graph representation isshown in Figure 12 . (For clarity, most of the inter-feature relationships are not labeled.)

D24

D13

D14

⊥12

⊥31

⊥42

α12

L1

P4

L2

P3

P2

P1

Figure 11 Geometric representation of a set of features

D12

⊥12

α12

P1 P2

P3

P4

L2

L1

Figure 12 Graph representation

Correspondence graph generation

Having made two feature graphs, with their node and edge labels, the creation of a Correspondence Graph (CG),Figure 14, may be considered at a higher level of abstraction than the data association problem. For two labeledundirected graphs, a CG represents the possibility of a node from one graph being identical to a node from thesecond [20]. Consider, for example, the two graphs in Figures 11 and 12 . Based on the node labels, node A1 iscompatible with B1 and B2 (i.e., they have label I). Each possible node pairing between the two graphs forms a nodeof the CG in 13.

1{I} 2{I}

6{K}

5{K} 4{J}

3{J}

h

jn

g

b

a

d

mi

c e

k

f

lj

1{I} 2{I}

5{K}

4{K}

3{J}

o

i

a

e

i

g

hj

h

d

Figure 13. Feature Graphs A & B

An edge in the CG indicates that two sets of pairings are mutually compatible. For example, the edge connectingnode 1 to node 4 in Figure 13 indicates that if A1 maps to B1 then it is possible for A2 to map to B2. Conversely, ifno edge connects two sets of pairings, they are mutually exclusive. CG edges are generated by matching edge labelsbetween the two graphs as follows:

• For an edge connecting two same labeled nodes, an edge match between the two graphs will result in twoedges being added to the CG. For example, the edge connecting A1 to A2 matches the edge connecting B1to B2 but, as these node pairs have the same label, it is unknown whether A1 maps to B1 or A1 maps to B2

Graph A Graph B

and so forth. This dual possibility results in edges connecting node 1 to node 4 and node 2 to node 3 in theCG.

• For an edge connecting two different node types, an edge match will result in a single edge being added tothe CG. For example, the edge connecting A2 to A3 matches the edge connecting B1 to B3. This results inan unambiguous mapping of A2 to B1 and A3 to B3, which connects node 3 to node 5 in the CG.

When dealing with real systems, edge matching is clearly not going to involve discrete labels (e.g., a, b, c, etc) butthe values of certain properties, such as distance, with variance bounds to limit the matching range.

Maximal clique search

A clique is a complete (i.e., fully connected) subgraph within a graph. In the case of the CG, finding the maximalclique means finding the maximum set of node pairings that are mutually compatible. This is equivalent to findingthe MCS between two graphs. The maximal clique for our example CG is shown in Figure 15 indicating themappings {A1 to B2}, {A2 to B1}, {A3 to B3}, and {A5 to B5}.

Finding the maximal clique of a general graph is known to be an NP complete problem [23], but given that the CG isusually sparse, a computationally tractable solution should be attainable for reasonable sized feature sets. A recentsurvey of maximal clique search methods [24] indicates the existence of fast approximate algorithms that wouldfurther extend this range.

1{A1,B1} 2{A1,B2}

5{A3,B3}

4{A2,B2}

3{A2,B1}

6{A4,B3}7{A5,B4}

8{A5,B5}

9{A6,B4}

10{A6,B5}

Figure 14. Correspondence Graph of A & B

1{A1,B1} 2{A1,B2}

5{A3,B3}

4{A2,B2}

3{A2,B1}

6{A4,B3}7{A5,B4}

8{A5,B5}

9{A6,B4}

10{A6,B5}

Figure 15. Maximal Clique of CG

Sensor Based Dead Reckoning

Odometry has long been the mainstay of mobile robot dead-reckoning but there are situations where odometricinformation can be very unreliable. This is particularly the case with non-holonomic vehicles in outdoorenvironments. The first concern arises with a vehicle's kinematic motion model since errors in the kinematic modelresult in systematic biases in the odometric motion estimate. A typical Ackerman steered vehicle requires accurately

calibrated wheel alignment and steering angle, and a precise estimate of the wheel radius and wheel-base. Wheelradius is particularly difficult to estimate as it can change over time with wear or load changes.

The second problem is the non-kinematic dynamics of the vehicle motion due to slip. The amount of slip isdependent on the speed and steering angle of the vehicle and is particularly an issue with outdoor vehicles wheredifferent surfaces will produce varying degrees of slip. Also, some kinematic systems are more prone to slip thanothers. For example, articulated skid-steered vehicles, such as a mining LHD vehicle [25], have very large amountsof slip induced when turning.

An alternative to odometric dead-reckoning is external sensor based dead-reckoning where observations arecompared sequentially to obtain a relative motion estimate. We define external sensors as sensors capable of directobservation of the surrounding environment, such as range and bearing laser, radar, stereo vision etc. This methodhas the advantage of being free from vehicle models so that estimates do not suffer from model dependent errors. Asecond advantage is that the dead-reckoning system is a self-contained module and can be transferred from onevehicle to another without modifications. Some disadvantages of sensor-based dead-reckoning are its reliance onregions with observable static features and the possibility of tracking failures through bad data associations ortracking moving objects. Tracking failure through lack of features is easily detectable but incorrect data associationsand mistaking slow moving objects for static ones are often very difficult to detect. They can fail subtly and givemotion estimates that are reasonable and consistent but wrong.

Clearly a desirable compromise would be the redundant use of odometry and sensor based dead-reckoning so that afused estimate could be obtained during normal operation and discrepancies could signal fault situations.Determining the source of fault (i.e., tracking failure or slip) is a more difficult issue. Frequency domain faultdetection [26] prescribes the need for a third sensor (e.g., inertial platform) to statistically determine the fault source.

In the absence of a vehicle model, a point-mass inertial model is used to predict the motion of the sensor betweenscans. The change in pose estimate and its covariance is determined by a Kalman filtered mapping of the two featuresets. The covariance information is then propagated to give an accumulated dead-reckoning uncertainty as shown inFigure 16.

newpose

Prev.pose

base coordinate frame

D

φθ

Figure 16. Covariance propagation after a dead-reckoning increment in pose

The equations for covariance propagation are defined below:

( 1| ) ( | ) ( )

1 0 sin( ) cos sin 0

0 1 cos( ) sin cos 0

0 0 1 0 0 1

T TP k k JP k k J TQ k T

D

J D T

φ θ φ φφ θ φ φ

+ = +− + −

= + =

(18)

Whether used to complement odometry or as a stand alone information source, sensor-based dead-reckoning is auseful and accurate means of motion estimation particularly in outdoor environments where slip and other dynamicsmay invalidate the assumptions of the kinematic vehicle model. In regions with sufficient static features, it providesan estimate of accumulated change in pose and pose uncertainty without reference to a vehicle model.

4 EXPERIMENTAL RESULTSThe navigation algorithms presented were tested in a variety of outdoor environment shown in Figure 4 , 5 and 7using a standard utility vehicle. This work presents the results obtained navigating in Victoria park, Figure 7 . Thevehicle was started at a location with known uncertainty and driven in this area for approximately 20 minutes.Figure 17 presents the vehicle trajectory and navigation landmarks incorporated to the relative map. The systembuilt a map of the environment and localized itself. The accuracy of this map is determined by the initial vehicleposition uncertainty and the quality of the combination of dead reckoning and external sensors. In this experimentalrun an initial uncertainty in coordinates x and y was assumed. No GPS information was used in this run.

Figure 17 Vehicle trajectory and landmarks

Figure 18 shows the deviation of the vehicle position and selected landmarks. The compressed algorithm wasimplemented using local regions of 30 meters. These regions are appropriate for the laser range sensor used in thisexperiment. Figure 19 shows part of the trajectory of the vehicle with the local area composed of 9 squaressurrounding the rover. The local area will move with the vehicle. To verify the fact that the algorithm proposedmaintains and propagates all the information obtained, the history of the covariances of the landmarks werecompared with the ones obtained with the full SLAM. Figure 20 presents a time period evolution of a fewlandmarks’ standard deviation. The dotted line corresponds to the compressed filter and the solid line to the fullSLAM. It can be seen that the covariance of some landmarks are not updated continuously in the compressed filter.These landmarks are not in the local area. Once the vehicle makes a transition the system updates all the landmarksperforming a full SLAM update. At this time the landmarks outside the local area are updated in one iteration andbecome exactly equal to the full SLAM. Figure 21 presents the difference between full SLAM and compressed filtercovariance landmarks. It can clearly be seen that at the full update time stamps the error becomes zero asdemonstrated in this paper.

0 200 400 600 800 1000 1200 1400 1600 18000

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

subsamples

met

ers

deviation of Xv,Yv and (x,y) of landmarks:[3][5][10][30][45]

Figure 18 History of selected state’s std deviations

Figure 19 Vehicle and local areas

Figure 20 Landmark standard deviation for full Slam and compressed filter

240 260 280 300 320 340 360 380

-5

-4

-3

-2

-1

0

1

x 10-3 Compression Filter - Full Slam Comparison

Time

Cov

aria

nce

Diff

eren

ce

Full Update

Figure 21 Covariance differences for full slam and compressed filter

Laser Based Dead Reckoning

Batch observations from a scanning range laser were compared sequentially and, using the MCS data associationalgorithm, the change in pose of the vehicle over two scans was obtained. By accumulating the change in poseincrements, a motion estimate can be evaluated.

A comparison of odometry and laser-based dead-reckoning using the Victoria park data is presented. Figure 22shows the estimated vehicle trajectory derived from wheel and steering encoder odometry. The considerable poseerror, when compared with the SLAM ground truth given in Figure 17 , is mostly due to non-linearities and wheelslip at high steer angles. The laser-based dead-reckoning performance shown in Figure 23, however, is unaffected bythese factors and displays remarkable accuracy over a significant period of time. This is an important result for off-road terrain applications where the kinematic model of the vehicle will be very inaccurate for predicting the vehiclepose.

Figure 22. Encoder dead-reckoning

Figure 23. Laser dead-reckoning

5 CONCLUSIONThis paper presented a review of different high integrity navigation techniques that are in use in several industrialoutdoor applications. The extension of this techniques to off-road application is also presented. In particular, optimalalgorithms for Simultaneous Localization and Map Building were demonstrated with experimental results in outdoorapplications using natural features. Finally a data association method based on graph theory was presented. This isof fundamental importance since it does not require a model of the vehicle to operate. It enabled the implementationof sensor based vehicle motion estimation that can, in many applications, outperform standard dead reckoningapproaches based on kinematic models.

REFERENCES[1] Stentz A., Ollis M., Scheding S., Herman H., Fromme C., Pedersen J., Hegardorn T, McCall R., Bares J., Moore

R., “Position Measurement for automated mining machinery”, Proc. of the Int. Conf. on Field and ServiceRobotics, August 1999.

[2] Hugh F. Durrant-Whyte. "An Autonomous Guided Vehicle for Cargo Handling Applications". Int. Journal ofRobotics Research, 15(5): 407-441, 1996.

[3] Nebot E., "High Integrity navigation for Autonomous Systems", Advances in Intelligent Autonomous Systems,Chapter 11, pp. 245-266, ISBN 0-7923-5580-6, March 1999, Kluwer Academic Publishers, Dordrecht.

[4] Nebot E., “Different type of GPS implementation for Autonomous Navigation”, Journal of Latin AmericanResearch, Vol 29, (3-4), p 183-190, Sep. 1999.

[5] Sukkarieh S., Nebot E., Durrant-Whyte , “A High Integrity IMU/GPS Navigation Loop for Autonomous LandVehicle applications”, IEEE Transaction on Robotics and Automation, June 1999, p 572-578.

[6] Nebot E., Durrant-Whyte H., “High Integrity Navigation Architecture for Outdoor Autonomous Vehicles”,Journal of Robotics and Autonomous Systems, Vol. 26, February 1999, pp. 81-97.

[7] Castellanos J., Tardos J, Schmidt G., “Building a global map of the environment of a mobile robot: theimportance of correlations”. Proc. of IEEE Int. Conf. On Robotics and automation,, New Mexico, USA, 1997,pp. 1053-1059.

[8] Smith R., Self M., Cheeseman P., “A stochastic map for uncertain spatial relationships”, 4th InternationalSymposium on Robotic Research, MIT Press, 1987.

[9] Motalier P., Chatila R., “Stochastic multisensory data fusion for mobile robot location and environmentalmodelling”, In Fifth Symposium on Robotics Research, Tokyo, 1989.

[10]Thrun S., Fox D., Bugard W., “Probabilistic Mapping of and Environment by a Mobile Robot”, Proc. Of 1998IEEE, Belgium , pp 1546-1551

[11]Gordon N., Salmond D, Smith A., “Novel approach to non-linear/ non-gaussian Bayesian state estimation.” IEEProc, V. 140, No. 2, 107-113, 1993

[12]Burgard W., Cremers A.B., Fox D., ¨ Ahnel D. H, Lakemeyer G., Schulz D., Steiner W., and Thrun S..“Experiences with an interactive museum tour-guide robot”, Art. Int.,114(1-2):3–55, 1999.

[13]Peter S. Maybeck, Stochastic Models, Estimation, and Control. Vol. I. Academic Press, New York, 1979.[14]Guivant J., Nebot E., Baiker S., “Localization and map building using laser range sensors in outdoor

applications”, J. of Robotic Systems., Vol. 17, Issue. 10, 2000, pp 565-583

[15]Guivant J., Nebot E., “Algorithm Optimizations for Real time Implementation of Simultaneous Localizationand Map Building”, Submitted IEEE, www.acfr.usyd.edu.au

[16]Scheding S., Dissanayake, Nebot E., Durrant-Whyte H., “An Experiment in Autonomous Navigation of anUnderground Mining Vehicle”, IEEE Transaction on Robotics and Automation, Vol. 15, No 1, February 1999,pp. 85-95.

[17]Reid D. B, "An Algorithm For Tracking Multiple Targets", IEEE Transactions on Automatic Control, vol. 24,no. 6, December 1979.

[18]Neira J., Tardos J. D., "Robust and Feasible Data Association for Simultaneous Localisation and MapBuilding", IEEE Int. Conference on Robotic and Automation, Workshop , San Francico, USA , April 2000.

[19]Bailey T., Nebot E., Rosenblatt J., Durrant Whyte H., “Data Association for Mobile Robot Navigation: A GraphTheoretic Approach”, ICRA 2000, SF, USA, April 2000.

[20]Chen C. K., Yun D. Y. Y., "Unifying Graph Matching Problems with a Practical Solution", InternationalConference on Systems, Signals, Control, Computers, Sept. 1998.

[21]McGregor J. J., "Backtrack Search Algorithms and the Maximal Common Subgraph Problem",Software−Practice and Experience, vol. 12, pp. 23-34, 1982.

[22]Shoukry A., Aboutabl M., "Neural Network Approach for Solving the Maximal Common Subgraph Problem",IEEE Transactions on Systems, Man, and Cybernetics, vol. 26, no. 5, pp. 785-790, 1996.

[23] Karp R. M., "Reducibility Among Combinatorial Problems", in Complexity of Computer Computations, R.E.Miller and J.W. Thatcher (eds.), Plenum Press, New York: 85-103, 1972.

[24]Bomze, I. M., Budinich, M., Pardalos, P. M., and Pelillo, M.: `The maximum clique problem', Handbook ofCombinatorial Optimization (Supplement Volume A), in D.-Z. Du and P. M. Pardalos (eds.). Kluwer AcademicPublishers, Boston, MA, 1999.

[25] Madhavan R., Dissanayake M. W. M. G., Durrant-Whyte H. F., "Autonomous Underground Navigation of an LHD using aCombined ICP-EKF Approach", Proceedings of the 1998 IEEE International Conference on Robotics and Automation, vol.4, May 1998.

[26]S. Scheding, E. M. Nebot, and H. Durrant-Whyte, “High integrity Navigation using frequency domaintechniques”, In press, IEEE Transaction of System Control Technology.