Download Navigation system of an outdoor service robot with hybrid

NAVIGATION SYSTEM OF AN OUTDOOR SERVICE ROBOT
WITH HYBRID LOCOMOTION SYSTEM
Jorma Selkäinaho, Aarne Halme and Janne Paanajärvi
Automation Technology Laboratory, Helsinki University of Technology, Espoo, Finland
Abstract: The paper deals with the navigation system of a hybrid service robot intended for outdoor
applications. The system uses different navigation sensors depending the current situation and task
being done. The main sensors are laser range finder, GPS and heading gyro.
Keywords: autonomous navigation, hybrid locomotion, outdoor robotics
1 INTRODUCTION
Service robots intended to work outdoors are
supposed to travel relative long distances on
variable terrain. Navigation requirements are
demanding, because the robot should do
accurate navigation close to the work positions
and long distance navigation when traveling
between them. Navigation system must be
reliable and easy to use by both the user
directly and by other tasks in the software
system.
WorkPartner shown in Fig. 1, is a service
robot presently under development at HUT
Automation Technology Laboratory [ Halme et
al., 2001]. It is a centaur-like general purpose
service robot intended to work in urban
environment. The working area includes
mainly outdoor places, like yards, parks etc,
but also covered areas like big halls or
storages. GPS is a natural positioning system
when available, but because availability is not
always guaranteed and accuracy is not enough
for all needs, other means have been
considered, too. These are a laser range finder,
heading gyro and odometry. Because of the
relatively complex kinematics of the robot
mobility system with different locomotion
modes, odometry is not a trivial thing. Fusing
sensor data from different sensors and
logically
changing
between
different
configurations of the navigation system are the
ways how variable situations can be managed.
Although navigation requirements may vary
during mission, we start from the fact that 3D
pose estimation is mostly needed.
Besides radio beacon system, like GPS
or gsm-network based systems in the future,
other artificial beacon based systems cannot be
considered, because this would restrict too
much the use of the robot. Therefore, only
natural landmarks can be utilized. The robot
has a stereo camera system available, but it
was decided to use a scanning laser range
finder, because distances to suitable structures
can vary much as well as the illumination
conditions from dark to sunshine. Cameras are,
however, used when controlling the tasks of
the two hand manipulator system. The
accuracy requirements depends much on the
tasks included in the mission, but one can say
that a typical accuracy needed for successful
task execution with the two hand manipulator
is few centimeters, and half a meter when
traveling.
2 NAVIGATION UNIT
Workpartner must know its attitude in
order to be able to control stability. The pitch
and roll angles of the front body are measured
by accelerometers.
Fig. 1 The workpartner robot.
In the beginning of the project, a three axial
magnetometer was chosen as the primary nondrifting heading sensor. It has been
successfully used inside a personal car after
iron calibration. However, it turned to be too
sensitive to disturbances originating from
electrical motor currents when installing on the
robot.
Odometry is commonly used on flat
surface navigation. However, the WorkPartner
will operate on difficult surfaces where
conventional odometry is not reliable. Also
changes in locomotion mode causes problems
to make odometry accurate. Therefore,
navigation is based on matching successive
laser scans in the vicinity of buildings and
trees where GPS navigation is not working. On
open areas GPS navigation can be used
whereas laser scanner does not work because
of the lack of targets. GPS and laser scanner
are complementary methods that support each
other. Odometry is considered as an optional
method.
The velocity and heading are measured
moving by using a GPS receiver. Required
velocity is approximately 0.1 m/s. Velocity
and heading measurements are based on
Doppler effect of signals from at least 3 GPS
satellites. The velocity measurement accuracy
is about 0.1 m/s.
When there is less than three satellites
available for the position fix then dead
reckoning based on heading gyro and laser
mapping is used. This is especially the case
when the robot is operating indoors.
Natural landmarks are detected by using a
2D-laser scanner. It is installed in the
manipulator. Other navigation sensors are
installed on a navigation unit. The navigation
unit includes a 400 MHz Pentium computer in
PC104 size. Additionally a 12 bit A/D
converter card in PC104 size is used for
reading the Murata ENV piezogyro and two
accelerometers. The laser scanner and GPS
receiver communicate through 2 serial ports on
motherboard. The operating system used is
QNX. The operation commands to the
computer controlling the articulation angle, leg
movements and wheel rpms are sent through
Ethernet cable.
3 SENSOR FUSION
Indoors or when less than 3 GPS satellites
are on sight the navigation is based on heading
gyro and laser scan matching. On structured
environments wall lines can be identified and
used for navigation [Jensfelt, 1999] and [Pears,
2000]. In our case the maps are made on laser
range measurements from unstructured objects
like bushes and tree trunks. Successive laser
scan matching is used to measure the change in
position and heading. The assumption behind
this approach is that the time interval, and
hence the displacement of the robot, between
successive scan lines is small. This reduces the
ambiguity associated with this type of
approach because scenery changes very little
between successive scans, ensuring high
overlap and limiting effects of occlusion.
Further, for small time intervals the
displacement can be predicted with high
accuracy together with the vehicle model and
GPS based odometry. This information can be
used to constraint the search for matches,
which also decreases ambiguity and the
amount of computation needed.
The laser range finder measures distance
and bearing to an obstacle that is not beyond
30 meters. On every scan total 361
measurements at half a degree intervals are
obtained. The laser range finder measures the
distance and bearing on a coordinate frame that
is fixed on the front part of the robot. When the
robot is moving same obstacles will be seen
from different viewing points. In order to build
a map, a static map coordinate system must be
chosen. A natural choice for the map
coordinate system is the position and heading
of robot front body when it takes the first laser
scan. This means that the 361 ranges and
bearings of a laser scan are transformed from
polar coordinates to cartesian coordinates. A
map consists of these 361 x,y-pairs. The next
laser scan is taken on the front body
coordinates of moving robot. The transition
and rotation of the robot body coordinate
system between two successive viewing points
can be found by matching the corresponding
laser maps in cartesian coordinates. The
matching is based on rotating and translating
the most recent map and then measuring the
goodness of fit between two successive maps.
A hit is obtained if the distance between one
point in previous map and any point in the last
map is less than 0.2 meters. This value comes
from the fact that the resolution of the laser
range finder is 0.26 meters at 30 meters
distance. The number of hits are maximized by
a direct search algorithm. The searched area in
x and y directions is from –0.4 meter to 0.4
meter in 0.2 meter steps around the predicted
position. The searched heading is from –6
degrees to 6 degrees in 1 degree steps around
the predicted heading. The predicted position
and heading are obtained from GPS velocity
and
heading
message.
When
GPS
measurements are not available, heading gyro
is used for predicting the heading. Odometry
can be used for predicting the travelled
distance.
The robot is able to initialize its position
and heading relative to a map made earlier in
the same area. The local initialization map
consists of 361 pairs of x and y coordinates.
The x-axis points to east and y-axis points to
north.
Since the piezogyro drifts a continuous
calibration of the
reference
voltage
corresponding to the zero angular velocity is
needed. At indoor use the heading increment
obtained from map matching is used to update
the reference voltage. At outdoor environment
the GPS heading value is used for reference
voltage calibration. Reference voltage update
is restricted to cases when robot is not making
any fast heading changes. The reference
voltage is initialized in the rest state when the
workpartner has not moved yet.
Odometry can be used for computing the
predicted position. In outdoor environment the
NMEA VTG message is used to compute the
predicted position and heading. The position
estimate drifts slowly and hence the NMEA
GGA message is used to make small
corrections to position estimates. The
correction gain is equal to 0.02/HDOP where
HDOP is horizontal dilution of precision
obtained from GPS receiver. When NMEA
GGA is used the GPS coordinates of the
starting point should be known with some
accuracy.
The route points to goal are picked from a
map (Fig. 5) showing the current working
environment. This map has been joined from
consecutive laser scan lines when the robot has
moved in the working area. When navigating
the robot orientates towards to the current
route point until the distance from current
position to the next route point is less than
distance between current and next route point.
The laser scanner is also used for reactive
obstacle avoidance. When an obstacle appear
at a distance less than 0.2 m from side of
planned trajectory on 9 s forward in time a turn
to other direction is commanded. When an
obstacle appears at a distance of less than 3 m
in front of robot a zero velocity command is
send to the main computer.
The simple navigation model that explains the
movements projected on a horizontal plane
fixed on the center of front body coordinates is
represented (1).
Figure 3 shows the GPS position fixes
obtained at the same route. There exist position
jumps that are over 10 meter. Because metal
plates cover of the university building some
multipath fixes did occur. The route computed
from velocity and heading measurements is
much more accurate than that obtained from
position message (NMEA GGA).
460
450
440
430
420
410
400
390
380
370
180
200
220
240
260
280
Fig. 2 Workpartner
route
in
meters
integrated from GPS velocity and heading.
Tests were also made to evaluate the odometry
information
obtained
from
matching
successive laser scans. The search area used in
finding the match was from –0.5 m to 0.5 m
sideways, -0.6 m to 0.9 m in heading direction
and from –20 degrees to 20 degrees in heading
displacement. The position was searched at 0.1
m steps and heading in steps of one degree.
460
450
440
x1  x4 * sin( x3 )
x 2  x4 * cos( x3 )
430
(1)
420
410
400
where x1 is position to east, x2 is position to
north, x3 is heading and x4 is velocity. The
laser range finder defines origin of the
coordinates.
4 NAVIGATION EXPERIMENTS
Some experiments have been performed at
the vicinity of a three floors high university
office building [Selkäinaho et al., 2000].
WorkPartner robot was driven manually along
a 6 meters wide road. Figure 2 shows the route
computed by integrating the velocity and
heading obtained from a GPS receiver. The
origin of the local coordinates is 60.11 N and
24.49 E. After 308 meters long route the
position drift caused by Euler integration is
equal to -0.5 meters north and 1.7 meters east.
390
380
370
180
200
220
240
260
280
Fig. 3 Workpartner route in meters plotted
from GPS position messages.
After the best estimate for displacement was
found, the estimate was fine-tuned by
minimizing the sum of squared angle and
range displacements of the matched points
with quasi-Newton method. Matching of two
361-point scans took under a half- second on
533 MHz Pentium III. The laser data was
gathered from a route along the side of the
university building, corresponding roughly to
lower portion of the route in figures 2 and 3. A
total of 220 laser scans was gathered and
integrated together. Figure 4 shows the route
obtained from the displacement estimates. One
can see a drift of some meters as the starting
and ending position differ. The environment
included bushes, group of young trees and the
wall of university building.
10
0
-10
-20
-30
-40
-50
-60
-70
-40
-30
-20
-10
0
10
20
30
40
Fig. 4 A route computed by matching
successive laser scans. Place is same as the left
wing of route in Fig. 2.
At the third experiment autonomous
navigation was tested. The robot first
initialized its position and heading by
comparing the laser scanner readings to a map
made by laser scanner earlier in the same area.
The navigation unit read a route map given
in local laser map (Fig. 5) coordinates. The
navigation task was to autonomously move to
a trailer placed about 50 meters away in the
yard and pick a cardboard box and return it to
start position.
The task was started without any GPS
readings because the building blocked many
satellites. Successive laser scan matching was
used. After about one minute the robot was
able to see enough satellites and it was
predicting new poses based on GPS and then
matching successive laser scans until to the
trailer.
computation in real time is required and the
old pose estimates are not updated with new
laser scan data and some drifting error will
accumulate to the pose. However, it is possible
to correct old pose estimates in real time, but
this requires some development work
An exact laser based map could be made by
taking only one laser scan with a long range
model like Riegl 3D laser range finder which
is reaching up to 350 m with 0.17 degrees
angular resolution [Forsman, 2001].
A Sick laser range finder installed in the
manipulator was used to form a map of a
university building neighborhood. The map
build from range measurements from several
viewpoints is named as global map and the
map build from range measurements from one
viewpoint is named as local map. A laser scan
consists of 361 range measurements
corresponding bearing values from 0 degrees
to 180 degrees in laser coordinates. Successive
local map matching was used to compute
incremental change in position and heading of
robot. Then the last local map was transformed
to global map coordinates and compared to the
global map. If the number of common points in
the two successive local maps was over a
predefined threshold then new points last local
map which are at least 0.2 m apart from old
global map points is added to the updated
global map. Spurious points that exist only in
one laser scan are not filtered out from the
map.
The map (Fig. 5) was used to define a
route to the goal of the working task. The map
was in the form of coordinates of occupancy
grid points. Occupancy grid size used in map
building was equal to 0.2 m and only
coordinates of occupied cells were stored.
When the map is matched accurately from
several consecutive scan lines it can be used
for position and heading estimation.
5 LASER MAPPING
A mobile robot can build a map of its
environment when it takes laser range
measurements from natural beacons like
bushes and tree trunks on its route. Map
building requires that the range and bearing
measurements taken from different viewpoints
are rotated and translated to the same
coordinates. In the coordinate transformations
accurate position and heading of the laser
range finder is needed. However, any
cumulative error in pose estimation causes
distortion in the map. [Lu and Milios, 1995]
have shown that taking every pose where a
laser scan has taken as an estimated state the
problem can be avoided. In the following
Fig. 5 A 2D map build from laser range
measurements at different successive
viewpoints.
The laser scanner is operating on horizontal
plane fixed on the robot manipulator. Because
the robot is moving in pitch and roll and the
manipulator is pitching the resulting depth
picture is partially 3-dimensional.
6 CONCLUSION AND FUTURE
WORK
Navigation sensors used in workpartner
robot have been presented in this paper. Some
early test results have been shown, too.
The ordinary GPS receiver became an
accurate velocity and heading sensor on May
1st, 2000 when USA government finished to
disturb the position signal. The NMEA VTG
message of the receiver gives non- drifting
heading information better than most optical
gyros and the velocity information is
comparable or better in accuracy to ground
speed radar.
Since the GPS receiver needs at least three
satellites at sight other sensors must be also
used. A piezogyro was used as a secondary
heading sensor. Workpartner articulation angle
and velocity commands can be used to
compute the velocity when GPS satellites are
blocked. A laser scanner offers an alternative
method for position and heading measurement
relative to natural landmarks. It also carries out
the task to detect obstacles in the environment.
By connecting pitch and roll information to a
2-dimensional laser scanner a partially 3dimensional view can be obtained.
The above presentation shows some early
tests that will be followed by new ones in near
future. Methods like iterative closest point
[Madhavan et al.,1998] or particle filters will
be used. Particle filters have been successfully
used at indoor environment [Thrun et al.,
2000].
ACKNOWLEDGEMENTS
This work has been supported by a grant from
Sandvik/Tamrock Ltd.
REFERENCES
[Forsman, 2001] Pekka Forsman. Feature
based Registration of 3D Perception Data for
Indoor and Outdoor Map Building.
Proceedings of the 3rd International Conference
on Field and Service Robotics, Espoo, Finland.
[Halme et al., 2001] Aarne Halme, Ilkka
Leppänen, Sami Salmi, and Sami Ylönen.
Hybrid Locomotion of the WorkPartner
Service Robot. Proceedings of the 3rd
International Conference on Field and Service
Robotics, Espoo, Finland.
[Jensfelt, 1999] Patric Jensfelt. Localization
using
Laser Scanning and Minimalistic
Environmental Models. Licentiate thesis,
Royal institute of technology, Stockholm.
129 p.
[Lu and Milios, 1995] Feng Lu and Evangelos
Milios. Optimal Global Pose Estimation for
Consistent Sensor Data Registration. IEEE
International Conference on Robotics and
Automation. Pp. 93-100.
[Madhavan et al., 1998] Ray Madhavan, M.
Dissanayake and Hugh Durrant-Whyte.
Autonomous Underground Navigation of an
LHD using a Combined ICP-EKF Approach.
Proceedings of the 1998 IEEE International
Conference on Robotics & Automation,
Leuven. Pp. 3703-3708.
[Pears, 2000] Nick Pears. Feature extraction
and tracking for scanning range sensors.
Robotics and Autonomous Systems 33.
Pp.43-58.
[Selkäinaho et al., 2000] Jorma Selkäinaho and
Janne Paanajärvi. On tuning of Kalman filter.
International Symposium on Robotics and
Automation, Monterrey, 2000. 5 p.
[Thrun et al., 2000] Sebastian Thrun, Dieter
Fox, Wolfram Burgard and Frank Dellaert.
Robust Monte Carlo Localization for Mobile
Robots. Artificial Intelligence.