Planning, Localization, and Mapping for a Mobile Robot in ... · PDF file Planning, Localization, and Mapping for a Mobile Robot in a Camera Network David Paul Meger Master of Science

Planning, Localization, and Mapping

for a Mobile Robot

in a Camera Network

David Paul Meger

Master of Science

School of Computer Science

McGill University

Montréal, Quebec

February 2007

A Thesis submitted to McGill University in partial fulfilment of the requirements for the degree of Master of Science

c©David Paul Meger MMVII

ACKNOWLEDGEMENTS

This thesis could not have been completed without the generosity and support

of a large group of people. My supervisor Professor Gregory Dudek has been an

amazing source of perspective, insight, and understanding and made the lab a fun

place to work. Equally, my co-supervisor Professor Ioannis Rekleitis provided tireless

enthusiasm and optimism for research. Despite often being at uncommon times and

locations, our meetings never failed to be productive and motivating.

Thanks to those students at McGill’s Centre for Intelligent Machines, and particu-

larly the Mobile Robotics Lab who have become great friends and supporters over

the last two years. I have gained equal portions of insight into research problems

and Montréal culture with their help.

Dimitri Marinakis was extremely generous in providing much of the networking code

to run the sensor network. Special thanks to Simon Drouin for translating the Ab-

stract to French.

I thank my family for their calm, unquestioning support and for being there without

being asked.

Finally, I would also like to thank the National Science and Engineering Research

Council of Canada for having provided me with the scholarship which has made it

possible for me to pursue this research.

ii

ABSTRACT

Networks of cameras such as building security systems can be a source of local-

ization information for a mobile robot assuming a map of camera locations as well

as calibration information for each camera is available. This thesis describes an au-

tomated system to acquire such information. A fully automated camera calibration

system uses fiducial markers and a mobile robot in order to drastically improve ease-

of-use compared to standard techniques. A 6DOF EKF is used for mapping and is

validated experimentally over a 50 m hallway environment. Motion planning strate-

gies are considered both in front of a single camera to maximize calibration accuracy

and globally between cameras in order to facilitate accurate measurements. For

global motion planning, an adaptive exploration strategy based on heuristic search

allows compromise between distance traveled and final map uncertainty which pro-

vides the system a level of autonomy which could not be obtained with previous

techniques.

iii

ABRÉGÉ

Des réseaux de caméras comme on en retrouve dans les systèmes de sécurité

d’édifice peuvent être utilisés comme source d’information de localisation pour un

robot automoteur si la position et les paramètres de calibration de chaque caméra

sont connus. Cette thèse présente un système qui permet d’obtenir cette informa-

tion automatiquement. Un système automatique de calibration de caméra utilise

des marqueurs et un robot automoteur pour améliorer la facilité d’utilisation com-

parée aux techniques existantes. Une méthode itérative de filtrage non linéraire

à 6DDL est utilisée pour cartographier l’environnement. Elle est validée de façon

expérimentale dans un réseau de couloirs de 50 m. Différentes stratégies de mouve-

ment sont étudiées: d’abord devant une seule caméra, pour maximiser la précision

de la calibration, puis, entre caméras pour améliorer la précision de la cartographie.

Pour la planification de mouvement globale, une stratégie d’exploration adaptive

basée sur l’algorithme A∗ permet de faire un compromis entre la distance parcourue

et la précision de la cartographie, ce qui procure au sytème un niveau d’autonomie

qui n’avait pu être obtenu avec les techniques existantes.

iv

TABLE OF CONTENTS

ACKNOWLEDGEMENTS . . . . . . . . . . . . . . . . . . . . . . . . . . . . ii

ABSTRACT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iii

ABRÉGÉ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iv

LIST OF TABLES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii

LIST OF FIGURES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . viii

1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.1 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 1.2 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.1 Related Sensor Network Applications . . . . . . . . . . . . . . . . 9 2.2 Camera Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.2.1 Fiducial Markers . . . . . . . . . . . . . . . . . . . . . . . . 13 2.3 Map Building . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14 2.4 Exploration Planning . . . . . . . . . . . . . . . . . . . . . . . . . 17

2.4.1 Coverage and Exploration . . . . . . . . . . . . . . . . . . . 18 2.4.2 Active Robot Localization . . . . . . . . . . . . . . . . . . 19 2.4.3 Map Error Reduction . . . . . . . . . . . . . . . . . . . . . 20 2.4.4 Multiple Objective Functions . . . . . . . . . . . . . . . . . 21

3 Calibration and Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

3.1 Automated Camera Calibration . . . . . . . . . . . . . . . . . . . 23 3.2 Localization and Mapping for a Camera Network . . . . . . . . . 28

3.2.1 Propagation Equations . . . . . . . . . . . . . . . . . . . . 30 3.2.2 Measurement Equations . . . . . . . . . . . . . . . . . . . . 30 3.2.3 UDU Factored Covariance Filter . . . . . . . . . . . . . . . 36

v

3.3 Large Scale Calibration and Mapping Experiments . . . . . . . . . 38

4 Local Calibration Path Planning . . . . . . . . . . . . . . . . . . . . . . 42

4.1 Heuristic Local Trajectories . . . . . . . . . . . . . . . . . . . . . 43 4.1.1 Experimental Evaluation of Heuristics . . . . . . . . . . . . 46

4.2 Calibration Error Study . . . . . . . . . . . . . . . . . . . . . . . 48 4.2.1 Ground Truth Parameter Determination . . . . . . . . . . 50 4.2.2 Calibration Error With Respect to Data Set Parameters . . 51

5 Global Exploratory Trajectories . . . . . . . . . . . . . . . . . . . . . . . 58

5.1 Challenges in Global Planning . . . . . . . . . . . . . . . . . . . . 60 5.1.1 Complexity Analysis of the Exploration Planning Problem 61 5.1.2 Distance and Uncertainty Trade-off . . . . . . . . . . . . . 62

5.2 Static Heuristic Exploration Trajectories . . . . . . . . . . . . . . 64 5.3 Adaptive Heuristic Exploration . . . . . . . . . . . . . . . . . . . 65

5.3.1 Heuristic Search For Distance and Uncertainty Planning . . 68 5.4 Simulated Exploration Results . . . . . . . . . . . . . . . . . . . . 72

5.4.1 Static Trajectory Results . . . . . . . . . . . . . . . . . . . 73 5.4.2 Single Path Results for the Adaptive Heuristic . . . . . . . 75 5.4.3 Global Exploration Results . . . . . . . . . . . . . . . . . . 79

6 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83

REFERENCES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88

vi

LIST OF TABLES Table page

4–1 Mean value and percentage standard deviation of the intrinsic parame- ters for each strategy over 10 trials. One Panel Translation-only is omitted due to divergence. Deviations are with respect to the mean; ground truth error is not provided. . . . . . . . . . . . . . . . 46

vii

LIST OF FIGURES Figure page

1–1 The experimental setup used throughout this thesis. The robot carries a calibration target which can be easily detected in images taken by the cameras in the network (such as the one mounted on a door here). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2–1 The pinhole camera model is a simple description for image formation. 13

2–2 A single ARTag marker. . . . . . . . . . . . . . . . . . . . . . . . . . 15

2–3 The calibration target is formed out of a regular grid of ARTag markers. 15

3–1 The calibration and mapping scenario described in this thesis. The robot moves through the environment, calibrating each camera and estimating both its own position as well as the positions of each camera in a common coordinate frame. . . . . . . . . . . . . . . . . 24

3–2 Sample images of the robot mounted calibration target which are used as input to the automated camera calibration procedure. . . . . . . 27

3–3 Positions of the robot, camera, and panel are related using homo- geneous transformation matrices T . Each arc in the diagram represents relative information either computed with the EKF, measured through calibration, or computed through combination of these sources. Deriving the measurement equation involves rep- resenting CRT using both the EKF information and the results of calibration so that these two results can be compared. . . . . . . . 35

3–4 Hard