An outdoor navigation system using GPS and inertial platform

Document Sample
An outdoor navigation system using GPS and inertial platform Powered By Docstoc
					2001 IEEUASME InternationalConference on
Advanced Intelligent Mechatronics Proceedings
6-12 July 2001 Como, Italy

                           An outdoor navigation system using
                               GPS and inertial platform
                                                S. Panzierit   F. PascucciS      G. Ulivit
               tDipartimento di Informatica e Automazione                 SDipartimento di Informatica e Sistemistica
                    Universith degli Studi "&ma n e "                        Universita degli Studi "La Sapienza"
               v i dell8 vasa Navale 79, 00146 &ma, Italy                   Via Eudossiana 18, 00184 &ma, Italy

     Abstract-The use of Global Positioning System (GPS) in       Outdoors, the Global Position System (GPS) is an in-
  outdoor localization i a quite common solution i large en-
                        s                            n         teresting possibility to cope with these problems without
  vironments where no other references are available and p-
  sitioning requirements are not so pressing. Of course, fine  adding special devices as reflectors or targets. Till May
  motion without the use of an expensive differential device   1st of this year, the available precision was downgraded for
  is not an easy task even now that available precision has    military reasons, so that only differential receivers could
  been greatly improved as the military encoding has been      be used for localization; now the full satellite precision can
  removed. In this paper we present a localization algorithm
                                                               be achieved with a simple, cheap unit. It requires very lit,
  based on Kalman filtering that tries to fuse information corn-
  ing from an inexpensive single GPS with inertial and, some   tle data processing and its error is independent from the
  times uncertain, map based data. The algorithm Is able       traveled distance and from the positions of the obstacles.
  to produce an estimated configuration for the robot that
  can be successfully fed back in a navigation system, leading The problem is, therefore, to characterize the performance
  to a motion whose precision is only related to current in-   of this system in different situations and t o adapt the lo-
  formation quality. Some experiments show di5culties and      calization and the navigation systems to get the maximum
  possible solutions to this sensor fusion problem.
                                                               benefit from its use.
                                                                  The paper is devoted to analyze the localization system
                       I. INTRODUCTION                         implemented for an ATRV-Jr unit that has t o move in a
                                                               parking lot on a paved but rather rugged terrain. After
    Outdoor navigation is an exciting and quite varied topic; a brief description of the available exteroceptive sensors,
  there are several types of environment that require differ- the accuracy of the GPS is discussed in some detail on the
  ent levels of autonomy and different kind of sensors. A basis of some experiments. Indeed, this is the most im-
  vehicle traveling in a wood shows requirements that are portant sensor for large movements in open spaces, when
  not needed by one exploring a parking lot; both, however, no other reference is available and its short-term error af-
  are doing outdoor navigation. In this paper we shall ad- fects the implementation of the entire navigation system.
  dress the latter situation that seems to be more promising The localization system is based on an Extended Kalman
  for applications; surveillance, cleaning, moving goods, and Filter, it is described giving emphasis to the peculiarities
  even searching for a specific item (a car, a container) are arising from characteristics of the used sensors. Finally,
  operations that can be, in a near future, assigned to out- some guidelines are given to design the planner according
  door robots. By the way, such units could be freed from the to the EKF achievable performance and the nature of the
  usual energy shortage, because they can adopt fuel instead environment. The description of an experimental result
  of batteries as the primary power supply.                    concludes the paper.
    Even in this rather simple environment, problems are
  very different from those encountered in indoor navigation;        11. PERFORMANCE OF A SIMPLE G P S SYSTEM
  indeed, the required sensors must be different; as an exam-     A GPS receiver relies on the signal received from several
  ple, ultrasonic sensors cannot be used, being them affected satellites that are not geostationary. Therefore, the number
  by rain and wind and having them, anyway, a too short and the position of the available satellites change in time
  range. However, the greatest difference is the kind of a ipfluencing the system precision. Several information are
  priori knowledge that can be used for localization, a skill available on sources of GPS errors; often they refer to long-
  required also outdoors. Indeed, the typical environment term averages and/or to high quality receivers that cannot
  features used indoors for this purpose are quite scarce out- be employed on a mobile robot.
  doors, moreover, large, stationary obstacles can appear and     To understand the level of precision and the error be-
  disappear: consider, e.g., the effect of some cars parked in haviour when using an inexpensive receiver (we used the
  front of a wall. When perceived by the exteroceptive sen- Garmin GPS35-HVS), some experiments have been con-
  sors, they can either be interpreted as the wall itself (and ducted in the parking lot of our department. We chose two
  therefore produce an erroneous localization) or cause the places, say points A and B; the first was far from obstacles
  distance measures to be discarded (so reducing the amount affecting the wave propagation, the other was in a sort of
  of available information).                                   corridor, delimited by two buildings. The robot was placed

0-7803-6736-7/01/$10.000 2001 IEEE                               1346
still in the two points where it acquired data in different
hours of the day. All the acquisitions lasted 15 minutes,
during which measures were collected with a sampling p e                                                Satellite         Firat                 Second             Third
riod of 1 second, resulting in 900 measures per acquisition.                                            number          muisition           acquisition          acquisition
The first two tables refer the satellite availability in the two
points. As it can be seen, the number of available satel-
                            TABLE I
                          AVAILABILITY                    AT POINT        A

                                                                                                    I       Total   I   3.0   I   7.9   I   2.4    I   0.9   I   10.8   I   9.2   I
         number          acquisition          acquisition                percentage

              6      1        197         I          127         1          18.0
              7      1        685         I          282         I         53.7
              a      1        18          I      427             I         24.7

  Satellite         is
                   Frt               Second                 Thud                 Overall
  number      acquisition          acquisition            acquisition           percentage
     3              180                0                      0                     6.6
     4              401                  298                  121                   30.3
     5              319                  602                 697                    59.9                I     I
     6               0                   0                       82                 3.0                      - 4 - 2              0         2          4         6          8

                                                                                              Fig. 1.       First acquisition at point A. Number of satellite: dot 4,
lites is quite different in the two points, being the most                                       plus 5, circle 6
probable value 7 in the open location and 5 in the partially
covered one. Note that 4 satellites is the minimum number
for a complete 3D localization.                                                               plotted with different symbols according to the number of
   The next two tables relate the Standard Deviation of the                                   satellites.
measures for each acquisition, computed according to the                                         As it can be seen, there is a lack of resolution in one
number of satellites and for all the measures.                                                direction, because the available satellites are aligned. Only
   Observing these tables, it easy to understand that the                                     the addition of the 6-th satellite, which probably was not
number of satellites provides just a very rough indication of                                 aligned, allows a remarkable increase of precision, as it can
the accuracy of the measure (look, in particular, at row 5 of                                 be shown by the positions of the circles.
Tab. 11). Indeed, a different indicator should be available,                                     Another important point is the way the error changes in
the Dilution of Precision (DOP), which is based on the                                        time, i.e., if it is white or colored. A fast changing error can
satellite geometry in sky, but the installed driver we have                                   be better filtered, but its residue adds some random motion
does not return this information. This is shown by the                                        to the one desired for the robot. On the contrary, a slow
Fig. 1 and Fig. 2 , where the two acquisition in point A are                                  varying one cannot be removed in short time, but allows
                                                                                              for a smoother movement; moreover it should be considered
                          TABLE 1 11                                                          that we are interested in the relative position accuracy, as
                        DEVIATION (M)                     AT POINT         A                  here and there the robot finds some environment feature
                                                                                              to improve its localization.
                  satellite   I      First            1      Second             I                An easy way to look at the phenomenon is to plot the
                  number          acquisition              acquisition                         “path” of the measured position for a given number of satel-
                                  EWIS-N                  EWIS-N                              lites. Looking at Fig. 3 (obtained for 5 satellites), it is easily
                     4                    I -              1.0       I    3.5                 understood that successive errors are tightly related, and
                     5                                I    1.2       I    3.6   I             therefore the error is strongly colored, in particular when
                     6             2.3         1.1         1.9            4.2                 the errors are large. A better characterization is shown by
                     7             2.1         1.7         2.6            4.1                 the auto-correlation function that is not reported here for
                     8             0.4         0.0         1.0            2.3                 sake of space. Moreover, in this case, there is also a strong
                   Total      I    2.2    I    1.7    I    2.3       I    4.2                 correlation between E W errors and N-S errors. jFrom
                                                                                              these considerations we can conclude that changing the c&


                                                                                        prediction          prediction

            -20         -15     -10    -5       0   5   10    15
                                                                                                     Z -l
Fig. 2. Second acquisition at point A. Number of satellite plus 4,            Fig. 4.
                                                                                -      Structure of the localization system   (2-l   denotes the unit
    dot 5, circle 6                                                               delay operator)

                                                                              the Cartesian coordinates of the vehicle center and 4 is the
                                                                              mobile platform orientation with respect to the x-axis.
                                                                                 The on-board sensory system includes two incremental
                                                                              encoders measuring the rotation of each motor, a inertial
                                                                              navigation system that provides measures of the robot lin-
                                                                              ear accelerations and angular velocities, a laser scanner
                                                                              which measures the distances between a fixed point on the
                                                                              robot body and obstacle surfaces in the environment, and
                                                                              a GPS antenna measuring absolute position in the geodetic
                                                                                 The structure of our localization system, shown in Fig. 4,
                                                                              proceeds directly from the EKF equations [2]. At the k-th
                                                                              step, the kinematic model of the robot is used to compute
                  -15         -10     -5        0   5    10        15
                                                                              an inertial prediction 5 k l k - l and an associated covariance
                                                                              matrix P k l k - 1 ; an observation prediction &k is formed and
 Fig. 3. Second acquisition at point A. Error path for 5 satellites
                                                                              compared with the measure z k provided by the sensory
                                                                              system. The results are the innovation term Vk and its
variance matrix of the GPS according to the number of                         covariance matrix s k , that are used by the EKF to produce
satellites is of little or no help in improving the overall p r e             the state estimate 2 k and the new associated covariance P k .
cision, whilst this could be done if the satellite geometry
were available.                                                               A . Inertial prediction
   Moreover, a the error is slow varying, approaching once
in a while some known environment feature can reset the                          Define the state vector as x = (P2,pY,~$,w,b)   where v
overall error and keep it bounded at a d u e lower than the                   is the linear velocity of the vehicle and b denotes the ac-
expected one.                                                                 celerometer bias [5]. The rate gyro bias is constant, so we
                                                                              estimate it only at the beginning of the navigation (see [8]
                  111. THELOCALIZATION              SYSTEM                    for an algorithm estimating gyro bias). The inputs of the
                                                                              inertid model are ?Lk = (az,wf        where a: is the mean
 We consider a mobile robot with differential drive kine                      robot acceleration along the motion direction and w i is the
matics [6],described by the following equation:                               mean angular velocity during the k-th sampling interval.

                  (i)                               (H)..               (1)
                                                                              The inertial prediction is then

   The motion is generated by four tires, actuated by two
independent motors. The robot location (i.e. robot con-
figuration [3]) is given by z = (pz,pu, q5), where p”,pu are

where  &  = &1+      (1/2)w+,Atk is the average vehicle ori-
entation during the sampling interval Atk. The covariance
matrix associated with the prediction error is written as                 Fig. 5. Holonomic and nonholonomic velocities

                                                                      The measurement noise depends on the interaction be-
                                                                   tween the sensor system and the environment, which is
                     J,f (uk)c (J,f( U k ) ) T Q*            (3) not invariant in space and time. For example some sur-
Here, J l ( . ) and ,Ti(.) the Jacobian matrices off(-) with faces (glass or hedges) are more difficult t o detect for laser
respect to 5k-1 and U k , Pk-1 is the covariance matrix at sensor. To model this fact, we have chosen to label each
time instant t k - 1 , C = diag{u,2,,o&) is the covariance segment of the map with a value representing the relia-
matrix of the gaussian white noise which corrupts the in- bility of the laser measure provided by that segment, and
put measure and Q = diag{og,, U&, .&U:,             ,}
                                                   ." is the CO- to modify the corrispondent rows of the covariance matrix
variance matrix of the gaussian whte-noise which directly R accordingly [ ] Also the GPS rows of R are modified
affects the state in the kinematic model.                          by the number of satellite that the antenna uses at the is
                                                                   tant time t k , because the sensor accuracy depend on their
B. Observation prediction and matching                             number and position.
   The observation prediction consists of subvectors 2; (pre-         Moreover to avoid the inclusion of outliers in the cor-
dicted encoder velocity), 2: (predicted laser readings), 2' ;      rection phase, we need to set up for the laser measure a
(predicted robot position by GPS). For the observationpre- validation gate as follows [I]. Suppose that vk,j is the in-
diction a;, we simply let                                          novation term for a laser ray; it will be passed t o the EKF
                                                                   only if
                    4 = he(Xk/k-l) = &/k-l.                  (4)
   The observation prediction of the laser mearmre is com-
puted over a given environment map M and the inertial where sj is the j-th diagonal term of S and parameter 7
prediction                                                         should be tuned by experimental trials. In the same way
                                                                   GPS measure obtained using less than four satellite are
                            = h'(%k/k-i, M ) .               ( 5 ) discharged.
We shall not detail here the above computation. It should
be noted that h reflects the way in which the environment C. Extended Kalman Filter
map is represented - in our case, a list of segment [4] -             The Ektended Kalman Filter is used to correct the in-
as well as the model interaction between the environment ertial configuration estimate one the basis of the validated
itself and the laser scanner. The subvector 2 is written observations. Particularly, the final configuration estimate
as                                                                 is obtained as

In (6),we consider a conversion of the GPS measure from          where Kk is the Kalman gain matrix
geodetic coordinates to the environment framework.
  The innovation term and the associated covariance, be-
ing & = (4,ak, ~ p "and Z k the measured outputs, are
                        )                                        The covariance associated with the final configuration e s
then computed 8s                                                 timate & is given by

where J,"(-) is the Jacobian matrix of h = (he,h', h9pS)                       Iv.
                                                                                PLANNING AND CONTROL
respect to xk/k-1. The first term used to compute s k r e p    As explained before, the GPS provides the position of
resents the uncertainty on the observation due to the un- the robot with a maximum error that can be as high as 20
certainty on the inertial prediction. The second term is the meters, even if generally this is quite lower. If the trajeo
observation noise covariance matrix.                         tory proceeds for a long time far from any reference point,

       -I    0
            -3   0   0.5   I          -I   .
                                           U    o    as   I

Fig. 6. Trajectorieswithout (left) and with (right) forward cut o f

the position error can reach that value. If the range of                            Fig. 7. The mobile robot ATEN-Jr
the other exteroceptive sensors is larger than the actual
position error, then this can be corrected when a feature
                                                                      has been set at zero until 11     11 is less than a threshold
appears in the robot field of view. Otherwise, as it happens
                                                                      of 1Odeg. In this case trajectories are more linear and, for
in our case, there is the possibility that the error cannot be
                                                                      this reason, this strategy has been implemented. The only
corrected. This event is uncommon, and its occurrence can
                                                                      point where this control fails is the origin itself where p
be avoided by planning a path that passes near some en-
                                                                      is undefined but to avoid also strong discontinuities in the
vironment feature before the errors grow too large, even if
                                                                      control law, possibly caused by sudden corrections of the
in this way the minimum length property is missed. More
                                                                      estimated position due t o new available laser measures, the
over, if the DOP is available, this behaviour can be acti-
                                                                      via-points have been defined as a circular region of radius
vated only when needed, perhaps by a real time trajectory
                                                                      0.5m and the robot has been forced to halt when it steps
                                                                      into this region.
   Thereafter, the proposed solution has been the one of
specifying the path as a sequence of via-points to be at-                Finally, to take into account the presence of obstacles,
tained using, for example, a simple static timeinvariant              has been added an obstacle avoidance module whose de-
feedback control law on each segment. It is well known                scription can be found in [9].
that, for nonholonomic systems lk unicycle robots, the
point tracking problem, i.e., tracking of only Cartesian c e
                                                                                     v. EXPERIMENTAL
ordinates, can be solved using a partial feedback lineariza-             The proposed algorithm has been tested on the mo-
tion of the dynamic model and a simple P D like control               bile robot ATRV-Jr produced by Real World Interface (see
law in the new coordinates [lo]. If we suppose that both              Fig. 7. The robot has four tires, actuated by two inde-
exact tracking along each segment and a particular pos-               pendent motors, so the kinematic model of the platform is
ture at the via-point are not required, the problem can be            a differential drive one. The sensory system is composed
relaxed to the stabilization of only the first two coordi-            by two incremental encoders mounted on the two motors, a
nates of system (1). This could be attained with an easy              180 degrees view laser scanner (LMSSick), a GPS (Garmin
strategy, obtained by simplifying the one presented in [ll]:          GPS35-WS) and the Crossbow 6-axis inertial navigation
first of all (see Fig. 5 ) define a suitable holonomic velocity       sensor. Each sensor is connected to the ATRV-Jr on-board
'uh, unaware of the nonholonomic constraint, able to per-             PC (Pentium II, 350MHz) through a serial port on a Rock-
form the stabilizing task, i.e., able to steer the origin of the      eport multiserial port card. On the same P C run the low
robot frame over the origin 0 of the reference frame; sec-            level control software (the RW's ReFlex accepting speed
ond, define two control laws one as projection of V h along           commands and returning odometric data) and the CORBA
the nonholonomic direction 2),h and the other steering the            servers, that distribute the sensor data over the network.
nonholonomic axis of the vehicle towards the holonomic                The high level control software, written in C++ language,
velocity vector (to reduce p)                                         is a client program that runs on a notebook (Pentium I1 at
                                                                      366MHz) that interacts with the ATRV-Jr using TCP/IP
                                                                      over Ethernet link. The experiment here reported has been
                                                                      conducted in the parking lot of our Department (Fig. 8).
                                                                      The navigation algorithm described in section l has been
   The convergence to the origin can be easily proved con-            implemented using data coming from the localization filter.
sidering that eq. 13b independently steers the angle $ to a
                                                      I                  The robot, starting its path in S (see Fig. 8), remains
value that will definitively let B = 0. Now, using a Lya-             still for 15sec to estimate both the initial bias for the in-
punov candidate V(z, y) =.x2 y2 it is easy to see that its            ertial navigation system and the offset for the GPS carte-
derivative is, when = 0, V = -2Ke(z2 y2), and is neg-                 sian position. In this figure the odometric path (dashed) is
ative defined. At the left of Fig. 6 trajectories generated           extremely inaccurate and rapidly diverges while the GPS
for different starting points are reported. To avoid back             estimate (plus) has a limited error. The output of the fll-
motion a different strategy has been applied and its results          ter (circles) does not coincide with the effective path (not
reported at the right of the same figure: the first control           reported) but its maximum deviation has been of 0.5m.

                                                                       25                       Laser correction

                                                                                Fig. 9. Laser correction on building corner
Fig. 8. Robot path in the Department parking lot: the path by the
    algorithm (circle), GPS (plus) and odometry (dashed line)
                                                                 ment an Extended Kalman Filter for an ATRV Jr robot.
                                                                 The results show that it is very important to have infor-
   After this initialization step, moving with constant speed mation about the Dilution Of Precision (DOP) or about
towards point 1, the robot integrates satellite measures the satellite geometry to estimate its error. This error also
with laser readings but, due to unpredictable obstacles affects the strategy of the planning system: if large errors
(parked cars) the matching with the map is not always are expected the trajectory must pass near some easily rec-
possible. Moreover, in the front direction, walls are too ognizable environment feature, even if this lengthens the
far (more than 8 meters) and the estimated position relies path.
only on GPS with an error that slowly increases. When
the robot approaches point 1, and the laser system starts Acknowledgments
to give a reliable measure that can be matched with the             This work has been partially supported by Murst funds
map, the error is reduced from 0 . b to O.lm. To help the inside RAMSETE project.
Kalman filter, an additional stop of a few seconds is used to
stabilize the estimate configuration. After that, circumnav-                                 REFERENCES
igating the obstacle, the robot moves to via-points 2 and [I]J. J. Leonard and H. F. Durrant-Whyte, "Mobile robot lo&
3. Although the robot, approaching point 3, has no infor-           tion by tracking geometric beacons," IEEE n-Om. on Robotics and
                                                                    Automation, vol. 7 , no. 3, pp, 376-382, 1991.
mation from the laser system and uses only GPS data, its [2] A. C. Gelb, Applied Optimal Estimation, MI" Preas, Cambridge,
position estimate does not appreciably drift towards North:         MA, 1994.
this could be expected due to the big difference with GPS [3] J. C. Latombe, Robot motion planning, Kluwer Academic Pub-
                                                                    lisher, Boston,MA, 1991.
information, but, being the covariance matrix associated [4] J. Gomala,A. Stem, and A. Ollero, "An iconic position estium
to the GPS data greater than P l - ,the filter output is
                                    kk1                             tor for a 2D laser rangefinder," 1992 IEEE Int. Conf. on Robotics
not greatly influenced. This dominance, with actual gains,          and Automation, San Diego, CA, pp. 2646-2651, 1992.
                                                                 ( 1 B.Barshan and H.F. Durrant-Whyte, "Inertial navigation system
would last only for about one minute; after this interval           for mobile robots," IEEE Iltons. on Robotics and Automation, vol.
P l - would degrade to the GPS value and the estimate
 kk1                                                                7,no. 3, pp. 376-382, 1991.
would coincide with it.                                          [SI Borenstei, J., Everett, H . R , Wng, L., 'Where am I?'
                                                                    Sensors and Methods for Mobile Robot Positioning lkchni-
   In point 3 the West limit of the parking lot is seen by laser    cal Report, The University of Michigan,? 1996. Available from
system and the longitudinal position is again accurate when         ftp:/,,
the robot starts to move towards the goal G. Along this
                                                                 [q E. Fabrizi, G. Oriolo, S. Panzieri, G. Ulivi "Enhanced Uncer-
                                                                    tainty modeling for robot localization," Proc. of 7th Int. Symp.
last segment a singular adjustment occurs: the particularly         on Robotics with A p p l h t w n (ISORA'98), Anchorage, AL, 1998.
bad latitude estimate is suddenly k e d as the corner is [8] E. Fabrizi, G. Oriolo, S. Panzieri, G. Ulivi, "Mobile robot local-
                                                                    ization via fusion o ultrasonic and inertial sensor," Proc. of 8th
recognized. In the same figure, the magnification in Fig. 9         Int. Symp. on Robotics with Application, Maui, HI, 2000.
shows the discontinuity that has no effect on the control [9]E. Fabrizi, S. Panzieri, G. Ulivi, "An integrated sensing-guidance
law as stated before.                                               system for a robotized wheelchair," 14th IFAC World Congress,
                                                                      pp. 457-462,  Beijing, China,July 5-9,1999.
                                                                    [IO] B. d'And&Novel, G. Champion, G. Bastin, "Control of non-
                      VI. CONCLUSION                                  holonomic wheeled mobile robots by state feedback linearization,"
                                                                      Int. J. of Robotic Reasearch, vol. 1 ,pp. 543-559, 1995.
  The elimination of the Selective Availability from the            [11] M. Aicardi, G Cannata, G. C d i o , G. Indiveri, "On the sta-
Global Positioning System has made outdoor localization               bilization of the unicycle model projecting a holonomic solution,"
                                                                      Proc. of 8th Int. Symp. on Robot& with Application, Maui, H ,   I
available even using inexpensive receivers. In the present            2000.
paper au experimental analysis of its performance is re-
ported and it used, together with other sensors, to impls


Shared By: