INTELLIGENT AUTONOMOUS SYSTEMS 9
This page intentionally left blank
Intelligent Autonomous Systems 9 IAS-9
Edited by
Tamio Arai The University of Tokyo, Japan
Rolf Pfeifer University of Zurich, Switzerland
Tucker Balch Georgia Institute of Technology, USA
and
Hiroshi Yokoi The University of Tokyo, Japan
Amsterdam • Berlin • Oxford • Tokyo • Washington, DC
© 2006 The authors All rights reserved. No part of this book may be reproduced, stored in a retrieval system, or transmitted, in any form or by any means, without prior written permission from the publisher. ISBN 1-58603-595-9 Library of Congress Control Number: 2006920171 Publisher IOS Press Nieuwe Hemweg 6B 1013 BG Amsterdam Netherlands fax: +31 20 687 0019 e-mail:
[email protected] Distributor in the UK and Ireland Gazelle Books Falcon House Queen Square Lancaster LA1 1RN United Kingdom fax: +44 1524 63232
Distributor in the USA and Canada IOS Press, Inc. 4502 Rachael Manor Drive Fairfax, VA 22032 USA fax: +1 703 323 3668 e-mail:
[email protected]
LEGAL NOTICE The publisher is not responsible for the use which might be made of the following information. PRINTED IN THE NETHERLANDS
v
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
Preface The IAS-9 conference aims to address the main issues of concern within the IAS community. The conference covers both the applied as well as the theoretical aspects of intelligent autonomous systems. Autonomy and adaptivity are key aspects of truly intelligent artificial systems, dating from the first IAS conference in 1989. New directions of research have recently emerged from the synergetic interaction of many fields, such as cognitive science, operations research, mathematics, robotics, mechanics, electronics, informatics, and economics, interdisciplinary as well as transdisciplinarily. One key insight is that to realize both intelligence and autonomy, it is crucial to build real-world devices and abstract principles of design from them. The goal of IAS-9 is to lay out new scientific ideas and design principles for artificial systems able to survive in nature and in our society. The conference proceedings stimulate novel challenges as well as exciting research directions. A total of 146 scientific papers were submitted from 16 countries. All of the submitted papers were reviewed by the program committee, and 112 were accepted as full papers. We have 5 invited guest speakers at IAS-9: Andrew Adamatzky from the University of West England addresses the new direction of computation; Hod Lipson from Cornell University shows the frontier study of evolutionary robotics; Tomomasa Sato from The University of Tokyo presents the COE project of Japan for the real world application; Masahiro Fujita from SONY addresses the communication and service robotic system; and Shigeyuki Hosoe from RIKEN Bio-mimetic control research center shows the human analysis toward robotic learning. The conference takes place at Kashiwa new campus of the University of Tokyo, where frontier sciences are being created as “transdisciplinary” studies. A novel research center on artifacts, RACE, is also located on this campus with other three interdisciplinary research centers. I hope all participants of IAS-9 will enjoy the atmosphere of the campus and the facilities of the research building, and experience the novel trend of “transdisciplinary” studies in Japan. We sincerely appreciate the support of the Inoue Foundation of Science and Kayamori Foundation of Informational Science Advancement, the Robotics Society of Japan and the Research into Artifact Center of Engineering at the University of Tokyo. We would also like to express our gratitude to everybody of the program committee who contributed to the collection and the selection of high-level papers, and to the local committee members who supported the management of IAS-9. We look forward to seeing you at the conference site of IAS-9 in Tokyo. Tamio Arai, Rolf Pfeifer, Tucker Balch and Hiroshi Yokoi
vi
IAS-9 Conference Organization General Chair Tamio Arai, The Univ. of Tokyo, Japan Steering Committee Rüdiger Dillmann, Univ. of Karlsruhe, Germany Maria Gini, Univ. of Minnesota, USA Frans Groen, Univ. of Amsterdam, the Netherlands Thomas C. Henderson, University of Utah Yukinori Kakazu, Hokkaido Univ., Japan Enrico Pagello, Univ. of Padua and LADSEB-CNR, Italy Anthony Stentz, Carnegie Mellon Univ., USA Program Committee Co-Chairs In America: Tucker Balch, Georgia Institute of Technology, USA In Europe/Africa: Rolf Pfeifer, The Univ. of Zurich, Switzerland In Asia/Oceania: Hiroshi Yokoi, The Univ. of Tokyo, Japan Publicity Chair Kanji Ueda, Univ. of Tokyo, Japan Organized Session Chair Jun Ota, Univ. of Tokyo, Japan Max Lungarella, Univ. of Tokyo, Japan Local Organization Hajime Asama, Univ. of Tokyo, Japan Yusuke Maeda, Yokohama National Univ., Japan Masao Sugi, Univ. of Tokyo, Japan Ryosuke Chiba, Univ. of Tokyo, Japan Ryuichi Ueda, Univ. of Tokyo, Japan Program Committee Europe Fumiya Iida, The University of Zurich, Switzerland Miriam Fend, The University of Zurich, Switzerland Gabriel Gomez, The University of Zurich, Switzerland Giorgio Metta, University of Genoa, Italy Giulio Sandini, University of Genova, Italy Frank Pasemann, Frauenhofer Institute for Autonomous Intelligent Systems, Germany Tom Ziemke, University of Skovde, Sweden
vii
Noel Sharkey, University of Sheffield, England Owen Holland, University of Essex, England Barbara Webb, University of Edinburgh, Scottland Philippe Gaussier, Cergy Pontoise University, France Arnaud Revel, ENSEA, France Pierre-Yves Oudeyer, Sony Computer Science Lab., France Francesco Mondada, EPFL, Switzerland Roland Siegwart, EPFL, Switzerland Rüdiger Dillmann, University of Karlsruhe, Germany Sven Behnke, University of Freiburg, Germany Raoul Rojas, Freie Universitat Berlin, Germany Auke Ijspeert, EPFL, Switzerland Aude Billard, EPFL, Switzerland Andrew Adamatzky, University of West England, England Huosheng Hu, University of Essex, England Stefano Carpin, International University Bremen, Germany Daniele Nardi, University of Roma “La Sapienza”, Italy Alicia Casals, Technical University of Catalonia, Spain Ray Jarvis, Monash University, Australia Pedro Lima, Technical University of Lisbon, Portugal Enzo Mumolo, University of Trieste, Italy Carme Torras, Institut de Robotica i Informatica Industrial, Spain Ernesto Burattini, University of Naples “Federico II”, Italy Andrea Bonarini, Politecnico di Milano, Italy Gerhard K. Kraetzschmar, University of Applied Sciences Bonn-Rhein-Sieg, Germany Antonio D’Angelo, University of Udine, Italy Angel P. del Pobil, Universitat Jaume I, Spain America Paul Scerri, Carnegie Mellon University Tony Stentz, Carnegie Mellon University Omead Amidi, Carnegie Mellon University Doug MacKenzie, Mobile Intelligence Inc. Maria Gini, University of Minnesota Sven Koenig, University of Southern California Daniel Lee, University of Pennsylvania Devin Balkom, Dartmouth University Wes Huang, Rensselaer Polytechnic Institute Joelle Pineau, McGill University Tom Collins, Georgia Institute of Technology Eric Johnson, Georgia Institute of Technology Artur Arsenio, Massachusetts Institute of Technology Jose Carmena, UC Berkeley Darrin Bentivegna, ATR, Kyoto, Japan Josh Bongard, Cornell University Paul Fitzpatrick, Massachusetts Institute of Technology Steve Collins, University of Michigan
viii
Chandana Paul, Cornell University Chris Atkeson, Carnegie Mellon University Asia Tetusnari Inamura, The University of Tokyo, Japan Yoshihiko Nakamura, The University of Tokyo, Japan Yuichi Kobayashi, RIKEN BMC, Japan Hiroaki Yamaguchi, Musashi Institute of Technology, Japan Yoshihiro Miyake, Tokyo Institute of Technology, Japan Yasutake Takahashi, Osaka University, Japan Shinkichi Inagaki, Nagoya University, Japan Kousuke Inoue, Ibaraki University, Japan Wenwei Yu, Chiba University, Japan Yuko Ishiwaka, Hokkaido University, Japan Toshiyuki Kondo, Tokyo Institute of Technology, Japan Toshio Hori, AIST, Japan Mitsuo Wada, Hokkaido University, Japan Mihoko Otake, The University of Tokyo, Japan Masashi Furukawa, Asahikawa National College of Technology, Japan Koichi Osuka, Kobe University Koichi Nishiwaki, AIST, Japan Koh Hosoda, Osaka University Keiji Suzuki, Future University – Hakodate, Japan Kazuhiro Ohkura, Kobe University, Japan Takashi Kawakami, Hokkaido Institute of Technology, Japan Katsuyoshi Tsujita, Osaka Institute of Technology, Japan Jun Hakura, Iwate Prefectural University, Japan Kosei Ishimura, Hokkaido University, Japan Hiroshi Ishiguro, Osaka University, Japan Akio Ishiguro, Nagoya University, Japan
ix
Contents Preface Tamio Arai, Rolf Pfeifer, Tucker Balch and Hiroshi Yokoi IAS-9 Conference Organization
v vi
Papers of Invited Guest Speakers Reaction-Diffusion Intelligent Wetware Andrew Adamatzky Evolutionary Robotics for Legged Machines: From Simulation to Physical Reality Hod Lipson, Josh Bongard, Victor Zykov and Evan Malone Real World Informatics Environment System Tomomasa Sato Understanding and Realization of Constrained Motion – Human Motion Analysis and Robotic Learning Approaches Shigeyuki Hosoe, Yuichi Kobayashi and Mikhail Svinin
3
11 19
30
Part 1. Navigation and Motion Planning The Specifiability Requirement in Mobile Robot Self-Localization Francesco Capezio, Antonio Sgorbissa and Renato Zaccaria
41
Ghost-Goal Algorithm for Reactive Safe Navigation in Outdoor Environments Mattia Castelnovi, Antonio Sgorbissa and Renato Zaccaria
49
Autonomous Robot Vision System for Environment Recognition Woong-Jae Won, Sang-Woo Ban and Minho Lee
57
Multi-Resolution Field D* Dave Ferguson and Anthony Stentz
65
Path and Observation Planning of Vision-Based Mobile Robots with Multiple Sensing Strategies Mitsuaki Kayawake, Atsushi Yamashita and Toru Kaneko
75
Mobile Robot Motion Planning Considering Path Ambiguity of Moving Obstacles Hiroshi Koyasu and Jun Miura
85
Robotic Navigation Using Harmonic Functions and Finite Elements Santiago Garrido and Luis Moreno
94
x
The Hippocampal Place Cells and Fingerprints of Places: Spatial Representation Animals, Animats and Robots Adriana Tapus, Francesco Battaglia and Roland Siegwart
104
Incremental Reconstruction of Generalized Voronoi Diagrams on Grids Nidhi Kalra, Dave Ferguson and Anthony Stentz
114
Learning from Nature to Build Intelligent Autonomous Robots Rainer Bischoff and Volker Graefe
124
Part 2. Tracking Control & Active Vision A Laser Based Multi-Target Tracking for Mobile Robot Masafumi Hashimoto, Satoshi Ogata, Fuminori Oba and Takeshi Murayama Simultaneous Environment Mapping and Mobile Target Tracking Abedallatif Baba and Raja Chatila
135
Omnidirectional Active Vision for Evolutionary Car Driving Mototaka Suzuki, Jacob van der Blij and Dario Floreano
153
145
Part 3. Localization Map of Color Histograms for Robot Navigation Takanobu Kawabe, Tamio Arai, Yusuke Maeda and Toshio Moriya
165
Designing a System for Map-Based Localization in Dynamic Environments Fulvio Mastrogiovanni, Antonio Sgorbissa and Renato Zaccaria
173
Appearance-Based Localization of Mobile Robots Using Local Integral Invariants Hashem Tamimi, Alaa Halawani, Hans Burkhardt and Andreas Zell
181
Enhancing Self Covertness in a Hostile Environment from Expected Observers at Unknown Locations Mohamed Marzouqi and Ray Jarvis
189
Part 4. Multi-Agent Robots An Experimental Study of Distributed Robot Coordination Stefano Carpin and Enrico Pagello Single-Sensor Probabilistic Localization on the SeReS Self-Reconfigurable Robot Kenneth Payne, Jacob Everist, Feili Hou and Wei-Min Shen Mutual Localization and 3D Mapping by Cooperative Mobile Robots Julian Ryde and Huosheng Hu
199
207 217
xi
Part 5. Network Agent Systems Exploration of Complex Growth Mechanics of City Traffic Jam for the Adaptive Signal Control Kouhei Hamaoka and Mitsuo Wada Coordinated Control of Mobile Antennas for Ad-Hoc Networks in Cluttered Environments Gianluca Antonelli, Filippo Arrichiello, Stefano Chiaverini and Roberto Setola An Adaptive Behavior of Mobile Ad Hoc Network Agents Masao Kubo, Chau Dan, Hiroshi Sato and Takashi Matubara
227
235
243
Part 6. Evolution and Learning Learning the Cooperative Behaviors of Seesaw Balancing Agents – An Actor-Critic Aproach – Takashi Kawakami, Masahiro Kinoshita and Yukinori Kakazu
255
Evolutionary Reinforcement Learning for Simulated Locomotion of a Robot with a Two-Link Arm Yohannes Kassahun and Gerald Sommer
263
Metric State Space Reinforcement Learning for a Vision-Capable Mobile Robot Viktor Zhumatiy, Faustino Gomez, Marcus Hutter and Jürgen Schmidhuber
272
Transition Entropy in Partially Observable Markov Decision Processes Francisco S. Melo and Isabel Ribeiro
282
Movement Control of Tensegrity Robot Masaru Fujii, Shinichiro Yoshii and Yukinori Kakazu
290
An Adaptive Neural Controller for a Tendon Driven Robotic Hand Gabriel Gómez, Alejandro Hernandez and Peter Eggenberger Hotz
298
Self-Organizing Route Guidance Systems Based on Coevolution of Multi-Layered Guidance Vector Fields Yasuhiro Ohashi and Kosuke Sekiyama
308
Object Transportation Using Two Humanoid Robots Based on Multi-Agent Path Planning Algorithm Shotaro Kamio and Hitoshi Iba
318
Cognitive Map Plasticity and Imitation Strategies to Extend the Performance of a MAS P. Laroque, E. Fournier, P.H. Phong and P. Gaussier
326
Examination of Abilities Based on Pseudolite System for Indoor Positioning Isamu Kitano and Keiji Suzuki
334
xii
A Memory-Based PID Controller for Indoor Airship Robot Takamasa Sato and Keiji Suzuki
341
Co-Evolutionary Design for AGV Systems Ryosuke Chiba, Jun Ota and Tamio Arai
349
An Update Method of Computer Simulation for Evolutionary Robotics Yoshiaki Katada and Kazuhiro Ohkura
357
Vision-Based Teleoperation of a Mobile Robot with Visual Assistance Naoyuki Kubota, Daisuke Koudu, Shinichi Kamijima, Kazuhiko Taniguchi and Yasutsugu Nogawa
365
Part 7. Adaptation Adaptive Control Strategy for Micro/Nano Manipulation Systems Hwee Choo Liaw, Denny Oetomo, Bijan Shirinzadeh and Gursel Alici Smart Roadster Project: Setting up Drive-by-Wire or How to Remote-Control Your Car Joachim Schröder, Udo Müller and Rüdiger Dillmann
375
383
A Reactive Approach for Object Finding in Real World Environments Abdelbaki Bouguerra
391
A Geometric Approach for an Intuitive Perception System of Humanoids David Israel Gonzalez-Aguirre and Eduardo Jose Bayro-Corrochano
399
Autonomous Learning of a Topological Model in a Road Network Gabriel Aviña Cervantès and Michel Devy
408
Reinforcement Learning Performance Evaluation: An Evolutionary Approach Genci Capi and Masao Yokota
416
Quantify Distinguishability in Robotics Aurélien Hazan, Frédéric Davesne, Vincent Vigneron and Hichem Maaref
425
Group Transport Along a Robot Chain in a Self-Organised Robot Colony Shervin Nouyan, Roderich Groß, Marco Dorigo, Michael Bonani and Francesco Mondada
433
Growing Virtual Neural Tissue: Binding Spiking Neurons Through Sensory Input Pascal Kaufmann and Gabriel Gómez
443
Part 8. Emergent Synthesis Hormone-Inspired Adaptive Distributed Synchronization of Reconfigurable Robots Feili Hou and Wei-Min Shen
455
xiii
Spatial Prisoner’s Dilemma in a Network Environment Introducing Heterogeneous Information Distribution Hiroshi Kuraoka, Nobutada Fujii and Kanji Ueda Behavioral Decision for Multi-Agent Systems with Dynamic Interaction Yusuke Ikemoto and Toshio Fukuda
463 471
Co-Creative Composition Using Multiagent Learning: Toward the Emergence of Musical Structure Shintaro Suzuki, Takeshi Takenaka and Kanji Ueda
479
Self-Assembly of Mobile Robots: From Swarm-Bot to Super-Mechano Colony Roderich Groß, Marco Dorigo and Masaki Yamakita
487
Lot Release Control Using Genetics Based Machine Learning in a Semiconductor Manufacturing System Ryohei Takasu, Nobutada Fujii, Kanji Ueda and Motohiro Kobayashi
497
Design of an AGV Transportation System by Considering Management Model in an ACT Satoshi Hoshino, Jun Ota, Akiko Shinozaki and Hideki Hashimoto
505
Analysis of Purchase Decision Making: Network Externalities and Asymmetric Information Yohei Kaneko, Nariaki Nishino, Sobei H. Oda and Kanji Ueda
515
Part 9. Dynamics, Morphology, and Materials in Intelligent Behavior Emergence of Insect Navigation Strategies from Homogeneous Sensorimotor Coupling Simon Bovet
525
Active Learning of Local Structures from Attentive and Multi-Resolution Vision Maxime Cottret and Michel Devy
534
Modular Design of Home Service Robot System with Hierarchical Colored Petri Net Guohui Tian, Feng Duan and Tamio Arai
542
Auctions for Task Allocation to Robots Maitreyi Nanjanath and Maria Gini
550
Exploration of Natural Dynamics Through Resonance and Chaos Alex Pitti, Max Lungarella and Yasuo Kuniyoshi
558
One-Legged Locomotion with a Compliant Passive Joint Juergen Rummel, Fumiya Iida and Andre Seyfarth
566
Analysis of Dynamical Locomotion of Two-Link Locomotors Kojiro Matsushita, Hiroshi Yokoi and Tamio Arai
574
xiv
Part 10. Mobiligence A Modular Robot That Self-Assembles Akio Ishiguro, Hiroaki Matsuba, Tomoki Maegawa and Masahiro Shimizu
585
Autonomous Robust Execution of Complex Robotic Missions Paul Robertson, Robert Effinger and Brian Williams
595
Emergence of Small-World in Ad-Hoc Communication Network Among Individual Agents Daisuke Kurabayashi, Tomohiro Inoue, Akira Yajima and Tetsuro Funato
605
Parametric Path Planning for a Car-Like Robot Using CC Steers Weerakamhaeng Yossawee, Takashi Tsubouchi, Masamitsu Kurisu and Shigeru Sarata
613
Self-Organizing Planner for Multiple Mobile Robot Exploration and Patrol Chomchana Trevai, Jun Ota and Tamio Arai
622
Adaptive Action Selection of Body Expansion Behavior in Multi-Robot System Using Communication Tomohisa Fujiki, Kuniaki Kawabata and Hajime Asama
632
From Mobility to Autopoiesis: Acquiring Environmental Information to Deliver Commands to the Effectors Antonio D’Angelo and Enrico Pagello
640
Part 11. RoboCup Cooperative Agent Behavior Based on Special Interaction Nets Oliver Zweigle, Reinhard Lafrenz, Thorsten Buchheim, Uwe-Philipp Käppeler, Hamid Rajaie, Frank Schreiber and Paul Levi A Framework for Advanced Robot Programming in the RoboCup Domain – Using Plug-In System and Scripting Language Hayato Kobayashi, Akira Ishino and Ayumi Shinohara Major Behavior Definition of Football Agents Through XML José Luis Vega, Ma. de los Ángeles Junco and Jorge Ramírez Getting Back on Two Feet: Reliable Standing-Up Routines for a Humanoid Robot Jörg Stückler, Johannes Schwenk and Sven Behnke
651
660 668
676
Ball Tracking with Velocity Based on Monte-Carlo Localization Jun Inoue, Akira Ishino and Ayumi Shinohara
686
Fast Vector Quantization for State-Action Map Compression Kazutaka Takeshita, Ryuichi Ueda and Tamio Arai
694
xv
Incremental Purposive Behavior Acquisition Based on Modular Learning System Tomoki Nishi, Yasutake Takahashi and Minoru Asada
702
Part 12. Real World Information Systems Simple Form Recognition Using Bayesian Programming Guy Ramel, Adriana Tapus, François Aspert and Roland Siegwart
713
Towards Robust State Estimation with Bayesian Networks: A New Perspective on Belief Propagation Jan Nunnink and Gregor Pavlin
722
HRP-2W: A Humanoid Platform for Research on Support Behavior in Daily Life Environments Tetsunari Inamura, Kei Okada, Masayuki Inaba and Hirochika Inoue
732
Human Supporting Production Cell “Attentive Workbench” Masao Sugi, Yusuke Tamura, Makoto Nikaido, Jun Ota, Tamio Arai, Kiyoshi Takamasu, Kiyoshi Kotani, Akio Yamamoto, Hiromasa Suzuki, Yoichi Sato, Fumihiko Kimura and Seiichi Shin
740
A Foveal 3D Laser Scanner Integrating Texture into Range Data Marcus Walther, Peter Steinhaus and Rüdiger Dillmann
748
Autonomous Collaborative Environment for Project Based Learning Mihoko Otake, Ryo Fukano, Shinji Sako, Masao Sugi, Kiyoshi Kotani, Junya Hayashi, Hiroshi Noguchi, Ryuichi Yoneda, Kenjiro Taura, Nobuyuki Otsu and Tomomasa Sato
756
Part 13. Humanoid Robots Pedaling Motion of a Cycle by Musculo-Skeletal Humanoid with Adapting Ability Based on an Evaluation of the Muscle Loads Tomoaki Yoshikai, Yuto Nakanish, Ikuo Mizuuchi and Masayuki Inaba
767
Behavior Transition Between Biped and Quadruped Walking by Using Bifurcation Kenji Asa, Kosei Ishimura and Mitsuo Wada
776
Tendon Arrangement Based on Joint Torque Requirements for a Reinforceable Musculo-Skeletal Humanoid Yuto Nakanishi, Ikuo Mizuuchi, Tomoaki Yoshikai, Tetsunari Inamura and Masayuki Inaba Vision-Based Virtual Information and Semi-Autonomous Behaviours for a Humanoid Robot Olivier Stasse, Jean Semere, Neo Ee Sian, Takashi Yoshimi and Kazuhito Yokoi
786
794
xvi
Load Distributed Whole-Body Motion Generation Method for Humanoids by Minimizing Average Joint Torque Ratio Ryusuke Adachi, Shigeru Kanzaki, Kei Okada and Masayuki Inaba Measurement and Simulation Verification of Reflexive Responses to Perturbation During Walking Shahed Sarwar, Wenwei Yu, Masaru Kumagai, Masaki Sekine, Tamotsu Katane, Toshiyo Tamura and Osami Saitou Toward a Human-Like Biped Robot with Compliant Legs Fumiya Iida, Yohei Minekawa, Juergen Rummel and Andre Seyfarth
804
812
820
Part 14. Service Robotics and Human Support Using JavaSpace for a PEIS Ecology Beom Su Seo, Mathias Broxvall, Marco Gritti, Alessandro Saffiotti and Jung Bae Kim A New Heating Method for the Actuation of the Shape Memory Alloy (SMA) Actuator Chee Siong Loh, Kojiro Matsushita, Hiroshi Yokoi and Tamio Arai
831
839
Informational Organization on Network Among Multiple Agents Yoshihito Shikanai, Koichi Ozaki and Sumio Yamamoto
847
A Flexible Task Knowledge Representation for Service Robots Steffen Knoop, Sven R. Schmidt-Rohr and Rüdiger Dillmann
856
Intelligent Autonomous Japanese Comic “MANGA” Designing Support System Yuko Ishiwaka and Yuka Kobayasi
865
Part 15. Human Behavior Analysis Behavior Induction by Geometric Relation Between Symbols of Multi-Sensory Pattern Naoki Kojo, Tetsunari Inamura and Masayuki Inaba Effects of Robotic Arm Orthosis Behaviors to User’s Motion Structure. Qualitative Assessment Using Arm Trajectory Profiles Yukio Horiguchi, Satoshi Tsukamoto, Hiroyuki Ono, Tetsuo Sawaragi and Masahiro Sato A Human Behavior Discrimination Method Based on Motion Trajectory Measurement for Indoor Guiding Services Hajime Asama, Atsushi Morimoto, Kuniaki Kawabata and Yasushi Hada
875
883
891
xvii
Part 16. Mutual Adaptation among man and machines Effects of Shared Communicational Modality to Joint Activity of Human Operator and Robot Autonomy Yukio Horiguchi and Tetsuo Sawaragi Learning of Object Identification by Robots Commanded by Natural Language Chandimal Jayawardena, Keigo Watanabe and Kiyotaka Izumi An f-MRI Study of an EMG Prosthetic Hand Biofeedback System Alejandro Hernández A., Hiroshi Yokoi, Takashi Ohnishi and Tamio Arai Wearable Inertial Sensors for Arm Motion Tracking in Home-Based Rehabilitation Huiyu Zhou, Huosheng Hu and Nigel Harris
903 913 921
930
Learning and Control Model of Arm Posture K.S. Kim, H. Kambara, D. Shin, M. Sato and Y. Koike
938
Competitive Learning Method for Robust EMG-to-Motion Classifier Ryu Kato, Hiroshi Yokoi and Tamio Arai
946
Mutual Adaptation Among Man and Machine by Using f-MRI Analysis Hiroshi Yokoi, Alejandro Hernandez Arieta, Ryu Katoh, Takashi Ohnishi, Wenwei Yu and Tamio Arai
954
Part 17. Women in Robotics, Human Science and Technology Behavior Generation of Humanoid Robots Depending on Mood Kazuko Itoh, Hiroyasu Miwa, Yuko Nukariya, Massimiliano Zecca, Hideaki Takanobu, Stefano Roccella, Maria Chiara Carrozza, Paolo Dario and Atsuo Takanishi Construction of Human-Robot Cooperating System Based on Structure/Motion Model Fumi Seto, Yasuhisa Hirata and Kazuhiro Kosuge
965
973
Motion Assist Devices for Rehabilitation Using Parallel Wire Mechanisms Keiko Homma
981
Analysis of Skill Acquisition Process – A Case Study of Arm Reaching Task – Kahori Kita, Ryu Kato, Hiroshi Yokoi and Tamio Arai
991
Generation of Size-Variable Image Template for Self-Position Estimation Considering Position Shift Kae Doki, Naohiro Isetani, Akihiro Torii and Akiteru Ueda Subjective Age Estimation Using Facial Images – the Effects of Gender, Expressions and Age Groups – Noriko Nagata, Naoyuki Miyamoto, Yumi Jinnouchi and Seiji Inokuchi
999
1007
xviii
Evaluation and Suppression of Overrun of Microorganisms Using Dynamics Model for Microrobotic Application Naoko Ogawa, Hiromasa Oku, Koichi Hashimoto and Masatoshi Ishikawa From Muscle to Brain: Modelling and Control of Functional Materials and Living Systems Mihoko Otake
1015
1025
Using the Sony AIBOs to Increase Diversity in Undergraduate CS Programs Maria Gini, Jan Pearce and Karen Sutherland
1033
Author Index
1041
Papers of Invited Guest Speakers
This page intentionally left blank
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
3
Reaction-Diffusion Intelligent Wetware Andrew Adamatzky Unconventional Computing Centre, Faculty of Computing, Engineering and Mathematical Sciences, University of the West of England, Bristol, United Kingdom
[email protected]
Abstract. We give an overview of recent results on implementation of computing, actions, and emotions in spatially extended reaction-diffusion chemical systems [1-3]. We pinpoint all essential ingredients of intelligence found in spatio-temporal dynamics of nonlinear chemical systems, and show outlines of future designs and prototypes of chemical intelligent ‘gooware’.
Reaction-diffusion chemical systems are well known for their unique ability to efficiently solve combinatorial problems with natural parallelism. In spatially distributed chemical processors, the data and the results of the computation are encoded as concentration profiles of the chemical species. The computation per se is performed via the spreading and interaction of wave fronts. The reaction-diffusion computers are parallel because the chemical medium’s micro-volumes update their states simultaneously, and molecules diffuse and react in parallel [1,3]. During the last decades a wide range of experimental prototypes of reaction-diffusion computing devices have been fabricated and applied to solve various problems of computer science, including: image processing, pattern recognition, path planning, robot navigation, computational geometry, logical gates in spatially distributed chemical media, arithmetical and memory units. These important ņ but scattered across many scientific fields ņ results convince us that reaction-diffusion systems can do a lot. Are they are capable enough to be intelligent? Yes, reaction-diffusion systems are smart ņ showing a state of readiness to respond, able to cope with difficult situations, capable for determining something by mathematical and logical methods ņ and endowed with capacity to reason. Sensing and Reacting Reaction-diffusion computers allow for massive parallel input if data. Equivalently, reaction-diffusion robots would need no dedicated sensors, each micro-volume of the medium, each site of the matrix gel, is sensitive to changes in one or another physical characteristic of the environment. Electric field, temperature and illumination are ‘sensed’ by reaction-diffusion devices, and these are three principle parameters in controlling and programming reaction-diffusion robots. Thus, for example, velocity of excitation waves in
4
A. Adamatzky / Reaction-Diffusion Intelligent Wetware
Belousov-Zhabotinsky reactors is increased by a negative and decreased by a positive electric field, waves are splitted in two by high intensity electric field, wave-fronts can be (de)stabilised, spiral waves are generated by alternating fields, morphological diversity of generated patterns can be increased. Increasing temperature is known to drive space-time dynamics of the Belousov-Zhabotinsky reactor from periodic to chaotic oscillations. Light is a key to sophisticated control of excitable (light-sensitive) chemical media: applying light of varying intensity we can tune medium’s excitability, waves’ velocities, pattern formation. Coping with Difficult Tasks Hard computational problems of geometry, image processing and optimisation on graphs are resource-efficiently solved in reaction-diffusion media due to intrinsic natural parallelism of the problems [1,3]. Let us look at three examples. The Voronoi diagram is a subdivision of plane by data planar set. Each point of the data set is represented by a drop of a reagent. The reagent diffuses and produces a colour precipitate when reacting with the substrate. When two or more diffusive fronts of the ‘data’ chemical species meet, no precipitate is produced (due to concentration-dependent inhibition). Thus, uncoloured domains of the computing medium represent bisectors of the Voronoi diagram. A skeleton of a planar shape is computed in the similar manner. A contour of the shape is applied to computing substrate as a disturbance in reagent concentrations. The contour concentration profile induces diffusive waves. A reagent diffusing from the data-contour reacts with the substrate and the precipitate is formed. Precipitate is not produced at the sites of diffusive waves’ collision. The uncoloured domains correspond to the skeleton of the data shape. To compute a collision-free shortest path in a space with obstacles, we can couple two reaction-diffusion media. Obstacles are represented by local disturbances of concentration profiles in one of the media. The disturbances induce circular waves travelling in the medium and approximating a scalar distance-to-obstacle field. This field is mapped onto the second medium, which calculates a tree of ‘many-sources-one-destination’ shortest paths by spreading wave-fronts [3]. Logical reasoning Most known so far experimental prototypes of reaction-diffusion processors exploit interaction of wave fronts in a geometrically constrained chemical medium, i.e. the computation is based on a stationary architecture of medium’s inhomogeneities. Constrained by a stationary wires and gates RD chemical universal processors pose a little computational novelty and none dynamical reconfiguration ability because they simply imitate architectures of conventional silicon computing devices. To appreciate in full massive-parallelism of thin-layer chemical media and to free the chemical processors from limitations of fixed computing architectures we adopt an unconventional paradigm of architecture-less, or collision-based, computing. A non-linear medium processor can be either specialised or general-purpose (universal). A specialized processor is built to solve only one particular problem, possibly
A. Adamatzky / Reaction-Diffusion Intelligent Wetware
5
with different data sets and variations in interpretations of results. Specialised computing devices are quite handy when we deal with image processing, problems of mathematical morphology, or computation on graphs [1]. A device is called computation universal if it computes any logical function. Therefore to prove a medium's universality we must represent quanta of information, routes of information transmission and logical gates, where information quanta are processes, in states of the given system. We could highlight two types of computational universality. They can be generally named architecture-based, or stationary, and architecture-less, or collision-based, types of a universal computation. An architecture-based, or stationary, computation implies that a logical circuit is embedded into the system in such a manner that all elements of the circuit are represented by the system's stationary states. The architecture is static. If there is any kind of 'artificial' or 'natural' compartmentalisation the medium is classified as an architecture-based computing device. Personal computers, living neural networks, cells, and networks of chemical reactors are typical examples of architecture-based computers.
Figure 1. Implementation of simple logical gate in numerical model of sub-excitable Belousov-Zhabotinsky medium. Boolean variables X and Y are represented by wave-fragments travelling west and east, respectively. In this particular example, both variables take value Truth. When wave-fragments collide, they change their trajectories. Thus signals following new trajectories represent logical conjunction X AND Y. If one of the wave-fragments were absent, then another fragment would travel undisturbed and its undisturbed trajectory represent operation NOT X AND Y or X AND NOT Y, respectively.
A collision-based, or dynamical, computation employs mobile compact finite patterns, mobile self-localized excitations or simply localisations, in active non-linear medium (Fig. 1). The localisations travel in space and do computation when they collide with each other. Essentials of collision-based computing are following. Information values (e.g. truth values of logical variables) are given by either absence or presence of the localisations or other parameters of the localisations. The localisations travel in space and do computation when they collide with each other. There are no predetermined stationary wires (a trajectory of the travelling pattern is a momentarily wire). Almost any part of the medium space can be used as a wire. Localisations can collide anywhere within a space sample, there are no fixed positions at which specific operations occur, nor location specified gates with fixed operations. The localisations undergo transformations, form bound states, annihilate or fuse when they interact with other mobile patterns. Information values of localisations are transformed in result of collision and thus a computation is implemented [1,3].
6
A. Adamatzky / Reaction-Diffusion Intelligent Wetware
Chemical Emotions Sensing, computing and reasoning are essential but not sufficient components of intelligence: emotions are the prime ingredient which turns the brew of computing into actions of apprehension, cooperation, creation and synergy. In reaction-diffusion systems, an affective mixture is a theoretical construct which represents emotions spreading and reacting one with another as a massive pool of locally interacting entities. Molecules of the affective mixture correspond to basic emotions ņ happiness, anger, fear, confusion and sadness ņ which diffuse and react with each other by quasi-chemical laws (Fig. 2).
Figure 2. Space-time development of one-dimensional chemical reactor mimicking evolution of emotional pool with happiness, anger and confusion [2].
In computational experiments with affective solutions, in well-stirred and thin-layer reactors, we uncover and classify varieties of behavioural modes and regimes determined by particulars of emotion interactions. We demonstrate that a paradigm of affective solutions offers an unconventional but promising technique for designing, analyzing and interpreting integral and spatio-temporal dynamics of emotional developments in artificial chemical minds [2]. Locomotion and manipulation We have demonstrated that reaction-diffusion chemical systems can solve complex problems, do logical reasoning and express emotions. Now we want the chemical reactors to stop minding their own business but act: move and manipulate. The physico-chemical artefacts are known to be capable for sensible motion. Most famous are Belousov-Zhabotinsky vesicles, self-propulsive chemo-sensitive drops and ciliary arrays (Yoshikawa Lab, Kyoto). Their motion is directional but somewhere lacks sophisticated control mechanisms.
A. Adamatzky / Reaction-Diffusion Intelligent Wetware
7
In computational experiments we established that two reagents are enough to produce a sustained amoeba-like pattern ņ compact domain of wave-like dynamics enclosed in undulating membrane ņ in reaction-diffusion system (Fig. 3). Such types of proto-cell like patterns are sensitive to stimulation of their membranes, which can provoke their movement.
Figure 3. Reaction-diffusion proto-cell emerged in cellular automaton (binary cell state set, eight-cell neighbourhood) model of chemical system. Each cell of the automaton takes two states, 0 (white pixel) and 1 (black pixel). A cell in state 0 (1) takes state 1 (0) if it has from 4 to 7 neighbours in state 1; otherwise the cell does not change its state.
At present stage of reaction-diffusion intelligence research it seems to be difficult to provide effective solutions for experimental prototyping of combined sensing, decisionmaking and actuating. However as a proof-of-concept we can always consider hybrid ‘wetware+hardware’ systems. For example, to fabricate a chemical controller for robot, we place a reactor with Belousov-Zhanotinsky solution onboard of a wheeled robot (Fig. 4), and allow the robot to observer excitation wave dynamics in the reactor. When the medium is stimulated at one point, target waves are formed. The robot becomes aware of the direction towards source of stimulation from the topology of the wave-fronts [3].
Figure 4. The robot controlled by liquid-phase Belousov-Zhabotinsky medium [3].
8
A. Adamatzky / Reaction-Diffusion Intelligent Wetware
A set of remarkable experiments were undertaken by Hiroshi Yokoi and Ben De Lacy Costello. They built interface between robotic hand and Belousov-Zhabotinsky chemical reactor (Fig. 5).
Figure 5. Closed-loop system of robotic hand interacting with Belousov-Zhabotinsky medium [3,4]. Photograph of Prof. Yokoi and Dr. De Lacy Costello experimental setup.
Excitation waves propagating in the reactor were sensed by photo-diodes, which triggered finger motion. When bending fingers touched the chemical medium with their glass nails filled with colloid silver, which triggered circular waves in the medium [3]. Starting from any initial configuration, the chemical-robotic system does always reach a coherent activity mode, where fingers move in regular, somewhat melodic patterns, and few generators of target waves govern dynamics of excitation in the reactor [4].
Figure 6. Transporting rectangular object in Belousov-Zhabotinsky chemical medium [5].
A. Adamatzky / Reaction-Diffusion Intelligent Wetware
9
Massively parallel smart manipulation is yet another hot topic of reaction-diffusion intelligence. This was demonstrated in our recent experiments on manipulating simple objects in a simulated massive-parallel array of actuators controlled by experimental excitation dynamics in a Belousov-Zhabotinsky chemical medium [5]. We assumed that manipulated objects should be pushed by travelling excitation wave-fronts. Therefore we encoded snapshots of gel-based Belousov-Zhabotinky media and converted them into arrays of local force vectors in such a manner that every pixel of the medium’s snapshot got its own force vector, the orientation of which was determined by the gradient of concentrations of chemical species in the medium in the neighbourhood of the pixel. We have shown that various types of excitation dynamics ņ target waves, spiral waves, wave-fragments ņ are capable of transportation and manipulation of small objects and larger, spatially extended objects, planar shapes (Fig. 6). Conclusion In this sketchy paper we provided basic paradigms and facts which will lead us to design and fabrication, and mass-production, of shapeless and sluggish but highly intelligent creatures. These artificial reaction-diffusion wet-bots will act as computers, transporters, builders, and, ultimately, parts of our body. The wet-bots will fill our life with care, understanding, gentleness and love (Fig. 7).
Figure 7. The slogan on the first poster of “The Blob” (1956) reflects our vision of future reaction-diffusion intelligence.
10
A. Adamatzky / Reaction-Diffusion Intelligent Wetware
When things can go wrong? The things could not possibly go wrong unless we step with both feet in the trap of conventional architectures! The field of reaction-diffusion computing started twenty years ago [6] as a sub-field of physics and chemistry dealing with image processing operations in uniform thin-layer excitable chemical media. The basic idea was to apply input data as two-dimensional profile of heterogeneous illumination, then allow excitation waves spread and interact with each, and then optically record result of the computation. The first even reaction-diffusion computers were already massively parallel, with parallel optical input and outputs. Later computer scientists came to the field, and started to exploit traditional techniques ņ wires were implemented by channels where wave-pulses travel, and specifically shaped junctions acted as logical valves. In this manner, most ‘famous’ chemical computing devices were implemented, including Boolean gates, coincidence detectors, memory units and more. The upmost idea of reaction-diffusion computation was if not ruined then forced into cul-de-sac of non-classical computation. The breakthrough happened when paradigms and solutions from the field of dynamical, collision-based computing and conservative logic, were mapped onto realms of spatiallyextended chemical systems. The computers became uniform and homogeneous again.
Refererence [1] Adamatzky A. Computing in Nonlinear Media and Automata Collectives (IoP Publishing, 2001). [2] Adamatzky A. Dynamics of Crowd-Minds (World Scientific, 2005). [3] Adamatzky A., De Lacy Costello B., Asai T. Reaction-Diffusion Computers (Elsevier, 2005). [4] Yokoi H., Adamatzky A., De Lacy Costello B., Melhuish C. Excitable chemical medium controlled for a robotic hand: closed loop experiments, Int. J. Bifurcation and Chaos (2004). [5] Adamatzky A., De Lacy Costello B., Skachek S., Melhuish C. Manipulating objects with chemical waves: Open loop case of experimental Belousiv-Zhabotinsky medium. Phys. Lett. A (2006). [6] Kuhnert L. A new photochemical memory device in a light sensitive active medium. Nature 319 (1986) 393.
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
11
Evolutionary Robotics for Legged Machines: From Simulation to Physical Reality Hod Lipson, Josh Bongard, Victor Zykov, Evan Malone Computational Synthesis Lab Cornell University, Ithaca NY 14853, USA
Abstract. This talk will outline challenges and opportunities in translating evolutionary learning of autonomous robotics from simulation to reality. It covers evolution and adaptation of both morphology and control, hybrid co-evolution of reality and simulation, handling noise and uncertainty, and morphological adaptation in hardware.
Keywords. Evolutionary robotics, co-evolutionary learning, estimation-exploration, rapid prototyping.
Introduction The idea that machine learning processes inspired by biological evolution can be used to design autonomous machines, has its roots in the early days of evolutionary computation and has been implemented numerous times, starting with the seminal work of Sims [9]. Nevertheless, the transition of evolutionary robotics from simulation to reality has been met with many challenges, as is evident from the relatively few examples of successful implementations of these methods in physical reality. Though many robotic experiments are carried out in simulation, a robot must ultimately function in physical reality. Consider the problem of evolving controllers for a dynamical, legged robot, shown in Figure 1 [13]. The nine-legged machine composed of two Stewart platforms back to back. The platforms are powered by twelve pneumatic linear actuators, with power coming from an onboard 4500psi paintball canister. While most robotic systems are use position-controlled actuators whose exact extension can be set, pneumatic actuators of the kind used here are force-controlled. Like biological muscle, the controller can specify the force and duration of the actuation, but not the position. It is therefore a challenging control problem. The controller architecture for this machine was an open-loop pattern generator that determines when to open and close pneumatic valves. The on-off pattern was evolved; Candidate controllers were evaluated by trying them out on the robot in a cage, and measuring fitness using a camera that tracks the red ball on the foot of one of the legs of the machine (see inset in Figure 1b for a view from the camera). Snapshots from one of the best evolved gates are
12
H. Lipson et al. / Evolutionary Robotics for Legged Machines: From Simulation to Physical Reality
shown in Figure 1c. Nolfi and Floreano [8] describe many other interesting hardware experiments evolving controllers for wheeled robots
(a)
(b)
(c) Figure 1: Evolving a controller for physical dynamic legged machine: (a) The nine-legged machine is powered by twelve pneumatic linear actuators arranged in two Stewart platforms. The controller for this machine is an open-loop pattern generator that determines when to open and close pneumatic valves. (b) Candidate controllers are evaluated by trying them out on the robot in a cage, and measuring fitness using a camera that tracks the red foot (see inset). (c) Snapshots from one of the best evolved gates. From [13].
H. Lipson et al. / Evolutionary Robotics for Legged Machines: From Simulation to Physical Reality
13
While evolution successfully generated viable gaits in this case, applying evolutionary processes to physical machines is difficult for two reasons. First, even if we are only evolving controllers for a machine with a fixed morphology, each evaluation of a candidate controller involves trying it out in reality. This is a slow and costly process that also wears out the target system. Performing thousand of evaluations is usually impractical. Second, if we are evolving morphology as well, then how would these morphological changes take place in reality? Changes to the controller can be done simply by reprogramming, but changes to the morphology require more sophisticated processes. Nature has some interesting solutions to this problem, such as growing materials, or self-assembling and self-replicating basic building blocks like cells.
1. Evolving controllers for physical morphologies One approach to evolving controllers for fixed morphologies is to make a simulator with such fidelity that whatever works in simulation will also work in reality equally well. This is possible only for some types of locomotion, such as quasi-static kinematics that can be accurately predicted [6][4]. Figure 2a shows some of the machines that evolved for quasistatic locomotion in simulation; these machines were “copied” from simulation into reality using rapid-prototyping technology (Figure 2b) where they functioned in a way similar to their simulation. Unfortunately, however, it is unlikely that a similarly predictive dynamic simulator would exist, given that machine dynamics are inherently chaotic and sensitive to initial conditions and many small parameter variations. But even if such simulators existed, creating accurate models would be painstakingly difficult, or may be impossible because the target environment is unknown.
Figure 2: Evolving bodies and brains: (a) Three evolved robots, in simulation (b) the three robots reproduced in physical reality using rapid prototyping. From [6].
14
H. Lipson et al. / Evolutionary Robotics for Legged Machines: From Simulation to Physical Reality
An alternative approach to “crossing the reality gap” is to use a crude simulator that captures the salient features of the search space. Techniques have been developed for creating such simulators and using noise to cover uncertainties so that the evolved controllers do not exploit these uncertainties [5]. Yet another approach is to use plasticity in the controller: Allow the robot to learn and adapt in reality. In nature, animals are born with mostly predetermined bodies and brains, but these have some ability to learn and make final adaptations to whatever actual conditions may arise. A third approach is to co-evolve simulators so that they are increasingly predictive. Just as we use evolution to design a controller, we can use evolution to design the simulator so that it captures the important properties of the target environment. Assume we have a rough simulator of the target morphology, and we use it to evolve controllers in simulation. We then take the best controller and try it – once – on the target system. If successful, we are done; but if the controller did not produce the anticipated result (as is likely to happen since the initial simulator was crude), then we observed some unexpected sensory data. We then evolve a new set of simulators, whose fitness is their ability to reproduce the actual observed behavior when the original controller is tested on them. Simulators that correctly reproduce the observed data are more likely to be predictive in the future. We then take the best simulator, and use to evolve a new controller, and the cycle repeats: If the controller works in reality, we are done. If it does not work as expected, we now have more data to evolve better simulators, and so forth. The co-evolution of controllers and simulators is not necessarily computationally efficient, but it dramatically reduces the number of trials necessary on the target system. The co-evolutionary process consists of two phases: Evolving the controller (or whatever we are trying to modify on the target system) – we call this the exploration phase. The second phase tries to create a simulator, or model of the system – we call this the estimation phase. To illustrate the estimation-exploration process, consider a target robot with some unknown, but critical, morphological parameters, such as mass distribution and sensory lag times. Fifty independent runs of the algorithm were conducted against the target robot. Figure 3a shows the 50 series of 20 best simulator modifications output after each pass through the estimation phase. Figure 3a makes clear that for all 50 runs, the algorithm was better able to infer the time lags of the eight sensors than the mass increases of the nine body parts. This is not surprising in that the sensors themselves provide feedback about the robot. In other words, the algorithm automatically, and after only a few target trials, deduces the correct time lags of the target robot's sensors, but is less successful at indirectly inferring the masses of the body parts using the sensor data. Convergence toward the correct mass distribution can also be observed. But even with an approximate description of the robot's mass distribution, the simulator is improved enough to allow smooth transfer of controllers from simulation to the target robot. Using the default, approximate simulation, there is a complete failure of transferal: the target robot simply moves randomly, and achieves no appreciable forward locomotion. It is interesting to note that the evolved simulators are not perfect; they capture well only those aspects of the world that are important for accomplishing the task. The exploration-estimation approach can be used for much more than transferring controllers to robots – it could be used by the robot itself to estimate its own structure. This
H. Lipson et al. / Evolutionary Robotics for Legged Machines: From Simulation to Physical Reality
15
would be particularly useful if the robot may undergo some damage that changes some of its morphology in unexpected ways, or some aspect in its environment changes. As each controller action is taken, the actual sensory data is compared to that predicted by the simulator, and new internal simulators are evolved to be more predictive. These new simulators are then used to try out new, adapted controllers for the new and unexpected circumstances. Figure 3b shows some results applying this process to design controllers for a robot which undergoes various types of drastic morphological damage, like loosing a leg, motor, or sensor, or combinations of these. In most cases, the estimation-exploration process is able to reconstruct a new simulator that captures the actual damage using only 4-5 trials on the target robot, and then use the adapted simulator to evolve compensatory controllers that recover most of the original functionality. There are numerous applications to this identification and control process in other fields.
(a)
(c)
(b) Figure 3: Co-evolving robots and simulators: (a) Convergence toward the physical characteristics of the target robot. Each pass through the estimation phase produces a set of mass changes for each of the nine body parts of the robot (top row) and a set of time lags for each of the eight sensors (bottom row). The open circles indicate the actual differences between the target robot and the starting default simulated robot [1]. (b) Three typical damage recoveries. a: The evolutionary progress of the four sequential runs of the exploration EA on the quadrupedal robot, when it undergoes a failure of one of its touch sensors. The hypotheses generated by the three runs of the estimation EA (all of which are correct) are shown. The dots indicate the fitness of the best controller from each generation of the exploration EA. The triangle shows the fitness of the first evolved controller on the target robot (the behavior of the ‘physical’ robot with this controller is shown in b); the filled circle shows the fitness of the robot after the damage occurs (the behavior is shown in c); the squares indicate the fitness of the ‘physical’ robot for each of the three subsequent hardware trials (the behavior of the ‘physical’ robot during the third trial is shown in d). e-h The recovery of the quadrupedal robot when it experiences unanticipated damage. i-l The recovery of the hexapedal robot when it experiences severe, compound damage. The trajectories in b-d, f-h and j-l show the change in the
16
H. Lipson et al. / Evolutionary Robotics for Legged Machines: From Simulation to Physical Reality
robot’s center of mass over time (the trajectories are displaced upwards for clarity) [2]. (c) The simulator progressively learns the entire robot morphology from scratch. Panels (a-g) are progressive intermediate selfinference stages, panel (h) is the true target system [3].
2. Making morphological changes in hardware An evolutionary process may require a change of morphology, or production of a new physical morphology altogether. One approach for generating new morphology is to use reconfigurable robots [12]. Reconfigurable robots are composed of many modules that can be connected, disconnected and rearranged in various topologies to create machines with variable body plans. Self-reconfigurable robots are able to rearrange their own morphology, and thus adapt in physical reality. Figure 4a shows one example of a self-reconfiguring robot composed of eight identical cubes [14]. Each cube can swivel around its (1,1,1) axis, and connect and disconnect to other cubes using electromagnets on its faces. Though this robot contains only 8 units, it is conceivable that future machine will be composed of hundreds and thousands of modules of smaller modules, allowing much greater control and flexibility in morphological change.
Figure 4: Transferring morphological changes into reality (a) Reconfigurable molecube robots [14], (b) Rapid prototyping, (c) Future rapid prototyping systems will allow deposition of multiple integrated materials, such as elastomers, conductive wires, batteries and actuators, offering evolution a larger design space of integrated structures, actuators and sensors, not unlike biological tissue. From [7].
H. Lipson et al. / Evolutionary Robotics for Legged Machines: From Simulation to Physical Reality
17
An alternative approach to varying morphology is to produce the entire robot morphology automatically. For example, the robots shown in Figure 2b were produced using rapid prototyping equipment: These are 3D printers, that deposit material layer by layer to gradually build up a solid object of arbitrary geometry, as shown in Figure 4b. This “printer”, when coupled to an evolutionary design process, can produce complex geometries that are difficult to produce any other way, and thus allow the evolutionary search much greater design flexibility. But even when using such automated fabrication equipment we needed to manually insert the wires, logic, batteries and actuators. What if the printer could print these components too? Future rapid prototyping systems may allow deposition of multiple integrated materials, such as elastomers, conductive wires, batteries and actuators, offering evolution an even larger design space of integrated structures, actuators and sensors, not unlike biological tissue. Figure 4c shows some of these printed components [7].
Figure 5: Macro-scale physical models of stochastic self-assembly. (a) Stochastic self-assembly and self reconfiguration of 10-cm scale modules on an oscillating air table: Top: units with electromagnets; Bottom: Units with swiveling permanent magnets [10]. (b) Three dimensional stochastic self assembly and reconfiguration of 10cm cubes in oil [11].
Looking at biology, one would ultimately like to emulate ‘growing structures’ – structures that can actively move material from one place to another, adapting to needs in situ. As we move to smaller and smaller scales, however, deterministically moving material becomes increasingly difficult. An interesting alternative is to exploit the random ‘Brownian’ motion of the particles in the environment to assist in stochastic self assembly. Figure 5 shows some macro-scale prototypes of such stochastically reconfiguring systems, both in 2D and in 3D. Implementation of such systems at the micro scale, using many thousands of units, entails many physical as well as computational challenges, involving local actuation, sensing, and control.
18
H. Lipson et al. / Evolutionary Robotics for Legged Machines: From Simulation to Physical Reality
3. Conclusions The transition of evolutionary robotics methods from simulation to reality has met several hurdles: Besides the scaling limits of evolutionary computation itself, we are confronted with the limits of simulation and modeling, the cost, time and risk of training machines in reality, and the technical challenge of adapting morphology in reality. Again we have resorted to inspiration from biology: Better ways to design adaptive simulations, better ways to determine the most useful physical evaluation, and new ways to adapt physical morphology through automatic reconfiguration and material growth, all leading to new ideas for engineering and new engineering insights into biology.
4. References [1]
Bongard J. C., Lipson H., (2004) “Once More Unto the Breach: Automated Tuning of Robot Simulation using an Inverse Evolutionary Algorithm”, Proceedings of the Ninth Int. Conference on Artificial Life (ALIFE IX), pp.57-62
[2]
Bongard J., Lipson H. (2004), “Automated Damage Diagnosis and Recovery for Remote Robotics”, IEEE International Conference on Robotics and Automation (ICRA04), pp. 3545-3550
[3]
Bongard, J. and Lipson, H. (2004) “Integrated Design, Deployment and Inference for Robot Ecologies”, Proceedings of Robosphere 2004, November 2004, NASA Ames Research Center, CA USA
[4]
Hornby G.S., Lipson H., Pollack. J.B., 2003 “Generative Encodings for the Automated Design of Modular Physical Robots”, IEEE Transactions on Robotics and Automation, Vol. 19 No. 4, pp 703-719
[5]
Jakobi, N. (1997). Evolutionary robotics and the radical envelope of noise hypothesis. Adaptive Behavior, 6(1):131–174.
[6]
Lipson H., Pollack J. B. (2000) Automatic design and manufacture of artificial lifeforms. Nature, 406:974–978
[7]
Malone E., Lipson H., (2004) “Functional Freeform Fabrication for Physical Artificial Life”, Ninth Int. Conference on Artificial Life (ALIFE IX), Proceedings of the Ninth Int. Conference on Artificial Life (ALIFE IX), pp.100-105
[8]
Nolfi S., Floreano D. (2004), Evolutionary Robotics: The Biology, Intelligence, and Technology of SelfOrganizing Machines, Bradford Books
[9]
Sims K. “Evolving 3D morphology and behaviour by competition”. Artificial Life IV, pages 28–39, 1994.
[10] White P. J., Kopanski K., Lipson H. (2004), “Stochastic Self-Reconfigurable Cellular Robotics”, IEEE International Conference on Robotics and Automation (ICRA04), pp. 2888-2893 [11] White P., Zykov V., Bongard J., Lipson H. (2005) Three Dimensional Stochastic Reconfiguration of Modular Robots, Proceedings of Robotics Science and Systems, MIT, Cambridge MA, June 8-10, 2005 [12] Yim, M., Zhang, Y. and Duff, D., "Modular Reconfigurable Robots, Machines that shift their shape to suit the task at hand," IEEE Spectrum Magazine cover article, Feb. 2002 [13] Zykov V., Bongard J., Lipson H., (2004) "Evolving Dynamic Gaits on a Physical Robot", Proceedings of Genetic and Evolutionary Computation Conference, Late Breaking Paper, GECCO'04 [14] Zykov V., Mytilinaios E., Adams B., Lipson H. (2005) "Self-reproducing machines", Nature Vol. 435 No. 7038, pp. 163-164
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
19
Real World Informatics Environment System Tomomasa SATO1 Department of Mechano-Informatics Graduate School of Information Science and Technology The University of Tokyo
Abstract. This paper proposes a real world informatics environment system realized by multiple human behavior support cockpits (HBSC’s). The human behavior support cockpit (HBSC’s) is synthesized by such supportive environment as illumination environment (physiological support), object access environment (physical support) and background music environment (psychological support). These HBSC’s are implemented by cooperating the real world informatics environment system components of humanoid robots, audio/visual agents and ubiquitous appliances. In the paper, the author describes images of real world informatics environment system and presents research results of its constituent elements by dividing them into the following research groups; a humanoid robot (HR), VR system (VR), attentive environment system (AE), neo-cybernetic system (NC), and human informatics (HI) research group. Keywords. Human Robot Symbiosis System, Human Robot Interaction, Human Support
1.
Introduction
Advanced robot in the future should be able to offer such services of a servant, a secretary, a friend and a pet simultaneously and continuously to human. To this end, a lot of research efforts have been devoted on humanoid robots, communication robots, mobile robots and manipulation robots. These efforts can be classified as an individual robot approach. An environmental robot is another research approach to this end. In the environmental robot system, multiple robotic elements are distributed over space to offers services to human. Thanks to the progress of micro devices and network technology, the environmental robot systems have been widely under study such as Smart Room [1], Intelligent Room [2], Easy Living [3], Self [4], Aware House [5], neural network House [6], and Intelligent Space [7]. While research approach of individual robot or environmental robot attracts many researchers independently, these two approaches are not exclusive but complimentary to 1 Department of Mechano-Informatics, Graduate School of Information Science and Technology, The University of Tokyo, 7-3-1 Hongo, Bunkyo-ku, Tokyo 113-8656 Japan. Tel: +81 3 5841 6441; Fax: +81 3 5802 5379; Email:
[email protected].
20
T. Sato / Real World Informatics Environment System
each other, i.e. the integrated system of both individual robot and environmental robot will realize better robotic performance than each independent robot. Taking account of the stated consideration, researchers of the 21st century COE (center of excellence) program of the University of Tokyo started a new project of a real world information system from 2002 supported by the Ministry of Education, Culture, Sports, Science and Technology. The real world information system project is intended to develop a real world informatics environment system. The system is a human symbiotic environment system where such intelligent entities as agents in the information world, avatars in the virtual reality world and humanoid robots or ubiquitous appliances with sensors and actuators in the real world are seamlessly linked with each other and are interacting with human to offer support. The social significance of constructing such real world informatics environment system is that it can demonstrate and show a prototype of the forthcoming ubiquitous society in a concrete manner. Scientific significance of the project is that the project reveals a model of human recognition and behavior by analyzing the relationship between the real world informatics system and the human inside it. In this paper, the author first of all makes clear basic concept of the real world informatics environment system and proposes to realize the system by combining multiple human behavior support cockpits (HBSC’s). The paper then touches upon several new functions of the human behavior support cockpits (HBSC’s) from the viewpoint of human behavior support environment by introducing research groups of the project. Lastly, the paper also reports realized new interactions between a human and a system in terms of the human behavior support cockpit taking account of the real world informatics related researches.
Higher-order communication with a remote place Assistance in reading or operations
Dialogue with an information agent Work assistance by AWB
The point of integration Show Room = Future Living Room x Natural x Always available
Life support by a humanoid Promotion of work efficiency and activation of communication
x Nonbinding x Coexisting with humans
Figure 1: Image of a real-world information system environment
T. Sato / Real World Informatics Environment System
2.
21
Project Target
2.1 Image of Real World Informatics Environment Figure 1 illustrates a conceptual sketch of the real world informatics environment system proposed by the members of 21st century COE project in 2002. In the environment, an information agent recognizes human daily movements continuously, a Virtual Reality (VR) system capable of natural dialogue interacts with the human, a humanoid robot handling complicated tasks comes close to the human to assist, and a ubiquitous appliance (a future information home electronics item) offers a helping hand to the human. As a result of discussion in the demonstration committee of the project, we obtained the stated research target image of the real world informatics environment system. This image sketch illustrates daily life in the future with multiple people, i.e. the daily life in the ubiquitous society in the future. Major characteristics of such an environment system are that it is a natural environment and is a comfortable place for a human to live, various system elements work all the time providing services while excessive physical and psychological constraints on a human are avoided as much as possible, and it is a humansystem symbiotic environment where the system adjusts itself to a human. 2.2 Human Behavior Support Cockpit In this section, the author proposes to realize the world informatics environment system by multiple human behavior support cockpits (HBSC’) which supports a human to perform that specific behavior by providing necessary and/or satisfactory environment. The human behavior support cockpit (HBSC) is synthesized by cooperating such elements as humanoid robots, information agents, VR systems and ubiquitous appliances depending upon human behavior. Here let us explain the human behavior cockpit by taking concrete example of reading newspaper behavior. When we want to read a newspaper, we have to have a newspaper at hand and we need illumination if it is dark in a room. These are the environments we need to read a newspaper. In this case, we need both illuminated environment and newspaper access environment. By synthesizing these two necessary environments, the newspaper reading cockpit is realized. This is the typical example of human behavior support cockpit by synthesizing multiple human support environments of illumination and object access. In the real world informatics environment system, the human behavior support cockpit is synthesized by the combination of not only a newspaper access environment realized by a humanoids robot bringing it near a human but also a newspaper readable illumination environment realized by a robotic lamp which is a component of the ubiquitous appliances of the room as described later.
22
T. Sato / Real World Informatics Environment System
2.3 Main Features of Human Behavior Support Cockpit 2.3.1 Elementary Human Behavior Support Environments to Realize Cockpit The human behavior support cockpit (HBSC) is synthesized by combining the following four categories of human behavior support environments by considering human property. These are the support environment of a) physiological support environment, b) physical support environment, c) informational support environment and d) psychological support environment. An illuminated space is an example of a physiologically supported environment. Physical support environment enables the human physically supported situation, for example a humanoid robot is able to realize a newspaper accessible physical environment by bringing the newspaper close to the human. An active projector easily realizes informational support environment by projecting necessary information required to perform particular behavior. Lastly the robot system should realize psychologically comfortable environment by providing BGM (back ground music) if it is adequate time for that. Thus by combining the stated human behavior support environment, the human behavior support cockpit (HBSC) is synthesized. A specific human behavior support cockpit (HBSC) is required depending upon the individual human behavior. 2.3.2 Factors of Human Behavior Support Cockpit The most important factors of a specific human behavior support cockpit (HBSC) are related to a) who, b) where, c) when, d) what, e) how and, f) why. When just one person is living in a room alone, then who to support is apparent, however in the case of multiple person, the system has to decide to whom the cockpit give support. The place or the space where the cockpit should be generated and when the support should be offered must be carefully examined, otherwise the support becomes useless or troublesome. The content of the support can be classified as explained in the previous section, i.e. a) physiological support, b) physical support, c) informational support and d) psychological support. In many cases, multiple superposed supports are needed in our daily life. In addition to this, even if the content of the support is the same, the cockpit has to select the way or the media of the support carefully. For example, the system has to select whether it uses voice channel or visual channel in case of informational support. The system always has to pay keen attention to a human so that it can provides adequate support to the human constantly, it means that the system has to have the capability to explain why some specific cockpit is offered to the human. 2.4 Realization of Human Behavior Support Cockpit The size of the human behavior support cockpit (HBSC) can be categorized by either a) a device level, b) an appliance level, c) a furniture level, c) an individual robot level, and d) a room level or combination of them depending upon the size of the cockpit. In addition to construction and sophistication of such individual components of the human behavior support cockpit realized by either devices, furniture, robots and/or room
T. Sato / Real World Informatics Environment System
23
itself, the combination of them have also to be investigated for better performance of support environment. 2.5 Research Group of Project Before realizing the final real world informatics environment system, the following subenvironment systems have been realized so far, i.e., a) a humanoid and robot environment system, b) a VR environment system, c) an ubiquitous environment system, and d) an audio/visual agent environment system. To realize the stated sub-systems, the following five research groups are formed in the project; x Human Robotics Research Group (HR), x VR System Research Group (VR), x Neo-Cybernetics Research Group (NC), x Attentive Environment Research Group (AE), x Human Informatics Research Group (HI). The first four groups correspond to the stated a) to d) sub-environment and the last research group of Human Informatics is formed to provide the project with basic scientific base of human. So far, discussions on a real world informatics environment system and its constituent elemental environment system were deepened. Details of these images will be presented in the following chapters along with individual research results.
3.
Human Robotics Research Group (HR)
Within the real world information system project that is intended to construct an intelligent environment interacting with a human in a new way, the Human Robotics Research Group (HR) will conduct researches focusing on innovative robots physically assisting a human such as a humanoid capable of equally interacting with a human in terms of physical handling of object and communication, wearable a ubiquitous supporting device that provides necessary information accompanying a human, and a room environment integrated robot casually watching over and giving helping hand to a human. Through collaboration with the other Human Informatics Research Group, NeoCybernetics Research Group and VR System Research Group within the real world information system project, we attempt to implement an intelligent system environment
Figure 2: Image of human robotics environment
24
T. Sato / Real World Informatics Environment System
interacting with a human by way of human behavior cockpit. Figure 2 illustrates the image of a human robot environment system. This image sketch shows a place where people get together, and a robot answers a human’s call correctly (communication environment), attends a human maintaining a comfortable distance (communication environment), exchanges information (communication environment) and goods (object access environment) with a human politely. They are related to provide assistance by knowing a person’s daily living behaviors. As a perceptual function of a cockpit, knowing of the existence of a human, responding to a human motion, and observing the direction of a sight line are basic and indispensable; however, there still remain some unsolved problems. To realize such a cockpit, this research group allotted and deepens research and development of an environment robot with a sensing floor and robotic lamp device (described later in section 7) collaborating with each other, a mobile and a humanoid robot.
4. VR System Research Group (VR) This research group promotes research and development of augmented reality (real space with information and images) technology and tele-existence technology (in which an operator controls a remote robot as if it A actually is on board.) to display a “cyber world essentially equivalent to real world” within daily space where a human actually lives rather than in a special room. This provides the human behavior cockpit with powerful tool to realize displaying environment Figure 3 is an image sketch of the VR system environment. The upper right and lower left of the figure are remote space— B here, referred to as Space A (upper right) and Space B (lower left). An entirecircumference cameras we installed around. Cameras in Space B are used to capture Figure 3: Image of a VR system environment a 3-D entire-circumference image of Person, and the image is presented by an entire-circumference 3-D image generator in Space A to bring about the effect of making a viewer in Space A feel as if the Person existing in Space B existed in Space A (communication environment). Bidirectional tele-existence so that Person B also feels as if he or she were in Space A is to be implemented. Moreover, construction of a system to realize high-level integration of the real world and information world using a self-propelled projector and projector capable of free adjustment of a projection direction is utilized.
T. Sato / Real World Informatics Environment System
25
5. Neo-Cybernetic Research Group (NC) The purpose of this research group is to provide a machine with functions and abilities such as vision, hearing, and touch senses, targeting a wide range of research subjects from intelligent high-performance hearing, vision and touch sensors to a high-speed robot or a voice interaction personalized agent that behaves like a human. Figure 4 shows an image sketch of the neo-cybernetics system environment. The plot currently under our consideration could be a system guiding through a museum demonstration site. A large self-propelled display with a voice interaction personalized agent has a dialogue with a user through speech recognition and synthesis (communication environment). A guest is to be led or followed by this equipment or by acoustic processing, directional hearing and high-speed vision functions, where a selection of information Figure 4: Image of neo-cybernetics environment media becomes important research issue (information display environment). Stated communication and information displaying environment are synthesized to realize support cockpit of museum walk around behavior.
6. Attentive Environment Research Group (AE) The group mainly studies an environment which we call the Attentive Environment (AE) that “offers a helping hand to an operator” as a new intelligent machinery system for assisting operations in cell production, under the theme of “a new interaction between a human and machine” in a production environment. Furthermore, as a specific development objective, a system that flexibly handles major parts during assembly was reviewed, and an Attentive Workbench (AWB) system that integrates a motion tray (object access environment), information display (information display environment), vital sign monitor (physiological support environment) and parts tag (computer readable information environment) have been under development. Figure 5 shows an image of an attentive environment system. This is an environment Figure 5: Image of an attentive environment system where a worker is doing assembly system environment
26
T. Sato / Real World Informatics Environment System
work, i.e. the figure illustrates the assembly behavior support cockpit in the configuration of a room. The system recognizes the intention of the worker from his motions, and tries to “offer a helping hand”—presenting information or moving parts which corresponds to information environment and physical object access environment. For example, the system may display necessary information on the desktop, bring necessary tools and parts to an operator’s hand as he extends his arm, or transport assembled parts to the next procedure to provide object access environment to the worker. Motions recognition shall be done without discomforting the operator. A visual system installed at the top of the system is used to recognize especially motions of the operator’s arm or finger and the direction of a sight line (head). Information presentation is done by presenting an icon, figure or document on the desktop by a projector, using a method of extended reality to provide object access environment. Furthermore, the operator’s fatigue degree is also recognized by letting the operator wear a monitor device for vital signs such as the heart rate, and work speed and the degree of assistance shall be varied accordingly to ensure suitable physiological environment.
7. Human Informatics Research Group (HI) The Human Informatics Research Group develops research for modeling human perception, behavior and recognition functions based on measurement and analysis to form human scientific basis of this project; establishing mathematical scientific theories of real world information processing; and constructing basic methods for new forms of recognition/behavior, communication, and coordination/assistance functions. This environment is called a showroom, intending not only to demonstrate interactive system technology truly compatible with a human but also to do analysis and modeling study of a human (i.e., mathematical science of real-world information system). A measurement interface for a robot experiment and interface construction, and international multipoint remote lectures for result transmission and educational feedback will also be tried in the future.
8. Example of Human Behavior Support Cockpits In this chapter, an example of a human behavior support cockpits (HBSC’s) will be presented. These cockpits are composed of an illumination environment to a adapt to several human behaviors such as entering the room, sitting on the chair, sitting on the cushion, being on bed and getting up from bed, i.e. a entering the room cockpit, a sitting on the chair cockpit, a sitting on the cushion cockpit, a being on bed cockpit and a getting up from bed cockpit respectively realized by the cooperation of a sensing floor and a robotic lamp to generate illumination support environment [8].
T. Sato / Real World Informatics Environment System
27
8.1 Sensing Floor A sensing floor is a high resolution pressure sensor distributed floor to detect human position and objects such as a chair and a cushion. The sensing floor is composed of 16 units (Fig.6). Each unit has 16*16=4096 on/off sensors distributed in a square of 500mm width. Therefore the sensor pitch is 7mm. The output of the sensors is digital one bit. The sampling frequency is 5 Hz. This sensing floor is able to detect the user's a) moving on the bed b) sitting on the chair c) sitting on the cushion d) being on bed.
Figure 7: Entrance Illumination
Figure 8: Desk Illumination
Figure 9: Table Illumination
Figure 10: Bed Illumination
Figure 6: Sensing Floor
8.2 Robotic Lamp The Robotic lamp is a robotic 5 DOF desk light with the following functional capabilities: a) brightness control, b) posture control and c) back drivability to detect human physical contact. 8.3 Human Behavior Support Illumination Environment The following figures show how the robotic lamp illuminated the entrance as the user entered the room (Fig.7) and illuminated the desk as the user sat on the chair (Fig.8). Robotic lamp illuminated the table as the user sat on the cushion near the table (Fig.9) and illuminated the bed after the user moved on the bed (Fig.10) which corresponds to the entering the room behavior support cockpit, the sitting on the chair behavior support cockpit,
28
T. Sato / Real World Informatics Environment System
the sitting on the cushion behavior support cockpit, the being on bed behavior support cockpit and the getting up from bed behavior support cockpit respectively. These figures presents the robotic lamp system is able to provide adequate illumination environment depending upon user’s behavior.
9. Conclusion Bearing realization of a new human-system interaction in mind, we have been promoting the project to realize a real world informatics environment system that is implemented by multiple human behavior support cockpits. This paper mentioned the research objectives of the real world information system project of the University of Tokyo and proposes human behavior support cockpit realized by synthesizing the behavior support environment of a physiological support environment (e.g. an illumination environment), a physical support environment (e.g. object access environment) and psychological support environment (in some case realized by a communication environment). The project is composed of research group such as a Human Robot Research Group (HI), a VR Research Group (VR), a Neocybernetics Research Group (NC), a Attentive Work Bench Research Group (AE) and Human Informatics Research Group (HI). Research target of each research group and examples of human behavior support cockpit are also explained. Our future tasks are promotion and integration of researches in sophistication of many system elements including a humanoid, VR system, information agent and ubiquitous appliance, and pursuit of the relation between those information systems and a human to realize human behavior environment. By synthesizing these environment, proposed human behavior support cockpit would be realized as the new function of the real world informatics environment system.
Acknowledgement The author would like to express great thanks to Professor Shigeki Sagayama, Professor Masayuki Inaba, Professor Yasuo Kuniyoshi, Professor Susumu Tachi, Professor Hiromasa Suzuki and Professor Kiyoshi Takamasu who are the leaders of the research group of the Neo-Cybernetics Environment, Human Robotics Environment, Human Informatics Environment, VR System Environment, Attentive Environment, and Attentive Environment of this project respectively. Great thank also to Dr. Hiroshi Morishita, Prof. Mihoko Otake, Dr. Shinji Sako, Dr. Kiyoshi Kotani, Dr. Masao Sugi for their keen discussion as specially appointed researchers and Mr. Yosuke Kita for nice picture of conceptual sketches of environments. This project is the part of the 21st century COE programs of and supported by the Ministry of Education, Culture, Sports, Science and Technology.
T. Sato / Real World Informatics Environment System
29
References [1] Alex Pentland, “Smart Rooms”, Scientific American, pp.54-62, April, 1996. [2] Mark C. Torrance, “Advances in human computer interaction: The intelligent room,” In Working Notes of CHI 95 Research Symposium, Denver, Colorado, May 6-7, 1995. [3] Barry Brumitt et.al., “EasyLiving: Technologies for Intelligent Environments,” Proceedings of International Symposium on Handheld and Ubiquitous Computing, 2000. [4] Y. Nishida, T. Hori, T. Suehiro, and S. Hirai, “Sensorized environment for selfcommunication based on observation of daily human behavior,” Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems pp.1364-1372, 2000. [5] I. A. Essa, “Ubiquitous sensing for smart and aware environments: technologies towards the building of an aware home,” Position Paper for the DARPA/NSF/NIST workshop on Smart Environment, July 1999. [6] Michael C. Mozer, “The neural network house: An environment that adapts to its inhabitants”. In Proceedings of the AAAI, 1998. [7] Joo-Ho Lee, Noriaki Ando, Hideki Hashimoto, “Design Policy for Intelligent Space”, IEEE Systems, Man, and Cybernetics Conference, 1999. [8] Tomomasa Sato, Mehrab Hosseinbor, Takeru Kuroiwa, Rui Fukui, Jun Tamura, and Taketoshi Mori: “Behavior and Personal Adaptation of Robotic Lamp,” Journal of Robotics and Mechatronics, Vol.17, No.1, 2005 pp69-76, 2005. [*] http://www.u-tokyo.ac.jp/coe/list06_e.html
30
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
Understanding and realization of constrained motion – human motion analysis and robotic learning approaches a,b
a
Shigeyuki Hosoe, Yuichi Kobayashi and Mikhail Svinin a RIKEN Bio-mimetic Control Research Center b Nagoya University Nagoya, Japan
[email protected], {koba,svinin}@bmc.riken.jp
a
Abstract. This paper presents two approaches toward the understanding and realization of constrained motion of the upper limbs. The first approach deals with the analysis on the constrained human movements under the framework of optimal control. It is shown that the combination of the optimality criteria constructed by muscle force change and hand contact force change can explain the formation of the constrained reaching movements. The second approach comes from robotics. It is illustrated by application of reinforcement learning to a robotic manipulation problem. Here, an assumption on the existence of holonomic constraints can accelerate learning by introducing function approximation techniques within model-based reinforcement learning. The idea of estimation (parameterization) of the constraints can relate two different problems under the common perspective of “learning constrained motion.” Keywords. Constrained motion, Human motion analysis, Optimal control, Reinforcement learning
1. Introduction Historically, the robotics research has grown from industrial applications. However, recently it attracts more and more attention for the reason that robots can be regarded as “bio-mimetic machines,” where the principles of the robot control are expected to help understanding of the principle of human motion control. That is, we may understand the function of human brain by constructing machines that mimics human body and by testing those machines using biologically inspired control principles. It has been pointed out that the interaction between the human body and the environment plays an important role for the understanding of human intelligence [1]. This perspective was emphasized in contrast to the symbol-based information processing researches, which mainly considered functions of brain independently of the motion of human body. Examples of interaction with the external environment can be found in locomotion and manipulation tasks. In locomotion tasks one deals with the control of lower limbs, while in the manipulation task we are interested in the control of upper limbs. Typically,
S. Hosoe et al. / Understanding and Realization of Constrained Motion
31
the external environment in the locomotion tasks is static. In comparison with the locomotion problems, the control of upper limbs motions may have different properties. For example, as can be seen in many manipulation problems, the interaction between the upper limbs and the environment can change the latter. In general, the property of “changing the world” can not be seen in locomotion tasks. This can require the agent to have qualitatively different abilities of adapting to the environment. Additionally, the interaction between the upper limbs and the environments can be physically constrained. The property of being constrained by the external environment requires further analysis of the basic mechanisms for the generation of constrained movements that can be exploited by the human control system for the creation of adaptation mechanisms with richer computational structures. In this paper, we present two approaches toward the understanding and realization of the constrained motion of the upper limbs. First, we show the analysis of upper limbs motion physically constrained by the environment. The analysis is based on the idea that the human motion can be explained using optimization principles. Secondly, we show the realization of constrained motions by a manipulator using reinforcement learning. Here, we introduce some assumptions on the learning problem so that learning performance is improved in comparison with the conventional reinforcement learning methods taken without such specific assumptions. Fig.1 shows the relation between two approaches described in this paper. Both these approaches have constrained motions as the common control problem. The two arrows in the figure denote the connection between the approaches. The left arrow indicates the research on human motion analysis which aims to the development of the robot technology as well as understanding of human itself. The right arrow indicates the expected contribution to the understanding of human motion by developing robot control methods while borrowing some ideas from human motion for the construction of robot control strategies. Understanding of human motion analysis of human motion
Constrained motion construction of robot control
Development of engineering technology
Fig. 1 : Two approaches for the understanding and realization of constrained motion
2. Analysis of human constrained motion It is known that unconstrained reaching movements of human arm have invariant features expressed by straight (slightly curved) hand path in the task space and a bell-shaped hand velocity profile [3]. One approach to account for the invariant
32
S. Hosoe et al. / Understanding and Realization of Constrained Motion
features of reaching movements is via optimization theory. The cost function for the optimal control problem can be generally described as
J
³
T 0
L(q, q , q,")dt ,
(1)
where q denotes the vector of the joint angles. The optimal control and the optimal trajectory for the upper limb system can be derived by minimizing this cost function. For example, Flash and Hogan proposed a minimum hand jerk criterion [3]. Other criteria, such as a minimum joint torque change criterion [4] and a minimum muscle tension change criterion, were also tested for the prediction of reaching movements. Roughly, the criteria of optimality can be classified depending on the planning space (task or joint) and the criterion domain (motion or force) as indicated in Fig.2.
Fig. 2 : Classification of optimization criteria (cost function)
Fig. 3 : Experimental setup for human motion measurement
The above-indicated criteria of optimality were tested mainly for the reaching movements in free space. However, humans also make arm movements interacting with the environment (opening a door, turning a steering wheel, rotating a coffee mill, and so on). To manipulate objects or to use tools, the central nervous system (CNS) has to take into account the constraints imposed by the external environment when planning hand trajectories. It should also be noted that constrained movements constitute a class of force interactive tasks with redundancy in the distribution of the force actuation across the muscles. Understanding how human deals with the external constraints and solves the redundancy problem is important not only from the point of view of computational neuroscience but also for the construction of intelligent control systems for robot manipulators interacting with the external environment. Considering optimal control problems for the prediction of reaching movements, we noticed that criteria defined in the force domain of the task space have not been considered in literature. Next, by analyzing human motion profiles in a crank rotation task (see Fig. 3), we found that the conventional criteria (minimum joint torque and minimum muscle force change criteria) do not predict well the experimental data in the motion domain (tangential direction of movement) but they can capture the tendency of the internal force profiles. To take into consideration the force interactive nature of the crank rotation task, we have proposed a minimum hand force change criterion which is essentially a dynamic version of the minimum hand jerk criterion. This criterion gives
33
S. Hosoe et al. / Understanding and Realization of Constrained Motion
much better prediction of the velocity profiles. However the level of the normal forces, given by this criterion, is much lower than that observed in the experimental data. Consequently, we proposed a combined form of the optimality criterion, which has two components‒the hand force change and the muscle force change–balanced by a weight factor. Our simulation results show that in comfortable point-to-point movements the combined criteria predict the experimental data better. In addition to the motion and force trajectories in the task space, the combined criterion formulated at the muscle level can also capture the basic patterns of muscle activity (see Fig. 4). Even though the mathematical model we employed is relatively simple and does not take into account the dynamics at the metabolic level, the predictions are quite satisfactory. This suggests that the smoothness of both hand and actuating forces is of primary importance for explaining human movements constrained by the external environment. 0 400
f6
0 400
f5
0 400
f4
0 400
f3
0 400
f2
EMG6
EMG4 EMG3 EMG2
Muscle force (N)
IEMG
EMG5
EMG1
0.2
0.4
0.6
0.8
1
1.2
1.4
0
0.25
Time (s)
0.5
0.75
1
1.25
1.5
f1
Time (s)
Fig. 4 : EMG signals (left) and muscle forces predicted by proposed criterion (right)
It should be noted that the combined criteria introduced in this research take into account the geometry of the external constraints through the force change term, which depends on the orientation of the moving contact frame. Knowledge of the normal and the tangential directions of the constraint is essential in the calculation of the combined criteria. It is therefore assumed that, after enough learning and adaptation, the CNS implicitly constructs a parameterization of the constraint manifold. This may raise new interesting questions about the nature of the biological motor control of human movements.
3. Reinforcement learning for robotic constrained motion Reinforcement learning (RL) was originally related to the psychology of animal learning. Later it was connected to the optimal control framework and recently applied to robot control problems (e.g. [10]). RL is characterized by the fact that we do not need the perfect information about the evaluation and the dynamics of the system [1]. The general objective of reinforcement learning is to maximize the discounted sum of reward. One of the key notions in RL is known as a Temporal-Difference (TD) learning, where the following TD-error G t is used to update the value function V ;
34
S. Hosoe et al. / Understanding and Realization of Constrained Motion
Gt
rt J (V ( xt 1 ) V ( xt )) ,
(2)
Here, xt and rt denote the state variable and reward at time t , respectively. One of the advantages of the TD-learning is that it does not require explicit representations of the state transition function and the reward function, which results in less computation. Another advantage is that the TD-learning does not require any specific assumptions except the one that the state transition should be Markov decision process [1]. This generality means wide applicability of the learning method in principle, though it is difficult to practically apply TD-learning to control problems due to the slow convergence. On the other hand, there is a possibility to improve the efficiency of learning. This can be done by the explicit approximation of the state transition function and the reward function, and by utilizing them for learning even at the higher computational cost. In the discrete state and action domains, such explicit approximation methods are known as Dyna [7] or real-time dynamic programming [8]. Here, we extend the approximation to the continuous state domain by introducing a suitable function approximation scheme and call such an approach a model-based reinforcement learning. In this paper, we focus on a manipulation problem where the actual motion is restricted to a sub-manifold, which is included in the manifold of the total system, as indicated in Fig.5. In other words, we deal with the problem which includes holonomic constraints. The actual motion of the manipulator is restricted to the motion of the object as long as the contacts between the manipulator and the object are maintained (see Fig.6). If we can obtain an appropriate expression for such a low-dimensional motion, learning will be more effective. In the control of the constrained motion, there are two objectives, 1) to reach the desired configuration and 2) not to break the constraints (not to let the fingertip slip or part from the object). The second objective can be transformed to a discontinuous reward (see Fig. 7), which is given when the manipulator fails to keep the constraint. Thus, we introduce two assumptions on the control problem, the holonomic constraint and the discontinuous reward.
x3 constrained motion space
p2 p1
x1
x2
original state space
Fig. 5 : Sub-manifold generated by constraint
Fig. 6 : Constrained manipulation
motion
of
35
S. Hosoe et al. / Understanding and Realization of Constrained Motion
Based on these assumptions, we propose a learning scheme which is specified by the following three features: a) Construction of the mapping from N to a smaller-dimensional parameter space. b) Model-based reinforcement learning taking into account the system dynamics and using an approximation of the reward function. c) Approximation of discontinuous reward functions. Fig.8 shows a specific example of the manipulation task used in our study. Here, a two-link manipulator rotates an object to the desired angle without slipping and parting from the object.
R
y
q2
x2
Fcx
W2
Fcy
x1
q1 W1
x
T
xr , yr
Fig. 8 : Rotational task of one-dof Fig. 7 : Discrete reward function with object continuous boundary The motion of the manipulator is restricted to one-dimensional space while the constraint is maintained as indicated in Fig.9 which shows the constraint curve the joint space obtained during the learning. Fig. 10 shows the reward profile of the proposed learning method.
q2 -1.6
-1.8
-2
-2.2
0.9
1
1.1
q1
Fig. 9 : Obtained curve of constraint in joint angle space
Fig. 10 : Reward profile of proposed learning method
First, we compared the developed model-based learning method (using the proposed mapping) with the conventional Q-learning method and found that it was
36
S. Hosoe et al. / Understanding and Realization of Constrained Motion
impossible to obtain a control policy achieving the goal with Q-learning. This implies that the explicit approximation of the reward plays an important role in learning. Secondly, we applied the model-based learning without the proposed mapping. This time, the learning method could achieve the goal, but the averaged performance of the obtained policies was much worse. Thus, the learning performance could be improved by changing the compactness of the representation of state.
4. Discussion In the analysis of constrained human motion, we showed that the combined criteria of hand force change criterion and muscle force change criterion explained the experimental results well. The combined criterion essentially requires the knowledge of the geometrical information of the constraint. This result implies that human might construct a parameterization of the constraint, which can be obtained after some trial movements. This idea of parameterization of the constrained manifold was then considered within reinforcement learning framework. In the application of the model-based reinforcement learning to a manipulation task, we confirmed that the parameterization of the constraint improved the learning performance. The idea of estimation of the geometrical information of the constraint was also applied to the estimation of a discontinuous reward that specifies the boundary of the region for a discrete reward. This also greatly helped to improve the learning performance. From the perspective of robot control, in addition to the parameterization of constrained manifold, there can be other kinds of knowledge that can accelerate reinforcement learning. This issue can be addressed by constructing reinforcement learning methods that effectively utilize the additional knowledge on the manipulation problem and by measuring human motion with different conditions and clarifying whether humans really utilize such knowledge or not. In the human motion analysis, we dealt with the human motions that had been obtained by sufficient amount of training. It is also very important to focus on the learning of such motions. The relation of RL and the brain function has been discussed in the context of reward estimation and TD-error [12, 13]. On the other hand, some correspondences between the activities of Purkinje cells in the cerebellum and the learning framework based on motion primitives have been pointed out [14]. It is natural that human motion skills are obtained not by a single RL algorithm but by the combination of several kinds of learning frameworks, and the combination can be characterized by an effective usage of acquired (and somehow abstracted) knowledge. We will focus on this aspect of the acquisition of human motion skill in the future study.
5. Conclusion In this paper, we presented two approaches for the understanding of human constrained motion and the realization of robotic learning. In the analysis of human constrained motion, we proposed a combined criterion of hand-force change criterion and muscle
S. Hosoe et al. / Understanding and Realization of Constrained Motion
37
force change criterion. The criterion predicted the experimental results well enough. The idea that human might construct the parameterization of the constraint was applied to the robotic motion learning. We proposed to introduce the knowledge of the constraint into the reinforcement learning framework by model-based learning with function approximation. The simulation results showed that the proposed learning method improved the learning performance. We expect that the further development of formal learning methods can be realized by mimicking the learning techniques witnessed in human movements.
Reference [1] Richard S.Sutton and Andrew G.Barto, “Reinforcement Learning,” MIT Press, 1998. [2] Rolf Pfeifer and Christian Scheier, “Understanding Intelligence,” MIT Press, 1999. [3] P. Morasso, “Spatial control of arm movements,” Experimental Brain Research, 42, pp. 223-227, 1981. [4]
T. Flash and N. Hogan, ”The coordination of arm movements”, Journal of Neuroscience, vol.5, pp.1688-1703,1985.
[5] K. Ohta, M. Svinin, Z.W. Luo, S. Hosoe, and R. Laboissiere, “Optimal Trajectory Formation of Constrained Human Arm Reaching Movements,” Biological Cybernetics, Vol. 91, No. 1, July 2004, pp. 23-36. [6] Mikhail Svinin, Youhei Masui, Zhiwei Luo, and Shigeyuki Hosoe, “On the Dynamic Version of the Minimum Hand Jerk Criterion,” Journal of Robotic Systems, Vol. 22, No. 11, pp.639-651, 2005. [7] R. S. Sutton, “Dyna, an Integrated Architecture for Learning, Planning, and Reacting,” Proc. of the 7th Int. Conf. on Machine Learning, pp. 216-224,1991. [8] Andrew G. Barto, Steven J. Bradke and Satinder P. Singh,
Learning to Act using Real-Time Dynamic
Programming,” Artificial Intelligence, Special Volume: Computational Research on Interaction and Agency, 72, 1995, pp.81-138. [9] C. G. Atkeson, A. W. Moore, and S. Schaal: “Locally Weighted Learning for Control,” Artificial Intelligence Review, Vol.11, pp.75-113,1997. [10] J. Morimoto and K. Doya:“Reinforcement Learning of dynamic motor sequences: learning to stand up,” Proc. of IEEE/RSJ Int. Conf. of Intelligent Robots and Systems, Vol.3, pp.1721-1726, 1998. [11] Y. Kobayashi and S. Hosoe, “Reinforcement Learning for Manipulation Using Constraint between Object and Robot,” Proc. of IEEE Int. Conf. on Systems, Man & Cybernetics, pp. 871-876, 2005. [12] A.G. Barto, “Adaptive critics and the basal ganglia,” In Models of Information Processing in the Basal Ganglia, Cambridge, MA: MIT Press, pp.215-232, 1995. [13] C.D. Fiorillo, P.N. Tobler, and W. Schultz, “Discrete coding of reward probability and uncertainty by dopamine neurons,” Science, vol. 299, pp. 1898-1902, 2003. [14] K. A. Thoroughman and R. Shadmehr, “Learning of action through adaptive combination of motor primitives,” Nature 2000 Oct 12;407(6805):682-3.
This page intentionally left blank
Part 1 Navigation and Motion Planning
This page intentionally left blank
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
41
The Specifiability Requirement in Mobile Robot Self-Localization Francesco Capezio, Antonio Sgorbissa 1 , and Renato Zaccaria DIST - University of Genova Abstract. In last years, Simultaneous Localization And Mapping techniques monopolize research in mobile robot self-localization, especially in the civilian, Service Robotics domain. In this paper we argue that techniques derived from the industrial scenario, in particular beacon-based triangulation systems, should be taken into considerations even for civilian applications whenever we deem important to provide accurate, a priori specifications about the system behavior in a generic, yet untested environment. The paper provides an analytical expression of sensitivity to errors for triangulation depending on the system geometry. Keywords. Mobile Robotics, Beacon-based triangulation, System design
1. Introduction Self-localization has been the key for success for all those systems which are agreed upon as “big achievements” in the last decade (some renown examples are [1] [2] [3]). This is not a surprise, if we consider long term autonomy as a discriminator (the “six months between missteps” that Moravec advocates [4]): whenever robots are required to work continuously, for a long period of time, carrying out their tasks with no performance degradation or human intervention, the ability to keep dead reckoning errors (which are intrinsically cumulative) limited over time plays a fundamental role. Among the criteria which are usually adopted for classifying localization methods, in this work we focus onto a single, specific criterion: i.e., by extending the dialectic between “artificial landmark” and “natural landmark” [5], we distinguish two categories: 1. Approaches which rely on features which are naturally present in the environment to update the robot’s perceptions. Autonomous systems in the civilian, Service domain mainly belong to this category; features in the environment are detected by means of proximity sensors (sonars and, more recently, laser rangefinders [2] [3]) or vision. Simultaneous Localization And Mapping falls in this class: it puts further constraints by assuming that, in addition to being unmodified, the environment is also unknown. 2. Approaches which introduce artificial, ad hoc designed features or devices in the environment in order to improve and simplify the perception process. AGVs in the industrial domain very often belong to this category; since both the signal emitting source and the sensor are designed ad hoc, the only limit is one’s imagination. Some examples 1 Correspondence to: Antonio Sgorbissa, DIST - University of Genova. Tel.: +39 010 353 2801; Fax: +39 010 353 2154; E-mail:
[email protected].
42
F. Capezio et al. / The Specifiability Requirement in Mobile Robot Self-Localization
are spray-painted tracks in the floor adopted by Bell and Howell Mailmobile, Komatsu Z-shaped metal landmarks [6], MDARS-I retroreflective landmarks [7], DBIR LaserNav [8], and TRC Beacon Navigation System [9]. Approaches belonging to the second class allow better performance: that is why they are often preferred in the industrial domain. However, there are often very good motivations to leave the environment unaltered: commonly raised questions address the system’s migrability (i.e., can it behave indifferently in different environments?), biological plausibility, cost, accessibility (i.e., is it possible at all to modify the environment?), and would deserve further discussion. Here we focus onto a different issue, which is rarely taken into consideration when evaluating a localization systems: we call it specifiability, a market-inspired term which means “how accurately can a consumer assess... whether or not the good will satisfy the purpose it is purchased for” [12]. In Section II we show that the current trend in mobile robotics (especially indoor, SLAM-based, Service Robotics) has many difficulties to answer to the specifiability requirement. In Section III we focus onto the triangulation algorithm: we provide a closed form expression of triangulation and we formally derive an expression of the algorithm sensitivity to measurement errors. In Section IV, MatLab simulations and experimental results with the beacon-based system that we designed are shown. Conclusions follow.
2. Specifiability In [3] a large scale application in the museum scenario is discussed: the localization subsystem, based on two laserscanner and a feature based approach, undoubtedly demonstrates to be robust and reliable enough for the assigned tasks. Suppose however to be interested in buying or renting a similar system, and to consequently ask for a priori, formal specifications about its localization accuracy in a new, untested environment. Suppose also that, as the only available information, you can provide interior plans of the new environment and, maybe, some photographic snapshots. Unfortunately, in the case of laser-based (or vision based) feature extraction and matching, predicting the system’s accuracy from elementary considerations about the geometry of the environment is not simple. The situation is even more dramatic if the environment dynamically changes during task execution. Notice that this lack of specifiability is not due to limitations of the sensor; the main problem is the complexity of the interactions of the sensor with the environment, which causes feature extraction and matching to combinatorially depend on many factors: the number, the geometry and distribution of available features, the relative position and orientation of the robot with respect to each of them, the similarities and differences between potentially confusable features, lighting conditions (for vision), etc. On the opposite, complete, formal specifications are usually provided for AGV localization and navigation in the industrial domain [5]. Let us focus onto positioning errors: either the robot’s motion is constrained along a fixed trajectory or it relies on a finite number of artificial landmarks to correct its position, distribution p(errr |r) of the positioning error errr can be predicted for every robot’s configuration r by knowing 1. measurement error distribution p(errs |r) for each landmark si sensed in r; 2. sensitivity to errors, i.e. how positioning errors depends on measurement errors. If the robot’s state can be expressed in closed form as r = F(s1 , .., sn ), sensitivity is given by the Jacobian J of the partial derivatives of F with respect to s1 , .., sn .
F. Capezio et al. / The Specifiability Requirement in Mobile Robot Self-Localization
43
About 1, notice that also statistical approaches relying on natural landmarks are required to compute p(errs |r). However, with artificial landmarks, measurement errors can be evaluated with higher fidelity both in the theoretical and the experimental noise analysis: since one usually designs both the sensor and the landmark, the underlying model is better known and - hopefully - simpler. About 2, sensitivity proves to be particularly relevant when using artificial landmarks. In this case, landmark detection does not depend anymore on the complexity of the environment (which can be ignored), but on the spatial arrangement of landmarks as it has been decided with the sole aid of interior plans: i.e., for every r, it is always possible to know in advance which landmarks are visible and the corresponding p(errs |r). Thus, once the spatial arrangement of landmarks has been decided and sensitivity has been computed on the basis of pure geometric considerations, positioning errors p(errr |r) are predictable and fully specifiable, for any area of the Workspace in which enough landmarks are available for localization, as a function of the sole robot state r. Among localization systems commonly used in the industrial domain, beacon-based localization through triangulation seems a good compromise to meet all the requirements introduced so far while suiting two major needs of civilian scenarios: beacons can be elevated from ground (thus avoiding occlusions due to surrounding people) and they allow the robot to exhibit complex metric navigation in the free space [8] [9]).
3. Triangulation expressed in closed form Triangulation assumes that the robot knows the position of (at least) three landmarks bi = (xbi , ybi ), i = 1, .., 3 (i.e., beacons in our system) with respect to a fixed Cartesian frame F . By measuring the relative angles θ1 , θ2 , θ3 between the vehicle’s heading (to be computed) and the three landmarks, triangulation allows to retrieve the position and orientation r= (xr , yr , θr ) of the vehicle with respect to F (Fig. 1a). The problem has a graphic solution which is very simple and evocative (Fig. 1b): if the robot observes two landmarks (say b1 , b2 ) separated by an angle θ12 (i.e., the difference θ1 − θ2 , with positive angles measured counter clockwise), it must be on a circle C12r univocally determined by that angle and the distance between the landmarks. When considering another couple of landmarks (say b2 , b3 ) and the corresponding angle θ23 , another circle C23r is defined, which intersects with the previous one in two points: b2 and (xr , yr ), i.e. the actual robot’s position. Finally, by knowing (xr , yr ) and any of the three angles θ1 , θ2 , θ3 , orientation θr can be retrieved. This graphic solution (also referred to as Circle Intersection [10]) can be implemented as a closed form expression xr = fx (θ12 , θ23 ), yr = fy (θ12 , θ23 ), θr = fθ (θ12 , θ23 )
(1)
which will be described in details in the following. The graphic approach helps to visualize singularity configurations which are well known in literature: 1. the robot lies on the same circle (possibly collapsing to a straight line) which connects the three beacons: C12r and C23r overlap and the solution is not univocal; 2. the robot lies on the line which connects two landmarks, say b1 , b2 : the subtended angle θ12 equals 0 or π and C12r consequently collapse to a line. Notice that, whilst in case 1 the problem has intrinsically infinite solutions, in case 2 a unique solution still exits: it just requires a special care to deal with it. Moreover, in
44
F. Capezio et al. / The Specifiability Requirement in Mobile Robot Self-Localization
Figure 1. a) The Triangulation Problem b) Graphical solution
both cases, all configurations which are close to singularities reveal to be problematic: a report [10] of triangulation algorithms in literature, compared on the basis of their sensitivity to measurement errors, shows that Circle Intersection provides the higher accuracy as possible (by ignoring iterative, approximate methods such as the Newton-Rapson which assume an a priori estimate of the robot’s position). In [11] authors claim that the Trigonometric Method is able to find a solution even for type 2 singularities whilst Circle Intersection is not. This is imprecise: of the three subtended angle θ12 , θ23 , θ31 (each uniquely defining a circle C12r , C23r , C31r ), at most one can equal 0 or π (unless all beacons lie on the same line). The information available is thus redundant, allowing to consider the two angles for which the singularity is not reached. Since its simplicity and efficiency, we consider only the Circle Intersection algorithm, of which we give a closed form expression which is much simpler than in [10]. Next, to express sensitivity to error, we analytically compute the Jacobian J of the partial derivatives of fx , fy and fθ with respect to θ12 , θ23 , θ31 . This idea is first introduced in [11]; surprisingly, authors do not provide a closed form expression of J, and limit themselves to simulating the effect of angular errors on an Error Sensitivity Map. Let us imagine to choose angles θ12 and θ23 , and to consequently consider the circles C12r and C23r . First, we compute the center of C12r : by elementary geometric considerations (Fig. 2a), we know that the angle between b1 and b2 , when measured from the center cr12 , is doubled with respect to θ12 . Thus, the distance between cr12 and b1 b2 can be computed as follows 2 2 d12 = 0.5 (xb2 − xb1 ) + (yb2 − yb1 ) tan(θ12 ) (2) notice that, if we consider θ12 in the interval 0 2π , d12 can assume negative values: this implicitly considers the fact that cr12 can be on both sides of b1 b2 . Notice also that the circles defined by θ12 and θ12 + π are the same; however, for every angle θ12 , cr12 is univocally defined. Next, we compute the midpoint m12 between b1 and b2 and we add to m12 a vector d12 which is perpendicular to b1 b2 and has length d12 After some substitutions, this yields the following expression for cr12 (and cr23 ): xcr12 = 0.5 (xb1 + xb2 + (yb2 − yb1 )/tan (θ12 )) ycr12 = 0.5 (yb1 + yb2 − (xb2 − xb1 )/tan (θ12 ))
(3)
There are some singular points in Equation 3: 1. θ12 = 0, π corresponds to a circle with infinite radius (i.e., collapsing to a line), a type 2 singularity which requires considering a different couple of landmarks
F. Capezio et al. / The Specifiability Requirement in Mobile Robot Self-Localization
45
2. θ12 = π, 3π/2 corresponds to a circle whose center lies on b1 b2 , i.e., the midpoint between the two landmarks. Next, we compute the Equation of the straight line cr12 cr23 (Fig. 2b) and the distance db2 between cr12 cr23 and b2 (the first intersection of C12r and C23r ) db2 =
(ycr23 − ycr12 ) xb2 − (xcr23 − xcr12 ) yb2 − ycr23 xcr12 + ycr12 xcr23 2 2 (ycr23 − ycr12 ) + (xcr23 − xcr12 )
(4)
since the second intersection (i.e., corresponding to xr , yr ) is symmetrical to b2 with respect to cr12 cr23 , we compute it by adding to b2 a vector db2 which is perpendicular to cr12 cr23 and is doubled than db2 (but has an opposite sign). 2 2 xr = xb2 − 2db2 (ycr23 − ycr12 ) (ycr23 − ycr12 ) + (xcr23 − xcr12 ) (5) 2 2 yr = yb2 + 2db2 (xcr23 − xcr12 ) (ycr23 − ycr12 ) + (xcr23 − xcr12 ) To compute orientation θr , we use Equation 6 (handling the special case ycr23 = ycr12 ). xcr23 − xcr12 yb2 − yr − θ2 = arctan − − θ2 θr = arctan (6) xb2 − xr ycr23 − ycr12 Notice that, at this point, we could simply substitute Equations 3 and 4 in 5 and 6, thus obtaining a single, closed form expression as in Equation 1. Sensitivity to errors can be analytically written as follows: Δxr = Δθ12 · ∂fx /∂θ12 + Δθ23 · ∂fx /∂θ23 Δyr = Δθ12 · ∂fy /∂θ12 + Δθ23 · ∂fy /∂θ23 Δθr = Δθ12 · ∂fθ /∂θ12 + Δθ23 · ∂fθ /∂θ23
(7)
which requires to compute the Jacobian J of the partial derivatives of fx , fy , fθ with respect to θ12 and θ23 . By noticing that the denominator in Equation 4 and 5 is the same, we are allowed to write: n = num (db2 ) , d = den (db2 )2 ∂xr ∂θ12 ∂yr ∂θ12 ∂θr ∂θ12
cr12 n = 2 ∂y ∂θ12 d − 2 (ycr23 − ycr12 )
1 ∂n d∂θ12
−
n ∂d d2 ∂θ12
1 ∂n n ∂d cr12 n = −2 ∂x + 2 (x − x ) − cr23 cr12 2 d ∂θ12 d ∂θ12 ∂θ12 d ∂ycr12 cr12 = d1 ∂x (y − y ) − (x − xcr12 ) cr23 cr12 cr23 ∂θ12 ∂θ12
(8)
Similarly, we can compute the partial derivatives with respect to θ23 . Equation 8 can be used 1) to improve the system’s accuracy in run-time and 2) to compute the sensitivity to errors for every r and with different beacon configurations. 1. In Equation 1, we expressed r= (xr , yr , θr ) as a function F(θ12 , θ23 ); however, as we already stressed, other two choice are available, namely to consider the couple (θ23 , θ32 ) or (θ12 , θ31 ). We call F1 = F(θ12 , θ23 ), F2 = F(θ23 , θ31 ) and F3 = F(θ12 , θ31 ) and J1 , J2 , J3 the corresponding Jacobian matrices of the partial derivatives. Whenever a triplet (θ12 , θ23 , θ32 ) is measured, we compute, for each component of the state (xr , yr , θr ), which Ji yields the minimum sensitivity to errors (it
46
F. Capezio et al. / The Specifiability Requirement in Mobile Robot Self-Localization
Figure 2. a) Computing the circle center b) Intersecting circles A. Circle Intersection
just requires to compute Equation 8 and 7 three times): this allows to choose which of the subtended angles (θ12 , θ23 , θ32 ) should be considered (and which one should be ignored). Angles which are close to 0 or π are automatically rejected; solutions which are very sensitive to errors can be rejected as well. Finally, when more than three beacons are available, Ji can be used to determine which triplet should be considered. 2. For any given beacon distribution, we are allowed to predict the positioning error distribution p(errr |r) for any location of the environment r in which at least three beacons are visible. To achieve this, for any given r we compute the three expected angles (θ12 , θ23 , θ32 ) = h(r) (i.e., h is the measurement model), we evaluate the three expected Jacobian matrices J1 , J2 , J3 as a function of r as in Equation 8, and we finally derive positioning errors from measurement errors p(errs |r) through Equation 7. If we limit ourselves to a worst case analysis, computing the maximum positioning error as a function of the maximum measurement error is trivial; otherwise, more complex manipulations are required. This procedure can provide guidelines for beacon arrangement.
4. Experimental Results In the following, to simplify computations, we assume a maximum error errθmax on the measure of θ12 , θ23 , and θ31 , thus ignoring the full distribution p(errs |r); moreover, we consider the x− and y−components of r, whilst θr is neglected. Since errθmax = Δθ12 = Δθ23 = Δθ31 in Equation 7 is constant, we can easily compute, for each Fi , i = 1, .., 3, the maximum position error errrmax as a function of errθmax and r: in Equation 9, (θ12 , θ23 ) = h(r) are considered.
∂fx 2 ∂fy 2 ∂fy 2 ∂fx 2 errr max = Δx2r + Δyr2 = errϑ max + + + (9) ∂θ12 ∂θ23 ∂θ12 ∂θ23 Under these assumptions, the Euclidean norm S of J is sufficient to determine the sensitivity to measurement errors for each position in the Workspace r, and can be effectively adopted to decide which angle in the triplet (θ12 , θ23 , θ32 ) should be ignored. Figure 3 shows the plot of S(r) versus (xr , yr ) when b1 = (0, 0), b2 = (10, 5), b3 = (10, 0) and different couples of subtended angles are considered, i.e., S1 (θ12 , θ23 ), S2 (θ23 , θ31 ), and S3 (θ12 , θ31 ); values are saturated to Smax = 100 for ease of representation. In all the Figures the highest sensitivity to error corresponds to the circle which connects the three beacons; positions which are close to this circle have an increasing sensitivity, thus
F. Capezio et al. / The Specifiability Requirement in Mobile Robot Self-Localization
47
Figure 3. Sensitivity computed with (θ12 , θ23 ), (θ23 , θ31 ), and (θ12 , θ31 ); minimum sensitivity.
yielding very inaccurate results during localization. Straight lines connecting couples of beacons yields inaccurate results as well (e.g., b1 b2 and b2 b3 for S1 ); however, it is sufficient to consider different couples: Figure 3 shows the plot of mini (Si ), in which the singularity is no more present. Figure 4a shows the plot of errrmax (expressed in meters) when errθmax = 1◦ , projected onto (x, errrmax ): it can be noticed that the highest accuracy (about 6cm) is achieved within the connecting circle, whilst errors increase outside. Figure 4c and d shows S(r) with different beacon configurations: by plotting sensitivity it is possible to infer how to arrange beacons in order to achieve the desired accuracy in any given area of the environment. Finally, the approach has been tested for many years with different experimental setups: Figure 4b shows the positioning errors of a real robot by considering the couple (θ12 , θ23 ); the resolution is obviously lower (data are collected every 0.3m), but the trend of errr and its magnitude approximately meet our expectations (values expressed in mm) 5. Conclusions If we want autonomous mobile robots to “hit the market” for civilian applications, it is fundamental to provide customers with a priori accurate specifications of the behavior of a system in a previously untested environment. To deal with the specifiability requirement, we propose to borrow localization methods from industrial AGVs; in particular, beacon based triangulation, which proves to be a good compromise for its simplicity and versatility. Analytical expressions of triangulation and its sensitivity to errors are given: simulations show how this information can be used to predict the system’s accuracy through geometric considerations.
48
F. Capezio et al. / The Specifiability Requirement in Mobile Robot Self-Localization
Figure 4. a) Plot of errrmax b) A real world experiment c, d) Sensitivity with different beacon configurations
References [1] Thrun, S., Beetz, M., Bennewitz, M., Burgard, W., Cremers, A., Dellaert, F., Fox, D., Hähnel, D., Rosenberg, C., Roy, N., Schulte, J., Schulz. D., 2000. Probabilistic Algorithms and the Interactive Museum Tour-Guide Robot Minerva. Int. J. of Robotic Research, Vol. 19, No 11. [2] Burgard, W., Cremers, A.B., Fox, D., Hähnel, D., Lakemeyer, G., Schulz, D., Steiner, W., and Thrun., S., 2000. Experiences with an interactive museum tour-guide robot, AI, 114 (1-2). [3] Siegwart R., Arras K.O., Bouabdallah S., Burnier D., Froidevaux G., Greppin X., Jensen B., Lorotte A., Mayor L., Meisser M., Philippsen R., Piguet R., Ramel G., Terrien G., Tomatis N., 2003. Robox at Expo.02: A Large-Scale Installation of Personal Robots, Robotics and Autonomous Syst., vol. 42, issue 3-4. [4] Moravec, H., Rise of the Robots, in Scientific American, December 1999, pp. 124-135. [5] J. Borenstein, H. R. Everett, L. Feng, Where am I? Sensors and Methods for Autonomous Mobile Robot Positioning - Available from ftp.eecs.umich.edu/people/johannb, 1996. [6] Matsuda, T. and Yoshikawa, E., 1989. Z-shaped Position Correcting Landmark, Proc. of the 28th SICE Annual Conference. [7] Laird, R.T, H.R. Everett, G.A. Gilbreath, R.S. Inderieden, and K.J. Grant, "Early User Appraisal of the MDARS Interior Robot," American Nuclear Society 8th International Topical Meeting on Robotics and Remote Systems (ANS’99), Pittsburgh, PA, 25-29 April, 1999 [8] Maddox, J., Smart Navigation Sensors for Automatic Guided Vehicles, Sensors, April 1994. [9] TRC - Transitions Research Corp., Beacon Navigation System, Product Literature, Shelter Rock Lane, Danbury, CT 06810, 203-798-8988. [10] Cohen, C. and Koss, F., 1992. A Comprehensive Study of Three Object Triangulation, Proc. of the 1993 SPIE Conference on Mobile Robots, Nov. 18-20, pp. [11] Shoval, S., and Sinriech, D., 2001. Analysis of Landmark Configuration for Absolute Positioning of Autonomous Vehicles, Journal of Manifacturing Systems, Vol. 20 No. 1. [12] Koppius, O., 1999. Dimensions of Intangible Goods, Proc. of the Thirty-second Annual Hawaii Int. Conference on System Sciences-Vo 5, 1999
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
49
Ghost-Goal Algorithm for Reactive Safe Navigation in Outdoor Environments Mattia Castelnovi, Antonio Sgorbissa, Renato Zaccaria Laboratorium DIST Università di Genova {mattia.castelnovi, antonio.sgorbissa, renato.zaccaria}@unige.it Abstract—A force field algorithm based on inclinometer readings is presented. It leads the robot to the goal by preventing it from turning upside down because of the inclination of the terrain. The method is supported by a navigation algorithm, which helps the system to overcome the well known local minima problem for force fieldbased algorithms. By appropriately selecting the “ghost-goal” position, known local minima problem can be solved effectively. When the robot finds itself in danger of local minimum, a “ghost-goal” appears while the true goal temporarily disappears in order to make the robot go outside dangerous configurations. When the robot escapes the possible dangerous configuration the system makes the true goal appear again and the robot is able to continue on its journey by heading towards it. Simulation results showed that the Ghost-Goal algorithm is very effective in environments with complex rugged terrain for a robot only equipped with an inclinometer.
1. Introduction Autonomous outdoor navigation in vast unknown environments is a special challenge for mobile robots. In the recent past, autonomous mobile robots have been able to accomplish a great deal of tasks in indoor environment, such as exploration[1], self-localization[2] and map building[3]. In most of these cases the problem of autonomous navigation supposed that the world is a flat surface. Such assumption allowed researchers to develop navigation algorithms that consider the robot a material point, and to consequently plan a path to the goal as a curve on a plane. One category of algorithms which proved to be especially suited to deal with unknown, dynamic environments is the so called force-field (also known as Artificial Potential Field) method. Unfortunately, when moving autonomous robots from indoor to outdoor environments many problems arise. In fact, in outdoor environments robotic platforms must deal with other hazardous situations [4], in addition to collision with obstacles one of them being the risk of turning upside down while trying to reach the goal [5, 6]. When the robot is climbing up or down a hill, moving through dunes, etc., the navigation system must take into consideration the maximum allowable pitch and roll values for the robot to avoid turning upside down. The following work proposes a forcefield algorithm which relies on data acquired from an inclinometer with the purpose of keeping the robot far from “dangerous slopes”. Obviously the difference between “dangerous slopes” and obstacles is quite hard to describe: in this paper the ground slope is considered as if it were continuous; we assume that obstacles would locally produce an
50
M. Castelnovi et al. / Ghost-Goal Algorithm for Reactive Safe Navigation in Outdoor Environments
infinite derivative, which cannot be detected by the inclinometer and requires ad hoc sensors (proximity sensors, for example). However obstacles are not considered here, we focus on a force-field algorithm which proves to be adequate to lead the robot to the goal, whilst preventing it from turning upside down because of the inclination of the terrain. Our method differs from other existing approaches, which usually rely on vision systems [7, 8] or proximity sensors [6], our system is thus provided with just an inclinometer which locally provides information about the current pitch and roll of the robot. As a consequence of the poor sensing capabilities that are available, the potential for getting stuck in local minima is higher. This problem is solved by a novel approach, which we call the “ghost goal” method. The method works in the following manner: when the robot finds itself in danger because of a local minimum, a “ghost-goal” appears, and the true goal temporarily disappears. Once the potential for local minima is reduced, it is able to continue on its journey by heading towards the true goal. Simulation results showed that the approach is very effective in environments with complex configuration for a robot equipped just with an inclinometer. The paper is structured as follows: section II describes how the force-field inspired method is implemented using data from an inclinometer; section III is focuses Ghost-goal algorithm which help the system not to fall into local minima; section IV describes experimental results. The final section is devoted to conclusions.
2. Inclinometer Based Force Field Method The presented system implements a force-field method based on the robot position and its pitch and roll angles. In force-field methods [9], the goal configuration exerts an attractive force on the robot, while obstacles exert a repulsive force. We extend the basic principle in order to deal with constraints on the robot’s maximum allowable pitch and roll. First of all we define a fixed Cartesian frame (Of, Xf, Yf) and a Cartesian frame (Or, Xr, Yr) centered on the robot, with the Xr axis oriented along the heading of the robot (Fig.1). In the following, all the quantities with subscript ‘f’ are defined with respect to (Of, Xf, Yf ), whilst those with subscript ‘r’ are defined with respect to (Or, Xr, Yr). Since the robot is going to be used in outdoor missions, we assume that it is equipped with a GPS and that the system is able to localize itself and to know the position of the goal with enough precision for our purpose. In particular, we define: x pr and rr , two unit vectors lying respectively along the Xr and the Yr axes. x qf(t), the configuration of (Or, Xr, Yr) with respect to the fixed frame at time t x tr(t), a unit vector directed from qf(t) to the goal x fr(t), the sum of the repulsive forces exerted by terrain slopes in qf(t) x gr(t), the gradient of the terrain in qf(t) expressed as inclination percentage x ir(t), a vector defined as follows: ir(t)·gr(t)=0, |ir(t)|=|gr(t)| (tangential to the isocline passing through qf(t), with the same magnitude as the gradient) It is straightforward to show that, if we want to minimize the pitch of the robot at time t, we need to minimize the absolute value of the dot product pr·gr(t), which can be done by forcing the robot to move along the isocline curve (i.e., by maximizing|pr·ir(t)|). On the
M. Castelnovi et al. / Ghost-Goal Algorithm for Reactive Safe Navigation in Outdoor Environments
51
opposite, if we want to minimize the roll, we have to minimize the absolute value of rr·gr(t), which can be done by forcing the robot to climb or descend the gradient (i.e., by maximizing |pr·gr(t)|). Thus, if we want to minimize both the pitch and the roll components, we have to choose the direction of motion of the robot in order to maximize the quantity |pr·ir(t)|+|pr·gr(t)|. This can be done by choosing the velocity vector (i.e. the robot heading) v(t)= ir(t) + gr(t). (1) However, in most cases, the maximum values allowed for the pitch and the roll components are different, since they depend on various physical characteristics (e.g. geometrical dimensions, weight distribution). Thus, at time t, given the current values pr(t) and rr(t) and their maximum allowed values (PitchUB, RollUB), we define the following quantities: 1 (2) mag r
mag p
RollUB
2
- RollAngle2
2
1
PitchUB
2
(3)
- PitchAngle 2
2
Finally, we calculate the velocity vector v (t)
mag p i r (t) mag r g r (t) mag p
2
i r (t)
2
mag r
2
g r (t)
(4) 2
as the most promising direction for the robot to safely move to (we are not considering the goal yet, nor obstacles to be avoided). In other words, as the pitch of the robot gets closer to its upper bound, magp quickly increases, thus forcing the robot to move along the isocline curve. Analogously, as the roll component gets closer to its upper bound, magr quickly increases, forcing the robot to climb/descend the gradient. That is, magp and magr are used to weight the sum of ir(t) and gr(t) in eq. 4, thus imposing different constraints for the maximum pitch and roll values (respectively PitchUB and RollUB) in order to safely keep the robot away from a dangerous configuration. Finally notice that, for each configuration qf(t), there are two vectors tangential to the isocline: ir(t) and -ir(t). As a consequence, since we want to maximize the absolute value | pr·ir(t)|, we can choose both ir(t) and -ir(t) in eq. 4. The choice is performed on the basis of the following rule: If ir(t) . tr(t ) < - ir(t). tr(t) choose ir(t) else choose - ir(t)
(5)
The vector pointing in the direction closer to the goal is selected to introduce a bias toward it (vector tr(t) ). In the same way the gr (t) vector is selected: if gr(t) · tr(t ) > - gr(t)·tr(t) choose gr(t) else choose - gr(t)
(6)
Finally, the attractive vector tr(t) heading towards the target is weighted and added to the velocity vector vr(t), as is typical in force-field models [1]. The resulting law of motion is:
52
M. Castelnovi et al. / Ghost-Goal Algorithm for Reactive Safe Navigation in Outdoor Environments
vr (t)=w1(magp ir(t) + magr gr(t))+w2 tr (t)
(7)
which drives the robot towards the goal while safely keeping pitch and roll values below the given threshold (w1, w2 are weighting factors) . In Fig. 3 it is possible to see different paths made by the robot when different upper bounds were imposed (in figures the robot icon just represents the robot and it is not scaled, but in the algorithm the robot is treated as a point). The Fig 3.a shows the path when both PitchUB and RollUB are at a very low value (5°=11.1%). The Fig 3.b shows the same mission with PitchUB = 5° and RollUB = 90° and the rest of the figures show the robot path when other values of PitchUB and RollUB (in Fig. 3 as p and r) are selected. In Figs. 13 and 14, the behavior of system with different PitchUB and RollUB values are shown when the Goal is located on the top of the hill. In the first of those Figure RollUB is much higher than PitchUB (PitchUB = 30°, RollUB = 90°). As a consequence, the robot moves towards the top of the hill by following a spiral shaped trajectory, which is necessary to keep the pitch very low with respect to the roll. In the second experiment (Fig. 14) the opposite is true: since PitchUB is much higher than RollUB (PitchUB = 90°, RollUB = 30°). The robot climbs toward the top of the hill by following a straight trajectory which allows to keep the roll value very low.
3. Ghost-goal algorithm 3.1. Local Minima problem in force- field methods The force-field method considers the robot as a particle under the influence of an artificial field of forces which is expected to summarize the “structure” of the free space [10]. The force-field function is usually defined as the sum of an attractive vector which pulls the robot toward the goal configuration and a repulsive vector which pushes the robot away from obstacles. One known drawback of the force-field method is the occurrence of local minima in the force function [15]. Local minima occur in configurations where the repulsive force generated by obstacles cancel exactly the attractive force generated by the goal (Figs. 6, 7, 12). The Ghost-Goal algorithm includes appropriate techniques for escaping from local minima. 3.2. Ghost-Goal applied on a inclinometer based Force Field Method This section explains how the Ghost-Goal algorithm avoids local minima. The Ghost-goal algorithm can be divided into 2 steps: 1) Barrier detection, 2) Ghost-Goal position selection. 1. Barrier detection: two unit vectors are introduced: the first one, which we call tdisc(t) heads to the goal like unit vector tr(t), but its direction is discretised with a very low resolution of 45° (i.e., like a wind star). The second one, which we call gdisc(t), points toward the direction of the gradient like gr(t) (with the same resolution than tdisc(t)). For example, in Fig. 4. , tdisc(t) is pointing West, and gdisc(t) is pointing North-West instead. The system periodically checks the direction of tdisc(t) and gdisc(t), and makes some
M. Castelnovi et al. / Ghost-Goal Algorithm for Reactive Safe Navigation in Outdoor Environments
53
considerations about the situation that the robot is probably about to face. If the two unit vectors “almost” lie over the same straight line, the robot is possibly approaching a local minimum (Fig. 5). It has been decided to discretized the directions because it is a fast and easy way to compare the directions of 2 vectors keeping a margin of safety: If (| tdisc(t) gdisc(t)| = 1 )AND(|pr| > PitchBarrierFinderTH OR |rr| > RollBarrierFinderTH) then barrier = TRUE (8) bdisc(t) = gdisc(t) else barrier = FALSE Of course, this can happen only if the slope of the ground is significantly high to influence the robot’s motion as stated in Equations 4 and 7. At the same time, the system monitors if both pitch and roll values are below their Barrier-Finder-thresholds defined as follows: PitchBarrierFinderTH = PitchUB/\ RollBarrierFinderTH = RollUB/\
(9)
where \ ҏis a constant which manage the sensibility of the system to the local minima. If one of them is over the threshold and the tdisc(t) and gdisc(t) almost lie over the same sector direction (for example in Fig. 5 gdisc(t) is pointing North-West), a flag barrier is set equal to TRUE and a new vector bdisc(t) is set equal to gdisc(t)..This means that there is a potential local minimum to the goal. This step allows the system to identify dangerous situations which are due either to the slope of the ground or the relative position of goal.
Figure 2. 3D model for Figure 1. Definition of the area of the the used vectors experiments in Fig.3.
Figure 4. Configuration Figure 5. Configuration Figure 3. Different experiments made with different without barrier detection with barrier detection PitchUB and RollUB .
2. Ghost-Goal position selection. Once we have detected a possible local minimum, the robot must avoid falling into it. To deal with this, the algorithm does not require to perform local search procedures; in fact, a solution can be easily found just by temporarily move the Goal position, thus providing the robot with fallacious perceptions which, in some sense, are the analogous of what we call “a mirage”. The robot “sees” the goal in a more
54
M. Castelnovi et al. / Ghost-Goal Algorithm for Reactive Safe Navigation in Outdoor Environments
conformable and less hazardous position and tries to reach it using the same rules used in its usual (= without dangerous situations) navigation. In this way it takes itself far away from the local minima. If barrier = TRUE Then Set the Ghost Goal(…)
(10)
As soon as the dangerous situation disappears, the system is able to “understand” that it was just a mirage, a ghost, and it is no more useful to follow that direction. In fact, when the relative position of the robot has changed with respect to the goal and the surrounding hills or valleys, the just avoided local minima -probably - are not dangerous anymore. The robot can start again heading to the real goal. Obviously, we must carefully select the Ghost-Goal in order to make the robot escape from local minima while moving closer to the real goal or, at least, while not going “too far” from it. To achieve this, the Ghost Goal is not explicitly represented as a position in the Workspace, but as a suggested direction tsafe(t) (a unit vector with the same 45° discretization than tdisc(t), gdisc(t), bdisc(t)), which is chosen according to the following rules: Set the Ghost Goal(…) If RobotDir[arg(bdisc(t)-HT), arg(bdisc(t))+HT] then arg(tsafe (t)) = arg(bdisc(t))±HT
(11)
where HT identifies the maximum deviation from the real goal allowed; its sign is chosen in order to minimize the discrepancy between tsafe (t) and tdisc(t). If barrier = FALSE then tsafe(t) = tdisc(t)
(12)
I.e., as soon as the gradient changes, either in its direction or its magnitude, the Ghost-Goal disappears and the robot starts heading again towards the real goal.
4. Experimental Results The system has been developed and tested using the AE-SIM simulator [11] a tool included in ETHNOS [12] (Expert Tribe in a Hybrid Network Operating System) a programming environment for multi autonomous robot systems. The software allows the robot to receive data from the simulated environment in the same way as it would receive it from a real sensor. To simulate an outdoor environment, Digital Elevation Models have been used. The size of each cell is 1 meter2, which is very accurate compared to other inclinometer-driven navigation algorithms implemented in simulation for analogous mobile robot tasks [13]. Figure 11 shows the path of the robot in a labyrinth which has a flat surface on the ground and sloped walls for making inclinometer sensing. This experiment shows that the robot reaches the goal even though it encounters 5 local minima. In Figs. 8, 9, 10, a “castle-like” environment is considered. Walls are inclined with an increasing slope (i.e., the second
M. Castelnovi et al. / Ghost-Goal Algorithm for Reactive Safe Navigation in Outdoor Environments
55
order derivative is positive). The goal is placed onto a “flat” square in the middle of the castle. A small entrance is present in the walls, to show that the system is able to find paths in very difficult situation too. The first mission (Fig.9) is made with a low value of PitchUB and a high value of RollUB. The robot reaches the entrance after a spiral shaped trajectory that makes the robot come closer to the goal without overcome the threshold. This simulation shows how the Ghost-Goal algorithm it is able to reach the goal even in extreme situations. Next figure (Fig.10) shows the same experiment made with a low value of RollUB and a high value of PitchUB. Since the walls at their top are almost vertical the robot turns in order not to damage itself. Two short turns allow it to reach the goal.
Figure 6. Same of Fig. 2.b. without Ghost-Goal algorithm
Figure 8. 3D model Figure 7. Same of for the castle-like Figs. 13 and 14 Figure 9. Spiral Figure 10. Straight to environment without Ghostgoal robot shaped robot trajectory the experiment Goal algorithm trajectory.
13. Spiral Figure 14. Straight to Figure 11. Path made inside Figure 12. Same of Fig.11 Figure without Ghost-Goal algorithm shaped trajectory goal trajectory. a wall-sloped Labyrinth
In Figs. 6 and 7 examples of navigation missions without the Ghost-goal algorithm is shown: in Fig. 6 is possible to see the same experiment of Fig 2.b. using the same threshold (PitchUB = 5° and RollUB = 90°). The robot stops in front of the hill in correspondence of a local minimum. It continues to wheel around itself, but it is not able to move away from that position. The same happens in fig, 7, the robot, after coasting around the hill, stops in a local minimum and it is not able to escape from that position. In Fig 12 the robot stops at the first local minimum of the labyrinth. It is important to remember that, in these situations, the robot would have been able to reach the goal. Because the PitchUB is lower than the slope of the hills or the walls of the labyrinth it could have reached the goal by simply coasting around the hill (or the walls of the labyrinth) or using a spiral shaped trajectory.
5. Conclusions This paper presents a force-field inspired method based on inclinometer data which implements a new algorithm for escaping from local minima. The results from experiments
56
M. Castelnovi et al. / Ghost-Goal Algorithm for Reactive Safe Navigation in Outdoor Environments
conducted on different kinds of slope and threshold configurations have been shown. The developed system seems to be robust enough to be used with real robot. Before and during the system’s development, many experiments were made with different slopes and robot configurations. Without the Ghost-Goal algorithm, the robot was often able to overstep the upper bounds on its allowed pitch and roll, i.e., a really dangerous situation in the case a real robot. After developing the Ghost-Goal algorithm, the simulated robot becomes able to complete missions without exceeding the maximum allowed pitch or roll. The Ghost-goal algorithm allows the system to better capitalize on its capabilities and to reach the goal overcoming the problem of local minima even if some issues still arise, for example, from situation like C-obstacles due also to the discretization of the direction. For a complete autonomous navigation using the Ghost-Goal algorithm, future work will focus on, installing the system on a real robot provided with a localization system and testing it with supplementary sensors (e.g., for obstacle avoidance). REFERENCES [1] [2] [3] [4]
[5] [6]
[7] [8] [9] [10] [11]
[12]
[13] [14]
[15]
W. Burgard, M. Moors, C. Stachniss, and F. Schneider Coordinated Multi-Robot Exploration. IEEE Transactions on Robotics, 21(3), 2005. S. Thrun, D. Fox, W. Burgard and F. Dellaert Robust Monte Carlo localization for mobile robots. Journal of Artificial Intelligence, 128 (1-2), pp. 99-141, 2001. Folkesson, J., Jensfelt, P., and Christensen, H. Vision slam in the measurement subspace. In Intl Conf. on Robotics and Automation (Barcelona, ES, Apr 2005), IEEE. ] Castelnovi, M., Arkin, R., and Collins, T., 2005. "Reactive Speed Control System Based on Terrain Roughness Detection", 2005 IEEE International Conference on Robotics and Automation, Barcelona, SP, April 2005. Arkin, R.C. and Gardner, W., 1990. "Reactive Inclinometer-Based Mobile Robot Navigation", Proc. 1990 IEEE International Conf. on Robotics and Automation, Cincinnati, OH, pp. 936-941. Marco Giaroli, Antonio Sgorbissa, Renato Zaccaria, 2002, "Reactive Safe Navigation in Outdoor Environments", International Symposium on Robotics and Automation (ISRA 02), September 2002, Toluca, Mexico. Langer, D., Rosenblatt, J., and Hebert, M.,.1994. A Behavior-Based System for Off-Road Navigation, IEEE Trans. Robotics and Automation, Vol. 10, No. 6. Matthies, L.H., 1992. Stereo Vision for Planetary Rovers: Stochastic Modeling to Near Real-Time Implementation, International Journal of Computer Vision, 8:1. Arkin, R.C., 1989. "Motor Schema-Based Mobile Robot Navigation", International Journal of Robotics Research, Vol. 8, No. 4, pp. 92-112. Brooks, R. A., 1986, "A Robust Layered Control System for a Mobile Robot." IEEE Journal of Robotics and Automation, Vol. RA-2, No. 1, pp. 14-23. Andrea Raggi, Antonio Sgorbissa, Renato Zaccaria, “AE-SIM: Simulating Intelligent Robots in Intelligent Environments”, Proceedings from The IEEE Conference on Control Applications/ International Symposium on Intelligent Control/ International Symposium on Computer Aided Control Systems Design (CCA/ISIC/CACSD 2004), Taipei, Taiwan, September 1-4 2004 Maurizio Piaggio, Antonio Sgorbissa , Renato Zaccaria ," A Programming Environment for Real Time Control of Distributed Multiple Robotic Systems " Advanced Robotics , The International Journal of the Robotics Society of Japan, Vol. 14, N.1, VSP Publisher, 2000. Arkin, R.C. and Gardner, W., 1990. "Reactive Inclinometer-Based Mobile Robot Navigation", Proc. 1990 IEEE International Conf. on Robotics and Automation, Cincinnati, OH, pp. 936-941 Zou Xi-yong and Zhu Jing “Virtual local target method for avoiding local minimum in potential field based robot navigation”, Journal of Zhejiang University SCIENCE (ISSN 1009-3095, Monthly) 2003 Vol. 4 No. 3 p.264-269. Arkin, R. 2000. Behavior-Based Robotics. MIT Press, Cambridge, MA .
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
57
Autonomous Robot Vision System for Environment Recognition Woong-Jae Won, Sang-Woo Ban and Minho Lee* School of Electrical Engineering and Computer Science, Kyungpook National University 1370 Sankyuk-Dong, Puk-Gu, Taegu 702-701, Korea *
[email protected] Abstract. In this paper, we propose a novel autonomous robot vision system that can recognize an environment by itself. The developed system has self-motivation to search an interesting region using human-like selective attention and novelty scene detection models, and memorize the relative location information for selected regions. The selective attention model consists of bottom-up saliency map and low level topdown attention model. The novelty detection model generates a scene novelty using the topology information and energy signature in the selective attention model. Experimental results show that the developed system successfully identify an environment as well as changing of environment in nature scene. Keywords. Autonomous robot vision system, selective attention model, novelty scene detection, environment recognition
Introduction The present intelligent system has been mainly based on task-specific manners and/or the programmer’s input about environmental information. Even through such a traditional artificial intelligent robot has been successfully used for simple tasks, we need a new artificial intelligent paradigm to continuously adapt the changes of an environmental and to voluntarily increase a new knowledge without any interrupt by a programmer. This concept would be useful in developing a truly human-like robot and it could be applied to many engineering fields such as artificial secretary, household robot, medical treatment nursing robot, etc. Recent researches have been toward developing a more human-like machine such as an autonomous mental development (AMD) mechanism [1,2,3]. The AMD mechanism is that robots can increase their own knowledge through continuous interaction with human and environment. In order to implement such an intelligent system with the AMD concept, we need to endow the developed robot system with environment perception and novelty detection based on human interaction [4]. In this paper, we propose an autonomous robot vision system which can not only learn the surroundings with self-motivated desire but also detect a novelty in a dynamic scene. The developed system is based on human-like selective attention model which imitates the human visual search mechanism. The selective attention model uses bottom-up process in conjunction with an interactive process that ignores an unwanted area and/or pays attention to a desired area in subsequent visual search process through human interaction [5,6,7]. Novelty detection has been used in video streams by
58
W.-J. Won et al. / Autonomous Robot Vision System for Environment Recognition
Gaborski et al. [8], in which the subtraction between two saliency maps for two consecutive input frames was used to detect a novel scene. In section 1, we describe an object preferred selective attention model with a scaleability using a maximum entropy algorithm. In section 2, we explain a novelty detection and environment recognition model that can memorize a visual scan path topology and an energy signature for a dynamic scene. In section 3, we describe the implementation of proposed autonomous robot vision system, and experimental results will be followed. In final section, we summarize our results and mention about a further research.
1. Visual selective attention model The proposed selective attention model is implemented by imitating the functions of the visual cortex from the retina to the LIP (Lateral Intra-Parietal cortex) through the LGN and the visual cortex area [9]. Reward and punishment function of the hypothalamus and associated limbic structures is considered to develop the low level top-down attention model reflecting the reinforcement and inhibition function [6,9]. Figure 1 shows the proposed object preferred selective attention model which consists of bottom-up processing and low level top-down attention model.
Figure 1. Object preferred selective attention model
The simple biological visual pathway for bottom-up processing is from the retina to the visual cortex through the LGN [9]. In our approach, we use the bottom-up saliency map (SM) model that reflects the functions of the retina cells, the LGN and the visual cortex. Since the retina cells can extract edge and intensity information as well as color opponency, we use these factors as the basis features of the bottom SM model. In order to consider the function of the LGN and the ganglian cells, we consider the oncenter and off-surround operation by the Gaussian pyramid image with different scale, and then constructing three feature bases such as intensity, edge, color feature map. These feature maps make a candidate of salient regions by the bottom-up processing. In order to reflect the human-like attention that has prefer selection to an object, we consider a symmetry operator because an arbitrary object contains a symmetry
W.-J. Won et al. / Autonomous Robot Vision System for Environment Recognition
59
information, which is realized by the noise tolerant generalized symmetry transformation (NTGST) algorithm based on edge information [10]. After obtaining the candidate of selective attention regions, we modify the scan path to pay attention on an object by considering the degree of symmetry. Then, we can construct a saliency map which has more preference on an object [7]. Then, we adapt an entropy maximization approach to select the proper scale of the salient areas by using the bottom-up SM model which has prefer attention to an object. The scale selection algorithm is based on Kadir’s approach [11]. For each salient location, the proposed model chooses those scales at which the entropy is its maximum or peaked, then it weights the entropy value at such scales by some measure of the self-dissimilarity in the scale space of the bottom-up SM model. The most appropriate scale for each salient area centered at location x is obtained by equation
scale ( x )
arg m ax{ H D ( s , x ) u W D ( s , x )}
(1)
s
where D is the set of all descriptor values, H D ( s, x) is the entropy defined by equation (2) and WD ( s, x) is the inter-scale measure define by equation (3)
H D ( s , x ) ¦ p d , s , x log 2 p d , s , x
(2)
d D
W D (s, x)
s2 2s 1
¦
p d , s ,x p d , s 1,x
(3)
dD
where pd , s , x is the probability mass function for scale s, position x, and descriptor value d, which takes on value in D. Although the proposed bottom-up the SM model is generated only by primitive features such as intensity, edge, color and symmetry information. In order to develop a more plausible selective attention model, we need to develop an interactive procedure with a human supervisor, together with bottom-up information processing. Human beings ignore uninteresting area, even if they have primitive salient features, and they can memorize the characteristics of the unwanted area, Humans do not pay attention to new areas that have characteristic similar to learned unwanted area. In addition, human perception can focus on an interesting area, even if it does not have primitive salient features, or if it is less salient than other areas. We propose a now selective attention model that mimics the human-like selective attention mechanism and that can consider not only primitive input features, but also interactive properties in the environment as reflecting limbic structures including the hypothalamus function in our brain [9]. We use a fuzzy ART network “learns” and “memorizes” the characteristics of uninteresting and/or interesting areas decided by a human supervisor [6]. After the successful training of the fuzzy ART network, an unwanted salient area is inhibited and a desired area is reinforced by the vigilance value U of the fuzzy ART network [6,12]. As shown in figure 1, inputs for the fuzzy ART network can be composed of intensity, edge and color feature maps within a salient area. However, the sizes of the selected salient areas can be different, and thus we need to consider how we can adapt the input dimension of the fuzzy ART network corresponding to the various sizes of the salient regions. In this paper, we use a log-polar transformation to generate a fixed size of input dimension for the fuzzy ART network for various sizes of salient areas in the selective attention model. It means that one of the quantized areas by log-polar transformation corresponds to one of input neurons in the fuzzy ART network. The proposed model can generate a fixed uniform scale for every salient area with different
60
W.-J. Won et al. / Autonomous Robot Vision System for Environment Recognition
scales, of which the proposed fuzzy ART model makes to train the arbitrary scaled feature maps for the selected area in order to reinforce and inhibit a salient region.
2. Environment recognition based on novelty detection In order to develop a low level novelty scene detection model, we developed a robust scene description model with a tolerance against noise and a slight affine transformation such as translation, rotation, zoom-in and zoom-out in a dynamic environment. Such a model can be used to detect novelty by comparing the description of the current scene with that of an experienced scene, like the assumable function of the hippocampus in our brain. In the proposed model, we developed a relative topology matching method by using visual scan paths obtained from a saliency map, which were generated by the low level top-down attention model, together with the bottom-up saliency map (SM) model. We can describe an input scene robustly by using the visual scan path based on the selective attention model. Moreover, the size information of every salient area is obtained by the entropy maximization method, which is used to represent a scan path topology of an input scene. In addition, the energy signatures obtained from the SM are used as an additional factor to decide whether a novelty is occurred in the input scene [13]. The obtained scan path topology and energy signature are used as the input of the developed incremental computation model, which can incrementally memorize scene information that is experienced at a new place or at different time for the same place. Moreover, the proposed model is able to recognize the placement and the direction of a field of view from memorized scene information in the incremental computation model. Figure 2 shows the proposed incremental computation model of which the input consists of geometry topology of locations of the salient areas, the scale information of the salient areas and the relative energy signature, which are defined as a relative distance score f d , a relative scale score f s , and a energy score f e , respectively. If the model is executed for the first time, the incremental computation model needs to be initiated with only one output neuron. When the input of the incremental computation model is prepared, the proposed model obtains the location information pi of the camera system and the direction information d j of the field of view of the camera system, by feedback information such as encoder sensors. If the location information is new, the incremental computation model generates a new output node for that location and direction. After getting the location and direction information, the proposed model selects a winner node from among the nodes related with the dedicated location and direction. Then, the model checks the degree of similarity using a vigilance value U ' . If the degree of similarity between the current input scene and the winner node is less than the vigilance value U ' , then the proposed model indicates it is a novelty scene and creates a new node, otherwise the current input scene is considered as previously seen and the proposed model adjusts the weights of the winner node according to the input scene through the unsupervised learning. The developed model can recognize the location of its placement by using the trained incremental computation model for scene memorization and scene novelty detection, as described
W.-J. Won et al. / Autonomous Robot Vision System for Environment Recognition
61
above. The proposed placement recognition algorithm uses the same procedure that makes an input vector ( f d , f s , f e ) for the incremental computation model.
Figure 2. Incremental model for recognizing an environment
Assume that the training for novelty scene detection is successfully finished using the proposed incremental computation model. When the model is placed in a room without a priori information, the proposed computation model autonomously selects a winner node from among all of the output nodes according to the input vector ( f d , f s , f e ) . Then, the proposed computation model can get location and direction information as well as the instance that the model has been seen the scene from the indices pi and d j , respectively, of the selected winner node. Finally, the proposed model can indicate its placement from the experienced scene memory, whenever it is placed at some place.
3.
Hardware implementation and experimental results
We implement a pan-title unit for autonomous robot vision system for recognizing environment. Figure 3 shows implemented system called by SMART-v1 (Self Motivated Artificial Robot with Trainable selective attention model version 1).
Figure 3. SMART-v1 platform
62
W.-J. Won et al. / Autonomous Robot Vision System for Environment Recognition
The SMART-v1 has four DOF and 1394 CCD camera and Text to Speech module (TTS) to communicate with human to inquire about an attentive object, and title sensor to set offset position before starting moving. We use the Atmega128 as the motor controller and zigbee to transmit motor command from a PC to SMART-v1. The SMART-v1 can continuously search an interesting region by the selective attention model which is explained in section 2. Also, it can detect a novelty in dynamic situation as well as environment recognition. Followings are one of operating mode to recognize an environment (1) We set the SMART-v1 in arbitrary place. The SMART-v1 does not have any a priori knowledge for environment. (2) Then, the visual selective attention model operates for the first scene. After deciding the salient regions, it asks whether the selected region interests to human supervisor. After getting an answer about the selected region, the fuzzy ART network learns the region as inhibition and reinforcement region according to supervisor’s decision. (3) The SMART-v1 memorizes the location and direction information of the selected regions by the explained the environment recognition model using the motor encoder signals. (4) If the SMART-v1 detects a novelty in a scene, then it asks the human what it is. Then human gives a labeling, which consists of the geometry topology of locations of the salient areas, the scale information of the salient areas and the relative energy signature, for the selected region. The SMART-v1 simply memorizes the characteristics of the selected region by color histogram and topology of orientation information. (5) After training the environment, we can ask environment information to the SMART-v1 where a specific object is. Then, the SMART-v1 can reply the direction and location information based on past experience. Figure 4 show the experimental results of the proposed selective attention model. Figure 4 (a) is the result of bottom-up SM model, Figure 4 (b) is the result after inhibition for the second attention region in Figure 4 (a). After considering the symmetry information, we can ignore the 5th attentive region because it does not contain symmetry information so much. Finally, we can change the scan path to pay attention a left computer monitor rather than a red cushion as most salient region using the reinforcement fuzzy ART network as shown in Figure 4 (d).
(a)
(b)
(c)
(d)
Figure 4. The result of selective attention model : (a) bottom-up attention point, (b)inhibit for 2nd attention region in (a), (c) ignore for 5th attention region in (b), (d)reinforcement for 2nd attention region in (c)
Figure 5 shows that it is hard to indicate a novel scene by simply subtracting the saliency maps in a dynamic environment, in which the obtained input scenes usually
63
W.-J. Won et al. / Autonomous Robot Vision System for Environment Recognition
have different affine transformed field of view each other. Therefore, Gaborski’s [8] approach can not apply to detecting novel scenes in the dynamic environment. Figure 6 shows that the proposed model can successfully detect a novelty scene in a dynamic environment. Figure 6 (c) has novelty information, while figures 6 (a) and (b) do not have novelty information, except for a slight change in the field of view. Moreover, figures 6 (d) and (e) show that the proposed model can get similar energy signatures in two corresponding SMs. Table 1 shows the performance of the proposed novelty scene detection model using total 61 scenes which consist of 44 novelty scenes and 17 nonovelty scenes at three directions in the interior environment.
(a)
(b)
(c)
(d)
(e)
Figure 5. Scan path topologies and saliency maps for two different time scenes for the assumed same scene that has a little affine transformed field of view: (a) and (b) scan path topologies at time t and t + dt, respectively, (c) and (d) saliency maps at time t and t+dt, respectively, (e) subtraction between (c) and (d)
(a)
(b)
(c)
(d)
(e)
(f)
Figure 6. Scan path topologies and saliency maps for three different time scenes for the assumed same scene that has a little affine transformed field of view: (a), (b) and (c) scan path topologies at time t, t + dt and t + 2dt, respectively, (d), (e) and (f) saliency maps at time t, t+dt and t+2dt, respectively.
Table 1. The performance of the proposed novelty scene detection model:61 scenes were considered to evaluate among which 44 scenes were novel scenes and the remainder 17 scenes were no novel scenes. (TP: True Positive, TN: True Negative, FP: False Positive, FN: False Negative)
Novelty perception No-novelty perception Correct Recognition Rate
Novelty scenes (44 scenes) TP 41 FN 3 95.3%
No-novelty scenes (17 scenes) FP 2 TN 15 88.2%
Total (61 scenes) F(P+N) 5 T(P+N) 56 91.8%
64
4.
W.-J. Won et al. / Autonomous Robot Vision System for Environment Recognition
Conclusion
We implemented a novel autonomous robot vision system that can detect a novelty as well as environment recognition. The proposed system is based on human-like selective attention model which consists of bottom-up SM and top-down reinforcement and inhibition models. The variant scale problem in selective attention regions can be solved by introducing the log-polar transformation. The incremental model based on novelty detection successfully recognizes both the environment and change of environment. As a future work, we are investigate in efficient way to describe an attention region including a task-nonspecific object representation Acknowledgment This research was funded by the Brain Neuroinformatics Research Program of the Ministry of Commerce, Industry and Energy. References [1]
C. Breazeal : Imitation as Social Exchange between Humans and Robots, in proceedings of the Symposium on Imitation in Animals and Artifacts (AISB99), Edinburg, Scotland, 1999, 96 - 104. [2] J. Weng et, all: A theory for mentally developing robots, The 2 International conference on Development and Learning on Development and Learning, 2002, 131 - 140. [3] B. Scassellati: Investigating models of social development using a humanoid robot, in proceedings of International Joint Conference on Neural Networks, 2003, Portland, USA. 2704 - 2709. [4] S. Singh and M. Markou: An approach to novelty detection applied to the classification of image regions, IEEE Trans. On Knowledge and Data Eng, 16 (2004), 396-407. [5] L. Itti, C. Koch, E. Niebur: A model of saliency-based visual attention for rapid scene analysis, IEEE Trans PATT Mach Intell ,20 (1998), no. 11, 1254 -1259. [6] S. B. Choi, S. W. Ban, M Lee: Human-Like selective Attention Model with Reinforcement and Inhibition Mechanism, in proceedings of the 11th International Conference on Neural Information Processing, 2004, Calcutta, India, 694- 699. [7] W. J. Won, S. W. Ban, M. Lee: Real Time Implementation of a selective attention model for the intelligent robot with autonomous mental development, in proceeding of International Symposium on Industrial Electronics 2005, Dubrovnik, Croatia, D7-D5. [8] R.S. Gaborski, V.S. Vanigankar, V.S. Chaoji and A.M. Teredesai, “VENUS: A system for novelty detection in video streams with learning,” in proceedings of AAAI 2004, San Jose, USA, 2004, pp. 1-5. [9] E.B GoldStein (Ed.), Sensation and perception 4th ed., USA: International Thomson Publishing Company, 1996. [10] K. S. Seo, C. J. Park, C. J. Cho and H. M. Choi: Context-free maker-controlled watershed transform for efficient multi-object detection and segmentation, IEICE Trans, 384-A (2001), no.6, 1392-1400. [11] T. Kadir and M. Brady: Scale, saliency and image description, International Journal of Computer Vision, 45 (2001), 83 -105. [12] G.A Carpenter, S. Grossberg, N. Markuzon, J.H. Reynolds and D.B. Rosen: Fuzzy ARTMAP: A neural network architecture for incremental supervised learning of analog multidimensional maps, IEEE Trans. on Neural Networks, 3 (1992), No.5, 698-713. [13] S.-W. Ban and M. Lee: Novelty analysis in dynamic scene for autonomous mental development, Lecture notes in Computer Science, 3696 (2005), 1-6.
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
65
Multi-resolution Field D* Dave Ferguson and Anthony Stentz The Robotics Institute, Carnegie Mellon University, Pittsburgh, PA {dif, tony}@cmu.edu Abstract. We present a multi-resolution path planner and replanner capable of efficiently generating paths across very large environments. Our approach extends recent work on interpolation-based planning to produce direct paths through non-uniform resolution grids. The resulting algorithm produces plans with costs almost exactly the same as those generated by the most effective uniform resolution grid-based approaches, while requiring only a fraction of their computation time and memory. In this paper we describe the algorithm and report results from a number of experiments involving both simulated and real field data.
1. Introduction A common task in mobile robot navigation is to plan a path from an initial location to a desired goal location. Typically, this is done by storing the information about the environment in a discrete traversability cost map, where the cost of traversing each area depends on any number of factors, including safety risk, traversal time, or fuel expended, and then planning over this cost map. In robotics, it is common to improve the efficiency of planning by approximating the cost map as a graph. For instance, given a cost map consisting of a 2D grid of cells of uniform size (or resolution), a graph is typically constructed by placing nodes at the centers or corners of the grid cells and inserting edges between each node and the nodes in its 8 adjacent cells. Several algorithms exist for planning paths over such graphs. Dijkstra’s algorithm and A* are useful for computing initial paths to the goal from every location or a single location, respectively [2,6]. D* and D* Lite are very effective at repairing these paths when new information about the environment is received [9,5]. Combining uniform resolution grids with these incremental planning algorithms has been a very popular approach for mobile robot navigation in partially-known or dynamic environments. However, such an approach has two significant limitations. Firstly, the small, discrete set of transitions modeled by the graph can reduce the quality of the resulting paths. In the case of a uniform resolution grid, each node in the extracted graph only has edges connecting it to its 8 closest neighboring nodes. This limits the heading of any solution path to increments of π4 . This can lead to suboptimal paths and undesirable behavior [3]. Secondly, uniform resolution grids can be very memory-intensive. This is because the entire environment must be represented at the highest resolution for
66
D. Ferguson and A. Stentz / Multi-Resolution Field D*
Figure 1. (left, center ) Multi-resolution Field D* produces direct, low-cost paths (in blue/dark gray) through both high-resolution and low-resolution areas. (right) The GDRS XUV robot used for autonomous navigation of outdoor terrain. Multi-resolution Field D* was initially developed in order to extend the effective range of rugged outdoor vehicles such as this by one to two orders of magnitude.
which information is available. For instance, consider a robot navigating a large outdoor environment with a prior overhead map. The initial information contained in this map may be coarse. However, the robot may be equipped with onboard sensors that provide very accurate information about the area within some field of view of the robot. Using a uniform resolution grid-based approach, if any of the high-resolution information obtained from the robot’s onboard sensors is to be used for planning, then the entire environment needs to be represented at a high-resolution, including the areas for which only the low-resolution prior map information is available. Storing and planning over this representation can require vast amounts of memory. To overcome the first of these limitations, improvements to uniform resolution grid-based planners have been developed that use interpolation to produce paths not constrained to a small set of headings [7,3]. In particular, the Field D* algorithm extends the D* family of algorithms by using linear interpolation to derive the path cost of points not sampled in the grid [3]. This algorithm efficiently produces very low-cost paths with a range of continuous headings. A number of techniques have been devised to address the second limitation. One popular approach is to use quadtrees rather than uniform resolution grids [8]. Quadtrees offer a compact representation by allowing large constant-cost regions of the environment to be modeled as single cells. They thus represent the environment using grids containing cells of varying sizes, known as non-uniform resolution grids or multi-resolution grids. However, paths produced using quadtrees and traditional quadtree planning algorithms are again constrained to transitioning between the centers or corners of adjacent cells and can thus be grossly suboptimal. More recently, framed quadtrees have been used to alleviate this problem somewhat [1,10]. Framed quadtrees add cells of the highest resolution around the boundary of each quadtree region and allow transitions between these boundary cells through the quadtree region. As a result, the paths produced can be much less costly, but the computation and memory required can be large due to the overhead of the representation (and in pathological cases can become significantly more than is required by a full high-resolution representation). Also, segments of the path between adjacent
D. Ferguson and A. Stentz / Multi-Resolution Field D*
67
high-resolution cells suffer the same π4 heading increment restrictions as classical uniform resolution grid approaches. In this paper, we present an approach that combines the ideas of interpolation and non-uniform resolution grid representations to address both of the limitations mentioned above. The resulting approach provides very cost-effective paths for a fraction of the memory and, often, for a fraction of the time required by uniform resolution grid-based approaches. We begin by describing how linear interpolation can be used to provide direct, low-cost paths through non-uniform resolution grids. We then present our new algorithm, Multi-resolution Field D*, along with a number of example illustrations and results comparing it to its uniform resolution predecessor.
2. Interpolation-based Path Planning Given a grid with nodes at the corners of every grid cell, classic grid-based planners restrict the available actions from each node to be direct paths to neighboring nodes. This restriction affects the quality and effectiveness of grid-based paths, as mentioned above. 2.1. Using interpolation to produce more accurate path cost estimates We can overcome the limitations of classical grid-based planning if we allow each node to transition to any point on any neighboring edge, rather than to just the small set of neighboring nodes. We define the neighboring edges of a node s to be all edges that can be reached from s via a straight-line path for which s is not an endpoint (see Figure 2 (left)). The rationale here is that the optimal path from s to the goal must pass through one of these neighboring edges, so if we knew the optimal paths from every point on any of these edges to the goal we could compute the optimal path for s. If E is the set of neighboring edges of s and PE is the set of all points on these edges, then the cost of the optimal path for s is g ∗ (s) = min (c(s, p) + g ∗ (p)), p∈PE
(1)
where c(s, p) is the cost of the cheapest path from s to p that does not travel through any other neighboring edges of s, and g ∗ (p) is the cost of the optimal path from point p to the goal. Unfortunately, there are an infinite number of points p on any one of the neighboring edges of s, so solving for the least-cost paths from each of these points is impossible. Fortunately, a method was presented in [3] that uses linear interpolation to approximate the path cost of any point on an edge using just the path costs of the two endpoints of the edge. In that work, this approximation was used to provide an efficient closed form solution to the above path cost calculation for any given corner node s in a uniform resolution grid. Using this method, very accurate paths through uniform resolution grids can be efficiently computed.
68
D. Ferguson and A. Stentz / Multi-Resolution Field D*
(a)
(b)
(c)
(d)
Figure 2. (left) The neighboring edges (dashed in red/gray, along with their endpoints in gray) from a given node (white) in a grid containing cells with two different resolutions: low-resolution and high-resolution. (a, b) Some of the possible paths to a neighboring edge/node from a low-resolution node. On the left are the possible optimal path types (in blue/dark gray) through the top low-resolution edge (dashed in red/gray) and its endpoint nodes (in gray). Linear interpolation is used to compute the path cost of any point along the top edge. On the right are the possible optimal path types (in blue/dark gray) to one neighboring high-resolution corner node (in gray). (c, d) Some of the possible paths (in blue/dark gray) from a high-resolution corner node (white) to a neighboring low-resolution edge (c) and to a high-resolution node (d, left) and edge (d, right).
2.2. Combining interpolation with multiple resolutions We can use the same basic idea to provide accurate path costs for nodes in non-uniform resolution grids. The main difference is that, while in the uniform resolution case each node s has exactly 8 neighboring edges of uniform length, in the non-uniform resolution case a node may have many more neighboring edges with widely-varying lengths. However, linear interpolation can still be used to approximate the path costs of points along these edges, exactly as in the uniform resolution case. As a concrete example of how we compute the path cost of a node in a multi-resolution grid, we now focus our attention on a grid containing cells of two different resolutions: high-resolution and low-resolution. This two-resolution case addresses the most common navigation scenario we are confronted with: a lowresolution prior map is available and the robot is equipped with high-resolution onboard sensors. Although we restrict our attention to this scenario, the approach is general and can be used with arbitrarily many different resolutions. In a grid containing two different resolutions, each node can reside on the corner of a low-resolution cell, the corner of a high-resolution cell, and/or the edge of a low-resolution cell. Examples of each of these possibilities can be seen in Figure 2(b): the white node is the corner of a low-resolution cell and the gray node is the corner of a high-resolution cell and on the edge of a low-resolution cell. Let’s look at each of these possibilities in turn. Firstly, imagine we have a node that resides on the corner of a low-resolution cell. We can calculate the least-cost path from the node through this cell by
D. Ferguson and A. Stentz / Multi-Resolution Field D*
69
ComputePathCost(s) 1 vs = ∞; 2 for each cell x upon which s resides 3 if x is a high-resolution cell 4 for each neighboring edge e of s that is on the boundary of x 5 vs = min(vs , minp∈Pe (c(s, p) + g i (p))); 6 else 7 for each neighboring edge e of s that is on the boundary of x 8 if e is a low-resolution edge 9 vs = min(vs , minp∈Pe (c(s, p) + g i (p))); 10 else 11 vs = min(vs , minp∈EPe (c(s, p) + g(p))); 12 return vs ;
Figure 3. Computing the Path Cost of a Node s in a Grid with Two Resolutions
looking at all the points on the boundary of this cell and computing the minimumcost path from the node through any of these points. We can approximate the cost of this path by using linear interpolation to provide the path cost of arbitrary boundary points, exactly as in uniform resolution Field D*. However, some of the boundary may be comprised of high-resolution nodes. In such a case, we can either use interpolation between adjacent high-resolution nodes and allow the path to transition to any point on an adjacent high-resolution edge, or we can restrict the path to transitioning to one of the high-resolution nodes. The former method provides more accurate approximations, but it is slightly more complicated and less efficient. Depending on the relative sizes of the high-resolution cells and the low-resolution cells, either of these approaches may be appropriate. For instance, if the high-resolution cells are much smaller than the low-resolution cells, then interpolating across the adjacent high-resolution edges when computing the path from a low-resolution node is not critical, as there will be a wide range of heading angles available just from direct paths to the adjacent high-resolution nodes. However, if the high-resolution cells are not significantly smaller than the lowresolution cells then this interpolation becomes important, as it allows much more freedom in the range of headings available to low-resolution nodes adjacent to high-resolution nodes. In Figure 2 we illustrate the latter, simpler approach, where interpolation is used to compute the path cost of points on neighboring, strictly low-resolution edges (e.g. the top edge in (a)), and paths are computed to each neighboring high-resolution node (e.g. the gray node on the right edge in (b)). For nodes that reside on the corner of a high-resolution cell, we can use interpolation to approximate the cost of the cheapest path through the highresolution cell (see the paths to the right edge in (d)). Finally, for nodes that reside on the edge of a low-resolution cell, we can use a similar approach as in our low-resolution corner case. Again, we look at the boundary of the low-resolution cell and use interpolation to compute the cost of points on strictly low-resolution edges (e.g. the top edge in (d)), and for each high-resolution edge we can choose between using interpolation to compute the cost of points along the edge, or restricting the path to travel through one of the endpoints of the edge. The latter approach is illustrated for computing a path through the left edge in (d).
70
D. Ferguson and A. Stentz / Multi-Resolution Field D*
key(s) 1 return [min(g(s), rhs(s)) + h(sstart , s); min(g(s), rhs(s))]; UpdateNode(s) 2 if s was not visited before, g(s) = ∞; 3 if (s = sgoal ) 4 rhs(s) = ComputePathCost(s); 5 if (s ∈ OPEN) remove s from OPEN; 6 if (g(s) = rhs(s)) insert s into OPEN with key(s); ComputeShortestPath() ˙ key(sstart ) OR rhs(sstart ) = g(sstart )) 7 while (mins∈OPEN (key(s))< 8 remove node s with the minimum key from OPEN; 9 if (g(s) > rhs(s)) 10 g(s) = rhs(s); 11 for all s ∈ nbrs(s) UpdateNode(s ); 12 else 13 g(s) = ∞; 14 for all s ∈ nbrs(s) ∪ {s} UpdateNode(s ); Main() 15 g(sstart ) = rhs(sstart ) = ∞; g(sgoal ) = ∞; 16 rhs(sgoal ) = 0; OPEN = ∅; 17 insert sgoal into OPEN with key(sgoal ); 18 forever 19 ComputeShortestPath(); 20 Wait for changes to grid or traversal costs; 21 for all new cells or cells with new traversal costs x 22 for each node s on an edge or corner of x 23 UpdateNode(s);
Figure 4. The Multi-resolution Field D* Algorithm (basic version)
Thus, for each node, we look at all the cells that it resides upon as either a corner or along an edge and compute the least-cost path through each of these cells using the above approximation technique. We then take the cheapest of all of these paths and use its cost as the path cost of the node. Pseudocode of this technique is presented in Figure 3. In this figure, Pe is the (infinite) set of all points on edge e, EPe is a set containing the two endpoints of edge e, g i (p) is an approximation of the path cost of point p (calculated through using linear interpolation between the endpoints of the edge p resides on), c(s, p) is the cost of a minimum-cost path from s to p, and g(p) is the current path cost of corner point p. We say an edge e is a ‘low-resolution edge’ (line 8) if both the cells on either side of e are low-resolution. An efficient solution to the minimizations in lines 5 and 9 was presented in [3].
D. Ferguson and A. Stentz / Multi-Resolution Field D*
71
Figure 5. Multi-resolution Field D* used to guide an agent through a partially-known environment. On the left is a section of the path already traversed showing the high-resolution cells. This data was taken from Fort Indiantown Gap, Pennsylvania.
3. Multi-resolution Field D* The path cost calculation discussed above enables us to plan direct paths through non-uniform resolution grids. We can couple this with any standard path planning algorithm, such as Dijkstra’s, A*, or D*. Because our motivation for this work was robotic path planning in unknown or partially-known environments, we have used it to develop an incremental replanning algorithm which we call Multi-resolution Field D*. A basic version of the algorithm is presented in Figure 4. Here, the ComputeMinPathCost function (line 4) takes a node s and computes the path cost for s using the path costs of all of its neighboring nodes and interpolation across its neighboring edges. Apart from this, notation follows the D* Lite algorithm: g(s) is the current path cost of node s, rhs(s) is the one-step lookahead path cost for s, OPEN is a priority queue containing inconsistent nodes (i.e., nodes s for which g(s) = rhs(s)) in increasing order of key values (line 1), sstart is the initial agent node, and sgoal is the goal node. h(sstart , s) is a heuristic estimate of the cost of a path from sstart to s. Because the key value of each node contains two ˙ key(s ) iff the first quantities a lexicographic ordering is used, where key(s) < element of key(s) is less than the first element of key(s ) or the first element of key(s) equals the first element of key(s ) and the second element of key(s) is less than the second element of key(s ). For more details on the D* Lite algorithm and this terminology, see [5]. As with other members of the D* family of algorithms, significant optimizations can be made to this initial algorithm. In particular, several of the optimizations presented in [4] are applicable and were used in our implementation. Because of its use of interpolation, Multi-resolution Field D* produces extremely direct, cost-effective paths through non-uniform resolution grids. Figures 1 and 5 show example paths planned using the algorithm.
72
D. Ferguson and A. Stentz / Multi-Resolution Field D*
Figure 6. Computation time and solution cost as a function of how much of the environment is represented at a high-resolution. The x-axis of each graph depicts the percentage of the map modeled using high-resolution cells, ranging from 0 (all modeled at a low-resolution) to 100 (all modeled at a high-resolution). (top) Initial planning. A path was planned from one side to the other of 100 randomly generated environments and the results were averaged. (bottom) Replanning. 5% of each environment was randomly altered and the initial paths were repaired.
4. Results Multi-resolution Field D* was originally developed in order to extend the effective range over which unmanned ground vehicles (such as the XUV vehicle in Figure 1) could operate by orders of magnitude. We have found the algorithm to be extremely effective at reducing both the memory and computational requirements of planning over large distances. To quantify its performance, we ran several experiments comparing Multi-resolution Field D* to uniform resolution Field D*. We used uniform resolution Field D* for comparison because it produces less costly paths than regular uniform grid-based planners and far better paths than regular non-uniform grid-based planners. Our first set of experiments investigated both the quality of the solutions and the computation time required to produce these solutions as a function of the memory requirements of Multi-resolution Field D*. We began with a randomly generated 100 × 100 low-resolution environment with an agent at one side and a goal at the other. We then took some percent p of the low-resolution cells (centered around the agent) and split each into a 10 × 10 block of high-resolution cells. We varied the value of p from 0 up to 100. We then planned an initial path to the goal. Next, we randomly changed 5% of the cells around the agent and replanned a path to the goal. We focussed the change around the robot to simulate new information being gathered in its vicinity. The results from these experiments are presented in Figure 6. The x-axis of each graph represents how much of the environment was represented at a high-resolution. The left graphs show how the time required for planning changes as the percent of high-resolution cells increases, while the middle and right graphs show how the path cost changes. The y-values in the middle and right graphs are path costs relative to the path cost computed when 100% of the environment is represented at a high-resolution. The right
73
D. Ferguson and A. Stentz / Multi-Resolution Field D*
Total Planning and Replanning Time (s) Initial Planning Time (s) Average Replanning Time (s) Percent high-resolution cells
Field D*
Multi-res Field D*
0.493
0.271
0.336
0.005
0.0007
0.0012
100
13
Figure 7. Results for uniform resolution Field D* versus Multi-resolution Field D* on a simulated robot traverse through real data acquired at Fort Indiantown Gap. The robot began with a low-resolution map of the area and updated this map with a high-resolution onboard sensor as it traversed the environment. The map was 350 × 320 meters in size.
graph shows the standard error associated with the relative path costs for smaller percentages of high-resolution cells. As can be seen from these results, modeling the environment as mostly low-resolution produces paths that are only trivially more expensive than those produced using a full high-resolution representation, for a small fraction of the memory and computational requirements. This first experiment shows the advantage of Multi-resolution Field D* as we reduce the percentage of high-resolution cells in our representation. However, because there is some overhead in the multi-resolution implementation, we also ran uniform resolution Field D* over a uniform, high-resolution grid to compare the runtime of this algorithm with our Multi-resolution version. The results of uniform resolution Field D* have been overlaid on our runtime graphs. Although uniform resolution Field D* is more efficient than Multi-resolution Field D* when 100% of the grid is composed of high-resolution cells, it is far less efficient than Multi-resolution Field D* when less of the grid is made up of high-resolution cells. Our second experiment simulated the most common case for outdoor mobile robot navigation, where we have an agent with some low-resolution prior map and high-resolution onboard sensors. For this experiment, we took real data collected from Fort Indiantown Gap, Pennsylvania, and simulated a robotic traverse from one side of this 350 × 320 meter environment to the other. We blurred the data to create a low-resolution prior map (at 10 × 10 meter accuracy) that the robot updated with a simulated medium-resolution sensor (at 1×1 meter accuracy with a 10 meter range) as it traversed the environment. The results from this experiment are shown in Figure 7. Again, Multiresolution Field D* requires only a fraction of the memory of uniform resolution Field D*, and its runtime is very competitive. In fact, it is only in the replanning portion of this final experiment that Multi-resolution Field D* requires more computation time than uniform resolution Field D*, and this is only because the overhead of converting part of its map representation from low-resolution to highresolution overshadows the trivial amount of processing required for replanning. In general, our results have shown that Multi-resolution Field D* produces paths that are trivially more expensive than those returned by uniform resolution Field D*, and it provides these paths for a fraction of the memory. Further, in many cases it is also significantly more computationally efficient than uniform resolution Field D*.
74
D. Ferguson and A. Stentz / Multi-Resolution Field D*
5. Conclusions We have presented Multi-resolution Field D*, an interpolation-based path planning algorithm able to plan direct, low-cost paths through non-uniform resolution grids. This algorithm addresses two of the most significant shortcomings of grid-based path planning: solution quality and memory requirements. By using a non-uniform resolution grid to represent the environment and linear interpolation to approximate the path costs of points not sampled on the grid, Multi-resolution Field D* is able to provide solutions of comparable quality to the most effective uniform resolution grid-based algorithms for a fraction of their memory and computational requirements. It is our belief that, by combining interpolation with non-uniform representations of the environment, we can ‘have our cake and eat it too’, with a planner that is extremely efficient in terms of both memory and computational requirements while still producing high quality paths. 6. Acknowledgements This work was sponsored in part by the U.S. Army Research Laboratory, under contract “Robotics Collaborative Technology Alliance” (contract number DAAD19-01-2-0012). The views and conclusions contained in this document are those of the authors and should not be interpreted as representing the official policies or endorsements of the U.S. Government. Dave Ferguson is supported in part by a National Science Foundation Graduate Research Fellowship. References [1] D. Chen, R. Szczerba, and J. Uhran. Planning conditional shortest paths through an unknown environment: A framed-quadtree approach. IEEE International Conference on Intelligent Robots and Systems (IROS), 1995. [2] E. Dijkstra. A note on two problems in connexion with graphs. Numerische Mathematik, 1:269–271, 1959. [3] D. Ferguson and A. Stentz. Field D*: An Interpolation-based Path Planner and Replanner. International Symposium on Robotics Research (ISRR), 2005. [4] D. Ferguson and A. Stentz. The Field D* Algorithm for Improved Path Planning and Replanning in Uniform and Non-uniform Cost Environments. Technical Report CMU-RI-TR-05-19, Carnegie Mellon School of Computer Science, 2005. [5] S. Koenig and M. Likhachev. D* Lite. National Conference on Artificial Intelligence (AAAI), 2002. [6] N. Nilsson. Principles of Artificial Intelligence. Tioga Publishing Company, 1980. [7] R. Philippsen and R. Siegwart. An Interpolated Dynamic Navigation Function. IEEE International Conference on Robotics and Automation (ICRA), 2005. [8] H. Samet. Neighbor Finding Techniques for Images Represented by Quadtrees. Computer Graphics and Image Processing, 18:37–57, 1982. [9] Anthony Stentz. The Focussed D* Algorithm for Real-Time Replanning. International Joint Conference on Artificial Intelligence (IJCAI), 1995. [10] A. Yahja, S. Singh, and A. Stentz. An efficient on-line path planner for outdoor mobile robots operating in vast environments. Robotics and Autonomous Systems, 33:129–143, 2000.
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
75
Path and Observation Planning of Vision-based Mobile Robots with Multiple Sensing Strategies1 Mitsuaki Kayawake a,2 , Atsushi Yamashita a and Toru Kaneko a a Department of Mechanical Engineering Shizuoka University Abstract. In this paper, we propose a new path and viewpoint planning method for a mobile robot with multiple observation strategies. When a mobile robot works in the constructed environments such as indoor, it is very effective and reasonable to attach landmarks on the environment for the vision-based navigation. In that case, it is important for the robot to decide its motion automatically. Therefore, we propose a motion planning method that optimizes the efficiency of the task, the danger of colliding with obstacles, and the accuracy and the ease of the observation according to the situation and the performance of the robots. Keywords. Path planning, Viewpoint planning, Mobile robot, Landmark
1. Introduction In this paper, we propose a new path and viewpoint planning method for a mobile robot that has two active cameras according to the performance of the robot. The robot navigation is a very important technology to execute various tasks. The navigation is usually executed while the robots move and estimate their positions and orientations by using the information from several sensors. A dead-reckoning is a method that can estimate the robot position and orientation with internal sensor. However the error of it is accumulated in proportion to the traveling distance and the recovery of the error is impossible only with internal sensors. Therefore, external sensors are always utilized for the robot navigation. The robot can observe landmarks in the environment and measure the relationship between these points and the robot itself in the image-based navigation. When the robots observe them, the problems are how to measure the accurate position and orientation of landmarks, and where and which landmarks the robots should observe while there are multiple landmarks. As to the former problem, there are a lot of studies that improve the accuracy of 3-D measurement, i.e., [1]. However, there is a limit in accuracy when the robot always observes the same landmark regardless of the distance between the robot and the landmark. 1 Tel.:
81 53 478 1604; Fax: 81 53 478 1604;
[email protected].
2 E-mail:
76
M. Kayawake et al. / Path and Observation Planning of Vision-Based Mobile Robots
Therefore, the latter problem is very important for the robot navigation. This means that the robot must decide the path and choose the observing landmarks according to its position while it moves[2]. Navigation planning methods under the uncertainty are also proposed[3]-[6]. On the other hand, the design of the optimal arrangement of artificial landmark is very important in the indoor environment[7]. Takeuchi et al. proposed a method to dispose artificial landmarks in the environment, and to navigate a mobile robot there[8]. The landmarks are disposed so that the robot can observe at least one landmark at any positions, and the robot plans the observation so that they can observe landmarks.
2. Purpose We have already proposed the landmark observation method with multiple strategies[9]. In [9], the robot used the C-obstacle (configuration obstacle) to escape the danger of the collision with the obstacle, and the only width of the expansion of C-obstacle was decided. However, when the given environment is complex, deciding the expanding width becomes difficult. If the expanding width of C-obstacle was unsuitable, the robot was not able to reach the destination, or it had to travel unnecessary long moving distance. Therefore, the robot had to look for appropriate expanding width of C-obstacle. Then, the robot in this work searches for the path of the expanding width of two or more Cobstacles, and discovers the path in appropriate width among that. In this method, a primary value of the expanding width of C-obstacle is given to the robot, and the robot continue to increase constant width about the expanding width of C-obstacle. The robot searches for the path in the expanding width of each C-obstacle. When the path that can be connected with the destination is not found, the robot ends the search for the path. Using observation technique of [9], we propose the improvement technique for path planning. We make the shape of C-obstacle better. In [9], the vicinity of the vertex of Cobstacle has been expanded more widely than original expanding width. Therefore, there was a problem to which it was not able to pass in the path that should be able to pass. In this work, the corner of C-obstacle is expressed by the polygon. The expanding width in the vicinity of the vertex approximates to original expanding width by this method, and solves problem of [9]. In [9], the n-th shortest paths were searched in each visibility graph by a simple brute force method. In our path planning, the path is planned by Dijkstra algorithm. Even if the combination of all the vertices is not confirmed, this technique can discover the shortest path. As the result, the amount of the path searching is decreased. Optimal path and viewpoint planning is necessary for the robot to execute works efficiently. At the same time, “optimal” path and viewpoints change according to the performance of the robot. Some previous works can plan the optimal paths and viewpoints of the robot [10]. However, they don’t consider the relationship between optimal planning results and the performance of the robot explicitly. For the robot with good performance, path can be planned by giving priority to moved distance more than safety. On the other hand, the robot with bad performance will be able surely to reach the goal the plan of path to which it gives priority to safety. Therefore, “optimal” path and viewpoints depend on the performance of the robot. There are multiple evaluation methods such as high accuracy, path length, and safety. Evaluation methods also change when the performance changes. Therefore, we propose a new path and viewpoint planning method for a mobile
M. Kayawake et al. / Path and Observation Planning of Vision-Based Mobile Robots
77
Environmental information
Setting primary error tolerance Replan
Shortest path planning Observation planning
Possible to reach the destination ? Yes
No
Alternative path planning Repeat
Observation planning
All vertexes of C-obstacles are selected
No Yes
Changing in error tolerance Can the path connected from the start to the goal be planned?
Yes
No
Selection of the shortest path that can reach Motion planning
Figure 1. Path and observation planning.
robot with multiple observation strategies according to the performance of the robot. We verify the effectiveness of this method through simulations. The accuracy of multiple observation strategies depends on the positions of landmarks that can be observed. Therefore, the robot must choose the optimal landmarkobservation strategy to consider the number and the configuration of visible landmarks in each place. The path and viewpoints are planned by considering not only the accuracy of the optimal observation strategy but also the dead-reckoning error of the robot also plans the path and viewpoints. Our motion planning method is designed to guarantee that the robot never collides with obstacle. The robot selects the shortest path from the safe paths. After deciding the shortest path, the number of viewpoints is minimized. This is because it is desirable that the cameras equipped with the robot are utilized for the other uses of the landmarkobservation. The motion planning begins from the input of environmental information including the landmark position, the start position, the goal position, and the extra information to the robot (Figure 1). The shortest path and the viewpoint are planned from input environmental information. As for shortest path, it is distinguished whether to reach the goal without colliding with the obstacle by the observation plan. When it is possible to reach goal position, error tolerance is changed and it plans from the shortest path plan again. When it is not possible to reach the goal, paths other than shortest path are planned and
78
M. Kayawake et al. / Path and Observation Planning of Vision-Based Mobile Robots
Landmark 1
Landmark
Landmark 1 Landmark 2 Landmark 3
Landmark 2 (a) Stereo observation.
(b) Two-landmark observation.
(c) Three-landmark observation.
Figure 2. Three observation strategies.
the viewpoint of those is planned. Shortest path that can reach the goal in searched path is assumed to be optimal path.
3. Three Observation Strategies 3.1. Problem Statement We make the assumptions for the planning of the robot navigation. The robot can move in all direction at any time and uses the information from two active cameras that can change their directions independently. The shape of the robot is expressed as the circle whose radius is R. The environment map that indicates the positions of walls, obstacles, and landmarks is also previously provided to the robot. Therefore the map is available in the robot. All landmarks whose heights are same with those of robot’s cameras are attached to the environment. The shape of the landmark is a circle and the radius of each landmark is constant. Each landmark can be distinguished with each other. The detail of each error of the observation is explained in [9]. We develop three observation strategies: (a) stereo observation, (b) two-landmark observation, and (c) three-landmark observation (Figure 2). 3.2. Stereo Observation The stereo observation can be executed when one or more landmarks are inside the robot’s field of view. The robot estimates its position and orientation with the triangulation. In this strategy, the 3-D positions of left and right ends of the observed landmark are measured with the information of disparities. The position and orientation of the robot in the world coordinate can be calculated from the coordinate value of two points. 3.3. Two-Landmark Observation The two-landmark observation can be executed when two or more landmarks are inside the robot’s field of view. Left and right ends of the nearer landmark and a center point of the distant landmark are extracted from two acquired images. This observation calculates only angle information from that image. The position and the orientation of the robot in the world coordinate can be decided as a result. 3.4. Three-Landmark Observation The three-landmark observation can be executed when three or more landmarks are inside the robot’s field of view. The relationship between three landmarks and the robot is estimated from the coordinate value of the center of three landmarks in images.
M. Kayawake et al. / Path and Observation Planning of Vision-Based Mobile Robots Accurate position of landmark
Accurate size of landmark
79
Observed size of landmark
v
u
Observed position of landmark
r
(a) Position error. (b) Size error. Figure 3. Image error.
Next position error from position error
Next viewpoint (ideal)
Next viewpoint (ideal) Robot
Angle error
Next viewpoint (actual)
Position error
Next position error from angle error
Next viewpoint (actual) Robot
(a) Position error. (b) Orientation error. Figure 4. Position and orientation error of robot.
The accuracy of the estimated position and orientation become higher as compared with two-landmark observation method. This is because the distance between the extracted points in images is larger in three-landmark observation than in two-landmark observation. In addition, the image noise at the edge of the landmark is generally larger than that at the center, because the coordinate value of the center position in the image coordinate can be obtained accurately by calculating the center of gravity of the pixels that belong to the landmark. 3.5. Optimal Observation Strategy The robot chooses the optimal landmark-observation strategy that can estimate its position and orientation precisely. The optimal strategy can be decided when the path and the position of the viewpoint is planned. At first, the visible landmarks in each place are selected to consider the robot’s field of view. The robot cannot observe landmarks when obstacles are between the robot and landmarks and cannot observe them from the back side. Incidentally, the error of image such as quantization error always occurs. Then, the theoretical estimation errors of robot’s position and orientation are calculated by considering the situation that the errors of landmark’s position and size (shape) in images occur (Figure 3). We assumed that the position error of the landmark’s center point in the image is (Δu, Δv). The size error of the landmark in the image Δr is also considered. It means that the observed landmark’s position in the image may shift (±Δu, ±Δv) from the true position at the maximum. The observed radius may also shift ±Δr from the true size.
80
M. Kayawake et al. / Path and Observation Planning of Vision-Based Mobile Robots
The robot estimates how the position and orientation errors occur in the world coordinate about all combination of visible landmarks when the errors occur in the images (image coordinates) of two cameras. However, the position error and the orientation error are not compared directly because the dimensions of them are different from each other. The position error is expressed as the dimension of length, i.e., [mm], and the orientation error is expressed as the dimension of angle, i.e., [deg]. Therefore, we transform the orientation error (the dimension of angle) into the position error (the dimension of length). The total sum of the error when the robot moves at a certain distance while the position and orientation error occur is calculated (Figure 4). This means that the total error at the next time’s position of the robot when it moves is the sum of the position error (Epos,max , Figure 4(a)) and the incorrect moving distance under the influence of the orientation error (Eang,max , Figure 4(b)). Epos,max is the distance between the ideal robot position without error and the real position with error derived from position error. Eang,max is the distance between the ideal robot position without error and the real position with error derived from orientation error. Therefore, Epos,max and Eang,max have the same dimension of length. In this way, the estimated error of the robot position in the world coordinate that is caused by the image error is calculated. The optimal observation strategy is equal to the observation method that has minimum position estimation error. The optimal observation strategy is decided as follows: Emax (p, m) = Epos,max (p, m) + Eang,max (p, m) → min,
(1)
where p is the present position, m is the moving distance, and Emax (p, m) is the total error when the robot move distance m from p. The direction of two cameras is decided by selecting the optimal observation strategy in each place. Therefore, the navigation planning of the mobile robot can be executed.
4. Path and viewpoint Planning 4.1. Path Planning The path of the robot is planned based on the visibility graph.In our motion planning method, original obstacles are expanded R + S where S is the margin for safety (Figure 5(a)). In this paper, we call S the error tolerance. When generating C-obstacles, shapes of their vertices are approximated with the polygons because of simplicity of computations. Therefore, their vertices were approximated to intersection T1 , T2 of Li (i = 1, 2, 3). The vertices of C-obstacles are connected with each other and a visibility graph is constructed (Figure 5(b)). In this step, multiple visibility graphs are generated by changing S for optimal planning. This is because it is difficult to find the best S in complex environment beforehand. The robot of bad performance can plan path that safely reaches the goal by changing S(Figure 5(c)). In each visibility graph, the shortest path from a start position to a goal one is searched by Dijkstra algorithm (Figure 5(d)). However, the robot cannot necessarily
M. Kayawake et al. / Path and Observation Planning of Vision-Based Mobile Robots
L1
T1
81
L2 T2 L3
C-Obstacle
R+S Obstacle
Obstacle
start
(a) C-obstacle.
Goal
(b) Visibility graph (S: small).
Shortest path
A Alternative path
(c) Visibility graph (S: large).
(d) Shortest path and alternative path.
Figure 5. Expanded C-obstacle and path planning.
move along the shortest path due to its performance and the configuration of obstacles and landmarks, and several candidates of path must be prepared. Therefore, to search for paths other than shortest path by Dijkstra algorithm, the vertex of C-obstacle is chosen at random as a relay point(= A) of the start position and the goal one. About the relay point, path from start position to A and path from A to goal position are searched by Dijkstra algorithm. The paths are composed by the connection of them. In this paper we call them alternative paths. Path for which it searched by choosing the relay point at random and quite different path can be planned. 4.2. Viewpoint Planning Viewpoint is planned about each path that is planned in the previous step. Viewpoint planning for arriving at the goal position safely is executed by considering the observation error of landmarks and dead-reckoning error. The robot estimates its position and orientation by dead-reckoning while moving after observing landmark(s). If it moves long distance after estimating its position with landmark(s), the error of estimated position and orientation is accumulated in proportion to the traveling distance. Therefore, the robot must estimate its position with landmark(s) frequently before position error is accumulated. Here, it is assumed that the robot observes landmark(s) where the distance between the present position ps and the start position is ms along the planned path. Let Dmax (ps , m) be the estimated maximum dead-reckoning error when the robot move distance m from ps , Emax (ps , m) be the estimated maximum error from the observation (1), and S (error tolerance) means the maximum error of the robot position for not colliding with obstacles. The maximum distance mmax that the robot can move without observing landmarks is expressed as follows (Figure 6(a)): Dmax (ps , mmax ) + Emax (ps , mmax ) ≤ R + S.
(2)
82
M. Kayawake et al. / Path and Observation Planning of Vision-Based Mobile Robots Ideal position of the future Ideal position
reach most distant point = next view point
mmax
R+S
Error
Ps
Actual position
P1
Position that comes off from range by error at first
(a) Distance that can be moved without observation.
A P2 Pg planned path path with error mPs,max (b) Selection of view points.
Figure 6. Observation planning
Here, let pg be the position whose distance from the start is ms + mps ,max . The path from ps to pg is divided into n + 1 position, and we define mi as the distance between the each divided position pi and the start position of the path (p0 = ps , pn = pg ). When the next viewpoint from ps is pi , the next viewpoint from pi must satisfy the following equation when the total number of observation becomes small (Figure 6(b)). mi + mpi ,max → max .
(3)
The robot selects the optimal landmark-observation strategy that satisfies (3). Therefore, the viewpoints can be decided one by one in the following way. 1. mps ,max and pg that satisfy (2) are calculated when the robot observes landmarks at ps . 2. The path from ps to pg is divided into n + 1 position pi . 3. pi that satisfies (3) is calculated, and pi is regarded as the next viewpoint. 4. If mi + mpi ,max is smaller than the distance between the start position and the goal position of planned path, ps is replaced with pi and go to step 1). If it is large, the viewpoint planning finishes. The optimal viewpoints, the optimal observation strategies in each viewpoint, the optimal observed landmark(s) and the direction of the cameras in each viewpoint could be planned in the above procedure.
5. Results of Motion Planning In this section, the effectiveness of the motion planning by the difference of the performance of the robot is verified. In this paper, robot performance means dead-reckoning error and image error. The dead-reckoning error shows at the rate how much error occurs about moving distance. The image error shows the position’s error of the landmark and the size error of the landmark. Figure 7(a) shows environment. The primary value of the Table 1. Robot performance. Name
Dead-reckoning error
Robot1
±10 %
±1 Pixel
Robot2
±40 %
±1 Pixel
Robot3
±40 %
±5 Pixel
Image error
M. Kayawake et al. / Path and Observation Planning of Vision-Based Mobile Robots 1m
: Landmark 1m
1m Robot3
Observation point
Obstacle Destination
83
Robot2
Robot1
Start
(a) Environment.
(b) Robot1 and Robot2 path.
(c) Robot3 path.
Figure 7. Simulation results. Table 2. Comparison of path. Name
Error torelance (cm)
Distance (cm)
Robot1
60
2359
6
Robot2
120
3246
19
Robot3
150
5140
31
The total number of observation
expanding width of C-obstacle is decided to S=0cm, and the increasing constant width is decided to 30cm. We compare Robot1 whose dead-reckoning error and image error is small, Robot2 whose dead-reckoning error is large, and Robot3 whose dead-reckoning error and image error are large, for evaluating the relationship between the planning result of the path and the viewpoints and the performance of the robots (Table 1). As the result of planning, Robot1 and Robot2 can reach the goal along the paths shown in Figure 7(b). In addition Robot3 can reach the goal along the path shown in Figure 7(c). The path of Robot1 is narrow and the landmark that can be observed is few. However, because the dead-reckoning performance of Robot1 is good, the path planned can be traced accurately. Moreover, because the observation accuracy is good, the error can be accurately corrected by a little observation frequency. On the other hand, because the dead-reckoning performance of Robot2 is worse than Robot1, it cannot trace accurately the planned path. Therefore, it cannot plan the path of Robot1. In addition, the path reaching the destination was not found at this error tolerance (=60cm). Then, error tolerance is changed, and Robot2 plans the path with wide width of the road though the distance of it is longer than the path of Robot1. Moreover, because the dead-reckoning performance of Robot2 is bad, it should correct frequently the dead-reckoning error by observing the landmark. As a result, the observation frequency of it is more than frequency of Robot1. The path of Robot3 is wide and landmarks that can be observed are many. Because the dead-reckoning performance and the observation accuracy of Robot3 are worst, it cannot be moved along the planned path. Therefore, it plans the path that safely reaches the destination though moving distance becomes long. In addition it is understood that Robot3 observed a lot of landmarks and moves along the route. From these results, it is shown that the optimal path observation points, observation strategies can be planned according to the performance of the robot. In concrete terms, the robot with high performance (small dead-reckoning and image error) can select the short path from the start to the goal, although there are few landmarks and this is a dangerous path. Contrarily, the robot with low performance selects the safe path in which a lot of landmarks can be observed and the accuracy of positioning is high, although the distance between the start and the goal is long.
84
M. Kayawake et al. / Path and Observation Planning of Vision-Based Mobile Robots
6. Conclusion In this paper, we propose a new path and viewpoint planning method for autonomous mobile robots with multiple observation strategies. The robot estimates its position by observing landmarks attached on the environments with two active cameras. It chooses the optimal landmark-observation strategy depending on the number and the configuration of visible landmarks in each place. The optimal path, viewpoints, and observation strategies that minimize the estimation error of the robot position and the number of observation can be selected properly. The effectiveness of our method is shown through simulations. As the future works, it should be better that models of sensor and motion error are based on probabilistic ones, such as Kalman filters, particle filters, SLAM.
Acknowledgment This research was partly supported by CASIO Science Promotion Foundation, Japan.
References [1] M. Okutomi and T. Kanade: “A Multiple-baseline Stereo,” IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol.15, No.4, pp.353–363, 1993. [2] K. Nagatani and S. Yuta: “Path and Sensing Point Planning for Mobile Robot Navigation to Minimize the Risk of Collision,” Proceedings of the 1993 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp.2198–2203, 1993. [3] A. Lazanas and J.-C. Latombe: “Motion Planning with Uncertainty: A Landmark Approach,” Artificial Intelligence, Vol.76, No.1-2, pp.287–317, 1995. [4] Th. Fraichard and R. Mermond: “Integrating Uncertainty and Landmarks in Path Planning for Car-Like Robots,” Proceedings of IFAC Symposium on Intelligent Autonomous Vehicles, Vol.2, pp,397–402, 1998. [5] N. Roy and S. Thrun: “Coastal Navigation with Mobile Robots,” Advances in Neural Processing Systems 12, Vol.12, pp.1043–1049, 1999. [6] J.P. Gonzalez and A. Stents: “Planning with Uncertainty in Position: An Optimal and Efficient Planner,” Proceedings of the 2005 IEEE International Conference on Intelligent Robots and Systems, pp,607–614 2005. [7] K. Tashiro, J. Ota, Y. C. Lin and T. Arai: “Design of the Optimal Arrangement of Artificial Landmarks,” Proceedings of the 1995 IEEE International Conference on Robotics and Automation, pp.407–413, 1995. [8] K. Takeuchi, J. Ota, K. Ikeda, Y. Aiyama and T. Arai: “Mobile Robot Navigation Using Artificial Landmarks,” Intelligent Autonomous Systems 6, pp.431–438, 2000. [9] A. Yamashita, K. Fujita, T. Kaneko, and H. Asama: “Path and Viewpoint Planning of Mobile Robots with Multiple Observation Strategies,” Proceedings of the 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp.3195-3200, 2004. [10] T. Kanbara, J. Miura, Y. Shirai, A. Hayashi and S. Li: “A Method of Planning Movement and Observation for a Mobile Robot Considering Uncertainties of Movement, Visual Sensing, and a Map,” Proceedings of the 4th Japan-France / 2nd Asia-Europe Congress on Mechatronics, pp.594–599, 1998.
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
85
Mobile Robot Motion Planning Considering Path Ambiguity of Moving Obstacles Hiroshi Koyasu and Jun Miura Department of Mechanical Engineering, Osaka University Suita, Osaka 565-0871, Japan Email: {koyasu,jun}@cv.mech.eng.osaka-u.ac.jp Abstract. This paper describes a motion planning method for mobile robot which considers the path ambiguity of moving obstacles. Each moving obstacle has a set of paths and their probabilities. The robot selects the motion which minimizes the expected time to reach its goal, by recursively predicting future states of each obstacle and then selecting the best motion for them. To calculate the motion for terminal nodes of the search tree, we use a randomized motion planner, which is an improved version of a previous method. Simulation results show the effectiveness of the proposed method. Keywords. motion planning, dynamic environment, mobile robot
1. Introduction Planning a path without collision with both static and dynamic obstacles is one of the fundamental function of mobile robots. Most of past research on motion planning in dynamic environments can be classified into two categories. One is for reactive methods, which do not predict the paths of moving obstacles [5]. In such a reactive method, not the optimality but the safety is more important. The other is for deliberative methods which are based on the assumption that the paths of moving obstacles are completely known; an optimal motion can be generated by employing a planning in space-time [3], or by using the concept of velocity obstacle [6,10]. However, it is difficult to predict paths of obstacles without uncertainty. We divide the uncertainty of an obstacle motion into the path ambiguity (which path to choose) and the error in following a path. The latter can be handled by considering the range of uncertainty in motion planning [2]. A similar approach is, however, difficult to apply to the former because a very inefficient motion may be planned when the possible paths of an obstacle spread in a wide area. It is, therefore, necessary to cope with multiple path candidates. Bennewitz et al. [1] proposed a motion planning method which considers multiple path candidates of obstacles. To avoid collision with an obstacle which has multiple path candidates, they use a heuristic cost function which is defined in space-time; the cost is set to be large near path candidates and proportional to the probability of each path candidate. Using this cost function, motion planning is done by using the A∗ search
86
H. Koyasu and J. Miura / Mobile Robot Motion Planning
algorithm. The method, however, does not consider the change of the environment to be recognized by future observations. Miura and Shirai [9] proposed to explicitly predict possible future states (i.e., which path a moving obstacle actually takes) of the environment and their probabilities for motion planning. By recursively searching for the optimal motion for each predicted state, they selected the optimal next motion minimizing the expected time to the goal. The method was, however, tested only for the case where there is only one moving obstacle with two path candidates and the possible motions of the robot are given in advance. In this paper, we expand our previous method [9] so that it can handle multiple moving obstacles with multiple path candidates and can plan robot motions according to the obstacle configuration and movements. We use a randomized motion planner, which is an improved version of a previous method. We show the effectiveness of the proposed method by simulation experiments. 2. Model of Moving Obstacles In the environment, there are static and moving obstacles. Positions of static obstacles are given in advance. Each moving obstacle has its own set of candidate destinations and moves to one of them with avoiding collision with static obstacles. We represent a set of possible paths by using a tangent graph [8]. The initial probability of each obstacle taking one of its paths is given. The robot periodically observes the position and the velocity of each moving obstacle, and updates the probabilities of its path candidates. When a new observation is obtained, the probability of the jth candidate path of the ith obstacle is updated by (1) P (pathij | oi (t)) = αP (pathij )P (oi (t) | pathij ), where oi (t) is the observed position of the obstacle at time t, and α is a normalization constant. P (pathij ) is given by P (pathij | oi (t − 1)). P (oi (t) | pathij ) is the likelihood of the path calculated by using the Gaussian positional uncertainty. 3. Predicting the State of Moving Obstacles We define a state of a moving obstacle as a set of possible obstacle path candidates. The states can be classified into two cases. One is the case where only one candidate is remaining. We call this case a fixed state. The other is the case where multiple candidates are still remaining. We call this case an ambiguous state. An ambiguous state will be changed into a fixed state by future observations; the probability of a fixed state thus becomes large as time elapses. 3.1. Uncertainty of obstacle motion on a path we assume that the error of a predicted position of an obstacle on a path is distributed within the so-called 3σ region of a 2D Gaussian. Its mean is the position predicted by assuming that the obstacle moves at the currently observed speed. The variance of the Gaussian perpendicular to the path is constant by supposing that an obstacle tries to follow the path as closely as possible. The variance along the path is proportional to the moving distance of the obstacle. 3.2. Calculating the probability of a state We first consider the case where there is one obstacle with two path candidates. Fig. 1 shows the relationship between the prediction of the positional uncertainty and the path ambiguity in such a case. In the figure, path1 and path2 are drawn as black arrows. Black points are predicted positions on the path candidates at time t1 and t2 . Let Ri (t) be the
H. Koyasu and J. Miura / Mobile Robot Motion Planning
87
Obstacle position and its uncertainty at time t 1 R 2 (t1 ) position and its uncertainty R 1 (t 1 ) at time t 2 path 2
R 2 (t 2 )
R 1 (t 2 ) path 1
Figure 1. Prediction of the positional uncertainty and the path ambiguity of a moving obstacle.
3σ region of the Gaussian on pathi at time t (i = 1, 2). If the obstacle is observed in the overlapped region of R1 (t) and R2 (t) (the hatched region in the figure), the state is an ambiguous state. The probability of this ambiguous state is calculated by (2) P (path1 )P (x1 (t) ∈ R2 (t)) + P (path2 )P (x2 (t) ∈ R1 (t)), where P (pathi ) is the prior probability of pathi , and xi (t) is the position at time t when the obstacle is actually on pathi . The first term of Eq. (2) is the probability that the obstacle on path1 cannot be determined to be on either of the paths. P (x1 (t) ∈ R2 (t)) is calculated by integrating the probability density function of x1 (t) in R2 (t). There are two fixed state. One is the case where the obstacle is determined to be on path1 , and the other is the case of path2 . The probability of the case of path1 is calculated by / R2 (t)). (3) P (path1 )P (x1 (t) ∈ Next, let us consider the case where there are n path candidates. The number of states of the obstacle is 2n − 1, that is, all combinations of paths. The probability of an ambiguous state where there are m remaining path candidates is calculated as follows. Let L = {l1, · · ·, lm } be a set of indices of the remaining path candidates. For the case where the obstacle is on pathli , the probability that the m paths are still possible is equal to the probability that xli is inside regions Rlj (j = 1, · · ·, i−1, i+1, · · ·, m) and is outside the other n − m regions. The weighted sum of such probabilities for the m remaining paths becomes the probability of the ambiguous state as follows: m P (pathli )P (xli (t) ∈ ∩j=i Rlj (t), xli (t) ∈ / ∪k∈L (4) / Rk (t)). i=1
Also, the probability of a fixed state where the obstacle is on pathi is given by / ∪j=i Rj (t)). (5) P (pathi )P (xi (t) ∈ When there are multiple moving obstacles, a state of the environment is represented by a set of the states of all obstacles, and its probability is the product of the probabilities of the states of the obstacles. A state of the environment is called fixed if the paths of all obstacles are uniquely determined. Otherwise, a state is ambiguous. 4. Planning Method The robot selects the motion which minimizes the expected cost. We use the time to reach the goal as the cost. We first predict possible future states of the environment and their probabilities. Then, by recursively searching for the optimal motion for each predicted state, we select the optimal next motion which minimizes the expected cost. This search process thus composes an AND/OR tree of robot motions and predicted states of the environment, as shown in Fig. 2. Once the next motion is determined by this search process, the robot executes it and observes the environment to update the state. The robot plans the subsequent motion based on the updated state.
88
H. Koyasu and J. Miura / Mobile Robot Motion Planning
Initial State
Motion i
Motion M
Minimization
State j
State L
Summation weighetd by probability
Motion 1
State 1
Motion i
fixed state
State j’
ambiguous state
Depth limit Motion i
Figure 2. AND/OR tree search.
S mi
Ci
mj 1
Sj C 1j
l
Sj C lj
prune Cj
goal n
Sj >C i
goal Figure 3. Prune a branch of the AND/OR tree.
Figure 4. A path for calculating the lower bound.
4.1. Search for the Motion Minimizing the Expected Cost We explain the search for the motion minimizing the expectation of the cost using Fig. 2. We first pick a next motion and predict a set of states after the robot executes the motion. If a state in the set is fixed, we calculate the cost of the state by using the motion planner described in Sec. 4.3. If a state is ambiguous, we recursively search for the optimal motion (and thus the cost) for the state. We limit the search depth to avoid combinatorial explosion. When the search reaches the depth limit, we calculate a feasible robot motion which considers all possible obstacle paths, i.e., plan a robot motion by assuming that every remaining path candidate is actually taken by some obstacle. The expectation of the cost of the next motion is then obtained by calculating the sum of the expectation of the costs of all possible states weighted by their probabilities. We calculate the expectation of the cost for every candidate for the next motion, and select the one which has the minimum expectation of the cost. 4.2. Pruning of the AND/OR Tree by Using Lower Bound To reduce the computation cost of the search, we perform a pruning of the AND/OR tree. Fig. 3 shows this pruning process, when searching for the motion minimizing the expected cost for state S. Suppose that for motion mi in state S, expected total cost Ci is calculated. This cost is the sum of the cost from the initial state to S and that from S to the goal state. For another motion mj , if its lower bound Cj is larger than Ci , mj can be pruned. This lower bound changes as the total cost of a predicted state to be reached after executing mj is obtained. Let Sjk (k = 1, · · · , n) be the predicted states. After the costs Cjk for Sjk (k = 1, · · ·, l ≤ n) are calculated, the lower bound becomes
H. Koyasu and J. Miura / Mobile Robot Motion Planning
Cj l = Cj +
l
P (Sjk ) Cjk − Cj .
89
(6)
k=1
If Cj l > Ci , we prune branch mj without calculating the costs of remaining n − l states. Lower bound Cj is calculated as follows. We consider a motion pair which is composed of a turning motion and a straight motion to reach the goal from the current position, with neglecting any collisions with obstacles, as shown in Fig. 4. We examine the cost of such motion pair for a given set of turning radii and set the minimum cost to Cj . 4.3. Calculating a Feasible Path To calculate a feasible path for a fixed state (including the case where a robot has to make a plan by considering all possible path candidates at the depth limit), we use an improved version of the method proposed by Hsu et al. [4]. Their method is a randomized search method which considers non-holonomic constraints of the robot motion. The method picks a robot motion at random and generates a probabilistic roadmap of sampled state×time points, called milestones, connected by short admissible trajectories. The probabilistic roadmap consists of a tree of milestones whose root is the initial robot pose. This search process finishes by finding a milestone which is in the endgame region. In the method, the completeness is an important issue, but the quality of planned path is not fully discussed. Since the method finishes the search by finding only one path, a found path may be much more inefficient than the optimal one. We improve their method in the following points: • Give weights to milestones so that a wide space is explored. • Iterate the path search until a sufficiently good path is found. In this paper, the endgame region is defined as the set of robot poses from which a feasible path to the goal can be generated as the one shown in Fig. 4. 4.3.1. Weighting Milestones In sampling milestones, if the weights of milestones are uniform, the tree may grow under a set of limited milestones; this may make a path quality low. To deal with this problem, Hsu et al. [4] give a weight to a milestone inversely proportional to the number of other milestones in the neighborhood of the milestone in the configuration space. However, the distance between milestones under a specific milestone is not necessarily small when their depths are largely different. Rapidly-exploring Random Tree (RRT) [7], which uses the size of the Voronoi regions for weighting, may have the same drawback. To make the tree grow relatively uniformly, we give the same weight to subtrees if their roots are at the same depth. Therefore the weight to milestone i with depth di and with bi motions which have not been expanded is given by (bi /B)(1/B)di , where B is the number of robot motions (i.e., branching factor). However, searching by using these weights is probabilistically equal to the breadthfirst search; this may require much computation. So we put more weights on deeper milestones. The weight of the milestone is modified as (bi /B)(a/B)di , where a (> 1) is a constant. 4.3.2. Evaluating the Quality of a Path We iterate the path search several times and select the best path among the generated ones. We here consider the problem of determining when to stop the iteration. To solve this problem, we need to be able to evaluate how good the current best path is. Therefore, we estimate the proportion of the number of potential paths whose costs are greater than
90
H. Koyasu and J. Miura / Mobile Robot Motion Planning
milestones milestones that found collision
di
found path
i
D
potential path
j
2
proportion
1
Figure 6. Milestones and their potential paths.
Figure 5. Number of potential paths on a milestone tree. 1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0
bi B
acutual path quality predicted path quality
0
1000
2000
3000
4000
5000
rank Figure 7. Predicted path qualities.
that of the current best path to the total number of all possible paths. After a feasible path is found, if the lower bound of a milestone is larger than the cost of the feasible path, no paths passing the milestone are more efficient than the feasible path. To estimate the proportion described above, we need to estimate the number of potential paths which passes a milestone. We assume that milestones at the same depth have the same number of potential paths. Under this assumption, a deeper milestone has fewer potential paths; Fig. 5 illustrates this property. In the figure, D denotes the maximum depth of the tree, which is set to that of the current best path. The total number of the potential paths is B D (B is the branching factor). Fig. 6 illustrates how to calculate the number of potential paths of a milestone. In the figure, milestone i has a child milestone j. We consider that a potential path belongs to the deepest milestone on the path. So in the figure, potential path set 1 belongs to milestone i and set 2 belongs to milestone j. A milestone which is a child of milestone i and has not been expanded has B D−(di +1) potential paths. If milestone i has bi such children, its number of potential paths is bi B D−(di +1) . If a milestone cannot be added to the tree due to collision, the potential paths passing it can never be obtained as feasible paths. So when milestone j cannot be added, the number of potential paths passing it is B D−dj , and this number should be excluded from the total number of the potental paths. Let I be the set of the indices of milestones whose lower bounds are greater than the cost of the current best path, and J be the set of the indices of milestones which cannot be added due to collision. The proportion of the number of potential paths whose costs are greater than that of the current best path to the total number of potential paths is thus
91
H. Koyasu and J. Miura / Mobile Robot Motion Planning
obstacle 1
goal obstacle 2
robot position
obstacle 1
robot position
t=0 obstacle 1
goal obstacle 2
robot position
goal obstacle 2
t=1 obstacle 2
goal obstacle 1
robot position
t=3
t=4
obstacle 1
goal obstacle 2
robot position
t=2 obstacle 2
goal obstacle 1
robot position
t=5
Figure 8. Simulation result.
estimated by
bi B D−di −1 . − j∈J B D−dj
i∈I
BD
(7)
If the estimated proportion becomes greater than 1−α, the current best path is expected to belong to the best 100α% of the entire set of potential paths. If this condition is satisfied with a small α, we consider that a sufficiently good path is obtained. Currently, we use 0.1 as α. To validate Eq. (7), we performed the following experiment. First, we generated 5000 paths from a milestone tree. Next, for each path, we calculated the estimated proportion given by Eq. (7) using the path as the current best path, and compared it with the actual proportion. Fig. 7 shows the comparison result. The estimated proportion is always less than the actual one mainly because the estimation is based on the lower bound. When the quality of a path is high enough, the estimated proportion is near to the actual; this suggests that this estimated proportion is a good measure of path quality. 5. Experimental Results Fig. 8 shows the results of a simulation experiment. In the figure, white regions are free space, dark gray lines are path candidates of moving obstacles. Gray lines are planned
92
H. Koyasu and J. Miura / Mobile Robot Motion Planning
goal obstacle 2 obstacle 1
robot position(start)
Figure 9. Probabilities of the paths of each obstacle.
Figure 10. Result of motion planning without considering ambiguity.
robot paths, among which a darker line indicates a path with a higher probability. On paths of both the robot and the obstacles, circles are drawn with a fixed time interval. In the experiment, the number of obstacles is two, the number of initial path candidates of each obstacle is three, and the depth limit of search is two. A motion of the robot is represented by a pair of the turning radius and the speed. The number of robot motions is nineteen. The prior probability of each path candidates is set equally. Obstacle 1 actually takes the right path, and obstacle 2 takes the middle. Fig. 9 shows the change of the probabilities of paths for each obstacle. For obstacle 1, the probability of taking the right path becomes dominant at t = 1, but the ambiguity of paths remains until t = 4. For obstacle 2, the probability of taking the middle path becomes dominant at t = 1, but the ambiguity of paths remains until t = 4. At time t = 0, since all paths of the obstacles are equally probable, the robot selects the motion to move a position where the robot can safely avoid collision with the obstacles even if they will take any paths. At time t = 1, the situation is still highly ambiguous and the robot selects a motion similar to the one selected at time t = 0. At time t = 2, since the path of each obstacle is almost uniquely determined, only a few robot motions in a generated plan have high probabilities. At time t = 3, the robot now knows that both obstacles will not take their left paths and generates a plan in which the probability of robot motions which will cross over the left paths. At time t = 4, there is no obstacle in front of the robot. However, since the environment is not fixed, the robot generates two slightly different paths due to the randomness of the planner. At time t = 5, the paths of the obstacles are fixed, and the robot plans a single path towards the goal. The computation time at t = 0 is 140 [sec] using an Athlon 2200+ PC. At t = 1, the time is 9.5 [sec]. After t = 1, the time does not exceed 0.5 [sec]. The computation time is decreased with time elapses, because the ambiguity of the environment is decreased. We also compared our method with the method which plans a robot motion to avoid all possible path candidates of obstacles. Fig. 10 shows the planning result at t = 0 by this method in the same situation as the one shown in Fig. 8. In the plan, the robot moves slowly until t = 2 to wait for the obstacles to go away. The expected cost of the plan is 12.90 [sec]. On the other hand, our method selects a faster motion in the same situation, and the expected cost is 10.99 [sec]. This result shows the effectiveness of the proposed method.
H. Koyasu and J. Miura / Mobile Robot Motion Planning
93
6. Conclusion This paper has described a path planning method which considers path ambiguity of moving obstacles. The method predicts possible future states and their probabilities, and searches for a robot motion for each predicted state. This prediction and search are recursively performed until every state becomes terminal (i.e., the ambiguity of a state is resolved or the search reaches the depth limit). The method finally selects the next motion which minimizes the expected time to the goal. The proposed method can deal with fairly general motion planning problems with arbitrary static obstacle configurations and multiple moving obstacles with multiple path candidates. The path planner which we use for calculating the costs of terminal nodes is an improved version of a previous randomized method; it iteratively generates a set of paths until a sufficiently good path is generated. The number of iteration is determined based on the quality evaluation of the current best path. The current method still needs too much calculation time to be used on-line. A future work is to consider the trade-off between the quality of a planned path and the computational cost in order to appropriately control the planning time. References [1] M. Bennewitz, W. Burgard, and S. Thrun. Adapting navigation strategies using motions patterns of people. In Proc. of the 2003 IEEE Int. Conf. on Robotics and Automation, pages 2000–2005, 2003. [2] A. Elnagar and A. Basu. Local Path Planning in Dynamic Environments with Uncertainty. In Proc. of IEEE Int. Conf. on Multisencor Fusion and Integration for Intelligent Systems, pages 183–190, 1994. [3] K. Fujimura. Time-Minimum Routes in Time-Dependent Networks. IEEE Tran. of Robotics and Automation, 11(3):343–351, 1995. [4] D. Hsu, R. Kindel, J.C. Latombe, and S. Rock. Randomized kinodynamic motion planning with moving obstacles. Int. J. Robotics Research, 21(3):233–255, 2002. [5] S. Ishikawa. A method of indoor mobile robot navigation by using fuzzy control. In Proc. 1991 IEEE/RSJ Int. Workshop on Intelligent Robots and Systems, pages 1013–1018, 1991. [6] F. Large, S. Sekhavat, Z. Shiller, and C. Laugier. Towards real-time global motion planning in a dynamic environment using the nlvo concept. In Proc. of the IEEE Int. Conf. on Intelligent Robots and Systems, 2002. [7] S. M. LaValle and J. J. Kuffner. Rapidly-exploring random trees: Progress and prospects. In B. R. Donald, K. M. Lynch, and D. Rus, editors, Algorithmic and Computational Robotics: New Directions, pages 293–308. A K Peters, Wellesley, MA, 2001. [8] Y.-H. Liu and S. Arimoto. Path planning using a tangent graph for mobile robots among polygonal and curved objects. Int. J. of Robotics Research, 11(4):376–382, 1992. [9] J. Miura and Y. Shirai. Probabilistic uncertainty modeling of obstacle motion for robot motion planning. J. of Robotics and Mechatronics, 14(4):349–356, 2002. [10] Z. Qu, J. Wang, and C. E. Plaisted. A new analytical solution to mobile robot trajectory generation in the presence of moving obstacles. IEEE Transactions on Robotics, 20(6):978– 993, 2004.
94
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
Robotic Navigation using Harmonic Functions and Finite Elements Santiago Garrido a,1 , Luis Moreno a a Carlos III University b Madrid, Spain Abstract. The harmonic potentials have proved to be a powerful technique for path planning in a known environment. They have two important properties: Given an initial point and a objective in a connected domain, it exists a unique path between those points. This path is the maximum gradient path of the harmonic function that begins in the initial point and ends in the goal point. The second property is that the harmonic function cannot have local minima in the interior of the domain (the objective point is considered as a border). Our approach has the following advantages over the previous methods: 1) It uses the Finite Elements Method to solve the PDE problem. This method permits complicated shapes of the obstacles and walls. 2) It uses mixed border conditions, because in this way the trajectories are smooth and the potential slope is not too small and the trajectories avoid the corners of walls and obstacles. 3) It can avoid moving obstacles in real time, because it works on line and the speed is high. 4) It can be generalized to 3D or more dimensions and it can be used to move robot manipulators. Keywords. Path Planning, Harmonic Functions, Finite Elements
1. Introduction Robot motion control can be viewed as an underconstrained problem. The robot exists at a certain configuration, and must get to a certain goal configuration, using any free path. If properly specified, such underconstrained interpolation problems can be solved using Laplace’s equation. Intuitively, Laplace’s equation can be thought of as governing the shape of a thin membrane: Obstacle positions are pulled up, and goal positions are pulled down. If a ball bearing is dropped on the membrane, it will always fall into one of the goal positions, never hitting the obstacles. The movement of the ball corresponds to a change in state of a robot (e.g., the movement of joints during a reach). The trajectory of the ball is always a smooth, obstacle-avoiding path. The harmonic potentials have proved to be a powerful technique for path planning in a known environment. They have two important properties: Given an initial point and a objective in a connected domain, it exists a unique path between those points. This path is the maximum gradient path of the harmonic function that begins in the initial point 1 Correspondence to: Santiago Garrido, Carlos III University, Leganés, Madrid, Spain. Tel.: +34 91 6245990; Fax: +34 91 6249430; E-mail:
[email protected]
S. Garrido and L. Moreno / Robotic Navigation Using Harmonic Functions and Finite Elements
95
and ends in the goal point. The second property is that the harmonic function cannot have local minima in the interior of the domain (the objective point is considered as a border). These properties means that a path can always be found if it exists and it is impossible to be stuck in a local minima. These are the most important properties that made harmonic functions extremely interesting for path planning purposes. Our approach has the following advantages over the previous methods: 1) It uses the Finite Elements Method to solve the PDE problem. This method permits complicated shapes of the obstacles and walls. 2) It uses mixed border conditions, because in this way the trajectories are smooth and the potential slope is not too small and the trajectories avoid the corners of walls and obstacles. 3) It can avoid moving obstacles in real time, because it works on line and the speed is high. 4) It can be generalized to 3D or more dimensions and it can be used to move robot manipulators. 1.1. Previous work Every method concerning robot motion planning has its own advantages and application domains as well as its disadvantages and constraints. Therefore it would be rather difficult either to compare methods or to motivate the choice of a method upon others. In contrast to many methods, robot motion planning through artificial potential fields (APF) considers simultaneously the problems of obstacle avoidance and trajectory planning (Arkin [1][2]. In addition the dynamics of the manipulator are directly taken into account, which leads in our opinion to a more natural motion of the robot. The first use of the APF concept for obstacle avoidance was presented by Khatib [8]. He proposed a Force Involving an Artificial Repulsion from the Surface (FIRAS, in French) which should be nonnegative, continuous and differentiable. However, the potential field introduced exhibits local minima other than the goal position of the robot. To solve the preceding problem, Volpe and Khosla [11] developed new elliptical potential functions called “superquadric artificial potential functions”, which do not generate local minima in the physical space. They have shown that superquadric potential fields can be constructed only for simple shapes like square or triangular figures. The problem of local minima remains, because the superquadric potential functions do not generate local minima in the workspace but the local minima can occur in the C-space of the robot. The contributions of Koditschek in [9][10][6] are worth to be mentioned because they introduce an analytic potential field in the C-space without local minima. However the topology of the application range is limited to obstacles, which have to be ball-, or star-shaped, otherwise no solution can be found. The contributions of Connolly [4] and of Kim and Khosla [7] are the most successful methods concerning robot motion planning with potential field. They used the harmonic functions to build a potential field in the C-space without local minima. The harmonic functions attain their extreme values at the boundary of the domain. Using a potential function to accomplish a certain motion implies that the trajectory of the robot is not known or calculated in advance, which means that the robot chooses autonomously the way to reach its goal. The main problem of this method is the complication of the Panel Method used that means that the obstacles have to be very simple, like rectangles. The method of harmonic functions based on the Finite Difference Method has been used for guiding robots by Connolly and Gruppen [3]. Edson Prestes [5] used this method for exploration of unknown environments. The method of harmonic functions is not very extended because up to now has been based in the Finite Difference Method, and for
96
S. Garrido and L. Moreno / Robotic Navigation Using Harmonic Functions and Finite Elements
this reason it is not easy to implement. It is quite slow and has to be used in static environments and the environment has to be very easy with obstacles and walls with parallel straight lines.
2. Harmonic Functions Equilibrium problems in two-dimensional, and higher, continuous give rise to elliptic partial differential equations. A prototype is the famous equation of Laplace: ∇2 φ = 0. This equation holds for the steady temperature in an isotropic medium, characterizes gravitational or electrostatic potentials at points of empty space, and describes the velocity potential of an irrotational, incompressible fluid flow. The two-dimensional counterpart of this equation lies at the foundation of the theory of analytic functions of a complex variable. An harmonic function φ on a domain Ω ∈ R is a function which satisfy the n ∂2φ = 0 where xi is the i − th Cartesian coordinate and Laplace’s equation: ∇2 φ = ∂x2 i=1
i
n is the dimension. In the case of robot path planning, the boundary ∂Ω of Ω, is formed by all the walls, obstacles and the goal, but not the start point. Maximum Principle: A nonconstant harmonic function φ(x) defined on Ω takes its Maximum value M on the boundary and guarantees that there are no local minima in the interior of Ω.
Figure 1. Robot’s trajectories obtained to solved Laplace’s equation with the boundary conditions of Dirichlet, Neumann and Robin, respectively.
S. Garrido and L. Moreno / Robotic Navigation Using Harmonic Functions and Finite Elements
97
2.1. Problems using Harmonic Functions The use of harmonic functions as path planning potentials is a powerful technique but it is not very extended because : First, the previous methods based on finite differences need that all obstacles and walls of the region are composed by parallel straight lines. Second, the previous methods based on finite differences are slow and difficult to implement in a dynamic environment that changes with time.Third, some implementations of these methods that does not assure the existence of a trajectory. For example, when using Harmonic functions as a potential field it is not possible to add an attractive potential to the goal and a repulsive potential from the obstacles because the domain is not the same. In the first case, the goal is one of the borders because this is the only way of being the minimum of the function. In the second case the borders are the maximum and the goal can’t be one of them because the resulting addition function does not have the minimum at the goal. Because of that, the only way of doing a potential attractive to the goal and repulsive from the obstacles is defining the obstacles borders value at the maximum value, the goal border at the minimum value and to solve the Laplace function with this border values. Fourth, in case the obstacles are not fixed, it is necessary to recalculate the harmonic function and path continuously, at each time that a movement is registered in order to obtain an adaptive version of the method.
3. Boundary Conditions The different kinds of contour conditions imposed to the Laplace equation have a critical importance in the solution obtained of the equation and the quality of the trajectory. The directions field associated to the partial derivatives problem give the path that the robot has to track. To achieve a good quality path to goal, some convenient properties of the direction fields are: 1. the trajectories have to be smooth and as short as possible. 2. the potential slope have not to be small. 3. The trajectories have to avoid the corners of walls and obstacles. Because of that, the gradient has to be perpendicular or tangent to the borders of the work space. The solution can be considered as the potential function and the trajectories associated to the direction field of the potential, that is the path of the robot. The contour conditions can be given in three ways: by the values of the potencial function in the contour (Dirichlet), by the gradient values in the contour (Neumann), or by a linear combination of the two of them (Robin or mixed): Dirichlet φ = c(x, y) in the contour. Usually is taken as a constant function φ = K. Neumann ∇(φ) = c(x, y) in the contour. Usually is taken as a constant function ∇(φ) = K. Robin or Mixed n · a · ∇(φ) + b · φ = c(x, y) in the contour. Usually is taken as a constant function n · a · ∇(φ) + b · φ = K. If the solution of the Laplace equation is the velocity potential of an irrotational, incompressible fluid flow, then the Dirichlet conditions represent the different contour levels, the Neumann conditions correspond to the entrance or exit of fluid flow and the Robin conditions correspond to the two of them at the same time.
98
S. Garrido and L. Moreno / Robotic Navigation Using Harmonic Functions and Finite Elements
Figure 2. Solutions of Laplace’s equation with the boundary conditions of Dirichlet, Neumann and Robin, respectively.
Dirichlet Conditions. In the subsection 2.1 was treated some problems in the use of harmonic functions and the reasons why this method is not very extended. Another problem in the way this method is used is that in some articles (e.g. Karnik et al.) three different values are given to the initial point, to the walls and obstacles and to the objective point (value 10 to the inicial point, value 8 to the walls and obstacles and value -10 to the objective). In this situation the trajectories can go to the walls as can be seen in the figure. This situation happens because the Maximum’s Principle says that the solution of the Laplace’s equation can’t have its maximum (and Minimum) in an interior point, but it says nothing about the border. The unique way of not having the end point of a trajectory in the walls is to give them the maximum value. The reason why some authors give to the initial point a higher potential value than the walls values is because in regions far from the objective point the potential values are quite flat ( for Dirichlet conditions). In this situation the gradient is very small and computational error can be produced (round off, quantization,...). The correct way to be sure that all the trajectories end at the objective point is to have only two values of the potential in the border conditions: the maximum value in the walls and obstacles and the minimum value in the objective point. It is important to note that the objective point, the obstacles and the walls belong to the contour of the Laplace’s equation problem. It is possible too, to give the high potential value to the initial point. But, in this way the unicity of the trajectories is lost, and it will be necessary a selection criteria in order to choose the initial direction.
S. Garrido and L. Moreno / Robotic Navigation Using Harmonic Functions and Finite Elements
99
This high potential value for the initial point is motivated for the logarithmic dependency of the distance to the objective point. Because of this reason, points distant to the objective have a small gradient that difficult the numerical calculus of the trajectory. In order to assure that the trajectories end at the objective point and the solution gradient is not small it is a preferable to use the Robin border conditions. Proposition.- If the borders have a maximum potential value (=1) and the objective point has a minimum value (=0), then there exist a unique trajectory from the initial point to the objective point.
.
Figure 3. Contour of Laplace’s equation with the boundary conditions of Dirichlet, Neumann and Robin, respectively.
This proposition shows the convenience of choosing the contour conditions of Dirichlet or Robin, because they give fixed values to the potential conditions in the contour. When Dirichlet’s contour conditions are used the solution is quite flat for values far from the objective point. For this reason, it is easy to have computational errors in the trajectory computation. Apart from that, the trajectories are very smooth and they are not very close to the walls corners and obstacles. Neumann’s Conditions. In the Neumann’s Conditions case, the descent is smooth and continuous, with a slope not close to zero and because of that it is more difficult to have computational errors in the trajectory calculation. In the Neumann’s conditions case it is possible, too, to assure the existence of a unique trajectory from the initial point to the objective point. If the walls and obstacles are sources, with positive flow and the objective point is the only sink, with negative flow, then by similitude with the problem of th velocity potential of an irrotational, incompressible flow there is a unique trajectory from the initial point to the objective. However, the problem with Neuman’s conditions is that the trajectories are very close to the contour corners and the trajectories are so close to these corners that it is impossible to continue the calculation of the trajectory, as can be seen in the figure. Robin Conditions (or mixed). Finally, with the contour conditions of Robin have the advantages of two both and none of the disadvantages: the solution have a smooth slope, continuous and big enough, and, at the same time, the trajectories are not close to the corners. This let us an easy calculation of the trajectories.
4. Comparison between Finite Elements Method and Finite Difference Method The finite elements method (FEM) is particularly useful when a robust approximation is sought to solve partial differential equations on an inhomogeneous mesh. Solid mathematical foundations and a great deal of generality allow for different implementations.
100
S. Garrido and L. Moreno / Robotic Navigation Using Harmonic Functions and Finite Elements
The most important difference between Finite Elements Method and Finite Difference Method is the treatment of the geometry of the domain. Finite Differences Method uses rectangular meshes and Finite Element Method usually uses triangular meshes. J. Rossell uses a hierarchical decomposition of the domain, but it can be awkward to try to work finite differences over nonuniform meshes. Generally speaking the finite element method is more flexible with curved geometries, etc. Clearly the details of finite element method are more complex than those of the finite difference technique. The end-points of both methods are essentially the same namely a system of linear or nonlinear algebraic equations. The finite element method, however, has a number of advantages: • it is well suited to problems involving complex geometries. • it can readily handle problems where the physical parameters vary with position within the domain. • it can also be used for nonlinear and/or time-varying problems. • complex boundary conditions can be readily dealt with. • general computer programs to perform finite element calculations can be, and have been, developed conventional numerical techniques can be used to solve the equations resulting from a finite element analysis. In few words, anything that can be done nicely with finite differences can be done well with finite elements, but that the converse does not hold.
.
Figure 4. Avoidance of stationary obstacles.
5. Implementation of the FEM method The method starts by approximating the computational domain with a union of simple geometric objects, in the 2D case triangles. The triangles form a mesh and each vertex is called a node. The mesh design has to strike a balance between the ideal rounded forms of the original sketch and the limitations of his simple building-blocks, triangles or quadrilaterals. If the result does not look close enough to the original objects, we can always improve it using smaller blocks. Now using the Laplace’s equation (expressed in Ω) −∇2 u = 0. If uh is a piecewise linear approximation to u, it is not clear what the second derivative term means. Inside each triangle, ∇uh is a constant (because uh is flat) and thus the second-order term vanishes. At the edges of the triangles, ∇uh is in general discontinuous and a further derivative makes no sense. This is the best approximation of u in the class of continuous piecewise polynomials. Therefore we test the equation for uh against all possible functions v of that class. Testing means formally to multiply the residual against any function and then integrate, i.e.,
S. Garrido and L. Moreno / Robotic Navigation Using Harmonic Functions and Finite Elements
101
determine uh such that Ω (∇ uh )vdx = 0 for all possible v. The functions v are usually called test functions. Partial integration (Green’s formula) yields that uh should satisfy Ω ∇uh ∇v − n · (c∇uh )vds = 0 where ∂Ω is the boundary of Ω and n is the outward pointing ∂Ω normal on ∂Ω. Note that the integrals of this formulation are well-defined even if uh and v are piecewise linear functions. Boundary conditions are included in the following way. If uh is known at some boundary points (Dirichlet boundary conditions), we restrict the test functions to v = 0 at those points, and require uh to attain the desired value at that point. For all the other points Robin boundary conditions are used,i.e., (∇uh ) · n + quh = g . The FEM formulation is: to find uh such that Ω ∇uh ∇v + ∂Ω1 quh vds = ∂Ω1 gvds ∀v where ∂Ω1 is the part of the boundary with Robin conditions. The test functions v must be zero on ∂Ω − ∂Ω1 . Any continuous piecewise linear uh is represented as a combination uh (x) = N i=1 Ui φi (x) where φi are some special piecewise linear basis functions and Ui are scalar coefficients. Choose φi like a tent, such that it has the "height" 1 at the node i and the height 0 at all other nodes. For any fixed v, the FEM formulation yields an algebraic equation in the unknowns Ui . To determine N unknowns, we need N different instances of v. What better candidates than v = φj , j = 1, 2, ..., N ?. We find a linear system KU = F where the matrix K and the right side F contain integrals in terms of the test functions φi , φj and the coefficients defining the problem: q, and g. The solution vector U contains the expansion coefficients of uh , which are also the values of uh at each node xi since uh (xi ) = Ui . If the exact solution u is smooth, then FEM computes uh with an error of the same size as that of the linear interpolation. It is possible to estimate the error on each triangle using only uh and the PDE coefficients (but not the exact solution u, which in general is unknown). The FEM method provides functions that assemble K and F automatically. To summarize, the FEM approach is to approximate the PDE solution u by a piecewise linear function which is expanded in a basis of test-functions φi , and the residual is tested against all the basis functions. This procedure yields a linear system KU = F . The components of U are the values of uh at the nodes. For x inside a triangle, uh (x) is found by linear interpolation from the nodal values.
6. Avoidance of stationary and moving obstacles
.
Figure 5. Avoidance of moving obstacles.
102
S. Garrido and L. Moreno / Robotic Navigation Using Harmonic Functions and Finite Elements
Figure 6. Local Path Planning using the trajectory done by the solution of the Laplace’s equation calculated by Finite Element Method, with sensor data.
This technique can be used at a global level to plan a free obstacles trajectory by considering the environment information contained in an a priory map. Besides, this technique can also be applied at a local level to consider sensor based information. This situation is required because in spite of the robot has an initial global map of the environment and makes an initial path planning from the initial point to the goal. This map is not perfect and can have errors such as obstacles that doesn’t appear in the map. This obstacles can be static or they can move. In order to avoid obstacles, the robot requires to consider local map information extracted from sensors. This local map is used in a reactive way to find the path avoiding the obstacles. When there are static obstacles, the followed strategy consist of the steps shown in figure 7. This kind of algorithm is possible to be performed today because of the speed of current computers and the flexibility of the resolution of the Laplace’s equation with the finite elements method that permits all kind of shapes in walls and obstacles. An important detail to be considered is that the gradient modulus is proportional to the objective distance (without obstacles). For this reason, it is better to use a constant speed in order to avoid big differences in speed of the robot.
7. Conclusions In this paper, we introduced the harmonic potential function for building potential fields for path planning and moving obstacle avoidance. We proposed the use of harmonic functions to eliminate the local minima even in very complicated environments. The effectiveness of the proposed strategy has been demonstrated using this new finite element method robot navigation, autonomous motion planning in known and unknown environments with dynamic obstacles is greatly improved. The computational efficiency of the proposed control scheme makes it particularly suitable for real-time implementation.The trajectory of the global map is done off-line and the trajectory in a local map of dimensions 40x40, with a grid composed by 3200 triangles lasts less than one second in a 1.6 Mhz laptop PC.
S. Garrido and L. Moreno / Robotic Navigation Using Harmonic Functions and Finite Elements
103
Begin
Calculation of the position of the obstacles t hat doesn’t appear in the map
Resolution of the Laplace’s equation with the border conditions given by the obstacles and walls positions . no
Calculation of the direction field given by the potential of the solution of the Laplace’s equation.
Calculation of the trajectory to the objective.
Tracking of the calculated trajectory a small distance , which depends on the distance to the next mobile object.
Has the robot found the objective? yes End
Figure 7. Flow chart of the algorithm.
References [1] R. C. Arkin. Integrating behavioral, perceptual, and world knowledge in reactive navigation. Robotics and Autonomous Systems, 6, 1990. [2] R. C. Arkin. Behavior-Based Robotics-Intelligent robots and autonomous agents. The MIT Press, 1998. [3] R.A. Grupen C.I. Connolly. The applications of harmonic functions to robotics. J. Robotic Systems, 10(7):931–946, 1993. [4] C. I. Connolly. Harmonic functions as a basis for motor control and planning. PhD thesis, University of Massachussets, Amherst, MA, USA, 1994. [5] M. Trevisan M.A.P. Idiart E. Prestes, P. M. Engel. Exploration method using harmonic functions. Rob. Aut. Systems, 40(1):2542–2558, 2002. [6] D.E. Koditschek E. Rimon. Exact robot navigation using artificial potential functions. IEEE Transactions on Robotics and Automation, 8(5):501–518, 1992. [7] P. K. Khosla J. O. Kim. Real-time obstacle avoidance using harmonic potential functions. IEEE Transactions on Robotics and Automation, 8(3):338–349, 1992. [8] O. Khatib. Commande dynamique dans l’espace opérationnel des robots manipulateurs en présence d’obstacles. PhD thesis, École Nationale Supérieure de l’Aéronautique et de l’Espace (ENSAE), Toulouse, France, 1980. [9] D. E. Koditschek. The control of natural motion in mechanical systems. Journal of Dynamic Systems, Measurement, and Control, 113:547–551, 1991. [10] D.E. Koditschek. Some applications of natural motion. Journal of Dynamic Systems, Measurement, and Control, 113:552–557, 1991. [11] P. Khosla R. Volpe. Manipulator control with superquadric artificial potential functions: Theory and experiments. IEEE Transactions on Systems, Man, and Cybernetics,, 20(6):1423– 1436, 1990.
104
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
The Hippocampal Place Cells and Fingerprints of Places: Spatial Representation Animals, Animats and Robots a.
Adriana Tapusa1, Francesco Battagliab and Roland Siegwarta Ecole Polytechnique Fédérale de Lausanne (EPFL), Autonomous Systems Lab, Switzerland b. Collège de France, LPPA, France
Abstract. In this paper we address the problem of autonomous navigation seen from the neuroscience and the robotics point of view. A new topological mapping system is presented. It combines local features (i.e. visual and distance cues) in a unique structure – the “fingerprint of a place” - that results in a consistent, compact and distinctive representation. Overall, the results suggest that a process of fingerprint matching can efficiently determine the orientation, the location within the environment, and the construction of the map, and may play a role in the emerging of spatial representations in the hippocampus
1. Introduction In all our daily behaviors, the space we are living and moving in plays a crucial role. Many neurophysiologists dedicate their work to understand how our brain can create internal representations of the physical space. Both neurobiologists and robotics specialists are interested in understanding the animal behavior and their capacity to learn and to use their knowledge of the spatial representation in order to navigate. The ability of many animals to localize themselves and to find their way back home is linked to their mapping system. Most navigation approaches require learning and consequently need to memorize information. Stored information can be organized as cognitive maps – term introduced for the first time in [31]. Tolman’s model advocates that the animals (rats) don’t learn space as a sequence of movements; instead the animal’s spatial capabilities rest on the construction of maps, which represent the spatial relationships between features in the environment. Several methods, each with its advantages and drawbacks, have been proposed to construct maps in the framework of autonomous robot navigation, from precise geometric maps based on raw data or lines to purely topological maps using symbolic descriptions. To mention only a few papers in the vast SLAM (Simultaneous Localization and Mapping) literature, Leonard and Durrant-Whyte introduced for the first time the concept of SLAM as the construction of maps while the robot moves 1
Corresponding Author: Ecole Polytechnique Fédérale de Lausanne (EPFL), Autonomous Systems Lab, 1015 Lausanne, Switzerland ; E-mail :
[email protected]
A. Tapus et al. / The Hippocampal Place Cells and Fingerprints of Places
105
through the environment and the localization with respect to the partially built maps [15]. Many works in space representations are based on metric maps. The stochastic map technique to perform SLAM [3, 7, 15] and the occupancy grids approaches [28] are typical examples belonging to this kind of space representation. More recent visionbased metric approaches use SIFT features [24]. However, metric SLAM can become computationally very expensive for large environments. Thrun in [29] proposes probabilistic methods that make the metric mapping process faster and more robust. However, one of the main shortcomings of metric maps is that they are not easily extensible so as to be useable for higher level, symbolic reasoning. They contain no information about the objects and places within the environment. Topological approaches to SLAM attempt to overcome the drawbacks of geometric methods by modeling space using graphs. Significant progress has been made since the seminal papers by Kuipers [13, 14]; Kortenkamp and Weymouth in [12] have also used cognitive maps for topological navigation. They defined the concept of gateways which mark the transition between two adjacent places in the environment. Their work has been an amelioration of Mataric’s approach [17], contributing towards the reduction of the perceptual aliasing problem (i.e. observations at multiple locations are similar). They have used the data from sonars combined with vision information in order to achieve a rich sensory place-characterization. A model by Franz, Schölkopf and Mallot [8] was designed to explore open environments within a maze-like structure and to build graph-like representations. The model described in [5] represents the environment with the help of a Generalized Voronoi Graph (GVG) and localize the robot via a graph matching process. This approach has been extended to H-SLAM (i.e. Hierarchical SLAM) in [16], by combining the topological and feature-based mapping techniques. In [30], Tomatis et al. have conceived a hybrid representation, similar to the previously mentioned work, in which a global topological map with local metric maps associated to each node for precise navigation is described. Topological maps are less complex, permit more efficient planning than metric maps and they are easier to generate. Maintaining global consistency is also easier in topological maps compared to metric maps. However, topological maps suffer from perceptual aliasing (i.e. observations at multiple locations are similar) and the difficulty in automatically establish a minimal topology (nodes). Our method uses fingerprints of places to create a cognitive model of the environment. The fingerprint approach, by combining the information from all sensors available to the robot, reduces perceptual aliasing and improves the distinctiveness of places. The main objective of this work is to enable the navigation of an autonomous mobile robot in structured environments without relying on maps a priori learned and without using artificial landmarks. A new method for incremental and automatic topological mapping and global localization [26] using fingerprints of places is described. The mapping method presented in this paper uses fingerprints of places to create a cognitive model of the environment. The construction of a topological mapping system is combined with the localization technique, both relying on fingerprints of places. This fingerprint-based approach yields a consistent and distinctive representation of the environment and is extensible in that it permits spatial cognition beyond just pure navigation.
106
A. Tapus et al. / The Hippocampal Place Cells and Fingerprints of Places
2. Navigation Framework Navigation strategies are based on two complementary sources of information (available on the mobile agent: animal, robot): idiothetic and allothetic. The idiothetic source yields internal information about the mobile agent movements (e.g. speed, acceleration) and the allothetic source provides external information about the environment (e.g. the cues coming from the visual, odor, laser range finders, sonars, etc.). Idiothetic information provides a metric estimate of the agent’s motion, suffering from errors accumulation, which makes the position estimation unreliable at long-term. In contrast, the allothetic (sensory) data is stationary over the time, but is susceptible to perceptual aliasing (i.e. observations at multiple locations are similar) and requires non-trivial processing in order to extract spatial information. The map-based navigation needs map-learning and localization. Map-learning is the process of constructing a map representing the environment explored by the mobile agent and localization is the phenomenon of finding the mobile agent’s location (position) in the map. Localization and mapping are interdependent – to localize the robot, a map is necessary and to update a map the position of the mobile agent is needed. This is usually known as Simultaneous Localization and Mapping (SLAM) problem that is of a “chicken and egg“ nature. While navigating in the environment, the mobile agent first creates and then updates the map.
3. Fingerprints of Places and Space Cognition The seminal discovery of place cells, by O’Keefe and Dostrovsky [20], in the rat hippocampus – cells whose firing pattern is dependent on the location of the animal in the environment – led to the idea that the hippocampus works as a cognitive map of space [21]. It was shown in [4] (for a review see e.g. [23]) that the lesion of the hippocampus impairs the performance of rodents in a wide variety of spatial tasks indicating a role of the hippocampus in map-based navigation. The framework for topological SLAM (Simultaneous Localization and Mapping) (see Figure 2) that we propose here organizes spatial maps in cognitive graphs, whose nodes correspond to fingerprints of places, and may be seen as a possible mechanism for the emergence of place cells. The computational model describes how a mobile agent can efficiently navigate in the environment, by using an internal spatial representation (similar to some extent to hippocampal place cells). This model builds a topological (qualitative) representation of the environment from the sequence of visited places. Many visual based systems for place fields based on metric information have been extensively discussed in literature (e.g. [10], [11] and [1] are just some of them). In this work, places in the environment are characterized by fingerprints of places. This characterization of the environment is especially interesting when used within a topological framework. In this case the distinctiveness of the observed location plays an important role for reliable localization and consistent mapping. A fingerprint of a place is a circular list of features, where the ordering of the set matches the relative ordering of the features around the robot. We denote the fingerprint sequence using a list of characters, where each character is the instance of a specific feature defining the signature of a place. In this work, we choose to extract color patches and vertical edges from visual information and corners (i.e. extremity of line-segments) from laser scanner. The letter cvc is used to characterize an edge, the letters cAc,cBc,cCc,...,cPc to
A. Tapus et al. / The Hippocampal Place Cells and Fingerprints of Places
107
represent hue bins and the letter ccc to characterize a corner feature (i.e. in this work, a corner feature is define as the extremity of a line-segment extracted with the DouglasPeucker algorithm). An cempty spacec between features is also denoted by the character cnc in the sequence, providing the angular distance between the features, which is some kind of very rough metric information. Figure 1 depicts how a fingerprint of a place is generated through an example. More details about the fingerprint approach can be found in [27]. With our fingerprint based-approach, the allothetic sensors are used (e.g. this choice has been made because similarly the animals are using multimodal sensory information). The fingerprints of places are integrating the information from the omnidirectional camera and the laser range finder, characterizing different places and being used to map (model) the environment. The relative angular position of the local features is also enclosed in the fingerprint of a place. A fingerprint of a place is associated to each distinctive place within the environment and so the result given by the fingerprint matching algorithm is strongly correlated to the location of the mobile agent in the environment, giving high or the highest probability to the correct place associated to the fingerprint. The firing of place cells units can be seen as the manifestation of fingerprint matching. The closer to the center of the place field the animal is, the higher the rate of neural firing.
a)
b)
c)
Figure 1: Fingerprint generation. (a) panoramic image with the vertical edges and color patches detected, denoted by cvc and cAc…cPc, respectively ; (b) laser scan with extracted corners ccc; (c) the first three images depict the position (0 to 360°) of the colors (I-light blue, B- orange and E-light green), vertical edges and corners, respectively. The forth image describes the correspondence between the vertical edge features and the corner features. By regrouping all these results together and by adding the empty space features, the final fingerprint is: cIfvnvcvfnvvncvnncvBnvBccE
108
A. Tapus et al. / The Hippocampal Place Cells and Fingerprints of Places
Similarly, the nearer the new observation of the robot (i.e. the new observed fingerprint of a place) will be with respect to the registered (learned) place (i.e. a known fingerprint of a place), the higher the probability of the mobile agent of being in an already explored place. One of the main issues in topological map building is to detect when a new node should be added in the map. Most of the existing approaches to topological mapping place nodes periodically in either space (displacement, 'd) or time ('t) or alternatively attempt to detect important changes in the environment structure. Any of these methods cannot result in an optimal topology. In contrast, our approach is based directly on the differences in the perceived features. Instead of adding a new node in the map by following some fixed rules (e.g. distance, topology) that limit the approach to indoor or outdoor environments, our method introduces a new node into the map whenever an important change in the environment occurs. This is possible using the fingerprints of places. A heuristic is applied to compare whether a new location is similar to the last one that has been mapped.
Figure 2: The spatial representation framework encodes the topological relationships between places, by comparing the actual observation (fingerprint of a place) of the mobile agent with the previously mapped places.
The process of introducing a new node in the topological map is split into several sequences of steps as follows: 1) Start with an initial node (i.e. fingerprint f0) 2) Move and at each 't (time) or 'd (distance), take a new scan with the laser scanner and a new image with the omnidirectional camera and generate the new fingerprint fi 3) Calculate the probability of matching, prob_matching, between the fingerprints fi-1 and fi respectively. Compute the dissimilarity factor, dissimilarity. prob_matching = P (fi ¸ fi-1)
A. Tapus et al. / The Hippocampal Place Cells and Fingerprints of Places
109
dissimilarity(fi , fi-1) = 1- prob_matching 4) If dissimilarity(fi , fi-1)
Figure 3: Adding a new node automatically to the topological map by moving in an unexplored environment. The image is composed of seven measurement points (i.e. fingerprints of places) represented by the black points. The blue points depict the data given by the laser range finder and they are used only for reference. The mapping system includes all the fingerprints of places in a node until a significant change in the environment occurs and the dissimilarity between the fingerprints is greater than the threshold T.
Closing the loop problem (i.e. the identification of a place previously visited, if the robot returned to it) is an important problem in SLAM. Thus, for topological maps, this means that if a place (i.e. a node) has been visited before, and the robot returns to it, the robot should detect it. Contrary to other methods used for solving this problem, based usually on the perception, in our case, loops are identified and closed with the help of the localization technique using POMDP (Partially Observable Markov Decision Processes). In order to accomplish consistency of the topological map, a method similar
110
A. Tapus et al. / The Hippocampal Place Cells and Fingerprints of Places
to the one described in [30] is used. Our method for closing the loops with fingerprints of places is detailed and fully described in [27]. Experiments were performed in a typically indoor environment (i.e. a portion of our institute building; see Figure 4). The resulting map is composed of 20 nodes. Each node is represented by a mean fingerprint which is a characterization of all the fingerprints composing the respective node. Nodes are automatically placed in locations where either a very salient landmark was present, or a door or another obstacle creates very different visual environments on its two sides (reflecting an intuitive notion of “place”). In fact, the doors of some rooms remained closed at the time of experimentation, and no node was placed in front of the respective rooms (see Figure 4). The representation obtained reproduces correctly the topological structure of the laboratory (see Figure 4). It is important to mention that the map is realized by using local features only, organized in fingerprints of places. The topological structure of the environment emerges in the map as a result of the computation, even though was not an input to the algorithm.
Raw Scan Map (for reference only)
Coffee Room
Corridor Bill’s Room Figure 4: Floor plan of the environment where the experiments have been conducted. The robot starts at the point S and ends at the point E. The trajectory length is 75 m. During this experiment, the robot collected 500 data sets (i.e. images and scans) from the environment. The extracted topological map is superimposed on an architectural sketch of the environment. The second image shows the extracted topological map given by our method, superimposed on the raw scan map.
A. Tapus et al. / The Hippocampal Place Cells and Fingerprints of Places
111
The quality of the topological maps obtained with our fingerprint-based technique can be easily estimated by testing the localization on it. To test the localization, more than 1000 fingerprint samples, acquired while the robot was traveling a new path of 250 m in the indoor environment shown above, were used to globally localize the robot. The results, obtained with the fingerprint matching algorithm (i.e. global alignment with uncertainty), have given a percentage of successful matches of 81%. However, false-classified nodes have delivered high probability (2nd or 3rd highest probability) and can be used with a POMDP (Partially Observable Markov Decision Processes). The POMDP localization improves the results obtained with the fingerprint matching approach. Adding the motion of the robot enables to decrease further the pose uncertainty to a level that could never be reached by fingerprint matching alone. A success rate of 100% was obtained for the tests performed in this work. More details about our indoor localization approach using POMDP and results can be found in [27]. The closing the loop problem has also been tested. The robot succeeded all the times to close the loops. As explained earlier, due to the fact that the offices are quite small, the fingerprints of places are very similar, and thus a single node per room is enough. Since a node contains a posterior knowledge about its environment and is the aggregation of all the fingerprints of places between the last node and the current place where an important change into the environment occurred, closing the loop problem does not appear in these cases (i.e. when one node per office is sufficient).
4. Hippocampal Place Cells as Fingerprints of Places? The method presented here can efficiently create representations of places in an environment and locate the robot/animat in the environment. The place cells in the hippocampus accomplish the same task: the activation of a place cell, or perhaps better, of an assembly of place cells connected to each other, indicates that the hippocampus is locating the animal in a certain place. It can be suggested here that the hippocampus may indeed extract place from its sensory input by constructing fingerprints of places similar to that described in this work. Indeed, in environments rich in landmarks, or features, the hippocampal cognitive map is dominated by the sensory inputs (see e.g. [19], [9], [2]). Changing the relative position of landmarks can cause a complete change in place cells activity (“remapping”) so that a new set of place cells gets an assigned to a given place, just as it would be the case for our fingerprint algorithm [6]. Many theoreticians have proposed models of place cells based on visual inputs, where the visual stream is encoded in metric terms, that is, in terms of the distances between the landmarks, and between each landmarks and the agent (e.g. [1], [10], [11]). Fingerprint representations are based on the relative angular position of the landmarks from a given point of view, a much simpler and robust measure, and may be able to explain many of the experimental evidences on place cells, at least those in which multiple landmarks were available to the animal. It is also to be remarked that some kind of metric information is contained in the fingerprint representation, through the “empty space” symbols in the fingerprint sequence, and that that information may allow the explanation of a larger class of experimental results. For the brain to perform the fingerprint matching, several building blocks are necessary: first, the identification of the landmarks, which may take place for example in the inferotemporal cortex, second, the determination of the relative position of multiple landmarks, which probably takes place in the parietal lobe ([6], [22]). The
112
A. Tapus et al. / The Hippocampal Place Cells and Fingerprints of Places
hippocampus may gather this information and produce a unitary representation (which would correspond to a fingerprint), presumably in terms of an attractor configuration of the CA3 module (which is very rich in recurrent synaptic connections and is thought to work as an attractor network module). At the moment of localization, the current input may be fed into the attractor dynamics, and, if the fingerprint matches one of the previously stored ones, the corresponding attractor is recalled. In the case of a failed match, the attractor dynamics will not produce an attractor state, and this fact may be use to signal a novel situation, and trigger the plasticity processes that allow the storage of a new memory. This vision of hippocampal space representations highlights the role of the hippocampus as a processor of combinatorial information, whose importance transcends the purely spatial domain. In the case of space computation the hippocampus would process combinations of landmark identity and relative position information, and produce an index, which can be attached to a physical location. It is important to remark here that in our scheme the place representation does not entail any notion of Euclidean space, contrarily to what hypothesized in [21] and in a number of more recent works (see review in [23]). In our view, the computation of place from sensory input (through a fingerprintlike procedure), could be integrated in the hippocampus by the idiothetic information, which plays an important role especially in conditions in which only poor sensory input is available (for example, in the dark), and to disambiguate situations of perceptual aliasing (see e.g. [25]).
5. Conclusions Here, we tried to present our research framework, underlying the interest of mutual inspiration between robotics, biology and neurophysiology. Our computational model has some foundation in neurobiology, being similar with the hippocampus, which plays a crucial role in spatial representation. In order to validate our model experimentally, we have tested it with a real autonomous mobile robot. The mobile agent continuously interacted with the environment and thereby accumulated information about its space. Thus, an incremental and dynamic navigation framework was built, allowing the mobile agent to cope with unknown situations. The proposed spatial representation is an incrementally learned representation, based on fingerprints of places; the fingerprint place modeling being comparable with the place coding model in the animals (rats) hippocampus. Acknowledgments The authors would like to thank the BIBA IST-2001-32115 EU project, which is funding this research. References [1] [2]
Arleo A. and Gerstner W., Spatial Cognition and Neuro-Mimetic Navigation: A Model of Hippocampal Place Cell Activity, Biological Cybernetics, 83:287-299, 2000. Battaglia FP, Sutherland GR, McNaughton BL (2004) Local sensory cues and place cell directionality: additional evidence of prospective coding in the hippocampus. J Neurosci 24:4541-4550.
A. Tapus et al. / The Hippocampal Place Cells and Fingerprints of Places
[3] [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] [14] [15] [16] [17] [18] [19] [20] [21] [22] [23] [24] [25] [26] [27] [28] [29] [30] [31]
113
Castellanos J.A., and Tardos J.D. (1999), Mobile Robot Localization and Map Building: Multisensor Fusion Approach, Kluwer. Cho, Y.H., Giese K.P., Tanila, H.T., Silva, A.J. and Eichenbaum, H. (1998), Abnormal hippocampal spatial representations in alphaCaMKIIT286A and CREBalphaDelta- mice, Science 279 (1998) 867869. Choset, H., and Nagatani, K.(2001), Topological Simultaneous Localization and Mapping (SLAM): Toward Exact Localization Without Explicit Localization, IEEE Trans. On Robotics and Automation, Vol 17, No.2, April. Cressant, A., Muller, R.U. and Poucet B. (2002), Remapping of place cells firing patterns after maze rotations, Exp. Brain. Res. 143, 470-9 Dissanayake, Newman, Clark, Durrant-Whyte and Csorba (2001), A Solution to the Simultaneous Localization and Map Building (SLAM) problem, IEEE Trans. On Robotics and Automation, Vol 17, No.3, June. Franz, M.O., Schölkopf, B., Mallot, H.A. and Bülthoff, Learning view graphs for robot navigation, Autonomous Robots 5 111-125, 1998. Gothard KM, Skaggs WE, Moore KM, McNaughton BL (1996b) Binding of hippocampal CA1 neural activity to multiple reference frames in a landmark-based navigation task. Journal of Neuroscience 16:823-835. Hartley T, Burgess N, Lever C, Cacucci F, O'Keefe J (2000) Modeling place fields in terms of the cortical inputs to the hippocampus. Hippocampus 10:369-379. Kali S, Dayan P (2000) The involvement of recurrent connections in area CA3 in establishing the properties of place fields: a model. J Neurosci 20:7463-7477. Kortenkamp, D. and Weymouth, T., Topological mapping for mobile robots using a combination of sonar and vision sensing, In Proceedings of AAAI-94, Seattle, WA, 1994. Kuipers, B. J., Modeling Spatial Knowledge, Cognitive Science, 2: 129-153, 1978. Kuipers, B. J., The Cognitive Map: Could it have been any other way?, In Spatial Orientation: Theory, Research and Application. Picks H.L. and Acredolo L.P. (eds.), New York. Plenum Press, 1983. Leonard J.J, H.F. Durrant-Whyte (1992), Directed Sonar Sensing for Mobile Robot Navigation, Kluwer Academic Publishers, Dordrecht. Lisien, B., et al. (2003), Hierarchical Simultaneous Localization and Mapping, In Proceedings of the IEEE/RSJ International Conference on Intelligent Robot and Systems, Las Vegas, USA, October 27-30. Mataric, M. J., Navigating with a rat brain: A neurobiologically-inspired model for robot spatial representation. In: J.A.Meyer, S.W.Wilson (Eds), From Animals to Animats, MIT Press, Cambridge, MA, 1991. Needleman, S.B., and Wunsch, C.D, A general method applicable to the search for similarities in the amino acid sequences of two proteins. Journal of Molecular Biology 48, 442-453, 1970. O'Keefe J, Burgess N (1996) Geometric determinants of the place fields of hippocampal neurons [see comments]. Nature 381:425-428. O’Keefe, J., and Dostrovsky, J., The hippocampus as a spatial map. Preliminary evidence from unit activity in the freely-moving rat. Brain Res. 34, 171-175, 1971. O’Keefe, J., and Nadel, L., The hippocampus as a cognitive map, Clarendon, Oxford, 1978. Poucet B, Lenck-Santini PP, Paz-Villagran V, Save E (2003) Place cells, neocortex and spatial navigation: a short review. J Physiol Paris 97:537-546. Redish AD (1999) Beyond the Cognitive Map: From Place Cells to Episodic Memory. Cambridge, MA: MIT Press. Se S., Lowe D. and Little, J. (2002), Mobile Robot Localization and Mapping with uncertainty using Scale-Invariant Visual Landmarks, The International Journal of Robotics Research, Vol. 21, No. 8, pp. 735-758. Skaggs WE, McNaughton BL (1998) Spatial firing properties of hippocampal CA1 populations in an environment containing two visually identical regions. Journal of Neuroscience 18:8455-8466. Tapus, A., Tomatis, N. and Siegwart, R., Topological Global Localization and Mapping with Fingerprint and Uncertainty. In Proceedings of the ISER, Singapore, June 2004. Tapus, A., Topological SLAM- Simultaneous Localization and Mapping with Fingerprints of Places. Ph.D Thesis, Ecole Polytechnique Federale de Lausanne, October 2005. Thrun, S. (1998), Learning metric-topological maps for indoor mobile robot navigation. In Artificial Intelligence 99(1):21-71. Thrun, S. (2000), Probabilistic algorithms in robotics. In Artificial Intelligence Magazine 21(4):93-109. Tomatis, N., I. Nourbakhsh, and R. Siegwart (2003). Hybrid simultaneous localization and map building: a natural integration of topological and metric. Robotics and Autonomous Systems, 44:3-14. Tolman, E. C., Cognitive maps in rats and men, Psychological Review, 55:189-208, 1948.
114
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
Incremental Reconstruction of Generalized Voronoi Diagrams on Grids Nidhi Kalra 1 , Dave Ferguson and Anthony Stentz The Robotics Institute, Carnegie Mellon University, Pittsburgh, USA Abstract. We present an incremental algorithm for constructing and reconstructing Generalized Voronoi Diagrams (GVDs) on grids. Our algorithm, Dynamic Brushfire, uses techniques from the path planning community to efficiently update GVDs when the underlying environment changes or when new information concerning the environment is received. Dynamic Brushfire is an order of magnitude more efficient than current approaches. In this paper we present the algorithm, compare it to current approaches on several experimental domains involving both simulated and real data, and demonstrate its usefulness for multirobot path planning. Keywords. Voronoi diagrams, incremental algorithms, robot navigation
1. Introduction Efficient path planning is a fundamental requirement for autonomous mobile robots. From a single robot navigating in a partially-known environment to a team of robots coordinating their movements to achieve a common goal, autonomous systems must generate effective trajectories quickly. The efficiency of path planning algorithms can be greatly affected by the type of representation used to encode the environment. Common representations include uniform and non-uniform grids, probabilistic roadmaps, generalized Voronoi diagrams (GVDs), and exact cell decompositions. GVDs in particular are very useful for extracting environment topologies. A GVD is a roadmap that provides all possible path homotopies in an environment containing obstacle regions. The GVD also provides maximum clearance from these regions. Such a representation has practical application to many robotic domains such as multirobot planning, stealthy navigation, surveillance, and area coverage. Figure 1 presents the GVD of an outdoor environment. GVDs can be used as complete representations of their environments [14,3] and to augment other representations such as probabilistic roadmaps [6,5]. Given a GVD, planning from a start position to a goal position consists of three steps. First, plan from the start to its closest point on the GVD (the access point). Second, plan along the GVD until the point closest to the goal is reached (the departure point). Then, plan from the departure point to the goal. Since the GVD is a graph, any graph search algorithm can be used to plan between the access and departure points, often at a fraction of the computational expense required to plan over the complete environment. 1 Correspondence to: Nidhi Kalra; Robotics Institute; Carnegie Mellon University; 5000 Forbes Ave, Pittsburgh, PA 15232 Tel.: +1 412 268 9923; E-mail:
[email protected]
N. Kalra et al. / Incremental Reconstruction of Generalized Voronoi Diagrams on Grids
115
Figure 1. An outdoor environment traversed by one of our John Deere eGator robotic platforms. (Left) The map generated by the robot (white is traversable area, black is untraversable, gray is unknown, and the grid has 5m spacing). (Center) The GVD constructed from this map by Dynamic Brushfire with C-space obstacle expansion. (Right) The eGator and the environment in which these maps were generated.
In many domains, robots must navigate in environments for which prior information is unavailable, incomplete, or erroneous. To harness the benefits of GVDs for planning in these environments, a robot must update its map when it receives sensor information during execution, reconstruct the GVD, and generate a new plan. GVD reconstruction and replanning must occur frequently because new information is received almost continuously from sensors. However, because new information is usually localized around the robot, much of the previous GVD remains correct and only portions require repair. Unfortunately, as we discuss in Section 2, the existing algorithms for constructing GVDs have no local reconstruction mechanism and cannot take advantage of this feature; instead, they discard the existing GVD and create a new one from scratch. This frequent full reconstruction is both computationally expensive and unnecessary. In this paper, we present the Dynamic Brushfire algorithm for incremental reconstruction of GVDs on grids. We first discuss related techniques for GVD construction. Next, we present our algorithm both intuitively and through pseudocode. We then compare this algorithm to existing algorithms on several common robot navigation scenarios on both real and simulated data. We also demonstrate the usefulness of GVDs and our algorithm in particular for coordinated multirobot path planning.
2. Related Work The Voronoi region of an obstacle O is the set of points whose distance to O is less than or equal to their distance to every other obstacle in the environment. The GVD of an environment is the intersection of two or more Voronoi regions. Each of these intersection points is equidistant from its two (or more) closest obstacles. Several methods exist for
116
N. Kalra et al. / Incremental Reconstruction of Generalized Voronoi Diagrams on Grids
computing the GVD. First, the GVD can be constructed in continuous space. For example, Nageswara et al. [12] represent the obstacles as simple polygons and geometrically construct the GVD, and Choset et al. [4] simulate an agent “tracing out” the GVD as it moves through the environment. Second, the GVD can be constructed in discrete space, e.g. on grids. In this paper we focus on GVDs constructed on grids because of the prevalence of grid-based environment representations in mobile robot navigation. Here, the Voronoi regions and the GVD are computed over the finite set of grid cells. Researchers have used graphics hardware to generate grid-based GVDs very quickly [7]; however, this is often infeasible for mobile robot platforms with limited on-board hardware. Alternatively, fast hardware-independent algorithms exist for constructing GVDs on low-dimensional grids [1,2,13]. These algorithms require as input a binary grid and a mapping from each occupied cell in the grid to the obstacle to which it belongs (the latter information helps determine the boundaries between different Voronoi regions). In general, these algorithms scan the grid and compute for each cell its closest obstacle and its distance to that obstacle; those cells that are equidistant from two or more obstacles are included in the GVD. These algorithms run in linear time O(mn) where m and n are dimensions of the grid. Thus, the computation required is a factor of the resolution of the representation rather than the complexity of the environment. The first of these is the well-known Brushfire algorithm [1]. Brushfire is analogous to Dijkstra’s algorithm for path planning in that it processes cells in an OPEN priority queue, where decreasing priority maps to increasing distance from an obstacle. Initially, each obstacle cell in the environment is placed on the queue with a priority of 0 (the cell’s distance to the nearest obstacle). Then, until the queue is empty, the highest-priority cell is removed, its neighboring cells’ distances are computed, and any cell c whose distance distc has been lowered is updated to be on the queue with priority distc . The distance distc of each cell is approximated from the distances of its neighbors: distc =
min [distance(c, a) + dista ]
a∈Adj(c)
(1)
where Adj(c) is the set of cells adjacent to c (usually 4- or 8- connected) and distance(c, a) is the distance between c and a (usually Manhattan or Euclidean distance). Brushfire only makes one pass through the grid but has the added expense of keeping a priority queue which usually requires O(log(x)) time for operations, where x is the number of elements in the queue. The quasi-Euclidean distance transform algorithm developed by Rosenfeld et al. [13] makes two sequential passes through the grid, from top to bottom and then bottom to top. For each cell encountered, it simply performs an 8-connected search of neighboring cells to determine the cell’s distance and nearest obstacle using Eq. 1. The Euclidean distance transform algorithm by Breu et al. [2] constructs the GVD by determining which Voronoi regions intersect each row in the grid. For each cell in a particular row, it computes the exact distance to the obstacle that is closest to it. Although these algorithms are fast, they provide no mechanism for incremental reconstruction. We compare their performances with Dynamic Brushfire on several example robotics scenarios in Section 4.
N. Kalra et al. / Incremental Reconstruction of Generalized Voronoi Diagrams on Grids 1
2
3
4
5
6
7
8
9
10
117
Figure 2. A sequence of distance propagations when obstacles (red/grey dots) are added and removed. (Top) A new obstacle cell in the center of the environment produces a wave of overconsistent states (in bold black) which terminates at the boundary of the new Voronoi region (in gray). (Bottom) The center cell is removed and produces a wave of underconsistent states (in bold blue/light gray) that pushes out to the corners of the old Voronoi region while an overconsistent propagation (in bold black) pulls back inwards to the boundaries of the new Voronoi regions.
3. The Dynamic Brushfire Algorithm Just as Brushfire is analogous to Dijkstra’s algorithm for planning, Dynamic Brushfire is analogous to the unfocused D* family of efficient replanning algorithms [15,10]. When new information is received concerning the environment (e.g. from a robot’s sensors), these algorithms only propagate the new information to portions of the map that could be affected. Thus, they avoid unnecessarily re-processing the entire state space. In the grid-based GVD context, new information consists of obstacles being asynchronously added and removed from the environment, in whole or in part. When an obstacle cell is removed, the distances increase for exactly those cells that were closer to it than to any other obstacle cell. When an obstacle cell is added, the distances decrease for exactly those cells that are closer to it now than to any other obstacle cell. Dynamic Brushfire is efficient because it determines and limits processing to only cells within these two sets. 3.1. Algorithm Intuition Dynamic Brushfire requires the same input as the other approaches discussed in Section 2: a grid and a mapping from obstacle cell to obstacle. For each cell s it maintains the obstacle obsts to which it is currently closest. It also maintains a distance dists to its closest obstacle from the previous GVD construction and a one-step lookahead distance distnew s to the closest obstacle given the changes that have occurred since that construction. A cell is consistent if dists = distnew , overconsistent if dists > distnew , or underconsistent s s new if dists < dists . Like A*, D*, and Brushfire, Dynamic Brushfire keeps a priority queue OPEN of the inconsistent cells to propagate changes. A cell s’s priority is always min(dists , distnew ) s and cells are popped with increasing priority values. When a cell is popped from the OPEN queue, its new distance is propagated to its adjacent cells, and any newlyinconsistent cells are put on the OPEN queue. Thus, the propagation emanates from the source of the change and terminates when the change does not affect any more cells.
118
N. Kalra et al. / Incremental Reconstruction of Generalized Voronoi Diagrams on Grids
When a cell is set to obstacle, it reduces the new minimum distance of adjacent cells which propagate this reduction to their adjacent cells, etc. This creates an overconsistent sweep emanating from the original obstacle cell (Figure 2, panels 1 to 3 ). An overconsistent sweep terminates when cells are encountered that are equally close or closer to another obstacle and thus cannot be made overconsistent. These cells lie on the boundary between Voronoi regions and will be part of the new GVD (Figure 2, panels 4 and 5). When an obstacle cell is removed, all cells that previously used it to compute their distances are invalid and must recompute their distances. This propagation occurs in two sweeps. First, an underconsistent propagation sweeps out from the original cell and resets the affected cells (Figure 2, panels 6 and 7). This sweep terminates when cells are encountered that are closer to another obstacle and cannot be made underconsistent. Thus, at most those cells in the removed obstacle cell’s Voronoi region are invalided. Then, an overconsistent propagation sweeps inwards and uses the valid cells beyond this region to recompute new values for the invalid cells (Figure 2, panels 8 and 9). 3.2. Algorithm Pseudocode Initialize () 1 OPEN =TIES = ∅; 2 foreach cell s 3 dists =distnew =∞ s 4 parents =ties =∅ 5
obsts =∅
SetObstacle (o, O) =0 1 2 obsto = O 3 parento =o distnew o
4 validO =TRUE 5 insert(OPEN , o, distnew ) o
RemoveObstacle (o, O) =∞ 1 distnew o 2 obsto =∅ 3 if O - o =∅ 4 validO =FALSE; 5 insert(OPEN , o, disto )
Figure 3. Pseudocode for initializing the grid and adding and removing obstacle cells.
Figures 3, 4, and 5 present pseudocode for the Dynamic Brushfire algorithm. In ad, and obsts , each cell s tracks the cell parents from which its dition to dists , distnew s distance is computed and a cell ties that is adjacent to s and equidistant to a different obstacle. Additionally an obstacle is invalid (validO = FALSE) when it is entirely removed from the environment. Dynamic Brushfire first initializes all distances to infinite and pointers to unknown. Then, obstacles are added and removed using SetObstacle and RemoveObstacle. When a cell o is added to obstacle O (SetObstacle(o, O)), its distance becomes zero, its parent cell is itself, and it is placed on the queue as an overconsistent cell. When a cell o is removed from obstacle O (RemoveObstacle(o, O)), its properties are reinitialized and the cell is placed on the queue as an underconsistent cell. The GVD is reconstructed by calling the function RebuildGVD which removes cells from the OPEN queue for processing. When an overconsistent cell is removed (lines 2-4), its lookahead distance will be correct, so its current distance is updated. When an underconsistent cell is removed (lines 7-10), its old distance is reset so that it can subsequently be processed as an overconsistent cell. The GVD is marked in two steps. First, when a cell s becomes consistent (line 3 in RebuildGVD), we check if it should be considered for inclusion in the GVD. In the function ConsiderForGVD, if a cell s has an adjacent cell n in a different Voronoi region, s is placed on the TIES list of possible GVD cells and n is recorded for later use. Thus, if any two adjacent cells lie in different Voronoi regions, at least one of them will be placed on the TIES list. Once the OPEN list is empty, the TIES list is examined in function ConstructGVD to determine which cells belong on the GVD using the same criterion.
N. Kalra et al. / Incremental Reconstruction of Generalized Voronoi Diagrams on Grids
RebuildGVD()
ConsiderForGVD(s) 1 foreach n ∈ Adj(s) 2 if obsts = obstn 3 ties = n
1 while s = remove(OPEN ) 2 3
119
if distnew < dists s dists = distnew ; s
4
ProcessLower(s)
5 6
ConsiderForGVD(s) else
7 8
dists =∞ if distnew = dists s
9 insert(OPEN , s, 10 ProcessRaise(s) 11 ConstructGVD()
4
insert(TIES , s)
ConstructGVD(s) 1 while s = remove(TIES )
distnew ) s
2 3
foreach n ∈ Adj(s) if obstn = obsts
4 5
n and s are on the GVD ties = n ; tien = s
6 7
else n and s are not on the GVD
8
ties = ∅ ; tien = ∅
Figure 4. Pseudocode for rebuilding the GVD. Function RebuildGVD removes cells from the OPEN queue to be processed and functions ConsiderForGVD and ConstructGVD mark the GVD. ProcessLower(s)
ProcessRaise(s)
1 foreach n ∈ Adj(s) 2 if tien =s 3 4 5
1 foreach n ∈ Adj(s) 2 if tien =s
insert (TIES , n) > distnew if distnew n s d = distance(n, s) + distnew ) s
3 4
6
if d < distnew n
5 6
7
distnew n
7
8 9
parentn = s obstn = obsts
10
=
d
insert(OPEN , n, distnew ) s
8 9 10 11 12 13
insert(TIES , n) if parentn =s new ; distnew =∞ distold n =distn n obstold n =obstn foreach a ∈ Adj(n) s.t. obsta is valid d =distance(n, a) + distnew a if d < distnew n
distnew = d ; parentn =a n obstn =obsta old if distnew = distold n n or obstn = obstn insert(OPEN , n, min(distnew , distn )) n
Figure 5. Pseudocode for propagating overconsistencies (ProcessLower) and underconsistencies (ProcessRaise).
The functions ProcessLower and ProcessRaise both update cells’ distances and place inconsistent cells on the queue. ProcessLower propagates overconsistencies: a cell examines its neighbors to see if it can lower their distances. Any changed neighboring cell has its lookahead distance, parent, and closest obstacle updated and is placed on the queue. ProcessRaise propagates underconsistencies: a cell examines its neighbors to see if any used its old distance to compute their own (line 4). For each such cell, the lookahead distance is reset and then recomputed using current values (lines 7-11). Any changed cell is placed back on the OPEN list. Additionally, both ProcessLower and ProcessRaise also find neighboring cells that were marked on the GVD because their Voronoi region was different from the current cell’s. Such cells are reinserted into the TIES list to be reexamined (lines 2-3). The termination and correctness of Dynamic Brushfire follow from the proof of D* Lite [11] and are shown in [8].
120
a
N. Kalra et al. / Incremental Reconstruction of Generalized Voronoi Diagrams on Grids
b
c
d
Figure 6. The maps used to generate results. From left to right, the complete correct map, the map with random errors, the map at lower resolution, and an illustration of an Unmanned Aerial Vehicle (UAV) updating the GVD to the correct map from the low resolution map.
4. Experiments and Results In this section we present and discuss statistical comparisons between Dynamic Brushfire and competing algorithms on several robotic scenarios. We also present results from applying Dynamic Brushfire to real robot data and to a multirobot planning problem. 4.1. Comparison to Other Algorithms We compared Dynamic Brushfire to the Brushfire, Euclidean Distance Transform, and quasi-Euclidean Distance Transform algorithms discussed in Section 2 on four scenarios. The first scenario is common to many domains and involves constructing the GVD once given a static environment. We use 200×200 cell environments that are 20% occupied by randomly placed obstacles ranging in size from 5×5 to 20×20 cells (see Figure 6 (a)). The remaining three scenarios are unique to and occur often in robotics: they require traversing the environment and repairing the GVD as new information is gathered from sensors. For this, we simulate an Unmanned Aerial Vehicle (UAV) sweeping this environment with a 10 cell omni-directional sensor and require that the UAV repair the GVD after every 10 cells of traverse. In the first of these three scenarios, the UAV has no prior map. In the second, it must repair the GVD given an erroneous prior map in which each obstacle has a 30% chance of having incorrect dimensions, of being placed incorrectly, or of being absent. An additional 10% of obstacles are randomly added (see Figure 6 (b)). Finally, the third involves repairing a GVD given a 20×20 cell low-resolution prior map (see Figure 6 (c)). Figure 6 (d) illustrates the UAV’s traverse as it incorporates new information into a prior low-resolution map. We ran each algorithm on each scenario on 100 different maps. The results from these experiments are shown in Table 1 and in Figure 7. The performance difference between Dynamic Brushfire and the other algorithms depends primarily on how much of the old computation can be preserved and how often repair must occur. This is true of most incremental repair algorithms (e.g. D* and D* Lite). Thus, in initial construction of the GVD (where prior computation does not exist), the extra processing that enables repair causes Dynamic Brushfire to be competitive to but slightly slower than the other algorithms. In the other three scenarios, Dynamic Brushfire outperforms its competitors by an order of magnitude. This improvement is most pronounced when updating from the low resolution and erroneous maps (Dynamic Brushfire is 20 and 17 times faster, respectively, than its closest competitor) because information about the environment is gained at every step and is localized around the robot. When constructing the GVD without a prior map,
121
N. Kalra et al. / Incremental Reconstruction of Generalized Voronoi Diagrams on Grids
Figure 7. A graph of the results from Table 1 on the three incremental update scenarios. From left to right in each group: Brushfire, EDT, Q-EDT, and Dynamic Brushfire. Table 1. A comparison of the performances of the four approaches on four GVD construction and repair scenarios: initial construction of a complete map (Initial), incremental construction without a prior map (Blank), incremental construction with an erroneous prior map (Error), and incremental construction with a low resolution prior map (Lowres). Each scenario was tested 100 times for each algorithm; the left column indicates the mean total time taken in seconds, and the right column indicates the standard deviation of this measure. Algorithm Brushfire
Initial
Blank
Error
Lowres
μ
σ
μ
σ
μ
σ
μ
σ
0.158
0.006
20.075
1.091
10.467
2.382
20.666
1.393
Q-EDT
0.091
0.003
12.161
0.061
5.662
1.256
11.356
0.065
EDT
0.104
0.003
11.264
0.078
7.227
1.669
15.212
1.276
DynamicBrushfire
0.162
0.005
1.887
0.174
0.328
0.051
0.551
0.068
large portions of the map have yet to be observed and the GVD is sparse. Thus, changes must be propagated over larger areas than in the other cases. As a result, the improvement is significant (Dynamic Brushfire is 6 times faster) but less than in the other scenarios. 4.2. GVD Construction on Real Data We have also tested Dynamic Brushfire on real environmental data obtained from traverses by a John Deere e-Gator robotic platform equipped with a SICK LMS laser in an outdoor wooded environment (Figure 1, top and bottom right). In one experiment, our robot explored a 75 × 40 meter area (Figure 1, left). We generated a GVD of the final data (Figure 1, center). We also repaired the GVD after each sensor scan from the laser. The roughly 2,400 cell updates over 90 repair episodes in a 375×200 cell environment took Dynamic Brushfire 2.7 seconds. For comparison, the same experiment took Brushfire 31.1 seconds. In a second experiment, we gathered sensor data from a traverse by the e-Gator platform in a similar but larger environment and then returned three weeks later for a second traverse over the same area. The first traverse provided a prior map for the second traverse. Here, repair on a prior map of size 680 ×480 cells with roughly 2,200 cell updates over 156 repair episodes took Dynamic Brushfire 11.4 seconds. For comparison, the same experiment took Brushfire 250.6 seconds. 4.3. Application to Multirobot Path Planning This work was originally motivated by the need for efficient path planning for multirobot teams. Specifically, we are interested in solving the constrained exploration problem in which a team of agents must navigate through an environment while maintaining com-
122
N. Kalra et al. / Incremental Reconstruction of Generalized Voronoi Diagrams on Grids
Figure 8. The path of two robots successfully completing a constrained exploration task from the left to the right of the environment. The paths are in bold blue/gray and bold black, obstacles are in light gray, and the GVD generated on this environment is in thin light gray. The coverage areas of the four communication towers are marked by dashed circles.
munication constraints. An instance of this problem is presented in Figure 8. Here, two robots must navigate through the environment to reach their goal locations, from the left to the right. Each robot must also always remain in contact with one of the communication towers in the environment at all times, either directly or indirectly via its teammate. Because the area of coverage of some neighboring communication towers do not overlap, the robots must coordinate to hop over the gaps. In such domains where teammates’ actions are highly interdependent, centrally planning for parts of the team can be beneficial [9]. However, algorithms for computing centralized plans typically have complexity exponential in the number of robots. While normally such planning might be intractable, it can be made feasible by reducing the search space (e.g. by planning only on the GVD). We compared planning on the GVD to planning over the entire space. In this instance, A* took only 0.36 seconds to determine a path when the search space was limited to the GVD, while it took 94.9 seconds when searching over the entire grid. We then repeated the same task but this time gave the robots an erroneous prior map of the environment. They constructed the GVD of the current map, planned a path, executed a portion of that plan while improving the map with sensor information, and then repaired the GVD and replanned again. Replanning after every five steps resulted in 74 planning and GVD construction episodes; the total planning time was 12.2 seconds and the GVD repair time using Dynamic Brushfire was 4.5 seconds. For comparison, GVD repair using Brushfire took 132.3 seconds.
5. Conclusions In this paper we have presented Dynamic Brushfire, a new algorithm for efficient, incremental reconstruction of GVDs on grids. Dynamic Brushfire operates analogously to the D* family of algorithms for path replanning: it restricts the propagation of changes in the environment to only those areas that could be affected. We have compared our algorithm to several leading approaches for constructing GVDs and found our algorithm to be significantly more efficient, particularly in typical robotics applications. We are currently using this algorithm for multirobot path planning in unknown and uncertain environments.
N. Kalra et al. / Incremental Reconstruction of Generalized Voronoi Diagrams on Grids
123
6. Acknowledgments This work was sponsored by the U.S. Army Research Laboratory, under contract “Robotics Collaborative Technology Alliance” (contract number DAAD19-01-2-0012). The views and conclusions contained in this document are those of the authors and should not be interpreted as representing the official policies or endorsements of the U.S. Government. Dave Ferguson is supported in part by an NSF Graduate Research Fellowship. References [1] Jerome Barraquand and Jean-Claude Latombe. Robot motion planning: A distributed representation approach. Technical Report STAN-CS-89-1257, Computer Science Department, Stanford University, May 1989. [2] Heinz Breu, Joseph Gil, David Kirkpatrick, and Michael Werman. Linear time euclidean distance transform algorithms. IEEE Transactions on Pattern Analysis and Machine Intelligence, 17:529–533, May 1995. [3] Howie Choset and Joel Burdick. Sensor based planning, part I: The generalized voronoi graph. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 1995. [4] Howie Choset, Sean Walker, Kunnayut Eiamsa-Ard, and Joel Burdick. Sensor-based exploration: Incremental construction of the hierarchical generalized voronoi graph. The International Journal of Robotics Research, 19(2):126 – 148, February 2000. [5] Mark Foskey, Maxim Garber, Ming Lin, and Dinesh Manocha. A voronoi-based hybrid motion planner. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2001. [6] Leonidas Guibas, Christopher Holleman, and Lydia Kavraki. A probabilistic roadmap planner for flexible objects with a workspace medial-axis-based sampling approach. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 1999. [7] Kenneth Hoff, Tim Culver, John Keyser, Ming Lin, and Dinesh Manocha. Fast computation of generalized voronoi diagrams using graphics hardware. In SIGGRAPH ’99: Proceedings of the 26th annual conference on Computer graphics and interactive techniques, 1999. [8] Nidhi Kalra, Dave Ferguson, and Anthony Stentz. The dynamic brushfire algorithm. Technical Report CMU-RI-TR-05-37, Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, September 2005. [9] Nidhi Kalra, Dave Ferguson, and Anthony Stentz. Hoplites: A market-based framework for complex tight coordination in multi-robot teams. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2005. [10] Sven Koenig and Maxim Likhachev. D* lite. In Proceedings of the National Conference on Artificial Intelligence (AAAI), 2002. [11] Maxim Likhachev and Sven Koenig. Lifelong Planning A* and Dynamic A* Lite: The proofs. Technical report, College of Computing, Georgia Institute of Technology, 2001. [12] Nageswara Rao, Neal Stoltzfus, and S. Sitharama Iyengar. A "retraction" method for learned navigation in unknown terrains for a circular robot. IEEE Transactions on Robotics and Automation, 7(5), October 1991. [13] Azriel Rosenfeld and John Pfaltz. Sequential operations in digital picture processing. Journal of the Association for Computing Machinery, 13(4), 1966. [14] Peter Forbes Rowat. Representing spatial experience and solving spatial problems in a simulated robot environment. PhD thesis, University of British Columbia, 1979. [15] Anthony Stentz. Optimal and efficient path planning for partially-known environments. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 1994.
124
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
Learning from Nature to Build Intelligent Autonomous Robots Rainer BISCHOFF and Volker GRAEFE Intelligent Robots Laboratory, Bundeswehr University Munich, Germany Abstract. Information processing within autonomous robots should follow a biomimetic approach. In contrast to traditional approaches that make intensive use of accurate measurements, numerical models and control theory, the proposed biomimetic approach favors the concepts of perception, situation, skill and behavior – concepts that are used to describe human and animal behavior as well. Sensing should primarily be based on those senses that have proved their effectiveness in nature, such as vision, tactile sensing and hearing. Furthermore, human-robot communication should mimic dialogues between humans. It should be situation-dependent, multimodal and primarily based on spoken natural language and gestures. Applying these biomimetic concepts to the design of our robots led to adaptable, dependable and human-friendly behavior, which was proved in several short- and long-term experiments. Keywords. Bionics, robot system architecture, situation, skill, behavior
1. Introduction Nature has created highly complex, efficient and dependable systems in the form of organisms since the very beginning of life on earth. Design and function of organisms have been optimized under evolutionary pressure over billions of years, a small step at a time. It is, therefore, an attractive idea to apply nature’s solutions to today’s engineering problems and, specifically, to the creation of robot intelligence. A first step is to base a robot’s sensor modalities, world modeling, control and communication with humans primarily on those principles that proved their effectiveness in nature. This biomimetic approach to the realization of robot intelligence differs from the traditional one in many respects. Table 1 gives an overview. 2. Human information processing Especially if a robot, such as a personal robot, is supposed to work closely together with humans in human-populated environments, its information processing design might benefit from understanding the human one and from employing similar concepts. An overall qualitative model of human performance was proposed by Rasmussen [1]. In his model he classified three typical levels of performance of skilled human operators: skill-, rule-, and knowledge-based performance (Figure 1). According to Rasmussen, a skill-based behavior represents „sensory-motor performance during acts or activities which [...] take place without conscious control as smooth, automated, and highly integrated patterns of behavior”. Performance is mostly based on feedforward control which depends upon a very flexible and efficient dynamic internal world model. Handwriting and sports are examples of such well coordinated rapid movements.
R. Bischoff and V. Graefe / Learning from Nature to Build Intelligent Autonomous Robots
125
Table 1. Summary of biomimetic vs. traditional approaches to robot intelligence Category
Traditional Approach
Biomimetic Approach
Sensor modalities
often only single modality
multi-modal, combination of:
often range sensors: laser range finder, sonar, radar, depth from stereo vision, motion, …
exteroceptors: vision, audition, touch, olfaction, gustation
proprioceptors: angle encoders, strain gauges, bodily acceleration sensors, kinesthetic sensors …
interoceptors: internal temperature measurements, battery gauging, …
Modeling
Robot control
seemingly suitable for constructing numerical models, e.g., 3-D surface reconstruction
suitable for situation assessment
global coordinates
local coordinates (if any)
accurate geometrical maps
attributed topological maps
detailed numerical models of the robot and the external world
qualitative models of relevant aspects of a vaguely known and ever-changing world
complete knowledge of all robot and world parameters
largely qualitative internal model of the robot’s situation
accurate and complete calibration of sensors, actuators, kinematics etc.
no or intrinsic calibration
accurate measurements
largely qualitative perception
control theory
highly adaptive calibration-free control situation-dependent behavior selection and execution
Communication
exact trajectory teaching
situation- and context-depending dialogues resembling inter-personal communication
artificial, well-structured (usually written) command language
spoken natural language augmented by gestures and visual and haptic events
commands and status reports based on coordinates
commands and status reports based on current situation and perceivable objects
Goal
Knowledge-Based Behavior Symbols
Identification
Decision of Task
Planning
Recognition
Association State / Task
Stored Rules for Tasks
Rule-Based Behavior Signs
Skill-Based Behavior Feature Formation
Sensory Input
(Signs)
Automated Sensori-Motor Patterns
Signals Actions
Fig. 1. Human information processing. Simplified model illustrating the three levels of performance of skilled human operators (Rasmussen [1]).
126
R. Bischoff and V. Graefe / Learning from Nature to Build Intelligent Autonomous Robots
At the next level – rule-based behavior – human performance is goal-oriented, but structured through stored rules (procedures). These rules may have been obtained through instructions, derived from experience or deduced by conscious problemsolving. Rules are typically defined as „if-then-else” clauses, and operators would experience them as stereotype acts. Rasmussen makes a distinction between skill- and rule-based behavior by the level of training and attention a person requires to perform a certain task. Whereas skill-based performance does not necessitate a person’s conscious attention, rule-based performance is generally based on explicit know-how. At the highest conceptual level human performance is goal-controlled and knowledge-based. It requires a mental model of the system or task enabling the person to develop and predict different plans to reach a certain goal. Rasmussen’s model is well suited to describe a typical service task, such as serving a drink, in terms of the required knowledge, rules, skills, symbols, signs and signals. It is interesting to see that both a human and a robotic servant could apply the same model to decompose a task into a set of elementary action patterns. Supposed that the servant would know how to serve a drink, he would probably apply the following rules: grasp bottle, grasp glass, fill glass from bottle, approach table, place glass onto table Each rule would in turn be composed of elementary sensorimotor skills, e.g.: grasp bottle := locate object bottle, open hand, move hand towards object, close hand around object Each sensorimotor skill would require signals (i.e., continuous sensory input) or a sign (to start some automated sensorimotor skill). The completion of a rule or sensorimotor skill is indicated by a sign representing a specific system state. The rule „grasp bottle” could thus be terminated by the sign „hand closed around the object”, the termination sign of the rule „approach table” could be, e.g., „docking position reached”. Knowledge-based operations are necessary when a set of rules does not yet exist to describe a specific task or when an unfamiliar situation occurs. For example, imagine that the servant experiences unexpected difficulties in opening a closed bottle. To handle this exception it is required to devise a plan and probably a set of rules allowing the removal of the cork (or any other stopper) before being able to fill the glass. On this knowledge-based level, sensory inputs are interpreted as symbols that are related to concepts and functional properties, i.e., problem solving requires knowledge about the process („when the stopper has been removed, the filling can begin”).
3. Robot Information Processing So far, basically three paradigms for organizing robot information processing have emerged: hierarchical (sequential), reactive (parallel), and hybrid. As a combination of hierarchical and reactive, the hybrid approach is meant to combine the best of both worlds. It allows the implementation of both purely reactive behaviors (= reflexive behaviors) and more cognitively challenging (computationally more expensive) functions to a varying degree. Although quite a number of hybrid architectures have been developed (see [2] for an overview) it is still an open research question how to organize and coordinate the reactive and deliberative parts of the system. As part of our solution to this question, we introduced a situation-oriented skill-based system architecture [3] that – according to our biomimetic approach – favors perception over measurement and situation-based calibration-free control over accurate modeling and classical control
R. Bischoff and V. Graefe / Learning from Nature to Build Intelligent Autonomous Robots
127
theory. This architecture is based on the concepts of situation, perception, behavior and skill, which are briefly explained in the sequel and illustrated in Figure 2. According to the classical approach, robot control is model-based and depends on a continuous stream of accurately measured data. Numerical models of the kinematics and dynamics of the robot and of the external objects that the robot should interact with, as well as quantitative sensor models, are the basis for controlling the robot’s motions. The main advantage of model-based control is that it lends itself to the application of classical control theory and, thus, may be considered a straight-forward approach. The weak point of the approach is that it breaks down when there is no accurate quantitative agreement between reality and the models, which is all too often the case. Organisms, on the other hand, have the ability to continuously cope with, and to survive in, the complex, dynamic and unpredictable real world. They are robust and adapt easily to changes of their own conditions and of the environment. They never need any calibration, and they normally do not know the values of any parameters related to the characteristics of their „sensors” or „actuators”. Obviously, they do not suffer from the shortcomings of model-based control which leads us to the assumption that they use something other than measurements and numerical models for controlling their behaviors. Perhaps their behavior control is based on a qualitative and holistic assessment of situations and the selection of actions to execute from an existing repertoire on that basis. We define a robot’s (internal) situation as the set of all decisive factors that should ideally be considered by a robot in selecting the correct action at a given moment. These decisive factors include: the goals of the robot, i.e., permanent goals, usually internal goals of the robot, such as survival, and satisfaction of desires, intentions and plans; and transient goals emerging from the actual mission; the state of the robot (state of motion, state of actuators and sensors, presently executed behaviors, focus of attention of the perceptual system,...); the state of the robot’s environment, i.e., perceivable objects in the environment and their suspected or recognized states; and static characteristics of the environment, even if they currently cannot be perceived by the robot’s sensors; the repertoire of available skills and knowledge of the robot’s abilities to change the present situation in a desired way by executing appropriate skills. Main basis for the recognition of situations are perceptions that integrate sensory impressions of events in the external world. Perceptions help the robot to become aware of something through its senses. Generally speaking, perception plays a similar role in the biomimetic approach as measurement does in the traditional one. In the proposed system architecture situations provide the framework for the set of the robot skills. A skill is a goal-directed, well organized ability that is inbuilt, can be acquired and improved through learning, and is performed with economy of effort (derived from psychological literature [4]). Whereas a skill describes functional underpinnings of a robot and is implemented as a specific software module, a behavior is an observable activity. Skills may thus be the basis for behaviors. It should be noted that a robot may show a certain behavior not only based on the execution of skills, but also based on purely reactive responses to external or internal stimulations, e.g., reflexes. Figure 2 (left) illustrates our definition of the term „situation” by embedding it in the perception-action cycle of a situation-oriented skill-based robot. As defined above, the robot’s image of the situation emerges at any given moment by combining internal and external goals, knowledge and percepts.
128
R. Bischoff and V. Graefe / Learning from Nature to Build Intelligent Autonomous Robots
Knowledge Base Storage
Situation Assessment & Skill Management
kills
Pr oc e
Com mu nic
ills Sk
skill execution
kills
rS
reflexive shortcut
&
knowledge (internal actions)
actuation
situation assessment
S ing
lls Ski
situation
ss
ive at
interaction
perception
supervision
or Sens
sensation
external goals
Data
internal goals
ROBOT
MMI Peripherals
oto
HUMAN
disturbances
skill management
M
ENVIRONMENT
Sen so
(external actions)
Sensors
r imotor Skil
ls
Actuators
disturbances
Fig. 2. Biomimetic robot information processing. Left: Perception-action cycle of a situation-oriented skillbased robot in interaction with its environment (and a human) based on the key concepts of perception, situation, and skill. Right: System architecture of a personal robotic assistant based on those key concepts.
4. Implementation Due to space limitations the actual implementation of the described fundamental concepts can only be sketched; for details cf. [3]. Figure 2 (right) shows the essence of the situation-oriented skill-based robot architecture as we have implemented it. The situation module (situation assessment & skill management) acts as the deliberator of the whole system. It is interfaced via skills in a bidirectional way with all hardware components – sensors, actuators, knowledge base storage and MMI peripherals (manmachine and machine-machine interface peripherals). Skills have direct access to the hardware components and, thus, let the robot sense, think (plan) and act. They obtain certain information, e.g., sensor readings, generate specific outputs, e.g., arm movements or speech, or plan a route based on map knowledge. Skills report to the situation module via events and messages on a cyclic or interruptive basis to enable a continuous and timely situation update and error handling. Whereas some skills require real-time connections to and from the situation module, e.g., sensor skills, non-real-time performance is sometimes allowed, e.g., for data base maintenance and route planning. The situation module fuses data and information from all system components to make situation assessment and skill management possible. As such, it provides cognitive skills, i.e., it is responsible for planning an appropriate skill sequence to reach a given goal. By activating and deactivating the inbuilt skills, the situation-dependent concatenation of skills is realized that leads to complex and elaborate robot behavior. Motor skills control simple movements of the robot’s actuators. They can be arbitrarily combined to yield a basis for more complex control commands. Encapsulating the access to groups of cooperating actuators leads to a simple interface structure and allows an easy generation of pre-programmed motion patterns. Sensor skills encapsulate the access to one or more sensors and provide the situation module with proprio-, extero- and interoceptive data. Sensorimotor skills combine both sensor and motor skills to yield sensor-guided robot motions. Data processing skills organize and access the system’s knowledge bases. They return specific information upon request and modify existing, or add newly gained, knowledge to the data bases. Various types of knowledge bases are being used, including an attributed topological map for storing the static characteristics of the environment, an object data base, a subject data base and a list of missions to accomplish. Communication takes place via various different channels (acoustic, optical, tactile) and requires a variety of Communicative skills to pre-
R. Bischoff and V. Graefe / Learning from Nature to Build Intelligent Autonomous Robots
129
process input from humans and to generate a valuable feedback for them according to the current situation and the given application scenario. They depend on various sensor skills to receive the required information, e.g., hearing skills for understanding speech, vision skills, and even tactile skills, to allow situated dialogues with references to visible and tangible objects and subjects in the world. When comparing Rasmussen’s model of human information processing (cf. Fig. 1) with our model of robot information processing (cf. Fig. 2) the three blocks „feature formation”, „recognition” and „identification” are realized by sensor skills, and „automated sensor-motor patterns” are mostly realized by motor and sensorimotor skills. The cognitive skills provided by the situation module correspond to the three blocks „decision of task”, „planning” and „association state/task”. Part of „planning” and „stored rules for task” are realized by data processing skills. As communicative skills involve the entire information processing system they cannot be assigned to an individual block of Figure 1. In our implementation all tasks and threads of the robot’s software system run asynchronously, but can nevertheless be synchronized via messages or events. Overall control is realized as a finite state automaton that is capable of responding to prioritized interrupts and messages. A finite state automaton has been preferred over other possibilities because of its simplicity. Motor skills are mostly implemented at the microcontroller level within the actuator modules. The other skills are implemented on a network of digital signal processors that is physically embedded in a PC.
5. Real-world experiments Over the years four of our robots have been equipped with different variants of the described system architecture and brought into short- and long-term field trials: two autonomous vehicles (ATHENE I and II), a stationary manipulator and a humanoid robot (HERMES). All robots were endowed from the start with certain basic skills, and were able to incrementally extend their knowledge and skills through learning. Calibration-free navigation. Two different vision-guided mobile robots were used as experimental platforms for learning to navigate a network of corridors by concatenating simple vision- and odometry-based sensorimotor skills: HERMES, a humanoid personal robotic assistant, and the older ATHENE II, a transportation robot with tricycle wheel configuration. HERMES has a unique wheel configuration with two independently driven and steered wheels mounted at the front and the back, and two caster wheels arranged at the centers of the left and right sides of a quadratic undercarriage allowing many different drive modes (differential, car-like, fork-lift, omnidirectional) [5]. While ATHENE II is equipped with a monochrome video camera mounted on a 1-DOF platform steerable around its vertical axis, HERMES’ main sensors are two monochrome video cameras mounted on independent pan/tilt drive units, in addition to a pan/tilt unit that controls the common „head” platform. Various standard lenses (from wide angle to tele) and a Suematsu lens were used during the experiments. It should be explicitly noted that it would have been difficult and time-consuming to measure all relevant kinematic parameters of such versatile robots with any degree of accuracy and to design model-based controllers for all possible modes of operation. Our calibration-free approach makes it unnecessary. For testing our calibration-free navigation concept we have equipped both robots with reliable, robust and sufficiently fast dynamic vision systems [5]. They yield the
130
R. Bischoff and V. Graefe / Learning from Nature to Build Intelligent Autonomous Robots
required data, such as the position of a guideline that is typically detected at the intersections of the floor with the walls despite adverse conditions, such as specular reflections and difficult illumination conditions, in general at video field rate (i.e., 50 Hz). In the beginning of the actual driving experiments HERMES and ATHENE II were placed somewhere in a corridor. Their exact orientation and position were unknown, but a section of the guideline had to be in the field of view. Corridor navigation was possible and worked fine in a tilt angle range from 0° (horizontal) to about 50° (looking downward). Negative tilt angles (i.e., looking upward) were not tested, but could have worked as long as the guideline was visible. Based on their navigation skills map building and delivery missions were successfully performed (Fig. 3. left). Calibration-free manipulation. Manipulation experiments were first carried out with the vision-guided stationary manipulator (Fig. 3, middle). The cameras are attached to the robot on metal rods at the first link so that they rotate around the axis of joint 1 together with the arm. They are mounted in a rather unstable way to make the impossibility of any calibration or precise adjustment obvious, and to allow easy random modifications of the camera arrangement. Objects of various shapes could be grasped, although no knowledge regarding the optical or kinematic parameters of the robot was used. Even arbitrary unknown rotations and shifts of the cameras were tolerated while the robot was operating, which demonstrated the robot’s extreme adaptability. Furthermore, it was shown that such an extreme adaptability does not necessarily have to degrade pick and place cycle times if appropriate learning schemes are used. With its two arms with 6 DOF and a two-finger gripper each and a bendable body, HERMES possesses an even more complex manipulation system which – besides grasping - allowed us to carry out human-robot interaction experiments. The biggest advantage of our skill-based approach is that it enormously reduces complexity in designing intelligent robot behaviors, such as taking/handing over objects from/to people, placing them onto other objects and gesturing in communicative acts. Communication and interaction. HERMES has been presented successfully at trade fairs, in television studios and in a museum, and at various demonstrations in our institute environment. The robot is able to deliver simple services within initially unknown environments for users that may be initially unknown, too. Services handled by the robot include, but are not limited to, transportation tasks, entertainment, and gathering and providing information about people and their living or working environment (Fig. 3, right). For example, many types of dialogues exist to cooperatively teach the robot new knowledge and to build a common reference frame for subsequent execution of service tasks. It is possible for the robot – besides learning an attributed topological map of the environment - to learn persons’ names, to learn how locations and objects are denominated by a specific person, where objects of personal and general interest are located, and how to grasp specific objects. This requires several databases and links between them in order to retrieve the required information [6]. One of the most promising results of our experiments is that the biomimetic approach seems to pay off. First, despite the complexity of the overall system the natureinspired system architecture allows the integration of newly developed skills very easily making the system scalable. Second, although we experienced drifting of system parameters in the long-term experiments due to temperature changes or simply wear of parts or aging, the robots’ performances were not affected by such drifts because our algorithms only rely on qualitatively (not quantitatively) correct information and adapt to parameter changes automatically. We found that the implemented skills worked very well not only around our laboratory, but also in other settings, despite the rather differ-
R. Bischoff and V. Graefe / Learning from Nature to Build Intelligent Autonomous Robots
131
ent appearance of objects of interest (e.g., corridors, docking stations). Third, according to the museum staff who ran our longest field trial for a period of more than 6 months (October 2001 – April 2002) in the Heinz Nixdorf MuseumsForum (HNF) in Paderborn, Germany, HERMES was one of the few robots at the show that could regularly be demonstrated in action, and among them it was considered the most intelligent and most dependable one. This statement is supported by the fact that the museum staff never called for advice once the initial setup was done.
gripper
camera
camera
object
Fig. 3. Experiments and Results. Left: Image taken by the HERMES' vision system during corridor navigation (tracking a guideline, indicating two forms of interruptions (door and junction)). Middle: Setup of stationary robot arm used for grasping (Mitsubishi Movemaster RV-M2, 5 DOF, two-finger gripper and a stereo vision system). Right: HERMES describing the way to a location of interest by means of voice and gestures.
6. Summary and conclusions A biomimetic approach to the realization of robot intelligence by studying mother nature’s best ideas and then imitating her designs and processes to solve robotic problems has been proposed. A robot information processing architecture was derived by studying Rasmussen’s model of human information processing. It has been implemented on various robots and has proved to endow robots with intelligent situation-oriented behavior. The ability to sense in a human-like way by means of vision, touch and hearing – the most powerful sensor modalities known – is enabling our robots to perceive their environments, to understand complex situations and to behave intelligently. While today’s robots are mostly strong with respect to a single functionality, e.g., navigation or manipulation, our results illustrate that many functionalities can be integrated within one single robot through a nature-inspired unifying situation-oriented skill-based system architecture. Furthermore, testing a robot in various environmental settings, both short- and long-term, with people having different needs and different intellectual, cultural and social backgrounds, is enormously beneficial for learning the lessons that will eventually enable us to build dependable personal robotic assistants. 7. References 1. 2. 3. 4. 5. 6.
Rasmussen, J. (1983): Skills, rules, and knowledge; Signals, signs, and symbols, and other distinctions in human performance models. IEEE Trans. on Systems, Man and Cybern. Vol. 13, No. 3, pp. 257-266. Arkin, R. C. (1998): Behavior-Based Robotics. MIT Press, Cambridge, MA, 1998. Bischoff, R.; Graefe, V. (2004): HERMES – a versatile Personal Robotic Assistant. Proc. of the IEEE, Spec. Issue on Human Interactive Robots for Psychological Enrichment, Vol. 92, No. 11, p. 1759-1779. Proctor, R. W.; Dutta, A. (1995): Skill Acquisition and Human Performance. In: Bourne, L. E. (ed.). Advanced Psychology Texts. SAGE Publications, Thousand Oaks. Graefe, V.; Bischoff, R. (2004): Robot Navigation without Calibration. Proceedings IEEE International Workshop on Intelligent Robots and Systems (IROS ‘04). Sendai, pp. 627-632 Bischoff, R.; Graefe. V. (2002): Dependable Multimodal Communication and Interaction with Robotic Assistants. 11th IEEE Int. Works. on Robot and Human Interactive Comm. Berlin, pp. 300-305.
This page intentionally left blank
Part 2 Tracking Control & Active Vision
This page intentionally left blank
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
135
A Laser Based Multi-Target Tracking for Mobile Robot Masafumi HASHIMOTO a1, Satoshi OGATA b, Fuminori OBA b, and Takeshi MURAYAMA c a
Faculty of Engineering, Doshisha University, Japan Faculty of Engineering, Hiroshima University, Japan c Faculty of Dentistry, Hiroshima University, Japan b
Abstract. This paper presents a method for tracking multiple moving objects with invehicle 2D laser range sensor (LRS) in a cluttered environment, where ambiguous/false measurements appear in the laser image due to observing clutters and windows, etc. Moving objects are detected from the laser image with the LRS via a heuristic rule and an occupancy grid based method. The moving objects are tracked based on Kalman filter and the assignment algorithm. A rule based track management system is embedded into the tracking system in order to improve the tracking performance. The experimental results of two people tracking validate the proposed method. Keywords. Mobile robot, Laser range sensor, Multi-target tracking, Occupancy grid method, Kalman filter, Data association, Assignment algorithm
1. Introduction Tracking (i.e., estimating position and velocity) of multiple targets (moving objects) is an important issue for achieving autonomous navigation and cooperation of mobile robots and vehicles. There has been much interest in the use of stereo vision or laser range sensor (LRS) in mobile robotics and vehicle automation [1-13]. Although a number of papers exist in tracking of multiple moving objects with the LRS in clean/crowded environments [2-4, 68, 11, 12], there are a few efforts [5, 13] in the use of the LRS in cluttered environments, where ambiguous/false measurements appear in the laser image by observing clutters, windows and so on. In this paper we present a method for tracking multiple moving objects with the LRS in cluttered environments. The tracking of multiple moving objects is achieved by two steps; target detection (finding moving objects from the laser image) and target tracking (tracking the moving objects detected). The occupancy grid method is one of effective methods for the target detection [2, 3]; another approach to find moving objects is based on the scan matching of successive laser scans [13]. In this paper we apply the occupancy grid based method for the target detection. In the occupancy grid method the cells occupied by obstacles are marked. For each new cell of occupation in the current scan the corresponding cell in the previous 1 Corresponding Author: Department of Information Systems Design, Faculty of Engineering, Doshisha University, Tatara, Kyotanabe, Kyoto, 610-0321 Japan; E-mail:
[email protected]
136
M. Hashimoto et al. / A Laser Based Multi-Target Tracking for Mobile Robot
scan is checked. If that cell is marked the cell in the current scan is considered as stationary, otherwise as moving. Most of the conventional occupancy grid based target detection are based on the measurements in a few successive scans of the LRS, and thus cluttered environments deteriorate the detection performance since ambiguous/false measurements appear in the laser image. In order to enhance the performance of the target detection in cluttered environments this paper presents an alternative method based on a heuristic rule and an occupancy grid method. The target tracking is divided into two approaches; model based approach (e.g., Kalman filter and Particle filter) [4, 6-13] and model free approach [2, 3, 5]. The model based approach makes it possible to track objects temporarily occluded by stationary/ moving objects, and thus it is more suitable than the model free approach. Data association (one-to-one matching of tracked objects and measurements) is necessary for the target tracking in multi-target environments. Many data association techniques have been addressed in the aerospace area (e.g., Joint Probabilistic Data Association Filter (JPDAF) and Multiple Hypothesis Tracker (MHT)) [15]. In this paper we apply the assignment algorithm [14] for the data association. The algorithm is simple extension of well-known Nearest Neighboring Standard Filter (NNSF) [15] for the data association in multi-target environments, and it allows the multi-target tracking in cluttered environments. Moreover in this paper a rule based track management system is presented in order to improve the tracking performance. This paper is organized as follows: In Section 2, our experimental system is overviewed. In Sections 3 and 4, methods of moving target detection and tracking, respectively, are presented. In Section 5, two experiments are conducted to validate our method, followed by conclusions.
2. Experimental System Figure 1 shows our mobile robot (a smart wheelchair) controlled with a joy-stick. It has two steered/drive wheels in its front and at rear and four passive casters at four corners. Each of the steered/drive wheels consists of a steering unit and a drive unit. A potentiometer is attached at the steering unit to sense the steering angle, and an encoder is attached at the drive unit to measure the wheel velocity. A yaw-rate gyro is attached at the robot body to sense the turn velocity accurately. These five internal sensors are exploited for determining the ego-motion of the robot with the dead reckoning. A forward-looking LRS (Sick LMS200) mounted on the robot takes the scan image, which is represented by a sequence of distance samples in the horizontal half plane; the distance samples are angularly ordered counterclockwise. The angular resolution of the LRS is 0.5deg, and then the number of distance samples are 361(=180/0.5+1) in one scan image. The scanning frequency is about 75Hz; however the performance of serial communication link imposes a restriction to about 10Hz.
M. Hashimoto et al. / A Laser Based Multi-Target Tracking for Mobile Robot
137
Laser range sensor (LRS)
Steered/drive wheel
Figure 1.
Overview of mobile robot (smart wheelchair)
Figure 2. Samples taken with LRS within an indoor environment. The symbol of open circle shows the sample point with LRS.
3. Target Detection We describe a method for finding moving objects from the scan image of the LRS based on Fig.2, where moving objects are the objects (D), (G) and (I). Detecting moving objects are achieved by the three steps; sample clustering, exclusion of distant objects (detection of nearby objects), and exclusion of stationary objects (detection of moving objects). The distance samples coming from the same object have the similar values, while those coming from different objects are significantly difference. Thus the distance samples taken with the LRS are clustered by checking the gap between two adjacent samples. Based on the sample clustering the ten objects (A) to (J) are detected as in Fig.2. It is assumed that for collision avoidance, it has interest in moving objects located in the vicinity of the robot. In order to exclude distant objects (objects located far away from the robot) from the sample clusters as many as possible, we use a heuristic rule; as shown in Fig.2 the two edge-samples related to the object (i), where i=B to I, are denoted by bsi and bei, respectively. The edge-sample related to the right object of the object (i) is denoted by be(i-1), and that related to the left object of the object (i) is done by bs(i+1). Objects that comply with bsi be ( i 1) t rbg ޓand bei bs (i1) t rbg , where rbg is a threshold, are considered as the distant objects. Based on this heuristic rule the objects (A),(C), (F), (H) and (J) are excluded, and the objects (B), (D), (E), (G) and (I) remain. It should be noted that the distant object (E) is not excluded since our exclusion algorithm is only based on the two gaps related to three adjacent sample-clusters and the relatively distant object is excluded. In our experiments described in Section 5, the threshold rbg is set at 0.3m. As shown in Fig.3 a grid map is represented in a reference coordinate frame 6 w ( Ow ; X wYw ). The LRS measurements related to the objects (B), (D), (E), (G) and (I) are mapped in 6 w , and those objects are marked on the grid map. In order to judge whether each cell on the grid map is occupied by stationary or moving objects, we count how many
138
M. Hashimoto et al. / A Laser Based Multi-Target Tracking for Mobile Robot
Yw
Robot
Ow
Xw
Figure3. Grid map; the gray cells show the field of view of the LRS.
successive scans each cell is marked in. The cell marked in more than ten successive scans is referred to as the stationary object cell. For the objects (B), (D), (E), (G) and (I) we obtain the value of D i b / a ; where a is the number of cells occupied by the object (i), where i= (B), (D), (E), (G) and (I); b is the number of the stationary object cell related to the object (i). The object that complies with D i 0.5 is decided as the moving object, and thus the moving objects (D), (G) and (I) are extracted. Finally the nearby objects (G) and (I) are merged. In our experiments the cell size is set at 0.3m x 0.3m. For mapping the laser image on the grid map we find the change of the robot position and orientation between the laser scans with the dead reckoning system. Although the dead reckoning causes the drift error over time, we use the change in a limited number of previous scans. Thus the effect of the drift error on the mapping is very small.
4. Target Tracking The position of the moving object in 6 w is denoted by ( x, y ) . The object is assumed to move at nearly constant velocity. A rate kinematics of the object motion is then given by xk
Fx k 1 G'xk 1
(1)
where x ( x, x , y, y )T . 'x ('x, 'y)T is an acceleration. The subscripts ( k 1 ) and ( k ) represent the ( k 1 )th scan and the k th scan, respectively, of the LRS. The system matrices F and G are constant. The LRS measurements related to the moving object (i.e., the distance zr and the bearing zT ) give the position in 6 w by ( z x , z y ) ( zr cos zT , zr sin zT ) . The measurement model related to the moving object is then
139
M. Hashimoto et al. / A Laser Based Multi-Target Tracking for Mobile Robot
zk
H k xk Lk pk 'z k
(2)
where z ( z x , z y )T . 'z is a measurement noise. p is the position vector of the LRS in 6 w . The system matrices are given by Hk
ª cos\ LRSk « sin\ LRSk ¬
0 sin\ LRSk 0 cos\ LRSk
0º and Lk 0»¼
ª cos\ LRSk « sin\ LRSk ¬
sin\ LRSk º cos\ LRSk »¼
\ LRS is the orientation of the LRS in 6 w . The position and orientation, p and \ LRS , are estimated with the dead reckoning system. The position and velocity of the moving object are predicted with Kalman filter [15]. As shown in Fig.4 a validation region with the radius of rVG is set around the predicted position of the tracked object. The measurement inside the validation region, which is considered to come from the tracked object, is applied for the track update with Kalman filter. In our experiments the radius of the validation region, rVG , is set at 0.5m. As in Fig.4 (a)-(c) in real world multiple measurements would exist inside a validation region; multiple tracked objects would also compete for measurements. For achieving the reliable data association (one-to-one matching of tracked objects and measurements), we exploit the assignment algorithm [14], in which a combined likelihood function relating to measurements is evaluated. We assume that the number of tracked objects, whose validation regions are overlapped partially, is N and that the number of measurements, which exist inside the validation region, is M . A binary assignment variable anm is defined such that anm =1 if the measurement m is assigned to the object n, otherwise anm =0, where m=1 to M and n=1 to N. The assignment variable is subject to the following constraints: M N
¦a
nm
m 1
N M
¦ anm n 1
½ 1ޓޓ for ޓn 1,, N , N 1,, N M ° ° ¾ 1ޓޓ for ޓm 1,, M , M 1,, M N ° ¿°
(3)
where anm =1 for n t N 1 and m d M denotes that measurements come from new moving
(a) Case 1
(b) Case 2
(c) Case 3
(d) Case 4
(e) Case 5
Figure 4. Tracking condition; the symbols of closed circle and open diamond mean the tracked object and the measurement, respectively. The circle drawn by broken line is the validation region.
140
M. Hashimoto et al. / A Laser Based Multi-Target Tracking for Mobile Robot
objects or false alarms; anm =1 for n d N and m t M 1 denotes that tracked objects are invisible; anm =1 for n t N 1 and m t M 1 is dummy. A set of assignment variables and a set of measurements in the kth scan of the LRS are denoted by a {anm : n, m 1,, N M } and Z k {z m ,k }mM 1 , respectively. The data association is achieved by finding the optimal assignment a * so that the combined likelihood function p>Z k | a @ relating to the measurements Z k might be maximized under the constraints in Eq.(3). Equivalently this can be rewritten as the following standard minimization problem: N M M N
a* arg min a
¦ ¦a n 1
c
nm nm
(4)
m 1
The cost function cnm is given by [14]
cnm
ln[ODVVG Lnm ,k|k 1 ] for n d N , m d M ° ® ln[1 OD ] ޓfor n d N , m t M 1ޓ ° for n t N 1 0 ¯
(5)
where OD is the detection probability of the tracked object, and VVG is the area of the validation region. Lnm is the likelihood function related to the measurement under the assumption that the measurement m comes from the object n . In our experiments we set OD =0.9. In cluttered environments ambiguous/false measurements may cause incorrect extraction of stationary and moving objects, and then false alarms occur (i.e., false moving objects are detected). Moreover in real world moving objects appear in and disappear from the field of view of LRS. They also meet the occlusion, crossing, merging and splitting situations. In order to cope with such conditions we build a track management system based on the following rules: a) Track initiation: As shown in Fig.4 (a) and (d) the measurements that are not matched with any tracked objects are considered to come from new moving objects or false alarms. The false alarms would disappear soon. From this reason the robot tentatively initiates tracking of the measurements with Kalman filter. If the measurements are constantly visible in more than 13 successive scans of the LRS, they are decided to come from new moving objects, and the tracking is continued. If the measurements disappear within 12 scans, they are decided as the false alarms, and the tentative tracking is terminated. b) Track termination: When the tracked objects leave the field of view of the LRS or they are occluded by other objects/obstacles in environments, no measurements exist within their validation regions as shown in Fig.4 (e). If no measurements arise from the temporal occlusion the measurements would appear again. The robot thus predicts the positions of the tracked objects with Kalman filter. If the measurements appear again within 30 scans of the LRS, the robot proceeds with tracking the objects. Otherwise the tracking is terminated.
M. Hashimoto et al. / A Laser Based Multi-Target Tracking for Mobile Robot
141
c) Merging and crossing: Merging or crossing of moving objects brings a situation as shown in Fig.4(b). In the crossing situation, however, new measurements would appear soon within the validation regions. If new measurements appear again within eight scans of the LRS the objects are considered as crossing, otherwise as merging.
5. Experimental Results
We conducted an experiment to evaluate the proposed method; two people walking around were tracked in a room environment as shown in Fig.5. They always exist in the field of view of the LRS, and they cross many times. Their waking speed is between 0.3 and 1.2m/s. The LRS is set on the robot at a height of 0.47m from the floor, and thus it observes around knees of the people. Figure 6 shows the experimental results; the subfigure (a) plots all the samples taken with the LRS in the experiment. (b) shows the result of detecting moving objects. (c) shows the tracking result, where black and gray bold lines show the tracks of two people, and the black dotted line shows the track of the robot. The LRS observes many kinds of stationary objects in the room environment (e.g., walls, desks, chairs, boxes, cables, and pipes), and ambiguous measurements appear. The measurements cause many false moving objects. However the tracking system works well, and thus only the two walking people are tracked as in Fig. 6(c). We conducted another experiment; two people walking around were tracked in room/corridor environments as shown in Fig.7. The room environment is the same as that conducted in the above experiment. The left side of the corridor is windows, while the right side is walls and doors as shown in Fig.7; the windows disturb the laser image due to the laser transmission/diffusion, and thus many spurious measurements appear at the window side of the corridor as in Fig.8 (a). However our tracking system achieves the people tracking correctly as in Fig.8(c).
6. Conclusions
In this paper we presented a laser based multi-target tracking in cluttered environments where ambiguous/spurious measurements are visible in the laser image. The moving objects were detected based on a heuristic rule and an occupancy grid based method. The detected objects were tracked based on Kalman filter and the assignment algorithm. In order to enhance the tracking performance a rule based track management method was incorporated into the tracking system. Two experimental results validated our method. The method proposed in this paper allows detecting and tracking only the small moving objects (e.g., walking people). Current research effort is directed toward detecting and tracking moving objects with a wide range of size (e.g., mobile robots and vehicles as well as walking people).
142
M. Hashimoto et al. / A Laser Based Multi-Target Tracking for Mobile Robot
Figure 5. Experimental environment
(a) Laser image with LRS
(b) Result of detecting moving object
(c) Result of tracking moving object
Figure 6. Experimental result; in the subfigure (c) black-dotted line, black-bold line and thin-bold line show the tracks of the mobile robot and two people, respectively.
M. Hashimoto et al. / A Laser Based Multi-Target Tracking for Mobile Robot
143
Figure 7. Experimental environment
(a) Laser image with LRS
(b) Result of detecting moving object
(c) Result of tracking moving object Figure 8. Experimental result; in the subfigure (c) black-dotted line, black-bold line and thin-bold line show the tracks of the mobile robot and two people, respectively.
144
M. Hashimoto et al. / A Laser Based Multi-Target Tracking for Mobile Robot
Acknowledgement This research was supported by the Academic Frontier Research Project on "New Frontier of Biomedical Engineering Research" of Ministry of Education, Culture, Sports, Science and Technology.
References [1] H.Koyasu, J.Miura, and Y.Shirai: Realtime Omnidirectional Stereo for Obstacle Detection and Tracking in Dynamic Environment, Proc. of the 2001 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS2001), CD-ROM (2001) [2] E.Prassler, J.Scholz and A.Elfes: Tracking Multiple Moving Objects for Real-Time Robot Navigation, Autonomous Robots, Vol.8, pp.105-116 (2000) [3] E.Prassler, J.Scholz and P.Fiorini: A Robotic Wheelchair for Crowded Public Environment, IEEE Robotics and Automation Magazine, pp.38-45 (2001) [4] B.Kluge, C.Kohler and E.Prassler: Fast and Robust Tracking of Multiple Moving Objects with a Laser Range Finder, Proc. of the 2001 IEEE Int. Conf. on Robotics & Automation (ICRA 2001), pp.1683-1688 (2001) [5] M.Lindstrom and J.-O.Eklundh: Detecting and Tracking Moving Objects from a Mobile Platform Using a Laser Range Scanner, Proc. of the 2001 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS2001), CD-ROM (2001) [6] D.Schulz, W.Burgard, D.Fox, and A.B.Cremers: People Tracking with Mobile Robot using Sample-based Joint Probabilistic Data Association Filters, The Int. J. of Robotics Research, pp.99-115 (2003) [7] A.Fod, A.Howard, and M.J Mataric: A Laser-Based People Tracker, Proc. of the 2002 IEEE Int. Conf. on Robotics & Automation (ICRA 2002), pp.3024-3029 (2002) [8] O.Frank, J.Nieto, J.Guivant, and S.Scheding: Multiple Target Tracking Using Sequential Monte Carlo Methods and Statistical Data Association, Proc. of the 2003 IEEE Int. Conf. on Robotics & Automation (ICRA 2003), pp.2718-2723 (2003) [9] C.Hue, J-P.Le Cadre, and P.Perez: Tracking Multiple objects with Particle Filtering, IEEE Trans. on Aerospace and Electronic Systems, Vol.38, No.3, pp.791-811 (2003) [10] D.Caveney, B.Feldman, and J.K.Hedrick: Comprehensive Framework for Multisensor Multitarget Tracking in the Adaptive Cruise Control Environment, Proc. of 6th Int. Symp on Advanced Vehicle Control (AVEC’02), pp.697-702 (2002) [11] K.Nakamura, H.Zhao, R.Shibasaki, K.Sakamoto, T.Ooga, and N.Suzukawa: Tracking Pedestrians by using Multiple Laser Range Scanners, Proc. of ISPRS Congress, vol. 35, part B4, pp.1260-1265 (2004) [12] B.Jensen, et al.: Narrative Situation Assessment for Human-Robot Interaction, Proc. of the IEEE Int. Conf. on Robotics and Automation (ICRA 2003), CD-ROM (2003) [13] C.Wang, C.Thorpe, and S.Thrun: Online simultaneous localization and mapping with detection and tracking of moving objects: theory and results from a ground vehicle in crowded urban areas, Proc. of Int. Conf. on Robotics and Automation (ICRA 2003), CD-ROM (2003) [14] K.R.Pattipati, R.L.Popp and T.Kirubarajan: Survey of Assignment Techniques for Multitarget Tracking,: Multitarget-Multisensor Tracking: Applications and Advances volume III (Edited by Y.Bar-Shalom and W.D. Blair), Artech House, Inc., pp.77-159 (2000) [15] Y.Bar-Shalom and T.E.Fortmann: Tracking and Data Association, Academic Press,Inc. (1988)
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
145
Simultaneous Environment Mapping And Mobile Target Tracking Abedallatif Baba 1 and Raja Chatila 2 LAAS-CNRS 7 avenue du Colonel Roche 31077 Toulouse Cedex 4 France Abstract. This paper presents an approach for building a dynamic environment model based on an occupancy grid using a SLAM technique, while detecting and tracking mobile objects using an Auxiliary Multiple-Model particle filter. The mobile objects, such as people, are distinguished from the fixed parts and not included in the model, and their motion is tracked. Keywords. Mobile Robot Navigation, SLAM, People tracking, Particle filter, Occupancy grid.
1. Introduction Most of the approaches addressing SLAM and environment mapping assume that the environment is static. In the case of service and personal robots interacting with people, this hypothesis is clearly incorrect. The resulting map will therefore include the moving objects, or a trace of their trajectories as phantom obstacles, and this might result in incorrect models [7]. Moreover, it is highly desirable that a robot can determines the positions and trajectories of the humans in its surroundings in order to be able to focus its attention on those which are willing to interact with it (e.g., those approaching the robot or moving in its close vicinity). For these reasons, it is necessary for the robot to detect and track of mobile objects, while it is mapping its environment. This paper presents an approach that relies on an occupancy grid to model the environment, and a particle filter to track moving objects, which are not included in the map. In the next section, we briefly overview related work. Then in section 3 we recall mapping using occupancy grids. Section 4 presents the target tracking approach and the Auxiliary Multiple-Model particle filter technique. Section 5 shows results obtained so far, then we conclude with future directions. 2. Related Work There has been recently a number of results on robot mapping and localization in populated environments. Most approaches suppose a known map, and focus on distinguishing 1 E-mail: 2 E-mail:
[email protected] [email protected]
146
A. Baba and R. Chatila / Simultaneous Environment Mapping and Mobile Target Tracking
mobile objects from the static map. For example, Burgard et al. [4] update a static map using the most recent sensory input to deal with people in the environment. Montemerlo et al. [10] present an approach to simultaneous localization and people tracking using a conditional particle filter. Arras et al. [1] use line features for localization and filter range-measurements reflected by persons. Fox et al. [5] use a probabilistic technique to filter out range measurements that do not correspond to the environment model. Our technique, in contrast, does not require a given map. On the other hand, there are a variety of techniques for tracking persons, for predicting future poses of persons, or for adopting the behavior of the robot according to the information obtained about the persons in its vicinity [9,2,11]. However, these techniques do not filter the measurements corresponding to persons in order to improve the model of the environment. They suppose a known map. Our approach is tobuild the map while tracking people circulating in the environment to avoid including them in the map. Recently, Wang and Thorpe [13] addressed this issue. Their approach segments the data to extract features. Hahnel, Schulz, and Burgard. [8] presented a probabilistic approach to filtering people out of sensor data and to incorporate the results of the filtering into the mapping process. Their approach is very sound, but it needs to use scan alignment for each two successive maps to detect the dynamic objects, which may be time consuming. The approach we use in this paper doesn’t need specific features, which makes it more robust. It’s not using scan alignment neither: the environment is represented by occupancy grids, which enables to be independent of any kind of representation or features, and the grid representation is less complex to deal with that the scans themselves. Human motion is detected by comparing the successive occupancy grid models, and tracking is accomplished by a bearing only tracking approach using a particle filter.
3. Map Building and Moving Target Detection Approaches to environment mapping usually rely on appearance representations (raw data), feature-based visual or geometrical representations (e.g., 2D segments), occupancy grids, or topological graphs. All these representations have advantages and drawbacks. It is not the purpose of this paper to discuss the representation issues. We have selected for this work occupancy grid because of the simplicity of their construction. In this representation the environment is discretized in cells, and to each cell is assigned a random variable describing its probable state of being empty or occupied. We won’t detail the building of the occupancy grid here, the algorithm being widely known. See [12] for the theoretical foundations and the algorithm. Figure 1 is an example of the grid built by the robot in our lab. Robot localization is currently done by using an edge-based representation of obstacles. The occupancy grids representation enables to compare successive maps (represented by a matrix of numbers in [0,1]) to detect changes rather easily. A sudden change in occupancy state for already perceived grid cells indicates a moving object. The probability of occupancy of a given cell is indeed going to change drastically in two successive views for a mobile object, whereas for fixed objects there will be smooth changes. A comparison of consecutively built grids followed by a thresholding of the result reveal the moving object. As a first approach we are using a grid difference and thresholding. Figure 2 shows three consecutive maps with a target moving to the left .
A. Baba and R. Chatila / Simultaneous Environment Mapping and Mobile Target Tracking
147
Figure 1. Example of occupancy grid built with a 2D scanning laser range finder. The occupancy value is coded as follows: green: unknown; white: free; blue occupied. The area is 4x4m2 ; a cell is 5x5cm2
Figure 2. Occupancy grid with an object moving towards the left. The occupancy probabilities change drastically when the object moves.
4. Target Tracking Tracking people in the robot environment is not a new problem. Stated otherwise, this is a classical target tracking problem, present in a large number of applications. The Kalman filter, quite popular for SLAM, was actually devised for this application. In our context, we however need a recursive bayesian filter, which does not have the limitations of linearity nor gaussianity. As mentioned before, particle filters are a common tool for coping with this problem. The problem can be stated as follows: given several targets defined by their state (position, velocity, acceleration) measured by noisy sensors, compute at each time step the estimate of the target state. To simplify the tracking poroblem, we consider only the bearing information. In the next section, we recall the basic technique for bearing tracking. 4.1. Bearing-Only tracking The approach for bearing only tracking as developed in [6] is as follows. In the 2D plane, we suppose a target X t defined by its position and velocity. The robot state is also defined
148
A. Baba and R. Chatila / Simultaneous Environment Mapping and Mobile Target Tracking
by its position and velocity, and the relative target state vector is their difference. This is the state to be estimated. From the target state dynamic equation Xk = fk−1 , νk−1 and sensor measurements Zk = hk (Xk , wk ), where νk−1 and wk are considered to be white noises with known probability density functions and mutually independent, we want to compute the posterior P (Xk |Zk ). The target is supposed to be changing directions (maneuvring target), which is quite natural for humans in an indoors environment. Given a straight trajectory, the behavior models of the target are defined as moving straight ahead (noted 1), turning clockwise (2), and turning anti-clockwise (3). S = {1, 2, 3} denotes the set of three models. A transition probability matrix (jump Markov Model) defines the state change probability among these three behaviors: ⎛
Πab
⎞ π11 π12 π13 = ⎝ π21 π22 π23 ⎠ π31 π32 π33
where πij ≥ 0, and the sum along the lines is equal to one. 4.2. Auxiliary Multiple-Model Particle Filter The Auxiliary Sequential Importance Resampling filter (ASIR) [6], generates particles from the sample at k − 1. Each particle is assigned a weight conditioned on the current measurement. Thus, ASIR does resampling at the previous time step based on estimates that characterize the posterior of X knowing Xk−1 . The advantage is that the generated new points are in a region of high likelihood, which means a lower sensitivity to outliers if the process noise is small, which we suppose here given the dynamics of the problem. On the other hand, the Multiple-Model Particle Filter (MMPF) is a technique for non-linear filtering with switching dynamic models, which is the case of our problem. A combination of both ASIR and MMPF is the Auxiliary Multiple-Model Particle Filter which we have implemented (see [6] for the detailed algorithm).
5. Results We show in this section simulation results for the tracking approach discussed above. We have first applied the approach in the case where the dynamic target is one person moving randomly in the vicinity of a robot which is the observer. In this simulation, we have chosen the number of particles for each model of the set S in section 4, to be 100 particles, for having an good accuracy. In this case, the computation time to estimate each step of the target trajectory was 500 ms. However, a reasonable accuracy can be obtained with 10 particles only, with a computing time of 80 ms. The sampling interval is T = 0.05 s, the frequency of the laser scan being 20 Hz. The typical maneuvre target acceleration is 0.2m.s−2 . The transition probability matrix is:
A. Baba and R. Chatila / Simultaneous Environment Mapping and Mobile Target Tracking
149
Figure 3. Person tracking. The robot moves along the piecewise straight line (in green); the red circles represent the real target trajectory, the blue points are the particles.
⎛
Πab
⎞ .32 .34 .34 = ⎝ .34 .34 .32 ⎠ .34 .32 .34
Here we have chosen the probability for the target to go straight ahead π11 , less than the others, i.e., the person prefers to change directions randomly during movement, which will be more difficult to be tracked. We have also chosen probabilities π23 and π32 to be also less than the other values, because there is less chance for one person to change suddenly directions from right to left or the opposite. The robot-person geometry is shown in figure 3, where the robot and the person move in variable speed and their movements are simulated along the axes x and y by a linear model in which one variable changes randomly. Figure 4 is a zoomed part showing the more weighted particles. Figure 5 shows another run with a more complex trajectory. As shown in the figures, the results have a good accuracy (a few cm). Tracking can be done while the robot is constructing its map with the occupancy gird, which enables to detect the moving targets and to supply the tracker technique with their coordinates, and to discard them from the map, as mentioned in section 3. With a laser scanner, the occupancy grid algorithm distinguishes between the dynamic and the static cells using the occupancy probability which is related to the hit rate in the cell. Each time the algorithm detects a dynamic cell, it starts up a tracker chain for this detected cell. To reduce the number of trackers, we must however group neighboring cells moving with the same velocity and direction, considering them to be only one target. This approach can be generalized to tracking several persons moving simultaneously in the robot vicinity. Figure 6 shows two persons (targets) being tracked. The case when a moving target hides another moving target is not addressed in this paper. This might produce additional noise if the two targets are moving together.
150
A. Baba and R. Chatila / Simultaneous Environment Mapping and Mobile Target Tracking
Figure 4. Person tracking. Zoomed from figure 3. The circles (red) represent the real target trajectory, the points (in blue) are the particles. The stars (in blue) are the more weighted particles which represent the more probable predicted state of the target.
Figure 5. Person tracking. A more complex trajectory.
6. Conclusion In this paper we have presented a Bearing-Only Tracking approach to track people in the data obtained with robot sensors, while the robot is also building a map of its environment using an uncertainty grid, itself used to detect the moving targets. We have shown simulation results using MATLAB with good accuracy. The next stage is to experiment this tracking technique on the robot. Further improvements are to label the moving targets so that grid cells occupied by moving persons can be identified as such, even if the
A. Baba and R. Chatila / Simultaneous Environment Mapping and Mobile Target Tracking
151
Figure 6. Tracking two persons. The robot moves again along the piecewise straight line (in green); the red circles represent the real target trajectory, the blue points are the particles.
person comes to a stop. The purpose is to trigger an interaction with this person, and also to keep the mapping process from including in the model.
Acknowledgements The authors wish to thank the anonymous reviewers for their valuable comments.
References [1] K.O. Arras, R. Philippsen, M. de Battista, M. Schilt, and R. Siegwart. A navigation framework for multiple, mobile robots and its applications at the Expo.02 exhibition. In Proc. of the IROS-2002 Workshop on Robots in Exhibitions, 2002. [2] M. Bennewitz,W. Burgard, and S. Thrun. Using EM to learn motion behaviors of persons with mobile robots. In Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2002. [3] D. Beymer and Konolige K. Tracking people from a mobile platform. In IJCAI-2001 Workshop on Reasoning, with Uncertainty in Robotics, 2001. [4] W. Burgard, A.B. Cremers, D. Fox, D. Hähnel, G. Lakemeyer, D. Schulz, W. Steiner, and S. Thrun. Experiences, with an interactive museum tour-guide robot. Artificial Intelligence, 114(1-2), 2000. [5] D. Fox, W. Burgard, and S. Thrun. Markov localization for mobile robots in dynamic environments. Journal of Artificial Intelligence Research (JAIR), 11:391-427, 1999. [6] B. Ristic, S. Arulampalam, N. Gordon. Beyond the Kalman filter, particle filter for tracking applications. Artech House 2004. [7] D. Hähnel, D. Schulz, and W. Burgard. Mapping with mobile robots in populated environments. In Proc. Of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2002. [8] D. Hähnel, R. Triebel, W. Burgard, S. Thrun. Map Building with Mobile Robots in Dynamic Environments. In Proc. of the IEEE International Conference on Robotics and Automation (ICRA), 2003.
152
A. Baba and R. Chatila / Simultaneous Environment Mapping and Mobile Target Tracking
[9] ]B. Kluge, C. Koehler, and E. Prassler. Fast and robust tracking of multiple moving objects with a laser range finder. In Proc. of the IEEE International Conference on Robotics and Automation (ICRA), 2001. [10] M. Montemerlo and S. Thrun. Conditional particle filters for simultaneous mobile robot localization and people-tracking (slap). In Proc. of the IEEE International Conference on Robotics and Automation (ICRA), 2002. [11] C. Stachniss and W. Burgard. An integrated Approach To Goal-directed Obstacle Avoidance Under Dynamic Constraints For Dynamic Environments. In Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2002. [12] S Thrun. Learning occupancy grids with forward sensor models. Autonomous Robots, 15:111-127, 2003. [13] C.-C. Wang and C. E. Thorpe. Simultaneous Localization And Mapping With Detection And Tracking of Moving Objects. In Proc. of the IEEE International Conference on Robotics and Automation (ICRA), 2002.
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
153
Omnidirectional Active Vision for Evolutionary Car Driving Mototaka Suzuki a,1 , Jacob van der Blij b , and Dario Floreano a a Laboratory of Intelligent Systems, Ecole Polytechnique F´ed´erale de Lausanne (EPFL) b Artificial Intelligence Institute, University of Groningen (RuG) Abstract. We describe a set of simulations to evolve omnidirectional active vision, an artificial retina scanning over images taken via an omnidirectional camera, being applied to a car driving task. While the retina can immediately access features in any direction, it is asked to select behaviorally-relevant features so as to drive the car on the road. Neural controllers which direct both the retinal movement and the system behavior, i.e., the speed and the steering angle of the car, are tested in three different circuits and developed through artificial evolution. We show that the evolved retina moving over the omnidirectional image successfully detects the taskrelevant visual features so as to drive the car on the road. Behavioral analysis illustrates its effective strategy in algorithmic, computational, and memory resources. Keywords. Active Vision, Omnidirectional Camera, Artificial Evolution, Neural Networks, Mobile Robots
1. Introduction The omnidirectional camera is a relatively new optic device that provides a 360 degrees field of view, and it has been widely used in many practical applications including surveillance systems and robot navigation [1,2,3]. However, in most applications visual systems uniformly process the entire image, which would be computationally expensive when detailed information is required. In other cases the focus is determined for particular uses by the designers or users. In other words, the system is not allowed to freely interact with the environment and selectively choose visual features. Contrarily, all vertebrates and several insects – even those with a very large field of view – share the steerable eyes with a foveal region [4], which means that they have been forced to choose necessary information from a vast visual field at any given time so as to survive. Such a sequential and interactive process of selecting and analyzing behaviorally-relevant parts of a visual scene is called active vision [5,6,7,8]. Our previous work has demonstrated that it can also be applied to a number of real world problems [9]. In this article we explore omnidirectional active vision applied to a car driving task: Coupled with an omnidirectional camera, a square artificial retina can immediately ac1 Correspondence
to: Mototaka Suzuki, Ecole Polytechnique F´ed´erale de Lausanne, EPFL-STI-I2S-LIS, Station 11, CH-1015 Lausanne, Switzerland. Tel.: +41 21 693 7742; Fax: +41 21 693 5859; E-mail: Mototaka.Suzuki@epfl.ch; WWW homepage: http://lis.epfl.ch.
154
M. Suzuki et al. / Omnidirectional Active Vision for Evolutionary Car Driving
cess any visual feature located in any direction, which is impossible for the conventional pan-tilt camera because of the mechanical constraints. It is challenging for the artificial retina to select behaviorally-relevant features in such a broad field of view so as to drive a car on the road. Omnidirectional active vision is not biologically plausible. But as claimed in [10] it is interesting to study visual systems from a broader point of view which contains those that have never been available in biology. Also, there are several engineering applications that could benefit from omnidirectional vision. Some promising applications are discussed in section 5. A 1/10 scale model car equipped with an omnidirectional camera and three different circuits are modeled in simulation. We show that the evolved retina moving over the omnidirectional image successfully detects the task-relevant visual features so as to drive the car on the road. Behavioral analysis illustrates its effective strategy in algorithmic, computational, and memory resources. In comparison to the results obtained with a pantilt camera mounted on the same car, we show that omnidirectional active vision performs the task very robustly in spite of more difficult initial conditions.
2. Experimental Setup Figure 1 shows the real and simulated model cars as well as the views through the real and simulated omnidirectional cameras. The omnidirectional camera consists of a spherical mirror and a CCD camera. It is mounted on a 1/10 scale model car (KyoshoT M ) which has four motorized wheels. We simulated the car and the circuits using Vortex1 libraries, a commercially available software package that models gravity, mass, friction, and collisions. Additionally we used a vision software for modeling the view from the omnidirectional camera, which had originally been developed in the Swarm-bots project2 . Figure 2 shows the three circuits; ellipse, banana, and eight shaped, used in the present evolutionary experiments. An artificial retina actively moves over the omnidirectional view3 . Figure 3 illustrates the unwrapping process from the polar view to the Cartesian view and the retina overlaid on each image. In order to evaluate the performance of the omnidirectional active vision system, we also simulate a pan-tilt camera mounted on the same car and compare the results obtained in the same experimental condition. The neural network is characterized by a feedforward architecture with evolvable thresholds and discrete-time, fully recurrent connections at the output layer (Figure 4). The input layer is an artificial retina of five by five visual neurons that receive input from a gray level image of 240 by 240 pixels. The activation of a visual neuron, scaled between 0 and 1, is given by the average gray level of all pixels spanned by its own receptive field or by the gray level of a single pixel located within the receptive field. The choice between these two activation methods or filtering strategies, can be dynamically changed by one output neuron at each time step. Two proprioceptive neurons provide input information about the measured position of the retina with re1 http://www.cm-labs.com 2 http://www.swarm-bots.org/ 3 A similar approach has been taken for evolving flocking behavior of three simulated robots independently in [11], inspired by our previous work [9].
M. Suzuki et al. / Omnidirectional Active Vision for Evolutionary Car Driving
155
Figure 1. The real 1/10 scale 4WD model car (KyoshoT M ) with an omnidirectional camera mounted on the roof of the car (top left) and the simulated one (top right). The car base is 19.5 cm (W), 43 cm (L), and 13.5 cm (H). View through the real omnidirectional camera (bottom left) and one through the simulated camera (bottom right).
Figure 2. Three circuits where the robot car is tested: from left to right, ellipse, banana, and eight shaped circuits. Each circuit is designed such that an 8 m×8 m room accommodates it. The width of the roads in all circuits is 50 cm. In the present experiments the sidelines are omitted.
spect to the chassis of the car: the radial and angular coordinates for the omnidirectional camera; or the pan and tilt degrees for the pan-tilt camera. These values are in the interval [retina size/2, radius − retina size/2] pixels (retina size = 240 pixels, radius = 448 pixels in these experiments) and [0, 360] degrees for radial and angular coordinates respectively. The values for the pan-tilt camera are in the interval [−100, 100] and [−25, 25] degrees respectively. Each value is scaled in the interval [0, 1] and encoded as a proprioceptive input. A set of memory units stores the values of the output neurons at the previous sensory motor cycle step and sends them back to the output units through a set of connections, which effectively act as the recurrent connections among output units [12]. The bias unit has a constant value of −1 and its outgoing connections represent the adaptive thresholds of the output neurons [13]. Output neurons use the sigmoid activation function f (x) = 1/(1 + exp(−x)) in the range [0, 1], where x is the weighted sum of all inputs. They encode the motor commands of the active vision system and of the car for each sensory motor cycle. One neuron determines the filtering strategy used to set the activation values of visual neurons for the
156
M. Suzuki et al. / Omnidirectional Active Vision for Evolutionary Car Driving
Figure 3. Left: The polar image taken by the omnidirectional camera and the overlaid retina. Right: The corresponding unwrapped image and the retina in Cartesian coordinate.
next sensory motor cycle. Two neurons control the movement of the retina (or camera), encoded as speeds relative to the current position. The remaining two neurons encode the directional and rotational speeds of the wheels of the car. Activation values above 0.5 stand for left (directional) and forward (rotational) speeds whereas activation values below 0.5 stand for right and backward speeds, respectively. Filter VangVrad D
S
Bias
Proprioceptive neurons
Memory neurons
Visual neurons
Figure 4. The architecture is composed of a grid of visual neurons with nonoverlapping receptive fields whose activation is given by the gray level of the corresponding pixels in the image; a set of proprioceptive neurons that provide information about the movement of the retina with respect to the chassis of the car; a set of output neurons that determine at each sensory motor cycle the filtering used by visual neurons, the new angular (Vang ) and radial (Vrad ) speeds of the retina (or pan and tilt speeds of the pan-tilt camera), and the directional (D) and rotational (S) speeds of the wheels of the car; a set of memory units whose outgoing connection strengths represent recurrent connections among output units; and a bias neuron whose outgoing connection weights represent the thresholds of the output neurons. Solid arrows between neurons represent fully connected layers of weights between two layers of neurons. Dashed arrows represent 1:1 copy connections (without weights) from output units to memory units, which store the values of the output neurons at the previous sensory motor cycle step.
The neural network has 165 evolvable connections that are individually encoded on five bits in the genetic string (total length= 825). A population of 100 individuals is evolved using truncated rank-based selection with a selection rate of 0.2 (the best 20 individuals make four copies each) and elitism (two randomly chosen individuals of the population are replaced by the two best individuals of the previous generation). Onepoint crossover probability is 0.1 and bit-toggling mutation probability is 0.01 per bit.
157
M. Suzuki et al. / Omnidirectional Active Vision for Evolutionary Car Driving
3. Evolution of Neural Controllers The fitness function was designed to select cars for their ability to move straight forward as long as possible for the evaluation time of the individual. Each individual is decoded and tested for three trials, each trial lasting 500 sensory motor cycles. A trial can be truncated earlier if the operating system detects a drop of the height of the center of the car, which corresponds to going off-road. The fitness criterion F is a function of the measured speed St of the four wheels and the steering direction Dt of the front wheels: F =
E T 1 f (St , Dt , t) t=0 e=0 E × T × Smax
f (St , Dt , t) = St ×
1−
(1)
|Dt |/Dmax
(2)
where St and Dt are in the range [−8.9, 8.9] cm/sec and [−45, 45] degrees, respectively; f (St , Dt , t) = 0 if St is smaller than 0 (backward rotation); E is the number of trials (three in these experiments), T is the maximum number of sensory motor cycles per trial (500 in these experiments), T is the observed number of sensory motor cycles (for example, 34 for a robot whose trial is truncated after 34 steps if the car goes off-road). At the beginning of each trial the position and orientation of the car as well as the position of the retina within the image are randomly initialized. We performed these replications of the evolutionary run starting with different genetic populations. In all cases the fitness reached stable values in less than 20 generations (Figure 5) which correspond to successful on-road driving. The fitness values both of the best individuals and of the population average obtained with the pan-tilt camera were higher than those with the omnidirectional camera in all three circuits. Notice that the fitness can never be one because the car must steer in corners so as to not go off-road. Omnidirectional camera, Eight-shaped circuit 0.6
0.5
0.5
0.4
0.4
Fitness
Fitness
Pan-tilt camera, Eight-shaped circuit 0.6
0.3
0.3
0.2
0.2
0.1
0.1
0
0 0
5
10
Generations
15
20
0
5
10
15
20
Generations
Figure 5. Evolution of neural controllers of the car with the pan-tilt camera (left) and the omnidirectional camera (right) in the eight shaped circuits. Thick line shows the fitness values of the best individuals and thin line shows those of the population average. Each data point is the average of three evolutionary runs with different initializations of the population. Vertical lines show the standard error.
158
M. Suzuki et al. / Omnidirectional Active Vision for Evolutionary Car Driving
4. Behavioral Analysis The behavioral strategy of the best evolved car equipped with the pan-tilt camera is as follows: At the beginning of the trial the car points the camera downward and to its right (or left depending on the evolutionary run), and it steers so to maintain the edge between the road and the background within the retina. Due to the lack of space, we show only the behavioral analysis of the best individual with the omnidirectional camera evolved in the eight shaped circuit because the circuit possesses all of the geometrical characteristics which the ellipse and banana shaped circuits have. It also has an intersection which would disturb the car’s perception if simple edge detection is the strategy developed by the evolved individual, which is sufficient for driving in the banana and ellipse shaped circuits. Indeed, since the best evolved individuals with the pan-tilt camera all adopted this strategy, they were disturbed more largely at the intersection and went off-road in several trials. Our preliminary tests also confirmed that the best individuals evolved in the eight shaped circuit were general in the sense that they could drive in the other two circuits as well. Figure 6 shows the strategy of the best individual evolved in the eight shaped circuit: During the starting period the evolved car moves back and forth, and then starts going forward at full speed once the retina finds the front road arena. Another effective strategy acquired by the best individuals in other evolutionary runs is that the car moves forward very slowly until the retina finds the road area, and starts driving at full speed immediately after finding it (data not shown). Both behaviors serve to spare time for the retina to find the road area during the most critical period at the beginning of the trial. In the intersection although the perception of the car is disturbed by the crossing road, which corresponds to the slight deviation of the trajectory, the evolved car manages to find the straight road beyond the intersection by moving the retina upward in the radial direction, which corresponds to “looking farther”, and corrects its trajectory (Figure 6, right). After passing the intersection, the retina moved downward again and maintained the straight road area in sight. The rotational speeds of the wheels and the angular position of the retina did not change significantly while passing the intersection.
5. Discussion The slightly lower fitness values of the evolved individuals with the omnidirectional camera than those with the pan-tilt camera are due to two main reasons: 1) It is harder to find the front road area out of the omnidirectional camera view than out of the pantilt camera view; 2) Evolved individuals can find the area, but it takes them some time because during the road search the car does not move much. Despite this more difficult initial condition, we have shown that artificial evolution selects the individuals capable of “waiting” until the retina finds the appropriate location and of driving at full speed after that. Therefore the slightly lower fitness values of the best evolved individuals with the omnidirectional camera do not mean that those individuals are inferior to those with the pan-tilt camera. The lower fitness is caused by the waiting behavior at the beginning of each test in order to robustly start driving at full speed afterward. Such a behavior has never been observed in any evolutionary run with the pan-tilt camera. Once the road area is detected, the best evolved car with the omnidi-
M. Suzuki et al. / Omnidirectional Active Vision for Evolutionary Car Driving
159
Figure 6. An example of the evolved car with the omnidirectional camera driving in the eight shaped circuit. Top left: Motor activations of the wheel speed (thick line) and the steering angle (thin line) during the first 150 time steps. Speed values above – and below – 0.5 stand for forward – and backward – rotational speeds. Angle values are mapped from 0.5 to 1 for rotations to the left of the car and from 0.5 to 0 for rotations to the right. Dotted vertical line denotes the moment when the retina fixated its position on the front road area. Bottom left: The corresponding trajectory of the car (shown only for the first 70 time steps). Top right: The radial position of the retina. Shaded period corresponds to while passing the intersection. The angular position of the retina remained stable and did not change even while passing the intersection. Bottom right: The corresponding trajectory of the car around the intersection.
rectional camera perfectly drives as that with the pan-tilt camera does. The advantage is lower algorithmic, computational, and memory resources. For comparison with the results obtained with the pan-tilt camera, we did not implement the zooming function in the present experimental setup. However it enables the system to select visual features more specifically by choosing an appropriate resolution for each feature. Indeed, our previous work has demonstrated that in a non-panoramic view the active zooming function played a crucial role in their performance [9], which encourages us to further apply it to the current system. The current setup also allows us to lay multiple retinas within a single omnidirectional camera image so that they each specialize in detecting separate features simultaneously and perform behaviors. In real circuits, there are a number of features to which car drivers must pay attention (e.g., sidelines, signals, other cars). Indeed, [14] developed a multi-window system with a conventional non-panoramic camera for detecting such features during real car navigation, but the position and shape of each window were predetermined by the designers. Active feature selection by multiple retinas which are moving independently over the omnidirectional view may display significant advantages over an active single retina or the fixed multi-window system in several tasks, e.g., navigation
160
M. Suzuki et al. / Omnidirectional Active Vision for Evolutionary Car Driving
in dynamic, unknown environments. Further investigations must be done to validate this hypothesis. The present method may also offer a significant advantage in landmark navigation of mobile robots because of its fast, parallel searching for multiple landmarks in any direction. Instead of processing the 360 degrees field of view, the system could actively extract only small task-related regions from the visual field, which dramatically lightens the computation and the memory consumption.
6. Conclusions In this article we have explored omnidirectional active vision applied to a car driving task. The present simulations have shown that the evolved artificial retina moving over the omnidirectional view successfully detects the behaviorally-relevant feature so as to drive the car on the road. Although it costs time to find the appropriate feature in such a broad field of view during the starting period, the best evolved car overcomes the disadvantage by moving back and forth or moving slowly until the retina finds the appropriate feature. Then the car starts driving forward at full speed. The best evolved car equipped with the omnidirectional camera performs robust driving on the banana, eight, and ellipse shaped circuits in spite of the more difficult initial condition. The advantage of the present methodology is lower algorithmic, computational, and memory resources. Currently we aim to implement an active zooming function of the retinas, to generalize the neural controllers in different-featured circuits (e.g., backgrounds, sidelines, textures), and to transfer the evolved neural controller into the real car shown in Figure 1.
Acknowledgments The authors thank Giovanni C. Pettinaro for the software to model the view of the omnidirectional camera, which has originally been developed in the Swarm-bots project. Thanks also to Danesh Tarapore for enhancing the readability of this article and to two anonymous reviewers for their useful comments. MS and DF have been supported by EPFL. JB has been supported by University of Groningen.
References [1] Y. Matsumoto, K. Ikeda, M. Inaba, and H. Inoue. Visual navigation using omnidirectional view sequence. In Proceedings of International Conference on Intelligent Robots and Systems, pages 317–322, 1999. [2] L. Paletta, S. Frintrop, and J. Hertzberg. Robust localization using context in omnidirectional imaging. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 2072–2077, 2001. [3] I. Stratmann. Omnidirectional imaging and optical flow. In Proceedings of the IEEE Workshop on Omnidirectional Vision, pages 104–114. IEEE Computer Society, 2002. [4] M. F. Land and D.-E. Nilsson. Animal Eyes. Oxford University Press, Oxford, 2002. [5] R. Bajcsy. Active perception. Proceedings of the IEEE, 76:996–1005, 1988.
M. Suzuki et al. / Omnidirectional Active Vision for Evolutionary Car Driving
161
[6] J. Aloimonos, I. Weiss, and A. Bandopadhay. Active vision. International Journal of Computer Vision, 1(4):333–356, 1987. [7] J. Aloimonos. Purposive and qualitative active vision. In Proceedings of International Conference on Pattern Recognition, volume 1, pages 346–360, 1990. [8] D. H. Ballard. Animate vision. Artificial Intelligence, 48(1):57–86, 1991. [9] D. Floreano, T. Kato, D. Marocco, and E. Sauser. Coevolution of active vision and feature selection. Biological Cybernetics, 90(3):218–228, 2004. [10] C. G. Langton. Artificial life. In Artificial Life, pages 1–48. Addison-Wesley, 1989. [11] S. Lanza. Active vision in a collective robotics domain. Master’s thesis, Faculty of Engineering, Technical University of Milan, 2004. [12] J. L. Elman. Finding structure in time. Cognitive Science, 14:179–211, 1990. [13] J. Hertz, A. Krogh, and R. G. Palmer. Introduction to the theory of neural computation. Addison-Wesley, Redwood City, CA, 1991. [14] E. D. Dickmanns, B. Mysliwetz, and T. Christians. An integrated spatio-temporal approach to automatic visual guidance of autonomous vehicles. IEEE Transactions on Systems, Man, and Cybernetics, 20(6):1273–1284, 1990.
This page intentionally left blank
Part 3 Localization
This page intentionally left blank
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
165
Map of Color Histograms for Robot Navigation Takanobu KAWABE a , Tamio ARAI b , Yusuke MAEDA c,1 and Toshio MORIYA d a Nippon Telegraph and Telephone West Corp., Japan b The University of Tokyo c Yokohama National University d Hitachi, Ltd. Abstract. In this paper, we propose a method to estimate a robot position and orientation in a room by using a map of color histograms. The map of color histograms is a distribution map of colors measured by a mobile robot in multiple directions at various points. The size of the map should be strongly compressed because the memory capacity of the robot is restricted. Thus, the histograms are converted by the discrete cosine transform, and their high-frequency components are cut. Then, residual low-frequency components are stored in the map of color histograms. Robot localization is performed by matching between the histogram of an obtained image and reconstructed histograms out of the map by means of histogram intersection. We also introduce a series of estimations with the aid of odometry data, which leads to accuracy improvement. Keywords. Color histograms, robot navigation, mobile robots
1. Introduction The number of robots used for entertainment and as pets has increased dramatically in the past decade [1]. Self-localization is one of the most fundamental functions of pet robots so that it can live and perform some useful tasks at home and in the office. Compared with conventional AGV (Automated Guided Vehicle) or mobile robots, pet robots cannot move precisely. This is partly because their environment is not well structured and partly because their locomotive mechanism is unsteady and their sensing systems detect too much noise. Thus, several methods of self-localization should be introduced to ensure that the robots have robust navigation systems. Some of them might be good in a global environment, and some others might be accurate enough in small areas. One easy and robust method of localization is to utilize artificial or natural landmarks as studied in [2][3]. Operators, however, need to teach the features and positions of the landmarks [4]. Robots can take a route that is accurately taught in advance as a sequence of sensor data [5][6]. However, such a data sequence must be relatively large. 1 Correspondence
to: Yusuke MAEDA, Div. of Systems Research, Fac. of Eng., Yokohama National University, 79-5 Tokiwadai, Hodogaya-ku, Yokohama 240-8501, JAPAN; Tel.: +81 45 339 3918; Fax: +81 45 339 3918; E-mail:
[email protected].
166
T. Kawabe et al. / Map of Color Histograms for Robot Navigation
Inverse Discrete Cosine Transform
Discrete Cosine Transform
(a) Original Image
(b) Normalized Color Histogram
(c) Low Frequency Components
(d) Compressed Color Histogram
Figure 1. The process of making a compressed color histogram
In this paper, we propose a simple and robust algorithm for a small robot to localize itself by matching an obtained image onto a map of color histograms. Here, “localization” means that both the position and orientation of a robot are estimated. The proposed algorithm is expected to be applied to a pet robot such as Sony’s AIBO [7] that has the following characteristics: 1. Vision system: The robot has a conventional low-resolution color camera installed in its forehead, whose height is close to the floor level. 2. Memory capacity: The available memory for the data for navigation is severely restricted. In this section, we discussed the requirement for pet robot navigation and its solution. Our proposed method is outlined in Section 2, and color histograms are then explained in Section 3. Section 4 deals with self-localization using the similarity between two histograms. In Section 5, maps of color histograms are generated in a room environment; experiments involving self-localization are conducted, and the results are discussed. The conclusion is presented in Section 6.
2. Outline of Map of Color Histograms Let us assume that a mobile robot moves around in a room with sets of furniture, such as chairs and desks, and items such as waste-paper baskets. Since the height of the robot’s camera is lower than that of the furniture, the images are directly affected by the position of the furniture. Therefore, the making of a map of a room requires that multiple images of the room be taken from various positions in various directions. The characteristics necessary for the image map include the following: • • • •
Robustness against changes in illumination Robustness against occlusions Small memory consumption Simple and quick matching
To meet the requirement, we propose a map of color histograms. The robot can be localized based on the similarity between color histograms of an obtained image and those stored in the map. The sequence of making a compressed color histogram for the map is illustrated in Figure 1: the obtained image (a) is converted into three sets of normalized color histograms (b); each histogram is transformed into frequency components by the discrete cosine transform and only low-frequency components are stored in the map (c); when the map is used, the stored low-frequency components are transformed inversely into histograms (d).
T. Kawabe et al. / Map of Color Histograms for Robot Navigation
167
However, color histograms of images taken at different poses can be very similar. Therefore, in order to improve the accuracy of self-localization, we also use a history of color histograms and odometry.
3. Representation of Color Histograms 3.1. Normalized Color Histograms A color histogram is obtained for each color component from an image taken by a camera. Each histogram has the number of pixels in the vertical axis against the amount of the corresponding color component in the horizontal axis, as shown in Figure 2. Then, the values of each color component are divided by the number of total pixels of the image in order to normalize the histogram in terms of the image size. As described in Section 2, the color histogram must be robust against changes of the lighting conditions in both color temperature and brightness. The robustness is strongly affected by the expression of color. Thus, we compared the color space, such as RedGreen-Blue (RGB), Hue-Saturation-Intensity (HSI, or HSV or HSL), and YUV. Generally speaking, the hue in HSI has little influence on the lighting conditions. The hue, however, is not stable when the chroma of the color is low. Thus, we adopt the RGB color space for histogram generation. The average brightness of the whole image should also be normalized for the robustness. We adjust the brightness of the image by means of gamma correction before we make the map of color histograms. Let us formulate the above procedure in more detail. A normalized color histogram on each color c out of RGB, hc , is defined as follows: hc = {hc (0), hc (1), . . . , hc (N − 1)} 1 δ(ic , n) hc (n) = NI δ(ic , n) =
(1) (2)
i∈I
1
n 1 when N ≤ ic γ < 0 otherwise,
n+1 N ,
(3)
where N is the number of discrete steps for the amount of each color component (in this research, N = 256); i is one of the pixels in an image, I; NI is the number of pixels of I; 0 ≤ ic ≤ 1 is defined as the amount of color c of the pixel i. The gamma-value γ is adjusted so that the average of the brightness of all pixels of the image might be set to 0.5 as follows: 2 γ = argmin (Y (i, γ) − 0.5) , (4) γ
i∈I
where 1/γ
1/γ
Y (i, γ) = 0.299 ired + 0.587 i1/γ green + 0.114 iblue .
(5)
168
T. Kawabe et al. / Map of Color Histograms for Robot Navigation
Histogram
400 300 200 100 0
0.025
Normalized Histogram
Normalized Histogram
500
0.02 0.015 0.01 0.005 0
0
64
128 192 Brightness (unit)
0.015 0.01 0.005 0
0
Figure 2. Original Histogram
0.02
64
128 192 Brightness (unit)
0
64
128 192 Brightness (unit)
Figure 3. Normalized Histogram Figure 4. Compressed Histogram
3.2. Histogram Compression by Discrete Cosine Transform We use the discrete cosine transform (DCT) for compression of color histograms. It is a technique for converting a signal into elementary frequency components, and widely used in compression, especially, in image compression, such as JPEG or MPEG. The eigenspace method is another possible algorithm of compression; however, a very large amount of calculation is required in order to obtain an orthogonal basis. On the other hand, the DCT is easy to calculate. When the histogram is used to express the characteristics of an image, the outward appearance is more important than the detailed appearance. Moreover, the form of color histograms of a natural image usually slopes gently. Therefore, the high-frequency components of a color histogram can be ignored. Let us denote the DCT of a histogram h = {h(i)|i = 0, 1, . . . , N − 1} by H. Then, H is given by: H = {H(u)|u = 0, 1, . . . , N − 1} N −1 (2x + 1)uπ 2 H(u) = k(u) h(x) cos N 2N x=0 1 √ when u = 0, 2 k(u) = 1 otherwise.
(6) (7)
(8)
The high-frequency components of the DCT may include a considerable amount of noise in the histogram. The noise is not only caused by colors on the texture of objects but is also generated as false intermediate colors in pixels; for example, pink is often found between black and white. Therefore, only low-frequency components, H(0), . . . , H(NA − 1) are stored in the map of color histograms (NA ≤ N ). ˆ can be reconstructed by applying the inverse An approximated original histogram h discrete cosine transform (IDCT) to the low-frequency components H(0), . . . , H(NA − ˆ is given by: 1). The reconstructed color histogram, h, ˆ = h(u)|u ˆ h = 0, 1, . . . , N (9) ˆ h(u) =
NA −1 (2u + 1)xπ 2 . k(x)H(x) cos N x=0 2N
(10)
In our experiments, NA is set to one-eighth of N (i.e., NA = 32). The three figures from Figure 2 to Figure 4 show the difference among the histograms. The original color
T. Kawabe et al. / Map of Color Histograms for Robot Navigation
(a-1)
169
(a-2)
(3)
(b-1) (1) Original Images
(b-2) (2) Filtered Histogram
(3) Histogram Intersection of (a) and (b)
Figure 5. Histogram Intersection
histogram, Figure 2, changes into Figure 3 by normalization with regard to the image size and the brightness. Figure 4 is a reconstructed histogram from the low-frequency components of the DCT; it shows that the rough form of Figure 3 is maintained, although the high-frequency components of Figure 3 were removed.
4. Self-Localization Using Map of Color Histograms 4.1. Similarity Between Two Histograms The similarity between two histograms is calculated by the algorithm of the histogram intersection [8]. The intersection between the two histograms, h1 and h2 , is given by: d∩ (h1 , h2 ) =
N −1
min(h1 (i), h2 (i)),
(11)
i=0
which means the area of the overlapping portion of the two histograms. The process of the algorithm is explained in Figure 5. Two original images, (a-1) and (b-1), are converted separately into RGB histograms: each histogram is transformed and squeezed by the DCT and a low-pass filter, and then transformed inversely to a compressed histogram, as shown in (a-2) and (b-2). The intersection is computed as (3). The matching for localization is taken in the space of the histogram rather than that of the frequency. This is because the former produces more direct and reliable results. ˆ red1 , h ˆ green1 , h ˆ blue1 ) ˆ 1 = (h The similarity between two sets of RGB histograms, H ˆ ˆ ˆ ˆ and H2 = (hred2 , hgreen2 , hblue2 ), can be calculated by the summation of the histogram intersections of RGB as follows: ˆ red1 , h ˆ red2 ) + d∩ (h ˆ green1 , h ˆ green2 ) + d∩ (h ˆ blue1 , h ˆ blue2 ). (12) ˆ 1, H ˆ 2 ) = d∩ (h d∩ (H
170
T. Kawabe et al. / Map of Color Histograms for Robot Navigation
A robot can be localized based on the similarity between a set of color histograms of ˆ and each set of reconstructed histograms from the map; we can an obtained image, H, ˜ as follows: estimate the position and orientation of the robot, p, ˆ H(m)) ˆ m ˜ ← argmax d∩ (H,
(13)
p˜ ← p(m), ˜
(14)
m∈M
ˆ where M is the map of color histograms; m is an element of the map; H(m) is a set of reconstructed RGB color histograms at m; p(m) is the position and orientation corresponding to m. 4.2. Localization Accuracy Improvement Using Odometry Self-localization of a robot as described above may sometimes fail, because color histograms of images taken at different poses can be very similar. Thus, we use a history of color histograms and odometry in order to improve the localization accuracy. Let us assume that the robot moves stepwise. We denote the relative pose difference before and after the i-th movement by Δpi , which can be roughly estimated from odometry. Then, self-localization after the n-th movement can be performed as follows: pn−k (m) = p(m) −
k−1
Δpn−i
(15)
i=0
mn−k (m) ← argmin p(m ) − pn−k (m) m ∈M
m ˜ ← argmax m∈M
n
(16)
ˆ n−k , H(m ˆ wk d∩ (H n−k (m)))
(17)
p˜ ← p(m), ˜
(18)
k=0
ˆ i is a set of RGB color histograms at the i-th step; w is a discount factor to where H alleviate the effect of odometry error and 0 ≤ w ≤ 1.
5. Experiments Localization experiments were conducted to estimate the error of the proposed method. They were made in a room 6 [m] wide by 8 [m] long in the daytime. A robot installed with a CCD color camera, XC-777A by Sony, was manually moved in the room and took images. The height of the camera was 505 [mm]. The number of pixels of the camera was 768[Horizontal] by 494[Vertical], but 160[H] by 120[V] was used for processing. 5.1. Procedure The experimental procedure was as follows:
171
T. Kawabe et al. / Map of Color Histograms for Robot Navigation 0.5
600
0.4 rotational error [rad]
translational error [mm]
700
500 400 300 200
with normalization
100
without normalization
0.3 0.2 with normalization 0.1
0
without normalization
0
0
2
4
6
8
10
12
14
16
0
2
4
6
8
10
12
14
16
step
step
Figure 6. Average Translational Error
Figure 7. Average Rotational Error
1. Image Acquisition for Map of Color Histograms: First, the images were taken in the area of 2.4 [m] by 1.2 [m] in the room. The robot pose (x, y, θ) at the sampling points was as follows: x = 305i − 1220 [mm] y = 305j − 610 [mm]
(i = 0, . . . , 8) (j = 0, . . . , 4)
θ = kπ/6 [rad] (k = 0, . . . , 11). Thus, the total number of the sampling points was 9 × 5 × 12 = 540. Then, the map was defined as a list of all the quadruplets m = (x, y, θ, V), where V = {(Hred (i), Hgreen (i), Hblue (i))|i = 0, . . . , NA − 1}. Since one pixel has one byte for each RGB, 3 × 160 × 120 = 57600 bytes are necessary for an image. 2. Generation of Map of Color Histograms: Now, each image was compressed into low-frequency components of a normalized color histogram on each color of RGB by means of the algorithm given in Section 3. As a frequency component was expressed by 8 bytes of a real number and only 32 components were stored for each color in the map, the size of the frequency components was 8 × 32 × 3 = 768 bytes. It was 1.33% of its original image size, 57600 bytes. The total size of color histograms in the map was 768[B] × 540 ≈ 0.4[MB]. 3. Localization: We made one hundred trials of self-localization. The initial position and orientation of the robot at each trial was chosen at random. For simplicity, we restrict the robot pose as follows: x = 305i/2 − 1220 [mm] y = 305j/2 − 610 [mm]
(i = 0, . . . , 16) (j = 0, . . . , 8)
θ = kπ/12 [rad] (k = 0, . . . , 23). The robot was moved at random to an adjacent pose manually for each step. 5.2. Experimental Results Average translational and rotational errors of localization in 100 trials are shown in Figure 6 and Figure 7, respectively. For comparison, results without normalization (i.e.,
172
T. Kawabe et al. / Map of Color Histograms for Robot Navigation
without gamma correction for adjustment of the brightness) are also shown. We can see that normalization leads to better localization. Each of the graphs tends to decrease as the step number increases; that is the accuracy improvement by the history of color histograms and odometry. The translational and rotational errors almost converge to a certain level. The residual translational error is larger than the distance interval of sampling points, 305 [mm], while the residual rotational error is smaller than the angle interval of the sampling points, π/6 ≈ 0.52 [rad]. This is because the color histograms are more sensitive to the rotational movement of the robot than the translational one (especially in the camera direction).
6. Conclusion In this paper, we proposed an algorithm that makes it possible for a robot to localize itself with a vision sensor by using a map of color histograms. • The normalized color histogram of an image by gamma correction is used for the robustness against changes of the lighting conditions. • The map is compressed by means of the DCT of color histograms, thus retaining only the low-frequency components of the histograms. The size of the compressed map is reduced to 1.33 % of that of original images. • In the experiments in a room environment, the localization error was about 400 [mm] in translation and 0.3 [rad] in rotation with the help of odometry, when the interval of sampling points for the map building was 305 [mm] in translation and 0.52 [rad] in rotation.
References [1] H. H. Lund (2003) “Adaptive Robotics in the Entertainment Industry,” Proc. 2003 IEEE Int. Symp. on Computational Intelligence in Robotics and Automation, pp. 595–602. [2] G. Jang, S. Kim, W. Lee, I. Kweon (2003) “Robust Self-localization of Mobile Robots using Artificial and Natural Landmarks,” Proc. 2003 IEEE Int. Symp. on Computational Intelligence in Robotics and Automation, pp. 412–417. [3] V. Ayala, J. B. Hayet, F. Lerasle, M. Devy (2000) “Visual Localization of a Mobile Robot in Indoor Environments Using Planar Landmarks,” Proc. of 2000 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp. 275–280. [4] R. Ueda, T. Fukase, Y. Kobayashi, T. Arai, H. Yuasa, J. Ota (2002) “Uniform Monte Carlo Localization - Fast and Robust Self-localization Method for Mobile Robots,” Proc. of 2002 IEEE Int. Conf. on Robotics and Automation, pp. 1353–1358. [5] Y. Adachi, H. Saito, Y. Matsumoto, T. Ogasawara (2003) “Memory-based Navigation Using Data Sequence of Laser Range Finder,” Proc. 2003 IEEE Int. Symp. on Computational Intelligence in Robotics and Automation, pp. 479–484. [6] T. Matsui, H. Asoh, S. Thompson (2000) “Mobile Robot Localization Using Circular Correlations of Panoramic Images,” Proc. of 2000 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp. 269–274. [7] M. Fujita, H. Kitano (1998) “Development of an Autonomous Quadruped Robot for Robot Entertainment,” Autonomous Robots, vol.15, no.1, pp. 7–18. [8] M. J. Swain, D. H. Ballard (1991) “Color Indexing,” Int. J. on Computer Vision, vol.7, no.1, pp. 11–32.
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
173
Designing a System for Map-Based Localization in Dynamic Environments Fulvio Mastrogiovanni, Antonio Sgorbissa 1 , and Renato Zaccaria DIST - University of Genova Abstract. This paper deals with map-based self-localization in a dynamic environment. In order to detect localization features, laser rangefinders traditionally scan a plane: we hypothesize the existence of a “low frequency cross-section” of the 3D environment (informally, over people heads), where even highly dynamic environments become more “static” and “regular”. We show that, by accurately choosing the laser scanning plane, problems related to moving objects, occluded features, etc. are simplified. Experimental results showing hours of continuous work in a realworld, crowded scenario, with an accuracy of about 2.5cm and a speed of 0.6m/sec, demonstrate the feasibility of the approach. Keywords. Mobile Robotics, Map-based self-localization, System design
1. Introduction This paper deals with the self-localization subsystem of an autonomous mobile robot within a dynamic, indoor environment. It is commonly accepted that, if we want the robot to operate autonomously for a long period of time while carrying out complex navigation tasks (e.g., for autonomous trasportation, autonomous surveillance, museum tour guiding, etc. [1] [4]), self-localization is a fundamental, yet-to-be solved issue. Since incremental dead-reckoning methods are not sufficient because of cumulative errors which increase over time, a very common approach consists in collecting observations from the surrounding environment (natural or artificial features or landmarks), and to periodically update the robot pose by comparing measurements with a map of the environment. If required, each new observation can be used to update the map itself, thus estimating both the pose of the robot and each surrounding feature in the environment (this is also known as Simultaneous Localization And Mapping). In this general scenario, existing localization methods are often classified along different axes (Local Vs. Global, Active Vs. Passive, etc. [5]). In spite of their differences, almost all the existing approaches rely on the so-called Markov assumption; i.e., sensor readings are assumed to depend only on the relative configuration of the robot with respect to the surrounding, presumed static features. Obviously, this is unrealistic in realworld scenarios, where people walk all around and objects are moved frequently from place to place. However, since the assumption allows to simplify computations, most ap1 Correspondence to: Antonio Sgorbissa, DIST - University of Genova. Tel.: +39 010 353 2801; Fax: +39 010 353 2154; E-mail:
[email protected].
174
F. Mastrogiovanni et al. / Designing a System for Map-Based Localization
proaches rely on post-processing techniques to filter out data originating from unmodeled, dynamically changing features. The present work is focused on a method relying on a SICK laser rangefinder as the only sensing device in the system, and an Extended Kalman Filter for position estimate. In particular, we face problems related to the dynamism and the inherent complexity of real-world environments by introducing a new dimension, i.e. elevation from the ground. Since laser rangefinders traditionally scan a plane, we hypothesizes the existence of a 2D Low Frequency Cross-Section (informally, “over people heads”), where even highly dynamic environments become more “static” and “regular”: the attribute “low frequency” thus refers both to the temporal and the spatial dimension. The localization process can be successfully managed here, with the robot standing “on the tips of its toes”, thus decoupling position tracking from obstacle avoidance: whilst features for localization should be extracted in the LFCS, which is static and has a simpler topology, sensors for obstacle avoidance should be put lower, in order to deal with the complex and ever changing topology of a dynamic and chaotic human environment. Section II clarifies the LFCS concept, and introduces an “unevenness index” to better formalize it. In Section III the feature extraction and matching algorithms are discussed. Section IV shows experimental results. Conclusions follows.
2. The Low Frequency Cross-section Most approaches in literature make assumptions about the stationarity of the environment. People walking around the robot, objects moved from place to place, or even other moving robots evidently violate this assumption, thus making feature detection harder both as a consequence of occlusions and the fact that features themselves change significantly over short periods of time. Two approaches are widely known in literature to deal with this problem. According to the first approach, the map of the world has to be continuously updated using current observations, in order to reflect the actual state of the environment. This approach is often unfeasible, because of the complexity and the large amount of relevant features which must be taken into account. Recently, important advances in the field of Simultaneous Localization and Mapping (SLAM) [7] [8] have lead to partial solutions, in particular for those features of the environment which appear/disappear at a low frequency (e.g., furniture); however, this is an open research issue. The second approach is based on techniques to filter out sensor readings which do not have a correspondence in the a priori map. For example, in [5] such a filter is used to compute the entropy value of the state estimate, thus considering only perceptions which reduce the entropy value itself. The main drawback of the approach is its inherent conservativeness, since it is not able to incorporate new information in the map. The main purposes of this work is to introduce a novel approach to deal with complex, dynamic indoor environments. We notice that the intrinsic dynamic nature of human populated environments is not a characteristic of the environment “as a whole”, but it mainly depends on elevation from the floor. In particular, it does exist a well defined height range, roughly located over our heads, which we call the Low Frequency CrossSection: experience shows that, in such 2D cross-section of the 3D Workspace, the status of the world varies at an extremely low frequency. Consider for example the displacement
F. Mastrogiovanni et al. / Designing a System for Map-Based Localization
175
Figure 1. Bottom, mid and top view.
of objects and furniture from one place to another: in a typical office-like environment, people usually move chairs, tables, wastebaskets, boxes, PCs, books, etc., from and to places which are “close” to the floor, in order to be handy for humans; these objects are moved around on a daily-basis. Shelves and closets, which are placed at higher elevation from the floor or have a bigger size which could cover an entire wall, are displaced (at most) on a yearly-basis, i.e. a time significantly longer than the life of most current robotic set-ups. We can summarize this by saying that in LFCS the environment is more “static” than in the area below. For analogous reasons, we claim that, over a given height, the environment is more “regular” and “simple” than below. This is a consequence of the fact that humans are inherently “conservative”, i.e., they tend to minimize the energetic strain when performing a particular task. Therefore, it is more common to put down a book on a messy stack on our desktop (if we use it everyday) rather than in a well ordered (but not easily reachable) shelf located over our heads, especially if we plan to take it again after a couple of minutes. Fig. 1 shows a picture of our lab, with laser scans taken at different heights: bottom view (10 cm from the floor, quite messy); middle view (90 cm, very messy); top view (200 cm, i.e. in the LFCS). The idea of a “freely sensing area” is not new. A successful attempt is described in [6], where authors deal with the localization problem by pointing a TV camera upward, with the purpose of detecting ceiling lights and matching them against a previously stored map in memory. The main difference between LFCS and this approach is straightforward: LFCS is a general method for laser based feature extraction, which opportunistically chooses the most promising laser scanning plane but can be adopted in any indoor environment; the approach in [6] is oriented towards solving a very specific problem, thus not being portable at all. To give a formal meanings to terms such as “static”, “regular” and “simple”, we introduce an “unevenness index”; unevenness measures and the fractal degree are commonly used in geography to measure the length and complexity of shore profiles, and can be applied to quantify the regularity of a given laser scan. The unevenness index U is simply provided by the following formula: U=
L C
(1)
L in Equation 1 is the length of the polyline which is built by connecting all the raw scan points, thus not relying on a particular segmentation algorithm. C – commonly referred to as the chord of the polyline – is the length of the segment located between the first and the last scan point. It is straightforward to depict the behavior of U :
176
F. Mastrogiovanni et al. / Designing a System for Map-Based Localization
1) U is always greater than 1. This comes from the fact that the link between the start and the final point of a polyline is always shorter than the polyline itself. U could theoretically be equal to 1 in the case the polyline is made up of a single segment: in other words, when all the points are collinear. 2) U is a measure of the discontinuity between contiguous scan points. High U values are detected if range measurements vary at high frequencies between contiguous scan points. On the contrary, low U values are detected if the points are mostly colinear or their range variation is very smooth. 3) U is proportional to the depth of the environment. Long corridors are mainly characterized by high values of U , while rooms by lower values. This “not explicitly modeled” effect can be reduced, but not removed, by dividing a full scan into disjoint subsets of range measurements, and considering each polyline and the corresponding chord separately. Notice that this is similar to the approach adopted in geography, where coastal profiles are divided into “mostly convex” and “mostly concave” areas before computing the fractal degree. All the data collected confirm the intuition that, at ground level, U is higher; it significantly decreases as we move the laser to a higher elevation, i.e., corresponding to the LFCS. Here, transitions between close scan points are generally smoother, since the environment is less cluttered. Finally, the same is obviously true in a dynamic environment, crowded with people moving around the robot: U substantially increases - with respect to its reference value computed in the LFCS - when scans are taken at a lower height.
3. Self-Localization Two phase are iteratively executed, namely Acquisition and EKF-Update. 3.1. Acquisition We adopt a typical Split & Merge method [9]. This family of line extraction methods exhibits a number of advantages compared to other existing techniques [10] [11] [12] [3] [2]: they exploit local scan templates, they are faster – due to the fact they act on smaller data sets – and produce more accurate results. The required steps are: Preprocessing, Scan Segmentation, Segment Line Extraction, and Feature Matching. Preprocessing and Scan Segmentation do not deserve further discussion: they basically consist in standard techniques to filter out invalid readings, and to segment range data into “almost continuous”, non overlapping, sets of points Sj to be feeded to the next computation step. Line extraction recursively iterates on each set, trying to find local subsets strictly made up of collinear points. This is accomplished by iteratively calculating the fitting value φj of each set Sj (equations are not shown here: see [9]). If φj ≤ Φ, where Φ is the fitting threshold, the points belonging to Sj are considered collinear. If not, the algorithm splits Sj into m smaller sets and computes φjm for each of them. This possibly originates several segment lines from the original set. When a segment line lj is found, ρlj and θlj are measured: in particular, by defining Lj (with capital letter) the straight (infinite) line on which lj lies, ρlj is the distance between Lj and the robot, θlj is the angle between ρlj and the robot heading. By relying on geometrical considerations, both end points of each segment line lj are found. Finally, a set of segment lines λ such that
F. Mastrogiovanni et al. / Designing a System for Map-Based Localization
177
Figure 2. Left: The plot of U versus time in a different 5 mins tests; Right: mean and variance.
Figure 3. Left: The plot of U versus time in a 3 hours test; Right: mean and variance.
λ = lj , j = 1, ..., |λ|
(2)
is returned, ordered according to the angle θlj associated to each lj (from right to left). During Feature Matching, each line lj is compared with the segment lines which compose the environmental model. The model is a set of |μ| oriented segment lines (each defined by a start and an end point) to allow to distinguish the opposite surfaces of the same wall (or object). μ = mk , k = 1, ..., |μ| (3) In particular, different heuristic conditions must be satisfied in order to obtain a correct match; equations are not shown here for sake of brevity. As a result, each sensed line lj can virtually match against more map lines mk , thus producing a set of matching couples: M Cj = < lj , mk >, mk ∈ M In the next phase, some criteria are adopted to select the best matching map line for each sensed line. 3.2. EKF-Update Two steps are involved: Best Match Selection and Kalman Filtering. As usual, to select the most likely match in the set M Cj , we refer the current estimated robot position. In
178
F. Mastrogiovanni et al. / Designing a System for Map-Based Localization
particular, for every map line mk included in M Cj , we compute the measurement model ρ˜mk and θ˜mk (its complete expression is given in equation 6, when describing the EKF), which returns the estimated distance and angle given the current position estimate. Next, we assign a likely score lsjk to each couple < lj , mk >∈ M Cj according to Equation 4.
lsjk = Θρ ρlj − ρ˜mk 2 + Θθ θlj − θ˜mk 2 (4) In Equation 4, Θρ and Θθ are experimentally determined in order to differently weight errors with respect to the distance and to the angle; the j th measure (ρlj , θlj ) feeded to the EKF corresponds to the matching couple mcjk such that: (5) mcjk = arg max lsjk k
If more sensed line lj are available, the following step is iteratively invoked in order to process all the matching pairs. In the last step, position is corrected through an Extended Kalman Filter. An EKF is a linear recursive estimator for the state of a dynamic system described by a nonlinear process f and a measurement model h. Its good properties and limitations are well known in literature, thus not requiring to be discussed here. However, it is interesting to notice that, in our case, the measurement model h is linear, therefore partly allowing to ignore some of the problems which are related to linearization (unfortunately, the kinematic model of our differentially driven vehicle is not linear). In particular, if
T sˆr,t = x ˆr,t , yˆr,t , θˆr,t is the a priori estimate of the robot’s position at the tth instant,
T and the 2-dimensional vector z˜m ,t = ρ˜m ,t , θ˜m ,t is the predicted measure (i.e., k
k
k
corresponding to the map segment mk in the matching couple mcjk ), the measurement model is given by the following linear equations: a mk x ˆr + bmk yˆr + cmk b mk ˜ − θˆr , θmk = atan (6) ρ˜mk = amk a2mk + b2mk I.e., we express segment lines mk in the map through the implicit equation amk x + bmk y + cmk = 0 of the corresponding straight line. An observability analysis have been performed, showing that the observability space, for each measurement lj , has dimension 2; i.e., the system is not completely observable. However, since the measurement model h varies depending on the equation of the line mk which is currently considered, subsequent measurements – corresponding to different, hopefully perpendicular lines – allow to correct all the component of the robot state.
4. Experimental Results Experiments allowed to validate the intuition about a Low Frequency Cross-Section. In particular, the laser scanner has been set at three different heights from the ground: 10 cm (bottom view), 90 cm (mid view) and roughly 2 meters (top view). From these views we collected about one thousand scans in different places of our department, a house, and a garage; each scan lasts about five minutes, thus requiring to average results over time. The experiments show that bottom and mid views (Fig. 2) almost exhibit a comparable behavior for the U parameter, whilst for the top view (in darker gray) U
F. Mastrogiovanni et al. / Designing a System for Map-Based Localization
179
Figure 4. Left: simulated robot (8 hours); Middle: real robot (2 hours); Right: snapshots of the experiment
is significantly smaller. Obstacle profiles, when detected in the LFCS, are much more regular than below; moreover, the variance over time of each scan is significant in the bottom and mid view, whilst it is almost zero in the top view (small fluctuations are due to sensor noise). Experiments with a longer duration of about 2 hours, shown in Fig. 3, confirm this evidence. Exploiting these results, we performed many localization experiments, both in simulation and with two real robots (a modified TRC Labmate base and the home-built robot Staffetta). For simulation, we simulate errors both in rangefinder measurements [2] and in odometry. In Fig. 4 (left), the simulated robot is asked to navigate through an environment which replicates the ground floor of our department: the figure depicts the superimposition between the ground truth and the estimated robot position after 8 hours simulation. In Fig. 4 (middle) a similar experiment is shown with a real robot; the system proves to be able to localize itself for almost 2 hours (the maximum time allowed by batteries), even it is almost continuously surrounded by students (Fig. 4 right). Moreover, it demonstrates its robustness when moved to a different scenario, i.e.: the second floor of our department (Fig. 5). During experiments the normal life of the department is not interrupted: people are walking by, furniture are displaced etc. In spite of this, positioning have an accuracy of about 2.5 cm when robots are moving at a speed of 0.6m/sec.
5. Conclusion The paper shows that map-based self-localization within a dynamic environment can be successfully performed by opportunely designing the laser to scan a 2D plane where the environment slowly changes, both temporally and spatially (i.e., which we call a Low Frequency Cross-Section). This concept is formalized through the unevenness parameter U , which attributes a score to the “roughness” of laser scans; as expected, scans taken outside the LFCS are much less stable and regular. Localization experiments – performed both in simulation and with a real robot – demonstrate that the approach is very robust and precise even in highly crowded environments.
180
F. Mastrogiovanni et al. / Designing a System for Map-Based Localization
Figure 5. Top: simulated robot (8 hours); Bottom: real robot (2 hours)
References [1] I.J. Cox, BLANCHE - An Experiment in Guidance and Navigation of an Autonomous Robot Vehicle, IEEE Int. J. of Robotics and Automation, Vol. 7 no 2 (1991), 193–204. [2] P. Jensfelt and H. I. Christensen, Pose Tracking Using Laser Scanning and Minimalistic Environmental Models, IEEE Int. J. of Robotics and Automation, Vol. 17 no 2 (2001), 138–147 [3] S. Thrun, C. Martin, Y. Liu and, Hahnel, R. Emery-Montemerlo, D. Chakrabarti and W. Burgard, A Real-Time Expectation-Maximization Algorithm for Acquiring Multilanar Maps of Indoor Environments With Mobile Robots, IEEE Int. J. of Robotics and Automation, Vol. 20 no 3 (2004), 433–442. [4] S. Thrun, M. Beetz, M. Bennewitz, W. Burgard, A.B. Creemers, F. Dellaert, D. Fox, D. Hahnel, C. Rosenberg, N. Roy, J. Schulte and D. Schulz, Probabilistic Algorithms and the Interactive Museum Tour-Guide Robot Minerva, Int. J. of Robotics Research, Vol. 19 no 11 (2000), 972–999. [5] D. Fox, Markov Localization: A Probabilistic Framework for Mobile Robot Localization and Navigation, PhD Thesis, Institute of Computer Science III, University of Bonn (1998) [6] D. Fox, S. Thrun, W. Burgard, and F. Dellaert, Particle Filters for Mobile Robot Localization, Sequential Monte Carlo Methods in Practice, Statistics for Engineering and Information Science, Springer-Verlag, eds. A. Doucet and N. de Freitas and N. Gordon, New York (2001). [7] J. Nieto, J. Guivant, and E. Nebot, DenseSLAM: The Unidirectional Information Flow (UIF), Proc. Fifth IFAC/EURON Symp. on Intell. Autonomous Vehicles (IAV’04), Portugal (2004). [8] J. B. Folkensson and H. Christensen, Robust SLAM, Proc. Fifth IFAC/EURON Symp. on Intell. Autonomous Vehicles (IAV’04), Portugal (2004). [9] D. Sack and W. Burgard, A Comparison of Methods for Line Extraction from Range Data, Proc. Fifth IFAC/EURON Symp. on Intell. Autonomous Vehicles (IAV’04), Portugal (2004) [10] M. Bennewitz, W. Burgard, and S. Thrun, Using EM to learn motion behaviors of persons with mobile robots, Proc. of IROS’02, Lausanne, Switzerland (2002). [11] R. O. Duda and P. E. Hart, Pattern classification and scene analysis, Wiley and Sons, New York (1973). [12] Y. Liu, R. Emery, D Chakrabati, W. Burgard, S. Thrun, Using EM to learn 3D models of indoor environments with mobile robots, Proc. of the Int. Conf. on Machine Learning (2001)
Intelligent Autonomous Systems 9 T. Arai et al. (Eds.) IOS Press, 2006 © 2006 The authors. All rights reserved.
181
Appearance-based Localization of Mobile Robots using Local Integral Invariants Hashem Tamimi a , Alaa Halawani b Hans Burkhardt b and Andreas Zell a a Computer Science Dept., University of Tübingen, Sand 1, 72076 Tübingen, Germany. b Institute for Computer Science, University of Freiburg, 79110 Freiburg, Germany. Abstract. In appearance-based localization, the robot environment is implicitly represented as a database of features derived from a set of images collected at known positions in a training phase. For localization the features of the image, observed by the robot, are compared with the features stored in the database. In this paper we propose the application of the integral invariants to the robot localization problem on a local basis. First, our approach detects a set of interest points in the image using a Difference of Gaussian (DoG)-based interest point detector. Then, it finds a set of local features based on the integral invariants around each of the interest points. These features are invariant to similarity transformation (translation, rotation, and scale). Our approach proves to lead to significant localization rates and outperforms a previous work. Keywords. Appearance-based robot localization, integral invariants, (DoG)-based interest point detector.
1. Introduction Vision-based robot localization demands image features with many properties. On one hand the features should exhibit invariance to scale and rotation as well as robustness against noise and changes in illumination. On the other hand they should be extracted very quickly so as not to hinder other tasks that the robot plans to perform. Both global and local features are used to solve the robot localization problem. Global features are extracted from the image as a whole, such as histograms [18], Principal Component Analysis (PCA)-based features [3], and integral invariants [17]. On the other hand, local features are computed from areas of high relevance in the image under consideration such as Scale Invariant Feature Transform (SIFT) [6], kernel PCA-based features [16], and wavelet-based features [15]. Local features are more commonly employed because they can be computed efficiently, are resistant to partial occlusion, and are relatively insensitive to changes in viewpoint. There are two considerations when using local features [5]: First, the interest points should be localized in position and scale. Interest points are positioned at local peaks in a scale-space search, and filtered to preserve only those that are likely to remain stable over transformations. Second, a signature of the interest point is built. This signature should be distinctive and invariant over transformations caused by changes in camera pose as well as illumination changes. While point localization and signature aspects of interest point algorithms are often designed together, they can be considered independently [7].
182
H. Tamimi et al. / Appearance-Based Localization of Mobile Robots
In this paper we propose the application of the integral invariants to the robot localization problem on a local basis. First, our approach detects a set of interest points in the image based on a Difference of Gaussian (DoG)-based interest point detector developed by Lowe [6]. Then, it finds a set of descriptive features based on the integral invariants around each of the interest points. These features are invariant to similarity transformation (translation, rotation, and scale). Our approach proves to lead to significant localization rates and outperforms a previous work that is described in Section 4.
2. Integral Invariants Following is a brief description of the calculation of the rotation- and translationinvariant features based on integration. The idea of constructing invariant features is to apply a nonlinear kernel function f (I) to a gray-valued image, I, and to integrate the result over all possible rotations and translations (Haar integral over the Euclidean motion): T[ f ](I) =
2π 1 M−1 N−1 P−1 ∑ ∑ f (g(n0 , n1 , p P )I) PMN n∑ 0 =0 n1 =0 p=0
(1)
where T[ f ](I) is the invariant feature of the image, M, N are the dimensions of the image, and g is an element in the transformation group G (which consists here of rotations and translations). Bilinear interpolation is applied when the samples do not fall onto the image grid. The above equation suggests that invariant features are computed by applying a nonlinear function, f , on the neighborhood of each pixel in the image, then summing up all the results to get a single value representing the invariant feature. Using several different functions finally builds up a feature space. To preserve more local information we remove the summation over all translations. This results in a map T that has the same dimensions of I: 1 P−1 2π I (2) (T [ f ] I) (n0 , n1 ) = ∑ f g n0 , n1 , p P p=0 P Applying a set of different f ’s will result in a set of maps. A global multidimensional feature histogram is then constructed from the elements of these maps. The choice of the non-linear kernel function f can vary. For example, invariant features can be computed by applying the monomial kernel, which has the form:
P−1
f (I) =
∏ I xp , yp
1
P
(3)
p=0
One disadvantage of this type of kernels is that it is sensitive to illumination changes. The work in [10] defines another kind of kernels that are robust to illumination changes. These kernels are called relational kernel functions and have the form: f (I) = rel (I (x1 , y1 ) − I (x2 , y2 )) with the ramp function
(4)
H. Tamimi et al. / Appearance-Based Localization of Mobile Robots
183
Figure 1. Calculation of f = rel (I(0, 3) − I(4, 0)), by applying Equation 2, involves applying the function rel to the difference between the grey scale value of the pixels that lie on the circumference of circle of radius 3 and pixels that lie on the circumference of another circle of radius 4 (taking into consideration a phase shift of π 2 between the corresponding points) and taking the average of the result [17].
rel (γ ) =
⎧ ⎨1 ⎩
ε −γ 2ε
0
if γ < −ε if − ε ≤ γ ≤ ε if ε < γ
(5)
centered at the origin and 0 < ε < 1 is chosen by experiment. Global integral invariants have been successfully used for many applications such as content-based image retrieval [12], texture-classification [9], object recognition [8], and robot localization [17]. Figure 1 illustrates how these features are calculated. Please refer to [12] for detailed theory.
3. DoG-based Point Detector The interest points, which are used in our work, were first proposed as a part of the Scale Invariant Feature Transform (SIFT) developed by Lowe [6]. These features have been widely used in the robot localization field [11] [14]. The advantage of this detector is its stability under similarity transformations, illumination changes and presence of noise. The interest points are found as scale-space extrema located in the Difference of Gaussians (DoG) function, D(x, y, σ ), which can be computed from the difference of two nearby scaled images separated by a multiplicative factor k: D (x, y, σ ) = (G (x, y, kσ ) − G (x, y, σ )) ∗ I (x, y) = L (x, y, kσ ) − L (x, y, σ )
(6)
where L(x, y, σ ) defines the scale space of an image, built by convolving the image I(x, y) with the Gaussian kernel G(x, y, σ ). Points in the DoG function, which are local extrema in their own scale and one scale above and below are extracted as interest points. The interest points are then filtered for more stable matches, and more accurately localized to scale and subpixel image location using methods described in [2].
184
H. Tamimi et al. / Appearance-Based Localization of Mobile Robots
4. Using Global Integral Invariants For Robot Localization In [17], integral invariants are used to extract global features for solving the robot localization problem by applying Equation 2 to each pixel (n0 , n1 ) in the image I. The calculation of the matrix T involves finding an invariant value around each pixel in the image which is time consuming. Instead of this, Monte-Carlo approximation is used to estimate the overall calculation [13]. This approximation involves applying the nonlinear kernel functions to a set of randomly chosen locations and directions rather than to all locations and directions. Global features achieve robustness mainly because of their histogram nature. On the other hand, local features, extracted from areas of high relevance in the image under consideration, are more robust in situations where the objects in images are scaled or presented in different views [4]. Such situations are often encountered by the robot during its navigation. In the next sections we modify the global approach by applying the integral invariants locally around a set of interest points.
5. Extracting Loc