universitat polit ecnica de catalunya - iri · universitat polit ecnica de catalunya summary...

Universitat Politecnica de Catalunya

Master:

Automatic Control and Robotics

Master Thesis

Outdoor Mobile Robot Navigation

using a Time-of-Flight (ToF) camera

Angel Santamaria Navarro

Director: Juan Andrade Cetto

Academic Course 2011/12

June 2012

Outdoor Mobile Robot Navigation using a ToF camera

2


Contents

Summary 7

Motivation 8

Objectives 9

1 Introduction 11

2 State of the Art 133.1 Related work conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

3 Design 193.1 Acquisition using ToF cameras . . . . . . . . . . . . . . . . . . . . . . . . . 19

3.1.1 ToF camera principle . . . . . . . . . . . . . . . . . . . . . . . . . . 193.1.2 Depth measurement errors and compensation . . . . . . . . . . . . 223.1.3 Intrinsic and extrinsic calibration . . . . . . . . . . . . . . . . . . . 26

3.2 Low level data processing . . . . . . . . . . . . . . . . . . . . . . . . . . . 273.2.1 Amplitude filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . 273.2.2 Average filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . 273.2.3 Point Cloud Downsampling . . . . . . . . . . . . . . . . . . . . . . 283.2.4 Sparse outlier removal . . . . . . . . . . . . . . . . . . . . . . . . . 293.2.5 Normal Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

3.3 Interpretation of low level data processing results . . . . . . . . . . . . . . 323.3.1 Hole detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

3.4 Robot Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 333.4.1 Mapping and localization . . . . . . . . . . . . . . . . . . . . . . . . 343.4.2 Costmap . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 343.4.3 Global Planner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353.4.4 Local Planner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

4 Experimental Results 414.1 Camera calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 414.2 Acquisition and low level data processing . . . . . . . . . . . . . . . . . . . 444.3 Obstacles and holes detection . . . . . . . . . . . . . . . . . . . . . . . . . 504.4 Outdoor mobile robot navigation using a ToF camera . . . . . . . . . . . . 54

3


5 Conclusions 595.1 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

Appendix 61

Acknowledgements 71

Bibliography 73

4


List of Figures

1 Scheme of the phase shift measurement . . . . . . . . . . . . . . . . . . . . 212 ToF distance measurement . . . . . . . . . . . . . . . . . . . . . . . . . . . 223 2D example of the Point Cloud downsampling . . . . . . . . . . . . . . . . 294 Example of Statistical Outlier Removal . . . . . . . . . . . . . . . . . . . . 305 Normal estimation for each point . . . . . . . . . . . . . . . . . . . . . . . 316 Interpretation of filtering results . . . . . . . . . . . . . . . . . . . . . . . . 327 Hole detection zones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 338 Inscribed and circumscribed radius related to the footprint . . . . . . . . . 359 Comparison between straight and B-spline paths . . . . . . . . . . . . . . . 3610 B-spline with control and knot points . . . . . . . . . . . . . . . . . . . . . 3811 Camera Calibration, Range-laser (black) and ToF camera (green and red)

comparison in a real situation . . . . . . . . . . . . . . . . . . . . . . . . . 4212 Relation between axis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4413 Point cloud without filtering . . . . . . . . . . . . . . . . . . . . . . . . . . 4514 Scheme of the filtering process . . . . . . . . . . . . . . . . . . . . . . . . . 4615 Amplitude and average filtering . . . . . . . . . . . . . . . . . . . . . . . . 4716 Point Cloud Downsampling . . . . . . . . . . . . . . . . . . . . . . . . . . 4817 Statistical Outlier Removal . . . . . . . . . . . . . . . . . . . . . . . . . . . 4918 Wall detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5019 Tree detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5120 Stairs detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5121 Hole detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5322 Costmap . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5423 Global path planning without obstacle . . . . . . . . . . . . . . . . . . . . 5524 Global path planning without obstacle . . . . . . . . . . . . . . . . . . . . 5625 Local planner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5626 Global path planning with obstacle (before and after obstacle detection) . 5727 Outdoor navigation using ToF camera and Teo sensoring devices (front

laser in black and rear laser in blue) . . . . . . . . . . . . . . . . . . . . . . 5828 ROS Navigation stack conceptual overview . . . . . . . . . . . . . . . . . . 6629 TEO robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68

5


6


Summary

Recently developed 3D Time-of-Flight (ToF) cameras have an enormous potential for mo-

bile robotic applications and have recently become available at reasonable prices.

This master thesis focuses on the developement of obstacle avoidance algorithms using as

main sensing device a Time-of-Flight (ToF) camera for the RMP400 mobile robot (Teo),

and its validation in outdoor unstructured navigation applications.

In this thesis we developed acquisition and filtering methods to obtain reliable 3D in-

formation of a section of terrain in front of the robot, and developed algorithms to treat

this data in real time for safe obstacle avoidance and navigation.

The algorithms developed include way point detection through a traversability analisys

of the terrain, and navigation strategies to reach these way paoints.

The software developement framework used is the Robotics Operating System (ROS),

and all algorithms have been validated in a real robotic setting. To this end, we modified

the ROS navigation stack to allow data inference from a ToF device and to use the robot

kinematic constraints during path planning.

7


Motivation

State of the art sensing devices allow to capture the surrounding area in three dimensions

of a mobile robot moving at high speed. The challenge becomes now the interpretation

of such data for reliable navigation and obstacle avoidance.

For both dynamic and static scenes the most cost effective off-the-self system that pro-

vides full range, high-resolution distance information in real time are ToF cameras.

Usually ToF cameras works well on indoor environments, basically because in such envi-

ronments infrared noise levels are low. The use of these type of sensors on outdoor mobile

robotics becomes a challenge because of the sun-light spectrum. The motivation for doing

this master thesis was primarily an interest in undertaking a challenging project in an

interesting area of research, the investigation on how ToF cameras can work on outdoor

environments, and more precisely, how these cameras can provide reliable information

about the scene to an outdoor mobile robot.

8


Objectives

The main objectives of this master thesis are:

• Development of a low-level driver and a ROS compatible driver to acquire data from

a ToF camera.

• Development of filtering methods to obtain reliable 3D information of a section of

terrain in front of the robot.

• Development of ROS compatible algorithms to treat obtained data in real time for

safe obstacle avoidance and navigation.

• Integration of a ToF camera as a new sensor of RMP400 mobile robot (Teo).

• Validation of the ROS compatible algorithms proposed on outdoor human environ-

ments.

9


10


1. Introduction

A large number of mobile robot applications are tailored to human environments, and the

robots used are expected to navigate and move efficiently on them.

A human environment could be classified as structured or unstructured. This main classi-

fication usually relies on the existence, or not, of a pre-defined model of the environment,

and on the structuredness of such model. This master thesis focuses on mobile robot

navigation for outdoor unstructured environments.

An elementary capability of all autonomous mobile robots to safely navigate in dynamic

and/or unstructured environments is obstacle avoidance. Several well established ob-

stacle avoidance techniques exist for planar domains, and frequently rely exclusively on

two-dimensional sensory input. However, the human environments contain many objects

which cannot be appropriately sensed and represented in this way and thus lead to dif-

ficulties in collision avoidance. Think for instance of objects located below or above the

scan plane of a 2D laser range finder.

To overcome this issue, sensor modalities that acquire dense three-dimensional measure-

ments of the environment are favored us. Their 2D counterpart, such as structure from

motion (SfM) and stereo vision (SV) systems, 3D laser range scanners (LRS) and time-

of-fligth (ToF) cameras.

ToF cameras combine the advantages of active range sensors and camera based approaches

as they provide a 2D intensity image and distance values in real-time for each pixel. No

data integration is needed since range, colour and intensity measurements are registered

for each pixel. Compared to SV systems ToF cameras can deal with prominent parts of

the evironment such as walls, floors, and ceilings even if they contain no texture. In ad-

11


dition to the 3D point cloud, contour and flow detection in the image plane yields motion

information that can be used for applications such as car or person tracking. Compared

to LRS, all range data are captured at one time without any time dependency between

different object sample points. In conclusion it can be said that ToF cameras are the

most suitable 3D imaging system for mobile robot navigation as they are able to deliver

the most reliable 3D dynamic information of objects. The work presented here uses this

type of devices.

In this thesis we developed acquisition and filtering methods using a ToF camera to obtain

reliable 3D information, and developed algorithms to treat this data in real time for safe

obstacle avoidance and navigation. During such navigation task from one intermediate

waypoint to the next, sensor data at high frame rate is analyzed in order to determine

whether the path is traversable, or whether an obstacle is found, and a countermeasure

to the navigation commands is computed to safely clear the obstacle.

This project has been done at the “Institut de Robotica i Informatica Industrial” (IRI) in

the context of the PAU+ project (http://www.iri.upc.edu/project/show/115). The robot

used is Teo (http://wikiri.upc.es/index.php/TEO).

This document is structured as follows. In the next section a brief description about

the state of the art on mobile robot navigation and more especifically in using ToF cam-

eras. In section 3, the design of the proposed algorithms is described. Section 4 contains a

brief description of the experimental results under different terrain conditions that verify

the functionallity of the implementation and integration of the ToF camera with a mobile

robot. Conclusions and future work are explained in section 5. The apendices include a

brief description of ROS (Robotic Operating System), and the specifications of the robot

Teo and the PMD camcube 3.0 ToF camera.

12


2. State of the Art

This chapter presents related work about mobile robot navigation and obstacle avoid-

ance as well as the different kinds of sensors used. Only some of the most representative

methods are analyzed, and conclusions are drawn to direct our research. The problem of

sensing the spatial propierties of the environment is fundamental for autonomous robotics

and, many groups have been working in this field for a long time.

Most mobile robot implementations until today rely on 2D sensors for creating maps,

self localitzation and collision avoidance. This is justified to some extend because most

applications have been on ground robots, which inherently move in a 2D space.

The most widely used sensor is the laser range scanner, wich provides a 180o view on

the plane of measurement (two such sensors can be mounted opposite to each other to

provide a full planar view). The field of view of such a system is reduced to a single plane

parallel to the floor, neglecting threedimensional information.

To reliably avoid collisions a high number of additional sensors (usually infrared or ultra-

sound) are incorporated around the robot, adding robustness as well as complexity to the

system, including additional sensor fusion algorithms [17], [11].

Today the most common techniques for 3D sensing excluding laser scanners are CCD

or CMOS cameras and recently, 3D time-of-flight cameras. CCD or CMOS cameras are

limited in providing reliable navigation or mapping information for a mobile robot in

real-time and like all passive visual sensors, they are subject to difficult imaging artifacts

in real world environments, such as changing lighthing conditions.

A very new and promising technique is that of ToF cameras, which are based on the

13


photon mixer device technology (PMD) [5], [6], [18]. First approaches in robot navigation

with an evaluation prototype of a Swiss Ranger SR-2 were presented by Weingarten et

al. in [5]. They have shown that obstacle avoidance with the Swiss Ranger has many

advantages in contrast to a 2D laser scanner. In comparison to laser scanner based 3D

sensors the SR-2 provides 3D information with a framerate of about 30 fps. But they also

pointed out that camera calibration and data preprocessing were necessary to get stable

sensor data.

There exist a number of approaches for calibrating the depth measurements of ToF cam-

eras [8], [18]. These are still evolving and a lot of work is devoted to understanding the

sources of errors and to minimize them, as well as to model their effect for camera simu-

lation. Depth measurements with ToF cameras face both systematic and nonsystematic

errors. Generally, systematic errors can be managed by calibration and nonsystematic

errors are dealt with filtering schemes.

There exist many algorithmic solutions in the field of mobile robot navigation to solve

obstacle avoidance problem. Although most of them are designed for indoor scenarios,

some implement strategies to detect obstacles in 3D. Nevertheless, almost all of them

lack the capability of navigating in real 3D environments. The objective of an obstacle

avoidance algorithm is to determine which part of the environment is traversable, and

which is an obstacle. This decision is carried out by analyzing proprioceptive sensor data,

and is obviously platform dependent.

One such strategy is to use a single rotating infrared sensor is [3]. They consider the

mobile robot to be the origin of a polar coordinate system and calculate force vectors

directly from sensor readings using the artificial potential field equations. At the end of a

360o sweep of the environment, the forces were resolved and summed in order to give an

output which can be considered to be the new repulsive velocity components Vx and Vy

14


both parallel and perpendicular to the vehicle’s center line, caused by the environment

surrounding the robot, and adding some extra terms to correct for errors due to robot

motion. In this new position the vehicle was again ready to repeat the process. The

problems they found with such method is that the robot could fall into a local minima or

potential trap. To solve this, they introduced a method called goal relocation, in which

the goal is temporarily moved according to the sensed geometry of the current environ-

ment. From odometric estimates and a single scan of the environment, the algorithm

decided whether or not the robot had a clear path to its goal. If that was not the case,

then the goal relocation algorithm “looks for a gap” and temporarily moves the goal into

that gap. Clearly these traced out a set of “beacons” or “way points” that the robot had

used whilst navigating.

A few years later, Fox et al. presented one of the most popular path following meth-

ods to date, the dynamic window approach for collision avoidance [4]. This method is

derived directly from the motion dynamics of synchro-drive mobile robots, hence it is

especially designed to deal with the constraints imposed by limited velocities and acceler-

ations. Their approach considers periodically only a short time interval when computing

the next steering command to avoid the complexity of the general motion planning prob-

lem. The aproximation of trajectories during such a time interval by circular curvatures

results in a two-dimensional search space of translational and rotational velocities. This

search space is reduced to the admissible velocities allowing the robot to stop safely. Due

to the limited accelerations of the motors, they imposed a further restriction on the veloc-

ities: the robot only considers velocities that can be reached within the next time interval.

This velocities form the dynamic window which is centered around the current velocities

of the robot in the velocity space. Among the admissible velocities within the dynamic

window a combination of translational and rotational velocity is chosen maximizing an

obejctive function. This objective function includes a measure of progress towards a goal

location, the forward velocity of the robot, and the distance to the next obstacle on the

15


trajectory. By combining these, the robot trades off its desire to move fast towards the

goal and its desire to circunvent obstacles.

An other interesting path following method is presented by K.Konolige et al. [9]. They

take an approach that is opposite to techniques such as DWA. Instead of searching the

space of feasible trajectories, they search the space of feasible controls. As is the case with

most differentially-driven platforms, the control commands are a pair (x, θ) of desired

translational and rotational velocities. Thus they have a 2D space of possible commands

to consider. This space is bounded in each dimension by velocity limits that reflect the

vehicle’s capabilities. Because they are seeking good, as opposed to optimal, control,

they sample, rather than exhaustively search, this rectangular region of allowed veloci-

ties. They take a regular sampling, and for each sample simulate the effect of applying

those controls to the robot over a short time horizon. The simulation predicts the robot’s

trajectory as a sequence of 5-dimensional (x, y, θ, x, θ) states with a discrete-time approx-

imation of the vehicle dynamics.

A more modern approach, refering to the navigation issue, is the method shown in [7],

that prorposes an algorithm that drives a unicycle type robot to a desired path, includ-

ing obstacle avoidance capabilities. The path-following control design relies on Lyapunov

theory, backstepping techniques and deals explicitly with vehicle dynamics. This is done

by controlling explicitly the rate of progression of a “virtual target” to be tracked along

the path. The obstacle avoidance part uses the Deformable Virtual Zone (DVZ) principle.

This principle defines a safety zone around the vehicle in which the presence of an ob-

stacle induces an “intrusion information” that drives the vehicle reaction. The porposed

method consists in a guidance solution that embeds the path-following requirements in a

desired proximity function with respect to the obstacles that drives the robot to circum-

vent the obstacels while guaranteeing the path-following convergence requirements when

there is no obstacle. This approach is based on the derivation of a Lyapunov function that

16


guarantees asymptotic convergence to the path without obstacles, and the boundedness

of a variable called the intrusion ratio, that captures the surrounding obstacle proxim-

ity and the current robot situation with respect to the path. The combination of path

following with reactive local obstacle avoidance has a natural limitation in the situation

that both controllers yield antagonistic reactions, that they called corner situation where

an heuristic switch between controllers is necessary (outside of this corner situation no

switch is required). Unfortunately, the kinematic model used and the kinematic control

laws derived which guarantee asymptotic convergence to the path (positive definiteness

of the Lyapunov function and negative semi definiteness of its derivative) are not easily

extensible to the 3D case.

A 3D navigation method is explained [15], a navigation system that allowed a robot

to complete 26.2 miles of autonomous navigation in a real office environment is described.

They present the methods required to achieve this level of robustness, including an ef-

ficient Voxel-based 3D mapping algorithm that explicitly models unknown space. The

voxel Grid is an efficient 3D occupancy grid that serves as the core of their approach

navigation. Each cell in the grid has one of three states:

• Occupied

• Free

• Unknown

Occupancy information is used to create safe plans for the navigation system even in

cluttered environments. There are two main operations that can be performed on the

grid:

• Marking

• Clearing

17


Marking refers to accesing an element of the grid and changing it status based on an

observation from a sensor, and clearing refers to raytracing in the grid from the origin of

a sensor to an observation while asserting free space along the way. This implementation

of Voxel Grid is remarkably efficient, but it does limit the number of grid cells in the Z

dimension to run at the rate needed depending on the robot. One of the main advantages

of the Voxel Grid structure is its ability to track unknown space in the world. In an ideal

world, every unknown grid cell would be treated as an obstacle. This would allow hard

guarantees to be made about the safety of an autonomous robot because it would never

traverse cells that it had not explicitly seen.

Related work conclusion

From the work analyzed, it seems interesting the use of ToF camera as a sensor for

the robot, taking into account that the laser scanners do not provide us with 3D informa-

tion as dense as ToF cameras without the need of mobile parts, and data analysis from

stereo cameras implies a process of correspondence between images which may introduce

some errors as well as time of computation in the acquisition process, and relies on having

textured scenes.

Since the main objective of our navigation system is safe obstacle avoidance, it is neces-

sary to design a custom 3D navigation method that includes a full 3D representation of

the world, where the robot is capable of determining which obstacles are traversable or

not. For these reason it seems interesting to use the dynamic window approach in com-

bination with a 3D Voxel grid analysis; which are perfectly compatible with the use of a

ToF camera, to perform a cost map which allows to compute safe navigation trajectories.

18


3. Design

3.1 Acquisition using ToF cameras

A Time-of-Flight (ToF) camera is a relatively new type of sensor that delivers range data

at a high frame rate, simultaneously providing intensity data for every pixel.

Despite that the number of pixels in ToF images is still small (i.e., 176x144 in the Swiss-

ranger SR3000 and SR4000 cameras, and 204x204 in the PMD CamCube 3.0 camera) and

that noise depth values cannot yet be completely removed after calibration, ToF imaging

is rapidly showing a great potential in numerous scientific domains.

These propierties, and robustness to illumination changes and low weight, make it foresee-

able that ToF cameras will replace previous solutions, or alternatively complement other

technologies, in many applications.

3.1.1 ToF camera principle

There are two main approaches currently employed in ToF camera technology, time-of-

flight can be measured by using either pulsed or continuous-wave (CW) modulation: one

measures distance by means of direct measurement of the runtime of a travelled light

pulse, using for instance arrays of single-photon avalanche diodes (SPADs) or an optical

shutter technology; the other method uses amplitude modulated light and obtains dis-

tance information by measuring the phase difference between a reference signal and the

reflected signal (PMD).

These two approaches are described in the following:

19


Direct time of flight measurement

The first category uses a single laser pulse for each frame acquisition: the outgoing laser

pulse passes through transmission optics, then it is reflected from the target and finally

focused on the sensor by means of a receiver lens where the independent pixel clocks are

used to measure the time of the travelled laser pulse. Each pixel of the Focal Plane Array

(FPA) can accurately and independently count time to the target and back to each pixel,

which is proportional to the range between the target and the sensor. The distance (D)

at each pixel is determined by:

D =1

2c T

where c is the speed of light and T is the time needed by the light pulse to go from the

emitter to the object and back to the sensor pixel.

In a similar way, the Single Photon Avalanche Diode (SPAD) arrays measure the dis-

tance by means of the direct measurement of the runtime of a traveled light pulse.

Indirect time of flight measurement (phase-shift measurement)

This second category has already been implemented successfully in several commercially

available 3D camera systems, such as PMD CamCube 3.0 which uses continuous-wave

(CW) modulation and more precisely uses demodulation lock-in pixels.

The PMD CamCube 3.0 camera used in this master thesis is a phase shift measurement

device, working with the so called Photonic Mixer Device (PMD) based on standard

CMOS technology using a modulation freqency of 20MHz.

Whereas sensors based on discrete pulsed modulation measure the time of a light pulse

trip to calculate depth, sensors based on lock-in measure phase differences between emit-

20


ted and received signals. A near-infrared light (NIR), via light-emitting diodes (LEDs),

is emitted by the system and then reflected back to the sensor.

A scheme of the phase shift measurement principle is reported in the Figure 1: while

the emitted signal has and amplitude E, the reflected signal, which is detected by the

sensor, has experienced an attenuation, therefore it has an amplitude M smaller than E.

The received signal is offset-shifted in intensity with respect to the emitted signal mainly

because of additional background light, resulting in a mean intensity of B.

Figure 1: Scheme of the phase shift measurement

The phase φ is extracted using a suitable demodulation device, which can perform either

a correlation or a sampling process. The received signal is usually sampled four times in

each cycle, at 14

period phase shifts (m0,m1,m2,m3), as shown in Figure 2,.

21


Figure 2: ToF distance measurement

This allows the parallel measurement of the parameters M (amplitude of the measured

signal), B (offset) and the phase φ:

M =

√(m0 −m2)2 + (m1 −m3)2

2

B =m0 +m1 +m2 +m3

4

φ = arctan(m0 −m2

m1 −m3

)

This phase demodulation technique is commonly known as “four-bucket” sampling and

it permits to calculate easily the target depth:

D = Lφ

2 π

λ

2

The amplitude M may be used as a measure of quality of the distance measurement,

or simply to generate a grayscale image of the observed scene. The intensity B may be

used to generate another grayscale image (intensity image) of the observed scene, which

contains also the additional background light of the scene.

3.1.2 Depth measurement errors and compensation

Depth measurements with ToF cameras could have different error types. These errors

could be classified generally as nonsystematic errors, produced randomly, and systematic

22


errors due to different known causes. Usually systematic errors can be managed by cal-

ibration and nonsystematic ones by filtering. In some cases, the influence of systematic

errors has been strongly reduced by the manufactures.

Systematic errors

The systematic errors that we have identified are:

• Camera model: ToF cameras are defined by the pinhole camera model. Therefore,

their images are corrupted by lens distortion effects, focal length, and shifting of the

optical center. Those effects are usually handled by the lateral (2D) calibration of

the camera.

• Depth distortion: The calculation of the distance assumes a perfectly sinusiodal

light source, which in practice is not given due to irregularities in the modulation

process. This type of error produces an offset that depends only on the measured

depth for each pixel. Those effects can be adrdessed by comparing camera depth

measurements with a reference ground truth distance, or by means of an optimiza-

tion process that models the error from multiple relative measurements.

• Integration time: Integration time (IT) can be selected by the user. It has been

observed that for the same scene different IT cause different depth values of the

scene. The main reason for this effect is still a subject of investigation. It can be

corrected choosing a fixed IT value, correcting the rest of the errors with this value,

and not changing it.

• Pixel error: These errors are related to the position of the pixel in the sensor array

produced by two main sources. On the one hand, errors due to different material

propierties in CMOS-gates produce a constant pixel-related distance offset, leading

to different depths measured in two neighbor pixels corresponding to the same real

depth. On the other hand, there are latency-related offset errors due to the capacitor

23


charge time delay during the signal correlation process. This can be observed as

a rotation of the image plane. Neighboring pixel errors are small, and can be

considered negligible. In that case, only the error from the rotation of the image

plane has to be modeled. Usually the camera manufacturer provides a fixed-pattern

noise table (FPN) in a calibration file to correct this.

• Amplitude errors: These errors occur due to low or overexposed reflected am-

plitudes. The higher the reflected amplitudes, the higher the depth accuracy. Low

aplitudes appear more often in the border of the image as the emitted light power

is lower than in the center, leading to overstimation of depth. Contrarily, when the

object is too close to the camera or integration time has been chosen too high, satu-

ration can appear and depth measurements will not be valid. The amplitude errors

can be avoided by filtering pixels with a low amplitude value, and also calibrating

the camera for different reflective surfaces and storing all the median values and

using them as a look-up table depending on intensity values.

• Temperature related error: Internal and external temperature of the camera

influences depth measurements. During the first minutes of operation, while the

camera warms up, the measured distance increases. To correct this we have to

define a warm up waiting time until the temperature is stabilized. Some cameras

even include an internal fun to keep the temperature within an operation range.

Nonsystematic errors

Typical nonsystematic errors in ToF distance measurements are signal-to-noise ratio, mul-

tiple light reception, light scattering and motion blurring. The occurrence of some of them

is unpredictable.

• Signal-to-noise ratio distortion: Appears in scenes not uniformly illuminated.

Low illuminated areas are more susceptible to noise than high illuminated ones. This

type of error is highly dependent on the amplitude, the IT parametrization and the

24


depth uniformity of the scene. Signal-to-noise ratio can be improved by several

means: Low-amplitude filtering, actively deciding the optimal IT depending on the

desired areas, or computing the average of those readings and using an accuracy

thershold based on pixel variance.

• Multiple light reception: Errors that appear due to interference of multiple light

reflections captured at each sensor’s pixel are mainly due to the presence of surface

edges (jump edges) and object concavities. Jump edges are generally removed by

comparing the angle of incidence of neighboring pixels. It is a still open question

how to deal with multiple reflections originated by concavities.

• Light scattering: The effect arises due to multiple light reflections between the

camera lens and the sensor. The effect produces depth underestimation over affected

pixels, because of the energy gain produced by its neighboring pixel reflections. Light

scattering effects could be minimized with diferent approaches: selecting an optimal

IT in order to minimize saturation problems, or removing scattering-affected pixels

using a filter based on the combination of amplitude and intensity values; and also

using a compensation method based on blind deconvolution based on a mathematical

model.

• Motion blurring: This effect is due to the physical motion of the objects or the

camera during the integration time used for sampling and can be classified in two

different types of artifacts depending on whether their apperance is due to lateral or

axial motion. Lateral motion artifacts are identified first, by estimating optical flow

from some preprocessed phase-sampled images. Afterwards, axial motion artifacts

are removed using an estimation approach and a theoretical model of the deviation

errors.

25


3.1.3 Intrinsic and extrinsic calibration

The next subsections briefly explaine the intrinsic and extrinsic calibration of a ToF cam-

era, similar to those used for traditional cameras that provide depth through stereo or

structured light.

Intrinsic

Intrinsic camera parameters have often been obtained by applying classical calibration

procedures based on intensity images and calibration patterns. However, the character-

istic low resolution of ToF cameras leads to a poor localization of the calibration pat-

tern characteristics and the obtained calibration parameters are usually of poor quality.

Therefore, intrinsic ToF camera calibration has to be improved by using also the depth

information provided by the camera, and it is usually included in the extrinsic calibration

methods explained below.

Extrinsic

The extrinsic parameters encode the coordinate system transformation from 3D world

coordinates to 3D camera coordinates. They are useful in two scenarios: first when a ToF

camera has to be referenced to an external device; second, when ToF images have to be

combined with other sensor data.

For the first scenario, the transformation between the robot coordinate system and the

camera coordinate system is the so-called hand-eye calibration. To compute it, the image

measures have to be used. For the second scenario, lasers and one or various color cameras

are rigidly mounted with one or various ToF cameras. Although extracting precise extrin-

sic parameters is highly recommended, in this case some simplifications can be performed

because sensors are mounted in a particular configuration.

26


The TEO robot used in this work combines a ToF camera and a 2-D laser scan, there-

fore the calibration parameters are obtained by comparing the output of the ToF camera

for a certain observable pattern with the 2D laser measurement determined with higher

accurancy.

3.2 Low level data processing

Systematic errors will be removed using the camera calibration, and nonsystematic errors

by filtering the data to get the information needed to perform obstacles avoidance and

navigation. The following section explains the different methods we used in this thesis to

perform filtering of the 3D data obtained from the ToF camera.

3.2.1 Amplitude filtering

Amplitude filtering is used to remove 3D points which have been read with a lower am-

plitude than a threshold. As we have seen above, pixels with low amplitude have higher

probability of having a poorly computed 3D estimate. Therefore, thresholding the ampli-

tude discards primarily data resulting from objects with low infrared reflectivity, higher

distance, or from objects which are located at the peripheral area of the measurement

volume due to inhomogeneous scene illumination.

3.2.2 Average filtering

The average filter consists on observing each pixel from a point cloud read at consecutive

time intervals, and keeping only those pixels whose measures is sustained temporally. The

use of the average filter is to improve the signal-to-noise ratio. The point cloud read from

ToF camera will be stored in a buffer of a certain size, depending on how many readings

we want to compare. Each pixel measure is compared to all other measures in the buffer.

Those pixels that do not exist on all measures will be considered noise and will be purged.

27


3.2.3 Point Cloud Downsampling

Once several points have been removed from the Point Cloud by the previous filters, we

still have a very large number of points, where most of them are not providing additional

information to detect possible obstacles to perform save navigation. It is interesting to

reduce this number of points without losing the scene representative information. After

this filtering, assuming that the error produced is acceptable, we do not lose the repre-

sentative scene information and the point cloud data is lighter.

To downsample the point cloud, one possibility is to subdivide the 3D space into a 3D

regular grid, distributed uniformly in the scene space with ploygons with the same size

and without aliasing. Then, each point from the point cloud will be reduced to its cor-

responding polygon centroid, Therefore we will have as many points as polygons used to

divide the secene with previous points inside. The centroid of the polygons without any

previous point inside will not be considered.

In Figure 3 we see a 2D example of this downsampling procedure. In this Figure the red

points correspond to previous data points before filtering, and the green points represent

the resulting point cloud after filtering (centroids of the polygons which had points inside).

The introduced error in the location of 3D data is not larger than half the distance

of a polygon edge, which in our case is considered negligible.

28


Figure 3: 2D example of the Point Cloud downsampling

3.2.4 Sparse outlier removal

Measurement errors produce also sparse outliers. in order to eliminate them, a statistical

analysis on each point neighborhood is performed, trimming those points that do not

meet a certain criteria.

Sparse outlier removal is based on the computation of the distribution of point neighbor

distances in the input dataset. For each point, its mean distance to all its 8-connectivity

neighbors is computed. By assuming that the resulting distribution is Gaussian, all points

whose mean distances are outside an interval defined by the point mean and standard de-

viation can be considered as outliers and trimmed from the dataset.

Figure 4 shows the effects of this sparse outlier analysis and removal: the original dataset

is shown on the left, while the resultant one on the right. The graphic shows the mean

k-nearest neighbor distances in a point neighborhood before and after filtering.

29


Figure 4: Example of Statistical Outlier Removal

3.2.5 Normal Estimation

Once the dataset is filtered, we can proceed to extract the characteristics of interest to

detect traversable and not traversable zones for a mobile wheeled robot.

An important characteristic to discern in which zones the robot will be able to navi-

gate, is the normal vector corresponding to the detected surface. Traversable regions

have a local planar results orientation nearly tangential to the robot footprint.

Figure 5 shows the surface estimation for each point in a point cloud, taking into account

his neighbors. Once this surface has been obtained, the normal vector to the surface is

calculated and associated to each point.

30


Figure 5: Normal estimation for each point

To estimate the normal vectors at each point we use a method similar to featured

by Ortega et al. in [12].

For each point in the point cloud with coordinates p = (x, y, z)T , we can define the

error between the estimated planar surface and the k nearest neighbors to p as

e =∑i∈k

(pTi n− d)2

where n = (nx, ny, nz)T is the normal vector to the local surface at point p, and d the

distance from p to the plane.

This error can be re-expressed in the following form:

e = nT (∑i∈k

(pi pTi ))n− 2d(

∑i∈k

(pTi ))n+ |k|2 d2

where

Q =∑i∈k

(pi pTi )

q =∑i∈k

pTi

Combining the above error metric with the orthonormality property for each local surface

normal into a Lagrangian of the form:

l (nT , d, λ) = e+ λ (1− nT n)

31


The local surface normal that best fits the patch k is the one that minimizes the above

expression.

Deriving l with respect to n and d, and setting the derivatives to zero, it turns out

that the solution is the eigenvector asociated to the smallest eigenvalue of:

(Q− q qT

|k|2)n = λn

3.3 Interpretation of low level data processing results

As we saw in the previous section, we estimate a normal vector for each point of the point

cloud that enables us to determine which zones will be navigables or not.

Using these normals, Figure 6 shows an example of a 2D simplified set of surfaces with

basic interpretation criteria to decided whether a surface is traversable or not, taking into

account the normal estimation (green: traversable, red: not traversable).

Figure 6: Interpretation of filtering results

Looking at this interpretation, we create a point cloud containing all the points that have

the normal vector estimate with a non desired orientation. So, we are creating a point

cloud with an interpretation of the scene obstacles.

One thing to take into account is the scene with holes in the floor. In such cases the

32


situation is diferent, we do not have neither information about traversability nor about

non traversability. We have to implement a different solution for the detection of holes as

explained in the following section.

3.3.1 Hole detection

To perform the hole detection we define a particular zone of the scene in front of the robot.

Taking into account the phisical camera position we could say that at a certain distance

from the camera we must find the ground. If this ground information is not retrieved (a

minimum amount of points founded in that region) we will consider that we founded a

hole in front of the robot.

Figure 7 shows three regions defined which we analize the number of points existing.

If the number of points existing on those zones are less than a threshold number, we

consider a hole. In this case we defined right, center and left possible hole.

Figure 7: Hole detection zones

Once a hole is detected, we generate a point cloud as if there is an obstacle in that region.

3.4 Robot Navigation

Should the robot know where the obstacles are it could navigate without collision. Thus,

in this section we explain how to perform this navigation using the information obtained

from the acquisition and filtering processes explained above. This navigation system takes

advantage of ROS navigation package and some new implementations have been done to

comply with the nonholonomic constraints of our platform.

33


3.4.1 Mapping and localization

The navigation system can be initialized with or without an a priori, static map.

• Without a static map: The robot only knows about obstacles that it has seen

with the ToF camera, and will make optimistic global plans through areas that it

has not yet visited.

At the ROS navigation stack, in this case, the robot’s pose is usually estimated

by integrating wheel odometry with an extended Kalman filter.

• With a static map: The robot will make informed plans about distant parts of the

environment, using the map as prior obstacle information. At the ROS navigation

stack, in this case, the robot localizes using Monte Carlo localization.

3.4.2 Costmap

The costmap is an environment information map, initialized with the static map (if avail-

able), but updated as new sensor data comes in.

The costmap is used as a two-dimensional structure by the navigation system because

the robot used is constrained to drive on surface ground, and cannot, for example, step

or jump over large level changes. For this reason the obstacle data is assembled into a 2D

plane. Although the costmap is used as a 2D structure, its underlying representation of

the world actually consists of the efficient three-dimensional Voxel Grid described in the

state of the art section.

Each column in the Voxel Grid is projected down into two dimensions where it is as-

signed a cost. Columns with occupied cells are assigned a lethal cost, meaning that no

part of the robots footprint is allowed to be inside of the corresponding two-dimensional

cell. Then, inflation is performed in two dimensions to propagate costs from obstacles out

34


to a user-specified inflation radius.

Figure 8 shows the inscribed and circumscribed radius related to the footprint. Cells

that are less than one inscribed radius of the robot away from an obstacle are assigned a

uniformly high cost, after which an exponential decay function is applied that will cause

the cost to drop off out to the inflation radius used for the costmap.

Figure 8: Inscribed and circumscribed radius related to the footprint

We modified the cost map input to accept ToF point clouds data. When a point cloud of

an obstacle is analyzed, the cost map will be updated.

3.4.3 Global Planner

The global planner uses the obstacle cost information contained in the costmap, infor-

mation from the robot‘s localitzation system, the start position and a goal in the world.

From these, it creates a high-level plan for the robot to reach the goal location.

The global planner can use Dijkstra ([1]) or A* ([2]) algorithms and plans directly in

the configuration space computed during obstacle inflation in the costmap without taking

into account the kinematics of the robot. These algorithms propagate a cost for each

cell in the map from the goal to the robot using the cells marked as an obstacle by the

costmap. Once this costs are assigned, the best path is the one with the less cost cells.

This means that if there is an obstacle between the robot and the goal, the global planner

will find a path around the obstacle. In the cases that no obstacle between the robot and

35


the goal will be found, the best path will be a straight one, and it means that it does not

take account about the initial and final pose, thus in some cases an initial or final in-place

rotation will be needed.

Because of Teo is a non holonomic robot, these in-place rotations carry some problems.

For this reason we performed some modifications on how these algorithms propagates the

cell costs, to smooth the path and reach the goal with previously reached pose.

To modify this global path algorithms we introduced a B-spline calculation between the

robot and the goal, taking account of the kinematics of the robot.

The Figure 9 shows the difference between calculate the path with or without the B-

spline.

Figure 9: Comparison between straight and B-spline paths

This B-spline is defined given m real values tj, called knots, with

t0 ≤ t1 ≤ ... ≤ tm−1

a B-spline of degree n is a parametric curve

S : [tn, tm−n−1]→ <d

36


composed of a linear combination of basis B-splines bi,n of degree n

S(t) =m−n−2∑

i=0

Pi bi,n(t), t ε [tn, tm−n−1]

where Pi ε<d are called the control points.

Looking at the path, a cubic B-spline is enought to reach the goal with the desired pose.

In that case we used the cubic B-spline with uniform knot-vector, which is one of the most

commonly used forms of B-spline. The blending function can easily be precalculated, and

is equal for each segment in this case.

Put in matrix-form, it is:

Si(t) =(t3 t2 t 1

) 1

6

−1 3 −3 1

3 −6 3 0

−3 0 3 0

1 4 1 0

Pi−1

Pi

Pi+1

Pi+2

for t ε [0, 1].

The Figure 10 shows how we calculate the B-spline control and knot points (Pi and

tm−1) to consider the orientation of the robot.

37


Figure 10: B-spline with control and knot points

The control points Pi are calculated using the robot kinematic formulas, and it means

that the B-spline will not have a point what the robot cannot reach. In the case of Teo,

the kinematics could be approximated by:

xt+1 = xt + vtx dt

yt+1 = yt + vty dt

θt+ 1 = θt + ωt dt

So, if the first control point P0 is the trajectory starting point and the last P3 is the goal,

the other control points can be calculated by:

Px0 = xstart

Py0 = ystart

θ0 = θstart

Px3 = xgoal

38


Py3 = ygoal

θ3 = θgoal

Px1 = xstart + vtx dt

Py1 = ystart + vty dt

θ1 = θstart + ωt dt

Px2 = xgoal − vtx dt

Py2 = ygoal − vty dt

θ2 = θgoal − ωt dt

Once this B-spline is calculated, we modify the potential propagation wave used by the

algorithm of the global planner, giving a less cost to the cells containing the spline path.

3.4.4 Local Planner

The local planner is responsible for generating velocity commands for the mobile base

that will safely move the robot towards a goal. The local planner is seeded with the plan

produced by the global planner, and attempts to follow it as closely as possible while

taking into account the kinematics of the robot as well as the obstacle information stored

in the Costmap.

The local planner can use a Dynamic Window Approach (DWA [4]) or the Rollout Tra-

jectory planner ([9]) to forward simulate and select among potential commands based on

a cost function that combines distance to obstacles, distance to the path produced by the

global planner, and the speed at which the robot travels. During this master thesis no

substantial modifications have been done at local planner algorithms.

39


40


4. Experimental Results

These experiments have been done at the “Institut de Robtica i Informtica Industrial“

(IRI) in the context of the PAU+ project (http://www.iri.upc.edu/project/show/115).

The experiments entailed a previous step, the robot and the camera integration. The

robot we used is Teo (http://wikiri.upc.es/index.php/TEO) and the ToF camera is PMD

Camcube 3.0 from PMD technologies. The Appendix-Teo robot shows the hardware con-

figuration used to obtain the experimental results during this master thesis.

4.1 Camera calibration

To obtain data from the ToF camera, the first step has been a driver creation to use it

in the ROS environment. We created a low-level driver using the SDK libraries from the

manufacturer (”PMD technologies”) with a high level ROS compatible driver node.

Once this driver has been done, the first thing we did is the camera calibration to obtain

reliable data. To perform this calibration we used ROS image pipeline node, which is a

monocular calibration node that uses a checkboard to obtain intrinsic parameters of the

camera.

The Intrinsic parameters obtained corresponds to:

• Focal length:(

280.359 282.251)

• Principal point coordinates:(

94.8891 115.451)

• Skew coefficient:(

0 0)

• Distortions:(−0.428663 0.221012 −0.000661 0.000732 0

)

41


• Camera Matrix: 280.359 0 94.8891

0 282.251 115.451

0 0 1

This calibration is used by the ROS camera driver to correct monocular distortions.

To check the depth measurement, one way to calibrate the camera is comparing the

obtained data with a “ground-truth” data. In our particular case, Teo has a range-laser

mounted below the camera and we used the comparison of both depth information.

The Figure 11 shows the comparison between the range-laser and the ToF camera, and

how the ToF camera detects de floor.

Figure 11: Camera Calibration, Range-laser (black) and ToF camera (green and red)

comparison in a real situation

As it is shown, the ToF camera gives a good depth information to our particular case

with these calibration parameters.

The camera axis are related to the robot base axis in a particular configuration:

• The PMD camera axis are related to the front-laser axis with:

42


– X: 130 mm

– Y: 0 mm

– Z: 0 mm

– Roll: -2.090 rad

– Pitch: 0 rad

– Yaw: -1.5707 rad

• The front laser axis are related to base link axes:

– X: -590 mm

– Y: 0 mm

– Z: 50 mm

– Roll: 0 rad

– Pitch: 0 rad

– Yaw: 0 rad

• Finally, the base link axes are related to the robot footprint:

– X: 0 mm

– Y: 0 mm

– Z: 266.7 mm

– Roll: 0 rad

– Pitch: 0 rad

– Yaw: 0 rad

43


The Figure 12 shows the relation between axis.

Figure 12: Relation between axis

4.2 Acquisition and low level data processing

Using the low-level driver and the ROS compatible high-level node, we obtain the ampli-

tude, depth and intensity images as well as point cloud data from the ToF camera.

In the next sections, the data processing is focused on the point cloud. Without the

filtering process, the ToF camera produces a point cloud data with a low signal to noise

ratio.

The Figure 13 shows the point cloud obtained from the camera driver without filter-

ing. In the next sections, we used the same scene to show each data process effect,

corresponding to a stairs in front of the robot.

44


Figure 13: Point cloud without filtering

As it is shown in the Figure 13, a filtering process is very important to obtain reliable

information.

After experimental tests, the low level camera driver parameters are set to:

- Integration time: 800 µ s

- Modulation frequency: 20Mhz

We divided this low level data processing process in two different ways, according to the

detection objective. On the one hand we want to obtain information to calculate normal

estimations for each point in the cloud. On the other hand we want to check the hole

existence when there are no points in a specific zone of the cloud.

The Figure 14 shows a scheme with the data processing methods applied.

45


Figure 14: Scheme of the filtering process

• Amplitude and average filter

As it is shown in the filtering scheme ( 14), the average filtering and sparse outlier removal

are done only at the obstacle detection segment. Basically, the hole detection only needs

to know if there is information or not i a particular zone of the scene, and no accurate

noise reduction is needed.

As it is shown in the figure 15, the amplitude and average filtering reduces the noise

of the acquisition, increasing the signal-to-noise ratio.

46


Figure 15: Amplitude and average filtering

After experimental tests, the amplitud and average filtering parameters are set to:

- Amplitude threshold: 200 (arbitrary units)

- Average buffer size: 10 frames

• Point Cloud Downsampling

After the initial filtering processes, we obtain a large number of points in the point cloud,

and most of them are not adding reliable scene information. For this reason, it is inter-

esting to downsample the point cloud because the loses of obstacle information is small

and the computer performance will be increased. In the normal estimation segment, the

downsampling could be bigger than in the segment of hole detection because the existence

of information is important to detect holes (more data will be better and computationally

it has not much computational cost added).

47


The next Figure 16 shows how the point cloud is downsampled and it is still containg

valuable information.

Figure 16: Point Cloud Downsampling

On the one hand, after experimental tests, the downsampling parameters for obstacle

detection are set to:

- Leaf size: 0.08 m

- Filter limit minimum: 0 m

- Filter limit maximum: 5.0 m

On the other hand, the downsampling parameters for hole detection are set to:

- Leaf size: 0.02 m

48


- Filter limit minimum: 0 m

- Filter limit maximum: 5.0 m

• Sparse Outlier Removal

We computed a sparse outlier removal to purge those points whose mean distances are

outside an interval of the gaussian defined by the neighborhood distance mean and stan-

dard deviation.

After experimental tests, the sparse outlier removal parameters are set to:

- Number of neighbor points to compute the mean distance: 20 points

- Standard Deviation to remove outliers: 1σ

The Figure 17 shows how this filter removes statistically the outliers.

Figure 17: Statistical Outlier Removal

49


4.3 Obstacles and holes detection

To detect obstacles, a normal estimation for each pixel in the point cloud is calculed es-

timating a surface from its neighbors.

In order to see the results of such algorithm, the classification of traversable or non

traversable zones are coloured. A green point means traversable point and a red one

means obstacle. As an example, in the next Figures is shown different outdoor scenarios

inwhich the robot detects the obstacles. The detection of a wall (Figure 18), a Tree

(Figure 19) and stairs (Figure 20).

Figure 18: Wall detection

50


Figure 19: Tree detection

Figure 20: Stairs detection

51


After experimental tests, the obstacle detection parameters are set to:

- Number of neighbors to estimate the surface: 100 points

- Normal vector X (limit to be accepted): 1

- Normal vector Y (limit to be accepted): -0.2

- Normal vector Z (limit to be accepted): 1

In order to see the results of the hole detection, the zones to be analyzed are coloured in

blue tones. When a hole is detected, we generate a simple point cloud to mark the hole

as obstacle and to be able to feed it to the cost map.

The Figure 21 shows the comparison between the scene with and without a hole.

52


Figure 21: Hole detection

After experimental tests, the hole detection parameters are set to:

- Points threshold: 50 points

- Detection zone X: 0.6 m

- Detection zone initial Y: -0.05 m

- Detection zone final Y: 0.05 m

- Detection zone initial Z: 0.8 m

- Detection zone final Z: 1 m

53


4.4 Outdoor mobile robot navigation using a ToF camera

During the navigation tasks from one intermediate waypoint to the next, sensor data at

high frame rate is analyzed in order to determine whether the path is traversable, or

whether an obstacle is found, and a countermeasure command to safe navigate is com-

puted. This obstacles information is assembled into the costmap.

• Cost map

The costmap contains a representation of the world inwhich the robot is navegating with

a particular cost in each cell of the map. This cost describes the cell as free, occupied,

or unknown space. The costmap also considers the footprint of the robot and adds an

inflation radius to the obstacles detected.

The Figure 22 shows the costmap whith the footprint of the robot, the obstacle de-

tected and the inflation radius.

Figure 22: Costmap

54


• Path planning with obstacle avoidance

Using the costmap, the global planner uses Dijkstra ([1]) or A* ([2]) and assigns a poten-

tial field to each cell with the B-spline modification. Once this potential field is assigned,

the best path corresponds to the path with less cost cells.

In the Figure 23 is shown the efect of adding the B-spline to the current algorithms

on the global planner, to smooth the trajectory pose orientations using the kinematic of

the robot.

Figure 23: Global path planning without obstacle

To calculate the B-spline for Teo we did some experiments in order to choose the best

dt parameter. The Figure 24 shows generated B-spline trajectories by the robot using

different dt parameters. Finally the parameter choosed is dt = 0.5.

55


Figure 24: Global path planning without obstacle

To follow the path calculated by the global planner, the local planner is following the

potential field using Rollout Trajectory planner ([9]).

The Figure 25 shows the local window generated by the algorithm to calculate possibles

local trajectory commands to the global path with a pseudo-colour assigned, indicating

the best local trajectories to the path.

Figure 25: Local planner

56


When an obstacle appears, it updates the costmap and the global path is changed to

reach the goal, giving the capability of obstacle avoidance.

The Figure 26 shows the efect of an obstacle between the start and the goal using

front laser (black) and ToF camera.

Figure 26: Global path planning with obstacle (before and after obstacle detection)

In the above situation, the spline is only followed on those parts of the path without

obstacles.

As it is shown in the hardware configuration at Appendix-Teo robot, Teo uses differ-

ent sensoring devices. Thus we integrated the ToF camera to use all of them together to

properly navigate in outdoor unstructured environments.

Finally the Figure 27 shows different example frames of an outdoor navigation using

the ToF camera integrated with all sensoring devices from Teo with some obstacles in the

outdoor scene.

57


Figure 27: Outdoor navigation using ToF camera and Teo sensoring devices (front laser

in black and rear laser in blue)

58


5. Conclusions

In this master thesis we designed and implemented acquisition and data processing meth-

ods to obtain reliable 3D information of a section of terrain in front of a mobile robot.

These acquisition and data processing methods have been designed especially to work on

outdoor unstructured environments with a ToF camera as a main sensing device.

We designed and implemented obstacles and holes detection algorithms using the point

cloud provided by the ToF camera. On the one hand estimating a normal vector for

each point to discern between traversable or non traversable zones. On the other hand

analyzing the information of a certain section of terrain in front of the robot to find the

possible holes. These algorithms are ROS compatibles and work on real-time. Thus, it

could also work for other ROS-enabled robots.

We added the kinematic model of the robot to the path planner to correct navigate,

and a new B-spline method has introduced, to smooth trajectories, taking into account

the kinematics and pose orientations, and to save navigate performing obstacle avoidance.

Finally it has been done a validation of ROS compatible algorithms proposed on out-

door human environments in different representative scenes.

59


5.1 Future Work

The ToF camera is currently a new device and entails a technological handicap, the angle

of view (oav). This angle of view is currently 40 degrees, and suited in front of the robot

has 20 dregrees of vision on each side. Thus the potential of the ToF camera has been

reduced due to this small field of view. The next ToF camera generations will solve this

issue.

The modifications we done at the global planner are to take account of the B-spline

trajectory. We modified the propagation algorithm, changing how assigns the cost to

each cell by adding the cost of the start-to-goal B-spline. An other interesting method

could be calculating for each cell a B-spline to the goal, and assigning this cost to the

cell for each possible orientation. Once this costs have been calculated, the path to find

is the best that goes through the cells with less B-spline costs and with best possible

concatenated orientations.

Regarding at the local planner, both DWA ([4]) and Rollout Trajectory planner ([9])

uses a 2D local window to calculate control commands for the robot. They are not con-

sidering the difference between the 3D possible situations and their indeed variations on

this commands. With the new upcomming sensoring devices such as ToF cameras it will

be needed to improve these algorithms in a 3D local window.

60


Appendix

In this appendix is described the main tools that we used in this research.

First, the “ROS“ integrated software environment in which it was developed. Secondly,

the robot used to implement our algorithm. And Finally the PMD Camcube 3.0 specifi-

cations.

Robot Operating System (ROS)

Robot Operating System (ROS) is a software framework for robot software develop-

ment, providing operating system-like functionality on a heterogenous computer cluster.

ROS was originally developed in 2007 under the name switchyard by the Stanford Arti-

ficial Intelligence Laboratory in support of the Stanford AI Robot (STAIR) project. As

of 2008, development continues primarily at Willow Garage, a robotics research insti-

tute/incubator, with more than twenty institutions collaborating in a federated develop-

ment model.

ROS provides standard operating system services such as hardware abstraction, low-level

device control, implementation of commonly-used functionality, message-passing between

processes, and package management. It is based on a graph architecture where processing

takes place in nodes that may receive, post and multiplex sensor, control, state, planning,

actuator and other messages. The library is geared toward a Unix-like system. During

this master thesis we used ROS-electric release on Ubuntu Linux platform.

ROS has two basic ”sides”: The operating system side ros (as described above) and ros-

pkg, a suite of user contributed packages (organized into sets called stacks) that implement

functionality such as simultaneous localization and mapping, planning, perception, simu-

lation etc.

61


ROS is released under the terms of the BSD license, and is open source software. It

is free for commercial and research use. The ros-pkg contributed packages are licensed

under a variety of open source licenses.

ROS Filesystem Level

The filesystem level concepts are ROS resources that it may be encounter on disk, such

as:

• Packages: Packages are the main unit for organizing software in ROS and it is

used to organize software. Packages contain runtime processes (nodes), ROS depen-

dent library, datasets, configuration files, or anything else that is usefully organized

together.

• Manifests: Manifests contain metadata about packages.

• Stack: Stacks are collections of packages that provide aggregate functionality, such

as a ”navigation stack.” Stacks are also how ROS software is released and have

associated version numbers.

• Stack Manifest: Stack Manifests are metadata about stacks.

• Message types: Message descriptions, define the data structures for messages sent

in ROS.

• Service type: Service descriptions, define the request and response data structures

for services in ROS.

ROS Computational Graph Level

62


The Computation Graph is the peer-to-peer network of ROS processes that are processing

data together. The concepts of computation graph are:

• Nodes: Nodes are processes that perform computation. It is an executable file

within a ROS package and it uses a ROS client library to communicate with other

nodes. Nodes can publish or subscribe to a topic and it can also provide or use a

service.

• Master: ROS master provides name registration and lookup to the rest of the

computation graph. The master enables the nodes to find each other, exchange

messages or invoke services

• Parameter Server: Parameter server allows data to be stored by key in a central

location and it is currently part of master.

• Messages: A message is a simple data structure similar to C structures. It is used

to communicate between nodes.

• Topics: Topics are messages routed via the publish/subscribe semantics. A node

publishes a message under a topic and a node interested in certain kind of data

will subscribe to that topic. There may be multiple concurrent publishers and

subscribers in a topic. Publishers and subscribers do not know each other and this

is done to decouple the production and consumption of information.

• Services: The publish/subscribe model is a very flexible communication paradigm,

but its many-to-many, one-way transport is not appropriate for request/reply inter-

actions, which are often required in a distributed system. Request/reply is done via

services, which are defined by a pair of message structures: one for the request and

one for the reply.

• Bags: Bags are formats for saving and playing back ROS message data. It is used

to store data like sensor data that can be difficult to collect.

63


Communications in ROS

ROS enables several different styles of communications

• Synchronous: RPC style communication over services

• Asynchronous: Streaming of data over topics

• Storage of data on a parameter server

ROS is not realtime framework though it is possible to integrate

Methodology

1. ROS master stores topics, services and registration information of all nodes

2. Nodes communicate with the master to provide registration information

3. Nodes make connections with other nodes using information present in the ROS

master

4. Master callbacks nodes when registration information changes

Nodes connect to other nodes directly; the Master only provides lookup information, much

like a DNS server. Nodes that subscribe to a topic will request connections from nodes

that publish that topic, and will establish that connection over an agreed upon connec-

tion protocol. The most common protocol used in a ROS is called TCPROS, which uses

standard TCP/IP sockets.

Navigation Stack

The Navigation Stack is fairly simple on a conceptual level. It takes in information from

odometry and sensor streams and outputs velocity commands to send to a mobile base.

64


Use of the Navigation Stack on an arbitrary robot, however, is a bit more complicated.

As a pre-requisite for navigation stack use, the robot must be running ROS, have a tf

transform tree in place, and publish sensor data using the correct ROS Message types.

Also, the Navigation Stack needs to be configured for the shape and dynamics of a robot

to perform at a high level.

Although this stacks groups several packages, the most important ones used in this Master

Thesis are:

• move base: The move base package provides an implementation of an action that,

given a goal in the world, will attempt to reach it with a mobile base. The move base

node links together a global and local planner to accomplish its global navigation

task.

• nav core: This package provides common interfaces for navigation specific robot

actions. Currently, this package provides the BaseGlobalPlanner, BaseLocalPlan-

ner, and RecoveryBehavior interfaces, which can be used to build actions that can

easily swap their planner, local controller, or recovery behavior for new versions

adhering to the same interface. It has been modified during this master thesis to

perform the global planner with B-splines.

• base local planner/dwa local planner: This package provides implementations

of the Trajectory Rollout and Dynamic Window approaches to local robot navigation

on a plane. Given a plan to follow and a costmap, the controller produces velocity

commands to send to a mobile base.

• costmap 2d: This package provides an implementation of a 2D costmap that takes

in sensor data from the world, builds a 2D or 3D occupancy grid of the data (de-

pending on whether a voxel based implementation is used), and inflates costs in a

2D costmap based on the occupancy grid and a user specified inflation radius.

65


• voxel grid: provides an implementation of an efficient 3D voxel grid. The occu-

pancy grid can support 3 different representations for the state of a cell: marked,free

or unknown.

Using this packages, the navigation stack is capable to lead the mobile base to a

determined goal. In Figure 28 a conceptual overview of the navigation stack is shown.

This figure shows how it works, what topics and their types are needed to accomplish a

successful navigation.

Figure 28: ROS Navigation stack conceptual overview

The sensor transform node (tf) indicates relationships between different used frames.

This node is necessary to transform input sensor data from sensor sources to the robot

base frame.

66


TEO robot

Teo is a non-holonomic robot designed at the ”Institut de Robotica i Informatica Indus-

trial“ (IRI), a joint research center of the Technical University of Catalonia (UPC) and

the Spanish Council for Scientific Research (CSIC).

This robot is a mobile robot based on a Segway RMP400, and is made of two Segway

units rigidly attached in a skid steer configurations.

TEO Specifications

• Weight: 109 kg

• Payload: 181 kg

• Top speed: 29 km/h (8.1 m/s)

• Tires: 4x53 cm (D)

• Working temp.: -10C to 50C

• Batteries: (4+1) Li-Ion 74V 5.4 Ah

• Autonomy: 19..24 km (8h)

• Processing: Arbor FPC7300 - Rugged Industrial PC

• Commanding: DELL XPS M1330 ”longinos”, DELL ”tablet”

• Communications: Router D-Link DIR-825, Switch D-Link DES-1005D, Blue-

tooth dongle Trust 16008, Wifi dongle Linksys AE1000 dualband N

• Sensors: TCM3 Compass, Septentrio AstRx1 GPS with antena, PointGrey Flea

Cameras (x2), H3D 3D Laser, Hokuyo Lasers (x3), PMD Camcube 3.0 (ToF camera)

67


• Power supply: VI-J41-EW-B1 Conversor 74..12V, SDS-035A-05 Conversor 12..5V

• Other: Viper S4 Emergency Remote Relay

TEO robot uses Robot Operating System (ROS) and TEO’s ROS packages and drivers

have been designed and developed at IRI.

In the Figure 29 we can see the robot TEO and the configuration of some of the sensors,

as well as the PMD ToF camera camcube 3.0 and its configuration used during this master

thesis.

Figure 29: TEO robot

68


PMD camcube 3.0 camera (PMD technologies)

69


70


Acknowledgements

First of all, I would like to express my gratitude to my supervisor Dr. Juan Andrade for

his guidance and support. Martı Morta for his support during experimental tests and all

the other components of the Institut de Robotica i Informatica Industrial (IRI).

I am grateful to my classmates for having shared this master degree and especially L.

Molina, L. Orozco, J. Roses, A. Vargas i Ll. Febrer.

Finally I also would like to thank my family for the support they provided me through

my entire life and in particular to Nuria, without whom I had never started this Master

degree.

71


72


References

[1] E.W. Dijkstra, A note on two problems in connexion with graphs, Numerische Math-

ematik 1: 269271., doi:10.1007/BF01386390, 1959.

[2] P.E. Hart, N.J. Nilsson, B. Raphael, Correction to ”A Formal Basis for the Heuristic

Determination of Minimum Cost Paths”, SIGART Newsletter 37: 2829, 1972.

[3] M.D. Adams, P.J. Probert, Towards a real-time navigation strategy for a mobile robot,

1990 IEEE International Workshop on Intelligent Robots and Systems. ’Towards a

New Frontier of Applications’, Proceedings, IROS ’90, pp.743-748, vol.2, 1990.

[4] D. Fox, W. Burgard, S. thrun The dynamic window approach to collision avoidance,

IEEE Robotics & Automation Magazine, vol.4, num. 1, 1997.

[5] J.W. Weingarten, G. Gruener, R. Siegwart, A state-of-the-art 3D sensor for robot nav-

igation, 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems,

pp.2155-2160, 2004.

[6] S. May, B. Werner, H. Surmann, K. Pervolz, 3D time-of-flight cameras for mobile

robotics, 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems,

pp.790-795, 2006.

[7] L. Lapierre, R. Zapata, P. Lepinay, Combined path-following and obstacle avoidance

control of a wheeled robot, The International Journal of Robotics Research, Vol. 26,

No. 4, pp.361-375, 2007.

[8] S. Fuchs, G. Hirzinger, Extrinsic and depth calibration of ToF-cameras, 2008 IEEE

Conference on Computer Vision and Pattern Recognition, CVPR 2008, pp.1-6, 2008.

73


[9] B.P. Gerkey, K. Konolige, Planning and Control in Unstructured Terrain, 2008 IEEE

International Conference on Robotics and Automation, Workshop on Path Planning

on Costmaps, ICRA’08, 2008.

[10] B. Siciliano, O. Khatib, Springer Handbook of Robotics, Ed. Springer, ISBN 978-3-

540-23957-4, 2008.

[11] F. Yuan, A. Swadzba, R. Philippsen, O. Engin, M. Hanheide, S. Wachsmuth, Laser-

based navigation enhanced with 3D time-of-flight data, 2009 IEEE International Con-

ference on Robotics and Automation, ICRA ’09, pp.2844-2850, 2009.

[12] A. Ortega, I. Haddad, J. Andrade-Cetto, Graph-based Segmentation of Range Data

with Applications to 3D Urban Mapping, 2009 In Proc. of European Conference on

Mobile Robots, Mlini/Dubrovnik, Croatia, pp. 193-198, 2009.

[13] D.C. Moore, A.S. Huang, M. Walter, E. Olson, L. Fletcher, J. Leonard, S. Teller,

Simultaneous local and global state estimation for robotic navigation, 2009 In Proc. of

the IEEE International Conference on Robotics and Automation, ICRA’09, 2009.

[14] T.H. Cormen, C.E. Leiserson, R.L. Rivest, C. Stein, Introduction to Algorithms, The

MIT Press, 3rd Edition, ISBN 978-0-262-03384-8, 2009.

[15] E. Marder-Eppstein, E. Berger, T. Foote, B. Gerkey, K. Konolige, The Office

Marathon: Robust navigation in an indoor office environment, 2010 IEEE Interna-

tional Conference on Robotics and Automation, ICRA ’10, pp.300-307, 2010.

[16] A. Das, O. Naroditsky, Z. Zhiwei, S. Samarasekera, R. Kumar, Robust visual path

following for heterogeneous mobile platforms, 2010 IEEE International Conference on

Robotics and Automation, ICRA ’10, pp.2431-2437, 2010.

[17] M. Guo, W. Liu, Z. Wang, Robot Navigation Based on Multi-sensor Data Fusion, 2010

IEEE International Conference on Robotics and Automation, ICRA ’10, pp.2431-2437,

2010.

74


[18] S. Foix, G. Aleny, and C. Torras, Lock-in Time-of-Flight (ToF) Cameras: A Survey,

IEEE Sensors Journal, Vol.11, no9, 2011.

[19] Institut de Robotica i Informatica Industrial, Universitat Politecnica de Catalunya,

Teo Robot, http://wikiri.upc.es, consulted October 2011-May 2012

[20] Willow Garage, Robot Operating System, http://www.ros.org, consulted October

2011-May 2012

[21] Willow Garage, NVidia, Google, and Toyota Point Cloud Library, http://www.

pointclouds.org, consulted October 2011-May 2012

75

http://wikiri.upc.es

http://www.ros.org

http://www.pointclouds.org

http://www.pointclouds.org


76

universitat polit ecnica de catalunya - iri · universitat polit ecnica de catalunya summary...

Documents