Robotics and Automation Vol 1

ICINCO 2007
Proceedings of the
Fourth International Conference on
Informatics in Control, Automation and Robotics
Volume RA-1
Angers, France
May 9 – 12, 2007
Co-organized by
INSTICC – Institute for Systems and Technologies of Information, Control
and Communication
and
University of Angers
Co-sponsored by
IFAC - International Federation of Automatic Control
GDR MACS - Groupe de Recherche “Modélisation, Analyse et Conduite
des Systèmes dynamiques
CNRS - Centre National de la Recherche Scientifique
and
EEA – Club des Enseignants en Electronique, Electrotechnique
et Automatique
In Cooperation with
AAAI – Association for the Advancement of Artificial Intelligence
I
Copyright © 2007 INSTICC – Institute for Systems and Technologies of
Information, Control and Communication
All rights reserved
Edited by Janan Zaytoon, Jean-Louis Ferrier, Juan Andrade Cetto and Joaquim Filipe
Printed in Portugal
ISBN: 978-972-8865-83-2
Depósito Legal: 257879/07
http://www.icinco.org
[email protected]
II
BRIEF CONTENTS
INVITED SPEAKERS........................................................................................................................IV
SPECIAL SESSION CHAIRS .............................................................................................................IV
ORGANIZING AND STEERING COMMITTEES .................................................................................... V
PROGRAM COMMITTEE ............................................................................................................... VII
AUXILIARY REVIEWERS ................................................................................................................XI
SELECTED PAPERS BOOK ........................................................................................................... XIII
FOREWORD.................................................................................................................................. XV
CONTENTS ................................................................................................................................ XVII
III
INVITED SPEAKERS
Dimitar Filev
The Ford Motor Company
U.S.A.
Mark W. Spong
University of Illinois at Urbana-Champaign
U.S.A.
Patrick Millot
Université de Valenciennes
France
SPECIAL SESSION CHAIRS

Samir Ladaci
IRCCyN
France
Jean-Louis Boimond
LISA
France
Jean Jacques Loiseau

IRCCyN Nantes
France
Oleg Gusikhin
Ford Research & Adv. Engineering
U.S.A.
IV
ORGANIZING AND STEERING COMMITTEES
Conference Co-chairs
Jean-Louis Ferrier, University of Angers, France
Joaquim Filipe, Polytechnic Institute of Setúbal / INSTICC, Portugal
Program Co-chairs
Juan Andrade Cetto, Institut de Robòtica i Informàtica Industrial, CSIC-UPC, Spain
Janan Zaytoon, CReSTIC, URCA, France
Proceedings Production
Andreia Costa, INSTICC, Portugal
Bruno Encarnação, INSTICC, Portugal
Vitor Pedrosa, INSTICC, Portugal
CD-ROM Production
Paulo Brito, INSTICC, Portugal
Webdesigner and Graphics Production

Marina Carvalho, INSTICC, Portugal
Secretariat and Webmaster

Marina Carvalho, INSTICC, Portugal
V
VI
PROGRAM COMMITTEE
Eugenio Aguirre, University of Granada, Spain Luis M. Camarinha-Matos, New University of
Lisbon, Portugal
Arturo Hernandez Aguirre, Centre for Research in
Mathematics, Mexico Marco Campi, University of Brescia, Italy
Frank Allgower, University of Stuttgart, Germany Marc Carreras, University of Girona, Spain
Fouad AL-Sunni, KFUPM, Saudi Arabia Jorge Martins de Carvalho, FEUP, Portugal
Bala Amavasai, Sheffield Hallam University, U.K. Alicia Casals, Technical University of Catalonia,
Spain
Francesco Amigoni, Politecnico di Milano, Italy
Alessandro Casavola, University of Calabria, Italy
Yacine Amirat, University Paris 12, France
Christos Cassandras, Boston University, U.S.A.
Nicolas Andreff, LASMEA, France
Riccardo Cassinis, University of Brescia, Italy
Stefan Andrei, National University of Singapore,
Singapore Raja Chatila, LAAS-CNRS, France
Plamen Angelov, Lancaster University, U.K. Tongwen Chen, University of Alberta, Canada
Luis Antunes, GUESS/Universidade de Lisboa, YangQuan Chen, Utah State University, U.S.A.
Portugal
Albert M. K. Cheng, University of Houston, U.S.A.
Peter Arato, Budapest University of Technology and
Economics, Hungary Graziano Chesi, University of Hong Kong, China
Helder Araújo, University of Coimbra, Portugal Sung-Bae Cho, Yonsei University, Korea
Gustavo Arroyo-Figueroa, Instituto de Ryszard S. Choras, University of Technology &

Investigaciones Electricas, Mexico Agriculture, Poland
Marco Antonio Arteaga, Universidad Nacional Carlos Coello Coello, CINVESTAV-IPN, Mexico
Autonoma de Mexico, Mexico Patrizio Colaneri, Politecnico di Milano, Italy
Vijanth Sagayan Asirvadam, University Technology António Dourado Correia, University of Coimbra,
Petronas, Malaysia Portugal
Nikos Aspragathos, University of Patras, Greece Yechiel Crispin, Embry-Riddle University, U.S.A.
Robert Babuska, TU Delft, The Netherlands Keshav Dahal, University of Bradford, U.K.
Ruth Bars, Budapest University of Technology and Mariolino De Cecco, DIMS - University of Trento,
Economics, Hungary Italy
Karsten Berns, University Kaiserslautern, Germany Bart De Schutter, Delft University of Technology,
Robert Bicker, University of Newcastle upon Tyne, The Netherlands
U.K. Angel P. del Pobil, Universitat Jaume I, Spain
Stjepan Bogdan, University of Zagreb, Faculty of Guilherme DeSouza, University of Missouri, U.S.A.
EE&C, Croatia
Rüdiger Dillmann, University of Karlsruhe, Germany
Patrick Boucher, SUPELEC, France
Feng Ding, Southern Yangtze University, China
Alan Bowling, University of Notre Dame, U.S.A.
Denis Dochain, Université Catholique de Louvain,
Edmund Burke, University of Nottingham, U.K. Belgium
Kevin Burn, University of Sunderland, U.K. Tony Dodd, The University of Sheffield, U.K.
Clifford Burrows, Innovative Manufacturing Alexandre Dolgui, Ecole des Mines de Saint Etienne,
Research Centre, U.K. France
VII
PROGRAM COMMITTEE (CONT.)
Marco Dorigo, Université Libre de Bruxelles, Hani Hagras, University of Essex, U.K.
Belgium
Wolfgang Halang, Fernuniversitaet, Germany
Petr Ekel, Pontifical Catholic University of Minas
Gerais, Brazil J. Hallam, University of Southern Denmark, Denmark
Heinz-Hermann Erbe, TU Berlin, Germany Riad Hammoud, Delphi Electronics & Safety, U.S.A.
Gerardo Espinosa-Perez, Universidad Nacional Uwe D. Hanebeck, Institute of Computer Science and
Autonoma de Mexico, Mexico Engineering, Germany
Simon Fabri, University of Malta, Malta John Harris, University of Florida, U.S.A.
Sergej Fatikow, University of Oldenburg, Germany Robert Harrison, The University of Sheffield, U.K.
Jean-Marc Faure, Ecole Normale Superieure de Vincent Hayward, McGill Univ., Canada
Cachan, France Dominik Henrich, University of Bayreuth, Germany
Jean-Louis Ferrier, Université d'Angers, France Francisco Herrera, University of Granada, Spain
Florin Gheorghe Filip, The Romanian Academy & Victor Hinostroza, University of Ciudad Juarez,
The National Institute for R&D in Informatics (ICI), Mexico
Romania
Weng Ho, National University of Singapore,
Georg Frey, University of Kaiserslautern, Germany Singapore
Manel Frigola, Technical University of Catalonia Wladyslaw Homenda, Warsaw University of
(UPC), Spain Technology, Poland
Colin Fyfe, University of paisley, U.K. Alamgir Hossain, Bradford University, U.K.
Dragan Gamberger, Rudjer Boskovic Institute, Dean Hougen, University of Oklahoma, U.S.A.
Croatia
Amir Hussain, University of Stirling, U.K.
Leonardo Garrido, Tecnológico de Monterrey,
Mexico Seth Hutchinson, University of Illinois, U.S.A.
Ryszard Gessing, Silesian University of Technology, Atsushi Imiya, IMIT Chiba Uni, Japan
Poland
Sirkka-Liisa Jämsä-Jounela, Helsinki University of
Lazea Gheorghe, Technical University of Technology, Finland
Cluj-Napoca, Romania
Ray Jarvis, Monash University, Australia
Maria Gini, University of Minnesota, U.S.A.
Odest Jenkins, Brown University, U.S.A.
Alessandro Giua, University of Cagliari, Italy
Ping Jiang, The University of Bradford, U.K.
Luis Gomes, Universidade Nova de Lisboa, Portugal
Ivan Kalaykov, Örebro University, Sweden
John Gray, University of Salford, U.K.
Dimitrios Karras, Chalkis Institute of Technology,
Dongbing Gu, University of Essex, U.K. Greece
Jason Gu, Dalhousie University, Canada Dusko Katic, Mihailo Pupin Institute, Serbia
José J. Guerrero, Universidad de Zaragoza, Spain Graham Kendall, The University of Nottingham,
U.K.
Jatinder (Jeet) Gupta, University of Alabama in
Huntsville, U.S.A. Uwe Kiencke, University of Karlsruhe (TH), Germany
Thomas Gustafsson, Luleå University of Technology, Jozef Korbicz, University of Zielona Gora, Poland
Sweden
Israel Koren, University of Massachusetts, U.S.A.
Maki K. Habib, Saga University, Japan
VIII
Bart Kosko, University of Southern California, Hervé Marchand, INRIA, France
U.S.A.
Philippe Martinet, LASMEA, France
George L. Kovács, Hungarian Academy of Sciences,
Hungary Aleix Martinez, Ohio State University, U.S.A.
Krzysztof Kozlowski, Poznan University of Aníbal Matos, Faculdade de Engenharia da

Technology, Poland Universidade do Porto (FEUP), Portugal
Gerhard Kraetzschmar, Fraunhofer Institute for Rene V. Mayorga, University of Regina, Canada
Autonomous Intelligent Systems, Germany Barry McCollum, Queen's University Belfast, U.K.
Cecilia Laschi, Scuola Superiore Sant'Anna, Italy Ken McGarry, University of Sunderland, U.K.
Loo Hay Lee, National University of Singapore, Gerard McKee, The University of Reading, U.K.
Singapore
Seán McLoone, National University of Ireland (NUI),
Soo-Young Lee, KAIST, Korea Maynooth, Ireland
Graham Leedham, University of New South Wales Basil Mertzios, (1)Thessaloniki Institute of
(Asia), Singapore Technology, (2) Democritus University, Greece
Cees van Leeuwen, RIKEN BSI, Japan José Mireles Jr., Universidad Autonoma de Ciudad
Kauko Leiviskä, University of Oulu, Finland Juarez, Mexico
Kang Li, Queen's University Belfast, U.K. Sushmita Mitra, Indian Statistical Institute, India
Yangmin Li, University of Macau, China Vladimir Mostyn, VSB - Technical University of
Ostrava, Czech Republic
Zongli Lin, University of Virginia, U.S.A.
Rafael Muñoz-Salinas, University of Cordoba, Spain
Cheng-Yuan Liou, National Taiwan University,
Taiwan Kenneth Muske, Villanova University, U.S.A.
Vincenzo Lippiello, Università Federico II di Napoli, Ould Khessal Nadir, Okanagan College, Canada
Italy Fazel Naghdy, University of Wollongong, Australia
Honghai Liu, University of Portsmouth, U.K. Tomoharu Nakashima, Osaka Prefecture University,
Luís Seabra Lopes, Universidade de Aveiro, Portugal Japan
Brian Lovell, The University of Queensland, Australia Andreas Nearchou, University of Patras, Greece
Peter Luh, University of Connecticut, U.S.A. Luciana Porcher Nedel, Universidade Federal do Rio
Grande do Sul (UFRGS), Brazil
Anthony Maciejewski, Colorado State University,
U.S.A. Sergiu Nedevschi, Technical University of
Cluj-Napoca, Romania
N. P. Mahalik, Gwangju Institute of Science and
Technology, Korea Maria Neves, Instituto Superior de Engenharia do
Porto, Portugal
Bruno Maione, Politecnico di Bari, Italy
Hendrik Nijmeijer, Eindhoven University of
Frederic Maire, Queensland University of Technology, The Netherlands
Technology, Australia
Juan A. Nolazco-Flores, ITESM, Campus Monterrey,
Om Malik, University of Calgary, Canada Mexico
Danilo Mandic, Imperial College, U.K. Urbano Nunes, University of Coimbra, Portugal
Jacek Mandziuk, Warsaw University of Technology, Gustavo Olague, CICESE, Mexico
Poland
IX
José Valente de Oliveira, Universidade do Algarve, Agostinho Rosa, IST, Portugal
Portugal
Hubert Roth, University Siegen, Germany
Andrzej Ordys, Kingston University in London,
Faculty of Engineering, U.K. António Ruano, CSI, Portugal
Djamila Ouelhadj, University of Nottingham, ASAP Carlos Sagüés, University of Zaragoza, Spain
GROUP (Automated Scheduling, Optimisation and Mehmet Sahinkaya, University of Bath, U.K.
Planning), U.K.
Antonio Sala, Universidad Politecnica de Valencia,
Manuel Ortigueira, Faculdade de Ciências e Spain
Tecnologia da Universidade Nova de Lisboa, Portugal
Abdel-Badeeh Salem, Ain Shams University, Egypt
Christos Panayiotou, University of Cyprus, Cyprus
Ricardo Sanz, Universidad Politécnica de Madrid,
Evangelos Papadopoulos, NTUA, Greece Spain
Panos Pardalos, University of Florida, U.S.A. Medha Sarkar, Middle Tennessee State University,
Michel Parent, INRIA, France U.S.A.
Thomas Parisini, University of Trieste, Italy Nilanjan Sarkar, Vanderbilt University, U.S.A.
Igor Paromtchik, RIKEN, Japan Jurek Sasiadek, Carleton University, Canada
Gabriella Pasi, Università degli Studi di Milano Daniel Sbarbaro, Universidad de Concepcion, Chile
Bicocca, Italy Carsten Scherer, Delft University of Technology,
Witold Pedrycz, University of Alberta, Canada The Netherlands
Carlos Eduardo Pereira, Federal University of Rio Carla Seatzu, University of Cagliari, Italy
Grande do Sul - UFRGS, Brazil Klaus Schilling, University Würzburg, Germany
Maria Petrou, Imperial College, U.K. Yang Shi, University of Saskatchewan, Canada
J. Norberto Pires, University of Coimbra, Portugal Michael Short, University of Leicester, U.K.
Marios Polycarpou, University of Cyprus, Cyprus Chi-Ren Shyu, University of Missouri-Columbia,
Marie-Noëlle Pons, CNRS, France U.S.A.
Libor Preucil, Czech Technical University in Prague, Bruno Siciliano, Università di Napoli Federico II,
Czech Republic Italy
Joseba Quevedo, Technical University of Catalonia, João Silva Sequeira, Instituto Superior Técnico,
Spain Portugal
Robert Reynolds, Wayne State University, U.S.A. Silvio Simani, University of Ferrara, Italy
A. Fernando Ribeiro, Universidade do Minho, Amanda Sharkey, University of Sheffield, U.K.

Portugal Michael Small, Hong Kong Polytechnic University,
Bernardete Ribeiro, University of Coimbra, Portugal China
Robert Richardson, University of Manchester, U.K. Burkhard Stadlmann, University of Applied

Sciences Wels, Austria
John Ringwood, National University of Ireland
(NUI), Maynooth, Ireland Tarasiewicz Stanislaw, Université Laval, Canada
Rodney Roberts, Florida State University, U.S.A. Aleksandar Stankovic, Northeastern University,
U.S.A.
Kurt Rohloff, BBN Technologies, U.S.A.
Raúl Suárez, Universitat Politecnica de Catalunya
Juha Röning, University of Oulu, Finland (UPC), Spain
X
Ryszard Tadeusiewicz, AGH University of Science Axel Walthelm, sepp.med gmbh, Germany
and Technology, Poland
Lipo Wang, Nanyang Technological University,
Tianhao Tang, Shanghai Maritime University, China Singapore
Adriana Tapus, University of Southern California, Alfredo Weitzenfeld, ITAM, Mexico
U.S.A.
Dirk Wollherr, Technische Universität München,
József K. Tar, Budapest Tech Polytechnical Germany
Institution, Hungary
Sangchul Won, Pohang University of Science and
Daniel Thalmann, EPFL, Switzerland Technology, Korea
Gui Yun Tian, University of Huddersfield, U.K. Kainam Thomas Wong, The Hong Kong Polytechnic
University, China
Antonios Tsourdos, Cranfield University, U.K.
Jeremy Wyatt, University of Birmingham, U.K.
Nikos Tsourveloudis, Technical University of Crete,
Greece Alex Yakovlev, University of Newcastle, U.K.
Ivan Tyukin, RIKEN Brain Science Institute, Japan Hujun Yin, University of Manchester, U.K.
Masaru Uchiyama, Tohoku University, Japan Xinghuo Yu, Royal Melbourne Institute of Techology,
Australia
Nicolas Kemper Valverde, Universidad Nacional
Autónoma de México, Mexico Du Zhang, California State University, U.S.A.
Marc Van Hulle, K. U. Leuven, Belgium Janusz Zalewski, Florida Gulf Coast University,
U.S.A.
Annamaria R. Varkonyi-Koczy, Budapest University
of Technology and Economics, Hungary Marek Zaremba, Universite du Quebec, Canada
Luigi Villani, Università di Napoli Federico II, Italy Dayong Zhou, University of Oklahoma, U.S.A.
Markus Vincze, Technische Universität Wien, Argyrios Zolotas, Loughborough University, U.K.
Austria
Albert Zomaya, The University of Sydney, Austrália
Bernardo Wagner, University of Hannover, Germany
AUXILIARY REVIEWERS
Rudwan Abdullah, University of Stirling, U.K. Dietrich Brunn, Intelligent Sensor-Actuator-Systems
Laboratory - Universität Karlsruhe (TH), Germany
Luca Baglivo, University of Padova, Italy
Maria Paola Cabasino, Dip.to Ingegneria Elettrica ed
Prasanna Balaprakash, IRIDIA, Université Libre de Elettronica Universita' di Cagliari, Italy
Bruxelles, Belgium
Joao Paulo Caldeira, EST-IPS, Portugal
João Balsa, Universidade de Lisboa, Portugal
Aneesh Chauhan, Universidade de Aveiro, Portugal
Alejandra Barrera, ITAM, Mexico
Paulo Gomes da Costa, FEUP, Portugal
Frederik Beutler, Intelligent Sensor-Actuator-
Systems Laboratory - Universität Karlsruhe (TH), Xevi Cufi, University of Girona, Spain
Germany
Sérgio Reis Cunha, FEUP, Portugal
Alecio Binotto, CETA SENAI-RS, Brazil
Paul Dawson, Boise State University, U.S.A.
Nizar Bouguila, Concordia University, Canada
Mahmood Elfandi, Elfateh University, Libya
XI
AUXILIARY REVIEWERS (CONT.)
Michele Folgheraiter, Politecnico di Milano, Italy Ana Respício, Universidade de Lisboa, Portugal
Diamantino Freitas, FEUP, Portugal Pere Ridao, University of Girona, Spain
Reinhard Gahleitner, University of Applied Sciences Kathrin Roberts, Intelligent Sensor-Actuator-
Wels, Austria Systems Laboratory - Universität Karlsruhe (TH),
Germany
Nils Hagge, Leibniz Universität Hannover, Institute
for Systems Engineering, Germany Paulo Lopes dos Santos, FEUP, Portugal
Onur Hamsici, The Ohio State Univeristy, U.S.A. Felix Sawo, Intelligent Sensor-Actuator-Systems
Renato Ventura Bayan Henriques, UFRGS, Brazil
Frederico Schaf, UFRGS, Brazil
Matthias Hentschel, Leibniz Universität Hannover,
Institute for Systems Engineering, Germany Oliver Schrempf, Intelligent Sensor-Actuator-
Systems Laboratory - Universität Karlsruhe (TH),
Marco Huber, Intelligent Sensor-Actuator-Systems Germany
Torsten Sievers, University of Oldenbvurg, Germany
Markus Kemper, University of Oldenbvurg,
Germany Razvan Solea, Institute of Systems and Robotics,
University of Coimbra, Portugal
Vesa Klumpp, Intelligent Sensor-Actuator-Systems
Laboratory - Universität Karlsruhe (TH), Germany Wolfgang Steiner, University of Applied Sciences
Wels, Austria
Daniel Lecking, Leibniz Universität Hannover,
Institute for Systems Engineering, Germany Christian Stolle, University of Oldenbvurg, Germany
Gonzalo Lopez-Nicolas, University of Zaragoza, Alina Tarau, Delft University of Technology,
Spain The Netherlands
Cristian Mahulea, University of Zaragoza, Spain Rui Tavares, University of Evora, Portugal
Cristian Mahulea, Dep.to Informática e Ingeneiría de Paulo Trigo, ISEL, Portugal
Sistemas Centro Politécnico Superior, Spain
Haralambos Valsamos, University of Patras, Greece
Nikolay Manyakov, K. U. Leuven, Belgium
José Luis Villarroel, University of Zaragoza, Spain
Antonio Muñoz, University of Zaragoza, Spain
Yunhua Wang, University of Oklahoma, U.S.A.
Ana C. Murillo, University of Zaragoza, Spain
Florian Weißel, Intelligent Sensor-Actuator-Systems
Andreas Neacrhou, University of Patras, Greece Laboratory - Universität Karlsruhe (TH), Germany
Marco Montes de Oca, IRIDIA, Université Libre de Jiann-Ming Wu, National Dong Hwa University,
Bruxelles, Belgium Taiwan
Sorin Olaru, Supelec, France Oliver Wulf, Leibniz Universität Hannover, Institute
for Systems Engineering, Germany
Karl Pauwels, K. U. Leuven, Belgium
Ali Zayed, Seventh of April University, Libya
Luis Puig, University of Zaragoza, Spain
Yan Zhai, University of Oklahoma, U.S.A.
XII
SELECTED PAPERS BOOK
A number of selected papers presented at ICINCO 2007 will be published by Springer, in a book entitled
Informatics in Control, Automation and Robotics IV. This selection will be done by the conference co-chairs and
program co-chairs, among the papers actually presented at the conference, based on a rigorous review by the
ICINCO 2007 program committee members.
XIII
XIV
FOREWORD
Welcome to the 4th International Conference on Informatics in Control, Automation and Robotics
(ICINCO 2007) held at the University of Angers. The ICINCO Conference Series has now
consolidated as a major forum to debate technical and scientific advances presented by researchers
and developers both from academia and industry, working in areas related to Control, Automation
and Robotics that require Information Technology support.
In this year Conference Program we have included oral presentations (full papers and short papers)
as well as posters, organized in three simultaneous tracks: “Intelligent Control Systems and
Optimization”, “Robotics and Automation” and “Systems Modeling, Signal Processing and
Control”. Furthermore, ICINCO 2007 includes 2 satellite workshops and 3 plenary keynote
lectures, given by internationally recognized researchers
The two satellite workshops that are held in conjunction with ICINCO 2007 are: Third
International Workshop on Multi-Agent Robotic Systems (MARS 2007) and Third International
Workshop on Artificial Neural Networks and Intelligent Information Processing (ANNIIP 2007).
As additional points of interest, it is worth mentioning that the Conference Program includes a
plenary panel subject to the theme “Practical Applications of Intelligent Control and Robotics” and
3 Special Sessions focused on very specialized topics.
ICINCO has received 435 paper submissions, not including workshops, from more than 50
countries, in all continents. To evaluate each submission, a double blind paper review was
performed by the program committee, whose members are researchers in one of ICINCO main
topic areas. Finally, only 263 papers are published in these proceedings and presented at the
conference; of these, 195 papers were selected for oral presentation (52 full papers and 143 short
papers) and 68 papers were selected for poster presentation. The global acceptance ratio was 60,4%
and the full paper acceptance ratio was 11,9%. After the conference, some authors will be invited
to publish extended versions of their papers in a journal and a short list of about thirty papers will
be included in a book that will be published by Springer with the best papers of ICINCO 2007.
In order to promote the development of research and professional networks the conference
includes in its social program a Town Hall Reception in the evening of May 9 (Wednesday) and a
Conference and Workshops Social Event & Banquet in the evening of May 10 (Thursday).
We would like to express our thanks to all participants. First of all to the authors, whose quality
work is the essence of this conference. Next, to all the members of the Program Committee and
XV
reviewers, who helped us with their expertise and valuable time. We would also like to deeply thank
the invited speakers for their excellent contribution in sharing their knowledge and vision. Finally, a
word of appreciation for the hard work of the secretariat; organizing a conference of this level is a
task that can only be achieved by the collaborative effort of a dedicated and highly capable team.
Commitment to high quality standards is a major aspect of ICINCO that we will strive to maintain
and reinforce next year, including the quality of the keynote lectures, of the workshops, of the
papers, of the organization and other aspects of the conference. We look forward to seeing more
results of R&D work in Informatics, Control, Automation and Robotics at ICINCO 2008, next
May, at the Hotel Tivoli Ocean Park, Funchal, Madeira, Portugal.
Janan Zaytoon
CReSTIC, URCA, France
Juan Andrade Cetto
Institut de Robòtica i Informàtica Industrial, CSIC-UPC, Spain
Jean-Louis Ferrier
Université d’Angers, France
Joaquim Filipe
Polytechnic Institute of Setúbal / INSTICC, Portugal
XVI
CONTENTS
INVITED SPEAKERS
KEYNOTE LECTURES
REAL TIME DIAGNOSTICS, PROGNOSTICS, & PROCESS MODELING IS-5

Dimitar Filev
SYNCHRONIZATION OF MULTI-AGENT SYSTEMS IS-7

Mark W. Spong
TOWARD HUMAN-MACHINE COOPERATION IS-9

Patrick Millot
ROBOTICS AND AUTOMATION
FULL PAPERS
ESTIMATION PROCESS FOR TIRE-ROAD FORCES AND VEHICLE SIDESLIP ANGLE

Guillaume Baffet, Ali Charara and Daniel Lechner 5
INTEGRATED DESIGN OF A MECHATRONIC SYSTEM - THE PRESSURE CONTROL IN

COMMON RAILS
Paolo Lino and Bruno Maione 11
ON THE FORCE/POSTURE CONTROL OF A CONSTRAINED SUBMARINE ROBOT

Ernesto Olguín-Díaz and Vicente Parra-Vega 19
FUZZY-SYNTACTIC APPROACH TO PATTERN RECOGNITION AND SCENE ANALYSIS

Marzena Bielecka, Marek Skomorowski and Andrzej Bielecki 29
VISUAL ALIGNMENT ROBOT SYSTEM: KINEMATICS, PATTERN RECOGNITION, AND

CONTROL
SangJoo Kwon and Chansik Park 36
DISTURBANCE FEED FORWARD CONTROL OF A HANDHELD PARALLEL ROBOT

Achim Wagner, Matthias Nübel, Essam Badreddin, Peter P. Pott and Markus L. Schwarz 44
THE TELE-ECHOGRAPHY MEDICAL ROBOT OTELO2 - TELEOPERATED WITH A MULTI

LEVEL ARCHITECTURE USING TRINOMIAL PROTOCOL
Gwenaël Charron, Aïcha Fonte, Pierre Vieyres, Philippe Fraisse, Lama Al Bassit and Cyril Novales 52
RPQ: ROBOTIC PROXIMITY QUERIES - DEVELOPMENT AND APPLICATIONS

Albert Hernansanz, Xavier Giralt, Alberto Rodriguez and Josep Amat 59
EVOLUTIONARY PATH PLANNING FOR UNMANNED AERIAL VEHICLES COOPERATION

Ioannis K. Nikolos and Nikos Tsourveloudis 67
A NOVEL STRATEGY FOR EXPLORATION WITH MULTIPLE ROBOTS

Jonathan Rogge and Dirk Aeyels 76
CALIBRATION OF QUASI-ISOTROPIC PARALLEL KINEMATIC MACHINES: ORTHOGLIDE

Anatoly Pashkevich, Roman Gomolitsky, Philippe Wenger and Damien Chablat 84
XVII
MAKING SENSOR NETWORKS INTELLIGENT
Peter Sapaty, Masanori Sugisaka and Joaquim Filipe 92
BAYES-BASED OBJECT TRACKING BOOSTED BY PARTICLE SWARM OPTIMIZATION

Yuhua Zheng and Yan Meng 101
SMARTMOBILE – AN ENVIRONMENT FOR GUARANTEED MULTIBODY MODELING AND

SIMULATION
Ekaterina Auer and Wolfram Luther 109
GRASP CONFIGURATION MATCHING - USING VISUAL AND TACTILE SENSOR

INFORMATION
Madjid Boudaba and Alicia Casals 117
MOTION CONTROL OF AN OMNIDIRECTIONAL MOBILE ROBOT

Xiang Li and Andreas Zell 125
IMPROVEMENT OF THE VISUAL SERVOING TASK WITH A NEW TRAJECTORY

PREDICTOR - THE FUZZY KALMAN FILTER
C. Pérez, N. García, J. M. Sabater, J. M. Azorín, O. Reinoso and L. Gracia 133
OBSTACLE DETECTION IN MOBILE OUTDOOR ROBOTS - A SHORT-TERM MEMORY

FOR THE MOBILE OUTDOOR PLATFORM RAVON
H. Schäfer, M. Proetzsch and K. Berns 141
METHODOLOGY FOR LEARNING VISUAL REACTIVE BEHAVIOURS IN ROBOTICS

THROUGH REINFORCEMENT AND IMAGE-BASED STATES
Pablo Quintía, José E. Domenech, Cristina Gamallo and Carlos V. Regueiro 149
FORMAL VERIFICATION OF SAFETY BEHAVIOURS OF THE OUTDOOR ROBOT RAVON

Martin Proetzsch, Karsten Berns, T. Schuele and K. Schneider 157
A MODIFIED IMPULSE CONTROLLER FOR IMPROVED ACCURACY OF ROBOTS

WITH FRICTION
Stephen van Duin, Christopher D. Cook, Zheng Li and Gursel Alici 165
COLLABORATIVE CONTROL IN A HUMANOID DYNAMIC TASK

Diego Pardo and Cecilio Angulo 174
A FUZZY SYSTEM FOR INTEREST VISUAL DETECTION BASED ON SUPPORT VECTOR

MACHINE
Eugenio Aguirre, Miguel García-Silvente, Rui Paúl and Rafael Muñoz-Salinas 181
POSTERS
DATA INTEGRATION SOLUTION FOR PAPER INDUSTRY - A SEMANTIC STORING

BROWSING AND ANNOTATION MECHANISM FOR ONLINE FAULT DATA
Sergiy Nikitin, Vagan Terziyan and Jouni Pyötsiä 191
FAST COMPUTATION OF ENTROPIES AND MUTUAL INFORMATION FOR MULTISPECTRAL

IMAGES
Sié Ouattara, Alain Clément and François Chapeau-Blondeau 195
PROBABILISTIC MAP BUILDING CONSIDERING SENSOR VISIBILITY

Kazuma Haraguchi, Jun Miura, Nobutaka Shimada and Yoshiaki Shirai 200
A NEWMARK FMD SUB-CYCING ALGORITHM

J. C. Miao, P. Zhu, G. L. Shi and G. L. Chen 207
XVIII
ROBOT TCP POSITIONING WITH VISION - ACCURACY ESTIMATION OF A ROBOT
VISUAL CONTROL SYSTEM
Drago Torkar and Gregor Papa 212
MULTI AGENT-BASED ON-LINE DIAGNOSTIC SCHEME OF SUBSTATION IED

Seong-Jeong Rim and Seung-Jae Lee 216
SCADA WEB - REMOTE SUPERVISION AND MAINTENANCE OF INDUSTRIAL PROCESSES

José Ramón Janeiro, Eduardo J. Moya, David García, Oscar Calvo and Clemente Cárdenas 220
PROPERTY SERVICE ARCHITECTURE FOR DISTRIBUTED ROBOTIC AND SENSOR SYSTEMS

Antti Tikanmäki and Juha Röning 226
ON GENERATING GROUND-TRUTH TIME-LAPSE IMAGE SEQUENCES AND FLOW FIELDS

Vladimír Ulman and Jan Hubený 234
HELPING INSTEAD OF REPLACING - TOWARDS A SHARED TASK ALLOCATION

ARCHITECTURE
Foad Ghaderi and Majid Nili Ahmadabadi 240
FUZZY LOGIC ALGORITHM FOR MOBILE ROBOT CONTROL

Viorel Stoian and Cristina Pana 244
ON THE BALANCING CONTROL OF HUMANOID ROBOT

Youngjin Choi and Doik Kim 248
ON COMPUTING MULTI-FINGER FORCE-CLOSURE GRASPS OF 2D OBJECTS

Belkacem Bounab, Daniel Sidobre and Abdelouhab Zaatri 253
PREDICTIVE CONTROL BY LOCAL VISUAL DATA - MOBILE ROBOT MODEL PREDICTIVE

CONTROL STRATEGIES USING LOCAL VISUAL INFORMATION AND ODOMETER DATA
Lluis Pacheco and Ningsu Luo 259
INTERNET-BASED TELEOPERATION: A CASE STUDY - TOWARD DELAY APPROXIMATION

AND SPEED LIMIT MODULE
Shengtong Zhong, Philippe Le Parc and Jean Vareille 267
OPTICAL NAVIGATION SENSOR - INCORPORATING VEHICLE DYNAMICS

INFORMATION IN MAPMAKING
Tibor Takács and Viktor Kálmán 271
VISION-BASED OBSTACLE AVOIDANCE FOR A SMALL, LOW-COST ROBOT

Chau Nguyen Viet and Ian Marshall 275
A DISTRIBUTED MULTI-ROBOT SENSING SYSTEM USING AN INFRARED LOCATION

SYSTEM
Anssi Kemppainen, Janne Haverinen and Juha Röning 280
AUTOMATIC VISION-BASED MONITORING OF THE SPACECRAFT ATV RENDEZVOUS /

SEPARATIONS WITH THE INTERNATIONAL SPACE STATION
A. A. Boguslavsky, V. V. Sazonov, S. M. Sokolov, A. A. Boguslavsky, V. V. Sazonov and S. M. Sokolov 284
NEW APPROACH TO GET AUTONOMOUS AND FAST ROBOT LEARNING PROCESSES

R. Iglesias, M. Rodríguez, C. V. Regueiro, J. Correa, Pablo Quintía and S. Barro 292
OBSERVER BASED OPTIMAL CONTROL OF SHIP ELECTRIC PROPULSION SYSTEM

Habib Dallagi, Ali Sghaïer Tlili and Samir Nejim 296
XIX
A NOVEL BLOCK MOTION ESTIMATION MODEL FOR VIDEO STABILIZATION
APPLICATIONS
Harish Bhaskar and Helmut Bez 303
THE PROTOTYPE OF HUMAN – ROBOT INTERACTIVE VOICE CONTROL SYSTEM

Miroslav Holada, Igor Kopetschke, Pavel Pirkl, Martin Pelc, Lukáš Matela, Jiří Horčička and Jakub Štilec 307
DEVELOPMENT OF AN AUTOMATED DEVICE FOR SORTING SEEDS - APPLICATION

ON SUNFLOWER SEEDS
Vincent Muracciole, Patrick Plainchault, Dominique Bertrand and Maria Rosaria Mannino 311
MATHEMATICAL MODEL FOR WALKING ROBOT WITH SHAPE MEMORY ALLOY ANKLE
Anca Petrişor, Nicu George Bîzdoacă, Daniela Roşca, Sonia Degeratu, Adrian Roşca and Raducu Petrisor 319
WEB-BASED INTERACTIVE POSITIONING CONTROL OF AN ELECTRIC FLATCAR VIA

WIRELESS NETWORK
Ken Ishii, Koki Abe and Yoshimi Takao 323
THEORY OF STRUCTURED INTELLIGENCE - RESULTS ON INNOVATION-BASED AND

EXPERIENCE-BASED BEHAVIOUR
Meike Jipp and Essameddin Badreddin 327
X3DIRECTOR - A FRONT-END FOR MODELING WEB VIRTUAL WORLD NAVIGATION AND

VISUALIZATION PARAMETERS
Berta Buttarazzi and Daniele Pizziconi 333
A STUDY OF TWO COLOR SYSTEMS USED IN CONTENT-BASED IMAGE QUERY ON

MEDICAL IMAGERY
Liana Stanescu, Dumitru Burdescu, Cosmin Stoica and Marius Brezovan 337
COMPARISON OF TWO IDENTIFICATION TECHNIQUES: THEORY AND APPLICATION

Pierre-Olivier Vandanjon, Alexandre Janot, Maxime Gautier and Flavia Khatounian 341
SPECIAL SESSION ON INTELLIGENT VEHICLE CONTROL SYSTEMS
VEHICLE MODELS AND ESTIMATION OF CONTACT FORCES AND TIRE ROAD FRICTION
Nacer K. M’Sirdi, Abdelhamid Rabhi and Aziz Naamane 351
BENCHMARKING HAAR AND HISTOGRAMS OF ORIENTED GRADIENTS FEATURES

APPLIED TO VEHICLE DETECTION
Pablo Negri, Xavier Clady and Lionel Prevost 359
MULTIMODAL COMMUNICATION ERROR DETECTION FOR DRIVER-CAR INTERACTION

Sy Bor Wang, David Demirdjian, Trevor Darrell and Hedvig Kjellström 365
PRIUS CONTROL WITH A HYBRID METHOD

Danil Prokhorov 372
AUTHOR INDEX 377
XX
INVITED
SPEAKERS
KEYNOTE
LECTURES
REAL TIME DIAGNOSTICS, PROGNOSTICS, & PROCESS
MODELING
Dimitar Filev
The Ford Motor Company
U.S.A.
Abstract: Practical and theoretical problems related to the design of real time diagnostics, prognostics, & process
modeling systems are discussed. Major algorithms for autonomous monitoring of machine health in
industrial networks are proposed and relevant architectures for incorporation of intelligent prognostics
within plant floor information systems are reviewed. Special attention is given to the practical realization of
real time structure and parameter learning algorithms. Links between statistical process control and real time
modeling based on the evolving system paradigm are analyzed relative to the design of soft sensing
algorithms. Examples and case studies of industrial implementation of aforementioned concepts are
presented.
BRIEF BIOGRAPHY
Dr. Dimitar P. Filev is a Senior Technical Leader,
Intelligent Control & Information Systems with Ford
Motor Company specializing in industrial intelligent
systems and technologies for control, diagnostics
and decision making. He is conducting research in
systems theory and applications, modeling of
complex systems, intelligent modeling and control
and he has published 3 books, and over 160 articles
in refereed journals and conference proceedings. He
holds15 granted U.S. patents and numerous foreign
patents in the area of industrial intelligent systems
Dr. Filev is a recipient of the '95 Award for
Excellence of MCB University Press and was
awarded 4 times with the Henry Ford Technology
Award for development and implementation of
advanced intelligent control technologies. He is
Associate Editor of Int. J. of General Systems and
Int. J. of Approximate Reasoning. He is a member of
the Board of Governors of the IEEE Systems, Man
& Cybernetics Society and President of the North
American Fuzzy Information Processing Society
(NAFIPS). Dr. Filev received his PhD. degree in
Electrical Engineering from the Czech Technical
University in Prague in 1979.
IS-5
SYNCHRONIZATION OF MULTI-AGENT SYSTEMS
Mark W. Spong
Donald Biggar Willett Professor of Engineering
Professor of Electrical and Computer Engineering
Coordinated Science Laboratory
University of Illinois at Urbana-Champaign
U.S.A.
Abstract: There is currently great interest in the control of multi-agent networked systems. Applications include
mobile sensor networks, teleoperation, synchronization of oscillators, UAV's and coordination of multiple
robots. In this talk we consider the output synchronization of networked dynamic agents using passivity
theory and considering the graph topology of the inter-agent communication. We provide a coupling control
law that results in output synchronization and we discuss the extension to state synchronization in addition
to output synchronization. We also consider the extension of these ideas to systems with time delay in
communication among agents and obtain results showing synchronization for arbitrary time delay. We will
present applications of our results in synchronization of Kuramoto oscillators and in bilateral teleoperators.
BRIEF BIOGRAPHY
Mark W. Spong received the B.A. degree, magna
cum laude and Phi Beta Kappa, in mathematics and
physics from Hiram College, Hiram, Ohio in 1975,
the M.S. degree in mathematics from New Mexico
State University in 1977, and the M.S. and D.Sc.
degrees in systems science and mathematics in 1979
and 1981, respectively, from Washington University
in St. Louis. Since 1984 he has been at the
University of Illinois at Urbana-Champaign where
he is currently a Donald Biggar Willett
Distinguished Professor of Engineering, Professor of
Electrical and Computer Engineering, and Director
of the Center for Autonomous Engineering Systems
and Robotics. Dr. Spong is Past President of the
IEEE Control Systems Society and a Fellow of the
IEEE. Dr. Spong's main research interests are in
robotics, mechatronics, and nonlinear control theory.
He has published more than 200 technical articles in
control and robotics and is co-author of four books.
His recent awards include the Senior U.S. Scientist
Research Award from the Alexander von Humboldt
Foundation, the Distinguished Member Award from
the IEEE Control Systems Society, the John R.
Ragazzini and O. Hugo Schuck Awards from the
American Automatic Control Council, and the IEEE
Third Millennium Medal.
IS-7
TOWARD HUMAN-MACHINE COOPERATION
Patrick Millot
Laboratoire d'Automatique, de Mécanique et d'Informatique Industrielle et Humaine
Université de Valenciennes
France
Abstract: In human machine systems human activities are mainly oriented toward decision-making: monitoring and
fault detection, fault anticipation, diagnosis and prognosis, and fault prevention and recovery. The
objectives combine the human-machine system performances (production quantity and quality) as well as
the global system safety. In this context human operators may have a double role: (1) a negative role as they
may perform unsafe or erroneous actions on the process, (2) a positive role as they can detect, prevent or
recover an unsafe process behavior due to an other operator or to automated decision makers.
Two approachs to these questions are combined in a pluridisciplinary research way : (1) human engineering
which aims at designing dedicated assistance tools for human operators and at integrating them into human
activities through a human machine cooperation, (2) cognitive psychology and ergonomics analysing the
human activities, the need for such tools and their use. This paper focuses on the concept of cooperation and
proposes a framework for implementation. Examples in Air Traffic Control and in Telecommunication
networks illustrate these concepts.
BRIEF BIOGRAPHY and netwoks (HCM networks 93-96, 2 projects since

02 on Urban Guided Transport Management
Systems and the Network of Excellence EURNEX
Born in 53 he received a PhD in Automatic Control
(79) an is Docteur d'Etat es Sciences (87). He is full since 04). Member of the IFAC Technical
Professor at the University of Valenciennes since 89. Committee 4.5 Human Machine Systems since 00.
IPC member of several International Conferences
He conducts research on Automation Sciences,
Artificial Intelligence, Supervisory Control, Human and Journals.
Machine Systems, Human Reliability with
applications to production telecommunication and
transport systems ( Air Traffic Control, Car Traffic,
Trains Metro.). His scientific production covers
about 175 publications, collective books, conference
proceedings. Research Director of 35 PhD students
and 9 HDR since 89, reviewer of 50 PhD Thesis and
9 HDR from other universities. Head of the research
group "Human Machine Systems" in LAMIH since
87 till 04 (25 researchers). Vice-head then head of
LAMIH between 96 and 05 (222 researchers and
engineers). Vice Chairman of the University of
Valenciennes since October 05 in charge of
research.
Scientific head or Member of the scientific board or
Manager of several regional research groups on
Supervisory Control (GRAISYHM 96-02) on
Transport System Safety (GRRT since 87,pôle ST2
since 01 with 80 researchers of 10 labs). Member of
the French Council of the Universities (96-03),
member of the scientific board of the french national
research group in Automation Sciences supported by
CNRS (96-01). Partner of several European projects
IS-9
ROBOTICS
AND AUTOMATION
FULL PAPERS
ESTIMATION PROCESS FOR TIRE-ROAD FORCES AND VEHICLE
SIDESLIP ANGLE
Guillaume Baffet, Ali Charara

Heudiasyc Laboratory (UMR CNRS 6599), Université de Technologie de Compiègne
Centre de recherche Royallieu, BP20529 - 60205 Compiègne, France
[email protected], [email protected]
Daniel Lechner
INRETS-MA Laboratory (Department of Accident Mechanism Analysis)
Chemin de la Croix Blanche, 13300 Salon de Provence, France
[email protected]
Keywords: State observers, vehicle dynamic, sideslip angle estimation, tire-force estimation, wheel cornering stiffness
estimation, linear adaptive force model.
Abstract: This study focuses on the estimation of car dynamic variables for the improvement of vehicle safety, handling
characteristics and comfort. More specifically, a new estimation process is proposed to estimate longitudi-
nal/lateral tire-road forces, sideslip angle and wheel cornering stiffness. This method uses measurements from
currently-available standard sensors (yaw rate, longitudinal/lateral accelerations, steering angle and angular
wheel velocities). The estimation process is separated into two blocks: the first block contains an observer
whose principal role is to calculate tire-road forces without a descriptive force model, while in the second block
an observer estimates sideslip angle and cornering stiffness with an adaptive tire-force model. The different
observers are based on an Extended Kalman Filter (EKF). The estimation process is applied and compared to
real experimental data, notably sideslip angle and wheel force measurements. Experimental results show the
accuracy and potential of the estimation process.
1 INTRODUCTION of this study is to separate the estimation modeling

into two blocks (shown in figure 1), where the first
The last few years have seen the emergence in cars block concerns the car body dynamic while the sec-
of active security systems to reduce dangerous situ- ond is devoted to the tire-road interface dynamic. The
ations for drivers. Among these active security sys- Measurements: yaw rate, steering angle, lateral acceleration
tems, Anti-lock Braking Systems (ABS) and Elec- longitudinal acceleration, angular wheel velocities
tronic Stability Programs (ESP) significantly reduce
the number of road accidents. However, these sys- Block 1 Extended Kalman Filter
tems may improved if the dynamic potential of a car Observer O1,4w : Four-wheel vehicle model
is well known. For example, information on tire-road Random walk force model
friction means a better definition of potential trajec- Longitudinal and lateral tire forces, yaw rate
tories, and therefore a better management of vehicle
controls. Nowadays, certain fundamental data relat- Extended Kalman Filter
Block 2
ing to vehicle-dynamics are not measurable in a stan- Observer O2,LM : Side-slip angle model
dard car for both technical and economic reasons. As Linear adaptive force model
a consequence, dynamic variables such as tire forces
and sideslip angle must be observed or estimated. Estimations : Longitudinal and lateral forces, sideslip angle,
yaw rate, cornering stiffness
Vehicle-dynamic estimation has been widely dis-
cussed in the literature, e.g. ((Kiencke and Nielsen,
2000), (Ungoren et al., 2004), (Lechner, 2002), Figure 1: Estimation process. Observers O1,4w and O2,LAM .
(Stephant et al., 2006)). The vehicle-road system is
usually modeled by combining a vehicle model with first block contains an Extended Kalman Filter (de-
a tire-force model in one block. One particularity noted as O1,4w ) constructed with a four-wheel vehicle
5
ICINCO 2007 - International Conference on Informatics in Control, Automation and Robotics
model and a random walk force model. The first ob- sideslip angle, Vg the center of gravity velocity, and L1
server O1,4w estimates longitudinal/lateral tire forces and L2 the distance from the vehicle center of gravity
and yaw rate, which are inputs to the observer in the to the front and rear axles respectively. Fx,y,i, j are the
second block (denoted as O2,LAM ). This second ob- longitudinal and lateral tire-road forces, δ1,2 are the
server is developed from a sideslip angle model and a front left and right steering angles respectively, and
linear adaptive force model. E is the vehicle track (lateral distance from wheel to
Some studies have described observers which take wheel).
road friction variations into account ((Lakehal-ayat In order to develop an observable system (notably
et al., 2006), (Rabhi et al., 2005), (Ray, 1997)). In in the case of null steering angles), rear longitudinal
the works of (Lakehal-ayat et al., 2006) road fric- forces are neglected relative to the front longitudinal
tion is considered as a disturbance. Alternatively, as forces. The simplified equation for yaw acceleration
in (Rabhi et al., 2005), the tire-force parameters are (four-wheel vehicle model) can be formulated as the
identified with an observer, while in (Ray, 1997) tire following dynamic relationship (O1,4w model):
forces are modeled with an integrated random walk  
model. In this study a linear adaptive tire force model L1 [Fy11 cos(δ1 ) + Fy12 cos(δ2 )
is proposed (in block 2) with an eye to studying road  +Fx11 sin(δ1 ) + Fx12 sin(δ2 )] 
 
friction variations. ψ̈ = I1z  −L2 [Fy21 + Fy22 ]  , (1)
 + E [F sin(δ ) − F sin(δ ) 
The rest of the paper is organized as follows. The 2 y11 1 y12 2
second section describes the vehicle model and the +Fx12 cos(δ2 ) − Fx11 cos(δ1 )]
observer O1,4w (block 1). Next, the third section where m the vehicle mass and Iz the yaw moment of
presents the sideslip angle and cornering stiffness ob- inertia. The different force evolutions are modeled
server (O2,LAM in block 2). In the fourth section an with a random walk model:
observability analysis is performed. The fifth section
provides experimental results: the two observers are [Fxi˙ j , Fyi˙ j ] = [0, 0], i = 1, 2 j = 1, 2. (2)
evaluated with respect to sideslip angle and tire force The measurement vector Y and the measurement
measurements. Finally, concluding remarks are given model are:
in section 6.
Y = [ψ̇, γy , γx ] = [Y1 ,Y2 ,Y3 ],
Y1 = ψ̇,
Y2 = m1 [Fy11 cos(δ1 ) + Fy12 cos(δ2 )
2 BLOCK 1: OBSERVER FOR +(Fy21 + Fy22 ) + Fx11 sin(δ1 ) + Fx12 sin(δ2 )],
(3)
TIRE-ROAD FORCE Y3 = m1 [−Fy11 sin(δ1 ) − Fy12 sin(δ2 )
+Fx11 cos(δ1 ) + Fx12 cos(δ2 )],
This section describes the first observer O1,4w con-
structed from a four-wheel vehicle model (figure 2), where γx and γy are the longitudinal and lateral accel-
where ψ̇ is the yaw rate, β the center of gravity erations respectively.
The O1,4w system (association between equations (1),
δ1 random walk force equation (2), and the measurement
Fx11 equations (3)) is not observable in the case where
Fy11 Fy21 and Fy22 are state vector components. For ex-
E
ample, in equation (1, 2, 3) there is no relation al-
y Fy11 lowing to distinguish the rear lateral forces Fy21 and
x
Fy22 in the sum (Fy21 +Fy22 ): as a consequence only
Vg δ2 the sum (Fy2 = Fy21 + Fy22 ) is observable. Moreover,
Fy21 β Fx12
Fx21 when driving in a straight line, yaw rate is small, δ1
ψ Fy12
and δ2 are approximately null, and hence there is no
significant knowledge in equation (1, 2, 3) differenti-
ating Fy11 and Fy12 in the sum (Fy11 + Fy12 ), so only
the sum (Fy1 = Fy11 + Fy12 ) is observable. These ob-
Fy22 L1
Fx22 servations lead us to develop the O1,4w system with a
state vector composed of force sums:
L2 X = [ψ̇, Fy1 , Fy2 , Fx1 ], (4)
Figure 2: Four-wheel vehicle model. where Fx1 is the sum of front longitudinal forces
(Fx1 = Fx11 + Fx12 ). Tire forces and force sums are as-
6
sociated according to the dispersion of vertical forces: the slip between tire and road ((Pacejka and Bakker,
Fz11 Fx1 Fz12 Fx1 1991), (Kiencke and Nielsen, 2000), (Canudas-De-
Fx11 = , Fx12 = , (5) Wit et al., 2003)). Figure 3 illustrates different lateral
Fz12 + Fz11 Fz12 + Fz11
tire-force models (linear, linear adaptive and Burck-
Fz11 Fy1 Fz12 Fy1 hardt for various road surfaces (Kiencke and Nielsen,
Fy11 = , Fy12 = , (6)
Fz12 + Fz11 Fz12 + Fz11 2000)). In this study lateral wheel slips are assumed to
Fz21 Fy2 Fz22 Fy2 be equal to the wheel sideslip angles. In current driv-
Fy21 = , Fy22 = , (7)
Fz22 + Fz21 Fz22 + Fz21
where Fzi j are the vertical forces. These are calcu-
lated, neglecting roll and suspension movements, with 5
C1
Front lateral tire force (kN)

the following load transfer model: 4.5
L2 mg − hcog mγx L2 hcog mγy 4
Fz11 = − , (8)
2(L1 + L2 ) (L1 + L2 )E 3.5
L2 mg − hcog mγx L2 hcog mγy 3
Fz12 = + , (9) 2.5
2(L1 + L2 ) (L1 + L2 )E
2 C1+∆Ca1 Linear
L1 mg + hcog mγx L2 hcog mγy Linear adaptive
Fz21 = − , (10) 1.5
2(L1 + L2 ) (L1 + L2 )E Burckhardt, asphalt dry
L1 mg + hcog mγx L2 hcog mγy 1 Burckhardt, asphalt wet
Fz22 = + , (11) 0.5 Burckhardt, cobblestone wet
2(L1 + L2 ) (L1 + L2 )E Burckhardt, ice
0
hcog being the center of gravity height and g the grav- 0 2 4 6 8 10 12 14 16
itational constant. The load transfer model follows Front lateral slip, front wheel sideslip angle (°)
the assumption of the superposition principle of in-
dependent longitudinal and lateral acceleration con- Figure 3: Lateral tire force models: linear, linear adaptive,
tributions (Lechner, 2002). The input vectors U of Burckhardt for various road surfaces.
O1,4w observer is:
U = [δ1 , δ2 , Fz11 , Fz12 , Fz21 , Fz22 ]. (12) ing situations, lateral tire forces may be considered
As regards the vertical force inputs, these are calcu- linear with respect to sideslip angle (linear model):
lated from lateral and longitudinal accelerations with
the load transfer model. Fyi (βi ) = Ci βi , i = 1, 2, (15)
where Ci is the wheel cornering stiffness, a parameter

closely related to tire-road friction.
3 BLOCK 2: OBSERVER FOR When road friction changes or when the nonlinear
SIDESLIP ANGLE AND tire domain is reached, ”real” wheel cornering stiff-
CORNERING STIFFNESS ness varies. In order a take the wheel cornering stiff-
ness variations into account, we proposed an adaptive
This section presents the observer O2,LAM constructed tire-force model (named the linear adaptive tire-force
from a sideslip angle model and a tire-force model. model, illustrated in figure 3). This model is based on
The sideslip angle model is based on the single-track the linear model at which a readjustment variable ∆Cai
model (Segel, 1956), with neglected rear longitudinal is added to correct wheel cornering stiffness errors:
force:
Fyi (βi ) = (Ci + ∆Cai )βi . (16)
Fx1 sin(δ − β) + Fy1 cos(δ − β) + Fy2 cos(β)
β̇ = − ψ̇.
mVg The variable ∆Cai is included in the state vector of the
(13) O2,LAM observer and it evolution equation is formu-
Rear and front sideslip angles are calculated as: lated according to a random walk model (∆C˙ai = 0).
β1 = δ − β − L1 ψ̇/Vg , State X ′ ∈R3 , input U ′ ∈R4 and measurement Y ′ ∈R3
(14)
β2 = −β + L2 ψ̇/Vg , are chosen as:
where δ is the mean of front steering angles. X′ = [x1′ , x2′ , x3′ ] = [β, ∆Ca1 , ∆Ca2 ],
The dynamic of the tire-road contact is usually U′ = [u′1 , u′2 , u′3 , u′4 ] = [δ, ψ̇,Vg , Fx1 ], (17)
formulated by modeling the tire-force as a function of Y′ = [y′1 , y′2 , y′3 ] = [Fy1 , Fy2 , γy ].
7
The measurement model is

y′1 = (C1 + x2′ )β1 ,
y′2 = (C2 + x3′ )β2 ,
y′3 = m1 [(C1 + x2′ )β1 cos(u′1 ) + (C2 + x3′ )β2
+ u′4 sin(u′1 )].
(18)
where
β1 = u′1 − x1′ − L1 u′2 /u′3 ,
(19)
β2 = −x1′ + L2 u′2 /u′3 . Wheel-force transducers
Consider the state estimation denoted as Xb′ =

[xb1 ′ , xb2 ′ , xb3 ′ ], the state evolution model of O2,LAM is: Figure 4: Laboratory’s experimental vehicle.
′ 1 ′ ′ ′ ′ ′
xḃ1 = mu3 [u4 sin(u1 − xb1 ) + Fyw1,aux cos(u1 − xb1 )
′ ′
+ Fyw2,aux cos(xb1 )] − u2 ,
xḃ2 ′ = 0, The vehicle trajectory and the acceleration diagram
are shown in figure 5. During the test, the vehicle first
xḃ3 ′ = 0, accelerated up to γx ≈ 0.3g, then negotiated a slalom
(20) at an approximate velocity of 12m/s (−0.6g < γy <
where the auxiliary variables Fyw1,aux and Fyw2,aux are 0.6g), before finally decelerating to γx ≈ −0.7g. The
calculated as:
Fyw1,aux = (C1 + xb2 ′ )(u′1 − xb1 ′ − L1 u′2 /u′3 ),
Lateral position (m)
acceleration (m/s2)
Fyw2,aux = (C2 + xb3 ′ )(−xb1 ′ + L2 u′2 /u′3 ). 1
(21) 0
0
Lateral
-1 -5
4 OBSERVABILITY 0 50 100 150 -6 -4 -2 0 2
Longitudinal position (m) Longitudinal acceleration (m/s2)
The different observers (O1,4w , O2,LAM ) were devel-
oped according to an extended Kalman filter method Figure 5: Experimental test, vehicle positions, acceleration
(Kalman, 1960), (Mohinder and Angus, 1993). diagram.
The two observer systems are nonlinear, so the
two observability functions were calculated using results are presented in two forms: figures of estima-
a lie derivative method (Nijmeijer and der Schaft, tions/measurements and tables of normalized errors.
1990). Ranks of the two observability functions cor- The normalized error εz for an estimation z is defined
responded to the state vector dimensions, so systems in (Stephant et al., 2006) as
O1,4w and O2,LAM were locally observable. Concern-
ing the observability of the complete systems (O1,4w εz = 100(kz − zmeasurement k)/(max kzmeasurement k).
and O2,LAM ), a previous work (Baffet et al., 2006a) (22)
showed that a similar system (in one block) is locally
observable. 5.1 Block 1: Observer O1,4w Results
Figure 6 and table 1 present O1,4w observer results.

5 EXPERIMENTAL RESULTS The state estimations were initialized using the max-
imum value of the measurements during the test (for
The experimental vehicle (see figure 4) is a Peugeot instance, the estimation of the front lateral force Fy1
307 equipped with a number of sensors including
GPS, accelerometer, odometer, gyrometer, steering
angle, correvit and dynamometric hubs. Among these Table 1: Maximum absolute values, O1,4w normalized mean
sensors, the correvit (a non-contact optical sensor) errors and normalized standard deviation (Std).
gives measurements of rear sideslip angle and vehi- max kk Mean Std
cle velocity, while the dynamometric hubs are wheel- Fy1 5816 N 3.1% 4.0%
force transducers. Fy2 3782 N 2.9% 5.4%
This study uses an experimental test representative Fx1 9305 N 3.1% 4.1%
of both longitudinal and lateral dynamic behaviors. ψ̇ 24.6 o /s 0.4% 2.6%
8
O1,4w, Front lateral tire force F (kN) in comparison (denoted Orl , equation 15, described
y1
5 in (Baffet et al., 2006b)). The robustness of the two
5155 N observers is tested with respect to tire-road friction
variations by performing the tests with different cor-
0 nering stiffness parameters ([C1 ,C2 ] ∗ 0.5, 1, 1.5). The
Measurement observers were evaluated for the same test presented
in section 5.
-5 O1,4w
Figure 7 shows the estimation results of observer Orl
0 5 10 15 20 25 for rear sideslip angle. Observer Orl gives good re-
sults when cornering stiffnesses are approximately
O1,4w, Rear lateral tire force F
y2
(kN) known ([C1 ,C2 ] ∗ 1). However, this observer is not
robust when cornering stiffnesses change ([C1 ,C2 ] ∗
2 0.5, 1.5).
0 Orl, Rear Sideslip angle (°)

5 measurement
-2 Measurement
(C , C )*1.5
1 2
O1,4w
0 5 10 15 20 25 0
(C 1 , C 2 )
O1,4w, Front longitudinal tire force Fx1 (kN)
(C , C )*0.5
-5 1 2
0 5 10 15 20 25
0
Figure 7: Observer Orl using a fixed linear force model, rear
sideslip angle estimations with different cornering stiffness
-5 Measurement settings.
O1,4w
-10
0 5 10 15 20 25 Figure 8 and table 2 show estimation results for
Times (s) the adaptive observer O2,LAM . The performance ro-
bustness of O2,LAM is very good, since sideslip angle
Figure 6: Experimental test. O1,4w results in comparison is well estimated irrespective of cornering stiffness
with measurements. settings. This result is confirmed by the normalized
mean errors (Table 2) which are approximately con-
stant (about 7 %). The front and rear cornering stiff-
was set to 5155 N). In spite of these false initial- ness estimations (Ci + ∆Ci ) converge quickly to the
izations the estimations converge quickly to the mea- same values after the beginning of the slalom at 12 s.
sured values, showing the good convergence proper-
ties of the observer. Moreover, the O1,4w observer Table 2: Observer OLAM , rear sideslip angle estimation re-
produces satisfactory estimations close to measure- sults, maximum absolute value, normalized mean errors.
ments (normalized mean and standard deviations er- O2,LAM 0.5(C1 ,C2 ) (C1 ,C2 ) 1.5(C1 ,C2 )
rors are less than 7 %). These good experimental re- max kβ2 k 3.0o 3.0o 3.0o
sults confirm that the observer approach may be an Mean 7.4% 7.0% 7.2%
appropriate way for the estimation of tire-forces.
5.2 Block 2: Observer O2,LAM Results

6 CONCLUSIONS AND FUTURE
During the test, (Fx1 ,Fy1 ,Fy2 ) inputs of O2,LAM were WORK
originally those from the O1,4w observer. The Vg in-
put of O2,LAM was obtained from the wheel angular This study deals with two vehicle-dynamic observers
velocities. In order to demonstrate the improvement constructed for use in a two-block estimation pro-
provided by the observer using the linear adaptive cess. Experimental results show that the first observer
force model (O2,LAM , equation 16), another observer O1,4w gives force estimations close to the measure-
constructed with a linear fixed force model is used ments, and the second observer O2,LAM provides good
9
O2,LAM, Rear sideslip angle (°) standard and critical driving situations: Simulations
and experiments. Proceedings of the 8th International
measurement
Symposium on Advanced Vehicle Control AVEC2006
(C , C )*1.5
2 1 2 Taipei Taiwan.
Canudas-De-Wit, C., Tsiotras, P., Velenis, E., Basset, M.,
0 and Gissinger, G. (2003). Dynamic friction models for
road/tire longitudinal interaction. volume 39, pages
(C 1 , C 2 )
-2 189–226. Vehicle System Dynamics.
(C , C )*0.5
1 2 Kalman, R. E. (1960). A new approach to linear filtering
0 5 10 15 20 25 and prediction problems. volume 82, pages 35–45.
Transactions of the ASME - PUBLISHER of Basic
4 -1 Engineering.
x 10 O2,LAM, Front cornering stiffness (N.rad )
Kiencke, U. and Nielsen, L. (2000). Automotive control
9 (C , C )*1.5
1 2 system. Springer.
8 (C , C )
1 2 Lakehal-ayat, M., Tseng, H. E., Mao, Y., and j. Karidas
7 (2006). Disturbance observer for lateral velocity es-
6 timation. Proceedings of the 8th International Sympo-
5 sium on Advanced Vehicle Control AVEC2006 Taipei
(C , C )*0.5
Taiwan.
4 1 2
Lechner, D. (2002). Analyse du comportement dynamique
0 5 10 15 20 25 des vehicules routiers legers: developpement d’une
methodologie appliquee a la securite primaire. Ph.D.
4 -1 dissertation Ecole Centrale de Lyon France.
x 10 O2,LAM, Rear cornering stiffness (N.rad )
Mohinder, S. G. and Angus, P. A. (1993). Kalman filtering
(C , C )*1.5
8
1 2 theory and practice. Prentice hall.
(C 1 , C 2 )
Nijmeijer, H. and der Schaft, A. J. V. (1990). Nonlinear
6 dynamical control systems. Springer-Verlag.
Pacejka, H. B. and Bakker, E. (1991). The magic formula
4 tyre model. pages 1–18. Int. colloq. on tyre models
(C , C )*0.5 for vehicle dynamics analysis.
1 2
0 5 10 15 20 25 Rabhi, A., M’Sirdi, N. K., Zbiri, N., and Delanne, Y.

(2005). Vehicle-road interaction modelling for esti-
Figure 8: O2,LAM adaptive observer, Sideslip angle estima- mation of contact forces. volume 43, pages 403–411.
tion results, Front and rear cornering stiffness estimations Vehicle System Dynamics.
Ci + ∆Ci , with different cornering stiffness settings. Ray, L. (1997). Nonlinear tire force estimation and road
friction identification : Simulation and experiments.
volume 33, pages 1819–1833. Automatica.
sideslip angle estimations with good robustness prop- Segel, M. L. (1956). Theorical prediction and experimen-
tal substantiation of the response of the automobile to
erties relative to cornering stiffness changes. This re- steering control. volume 7, pages 310–330. automo-
sult justifies the use of an adaptive tire-force model to bile division of the institut of mechanical engineers.
take into account road friction changes.
Stephant, J., Charara, A., and Meizel, D. (Available online 5
Future studies will improve vehicle/road models in or- June 2006). Evaluation of a sliding mode observer for
der to widen validity domains for observers. Subse- vehicle sideslip angle. Control Engineering Practice.
quent vehicle/road models will take into account roll Ungoren, A. Y., Peng, H., and Tseng, H. E. (2004). A study
and vertical dynamics. on lateral speed estimation methods. volume 2, pages
126–144. Int. J. Vehicle Autonomous Systems.
REFERENCES
Baffet, G., Stephant, J., and Charara, A. (2006a). Sideslip
angle lateral tire force and road friction estimation in
simulations and experiments. Proceedings of the IEEE
conference on control application CCA Munich Ger-
many.
Baffet, G., Stephant, J., and Charara, A. (2006b). Vehi-
cle sideslip angle and lateral tire-force estimations in
10
INTEGRATED DESIGN OF A MECHATRONIC SYSTEM
The Pressure Control in Common Rails
Paolo Lino and Bruno Maione

Dipartimento di Elettrotecnica ed Elettronica, Politecnico di Bari, via Re David 200, 70125, Bari, Italy
Keywords: Mechatronic systems, Virtual prototyping, AMESimr , Modelling, Automotive control.
Abstract: This paper describes the integrated design of the pressure control in a common-rail injection system. Mechan-
ical elements and the embedded controller are considered as a whole, using a multi-disciplinary approach to
modelling and simulation. The virtual prototype, which provides the detailed geometrical/physical model of
the mechanical parts, plays the role of a surrogate of a reference hardware prototype in reduced-order mod-
elling, validation, and/or in tuning the control parameters. The results obtained by the proposed approach are
compared and validated by experiments.
1 INTRODUCTION (Stobart et al., 1999),(van Amerongen and Breedveld,

2003).
The innovative characters of mechatronic systems If the design choices must be made before assem-
lie with a special design process thinking about the bling interacting parts of a physical prototype and if
mechanical parts and the embedded controllers as a these parts belong to different physical domains (such
whole. On the contrary, the traditional design of de- as mechanics, electronics, hydraulics and control) the
vices combining mechanical and electronic elements virtual prototyping approaches, which are surrogates
considers components belonging to different physi- for preliminary physical realizations, face many diffi-
cal domain separately. Moreover controllers are con- culties. The first one is due to the fact that each phys-
ceived for already existing plants. Hence the physi- ical domain has specific modelling and simulation
cal properties and the dynamical behaviour of parts, tools. Unfortunately, indeed, there is a gap between
in which energy conversion plays a central role, are tools for evaluating the designed components in dif-
not determined by the choices of the control engi- ferent domains. For example, mechanical engineers
neers and therefore are of little concern to them. Their usually refer to finite element approaches/packages as
primary interests, indeed, are signal processing and an important tool for dimensioning mechanical parts
information management, computer power require- and for evaluating their dynamical properties. Only
ments, choice of sensors and sensor locations, and so after a complex order-reduction process (modal anal-
on. So it can happen that poorly designed mechan- ysis) the so obtained models are reduced in a form
ical parts do never lead to good performance, even suitable for the control analysis and design. On the
in presence of advanced controllers. On the other other hand, control modelling tools are based on oper-
hand, a poor knowledge of how controllers can di- ators (transfer functions, frequency response) and/or
rectly influence and balance for defects or weaknesses on state equations descriptions, which, in many cases,
in mechanical components does not help in achieving do not have a straightforward connection with the crit-
quality and good performance of the whole process. ical parameters of physical design. However, it is
Aiming at an integrated design, in mechatronic sys- clear that an appropriate modelling and simulation ap-
tems the interactions among mechanical, electronic proach can not be fitted into the limitations of one for-
and information processing elements are considered malism at time, particularly in the early stages of the
in all the design steps, beginning with the early stages design process. Hence, it is necessary a combination
11
of different methodologies in a multi-formalism ap- represents a benchmark for the evaluation of perfor-
proach to modelling supported by an appropriate sim- mances of the approach. Experimental results give a
ulation environment. feedback of benefits of the integration of the mechan-
In mechatronic applications, the Bond Graphs in- ical and control subsystems design.
troduced by (Paynter, 1960) provide a continuous sys- The integrated design involves the development
tem modelling approach, oriented to a class of inter- of a virtual prototype of the considered system using
connected and completely different systems and tar- an advanced modelling tool (AMESim in this paper),
geted to many user groups. Bond Graphs are very use- which is employed for the analysis of the system per-
ful in analysing and designing components built from formances during the different developing steps. Ac-
different energetic media and can represent dynamical tually, the virtual prototype could be assumed as a re-
systems at higher level of abstraction than differen- liable model of the real system. Further, a low order
tial equations (van Amerongen and Breedveld, 2003), analytical model of the real system can be developed
(Ferretti et al., 2004). to simplify the controller design. Since AMESim can
In this paper, we referred to AMESimr (Ad- represent physical phenomena at different level of de-
vanced Modelling Environment for performing Sim- tails, it is exploited to verify assumptions in build-
ulations of engineering systems) (IMAGINE S.A., ing the analytical model. Then, the control system
2004) a Bond Graph-based, multi-domain mod- is designed using a specific software package, i.e.
elling/optimization tool for the virtual prototyping MATLAB/Simulinkr , and tested on the virtual pro-
of the physical/geometrical characteristics of a Com- totype. The virtual prototype allows to perform safer,
pressed Natural Gas (CNG) injection system. In a first less expensive, and more reliable tests than using the
step, we used this tool to obtain a virtual prototype, as real system.
similar as possible to the actual final hardware. Then,
with reference to this prototype, we also determined
a reduced order model in form of transfer function
and/or state space model, more suitable for analytical
3 APPLICATION EXAMPLE
(or empirical) tuning of the pressure controller of the
CNG injection systems. Using the virtual prototype We consider a system composed of the following el-
in these early design stages enabled the evaluation of ements (Fig. 1): a fuel tank, storing high pressure
the influence of the geometrical/physical alternatives gas, a mechanical pressure reducer, a solenoid valve
on the reduced model used for the controller tuning. and the fuel metering system, consisting of a com-
Then, based on this reduced model, the controller set- mon rail and four electro-injectors. Two different con-
tings were designed and adjusted in accordance with figurations were compared for implementation, with
the early stages of the mechanical design process. Fi- different arrangements of the solenoid valve affecting
nally, the detailed physical/geometric model of the system performances (i.e. cascade connection, figure
mechanical parts, created by the AMESim package, 1(a), and parallel connection, Fig.1(b), respectively).
was exported ad used as a module in a simulation Detailed AMESim models were developed for each
program, which enabled the evaluation of the con- of them, providing critical information for the final
troller performance in the closed-loop system. In choice. Few details illustrate the injection operation
other words, the detailed simulation model surrogated for both layouts.
for a real hardware. With reference to Fig. 1(a), the pressure reducer
receives fuel from the tank at a pressure in the range
between 200 and 20 bars and reduces it to a value of
about 10 bar. Then the solenoid valve suitably reg-
2 THE INTEGRATED DESIGN ulates the gas flow towards the common rail to con-
APPROACH trol pressure level and to damp oscillations due to in-
jections. Finally, the electronically controlled injec-
In this paper, we consider the opportunity of integrat- tors send the gas to the intake manifold for obtain-
ing different models, at different level of details, and ing the proper fuel air mixture. The injection flow
different design tools, to optimize the design of the only depends on rail pressure and injection timings,
mechanical and control systems as a whole. To sum which are precisely driven by the Electronic Control
up, the integrated design can take advantage of pecu- Unit (ECU). The variable inflow section of the pres-
liarities of different specific software packages. The sure reducer is varied by the axial displacement of a
effectiveness of the approach is illustrated by means spherical shutter coupled with a moving piston. Pis-
of a case study, the pressure control of a CNG injec- ton and shutter dynamics are affected by the applied
tion system for internal combustion engines, which forces: gas pressure in a main chamber acts on the
12
INTEGRATED DESIGN OF A MECHATRONIC SYSTEM - The Pressure Control in Common Rails
ters the control chamber, causing the pressure on the

upper surface of the piston to build up. As a con-
sequence, the piston is pushed down with the shut-
ter, letting more fuel to enter in the main chamber,
where the pressure increases. On the contrary, when
the solenoid valve is non-energized, the pressure on
the upper side of the piston decreases, making the pis-
ton to raise and the main chamber shutter to close un-
der the action of a preloaded spring (see (Lino et al.,
2006) for details).
On the basis of a deep analysis performed on
AMESim virtual prototypes the second configuration
was chosen as a final solution, because it has advan-
(a) tages in terms of performances and efficiency. To
sum up, it guarantees faster transients as the fuel can
reach the common rail at a higher pressure. Moreover,
leakages involving the pressure reducer due to the al-
lowance between cylinder and piston are reduced by
the lesser pressure gradient between the lower and up-
per piston surfaces. Finally, allowing intermediate po-
sitions of the shutter in the pressure reducer permits
a more accurate control of the intake flow from the
tank and a remarkable reduction of the pressure oscil-
lations due to control operations. A detailed descrip-
tion of the AMESim model of the system according
the final layout is presented in section 4 (Fig. 2).
(b) 4 AMESIM MODEL OF THE CNG

Figure 1: Block schemes of the common rail CNG injec- INJECTION SYSTEM
tion systems; (a) cascade connection of solenoid valve; (b)
parallel connection of solenoid valve. AMESim is a virtual prototyping software produced
by IMAGINE S.A., which is oriented to lumped pa-
rameter modelling of physical elements, intercon-
piston lower surface pushing it at the top, and elastic nected by ports enlightening the energy exchanges be-
force of a preloaded spring holden in a control cham- tween pairs of elements and between an element and
ber pushes it down and causes the shutter to open. The its environment. AMESim enables the modelling of
spring preload value sets the desired equilibrium re- components from different physical domains and the
ducer pressure: if the pressure exceeds the reference integration of these different elements in an overall
value the shutter closes and the gas inflow reduces, system framework. It also guarantees a flexible archi-
preventing a further pressure rise; on the contrary, if tecture, capable of including new components defined
the pressure decreases, the piston moves down and by the users.
the shutter opens, letting more fuel to enter and caus- By assumption, the pressures distribution within
ing the pressure to go up in the reducer chamber (see the control chamber, the common rail and the injec-
(Maione et al., 2004) for details). tors is uniform, and the elastic deformations of solid
As for the second configuration (Fig. 1(b)), the parts due to pressure changes are negligible. The
fuel from the pressure reducer directly flows towards pipes are considered as incompressible ducts with
the rail, and the solenoid valve regulates the in- friction and a non uniform pressure distribution. Tem-
take flow in a secondary circuit including the con- perature variations are taken into account, affecting
trol chamber. The role of the force applied by the the pressure dynamics in each subcomponent. Be-
preloaded spring of control chamber is now played by sides, only heat exchanges through pipes are con-
the pressure force in the secondary circuit, which can sidered, by properly computing a thermal exchange
be controlled by suitably driving the solenoid valve. coefficient. The tank pressure plays the role of a
When the solenoid valve is energized, the fuel en- maintenance input, and it is modelled by a constant
13
compute the pressure dynamics as a function of tem-

perature T and intake and outtake flows ṁin , ṁout , as
well as of volume changes due to mechanical part mo-
tions, according to the following equation:

RT dV
ṗ = ṁin − ṁout + ρ , (1)
V dt
where p is the fuel pressure, ρ the fuel density and
V the taken up volume. The same component is used
to model the common rail by neglecting the volume
changes.
Both pressure and viscous stresses contribute to
drag forces acting on a body immersed in a moving
fluid. In particular, the total drag acting on a body
is the sum of two components: the pressure or form
drag, due to pressure gradient, and the skin friction
or viscous drag, i.e. Drag force = Form drag + Skin
friction drag. By introducing a drag coefficient CD
depending on Reynolds number, the drag can be ex-
pressed in terms of the relative speed v (Streeter et al.,
1998):
Drag = CD ρAv2 /2. (2)
Moving shutters connecting two different control vol-
umes are subject to both form drag and skin fric-
tion drag. The former one is properly computed by
AMESim algorithms for a variety of shutters, con-
sidering different poppet and seat shapes. As for the
latter, it is computed as a linear function of the fluid
Figure 2: AMESim model of the CNG injection system. speed by the factor of proportionality. It can be ob-
tained by noting that for a spherical body it holds
Form Drag = 2πDµv (Streeter et al., 1998), being µ
pneumatic pressure source. To simplify the AMESim the absolute viscosity and D the shutter diameter. The
model construction some supercomponents have been moving anchor in the solenoid valve experiences a
suitably created, collecting elements within a single viscous drag depending on the body shape. In par-
one. ticular, the drag coefficient of a disk is CD = 1.2, and
the skin friction drag can be computed using eq. 2.
4.1 Pressure Reducer and Common Since, by hypothesis, the anchor moves within a fluid
with uniform pressure distribution, the form drag is
Rail neglected.
The main components for modelling the pressure re- 4.2 Pipes
ducer are the Mass block with stiction and coulomb
friction and end stops, which computes the piston The continuity and momentum equations are used to
and the shutter dynamics through the Newton’s sec- compute pressures and flows through pipes so as to
ond law of motion, a Pneumatic ball poppet with con- take into account wave propagation effects. In case
ical seat, two Pneumatic piston, and an Elastic con- of long pipes with friction, a system of nonlinear par-
tact modelling the contact between the piston and the tial differential equations is obtained, which is imple-
shutter. The Pneumatic piston components compute mented in the Distributive wave equation submodel
the pressure forces acting upon the upper and lower of pneumatic pipe component from the pneumatic li-
piston surfaces. The viscous friction and leakage due brary. This is the case of pipes connecting pressure re-
to contact between piston and cylinder are taken into ducer and common rail. The continuity and momen-
account through the Pneumatic leakage and viscous tum equations can be expressed as follows (Streeter
friction component, by specifying the length of con- et al., 1998):
tact, the piston diameter and the clearance. Finally, a ∂ρ ∂v
Variable volume with pneumatic chamber is used to + ρ = 0, (3)
∂t ∂x
14
∂u α2 ρ f signal duty cycle. This signal is reconstructed by us-

+ + u|u| = 0, (4)
∂t ρ x 2d ing a Data from ASCII file signal source that drives a
where α is the sound speed in the gas, d is the pipe Pulse Width Modulation component.
diameter, f is the D’Arcy friction coefficient depend- To compute the magnetic force applied to the
ing on the Reynolds number. AMESim numerically anchor, a supercomponent (Solenoid with preloaded
solves the above equations by discretization. spring in Fig. 2) modelling the magnetic circuit has
For short pipes, the Compressibility + friction been suitably built, as described in the following. The
submodel of pneumatic pipe from is used, allowing to magnetic flux within the whole magnetic circuit is
compute the flow according the following equation: given by the Faraday law:
s ϕ̇ = (eev − Rev iev ) /n, (8)
2D∆p where ϕ is the magnetic flux, R the n turns winding
q= , (5)
Lρ f resistance, eev the applied voltage and iev the circuit
current. Flux leakage and eddy-currents have been
where ∆p is the pressure drop along the pipe of length
neglected. The magnetomotive-force MMF able to
L. The pipes connecting common rail and injectors
produce the magnetic flux has to compensate the mag-
are modelled in such a way.
netic tension drop along the magnetic and the air gap
Heat transfer exchanges are accounted for by the
paths. Even though most of the circuit reluctance is
above mentioned AMESim components, provided
applied to the air gap, nonlinear properties of the mag-
that a heat transfer coefficient is properly specified.
net, due to saturation and hysteresis, sensibly affect
For a cylindrical pipe of length L consisting of a ho-
the system behaviour. The following equation holds:
mogeneous material with constant thermal conductiv-
ity k and having an inner and outer convective fluid MMF = MMFs + MMFa = Hs ls + Ha la , (9)
flow, the thermal flow Q is given by (Zucrow and where H is the magnetic field strength and l is the
Hoffman, 1976): magnetic path length, within the magnet and the gap
2πkL∆T respectively. The air gap length depends on the actual
Q= , (6) position of the anchor. The magnetic induction within
ln rroi the magnet is a nonlinear function of H. It is assumed
where ∆T is the temperature gradient between the in- that the magnetic flux cross section is constant along
ternal and external surfaces, and ro and ri are the ex- the circuit, yielding:
ternal and internal radiuses, respectively. With ref- B = ϕ/Am = f (Hs ) = µ0 Ha , (10)
erence to the outside surface of the pipe, the heat- where Am is the air gap cross section and µ0 is the
transfer coefficient U is: magnetic permeability of air. The B − H curve is the
k hysteresis curve of the magnetic material. Arranging
Uo = . (7) the previous equations yields to ϕ, B and H. The re-
ro ln rroi
sulting magnetic force and circuit current are:
4.3 Solenoid Valve and Magnetic Fev = Am B2 /µ0 , (11)
Circuits iev = MMF/n. (12)
The force computed by the previous equation is ap-
The AMESim model for the solenoid valve is com- plied to the mass component representing the anchor,
posed of the following elements: a solenoid with so that the force balance can be properly handled by
preloaded spring, two moving masses with end stops AMESim.
subject to viscous friction and representing the mag-
net anchor and the shutter respectively, and a compo- 4.4 Injectors
nent representing the elastic contact between the an-
chor and the shutter. The intake section depends on The injectors are solenoid valves driven by the ECU
the axial displacement of the shutter over the conical in dependence of engine speed and load. The whole
seat and is computed within the Pneumatic ball pop- injection cycle takes place in a 720o interval with a
pet with conical seat component, which also evaluates delay between each injection of 180o . A supercompo-
the drags acting on the shutter. The solenoid valve nent including the same elements as for the solenoid
is driven by a peak-hold modulated voltage. The re- valve has been built to model the electro-injectors.
sulting current consists of a peak phase followed by The command signal generation is demanded to the
a variable duration hold phase. The valve opening ECU component, which provides a square signal driv-
time is regulated by varying the ratio between the hold ing each injector and depending on the current engine
phase duration and signal period, namely the control speed, injector timings and pulse phase angle.
15
5 CONTROLLER DESIGN where λ is a sequence of weights on future control

actions. The minimization of J with respect of ũ gives
In designing an effective control strategy for the in- the optimal control law for the prediction horizon N:
jection pressure it is necessary to satisfy physical −1 T
and technical constraints. In this framework, model ũ = GT G + λI G (w − r). (16)
predictive control (MPC) techniques are a valuable At each step, the first computed control action is ap-
choice, as they have shown good robustness in pres- plied and then the optimization process is repeated af-
ence of large parametric variations and model uncer- ter updating all vectors. It can be shown (Lino et al.,
tainties in industrial processes applications. They pre- 2006) that the resulting control law for the case study
dict the output from a process model and then impress becomes:
a control action able to drive the system to a reference
trajectory (Rossiter, 2003). A 2nd order state space ∆u(t) = k1 w(t) + k2 + k3 q−1 y(t) + k4 ∆u(t − 1),
analytical model of the plant (Lino et al., 2006) is (17)
used to derive a predictive control law for the injection where [k1 , k2 , k3 , k4 ] depends on N.
pressure regulation. The model trades off between ac-
curacy in representing the dynamical behaviour of the
most significant variables and the need of reducing 6 SIMULATION AND
the computational effort and complexity of controller
structure and development. The design steps are sum-
EXPERIMENTAL RESULTS
marized in the following. Firstly, the model is lin-
To assess the effectiveness of the AMESim model in
earized at different equilibrium points, in dependence
predicting the system behaviour, a comparison of sim-
of the working conditions set by the driver power re-
ulation and experimental results has been performed.
quest, speed and load. From the linearized models it
Since, for safety reasons, air is used as test fluid, the
is possible to derive a discrete transfer function repre-
experimental setup includes a compressor, providing
sentation by using a backward difference method. Fi-
air at a constant input pressure and substituting the
nally, a discrete Generalised Predictive Contrl (GPC)
fuel tank. The injection system is equipped with four
law suitable for the implementation in the ECU is de-
injectors sending the air to a discharging manifold.
rived from the discrete linear models equations.
Moreover, a PC system with a National Instrument
By considering the duty cycle of the signal driving
acquisition board is used to generate the engine speed
the solenoid valve and the rail pressure as the input
and load signals, and a programmable MF3 develop-
u and output y respectively, a family of ARX mod-
ment master box takes the role of ECU driving the
els can be obtained, according the above mentioned
injectors and the control valve.
design steps (Lino et al., 2006):
Figure 3 refers to a typical transient operating con-

1 − a1 q−1 y(t) = b0 q−1 − b1 q−2 u(t), (13) dition, and compares experimental and simulation re-
sults. With a constant 40 bar input pressure, the sys-
where q−1 is the shift operator and a1 , b0 , b1 are con- tem behaviour for a constant t j = 3ms injectors open-
stant parameters. The j-step optimal predictor of a ing time interval, while varying engine speed and
system described by eq. 13 is (Rossiter, 2003): solenoid valve driving signal has been evaluated. The
engine speed is composed of ramp profiles (Fig. 3(c)),
ŷ(t + j|t) = G j ∆u(t + j − 1) + Fj y(t), (14) while the duty cycle changes abruptly within the inter-
val [2%, 12%] (Fig. 3(d)). Figures 3(a) and 3(b) show
where G j and Fj are polynomials in q−1 , and ∆ is that the resulting dynamics is in accordance with the
the discrete derivative operator. Let r be the vec- expected behaviour. A maximum error of 10% con-
tor of elements y(t + j), j = 1, . . . , N, depending on firms the model validity.
known values at time t. Then eq. (14) can be ex- After the validation process, the AMESim vir-
pressed in the matrix form ŷ = Gũ + r, being ũ = tual prototype was used to evaluate the GPC con-
[∆u(t), . . . , ∆u(t + N − 1)]T , and G a lower triangular troller performances in simulation by employing the
N × N matrix (Rossiter, 2003). AMESim-Simulink interface, which enabled us to ex-
If the vector w is the sequence of future reference- port AMESim models within the Simulink environ-
values, a cost function taking into account the future ment. The interaction between the two environments
errors can be introduced: operates in a Normal mode or a Co-simulation mode.
n o As for the former, a compiled S-function containing
J = E (Gũ + r − w)T (Gũ + r − w) + λũT ũ , the AMESim model is generated and included in the
(15) Simulink block scheme, and then integrated by the
16
12 12
experimental pressure
Control Chamber Pressure [bar]

10 10
Rail Pressure [bar]

8 8
6 6
4 4
simulated pressure
2 2
simulated pressure
0 20 40 60 80 100 120 140 160 0 20 40 60 80 100 120 140 160

Time [s] Time [s]
(a) (b)
7 14
6 12
Engine Speed [rpm x 1000]
5 10
duty cycle [%]

4 8
3 6
2 4
1 2
0 0
0 20 40 60 80 100 120 140 160 0 20 40 60 80 100 120 140 160
Time [s] Time [s]
(c) (d)
Figure 3: AMESim simulation and experimental results when varying duty cycle. and engine speed, with a constant t j = 3ms;
(a) control chamber pressure; (b) common rail pressure; (c) engine speed; (d) control signal duty cycle.
1100rpm, within a 20s time interval (Fig. 5(b)).

The control action applied to the real system guar-
antees a good reference tracking, provided that its
slope does not exceed a certain value (Fig. 5(a), time
intervals [0, 14] and [22, 40]). Starting from time
14s, the request of a quick pressure reduction causes
the control action to close the valve completely (Fig.
Figure 4: AMESim-MATLAB co-simulation interface.
5(c)) by imposing a duty cycle equal to 0. Thanks to
injections, the rail pressure (Fig. 5(a)) decreases to
the final 5bar reference value, with a time constant
Simulink solver. As for the latter, which is the case depending on the system geometry; the maximum er-
considered in this paper, AMESim and Simulink co- ror amplitude cannot be reduced due to the actuation
operate by integrating the relevant portions of models, variable saturation. Fig. 5(d) shows the injectors’ ex-
as shown in figure 4. citing time during the experiment. It is worth to note
The GPC controller was tuned referring to models that simulation and experimental results are in good
linearized at the starting equilibrium point, according accordance, supporting the proposed approach.
to design steps of Section 5. The test considered ramp
variations of the engine speed and load, for the system
controlled by a GPC with a N = 5 (0.5s) prediction 7 CONCLUSIONS
horizon. The input air pressure from the compres-
sor was always 30bar. The rail pressure reference was In this paper, we presented a procedure for integrat-
read from a static map depending on the working con- ing different models and tools for a reliable design,
dition and had a sort of ramp profile as well. The final optimization and analysis of a mechatronic system as
design step consisted in the application of the GPC a whole, encompassing the real process and the con-
control law to the real system. trol system. The effectiveness of the methodology has
In Fig. 5, the engine speed accelerates been illustrated by introducing a practical case study
from 1100rpm to 1800rpm and then decelerates to involving the CNG injection system for internal com-
17
24 2
20
Engine Speed [rpm x 1000]

1.8
Rail Pressure [bar]

16
1.6
12 simulated pressure
1.4
8
1.2
4
pressure reference
1
0 5 10 15 20 25 30 35 40 0 5 10 15 20 25 30 35 40
Time [s] Time [s]
(a) (b)
50 30
25
40
Injection Timing [ms]

20
duty cycle [%]
30
experimental duty cycle 15
20
simulated duty cycle 10
10
5
0 0
0 5 10 15 20 25 30 35 40 0 5 10 15 20 25 30 35 40
Time [s] Time [s]
(c) (d)
Figure 5: AMESim model and real system responses for speed and load ramp variations and a 30bar input pressure, when
controlled by a GPC with N = 5; (a) common rail pressure; (b) engine speed (c) duty cycle; (d) injectors exciting time interval.
bustion engines. The design process was organized In IEEE CCA 2006 International Conference, Mu-
in few steps: analysis of different candidate configu- nich, Germany.
rations carried out with the help of virtual prototypes Maione, B., Lino, P., DeMatthaeis, S., Amorese, C., Man-
developed in the AMESim environment; design and odoro, D., and Ricco, R. (2004). Modeling and Con-
performance evaluation of controllers designed on a trol of a Compressed Natural Gas Injection System.
WSEAS Transactions on Systems, 3(5):2164–2169.
simpler model of the plant employing the virtual pro-
totypes; validation of the control law on the real sys- Paynter, H. (1960). Analysis and Design of Engineering
tem. The implementation of the control law on the Systems. M.I.T. Press, Cambridge, MA.
real system represented a benchmark for the evalua- Rossiter, J. (2003). Model-Based Predictive Control: a
tion of performances of the approach. Experimental Practical Approach. CRC Press, New York.
results gave a feedback of benefits of the integration Stobart, R., A.May, Challen, B., and Morel, T. (1999). New
of the mechanical and control subsystems design, and Tools for Engine Control System Development. An-
proved the validity of the methodology. nual Reviews in Control, (23):109–116.
Streeter, V., Wylie, K., and Bedford, E. (1998). Fluid Me-
chanics. McGraw-Hill, New York, 9th edition.
IMAGINE S.A. (2004). AMESim User Manual v4.2.
REFERENCES Roanne, France.
van Amerongen, J. and Breedveld, P. (2003). Modelling
Ferretti, G., Magnani, G., and Rocco, P. (2004). Virtual of Physical Systems for the Design and Control of
Prototyping of Mechatronic Systems. Annual Reviews Mechatronic Systems. Annual Reviews in Control,
in Control, (28):193206. (27):87–117.
Zucrow, M. and Hoffman, J. (1976). Gas Dynamics. John
Lino, P., Maione, B., Amorese, C., and DeMatthaeis, S. Wiley & Sons, New York.
(2006). Modeling and Predictive Control of a New In-
jection System for Compressed Natural Gas Engines.
18
ON THE FORCE/POSTURE CONTROL OF A CONSTRAINED
SUBMARINE ROBOT
Ernesto Olguı́n-Dı́az and Vicente Parra-Vega

Robotics and Advanced Manufacturing, CINVESTAV-IPN, Saltillo, México
(ernesto.olguin,vicente.parra)@cinvestav.edu.mx
Keywords: Underwater vehicles, Force/position control, Sliding Modes Control.
Abstract: An advanced, yet simple, controller for submarine robots is proposed to achieve simultaneously tracking of
time-varying contact force and posture, without any knowledge of its dynamic parameters. Structural proper-
ties of the submarine robot dynamic are used to design passivity-based controllers. A representative simulation
study provides additional insight into the closed-loop dynamic properties for regulation and tracking cases. A
succinct discussion about inherent properties of the open-loop and closed-loop systems finishes this work.
1 INTRODUCTION control properly a fully immersed vehicle constrained

by rigid object; and on the other hand, the submarine
In the last decade, we have witness a surprising leap force control technology lies behind system require-
on scientific knowledge and technological achieve- ments, such as very fast sampling rates, even when
ments for submarine robots, from a simple torpedo the bandwidth of the submarine robot is very low.
to modern UAV. Those vehicles are at the same time Despite brilliant control schemes for free motion
tantamount scientific and technological challenges to submarine robots in the past few years, in particular
robotics, control and mechatronic researchers. First those of (Yoerger, 1985; Smallwood, 2001; Small-
efforts for free motion submarine robots were focused wood, 2004) those schemes does not guarantee for-
for motion control, with sensing capabilities, similar mally convergence of tracking errors, let alone si-
to the case of fix-base robots in our labs. Of course, multaneous convergence of posture and contact force
stable contact for submarine robots is a more com- tracking errors, there are several results that suggest
plex problem in comparison to the typical force con- empirically that a simple PD control structure be-
trol problem of robot manipulators fixed to ground be- haves as stiffness control for submarine robots. So far
cause not only due complementary complex dynam- the simultaneous convergence of time-varying contact
ics such as buoyancy and added masses, but to de fact forces and posture remains an open problem for the
that the vehicle reference frame is not longer inertial. PD-like controller (notice that for submarine robots
However, some tasks requires more challeng- the role of model-free controllers are very important
ing goal of establishing stable contact, which leads because it is very hard to known exactly the dynamic
us to study the problem of simultaneous force and model and its dynamic parameters).
pose (position and orientation) control of submarine Recently, some efforts have focused on how to
robots. When the submarine robot achieves contact, obtain simple control structures to control the time-
like pushing itself against a wall or polishing a sunken varying pose of the UAV under the assumption that
surface vessel or manipulating tools on submarine the relative velocities are low. For force control, vir-
pipe lines, forces are presented, and little is known tually none complete control system has been pub-
about these contact forces, let alone exploit them ei- lished. However, to move forward more complex
ther for design or control. The main general reason force controllers, we believe that more understanding
that this problem remains rather open is that we really of the structural properties of submarine robots in sta-
know so little about, on one hand, in how to model and ble contact to rigid objects are required.
19
To understand more about this problem, we show The generalized coordinates vector q ∈ IR6 is
that the open-loop rigid dynamics of submarine robots given on one hand by the 3 Cartesian positions x, y, z
subject to holonomic constraints (rigid contact) ex- of the origin of the submarine frame (Σv ) with respect
hibits similar properties of fixed-base robots. Then to a inertial frame (Σ0 ) fixed in some place on the sur-
almost any force/position control scheme with the face of the Earth; and on the other hand by any set of
proper adequation can be used. In this paper we attitude parameters that represent the rotation of the
have chosen the orthogonalization principle (Parra- vehicle’s frame with respect to the inertial one. Most
Vega and Arimoto, 1996) to propose a simple con- common sets of attitude representation such a Euler
troller with advanced tracking stability properties. angles, in particular roll-pitch-yaw (φ, θ, ψ), use only
3 variables (which is the minimal number of orien-
1.1 Contribution tation variables). Then, for a submarine vehicle, the
generalized coordinates q = (xv , yv , zv , φv , θv , ψv ) rep-
Despite the complexity of the problem, in this pa- resents its 6 degrees of freedom.
per, the performance of a robot submarine carry- The vehicle’s velocity ν ∈ IR6 is the vector repre-
ing out a very simple contact tasks under a novel senting both linear and angular velocity of the subma-
orthogonalization-based model-free controller is an- rine expressed in its own vehicle’s frame. This vector
(v) T (v) T
alyzed and discussed, with emphasize on structural is then defined as ν = (vv , ωv )T . The relation-
properties. The closed-loop shows robust tracking of ship between this vector and the generalized coordi-
time-varying contact force and posture, without any nates is given by the kinematic equation. The linear
knowledge of submarine dynamics. The proposed operator Jν (q) ∈ IR6×6 in (2) is built by the concate-
controller stands itself as a new controller which guar- nation of two transformations. The first is defined as:
antees simultaneous tracking of submarine robot with
I 0
a model-free decentralized control structure. Jq (q) = ∈ IR6×6
0 Jθ (φv , θv , ψv )
1.2 Organization Where Jθ ∈ IR3×3 is an operator that converts
time derivatives of attitude (orientation) parameters
Section 2 introduces the nonlinear differential alge- (0)
in angular velocity: ωv = Jθ (θv )θ̇v , with θv =
braic equations of the robot submarine dynamics in
(φv , θv , ψv ) defined as the orientation parameter vec-
T
error coordinates. Section 3 presents the control de-
tor. The operator Jq (q) maps the first time derivative
sign. Simulation data is presented for a Testbed robot
of the generalized coordinates to linear and angular
of IFREMER in 4, and finally section 5 discusses
velocities of the submarine vehicle, both expressed in
some structural properties useful for further under-
the inertial frame Σ0 .
stand robot submarine in contact tasks. Conclusions
The second operator transforms a 6 dimension
are stated in Section 6.
tensor from the inertial frame to vehicle’s frame by
JR (q) = diag{Rv0 , Rv0 } ∈ IR6×6 . Thus the Jacobian of
the vehicle used in the kinematic equation is a linear
2 THE MODEL operator defined as:
Jν (q) = JRT (q)Jq (q)
2.1 A Free Floating Submarine
In the dynamic equation (1), matrices
The model of a submarine can be obtained via Kirch- Mv ,Cv (ν), Dv (·) ∈ IR6×6 are respectively Inertia
hoff formulation (Fossen, 1994), the inclusion of hy- matrix, Coriolis matrix and Damping matrix. Mv
drodynamic effects such as added mass, friction and includes the terms of classical inertia plus the hydro-
buoyancy and the account of external forces/torques dynamic coefficients of the added mass effect (due
like contact effects (Olguı́n-Dı́az, 1999). The model to the amount of extra energy needed to displace the
is then expressed by the next set of equations: surrounding water when the submarine is moving).
This Inertia matrix is constant, definite positive and
(v)
Mv ν̇ +Cv (ν)ν + Dv (·)ν + gv (q) = u + F c (1) symmetric only when the submarine is complete
ν = Jν (q)q̇ (2) immersed and the relative water incidence velocity is
small (Fossen, 1994). This condition is met for most
From this set, (1) is called the dynamic equation while common activities of a ROV, for example during
(2) is called the kinematic equation. Both had been sampling recollection in not shallow waters. The
used long widely to express the dynamical behavior Coriolis vector Cv (ν)ν represents the Coriolis forces,
of free moving object in the 3D space. gyroscopic terms of the classical inertia effects plus
20
the quadratic velocity terms induced by the added as F c from this point further. The relationship with
mass. The Coriolis matrix in this representation the one expressed in the vehicle’s frame is given by
does not depend on the position but only on the (v)
F c = JRT (q)F c . This wrench represents the contact
velocity, in contrast to the same expression for a forces/torques exerted by the environment to the sub-
Robot Manipulator (Spong M.W., 1989). However marine as if measured in a non moving (inrtial) frame.
it fulfills the classic relationship for Lagrangian These forces/torques are given by the normal force of
systems: Ṁv − 2Cv (ν) = Q; Q + QT = 0, and is an holonomic constraint when in contact and the fric-
indeed skew symmetric. The Damping matrix tion due to the same contact. For simplicity in this
represents all the hydrodynamic effects of energy work, tangential friction is not considered.
dissipation. For that reason it is a strictly positive
definite matrix, Dv (q, ν,t) > 0 (Fossen, 1994). Its 2.2 Contact Force Due to a Holonomic
arguments are commonly the vehicle’s orientation
φv , θv , ψv , the generalized velocity ν, and the velocity Constraint
of the surrounding water. The diagonal components
represents the drag forces while the off-diagonal A holonomic constraint can be expressed as a function
components represent the lift forces. of the generalized coordinates of the submarine as
(v)
Finally, vectors gv (q), u, F c ∈ IR6 are all force ϕ(q) = 0 (6)
wrenches (force-torque vector) in the vehicle’s frame.
They represent respectively: gravity, input control with ϕ(q) ∈ IR . The number r stand for the num-
r
and the contact force. Gravity vector includes buoy- ber of contact points between the submarine and the
ancy effects and it does not depend on velocity but motionless rigid object. Stable contact appears only
on the orientation (attitude) of the submarine with re- when the robot submarine does not deattach from the
spect to the inertial frame. The contact force wrench object modeled by ϕ(q) = 0. In this case, the time
is the one applied by the environment to the subma- derivatives of this constraint are zero
rine. The input control are the forces/torques induced d n ϕ(q)
by the submarine thrusters in the vehicle frame. = 0; ∀ n ≥ 1 (7)
dt n
The dynamic model (1)-(2) can be rearranged by
replacing (2) and its time derivative into (1). The re- For n = 1 the last constraint becomes:
sult is one single equation model: ϕ̇(q) = Jϕ (q)q̇ = 0 (8)
Mq (q)q̈ +Cq (q, q̇)q̇ + Dq (·)q̇ + gq (q) = uq + τc (3) ∂ϕ(q)
where Jϕ (q) = ∂q ∈ IRr×n is the constraint jaco-
which has the form of any Lagrangian system. Its bian. Last equation means that velocities of the sub-
components fulfills all properties of such systems i.e. marine in the directions of constraint jacobian are re-
definite positiveness of inertia and damping matrices, stricted to be zero. This directions are then normal to
skew symmetry of Coriolis matrix and appropriate the constraint surface ϕ(q) at the contact point. As
bound of all components (Sagatun, 1991). a consequence, the normal component of the contact
The control input in this equation is obtained by a force has exactly the same direction as those defined
linear transformation of the real input using the linear by Jϕ (q). Then the contact force wrench can be ex-
operator given by the kinematic equation: pressed as
T
F c = Jϕ+ (q)λ (9)
uq = JνT (q)u. (4)
△ J
This last equation becomes important because it re- where Jϕ+ (q) = kJϕϕ k is a normalized version of the
lates the nominal control signal uq to be designed in constraint jacobian; and λ is the magnitude of the
the general coordinates space with the real physical normal contact force at the origin of vehicle frame:
input u that shall be implemented in the real vessel. λ = kF c k.
The contact effect is also obtained by the same trans- The free moving model expressed by (1)-(2),
formation. However it can be expressed directly from when in contact with the holonomic constraint can be
the contact wrench in the inertial frame (Σ0 ) by the rewritten as:
next relationship
Mv ν̇ + hv (q, ν,t) = u + JRT (q)Jϕ+
T
(q)λ (10)
(v) (0)
τc = JνT (q)F c = JqT (q)F c (5) ν = Jν (q)q̇ (11)
(0) ϕ(q) = 0 (12)
where the contact force F c is the one expressed in
the inertial frame. By simplicity it will be noted where hv (q, ν,t) = Cv (ν)ν + Dv (q, ν,t)ν + gv (q).
21
Equivalently, the model (3) is also expressed as the value of the normal contact force magnitude be-
comes:
Mq (q)q̈ + hq (q, q̇,t) = uq + Jϕ̄T (q)λ (13) h i−1
ϕ(q) = 0 (14) λ = Jϕ Jν−1 Mv−1 JRT Jϕ+
T
Jϕ Jν−1 Mv−1 hv − u
d
with hq (q, q̇,t) = Cq (q, q̇)q̇ + Dq (q, q̇,t)q̇ + gq (q) and − Jϕ Jν−1 ν − DJϕ Jν−1 ν − Pϕ(q) (18)
Jϕ̄ (q) = Jϕ+ (q)Jq (q). Equations (13)-(14) are a set dt
of Differential Algebraic Equations index 2 (DAE-2). h i−1
To solve them numerically, a DAE solver is required. = Jϕ Mq−1 Jϕ̄T Jϕ Mq−1 (hq − uq ) − J˙ϕ q̇
This last representation has the same structure and
properties as those reported in (Parra-Vega, 2001). −DJϕ q̇ − Pϕ(q) (19)
It can be pointed out that equation (19) compute the

2.3 Numerical Considerations contact force magnitude using the generalized coor-
dinates variables only. From this equation, desired
To compute the value of λ, the constrained La- force can be easily obtained. Also note that the pre-
grangian that fulfils the constrained movement, can be liminary inverse operator is only expressed by the in-
calculated using the second derivative of (7): ϕ̈(q) = ertia matrix inverse and the two constraint jacobians.
0. Then using the dynamic equation (either of them) The first is the gradient of the contact surface, while
and after some algebra its expression becomes either: the second is the normalized gradient with a simple
kinematic variation given by the operator Jq .
h i−1
λ = Jϕ Jν−1 Mv−1 JRT Jϕ+
T
Jϕ Jν−1 Mv−1 hv − u
d
− Jϕ Jν−1 ν (15) 3 CONTROL DESIGN
dt
h i−1 The introduction of a so called Orthogonalization
= Jϕ Mq−1 Jϕ̄T Jϕ Mq−1 (hq − uq ) − J˙ϕ q̇ Principle has been a key in solving, in a wide sense,
the force control problem of a robot manipulators.
(16)
This physical-based principle states that the projec-
The set of equations (10)-(11)-(15) or the set (13)- tion (dot product) of contact forces and joint gener-
(16) describes the constrained motion of the subma- alized velocities is zero. Then passivity arises from
rine when in contact to infinitely rigid surface de- torque input to generalized velocities. To preserve
scribed by (6). Numerical solutions of these sets can passivity, then, the closed-loop system must satisfy
be obtained by simulation, however the numerical so- the passivity inequality for controlled generalized er-
lution, using a DAE solver, can take too much effort ror velocities. This is true for robot manipulators with
to converge due to the fact that these sets of equa- fixed frame, and here we prove that this also hap-
tion represent a highly stiff system. In order to min- pens with robot whose reference frame is not inertial
imize this numerical drawback, the holonomic con- (fixed).
straint has been treated as a numerically compliant
surface which dynamic is represented by 3.1 Orthogonalization Principle and
Linear Parametrization
ϕ̈(q) + Dϕ̇(q) + Pϕ(q) = 0 (17)
Similar to (Liu et al., 1997), an orthogonal projection
This is known in the force control literature of robot
of Jϕ (q) arises onto the tangent space at the contact
manipulators as constrained stabilization method,
point. This tangent space can be calculated by the
which bounds the nonlinear numerical error of in-
definition of the following operator
tegration of the backward integration differentiation
formula. With a appropriate choice of parameters △
Q(q) = In − JϕT (q)Jϕ (q) ∈ IRn×n (20)
P ≫ 1 and D ≫ 1, the solution of ϕ(q,t) → 0 is
bounded. This dynamic is chosen to be fast enough to where In is an identity matrix of proper size. No-
allow the numerical method to work properly. In this tice that rank{Q(q)} = n − r since rank{Jϕ (q)} = r.
way, it is allowed very small deviation on the com- Also notice that Qq̇ = q̇ due to the definition of Q and
putation of λ, typically in the order of −106 or less, (8). Therefore, and according to the Orthogonaliza-
which may produce, according to some experimental tion Principle the integral of (τ, q̇) is upper bounded
comparison, less than 0.001% numerical error. Then, by −H (t0 ), for H (t) = K + P, and passivity arise
22
for fully immersed submarines without inertial frame. with α > 0, η > 0. Finally, consider the following
Therefore, passivity-based control is a powerful op- reference q̇r function:
tion to control such systems. Zt
On the other hand it is known that equation (13) q̇r = Q q̇d − σq̃ + Sd p − γ1 sgn{Sqp (t)}dt (30)
t0
can be linearly parameterized as follows Zt
+βJϕ SF − SdF + γ2 sgn{SqF (t)}dt
T
Mq (q)q̈ +Cq (q, q̇)q̇ + Dq (·)q̇ + gq (q) = Y (q, q̇, q̈)Θ t0
(21) where the parameters β, σ, γ1 and γ2 are constant ma-
where the regressor Y (q, q̇, q̈) ∈ IRn×p is composed trices of appropriate dimensions; and sgn(y) stands
of known nonlinear functions and Θ ∈ IR p by p un- for the entrywise signum function of vector y,
known but constant parameters. Using (30) into s, it arises
s = Q(q)Svp − βJϕT (q)SvF (31)
3.2 Change of Coordinates where the orthogonal extended position and force
manifolds Svp and SvF , respectively, are given by
Z
A variation Y r of regressor Y can be designed such Svp = Sqp + γ1 sgn(Sqp (ς))dς (32)
that Z
SvF = SqF + γ2 sgn(SqF (ς))dς (33)
Y r (q, q̇, q̇r , q̈r )Θ = (22)
Mq (q)q̈r + [Cq (q, q̇) + Dq (·)] q̇r + gq (q) Notice that although the time derivative of q̇r is
discontinuous, it will not be used in the controller.
where (q˙r , q̈r ) are some time functions yet to be de- In the next subsection we show that if Ṡvp , ṠvF are
fined. Then the open loop equation (13) can be written bounded, and for large enough γ1 , γ2 , then simultane-
by adding and subtracting (22) as ous tracking of contact force and posture is achieve.
Mq (q)ṡ = − [Cq (q, q̇) + Dq (·)] s (23) 3.4 Model-Free Second Order Sliding
−Y r (q, q̇, q̇r , q̈r )Θ + Jϕ̄T (q)λ + uq Mode Control
△ Consider the following nominal continuous control

where s = q̇ − q̇r is called the extended error. The law:
problem of control design for the open loop equation
uq = −Kd Q(q)svq − βJϕT (q)svF (34)
(23) is to find uq such that exponential convergence
arises when Y r Θ is not available. +Jϕ̄T (q) −λd + SF
Z
3.3 Orthogonal Nominal Reference +γ2 Jϕ̄ (q) tanh(µSqF ) + η sgn(SqF )
T
with µ > 0 and Kd = KdT > 0, ∈ IRn×n . This nominal

Consider that qd (t) and λd (t) are the desired trajecto- control, designed in the q-space can be mapped to the
ries functions for position and contact force. Consider original coordinates model, expressed by the set (1)-
△
as well, the tracking errors defined as q̃ = q(t) − qd (t) (2), using the next relationship
△
for position error while λ̃ = λ(t) − λd (t) stand for the u = Jν−T (q)uq (35)
force contact error. Consider also the next set of defi- thus, the physical controller u is implemented in terms
nitions: of a key inverse mapping Jν−T .
△ 3.4.1 Closed-loop System
Sp = q̃˙ + σq̃ (24)
△
Sd p = S p (t0 )e−α(t−t0 ) (25) The open loop system (23) under the continuous
△ model-free second order sliding mode control (34)
Sqp = S p − Sd p (26) yields to
Zt
SF
△
= λ̃dt (27) Mq ṡ = − [Cq + Dq + Kd ] s −Y r Θ (36)
t0

△
+Jϕ̄ λ̃ + ηSF + γ2 Jϕ̄ Z + τ
T T ∗
SdF = SF (t0 )e−η(t−t0) (28) Z
SqF
△
= SF − SdF (29) +γ2 Jϕ̄ sgn(SqF ) + η sgn(SqF )
T
23
where Z = tanh(µSqF ) − sgn(SqF ) and τ∗ ≡ 0 is the small initial conditions, if we multiply the derivative
virtual control input for the passivity analysis. T , we obtain
of Sqp in (32) by Sqp
T T
3.4.2 Stability Analysis Sqp Ṡqp = −γ1 |Sqp | + Sqp Ṡvp (42)
≤ −γ1 |Sqp | + |Sqp ||Ṡvp |
Theorem 1 Consider a constrained submarine (13)- Substituting (40) into (42) yields
(16) under the continuous model-free second order
sliding mode control (34). The Underwater system
T
Sqp Ṡqp ≤ −(γ1 − ε3 )|Sqp | (43)
yields a second order sliding mode regime with local where γ1 must be chosen such that γ1 > ε3 . The equa-
exponential convergence for the position, and force tion (43) is precisely the condition for the existence of
tracking errors. a sliding mode at Sqp (t) = 0. The sliding mode is es-
Proof. A passivity analysis hS, τ∗ i indicates that tablished in a time t ≤ |Sqp (t0 )|/(γ1 − ε3 ), and accord-
the following candidate Lyapunov function V quali- ing to the definition of Sqp (below (32)), Sqp (t0 ) = 0,
fies as a Lyapunov function which simply means that Sqp (t) = 0 for all time. We
see that if we multiply the derivative of (33) by SvTf ,
1 we obtain
V = (sT Mq s + βSvF
T
SvF ) (37)
2 T
SqF ṠqF T
= −γ2 |SqF | + SqF ṠvF (44)
for a scalar β > 0. The time derivative of the Lya- ≤ −γ2 |SqF | + |SqF ||ṠvF |
punov candidate equation immediately leads to
substituting (41) into (44) yields
V̇ = −sT (Kd + Dq )s − βηSvF
T
SvF − sT Y r Θ SqT f Ṡq f ≤ −(γ2 − ε4 )|Sq f | (45)
+s T
γ2 Jϕ̄T Z (38) where γ2 must be chosen such that γ2 > ε4 . The equa-
≤ −sT Kd s − βηSvF
T
SvF + kskkY r Θk tion (45) is precisely the condition for the existence
+kskkγ2 kkJϕ̄ kkZk of a sliding mode at SqF (t) = 0. The sliding mode
is established in a time t ≤ |SqF (t0 )|/(γ2 − ε4 ) and,
≤ −sT Kd s − βηSvF
T
SvF + kskkεk according to (33), SqF (t0 ) = 0, which simply means
that Sq f (t) = 0 for all time, which simply implies that
where it has been used the skew symmetric property
λ → λd exponentially fast.
of Ṁ − 2C(q, q̇), the boundedness of both the feed-
back gains and submarine dynamic equation (there
exists upper bounds for M,C(q, q̇), g(q), q̇r , q̈r ), the
smoothness of ϕ(q) (so there exists upper bounds for 4 SIMULATION RESULTS
Jϕ and Q(q)), and finally the boundedness of Z. All
these arguments establish the existence of the func- Force control of submarine robots is in its infancy,
tional ε. Then, if Kd , β and η are large enough such therefore simulations study must be kept rather simple
that s converges into a neighborhood defined by ε cen- to enter into the intricacies of the numerical solutions.
tered in the equilibrium s = 0, namely Later on, the complexity of the simulation study will
be be increased such that underwater disturbances can
s → ε as t → ∞ (39) include unmodelled submarine dynamics, such as lat-
eral marine current with high Reynolds number, to
This result stands for local stability of s provided that mention one. Then, the simulation study has been
the state is near the desired trajectories for any initial made on a simplified platform of a real submarine.
condition. This boundedness in the L ∞ sense, leads to Data has been obtained from the Vortex system of
the existence of the constants ε3 > 0 and ε4 > 0 such IFREMER (IFREMER, 1992). Simulator presents
that only vertical planar results (only in the x-z plane), so
|Ṡvp | < ε3 , (40) the generalized coordinates for this case of study are:
 
|ṠvF | < ε4 (41) xv
q =  zv  (46)
An sketch of the local convergence of Svp is as fol-
θv
lows1 . Locally, in the n − r dimensional image of Q,
∗ = QS ∈ IRn . Consider now that un-
we have that Sqp The vehicle velocities are
qp  
∗ , such that for
der an abuse of notation that Sqp = Sqp uv
v =  wv  (47)
1 The strict analysis follows Liu, et. al. qv
24
400
where uv and wv are linear velocities (surge and
Input force on x [N]

200
heave) and qv is the angular velocity in the x-z plane. 0
The holonomic constraint is given by a vertical sur- −200
face located at 2 meters from the origin. This is ex- −400
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
pressed as: 400
ϕ(q) ≡ x − 2 (48)
Input force on z [N]

200
Initial conditions were calculated to be at the con- 0
tact surface with no force. Simulations with simple −200
−400
PD were also performed as a comparison tool. The 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
model-free control parameters are as follows: 40
Input torque [Nm]

20
α β γ1 γ2 0
4 0.025 0.0025 10−3 −20
Kd σ η µ −40
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
time [sec]
200M̂v 5 1000 1
Figure 2: Inputs controlled forces u, in vehicle’s frame (con-
where M̂v is made by the diagonal values of the continuous line for model-free second order sliding mode con-
stant Inertia matrix when expressed in the vehicle’s trol, dotted line for PD control).
frame. For de PD the gains were defined as Kp =
100M̂v and Kd = 20M̂v . The numerical dynamic in- 2000
duced in the constraint surface were performed with

1000
P = 9x106 and D = 36x103 .
Contact force [N]
4.1 Set-Point Control Case −1000
−2000
The task is very simple. It consist on a change of deep, 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
from 1m to 1.5m, and orientation from 5 degrees to 0, 200

while the submarine remains in contact with a con-
Contact force (detail) [N]
stant contact force of 170N. 100
0
2.01 1
Surge (uv) [m/s]
x position [m]
2.005 0.5 −100
2 0
−200
1.995 −0.5 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
time [sec]
1.99 −1
0 0.5 1 1.5 2 0 0.5 1 1.5 2
1.6
Figure 3: Contact force magnitude λ (continuous line for
model-free second order sliding mode control, dotted line
Heave (wv) [m/s]
1.5
1.4
Deep [m]
1 for PD control).
1.2
0.5
1
0
0 0.5 1 1.5 2 0 0.5 1 1.5 2
6 0.1 variability in the position of the contact point in the

Angular velocity [rad/s]
Orientation [deg]
4
0
−0.1
the direction normal to that force (the x coordinate).
2 −0.2 This is more visible in the velocity curve where veloc-
0
−0.3 ity in the x direction should not occur if the constraint
−0.4
0 0.5 1
time [sec]
1.5 2 0 0.5 1
time [sec]
1.5 2 is infinitely rigid. The same transients are present in
the force graphic where a noticeable difference be-
Figure 1: Generalized coordinates q and vehicle velocities tween simple PD and model-free second order sliding
ν, in vehicle’s frame (continuous line for model-free second
order sliding mode control, dotted line for PD control).
mode control is present. The big difference is that al-
though PD can regulate with some practical relative
accuracy it is not capable to control force references,
Figures 1 and 2 shows respectively position, ve- either constant nor variable. Figure 3 shows the tran-
locity and force inputs. The difference in settling time sient of the contact force magnitude λ (only the 1st
has been explicitly computed in order to visualize the second is shown). The second window shows with
qualitative differences. In any case, this time inter- more detail the transient in this contact force due to
val can be modified with appropriate tuning of gain the numerical modification. In this graphic is is clear
parameters. Notice that there is some transient and that no force is induced with the PD scheme. In the
25
−4 2.01 1
x 10
2 400
x position [m]
2.005 0.5
Surge [m/s]
0
Force contac error [N]

200 2 0
−2
x error [m]
1.995 −0.5
−4 0
1.99 −1
0 1 2 3 4 5 0 1 2 3 4 5
−6
−200 1
−8 1.5
0.5
Heave [m/s]
Deep [m]
−10 −400
0 0.5 1 1.5 2 0 0.5 1 1.5 2 1 0
−0.5
0.1 0.5
−1
0 1 2 3 4 5 0 1 2 3 4 5
0 5
Orientation error [deg]
6 0.1
Angular velocity [rad/s]

−0.1 4
Deep error [m]
Orientation [deg]
0
−0.2 3 4
−0.1
−0.3 2 2 −0.2
−0.4 1
−0.3
0
−0.5 0 −0.4
0 1 2 3 4 5 0 1 2 3 4 5
−1 time [sec] time [sec]
0 0.5 1 1.5 2 0 0.5 1 1.5 2
time [sec] time [sec]
Figure 6: Generalized coordinates q and vehicle velocities
Figure 4: Set-point control errors: q̃ = q(t) − qd and λ̃ = ν, in vehicle’s frame for the tracking case (continuous line
λ(t) − λd (continuous line for model-free second order slid- for model-free second order sliding mode control, dotted
ing mode control, dotted line for PD control). line for PD control, dashed line for the tracking reference).
0.15 0.01 500

Input force on x [N]
0.1
0.005
0.05 0
Sqp
0 0
−0.05 −500
−0.005 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
−0.1
500
Input force on z [N]
−0.01
0 1 2 3 4 5 0 0.5 1 1.5 2
0.3 0.04
0.2
−500
0.02 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
0.1
0 40
SqF
Input torque [Nm]
0
−0.1 20
−0.2 0
−0.02
−0.3
−20
−0.4 −0.04
0 1 2 3 4 5 0 0.5 1 1.5 2 −40
time [sec] time [sec] 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
time [sec]
Figure 5: Control variables Sqp and SqF for the model-free Figure 7: Inputs controlled forces u, in vehicle’s frame (con-
second order sliding mode control. tinuous line for model-free second order sliding mode con-
trol, dotted line for PD control).
case of the model-free 2nd order sliding mode, force

set-point control is achieved very fast indeed. Fig- As for the set-point control case, the position error
ure 4 shows the set-point control error for position (3 and velocities in the constrained direction remain very
curves) and force. Is again evident the substantial dif- small as can be see in figure 6. Also in this figure, the
ference in schemes for force control. Figure 5 shows deep response has a lag from the tracking reference.
the convergence of the extended position and force As for the input control forces u the steady state re-
manifolds. They do converge to zero and induce there semble to those of the set-point control. For the deep
after the sliding mode dynamics. coordinate, the control input tends to to be the same
after some relative small time. Again, the PD does
4.2 Tracking Control Case not provide any force in the constraint direction. The
last can be verified in figure 8 where, for the PD case,
This task differs from the set-point control case in that there is no contact force although the position of the
the deep is a periodic desired trajectory center at 1 contact point is indeed in contact. For the model-free
meter deep with 1 meter amplitude (pick to pick) and 2nd order sliding mode control, the contact force has
a 5 sec period. Contact desired force remains constant the same shape as for the set-point control case. The
at 170N. same transient, due to the numerical consideration for
26
2000 0.05 0.01
1000 0.025 0.005

Contac force [N]
0 0 0
−1000
−0.025 −0.005
−2000
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 −0.05 −0.01
time [sec] 0 1 2 3 4 5 0 0.5 1 1.5 2
200
0.3 0.04
Contac force (detail) [N]
0.2
100
0.02
0.1
0 0
0
−0.1
−100
−0.2
−0.02
−0.3
−200
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7
−0.4 −0.04
time [sec] 0 1 2 3 4 5 0 0.5 1 1.5 2
Figure 8: Contact force magnitude λ (continuous line for Figure 10: Control variables Sqp and SqF for the model-free
model-free second order sliding mode control, dotted line second order sliding mode control.
for PD control).
−4
5 SOME DISCUSSIONS ON
x 10
2
0
400
STRUCTURAL PROPERTIES
Force contact error [N]
200
−2
x error [m]
−4 0 In the beginning of this study, we were expecting an

−6 involved control structure for submarine robots, in
−200
−8 comparison to fixed-base robots, with additional con-
−10
0 1 2 3 4 5
−400
0 1 2 3 4 5
trol terms to compensate for hydrodynamic and buoy-
ancy forces. However, surprisingly, no additional
0.15 6 terms were required! Due to judicious treatment of
0.1 5
the mathematical model, and careful mapping of ve-
Orientation error [deg]
0.05 4
Deep error [m]
0 3
locities through jacobians in vehicle frame and gen-
−0.05 2 eralized frame, it suffices only proper mapping of the
−0.1 1 gradient of contact forces. This results open the door
−0.15 0
to study similar force control structures developed for
−0.2 −1
0 1 2 3
time [sec]
4 5 0 1 2 3
time [sec]
4 5
fixed-base robots, and so far, the constrained subma-
rine robot control problem seems now at reach.
Figure 9: Set-point control errors: q̃ = q(t) − qd and λ̃ =
λ(t) − λd (continuous line for model-free second order slid-
ing mode control, dotted line for PD control). 5.1 Properties of the Dynamics
As it as pointed out in Section 2, the model of a sub-

marine robot can be expressed in either a self space
the almost rigid constraint surface, is present. The (where the inertia matrix is constant for some con-
tracking errors for the x position and force has been ditions that in practice are not difficult to met) or
reduced for the PD case certainly due to numerical in a generalized coordinate space (in which the in-
initial conditions. In the tracking of deep, the model- ertial matrix is no longer constant but the model is
free 2nd order sliding mode has apparently the same expressed by only one equation, likewise the kine-
performance as the simpler PD controller. As it has matic lagrangian chains). Both representations has
been pointed out by other authors, for this kind of sys- all known properties of lagrangian systems such skew
tem, the PD controller remains as the best and simpler symmetry for the Coriolis/Inertia matrix, bound-
controller for position tracking. However, it does not less of all the components, including buoyancy, and
track any force reference, which is the essence of the passivity preserved properties for the hydrodynamic
model-free 2nd order sliding mode control. The last added effects. The equivalences of these representa-
figure 10 shows the convergence of the extended position of the same model can be found by means of the
tion and force manifolds to zero and induce there after kinematic equation. The last one is a linear opera-
the sliding mode dynamics. tor that maps generalized coordinates time derivative
27
with a generalized physical velocity. This relationship Structural properties of the open-loop dynamics of
is specially important for the angular velocity of a free submarine robots are key to design passivity-based
moving object due to the fact that time derivative of controllers. In this paper, an advanced, yet sim-
angular representations (such a roll-pitch-yaw) do not ple, controller is proposed to achieve simultaneously
stand for the angular velocity. However, there is al- tracking of time-varying contact force and posture,
ways a correspondence between these vectors. For without any knowledge of his dynamic parameters.
external forces this mapping is indeed important. It This is a significant feature of this controller, since in
relates a physical force/torque wrench to the general- particular for submarine robots the dynamics are very
ized coordinates q = (xv , yv , zv , φv , θv , ψv ) whose last difficult to compute, let alone its parameters.
3 components does not represent a unique physical A simulation study provides additional insight
space. In this work, such mapping is given by Jq and into the closed-loop dynamic properties for set-point
appears in the contact force mapping by the normal- control and tracking case.
ized operator Jϕ̄ . The operator Jq can be seen as the
difference of a Analytical Jacobian and the Geometric
Jacobian of a robot manipulator. REFERENCES
5.2 The Controller Fossen, T. I. (1994). Guidance and Control of Ocean Vehi-
cles. Chichester.
Notice that the controller exhibits a PD structure IFREMER (1992). Project vortex: Modélisation et simula-
plus a nonlinear I-tame control action, with nonlin- tion du comportement hidrodynamique d’un véhicule
sous-marin asservi en position et vitesse. Technical
ear time-varying feedback gains for each orthogonal report, IFREMER.
subspace. It is in fact a decentralized PID-like, coined
Liu, Y., Arimoto, S., Parra-Vega, V., and Kitagaki, K.
as ”Sliding PD” controller. It is indeed surprising that (1997). Decentralized adaptive control of multiple
similar control structures can be implemented seem- manipulators in cooperations. International Journal
ingly for a robot in the surface or below water, with of Control, 67(5):649–673.
similar stability properties, simultaneous tracking of Olguı́n-Dı́az, E. (1999). Modélisation et Commande d’un
contact force and posture. Of course, this is possible Système Véhicule/Manipulateur Sous-Marin. PhD
under a proper mapping of jacobians, key to imple- thesis, Laboratoire d’Automatique de Grenoble.
ment this controller. Parra-Vega, V. (2001). Second order sliding mode control
for robot arms with time base generators for finite-
5.3 Friction at the Contact Point time tracking. Dynamics and Control.
Parra-Vega, V. and Arimoto, S. (1996). A passivity-based
adaptive sliding mode position-force control for robot
When friction at the contact point arises, which is ex- manipulators. International Journal of Adaptive Con-
pected for submarine tasks wherein the contact object trol and Signal Processing, 10:365–377.
is expected to exhibits a rough surface, with high fric- Sagatun, S.I.; Fossen, T. (1991). Lagrangian formulation of
tion coefficients, a tangential friction model should underwater vehicles’ dynamics. Decision Aiding for
be added in the right hand side. Particular care must Complex Systems, IEEE.
be employed to map the velocities. Complex friction Smallwood, D.A.; Whitcomb, L. (2001). Toward model
compensators can be designed, similar to the case of based dynamic positioning of underwater robotic ve-
force control for robot manipulators, therefore details hicles. volume 2. OCEANS,IEEE Conference and Ex-
are omitted. hibition.
Smallwood, D.A.; Whitcomb, L. (2004). Model-based dy-
namic positioning of underwater robotic vehicles: the-
ory and experiment. IEEE Journal of Oceanic Engi-
6 CONCLUSIONS neering, 29(1).
Spong M.W., V. M. (1989). Robot Dynamics and Control.
Although a PD controller, recently validated by Yoerger, D.; Slotine, J. (1985). Robust trajectory control
(Smallwood, 2004) has a much simpler structure and of underwater vehicles. Oceanic Engineering, IEEE
very good performance for underwater vehicles in po- Journal, 10(4).
sition control schemes, it does not deal with the task
of contact force control.
The desired contact force can be calculated via
equation (19) directly in the generalized frame in
which it occurs.
28
FUZZY-SYNTACTIC APPROACH TO PATTERN RECOGNITION
AND SCENE ANALYSIS
Marzena Bielecka
Department of Geoinformatics and Applied Computer Science
Faculty of Geology, Geophysics and Environmental Protection
AGH University of Science and Technology, Kraków, Poland
[email protected]
Marek Skomorowski, Andrzej Bielecki

Institute of Computer Science, Jagiellonian University, Kraków, Poland
{skomorowski,bielecki}@softlab.ii.uj.edu.pl
Keywords: Syntactic pattern recognition, graph grammars, fuzzy graphs, parallel parsing, robot vision system.
Abstract: In syntactic pattern recognition an object is described by symbolic data. The problem of recognition is to
determine whether the describing mathematical structure, for instance a graph, belongs to the language
generated by a grammar describing the mentioned mathematical structures. So called ETPL(k) graph
grammars are a known class of grammars used in pattern recognition. The approach in which ETPL(k)
grammars are used was generalized by using probabilistic mechanisms in order to apply the method to
recognize distorted patterns. In this paper the next step of the method generalization is proposed. The
ETPL(k) grammars are improved by fuzzy sets theory. It turns out that the mentioned probabilistic approach
can be regarded as a special case of the proposed one. Applications to robotics are considered as well.
1 INTRODUCTION analyse the scene they act on (Ferber, 1999; Scheier

and Pfeifer, 1999). For instance in (Kok et al., 2005)
The fundamental idea in syntactic pattern so called coordination graphs are used for solving a
recognition is using of symbolic data like strings, behaviour management problem in a multi-robot
trees and graphs for representation of a class of system. In this graph a node represents an agent and
recognized objects (Chen et al., 1991; Fu, 1982; an edge indices that a corresponding agents have to
Jakubowski, 1997; Jakubowski and Stąpor, 1999). coordinate their actions.
The general scheme of syntactic pattern recognition The use of graph grammars for syntactic pattern
and a scene analysis is following (Fu, 1982). After recognition is relatively rare because of difficulties
in building a syntax analyser of such grammars.
pre-processing the recognized object is segmented in
Therefore every result in building efficient parser for
order to recognize the primitives the pattern consists
graph grammars is valuable. An example of such
of and relations between them. Decision whether the result is a parser for, so called, ETPL(k) (embedding
analysed pattern representation belongs to the class transformation-preserving production-ordered k-left
of objects describing by a given grammar is made nodes unambiguous) grammars introduced in
basing on the parsing algorithm. This classical (Flasiński, 1993 and 1998). An efficient parsing
approach can be applied in robotics, for instance in algorithm for ETPL(k) graph grammars, which the
vision systems and in manufacturing for description computational complexity is O(n2), has been
and analysis of the production process (Chen et al., constructed in (Flasiński, 1993). The so-called IE
1991; Yeh et al. 1993). It seems also be effective for (indexed edge-unambiguous) graphs have been
applying in multi-agent systems, particularly in defined in (Flasiński, 1993) for a description of
embodied cognitive ones because such agents should pattern (scenes) in syntactic pattern recognition.
be equipped with symbolic and explicit Nodes in an IE graph denote pattern primitives.
representation of the surrounding world in order to Edges between two nodes in an IE graph represent
29
spatial relations between pattern primitives. a) b)

However, in practice, structural descriptions may
contain pattern distortions. An idea of a probabilistic
improvement of syntactic recognition of distorted
patterns represented by graphs is described in
(Flasiński and Skomorowski, 1998, Skomorowski
1998) and (Skomorowski, 1999). To take into
account all variations of a distorted pattern under
study, a probabilistic description of the pattern was
introduced. A random IE graph approach (Flasiński
and Skomorowski, 1998, Skomorowski, 1999, Figure 1: Holes in plate and their graph representation.
Skomorowski, 2000) is proposed for such a
description and an efficient parsing algorithm for IE 2.2 Second Example
graphs is presented. Its computational complexity is
O(n2) as well. Considering the previous example assume that
The purpose of this paper is to present an idea of robotic inspection of technological process is based
approach to syntactic recognition of fuzzy patterns on statistical distribution of inaccuracy frequencies
represented by fuzzy IE graphs, followed the (Flasiński and Skomorowski, 1998, Noori and
example of random IE graphs used for distorted Radford, 1995). If a hole is made in a sufficient
pattern description. It turns out that, in a way, the accuracy it is accept by the system. Not only the
fuzzy approach is a generalization of the hole shape but also its location should be taken into
probabilistic one. Fuzziness allows us not only consider. Since inaccuracies of holes location
described distortions in analysed patterns but also influence each other, the simple statistical analysis
give us possibility to describe in proper way patterns can be insufficient to make a decision. In such a case
that can not be presented univocally. Furthermore a fuzzy inference can be applied. Then, holes and
there are a wide class of problems in which objects their locations can be represented by fuzzy sets and
and/or spatial relations are described by fuzzy sets. membership functions can be calculated using the
statistical distribution according to the methodology
described in (Bielecka, 2006). Let, like in the first
example, the inspection system is based on a
2 MOTIVATIONS syntactic pattern recognition in which the holes are
represented by nodes of graphs and spatial relations
In this section a few example, in which the fuzzy- between holes by graph edges. In this example both
syntactic approach seems to be natural, are the graph nodes and its edges would be described as
presented. fuzzy sets. Automatic focusing vision system for
inspection of size and shape and positions of small
2.1 First Example holes in the context of precision engineering,
described in (Han and Han, 1999), is an example of
Assume that during a manufacturing process a a system performing such type of task.
robotic inspection system checks type of a hole in a
making elements, for instance plates, and spatial
2.3 Third Example
relations between holes. Assume also that there are a
few standard types of holes and circular and Consider an autonomous mobile agent. Assume that
quadratic ones are among them – Fig.1a. Let, it has to navigated in an unchanging environment. A
furthermore, the inspection system be based on a helicopter flying autonomously in a textured urban
syntactic pattern recognition approach in which the environment is an example of such agent (Muratet et
holes are represented by nodes of graphs and spatial al., 2005). As it has been already mentioned it
relations between holes by graph edges – see Fig.1b. should be equipped with symbolic representation of
A quadratic hole with rounded vertices can be the surrounding world in order to analyse the scene
regarded as a fuzzy object with partial membership they act on (Ferber, 1999; Scheier and Pfeifer,
to classes of both circular and quadratic holes – see 1999). Let its vision system be a syntactic one based
Fig.1. In this example nodes description as fuzzy on graph representation of the spatial relationships
sets is a natural approach. In this case membership
between obstacles the agent should navigate among.
functions describing fuzzy sets can be define basing
Let according to, for instance, the optimization
on axiomatic method (Bielecka, 2006).
requirements, the system prefers one direction but
30
admits also another ones allowing to navigate applied to classical graphs (i.e. non-fuzzy ones) –
without collision. In such a case the scene would be example 3.
represented by a classical (i.e. not fuzzy) IE graph
but directions the agent can choice would be
represented by fuzzy sets – see Fig.2. The decision 3 FUZZY IE GRAPHS
making system would be based on fuzzy inference.
Recall a definition of IE graph (Flasiński, 1993).
Definition 3.1
An IE graph is a quintuple H=(V, E, Σ, Γ, ϕ) where:
V is a finite, non-empty set of nodes of a graph with
assigned indexes in univocally way,
Σ is a finite, non-empty set of node labels,
Γ is a finite, non-empty set of edge labels,
E is a set of graph edges represented by triplet
(v, λ, w) where v, w∈V, λ∈Γ and an index of v
is smaller than an index of w,
ϕ:V→Σ is a nodes labeling function.
Let us assume that, due to pattern fuzziness, possible

Figure 2: Detection of possible directions of motion.
IE graphs associated with a given example pattern
(scene) may look like IE graphs shown in Fig.3.
2.4 Fourth Example
(a) (b)
Let us consider computer-aided analysis and
recognition of pathological wrist bone lesions
(Tadeusiewicz and Ogiela, 2005; Ogiela et al.,
2006). This method consists on analysis of the
structure of the said bones based on palm
radiological images. During pre-processing Figure 3: Possible IE graphs describing a given scene.
operations in the examined X-ray images the bones
contours were separated and a graph representing In the case of the IE graph shown in Fig.3a fuzziness
bones and spatial relation between them was concerns pattern primitives represented by the node
spanned. In the beginning, spatial relationships 2 labeled by tree and the node 3 labeled by bus. In
given by the graph edges were represented by single the case of the IE graph shown in Fig.3b fuzziness
directions (Tadeusiewicz and Ogiela, 2005) but later concerns a pattern primitive represented by the node
each basic spatial relationship was represented as 4 labeled by bus and a spatial relation between
angular interval (Ogiela et al., 2006). The second pattern primitives represented by the edge
approach can be interpreted in such a way that every connecting the node 3 with the node 4. Assume that
basic spatial relationship is described as a fuzzy set both labeled objects in nodes of a graph and spatial
for which its membership function has positive relations are represented by fuzzy sets of a first order
values on the specified angular interval and is equal (Zadeh L.A., 1965) with membership functions μi
to zero outside this interval. It should be mentioned and νi respectively. Let, furthermore, the set of all
that in (Ogiela et al., 2006) such interpretation was objects Σ be m-elemental and the set of all spatial
not considered. relations be k-elemental. Let us define, informally, a
Recapitulating, four examples in which various fuzzy IE graph as an IE graph in which nodes labels
are replaced by a vector μ = [µ1,...,µm] of values of
aspects of possibility of improve syntactic
membership functions μi, i∈{1,...,m} and edges
approach by fuzzy sets has been discussed. The
labels are replaced by vector ν = [ν1,...,νk] of values
classical IE graphs can be generalized by including of membership functions νj, j∈{1,...,k}.
fuzzy sets to description their nodes (example 1), Let propose a formal definition of a fuzzy IE
edges (example 4), both the nodes and edges graph
(example 2) and fuzzy inference approach can be
31
Definition 3.2 being a function of n variables can be introduced:

A fuzzy IE graph is a quintuple H=(V, E, Σ, Γ, Φ)
where: n n −1
V is a finite, non-empty set of nodes of a graph with T (a1 ,..., an ) = T (ai ) = T ( T (ai ), an ) (4)
i =1 i =1
assigned indices in univocal way, Having a fuzzy IE graph R the fuzzy measure of an
Σ is a finite, non-empty set of node labels, outcome graph r is calculated as
containing, say, n elements, P S
Γ is a finite, non-empty set of edge labels, λ (r ) = T ( T ( μ αf (α ) ), T (ν gβ( β ) )) (5)
p =1 s =1
containing, say, k elements,
E = V× Θ ×V is a set of fuzzy graph edges
where α is a number of a regarded node, β is a
represented by triplet (v, Θs, w) where v, w∈V number of an edge, f(α) - is a chosen component
and i(v) < i(w) i.e. an index of v is smaller than an number of a vector μα whereas g(β) is a number of
index of w, Θ = [Θ1 ,..., Θ k ] is represented by
s s s
component of a vector νβ. If a product is used as a T-
norm then the presented parsing (see section 4) is
[(λ1 ,ν 1s ),..., (λk ,ν ks }] where ν is is a value of a identical as the random parsing described in
membership function of succeeding edge labels (Skomorowski, 1998). In calculations presented in
for a s-th edge, the next section the minimum Tm-norm is used.
Φ: V → Π1×…×Πm where for every nodes
Φ (ν i ) = (Π1i ,..., Π im ) where Π ik = (σ k , μ ki ) , 4 PARALLEL PARSING
σ k ∈ Σ , μ ki is a value of a membership function
Given an unknown pattern represented by a fuzzy IE
of succeeding nodes labels for an i-th node. graph R, the problem of recognition of a pattern
under study is to determine if an outcome IE graph r,
The fuzzy measure of an outcome IE graph, obtained from the fuzzy IE graph R, belongs to a
obtained form a given fuzzy IE graph, is equal to the graph language L(G) generated by an ETPL(k) graph
value of T-norm of the values components of the grammar G. In the proposed parallel and cut-off
node and edge vectors. Recall axiomatic definition strategy of fuzzy IE graph parsing for an efficient,
of T-norms which is given in, for instance, that is with the computational complexity O(n2),
(Rutkowski, 2005) - definition 4.22, page 80. analysis of fuzzy patterns (scenes) a number of
simultaneously derived graphs is equal to a certain
Definition 3.3 number limit. In this case, derived graphs spread
through the search tree, but only the best, that is with
T-norm is a function T:[0,1]×[0,1]→[0,1] satisfying maximum measure value, limit graphs are expanded.
the following axioms: Let us consider a graph shown in Fig.4a and a
(i) T(a,b) = T(b,a), production shown in Fig.4b. Suppose that the
(ii) T( T(a,b),c ) = T( a,T(b,c) ), embedding transformation for the production shown
(iii) if a ≤ b and c ≤ d then T(a,b) ≤ T(c,d), in Fig.4b is C(r, input) = {(d, b, r, input)} and C(u,
(iv) T(a,0) = 0 and T(a,1) = a. output) = {(e, B, r, input)}. During a derivation, a
non-terminal A in the node 2 of a graph shown in
Theorem Fig.4a is removed and the graph of the production
The functions Tm and Tu given by the formulae shown in Fig.4b is put in the place of the removed
non-terminal A. The first item of the embedding
transformation for the production: C(r,input) = {(d,
Tm(a,b) = min{a,b} and Tu(a,b) = a⋅b (1) b, r, input)} means that the edge r of the graph
shown in Fig.4a should connect the node d of the
are T-norms. The function Tw given by the formulae production graph with the node b of the graph shown
in Fig.4a. The second item of the embedding
Tw(a,1) = a, Tw(a,b) = 0 for a≠1 and b≠1 (2) transformation for the production: C(u, output) =
is a T-norm as well. Furthermore, for every {(e, B, r, input)} means that the edge u of the graph
a,b∈[0,1] if a function T is a T-norm then shown in Fig. 4a should be replaced by the edge r
connecting the node e of the production graph with
the node B of the graph shown in Fig.4a. Thus, after
Tw(a,b) ≤ T(a,b) ≤ Tm(a,b) (3)
the application of the production shown in Fig.4b to
Thanks to the property (ii) in Definition 3.3 T-norm
32
the node indexed by 2 of the graph shown in Fig.4a C(r, input) = {(b, a, r, input)}
we obtain a graph shown in Fig.4c. C(t, output) = {(b, A, t, output), (a, A, r, input)}
(3)
C(r, input) = {(d, a, r, input)}
C(t, output) = {(d, A, t, output), (E, A, r, input)}
(4)
C(s, input) = {(d, a, s, input)}
Figure 4: An example derivation step in an ETPL(k) graph (5)

grammar.
C(s, input) = {(d, a, s, input)}, C(t, input) = {(d, b, t, input)}
Suppose that we analyze an unknown fuzzy pattern C(r, output) = {(d, c ,r, output)}, C(v, output) = {(d, D ,v, output)}
represented by a fuzzy IE graph shown in Fig.5. (for
clarity, only non-zero membership functions vectors (6)
components are specified).
C(s, input) = {(d, a, s, input)}, C(t, input) = {(d, b, t, input)}

C(r, output) = {(d, c ,r, output), (d, a, r, output)}
C(v, output) ={(d, D ,v, output)}
(7)
C(s, input) = {(b, a, s, input)}, C(t, input) = {(b, b, t, input)}

C(r, output) = {(b, c, r, output),(b, a, r, output)}
C(v, output) = {(b, D , v, output)}
(8)
Figure 5: An example fuzzy IE graph representing an C(s, input) = {(g, b, s, input)}

unknown distorted pattern. C(v, output) = {(g, f, v, output)}
(9)
Let us assume that a number of simultaneously
derived graphs is equal to 2 (that is limit = 2). C(s, input) = {(a, b, s, input)}, C(v, output) = {(a, f, v, output)}
Furthermore let us assume that we are given an
ETPL(k) graph grammar G with a starting graph Z (10)
shown in Fig.6 and a set of productions shown in
Fig.7. C(t, input) = {(g, a, t, input)}
C(v, input) = {(h, d ,u, input), (h, b, u, input)}
(11)
C(t, input) = {(a, a, t, input)}
C(v, input) = {(b, d, u, input), (b, b, u, input)}
Figure 7: A set of productions of an ETPL(k) graph.

grammar G.
Figure 6: A starting graph Z of an example ETPL(k) graph
grammar G.
In the first step of the derivation, after the
application of the production (1), shown in Fig.7, to
(1)
the node indexed by 2 of the starting graph Z, shown
C(r, input) = {(b, a, r, input)} in Fig.6, we obtain a graph q1 shown in Fig.8a.
C(t, output) = {(b, A, t, output), (c, A, r, input)} Similarly, after the application of the production (2)
(2)
33
to the node indexed by 2 of the starting graph Z we The contributions of nodes indexed by 6 of the
obtain a graph q2 shown in Fig.8b. The graphs q1 graphs q1,6 and q1,7 are not taken into account in this
case as the node indexed by 6 and labeled by F in
(a) (b) the graph q1,7 is not a terminal one. Consequently,
the contribution of the edge connecting nodes
indexed by 3 and 6 as well as the contribution of the
edge connecting nodes indexed by 6 and 7 in the
graphs q1,6 and q1,7 are not taken into account.
Similarly, we compute the following values: λ(q2,6)
Figure 8: Derived graphs q1 and q2. = 0.2 and λ(q2,7) = 0.2. As λ(q1,6) > λ(q1,7) > λ(q2,6)
= λ(q2,7) we choose the graphs q1,6 and q1,7 for
and q2 (Fig.8) are admissible for further derivation, further derivation, that is we choose two graphs with
that is they can be outcome graphs obtained from the maximum value (limit = 2). Similarly, in two next
fuzzy IE graph shown in Fig.5. The application of the steps of derivation the final outcome IE graph is
production (3) to the node indexed by 2 of the obtained – see Fig.11. The derived graph q1,7,10,8 is
starting graph Z does not lead to a graph which can also an outcome IE graph obtained from the parsed
be an outcome graph obtained from the fuzzy IE fuzzy IE graph shown in Fig.5.
graph shown in Fig.5. Thus, a graph obtained after
the application of the production (3) to the node
indexed by 2 of the starting graph Z is not
admissible for further derivation. As in the analyzed
example a number of simultaneously derived graphs
is equal to 2 we expand the graphs q1 and q2 in the
second step of derivation.
In the second step of derivation, after the application

of the productions (6) and (7) (Fig.7) to the node
indexed by 3 of the graph q1 (Fig.8a) we obtain Figure 9: Derived graphs q1,6 and q1,7.
graphs q1,6 and q1,7 shown in Fig.9. Similarly, after
the application of the productions (6) and (7) to the
node indexed by 3 of the graph q2 (Fig.8b) we obtain
graphs q2,6 and q2,7 shown in Fig.10. The
application of the production (5) to the node indexed
by 3 of the graphs q1 and q2 (Fig.8) leads to graphs
which can not be outcome graphs obtained from the
fuzzy IE graph shown in Fig.5, as they miss the node
indexed by 7 and labeled by f of the fuzzy IE graph
shown in Fig.5. Thus, graphs obtained after the
application of the production (5) to the nodes
Figure 10: Derived graphs q2,6 and q2,7.
indexed by 3 of the graphs q1 and q2 (Fig.8) are not
admissible for further derivation. The graphs q1,6,
q1,7 (Fig.9) and q2,6, q2,7 (Fig.10) are admissible for
further derivation, that is they can be outcome
graphs obtained from the fuzzy IE graph shown in
Fig.5.
Because in the analyzed example a number of
simultaneously derived graphs is equal to 2 we
should choose only two graphs from among the
graphs q1,6, q1,7 (Fig.9) and q2,6, q2,7 (Fig.8) for
further derivation. In order to do it, compute the
following values: λ(q1,6) = 0.7 and λ(q1,7) = 0.3. Figure 11: A derived graph q1,7,10,8.
34
5 CONCLUSIONS Jakubowski R., 1997. Structural approach to modeling and

analysis of 2D-shapes, Bulletin of the Polish Academy
of Sciences – Technical Sciences, 45, 373-387.
In this paper we have proposed an idea of a new
Jakubowski J., Stąpor K., 1999. Structural method in
approach to recognition of fuzzy patterns
perceptual organization of 2D-curve detection,
represented by graphs. The problem has been representation and analysis, Bulletin of the Polish
considered in the context of pattern recognition and Academy of Sciences – Technical Sciences, 47, 309-
scene analysis with references to robotics (Han and 324.
Han, 1999; Kok et al., 2005; Muratet et al., 2004; Kok J.R., Spaan M.T.J., Vlassis N., 2005. Non-
Petterson, 2005) and applications in medicine communicative multi-robot coordination in dynamic
(Tadeusiewicz and Ogiela, 2005, Ogiela et al., environments, Robotic and Autonomous Systems, 50,
2006). To take into account variations of a fuzzy 99-114.
pattern under study, a description of the analysed Muratet L., Doncieux S., Briere Y., Meyer J.A., 2005. A
pattern based on fuzzy sets of the first order was contribution to vision-based autonomous helicopter
introduced. The fuzzy IE graph has been proposed flight in urban environments, Robotics and
here for such a description. The idea of an efficient, Autonomous Systems, 50, 195-229.
that is with the computational complexity O(n2), Noori, H., Radford R., 1995. Production and Operation
parsing algorithm presented in (Flasiński, 1993) is Management. Tostal Quality and Responsiveness,
extended, so that fuzzy patterns, represented by fuzzy McGraw-Hill.
Ogiela M.R., Tadeusiewicz R., Ogiela L., 2006. Image
IE graphs, can be recognized. In the algorithm a T-
languages in intelligent radiological palm diagnostics,
norm is used for calculation of value of membership Pattern Recognition, 39, 2157-2165.
measure of output graphs. Such solution makes that Petterson Ola, 2005. Execution monitoring in robotics: A
the algorithm is very flexible. In particular if survey, Robotics and Autonomous Systems, 53, 73-88.
arithmetic product is used as a T-norm, the Rutkowski L., 2005. Artificial Intelligence Techniques and
algorithm is the same as the random one described in Methods, PWN, Warszawa (in Polish).
(Skomorowski, 1998). Scheier C., Pfeifer R., 1999. The embodied cognitive
science approach. In: Tschacher W., Dauwalde J.P.
(eds): Dynamics, Synergetics, Autonomous Agents,
Studies of Nonlinear Phenomena and Life Science
REFERENCES Vol.8., World Scientific, Singapore, New Jersey,
London, Hong Kong, 159-179.
Bielecka M, 2006. A method of membership function Skomorowski M., 1998. Parsing of random graphs for
construction in fuzzy systems. In: Cader A., scene analysis, Machine Graphics and Vision 7, 313-
Rutkowski L., Tadeusiewicz R., Żurada J. (eds): 323.
Challenging Problems of Science – Computer Science, Skomorowski M., 1999. Use of random graph parsing for
Academic Publishing House EXIT, Warszawa, 111- scene labeling by probabilistic relaxation, Pattern
117. Recognition Letters 20, 949-956.
Chen S.L., Chen Z., Li R.K., 1991. A DGR method for Skomorowski M., 2006. Syntactic recognition of syntactic
extracting the topology of an upper-half profile of a patterns by means of random graph parsing, Pattern
turned part from CAD data, Computer Integrated Recognition Letters 28, 572-581.
Manufacturing, 4, 45-56. Tadeusiewicz R., Ogiela M.R., 2005. Picture languages in
Ferber J., 1999. Multi-Agent Systems. An Introducing to automatic radiological palm interpretation,
Distributed Artificial Intelligence. Addison-Wesley, International Journal of Applied Mathematics and
Harlow. Computer Science, 15, 305-312.
Flasiński M., 1993. On the parsing of deterministic graph Yeh S., Kamran M., Nnaji B., 1993. CAD-based
languages for syntactic pattern recognition. Pattern automatic object recognition, Journal of Design and
Recognition 26, 1-16. Manufacturing, 3, 57-73.
Flasiński M., 1998. Properties of NLC graph grammars Zadeh L.A., 1965. Fuzzy sets, Information and Control, 8,
with a polynomial membership problem. Theoretical 338-353.
Computer Science 201, 189-231.
Flasiński M., Skomorowski M, 1998. Parsing of random
graph languages for automated inspection in
statistical-based quality assurance system. Machine
Graphics and Vision 7, 565-623.
Fu, K.S., 1982. Syntactic pattern recognition and
applications, Prentice-Hall, New York.
Han M., Han H., 1999. Automatic focusing vision system
for inspection of size and shape of small hole, Journal
of the Korean Society of Precision Engineering, 16,
no.10, 80-86.
35
VISUAL ALIGNMENT ROBOT SYSTEM: KINEMATICS, PATTERN
RECOGNITION, AND CONTROL
SangJoo Kwon and Chansik Park

School of Aerospace and Mechanical Engineering, Korea Aerospace University, Goyang-city, 412-791, Korea
Keywords: Visual alignment, robotics, parallel mechanism, precision control, pattern recognition.
Abstract: The visual alignment robot system for display and semiconductor fabrication process largely consists of multi-
axes precision stage and vision peripherals. One of the central issues in a display or semiconductor mass pro-
duction line is how to reduce the overall tact time by making a progress in the alignment technology between
the mask and panel. In this paper, we suggest the kinematics of the 4PPR parallel alignment mechanism with
four limbs unlike usual three limb cases and an effective pattern recognition algorithm for alignment mark
recognition. The inverse kinematic solution determines the moving distances of joint actuators for an iden-
tified mask-panel misalignment. Also, the proposed alignment mark detection method enables considerable
reduction in computation time compared with well-known pattern matching algorithms.
1 INTRODUCTION paring with normal three-limb stages but inevitably it

leads to a more difficult control problem. Although
In the flat panel display and semiconductor industry, a commercial alignment stage with four driving axes
the alignment process between mask and panel is con- was announced in (Hephaist Ltd., 2004), reports on
sidered as a core technology which determines the the kinematics and control can be rarely found.
quality of products and the productivity of a manu- The next issue is the vision algorithm to extract the
facturing line. As the sizes of panel and mask in the position of alignment marks. In many machine vision
next generation products increases but the dot pitch systems, the normalized gray-scale correlation (NGC)
becomes smaller, the alignment systems must fulfill method (Manickam et al., 2000) has been used as
more strict requirements in load capacity and con- a representative template matching algorithm. How-
trol precision. The alignment system largely has two ever, it requires long computation time since all pix-
subsystems. One is the multi-axes robotic stage to els in the template image are compared in the match-
move the mask in a desired location with acceptable ing process. An alternative to reduce the computation
alignment errors and the other one is the vision sys- time is the point correlation (PC) algorithm (Kratten-
tem to recognize the alignment marks printed in mask thaler et al., 1994) but it is still weak to the rotation of
and panel surfaces. In a display or semiconductor object and the change of illumination condition. As
production line, the alignment systems are laid out an another, the edge-based point correlation (Kang
in series as subsystems of pre-processing and post- and Lho, 2003) was proposed to mitigate the effect
processing equipments such as evaporation, lithogra- of illumination change. In fact, commercial vision
phy, and joining processes. libraries, e.g., (Cognex Ltd., 2004), are adopted in
The alignment stage has at least three active joints many visual alignment systems considering the sta-
to determine planar three degrees of freedom align- bility of the vision algorithm. However, they are com-
ment motions. It usually adopts a parallel mecha- putationally inefficient from the point of view that
nism, specifically when it is used for display panel they have overspec for the monotonous vision en-
alignment, since it has the advantage of high stiffness vironment of alignment systems (e.g., simple mark
and high load capacity. In this paper, we are to dis- shape, fine light, and uniform background). In this
cuss the inverse kinematics of the parallel stage which paper, by incorporating the binarization and labeling
has four prismatic-prismatic-revolute (4PPR) limbs algorithm (Gonzalez and Wood, 2002) together and
where all the base revolute joints are active ones. For designing a geometric template matching scheme in-
the same-sized moving platform, the four-limb mech- stead of the general methods: NGC (Manickam et al.,
anism brings higher stiffness and load capacity com- 2000) and PC (Krattenthaler et al., 1994), an efficient
36
VISUAL ALIGNMENT ROBOT SYSTEM: KINEMATICS, PATTERN RECOGNITION, AND CONTROL
CCD/optics/illumination
pattern recognition algorithm for alignment marks is
Frame
suggested, which can greatly reduce vision process- Grabber
ing time comparing with commercial products.

DAC
Related to the autonomous alignment system, sev- mask
panel
eral articles can be found. A two step alignment
counter/
algorithm was suggested for wafer dicing process PC processor decoder
(Kim et al., 2004) and a self-alignment method for

wafers was investigated using capillary forces (Mar-
AC servo motor
tin, 2001). A visionless alignment method was also
studied (Kanjilal, 1995). As a trial to improve the Figure 1: Schematic of visual alignment control system.
alignment speed, an additional sensor was integrated
S/W
(Umminger and Sodini, 1995) and a modified tem-
pattern recognition
plate matching algorithm was presented (Lai and inverse kinematics precision motion
trajectory planning control design
Fang, 2002).
align
H/W
align mark alignerror
error align
compensation
feedback
feedback alignstage
stage
template compensation control (UVW)
algorithm
algorithm control (UVW)
2 VISUAL ALIGNMENT SYSTEM internal
internal
sensor fusion sensor
sensor
optimal filtering
2.1 System Configuration vision
vision
system
system UVW stage
design
Figure 1 shows the schematic of a PC-based experi- vision/optics
/illumination
mental setup for the control of visual alignment sys-
tem. Broadly speaking, it consists of the vision sys- Figure 2: S/W and H/W components of visual alignment
tem to detect alignment marks on mask and panel system.
and the stage control system to compensate misalign-
ments. The vision system has normally two CCD The advantages of parallel manipulation mechanisms
cameras and optical lenses, illumination equipment, comparing with serial ones are i) high payload-to-
and frame grabber board to capture mark images. In weight ratio, ii) high structural rigidity since the pay-
real production lines, prior to the visual alignment, load is carried by several limbs, iii) high manipulation
the pre-alignment process puts the marks of mask and accuracy since active joints are distributed in parallel
panel into the field-of-view of CCD Cameras. Then, and the joint errors are non-cumulative, and iv) sim-
the vision system determines the mark positions in the ple inverse kinematic solution. But, they suffer from
global frame and the misalignment distance in camera smaller workspace and singular configurations. Al-
coordinate can be converted into the moving distances though the movable range of the parallel stage is very
of joint actuators through the inverse kinematic solu- small (usually a few mms), it is actually enough to
tion of alignment stage. compensate misalignments between mask and panel.
As denoted in Fig 2, the feedback control system To position the moving platform (in which the
in the alignment stage has the hierarchical feedback mask is mounted) in a planar location, the task space
loops, where the outer visual servoing loop deter- of alignment stage must have at least three degrees
mines the misalignment quantity between mask and of freedom and then it requires at least three active
panel and the inner joint control loop actively com- joints. Hence, it is common for the parallel alignment
pensates it. Due to system uncertainties such as fric- stage to have three active limbs. However, if an ex-
tion and backlash, the alignment process is usually not tra driving limb is added to support the moving plat-
completed by one visual feedback but a few cycles are form, the stiffness and load capacity can be much in-
repeated. creased. The visual alignment testbed with four driv-
ing axes is shown in Fig. 3, where the motion of the
2.2 Parallel Alignment Stage rotary motor in each limb is transferred to the moving
platform through a ballscrew, cross-roller guide (for
As the size of flat panel displays including TFT/LCD, planar translational motion) and cross-roller ring(for
PDP, and OLED becomes larger and larger, the align- rotational motion).
ment stage is required to have higher load capacity
and wider moving platform. In this regard, the motion
control performance of alignment stage is directly re-
lated to the productivity of the manufacturing process.
37
CCD C2
L2
OM (mx , my )
X ∆x ∆φ
CCD 2
∆y
U W
Moving platform L1
OP ( px , p y )
V
Cross roller guide & ring C1
CCD 1
AC servo motor & Ballscrew
Figure 4: Determination of misaligned posture between
mask and panel (circle: mask marks, cross: panel marks).
Figure 3: Visual alignment stage with four active joints.
fixed base
+V
3 KINEMATICS OF 4PPR +W
PARALLEL MECHANISM
3.1 Determination of Misalignments moving platform

y
When the centroids of alignment marks of panel and
mask have been obtained through the image process- x
ing, it is trivial to get the misaligned posture between
panel and mask. In Fig. 4, let the two align marks
PPR joints
in the panel from the respective CCD image have
the centroids of C1 = (xC1 , yC1 ) and C2 = (xC2 , yC2 )
and those in the mask have L1 = (xL1 , yL1 ) and L2 = +U
(xL2 , yL2 ) in the global frame. Then, the center of line +X
connecting two centroids can be written as
Figure 5: Planar 3-DOF, 4PPR parallel mechanim.
px = (xC1 + xC2 )/2, py = (yC1 + yC2 )/2 (1)
for the panel marks and also where λ is the dimension of the space in which the
mechanism is intended to function, n the number of
mx = (xL1 + xL2 )/2, my = (yL1 + yL2 )/2 (2) links, j the number of joints, fi degrees of relative mo-
tion permitted by joint i, and f p the passive degrees of
for the mask ones. If the mask is to be aligned to
freedom. In the case of a planar 4PPR parallel mech-
the panel, the misaligned distance between mask and
anism shown in Fig. 5, in which the moving platform
panel is given by
is supported by four limbs with prismatic-prismatic-
xC1 − xL1 + xC2 − xL2 revolute joints, we have λ = 3, n = 2 × 4 + 1 + 1 in-
∆x = px − mx = (3)
2 cluding fixed base and moving platform, j = 4 × 3,
yC − yL1 + yC2 − yL2 ∑ fi = 12 × 1 and f p = 0. Hence, the degrees of free-
∆y = py − my = 1 (4)
dom of the 4PPR mechanism is F = 3 as is expected.
2
yC2 − yC1 yL2 − yL1 In parallel manipulators, every limb forms closed-
∆φ = tan−1 − tan−1 (5) loop chain and the number of active limbs is typically
xC2 − xC1 xL2 − xL1
equal to the number of degrees of freedom of the mov-
in (x, y) directions and orientation, respectively. ing platform. Moreover, each limb has the constraint
of having more joints than the degrees of freedom.
3.2 Mobility of 4ppr Alignment Stage Hence, at least three limbs must have an active joint
(the first P in this case) to achieve the 3-DOF motion
The degrees of freedom of a mechanism can be found of moving platform and the remaining limb becomes
by the Grubler criterion (Tsai, 1999): passive. On the other hand, if all the four limbs are
F = λ(n − j − 1) + ∑ fi − f p ,
actuated to increase the rigidity of motion, the actu-
(6)
i
ation redundancy problem is present and a sophisti-
38
cated control logic is required to avoid mechanical

C
singularity.
D
V
3.3 Inverse Kinematics S R
W
The inverse kinematic problem is to seek the moving O2 ∆φ
ranges of input joints which correspond to the end-
∆y
effector movement of a mechanism, i.e., moving plat- O1 ∆x
form in this case. In Fig. 6, the square of a fixed base
is defined by the four fixed points (P, Q, R, S) and the B
moving platform is connected to the limbs at the four A
points (A, B,C, D). We assume that the two squares X
y
have the same side length of h and the global coordi- P Q U
nates system is located at the center (O1 ) of the fixed
base. x
Then, the positions of fixed points (P, Q, R, S) Figure 6: Moving distances of active (solid line) and pas-
are given by (xP , yP ) = (− 12 h, − 21 h), (xQ , yQ ) = sive (dotted line) joints for a misalignment posture.
( 12 h, − 21 h), (xR , yR ) = ( 12 h, 21 h), and (xS , yS ) =
(− 12 h, 21 h). Moreover, assuming that the position of straightforward. However, the situation is reversed in
point A in the moving platform is known, the positions parallel mechanisms, where the inverse kinematics is
of the other three connecting points can be expressed rather simple as shown in the former section but the
as direct kinematics is more complicated. The inverse
(xB , yB ) = (xA + h cos ∆φ, yA + h sin ∆φ) (7) kinematics is important for the control purpose while
the direct kinematics is also required for the kinematic
(xC , yC ) = (xB − h sin ∆φ, yB + h cos ∆φ) (8) analysis of end-point.
(xD , yD ) = (xA − h sin ∆φ, yA + h cos ∆φ) (9) Although finding out the direct kinematics in po-
In Fig. 6, we have (xA , yA ) = O1 A = O1 O2 + O2 A sition level could be a long-time procedure, it be-
where O1 O2 = (∆x, ∆y) and comes an easy task in velocity level. By differenti-
ating (13)-(16) with respect to the time, the follow-
cos ∆φ − sin ∆φ − 21 h ing Jacobian relationship between joint space velocity
O2 A = R(∆φ)O1 P =
sin ∆φ cos ∆φ − 12 h and task space one is given.
(10)    
2 h(cos ∆φ − sin ∆φ)
1  
Hence, the position of point A can be written by the U̇ 1 0
 V̇   0 1 1
h(cos ∆φ − sin ∆φ)  ẋ
misalignment variables as  = 2   ẏ  (17)
2 h(cos ∆φ − sin ∆φ)
Ẇ  −1 0 1 
xA = ∆x − h(cos ∆φ − sin ∆φ)/2 (11) φ̇
Ẋ 0 −1 12 h(cos ∆φ − sin ∆φ)
yA = ∆y − h(sin ∆φ + cos ∆φ)/2 (12)
which can be simply represented by
As denoted in Fig. 6, the moving distances of in-
put prismatic joints have the following relationships q̇(t) = J(p) ṗ(t) (18)
(note the positive directions defined in Fig. 5): U = Considering the Jacobian J ∈ ℜ4×3 in (18),
xB − xQ , V = yC − yR , W = xS − xD , and X = yP − yA . the columns are linearly independent (rank(J)= 3).
Now, by substituting (7)–(12) into the above expres- Hence, J T J ∈ ℜ3×3 is invertible and there exists a left-
sions, we finally have inverse J + ∈ ℜ3×4 such that J + = (J T J)−1 J T and the
U = ∆x + h(cos ∆φ + sin ∆φ − 1)/2 (13) linear system (18) has an unique solution ṗ for every
V = ∆y + h(cos ∆φ + sin ∆φ − 1)/2 (14) q̇ (Strang, 1988). In the sequel, the direct kinematic
solution at current time can be determined by
W = −∆x + h(cos ∆φ + sin ∆φ − 1)/2 (15)
X = −∆y + h(cos ∆φ + sin ∆φ − 1)/2 (16) ṗ(t) = J + q̇(t) (19)
Zt
→ p(t) = (J T J)−1 J T q̇(t)dt (20)
t0
3.4 Forward Kinematics
In serial mechanisms, the inverse kinematic solution
is generally hard to find, but the direct kinematics is
39
4 PATTERN RECOGNITION
A typical visual alignment system provides a struc-
tured vision environment. For example, CCD cam-
era and illumination is under static condition and the
objects to be recognized are just two kinds of align-
ment mark for mask and panel. Moreover, the shape (a) Original image and the histogram
of marks are usually symmetric ones such as circle,
cross, rectangle, and diamond, in which case the fea-
ture points of an object can be easily determined. We
consider just circle and cross shaped marks because
they are most common in display and semiconductor
industry. In this section, an efficient alignment mark
recognition algorithm in Fig 10 is suggested by com-
bining the conventional labeling technique and a ge-
ometric template matching method which is designed (b) Bottom-Hat transformed image and the histogram
by analyzing the characteristics of alignment marks, . Figure 7: Comparison of original and transformed image
Basically, it is assumed that the alignment marks are and the histograms (b = 20 pixels).
in the field-of-view (FOV) of cameras after the pre-
alignment process in the industrial visual alignment
system. Small noise patterns in the binary image can be
recognized as independent areas during the labeling
4.1 Preprocessing process. They can be eliminated by the opening and
closing method:
Bottom-Hat Transform: Before applying the label- f˙ = ( f ◦ b) · b (22)
ing algorithm, the gray image from the CCD cam-
era is converted into the binary image. However, a where the morphology b is a small (e.g., 3 by 3)
proper binarization is impossible when the illumina- square matrix whose components are all 1, ( f ◦ b) an
tion is nonlinear or non-steady due to the limit of light opening operator, and f˙ the filtered image.
source or other unexpected light disturbances. In or- Labeling: If the objects in a captured image are
der to eliminate these effects, we can use the bottom- not overlapped, an easily accessible algorithm to ex-
hat transform which is a morphology method (Gonza- tract them is the labeling technique. To separate
lez and Wood, 2002), where the transformed image h marks from the noise-filtered image, we first apply
can be calculated by the labeling algorithm to the area chained by eight-
h = ( f · b) − f (21) connectivity. Once the labeling process is finished,
a number of areas including the marks will be la-
where f represents the original image, b the circular
beled. As explained in Fig 10, if the labeled area is
morphology, and ( f · b) the closing operation between
not counted, a new image should be captured by re-
f and b. By the closing operation, objects smaller
peating the pre-alignment process. If the labeled area
than the size of b are eliminated and the rest of back-
is only one, it is highly possible that two marks are
ground is extracted. The size of b is usually deter-
overlapped and each mark should be extracted by ap-
mined experimentally. The figure 7 shows an exam-
plying another pattern recognition scheme. If there
ple.
are 2 labeled areas, the marks of mask and panel are
Dynamic Thresholding and Noise Filtering: To not overlapped. Then, the centroids of marks can be
segregate marks from the background in the Bottom- simply calculated by the center of area method:
Hat transformed image, we apply the binarzation al-
gorithm. Among several methods to determine best 1 n−1 1 n−1
threshold value, the following repetition method can Xc = ∑
n i=0
Xi , Yc = ∑ Yi
n i=0
(23)
be used: 1) Set the initial threshold T , which is the
average between the maximum and the minimum of where Xc and Yc represent the central point of labeled
brightness from the binary image. 2) Divide the im- image and Xi and Yi the horizontal and vertical po-
age into class G1 and G2 by T and calculate the av- sitions of the pixels. Finally, if there are more than
erages m1 and m2 of them. 3) Calculate T = (m1 + 3 labeled areas, as denoted in Fig 10, the areas which
m2 )/2. 4) Repeat step 2 and step 3 until m1 and m2 have less pixels than a specified number must be elim-
are not changed. inated through an extra filtering process.
40
4.2 Geometric Template Matching Image capture
When the two marks of mask and panel are over- Bot-Hat transform
lapped, i.e., when the number of labeled area is only
one in Fig 10, the labeling algorithm alone is not Binarization
enough but the marks can be separated in terms of

Noise filtering
any pattern matching method.
Since the alignment marks used in the display or
Labeling
semiconductor masks are very simple, their templates
0
can be readily characterized by a few feature points.
How many 2
First, for the circular mark in Fig. 8(a) where the ra- Filtering
over 3 regions?
dius (r) is a unique trait, for example, the eight pixels
1
along the circumference can be selected as the fea- 1
How many? Pattern matching
ture points. All the pixels in the matching area can
be scanned by assuming them as the center of circle. 2
Then, the centroid of circular mark can be found when Calculate centroids
all the feature pixels have the same brightness of 255

Figure 10: Overall flow of alignment mark recognition.
(white) in the binarized image. In reality, since the
actual circular mark has a thickness, every pixel that
fulfills this condition must be stored in the memory by the rectangle in Fig. 9(a) with the size of (M −
stack and the final centroid can be calculated by av- 2r) × (N − 2r) pixels in the M × N pixel image. Un-
eraging the coordinate values. Similarly, five feature der the assumption that the exact central point of the
points can be chosen for the cross mark with length l circular mark has been found, the matching area for
as in Fig. 8(b). However, differently from the circular the cross mark is equal to 2(r + l) × 2(r + l) pixels as
mark, we have to consider the rotation factor. Hence, shown in Fig. 9(b) regarding the farthest positions of
the matching process should be performed by rotating the cross mark in the overlapped image.
the template from 1 to 90 degrees for all pixels in the The normalized correlation (NC) (Manickam
matching area. et al., 2000) and the point correlation (PC) algorithm
(Krattenthaler et al., 1994) are most widely used tem-
plate matching methods in machine vision systems.
However, since the NC requires a vector operation
r for all pixels in the image and template to determine
the correlation coefficient, it is too time consuming.
Although the PC algorithm reduces the dimension of
computation time, it is still not appropriate for real-
time applications because it includes feature points
(a) Circular mark (b) Cross mark extraction process. On the contrary, in our matching
algorithm, the feature points of alignment marks are
Figure 8: Feature pixels of circular mark (radius = 2 mm)
and cross mark (length = 1 mm). geometrically extracted based on the analysis of mark
shape and the individual feature points and the image
pixels at the same coordinates are directly compared
without any vector operations.
The overall sequence of the suggested algorithm
for alignment mark recognition is described in Fig 10,
where the feature pixels of marks can be determined
in advance of labeling and the computation will be
finished at the labeling process when the marks are
not overlapped. As far as the mark shape is geomet-
rically simple, as is the case in semiconductor and
(a) (b)
display industry, the combined algorithm of labeling
Figure 9: Matching areas for (a) circular mark and (b) cross and geometric pattern matching can be considered as
mark. a reasonable way to reduce the overall tact time.
The matching area for the circular mark is given
41
5 CONTROL
The first step in the visual alignment process is to de-
tect the centroids of alignment marks from the raw
images. The visual processing can be divided into
pre-processing and image analysis. As described in
the former section 4, the pre-processing includes bi-
narization, edge detection, and noise filtering etc. In
the image analysis procedure, a few object recogni-
tion techniques can be applied such as labeling and
template matching. Once the centroids of marks are
determined, the misalignment distance and angle be-
tween mask and panel, which is usually less than hun-
dreds of microns, can be readily determined using ge-
ometric relationships. Given the misalignment quan-
tity for the current image, the driving distances of Figure 13: Graphic User Interface of operating software.
joint actuators can be produced by the inverse kine-
matic solution for a specific parallel mechanism as
shown in Section 3. Finally, the misalignment can has the size of 640 × 480 pixels and the field of view
be compensated by the joint controller as in Fig. 11, (FOV) is 6.4 × 4.8 mm and the rate of capture in the
where the outer visual feedback loop should be re- frame grabber is 15 fps. Circular marks have a diam-
peated until it meets permissible alignment errors. eter of 2 mm and thickness of 200 µm and the cross
marks have the length of 1 mm. The graphic user in-
Fast and Fine
Motion Control
terface shown Fig. 13 was developed to integrate all
the functions to achieve the autonomous visual align-
Misalignment Inverse Joint ment.
Determination Kinematics Controller
First of all, we compared the performance of the
alignment mark recognition algorithm in Fig. 10
Efficient
Image Analysis with the popular NC (Manickam et al., 2000) and PC
method (Krattenthaler et al., 1994). For the captured
Centroids Feature images in Fig. 13, the proposed method takes 37 msec
Extraction Extraction
mark images
on average to find the centroids of all marks including
Efficient x 2EA pre-processing time, while the NC and PC required
Image Processing
661 msec and 197 msec, respectively. As a result,
Figure 11: Vision-based look and move motion control. the proposed method reduces one order of recogni-
tion time. As explained before, this is mainly because
in the geometric template matching the vector oper-
ations for all pixels are not necessary unlike the NC
Frame and the feature points of objects are selected in ad-
Illumination control Grabber
vance unlike the PC. Although the geometric pattern
matching is confined to simple objects, it is actually
enough to extract alignment marks.
Control PC
CCD 1
Figure 14 shows the visual alignment process for
a given misaligned posture between mask and panel.
CCD 2
Mask mark (d=2mm) As explained in Fig. 11, the inverse kinematic solu-
tions are cast into the joint controllers as a reference
XYZ manual
stage
input. To avoid excessive chattering, at every align-
ment cycle, we have applied the polynomial trajec-
Glass mark (d=1mm)
CCD Ch. 1 4PPR stage
tory with rise time 0.6 sec for the reference values. In
Fig. 14, the mask and panel were almost aligned after
Figure 12: Experimental setup for visual alignment. the 1st cycle and the 2nd cycle was activated since the
misalignments in U-axis and W -axis are still over the
Figure 12 shows the experimental setup for the tolerance. Considering the joint control error in Fig.
mask and panel alignment, where the CCD image 15 for the 2nd cycle, the controlled motion of V -axis
42
is not smooth. When the reference input is too small, 6 CONCLUSION

a stick-slip motion may occur at low velocities and
this makes precision control very difficult. In this paper, we investigated the visual alignment
problem which is considered as a core requirement in
(a) Initial postures flat panel display and semiconductor fabrication pro-
Ch. 1 Ch. 2 cess. The kinematics of the 4PPR parallel mechanism
was given and an efficient vision algorithm in terms of
geometric template matching was developed for fast
recognition of alignment marks. Through the control
experiment, the proposed method was proven to be
very effective.
(b) After 1st alignment
Ch. 1 Ch. 2
REFERENCES
Cognex Ltd. (2004). In http://www.cognex.com/ prod-
ucts/VisionTools/PatMax.asp.
(c) After 2nd alignment Gonzalez, R. C. and Wood, R. E. (2002). Digital Image
Ch. 1 Ch. 2
Processing. Prentice Hall.
Hephaist Ltd. (2004). In http://www.hephaist.co.jp/
e/pro/n 4stage.html.
Kang, D. J. and Lho, T. J. (2003). Development of an edge-
based point correlation algorithm for fast and stable
visual inspection system. In Journal of Control, Au-
Figure 14: Visual alignment experiment: the inverse kine- tomation and System Engineering. Vol. 9, No. 2, Au-
matic solution is (U,V,W ) = (132.4, −592.6, −1367.6)µm gust, 2003 (in Korean).
at initial posture and (U,V,W ) = (−73.4, −3.5, 66.5)µm af- Kanjilal, A. K. (1995). Automatic mask alignment without
ter the 1st alignment. a microscope. In IEEE Trans. on Instrumentation and
Measurement. Vol. 44, pp. 806-809, 1995.
(a) U-axis control error

Kim, H. T., Song, C. S., and Yang, H. J. (2004). 2-step al-
gorithm of automatic alignment in wafer dicing pro-
cess. In Microelectronics Relaiability. Vol. 44, pp.
1165-1179, 2004.
mm
Krattenthaler, W., Mayer, K. J., and Zeiller, M. (1994).

Point correlation: a reduced-cost template matching
technique. In IEEE Int. Conf. on Image Processing.
time(sec) pp. 208-212, 1994.
(b) V-axis control error
Lai, S.-H. and Fang, M. (2002). A hybrid image alignment
system for fast and precise pattern localization. In
Real-Time Imaging. Vol. 8, pp. 23-33, 2002.
mm
Manickam, S., Roth, S. D., and Bushman, T. (2000). In-

telligent and optimal normalized correlation for high-
speed pattern matching. In Datacube Technical Paper.
time(sec) Datacube Incorpolation.
(b) W-axis control error
Martin, B. R. (2001). Self-alignment of patterned wafers
using capillary forces at a wafer-air interface. In Ad-
vanced Functional Materials. Vol. 11, pp.381-386,
mm
2001.
Strang, G. (1988). Linear Algebra and Its Applications.
College Publishers, 3rd edition.
time(sec)
Tsai, L.-W. (1999). Robot Analysis: The mechanics of se-
Figure 15: Joint control errors during the 2nd alignment rial and parallel manipulators. Wiley-Interscience.
(the 4th X-axis was not active). Umminger, C. B. and Sodini, C. G. (1995). An integrated
analog sensor for automatic alignment. In IEEE Trans.
on Solid-State Circuits. Vol. 30, pp. 1382-1390, 1995.
43
DISTURBANCE FEED FORWARD CONTROL OF A HANDHELD
PARALLEL ROBOT
Achim Wagner, Matthias Nübel, Essam Badreddin

Automation Laboratory
University of Mannheim, D-68131 Mannheim, Germany
[email protected]
Peter P. Pott, Markus L. Schwarz

Laboratory for Biomechanics and experimental Orthopaedics, OUZ
Faculty of Medicine Mannheim, University of Heidelberg, Germany
Keywords: Robotic manipulators, Medical systems, Dynamic modelling, Disturbance rejection, Feedforward compensa-
tion.
Abstract: A model-based control approach for a surgical parallel robot is presented, which combines a local tool stabi-
lization with a global disturbance feed forward control. The robot is held in the operator’s hand during the
manipulation of bones. For a precise processing the tool has to be decoupled from disturbances due to unin-
tentional hand movements of the surgeon at the robot base. The base disturbances are transformed for a feed
forward control using the inverse dynamics of the robot. Simulations show that disturbances can be reduced
by many orders depending on sensor errors and delay.
1 INTRODUCTION is transforming the desired tool motion into the de-

sired leg motion with the inverse kinematics and con-
Parallel robots are widely used, where high stiff- trolling the leg motion separately on the axes level
ness, high dynamics or low error propagation over (Tönshoff et al., 2002). However, if high dynamics
the kinematic chains is required, e.g. flight sim- is required this simple kinematics approach does not
ulators (Koekebakker et al., 1998), processing ma- lead to a high precision tool pose control, especially
chines (Tönshoff et al., 2002), positioning and stabi- if the robot’s base is freely movable. Therefore, full
lization platforms (Huynh, 2001), vibration isolation dynamic models of special parallel robots are intro-
(Chen and McInroy, 2004) and medical manipulators duced to improve the quality of a fast platform control
(Pott et al., 2004; Wagner et al., 2004). The prob- (Riebe and Ulbrich, 2003; Honegger, 1999). The non-
ably most famous Hexapod parallel kinematic struc- linear inverse dynamic large-signal model for a paral-
ture is the Stewart-platform (Stewart, 1966) which has lel robot with two movable platforms is introduced in
six degrees-of-freedom (DOF). An obvious advantage (Wagner et al., 2006). This model is used here to con-
considering hand-held applications is, that a parallel trol the pose of the tool, while it is decoupled from
robot has high potential for a lightweight construc- base disturbances. Since the robot is movable freely
tion. By fixing the most massive part of the actu- in space adequate co-ordinate systems must be de-
ators to base (Merlet, 2000), the actively positioned fined. The local control of the tool is performed in
mass can be further decreased. This leads to a re- the base instantaneously coincident reference frame
duction of static and dynamic forces (Honegger et al., using the inverse kinematics description of the robot.
1997; Huynh, 2001). The forces of the actuators can The same reference frame can be used to achieve a dy-
be transferred to the tool platform using light struts. namic feed forward compensation. However, for the
Within the hand-held surgical robot project ”Intelli- compensation of the gravity influences and for a ab-
gent Tool Drive” (ITD), a parallel robot is designed solute position referencing a world coordinate system
to align a milling tool relatively to a moving bone must be defined. In the following sections the control
of the patient (workpiece) and to decouple the tool structure and simulations are presented to show the
from unintentional hand movements at the base. The advantages and drawbacks of the approach.
standard procedure to control a parallel manipulator
44
2 SYSTEM DESCRIPTION
z
base platform {B},{B’}
The goal of the ITD robot project is to adjust and
to stabilize a drilling or milling tool (mounted at the y x
tool platform) with respect to a moving bone of the actuator i
patient (workpiece), while the complete machine is
held in the surgeon’s hand. Therefore it is necessary qi z {W}
to isolate the tool from disturbances at the base pro-
duced by the operator’s unintional hand movements. Br
y x
slider i BR
T
gz
The amplitude and frequency of the human arm dis- Br
s,i T
turbances is available from literature (Takanokura and
Sakamoto, 2001). Corresponding to the milling pro-
Br
cess with different tools a 6 DOF control of the tool strut i st,i
is required. Assuming an additional safety margin the
selected workspace of the robot ranges from -20 mm Tr z {T}
t,i
to +20 mm in the three Cartesian axes and respec- tool
tively from -20o to +20o in the three rotational axes. x
platform
y
The mechanical device designed is a parallel robot
with six base-fixed actuators. The CAD model of
the surgical robot ITDII (Intelligent Tool Drive II) is Figure 2: Topology of the parallel robot with fixed actua-
shown in Fig. 1. The robot base has six fixed linear tors.
motors, i.e. the electrical part of the motors. Further-
more, there are guide rails, housing, and handles for
the hand-held operation, building together the robot in space, adequate coordinate systems have to be de-
base. The sliders of the linear motors are connected fined. For the calculation of the kinematic equations
to the tool platform via six lightweight struts. At the four coordinate frames are considered (Fig. 2): (1)
tool platform and at the sliders the struts are mounted the world (earth) frame {W}, (2) the tool frame {T}
with spherical joints. in the tool’s centre of gravity, (3) the movable base
Because the base of the robot can be moved freely frame {B} in the base’s center of gravity, and (4) the
instantaneously coincident base frame {B’}, which is
fixed in {W}. The frame {B’} and the frame {B} are
aligned, if the base stands still. Without loss of gen-
erality the motion of both platforms is represented in
the {B’} frame, which reduces the complexity of the
kinematic description. The choice of the coordinates
has no influence on the value of the inertial forces.
For simplicity the gravity is calculated in the world
frame. Afterwards, the resulting force can be trans-
formed into the {B’} frame with little effort.
The pose of the tool is defined by the position
vector B rT and the orientation matrix BT R in the {B}
frame, which build together the pose vector B XT =
(B rT , BT R). From the matrix BT R the fixed zyx-Euler
angles φ, θ, and ψ can be derived. The positions of
the tool joints T rt,i in the tool frame and the initial po-
sitions of the slider joints B rs0 ,i in the base frame for
each actuator i are known from the geometry of the
construction. The sliders actual positions B rs,i move
in z-direction of the base according to the guidance,
if the tool changes its position. Therefore, the initial
positions Brs,i,x = B rs0 ,i,x and Brs,i,y = Brs0 ,i,y in the
x-direction respectively the y-direction are not alter-
nated. The struts have the lengths li .
Figure 1: Hand-held surgical robot ITDII - CAD model.
45
3 MODELLING result from the constraint movement on a sphere with

radius li . The relative velocities of the sliders, which
For the model-based control of the robot, the non- can be measured and controlled on the actuator level,
linear inverse dynamic description is required, espe- is ′ ′ ′
cially if both platforms can be moved freely in the q̇i = B vst,i,z + B vt,i,z − B v′s,i,z (7)
entire workspace. ′
with the tool joint velocities B vt,i,z and the velocities
′
of the initial joint positions B v′s,i,z .
3.1 Inverse Kinematics
3.1.3 Acceleration
The inverse kinematics of the robot describes the non-
linear relationship between the relative tool-base pose Corresponding to the velocity derivation, the gener-
and the slider positions, which states the operating alized accelerations of the tool and the base are de-
′ ′ ′ ′ ′ ′
point of the robot in the instantaneously coincident fined by B ẌT = (B aT , B αT ) and B ẌB = (B aB , B αB )
base frame {B’}. Furthermore, the velocities and ac- ′ ′
with the translational accelerations B aT and B aB and
celerations of the sliders are given from the relative ′ ′
the angular accelerations B αT and B αB . The acceler-
motion of the tool and the base in {B’}. ations of the joints are assemblies of three terms: (1)
3.1.1 Pose inertial acceleration a′ , 2) centripetal acceleration a′′ ,
and (3) Coriolis acceleration a′′′ . The inertial terms of
Since the slider positions are constrained by the six the slider joints are
struts i = 1..6, the joint x and y distances between B′ ′ ′ ′
as,i = B aB + B αB × B rs,i , (8)
slider and strut joints are given by
B
and the centripetal accelerations are
rst,i,x = B rs,i,x − B rt,i,x (1)
B′ ′′ ′ ′
as,i = B ωB × B ωB × B rs,i . (9)
and
B B B The Coriolis acceleration terms are non-zero in x-
rst,i,y = rs,i,y − rt,i,y . (2)
direction
With the assumption of constant strut lengths li the B′ ′′′ ′
as,i,x = 2 ·B ωB,y · q̇i (10)
z-component of the joint distance vectors can be cal-
culated as and in y-direction
q B′ ′′′ ′
B
rst,i,z = li2 − (B rst,i,x )2 − (B rst,i,y )2 (3) as,i,y = − 2 ·B ωB,x · q̇i . (11)
The z-component a′′′ B′

and the slider joint positions yield s,i,z is identical zero.
Summarizing the three terms, the slider joint ac-
B
rs,i,z = B rt,i,z + B rst,i,z . (4) celerations in x and y direction result in
The required shift of the actuator positions with re- B′ ′ ′ ′
as,i,x = B a′s,i,x + B a′′s,i,x + B a′′′
s,i,x (12)
spect of the starting positions B rs0 ,i,z is
B′ ′ ′ ′
as,i,y = B a′s,i,y + B a′′s,i,y + B a′′′
s,i,y . (13)
qi = B rs,i,z − B rs0 ,i,z , (5)
The slider acceleration in z-direction is not a simple
which is measured by local positioning sensors. sum, because the slider motion is constrained by the
struts according to the sphere equation
3.1.2 Velocity
Br
B′ st,i,x B′ ′
ast,i,z = − B ast,i,x +B anst,i,x
The generalized tool and base velocities rst,i,z
B′Ẋ = (B′v , B′ω ) and B′Ẋ = (B′v , B′ω ) em- Br
T T T B B B st,i,y B′ ′
′
brace the translational velocities B vT respectively −B ast,i,y +B anst,i,y
rst,i,z
B′v and the angular velocities B′ω and B′ω of both ′
B T B
rigid bodies. According to a rigid body motion the − B anst,i,z (14)
tool joint positions and the base-fixed initial slider
joint positions can be determined from the general- with the normal acceleration
′ 2
B′ ′
ized velocities. Due to the constant strut lengths, the anst,i = − B rst,i ×B vst,i ×B vst,i / B
rst,i (15)
z-components of the relative slider velocities
Br Br Finally, the slider joint accelerations are
B′ st,i,x B′ st,i,y B′
vst,i,z = − B vst,i,x − B vst,i,y (6) B′ ′
as,i,z = B ast,i,z + B at,i,z .
′
(16)
rst,i,z rst,i,z
46
and the acceleration of a slider’s center of gravity vector sums of the tool and the base related forces,
yields since the equation of motion is non-linear. However,
′
B′ ′ ′ ′ if the tool is fixed, the B ΛT term vanishes and the re-
acg,i,z = B as,i,z +B ωB × B ωB × B rscg,i (17) maining forces for the slider mass acceleration yields
with the position offset Br between the slider joint
scg,i
a quite simple form (19).
position and the slider’s centre of gravity.
3.2 Inverse Dynamics 4 CONTROL STRUCTURE

The inverse dynamics of the robot describes the actu- The robot control must be suited to adjust the tool
ator forces needed to move the tool’s centre of gravity against a movable workpiece and to decouple the tool
with a desired velocity and acceleration. Additionally, from disturbances at the base. Therefore, a control
the actuator forces are derived to stabilize the tool, if structure was designed, which combines a servo con-
the base is disturbed. In this section a brief sketch of trol on the local axis level with a disturbance feed-
the ITDII robot’s inverse dynamics is given. A more forward control in the global Cartesian space (Fig. 3).
detailed description can be found in (Wagner et al., For the controller design the adequate choice of
2006). co-ordinate systems is essential, since the complete
The rigid body dynamics of a parallel robot can be robot can be moved freely in space and the component
described generally by the equation of motion inertia forces are related to a earth fixed (respectively
B′ inertia) reference system. The local stabilization is
ΛT = M(q) · q̈ + C(q, q̇) · q̇ + G(q). (18) based on the robot kinematics, which describes the
B′
with the generalized force ΛT , which is necessary to relative pose between tool and base in relation to the
move the tool platform in a fixed frame {B’}. Within actuator positions. Here, the actuator position are not
this equation M is the generalized mass matrix, C is dependent on the tool and base reference. In contrast,
the centripedal and Coriolis component and G is the the feed-forward control uses the inverse dynamics
gravity component of the platform. These parameters block, which is definded in {B’}. The sensor sig-
depend on the operationg point of the robot. The gen- nals for the base pose, velocity and acceleration are
eralized force is converted into actuator forces using retrieved in the world frame. Furthermore, the de-
the Jacobian matrix of the robot. Additionally, the in- sired tool motion is given in the world frame as well.
ertia forces Therefore, the tool and base motion signal are con-
h ′ i verted to {B’} co-ordinates before they are applied to
Fs,i = ms,i B acg,i,z − W
B
Rg z . (19) the inverse kinematics. This geometric transforma-
tion can be done with little effort.
are taken into acount with the slider masses ms,i . The The local control is based on the slider position
gravitiy vector g = (0 0 − 9, 81 m/s2 )T can be trans- and velocity sensor signals q and q̇. The actuator sig-
formed from the {W} frame into the {B} frame using nals are compared to the desired actuator signals qd
the rotation matrix W B R. Since the major part of the and q˙d . Using the generalized tool mass matrix M a
strut masses is concentrated in the joints and since the PD-controller generates the force signals Ffb needed
link beetween the joints is lightweight, the struts can to stabilize the tool. The reference signals qd and q˙d
be separated into two parts, which can be added to the result from the desired tool signals WXT d , and WẊT d
slider and tool components (Honegger, 1999). Corre- and from the actual base coordinates WXB , and WẊB
spondingly the strut masses and inertias are not con- both in Cartesian world coordinates, using the inverse
sidered in the inverse dynamics explicitly. In a first kinematics of the parallel robot.
attempt the friction forces are neglected as well. The The feed-forward control block consists of two
latter can easily be extended on the actuator level, if inputs, the tool reference signals WXT d , WẊT d , and
necessary. The non-working reaction forces/torques WẌ W W
T d and the actual base signals XB , ẊB , and
of the actuators are perpendicular to the actuator mo- WẌ . Using the inverse dynamic model the feed-
B
tions and, therefore, not considered in the robot dy- forward forces Fff are calculated which are required
namics. to move the tool as desired, while the disturbances
For the calculation of all force components the rel- from the base are canceled. If the inverse dynamic
ative velocity and acceleration between the tool and model and the sensor/actuator signals are assumed to
the base as well as the absolute orientation of the robot be perfect, the local control error will be zero. How-
in the world coordinates must be available. It should ever, the inverse model is not really complete due to
be mentioned, that the actuator forces are not simple the neglected strut masses, actuator friction and un-
47
Figure 3: Control structure; local servo loop and global disturbance feed forward control.
measured disturbances. These model uncertainties jectory tracking quality is described at the end using
are compensated by the local pose control. The pre- a standard circle test (ballbar-test).
sented structure is not a classical cascaded structure,
because the base coordinates are not influenced im-
5.1 Tool Stabilization
mediately by the control action. The base motion re-
sults from the disturbance forces of the human opera-
tor. That means that no feed-back from the actuators The first simulations describe the system disturbance
to the base motion is assumed due to the large base response in the frequency domain. In the first sim-
mass and that the sensing of the base motion serves ulations the control parameters are configured for a
for the referencing of coordinates only. critically damped PD control loop with a system fre-
In the realized system it is necessary to measure quency Ω = 60Hz. Figure 4(a) shows the tool motion
the pose, the velocity and the acceleration of the base. after application of a 12 Hz, 1 mm sinusoidal signal
The tool coordinates are calculated from the desired at the base. The straight line represents the stimu-
tool trajectory. To take care of force feed-back dur- lus in the x-direction, which can be referenced to the
ing the processing of the workpiece, additional sen- left hand scale. The amplitude of the tool position re-
sors measuring the tool motion or forces could be im- sponse using a PD control (dotted line) and using a
plemented in a feed-back control. This may make PD control with additional feed forward disturbance
sense in special cases. However, backlash effects in
0.50
the joints could lead to a destabilization of the con- 1.0
a)
Base position
tool x-axis (mm)

base x-axis (mm)
trolled system. Therefore, and for the sake of sim- PD

0.25
0.5
plicity we are content with the local stabilization. PD-FF x2
As shown in literature (Riebe and Ulbrich, 2003), 0.0 0.00
a friction compensation is essential in the real appli-

-0.5 -0.25
cation, which can be added on the local axis level. To
separate the kinetic effects from the friction effects, -1.0
b)
such a compensation is neglected in this paper. The sum
inertial
control structure has been implemented in the simula- normal
0.00
tion environment Matlab/Simulink using a fixed sam-
F (N)
pling time of 1 ms. As a consequence the minimum

delay in the control loop is 1 ms as well. The forward -0.02
dynamics of the robot ITD used in the closed loop

simulations has been modelled with the SimMechan-
0.25 0.30 0.35 0.40
ics toolbox.
time (s)
Figure 4: Response on a 12 Hz, 1 mm sinusoidal trans-

lational base disturbance in x-axis; (a) base position stim-
5 SIMULATION ulus (straight line, left-hand scale), tool position response
(PD control, dotted line, right-hand scale), tool position re-
The simulations presented in this section mainly sup- sponse multiplied by a factor of two (PD control with distur-
bance feed-forward, dotted line, right-hand scale); (b) Force
port the description of the disturbance decoupling needed for feed-forward compensation.
ability of the system. Additionally, the overall tra-
48
compensation (dashed line) can be extracted from the 20 a)

Base position
right hand scale. For a better readability the later is 0.01
tool x-axis (°)

base x-axis (°)
PD
10
multiplied by a factor of two. PD-FF x10
0.00
As a result the PD controlled tool is damped 0
-0.01
against the base disturbance with a transfer factor of
-10
0.3 (-10dB) due to its limited bandwidth. Adding the -0.02
feed-forward control the transfer factor decreases to -20

b)
0.06 (-24dB). sum
0.10 inertial 0.10
The actuator force needed to cancel the distur- normal
0.05 0.05
bances is plotted in Fig. 4(b) for the first actuator. The centripedal
F (N)
coriolis
resulting force F (straight line) is the total of all force 0.00 0.00
components. The inertial component (dashed line) re- -0.05 -0.05
sults from the tool and the slider acceleration (8) and -0.10 -0.10
the normal component (dotted line) is a consequence 0.5 1.0 1.5 2.0
of the strut rotation velocity (6). No Coriolis or cen- time (s)
tripedal forces are generated for a translational tool

Figure 6: Response on a 1 Hz, 20o sinusoidal rotational base
movement. While the tool-pose reaction seams to be
disturbance around the x-axis; (a) base orientation stimu-
more or less a sinusoidal function, higher harmonic lus (straight line, left-hand scale), tool orientation response
distortions are noticeable in the inertial component. (PD control, dotted line, right-hand scale), tool orientation
Now, the sinusoidal stimulus is shifted to 1 Hz and response multiplied by a factor 10 (PD control with distur-
20 mm amplitude (Fig. 5) compared to Fig. 4. Here, bance feed-forward, dotted line, right-hand scale); (b) Force
the feed-forward related position response is multi- needed for feed-forward compensation.
plied by a factor of 50. As a result of the lower fre-
quency the PD-loop has an increased damping effect
with a transfer factor of 6 · 10−3 (-45dB) respectively around the x-axis and its response is shown in Fig.
6.5 · 10−5 (-84dB) with feed forward control (Fig. 5). 6(a). The forces needed to stabilize the tool are plot-
The pose response signal in the large signal case is ted in Fig. 6(b). Here, the Coriolis (large dotted line)
not a sinusoidal signal anymore due to the non-linear and centripedal (large dashed line) forces are in the
coupling between tool and base. range of the inertia forces. Unlike an intuitive estima-
A sinusoidal stimulation of the base orientation tion the Coriolis and centripedal components cannot
be neglected in this special robot application.
20 3.5
a)
Base position
base x-axis (mm)
x PD
tool x-axis (mm)
PD 0.1
10 3.0 x PD-FF
PD-FF x50
0 0.0 2.5
max x-error (mm)
-10 2.0
-0.1
-20 1.5
b)
sum
0.03 0.03 1.0
inertial
normal
0.5
F (N)
0.00 0.00
0.0
-0.03 -0.03 10 20 30 40 50 60
(1/s)
0.5 1.0 1.5 2.0
time (s) Figure 7: x- axis response on a 1 Hz, 20 mm sinusoidal

translational base disturbance in dependence on the servo
Figure 5: Response on a 1 Hz, 20 mm sinusoidal trans- loop frequency.
lational base disturbance in x-axis; (a) base position stim-
ulus (straight line, left-hand scale), tool position response
(PD control, dotted line, right-hand scale), tool position re- The decoupling behaviour from the base motion
sponse multiplied by a factor of 50 (PD control with distur- strongly depends on the choice of the parameter Ω
bance feed-forward, dotted line, right-hand scale); (b) Force
needed for feed-forward compensation. and the delay in the feed-back loop, which is at least
one sampling period. This is shown in Fig.7 and Fig.8
49
1.50
X PD
tool position y-axis (mm)

1.25 X PD-FF
10
max x-error (mm)
1.00
0.75
0
0.50
0.25
desired
-10
0.00 PD error x10
PD-FF error x10

1 2 3 4 5 6 7 8 9 10
delay (ms) -10 0 10
tool position x-axis (mm)

Figure 8: x- axis response on a 1 Hz, 20 mm sinusoidal
translational base disturbance in dependence on the sensor
delay. Figure 9: Standard circle test with white Gaussian sensor
noise; σaccel = 0.17mm/s2 translational standard deviation,
σrot = 0.17rad/s rotational standard deviation, v = 0.1m/s
feed speed.
for a base disturbance of 1 Hz and 1 mm.
The pose error amplitude and phase decrease with
increasing control loop frequency. With disturbance
feed forward control the error is generally smaller,
6 DISCUSSION
however the quality strongly depends on the sensor
delay. To estimate the limit time lack that can be al- The simulations show, that if a complete model of the
lowed the pose error is plotted against the sensing de- parallel robot and adequate sensor signals are avail-
lay. For instance, the delay should not exceed 3 ms, able, a feed forward control can lead to a decoupling
if a pose accuracy of 0.3 mm is required. (Certainly, from base disturbances with high damping factors.
additional error sources must be considered). For the quality of the decoupling the sensor noise and
the sensor delay are essential. The simulated damp-
ing value of 6.5 · 10−5 in the 1 Hz case (Fig. 5) must
5.2 Tracking Control be interpreted with care, because the quality of the
decoupling is influenced by numerical errors in this
To assess the possibilities of the disturbance feed for- range. Furthermore, model uncertainties and sensor
ward control the standard circle (ballbar) test was sim- errors do not allow such a high precision in real-world
ulated assuming an additional white Gaussian sen- applications. Within the control approach the base
sor noise with a translational standard deviation of pose, velocity and acceleration must be measured or
σaccel = 0.17mm/s2 and a rotational standard devia- observed accurately in the inertia system. This is a
tion of σrot = 0.17rad/s for all axes. Figure 9 shows serious problem, because not only sensor latency and
the reference trajectory, which has to be followed with noise must be considered but also misalignment, bias,
a velocity of 0.1 m/s, and the actual trajectories using drift, etc..
a PD control and using a PD control with disturbance For the simulations an ideal model is assumed
feed forward. The deviation from the reference tra- without geometric or parametric errors and without
jectory is exaggerated by a factor of five. The mean considering the friction in the actuators and joints.
pose error are 0.35 mm and 0.10 mm with respectively An enhancement of the dynamic model can be done
without feed forward control. The standard deviations with acceptable effort, e.g. the modelling of friction
are 0.051 mm and 0.052 mm. Also for the reference on the actuator level. However, the quantity of fric-
action the feed forward control diminishes the pose tion and its influence on the dynamics depends on
error remarkably. Because the signal noise from the the special mechanical implementation and many pa-
base sensors is injected into the inverse kinematics rameters. Therefore a possible model extension must
block as well as into the inverse dynamics block, it in- be considered if the robot is realized. In contrast to
fluences the PD control loop once and the PD control the intuitive impression that for the size, mass and
with feed forward control twice. However, the persis- dynamics of a hand-held robot the Coriolis and cen-
tent pose noise is not much larger with a feed forward tripedal forces do not play any role, the simulations
control compared to the pure PD control approach. show that these force components make a remarkable
50
contribution to the total force in the actuators. Thus, Pott, P., Wagner, A., Köpfle, A., Badreddin, E., Männer,
a simplification of the model by neglecting the veloc- R., Weiser, P., Scharf, H.-P., and Schwarz, M. (2004).
ity related force components is not suitable. Finally, A handheld surgical manipulator: Itd - design and first
results. CARS 2004 Computer Assisted Radiology and
the simulations show in which range of precision a Surgery, Chicago, USA.
feed forward control does improve the decoupling be-
Riebe, S. and Ulbrich, H. (2003). Modelling and online
haviour and in which range the feed forward control
computation of the dynamics of a parallel kinematic
can be neglected. with six degrees-of-freedom. Archive of Applied Me-
chanics, 72:817–829.
Stewart, D. (1965-1966). A platform with six degrees of
7 CONCLUSION freedom. Proceedings of the Institute of Mechanical
Engineering, 180:371–386.
A model-based approach is presented to control the Takanokura, M. and Sakamoto, K. (2001). Physiological
tool pose of a handheld robot and to decouple the tool tremor of the upper limb segments. Eur. J. Appl. Phys-
from base disturbances. An adequate definition of co- iol., 85:214–225.
ordinate frames for the dynamics modelling and the Tönshoff, H., Grendel, H., and Grotjahn, M. (2002). Mod-
controller design reduces the effort for the implemen- elling and control of a linear direct driven hexapod.
Proceedings of the 3rd Chemnitz Parallel Kinemat-
tation. The feasibility of a feed-back control on the lo- ics Seminar PKS 2002, 2002 Parallel Kinematic Ma-
cal axis level in combination with a disturbance feed chines Int.Conf.
forward control on the robot level in world coordi- Wagner, A., Pott, P., Schwarz, M., Scharf, H.-P., Weiser,
nates is shown. The local control is able to stabilize P., Köpfle, A., Männer, R., and Badreddin, E.
the robot and to avoid huge errors due to model un- (2004). Control of a handheld robot for orthopedic
certainties and disturbances. The feed forward control surgery. 3rd IFAC Symposium on Mechatronic Sys-
ensures the free movement of the robot in space, while tems, September 6-8, Sydney, Australia, page 499.
measured disturbances can be compensated for. Fur- Wagner, A., Pott, P., Schwarz, M., Scharf, H.-P., Weiser, P.,
thermore, the usage of a non-linear inverse dynamics Köpfle, A., Männer, R., and Badreddin, E. (2006). Ef-
model enables the precise disturbance feed-forward ficient inverse dynamics of a parallel robot with two
movable platforms. 4rd IFAC Symposium on Mecha-
control under different operational conditions. For the
tronic Systems, Heidelberg, Germany.
feed-forward control sensor error and delay are cru-
cial.
REFERENCES
Chen, Y. and McInroy, J. E. (2004). Decoupled control of
flexure-jointed hexapods using estimated joint-space
mass-inertia matrix. IEEE Transactions on Control
Systems Technology, 12.
Honegger, M. (1999). Konzept einer Steuerung mit adap-
tiver nichtlinearer Regelung für einen Parallelma-
nipulator. Dissertation, ETH Zurich, Switzerland,
http://www.e-collection.ethbib.ethz.ch.
Honegger, M., Codourey, A., and Burdet, E. (1997). Adap-
tive control of the hexaglide, a 6 dof parallel manipu-
lator. IEEE International Conference on Robotics and
Automation, Albuquerque, USA.
Huynh, P. (2001). Kinematic performance comparison of
linar type parallel mechanisms, application to the de-
sign and control of a hexaslide. 5th International con-
ference on mechatronics technology (ICMT2001), Sin-
gapore.
Koekebakker, S., Teerhuis, P., and v.d. Weiden, A. (1998).
Multiple level control of a hydraulically driven flight
simulator motion system. CESA Conference, Ham-
mammet.
Merlet, J. (2000). Parallel robots. Kluwer Academic Pub-
lisher, Dordrecht, Netherlands.
51
THE TELE-ECHOGRAPHY MEDICAL ROBOT OTELO2
Teleoperated with a Multi Level Architecture using Trinomial Protocol
Gwenaël Charron(1), Aïcha Fonte(1), Pierre Vieyres(1)

Philippe Fraisse(2), Lama Al Bassit(1) and Cyril Novales(1)
(1)
Laboratoire Vision & Robotique, University of Orleans, Bourges, France
{firstname.name}@bourges.univ-orleans.fr
(2)
Laboratoire d’Informatique, de Robotique et de Microélectronique de Montpellier, University of Montpellier II
Montpellier, France
[email protected]
Keywords: Teleoperated system, Telerobotics, OTELO medical robot, Tele-echography, Multi level archecture.
Abstract: This paper presents a novel architecture applied to a mobile teleoperated medical robotic system: OTELO2
(MObile Tele-Echography using an Ultra-Light RObot); OTELO2 performs a tele-echography at a distance
for the benefit of medically isolated sites. First, this paper presents an overview of the OTELO2
teleoperated system. Then, it describes the modular control architecture used and the integration of the
teleoperated layer on this multi level architecture. Finally, it presents the communication links used to
control this system, as well as some experimental results.
1 INTRODUCTION goal is to homogenize the whole mobile robotics

developments and to be able to re-use already
Telerobotics and teleoperation have currently a very designed modules. The AuRA architecture (Arkin,
important role to play in the medical field especially 1998) is made up of two parts (reactive and
deliberate), each using distinct method to solve
in non invasive medical application (i.e. tele-
echography) needed by the patients living in isolated problems. The reactive part is based on sensors and
sites with reduced medical facilities. The aim of the the deliberate part uses artificial intelligence method
contains a mission planner, a spatial reasoner and
OTELO2 system is to provide people with the best
medical examination conditions and thus to have the plan sequencer. The OTELO2 architecture relies on
best diagnostic as possible. the concept of levels initially developed by Brooks
Based on the concept of a mechanical probe and which appear in architectures proposed by
holder, OTELO2 is a teleoperated robotic AuRA or LAAS. The originality of this architecture
manipulator arm. Teleoperated systems are exposed is to decompose the control in multi-levels which
to possible instability due to the transmission delay allows to decompose in clear way the different
of the communication link and to the need of remote functions realized and also to decompose each level
safety and maintenance of the robot. The goal of the in several blocks which allow to retail and to
proposed combined approach, i.e. to use a specific separate the connections with the sensors and the
protocol to reduce data loss and to implement a actuators.
modular architecture to enhance the tele-echography A description of the teleoperated robotic system,
including the “expert” and the “patient” station, is
system, is to favor the overall remote medical act for
the benefit of the patients. given in the first section. The second section
To control teleoperated system, lots of presents the architecture developed to control the
robot. The next section presents the communication
architectures have been proposed. The
“Subsumption Architecture” (Brooks, 1986) is links between the two stations composing the system
composed of parallel different levels which process and the protocol used to control the robot. The last
section presents some experimental result obtained
information supplied by the sensors in order to
determine the control to be sent to actuators. The during a teleoperation between Bourges (France)
LAAS architecture (Alami, 1998) is made up of and Montpellier (France).
three levels: decisional, executive and functional. Its
52
THE TELE-ECHOGRAPHY MEDICAL ROBOT OTELO2 - Teleoperated with a Multi Level Architecture using
Trinomial Protocol
2 THE OTELO2 SYSTEM

OTELO2 is a teleoperated robotic prototype system
composed of an “expert” station and a “patient”
(time)
station. A communication network (e.g. terrestrial or
satellite) allows data transmission (i.e. ultrasound
images, robot controls, haptic feedback and ambient
images) between the two stations (Figure 1).
Figure 2: Histogram of inclination angle θ of the probe

axis during an abdominal ultrasound examination.
The OTELO2 robot prototype was developed by

the Laboratory of Vision and Robotic (LVR) in
Figure 1: The OTELO2 teleoperation chain. collaboration with European project partners in
order to answer the previously mentioned criteria.
2.1 The “Patient” Station OTELO2 is a serial six DOF (Degree Of Freedom)
probe holder system; it includes a positioning
The “patient” station is located near the patient at a module (with two prismatic articulations, P1 and P2,
medically isolated site or in a secondary hospital. It with perpendicular axis), a spherical module with
includes a portable echograph device allowing distant rotation center (with three revolute pairs R1,
ultrasound frames acquisition and the hardware R2 and R3, the R1-R2 and R2-R3 angles are equal to
system for the control of the probe holder robot. α) and a translation along the probe axis (P3, the
The results of a previous study (Al Bassit, 2003), R3-P3 angle is equal to β) allowing, for a given
on the medical gesture performed by a specialist orientation, to modify the probe/skin contact force
during an abdominal echography, gave the (Figure 3).
mechanical constraints and characteristics of the
robot work space with respect to the tele-echography
medical application. The probe must have a
spherical displacement around a contact point of the Positioning
probe with the patient’s skin. Displacement Module
amplitudes (Figure 2) are characterized by: a
maximal probe inclination of 60° with respect to the
normal of the skin plan. (larger inclination is
considered useless by the medical expert); a minimal
inclination of 35° is necessary, as well as a full
rotation of 360° around the probe symmetric axis are Orientation
needed to fulfill users’ requirements. Finally, a Module
translation along the probe axis is necessary to
obtain quality ultrasound images and maintain a
continuous contact between the probe and the
patient’s skin. For safety functioning, patient
comfort and force control, this displacement End Effector
amplitude is limited to the interval [-30mm, 10mm]. Support system
Hence, the maximal admissible force of the probe on Figure 3: Kinematics diagram of OTELO2 robot
the skin does not exceed 20 Newton. prototype.
The α and β angles, respectively 27,5° and 10°,

allow a probe maximal inclination of 65° which
complies with the medical requirements. The
positioning module allows a probe displacement
with maximal amplitude of ±25mm for each axis
and offers two DOF to search for an organ. The
53
translation amplitude along the probe axis is about 3 THE OTELO2 SYSTEM
40mm. This axis is coupled with a force sensor CONTROL ARCHITECTURE
giving the force applied by the ultrasonic probe on
the patient’s skin and enabling its control. The force
is transmitted back to the “expert” station in order to For the teleoperated robot and in order to integrate
ensure the most realistic examination conditions for teleoperation layer, it was decided to set up a layered
the medical expert. Finally, depending on the architecture (Novales, 2006). It is a multi level
examination type (e.g. ObGyn, Abdominal), various architecture where each level corresponds to a
types of manufactured probe can be attached to the decision/perception loop.
end effector of the support system. In this section, we present the control
The end effector of the remote robot moves the architecture of the OTELO2 robot, and the global
ultrasound probe in order to reproduce the expert architecture of the OTELO2 system is described
gestures which are being analyzed by a dedicated with the two MMI (Man Machine Interface)
input device (Figure 4). Images coming from the developed to control the system.
ultrasound system are compressed and sent to the
“expert” station, using the H263 protocol, and 3.1 The “Patient” Architecture
analyzed by the specialist.
The control architecture of the OTELO2 robot
2.2 The “Expert” Station prototype is a three level architecture partitioned in
two parts, namely the “Perception” and the
“Decision” parts. Each one of these levels
The “expert” station is located in a main hospital
correspond to either a software layer or a hardware
center and is operated by a medical expert. Based on layer (Figure 5).
the received ultrasound images, the expert uses a
pseudo haptic input device (Figure 4) to control the
positions and orientations of the remote ultrasound
probe. A videoconferencing system between the two
stations allows the medical expert to communicate
with the patient, to give instruction to the assistant
holding the robot, and to check the good positioning
of the robot on the patient’s body.
Figure 4: The pseudo haptic input device used to control Figure 5: The layered control architecture of OTELO2
the orientations and positions of the remote robot. robot.
To control of the teleoperated echography robot Level 0 represents the Articulated Mechanical
was supervised under a novel multi layered and System (AMS); it contains the input/output direct
modular architecture. This hardware and software interface between the physic world and the robot.
structure was added with specific communication This level receives physical data necessary to its
protocols used to control the robot on Internet actuators and sends information to the sensors at
network. The following section presents the level 1.
proposed control architecture and its layout. It is Level 1 of the decision part corresponds to the
followed by the description of the protocol used for servoings level; it determines the physics data, to be
data transmission for the robot control. addressed to level 0, from the setting points imposed
directly by the upper level.
54
Trinomial Protocol
Figure 6: The global architecture of OTELO2 teleoperated system.
The level 1 perception part receives the In the OTELO2 system global architecture
information supplied by the sensors, and it translates (Figure 6), the teleoperation level is located at level
this information to the upper level and to the 3; it corresponds to the navigation level. This part
servoings module. This level ensures the articular generates the trajectories which are executed by the
servoings with six modules in each part, robot and are sent to the pilot of the level 2 decision
corresponding to the six axes and associated sensors part. Moreover, the echograph device delivers
of the robot. information of a high level (ultrasound images) from
Finally, Level 2 decision part corresponds to the its own sensors. Thus, this teleoperation level
pilot level; it generates the articular setting points to receives information from the level 3 perception part
the lower level from a trajectory (position and including the robot positions and orientations, and
orientation of the probe) supplied by the user. The the ultrasound images coming from the ultrasound
pilot block uses the IGM (Inverse Geometric Model) probe.
to generate the setting points taking into account the This global architecture offers the possibility of
physical constraints of the system. The level 2 lower control level required for remote maintenance
perception part presents a proximity model using a and testing of the teleoperated robot (Figure 7).
DGM (Direct Geometric Model) to transmit the
robot current positions and orientations to the user.
We can note, for our application, that there is not a
direct feedback loop on this second level. The
control loop is accounted for through the distant
human teleoperator.
Perception and Decision parts constitute the so-
called autonomous part of the robot. A third part,
called the teleoperation, is added to the two previous
one in the framework of a teleoperated system.
3.2 The Global Architecture

The global architecture includes the Perception,
Decision and Teleoperation parts. Each level of the Figure 7: The level 2 teleoperation architecture.
teleoperation part receives the data stemming off the
level corresponding of the perception part and can a This teleoperation level can control the robot
by-pass the corresponding level of the decision part directly, sending articulars setting points. Hence, the
in order to generate the controls for the lower level. user is able to directly and remotely control the robot
55
actuators and is able to detect which of the actuators

has a malfunction.
These two teleoperation levels are associated
with two MMI allowing an efficient and flexible
utilisation of the remote robot.
3.3 Man Machine Interface

With the intention to support the medical expert and
in order to ensure the best possible diagnostic, two Figure 9: Design of the pseudo haptic input device
MMI have been developed for the “expert” station. prototype.
The first MMI is a graphical interface provided
to the medical expert to visualize the ultrasound
images and to choose the appropriate teleoperation 4 THE COMMUNICATION LINK
level. According to the teleoperation type, the
medical expert can control the robot in two different AND DATA TRANSMISSION
ways. When the expert controls the robot with the
high teleoperation level, he/she uses the pseudo The communication between the two stations can be
haptic input device (second MMI). When the expert carried out using different communication networks
controls the robot at a lower teleoperation level, such as satellite, ISDN lines (Integrated Services
he/she uses a visual interface (Figure 8) to control Digital Network) or the Internet. To perform the
each individual actuator. robotic tele-echography, three communications
protocols are used to transmit all data between the
two stations (Figure 10).
Figure 10: Data Flow representation between the expert

and the patient station and selected protocol.
Figure 8: Level 2 teleoperation control interface. The TCP (Transmission Control Protocol) is an
oriented connection protocol. It is used to establish
The second MMI is a pseudo haptic interface the connection between the two stations, and allows
that resembles an ultrasonic probe. The expert uses a continuous control of the connection state between
the pseudo haptic input device equipped with a six the two stations.
DOF localization magnetic sensor giving positions To transfer the robot controls, a reliable
and orientations. communication protocol with small transmission
The pseudo haptic input device (Poisson, 2003) delays is needed. Two protocols were firstly
holds a constant stiffness coefficient which provides considered: the TCP and UDP protocols. TCP
the medical expert with a rendering of the patients’ ensures reliability in the exchange but can block the
body compliance and the force applied by the probe communication in case of data loss. UDP protocol
on the patients’ body. The Figure 9 shows the design
(User Datagram Protocol) due to its simplicity
of the pseudo haptic input device prototype; it includes
a force sensor to measure the force applied by the generates small transmission delays. However it
medical expert in accordance with principal axis of cannot adapt its flow to the network bandwidth and
the probe. cannot confirm that data have arrived to the distant
56
Trinomial Protocol
site. It was then decided to use a compromise

between these two protocols: the trinomial 12
(Xiaoping Liu, 2003). It allows the network not to
Time (ms)
10
remain blocked in case of data loss as there is no 8
reemission of the lost data. However, contrary to 6

(a)
UDP, trinomial takes into account the transmission 0 100 200 300 400 500 600 700
Data sent
delay (i.e. the received data acknowledgement) Desired Control
150 Robot Feedback200
which allows a modulation of the flow and thus a 100
limitation of the network saturation. In our case, the 50

100
Angle (°)
Angle (°)
“expert” station sends the trajectories to the 0 0
-50
“patient” station using this protocol, and it receives -100
-100
the sensors feedback through the reception of the -150

3600
(b)
3700 3800 3900 4000 4100
-200
5600
(c)
5800 6000 6200
data acknowledgement. Time (ms) Time (ms)
Finally, a connection is established in order to

transmit the ambient images or the ultrasound Figure 11: Result during test between Montpellier and
images to the “expert” station via the Bourges (a) represents the data Round Trip Time, (b) and
videoconferencing system. A bandwidth of 256-384 (c) represent respectively the second and the third axis
kbps is required depending on the quality of servoing time delay.
ultrasonic device to offer the best image quality to
the “expert” station.
6 PERSPECTIVES
5 RESULTS Some improvements (transmission and architectural)

have to be considered to provide the medical expert
To validate the control architecture and to test the with better examination conditions thus ensuring the
efficiency of our control transmission protocol, a set best diagnostic as possible.
of tests was performed during a teleoperation The communication link (Internet, ISDN,
between the LIRMM in Montpellier and the LVR in satellite…) used to emit data from the expert station
Bourges using the Ethernet RENATER-4 public induces transmission delays that can provoke
network; this network provides a flow of about aperiodic data reception and even destabilize the
30Mbit/s. closed loop global system. This can disturb the
These results show Round-Trip Time (RTT) medical expert medical act when this delay varies
between Montpellier and Bourges (Figure 11 (a)), too strongly. To avoid that, it is possible to use a
which corresponds to the delays measured between FIFO regulator type (Lelevé, 2000) to synchronise
the data transmission and the acknowledgement the data reception, and thus to provide the expert
reception of this data. The RTT varies between 7 with a more steady date flow and to adapt more
and 11 ms proving the efficiency of the chosen data easily his control of the distant robot to the
transmission protocol. transmission delays.
Moreover, these results show delays obtained It is also possible to add an autonomous mode
between sending out the control data and the robot coupled with a level 4 of the teleoperation part, with
position feedback (Figure 11 (b) and (c)) (which the intention to realize a full echography of an organ
include transmission and the time of the servoings). allowing a 3D reconstruction. Thus, the medical
We can see that the system needs approximately 20 expert would select the organ to be investigated and
ms to reach the desired position. the robot would fallow all trajectories needed to
These results are quite satisfactory and allow us supply the 3D reconstruction wanted by the medical
to perform in real time examination in very good expert. The virtual diagnosis by the expert is made
conditions without disturbing the rendering of the from the 3D reconstruction.
distant environment to the expert (Arbeille, 2004).
7 CONCLUSION
From a mechanical view point, the OTELO2
prototype robot (Figure 12) corresponds to the criteria
imposed by the medical gesture study and experts’
requirement; it thus ensures identical examination
57
conditions than to the standard ultrasound one. The Mouriox, G., Novales, C., Smith-Guérin, N., Vieyres, P.,
modular architecture developed to control it permits Poisson, G., 2005. A hands free haptic device for tele-
easy insertion of new control modules whenever echography. 6TH International Workshop on Research
upgrade of the control architecture is needed. and Education Mechatronics, Annecy, France.
Finally, the communication protocol used to control Novales, C., Mourioux, G., Poisson, G., 2006. A multi-
the robot allows small transmission delays and offers level architecture controlling robots from autonomy to
teleoperation. First National Workshop on Control
moreover adaptability to the network condition.
Architectures of Robots. April 6, 7 2006. Montpellier,
The experimental results collected during the
France.
teleoperation between Montpellier and Bourges Poisson, G., Vieyres, P., Courreges, F., 2003. Sonde fictive
show the viability of the tele-echograph system and échographique European patent n°03290168.8.
provided good clinical results. Xiaoping Liu, P., Max. Q.-H. Meng and Simon. X. Yang,
2003. Data Communications for Internet Robots.
Autonous Robot Volume 15, pages 213 to 223.
Novembre 2003. Kluwer Academic Publishers.
Figure 12: The OTELO2 prototype system.
REFERENCES
Alami, R., Chatila, R., Fleury, S., Ghallab M., and Ingrand
F., 1998. An architecture for autonomy. The
International Journal of Robotics Research, Special
Issue on Integrated Architectures for Robot Control
and Programming, vol. 17, no 4, pp. 315-337.
Al Bassit, L., Poisson, G., Vieyres, P., 2003. Kinematics of
a Dedicated 6DOF Robot for Tele-Echography,
Proceedings of the 11th International Conference on
Advanced Robotics, ICAR 2003, pp. 906-910,
Portugal.
Arbeille, Ph., Ayoub, J., Vieyres, P., Porcher, M., Boulay,
J., Moreau, V., Poisson, G., 2004. Echographic
diagnostic on a patient far from the hospital using a
teleoperated robotic arm and the satellite
communications. Conference on Tele-Health and
Satellites, 8-9 July. Rabat, Marocco.
Arkin, R.C, 1998. Behavior-based Robotics. MIT Press.
Brooks, R.A., 1986. A robust layered control system for a
mobile robot. IEEE Journal of Robotics and
Automation, vol. 2, n°. 1, pp. 14–23.
Camarinha-Matos, L. M., Castolo, O., Vieira, W., 2002. A
mobile agents approach to virtual laboratories and
remote supervision. Journal of Intelligent and Robotic
Systems, no 35, pp. 1-22.
Lelevé, A., 2000. Contribution à la téléopération de
robots en présence de délais de transmission
variables. PhD thesis from Montpellier II University.
58
RPQ: ROBOTIC PROXIMITY QUERIES
Development and Applications
Albert Hernansanz, Xavier Giralt

Research Group On Intelligent Robotics and Systems, Technical University of Catalonia, 08028 Barcelona, Spain
Alberto Rodriguez
Robotics Institute, Carnegie Mellon University, Pittsburgh, PA 15213, USA
[email protected]
Josep Amat
Institute of Robotics and Industrial Informatics, Technical University of Catalonia, 08028 Barcelona, Spain
[email protected]
Keywords: Collision detection, Proximity queries, Surgical application.
Abstract: This paper presents a robotic proximity query package (RPQ) as an optimization of the general collision
library PQP (Proximity Query Package) for the detection of collisions and distance computation between
open kinematic chains such as robotic arms. The performance of the optimizations to non specific collision
query packages are explained and evaluated. Finally, a robotic assisted surgical application is presented which
has been used as a test bed for the proximity package.
1 INTRODUCTION The original package has been used to optimize the

queries when working with open kinematic chains,
One of the most important problems to solve in like robots. These optimizations have been done with
robotics is the collision avoidance between a robot the aim of improving the time performance of the
and its environment. A robot should perceive the risk generic package and simplifying its use in robotic en-
and have a reactive behavior before an imminent col- vironments. A system composed of two robots has
lision occurs. Path planning is a hard computational been used as a test bed to show the performance of
problem, so having a fast tool to calculate collisions is the RPQ library.
a key factor to decrease the necessary time to gener- Finally a robotic assited surgical application that
ate safety trajectories. In applications where no path benefits from RPQ performance is presented. The
planning exists, for instance in manual guidance or application consists of the execution of an assisted
teleoperation, a real time collision detector is needed cut of a rigid tissue. The surgeon guides freely the
so as to avoid collisions and to be able to interact with driller held by a slave robotic arm that avoids unde-
the environment, for example sliding over a surface. sired drillings by means of virtual protections. With
The knowledge of minimum distances between this application not only proximity queries are shown,
robots or objects that share a workspace enables but also the graphical interface and the use of a virtual
robots to behave in a predictive way. In the human- robot based on RPQ. More information and videos are
robot interaction field, virtual fixtures can be used available at http://grins.upc.edu
both to prevent collisions and help the human oper-
ator by increasing his performance (Stanisic et al.,
1996). In this kind of applications minimum distance 2 RELATED WORK
and collision detection must be known in real time.
A new library: Robotic Proximity Queries (RPQ) During the last years, great efforts have been devoted
package (Giralt and Hernansanz, 2006) has been to the development of efficient collision detection al-
developed to deal with these requirements, using gorithms due to their wide range of applications, such
PQP (UNC, 1999) as the a proximity query engine. as CAD/CAM, manufacturing, robotics, simulation
59
and computer animation. A wide study of the perfor- 3. Ability to use geometrical models based on trian-
mance and applicability of such methods can be found gulated meshes of points.
in (B.Geiger, 2000). Proximity query algorithms vary 4. Lack off restrictions on possible geometrical mod-
in terms of their range of applicability and the type els.
of queries they can solve, mainly collision detection,
minimum distance computation and interpenetrations The PQP library has been developed by UNC Re-
modelling. Although most algorithms allow as input search Group on Modelling, Physically-Based Simu-
triangulated meshes of 3D points, they differ in the lation and Applications and offers three different kind
way those points are pre-processed and represented of queries:
internally in order to speed up specific queries. - Collision detection: detecting whether two mod-
There is a wide set of methods that rely on Lin- els overlap, and optionally, give the complete list
Canny or Gilbert-Johnson-Keiethi like algorithms for of overlapping triangle pairs.
computing minimum distances between pairs of ob-
jects as I-Collide, Swift, Swift++, SOLID, DEEP. . ., - Distance computation: computing the minimum
but they are only applicable to convex polytopes distance between a pair of models.
(S.Ehmann and Lin, 2000; S.Ehmann and Lin, 2001; - Tolerance verification: determining whether two
Kim et al., 2002; Bergen, 2002). This restriction models are closer or farther than a given tolerance
makes them inappropriate for RPQ purposes, due to distance.
the need to deal with more general geometric models.
RPQ has been implemented in C++ language
More general collision detection methods usu- and its graphical interface has been developed using
ally base their efficiency on pre-computed represen- OpenGL. The RPQ library can be easily integrated
tations by means of hierarchies of bounding volumes. into any software application.
Their differences rely on the specific type of bound-
The library interface allows non expert program-
ing volumes used, ranging from binary space decom-
mers to use it in an easy manner. The graphical inter-
positions, spheres trees to oriented bounding boxes
face is a separate module, allowing the programmer
(OBB).
to decide whether using it or not. Fig. 1 shows the in-
Among this set of algorithms, RAPID and PQP tegration of the library and its graphical interface into
turn to be those that have both, fewer restrictions a generic application.
in the range of allowable geometric models and an
easier application programming interface (API). Both
of them use oriented bounding boxes for performing 3.1 Class Description
collision tests, and have similar time performances.
However, the fact that PQP offers a wider range The RPQ library is based on the Object Oriented
of queries, including minimum distance computation paradigm. Focused on this paradigm, and based on
and tolerance tests makes PQP the best option for the robotic environments, three main classes have been
proximity queries engine of RPQ, the Robotics Query developed: Scenario, Object and Robot.
Package presented in this paper.
3.1.1 Scenario
Scenario is the workspace where the objects cohabit.

3 LIBRARY DESCRIPTION Concerning its implementation, Scenario is a class
that contains all the objects (Robots and generic ob-
The goal of the Robotic Proximity Queries (RPQ) li- jects), a global reference frame, and all the methods
brary is to offer an easy, modular and fast proxim- necessary to generate the proximity query.
ity query package oriented to robotics. As explained
above, the aim of the project was not the development 3.1.2 Object
of a new collision detector, but specialize an existing
one into the robotics field. An Object is the minimum entity that exists in a Sce-
As described in section 2, there is a wide set of nario. There are two types of Objects: simple and
general purpose proximity query packages. The crite- complex. A simple Object is represented by a geo-
rions used to choose PQP as the best candidate for the metrical model composed of a set of triangles referred
development of RPQ are: to a frame tied to the Object. The Object has also a
transformation matrix to refer itself to the world refer-
1. Types of proximity queries available. ence frame. A complex Object is an Object composed
2. High time performance on proximity queries. of a set of geometrical models with joints (rotational
60
RPQ: ROBOTIC PROXIMITY QUERIES - Development and Applications
or prismatic) between them. Thus, a complex Ob- Three optimizations have been developed and
ject is an open kinematic chain composed of sub ob- tested to improve the performance offered by PQP:
jects. The transformation matrix Mi refers subobjecti • Different resolution levels of object’s representa-
to subobjecti−1 . The transformation matrix M0 refers tion
the object base (subobject0 ) to the world. The object
• Collision queries sorted using a Weight Matrix
stores its own geometrical model. Concerning its im-
plementation, an Object is a class containing the geo- • Collision Matrix
metrical model, the transformation matrix and a set of
methods to position and to orient itself in space. This 3.2.1 Different Resolution Levels of Object’s
class also contains methods to calculate the different Representation
detail representations of its geometrical model.
Objects can be represented in very different resolu-
3.1.3 Robot tion levels. The idea of this optimization is to use the
simplest representation models (minimum number of
A Robot is a particularization of a complex Object triangles) to discard collisions. The lower the number
where each of its links is represented by a simple Ob- of triangles of the geometric model are, the faster the
ject. A Robot has a set of functions to make a com- collision queries are executed.
plex Object as similar as possible to a real robot. For Three resolution levels are used to represent
instance, the spatial relationship between links is de- robots and two for the rest of objects. The highest res-
scribed using the Denavit-Hartenberg notation. Direct olution level is the complete geometrical model. The
and inverse kinematics can be calculated considering second level is the oriented bounding box (OBB) of
the robots own restrictions (joint limitations, config- each sub object in which a complex object is divided.
urations, etc). Concerning implementation, the class The lowest resolution level is the bounding box of the
Robot is derived from the class Object. Robot adds all whole complex object. This level is only defined for
the functions that are necessary to control a robot. For complex objects with more than a sub object, as in
instance joint positioning of the robot (direct kinemat- robots with several links. There are other possible in-
ics), position and orientation of its tool center point termediate resolution levels that can be used, for in-
(inverse kinematics), change of the robot configura- stance the convex hull. It offers a good ratio between
tion, joints overshoot . . . These added functions with resolution and the quantity of triangles, although it
respect an Object are very helpful when a new robot is does not reduce it as drastically as the low resolution
created or used in robotic applications like simulators, levels chosen.
path planners, etc. This optimization is useful in two different situa-
tions. First, in applications where no high precision is
required, for instance when the precision of the OBB
or the convex hull of each link is enough. The second
situation occurs when the different resolution levels
are used in a complementary manner.
Figure 1: Schema of integration of RPQ into a generic ap-

plication.
3.2 Optimizations
PQP is a generic package that does not use the knowl-
edge of the object’s kinematics. In contrast, RPQ is Figure 2: Robot with three resolution level representation:
oriented to robotics, and the knowledge of robot’s The geometrical model of each link (L3), the OBB of each
kinematics is the base of the optimizations that spe- link (L2) and the whole bounding box(L1).
cialize it. RPQ is designed to answer proximity
queries between two robots or between a robot and When a collision query is performed, a low to high
any kind of rigid object. resolution level list of collision queries is generated.
61
Starting with the lowest resolution level, queries are

generated until any collision can be completely dis-
carded. For instance, if a possible collision between
two 6-DOF robots is studied, the first query is done
between the bounding boxes of each robot. If the col-
lision can not be discarded, then the bounding box of
each link is used. If at this level collisions still can not
be discarded, the geometrical models of each link are
checked. As shown in Fig. 2 a Robot with its three
resolution levels of representation.
A test has been developed for the evaluation of
the performance of the optimizations. It consists on Figure 3: Time to solve a collision query between two
a couple of virtual robotic arms (Staubli RX60B) that Staubli RX60B robots using different resolution levels. L3:
are placed one close to the other in a scenario. The Geometrical model of each link. L2: The OBB of each link.
geometrical models used for the test are high resolu- L1: Bounding box of the robot.
tion (composed of 23012 triangles each). The robots
are placed at a variable distance between them and
the scenario is checked for collisions for an equidis-
The knowledge of the kinematics and the morphol-
tributed set of joint positions in their 6 dof.
ogy of the robots gives us the possibility of assign-
This test allows us to study the dependency on the
ing an a priori collision probability to each link of
performance of the proposed improvements in terms
the robot with respect to the rest of robots and ob-
of the probability of collision. This is because, as
jects present in the same workspace. During exe-
shown in Table 1, in the designed test, the closer the
cution time, these probabilities are automatically up-
robots are, the greater the number of joint configura-
dated depending on the result of the collision queries
tions that result in collision.
(Probability increases in case of detecting a collision
and decreases otherwise). Therefore, a weight matrix
Table 1: Dependence of the amount of colliding configura-
tions on the distance between robots. C is generated combining the probability of collision
between each pair of objects in the workspace. Each
Dist. Joint config. Collis. Not Collis.
component cij ∈ C verifies cij = Pi + Pj where
robots(m) checked
Pi and Pj are the assigned probability of collision of
0,4 9216 4864 4352 Objecti and Objectj respectively. These weights de-
0,5 9216 3530 5686 termine the order of the collision queries, that is if
0,6 9216 2500 6716 cij > ckt the collision query between Objecti and
0,7 9216 1121 8095 Objectj will be generated before Objectk and Objectt .
0,8 9216 140 9076
A simple way to assign a collision probability to
Fig. 3 shows the consequences of using differ- the links of a robot is to assign higher probability to
ent resolution levels. When the distance between the those links that are farther in the kinematic chain, with
robots increases, the queries solved with the bounding respect to the base of the robot.
box of the robot increases, and consequently the time
to solve a collision query between the robot decreases. 3.2.3 Collision Matrix
If the distance decreases, the best combination is us-
ing levels two and three or only level three.
The Collision Matrix is a binary matrix that reflects
3.2.2 Collision Queries Sorted using a Weight the possibility of collision between two objects. If
Matrix the collision matrix indicates that a collision between
two objects is impossible, its correspondent collision
This optimization is based on two assumptions. The query is not performed. Of course, a matrix query is
first one is that the goal of the query is just to know much less expensive than a collision query in compu-
whether there is a collision or not, but not the number tational terms.
of them. The second assumption is that the kinematics This optimization improves the performance of
and the morphology of the robots are well known. the system when a high number of collision queries
Given these assumptions, the objective is to find are discarded by the Collision Matrix. Computation-
quickly whether there is collision or not, by means ally, this condition can be expressed as in equation
of minimizing the number of partial collision queries. (1):
62
is performed. If at this level the collision cannot be

solved then it is necessary to study collisions among
n · QC > m · QM + k · (QC + QM ) (1) the whole set of links of both robots. The order in
with n=m+k which these queries must be performed is given by the
where: Weight Matrix. The query finishes as soon as a colli-
sion appears between a pair of links either in the sec-
n Total number of queries. ond or third level, or when all pairs have not reported
m Queries resolved with the Collision Matrix. any collision. For each pair of links, the second and
third representation levels are studied consecutively,
k Queries resolved with the Query Collision.
so if a collision is detected in the second level, the
QC Average time to solve a Query Collision. third level has to be studied as well.
QM Time to solve a query with the Col. Matrix.
The performance of the Collision Matrix has been
studied using the same test designed for testing the
use of different levels of representation. The results
are shown in Fig. 4. The farther the robots are,
the lower is the number of links that can collide, as
seen in Table 2. Therefore, the higher the number of
queries that are solved with the Collision Matrix. As
it is shown in the figure, using the Collision Matrix
the number of collision queries decreases, so does the
time to solve the query.
Table 2: Dependence of the percentage of collision queries

solved by the collision matrix on the distance between
robots.
Dist. between robots (m) Pairs solved with CM
0,4 4,94
0,6 9,88
0,9 24,69
1,2 64,20
Figure 5: Algorithm for collision detection between two

robots using all the optimizations.
4 APPLICATION
One of the advantages of making RPQ generic (al-
though it is optimized for robots) is that this library
Figure 4: Time necessary to solve a collision query between can be applied in a wide set of applications. In this
two Staubli RX60B robots using the Collision Matrix (CM) paper a robotic assisted surgical application is pre-
or not (NoCM). sented. This application has the aim of helping the
surgeon to make a cut on a rigid tissue, for example
Each one of the optimizations proposed improves in the cranium, avoiding undesired collisions between
the performance of the original library, PQP. How- the patient and the driller. The surgeon guides freely
ever, the combined use of all of them improves even the driller that is held by the robot acting in a pas-
more the global performance of the application. sive mode as seen in Fig. 6, allowing all movements
The complete algorithm with all three optimiza- except those which produce undesired collisions.
tions is shown in Fig. 5. First of all, a query colli- The system is composed by a Staubli RX60B
sion between the whole bounding box of both robots robotic arm and a driller. Between the robot and the
63
nally the projected point that is closer to the desired

one is selected. A problem can occur when the pro-
jected point falls outside the triangle region. In this
situation it is not possible to ensure that this new pro-
jected point is always be outside the object Fig. 7.c.
In this case the new point is projected to the perime-
ter of the triangle. To accomplish this, the outside
region of the triangle is divided into six new regions
(R1, R12, R2, R23, R3, R31), which are defined by
the normals of the edges of the triangle applied to the
vertices of the triangle. The point is then projected
Figure 6: Robotic assisted surgical application.
to the closest point of the triangle border of its region
Fig. 7.b. Now, the new destination point is collision
driller there’s a ATI Multi-Axis Force/Torque Sensor.
free Fig. 7.d.
The system transforms forces and torques generated
in the driller by the surgeon in new destination points
were the robot must go.
The geometric model of the patient’s cranium is
obtained transforming Computer Tomography data
into a geometrical model composed of triangles.
The surgeon can define the cutting area in the pre-
operative phase and, as will be explained latter, virtual
fixtures are automatically generated to protect the pa-
tient in the operative phase. The possibility of intro-
ducing virtual objects in the scene and the interaction
between them is one of the key factors of RPQ.
The surgeon has a friendly and easy-use graphical
interface that allows the navigation over the virtual Figure 7: Different contact situations between the tool and
system. This graphical interface helps the surgeon a triangle or set of triangles.
not only in the pre-operative phase but also during the
surgical operation, providing an augmented reality vi-
sual feedback (collisions, minimal distance between
4.2 Virtual Protections
the cranium and the tool, different points of view of
the scene, virtual fixtures ).
Virtual Protections are constraints that rule the be-
haviour of the robot that are specifically designed
4.1 Surface Navigation to prevent motion into a forbidden region of the
workspace. In this work, the surgeon guides freely
The library developed is useful not only to avoid colli- a driller held by the slave robot. The main idea is to
sions but also to navigate over an object’s surface. For develop a system helpful for the surgeon that prevents
instance, the robot tool slides over the surface of the undesired drillings.
virtual shield described in section 4.2. This surface
navigation allows the surgeon to feel smooth move- 4.2.1 Strategy Description
ments of the robot when the tool is in contact with the
virtual fixtures. The navigation algorithm helps the The main goal of the system is to give the robot a re-
surgeon not only avoiding the forbidden regions de- active behaviour by means of the definition of virtual
fined in the pre-operative phase but also guiding him objects in the PQP Scenario with two objectives:
to the desired cutting path. • Protect the patient from the robot and the driller.
The navigation algorithm modifies the position of
the robot tool, but not its orientation. The algorithm is • Help the surgeon to locate the desired cutting area.
based on three steps: Knowing the new desired des- PQPs ability to check collisions in real time allows
tination of the robot tool, the first step consists of us not only to achieve these objectives but to operate
detecting all collisions between the tool and the ob- in an efficient manner.
ject’s surface. When all collisions are detected, the Throughout a simple interface, in the pre-
second step consists of projecting the desired point operative phase, the surgeon specifies the exact loca-
to the plane of the collision triangle, Fig. 7.a. Fi- tion and shape of the cut that must be done. With that
64
information, the system generates a shield that covers have to be parametrized, (2) and (3). There are
the patient while it adapts perfectly to the target area some restrictions regarding them in order to make
defined by the surgeon, as shown in Fig. 8. the surface easy and intuitive.
The proposed behaviour is achieved by the navi-
gation algorithm exposed in chapter 4.1. The system - i (t) and o (t), must be parametrized radially,
gives the surgeon the confidence of knowing that the from an inner point, with constant radial veloc-
robot will never let him approach the patient in a non- ity, in order to avoid self intersections of the
desired area. However, while the surgeon does not surface, as in Fig. 9 a) and b).
attempt to cross the shield, the reobot can be moved - The polygon defined by i (t), must be star-
freely. shaped from the parametrization origin, in or-
der to avoid self intersections of the surface, as
4.2.2 Shield Generation in Fig. 9 c) and d).
- If i (t) defines a non star-shaped polygon a pre-
The problem here is to generate a surface that con- vious step must be done. An adaptation disc be-
nects the inner polygonal curve defined by the sur- tween the polygon and its convex hull is needed
geon with an outer curve. The connection between before constructing the shield, so that the latter
them is done in a smooth way, as Fig. 8 illustrates. can be parametrized radially from a suitable in-
ner point, as in Fig. 10.
Figure 9: a) Non valid parametrization of curves (self inter-

Figure 8: Lateral view of a face and the virtual shield. section of the surface). b) Valid parametrization of curves.
c) Non valid point for radial parametrization of curves.
The surface, s(u, v), is parametrized as the upper Polygon is not star-shaped from that point. d) Valid point
part of a regular torus, but adapting its equators to the for radial parametrization of curves.
inner and outer curves, as in (4).
Inn. curve i (t) = (ix (t) , iy (t)) t ∈ 0..2π(2)

Out. curve o (t) = (ox (t) , oy (t)) t ∈ 0..2π(3)
v v
s(u, v) = sin2 ix (u) + cos2 ox (u) ,
v 2 v 2
sin2 iy (u) + cos2 oy (u) , Figure 10: Adaptation disc between an non star-shaped
2 2 polygon and its correspondent 2D Convex Hull.
H sin (v) u ∈ 0..2π v ∈ 0..π (4)
• H, the height of the shield, must be chosen in or-
There are peculiarities of the shield to consider: der to protect the patient.
• The shield should be smooth, in order to make Once the required smoothness for the surface is de-
navigation over it as comfortable as possible. fined, the parametrization for i (t) and o (t), solved
Once the surface is generated it is transformed the problem with non star-shaped polygons and cho-
into a triangle model. By augmenting or decreas- sen a desired height, the surface is generated by eq.
ing the number of triangles, one can vary the (4).
smoothness of the surface. Finally, the intersections of the discrete set of
• Reaching the inner curve from the surface it has geodesics u = 2π π
N n and v = N n for n ∈ 1 . . . N is
to be easy and intuitive. This implies certain re- triangulated in order to obtain the geometrical model.
strictions on the shield design. This property de- The parameter N allows us to choose the number of
pends on the way inner points are connected to points and triangles of the surface, and ultimately its
outer points. First of all inner and outer curves smoothness.
65
4.2.3 Cutting Aid This is a really important point taking into account
that they are much more time consuming. There are
Besides the shield generated, another virtual object is cases where low resolution queries do not solve the
proposed in order to aid the surgeon to carry out the whole collision query. This increases the computation
cut with great accuracy. It consists of an inner cover time. However, choosing a suitable order for check-
that prevents the driller from escaping the perimeter ing collisions helps to find them in a quicker manner.
of the shape defined. The precomputation of impossibilities of collision be-
Since (2) has the shape of the cut to be done, we tween different objects (Collision Matrix) increases
define bi (t) as an under scaled version of i (t). bi (t) the performance of the system in case of having ob-
has the same shape as i (t) but its sides are placed at jects with restricted mobility in the workspace.
a distance ∆x from i (t) sides, being ∆x the desired The combined use of optimizations generate good
width for the cut. results in workspaces shared by at least two robots
Therefore, a new shield is generated, sb(u, v), hav- and objects.
ing a semisphere shape, with bi (t) as its equator, RPQ has a wide range of applicability. RQP li-
parametrized by (5). brary is not only useful for proximity queries but has
also proved to be a good tool for surface navigation
and virtual representations, due to its ability to intro-
sb(u, v) = duce virtual objects in the shared workspace. The vir-

= b cos2 (v) cos (u) + sin2 (v)bix (u) sin (v),
H tual fixtures developed in the paper are examples of
how RPQ can be used to modify robot’s behaviour.
b cos2 (v) sin (u) + sin2 (v)biy (u) sin (v),
H
As proved in the application presented, RPQ is not
b cos (v) u ∈ 0..2π v ∈ 0.. π
H (5) only useful for developers of robotic applications, but
2 also for users of robotic applications, i.e. surgeons
that require new robotic tools for improving surgical
procedures.
5 CONCLUSION
This paper presents the development of RPQ, a prox-
REFERENCES
imity queries library optimized for applications where Bergen, G. V. D. (2002). Solid collision detection library
robots share a common workspace and interact with users guide.
objects. Due to the amount of collision detection B.Geiger (2000). Real-time collision detection and re-
packages, RPQ is built above a generic collision pack- sponse for complex environments. In International
age, PQP. It is the generic collision package that better Conference on Computer Graphics. IEEE Computer
fits RPQ purposes. Society.
The goal of developing RPQ was to fill an exist- Giralt, X. and Hernansanz, A. (2006). Optimization of
ing gap in computer tools for robotic applications, proximity queries in robotic environments. In AVR
where robots interact with objects in their environ- - 2es Jornades UPC de Recerca en Automtica, Visio i
Robotica (in catalan).
ment. Three optimizations have been performed to a
generic collision library: working with different reso- Kim, Y., Lin, M., and Manocha, D. (2002). Deep: Dual-
space expansion for estimating penetration depth be-
lution levels of representation, the use of a weighted
tween convex polytopes. In International Conference
matrix for choosing the best order for collision check- on Robotics and Automation. IEEE.
ing and the definition of a binary matrix that deter-
S.Ehmann and Lin, M. (2000). Accelerated proxim-
mines the possibility of collision between objects. ity queries between convex polyhedra by multilevel
RQP has been validated in different applications such voronoi marching. Technical report, Department of
as multirobot collision avoidance, virtual robot con- Computer Science, University of North Carolina.
troller and surface navigation. S.Ehmann and Lin, M. (2001). Accurate and fast proximity
As expected, optimizations improve the time per- queries between polyhedra using convex surface de-
formance of the system, although this improvement is composition. In Eurographics, volume 3.
highly application dependent. Stanisic, Z., Jackson, E., and Payandeh, S. (1996). Virtual
The introduction of different levels of resolution fixtures as an aid for teleoperation. In 9th Canadian
in the geometric models of the objects and robots gen- Aeronautics and Space Institute Conference.
erally decreases the computational time for collision UNC (1999). Pqp - a proximity query package by research
checking. The use of bounding boxes decreases dras- group on modeling, physically-based simulation and
applications.
tically the number of high resolution queries needed.
66
EVOLUTIONARY PATH PLANNING FOR UNMANNED AERIAL
VEHICLES COOPERATION
Ioannis K. Nikolos and Nikos Tsourveloudis

Intelligent Systems and RoboticsLaboratory, Technical University of Crete, Chania, Greece
Keywords: 3-D Path Planning, Evolutionary Algorithms, Navigation, UAV cooperation, B-Splines.
Abstract: We suggest an evolutionary based off-line/on-line path planner for cooperating Unmanned Aerial Vehicles
(UAVs) that takes into account the environment characteristics and the flight envelope and mission
constraints of the cooperating UAVs. The scenario under consideration is the following: a number of UAVs
are launched from the same or different known initial locations. The main issue is to produce 3-D
trajectories that ensure a collision free operation with respect to mission constraints. The path planner
produces curved routes that are represented by 3-D B-Spline curves. Two types of planner are discussed:
The off-line planner generates collision free paths in environments with known characteristics and flight
restrictions. The on-line planner, which is based on the off-line one, generates collision free paths in
unknown static environments, by using acquired information from the UAV’s on-board sensors. This
information is exchanged between the cooperating UAVs in order to maximize the knowledge of the
environment. Both off-line and on-line path planning problems are formulated as optimization problems,
with a Differential Evolution algorithm to serve as the optimizer.
1 INTRODUCTION system displays cooperative behavior if, due to some

underlying mechanism, i.e. the “mechanism of
Path planning is the generation of a space path cooperation”, there is an increase in the total utility
between an initial location and the desired of the system.
destination that has an optimal or near-optimal Path planning problems are computationally
performance under specific constraints (Gilmore, demanding multi-objective multi-constraint
1991). Searching for optimal paths is not a trivial optimization problems (Mettler et al., 2003). The
task; in most cases results in excessive computation problem complexity increases when multiple UAVs
time and in some cases even the computation of just should be used. Various approaches have been
a feasible path is a very difficult problem. Therefore, reported for UAVs coordinated route planning, such
in most cases we search for suboptimal or just as Voronoi diagrams (Beard et al., 2002), mixed
feasible solutions. integer linear programming (Richards et al., 2002),
Unmanned Aerial Vehicles (UAVs) path planning (Schouwenaars et al., 2004) and dynamic
algorithms were initially developed for the solution programming formulations (Flint et al., 2002).
of the single vehicle case. However, the Computational intelligence methods, such as Neural
continuously increasing interest for cooperating Networks (Ortega and Camacho, 1996), Fuzzy
UAVs resulted in the development of algorithms that Logic (Kwon and Lee, 2000) and Evolutionary
take into account the special issues and Algorithms (EAs), (Nikolos et al., 2003), (Zheng et
characteristics of such missions. Cooperation al., 2005) have been successfully used to produce
between UAVs has gained recently an increased trajectories for guiding mobile robots in known,
interest as systems of multiple vehicles engaged in unknown or partially known environments.
cooperative behavior show specific benefits EAs have been successfully used in the past for
compared to a single vehicle. Cooperative behavior the solution of the path-finding problem in ground
may be defined as follows (Cao et al., 1999): Given based or sea surface navigation (Michalewicz,
some tasks specified by a designer, a multiple robot 1999). A common practice was to model the path
67
using straight line segments, connecting successive position and the final destination. Both path
way points. In the case of partially known or planning problems are formulated as optimization
dynamic environments a feasible and safe trajectory (minimization) problems, where specially
was planned off-line by the EA and the algorithm constructed functions take into account mission and
was used on-line whenever unexpected obstacles cooperation objectives and constraints, with a
were sensed (Smierzchalski, 1999), (Smierzchalski differential evolution algorithm to serve as the
and Michalewicz, 2000). EAs have been also used optimizer.
for solving the path-finding problem in a 3-D The rest of the chapter is organized as follows:
environment for underwater vehicles, assuming that section 2 contains B-Spline and differential
the path is a sequence of cells in a 3-D grid evolution algorithms fundamentals. The off-line path
(Sugihara and Yuh, 1997). planner will be briefly discussed in section 3.
In (Nikolos et al., 2003) an EA based framework Section 4 deals with the concept of cooperating
was utilized to design an off-line / on-line path UAV on-line path planning using differential
planner for UAVs, which calculates a curved path evolution. The problem formulation is described,
line represented by B-Spline curves in a 3-D including assumptions, objectives, constraints, cost
environment. The coordinates of the B-Spline function definition and path modeling. Simulations
control points serve as design variables. For both results are presented in section 5, followed by
off-line and on-line planners, the problem is discussion and conclusions in section 6.
formulated as an optimization one. Constraints are
formulated using penalty functions.
The work in this paper is the outgrowth of the one 2 DIFFERENTIAL EVOLUTION
presented in (Nikolos et al., 2003) for the case of OF B-SPLINE PATHS
multiple and cooperating UAVs. The scenario
considered here is the following: a number of UAVs 2.1 Path Modelling using B-Splines
are launched from the same or different known
initial locations with predefined initial directions. Straight-line segments that connect a number of
The main issue is to produce 3-D trajectories, waypoints have been used in the past to model UAV
represented by 3-D B-Spline curves, which connect paths in 2D or 3D space (Moitra, 2003), (Zheng et
the initial locations with a single destination location al., 2005). However, these simplified paths cannot
and ensure a collision free operation with respect to be used for an accurate simulation of UAV’s flight,
mission constraints. unless a large number of waypoints is adopted.
Two types of path planner are discussed: The off- Furthermore, if an optimization procedure is used,
line planner generates collision free paths in the number of design variables explodes, especially
environments with known characteristics and flight if cooperating flying vehicles are considered. As a
restrictions. The on-line planner, being an extension result, the computation time becomes impractical for
of the off-line one and based on the ideas presented real world applications.
in (Nikolos et al., 2003) was developed to generate B-Spline curves have been used in the past for
collision free paths in unknown environments. The trajectory representation in 2-D (Alfaro and Garcia,
knowledge of the environment is gradually acquired
1988) or in 3-D environments (Nikolos et al., 2003),
through the on-board sensors that scan the area
(Nikolos et al., 2001). Their parametric construction
within a certain range from each UAV. This
information is exchanged between the cooperating provides the ability to produce non-monotonic
UAVs in order to maximize the sensors curves, like the trajectories of moving objects. If the
effectiveness. The on-line planner rapidly generates number of control points of the corresponding curve
a near optimum path for each vehicle that will guide is n+1, with coordinates (x0, y0, z0) ,…, (xn, yn, zn),
the vehicle safely to an intermediate position within the coordinates of the B-Spline curve may be written
the known territory, taking into account the mission as:
and cooperation objectives and constraints. The n
process is repeated until the corresponding final x (u ) = ∑x i ⋅ N i , p (u ) , (1)
position is reached by an UAV. Then, each one of i=0
the remaining UAVs uses the acquired information

about the environment and the off-line planner
output to compute a path that connects its current
68
n
analytical description of the algorithm’s structure is
y (u ) = ∑
i=0
yi ⋅ N i , p (u ) , (2) presented.
Given a cost function f ( X ) :
n param
→ ,
the optimization target is to minimize the value of
n this cost function by optimizing the values of its
z (u ) = ∑z
i=0
i ⋅ N i, p (u ) , (3) parameters (design variables), that is
where u is the free parameter of the curve, Ni,p(u) are

(
X = x1 , x 2 , … , x n param , x j ∈ ) , (7)
the blending functions of the curve and p is its where X denotes the vector composed of nparam cost
degree, which is associated with curve’s smoothness function parameters (design variables). These
(p+1 being its order). Higher values of p correspond parameters take values between specific upper and
to smoother curves (Farin, 1988). lower bounds, as follows:
The blending functions are defined recursively in (L ) (U ) (8)
xj ≤ xj ≤ xj , j = 1, … , n p a ra m
terms of a knot vector U={u0,…, um}, which is a
non-decreasing sequence of real numbers, with the The DE algorithm implements real encoding for
most common form being the uniform non-periodic the values of the objective function’s variables. In
one, defined as: order to obtain a starting point for the algorithm, an
initialization of the population takes place. The
⎧ 0 if i < p +1
⎪ initialization (for G = 1 ) is established by randomly
ui = ⎨ i − p if p +1 ≤ i ≤ n (4) assigning values to the parameters j of each
⎪ n − p + 1 if n < i.
⎩ i member of the population, within the given
boundaries
The blending functions Ni,p are computed using the
knot values defined above, as: 1
( U L L
)
xi(, j) = r ⋅ x(j ) − x(j ) + x(j ) , i = 1,…, npop , j = 1,…, nparam (9)
⎧1 if u i ≤ u < u i +1
N i ,0 ( u ) = ⎨ (5) where r is a uniformly distributed random value
⎩0 otherw ise ,
within range [0, 1]. DE’s mutation operator is based
on a triplet of randomly selected different
u − ui ui + p+1 − u individuals. For each i member of the population, a
Ni, p ( u ) = Ni, p−1 ( u ) + Ni+1, p−1 ( u ) . (6)
ui + p − ui ui + p+1 − ui+1 new parameter vector Vi(G) is generated by adding
the weighted difference vector between the two
members of the triplet to the third one (the donor).
If the denominator of the two fractions in (6) is That is:
zero, that fraction is defined to have zero value.
Parameter u varies between 0 and (n-p+1) with a
Vi
(G ) (G )
= X r 3 + F ⋅ X r1
i ( (G )
i
− X r2
(G )
i ),
constant step, providing the discrete points of the B-
Spline curve. The sum of the values of the blending
G G
(
V i ( ) = v i(,1 ) , v i(, 2 ) , … , v i(, n )
G G
p ara m ), (10)
functions for any value of u is always 1.

where X r( 3 ) is called the “donor”, G is the current
G
i
2.2 Differential Evolution Algorithm generation, and

i = 1,… , n pop , j = 1,… , n param
Differential Evolution (DE) (Price et al., 2005) is
used in this work as the optimization tool. The r1i ∈ ⎡⎣1,… , n pop ⎤⎦ , r 2i ∈ ⎡⎣1,… , n pop ⎤⎦ , r 3i ∈ ⎡⎣1,… , n pop ⎤⎦
(11)
classic DE algorithm evolves a fixed size r1i ≠ r 2i ≠ r 3i ≠ i
population, which is randomly initialized. After F ∈ [ 0,1 + ] , r ∈ [ 0,1]
initializing the population, an iterative process is
started and at each generation G , a new population In this way a perturbed individual is generated.
is produced until a stopping condition is satisfied. At The perturbed individual Vi(G) and the initial
each generation, each element of the population can population member Xi(G) are then subjected to a
be replaced with a new generated one. The new crossover operation that generates the final
element is a linear combination between a randomly candidate solution U i( G +1 )
selected element and the difference between two
other randomly selected elements. Below, a more
69
3 OFF-LINE PATH PLANNER

⎧v( G) if ( r ≤ C ∨ j = k ) ∀ j = 1,…, n
( G+1) ⎪ i, j r param
ui, j =⎨
( G)
⎪⎩xi, j The off-line planner generates collision free paths in
otherwise (12)
environments with known characteristics and flight
Cr ∈[0,1] restrictions. The derived path lines are continuous 3-
D B-Spline curves.
where k is a random integer within [1, nparam],
Each B-Spline control point is defined by its
chosen once for all members of the population. The
three Cartesian coordinates xk,j, yk,j, zk,j (k=0,…,n,
random number r is seeded for every gene of each
j=1,…,N), N is the number of UAVs, while n+1 is
chromosome. F and Cr are DE control parameters,
the number of control points in each B-Spline curve
which remain constant during the search process and
(the same for all curves). The first (k=0) and last
affect the convergence behaviour and robustness of
(k=n) control points of the control polygon are the
the algorithm. Their values also depend on the
initial and target points of the jth UAV, which are
objective function, the characteristics of the problem
predefined by the user. The second (k=1) control
and the population size.
point is positioned in a pre-specified distance from
The population for the next generation (G+1) is
the first one, in a given altitude, and in a given
selected between the current population and the final
candidates. If each candidate vector is better fitted direction, in order to define the initial direction of
than the corresponding current one, the new vector the corresponding path.
replaces the vector with which it was compared. The For the case of a single UAV the optimization
DE selection scheme is described as follows (for a problem to be solved minimizes a set of five terms,
minimization problem): connected to various objectives and constraints; they
are associated with the feasibility of the curve, its
Xi
(G +1 )
⎧U (G + 1 )
⎪ i
= ⎨
(
if f U i
(G + 1 )
) ≤ f (X ( ))
i
G
(13)
length and a safety distance from the ground. The
cost function to be minimized is defined as:
⎪ X (G ) o th e r w is e
⎩ i
5
Α new scheme (Hui-Yuan et al., 2003) to

f = ∑
i =1
wi fi (15)
determine the donor for mutation operation has been
adopted for accelerating the convergence rate. In this Term f1 penalizes the non-feasible curves that
scheme, donor is randomly selected (with uniform pass through the solid boundary. In order to compute
distribution) from the region within the “hyper- this term, discrete points along each curve are
triangle”, formed by the three members of the triplet computed, using B-Spline equations (Eq. 1 to 6) and
presented below: a pre-specified step for B-Spline parameter u.
Term f2 is the length of the curve (non-
3 ⎛ 3 ⎞ (G )
donori( ) =
G
∑ ⎜⎜ λ ∑ λ
k =1 ⎝
k m ⎟ X rk ,
⎟ i
λm = rand [ 0 ,1] (14) dimensional with the distance between the starting
and destination points) and is used to provide shorter
m =1 ⎠
paths.
where rand [0, 1] denotes a uniformly distributed Term f3 is designed to provide flight paths with a
value within the range [0, 1]. With this scheme the safety distance from solid boundaries.
donor comprises the local information of all
Term f4 is designed to provide B-Spline curves
members of the triplet, providing a better starting-
point for the mutation operation that result in a better with control points inside the pre-specified space.
distribution of the trial-vectors. Term f5 was designed to provide path lines within
The random number generation (with uniform the known terrain. This characteristic is particularly
probability) is based on the algorithm presented in useful when the off-line path planner is used
(Marse and Roberts, 1983). In each different together with the on-line one, as it will be explained
operation inside the DE algorithm that requires a later.
random number generation, a different sequence of Weights wi are experimentally determined, using
random numbers is produced, by using a different as criterion the almost uniform effect of the last four
initial seed for each operation and a separate storage terms in the objective function. Term w1f1 has a
of the corresponding produced seeds.
dominant role in (15) providing feasible curves in
few generations, since path feasibility is the main
concern. The minimization of (15), through the DE
procedure, results in a set of B-Spline control points,
which actually represents the desired path.
70
4 ON-LINE PATH PLANNING Therefore, what is finally generated is a group of

smooth curve segments connected to each other,
Having N UAVs launched from the same or eventually connecting the starting point to the final
different known initial locations, the issue is to destination for each UAV.
produce N 3-D trajectories, aiming at reaching a In the on-line problem only four control points
predetermined target location, while ensuring define each B-Spline curve, the first two of which
collision avoidance with the environmental are fixed and determine the direction of the current
UAV path. The remaining two control points are
obstacles. Additionally, the produced flight paths
allowed to take any position within the scanned by
should satisfy specific route constraints. Each
the radars known space, taking into consideration
vehicle is assumed to be a point, while its actual size given constraints.
is taken into account by equivalent obstacle – When the next path segment is to be generated,
ground growing. only the first control point of the B-Spline curve is
The general constraint of the problem is the known (it is the last control point of the previous B-
collision avoidance between UAVs and the ground. Spline segment). The second control point is not
The route constraints are: random as it is used to make sure that at least first
(a) Predefined initial and target coordinates for derivative continuity of the two connected curves is
all UAVs provided at their common point. Hence, the second
(b) Predefined initial directions for all UAVs, control point of the next curve should lie on the line
(c) Predefined minimum and maximum limits of defined by the last two control points of the previous
allowed-to-fly space (minimum and maximum curve.
allowed Cartesian coordinates for all path points).
The cooperation objective is that all UAVs
should reach the same target point.
The on-line planner uses acquired information
from all UAV’s on-board sensors (that scan the area
within a certain range from the corresponding
UAV). The planner rapidly generates a near
optimum path, modeled as a 3-D B-Spline curve that
will guide each vehicle safely to an intermediate
position within the already scanned area. The
information about the already scanned area by each
UAV is passed to the rest cooperating UAVs, in
order to maximize the knowledge of the
environment. The process is repeated until the final
position is reached by a UAV. Then the rest of
UAVs turn to the off-line mode and a single B-
Spline path for each UAV is computed to guide the Figure 1: On-line path planning for a single UAV. The
corresponding vehicle from its current position, path is shown in an intermediate position of the UAV’s
through the already scanned territory to the common flight. The already scanned area is presented in color.
final destination. As a result, each path line from the
corresponding starting point to the final goal is a The path-planning algorithm considers the
smooth, continuous 3-D line that consists of scanned surface as a group of quadratic mesh nodes.
successive B-Spline curves, smoothly connected to All ground nodes are initially considered unknown.
each other (Figure 1). Radar information is used to produce the first path
As the terrain is completely unknown and the line segment for the corresponding UAV. As the
radars (or equivalent sensors) gradually scan the vehicle is moving along its first segment and until it
area, it is impossible to generate feasible paths that has travelled about 2/3 of its length, its radar scans
connect each starting point with the target one. the surrounding area, returning a new set of visible
Instead, at certain moments, each sensor scans a nodes, which are subsequently added to the common
region around the corresponding moving UAV and set of scanned nodes. The on-line planner, then,
this region is added to the already scanned regions produces a new segment for each UAV, whose first
by all cooperating UAVs. For the UAV under point is the last point of the previous segment and
consideration a path line is generated that connects a whose last point lies somewhere in the already
temporary starting point with a temporary ending scanned area, its position being determined by the
point. Each temporary ending point is also the next on-line procedure. The on-line process is repeated
curve’s starting point for the corresponding vehicle. until the ending point of the current path line
71
segment of one UAV lies close to the final point for the corresponding UAV and the final
destination. Then the rest UAVs turn into the off- destination and c is a constant. This potential allows
line process, in order to reach the target using B- for selecting curved paths that bypass obstacles lying
Spline curves that pass through the scanned terrain. between the starting and ending point of each B-
The computation of intermediate path segments Spline curve (Nikolos et al., 2003).
for each UAV is formulated as a minimization Term f6 is similar to term f5 but it corresponds to
problem. The cost function to be minimized is a potential field between the current starting point
formulated as the weighted sum of eight different (of the corresponding path segment) and the final
terms: target.
8 Term f7 is designed to prevent UAVs from being
f = ∑
i =1
wi fi , (16) trapped in small regions and to force them move
towards unexplored areas. In order to help the UAV
leave this area, term f7 repels it from the points of
where wi are the weights and fi are the corresponding the already computed path lines (of all UAVs).
terms described below. Furthermore, if a UAV is wandering around to find a
Terms f1, f2, and f3 are the same with terms f1, f3, path that will guide it to its target, the UAV will be
and f4 respectively of the off-line procedure. Term f1 forced to move towards areas not visited before by
penalizes the non-feasible curves that pass through this or other UAVs. This term has the form:
the solid boundary. Term f2 is designed to provide
N po
flight paths with a safety distance from solid 1 1 ,
boundaries. Only already scanned ground points are f7 =
N po ∑r k
(19)
considered for this calculation. Term f3 is designed k =1
to provide B-Spline curves with control points inside where Npo is the number of the discrete curve points
the pre-specified working space. produced so far by all UAVs and rk is their distance
Term f4 is designed to provide flight segments from the last point of the current curve segment.
with their last control point having a safety distance Term f8 represents another potential field, which
from solid boundaries. This term was introduced to
is developed in a small area around the final target.
ensure that the next path segment that is going to be
computed will not start very close to a solid When the UAV is away from the final target, the
boundary (which may lead to infeasible paths or term is given a constant value. When the UAV is
paths with abrupt turns). The minimum distance Dmin very close to the target the term’s value decreases
from the ground is calculated for the last control proportionally to the square of the distance between
point of the current path segment. Only already the last point of the current curve and the target.
scanned ground points are considered for this Weights wi in (16) are experimentally determined,
calculation. Term f4 is defined as: using as criterion the almost uniform effect of all the
terms, except the first one. Term w1f1 has a dominant
( ) , (17)
2
f 4 = d safe D m in
role, in order to provide feasible curve segments in a
where dsafe represents a safety distance from the few generations, since path feasibility is the main
solid boundary. concern.
The value of term f5 depends on the potential
field strength between the initial point of the UAVs
path and the final target. This potential field between 5 SIMULATION RESULTS
the two points is the main driving force for the
gradual development of each path line in the on-line The same artificial environment was used for all test
procedure. The potential is similar to the one cases considered here. The artificial environment is
between a source and a sink, defined as: constructed within a rectangle of 20x20 (non-
r 2 + c ⋅ r0 , dimensional lengths). The (non-dimensional) radar’s
Φ = ln (18) range for each UAV was set equal to 4. The safety
r1 + c ⋅ r0
distance from the ground was set equal to dsafe=0.25.
where r1 is the distance between the last point of the The (experimentally optimized) settings of the DE
current curve and the initial point for the algorithm during the on-line procedure were as
corresponding UAV, r2 is the distance between the follows: population size = 20, F = 0.6, Cr = 0.45,
last point of the current curve and the final number of generations = 70. For the on-line
destination, r0 is the distance between the initial procedure we have two free-to-move control points,
resulting in 6 design variables. The corresponding
72
settings during the off-line procedure were as Figure 5. The starting point for the first and second
follows: population size = 30, F = 0.6, Cr = 0.45, UAVs are the same as in case 1, while the third
number of generations = 70. For the off-line UAV is near the middle of the left side of the terrain.
procedure eight control points were used to construct
each B-Spline curve (including the initial (k=0) and
the final one (k=7). These correspond to five free-to-
move control points, resulting in 15 design variables.
All B-Spline curves have a degree p equal to 3. All
experiments have been designed in order to search
for path lines between “mountains”. For this reason,
an upper ceiling for flight height has been enforced
in the optimization procedure, by explicitly
providing an upper boundary for the z coordinates of
all B-Spline control points.
Figure 3: The final path lines for the test case 1.
Figure 2: Test case 6 corresponds to the on-line path

planning of two UAVs. When the first UAV (blue line)
reaches the target the second one turns into the off-line
mode. The starting point for the first UAV is close to the
lower left corner of the terrain, while for the second one is
close to the upper left corner. Figure 4: On-line path planning of three cooperating
UAVs (test case 2). The picture shows the paths when the
Test case 1 corresponds to on-line path planning first UAV (blue line) reaches the target.
of two UAVs. Figure 2 shows the path lines when
the first UAV (blue line) reaches the target. In that Test case 3 also corresponds to the on-line path
moment the second UAV (red line) turns into the planning of three UAVs but with distant starting
off-line mode, in order to compute a feasible path points. Figure 6 shows the status of the two path
line that connects its current position with the target, lines when the first UAV (blue line) reaches the
through the already scanned area. The final status is target. The final status is demonstrated in Figure 7.
demonstrated in Figure 3. The starting point for the As the first UAV (blue line) is close to the target, it
first UAV is near the lower left corner of the terrain, succeeds in reaching it using just one B-Spline
while for the second one is near the upper left segment. Then, the other two UAVs turn into off-
corner. line mode to reach the target.
Test case 2 corresponds to the on-line path In the test case 4 three UAVs are launched from
planning of three UAVs. Figure 4 shows the status the centre of the working space but towards different
of the two path lines when the first UAV (blue line) directions. Figure 8 shows the status of the two path
reaches the target. In that moment the second UAV lines when the first UAV (blue line) reaches the
(red line) and the third one (black line) turn into the target. The final status is demonstrated in Figure 9.
off-line mode, in order to compute feasible path When the final point of a curve segment is within a
lines that connect their positions with the target. The small distance from the final destination, the on-line
final paths for all three UAVs are demonstrated in procedure is terminated; this is the reason for the
73
absence of coincidence between the final points of

the first (blue line) and the rest path lines.
Figure 8: On-line path planning for three UAVs launched

from the same point but different initial directions. The
first UAV (blue line) just reached the target.
Figure 5: The final paths of the test case 2.
Figure 9: The final paths of test case 4.
6 CONCLUSIONS
Figure 6: Path lines of three distant UAVs when the first A path planner for a group of cooperating UAVs
one (blue line) reaches the target.
was presented. The planner is capable of producing
smooth path curves in known or unknown static
environments. Two types of path planner were
presented. The off-line one generates collision free
paths in environments with known characteristics
and flight restrictions. The on-line planner, which is
based on the off-line one, generates collision free
paths in unknown environments. The path line is
gradually constructed by successive, smoothly
connected B-Spline curves, within the gradually
scanned environment. The knowledge of the
environment is acquired through the UAV’s on-
board sensors that scan the area within a certain
range from each UAV. This information is
exchanged between the cooperating UAVs; as a
Figure 7: The final paths of test case 3. result, each UAV utilizes the knowledge of a larger
region than the one scanned by its own sensors. The
on-line planner generates for each vehicle a smooth
74
path segment that will guide the vehicle safely to an Gomez Ortega, J., and Camacho, E.F., 1996. Mobile
intermediate position within the known territory. Robot navigation in a partially structured static
environment, using neural predictive control. Control
The process is repeated for all UAVs until the
Eng. Practice, 4, 1669-1679.
corresponding final position is reached by an UAV. Kwon, Y.D., and Lee, J.S., 2000. On-line evolutionary
Then, the rest vehicles turn into the off-line mode in optimization of fuzzy control system based on
order to compute path lines consisting of a single B- decentralized population. Intelligent Automation and
Spline curve that connect their current positions with Soft Computing, 6, 135-146.
the final destination. These path lines are enforced to Nikolos, I.K., Valavanis, K.P., Tsourveloudis, N.C.,
lie within the already scanned region. Both path Kostaras, A., 2003. Evolutionary Algorithm based
offline / online path planner for UAV navigation.
planners are based on optimization procedures, and IEEE Transactions on Systems, Man, and Cybernetics
specially constructed functions are used to encounter – Part B: Cybernetics, 33, 898-912.
the mission and cooperation objectives and Michalewicz, Z., 1999. Genetic Algorithms + Data
constraints. A differential evolution algorithm is Structures = Evolution Programs. Springer
used as the optimizer for both planners. No Publications.
provision is taken by the on-line planner for Smierzchalski, R., 1999. Evolutionary trajectory planning
of ships in navigation traffic areas. Journal of Marine
collision avoidance between the cooperating Science and Technology, 4, 1-6.
vehicles; this can be encountered by an on board Smierzchalski, R., and Michalewicz Z., 2000. Modeling of
controller for each vehicle. ship trajectory in collision situations by an
evolutionary algorithm. IEEE Transactions on
Evolutionary Computation, 4, 227-241.
Sugihara, K., and Yuh, J., 1997. GA-based motion
REFERENCES planning for underwater robotic vehicles. UUST-10,
Durham, NH.
Gilmore, J.F., 1991. Autonomous vehicle planning Moitra, A., Mattheyses, R.M., Hoebel, L.J., Szczerba, R.J.,
analysis methodology. In Proceedings of the Yamrom, B., 2003. Multivehicle reconnaissance route
Association of Unmanned Vehicles Systems and sensor planning. IEEE Transactions on Aerospace
Conference. Washington, DC, 503–509. and Electronic Systems, 37, 799-812.
Zheng, C., Li, L., Xu, F., Sun, F., Ding, M., 2005. Martinez-Alfaro H., and Gomez-Garcia, S. 1988. Mobile
Evolutionary Route Planner for Unmanned Air robot path planning and tracking using simulated
Vehicles. IEEE Transactions on Robotics, 21, 609- annealing and fuzzy logic control. Expert Systems with
620. Applications, 15, 421-429.
Uny Cao, Y., Fukunaga, A.S., Kahng, A.B., 1997. Nikolos, I.K., Tsourveloudis, N., and Valavanis, K.P.,
Cooperative Mobile Robotics: Antecedents and 2001. Evolutionary Algorithm Based 3-D Path Planner
Directions. Autonomous Robots, 4, 7-27. for UAV Navigation. In Proceedings of the 9th
Mettler, B., Schouwenaars, T., How, J., Paunicka, J., and Mediterranean Conference on Control and
Feron E., 2003. Autonomous UAV guidance build-up: Automation, Dubrovnik, Croatia.
Flight-test Demonstration and evaluation plan. In Farin, G., 1988. Curves and Surfaces for Computer Aided
Proceedings of the AIAA Guidance, Navigation, and Geometric Design, A Practical Guide. Academic
Control Conference, AIAA-2003-5744. Press.
Beard, R.W., McLain, T.W., Goodrich, M.A., Anderson, Price, K.V., Storn, R.M., Lampinen, J.A., 2005.
E.P., 2002. Coordinated target assignment and Differential Evolution, a Practical Approach to Global
intercept for unmanned air vehicles. IEEE Optimization. Springer-Verlag, Berlin Heidelberg.
Transactions on Robotics and Automation, 18, 911- Marse, K. and Roberts, S.D., 1983. Implementing a
922. portable FORTRAN uniform (0,1) generator.
Richards, A., Bellingham, J., Tillerson, M., and How., J., Simulation, 41-135.
2002. Coordination and control of UAVs. In
Proceedings of the AIAA Guidance, Navigation and
Control Conference, Monterey, CA.
Schouwenaars, T., How, J., and Feron, E., 2004.
Decentralized Cooperative Trajectory Planning of
multiple aircraft with hard safety guarantees. In
Proceedings of AIAA Guidance, Navigation, and
Control Conference and Exhibit, AIAA-2004-5141.
Flint, M., Polycarpou, M., and Fernandez-Gaucherand, E.,
2002. Cooperative Control for Multiple Autonomous
UAV’s Searching for Targets. In Proceedings of the
41st IEEE Conference on Decision and Control.
75
A NOVEL STRATEGY FOR EXPLORATION WITH MULTIPLE
ROBOTS
Jonathan Rogge and Dirk Aeyels

SYSTeMS Research Group, Ghent University, Ghent, Belgium
[email protected],[email protected]
Keywords: Multi-robot systems, coverage, exploration, demining.
Abstract: The present paper develops a novel strategy for the exploration of an unknown environment with a multi-
robot system. Contrary to most exploration problems, the topographical properties of the space need not be
mapped. The algorithm we propose is inspired by methods used for complete coverage of an area, where all
free space has to be physically covered by all robots. In the present paper it is required that the entire free
space is covered by the sensors of the robots, with a certainty of 100%. This weaker requirement enables us
to scan more space in less time, compared to complete coverage algorithms. Moreover the shape of the robot
formation adjusts itself to situations where obstacles, narrow spaces, etc. have to be passed. Communication
between the robots is restricted to line-of-sight and to a maximum interdistance between robots. A direct
application of the algorithm is mine field clearance.
1 INTRODUCTION region and sends its findings to a central device which

combines the data received from the robots into one
The research domain of multi-agent robot systems global map of the area.
can be divided into subdomains according to the task Closely related to the exploring/mapping task is
given to the robot group (Ota, 2006). At present well- the second task, called complete coverage, where the
studied subdomains are motion-planning (also called robots have to move over all of the free surface in
path-planning), formation-forming, region-sweeping, configuration space. Typical applications are mine
and combinations of the foregoing. The problem con- field clearance, lawn mowing and snow cleaning. The
sidered in the present paper belongs to the discipline coverage problem has been addressed in the litera-
comprising region-sweeping. In this discipline two ture both in a deterministic and a probabilistic setting.
different robot tasks are usually considered. In the probabilistic approach the robots are consid-
In the first task a group of robots receives the order ered as if they were fluid or gas molecules satisfying
to explore/map an unknown region. The goal is to ob- the appropriate physical laws of motion (Kerr et al.,
tain a detailed topography of the desired area. A typ- 2004), (Keymeulen and Decuyper, 1994). Just as a
ical approach to tackle the above problem with mul- gas by diffusion fills an entire space, the robots will
tiple robots assumes unlimited communication (Bur- cover all free space when time tends to infinity. In
gard et al., 2005): since exploration algorithms are the remainder of the paper we focus on the determin-
already devised for a single robot it seems straight- istic setting. In this setting the robot group typically
forward to divide the area to be explored into disjunct forms (partial) formations to solve the task. Refer-
regions, each of which is assigned to a single robot. ence (Choset, 2001) gives a short overview of existing
The robots communicate to each other the area they techniques for multi-robot coverage problems. Dif-
have explored so that no part of the free space will be ferent approaches to the coverage problem are found
explored twice unnecessarily. At no point during the in (Cortés et al., 2004), (Kurabayashi et al., 1996),
task are the robots trying to form a fixed formation. (Wong and MacDonald, 2004), (Zheng et al., 2005)
Each robot explores a different part of the unknown (Rekleitis et al., 2004) and (Kong et al., 2006).
76
The problem statement of the present paper 2 AN ALGORITHM FOR

does not differ that much from the common explo- COMPLETE SENSOR
ration/mapping task and the complete coverage prob-
lem, but is rather a combination of both. It is required COVERAGE
that all of the free space is sensed by the robots, but
not necessarily physically covered. However, unlike 2.1 Setting
the common exploration case, the sensing of the area
does not have as goal to map the topography of the Consider a population of N identical robots. Each
free space and the location of the obstacles in it. Our robot is equipped with two types of sensors. One type
aim is to locate several unknown targets within the serves as a means to detect the goal targets to be found
free space. Moreover, similar to the complete cov- in the assigned area, e.g. landmines; the other type is
erage setting we demand a 100% certainty that all used to detect and locate other robots and obstacles
free space has been covered by the sensors at the in the neighborhood of the robot1 . Both sensors have
end of the exploration procedure, implying that all a maximum detection range st and sr respectively. It
targets have been found. Since the robots no longer is assumed that targets which come within the radius
have to cover all free space physically, the novel al- of the corresponding sensor area st or sr of the robot
gorithm will yield a time gain compared to complete are always detected, and that if they are located far-
coverage strategies. The developed algorithm is in- ther away than the distance st , sr they are never de-
spired by complete coverage strategies as (Rekleitis tected. The robot configuration allows limited com-
et al., 2004), (Kong et al., 2006). In these strategies munication. First, this is expressed by the maximum
it is assumed that the space to be explored does not detection range sr as described above. Second, line-
have a maze-like structure with many narrow corri- of-sight communication is assumed: two robots can
dors, but is an open space containing only convex only sense each other if they are sufficiently close to
obstacles sparsely spread throughout. In the present each other and if there is no obstacle located on the
paper we adopt these assumptions, postponing more straight line connecting both robots.
complex geometries to future work. Our algorithm is Two robots are called connected to each other
presented in Section 2 of the paper. A short compari- when they sense each other. Every robot is assigned
son between the algorithm presented here and that of an index number. The initial state of the robot con-
(Rekleitis et al., 2004) is given in Section 3. figuration is such that robot i is connected to robots
A specific application we have in mind is mine i − 1 and i + 1, ∀i ∈ {2, . . . , N − 1}. (Robot 1 is only
field clearance using chemical vapor microsensors connected to robot 2 and robot N is only connected
(Gage, 1995). Once a landmine is deployed, the en- to robot N − 1.) An example of such a configuration
vironment near the mine becomes contaminated with is depicted in Figure 1 for N = 6. The dashed circles
explosives derived from the charge contained in the
mine. The vapor microsensors are able to detect the
chemical vapor signature of the explosives emanating 1
5
from the landmines from a distance. This implies that
complete coverage algorithms may be too restrictive 3
6
with respect to the demining problem. Performing the 2
algorithm of the present paper, with the weaker re- 4
quirement of sensor coverage, will result in a gain of

time.
The algorithm can also be used in problems where Figure 1: Overlapping sensor areas in a possible robot con-
figuration.
a robot formation has to cross a terrain containing
sparsely spread obstacles. There is a natural trade-off
between coherence of the formation and avoidance of have radius st and signify the sensed area for goal tar-
the obstacles. The robot group is allowed to split in gets of each robot. It is assumed that
order to pass the obstacles, resulting in faster progress
sr
of the group across the terrain. The algorithm ensures < st < sr . (1)
that once the obstacle is passed, the robots regroup. 2
1 In practice the latter type consists of two distinct mini-
mally interfering IR-sensors: one sensing obstacles and the
other sensing robots. Since this is not relevant for the the-
oretical description of the algorithm, these sensors are con-
sidered as if they are one and the same.
77
The lower bound on st in (1) ensures that the areas and N are allocated the task to follow the boundaries
sensed for goal targets of neighboring robots partially of the strip at a constant distance at the constant ve-
overlap, as illustrated by Figure 1. locity v. They can be considered leaders of the robot
In a second step, we impose a preferred formation group. These two leader robots do not try to stay in
on the robot group as follows: each robot keeps a con- the preferred formation, i.e. the condition on the cor-
stant distance d < sr with its neighbors and observes responding angles αN , β1 is removed, and they do not
them at preferred angles with respect to its forward maintain a fixed interdistance d with their neighbors.
direction. With notation from Figure 2, these angles The remaining robots, however, still maintain the pre-
are defined by: ferred formation. When no obstacles are present in
( the strip, the robots scan the strip for goal targets in
π/4, i even, the above defined preferred (rigid) formation moving
αi =
3π/4, i odd, at a velocity v.
( (2)
−π/4, i even,
βi =
−3π/4, i odd.
Furthermore each robot is equipped with a compass.
Together with the above defined angles, the forward
direction of each robot (the same for all robots) is im-
posed at the initialization of the algorithm. The above
conditions imply a robot formation with zigzag shape,
as shown in Figure 3. Once the preferred formation is
attained the scanning algorithm is started.
i−1 i+1 Figure 3: A depiction of the algorithm. The arrows indicate

the constant velocity of both leader robots. The dashed lines
represent the strip boundaries.
αi βi
When an obstacle is encountered the algorithm
i aims to guide the robot group past it in a time-optimal
way. The leader robots start driving at a preset veloc-
Figure 2: Defining the angles of the preferred robot config-
ity v0 < v (see Section 3); the group is split into two
uration. subgroups in order to move around the obstacle. The
subgroups rejoin after passing the obstacle to resume
the preferred formation structure.
Assume for reasons of simplicity that the number
2.2 The Scanning Algorithm of robots N is even. Consider the situation where
robot m encounters an object on its path such that
Assume for simplicity that the area to be explored is it cannot stay in the preferred formation any longer.
a rectangular subset S of R2 . All obstacles contained More precisely, the sensors of robot m measure
in S are assumed disjoint and convex. Divide the set
S into parallel (scanning) strips of width ( N2 − 1)d. • an interdistance between the obstacle and the
This choice of the value of the width will be motivated robot smaller than a preset distance do < sr ,
later, in Section 2.4. Furthermore assume that N and d • the position of the obstacle at an angle with its
are such that all obstacles in S have a diameter smaller forward direction inside the interval (−γ, γ), with
than the width of a scanning strip. Fix the maximum γ a fixed value inside the interval (0, π4 ).
allowed diameter at ( N2 − 3)d. The main idea of the
algorithm is to let the group of robots sweep the area The presence of the obstacle is communicated to all
S strip after strip in a zigzag-like pattern. Clearly, the robots in the group. Each robot takes on a dif-
when there is a sufficient number of robots available ferent role such that two subgroups will be formed.
the set S can be regarded as one big strip, simplifying The robots with index i ∈ S1 := {2, . . . , N/2} now
the algorithm since no transitions between consequent follow the neighboring robot with corresponding in-
strips have to be performed. dex i − 1. Similarly, robots with index i ∈ S2 :=
In a first case we consider a strip where no objects {N/2 + 1, . . . , N − 1} follow the neighboring robot
are located on the boundary (see Figure 3). Robots 1 with index i + 1. More precisely, the robot with in-
78
Figure 4: A group of 10 robots passing an obstacle. Only robots 5, 6 and 7 apply wall-following around the obstacle during the
maneuver. This is shown in the second and third picture. The first picture shows the formation at the moment of encountering
the obstacle. The fourth picture presents the formation after having completely past the obstacle.
dex i tries to reach the following coordinates: Remark. It may occur that a robot cannot reach its
(
(xi−1 + d sin π4 , yi−1 + (−1)i d cos π4 ), if i ∈ S1 , desired position because it is located too far away
π i π from its present position. Then the robot simply rides
(xi+1 − d sin 4 d, yi+1 + (−1) d cos 4 ), if i ∈ S2 . towards the desired position at the maximum allowed
(3) velocity, trying to catch up.
These coordinates are considered with respect to a
right-handed (x, y)-frame with the y-axis parallel to
the strip boundary, and directed into the driving di- Remark. If the number of robots N ∗ in the
rection of the leader robots. Each robot still tries to (sub)group is not even, then the indices of the robots
stay in the preferred formation, but in order to do so where the robot group splits are ⌊N ∗ /2⌋ and ⌈N ∗ /2⌉,
only takes information of one neighbor into account. where ⌊.⌋ is the function giving the largest integer less
Moreover, the condition on the relative position be- than or equal to its argument, and similarly, ⌈.⌉ gives
tween the neighboring robots N/2 and N/2 + 1 is sus- the smallest integer greater than or equal to its argu-
pended, which will lead to the splitting of the robot ment.
group. Notice that indifferent of the robot that ob-
serves the obstacle first, the group will split between 2.3 Multiple Obstacles
robots N/2 and N/2 + 1. This choice is motivated in
Section 2.4. Suppose the robot group is already split into two sub-
Consider the situation for robot i where one of the groups and a robot in one of the subgroups encounters
following occurs: a second obstacle. The above obstacle avoidance al-
• The desired position (3) cannot be reached, gorithm can be made modular in order to solve this
problem. A group can only split if both robots at the
• The obstacle is blocking the straight path between extremities of the group are leader robots, similar to
the present position of robot i and its desired po- the initial configuration. Assume group S1 encoun-
sition, ters a second obstacle. Robot N/2 is then turned into
• Robot i does not detect its neighbor necessary to a leader robot. Instead of following a strip boundary it
determine its preferred position. is ordered to follow the edge of the first obstacle, un-
If this situation occurs, the robot receives the order to til it meets its neighbor N/2 + 1 or until group S1 has
follow the edge of the obstacle, keeping the obstacle past the second obstacle. In the latter case, robot N/2
on its right if i ∈ S1 , or its left if i ∈ S2 . This be- takes on its role as a follower of robot N/2 − 1 again,
havior is called wall-following. The robot continues in the former case it turns into a follower of N/2 + 1.
to wall-follow around the obstacle until none of the The group S1 is split into the middle and the algo-
above conditions is satisfied. After that, it assumes its rithm described in the previous section is performed
desired position again. If all robots have past the ob- with leader robots 1 and N/2. In order for each robot
stacle, each robot is again able to reach its desired po- to know which role to assume, it keeps track of how
sition in the preferred formation. In particular, robots many times its subgroup is split.
N/2 and N/2 + 1 will meet again in their desired rel- Clearly, the number of times this splitting can be
ative position. When this happens a signal is sent to repeated is limited. We require a subgroup to consist
all robots with the message that the group has past the of at least 3 robots: two leader robots on each side
obstacle. of the group, plus at least one robot in the middle at-
A simulation of the above described algorithm is tempting to follow both leaders while maintaining the
presented in Figure 4 with N = 10 and m = 6. formation structure. The middle robot ensures that the
79
discs of sensed area of the separate robots overlap for sensor range of each robot is limited and the robots
all time instants. operate in an unknown environment, the shape of
each obstacle is unknown. To guarantee that the area
2.4 Adaptation of the Basic Algorithm around the obstacle is fully covered by the sensors, we
have to supply a sufficient number of robots to both
Consider a worst case scenario as sketched in Fig- sides of the obstacle. For instance, when the shape
ure 5. The robot formation splits into two subgroups, of the obstacle in Figure 5 is known a priori, one can
and the group on the left hand side moves through decide to send more than half of the robots to the left
the gap between the obstacle and the left boundary of of the obstacle. Consider the case where the obstacle
the scanning strip. Once past the gap the robots in is reflected with respect to the vertical axis. In this
this subgroup have to spread out, since the distance case sending less than half of the robots to the right
between the obstacle and the left boundary increases would lead to uncovered areas or an error message in
and we want to sense all of free space between the the algorithm. With limited sensor information it is
boundary and the obstacle. The obstacle has such a not possible to discriminate between the situation of
shape that the robots have to spread out across almost Figure 5 and its reflected version. This leads us to
the entire width of the scanning strip before meet- always split the group into two equal parts.
ing a robot member of the right subgroup. The basic An alternative solution for this problem could be
algorithm is modified as follows. When robot N/2 a more intelligent algorithm where individual robots
(resp. N/2 + 1) encounters the obstacle, it is now pro- transmit more sensor data to the others in order to find
grammed to follow the obstacle’s edge until it meets possible bounds on the size of the encountered obsta-
its neighbor N/2 + 1 (resp. N/2). Additionally, it en- cle. If the obstacle is small, a better way to split is
sures that its neighbor N/2 − 1 (resp. N/2 + 1) stays right at the place where the obstacle is encountered.
in its detection range by sending a signal to the other The robots do not have to deviate much from the pre-
robots of its subgroup to increase the angle π/4 of (3). ferred formation, which decreases the probability on
This changes the desired position of each robot in the error messages from deforming the robot configura-
subgroup resulting in a stretching out of the group, tion. This idea may be incorporated in future work.
as far as necessary. The above modified algorithm
2.5 Obstacles Located on the Boundary
between Two Strips
Throughout the paper the obstacles are assumed to
have a convex shape, in order to avoid robot groups
getting stuck in a dead end. However, there is one
case of dead ends we cannot avoid by the above as-
sumption. A dead end can occur when an obstacle is
located on the boundary between two strips, as pre-
sented on the left hand side of Figure 6. Since the
robots have limited sensor information, they cannot
Figure 5: A depiction of the worst case scenario in the al- conclude a priori whether an encountered obstacle
gorithm. stretches out into a neighboring strip or not. We are
forced to let the algorithm run until a dead end is ob-
justifies our choice of initial formation and width of served.
the scanning strip. If we had naively chosen a value
(N − 1)d as the width of a scanning strip, the initial
preferred robot formation would be able to span this
entire distance, namely by forming a line with the an-
gles defined in Section 2.1 equal to αi = −βi = π/2.
However, one subgroup, consisting of only half of the
number of robots, would not be able to span this dis-
tance, resulting in either an error message from the al-
gorithm or in unscanned areas, if a situation described Figure 6: Two situations where an obstacle is located on
in Figure 5 was encountered. the boundary between strips. On the left hand side a dead
Closely related to this observation is the choice to end situation arises; on the right hand side one of the leader
split the robot group precisely in the middle. Since the robots guides the group around the obstacle.
80
Before tackling the dead end problem, let us treat 2.6 The Transition from One Strip to
the case presented on the right hand side of Figure 6, the Next
which does not lead to a dead end situation. Con-
sider an (x, y)-frame with the y-axis parallel to the
When the robot group reaches the end of a scanning
strip boundary, and directed into the driving direction
strip, it needs to be transported to the next strip. This
of the leader robots. When the leader robot encoun-
is done in a few easy steps. Consider the situation of
ters the obstacle, the algorithm assigns to this leader
Figure 7. First the right leader changes its behavior
a wall-following procedure around the obstacle. The
into that of a robot in the interior of the formation, i.e.
leader keeps the obstacle either on its right or left (de-
it tries to attain the desired formation. The left leader
pending on its position in the robot formation) while
moves (N/2 − 1)d units to the left perpendicular to
moving into the interior of the strip away from the
the strip boundary. The rightmost robot resumes its
strip boundary. As can be concluded from the picture,
leader role and all robots reverse their forward direc-
the y-coordinate of the leader increases while mov-
tion with respect to the desired direction in the previ-
ing around the obstacle. We wish to keep the veloc-
ous strip. Naturally, every time the end of a strip is
ity component of the leader robot parallel to the strip
reached, the roles of left and right leader alternate, so
boundary equal to v. Since the robot deviates from
that the robot group does not get trapped into a loop
its straight path parallel to the strip boundary, this im-
consisting of two strips.
plies it has to speed up. When the leader reaches the
strip boundary again, it switches back to the original
task of moving parallel to the boundary.
Now consider the left hand side of Figure 6. A
dead end is detected by the algorithm when two con-
ditions are satisfied:
• one of the leader robots cannot move into the de-
sired direction parallel to the strip boundary, be-
cause an obstacle is blocking the way.
• when the leader robot starts wall-following the
obstacle as described above, the value of its y-
coordinate decreases.
As soon as a dead end is observed by the leader robot,
it changes its behavior and stops its wall following al-
gorithm. Instead, it projects its corresponding strip
boundary (N/2 − 1)d/8 units outwards and resumes
the original scanning algorithm with respect to the
new boundary. If the extra width turns out to be insuf-
ficient to guide the robot subgroup around the obsta-
cle outside of the original scanning strip, the bound-
ary is projected a second (third,...) time. This way the
subgroup which was stuck in the dead end is guided
around the obstacle. When both subgroups reestab-
lish contact, the leader robot returns to the original Figure 7: The robot group moves from the end of a scanning
strip boundary. This behavior is faster and easier to strip to the start of the next strip.
implement than a turning-back scenario, where the
subgroup of robots which meets a dead end retraces
it steps to go around the obstacle inside the original
scanning strip. 3 COMPARISON WITH
COMPLETE COVERAGE
Remark. The above situation with a solid wall as ALGORITHMS
strip boundary, forcing a turning-back maneuver, is
precluded.
The algorithm presented in this paper is inspired by
the complete coverage algorithm presented in (Rek-
leitis et al., 2004), which is depicted in Figure 8. The
authors of (Rekleitis et al., 2004) propose the follow-
81
ing robot configuration: 2 leader robots, following ary, since the space occupied by the obstacle does not
the strip boundaries, and a number of interior robots, have to be covered. The robot group will proceed
traveling back and forth between the strip boundaries fastest when the shape of the obstacle is such that
physically covering all the free space. Contrary to the there is no space left for the robots to travel back and
algorithm proposed in the present paper, the leader forth between obstacle and strip boundary. Hence, de-
robots maintain line-of-sight contact between each pending on size and shape of the obstacle the robots
other. When an obstacle appears between the two advance with a speed between vmax /6 and vmax . Now,
leaders the line-of-sight contact is lost and the obsta- consider the algorithm of the present paper. Some in-
cle is detected. An appropriate control action is then terior robots perform wall-following around the ob-
taken by splitting the platoon and the algorithm is re- stacles. This implies their path is longer than the path
peated on both sides of the obstacle. The splitting of the leader robots. If the leader robots keep mov-
procedure includes the creation of two extra leader ing at the maximum allowed velocity, those interior
robots, as shown in Figure 8. Remark that the lead- robots will never again be able to reach their desired
ers are allowed to move ahead of the rest of the robot position inside the formation after the obstacle is past.
group and hence group coherence is not maintained Hence, when an obstacle is encountered the leaders
or desired, contrary to our approach. have to take on a velocity v0 which is smaller than
vmax . This velocity v0 is determined as follows. The
middle robots N/2 and N/2 + 1 transmit their posi-
tions via the other robots to their respective leader
robots. The leaders adjust their velocity v0 such that
the difference between their y-coordinate and the y-
coordinate of the corresponding robot N/2 or N/2 + 1
stays at all time within a prespecified bound. The
middle robots only slow down the group significantly
during the first and last stage of their obstacle follow-
ing, i.e. when moving away from or towards the strip
boundary without significantly advancing parallel to
Figure 8: A depiction of the complete coverage algorithm
by Rekleitis et al. it. As soon as there is enough free space ahead of the
middle robots, the subgroup is again allowed to move
parallel to the strip boundary with a speed close to
In the remainder of this section we will compare vmax .
speed of performance of the present algorithm with From the above observations the following is con-
the algorithm of (Rekleitis et al., 2004). In order to cluded. The robot group in the present algorithm
do so, realistic distance values are considered. Chem- slows down to pass an obstacle, but for most of the
ical vapor sensors detecting mines have a range st = time the speed will be close to vmax . The robot group
1.70 m. Obstacles and other robots can be detected by of the complete coverage algorithm speeds up when
laser based sensors with a range of sr = 3.3 m such passing an obstacle, but for most obstacles the algo-
that (1) is satisfied. Assume the robots themselves rithm still requires a robot group moving back and
possess a diameter of 0.3 m and set the fixed interdis- forth between the obstacle and the strip boundary.
tance d between neighboring robots in the preferred This implies that the increased speed will on average
formation equal to sr . With N the number of robots in be closer to vmax /6 than to vmax . Hence, in generic
the group, this yields a strip width of 1.65(N − 2) m. cases, the present algorithm performs faster than the
When no obstacles are encountered, the robots are complete coverage strategy even in the presence of
allowed to move at a preset maximum velocity vmax . obstacles.
In the algorithm of the present paper vmax is directed
parallel to the strip boundary, whereas the interior
robots in (Rekleitis et al., 2004) travel back and forth
inside the strip at vmax . It can be proven that for the 4 CONCLUSIONS
latter case with the values given above the speed of
progress parallel to the strip boundary is vmax /6. The present paper described a novel strategy for
In the presence of obstacles a comparison is more multi-robot exploration of an unknown environment
difficult. First consider the complete coverage algo- with guarantee of total sensor coverage. The algo-
rithm (Rekleitis et al., 2004). As can be concluded rithm we proposed is inspired by methods used for
from Figure 8, in the presence of an obstacle the complete coverage as described in (Rekleitis et al.,
robots will advance faster parallel to the strip bound- 2004). We took advantage of the fact that only com-
82
plete sensor coverage is required. We let the robots Wong, S. and MacDonald, B. (2004). Complete coverage
form a spring-like formation which scans the area in by mobile robots using slice decomposition based on
strips. In the presence of obstacles the formation is natural landmarks. In Proc. Eighth Pacific Rim Inter-
national conference on Artificial Intelligence. Lecture
deformed and split in two in order to circumvent the Notes in Artificial Intelligence., volume 3157, pages
obstacles and to adapt to the varying width of the free 683–692.
space.
Zheng, X., Jain, S., Koenig, S., and Kempe, D. (2005).
Multi-robot forest coverage. In Proceedings of the
IEEE International Conference on Intelligent Robots
ACKNOWLEDGEMENTS and Systems.
This paper presents research results of the Belgian

Programme on Interuniversity Attraction Poles, ini-
tiated by the Belgian Federal Science Policy Office.
The scientific responsibility rests with its authors.
REFERENCES
Burgard, W., Moors, M., Stachniss, C., and Schneider, F.
(2005). Coordinated multi-robot exploration. IEEE
Transactions on Robotics, 21(3):376–386.
Choset, H. (2001). Coverage for robotics – a survey of re-
cent results. Annals of Mathematics and Artificial In-
telligence, 31:113–126.
Cortés, J., Martı́nez, S., Karatas, T., and Bullo, F. (2004).
Coverage control for mobile sensing networks. IEEE
Transactions on Robotics and Automation, 20(2):243–
255.
Gage, D. (1995). Many-robots mcm search systems. In
Proceedings of Autonomous Vehicles in Mine Coun-
termeasures Symposium.
Kerr, W., Spears, D., Spears, W., and Thayer, D. (2004).
Two formal gas models for multi-agent sweeping and
obstacle avoidance. In Formal Approaches to Agent-
Based Systems, Third International Workshop, pages
111–130.
Keymeulen, D. and Decuyper, J. (1994). The fluid dynam-
ics applied to mobile robot motion: the stream field
method. In Proceedings of 1994 IEEE International
Conference on Robotics and Automation, pages 378–
385, Piscataway, NJ, USA.
Kong, C. S., Peng, N. A., and Rekleitis, I. (2006). Dis-
tributed coverage with multi-robot system. In Pro-
ceedings of 2006 IEEE International Conference on
Robotics and Automation, pages 2423–2429, Orlando,
Florida, USA.
Kurabayashi, D., Ota, J., Arai, T., and Yosada, E. (1996).
Cooperative sweeping by multiple robots. In Proc.
1996 IEEE International Conference on Robotics and
Automation.
Ota, J. (2006). Multi-agent robot systems as distributed au-
tonomous systems. Advanced engineering informat-
ics, 20:59 – 70.
Rekleitis, I., Lee-Shue, V., New, A. P., and Choset, H.
(2004). Limited communication, multi-robot team
based coverage. In Proc. 2004 IEEE International
Conference on Robotics and Automation.
83
CALIBRATION OF QUASI-ISOTROPIC PARALLEL
KINEMATIC MACHINES: ORTHOGLIDE
Anatoly Pashkevich, Roman Gomolitsky

Robotic Laboratory, Department of Control Systems
Belarusian State University of Informatics and Radioelectronics
6 P.Brovka St., Minsk 220027, Belarus
[email protected]
Philippe Wenger, Damien Chablat

Institut de Recherche en Communications et Cybernétique de Nantes
1, rue de la Noe B.P. 6597, 44321 Nantes Cedex 3, France
{ Philippe.Wenger, Damien.Chablat}@irccyn.ec-nantes.fr
Keywords: Parallel robots, calibration, parameter identification, error compensation.
Abstract: The paper proposes a novel approach for the geometrical model calibration of quasi-isotropic parallel
kinematic mechanisms of the Orthoglide family. It is based on the observations of the manipulator leg
parallelism during motions between the specific test postures and employs a low-cost measuring system
composed of standard comparator indicators attached to the universal magnetic stands. They are
sequentially used for measuring the deviation of the relevant leg location while the manipulator moves the
TCP along the Cartesian axes. Using the measured differences, the developed algorithm estimates the joint
offsets and the leg lengths that are treated as the most essential parameters. Validity of the proposed
calibration technique is confirmed by the experimental results.
1 INTRODUCTION efficient (contrary to the direct kinematics, which is

usually analytically unsolvable for the PKM)
Parallel kinematic machines (PKM) are commonly (Innocenti, 1995; Iurascu & Park, 2003; Jeong et al.,
claimed as appealing solutions in many industrial 2004, Huang et al., 2005). But the main difficulty
applications due to their inherent structural rigidity, with this technique is the full-pose measurement
good payload-to-weight ratio, high dynamic requirement, which is very hard to implement
capacities and high accuracy (Tlusty et al., 1999; accurately. Hence, a number of studies have been
Merlet, 2000; Wenger et al., 2001). However, while directed at using the subset of the pose measurement
the PKM usually exhibit a much better repeatability data, which however creates another problem, the
compared to serial mechanisms, they may not identifiability of the model parameters.
necessarily posses a better accuracy, which is Popular approaches in the parallel robot
limited by manufacturing/assembling errors in calibration deal with one-dimensional pose errors
numerous links and passive joints (Wang and using a double-ball-bar system or other measuring
Masory, 1993). Thus, the PKM accuracy highly devices, as well as imposing mechanical constraints
relies on the accurate kinematic model, which must on some elements of the manipulator (Daney, 2003).
be carefully tuned for each manipulator separately. However, in spite of hypothetical simplicity, it is
Similar to the serial manipulators, the PKM hard to implement in practice since an accurate extra
calibration techniques are based on the minimization mechanism is required to impose these constraints.
of a parameter-dependent error function, which Additionally, such methods reduce the workspace
incorporates residuals of the kinematic equations. size and consequently the identification efficiency.
For the parallel manipulators, the inverse kinematic Another category of the methods, the self- or
equations are considered computationally more autonomous calibration, is implemented by
84
minimizing the residuals between the computed and the calibration accuracy under the measurement
measured values of the active and/or redundant joint noise. Section 4 contains experimental results that
sensors. Adding extra sensors at the usually validate the proposed technique, while Section 5
unmeasured joints is very attractive from summarizes the main contributions.
computational point of view, since it allows getting
the data in the whole workspace and potentially
reduces impact of the measurement noise. However, 2 ORTHOGLIDE MECHANISM
only a partial set of the parameters may be identified
in this way, since the internal sensing is unable to
provide sufficient information for the robot end-
2.1 Manipulator Architecture
effector absolute location.
The Orthoglide is a three degrees-of-freedom
More recently, several hybrid calibration
parallel manipulator actuated by linear drives with
methods were proposed that utilize intrinsic
mutually orthogonal axes. Its kinematic architecture
properties of a particular parallel machine allowing
is presented in Figure 1 and includes three identical
extracting the full set of the model parameters (or
parallel chains, which will be further referred as
the most essential of them) from a minimum set of
“legs”. Each manipulator leg is formally described
measurements. It worth mentioning an innovative
as PRPaR - chain, where P, R and Pa denote the
approach developed by Renaud et al. (2004, 2005)
prismatic, revolute, and parallelogram joints
who applied the vision-based measurement system
respectively (Figure 2). The output machinery (with
for the PKM calibration from the leg observations.
a tool mounting flange) is connected to the legs in
In this technique, the source data are extracted from
such a manner that the tool moves in the Cartesian
the leg images, without any strict assumptions on the
space with fixed orientation (translational motions).
end-effector poses. The only assumption is related to
the manipulator architecture (the mechanism is
actuated by linear drives located on the base).
However, current accuracy of the camera-based
measurements is not high enough yet to apply this
method in industrial environment.
This paper extends our previous research
(Pashkevich et al., 2006) and focuses on the
calibration of the Orthoglide-type mechanisms,
which is also actuated by linear drives located on the
manipulator base and admits technique of Renaud et
al. (2004, 2005). But, in contrast to the known
works, our approach assumes that the leg location is
observed for specific manipulator postures, when the
tool-center-point moves along the Cartesian axes. Figure 1: The Orthoglide kinematic architecture.
For these postures and for the nominal Orthoglide (© CNRS Photothèque / CARLSON Leif)
geometry, the legs are strictly parallel to the
corresponding Cartesian planes. So, the deviation of The Orthoglide workspace has a regular, quasi-
the manipulator parameters influences on the leg cubic shape. The input/output equations are simple
parallelism that gives the source data for the and the velocity transmission factors are equal to
parameter identification. The main advantage of this one along the x, y and z direction at the isotropic
approach is the simplicity and low cost of the configuration, like in a conventional serial PPP
measuring system that can avoid using computer machine (Wenger et al., 2000; Chablat and Wenger,
vision and is composed of standard comparator 2003). The latter is an essential advantage of the
indicators attached to the universal magnetic stands. Orthoglide architecture, which also allows referring
The remainder of the paper is organized as it as the “quasi-isotropic” kinematic machine.
follows. Section 2 describes the manipulator Another specific feature of the Orthoglide
geometry, its inverse and direct kinematics, and also mechanism, which will be further used for the
contains the sensitivity analysis of the leg calibration, is displayed during the end-effector
parallelism at the examined postures with respect to motions along the Cartesian axes. For example, for
the geometrical parameters. Section 3 focuses on the the x-axis motion in the Cartesian space, the sides of
parameter identification, with particular emphasis on the x-leg parallelogram must also retain strictly
parallel to the x-axis. Hence, the observed deviation
85
of the mentioned parallelism may be used as the data 2.3 Basic Equations
source for the calibration algorithms.
Since the kinematic parallelograms are admitted to
Ai be non-identical, the kinematic model developed in
in our previous papers (Pashkevich et al., 2005,
ρi 2006) should be extended to describe the
ii
manipulator with different length of the legs.
Under the adopted assumptions, similar to the
Bi equal-leg case, the articulated parallelograms may be
ji replaced by the kinematically equivalent bar links.
Besides, a simple transformation of the Cartesian
Li
d coordinates (shift by the vector (r, r, r)T, see Figure
Ci
2) allows to eliminate the tool offset. Hence, the
Orthoglide geometry can be described by a
r simplified model, which consists of three rigid links
connected by spherical joints to the tool centre point
P (TCP) at one side and to the allied prismatic joints at
Figure 2: Kinematics of the Orthoglide leg. another side. Corresponding formal definition of
each leg can be presented as PSS, where P and S
For a small-scale Orthoglide prototype used for denote the actuated prismatic joint and the passive
the calibration experiments, the workspace size is spherical joint respectively.
approximately equal to 200×200×200 mm3 with the Thus, if the origin of a reference frame is located
velocity transmission factors bounded between 1/2 at the intersection of the prismatic joint axes and the
and 2 (Chablat & Wenger, 2003). The legs nominal x, y, z-axes are directed along them, the manipulator
geometry is defined by the following parameters: kinematics may be described by the following
Li = 310.25 mm, d = 80 mm, r = 31 mm where Li, d equations
are the parallelogram length and width, and r is the
distance between the points Ci and the tool centre ⎡( ρ x + Δρ x ) + cosθ x cos β x Lx + e⎤
⎢ ⎥
point P (see Figure 2). p=⎢ sin θ x cos β x Lx ⎥; (1a)
⎢⎣ − sin β x Lx ⎥⎦
2.2 Modelling Assumptions
⎡ − sin β y L y ⎤
Following previous studies on the PKM accuracy ⎢ ⎥
(Wang & Massory, 1993; Renaud et al., 2004), the p = ⎢( ρ y + Δρ y ) + cosθ y cos β y L y + e⎥ ; (1b)
⎢ ⎥
influence of the joint defects is assumed negligible
⎣⎢ sin θ y cos β y L y ⎥⎦
compared to the encoder offsets and the link length
deviations. This validates the following modelling ⎡ sin θ z cos β z Lz ⎤
assumptions: ⎢ ⎥
p=⎢ − sin β z Lz ⎥, (1c)
(i) the manipulator parts are supposed to be rigid
bodies connected by perfect joints; ⎢⎣( ρ z + Δρ z ) + cosθ z cos β z Lz + e⎥⎦
(ii) the manipulator legs (composed of a prismatic where p = (px, py, pz)T is the output vector of the TCP
joint, a parallelogram, and two revolute joints) position, ρ = (ρx, ρy, ρz)T is the input vector of the
generate a four degrees-of-freedom motions; prismatic joints variables, Δρ = (Δρx, Δρy, Δρz)T is
(iii) the articulated parallelograms are assumed to the encoder offset vector, θi, βi, i∈{x, y, z} are the
be perfect but non-identical; parallelogram orientation angles (internal variables),
(iv) the linear actuator axes are mutually orthogonal and Li are the length of the corresponding leg.
and intersected in a single point to insure a After elimination of the internal variables θi , βi ,
translational movement of the end-effector; the kinematic model (1) can be reduced to three
(v) the actuator encoders are perfect but located equations
with some errors (offsets).
( pi − ( ρ i + Δρ i ) )2 + p 2j + p k2 = L2i , (2)
Using these assumptions, there will be derived
new calibration equations based on the observation which includes components of the input and output
of the parallel motions of the manipulator legs. vectors p and ρ only. Here, the subscripts
86
z
i, j , k ∈ {x, y, z} , i ≠ j ≠ k are used in all (a) : XMax posture
combinations, and the joint variables ρi are obeyed ρz = L cosα
the prescribed limits ρ min < ρ i < ρ max defined in
the control software (for the Orthoglide prototype,
ρmin = -100 mm and ρmax = +60 mm). α
It should be noted that, for the case O
p α y
Δρ x = Δρ y = Δρ z = 0 and Lx = L y = Lz = L , the ρy = L cosα
nominal ‘‘mechanical-zero’’ posture of the
manipulator corresponds to the Cartesian
x ρx = L + L sinα
coordinates p0 = (0, 0, 0)T and to the joints variables
ρ0 = (L, L, L). Moreover, in such posture, the x-, y-
and z-legs are oriented strictly parallel to the z
(b) : Zero posture
corresponding Cartesian axes. But the joint offsets
and the leg length differences cause the deviation of ρz = L
the “zero” TCP location and corresponding
deviation of the leg parallelism, which may be
measured and used for the calibration. p
Hence, six parameters (Δρx, Δρy, Δρz , Lx, Ly, Lz) O
y
define the manipulator geometry and are in the focus ρy = L
of the proposed calibration technique.
ρx = L
2.4 Inverse and Direct Kinematics x
The inverse kinematic relations are derived from the z

(c) : XMin posture
equations (2) in a straightforward way and only
slightly differ from the “nominal” case: ρz = L cosα
ρ i = pi + s i L2i − p 2j − pk2 − Δρ i , (3)

p α
where sx, sy, sz ∈{ ±1} are the configuration indices ρy = L cosα
O α
defined for the “nominal” geometry as the signs of y
ρx – px , ρy – py, ρz – pz, respectively. It is obvious
that expressions (3) give eight different solutions, ρx = L - L sinα
x
however the Orthoglide prototype assembling mode
and the joint limits reduce this set to a single case
corresponding to the sx = sy = sz = 1. Figure 3: Specific postures of the Orthoglide (for the x-leg
For the direct kinematics, equations (2) can be motion along the Cartesian axis X).
subtracted pair-to-pair that gives linear relations
between the unknowns px, py, pz, which may be
expressed in the parametric form as B= ∏ ( ρ + Δρ )
i
i i
2
− ∑ L (ρ
i≠ j≠k
2
i j+ Δρ j ) 2 ( ρ k + Δρ k ) 2 ;
ρ i + Δρ i t L2i
pi = + − , ⎛ ⎞
∏ (ρ + Δρ ) ⋅ ⎜⎜⎝ ∑ ( ρ + Δρ ) ∑L
(4)
2 ρ i + Δρ i 2( ρ i + Δρ i ) C= 2 2
/4 − 2
2⎟+
i i i i i ⎟
where t is an auxiliary scalar variable. This reduces
i i i ⎠
the direct kinematics to the solution of a quadratic + ∑
i≠ j≠k
L4i ( ρ j + Δρ j ) ( ρ k + Δρ k ) / 4
2 2
equation At 2 + Bt + C = 0 with the coefficients

where i, j , k ∈ {x, y, z} . From two possible solutions
A= ∑ (ρ + Δρ ) (ρ
i≠ j
i i
2
j + Δρ j )
2
;
that gives the quadratic formula, the Orthoglide
prototype (see Figure 1) admit a single one
t = ( − B + B 2 − 4 AC ) / 2 A corresponding to the
manipulator assembling mode.
87
2.5 Differential Relations It should be noted that, for the computing

convenience, the above expression includes both the
To obtain the calibration equations, first let us derive Cartesian coordinates px, py, pz and the joint
the differential relations for the TCP deviation for coordinates ρx, ρy, ρz, but only one of these sets may
three types of the Orthoglide postures: be treated as an independent taking into account the
(i) “maximum displacement” postures for the inverse/direct kinematic relations.
directions x, y, z (Figure 3a); For the “Zero” posture, the differential relations
(ii) “mechanical zero” or the isotropic posture are derived in the neighbourhood of the point
(Figure 3b); {p0 = (0, 0, 0) ; ρ0 = (L, L, L)}, which after
(iii) “minimum displacement” postures for the substitution to (5) gives the Jacobian matrix
directions x, y, z (Figure 3c);
These postures are of particular interest for the ⎡ 1 0 0 −1 0 0 ⎤
calibration since, in the “nominal” case, a J 0 = ⎢⎢ 0 1 0 0 −1 0 ⎥⎥ . (6)
corresponding leg is parallel to the relevant pair of
⎢⎣ 0 0 1 0 0 − 1 ⎥⎦
the Cartesian planes.
The manipulator Jacobian with respect to the Hence, in this case, the TCP displacement is related
parameters Δρ =(Δρx, Δρy, Δρz ) and L = ( Lx, Ly, Lz) to the joint offsets and the leg legs variations ΔLi by
can be derived by straightforward differentiating of trivial equations
the kinematic equations (2), which yields
Δpi = Δρ i − ΔLi ; i ∈{x, y, z} . (7)
⎡ px − ρx p y pz ⎤ ⎡ px − ρx 0 0 ⎤ For the “XMax” posture, the Jacobian is
⎢ ⎥ ∂p ⎢ ⎥
⎢ p x p y − ρ y p z ⎥ ⋅ ∂ρ = ⎢ 0 p y − ρ y 0 ⎥ computed in the neighbourhood of the point
⎢ px p y p z − ρ z ⎥⎦ ⎢⎣ 0 0 p z − ρ z ⎥⎦ { p = ( LSα , 0, 0) ; ρ = ( L+ LSα , LCα , LCα ) }, where
⎣
⎡ px − ρx p y
α is the angle between the y-, z-legs and the X-
pz ⎤ ⎡ Lx 0 0 ⎤
⎢ ⎥ ∂p ⎢ ⎥ axes: α = a sin( ρ max / L) ; Sα = sin (α ) , Cα = cos(α ) .
⎢ p x p y − ρ y p z ⎥ ⋅ ∂L = ⎢ 0 L y 0 ⎥ . This gives the Jacobian
⎢ px p y p z − ρ z ⎥⎦ ⎢⎣ 0 0 Lz ⎥⎦
⎣
⎡ 1 0 0 −1 0 0 ⎤
⎢ ⎥
Thus, after the matrix inversions and J +x = ⎢ Tα 1 0 − Tα − Cα −1
0 ⎥, (8)
multiplications, the desired Jacobian can be written ⎢ T 0 1 − Tα 0 − Cα−1 ⎥
as ⎣ α ⎦
where Tα = tan(α ) . Hence, the desired equations
[
J (p, ρ) = J ρ (p, ρ); J L (p, ρ) ,] (5) for the TCP displacement may be written as
where Δp x = Δρ x − ΔL x
−1
⎡ py pz ⎤ Δp y = Tα Δρ x + Δρ y − Tα ΔL x − Cα−1 ΔL y (9)
⎢ 1 ⎥
⎢ px − ρx px − ρx ⎥ Δp z = Tα Δρ x + Δρ z − Tα ΔL x − Cα−1 ΔL z
⎢ px pz ⎥
J ρ (.) = ⎢ 1 ⎥ It can be proved that similar results are valid for the
⎢ py− ρy py− ρy ⎥ “YMax” and “ZMax” postures (differing by the indices
⎢ px py ⎥ only), and also for the “XMin”, “YMin”, “ZMin” postures.
⎢ 1 ⎥ In the latter case, the angle α should be computed
⎢⎣ p z − ρ z pz − ρz ⎥⎦
as α = a sin( ρ min / L) .
−1
⎡ px − ρx py pz ⎤
⎢ ⎥
⎢ Lx Lx Lx ⎥
⎢ px py− ρy pz ⎥ 3 CALIBRATION METHOD
J L (.) = ⎢ ⎥
⎢ Ly Ly Ly ⎥
⎢ px py pz − ρz ⎥ 3.1 Measurement Technique
⎢ ⎥
⎣⎢ Lz Lz Lz ⎦⎥ To evaluate the leg/surface parallelism, we propose
a single-sensor measurement technique. It is based
88
on the fixed location of the measuring device for two ⎛ L − ΔL x Δρ y − ΔL y Δρ z − ΔL z ⎞

distinct leg postures corresponding to the g 0x = ⎜⎜ + Δρ x ; ; ⎟
⎟
minimum/maximum values of the joint coordinates ⎝ 2 2 2 ⎠
(Figure 4). Relevant calibration experiment consists
of the following steps: ⎛ Δρ x − ΔL x L − ΔL y Δρ z − ΔL z ⎞
Step 1. Move the manipulator to the “Zero” g 0y = ⎜⎜ ; + Δρ y ; ⎟
⎟
posture; locate two gauges in the middle of the ⎝ 2 2 2 ⎠
X-leg (parallel to the axes Y and Z); get their
readings. ⎛ Δρ x − ΔL x Δρ y − ΔL y L − ΔL z ⎞
g 0z = ⎜⎜ ; ; + Δρ z ⎟⎟
Step 2. Move the manipulator to the “XMax” and ⎝ 2 2 2 ⎠
“XMin” postures, get the gauge readings, and
compute differences.
Afterwards, in the “XMax”, “YMax”, “ZMax”
Step 3+. Repeat steps 1, 2 for the Y- and Z-legs
postures, the leg location is also defined by two
and compute corresponding differences.
points, namely, (i) the TCP, and (ii) the centre of the
prismatic joint ri. For example, for the “XMax”
Posture #1 posture, the TCP position is
Manipulator legs
x = ( LSα +Δρ x − ΔL x ; ∗; ∗) ,
p max
and the joint position is

d1 rxmax = ( L+ LSα +Δρ x ; 0; 0) .
So, the leg is located along the line
Base plane
s x ( μ ) = μ ⋅ p max
x + (1 − μ ) ⋅ rx ,
max
Posture #2 where μ is a scalar parameter, μ∈[0, 1]. Since the x-

Manipulator legs coordinate of the gauge is independent of the
posture, the parameter μ may be obtained from the
equation [s x ( μ )] x = [g 0x ] x , which solution yields:
Δ = d2 - d1
d2 μ = 0.5 + Sα − Sα ⋅ ΔL x / L ,
Hence, after some transformations, the deviations of

the X-leg measurements (between the “XMax” and
Base plane
“Zero” postures) may be expressed as
Figure 4: Measuring the leg/surface parallelism. Δy x+ = (0.5 + Sα )Tα Δρ x + Sα Δρ y −
3.2 Calibration Equations − (0.5 + Sα )Tα ΔL x − ((0.5 + Sα )Cα− 1 − 0.5)ΔL y
Δz x+ = (0.5 + Sα )Tα Δρ x + Sα Δρ z −
The system of calibration equations can be derived
in two steps. First, it is required to define the gauge
− (0.5 + Sα )Tα ΔL x − ((0.5 + Sα )Cα− 1 − 0.5)ΔLz
initial locations that are assumed to be positioned at
the leg middle at the “Zero” posture, i.e. at the points
(p + ri ) / 2 , i ∈ {x, y, z} where the vectors ri define
the prismatic joints centres: rx = ( L + Δρ x ; 0; 0) ; Similar approach may be applied to the “XMin”
r y = (0; L + Δρ y ; 0) ; rz = (0; 0; L + Δρ z ) . posture, as well as to the corresponding postures for
the Y- and Z-legs. This gives the system of twelve
Hence, using the equation (7), the gauge initial linear equations in six unknowns:
locations can be expressed as
89
⎡Δxy+ ⎤ for the parameter identification; and (#3) verification

⎡a1 b1 0 −c1 −b1 0 ⎤
of the calibration results.
⎢ +⎥
⎢b a 0 −b −c 0 ⎥ ⎢Δyx ⎥
⎢1 1 1 1 ⎥ ⎢ −⎥
⎢a2 b2 0 −c2 −b2 0 ⎥ ⎢Δxy ⎥
⎢ ⎥ ⎢Δy− ⎥
⎢b2 a2 0 −b2 −c2 0 ⎥ ⎡Δρx ⎤ ⎢ x ⎥
⎢0 a b 0 −c −b ⎥ ⎢Δρ ⎥ ⎢Δy+ ⎥
⎢ 1 1 1 1
⎥⎢ y⎥ ⎢ z ⎥
⎢ 0 b1 a1 0 −b1 −c1 ⎥ ⎢Δρz ⎥ ⎢Δzy+ ⎥
⎢ ⎥⎢ ⎥ = ⎢ ⎥ (10)
⎢ 0 a2 b2 0 −c2 −b2 ⎥ ⎢ΔLx ⎥ ⎢Δyz ⎥
−
⎢ 0 b2 a2 0 −b2 −c2 ⎥ ⎢ΔLy ⎥ ⎢ − ⎥

⎢ ⎥⎢ ⎥ ⎢Δzy ⎥
⎢a1 0 b1 −c1 0 −b1 ⎥ ⎣ΔLz ⎥⎦ ⎢ + ⎥
⎢
⎢ ⎥ ⎢Δxz ⎥
⎢ b1 0 a1 −b 1 0 −c1 ⎥ ⎢Δz+ ⎥
Figure 5: Experimental Setup.
⎢a2 0 b2 −c2 0 −b2 ⎥ ⎢ x⎥ Table 1: Calibration results.

⎢ ⎥ ⎢Δxz− ⎥
⎣⎢ 2
b 0 a 2 − b 2 0 − c ⎥
2 ⎦ ⎢ −⎥ Parameters (mm)
⎣⎢Δzx ⎦⎥
R.m.s.
Δρx Δρy Δρz ΔLx ΔLy ΔLz (mm)
where 4.66 -5.36 1.46 5.20 -5.96 3.16 0.12
ai = S αi , bi = (0.5 + S αi )Tαi , ci = (0.5 + Sα i )Cα−i1 − 0.5
-0.48 0.49 -1.67 – – – 0.14
and α1 = asin (ρ max / L) > 0 , α 2 = asin (ρ min / L) < 0 .
– – – 0.50 -0.52 1.69 0.14
This system can be solved using the
pseudoinverse of Moore-Penrose, which ensures the
minimum of the residual square sum. The first experiment produced rather high
parallelism deviation, which impels to conclude that
the mechanism mechanics requires more careful
tuning. Consequently, the location of the joint axes
4 EXPERIMENTAL RESULTS was adjusted mechanically to ensure the leg
parallelism for the Zero posture.
The measuring system is composed of standard The second experiment (after mechanical tuning)
comparator indicators attached to the universal yielded lower deviations, twice better than for the
magnetic stands allowing fixing them on the first experiment. For these data, the developed
manipulator bases. The indicators have resolution of calibration algorithm was applied for three sets of
10 μm and are sequentially used for measuring the
X-, Y-, and Z-leg parallelism while the manipulator the model parameters: for the full set {Δρ, ΔL} and
moves between the Max, Min and Zero postures. For for the reduced sets {Δρ} and {ΔL}. As follows
each measurement, the indicators are located on the from the identification results (Table 1), the
mechanism base in such manner that a algorithms is able to identify simultaneously both
corresponding leg is admissible for the gauge the joint offsets and Δρ and the link lengths ΔL.
contact for all intermediate postures (Figure 5). However, both Δρ and ΔL (separately) demonstrate
For each leg, the measurements were repeated roughly the same influence on the residual
three times for the following sequence of motions: reduction, from 0.32 mm to 0.14 mm, while the full
Zero → Max → Min → Zero→ …. Then, the results set {Δρ, ΔL} gives further residual reduction to the
were averaged and used for the parameter 0.12 mm only. This motivates considering Δρ as the
identification (the repeatability of the measurements most essential parameters to be calibrated.
is about 0.02 mm). Accordingly, the identified vales of the joint offsets
To validate the developed calibration technique were input into the control software.
and the adopted modelling assumptions, there were
The third experiment demonstrated good
carried out three experiments targeted to the
agreement with the expected results. In particular,
following objectives: (#1) validation of modelling
assumptions; (#2) collecting the experimental data the average deviation reduced down to 0.15 mm,
90
which corresponds to the measurement accuracy. On Innocenti, C., 1995. Algorithms for kinematic calibration
the other hand, further adjusting of the model to the of fully-parallel manipulators. In: Computational
new experimental data does not give the residual Kinematics, Kluwer Academic Publishers, pp. 241-
reduction. 250.
Iurascu, C.C. Park, F.C., 2003. Geometric algorithm for
Hence, the calibration results confirm validity of kinematic calibration of robots containing closed
the proposed identification technique and its ability loops. ASME Journal of Mechanical Design, Vol.
to tune the joint offsets and link lengths from 125(1), pp. 23-32.
observations of the leg parallelism. Other conclusion Jeong J., Kang, D., Cho, Y.M., Kim, J., 2004. Kinematic
is related to the modelling assumption: for further calibration of redundantly actuated parallel
accuracy improvement it is prudent to generalize the mechanisms. ASME Journal of Mechanical Design,
manipulator model by including parameters Vol. 126 (2), pp. 307-318.
describing the orientation of the prismatic joint axes, Merlet, J.-P., 2000. Parallel Robots, Kluwer Academic
i.e. relaxing assumption (iv) (see sub-section 2.2). Publishers, Dordrecht, 2000.
Pashkevich, A., Wenger, P., Chablat, D., 2005. Design
Strategies for the Geometric Synthesis of Orthoglide-
type Mechanisms. Mechanism and Machine Theory,
5 CONCLUSIONS Vol. 40 (8), pp. 907-930.
Pashkevich A., Chablat D., Wenger P., 2006. Kinematic
Calibration of Orthoglide-Type Mechanisms.
This paper proposes further developments for a
Proceedings of IFAC Symposium on Information
novel calibration approach for parallel manipulators,
Control Problems in Manufacturing (INCOM’2006),
which is based on observations of manipulator leg Saint Etienne, France, 17-19 May, 2006, p. 151 - 156
parallelism with respect to some predefined planes. Renaud, P., Andreff, N., Pierrot, F., Martinet, P., 2004.
This technique employs a simple and low-cost Combining end-effector and legs observation for
measuring system composed of standard comparator kinematic calibration of parallel mechanisms. IEEE
indicators, which are sequentially used for International Conference on Robotics and Automation
measuring the deviation of the relevant leg location (ICRA’2004), New-Orleans, USA, pp. 4116-4121.
while the manipulator moves the TCP along the Renaud, P., Andreff, N., Martinet, P., Gogu, G., 2005.
Cartesian axes. From the measured differences, the Kinematic calibration of parallel mechanisms: a novel
calibration algorithm estimates the joint offsets and approach using legs observation. IEEE Transactions
the link lengths that are treated as the most essential on Robotics and Automation, 21 (4), pp. 529-538.
parameters to be tuned. The validity of the proposed Tlusty, J., Ziegert, J.C., Ridgeway, S., 1999. Fundamental
approach and efficiency of the developed numerical Comparison of the Use of Serial and Parallel
algorithm were confirmed by the calibration Kinematics for Machine Tools. CIRP Annals, Vol. 48
experiments with the Orthoglide prototype, which (1), pp. 351-356.
allowed essential reduction of the residuals and Wang, J. Masory, O. 1993. On the accuracy of a Stewart
platform - Part I: The effect of manufacturing
corresponding improvement of the accuracy.
tolerances. IEEE International Conference on
Future work will focus on the expanding the set of Robotics and Automation (ICRA’93), Atlanta,
the identified model parameters, their identifiably Georgia, pp. 114–120.
analysis, and compensation of the non-geometric Wenger, P., Gosselin, C., Chablat, D., 2001. Comparative
errors. study of parallel kinematic architectures for machining
applications. In: Workshop on Computational
Kinematics, Seoul, Korea, pp. 249-258.
REFERENCES Wenger, P., Gosselin, C. Maille, B., 1999. A comparative
study of serial and parallel mechanism topologies for
machine tools. In: Proceedings of PKM’99, Milan,
Chablat, D., Wenger, Ph., 2003. Architecture Optimization Italy, pp. 23–32.
of a 3-DOF Parallel Mechanism for Machining
Applications, the Orthoglide. IEEE Transactions On
Robotics and Automation, Vol. 19 (3), pp. 403-410.
Daney, D., 2003. Kinematic Calibration of the Gough
platform. Robotica, 21(6), pp. 677-690.
Huang, T., Chetwynd, D.G. Whitehouse, D.J., Wang, J.,
2005. A general and novel approach for parameter
identification of 6-dof parallel kinematic machines.
Mechanism and Machine Theory, Vol. 40 (2), pp. 219-
239.
91
Peter Sapaty
Institute of Mathematical Machines & Systems, National Academy of Sciences
Glushkova Ave 42, Kiev 03187, Ukraine
[email protected]
Masanori Sugisaka
Department of Electrical and Electronic Engineering, Oita University
700 Oaza Dannoharu 870-1192 Japan
Tel: 097-554-7831, Fax: 097-554-7841
[email protected]
Joaquim Filipe
Departamento Sistemas e Informática, Escola Superior de Tecnologia de Setúbal
Setúbal 2910-761, Portugal
[email protected]
Keywords: Sensor networks, intelligent management, distributed scenario language, distributed interpreter, tracking
objects, hierarchical data fusion.
Abstract: A universal solution for management of dynamic sensor networks will be presented, covering both
networking and application layers. A network of intelligent modules, overlaying the sensor network,
collectively interprets mission scenarios in a special high-level language that can start from any nodes and
cover the network at runtime. The spreading scenarios are extremely compact, which may be useful for
energy saving communications. The code will be exhibited for distributed collection and fusion of sensor
data, also for tracking mobile targets by scattered and communicating sensors.
1 INTRODUCTION pollutants, at different locations. WSN, however,

have many additional problems in comparison to the
Sensor networks are a sensing, computing and wired ones. The individual devices in WSN are
communication infrastructure that allows us to inherently resource constrained--they have limited
instrument, observe, and respond to phenomena in processing speed, storage capacity, and
the natural environment, and in our physical and communication bandwidth. These devices have
cyber infrastructure (Culler at al., 2004; Chong, substantial processing capability in the aggregate,
Kumar, 2003). The sensors themselves can range but not individually, so we must combine their many
from small passive microsensors to larger scale, vantage points on the physical phenomena within the
controllable platforms. Their computation and network itself. In addition to one or more sensors,
communication infrastructure will be radically each node in a sensor network is typically equipped
different from that found in today's Internet-based with a radio transceiver or other wireless
systems, reflecting the device- and application- communications device, a small microcontroller, and
driven nature of these systems. an energy source, usually a battery. The size of a
Of particular interest are wireless sensor single sensor node can vary from shoebox-sized
networks, WSN (Wireless; Zhao, Guibas, 2004) nodes down to devices the size of grain of dust.
consisting of spatially distributed autonomous Typical applications of WSNs include
devices using sensors to cooperatively monitor monitoring, tracking, and controlling. Some of the
physical or environmental conditions, such as specific applications are habitat monitoring, object
temperature, sound, vibration, pressure, motion or tracking, nuclear reactor controlling, fire detection,
92
traffic monitoring, etc. In a typical application, a imaginable (or even so far unimaginable) distributed
WSN is scattered in a region where it is meant to problems can be solved by dynamic self-organized
collect data through its sensor nodes. They could be sensor networks if to increase their intelligence as a
deployed in wilderness areas, where they would whole, with a novel distributed processing and
remain for many years (monitoring some control ideology and technology effectively
environmental variable) without the need to operating in computer networks.
recharge/replace their power supplies. They could
form a perimeter about a property and monitor the
progression of intruders (passing information from 2 THE DISTRIBUTED
one node to the next). At present, there are many
uses for WSNs throughout the world. MANAGEMENT MODEL
In a wired network like the Internet, each router
connects to a specific set of other routers, forming a The distributed information technology we are using
routing graph. In WSNs, each node has a radio that here is based on a special Distributed Scenario
provides a set of communication links to nearby Language (DSL) describing parallel solutions in
nodes. By exchanging information, nodes can computer networks as a seamless spatial process
discover their neighbors and perform a distributed rather than the traditional collection and interaction
algorithm to determine how to route data according of parts (agents). Parallel scenarios in DSL can start
to the application’s needs. Although physical from any interpreter of the language, spreading and
placement primarily determines connectivity, covering the distributed space at runtime, as in Fig. 2.
variables such as obstructions, interference,
environmental factors, antenna orientation, and Hierarchical
echoing and
mobility make determining connectivity a priori control
difficult. Instead, the network discovers and adapts
Spreading
to whatever connectivity is present. activities
Fig. 1 shows what we will mean as a sensor
network for the rest of this paper. Start
Spreading
activities
Transmitter
Transmitter
S
S Advances in space
Sensor S
S Figure 2: Runtime coverage of space by parallel scenarios.
S Sensor
S The overall management of the evolving scenarios is
accomplished via the distributed track system
providing hierarchical command and control for
S S
S Sensor scenario execution, with a variety of special echo
Local communication messages. We will mention here only key features of
capabilities DSL, as the current language details can be found
elsewhere (Sapaty et al., 2007), also its basics from
Figure 1: Distributed sensors and their emergent network. the previous versions (Sapaty, 1999, 2005; Sapaty et
al., 2006).
It will hypothetically consist of (a great number of) A DSL program, or wave, is represented as one
usual sensors with local communication capabilities, or more constructs called moves (separated by a
and (a limited number of) those that can additionally comma) embraced by a rule, as follows:
transmit collected information outside the area (say,
via satellite channels). Individual sensors can be on a wave → rule ({ move , })
move, some may be destroyed while others added at
runtime (say, dropped from the air) to join the Rules may serve as various supervisory, regulatory,
existing ones in solving cooperatively distributed coordinating, integrating, navigating, and data
problems. processing functions, operations or constraints over
The aim of this paper is to show how any moves.
93
A move can be a constant or variable, or 3 ELEMENTARY EXAMPLE

recursively an arbitrary wave itself:
3.1 The Task
move → constant | variable | wave
An elementary task to be programmed in DSL may
Variables classify as nodal, associated with space look like follows. Let it needs to go to the physical
positions and shared by different waves, frontal, locations of a disaster zone with coordinates (using
moving in space with program control, and x-y pair here for simplicity) x25_y12, x40_y36, and
environmental, accessing the environment navigated. x55_y21, measure temperature there, and transmit
Constants may reflect both information and physical its value, together with exact coordinates of the
matter. locations reached, to a collection center. The
Wave, being applied in a certain position of the corresponding program in DSL will be as follows:
distributed world, can perform proper actions in a
distributed space, terminating in the same or in other Hop(x25_y12, x40_y36, x55_y21).
positions. It provides final result that unites local Transmit(Temperature & Location)
results in the positions (nodes) reached, and also
produces resultant control state. The (distributed) The program moves independently to the three
result and the state can be subsequently used for locations given, and in every destination reached
further data processing and decision making on measures temperature using special environmental
higher program levels. Parallel waves can start from variable Temperature. Using another
different nodes in parallel, possibly intersecting in
environmental variable Location, it attaches to
the common distributed space when evolving in it
the previous value exact coordinates of the current
independently.
physical position (which, by using GPS, may differ
If moves are ordered to advance in space one
from the initially intended, rough coordinates). The
after the other (which is defined by a proper rule),
two-value results are then independently transmitted
each new move is applied in parallel in all the nodes
from the three locations to a collection center.
reached by the previous move. Different moves (by
This program reflects semantics of the task to be
other rules) can also apply independently from the
performed in a distributed space, regardless of
same node, reaching new nodes in parallel. The
possible equipment that can be used for this. The
functional style syntax shown above can express any
latter may, for example, be a set of sensors scattered
program in DSL, but if useful, other notations can be
in advance throughout the disaster zone, where
used, like the infix one. For example, an
hopping by coordinates may result in a wireless
advancement in space can use period as operator
access of the sensors already present there--not
(separator) between successive steps, whereas
necessarily moving into these points physically. As
parallel actions starting from same node can be
another solution, this program may task mobile
separated by a semicolon. For improving readability,
robots (single or a group) to move into these
spaces can be inserted in any places of the programs-
locations in person and perform the needed
-they will be automatically removed before
measurement and transmission upon reaching the
execution (except when embraced by quotes).
destinations.
The interpreter may have its own physical body
(say, in the form of mobile or humanoid robot), or
can be mounted on humans (like in mobile phones).
3.2 Single-Robot Solution
A network of the interpreters can be mobile and
Let us consider how the previous program will be
open, changing its volume and structure, as robots or
executed with only a single robot available (let it be
humans can move at runtime. We will be assuming
Robot 1, assuming other robots not reachable). After
for the rest of this paper that every sensor has the
its injection into the robot’s interpreter (see Fig. 3),
DSL interpreter installed, which may have a
the first, broadcasting statement:
software implementation or can be a special
hardware chip.
Hop(x25_y12, x40_y36, x55_y21)
In the following sections we will show and
explain the DSL code for a number of important
will be analyzed first. It naturally splits into three
problems to be solved by advanced sensor networks,
independent hops, but only one can be performed at
which confirms an efficiency of the proposed
the start by a single robot.
distributed computational and control model.
94
of them the replicated rest of the program (i.e.

Hop(x25_y12, x40_y36, x55_y21).
Transmit(Temperature & Location) Hop(x55_y21). second statement) the interpreter in Robot 1 will
Robot 1
Transmit
(Temperature produce the following three independent programs:
Program
injection
& Location)
Hop(x25_y12).
Robot 1 x40_y36 Transmit(Temperature & Location)
Hop(x25_y12, x40_y36, x55_y21
x55_y21). Transmit
(Temperature &
Transmit
Hop(x40_y36).
Location)
(Temperature Robot 1 Transmit(Temperature & Location)
& Location)
x25_y12
Hop(x40_y36, x55_y21). Hop(x55_y21).
Robot 1 Transmit (Temperature
& Location)
Transmit(Temperature & Location)
Leaving one of them (say, the first) for execution in

Figure 3: Single-robot solution. itself, Robot 1 requesting other available robots (let
them be Robot 2 and Robot 3) by a wireless channel
The interpreter hops virtually into the point sends electronically the obtained other two programs
x25_y12 ordering robot to move into the to them. After this, all three programs will be
corresponding physical location. Upon arrival, the operating in the three mentioned robots
second statement: independently and in parallel.
Each robot first executes the hop statement,
Transmit(Temperature & Location) moving into the location given by physical
coordinates, and upon reaching the destination,
will be executed by measuring temperature, executes the second statement measuring
attaching coordinates, and transmitting the result via temperature and detecting exact coordinates,
channels available. The rest of the program, subsequently transmitting these to the collection
represented as: center, exactly as for the previous case with a single
robot. All this is depicted in detail in Fig. 4.
Hop(x40_y36, x55_y21).
Hop(x25_y12, x40_y36, x55_y21).
Transmit(Temperature & Location) Robot 2
will be analyzed again, with hop to x40_y36
Initial
extracted, robot moved into the second location, and program Hop(x40_y36). Transmit Transmit
injection
measurement result transmitted as before. The (Temperature & Location)
Robot 2
(Temperature
& Location)
x40_y36
program’s remainder, now as: Robot 1
Spreading
to other
robots Transmit
(Temperature
Hop(x25_y12).
Hop(x55_y21). Transmit Hop(x55_y21). Transmit
Robot 3 & Location)
(Temperature & x55_y21
Transmit(Temperature & Location) Location)
(Temperature & Location)
will cause movement and operation in the third x25_y12 Robot 1

Robot 3
location x55_y21. The program terminates after Transmit(Temperature

& Location)
shrinking to:
Figure 4: Multiple-robot solution.
The shown above were only elementary examples of
All these steps are depicted detail in Fig. 3. DSL code and rules of its execution. In other, more
complex cases DSL allows us to dynamically form
3.3 Multiple-Robot Solution networked knowledge arbitrarily distributed between
dynamic resources, also providing hierarchical
Let us consider now the case where other robots can control of distributed processes. DSL can provide
be requested and accessed from the robot into which description of system missions on a semantic level,
we injected our program (let it be Robot 1 again), with telling what, where, and why to do rather than
see Fig. 4. After analyzing the first statement, how and who should do this, effectively abstracting
splitting it into individual hops and attaching to each from the resources (computer networks, robots,
95
humans) that can implement them, which can be the first arrival in them. The hierarchical fusion rule
emergent and not known in advance. These fuse, collecting the scattered results, also removes
programs can be executed by any number of record duplicates, as the same event can be detected
technical or biological processors, where by different sensors, leaving only most credible in
organization of resources, their communication and the final result. To distinguish each new global
synchronization are delegated to efficient automatic navigation process from the previous one, it always
interpretation, with strict and simple implementation spreads with a new identity for which, for example,
rules. current system time may be used (using
Application programs in DSL are often hundreds environmental variables IDENTITY and TIME of
of times shorter than in conventional languages (like the language).
C or Java). The DSL programs are interpreted rather
than compiled, adjusting to the execution Global
environment at runtime, which is especially loop Global
S loop
important when system components may be changed
Start S
or damaged during the execution. The approach is S
based on the new, universal, formal system within Fused
which any distributed and parallel algorithms can be S data Start
expressed and implemented. Repeated S
parallel S
navigation
Failed
4 COLLECTING EVENTS S
S
S
THROUGHOUT THE REGION
Starting from all transmitter nodes, the following
program regularly (with interval of 20 sec.) covers Figure 5: Parallel navigation and data collection.
stepwise, via local communications between sensors,
the whole sensor network with a spanning forest,
lifting information about observable events in each 5 CREATING HIERARCHICAL
node reached. Through this forest, by the internal INFRASTRUCTURES
interpretation infrastructure, the lifted data in nodes
is moved and fused upwards the spanning trees, with In the previous program, we created the whole
final results collected in transmitter nodes and sent spanning forest for each global data collection loop,
in parallel outside the system using rule Transmit which may be costly. To optimize this process, we
(See Fig. 5). may first create a persistent forest infrastructure,
remembering which nodes were linked to which, and
Hop(all transmitters). then use it for a frequent regular collection and
Loop( fusion of the scattered data. As the sensor
Sleep(20). neighborhood network may change over time, we
IDENTITY = TIME. can make this persistent infrastructure changeable
Transmit( too, updating it with some time interval (much larger,
Fuse( however, than the data collection one), after
Repeat(free(observe(events)); removing the previous infrastructure version. This
Hop(directly reachable, can be done by the following program that regularly
first come)))))
creates top-down oriented links named infra
starting from the transmitter nodes (as shown in Fig.
Globally looping in each transmitter node (rule
6).
loop), the program repeatedly navigates (rule
repeat) the sensor set (possibly, in competition Hop(all transmitters).
with navigation started from other transmitters), Loop(
activating local space observation facilities in Sleep(120).
parallel with the further navigation. IDENTITY = TIME.
The resultant forest-like coverage is guaranteed Repeat(
by allowing sensor nodes to be visited only once, on Hop(directly reachable,
96
first come). rarely) and the event collection and fusion one
Remove links(all). (looping frequently) can operate simultaneously,
Stay(create link(-infra, BACK))) with the first one guiding the latter on the data
collection routes, which may change over time.
Looping
Looping
updates
updates
S 6 ROUTING LOCAL EVENTS TO
S
S
TRANSMITTERS
S infra
infra We have considered above the collection of
S distributed events in the top-down and bottom-up
Persistent S
links infra mode, always with the initiative stemming from root
Active nodes of the hierarchy--the latter serving as a
S code S parallel and distributed tree-structured computer. In
S this section, we will show quite an opposite, fully
distributed solution, where each sensor node, being
an initiator itself, is regularly observing the vicinity
Figure 6: Runtime creation of hierarchical infrastructure. for the case an event of interest might occur.
Having discovered the event of interest, each
This infrastructure creation program provides node independently from others launches a spatial
competitive asynchronous spatial processes, so each cyclic self-routing process via the infrastructure
time even if the sensors do not change their positions, links built before, which eventually reaches the
the resultant infrastructure may differ, as in Fig. 7. transmitter node, bringing with it the event
information, as shown in Fig. 8. The data brought to
the transmitters should be fused with the data
already existing there.
S
S infra
S
S infra S
infra
S
S Observing
S S
infra infra
S Observing
S S Observing S
S S
infra
S S
S
Figure 7: Another possible infrastructure. Event Event
discovered discovered
Having created a persistent infrastructure, we can
use it frequently by the event collection program, Figure 8: Routing scattered events to transmitters.
which can be simplified now as follows:
The corresponding program will be as follows.
Hop(all transmitters).
Loop( Hop(all nodes).
Sleep(20). Frontal(Transfer).
Transmit( Nodal(Result).
Fuse( Loop(
Repeat( Sleep(5).
Free(observe(events)); Nonempty(
Hop(+infra))))) Transfer = observe(events)).
Repeat(hop(-infra)).
The global infrastructure creation program (looping Result = Result & Transfer)
97
The transmitter nodes accumulating and fusing local

events, arriving from sensor nodes independently, Looping
in nodes
can subsequently send them outside the system. S
Different strategies can be used here. For example,
S
one could be waiting until there are enough event S
records collected in the transmitter before sending Tracking mobile
S intelligence
them, and the other one waiting for a threshold time
S
and only then sending what was accumulated (if any S
at all). The following program combines these two
cases within one solution, where arriving data from
sensors is accumulated in nodal variable Result. S S
S
Hop(all transmitters). Moving

object
Loop(
Or( Figure 9: Active tracking of a mobile object.
Quantity(Result) >= 100,
(Sleep(60). Result != nil)). Hop(all nodes).
Transmit(Result)) Frontal(Threshold) = 0.1.
Frontal(Object) = search(aerial).
This program in every transmitter can work in Visibility(Object) > Threshold.
parallel with the previous program collecting events Repeat(
and looping in every sensor (in transmitters as well, Loop(
assumed to be sensors too), and also with the earlier Visibility(Object) > Threshold).
program, starting in transmitters, for the regular Maximum destination(
infrastructure updates. Hop(directly reachable).
Visibility(Object) > Threshold))
7 TRACKING MOBILE OBJECTS The program investigates the object’s visibility in all
neighboring sensors in parallel and moves control
along with program code and accumulated data to
Let us consider some basics of using DSL for
the neighboring sensor seeing it best (if visibility
tracking mobile (say ground or aerial) objects
there exceeds the threshold given).
moving through a region controlled by
This was only a skeleton program in DSL,
communicating sensors, as shown in Fig. 9. Each
showing the space tracing techniques for controlling
sensor can handle only a limited part of space, so to
single physical objects. It can be extended to follow
keep the whole observation continuous the object
collectively behaving groups of physical objects (say,
seen should be handed over between the neighboring
flocks of animals, mobile robots, or troops). The
sensors during its movement, along with the data
spreading individual intelligences can cooperate in
accumulated during its tracking and analysis.
the distributed sensor space, self-optimizing jointly
The space-navigating power of the model
for the pursuit of global goals.
discussed can catch each object and accompany it
individually, moving between the interpreters in
different sensors, thus following the movement in
physical space via the virtual space (Sapaty, 1999). 8 AVERAGING PARAMETERS
This allows us to have an extremely compact and FROM A REGION
integral solution unattainable by other approaches
based on communicating agents. The following Let us consider how it could be possible to asses the
program, starting in all sensors, catches the object it generalized situation in a distributed region given,
sees and follows it wherever it goes, if not seen from say, by a set of its border coordinates, in a fully
the current point any more (more correctly: if its distributed way where sensors located in the region
visibility becomes lower than a given threshold). communicate with direct neighbors only. Assume,
for example, that the data of interest is maximum
pollution level throughout the whole region (it may
also be temperature, pressure, radiation level, etc.)
together with coordinates of the location showing
98
this maximum.
The following program, starting in all sensors
located in the region, regularly measures the S
pollution level in its vicinity, updates local Generalized
S result S
maximum and, by communication with direct Emergent
neighbors, attempts to increase the recorded leader
S
maximum there too. Eventually, after some time of node S Self-routing
this purely local communication activity all sensors S to transmitter
will have the same maximum value registered in
them and corresponding to the maximum on the
S S
whole region (see the overall organization in Fig. S
10).
Region of interest Local communications
Nodal(Level, Max, Region).
Frontal(Transfer). Figure 10: Distributed averaging with active routing.
Region = region definition.
Hop(all nodes, Region).
Loop( 9 ASSEMBLING FULL PICTURE
Or parallel( OF THE REGION
Loop(
Sleep(5).
Level = measure(pollution). To collect details from some region via local sensors
Stay(Level > Max. Max=Level). and merge them into the whole picture could, in
Transfer = Max. principle, be possible via local single-level
Hop(directly reachable,Region). exchanges only, as in the previous section, but the
Transfer > Max. Max=Transfer), amount of communications and data transfer as well
sleep(120)). as time needed may be unacceptably high. We were
Level == Max. finding only a single value (maximum) via frequent
Transfer = Max & WHERE. internode communications, with minimal code
Repeat(hop(-infra)). length.
Transmit(Transfer)) But for obtaining the detailed global picture of
the region or of some distributed phenomenon, we
As there may be many sensors located in the region may have to gradually paint (grow) this picture in
of interest, we will need forwarding only a single every sensor node simultaneously, with high
copy of this resultant maximum value to a communication intensity between the nodes. Also,
transmitter for an output. This can be achieved by there may be difficult to determine the completeness
delegating this task only to the sensor whose of this picture staying in local sensors only. A higher
measured local value is equal to the accumulated integrity and hierarchical process structuring may be
maximum in it, which corresponds to the overall needed to see a distributed picture via the dispersed
region’s maximum. sensors with limited visual capabilities and casual
Having discovered that it is the leader (after a communications.
certain time delay), such a sensor organizes repeated Different higher-level approaches can be
movement to the nearest transmitter via the earlier proposed and expressed in DSL for this. We will
created virtual infrastructure, carrying the resultant show only a possible skeleton with spanning tree
maximum value in frontal variable Transfer, and coverage of the distributed phenomenon and
sending it outside the system in the transmitter hierarchical collection, merging, and fusing partial
reached, as shown in Fig. 10. results into the global picture. The latter will be
Similar, fully distributed, organization may be forwarded to the nearest transmitter via the
introduced for finding averaged values, or even for previously created infrastructure (using links
assembling the global picture of the whole region infra), as in Fig. 11.
with any details collected by individual sensors (the
latter may be costly, however, with a more practical Hop(random, all nodes,
solution skeleton shown in the next section). detected(phenomenon)).
Loop(
Frontal(Full) = fuse(
99
Repeat( problems as well as making autonomous decisions in

Free(collect(phenomenon)); distributed environments.
Hop(directly reachable, The approach is based on quite a different type
first come, of high-level language allowing us to represent
detected(phenomenon)))). system solutions in the form of integral seamless
Repeat(hop(-infra)). spatial processes navigating and covering
Transmit(Full)) distributed worlds at runtime. This makes parallel
and distributed application programs extremely short,
In the more complex situations, which can be which may be especially useful for the energy
effectively programmed in DSL too, we may have a saving communications between sensors.
number of simultaneously existing phenomena, The code compactness and simplicity are
which can intersect in a distributed space. We may achieved because most of traditional synchronization
also face combined phenomena integrating features and data or agent exchanges (which are also on a
of different ones. The phenomena (like flocks of high level, with minimum code sent) are shifted to
birds, manned or unmanned groups or armies, efficient automatic implementation, allowing us
spreading fire or flooding) covering certain regions concentrate on global strategies and global solutions
may change in size and shape, they may also move instead.
as a whole preserving internal organization, etc.
In the previous language versions (Sapaty, 1999,
2005; Sapaty et al., 2006), a variety of complex REFERENCES
topological problems in computer networks were
investigated and successfully programmed in a fully
Chong, C.-Y., Kumar, S. P., 2003. Sensor networks:
distributed and parallel manner, which included Evolution, opportunities, and challenges. Proc. of the
connectivity, graph patterns matching, weak and IEEE, Vol. 91, No. 8, August, pp.1247-1256.
strong components like articulation points and Culler, D., Estrin, D., Srivastava, M., 2004. Overview of
cliques, also diameter and radius, optimum routing sensor networks, Computer, August, pp.41-49, publ.
tables, etc., as well as topological self-recovery after by the IEEE Computer Society.
indiscriminate damages (Sapaty, 1999). Sapaty, P. S., 1999. Mobile Processing in Distributed and
Open Environments, John Wiley & Sons, ISBN:
0471195723, New York, February, 436p.
(www.amazon.com).
S Sapaty, P. S., 2005. Ruling Distributed Dynamic Worlds,
Full John Wiley & Sons, New York, May, 256p, ISBN 0-
S picture S 471-65575-9 (www.amazon.com).
Initiator Sapaty, P., Sugisaka, M., Finkelstein, R., Delgado-Frias, J.,
S Mirenkov, N., 2006. Advanced IT support of crisis
Self-routing
S to transmitter
relief missions. Journal of Emergency Management,
Vol.4, No.4, ISSN 1543-5865, July/August, pp.29-36
S
(www.emergencyjournal.com).
Sapaty, P., Morozov, A., Finkelstein, R., Sugisaka, M.,
S S Lambert, D., 2007. A new concept of flexible
S
organization for distributed robotized systems. Proc.
Echoed Active space
Twelfth International Symposium on Artificial Life and
partial results coverage
Robotics (AROB 12th’07), Beppu, Japan, Jan 25-27,
8p.
Figure 11: Space coverage with global picture fusion.
Wireless sensor network. Wikipedia, the free encyclopedia,
www.wikipedia.org.
Zhao, F., Guibas, L., 2004. Wireless Sensor Networks: An
10 CONCLUSIONS Information Processing Approach (The Morgan
Kaufmann Series in Networking). Morgan Kaufmann,
376p.
We have presented a universal and flexible approach
of how to convert distributed sensor networks with
limited resources in nodes and casual
communications into a universal spatial machine
capable of not only collecting and forwarding data
but also solving complex computational and logical
100
BAYES-BASED OBJECT TRACKING BOOSTED BY PARTICLE
SWARM OPTIMIZATION
Yuhua Zheng and Yan Meng

Department of Electrical and Computer Engineering, Stevens Institute of Technology, Hoboken, NJ 07030, USA
Keywords: Vision, object detection, tracking, particle swarm optimization, Bayes law.
Abstract: This paper presents a novel Bayes-based object tracking framework boosted by a particle swarm
optimization (PSO) algorithm, which is a population based searching algorithm. Basically two searching
steps are conducted in this method. First, the object model is projected into a high-dimensional feature space,
and a PSO algorithm is applied to search over this high-dimensional space and converge to some global
optima, which are well-matched candidates in terms of object features. Second, a Bayes-based filter is used
to identify the one with the highest possibility among these candidates under the constraint of object motion
estimation. The proposed algorithm considers not only the object features but also the object motion
estimation to speed up the searching procedure. Experimental results demonstrate that the proposed method
is efficient and robust in object tracking.
1 INTRODUCTION on how to sample probabilities and likelihoods.

When these methods are applied to multiple objects,
Object detection and tracking in images is an active a dominant peak is established if an object has large
research area which has attracted extensive attentions likelihood values more frequently, which may
from multi-disciplinary fields, and it has wide depress and lose other objects. The performance of
applications in many fields like service robots, particle filter based methods is limited by
surveillance systems, public security systems, and dimensionality of state space, which may be feasible
virtual reality interfaces. Detection and tracking of in the cases with fewer targets, but may be intractable
moving object like car and walking people are more with a large amount of targets. Generally speaking,
concerned, especially flexible and robust tracking the mean-shift algorithm is efficient for object
algorithms under dynamic environments, where tracking. However the searching window may drift
lightening condition may change and occlusions may away from the object under dynamic conditions. For
happen. example, if the kernel is lost from the tracked target
Up to now, the underlying mathematical models in one frame under some emergent situations, such as
of most tracking methods are Bayes’ law estimation illumination condition change, it would be difficult
and Hidden Markov Model (HMM). The most for the tracker to recover itself from this unpredicted
popular approaches to predict discrete probability event.
distribution are Kalman filter (G. Welch and G. Usually for object tracking, an analysis window
Bishop, 2001), condensation (M. Isard, 1998), based on the expectation of objects features is built
particle filter (S. Maskell and N. Gordon, 2001) and and scan over the image to find out areas of interest
mean shift (D. Comaniciu, and P. Meer, 2002). (AOI). However, most conventional analysis-window
Kalman filter has the same idea with HMM, while based trackers are influenced by the shape and size of
Kalman filter deals with discrete variables. Some the window, which may vary from one frame to
researchers proposed different control and noise another. It is difficult to find the appropriate window
models into the recursion function for image for each frame, especially under dynamic
processing, however those assumptions are environments where the content of the images may
dependent on varied applications and need to be be dramatically changed.
tuned carefully. Condensation methods mainly focus There are various features can be used for object
detection and tracking, such as color, shape, texture,
101
gesture, contour, and motion. Some successful This paper is organized as follows. Section 2
methods take advantage of knowledge of objects, simply reviews some related work in object detection
such as shape or structures. However, the shape- and tracking. Section 3 introduces the PSO algorithm.
based methods cannot handle the cases with The Bayes-based adaptive-window approach boosted
occlusions efficiently. Appearance histogram is by the PSO algorithm is described in Section 4.
applied as tracking cue in this paper due to its Experimental results are discussed and analyzed in
independency with objects’ shape and structure. Section 5. Conclusion and further work are given in
A Bayes-based object tracking approach using a section 6.
particle swarm optimization (PSO) algorithm is
employed to search for an optimal window in a super
feature space based on appearance histogram instead 2 RELATED WORKS
of image plane directly. The PSO algorithm (J.
Kennedy, R. C. Eberhart, and Y. Shi, 2001) was There are many systems proposed in the past few
inspired by the social behavior of a flock of birds. In decades for object detection and tracking. Zhang et al.
PSO algorithm, birds in a flock are symbolically
(Zhang et al., 2006) proposed a robust method to
represented as particles. These particles can be detect moving objects at distance using a mobile
considered as simple agents “flying” through a camera. Through the utilization of the focus of
problem space. A particle’s location in the multi-
expansion (FOE) and its associated residual map, the
dimensional problem space represents one solution proposed method is able to detect and separate
for the problem. When a particle moves to a new independently moving objects (IMOs) from the
location, a different solution is generated. This
"moving" background caused by the camera motion.
solution is evaluated by a fitness function that Leykin and Hammoud (Leykin and Hammoud, 2006)
provides a quantitative value of the solution’s utility.
used a combined input from RGB and thermal
The PSO algorithm is effective for optimization
cameras to build background model and tracker for
of a wide range of searching problems. In this
pedestrians. This method showed robustness for
problem, particles fly around the feature space, trying
outdoor environments. Olson and Brill (T. Olson and
to find the best-fit tracking window parameters based
F. Brill, 1997) built a general purpose system for
on the fitness function of object features using
moving object detection and event recognition,
appearance histogram. When some particles
where objects were detected and tracked by both
successfully detect the objects, they will share that
first-order prediction and nearest neighbor matching.
information with their neighbors, and their neighbors The work which is most related to our method is
may follow the directions to reach objects very
(Yuri Owechko, Swarup Medasani, and Narayan
quickly. Each particle makes its own decision not Srinivasa, 2004), where the authors treated every
only based on its neighbors, but also on its own particle as a classifier with different parameters.
cognition, which provides the flexibility and ability
Those classifiers swarm in the solution space to
of exploring new areas. This decision-making converge to the optimal analysis window. However
procedure can efficiently prevent the local optimum, this is a simple application of PSO for people
which may cause the searching window drift.
detection only. Reza Akbari etc. (Reza Akbari,
By using PSO, the problem of identifying Mohammad Davarpanah Jazi, and Maziar Palhang,
tracking window is translated from one-to-one 2006) employed both PSO algorithm and Kalman
estimation into one-to-many searching, which brings
filter in a hybrid framework of region and object
more flexibility. Since this searching procedure is tracking, where vehicles were tracked in a cluttered
conducted only in the object feature space, to
background. A PSO algorithm was proposed in (Luis
improve searching results, a Bayes law filter is
Anton-Canalis, Mario Hernandez-Tejera, and Elena
constructed based on the motion constraints of Sanchez-Nielsen etc., 2006) to drive particles flying
tracked objects to identify the most possible solution.
over image pixels directly, where object tracking
Generally it is reasonable to assume that objects emerged from interaction between particles and their
move consecutively in successive frames. The Bayes environment.
law filter tries to keep inertia of the object motion.
Compared with conventional window-tracking
algorithms, the proposed method can be executed
automatically, and moving objects can be detected
and tracked in a more flexible and robust way.
102
3 PARTICLE SWARM tendency to move towards the neighborhood's best

previous position.
OPTIMIZATION The velocity and position of the particle at any
iteration is updated based on the following equations:
PSO algorithm is an efficient optimization method
proposed by Kennedy and Eberhart in 1995 (R. vidt +1 = w ⋅ vidt + c1 ⋅ϕ1 ⋅ ( pidt − xidt ) + c2 ⋅ϕ2 ⋅ ( pgd
t
− xidt ) (1)
Eberhart and J. Kennedy, 1995) (J. Kennedy and R.C. xidt +1 = xidt + vidt +1 (2)
Eberhart,1995) from the simulation of a simplified
social model, which obviously has its root in where vidt is the component in dimension d of the
artificial life in general, and in bird flocking, fish ith particle velocity in iteration t, xidt is the
schooling and swarming theory in particular. On the
component in dimension d of the ith particle position
other hand, it is also a method of evolutionary
computation, related with both genetic algorithm and in iteration t, c1 ,c2 are constant weight factors, pidt is
evolutionary programming. t
the best position achieved by particle i, p gd is the
The PSO algorithm is population-based: a set of
potential solutions evolves to approach a convenient best position found by the neighbors of particle i,
solution for a problem. Being an optimization ϕ1 , ϕ 2 are random factors in the (0,1) interval, and
method, the aim is to find the global optimum of a w is the inertia weight. The PSO requires tuning of
real-valued fitness function defined in a given search some parameters: the individual and sociality
space. Rather than just being a social simulation, weights c1 , c2 , and the inertia factor w .
PSO can be treated as a powerful new search The mechanism of PSO implicitly assumes that in
algorithm, capable of optimizing a wide range of N- most real world situations, the optima have better
dimensional problems. residence around them. Experimentally during the
The social metaphor that leads to this algorithm search, regions with high fitness values attract more
can be summarized as follows: the individuals that particles and make particles concentrated after a few
are part of a society hold an opinion that is part of a iterations. So this type of search is faster and more
"belief space" (the search space) shared by effective than traditional scanning and gradient
neighboring individuals. Individuals may modify this methods. On the other hand, PSO is simpler than
"opinion state" based on three factors: genetic algorithm since all particles employ the same
The knowledge of the environment (inertia mechanism during evolutions. Although basic PSO is
part) designed for only single optimum, there are many
The individual's previous history of states works have been done to process more complex
(individual part) issues (Kennedy, J. &R.Eberhart, 1997).
The previous history of states of the
individual's neighborhood (social part)
An individual's neighborhood may be defined in
several ways, configuring somehow the "social 4 THE APPROACH
network" of the individuals. Following certain rules
of interaction, the individuals in the population adapt 4.1 General Idea
their scheme of belief to the ones that are more
successful among their social network. Over the time, Basically, object tracking can be considered as a
a culture arises, in which the individuals hold probability-based classification and estimation,
opinions that are closely related. which searches for the best match of the target model.
In the PSO algorithm each individual is called a Usually searching algorithms rely on two factors:
"particle", and is subject to a movement in a searching space and searching window. In terms of
multidimensional space that represents the belief the searching space, the more features the object has,
space. Particles have memory, thus retaining part of the larger the searching space will be. To expedite
their previous states. There is no restriction for the search, we can either bound the searching space
particles to share the same point in belief space, but with some constraints, or develop an efficient
in any case their individuality is preserved. Each searching algorithm. Considering the searching
particle's movement is the composition of an initial window, adaptive windows have been extensively
random velocity and two randomly weighted utilized due to its robustness.
influences: individuality, the tendency to return to the In this paper, we propose a framework which
particle's best previous position, and sociality, the combines a PSO-based searching algorithm and a
Bayes-based probability algorithm to achieve the
103
efficiency and robustness of the tracking systems. parameters will be identified to describe the rectangle
Basically, a PSO-based searching algorithm windows, including 2D location of the central point,
identifies the changes in the scene, and the width and height of the rectangle, as shown in Figure
probability-based algorithm estimates the best 1. These parameters can build up a four-dimensional
candidate of the object with the highest possibility. search space.
More specifically, the PSO algorithm takes fast
scouting in a high-dimensional feature space and W
finds out some object candidates. Then Bayes law
filter decides which one is the best match.
L x,y
4.2 Object Detection
Usually object detection and recognition depend on
the features of the object, such as color, texture, and
shape. As indicated in (J. R. Jain and A. K. Jain, Figure 1: The four parameters associated with a particle
window.
1981), most changes in video content are typically
due to the motion of objects in the depicted scene
relative to the imaging plane, and a small amount of So in such a space, each particle presents a search
motion can result in large differences in the values of window with specific values of parameters, which
the samples in a picture, especially near the edges of can be defined as:
objects. Often, predicting an area of the current P = { pi | pi ( xi , yi , li , wi ), i = 1,2,..., N } (3)
picture from a region of the previous picture that is Where xi and yi represent the central point of the
displayed by a few samples in spatial location can rectangle related to particle i; li and wi represents
significantly reduce the need for a refining difference
the length and width related to particle i; and N is the
approximation. We call this special displacement
population of swarm particles. Each individual
motion vectors.
particle has different values of these parameters. In
Since only the moving objects are considered to
other words, they are distributed in a four-
be tracked in this paper, the object detection turns
dimensional search space.
into motion detection where a simple background
Generally a four-dimensional feature space is
subtraction method is applied. When the detection
very large, which makes search algorithms to be
starts, the first several frames are looked as the
computation extensive. Some motion-based
background. In the following frames, the moving
constraints can be applied to limit the search area to a
targets can be easily detected by a motion detection
smaller region where particles are initialized and
algorithm using background subtraction. During this
move around. A straightforward constraint is using
procedure, the histogram model of background is
the continuity of movement since it is reasonable to
built and updated by averaging every coming frame
assume that motion is continuous under most
to achieve higher robustness. The motion vector
tracking situations. In other words, the tracking
Vi , i = 1,2,..., N can be obtained, where Vi represents window of a new frame should be adjacent to its
motion vectors of particle i, and N represents the total previous one. In this way, the initialization of
number of particles. Once a valid object is identified, particles could be accelerated.
the tracking algorithm kicks in. Suppose pb ' ( xb ' , yb ' , lb ' , wb ' , θ b ' ) is the best particle
(i.e., tracking window) in last frame, the initialized
4.3 PSO-based Searching Algorithm particles pi ( xi , yi , li , wi , θ i ) , where i = 1,2,…,N, in
From the view of searching, the PSO algorithm is a the new frame should be around pb ' with some
distributed convergence method. The key is to take offsets in each dimension. In our experiments,
advantage of sharing information between the locations are shifted up to 15 pixels, and sizes are
particles as well as their own past experiences to shrunk and extended up to 20 percent. Therefore, by
accelerate the convergence. The PSO algorithm dispersing particles in a relatively smaller region
would provide an optimal or near-optimal solution instead of the whole space, searching procedure can
using appropriate fitness functions without the be definitely accelerated.
complete knowledge of the searching space. Then particles move around, communicate and
To identify an object in an image, usually share information among the society, follow the
rectangle windows are utilized in most cases. Four better directions of their neighbors, and converge to
104
the optima. This process is automatic and of hue values from 0 to 255. H i ( x) and H g (x) are
independent on knowledge of image contents. After a
pixel numbers with a specific hue value x for the
number of iterations, particles cluster around one or
particle and target, respectively.
several optimal points in the space, which correspond
By using (4), the distance between two
to some regions with varied locations and sizes.
histograms can be defined as (D. Comaniciu, V.
These regions are candidates for the Bayes filter,
Ramesh, and P. Meer, 2004):
which will be discussed in later section.
D( H i , H g ) = 1 − BC ( H i , H g ) . (5)
4.4 Fitness Function This distance is invariant to the scale of the target,
while the popular used histogram intersection is scale
The behaviors of particles are guided by the variant (M.J. Swain, D.H. Ballard, 1991). The
associated fitness function, which defines the search smaller this distance is, the better the particle is
criteria underlying the PSO searching algorithm. In matched with the target object. Thus given the target
terms of object tracking, fitness function can be histogram, the fitness function for particle i is
defined as a function of features of the tracked object. inversely proportional to the distance between
Lots of features are used for objects detection and H i and H g :
tracking, including color, texture, shape and motion,
which can be employed independently or several F ( pi , g ) = 1 / D( H i , H g ) (6)
features can be combined together. In this paper, the The higher the fitness value, the more similar the
appearance histogram is applied to construct the corresponding area is with the target.
fitness function.
First, images are transformed from RGB format 4.5 Bayes-Based Filter
into HSV format, and the later one is more natural
for people’s eyes. Then, the values of hue are For each frame a motion vector V can be calculated
abstracted to build the histogram. Such histogram according to a motion trajectory of the tracking
refers to the gradation of color within the visible window. The motion vector is zero in the first frame.
spectrum. When a PSO-based searching algorithm is And for others, it is the shift from the previous
applied, each particle at every moment is associated position to the current one.
with a histogram. The best matched one can be Given the previous tracking window associated
obtained by comparing these histograms with the with the target histogram and the motion
target histogram. Therefore, a special criterion is
required to measure the similarity between the
{ }
vector H g ,Vg , where Vg represents the motion
searched window and the target window, which vector of target. The PSO-based searching algorithm
means a method to measure the distance between two returns a set of candidate windows, which can be
histograms is required. represented by {H i ,Vi | i = 1,2,..., m} , where H i
In statistics (T. Kailath, 1967), the Bhattacharyya represents histograms of particle i, Vi represents
Coefficient measures the similarity of two discrete motion vectors of particle i, and m is the number of
probability distributions. It is a divergence-type the selected candidate windows. All of these
measure that can be seen as the scalar product of the candidate windows are good enough in terms of
two vectors having components as the square root of appearance features and their fitness values are
the probability of the points x ∈ X. It thereby lends higher than a preset threshold.
itself to a geometric interpretation: the Bhattacharyya According to Bayes law, the problem can be
Coefficient is the cosine of the angle enclosed described as:
between these two vectors. Therefore, the p( H g ,Vg | H i ,Vi ) p( H i ,Vi )
Bhattacharyya Coefficient is used to measure the p( H i ,Vi | H g ,Vg ) = (7)
p( H g ,Vg )
similarity between these two histograms, which is
defined as:
p( H i , Vi | H g , Vg ) represents the condition
BC ( H i , H g ) = ∑ H i ( x) H g ( x)
{ }
(4)
x∈ X probability of a particle with {H i ,Vi } given H g ,Vg .
p( H g , Vg ) represents the probability of the target
Where H i represents the histogram of particle i,
window, which is same for all particles.
H g represents the histogram of the target, and X
p( H g , Vg | H i ,Vi ) represents the back projection
denotes the distribution domain, which is the range
105
from candidates to the previous tracking window. recognized as an object. As shown in Figure 2(b), a
Since all of particles can point back to the target car shape appears in the foreground while the
window in different ways, it is hard to tell which background keeps the same with the true image. For
particle is the most possible one without any most testing data with static background, motion
predefined knowledge of the image environment. In detection can detect moving objects quickly. For
this paper, we simply assume that those testing data under dynamic environment, some
all p ( H g ,Vg | H i ,Vi ) , i = 1,2,…,m, are equal. pre-knowledge of objects, such as moving behaviors,
would help to improve the detection performance.
However this assumption may not hold in some
Figure 3 shows the procedure of the proposed
practical applications, for instance a mobile vision
PSO algorithm searching for candidate windows. A
system where the previous motion trajectory of the
number of particles are distributed around the target
mobile platform would provide more information for
according to the tracking window of previous frame
the back projection, which will be investigated in our
in Figure 3(a). Due to the uncertainty of the object
future work.
movement, initially, these windows are set up as
Considering that the PSO-based searching
different sizes and locations near the detected object
algorithm returns all of candidates which are good
using motion detection. Then particles start to move
enough in appearance histogram, it is reasonable to
around and eventually converge to some optimal
ignore the histogram here and simplify (7) as:
points under PSO rules. Figure 3(b) shows these
p(Vi | Vg ) = cp(Vi ) , (8) optimal points, which are good candidates of tracking
windows. As shown in Figure 3(b), it is obviously
where c is a positive constant factor, and
that these candidate windows are much closer to the
p(Vi ) represents the probability of a particle on the car compared with those initial windows in Figure
motion trajectory. According to the inertia of motion, 3(a), which demonstrates the efficiency of the PSO-
p(Vi ) depends on the distance between Vi and Vg . based searching algorithm. Then Bayers filter is
The closer two vectors are, the higher the possibility applied to select the best match from those good
of the corresponding particle, which makes (8) as the candidates, as shown in Figure 3(c). Usually, the
following equation: PSO-based searching algorithm converges quickly.
In our experiments, initially 20 windows are
k generated, then after 10 to 15 time steps, those
p(Vi | Vg ) = cp(Vi ) = (9)
D(Vi ,Vg ) windows cluster to the object.
To evaluate the robustness of the proposed
where k is a positive factor. If two vectors are tracking method under occlusion, another experiment
shifted to the same original point, the distance is carried out as shown in Figure 4. First, a white car
between two vectors turns into the distance between drives in and is detected as the target by a blue
two points, where Euclidean distance can be rectangle window as shown in Figure 4(a). Then, the
calculated. white car traverses the scene and is occluded by a
block of texts in the image, as shown in Figure 4(b)
and (c). During the occlusion, the tracking window
5 EXPERIMENTAL RESULTS changes with scenes, but still tracks the car. As
shown in Figure 4(b), when the car starts moving into
To evaluate the proposed algorithm, some video clips the block, the tracking has almost the same size with
from PETS database are applied in this paper. The the one in Figure 4(a). Under the influence of the
program is written in C++ using OPENCV library, block, the tracking window shifts a little and shrinks.
running on a Pentium4 desktop. Most data come But the object is still locked. When the car moves
from a surveillance system with a stationary camera. away as shown in Figure 4(d), the window becomes
Figure 2 shows the process of identifying moving smaller until disappeared. It can be seen that the
objects by motion detection, where pictures from left tracker can still lock the object under occlusion.
to right are true data, foregrounds, and backgrounds, The above experiments demonstrate the proposed
respectively. If there is no moving object, as shown algorithm is efficient and robust. However under
in Figure 2(a), the background is the same with true some complex situations, such as dynamic
image and the foreground is empty since no object is background, more robust motion detection is
detected. With some general preprocessing, the noise required. For some noisy videos, the tracking
can be depressed and the model of background can window may be lost due to frame skips. A recovery
be enhanced. When a car drives in, it is detected and algorithm may need to increase the system reliability.
106
6 CONCLUSION results demonstrate that the proposed algorithm is

robust and efficient in some popular used video data
In this paper, a robust adaptive-window based There are still several issues remained and need
tracking algorithm is proposed to automatically to be improved and extended in the future work. The
detect and track moving objects. First, a motion first one is to investigate new object detection
detection algorithm is applied to detect the moving approaches under dynamic environment where the
object. Then a PSO-based searching algorithm comes background of the image and illumination conditions
to search for good candidates of adaptive tracking may be dramatically changed and the motion
windows with parameters on the new fame. Last, detection and histogram-based method applied in this
Bayes-based filter is employed to identify the best- paper will not be reliable any more. The second one
matched tracking window under the motion is to concrete the Bayes filter using some predefined
constraints of the tracked object. The experimental knowledge of the tracked targets.
(a) (b)
Figure 2: Motion detection to recognize objects, (a) to (b) from left to right.
(a) (b) (c)

Figure 3: Tracking procedure using PSO-based searching.
(a) (b) (c) (d)

Figure 4: Tracking under occlusion.
107
REFERENCES Workshop, OTCBVS'06, New York,USA, 17-22 June

2006 pp:136 – 141
Y. Zhang, S. J. Kiselewich, W. A. Bauson, and R.
G. Welch and G. Bishop, “An Introduction to Kalman Hammoud, “Robust Moving Object Detection at
Filter”, SIGGRAPH 2001, Course 8, 2001. Distance in the Visible Spectrum and Beyond Using a
M. Isard, “Visual Motion Analysis by Probabilistic Moving Camera”, IEEE Computer Vision and Pattern
Propagation of Conditional Density”, D.Phil. Thesis, Recognition Workshop (CVPRW), on Object Tracking
Oxford University, 1998. and Classification Beyond the Visible Spectrum, New
S. Maskell and N. Gordon, “A Tutorial on Particle Filters York, USA, 17-22 June 2006, pp: 1331-137
for On-line Nonlinear/Non-Gaussian Bayes’Law
Tracking”, 2001.
D. Comaniciu, and P. Meer, “Mean shift: a robust approach
toward feature space analysis,” IEEE Trans. Pattern
Analysis Machine Intel., Vol. 24, No. 5, pp. 603-619,
2002.
J. Kennedy, R. C. Eberhart, and Y. Shi, Swarm intelligence,
San Francisco, Morgan Kaufmann Publishers, 2001.
T. Olson and F. Brill. “Moving Object Detection and Event
Recognition Algorithms for Smart Cameras”, in Proc.
DARPA Image Understanding Workshop, pp. 159-175,
1997.
Yuri Owechko, Swarup Medasani, and Narayan Srinivasa,
"Classifier Swarms for Human Detection in Infrared
Imagery,” in 2004 Conference on Computer Vision and
Pattern Recognition Workshop (CVPRW’04), Vol. 8,
pp. 121, 2004.
Reza Akbari, Mohammad Davarpanah Jazi, and Maziar
Palhang “A Hybrid Method for Robust Multiple
Objects Tracking in Cluttered Background”, Vol. 1,
24-28, ICTTA ’06.
Luis Anton-Canalis, Mario Hernandez-Tejera, and Elena
Sanchez-Nielsen etc. “Particle Swarms as Video
Sequence Inhabitants For Object Tracking in Computer
Vision”, Vol. 2, pp. 604-609, ISDA ’06.
R. Eberhart and J. Kennedy. "A New Optimizer Using
Particles Swarm Theory", in Proc. Sixth International
Symposium on Micro Machine and Human Science,
Nagoya, Japan., IEEE Service Center, Piscataway, NJ,
39-43, 1995.
J. Kennedy and R.C. Eberhart, "Particle Swarm
Optimization", in Proceedings of IEEE International
Conference on Neural Networks, 1942-1948, 1995.
Kennedy, J. and R. Eberhart, “A discrete binary version of
the particle swarm algorithm,” in Proceedings of the
Conference on Systems, Man and Cybernetics, pp.
4104-4109, Piscataway, New Jersey, IEEE Press, 1997.
J. R. Jain and A. K. Jain, “Displacement measurement and
its application in interframe image coding”, IEEE
Trans. Commun. Vol. COM-29, no.12, pp.1799-1808,
Dec. 1981.
T. Kailath, “The Divergence and Bhattacharyya Dis-tance
Measures in Signal Selection," IEEE Trans. Com-mun.
Tech., COM-15:52-60, 1967.
D. Comaniciu, V. Ramesh, and P. Meer, “Real-Time
Tracking of Non-Rigid Objects using Mean Shift”,
CVPR, Vol. 2, pp. 142-149, 2000.
M. J. Swain, D.H. Ballard, “Color Indexing," Intern.
J.Comp. Vis., 7(1):11-32, 1991.
A. Leykin and R. I. Hammoud, “Robust Multi-Pedestrian
Tracking in Thermal-Visible Surveillance Videos”,
IEEE Computer Vision and Pattern Recognition
108
S MART MOBILE – AN ENVIRONMENT FOR GUARANTEED
MULTIBODY MODELING AND SIMULATION
Ekaterina Auer and Wolfram Luther

IIIS, University of Duisburg-Essen, Lotharstr. 63, Duisburg, Germany
{auer,luther}@inf.uni-due.de
Keywords: Validated method, interval, Taylor model, initial value problem, guaranteed multibody modeling and simula-
tion.
Abstract: Multibody modeling and simulation is important in many areas of our life from a computer game to space ex-
ploration. To automatize the process for industry and research, a lot of tools were developed, among which the
program MOBILE plays a considerable role. However, such tools cannot guarantee the correctness of results,
for example, due to possible errors in the underlying finite precision arithmetic. To avoid such errors and si-
multaneously prove the correctness of results, a number of so called validated methods were developed, which
include interval, affine and Taylor form based arithmetics. In this paper, we present the recently developed
multibody modeling and simulation tool S MART MOBILE based on MOBILE, which is able to guarantee
the correctness of results. The use of validated methods there allows us additionally to take into account the
uncertainty in measurements and study its influence on simulation. We demonstrate the main concepts and
usage with the help of several mechanical systems, for which kinematical or dynamic behavior is simulated in
a validated way.
1 INTRODUCTION ployment of a technique called interval arithmetic and

its extensions in multibody modeling and simulation
Modeling and simulation of kinematics and dynamics tools. Essential ideas of interval arithmetic were de-
of mechanical systems is employed in many branches veloped simultaneously and independently by several
of modern industry and applied science. This fact people whereas the most influential theory was for-
contributed to the appearance of various tools for au- mulated by R. E. Moore (Moore, 1966). Instead of
tomatic generation and simulation of models of multi- providing a point on the real number axis as an (in-
body systems, for example, MOBILE (Kecskeméthy, exact) answer, intervals supply the lower and upper
1993). Such tools produce a model (mostly a system bounds that are guaranteed to contain the true result.
of differential or algebraic equations or both) from These two numbers can be chosen so as to be ex-
a formalized description of the goal mechanical sys- actly representable in a given finite precision arith-
tem. The system is then solved using a correspond- metic, which cannot be always ensured in the usual
ing numerical algorithm. However, the usual imple- finite precision case. The ability to provide a guar-
mentations are based on finite precision arithmetic, anteed result supplied a name for such techniques –
which might lead to unexpected errors due to round ”validated arithmetics”. Their major drawback is that
off and similar effects. For example, unreliable nu- the output might be too uncertain (e.g. [−∞; +∞]) to
merics might ruin an election (German Green Party provide a meaningful answer. Usually, this is an in-
Convention in 2002) or cost people lives (Patriot Mis- dication that the problem might be ill conditioned or
sile failure during the Golf War), cf. (Huckle, 2005). inappropriately formulated, and so the finite precision
result wrong.
Aside from finite precision errors, possible mea-
surement uncertainties in model parameters and er- To minimize the possible influence of overestima-
rors induced by model idealization encourage the em- tion on the interval result, this technique was extended
109
with the help of such notions as affine (de Figueiredo 2 VALIDATED METHODS AND
and Stolfi, 2004) or Taylor forms/models (Neumaier, SOFTWARE
2002). Besides, strategies and algorithms much less
vulnerable to overestimation were developed. They To guarantee the correctness of MOBILE results, it
include rearranging expression evaluation, coordinate is necessary to enhance this tool with validated con-
transformations, or zonotopes (Lohner, 2001). cepts. Fortunately, we do not need to implement these
The second focus in this paper is MOBILE, an concepts from scratch. In the last decades, various li-
object oriented C++ environment for modeling and braries were implemented that supported different as-
simulation of kinematics and dynamics of mechani- pects of validated calculus. In the first Subsection, we
cal systems based on the multibody modeling method. name several of these tools. After that, we give a very
Its central concept is a transmission element which brief overview of interval arithmetic and an algorithm
maps motion and force between system states. For for solving IVPs (initial value problems) to provide a
example, an elementary joint modeling revolute and reference about the difference of validated methods to
prismatic joints is such a transmission element. Me- the usual floating point ones.
chanical systems are considered to be concatenations
of these entities. In this way, serial chains, tree type
2.1 Validating Multibody Tools of the
or closed loop systems can be modeled. With the
help of the global kinematics, the transmission func- Numerical Type
tion of the complete system chain can be obtained
from transmission functions of its parts. The inverse To validate the results of multibody modeling and
kinematics and the kinetostatic method (Kecskeméthy simulation software of the numerical type, the
and Hiller, 1994) help to build dynamic equations of following components are necessary. First, the
motion, which are solved with common initial value means are required to work with arithmetic op-
problem (IVP) solvers. MOBILE belongs to the nu- erations and standard functions such as sine or
merical type of modeling software, that is, it does cosine in a guaranteed way. Here, the basic
not produce a symbolic description of the resulting principles of interval calculus and its extensions
model. Only the values of output parameters for the are used. Interval arithmetic is implemented in
user-defined values of input parameters and the source such libraries as PROFIL/BIAS (Knüppel, 1994),
code of the program itself are available. F ILIB ++ (Lerch et al., 2001), C-XSC (Klatte et al.,
S MART MOBILE (Simulation and Modeling of 1993). L IBA FFA (de Figueiredo and Stolfi, 2004) is
dynAmics in MOBILE: Reliable and Template a library for affine arithmetic, whereas COSY (Berz
based) enhances the usual, floating point based and Makino, 2002) implements Taylor models.
MOBILE with validated arithmetics and IVP Second, validated algorithms for solving sys-
solvers (Auer et al., 2006a). In this way, it can model tems of algebraic, differential or algebraic-differential
and perform validated simulation of the behavior of equations are necessary. C-XSC TOOLBOX (Ham-
various classes of mechanical systems including non- mer et al., 1995) offers a general means of solving dif-
autonomous and closed-loop ones as well as provide ferent classes of systems as well as an implementation
more realistic models by taking into account the unin C-XSC. For IVP solving in interval arithmetic,
certainty in parameters. there exist such packages as AWA (Lohner, 1988),
In this paper, we give an overview of the struc- VNODE (Nedialkov, 2002), and recently developed
ture and the abilities of S MART MOBILE. First, the VAL E NC IA-IVP (Auer et al., 2006a). In the frame-
main validated techniques and software are refer- work of Taylor models, the solver COSY VI (Berz
enced briefly in Section 2. In Section 3, the main and Makino, 1998) was developed.
features of MOBILE are described in short to pro- Finally, almost all of the above mentioned solvers
vide a better understanding of the underlying struc- need means of computing (high order) derivatives au-
ture of S MART MOBILE. In Section 4, we describe tomatically. Some of them, for example, COSY VI,
the implementation of S MART MOBILE in some de- use the facilities provided by the basis arithmetic in
tail and validate kinematical and dynamic behavior of COSY. Interval implementations do not possess this
several example systems with the help of this environ- facility in general; external tools are necessary in this
ment. We summarize the paper in Section 5. On the case. The symbolic form of the mathematical model,
whole, we give an overview of the potential of vali- which is necessary to be able to obtain derivatives au-
dated methods in mechanical modeling, and, in par- tomatically, is not available in case of software of the
ticular, the potential of S MART MOBILE. numerical type. However, a method called algorith-
mic differentiation (Griewank, 2000) offers a possi-
bility to obtain the derivatives using the code of the
110
SMARTMOBILE – AN ENVIRONMENT FOR GUARANTEED MULTIBODY MODELING AND SIMULATION
program itself. To obtain the corresponding machine interval for the

There are two main techniques to implement al- real interval [x; x], the lower bound is rounded down to
gorithmic differentiation of a piece of program code: the largest machine number equal or less than x, and
overloading and code transformation. In the first case, the upper bound is rounded up to the smallest machine
a new data type is developed that is capable of com- number equal or greater than x.
puting the derivative along with the function value. Consider an algorithm for solving the IVP
This new data type is used instead of the simple one
in the code piece. The drawback of this method is the ẋ(t) = f (x(t)), x(t0 ) ∈ [x0 ], (1)
lack of automatic optimization during the derivative
computation. FADBAD++ (Stauning and Bendtsen, where t ∈ [t0 ,tn ] ⊂ R for some tn > t0 , f ∈ C p−1 (D )
2005) is a generic library implementing this approach for some p > 1, D ⊆ Rm is open, f : D 7→ Rm ,
for arbitrary user-defined basic data types. The tech- and [x0 ] ⊂ D . The problem is discretized on a
nique of code transformation presupposes the devel- grid t0 < t1 < · · · < tn with hk−1 = tk − tk−1 . De-
opment of a compiler that takes the original code note the solution with the initial condition x(tk−1 ) =
fragment and the set of differentiation rules as its xk−1 by x(t;tk−1 , xk−1 ) and the set of solutions
input and produces a program delivering derivatives {x(t;tk−1 , xk−1 ) | xk−1 ∈ [xk−1 ]} by x(t;tk−1 , [xk−1 ]).
as its output. This approach might be difficult to The goal is to find interval vectors [xk ] for which the
implement for large pieces of code which are self- relation x(tk ;t0 , [x0 ]) ⊆ [xk ], k = 1, . . . , n holds.
contained programs themselves. However, derivatives The (simplified) kth time step of the algorithm
can be evaluated more efficiently with this technique. consists of two stages (Nedialkov, 1999) :
An implementation is offered in the library ADOL- 1. Proof of existence and uniqueness. Compute a step
C (Griewank et al., 1996). size hk−1 and an a priori enclosure [x̃k−1 ] of the solu-
This list of tools is not supposed to be com- tion such that
plete. All of the above mentioned packages are im- (i) x(t;tk−1 , xk−1 ) is guaranteed to exist for all t ∈
plemented (or have versions) in C++, an important [tk−1 ;tk ] and all xk−1 ∈ [xk−1 ],
criterium from our point of view since MOBILE is
(ii) the set of solutions x(t;tk−1 , [xk−1 ]) is a subset of
also implemented in this language.
[x̃k−1 ] for all t ∈ [tk−1 ;tk ].
2.2 Theory Overview Here, Banach’s fixed-point theorem is applied to the
Picard iteration.
In this Subsection, we consider the basic principles of 2. Computation of the solution. Compute a tight en-
validated computations using the example of interval closure [xk ] ⊆ [x̃k−1 ] of the solution of the IVP such
arithmetic. First, elementary operations in this arith- that x(tk ;t0 , [x0 ]) ⊆ [xk ]. The prevailing algorithm is
metic are described. Then a basic interval algorithm as follows.
for solving IVPs is outlined to give an impression of 2.1. Choose a one-step method
the difference to floating point analogues. In particu- x(t;tk , xk ) = x(t;tk−1 , xk−1 ) + hk−1 ϕ(x(t;tk−1 , xk−1 )) + zk ,
lar, the latter passage makes clear why automatic dif- where ϕ (·) is an appropriate method function, and zk
ferentiation is unavoidable while simulating dynam- is the local error which takes into account discretiza-
ics of mechanical systems, that is, solving systems of tion effects. The usual choice for ϕ (·) is a Taylor se-
differential equations. ries expansion.
An interval [x; x], where x is the lower, x the up- 2.2. Find an enclosure for the local error zk . For
per bound, is defined as [x; x] = {x ∈ R : x ≤ x ≤ x}. the Taylor series expansion of order p − 1, this en-
For any operation ◦ = {+, −, ·, /} and intervals [x; x], p
closure is obtained as [zk ] = hk−1 f [p] ([x̃k−1 ]), where
[y; y], the corresponding interval operation can be de-
fined as [x; x] ◦ [y; y] = f [p] ([x̃k−1 ]) is an enclosure of the pth Taylor coeffi-
cient of the solution over the state enclosure [x̃k−1 ]
[min(x ◦ y, x ◦ y, x ◦ y, x ◦ y); max(x ◦ y, x ◦ y, x ◦ y, x ◦ y))] . determined by the Picard iteration in Stage One.
Note that the result of an interval operation is also an 2.3. Compute a tight enclosure of the solution. If
interval. Every possible combination of x ◦ y, where mean-value evaluation for computing the enclosures
x ∈ [x; x] and y ∈ [y; y], lies inside this interval. (For of the ranges of f [i] ([xk ]), i = 1, ..., p − 1, instead of
division, it is assumed that 0 ∈ / [y; y].) the direct evaluation of f [i] ([xk ]) is used, tighter en-
To be able to work with this definition on a com- closures can be obtained.
puter using a finite precision arithmetic, a concept of Note that Taylor coefficients and their Jacobians (used
a machine interval is necessary. The machine interval in the mean-value evaluation) are necessary to be able
has machine numbers as the lower and upper bounds. to use this algorithm.
111
3 MOBILE T the introduction of a state vector x =

T After
q , q̇T , the state-space form of the state equations
A transmission element, the basis of MOBILE, maps is obtained as
motion and loads between state objects (coordinate
q̇ q̇
frames or variables) according to ẋ = = b , (4)
q̈ −M −1 Q
q′ = φ(q), q̇′ = Jφ q̇, b q̇;t) = b(q, q̇;t) − Q(q, q̇;t). This is the
(2) where Q(q,
q̈′ = Jφ q̈ + J̇φ q̇, Q = JTφ Q′ .
responsibility of the class TMoMechanicalSystem.
Jφ is the Jacobian of φ, the mapping for the motion Finally, an IVP corresponding to (4) is
transmission. Other characteristics are vectors of di- solved by an appropriate integrator algorithm,
mension depending on the degrees of freedom of a for example, Runge–Kutta’s using the class
mechanical system. Here, q and q′ are the general- MoRungeKuttaIntegrator derived from the
ized positions, q̇ and q̇′ the velocities, q̈ and q̈′ the basic class MoIntegrator.
accelerations, as well as Q and Q′ the forces of the MOBILE models and simulates mechanical sys-
transmission element in the original and final states, tems directly as executable programs. This allows
respectively. The transmission of force is assumed to the user to embed the resulting modules in existing
be ideal. That is, power is neither generated nor con- libraries easily. Besides, the core of MOBILE is ex-
sumed. tendable owing to its open system design.
Models in MOBILE are concatenations of trans- As already mentioned, MOBILE belongs to the
mission elements. The overall mapping of this con- numerical type of the modeling software, by which
catenation from the original state into the final one we mean that it does not produce the symbolic de-
is obtained by the composition of the corresponding scription of the resulting mathematical model, as op-
mappings of the intermediate states. Concatenated el- posed to the symbolical type. In the latter case, the
ements are considered as a single transmission ele- process of validation of the model is basically reduced
ment. This helps to solve the task of the global kine- to the application of the validated methods to the ob-
matics: to obtain the positions, the orientations, the tained system of equations. In the former case, it is
velocities, and the accelerations of all bodies of a me- necessary to integrate verified techniques into the core
chanical system from the given q, q̇, and q̈. of the software itself, the task which cannot always
All transmission elements are derived from the ab- be solved since many modeling tools are not open
stract class MoMap, which supplies their main func- source.
tionality including the methods doMotion() and
doForce() for transmission of motion and force.
For example, elementary joints are modeled by the 4 S MART MOBILE
MoMap-derived class MoElementaryJoint. Besides,
there exist elements for modeling mass properties and In this Section, we first describe the main features of
applied forces. The corresponding representations the recently developed multibody modeling and sim-
of the mapping (2) for these elements are described ulation tool S MART MOBILE which produces guar-
in (Kecskeméthy, 1993). anteed results in the constraints of the given model.
Transmission elements are assembled to chains There, the modeling itself can be enhanced by tak-
implemented by the class MoMapChain. The methods ing into account the uncertainty in parameters, which
doMotion() and doForce() can be used for a chain might result, for example, from measurements. Af-
representing the system to determine the correspond- ter that, we demonstrate the possibilities S MART MO-
ing composite transmission function. BILE offers by simulating kinematical and dynamic
To model dynamics of a mechanical system, the behavior of two example systems in a validated way.
equations of motion have to be built and solved. Their
minimal form is given by 4.1 Main Features
M(q;t) q̈ + b(q, q̇;t) = Q(q, q̇;t) , (3)
The focus of S MART MOBILE is to model and simu-
where M(q;t) is the generalized mass matrix, late dynamics of mechanical systems in a guaranteed
b(q, q̇;t) the vector of generalized Coriolis and cen- way. The concept behind MOBILE, however, pre-
trifugal forces, and Q(q, q̇;t) the vector of applied supposes that kinematics is also modeled (almost as a
forces. The class MoEqmBuilder is responsible for by-product) and so it is easy to simulate it afterwards.
generation of such equations, that is, computation of That is why S MART MOBILE is one of the rare vali-
M, b, and Q for each given q, q̇, and t. dated tools that possess both functionalities.
112
To simulate dynamics, it is necessary to solve an placeholder and not with a concrete data type. For ex-
IVP for the differential(-algebraic) equations of mo- ample, the transmission element MoRigidLink from
tion of the system model in the state space form. MOBILE is replaced with its template equivalent
As already mentioned, validated IVP solvers need TMoRigidLink, the content of the placeholder for
derivatives of the right side of these equations. They which (e.g. TMoInterval or MoReal, cf. Figure 1)
can be obtained using algorithmic differentiation, the can be defined at the final stage of the system as-
method that is practicable but might consume a lot of sembly. This allows us to use a suitable pair con-
CPU time in case of such a large program as MO- sisting of the data type and solver depending on the
BILE. An alternative is to make use of the system’s application at hand. If only a reference about the
mechanics for this purpose. This option is not pro- form of the solution is necessary, MoReal itself and
vided by MOBILE developers yet and seems to be a common numerical solver (e.g. Runge-Kutta’s) can
rather difficult to algorithmize for (arbitrary) higher be used. If a relatively fast validation of dynamics
orders of derivatives. That is why it was decided to without much uncertainty in parameters is of inter-
employ the first possibility in S MART MOBILE. est, TMoInterval and TMoAWAIntegrator might be
To obtain the derivatives, S MART MOBILE uses the choice. For validation of highly nonlinear systems
the overloading technique. In accordance with with a considerable uncertainty, the slower combina-
Subsection 2.1, all relevant occurrences of MoReal tion of TMoTaylorModel and TMoRiOTIntegrator
(an alias of double in MOBILE) have to be re- can be used.
placed with an appropriate new data type. Almost MOBILE SmartMOBILE
each validated solver needs a different basic vali- TMoRigidLink<TMoInterval> R;
ր
dated data type. Therefore, the strategy in S MART- MoRigidLink R;
ց
MOBILE is to use pairs type/solver. To pro- TMoRigidLink<MoReal> R;
vide interval validation of dynamics with the help
of VNODE-based solver TMoAWAIntegrator, the Figure 1: Template usage.
basic data type TMoInterval including data types
necessary for algorithmic differentiation should be A MOBILE user can easily switch to S MART-
used. The data type TMoFInterval enables the use MOBILE because the executable programs for the
of TMoValenciaIntegrator, an adjustment of the models in both environments are similar. In the val-
basic version of VAL E NC IA-IVP. The newly de- idated environment, the template syntax should be
veloped TMoRiotIntegrator is based on the IVP used. The names of transmission elements are the
solver from the library R I OT, an independent C++ same aside from the preceding letter T. The methods
version of COSY and COSY VI, and requires of the classes have the same names, too. Only the
the class TMoTaylorModel, a S MART MOBILE- solvers are, of course, different, although they follow
compatible wrapper of the library’s own data type the same naming conventions.
TaylorModel. Analogously, to be able to use an ad-
justment of COSY VI, the wrapper RDAInterval 4.2 Examples
is necessary. Modification of the latter solver for
S MART MOBILE is currently work in progress. First, we give an example of the guaranteed simula-
In general, kinematics can be simulated with tion of kinematics of a five arm manipulator, the sys-
the help of all of the above mentioned basic tem defined in detail in (Traczinski, 2006). The mod-
data types. However, other basic data types eling of the system itself can be enhanced in S MART-
might become necessary for more specific tasks MOBILE by using so called sloppy joints (Traczin-
such as finding of equilibrium states of a sys- ski, 2006) instead of usual revolute ones. In the trans-
tem since they require specific solvers. S MART- mission element responsible for the modeling of the
MOBILE provides an option of modeling equilib-
rium states in a validated way with the help of
Ki+1
the interval-based data type MoFInterval and the
class MoIGradientStaticEquilibriumFinder, a
version of the zero-finding algorithm from the C- Ki Fi+1 = Fi
body i + 1
XSC T OOLBOX. Mi+1 = Mi + li × Fi
ϕi
The availability of several basic data types in
body i li
S MART MOBILE points out its second feature: the αi
general data type independency through its template Figure 2: A sloppy joint.
structure. That is, MoReal is actually replaced with a
113
Table 1: Widths of the position enclosures. Table 2: Performance of TMoAWAIntegrator,

TMoRiOTIntegrator, and TMoValenciaIntegrator. for
x y CPU (s) the double pendulum over the time interval [0; 0.4].
TMoInterval 1.047 1.041 0.02
TMoTaylorModel 0.163 0.290 0.14 Strategy AWA RiOT Valencia
Break-down 0.424 0.820 0.504
CPU time 1248 9312 294
sloppy joint, it is no longer assumed that the joint con-
nects the rotational axes of two bodies exactly con-
centrically. Instead, the relative distance between the that the results, especially those for Taylor mod-
axes is supposed to be within a specific (small) range. els, are acceptable. Although the set of all possi-
Two additional parameters are necessary to describe ble positions obtained statistically with the help of
the sloppy joint (cf. Figure 2): radius li ∈ [0; lmax ] 50,000 simulations (solid black area in Figure 3)
and the the relative orientation angle αi ∈ [0; 2π) (the is not as large as even the rectangle obtained with
parameter ϕi that describes the relative orientation be- TMoTaylorModel, there might exist parameter con-
tween two connected bodies is the same both for the stellations which lead to the results from this rectan-
sloppy and the usual joint). gle. Besides, statistical simulations require a lot of
The considered system consists of five arms of the CPU time, which is not the case with S MART MO-
lengths l0 = 6.5m, l1 = l2 = l3 = 4m, and l4 = 3.5m, BILE. Additionally, the results are proven to be cor-
each with the uncertainty of ±1%. The arms are con- rect there through the use of validated methods.
nected with five sloppy joints, for which lmax = 2mm The next example is the double pendulum with an
and initial angle constellation is ϕ0 = 60◦ , ϕ1 = ϕ2 = uncertain initial angle of the first joint from (Auer
ϕ3 = −20◦ , and ϕ4 = −30◦ . Each of these angles has et al., 2006a). The lengths of both massless arms
an uncertainty of ±0.1◦ . of the pendulum are equal to 1m and the two point
In Figure 3, left, the (abridged) S MART MOBILE masses amount to 1kg each with the gravitational con-
model of the manipulator is shown (the system geom- stant g = 9.81 ms2 The initial values for angles (speci-
etry described above is omitted). First, all the nec- fied in rad) and angular velocities (in rads ) are given
essary coordinate frames are defined with the help of as
the array K. Analogously, the required rigid links L T
3π 11π
and sloppy joints R, with which arms and their con- − 0.43 0.67 ,
nections are modeled, are declared. They are defined 4 20
later inside the for loop. The array l characterizes where the initial angle of the first joint has an uncer-
the lengths of the rigid links, and phi is used to define tainty of ±1% of its nominal value.
the angles of sloppy joints. All elements are assem- The interval enclosures of the two angles β1 and
bled into one system using the element manipulator. β2 of the double pendulum are shown for identical
By calling the method doMotion() for this element, time intervals in Figure 4. Besides, Table 2 summa-
we can obtain the position of the tip of the manipu- rizes the results. The line ”Break-down” contains the
lator, which equals the rotational matrix R multiplied time in seconds after which the corresponding method
by the translational vector r, both stored in the last no longer works. That is, the correctness of results
coordinate frame K[10]. cannot be guaranteed after that point. This happens
We simulate kinematics with the help of intervals here due to both the chaotic character of the consid-
and Taylor models. That is, the placeholder type is ered system and the resulting overestimation. The last
either TMoInterval or TMoTaylorModel. Both po- line indicates the CPU time (in seconds) which the
sition enclosures are shown in Figure 3, right. Note solvers take to obtain the solution over the integration
that enclosures obtained with intervals are wider than interval [0; 0.4]. Note that the CPU times are provided
whose obtained with Taylor models (cf. also Table 1, only as a rough reference since the solvers can be fur-
where the widths of the corresponding intervals are ther optimized in this respect.
shown). Taylor models are bounded by intervals to The use of TMoValenciaIntegrator improves
provide a comparison. Numbers are rounded up to both the tightness of the resulting enclosures and the
the third digit after the decimal point. CPU times are CPU time in comparison to TMoAWAIntegrator for
measured on a Pentium 4, 3.0 GHz PC under C YG - this example. Although TMoRiOTIntegrator breaks
W IN. down much later than the both former solvers, it needs
The statistic analysis of the same system with a lot of CPU time.
the help of the Monte-Carlo method (Metropolis and The double pendulum is a simple example of the
Ulam, 1949) carried out in (Hörsken, 2003) shows opportunities that S MART MOBILE offers for dy-
114
TMoFrame<type> *K=new TMoFrame<type>[2*5+1];

TMoAngularVariable<type> *phi=new Interval enclosure
Taylor model enclosure (order 5)
TMoAngularVariable<type>[5];
8.4
TMoVector<type> *l=new TMoVector<type>[5];
TMoSlacknessJoint<type> *R=new 8.2
TMoSlacknessJoint<type>[5]; 8
TMoRigidLink<type> *L=new TMoRigidLink<type>[5];
7.8
TMoMapChain<type> manipulator;
y(m)
double l_max; 7.6
for(int i=0;i<5;i++){ 7.4
TMoSlacknessJoint<type> joint(K[2*i],K[2*i+1], 7.2
phi[i],zAxis,l_max);
TMoRigidLink<type> link(K[2*i],K[2*i+1],l[i]);
R[i]=joint; L[i]=link;
16.4 16.6 16.8 17 17.2 17.4 17.6 17.8
Manipulator<<R[i]<<L[i];
} x(m)
Manipulator.doMotion();
cout<<"Position="<<K[10].R*K[10].r;
(a) S MART MOBILE model. (b) Enclosures of the position of the tip.
Figure 3: Kinematics of the five arm manipulator with uncertain parameters.
2.4 -1.2
TMoAWAIntegrator
2.3 TMoRiOTIntegrator
-1.32
TMoValenciaIntegrator
second angle (rad)
2.2
first angle (rad)
-1.44
2.1
TMoAWAIntegrator -1.56
2
TMoRiOTIntegrator
TMoValenciaIntegrator
1.9 -1.68
1.8 -1.8
0 0.082 0.164 0.246 0.328 0.41 0 0.082 0.164 0.246 0.328 0.41
time (s) time (s)
(a) Enclosure of the first joint angle. (b) Enclosure of the second joint angle.
Figure 4: Interval enclosures for the first and second state variable of the double pendulum.
namic simulation. More close-to-life examples are 2: ([-3.142;-3.142];[-0.000; 0.000])

treated in (Auer et al., 2006b), (Auer, 2006), (Auer 3: ([-0.000; 0.000];[-3.142;-3.142])
et al., 2004). 4: ([-0.000; 0.000];[-0.000; 0.000])
At last, consider the previous example once again Since we do not have any uncertainties in the model,
but without the uncertainty in β1 . To find equilibrium the intervals obtained are very close to point intervals,
states of this system, we use the basic data type that is, βi ≈ βi . The difference is noticeable only af-
MoFInterval instead of TMoInterval and ap- ter the 12-th digit after the decimal point. However, if
ply MoIGradientStaticEquilibriumFinder the same problem is modeled using the non-verified
to the element manipulator instead of us- model in MOBILE, only one (unstable) equilibrium
ing TMoMechanicalSystem and an integra- state [β1 , β2 ] =[3.142;-3.142] is obtained (using
tor. All four possible equilibria (stable and the identical initial guess).
unstable) are found by the validated solver
MoIGradientStaticEquilibriumFinder in
the starting interval [−3.15; 1] for all coordinates
(shown rounded up to the third digit after the decimal
5 CONCLUSIONS
point):
In this paper, we presented a recently developed tool
1: ([-3.142;-3.142];[-3.142;-3.142]) S MART MOBILE for guaranteed modeling and simu-
115
lation of kinematics and dynamic of mechanical sys- Hammer, R., Hocks, M., Kulisch, U., and Ratz, D. (1995).
tems. With its help, the behavior of different classes C++ toolbox for verified computing I - Basic Numer-
of systems including non-autonomous and closed- ical Problems. Springer-Verlag, Heidelberg and New
York.
loop ones can be obtained with the guarantee of cor-
rectness, the option which is not given in tools based Hörsken, C. (2003). Methoden zur rechnergestützten
Toleranzanalyse in Computer Aided Design und
on floating point arithmetics. Besides, the uncertainty
Mehrkörpersystemen. PhD thesis, Univerisity of
in parameters can be taken into account in a natural Duisburg-Essen.
way. Moreover, S MART MOBILE is flexible and al-
Huckle, T. (2005). Collection of software bugs.
lows the user to choose the kind of underlying arith- www5.in.tum.de/∼huckle/bugse.html.
metics according to the task at hand. The functional-
Kecskeméthy, A. (1993). Objektorientierte Modellierung
ity of the tool was demonstrated using three examples. der Dynamik von Mehrkörpersystemen mit Hilfe von
The main directions of the future development Übertragungselementen. PhD thesis, Gerhard Merca-
will include enhancement of validated options for tor Universität Duisburg.
modeling and simulation of closed-loop systems in Kecskeméthy, A. and Hiller, M. (1994). An object-oriented
S MART MOBILE as well as integration of further approach for an effective formulation of multibody
verified solvers into its core. dynamics. CMAME, 115:287–314.
Klatte, R., Kulisch, U., Wiethoff, A., Lawo, C., and Rauch,
M. (1993). C–XSC: A C++ Class Library for Ex-
tended Scientific Computing. Springer-Verlag, Berlin
REFERENCES Heidelberg.
Knüppel, O. (1994). PROFIL/BIAS — a fast interval li-
Auer, E. (2006). Interval Modeling of Dynamics for Multi- brary. Computing, 53:277–287.
body Systems. In Journal of Computational and Ap-
plied Mathematics. Elsevier. Online. Lerch, M., Tischler, G., Wolff von Gudenberg, J., Hofschus-
ter, W., and Krämer, W. (2001). The Interval Library
Auer, E., Kecskeméthy, A., Tändl, M., and Traczinski, H. filib++ 2.0 : Design, Features and Sample Pro-
(2004). Interval Algorithms in Modeling of Multibody grams. Technical Report 2001/4, Wissenschaftliches
Systems. In Alt, R., Frommer, A., Kearfott, R., and Rechnen / Softwaretechnologie, Bergische Universität
Luther, W., editors, LNCS 2991: Numerical Software GH Wuppertal.
with Result Verification, pages 132 – 159. Springer,
Berlin Heidelberg New York. Lohner, R. (1988). Einschließung der Lösung gewönlicher
Anfangs- und Randwertaufgaben und Anwendungen.
Auer, E., Rauh, A., Hofer, E. P., and Luther, W. (2006a). PhD thesis, Universität Karlsruhe.
Validated Modeling of Mechanical Systems with
S MART MOBILE: Improvement of Performance by Lohner, R. (2001). On the ubiquity of the wrapping effect
VAL E NC IA-IVP. In Proc. of Dagstuhl Seminar in the computation of the error bounds. In Kulisch,
06021: Reliable Implementation of Real Number Al- U., Lohner, R., and Facius, A., editors, Perspectives
gorithms: Theory and Practice, Lecture Notes in on Enclosure Methods, pages 201–217. Springer Wien
Computer Science. To appear. New York.
Metropolis, N. and Ulam, S. (1949). The monte carlo
Auer, E., Tändl, M., Strobach, D., and Kecskeméthy, A. method. Journal of the American Statistic Associa-
(2006b). Toward validating a simplified muscle action, 44:335–341.
tivation model in SMARTMOBILE. In Proceedings of
SCAN 2006. submitted. Moore, R. E. (1966). Interval Analysis. Prentice-Hall, New
York.
Berz, M. and Makino, K. (1998). Verified integration of
ODEs and flows using differential algebraic methods Nedialkov, N. S. (1999). Computing rigorous bounds on
on high-order Taylor models. Reliable Computing, the solution of an initial value problem for an ordi-
4:361–369. nary differential equation. PhD thesis, University of
Toronto.
Berz, M. and Makino, K. (2002). COSY INFINITY Version
8.1. User’s guide and reference manual. Technical Re- Nedialkov, N. S. (2002). The design and implementation
port MSU HEP 20704, Michigan State University. of an object-oriented validated ODE solver. Kluwer
Academic Publishers.
de Figueiredo, L. H. and Stolfi, J. (2004). Affine arith-
metic: concepts and applications. Numerical Algo- Neumaier, A. (2002). Taylor forms — use and limits. Reli-
rithms, 37:147158. able Computing, 9:43–79.
Stauning, O. and Bendtsen, C. (2005). Fadbad++. Web
Griewank, A. (2000). Evaluating derivatives: principles
page http://www2.imm.dtu.dk/∼km/FADBAD/.
and techniques of algorithmic differentiation. SIAM.
Traczinski, H. (2006). Integration von Algorithmen und Da-
Griewank, A., Juedes, D., and Utke, J. (1996). ADOL– tentypen zur validierten Mehrkrpersimulation in MO-
C, a package for the automatic differentiation of algo- BILE. PhD thesis, Univerisity of Duisburg-Essen.
rithms written in C/C++. ACM Trans. Math. Software,
22(2):131–167.
116
GRASP CONFIGURATION MATCHING
Using Visual and Tactile Sensor Information
Madjid Boudaba
Design Center
TES Electonic Solution GmbH, Zettachring 8, 70567 Stuttgart, Germany
[email protected]
Alicia Casals
GRINS: Research Group on Intelligent Robots and Systems
Technical University of Catalonia, Pau Gargallo 5, 08028 Barcelona, Spain
[email protected]
Keywords: Visual image, Tactile image, Grasp planning, Block matching algorithm.
Abstract: Finding the global shape of a grasped object directly from touch is time consuming and not highly reliable.
This paper describes the relationship between visual features and grasp planning, and correlates visual and tac-
tile information for a better description of the object’s shape and grasping points determination. The grasping
process proposed is experimented with a three fingered robotic hand.
1 INTRODUCTION 2001), an image also contains redundant information

that could become a source of errors and inefficient
Grasping has been an active area of robotics research in the processing.
in the last decades. Although a great number of
sensory systems have been used to monitor and to This paper is an extension of our previous work
control grasping, their usefulness is often limited (Boudaba et al., 2005) and (Boudaba and Casals,
by the ability of handling all aspects of detec- 2006) on grasp planning using visual features. In
tion/recognition, guidance, alignment and grasping. this work, we demonstrate its utility in the context of
To place our approach in perspective, we review grasp (or fingers) positioning. Consider the problem
existing methods for sensor based grasp planning. of selecting and executing a grasp. In most tasks,
The existing literature can be broadly classified into one can expect various uncertainties. Grasping an
two categories; vision based and tactile based. For object implies building a relationship between the
both categories, the extracted image features are of robot hand and the object model. The latter is often
concern, they can range from geometric primitives unavailable or poorly known. Thus, selecting a grasp
such as edges, lines, vertices, and circles to optical position from such model can be unprecise or unprac-
flow estimates. The first category uses image features ticable in real time applications. In our approach, we
to estimate the robot’s motion with respect to the avoid using any object model and instead we work
object pose (Maekawa et al., 1995), (Smith and directly from edge features. In order to avoid fingers
Papanikolopoulos, 1996), (Allen et al., 1999). Once positioning errors, a sizable image blocks are defined
the robot hands is already aligned with the object, that represent the features of grasping contact points.
then, it only needs to know where the fingers are This not only avoids detection/localization errors
placed on the object. The second category of sensor but also saves computation effort that could affect
uses image features to estimate the touch sensing the reliability of the system. Our features matching
area in contact with the object (Berger and Khosla, based approach can play the critical role of forcing
1991), (Chen et al., 1995), (Lee and Nicholls, 1999). the fingers to move to the desired positions before
A practical drawback is that the grasp execution the task of grasping is executed. To achieve a high
is hardly reactive to sensing errors such as finger level of matching efficiency, the visual image is first
positioning errors. A vision sensor, meanwhile, divided into squared blocks of pixels. Then for each
is unable to handle occlusions. Since an object is one of these blocks the algorithm tries to find its
grasped according to its CAD model (Kragic et al., correspondence in the target block that is the closest
117
to it according to a predetermined criterion. Finally, precision grasps, where only the fingertips are in
we reduce or eliminate redundant information con- contact with the object. After discussing the friction
tained in the image by transforming the result of the cone modeling, a formalizme is used to analyze force
matching algorithm to the frequency domain. Then a closure grasps using the theory of polyhedral convex
compression scheme is proposed to the image coding. cones.
The proposed work is divided into two phases: x2

1. Grasp planning phase: For each two- li
CCW
Q u1
dimensional view of an object in a visual
u3
frame features of its contour are calculated. gi
These features are then used as input data, both vci
Ci Ci
for the grasp planning and features matching
phases. The grasping positions are generated u2 x1
in the planning task, so a relationship between (a) (b)
visual features and grasp planning is proposed.
Then a set of geometrical functions is analysed to Figure 1: Friction cone modelleing.
find a feasible solution for grasping. The result of
grasp planning is a database containing a set of 2.1 Friction Cone Modelling
valid grasps, the most favorable as well as those
rejected. For the analysis of the contact forces in planar grasps,
2. Sensor features matching phase: Unlike vision we simplify the problem by linearizing the friction
which provides global features of the object, tac- cone by a polyhedral convex cone. In the plane, a
tile sensor provides local features when the fin- cone has the appareance shown in Figure 1(b). This
gertip is in touch with the object. In order to iden- means that we can reduce the number of cone sides,
tify and locate the features that best fit the two m=6 to one face.
domains (vision and touch) of features, a contour
splitting process divides the object’s contour into Let’s denote by P, the convex polytopes of a face
blocks, so that different matching techniques can cone, and {u1 , u2 , u3 } its three representative vertices.
be applied. For the purpose of features match- We can define such polytopes by
ing, extracting edge features are of concern using
( )
the basic results from different approaches. The 3 3
matching is conducted in two-dimensional space. P= x = ∑ δi ui : 0 ≤ δi ≤ 1, ∑ δi = 1 (1)
Each edge in the block is treated as features. i=1 i=1
2.2 Grasp Space Evaluation

2 GRASP BACKGROUND
The full space of a grasp is evaluated by analysing its
Geometric formulation and grasp feasibility are re- convex hull. For a set of friction cone intersections,
viewed and discussed based on (Hirai, 2002). Given the full space can be defined by
a grasp which is characterized by a set of contact
points and the associated contact models, the problem C1k = C(P1 ) ∩C(P2 ) ∩C(Pk ) (2)
is determining whether the grasp has a force-closure.
where k is the number of grasping contacts. Note
For finger contact, a commonly used model is point
that the result of C1k is a set of friction cone intersec-
contact with friction (PCWF). In this model, fingers
tions and produces either an empty set or a bounded
can exert any force pointing into the friction cone at
convex polytope. Therefore, the solution of (2) can be
the edge of contacts (We use edge contact instead
expressed in terms of its extreme vertices
of point contact, which can be described as a linear
combination of two vectors, see Figure 1(b)). To (v )
p vp
fully analyze grasp feasibility, we need to examine
∑ αi uci , ∑ αi = 1,
v
the full space of forces acting on the object. Forming Ω1p (U) = αi ≥ 0 (3)
i=1 i=1
the convex hull of this space is difficult due to the
nonlinear friction cone constraints imposed by the where v p is the total number of extreme vertices.
contact models. In this section, we only focus in
118
GRASP CONFIGURATION MATCHING - Using Visual and Tactile Sensor Information
Tactile Layer Image acquisition and processing Vision Layer

Figure 2 illustrates an example of feasible solu-
v
tion of Ω1p (U) and its grasp space represented by its Tactile Image (S) Visual Image (V)
extreme vertices P = {v1 , v2 , ..., v5 }. Edge detection

Edge detection
and contour extraction and contour extraction
Three friction cones intersection: C13
Convex polytope: P
f x2 Computation of line segments Computation of line segments

3 C3 n
1
v5 v4
C
2 v
n2 f 3 Segment grouping Contour approximation using
2 polygonal approximation
n3 v1 v2
C1
Grasping Contour splitting into
Region x1 macroblocks
f
1
(a) (b)
Tactile and Visual Features Matching.
Figure 2: Feasible solution of a three-fingered grasp.

Figure 3: Tactile and visual features data processing.
3 FEATURES-BASED GRASPING points is that both, high data compression and feature
extraction can be achieved. Other works prefer the
In robotic grasping tasks, when several input sensors method of polygonal approximation using linking
are available simultaneously, it is generally necessary and merging algorithms (Rosin, 1997) and curvature
to precisely analyze all the data along the entire scale space (CSS).
grasping task. We can acquire the precise position
and orientation of the target object and robot hand We denote by V a function regrouping parameters
motion from a vision system and can acquire force, of visual features defined by
torque, and touch information from tactile sensing.
The object being extracted from a video sequence V = {vlist, slist, llist, com} (4)
requires encoding its contour individually in a where vlist and slist are the lists of consecutive
layered manner and provide at the receiver’s side contour’s vertices and segments, respectively. llist
an enhanced accessibility of visual information. is a list containing the parameters of segments,
In the same way, for the object being extracted calculated with respect to the object’s center of mass,
from a tactile sensor, the tactile layer processes com. The resulting parameters of V fully describe
and provides the tactile information at its receiver’s the two-dimensional location of features with respect
side. Obviously, the accuracy of this data is of signif- to the image plane. The visual features obtained can
icant importance for the eventual matching algorithm. be used as input data for both, grasp planning and
features matching for grasp control.
Figure 3 illustrates a layered composition of
a tactile and a vision sensor. Given as input two
consecutive images data S (tactile) and V (visual), the
success (or simply the completion time) of the task
depends on the level of processing efficiency.
3.1 Visual Features Extraction

Due to their semantically rich nature, contours are Figure 4: Visual features extraction.
one of the most commonly used shape descriptors,
and various methods for representing the contours
of 2D objects have been proposed in the literature 3.2 Tactile Features Extraction
(Costa and Cesar, 2001). Extracting meaningful
features from digital curves, finding lines or seg- Unlike vision which provides global features of the
ments in an image is highly significant in grasping object, tactile sensor provides local features when the
applications. Most of the available methods are fingertip is in touch with the object. To simplify the
variations of the dominant point detection algorithms problem, tactile features are treated as visual features
(M. Marji, 2003). The advantage of using dominant using the basic results from different approaches (Lee
119
and Nicholls, 1999). For the purpose of sensor fea- quite time consuming. Thus a preprocessing (or pre-
tures matching, extracting edge features are of inter- filtering) is necessary before the grasping points gen-
est. Figure 5 illustrates three examples of tactile sen- eration takes place. A fingertip is estimated to be as a
sor in touch with the object. From left to right side, sphere with radius fr (see Figure. 2(b)), a grasping re-
the sensitive area is shown with hashed region and lo- gion must be large enough for placing contact points
cated at upper corner side, bottom side and covering on it. Hence, a prefiltering is applied to the list, slist
the entire object area, respectively. The tactile sensor defined in (4) to discard those segments with length
device consists of a printed circuit board with a tactile less than the diameter of the sphere (si < 2fr ). Fig-
array of electrodes (called tactels) on its surface and ure. 4 illustrates the result of prefiltering processes as
circuitry to deliver the tactile data of local features described by the following equation:
extracted from the sensing elements, as well as circu-
ity to monitor and adjust the power supply voltage to glist = {g1 , g2 , · · · , gm } (6)
the sensor. The raw data from the sensing elements is
processed and produces as output a vector S contain- where glist is a linked list of grasping regions and
ing the parameters that define tactile features. m its number.
S = {elist, slist, plist} (5) A very important aspect of (6) is the way how
knowledge about grasping regions are represented in
where elist and slist are list of consecutive con- the extraction process, knowledge that will be used
tour edges and segments, respectively. plist is a list for generating grasping points.
containing the parameters tied to segments, such as
location and orientation in the tactile sensor plane.
The following equation describes the relationship
between the visual features and grasp planning
G = f (glist, gparam, com) (7)

where glist, gparam, and com are the visual fea-
tures observed on the image plane and G is a grasp
map of outputs defined by the relationship between
fingers and the location of contact points on its cor-
responding grasping regions. From the grasp map G
three possible solutions are derived:

 Gs ={Gs1 , Gs2 , · · · , Gsis }
G: G = Gb1 , Gb2 , · · · , Gbib (8)
Object area Sensitive area  b
Gr = {Gr1 , Gr2 , · · · , Grir }
Figure 5: Sensitive area of the tactile sensor. This infor-
mation can be determined by examination of the frequency where Gs , Gb , and Gr are selected, best, and
domain, and is shown in Figure 10. rejected grasp, respectively. The is, ib, and ir are
the number of selected, best, and rejected grasps,
respectively.
4 GRASP PLANNING For a three-finger grasps, the selected grasps (Gs )

are given in the following form:
Grasp planning can be seen as constructing proce-
dures for placing point contacts on the surface of a 
given object to achieve force-closure grasps. Tak-  Gs1 = {( f1 , g1 ) , ( f2 , g6 ) , ( f3 , g9 )}

 Gs2 = {( f1 , g2 ) , ( f2 , g6 ) , ( f3 , g10 )}
ing as input the set of visual features extracted from
Gs : ..
the contour of the object, the output is a set of valid 
 .
grasps. The relationship between visual features and 
Gsis = {( f1 , g1 ) , ( f2 , g8 ) , ( f3 , g12 )}
grasp planning is given in the next section.
where fi and gi are the i − th finger and grasping
4.1 Grasp Point Generation region, respectively.
Generating a number of valid grasps from a list of A similar form can be given for representing the
candidates and classifying the best among them is best grasps Gb and those rejected Gr .
120
4.2 Algorithm the contour image into subregions in an efficient

way so that the matching algorithm can be applied.
The grasp planning algorithm is divided into several The basic principle is similar to the image (or video)
procedures and operates as follows: compression techniques, which defines how the
component (RGB, YUV or YCrCb) can be down-
1. Visual features procedure
sampled and digitalised to form discrete pixels. The
• Function grouping visual features using (4) terms 4:2:2 and 4:2:0 are often used to describe the
2. Grasping point generation procedure sampling structure of the digital image. 4:2:2 means
the chrominance is horizontally sub-sampled by a
• Pick three grasp regions from (6) factor of two relative to the luminance; 4:2:0 means
• Determine the initial position of f1 , f2 and f3 the chrominance is horizontally and vertically sub-
• Compute their friction cones using (1) sampled by a factor of two relative to the luminance.
• Compute the friction cones intersection of (2) In the case of a 704x576 PAL/SECAM standard for
example, the QCIF (Quarter Common Immediate
3. Grasping test procedure Format) can be obtained by scaling down the image
• Compute the solution friction cones using (3) with a factor of 4 in the horizontal/vertical direction.
• Check whether the polytopes given by (3) is For a QCIF format of size 176x144, there are 25.344
bounded. If so, stop and save the selected pixels in the frame. A macro block defines a 16x16
grasps to Gs . pixel area (256 pixels), so there are 99 macro blocks
• Else save the rejected grasps to Gr . to process (see Figure. 6).
4. Quality test procedure
• The last step of the algorithm consists of select-
ing the best grasps from a range of valid grasps
from lower to upper acceptance measures by
using the parameters measure given in table 1.
Save to Gb .
Macro Block (MB)

5 FEATURES MATCHING 16x16 pixels
Object contour:
Our goal is to match the grasping positions correspon- QCIF: 176 x 144 pixels
dence between the visual and tactile sensor features.
Figure 6: Image subregions.
The matching process works first getting a grasping
position within its searching area and next it updates
the tactile features using a tactile sensor. The size of 5.2 Block-Matching Algorithm
the search windows is very important when configur-
ing a matching process. The larger the search window, The Mean Absolute Difference (MAD) is a well
the longer it takes to process the search. The match- known matching criteria and widely used due to its
ing is conducted in the pixel domain, so the contrast lower computational complexity (Lu and Liou, 1997).
is necessary for identifying edges in reference (visual) Given two blocks represented by two set of features:
and target (tactile) image. Images with weak contrast S = a1 , a2 , ..., aq and V = b1 , b2 , ..., bq , the corre-
should be avoided since the matching algorithm uses sponding features from each block are compared and
the edges based searching. The weaker the contrast, their differences accumulated, as described by equa-
the less the amount and accuracy of edge-based infor- tion
mation with which the searching is performed. Fig-
ure 7 shows two tables in grayscale values assigned
1 N N
to each block of matching. MAD (dx, dy) = ∑ ∑ |V [i, j] − S [i + dx, j + dy]|
N 2 i=1 j=1
5.1 Image Subregions (9)
where S(i, j) represents a (N×N) macroblock of
In order to identify the location of the best fitting pixel intensity in the target frame and V (i, j) repre-
between the tactile sensor frames and visual frames, sents (N×N) macroblock of pixel intensity in the ref-
a subregion process is performed that scales down erence frame. (dx, dy) is a vector representing the
121
Searching area (2p+N)x(2p+N)

42 14 27 35 49 44 30 24 24 (00011000 )
49 49 35 28 49 20 24 30 2
21 35 35 42 42 28 28 42
56 70 77 84 95 28 28 21 Searching area (2p+N)x(2p+N)
70 26 13 14 16 95 35 14 Pixels valaues (0~255)
Reference 26 20 18 18 17 17 35 21
16
40 49 49 35 28 35 35 35
20 18 24 21 18 84 21 21
16
Current
MacroBlock
p=6
2p + N
v
p=6
2p + N
Mean
Image Bitstream output
Absolute
Difference
Target Difference
Coding (IDC)
(MAD)
First step second step third step v Searching vector
12 14 27 35 19 14 24 10 Figure 8: Searching area.

9 4 45 28 49 20 24 30
1 35 15 42 49 20 24 42
Pixels valaues (0~255)
16 30 17 44 35 28 28 21 sensors.
20 26 13 14 16 25 35 14
26 20 18 18 17 17 15 21
40 49 19 35 28 15 15 10
20 18 19 21 18 44 10 5 The result of block matching algorithm (see
Figure 8) is a two-dimensional vector called motion
Figure 7: Block matching algorithm. vector, v(l, m). The Image Difference Coding (IDC)
processes these measurements and produces as output
search area. For our application the search area is a vector image containing the parameters of grasping
specified by dx = (−p, p) and dy = (−p, p) where positions, which are compressed in a suitable format
p = 6 and N = 16. to reduce the data transmission bandwidth. The
The matching of a macroblock with another is digital cosine transform (DCT) is used due to its
based on the output of a cost function (9). The mac- capability of removing spatial redundancy to achieve
roblock that results in the least cost is the one that low bit rates.
matches the closest to current macroblock, Figure 7.
For each fingertip that gets in touch with the object, v (l , m) g (u, v)
the tactile features are matched to those of visual fea- 42
49
28
49
20
35
35
28
49
49
24
20
24
24
30
30
70
-53
24
-35
-28 -4
43 13
-2 -10
7 13
-1 0
1 3
tures inside a predefined searching area. A motion 21
56
35
70
35
77
42
84
42
95
28
28
28
28
42
21 DCT
23
6
9
2
-10 -8
-2 8
-7 -6
2 -1
5 -3
0 -1
-10 -2 -1 -12 2 1 -1 4
vector is then applied to search the features correspon- 70
26
12
20
33
18
47
18
61
75
95
75
35
35
14
21 3 0 0 11 -4 -1 5 6
-3 -5 -5 -4 3 2 -3 5
dence between blocks in the target frame and those in 40
20
49
18
49
45
35
21
28
82
35
84
35
21
35
21 3 0 4 5 1 2 1 0
the reference frame. Figure 8 illustrates the searching

method that evaluates the MAD cost function within Figure 9: DCT Image compression.
the search area. Many other search methods can be
found in (Furht, 1995). The DCT transforms each 8x8 block of greyscale
values into a block of 8x8 spectral frequency coeffi-
cients. The energy tends to be concentrated into few
5.3 Image Difference Coding significant coefficients. Other coefficients are close to
zero and insignificant (see Figure 9).
In order to control the grasping position, the result of Next step of IDC is to compress the frequency do-
the matching algorithm can be defined as error posi- main, by not transmitting (or not coding) the close-
tion or that so called error grasp, is then calculated zero coefficient (insignificant coefficients) and by
using the following expression: quantizing and coding the remaining coefficients.
Ge (i, j) = V (i, j) − S(i + dx, j + dy) (10)

Since we want to guide the robot hand towards 6 EXPERIMENTAL RESULTS
these grasping references Gre f , the solution consists
of reducing the grasp error Ge by moving the tactile Figure 10 illustrates our experimental system which
sensors towards the set of corresponding positions consists of a anthropomorphic robot hand equipped
of grasping references. The cost of a solution is with a tactile array sensor on each fingertip and a
expressed as the total sum of contact displacements stereo vision system. The spacial resolution of the
over the surface of the object from an initial contact tactile sensor has 256 (16x16) sensing cells over an
configuration. If the result of matching is outside a area of 100 square millimeter. The sensory data pro-
given margin, then the grasp controller should launch cessing were performed using MCAGUI and SVS
a new measurement via joint angle and position tools for tactile and vision data, respectively, devel-
122
oped at the Institute of Process Control and Robotics

(Boudaba et al., 2005). Figure 10(b) shows the tac-
tile sensor response frames. Every cell of the sensing
matrix is sampled at 10 frames per second. Figure 11
and Figure 12 show the result of five grasp configura-
tions and Table 1 resumes their parameter measures.
d1 , d2 and d3 are distance measures of finger posi-
(a) (b)
tion f1 , f2 and f3 from the object’s center of mass.
x1 x2 are the coordinates of the focus point F in the
plane. d is the measured distance between focus point
and center of mass. R is the vector radius of the ball
centered at F. The object center of mass is located
at com = (121.000, 98.000. The angle of the friction
cone, α= 17.000 for all configurations. We have im-
plemented the grasp planning algorithms in Matlab
environment for computing feasible grasping regions (c) (d)
for three-finger hands. Figure 11: Grasp planning setup.
Table 1: Parameter measures of five grasp configurations.
ob j = {GC1, GC2, GC3, GC4, GC5}

d1 d2 d3 F(x1 , x2 ) d R
86.80 63.70 35.52 119.97 96.95 1.80 7.94
86.80 33.82 65.99 118.41 96.69 2.98 9.37
24.47 86.80 23.82 99.19 122.88 32.53 2.44
(a) Three fingers hand in contact with the object 23.82 33.82 71.22 127.26 102.97 7.88 5.34
81.51 65.99 35.52 114.59 84.46 15.39 4.49
der to provide a suitable description of object con-

tour, a method for grouping visual features has been
(b1) Full frame
proposed. Then a function defining the relationship
between visual features and grasp planning has been
described. A very important aspect of this method is
the way knowledge about grasping regions are rep-
resented in the extraction process, which excluded
all undesirable grasping points (unstable points) and
(b2) Individual frame
all line segments that do not fit to the fingertip posi-
Figure 10: Tactile sensor response frames. tion. This filtering process has been used to reduce the
time consumption during the process of determining
suitable grasping points. For extracting the local fea-
tures of the curves representing the object contour, the
7 CONCLUSION solution adopted is a polygonal approximation using
a linking/merging algorithms.Then the force-closure
A framework to discuss sensor frames matching us- condition is applied to evaluate grasping points deter-
ing tactile and visual features is presented in this pa- mination. The method implemented here is currently
per. As a new approach to the grasp planning prob- restricted to any kind of 2D objects. In a future work,
lem, the idea of vision-based grasping has been exit is intended to extend our method to 3D object.The
plored, within the specific context of using visual fea- object therefore needs to be seen from different points
tures relevant to the grasping and manipulation tasks, of view which is desirable for grasp planning that per-
as complementary information to tactile data. In or- forms well in the real world.
123
(a) (b) (a) (b)
(c) (d) (c) (d)
(a) Grasp Configuration(GC): 2-3 (b) Grasp Configuration(GC): 4-5
Figure 12: Result of Grasp planning with three-fingered Grasps.
ACKNOWLEDGEMENTS Furht, B. (1995). A survey of multimedia compression

techniques and standards, part ii: Video compression.
The authors would like to thank Prof. Dr. H. Wo- Real-Time Imaging Journal, 1:319–337.
ern and his co-workers from the IPR institute for Hirai, S. (2002). Kinematics of manipulation using the the-
their support in providing the facilities and the an- ory of polyhedral convex cones and its application to
grasping and assembly operations. Trans. of the Soci-
thropomorphic robot hand for testing the proposed ap- ety of Inst. and Control Eng., 2:10–17.
proach.
Kragic, D., Miller, A., and Allen, P. (2001). Real-time
tracking meets online grasp planning. In Proc. IEEE
International Conference on Robotics and Automata-
REFERENCES tion (ICRA’2001), pages 2460–2465, Seoul, Korea.
Lee, M. H. and Nicholls, H. R. (1999). Tactile sensing for
Allen, P., Miller, A., Oh, P., and Leibowitz, B. (1999). Inte- mechatronics - a state of the art servey. Mechatronics,
gration vision, force and tactile sensing for grasping. 9:1–31.
Int. Journal of Intell. Mechatronics, 4(1):129–149. Lu, J. and Liou, M. L. (1997). A simple and efficient
Berger, A. D. and Khosla, P. K. (1991). Using tactile data search algorithm for block matching motion estima-
for real-time feedback. International Journal of of tion. IEEE Trans. Circuits And Systems for Video
Robotics Research (IJR’91), 2(10):88–102. Technology, 7:429–433.
Boudaba, M. and Casals, A. (2006). Grasping of planar M. Marji, P. S. (2003). A new algorithm for dominant points
obbjects using visual perception. In Proc. IEEE 6th detection and polygonization of digital curves. Jour-
International Conference on Humanoid Robots (HU- nal of the Pattern Recognition Society, 36:2239–2251.
MANOIDS’06), pages 605–611, Genova, Italy. Maekawa, H., Tanie, K., and Komoriya, K. (1995). Tac-
Boudaba, M., Casals, A., Osswald, D., and Woern, H. tile sensor based manipulation of an unknown object
(2005). Vision-based grasping point determination by a multifingered hand with rolling contact. In Proc.
on objects grasping by multifingered hands. In Proc. IEEE International Conference on Robotics and Au-
IEEE 6th International Conference on Field and Ser- tomatation (ICRA’95), pages 743–750.
vice Robotics (FRS’05), pages 261–272, Australia. Rosin, P. L. (1997). Techniques for assesing polygonal ap-
Chen, N., Rink, R. E., and Zhang, H. (1995). Edge track- proximation of curves. IEEE Transaction on Pattern
ing using tactile servo. In Proc. IEEE/RSJ Interna- Analysis and Machine Intelligence, 19:659–666.
tional Conference on Intelligent Robots and Systems
Smith, C. and Papanikolopoulos (1996). Vision-guided
(IROS’95), pages 84–99.
robotic grasping: Issues and experiments. In Proc.
Costa, L. and Cesar, R. (2001). Shape Analysis and Clas- IEEE International Conference on Robotics and Au-
sification Theory and Practice. CRC Press, Florida, tomatation (ICRA’96), pages 3203–3208.
USA, 1st edition.
124
Xiang Li and Andreas Zell

Wilhelm-Schickard-Institute, Departmant of Computer Architecture, University of Tübingen
Sand 1, 72076 Tübingen, Germany
Keywords: Mobile robots and autonomous systems, system identification, actuator saturation, path following control.
Abstract: This paper focuses on the motion control problem of an omnidirectional mobile robot. A new control method
based on the inverse input-output linearized kinematic model is proposed. As the actuator saturation and
actuator dynamics have important impacts on the robot performance, this control law takes into account these
two aspects and guarantees the stability of the closed-loop control system. Real-world experiments with an
omnidirectional middle-size RoboCup robot verifies the performance of this proposed control algorithm.
1 INTRODUCTION high accuracy and ground is planar enough, kinematic

models are widely used in designing robots behav-
Recently, omnidirectional wheeled robots have re- iors because of the simpler structures. As the inputs
ceived more attention in the mobile robots applica- of kinematic models are robot wheels velocities, and
tions, because the omnidirecitional robots “have full outputs are the robot linear and angular velocities,
mobility in the plane, which means that they can move the actuator dynamics of the robot are assumed fast
at each instant in any direction without any reorien- enough to be ignored, which means the desired wheel
tation ”(Campion et al., 1996). Unlike the nonholo- velocities can be achieved immediately. However, the
nomic robots, such as car-like robots, having to rotate actuator dynamics limit and even degrade the robot’s
before implementing any desired translation velocity, performance in real situations.
omnidirecitonal robots have higher maneuverability Another important aspect of robot control in prac-
and are widely used in dynamic environment appli- tice is actuator saturation. Because the commanding
cations, for example, in the middle-size league of the motor speeds of the robot’s wheels are bounded by
annual RoboCup competition. the saturation limits, the actuator saturation can af-
Most motion control methods of mobile robots are fect the robot’s performance, even make robots mo-
based on robots dynamic models (Watanabe, 1998; tion become unstable (Indiveri et al., 2006; Scolari
Liu et al., 2003; Purwin and Andrea, 2006; Tsai et al., Conceição et al., 2006).
2006) or robots kinematic models (Muir and Neu- This paper presents a motion control method for
man, 1990; Terashima et al., 2004; Rojas and Förster, an omnidirectional robot, based on the inverse input-
2006). A dynamic model directly describes the rela- output linearization of the kinematic model. It takes
tionship between the forces exerted by the wheels and into account not only the identified actuator dynamics
the robot movement, with the applied voltage of each but also the actuator saturation in designing a con-
wheel the input and the robot movement in terms of troller, and guarantees the stability of the closed-loop
linear and angular accelerations as output. But the dy- control system.
namic variations caused by the changes in the robot’s The remainder of this paper introduces the
inertia moment and perturbations from the mechanic kinematic model of an omnidirectional middle-size
components (Scolari Conceição et al., 2005) make the Robocup robot in section 2; Path following and ori-
controller design more complex. With the assump- entation tracking problems are solved based on the
tion that no slippage of wheels occurs, sensors have inverse input-output linearized kinematic model in
125
section 3, where the actuator saturation is also ana- tem as

lyzed; section 4 presents the identification of actua- w cos θ − sin θ
Rm = , (2)
tor dynamics and their influence on the control per- sin θ cos θ
formance; Finally the experiment results and conclu- the kinematic model with respect to the world co-
sions are discussed in sections 5 and 6, respectively. ordinate system is deduced as:
 
3 cos(θ + δ) − 32 cos(θ − δ)
2 2
3 sinθ
2 ROBOT KINEMATIC MODEL ẋ =  3 sin(θ + δ) − 32 sin(θ − δ) − 23 cosθ  q̇,
2
1 1 1
3L 3L 3L
The mobile robot used in our case is an omnidirec- (3)
tional robot, whose base is shown in figure 1. It has where ẋ = [ẋR ẏR θ̇]T is the robot’s velocity vector
three Swedish wheels mounted symmetrically with with respect to the world coordinate system; ẋR and ẏR
120 degrees from each other. Each wheel is driven are the robot translation velocities; θ̇ is the robot ro-
by a DC motor and has a same distance L from its tation velocity; δ refers to the wheel orientation in the
center to the robot’s center of mass R. robot coordinate system and is equal to 30 degrees.
It is important to notice that the transformation
matrix in model 1 is full rank, which denotes that
the translation and rotation of the robot are decou-
pled, and guarantees the separate control of these two
movements.
For the high level control laws without consider-
ing the wheel velocities, the kinematic model
ẋ = Gv (4)
is used in our control method, where the transforma-
tion matrix G is equal to [w Rm 0 ; 0 1]. Because G is
full rank, the characteristics of decoupled movement
Figure 1: Kinematics diagram of the base of an omnidirec-
is also kept.
tional robot.
Besides the fixed world coordinate system 3 INVERSE INPUT-OUTPUT

[Xw ,Yw ], a mobile robot fixed frame [Xm ,Ym ] is de-
fined, which is parallel to the floor and whose origin
LINEARIZATION BASED
locates at R. θ denotes the robot orientation, which is CONTROL
the direction angle of the axis Xm in the world coor-
dinate system. α and ϕ denote the direction of the The trigonometric functions of angle θ in the transfor-
robot translation velocity vR observed in the world mation matrix G determine the nonlinearities of the
and robot coordinate system, respectively. The kine- kinematic model 4. Since the matrix G is full rank,
matic model with respect to the robot coordinate sys- this nonlinear model can be exactly linearized by in-
tem is given by : troducing a simple compensator C = G−1 . The lin-
 √ √  earized system becomes ẋ = u with a new input vector
3/3 − 3/3 0 u = [u1 u2 u3 ]T .
v =  1/3 1/3 −2/3  q̇, (1)
1/(3L) 1/(3L) 1/(3L)
where v = [ẋRm ẏm R ω] is the vector of robot veloc-

T
ities observed in the robot coordinate system; ẋRm and Figure 2: Linearized system by the component C.
R are the robot translation velocities; ω is the robot
ẏm
rotation velocity. q̇ is the vector of wheel velocities This linear system shown in figure 2 is completely
[q̇1 q̇2 q̇3 ]T , and q̇i (i = 1, 2, 3) is the i-th wheel’s ve- decoupled and allows the controlling of the robot’s
locity, which is equal to the wheel’s radius multiplied translation and rotation in a separate way. When a
by the wheel’s angular velocity. controller K is designed based on this simple linear
Introducing the transformation matrix from the system, the controller of the original system is gener-
robot coordinate system to the world coordinate sys- ated as CK. The overall control loop, which consists
126
stants. The time derivation of V results in

V̇ = K x ẋ + K θ̃ θ̃˙ .
d n n θ R R (6)
Figure 3: Closed-loop control system. Mojaev (Mojaev and Zell, 2004) presents a sim-
ple control law based on the deviation xn , where R
is controlled to move along an exponential curve and
of the nonlinear system, the compensator and the con- to converge to the axis Xt . The exponential curve is
troller, is shown in figure 3, expressed as
where x denotes the robot state vector [xR yR θ]T
and xd is the desired state vector; xR and yR are robot xn = xn0 exp(−kxt ), (7)
position observed in the world coordinate system.
where xn0 is the initial deviation and the positive con-
Based on this input-output linearized system, path stant k determines the convergence speed of the devi-
following and orientation tracking problems are ana- ation. Differentiating (7) with respect to xt , we get the
lyzed with respect to the robot translation and rotation tangent direction of the exponential curve as
control in the following subsections. The influence of
actuator saturation is also accounted to keep the de- dxn
θ̃R = arctan( ) = arctan(−kxn ). (8)
coupling between the translation and rotation move- dxt
ments. Therefore, for a non-zero constant desired velocity vd ,
the translation velocity of robot in the coordinate sys-
3.1 Path Following Control tem xt Oxn results in
ẋn = vd sinθ̃R , (9)
As one high-level control problem, path following is
chosen in our case to deal with the robot translation ẋt = vd cosθ̃R . (10)
control. The path following problem is illustrated in Substituting the time derivative of θ̃R into (6), we
figure 4. P denotes the given path. Point Q is the get
orthogonal project of R on the path P. The path coor-
dinate system xt Qxn moves along the path P and the −ẋn
V̇ = Kd xn ẋn + kKθ arctan(−kxn ) < 0,
coordinate axes xt and xn direct the tangent and nor- 1 + (kxn )2
mal directions at point Q, respectively. θP is the path (11)
tangent direction at point Q. because xn ẋn = xn vd sin(arctan(−kxn )) < 0 and
ẋn arctan(kxn ) < 0. This solution of V̇ guarantees the
global stability of the equilibrium at xn = 0, θ̃R = 0,
which means this control law solves the path follow-
ing problem.
Transforming the robot velocity into the world co-
ordinate system, we get the control values of the lin-
earized system as
u1 = vd cos α, (12)
u2 = vd sin α, (13)
where α = θ̃R + θP .
The input of controller 12 and 13 is the devia-
Figure 4: Illustration of the path following problem. tion distance between point R and the given path,
which normally can be directly obtained by the sen-
Based on the above definitions, the path follow- sors on the robot. Moreover, the deviation converges
ing problem is to find proper control values of the smoothly to zero with the speed controlled by param-
robot translation velocity vR and angular velocity α̇ eter k, which can be chosen according to the perfor-
such that the deviation distance xn and angular error mance requirement.
θ̃R = α − θP tend to zero.
To solve this problem, a Lyapunov candidate func- 3.2 Orientation Tracking
tion
1 1
V = Kd xn2 + Kθ θ̃2R (5) Unlike a car-like wheeled robot, the orientation of an
2 2 omnidirectional robot can be different from the di-
can be considered, where Kd and Kθ are positive con- rection of the robot translation velocity by any angle
127
ϕ. This relationship is denoted as α = θ + ϕ. That where lb and ub are the low and up boundary.
means the robot orientation can track any angle when Considering the saturation function
the robot is following a given path. Based on the lin- 
 ub , if x1 > ub
earized model, the orientation tracking task is to find x2 = x1 , if lb ≤ x1 ≤ ub (18)
a suitable u3 , which is equal to the robot rotation ve-  l , if x < l ,
b 1 b
locity ω, such that
and its gain characteristics illustrated in figure 5, we
lim (θd (t) − θ(t)) = 0, (14) can take the saturation function as a dynamic gain
t→∞
block ka , which has maximum value one and con-
where θd (t) is the desired orientation. verges to zero when the input saturates. Then the
As the system between input variable u3 and out- closed-loop system of controlling the robot orienta-
put variable θ is an integrator, a commonly used PD tion is as shown in figure 6, in which a PD controller
controller can be designed to fulfill the orientation is used to control the robot orientation converging to
tracking task. the ideal θd ,
ω = k1 (eθ + k2 ėθ ), (19)
3.3 Actuator Saturation where k1 and k2 are the proportional and deriva-
tive gains, respectively. It can be obtained that the
Based on the inverse input-output linearization, the closed-loop has only one pole 1+k −ka k1
and one zero
translation and rotation of an omnidirectional robot a k1 k2
−1/k2 . Therefore, when k2 is negative and k1 is cho-
can be easily achieved in a separate way. This lin-
sen such that the pole is negative too, the stability of
earization is with respect to the input-output relation-
the closed-loop system can be guaranteed whenever
ship, which requires the internal parts having suffi-
ka decreases.
cient capability to achieve the desired inputs. How-
ever, the power of the robot’s motors is bounded and
the actuators will saturate when the commanding ve-
locities are too large. The presence of actuator satura-
tion can influence the decoupling between robot trans-
lation velocity and rotation velocity, such that the sys-
tem performance and stability is severely impacted.
Therefore, it is necessary to deal with the actuator sat-
Figure 5: Saturation function and its gain characteristics.
uration in the controller design.
For our omnidirectional robot, the maximal veloc-
ity of each wheel is limited by q̇m , namely |q̇i | ≤ q̇m .
Substituting the above control values from equations
(12) (13) and u3 into the inverse kinematic models (2)
and (1), the wheel velocities are computed as:
Figure 6: Closed-loop of robot orientation control.
   
q̇1 vd cos(α − θ − δ) + Lu3
 q̇2  =  −vd cos(α − θ + δ) + Lu3  , (15)
q̇3 vd sin(θ − α) + Lu3 4 ACTUATOR DYNAMICS
To achieve orientation tracking based on the above The results in the last section are only practical when
path following control, the desired translation veloc- we assume that the low level actuator dynamics is
ity’ magnitude Vd is assumed to be less than q̇m . Sub- faster than the kinematics, or the delay of actuator dy-
stituting q̇m into (15), the lower and upper boundary namics can be ignored. It is necessary to analyze the
of each wheel’s velocity (Lbi and Ubi ) can be calcu- actuator dynamics and take it into account when de-
lated from the following three inequalities, signing a controller. In the following subsections, the
actuator dynamics is identified based on the observed
|vd cos(α − θ − δ) + Lu3 | < q̇m input-output data, and its influence on the robot mo-
|−vd cos(α − θ + δ) + Lu3 | < q̇m (16) tion control is presented.
|vd sin(θ − α) + Lu3 | < q̇m ,
Then the dynamic boundary values of u3 are com- 4.1 Actuator Dynamics Identification
puted as
lb = max(lb1 , lb2 , lb3 ) The system identification problem is to estimate a
(17) model based on the observed input-output data such
ub = min(ub1 , ub2 , ub3 ),
128
Identified ARMAX model (na =3, nb =3, nc = 2, nk = 1)

that a performance criterion is minimized. Because 0.5
the full rank transformation matrix in the low level

R and ω
Y−component velocity (m/s)

dynamics model (1) denotes the outputs ẋRm , ẏm 0
are not relevant, we identify the actuator models for

these three values. The inputs of the actuator models −0.5
Rc and ωc ), and the

are required velocity values(ẋRmc , ẏm
−1
outputs are corresponding measured values. As one
commonly used parametric model, ARMAX is cho- −1.5
sen as the identified model, which has the following model outputs
measured outputs
structure −2
0 50 100 150 200 250 300
Data number
A(z)y(t) = B(z)u(t − nk ) +C(z)e(t), (20)
Figure 8: Identified model for ẏm
R.
A(z) = 1 + a1 z−1 + ... + ana z−na , (21)
B(z) = 1 + b1 z−1 + ... + bnb z−nb +1 , (22) 3
model outputs
2.5 measured outputs
C(z) = 1 + c1 z−1 + ... + cnc z−nc . (23)
Rotation velocity (rad/s)

2
nk denotes the delay from input u(t) to output y(t). 1.5
e(t) is white noise. z is the shift operator resulting 1
in q−1 u(t) = u(t − 1). na , nb and nc are the orders 0.5
of polynomials A(z), B(z) and C(z), respectively. To 0
choose the optimal parameters of this model, we use −0.5
the prediction error method, which is to find the op- −1
timal nk and parameters of A(z), B(z) and C(z) such −1.5

0 100 200 300 400 500 600 700
that the prediction error E is minimized, namely Data number
Figure 9: Identified model for ω.

N
[A(z), B(z),C(z), nk]opt = argmin ∑ E 2 (24) into continuous ones using ’zoh’(zero-order hold)
t=1 method,
8.7948(s + 58.47) m
E = yo (t) − A−1 (z)(B(z)u(t − nk) +C(z)e(t)), (25) ẋRm = ẏ , (26)
(s + 73.66)(s + 6.897) Rc
where yo (t) denotes the measured output data.
The system identification toolbox of Matlab has 2.4525(s + 48.83)(s + 6.185) m
been used to identify the actuator dynamics model. ẏm
R = ẏ (27)
(s + 28.45)(s2 + 6.837s + 25.97) Rc
Figures 7 8 and 9 show the optimal parameters and
comparison between models outputs and measured 1.667(s + 45.37)
outputs with respect to the actual inputs. ω= ωc . (28)
(s2 + 6.759s + 76.11)
2.5
4.2 Actuator Influence

X−component velocity (m/s)
1.5
With consideration of the actuator, the whole structure
of the control system is shown in figure 10,
1
0.5
model outputs
measured outputs
0
0 50 100 150 200 250 300
Figure 10: Closed-loop control system including actuator
Data number dynamics.
Figure 7: Identified model for ẋRm . m ẏm ω ] is the commanding robot
where Vc = [ẊRc Rc c
velocity vector with respect to the robot coordinate
To coincide with the robot’s continuous model, the system. Because the poles of the actuators dynam-
identified models are transformed from discrete ones ics (26) and (27) have negative real parts, these two
129
systems are stable. That means there exits a finite 5 EXPERIMENT

short time t ∗ , after which the real velocities ẊRm and
ẎRm can converge to the desired ones ẊRcm and Ẏ m , and
Rc The control algorithm discussed above has been
the inputs u1 and u2 begin to take effect. Therefore, tested in our robot laboratory having a half-field of the
the above path following law can also guarantee the RoboCup middle size league. The omnidirectional
robot approach to the reference path, although during robot used is shown in figure 13.
t ∗ the deviation distance xn and angular error θ̃R may
increase.
In the orientation tracking control, as the dynamic
system (28) adds another two poles to the closed-loop
system, shown in figure 11, the controller parameters
decided in the above section may result the system
losing the stability.
Figure 13: The real omnidirectional robot.

Figure 11: Closed-loop of robot orientation control includ-
ing actuator dynamics.
a AVT Marlin F-046C color camera with a reso-
lution of 780 × 580 is assembled pointing up towards
By setting the positions of poles and zeros of the
a hyperbolic mirror, which is mounted on the top of
closed-loop system with the locus technique, we ob-
the omnidirectional robot, such that a complete sur-
tain that the conditions k1 > 0 and k2 > 0.0515 can
rounding map of the robot can be captured. A self-
guarantee the system stability, even when the actua-
localization algorithm described in (Heinemann et al.,
tors saturate. Figure 12 shows the root locus of an
2004) based on the 50 Hz output signal of the cam-
open-loop system in the critical situation with k2 =
era gets the robot’s position in the play field in real
0.0515, where all the poles of closed-loop system
time. The wheels are driven by three 60W Maxon DC
locate in the left-half plane whatever positive value
motors and the maximum wheel velocity is 1.9m/s.
Ka K1 is. Otherwise, when k2 is less than 0.0515,
Three wheel encoders measure the real wheel veloci-
the root locus may cross the imaginary axis, and the
ties, which are steered by three PID controllers.
poles of closes-loop system may move to the right-
An eight-shaped path is adopted as the reference
half plane when ka goes to zero.
path, whose geometrical symmetry and sharp changes
in curvature make the test challenging. With a scale
variable s, the chosen eight-shaped path is as
xr = 1.8sin(2s)
(29)
yr = 1.2sin(s),
The robot was controlled to follow the eight-

shaped path with a constant translation velocity vd =
1m/s, and the parameters of our control algorithm
were chosen as k = 2.5, k1 = 4.15, k2 = 3. The first
experiment selected the path tangent direction θ p as
the desired robot orientation. Figures 14, 15, 16 and
17 show us that the proposed control method steers
the robot center R converging to the given path and
Figure 12: Root locus of open-loop model. the robot orientation tracking the desired ones with
acceptable errors, where the actuator saturation did
not appear. In order to check the influence of the ac-
tuator saturation, the second experiment selected the
desired robot orientation as
θd = θP + 0.9cP v2d , (30)
130
where cP is the path curvature at point P. The results

illustrated in figures 18, 19, 20 and 21 show us that
the robot center R converges to the given path, even
though the wheels velocities come in the saturation
when the path turns sharply.
Figure 17: Real wheel velocities.
Figure 14: Reference path and robot path.
Figure 18: Reference path and robot path.
Figure 15: Distance error.
Figure 19: Distance error.
is based on the inverse input-output linearized robot

kinematic model, which completely decoupled the
robot translation and rotation. The robot translation
is steered to follow a reference path, and the robot
rotation is controlled to track the desired orientation.
Because the actuator dynamics and saturation can
Figure 16: Orientation error.
greatly affect the robot performance, they are taken
into account when designing the controller. With the
Lyapunov stability theory, the global stability of the
6 CONCLUSION path following control law has been proven. The lo-
cus technique is used to analyze and choose the suit-
In this paper a new motion control method for an able parameters of the PD controller, such that the
omnidirectional robot is presented. This approach robot orientation can converge to the desired one even
131
Mojaev, A. and Zell, A. (2004). Tracking control and adap-

tive local navigation for nonholonomic mobile robot.
In Proceedings of the IAS-8 conference.
Muir, P. F. and Neuman, C. P. (1990). Kinematic mod-
eling for feedback control of an omnidirectional
wheeled mobile robot. In Autonomous Robot Vehicles.
Springer-Verlag.
Purwin, O. and Andrea, R. D. (2006). Trajectory generation
and control for four wheeled omnidirectional vehicles.
volume 54(1), pages 13–22.
Rojas, R. and Förster, A. G. (2006). Holonomic Control of
a Robot with an Omni-directional Drive. BöttcherIT
Figure 20: Orientation error. Verlag, Bremen.
Scolari Conceição, A., j. Costa, P., and Moreira, A. (2005).
Control and model identification of a mobile robot’s
motors based in least squares and instrumental vari-
able methods. In MMAR’05, 11st International Con-
ference on Metgids abd Models in Automation and
Robotics.
Scolari Conceição, A., Moreira, A., and j. Costa, P. (2006).
Trajectory tracking for omni-directional mobile robots
based on restrictions of the motor’s velocities. In
SYROCO’06, 8th International IFAC Symposium on
Robot Control.
Terashima, K., Miyoshi, T., Urbano, J., and Kitagawa, H.
(2004). Frequency shape control of omni-directional
wheelchair to increase user’s comfort. In ICRA’04,
Figure 21: Real wheel velocities. Proceedings of the 2004 IEEE International Confer-
ence on Robotics and Automation.
Tsai, C.-C., Huang, H.-C., Wang, T.-S., and Chen, C.-M.
when the wheels velocities saturate. (2006). System design, trajectory planning and con-
In real-world experiments, the robot was control of an omnidirectional mobile robot. In 2006 CACS
trolled to follow an eight-shaped curve with a con- Automatic Control Conference.
stant translation velocity of 1m/s, and to track sharp Watanabe, K. (1998). Control of an omnidirectional mobile
changing orientations. The result shows the effective- robot. In KES’98, 2th International Conference on
ness of the proposed control method in the case of Knowledge-Based Intelligent Electronic Systems.
both actuator saturation and non-saturation.
REFERENCES
Campion, G., Bastin, G., and D’Andréa-Novel, B. (1996).
Structural properties and classification of kinematic
and dynamic models of wheeled mobile robots. In
IEEE Transactions on Robotics and Automation, vol-
ume 12, pages 47–62.
Heinemann, P., Rueckstiess, T., and Zell, A. (2004). Fast
and accurate environment moddelling using omnidi-
rectional vision. In Dynamic Perception. Infix.
Indiveri, G., Paulus, J., and Plöger, P. G. (2006). Motion
control of swedish wheeled mobile robots in the pres-
ence of actuator saturation. In 10th annual RoboCup
International Symposium.
Liu, Y., Wu, X., Zhu, J. J., and Lew, J. (2003). Omni-
directional mobile robot controller design by trajec-
tory linearization. In ACC’03, Proceeding of the 2003
American Control Conference.
132
IMPROVEMENT OF THE VISUAL SERVOING
TASK WITH A NEW TRAJECTORY PREDICTOR
The Fuzzy Kalman Filter
C. Pérez, N. Garcı́a, J. M. Sabater, J. M. Azorı́n and O. Reinoso

Miguel Hernández University, Avda. de la Universidad S/N, Elche, Spain
[email protected]
L. Gracia
Technical University of Valencia, Camino Vera S/N, Valencia, Spain
[email protected]
Keywords: Visual servoing, fuzzy systems, vision / image processing, Kalman filter.
Abstract: Visual Servoing is an important issue in robotic vision but one of the main problems is to cope with the
delay introduced by acquisition and image processing. This delay is the reason for the limited velocity and
acceleration of tracking systems. The use of predictive techniques is one of the solutions to solve this problem.
In this paper, we present a Fuzzy predictor. This predictor decreases the tracking error compared with the
classic Kalman filter (KF) for abrupt changes of direction and can be used for an unknown object’s dynamics.
The Fuzzy predictor proposed in this work is based on several cases of the Kalman filtering, therefore, we have
named it: Fuzzy Kalman Filter (FKF). The robustness and feasibility of the proposed algorithm is validated
by a great number of experiments and is compared with other robust methods.
1 INTRODUCTION strate that steady-state Kalman filters (αβ and αβγ

filters) performs better than the KF in the presence
During the last few years, the use of visual servoing of abrupt changes in the trajectory, but not as good
and visual tracking has been more and more common as the KF for smooth movements. Some research
due to the increasing power of algorithms and com- works about the motion estimation are presented in
puters. (S. Soatto and Perona, 1997) and (Z. Duric and Rivlin,
Visual servoing and visual tracking are techniques 1996). Further, some motion understanding and tra-
that can be used to control a mechanism according to jectory planning based on the Frenet-Serret formula
visual information. This visual information is avail- are described in (J. Angeles and Lopez-Cajun, 1988),
able with a time delay, therefore, the use of predictive (Z. Duric and Rosenfeld, 1998) and (Z. Duric and
algorithms are widely extended (notice that prediction Davis, 1993). Using the knowledge of the motion
of the object’s motion can be used for smooth move- and the structure, identification of the target dynamics
ments without discontinuities). may be accomplished.
The Kalman filter (Kalman, 1960) has become a To solve delay problems, taking into account these
standard method to provide predictions and solve the considerations, we propose a new prediction algo-
delay problems (considered the predominant problem rithm, the fuzzy Kalman filter (FKF). This filter min-
of visual servoing) in visual based control systems imizes the tracking error and works better than the
(Corke, 1998), (Dickmanns and V., 1988) and (Wil- classic KF because it decides what of the used filters
son and Bell, 1996). (αβslow /αβ f ast (Chroust and Vincze, 2003), αβγ, Kv,
The time delay is one of the bigger problems in Ka and Kj) must be employed. The transition between
this type of systems. For practically all processing ar- them is smooth avoiding discontinuities.
chitectures, the vision system requires a minimum de- These five filters should be used in a combination
lay of two cycles, but for on-the-fly processing, only because: The Kalman filter is considered one of the
one cycle of the control loop is needed (Chroust and reference algorithms for position prediction (but we
Vincze, 2003). must consider the right model depending on the ob-
Authors of (Chroust and Vincze, 2001) demon- ject’s dynamics: velocity−acceleration−jerk). When
133
the object is outside the image plane, the best predic- of the state from k to k + 1 and C ∈ ℜmxn describes
tion is given by steady-state filters (αβ/αβγ depend- the way in which the measurement is generated out of
ing on the object’s dynamics: velocity−acceleration). the state xk . In our case of visual servoing m is 1 (be-
Obviously, considering more filters and more because only the position is measured) and n = 4. The
haviour cases, FKF can be improved but computa- matrix G ∈ ℜnx1 distributes the system noise ξk to the
tional cost of additional considerations can be a prob- states and ηk is the measurement noise. In the KF the
lem in real-time execution. These five filters are con- noise sequences ηk and ξk are assumed to be gaussian,
sidered by authors as the best consideration (solu- white and uncorrelated. The covariance matrices of
tion taking into account the prediction quality and the ξk and ηk are Q and R respectively (these expressions
computational cost). This is the reason to combine consider 1D movement). A basic explanation for the
these five filters to obtain the FKF. assumed gaussian white noise sequences is given in
This paper is focused on the new FKF filter and (Maybeck, 1982).
is structured as follows: in section 2 we present the In the general case of tracking, the usual model
considered dynamics, the considered dynamics is a considered is a constant acceleration model (Chroust
Jerk model with adaptable parameters obtained by and Vincze, 2003), but in our case, we consider a con-
KFs (Nomura and T., 2000), (Li and Jilkov, 2000) stant jerk model described by matrices F and C are:
and (Mehrotra and Mahapatra, 1997). In section 3, 1

T T 2 /2 T 3 /6

we present the block diagram for the visual servo-  0 1 T T 2 /2  ;C = 1
ing task. This block diagram is widely used in sev- F =  0 0 0
0 0 1 T
eral works like (Corke, 1998) or (Chroust and Vincze, 0 0 0 1
2003). Section 4 presents the basic idea applied in our
case (see (Wang, 1997b) and (Wang, 1997a)), but the where T is the sampling time. This model is called a
main work done is focused in one of the blocks de- constant jerk model because it assumes that the jerk
scribed in section 3, the Fuzzy Kalman Filter (FKF) (dx3 (t)/dt 3 ) is constant between two sampling in-
is described in section 5. stants.
In section 6, we can see the results with simulated F and C matrices are obtained from expression 3 to 7:
data. These results show that FKF can be used to im- a − ai ∆a
= = J0 (3)
prove the high speed visual servoing tasks. This sec- t − ti ∆t
tion is organized in two parts: in the first one (Sub- 1 1
x(t) = xi + vi (t − ti ) + ai (t − ti )2 + Ji (t − ti )3 (4)
section 6.1), the analysis of the FKF behaviour is fo- 2 6
cussed and in the second one (Subsection 6.2) their 1
v(t) = vi + ai (t − ti ) + J0 (t − ti )2 (5)
results are compared those with achieved by Chroust 2
and Vince (Chroust and Vincze, 2003) and with CPA a(t) = ai + J0 (t − ti ) (6)
(Tenne and Singh, 2002) algorithm (algorithm used J(t) = J0 (7)
for aeronautic/aerospace applications). Conclusions
where, x is the position, v is the velocity, a is the ac-
and future work are presented in section 7.
celeration and J is the jerk. So the relation between
them is:
...
2 THE DYNAMICS OF A MOVING x(t) = f (t); ẋ(t) = v(t); ẍ(t) = a(t); x (t) = J(t)
OBJECT
The object’s movement is not known (a priori) in 3 DESCRIPTION OF THE
a general visual servoing scheme. Therefore, it is CONTROL SYSTEM
treated as an stochastic disturbance justifying the use
of a KF as a stochastic observer. The KF algorithm The main objective of the visual servoing is to bring
presented by Kalman (Kalman, 1960) starts with the the target to a position of the image plane and to keep
system description given by 1 and 2. it there for any object’s movement. In figure 1 we
can see the visual control loop presented by Corke in
xk+1 = F · xk + G · ξk (1) (Corke, 1998). The block diagram can be used for a
yk = C · xk + N · ηk (2) moving camera and for a fixed camera controlling the
motion of a robot. Corke use a KF to incorporate a
where xk ∈ ℜnx1 is the state vector and yk ∈ ℜmx1 is feed-forward structure. We incorporate the FKF algo-
the output vector. The matrix F ∈ ℜnxm is the so- rithm in the same structure (see figure 2) but reorder-
called system matrix witch describes the propagation ing the blocks for an easier comprehension.
134
IMPROVEMENT OF THE VISUAL SERVOING TASK WITH A NEW TRAJECTORY PREDICTOR - The Fuzzy
Kalman Filter
4 THEORETICAL BACKGROUND
OF THE FUZZY KALMAN
FILTER (FKF)
The most common fuzzy inference process used is

known as Mamdani’s fuzzy inference method, but on
the other hand, we can find a so-called Sugeno, or
Takagi-Sugeno-Kang, method of fuzzy inference. It
was introduced in 1985 (Sugeno, 1985) and is simi-
lar to the Mamdani’s method in many respects. The
first two parts of the fuzzy inference process, fuzzi-
Figure 1: Operation diagram using KF presented by Corke.
fying the inputs and applying the fuzzy operator, are
exactly the same. The main difference between Mam-
dani and Sugeno is that the Sugeno output member-
ship functions are either linear or constant (for more
information see (Passino and S., 1988)).
For Sugeno regulators, we have a linear dynamic sys-
tem as the output function so that the ith rule has the
form:
j
If z˜1 is Ã1 and z˜2 is Ãk2 and, ..., and z˜p is Ãlp Then
ẋi (t) = Ui x(t) +Vi u(t)
Figure 2: Operation diagram using FKF. where x(t) = [x1 (t), x2 (t), ..., xn (t)]T is the state
vector, u(t) = [u1 (t), u2 (t), ..., um (t)]T , Ui and Vi
are the state and input matrices and z(t) =
V(z) in figure 2 represents the camera behaviour, [z1 (t), z2 (t), ..., z p (t)]T is the input to the fuzzy sys-
which is modeled as a simple delay: V (z) = kv · z−2 tem, so:
(see (Corke, 1998), (Hutchinson and Corke, 1996),
(Vincze and Hager, 2000), (Vincze and Weiman, R
1997) and (Vincze, 2000)). C(z) is the controller (A ∑ (Ui x(t) +Vi u(t))µ(z(t))
i=1
simple proportional controller is implemented in ex- ẋ(t) = R
periments presented in this paper). R(z) is the robot
(for this work: R(z) = z/z − 1) and the Prediction ∑ (µ(z(t))
i=1
filter generates the feedforward signal by prediction
the position of the target. The variable for been mini- or
mized is ∆x (generated by the vision system) that rep-
resents the deviation of the target respect to the de- ! !
R R
sired position (error). The controller calculates a ve- ẋ(t) = ∑ (Ui ξi (z(t)) x(t) + ∑ (Vi ξi (z(t)) u(t)
locity signal ẋd which moves the robot in the right di- i=1 i=1
rection to decrease the error. Using this approach, no
path planning is needed (the elimination of this path where
planning is important because it decreases the com-
putational load (Corke, 1998)). 1
ξT = [ξ1 , ..., ξR ] = [µ1 , ..., µR ]
The transfer function of the robot describes the be- R
haviour from the velocity input to the position reached ∑ µi
i=1
by the camera, which includes a transformation in the
image plane. Therefore, the transfer function consid- Our work is based on this idea and these expressions
ered is (Chroust and Vincze, 2003): (see (Passino and S., 1988) for more details). We have
z mixed the Mamdani’s and the Sugenos’s idea because
R(z) = we have implemented an algorithm similar to Sugeno
z−1
but not for linear systems. We obtain a normalized
The FKF block is explained in the next sections (sec-
weighting of several non linear recursive expressions.
tions 4 and 5).
The system works like we can see in figure 3 (see sec-
tion 5).
135
figure 4. The biggest KF block (rounded) shown in his

figure is a combination of all used algorithms in the
fuzzy filter (αβslow and αβ f ast (Chroust and Vincze,
2003), αβγ, Kv, Ka and Kj). This block obtains the
output of all specified filters. The ’Output MF’ calcu-
lates the final output using the Ri rules.
Now, we present the rules (Ri ) considered for the

fuzzy filter:
R1 : IF object IS inside AND velocity IS low AND

acceleration IS low AND jerk IS low THEN FKF=Kv
R2 : IF object IS inside AND velocity IS medium
AND acceleration IS low AND jerk IS low THEN
FKF=Kv
R3 : IF object IS outside AND velocity IS low
FKF=αβslow
Figure 3: Fuzzy Kalman Filter proposed − FKF.
R4 : IF object IS outside AND velocity IS medium
FKF=αβ f ast
5 THE FUZZY KALMAN FILTER R5 : IF object IS inside AND velocity IS high AND
(FKF) acceleration IS low AND jerk IS low THEN FKF=Kv
R6 : IF object IS inside AND acceleration IS medium
We have developed a new filter that mixes different
AND jerk IS low THEN FKF=0.2 · αβγ + 0.8 · Ka
types of Kalman filters depending on the conditions
of the object’s movement. The main advantage of this R7 : IF object IS outside AND acceleration IS medium
new algorithm is the non-abrupt change of the filter’s AND jerk IS low
output. THEN FKF=0.8 · αβγ + 0.2 · Ka
Consider the nonlinear dynamic system R8 : IF object IS inside AND acceleration IS high
ẋ = f1 (x, u); y = g1 (x, u) AND jerk IS low
as each one of the filters used. The application of the THEN FKF=Ka
fuzzy regulator in our case produces the next space- R9 : IF object IS outside AND acceleration IS high
state expression: AND jerk IS low
N THEN FKF=αβγ
∑ fi (x, u) · ω(x, u) R10 : IF jerk IS high THEN FKF=K j
i=1
where
µi (x, u) These rules have been obtained empirically, based
ω(x, u) = N on the authors experience using the Kalman filter in
∑ µ j (x, u) different applications.
i=1
The final system obtained has the same structure than Notice that rule R10 (when jerk is high) shows that
filters used: the best filter considered is K j and it does not de-
ẋ = f2 (x, u); y = g2 (x, u) pend on the object’s position (inside or outside) ve-
Figure 3 shows the FKF block diagram. In this fig- locity/acceleration value (low, medium or high).
ure, we can see that the general input is the position
sequence of the target (xk ). Using this information, we We have used a product inference engine, singleton
estimate the velocity, acceleration and jerk of the tar- fuzzifier and centre average defuzzifier. Figure 4
get in three separate KFs (Nomura and Naito present presents the fuzzy sets definition where (umax , vmax ) is
the advantages of this hybrid technique in (Nomura the image size, µvel = µacc = 2m/s, σvel = σacc = 0.5,
and T., 2000)). This information is used as ’Input MF’ cvel = cacc = 1, dvel = dacc = 3, ivel = iacc = 1 and
to obtain F1 (Ins), F2 (v), F3 (a) and F4 (j). These MF jvel = jacc = 1 (these values have been empirically
inputs are the fuzzy membership functions defined in obtained).
136
Kalman Filter
6 RESULTS
This section is composed by two different parts: first
(section 6.1), we analyze the prediction algorithm pre-
sented originally in this paper (FKF block diagram
shown in figure 3) and second (section 6.2), some
simulations of the visual servoing scheme (see figure
2) are done including the FKF algorithm.
6.1 Fuzzy Kalman Filter (FKF) Results

In figure 5, we show the effectiveness of our algo-
rithm’s prediction compared with the classical KF
methods. In this figure, we can see positions Pkr (ac-
Figure 4: Parameter definition of the fuzzy system. r
tual object position), Pk−1 (object position in k − 1)
r
and Pk−2 (object position in k − 2). Next real position
of the object will be Pk+1r , and points from P e1 to
k+1
20 Pek+1
6 , represent the prediction obtained by each single
Real Position
18
filter. The best prediction is given by the FKF filter.
Prediction Ab ∼
16
Prediction Abg P6k+1 This experiment is done for a parabolic trajectory of
Prediction Kv ∼
Prediction Ka ∼
2
Pk+1
5
Pk+1 an object affected by the gravity acceleration. (See
14 ∼
Prediction Kj
Prediction FKF
∼
P4k+1
7
Pk+1 figures 5 and 6).
Position (pixels)
12
Prk+1 We have done a lot of experiments for different
∼
10
3
Pk+1 movements of the object and we have concluded that
r
8
Pk
our FKF algorithm works better than the others filters
6
compared (filters compared are: αβ, αβγ, Kv, Ka, Kj
Prk−1
and CPA -see section 6.2- with our FKF). Figure 6
4
shows the real trajectory and the trajectory predicted
Prk−2
2
for each filter. For this experiment, we have used the
0
18 18.5 19 19.5 20 20.5 21 21.5
first four real positions of the object as input for all
t (miliseconds) filters and they predict the trajectory using only this
Figure 5: Real position vs. prediction. information. As we can see in this figure, the best
prediction is again the FKF.
6.2 Visual Servoing Control Scheme

50
Results
40
To prove the control scheme presented in figure 2, we
have used the object motion shown in figure 7 (up).
30
This target motion represents a ramp-like motion be-
Position (pixels)
tween 1 < t < 4 seconds and a sinusoidal motion for

20
t > 6 seconds. This motion model is corrupted with
Real Position a noise of σ=1 pixel. This motion is used by Stefan
10
Prediction Ab
Chroust and Markus Vincze in (Chroust and Vincze,
Prediction Abg
Prediction Kv
2003) to analyze the switching Kalman filter (SKF).
0
Prediction Ka
Prediction Kj
Prediction FKF For this experiment, we compare the proposed fil-
−10
0 2 4 6 8 10 12 14 16 ter (FKF) with a well known filter, the Circular Pre-
t (miliseconds)
diction Algorithm (CPA) (Tenne and Singh, 2002). In
Figure 6: Prediction of a smooth trajectory. figure 7 (down), we can see the results of FKF and
CPA algorithms. For changes of motion behaviour,
the FKF produce less error than CPA. For the change
in t=1, the FKF error is [+0.008,-0] and [+0.015,-
0.09] for the CPA. For the change in t=4, FKF error =
137
Target motion (m)

Table 1: Numerical comparative for dispersion value of all
filters implemented (bounce of a ball experiment). 2
0
Init. pos. αβ αβγ Kv Ka Kj FKF 0 1 2 3 4 5 6 7 8 9
0.1
CPA
0.08
40 0.619 0.559 0.410 0.721 0.877 0.353 FKF
40(bis) 0.547 0.633 0.426 0.774 0.822 0.340 0.06
50 0.588 0.663 0.439 0.809 0.914 0.381 0.04
70 0.619 0.650 0.428 0.700 0.821 0.365
90 0.630 0.661 0.458 0.818 0.857 0.343 0.02
∆x (m)
150 0.646 0.682 0.477 0.848 0.879 0.347 0
p
−0.02
−0.04
−0.06
[+0,-0.0072] and CPA error = [+0.09,-0.015]. For the 0 1 2 3 4

t (seconds)
5 6 7 8 9
change in t=6, FKF error = [+0.022,-0] and CPA error

= [+0.122,-0.76]. For the region 6 < t < 9 (sinusoidal Figure 7: Simulation result for tracking an object.
movement between 2.5m and 0.5m) both algorithms
works quite similarly: FKF error = [±0.005] and CPA
error = [±0.0076]. CPA filter works well because it is 0.02
CPA
designed for movements similar to a sine shape, but FKF
we can compare this results with the SKF filter pro- 0.015
posed in (Chroust and Vincze, 2003) and SKF works 0.01
better (due to the AKF (Adaptive Kalman Filter) ef-

fect). Therefore, the FKF filter proposed works better 0.005
∆xp (m)
than CPA for all cases analyzed but comparing FKF 0
with SKF, FKF is better for t=1, t=4 and t=6 but not
for 6 < t < 9 (sinusoidal movement). −0.005
−0.01
Figure 9 shows the zoom region 0 < t < 2 and

−0.02 < ∆x p < 0.02 of the same experiment. In this −0.015
figure, we can see the fast response of the FKF pro- −0.02
0 1 2 3 4 5 6 7 8 9
posed. t (seconds)
Figure 8: Zoom of the simulation.

6.3 Experimental Results
Experimental results are obtained for this work us-

ing the following setup: Pulnix GE series high speed 0.02
CPA
camera (200 frames per second), Intel PRO/1000 0.015
FKF
PT Server Adapter card, 3.06GHz Intel processor

PC computer, Windows XP Professional O.S. and 0.01
OpenCV blob detection library. 0.005
For this configuration, the bounce of a ball on the

∆x (m)
ground is processed to obtain data shown in figure 10. 0

p
Results of this experiment are presented in table 1. −0.005
In this table, we can see the dispersion of several fil-

ters. The FKF dispersion is less than αβ, αβγ, Kv, −0.01
Ka and Kj although FKF is a combination of them. −0.015
This table contains data from this particular experi-

ment (the bounce of a ball on the ground). For this −0.02
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
t (seconds)
experiment, the position of the ball is introduced to
the filters to prove the behaviour of them. The filter Figure 9: Zoom between 0 and 2 seconds.
proposed (FKF) is the best analyzed.
In figure 11 we can see some frames of the experi-
ment ’bounce of a ball on the ground’. For each frame
the center of gravity of the tennis ball is obtained.
138
Kalman Filter
40
Real Position 7 CONCLUSIONS AND FUTURE
35 Prediction Ab
Prediction Abg WORK
30 Prediction Kv
Prediction Ka
Prediction Kj
25 In section 6.1 (figures 5 and 6), we can see the qual-
Position (pixels)
Prediction FKF
20 ity of the new filter presented (FKF) which shows

15
good behaviour for smooth and discontinuous mo-
tions. The object’s position is estimated even when
10
it is inside the image plane and when it is outside the
5
image plane. Therefore, combine classic filters (KF)
0 when inside and steady-state filters (αβ/αβγ) when
−5
outside.
We have compared our filter with αβ, αβγ, Kv,
0 10 20 30 40 50 60 70 80 90 100
t (miliseconds)
Ka and Kj in experiments of pure prediction. We
Figure 10: Bounce of the ball on the ground. Data.
have compared too, our filter with Circular Predic-
tion Algorithm (CPA) in this paper reproducing the
same experiment as (Chroust and Vincze, 2003) for a
direct comparison with the work done by Chroust and
Vincze. The filter proposed works very well but not
better than SKF for all conditions, therefore, the addi-
tion of a AKF action can improve the filter behaviour
(future work).
The FKF is evaluated with a ramp-like and
sinosoidal motions. ∆x p is reduced in all tests done
and the overshoot is decreased significantly.
Results presented in this paper are obtained for C(z) =
KP . Other controllers like PD, PID, ... will be imple-
mented in future work.
ACKNOWLEDGEMENTS
This work is supported by the Plan Nacional de
I+D+I 2004-2007, DPI2005-08203-C02-02 of the
Spanish Government (Técnicas Avanzadas de Tele-
operación y Realimentación Sensorial Aplicadas a la
Cirugı́a Asistida por Robots).
REFERENCES
Chroust, S. and Vincze, M. (2003). Improvement of the
prediction quality for visual servoing with a switching
kalman filter. I. J. Robotic Res., 22(10-11):905–922.
Chroust, S., Z. E. and Vincze, M. (2001). Pros and cons of
control methods of visual servoing. In In Proceedings
of the 10th International Workshop on Robotics in the
Alpe-Adria-Danube Region.
Corke, P. I. (1998). Visual Control of Robots: High Per-
formance Visual Visual Servoing. Research Studies
Figure 11: Bounce of the ball on the ground. Frames. Press, Wiley, New York, 1996 edition.
Dickmanns, E. D. and V., G. (1988). Dynamic monocular
machine vision. In Applications of dinamyc monoclar
machine vision. Machine Vision and Applications.
139
Hutchinson, S., H. G. D. and Corke, P. (1996). Visual ser- Z. Duric, E. R. and Davis, L. (1993). Egomotion analysis
voing: a tutorial. In Transactions on Robotics and based on the frenet-serret motion model. In Proceed-
Automation. IEEE Computer Society. ings of the 4th International Conference on Computer
Vision. IEEE Computer Society.
J. Angeles, A. R. and Lopez-Cajun, C. S. (1988). Trajectory
planning in robotics continuous-path applications. In Z. Duric, E. R. and Rosenfeld, A. (1998). Understanding the
Journal of Robotics and Automation. IEEE Computer motions of tools and vehicles. In Proceedings of the
Society. Sixth International Conference on Computer Vision.
IEEE Computer Society.
Kalman, R. E. (1960). A new approach to linear filter-
ing and prediction problems. In IEEE Transactions Z. Duric, J. A. F. and Rivlin, E. (1996). Function from mo-
on Pattern Analysis and Machine Intelligence. IEEE tion. In Transactions on Pattern Analysis and Machine
Computer Society. Intelligence. IEEE Computer Society.
Li, X. and Jilkov, V. (2000). A survey of maneuvering target

tracking: Dynamic models. In Signal and Data Pro-
cessing of Small Targets. The International Society for
Optical Engineering.
Maybeck, P. S. (1982). Stochastic Models, Estimation and
Control. Academic Press, New York.
Mehrotra, K. and Mahapatra, P. R. (1997). A jerk model
for tracking highly maneuvering targets. In Trans-
actions on Aerospace and Electronic Systems. IEEE
Computer Society.
Nomura, H. and T., N. (2000). Integrated vsual servoing
sysem to grasp industrial parts moving on conveyer
by controlling 6dof arm. In Internacional Conference
on Systems, Man. and Cybernetics. IEEE Computer
Society.
Passino, K. M. and S., Y. (1988). Fuzzy Control. Addison-
Wesley, Ohio, USA.
S. Soatto, R. F. and Perona, P. (1997). Motion estimation via
dynamic vision. In IEEE Transactions on Automatic
Control. IEEE Computer Society.
Sugeno, M. (1985). Industrial applications of fuzzy control.
Elsevier Science Publications Company.
Tenne, D. and Singh, T. (2002). Circular prediction
algorithms-hybrid filters. In American Control Con-
ference. IEEE Computer Society.
Vincze, M. (2000). Real-time vision, tracking and control-
dynamics of visual servoing. In International Con-
ference on Robotics and Automation. IEEE Computer
Society.
Vincze, M. and Hager, G. D. (2000). Robust Vision for
Vision-Based Control of Motion. SPIE Press / IEEE
Press, Bellingham, Washington.
Vincze, M. and Weiman, C. (1997). On optimizing window-
size for visual servoing. In International Conference
on Robotics and Automation. IEEE Computer Society.
Wang, L.-X. (1997a). Course In Fuzzy Systems and Control,
A. Prentice Hall.
Wang, L.-X. (1997b). Course in Fuzzy Systems and Con-
trol Theory. Pearson US Imports & PHIPEs. Pearson
Higher Education.
Wilson, W. J., W. H. C. C. and Bell, G. S. R. (1996).
Relative end-effector control using cartesian position
based visual servoing. In IEEE Transactions on
Robotics and Automation. IEEE Computer Society.
140
OBSTACLE DETECTION IN MOBILE OUTDOOR ROBOTS
A Short-term Memory for the Mobile Outdoor Platform RAVON
H. Schäfer, M. Proetzsch and K. Berns

Robotics Research Lab, University of Kaiserslautern, P.O. Box 3049, D-67653 Kaiserslautern, Germany
b [email protected], [email protected]
Keywords: Obstacle Detection, Obstacle Avoidance, Behaviour-based Navigation, Mobile Robotics, Short-term Memory,
Distributed Minimal World Model, Algorithms, Biologically Inspired Robotics.
Abstract: In this paper a biologically inspired approach for compensating the limited angle of vision in obstacle detection
systems of mobile robots is presented.
Most of the time it is not feasible to exhaustively monitor the environment of a mobile robot. In order to
nonetheless achieve safe navigation obstacle detection mechanisms need to keep in mind certain aspects of
the environment. In mammals this task is carried out by the creature’s short-term memory. Inspired by this
concept an absolute local map storing obstacles in terms of representatives has been introduced in the obstacle
detection and avoidance system of the outdoor robot RAVON. That way the gap between the fields of vision of
two laser range finders can be monitored which prevents the vehicle from colliding with obstacles seen some
time ago.
1 INTRODUCTION
In recent years complex mobile robotic systems have

been developed for autonomous navigation in various
environments. The more complex a robot’s working
place gets the more sophisticated its obstacle detec-
tion and avoidance facilities need to designed. First
of all, sensor equipment must be appropriate for the
requirements of given scenarios. In most cases such
sensors have a limited field of vision and therefore
cannot cover the complete area around the robot. One
possibility to overcome this limitation is mounting
several devices of the same kind. However, this leads
to higher weight and costs and the evaluation of more Figure 1: The outdoor robot RAVON in rough terrain.
data might exceed the computational capacity.
In mammals the short-term memory is used to
compensate for the relatively small field of vision. ple of a local short-term obstacle memory for mobile
The hindlegs of a cat for example have to avoid ob- outdoor robots is presented. The implemented sys-
stacles some time after the cat has visually perceived tem is used to gather additional information for the
the hindrance (McVea and Pearson, 2006). Clearly behaviour-based control system of the robot RAVON
the cat has to keep in mind certain information about (Robust Autonomous Vehicle for Outdoor Naviga-
its local environment in order to safely navigate com- tion, see Figure 1). For now data of two laser scan-
plex terrain. ners – mounted at the front and the rear of the robot
In this paper an approach for adopting the princi- – is evaluated and inserted into the obstacle memory.
141
The approach, however, is kept in a generic way such

that additional sensor systems as e.g. stereo camera
systems can easily be integrated.
The outline of the paper is as follows: In the
next section work related to the topic of this paper
is described. Afterward we give some details on
the outdoor robot RAVON which is used for vali-
dating the presented approach. Section 4 describes
the concept of the short-time memory including some
details about the obstacle detection methods. Sec- Figure 2: Regions monitored by the obstacle avoidance fa-
tion 5 shows an experiment used for evaluating the cilities.
presented approach. Finally, we conclude with a sum-
mary and directions for future work.
wheel drive with independent motors yielding maxi-
mal velocities of 3 m/s. In combination with its off-
road tires, the vehicle can climb slopes of 100% incli-
2 STATE OF THE ART nation predestining it for the challenges in rough ter-
rain. Front and rear axis can be steered independently
Obstacle avoidance in outdoor terrain is a topic of which supports agile advanced driving manoeuvres
several publications. Mostly there is the distinction like double Ackerman and parallel steering.
between reactive obstacle avoidance and building up In order to navigate in a self-dependent fashion,
complete geometric maps. Reactive approaches like RAVON has been equipped with several sensors. For
(Badal et al., 1994) compute steering vectors ac- self localisation purposes, the robot uses its odometry,
cording to the proximity of obstacles in the current a custom design inertial measurement unit, a mag-
view. However, for vehicles supporting agile steer- netic field sensor, and a DGPS receiver. The sensor
ing manoeuvres like sideward motion this neglects data fusion is performed by a Kalman filter (Schmitz
the problem of possible collisions outside the field of et al., 2006) which calculates an estimated pose in
view. Other similar approaches ((Kamon et al., 1996), three dimensions.
(Laubach and Burdick, 1999)) add some kind of state In order to protect the vehicle in respect to ob-
to the obstacle avoidance system but do not explicitly stacles, several safety regions are observed by dif-
consider positions of hidden obstacles. ferent sensor systems (Schäfer and Berns, 2006) (see
Work describing building up maps which contain Fig. 2). First of all, hindrances can be detected using
information about terrain elevation ((Shiller, 2000), the stereo camera system mounted at the front of the
(Bonnafous et al., 2001)) also neglect the need to en- vehicle. This obstacle detection facility is comple-
large the virtual field of vision. A high degree of commented with two laser range finders (field of vision:
putation is used to evaluate the traversability of the 180 degrees, angular resolution: 0.5 degrees, distance
detected terrain region and to calculate a feasible tra- resolution: about 0.5 cm) monitoring the environment
jectory. However, in outdoor terrain given paths can- nearby the vehicle. Data from both sources of prox-
not be followed precisely due to disturbances. There- imity data is used for obstacle avoidance by appropri-
fore, an evaluation of possibly dangerous obstacles ate behaviours, the fusion of which is performed in-
needs to be undertaken during the driving manoeuvre. side the behaviour network (Schäfer et al., 2005). In
The approach presented here can be seen in be- case of emergency, the system is stopped on collision
tween the completely reactive and the mapping ap- by the safety bumpers which are directly connected to
proach. A short-term memory keeping only the rele- the emergency stop to ensure maximal safety. In the
vant information deals as the source for a behaviour- future, the compression of the spring system shall be
based system keeping the robot away from currently used to detect occluded obstacles in situations where
relevant obstacles. geometric obstacle detection cannot be used.
3 VEHICLE DESCRIPTION
The platform used to examine obstacle avoidance is
the four wheeled off-road vehicle RAVON (see Fig-
ure 1). It measures 2.35 m in length and 1.4 m in
width and weighs 400 kg. The vehicle features a four
142
OBSTACLE DETECTION IN MOBILE OUTDOOR ROBOTS - A Short-term Memory for the Mobile Outdoor Platform
RAVON
Listing 1: Declarations.
# Raw s e n s o r i n f o r m a t i o n from t h e
# l a s e r range f i n d e r s
scanner data
# FIFO c o n t a i n i n g t h e c a n d i d a t e o b s t a c l e s
obstacle fifo
# The maximal l e n g t h o f t h e q u e u e
# assures that the obstacle l i s t s
# h a v e a c e r t a i n a g e when r e t r i e v e d .
# Furthermore the scans are discarded
# i f t h e v e h i c l e i s n o t moving t o p r e v e n t
# the accumulation of outdated information .
max number of scans
# S t o r e s the c u r r e n t r o b o t pose in the

# robot ’ s a b s o l u t e working coord . system
current robot pose
# Stores the c u r r e n t v e l o c i t y of the robot

current velocity
# Stores r e p r e s e n t a t i v e s in the blind

# angle to the l e f t of the robot .
short term memory left
# Stores r e p r e s e n t a t i v e s in the blind

# angle to the r i g h t of the robot .
short term memory right
# T h r e s h o l d o f t h e maximal e u c l i d e a n d i s t a n c e
# b e t w e e n t h e c u r r e n t v e h i c l e p o s e and t h e
# one a t which a s c a n was t a k e n .
Figure 3: Top-level concept of RAVON’s Obstacle Avoid-
progress threshold
ance Facility.
Figure 3 illustrates the structure of the laser-
scanner-based part of the obstacle avoidance system.
4 A SHORT-TIME MEMORY FOR The sensor flow1 of the main loop which is dealing
with the short-term memory is outlined in Listing 2
RAVON (For explanation of the particular variables used see
Listing 1). The first step of the main loop is the de-
In order to compensate for the blind angles of the sen- tection of obstacles from the laser range data which is
sor systems local short-term memories shall succes- carried out in the Obstacle Detection facility. The cur-
sively be introduced. Any of these can be regarded as rent vehicle pose in the robot’s absolute working coor-
a virtual sensor system which can seamlessly be inte- dinate system is applied to the resulting obstacle lists
grated into RAVON’s control system as described in in order to yield an absolute reference map through
(Schäfer et al., 2005). That way step by step blind which the robot can virtually be navigated. After that
regions can be covered with increasing accuracy as the lists are casted into sector maps which are used
overlapping virtual sensors yield more and more ad- by the Obstacle Avoidance behaviours to com-
ditional information. In this paper the validation of pute evasive steering commands if necessary.
the general approach and connected technical facili- For the extension described in this paper a FIFO
ties are presented by the example of closing the gap queue was introduced which holds in stock the ob-
between the two laser range finders (See Figure 2). stacle lists of several subsequent scans. In every
sense cycle older obstacle lists are examined for ob-
1
The control concept underlying all robot projects at the
Robotics Research Lab strictly separates the main loop into
a buttom up sense and a top down control cycle which are
alternately invoked.
143
stacle representatives which may have migrated into

the blind angles. Older in this sense can be interpreted
in two ways: a) the scan was taken a certain time ago
(age criterion2 ) or b) the scan was taken a certain dis-
tance ago (progress criterion3 ).
Listing 2: Main Loop.

Sense ( )
# D e t e c t o b s t a c l e s from r a n g e i n f o r m a t i o n
DetectObstacles ( scanner data ,
obstacle list fifo )
i f ( c u r r e n t v e l o c i t y ! = 0 ) do
# remove r e p r e s e n t a t i v e s which a r e no
# longer in the scanner ’ s blind angle
Trim ( s h o r t t e r m m e m o r y l e f t , c u r r e n t r o b o t p o s e )
Trim ( s h o r t t e r m m e m o r y r i g h t , c u r r e n t r o b o t p o s e )
# check age c r i t e r i o n ( a )
while ( Size ( o b s t a c l e l i s t f i f o ) >
m a x n u m b e r o f s c a n s ) do
obstacle list = First ( obstacle list fifo )
FillObstacleMemory ( short term memory left ,
short term memory right ,
obstacle list ,
current robot pose )
RemoveFirst ( o b s t a c l e l i s t f i f o )
od
# check p r o g r e s s c r i t e r i o n ( b )
while ( D i s t a n c e ( Pose ( o b s t a c l e l i s t ) ,
> p r o g r e s s t h r e s h o l d ) do
FillObstacleMemory ( short term memory left ,
obstacle list ,
od
else
# d i s c a r d o l d e r s c a n s i f n o t moving
while ( Size ( o b s t a c l e l i s t f i f o ) >
m a x n u m b e r o f s c a n s ) do
od
esle
esneS
If one of the two criteria applies to an obstacle list

FillObstacleMemory checks the obstacle rep-
resentatives and places them into the corresponding
short-term memory. In order to prevent the short-
term memory from overfilling Trim (See Listing 4) Figure 4: Screenshot showing clustering of detected range
removes obstacle representatives which are no longer values (top) and photo showing the scenario (bottom).
in the robot’s blind angle. Furthermore the obstacle
list FIFO has a limited maximal size. That way only
a limited number of scans is keept in mind and is ex- from the Homing facility and the Obstacle
amined which minimises memory consumption and Avoidance are fused according to the behaviour ar-
computational expenses. chitecture described in (Proetzsch et al., 2005). If no
As the Obstacle Sector Maps the short- obstacles are present Homing successively navigates
term memories are evaluated by the Obstacle along a trajectory given in subsequent target poses
Avoidance component which computes evasive provided by the Navigator component.
steering commands and inhibits the Homing be-
haviours if applicable. The steering commands 4.1 Obstacle Detection
2
The age of a scan is modelled implicitly by the maximal
number of scans in the queue. The range data from the laser scanners is first of all
3
The progress threshold assures that no large gaps oc- clustered according to point-wise adjacency in order
cur in the obstacle memory at higher speeds as scans are to filter artefacts and entities which are likely to rep-
examined earlier. resent flexible vegetation (See Figure 4). In this figure
144
RAVON
the current laser scanner data (dark points) are inter-

preted as clusters (red boxes) according to their dis-
tance. Each of the clusters is then evaluated concern-
ing its dimension and its number of cluster represen-
tatives. If both criteria are true clusters are interpreted
as obstacles.
In order to have a uniform interpretation of near
obstacles which can be used by obstacle avoidance
behaviours the sensor coverage area is divided into
sectors. In Figure 4 the closest obstacle in each of
these sectors is indicated by a green line. In Figure 4
the human to the left and the bushes to the right of the
robot have clearly been detected as obstacles. Note
that the bushes are subject to severe noise and that
they are therefore detected as a large number of small Figure 5: Schema of how to determine whether an obstacle
fluctuating objects. For that reason obstacles cannot representative resides in the blind region of the vehicle.
be correlated over time which is a problem for local
map building as representatives accumulate over time
if the robot is moving slowly. Using an occupancy pose previously attached to the obstacle list (Subrou-
grid would be a solution to this problem but it lacks tine ToAbsoluteContext). Intuitively explained
accuracy because of rasterisation. Furthermore the this is as if you would virtually drive a robot model
consideration of objects moving independently from through an absolute landscape while checking for
the vehicle – which shall be introduced in the future – points in the blind regions of the robot. That way it is
would be complicated a lot. not necessary to transform the representatives when-
ever the robot changes its pose.
4.2 Short-term Memory
Listing 3: Fill the Obstacle Memory.
As already indicated above obstacles detected in laser FillObstacleMemory ( short term memory left ,
rage data consist of points which represent the bound-
obstacle list ,
ary of a rigid object. Thus in every sensor cycle a list current robot pose )
of such point clusters4 is generated and stored in a f o r a l l ( o b s t a c l e i n o b s t a c l e l i s t ) do
FIFO queue for further processing. To every obstacle f o r a l l ( r e p r e s e n t a t i v e i n o b s t a c l e ) do
list the obstacle detection facility attaches the current absolute representative =
robot pose in an absolute working coordinate system. ToAbsoluteContext ( Pose ( o b s t a c l e l i s t ) ,
In later sense cycles older obstacle lists are re- representative )
trieved from the FIFO queue in order to be examined

y = P o i n t O n L i n e ( XAxis ( c u r r e n t r o b o t p o s e ) ,
for obstacle representatives which might now be in absolute representative )
the blind angle of the laser scanners. Note that in this x = P o i n t O n L i n e ( YAxis ( c u r r e n t r o b o t p o s e ) ,
context the algorithm drops the abstraction of obsta- absolute representative )
cles in favour of simplicity. This does not result in a
loss of information as all the robot can actually do if i f ( Abs ( y ) < Width ( s h o r t t e r m m e m o r y ) / 2 AND
Abs ( x ) < L e n g t h ( s h o r t t e r m m e m o r y ) / 2 ) {
it detects a remembered obstacle at its flank is try to
i f ( ToRightOfRobot ( c u r r e n t r o b o t p o s e ,
keep away from it by adding a translational compo- absolute representative ))
nent to the steering command. Which representative Append ( s h o r t t e r m m e m o r y r i g h t ,
belongs to what obstacle is in that sense irrelevant. absolute representative )
To fill the obstacle memory routine Fill- else
ObstacleMemory (See Listing 3) examines all ob- Append ( s h o r t t e r m m e m o r y l e f t ,
stacles in a provided obstacle list. In order to deter- absolute representative )
fi
mine the relation between the vehicle at its current
od
pose and the scan points registered earlier, all rep- od
resentatives are first of all transferred into the abso- od
lute working coordinate system of the robot using the yromeMelcatsbOlliF
4
In the following this list shall be referred to as obstacle Once in an absolute context the coordinates
list. can reside where they are. With the subroutine
145
PointOnLine the distance of the representative to

the X-axis and the Y-axis of the robot-local coordi-
nate system is computed (See Figure 5). This yields
the robot-local coordinate of the representative which
offers a straightforward check whether the represen-
tative resides in a blind area of the robot. Depend-
ing whether the representative is rather to the left or
the right of the robot it is added to the corresponding
short-term memory. Routine Trim (See Listing 4)
works in analogy just the other way around to sweep
representatives from the short-term memories which
Figure 6: Image of the test scenario with overlaid trace.
are no longer in the blind angle of the scanners.
Listing 4: Trim the Short-term Memory.

Trim ( s h o r t t e r m m e m o r y , c u r r e n t r o b o t p o s e )
RAVON. In order to evaluate the approach several test
f o r a l l ( r e p r e s e n t a t i v e i n s h o r t t e r m m e m o r y ) do runs have been executed. First of all autonomous nav-
igation along predefined tracks has been executed. In
y = P o i n t O n L i n e ( XAxis ( c u r r e n t r o b o t p o s e ) , this case the vehicle follows the given direction to the
representative ) target point as long as there is no hindrance. In case of
x = P o i n t O n L i n e ( YAxis ( c u r r e n t r o b o t p o s e ) , obstacles, however, the straight motion is interrupted
representative )
by avoidance behaviours. This effect remains active
# Check w h e t h e r t h e r e p r e s e n t a t i v e i s
even if the obstacle is in the blind side region. There-
# s t i l l in the blind angle fore the robot gets back on track only as soon as all
i f ( Abs ( y ) > Width ( s h o r t t e r m m e m o r y ) / 2 OR hindrances are left behind.
Abs ( x ) > L e n g t h ( s h o r t t e r m m e m o r y ) / 2 ) For clarification of the properties of the short-term
Remove ( r e p r e s e n t a t i v e , s h o r t t e r m m e m o r y ) memory in this section a test run is presented where
fi
the robot is remote controlled. In this experiment the
od
mirT
robot is driven through a locally flat environment. All
the time the behaviour-based control system evaluates
Note that the robot pose used during the proce- sensor information and overrides the given commands
dures outlined above currently relies on odometry if necessary in order to avoid collisions.
only. This is due to the fact that odometry is locally The scenario for this run is hilly grass land as
coherent in comparison to data fused from odometry, demonstrated in Figure 6. The image shows some
GPS, the inertial measurement unit and the magnetic trees to be avoided as well as some bushes with un-
field sensor. Eventhough globally more accurate, the known load bearing surface. For clarification the ap-
latter three introduce certain local jumps into the pose, proximate path of the vehicle is overlaid. Four mark-
which are a problem for precise local navigation. The ers indicate positions referred to in the following de-
pose drift immanent in odometry on the other hand is scription.
not an issue for the approach presented in this paper Using the localisation system the trace of the ve-
as it is enough to be rather precise over the distance hicle can be plotted as presented in Figure 7. Some of
of a vehicle length. When the robot has passed the the trees and bushes are integrated in order to point
obstacle it is erased from the short-term memory any- out the sensor data and vehicle reaction. Figure 8
way. In the future visual odometry shall additionally shows the scanner data as well as clusters and sector
be called into service in order to gain more accurate values (see Section 4.1) for the four positions indi-
pose information. In particular on slippery surfaces cated by markers in the trace plot. Additionally data
the authors expect this combination to be a lot more of the obstacle memory at the side of the vehicle is
robust than banking on odometry only. depicted as blue lines.
Position 1 shows a situation where the robot was
steered into the direction of a tree. After an evad-
ing manoeuvre the robot is now located next to the
5 EVALUATION IN tree. The sensor data shows that the obstacle is at
EXPERIMENTS the edge of the side region and would eventually be
hidden. However, the blue lines indicate data stored
The short-term memory described before was im- in the obstacle memory. Therefore the robot contin-
plemented and integrated into the control system of ues preserving a minimal distance to the obstacle. At
146
RAVON
Figure 7: Top view position diagram of the experiment.
position 2 the robot is in a similar situation. A tree

which was detected by the laser range finder at the
front changed its relative position such that it is lo-
cated at the right side of the robot.
Positions 3 and 4 show the situation where the
robot is manually steered to the left. Due to the ob-
stacles detected and stored in the short-term memory
the evading behaviours force the robot away from the
hindrances. This interaction results in a smooth fol-
lowing of the bushes.
The experiments showed a major improvement of
the reaction to obstacles. Behaviours regarding the
short-term memory prevent the robot of driving into
the direction of hidden obstacles. For static obstacles
this leads to a system reacting on hindrances which
can be located anywhere around the vehicle.

WORK
In this paper we presented an approach for virtually
extending the coverage of obstacle detection sensor
systems. The formulated algorithm uses information
about the vehicle motion to propagate sensor data into
the blind zone of the robot. Due to a massive reduc-
tion of stored data the system features low memory
usage. Additionally there is no need for calculating
the correspondence between consecutive data making
the approach applicable for realtime scenarios. Figure 8: Sensor data visualisation during the experiment.
Next steps involve the integration of a stereo cam-
147
era system. Due to the device’s limited field of view

the presented algorithm leads to an eminent extension
of the supervised area surrounding the vehicle. Addi-
tionally the accuracy of the local positioning system
will be enhanced by visual ego motion estimation.
REFERENCES
Badal, S., Ravela, S., Draper, B., and Hanson, A. (1994). A
practical obstacle detection and avoidance system. In
IEEE Workshop on Applications of Computer Vision.
Bonnafous, D., Lacroix, S., and Simon, T. (2001). Motion
generation for a rover on rough terrain. In IEEE/RSJ
International Conference on Intelligent Robots and
Systems.
Kamon, I., Rivlin, E., and Rimon, E. (1996). A new
range-sensor based globally convergent navigation al-
gorithm for mobile robots. In IEEE International
Conference on Robotics and Automation (ICRA).
Laubach, S. L. and Burdick, J. W. (1999). An autonomous
sensor-based path-planner for planetary microrovers.
In IEEE Int. Conf. on Robotics and Automation.
McVea, D. and Pearson, K. (2006). Long-lasting memories
of obstacles guide leg movements in the walking cat.
The Journal of Neuroscience.
Proetzsch, M., Luksch, T., and Berns, K. (2005). Fault-
tolerant behavior-based motion control for offroad
navigation. In 20th IEEE International Conference on
Robotics and Automation (ICRA), Barcelona, Spain.
Schäfer, H. and Berns, K. (2006). Ravon - an autonomous
vehicle for risky intervention and surveillance. In In-
ternational Workshop on Robotics for risky interven-
tion and environmental surveillance - RISE.
Schäfer, H., Proetzsch, M., and Berns, K. (2005). Exten-
sion approach for the behaviour-based control system
of the outdoor robot ravon. In Autonome Mobile Sys-
teme.
Schmitz, N., Proetzsch, M., and Berns, K. (2006). Pose esti-
mation in rough terrain for the outdoor vehicle ravon.
In 37th International Symposium on Robotics (ISR).
Shiller, Z. (2000). Obstacle traversal for space exploration.
In IEEE International Conference on Robotics and
Automation.
148
METHODOLOGY FOR LEARNING VISUAL REACTIVE
BEHAVIOURS IN ROBOTICS THROUGH REINFORCEMENT AND
IMAGE-BASED STATES
Pablo Quintı́a, José E. Domenech, Cristina Gamallo and Carlos V. Regueiro

Facultad de Informática, Universidad de A Coruña, 15071 A Coruña, Spain
[email protected], [email protected], [email protected], [email protected]
Keywords: Reinforcement Learning, Mobile Robotics, Artificial Vision, Visual Reactive Behaviours, Motivation.
Abstract: This article describes the development of a methodology for the learning of visual and reactive behaviours
using reinforcement learning. With the use of artificial vision the environment is perceived in 3D, and it
is possible to avoid obstacles that are invisible to other sensors that are more common in mobile robotics.
Reinforcement learning reduces the need for intervention in behaviour design, and simplifies its adjustment to
the environment, the robot and the task. The designed methodology is intended to be general; thus, in order
to change the desired behaviour, only the reinforcement and the filtering of the image need to be changed.
For the definition of the reinforcement a laser sensor is used, and for the definition of the states a fixed 3x3
grid is used. The behaviours learned were wall following, object following, corridor following and platform
following. Results are presented with a Pioneer 2 AT. A Gazebo 3D simulator was used for the Learning and
testing phase, and a test of the wall following behaviour was carried out in a real environment.
1 INTRODUCTION The results obtained are generalisable, with the possi-

ble exception of the discrimination between floor and
The design and implementation of reactive be- obstacles. This perception would be more robust and
haviours for the control of autonomous mobile robots efficient if in-depth information were used.
has been shown to be one of the most efficient ways We now comment on related work and go on to de-
of carrying out low-level tasks. These require a very scribe the TTD(λ) - Q-learning algorithm and its ap-
short response time and continuous interaction with plication to the four behaviours chosen. We then show
the environment, which is almost totally unknown, the results obtained in simulation. For this phase the
complex and dynamic. Thus arises the challenge of multirobot simulator Player/Stage/Gazebo was cho-
specifying how each one should be implemented. sen as it is highly generalized, it supports the Pioneer
One of the most promising techniques is rein- 2-AT mobile robot, and simulates in 3-D (Gazebo).
forcement learning (RL) (Millán et al., 2002; Wyatt, Lastly, we finish off with a conclusions and future
1995), one of the principal advantages of which is that work section.
it minimizes interaction with the designer, since only
the set of states and actions and the reinforcement has
to be established. There is no need to identify all 2 RELATED WORK
the situations in which the robot may find itself, nor
the action to be implemented in each of them; only Only a small number of studies have used vision as
whether the result of the action is acceptable or not the principal sensorial input for visual behaviours in
need be stated. a mobile robot. This is probably due to the high cost
This work describes the design and implementa- associated with processing visual information (Naka-
tion of four visual behaviours using a single camera, mura and Asada, 1995).
with the aim of studying the feasibility of the project In some works, reinforcement is used to learn vi-
and enabling a simple, economical implementation. sual behaviours that are similar to wall following (e.g.
149
servoing and wandering), but which are simpler as or is excessively close to one).
there is no need to maintain a distance from the wall Learning consists of approximating a quality func-
the robot is following. Gaskett et al. (Gaskett et al., tion Q(s, a). The optimal action in each state is the
2000) use an improved version of Q-learning (“Ad- one that maximizes its evaluations. The algorithm
vantage Learning”) which handles continuous states chosen for this work is TTD(λ) - Q-learning, due to
and actions thanks to neural networks. it being based on the basic and simply Q-Learning al-
Another implementation of visual servoing be- gorithm but applied to several states obtaining a faster
haviour can be found in (Martı́nez-Marı́n and Duck- convergence time. The updating equation for the val-
ett, 2005). The algorithm used is another variant of Q- uations is:
learning that permits real-time learning. Unlike in the
present work, the state of the agent is defined by the ∆Q(st , at ) = α[rt + γmaxa Q(st+1 , a) − Q(st , at )]et (s) ,
position of the camera (inclination and angle of turn) (1)
focused on the objective, and not by the image. Thus, for all st ∈ Sn where α is the learning coefficient, γ is
active vision is required, along with a perfect identifi- the reinforcement discount coefficient, Sn is the set of
cation of the objective, which is not always possible. the n-last visited states and et is the eligibility trace of
It is also difficult to use the same system to implement the state:
more than one behaviour simultaneously.
A similar, but more complete, general approach γ λ et−1 (s) if s 6= st ,
et (s) = (2)
was taken in (Boada et al., 2002). This system learns 1 if s = st ,
basic behaviours (watching and orientation) for con-
trolling a pan-tilt unit and the robot, and combines where λ is a decay parameter. Initial Q-values ∈
them to obtain complex ones (approach). Neverthe- [−0, 9, −1, 0].
less, they need to detect and identify the objective, One drawback of Q-learning is the need to strike a
which is very difficult with a wall, a corridor or a plat- balance between exploration and exploitation. In or-
form. Once again, states were not defined directly by der to do so, the Softmax method (Moreno et al., 2004)
the image. has been applied, where the probability of taking the
Ruiz et al. (Ruiz et al., 2005) have implemented action ai in the state s at time t is:
two visual behaviours. Their approach is based on
the detection of straight segments in the image, which eQt (s,ai )/Ts
to a certain degree limits its mode of use. Moreover, Pr(s, ai ) = , (3)
∑nj=1 eQt (s,a j )/Ts
control is totally heuristic, with no type of learning.
Our approach is more general and is not conditioned
by the characteristics of the environment. where {a1 , ...., an } is the set of possible actions in the
state s and Ts is the temperature associated to state s.
With temperature it is possible to regulate the
probability distribution between actions, and thus, the
3 REINFORCEMENT LEARNING balance between exploration and exploitation. Ini-
tially we start from a high temperature (greater ex-
Reinforcement learning (RL) is based on the use of ploration of actions) and this is progressively reduced
a qualification (reinforcement) of the agent’s actions throughout the learning in order to principally select
by the environment. The reinforcement does not in- those actions with the best evaluation.
dicate the correct action (supervised learning), only Temperature is modified for each state in accor-
whether it has been satisfactory or not, and is not usu- dance with the equation:
ally immediate in time. The situations of the agent are
usually codified into discrete states (s) in which vari- ( T
− t ln 0
ous actions (a) can be implemented (may be different T (t) = T0 e tk k if t ≤ tk , (4)
for each state). k if t > tk ,
A simple and intuitive definition of reinforcement
has been sought, as we believe that it is one of the where t is the number of times the current state has
main advantages of this type of algorithm. Rein- appeared, T0 is the ”initial” temperature, k is the min-
forcement indicates only those situations in which it imum temperature (the state does not explore more
is highly probable that the robot has ceased to imple- actions) and tk is the number of appearances that are
ment the task correctly (i.e. the robot is excessively required for the temperature to reach k.
far from a wall), or has definitively carried it out badly Table 1 shows a summary of the values used for
(the robot collides with an element in its environment the parameters of the TTD(λ) - Q-learning algorithm.
150
METHODOLOGY FOR LEARNING VISUAL REACTIVE BEHAVIOURS IN ROBOTICS THROUGH
REINFORCEMENT AND IMAGE-BASED STATES
4 BEHAVIOURS be avoided; that is, the system should not classify two
situations in which the robot must execute very differ-
4.1 Common Aspects ent commands in the same state.
Lastly, due to the quantity of information gener-
ated by a camera, the image needs to be processed in
On a reactive behaviour and with RL, each state must
order to reduce the amount of pertinent information
contain all the information for selecting the next ac-
(Nehmzow, 1999). In order to resolve these problems,
tion. After several tests the camera, which uses a 80
a simple, computationally efficient methodology has
wide-angle lens, (Fig. 1) was placed 53 cm over the
been employed, which can be run in real time on a
robot and inclined 42 degrees towards the floor. Using
mobile robot (Regueiro et al., 2006). This approach
these values there is no need to change the position of
is easily generalisable.
the camera for the different behaviours.
Firstly, with the exception of object following, the
image is processed with a Sobel edge enhancement
filter (Fig. 2(b)) to highlight the pertinent information:
obstacles (positive and negative) on the floor. This
floor detection process is highly sensitive to changes
in lighting and textures. Nevertheless, it can be im-
proved in different ways: with stereo vision, by cali-
brating the camera to detect the ground plane or by ap-
plying Machine-Learning techniques for ground de-
tection (Michels et al., 2005). For the object follow-
ing behaviour the image is processed to detect a spe-
Figure 1: Position of the camera and the laser in the Gazebo cific colour (red was chosen). Then for each pixel its
simulator. red value is analysed, and if this exceeds a specified
threshold it is set to white, otherwise it is set to black.
The position of the laser depends on the reinforce- Secondly, the image is divided into a grid made up
ment wanted but, with the exception of the platform of 3 rows and 3 columns (Fig. 2(c)) for codification. A
following, the laser was placed over the front of the cell is considered occupied if the percentage of edge
robot, centred and parallel to the floor. For the plat- pixels reaches a given threshold. This step is essential
form following behaviour it was inclined 45 degrees for avoiding “perceptual aliasing”. Thus defined, the
towards the floor in order to detect the limits of the state space is 29 , and in order to reduce it, it is sup-
platform. A SICK LMS 200 laser sensor was used. posed that if a cell in one of the columns is occupied,
all those cells above it are also occupied (Fig. 2(c)).
4.1.1 State Space Hence the number of possible states is (3 + 1)3 ; i.e.
64. The state space may be further reduced, but dras-
As was to be expected, the definition of the state space tic modifications would be needed in the codification,
was critical in the development of this work, since it which would be difficult to justify.
has to respond to criteria that are at times conflicting.
On one hand, the space must be small, as convergence 4.1.2 Action Space
time in RL increases exponentially with the number of
states. On the other hand, “perceptual aliasing” must The action space was chosen in order to allow a flex-
ible behaviour, and at the same time avoid a series
of actions that would leave the robot immobile and
Table 1: Values used in this work for the TTD(λ) - Q- without negative reinforcement. One constraint of Q-
learning algorithm. learning is that actions must be discrete. Hence, the
action space chosen is as in Table 2
Par. Description Value
α Learning coefficient 0.2 4.2 Wall Following Behaviour
γ Reinforcement discount coefficient 0.99
T0 Initial temperature 0.07 It has been shown that wall following behaviour is one
k Lower temperature limit 0.009 of the most useful when robots need to move reac-
tk State exploration limit 500 tively and safely through their environment (Regueiro
λ Decay parameter 0.9 et al., 2002). One of its advantages is that it only uses
n Number of last visited states 3 local information, and it makes use of the topological
structure of the environment.
151
(a) (b) (c)
Figure 2: Determination of state with an image: a) original image; b) edge pixels (Sobel filter); and c) final codified state
(showing the codification grid and the free and occupied cells).
4.2.1 Definition of Reinforcement 4.2.2 Results
The defined reward is always negative (-1), spurious The environment used is shown in Fig. 4(a).
in time, and has two different components: Wall following behaviour belongs to the class of
continuous tasks which persist over time. This means
1. Close reinforcement: if any of the laser beams de- that they are not divided naturally into episodes, as
tects an obstacle at 25 cm or less. would be desirable for application of a reinforcement
2. Far reinforcement: if the right-hand beam detects algorithm. In order to avoid this drawback a rein-
a distance greater of 1 m. forcement is generated after each serious system error
(collision or excessive distancing) and the robot exe-
The reinforcement has been defined in two ways: cutes a recovery function which returns the robot to a
1. Instant reinforcement: the reinforcement is ap- safe state.
plied when it finds a non desired situation. Used Fig. 3 shows the reinforcement received during the
with the close reinforcement. learning phase in TTD(λ). Each point represents the
reinforcement accumulated in the previous 400 learn-
2. Delayed reinforcement: the reinforcement is ap- ing cycles. Thus, the first point indicates that the rein-
plied when a undesired situation persists over a forcement received since the onset of the experiment
period of time. Used with the “far reinforcement”. up until 400 cycles has been -12, and so forth. The
learned Q-values are stored for their subsequent test-
With delayed reinforcement the need to always
ing.
have a wall on the right side is relaxed and thus a more
As can be seen in the diagrams, the agent learns
robust behaviour is possible.
the task in 2,000 learning cycles with TTD(λ) as op-
posed to 80,000 cycles obtained with Q-Learning al-
gorithm and 8x4 grid (Regueiro et al., 2006).
Table 2: Action space chosen.
Act. n. Linear speed m/s Angular speed rad/s

0 0.4 0.2
1 0.4 0
2 0.4 -0.2
3 0.2 0.8
4 0.2 0.6
5 0.2 0.4
6 0.2 0.2
7 0.2 0
8 0.2 -0.2
9 0.2 -0.4
10 0.2 -0.6
11 0.2 -0.8 Figure 3: Reinforcement accumulated during the learning
12 0.05 0.8 of wall following behaviour.
13 0.05 -0.8
152
(a) (b)
Figure 4: Wall following behaviour: a) Trajectories obtained during initial stages; and b) Trajectories obtained during final
stages.
4.3 Corridor Following Behaviour wall, and the right turns, where the robot tries to go
straight until the wall is directly in front of it.
The corridor following behaviour is one of the sim- Fig. 5 shows the reinforcement received during the
plest behaviours to learn; nevertheless, when work- learning phase in TTD(λ). As can be seen in the dia-
ing with RL it has problems with decision selection grams, the agent learns the task in 800 learning cycles.
at crossroads. It simply makes no decision, as could
be expected from a reactive behaviour, thus the robot
cannot be ordered to go straight on or to turn at a
junction. Several positive reinforcements were used
to make the robot go straight on, but without success.
4.3.1 Definition of Reinforcement
Two classes of reinforcements were defined:

1. Critical reinforcement: if any of the laser beams
detects an obstacle at 25 cm or less the reward is
-1.
Figure 5: Reinforcement accumulated during the learning
2. Speed reinforcement: used to favour the use of of corridor following.
straight and fast actions or/and penalise slow and
turning actions with values in [−0.4, 0.3].
4.4 Platform Following Behaviour
There were no significant variations in the final
behaviours with the use of speed reinforcement. We The platform following behaviour is very similar to
have to mention that even when defining an erroneous the corridor following behaviour, but the laser sensor
reinforcement the robot learned the correct behaviour. points 45 degrees towards the ground, as it has to de-
tect uneven features in the floor. This behaviour also
4.3.2 Results has the same problem at crossroads.
The environment used is shown in Fig. 6(a). 4.4.1 Definition of Reinforcement

As has previously been mentioned, the final be-
haviours show a tendency to turn to right or left. The A simple calibration of the laser is needed at the be-
example in Fig. 6(b) shows a tendency to turn left; if ginning of the execution. The laser measures the dis-
not to straight on and then turn right. That is the rea- tance of the front beam (number 90) and this distance
son for the difference between turns to the left, where is used to calculate the expected distance for the rest
the robot turns smoothly and passes close to the left of the beams (values between 50 and 130). If a laser
153
(a) (b)
Figure 6: Corridor following behaviour: a) Trajectories obtained during initial stages; and b) Trajectories obtained during
final stages.
beam value is greater than the expected distance plus 4.5.1 Definition of Reinforcement
15 cm, then the reward is -1.
The defined reward is negative (-1.0) when any of the
4.4.2 Results following conditions is accomplished:
1. Close reinforcement: if any of the laser beams de-
The environment used is shown in Fig. 8(a).
tects an obstacle at 30 cm or less.
Fig. 7 shows the reinforcement received during the
learning phase in TTD(λ). As can be seen in the dia- 2. Image reinforcement: if the prey is not viewed by
grams, the agent learns the task in 2,000 learning cy- the camera.
cles.
4.5.2 Results
Fig. 9 shows the reinforcement received during the

learning phase in TTD(λ). As can be seen in the di-
agrams, the agent learns the task in 10,500 learning
cycles. The learning is slower for this behaviour due
to the fact that the path of the prey is very long, an en-
tire lap taking approximately 7 minutes, so a number
of difficult situations appeared very sporadically.
As can be seen in Fig. 10(b) the path of the final
behaviour is smoother than the path of the prey. The
average distance between the robot and a prey moving
at constant speed of 0.2 m/s is 0.9 m. We have tested
several speeds for the prey: if the speed is slower than
of platform following behaviour.
0.35 m/s the robot can follow it (it should be remem-
bered that the maximum speed of the robot is 0.4 m/s,
4.5 Object Following Behaviour see table 2).
The main change for this behaviour was the filtering 4.6 Test in a Real Robot
of the image. Here, instead of using a Sobel filter the
image is preprocessed with a simple colour filter. Red Finally we have tested the behaviour with minor
was chosen and a red box is attached to the prey, an- changes on a real environment: a long corridor, nar-
other Pioneer 2-AT executing a follow line behaviour. rower than learning environment, with many new fea-
The prey has a follow line behaviour, the path of the tures, such as trash baskets, radiators, plants, glass
pray is shown in Fig. 10(a). walls, open and closed doors. This real environment
154
(a) (b)
Figure 8: Platform following behaviour: a) Trajectories obtained during initial stages; and b) Trajectories obtained during
final stages.
(a) (b)
Figure 10: Object following behaviour: a) Trajectories obtained during initial stages; and b) Trajectories obtained during final
stages.
also has both natural and artificial lighting. As can

be seen by examining the robot’s path on Fig. 11, our
behaviour works perfectly.

WORK
In this work four visual reactive behaviours for the
Pioneer 2 AT robot have been implemented with
RL algorithms. All behaviours use the same state
of object following behaviour.
and action spaces, only reinforcement changes. A
rangefinder laser is used to define the reinforcement
of each behaviour. Learning and testing were carried
155
ACKNOWLEDGEMENTS
This paper was supported in part by the Xunta
de Galicia and Spanish Government under Grants
PGIDIT04-TIC206011PR, TIN2005-03844, and de-
partment colaboration grant (B.O.E. 16-06-2006).
REFERENCES
Boada, M., Barber, R., and Salichs, M. (2002). Visual ap-
proach skill for a mobile robot using learning and fu-
sion of simple skills. Robotics and Autonomous Sys-
Figure 11: Robot’s path carrying out the visual wall fol-
tems, 38:157–170.
lowing behaviour on a real environment (right wall). The
map is a certainty grid created with a LMS200 laser (white Gaskett, C., Fletcher, L., and Zelinsky, A. (2000). Rein-
pixels are free space, black obstacles and grey uncertainty). forcement learning for a vision based mobile robot. In
Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and
Systems (IROS), volume 1, pages 403–409.
Martı́nez-Marı́n, T. and Duckett, T. (2005). Fast reinforce-
out on the 3-D Gazebo simulator. A wall following ment learning for vision-guided mobile robots. In
test in a real environment is also shown. IEEE Int. Conf. on Robotics and Automation (ICRA),
Using a fisheye camera the environment is per- pages 4170–4175.
ceived in 3D, and it is possible to avoid obstacles that Michels, J., Saxena, A., and Ng, A. (2005). High speed
are invisible to other sensors which are more common obstacle avoidance using monocular vision and rein-
in mobile robotics (laser or ultrasounds). forcement learning. In Proc. Int. Conf. on Machine
In the proposed methodology the states are de- Learning.
fined directly from the image after a simple prepro- Millán, J. R., Posenato, D., and Dedieu, E. (2002).
cessing (Sobel or colour filtering) with no calibration Continuous-action Q-Learning. Machine Learning,
process. With a 3x3 grid, we can define a state space 49:247, 265.
of only 64 states. It has a certain degree of “percep- Moreno, D., Regueiro, C., Iglesias, R., and Barro, S.
tual aliasing”, but RL algorithm converges. We have (2004). Using prior knowledge to improve reinforce-
ment learning in mobile robotics. In Towards Au-
also tested grids of different dimensions with similar
tonomous Robotic Systems (TAROS).
results but greater convergence time. A delicate bal-
ance need be struck between reducing the number of Nakamura, T. and Asada, M. (1995). Motion sketch: Ac-
quisition of visual motion guided behaviors. In IJCAI,
states and avoiding “perceptual aliasing”. pages 126–132.
The proposed codification and methodology is
Nehmzow, U. (1999). Vision processing for robot learning.
general, not specific for the task, and has proved to Industrial Robot, 26(2):121–130.
be efficient and valid, and easy to adapt to distint be-
Regueiro, C., Domenech, J., Iglesias, R., and Correa, J.
haviours. The system works with different types of
(2006). Acquiring contour following behaviour in
reinforcement and filtering. robotics through q-learning and image-based states. In
Various tests were carried out to verify the robust- Proc. XV Int. Conf. on Computer and Information Sci-
ness of the learned behaviours. We used obstacles that ences Engineering (CISE), pages 403–409.
were not detected by the laser device, and walls with Regueiro, C., Rodrı́guez, M., Correa, J., Moreno, D., Igle-
gaps. In both cases the system generalized perfectly sias, R., and Barro, S. (2002). A control architecture
and the results were optimal. If the gaps in the walls for mobile robotics based on specialists. In Leondes,
were large (over 40 cm) a large number of new states C., editor, Intelligent Systems: Technology and Appli-
appeared with respect to the training process, and the cations, volume 6, pages 337–360. CRC Press.
final result deteriorated. Ruiz, J., Montero, P., Martı́n, F., and Matellán, V. (2005).
Future lines of work include on-line real robot Vision based behaviors for a legged robot. In Proc.
Workshop en Agentes Fı́sicos (WAF), pages 59–66.
learning, the integration of several behaviours (e.g.
follow objects and avoid obstacles) and establishing Wyatt, J. (1995). Issues in putting reinforcement learning
onto robots. In 10th Biennal Conference of the AISB,
a mechanism for automatically defining the states of Sheffield, UK.
RL (e.g. neural networks).
156
FORMAL VERIFICATION OF SAFETY BEHAVIOURS OF THE
OUTDOOR ROBOT RAVON
Martin Proetzsch, Karsten Berns

Robotics Research Lab, University of Kaiserslautern, Germany
T. Schuele, K. Schneider
Reactive Systems Group, University of Kaiserslautern, Germany
Keywords: Behaviour-based control, formal verification, outdoor robotics.
Abstract: This paper presents an approach to the formal verification of safety properties of the behaviour-based control
network of the mobile outdoor robot RAVON. In particular, we consider behaviours that are used for the com-
putation of the projected vehicle’s velocity from obstacle proximity sensor data and inclination information.
We describe how this group of behaviours is implemented in the synchronous language Quartz in order to
be formally verified using model checking techniques of the Averest verification framework. Moreover, by
integrating the automatically generated and verified code into the behaviour network, it can be guaranteed that
the robot slows down and stops as required by the given safety specifications.
1 INTRODUCTION
More and more applications like unmanned space
travelling, autonomous farming, civil protection, and
humanitarian demining require autonomous vehicles
navigating in unstructured natural terrain. The di-
versity as well as missing information for physical
models of outdoor scenarios call for a flexible con-
trol architecture not requiring a complete knowledge
of the environment to achieve robust locomotion. In
this context, behaviour-based control networks have
proven suitable for appropriate reaction on external
influences.
One of the advantages of behaviour-based ar-
chitectures is emergence: The combination of be- Figure 1: RAVON in rough outdoor terrain.
haviours leads to proper reactions not directly ex-
plainable by the individual components. On the other by formal verification methods, it is guaranteed that
hand, this feature also poses problems concerning the checked specifications hold under all circum-
predictability, making it difficult to reason about the stances. In particular, different kinds of model check-
correctness of the overall system. However, safety ing (Schneider, 2003; Schuele and Schneider, 2006)
critical applications require proofs that guarantee that are popular verification methods due to the high de-
given specifications are met. In particular, the correct- gree of automation.
ness of reactive control layers is mandatory for the fi- In this paper, we consider the formal verifica-
nal behaviour of the robot. tion of a part of the behaviour-based control network
In circuit design, formal verification has already of the mobile outdoor platform RAVON (Robust
become a standard to avoid design errors. Since Autonomous Vehicle for Off-road Navigation, see
all possible input traces of a system are considered Fig. 1), namely RAVON’s control system that is re-
157
sponsible for slowing down and stopping the vehi-

cle. It is clear that this part is highly safety criti-
cal, and therefore, it is very important to guarantee
its correctness. To this end, we have implemented
this part of the behaviour-based control network in the
synchronous programming language Quartz (Schnei-
der, 2001b; Schneider, 2006). We then used the for-
mal verification methods of the Averest framework
(Schneider and Schuele, 2005) to check the correct-
ness of our implementation with respect to given
safety conditions. Figure 2: Regions monitored by the obstacle detection and
The application of formal methods to verify the avoidance facilities.
correctness of a robot system is not new: In (Diethers
et al., 2003), the model checker HyTech was used to
analyse a robot program based on skill primitive nets. 2 THE OUTDOOR ROBOT
While HyTech considers hybrid automata as system
models to model continuous values of physical prop-
RAVON
erties, our approach is based on discrete transition
systems that benefit directly from symbolic model RAVON is a four wheeled off-road vehicle measur-
checking techniques. The use of symbolic model ing 2.35 m in length and 1.4 m in width and weight-
checking for the formal verification of a robot sys- ing 400 kg. The vehicle features a four wheel drive
tem has been reported in (Sharygina et al., 2004). In with independent motors yielding maximal velocities
contrast to our approach, however, the verification is of 3 m/s. In combination with its off-road tires, the
not integrated with code generation. vehicle can climb slopes of 100% inclination predes-
The use of synchronous languages (Benveniste tining it for the challenges in rough terrain. Front and
et al., 2003) for the implementation of safety critical rear axis can be steered independently which supports
control systems of robots is also not new: In (Sowmya agile advanced driving manoeuvres like double Ack-
et al., 2002), a controller for a mobile robot (Rug erman and parallel steering.
Warrior) has been implemented in the synchronous In order to navigate in a self-dependent fashion,
language Esterel (Berry, 1998). In (Kim and Kang, RAVON has been equipped with several sensors. For
2005), the core of the Samsung Home Robot SHR100 self localisation purposes, the robot uses its odometry,
has been re-engineered to be verified by means of a custom design inertial measurement unit, a mag-
the Esterel framework. In contrast to our approach, netic field sensor, and a DGPS receiver. The sensor
the systems considered in (Sowmya et al., 2002) and data fusion is performed by a Kalman filter (Schmitz
(Kim and Kang, 2005) were quite small control pro- et al., 2006) which calculates an estimated pose in
grams with only a few control states. three dimensions. Due to gravity measurements of the
In contrast to the previous work in this area, our inertial measurement unit, the control system receives
approach considers verification as well as (verified) quite precise absolute data for the roll and pitch angle
code generation. Moreover, the considered system is of the vehicle. These are fed into behaviours that are
not simply a small control program, but a behaviour- responsible for supervising whether the vehicle might
based control network with difficult interdependen- tip over due to critical inclination.
cies. In order to protect the vehicle in respect to ob-
The outline of the paper is as follows: In the stacles, several safety regions are observed by dif-
next two sections, we give some details on the out- ferent sensor systems (Schäfer and Berns, 2006) (see
door robot RAVON and its behaviour-based control Fig. 2). First of all, hindrances can be detected using
system. In Section 4, we describe the verification the stereo camera system mounted at the front of the
framework Averest and give some basics about syn- vehicle. The stereo camera’s narrow field of vision
chronous languages and model checking. Section 5 is compensated by local obstacle memories to either
contains the results of the verification. Finally, we side of the robot realising a short-term representation
conclude with a summary and directions for future of detected obstacles. This obstacle detection facility
work. is complemented with two laser range finders (field
of vision: 180 degrees, angular resolution: 0.5 de-
grees, distance resolution: about 0.5 cm) monitoring
the environment nearby the vehicle. Data from both
sources of proximity data is used for obstacle avoid-
158
achieved by inhibition i ∈ [0, 1] which is used to re-

duce the activation of a behaviour: i = 1 refers to full
inhibition, i = 0 to no inhibition.
Information about the activity of a behaviour is
provided by the output a ∈ [0, 1]. The maximal ac-
tivity is described by a = 1, inactivity by a = 0. It is
defined by the activity function
a(~e, ι, i) = aint (~e) · ι · (1 − i) (2)
Figure 3: Basic behaviour module. where aint (~e) ∈ [0, 1] is an internal function represent-
ing the intended activity of the behaviour.
ance by appropriate behaviours, the fusion of which The target rating r ∈ [0, 1] deals as an indicator
is performed inside the behaviour network.In case of for the contentment of a behaviour. A value of r = 0
emergency, the system is stopped on collision by the indicates that the behaviour is content with the actual
safety bumpers which are directly connected to the state, while r = 1 shows maximal dissatisfaction.
emergency stop to ensure maximal safety. The output vector ~u of a behaviour is determined
using its transfer function F(~e, ι, i) where
F : ℜn × [0, 1]2 → ℜm , F(~e, ι, i) = ~u
3 BEHAVIOUR-BASED This function provides the intelligence of a behaviour,
CONTROL SYSTEM OF RAVON calculating actions depending on input values and in-
ternal representations. This can be a reactive respond
This section introduces the components used for to input values but also a more complex calculation
building up the behaviour-based network controlling as a state machine or sophisticated algorithms. Both
RAVON. reactive and deliberative behaviours can be imple-
mented that way.
3.1 Behaviour Module
3.2 Example Behaviour Roll Stop
The fundamental unit of the proposed control archi-
In this section a behaviour reacting on high roll is de-
tecture is the behaviour module (see Fig. 3). Each
scribed in order to exemplify the behaviour properties
atomic behaviour is wrapped into such a module with
described before. As the behaviour wants to stop the
a defined interface. Behaviours can be described as
vehicle, the output ~u is a velocity of zero. Therefore
three-tuples of the form
the transfer function is:
B = (r, a, F) (1) ~u = vout = 0
This velocity has an effect if the activity a rises:
where r is the target rating function, a is the activ- a = Threshold(roll) · ι · (1 − i)
ity function, and F is the transfer function of the be- Here Threshold(roll) is a function returning 0 or 1
haviour. Additionally each behaviour receives an in- depending on the roll value being below or above a
put vector ~e, an activation ι, and an inhibition i and given threshold. The activity is scaled by the activa-
generates an output vector ~u. tion ι and the inhibition i as stated above. Similarly
More precisely behaviours receive data needed for the target rating r is
fulfilling their work via the sensor input~e ∈ ℜn which r = Threshold(roll)
can be composed of sensory data or information from In case of normal roll angles the behaviour is con-
other behaviours. The output vector ~u ∈ ℜm trans- tent (r = 0) while for high roll angles it is dissatisfied
mits data generated by the behaviour. This output (r = 1).
describes the influence a behaviour can have on the
environment or on other behaviours. 3.3 Fusion Behaviour Module
Each behaviour owns an input determining its ac-
tivation ι ∈ [0, 1]. In this notation ι = 0 indicates de- In case of competing behaviours so called fusion be-
activation and ι = 1 a fully activated behaviour. Val- haviours (see figure 4) are used for coordination. The
ues between 0 and 1 refer to a partially activated be- underlying assumption of the fusion of output values
haviour. Activation can be used to adjust the rele- is that behaviours having a high activity deserve a
vance of competing behaviours. The inverse effect is higher influence on the control than those with lower
159
3.4 Behaviour Network of Ravon
The behaviour network implemented on RAVON is

shown in Fig. 5. Here the flow of control data is
marked as blue drawn through line while data de-
rived from sensors as well as interpreted sensor data
(e.g. the activity output of behaviours used as inhi-
bition input for fusion behaviours) is marked as red
Figure 4: Fusion behaviour module. dashed line.
The behaviour network comprises three control
chains affecting desired rotation, sideward motion,
activity. The interface of fusion behaviours imple-
and velocity of the vehicle. The rotational and
ments a refinement of usual behaviours. For each of
the sideward components are influenced by obstacle
the competing behaviours B i the activity (indicated by
avoidance behaviours. For safety reasons the velocity
~a), target rating (indicated by~r) and output vector ~u is
is adjusted according to obstacle proximity and criti-
provided. The output vector is fed into the fusion be-
cal vehicle inclination. In this behaviour network the
haviour as ~e. Additionally there is a fusion of inhibit-
following types of behaviours are used:
ing behaviours by the inhibition inputs~i. The transfer
function then is the fusion function f (~a,~e) which pro- • Emergency Stop: Stop due to laser scanner data,
cesses these input values to a merged output control positive or negative obstacles detected by the cam-
vector ~u. era system; independently used for forward and
The fusion function can have several implementa- backward motion.
tions, in this work the weighted fusion is used: Here
the control values are weighted with the activity of the • Slow Down: Reduce velocity due to obstacle
corresponding behaviour, leading to a fusion function proximity (laser scanner, positive/negative obsta-
fweighted , where cles); independently used for forward and back-
n−1
ward motion.
∑ a j ·~u j
j=0 • Keep Distance Rotational: Turn away from obsta-
~u = fweighted (a0 ,~u0 , . . . , an−1 ,~un−1 ) = n−1
(3) cles (laser scanner, positive/negative obstacles);
∑ ak independently used for both sides of the vehicle.
k=0
The activity is set according to the weighted input ac- • Keep Distance Sideward: Accomplish sideward
tivities, the activation, and the maximally activated in- motion due to obstacles at the side; independently
hibiting behaviour: used for both sides of the vehicle.
n−1 • Evasion: Evade obstacles at the front by arbitrat-
∑ a2j ing between the keep distance behaviours.
j=0
a= n−1
· ι · (1 − im ) where im = max(il )
l • Point Access: Accessing a given position.
∑ ak
k=0
• Point Access Ranking: Perform ranking manoeu-
The target rating of a fusion behaviour indicates its vres accounting for kinematic constraints.
goal to satisfy highly activated input behaviours and
is calculated as follows: • Trace Back: Follow just driven path backwards in
n−1 order to escape dead ends.
∑ aj ·rj
r=
j=0 The advantage of this approach is the emergent
n−1 vehicle behaviour leading to unforeseen, but suitable
∑ ak reaction on several external influences at a time. How-
k=0
ever, especially the maintenance of vehicle and person
The weighted fusion function provides a subtle safety requires methods for guaranteeing fundamental
gradation of coordinating behaviour control outputs characteristics of the vehicle motion e.g. in critical sit-
regarding their activity. uations. Therefore, it is necessary to formally verify
the behaviour network with respect to a given set of
specifications.
160
Figure 5: Behaviour network of RAVON.
4 THE AVEREST SYSTEM

In this section, we describe the Averest1 frame-
work (Schneider and Schuele, 2005; Schneider and
Schuele, 2006) that provides tools for verifying
temporal properties of synchronous programs (Ben-
veniste et al., 2003; Halbwachs, 1993) as well as for
compiling these programs to equivalent hardware and Figure 6: Averest design flow.
software systems. In particular, many formal verifi-
cation techniques, including model checking of tem- (McMillan, 1992). In the remainder of this section,
poral properties of finite and infinite state systems we describe some background information on Averest
are available. In Averest, a system is described us- focusing on the verification of behaviour networks.
ing the Esterel–like synchronous programming lan- The basic paradigm of synchronous languages
guage Quartz (Schneider, 2001a), and specifications (Benveniste et al., 2003; Halbwachs, 1993) is the
can be given in temporal logics such as LTL and CTL distinction between micro and macro steps in a pro-
(Schneider, 2003). Currently, Averest consists of the gram. From a programmer’s point of view, micro
following tools: steps do not take time, whereas macro steps take one
• ruby: a compiler for translating Quartz programs unit of time. Hence, consumption of time is explicitly
to finite and infinite state transition systems programmed by partitioning the program into macro
• beryl: a symbolic model checker for finite and in- steps. This programming model, referred to as per-
finite state transition systems fect synchrony (Benveniste et al., 2003; Halbwachs,
• topaz: a code generator to convert transition sys- 1993), together with a deterministic form of concur-
tems into hardware and/or software rency allows the compilation of multi–threaded syn-
Figure 6 shows the typical design flow. A given chronous programs to deterministic single–threaded
Quartz program is first translated to a symbolically code. A distinct feature of synchronous languages is
represented transition system in Averest’s Interchange their detailed formal semantics that is usually given
Format AIF that is based on XML. The AIF de- by means of transition rules in structural operational
scription can then be used for verification and code semantics. This makes synchronous languages attrac-
generation. Moreover, there are interfaces to third– tive for safety–critical applications where formal ver-
party tools, e.g. other model checkers such as SMV ification is mandatory.
After translating a Quartz program to a transi-
1 http://www.averest.org tion system, it can be verified using symbolic model
161
checking techniques. For that purpose, the specifi-

cations are given as a set of temporal logic formulas
that describe the desired properties of the system. The
most frequently used properties are safety and live-
ness properties. Intuitively, a safety property states
that a condition invariantly holds on a given path of
the transition system. Similarly, a liveness property
states that a condition holds at least once on a given
path. As an example for a safety property, the formula
AGϕ states that ϕ holds on all possible computation
paths of the system. An example for a liveness prop-
erty is the formula EFϕ, stating that there exists a path
such that ϕ eventually holds.
A breakthrough in formal verification was
achieved in the early nineties, where it was observed
that finite sets can be efficiently represented by means
of binary decision diagrams (BDDs), a canonical nor-
mal form for propositional logic formulas (Bryant,
1986). The development of BDDs was a cornerstone
for symbolic model checking procedures based on fix- Figure 7: Structure of the verified part of RAVON’s be-
point computations (Burch et al., 1990) (see textbooks haviour network.
like (Clarke et al., 1999; Schneider, 2003) for more
details). With sophisticated implementations and re-
finements of symbolic model checking, it has become stop behaviours is active, the velocity given by higher
possible to verify systems of industrial size, and to de- layers (ci velocity) is piped through the three fusion
tect errors that can hardly be found using simulation behaviours without change to co velocity. As soon as
(Clarke and Wing, 1996). one of the mentioned behaviours becomes active (in
case the inclination rises above a threshold or the ob-
stacle distance becomes too low), the active behaviour
uses its activity to inhibit the fusion behaviour which
5 FORMAL VERIFICATION OF is above it. At the same time, it proposes a velocity
THE BEHAVIOUR NETWORK of zero to the underlying fusion behaviour. This fu-
sion behaviour calculates a weighted sum (using the
In case of RAVON, the most important behaviour input activities) of the input velocity values. The more
network property is the control of the vehicle ve- active a behaviour becomes, the less active the fu-
locity due to obstacles or critical inclination. The sion behaviour above is. Therefore, the influence of
behaviours affecting the control in this respect are the slow-down behaviour rises and the velocity output
marked hatched in Fig. 5. In order to formally verify of the underlying fusion behaviour decreases. This
this part of the behaviour network, the correspond- mechanism is implemented on two layers here.
ing behaviours have been implemented in the syn- For this behaviour network, we checked eight
chronous language Quartz. In this way, the correct- specifications including the ones we list below. In this
ness of every single behaviour can be shown by means context, uppercase words indicate system parameters,
of a symbolic model checker. By forming a group of ci indicates controller inputs, co controller outputs,
the mentioned behaviours, it is even possible to verify and si sensor inputs. Numbers are marked as un-
specifications concerning the overall behaviour of the signed integers (0u).
complete velocity control part of the behaviour net- • The output velocity is never higher than the input
work. Moreover, the Quartz code can be exported to velocity:
C code and can be wrapped into a module that re-
places the original (unverified) code. As the output of A G (co_velocity <= ci_velocity);
the verified module is directly transferred to the vehi- • In case of no near obstacle and tolerable inclina-
cle motion actuators, it can be guaranteed that slowing tion, the output velocity equals the input velocity.
down and stopping has the intended effect. Therefore, the vehicle is not slowed down without
The verified and synthesized parts of RAVON’s a reason:
behaviour network are depicted in in Fig. 7. It shows A G ((si_camera_distance
the following structure: If none of the slow-down and >= MAX_VELOCITY_OBSTACLE_DISTANCE)
162
& (si_scanner_distance In our case study, global model checking was able
>= MAX_VELOCITY_OBSTACLE_DISTANCE) to check the specifications up to a sufficiently large
& (si_roll bitwidth. We used CadenceSMV as backend of Aver-
<= MAX_VELOCITY_INCLINATION)
est. In the following table, we list experimental re-
& (si_pitch
<= MAX_VELOCITY_INCLINATION) sults for some bitwidths. For each bitwidth, we list
-> co_velocity == ci_velocity); the number of reachable states, the runtime that was
necessary to verify the system, and the number of re-
• If very high inclination or very near obstacles oc- quired BDD nodes for the entire verification of the
cur, the output velocity is set to zero, i.e., the ve- eight specifications. All experiments were performed
hicle stops:
on a PentiumIV with 512 MByte main memory.
A G (si_roll >= STOP_INCLINATION
-> co_velocity == 0u); Table 1: Experimental results of the verification process.
A G (si_pitch >= STOP_INCLINATION
-> co_velocity == 0u); bits states runtime (s) BDD nodes
A G (si_camera_distance 3 1024 0.95 453
<= STOP_OBSTACLE_DISTANCE
-> co_velocity == 0u);
4 8192 1.59 10012
A G (si_scanner_distance 5 65536 2.54 10063
<= STOP_OBSTACLE_DISTANCE 6 524288 3.87 10521
-> co_velocity == 0u); 7 4194204 5.54 15399
8 33554432 8.85 49500
• In case of a rising roll angle, the vehicle slows
down. Similar specifications hold for the pitch an- In the process of implementing the behaviour net-
gle and for near obstacles: work, the question arose if the fusion behaviours
A G (ci_velocity == MAX_VALUE could implement a maximum fusion function (i.e. the
& si_camera_distance most active behaviour has full influence) instead of
> MIN_VELOCITY_OBSTACLE_DISTANCE the weighted fusion function. By means of formal
& si_scanner_distance verification, it was possible to show that in this case
> MIN_VELOCITY_OBSTACLE_DISTANCE not all specifications were valid. Depending on the
& si_pitch implementation, there was either no slowing down of
< MIN_VELOCITY_INCLINATION
the vehicle (but only abrupt stopping) or it was possi-
& si_roll
> MAX_VELOCITY_INCLINATION ble that the velocity output was higher than the veloc-
& si_roll ity input. Experimental changes concerning the struc-
< MIN_VELOCITY_INCLINATION ture and implementation of the behaviour network
-> co_velocity < MAX_VALUE can therefore be performed with immediate feedback
& co_velocity > 0u ); about the correct properties stated in the specifica-
tions.
In order to avoid vacuous specifications, formulas of
the type AG(ϕ → ψ) are always complemented with
EFϕ (not shown here). In this way, it is guaranteed
that the antecedent of the implication is not always 6 CONCLUSIONS AND FUTURE
false. All of our specifications can be expressed in WORK
the temporal logic CTL. Hence, we can use state-
of-the-art model checking techniques to verify these We presented an approach to the formal verification of
properties. Using global model checking, the model a behaviour-based control network. Without the need
checkers perform a complete traversal of the reach- of testing, it is guaranteed that the specified proper-
able states and thereby check whether the given speci- ties are valid for all possible input traces. Of course,
fications hold. Using local model checking, the model it is necessary to verify more parts of the system or
checkers perform some sort of an induction proof to even the complete behaviour-based network. Due to
avoid a complete state space traversal. the enormous number of states, this inevitably leads to
BDD-based symbolic model checkers do usually the need of improving the model checking approach.
not support floating point numbers. Therefore, inte- Therefore, methods like modular model checking and
gers with a given bitwidth are used for the implemen- abstraction will have to be analysed in this respect.
tation in Quartz instead of floating point numbers. In The uniformity of the behaviours is seen to be an ad-
order to integrate the verified module in the control vantage in this context that can be exploited by tai-
system, a conversion from floating point to fixpoint lored verification and abstraction techniques.
numbers is performed for control and sensor values.
163
ACKNOWLEDGEMENTS Schneider, K. (2001a). Embedding imperative synchronous

languages in interactive theorem provers. In Confer-
ence on Application of Concurrency to System Design
The research work presented in this paper is funded (ACSD), pages 143–156, Newcastle upon Tyne, UK.
by the German Federal State of Rhineland-Palatinate IEEE Computer Society.
within the excellence cluster ”Dependable Adaptive
Schneider, K. (2001b). Exploiting Hierarchies in Temporal
Systems and Mathematical Modeling”. Logics, Finite Automata, Arithmetics, and µ-Calculus
for Efficiently Verifying Reactive Systems. Habilitation
Thesis. University of Karlsruhe.
REFERENCES Schneider, K. (2003). Verification of Reactive Systems –
Formal Methods and Algorithms. Texts in Theoretical
Computer Science (EATCS Series). Springer.
Benveniste, A., Caspi, P., Edwards, S., Halbwachs, N., Le
Guernic, P., and de Simone, R. (2003). The syn- Schneider, K. (2006). The synchronous programming lan-
chronous languages twelve years later. Proceedings guage Quartz. Internal Report (to appear), Department
of the IEEE, 91(1):64–83. of Computer Science, University of Kaiserslautern.
Berry, G. (1998). The foundations of Esterel. In Plotkin, G., Schneider, K. and Schuele, T. (2005). Averest: Specifica-
Stirling, C., and Tofte, M., editors, Proof, Language tion, verification, and implementation of reactive sys-
and Interaction: Essays in Honour of Robin Milner. tems. In Conference on Application of Concurrency
MIT. to System Design (ACSD), St. Malo, France. partici-
pant’s proceedings.
Bryant, R. (1986). Graph-based algorithms for Boolean
Schneider, K. and Schuele, T. (2006). A framework for
function manipulation. IEEE Transactions on Com-
verifying and implementing embedded systems. In
puters, C-35(8):677–691.
Straube, B. and Freibothe, M., editors, Methoden und
Burch, J., Clarke, E., McMillan, K., Dill, D., and Hwang, Beschreibungssprachen zur Modellierung und Veri-
L. (1990). Symbolic model checking: 1020 states and fikation von Schaltungen und Systemen, pages 242–
beyond. In Symposium on Logic in Computer Science 247, Dresden, Germany. GI/ITG/GMM, Fraunhofer
(LICS), pages 1–33, Washington, D.C. IEEE Com- Institut fr Integrierte Schaltungen, ISBN 3-9810287-
puter Society. 1-6.
Clarke, E., Grumberg, O., and Peled, D. (1999). Model Schuele, T. and Schneider, K. (2006). Bounded model
Checking. MIT, London, England. checking for infinite state systems. Formal Methods
in System Design (FMSD). DOI 10.1007/s10703-006-
Clarke, E. and Wing, J. (1996). Formal methods: State 0019-9.
of the art and future directions. Technical Report
CMU-CS-96-178, Carnegie Mellon University. Sharygina, N., Browne, J., Xie, F., Kurshan, R., and Levin,
ftp://reports.adm.cs.cmu.edu/usr/anon/1996/CMU- V. (2004). Lessons learned from model checking a
CS-96-178.ps. NASA robot controller. Formal Methods in System
Design (FMSD), 25(2-3):241–270.
Diethers, K., Firley, T., Krger, T., and Thomas, U. (2003).
A new framework for task oriented sensor based robot Sowmya, A., So, D., and Tang, W. (2002). Design of a mo-
programming and verification. In International Con- bile robot controller using Esterel tools. Electronic
ference on Advanced Robotics (ICAR), pages 1208– Notes in Theoretical Computer Science (ENTCS),
1214, Coimbra,Portugal. IEEE Computer Society. 65(5). Workshop on Synchronous Languages, Appli-
cations, and Programming (SLAP).
Halbwachs, N. (1993). Synchronous programming of reac-
tive systems. Kluwer.
Kim, M. and Kang, K. (2005). Formal construction and
verification of home service robots: A case study. In
Peled, D. and Tsay, Y.-K., editors, International Sym-
posium on Automated Technology for Verification and
Analysis (ATVA), volume 3707 of LNCS, pages 429–
443, Taipei, Taiwan. Springer.
McMillan, K. (1992). The SMV system, symbolic model
checking - an approach. Technical Report CMU-CS-
92-131, Carnegie Mellon University.
Schäfer, H. and Berns, K. (2006). Ravon - an autonomous
vehicle for risky intervention and surveillance. In In-
ternational Workshop on Robotics for risky interven-
tion and environmental surveillance - RISE.
Schmitz, N., Proetzsch, M., and Berns, K. (2006). Pose esti-
mation in rough terrain for the outdoor vehicle ravon.
In 37th International Symposium on Robotics (ISR).
164
A MODIFIED IMPULSE CONTROLLER FOR IMPROVED
ACCURACY OF ROBOTS WITH FRICTION
Stephen van Duin, Christopher D. Cook, Zheng Li and Gursel Alici

Faculty of Engineering, University of Wollongong, Northfields Avenue, Gwynnville, Australia
Keywords: Impulsive control, static friction, limit cycle, stick-slip, impulse shape, friction model, accuracy.
Abstract: This paper presents a modified impulse controller to improve the steady state positioning of a SCARA robot
having characteristics of high non-linear friction. A hybrid control scheme consisting of a conventional PID
part and an impulsive part is used as a basis to the modified controller. The impulsive part uses short width
torque pulses to provide small impacts of force to overcome static fiction and move a robot manipulator
towards its reference position. It has been shown that this controller can greatly improve a robot’s accuracy.
However, the system in attempting to reach steady state will inevitably enter into a small limit cycle whose
amplitude of oscillation is related to the smallest usable impulse. It is shown in this paper that by modifying
the impulse controller to adjust the width of successive pulses, the limit cycle can be shifted up or down in
position so that the final steady state error can be even further reduced.
1 INTRODUCTION error. Static and Coulomb friction can cause a

conventional PID controller having integral action
Precision robot manufacturers continually strive to (I), to overshoot and limit cycle around the reference
increase the accuracy of their machinery in order to position. This is a particular problem near zero
remain competitive. The ability of a robot velocities where friction is highly non linear and the
manipulator to position its tool centre point to within servomechanism is most likely to stick-slip. Despite
a very high accuracy, allows the robot to be used for the above difficulties, PID controllers are still
more precise tasks. For positioning of a tool centre widely used in manufacturing industries because of
point, the mechanical axes of a robot will be their robustness to parameter uncertainty and
required to be precisely controlled around zero unknown disturbances.
velocity where friction is highly non-linear and Stick-slip can be reduced or eliminated by using
difficult to control. impulsive control near or at zero velocities. The
Non-linear friction is naturally present in all impulsive controller is used to overcome static
mechanisms and can cause stick-slip during precise friction by impacting the mechanism and moving it
positioning. In many instances, stick-slip has been by microscopic amounts. By combining the
reduced or avoided by modifying the mechanical impulsive controller and conventional controller
properties of the system; however this approach may together, the PID part can be used to provide
not always be practical or cost effective. stability when moving towards the reference
Alternatively, advances in digital technology have position while the impulse controller is used to
made it possible for the power electronics of improve accuracy for the final positioning where the
servomechanisms to be controlled with much greater error signal is small.
flexibility. By developing better controllers, the By applying a short impulse of sufficient force
unfavourable effects of non-linear friction may be plastic deformation occurs between the asperities of
reduced or eliminated completely. mating surfaces resulting in permanent controlled
Impulse control has been successfully used for movement. If the initial pulse causes insufficient
accurate positioning of servomechanisms with high movement, the impulsive controller produces
friction where conventional control schemes alone additional pulses until the position error is reduced
have difficulty in approaching zero steady state to a minimum.
165
a) b)
Figure 1: Bristle model; Figure a) shows the deflection of a

single bristle. Figure b) shows the resulting static friction Figure 2: The Hirata SCARA robot.
model for a single instance in time.
A number of investigators have devised

impulsive controllers which achieve precise motion
in the presence of friction by controlling the height
or width of a pulse. Yang and Tomizuka (Yang et al,
1988) applied a standard rectangular shaped pulse
whereby the height of the pulse is a force about 3 to
4 times greater than the static friction to guarantee
movement. The width of the pulse is adaptively
adjusted proportional to the error and is used to
control the amount of energy required to move the
mechanism towards the reference positioning. Figure 3: Block diagram of the experimental system
Alternatively, Popovic (Popovic et al, 2000) controller.
described a fuzzy logic pulse controller that
determines both the optimum pulse amplitude and In this paper, a hybrid PID + Impulsive
pulse width simultaneously using a set of controller is used to improve the precision of a
membership functions. Hojjat and Higuchi (Hojjat servomechanism under the presence of static and
et al, 1991) limited the pulse width to a fixed Coulomb friction. The design and functioning of the
duration of 1ms and vary the amplitude by applying controller does not require the mechanism to come
a force about 10 times the static friction. Rathbun et to rest between subsequent pulses, making it suitable
al (Rathbun et al, 2004) identify that a flexible-body for both point to point positioning and speed
plant can result in a position error limit cycle and regulation. The experimental results of this paper
that this limit cycle can be eliminated by reducing show that the shape of the impulse can be optimised
the gain using a piecewise-linear-gain pulse width to increase the overall precision of the controller. It
control law. is shown that the smallest available movement of the
In a survey of friction controllers by Armstrong- servomechanism can be significantly reduced
Hélouvry (Armstrong- Hélouvry et al, 1994), it is without modification to the mechanical plant.
commented that underlying the functioning of these
impulsive controllers is the requirement for the
mechanism to be in the stuck or stationary position 2 MODELLING AND
before subsequent impulses are applied. Thus, EXPERIMENTAL SYSTEM
previous impulse controllers required each small
impacting pulse to be followed by an open loop slide
ending in a complete stop.
2.1 Friction Model
On a broad scale, the properties of friction are both
well understood and documented. Armstrong-
Hélouvry (Armstrong- Hélouvry et al, 1994) have
surveyed some of the collective understandings of
how friction can be modelled to include the
166
A MODIFIED IMPULSE CONTROLLER FOR IMPROVED ACCURACY OF ROBOTS WITH FRICTION
complexities of mating surfaces at a microscopic 2.2 Experimental System

level. Canudas de Wit (Canudas de Wit et al, 1995)
add to this contribution by presenting a new model For these experiments, a Hirata ARi350 SCARA
that more accurately captures the dynamic (Selective Compliance Assembly Robot Arm) robot
phenomena of rising static friction (Rabinowicz, was used. The Hirata robot has four axes named A,
1958), frictional lag (Rabinowicz, 1958), varying B, Z and W. The main rotational axes are A-axis
break away force (Johannes et al, 1973), (radius 350mm) and B-axis (radius 300mm) and
(Richardson et al, 1976), dwell time (Kato et al, they control the end-effector motion in the
1972), pre-sliding displacement (Dahl, 1968), (Dahl, horizontal plane. The Z-axis moves the end-effector
1977), (Johnson, 1987) and Stribeck effect (Olsson, in the vertical plane with a linear motion, while the
1996). The friction interface is thought of as a W-axis is a revolute joint and rotates the end effector
contact between elastic bristles. When a tangential about the Z-axis. A photograph of the robot is
force is applied, the bristles deflect like springs shown in Figure 2.
which give rise to the friction force (Canudas de Wit For these experiments, only the A and B axis of
et al, 1995); see Figure 1(a). If the effective applied the Hirata robot are controlled. Both the A and B
force Fe exceeds the bristles force, some of the axes have a harmonic gearbox between the motor
bristles will be made to slip and permanent plastic and robot arm. Their gear ratios are respectively
movement occurs between each of the mating 100:1 and 80:1. All of the servomotors on the Hirata
surfaces. The set of equations governing the robot are permanent magnet DC type and the A and
dynamics of the bristles are given by (Olsson, 1996): B axis motors are driven with Baldor® TSD series
DC servo drives. Each axis has characteristics of
v high non-linear friction whose parameters are
dz
=v− z (1) obtained by direct measurement. For both axes, the
dt g (v ) static friction is approximately 1.4 times the
Coulomb friction.
Matlab’s xPC target oriented server was used to
g (v) =
1
σ0
(F C + ( Fs − FC ) e
− ( v vs ) 2
) (2) provide control to each of the servomotor drives. For
these experiments, each digital drive was used in
current control mode which in effect means the
dz output voltage from the 12-bit D/A converter gives a
F = σ 0 z + σ 1 (v ) + Fv v (3) torque command to the actuator’s power electronics.
dt The system controller was compiled and run using
Matlab’s real time xPC Simulink® block code. A
σ 1 ( v ) = σ 1e − ( v v d )2
(4) 12-bit A/D converter was used to read the actuator’s
shaft encoder position signal.
where v is the relative velocity between the two
surfaces and z is the average deflection of the 2.3 PID + Impulse Hybrid Controller
bristles. σ0 is the bristle stiffness and σ1 is the bristle Figure 3 shows the block diagram of a PID linear
damping. The term vs is used to introduce the controller + impulsive controller. This hybrid
velocity at which the Stribeck effect begins while controller has been suggested by Li (Li et al, 1998)
the parameter vd determines the velocity interval whereby the PID driving torque and impulsive
around zero for which the velocity damping is controller driving torque are summed together. It is
active. Figure 1(b) shows the friction force as a unnecessary to stop at the end of each sampling
function of velocity. Fs is the average static friction period and so the controller can be used for both
while FC is the average Coulomb friction. For very position and speed control.
low velocities, the viscous friction Fv is negligible The controller can be divided into two parts; the
but is included for model completeness. Fs, FC, and upper part is the continuous driving force for large
Fv are all estimated experimentally by subjecting a scale movement and control of external force
real mechanical system to a series of steady state disturbances. The lower part is an additional
torque responses. The parameters σ0, σ1, vs and vd are proportional controller kpwm with a pulse width
also determined by measuring the steady state modulated sampled-data hold (PWMH), and is the
friction force when the velocity is held constant basis of the impulsive controller for the control of
(Canudas de Wit et al, 1995). stick-slip.
167
The system controller is sampled at 2 kHz. The f p = f p ⋅ sign ( e( k ) ) (7)

impulse itself is sampled and applied at one
twentieth of the overall sampling period (i.e. 100
Hz) to match the mechanical system dynamics. where e(k) is the error input to the controller, |fp|
Figure 4 shows a typical output of the hybrid is a fixed pulse height greater than the highest static
controller for one impulse sampling period τs. The friction and τs is the overall sampling period. For
pulse with height fp is added to the PID output. the experimental results of this paper, the impulsive
Because the PID controller is constantly active, the sampling period τs was 10ms and the pulse width
could be incrementally varied by 1ms intervals. The
Δ pulse width gain kpwm, is experimentally determined
Force by matching the mechanism’s observed
displacement d to the calculated pulse width tp using
fp the equation of motion:
f p ( f p − fC )
PID Output d= t p2
2mf C , fp > 0 (8)
τs
The gain is iteratively adjusted until the net
Figure 4: Friction controller output.
displacement for each incremental pulse width is as
small as practical.
system has the ability to counteract random
disturbances applied to the servomechanism. The
continuous part of the controller is tuned to react to
2.4 Minimum Pulse Width
large errors and high velocity, while the impulse part
The precision of the system is governed by the
is optimized for final positioning where stiction is
most prevalent. smallest incremental movement which will be
For large errors, the impulse width approaches produced from the smallest usable width pulse.
Because the shape of the pulse is affected by the
the full sample period τs, and for very large errors, it system’s electrical circuit response, a practical limit
transforms into a continuous driving torque. When is placed on the amplitude of the pulse over very
this occurs, the combined control action of the PID
short durations and this restricts the amount of
controller and the impulsive controller will be
energy that can be contained within a very thin
continuous. Conversely, for small errors, the PID
pulse. Consequently, there exists a minimum pulse
output is too small to have any substantial effect on
the servomechanism dynamics. width that is necessary to overcome the static
The high impulse sampling rate, combined with a friction and guarantee plastic movement.
small error, ensures that the integral (I) part of the For the Hirata robot, the minimum pulse width
PID controller output has insufficient time to rise guaranteeing plastic displacement was determined to
and produce limit cycling. To counteract this loss of be 2ms and therefore the pulse width is adjusted
driving torque, when the error is below a threshold, between 2 and 10ms. Any pulse smaller than 2ms
the impulsive controller begins to segment into results in elastic movement of the mating surfaces in
individual pulses of varying width and becomes the the form of pre-sliding displacement. In this regime,
primary driving force. One way of achieving this is short impulses can produce unpredictable
to make the pulse width Δ determined by: displacement or even no displacement at all. In some
cases, the mechanism will spring back greater than
the forward displacement resulting in a larger error.
k pwm ⋅ e(k )τ s Figure 5 shows the displacement of the experimental
Δ= if k pwm ⋅ | e( k ) |≤| f p |
fp system of five consecutive positive impulses
followed by five negative impulses. The experiment
Δ =τs otherwise (6) compares impulses of width 2ms and 1.5ms. For
impulses of 2ms, the displacement is represented by
In (6) the consistent staircase movement. For a width of
1.5ms, the displacement is unpredictable with
168
3 LIMIT CYCLE OFFSET

3.1 Motivation
Figure 6 shows the simulated displacements of
varying pulse widths which have been labelled d1,
d2, d3…dn respectively, where d1 is the minimum
pulse width which will generate non elastic
movement and defines the system’s resolution.
Using the variable pulse width PID + impulse
controller for a position pointing task, the torque will
incrementally move the mechanism towards the
reference set point in an attempt to reach steady
state. Around the set point, the system will
inevitably begin to limit cycle when the error e(k) is
approximately the same magnitude as the system
Figure 5: Experimentally measured displacement for both resolution (the displacement for the minimum pulse
positive and negative impulses using successive pulse width d1).
widths 1.5ms and 2ms.
Figure 6: Simulated displacements as a function of pulse

width.
mostly elastic pre-sliding movement which results in Parameter Fs FC σ0 σ1 Fv vs vd

zero net displacement.
Wu et al (Wu et al, 2004) use the pre-sliding Value 2 1 4.5*105 12,000 0.4 0.001 0.0004
displacement as a means to increase the precision of
Figure 7: Simulation of the impulse controller limit
the controller by switching the impulse controller off cycling around a position reference set-point where the
and using a continuous ramped driving torque to final torque output is a pulse with a minimum width and
hold the system in the desired position. The torque is the mean peak to peak oscillation is d1. The friction
maintained even after the machine is at rest. This is parameters used for the simulation are also given in the
difficult in practice as pre-sliding movement must be accompanying table.
carefully controlled in the presence of varying static
friction so that inadvertent breakaway followed by For the limit cycle to be extinguished, the
limit cycling is avoided. controller must be disabled. As an example, the limit
cycle in Figure 7 is extinguished by disabling the
impulse controller at t=0.18s, and in this case, the
resulting error is approximately half the
displacement of the minimum pulse width d1.
169
Limit cycling will occur for all general that the peak to peak centreline of the oscillation has
servomechanisms using a torque pulse because now been shifted away from the reference point.
every practical system inherently has a minimum However, at least one of the peaks of the
pulse width that defines the system’s resolution. oscillation has been shifted closer to the set point. If
Figure 7 simulates a typical limit cycle with a peak the controller is disabled when the mechanism is
to peak oscillation equal to the displacement of the closest to the reference set point, a new reduced
minimum pulse width d1. error is created. For this to be realised, the
One way to automatically extinguish the limit incremental difference in displacement between
cycle is to include a dead-zone that disables the successively increasing pulses must be less than the
controller output when the error is between an upper displacement from the minimum pulse width; for
and lower bound of the reference point (see Figure example d2 – d1 < d1.
7). The final error is then dependent on the amount
of offset the limit cycle has in relation to the 3.3 Modified Controller Design
reference point. Figure 7 shows a unique case where
the ± amplitude of the limit cycle is almost evenly For the limit cycle to be offset at the correct time,
distributed either side of the reference set point; i.e. the impulse controller must have a set of additional
the centre line of the oscillation lies along the control conditions which identify that a limit cycle
reference set point. In this instance, disabling the has been initiated with the minimum width pulse.
controller would create an error e(k) equal to The controller then readjusts itself accordingly using
d1 a ‘switching bound’ and finally disables itself when
approximately . This however, would vary in within a new specified error ‘dead-zone’. One way
2
to achieve this is to adjust the pulse width so that it
practice and the centreline is likely to be offset by
is increased by one increment when satisfying the
some arbitrary amount. The maximum precision of
following conditions:
the system will therefore be between d1 and zero.
if switching bound > |e(k)| ≥ dead-zone
k pwm ⋅ e(k )τ s
reference
d2 - d1 then Δ= +1
position new fp
error
k pwm ⋅ e(k )τ s
Position
otherwise Δ= (9)
fp
where the switching bound is given by:

d1
Time |switching bound| < (10)
2
Figure 8: Conceptual example of reducing the steady state
error using ‘Limit Cycle Offset’ with the limit cycle shifted
up by d2-d1 and the new error that is guaranteed to fall and the dead-zone is given by:
within the dead-zone. (d2 - d1)
dead-zone = (11)
2
3.2 Limit Cycle Offset Function
By controlling the offset of the limit cycle
The steady state error e(k) becomes:
centreline, it is possible to guarantee that the final
error lies within the dead-zone, and therefore to deadzone
e(k)steady state ≤ (12)
increase the precision of the system. As a conceptual 2
example, Figure 8 shows a system limit cycling
either side of the reference point by the minimum
displacement d1. By applying the next smallest
pulse d2, then followed by the smallest pulse d1, the
limit cycle can be shifted by d2 – d1. The effect is
170
3.4 Simulation of the Limit Cycle improvement in the system’s accuracy by a factor of
Offset Function 4.
To demonstrate the limit cycle offset function, the

modified controller is simulated using a simple unit 4 EXPERIMENTAL
mass with the ‘new’ friction model using Eqs. 1 to 4.
A simulated step response is shown in Figure 9 4.1 Position Pointing
to demonstrate how the modified controller works.
Here the mechanism moves towards the reference This section evaluates the limit cycle offset function
set point and begins limit cycling. Because at least using the experimental Hirata robot having position
dependent variables. Figure 10 shows a steady state
limit cycle for a position pointing step response of
0.001 radians using a PID + impulse hybrid
controller. The mean peak to peak displacement of
the smallest non-elastic part of the limit cycle is μd.
The experiment was repeated using the limit
cycle offset function with the same position step
reference of 0.001 radians. Figure 11 shows a
sample experiment and in this example, the limit
cycle offset function is activated at t=0.9s. At this
time, the amplitude of the non elastic part of the
limit cycle is identified as lying between the
switching bounds. The switching bounds and dead-
zone are set according to the methodology given
earlier. Once the offset function is activated, the
controller adjusts itself by forcing the proceeding
pulse to be one increment wider before returning to
the smallest pulse width. This results in the limit
cycle being shifted down into the dead-zone region
where the impulse controller is automatically
Figure 9: Simulation of the limit cycle offset function disabled at t=0.95s. At this time, the final error is
used with the PID + impulse controller. guaranteed to fall within the error dead zone which
can be seen from Fig 11 to be in the vicinity of ±1e-
one of the peaks of the limit cycle immediately lies 4 radians.
within the switching bound, the controller shifts the
peak to peak oscillation by d2 - d1 by applying the
next smallest pulse, and then followed by the
smallest pulse. In this example, the first shift is
insufficient to move either peak into the set dead-
zone so the controller follows with a second shift. At
time 0.1 seconds, the controller is disabled;
however, the elastic nature of the friction model
causes the mechanism’s position to move out of the
dead-zone. As a result, the controller is reactivated
(time 0.12s) and the controller follows with a third
shift. In this instance, the mechanism reaches steady
state at t=0.2s, and the final error is
e(k ) ≤ 12 ⋅ (dead zone) which in this case is ± 1e-6 Figure 10: Steady state limit cycle for the PID + impulse
hybrid controller when applying a unit step input to the
radians. A final analysis of the result shows that the Hirata robot. The mean peak to peak displacement μd is
new controller has reduced the error by an amount the non-elastic part of limit cycle.
significantly more than a standard impulse
controller. This reduction correlates directly to the
171
linear friction where conventional control schemes

alone under perform. A previously developed hybrid
PID + Impulse controller which does not require the
mechanism to come to a complete stop between
pulses has been modified to further improve
accuracy. This modification shifts the limit cycling
into a different position to provide substantial
additional improvement in the mechanism’s position
accuracy. This improvement has been demonstrated
both in simulations and in experimental results on a
SCARA robot arm. The mechanism does not have to
come to a complete stop between pulses, and no
mechanical modification has to be made to the robot.
REFERENCES
Figure 11: Using the ‘Limit Cycle Offset’ function to Armstrong-Hélouvry, B., 1991, “Control of Machines with
reduce the final steady state error of the Hirata robot. Friction” Kluwer Academic Publishers, 1991, Norwell
MA.
Armstrong-Hélouvry, B., Dupont, P., and Canudas de
Wit, C., 1994, “A survey of models, analysis tools and
4.2 Discussion of Results compensation methods for the control of machines
with friction” Automatica, vol. 30(7), pp. 1083-1138.
This set of results demonstrates the Limit Cycle Canudas de Wit, C., Olsson, H., Åström, K. J., 1995 ”A
Offset function can be successfully applied to a new model for control of systems with friction” IEEE
commercial robot manipulator having characteristics Tansactions on Automatic Control, vol. 40 (3), pp.
of high non-linear friction. The results show that the 419-425.
unmodified controller will cause the robot to limit Dahl, P., 1968, “A solid friction model” Aerospace Corp.,
cycle near steady state position and that the peak to El Segundo, CA, Tech. Rep. TOR-0158(3107-18)-1.
Dahl, P, 1977, “Measurement of solid friction parameters
peak displacement is equal to the displacement of
of ball bearings” Proc. of 6th annual Symp. on
the smallest usable width pulse. Incremental Motion, Control Systems and Devices,
By using the Limit Cycle Offset function, the University of Illinois, ILO.
limit cycle can be detected and the pulse width Hojjat, Y., and Higuchi, T., 1991 “Application of
adjusted so that at least one of the peaks of the limit electromagnetic impulsive force to precise
cycle is moved towards the reference set point. positioning” Int J. Japan Soc. Precision Engineering,
Finally, the results show that the controller vol. 25 (1), pp. 39-44.
recognises the limit cycle as being shifted into a Johannes, V. I.., Green, M.A., and Brockley,C.A., 1973,
defined error dead-zone whereby the controller is “The role of the rate of application of the tangential
force in determining the static friction coefficient”,
disabled. The steady state error is therefore
Wear, vol. 24, pp. 381-385.
guaranteed to fall within a defined region so that the Johnson, K.L., 1987, “Contact Mechanics” Cambridge
steady state error is reduced. For the SCARA robot, University Press, Cambridge.
the improvement in accuracy demonstrated was Kato, S., Yamaguchi, K. and Matsubayashi, T., 1972,
1.1e-4 radians in comparison to 4.5e-4 radians “Some considerations of characteristics of static
achieved without the limit cycle offset. friction of machine tool slideway” J. o Lubrication
Technology, vol. 94 (3), pp. 234-247.
Li, Z, and Cook, C.D., 1998, ”A PID controller for
Machines with Friction” Proc. Pacific Conference on
5 CONCLUSION Manufacturing, Brisbane, Australia, 18-20 August,
1998, pp. 401-406.
Advances in digital control have allowed the power Olsson, H., 1996, “Control Systems with Friction”
electronics of servo amplifiers to be manipulated in Department of Automatic Control, Lund University,
a way that will improve a servomechanism precision pp.46-48.
without modification to the mechanical plant. This is Popovic, M.R., Gorinevsky, D.M., Goldenberg, A.A.,
2000, “High precision positioning of a mechanism
particularly useful for systems having highly non-
with non linear friction using a fuzzy logic pulse
172
controller” IEEE Transactions on Control Systems

Technology, vol. 8 (1) pp. 151-158.
Rabinowicz, E., 1958, “The intrinsic variables affecting
the stick-slip process,” Proc. Physical Society of
London, vol. 71 (4), pp.668-675.
Rathbun, D,. Berg, M. C., Buffinton, K. W., 2004,
“Piecewise-Linear-Gain Pulse Width Control for
Precise Positioning of Structurally Flexible Systems
Subject to Stiction and Coulomb Friction”, ASME J
.of Dynamic Systems, Measurement and Control,
vol. 126, pp. 139-126.
Richardson, R. S. H., and Nolle, H., 1976, “Surface
friction under time dependant loads” Wear, vol. 37
(1), pp.87-101.
Wu, R,. Tung, P., 2004, “Fast Positioning Control for
Systems with Stick-Slip Friction”, ASME J .of
Dynamic Systems, Measurement and Control, vol.
126, pp. 614-627.
Yang, S., Tomizuka, M., 1988, “Adaptive pulse width
control for precise positioning under the influence of
stiction and Coulomb friction” ASME J .of Dynamic
Systems, Measurement and Control, vol. 110 (3), pp.
221-227.
173
Diego Pardo and Cecilio Angulo

GREC - Knowledge Engineering Research Group
UPC - Technical University of Catalonia
Avda. Victor Balaguer s/n. Vilanova i la Geltrú, Spain
Keywords: Robot control architecture, Sensorimotor learning, Coordination policy, Reinforcement learning.
Abstract: This paper describes a collaborative control scheme that governs the dynamic behavior of an articulated mobile
robot with several degrees of freedom (DOF) and redundancies. These types of robots need a high level of co-
ordination between the motors performance to complete their motions. In the employed scheme, the actuators
involved in a specific task share information, computing integrated control actions. The control functions are
found using a stochastic reinforcement learning technique allowing the robot to automatically generate them
based on experiences. This type of control is based on a modularization principle: complex overall behavior
is the result of the interaction of individual simple components. Unlike the standard procedures, this approach
is not meant to follow a trajectory generated by a planner, instead, the trajectory emerges as a consequence
of the collaboration between joints movements while seeking the achievement of a goal. The learning of the
sensorimotor coordination in a simulated humanoid is presented as a demonstration.
1 INTRODUCTION the control design philosophy is focused on the ac-

tion performance of the robot and not on the trajectory
Robots with several Degrees of Freedom (DOF) and achievement by its joints; as an alternative, stochastic
redundant configurations are more frequently con- reinforcement learning techniques applied to numeri-
structed; humanoids like Qrio (Kuroki et al., 2003), cal simulation models are studied. An optimal control
Asimo (Hirai et al., 1998) or HRP-2 (Kaneko et al., problem, where a cost function is minimized to com-
2004), and entertainment robots like Aibo (Fujita and pute the policies, is stated.
Kitano, 1998) are examples of it. Complex move- The goal of this work is to solve a multi-joint robot
ments in complex robots are not easy to calculate, the motion problem where coordination is need. Plan-
participation of multiple joints and its synchroniza- ners and trajectory generators are usually in charge of
tion requirements demands novel approaches that en- complex motions where many joints are involved and
dow the robot with the ability of coordination. An the mechanical stabilization of the robot is in risk; the
attempt to drive the dynamics of its body optimally, relationship between sensory signals and motor com-
measuring its performance in every possible config- mands at the level of dynamics is viewed as a low
uration, would bring to a combinational explosion of level brick in the building of control hierarchy. Here
its solution space. we use a dynamic control scheme that also solves the
The habitual use of simplified mathematical mod- trajectory problem.
els to represent complex robotic systems, i.e., approx- It is important to mention that a previous work
imating non-linearities and uncertainties, would result uses coordination at the level of dynamics to extend
in policies that execute approximately optimal con- a manipulator robot capacity (Rosenstein and Barto,
trol, thus, two major assumptions command the devel- 2001), their objective was to use a biologically in-
oped work. First, there is no pre-established mathe- spired approach to profit synergical movements be-
matical model of the physics of the robot’s body from tween joints, like human muscles relationship. They
which a control law could be computed; and second, use a hand-made pre-established PID controllers in
174
every joint of the robot and the final applied torque

in each motor is the linear combination of the PID’s
output, then collaboration is shown. The combina-
tion parameters are obtained by direct search meth-
ods (Rosenstein, 2003). By using linear controllers
to solve a nonlinear problem, they implement a hi-
erarchical motor program that runs various feedback
controllers, i.e, they switch between PID’s (parame-
ters and goal) in the middle of the motion.
Here we extend that result by proposing a more
general scheme, where sensor information is pro-
cessed in independent and specific layers to produce
coordinate control actions. Furthermore, at the ar-
chitecture definition, the control functions are no re- Figure 1: LCC Basic Idea : Two layers of control.
stricted to PID’s and its linear combination. Addition-
ally, we use reinforcement learning to compute all the
architecture controllers. ture is layered; every time step two mappings take
This paper is organized as follows Section 2 is de- place, one for error position reasons and the other
voted to the presentation of the architecture and a lin- seeking coordination between joints.
ear implementation of practical use is outlined in 3; Every controller of the second layer ( f ) uses the
while section 4 validates it through a simulation ex- information originated in all the first layer’s con-
periment of a robot equilibrium dynamical task. Sec- trollers. This collaboration allows joint controllers to
tion 6 gathers the conclusions and points to the future know the dynamical state of the others, and then to
work. act according to it in order to complete the DT.
2.1 Architecture Formulation

2 LAYERED COOPERATIVE Let the articulated mobile robot have n joints. Each
CONTROL joint has an error based controller Di with (i = 1, ..., n)
in the first layer. This is a SISO function, whose input
The Layered Cooperative Control (LCC) scheme is is the error of the position of its respective joint (ei )
meant to generate motions while solving a Dynamical and whose output is a control action (ui ). The error is
Task (DT). It is assumed that the final configuration of calculated using the sensor information providing the
the DT is known, i.e. the set point of the joints angles actual position of the joint (θi ) and the already known
is preestablished; but how to coordinate motions to set point or goal configuration (θ∗i ).
reach this configuration, restricted to its body dynam- ei = θi − θ∗i
ics, is unknown. It is also assumed that the states of
the system are observable and that each joint position ui = Di (ei ) i = 1, ..., n
is commanded by independent servos. Let the dynamical task have associated n controllers fi
Figure 1 represents schematically the main idea in the second layer with (i = 1, ..., n), these are MISO
of the LCC. The control actions that drive the motors functions whose inputs are the n outputs ui of the first
are processed by two layers of controller functions. layer described above. The output of the second layer
Each component of the first layer (D) manipulates the functions vi is the velocity applied on the correspond-
dynamics of a singular joint; the output of this layer ing motor (Mi ).
intends to position its corresponding joint in a pre-
vi = fi (u1 , ..., un ) i = 1, ..., n
viously specified angle, but it is filtered by the sec-
ond layer of controllers ( f ). Each motor is driven by The robot state is represented by a continuous time
a different function, which calculates the control ac- dynamical system controlled by a parameterized pol-
tion based on all the signals originated in the previous icy,
layer. ẋ = g(x, v) + h(x) · v (1)
The controllers of the first layer are related with
the dynamics of the link, they must be established v = πw (x, v) (2)
once. The second layer of functions ( f ) must be de- where vector x represents the states of the robot, g and
signed facing the DT intended to be completed. Func- h its dynamics, v the control action and π the control
tionally and computationally speaking the architec- policy parameterized by the vector w.
175
2.2 LCC Synthesis that the movement has to take place between a limited
amount of time T , where signals are collected to com-
Once the control scheme has been stated, the next pute the cost function (3), then, the gradient is esti-
step is the definition of a methodology to compute the mated using (5) and a new set of parameters obtained
functions ( fi , Di ). Here a policy gradient reinforce- applying the update strategy of (4). Notice that in the
ment learning (PGRL) algorithm is employed. PGRL case of the first layer, each function Dki is trained and
methods (see (Williams, 1992),(Sutton et al., 2000)) updated separately from the others of the same layer,
are based on the measurement of the performance of but when learning the coordinate layer, all the func-
a parameterized policy πw applied as control function tions fwi must be updated at the same time, because
during a delimited amount of time. In order to mea- in this case the performance being measured is that of
sure the performance of the controller, the following the whole layer.
function is defined,
V (w) := J(x, πw (x)) (3)
3 LINEAR IMPLEMENTATION
where the measurement of the performance V of the
parameters w is done by defining the cost function J. The previous description of the LCC is too general
By restricting the scope of the policy to certain to be of practical use. Then, it is necessary to make
class of parameterized functions u = πw (x), the per- some assumptions about the type of functions to be
formance measure (3) is a surface where the maxi- used as controllers. A well known structure to control
mum value corresponds to the optimal set of param- the dynamic position of one link is the Proportional
eters w ∈ Rd . The search for the maximum can be Integral Derivative (PID) error compensator. It has
performed by standard gradient ascent techniques, the following form,
wk+1 = wk + η∇wV (w) (4) Z
dei
ui = KPi · ei + KDi · + KIi · ei dt (7)
where η is the step size and ∇wV (w) is the gradient dt
of V (w) with respect to w. The analytical formulation The functionality of its three terms (KP , KD , KI ) of-
of this gradient is not possible without the acquisition fers management for both transient and steady-state
of a mathematical model of the robot. The numeri- responses, therefore, it is a generic and efficient so-
cal computation is also not evident, then, a stochastic lution to real world control problems. By the use
approximation algorithm is employed : the ‘weight of this structure, a link between optimal control and
perturbation’ (Jabri and Flower, 1992), which esti- PID compensation is revealed to robotic applications.
mates the unknown gradient using a Gaussian random Other examples of optimization based techniques for
vector to orientate the change in the vector parame- tuning a PID are (Daley and Liu, 1999; Koszalka
ters. This algorithm is selected due to its good per- et al., 2006).
formance, easy derivation and fast implementation; The purpose of the second layer is to compute the
note that the focus of this research is not the choice actual velocity to be applied on each motor by gath-
of a specific algorithm, nor the development of one, ering information about the state of the robot while
but rather the cognitive architecture to provide robots processing a DT. The PID output signals are collected
with the coordination learning ability. and filtered by this function to coordinate them. Per-
This algorithm uses the fact that, by adding to w haps the simplest structure to manage this coordina-
a small Gaussian random term z with E{zi } = 0 and tion is a gain row vector Wi , letting the functions in
E{zi z j } = σ2 δi j , the following expression is a sample the second layer be the following,
of the desired gradient
fwi (u) = Wi · u
α(w) = [J(w + z) − J(w)] · z (5)
Then, a linear combination of u commands the coor-
Then, both layers’ controllers can be found using this
dination. The matrix WDTm encapsules all the infor-
PGRL algorithm.
mation about this layer for the mth DT.
ui = Dki (ei ) (6)  
w11 . . . win
vi = fwi (u1 , ..., un )
WDTm =  ... .. ..  (8)

. . 
In order to be consequent with the proposed definition wn1 ... wnn
of each layer, the training of the vector parameters
must be processed independently; first the dynami- Where the term wi j is the specific weight of the jth
cal layer and then the coordination one. It is assumed PID in the velocity computation of the joint i.
176
4 HUMANOID COORDINATION
Classical humanoid robot motions are typically very

slow in order to maintain stability; e.g., the biped
walking based on Zero Moment Point (ZMP) (Vuko-
bratovic and Stepanenko, 1972) condition avoids
those states far from the pre-determined trajectory in
(a) (b)
which the momentum is guaranteed. By doing this,
thousands of trajectories and capabilities of the robot
Figure 2: Simulated Humanoid (a). Joints involved in the
could be restricted. Highly dynamic capabilities need motion. (b) Initial and goal configuration.
those avoided states to perform quick and realistic
motions.
4.1 The Simulation Robot Environment
A simulated humanoid is employed as a test-bed to Figure 3: Direct Solution: Failure Demonstration.

evolve the controllers. It was modeled using Webots
(Michel, 2004), a 3D robot simulation platform . De-
tails can be obtained in (Webots, ).
This particular model of the full body humanoid 4.3 The Learning Procedure
robot has a total of 25 DOF, each link torque is limited
to [−10, 10] Nm, additionally it has a camera, a dis- Table 1 shows the PGRL algorithm. A supervisor pro-
tance sensor, a gps, 2 touch sensors in the feet, and a gram is in charge of the training; its EVALUATE sub-
led in the forehead. The model includes physical char- routine delivers the set of values to be evaluated in
acteristics; the software processes kinematic and dy- the robot controllers. It starts a controlled episode
namic simulation of the interaction between the robot from the initial configuration and intends to achieve
and its environment. the goal states; every evaluation takes T = 12s, within
this time the cost function is measured and returned.
4.2 The One-Leg Equilibrium Task The supervisor repeats this operation until a conver-
gence criterium is matched or the number of iterations
The goal of the simulation experiment is to reach a is too large.
final configuration called ’one-leg equilibrium point’ In this experiment the PID values are learned but not
starting from the passive stand up position. Three calculated. The following is the discrete implementa-
joints are involved in this motion (see Figure 2(a).):
The longitudinal axis of the Back, the transversal axis
of the left hip and the left knee; their goal states, Table 1: Weight perturbation PGRL Algorithm.
i.e. angles in radians, are θ∗1 = 0.4, θ∗2 = 1.5 and
input
θ∗3 = 1.5 respectively, with final velocity θ̇∗ = 0 for step size η ∈ [0, 1]
all the joints. Both configurations, initial and final, search size σ ≥ 0
are shown in Figure 2(b). The goal configuration has max step ∆max ∈ [0, 1]
been designed to generate a slight inclination of the initialize
back to the right side, changing the center of mass of W0 ← I ∈ R3×3 (Identity Matrix)
the body of the robot, thus, letting more weight sup- α←0
ported on the right leg, and then allowing the left leg J ← EVALUATE(W)
repeat
to lift, by means of the blending of the hip and knee 1. z ← N(0, σ)
articulations. 3. Jz ← EVALUATE(W+z)
Figure 3 shows the simulation sequence of the mo- 4. α ← [Jz − J] · z
tion of the robot using standard low level controllers. 5. ∆W ← −ηα
The robot falls down when attempts to reach the goal 6. ∆W ← min(∆W, ∆max )
configuration due to the forces generated by the fric- 7. W ← W + ∆W
8. J ←EVALUATE(W)
tion between the floor and its foot soles. The whole until convergence or number of iterations too large
body of the robot suffers a destabilization at the start return W, J
of the movement.
177
tion of the PID employed,

t
j
uti = KP · eti + KD · (eti − et−1
i ) + KI · ∑ ei (9)
j=0
Where uti represents the control signal associated with

joint i at step time t, it depends on its corresponding
position error eti = θ∗i − θti . This controller must pro-
vide zero position error in a single joint motion, there-
fore its parameters are learned using as performance
criteria the following expression,
T T (a)
Ji (V ) = − ∑ (θ∗i − θti )2 + ∑ (θ˙ti )2 (10)
t=0 t=0
The PID is tested using a time step of ∆T = 32ms

during T = 12s. The algorithms constants used are
σ = 0.032 and η = 0.05. For the learning of the par-
ticular PID in charge of controlling the back, special
restrictions are implemented to the reward function,
penalizing those episodes where the robot falls down.
The penalization is proportional to the time the robot
stays on the ground.
The linear parameterized function selected for the
coordination layer is: (b)
vt = W · ut (11) Figure 4: (a) Back-Joint position output (radians), using a
learned PID (set point=0.3rad), (b) Hip-Joint position out-
where vt ∈ ℜ3 are the actual final velocities; ut ∈ ℜ3 put (radians) using a learned PID (set point=1.5rad).
the outputs of the PIDs; and W ∈ ℜ3×3 is the transfor-
mation that contains the parameters to be found (Wi j ).
The algorithm starts with a diagonal matrix as co-
ordination controller, i.e. W0 = I, meaning that the a better solution is achieved, this is because the train-
first collaborative control function attempted is that in ing of this PID is done in a lay down position, with
which just the original PID controllers act. The non- no risk of falling, and the mass of the link being con-
diagonal values of K deliver coordination. Same algo- trolled is widely insignificant compared with that in
rithms constants and time step are used in this stage the back-joint case.
of the LCC implementation. Final values for the independent joints controllers
(Back, Hip and Knee) are :
PID1 = [0.746946 0.109505 0.0761541]
5 RESULTS PID2 = [0.76541 0.0300045 0]
PID3 = [0.182792 0.0114312 0.00324704]
5.1 PID Controller
5.2 Coordination Layer
Several local minima are found. Figure 4(a) shows the
output of the back joint using one of the PID solutions After several iterations, the best solution is found; a
found. It presents big overshoot, but no stationary er- sequence of the humanoid learned motion is depicted
ror to a set point of θ∗ = 0.3, and mainly: The robot in Figure 5. The associate coordination controller is
learns how ’not to fall’. the following:
The behavior of the Back joint is equivalent with an  
0.6612900 −0.3643360 −0.1936260
inverted pendulum system with a big mass being ma-
nipulated (robot torso). For this joint, and for a better W =  −0.0497653 1.0266600 −0.0684569 
0.0285586 0.1469820 0.9642390
controlled output, a more complex parameterized pol-
icy would be needed. On the other hand, Figure 4(b) Note how the non-diagonal parameters have grown,
presents the output of the hip joint. It is evident that allowing the coordination. This characteristic has
178
6 CONCLUSIONS
The LLC scheme proposes to use the dynamic inter-
action of the robots articulations as a trajectory gen-
Figure 5: Simulated Humanoid One-Leg Equilibrium task.
erator, without the use of a trajectory planner. Ex-
ploiting dynamics gives the robot the ability to ex-
plore motion interactions that result in optimized be-
haviors: Learning at the level of dynamics to succeed
in coordination.
The presented solution overcomes the model de-
pendency pointed above; by the presentation of a sys-
tematic control scheme the ideas of (Rosenstein and
Barto, 2001) are extended. Here, the low level con-
trollers are found using learning techniques and the
formulation of a control architecture allows the im-
plementation of different parameterized policies.
The interaction between Machine Learning, Con-
trol Systems and Robotics creates an alternative for
(a)
the generation of artificial systems that consistently
demonstrate some level of cognitive performance.
Currently, highly dynamic motions in humanoids
are widely unexplored. The capability of this robots
will be extended if motions that compromise the
whole-body dynamic are explored. The LCC is tested
within a simulated humanoid and succeed in the per-
formance of a very fast motion, one in which the
whole-body equilibrium is at risk.
Coordination is possible thanks to the sharing of
information.
(b)
Figure 6: (a) Reward Evolution J(W ) of the best learned so- ACKNOWLEDGEMENTS
lution (last 20 tirals); (b) Back-joint position, before (dotted
line) and after the coordination learning. This work has been partly supported by the Spanish
MEC project ADA (DPI2006-15630-C02-01). Au-
thors would like to thank Olivier Michel and Ricardo
been previously pointed out in (Rosenstein and Barto, Tellez for their support and big help.
2001), but here in an unstable 3D system task.
For this particular solution, the evolution of the
reward is presented in Figure 6(a). Once the robot REFERENCES
learns how ‘not to fall’ the convergence rate increases
and the final results get to lower values. Daley, S. and Liu, G. (1999). Optimal pid tuning using
It is important to mention that the robot presents direct search algorithms. Computing and Control En-
some absolute displacement, tending to go back- gineering Journal, 10(2):251–56.
wards. A better designed reward function is needed Fujita, M. and Kitano, H. (1998). Development of an au-
to avoid absolute displacements, but a good solution tonomous quadruped robot for robot entertainment.
is harder to find in this scenario. Finally, it is impor- Autonomous Robots, 5(1):7–18.
tant to emphasize the change in the behavior of the Hirai, K., Hirose, M., Haikawa, Y., and Takenaka, T. (1998).
back joint after the second layer phase. It seems like The development of honda humanoid robot. In Pro-
the system learns that the overshoot must be shorter in ceedings of the IEEE International Conference on
time in order to be able to lift the leg with out falling Robotics and Automation, ICRA.
down. Figure 6(b). shows the output of the back joint Jabri, M. and Flower, B. (1992). Weight perturbation: An
before and after the second layer of learning. optimal architecture and learning technique for analog
VLSI feedforward and recurrent multilayer networks.
179
IEEE Transactions on Neural Networks, 3(1):154–

157.
Kaneko, K., Kanehiro, F., Kajita, S., Hirukawa, H.,
Kawasaki, T., Hirata, M., Akachi, K., and Isozumi,
T. (2004). Humanoid robot hrp-2. In Proceedings of
the IEEE International Conference on Robotics and
Automation, ICRA.
Koszalka, L., Rudek, R., and Pozniak-Koszalka, I. (2006).
An idea for using reinforcement learning in adaptive
control systems. In Proceedings of the IEEE Int Con-
ference on Networking, Systems and Mobile Com-
munications and Learning Technologies, Kerkrade,
Netherlands.
Kuroki, Y., Blank, B., Mikami, T., Mayeux, P., Miyamoto,
A., Playter, R., Nagasaya, K., Raibert, M., Nagano,
M., and Yamaguchi, J. (2003). A motion creating sys-
tem for a small biped entretainment robot. In Proceed-
ings of the IEEE International Conference on Intelli-
gent Robots and Systems, IROS.
Michel, O. (2004). Webots: Professional mobile robot
simulation. Journal of Advanced Robotics Systems,
1(1):39–42.
Rosenstein, M. T. (2003). Learning to exploit dynamics for
robot motor coordination. PhD thesis, University of
Massachusetts, Amherst.
Rosenstein, M. T. and Barto, A. G. (2001). Robot
weightlifting by direct policy search. In Proceedings
of the IEEE International Conference on Artificial In-
telligence, IJCAI, pages 839–846.
Sutton, R., McAllester, D., Singh, S., and Mansour, Y.
(2000). Policy gradient methods for reinforcement
learning with function approximation. Advances in
Neural Information Processing Systems, 12:1057–
1063.
Vukobratovic, M. and Stepanenko, J. (1972). On the stabil-
ity of anthropomorphic systems. Mathematical Bio-
sciences, 15:1–37.
Webots. http://www.cyberbotics.com. Commercial Mobile
Robot Simulation Software.
Williams, R. J. (1992). Simple statistical gradient-following
algorithms for connectionist reinforcement learning.
Machine Learning, 8:229–256.
180
A FUZZY SYSTEM FOR INTEREST VISUAL DETECTION BASED
ON SUPPORT VECTOR MACHINE
Eugenio Aguirre, Miguel Garcı́a-Silvente, Rui Paúl

Department of Computer Science and A.I., E.T.S. Ingenierı́a Informática
University of Granada, 18071 Granada, Spain
{eaguirre, M.Garcia-Silvente, ruipaul}@decsai.ugr.es
Rafael Muñoz-Salinas
Deparment of Computing and Numerical Analysis, E.P.S.
University of Cordoba, Cordoba, Spain
[email protected]
Keywords: Human-Robot Interaction, Interest Detection, Head Pose Estimation, Fuzzy Logic, Support Vector Machine.
Abstract: Despite of the advances achieved in the past years in order to design more natural interfaces between intelligent
systems and humans, there is still a great effort to be done. Considering a robot as an intelligent system,
determining the interest of the surrounding people in interacting with it is an interesting ability to achieve.
That information can be used to establish a more natural communication with humans as well as to design
more sophisticated policies for resource assignment. This paper proposes a fuzzy system that establishes a
level of possibility about the degree of interest that people around the robot have in interacting with it. First, a
method to detect and track persons using stereo vision is briefly explained. Once the visible people is spotted,
their interest in interacting with the robot is computed by analyzing its position and its level of attention
towards the robot. These pieces of information are combined using fuzzy logic. The level of attention of a
person is calculated by analyzing the pose of his head that is estimated in real-time by a view based approach
using Support Vector Machines (SVM). Although the proposed system is based only on visual information, its
modularity and the use of fuzzy logic make it easier to incorporate in the future other sources of information
to estimate with higher precision the interest of people. At the end of the paper, some experiments are shown
that validate the proposal and future work is addressed.
1 INTRODUCTION over time. People tracking is not an easy task since

several persons could be moving at the same time,
The interaction between Intelligent Systems and hu- crossing their trajectories and occluding each others.
man beings is a topic that is focusing a great research We can find many works in the literature on this topic
effort nowadays. The development of natural and (Fritsch et al., 2003; Snidaro et al., 2005). The tech-
multimodal interfaces is needed to enhance the inter- niques to perform the detection and tracking are fre-
action abilities of current Intelligent Systems. In par- quently based on the integration of different informa-
ticular, within the area of Robotics, the development tion sources such as: skin color, face detectors, visual
of successful robotic systems applied to service tasks analysis of the motion or laser range finder.
in home and office environments implies the genera- Once a robot is able to recognize and track the
tion of natural human-robot interfaces. In that sense, persons in its vicinity, it should be able to detect their
important issues that must be taken into account are interest in establishing an interaction with it. In that
how robots can detect the presence of persons around task, several types of signals from the human can
them and how do they recognize when and how long be taken into account (both verbal and non-verbal).
a person is interested in establishing an interaction. Some authors (Bennewitz et al., 2005) use sound
In order to achieve this goal, it is necessary to solve source localization or speech recognition besides vi-
several problems. First, a robot must be able to de- sual perception to detect which persons are the most
tect persons in its vicinity and track their movements interested. In other cases, facial expressions (Song
181
et al., 2001) or hand gestures (Ghidary et al., 2002) For example, it can be applied in intelligent spaces
are analyzed. Finally, other authors (Kulic and Croft, where one or several intelligent devices wish to inter-
2003) propose the use of non-verbal signals present act with people according the interest shown by each
in physiological monitoring systems that include skin person.
conductance, heart rate, pupil dilation and brain and The remainder of this paper is structured as fol-
muscle neural activity. lows. Section 2 gives an general overview of the
In regards to the role of the fuzzy logic in robotics, hardware and software system, describing the method
an extensive catalogue of the uses of fuzzy logic in employed for the detection and tracking of people in
autonomous robots can be found in (Saffiotti, 1997). the surroundings of the robot. In Section 3 it is ex-
Fuzzy logic has been successfully applied to a multi- plained the SVM based approach to estimate the head
tude of problems such as: design of controlling be- pose and the fuzzy system for estimating the interest
haviors for navigation, behavior coordination, map of people. In Section 4 it is shown the experimenta-
building, integration of deliberative and reactive lay- tion carried out, and finally, Section 5 outlines some
ers, etc (Aguirre and González, 2000). Lately, fuzzy conclusions and future works.
logic has also been applied to the area of human-
robot interaction. In (Bien and Song, 2003) sev-
eral soft computing techniques are applied to service 2 PEOPLE DETECTION AND
robotic systems for comfortable interaction and safe
operation. Fuzzy logic is used for recognizing facial TRACKING
emotional expression and for coordinating bio-signals
with robotic motions. In (Kulic and Croft, 2003) sev- The hardware system is comprised by a laptop to pro-
eral sets of fuzzy rules are used for estimating intent cess the information, a stereoscopic system with a
based on physiological signals. binocular camera (PtGrey, 2005) and a Nomad 200
In this work we are interested in computing a mobile robot (see Fig. 1).
value of possibility of the interest of a person to inter-
act with the robot. This value is computed using only
visual information, but the modularity of the system
makes easy the posterior incorporation of other types
of input data as sound or laser range finder. The inter-
est is computed according to the position of the person
and its degree of attention. In a first step, people de-
tection and tracking problems are solved by a stereo-
scopic system. The use of stereo vision brings sev-
eral advantages when developing human-robot appli-
cations. On the one hand, the information regarding
disparities becomes more invariable to illumination
changes than the images provided by a single camera,
being a very advantageous factor for the background
estimation (Darrell et al., 2001). Furthermore, the Figure 1: Robot with stereo vision system.
possibility to know the distance to the person could
be of great assistance for the tracking as well as for a The ability of detecting and tracking people is fun-
better analysis of their gestures. damental in robotic systems when it is desirable to
Once the surrounding people is spotted, we pro- achieve a natural human-robot interaction. They are
pose a new method for estimating the interest of the achieved in our architecture by combining stereo vi-
detected people in interacting with the robot by means sion and color using plan-view maps. Following, the
of computer vision and fuzzy logic. The person’s at- process for people detection and tracking is explained
tention is detected by the analysis of the pose of his in a summarized way. The readers more interested
head. To detect the head pose we have employed a in this process are referred to (Muñoz-Salinas et al.,
view based approach using Support Vector Machines 2006).
(SVM) (Cristianini and Shawe-Taylor, 2000) that let Our robot has a stereo camera that is mounted
us classify the head pose in real time. on a pan-tilt unit (PTU). The stereo camera captures
The approach presented in this work is not only two images from slightly different positions (cali-
valid for robotic applications. It can also be employed brated stereo pair) that are transferred to the com-
in intelligent systems that use stereoscopic devices. puter to calculate a disparity image containing the
182
A FUZZY SYSTEM FOR INTEREST VISUAL DETECTION BASED ON SUPPORT VECTOR MACHINE
points matched in both images. Knowing the intrin- tracking process by capturing information about the
sic parameters of the stereo camera it is possible to color of the clothes of the user so that the robot can
reconstruct the three-dimensional position pcam of a distinguish him/her from other people in the environ-
matched pixel (u, v). Then, the points captured are ment. Therefore, pixels around what it should be the
translated to a “robot” reference system, placed at chest of the person are used. The position of the chest
the center of the robot at ground level in the direc- in the camera image is estimated as 40 cm below the
tion of the heading of the robot. Generally, the num- top of the head region. The size of the region used to
ber of points captured by our stereo system is very create the color model depends on the distance of the
high. In order to perform a reduction of the amount person from the camera. When the object is far from
of information, the points captured by the camera are the camera the region used is smaller to avoid includ-
orthogonally projected into a 2D plan-view map O ing pixels from the background and it becomes bigger
named occupancy map (Harville, 2004; Haritaoglu when the object is near to the camera.
et al., 2002; Hayashi et al., 2004).
The next step in our processing, is to identify the Tracking consists in detecting in subsequent
different objects present in O that could correspond to frames the human-like object that corresponds to the
human beings (human-like objects). For that purpose, person being tracked. The Kuhn’s well-known Hun-
O is processed with a closing operator in order to link garian Method for solving optimal assignment prob-
possible discontinuities in the objects caused by the lems (Kuhn, 1955) is employed for that purpose.
errors in the stereo calculation. Then, objects are de- Two pieces of information are combined (position and
tected as groups of connected cells. Those objects color) to assign a value to each human-like object in-
whose area are similar to the area of a human being dicating its likelihood to be the person being tracked.
and whose sum of cells (occupancy level of the ob- On one hand, a prediction of the future position of the
ject) is above a threshold θocc are considered human- person being tracked is calculated using the Kalman
like objects. This test is performed in a flexible way filter. The nearer a human-like object is from the
so that it is possible to deal with the stereo errors and position estimated for the person being tracked, the
partial occlusions. However, the human-like objects higher likelihood it will have to be him/her. On the
detected might not belong to real people but to ele- other hand, color information is employed to achieve
ments of the environment. The approach employed a more robust tracking. The more similar the color
in this work to detect a person consists in detecting if of a human-like object is to the clothes’ color of the
any of the human-like objects found in O show a face person being tracked, the higher likelihood it will
in the camera image. have to be him/her. Both likelihood are combined
so that when the person being tracked is near oth-
Face detection is a process that can be time con- ers, color information can help to distinguish him/her.
suming if applied on the entire image, thus, it is only The human-like object with highest likelihood is con-
applied on regions of the camera image where the sidered to be the person being tracked if its likelihood
head of each object should be (head region). As the value exceeds a certain threshold. In that case, the
human head has a typical average width and height, Kalman filter is updated with the new observations
the system analyzes first if the upper part of a human- and also the color model of the person is updated so
like object has similar dimensions. If the object does that it can adapt to the illumination changes that take
not pass this test, the face detector is not applied to place.
it. This test is performed in a flexible manner so
that it can handle stereo errors and people with dif- When the position of the person being tracked
ferent morphological characteristics can pass it. If the is located, the system determines the location of his
human-like object passes this test, the corresponding head in the camera image. In this work, the head is
region in the image is analyzed to detect if it con- modeled as an ellipse whose size in the camera image
tains a face. The face detector employed is based on is determined according to the distance of the person
the face detector of Viola and Jones (Viola and Jones, to the camera. Firstly, the system calculates an initial
2001) which was later improved by Lienhart (Lienhart estimation of the head position in the camera image
and Maydt, 2002). We have employed the OpenCv’s based on stereo information. Then, the initial position
Library (Intel, 2005) implementation that is trained to is refined by a local search process. For that purpose,
detect frontal human faces and works on gray level the gradient around the ellipse perimeter is examined
images. in order to determine the likelihood of a position using
Once a face has been detected on a human-like the Birchfield’s method (Birchfield, 1998). The posi-
object, a color model of the person torso is created tion with higher likelihood is considered the person’s
(Comaniciu et al., 2000). The idea is to assist the head position.
183
Distance Angle Attention Interest
Low Medium High Left Center Right Low Medium High

1 1 1 1 VL L M H VH
0m 1.5m 2.5m 8m 0 deg 90 deg 180 deg 0 1 0 1

(a) (b) (c) (d)
Figure 2: Fuzzy sets of the linguistic variables: (a) Distance (b) Angle (c) Attention (d) Interest.
3 INTEREST DETECTION in interacting with the robot. Thus, the third feature
shown in this paper is the person’s attention detected
In the previous section, we have described as the robot by the analysis of the pose of his head. To detect the
is able to detect and track them the persons in its head pose we have employed a view based approach
vicinity by using the stereo system. This section ex- using SVM that is explained in the next section.
plains our approach for estimating the interest of the
detected people in interacting with the robot by means 3.1 Estimating Face Attention using
of fuzzy logic. The approach presented in this work SVM
is based on stereo vision but the system can be eas-
ily expanded to merge other sources of information. One of the most prominent cues to detect if a person is
The advantages of using fuzzy logic are mainly three. paying attention to the system is the orientation of his
Firstly, the robot has to deal with information from face, i.e., a higher degree of attention can be assumed
the stereo system that is affected by uncertainty and when a person is looking at the system than when it
vagueness. Fuzzy logic is a good tool to manage is backwards. This section describes our approach for
these factors using linguistic variables. Secondly, the face attention estimation.
human knowledge can be usually expressed as rules. We have divided head poses in three main cat-
Fuzzy logic allows to establish relationships among egories: “A” that comprehends all the frontal faces
the variables of a problem through fuzzy rules pro- (faces looking directly at the camera), “B” that com-
viding an inference mechanism. Finally, there are prehends all the slightly sided faces (faces looking to
methods in fuzzy logic to fuse the results from several some point slightly above, below or aside from the
fuzzy rules in order to achieve a final overall result. camera) and “C” that comprehends all the other faces
Therefore, the system designed in this work, based (side faces, faces looking at some point in the ceiling
exclusively in stereo information, can be integrated or ground, backward heads). Figure 3 shows exam-
with other fuzzy systems using other types of infor- ples of each one of the categories employed.
mation as source sound localization, gesture analysis
or speech recognition systems.
In this work, the determination of the degree of
interest of a person is based on its position and its de-
gree of attention. The position of a person is analyzed
using both its distance to the center of the robot and its
angle in respect to the heading direction of the robot.
The first feature is measured by the linguistic variable
Figure 3: Head Pose Estimation: Classes A, B and C.
Distance and the second one by the linguistic variable
Angle. These linguistic variables have three possible
values each of them, that are shown in Fig. 2. The We have created a head pose database comprised
meaning of these two variables is the following: if the by a total of 4000 samples equally distributed among
person is detected near to the robot and more or less the three classes. The database contain images of 21
centered with respect to it, then we consider that the different people (men and women), of different races,
person is more interested in establishing interaction with different hair cuts and some of them wearing
with the robot than when the person is far or at the left glasses. The database samples were manually clas-
or right side of the robot. Nevertheless, the position sified into categories “A”, “B” or “C” according to
of the person is not enough to determine his interest where people were looking at. All the images are
184
gray-scale and 48x40 sized. α = 0.3 that is sufficient to avoid abrupt variations
Since the information contained in the patterns and isolated pose estimation errors.
is redundant, we have applied Principal Component To deal with the uncertainty and vagueness in this
Analysis (PCA) to reduce the data dimensionality. process we use a linguistic variable called “Attention”
PCA (Henry and Dunteman, 1989) is a technique and divide it into “High”, “Medium” and “Low” val-
widely employed for dimensionality reduction able to ues (see Fig. 2). This variable will take as input values
retain those characteristics of the data set that con- the measures of face attention estimation considered
tribute most to its variance, by keeping lower-order by HP (Eq. 1). In figure 2 it is possible to see the
principal components and ignoring higher-order ones. labels for the variable “Attention”.
Such low-order components often contain the “most
important” aspects of the data. The more high-order 3.2 Fuzzy System for Interest
characteristics we remove, the faster the process of Estimation
training and estimation. However, it is important
to use a number of characteristics that allow us to
Once the three linguistic variables have been defined,
achieve good results without affecting our need to
the rules base that integrates them are explained in
have real time results. We made some tests with dif-
this section. The idea that governs the definition of
ferent number of characteristics and we determined
the rules base is dominated by the value of the vari-
that 50 characteristics allowed a good trade-off be-
able Attention. If the attention has an high value the
tween classification accuracy and computing time.
possibility of interest is also high depending on the
The training process has been carried out using
distance and the angle of the person to the robot. If
SVM. For that purpose, we have employed the libsvm
the attention is medium then the possibility of interest
library (free software available in Internet (Chang and
has to be decrease but like in the former case depend-
Lin, 2006)). For more information about SVM, the
ing on the distance and angle. Finally if the attention
interest reader is referred to (Cristianini and Shawe-
is low, it means that the person is not looking at all to
Taylor, 2000). To certificate that results were satisfac- the area where the robot is located and the possibility
tory before applying the model we trained the SVM of interest is defined as low or very low depending on
with 85% of the data set and kept the remainder 15% the other variables. The rules for the case in which
to test the model generated. The result on the test set Attention is High are shown by Table 1. The other
was of 93.14% of accuracy. cases are expressed in a similar way using the appro-
Our system estimates the attention of each de- priate rules. The output linguistic variable is Interest
tected person in real time despite of his/her move- that has the five possible values shown by Figure 2(d).
ments. For that purpose, the head location in the cam-
era image of each person is determined using the tech-
niques described in Sect. 2. The head region of each Table 1: Rules in the case of high Attention.
person is resized to 48x40 pixels and then, the first 50 IF THEN
principal components are extracted and passed to the Attention Distance Angle Interest
SVM classifier. High Low Left High
SVM estimates the head pose in one of the three High Low Center Very High
categories previously indicated. However, the classi- High Low Right High
High Medium Left Medium
fier output is an instantaneous value that does not take High Medium Center High
into account past observations. In order to consider High Medium Right Medium
past observations, we define the variable HP(t) as: High High Left Low
High High Center Medium
High High Right Low
HP(t) = αHP(t−1) + (1 − α)SvmOutt (1)
where SvmOutt is defined on the basis of the classifier Finally to compute the value of possible interest, a
output as: fuzzy inference process is carried out using the opera-
tor minimum as implication operator. Then the output
 fuzzy sets are aggregated and the overall output is ob-
1
 if current output of SVM = “A”; tained. The overall output fuzzy set can be understood
SV MOutt = 0.5 if current output of SVM = “B”; as a possibility distribution of the interest of the per-
son in the [0, 1] interval. Therefore values near to 1

0 if current output of SVM = “C”.
mean a high level of interest and vice versa.
In Eq. 1, α is a weighting factor that ponders the
influence of past observations. In this work we set
185
Figure 4: Interest according distance and attention. Frames in each row (a, b and c), (d, e and f ) and (g, h and i) show the
person at different distances while frames in each column (a, d and g), (b, e and h) and (c, f and i) show the person with
different attention.
4 EXPERIMENTATION from this video can be seen in Fig. 4. In this figure,

frames a, b, and c show the person close to the robot
A broader experimentation has been done to validate (”Low Distance”) and looking in different ways. In-
our system. All of these experimentations results terest is higher when when person looks towards the
were very satisfactory in respect to interest estimation robot. In frames d, e and f it is possible to observe
using our system. Because of space reasons we opted the same situation but at a higher distance (”Medium
to describe in detail only one of the experimentations. Distance”). Therefore the final interest value com-
To perform the stereo process we have used images of puted is slightly lower. In frames g, h and i we have
size 320x240 and sub-pixel interpolation to enhance the same person in the same situations (in respect to
the precision in the stereo calculation. The operation where he is looking at) but at even a higher distance
frequency of our system is about 30 Hz without con- from the robot. Therefore the final interest value com-
sidering the time required for stereo computation. puted is even lower. It is also possible to observe that
Regarding the interest estimation, we have when the person is looking approximately the same
checked that the interest degree assigned to each way (in frames a, d, g person is showing low atten-
tracked person increases and decreases dynamically tion, in frames b, e, h person is showing medium at-
accordingly to the behavior of the person in relation tention and in frames c, f , i he is showing high atten-
to the robot. To test it, one person has been recorded tion) interest varies usually according distance. The
moving in front of the robot in a manner that it was closer to the robot the higher the interest.
possible to have frames of the person in all the situ-
ations regarding “Attention” and “Distance”. Frames As it was expected, the higher value of interest
186
Figure 5: Graphs that show the variation of variables “Attention” (a), “Distance” (b), “Angle” (c) and “Interest” (d) during the
video referred in Section 4.
is achieved in frame c, because the person is look- 5 CONCLUSIONS AND FUTURE

ing directly at the robot and it is very close to it. On WORK
the other hand, in frame g the person is not paying
any attention to the robot, moving at a distance far-
away from it. Therefore the lowest interest value is In this paper we have shown a system for detecting,
achieved in this frame. tracking and estimating the interest of the people in
It is also possible to observe in the graphs showed the surroundings of a mobile robot, using stereo vi-
in Fig. 5 the variation of variables “Attention”, “Dis- sion, head pose estimation by SVM and fuzzy logic.
tance” and “Angle” in Fig. 5.a, Fig. 5.b and Fig. 5.c As a first step, the method for people detection and
respectively during the whole video. The forth graph tracking has been briefly shown. While a person is
Fig. 5.d is the interest computed also for the whole being tracked, the fuzzy system computes a level of
video. It is also possible to observe in these graphs possibility about the interest that this person has in
the relationship among the interest computed and the interacting with the robot. This possibility value is
other variables. based on the position of the person in relation with
In order to better understand the per- the robot, as well as on an estimation of the atten-
formance of the system, several videos tion that the person pays to the robot. To examine the
are available in the following web site attention that a person pays to the robot we analyze
http://decsai.ugr.es/∼ruipaul/interest.htm. in real time the head pose of the person. This analy-
sis is solved by a view based approach using Support
Vector Machines. Thanks to SVM head pose can be
detected achieving a great percentage of success that
187
is no dependent of the morphological features of the Fritsch, J., Kleinehagenbrock, M., Lang, S., Plötz, T., Fink,
heads. The experimentation shows that the system is G. A., and Sagerer, G. (2003). Multi-modal anchor-
able to detect the persons present in its vicinity, track ing for human-robot interaction. Robotics and Au-
tonomous Systems, 43(2-3):133–147.
their motions and give a value of possible interest on
the interaction of the persons with the robot. Ghidary, S. S., Nakata, Y., Saito, H., Hattori, M., and
Takamori, T. (2002). Multi-modal interaction of hu-
The proposed method can be easily updated in man and home robot in the context of room map gen-
future works to analyze other types of input data as eration. Autonomous Robots, 13(2):169–184.
sounds or laser range finder. Also, the degree of in- Haritaoglu, I., Beymer, D., and Flickner, M. (2002). Ghost
terest will be useful to plan the actions of the robot 3d: detecting body posture and parts using stereo. In
towards the persons in order to allow a more natural Workshop on Motion and Video Computing, pages 175
human-robot interaction. – 180.
Harville, M. (2004). Stereo person tracking with adaptive
plan-view templates of height and occupancy statis-
tics. Image and Vision Computing, 2:127–142.
ACKNOWLEDGEMENTS Hayashi, K., Hashimoto, M., Sumi, K., and Sasakawa, K.
(2004). Multiple-person tracker with a fixed slanting
This work has been partially supported by the Span- stereo camera. In 6th IEEE International Conference
ish MEC project TIN2006-05565 and Andalusian Re- on Automatic Face and Gesture Recognition, pages
681–686.
gional Government project TIC1670.
Henry, G. and Dunteman (1989). Principal Components
Analysis. SAGE Publications.
Intel (2005). OpenCV: Open source Computer Vision li-
REFERENCES brary.
Kuhn, H. W. (1955). The hungarian method for the assign-
Aguirre, E. and González, A. (2000). Fuzzy behaviors for ment problem. Naval Research Logistics Quarterly,
mobile robot navigation: Design, coordination and fu- 2:83–97.
sion. International Journal of Approximate Reason- Kulic, D. and Croft, E. (2003). Estimating intent for hu-
ing, 25:255–289. man robot interaction. In International Conference on
Bennewitz, M., Faber, F., Joho, D., Schreiber, M., and Advanced Robotics, pages 810–815.
Behnke, S. (2005). Integrating vision and speech Lienhart, R. and Maydt, J. (2002). An Extended Set of
for conversations with multiple persons. In IROS’05: Haar-Like Features for rapid Object detection. In
Proceedings of the IEEE/RSJ Int. Conf. on Intelligent IEEE Conf. on Image Processing, pages 900–903.
Robots and Systems, pages 2523 – 2528.
Muñoz-Salinas, R., Aguirre, E., and Garcı́a-Silvente, M.
Bien, Z. and Song, W. (2003). Blend of soft computing (2006). People detection and tracking using stereo vi-
techniques for effective human-machine interaction sion and color. To appear in Image and Vision Com-
in service robotic systems. Fuzzy Sets and Systems, puting. Available online at www.sciencedirect.com.
134(1):5–25. PtGrey (2005). Bumblebee. Binocu-
Birchfield, S. (1998). Elliptical Head Tracking Using Inten- lar stereo vision camera system.
sity Gradients and Color Histograms. In IEEE Con- http://www.ptgrey.com/products/bumblebee/index.html.
ference on Computer Vision and Pattern Recognition, Saffiotti, A. (1997). The uses of fuzzy logic in autonomous
pages 232–237. robot navigation. Soft Computing, 1:180–197.
Chang, C. and Lin, C. (2006). Libsvm. Snidaro, L., Micheloni, C., and Chiavedale, C. (2005).
a library for support vector machines. Video security for ambient intelligence. IEEE Trans-
http://www.csie.ntu.edu.tw/ cjlin/libsvm/. actions on Systems, Man and Cybernetics, Part A,
35:133 – 144.
Comaniciu, D., Ramesh, V., and Meer, P. (2000). Real-Time
Tracking of Non-Rigid Objects using Mean Shift. In Song, W., Kim, D., Kim, J., and Bien, Z. (2001). Visual
IEEE Conference on Computer Vision and Pattern servoing for a user’s mouth with effective intention
Recognition, volume 2, pages 142–149. reading in a wheelchair-based robotic arm. In ICRA,
pages 3662–3667.
Cristianini, N. and Shawe-Taylor, J. (2000). An Introduction
To Support Vector Machines (and other Kernel Based Viola, P. and Jones, M. (2001). Rapid object detection using
Methods). Cambridge University Press. a boosted cascade of simple features. In IEEE Conf.
on Computer Vision and Pattern Recognition, pages
Darrell, T., Demirdjian, D., Checka, N., and Felzenszwalb, 511–518.
P. (2001). Plan-view trajectory estimation with dense
stereo background models. In Eighth IEEE Interna-
tional Conference on Computer Vision (ICCV 2001),
volume 2, pages 628 – 635.
188
POSTERS
DATA INTEGRATION SOLUTION FOR PAPER INDUSTRY
A Semantic Storing Browsing and Annotation Mechanism for Online Fault Data
Sergiy Nikitin, Vagan Terziyan

Agora Center, University of Jyväskylä, Mattilanniemi 1, Jyväskylä, Finland
Jouni Pyötsiä
Metso Automation
[email protected]
Keywords: Semantic Web, Semantic Storage, Information Integration, Paper Industry.
Abstract: A lot of IT solutions exist for simplification and time saving of industrial experts’ activities. However, due
to large diversity of tools and case-by-case software development strategy, big industrial companies are
looking for an efficient and viable information integration solution. The companies have realized the need
for an integrated environment, where information is ready for extraction and sophisticated querying. We
present here a semantic web-based solution for logging and annotating online fault data, which is designed,
and implemented for a particular business case of a leading paper machinery maintenance and automation
company.
1 INTRODUCTION create a huge amount of new information during the

life cycles of machines and systems. Message flow
Rapid changes and discontinuities in the 21st management and compression to on-line knowledge
century business environment will challenge are already a demanding issue for the logging of
companies with the growing demand for competitive product-related activities. On the other hand,
advantages within their business solutions. To optimization requirements demand more effective
ensure high flexibility, sustainable growth and knowledge utilization and the speeding up of
profitability, companies have to search for new network-based learning in the process of
innovative approaches to products and services collaboration between different resources.
development. New innovative business solutions call Industry challenges the IT-sector with the new
for strong integration of automation technology, requirements that are dictated by the need to offer
information and communication technology (ICT), essentially new services to customers in order to be
and business processes. At the same time, embedded competitive in the market. These requirements may
intelligence in different machines and systems gives become hard to meet using conventional tools and
new possibilities for automated business process approaches. The growth in the information volumes
operation over the network throughout the machines we want to store and process by integrating data
and systems life cycles. from different sources leads to an unprecedented
The new emerging remote service solutions level of complexity. Modern Enterprise Resource
imply that products transform into life cycle services Planning (ERP) systems are trying to provide
and these services, in turn, transform into customers' integrated solutions for large companies. However
service processes. Business messages coming from the installation and adjustment of such systems may
intelligent machines and systems drive these take a half a year, involving hundreds of consultants
processes, utilizing embedded intelligence and ICT and subcontractors.
solutions. The current trend towards more open service-
In the future, a variety of collaborative resources, based computing environments is a new approach to
like intelligent machines, systems and experts, will componentization and components distribution.
Service-oriented architecture aims to achieve a new
191
level of reusability and business process flexibility; Customer Site 1 Metso Site
Service Network
however, to ensure the interoperability between the Legacy Global
components we need a common “glue” that would Systems Administrator
Firewall
Interface
adjust the semantics of the data being exchanged. Site
Hub
The Semantic Web technology (Berners-Lee, T., et
Firewall
Central
Hub
al., 2001) introduces a set of standards and
languages for representation of a domain model with Local Administrator
the explicit semantics. The main instrument of Interface
Single Devices
domain model construction is ontology, which Customer Site 2 GPRS/
allows for domain data representation in a GSM
Fi
formalized and unified way.
re
w
al
In this paper we present a solution that utilizes Site Firewall Customer Site n
l
Hub
Semantic Web technology to provide a tool for Site
Legacy
Hub Systems
online maintenance data browsing, analysis and
annotation. We utilize experience obtained in the Local Administrator
SmartResource project (SmartResource, 2006) and Interface Local Administrator
apply the General Adaptation Framework (Kaykova Interface
et al., 2005) to align data with the domain ontology.
The paper is organized as follows: In the next Figure 1: Site Hub network architecture.
section we describe the paper machinery ICT
infrastructure, and Section 3 presents the solution we Message Center is the main component of the
have developed using Semantic Web tools and Site Hub. It checks the validity of messages and
standards. We end with conclusions and future work. routes them to the correct receivers. Messaging in
Site Hub is based on Web Services technology.
Hub-based integrated infrastructure combined
with secure connectivity allows easy incorporation
2 IT INFRASTRUCTURE IN of new business logic on both customer and Metso
PAPER INDUSTRY sites. A messaging mechanism between customers
and Metso provides a very flexible medium for
Metso Corporation is a global supplier of process information exchange and new service provisioning.
industry machinery and systems as well as know-
how and aftermarket services. The corporation's core
businesses are fiber and paper technology, rock and 3 LOGGING AND ANNOTATION
minerals processing, and automation and control
technology. Metso's strategy is based on an in-depth OF MAINTENANCE DATA
knowledge of its customers' core processes, close
integration of automation and ICT, and a large The main purpose of the system we present here is
installed base of machines and equipment. Metso's to store alarm data, generated by paper machine
goal is to transform into a long-term partner for its monitoring systems. When an alarm happens, a
customers. Based on the remote service SOAP/XML message (SOAP, 2003) is generated
infrastructure, it develops solutions and services to and sent to the Site Hub, which then forwards it to
improve efficiency, usability and quality of the Central Hub. We have established a message
customers' production processes throughout their flow from the Central Hub to the computer at the
entire life cycles. university network, where messages are processed
by our system.
2.1 Remote Service Infrastructure
3.1 Architecture of the System
Metso's remote service infrastructure consists of a
service provider's Central Hub and several The system can be divided into two main
customers' Site Hubs, which are integrated over the subcomponents – Message Handler and Message
network (see Figure 1). Browser (see Figure 2).
The key issues in a Site Hub solution are: open Message Handler receives and processes
standards, information security, reliability, SOAP/XML messages from customers. It invokes
connectivity and manageability. the Adapter to transform the XML content into an
192
DATA INTEGRATION SOLUTION FOR PAPER INDUSTRY - A Semantic Storing Browsing and Annotation
Mechanism for Online Fault Data
RDF-graph (RDF, 2004) object and store it in the structure for labelling groups of messages with an
Sesame RDF storage (Sesame). expert’s decision and has references to instances of
Metso Site Browsing and Annotation tool the Message class.
The Message Browser component provides a
Central web-based interface for browsing and filtering
Hub messages stored in the RDF-storage, according to
user-defined filtering criteria.
SOAP
message The purpose of message filtering is to distinguish
the groups of messages leading to exceptional
situations. The expert provides annotations for
MESSAGE MESSAGE message groups which are stored to the RDF-storage
XML HTML XML
HANDLER BROWSER
and that can be used as samples for machine learning
Web Query algorithms. The client-server interaction is
Service Adapter SeRQL Results Graph
implemented using AJAX technology (Garrett,
2005), which provides a more dynamic script-based
interaction with the server (see Figure 4). When a
RDF graph user performs any action that requires invocation of
Application server
server functionality, the script on a client side wraps
Figure 2: Architecture of the system. the required parameters into XML format and sends
it to the server. For example, in order to filter the
The RDF storage contains an Ontology that plays messages, a user selects the needed parameters and
the role of a schema for all data within the storage. specifies parameter values within the corresponding
Based on the analysis of SOAP/XML messages, we textboxes (see Figure 4).
have defined main concepts (classes) with the MESSAGE BROWSER
Browsing
corresponding properties (see Figure 3).
XML SeRQL
ExpertAnnotation
Control
Servlet
-annotationName HTML Query
Expert XML Results
-numOfMessagesReferred
Script
AJAX
-expertName
Annotation
-hasAnnotation -annotationTime HTML XML Graph
-annotationDescription
Web
-madeByExpert Application server
Browser
-messageReference
Figure 4: Client-server interaction.
Message Alarm
-messageUID -value
-failureDescription On the server side, the Control Servlet handles
-securityLevel
-lowLimit the XML document. For filtering, it generates a
-time
-tag SeRQL query (Broekstra, 2004) and executes it. On
-hash -alarmTime the client side, a dedicated callback script function
-hasAlarm -highLimit
-status processes the response and shows the result in a web
-receiverGroup
-productionLine browser.
-hasMessageType -situationDescription
-messageSender -alarmSource
-customer
3.2 Integration with the Agent
-messageReceiver
-measurementUnit Platform
Figure 3: Ontology classes. We realize that the extension of the system will
challenge the complexity of development and
The Message class describes such message maintenance. That is why, following the autonomic
properties as message sender and receiver, message computing paradigm (Kephart, 2003), we have
reception time, etc. The Message class also refers to tested agent-based scenario (see Figure 5)
an Alarm class, which contains information about implemented on a JADE agent platform
the reason for message generation, such as (Bellifemine; 2001). We have assigned an agent to
measurements of sensors, status data and exact manage RDF-storage activities (Metso Storage
module of the production line where the alarm Agent) and provided a Metso Expert Agent to
happened. The ExpertAnnotation class defines the interact with a maintenance expert.
193
Metso Site Browsing and Annotation tool complexity, we introduce self-manageable entities in
the agent-based communication scenario.
Central
Hub
SOAP/XML ACKNOWLEDGEMENTS
This research has been supported by the
MESSAGE MESSAGE
HANDLER BROWSER
XML HTML XML SmartResource project, funded by TEKES, and the
industrial consortium of Metso Automation,
Web
Service Query TeliaSonera and ABB. The preparation of this paper
SeRQL Results Graph
was partially funded by the COMAS graduate school.
Application server REFERENCES

RDF graph
METSO METSO EXPERT AGENT Bellifemine, F., Poggi, A., and Rimassa, G. 2001. JADE: a
STORAGE SOAP
FIPA2000 compliant agent development environment.
AGENT Adapter In Proceedings of the Fifth international Conference
KML
SOAP/XML
on Autonomous Agents (Montreal, Quebec, Canada).
AGENTS '01. ACM Press, New York, NY, 216-217.
Real-Time DOI= http://doi.acm.org/10.1145/375735.376120
Monitoring Berners-Lee, T., Hendler, J., and Lassila, O. (2001) The
tool
Semantic Web, Scientific American, Vol. 284, No. 5,
pp. 34-43.
Figure 5: Agent-enabled system. Broekstra, J., Kampman A., and F. van Harmelen.
Sesame: An Architecture for Storing and Querying
The messages coming from customers are RDF Data and Schema Information. In D. Fensel, J.
handled by the Metso Storage Agent, which Hendler, H. Lieberman, and W.Wahlster, editors,
incorporates Adapter to perform transformation and Semantics for the WWW. MIT Press, 2001.
storage. Then, the Metso Storage Agent sends the Broekstra, J., Kampman, A. SeRQL: An RDF query and
transformation language. In Proceedings of the
message to the Metso Expert Agent, which updates
International Semantic Web Conference, ISWC 2004,
the situation on a Real-time Monitoring Tool and Hiroshima, Japan.
provides an expert with the message content and a Garrett, J., Ajax: A New Approach to Web Applications
link to the browsing and annotation tool. (white paper).,http://www.adaptivepath.com/publica-
tions/essays/archives/000385.php, February 2005.
Kaykova O., Khriyenko O., Kovtun D., Naumenko A.,
Terziyan V., Zharko A., General Adaption
4 CONCLUSIONS Framework: Enabling Interoperability for Industrial
Web Resources, In: International Journal on Semantic
Although we have succeeded with the Web and Information Systems, Idea Group, ISSN:
implementation of the solution presented here, there 1552-6283, Vol. 1, No. 3, July-September 2005,
are still many issues to cope with in order to meet pp.31-63.
key industrial requirements, such as scalability, Kephart J.O., Chess D.M., 2003. The vision of autonomic
maintainability and robustness. RDF-storages can computing, IEEE Computer, Vol. 36, No. 1, pp. 41-50
handle billions of triples, but there are no mature RDF – Resource Description Framework, A W3C
Recommendation, Feb 2004, http://www.w3.org/RDF/
semantic storage-oriented development patterns or
SmartResource – a TEKES funded project, http://www.
guidelines. Nevertheless, the simplicity and cs.jyu.fi/ai/OntoGroup/SmartResource_details.htm
efficiency of querying, as well as model extending, SOAP – Simple Object Access Protocol, A W3C
provide incontestable arguments in favour of Recommendation, 2003, http://www.w3.org/TR/soap/
semantic data storages. The ontological domain
model brings more benefits to customers when there
are more sources integrated. However, the
complexity of such a system, if developed using
conventional approaches, will be too burdensome to
maintain and extend. In order to distribute the
194
FAST COMPUTATION OF ENTROPIES
AND MUTUAL INFORMATION FOR MULTISPECTRAL IMAGES
Sié Ouattara, Alain Clément and François Chapeau-Blondeau

Laboratoire d’Ingénierie des Systèmes Automatisés (LISA), Université d’Angers
62 avenue Notre Dame du Lac, 49000 Angers, France
Corresponding author: [email protected]
Keywords: Multispectral images, Entropy, Mutual information, Multidimensional histogram.
Abstract: This paper describes the fast computation, and some applications, of entropies and mutual information for
color and multispectral images. It is based on the compact coding and fast processing of multidimensional
histograms for digital images.
1 INTRODUCTION the cells of the D-dimensional histogram are in fact

empty. For example, for a common 512 × 512 RGB
Entropies and mutual information are important tools color image with D = 3 and Q = 256 = 28 , there are
for statistical analysis of data in many areas. For im- QD = 224 ≈ 16 × 106 colorimetric cells with at most
age processing, so far, these tools have essentially only N = 5122 = 262 144 of them which can be occu-
been applied to scalar or one-component images. The pied. We developed the idea of a compact represen-
reason is that these tools are usually derived from tation of the D-dimensional histogram (Clément and
multidimensional histograms, whose direct handling Vigouroux, 2001; Clément, 2002), where only those
is feasible only in low dimension due to their memory cells that are occupied are coded. The D-dimensional
occupation and related processing time which become histogram is coded as a linear array where the entries
prohibitively large as the number of image compo- are the D-tuples (the colors) present in the image and
nents increases. Here, we use an appraoch for multi- arranged in lexicographic order of their components
dimensional histograms allowing compact coding and (X1 , X2 , . . . XD ). To each entry (in number ≤ N) is as-
fast computation, and show that this approach easily sociated the number of pixels in the image having this
authorizes the computation of entropies and mutual D-value (this color). An example of this compact rep-
information for multicomponent or multispectral im- resentation of the D-dimensional histogram is shown
ages. in Table 1.
The practical calculation of such a compact his-
togram starts with the lexicographic ordering of the
2 A FAST AND COMPACT N D-tuples corresponding to the N pixels of the im-
age. The result is a linear array of the N ordered D-
MULTIDIMENSIONAL tuples. This array is then linearly scanned so as to
HISTOGRAM merge the neighboring identical D-tuples while accu-
mulating their numbers to quantify the correspond-
We consider multispectral images with D components ing population of pixels. With a dichotomic quick
Xi (x1 , x2 ), for i = 1 to D, each Xi varying among Q sort algorithm to realize the lexicographic ordering,
possible values, at each pixel of spatial coordinate the whole process of calculating the compact multidi-
(x1 , x2 ). A D-dimensional histogram of such an im- mensional histogram can be achieved with an average
age would comprise QD cells. For an image with complexity of O(N log N), independent of the dimen-
N1 × N2 = N pixels, only at most N of these QD cells sion D. Therefore, both compact representation and
can be occupied, meaning that, as D grows, most of its fast calculation are afforded by the process for the
195
Table 1: An example of compact coding of the 3- simply by a linear scan of the array while summing
dimensional histogram of an RGB color image with Q = the terms −p(~X) log p(~X) with p(~X) read from the
256. The entries of the linear array are the components last column. This preserves the overall complexity of
(X1 , X2 , X3 ) = (R, G, B) arranged in lexicographic order, O(N log N) for the whole process leading to H(~X).
for each color present in the image, and associated to the
poupulation of pixels having this color.
The compact histogram also allows one to en-
visage the joint entropy of two multicomponent im-
R G B population ages ~X and ~Y , with dimensions DX and DY respec-
0 0 4 13 tively. The joint histogram of (~X,~Y ) can be calculated
0 0 7 18 as a compact histogram with dimension DX + DY ,
0 0 23 7 which after normalization yields the joint probabili-
.. .. .. .. ties p(~X,~Y ) leading to the joint entropy
. . . .
255 251 250 21 H(~X,~Y ) = − ∑ ∑ p(~X,~Y ) log p(~X,~Y ) . (2)
255 251 254 9 ~X ~Y
A mutual information between two multicomponent

multidimensional histogram. images follows as
For example, for a 9-component 838 × 762 satel-
lite image with Q = 28 , the compact histogram I(~X,~Y ) = H(~X) + H(~Y ) − H(~X,~Y ) . (3)
was calculated in about 5 s on a standard 1 GHz-
And again, the structure of the compact histogram
clock desktop computer, with a coding volume of
preserves the overall complexity of O(N log N) for the
1.89 Moctets, while the classic histogram would take
3.60 × 1016 Moctets completely unmanageable by to- whole process leading to H(~X,~Y ) or I(~X,~Y ).
day’s computers. So far in image processing, joint entropies and
mutual information have essentially been used for
scalar or one-component images (Likar and Pernus,
2001; Pluim et al., 2003), because the direct handling
3 ENTROPIES AND MUTUAL of joint histograms is feasible only in low dimension,
INFORMATION FOR IMAGES due to their memory occupation and associated pro-
cessing time which get prohibitively large as dimen-
3.1 Fast Computation from Compact sion increases. By contrast, the approach of the com-
pact histogram of Section 2 makes it quite tractable to
Histogram handle histograms with dimensions of 10 or more. By
this approach, many applications of entropies and mu-
The multidimensional histogram of an image, after tual information become readily accessible to color
normalization by the number of pixels, can be used and multispectral images. We sketch a few of them in
for an empirical definition of the probabilities p(~X) the sequel.
associated to the D-values present in image ~X. In the
compact histogram, coded as in Table 1, only those D- 3.2 Applications of Entropies
values of ~X with nonzero probability are represented.
This is all that is needed to compute any property of
the image that is defined as a statistical average over The entropy H(~X) can be used as a measure of com-
the probabilities p(~X). This will be the case for the plexity of the multicomponent image ~X, with applica-
statistical moments of the distribution of D-values in tion for instance to the following purposes:
the image (Romantan et al., 2002), for the principal • An index for characterization / classification of tex-
axes based on the cross-covariances of the compo- tures, for instance for image segmentation or classifi-
nents Xi (Plataniotis and Venetsanopoulos, 2000), and cation purposes.
for the entropies, joint entropies and mutual informa- • Relation to performance in image compression, es-
tion that we consider in the sequel. pecially lossless compression.
An entropy H(~X) for image ~X can be defined as For illustration, we use the entropy of Eq. (1) as
(Russ, 1995) a scalar parameter to characterize RGB color images
H(~X) = − ∑ p(~X) log p(~X) .
carrying textures as shown in Fig. 1. The entropies
(1)
~X
H(~X) given in Table 2 were calculated from 512×512
three-component RGB images ~X with Q = 256. The
The computation of H(~X) of Eq. (1) from the nor- whole process of computing a 3-dimensional his-
malized compact histogram from Table 1, is realized togram and the entropy took typically less than one
196
FAST COMPUTATION OF ENTROPIES AND MUTUAL INFORMATION FOR MULTISPECTRAL IMAGES
second on our standard desktop computer. For com- Table 2: For the nine distinct texture images of Fig. 1: en-
parison, another scalar parameter σ(~X) is also given tropy H(~X) of Eq. (1) in bit/pixel, and overall average dis-
in Table 2, as the square root of the trace of the persion σ(~X) of the components.
variance-covariance matrix of the components of im-
age ~X. This parameter σ(~X) measures the overall texture H(~X) σ(~X)
average dispersion of the values of multicomponent Chessboard 1.000 180.313
image ~X. For a one-component image ~X, this σ(~X) Wallpaper 7.370 87.765
would simply be the standard deviation of the gray Clouds 11.243 53.347
levels. The results of Table 2 show a specific signif- Wood 12.515 31.656
icance for the entropy H(~X) of Eq. (1), which does Marble 12.964 44.438
not simply mimic the evolution of a common mea- Bricks 14.208 57.100
sure like the dispersion σ(~X). As a complexity mea- Plaid 14.654 92.284
sure, H(~X) of Eq. (1) is low for synthetic images as Denim 15.620 88.076
Chessboard and Wallpaper, and is higher for natu- Leaves 17.307 74.966
ral images in Table 2.
• Homogeneity / contrast assessment for segmenta-

tion or classification purposes.
• Performance evaluation of image reconstruction, es-
pecially in lossy compression.
• Analysis via principal, or independent, component
analysis.
Chessboard Wallpaper Clouds For illustration, we consider a lossy compression
on an RGB color N1 × N2 image ~X via a JPEG-like
operation consisting, on the N1 × N2 discrete cosine
transform of ~X, in setting to zero a given fraction (1 −
CR−1 ) of the high frequency coefficients,
√ or
√ equiva-
lently in retaining only the N1 / CR × N2 / CR low-
frequency coefficients. From this lossy compression
Wood Marble Bricks
of initial image ~X, the decompression reconstructs a
degraded image ~Y . While varying the compression
ratio CR, the similarity between images ~X and ~Y is
measured here by the mutual information I(~X,~Y ) of
Eq. (3) based on the 6-dimensional joint histogram
for estimating the joint probabilities p(~X,~Y ). In ad-
dition, for comparison, we also used a more common
Plaid Denim Leaves
measure of similarity formed by the cross-correlation
Figure 1: Nine three-component RGB images ~X with Q = coefficient C(~X,~Y ) between ~X and ~Y , computed as
256 carrying distinct textures. one third of the sum of the cross-correlation coeffi-
cient between each marginal scalar component, R, G
or B, of ~X and ~Y . This C(~X,~Y ) = 1 if ~X and ~Y are
two identical images, and it goes to zero if ~X and ~Y
3.3 Applications of Mutual Information are two independent unrelated images. For the re-
sults presented in Fig. 2, the choice for initial im-
The mutual information I(~X,~Y ), or other measures age ~X is a 512 × 512 RGB lena.bmp with Q = 256.
derived from the joint entropy H(~X,~Y ), can be used The whole process of the computation of an instance
as an index of similarity or of relationship, between of the 6-dimensional joint histogram and the mutual
two multicomponent images ~X and ~Y , with applica- information took around 3 s on our standard desktop
tion for instance to the following purposes: computer.
• Image matching, alignment or registration, espe- Figure 2 shows that, as the compression ratio
cially in multimodality imaging. CR increases, the mutual information I(~X,~Y ) and the
• Reference matching, pattern matching, for pattern cross-correlation coefficient C(~X,~Y ) do not decrease
recognition. in the same way. Compared to the mutual informa-
• Image indexing from databases. tion I(~X,~Y ), it is known that the cross-correlation
197
1 ponents, does not cancel dependence between them,

and sometimes it may even increase it in some sense,
as illustrated by the behavior of the mutual informa-
0.95 tion in Table 3, with I(P1 , P2 ) larger than I(X1 , X2 )
for image (2). The mutual information can serve as a
measure to base other separation or selection schemes
0.9 of the components from an initial multispectral im-
age ~X.
Table 3: For a 512 × 512 RGB color image ~X with D = 3
0.85
and Q = 256: cross-correlation coefficient C(·, ·) and mu-
tual information I(·, ·) of Eq. (3), between the two initial
components X1 and X2 with largest variance, and between
0.8 0 1 2 3 the two first principal components P1 and P2 after principal
10 10 10 10
compression ratio component analysis of ~X. (1) image ~X is lena.bmp. (2)
image ~X is mandrill.bmp.
Figure 2: As a function of the compression ratio CR: (◦)
cross-correlation coefficient C(~X,~Y ) between initial RGB C(X1 , X2 ) I(X1 , X2 ) C(P1 , P2 ) I(P1 , P2 )
color image ~X and its compressed version ~Y ; (∗) normalized (1) 0.879 1.698 0.000 0.806
mutual information I(~X,~Y )/H(~X) with the entropy H(~X) = (2) 0.124 0.621 0.000 0.628
16.842 bits/pixel.
C(~X,~Y ) measures only part of the dependence be- 4 CONCLUSION

tween ~X and ~Y . Figure 2 indicates that when the com-
pression ratio CR starts to rise above unity, I(~X,~Y ) We have reported the fast computation and compact
decreases faster than C(~X,~Y ), meaning that informa- coding of multidimensional histograms and showed
tion is first lost at a faster rate than what is captured that this approach authorizes the estimation of en-
by the cross-correlation. Meanwhile, for large CR in tropies and mutual information for color and multi-
Fig. 2, I(~X,~Y ) comes to decrease slower than C(~X,~Y ). spectral images. Histogram-based estimators of these
This illustrates a specific contribution of the mutual quantities as used here, become directly accessible
information computed for multicomponent images. with no need of any prior assumption on the images.
The performance of such estimators clearly depends
Another application of mutual information be-
on the dimension D and size N1 × N2 of the images;
tween images can be found to assess a principal com-
we did not go here into performance analysis, espe-
ponent analysis. On a D-component image ~X =
cially because this would require to specify statistical
(X1 , . . . XD ), principal component analysis applies a
models of reference for the measured images. Instead
linear transformation of the Xi ’s to compute D prin-
here, more pragmatically, on real multicomponent im-
cipal components (P1 , . . . PD ) with vanishing cross-
ages, we showed that, for entropies and mutual in-
correlation among the Pi ’s, in such a way that some
formation, direct histogram-based estimation is feasi-
Pi ’s can be selected for a condensed parsimonious
ble and exhibits natural properties expected for such
representation of initial image ~X. An interesting
quantities (complexity measure, similarity index, . . . ).
quantification is to consider the mutual information
The present approach opens up the way for further ap-
I(~X, Pi ). From its theoretical properties, the joint en-
plication of information-theoretic quantities to multi-
tropy H(~X, Pi ) reduces to H(~X) because Pi is deter- spectral images.
ministically deduced from ~X, henceforth I(~X, Pi ) =
H(Pi ). This relationship has been checked (on sev-
eral 512 × 512 RGB color images with D = 3) to be
precisely verified by our empirical entropy estima- REFERENCES
tors for I(~X, Pi ) based on the computation of (D + 1)-
Clément, A. (2002). Algorithmes et outils informatiques
dimensional histograms for (~X, Pi ). This offers a
pour l’analyse d’images couleur. Application à l’étude
quantification of the relation between ~X and its prin- de coupes histologiques de baies de raisin en micro-
cipal components Pi ; a subset of the whole Pi ’s could scopie optique. Ph. D. thesis, University of Angers,
be handled in a similar way. Another useful quantifi- France.
cation shows that principal component analysis, al- Clément, A. and Vigouroux, B. (2001). Un histogramme
though it cancels cross-correlation between the com- compact pour l’analyse d’images multi-composantes.
198
FAST COMPUTATION OF ENTROPIES AND MUTUAL INFORMATION FOR MULTISPECTRAL IMAGES
In Proceedings 18è Colloque GRETSI sur le Traite-

ment du Signal et des Images, pages 305–307,
Toulouse, France, 10–13 Sept. 2001.
Likar, B. and Pernus, F. (2001). A hierarchical approach to
elastic registration based on mutual information. Im-
age and Vision Computing, 19:33–44.
Plataniotis, K. N. and Venetsanopoulos, A. N. (2000). Color
Image Processing and Applications. Springer, Berlin.
Pluim, J. P. W., Maintz, J. B. A., and Viergever, M. A.
(2003). Mutual-information-based registration of
medical images: A survey. IEEE Transactions on
Medical Imaging, 22:986–1004.
Romantan, M., Vigouroux, B., Orza, B., and Vlaicu, A.
(2002). Image indexing using the general theory of
moments. In Proceedings 3rd COST 276 Workshop
on Information and Knowledge Management for Inte-
grated Media Communications, pages 108–113, Bu-
dapest, Hungary, 11–12 Oct. 2002.
Russ, J. C. (1995). The Image Processing Handbook. CRC
Press, Boca Raton.
199
PROBABILISTIC MAP BUILDING CONSIDERING SENSOR
VISIBILITY
Kazuma Haraguchi, Jun Miura

Department of Computer-Controlled Mechanical Systems, Graduate School of Engineering, Osaka University
2-1 Yamadaoka, Suita, Osaka, Japan
Nobutaka Shimada, Yoshiaki Shirai

Department of Computer Science Faculty of Science Engineering Ritsumeikan University
1-1-1 Nojihigashi, kusatsu, shiga, Japan
Keywords: Obstacle Map Generation, Bayes Theorem, Occlusion, Spatial Dependency, Visibility.
Abstract: This paper describes a method of probabilistic obstacle map building based on Bayesian estimation. Most
active or passive obstacle sensors observe only the most frontal objects and any objects behind them are oc-
cluded. Since the observation of distant places includes large depth errors, conventional methods which does
not consider the sensor occlusion often generate erroneous maps. We introduce a probabilistic observation
model which determines the visible objects. We first estimate probabilistic visibility from the current view-
point by a Markov chain model based on the knowledge of the average sizes of obstacles and free areas.
Then the likelihood of the observations based on the probabilistic visibility are estimated and then the poste-
rior probability of each map grid are updated by Bayesian update rule. Experimental results show that more
precise map building can be bult by this method.
1 INTRODUCTION sors, ultrasonic sensors and stereo image sensors can

precisely control the direction of transmitting and re-
The well known SLAM(Simultaneous Localization ceiving light or sound, the place from which observa-
And Mapping)(Montemerlo and Thrun, 2002; Thrun tion comes is easy to identify. Thus we use the latter
et al., 2004; Grisetti et al., 2005) process could be map representation here.
divided in two parts: position localization using cur- The obstacle sensor output always includes obser-
rent observations and the current map, and the map vation errors. For example flight time measurement
update using the estimated position. This paper dis- of light or sound have errors caused by wave refrac-
cusses the problem of the conventional map update tion, diffraction and multi-path reflection. Stereo im-
methods, sensor occlusion, and proposes a novel up- age sensor also has correspondence failures of image
dating method solving it. features. These errors lead to large errors or ghost
The positions of landmarks and obstacles should observations. Therefore the map building should be
be represented on the map for navigation(Thrun, established in a probabilistic way based on a certain
2002b). There are two representations available for error distribution model.
them: Here we use a probabilistic occupancy grid map
1. Feature point on the obstacle for the map representation, which stores the obstacle
existence probability in each map grid. In this repre-
2. Obstacle existence on small map grid. sentation, the obstacle existence probability of each
Former representation requires feature identification grid is updated by evaluating the likelihood of ob-
and matching and the feature position is update when tained observation for the grid and integrate it with its
new observation is available(Suppes et al., 2001). prior probability in the way of Bayesian estimation.
Latter representation identifies the map grid from While a generic solution of this map building for-
which the observation comes. Since laser range sen- mulation is solved by Markov Random Field (MRF)
200
continuity. Since the co-occurrence becomes larger

observation when the grid resolution is more precise, the free-
space probability can be correctly estimated regard-
less of the grid resolution. Thus the assumption 2 also
should be rejected.
distribution of obstacle
Our map building method correctly considers sen-
Figure 1: Result of update when the robot gets the observa- sor occlusion and spatial continuity. A certain ob-
tion of the obstacle of substantial margin of error.
stacle is visible if and only if the space between the
sensor and the obstacle is entirely open as free-space.
Therefore we introduce a novel method of estimating
(Rachlin et al., 2005), its computational cost is huge, visibility of the obstacle on each map grid by consid-
especially in case of high grid resolution. Previous ering spatial continuity, and updating the obstacle ex-
methods (Miura et al., 2002; Thrun, 2002a; Moravec, istence probability with proper consideration of sen-
1988) use the following assumptions for reduction of sor occlusion.
computation:
1. Observation obtained for each map grid is prob-
abilistically independent (it depends on only the 2 MAP BUILDING
state (obstacle existence) of that grid). CONSIDERING SENSOR
2. Obstacle existence of each map grid is indepen- VISIBILITY
dent to each other.
Most obstacle sensors observe the most frontal ob- 2.1 Joint Probability of Adjacent Grids
jects and any objects behind them are occluded. Since on Each Viewing Ray
such sensors having sensor occlusion characteristic
does not satisfy the above assumption 1, the follow- We first divide the map grids into multiple viewing
ing serious problems are caused by forcedly using the rays. The sensor observes the existence of obstacle
assumption. on each ray. In this paper, we represent the viewing
As shown in the left side picture of Figure 1, sup- ray as the 4-connected digital line as shown in Fig-
pose that dark grey ellipsoidal region has higher prob- ure 2. On each ray we consider the probabilistic grid
ability of obstacle existence than the outside. Then state (whether obstacle exists on the grid or not) as
suppose that the obstacle observation with large error a simple Markov chain. Thus each grid state can be
(ex. distant observation with stereo vision) is obtained represented as the joint probability of two grid states
shown as the light grey region in the figure. In that adjacent on each ray.
situation, that observation probably comes from the
most frontal part of the dark grey region. The assump-
tion 1, however, the obstacle existence probability of
each map grid is independently updated by integrat-
ing the observation and it is obviously overestimated.
As a result everywhere distant from the current robot
position tends to be estimated as obstacle in the map.
Therefore the assumption 1 should be rejected.
In addition, the assumption 2 also leads to the
other problems. If the obstacle existence in each map
grid is independent, the probability of that a certain Figure 2: Approximated Lines of view.
area is open as free-space is estimated as the product
of the probability of each map grid. This leads to an
obviously irrational result that more precise grid reso- We first estimate the joint probability of the jth
lution is adopted, abruptly smaller becomes the free- and j + 1th grid (the jth grid is nearer tonthe sensor)o
space probability of the same area (in other words, on a viewing ray (see Figure 2). Let elj = E lj , Ē lj be
the viewing field is more invisible due to occluding the state of the jth grid on the lth ray (E:occupied by
obstacles). an obstacle, Ē: not occupied), and P(elj , elj+1 |O) be
In real scenes, obstacles and free-spaces has a the joint probability of the jth and j + 1th grid under
certain size. This points out the existence of co- O, the series of the previous observations. Then the
occurrence between the adjacent grids, called spatial joint probability after the latest observation o obtained
201
Since F0 , · · · , Fk are mutually exclusive events, the

right-hand of Eq.3 is expanded as follows:
j
Figure 3: Occluded area behind the j-th grid. P(ol |E lj , O)= ∑ P(ol |Fkl , E lj , O)P(Fkl |E lj , O)
k=0
j
= ∑ P(ol |Fkl )P(Fkl |E lj , O) (4)
k=0
because ol is no longer dependent on any grid behind

Figure 4: Occluded area behind the k-th grid. grid k nor the previous observations O.
The likelihoods for the rest three cases (E j , Ē j+1 ),
(Ē j , E j+1 ), (Ē j , Ē j+1 ), is represented as follows in the
is calculated as : same considerations:
P(elj , elj+1 |o, O) = αl P(ol |elj , elj+1 , O)P(elj , elj+1 |O). (1) j
P(ol |E lj , Ē lj+1 , O) = ∑k=0 P(ol |Fkl )P(Fkl |E lj , O) (5)
ol , αl , P(elj , elj+1 |O) and P(ol |elj , elj+1 , O) respectively j−1
P(ol |Ē lj , E lj+1 , O) = ∑k=0 P(ol |Fkl )P(Fkl |Ē lj , O)
denote the observation on the ray l, a normalizing
constant, the prior probability and the likelihood. The +P(ol |Fj+1
l )P(F l |O)
j+1 (6)
likelihood P(ol |elj , elj+1 , O) should be calculated by j−1
P(o l
|Ē lj , Ē lj+1 , O) =∑k=0 P(ol |Fkl )P(Fkl |Ē lj , O)
considering the sensor visibility as the following sec-
tion. + ∑∞ l l l
k= j+1 P(o |Fk )P(Fk |O).(7)
2.2 Likelihood Considering Sensor P(ol |Fkl ) in Eqs.(4)-(7) is a sensor kernel model which
defines the measurement error distribution of the sen-
Visibility sor observation. This is provided for each kind of sen-
sor. These equations say that the likelihood of grid j
Since an observation on the ray l, ol , depends and j + 1 is contributed by grid k ahead grid j accord-
on grid states just on the the ray, the likelihood ing to P(Fkl |E lj , O), P(Fkl |Ē lj , O), P(Fkl |O), which mean
P(ol |elj , elj+1 , O) is calculated as: conditional visibilities of grid k.
Since these visibilities are required to be estimated
P(ol |elj , elj+1, O) = ∑ P(ol |ml, elj , elj+1, O)P(ml |elj , elj+1, O)
before the above likelihood calculations, they should
ml ∈Ωl
(2) be estimated by calculating the Markov chain because
where ml denotes grid states on the ray l except the actual grid states are not independent due to spa-
elj ,elj+1 . The direct calculation of Eq. 2 requires huge tial continuity. The Markov chain is calculated based
summation of 2Nl order, where Nl denotes the number on the grid state probabilities (joint probability of e j ,
of grids on the ray l. and e j+1 ) obtained in the previous time slice:
Actually this calculation is drastically reduced P(Fkl |E lj , O) =P(Ē0l |Ē1l , O)P(Ē1l |Ē2l , O)P(Ē2l |Ē3l , O)
by considering the sensor visibility. There exist
l |E l , O)P(E l |E l , O)
· · · P(Ēk−1 (8)
four cases of the adjacent grid states: (E j , E j+1 ), k k j
(E j , Ē j+1 ), (Ē j , E j+1 ), (Ē j , Ē j+1 ). In (E j , E j+1 ) case P(Fkl |Ē lj , O) =P(Ē0l |Ē1l , O)P(Ē1l |Ē2l , O)P(Ē2l |Ē3l , O)
(this means both the grid j and j + 1 are occupied),
l |E l , O)P(E l |Ē l , O)
· · · P(Ēk−1 (9)
since grid j occludes j + 1 as shown in Figure 3, the k k j
likelihood P(ol |E lj , E lj+1 , O) is no longer dependent P(Fkl |O) =P(Ē0l |Ē1l , O)P(Ē1l |Ē2l , O)P(Ē2l |Ē3l , O)
on j + 1, and then it is represented as l |E l , O)P(E l |O)
· · · P(Ēk−1 k k (10)
P(ol |E lj , E lj+1 , O)= P(ol |E lj , O). (3)
where P(Ekl |O), P(Ēk−1
l |E l , O) and P(Ē l |Ē l , O)
k q q+1
The above likelihood is acceptable only when grid j is (0 ≤ q < k − 1) are calculated as follows:
visible from the sensor, namely whole grids between
the sensor and grid j are empty. If not so, the most P(Ekl |O)= P(Ekl , Ek+1
l |O) + P(E l , Ē l |O)(11)
k k+1
frontal occupied grid k(< j) is observed (see Figure l ,E l |O)
P(Ēk−1
l
4). Here, define an stochastic event Fk as follows: P(Ēk−1 |Ekl , O) = l l
P(Ek−1 ,Ek |O)+P(Ēk−1
k
l ,E l |O) (12)
k

E (k = 0) l l
P(Ēq ,Ēq+1 |O)
Fk = 0 P(Ēql |Ēq+1
l
, O) = . (13)
Ē0 ∩ Ē1 ∩ · · · ∩ Ēk−1 ∩ Ek (k > 0). l |O)+P(E l ,Ē l |O)
P(Ēql ,Ēq+1 q q+1
202
each neighbor(m-th) cell
Figure 5: Adjacent grids.

pixel
True disparity
Figure 6: Probability density of the observation of the dis-
P(Ekl |E lj , O) and P(Ekl |Ē lj , O) are also obtained by cal- parity.
culating the Markov chain:
j−1
P(Ekl , E lj |O)= P(Ekl |O)P(E lj |O) + αk, j ∏q=k clq,q+1 (14)
3 EXPERIMENTS
j−1
P(Ekl , Ē lj |O)= P(Ekl |O)P(Ē lj |O) − αk, j ∏q=k clq,q+1 (15)
where 3.1 Sensor Kernel Model of Stereo
P(Eql ,Eq+1
l |O)−P(E l |O)P(E l |O)
q
Vision
clq,q+1 = q q+1
l |O)P(Ē l |O)
(16)
P(Eq |O)P(Ēql |O)P(Eq+1
l
q+1
q We compared a map building result for the simula-
αk, j = P(Ekl |O)P(Ēkl |O)P(E lj |O)P(Ē lj |O).(17) tion on a viewing ray and a real indoor scene using
our method and a conventional method considering
clq,q+1 means correlation between the adjacent grid no sensor visibility nor spatial continuity.
In these experiments, we used edge-based stereo
elq and elq+1 . The grid visibilities are estimated from vision for observation of obstacles. Stereo vision pro-
Eqs.(8)-(17), then the likelihoods considering sensor vides depth information for each edge feature and it
visibility, Eqs.(4)- (7), are calculated. Finally the pos- is well-known that the measurement error is inversely
teriors, Eq.(1) is updated for each grid. proportional to square of the depth. In addition er-
roneous feature matching can be found in a certain
2.3 Posterior Update Across Viewing probability. Thus its sensor kernel model P(ol |Fkl ),
required in Eqs.(4)- (7), are defined here as follows:
Rays
P(ol |Fkl ) = P(T )P(ol |Fkl , T ) + P(T̄ )P(ol |Fkl , T̄ ) (21)
The grid posteriors for each viewing ray, estimated where ol is the measured disparity for the viewing
in the way of the previous section, conflict with the ray l, P(T ) (fixed to 0.8 in the following experiment,
posteriors of the adjacent rays, because they actually P(T )+P(T̄ ≡ 1)) is the probability of obtaining a cor-
have interactions across the rays and the independent rect matching, P(ol |Fkl , T ) is a gaussian (see Figure
update of each ray is just an approximation. True pos- 6), P(ol |Fkl , T̄ ) is a uniform distribution over dispar-
teriors should satisfy the following two constraints: ity range [0,60]. The obtained sensor kernel model
the first one is P(ol |Fkl ) is shown in Figure 7.
∑ P(ei , e j ) ≡ 1 (18) 1
ei ,e j ∈{E,Ē}
where grid i and j is adjacent, and the second one is

P(ei ) ≡ P(ei , Em ) + P(ei , Ēm ) (19) 0.5 25
10
for every grid m adjacent to grid j (see Figure 5).
3
Therefore we estimate the true posteriors by least 1
squares method under the constraints Eqs.(18),(19).
The minimised error ∆2 is written as 0
0 50 100 150 200 250
2 distance[grid]
∆2 = ∑i, j ∑ei ,e j P∗ (ei , e j ) − P(ei , e j ) (20)
Figure 7: Observation model.
where P∗ (ei , e j ) is the conflicting posteriors, P(ei , e j )
is the estimated posteriors satisfying the constraints.
This minimization is easily solved by Lagrange’s Our method updates the posteriors based on the
method in low cost. likelihoods considering sensor visibility, Eqs.(4)-(7).
203
10
In the conventional method, we assume that state 1
likelihood ratio
of each grid is independent from state of other grid
probability
as: 1 0.5
P(oli |eli , O)P(eli |O)

P(eli |o, O) = 0.10
P(oi |Ei , O)P(Eil |O) + P(oli |Ēil , O)P(Ēil |O)
l l 50 100 00 50 100
distance[grid] distance[grid]
(22)
(a) Likelihood (b) Result of posterior update
where oli denotes the observation on ith grid and on
Figure 8: Result of posterior update when the robot get
lth ray. On this assumption we can calculate each the observation of the obstacle in the uniformly-distributed
grid indipendently. The following conventional like- probablity area. ”+” represents results of the conventional
lihoods (Miura et al., 2002; Thrun, 2002a; Moravec, method. ” ” represents results of our method. The gray
1988) are adopted as a benchmark: curve in (b) represents the prior probablity.
P(olk = oE |Ekl ) = P(ol |Fkl ) (23)

P(olk = oE |Ēkl ) = P(T̄ ) (24)
the observations. Figure 9 shows the likelihood and
P(olk = oĒ |Ekl ) = P(T̄ ) (25) the posteriors when we get the observation ( equiva-
P(olk = oĒ |Ēkl ) = P(T ){1 − P(ol |Fkl , T )}. (26) lent to the disparity of 3 pixels) with large error. With-
out considering visibility, posteriors in the range from
3.2 Posterior Update on a Viewing Ray the 80th to the 120th grid ( equivalent to the dispar-
ity uncertainty of 3 pixels ) becomes high. But we
Our method of posterior update, of course, requires can not obtain the information in the range from the
an initial prior distribution. In addition it requires 90th to the 120th grid because we obtain the observa-
an initial correlation parameter using for estimating tion with the error which is larger than distribution of
the spatial continuity. In the following experiment, obstacles. For this reason with considering visibility,
the initial prior P(Ei ) is uniformly set to 0.1 and the posteriors in the range from the 90th to the 120th grid
spatial correlation cq,q+1 to 0.871 where the grid size does not change as Figure 9(b). On the other hand,
is 5cm × 5cm These initial parameters are estimated in front of the range we can obtain the information
based on average obstacle size ( 40cm × 40cm ) in that the obstacles does not exist. For this reason both
actual room scene samples. methods show that posterior probability of obstacles
Since the aim of this paper is to show the effec- existence becomes low in front of the range.
tiveness of our posterior updating considering sensor
10 1
visibility, we suppose that the exact robot position and
likelihood ratio
probability
orientation is known (NOT SLAM problem).

1 0.5
We compare our method with the convensional
one without considering sensor visibility. We use
three situations for the map update. 0.10 100 00
50 150 50 100 150
3.2.1 Case of Prior Probability of Uniform (a) Likelihood (b) Result of posterior update
Distribution
Figure 9: Result of posterior update when the robot get
the observation of the obstacle in the uniformly-distributed
Figure 8 shows the likelihood ratio and the posteriors probablity area. ”+” represents results of the conventional
when we take the observation ( equivalent to the dis- method. ” ” represents results of our method. The gray
parity of 10 pixels ) in the uniform disribution. With curve in (b) represents the prior probablity.
considering sensor visibility, posteriors in the grid just
behind the 30th grid is higher than that without con-
sidering visibility because we consider the average
size of obstacles( equivalent to 40cm ) in this experi- 3.2.3 Case of Failure Observation for Occluded
ment (see section 3.1). Area
3.2.2 Case of Uncertain Observation from In Figure 10 we establish that we obtain the failure
Distant Place observation for an occluded area when existence of
obstacle is obvious. The error of this observation is
In Figure 9 we establish that posteriors are estimated about from the 200th to the 560th grid ( equivalent to
high from the 95th to the 110th grids after we obtain the disparity of 1 pixel ). But it is highly possible that
204
the observation is failure because obstacle existence

about the 100th grid is already know. With consid-
ering sensor visibility, the failure observation is auto-
matically detected and the posteriors kept unchanged.
10 1
likelihood ratio
probability
(a) From A in Figure 11 (b) From D in Figure 11
1 0.5
Figure 12: Robot’s view.
0.10 0 200 300
100 200 300 0 100
(a) Likelihood (b) Result of posterior update
13(a)). When the robot re-observed X region from far-
Figure 10: Result of posterior update when the robot get ther point, however, the detail information of X was
the observation of the obstacle in the uniformly-distributed missing and estimated as ’obstacle’ rather than free-
probablity area. ”+” represents results of the conventional space due to the erroneous stereo observation from far
method. ” ” represents results of our method. The gray
curve in (b) represents the prior probablity.
point. (Figure 13(b),(c)). In contrast, our method cor-
rectly estimated the free-space in X without compro-
mise due to the erroneous observation from far point
(Figure 14(a),(b),(c)).
3.3 Result of Map Building The process time for one update was about
1800ms on a PC with Athlon X2 4400+ CPU and
Next we show the result of map building for an ac- 2GB memories.
tual room as shown in Figure 11. The mobile robot
moved in the room observing obstacles with stereo
cameras, starting from A point in Figure 11 via B, C, 4 CONCLUSIONS
B, D through the grey line in the figure, and finally
arrived at B point. We introduced a probabilistic observation model
Figure 12 shows a view captured by the left cam- properly considering the sensor visibility. Based on
era. Figure 12(a) is a view from A point, and Fig- estimating probabilistic visibility of each grid from
ure 12(b) is another view from B point. We com- the current viewpoint, likelihoods considering sensor
pared the built map of our method (Figure 14) to that visibility are estimated and then the posteriors of each
of the conventional one (Figure 13). Sub-figure (a) map grid are updated by Bayesian update rule. For
and (b) of each figure shows the temporary map when estimating grid visibility, Markov chain calculation
the robot reached to B point via A, B, C and D point based on spatial continuity is employed based on the
via A, B, C, B respectively, and sub-figure (c) shows knowledge of the average sizes of obstacles and free
the final map after the robot arrived at B point. Our areas.
method and the conventional method show the sig- This paper concentrates the aim on showing the
nificantly difference at the circular region labelled X. proof of concept of the probabilistic update consid-
The conventional method once estimate X as ’free- ering sensor visibility and spatial continuity. For ap-
space’ clearly when the robot was close to X (Figure plication to the real robot navigation, SLAM frame-
work is necessary. In addition there are moving ob-
jects like human and semi-static objects like chairs
A or doors in the real indoor scene. The expansion to
SLAM and environments with movable objects is the
future works.
C
D
B
Figure 11: Rough map of obstacles.
205
Rachlin, Y., Dolan, J., and Khosla, P. (2005). Efficient map-

ping through exploitation of spatial dependencies. In
Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots
and Systems (IROS), pages 3117–3122.
Suppes, A., Suhling, F., and Hötter, M. (2001). Robust Ob-
stacle Detection from Stereoscopic Image Sequences
(a) At B via A, B, C (b) At D via A, B, C, B Using Kalman Filtering. In DAGM-Symposium, pages
385–391.
Thrun, S. (2002a). Learning occupancy grids with forward
sensor models. In Autonomous Robots.
Thrun, S. (2002b). Robotic Mapping: A Survey. In Lake-
meyer, G. and Nebel, B., editors, Exploring Artifi-
cial Intelligence in the New Millenium. Morgan Kauf-
mann.
(c) At B via A, B, C, B,
D Thrun, S., Montemerlo, M., Koller, D., Wegbreit, B., Nieto,
J., and Nebot, E. (2004). FastSLAM: An Efficient So-
Figure 13: Result of map update without considering visibility
lution to the Simultaneous Localization And Mapping
and spatial dependencies. Problem with Unknown Data Association. Journal of
Machine Learning Research.
(a) At B via A, B, C (b) At D via A, B, C, B
(c) At B via A, B, C, B,
D
Figure 14: Result of map update considering visibility and spa-
tial dependencies.
REFERENCES
Grisetti, G., Stachniss, C., and Burgard, W. (2005). Improv-
ing Grid-based SLAM with Rao-Blackwellized Parti-
cle Filters by Adaptive Proposals and Selective Re-
sampling. In Proc. of the IEEE International Con-
ference on Robotics and Automation (ICRA), pages
2443–2448.
Miura, J., Negishi, Y., and Shirai, Y. (2002). Mobile
Robot Map Generation by Integrationg Omnidirec-
tional Stereo and Laser Range Finder. In Proc. of the
IEEE/RSJ Int. Conf. on Intelligent Robots and Systems
(IROS), pages 250–255.
Montemerlo, M. and Thrun, S. (2002). FastSLAM 2.0:
An Improved Particle Filtering Algorithm for Simul-
taneous Localization and Mapping that Provably Con-
verges.
Moravec, H. P. (1988). Sensor fusion in certainty grids for
mobile robots. In AI Magazine, Summer, pages 61–74.
206
J. C. Miao (1,2), P. Zhu (1), G. L. Shi (1) and G. L. Chen (1)

School of Mechanical Engineering, Shanghai JiaoTong University, Shanghai, China(1)
Department of Mechanical Engineering, Shazhou institute of technology, Zhangjiagang, China(2)
Keywords: Flexible multi-body dynamics, Condensed FMD equation, Sub-cycling algorithm, Energy balance.
Abstract: This paper presents a Newmark method based sub-cycling algorithm, which is suitable for solving the
condensed flexible multi-body dynamic (FMD) problems. Common-step update formulations and sub-step
update formulations for quickly changing variables and slowly changing variables of the FMD are
established. Stability of the procedure is checked by checking energy balance status. Examples indicate that
the sub-cycling is able to enhance the computational efficiency without dropping results accuracy greatly.
1 INTRODUCTION
Flexible multi-body system (FMS) can be applied in
various domains such as space flight, automobiles
and robots. In these domains, accurate and efficient
computation of the flexible bodies undergoing large
overall motion is important for design and control of
the system (Huang and Shao, 1996).
Conventional integration methods, such as the
Newmark algorithm, the Runge-Kutta algorithm and
the Gear algorithm and etc (Dan Negrut, et al, 2005), Figure 1: A spatial arbitrary flexible body.
were widely applied to solve FMD equations.
Sub-cycling was proposed by Belytschko T. et al The FMD equation can be established by means
(Belytschko T.,1978). Mark et al (Mark, 1989) of the Lagrange formation.
applied the method to simulate an impact problem
and computational cost of the sub-cycling was only
+ Kq + C Tq λ = Q F + Q v " ( 2 − 1)
Mq
15% of that without sub-cycling. Gao H et al. (Gao,
2005) used sub-cycling to simulate auto-body C(q, t ) = 0 """""""" (2 − 2)
crashworthiness and declared that the cost is only
39.3% of no sub-cycling. Tamer et al (Tamer, 2003) Thereinto, M is a general mass matrix, K is a
pointed out that the FMD sub-cycling methods have general stiffness matrix, C(q,t) is the constrains, QF
not yet been presented in literatures. and QV are general external load and general
centrifugal load. Λ is the Lagrange multiplier. By
means of two times of differentials of the constrain
2 SUB-CYCLING FOR FMD equation, an augmented FMD equation can be
obtained as below.
A sub-cycling is constituted by two types of cycles,
main-cycle and sub-cycle. The key for sub-cycling is
to appropriately treat with interactions of nodes on ⎡M r C Tq ⎤ ⎡q
⎤ ⎡Q ∗F ⎤
⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ """ (2 − 3)
the interface (Daniel, 1997). ⎢⎣C q 0 ⎥⎦ ⎣λ ⎦ ⎣Q C ⎦
2.1 Condensed Model of FMD

Due to the constraints, variables in (2-3) are
A flexible body is displayed in figure 1. Definition independent. By decomposition, a condensed FMD
of variables can be referenced in the literature (Lu, equation can be established as following (Haug,
1996). 1989).
207
ˆ (q , q , t )q
M ˆ (q , q , q , q , t )" (2 − 4)
i = Q Thereinto, the top left corner marks represent the
i i d i i i d d
number of the iteration step. Start-up initialisation of
iteration can be set-up below.
Thereinto:
ˆ = M − M C −1 C − CT C −1
M i ii id q d qi qd qd ( ) [M − M C C ]
T
di dd
−1
qd qi
(1)
qt +Δt = qt + q t Δt +
Δt 2
2
[(1− 2β )q
t + 2β (1)q ]
t +Δt "(2 −10)
ˆ = Q −M C γ −C
Q i
*
Fi
id
−1
qd
T
qi (C ) [Q − M C γ ]
−1 T
qd
*
Fd dd
−1
qd
2.3 Newmark Sub-cycling for FMD

Equation（2-4） is a pure differential formation.
For simplification, all variables are separated into
It is suitable for sub-cycling (Lu, 1996).
two categories. The smaller step size is set to be Δ t.
2.2 Newmark Integration for FMD The larger step size is set to be nΔ t. n is a positive
The Newmark integration is as following. There, qt
is expressed as a decomposed
integer. Thus, q
format. And the condensed FMD formula can be
denotes value of the general variable at time t. Δ t is
decomposed as a block matrix format as following.
the step size. β andγ are adjustable parameters.
⎡Mˆt ˆ t ⎤ ⎡q
M ˆt ⎤
L ⎤ ⎡Q
LL LS L
⎢ t ⎥ ⎢ ⎥ = ⎢ t ⎥ "(2 − 12)
Δt 2 ˆ
⎢⎣M SL ˆ t ˆ
M SS ⎥⎦ ⎣q S ⎦ ⎢⎣Q S ⎥⎦
qt +Δt = qt + q t Δt + [(1− 2β )qt + 2βqt+Δt ]" (2 − 5)
2
q t +Δt = q t +Δt [(1− γ )q t+Δt ]""""""(2 − 6)
t + γ q The subscript symbol, L and S, represent the
larger step size and the smaller step size. M and Q
are the general mass matrix and the general external
Define symbols below. force. According to balance status of the FMS at t+
Δ t and t+n Δ t, two groups of equations can be
1 γ 1 1
a0 = a1 = a2 = a3 = −1 obtained as following.
βΔt 2 βΔt βΔt 2β
γ Δt ⎛ γ ⎞
a4 = −1 a5 = ⎜ − 2⎟⎟ a6 = Δt(1 − γ ) a7 = γΔt ˆ q ˆ ˆ
LLn Ln + M LSn q Sn = Q Ln " (2 − 13)
β 2 ⎜⎝ β M
⎠
ˆ q
M + Mˆ q = Q ˆ " (2 − 14)
SL1 L1 SS 1 S1 S1
The dynamic equation at time t+ Δ t can be

established below. Ln and q
Thereinto, q Sn are accelerations at
t + nΔt . qL1 and qS1 are accelerations at t + Δt .
i i ( d 0 i )[ (
ˆ qt+Δt , qt+Δt a qt+Δt − qt − a q t − a q
M i 2 i
t
3 i ) ] The general mass matrix and the general external
i (
ˆ (1) qt+Δt ,(1) q t+Δt ,(1) qt+Δt ,(1) q t+Δt "(2 − 7)
=Q i i d d ) force are defined similarly. Define symbols below.
Finally, we can get the results. a10 a12 1

a10 = a0 , a0n = 2
, a12 = a2 , a2n = , a3 = a3 , a3n = a31
n n
ˆ
qti+Δt = a0M [ i ]
t +Δt −1 ˆ t+Δt + qt + a2 q t + a3 q
Q i i
a0
i
a0
ti "(2 −8) In order to compute interaction between the
coupling variables at the common update, qSn and
qL1 need be estimated simultaneously. Similar to the
Equation (2-8) need be solved iteratively. The method (Daniel, 1997), qL1 can be linearly
iteration process is as following. interpolated and qSn can be linearly extrapolated by
means of the trapezoid rule. The formats of the
−1
qit +Δt = ⎡⎣a0 (k −1) M
(k ) ˆ t +Δt ⎤ ( k −1) ˆ t +Δt
Q interpolations are expressed below.
i ⎦ i
a a t n −1 1
+qti + 2 q ti + 3 q i " (2 − 9) q L1 = q L 0 + q Ln """" (2 − 15)
a0 a0 n n
q Sn = n ( q Sn − q S 0 ) + q S 0 """ (2 − 16)
208
Hence action of the slowly changing variables to In terms of equations (2-22) and (2-23), the
the rapidly changing variables is calculated below. action of the slowly changing variables to the
rapidly changing variables can be approximately
assessed.
ˆ ⎡a0 (q − q ) − a1q − a1q ⎤
1
− FSL1 = MSL1 ⎢ Ln L0 2 L0 3 L0 ⎥"(2 −17)

n 1ˆ
SL(i+1) [ ( a0 ( qLn − qL0 ) − a2 ( ( n − i ) qL0 + iqLn )
⎣ ⎦ −FSL(i+1) = M
n
The action of the rapidly changing variables to − a31 ( ( n −i) q L0 +iq Ln ) ⎤⎦"""" """(2 − 24)
the slowly changing variables is calculated below.
Imposing equation (2-24) into equation (2-21),
−FLSn = MLSn 0 S1 S0 [ (
ˆ nan q −q −anq −anq
2 S0 )
3 S0 "(2−18)
] the sub-step update format can be got as following.
SS (i +1)qS (i +1) = QS (i +1) + MSS (i +1) ( a0 qSi + a2qSi + a3 qSi )

ˆ
a0 M ˆ ˆ
Imposing equations (2-17) and (2-18) into
a0 ˆ a ˆ
SL(i +1) ( ( n − i ) qL0 + iqLn )
equations (2-13) and (2-14), and co-multiplying a − MSL(i+1) ( qLn − qL0 ) + 2 M
number, n, to the left hand and the right hand of n n
equation (2-13), the common update formula of the a1 ˆ
+ 3M SL(i +1) ( ( n − i ) qL0 + iqLn )""""" "(2 − 25)

slowly changing variables and the rapidly changing n
variables can be obtained below.
The energy balance status can be calculated by
⎡Mˆ ˆ ⎤⎡qS1 ⎤ ⎡Q
M ˆ ⎤ ⎡Mˆ ˆ ⎤⎡qS 0 ⎤
M
means of the equation below (Mark and Belytschko,
Sl1 ⎢
⎥ qLn ⎥ = ⎢
Sl1 ⎢
⎥ q ⎥ 1989).
SS1 S1 SS1
a0 ⎢ ⎥ + a0 ⎢
ˆ
⎢⎣M ˆ ⎢ ⎥ ˆ
MLLn ⎥⎦⎢ ⎥ ⎢⎣nQLn ⎥⎦ ˆ
⎢⎣M ˆ ⎥ ⎢ L0 ⎥
M LLn ⎦ ⎢
LSn
⎣ n ⎦ LSn
⎣ n ⎦⎥
⎡Mˆ
SS1
ˆ ⎤⎡q ⎤
M Sl1 S0
⎡M ˆ
SS1
ˆ
M Sl1
⎤⎡q
S 0 ⎤ Wnkin + Wnint − Wnext ≤ δ W "" (2 − 26)
+ a2 ⎢ ⎥⎢ ⎥ + a3 ⎢ ⎥⎢ ⎥"(2 −19)
ˆ ˆ
q ˆ ˆ
q
⎣⎢MLSn MLLn ⎥⎦⎣ L0 ⎦ ⎣⎢nMLSn nMLLn ⎦⎥⎣ L0 ⎦
Thereinto, Wnext is the work of the external forces
In order to get the sub-step update formula, the at nΔ t. Wnint is the internal energy at nΔ t. Wnkin is
following equation need be calculated at t+（i+1） the kinetic energy of the system at nΔ t. δ is the
Δ t，i=1,2…(n-2). available error coefficient.
ˆ ˆ ˆ
SL(i+1)qL(i+1) + MSS(i+1)qS(i+1) = QS(i+1) "(2 − 20)
M
3 NUMERICAL EXAMPLES
Also, according to equation (2-7), the following In this section, two numerical examples will be
equation can be obtained. performed to validate availability and efficiency of
the present sub-cycling algorithm.
SL(i+1) ⎣a0 ( qL(i+1) −qLi ) − a2qLi − a3qLi ⎦ +
ˆ
M ⎡1 1
1
⎤
3.1 A Bar-slider System
SS(i+1) ⎣a0 ( qS(i+1) −qSi ) − a2qSi − a3qSi ⎦ = QS(i+1) "(2− 21)
ˆ
M ⎡1 1
1
⎤ ˆ
A bar-slider system is shown in figure 2. Mass of the
rod is 2.0 kilograms and mass of the slider is 5.0
In equation (2-21), the slowly changing variables, kilograms. The driving torque is 100 Nm/s. Stiffness
which are used to compute the interaction of the of the spring is 1000 N/m. length of the rigid rod is 2
coupling variables, can be linearly interpolated in meters.
terms of the trapezoid rule. Results of rotational angle of the rod, the
vibration amplification of the sliding block and the
n−i i energy balance status computed by means of the
q Li = q L 0 + q Ln """"" (2 − 22)
n n sub-cycling and without sub-cycling are shown in
n−i i figure 3 to figure 6. The scale values in brackets of
Li
q = L 0 + q
q Ln """"" (2 − 23) the figure captions, such as (5:1), represent a sub-
n n
cycling with 5 sub-steps in one major step.
209
4
no subcycling
subcycling
3
Amplification(m)
2
0
0 1 2 3 4 5 6 7 8 9 10
Figure 2: Rotational rod-spring-slider system. Time(s)
Figure 5: Vibration amplification of the slider (5:1).

Comparing the results in figure 3 to figure 6, we can
see that no matter which scale of the sub-cycling is 4
adopted, the results are very similar. The error of no subcycling
sub-cycling with scale 10:1 is a little larger than that subcycling
3
of sub-cycling with scale 5:1. Yet all these two
Amplification(m)
errors are very small if compare with the original
2
results. The time cost of sub-cycling with scale 5:1
is 118 seconds and the time cost of the original
algorithm without sub-cycling is 218 seconds. The 1
proportion of time cost of these two algorithms is

54%. The time cost of sub cycling with scale 10:1 is 0
0 1 2 3 4 5 6 7 8 9 10
44 seconds and the proportion of time cost is only Time(s)
20.2%. The results of balance checking illustrate that
the sub-cycling is stable during the integral process. Figure 6: Vibration amplification of the slider (10:1).
140
3.2 Airscrew of a Jet Engine
120 no subcycling
Rotational angle(radian)
subcycling
100 FEM model of the airscrew of a jet engine are
80 displayed in figure 7. Parameters of the model are as
following (Units: kg/N/mm/s). The material of
60
airscrew is aluminium alloy. EX=7e6, PR=3,
40 DEN=2.78e-6. Diameter of the airscrew is D=900
20 mm, rotate speed is 8000 rpm.
We simulate the large range overall motion of
0
0 1 2 3 4 5 6 7 8 9 10 the airscrew by means of the Newmark sub-cycling
Time(s)
and the original Newmark respectively. The
Figure 3: Rotational angle of the bar (5:1). dynamic stress at the blade root is described in
figure 8. The time cost of the Newmark sub-cycling
is 1660 seconds and that of the Newmark is 2810
140 seconds. The computational efficiency is enhanced
120 no subcycling about 70%. The compared results show the good
subcycling
100
precision and stability of the Newmark sub-cycling.
80
60
40
20
0
0 1 2 3 4 5 6 7 8 9 10
Time(s) Figure 7: The FEM model and the local mesh.
Figure 4: Rotational angle of the bar (10:1).
210
80 REFERENCES
no subcycling
subcycling Huang Wenhu, Shao Chengxun, 1996. Flexible multi-body
Dynamic stress(MPa)
70
dynamics, The science Press.
Dan Negrut, Jose L, Ortiz., 2005. On an approach for the
60 linearization of the differential algebraic equations of
multi-body dynamics. Proceedings of IDETC/MESA.
50
DETC2005-85109. 24-28.Long Beach, USA.
Dan Negrut, Gisli Ottarsson, 2005. On an Implementation
of the Hilber-Hughes-Taylor Method in the Context of
40
0 0.005 0.01 0.015 0.02
Index 3 Differential-Algebraic Equations of Multibody
Time(s) Dynamics. DETC2005-85096.
Belytschko T., Mullen, Robert, 1978. Stability of explicit-
Figure 8: Dynamic stress of the blade root during rotation. implicit mesh partitions in time integration.
International Journal for Numerical Methods in
10
Engineering, 12(10): 1575-1586.
Neal, Mark O., Belytschko T., 1989. Explicit-explicit sub-
no subcycling
cycling with non-integer time step ratios for structural
8 subcycling
dynamic systems. Computers and Structures, 31(6):
6 871-880.
Gao H.,Li G. Y.,Zhong Z. H., 2005. Analysis of sub-
4 cycling algorithms for computer simulation of
crashworthiness. Chinese journal of mechanical
2 engineering, 41(11): 98-101.
Tamer M Wasfy, Ahmed K Noor, 2003. Computational
0
0 0.005 0.01 0.015 0.02 strategies for flexible multi-body systems. Applied
Time(s) Mechanics Reviews. 56(6): 553-613.
W. J. T. Daniel, 1997. Analysis and implementation of a
Figure 9: Rotational angle of the blade root. new constant acceleration sub-cycling algorithm.
international journal for numerical methods in
engineering. 40: 2841-2855.
Haug, E. J., 1989. Computer-Aided Kinematics and
4 CONCLUSIONS Dynamics of Mechanical Systems. Prentice-Hall.
Englewood Cliffs, NJ.
This paper firstly presents a Newmark-based FMD Lu Youfang, 1996. Flexible multi-body dynamics. The
sub-cycling algorithm. By modifying the Newmark higher education Press.
integral formula to be fitted for sub-cycling of the
FMD problems, not only the integral efficiency can
be greatly improved, but also be more easy for
convergence of the integral process.
Because of that different integral step sizes are
adopted during the sub-cycling, the integral process
can be more efficient and easier for convergence. At
the same time, Unconditional stability of the original
Newmark are still kept in the Newmark sub-cycling.
The number of the sub-step in one major cycle
can be a little infection of the integral precision of a
sub-cycling process. However, it is unobvious as the
number is within a range. Generally speaking, the
enhancement of the integral efficiency is more
significant when the number is under a limitation.
By checking the energy balance status of the
integral process real time and adjusting the step size
when necessary, the sub-cycling procedure can keep
a well convergence property and obtain the
reasonable numerical computation results.
211
ROBOT TCP POSITIONING WITH VISION
Accuracy Estimation of a Robot Visual Control System
Drago Torkar and Gregor Papa

Computer Systems Department, Jožef Stefan Institute, Jamova c. 39, SI-1000 Ljubljana, Slovenia
Keywords: Robot control, Calibrated visual servoing.
Abstract: Calibrated 3D visual servoing has not fully matured as a industrial technology yet, and in order to widen its
use in industrial applications its technological capability must be precisely known. Accuracy and
repeatability are two of the crucial parameters in planning of any robotic task. In this paper we describe a
procedure to evaluate the 2D and 3D accuracy of a robot stereo vision system consisting of two identical 1
Megapixel cameras, and present the results of the evaluation.
1 INTRODUCTION are much larger, especially with larger robots that

can reach several meters. Measuring at such
In the last decades, more and more robots distances with high precision requires much higher
applications were used in industrial manufacturing resolution which very soon reaches its technological
which was accompanied by an increased demand for and price limits. For example, nowadays 4
versatility, robustness and precision. The demand Megapixel cameras are the state of the art in vision
was mostly satisfied by increasing the mechanical technology but are not affordable in many robotic
capabilities of robot parts. For instance, to meet the applications since their price almost reaches the
micrometric positioning requirements, stiffness of robot price. The dynamics of the industrial processes
the robot’s arms was increased, high precision gears requires high frame rates which in connection with
and low backlash joints introduced, which often led real time processing puts another difficult constraint
to difficult design compromises such as the request on system integrators. The unstructured industrial
to reduce inertia and increase stiffness. This results environment with changing environmental lighting
in approaching the mechanical limits and increased is another challenge for the robot vision specialists.
cost of robots decreasing the competitiveness of the When designing the vision system within robot
robot systems on the market (Arflex, 2005). applications it is very important to choose the
Lately, the robot producers have put much effort optimal equipment for the task and to get maximal
into incorporating visual and other sensors to the performance out of each component. In the paper we
actual industrial robots thus providing a significant represent a procedure for the precision estimation of
improvement in accuracy, flexibility and a calibrated robot stereo vision system in 2D and 3D
adaptability. Vision is still one of the most environment. Such a system can be used in visual
promising sensors (Ruf and Horaud, 1999) in real servoing applications for precise tool center point
robotic 3D servoing issues (Hutchinson et al., 1995). (TCP) positioning.
It has been vastly investigated for the last two
decades in laboratories but it's only now that it finds
its way to industrial implementation (Robson, 2006) 2 METHODOLOGY
in contrast to machine vision which became a well
established industry in the last years (Zuech, 2000). Four types of accuracy tests were performed: a static
There are many reasons for this. The vision systems 2D test, a dynamic 2D test, a static 3D test, and a
used with the robots must satisfy a few constraints dynamic 3D test. Throughout all the tests, an array
that differ them from a machine vision measuring of 10 infrared light emitting diodes (IR-LED) was
systems. First of all, the camera working distances used to establish its suitability for being used as a
212
ROBOT TCP POSITIONING WITH VISION - Accuracy Estimation of a Robot Visual Control System
marker and a calibration pattern in the robot visual CINEGON 10mm/1,9F with IR filter; exposure time
servoing applications. was 10.73ms, while frame time was 24.04ms, both
Within the static 2D test, we were moving the obtained experimentally.
IR-LED array with the linear drive perpendicular to For the dynamic 2D test conditions were the
the camera optical axes and measured the increments same as in static test, except the linear guide was
in the image. The purpose was to detect the smallest moving the IR-LED array with a speed of 460mm/s
linear response in the image. The IR-LED centroids and the exposure time was 1ms.
were determined in two ways: on binary images and In the 3D reconstruction test the left camera
on grey-level images as centers of mass. During distance to IR-LED array and right camera distance
image grabbing the array did not move thus to IR-LED array were about 205cm; baseline
eliminating any dynamic effects. We averaged the distance was 123cm; Schneider-Kreuznach lens
movement of centroids of 10 IR-LEDs in a sequence CINEGON 10mm/1,9F with IR filter; Calibration
of 16 images and calculated the standard deviation region-of-interest (ROI): 342 x 333 pixels;
to obtain accuracy confidence intervals. With the Calibration pattern: 6 x 8 black/white squares;
dynamic 2D test shape distorsions in the images due Calibration method (Zhang, 1998); Reconstruction
to fast 2D movements of linear drive were method (Faugeras, 1992). The reconstruction was
investigated. We compared a few images of IR-LED done off-line and the stereo correspondence problem
array taken during movement to statically obtained was considered solved due to a simple geometry of
ones which provided information of photocenter the IR-LED array and is thus not addressed here.
displacements and an estimation of dynamic error. For the 3D dynamic test, an ABB industrial robot
We performed the 3D accuracy evaluation with 2 IRB 140 was used with the standalone fully
fully calibrated cameras in a stereo setup. Using calibrated stereo vision setup placed about 2m away
again the linear drive, the array of IR-LEDs was from its base and calibrated the same way as before.
moved along the line in 3D space with different The robot wrist was moving through the corners of
increments and the smallest movement producing a an imaginary triangle with side length of
linear response in reconstructed 3D space was approximately 12cm. The images were taken
sought. In the 3D dynamic test, we attached the IR- dynamically when the TCP was passing the corner
LED array to the wrist of an industrial robot, and points and reconstructed in 3D with an approximate
dynamically guided it through some predefined speed of 500mm/s. The relative length of such
points in space and simultaneously recorded the triangle sides were compared to the sides of a
trajectory with fully calibrated stereo cameras. We statically-obtained and reconstructed triangle. The
compared the reconstructed 3D points from images robot native repeatability is 0.02 mm and its
to the predefined points fed to robot controller. accuracy is 0.01mm.
3 TESTING SETUP 4 RESULTS

The test environment consisted of: 4.1 2D Accuracy Tests
PhotonFocus MV-D1024-80-CL-8 camera with
CMOS sensor and framerate of 75 fps at full The results of the evaluation tests are given below.
resolution (1024x1024 pixels), Tests include the binary and grey-level centroids.
Active Silicon Phoenix-DIG48 PCI frame For each movement increment the two figures are
grabber, presented, as described below.
Moving object (IR-LED array) at approximate Pixel difference between the starting image and
distance of 2m. The IR-LED array (standard the consecutive images (at consecutive positions) –
deviation of IR-LED accuracy is below 0.007 for each position the value is calculated as the
pixel, as stated in (Papa and Torkar, 2006)) average displacement of all 10 markers, while their
fixed to Festo linear guide (DGE-25-550-SP) position is calculated as the average position in the
with repetition accuracy of +/-0.02mm. sequence of the 16 images grabbed at each position
For then static 2D test the distance from camera in static conditions. The lines in these figures should
to a moving object (in the middle position) that be as straight as possible.
moves perpendicularly to optical axis was 195cm; The 0.01mm, 0.1mm, and 1mm increments for
camera field-of-view was 220cm, which gives pixel 2D tests are presented in Figure 1.
size of 2.148mm; Schneider-Kreuznach lens
213
difference [pixel] 2
⎛
∑ ( x − x )( y − y ) ⎞⎟
0,03
2 ⎜
R =⎜ ⎟ (1)
⎜ ∑ ( x − x ) 2 ( y − y ) 2 ⎟⎠
0,02
binary
grey level
⎝
0,01
Considering the R2 threshold of 0.994 we were

0,00
position [mm]
able to detect increments of the moving object in the
0,00 0,01 0,02 0,03 0,04 0,05
range of 1/5 of a pixel. The value of the threshold is
difference [pixel]
0,30 set to the value that gives a good enough
approximation of the linear regression model, to
0,20 ensure the applicable results of the measurements.
binary
grey level
0,10
Table 1: Comparison of standard deviations and R2 values
position [mm] for different moving increments in 2D.
0,00
0,00 0,10 0,20 0,30 0,40 0,50
standard
R2
difference [pixel]
3,00
increments deviation[mm]
[mm] grey- grey-
binary binary
level level
2,00
0.01 0.045 0.027 0.4286 0.6114
binary
grey level 0.1 0.090 0.042 0.8727 0.9907
1,00
1 0.152 0.069 0.9971 0.9991
position [mm]
0,00
0,00 1,00 2,00 3,00 4,00 5,00
The dynamic 2D test showed that when
comparing the centers of the markers of the IR-LED
Figure 1: Pixel difference for 0.01mm (top), 0.1mm array and the pixel areas of each marker in statically
(middle), and 1mm (bottom) increments. and dynamically (linear guide moving at full speed)
grabbed images there is a difference in center
Figure 2 compares normalized pixel differences positions and also the areas of markers in
in grey-level images of a single marker. dynamically grabbed images are slightly larger than
those of statically grabbed images.
normalized
difference
1,0
Table 2 presents the differences of the centers of
0,9
the markers, and difference in sizes of the markers
0,8
of the statically and dynamically grabbed images.
0,7
0,6
Table 2: Comparison of the images grabbed in static and
0.01mm
0,5
0.1mm dynamic mode.
1mm
X Y width height area

0,4
0,3
static 484.445 437.992 6 6 27
0,2
dynamic 484.724 437.640 7 6 32
0,1
position
0,0
1 2 3 4 5 6
Regarding the results presented in Table 2, the
Figure 2: Normalized differences of grey-level images for accuracy of the position in direction x of
each position comparing different increments. dynamically grabbed images comparing to statically
grabbed is in the range of 1/3 of a pixel, due to the
A linear regression model was applied to gravity centre shift of pixel area of marker during
measured data, and the R2 values calculated to asses the movement of the linear guide.
the quality of fit. The results are presented in Table 1
for 2D tests and in Table 2 for 3D tests. The R2 4.2 3D Reconstruction Tests
value can be interpreted as the proportion of the
variance in y attributable to the variance in x (see We tested the static relative accuracy of the 3D
Eqn. 1), where 1 stands for perfect matching (fit) reconstruction of the IR-LED array movements by
and a lower value denotes some deviations. linear drive. The test setup consisted of the two
calibrated Photonfocus cameras focused on the IR-
214
ROBOT TCP POSITIONING WITH VISION - Accuracy Estimation of a Robot Visual Control System
LED array attached to the linear drive which lens distortions since it is almost the same in the
exhibited precise movements of 0.01mm, 0.1mm dynamic and in the static case. The images and the
and 1mm. The mass centre points of 10 LEDs were reconstruction in dynamic conditions vary only a
extracted in 3D after each movement and relative 3D little in comparison to static ones.
paths were calculated and compared to the linear
drive paths. Only grey-level images were
considered, due to the better results obtained in 2D 5 CONCLUSIONS
tests, as stated in Figure 2 and in Table 1. The
0.01mm, 0.1mm, and 1mm increments for the 3D
We performed the 2D and 3D accuracy evaluation of
tests are presented in Figure 3. the 3D robot vision system consisting of 2 identical
The accuracy in 3D is lower than in the 2D case, 1 Megapixel cameras. The measurements showed
due to calibration and reconstruction errors, and
that the raw static 2D accuracy (without any
according to the tests performed it is approximately subpixel processing approaches and lens distortion
1/2 of a pixel. compensation) is confidently as good as 1/5 of a
Table 4 presents the results of the 3D dynamic
pixel. However, this is reduced to 1/2 of a pixel
tests where the triangle area and side lengths a, b when image positions are reconstructed in 3D due to
and c, reconstructed from dynamically-obtained reconstruction errors.
images were compared to static reconstruction of the
In the dynamic case, the comparison to static
same triangles. 10 triangles were compared, each conditions showed that no significant error is
formed by a diode in IR-LED array. The average introduced with moving markers in both, 2D and 3D
lengths and the standard deviations are presented.
environment. For the speed level of an industrial
robot the accuracy is though not reduced
Table 3: Comparison of standard deviations and R2 values
for different moving increments in 3D.
significantly.
standard
increments [mm] R2
deviation [mm]
0.01 0.058 0.7806 ACKNOWLEDGEMENTS
0.1 0.111 0.9315
1 0.140 0.9974 This work was supported by the European 6th FP
project Adaptive Robots for Flexible Manufacturing
normalized
difference
Systems (ARFLEX, 2005-2008) and the Slovenian
1,00 Research Agency programme Computing structures
0,90 and systems (2004-2008).
0,80
0,70
0,60
0.01 mm REFERENCES
0,50 0.1 mm
1 mm
0,40
Arflex European FP6 project official home page:
0,30
http://www.arflexproject.eu
0,20
Faugeras, O., 1992. Three-Dimensional Computer Vision:
0,10 A geometric Viewpoint, The MIT Press.
0,00 Hutchinson, S., Hager, G., Corke, P.I., 1995. A tutorial on
1 2 3 4 5
positions
6
visual servo control, Yale University Technical Report,
RR-1068.
Figure 3: Pixel difference in the 3D reconstruction. Papa, G., Torkar, D., 2006. Investigation of LEDs with
good repeatability for robotic active marker systems,
Table 4: comparison of static and dynamic triangles. All Jožef Stefan Institute technical report, No. 9368.
measurements are in mm. Robson, D., 2006. Robots with eyes, Imaging and
machine vision-Europe, Vol. 17, pp. 30-31.
a σ b σ c σ Ruf, A., Horaud, R., 1999. Visual servoing of robot
static 193.04 12.46 89.23 2.77 167.84 12.18 manipulators, Part I: Projective kinematics, INRIA
dynamic 193.51 12.43 89.03 2.77 167.52 12.03
technical report, No. 3670.
Zhang, Z., 1998. A flexible new Technique for Camera
Calibration, Technical report, MSRTR-98-71.
We observe a significant standard deviation (up Zuech, N., 2000. Understanding and applying machine
to 7%) of triangle side lengths which we ascribe to vision, Marcel Dekker Inc.
215
MULTI AGENT-BASED ON-LINE DIAGNOSTIC SCHEME
OF SUBSTATION IED
Seong-Jeong Rim and Seung-Jae Lee

Next-generation Power Technology Center(NPTC), Myongji University, Yongin, Korea
Keywords: Multi-agents, On-Line Diagnosis, SA (substation automation), IED (intelligent electronic device).
Abstract: This paper presents a novel multi agent-based diagnostic scheme of substation IED. The proposed method is
the on-line diagnostic scheme for the IEDs in IEC61850-based SA system using multi-agents. Multi-agents
detect the fault of IED installed in substation and apply to improvement of reliability in protection and
control system of substation. These agents in the SA system embedded in the station unit, the engineering
unit, the trouble manager, backup IED and protection and control IED as the thread form. Through the
implementation and the case studies of the proposed scheme, the availability is proven.
1 INTRODUCTION This method is the ODS (on-line diagnostic scheme)

for the IEDs in IEC61850-based SA system using
IED (intelligent electronic device) mis-operations, multi-agents (IEC 61850, 2002-2005). Multi-agents
such as failing to isolate a faulted line or incorrectly detect the fault of IED installed in substation and
tripping a healthy line, are involved in major apply to improvement of reliability in protection and
disturbances within power systems and can lead to control system of substation. These agents in the SA
catastrophic cascading events or even blackouts system embedded in the station unit, the engineering
(Bertsch, J. et al. 2003). Giving IEDs the ability to unit, the trouble manager, backup IED and
diagnose would be a significant addition at critical protection and control IED as the thread form.
locations in the power system by helping to reduce Through the implementation and the case studies of
the number of mis-operations. the proposed scheme, the availability is proven.
The existing diagnostic method of IED mainly
uses off-line TBM (time-based maintenance) or
CBM (condition-based maintenance) (Yu X.Z. et al, 2 IEC61850-BASED SUBSTATION
2000). Especially IED doesn’t operate in normal AUTOMATION
condition and operate it when the disturbance occurs
in power system. It is hard to determine the fault The scheme presented in this paper used many
condition before IED is tested. features of IEC61850 to diagnose the substation
On the other hand, the conventional scheme to IED. The typical features of IEC61850, which make
improve the IED’s reliability, have the redundant our scheme possible, consists of the network based
structure as the backup device (Ding M. et al., data transfer (process bus), the standardized
2004). Redundant structure employs the dual device interface and SCL (Substation Configuration
with the same function so that improves the Language)-based engineering.
reliability of the system. However, this system has
the drawback which not only increases the
2.1 Process Bus
probability of mis-operation but cost. Thus, the
improvement scheme of the reliability is required to
In the existing substation protection and control
overcome these problems. system, analogue data such as voltage and current
are fed into protection relays from CT and VT which
In this paper, a novel multi agent-based are installed on the primary protection equipments
diagnostic scheme of substation IED is proposed.
such as transformers and circuit-breakers. In the
216
IEC61850 based SA system, analogue signal will be Single Line

Diagram
Functional
Specification
converted into digital data by the process IED
SSD file
installed on the primary protection equipment and be (Sys. Spec.
sent to the bay IED through the process bus (Fig.1). Descrip.)
SCD file
(Sys. Config.
The largest difference between the two schemes
Device Selection
Descrip.)
System
above is whether the data transfer path can be Configurator
changed during operation. In the existing system,
analogue data are transferred through hardware, so
data transfer path cannot be changed unless the ICD file
CID file
physical connection is changed. In this scheme, data (IED
Capability (Config. IED
Descrip.)
are transferred on process bus, so the data transfer Devices
Descrip.)
IED
path can be changed by changing the destination of (IEDs) Configurator
data packet.
Station Engineering
Figure 2: SCL based engineering.
Station level
HMI Unit
2.3 Standardized Interface
Station bus
In IEC61850, all the information about a substation

Bay level Bay IED IED is modelled into a standard interface called LN
Controller X1 X2
(Logical Node). Data exchange between these IEDs
is possible as long as all these IEDs follow the
Process bus standard LN interface regardless of IED hardware
structure or algorithm. For example, if there are two
Process level Intelligent Non-conventional
Switchgear CT/VT distance relays made by different companies, whose
signal filtering, calculation of RMS, error detection
Figure 1: Structure of IEC61850-based SA System.
algorithm, and hardware are different, the two relays
can be exchanged when they use the same services
and the same LN interface.
2.2 SCL-Based Engineering
For integration of various IEDs, IEC61850 suggests
a standard engineering technique. The key is in two 3 MULTI AGENT-BASED
softwares called system configurator and IED ON-LINE DIAGNOSTIC
configurator. Figure 2 shows the course of SCHEME
IEC61850 engineering. First, system configurator
gets all the information about the substation from the
SSD (System Specification Description) file that 3.1 On-Line Diagnostic Scheme
contains system related information and the ICD
(IED Capability Description) file that contains IED The structure of the ODS (on-line diagnostic
related information, and creates the SCD (Substation scheme) is shown in Fig. 3. Note that the station
Configuration Description) file that configures the unit, bay IED, process IED, and engineering unit
function and data flow for each IED. All the files constitute the substation automation system, which
mentioned above are made by XML-based SCL. An follows IEC61850 standard. Backup IED and trouble
IED configurator receives the SCD file and creates manager are added to the system to implement the
CID (Configured IED Description) file that contains ODS. Trouble manager is in charge of detecting the
the format suitable for IED. Finally to download IED error by sending a test pattern to bay IED and
CID file, each IED is able to communicate with comparing the results with expected ones. Backup
among them. IED is a redundant IED that downloads software
In this scheme, when an IED error is detected, it from engineering unit and then starts to operate
reconfigures its data path so that the data that were when error is detected on bay IED.
transferred to bay IED is now transferred to the The TM (trouble manager) periodically checks
backup IED. The reconfiguration is feasible because the functions of each bay IED to determine whether
of the SCL based engineering. the IED has an error. The fault detection procedure
in trouble manager is as follows:
217
1) EU (engineering unit) assigns the DUT (device As shown in figure 4, the multi agent-based ODS
under test) and inform to TM. structure consists of data source, data storage,
2) TM transfers the information such as LN, preco communication mechanism and data consumer.
nfigured control block and settings for the DUT When agents detect the fault of IED in protection
to backup IED. system, it replaces faulted IED and operates with
3) TM starts the sample value path from process IE backup IED properly.
D #1 to backup IED.
4) TM stops the sample value path from process IE
D to DUT. H M I p la tfo rm
5) TM sends the test input pattern to DUT via proc

User
ess bus.
6) DUT sends the response for the test input patter
n to TM. DB User
DB agent agent
7) TM notifies the fault status of the DUT to statio
n HMI if TM detects the abnormal status of DU
T. F a u lt
R e p la c e
d e te c tio n
agent
Station Engineering agent
Unit Unit
7 IE D B ackup
Station Bus
agent agent
6 1
2 S u b s ta tio n
CT
Bay IED Backup Trouble
(DUT) IED Manager VT
5 Figure 4: The proposed multi agent-based ODS structure.

Process Bus
4 3 Table 1 list a types and functionality of multi-agents

required for performing the multi agent-based ODS.
Process Process
IED #1 IED #2
Table 1: Types and functionality of agents.
Types Duty Ability
Figure 3: Structure of ODS. Send the test
pattern, Create the
If no fault is detected on DUT, TM requires EU Fault
Detect the fault of DUT expected result,
to reconfigure the system to recover the original detection
Compare with actual
operation of DUT and then continue to check next result
IED. Also if fault is detected, system waits for Control the
Replace DUT with
repairing. Backup IED will replace the failed DUT Replace communication
backup IED
until it is repaired. service
Add the information
Response the required
3.2 Structure of Multi Agent-Based DB
information
to DB, Response for
ODS query
Assign the IED to be Display the
The proposed method uses several agents which User tested and display the information,
distributed in a protection/control IED. This method fault Transfer the query
detects the IED fault and replaces IED to be tested. Perform function of the Perform the function
Backup
In order to operate efficiently, these distributed DUT of DUT
systems, agents have the functionality which Perform function of
protection/control IED Response for the
autonomously interacts between them. Figure 4 IED
and communicate with request
shows the proposed multi agent-based ODS
other agents
structure.
218
The agents listed in above table communicate

each other with MMS (Manufacturer Message
Specification) instead of ACL (Agent
Communication Language), since the agents
embedded in IEC61850-based SA system.
4 CASE STUDY
4.1 Test System
Figure 5 shows the test system to verify the
performance the multi agent-based ODS. The part
inside dotted line in this figure means the additional Figure 6: Response message from DUT.
equipments to conventional SA system.
5 CONCLUSION
A novel multi agent-based ODS can be applied in
the IEC61850-based SA system is proposed in this
paper. The technique presented in this paper detects
the fault of IED automatically and it replaces the
failed IED by backup IED to tolerate the fault in
system level. Through the implementation and the
case studies of the multi agent-based ODS, the
availability is proven.
ACKNOWLEDGEMENTS
Figure 5: Test system. This work was supported by the ERC program of
MOST/KOSEF (Next-generation Power Technology
In the test system, HyperSim (real-time Center).
simulator) uses to generate the fault in power system
(HyperSim). MU (merging unit) transfers the digital
signal to protection IED (OCR/Recloser) via
network after it had A/D conversion. Actuator has REFERENCES
the function such as on/off of circuit breaker as
Bertsch, J. et al. 2003, “Experiences with and perspectives
process IED. Backup IED performs the same
of the system for wide area monitoring of power
function of DUT. systems,” CIGRE/IEEE PES International
Symposium, Quality and Security of Electric Power
4.2 Simulation: DUT Diagnosis Delivery Systems, 8-10 Oct. pp.5-9.
Yu, X. Z. et al., 2000, “Condition Monitoring of a power
In this simulation, we consider two types of fault station,” International Conference on Power System
such as communication and IED hardware part. We Technology, Vol.2, 4-7 Dec., pp.1029-1033.
assume that the fault in communication part is LAN Ding M. et al., 2004, “Reliability analysis of digital relay,”
cable broken and the fault in IED hardware part is Eighth IEE International Conference on Developments
in Power System Protection, Volume 1, 5-8 April,
power off.
Vol.1, pp. 268 – 271.
We perform the test that TM can detect the fault. IEC 61850, 2002-2005, Communication networks and
After the DUT’s function replaces with backup IED, systems in substations.
TM sent the test input pattern and received the HyperSim, http://www.transenergie-tech.com/en/
response from DUT. Figure 6 shows the GOOSE
message from DUT as response.
219
SCADA WEB
Remote Supervision and Maintenance of Industrial Processes
José Ramón Janeiro, Eduardo J. Moya, David García, Oscar Calvo and Clemente Cárdenas
Fundación CARTIF, Parque Tecnológico de Boecillo. Parcela 205. 47151 Boecillo, Valladolid, Spain
[email protected], [email protected], [email protected], [email protected], [email protected]
Keywords: Remote Supervision, Control, Maintenance, Industrial Process, PLC, OPC, OPC-DA, SCADA, Web,
Internet, Java, GSM, GPRS, SMTP, PLC.
Abstract: This article explains a SCADA System called SCADA Web developed by CARTIF Foundation. This
SCADA System makes possible remote monitoring and control from the process controlled by PLC using
Web Technology. SCADA Web has been developed on platform Java EE and provides visualization and
control signals, trend charts, alarms, historical data and alarm reports.
1 INTRODUCTION 2 REMOTE SUPERVISION AND

MAINTENANCE OF
Internet, together with the use of technologies such
as GMS/GPRS/UMTS (International Engineering
INDUSTRIAL PROCESSES
Consortium), provides communication mechanisms
with a great number of possibilities (GeneraLynx). Nowadays, more and more systems are being
These technologies are present more and more in the supervised in a remote manner in order to keep the
industry. process running in a correct state. The most
Nowadays, a large extent of automation important applications to be carried out are
industrial solutions demand maintenance and maintenance actions, although this depends on the
supervision systems because of the high complexity nature of the process.
and the need of managing a large volume of data. The traditional arquitecture of these systems in
Computers are the main support for supervision relation to remote supervision is composed by a
and maintenance systems due to the high Web Server that, after a correct authentication,
performances and informations that they can allows access to remote supervision services of an
process. industrial process. The supervision systems have to
The main reasons to develop a SCADA Web fulfill the next requirements:
(Janeiro, J. R., 2006) have been the growing use of It is only necessary one computer and an
the supervision and maintenance system on Internet connection to supervise the system
computers and the features that support the There is not need of special software
technologies GSM/GPRS/UMTS and Internet. installation in the remote computer, except for
This paper is organized as follows. In Section 2 some specific plug in.
the characteristics that a remote supervision and Independence of the remote computer operative
maintenance system have to support are presented. system.
The SCADA Web Application arquitecture The access to applications must be controlled to
necessary to supervise a process in a remote way is avoid risks, due the that these remote
presented in Section 3. supervision tasks can be accessible through a
In Section 4 the main features and an operation network like internet.
manual of the SCADA Web Application is Be careful with the exchange between remote
presented. computer and the process due to security
Conclusions and open issues for future research problems. One possible solution is the use of
are discussed in Section 5. safe communication protocols like HTTPS.
220
SCADA WEB - Remote Supervision and Maintenance of Industrial Processes
Maintenance systems send alarms or warning by Apache Tomcat (The Apache Software
means of SMS and e-mail. These methods supply an Foundation) has been the server used. SCADA Web
efficient notification reducing failure correction Application is based on the next technologies: JSP,
times and therefore reducing process costs. Servlet, Applet, Beans, JDBC, JavaMail, JAF and
JAXB. The use of these technologies leads to a high
development cost of the application, a null or low
3 SCADA WEB ARCHITECTURE acquisition cost and high flexibilility and scalability
compared, for example, to commercial SCADA.
The acronym of SCADA Web Application comes
from SCADA (Supervisory Control And Data
Acquisition) and WEB (World Wide Web). This
application makes remote supervision and
maintenance of any industrial process possible. The
above-mentioned process can be controlled by
PLC´s of different brands by means of a web
browser, an e-mail client and a mobile.
This application offers a number of features
beyond the usual characteristics of a simple SCADA
system. It contains elements focused towards
industrial process maintenance like SMS messaging
(Short Message Service) using technology
GSM/GPRS (Global System for Mobile
Communications /General Packet Radio Service)
Figure 1: SCADA Web Application Structure.
and e-mails.
Using OPC-DA(OPC-Foundation) protocol
The configuration parameters of SCADA Web
(OLE for Process Control-Data Access) based on
Application are inserted in an XML file called
OLE/COM/DCOM (Object Linking and
scadaweb.xml. This file has to keep the xml schema
Embedding/Component Object Model/Distributed
which is defined in the file scadaweb.xsd The use of
Component Object Model) it is possible to access
an XML file to configure the application allows a
the variables.
high degree of flexibility and scalability in the
In order to avoid the use of DCOM technology
parameters configuration, as well as a simplification
due to problems with the firewalls (OPC
of the configuration process to the user.
Programmers’ Connection), we have developed an
Thanks to JAXB (Java Architecture for XML
application called OPC gateway to supervise the
Binding) the schema XML is mapped to JAVA
process without limiting the structures of control.
classes providing a simple way to access the
This application uses OPC-DA implementation in
configuration file scadaweb.xml. The SCADA Web
its interface “Automation” allowing the SCADA
Application is divided in two modules called
Web Application to access the variables by means of
Supervision and Maintenace modules. Next, these
strings UNICODE based on Sockets. Although the
modules will be explained.
OPC gateway application support DCOM, it is
expected to be installed in the same computer where
the OPC-DA server is located so that it is only
3.1 Supervision Module
necessary to use COM technology.
This module has the typical characteristics of a
The use of OPC-XDA(OPC Foundation) (OLE
comercial SCADA. An industrial process can be
For Process Control- XML Data Access) instead of
remotely supervised via WEB thanks to this module.
OPC-DA was ruled out because of the generally
Although this module was designed for remote
scarce in utilization of this protocol by PLC´s
supervision via WEB, it can be used for local
manufacturers. Even so, it is desirable that the
supervision of a process. Furthermore, thanks to its
application supports both protocols in future
generic design, it’s possible to supervise the process
versions.
using informatics nets so complex as desired (LAN,
In fact, the SCADA Web Application is a Web
Internet, Routers, ...).
application developed using the platform Java EE
The Supervision Module of SCADA Web
(Java Enterprise Edition) (Sun Microsystems).
Application is a multiuser system that lets to several
221
remote users supervise the same process at the same 4 SCADA WEB
time.
This module is based on a client-server SCADA Web Application is executed within
architecture (Applet-Servet/JSP) which allows to applications servers. This application allows an
distribute the computacional load of the application interaction between user and process by means of
between Tomcat Apache server and remote three interfaces, HTTP, GSM/GPRS and e-mails.
machines. The first interface, HTTP, makes it possible to
An Applet is an application, written on Java, supervise the process in a remote manner from a
which is executed on a Web browser. Using an web browser.
applet allows us to have a more complete interface The interface GSM/GPRS is oriented to offer an
from the point of view of the client. instantaneous warning procedure reducing failure
A Servlet is a Java class that is carried out on an correction times and the costs of industrial
application server in order to answer clients processes.
requests. The clients are Applet´s in the case of Finally, the e-mail interface carries out the same
SCADA Web. functions that GSM/GPRS interface, although it
A page JSP is a file that combines HTML targets allows to send more information. The main reason is
with script targets which belong to Java Server to provide redundant alarm warning procedures, so
Pages specification. The page JSP has a similar that the user is notified as soon as possible. E-mail,
aspect to HTML targets, but they are changed in unlike GSM/GPRS, is used to notify less critical
Servlet in order to compile and generate the alarms. The HTTP interface has been grouped inside
appropriated class files. The resulting file from the what has been called Supervision Module, while
transformation of page JSP is a combination of GSM/GPRS interface and e-mails are in
HTML code cointained in JSP page and dynamic Maintenance. Next, these modules are explained.
content which is especified by JSP specification
targets. The exchange of information between 4.1 SCADA Web Configuration
Applet and Servlet is carried out by HTTP
tunneling, in other words, transmitting binary As in other software applications, the configuration
information using HTTP protocol. of SCADA Web Application is done by an XML file
Due to security obsession this protocol is usually according to XML Schema called scadaweb.xsd.
the only possible method of information This configuration file has four blocks well
transmission. differentiated: Parameters, Maintenance, Protocols,
and Users.
3.2 Maintenance Module The reading frecuency of PLC´s variables in
“Paramaters Block” is normally about half a second.
This module is executed as a daemon checking The update frequency of the different databases of
permanently that alarms have been configured to the supervisory system is also configurated These
take place. Alarm notification is carried out by are called reports or historical data.
maintenance workers using two diferent methods Inside the “Maintenance Block” the neccesary
depending on configuration. basic parameters required by the above-mentionated
The first method consist of an alarm notification block are configurated. There are two sections in this
using GSM/GPRS technology by SMS. It is block: SMS and E-MAIL. In SMS Section the port
necessary that the computer where SCADA Web is configurated. It contains the modem GSM/GPRS
application is installed, is connected to a the PIN, the SMS Server phone and the alarms to
GSM/GPRS modem, normally through a serial port. detect besides text message and the phone where it
Communication between modem and SCADA Web will be sent.
is made by standard AT commands. The library The SMTP server (address and port), the sender
command used to operate the computer ports is one address, alarms to detect together with the content
from the RXTX organization (RXTX Serial and and message addressee are configurated in E-MAIL
parallel I/O libraries supporting Sun’s CommAPI). Section.
The second method consist of alarm notification In Protocol Block the available protocols, in this
using e-mails. A SMTP server which is configurable case OPC-DA, are added together with the adress
in SCADA Web Application is used. In this case the and port of OPC-DA server.
library used is JavaMail from Sun Microsystems.
222
In Users Block passwords, usernames and access The application allows to add windows so that a
permission to the different parts of the program are SCADA system can be built with so many windows
configurated. as desired. Each window can be associated to a
wallpaper.
4.2 Supervision Module Hereafter the available components of the
application will be explained. Every component has
The necessary requirements to access SCADA Web a properties dialogue box associated that allows to
application are Internet connection and a computer. realize its configuration.
After previously stated requirements are fulfilled Some of the components that can be added
it is necessary to open a web browser window and to (Besides of labels, rectangles and image
connect to the server where the SCADA Web components) are:
application is installed. A similar window is showed
in Figure 2. Text Field displays analog and digital
variables.
Figure 4: Text Field Component.
LED can be associated with one or more digital

variables. This element makes it possible, by
means of colors, to show the state of the
variables. A typical example is failure-non
failure state monitoring and on/off motor state
Figure 5: LED Component.
Figure 2: Initial Window of SCADA Web. Button is used to control digital or analog
variables. When the button is pressed,
In this window it is neccesary to authenticate in depressed or is pressed during a time period,
order to access to the application. After a correct the variable changes of value.
authentication, it is possible to access the
applications: “Builder” and “Viewfinder”.
The “Builder Application” is going to allow the
creation of a series of windows with components Figure 6: Button Component.
thanks to a high level graphic interface with the user.
Linking ones with the others it is possible to Vertical Bar Graph simulates a level and it is
build a SCADA system of a specific installation. associated with analog and digital variables.
In the figure 3 an image of “Builder Application
is showed”.
Figure 7: Vertical Bar Graph.
Toggle allows to supervise digital variables.

Figure 3: SCADA Web Application: “Constructor”.
The monitoring of variables is carried out by
the state (pressed/depressed) in which the
223
toggle can be found. When the user presses,

the toggle the variable value is modified. In
Figure 9 the dialogue window to configure the
component is depicted.
Figure 8: Toggle Component.

Figure 11: Reports Component.
Trend Charts. Besides monitoring process

variables using controls explained before is possible
to obtain the trend of these variables by graphics
using their corresponding component. Their value
can be also scaled as it is showed in the Figure
12(JFree.org).
Figure 12: Graphics Component.
Historical Data In this last component the data

Figure 9: Dialogue window of a toggle. to register in a database are configurated. Later,
during the process supervision it will be possible to
Alarms. In the SCADA Web Application it is do consultations of these data by graphics or
also allowed to configure alarms. It is possible the accessing directly to the database to obtain the real
monitoring and recognition of alarms. Alarms are value. An example is depicted in Figure 13
administered in the server. Because of this, the (JFree.org).
consistency of all alarms by users can be saved. In Besides, is necessary to configure users that
this way we can supervise remotly and could use this SCADA system and the permission
simultaneously the process. level of each one. Due to this and to the fact that
Moreover, in order to avoid lacking firmness in each component has a permission level associated is
relation to dates, every happened event is registered possible to control the access as well as to adapt the
according to server time. All these events are SCADA system according to permission level of the
registered in a database to a subsequent consultation. authenticated user.
In Figure 10 the alarms component is showed.
Figure 10: Alarms Component.
Alarm Reports. Alarms are registered in a

Figure 13: Historical Component.
database which can be looked up using the “Report
Component”. The generated reports can be printed.
After having built the SCADA system of a
particular installation, then it will be possible to keep
224
in a local hard disk or in the server. In the last necessary to have a computer and Internet
instance it is possible to do a remote supervision of connection. The SCADA system can also represent
the process if you have a computer and internet by graphics the current evolution of process
connection. variables and the configuration of alarms to warn
“Viewfinder Application” makes it possible to when a failure takes place. Besides, these alarms are
carry out the SCADA system created with the registered and can be recognized by user.
“Builder Application”. In order to configurate these There is a possibility of printing and the
configurations it is necessary to open them and after monitoring reports by SCADA system of industrial
a correct authentication the variable update plant.
frequency will be fixed. The SCADA system allows to create historical
The configuration of update variable frequency data in order to a later consulting in a visual way.
make it possible to adapt the refresh time to the In the SCADA permission levels to adapt the
necessities of the person that wants to supervise the posibilites of supervision tasks to worker
system. A clear example happens in industry when permission level who supervises the installation are
the system goes to be supervised simultaneously by configurated.
workers in the monitoring process and quality Finally, it shoud be remarked that thanks to use
control. an object orientation programming language and an
organization of application in components it is
4.3 Maintenance Module possible easily add new elements in this application.
This module sends alarms of the industrial process

by SMS messages and e-mails. The complete ACKNOWLEDGEMENTS
configuration is done by XML configuration file.
Thanks to this module the alarm notification time This work was supported in part by “Programa de
generated is decreased and the costs in the industrial Fomento de la Investigación Técnica para los
process associated with alarms are reduced. Centros Tecnológicos”, (PROFIT grant FIT 330220-
Furthermore, all alarm events and e-mail and SMS 2005-138 ) from the Spanish Education and Culture
messages associated are registrated in SMS and e- Ministry.
mail databases.
REFERENCES
International Engineering Consortium. Global System for
Mobile Communication, <http://www.iec.org/online/tu
torials/gsm/> (last accessed on January 10, 2007)
Janeiro, J. R., 2006. Supervisión remota de procesos
industriales controlados por Autómatas Programables,
Figure 14: An alarm e-mail. University of Valladolid. Spain, 1nd edition.
JFree.org. JFreeChart, <http://www.jfree.org/jfrechart/>
(last accessed on January 10, 2007)
OPC Foundation. Specifications,http://ww.opcfoundati
5 CONCLUSIONS on.org (last accessed on January 10, 2007)
OPC Programmers' Connection. OLE for Process Control,
A system SCADA for remote monitoring and http://ethernet.industrial-networking.com/ieb/article
control of procceses is suggested. This SCADA Web display.asp?id=21 (last accessed on January 10, 2007)
RXTX. Serial and parallel I/O libraries supporting Sun's
Application consists of a sub-application called CommAPI, <http://ww.rxtx.org> (last accessed on
“Builder” that allows to make SCADA systems for January 10, 2007)
an industrial plant and a sub-application Sun Microsystems. Java Enterprise Edition, <http://java
“Viewfinder” whose function is supervise an .sun.com/javaee/> (last accessed on January 10, 2007)
industrial process based on SCADA systems built by The Apache Software Foundation. Apache Tomcat,
the “Builder”. <http://tomcat.apache.org> (last accessed on January
The process variables of an industrial plant 10, 2007)
controlled by different PLC´s brands can be GeneraLynx. Remote supervision and control by WAP,
remotely supervised by this application. It is only <www.euroines.com/down/DemoDoc/WapScada%20
DD.pdf> (last accessed on January 10, 2007)
225
PROPERTY SERVICE ARCHITECTURE FOR DISTRIBUTED
ROBOTIC AND SENSOR SYSTEMS
Antti Tikanmäki and Juha Röning

Computer Engineering laboratory, University of Oulu, [email protected], [email protected]
Keywords: Distributed robots and systems, multi-robot systems, sensor networks, Property Service architecture.
Abstract: This paper presents a general architecture for creating complex distributed software systems, called Property
Service architecture. The system may contain resources like robots, sensors, and different kinds of system
services, such as controller units, data storages, or a collective model of the environment. This architecture
contains several solutions and distributed system design methods for developing advanced and complex
systems. It also provides the possibility to add new resources to the system easily and during operation.
Each service has a very simple generalized interface. This meets the requirements of distributed robotic
applications, such as remote operation, multi-robot cooperation, and the robot's internal operation. The
simplicity of the interface also provides a possibility to scale down the service even on the low-cost, low-
performance microcontrollers used in small-sized robots. The main features of the architecture are the
dynamic properties of the resources, automatic reconfiguration, and the high level of reusability of the
implemented methods and algorithms.
1 INTRODUCTION process of maintaining and updating the interfaces

becomes very challenging.
Creating a distributed system from several kinds of For heterogeneous devices that take part in a
devices and functionalities is a very demanding task. larger system, a common problem is how to define
Since, for example, a robot may contain many an interface for each service in the system. This
different kinds of functionalities, actuators, and becomes even harder in multi-robot systems, where
sensors, the process of controlling is complex and different kinds of robots with different capabilities.
the interface becomes very complex. Different As the system improves, the interface must be
systems cannot communicate with each other if there changed and the whole system must be recompiled.
is no single commonly used communication In real world applications when the
standard. Figure 1 shows an example of a set of communication channel between robots also
resources that a distributed system might have. The unreliable, a common solution is to reduce
operating environment might have several sensors, communication and increase autonomous
lights and cameras. Different robots might need to functionalities. Instead of sending drive commands
be used, some of them might be legged, and they to a robot, a target of movement is delivered to the
might contain several kinds of sensors. The human robot. Increasing the capabilities of the robot leads
operator has a graphical user interface where tasks to an increase in features that must be accessible
can be selected, and the system might have several through the interface. This sets either dynamic
automatic functionalities. Each device is used in requirements for the interface or a need for
different ways, and they can communicate in continuous updating of interfaces.
different ways. The lighting of the room is To be able to achieve inter operation of a
connected to a special control protocol, room versatile set of robots and devices, very simple but
cameras can be controlled using a serial port, and flexible middleware is needed. The main
mobile robots use a wireless LAN. As the number of requirement for the middleware is that it is possible
resources in the system increases, for example when to scale down to small-sized devices with a very
a new kind of robot is developed to the system, the limited set of capabilities. On the other hand, it must
be able to provide an interface to varying features of
resources in the system and expand as new resources
226
are added to the system. This paper proves that it is are Mobility software (Rea, 1999), the Player/Stage
possible to implement flexible application layer that project (Gerkev et. al. 2003), Miro (Utz et. al. 2002)
can be implemented on all kinds of transportation and ORCA (Brooks 2005), which is based on a
layers while providing all the capabilities of state-of- CORBA component model. Later in ORCA2,
the-art distributed architectures. CORBA has been replaced with Ice (Hemming,
Our solution, called Property Service 2004) middleware, which provides improved
architecture, has been developed to be a generalized features compared with CORBA
interface for all kinds of devices or resources in a Wang et al. presented a COM-based architecture for
distributed system. Property Service provides the the fusion of logical sensors (Wang et al. 2001).
possibility to easily create and add new resources to However their approach has some drawbacks,
a distributed system. The main criteria for the including platform dependency and a lack of
architecture design were simplicity, scalability, network transparency. DDX architecture (Corke
dynamics and expandability, and high reusability of 2004) for a distributed system has been developed
system components. Dynamics and expandability using UDP/IP to transfer data from remote sensors
make it possible to add new features and to a central data store. However their support for
functionalities to services even during operation, as only lower-level data structures increase the amount
well as to integrate the results of different research of data transferred during operation and lacks
topics into a larger system. This is essential to be expandability.
able to build robotic systems with multiple Algorithms and remote control code on
capabilities. heterogeneous robotics systems have also been
developed with higher reusability in mind.
CARMEN (Montemerlo, 2003) uses IPC to
communicate between system components and
provides a reusable navigation toolkit for multiple
robotics. URBI (URBI www) scripting language
support several kinds of robots and robot simulators.
URBI also provides client/server-type networking
for remote control of robots, in-language parallel
processing and support for several commercial
mobile robots.
Even though several possible solutions exist
already, they have several disadvantages. Relying on
a single communication protocol or middleware
limits the possible platforms on which the service
Figure 1: Set of resources that might be a part of a can run. As new features are added to the resource
distributed system. (e.g. a robot or sensor), the interface must be
modified, which causes system-wide changes in the
This paper describes the whole architecture that remote software that uses the interface. General
has been partly introduced on several papers middleware also requires a lot from the CPU,
previously. It will describe system-level services that memory, and disc space, and it cannot be run on
improve the creation and management of distributed robots with an embedded CPU.
systems and heterogeneous multi-robot systems.
3 PROPERTY SERVICE
2 RELATED WORK
Property Service architecture provides tools for
Many efforts have been made to create a more building new networked systems in just a few hours.
generally useful interface for networked robots. In It provides standardized data types and service types
many cases, communication is based on commonly that can operate on different kinds of platforms.
used technologies like TCP/IP protocols and Property Service has been successfully used for
middleware like CORBA, Soap, etc. In multi robot remote operation of mobile robots (Tikanmäki,
architectures, several of these are based on CORBA 2003), multi-robot cooperation (Mäenpää et al.
or a real-time CORBA (OMG, 2002) extension. 2004), remote operation of a swarm of robots
Examples of robotic architectures using RT-CORBA (Tikanmäki, 2004), and to create a dynamic interface
227
for a modular robot (Tikanmäki and Vallius 2004). architecture, user interface components can be
This paper presents the complete architecture and thought of as a sensor that senses user inputs and
several system services, such as a storage service, a provides these as properties. Therefore, the user
resource service, and grouping services, and interface can have properties that other services can
explains how they can be used to build more set or get. This feature is used in the listening
complex distributed systems containing robots and mechanism. A client service can request listening of
other devices and system resources. a certain property of the other service. This service
Properties are the features of each service, such registers information concerning the connection of
as the sensors of a robot, or they are functional the client service to the requested property. Each
properties related to the operation of the service. time the value of the property changes in the service,
Functional properties include, for example, the an event is sent to the client service. The main
autonomous features of robots; or the tracking advantage of this is that the client service does not
capabilities of a sensing device. Each property is a need to continuously request value changes, thus
paired name and value and it can be set or requested. reducing the amount of required communication.
The value of each property is transferred as This is especially useful when changes in the
character arrays through a communication channel, property value occur over a long period.
and structured data is represented in XML format.
Each service has a special property called
"properties", which contains a list of the properties
currently available on the service. The value of
“properties” changes during operation as the set of
properties of the service changes, providing dynamic
interface functionality.
Some properties may contain sub-properties.
This hierarchic representation provides the
possibility to request complex representations or, on
the other hand, only special features of a property.
The format is the same as that used in several
programming languages for separating fields of
structures. For example, by requesting the "location" Figure 2: Principle and examples of property service
property from a robot service, the client receives a layers.
6D vector of the current location of the robot. The
client can also request the "location.x" property,
which returns only the x coordinate of the robot's 3.2 Data Types
current location.
A request for certain properties may also contain Property Service has a simple interface, which
some parameters that specify, for example, the Several commonly used data types have been
context of the return value. A good example of this standardized to provide compatibility between
is a request for robot’s location, which might be services. The most commonly used ones are vector,
requested in several coordinate systems. The client list and markers. These data types are briefly
can request possible parameters using the introduced in the following.
“.parameters” extension on property name. A vector is an array of decimal numbers and its
length can vary. For example, a request for a robot's
3.1 Service Interface location returns a 6-dimensional vector containing x,
y, and z translations and a, b, and c rotations along
Property Service has a simple interface, which each axis. A vector is also used to deliver
contains only two methods, "SET" and "GET", for measurements from various sensors, like distance
getting and setting the properties of a service. For sensors. A vector is also used to represent
example, a connected client can GET the velocity of sequences, histograms, etc.
a robot through Property Service or SET the velocity A list is a set of data types. It is used, for
of the robot. The terms client and server are example, to list the properties that a service has. A
misleading, as both the client side and the server list can wrap several kinds of data types, including a
side implement the Property Service interface. The robot's path, which is presented as a list of locations
term 'client service' is used when talking about user (presented as vector types). The shape of an edge of
interfaces or other services that request properties an object can also be presented as a list of vectors
from other services. Figure 2 shows the general containing image coordinates.
principle of the architecture. In the Property Service
228
A marker is a structure that stores information and ICE. To be able to communicate between
about detected and tracked objects in the various protocols, special proxies have been made to
environment. For example, a robot's vision system transfer a call from one media to another. As an
provides markers for objects currently in the field of example, the Property Service in Sony's AIBO robot
view. The marker structure contains location communicates through a TCP/IP socket and a
information and a set of detected features that the Wireless LAN with a PC that contains a wrapper to
object has. For example, a ball recognized from the convert property calls to other protocols, such as
camera view can have recognized features like shape ICE or CORBA calls. This provides the possibility
and color. Markers provided by different services to control AIBO from any property client without
can be collected into a model that represents current knowledge of how the AIBO is actually connected
knowledge about the current environment. This
to the system. The implementation of a proxy is
provides the possibility to easily build sensor
simple, as it simply delivers property calls from one
networks and swarm robotic systems.
Markers are used as input parameters for the protocol to another.
behaviors of various services. In addition to physical
objects they can also represent abstract entities that 3.3 GUI as a Property
can be used to create new behaviors. For example, a
target marker for a "move to" behavior that stays in A service may contain special properties that can be
front of a robot causes the robot to move forward. used to create user interfaces automatically. Several
The measurements of each sensor can be presented of properties may have a ".gui" extension property
as markers, which is highly useful in creating that returns the preferred GUI component for the
distributed sensing systems. For example, a swarm property. The service can also provide a class that
of robots produces a large amount of markers of the contains the compiled code for the GUI component.
objects they detect. These markers are collected into For example, the "movement.gui.java" property
one place to create an environmental model based on returns the java class that can operate property
where the swarm operates. movement on the service. If a new version of the
Table I shows an example of each of the basic GUI component is then developed, it can be received
data types. As it can be seen, for example, the color from the service without a need to change the client
of the marker is represented with a vector type. In side user interface. Instead of requesting the GUI
most cases these data types are used, but each components from each service they can also be
developer can also define their own data structures. requested from a special “GUIPropertyService”,
Even the interface remains the same; it is whose purpose is to provide standard GUI
possible to make dynamic "interfaces" by changing
components for various property types
the set of properties provided by a service. New
properties can be attached to a service during
operation, and they are shown when the client 3.4 Data Transfer Principles
requests "properties" again. This feature is used in
the Qutie robot (Tikanmäki and Vallius 2004), The ideology of Property Service is to always reduce
where the properties of a new attached hardware the need for data delivery if possible. One way to do
component appear on the list of properties as soon as this is by implementing several automatic features
they are available. for services. Instead of continuously sending moving
commands to a robot, we prefer to send only target
Table 1: Examples of commonly used data types. location or several route points. Instead of sending
raw image data, we prefer to request only the targets
Data type Example that vision system can recognize from the view. If
vector (1.0 0.1 0.1) raw images are needed for remote driving of a robot,
list ((1.0 0.0) (2.0 0.1)) a commonly used compression format, such as
marker <marker> JPEG, is used.
<location>(1.0 0.1)</location> Several standard sets of properties have been
</marker> used for different services. As each service delivers
data in the same format, and understands the same
As the value of the property in communication is commands, different services can be used by the
an array of characters, a service can be implemented client service. A good example of this is to use
over any communication protocol containing the different robots with the same user interface.
possibility to transfer data characters. The current
implementation contains RS232, TCP/IP sockets,
HTTP as well as several middleware like CORBA
229
3.5 Platforms 3.7 Standard Services

Currently services are implemented using C, C++, Some commonly used types of services have been
Java, PHP or Python. Services can be compiled and standardized, so that each service in the type
run with Windows and different Unix systems. A category provides at least a certain set of properties.
Property Service for AIBO's operating system Open- The main advantage of this is the possibility to use
R has also been developed. several kinds of services (or system resources)
As one criterion for the architecture was without a need to modify the control program. A
simplicity, services can also be implemented on good example of this is that the user can change the
devices containing low calculation power. In many robot into a different kind of robot to do the same
types of robotics or sensor nodes on sensor task without modifying the control program.
networks, it is reasonable to use embedded
microcontrollers instead of a PC. Property Service 3.7.1 Mobile Robot Service
has also been implemented using standard C and can
be used on many embedded platforms, for example, The main standardized service is a mobile robot
in “Atomi” boards containing an Atmel 8-bit AVR service. A mobile robot service contains properties
microcontroller (Tikanmäki and Vallius 2004) and related to movement, sensing and physical
ARM-based embedded computers running dimensions. Each robot's property service provides
embedded Linux. These two platforms are good the same properties. Table 2 shows a basic set of
examples of systems that cannot run complex properties of a mobile robot service.
middleware.
Table 2: Examples of commonly used data types.
3.6 Implementing New Resources
location 6D global location of the robot,
containing x, y, z and rotations around the
The implementations of Property Service include axis
several functions and classes that facilitate the velocity 6D movement of the robot, including
creation of new services. Automatic features, such as translations and rotations
a dynamic set of properties, are ready for use in the actuators List of actuators on the robot, contains
PS core library. One of the most useful tools is sub-properties for each actuator
called Class Property Service. Class Property sensors List of sensors, with several subproperties
Service is a class that creates a service automatically behaviors List of the robot’s primitive
from given class. The automatic service creation is functionalities. All robots must have at
available for Java and C++ classes. The main least a "move to" behavior.
advantage of this service is that it facilitates the
reuse of previously built classes. For example, Standard properties are the same regardless of
researchers may want to be able to use their old the moving mechanism of the robot. When the robot
manipulator remotely. The manipulator has a control is requested to move forward one meter (by using a
box that can be used from a PC through a serial port. “behaviors.moveto” functional property with a
By using Class Property Service, a previously made
marker as a parameter), a walking Aibo or a wheeled
control class can be used as a part of the distributed
Scout robot moves one meter forward according to
system, as the Class Property Service creates
properties from methods and attributes of the class its moving capabilities. If a robot contains additional
according to defined rules. actuators, such as arms or legs, properties for them
In addition to robots and sensors, Property are added to a list of actuators properties, but the
Service can be used to control various other devices basic functionality remains the same. The main
in the environment. Since the interface is simple and advantage of this is the possibility to control all
can also be used in simple microcontrollers, kinds of mobile robots with the same control
different kinds of devices, like electric locks, motion software or user interface while also providing
sensors or room light control units, can be included special features of each robot.
in the distributed system. This provides the
possibility for a robot to use these resources, which 3.7.2 Sensor Service
increases the possibilities of new applications.
One main type of service is sensor. A sensor service
includes different sensors, such as sonar and
cameras, as well as different controlling devices, e.g.
joysticks, GPS devices and touch pads. User
interfaces are also like sensors, because they sense
230
the operational requests of the human user. The 3.7.4 Grouping Services
basic properties of sensor services are "location",
which contains the location of the sensor in its Another architectural solution is to group several
context, and "data", which returns the raw data services into one Property Service. A good example
captured by the sensor. The default coordinate of this is to group a set of simple miniature robots
system for sensors location is relative to its base, for into one Swarm Property Service (Tikanmäki 2004).
example, the robots origin. By using request Miniature robots with low computational power are
parameters, different coordinate systems can also be commanded through a common radio channel. Each
used. In addition to raw sensor data, various kinds of robot's properties are shown in a hierarchical set of
refined information can be attached to the sensor's properties of the swarm service, shown in Table 3.
properties. The most advanced system is the vision The properties of a single robot can be reached
sensor's property set. through these properties, and each property of the
Vision service is the most advanced sensor mobile robot service interface becomes a sub-
service and a good way to integrate several kinds of property of Group Property Service with a
machine vision methods to a robotic system. Several
“robots.<name>” prefix. The “<name>” parameter
different methods can be activated separately, and changes according the robot currently used. In
the result of image processing can be requested from addition to a single robot's properties, Grouping
the service. Results can be requested in several
Property Service contains various functional
forms. For example, a remote client can request an properties that can simultaneously control a larger
edge-detected image or segments of edges from the set of robots. For example, the client wants a swarm
vision service. The vision service is the interface to
of robots to move to a certain destination. Grouping
one or several cameras, but it can also process Property Service can create a sub set from selected
images sent by other services. As the interface is robots and order each individual to move to the
always the same, different kinds of cameras can be destination. In the case of group of mobile robots, a
used in the same way. All targets detected by the groups of robots are controlled by properties similar
vision sensor can be received in marker format. to those used to control an individual robot. For
example, Swarm Property Service has “behaviors”
3.7.3 Resource Service
properties that control the whole group.
To be able to find the available resources in s
distributed system, a special Resource Property Table 3: Examples of properties of swarm service.
Service has been introduced. The properties of this robots robots currently belonging to this swarm
service are references to services available in the each robot has sub-properties included in
system. New resources can contact it to notify of robot service properties
their presence. When a service contacts the resource location location of the swarm
service, it gives information how it can be contacted, behaviors List of the primitive functionalities.
like its address and protocol(s). Services are then Similarly to single robots, group can be
shown as properties of Resource Property Service controlled with same behaviors like
and each property contains the name of the service "move to" behavior.
and contact information. For example, CORBA-
based services' contact information is the IOR of the Resource Property Service and Group Property
service and TCP/IP or ICE services indicate their IP Service have several similarities, and Resource
address and the port where they can be reached. Property Service can be expanded to act as a
The resource service actively checks the grouping service. In this case, the resource service
existence of services and removes the resources that reroutes the property request. Instead of giving the
are not available any more. Resource Property reference of the service to the client, it acts as a
Service also contains several functional properties proxy and asks for the property value from the
that can be used to search services using parameters. service and delivers the reply to the client. This
For example, a client might want to find all available feature is useful in, for example, a long latency
mobile robots located in a certain room. For the system and a long request time in some cases,
search request, Resource Property Service searches because the grouping service is already connected to
for services that are mobile robots and whose the service, and therefore the client does not need to
"location" property matches the client’s request. The make a direct connection. These services also
resource service also starts up new system services provide full transparency to the system, as the client
when necessary.
231
does not need to know the address or even the and updates the model. Each service might also have
number of robots taking a part in the action. its own model, which is a local representation of the
service’s way of receiving information from the
3.7.5 State Machine Service environment. For example, a ceiling camera might
have an environment model service that provides
In addition to user interface client services, some information received using the camera. Markers are
control services have also been created. One of the used in most communication and data storing, but
main services is State Machine Service. It provides a other methods are also possible. For example, a
resource that can execute RDF-formed state model can contain a grid of temperature values
machines, which can control complex conditional gathered by a swarm of small robots or a sensor
sequences, being able to provide multiple property
network. In this case, the temperature map is
services at the same time. This has been described
updated according to the locations of the
previously in (Mäenpää 2004). State Machine
Service can control a large set of services measurements.
simultaneously. Currently, State Machine Service
also has a visual user interface, where the user can
create and modify state machines graphically and 4 COMPARISON
during operation. By using Storage Service,
introduced below, the state machine or part of it can The overall performance of the system depends on
be stored into a database for further usage or the computational power of the platform and the
execution. speed of the communication channel used. However,
in a comparison of the development times of a new
3.7.6 Storage Service service and cooperation between different
middleware, Property Service provides a great
Another architectural service is Storage Service. It is advantage. The amount of required lines of code is
a combination of file transfer systems and databases. very small compared with, for example, CORBA-
The service can be started for a specified directory in based robot middleware that provides the same
operating systems and all (or only selected) files are features and functionalities of the resource. As the
shown as properties of the service. A remote client capabilities of a robot or other service increase, it
can request a property, named according to the becomes even more useful that the actual interface
filename, and receive the value of the property, does not need to be changed.
which is the content of the file. The same procedure As the code is highly reused, new services are
can be used to store new files and data into a service, fast to implement and all system services like state
by setting a new property in the service. For machines, and storage services are available for use
example, a robot can send captured images to a immediately. For example, the “move to” behavior
storage service, which stores the images in the is common to all robots; no matter if they are legged,
memory (or on a hard disc) of the service. As in wheeled, or even a manipulator. System services
Resource Property Service, Storage Service may also provide great advantages in building
also contain several intelligent search methods, and applications for distributed systems. This provides
the client can request files according to specified even more advantages compared with other systems.
features. Storage Service has been used to store Passing messages as text strings is expensive
captured data such as images, to upload and compared with other formats (like language-specific
download control sequences and state machines, and byte arrays). The performance of communication has
to request GUI components remotely. been improved by sending only essential
information and by using refined data instead of
3.7.7 Environment Model as a Service continuous control commands or raw sensor data.
Several applications and example systems have been
To be able to operate, a distributed system must
created using Property Services. Property Service
have knowledge of the environment where the has been successfully used for remote control of
services, such as robots and sensors, are operating. If several mobile robots that provide audio and video
the system is not designed to be fully reactive, it is
data, and receive several moving commands
reasonable to collect information received by (Tikanmäki 2003) using CORBA middleware and a
services. The system can have one or several wireless LAN. It has also been used to create remote
environment model services. If only one model is
operation for a swarm of simulated robots
used, all the services send their measurements to this (Tikanmäki 2004). Both are good examples of
service, and the model service performs data fusion applications where quick response between services
232
is an essential requirement. Multi-robot cooperation Brooks A., Kaupp T., Makarenko A., Orebäck A. and
and designing of operation using state machines has Williams S. 2005. "Towards Component-Based
been demonstrated in reference (Mäenpää 2004). Robotics". IEEE/RSJ International Conference on
Intelligent Robots and Systems 2005.
Henning M., Spruiell M. 2004 Distributed Programming
with Ice, ZeroC Inc., http://www.zeroc.com
5 CONCLUSION Wang J., Su J., and Xi Y., 2001 “COM-based software
architecture for multisensor fusion system,”
The main advantage of Property Service in Information Fusion, vol. 2, no. 4, pp. 261– 270,.
compared with competitive architectures is the ease Corke P., Sikka P., Roberts J., E. Duff, "DDX: A
Distributed Software Architecture for Robotic
of adding new resources to a distributed robotic
Systems", Australasian Conference on Robotics and
system. Using Class Property Service, classes that
Automation 2004
are already available can be used remotely and Montemerlo M., Roy N., and Thrun S.. Perspectives on
connected to a larger system quickly and without a standardization in mobile robot programming: The
need to implement a code related to communication carnegie mellon navigation (carmen) toolkit. In
between services. As Property Service can be IEEE/RSJ Intl. Workshop on Intelligent Robots and
implemented on various communication channels Systems, 2003.
and platforms, different kinds of new resources can URBI http://www.urbiforge.com/
be attached to the system. The usability of the Tikanmäki A., Röning J., Riekki J. 2003 Remote operation
architecture is not limited to robotics, it can also be of robotics systems using WLAN- and CORBA based
used in other distributed systems, for example, in architecture, SPIE / Intelligent Robots and Computer
home automation systems, sensor networks, and Vision XXI, Oct 27 - 31, Rhode Island, Providence, RI
industrial automation. As these devices become part USA, 2003
of the architecture, they can be operated remotely or Mäenpää T., Tikanmäki A., Riekki J. and Röning J., ,
by the robot's control software, and robots easily 2004 A Distributed Architecture for Executing
became a part of the system. Using Resource Complex Tasks with Multiple Robots, IEEE 2004
ICRA, International Conference on Robotics and
Property Service, the robot can, for example, search
Automation,Apr 26 - May 1, New Orleans, LA, USA
for the light service of a room and switch on the Tikanmäki A., Röning J. 2004 Advanced Remote
light on the room upon entering the room. Complex Operation of swarms of Robots, SPIE / Intelligent
applications built using state machines are easy to Robots and Computer Vision XXII, Philadelphia,
change, and the user can monitor their operation USA
online using state machine visualization. Tikanmäki A., Vallius T., Röning J., 2004 Qutie -
Modular methods for building complex mechatronic
systems, ICMA - International Conference on Machine
Automation, Nov. 24.-26., Osaka, Japan
ACKNOWLEDGEMENTS
This work has been partly funded by Finnish
Academy.
REFERENCES
OMG. Real-Time CORBA Specification. Object
Management Group, Inc., 1.1 edition, August 2002.
Rea 1999. Mobility 1.1, Robot Integration Software,
User’s Guide, iRobot Corporation, MobilityTM
software
Gerkey, B., Vaughan, R. T. & Howard, A. 2003, The
player/stage project: Tools for multi-robot and
distributed sensor systems, in `Proceedings of the 11th
International Conference on Advanced Robotics
(ICAR'03)', Coimbra, Portugal, pp. 317–323.
Utz H., Sablatn¨og S., Enderle S., and Kraetzschmar G.
2002. Miro - middleware for mobile robot
applications. IEEE Transactions on Robotics and
Automation, 18(4), August 2002.
233
ON GENERATING GROUND-TRUTH TIME-LAPSE IMAGE
SEQUENCES AND FLOW FIELDS
Vladimı́r Ulman and Jan Hubený

Centre for Biomedical Image Analysis, Masaryk University, Botanická 68a, Brno 602 00, Czech Republic
Keywords: Optical flow evaluation, ground-truth flow field.
Abstract: The availability of time-lapse image sequencies accompanied with appropriate ground-truth flow fields is cru-
cial for quantitative evaluation of any optical flow computation method. Moreover, since these methods are
often part of automatic object-tracking or motion-detection solutions used mainly in robotics and computer vi-
sion, an artificially generated high-fidelity test data is obviously needed. In this paper, we present a framework
that allows for automatic generation of such image sequences based on real-world model image together with
an artificial flow field. The framework benefits of a two-layered approach in which user-selected foreground
is locally moved and inserted into an artificially generated background. The background is visually similar to
the input real image while the foreground is extracted from it and so its fidelity is guaranteed. The framework
is capable of generating 2D and 3D image sequences of arbitrary length. A brief discussion as well as an
example of application in optical microscopy imaging is presented.
1 INTRODUCTION is obviously the ideal choice. Unfortunately, the real

images do not explicitly provide the ground-truth in-
The growing importance of image processing meth- formation about the motion expressed in the data.
ods is unquestionable in the field of automation and There exist methods that extract such motion in-
robotics. Especially, optical flow computation meth- formation, for example other, than currently tested,
ods are often involved in solutions adopted in, for in- optical flow methods (Horn and Schunck, 1981; Bar-
stance, autonomous systems and agents, vehicle con- ron et al., 1994) or image registration techniques (Zi-
trol applications, surveillance or live-cell microscopy. tová and Flusser, 2003). Unfortunately, these practi-
The outcome of these methods is often not the fi- cally always incur some sort of error or imprecision in
nal product. It is usually further analyzed by object- the flow field. The same holds for manually processed
tracking or motion-detection methods (Cédras and data (Webb et al., 2003), not mentioning the tedious
Shah, 1995; Gerlich et al., 2003; Eils and Athale, extraction of ground-truth motion information.
2003). We decided to automatically generate vast amount
Important is also thorough testing of a particular of artificial test images with the stress on their near-
method before its application. This is even more evi- perfect visual similarity to the real data of the ap-
dent due to the continuous development of image ac- plication in mind. The aim was to confidently test
quisition devices, since the usability of given image the reliability of the given optical flow computation
processing method is changing with the nature of ex- method using this data. Moreover, we wanted to gen-
amined image data. Verification is, therefore, an ob- erate image sequences together with associated flow
vious need. fields reasonably fast (i.e. faster than the execution
For the purpose of fully automatic testing one has of an optical flow computation method) to be able to
to have a large data set together with correct results simulate the behaviour of real-time decision system
prepared. Or, the dataset should be generated online incorporating optical flow computation. Tracking of
reasonably fast. A dataset consisting of real images a live cell in microscopy can be taken as an exam-
234
ple of such a decision-making system. Due to tech- flow field assigns a vector to each voxel in the im-
nology limits the cell can be acquired with only re- age. When generating a sequence, the voxel value
stricted (small) surroundings and needs on-line 2D or is expected to move along its vector into the follow-
3D tracking (moving the stage with the cell based on ing image. The backward transformation works in the
its motion). opposite direction: the preceding image is always cre-
The next section gives a motivation to the adopted ated. Basically, the voxel at vector’s origin is fetched
solution by means of brief overview of possible ap- into an output image from vector’s end in the input
proaches. The third section describes the proposed image. An interpolation in voxel values often occurs
framework which automatically generates 2D and 3D due to real numbers in vectors.
image sequences of arbitrary length. It is followed by A few drawbacks of the backward transformation
the section in which behaviour and sample image data must be taken into account when used. Owing to the
for the case of optical microscopy is presented. A 3D interpolation the transformed image is blurred. The
image is considered as a stack of 2D images in this severity depends on the input flow field as well as in-
paper. terpolation method used. Moreover, the blur becomes
more apparent after a few iterative transformations of
the same image. Thus, the number of transformations
should be as low as possible. Another issue appears
2 MOTIVATION when the flow field is not continuous. In this case, two
(or even more) vectors may end up in the same posi-
Basically, there are just two possible approaches to tion which copies the same voxel into distinct places
obtain image sequences with ground-truth flow fields. in the output image. Unfortunately, non-continuous
One may inspect the real data and manually determine flow field is the case when local movements are to be
the flow field. Despite the bias (Webb et al., 2003) and simulated. Both drawbacks are demonstrated in the
possible errors, this usually leads to a tedious work, example in Fig. 1.
especially, when inspecting 3D image sequences from
a microscope. The other way is to generate sequences A B C
of artificial images from scratch by exploiting some
prior knowledge of a generated scene. This is usually
accomplished by taking 2D snapshots of a changing
3D scene (Galvin et al., 1998; Barron et al., 1994;
Beauchemin and Barron, 1995). The prior knowledge Figure 1: Backward transformation. A) An input image. B)
is encoded in models which control everything from Visualization of the input flow field with two homogeneous
regions. C) A transformed image. Notice the blurred corona
the shape of objects, movements, generation of tex-
as well as the partial copy of the moved object. Images were
tures, noise simulation, etc. (Lehmussola et al., 2005; enhanced to be seen better.
Young, 1996). This may involve a determination of
many parameters as well as proper understanding of
the modeled system. Once the two consecutive im-
ages are created, the information about movement be- 3 THE FRAMEWORK
tween these two can be extracted from the underlying
model and represented in a flow field. In this section we described the framework based
We have adopted the approach in which we rather on two-layered component-by-component backward
modify an existing real sample image in order to gen- transformation. The input to the framework was an
erate an image sequence. This enabled us to avoid real-world sample image, a background mask, a fore-
most of the modeling process as we shall see later. ground mask and a movements mask. The back-
Moreover, we could easily create a flow field we ground mask denoted what, as a whole, should be
were interested in. Consecutive images from the sam- moved in the sample image. The foreground mask de-
ple image could be then transformed by using either noted independent regions (components) in the sam-
backward or forward transformations (Lin and Bar- ple image that were subjects to local movements.
ron, 1994). Both transformations are possible. Never- The movements of components had to remain inside
theless, we observed that forward transformation was the movements mask. The output of the framework
substantially slower. Hence, we described the frame- was a sequence of artificially generated images to-
work based only on backward transformation in this gether with appropriate flow fields. The schema of the
paper. framework is displayed in Fig. 2. The foreground and
The backward transformation moves the content background masks were results of advanced segmen-
of an image with respect to the input flow field. The tation method (Hubený and Matula, 2006) which was
235
A simulation of some global movement of the en-

tire sample image was achieved in the unit global mo-
tion. In this unit, the flow field for the ith frame was
formed for the first time. The foreground and back-
INPUT ground masks as well as movements mask were trans-
formed according to this flow field. There was a zero
RANDOM GLOBAL flow field created when processing the last image of
POOL MOTION the sequence, i.e. the first image created by the frame-
work.
BACKGROUND LOCAL
PREPARATION MOTIONS The generation of background started from prepa-
ration of the sample image. The sample image had to
BACKGROUND FOREGROUND be positioned to fit the background mask. We made
GENERATION PREPARATION use of a special flow field for that purpose. This flow
field was concatenated to the flow created in the pre-
iTH FRAME vious unit and the result was kept until the next se-
quence image is considered. Copy of sample image
was transformed according to this special flow field.
Note that the backward transformation may be used
for concatenation of flow fields if we store flow vec-
tor’s elements in separate images. The concatenated
OUTPUT flow is transformed and added to the flow.
A new similar background was created in two
Figure 2: The schema of the framework. From left to right
in INPUT: sample image, background mask, foreground steps. The foreground components were removed
mask, movements mask; in OUTPUT: examples of 1st, from the transformed copy of sample image and the
10th, 20th and 30th image of a generated sequence, respec- holes were filled in as described in Fig. 3. The result
tively. Images were enhanced to be seen better. was filtered in order to estimate local averages. We
used the 9 × 9 filter of ones which still reflected local
intensity values sensitively, yet the filtered image was
initiated with manually thresholded mask images. smooth. In the second step, the background mask was
The framework was aimed against two obstacles. filled in with randomly chosen values from the pool
Firstly, when foreground component was moved away created in the random pool unit. Finally, correspond-
from its place, the empty region had to be filled ing local average subtracted to the value of the mean µ
in. Therefore, only image regions corresponding to was added to each voxel within the background mask.
foreground components could be directly used. The This ensured the high fidelity of the generated texture.
whole background had to be artificially generated.
Secondly, several transformations of the same image A B
data was not acceptable. In order to generate long se-
quences without increasing blur in generated images,
we developed a concept of generating ith image di-
rectly from the sample image instead of the i + 1th
image. Last but not least, owing to the backward
transformation property the framework generated im-
age sequence from the last to the first image.
The random pool was the initiating step of the pro-
cess. Here, specific voxels from the sample image Figure 3: The filling of removed foreground regions. A)
were collected and stored into a separate image. Vox- For each examined voxel, nearest voxel in each direction
els had to lay inside the background mask and out- outside the black region is found and the distance is deter-
side the foreground mask. The mean intensity value mined. A weighted average of 1/distance-based values is
supplied. B) The result of such filling.
µ over these voxels was computed. The selection was
then even restricted. In the separate image, the pool,
remained only such voxel which intensity value was The foreground mask was first decomposed into
inside the interval (µ − σ, µ + kσ) where σ and k were independent components in the local motions unit.
supplied manually. We set σ = 11 and k = 3/2 to fit Each one is treated separately. A translating motion
the real histogram better. This will probably change vector was randomly chosen from all such vectors that
when different sort of images is generated. keep the component within the movement mask. We
236
also made use of user supplied parameter for maxi- generated images were inspected. The framework
mum allowed length of motion vector which enabled generates every last image in the sequence as a re-
us to control the magnitude of independent local mo- placement for the sample image. Thus, we com-
tions. A temporary flow field was created and uni- puted correlation coefficient (Corr.), average abso-
formly filled with this vector. The mask of this com- lute difference (Avg. diff.) and root mean squared
ponent only and a copy of the ith flow field were trans- (RMS) differences. The results are summarized in
formed according to this uniform flow. This moved Table 1. The generator achieved minimal value of
the component mask and prepared the concatenation 0.98 for correlation. This quantitatively supports our
of the corresponding region of the flow field. The observations that generated images are very close to
concatenation was finished by pasting this region fol- their originals. The suggested framework also guaran-
lowed by addition of chosen flow vector to each vec- tees exactly one transformation of the sample image,
tor inside the region into the ith flow. Note that more hence the quality of the foreground texture is best pos-
complex foreground movement may be used by sub- sible thorough the sequence. Refer to Fig. 5 for ex-
stituting any smooth flow field for the uniform one ample of 3 images of a 50 images long sequence. A
as well as corresponding vectors should be added in- decent improvement was also observed when an arti-
stead of constantly adding the chosen one. After all, ficial background of 3D image was formed in a slice-
new foreground mask was created by merging all sin- by-slice manner, see rows C and D in Table 1. In the
gle locally moved masks. case of row D, a separate random pools and mean val-
In the foreground preparation unit, similarly to ues were used for each slice of the 3D image.
the background preparation, another special flow field Inappropriately created foreground mask may em-
was used. It was again concatenated to the current phasize the borders of extracted foreground when
ith flow and the result was stored for the next frame- inserted into artificial background. The weighted
work’s iteration. Another copy of sample image was foreground insertion was observed to give visually
transformed according to this another special flow better results. Table 1 quantitatively supports our
field to position the foreground texture. claim: merging the foreground components accord-
In the ith frame unit, the foreground regions were ing to twice dilated foreground mask was comparable
extracted from the transformed copy of sample im- to the plain overlaying of foreground components ac-
ages. The extraction was driven by the new fore- cording to non-modified masks.
ground mask which was dilated (extended) only for The use of user-supplied movements mask pre-
that purpose beforehand. Finally, the ith image was vented the foreground components from moving into
finished by weighted insertion (for details refer to (Ul- regions where there were not supposed to appear, e.g.
man, 2005)) of the extracted foreground into the artifi- outside the cell. The masks are simple to create, for
cially generated background. The weights were com- example by extending the foreground mask into de-
puted by thresholding the distance transformed (we manded directions. The generated sequences then be-
used (Saito and Toriwaki, 1994)) foreground mask. came even more real. Anyway, randomness of com-
An illustration of the whole process is shown in Fig. 4. ponents’ movements prohibited their movements con-
sistency. Pre-programming the movements would en-
able the consistency. Clearly, the movement mask
wouldn’t be necessary in this case.
4 RESULTS
We implemented and tested the presented frame-
work in C++ and in two versions. The first version 5 CONCLUSION
created only image pairs while the second version cre-
ated arbitrarily long image sequences. It was imple- We have described the framework for generating
mented with both backward and forward transforma- time-lapse pseudo-real images together with unbiased
tions. We observed that for 2D images the forward flow fields. The aim was to automatically generate a
variant was up to two orders of magnitude slower than large dataset in order to automatically evaluate meth-
the backward variant. Therefore, the second version ods for optical flow computation. However, one may
was implemented based only on backward transfor- discard the generated flow fields and use just the im-
mation. The program required less then 5 minutes on age sequence.
Pentium4 2.6GHz for computation of a sequence of The framework allows for the synthesis of 2D and
50 images with 10 independent foreground regions. 3D image sequences of arbitrary length. By suppling
The generator was tested on several different 2D real-world sample image and carefully created masks
real-world images and one real-world 3D image. All for foreground and background, we could force im-
237
Figure 4: Example of image formation. A) A sample image. B) Its intensity histogram and thresholded image with thresholds
set as shown in the histogram. C) The background filled with randomly chosen values. D) The sample image with foreground
regions filled in. E) The same image after the averaging filter. F) The weights used together with the extended foreground
mask, brighter intensity shows higher weight. G) The artificial image (the last image in the sequence). H) A map of intensity
differences between A) and G), maximum brightness is at value of 30. Note that the highest errors were due to erroneous
segmentation of the background. All images were enhanced for the purpose of displaying.
Figure 5: Example of 3 frames from image sequence. A) The first frame (the last generated). B) The middle frame. C) The
last frame (the first generated). All images were enhanced for the purpose of displaying.
ages in the sequence to look more realistic. We made ground segmentation. We also made use of local
use of rotation and translation transformations for movements mask which gave us ultimate control over
global motion (of the entire cell) and only translations the independent foreground movements.
for independent local movements of foreground com-
ponents (selected intracellular structures). The trans- We believe that the framework is applicable to
formations used can be arbitrary, owing to the formal- other fields as well. In some circumstances, image
ism of the flow field, provided they are continuous be- processing subroutines may differ as well as different
cause of limitation of both transformation methods. foreground movements may be desired. The require-
Seamless overlaying of the foreground was achieved ment is that images should be separable into just two
by the weighted insertion of foreground which im- layers and that the background should be reasonably
proved the robustness to any imprecision in the fore- easy to generate. For instance, in the vehicle control
applications one may meet the requirement: observ-
238
Table 1: Similarity comparison from several aspects. The column “Ext.” shows the number of dilations performed on the input
foreground mask beforehand. The upper indices denote whether the foreground regions were simply overlaid1 or merged2
into the background. A) and B) Comparisons over 2D images. C) Comparison over a 3D image. D) Comparison over the
same 3D image, separate pools of voxel intensities were used for each slice during the formation of the artificial background.
Ext. Corr.1 Avg. diff.1 RMS1 Corr.2 Avg. diff.2 RMS2
A 0 0.989 3.87 5.13 0.989 3.87 5.12
1 0.989 3.80 5.03 0.989 3.85 5.05
2 0.989 3.73 4.94 0.989 3.82 5.00
3 0.989 3.68 4.90 0.989 3.83 4.98
B 0 0.992 2.76 3.83 0.992 2.77 3.85
1 0.992 2.62 3.69 0.992 2.74 3.75
2 0.993 2.41 3.46 0.992 2.62 3.58
3 0.993 2.33 3.40 0.992 2.64 3.57
C 0 0.980 3.67 4.79 0.980 3.67 4.79
1 0.980 3.73 4.89 0.980 3.81 4.92
2 0.981 3.53 4.69 0.981 3.70 4.77
3 0.981 3.42 4.59 0.981 3.66 4.72
D 0 0.982 3.15 4.16 0.982 3.16 4.17
1 0.983 3.07 4.08 0.982 3.13 4.11
2 0.983 3.00 4.03 0.983 3.11 4.08
3 0.984 2.92 3.96 0.983 3.10 4.05
ing an image of a car on the road can be split into the Gerlich, D., Mattes, J., and Eils, R. (2003). Quantitative
car foreground and rather uniform road background. motion analysis and visualization of cellular struc-
tures. Methods, 29(1):3–13.
Horn, B. K. P. and Schunck, B. G. (1981). Determining
optical flow. Artificial Intelligence, 17:185–203.
ACKNOWLEDGEMENTS Hubený, J. and Matula, P. (2006). Fast and robust segmen-
tation of low contrast biomedical images. In In Pro-
ceedings of the Sixth IASTED International Confer-
The presented work has been supported by the Min- ence VIIP, page 8.
istry of Education of the Czech Republic (Grants No.
Lehmussola, A., Selinummi, J., Ruusuvuori, P., Niemisto,
MSM0021622419, LC535 and 2B06052). A., and Yli-Harja, O. (2005). Simulating fluorescent
microscope images of cell populations. In IEEE En-
gineering in Medicine and Biology 27th Annual Con-
ference, pages 3153–3156.
REFERENCES Lin, T. and Barron, J. (1994). Image reconstruction error
for optical flow. In Vision Interface, pages 73–80.
Barron, J. L., Fleet, D. J., and Beauchemin, S. S. (1994). Saito, T. and Toriwaki, J. I. (1994). New algorithms for Eu-
Performance of optical flow techniques. Int. J. Com- clidean distance transformations of an n-dimensional
put. Vision, 12(1):43–77. digitized picture with applications. Pattern Recogni-
Beauchemin, S. S. and Barron, J. L. (1995). The computation, 27:1551–1565.
tion of optical flow. ACM Comput. Surv., 27(3):433– Ulman, V. (2005). Mosaicking of high-resolution biomed-
466. ical images acquired from wide-field optical micro-
scope. In EMBEC’05: Proceedings of the 3rd Euro-
Cédras, C. and Shah, M. A. (1995). Motion based recog- pean Medical & Biological Engineering Conference,
nition: A survey. Image and Vision Computing, volume 11.
13(2):129–155.
Webb, D., Hamilton, M. A., Harkin, G. J., Lawrence, S.,
Eils, R. and Athale, C. (2003). Computational imaging in Camper, A. K., and Lewandowski, Z. (2003). Assess-
cell biology. The Journal of Cell Biology, 161:447– ing technician effects when extracting quantities from
481. microscope images. Journal of Microbiological Meth-
ods, 53(1):97–106.
Galvin, B., McCane, B., Novins, K., Mason, D., and Mills,
S. (1998). Recovering motion fields: An evaluation Young, I. (1996). Quantitative microscopy. IEEE Engineer-
of eight optical flow algorithms. In In Proc. of the 9th ing in Medicine and Biology Magazine, 15(1):59–66.
British Mach. Vis. Conf. (BMVC ’98), volume 1, pages Zitová, B. and Flusser, J. (2003). Image registration meth-
195–204. ods: a survey. IVC, 21(11):977–1000.
239
HELPING INSTEAD OF REPLACING
Towards A Shared Task Allocation Architecture
Foad Ghaderi and Majid Nili Ahmadabadi

Control and Intelligent Processing Center of Excellence, Mobile Robot Lab
Dept. of Elect. and Comp. Eng., Faculty of Eng., University of Tehran
Keywords: Cooperative robotics, shared task allocation, fault tolerance.
Abstract: Some failures cause the robots to loss parts of their capabilities, so that they cannot perform their assigned
tasks. Considering requirements of typical robotic teams during different missions, a distributed behavior
based control architecture is introduced in this paper. This architecture is based on an enhanced version of
ALLIANCE, and provides the robots the ability of performing shared tasks based on help requests. The
architecture contains a mechanism for adaptive action selection and a communication protocol for
information and task sharing which are required for coordination of team members. The proposed
architecture is used in a box pushing mission where heterogeneous robots push several boxes with different
masses.
1 INTRODUCTION based on Subsumption architecture (Brooks, 1986).

It supports adaptive action selection for faulty robot
Sensitivity of distributed robotic systems to changes replacement (Parker, 1998), (Parker, 1994).
in working environments and common failures in Kasahara has considered reorganization in an
their mechanical and electrical components is a organizational learning model (Kasahara, 1998).
barrier to their wide physical implementation Whenever a fault occurs, agents try to compose new
(Ahmadabadi, 2001). So a dynamic task allocation organizations using learning techniques. This
mechanism that supports fault tolerance behavior is process can lead the team to its goal.
a mandatory requirement for cooperative robotics In this paper, a new task allocation method is
solutions (Ghaderi, 2002). introduced which is based on ALLIANCE and
Mataric et al. showed empirically that there is no supports help in a team of cooperative robots. In this
optimal task allocation strategy for all domains, and method partially faulty robots do not leave the team,
even it is difficult to identify the optimal task but the team tries to redistribute task among other
allocation strategy for a particular task (Mataric, members in order to use the whole capabilities of the
2003). In their framework (Gerkey, 2003) only one robots. The suggested architecture supports adaptive
robot could be assigned to a task, and no action selection with help request processing and
redundancies were allowed. allows the group to perform its mission coordinated.
In (Ahmadabadi, 2004) two distributed and In the next section importance of help in a
cooperative methods for load reallocation among cooperative team of robots is discussed. The help
some object lifting robots without requiring them to supporting method is introduced in section 3. Box
change their grasp positions are introduced. pushing problem is reported in section 4.
Vig et al. provided RACHNA (Vig, 2005) which Conclusions are presented in section 5.
is a market-based architecture for allocating multi-
robot tasks based on individual robot capabilities. In
this architecture, if a robot can not support the team, 2 HELP IMPORTANCE IN
it is more likely that another robot with similar COOPERATIVE MISSIONS
capabilities replaces this robot.
Some advanced behavior based control Most of the real world mobile robots applications
architectures are introduced that allow adaptive task are performed in dynamic environments. Missions of
allocation. Parker’s architecture (ALLIANCE) is
240
HELPING INSTEAD OF REPLACING - Towards A Shared Task Allocation Architecture
rescue robots, cleaning robots, robots used for replacement would be possible if there is another
defense purposes and every other mission that robots robot with at least the same capabilities.
are used instead of human in order to decrease It is clear that the traditional way of replacing
dangers are examples of these applications. In these robots will not have considerable effect in
applications a lot of changes may occur in performance of the team. In this case, the best way
environment by time, and sometimes these changes to achieve the group’s goal is helping the weak
cause a team to fail in some (or all) of its tasks. If robots.
the task selection mechanism is static and without
flexibility, then there is no way to complete the
tasks, except waiting for some robots to complete 3 THE HELP SUPPORTING
their tasks and replacing them with those robots that
are not able to continue. Here we refer to an action METHOD
selection mechanism as static when robots do not
change their tasks even if there are some tasks in the There are two important issues to be considered in
team with higher priorities. any architecture that supports help for faulty agents.
In this case if there is redundancy in the team, Let’s review a scenario first. Assume that some
higher priority tasks are assigned to idle robots, robots are cooperating to perform a mission. During
otherwise these tasks would be assigned after a time their action, one of them senses that it is not possible
interval that is unknown. Obviously such a task to complete its task lonely and so broadcasts a help
assignment may cause a disaster for the group or request. Receiving this message, other robots must
environment if the critical tasks are not assigned and decide about helping disabled robot. After making
performed in an acceptable time. We define that a decision, the helping robots must cooperate to
task in a cooperative mission is unassigned, if no complete the disabled robot’s task. During lots of
robots have selected it ever, or if the selecting cooperative help tasks a closed coordination
robot(s) is (are) not able to perform the task. between robots is required. So the help supporting
Disability of a robot in performing a task may architecture must include appropriate mechanisms to
have two reasons. First, the robot is faulty and hence do action selection and closed coordination for a
it can not complete the task, and second the robot is cooperative task.
not faulty but its capabilities are not enough to
complete the task. 3.1 Action Selection
Sometimes replacing the disabled robot with a
new one provides a new chance to the team to The way in which robots process the help request
achieve its goal. If there is redundancy in quantity of and make decision has great effects on functionality
the robots, problem is solved easily, and otherwise and performance of the team. In addition, there are
one of the robots must ignore its task and perform many factors to be considered when processing a
the uncompleted task. But there are some situations help request. Some of them are listed below:
that this method does not acquire mission’s goals. - Distance to the disabled robot (in general the cost
We divide these missions into two categories: to reach to the position in which the helper robot
None of the team members can finish the can help the disabled robot),
uncompleted task alone. In the other words, - Cost of performing the task,
performing the task requires efforts of more than one - Having useful mechanical capabilities,
of the existing robots and the task is not divisible to knowledge and experience,
simpler subtasks. This kind of tasks is called “shared - Priority of robot’s current task compared with
task” which requires closed coordination and real the shared task,
time action of a team of robots. Transferring of an - Criticality of the disabled robot’s task,
injured person in a rescue mission is a good example - Progress of the task of the team members
of shared tasks. In this mission, none of the robots (including the faulty robot).
can complete the task alone and since some These are general parameters that can affect
member’s actions may disturb efforts of the others, a performance of a help process. Besides these, other
close coordination between robots is mandatory. parameters might be chosen according to specific
nature of the mission.
Performing the task depends on some
We used ALLIANCE architecture (Parker, 1998)
robot’s capabilities. These capabilities may include
as a base for adaptive action selection. This behavior
some special mechanical mechanisms, processing
based control architecture has adaptive action
power, knowledge and etc. Therefore, the
selection capabilities that can be used in different
241
missions, but this architecture does not support help. behavior in order to increase team's efficiency or
So we enhanced ALLIANCE to address the help other robots in critical tasks.
mentioned requirements. If priority of task j is higher than active behavior
Considering motivation function and it’s in robot i, this parameter is equal to 1, otherwise it's
parameters in ALLIANCE architecture, it’s apparent 0. In Parker's architecture (Parker, 1994), (Parker,
that if a robot selects a task, (except in some special 1998), the criticality of the tasks is not taken into
conditions), it will not ignore it till the task is account in evaluating this parameter. So if any
finished. So while a robot is performing a task and behavior is active, others would be suppressed,
receives a help request it is not possible to change its regardless of the criticality of the others.
behavior and select a new task. In this case the help Impatience_Resetijk(t): The default value of this
request is not answered if there is no idle robot in the parameter is 1. When a robot selects behavior j to
team. help robot k, it broadcasts its decision. As a result,
We supposed that each task is allowed to be Impatience_Reset in the other robots becomes zero
shared just between two robots. In order to achieve for a defined period of time, and then increases to 1
adaptive action selection, some changes were again, and the robots motivation to participate in
applied to ALLIANCE. In fact, we designed a two task j resets consequently.
dimensional motivation vector for each robot. Two Help_requestijk(t): Default value of this parameter
axes of this vector represent team members and is zero and takes a positive value for a predefined
tasks of the team. So mijk in robot i is motivation interval, whenever robot i receives a help request
function corresponding to behavior j, while robot k from robot k requesting to cooperate in task j. If in
broadcasts a help request. The new motivation this period the value of impatience_reset is not zero,
function for the shared task j in robot i is defined by the motivation value will be multiplied by a value
the following equation: bigger than one. (Notice that faulty robot broadcasts
m ijk (0 ) = 0 help request periodically until its task is finished.)
m ijk (t ) = [m ijk (t − 1 ) + impatience ijk (t )] Acquiesenceij(t): The same as that in ALLIANCE
× sensory_fe edback ijk (t ) architecture.
× activity_ sup pression ij (t ) (1) Initial values of the parameters and their changes
× impatience _reset ijk (t )
deeply depend on the general requirements of the
(
× 1 + Help_ Re quest ijk (t ) ) mission, robots type and different performance
× acquiescen ce ij (t )
indexes used. Therefore, the initial values must be
The parameters are defined below: optimized for each task.
Impatienceijk(t): This parameter shows robot's
impatience for interfering other robot's tasks. If task 3.2 Robots Coordination in Help
j is not selected by any other robot, Impatience will
Process
have a large value; otherwise its quantity depends on
the type of task, environmental conditions, and After deciding to help, robots must coordinate to
progress of the task. In shared task, some more perform the shared task. The coordination strategy
factors such as the criticality of the task, the depends on the special characteristics of the task and
efficiency of the robot in performing the task and there is no general method to do this. But, the
type of the help request are important. coordination methods are generally based on the
Sensory_feedbackijk(t): This parameter indicates information about activities of the other robots.
whether behavior j is required at the time to reach Then, we used the communication method used in
the goals of the team or not. Often, physical robots (Asama, 1992) to obtain information about others
sensors produce this information, but in practical tasks, activities, and their progress in order to
robotics applications it's possible to use virtual coordinate the robots.
sensors such as memory (as a flag), and In Asama’s protocol, nine procedures are defined
communication lines to indicate this parameter. If to communicate information (Asama, 1992). We
robot k needs some help to complete task j and robot selected seven of them to coordinate the team:
i can cooperate in this task, sensory_feedback is one, - Report for ready state,
otherwise it is equal to zero. - Declaration of task selection,
Activity_Suppressionij(t): When a behavior is - Request for Cooperative task,
active in a robot, this parameter suppresses - Acceptance of Cooperative task,
behaviors with lower priorities to be activated. This - Report of completion of preparation for cooperation,
suppression is not applied to higher priority tasks - Command for start of cooperative task,
and so permits the robot to change its active - Report of task completion.
242
HELPING INSTEAD OF REPLACING - Towards A Shared Task Allocation Architecture
4 EVALUATING THE ACKNOWLEDGEMENTS

ARCHITECTURE
This work is partially supported by Control and
Task assignment and adaptive behavior of a Intelligent Processing Center of Excellence,
cooperative team that uses help mechanism is University of Tehran.
evaluated in a box pushing problem. Box pushing is
a common test bed in the field of cooperative
robotics. It is assumed that some boxes are REFERENCES
distributed in an environment and some robots must
push them to the front wall. Ahmadabadi, M. N. and Nakano E., “A Constrain and
There are two kinds of boxes. Some of them are Move approach to distributed object manipulation”,
IEEE Trans. on Robotics and Automation, Vol. 17,
light and can be moved by a single robot. Others are No. 2, pp. 157-172 , April 2001.
heavy such that one robot is not able to transfer them Ghaderi, F., Ahmadabadi, M. N. “Distributed Cooperative
alone. Each robot selects a box to transfer while it Fault Tolerance In A Team Of Object Lifting Robots”,
has no information about weight of the box. In IEEE Proc. of 1996 Int. Conf. on Intelligent
Whenever the robot detects that the selected box is Systems and Robots, 2002, pp. 2721-2727.
heavy and it’s not possible to move it alone, it will Mataric, M. J., Sukhatme, G. S., Østergaard, E. H.,
broadcast a help request. “Multi-Robot Task Allocation in Uncertain
Robots have some inexact information about the Environments”, Autonomous Robots 14, 255–263,
position of the boxes, so they must search for them. 2003, Kluwer Academic Publishers.
Gerkey, B. P., Mataric, M. J., “A Framework for studying
At the beginning, the robots assume that all of the multi robot task allocation”, In Multi-Robot Systems:
boxes are light. So after selecting a box, the robot From Swarms to Intelligent Automata, Volume II,
goes towards it and tries to move it. If the box is Pages 15-26, the Netherlands, 2003, Kluwer Academic
heavy, the robot broadcasts a help request, and waits Publishers.
for other’s responses. Ahmadabadi, M. N., Ghaderi, F. “Distributed cooperative
Experiments show that the team can manage load redistribution for fault tolerance in a team of four
existing resources to complete the mission in cases object-lifting robots”, Advanced Robotics, Vol. 18,
that some robots are not able to perform their No. 1, pp. 61–81, 2004.
assigned task. Vig, L., Adams, J. A., “A Framework for Multi-Robot
Coalition Formation” In Proc. of the 2nd Indian
International Conference on Artificial Intelligence,
2005, India.
5 SUMMARY Brooks, R. A., "A Robust Layered Control System for a
Mobile Robot", IEEE Journal of Robotics and
Automation, Vol.2, No.1, March 1986 pp. 14-23.
In this paper, we have introduced a help supporting
Parker, L. E., “ALLIANCE: An Architecture for Fault
architecture that focuses on task allocation in cases tolerant Multi-robot Cooperation”, IEEE Trans.
that some of the team members are not able to robotics and automation vol. 14, No. 2, pp. 220-240,
complete their tasks. This architecture supports fault April 1998.
tolerance in cooperative missions that have various Parker, L. E., “Heterogeneous Multi-Robot Cooperation”,
tasks with different criticalities. In this method, the Ph.D. Thesis, Massachusetts Institute of Technology,
team tries to redistribute the tasks among members Cambridge, MA, Feb.1994.
by processing help requests from disabled robots in Kasahara, H., Takadama, K. , Nakasuka, S., Shimohara,
order to use all robots capabilities. The suggested K.,“Fault Tolerance in a Multiple Robots Organization
Based on an Organizaional Learning Model” The
architecture supports adaptive action selection and
IEEE 1998 International Conference On Systems, Man
let’s the group to perform its mission in cooperation. and Cybernetics (SMC'98), pp. 2261-2266, 1998.
In our method the robots are committed unless some Asama H., Ozaki K., Matsumoto A., Ishida Y., and Endo
critical tasks are not assigned and they are I. , “Development of task assignment system using
individualistic unless some robots require help. The communication for multiple autonomous robots”,
architecture is evaluated in a box pushing mission Journal of Robotics and Mechatronics, pp. 122-127,
and results show acceptable performance. 1992.
243
Viorel Stoian and Cristina Pana

University of Craiova, Mechatronics Department, Decebal Street No. 107, Craiova, Romania
Keywords: Fuzzy logic algorithm, mobile robots, obstacole avoidance, trajectory controller.
Abstract: This paper presents a fuzzy control algorithm for mobile robots which are moving next to the obstacle
boundaries, avoiding the collisions with them. Four motion cycles (programs) depending on the proximity
levels and followed by the mobile robot on the trajectory (P1, P2, P3, and P4) are shown. The directions of
the movements corresponding to every cycle, for every reached proximity level are presented. The sequence
of the programs depending on the reached proximity levels is indicated. The motion control algorithm is
presented by a flowchart showing the evolution of the functional cycles (programs). The fuzzy rules for
evolution (transition) of the programs and for the motion on X-axis and Y-axis respectively are described.
Finally, some simulations are presented.
1 INTRODUCTION
Fuzzy set theory, originally developed by Lotfi
Zadeh in the 1960’s, has become a popular tool for
control applications in recent years (Zadeh, 1965).
Fuzzy control has been used extensively in
applications such as servomotor and process control.
One of its main benefits is that it can incorporate a
human being’s expert knowledge about how to
control a system, without that a person need to have
a mathematical description of the problem.
Many robots in the literature have used fuzzy
logic (Song, 1992, Khatib, 1986, Yan, Ryan, Power,
1989 …). Computer simulations by Ishikawa feature a) The proximity levels.
a mobile robot that navigates using a planned path
and fuzzy logic. Fuzzy logic is used to keep the
robot on the path, except when the danger of
collision arises. In this case, a fuzzy controller for
obstacle avoidance takes over.
Konolige, et al. use fuzzy control in conjunction
with modeling and planning techniques to provide
reactive guidance of their robot. Sonar is used by
robot to construct a cellular map of its environment.
Sugeno developed a fuzzy control system for a
model car capable of driving inside a fenced-in
track. Ultrasonic sensors mounted on a pivoting
b) The two degrees of freedom of the locomotion system
frame measured the car’s orientation and distance to
of the mobile robot.
the fences. Fuzzy rules were used to guide the car
parallel to the fence and turn corners (Sugeno et al., Figure 1: The proximity levels and the degrees of freedom
1989). of the robot motion.
244
2 CONTROL ALGORITHM robot on the trajectory (P1, P2, P3, and P4). Inside
every cycle are presented the directions of the
The mobile robot is equipped with a sensorial movements (with arrows) for every reached
system to measure the distance between the robot proximity level. For example, if the mobile robot is
and object that permits to detect 5 proximity levels moving inside first motion cycle (cycle 1 or program
(PL): PL1, PL2, PL3, PL4, and PL5. Figure 1a P1) and is reached PL3, the direction is on Y-axis
presents the obstacle (object) boundary and the five (sense plus) (see Figure 1b, too). In Figure 2b we
proximity levels and Figure 1b presents the two can see the sequence of the programs.
degrees of freedom of the locomotion system of the One program is changed when are reached the
mobile robot. This can move either on the two proximity levels PL1 or PL5. If PL5 is reached the
rectangular directions or on the diagonals (if the two order of changing is: P1ÆP2ÆP3ÆP4Æ P1Æ
degrees of freedom work instantaneous). If PL1 is reached the sequence of changing
becomes: P4ÆP3ÆP2ÆP1Æ P4Æ
The motion control algorithm is presented in
Figure 3 by a flowchart of the evolution of the
functional cycles (programs). We can see that if
inside a program the proximity levels PL2, PL3 or
PL4 are reached, the program is not changed. If PL1
or PL5 proximity levels are reached, the program is
changed. The flowchart is built on the base of the
rules presented in Figure 2.1 and Figure 2.2.
2.1) Motion cycles (programs)
2.2) The sequence of the programs
Figure 2: The sequences of the motion.
The goal of the proposed control algorithm is to

move the robot near the object boundary with Figure 3: The flowchart of the evolution of the functional
collision avoidance. Figure 2a shows four motion cycles (programs).
cycles (programs) which are followed by the mobile
245
Figure 4: The inputs and outputs of the fuzzy algorithm.
3 FUZZY ALGORITHM outputs (movement on X and Y-axes and the

program on k+1 step) of the fuzzy algorithm.
For the linguistic variable “distance proximity
level” we establish to follow five linguistic terms:
“VS-very small”, “S-small”, “M-medium”, “B-big”,
and “VB-very big”. Figure 5a shows the
membership functions of the proximity levels
(distance) measured with the sensors and Figure 5b
shows the membership functions of the angle (the
a) Membership functions of the proximity levels (distance) programs). If the object is like a circle every
measured with the sensors program is proper for a quarter of the circle.
Figure 5c and Figure 5d present the membership
functions of the X, respectively Y commands
(linguistic variables). The linguistic terms are: NX-
negative X, ZX-zero X, PX-positive X, and NY, ZY,
PY respectively.
Table 1: Fuzzy rules for evolution of the programs.

VS S M B VB
b) Membership functions of the angle (the programs) P1 P4 P1 P1 P1 P2
P2 P1 P2 P2 P2 P3
P3 P2 P3 P3 P3 P4
P4 P3 P4 P4 P4 P1
Table 2: Fuzzy rules for the motion on X-axis.
c) Membership functions of the X comands VS S M B VB

P1 PX PX ZX NX NX
P2 ZX NX NX NX ZX
P3 NX NX ZX PX PX
P4 ZX PX PX PX ZX
Table 3: Fuzzy rules for the motion on Y-axis.

d) Membership functions of the Y comands
VS S M B VB
Figure 5: Membership functions of the I/O variables. P1 ZY PY PY PY ZY
P2 PY PY ZY NY NY
The fuzzy controller for the mobile robots based P3 ZY NY NY NY ZY
on the algorithm presented above is simple. Most P4 NY NY ZY PY PY
fuzzy control applications, such as servo controllers,
feature only two or three inputs to the rule base. This Table 1 describes the fuzzy rules for evolution
makes the control surface simple enough for the (transition) of the programs and Table 2 and Table 3
programmer to define explicitly with the fuzzy rules. describe the fuzzy rules for the motion on X-axis
The above robot example uses this principle, in and Y-axis, respectively. Table 1 implements the
order to explore the feasibility of using fuzzy control sequence of the programs (see Figure 2.2 and Figure
for its tasks. Figure 4 presents the inputs (distance- 3) and Table 2 and Table 3 implement the motion
proximity levels and the program on k step) and the cycles (see Figure 2.1 and Figure 3).
246
5 CONCLUSIONS
This paper presents a fuzzy control algorithm for
mobile robots which are moving next to the obstacle
boundaries, avoiding the collisions with them. Four
motion cycles (programs) depending on the
proximity levels and followed by the mobile robot
on the trajectory (P1, P2, P3, and P4) are shown.
The directions of the movements corresponding to
every cycle, for every reached proximity level are
presented. The sequence of the programs depending
on the reached proximity levels is indicated. The
motion control algorithm is presented by a flowchart
showing the evolution of the functional cycles
(programs). The fuzzy rules for evolution
(transition) of the programs and for the motion on X-
axis and Y-axis respectively are described. The
Figure 6: The trajectory of the mobile robot around a
circular obstacle. fuzzy controller for the mobile robots based on the
algorithm presented above is simple. Finally, some
simulations are presented. If the object is like a
circle, every program is proper for a quarter of the
circle.
REFERENCES
Zadeh, L. A., 1965. Fuzzy Sets, Information and Control,
No 8, pp. 338-353.
Sugeno, M., Murofushi, T., Mori, T., Tatematasu, T., and
Tanaka, J., 1989. Fuzzy Algorithmic Control of a
Model Car by Oral Instructions, Fuzzy Sets and
Systems, No. 32, pp. 207-219.
Song, K.Y. and Tai, J. C., 1992. Fuzzy Navigation of a
Mobile Robot, Proceedings of the 1992 IEEE/RSJ
Intern. Conference on Intelligent Robots and Systems,
Raleigh, North Carolina.
Khatib, O., 1986. Real-Time Obstacle Avoidance for
Figure 7: The trajectory of the mobile robot around a Manipulators and Mobile Robots, International
irregular obstacle. Journal of Robotics Research, Vol. 5, No.1, pp. 90-98.
Boreinstein, J. and Koren, Y., 1989. Real-time Obstacle
Avoidance for Fast Mobile Robots, IEEE Transactions
on Systems, Man., and Cybernetics, Vol. 19, No. 5,
4 SIMULATIONS Sept/Oct. pp. 1179-1187.
Jamshidi, M., Vadiee, N. and Ross, T. J., 1993. Fuzzy
In the simulations can be seen the mobile robot Logic and Control. Software and Hardware
trajectory around an obstacle (object) with circular Applications, PTR, Prentice Hall, New Jersey, USA.
boundaries (Figure 6) and around an obstacle Yan, J., Ryan, M., and Power, J., 1994. Using Fuzzy
Logic. Towards intelligent systems, Prentice Hall, New
(object) with irregular boundaries (Figure 7). One
York.
program is changed when are reached the proximity
levels PL1 or PL5. If PL5 is reached the order of
changing becomes as follows: P1ÆP2ÆP3ÆP4Æ...
If PL1 is reached the order of changing is becomes
follows: P4ÆP3ÆP2ÆP1Æ P4Æ ……
247
Youngjin Choi
School of electrical engineering and computer science, Hanyang University, Ansan, 426-791, Republic of Korea
[email protected]
Doik Kim
Intelligent Robotics Research Center, Korea Institute of Science and Technology (KIST), Seoul, 136-791, Republic of Korea
[email protected]
Keywords: WBC (whole body coordination), Humanoid robot, Posture control, CoM (center of mass) Jacobian.
Abstract: This paper proposes the kinematic resolution method of CoM(center of mass) Jacobian with embedded mo-
tions and the design method of posture/walking controller for humanoid robots. The kinematic resolution of
CoM Jacobian with embedded motions makes a humanoid robot balanced automatically during movement of
all other limbs. Actually, it offers an ability of WBC(whole body coordination) to humanoid robot. Also, we
prove that the proposed posture/walking controller brings the ISS(disturbance input-to-state stability) for the
simplified bipedal walking robot model.
1 INTRODUCTION trated point mass m can be obtained as the simplified

model for bipedal robot as shown in Fig. 1. The mo-
Recently, there have been many researches about hu- tion of the rolling sphere on a massless plate is de-
manoid motion control, for example, walking con- scribed by the position of CoM, c = [cx , cy , cz ]T , and
trol(Choi et al., 2006; Kajita et al., 2001), and whole the ZMP is described by the position on the ground,
body coordination(Sugihara and Nakamura, 2002). p = [px , py , 0]T . Second, let us take the moments
Especially, the WBC(whole body coordination) al- about origin on the ground of the linear equations of
gorithm with good performance becomes the essen- motion for the rolling sphere (with a point mass = m)
tial part in the development of humanoid robot be- confined to motion on a plane z = cz as shown in Fig.
cause it offers the enhanced stability and flexibility 1, then the following equations are obtained:
to the humanoid motion planning. In this paper, we τx = mgcy − mc̈y cz (1)
suggest the kinematic resolution method of CoM Ja- τy = −mgcx + mc̈x cz (2)
cobian with embedded motions, actually, which of-
fers the ability of WBC to humanoid robot. For τz = −mc̈x cy + mc̈y cx (3)
example, if humanoid robot stretches two arms for- where g is the acceleration of gravity, cz is a height
ward, then the position of CoM(center of mass) of constant of constraint plane and τi is the moment
humanoid robot moves forward and its ZMP(zero mo- about i-coordinate axis, for i = x, y, z. Now, if we in-
ment point) swings back and forth. In this case, the troduce the conventional definition of ZMP as follow-
proposed kinematic resolution method of CoM Jaco- ing forms:
bian with embedded (stretching arms) motion offers △ τy △ τx
px = − and py =
the joint configurations of supporting limb(s) calcu- mg mg
lated automatically to maintain the position of CoM to two equations (1) and (2), then ZMP equations can
fixed at one point. be obtained as two differential equations:
Also, we will simplify the dynamics of bipedal 1
robot as the equation of motion of a point mass con- pi = ci − 2 c̈i for i = x, y (4)
ωn
centrated on the position of CoM. First, let us assume
△p
that the motion of CoM is constrained on the surface where ωn = g/cz is the natural radian frequency of
z = cz , then the rolling sphere model with the concen- the simplified biped walking robot system. Above
248
Body Center (3 × 3) identity and zero matrix, respectively. Ro o ri

Frame
CoM(cx,cy,cz) O is the position vector from the body center to the end
point of the i-th limb represented on the world coordi-
nate frame. [(·)×] is a skew-symmetric matrix for the
Z
cross product. The transformation matrix X o is
ro
−mg Ro o ri
R 03
Supporting Xo = o ∈ ℜ6×6 (8)
Y Foot
Walking
03 Ro
X Direction
World Coordinate ri
Shifting
where Ro ∈ ℜ3×3 is the orientation of the body center
Frame Foot represented on the world coordinate frame, and here-
△
after, we will use the relation J i = X o o J i .
Z Z
CoM(cy,cz) CoM(cx,cz) All the limbs in a robot should have the same body
mcɺɺy mcɺɺx
center velocity, in other words, from Eq. (6), we can
−mg −mg
see that all the limbs should satisfy the compatibility
τx τy condition that the body center velocity is the same,
Y x x X
ZMP(py) ZMP(px) and thus, i-th limb and j-th limb should satisfy the
Figure 1: Rolling Sphere Model for Dynamic Walking.
following relation:
X i (ẋi − J i q̇i ) = X j (ẋ j − J j q̇ j ). (9)
equations will be used to prove the stability of the From Eq. (9), the joint velocity of any limb can be
posture/walking controller in the following sections. represented by the joint velocity of the base limb and
cartesian motions of limbs. Actually, the base limb
should be chosen to be the support leg in single sup-
port phase or one of both legs in double support phase.
Let us express the base limb with the subscript 1, then
2 KINEMATIC RESOLUTION the joint velocity of i-th limb is expressed as:
Let a robot has n limbs and the first limb be the base q̇i = J + +
i ẋi − J i X i1 (ẋ1 − J 1 q̇1 ), (10)
limb. The base limb can be any limb but it should be
on the ground to support the body. Each limb of a for i = 2, · · · , n, where J +
i means the Moore-Penrose
robot is hereafter considered as an independent limb. pseudoinverse of J i and
In general, the i-th limb has the following relation:
△ −1 I 3 [Ro (o r1 −o ri )×]
o
ẋi = o J i q̇i (5) X i1 = X i X 1 = . (11)
03 I3
for i = 1, 2, · · · , n, where i ∈ o ẋ
is the velocity of ℜ6 The position of CoM represented on the world co-
the end point of i-th limb, q̇i ∈ ℜni is the joint ve- ordinate frame, in Fig. 1, is given by
locity of i-th limb, o J i ∈ ℜ6×ni is the usual Jacobian
matrix of i-th limb, and ni means the number of active n
links of i-th limb. The leading superscript o implies c = ro + ∑ Ro o ci (12)
i=1
that the elements are represented on the body center
coordinate system shown in Fig. 1, which is fixed on where n is the number of limbs, c is the position vec-
a humanoid robot. tor of CoM represented on the world coordinate sys-
In the humanoid robot, the body center is floating, tem, and o ci means the CoM position vector of i-th
and thus the end point motion of i-th limb about the limb represented on the body center coordinate frame
world coordinate system is written as follows: which is composed of ni active links. Now, let us dif-
ẋi = X −1 o ferentiate Eq. (12), then the it is obtained as follows:
i ẋo + X o J i q̇i (6)
n
where ẋo = [ṙTo ; ωo T ]T ∈ ℜ6
is the velocity of the body ċ = ṙo + ωo × (c − ro ) + ∑ Ro o J ci q̇i . (13)
center represented on the world coordinate system, i=1
and
I 3 [Ro o ri ×] where o J ci ∈ ℜ3×ni means CoM Jacobian matrix of
Xi = ∈ ℜ6×6 (7)
03 I3 i-th limb represented on the body center coordinate
△
is a (6 × 6) matrix which relates the body center ve- frame, and hereafter, we will use the relation J ci =
locity and the i-th limb velocity. I 3 and 03 are an Ro o J ci .
249
Remark 1 The CoM Jacobian matrix of i-th limb rep- Here, if the CoM Jacobian is augmented with the
resented on the body center frame is expressed by orientation Jacobian of body center (ωo = −J ω1 q1 )
ni
∂o ci,k and all desired cartesian motions are embedded in Eq.
∑ µi,k
o △
J ci = , (14) (20), then the desired joint configurations of base limb
k=1 ∂qi (support limb) are resolved as follows:
+
where o ci,k ∈ ℜ3 means the position vector of center J fsem ċfsem,d
q̇1,d = , (22)
of mass of k-th link in i-th limb represented on the −J ω1 ωo,d
body center frame and the mass influence coefficient
where the subscript d means the desired motion and
of k-th link in i-th limb is defined as follow:
ċfsem,d = ċd − ∑ni=2 J ci J +
i ẋi,d . (23)
△ mass of k-th link in i-th limb
µi,k = . (15) All the given desired limb motions, ẋi,d are embedded
total mass
in the relation of CoM Jacobian, thus the effect of the
The motion of body center frame can be obtained
CoM movement generated by the given limb motion
by using Eq. (6) for the base limb as follows:
is compensated by the base limb. The CoM motion
ẋo = X 1 {ẋ1 − J 1 q̇1 } with fully specified embedded motions,
After solving Eq. (22), the desired joint motion of
ṙo I 3 [Ro o r1 ×] ṙ1 J
= − v1 q̇1 ,
ωo 03 I3 ω1 J ω1 the base limb is obtained. The resulting base limb mo-
(16) tion makes a humanoid robot balanced automatically
during the movement of the all other limbs. With the
where J v1 and J ω1 are the linear and angular velocity desired joint motion of base limb, the desired joint
part of the base limb Jacobian J 1 expressed on the motions of all other limbs can be obtained by Eq. (10)
world coordinate frame, respectively. Now, if Eq. (10) as follow:
is applied to Eq. (13) for all limbs except the base
q̇i,d = J +
i (ẋi,d + X i1 J 1 q̇1,d ), for i = 2, · · · , n. (24)
limb with subscript 1, the CoM motion is rearranged
as follows: The resulting motion follows the given desired mo-
tions, regardless of balancing motion by base limb.
ċ = ṙo + ωo × (c − ro ) + J c1 q̇1 In other words, the suggested kinematic resolution
n n
method of CoM Jacobian with embedded motion of-
+ ∑ J ci J +
i (ẋi − X i1 ẋ1 ) + ∑ J ci J i X i1 J 1 q̇1 . (17)
+
fers the WBC(whole body coordination) function to
i=2 i=2
the humanoid robot automatically.
Here, if Eq. (16) is applied to Eq. (17), then the
CoM motion is only related with the motion of base
limb. Also, if the base limb has the face contact with
the ground (the end-point of base limb represented 3 STABILITY
on world coordinate frame is fixed, ẋ1 = 0, namely,
ṙ1 = 0, ω1 = 0), then Eq. (17) is simplified as follows: The control system is said to be disturbance input-
n
to-state stable (ISS), if there exists a smooth positive
ċ − ∑ J ci J +
i ẋi = − J v1 q̇1 + rc1 × J ω1 q̇1 + J c1 q̇1
definite radially unbounded function V (e,t), a class
i=2 K ∞ function γ1 and a class K function γ2 such that
n the following dissipativity inequality is satisfied:
+ ∑ J ci J +
i X i1 J 1 q̇1 . (18) V̇ ≤ −γ1 (|e|) + γ2 (|ε|), (25)
i=2
where V̇ represents the total derivative for Lyapunov
where rc1 = c − r1 .
function, e the error state vector and ε disturbance in-
Finally, 3 × n1 CoM Jacobian matrix with embed-
put vector.
ded motions can be rewritten like usual kinematic Ja-
In this section, we propose the posture/walking
cobian of base limb:
controller for bipedal robot systems as shown in Fig.
ċfsem = J fsem q̇1 , (19) 2. In this figure, first, the ZMP Planer and CoM Planer
where generate the desired trajectories satisfying the follow-
n ing differential equation:
ċfsem = ċ − ∑ J ci J +
△
i ẋi , (20) pi,d = ci,d − 1/ω2n c̈i,d for i = x, y. (26)
i=2
n Second, the simplified model for the real bipedal
J fsem = −J v1 + rc1 × J ω1 + J c1 + ∑ J ci J +
△
i X i1 J 1 .
walking robot has the following dynamics:
i=2 ċi = ui + εi
(21) (27)
pi = ci − 1/ω2n c̈i for i = x, y,
250
Measured
ZMP Des. ZMP _ Actual ZMP ZMP Force/Torque Second, another error dynamics is obtained by using
Planner pid pi Calculation F/T
Eq. (27) and (30) as follows:
ZMP
Posture/Walking
Controller
Controller k p,i e p,i = ėc,i + kc,i ec,i + εi , (34)
_ Control Kinematic
Real
Input Resolution Joint
.
cid
+
+ ui of CoM qd _ Servo
Bipedal also, this equation can be rearranged for ėc :
Robot
Jacobian
d/dt CoM
q
ėc,i = k p,i e p,i − kc,i ec,i − εi . (35)
Controller
CoM Des. CoM _ Actual CoM CoM

Actual
Configuration
Third, by differentiating the equation (34) and by us-
Planner cid ci Calculation q ing equations (33) and (35), we get the following:
Figure 2: Posture/Walking Controller for Humanoid Robot. ė p,i = 1/k p,i (ëc,i + kc,i ėc,i + ε̇i )
= ω2n /k p,i (ec,i − e p,i )
+kc,i /k p,i (k p,i e p,i − kc,i ec,i − εi ) + (1/k p,i )ε̇i
where εi is the disturbance input produced by ac- ! 2
tual control error, ui is the control input, ci and pi ω2n − kc,i
2
ωn − k p,i kc,i
= ec,i − e p,i
are the actual positions of CoM and ZMP measured k p,i k p,i
from the real bipedal robot, respectively. Here, we as-
1
sume that the the disturbance produced by control er- + (ε̇i − kc,i εi ). (36)
ror is bounded and its differentiation is also bounded, k p,i
namely, |εi | < a and |ε̇i | < b with any positive con- Fourth, let us consider the following Lyapunov func-
stants a and b. Also, we should notice that the control tion:
error always exists in real robot systems and its mag- △ 1 2
nitude depends on the performance of embedded local V (ec,i , e p,i ) = (kc,i − ω2n )e2c,i + k2p,i e2p,i , (37)
2
joint servos. The following theorem proves the stabil- where V (ec , e p ) is the positive definite function for
ity of the posture/walking controller to be suggested k p,i > 0 and kc,i > ωn , except ec,i = 0 and e p,i = 0.
for the simplified robot model. Now, let us differentiate the above Lyapunov func-
Theorem 1 Let us define the ZMP and CoM error for tion, then we can get the following:
the simplified bipedal robot control system (27) as fol- V̇ ≤ −(kc,i − α2 )(kc,i
2
− ω2n )e2c,i
lows:
−k p,i [ω2n − (k p,i + γ2 )kc,i − β2 ]e2p,i
△ " #
e p,i = pi,d − pi (28) 2 − ω2 )
(kc,i n k p,i kc,i 2 k p,i 2
△ + + εi + 2 ε̇i(38)
ec,i = ci,d − ci for i = x, y. (29) 4α2 4γ2 4β
If the posture/walking control input ui in Fig. 2 has where e2c,i term is negative definite with any pos-
the following form: √
itive constant satisfying α < ωn and e2p,i term
ui = ċdi − k p,i e p,i + kc,i ec,i (30) is negative definite under the given conditions
(31). Here, since the inequality (38) follows the
under the gain conditions: ISS property (25), we concludes that the pro-
posed posture/walking controller gives the distur-
ω2n − β2
kc,i > ωn and 0 < k p,i < − γ2 (31) bance input(εi , ε̇i )-to-state(e p,i , ec,i ) stability (ISS) to
ωn
the simplified control system model of bipedal robot.
satisfying the following conditions:
s To make active use of the suggested control
ω2n − β2 scheme, the control input u of Eq. (30) suggested in
β < ωn and γ < , (32)
ωn Theorem 1 is applied to the place of the term ċd in
Eq. (23). In other words, equation (23)is modified
then the posture/walking controller gives the distur- to include the ZMP and CoM controllers as following
bance input(εi , ε̇i )-to-state(e p,i , ec,i ) stability (ISS) to forms:
a simplified bipedal robot, where, the k p,i is the pro- n
portional gain of ZMP controller and kc,i is that of ċfsem,d = u − ∑ J ci J +

i ẋi,d (39)
i=2
CoM controller in Fig. 2.
△
Proof. First, we get the error dynamics from Eq. where u = ċd − k p e p + kc ec . And then, the suggested
(26) and (27) as follows: kinematic resolution method of Eq. (22) and (24) are
utilized to obtain the desired base limb and other limb
ëc,i = ω2n (ec,i − e p,i ). (33) motions in the joint space as shown in Fig. 2.
251
ACKNOWLEDGEMENTS
This work was supported in part by the Korea
Research Foundation Grant funded by the Korea
Government (MOEHRD, Basic Research Promotion
Fund) (KRF-2006-003-D00186) and in part by MIC
& IITA through IT leading R&D support project, Re-
public of Korea.
REFERENCES
Choi, Y., Kim, D., and You, B. J. (2006). On the walk-
ing control for humanoid robot based on the kinematic
resolution of com jacobian with embedded motion.
Proc. of IEEE Int. Conf. on Robotics and Automation,
pages 2655–2660.
Kajita, S., ad M. Saigo, K. Y., and Tanie, K. (2001). Bal-
ancing a humanoid robot using backdrive concerned
torque control and direct angular momentum feed-
back. Proc. of IEEE Int. Conf. on Robotics and Au-
tomation, pages 3376–3382.
Sugihara, T. and Nakamura, Y. (2002). Whole-body coop-
erative balancing of humanoid robot uisng COG ja-
cobian. Proc. of IEEE/RSJ Int. Conf. on Intelligent
Robots and Systems, pages 2575–2580.
252
ON COMPUTING MULTI-FINGER FORCE-CLOSURE GRASPS
OF 2D OBJECTS
Belkacem Bounab * **, Daniel Sidobre ** ‡

*
Mechanical Laboratory of Structures, EMP, Algiers, Algeria
**
LAAS-CNRS, Toulouse, France
‡
Paul Sabatier University, Toulouse, France
Abdelouhab Zaatri
Advanced Technologies Laboratory, Mentouri University, Constantine, Algeria
[email protected]
Keywords: Multi-Fingered grasps, Force-Closure, Central Axis, Grasp Wrenches.
Abstract: In this paper, we develop a new algorithm for computing force-closure grasps of two-dimensional (2D)
objects using multifingred hand. Assuming hard-finger point contact with Coulomb friction, we present a
new condition for multi-finger to form force-closure grasps. Based on the central axis of contact wrenches,
an easily computable algorithm for force-closure grasps has been implemented and its efficiency has been
demonstrated by examples.
1 INTRODUCTION quality is obtained using the mechanical properties

of the grasp wrench.
Grasping remains one of the fundamental problems
in robotics. Research has been directed towards the
design and control of multifingred dexterous robot 2 RELATED WORK
hand to increase robot dexterity and adaptability (Li
J-W., Jin M-H. and Liu H. 2003). Force-closure test is an essential problem in
A main property of a multi-finger stable grasp is grasping. However, The notion of force-closure does
force-closure. It’s the ability to balance any external not directly yield a method for force-closure test
object wrenches by applying appropriate finger (Sudsang A. and Phoka T. 2005). Some necessary
wrenches at the contact points. In other words, a and sufficient conditions for force-closure were
grasp on an object is force-closure if and only if formulated in order to derive force-closure tests. A
arbitrary force and torque can be exerted on the commonly used necessary and suffecient force-
object through the fingers (Yan-Bin Jia 2004). It’s closure condition given by (Salisbury J.K. and Roth
complicated to assure that the applied finger forces B. 1982) allowed a force-closure test to be
remain in the friction cone at all times so as to avoid performed by checking whether the origin is strictly
fingers slippage on the object surface (Murray R., Li inside the convex hull of the primitive contact
Z. and Sastry S. 1994). wrenches. This test also provided an underlying idea
Human can use more than three/four fingers of to recent work in grasping (D. Ding, Y-H Liu, and S.
his hand to manipulate objects. During such tasks, Wang 2001). Nguyen (Nguyen, V.D. 1988) formally
there exists a lot of contact points between the hand demonstrated for 2-fingered grasps that non-
and grasped object. The question is: how can we marginal equilibrium grasps achieve force-closure.
evaluate or compute force-closure of such grasps? Recently, (Li J-W., Jin M-H. and Liu H. 2003)
In this paper, we are focused on the problem of proposed a necessary and suffecient condition for 3-
computing force-closure of multifingered grasps of fingered force-closure grasps based on (Ponce J. and
2D objects. We develop a new approach for force- Faverjon B. 1995) and developed an algorithm for
closure test independently of fingers's number. This three-finger force-closure test. Their method begins
by the processing of friction cones using an
253
operation called disposition H, then, they attack the ⎛ Cos( βi + α ) ⎞ ⎛ Cos( βi − α ) ⎞

problem of determining the intersection of the three n i1 = ⎜⎜ ⎟⎟ ; n i2 = ⎜⎜ ⎟⎟ (3)
⎝ Sin( β i + α ) ⎠ ⎝ Sin( βi − α ) ⎠
double-side friction cones.
The rest of the paper is organized as follow, in The contact wrench produced by f i reduced at the
section 3, we present the background of grasp point O is defined by
wrenches central axes and the relationship between ⎡ fi ⎤
wi = ⎢ ⎥
these axes and grasp force-closure. In section 4, we ⎣OCi × fi ⎦
propose a new multi-finger force-closure condition.
⎡ ai1 n i1 + ai 2 n i 2 ⎤
Hence, a novel algorithm is presented, which its =⎢ ⎥ (4)
implementation needs little geometric computations. ⎣OCi × (ai1 n i1 + ai 2 n i 2 )⎦
In section 5, we present some multi-finger grasps
examples. Finally, we conclude with future works. The external wrench applied by the robotic hand
on the grasped object is given by
3 CENTRAL AXIS OF GRASP

∑ ⎡ Fc ⎤
m
Wc / o = wi = ⎢ ⎥ (5)
WRENCHES i =1 ⎣τ c / o ⎦
With
Based on Coulomb friction model, a contact force is
∑ f = ∑ (a n
m m
Fc = i i1 i1 + ai 2 n i 2 )
constrained to lie in a friction cone centered about i =1 i =1
(6)
= ∑ (OC × (a n
the internal surface normal at contact point. m
τc / o i i1 i1 + ai 2 n i 2 ))
i =1
n i1 fi
In two-dimensional grasps case, we have
Ni
βi
α
ni2 ( )
Fc = Fcx , Fcy ,0 T and τ c / o = (0, 0, τ z / o )T (7)
finger
Y Ci With
⎧ m
object
⎪ Fcx = X ⋅
⎪ ∑
(ai1 n i1 + ai 2 n i 2 )
⎪ i =1
O X ⎪ m
Figure 1: Contact between the finger and an object

⎪
⎨ Fcy = Y ⋅
⎪
∑
(ai1 n i1 + ai 2 n i 2 ) (8)
i =1
showing friction cone. ⎪
m
⎪
As shown in figure 1, a friction cone at Ci is
⎪τ z / o = Z ⋅
⎪
∑
(OCi ∧ (ai1 n i1 + ai 2 n i 2 ))
⎩ i =1
bounded by vectors ni1 and ni 2 , and any force fi is a
nonnegative combination of these two vectors. In 2D Poinsot’s theorem: “Every collection of wrenches
case, contact forces are applied to a rigid body is equivalent to a force
f i = ai1 n i1 + ai 2 n i 2 i = 1,...,m (1) applied along a fixed axis (central axis) and a
torque around the same axis” (Murray R., Li Z.,
Sastry S. 1994). Using this theorem, points of the
With ai1 ≥ 0, ai 2 ≥ 0 to avoid fingertips slippage. central axis ΔC of contact wrench are given by
m is the number of contact points.
If Ni is the surface normal at the contact
⎧ Fc × τ c / o
point Ci and α is the friction angle that depends on ⎪ + λ Fc if Fc ≠ 0
ΔC = ⎨ Fc 2 : (λ ∈ ℜ) (9)
materials in contact (finger and object) then, ⎪ λτ if Fc = 0
⎩ c/o
The axis ΔC is a directed line through a point.
βi = Atan 2( N i Y , N i X ) (2) For Fc ≠ 0 , the central axis is a line in the
and
254
Fc direction going through the point Q0 such as

OQ 0 = (Fc × τ c / o ) / Fc 2 .
For Fc = 0 , the axis is a line in the τc / o direction
going through the origin (Murray R., Li Z., Sastry S.
1994).
In two-dimentional case with non null forces (a) (b)
( Fc ≠ 0 ), the torque around the central axis is zero.
The force Fc is an invariant vector and always Figure 4: a) non-force-closure five-fingers grasp;
parallel to ΔC . Figure 2 shows the central axis in 2D b) central axes of grasp wrenches ( α = 10° ).
grasps when Fc ≠ 0 . it is Characterized by the
following equation When a grasp is force closure, the central axes of
⎛ Fcy ⎞ ⎛τ ⎞ grasp wrenches can wholly sweep the plan (X, Y ) . In
y = ⎜⎜ ⎟⋅ x + ⎜ z/ o ⎟
⎟ ⎜ F ⎟ (10)
⎝ Fcx ⎠ ⎝ cx ⎠ the third example, shown in figure 5, we use the
ΔC same finger’s configuration as figure 3 but we
Y change the friction angle α = 20° . The grasp
Fc becomes force-closure.
Q0
X
O
(a) (b)
Figure 2: Central axis parameters. Figure 5: a) three fingers force-closure 2D grasp,
b) central axes of grasp wrenches ( α = 20° ).
In figure 3-a, we present a first example of three-
finger 2D grasp. By varying forces fi randomly (in
According to these three examples, we can
orientation and amplitude) inside friction cones (the conclude that the distribution of central axes can
friction angle), figure 3-b illustrates all possible confirm if a grasp is force-closure or not (for any
central axes of grasp wrenches. contact points number).
There is no central axis passing through the gray
region. In this region, positive torque can’t be
exerted on the object through the finger contacts.
This grasp can’t achieve force-closure. Exactly, the 4 FORCE-CLOSURE AND
grasp can not achieve torque-closure because the EQUILIBRIUM CONDITION
object turn around the gray region in figure 3-a.
+Torque In 2D grasps and based on Poinsot’s theorem, we
can give the following definition.
C1
Definition 1: Any external wrench applied by the
robotic hand on a 2D object is equivalent to a force
C2
along a central axis of this wrench. When the force
C3
is equal to zero, the external wrench is equivalent to
a torque about the grasp plan normal.
Definition 2: A grasp on an object is force-closure
(a) (b) if and only if any arbitrary force and torque can be
Figure 3: a) no force-closure 2D grasp, b) all central axes exerted on the object through the finger contacts
of grasp wrenches ( α = 5° ). (Yan-Bin Jia 2004). There are other force-closure
definitions, but this one is more useful for our
A second example is shown in figure 4; we deduction.
present a non-force-closure grasp using five contact Definition 3: A grasp is said to achieve
points. This grasp is instable and the object turn equilibrium when it exists forces (not all of them
around Z axis in the gray region (figure 4-a). being zero) in the friction cones at the fingertips
255
such that the sum of the corresonding wrenches is a) if Ft = 0 then the central axis of the task wrench is
zero (Sudsang A., Phoka T. 2005). defined by the unit vector U t where:
τt / o
Ut =
4.1 Equilibrium Condition τt / o
If the two wrenches have the same central line
During objects grasp operations there exist two with opposite direction then the contact central axis
kinds of external wrenches applied on the is defined by the following unit vector:
manipulated object, task wrench (applied by the
τc / o
environment) and contact wrench (applied by the Uc = −Ut =
robotic hand fingers). Based on definitions 1 and τc / o
definitions 3, we derive a new proposed necessary We conclude that:
and sufficient equilibrium condition.
Sgn(τ c / o ⋅ U c ) Sgn(τ t / o ⋅ U c ) < 0 (17)
Proposition 1: A multifingers grasp is said to
achieve equilibrium if and only if the central lines of Sgn is the sign function that computes the sign of
contact wrench and task wrench have the same the leading coefficient of expression.
support and opposite direction.
b) if Ft ≠ 0 , having the same central axis with
Proof: opposite direction implies
i) Sufficient Condition:
Sgn(Fc ⋅ U c ) Sgn(Ft ⋅ U c ) < 0 (18)
the external contact wrench given by equation (5)
and task wrench is given by
Where U c and U t define the unit vectors of the two
⎡ F ⎤ central axes. We have:
Wt / o =⎢ t ⎥ (11)
⎣τ t / o ⎦ Fc Ft
Uc = ; Ut =
The object is in equilibrium if: Fc Ft
Using hypothesis that there is one central line and
⎧ Fc = − Ft form relation (9), we have
Wc / o + Wt / o = {0} ⇒ ⎨ (12)
⎩τ c / o = − τ t / o Fc × τ c / o Ft × τ t / o
2
= (19)
From Relation (9), the central axis of contact Fc Ft 2
wrench is defined by
Then, replacing Fc = Fc U c ; Ft = − Ft U c in
Fc × τ c / o Relation (19) we obtain
OPc = + λc Fc (λc ∈ ℜ) (13)
Fc 2
⎛ τc / o τt / o ⎞
Substuting (12) in (13) lead to ⎜
⎜ Fc
+ ⎟ = λ ⋅ Uc :
⎟
(λ ∈ ℜ) (20)
⎝ F t ⎠
Ft × τ t / o
OPc = + λt Ft (λt = − λc ∈ ℜ) (14)
Ft 2 In 2D case, the equation (20) can be only
satisfied when λ = 0 . therefore, the two torques have
Relation (14) defines the central axis of task
opposite signes:
wrench given by
Sgn(τ c / o ⋅ Z ) Sgn(τ t / o ⋅ Z ) < 0 (21)
Ft × τ t / o
OPt = + λt Ft (λt ∈ ℜ) (15)
Ft 2 Relations (17, 18 and 21) imply that the contact
wrench can generate grasp wrenches that opposite
In the case Ft = 0 , the points Pc are given by the external task wrench. Robotic hand can control
its fingers force to produce the appropriate
OPc = λc τ c / o = λt τ t / o : (λt = − λc ∈ ℜ) (16) force/torque magnitude that achieving equilibrium.■
In both cases, Relations (14) and (16), the two
wrenches (contact and task) should have the same 4.2 Force-Closure Condition
central line with opposite directions. ■
ii) Necessary Condition: In particular, force-closure implies equilibrium, but
Now, if we consider two wrenches reduced at the there are wrench systems that achieve equilibrium
same point O and they have the same central line but not force closure (Li Jia-Wei. and Cia He-Gao,
with opposite directions. We have two cases: 2003).
256
Using force-closure condition in definition 2, we m m

can derive this definition
Definition 4: A grasp is force closed, if and only if
Y⋅ ∑ (n i1 + n i 2 ) < ∑ ( Y ⋅ n i1
i =1 i =1
+ Y ⋅ ni2 ) (23-2)
it is in equilibrium for any arbitrary wrench (Bicchi

A., Kumar V. 2000, Nguyen, V.D. 1988). Thus, m
force closure implies, fingers contact wrenches can
balance any external task wrenches. ∑ Z ⋅ (B C
i =1
k i ∧ (n i1 + n i 2 )) <
According to proposition 1 and definition 4, we (23-3)

m
∑ ( Z ⋅ (B C )
propose a new force-closure necessary and sufficient
k i ∧ n i1 ) + Z ⋅ (B k C i ∧ n i 2 )
condition.
i =1
Proposition 2: A multifingred grasp of 2D objects
is said to achieve force-closure if and only if the From mechanical viewpoint, inequality (23-1)
central axis of the fingers contact wrenches can implies that fingers can generate forces
sweep the grasp plan at any direction. along X and − X , (23-2) means that fingers can exert
force on the object along Y and − Y . If the last
4.3 Force-Closure Test Algorithm (
inequality (23-3) is true for k = 1...N Bk then the )
finger can exert torque on object about the vertical
According to the proposition 2, we present a new
axis Z in both directions.
algorithm for computing 2D multi-fingers grasps of
arbitrary object.
Based on the central axis equation defined in
relation (10), this central line can sweep the plan in 5 EXAMPLES
all directions if
∀(k1 , k 2 ) ∈ ℜ 2 , ∃ Δ c Satisfy y = k1 ⋅ x + k 2 We present bellow some grasp examples using three,
four and five fingers. In both cases (force-closure
Where and no force-closure), we show the distribution of
grasp wrench central axes.
⎛ Fcy ⎞ ⎛τ ⎞
k1 = ⎜⎜ ⎟
⎟ ; k1 = ⎜⎜ z / o ⎟
⎟ a) Three-finger grasps
⎝ Fcx ⎠ ⎝ Fcx ⎠
In other word, for any axis on the (X,Y ) plan or
along the vertical Z , this axis must be one of the
grasp wrench central axes.
This condition implies that the quantities k1 and
k 2 must take all real number, therefore
(a) (b)
⎧ Fcx ∈ [− ∞ ,+∞ ]
⎪
⎨ Fcy ∈ [− ∞ ,+∞]
Figure 6: a) a three-finger force-closure 2D grasp, b)
(21)
central axes of grasp wrenches ( α = 15° ).
⎩ c / o ∈ [− ∞ ,+∞]
⎪τ ∀O
The third sub-condition is function of the reduced
point of the torque, to cover the entire grasp plan; we
test this condition at all the vertices of the
intersection of the m double-side friction cones
(named Bk ). In general case of m contact points, the
number of intersection points is given by
∑
m −1
N Bk = 4 ⋅
k =1
(m − k ) (22) (a) (b)
Figure 7: a) a three-finger force-closure 2D grasp, b)
Hence, a multifingred 2D grasp is said to achieve central axes of grasp wrenches ( α = 20° ).
force-closure if each of these inequalities are true.
m m
X⋅ ∑ (n i1 + n i2 ) < ∑ ( X ⋅ n i1
i =1 i =1
+ X ⋅ ni2 ) (23-1)
257
6 CONCLUSION AND FUTURE

WORK
We have presented a new equilibrium and force-
closure conditions for multifingred 2D grasps. A
novel algorithm for computing 2D multi-finger
(a) (b) force-closure grasps of arbitrary objects was
developed, which is very simple and needs little
Figure 8: a) no force-closure 2D grasp, b) central axes of geometric computations. Therefore, it can be
grasp wrenches ( α = 10° ). Grasp wrenches can’t generate implemented in real-time multifingred grasp
a negative torque in grey zone. programming. Our future work will be concentrated
on the extending of this algorithm to the 3D grasps
and the quality measurement of grasps.
REFERENCES
Bicchi A., Kumar V. (2000). Robotic grasping and
Contact: A review. In Proc. IEEE ICRA, pp. 348–352.
(a)
Ding D., Liu Y-H, and S. Wang (2001). Computation of 3-
(b)
d form-closure grasps. In IEEE Transactions on
Figure 9: a) no force-closure 2D grasp, b) central axes of Robotics and Automation, 515-522.
grasp wrenches ( α = 10° ).Grasp wrenches can’t exert a Li J-W., Cia He-Gao (2003). On Computing Three-Finger
force along (−Y ) axis and can’t generate torques in two- Force-Closure Grasps of 2-D and 3-D Objects.
direction in unreachable zones in (b). Transaction on Robotics & Automation. IEEE
Proceeding, 155-161.
Li J-W., Jin M-H., Liu H. (2003). A New Algorithm for
b) Four-finger grasps Three-finger Force-Closure Grasp of Polygonal
Objects. In International Conference on Robotics &
Automation. IEEE Proceeding, 1800-1804.
Murray R., Li Z., Sastry S. (1994). A Mathematical
Introduction to Robotic Manipulation, Boca Raton,
FL: CRC press.
Nguyen, V.D. (1988). Constructing force-closure grasps.
In Journal of Robotics Research, vol. 7, no. 3, pp. 3-
16.
(a) (b) Ponce J. and Faverjon B.. (1995). On computing three-
finger force-closure grasps of polygonal objects. In
Figure 10: a) four-finger force-closure 2D grasp, b) central IEEE Transactions on Robotics and Automation, 868-
axes of grasp wrenches ( α = 10° ). 881.
Salisbury J.K. and Roth B. (1982) Kinematic and force
c) Five-finger grasps analysis of articulated hands. In Journal of
Mechanisms, Transmissions, and Automation in
Design, 105, 33-41.
Sudsang A., Phoka T. (2005). Geometric Reformulation of
3-Fingred Force-Closure Condition. In International
Conference on Robotics & Automation. IEEE
Proceeding, 2338-2343.
Yan-Bin Jia (2004). Computation on Parametric Curves
with an Application in Grasping. In the International
Journal of Robotics Research, vol. 23, No. 7-8, 825-
(a) (b) 855.
Figure 11: a) five-finger force-closure 2D grasp, b) central

axes of grasp wrenches ( α = 25° ).
258
PREDICTIVE CONTROL BY LOCAL VISUAL DATA
Mobile Robot Model Predictive Control Strategies Using Local Visual
Information and Odometer Data
Lluis Pacheco and Ningsu Luo

Institute of Informatics and Applications, University of Girona, Av. Ll. Santaló s/n, Girona, Spain
Keywords: Autonomous mobile robots, computer vision control, system identification, model based control, predictive
control, trajectory planning, obstacle avoidance, robot vision.
Abstract: Nowadays, the local visual perception research, applied to autonomous mobile robots, has succeeded in
some important objectives, such as feasible obstacle detection and structure knowledge. This work relates
the on-robot visual perception and odometer system information with the nonlinear mobile robot control
system, consisting in a differential driven robot with a free rotating wheel. The description of the proposed
algorithms can be considered as an interesting aspect of this report. It is developed an easily portable
methodology to plan the goal achievement by using the visual data as an available source of positions.
Moreover, the dynamic interactions of the robotic system arise from the knowledge of a set of experimental
robot models that allow the development of model predictive control strategies based on the mobile robot
platform PRIM available in the Laboratory of Robotics and Computer Vision. The meaningful contribution
is the use of the local visual information as an occupancy grid where a local trajectory approaches the robot
to the final desired configuration, while avoiding obstacle collisions. Hence, the research is focused on the
experimental aspects. Finally, conclusions on the overall work are drawn.
1 INTRODUCTION can be achieved using the dynamic constraints of

WMR (Fox, et al., 1997). Rimon and Koditschek
The research presented in this paper addresses to a (1992) presented the methodologies for the exact
kind of differential driven WMRs (wheeled mobile motion planning and control, based on the artificial
potential fields where the complete information
robots). Nowadays, the computer vision techniques
applied to WMR have solved the problem of about the free space and goal are encoded. Some
obstacle detection by using different methods as approaches on mobile robots propose the use of
potential fields, which satisfy the stability in a
stereo vision systems, optical flow or DFF (depth
from focus). Stereo vision systems seem to provide Lyapunov sense, in a short prediction horizon
the easiest cues to infer scene depth (Horn, 1998). (Ögren and Leonard, 2005). The main contribution
of this paper is the use of the visual information as a
The optical flow techniques used in WMR result in
several applications as i.e. structure knowledge, dynamic window where the collision avoidance and
obstacle avoidance, or visual servoing (Campbell, et safety stop can be planned. Thus, local visual data,
instead of artificial potential fields, are used in order
al., 2004). The DFF methods are also suitable for
WMR. For example, three different focused images to achieve the Lyapunov stability. The use of MPC
were used, with almost the same scene, acquired (model predictive control) with available on-robot
information is possible. Moreover, the local visual
with three different cameras (Nourbakhsh, et al.,
1997). In this work, it is supposed that available information is used as an occupancy grid that allows
obstacle positions are provided by using computer planning feasible trajectories towards the desired
objective. The knowledge of the objective allows the
vision systems. In this context, the allowed
navigation control signals should achieve the optimal solution of the local desired coordinates
obstacle avoidance as well as the final desired based on the acquired images. The sensor fusion is
done using visual perception, as the meaningful
coordinates. Scientific community has developed
several studies in this field. Based on the dynamic source of information in order to accomplish with
window approach with available robot speeds, the the robot tasks. Other data provided by the encoder-
based odometer system are also considered.
reactive avoidance collisions, safety stop and goal
259
This paper is organized as follows: Section 1

gives a brief presentation about the aim of the
present work. In the Section 2, the platform is
introduced as an electromechanical system. This
section also describes the experiments to be realized
in order to find the parametric model of the robot
suitable for designing and implementing MPC
methods. In the Section 3, the use of visual data is
presented as a horizon where optimal trajectories can
be planned. Section 4 presents the MPC strategies
used for achieving the path following of the
reference trajectories. In the Section 5, some
conclusions are made with special attention paid into Figure 1: The robot PRIM used in this work.
the future research works.
sing speed commands. The maximal speed is near
0.5m/s. A set of microcontroller boards (MCS-51) is
used to read the information available from different
2 ROBOT AND BASIC CONTROL connected sensors. The rate of communication with
METHODS these boards is 9600 b/s. Figure 2 shows the
electronic and sensorial system blocks. The data
This section gives some description on the main gathering and the control by digital computer is set
robot electromechanical and sensorial systems of the to 100ms.
platform tested in this work. Hence, the WMR
PRIM, available in our lab, has been used in order to
test and orient the research. The experimental
modelling methodologies as well as the model
predictive control are also introduced.
2.1 Electromechanical and Sensorial

System of the Robot
Figure 1 shows the robot PRIM used in the research
work. The mechanical structure of the robot is made
of aluminum, with two independent wheels of 16cm
Figure 2: The sensorial and electronic system blocs.
diameters actuated by two DC motors. The distance
between two wheels is 56.4cm. A third spherical The system flexibility is increased with the
omni-directional wheel is used to guarantee the possibility of connecting with other computer
system stability. The maximum continuous torque of systems through a local LAN. In this research, it is
each motor is 131mNm. The proportion of gear connected to a machine vision system that controls a
reduction for each motor is 86:1 and thus the total colour camera EVI-D70P-PAL through the VISCA
force actuating on the robot is near 141N. Shaft RS232-C control protocol. For instance, the camera
encoders with 500 counts/rev are placed at the motor configuration used in this work is of a horizontal
axes, which provide 43000 counts for each turn of field of view of 48º, and a vertical field of 37º. The
the wheel. A set of PLD (programmable logic focus, pan and tilt remain fixed under present
device) boards is connected to the digital outputs of configuration. Hence, the camera pose is set to
the shaft encoders. The printed circuits boards 109cm from the floor with a tilt angle of 32º. The
(PCB) are used to measure the speed of each motor local desired coordinates, obtained by the visual
at every 25ms. perception information, are transmitted to the control
An absolute counter provides the counts in unit connecting the USB port to the LAN.
order to measure the robot position by the odometer
system. Another objective of these boards is to 2.2 Experimental Model
generate a signal of 23khz PWM for each motor.
The communication between the central digital The parametric identification process is based on
can computer and the boards is made through the black box models (Lju, 1989), (Norton, 1986) and
thus it parallel port. The speed is commanded by a (Van Overschee, Moor, 1996). Thus, the transfer
byte and generate from 0 to 127 advancing or rever- functions are related to a set of polynomials that
260
PREDICTIVE CONTROL BY LOCAL VISUAL DATA - Mobile Robot Model Predictive Control Strategies Using Local
Visual Information and Odometer Data
allow the use of analytic methods in order to deal The treatment of experimental data is done
with the problem of controller design. The before the parameter estimation. In concrete, it
nonholonomic system dealt with in this work is includes the data filtering, using the average value of
considered initially as a MIMO (multiple input five different experiments with the same input
multiple output) system, which is composed of a set signal, the frequency filtering and the tendency
of SISO subsystems with coupled dynamic influence suppression. The system is identified by using the
between two DC motors. The approach of multiple identification toolbox “ident” of Matlab for second
transfer functions consists in making the order models. The following continuous transfer
experiments with different speeds. In order to find a function matrix for medium speed is obtained:
reduced-order model, several studies and ⎛ 0.35s 2 + 4.82s + 4.46 0.02s 2 + 0.27s + 0.32 ⎞
experiments have been done through the system ⎜ ⎟
⎛ R ⎞ ⎜ s 2 + 5.84s + 4.89
Y s 2 + 5.84s + 4.89 ⎟⎛⎜U R ⎞⎟ (3)
identification and model simplification. ⎜⎜ ⎟⎟ =
⎝ YL ⎠ ⎜ 0.26s + 3.41s + 0.28
2
0.11s + 1.72s + 5.12 ⎟⎜⎝ U L ⎟⎠
2
⎜ ⎟
⎝ s + 5.84s + 4.89
2
s 2 + 5.84s + 4.89 ⎠
2.2.1 System Identification
It is shown by simulation results that the obtained
The parameter estimation is done by using a PRBS model fits well with the experimental data.
(Pseudo Random Binary Signal) as excitation input
signal. It guarantees the correct excitation of all 2.2.2 Simplified Model of the System
dynamic sensible modes of the system along the
spectral range and thus results in an accurate This section studies the coupling effects and the way
precision of parameter estimation. The experiments for obtaining a reduced-order dynamic model. It is
to be realized consist in exciting two DC motors in seen from (3) that the dynamics of two DC motors
different (low, medium and high) ranges of speed. are different and the steady gains of coupling terms
The ARX (auto-regressive with external input) are relatively small (less than 20% of the gains of
structure has been used to identify the parameters of main diagonal terms). Thus, it is reasonable to
the robot system. The problem consists in finding a neglect the coupling dynamics so as to obtain a
model that minimizes the error between the real and simplified model.
estimated data. By expressing the ARX equation as a
lineal regression, the estimated output can be written
as:
ŷ = θϕ (1)
with ŷ being the estimated output vector, θ the
vector of estimated parameters and φ the vector of
measured input and output variables. By using the
coupled system structure, the transfer function of the
robot can be expressed as follows:
⎛ YR ⎞ ⎛ GRR GLR ⎞⎛U R ⎞
⎜ ⎟=⎜ ⎟⎜ ⎟ (2)
⎝ YL ⎠ ⎝ GRL GLL ⎠⎝ U L ⎠ Figure 4: Coupling effects at the left wheel.
where YR and YL represent the speeds of right and In order to verify it from real results, a set of
left wheels, and UR and UL the corresponding speed experiments have been done by sending a zero speed
commands, respectively. In order to know the command to one motor and other non-zero speed
dynamics of robot system, the matrix of transfer commands to the other motor. In Figure 4, it is
function should be identified. Figure 3 shows the shown a response obtained on the left wheel, when a
speed response of the left wheel corresponding to a medium speed command is sent to the right wheel.
left PBRS input signal. The experimental result confirms the above facts.
The existence of different gains in steady state is
also verified experimentally. Finally, the order
reduction of system model is carried out trough the
analysis of pole positions by using the method of
root locus. Afterwards, the system models are
validated through the experimental data by using the
PBRS input signal. A two dimensional array with
three different models for each wheel is obtained.
Hence, each model has an interval of validity where
the transfer function is considered as linear.
Figure 3: Left speed output for a left PRBS input signal.
261
2.3 Odometer System Expression local model predictive control, LMPC, in order to
use the available visual data in the navigation
Denote (x, y, θ) as the coordinates of position and strategies for the goal achievement.
orientation, respectively. The Figure 5 describes the The MPC is based on minimizing a cost
positioning of robot as a function of the radius of left function, related to the objectives, through the
and right wheels (Re, Rd), and the angular selection of the optimal inputs. In this case, the cost
incremental positioning (θe, θd), with E being the function can be expressed as follows:
distance between two wheels and dS the incremental ⎧ ⎫
displacement of the robot. The position and angular ⎪[X (k + n k ) − X ]T P[X (k + n k ) − X ] ⎪
incremental displacements are expressed as: ⎪ d d
⎪
⎪⎪ n −1 ⎪⎪
J (n, m ) = min ⎨+ ∑ [X (k + i k ) − X d ] Q[X (k + i k ) − X d ]⎬ (6)
T
Rd dθ d + Re dθ e Rd dθd − Redθe ⎧
⎨U (k + i k )
j = m −1⎫
⎬⎪ ⎪
(4)
i =1
dS = dθ = ⎩ j =0 ⎭
⎪ m−1 T ⎪
2 E ⎪ ∑
+ U (k + i k )RU (k + i k ) ⎪
⎩⎪ j =0 ⎭⎪
The coordinates (x, y, θ) can be expressed as:
Denote Xd=(xd,yd,θd) as the desired coordinates. The
first term of (6) is referred to the final desired
x n = x n − 1 + dS cos (θ n − 1 + d θ ) coordinate achievement, the second term to the
y n = y n − 1 + dS sin (θ n − 1 + d θ ) (5 ) trajectory to be followed, and the last one to the
θ n = θ n −1 + d θ input signals minimization. The parameters P, Q and
R are weighting parameters. X(k+n|k) represents the
terminal value of the predicted output after the
horizon of prediction n and X(k+i|k) represents the
predicted output values within the prediction
horizon. The system constrains are also considered:
⎧ U (k + i k ) ≤ G1 α ∈ [0,1) ⎫
⎪⎪ ⎪⎪
⎨ [xk + i , yk + i ] − [xo , yo ] ≥ G2 ⎬ (7 )
⎪
[x , y ] − [xd , yd ] ≤ α [xk , yk ] − [xd , yd ]⎪⎭⎪
⎩⎪ k + n k + n
Figure 5: Positioning of the robot as functions of the
angular movement of each wheel. The limitation of the input signal is taken into
account in the first constraint. The second constraint
Thus, the incremental position of the robot can be is related to the obstacle points where the robot
obtained through the odometer system with the should avoid the collision. The last one is just a
available encoder information obtained from (4) and convergence criterion.
(5).
2.4 Model Predictive Control 3 THE HORIZON OF LOCAL

The model predictive control, MPC, has many
VISUAL PERCEPTION
interesting aspects for its application to mobile robot
control. It is the most effective advanced control The use of sensor information as a useful source to
technique, as compared to the standard PID control, build 2D environment models consists of a free or
that has made a significant impact to the industrial occupied grid proposed by (Elfes, 1989). The
process control (Maciejowski, 2002). Recently, real knowledge of occupancy grids knowledge has been
time mobile robot MPC implementations have been used for static indoor mapping with a 2D grid
developed using global vision sensing (Gupta, (Thrun, 2002). In other works of multidimensional
Messom et al., 2005). In (Küne, Lages et al., 2005), grids, multi target tracking algorithms are employed
it was studied the MPC based optimal control useful by using obstacle state space with Bayesian filtering
for the case when nonlinear mobile robots are used techniques (Coué et al., 2006). In this work it is
under several constraints, as well as the real time proposed the use of the local visual information
implementation possibilities when short prediction available from the camera as a local map that has
horizons are used. In general, the global trajectory enough information in order to achieve a global
planning becomes unfeasible since the sensorial objective. The occupancy grid can be obtained in
system of some robots is just local. By using a MPC, real time by using computer vision methods. The use
the idea of the receding horizon can deal with the of the optical flow has been proposed as a feasible
local sensor information. In this way, it is proposed a
262
α
obstacle avoidance method; as i.e., (Campbell et al.,
2004), in which it was used a Canny edge detector
Δα = K j (0 ≤ K j ≤ R)
R
algorithm that consists in Gaussian filtering and xi , j = ±
H
tan (Δβ ) (9)
edge detection by using Sobel filters. Thus, optical cos (ϕ − α 2 + Δα )
flow was computed over the edges providing β
Δβ = K i (0 ≤ K i ≤ C 2)
obstacle structure knowledge. The present work C
assumes that the occupancy grid is obtained by the
The Ki and Kj are parameters used to cover the
machine vision system. It is proposed an algorithm
image pixel discrete space. Thus, R and C represent
that computes the local optimal desired coordinate as
the image resolution through the total number of
well as the local trajectory to be reached. The rows and columns. It should be noted that for each
research developed assumes indoor environments as row position, which corresponds to scene
well as flat floor constraints. However, it can be also coordinates yj, there exist C column coordinates xi,j.
applied in outdoor environments. The above equations provide the available local map
This section presents firstly the local map coordinates when no obstacle is detected. Thus,
relationships with the camera configuration and considering the experimental setup reported in
poses. Hence, the scene perception coordinates are Section 2, the local on-robot map depicted in Figure
computed. Then, the optimal control navigation 7 is obtained.
strategy is presented, which uses the available visual
data as a horizon of perception. From each frame, it
is computed the optimal local coordinates that
should be reached in order to achieve the desired
objective. Finally, the algorithm dealing with the
visual data process is explained. Some involved
considerations are also made.
3.1 Scene Perception

The local visual data provided by the camera are Figure 7: Local visual perception free of obstacles, under
used in order to plan a feasible trajectory and to 96x72 or 9x7 low resolution grids.
avoid the obstacle collision. The scene available
coordinates appear as an image, where each pixel 3.2 Local Optimal Trajectory
coordinates correspond to a 3D scene coordinates. In
the case attaining to this work, flat floor surface is The available information provided by the camera is
assumed. Hence, scene coordinates can be computed considered as a local horizon where the trajectory is
using camera setup and pose knowledge, and planned. Hence, a local map with free obstacle
assuming projective perspective. The Figure 6 shows coordinates is provided. In this sense, the available
the robot configuration studied in this work. The local coordinates are shown in Figure 7. It is noted
angles α, β and ϕ are related to the vertical and that low resolution scene grids are used in order to
horizontal field of view, and the tilt camera pose, speed up the computing process.
respectively. The vertical coordinate of the camera is The minimization of a cost function, which
represented by H. consists in the Euclidean distance between the
desired coordinates and the available local scene
coordinates, can be optimally solved by finding the
local desired coordinates. Hence, the algorithm
explores the image pixels, IMAGE(i,j), considering
just the free obstacle positions. Once the local
desired point is obtained, a trajectory between the
robot coordinates, at the instant when the frame was
acquired, and the optimal scene coordinates is
Figure 6: Fixed camera configuration including vertical planned. Thus, the current robot coordinates are
and horizontal field of view, and vertical tilt angle. related to this trajectory, as well as to control
Using trigonometric relationships, the scene methods.
coordinates can be computed:
y j = H tan(ϕ − α 2 + Δα ) (8)
263
3.3 Algorithms and Constraints coordinates are considered as final points, until not
any new optimal local desired coordinates are
In this subsection, some constraints that arise from provided. The image information is explored starting
the experimental setup are considered. The narrow at the closer positions, from bottom to upside. It is
field of view and the fixed camera configuration suggested to speed up the computing process based
make necessary that the robot stays oriented towards on a previously calculated LUT, (look up table),
the desired coordinates. WMR movements are with the scene floor coordinates corresponding to
planned based on the local visual data, and always in each pixel.
advancing sense. Hence, the algorithms provide
local desired coordinates to the control unit. If WMR 3.3.2 Local Minimal Failures
orientation is not appropriate, the robot could turn
around itself until a proper orientation is found. The local minimal failures will be produced when a
Another possibility is to change the orientation in convergence criterion, similar to that used in (7), is
advancing sense by the use of the trajectory/robot not satisfied. Thus, the local visual map cannot
orientation difference as the cost function computed provide with closer optimal desired coordinates,
over the available visual data. This subsection because obstacles blocks the trajectory to the goal.
proposes the local optimal suggested algorithms that In these situations, obstacle contour tracking is
have as special features an easy and fast proposed. Hence, local objectives for contour
computation. Some methods are presented in order tracking are used, instead of the goal coordinates, as
to overcome the drawback of local minimal failures. the source for obtaining a path until the feasible goal
trajectories are found. The Figure 8 shows an
3.3.1 The Proposed Algorithms example with local minimal failures. It is seen that
in A, the optimal trajectory is a straight line between
The proposed algorithm, concerning to obtaining the A and E. However, an obstacle is met at B, and local
local visual desired coordinates, consists of two minimal failure is produced at B. When this is
simple steps: produced, no trajectory can approach to the desired
To obtain the column corresponding to best goal, (Xd, Yd). Then, obstacle con-tour tracking is
optimal coordinates that will be the local desired proposed between B and C. Once C is attained, local
Xi coordinate. minimization along coordinates Y is found and the
To obtain the closer obstacle row, which will be trajectory between C and D is planned. From D to E
the local desired Yj coordinate. local minimums are reached until the final goal is
The proposed algorithm can be considered as a first achieved. It should be noted that once B is reached,
order approach, using a gross motion planning over the left or right obstacle contour are possible.
a low resolution grid. The obstacle coordinates are However, the right direction will bring the robot to
increased in size with the path width of the robot an increasing Yj distance.
(Schilling, 1990). Consequently, the range of
visually available orientations is reduced by the path
width of WMR. Other important aspects as visual
dead zone, dynamic reactive distance and safety stop
distance should be considered. The dynamic reactive
distance, which should be bigger than the visual
dead zone and safety stop distance, is related to the
robot dynamics and the processing time for each
frame. Moreover, the trajectory situated in the visual Figure 8: Example of local minimal failures produced at B
map should be larger than a dynamic reactive with A being the starting point and E the desired goal.
distance. Thus, by using the models corresponding
to the WMR PRIM, three different dynamic reactive The robot follows the desired goals except when the
distances are found. As i.e. considering a vision situation of obstacle contour tracking is produced,
system that processes 4 frames each second, using a and then local objectives are just the contour
model of medium speed (0.3m/s) with safety stop following points. The local minimal failures can be
distance of 0.25m and an environment where the considered as a drawback that should be overcome
velocity of mobile objects is less than 0.5m/s, a with more efforts. In this sense, the vision
dynamic reactive distance of 0.45m is obtained. navigation strategies (Desouza, Kak, 2002) should
Hence, the allowed visual trajectory distance will set be considered. Hence, it is proposed the use of
the speed that can be reached. The desired local feasible maps or landmarks in order to provide local
264
objective coordinates that can be used for guiding of each zone. It is considered the region that offers
the WMR to reach the final goal coordinates. better optimization, where the algorithm is repeated
for each sub-zone, until no sub-interval can be
found. Once the algorithm is proposed, several
simulations have been carried out in order to verify
4 LMPC ALGORITHMS the effectiveness, and then to make the
improvements. Thus, when only the desired
This section gives the LMPC algorithms by using coordinates are considered, the robot could not
the basic ideas presented in the Section 2. The arrive in the final point. Figure 10 shows that the
LMPC algorithm is run in the following steps: inputs can minimize the cost function by shifting the
To read the actual position robot position to the left.
To minimize the cost function and to obtain a
series of optimal input signals
To choose the first obtained input signal as the
command signal.
To go back to the step 1 in the next sampling
period
The minimization of the cost function is a
nonlinear problem in which the following equation
should be verified:
Figure 10: The left deviation is due to the greater left gain
f (α x + β y ) ≤ α f (x ) + β f ( y ) (10) of the robot.
It is a convex optimization problem caused by the
trigonometric functions used in (5), (Boyd, The reason can be found in (3), where the left motor
Vandenberghe, 2004). The use of interior point has more gain than the right. This problem can be
methods can solve the above problem (Nesterov, easily solved by considering a straight-line trajectory
Nemirovskii, 1994). Among many algorithms that from the actual point of the robot to the final desired
can solve the optimization, the descent methods are point. Thus, the trajectory should be included into
used, such as the gradient descent method among the LMPC cost function. The Figure 11 shows a
others, (Dennis, et al. 1996), (Ortega, et al. 2000). simulated result of LMPC for WMR obtained by
The gradient descent algorithm has been using firstly the orientation error as cost function
implemented in this work. In order to obtain the and then the local trajectory distance and the final
optimal solution, some constraints over the inputs desired point for the optimization. The prediction
are taken into account: horizons between 0.5s and 1s were proposed and the
The signal increment is kept fixed within the computation time for each LMPC step was set to
prediction horizon. less than 100ms, running in an embedded PC of
The input signals remain constant during the 700MHz. In the present research, the available
horizon is provided by using the information of local
remaining interval of time.
visual data. Thus, the desired local points as well as
The input constraints present advantages such like
the optimal local trajectory are computed using
the reduction in the computation time and the
machine vision information.
smooth behavior of the robot during the prediction
horizon. Thus, the set of available input is reduced to
one value. In order to reduce the optimal signal
value search, the possible input sets are considered
as a bidimensional array, as shown in Figure 9.
Figure 11: LMPC simulated results with a 45º trajectory.
Figure 9: Optimal interval search.

5 CONCLUSIONS
Then, the array is decomposed into four zones, and This paper has integrated the control science and the
the search is just located to analyze the center points robot vision knowledge into a computer science
environment. Hence, global path planning by using
265
local information is reported. One of the important DeSouza, G.N., Kak, A.C., 2002. Vision for Mobile Robot
aspects of the paper has been the simplicity, as well Navigation: a survey, PAMI, 24, 237-267.
as the easy and direct applicability of the Elfes, A., 1989. Using occupancy grids for mobile robot
approaches. The proposed methodology has been perception and navigation, IEEE Computer, 22, 46-
attained by using the on-robot local visual 57.
information, acquired by a camera, and the Fox, D., Burgard, W., and Thun, S., 1997. The dynamic
window approach to collision avoidance, IEEE Robot.
techniques of LMPC. The use of sensor fusion,
Autom. Mag. 4, 23-33.
specially the odometer system information, is of a
Gupta, G.S., Messom, C.H., Demidenko, S., 2005. Real-
great importance. The odometer system uses are not time identification and predictive control of fast
just constrained to the control of the velocity of each mobile robots using global vision sensor, IEEE Trans.
wheel. Thus, the absolute robot coordinates have On Instr. and Measurement, 54, 1.
been used for planning a trajectory to the desired Horn, B. K. P., 1998. Robot Vision, Ed. McGraw –Hill.
global or local objectives. The local trajectory Küne, F., Lages, W., Da Silva, J., 2005. Point stabilization
planning has been done using the relative robot of mobile robots with nonlinear model predictive
coordinates, corresponding to the instant when the control, Proc. IEEE Int. Conf. On Mech. and Aut.,
frame was acquired. The available local visual data 1163-1168.
provides a local map, where the feasible local Lju, L., 1989. System Identification: Theory for the User,
minimal goal is selected, considering obstacle ed., Prentice Hall.
avoidance politics. Maciejowski, J.M., 2002. Predictive Control with
Nowadays, the research is focused to implement the Constraints, Ed. Prentice Hall.
presented methods through developing flexible Nesterov, Y., Nemirovskii, A., 1994. Interior_Point
Polynomial Methods in Convex Programming, SIAM
software tools that should allow to test the vision Publications.
methods and to create locally readable virtual Norton, J. P., 1986. An Introduction to Identification, ed.,
obstacle maps. The use of virtual visual information Academic Press, New York.
can be useful for testing the robot under synthetic Nourbakhsh, I. R., Andre, D., Tomasi, C., Genesereth, M.
environments and simulating different camera R., 1997. Mobile Robot Obstacle Avoidance Via Depth
configurations. The MPC studies analyzing the From Focus, Robotics and Aut. Systems, 22, 151-58.
models derived from experiments as well as the Ögren, P., Leonard, N., 2005. A convergent dynamic
relative performance with respect to other control window approach to obstacle avoidance, IEEE T
laws should also be developed. Robotics, 21, 2.
Ortega, J. M., Rheinboldt, W.C., 2000. Iterative Solution
of Nonlinear Equations in Several Variables, Society
for Industrial and Applied Mathematics.
ACKNOWLEDGEMENTS Rimon, E., and Koditschek, D., 1992. Exact robot
navigation using artificial potential functions, IEEE
Trans. Robot Autom., 8, 5, 501-518.
This work has been partially funded the Commission
Schilling, R.J., 1990. Fundamental of Robotics, Prentice-
of Science and Technology of Spain (CICYT) Hall.
through the coordinated research projects DPI-2005- Thrun, S., 2002. Robotic mapping: a survey, Exploring
08668-C03, CTM-2004-04205-MAR and by the Artificial Intelligence in the New Millennium, Morgan
government of Catalonia through SGR00296. Kaufmann, San Mateo, CA.
REFERENCES
Boyd, S., Vandenberghe, L., 2004. Convex Optimization,
Cambridge University Press.
Campbell, J., Sukthankar, R., Nourbakhsh, I., 2004.
Techniques for Evaluating Optical Flow in Extreme
Terrain, IROS.
Coué, C., Pradalier, C., Laugier, C., Fraichard, T.,
Bessière, P., 2006. Bayesian Occupancy Filtering for
Multitarget Tracking: An Automotive Application,
International Journal of Robotics Research.
Dennis, J.E., Shnabel, R.S., 1996. Numerical Methods for
Unconstrained Optimization and Nonlinear Equations,
Society for Industrial and Applied Mathematics.
266
INTERNET-BASED TELEOPERATION: A CASE STUDY
Toward Delay Approximation and Speed Limit Module
Shengtong Zhong, Philippe Le Parc and Jean Vareille

Laboratoire Informatique des Systèmes Complexes (LISyC), Université de Bretagne Occidentale
20, avenue Le Gorgeu, 29285 Brest Cedex, France
{shengtong.zhong, philippe.le-parc, jean.vareille}@univ-brest.fr
Keywords: Remote control, teleoperation, mobile robot, Internet delay, path error, delay approximator, speed limit.
Abstract: This paper presents the internet-based remote control of mobile robot. To face unpredictable Internet delays
and possible connection rupture, a direct teleoperation architecture with “Speed Limit Module” (SLM) and
“Delay Approximator” (DA) is proposed. This direct control architecture guarantees the path error of the
robot motion is restricted within the path error tolerance of the application. Experimental results show the
effectiveness and applicability of this direct internet control architecture in the real internet environment.
1 INTRODUCTION turning/stop point is restricted within a path error

tolerance which depends on application itself.
Internet is not only an information highway, but also Two kinds of control strategy are introduced in
the Section 2, followed by the detail of “Speed Limit
a mean to remotely control mechanical systems, such
as robotic devices. But Internet doesn’t provide a Module” (SLM) with the quality level idea of
guaranteed Quality of Service (QoS); it entitles a GEMMA-Q (Ogor, 2001) and how it works together
with the “Delay Approximator” (DA) to meet the
number of limitation and difficulties, such as
bandwidth constraint, transmission delays, packet application requirements. During the Section 3, the
lost, connection rupture etc. The situation above software implementation is presented with an
application over Miabot (two-wheeled robot widely
influence the performance of Internet based
telerobotics systems, which is a new field in the used in soccer competition organized by FIRA).
recent decade.
The Mercury project (Goldberg et al., 1994) is
one of the earliest telerobotics implementation over 2 TELEOPERATION
the internet, then coming with the Telerobot in ARCHITECTURE
Australia (Taylor and Dalton, 1997), the painting
PumaPaint Robot (Stein, 1998), and Khepera robot
(Saucy and Mondada, 2000) etc. During the past ten 2.1 Generic Architecture
years, lots of such systems have been introduced by
different researchers all over the world. The teleoperation over Internet lies on a generic
Most of these researches use a supervisory teleoperation architecture (Figure 1), through which
control scheme which enables operator to issue high commands are exchanged between remote system
level commands. As the internet time delay is and operator (user). The user, through his Internet
unpredictable, the design of direct control scheme navigator, addresses a request to a Web Server (step
which enables user to control the motion of robot 1) and downloads an application, such as a Java
continuously may not be easy. Such control schemes applet (step 2), on his own workstation. A
have been proposed, but are not adequate to alleviate connection is then established towards the
the influence of Internet time delay. Application Server in charge of robots and client
This paper presents direct teleoperation management (step 3). In the same time, another
connection with Multimedia Server is also
architecture of a continuous robot motion control
which meets path error tolerance under the established in the form of exchanging media signals.
unpredictable Internet time delay. Here, the path The user is now able to control the remote robot
upon request.
error is guaranteed only if the path error at every
267
The generic architecture is the same in most Where

applications. The key problem is to alleviate the v indicates the current speed of the robot, Δt is
influence of Internet time delay and towards a the current Internet time delay between sending the
continuous control within the path error requirement. command from the user and executing the command
by robot.
As Internet doesn’t provide a guaranteed Quality
of Service (QoS), the time delay is unpredictable.
Figure 2 is an execution result of the task A → E
with “Speed control” strategy. The path error is
accumulative, and path deviation is significant which
may result in the failure of the task.
Figure 1: Generic architecture.
2.2 Control Strategies

2.2.1 “Move and Wait” Strategy
The “ Move and Wait” strategy (Sheridan, 1992), Figure 2: A path result with “Speed Control”.
which is typical for space robots with long distance
communication has been applied first. In the Miabot 2.3 Speed Limit Module and Delay
case, five commands: “move forward” (MF), “move
backward” (MB), “turn left” (TL), “turn right” (TR) Approximator
and “stop” (ST) have been defined. These commands
are enough to perform any complex task, but the user 2.3.1 Speed Limit Module (SLM)
has to send lots of commands and change the move
or turn parameters from time to time to meet the The GEMMA-Q (Ogar, 2001) methodology derived
requirement. from GEMMA (ADEPA, 1981) has been proposed.
The basic idea is to define the quality of
2.2.2 “Speed Control” Strategy communication as a discrete value. Following the
value, the automated system has to follow different
To reduce user’s workload and towards a fluent rules to ensure the security (path error). According to
process during the complex task, a “Speed control” the type of quality, these rules might be different. If
strategy, similar to real driving, is introduced. “No-
the user has a slightly deteriorated communication
stop move forward” (NSMF), “no-stop move
backward” (NSMB), “turn left” (TL), “turn right” quality, the user might keep the control and the
(TR) and “stop” (ST) commands are defined in this automated system has to work in deteriorated mode.
strategy. When the robot executes a turning The GEMMA-Q has been used in the application
command, it stops running first, perform the entire over robot arm and prototype machine (Le Parc et al.,
turning next, and then runs in previous speed again. 2001). In this paper, which aims to control a simple
The user may choose different running speed as well robot, use only the quality level idea of GEMMA-Q
as turning degree. to design SLM. n communication quality levels are
The user’s operation reduce is significant. The introduced which corresponds Internet time delay
robot is running with speed v during the whole task zone (Time D.). Each time delay zone has its own
even though there is a turning/stop point. When the sub path error tolerance Δε j (Sub T.) and Max
user sends a turning/stop command to robot, the
Speed v j as shown in the Table 1.
command reaches the robot with a discrete time
delay Δt due to Internet time delay. The robot may
run an extra distance Δd before it performs the
turning/stop.
Δ d = v × Δt
268
INTERNET-BASED TELEOPERATION: A CASE STUDY - Toward Delay Approximation and Speed Limit Module
Table 1: Quality levels in SLM. actual Internet time delays. The worst case is
from Q0 to Qn :
Qual. Time D. Sub T. Max Speed
Q0 0 ~ t0 Δδ 0 v0 = Δδ 0 / t0
Δδ1 Δ d = Δ δ 0 + Δ δ 1 + " + Δδ n
Q1 t0 ~ t1 v1 = Δδ1 /(t1 − t0 )
Q2 t1 ~ t2 Δδ 2 v2 = Δδ 2 /(t2 − t1 ) n
Q3 t 2 ~ t3 Δδ 3 v3 = Δδ 3 /(t3 − t2 ) = ∑ Δδ j
j =0
... ... ... ...
Qn−1 tn−1 ~ tn Δδ n−1 vn−1 = Δδn−1 /(tn −tn−1) ≤δ
Qn ≥ tn 0 vn = 0 The path error Δd is within the restriction of
When the quality level changes, the Application path error tolerance δ in both situations. Then the
Server evaluates the Current Robot Speed (CRS) and path error at every turning/stop point in the task is
the Max Speed of this quality level (MSoT). If guaranteed and the successfully continuous control
CRS ≤ MSoT, no command is sent to robot; else the of the robot is achieved.
Application Server sends MSoT command to robot With the constraints, the different quality levels
(change CRS to MSoT). with its respective time delay zone value, sub
MSoT is calculated by the time delay zone and tolerance and max speed are defined according to
sub tolerance which are defined by user according to different application. e.g.: the normal quality level of
application. In order to meet the path error tolerance Internet time delay is Q j in the application, its sub
δ at the turning/stop point, the following constraints tolerance Δδ j should take a larger percentage of
are used when designing SLM: δ . It means the robot is preferred to have larger sub
tolerance in the normal quality level; in the same
⎧ n time, speed limit rules guarantees Δd ≤ δ in any
⎪
⎨
∑j =0
Δδ j ≤ δ situation.
⎪v j +1 ≤ v j j ∈ (0,1, " n − 1)
⎩
The above constraint guarantees the path error at
the turning/stop point is restricted within δ in any
Internet time delay situation. Q0 is the best quality
level, its Max Speed v0 is the fastest running speed
of the robot. Qn is the disconnection situation, and
the robot stops running immediately (vn = 0) . The
proof of how the constraint works are shown below.
There are two kinds of situation when the robot Figure 3: A path result using “Speed Control” with SLM.
runs under the speed limit rules.
1. Stable network delay In Figure 3, the path deviation at every turning/stop
It means the robot is running in the same time point is restricted within δ . Then a successful task
delay quality level Qi between two continuous is achieved using “Speed control” with SLM. The
actual Internet time delay (from the real clock above describes the detail SLM with the quality level
time of receiving previous actual Internet time idea, next part emphasize on how the gets the quality
delay to the real clock time of receiving next level information of current time delay from DA.
actual Internet time delay). Then
2.3.2 Delay Approximator (DA) Principals
Δ d = vi × t i
In the generic architecture, Pinger-Ponger is the
≤ v0 × t 0 + v1 × (t1 − t 0 ) + " + vi (t i − ti −1 ) mechanism to provide the Internet time delay
= Δδ 0 + Δδ 1 + " + Δδ i information to DA. The next actual Internet time
delay Δt n is calculated as follows:
i
= ∑ Δδ j Δt n = Tn − T p
j =0
≤δ Where
T p is the real clock time of receiving previous
actual Internet time delay from Pinger-Ponger; Tn is
2. Unstable network delay the real clock time when Pinger-Ponger sends the
The robot is running in the different time next Internet time delay to DA.With above feature,
delay quality levels between two continuous
269
the current (up-to-now) time delay Δt c from T p is all the users, but certain SLM works fine for some
formed as: users. The test is still on-going currently. The
⎧ Δtc = t j j=0 average Internet time delay of the user is able to
⎨ observe during the test, so next step consideration is
⎩Δtc = t j − t j −1 j ∈ (1,2, " n) to build SLM for respective user group
(distinguished by average Internet time delay. e.g.:
t j is the time delay zone in SLM. Δtc is the Asia, Europe, etc.) and the system can choose
watchdog and j is the automatic counter initialized different SLM automatically due to the user group.
with 0. Δtc is only activated when there is quality
level change, and this quality level change is used as
current time delay change information. 4 CONCLUSION
Pinger-Ponger only provides the Internet time
delay when it gets one. The Internet time delay is In this paper, the SLM and DA based architecture is
unpredictable, so there is no idea when Pinger- proposed to face the unpredictable Internet delay in
Ponger gets new information. Internet-based robot control. This approach
There is no idea about the future, but the current guarantees the path error of the continuous robot
situation is supervised by DA as following principals: motion. Here, the path error is guaranteed only if the
1. DA receives information from Pinger-Ponger. path error at every turning/stop point is within a path
Pinger-Ponger informs DA of receiving the error tolerance δ which depends on application
next actual Internet time delay, and then DA itself.
forwards the quality level of calculated Δt n to In this architecture, the current time delay is
SLM. Meanwhile the previous parameters are set supervised by DA; and SLM applies different speed
to be the current ones: the value of T p is set limit rules according to the current time delay
to Tn , the quality level of previous actual situation. Then the robot is always running in a
Internet time is changed to the quality level of proper speed which meets the path error restriction.
Δt n , reset the watchdog (Δtc ) . Finally, a continuous control of the internet-based
2. No information from Pinger-Ponger and Δtc robot is achieved successfully.
is activated to a quality level change.
Δt p is the previous actual Internet time delay.
There are two kinds of situations:
• Δ t c ≤ Δt p REFERENCES
The quality level of current time delay is
no worse than that of previous actual time Andreu, D., Fraisse, P., Roqueta, V., Zapata, R., 2003.
delay; there is no action and DA keeps Internet enhanced teleoperation toward a remote
supervising. supervised delay regulator. ICIT'03, IEEE Int. Conf. on
• Δt c > Δt p Industrial Technology, Maribor.
The quality level of current time delay Ogor, P., 2001. Une architecture générique pour la
situation is worse than that of previous actual supervision sure à distance de machines de production
Internet time delay. When Δtc is activated, it
avec Internet. Ph.D. thesis, Université de Bretagne
Occidentale.
indicates a change of quality level and “DA” ADEPA, 1981. GEMMA: Guide d’études des modes de
forwards the new quality level to SLM. DA marche et d’arrêt.
keeps supervising. Meng Wang, and James N.K. Liu, 2005. Interactive
DA keeps supervising the Internet delay situation control for Internet-based mobile robot teleoperation.
all the time. It provides the real Internet time delay Robotics and Autonomous Systems, Volume 52, Issues 2-
or current time delay to SLM. 3, 31 August 2005, Pages 160-179.
K.-H. Han, J.-H. Kim, 2002. Direct Internet Control
Architecture for Personal Robot. Proc. of the 2002 FIRA
Robot World Congress, pp. 264-268, Seoul.
3 EXPERIMENT P. Ogor, P. Le Parc, J. Vareille et L. Marcé, 2001. Control
a Robot on Internet. 6th IFAC Symposium on Cost
The remote control system server has been built in Oriented Automation, Berlin.
UBO, France. Some users (mainly from France, J. Vareille, P. Le Parc et L. Marcé, 2004. Web remote
Sweden, and China) are invited to test the feasibility control of mechanical systems: delay problems and
of effectiveness of the system. The average Internet experimental measurements of Round Trip Time. 2nd
Workshop CNRS-NSF Applications of Time-Delay
time delay varies a lot between European users and
systems, Nantes.
Asian users. It’s not easy to find a uniform SLM for
270
OPTICAL NAVIGATION SENSOR
Incorporating Vehicle Dynamics Information in Mapmaking
Tibor Takács and Viktor Kálmán

Department of Control Engineering and Information Technology, Budapest University of Technology and Economics
Magyar Tudósok körútja 2.,Budapest, Hungary
Keywords: Mobile robot navigation sensor, vehicle dynamics, map making.
Abstract: Accurate odometry and navigation may well be the most important tasks of a mobile robot’s control system.
To solve this task it is necessary to utilize proper sensors which provide reliable information about the
motion. This paper presents the prototype of an optical navigation sensor which can be an alternative choice
for the dead reckoning navigation system of a vehicle or mobile robot. The last part of the paper presents
another application in an inertial navigation system that enables a new approach to map making which
incorporates vehicle dynamics into the world map.
1 INTRODUCTION previous position using known course, velocity and

time information (Borenstein, 1996).
Autonomous mobile robots gain more and more The odometry (the most simplistic and prevalent
importance in automotive, domestic and military implementation of dead reckoning) estimates the
applications. Sensing and interpreting their course and distance of the moving agent’s path to
surroundings correctly are crucial in all these areas. calculate the global location by measuring the wheel
The sensor presented in this paper aims to improve rotation and/or the steering orientation (Dixon,
existing odometry methods, and to aid the creation 1997). This position estimating can be strongly
of world maps that incorporate the state of the inaccurate under real conditions through the
vehicle in the world map and classifies terrain mobility configuration (e.g. tracked vehicles) or
accordingly. In the first half of this document an through the wheel-slippage, overacceleration or
overview of the principle and the prototype of the driving over uneven floor.
optical navigation sensor are presented, the second
section of the paper proposes a method for world
model building for mobile robots and shows some 3 OPTICAL NAVIGATION SENSOR
other application where this sensor can be used.
3.1 Optical Flow
2 NAVIGATION TECHNIQUES Through of the inaccuracy of the odometry it is

necessary to develop a cheap sensor-system that
Accurate self localization is one of the most provides well-authenticated dislocation-data for
important topics in autonomous vehicle technology. dead reckoning navigation in real world conditions.
During centuries researchers have developed a lot of The optical navigation sensor is a possible solution
techniques to measure the precise position of land or to fulfil these conditions.
aerial vehicles and mobile robots. The working principle of the sensor is optical
Global methods (e.g. GPS) determine directly the flow, namely the motion information is generated
absolute position, but unfortunately in several cases from visual information (from an image-sequence
these are not useable, due to reception problems. provided by a camera facing the ground). Typically
Dead reckoning methods estimate the present the motion is represented as vectors originating or
location of a navigating agent by advancing some
271
terminating at pixels in a digital image sequence

(CVRG, 2000). An example is shown in figure 1.
a.) b.) c.) d.) e.)

Figure 1: The principle of the optical flow. From above From under
3.2 The Prototype of the Sensor Figure 3: The prototype of the sensor.
As a basis for further research, we have built the 3.3 Testing, Error Measurement
prototype of the optical navigation sensor (see figure
2 and figure 3). To measure the errors of the sensor we mounted it
As a camera (what provides the image-sequence on a test platform (figure 3/a) and performed some
for calculating the dislocation) we have chosen the experiments.
sensing and processing chip of a low-cost and wide- The first group of tests analyzes the dependence
spread optical mouse. (Hereinafter this chip is from the texture of the floor and from the height
referred to as “mouse-chip”.) The chip includes a over ground. The experience showed that the sensor
low-resolution camera and a DSP for calculating the is really insensitive to the height (thanks to the
optical flow by hardware at a very high rate. telecentric optics) but unfortunately it sometimes
Although its resolution is relatively low, for motion misses on shining or totally homogeneous surfaces.
detection and measurement it is sufficient because This property is inherited from the mouse-chip of
only the texture is important. It measures the relative the cheap optical mice. Laser equipped optical mice
dislocations in the x and y direction. might be better from this aspect. Therefore using the
mouse-chip of a laser mouse presumably solves this
Mouse-chip Microcontroller Control problem.
unit With the second type of the tests we measured
Telecentric Optics the accuracy of the sensor. First we determined the
model of the platform then we executed the so called
LEDs
unidirectional square-path test (Borenstein, 1996).
The model of the navigating agent and the
Figure 2: The model of the sensor. connection between the motion and the measured
sensor values is shown on figure 3/b. The y
Because of the uneven floor the distance between component measures orientation change, the linear
the sensor and the floor is continuously changing. To dislocation appears in the value of the x component.
compensate this effect the sensor has telecentric
optics, which has a constant magnification rate
dy
therefore in a certain range the measurements are
dx
independent from the height of the sensor relative to
A
the ground.
Naturally, it was necessary to design a D
microcontroller based architecture round the mouse-
chip which offers a useable interface for a dead a.) b.)
reckoning navigation system. The microcontroller
reads the motion or the image information of the Figure 3: The test platform and its model.
mouse-chip and sends them to the processing unit,
for example to a PC in the development stage or to a By normal conditions the measured error was
navigation or control unit at real conditions (Takács, relatively small in the course of movement around
2007). the square. 4,5 meters of travel yielded 0,01-0,02
meters of the inaccuracy (meaning 0,3% overall
error).
272
OPTICAL NAVIGATION SENSOR - Incorporating Vehicle Dynamics Information in Mapmaking
It is important to add that slip free wheel contact 4.2 Linking Vehicle Dynamic Properties
was supposed. In case of slippage the error grew into World Map Building
dramatically, the inaccuracy could even reach the
0,3-0,4 meters (7% error). It was caused by the Another interesting area of application is a multi
appearance of linear dislocation in the value of the y sensor navigation unit of a mobile robot or
component, and the ratio between the linear automated vehicle. By fusing information from the
dislocation and the orientation change can not been optical correlation sensor and other sensors
determined. measuring the vehicles inner state - such as
It seems to be a paradox situation since this acceleration, tilt, wheel speed, GPS sensors - very
optical navigation sensor was developed for precise dynamical information can be obtained. This
replacing the wheel encoder based odometry, information can be used to build a world map that
however we faced the same problem (the inaccuracy contains information not only about the environment
from the slippage of the wheels). But as opposed to but also its effects on the dynamical behaviour of the
the odometry, out experiences showed, that this robotic vehicle. The optical sensor plays an
method has a simple and effective solution. By using important role in this unit because it is able to
two optical sensors, a platform independent an measure true ground velocity relative to our vehicle,
accurate dead reckoning navigating sensor-system making us able to determine wheel slippage and slip
can be developed that works well under real angle.
conditions (wheel-slippage, travel over uneven floor Intelligent mobile robots navigate around in their
etc.) too. environment by gathering information about their
surroundings. The most common approach is to use
ranging sensors mounted on the robot to form
4 ALTERNATIVE APPLICATIONS occupancy grids or equivalent. Other approaches
avoid this metric division of space and favour
4.1 Electronic Stability Control of topological mapping. By combining these mapping
techniques it is possible to form a hierarchical map
Vehicles that has the advantages of both methods while some
of the disadvantages can be avoided (Thrun, 1998).
One of the most critical areas of stability systems
Occupancy grids classify the individual cells
(ESP, ABS etc.) is the accurate measurement of the
based on range data and possibly other features such
dynamic state of the vehicle. Probably the most
as colour or surface texture or variation. This
important state variable is the slip angle of the
becomes very important in outdoor mobile robotics
moving vehicle. It is needed for the calculation of
when the robot needs to distinguish between real
the force between the wheels and the ground and
obstacles and traversable terrain. An extreme case is
also for the exact description of the motion-state.
given by navigation in a field of tall grass. The
Presently used measurement procedures of the
elevation map will represent the scene as a basically
slip angle are in general very expensive therefore it
horizontal surface above the ground level; that is, as
they are not really usable in mass production
a big obstacle in front of the vehicle. It is apparent
(Kálmán, 2005). Hence it is necessary to use
that only by integrating the geometry description
estimation methods to provide the value of the slip
with terrain cover characterization will a robot be
angle. Unfortunately these procedures can not
able to navigate in such critical conditions (Belluta,
provide accurate results (Bári, 2006).
2000).
This optical sensor provides an effective and
Topological maps describe the world in terms of
cheap solution to solve the problem of the slip angle
connections between regions. This is usually enough
because it can measure (instead of estimation)
indoors, or in well structured environments, but
directly the slip angle, the ground velocity in 2D and
when travelling through more complex terrain a
other important motion information. Therefore it can
different representation might be necessary. For
be an additional sensor of any vehicle handling
example a sloping gravel road or sand dune may
enhancement system, for example ABS or ESP.
only be traversable at a certain speed or only one
Naturally for utilization in the automotive industry it
way, up or downwards. By applying information
is necessary to fulfil numerous standards and
from the inertial navigational unit, such as slope
reliability requirements, which will not be discussed
angle, wheel slippage, actual movement versus
in this paper.
desired movement, these characteristics can be
learned (or used from apriori information) and the
273
connections of the topological graph can be updated experimental setup. Experimental results are given
accordingly. in the last part. Conclusions: the current system
Terrain characteristics (and those of our vehicle) should be made platform independent by using two
determine the maximum safe speed, braking distance rigidly linked sensors, use of laser mouse chips is
curve radius at a given speed, climbing manoeuvres recommended to overcome the problem of texture
etc. It is obvious that the more information we have dependency.
about a certain region we are planning to travel Finally examples of alternative application areas
through, the more driving efficiency we can achieve, were presented: slip angle measurement for the
as it is generally unsafe to drive at high speed safety systems of vehicles and hierarchical map
through bumpy terrain or make fast turns on a building with additional driveability information.
slippery surface. By incorporating the data from the As a conclusion we can say that the device and
navigational unit into the world map, we can dependent methods presented here can serve as
associate driving guidelines to a given map segment. cheap and accurate alternative solutions to numerous
Also on the higher, topological level - using apriori problems of the robot and automotive industry.
information - we can identify the type of the terrain
for a given point of our topological graph, as office
environment, forest, urban area, desert etc. By doing REFERENCES
so, we narrow down our choices when making
decisions about terrain coverage. For example it is Borenstein, J., Everett, H. R., Feng, L., 1996. “Where am
unlikely to encounter sand, water or foliage in an I?” Sensors and Methods for Mobile Robot
office environment. If we know the type of terrain Positioning, 13-29., 130-150.
ahead we can make a more accurate estimate of the Takács, T., 2007. Alternative Path-Tracking Sensor-
driveability of the area thus increasing driving System for Mobile Robots, microCAD – International
efficiency. Scientific Conference.
In this section a hierarchical map making method Bári, G., Kálmán V., Szabó, B., 2006. Járműdinamikai
was proposed which uses data from a multi-sensor állapotbecslő algoritmus fejlesztése. A jövő járműve –
Járműipari innováció, BME-EJJT, in Hungarian.
navigation unit that supplies information about
Kálmán, V., 2005. Elemzés az "optical flow"
vehicle dynamics. This unit heavily relies on the (~képfolyam) elven működő jármű és mobil robot-
optical correlation sensor described in the preceding technikai érzékelők kínálatáról, in Hungarian.
sections. By measuring wheel slip and vehicle slip Dixon, J., Henlich, O., 1997. Mobile Robot Navigation,
angle we are able to associate drivability guidelines SURPISE. Retrieved from
such as safe speed, friction coefficient, minimal http://www.doc.ic.ac.uk/~nd/surprise_97/journal/vol4/
driving speed etc. to a given map segment or type of jmd/
terrain. A higher level of environment recognition Horn, B.K.P., Schunck, B.G., 1980. Determining optical
was also proposed: based on apriori information, or flow. AI Memo 572. Massachusetts Institue of
Technology.
sensor data the vehicles control system decides the
Thrun, S., 1998. Learning maps for indoor mobile robot
type of environment (e.g. office, forest, desert) the navigation. Artificial Intelligence, 99(1): 21–71..
robot traverses at the time, and changes the Belluta, P., Manduchi, R., Matthies, L., Owens, K.,
probability of terrain types, characteristic of the type Rankin, A., 2000. Terrain Perception for Demo III.
of environment, thus simplifying terrain Proceedings of the Intelligent Vehicles Symposium.
classification. Dearborn, Michigan.
5 SUMMARY
In the previous sections we presented an optical
navigation sensor which measures motion
information (velocity, dislocation, slip angle)
without ground contact.
An alternative dead reckoning technique is
proposed in section 3, that yields superior accuracy
compared to wheel encoder based methods. In the
first part a short overview of principle (e.g. optical
flow) is given followed by the description of our
274
VISION-BASED OBSTACLE AVOIDANCE FOR A SMALL,
LOW-COST ROBOT
Chau Nguyen Viet and Ian Marshall

Computer Science Department, University of Kent, Canterbury, United Kingdom
Keywords: Obstacle-avoidance, robot vision.
Abstract: This paper presents a vision-based obstacle avoidance algorithm for a small indoor mobile robot built from
low-cost, and off-the-shelf electronics. The obstacle avoidance problem in robotics has been researched ex-
tensively and there are many well established algorithms for this problem. However, most of these algorithms
are developed for large robots with expensive, specialised sensors, and powerful computing platforms. We
have developed an algorithm that can be implemented on very small robots with low-cost electronics and
small computing platforms. Our vision-based obstacle detection algorithm is fast and works with very low
resolution images. The control mechanism utilises both visual information and sonar sensor’s measurement
without having to fuse the data into a model or common representation. The robot platform was tested in an
unstructured office environment and demonstrated a reliable obstacle avoidance behaviour.
1 INTRODUCTION rithm can process more than 500 frames of 87 ∗ 44

pixels per second. The vision algorithm presented in
Obstacle avoidance is one of the most fundamental (Lenser and Veloso, 2003) uses 3 array access opera-
problems in the field of mobile robotics. Despite the tions and an AND bitwise operations for each pixel.
problem being studied extensively, a reliable obstacle Our algorithm uses only one array access operation.
avoidance behaviour in a dynamics and unstructured Lugino and her group developed an algorithm that can
environment, i.e. an environment that is not modi- work with low resolution image 64*64 pixels frame in
fied specifically to suit the robot, is still very hard (Lorigo et al., 1997). Our algorithm works with even
to achieve especially for small robots. Vision can lower resolution of 22*30 pixels frame. This reduces
be used to detect obstacles and one of the developed the computing cycle required for the vision algorithm
class of algorithms is based on colour-based terrain and enables our algorithm to run on embedded com-
segmentation (Lorigo et al., 1997; Lenser and Veloso, puting devices. Our robot is small, less than a kilo,
2003; Ulrich and Nourbakhsh, 2000). If we can as- and energy efficient; it is powered by AA batteries.
sume that a robot with a digital camera pointing for- We present both the vision algorithm and the robot
ward is operating on a flat surface and all objects have design.
their bases on the ground, then the distance from the Due to the camera’s narrow field of view (FOV),
robot to an object is linear to the y-axis coordinate two sonar sensors were added to expand the robot’s
of the object’s appearance in the perceived image. FOV. The control mechanism is reactive, it has no
We have developed a similar algorithm that utilises memory and acts upon the most current sensor read-
a low resolution digital camera and a low powered ings only. This allows the robot to respond quickly to
micro processor. What makes our algorithm different changes in the environment. The approach we used
from existing algorithms is the use of a lookup map is inspired by the subsumption architecture (Brooks,
for colour classification and a reduced colour space. 1985) and Braintenberg vehicles (Braitenberg, 1984).
Lookup map is a very fast classification method. On The obstacle avoidance algorithm might be used as a
a Gumstix computer clocks at 200 MHz, our algo- module in a more complex system e.g. the first level
275
of competence in a subsumption architecture. It can

be used on it own in applications such as exploration,
surveillance. Because only a small fraction of the
CPU is required for obstacle avoidance, more spaces
are available for complex behaviours.
This paper is organised as follows. In section II, A B
we present the vision algorithm and control mecha-
nism. Section III describes the hardware configura-
tion and software implementation. The experiments
and results are reported in section IV.
C D
2 VISION AND CONTROL Figure 1: The images processed by the vision module. A
ALGORITHM is a full resolution colour image. B is the binary obstacle-
ground image of A. C is a low resolution image from A and
D is its corresponding binary image.
2.1 Ground and Obstacles
Segmentation
the size of lookup table from 224 elements to 212 ele-
In our classification algorithm, pixels are classified ments. Effectively, we make the classifier more gen-
according to their colour appearance only. The colour eral since each element of the reduced table represents
space we use is the RGB colour space. Each colour a group of similar colours in the original space. We
in the colour space is set to be a ground or obsta- also use very low resolution images of 22 ∗ 30 pixles.
cle colour. This classification information is stored Fig. 1 has two examples of the outputs from this seg-
in a binary lookup map. The map is implemented as mentation procedure. At the top row is a picture taken
a three dimensions vector of integers. To classify a from the camera mounted on our robot at the max-
pixel, its RGB components are used as indices to ac- imum resolution and the binary image produced by
cess the class type of the pixel. The classification pro- the segmentation procedure. At the bottom row is the
cess is very fast since for each pixel only one array down-sampling version of the top row picture and its
lookup operation is needed. corresponding binary image.
The lookup map is populated from example pic- The output of the image segmentation is a binary
tures of the ground. First, the algorithm counts the image differentiating obstacles from the ground. As-
number of pixels of each colour in the example pic- suming all objects have their bases on the ground,
tures. Then if the number of pixels of a colour is the distance to an object is the distance to its base.
more than 5% of the total number of pixels in those This distance is linear to the y-coordinate of the edge
pictures, that colour is set to be a ground colour. The between the object and the ground in the binary im-
5% threshold is used to eliminate noises in the image. For obstacle avoidance, we only need the dis-
ages. Procedure 1 describes this calibration process. tance and width of obstacles but not their height and
A lookup map is also very efficient for modification. depth. Therefore a vector of distance measurements
At the moment, the calibration process is done once to the nearest obstacles is sufficient, we call this ob-
before the robot starts moving and the lookup map stacle distance vector (ODV). We convert the binary
remains unchanged. We anticipate that the colour ap- image to the required vector by copying the lowest y-
pearance of the ground and the lightning condition are coordinate of a non-floor pixel in each column to the
likely to change if the robot operates for a long period corresponding cell in the vector. Each element of the
or moves into different environments therefore any vector represents the distance to the nearest obstacle
classification technique is required to adapt to these in a specific direction.
changes. In the near future, we plan to implement an
on-line auto-calibrating algorithm for the vision mod- 2.2 Control Algorithm
ule. Procedure 2 describes how a pixel is classified
during the robot’s operation. The control algorithm we adopted is reactive, deci-
In a constrained platform the amount of memory sions are made upon the most recent sensory readings.
needed to store the full 24 bits RGB space is not avail- The inputs to the controller are the obstacle distance
able. To overcome this problem, we reduce the origi- vector, produced by the visual module, and distance
nal 24 bits RGB colour space to 12 bits and decrease measurements from the two sonar sensors pointing at
276
A B
Procedure 1 PopulateLookupMap ( n: number of
pixels , P : array of n pixels.
for i = 0 to n − 1 do
(R, G, B) =⇐ rescale(P[i]r , P[i]g , P[i]b ) 00000000000
11111111111 000000000000000
111111111111111
0000000000
1111111111
00000000000
11111111111
0000000000000000
1111111111111111 000000000000000
111111111111111
0000000000
1111111111
000000000000000
00000000000
11111111111 111111111111111
0000000000
1111111111
pixel counter[R][G][B] ⇐ 0000000000000000
1111111111111111
00000000000
11111111111
0000000000000000
1111111111111111
00000000000 E
11111111111
0000000000000000
1111111111111111
000000000000000
111111111111111
0000000000
1111111111
000000000000000
111111111111111
0000000000
F 1111111111
00000000000
11111111111
0000000000000000
1111111111111111 000000000000000
111111111111111
0000000000
1111111111
000000000000000
111111111111111
0000000000
1111111111
pixel counter[R][G][B] + 1 00000000000
11111111111
0000000000000000
1111111111111111
00000000000
11111111111
0000000000000000
1111111111111111
00000000000
11111111111
0000000000000000
1111111111111111
000000000000000
111111111111111
0000000000
1111111111
000000000000000
111111111111111
0000000000
1111111111
00000000000
11111111111
0000000000000000
1111111111111111 000000000000000
111111111111111
0000000000
1111111111
000000000000000
00000000000
11111111111 15cm
0000000000000000
1111111111111111 111111111111111
0000000000
1111111111
end for 00000000000
11111111111
0000000000000000
1111111111111111
00000000000
11111111111
0000000000000000
1111111111111111
000000000000000
111111111111111
0000000000
1111111111
000000000000000
111111111111111
0000000000
1111111111
00000000000
11111111111
0000000000000000
1111111111111111 000000000000000
111111111111111
0000000000
1111111111
000000000000000
for (R,G,B) = (0,0,0) to MAX(R,G,B) do 00000000000
11111111111
0000000000000000
1111111111111111 111111111111111
0000000000
1111111111
000000000000000
111111111111111
00000000000
11111111111
0000000000000000
1111111111111111 0000000000
1111111111
000000000000000
111111111111111
0000000000
00000000000
11111111111
0000000000000000
1111111111111111
D 15cm C 1111111111
000000000000000
111111111111111
0000000000
1111111111
0000000000000000
1111111111111111 000000000000000
111111111111111
is ground map[R][G][B] ⇐ 0000000000000000
1111111111111111
0000000000000000
1111111111111111 000000000000000
111111111111111
000000000000000
111111111111111
0000000000000000
1111111111111111
0000000000000000 000000000000000
111111111111111
1111111111111111 000000000000000
111111111111111
pixel counter[R][G][B] > n ∗ 5% 0000000000000000
1111111111111111
0000000000000000
1111111111111111 000000000000000
111111111111111
000000000000000
111111111111111
0000000000000000
1111111111111111 000000000000000
111111111111111
0000000000000000
1111111111111111 000000000000000
111111111111111
end for 0000000000000000
1111111111111111
0000000000000000
1111111111111111 000000000000000
111111111111111
000000000000000
111111111111111
0000000000000000
1111111111111111
0000000000000000 000000000000000
111111111111111
1111111111111111 000000000000000
111111111111111
return is ground map 0000000000000000
1111111111111111
0000000000000000
1111111111111111 000000000000000
111111111111111
Figure 2: A visualisation of the monitored area. ABCD :

Procedure 2 is ground(p : pixel ).
the area captured by the camera. Shaded areas represent
(R, G, B) ⇐ rescale(pr , pg , pb ) the sonar sensors views. Segment EF is the trigger distance
return is ground map[R][G][B]; line.
the sides of the robot. The ODV gives a good resolu- 3 PLATFORM CONFIGURATION
tion distance map of any obstacle in front of the robot. AND IMPLEMENTATION
Each cell in the vector is the distance to the nearest
obstacle in a direction of an angle of about 2.5◦ . The The robot control software runs on a Gumstix (gum,
angular resolution of the two sonar sensors are much ), a small Linux computer that has an Intel 200 MHz
lower. So the robot has a good resolution view at the ARM processor with 64 Mb of RAM. The vision sen-
front and lower at the sides. The controlling mecha- sor is a CMUcam2 module connected to the Gumstix
nism consists of several reflexive rules. via a RS232 link. A Brainstem micro-controller is
used to control sonar sensors and servos. The robot
• If there are no obstacles detected in the area mon- is driven by two servos. These electronic devices are
itored by the camera, run at maximum speed. mounted on a small three wheeled robot chassis. The
total cost of all the components is less than 300 US
• If there are objects in front but further than a trig-
dollars. The robot can turn on the spot with a small
ger distance, slow down.
radius of about 5 cm. Its maximum speed is 15cm/s.
• If there are objects within the trigger distance, The robot is powered by 12 AA batteries. A fully
start to turn to an open space. charged set of batteries can last for up to 4 hours.
Fig. 2 shows the area in front of the robot that is
• If a sonar sensor reports a very close object, within monitored by the robot’s sensors. The CMUcam is
5 cm, turn to the opposite direction. mounted on the robot pointing forward at horizontal
level,15 cm above the ground, and captures an area of
The control algorithm does not calculate how far the about 75cm2 . Because the camera has a relatively nar-
robot should turn. It will keep turning until the area row FOV of about 55◦ , the two sonar sensors on the
in front is clear. The robot looks for an open space side are needed. In total, the robot’s angle of view is
by first looking in the opposite direction to the per- 120◦ . The robots dimensions are 20cm ∗ 20cm ∗ 15cm.
ceived obstacle, if the half image in that side is free The hardware configuration was determined by the
of obstacles, the robot will turn to this direction. If trial and error method. The parameters we presented
there are obstacles in both left and right half of the here were the best that we found and were used in the
image, the two measurements from sonar sensors are experiments reported in section IV.
compared and the robot will turn to the direction of The maximum frame resolution of the CMUCam2
the sonar sensor that reports no existence of obstacles is 87 ∗ 144 pixels, we lower the resolution to only
or a biger distance measurement. There is no attempt 22 ∗ 30 pixels. We only need the first bottom half
to incorporate or fuse data from the camera and sonar of the picture so the final image has dimensions of
sensors together into a uniformed representation. The 22 ∗ 15. In our implementation, the obstacle distance
algorithm uses the sensor readings as they are. vector has 22 cells, each cell corresponds to an angle
277
of 2.5◦ . The vector is then outputted to the controller.

Since the control algorithm doesn’t build a model,
there is no need to convert the pixel’s y-coordinates
to an absolute measurement e.g. cm or inch. Because
the resolution of the images is very low, the distance
estimation is not very accurate. At the lowest row of
the image, where the ratio between pixel and the pro- A B
jected real world area is highest, each pixel represents
an area of 2 ∗ 1.5cm2 .
The distance that triggers the robot to turn is set
to 30 cm. The robot needs to turn fast enough so that
an obstacle will not be closer than 15 cm in front of it
since the distance of any object in this area can not be
calculated correctly. At maximum speed , the robot C D
will have about two seconds to react and if the robot
Figure 3: Snapshots of the robot in the test environments
has already slowed down while approaching the ob- and its trajectories. A: the artificial arena with 4 objects.
ject, it will have about three seconds. We have tried B: A small area near the office corner. C: A path that went
different combinations of trigger distances and turn- through a chair’s legs. D: An object with no base on the
ing speeds to achieve a desirable combination. The ground.
first criteria is that the robot must travel safely, this
criteria sets the minimum turning speed and distance.
The width of view of the camera at the distance of For each test, the robot run for 5 mins. We placed
30 cm from the robot or 35 cm from the camera is the robot in different places and put different objects
30 cm. The width of our robot is 20 cm, so if the vi- into the test area. In general, the robot is quite compe-
sion module does not find an obstacle inside the trig- tent; Table I summaries the result. The vision-based
ger range, the robot can safely move forward. The obstacle detection module correctly identified obsta-
second criteria is the robot needs to be able to go cle with almost 100% accuracy, that is if there was an
to cluttered areas. This means it should not turn too obstacle in the camera view, the algorithm would reg-
early when approaching objects. Also when the robot ister an non-ground area. Although the calculated dis-
is confronted by the wall or a large object, it should tances of obstacles are not very accurate, they provide
turn just enough to move along the wall/object and enough information for the controller to react. The
not bounce back. This criteria encourages the robot simple mechanism of finding an open space worked
to explore the environment. surprisingly well. The robot was good at finding a
way out in a small area such as the areas under tables
and between chairs. The number of false positives are
also low and only occured in the office environment.
4 EXPERIMENTS This is because the office’s floor colours are more dif-
ficult to capture thoroughly. Further analysis revealed
4.1 Experiment Setup and Results that false positives often occurred in the top part of the
images. This is explained by the ratio of pixels/area
We tested the robot in two environments, a 1.5∗2.5m2 in the upper part of the image being lower than the
artificial arena surrounded by 30cm height walls and bottom part. At the top row of the image, each pixel
an office at the University of Kent Computing de- corresponds to an area of 7 ∗ 4cm while at the bot-
partment, shown in Fig. 3. The surface of the arti- tom row the area is 2 ∗ 1.5cm. Fortunately, the upper
ficial arena is a flat cartoon board with green wall- part also corresponds to the further area in real world.
papers on top. We put different real objects such as Therefore, most false positive cases resulted in unnec-
boxes, shoes, books onto the arena. We first tested the essary decreasing of speed but not changing direction.
robot in the arena with no objects (the only obstacless Because of the robot’s reactive behaviour, it is capable
are walls) and then made the tests more difficult by of responding quickly to changes in the environments.
adding objects. The office is covered with a carpet. During some of the tests, we removed and put obsta-
The arena presents a more controlable environment cles in front of the robot. The robot could react to the
where the surface is smooth and relatively colour- changes and altered it’s running direction accordingly.
uniformed. The office environmnent is more chal- Fig. 3 shows 4 snapshots of the robot during op-
lenging where even though the ground is flat its sur- eration and its trajectory. In picture A, the robot ran
face is much more coarse and not colour-uniformed. in the arena with 4 obstacles, it successfully avoided
278
Table 1: Performance summary.
Environment No of Obstacles Duration Average speed No. of collisions False positive

Arena 0 60 min 13 cm/c 0 0%
Arena 1 60 min 10 cm/s 0 0%
Arena 4 60 min 6 cm/s 2 0%
Office > 10 180 min 9 cm/s 7 3%
all the objects. On picture B, the robot went into a Table 2: Speed performance.
small area near a corner with a couple of obstacles
and found a way out. On picture C, the robot success- Task Time
fully navigated through a chair’s legs which presented Image acquiring 95 ms
a difficult situation. Picture D was a case where the Sonar sensor reading 125 ms
robot failed to avoid an obstacle. Because the table
leg cross-bar is off the floor, the robot underestimated Image processing 5 ms
the distance to the bar. Controller 1 ms
Servos updating < 1ms
4.2 Discussion
Logging 3 ms
We found that adding more obstacles onto the arena Total 150 ms
did not increase the number of collisions significantly.
However the average speed of the robot dropped as
the arena became more crowded. The speed loss is
admitedly due to the robot’s reactive behaviour. The REFERENCES
robot does not do path planning therefore it can not
http://www.gumstix.org.
always select the best path. In some cluttered areas,
the robot spent a lot of time spinning around before it Braitenberg, V. (1984). Vehicles: Experiments in Synthetic
could find a viable path. We can improve the robot’s Psychology. MIT Press/Bradford books.
behaviour in these situations by having a mechanism Brooks, R. A. (1985). A robust layered control system for a
to detect cluttered and closed areas so the robot can mobile robot. Technical report, MIT, Cambridge, MA,
avoid them. USA.
Each control cycle takes about 150ms or 7Hz. Ta- Lenser, S. and Veloso, M. (2003). Visual sonar: fast ob-
ble II shows the time spent on each task in the cycle. stacle avoidance using monocular vision. In Intelli-
gent Robots and Systems, 2003. (IROS 2003). Pro-
The algorithm used only 15% of the CPU during op- ceedings. 2003 IEEE/RSJ International Conference
eration. This leaves plenty of resources for higher be- on, volume 1, pages 886–891.
haviours if needed. It is possible to implement this al-
Lorigo, L., Brooks, R., and Grimsou, W. (1997). Visually-
gorithm with a less powerful CPU. Since only 10% of guided obstacle avoidance in unstructured environ-
the CPU time is spent on processing data, a CPU run- ments. In Intelligent Robots and Systems, 1997. IROS
ning at 20 MHz would be sufficient. So instead of the ’97., Proceedings of the 1997 IEEE/RSJ International
Gumstix computer we can use a micro-controller such Conference on, volume 1, pages 373–379, Grenoble,
as a Brainstem or a BASIC STAMP for both image France.
processing and motion control without any loss in per- Ulrich, I. and Nourbakhsh, I. R. (2000). Appearance-based
formance. The memory usage is nearly one Mb which obstacle detection with monocular color vision. In
is rather big. We did not try to optimise memory us- AAAI/IAAI, pages 866–871.
age during implementation so improvements could be
made. We plan to implement this control algorithm
on a micro-controller instead of the Gumstix. This
change will reduce the cost and power usage of the
robot by a large amount. To the best of our knowl-
edge, there has not been a mobile robot that can per-
form reliable obstacle avoidance in unconstrained en-
vironments using such low resolution vision and slow
microprocessor.
279
A DISTRIBUTED MULTI-ROBOT SENSING SYSTEM USING AN
INFRARED LOCATION SYSTEM
Anssi Kemppainen, Janne Haverinen and Juha Röning

Department of Electrical and Information Engineering, University of Oulu, Finland
{pikinen, johannes, jjr}@ee.oulu.fi
Keywords: Distributed sensing, relative pose estimation, multi-robot system, formation control.
Abstract: Distributed sensing refers to measuring systems where, instead of one sensor, multiple sensors are spatially
distributed to improve the robustness of the system, increase the relevancy of the measurements and cut costs,
since smaller and less precise sensors are used. Spatially distributed sensors fuse their measurements into the
same co-ordinates, which requires the relative positions of the sensors. In this paper we present a distributed
multi-robot sensing system in which the relative poses (positions and orientations) of the robots are estimated
using an infrared location system. The relative positions are estimated using intensity and bearing measure-
ments of received infrared signals. The relative orientations are obtained by fusing the position estimates of
the robots. The location system enables a group of robots to perform distributed and co-operative environ-
ment sensing by maintaining a given formation while the group measures distributions of light and a magnetic
field, for example. In the experiments, a group of three robots moved and collected spatial information (i.e.
illuminance and compass headings) from the given environment. The information was stored on grid maps
that present illuminance and compass headings. The experiments demonstrated the feasibility of using the
distributed multi-robot sensing system in mobile sensing applications.
1 INTRODUCTION do not require an external infrastructure for position-

ing (GPS, WLAN, etc.). However, the measurement
Distributed sensing (Brooks and Iyengar, 1998), (Cat- range of the relative location system must be sufficient
terall et al., 2003) refers to measuring systems where, to encompass the sensing area.
instead of one sensor, multiple sensors are spatially In this paper we present a distributed multi-robot
distributed to improve the robustness of the system, sensing system that uses an infrared location system
increase the relevancy of the measurements and cut (Kemppainen et al., 2006). The location system es-
costs, since smaller and less precise sensors are used. timates the relative poses (positions and orientations)
In the robotic domain, distributed sensing enables of the robots. Related systems have been presented
multi-robot systems (MRS) to perform mapping and that exploit several techniques, including laser range
exploration (Sujan et al., 2004), allocate tasks among finders (Schneider and Wildermuth, 2004), (Mon-
robots (Pagello et al., 2006), and plan paths and navi- tesano et al., 2004), (Howard et al., 2003),(Moors
gate in an unknown or partially unknown environment et al., 2003), ultrasonic TOF measurement (Shoval
(Cai et al., 1996), for example. and Borenstein, 2001), (Grabowski et al., 2000) and
Distributed sensing in an unknown enviroment vision (Montesano et al., 2005), (Spletzer et al.,
benefits from knowing the relative locations of sen- 2001), to locate and recognise other robots. A com-
sors. The relative locations enable the sensors to place parison of the infrared location system and related
their measurements into the same sensor-centric map. systems was discussed in (Kemppainen et al., 2006).
Autonomous sensing systems are able to use one sen- We selected infrared sensors since they are small and
sor as an origin of the co-ordinates, while the other capable of relative angle measurements between an
sensors measure the surrounding area. Such systems emitter and receiver. In addition, infrared radiation
280
A DISTRIBUTED MULTI-ROBOT SENSING SYSTEM USING AN INFRARED LOCATION SYSTEM
does not reflect from walls and object surfaces as 1

much as ultrasound. In relative angle measurements, 3
2
1
the effects of multipath reflection would be crucial be- 5 4
6 2
7
cause of ambiguous angle estimates. 8 3
To demonstrate the distributed sensing system in 5 4 7
mobile sensing applications, we conducted experi-
ments where a group of three robots measured the dis- 6
tributions of illuminance and a magnetic field while

maintaining a triangle formation.
Figure 1: The actual system and the illustration of mechan-
ics: 1) mirror, 2) emitter, 3) receiver, 4) beam collector,
2 INFRARED LOCATION 5) aperture, 6) DC motor and Hall-effect-sensors, 7) see-
through body, 8) control electronics.
SYSTEM
The infrared location system, originally presented in implemented a distributed sensing system in which a
(Kemppainen et al., 2006), is a vital part of the multi- group of three robots measured spatial information in
robot system (Haverinen et al., 2005) that enables the a given environment.
robots to maintain a given formation while sensing the
environment. The key idea of the location system is to
estimate the relative positions without data transmis-
3.1 Formation Control
sion between the robots. However, radio transmission
Formation control enables a multi-robot system to
is used to share estimates among the group in order
measure spatial distributions while moving across the
to enable the robots to estimate their relative orienta-
measurement area. The measurement range of the
tions.
infrared location system is restricted to five metres,
The location system uses intensity and bearing
and to be able to fuse spatial measurements onto the
measurements of received signals to estimate the lo-
same map, the multi-robot system is required to main-
cations of other robots in polar co-ordinates. By shar-
tain limited relative distances. Formation control is
ing these estimates among the group of robots, it is
required not only to limit the distance between the
possible to also estimate their relative orientations. In
robots, but also to enable co-ordinated sensing in or-
addition, each robot is identified through different fre-
der to reduce mapping time.
quencies in the received signal.
In our experiments one of the robots acted as a
Figure 1 presents the main components of the lo-
leader and the other two followed the leader. To-
cation system. A conical mirror reflects an emitted
gether they constituted a right-angle triangular forma-
signal sideways into a unified zone, whereas beam
tion where the distance between the leader and the fol-
collector collects signals from other robots. A servo
lowers was 1.5 metres. For each following robot, for-
system with a DC motor, Hall-effect sensors and dis-
mation control consisted of two P controllers; one for
crete PID controller is used to rotate the beam collec-
translational and the other for rotational speed con-
tor at a constant angular velocity. The measurement
trol. The infrared location system updated the relative
range of the system is approximately five metres, giv-
pose measurements approximately every three sec-
ing the most accurate estimates for radial co-ordinates
onds, giving the relative poses of the following robots
when the distance between two robots is in the range
in the leader’s co-ordinates. Fig. 2 presents how er-
of [0.5; 2.5 m].
rors in the relative orientation and position were used
in the rotational and translational speed controllers
(respectively).
3 EXPERIMENTS The control cycle length in each controller was
200 ms, while the location system updated poses only
Experiments were conducted to demonstrate the fea- once every three seconds. In order to update pose er-
sibility of using the distributed multi-robot sensing rors in each control cycle, each robot estimated their
system in mobile sensing applications. For exam- relative movements using odometer readings. In the
ple, co-operative mapping in an unknown environ- experiments, the odometer readings enabled the for-
ment requires moving platforms that are capable of mation to be driven at a 10 cm/s translational velocity.
measuring spatial information and estimating the rel-
ative positions of the robots. For our experiments, we
281
The leader robot the cells with the smallest intensities are shadowed ar-
eas close to chairs, plants and walls.
Illuminance
500
400
100
Objective position
300
200 80
Y coordinate (cm)
er
et 100
60
0
The following robot −100

40
Figure 2: The pose of the following robot relative to the −200
leader is estimated and used to control the robot to the ob- −300
20
jective position. P controllers use rotation er and translation
−400
et errors to control rotational and translational speeds of the
robot. −500 0
−500 0 500
X coordinate (cm)
Figure 4: Illumincance in global coordinates.

3.2 Spatial Measurements
In the experiments, three robots measured distribu- Fig. 5 presents a grid map of compass headings
tions of light and a magnetic field over a given en- with a bidirectional arrow, where red points to the
vironment, producing maps of illuminance and com- north and white to the south. Since compass head-
pass headings. Fig. 3 presents the trajectories of the ings are disturbed indoors by electric cables and metal
robots while moving and measuring the environment. structures, the values of the compass headings give us
The leader was driven from position (50 cm, -250 cm) spatial information about the magnetic field in the en-
to position (20 cm, 450 cm), while the group main- vironment. However, in the experiments, the mag-
tained a triangular formation. Spatial measurements netic field of the measured environment was paral-
were stored on grid maps presented in the leader’s co- lel, which gave us small spatial variations in compass
ordinates. heading.
Trajectories Compass heading

500 500
400 400
300 300
200 200
Y coordinate (cm)
Y coordinate (cm)
100 100
0 0
−100 −100
−200 −200
−300 −300
−400 −400
−400−300−200−100 0 100 200 300 400 500 −400−300−200−100 0 100 200 300 400 500
X coordinate (cm) X coordinate (cm)
Figure 3: Trajectories in global coordinates. Figure 5: Compass heading in global coordinates.
Fig. 4 presents a grid map of illuminance, where The experiments demonstrated distributed sens-
the highest intensity is depicted with white colour and ing in a group of robots to produce distributions of
the lowest intensity, for the cells not visited, with illuminance and a magnetic field. All the measure-
black colour. This gives spatial information about the ments were tied to the co-ordinates of the leading
distribution of light in the given environment. The robot. Since the ground truth positions were miss-
cells with the highest intensities are close to lights and ing, positioning errors in the global co-ordinates of
282
A DISTRIBUTED MULTI-ROBOT SENSING SYSTEM USING AN INFRARED LOCATION SYSTEM
the sensing robots were affected by the odometer er- on Robotics and Automation (ICRA-96), pages 2079 –
ror of the leading robot and the errors of the infrared 2084, Minneapolis.
location system. In addition, the grid size in the ex- Catterall, E., Laerhoven, K. V., and Strohbach, M. (2003).
periments was 1x1 metres, which gave only a coarse Self-organization in ad hoc sensor networks: an em-
picture of the true distributions. However, these pre- pirical study. In ICAL 2003: Proceedings of the eighth
liminary experiments demonstrated the feasibility of international conference on Artificial life, pages 260–
263, Cambridge, MA, USA. MIT Press.
using the infrared location system in distributed au-
tonomous sensing systems. Grabowski, R., Navarro-Serment, L., Paredis, C., and
Khosla, P. (2000). Heterogeneous teams of modu-
lar robots for mapping and exploration. Autonomous
Robots, 8(3):293–308.
4 CONCLUSION AND FUTURE Haverinen, J., Parpala, M., and Röning, J. (2005). A minia-
WORKS ture mobile robot with a color stereo camera system
for swarm robotics research. In IEEE International
Conference on Robotics and Automation (ICRA2005),
In this paper we presented a distributed multi-robot pages 2494–2497, Barcelona, Spain.
sensing system that uses an infrared location sys- Howard, A., Mataric, M., and Sukhatme, G. (2003). Putting
tem. The infrared location system estimates poses in the ’i’ in team: an ego-centric approach to cooperative
a multi-robot system, enabling the robots to maintain localization. In International Conference on Robotics
a given formation while sensing the environment. In and Automation.
addition, knowing their poses enables the robots to Kemppainen, A., Haverinen, J., and Röning, J. (2006). An
place their measurements on the same map. infrared location system for relative pose estimation
We conducted an experiment where a group of of robots. In 16-th CISM-IFToMM Syposium of Robot
three robots moved and measured spatial informa- Design, Dynamics, and Control (ROMANSY 2006),
pages 379–386, Warsaw, Poland.
tion in a right-angle triangular formation. Leader-
follower formation control used pose estimates and P Montesano, L., Gaspar, J., Santos-Victor, J., and Montano,
L. (2005). Fusing vision-based bearing measurements
controllers to control the rotational and translational
and motion to localize pairs of robots. In International
speeds of the following robots. In the experiments we Conference on Robotics and Automation.
measured spatial distributions of illuminance and a
Montesano, L., Montano, L., and Burgard, W. (2004). Rel-
magnetic field, which gave us information about shad- ative localization for pairs of robots based on uniden-
owing objects, metal structures and electric cables. In tifiable moving features. In International Conference
addition, since the information is spatially distributed, on Intelligent Robots and Systems.
it can be used in mapping and localization applica- Moors, M., Schneider, F. E., and Wildermuth, D. (2003).
tions. Relative position estimation in a group of robot. In
The main contribution of the research was the con- International Conference on Climbing and Walking
struction and validation of a distributed multi-robot Robots.
sensing system for mobile sensing applications. Fu- Pagello, E., D’Angelo, A., and Menegatti, E. (2006). Co-
ture research will focus on developing methods for operation issues and distributed sensing for multirobot
multi-robot exploration utilising spatial information. systems. Proceedings of the IEEE, 94(7):1370–1383.
Schneider, F. and Wildermuth, D. (2004). Using an ex-
tended kalman filter for relative localisation in a mov-
ing robot formation. In International Workshop on
ACKNOWLEDGEMENTS Robot Motion and Control.
Shoval, S. and Borenstein, J. (2001). Measuring the relative
The authors gratefully acknowledge the support of the position and orientation between two mobile robots
Academy of Finland. with binaural sonar. In International Topical Meeting
on Robotics and Remote Systems.
Spletzer, J., Das, A., Fierro, R., Taylor, C., Humar, V., and
Ostrowski, J. (2001). Cooperative localization and
REFERENCES control for multi-robot manipulation. In Conference
on Intelligent Robots and Systems.
Brooks, R. R. and Iyengar, S. S. (1998). Multi-sensor fu-
sion: fundamentals and applications with software. Sujan, V. A., Dubowsky, S., Huntsberger, T. L., Aghazar-
Prentice-Hall, Inc., Upper Saddle River, NJ, USA. ian, H., Cheng, Y., and Schenker, P. S. (2004). An
architecture for distributed environment sensing with
Cai, A., Fukuda, T., Arai, F., and Ishihara, H. (1996). Co- application to robotic cliff exploration. Auton. Robots,
operative path planning and navigation based on dis- 16(3):287–311.
tributed sensing. In IEEE International Conference
283
AUTOMATIC VISION-BASED MONITORING OF THE
SPACECRAFT ATV RENDEZVOUS / SEPARATIONS WITH THE
INTERNATIONAL SPACE STATION
A. A. Boguslavsky, V. V. Sazonov, S. M. Sokolov

Keldysh Institute for Applied Mathematics, Miusskaya Sq., 4, Moscow, Russia
{boguslav, sazonov, sokolsm}@keldysh.ru
A. I. Smirnov, K. U. Saigiraev
RSC ENERGIA, Korolev, Russia
{smirnov, cxy}@scsc.ru
Keywords: Algorithms of determining the motion parameters, space rendezvous / separations processes, accuracy, real
time vision system.
Abstract: The system which allows automating the visual monitoring of the spacecraft ATV rendezvous / separations
with the international space station is being considered. The initial data for this complex is the video signal
received from the TV-camera, mounted on the station board. The offered algorithms of this video signal
processing in real time allow restoring the basic characteristics of the spacecraft motion with respect to the
international space station. The results of the experiments with the described software and real and virtual
video data about the docking approach of the spacecraft with the International Space Station are being
presented. The accuracy of the estimation of the motion characteristics and perspectives of the use of the
package are being discussed.
1 INTRODUCTION received the TV image of the station target from a

spacecraft, carried out the manual docking approach.
One of the most difficult and crucial stages in At the center of the flight management the
managing the flights of space vehicles is the process control of objects relative motion parameters (range,
of their docking approach. The price of a failure at speed, angular deviations) should also be carried out.
performing of this process is extremely high. The The semi-automatic TV methods of the monitoring
safety of crew, station and space vehicles also in which are being used till now, do not satisfy the
many respects depends on a success of its modern requirements anymore. Recently appeared
performance. means of both the methods of the visual data
The radio engineering means of the docking acquisition and processing provide an opportunity of
approach, which within many years have been used the successful task decision of a complete automatic
at docking of the Russian space vehicles, are very determination and control of space vehicles relative
expensive and do not allow to supply docking to not motion parameters.
cooperated station. Flights of the ATV have not begun yet.
As reserve methods of docking approach Therefore images of the ATV in ground-based
monitoring the manual methods are applied, for conditions and simulated space images are available
which in quality viewfinders the optical and now. But already there are some papers discussing
television means are used. For docking approach of possible approaches to the decision of similar
the pilotless cargo transport vehicle “Progress” to images analysis problems (Chi-Chang, H.J. and
the orbital station “Mir” a teleoperation mode of McClamroch, N.H., 1993; Casonato, G. and
manual control (TORU) was repeatedly used, at Palmerini, G.B., 2004). The described approach is
which the realization the crew of the station, having based on experiments with similar system for the
“Progress” spaceship (Boguslavsky et al., 2004).
284
AUTOMATIC VISION-BASED MONITORING OF THE SPACECRAFT ATV RENDEZVOUS / SEPARATIONS
WITH THE INTERNATIONAL SPACE STATION
The system for ATV has a number of differences 4) Performing the measurements of the sizes and
from the system used for “Progress”. First of all, this coordinates of the recognized objects.
various arrangement of the TV-camera and special All the listed above operations should be
target for the monitoring of rendezvous / separations performed in real time. The real time scale is
processes. In the ATV case the TV-camera settles determined by the television signal frame rate. The
down on ISS, and by the transport vehicle the other significant requirement is that in the
special targets (basic and reserve) are established. considered software complex it is necessary to
Besides that is very important for visual system, the perform updating of the spacecraft motion
images of objects for tracking in a field of view have parameters with a frequency of no less than 1 time
changed. per second.
In view of operation experience on system For reliability growth of the objects of interest
Progress - ISS, in system ATV - ISS essential the extraction from the images of the following
changes to the interface of the control panel (see features are provided:
Part 4) were made. - Automatic adjustments of the brightness and
The main stages of the visual data acquisition contrast of the received images for the optimal
and processing in the complex are realized mostly in objects of interest recognition.
the same way as the actions performed by an - Use of the objects of interest features of the
operator. several types. Such features duplication (or even
In the presented field of vision: triplication) raises reliability of the image processing
1. An object of observation is marked out when not all features are well visible (in the task 3).
(depending on the range of observation, it can be the - Self-checking of the image processing results
vehicle as a whole or a docking assembly or a on a basis of the a priori information about the
special target); observed scenes structure (in the tasks 1-4).
2. Specific features of an object under The ways of performing the calculation of the
observation defined the patterns of which are well ROI position on the current image are (the task 1):
recognizable in the picture (measuring subsystem). 1a) Calculation of the ROI coordinates (figure 1)
3. Based on the data received: on the basis of the external information (for
The position of these elements relative to the example, with taking into account a scene structure
image coordinate system is defined; or by the operator selection).
Parameters characterizing the vehicle and station 1b) Calculation of the ROI coordinates and sizes
relative position and motion are calculated on the basis of the information received from the
(calculation subsystem). previous images processing.
In addition to operator actions, the complex
calculates and displays the parameters characterizing
the approach/departure process in a suitable for
analysis form.
This research work was partially financed by the
grants of the RFBR ## 05-01-00885, 06-08-01151.
2 MEASURING SUBSYSTEM
The purpose of this subsystem is the extraction of
the objects of interest from the images and
performance of measurements of the points’ a)
coordinates and sizes of these objects. To achieve
this purpose it is necessary to solve four tasks:
1) Extraction of the region of interest (ROI)
position on the current image.
2) Preprocessing of the visual data inside the
ROI. b)
3) Extraction (recognition) of the objects of Figure 1: An example of identification of a region of
interest. interest in the TV camera field of view (1.а) way): а) –
total field of view, b) – a region of interest.
285
The second (preprocessing) task is solved on the of labels and of radius of a target (figure 3) is
basis of the histogram analysis. This histogram performed. After that the detection of a cross of a
describes a brightness distribution inside the ROI. target is performed.
The brightness histogram should allow the reliable
transition to the binary image. In the considered task
the brightness histogram is approximately bimodal.
The desired histogram shape is provided by the
automatic brightness and contrast control of the
image source device. a) b) c)
Figure 3: An example of allocation of an image of a target
on the basis of the coarse coordinates of a target plate and
a priory of the information on an arrangement of labels of
a target. а) - definition of area of interest on a basis a
priory of the information and found coarse location of the
plate; b) area of interest for search of labels of a target; c) -
result of allocation of labels of a target.
Figure 2: Coarse allocation of a target from all candidates

on a basis of the a priory information: the relative sizes of
labels and target plate and their relative location.
At the third processing stage (the task 3) the

extraction of the objects of interest is performed.
These objects are the spaceship contour, the docking
unit disk, the target cross and the target disk with
small labels. The main features are the spaceship
contour, the cross and the target labels. These
features are considered the main structured elements
of the recognized scene and used for measurement.
Figure 4: An example of allocation of edge of target boxes
At features extraction both edge-based (Canny,
and external contour of spaceship (for distance >10
1986; Mikolajczyk et al., 2003) and region-based meters). Top-down: extracted edges of the left and the
methods are used (Sonka and Hlavac, 1999). right boxes; contour of the spaceship; results of the frame
With accordance to such elements set the processing (with the marked docking unit image centre).
processing of the current field image is being
performed. For example, on distances less than 15 On distances more than 15 m a label and a cross
meters are made detection of a target plate, its labels of a target reliably are not visible. Therefore the
and a cross. During such processing (figures 1, 2, 3) detection of an external contour of the ship and a
the results of the previous measurements can be used target boxes (figure 4) is realized. Boxes of a target
for estimation of the small area of interest position allow to confirm a hypothesis about the detection of
(figure 1, below). If the previous results are not the ship and to specify its roll position.
present (for example, immediately after system The fourth operation (performing of the
start), as area of interest is processed the full field measurement) is very straightforward. But for this
image. During processing among candidates for a operation it is necessary to recognize the reliable
target image of a target it selects the most probable objects results from the previous processing stages.
candidate (figure 2). For its verification the detection
286
3 CALCULATION PART on a, c1 , c 2 , i.e. by solving the linear least squares

problem. The coordinates of the cross center are
Preprocessing of a frame (more exactly, a half- ac + c c − ac 2
frame) gives the following information: X C* = 1 2 2 , YC* = 1 .
1+ a 1+ a2
T – reference time of a frame (in seconds);
Determining the radius and the center of the
X C , YC – real values coordinates of the center of
target circle is realized in two stages. In the first
the cross on the target; N1 –number of couples of stage, we obtain preliminary estimations of these
points on the horizontal crossbar of the cross; quantities based on elementary geometry. In the
second stage, we solve the least squares problem
X i , Yi (i = 1, 2,…, N 1 ) – integer coordinates of
minimizing the expression
points on the top of the horizontal crossbar of the N3 2
cross; X i , Yi′ (i = 1, 2,…, N 1 ) – coordinates of points Φ2 = ∑ ⎡⎢⎣ ( Ai − X O )2 + (Bi −Y O )2 − R⎤
⎥⎦
on the bottom of the horizontal crossbar of the cross; i =1
N 2 – number of couples of points on the vertical on X O , YO , R by Gauss-Newton method (Bard,
crossbar of the cross; U i , Vi (i = 1, 2,…, N 2 ) – 1974). Let its solution be X O* , YO* , R * . As a rule,
coordinates of points on the left side of vertical | X O − X O* | and | YO − YO* | do not exceed 1.5
crossbar of the cross; U i′, Vi (i = 1, 2,…, N 2 ) – pixels. Below for simplicity of notations, we will not
coordinates of points on the right side of vertical use an asterisk in designations of recomputed
crossbar of the cross; X O , YO , R – coordinates of parameters.
the center of the circle on the target and its radius;
N 3 , Ai , Bi (i = 1, 2,…, N 3 ) – number of points on 3.1 Basic Geometrical Ratios
the circle and their coordinates; X S , Y S , R S – We use the right Cartesian coordinate system
coordinates of the center of the circle, which is the Cy1 y 2 y 3 , which is connected with the target. The
station outline, and its radius (real numbers).
Here all coordinates are expressed in pixels. point C is the center of the target circle, the axis
Successful preprocessing a frame always gives the Cy 3 is directed perpendicularly to the circle plane
values of X C , YC and X O , YO , R , but if there is an away from station, i.e. is parallel a longitudinal axis
opportunity, when appropriate N k > 0 , those of the Service module, the axis Cy 2+ intersects a
quantities are determined once again by original longitudinal axis of the docking device on the
information. Bellow we describe the calculation of Service Module. Also, we use right Cartesian
those quantities in the case when coordinate system Sx1 x 2 x 3 connected with the TV
N 1 > 0, N 2 > 0, N 3 > 0 and R S = 0 . If RS differs camera on the spacecraft. The plane Sx1 x 2 is an
from zero, the data are used in other way (see image plane of the camera, the axis Sx 3 is a camera
bellow). optical axis and directed on movement of the
Determining the coordinates of the cross center.
We change the data spacecraft, the axis Sx 2− intersects an axis of the
Yi + Yi′ U i + U i′ docking device of the spacecraft. Let || a ij || i3, j =1 be the
→ Yi , → Ui (i = 1, 2, … )
2 2 transition matrix from the system Sx1 x 2 x 3 to the
and get coordinates of two sequences of points, system Cy1 y 2 y 3 . The transition formulas are
which lie in centerlines of horizontal and vertical 3
crossbars of the cross. Those centerlines have
equations ax − y = c1 (horizontal) and x + ay = c 2
yi = di + ∑ aij x j (i = 1, 2, 3) ,
j =1
(vertical). Here a , c1 and c 2 are coefficients. The
form of the equations takes into account the xj = ∑ ( yi − d i )aij ( j = 1, 2, 3) .
orthogonality of these lines. The coefficients are
determined by minimization of the quadratic form Here d 1 , d 2 , d 3 are coordinates of the point S
N1 N2
in the system Cy1 y 2 y 3 .
∑ (a X i − Yi − c1 ) 2 + ∑ (U j + aV j + c2 ) 2
i =1 j =1
287
The matrix characterizes ideal docking The processing a frame is considered to be

|| aij || = diag(1, − 1, − 1) . In actual docking the transition successful, if the quantities d i , ϕ i (i = 1, 2, 3) were
matrix is estimated. As a result of successful processing a
1 − ϕ3 ϕ2 sequence of frames, it is possible to determine
|| a ij || = − ϕ 3 − 1 ϕ1 spacecraft motion with respect to the station. The
ϕ2 − ϕ1 − 1 successfully processed frames are used only for
where ϕ , ϕ 2 , ϕ 3 are components of the vector of an motion determination.
infinitesimal rotation of the system Sx1 x 2 x 3 with
3.2 Algorithm for Determination of the
respect to its attitude in ideal docking. We suppose Spacecraft Motion
deviations from ideal docking are small.
If any point has in the system Sx1 x 2 x 3 the The spacecraft motion is determined in real time as a
coordinates ( x1 , x 2 , x 3 ) , its image has in the image result of step-by-step processing of a sequence of
TV images of the target. The data are processed by
plane the coordinates ξ1 = fx1 , ξ2 =
fx 2 .
separate portions.
x3 x3
In each portion is processed in two stages. The
Here f is focal length of the camera. The first stage consists in determining the motion of the
coordinates ξ1 and ξ 2 , expressed in pixels, were spacecraft center of mass; the second stage consists
meant in the above description of processing a single in determining the spacecraft attitude motion.
video frame. Let coordinates of the same point in the Mathematical model of motion is expressed by
system Cy1 y 2 y 3 be ( y1 , y 2 , y 3 ) . Then formulas
( y1 − d 1 )a1i + ( y 2 − d 2 )a 2i + ( y 3 − d 3 )a 3i . d 1 = z1 + z 2 t , d 2 = z 3 + z 4 t , d 3 = z 5 + z 6 t ,
ξi = f
( y1 − d 1 )a13 + ( y 2 − d 2 )a 23 + ( y 3 − d 3 )a 33 ϕ 1 = v1 + v 2 t , ϕ 2 = v 3 + v 4 t , ϕ 3 = v5 + v 6 t .
The coordinates of the center of the target circle Here t is time counted off the beginning of
the C in the system Cy1 y 2 y 3 are (0, 0, 0) , therefore processing the frame sequence, z i and v j are
d 1 − d 2ϕ 3 + d 3ϕ 2 , d ϕ + d 2+ d 3ϕ 1 .
XO = f YO = − f 1 3 constant coefficients. The ratios written out have the
d 1ϕ 2 + d 2ϕ 1 −d 3 d 1ϕ 2 + d 2ϕ 1 −d 3
obvious kinematical sense. We denote the values of
In docking | d 1 | << d 3 , | d 2 | << d 3 , so it is the model coefficients, obtained by processing the
possible to use the simplified expressions (n )
portion of the data with number n , by z i , v (n )
j
fd 1 fd 2
XO = − − fϕ 2 , YO = + fϕ 1 .
d3 d3 and the functions d i (t ) , ϕ i (t ) , corresponding to
(n) (n)
The center of the cross in the system Cy1 y 2 y 3 those coefficients, by Di (t ) , Φ i (t ) .
has the coordinates (0, 0, b) . In this case under the Determining the motion consists in follows. Let
similar simplification, we have there be a sequence of successfully processed
fd1 fd 2 frames, which correspond to the instants
XC = − − fϕ 2 , YC = + fϕ 1 . t1 < t 2 < t 3 < ... . The frame with number k
d3 − b d 3−b
f bd f bd corresponds to the instant t k . Values of the
So X − X = − 1 ,Y −Y = 2 .
C O d 3( d 3 − b ) C O d ( d − b) quantities X C , YC , a , X O , YO , R , which were
3 3
(k ) (k )
The radius r of the target circle and radius R of found by processing this frame, are X C , YC , etc.
its image in the image plane are connected by the These values with indexes k = 1, 2, … , K 1 form the
ratio R = fr / d3 . first data portion, the value with indexes
The last three ratios allow to express d 3 , d 1 and k = K1 + 1, K1 + 2,…, K 2 – the second one, with
d 2 through R , X C − X O and Y C − Y O . Then it is indexes k = K n −1 + 1, K n −1 + 2,…, K n – the n -th
possible to find ϕ 1 and ϕ 2 , having solved portion.
The first data portion is processed by a usual
concerning these angles the expressions for X O , YO method of the least squares. The first stage consists
or X C , YC . As to the angle ϕ 3 , the approximate in minimization of the functional
ratio ϕ 3 = a takes place within the framework of K1
our consideration.
Ψ1 ( z ) = ∑ Ak ,
k =1
288
⎡ ⎤
2 Ψ2 [z ( 2 ) ] ,
f bd ( k ) σ2 =
Ak = w1 ⎢ X C( k ) − X O( k ) + ( k ) ( k1) ⎥ + 3( K 2 − K1 ) − 6
⎢⎣ d 3 [d 3 − b] ⎥⎦
2 2 where B2 is the matrix of the system of the normal
⎡ f bd ( k ) ⎤ ⎡ fr ⎤
w2 ⎢YC( k ) − YO( k ) − ( k ) ( k2) ⎥ + w3 ⎢ R ( k ) − ( k ) ⎥ equations, which arise at minimization of Ψ2 ,
⎢⎣ d 3 [d 3 − b] ⎥⎦ ⎢⎣ d 3 ⎥⎦
calculated at the point z ( 2 ) .
(k )
Here d i = d i (t k ) , z = ( z1 , z 2 ,…, z 6 ) T is a (k )
At the second stage, the quantities α1 and
vector of the coefficients, which specify the
α 2( k ) (see above) are calculated and the estimation
functions d i (t ) , wi , is positive numbers (weights).
The minimization is carried out by Gauss -Newton of the coefficients v (j2 ) are found. The estimation
method (Bard, 1974). The estimation z (1) of z and v ( 2 ) provides the minimum to the quadratic form
the covariance matrix P1 of this estimation are 2
] + ∑ [α ]
K2
F2 (v ) = q′ [ ] [ (k )
defined by the formulas T
v − v (1) Q1 v − v (1) 1 − v1 − v2tk
.
[
z (1) = z1(1) , z 2(1) , … , z 6(1) ] T
= arg min Ψ1 ( z ) ,
k = K1 +1
Here q ′ is a parameter, 0 ≤ q ′ ≤ 1 . Let Q2 be

P1 = σ 2 B1−1 , Ψ [z (1) ] .
σ 2 = 1 the matrix of this form. The covariance matrix of the
3K 1 − 6
estimation v ( 2 ) is Q2−1 F2 [ v ( 2 ) ] /( K 2 − K1 − 2) .
Here B1 is the matrix of the system of the
The third and subsequent data portions are
normal equations arising at minimization of Ψ1 . The processed analogously to the second one. The
matrix is calculated at the point z (1) . formulas for processing the portion with number n
At the second stage, the quantities are obtained from the formulas for processing the
1 ⎡ (k ) fD2(1) (t k ) ⎤ , ( k ) 1 ⎡ ( k ) fD1(1) (tk ) ⎤ second portion by replacement of the indexes
α1( k ) = ⎢YO − (1) ⎥ α 2 = − ⎢ X O + (1) ⎥ expressed the portion number: 1 → n − 1 , 2 → n .
f ⎣⎢ D3 (t k ) ⎦⎥ f ⎢⎣ D3 (tk ) ⎦⎥
are calculated and three similar linear regression Our choice of C n and Ψn ( z ) means that the
problems covariance matrix of errors in X C − X O , YC − YO
α1( k ) ≈v 1 +v 2 t k , α 2( k ) ≈v 3 + v 4 t k , and R is equal to diag ( w1−1 , w2−1 , w3−1 ) .
a ( k ) ≈v 5 + v 6 t k ( k = 1, 2,…, K1 ) It is easy to see that C n < Bn −1 , i.e. the matrix
are solved using the standard least squares method Bn−1 − Cn is positive definite. The introduction of
(Seber, 1977). We content ourselves with
the matrix Gn provides diminution of influence of
description of estimating the couple of parameters
v1 , v 2 . We unite them in the vector v = ( v1 , v 2 ) T . the estimation z ( n −1) on the estimation z ( n ) .
(1) (1) Unfortunately, the matrix Gn is unknown. In such
The estimations v1 and v 2 provide the minimum
to the quadratic form situation, it is natural to take C n = qBn −1 . One has
2 Cn < Bn−1 if q < 1 . The described choice of C n
∑ [α1(k ) − v1 − v 2 t k ]
K1
F1 ( v ) = .
means, that procession of the n -th data portion takes
k =1
into account the data of the previous portions. The
Let Q1 be the matrix of this form. Then the data of the n -th portion are taken in processing with
(1) (1)
covariance matrix of the vector v (1) = [ v1 , v 2 ] T the weight 1, the (n − 1) -th portion is attributed the
is Q1−1 F1 [ v (1) ] /( K1 − 2) . weight q , the ( n − 2) -th portion has the weight q 2 ,
The second data portion is carried out as follows. etc.
At the first stage, the functional The results of processing the n -th data portion
K2
Ψ2 ( z ) = [z − z (1) ] C 2 [z − z (1) ] + are represented by numbers Di( n ) ( t K n ) , Φ i( n ) (t K n )
∑ Ak
T
k = K1 +1 (i = 1, 2, 3; n = 1, 2,…) . We calculate also the

is minimized. Here C 2 = qB1 , q is a parameter, dρ
quantities ρ = d 12 + d 22 + d 32 , u= ,
0 ≤ q ≤ 1 . The estimation of z and its covariance dt
matrix have the form d2 d1
α = arctan , β = arctan .
d3
, P2 = σ B2 ,
z ( 2 ) = arg min Ψ2 ( z ) 2 −1 d 12 + d 32
289
The angle α is called a passive pitch angle, the motion. Figure 5 contains the plots of the functions
angle β is a passive yaw angle. If docking is close ρ (t ) , u(t ) , α (t ) and β (t ) and ϕi (t ) (i = 1, 2, 3) .
to ideal (considered case), then | d 1 | << d 3 , On ρ (t ) and u(t ) plots the scale on a vertical axis
| d 2 | << d 3 and α = d 2 / d3 , β = d1 / d 3 . differs on 2 order.
ρ (m), u (m/s)
The angle ϕ 1 is called an active pitch angle, ϕ 2
is an active yaw angle, ϕ 3 is an active roll angle.
We remind these angles have small absolute values.
Characteristics of accuracy of the motion
parameter estimations are calculated within the
framework of the least squares method. For
example, we defined above the covariance matrix
Pn of the estimation z ( n ) . In terms of this matrix
the covariance matrix C w (t ) of the vector w(t ) =
(z1 + z 2 t, z 2 , z 3 + z 4 t, z 4 ,…, v5 + v6 t, v 6 ) T ∈ R12 is t (s)
calculated by formulas α , β (deg.)
T
∂w ⎛ ∂w ⎞ , ∂w 1 t .
Cw = Pn ⎜ ⎟ = diag (U, U, U) , U =
∂z ⎝ ∂z ⎠ ∂z 0 0
These formulas concern to the motion which was
found by processing the n -th of a portion of the
data.
Knowing C w (t ) , it is possible to calculate the
standard deviations σ ρ (t ) , σ u (t ) , σ α (t ) and
σ β (t ) of the quantities ρ (t ) , u(t ) , α (t ) and β (t ) .
The standard deviation σ ρ (t ) has the form t (s)
T T ϕ 1 , ϕ 2 , ϕ 3 (deg.)
∂ρ ⎛ ∂ρ ⎞ , ∂ρ ⎛ d 1 d d ⎞
σρ = Cw ⎜ ⎟ = ⎜⎜ , 0, 2 , 0, 3 ⎟⎟ .
∂w ⎝ ∂w ⎠ ∂w ⎝ ρ ρ ρ ⎠
The similar formulas define the others standard
deviations. The values of ρ , σ ρ , u , σ u , etc.,
referring to the last instant of the processed data
portion, are written on the computer display.
4 EXAMPLES
The experiments with the described complex were
carried out on various samples of the initial data.
The adjustment of system on the images of the
transport ship ATV was carried out on photos of a
breadboard model of the ship in a hangar and on
rendered samples of the 3D ship model trajectories. t (s)
Other initial data were real video received from a Figure 5: Results of the determination of the spacecraft
board of the ship "Progress". These data were used motion in docking approach.
for an estimation of reliability and correctness of the
detection of real objects, and also for the verification Figure 6 presents the plots of the standard
of algorithms of relative movement parameters deviations σ ρ (t ) , σ u (t ) , σ α (t ) , σ β (t ) . The values
calculation. of all these functions were calculated at the last
Figures 5, 6 give examples of the operation of instants of processed data portions. These values
the described algorithm estimating the spacecraft
290
were shown by marks. Each portion contained 10 processing is much more complicated than the
instants with measurements: K n − K n−1 = 10 . For organizational point of view and more expensive
clearness, the markers were connected by segments than processing the video image. It is necessary to
of straight lines, therefore presented plots are broken note, that the estimation of kinematical parameters
lines. Only the vertexes of these broken lines are of the moving objects on the video signal, becomes
significant. Their links are only interpolation, which now the most accessible and universal instrument of
is used for visualization and not always exact. As it solving such kind of problems in situations, when
is shown in figure 6, the spacecraft motion on the the price of a failure is rather insignificant.
final stage of docking was defined rather accurately.
Figure 7 shows an example of the basic screen of
the main program of a complex.
σρ (m), σ u (m/s)
Figure 7: Main screen of the VSATV program.
t (s) REFERENCES
σ α , σ β (deg.)
Boguslavsky A.A., Sazonov V.V., Smirnov A.I., Sokolov
S.M., Saigiraev K.S., 2004. Automatic Vision-based
Monitoring of the Spacecraft Docking Approach with
the International Space Station. In Proc. of the First
International Conference on Informatics in Control,
Automation and Robotics (ICINCO 2004), Setúbal,
Portugal, August 25-28, 2004, Vol. 2, p.79-86.
Chi-Chang, H.J., McClamroch, N.H., 1993. Automatic
spacecraft docking using computer vision-based
guidance and control techniques. In Journal of
Guidance, Control, and Dynamics, vol.16, no.2:
t (s) pp. 281-288.
Figure 6: Accuracy estimations for the motion presented Casonato, G., Palmerini, G.B., 2004. Visual techniques
on Figure 5. applied to the ATV/ISS rendezvous monitoring. In
Proc. of the IEEE Aerospace Conference, vol. 1,
pp. 619-625.
Canny, J. 1986. A computational approach to edge
5 CONCLUSION detection. In IEEE Trans. Pattern Anal. and Machine
Intelligence, 8(6): pp. 679-698.
Mikolajczyk, K., Zisserman, A., Schmid, C., 2003. Shape
The described complex is being prepared for the use
recognition with edge-based features. In Proc. of the
as a means allowing the ground operators to receive 14th British Machine Vision Conference
the information on the motion parameters of the (BMVC’2003), BMVA Press.
spacecraft docking to ISS in real time. Sonka, M., Hlavac, V., Boyle, R. 1999. Image Processing,
The most essential part of this information is Analysis and Machine Vision. MA: PWS-Kent.
transferred to the Earth (and was always transferred) Bard, Y., 1974. Nonlinear parameter estimation.
on the telemetering channel. It is also displayed on Academic Press, New York.
the monitor. However this so-called regular Seber, G.A.F., 1977. Linear regression analysis. John
information concerns the current moment and Wiley and Sons, New York.
without an additional processing can’t give a
complete picture of the process. Such an additional
291
NEW APPROACH TO GET AUTONOMOUS AND FAST ROBOT
LEARNING PROCESSES
R. Iglesias1 , M. Rodrı́guez1 , C.V. Regueiro2 , J. Correa1 , Pablo Quintı́a2 and S. Barro1

1 Electronics and Computer Science, University of Santiago de Compostela, Spain
2 Dept. of Electronic and Systems, University of Coruña, Spain
[email protected]
Keywords: Reinforcement learning, mobile robotics, robot control, autonomous agents, genetic algorithms.
Abstract: Research on robot techniques that are fast, user-friendly, and require little application-specific knowledge
by the user, is more and more encouraged in a society where the demand of home-care or domestic-service
robots is increasing continuously. In this context we propose a methodology which is able to achieve fast
convergences towards good robot-control policies, and reduce the random explorations the robot needs to
carry out in order to find the solutions. The performance of our approach is due to the mutual influence
that three different elements exert on each other: reinforcement learning, genetic algorithms, and a dynamic
representation of the environment around the robot.
The performance of our proposal is shown through its application to solve two common tasks in mobile
robotics.
1 INTRODUCTION Table 1: Truncated temporal differences algorithm.
1. Observe the current state, s(t): s[0]=s(t)

Reinforcement learning seems to be a very interest-
2. Select an action a(t) for s(t): a[0]=a(t)
ing strategy, since all the robot needs for learning a 3. Perform action a(t)
behaviour is a reinforcement function which tells the 4. Observe new state s(t+1) and reinforcement value
robot how good or bad it has performed but nothing r(t)
about the desired set of actions it should have carried 5. r[0]=r(t), u[0]=maxa Qt (s(t + 1), a)
out. 6. For k=0,1,...,m-1 do:
The maximization of the reinforcement allows the if k=0 then z = r[k] + γu[k]
robot to learn a utility function of states and actions else z = r[k] + γ(λz + (1 − λ)u[k]), 0 < γ, λ ≥ 1
7. Update the Q-values:
called Q-function, which reflects the consequences of δ = (z − Qt (s[m − 1], a[m − 1])])
executing every possible action in each state – we will ∆Qt (s[m − 1], a[m − 1]) = βδ
assume that the robot interacts with the environment
at discrete time steps and it is able to translate the dif-
ferent situations that it may detect through its sensors
into a finite number of states, S –. Table 1 shows pending on whether the robot receives or not negative
one of the many reinforcement learning algorithms reinforcements. The parameters γ, λ, and β (that ap-
that can be applied to teach a robot: the truncated pear in table 1), determine how much the Q-values are
temporal differences algorithm, T T D(λ, m) (Cichosz, changed for every positive or negative reinforcement
1997). Basically the robot begins with an initial set the robot receives. As the learning process progresses,
of random negative Q-values: Q(s, a) ≤ 0, ∀s, a, and the robot should tend to execute those actions which
then it performs a stochastical exploration of its envi- seem to be the best ones according to the Q-values,
ronment. While the robot is moving around, it keeps this is called greedy policy.
track of the m last executed actions so that their corre- Despite the benefits of the RL paradigm in au-
sponding Q-values will be decreased or increased de- tonomous robot-learning, there are important prob-
292
Figure 1: Schematic representation of our approach. Initially the robot moves using the greedy control policy until it finds a
situation it doesn’t know how to solve (a), a genetic algorithm is applied to find a solution (b), once the problem is solved the
greedy policy is applied again (c).
lems to consider when it is applied. First, the time

required to learn a good greedy policy increases expo-
nentially with the number of states and the number of
actions that are possible to execute in each state. On
the other hand, the robot wastes an enormous amount
of time trying actions that are clearly inappropriate
for the task but that are selected randomly during the
learning process.
2 OUR APPROACH
To solve part of the drawbacks just mentioned, we
propose a learning strategy which combines three el-
ements: reinforcement learning (RL), a genetic algo-
rithm (GA), and a dynamic representation of the en-
vironment around the robot (states). Basically, when
our approach is applied the robot goes through three
cyclical and clearly differentiated stages –figure 2–:
a) looking for a new starting position or convergence.
b) exploration, and c) generation of a new population
of solutions (chromosomes) for the next exploration
stage.
2.1 Looking for a New Starting Position
During this first stage the greedy policy is applied to

control the robot. If the robot encounters a situation
where it doesn’t know how to move – local problem,
(figure 1.a) –, it receives a sequence of consecutive
negative reinforcements, and its position several steps
before the failure is established as a new starting po-
sition for the next exploration stage. Figure 2: Flow diagram which describes the combination of
RL and GA.
2.2 Exploration Stage
lem the robot finds. The GA starts with a population
Our strategy applies a GA (Davidor, 1991; Cobb and of solutions called chromosomes. Each chromosome
Grefenstette, 1993) in order to solve each local prob- –represented as π– determines the action the robot has
293
to carry out at each state, s: π(s). The population contains the probability of finding the robot in each
of chromosomes is evaluated according to an objec- possible state in the long-term.
tive function called fitness function, which reflects for
how long a chromosome is able to properly control
the movement of the robot – figure 1.b –. Once a
population of chromosomes has been evaluated, the 3 EXPERIMENTAL RESULTS
sequence of states, actions, and rewards the robot re-
ceived under the control of the best chromosome, is We applied our approach to teach a mobile robot two
replicated off-line several times to speed up the con- common tasks: “wall following” and “door traver-
vergence of the Q-values. sal”. We have used a Nomad200 robot equipped with
16 ultrasound sensors encircling its upper part and
2.3 Generation of a New Population of bumpers. In all the experiments the linear velocity
of the robot was kept constant (15.24 cm/s), and the
Solutions (Chromosomes) robot received the commands it had to execute every
300ms.
The population of chromosomes has to be evolved We used a set of two layered Kohonen networks to
according to the fitness values. In order to do this, translate the large number of different situations that
certain genetic operators like mutation –which carries the ultrasound sensors located on the front and right
out random changes in the chromosomes–, or chro- side of the robot may detect, into a finite set of 220
mosome crossover –which combines chromosomes to neurones – states – (R. Iglesias and Barro, 1998).
raise new solutions– have to be applied. We use the
Q-values to bias the genetic operators and thus reduce
the number of chromosomes which are required to 3.1 Wall Following
find a solution. Given a particular chromosome π, the
probability that mutation changes the action that this To teach the robot how to follow a wall located on
chromosome suggests for a particular state s: π(s), its right at a certain distance interval, we used a rein-
depends on how many actions look better or worse forcement signal that is negative whenever the robot
than π(s) according to the Q-values. goes too far or too close from the wall being followed.
On the other hand, one of the chromosomes The robot was taught how to perform the task in a sim-
should always be the greedy policy because it brings ulated training environment, but its performance was
together all that has been already learnt by the robot, tested in a different one. Convergence was detected
and it represents the best chance to have a fast conver- when the greedy policy was able to properly control
gence towards the desired solution. the movement of the robot for an interval of 10 min-
Finally, when the robot is looking for a new start- utes.
ing position and the greedy policy is being used to
control it, if the robot moves properly during M steps
before it receives negative reinforcements, only the
states involved in the last K robot’s movements are
susceptible of being changed through the GA, while
the states involved in the initial M-K actions are la-
belled as learnt, so that neither chromosome selection
nor chromosome crossover can alter them.
The population of chromosomes is resized after
its evaluation according to how close the GA is to the
desired solution.
2.4 Dynamic Representation of States
We use the properties of the regular Markov chains Figure 3: Real robot’s trajectory along a corridor when a
(Bertsekas and Tsitsiklis, 1996) to reduce the num- control policy learnt through our approach was used. For
ber of states which are considered during the learn- a clear view of the trajectory, figure a) shows the robot’s
ing process. The transition matrix and what is called movement in one direction and b) shows the movement
steady vector are estimated, so that only those states along the opposite direction. Points 1 and 2 in both graphs
correspond to the same robot position. The small dots rep-
with a non-cero entry in the steady vector are con- resent the ultrasound readings.
sidered in the learning procedure. The steady vector
294
When we applied our approach the average re- 4 CONCLUSION

quired learning time after 19 experiments was only
25 minutes and 9 seconds –the number of chromo- In this article we suggest the use of a new approach
somes varied dynamically within the interval [3,20]. based on the combination of a genetic algorithm and
When the T T D(λ, m) was applied with the best com- the reinforcement learning paradigm to teach a robot
bination of parameters we found – β = 0.35, λ = 0.8, how to accomplish different tasks. Our approach is
γ = 0.95 and m = 30–, the average learning time after mainly characterized by three aspects: 1) There are
6 experiments was 97 minutes and 30 seconds. clearly differentiated exploration stages where the use
of a GA which keeps less information than the RL,
and through which the attention is only focused on
those actions which seem to be profitable, allows the
robot to carry out a fast search of solutions to those
situations where the robot doesn’t know how to move.
2) The information learnt through the reinforcements
– Q-values – is used to bias the genetic operators –
mutation, crossover, selection– and thus improve the
performance of the GA. 3) Finally, the properties of
the regular Markov chains represent a powerful tool
to focus the attention on those states relevant in the
robot’s long term behaviour, avoiding the use of a big
and unnecessary number of states which would delay
Figure 4: Real robot’s trajectory when the same control pol- the achievement of a good robot-control policy.
icy as in figure 3 was used.
To prove that the behaviours learnt through our ap- ACKNOWLEDGEMENTS

proach are useful, figures 3 and 4 show the movement
of the robot in two real and noisy environments. The authors thank the support from grants TIN2005-
03844, PGIDIT04TIC206011PR, TIC2003-09400-
3.2 Door Traversal C04-03.
We also applied our approach to teach a robot how

to cross a door in the experimental scenario shown REFERENCES
in figure 5. To learn this task the reinforcement is
negative whenever the robot collides with the door- Bertsekas, D. P. and Tsitsiklis, J. N. (1996). Neuro-Dynamic
frame, the robot goes too far from the door, or the Programming. Athena Scientific.
movement direction is so wrong that the robot is not Cichosz, P. (1997). Reinforcement Learning by Truncating
facing the door any more. After 21 experiments, the Temporal Differences. PhD thesis, Dpt. of Electron-
ics and Information Technology, Warsaw University
average learning time was 86 minutes and 42 seconds. of Technology.
Cobb, H. G. and Grefenstette, J. J. (1993). Genetic al-
gorithms for tracking changing environments. In
Proc. Fifth International Conference on Genetic Al-
gorithms.
Davidor, Y. (1991). Genetic algorithms and robotics. A
heuristic strategy for optimization. World Scientific.
R. Iglesias, C. V. Regueiro, J. C. and Barro, S. (1998). Im-
proving wall following behaviour in a mobile robot
using reinforcement learning. In ICSC International
Symposium on Engineering of Intelligent Systems,
EIS’98.
Figure 5: Experimental scenario for the door traversal be-

haviour. The initial positions of the robot were within the
shaded area.
295
OBSERVER BASED OPTIMAL CONTROL OF SHIP ELECTRIC
PROPULSION SYSTEM
Habib Dallagi1, Ali Sghaïer Tlili2

1
Académie Navale Menzel Bourguiba, BP 7050 , 1, 2 Ecole Polytechnique de Tunisie
BP. 743 2078 La Marsa, Tunisie
Samir Nejim1
1
Académie Navale Menzel Bourguiba, BP 7050 ,1, 2 Ecole Polytechnique de Tunisie
BP. 743 2078 La Marsa, Tunisie
nejim.samir @planet.tn
Keywords: Electric Propulsion Ship, Optimal Control, State Observer.
Abstract: This paper describes the synthesis of a linear state observer based optimal control of ship electric propulsion
using permanent magnet synchronous motor. The proposed approach is used for the ship speed control by
measuring the stator current and the motor speed. This strategy of control is made possible by using a ship
speed state observer. A numerical simulation study, applied to the global system, has confirmed the
efficiency and the good performances of the proposed control law.
1 INTRODUCTION
The characterization of industrial processes leads, in 2 MODELISATION OF THE
most cases, to nonlinear models which are generally ELECTRIC PROPULSION
difficult to control. The study of such processes was
generally used by a linearization leading to a linear SYSTEM
model on which the linear arsenal of controls can be
applied. These different control laws use often a
state feedback. However the state vector is not 2.1 Different Parts of the Ship Electric
always measurable, so it is necessary to use state Propulsion System
observers.
The work presented in this paper concerns the An electric ship is generally composed by two
modelisation of a ship electric propulsion system. principal parts ( Dallagi and Nejim, 2004 ).
The obtained global model is strongly nonlinear, - a first part ensuring the energy production using
coupled and presenting non measurable variables. several alternators drived either by diesel motors, or
Indeed, a linearization was firstly elaborated and the by gas turbines. It feeds the board network and the
synthesis of a control law with state feedback, for propulsion equipment.
the regulation of the stator current and the ship speed - a second part of electric propulsion composed by
of the synchronous motor, was secondly designed. one or two electric motors, each one of them driving
This control strategy is carried out using a linear a fixed blade propeller.
state observer allowing the ship speed
reconstruction.
This paper is organized as follows: the modeling of
2.2 Modelling of the Permanent
the different subsystems of the ship is developed in Magnet Synchronous Motor
the section 2. The linearization model of the global
system is elaborated in section 3. Section 4 is By the Park transformation, the voltage equations of
devoted to the optimal control development based on the permanent magnet synchronous motor are
state observer and in section 5 simulation resultats
written as follows (Grellet and Clerc, 2000):
are reported and discussed. Finally some conclusions
ended this work.
296
⎧ di d Rt : advance total resistance,

⎪⎪v d = R s i d + L d − pΩ L q i q R f : friction resistance,
dt
⎨ di q (1) R W : waves resistance,
⎪v q = R s i q + L q + pΩ Ld i d R app : appendices resistance,
⎪⎩ dt R air : air resistance.
The motor torque is given by: This modeling is based on the resistance tests of the
ship. Thus, total resistance to advance can be
(
C e = pΦ f iq + p Ld − Lq id iq ) (2) represented by the sum of four resistances (4). It is
obtained by applying different practical pulling tests
The mechanical equation can be written as on the similarity model (Izadi-Zamanabadi and
Blank, 2001. Doutreleau and Codier, 2001).
following: Resistance to advance can be modeled by a function
of the following form ( Dallagi and Nejim, 2006 ):
I m Ω = C e − Q prop (3)
with: Rt = av 2 (5)
Φf : inductor flux, with:
RS : stator phase resistance, v: ship speed,
vd : stator voltage longitudinal component, a: constant coefficient of the following curve.
vq : stator voltage transverse component,
id : stator current longitudinal component, Total resistance
2500000
iq : stator current transverse component,
Ld : longitudinal inductance,
Lq : transverse inductance,
Im : shaft inertia, 2000000
P : pole pairs numbers,

Ce : electromagnetic torque,
Qprop : propeller torque,
Ω: shaft speed. 1500000
2.3 Hull Resistance

1000000
During displacement, the ship is confronted to

several constraints among them, the sea state,
conditioned by the climatic data which is a
significant factor influencing the ship behaviour. 500000
The sea applies a resistance which is opposed to the

ship moving forward. Thus, to study the ship
movement, it is necessary, on the one hand, to model 0
its displacement and, on the other hand, to know the

1 2 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 32
constraints which are opposed to its movement as Speed (Nœuds)
presented in the figure 1 ( Dallagi and Nejim, 2006). Figure 2: Advance total resistance.
Text
v
2.4 Equations of the Propeller
Rt Hull
Motor
The model of propeller thrust can be written as
Propeller
T follows (Fosen and Blanke, 2000 ).( Guo, Zheng,
Wang and Shen, 2005):
Figure 1: Ship movement. T
KT = (6)
ρ N 2 D4
The advance total resistance to is given by:
with T the propeller thrust given by:
Rt = R f + R w + R app + R air (4)
T = ρ n 2 D4 KT
with: (7)
297
The model propeller torque can be written as mv = − R + ( 1 − t )T − Text (16)

follows:
Q prop 2.5 Global Model of the Ship Electric
KQ = (8)
ρ n2D5 Propulsion System
with Qprop the propeller torque given by: The global model of the ship electric propulsion
(9) using synchronous motor is represented by the
Q prop = ρ n 2 D 5 K Q following system.
The coefficients KT and KQ given respectively by (6) ⎧ x = f ( x ) + g( x ) u

and (8) depend on the following parameters (Izadi- ⎨ (17)
Zamanabadi and Blank): ⎩ y s = h( x )
va : advance speed (m/s), pa : propeller pitch, with:
v : ship speed (m/s), J : advance coefficient,
w : wake coefficient , n : propeller speed ( tr/s). ⎛( 1 / 2Π I m )[( p( Ld − Lq )id iq + pΦ f iq ) ⎞
Coefficients KT and KQ are given from ship practical ⎜ ⎟
tests. The advance coefficient is given by ⎜ − ( s1ρ n D ) − ( s2 ρ nD 2Π ( 1 − w )v )] ⎟
2 5 4
(Devauchell, 1986). (Lootsma, Izadi-Zamanabadi ⎜ ⎟

⎜ ⎟
and Nijmeijer, 2002): ⎜( 1 / m )[ −av2 + ( 1 − t ) r ρ D4n2 ⎟
2Π v a ⎜ 1
⎟
J= ⎜
nD (10) + ( 1 − t )2Π ( 1 − w ) r2 ρ D3 nv − Text ] ⎟
and the advance speed is written as: f(x) = ⎜ ⎟
⎜ ⎟
v a = ( 1 − w )v (11) ⎜ − Rs p2Π nLq ⎟
⎜ id + iq ⎟
⎜ Ld Ld ⎟
Coefficients KT and KQ can be represented by the ⎜ ⎟
affine lines having the following forms: ⎜ ⎟
Π Φ
⎜ − p2Π nLd id − Rs iq −
p 2 n f ⎟
K T = r1 + r2 J (12) ⎜ Lq Lq Lq ⎟
⎝ ⎠
KQ = s1 + s2 J (13) ⎡0 0 ⎤
The substitution of equations (10), (11), (12) in (7) ⎢0 ⎥
⎢ 1 0 ⎥
0 ⎥ and h( x ) = ⎡⎢ ⎤⎥
gives: n
g( x ) = ⎢
2Π ( 1 − w ) v ⎢ Ld ⎥ ⎣id ⎦
T = ρ n 2 D 4 ( r1 + r2 ) (14) ⎢ 1 ⎥
nD ⎢0 ⎥
by replacing the equations (10), (11) (13) in (9), it ⎣⎢ Lq ⎦⎥
yields: The following figure gives the structure of the ship
2Π ( 1 − w ) v electric propulsion system and its different
Q prop = ρ n 2 D 5 ( s 1 + s 2 ) (15)
nD subsystems:
The ship motion equation is given by (Fosen and
Blanke 2000):
Vd
id iq
Control MS Text
Reference And model n T Ship ys
1-t Tu Movement
Regulation Propeller Qprop
Vq
n Qprop va Advance resistance
R
Va 1-w
v̂
Figure 3: Synoptic of the ship propulsion system.
298
3 LINEARIZATION OF THE SHIP Lq Rs 2Πpn0 Lq

ELECTRIC PROPULSION cn = p , ciq =
iq0 , c id = −
Ld Ld Ld
SYSTEM
L Φf R
d n = − p d id 0 − p 2Π , d iq = − s
An industrial system is often intended to operate in Lq Lq Lq
regulation mode, i.e. the system output has to track p 2Π n0 L d
an imposed the reference signal despite of the d id =
various disturbances. Under these conditions, the Lq
use of nonlinear state representation for the
purpose of control is not necessary. A linear local
state representation is sufficient.
The linearization of (17), around an operating point 4 OPTIMAL CONTROL OF THE
characterized by (x0, y0, u0), is given by: SHIP ELECTRIC PROPULSION
⎧ x = Ax + Bu 4.1 Principle of the Optimal Control

⎨ (18)
⎩ y = Cx To obtain an optimal control law for the ship electric
propulsion system, we minimize the following
with: criterion (Toscano, 2005). (Rachid and Mehdi, 1997
[
x = n, v ,id ,iq ]T the state vector ( Corriou , 1996.):
u = [v q ]T
∞
1
vq the input vector, J=
2 ∫
( u T R u + ε T Q ε )dt (19)
y s = [n i d ] the output vector.
T 0
with:
A, B and C are the Jacobien matrices given by: R a symmetric positive definite matrix,
∂f ∂f ∂h Q a symmetric non-negative definite matrix,
A= x = x0 ; B = u =u0 and C = x = x0
∂x ∂u ∂x ε ( t ) = e( t ) − y( t ) is the difference between the
⎡ an av a id a iq ⎤ reference and the output vector.
⎢ ⎥ The control law is then given by:
⎢ 2Π I m 2Π I m 2Π I m 2Π I m ⎥
⎢ b bv ⎥
A=⎢ n 0 0 ⎥ u( t ) = Fe( t ) − Kx( t ) (20)
m m
⎢ c 0 c id c iq ⎥ where:
⎢ n ⎥
⎢⎣ d n 0 d id d iq ⎥⎦ . e( t ) = [i v ] is the reference vector.
dref ref
T
⎡ 0 0 ⎤
⎢ 0
⎢ 1 0 ⎥⎥ . K is control gain matrix defined by:
0 ⎥ , C = ⎡⎢
1 0 0 0⎤ K = R −1 B T P (21)
B=⎢ ⎥
⎢ Ld ⎥
⎢
⎢ 0
1 ⎥
⎥
⎣0 0 0 1⎦
. F is reference gain matrix given by:
⎢⎣ Lq ⎥⎦ F = R −1 B T ( AT − PBR −1 B T ) −1 PC T Q (22)
with:
a iq = 2 Π p Φ f + p ( L d − L q ) i d 0 with P the solution of the following Riccati
equation:
a v = − ρ 2ΠD 4 s 2 ( 1 − w0 ) n0 (23)
A T P + PA − PBR −1 B T P + C T QC = 0
a id = p( L d − L q )i d 0
an = −2s1 ρ D5n0 − s2 ρ 2ΠD4( 1 − w0 )v0 + p(Ld − Lq ) id0 4.2 The Ship Speed State Observer
To design the sate feedback optimal control law, it is
bn = 2a1ρ D4( 2Π )2( 1− t0 ) n0 +( 1−t0 )2Π ( 1− w0 ) a2ρ D3v0 necessary to reconstruct the ship speed v in order to
be controlled. For this purpose, we propose a linear
bv = −2 av0 + a 2 ρD 3 ( 1 − t0 )2Π ( 1 − w0 )n0 state observer using the output vector
y s = [i d n] and the vector u = u d u q . [ ]
299
with on the ship electric propulsion system using the

Electric
vref following characteristics.
Optimal Motor
idref Control + Ship
Para. value Par. Value
x̂ ρ 1025 Kg/m 3 Qn 3.1480010 5 Nm
v Vq D 5.9 m Tn 3.82000 10 5 N
Vd
m 20690000 kg, Qf 0.382 10 5
Observer id, n t 0.178 T ext. -1.8*0.1*10^5N
w 0.2304 s1 0.075
a 1.54 10 6 s2 0.1375
Figure 4: Control with ship speed observer.
r2 1.1 r1 0.5
The structure a luenberger observer is given by The resolution of the Riccati equation (23) yields to:
(Stoorvogel , Saberi and Chen 1994). ( Mieczarski, • The optimal control gain:
1988):
⎧⎪ x̂ = Ax̂ + Bu + L( y s − ŷ ) ⎡0.9807 0.0520 10.3830 − 0.0002⎤
⎨ (24) K=⎢ ⎥
⎪⎩ ŷ = Cx̂ ⎣0.1064 0.0467 9.7618 0.0030 ⎦
where:
• The observer gain:
x̂ is the output vector of the state observer
L is the observer gain ⎡ 3.5 1964.8 ⎤
This structure can be written in this form: ⎢− 10.6 − 339.6 ⎥
x̂ = Âx̂ + Bu + Ly s (25) L=⎢ ⎥
⎢ 0.1 6.6 ⎥
⎢ ⎥
with : Â = A − LC ⎣ 10.3 1362.9 ⎦
To have an asymptotic convergence of the The resolution of the equation (22) gives the
observed state towards the real state, it is necessary reference gain:
to choose the gain L such that the matrix ( A − LC )
has negative real part eigen values. The control law ⎡0.9963 − 3.05078⎤
F=⎢ ⎥
using the state observer is presented as follows: ⎣0.1064 26.3032 ⎦
u( t ) = Fe( t ) − K x̂( t ) (26) For the designed control, we impose idref=0, so that
the electromagnetic torque Ce will be proportional to
e(t) F u(t) ys(t) iq.
+ System In order to control the ship speed v it is
necessary to change the motor speed n through the
stator component iq which modify the
L
electromagnetic torque ( Dallagi and Nejim, 2005 ).
The performances of the proposed strategy
control law are depicted in the figures 6, 7, 8 and 9.
x̂ The ship speed is needed to reach the reference
B ∫
+ speed value Vref=8m/s in the interval [0 150s] and
Vref=10m/s in the interval [150 300s].
It appears from figure 6, that the proposed
A-LC
control law allows a convergence towards the
desired value of the ship speed v.
-K
The figure 7 shows the behavior of the motor speed.
It's clear that the ship speed changes where the
Figure 5: Observed state feedback control.
variation of the propeller speed changes.
Furthermore, we impose idref=0, so the
electromagnetic torque becomes proportional to
5 SIMULATION RESULTS statoric current iq.
SECOND SECTION In order to control the motor speed n, one modify the
electromagnetic torque Cem by changing the stator
A digital simulation of the proposed control law current iq through the regulation of the voltage
with the designed state observer has been carried out component vq.( Grellet and Clerc,2000).
300
6 CONCLUSIONS
In this paper we have proposed an optimal control
law using a Luenberger sate observer to control the
ship speed.
The designed observer is used to reconstruct the
ship speed in order to complete the control strategy.
It has been shown from the simulated results that the
proposed estimated state feedback optimal control
permits the regulation of the ship speed which
Figure 6: Ship Speed. converges exactly to the imposed reference.
REFERENCES
Dallagi, H., Nejim, S., 2004. Modélisation et simulation
d’un système de propulsion diesel-électrique de
Navire. In 3ème Conférence Internationale, JTEA,
Tunisie.
Grellet,Guy., Clerc, Guy., 2000. Actionneurs Electriques,
principe modèle et commande. In Editions Eyrolles.
Dallagi, H,. Nejim, S., 2006. Conception d’un programme
de prédiction et d’estimation de la puissance
Figure 7: Motor Speed. propulsive des navires à propulsion électrique. In 4ème
Conférence Internationale, JTEA, Tunisie.
Izadi-Zamanabadi, R, Blanke, M., Katebi, S.2001. Cheap
diagnostisis using structural modelling and fuzzy-logic
based detection.
Fosen, T.I Blanke, M. 2000. Nonlinear output feedback
control of underwater vehicle propellers usingfeedbak
from estimated axial flow velocity. In IEEE journal of
oceanic engineering, vol 25, no2
Guo Y, Zheng, Y Wang B Shen A. 2005. Design of ship
electric propulsion simulation system. In Proceeding
of the fourth international conference on machine
learning and cybernetics. Guangzhou.
Devauchell, P., 1986 “Dynamique du Navire. In Masson,
Figure 8: Current id. Paris.
Toscano, R,. 2005. Commande et diagnostic des système
dynamique,. In Elipses Edition , Paris.
Corriou J.P,. 1996. Commande des procédés. In
Techniques et documentations, Paris.
Rachid, A,. Mehdi, D,. 1997. Réalisation réduction et
commande des systèmes linéaire. In édition technip
Paris.
Stoorvogel , A.A., Saberi, A., Chen, B.M., 1994. A
Reduced order observer base control design for
optimization. In IEEE transaction automatic
Mieczarski.,W., 1988. Very fast linear and nonlinear
observer. In int. J. control.
Dallagi, H., Nejim,S., 2005. Modélisation and Simulation
of an Electric Drive for a Two Propeller Ship. In 17ème
Figure 9: Current iq. Congrès Mondial IMACS, Calcul Scientifique,
Mathématiques Appliquées et Simulation, Paris,
France.
Doutreleau, Y., Codier ,.S. 2001. Les problèmes des
navires à grande vitesse. In J.T .
Snitchler G., Gambe B.B., Kalsi S., Ige, S.O. 2006. The
stats of HTS ship propulsion motor developments. In
IEEE Transaction on applied superconductivity.
301
Kalsi S, Gambe B.B, Snitchler G,. 2005. The performance

of 5Mw high temperature superconductor ship
propulsion motor. In IEEE Transaction on applied
superconductivity, vol, 15, n°2
Dallagi, H., Nejim, S., 2005. Optimization of an
integrated power system of an electric ship. In
International Conference on ship propulsion and
railway traction systems, Bologna–Italy,
Zimin, W.Vilar., Roger, A.Douglas., 2005. Effectiveness
of generator Strategies on meeting pulsed load
requirements in ship electric system. In IEEE electric
ship technologies symposium.
Izadi-Zamanabadi R. Blank, M,. A ship propulsion
system model for fault-tolerant Control,” Department
of control Engineering Aalborg university,
Dennemark.
Gillmer T.C., Jonson B,. 1982. Propulsive force and
propulsion system. In Naval Institue Press, Annapolis,
Maryland.
Blanke, M., Izadi-Zamanabadi, R,. 1998. Reconfigurable
control of a ship propulsion plant. In Control
Applications in Marine Systems, CAMS, Fukuoka,
Japan.
Lootsma, T.F., Zamanabadi, R.I., Nijmeijer, H., 2002. A
Geometric approach to diagnosis applied to a ship
propulsion problem. In 15th Triennal World Congress,
IFAC , Barcelona, Spain.
Steinar J.Dale., 2005. Ship power system testing and
simulation. In IEEE Electric ship technology
symposium.
Rudly, Limpaecher., 2000. Novel converters for electric
ship propulsion system and shipboard power
distribution. In IEEE transactions on energy
conversion.
302
A NOVEL BLOCK MOTION ESTIMATION MODEL FOR VIDEO
STABILIZATION APPLICATIONS
Harish Bhaskar and Helmut Bez

Research School of Informatics, Loughborough University
Keywords: Video stabilization, motion compensation, motion estimation, genetic algorithms, kalman filtering.
Abstract: Video stabilization algorithms primarily aim at generating stabilized image sequences by removing unwanted
shake due to small camera movements. It is important to perform video stabilization in order to assure more
effective high level video analysis. In this paper, we propose novel motion correction schemes based on
probabilistic filters in the context of block matching motion estimation for efficient video stabilization. We
present a detailed overview of the model and compare our model against other block matching schemes on
several real-time and synthetic data sets.
1 INTRODUCTION frames. (T.Chen, 2000) propose a 3 stage video sta-

bilization algorithm based on motion estimation. The
Video data obtained from compact motion capture de- process includes motion estimation for computing lo-
vices such as hand-held, head mounted cameras, etc. cal and global motion parameters, motion smoothing
has gained significant attention in recent years. Video for removing abrupt motion changes between sub-
stabilization, as the name suggests, deals with gen- sequent frame pairs and finally a motion correction
erating stabilized video sequences by removing un- methodology for stabilization. In this paper we ex-
wanted shakes and camera motion. Several meth- tend the work presented in (T.Chen, 2000) to accom-
ods have been proposed in the literature for ac- modate a novel motion correction mechanism based
complishing video stabilization. However, the ac- on moving average filters and Kalman filtering along-
curacy of motion estimation is a key to the per- side a motion estimation strategy that combines vec-
formance of video stabilization. (Y.Matsushita and tor quantization based block partitioning with a ge-
H.Y.Shum, 2005) propose a combination of motion netic algorithm based block search for motion esti-
inpainting and deblurring techniques to accomplish mation.
robust video stabilization. Several other research
contributions have been made to video stabiliza-
tion including, probabilistic methods (A.Litvin and 2 PROPOSED MODEL
W.C.Karl, 2003), model based methods, etc. Meth-
ods such as (M.Hansen and K.Dana, 1994)(Y.Yao and The video stabilization model proposed in this pa-
R.Chellappa, 1995)(P.Pochec, 1995)(J.Tucker and per extends a parametric motion model proposed in
Lazaro, 1993)(K.Uomori and Y.Kitamura, 1990), pro- (T.Chen, 2000). A detailed overview of the proposed
pose to combine global motion estimation with filter- model in the form of a pseudo code is as follows.
ing to remove motion artifacts from video sequences.
These schemes perform efficiently only under re- • Input at a time instant t two successive frame pairs
stricted conditions and are again limited by the effi- of a video sequence, ft & ft+1 where 1 ≤ t ≤ N ,
ciency of the global motion estimation methodology. where N is total number of frames in the video
(K.Ratakonda, 1998) have used an integral match- • Image frame ft is initially partitioned into 4
ing mechanism for compensating movement between blocks using the vector quantization algorithm
303
described in the subsection below, Note: Every 2.1.1 Block Partitioning Based on Vector
block represents an image region Quantization
• For every block b
For the block partitioning phase, we start by using
– The centroid (xc , yc ) of the block is computed vector quantization to provide the block matching
– A genetic algorithm as described below is used scheme with the position of partitioning.
to accurately match the block in the successive
frame ft+1 • Set the number of codewords, or size of the code-
– If the genetic algorithm accurately matched the book to 4. This assumes that we need 4 regions to
block in frame ft to frame ft+1 (with error emerge out of the image frame during the quanti-
= 0), then the motion vector is evaluated as zation process.
(x∗ − x, y ∗ − y) where (x∗ , y ∗ ) is the estimated • Initialize the positions of the codewords to
transformed centroid of the block in frame ft+1 ( w4 , h4 ), ( w4 , 3h 3w 3h 3w 3h
4 ), ( 4 , 4 ), ( 4 , 4 ) where (w, h)
– If the genetic algorithm returned non-zero is the width and height of the block respectively.
matching error then the process is repeated by By this we assume that the worst case partition
further sub dividing block. could be the quad-tree partition.
• The process is terminated either when no further • Determine the distance of every pixel from the
splitting is needed or a predefined block size is codewords using a specific criterion. The distance
reached. measure is the sum of differences in the gray in-
• If the processed frame pair is (ft , ft+1 ) where t = tensity and the locations of the pixels.
1, then proceed to next frame pair, otherwise if • Group pixels that have the least distance to their
t > 1, then run motion correction using any of the respective codewords.
proposed filter mechanisms specified to generate • Iterate the process again by recomputing the code-
smoothed motion vectors M Vℵ word as the average of each codeword group
• Compute the difference between the original mo- (class). If m is the number of vectors in each class
tion vectors M V and the smoothed motion vec- then,
tors M Vℵ adjust the original motion vectors by m
1 X
the factor of difference M Vcomp = M V ±(M V − CW = xj (2)
m j=1
M Vℵ )
• Generate Stabilized frames using the original mo- • Repeat until either the codewords don’t change or
tion vector M V and compensated motion vectors the change in the codewords is small
∗ ∗comp
M Vcomp and represent them as ft+1 and ft+1 • Associated with these 4 codewords, there are 4
• Deduce the PSNR of the two versions of stabi- configurations possible for partitioning the image
lized frames using, PSNR for a gray scale image frame into blocks. The configurations arise if we
is defined as: assume one square block per configuration. It is
" #
2552 logical thereafter to find the best configuration as
10 log10 1
P P 2 the center of mass of these 4 possible configura-
HW H W kft+1 − fcomp k tions. The center of mass will now be the partition
(1) that splits the image frame into blocks.
where, (H, W ) is the dimensionality of the frames
and ft+1 and fc omp are the intensity components 2.1.2 Genetic Algorithm Search
of the original target and the motion compensated
∗ ∗comp
images which will equal ft+1 and ft+1 re- The inputs to the genetic algorithm are the block bt
spectively. PSNR values generally range between and the centroid (xc , yc ) of the block.
20dB and 40dB; higher values of PSNR indicate
better quality of motion estimation. • Population Initialization: A population P of these
∗comp
n chromosomes representing (Tx , Ty , θ) is gener-
• If P SN Rcomp ≥ P SN R then use ft+1 as sta- ated from uniformly distributed random numbers
bilized frame for subsequent analysis otherwise where,
∗
use ft+1 .
– 1 ≤ n ≤ limit and limit (100) is the maxi-
2.1 Motion Estimation mum size of the population that is user defined.
• To evaluate the fitness E(n) for every chromo-
A brief description of the algorithms is specified. some n:
304
A NOVEL BLOCK MOTION ESTIMATION MODEL FOR VIDEO STABILIZATION APPLICATIONS
– Extract the pixels locations corresponding to where, 1 ≤ i ≤ J (Use these weighting factors as
the block from frame ft using the centroid a kernel for the convolution process)
(xc , yc ) and block size information • Generate a vector of the motion vectors and rota-
– Affine Transforming these pixels using the tion parameter theta across all frames; M V and
translation parameters (Tx , Ty ) and rotation an- θ
gle
2
θ 3using,
2 32 32 3
x′ cosθ −sinθ 0 1 0 Tx x • Perform Convolution to generate the smoothed
4 5=4 54 54 5
y′
1
sinθ
0
cosθ
0
0
1
0
0
1
0
Ty
1
y
1
motion vectors, M Vℵ = M V ⊗w and θℵ = θ⊗w
– If bt represents the original block under consid-
2.2.2 Kalman Filter
eration, b∗t+1 represents the block identified at
the destination frame after transformation and
A 2D Kalman filter can be used to predict motion vec-
(h, w) the dimensions of the block, then the fit-
tor of successive frames given the observation or mo-
ness E can be measured as the mean absolute
tion vectors of the previous frames. An algorithm de-
difference (MAD).
scribing the smoothing process is listed below.
h w
1 X X • Initialize the state of the system using
M AD = bt (i, j) − b∗t+1 (i, j)
hw i=1 j=1 (x, y, dx, dy), where (x, y) is the observa-
(3) tion (i.e. the centroid of the block) and (dx, dy) is
the displacement of the centroids. The values of
• Optimization: Determine the chromosome with state can be initialized using the motion estimates
minimum error nemin = n where E is mini- between the first successive frame pair.
mum. As this represents a pixel in the block,
• The state of the system S at time instant t + 1 and
determine all the neighbors (N Hk ) of the pixel,
the observation M at time t can be modeled using
where 1 ≤ k ≤ 8.
– For all k, determine the error of matching as in S(t + 1) = AS(t) + N oise(Q) (4)
Fitness evaluation. M (t) = S(t) + N oise(R) (5)
– If E(N Hk ) < E(nemin ), then nemin = N Hk
• Initialize A and noises Q, R as Gaussian.
• Selection: Define selection probabilities to select
• Perform the predict and update steps of standard
chromosomes for mutation or cloning. Perform
Kalman filter
cross-over and mutation operations by swapping
random genes and using uniform random values. – Initialize state at time instant t0 using
−1
S
0 = B M0 and error covariance U0 =
• Termination: Three termination criterion such as
∈ 0
zero error, maximum generations and stall gener-
0 ∈
ations. Check if any condition is satisfied, other-
wise iterate until termination. – Iterate between the predict and update steps
– Predict: Estimate the state at time instant t + 1
2.2 Motion Smoothing using Sk− = ASk−1 and measure the predicted
error covariance as Uk− = AUk−1 AT + Q
The work of (T.Chen, 2000) suggested the use of a – Update: Update the correct, state of the system
moving average low pass filter for this process. In Sk = Sk− + K(Mk − BSk− ) and error covari-
this paper, we extend the moving average filter to an ance as Uk = (I − KB)U −
exponentially weighted moving average filter. – Compute K, the Kalman gain using K =
Uk− B T (BUk− B T + R)−1
2.2.1 Exponentially Weighted Moving Average
Filter • Smooth the estimates of the Kalman filter and
present the smoothed outcomes as M Vℵ
A detailed pseudo code describing the process is as
follows.
• Set the number of frame pairs across which the 3 RESULTS AND DISCUSSION
moving average filter to be any scalar J
Here, in this section, we present some sample results
• Compute the parameter alpha ∝ = (1 ÷ J) of the stabilization task on wildlife videos taken at
• Compute the weighting factors for every frame a zoological park. Performance of the video stabi-
pair between 1 and J as w =∝i−1 ×(1− ∝), lization scheme can only be visually evaluated. We
305
Baseline Model
provide some sample frames illustrating the quality

of video stabilization. Figure 1 compare the video
stabilization quality of the base-line model versus the
proposed model. As we can clearly visualize there
is quite a increased quality in the stabilized version
of the proposed model in comparison to the stabi- Unstabilized Frame Stabilized Frame
lized version of the base model. The motion correc- Proposed Model
tion scheme using the Kalman filter was sufficient to

smooth the motion vector correctly. The reason to this
is because, the changes observed in the capture was
linear. Similarly in figures 2, we compare the quality
of video stabilization using another sample clip from
Unstabilized Frame Stabilized Frame
the same wildlife video. The movement of the cam-

era in this sequence was more abrupt and random in Figure 2: Model Performance on Video Sample Clip 6.
directions. We observed that the proposed model us-
ing Kalman filter could not handle these changes well
and as well generate a good quality stabilized output.
However, the motion correction mechanism using the REFERENCES
exponentially weighted moving average filter could
produce much better results. A.Litvin, J. and W.C.Karl (2003). Probabilistic video stabi-
lization using kalman filtering and mosaicking. Image
Baseline Model
and Video Communication and Processing.
J.Tucker and Lazaro, A. S. (1993). Image stabilization for a
camera on a moving platform. In IEEE Conference on
Communications, Computers and Signal Processing,
pages 734 – 737.
K.Ratakonda (1998). Real-time digital video stabilization
for multi-media applications. In Proceeding of the
1998 IEEE International Symposium on Circuits and
Proposed Model
Systems, pages 69–72.
K.Uomori, A.Morimura, H. T. and Y.Kitamura (1990). Au-
tomatic image stabilizing system by full-digital signal
processing. In IEEE Transactions on Consumer Elec-
tronics, pages 510–519.
M.Hansen, P. and K.Dana (1994). Real-time scene stabi-
lization and mosaic construction. Image Understand-
ing Workshop Proceedings.
P.Pochec (1995). Moire based stereo matching technique.

Figure 1: Model Performances on Video Sample Clip 3. In Proceedings of ICIP, pages 370–373.
T.Chen (2000). Video stabilization algorithm using a block
based parametric motion model. Master’s thesis.
Y.Matsushita, E.Ofek, X. and H.Y.Shum (2005). Full-frame
4 CONCLUSION video stabilization. In IEEE International Conference
on Computer Vision and Pattern Recognition.
In this paper, we have presented a novel mechanism Y.Yao, P. and R.Chellappa (1995). Electronic image stabi-
of motion correction and block based motion estima- lization using multiple image cues. In Proceedings of
tion strategy that combines vector quantization based ICIP, pages 191–194.
block partitioning mechanism with the genetic algo-
rithm based block search scheme applied to video sta-
bilization. The model was tested on several real time
datasets and the results have revealed a high degree
of performance improvement when compared to ex-
isting video stabilization model based on motion esti-
mation and filtering.
306
THE PROTOTYPE OF HUMAN – ROBOT INTERACTIVE VOICE
CONTROL SYSTEM
Miroslav Holada, Igor Kopetschke, Pavel Pirkl, Martin Pelc, Lukáš Matela, Jiří Horčička
and Jakub Štilec
Faculty of Mechatronics and Interdisciplinary Engineering Studies, Technical University of Liberec
Hálkova 6, Liberec, Czech Republic
{miroslav.holada, igor.kopetschke, pavel.pirkl, martin.pelc, lukas.matela, jiri.horcicka, jakub.stilec}@tul.cz
Keywords: Distributed speech recognition, robot control, image processing.
Abstract: This contribution shows our recent activities of human – robot voice-control system. The keynote lays in
design of a system that allows easy control and programming of various robots by a uniform interactive
interface. Generally the robots are actuated by sets of control commands, sometimes by a manual control
interface (touchpad, joystick). The operator has to know the control commands, syntax rules and other
properties necessary for successful robot control. Our system offers commands like “move left” or “elevate
arm” that are translated and sent into the corresponding device (robot). Speech recognition is based on an
isolated-word HMM engine with distributed recognition system (DSR). The vocabulary may contain
thousands of word and short phrases. It allows us to design an easy dialogue scenario for the robot control
system. The system allows direct robot control like moving or programming short sequences. A video
camera is included for displaying the working place and employed algorithms of image processing allow the
system to detect the position of objects in the working area and facilitate robot’s navigation.
1 INTRODUCTION system as shown in figure 1. The components are

described below.
The goal of this project is a PC-software-based
interactive system for general-purpose robot voice- 2.1 Scene Manager
control. This paper describes the designed prototype,
its structure and the dialogue strategy in particular. The scene manager forms a connection between the
The interactive control of robots could be used in main program (engine) and the image processing
special situations, when a robot is working in part. It actually controls the image processing
dangerous areas and no programming beforehand is module and initiates image acquisition and
possible. It could also be used in a situation when processing. Using the processed image data, it
supervised learning for robot’s later autonomous updates the scene database, keeps track of objects
operation has to be done, without knowledge about exposed on the scene and provides the scene object
the robot’s programming language. The presented and image data to the main engine. It is also aware
paper follows on this reality. of the robot's coordinate system and plans the
robot’s movement when requested by the engine.
The database itself consists of two types of data.
It contains the list of parametrized objects detected
2 PROJECT FEATURES on the scene as well as the robot calibration data.
The latter allows mutual image-space to robot-space
The project is based on a former research. The coordinate translation which is used in robot
research involved a voice-control dialog system, navigation. Each object detected on the scene is
speech recognition vocabulary design and speech internally represented as a data object (class
synthesis feedback for user command confirmation. instance), all the objects are stored in a dynamic list.
Together with a scene manager and a digital image Some of the attributes are: a unique object identifier,
processing module, it forms the core of the control object's shape descriptor, central point coordinates,
307
Robotic Control Microphone

Sensors Module
Headphones
RS232
Events manager
Robot 1 TCP/IP
Voice Recogniser
control (HMM)
CAN
Main project engine &
Robot 2 with GUI Dialogue
manager
TTS
Cameras
Repository
(Solved tasks,
commands, program,
Digital image Scene manager dialogue log)
processing module
- collision detection
- calibration - trajectory
- object tracking
- image
enhancement Scene image
- segmentation output
- object detection DB scene
- ... objects, working area
robot’s range
Figure 1: Scheme of the designed system.
bounding rectangle etc. Such data allows smooth compensate geometric distortions introduced by a
object manipulation and serves as a base for object camera lens.
collision avoidance along the manipulation
trajectory.
The scene manager also combines unprocessed
camera image with scene data to highlight detected
objects and to present them to the user via a GUI as
shown in figure 2. The user has a view of the
computer's scene understanding and may correctly
designate objects of interest in his or her commands.
Being in its early stages, the project currently Figure 2: Working scene with highlighted objects of
works only with 2D data and relies on the user's z- circular shape and robot’s coordinate system.
axis navigation aid. The system is expected to
incorporate a second camera and 3D computer Digital image processing methods are placed in a
vision in the future to become fully 3D aware. DIP library which is served by the scene manager
with the object database. Figure 2 shows the circular
2.2 Image Recognition object detection using the reliable Hough transform
(HT). HT is commonly used for line or circle
The robot’s working area is captured by a colour detection but could be extended to identify positions
high-resolution digital camera (AVT Marlin F-146C, of arbitrary parametrizable shapes. Such edge-based
1/2’’ CCD sensor). The camera is placed directly object detection is not too sensitive to imperfect
above the scene in a fixed position. We implemented input data or noise. Using a touch-display or verbal
a simple interactive method to synchronize the commands it is possible to focus the robot onto a
robot’s coordinate system (XY) and the camera’s chosen object (differentiated by its color or
one using pixel units. We prepare modifications to numbering) and than tell the robot what to do.
308
THE PROTOTYPE OF HUMAN – ROBOT INTERACTIVE VOICE CONTROL SYSTEM
2.3 Robots Description 3 DIALOGUE STRATEGY

The prototype system uses a compact industrial The voice interface between an operator and the
general-purpose robotic arm (ABB IRB 140). The controlled process is provided by a speech
robot is a 6-axes machine with fast acceleration, recogniser and a text-to-speech synthesis (TTS)
wide working area and high payload. It is driven by system (both for Czech language). The TTS
a high performance industrial motion control unit synthesis system named EPOS was developed by
(S4Cplus) which employs the RAPID programming URE AV Prague. It allows various male or female
language. The control unit offers extensive voices with many options of setting.
communication capabilities - FieldBus, two Ehternet The speech recognition is based on a proprietary
channels and two RS-232 channels. The serial isolated word engine that was developed in previous
channel was chosen for communication between the projects. The recogniser is speaker independent,
robot and the developed control system running on a noise robust, phoneme based with 3-state HMM
PC. (Hidden Markov Models) and 32 Gaussians. It is
suitable for large vocabularies (up 10k words or
short phrases) and allows us to apply various
commands and their synonyms.
The dialog system is event-driven. We can
categorize the events into three fundamental
branches: operator events, scene manager events and
device events.
3.1 Operator Events

Operator events usually occur in response to
operator’s requests. For example commands which
Figure 3: Robots used. are supposed to cause robot’s movement, object
detection, new command definition or detection of a
The robotic control SW module simplifies the new object. This kind of event can occur at any time,
robot use from the main engine's point of view. It but the dialog manager has to resolve if it was a
abstracts from the aspects of physical relevant and feasible request or if it was just a
communication and robot's programming interface. random speech recognition error.
It either accepts or refuses movement commands Although the acoustic conditions in robotic
issued by the core engine (depending on command's applications usually involve high background noises
feasibility). When a command is accepted, it is (servos, air-pump), the speech recogniser works
carried out asynchronously, only notifying the commonly with 98% recognition score. If the
engine once the command is completed. operator says a wrong command or a command out
of context (for example, the operator says “drop” but
2.4 Distributed Computation the robot doesn’t hold anything) then the manager
asks him or her for a feasible command in the stead
Most of the system's modules are developed and run of the nonsensical one.
on a standard PC to which the robot is connected.
Since some of the software modules require 3.2 Scene Manager Events
significant computational power, the system's
response time was far from satisfactory when the This sort of event occurs when the scene manager
whole system ran on a single computer. Therefore, detects a discrepancy in the scene. For example
the most demanding computations (namely the when the operator says “move up” and the robot’s
object recognition and the voice recognition) were arm moves all the way up until the maximum range
distributed to other (high performance) computers is reached. When this happens a scene event is
via network (TCP connections). generated and the system indicates that the top
position was reached.
Other scene event occurs when the operator
wants to take up an object, but the system does not
know which one because of multiple objects
309
detected on the scene. This event generates a query “Repeat command”

to the operator for proper object specification. “Enter command”
“Search disks”
“OK”
3.3 Device Events ....
“No object found. Repeating done.”
These events are produced by external sensors and
other components and devices connected to the The robot finds the remaining three disks and
system. They are processed in the event manager puts them in the selected area. If any disk cannot be
where corresponding action is taken. The response found then the robot interrupts executing the given
manifests itself in the form of a request for the command and waits for next action.
operator, or more often causes a change in robot’s
behaviour.
The difference between scene manager events
and device events is that scene events are generated
4 CONCLUSIONS
by the system itself (based on a known scenario,
robot geometry, object shape and position). They are The system is especially usable as an accessory
computed and predictable. On the other hand, device robot control interface for assistant and second-rate
events’ time cannot be exactly predicted before they operations. The presented prototype now cooperates
actually happen. only with one industry robot (ABB) but the robotic
control module may easily be extended to support
other robots (Katana, mobile robots, etc.) as well.
3.4 Example of Dialog
The designed system offers robot control and
robot task programming even to people without
For a simpler robot orientation and navigation the
explicit programming language knowledge. It is
positions on the scene are virtualized. They are
sufficient for the operator to know the Czech voice
named by Greek letters like “Position α” or
interface of the presented system.
“Position β”. These virtual positions may be
redefined to suit the operator’s needs. A blind-area
may also be defined and it is completely omitted
from any image processing and anything in this area ACKNOWLEDGEMENTS
is completely ignored.
We can gather up black disks (see figure 2.) and This work was supported by the internal grant IG
put them to some other place on the scene. This FM 2007/002.
place is defined like “Position α” and we setup the
“blind area” on the same coordinates. After that the
operator starts a dialog, for example: REFERENCES
“Start recording new command.”
Nouza, J., Nouza, T.: A Voice Dictation System for a
“I’m recording” ...robot says
Million-Word Czech Vocabulary. In: Proc. of ICCCT
“Search objects”
2004, August 2004, Austin, USA, pp. 149-152,
“I’m searching ... Four objects were
Holada, M.: The experiences and usability of distributed
found”
speech recognition system DUNDIS. In: Proc. of 14th
“Move on first”
Czech-German Workshop „Speech Processing",
“I’m moving ... Done”
September 2004, Prague, Czech Republic, pp. 159-
“Take it.”
“Ok” 162,
Hanika, J, Horak, P.: Text to Speech Control Protocol. In:
“Move on position alpha.”
“I’m moving ... Done” Proc of the Int. Conf. Eurospeech'99, Budapest,
“Put it” Hungary, September 5-9, 1999, Vol. 5, pp. 2143-2146.
“Ok” Šonka, M., Hlaváč, V., Boyle, R. D., Image Processing,
“Stop recording.” Analysis and Machine Vision. PWS, Boston, USA,
“I stop the recording. Please, say second edition, 1998.
new command”
“Search” “Disks” “Done”
“New command is entered and named:
Search disks. Is it right.”
“Yes”
310
DEVELOPMENT OF AN AUTOMATED DEVICE
FOR SORTING SEEDS
Application on Sunflower Seeds
Vincent Muracciole, Patrick Plainchault

INRA;ESEO;LISA;GEVES-SNES, Université d’Angers, Angers, France
CER ESEO, Angers, France
Dominique Bertrand, Maria Rosaria Mannino

ENITIAA-INRA, Sensométrie - Chimiométrie, Nantes, France
GEVES-SNES, Beaucouzé, France
Keywords: Machine vision, computer vision, seed identification, grain quality, seed quality, embedded systems.
Abstract: Purity analysis and determination of other seeds by number are still made manually. It is a repetitive task based
upon visual analysis. Our work objective is to create and use a simple and quick automated system to do this
task. A first step of this machine has been reached by validating the image acquisition and feeding process.
The principle of this machine is based on a seeds fall with stroboscopic effect image acquisition. This article
presents the first step of creating a dedicated and autonomous machine which combines embedded constraints
and real time processes.
1 INTRODUCTION ators to be trained. It is thus very desirable to develop

an automaton making it possible to do such controls.
In most countries, seeds cannot be commercialized Some mechanical devices are commonly used to
without an assessment of their quality. Quality control clean commercial seeds and separate them according
tests generally includes the evaluation of unwanted to their qualitative nature. It seems possible to add an
materials in the batches. This control is currently artificial vision system for completing the mechani-
performed manually by operators able to separate the cal step. The artificial vision domain follows closely
pure seeds from impurities. the quick growth of computer power. This evolu-
In France, the official assessment of seeds is per- tion allows creating new applications which was not
formed by GEVES (Groupe d’Etude et de contrle des possible until today. (Jayas D.S and Bulley, 1999)
Varits; Variety and seed study and control group). The (Moltó Enrique, 1998) (Egelberg P., 1994) (Pearson,
standardized methods of the ISTA (International Seed 1994) (Wan, 2002) (Bennett, 2005)
Testing Association) and of the AOSA (Association Image analysis is not currently restrained to sim-
of Official Seed Analysts) are applicable to two kinds ple geometrical shapes and can be applied to complex
of analyses of the seeds: purity analysis and deter- objects such as biological materials.
mination of other seeds by number. In the analytical All the work dealing with the identification of
analysis of purity, all the elements in a sample (pure seeds proceed in two steps: extraction of features
seeds (PS), inert matter (IM) and seeds of other plants from a digital images of seeds, processing the fea-
(SOP)) are identified, separated and weighed. The re- tures for building up a discriminant model. In most
sult is then given as percentage of each part’s weight. of the published studies, the proportions of correctly
In the determination of other seeds by number classified seeds reported by the authors hardly exceed
analysis, only the impurities are separated, identified 95 percent. Such figures are encouraging but proba-
and counted. The standardized methods of purity bly not sufficient to allow the replacement of human
analysis are very requiring since a one hundred per- operators by machines.
cent correct identification is expected. The visual in- Most of the recent studies are based on the
spection of seeds is time-consuming and needs oper- use of very complex discriminant methods involv-
311
ing neural or Bayesian approaches (Chtioui, 1997) and increase efficiency.

(Granitto P.M. and Ceccatto, 2003) (Granitto
Pablo M and Ceccatto, 2003) (Majumdar S., 2000b).
These methods are often efficient but involve a very
time-consuming training phase. The parameters of
these predictive models (Kernels, neural weights) re- Figure 1: Vision chain.
quire to be stored in computer memories.
In a previous work, a vision system for the auto-
matic acquisition of images of individual seeds has 2.1 Image Acquisition
been designed and developed. Unlike other systems,
our device is based on dedicated hardware. This ded- Acquisition is a very important part of the perfor-
icated hardware has some limitations. These limi- mance system. It determines the quality of the image
tations are directed by the embedded constraints: a and the image processing time. A good image quality
limited code size implementation, a low memory size is without noise and without defect of illumination.
available, a limited binary mathematical operation, a These conditions are determinant for the image pro-
low frequency compared to a PC processor, limited cessing speed and the level of the results.
temperature dissipation and a limited integrated cir- The quality depends on the choice of three princi-
cuit size. Design and development of the vision sys- pal types of sensor, on the choice of the lighting sys-
tem include these different constraints (Plainchault P., tem and the selected background.
2003). This is why it is not possible to implement
very complex algorithms for discrimination into an 2.1.1 Sensor
embedded board and some choices have been made
in the design and the development of system. The general principle of the sensors lies in the pro-
A linear discriminant analysis (LDA) has been jection of a 3D scene on a 2D matrix where each cell
chosen as a decision algorithm. LDA is applied on the carries out the summation of the photons reflected by
features extracted from the individual image of seeds. the object into the scene.
In the present work, the features include morpho-
logical, colour and texture characteristics. Accord- • Mono CCD:
ing to the application, the discriminating power of A mono-CCD sensor is generally organized ac-
variables may change. Another way to reduce the cording to the Bayer mosaic. This matrix rep-
algorithms complexity is to select a few predictive resents an alternation of separating cells of red,
variables adapted to a given application. Moreover green and blue colours organized as shown in Fig-
in practical applications, a large part of the sample ure 2. This matrix indeed leaves zones where the
can be accurately identified and it may remain only a two others colours are not represented. In order
small proportion in which the discrimination is more to fill this absence, some methods of interpola-
difficult. A relevant approach consists in using an au- tion have been presented in this article (Gunturk
tomated system for a first screening and let the more Bahadir K., 2005) . Some methods can privilege
difficult part of the sample to be manually analysed. certain features like anti-aliasing, shape enhance-
Instead of computers, we propose to use an FPGA ment or noise reducing.
(Field Programmable Gate Array). These hardware
components allow the implementation of a dedicated
architecture for vision chain. FPGA gives the pos-
sibility to apply massively parallel processes. We
present below the chain of vision and the mechanism
of image acquisition already in place. We finish by Figure 2: Mono CCD Sensor : Bayer Mosaic.
the presentation of the method and the results.
• Three CCD:
Three ccd sensors do not need an interpolation
2 ARTIFICIAL VISION phases. Colour separation is made by a prism
which projects it onto three two-dimensional ma-
The chain of artificial vision (Figure 1) includes sev- trices, one red, one green, one blue as presented
eral phases: acquisition, image processing, feature in Figure 3.
extraction and decision. Each element of this chain • Mono-chip Multichannel CCD:
must be optimised in order to reduce computing time This sensor has been developed by the Foveon
312
DEVELOPMENT OF AN AUTOMATED DEVICE FOR SORTING SEEDS - Application on Sunflower Seeds
and parallel processing system objectives. These in-

terpolations are reversible, avoid memory overloading
and reduce image transfer time.
There are two possible methods : the bilinear in-
terpolation represented in figure 6 and equation 2 and
5 (equation for red and blue channels are the same)
Figure 3: Three CCD Sensor. and the constant-difference-based interpolation repre-
sented in Figure 7.
company. It performs colour separation on the sil-
icon. This sensor comes back to the photosensi- G12 + G32 + G21 + G23
G22 = (2)
tive film principle as represented in Figure 4. 4
R11 + R13 + R31 + R33
R22 = (3)
4
R11 + R13
R12 = (4)
2
Figure 4: X3 Sensor. R11 + R23
R21 = (5)
2
2.1.2 Illumination Scene
Illumination is an important part to reduce image pro-

cessing time. The more homogeneous illumination,
we have. The quicker image processing, we have.
In our case, a good separation between background
and object is needed to maintain the real-time pro-
cess. The illumination system can be composed of
incandescent light, discharge lamps or light-emitting
diodes. The choice of these systems is determined by
the application and their characteristics.
2.1.3 Interpolation
Figure 6: Bilinear Interpolation
The interpolation is a demosaicing phases for mono- .
CCD sensors. There are two methods to obtain im-
ages in three channels. The first method, is to reduce
image size by calculating the average of green chan-
nel (1) and leave the red and blue channel as repre-
sented in the figure 5. The second is to use interpola-
tion methods that can maintain sensor size.
Figure 7: Constant-difference-base Interpolation.

Figure 5: Representation of the image into the three chan-
nel.
2.1.4 Color Space
G1+G2 In general, the embedded systems operate using the

G= 2 } (1)
sensor’s RGB color space. Each channel in this space
The requirements of embedded systems are elemen- are strongly correlated. The choice of the colour
tary binary mathematical operators. For that reason, space is dependant on the colour background. Gen-
only bilinear and associated interpolation methods erally a blue background is used for seed inspection.
have been retained. These methods are totally inde- This background needs a new color space to perform
pendent and deterministic which respect the real time seed extraction. The RGB space does not perform
313
very well this process, so the conversion in the YCbCr 8 and the equation. It is the type of decision algo-
(equation 6) space can be possible and is the best to rithm easily parallelisable, where each class are inde-
perform this process. The choice of this space has pendent of its neighbours. These methods and algo-
been made for the following reasons: rithm has been associated to the Mahalanobis distance
1. It is a matrix based conversion (equation 7) in order to increase the distance between
classes. The x vector is the unknown seed to be clas-
2. It is a reversible transformation (linear transfor- sified. The µ vector is the mean value of a labelled
mation) group features. The Σ value is the covariance matrix
3. The distance between seed and background is for multivariate vector.
maximized
d 2 (x) = (x − µ)T Σ−1 (x − µ) (7)
     
Y 0.299 0.587 0.114 R
Cb = −0.169 −0.331 0.5  ∗ G (6)
Cr 0.5 −0.419 −0.081 B
2.2 Feature Extraction
The extraction aims at defining object based on its

morphology, its colour and its texture. For this ex-
traction we need to extract the object and define his
position, we use for that a segmentation task and a
labelling task.
The segmentation algorithms used to perform
the seed positionning task is a variance-based algo-
rithm (Otsu, 1979).
The extracted features defining seeds are detailled
by many publications. These features are summa-
rized in (Majumdar S., 2000a) (Majumdar S., 2000b)
(Majumdar S., 2000c) and in the thesis of Younes
Chtioui (Chtioui, 1997). These features are around
110. There are three groups: morphological, colour
and textural. In the morphological group, there is
dimension, invariant moment and fast fourrier trans-
form type. In the colour group, there is standard de-
viation, mean, variance. In the textural group, there is
descriptor of texture, co-occurence, entropy, kurtosis Figure 8: Principle of Linear Discriminant Analysis.
etc...
2.4 Hardware
2.3 Pattern Recognition: Linear
Discriminant Analysis The architectures presented in this section allow the
creation of dedicated systems based on work from the
Many identification methods (Chtioui, 1997) computer world. They combine good computing effi-
(Visen N.S., 2002) (Majumdar S., 2000d) in the ciency with low power consumption, low memory and
domain of seeds are presented. Some of them cannot device monitoring constraints. These various points
be retained because they do not answer the embedded make them the ideal components for the embedded
constraints. This is why we privilege the simple world.
methods based on the distances between groups and
the methods of the regression type. 2.4.1 MCU
Some previous work show the feasibility of pat-
tern recognition in FPGA based systems (Miteran J., Microcontrollers are circuits largely used for the man-
2005). An LDA method has been chosen. This agement of the automated control processes. Al-
method is based on the principle of the minimal dis- though not intended for mathematical computation,
tance to a group of a given seed as shown in figure the new evolutions of these components integrate
314
more and more mathematical calculating units (ex- • Sensor :

ample EP9315A from Cirrus Logic with mathemati- The sensor provides the frame grabbing function.
cal coprocessor), and even cores DSP (example archi- It is a Kodak KAC-1310 sensor with a resolution
tecture OMAP Texas Instrument(TI)). However their of 1280*1024 and a clock frequency of 10MHz.
performance is still limited for signal processing. It has the advantage of having a window activa-
tion authorizing the activation of only a part of
2.4.2 DSP the sensor. This function can increase speed of
acquisition because this speed is proportional to
DSP are processors dedicated to signal processing. the activated dimension.
They combine the flexibility of a programmable pro- • Flash Light :
cessor with performance in real time signal process- The flash is ensured by 8 diodes luxeon lumileds
ing. The new evolutions of these architectures saw the with a unit power of one Watt. This choice has
capacities of calculation increased to the detriment of been done for the response time and robustness of
consumption. However now they represent now a true this component for the rate of one seed per sec-
general purpose processor for the embedded domain. ond.
2.4.3 FPGA • Barrier :

The detection barrier is based on infra red cells
functioning in saturated mode. It determines the
FPGAs see their uses and their applicability grow.
cut of the beam. For the moment, this barrier is
They combine the performances of a dedicated archi-
able to treat only sunflower seeds, sunflower ker-
tecture and the flexibility of programming. FPGAs
nels, or sclerotia.
are composed of independent logical cells which have
to be activated to create dedicated processes. Nowa- • Background :
days some signal and image processing are imple- A blue background was selected because it allows
mented into these components and represent new ap- a better segmentation of the object by using the
plication for high efficiency and low cost systems. YCbCr colour space and extent the seeds varieties
• Embedded Card :
The electronic system lies on an embedded card.
3 MATERIALS AND METHOD It is a sundance SMT 355 card composed of a TI
DSP TMS320C32 and an ALTERA FPGA Flex
10K130E and 512 Kbytes of RAM. Its task is to
The automaton is based on a principle of free fall
control the peripherals and perform simple pro-
seed (Plainchault P., 2003). It acquires an image of
cessing. Due to the architecture of the card, it
each seed. It adds to it a stroboscopic effect breaking
is not simple to implement image processing into
up the movement of fall. In the long term, the au-
the FPGA. The FPGA does not have direct access
tomaton must reach a sorting at the rate of one seed
to memory and the memory space is too low to
per second. The sorting in the broad sense covers ac-
implement image processing. But this card has
quisition, the image processing, the extraction of the
tested and validates the feasibility of acquisition
parameters, the identification and, finally, the sorting
in free fall of object into embedded card.
at the same time.
3.1.2 Computer Validation of Embedded
3.1 Device Algorithm
The automaton is composed of a sensor, a flash, a • Computer

detection barrier, a background, an electronic system. The electronic system also includes a computer.
Figure 9 shows this association. It is a DELL computer with an Intel Pentium IV
3GHz with HyperThreading processor, 1GB of
DDR-SDRAM, a graphics board AMD/ATI X300
3.1.1 Seed Sorting System with dedicated memory and a 160GB hard disk.
The computer provides the Human-Machine in-
• Mechanical feeding: terface to control the automaton. It saves the im-
The mechanical feeding is based on an electro- age acquired in Bitmap format on the hard disk
magnet which attracts a support heightened by drive. The computer also plays a part in the al-
blades of metal. gorithm validation. The identification algorithm
315
Figure 9: Presentation of the wanted system.
has been tested off line in order to determine its Table 1: Principle of the analysis in the framework of purity
efficiency for the sorting. and counting seed.
• Software Object Purity counting
The automaton is controlled by a program imple- sunflower kernels PS
mented in the computer using Visual C++. It pro- sunflower achenes PS
vides a window interface to control the acquisi- broken seeds PS
tion process. The algorithm validation is carried mutilated seeds IM
out off line with the image acquired by the au- soil IM
tomaton under the environment of matrix algebra sclerotia IM counting
MATLAB 7. Firstly, we calculate the parameters Seeds of other plants SOP counting
for all the datasets. In order to have all the fea-
tures algorithms we just use the bwmorph func-
tion from the matlab image processing toolbox.
Secondly, we train and test the identification algo-
rithm on the dataset to select the best algorithm.
3.2 Methods
(a) Sun- (b) Sun-
Seeds and impurities observed by the vision system flower flower
achenes Kernels
were extracted from 23 samples of sunflower seeds
representing commercial seed lots of various origins
and 18 varieties, differing by seed colour and size.
The dataset for impurities with a low frequency in the
samples was enlarged using the reference collection
of the GEVES-SNES. A dataset including 1051 im-
ages has been created. This dataset includes 6 classes: (c) Soil (d) Sclerotia
sunflowers kernels, broken seeds (fragments of seeds
with a size higher than 50 percent of the size of the
seed), mutilated seeds (fragments of seeds with a size
lower or equal to 50 percent), sunflower achenes (in-
tact seeds), sclerotia and soil.
At this stage of the study, our dataset includes
all the categories (seeds and impurities) that have to
(e) Broken seeds (f) Mu-
be identified in official analysis of commercial seed tilated
lots (table 1) with the exception of seeds belonging seed
to other species than the sunflower. Figure 10 shows
colour, texture and morphology differences among Figure 10: Image of pure seeds (a,b,e) and impurities (c,d,f).
the different classes.
This dataset has been labelled by an expert of the
316
GEVES-SNES. With this dataset we have conducted

a linear discriminant analysis using cross-validation.
We have randomly selected 348 images for the train-
ing sets and 703 for the testing sets in accordance with
table 2. We obtain a reduction of the feature space
with a stepwise canonical discriminant analysis ac-
cording to the article (Bertrand D., 1990). The selec-
tion of parameters is made incrementally by testing Figure 12: Differentiation sclerotia, achenes, broken seeds,
the increase of identification percentage. mutilated seeds, soil.
Table 2: Dataset representation of each groups.

results expressed in table 12 is in accord with the map
Object training set testing set total of labelled groups (figure 11). The table 12 shows all
sunflower kernels 56 113 169 of the misclassifications identified on the map. Many
broken seeds 72 145 217 of the errors of classification are due to a lack of in-
mutilated seeds 36 72 108
formation in the image. The free fall principle with
sclerotia 61 123 184
sunflower achenes 66 134 200 only one camera has the same problem as on a rolling
soil 57 116 173 carpet (Egelberg P., 1994): it is possible that the cam-
total 348 703 1051 era does not take the image of the face bringing infor-
mation, which is the case after analysis of the badly
identified seeds. As we can note in table 12, there is
a strong confusion between broken seeds and muti-
4 RESULTS lated seeds. The algorithm can not perform a virtual
representation of the associated pure seeds as the hu-
man expert can. From a practical point of view, we
could accept a misclassification when it affects pure
seeds (kernels, achenes and broken seeds) identified
as impurities (e.g. sclerotia classified as soil). In this
case, to achieve the analysis, the analyst will observe
only a fraction of the sample classified as impurities
by the system. Following this principle, the system
classification was less satisfactory for mutilated seeds
and soil: 13% of mutilated seeds were classified as
pure seeds (broken seeds or achene), 4% of soil as
achene. This first result shows the interest of the sys-
tem when a determination of other seeds is carried out
in sunflower: the algorithm makes possible 100% of
correct separation of pure seed (achenes or kernels of
sunflower) from sclerotia that are searched for in this
kind of analysis.
Figure 11: representation map of the sample.
The parameters selected by the LDA method show a
great extent of texture (energy, entropy, occurrence), 5 CONCLUSION
colour (mean, variance) and a few morphological
(perimeter, area, moments, Fast Fourier Transform) The widening of the dataset of image seeds to new
parameters. Figure 11 shows the representation map species is under acquisition.
of the sample and table 12 indicates the identification The remaining work will consist of performing ac-
results for all the classes. A high percentage of each quisition, features extractions and decision in a com-
class has been correctly identified: from 75% of bro- plete automaton. Optimisation and testing on other
ken seeds to 97% of sclerotia. Some misclassification varieties of the decision algorithm have to be con-
occurred, with the extent and importance that vary in ducted.
relation to the classes. If we seek to identify indepen- Moreover the creation of a multi-camera system
dently each group, the selected features change but of acquisition according to the same principle would
follow what we have said before in the part 3.2.1. The allow on certain ambiguities at the time of the phase
317
of decision. It is also possible to make this approach Jayas D.S, M. C. and Bulley, N. (1999). An automated seed
more robust by creating a unknown class for seeds at presentation device for use in machine vision identi-
equal distance from the various groups. fication of grain. Canadian agricultural engineering,
41(2):113–118.
The testing on the identification algorithm on
seeds of other plants has to be made. System enhance- Majumdar S., J. D. S. (2000a). Classification of cereal
grains using machine vision i. morphological mod-
ments have to be made like changing the infrared bar- els. American Society of Agricultural Engineers,
rier in order to extend seed varieties acquisition. The 43:1669–1675.
design and the development of a new hardware sys- Majumdar S., J. D. S. (2000b). Classification of cereal
tem have to be made in order to implement a three grains using machine vision ii. color models. Ameri-
camera systems. can Society of Agricultural Engineers, 43:1677–1680.
Majumdar S., J. D. S. (2000c). Classification of ce-
real grains using machine vision iii. texture mod-
els. American Society of Agricultural Engineers,
ACKNOWLEDGEMENTS 43:1681–1687.
Majumdar S., J. D. S. (2000d). Classification of cereal
This work was supported by the National Institut of grains using machine vision iv.combined morphology,
Agronomical Research (INRA), the variety and seed color, and texture models. American Society of Agri-
study and control group (GEVES), the ESEO gradu- cultural Engineers, 43:1689–1694.
ate school of engineering in electronic, and the LISA Miteran J., M. J. (2005). Automatic hardware implementa-
laboratory of the University of Angers. tion tool for a discrete adaboost-based decision algo-
For their financial support the region Pays de Loire rithm : Prototyping for machine perception on a chip.
and the CER Vegetal (French contract). EURASIP, 2005:1035–1046.
Moltó Enrique, Blasco José, B. J. V. (1998). Computer vi-
sion for automatic inspection of agricultural produces.
In SPIE, editor, Precision agriculture and biological
REFERENCES quality, volume 3543, pages 91–100.
Otsu, N. (1979). A threshold selection method from gray-
Batlle J., M. J. (2002). A new fpga/dsp-based parallel. Real level histograms. IEEE Transactions on Systems, Man
time imaging, 8:345–356. and Cybernetics, 9:62–66.
Pearson, T. (1994). Machine vision system for automated
Bennett, T. D. K. F. M. M. M. (2005). A computer- detection of stained pistachio nuts. In SPIE, editor,
based system for seed identification. Seed Technology, Optics in Agriculture, Forestry, and Biological Pro-
27:190–202. cessing, volume 2345.
Bertrand D., Coucroux P. Autran J.-C., M. R. R. P. (1990). Plainchault P., Demilly D., F. A. T. S. B. D. (2003). De-
Stepwise canonical discriminant analysis of continu- velopment of an imaging system for the high speed
ous digitalized signals : applicatoin to chromatograms identification of seeds. IEEE Sensor.
of wheat proteins. Journal of chemometrics, 4:413–
427. Visen N.S., J. D. (2002). Specialist neural network for
cereal grain classification. Biosystems Engineering,
Chtioui, Y. (1997). Reconnaissance automatique des se- 82:151–159.
mences par vision artificielle basée sur des approches Wan, Y.-N. (2002). Automatic grain quality inspection with
statistiques et connexionnistes. PhD thesis, IRESTE. learning mechanism. Information technology in Agri-
Davies, E. (2005). Machine Vision - Theory, Algorithms, culture.
Practicalities.
Egelberg P., Mansson O., P. C. (1994). Assessing cereal
grain quality with a fully automated instrument using
artificial network processing of digitized color video
images. In Forestry and Biological Processing, vol-
ume 2345, pages 146–156. SPIE congress.
Granitto Pablo M, P. F. V. and Ceccatto, H. A. (2003). Au-
tomatic identification of weed seeds. ASAI.
Granitto P.M., P.A. Garralda, P. V. and Ceccatto, H. (2003).
Boosting classifiers for weed seeds identification.
JCS&T.
Gunturk Bahadir K., e. a. (2005). Demosaicking : Color fil-
ter array interpolation. IEEE Signal Processing Mag-
azine, pages 44–54.
318
MATHEMATICAL MODEL FOR WALKING ROBOT
WITH SHAPE MEMORY ALLOY ANKLE
Anca Petrişor, Nicu George Bîzdoacă

Faculty of Electromechanical Engineering, University of Craiova, Romania
Daniela Roşca, Sonia Degeratu, Adrian Roşca, Raducu Petrisor

Faculty of Electromechanical Engineering, University of Craiova, Romania
Keywords: Robotics, shape memory alloy applications, robotic ankle, walking robot, mathematical model.
Abstract: The paper presents a simultaneous force and length variation mode in shape memory alloy (SMA) robotic
application. The robotic ankle contains four SMA actuators and a spherical articulation. In order to assure a
high efficient robotic architecture, the mechanical and the control structure have to assure a real-time
response to the work environment changes. The load variations or the difference between the moment of
full contact step and the non contact moment for a waking robot are the standard situations for a SMA
robotic ankle. The paper is divided in five sections. First section makes a short introduction in the physical
description and conventional applications of shape memory alloy materials. Then, are presented the
mathematical model for robotic ankle, the walking robot geometrical structure and the causality ordering of
the active pair of legs, in this case with one free joint. In the last section some experimental results are
presented. These results were obtained by using MATLAB programs, conceived by authors, for design and
simulation of walking robots control algorithms.
1 INTRODUCTION and the ability to be electrically heated for shape

recovery.
Shape memory actuators are considered to be
The shape memory effect was first noted over 50
years ago; it was not until 1962, however, with the low power actuators and such as compete with
discovery of a nickel titanium shape memory alloy solenoids, bimetals and to some degree was motors.
It is estimated that shape memory springs can
by Buehler, that serious investigations were
undertaken to understand the mechanism of the provide over 100 times the work output of thermal
shape memory effect (Bîzdoacă and Pană, 2004), bimetals.
The use of shape memory alloy can sometimes
(Funakubo, 1987), (Graesser and Cozarelli, 1994),
(Schroeder and Boller, 1993). simplify a mechanism or device, reducing the
The shape memory alloys possess the ability to overall number of parts, increasing reliability and
therefore reducing associated quality costs.
undergo shape change at low temperature and retain
this deformation until they are heated, at which point Because of its high resistivity of 80 – 89 micro
they return to their original shape. ohm-cm, nickel titanium can be self heated by
passing an electrical current through it. The basic
The nickel titanium alloys, used in the present
research, generally refereed to as Nitinol, have rule for electrical actuation is that the temperature of
compositions of approximately 50 atomic % Ni/ 50 complete transformation to martensite Mf , of the
actuator, must be well above the maximum ambient
atomic % Ti, with small additions of copper, iron,
cobalt or chromium. The alloys are four times the temperature expected (Delay and Chandrasekaran,
cost of Cu-Zn-Al alloys, but it possesses several 1987).
advantages as greater ductility, more recoverable
motion, excellent corrosion resistance, stable
transformation temperatures, high biocompatability
319
2 MATHEMATICAL MODEL OF H ankle ( α1, α 2 , lc ) =

SHAPE MEMORY ALLOY cos α 2 0 sin α 2 lc sin α 2
ANKLE sin α1 sin α 2 cos α1 − sin α1 cos α 2 −lc sin α1 cos α 2
− cos α1 sin α 2 sin α1 cos α1 cos α 2 lc cos α1 cos α 2
The robotic researches develop up to the present a 0 0 0 1 (1)
various mechanical architecture for ankle structure.
All projects use the human ankle as model. Analysing the angle dependence versus the SMA
The problem in developing efficient ankle spring variation, a highly nonlinear function results:
structure, concern the dimension and the efficiency
of actuators. (2)
The proposed robot ankle structure contains units ⎡ 2l2 + lc2 ⎤
with SMA actuators. The unit has 4 SMA actuators ⎢
(
α = arcsin ⎢ cos arctg ( lh ) b
2lb ⎥
)
⎥ + arctg ( l )
h
and a spherical articulation. ⎣ ⎦
where lh is the length after the heating process; lc is

the spring length after cooling and lb the base length.
As the real variation is restricted (between 100%
and 92 %), the linearization can occur because of
linear behaviour for the specified evolution
(Bîzdoacă and Pană, 2003).
3 WALKING ROBOT
Figure 1: The proposed SMA robotic ankle. GEOMETRICAL STRUCTURE
It is considered the walking robot structure as
depicted in Fig.4, having three normal legs
Li , Lj , Lp and a head equivalent to another leg, L0,
containing the robot centre of gravity, G, placed in
its foot. The robot body RB is characterized by two
position vectors O0, O1 and the leg joining points
denoted Ri, Rj, Rp. The joining point of the head, L0,
is the central point O0, R0= O0, so the robot body RB
is univocally characterized by the set,
Figure 2: Schematically representation of SMA ankle.
RB = {O0 , O1 , λi , λ j , λ p , λ 0 } (3)
where λ0 = 0 .
u1,0
G0=G
Rp
2,0 j
R u2,p
u
R0 u2,j
Ri 0 1 u1,p
u2,i O O
Gp
u1,i
Figure 3: Kinematics representation of SMA ankle. Gj u1,j
Gi
The mathematical model of the ankle is very
simple: Figure 4: Geometrical structure of the robot.
320
MATHEMATICAL MODEL FOR WALKING ROBOT WITH SHAPE MEMORY ALLOY ANKLE
The robot position in the vertical plane is defined Therefore, in this causality structure the
by the pair of the position vectors O0, O1 where j i
kinematics restriction R − R = rij is accomplished
| O1 − O0 |= 1 , or by the vector O0 and the scalar θ,
the angular direction of the robot body. by changing the value of u 2, j at u 2, j giving by the
Each of the four robot legs Li , Lj , Lp , L0 is equation (5), (6), (7).
characterized by a so-called Existence Relation
ER(L) depending on specific variables as it is
presented in (Petrişor, 2005), (Petrişor, Marin,
Bîzdoacă and Cazacu, 2006). 5 EXPERIMENTAL RESULTS
The mathematical model of this object is a
Variable Causality Dynamic Systems VCDS and it The causal ordering c=[motor12, motor 01, cp] is
is analiyzed from this point of view. implemented together with other causal orderings, in
A pair of legs {Li, Lj} constitutes the so called the RoPa platform for design and simulation of
Active Pair of Legs (APL) if the robot body position walking robots control algorithms. The RoPa
is the same irrespective of the feet position of all the platform is a complex of Matlab programs to
other legs different of Li and L j . A label is assigned analyze and design walking robots, evolving in
to each possible APL. The APL label is expressed uncertain environments according to a new control
by a variable q called Index of Activity (IA), which approach called Stable State Transition Approach
can take Na values, numbers or strings of characters. (SSTA), also conceived by the authors.
All the other legs that at a time instant do not belong The causal ordering developed in this paper is
to APL are called Passive Legs (PL). The leg in activated by selecting the causal variable
APL, having a free joining point is called slave leg, cz=[12 1 0]. The RoPa platform allows animation,
the opposite of the motor leg whose both joining recording of the evolutions and playback them.
points are external controlled. In the following there are presented some
experimental results of walking robot behaviour
considering this causal ordering.
4 CAUSALITY ORDERING OF AN RoP a1E x 2S 1V 1q132c z 1210
ACTIVE PAIR OF LEGS WITH 5
ONE FREE JOINT 4
In this structure, only one angle is free so three joints 2

y [m]
are external controlled (EC).It is denoted this by 1
q = 'ij' , c = [motor12, motor01, cp ] (4) 0
-1
-2
This causality ordering is corresponded to the -3
state having the leg Li as a motor leg which controls
two degree of freedom and the leg Lj, a slave leg,
-2 0 2 4 6
x [m ]
which can control only one scalar component , so
the angle u 2, j is free and the angle u1, j is EC. Figure 5: The robot kinematics evolution.
In this structure u 2, j is obtained from the
following relations: 3
u 1 ,3
⎧ψ if u2,j (t −ε) > 0 ⇔ sˆ j = up (5) ← u 2,3
2,j ⎪ 2j
2
2 ,3
←u
u (t) =⎨ u 1,3
u1,1 u2,1 u1,3 u2,3
2,j j
⎪⎩−ψ2j if u (t −ε) ≤ 0 ⇔ sˆ = down 1
0
G1 = 0
⎧⎪−ϕ2 j + ψ2j dacă −ϕ2 j + ψ2 j ≤ 0 (6) -1

G3 = 3
2, j
u (t) =⎨ u 2 ,1
⎪⎩−ϕ2 j + ψ2j − π dacă −ϕ2 j + ψ 2j > 0 -2
1 ,1 u 2 ,1
←u
-3
⎧⎪−ϕ2 j + ψ2 j + π dacă −ϕ2 j + ψ2 j ≤ 0 (7) -4 -2 0 2 4
2, j u1 ,1
u (t) =⎨
⎪⎩−ϕ2 j + ψ2 j dacă −ϕ2 j + ψ2 j > 0
Figure 6: Controlled angles with respect to the input angle.
321
4
6 CONCLUSIONS
3
← R e (O 0 )
← R e (O 0 ) Shape memory alloy ankle add to the walking robot
Im(O ) Re(O ) θ
architecture additional control possibility for

0
2
G1 = 0
G3 = 3
adapting the structure to various environment.
1 The causal ordering developed in the paper is
0
← Im (O 0 )
←θ ← Im (O 0 ) useful when it must control the shoulder position Ri
0
irrespective of the other variables. It can be
←θ controlled by the angle u1,j, the shoulder angle of the
-1
-4 -2 0 2 4 second active leg Lj.
u 1 ,1 The approach of the kinematics structure by
Figure 7: Robot body position with respect to the input complex number representation of the variables
angle. allowed to solve the equations system that describe
6
the position of the variables that are involved,
between witch there are kinematics correlations. In
← R e (R 3 ) ← R e (R 3 )
this way, it is obtained an explicit representation of
Im(R ) Re(R ) Im(R ) Re(R )
3
4
the input-output dependence, in this causality
Im (R )
structure.
3
2 3
← R e (R 1 )
← R e (R 1 ) Im (R 1 ) As it can be seen from the above experimental
results, this causal ordering is perfectly integrated in
1
0
← Im (R 1 )
← Im (R 3 ) the RoPa structure proving the correctness of the
1
-2
G = 0 theoretical results.
1
G3 = 3 The mathematical model developed in the
-4
-4 -2 0 2 4 paper becomes an element of the VCDS walking
u 1,1 robot model. The simulations used MATLAB
Figure 8: Joints positions with respect to the input angle. environment show the robustness of the
mathematical model.
1
← Im ag(O 0 )
0.5 REFERENCES
Imag(O0)
0 Bîzdoacă, N., Pană D., 2003. Shape memory alloy tendon

G1 = 0
driven finger. In Proceedings of the 14th international
G3 = 3
conference on control systems and computer science,
-0.5
CSCS15, Bucharest, Romania, May 25-27.
Bîzdoacă, N., Pană D., 2004. Shape Memory Alloy Based
-1
2 2.5 3 3.5
Robotic Ankle. In Proceedings of the 5th International
R eal(O 0 ) Carpathian Control Conference, ICCC 2004, Vol. I.
Delay, L., Chandrasekaran M., 1987. Les Editions
Figure 9: Reference point O0 locus in evolution scene. Physique. Les Ulis.
Funakubo, H., 1987. Shape Memory Alloys. Gordon and
Breach Science Publishers.
2
← Im (R 3 )
Graesser, E.J., Cozarelli F.A., 1994. Journal of Intelligent
← Im (R 1 ) Material Systems and Structures, no.5.
Im(R ) Im(R ) Im(R )
2
1
← Im (R 2 ) Petrişor, A., 2005. Walking mouvement with controlled
style of the passive leg. In Proceedings of the 15th
3
0
international conference on control systems and
computer science, CSCS15, Bucharest, Romania, May
1
-1
G1 = 0 25-27, 2005, pag.483-487.
-2
G3 = 3 Petrişor, A., Marin, C., Bîzdoacă, N., Cazacu, D. 2006.
Variable Causality Dynamic System Approach for
1 2 3 4 5 Robot Control. WSEAS Transactions on Systems and
R e(R 1 ) R e(R 3 ) R e(R 2 )
Control Journal, Issue 1, Volume 1, Nov., pp.26-30
Schroeder, B., Boller, Ch., 1998. Comparative Assessment
Figure 10: Joints points' locus in evolution scene. of Models for Describing the Constitutive Behaviour
of Shape Memory Alloy, Smart Materials and
Structures.
322
WEB-BASED INTERACTIVE POSITIONING CONTROL OF AN
ELECTRIC FLATCAR VIA WIRELESS NETWORK
Ken Ishii, Koki Abe and Yoshimi Takao

Natl. Res. Inst. of Fisheries Engineering, Fisheries Research Agency,
7620-7, Hasaki, Kamisu city, Ibaraki 314-0408, Japan
[email protected]
Keywords: Electric flatcar, manipulator probe, model of WEB direct-access monitoring, double exclusive control, con-
troller and multiple-monitor system.
Abstract: A large tank has been used for target strength pattern measurements of fish. It is equipped with an electric
flatcar. Further an elevation-rotating unit runs on the rails above it. The probe on the top of its elevation unit
is equipped with an attachment for an ultrasonic transducer. The manipulator probe is movable in the four
directions of the x, y, z and θ axes. Installation of a remote control switch has been required for the purpose of
efficient operation of an electric flatcar. A remote control system using a notebook personal computer has been
developed with good cost performance. The PC is equipped with a wireless network interface card. A model
of WEB direct-access monitoring has been designed newly on the basis of the concept that an operator can
control a mechanical device using a WEB Browser via LAN. Furthermore it performs double exclusive control
for access from multi PCs, and has made possible a controller and multiple-monitor system. The mission was
performed for the purpose of evaluation of WEB operation. The result has made clear the specifications for
motion, and an external interface of the electric flatcar is applicable to the new protocol developed for WEB
Browser control.
1 INTRODUCTION nication method of this system is described in detail

in this paper.
An indoor tank filled with fresh water has been used
for target strength pattern measurements of fish (Ishii
and Takao, 2000), and its shape is 10(W) x 15(L) 2 METHOD
x 10(H) m3 . It is equipped with an electric flatcar.
Further an elevation-rotating unit runs on the rails The probe can move four-dimensionally. The nominal
above it. The probe on the top of its elevation unit is precision is ±1 cm along the longitudinal and trans-
equipped with an attachment for an ultrasonic transverse axes, ±2 cm along the elevation axis, and ±0.2
ducer as shown in Figure 1 (Ishii et al., 1995). Instal- degree around the rotation axis. Resolutions of abso-
lation of a remote control switch has been required for lute encoders for positioning are 1 mm and 0.01 de-
the purpose of efficient operation of an electric flatcar. gree, respectively. Parameters of velocity are limited
A remote control system by a WEB Browser using between 1 and 20 cm/s along the longitudinal axis, 1
a personal computer (PC) has been developed with and 10 cm/s along the transverse axis, and 0.2 and 3.0
good cost performance. Generally, WEB Browser degree/s around the rotation axis.
displays a page from a web sever responding to a re- The sequencer (PLC, Programmable Logic Con-
quest of a client. A long reply time is not applicable to troller) directly controls the electric flatcar every
WEB Browser. But, the reply time of a target mechan- 10ms. Interval of communication between the PC
ical device accessed by a web server varies widely. and the sequencer is one second using DDE (Dy-
The issue is development of a protocol for receiving namic Data Exchange) protocol via P-LINK (Proces-
reply messages from a target machine. The commu- sor Link) (Ishii et al., 1999).
323
(c)
(a)
Longitude
(b)
Transverse (d)
Rotation Elevation
Figure 1: The electric flatcar and its movable directions shown by arrows. (a) The electric flatcar on a large tank. (b) An
elevation-rotating unit on it includes a movable probe. Two devices of (c) and (d) are used in the joint mission. (c) An
attachment on the top of a probe in the electric flatcar. (d) An adaptor for a transducer.
The function of a remote control switch has been WEB Monitoring

PC_Moni
realized by a notebook PC equipped with a wireless *2 LAN
network interface card that is implemented more eas-

Router
ily than a specified low power radio wave unit. The *1
Hub
model of WEB direct-access monitoring has been de- Water tank

Intranet
signed newly on the basis of the concept that mul- *3
Wireless NIC
tiple users control or monitor a mechanical device 123.8
567.4
PC_Ctrl
Notebook PC
PC_ Device PC_Log
Device driver
using a WEB Browser via LAN (Local Area Net- 97.84
473.1
− +
Electric flatcar
program Main server
program
DDE, P-LINK
work). It consists of a main server program that per- − + Logging program
forms exclusive control for access from multi PCs, Remote control
PLC
switch function Control room

and makes possible a controller and multiple-monitor
system (Ishii and Sawada, 2005). Figure 2 shows Figure 2: The model of WEB direct access monitoring ap-
its application to the remote control switch function plied to a remote control switch function. Widely located
WEB Browsers post request commands to the main server
for an electric flatcar. The driving control command program respectively via the routes *1 or *2 on a network.
through the main server program does not access the The main server program directly accesses, via route *3,
database that holds the values, but accesses the device a device driver program of an electric flatcar in order of a
driver program that operates the instrument instead. session.
The mission was performed for the purpose of
evaluation of WEB operation. The mission is
achieved by bringing an attachment of a probe close • A negotiation command for priority was sub-
to an adaptor for a transducer shown in Figure 1. In mitted from the controller PC at a time of
this mission, the speed is one or two cm/s in the trans- 15:38:38.040, and a transverse driving command
verse direction, one cm/s in the elevation direction, was submitted at 15:38:38.056. The left command
and 0.2 degree/s in the rotation direction (Ishii et al., frame was shifted to Figure 3-(b) from Figure 3-
2006). (a).
• During movement, polling is repeated by a moni-
toring frame.
3 RESULTS • After a submission of a STOP command at
15:38:51.415, an end flag of driving was detected
at a time of 15:38:52.087.
A WEB Browser display of a remote control switch
function has dual modes of ”DRIVE” and ”JOG”. • By control from a right polling frame, a page
Furthermore, each mode consists of two inde- of left command frame was renewal to a waiting
pendent frames of ”SUBMIT COMMAND” and page of Figure 3-(a) at a time of 15:38:52.368.
”POLLING”. An example of ”DRIVE” mode is Among the mission for evaluation, access of a
shown in Figure 3. The time chart is the following: PLC from a remote control switch and monitor PCs
324
WEB-BASED INTERACTIVE POSITIONING CONTROL OF AN ELECTRIC FLATCAR VIA WIRELESS NETWORK
(a) (b)
Figure 3: Design of the DRIVE mode with START/STOP. From upper icon, control buttons show the four directions ”longi-
tudinal”, ”transverse”, ”rotation”, and ”elevation”. A driven direction is selected by a tab key or a mouse. The left figure (a)
shows a mode of WAITING, and a foreward direction of ”transverse” is focused. The right figure (b) shows a status of driving
operation.
was performed via Intranet or LAN as shown in Fig- in Table 1 in addition to needed times for acquisition
ure 4 for a typical example. In this figure, the X-axis of moved position information.
shows time in seconds. ”PC Ctrl” of the Y-axis works
as a controller with a remote control switch function.
A WEB Browser performed polling in intervals of five 4 DISCUSSIONS
seconds for the monitoring, or two seconds for the
driving. Each access to a main server is classified by
Dual modes of motion operation are available to be
a session ID, and is ordered by double exclusive con-
changed to the other interactively. The DRIVE mode
trol. A transverse driving command ($DRIVE) was
with START/STOP is used for driving of long dis-
submitted from the controller PC (named PC Ctrl)
tance. The JOG mode is useful for short range driv-
at a time of 17:35:38.571, and a stopping command
ing, about one cm. And Driving of a fixed distance
($STOP) was submitted at 17:35:56.096. Positioning
such as JOG mode is needed for safety of operations.
information of the electric flatcar is given per access.
In Figure 4-(b), it is shown that access timing of
On the other hand, a logging PC (named PC Log) re-
multiple PCs (PC Moni and PC Ctrl) was success-
ceives renewal data at an interval of about one second
fully ordered by double exclusive control of a main
linked with changing of values of position. The probe
server program.
of the electric flatcar moved from 384.6 cm to 370 cm
In Table 1, it takes several seconds for acquisition
at the minimum velocity of one cm/s along the trans-
of first positioning data owing to lower initial speed.
verse axis. It has been confirmed that a WEB Browser
After second positioning data, it takes about one sec-
on the controller PC is available to be performed in
ond. It takes about six ms for negotiation of priority.
repetition of one second. Furthermore, the number
Positioning data is obtained at the same time of ne-
of PCs for WEB monitoring has been confirmed nor-
gotiation of priority. And in the monitoring frame of
mally for Four PCs.
a WEB Browser, a negotiation command also works
Three kinds of commands such as a negotiation as a polling command for receiving a reply message
command for priority, driving commands and a stop- from the mechanical device (PLC). A reply to a mon-
ping command, are submitted sequentially from a itoring command is performed immediately. The re-
command frame of a controller PC. On the other turn time of driving commands is about 30ms.
hand, in a monitoring frame, polling is performed for Furthermore, submission of a driving command
the purpose of acquisition of positioning information. hardly coincides with the renewal of a WEB page
Averaged reply times for drive commands are shown for polling, because of the two independent frame
325
Table 1: Reply times for commands and needed times for position information.
Drive axis Priority $DRIVE First Priority $STOP Last

position position
ms ms sec. ms ms sec.
Transverse 6 32 2.5 6 31 1.6
Elevation 6 28 1.5 7 28 1.0
Rotation 7 30 3.2 7 27 1.2
architecture. Therefore retry of the selection is not flatcar, the server program for WEB pages that are
required. Consequently remote control by a WEB available for control and monitoring of a machine has
Browser has been working well. developed in a sever PC. Usage of a WEB browser
Residual issues are the following: makes possible a flexible constitution of the remote
• A notebook PC lacks solidity. terminal system. Then it has a merit of a smaller task
to a terminal PC.
• It takes longer time in a FORCED STOP oper- This system has been developed on the basis of
ation on a wireless PC than in an operation by two contents such as a WEB direct-access model, and
an EMERGENCY STOP button on an operation a controller / multi-monitor system model. It was
desk. clear by the above-described mission that the commu-
nication speed of the electric flatcar is applicable to
(a) the model of WEB direct-access control and monitor-
ing, in the case of attainability of the electric flatcar.
An attachment of a probe is also available for in-
stallation of underwater cameras. The optical calibra-
tion of an underwater observation bench is scheduled
using an electric flatcar. It is hopeful that the wireless
(b) operation on the spot instead of a push button opera-
tion on an operation desk is effective.
REFERENCES
Ishii, K., Abe, K., and Takao, Y. (2006). Positioning control
Waiting $DRIVE $STOP of the electric flatcar using web browser via wireless
command command lan. In Abstracts of 2006FY Autumn Annual Meeting.
Figure 4: Ordered accesses by three PCs to a main server AMSTEC.
PC via LAN. The lower figure (b) shows access timing from Ishii, K. and Sawada, K. (2005). Network-integrated control
multi PCs. PC Mon and PC Ctrl use WEB Brower with an program for a transducer-rotating mechanism to mea-
interval time of two or five seconds. PC Log uses an exclu- sure target strength (ts) pattern of squid. In Journal
sive application program for logging positioning data with of Advanced Marine Science and Technology Society.
an interval time of about one second. The upper figure (a) AMSTEC.
shows positioning information acquisitioned by PC Log. In Ishii, K., Sawada, K., and Miyanohana, Y. (1999). Design
the lowest row, a transition of motion modes about PC Ctrl of transducer-positioning system scanning around an
is shown. aquatic target in a tank. In Collected Papers of FO-
RUM ACUSTICUM BERLIN 99. ASA and DEGA.
Ishii, K., Sawada, K., Takao, Y., Miyanohana, Y., and Oku-
mura, T. (1995). Control system of electric flat-car
5 CONCLUSION equipped above an acoustic tank. In Proceeding of
the Annual Meeting. the Marine Acoustics Society of
The development of a remote operation system via a Japan.
wireless PC didn’t require editing of a complex se- Ishii, K. and Takao, Y. (2000). Evaluation of transducer-
quencer program in the electric flatcar, so that this positioning system scanning around an aquatic target
system starts working well as soon as possible. Using in a tank. In Abstracts of 2000FY Spring Annual Meet-
a function of PRESET DRIVE and a simple commu- ing. Advanced Marine Science and Technology Soci-
nication protocol that are implemented in the electric ety.
326
THEORY OF STRUCTURED INTELLIGENCE
Results on Innovation-based and Experience-based Behaviour
Meike Jipp and Essameddin Badreddin

Automation Laboratory, University of Mannheim, B6, 23-29, Mannheim, Germany
Keywords: Intelligence, intelligent interfaces, innovation, human computer interaction.
Abstract: An agreed-upon general theory of intelligence would enable significant scientific progress in all disciplines
doing research on intelligence. Such a theory, namely the theory of structured intelligence is tested by
relating it to other theories in the field and by empirically testing it. The results demonstrate (1) that the
theory of structured intelligence uses a similar concept of intelligence as do other theories but offers greater
scientific insights in the how intelligent behaviour emerges and (2) that its distinction between innovation-
and experience-based solutions can be found in the behaviour of the study’s participants. This yields the
opportunity to (1) allow technically testing intelligence in an easier and less time-consuming ways as do
traditional intelligence tests, and (2) allow technology classifying the intelligence of its user and using
adaptive interfaces reducing the possibility of serious handling errors.
1 INTRODUCTION solutions to a problem can be developed. Fig. 1

gives an overview over the described structure.
Many different theories of intelligence have been New solution
developed in the varying disciplines (for a summary Innovation
Solution
see Badreddin & Jipp, 2006). So far, these Problem
approaches have been isolated not sufficiently taking Fusion
into account results from other disciplines. An Experience Past solution
opposite example is the theory of structured
intelligence developed by Badreddin and Jipp
Learning
(2006). The authors make use of research results in
neurophysiology, psychology and system theory and Figure 1: Theory of structured intelligence (see Badreddin
present the theory itself and discuss ways of & Jipp, 2006).
technical implementation. The theory defines
intelligence as the ability to solve problems using
limited space and time resources. The concepts of
Innovation, Experience, Fusion, and Learning are
2 PROBLEM STATEMENT
distinguished to explain problem solving behavior.
Innovation reflects the capability to come up with The, by Badreddin and Jipp (2006) developed theory
totally new, unpredictable solutions to the current of structured intelligence needs further testing to see
problem. Experience refers to using past, known, whether it (1) allows the realization of a
and successful solutions for the current problem. If a qualitatively different concept of artificial
problem is faced, two solutions are worked out, one intelligence, (2) allows easier testing of human
based on innovation (“new solution”), the other intelligence, (3) enables technology to test their
based on experience (“past solution”). These two user’s intelligence and adapt interfaces according to
solutions are fused by appropriate algorithms and the the level and structure of intelligence to avoid
final solution will be applied to the current problem. possible errors, which is especially of importance in
This derived solution is saved, so that it is available safety-critical systems.
the next time the same or a similar problem is faced.
Hence, any combination of new and well-known
327
3 SOLUTION APPROACH can be consistently encoded and compared

(Ackerman, 1989). Last, psychomotor abilities
To test the theory at hand, a way similar to what is represent individual differences in the speed of
known as construct validation has been taken motor responses to problems without information
(Campbell & Fiske, 1959): First, the concept of processing demands.
intelligence as described in Section 1 will be set into Ackerman (1988) proposes that general
relation with other existing theories (see Section intelligence determines initial performance on a task
2.1). To empirically test the defined relations and with new information processing demands, i.e. the
related hypotheses, a study has been conducted in a first phase of skill acquisition. The influence of
second step, which results are presented in Section general intelligence diminishes, when potential ways
2.2. for the solution have been formulated (for empirical
support, see e.g., Ackerman, 1988). The learner
3.1 Theoretical Foundations proceeds to the second phase of the skill acquisition
process, when an adequate cognitive representation
In order to perform the construct validation, the of the task has been built. Then, performance
theory of structured intelligence must be put in depends more on psychosensoric abilities. It is
theoretical relation with another theory which has required to fine-tune and compile the determined
proven empirical adequacy. The theory used here is solutions, which equals the definition of the abilities
the theory of skill acquisition, which was developed underlying psychosensoric abilities. Sequences of
by Ackerman (1988) and is based on research cognitive and motor processes get integrated, ways
performed by Fitts (1964), Anderson (1982), of solution adapted for successful task performance.
Fleishman (1972), as well as Schneider and Shiffrin With further practice the impact of psychosensoric
(1977). Ackerman (1988) distinguishes three phases abilities on performance decreases and psychomotor
of skill acquisition: abilities play a more important role. In this third
The first phase is characterized by a phase of the skill acquisition process, the skill has
relatively strong demand on the cognitive-attentional been automated, so that performance is only limited
system, so that performance is slow and error prone. by psychomotor speed and accuracy (Ackerman,
Ackerman (1988) explains this phase as the one in 1988).
which potential ways for executing the current task Fig. 2 summarizes the described relationship
are worked out and (mentally) tested. Attention is between skill acquisition and ability-performance
focused on thoroughly understanding the task’s correlations.
constraints in question. With consistent practice, 1st phase
performance gets faster (see Schneider & Shiffrin, 2nd phase
1977) and attentional demands are reduced (see Fisk ability- 3nd phase
& Schneider, 1983). performance
During the second phase, the applied and correlation
successful ways of executing the task in question are
strengthened and fine-tuned. More efficient ways of
solving the task in question are found. number of practice
Finally, performance is fast and accurate.
The task is automated and can be completed without Figure 2: Theory of Skill Acquisition (adapted from
much attention. Ackerman, 1989).
Performance in each of these three phases is
determined by abilities, namely by general Taken into account the above described theory of
intelligence, perceptual speed ability, and structured intelligence it is to be assumed that in the
psychomotor abilities. General intelligence was first phase of skill acquisition, in which the learner is
defined by Ackerman (1989) in accordance to confronted the first time with a new problem,
Humphreys (1979), as the ability to acquire, store, innovation processes affect the derived solution. In
retrieve, combine, compare, and use information in contrast, the solution to a well-known problem will
new contexts. Perceptual speed refers to the ability be chosen based on the memory traces of the already
to complete very easy cognitive tasks. The core successfully applied solution alternatives. Hence, in
cognitive activity is to generate very simple potential this case, behaviour is experience-based.
solutions to effectively solve tasks as quickly as
possible. The key is the speed with which symbols
328
THEORY OF STRUCTURED INTELLIGENCE - Results on Innovation-based and Experience-based Behaviour
3.2 Conducted Study and Relevant two categories: sowing seeds (either sunflower or
Research Results ramson seeds) and setting in seedlings (either
flowering or foliage plants). The following actions
3.2.1 Research Questions and Hypotheses were required in order to sow the seeds:
The pots had to be placed in a seed box.
The, in the previous section described theory of skill The pots had to be filled with loosened
acquisition has been tested by various researchers soil.
(see e.g., Jipp, Pott, Wagner, Badreddin, & A hole had to be made into the soil.
Wittmann, 2004) and proved its empirical adequacy One seed had to be put in each hole.
in various settings and circumstances. The If the seeds were light seeds (as indicated on
relationship to the theory of structured intelligence the customer wish), the holes had to be
has been established theoretically in Section 2.1 and covered with wet pieces of newspaper.
will be tested empirically. Therefore, the following If the seeds were dark seeds (as indicated on
research questions and hypotheses are of interest: the customer wish), the holes had to be
It is hypothesized that the degree of covered with a 0.5 cm level of soil.
consistency with which given tasks are tackled The pots had to be watered. The water had to
decreases with the familiarity of the task. This is the be prepared so that it had a temperature of
case as formerly applied successful solutions will be 25°C and a, in the instructions specified acid
used to solve the problem at hand. Variation does value.
only occur when the former solution has not lead to For setting in the seedlings, the following actions
a satisfying result. had to be performed by the participants:
It is hypothesized that the transition from The required pots had to be filled half with
innovation-related processes to experience-based soil, which had to be loosened before.
solutions is determined by intelligence factors as The seedlings had to be put into the pot.
measured with traditional measurement scales for The correct fertilizer had to be chosen (as
intelligence factors. This is based on Ackerman’s indicated on the instructions, which were
theory (1989) combining skill acquisition processes handed out to the participant).
with factors of individual differences, i.e., general The pot had to be filled with layers of soil
intelligence, perceptual ability and motor skills. The and fertilizer until the roots of the seedlings
last is not considered in this paper due to the focus were covered.
on structured intelligence. The seedling had to be watered with
appropriate water (25°C and an acid value of
3.2.2 Description of the Sample and Course 5-6).
of the Study In order to acquire the task of leading the market
garden, four customer requirements had to be
To be able to answer these research questions, a executed: the first required the participants to sow
study has been conducted at the vocational school of sunflower seeds, the second to set in flowering
the Protestant Foundation Volmarstein seedlings, the third to set in foliage plants and the
(Evangelische Stiftung Volmarstein, Germany). last one to sow ramson seeds. The two categories of
Data from 13 students (6 male, 7 female students) tasks have been defined based on only minor
was at hand for the present analyses. The students differences in order to allow the participants to
were wheelchair-users and have been disabled for acquire the skill in question. Further customer
more than 12-15 years. Their average age was 22.5 wishes could not be executed by the participants due
years (SD = 1.6 years). The disabilities of the to problems related to maintaining attention for such
participants were spasticity, spina bifida, dysmelia a long time frame. The actions of the participants
or incomplete paralysis. were filmed with a standard web camera.
The study was conducted within two sessions. In the second session of the study, the
The first session lasted between one and two hours participants performed tasks of the Berlin
depending on the speed with which the participants Intelligence Structure Test (BIS, Jäger, Süß &
performed the designated tasks (see also Jipp, Beauducel, 1997). These tasks were based on the
Bartolein, & Badreddin, 2007). The tasks the Berlin Intelligence Structure Model (Jäger, 1982),
participants conducted referred to leading a little which is a hierarchical model of intelligence.
garden market. More specifically, the participants General intelligence, at the top, is composed of two
had to prepare products potential customers facets, which are categories for factors at the next
requested. These customer wishes were sorted in
329
lower level (Guttman, 1954). Jäger (1982) 3.2.3 Data Analyses and Results
distinguished the facet operations and contents. The
last subsumes three content abilities (i.e., numerical The following variables were derived:
abilities, verbal abilities, and numerical abilities), general intelligence, perceptual speed,
which refer to how a person cognitively deals with reasoning, memory, figural abilities, verbal abilities,
the different types of contents. The facet operation numerical abilities, figural perceptual speed, verbal
subsumes what is cognitively done with the given perceptual speed, numerical perceptual speed,
contents. Four operations are distinguished: figural reasoning, verbal reasoning, numerical
Reasoning is the ability to solve complex problems reasoning, figural memory, verbal memory,
(Jäger, Süß, and Beauducel, 1997). Memory asks the numerical memory were derived based on the
participants to memorize pieces of information and reduced set of test items applied from the BIS
retrieve them from short-term memory or recognize number of strategic changes in the
them after a short time period. Creativity refers to participants’ behavior for each of the four customer
the ability to produce a variety of differing ideas wishes
controlled by a given item. Last, perceptual speed is index for the continuity of the order of
the ability to work as fast as possible on simple actions while performing each of the four customer
tasks, requiring no or only little information wishes
processing demands. The BIS tests all these factors In order to derive the numerical values for the
of intelligence. However, the original test has been intelligence factors, the test items were analyzed as
shortened. Test items were deleted which required indicated in the BIS’s handbook (Jäger, Süß, and
the participants to write a lot, as – due to the given Beauducel, 1997).
time constraints for working on the test items – Altogether eight variables have been used to
especially participants with spacticity would have operationalize the degree of the innovation in the
been disadvantaged. The conducted test comprised observable behavior (i.e. in this case the gardening
the tasks as indicated in Table 1 and took the tasks): the first is the number of strategic changes in
participants about two hours to complete. the behavior of the participants for each of the
customer wishes. For this purpose, the videos have
Table 1: The, from the BIS chosen and in this study been transliterated: A list of possible actions has
applied test items and their sorting in the factors of been identified and the order of actions conducted
intelligence according to the Berlin Intelligence Structure has been analyzed. Each participant used a typical
Model. order of how to perform the task in question. The
General Figural Verbal Numerical number of changes to this typical order has been
Intell- abilities abilities abilities counted as indicating the number of strategic
igence changes in the behavior.
Perce- - Erasing - Part- - X-Greater To derive the four indices for the continuity of
ptual letters Whole - Calcul- the order of actions while performing the customer
speed - Old English - Classi- ating wishes, the number of grouped actions was counted.
- Number fying words characters A participant conducting all required actions for one
Symbol Test - Incomplete
pot received a very low index of continuity (i.e. 1);
records
Memory - Test of - Mean- - Pairs of whereas a participant executing one action for all
orientation ingful text numbers pots received a high level of continuity (i.e., 10).
- Company’s - Remem- - Two-digit Participants with a medium-sized index changed
symbols bering numbers their strategy within the task. In order to be able to
- Remem- words distinguish the participants who did not change their
bering routes - Language strategy and did change their strategy, a
of fantasy dichotomization was performed – the medium-sized
Reason- - Analogies - Word - Reading numbers received the final index number 0, the high-
ing - Charkow analogies tables and low-sized numbers received the final index
- Bongard - Fact - Arithmetic
- Winding opinion thinking
number 1.
- Comparing - Arrays of In order to test the hypotheses, repeated
conclusions letters measurement analyses have been performed with
each of the intelligent variables derived (list see
above) as independent variables and either the
number of strategic changes for all four customer
wishes or the index of continuity for all four
330
THEORY OF STRUCTURED INTELLIGENCE - Results on Innovation-based and Experience-based Behaviour
customer wishes as dependent variables. The strategic changes. The graph showing the course of
significant results are given in Table 2. More the number of strategic changes while performing
specifically, the tested variables, the value of the the four customer wishes for the participants with
used test statistic with the number of degrees of less verbal perceptual speed abilities demonstrates
freedom, the probability that the expected effect did (1) that the general level of the number of strategic
not occur due to chance and the size of the detected changes is greater compared to the number of
effect using a classification presented by Cohen strategic changes executed by the participants with
(1992) are given. higher verbal perceptual speed abilities and (2) that
the variance of change is bigger.
Table 2: Results of the repeated measurement analyses.
Tested variables Value of Prob- Effect

the used ability size
test
statistics
1) Learning factor F(3,36) p = 0.00 f² =
of the number of = 5.80 0.491
strategic changes
2) a) Learning F(3, 30) p = 0.02 f² =
factor of the number = 4.09 0.411
of strategic changes
b) Two way F(3, 30) p = 0.06 Figure 3: Line graph with standard error bars of the
interaction between = 2.75 f² = relationship between the strategic changes for the four
the learning factor 0.28² customer wishes and the verbal perceptual speed of the
and the verbal participants (Graph 0 shows the less intelligent
perceptual speed participants, Graph 1 the more intelligent participants).
factor
3) a) Learning F (3, 30) p = 0.01 f² = The third significant analysis tested the learning
factor of the number = 4.20 0.421 factor and the two way interaction effect between
of strategic changes
b) Two way F (3, 30) p = 0.03
the learning factor and the verbal memory factor.
interaction between = 3.31 f² = Figure 4 shows the direction of the effect. The two
the learning factor 0.33² graphs comparing the course of the number of the
and the verbal strategic changes while performing the four
memory factor customer wishes for the two groups of participants
4) a) Learning F (3, 30) p = 0.01 f² = with high and low scores on the verbal memory
factor of the index = 4.34 0.431 factor closely resemble the graphs displayed before
of continuity (see Fig. 3). Again, the less able participants
b) Two way F (3, 30) p = 0.01 demonstrate (1) a greater number of strategic
interaction between = 4.66 f² =
changes and (2) a bigger change in the course of the
the learning factor 0.471
and the numerical skill acquisition/learning process.
memory factor
*significant with α < 0.01
1
= large effect, ² = medium-sized effect
As Table 2 indicates, the first analysis testing the

learning factor of the number of strategic changes
over the performed tasks is significant. A large
effect has been found: The number of strategic
changes shrinks with the number of customer wishes
performed.
The second analysis tested the learning factor Figure 4: Line graph with standard error bars of the
and the two way interaction effect between the relationship between the strategic changes performed for
learning factor and the verbal perceptual speed four customer wishes and the verbal memory factor of the
factor. Figure 3 shows this interaction. The participants (Graph 0 shows the less intelligent
participants with greater verbal perceptual speed participants, Graph 1 the more intelligent participants).
abilities demonstrate more continuity regarding the
331
The fourth analyses tested (1) the learning factor were capable of judging on its user’s intelligence,
of the index of continuity and (2) the two way the interface can be adapted to the user and different
interaction between the learning factor and the levels of support given. This might have a big
numerical memory factor. The results were impact on safety-critical applications with the user is
significant as well. Hence, the index of continuity in the loop of controlling e.g., nuclear power plants.
changes with the number of practice trials performed Second, the theory of intelligence gives the artificial
and this change depends on the level of numerical intelligence research a new direction, as not only
memory abilities of the participants. experience is relevant, but also innovation-driven
behaviour, which can be modelled as chaotic
behaviour (see also Badreddin & Jipp, 2006).
4 CONCLUSIONS AND
TECHNICAL IMPACT
REFERENCES
Summarizing, relevant research results regarding the
Ackerman, P. L., 1988. Determinants of individual
theory of structured intelligence are presented
differences during skill acquisition: Cognitive abilities
(Badreddin & Jipp, 2006). More specifically, the and information processing. Journal of Experimental
study confirmed the hypotheses: First, the degree of Psychology: General, 117 (3), pp. 288-318.
consistency in the behaviour of the participants gets Anderson, J. R., 1982. Acquisition of cognitive skill.
greater with the task’s familiarity. This transition Psychological Review, 89, pp. 369-406.
shows the learning effect, i.e. the transition from Badreddin, E., Jipp, M., 2006. Structured Intelligence.
innovation-based to experience-based, i.e., well International Conference on Computational
known solutions. Second, a relationship between this Intelligence for Modelling, Control, and Automation
transition and traditional measures of intelligence CIMCA 2006, Sydney, Australia.
Campbell, D. T., Fiske, D. W., 1959. Convergent and
has been found. Significant predictors were
discriminant validity by the multitrait-multimethod
intelligent factors such as the verbal perceptual matrix, Psychological Bulletin, 56, pp. 81-105.
speed factor. However, compared to traditional Fisk, A. D., Schneider, W., 1983. Category and word
research on intelligence, the theory of structured search: Generalizing search principles to complex
intelligence goes one step further: it provides an processing. Journal of Experimental Psychology:
explanation of how intelligent behaviour emerges Learning, Memory & Cognition, 9, pp. 177-195.
and not only a classification of intelligent behavior. Fleisman, E. A., 1972. On the relations between abilities,
Drawbacks of the study refer to the small number of learning, and human performance. American
participants, which has reduced the power of the Psychologist. 27, 1017-1023.
Guttman, L. (ed.), 1954. Mathematical thinking in the
study, so that some possibly existing effects might
social sciences. Glencoe, IL. The French Press.
not have been detected. Further, the creativity items Humphreys, L. G., 1979. The construct of general
had to be deleted from the intelligence test as they intelligence. Intelligence, 3, 105-120.
required the participants to draw solutions, which Jäger, A. O., 1982. Mehrmodale Klassifikation von
would disadvantage some of the participants due to Intelligenzleistungen: Experimentell kontrollierte
their disability. Hence, future research should Weiterentwicklung eines deskriptiven
investigate the relationship between traditional Intelligenzstrukturmodells, Diagnostica, 28(3), 195-
creativity tests with innovation-based behaviour. 225.
The study’s main contribution is twofold: First, Jäger, A. O., Süß, H.-M., Beauducel, A., 1997. Berliner
Intelligenzstruktur-Test, Göttingen, Hogrefe.
the theory of structured intelligence demonstrates
Jipp, M, Bartolein, C., Badreddin, E., 2007. Assisted
links to traditional measurements of intelligence, but Wheelchair Control: Theoretical Advancements,
also gives an explanation of how intelligent Empirical Results and Technical Implementation,
behaviour emerges and provides the opportunity to Submitted for publication.
measure intelligence in easier and less time- Jipp, M., Pott, P., Wagner, A., Badreddin, E., Wittmann,
consuming ways and. It allows intelligence to be W. W., 2004. Skill acquisition process of a robot-
judged on (1) by using activity detection and (2) by based and a traditional spine surgery, 1st International
observing the participants’ actions when being Conference on Informatics in Control, Automation and
aware of the familiarity of the task. The degree of Robotics, pp. 56-63.
Schneider, W., Shiffrin, R. M., 1977. Controlled and
consistency gives valuable information on
automatic human information processing: 1.
intelligence. This has not only the potential to Detection, search, and attention. Psychological
revolutionize intelligence diagnostics but also Review, 84, pp. 1-66.
intelligent interface design: if an intelligent machine
332
X3DIRECTOR
A Front-end For Modeling Web Virtual World Navigation And Visualization
Parameters
Berta Buttarazzi and Daniele Pizziconi

University of Rome “Tor Vergata”, Rome, Italy
[email protected],[email protected]
Keywords: Virtual Reality, Virtual Environment, interactivity, fully immersive viewing systems, post-production phase,
digital technologies.
Abstract: With the advent of digital communication technologies, a huge quantity of information can be stored and
transferred. However, with increased possibility comes increasing complexity. As indicated by the
dissatisfaction often experienced by users of information technology, current systems of accessing,
organizing and navigating information are proving insufficient. For this reason, in the Web world, besides
new media contents, new technologies for implementing more dynamic and user-friendly interfaces are
being developed. Currently, the last step in the evolution of digital technologies for web interface and
contents is represented by virtual environments which, despite the appearances, are, in some cases, very
accessible and economic. This paper will provide a panorama on the functionalities of language X3D and
the identification of the cases in which is possible to use X3Director.
1 INTRODUCTION As the technologies of VR evolve, the

applications of VR become literally unlimited. It is
With the advent of digital communication assumed that VR will reshape the interface between
technologies, a huge quantity of information can be people and information technology by offering new
stored and transferred. The last step in the evolution ways for the communication of information, the
of digital technologies for web interface and visualization of processes, and the creative
contents is represented by virtual environments, and expression of ideas. Note that a virtual environment
so by X3D. can represent any three-dimensional world that is
The syntax and data structure of X3D provide an either real or abstract. This includes real systems like
excellent tool for the modeling of three-dimensional buildings, landscapes, underwater shipwrecks,
worlds that are functional and interactive and that spacecrafts, archaeological excavation sites, human
can, ultimately, be transferred into fully immersive anatomy, and so on and abstract systems like
viewing systems. magnetic fields, molecular models, mathematical
X3D allows to combine other Virtual Reality systems and any other conceivable system including
(VR) technologies and real environment, for artistic and creative works of abstract nature.
example motion trackers are employed to monitor This paper and therefore X3Directior is proposed
the movements of dancers or athletes for subsequent like an aid in virtual environment refining those
studies in immersive VR. These ‘Augmented already existing, but not complete and not still ready
Reality’ technologies allow for the viewing of real for the distribution in the Web world.
environments with superimposed virtual object; for In particular, the paper mainly deals with a
example telepresence systems (e.g., telemedicine, specific category of users who might be called “non-
telerobotics) immerse a viewer in a real world that is expert developer”. They are users that use, for work
captured by video cameras at a distant location and or pleasure, 3D graphics development software
allow for the remote manipulation of real objects via which supports virtual development language as
robotic arms and manipulators. X3D. These users can get a virtual representation of
their 3D projects without having any knowledge of
the X3D language. The goal is therefore to provide a
333
software that helps this category of users in the saving, thanks to the preview function (it is
refining phase, again without the exigency of any necessary to have a player X3D installed in the
particular knowledge. operative system). This function can be used in order
The second chapter introduces the X3Director to evidence the modifications carried out globally in
software and its features. the file X3D and therefore to have a preview of the
final complete file X3D or to evidence single
geometries and therefore to have a visual reply on
2 X3DIRECTOR the single Transform or Group nodes. In order to
complete this global view on the X3Director it is to
The production of the three-dimensional structure of be underlined that such a software does not forget
an X3D environment can completely be delegated to the more expert users that they do not have need of
the software of authoring 3d and, that concurs to the driven procedure. In order to edit or to add the
customers non-expert of X3D to develop, in above mentioned nodes, these users can either use
immediate and practically free way, virtual simple functions of “add” or “delete” or can directly
environments compatible with the Web. After such modify the values of the attributes editing them with
phase it is necessary, for the effective fruibility of a simple double click on the data structure JTable
the virtual representation, a phase of post-production left on purpose editable for this function.
that proceeds along one series of tried and
modifications. Phase that cannot be delegated to the 2.1 Characterizing Features
software of authoring 3D is because often, being
thought for other, do not support some of the The absence of the necessity of specific knowledge
demanded characteristics (e.g. the Background differentiates X3Director from the others editor X3D
node), or, in case they come supported, their and it renders it ideal for the phase of post-
implementation demands specific knowledge of production. The below table shows a comparison of
language X3D. main editors X3D and X3Director. From the
X3Director helps the “non-expert developer” to comparison, that refers exclusively to the phase of
accomplish the post-production phase. The software post-production, tool of optimization of the code and
provides a series of functions to manage the 3D the software of authoring 3d have been omitted.
aspect of an X3D file. It allows realizing different
Table 1: Comparison between X3D post-production tools.
actions so as to add localized audio contexts or
sounds that start after having clicked on a particular Software Knowledge Potentiality Complexity
area or object, thanks to the use of the TouchSensor X3D
X3Director null medium low
node.
SwirlX3D medium medium medium
X3Director proposes a linear, intuitive and
X3D-Edit high high medium
provided of continuous assistance interface that
allows the user in few mouse clicks to edit or to add
Everyone of the software shown can be used to
ex novo the Background node in order to better
modify file X3D exported from the software of
characterize a three-dimensional environment, the
authoring. It is obvious that, with the exception of
NavigationInfo node that describes the view and the
X3Director, is however necessary a sure level of
characteristics of the avatar, user’s alter-ego, and the
knowledge of language X3D.
Sound and AudioClip nodes. Moreover, thanks to
SwirlX3D is a powerful X3D authoring
integration of parser DOM with the data structure
environment that gives full access to the data
JTree provided from the Java Swing package,
structures in an X3D file and is able to preview the
X3Director provides, at nodes level, for the loaded
completed scene. The specific knowledge of X3D
X3D file, an interface characterized from tree
are not excessive but of against it does not offer, as
structure that reflects the characteristic hierarchical
instead X3Director offers, no visual procedures that
structure of XML files. As far as the attributes,
it renders the formulation of the parameters
instead, X3Director has integrated an other data
independent from the knowledge of language X3D.
structure, the JTable, in order to organize and to edit
The graphics file editor X3D-Edit includes the
in “expert” way the attributes of the nodes.
typical characteristics of this type of editor: context-
Such a software, conceived in order to come
sensitive editing prevents creation of incorrect
towards requirement of a non-expert user in the
scene-graph structures and detects invalid numeric
modeling of virtual environment in X3D, allows to
values, context-sensitive tooltips provide concise
verify the user’s modifications, before the final
334
X3DIRECTOR - A Front-end For Modeling Web Virtual World Navigation And Visualization Parameters
explanations and warnings of each X3D node and 2.3 Update

field, tag-inspectors, syntax highlighting, tag hints,
code fragments, objects, edit dialogs and much Once the file is loaded, a series of operations can be
more. They demand a deepened knowledge of carried out on it as addition or the modification of
language X3D with the exception of X3Director that nodes Background, NavigationInfo, Sound,
has a complexity of use practically null thanks to the AudioClip and TouchSensor.
friendly and intuitive interface, and offers to the In the next subsections the driven procedures
developer the possibility to act on the independent, provided by X3Director will be analyzed one by
from the three-dimensional structure, aspects of the one.
virtual environment (phase of post-production for
non-expert customers), or to modify whichever other 2.3.1 Navigation and Avatar Parameters
parameter (expert way). In the next sections the
several features are described in more detail. Navigation and Avatar parameters are included in
the NavigationInfo node. In order to open frame
2.2 File Selection and Parser (Figure 3), where it is possible to update such
parameters, it is necessary in a loaded X3D file,
When the X3D file is selected and loaded, either the presence of a NavigationInfo node to be
X3Director displays it using a JTree that reflects the modified, or clicking on the Scene or Group node
hierarchical structure of an X3D/XML file (see and to push the AddNavigationInfo button present in
Figure 1 below). the toolbar of X3Director in order to add a node ex
novo.
Figure 1: Node X3D in tree structure. Figure 3: NavigationInfo procedure.
2.3.2 Background Parameters

Also for this node, as for the NavigationInfo, the
driven procedure (Figure 4) can be opened in order
to modify an existing Background node or in order
to add a Background node ex novo. Such procedure
allows the user to select three colors for sky and
three for the land through the JColorChooser class
supplied from the Java Swing package.
X3Director provides an interface with a text field
for every cube’s face with relative file browser
Figure 2: Attribute X3D in JTable structure. button. The image path can be directly put in the
relative text field or it can be obtained by file
Now it is possible to have access of all browser. The software, moreover, features a preview
functionalities provided by X3Director. As an of images selected.
example clicking on any node, such software will
show to right, organized in a JTable, the attributes
and their values of the selected node (Figure 2):
then, as pointed out in the previous cross-section, it
is possible to edit manually, by the more expert
users, the values of the attributes.
335
Figure 6: Sound procedure.

Figure 4: Background procedure.
2.3.3 Sound Parameters

3 CONCLUSIONS
For the adjustment of sound parameters X3Director
provides a further driven procedure (Figure 6). Here The Web represents today a fantastic showcase for
it is possible to modify the parameters regarding the commercial or non-commercial messages. Its
ellipsoids representing localization and propagation diffusion and inexpensiveness makes it the best way
of the audio source (it is also possible to listen in to obtain visibility and in this scenario it is
preview the file audio selected) that otherwise have fundamental to offer a good Web experience that is
to be specified in a procedure regarding the not frustrating and that has a good impact on the
visitors. It is therefore developed X3Director, a
AudioClip node (Figure 5).
front-end which helps the X3D non-expert users (but
Taking up the Sound procedure again,
it could also be helpful to expert users) for modeling
X3Director gives the possibility to add audio file navigation and visualization parameters in X3D
with use of TuchSensor node. In this case the audio environment. The software allows realizing the post-
will start from the user’s click on the geometry production phase without any knowledge of X3D
before selected in the Sound procedure. Once added language.
the Sound node with TouchSensor it is possible to An X3D non-expert user could therefore develop
add to a further TouchSensor node and respective and deliver a X3D content on the Web by using the
node ROUTE required to assume the task of “stop” combination 3D authoring software-X3Director.
to the file audio enabled with a click thanks to the
previous TouchSensor.
REFERENCES
Pohja, M., 2003. X3D and VRML Sound Components.
Bouras, Ch., Panagopoulos, A., Tsiatos, Th., 2005.
Advanced in X3D Multi-user Virtual Environments,
Seventh IEEE International Symposium on
Multimedia (ISM’05).
Behr, J., Dahne, P., Roth, M., 2004. Utilizing X3D for
Immersive Environments.
Figure 5: AudioClip procedure. Figueroa, P., Medina, O., Jimenez, R., Martinez, J.,
Albarracin, C., 2005. Extensions for Interactivity and
Retargeting in X3D.
336
A STUDY OF TWO COLOR SYSTEMS USED IN
CONTENT-BASED IMAGE QUERY ON MEDICAL IMAGERY
Liana Stanescu, Dumitru Burdescu, Cosmin Stoica and Marius Brezovan

University of Craiova, Faculty of Automation, Computers and Electronics
Keywords: Image feature extraction, image processing, content-based visual query, color, histogram, HSV color space,
l color space, medical images.
Abstract: The article presents a comparative study over two methods used in content-based visual query. The two
methods refer at two different color systems used for representing color information from images: HSV
quantized to 166 colors and l1l2l3 quantized to 64 colors. The originality of the study comes from the fact
that it was made on a database with medical images from digestive tract area, captured by an endoscope.
The scope was to check the quality of the content-based visual query on images representing five different
diagnoses (colitis, ulcer, esophagitis, polyps and ulcerous tumor) and taking into consideration that some
parameters were modified during the capturing process: viewing direction, intensity and direction of the
illumination, parameters that affect mostly the medical images captured during the diagnosis process.
1 INTRODUCTION In the second step the content based retrieval is

made, by choosing a query image, calculating the
characteristics vector, comparing this vector with
As the world is in the middle of the digital era, the
quantity of visual information is increasing (Sebe each vector of the images stored in the database and
and Lew, 2001). More than 2700 digital pictures are viewing the most similar images.
The color is one of the base image properties. In
made in every second (in total 85 billion images
yearly). For example, PhotoWorks includes tens of content based retrieval on color feature it is desired
millions of images on its web site. The Internet to find the images from the database having the
color composition closest to the color composition
allows us to have access to a big part of these
images. The common images are completed by of the query image (Del Bimbo, 2001, Gevers and
images with special purpose, like medical images Smeulders, 1999).
The color content of an image is best represented
with an estimation of 2 billion per year. Because of
the tendency for digital (television, movies) and by color histograms.
because everybody will have access to everything, Comparing color histograms of the query image
and target image is done by histograms intersection
the number of images will be increasing. The world
production of digital information in 2007 is or by the quadratic distance between histograms that
estimated to be more than 109 GB (250 MB for each takes into consideration the conceptual similitude
between two colors (Sebe and Lew, 2001, Smith,
man on the planet, ignoring his technological
development). It is estimated that in the next 10 1997).
years, each of us will manipulate terabytes of As was said before, one of the domains where a
large number of images are accumulated is the
information (video, static images, music, photos and
documents) every day. medical domain. The advantages of using content-
These image databases are associated with the based visual query on medical images are on the
following directions (Muller et al, 2004):
problem of the content-based retrieval, solved in two
steps (Sebe and Lew, 2001). • Medical teaching
In the first step, when inserting a new image, it • Medical research
will be pre-processed and some features will be • Diagnostic aid
automatically extracted: color, texture and shape. • Electronic patient records
The result will be a characteristics vector that will be
stored in the database.
337
The medical images are being produced directly • Robust against imaging conditions: invariant to a
by medical equipment used in patient diagnosis, or change in viewing direction, invariant to a
by digitalizing the images stored on other devices. change in object geometry, invariant to a change
In each of these methods some changes can in direction and intensity of the illumination and
occur: invariant to a change in the spectral power
• Changes in viewing direction distribution of the illumination.
• Changes in direction of the illumination The studies have shown that two of these color
• Changes in the intensity of the illumination systems can be used, with good results in a content-
As a result, the purpose of the paper is to make a based visual query process, namely HSV and l1l2l3
comparative study of the content-based query results (Gevers et al, 2006).
effectuated on medical images database where the It was proved that the HSV color system has the
color information is represented by HSV and l1l2l3 following properties (Gevers, 2004):
color systems. • It is close to the human perception of colors
The originality of the study is given by the fact • It is intuitive
that the experiments are made on medical images • It is invariant to illumination intensity and
from digestive area produced by an endoscope. The camera direction
ill area is seen from different directions and in The studies made on nature and medical images
different illumination intensity. This study, unlike have shown that in the case of the HSV, RGB and
the others made on CorelDraw images, uses images CieLuv color systems, the HSV color space
produced in real condition, in patient diagnosis. produces the best results in content based retrieval
The paper has the following structure: In section (Stanescu et al, 2006).
2 the two color systems are presented. In section 3 Still, the HSV color space has several problems
the conditions and the results of the experiments are (Gevers, 2004) :
presented, and in section 4 the conclusions of the • Nonlinear (but still simple) transformation from
comparative study are discussed. RGB to HSV
• Device dependent
• the H component becomes instable when S is
2 CONTENT-BASED IMAGE close to 0
• the H component is dependent of the
QUERY ON COLOR FEATURE illumination color
Gevers and Smeulders have proposed a new
The color is the visual feature that is immediately
color system, named l, whose components are
perceived on an image. The color space used for
defined using the equations (Gevers and Smeulders,
representing color information in an image has a
great importance in content-based image query, so 1999):
this direction of research was intensely studied (Del
Bimbo, 2001). ( R − G)2
There is no color system that it is universal used, l1( R, G , B ) =
because the notion of color can be modeled and ( R − G ) 2 + ( R − B ) 2 + (G − B ) 2
interpreted in different ways. Each system has its ( R − B) 2
own color models that represent the system l 2( R, G, B) = (1)
( R − G)2 + ( R − B)2 + (G − B)2
parameters (Gevers, 2004).
There were created several color spaces, for (G − B)2
l 3( R, G, B) =
different purposes: RGB (for displaying process), ( R − G ) 2 + ( R − B)2 + (G − B)2
XYZ (for color standardization), rgb, xyz (for color
normalization and representation), CieLuv, CieLab
(for perceptual uniformity), HSV (intuitive Where R, G, B are the color values in the RGB
description) (Gevers, 2001, Gevers, 2004). The color color space. They also showed that the l color
systems were studied taking into consideration system is invariant to viewing direction, illumination
different criteria imposed by content-based visual direction and intensity. In this case it is also a
query (Gevers and Smeulders, 1999): nonlinear, but simple, transforming from RGB space
• The independence of the imaging device to l space.
• Perceptual uniformity In case of medical images the main problems are
• Linear transformation regarding changing illumination intensity and
viewing direction. That is why the two color spaces
• Intuitive for user
presented above are chosen.
338
A STUDY OF TWO COLOR SYSTEMS USED IN CONTENT-BASED IMAGE QUERY ON MEDICAL IMAGERY
The operation of color system quantization is In order to make the query the procedure is:
needed in order to reduce the number of colors used • a query image is chosen
in content-based visual query: from millions to tens. • the dissimilitude between the query image
The quantization of the HSV color space to 166 and every target image from the database is
colors, solution proposed by J.R. Smith, is the idea computed, for each two specified criteria
used in this study (Smith, 1997). For the color space (color histograms with 166 colors and the
l1l2l3 the solution of quantization to 64 colors is color histogram with 64 colors);
chosen, keeping 4 values for each component of the • the images are displayed on 2 columns
system. The fact that a color system is quantized to corresponding to the 2 methods in
166 colors and the other to 64 colors does not ascending order of the computed distance.
influence the quality of the content-based image For each query, the relevant images have been
query process, the research studies showing clearly established. Each of the relevant images has become
this aspect (Stanescu et al, 2006). The color in turn a query image, and the final results for a
query are an average of these individual results.
histograms represent the traditional method of
The experimental results are summarized in table
describing the color properties of the images. They
1. Method 1 represents the query using the HSV
have the advantages of easy computation and up to color space quantized at 166 colors and Method 2
certain point are insensitive to camera rotating, represents the query on color using the l1l2l3 color
zooming, and changes in image resolution (Del space quantized at 64 colors. The values in the table
Bimbo, 2001). In case of both color systems, to represent the number of relevant images of the first 5
compute the distance between the color histograms images retrieved for each query and each of the
of the query image and the target image, the methods, as an average of the values obtained on
intersection of the histograms is used (Smith, 1997). each executed query.
The studies have also shown that using this metric in
content-based visual query gives very good results Table 1: Experimental results.
as quadratic distance between histograms that is
more difficult to calculate (Smith, 1997, Stanescu et Query Method 1 Method 2
al, 2006). Polyps 3.6 3.2
Colitis 3.5 3.1
Ulcer 3.2 2.9
Ulcerous Tumor 3.5 3.1
3 EXPERIMENTS Esophagitis 3.4 3.1
The experiments were performed in the following It must be mentioned that the queries were made
conditions. for each of the 5 diagnostics in part. The notion of
A database with 520 color images from the field relevant image was strictly defined. The images
of the digestive apparatus was created. The images from the same patient captured at different
are from patients with the following diagnosis: illumination intensity and from different points of
polyps, ulcer, esophagitis, ulcerous tumors and view were considered relevant for a query, and not
colitis. For each image there are several images with the ones with the same diagnosis. The quality of the
affected area captured from 3 or 4 viewing content-based image query process was strictly
directions. For each image in the database there is analyzed. In figure 1 there is an example of content-
another identical image, but having the illumination based image query considering the two specified
intensity changed. methods for images categorized as colitis. The first
A software tool that permits the processing of column contains 5 images retrieved by Method1 and
each image was created. The software tool executes the second contains the images retrieved using
the following steps: Method2. In the first case there are 5 relevant
1. the transformation of image from RGB images and in the second case, 4 relevant images.
color space to HSV color space and the
quantization to 166 colors
2. the transformation of image from RGB
color space to l1l2l3 color space and the 4 CONCLUSION
quantization to 64 colors
3. calculation of the two color histograms The paper presents the condition in which the
with 166, respectively 64 values, that quality of the content-based visual query process
represent the characteristics vectors and was studied, using a collection of medical images
storing them in the database from digestive tract. The quality was measured
339
2. for images representing the same ill area,

captured to different illumination intensities,
the solution that uses l1l2l3l color system
quantized to 64 colors, gives the best results
in querying process
3. globally, the solution that uses HSV color
space gives most satisfying results, because
the database includes both types of images
In general, for medical images, the first case,
with images representing ill area captured from
different angles is the most frequent case. So, that is
why the use of HSV color space, quantized to 166
colors, is recommended. The situation in the
database that was studied was the same, namely, the
number of images captured from different angles
was higher than the number of images where only
the illumination intensity was different.
In the future the study will be extended by using
a bigger database with much more images in order to
see if this conclusion will be also confirmed. New
experiments with images from other parts of the
human body or images produced by other medical
devices will be effectuated.
REFERENCES
Del Bimbo, A., 2001. Visual Information Retrieval,
Morgan Kaufmann Publishers. San Francisco USA.
Gevers, T., Smeulders, W.M., 1999. Color-based object
recognition. Pattern Recognition. 32, 453-464
Gevers, T., 2001. Color in Image Search Engines. In
Principles of Visual Information Retrieval. Springer-
Verlag, London.
Gevers, T., 2004. Image Search Engines: An Overview. In
Emerging Topics in Computer Vision. Prentice Hall.
Gevers, T., Van de Weijer. J., Stokman, H., 2006. Color
Feature Detection. In Color Image Processing:
Figure 1: The retrieved images using the two specified Methods and Applications. CRC Press.
methods. Muller, H., Michoux, N., Bandon, D., Geissbuhler, A.,
2004. A Review of Content_based Image Retrieval
Systems in Medical Application – Clinical Benefits
calculating the precision and recall parameters. HSV and Future Directions. Int J Med Inform. 73(1)
system, quantized to 166 colors and l1l2l3 color Sebe, N., Lew, M., 2001. Color-based retrieval. Pattern
system quantized to 64 colors were considered Recognition Letters. 22, 223-230
highlighting the way they influence the process of Smith, J.R., 1997. Integrated Spatial and Feature Image
content-based visual query if some important Systems: Retrieval, Compression and Analysis, Ph.D.
parameters that often affects medical images are thesis, Graduate School of Arts and Sciences.
modified: viewing direction, direction and intensity Columbia University.
of the illumination. Stanescu, L., Burdescu, D.D., Ion, A., Brezovan, M.,
Several conclusions can be formulated after the 2006. Content-Based Image Query on Color Feature in
experimental results were analyzed: the Image Databases Obtained from DICOM Files. In :
1. to find images representing the same ill area, International Multi-Conference on Computing in the
that were captured by an endoscope from Global Information Technology. Bucharest. Romania
several viewing directions, the solution that
uses HSV color system quantized to 166
colors gives the best results
340
COMPARISON OF TWO IDENTIFICATION TECHNIQUES:
THEORY AND APPLICATION
Pierre-Olivier Vandanjon(1), Alexandre Janot(2)

(1)
LCPC, Route de Bouaye BP 4129, 44341Bouguenais Cedex, France
(2)
CEA/LIST Interactive Robotic Unit, 18 route du Panorama, BP 6, 92265 Fontenay aux Roses, Cedex, France
Maxime Gautier(3), Flavia Khatounian(2)

(3)
IRCCyN, Robotic team, 1, rue de la Noë - BP 92 101 - 44321 Nantes Cedex 03, France
[email protected]
Keywords: Parameters identification, inverse model, least squares method, simple refined instrumental variable method.
Abstract: Parametric identification requires a good know-how and an accurate analysis. The most popular methods
consist in using simply the least squares techniques because of their simplicity. However, these techniques
are not intrinsically robust. An alternative consists in helping them with an appropriate data treatment.
Another choice consists in applying a robust identification method. This paper focuses on a comparison of
two techniques: a “helped” least squares technique and a robust method called “the simple refined
instrumental variable method”. These methods will be applied to a single degree of freedom haptic interface
developed by the CEA Interactive Robotics Unit.
1 INTRODUCTION At the same time, robust methods have been

successfully used (Mili, Phaniraj and Rousseeuw,
In most of cases, parametric identification is a hard 1990), (Mili et al, 1996), (Young 2006) and (Gilson
task and requires a good know-how. A popular et al, 2006). An interesting robust method is the
method consists in using the least squares technique simple refined instrumental variable (SRIV) because
(LS) because of its simplicity. However, it has been of its simplicity (Young 1979). Indeed, no noise
proven that the LS are sensitive to outliers, leverage model identification is needed and this method is
points and noise models (Hampel, 1971) and consistent even in the coloured noise situation
(Hueber, 1981). In other words, the LS are not (Gilson et al, 2006). This method is implemented in
intrinsically robust and we must “help” them in the MATLAB CONTSID toolbox developed by the
order to obtain reliable results. CRAN team (Garnier, Gilson and Huselstein, 2003)
In the last decade, the IRCCyN robotic team has and (Garnier, Gilson and Cervellin, 2006). To our
designed a identification method based on LS knowledge, this method has not been used to
technique, inverse model, base parameters, exciting identify inertial parameters of robots.
trajectories and appropriate data treatment (Gautier Hence, it seems interesting to apply the SRIV
and Khalil, 1990), (Gautier and Khalil, 1991) and method to a single degree of freedom haptic
(Gautier, 1997). This technique has been interface and to compare it with the LS method.
successfully applied to identify inertial parameters The paper is organized as follows: second
of industrial robots (Gautier, Khalil and Restrepo, section presents the general inverse dynamic model
1995) and (Gautier, 1997). Recently, it was used for of robots, the LS technique and the SRIV method;
identifying electrical parameters of a synchronous experimental results are presented in section 3;
machine (Khatounian et al, 2006), inertial finally, section 4 introduces a discussion.
parameters of a haptic device (Janot et al, 2006) and
the parameters of a compactor (Lemaire et al, 2006).
The obtained results were interesting and reliable.
341
2 THEORY 2.2 LS Method
2.1 General Inverse Dynamic Model 2.2.1 General Theory
The inverse dynamic model (commonly called Generally, ordinary LS technique is used to estimate
dynamic model) calculates the joint torques as a the base parameters solving an over-determined
function of joint positions, velocities and linear system obtained from a sampling of the
accelerations. It is usually represented by the dynamic model, along a given trajectory (q, q , q
),
following equation: (Gautier, Khalil and Restrepo, 1995), (Gautier,
Γ = A(q)q
+ H(q, q ) + Fv q + Fc sign(q ) (1) 1997). X being the b minimum parameters vector to
be identified, Y the measurements vector, W the
Where, Γ is the torques vector, q, q and q are observation matrix and ρ the vector of errors, the
respectively the joint positions, velocities and system is described as follows:
accelerations vector, A(q) is the inertia matrix,
Y(Γ ) = W(q, q , q
)X + ρ (4)
H(q, q ) is the vector regrouping Coriolis, centrifugal
and gravity torques, Fv and FC are respectively the The L.S. solution X̂ minimizes the 2-norm of the
viscous and Coulomb friction matrices. vector of errors ρ. W is a r×b full rank and well
The classical parameters used in this model are the conditioned matrix, obtained by tracking exciting
components XXj, XYj, XZj, YYj, YZj, ZZj of the trajectories and by considering the base parameters,
j
inertia tensor of link j denoted Jj, the mass of the r being the number of samplings along a trajectory.
link j called Mj, the first moments vector of link j
j Hence, there is only one solution X̂ (Gautier, 1997).
around the origin of frame j denoted MSj=[MXj MYj
Standard deviations σ X̂ are estimated using
MZj]T, and the friction coefficients FVj, FCj. i
The kinetic and potential energies being linear classical and simple results from statistics. The
with respect to the inertial parameters, so is the matrix W is supposed deterministic, and ρ, a zero-
dynamic model (Gautier and Khalil, 1990). It can mean additive independent noise, with a standard
thus be written as: deviation such as:
Γ = D(q, q , q
)χ (2) ( )
C ρ = E ρρ T = σ ρ2 I r (5)
) is a linear regressor and χ is a
Where D(q, q , q
where E is the expectation operator and Ir, the r×r
vector composed of the inertial parameters.
identity matrix. An unbiased estimation of σ ρ is:
The set of base parameters represents the minimum 2
number of parameters from which the dynamic ˆ
Y − WX
model can be calculated. They can be deduced from σ ρ2 = (6)
the classical parameters by eliminating those which
(r − b )
have no effect on the dynamic model and by The covariance matrix of the standard deviation is
regrouping some others. In fact, they represent the calculated as follows:
only identifiable parameters.
Two main methods have been designed for
calculating them: a direct and recursive method
(
C Xˆ Xˆ = σ ρ2 W T W )
−1
(7)
based on calculation of the energy (Gautier and σ 2X̂i = C X̂X̂ is the ith diagonal coefficient of C Xˆ Xˆ .
Khalil, 1990) and a method based on QR numerical ii
decomposition (Gautier, 1991). The numerical The relative standard deviation %σ X̂ is given by:
ri
method is particularly useful for robots consisting of
closed loops. σ X̂
By considering only the b base parameters, (2) can %σ X̂ = 100 j
(8)
be rewritten as follows:
jr
Xj
Γ = W(q, q , q
)χ b (3) However, in practice, W is not deterministic. This
) is the linear regressor and χb is
Where W(q, q , q problem can be solved by filtering the measurement
matrix Y and the columns of the observation matrix
the vector composed of the base parameters. W.
342
2.2.2 Data Filtering If the trajectories are not enough exciting, then the
results have no sense because the system is ill
Vectors Y and q are samples measured during an conditioned and some undesirable regroupings
experimental test. We know that the LS method is occur.
sensitive to outliers and leverage points. A median
filter is applied to Y and q in order to eliminate 2.3 SRIV Method
them.
The derivatives q and q are obtained without From a theoretical point of view, the LS assumptions
phase shift using a centered finite difference of the are violated in practical applications. In the equation
vector q. Knowing that q is perturbed by high (4), the observation matrix W is built from the joint
frequency noises, which will be amplified by the positions q which are often measured and from q , q
numeric derivation, a lowpass filter, with an order which are often computed numerically from q.
greater than 2 is used. The choice of the cut-off Therefore the observation matrix is noisy. Moreover
frequency ωf is very sensitive because the filtered identification process takes place when the robot is
data (q f , q f , q f ) must be equal to the vector controlled by feedback. It is well known that these
(q, q , q ) in the range [0, ωf], in order to avoid violations of assumption imply that the LS solution
is biased. Indeed, from (4), it comes:
distortion in the dynamic regressor. The filter must
have a flat amplitude characteristic without phase W T Y = W T WX + W T ρ (12)
shift in the range [0, ωf], with the rule of thumb
ωf>10*ωdyn, where ωdyn represents the dynamic As ρ includes noises from the observation matrix
frequency of the system. Considering an off-line
identification, it is easily achieved with a non-causal
( )
E WTρ ≠ 0 .
zero-phase digital filter by processing the input data The Refined Instrumental Variable approach deals
through an IIR lowpass butterworth filter in both the with this problem of noisy observation matrix and
forward and reverse direction. The measurement can be statistically optimal (Young, 1979). It is the
vector Y is also filtered, thus, a new filtered linear reason why we propose to enrich our identification
system is obtained: methods by using concepts from this approach. In
the following, we describe the application of this
Yf = Wf X f + ρ f (9) method in our field.
The Instrumental Variable Method proposes to
Because there is no more signal in the range [ωf, remove the bias on the solution by building the
ωs/2], where ωs is the sampling frequency, vector Yf
and matrix Wf are resampled at a lower rate after
( )
instrument matrix V such as E V T ρ = 0 and VTW
lowpass filtering, keeping one sample over nd is invertible.
samples, in order to obtain the final system to be The previous equation becomes:
identified:
V T Y = V T WX + V T ρ (13)
Yfd = Wfd X fd + ρ fd (10)
The instrumental variable solution is given by
with:
n d = 0.8ωs /2ω f (11)
ˆ = VTW
X V ( )−1
VTY (14)
The main problem is to find the instrument matrix

V. A classical solution is to build an observation
2.2.3 Exciting Trajectories matrix from simulated data instead of measured
data. The following iterative algorithm describes this
Knowing the base parameters, exciting trajectories
solution:
must be designed. In fact, they represent the
Step 0: a first set of parameters is given by using
trajectories which excite well the parameters. If the
classical LS.
trajectories are enough exciting, then the
ˆ
conditioning number of W, (denoted cond(W)) is Step k: From a set of parameters X V/k −1 given at
close to 1. However, in practice, this conditioning the previous step, the following ordinary differential
number can reach 200 because of the high number of equation, describing the robot dynamic, is simulated:
base parameters. Design and calculations of these
trajectories can be found in (Gautier and Khalil, = A −1 (Γ + H(q, q ) + Γ f
q ) (15)
1991).
343
Γ is the motor vector which is generally given

by a feedback built by comparison between the The frames defined to model the interface are
desired and the real movement. presented Figure 2. The inverse dynamic model is
A , H , Γ f are respectively the inertia matrix, the given by the following equation:
Coriolis-centrifugal vector and the friction Γ = Jq − M X gcos(q) + M Y gsin(q) + Γf (16)
torques. They are computed with the parameters
identified at the step k-1: X ˆ where q and q are respectively the joint position and
V/k −1
By simulation of this differential equation, acceleration, J is the global inertia of the system.
q k , q k , q
k are obtained. Then the Instrument Matrix The torque Γ is calculated through the current
measurement. We have verified that we have
at the step k is computed: Vk = W(q k , q k , q k ) . The
Γ=NKcI, where N is the gear ratio, Kc the torque
equation (14) gives the set of parameters X̂ Vk . constant and I the measured current. More details
This iterative process is conducted until the about the implemented control law can be found in
convergence of the parameters. Practically, 4 or 5 (Khatounian et al, 2006).
steps are enough to decrease dramatically the The friction torque Γf was measured according to
correlation between the instrument matrix and the the method developed by (Spetch and Isermann,
noise on the system. 1988), which consists in measuring the torque at
different constant speeds and eliminating all
transient modes. Figure 3 shows that the friction
3 APPLICATION model can be considered at non-zero velocity as the
following, where sign(q ) is the sign function of the
3.1 Presentation and Modelling of the velocity q , and fc, fv denote the Coulomb and
Interface viscous friction coefficients.
Γ f = f v q + f c sign(q ) (17)
The interface to be identified is presented Figure 1.
It consists of synchronous machine and a handle The dynamic model can be thus written as a
actuated thanks to a cable transmission. This type of sampled linear form:
transmission is a good comprise between friction,
slippage and losses. The modelling and the Γ=WX (18)
identification are made under the rigid model
⎡q1 − gcos(q1 ) gsin(q1 ) q 1 sign(q 1 ) ⎤
assumption. Naturally, the authors have checked that
this hypothesis is valid in the case of this study. W = ⎢⎢ # # # # # ⎥⎥ ,
The modelling and the identification of the ⎢⎣q r − gcos(q r ) gsin(q r ) q r sign(q r )⎥⎦
synchronous machine are given in (Khatounian et al,
⎡Γ1 ⎤
2006).
Y = ⎢⎢ # ⎥⎥ and X = [J M X M Y f v f c ]T .
⎢⎣Γr ⎥⎦
In this simple case, the parameters J, MX, MY, fv
and fc constitute the set of base parameters (QR
decomposition confirms that).
y0
y1
x1
G
q
O x0
Figure 1: Presentation of the interface to be identified. Figure 2: Notations and frames used for modelling the
interface.
344
0.15
Frictions torque measured references and the differential equation given by
(15) is solved with the Euler’s method (ODE 1). The
0.1
step time integrator is of 240μs. Figure 5 shows the
0.05
measured and the simulated states. They are close to
each others.
0
Torque Nm
Table 1: Estimated mechanical parameter values.

-0.05
Mechanical Estimated Relative

parameters values deviation %
-0.1
-0.15 J (kg.m²) 1,45.10-3 0,5

MX (kg.m) 2,2.10-3 5,0
MY (kg.m) 0,9.10-3 8,7
-0.2
-20 -15 -10 -5 0 5 10 15 20
Velocity rads-1
fv (Nm.s.rad-1) 2,4.10-3 2,6

Figure 3: Friction torque measured. fc (Nm) 5,9.10-2 0.8
Cond(W) = 30
3.2 Identification of the Base
Parameters Direct Validation
0.3
3.2.1 LS Method Mesured
Estimated
0.2
Current I and joint position q were measured, with a
sampling period of 240μs. Exciting trajectory 0.1
consists of trapezoidal speed reference. This

trajectory was chosen because it is equivalent to a 0
Nm
rectangular trajectory for the joint acceleration. It

allows the estimation of the inertia term due to the -0.1
change of the acceleration sign, of the gravitational

and viscous friction terms because the velocity is -0.2
constant over a period and of the Coulomb term due -0.3

to the change of sign of the velocity. Vectors q and
0 200 400 600 800 1000 1200 1400 1600 1800
were derived from the position vector q, and all
q
data were filtered as described in section 2.2.2. Figure 4: Comparison between the measured and
estimated torque calculated through the LS technique.
Because of the friction model, the velocities close to
zero are eliminated.
The cut-off frequency of the butterworth filter and 1
Measured and Simulated Position
of the decimate filter is close to 20Hz. This value

0
can be calculated thanks to a spectral analysis of the
rad
-1
data. In our case one has ωdyn close to 2Hz. Thus
with the rule of thumb ωf>10*ωdyn, we retrieve the 1550 1600 1650 1700 1750 1800 1850
Measured and Simulated Velocity
1900 1950 2000
value given above. 10
The identified values of the mechanical parameters

rads-1
are summed up in Table 1. -10
The conditioning number of W is close to 30. 1600 1650 1700 1750 1800 1850 1900 1950 2000
Hence, the system is well conditioned and the Measured and Simulated Acceleration
designed trajectories are enough exciting. 100
Direct comparisons have been performed. These

rads-2
tests consist in comparing the measured and the -100
estimated torques just after the identification 1700 1750 1800 1850 1900 1950 2000
procedure. An example is illustrated Figure 4.

Figure 5: Comparison between the measured states (blue)
Notice that the estimated torque follows the and the simulated states (green).
measured torque closely.
Finally, we simulate the system by means of a
SIMULINK model. We use the same speed
345
It comes that the LS technique identification gives 4 DISCUSSION

interesting and reliable results even if the
assumptions on the noise model are violated. In the case of this study, the experimental results
tend to prove that the identification technique
3.2.2 SRIV Method developed by the IRCCyN robotic team is as
efficient and consistent as the SRIV method. This
As explained section 2.3, the instrument matrix is mainly due to the data treatment described section
built with the simulated states (see previous section 2.2.2. Indeed, it helps the LS estimator: outliers and
for the simulation parameters) and the algorithm is leverage points are eliminated while the derivatives
initialized with the values given in Table 1. The of q are calculated without magnitude and phase
parameters are estimated thanks to (14). These shift. In addition, the data filtering is well designed.
values do not vary consequently (only 2 steps are It is interesting to mix both methods. Indeed, with a
enough to decrease the correlation between the simple analysis, we can detect which parameters are
instrument matrix and the noise on the system). The sensitive to noise measurement, noise modelling,
results are summed up in Table 2. high frequency disturbances…
We have also performed direct comparison (Figure To do so, we introduce an estimation relative error
6). Once again, the estimated torque follows the given by (19):
measured torque closely. As an expected result, the
SRIV method gives reliable and interesting results X̂ SRIVj
and can be used to identify inertial parameters of %e = 100 1 − (19)
robots. The results given Table 2 are close to those X̂ LSj
exposed Table 1. That means that, in this case, the
LS method detailed section 2.2 is as efficient as the Where X̂ SRIVj is the jth parameter estimated by
SRIV technique detailed section 2.3.
means of the SRIV method and X̂ LSj is the jth
Table 2: Estimated mechanical parameter values parameter estimated thanks to the LS technique. The
calculated through the SRIV method after 5 steps. results are summed up in Table 3.
In our case, only the parameters of friction torque
Mechanical Estimated values Relative
are quite sensitive. This is mainly due to the fact that
parameters deviation %
they “see” all undesirable effects such as torque
J (kg.m²) 1,45.10-3 0,4
ripple, cable wear and imperfect contacts which are
MX (kg.m) 2,2.10-3 5,0 not modelled. Hence, that proves the interest of this
MY (kg.m) 0,9.10-3 8,2 approach and the use of the SRIV method.
fv (Nm.s.rad )
-1
1,9.10-3 3,5
fc (Nm) 6,3.10-2 0.8 Table 3: Estimation relative error.
Cond(W) = 30
Parameter %e
Direct Validation
J (kg.m²) 0,1 %
0.3
Mx (kg.m) 0,2 %
Mesured
Estimated My (kg.m) 1,3 %
0.2 fv (Nm.s.rad-1) 20 %
fc (Nm) 7,3 %
0.1
0
During the experiments, it is appeared that the
Nm
SRIV could be helpful when we are not familiar

-0.1 with the data filtering. We know that the LS
estimators are sensitive to noises filtering and if the
-0.2
filtering is not well designed, the results could be
-0.3
controversial. These results will be published in a
later publication.
0 200 400 600 800 1000 1200 1400 1600 1800
Figure 6: Comparison between the measured and

estimated torque calculated through the SRIV.
346
5 CONCLUSION Khatounian F., Moreau S., Monmasson E., Janot A. and

Louveau F., 2006. “Simultaneous Identification of the
Inertial Rotor Position and Electrical Parameters of a
In this paper, two identification methods have been
PMSM for a Haptic Interface”, In: 12th EPE-PEMC
tested and compared: the first based on the ordinary Conference, Portoroz, Slovenia, August September
LS technique associated with an appropriate data 2006, CD-ROM, ISBN 1-4244-0121-6
treatment and the second based on the SRIV method. Lemaire Ch.E., Vandanjon P.O., Gautier M. and Lemaire
Both methods give interesting and reliable results. Ch., 2006. “Dynamic Identification of a Vibratory
Hence, we can choose these techniques for a Asphalt Compactor for Contact Efforts Estimation”,
parametric identification. In: 14th IFAC Symposium on System Identification,
In addition, the authors have introduced a simple SYSID 2006, Newcastle Australia, March 2006,pp.
calculation which enables us to know the parameters 973-978
Mili L., Phaniraj V. and Rousseeuw P.J., 1990. “Robust
which are sensitive to noises and undesirable effects.
estimation theory for bad data diagnostics in electrical
Future works concern the use of both techniques to power systems”, In: C. T. Leondes editor, Control and
identify a 6 degrees of freedom robot. Dynamic Systems:Advances in Theory and
Applications, pp. 271-325, San Diego 1990, Academic
Press
REFERENCES Mili L., Cheniae M.G., Vichare N.S. and Rousseeuw P.J.,
1996. “Robust state estimation based on projection
statistics”, IEEE Trans. On Power Systems, 11(2),
Garnier H., Gilson M. and Huselstein E., 2003. 1118-1127, 1996
“Developments for the MATLAB CONTSID Specht R., and Isermann R., 1988. “On-line identification
toolbox”, In: 13th IFAC Symposium on System of inertia, friction and gravitational forces applied to
Identification, SYSID 2003, Rotterdam Netherlands, an industrial robot”, Proc. IFAC Symp. on Robot
August 2003, pp. 1007-1012 Control, SYROCO’88, 1988, p. 88.1-88.6
Garnier H., Gilson M. and Cervellin O., 2006. “Latest Young P.C. and Jakeman A.J., 1979. Refined instrumental
developments for the MATLAB CONTSID toolbox”, variable methods of time-series analysis: Part 1, SISO
In: 14th IFAC Symposium on System Identification, systems, International Journal of Control, 29:1-30,
SYSID 2006, Newcastle Australia, March 2006, pp. 1979
714-719 Young P.C., 2006. “An instrumental variable approach to
Gautier M. and Khalil W., 1990. "Direct calculation of ARMA model identification and estimation”, In: 14th
minimum set of inertial parameters of serial robots", IFAC Symposium on System Identification, SYSID
IEEE Transactions on Robotics and Automation, Vol. 2006, Newcastle Australia, March 2006, pp. 410-415
6(3), June 1990
Gautier M. and Khalil W., 1991. “Exciting trajectories for
the identification of base inertial parameters of
robots”, In: Proc. Of the 30th Conf. on Decision and
Control, Brigthon, England, December 1991, pp. 494-
499
Gautier M., 1997. “Dynamic identification of robots with
power model”, Proc. IEEE Int. Conf. on Robotics and
Automation, Albuquerque, 1997, p. 1922-1927
Gautier M., Khalil W. and Restrepo P. P., 1995.
“Identification of the dynamic parameters of a closed
loop robot”, Proc. IEEE on Int. Conf. on Robotics and
Automation, Nagoya, may 1995, p. 3045-3050
Gilson M., Garnier H., Young P.C. and Van den Hof P.,
2006. “A refined IV method for closed loop system
identification”, In: 14th IFAC Symposium on System
Identification, SYSID 2006, Newcastle Australia,
March 2006, pp. 903-908
Hampel F.R., 1971. “A general qualitative definition of
robustness”, Annals of Mathematical Statistics, 42,
1887-1896, 1971
Huber P.J., 1981. “Robust statistics”, Wiley, New-York,
1981
Janot A., Bidard C., Gautier M., Keller D. and Perrot Y.,
2006. “Modélisation, Identification d’une Interface
Médicale”, Journées Identification et Modélisation
Expérimentale 2006, Poitiers, November 2006
347
SPECIAL SESSION ON
INTELLIGENT VEHICLE
CONTROL SYSTEMS
CHAIR:
OLEG GUSIKHIN
VEHICLE MODELS AND ESTIMATION OF CONTACT FORCES
AND TIRE ROAD FRICTION
Nacer K. M’Sirdi
LSIS, CNRS UMR 6168. Dom. Univ. St Jrme, Av. Escadrille Normandie - Niemen 13397, Marseille Cedex 20, France
[email protected]; www.nkms.free.fr
Abdelhamid Rabhi
C.R.E.A, 7 Rue du Moulin Neuf, 80000 Amiens, France
Aziz Naamane
LSIS, CNRS UMR 6168. Dom. Univ. St Jrme, Av. Escadrille Normandie - Niemen 13397, Marseille Cedex 20, France
Keywords: Vehicle dynamics, Sliding Modes observer, Robust nonlinear observers, tire forces estimation.
Abstract: In this paper a 16 DoF vehicle model is presented and discussed. Then some partial models are considered
and justified for the design of robust estimators using sliding mode approach in order to identify the tire-
road friction or input variables. The estimations produced are based on split system equations in as cascaded
observers and estimators. The first produces estimations of vehicle states.
1 INTRODUCTION son). The knowledge of tire parameters and variables

(forces, velocities, wheel and slip), tire forces is es-
In recent years, the increasing demand for the safety sential to advanced vehicle control systems such as
in car vehicles has promoted research and develop- anti-lock braking systems (ABS), traction control sys-
ment of the technology of active safety. However tems (TCS) and electronic stability program (Acker-
more and more new active safety systems are devel- mann)(Msirdi04)(Canudas03). Recently, many ana-
oped and installed on vehicles for real-time monitor- lytical and experimental studies have been performed
ing and observers for controlling the dynamic stability on estimation of the frictions and contact forces be-
(EBS, ABS, ESP). tween tires and road.
Car accidents occur for several reasons which may
We focus our work, as presented in this paper, first
involve the driver or vehicle components or environ-
on modeling and second on on-line estimation of the
ment. Such situations appears when the vehicle is
tires forces (Ackermann)(Msirdi04). We estimate the
driven beyond the adherence or stability limits. One
vehicle state and identify tire forces ((Msirdi03)). The
of the important factors determining vehicle dynam-
main contribution is the emphasize of the rational be-
ics including safety is road friction and the tire forces
hind partial approximated models and the on-line es-
(ground-vehicle interactions). In general partial and
timation of the tire force needed for control.
approximated models are used. They are not fully jus-
tified and their validity is often limited. In this work Tire forces can be represented by the nonlinear
we try to highlight some of the approximations made (stochastic) functions of wheel slip. The determin-
and give some details allowing to evaluate what is re- istic tire models encountered are complex and de-
ally neglected. pend on several factors (as load, tire pressure, en-
Robust observers looking forward are based on vironmental characteristics, etc.). This makes on
the physics of interacting systems (the vehicle, the line estimation of forces and parameters difficult for
driver and the road). However, tire forces and road vehicle control applications and detection and diag-
friction are difficult to measure directly and to repre- nosis for driving monitoring and surveillance. In
sent precisely by some deterministic model equations. (Drakunov)(Canudas03), application of sliding mode
In the literature, their values are often deduced by control is proposed. Observers based on the sliding
some experimentally approximated models (Gustafs- mode approach have been also used in (Rabhi04).
351
In (Ray) an estimation method is based on the least

squares algorithm and combined with a Kalman filter
to estimate the contact forces. The paper of (Gustafs-
son) presents an estimation of tire/road frictions by
means of a Kalman filter. It gives relevant estimates
of the slope of µ versus slip (λ). In (Carlson) esti-
mations of longitudinal stiffness and wheel effective
radius are proposed using vehicle sensors and a GPS
for low values of the slip.
Robust observers with unknown inputs have been Figure 1: Vehicle dynamics and reference frames.
shown to be efficient for estimation of road pro-
file (Imine) and for estimation of the contact forces
(Msirdi04)(Rabhi04). Tracking and braking control 1 and define the following notations.
reduce wheel slip. This can be done also by means of The vehicle body receives as excitations external
its regulation while using sliding mode approach for forces and moments following the three axes: - Lon-
observation and control.(Msirdi04)(Rabhi04). This gitudinal, - Lateral, - Vertical. These come from in-
enhances the road safety leading better vehicle ad- teraction of the wheels and road, from perturbations
herence and maneuvers ability. The vehicle control- (wind for example), gravity and vehicle drive line. Let
lability in its environment along the road admissible us consider the basic reference fixed frame R. We can
trajectories remain an important open research prob- consider the vehicle as made of 5 sub-systems: chas-
lem. sis whit 6 DoF and then 4 wheels with their suspen-
The proposed estimation procedure has to be ro- sions. Each of the rear wheels has 2 DoF. The front
bust enough to avoid model complexity. It can then ones are driven wheels with 3 DoF each. Then we
be used to detect some critical driving situations in have 16 DoF. Let the generalized variables be in the
order to improve the security. This approach can be vector q ∈ R16 , defined as
used also in several vehicle control systems such as qT = [x, y, z, θz , θy , θx , z1 , z2 , z3 , z4 , δ3 , δ4 , ϕ1 , ϕ2 , ϕ3 , ϕ4 ]
Anti-look Brake Systems (ABS), traction control sys-
tem (TCS), diagnosis systems, etc... The main char- where x, y, et z represent displacements in longitu-
acteristics of the vehicle longitudinal dynamics were dinal, lateral and vertical direction. angles of roll,
taken into account in the developed model used to de- pitch and yaw are θx , θy et θz respectively. The
sign robust observer and estimations. The estimations suspensions elongations are noted zi : (i = 1..4). δi :
are produced using only the angular wheel position stands for the steering angles (for wheels numbered
as measurement by the specially designed robust ob- as i = 3, 4), finally ϕi : are angles wheels rotations
server based on the super-twisting second-order slid- (i = 1..4.). Vectors q̇, q̈ ∈ R16 are respectively veloci-
ing mode. The proposed estimation method is veri- ties and corresponding accelerations. M(q) is the in-
fied through simulation of one- wheel model (with a ertia matrix and C(q, q̇)q̇ are coriolis and centrifugal
”Magic formula” as tire model). In a second step of forces. The gravity term is G . Suspensions forces
validation we present some application results (on a are V (q, q̇) = Kv q̇ + K p q with respectively damping
Peugeot 406) showing an excellent reconstruction of and stiffness matrices Kv , K p . We can define as dy-
the velocities, tire forces and estimation of wheel ra- namic equations of the vehicle by applying the prin-
dius. ciples fundamental of the dynamics (see (Beurier)):
.. . .
Γ + J T F = M q +C(q, q)q + Kq + G (1)
2 VEHICLE MODELING with as parameters only to give an idea

M̄1,1 M̄1,2 M̄1,3 0 0
 
2.1 Complete 16 DoF Model  M̄2,1 M̄2,2 M̄2,3 M̄2,4 M̄2,5 
M =  M̄3,1 M̄3,2 M̄3,3 0 0
 

In literature, many studies deal with vehicle model-  0 M̄4,2 0 M̄4,4 0 
ing (Kien)(Ramirez)(Mendoza). This kind of systems 0 M̄5,2 0 0 M̄5,5
are complex and nonlinear composed with many cou- 
0 C̄12 C̄13 0 0

pled subsystems: wheels, motor and system of brak-  0 C̄22 C̄23 C̄24 C̄25 
ing, suspensions, steering, more and more inboard C =  0 C̄32 C̄33 0

0 

and imbedded electronics. Let us represent the ve-  0 C̄
42 0 0 0 
hicle (like eg a Peugeot 406) by the scheme of figure 0 C̄52 0 0 C̄55
352
J1,1 J1,2 J1,3 J1,4 J7F

     ..   .. 
q1 q4
 J2,1 J2,2 J2,3 J2,4   J8F  .. ..
0   = M̄1,3  q.. 2  + M̄2,3  q.. 5 +
 J J J
   J
and J =  3,1 3,2 3,3
T
. 9F
q3 q6
 0 0 0 J4,4  J10F
 0 .. .
0 0 0  
q11
  .  
q11

0 0 0 0
.. q4 .
 q  . q21 
+M̄3,3  .. 21  + C̄32  q5  + C̄33  +
 .
This is just to show that we can decompose our q31 . q31 
.. q6 .
system as coupled subsystems. Let us say five cou- q41 q41
pled subsystems, that we have considered in our pre- 
R11

q11
 
G7

vious works. This has been computed using a sym- R21   q21   G8 
+  q + (4)

bolic computation software considering 16 general- R31 31 G9 
ized variables: 6 for position and orientation of body, R41 q41 G10
4 as suspensions ones, 2 for front wheels steering and
The previous 16 DoF model is then equivalent to:
4 as wheels rotations. The matrices M, C and K are
of dimensions 16 × 16. F is input forces vector acting  ..   . 

FT
 ξ1 ξ1 
0
 
G1

on wheels, it has 12 components(3 forces ( longitu-  ..   . 
F2  ξ2  ξ2 0 G2
dinal, lateral and normal) × 4 wheels), Γ represent
       
   ..   .     
 F3 = M  ξ  +C  ξ + R33 .ξ3 + G3 
extra inputs for perturbations.. In the following ap- 
 0


 .. 3



 .3

 
  0
 
  0


 ξ4  ξ4
plication this model has been reduced and simplified U5 ..

.

0 0
assuming as nominal behavior a normal driving situ- ξ5 ξ5
ation (Msirdi03). In the last expression, we can remark that splitting
the model can be realized and this model is helpful,
2.2 Coupled Sub Models when using reduced models, to identify what is ne-
glected regard to our proposed nominal model with
16 DoF. The dynamic equations can be reduced, in
We can split the previous model, without approxima- case where we assume that motion is normal driving
tions, in five parts as follows. This leads us to the in a normal staith road, to translations and rotations
body’s translations dynamics: of the body, and wheels plus suspension motions. For
   ..   ..  translations we find often in literature:
FLT x q4
..
 FT T  = M̄11  y  +M̄ 12  q.. 5  + (2) .
mvx = ∑ Fx
.. .. .
FNT z q6 mvy = ∑ Fy (5)
.
 ..  mvz = ∑ Fz
q11  . 
.. q4
 q.. 21  . where m is the total mass of the vehicle and v =
+M̄ 13  +C̄12  q5  (3)
q31 . [vx , vy , vz ]T describe the vehicle velocities along x, y,

.. q6
q41 z. In the left hand side of this approximate model
are the forces ∑ Fx , ∑ Fy and ∑ Fz applied in direc-
Rotations and orientation motions of the body: tions of x, y and z and the balance of the moments
   ..   ..  (∑ Mx , ∑ My , ∑ Mz ), give rotations following the three
J4F q1 q4
 J5F  = M̄ T  q.. 2  + M̄22  q.. 5  + directions x, y and z, is given by:
12 .. ..
 ..   
J6F q3 q6 θ ∑ M x
..  .. 

q11
  ..   ..  J  φ  =  ∑ My  (6)
.. q12 q42 ..
∑ M
 q  .. .. ψ z
+M̄23  .. 21  + M̄24  q22  + M̄25  q33  +
q31 .. ..
.. q32 q43 The wheel angular motions can be written:
q41
 .  . 1
 .  q11 ωfl = (C f l − Rω Fx f 1 ) (7)
q4 .
Iω
. q 
+C̄22  q5  + C̄23  . 21  +

. q31 . 1
q6 . ωfr = (C f r − Rω Fx f 2 )
 .   q41.  Iω
q12 q42 . 1
. .
+C̄24  q22  + C̄25  q33  ωrl = (Crl − Rω Fxr1 )
. . Iω
q32 q43 . 1
Suspensions dynamics: ωrr = (Crr − Rω Fxr2 )
Iω
353
with ω f and ωr are the rotation velocities of the front

and rear wheel, Cmi is the motor couple applied at
wheel i and Ti is the braking couple applied at wheel
i. Let r1 be the distance between the center of grav-
ity and the front axis and r2 the distance between the
center of gravity and the rear axis.
2.3 Partial Models
The complete model is difficult to use in control ap-

plications. It involves several variables which are not
available for measurement or not observable. The Figure 2: Wheel slip.
most part of applications deal with simplified and par-
tial models. Let us consider, for our robust observer,
the simplified motion dynamics of a quarter-vehicle different road surface conditions. It can be observed
model, capturing only nominal behavior (Msirdi04) that all curves µ(λ) start at µ = 0 for zero slip, which
(Msirdi03). This model retains the main characteris- corresponds to the non-braking and non accelerating,
tics useful for the longitudinal dynamic. For a global free rolling wheel. With a linear increasing slip ratio
application, this method can be easily extended to the from 3% to 20%. Beyond this maximum value the
complete vehicle and involve the four coupled wheels. slope of the adhesion characteristic is maximum and
The amount of neglected parts in the modeling can be then slope becomes negative. At a slip ratio of 100%
considered to evaluate robustness of proposed estima- the wheel is completely skidding, which corresponds
tors. to the locking of the wheel. The adhesion character-
Applying Newton’s law to one isolated wheel istic plays an essential role for both the design and
gives:. the validation of ABS. Overall, to improve the perfor-
. mance of an ABS it is desirable to have some real-
mvx = Fx
. time information about the adhesion characteristic.
Jr ω = T − rFx By assuming that the longitudinal forces are pro-
portional to the transversal ones, we can expressed
where m is the vehicle mass and Jr , r are the in- theses forces as follows, where Fz is the vertical force
ertia and effective radius of the tire respectively. vs of the wheel.
is the linear velocity of the vehicle, ω is the angular
velocity of the considered wheel. T is the accelerat- Fx = µFz (9)
ing (or braking) torque, and Fx is the tire/road friction
force. The tractive (respectively braking) force, pro- The vertical forces that we use in our model
duced at the tire/road interface when a driving (brak- are function of the longitudinal acceleration and the
ing) torque is applied to pneumatic tire, has oppo- height of the center of gravity. The vertical force can
site direction of relative motion between the tire and be represented as:
road surface. This relative motion exhibits the tire slip
m .
properties. The wheel-slip is due to deflection in the Fz = (glr − hvx ) (10)
contact patch. The longitudinal wheel slip is gener- 2(l f + lr )
ally called the slip ratio and is described by a kine-
where h is the height of the center of gravity, l f
matic relation as (Carlson).
is the distance between the center of gravity and the
|vr − vx | front axis center of gravity and lr is the distance be-
λ= (8) tween the center of gravity and the rear axis center of
max(vr , vx ) gravity.
where vr is the wheel velocity. Representing the
adhesion coefficient as a function of the wheel slip
yields the adhesion characteristic µ(λ), which de- 3 OBSERVER DESIGN
pends on the road surfaces as shown in the following
figure 2. The sliding mode technique is an attractive approach
The figure 2 shows the relations between coeffi- (Davila). The primary characteristic of SMC is that
cient of road adhesion µ and longitudinal slip λ for the feedback signal is discontinuous, switching on
354
one or several manifolds in the state-space. In what complete modeling equations. For vehicle systems it
follows, we use a second order differentiator in order is very hard to build up a complete and appropriate
to derive robust estimates of the tire road friction. model for global observation of all the system states
in one step. Thus in our work, we avoid this prob-
3.1 High Order Sliding Mode Observer lem by means of use of simple and cascaded models
(HOSM) suitable for robust observers design.
The first step produces estimations of velocities.
In this part we will use a Robust Differentiation The second one estimate the tire forces (vertical and
Estimator (RDE) to deduce our robust estimations. longitudinal ones) and the last step reconstruct the
Consider a smooth dynamics function, s(x) ∈ R. friction coefficient.
The system containing this variable may be closed The robust differentiation observer is used for es-
by some possibly-dynamical discontinuous feedback timation of the velocities and accelerations of the
where the control task may be to keep the output wheels. The wheels angular positions and the veloc-
s(x(t)) = 0. Then provided that successive total time ity of the vehicles body vx , are assumed available for
. .. measurements. The previous Robust Estimator is use-
derivatives s, s, s...s(r−1) are continuous functions of
ful for retrieval of the velocities and accelerations.
the closed system state space variables, and the t-
1st Step:
sliding point set is non-empty and consist locally of
. 2
Filippov trajectories.
3
θ = v0 = ω
b b − λ0 θ − b
θ sign(θ − bθ)
. ..
s = s = s = ... = s(r−1) = 0 (11) . . 1
b − λ1 sign(ω
b = v1 = ω
ω b − v0 ) 2 sign(ω
b − v0 )
is non-empty and consists locally of Filippov tra- .. .
jectories. The motion on set (Filippov)(Utkin99) b = −λ2 sign(ω
ω b − v1 )
is called r-sliding mode (rth -order sliding mode)
(Orlov)(Levant).
The convergence of these estimates is guaranteed
The HOSM dynamics converge toward the origin
in finite time t0 .
of surface coordinates in finite time always that the or-
2nd Step: In the second step we can estimate the
der of the sliding controller is equal or bigger than the
forces Fx and Fz . Then to estimate Fx we use the fol-
sum of a relative degree of the plant and the actuator.
lowing equation,
To estimate the derivatives s1 and s2 without its direct .
calculations of derivatives, we will use the 2nd -order b = T − Re f Fbx
Jω (12)
exact robust differentiator of the form (Levant)
. 2
z0 = v0 = z1 − λ0 |z0 − sω | 3 sign(z0 − sω ) In the simplest way, assuming the input torques
. 1 known, we can reconstruct Fx as follows:
z1 = v1 = −λ1 sign(z1 − v0 ) sign(z1 − v0 ) + z2
2
. .
z2 = −λ2 sign(z2 − v1 ) (T − J ωb)
Fbx = (13)
Re f
where z0 , z1 and z2 are the estimate of sω , s1 and .
s2 , respectively, λi > 0, i = 0, 1, 2. Under condition b is produced by the Robust Estimator (RE). Note
ω
λ0 > λ1 > λ2 the third order sliding mode motion will that any estimator with output error can also be used
be established in a finite time. The obtained estimates to enhance robustness versus noise. In our work, in
. ..
are z1 = s1 = sω and z2 = s2 = sω then they can be progress actually, the torque T will be also estimated
used in the estimation of the state variables and also by means of use of additional equation from engine
in the control. behavior related to accelerating inputs.
After those estimations, their use in the same time
3.2 Cascaded Observers - Estimators with the system equations allow us to retrieve de ver-
tical forces Fz as follows. To estimate Fz we use the
In this section we use the previous approach to build following equation
m .
an estimation scheme allowing to identify the tire
Fbz = (glr − h.vbx ) (14)
road friction. The estimations will be produced in 2(l f + lr )
three steps, as cascaded observers and estimator, re-
construction of information and system states step by vbx is produced by the RE.
step. This approach allow us to avoid the observ- 3rd Step: At this step it only remains to estimate
ability problems dealing with inappropriate use of the the adherence or friction coefficient. To this end we
355
assume the vehicle rolling in a normal or steady state

situation in order to be able to approximate this coef-
ficient by the following formula
Fbx
µ=
b (15)
Fbz
4 SIMULATION AND Figure 5: Angular Velocity.

EXPERIMENTAL RESULTS
In this section, we give some realistic simulation re-
sults in order to test and validate our approach and
the proposed observer. In simulation, the state and
forces are generated by use of a car simulator called
VeDyna (VEDYNA). In this simulator the model in-
volved is more complex than the one of 16 DoF pre-
sented in the first part of the paper. Comparind the
simplified model to the 16 DoF one, let us evaluate Figure 6: Vehicle velocity.
the robustness of estimation The VeDyna simulated
brake torque is shown in figure 3.
observer allows a good estimation of angular velocity
and acceleration. The last step gives us the estimated
Figure 3: Braking torque.
Figure 4 shows the measured and estimated wheel Figure 7: Estimated and measured acceleration.
angular position. This signal is used to estimate ve-
locities and accelerations. Figure 4 shows the esti-
longitudinal forces Fx and normal forces Fz which are
presented in figure 8 and 9. Finally road friction
coefficient is deduced and presented in figure (10).
5 CONCLUSION
In this work we have tried to highlight all approxima-
tions made in general when using simplified models
and this paper gives some details allowing to evaluate
Figure 4: Angular displacements.
what is really neglected. In second part od this paper,
we have proposed an efficient and robust estimator
mated wheel angles. In the figure 6, we represent baser on the second order sliding mode differentiator.
the estimation of vehicle velocity. The figure shows This is used to build an estimation scheme allowing to
the good convergence to the actual vehicle velocity. identify the tire road frictions and input forces which
Figure 7 shows the obtained vehicle acceleration. The are non observable when using the complete model
356
Figure 8: Longitudinal force. Figure 10: Road friction.
REFERENCES
F. Gustafsson, ”Slip-based tire-road friction estimation”,
Automatica, vol 33, no. 6, pp. 1087-1097, 1997.
J. Ackermann ”Robust control prevents car skidding. IEEE
Control systems magazine, V17, N3, pp23-31, 1997
N. K. M’sirdi, A. Rabhi, N. Zbiri and Y. Delanne. VRIM:
Vehicle Road Interaction Modelling for Estimation of
Figure 9: Normal force Fz . Contact Forces. Accepted for TMVDA 04. 3rd Int.
Tyre Colloquium Tyre Models For Vehicle Dynamics
Analysis August 30-31, 2004 University of Technol-
ogy Vienna, Austria
and standard sensors. The estimations produced fi-
C. Canudas de Wit, P.Tsiotras, E.Velenis, M.Basset,
nite time converging measurements of model inputs, G.Gissinger. Dynamic Friction Models for Road/Tire
in three steps by cascaded observers and estimators. Longitudinal Interaction. Vehicle Syst. Dynamics
This method shows very good performances in simu- 2003. V39, N3, pp 189-226.
lations conducted using a more complex model (than Laura Ray. Nonlinear state and tire force estimation for ad-
the 16 DoF one) involved in VeDyna car simulator. vanced vehicle control. IEEE T on control systems
Tire forces (vertical and longitudinal ones) are also technology, V3, N1, pp117-124, march 1995.
estimated correctly. Simulation results are presented Nacer K. M’Sirdi. Observateurs robustes et estimateurs
to illustrate the ability of this approach to give es- pour l’estimation de la dynamique des véhicules et du
timation of both vehicle states and tire forces. The contact pneu - route. JAA. Bordeaux, 5-6 Nov 2003
robustness versus uncertainties on model parameters S. Drakunov, U. Ozguner, P. Dix and B. Ashrafi. ABS con-
and neglected dynamics has also been emphasized in trol using optimum search via sliding modes. IEEE
simulations. Application of this approach with inclu- Trans. Control Systems Technology, V 3, pp 79-85,
sion of torque estimation using a simplified model for March 1995.
the engine behavior, is in progress. A. Rabhi, H. Imine, N. M’ Sirdi and Y. Delanne. Observers
With Unknown Inputs to Estimate Contact Forces and
Road Profile AVCS’04 International Conference on
Advances in Vehicle Control and Safety Genova -
ACKNOWLEDGEMENTS Italy, October 28-31 2004
Christopher R. Carlson. Estimation With Applications for
This work has been done in a collaboration managed Automobile Dead Reckoning and Control. PhD the-
by members of the LSIS inside the GTAA (research sis, University of STANDFOR 2003.
group supported by the CNRS).
U. Kiencken, L. Nielsen. Automotive Control Systems.
Thanks are addressed to the LCPC of Nantes for
Springer, Berlin, 2000.
experimental data and the trials with their vehicle
Peugeot 406. Mendoza, Sur la modèlisation et la commande des
véhicules automobiles, Thèse, Juillet 1997.
H. Imine, N. M’Sirdi, L. Laval, Y. Delanne - Sliding Mode
Observers for Systems with Unknown Inputs: Appli-
cation to estimate the Road Profile. A paraı̂tre dans
ASME, Journal of Dynamic Systems, Measurement
and Control en mars 2003.
357
M7,7 0 0 0
 
R. Ramirez-Mendoza. Sur la modélisation et la commande
des véhicules automobiles. PHD thesis Institut Na- 0 M8,8 0 0
M̄3,3 =  ;
 
tional Polytechnique de Grenoble, Laboratoire d Au- 0 0 M9,9 0
tomatique de Grenoble 1997. 0 0 0 M10,10
 
G. Beurier. Modélisation et la commande de système. PHD M11,11 0 0
thesis LRP. UPMC Paris 6, 1998. M̄4,4 =  0 M12,12 0 
J. Davila and L. Fridman. “Observation and Identifica- 0 0 M13,13
tion of Mechanical Systems via Second Order Slid-
 
M14,14 0 0
ing Modes”, 8th. International Workshop on Variable
Structure Systems,September 2004, Espana M̄5,5 =  0 M15,15 0 
0 0 M16,16
A. Levant, ”Robust exact differentiation via sliding mode
technique”, Automatica, vol. 34(3), 1998, pp 379-384.
A. F. Filippov, Differential Equations with Discontin-
uous Right-hand Sides, Dordrecht, The Nether-
lands:Kluwer Academic Publishers; 1988.
V. Utkin, J. Guldner, J. Shi, Sliding Mode Control in Elec-
tromechanical Systems, London, UK:Taylor & Fran-
cis; 1999.
J. Alvarez, Y. Orlov, and L. Acho, ”An invariance principle
for discontinuous dynamic systems with application
to a coulomb friction oscillator”, Journal of Dynamic
Systems, Measurement, and Control, vol. 122, 2000,
pp 687-690.
Levant, A. Higher-order sliding modes, differentiation and
output-feedback control, International Journal of Con-
trol, 2003, Vol.76, pp.924-941
Simulator VE-DYNA. www.tesis.de/index.php.
APPENDIX
Definition
 of the matrices involved.in
 the model.
M1,1 0 0
M̄11 =  0 M2,2 0 
0 0 M3,3
 
M1,4 M1,5 M1,6
T = M
M̄12 = M̄21 2,4 M3,5 M2,6  ;
0 M3,5 M3,6
 
M1,7 M1,8 M1,9 M1,10
T = M
M̄13 = M̄31 2,7 M2,8 M2,9 M2,10  ;
M3,7 M3,8 M3,9 M3,10
 
M4,7 M4,8 M4,9 M4,10
t = M
M̄23 = M̄32 5,7 M5,8 M5,9 M5,10 
M6,7 M6,8 M6,9 M6,10
 
M4,11 M4,12 M4,13
T = M
M̄24 = M̄42 5,11 M5,12 M5,13 
0 0 0
 
M4,14 M4,15 M4,16
T  M
M̄2,5 = M̄52 5,14 M5,15 M5,16 
0 M6,15 M6,16
 
M4,4 M4,5 M4,6
M̄2,2 =  M5,4 M5,5 M5,6  ;
M6,4 M6,5 M6,6
358
BENCHMARKING HAAR AND HISTOGRAMS OF ORIENTED
GRADIENTS FEATURES APPLIED TO VEHICLE DETECTION
Pablo Negri, Xavier Clady and Lionel Prevost

Université Pierre et Marie Curie-Paris 6, ISIR, CNRS FRE 2507
3 rue Galilée, 94200 Ivry sur Seine, France
pablo . negri [a] lisif.jussieu.fr
Keywords: Intelligent vehicle, vehicle detection, AdaBoost, Haar filter, Histogram of oriented gradient.
Abstract: This paper provides a comparison between two of the most used visual descriptors (image features) nowadays
in the field of object detection. The investigated image features involved the Haar filters and the Histogram of
Oriented Gradients (HoG) applied for the on road vehicle detection. Tests are very encouraging with a average
detection of 96% on realistic on-road vehicle images.
1 INTRODUCTION edges (Srinivasa, 2002), regions, deformable patterns

(Collado et al., 2004) and rigid patterns (Yang et al.,
On road vehicle detection is an essential part of the In- 2001). Appearance-based methods learn the charac-
telligent Vehicles Systems and has many applications teristics of the classes vehicle and non-vehicle from
including platooning (i.e. vehicles travelling in high a set of training images. Each training image is rep-
speed and close distances in highways), Stop&Go resented by a set of local or global descriptors (fea-
(similar that precedent situation, but at low speeds), tures) (Agarwal et al., 2004). Then, classification al-
and autonomous driving. gorithms can estimate the decision boundary between
Most of the detecting methods distinguish two ba- the two classes.
sic steps: Hypothesis Generation (HG) and Hypoth- One of the drawbacks of optical sensors are the
esis Verification (HV) (Sun et al., 2006). HG ap- considerable time processing and the average robust-
proaches are simple low level algorithm used to lo- ness. In that way, Viola & Jones (Viola and Jones,
cate potential vehicle locations and can be classified 2001) developed simple an appearance-based system
in three categories: obtaining amazing results in real time. Their appear-
- knowledge-based: symmetry (Bensrhair et al., ance based method uses Haar-based representation,
2001), colour (Xiong and Debrunner, 2004; Guo combined with an AdaBoost algorithm (Freund and
et al., 2000), shadows (van Leeuwen and Groen, Schapire, 1996). They also introduce the concept of
2001), edges (Dellaert, 1997), corners (Bertozzi a cascade of classifiers which reaches high detection
et al., 1997), texture (Bucher et al., 2003), etc., results while reducing computation time.
The present article compares the Haar-based fea-
- stereo-based: disparity map (Franke, 2000), inverse tures with the Histograms of Oriented Gradient (HoG)
perspective mapping (Bertozzi and Broggi, 1997), based features using the same cascade architecture.
etc, The next section describes briefly the Haar and
- and motion-based (Demonceaux et al., 2004). the HoG features. Section two introduces the learn-
HV approaches perform the validation of the Re- ing classification algorithms based on AdaBoost. We
gions of Interest generated by the HG step. They can finish the article with the results and conclusions.
be classified in two categories: template-based and
appearance-based. Template-based methods perform
a correlation between a predefined pattern of the vehi-
cle class and the input image: horizontal and vertical
359
Figure 1: 2D Wavelet set.
2 FEATURES
The common reasons why features are choosen in-

stead of pixels values are that features can code high
level object information (segments, texture, ...) while
intensity pixel values based system operates slower
than a feature based system. This section describes
the features used to train the Adaboost cascade.
Figure 2: 2D Haar Wavelet example on a vehicle image.
2.1 Haar Filters
2.2 Histogram of Oriented Gradient
Each wavelet coefficient describes the relationship
between the average intensities of two neigh-boring The Histograms of Oriented Gradient (HoG) is an-
regions. Papageorgiou et al. (Papageorgiou and other way to encode an input image to obtain a vec-
Poggio, 1999) employed an over-complete set of 2D tor of visual descriptors. This local descriptor, based
wavelets to detect vehicles in static images. on Scale Invariant Feature Transform (SIFT) (Lowe,
Figure 1 shows basic Haar filters: two, three and 1999), uses the gradient magnitude and orientation
four rectangle features, where the sum of the pixels around a keypoint location to construct an histogram.
which lie within the white rectangles are subtracted Orientations are quantized by the number of bins in
from the sum of pixels in the grey rectangles. We con- the histogram (four orientations are sufficient). For
serve the two and three rectangle features since vehi- each histogram bin, we compute the sum in the re-
cles have rectangular shape: diagonal features (four gion of all the magnitudes having that particular ori-
rectangle template) doesn’t give extra information for entation. The histogram values are then normalised
this type of pattern. by the total energy of all orientations to obtain values
Viola & Jones (Viola and Jones, 2001) have intro- between 0 and 1.
duced the Integral Image, an intermediate representa- Gepperth (Gepperth et al., 2005) train a neural net-
tion for the input image. The sum of the rectangular work classifier using these features for a two class
region in the image can be calculated in four Integral problem: vehicle, non-vehicle. First, a ROI is sub-
Image references. Then, the difference between two divided into a fixed number of regions called recep-
adjacent rectangles, can be computed with only six tive fields. From each receptive field, they obtain an
references, eight in the case of the three rectangle fea- oriented histogram feature.
ture. The HoG features set is composed of histograms
The Haar feature set is composed of the resulting calculated inside a rectangular region on the original
value of the rectangular filters at various scales in a image. We evaluate the the gradient of the image us-
image. ing the Sobel filters to obtain the gradient magnitude
In figure 2 we can see the results of two rectan- and orientation.
gular filters (vertical and horizontal) at two scales: There are three types of rectangle regions: r1
2x2 and 4x4 pixels. Lightness pixels mean important square l*l, r2 vertical rectangle l*2l, r3 horizontal
subtraction values (the result is always calculated in rectangle 2l*l. Considering l : {2, 4, 8, 16} scales, we
modulus). The complete set of Haar’s features uti- have a total of 4678 features. A single histogram j in
lizing the three rectangular filters (see fig. 1) in a the set could be defined as: h j = (x j , y j , s j , r j ), where
32x32 pixel image at {2,4,8,16} scales is 11378. Ev- r j is the rectangular filter type, s j the scale and (x j , y j )
ery single feature j in the set could be defined as: are the position over the 32x32 image.
f j = (x j , y j , s j , r j ), where r j is the rectangular filter
type, s j the scale and (x j , y j ) are the position over the
32x32 image.
360
BENCHMARKING HAAR AND HISTOGRAMS OF ORIENTED GRADIENTS FEATURES APPLIED TO VEHICLE
DETECTION
3.1 Haar Weak Classifier

We define the weak classifier as a binary function g:

1 if p j f j < p j θ j
g (2)
0 otherwise
where f j is the feature value, θ j the feature threshold
and p j the threshold parity.
3.2 Hog Weak Classifier

This time, instead of evaluate a feature value, we es-
timate the distance between an histogram h j of the
input image and a model histogram m j . The model
is calculated like the mean histogram between all the
training positive examples. For each histogram h j of
Figure 3: HoG example on a vehicle image.
the feature set, we have the corresponding m j . A ve-
hicle model is then constructed and AdaBoost will
found the most representative m j which best separate
3 ADABOOST the vehicle class from the non-vehicle class.
We define the weak classifier like a function g:
As we saw in previous sections, Haar and HoG rep-
resentations are used to obtain a vector of visual de- 1 if d(h j , m j ) > θ j
g (3)
scriptors describing an image. The size of these vec- 0 otherwise
tors is clearly bigger than the number of pixel in the where d(h j (x), m j ) is the Bhattacharyya distance
image. Using the total number of features to carry (Cha and Srihari, 2002) between the feature h j and m j
out a classification is inadequate from the computing and θ j is the distance feature threshold.
time point of view of the and the robustness, since
many of these features do not contain important infor-
mation (noise). Different methods: statistics (Schnei-
derman and Kanade, 2000), PCA, genetic algorithms 4 TEST AND RESULTS
(Sun et al., 2004), etc. can be used to select a limited
quantity of representative features. 4.1 Dataset
Among these methods, the Boosting (Freund and
Schapire, 1996) classification method improves the The images used in our experiments were collected
performance of any algorithm. It finds precise hy- in France using a prototype vehicle. To ensure data
pothesis by combining several weak classifiers which, variety, 557 images where captured during different
on average, have a moderate precision. The weak time, and on different highways.
classifiers are then combined to create a strong clas- The training set contains 745 vehicle sub-images
sifier: of typical cars, sport-utility vehicles (SUV) and mini-
van types. We duplicate this quantity flipping the sub-
images around y-axis, obtaining 1490 examples. We
G=
1 ∑Nn=1 αn gn ≥ 21 ∑Nn=1 αn = T (1) split this new set keeping 1000 of the examples for
0 otherwise
training and the others for validation: the training set
Where G and g are the strong and weak classifiers (TS) contains 1000 sub-images aligned to a resolution
respectively, and α is a coefficient wheighting each of 32 by 32 pixels, the validation set (VS) contains
feature result. T is the strong classifier threshold. 490 vehicle sub-images with the same resolution. The
Different variants of boosting are known such as negative examples come from 3196 images without
Discrete Adaboost (Viola and Jones, 2001), Real Ad- vehicles.
aBoost (Friedman et al., 2000), Gentle AdaBoost, etc. The test set contains 200 vehicles in 81 images.
The procedures (pseudo-code) of any of this variants
are widely developed in the literature. 4.2 Single Stage Detector
We need, however, to study the construction of the
weak classifier for both cases: Haar and HoG fea- First experiments were carried out with a strong clas-
tures. sifier constructed with 100, 150 and 200 Haar or HoG
361
features using the Discrete Adaboost algorithm (Viola Table 1: Single stage detection rates (Haar and HoG classi-
and Jones, 2001). fiers).
We used the TS for the positive examples. The Classifier DR (%) FP Time
non-vehicle (negatives) examples were collected by HoG - 100 fts 69.0 1289 3,52
selecting randomly 5000 sub-windows from a set of HoG - 150 fts 72.5 1218 4,20
250 non-vehicle images at different scales. HoG - 200 fts 83.1 1228 5,02
To evaluate the performance of the classifiers, the
average detection rate (DR) and the number of false Haar - 100 fts 96.5 1443 2,61
positives (FP) were recorded using a three-fold cross Haar - 150 fts 95.7 1278 3,93
validation procedure. Specifically, we obtain three Haar - 200 fts 95.8 1062 5,25
sets of non-vehicle sub-windows to train three strong
1
classifiers. Then, we test these classifiers on the test
set.
0.99
4.3 Multi Stage Detector 0.98
Haar 100 fts

Haar 150 fts
This section shows the test realised using a cascade of 0.97
AA
Haar 200 fts
HoG 100 fts
strong classifiers (Viola and Jones, 2001). The multi HoG 150 fts
stage detector increases detection accuracy and re- 0.96 HoG 200 fts
duces the computation time. Simpler classifiers (hav-

0.95
ing a reduced number of features) reject the majority
of the false positives before more complex classifiers
0.94
(having more features) are used to reject difficult sub- 0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045
windows. FP
Stages in the cascade are constructed with the Ad- Figure 4: ROC curves for Haar and HoG Single Stage de-
aboost algorithm, training a strong classifier which tectors.
achieves a minimum detection rate (dmin = 0.995) and
a maximum false positive rate ( fmax = 0.40). The
training set is composed of the TS positive examples ble. On the other hand, Haar classifiers are discrimi-
and the non-vehicle images separated in 12 different native classifier evaluating a fronteer between positive
folders (the maximum number of stages). Subsequent and negative samples. Now, the fronteer is refined -
classifiers are trained using those non-vehicle images and the number of false positives decreases - when
of the corresponding folder which pass through all the the number of features increases. Figure 4 presents
previous stages. the ROC curves for each detector. As told before, for
An overall false positive rate is defined to stop the a given detection rate, the number of false positives is
cascade training process (F = 43 ∗ 10−7 ) within the lower for Haar classifiers than for HoG classifiers.
maximum number of stages.
This time, the average accuracy (AA) and false
positives (FP) where calculated using a five-fold cross 1
validation procedure. We obtain five detectors from

five differents TS and VS randomly obtained. 0.95
4.4 Results 0.9

AA
Table 1 shows the detection rate of the single stage Haar − N = 1000
0.85 Haar − N = 2000
detector trained either on Haar features or on HoG Haar − N = 3000
HoG − N = 1000
features with respectively 100, 150 and 200 features. HoG − N = 2000
HoG − N = 3000
These results are very interesting though quite pre- 0.8
dictible. As seen before, HoG classifiers computes

a distance from the test sample to a ”vehicle model” 0.75
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5
(the mean histograms). These are generating classi- FP x 10
−3
fiers. When the number of considered features in-

Figure 5: ROC curves for Haar and HoG Multi Stage detec-
creases, the model is refined and the detection rate in- tors.
creases while the number of false positives keeps sta-
362
BENCHMARKING HAAR AND HISTOGRAMS OF ORIENTED GRADIENTS FEATURES APPLIED TO VEHICLE
DETECTION
Table 2: Multi stage detection rate (Haar and HoG classi- 5 CONCLUSION
fiers).
Classifier Stages # Fts # Neg DR (%) FP t (seg) This communication deals with a benchmark com-
Haar 12 306 1000 94.3 598 0.75 paring Haar-like features and Histograms of Oriented
Haar 12 332 2000 94 490 0.71 Gradients features applied to vehicle detection. These
Haar 12 386 3000 93,5 445 0.59 features are used in a classification algorithm based
HoG 12 147 1000 96.5 935 0.51 on Adaboost. Two strategies are implemented: a sin-
HoG 12 176 2000 96.1 963 0.59 gle stage detector and a multi-stage detector. The
HoG 11 192 3000 96.6 954 0.55 tests - applied on realistic on-road images - show two
different results: for the HoG (generative) features,
when the number of considered features increases, the
detection rate increases while the number of false pos-
itives keeps stable; for the Haar-like (discriminative)
features, the number of false positives decreases. Fu-
ture works will be oriented to combined these behav-
iors. An approach could be build using simultane-
ously both feature types. We should also select rele-
vant features.
ACKNOWLEDGEMENTS
This research was supported by PSA Peugeot Citroën.
(a) (b) The authors would liko to thank Fabien Hernan-
dez from PCA Direction de la Recherche et de
Figure 6: Detection results for (a) HoG and (b) Haar Multi l’Innovation Automobile for their help with the data
Stage detectors. collection.
Table 2 shows results of cascade detectors using

Haar and HoG based features. We also tested the
REFERENCES
effect of increasing the size of the negative set in Agarwal, S., Awan, A., and Roth, D. (2004). Learning to
each training stage. The behavior of each detector is detect objects in images via a sparse, part-based rep-
the same as described before. HoG detector try to resentation. IEEE Transactions on Pattern Analysis
construct a finer vehicle model to take into account and Machine Intelligence, 26(11):1475–1490.
the new negatives. The number of features used in- Bensrhair, A., Bertozzi, M., Broggi, A., Miche, P., Mousset,
creases as the model refines. But the detection rate S., and Toulminet, G. (2001). A cooperative approach
and the number of false positives does not change to vision-based vehicle detection. In Proceedings on
significantly. Haar detector refines the fronteer using Intelligent Transportation Systems, pages 207–212.
somemore features and the number of false positives Bertozzi, M. and Broggi, A. (1997). Vision-based vehicle
decreases while the detection keeps quite stable. Fig- guidance. Computer, 30(7):49–55.
ure 5 shows the ROC curves for each detector applied Bertozzi, M., Broggi, A., and Castelluccio, S. (1997). A
for the last stage in the cascade. For a given detection real-time oriented system for vehicle detection. Jour-
rate, these curves show a similar behavior as the sin- nal of Systems Architecture, 43(1-5):317–325.
gle stage detector, where the number of false positives Bucher, T., Curio, C., Edelbrunner, J., Igel, C., Kastrup,
is lower for the Haar classifiers than for the HoG clas- D., Leefken, I., Lorenz, G., Steinhage, A., and von
sifiers; except for the HoG detector trained with 3000 Seelen, W. (2003). Image processing and behavior
planning for intelligent vehicles. IEEE Transactions
negatives, which has a similar behavior with a half on Industrial Electronics, 50(1):62–75.
quantity of features (see table 2). Figure 6 presents
Cha, S. and Srihari, S. N. (2002). On measuring the
some detection results and false alarms. distance between histograms. Pattern Recognition,
35(6):1355–1370.
Collado, J., Hilario, C., de la Escalera, A., and Armingol, J.
(2004). Model based vehicle detection for intelligent
vehicles. In International Symposium on Intelligent
Vehicles, pages 572–577.
363
Dellaert, F. (1997). Canss: A candidate selection and search Yang, H., Lou, J., Sun, H., Hu, W., and Tan, T. (2001).
algorithm to initialize car tracking. Technical report, Efficient and robust vehicle localization. In Interna-
Robotics Institute, Carnegie Mellon University. tional Conference on Image Processing, volume 2,
Demonceaux, C., Potelle, A., and Kachi-Akkouche, D. pages 355–358, Thessaloniki, Greece.
(2004). Obstacle detection in a road scene based
on motion analysis. IEEE Transactions on Vehicular
Technology, 53(6):1649 – 1656.
Franke, U. (2000). Real-time stereo vision for urban traf-
fic scene understanding. In Proceedings IEEE Intel-
ligent Vehicles Symposium 2000, pages 273–278, De-
troit, USA.
Freund, Y. and Schapire, R. (1996). Experiments with a
new boosting algorithm. In International Conference
on Machine Learning, pages 148–156.
Friedman, J., Hastie, T., and Tibshirani, R. (2000). Additive
logistic regression: a statistical view of boosting. The
Annals of Statistics, 28(2):337–374.
Gepperth, A., Edelbrunner, J., and Bocher, T. (2005). Real-
time detection and classification of cars in video se-
quences. In Intelligent Vehicles Symposium, pages
625–631.
Guo, D., Fraichard, T., Xie, M., and Laugier, C. (2000).
Color modeling by spherical influence field in sensing
drivingenvironment. In IEEE, editor, Intelligent Vehi-
cles Symposium, pages 249–254, Dearborn, MI, USA.
Lowe, D. (1999). Object recognition from local scale-
invariant features. In Proceedings of the International
Conference on Computer Vision, pages 1150–1157.
Papageorgiou, C. and Poggio, T. (1999). A trainable ob-
ject detection system: Car detection in static images.
Technical report, MIT AI Memo 1673 (CBCL Memo
180).
Schneiderman, H. and Kanade, T. (2000). A statistical
method for 3d object detection applied to faces and
cars. In ICCVPR, pages 746–751.
Srinivasa, N. (2002). Vision-based vehicle detection and
tracking method for forward collision warning in au-
tomobiles. In IEEE Intelligent Vehicle Symposium,
volume 2, pages 626–631.
Sun, Z., Bebis, G., and Miller, R. (2004). Object detection
using feature subset selection. Pattern Recognition,
37(11):2165–2176.
Sun, Z., Bebis, G., and Miller, R. (2006). On-road vehicle
detection: A review. IEEE Trans. Pattern Anal. Mach.
Intell., 28(5):694–711.
van Leeuwen, M. and Groen, F. (2001). Vehicle detec-
tion with a mobile camera. Technical report, Com-
puter Science Institute, University of amsterdam, The
Netherlands.
Viola, P. and Jones, M. (2001). Rapid object detection using
a boosted cascade of simple features. In Conference
511–518.
Xiong, T. and Debrunner, C. (2004). Stochastic car track-
ing with line- and color-based features. Advances
and Trends in Research and Development of Intelli-
gent Transportation Systems: An Introduction to the
Special Issue, 5(4):324–328.
364
MULTIMODAL COMMUNICATION ERROR DETECTION FOR
DRIVER-CAR INTERACTION
Sy Bor Wang, David Demirdjian, Trevor Darrell

Massachusetts Institute of Technology, 32 Vassar Street, Cambridge MA 02139, USA
Hedvig Kjellström
KTH (Royal Institute of Technology), CSC, SE-100 44 Stockholm, Sweden
[email protected]
Keywords: Audio-Visual Recognition, System Error Identification, Conversational systems.
Abstract: Speech recognition systems are now used in a wide variety of domains. They have recently been introduced in
cars for hand-free control of radio, cell-phone and navigation applications. However, due to the ambient noise
in the car recognition errors are relatively frequent. This paper tackles the problem of detecting when such
recognition errors occur from the driver’s reaction. Automatic detection of communication errors in dialogue-
based systems has been explored extensively in the speech community. The detection is most often based on
prosody cues such as intensity and pitch. However, recent perceptual studies indicate that the detection can be
improved significantly if both acoustic and visual modalities are taken into account. To this end, we present a
framework for automatic audio-visual detection of communication errors.
1 INTRODUCTION 1998). For example, users tend to speak slower or

louder when speech misrecognition occurs. Such a
In recent years, there has been an increased interest change in speaking style usually leads to worse recog-
in more intelligent and emotional car interfaces. This nition results since standard speech recognizers are
has been motivated by the need to make driver-car in- trained on normal, non-hyperarticulated speech (Ovi-
teractions easier and more natural and to reduce the att and VanGent, 1998). These problems motivated
cognitive load of the driver, who is now confronted the monitoring of prosodic aspects of a speaker’s ut-
with multiple device, e.g. phone, radio, navigation terances, and several studies have shown that using
system. To this extent, speech recognition technol- automatically extracted prosodic features helps in er-
ogy has recently been introduced in the car. However, ror detection (Litman et al., 2001). However, the
due to the difficulty of the environment (e.g. noise level of effectiveness of these prosodic features dif-
produced by the engine, vibrations), speech recog- fers across studies and the analysis of prosodic fea-
nition is still brittle and satisfying recognition rates tures are done only on user utterances and not on au-
can be obtained only for relatively small vocabular- dio cues of users while they are listening to the sys-
ies, limiting the extent of the driver-car interaction. A tem response. Such limitations hint at the possible
speech recognition system that can automatically de- use of additional modalities or other types of features
tect recognition errors would allow for smoother in- (e.g. visual features) to improve error detection.
teraction. The co-occurrence of audio and visual modali-
Many spoken dialogue systems have difficulty de- ties has been widely explored for emotion recogni-
termining whether the communication is going well tion. Recent work in multimodal recognition of emo-
or has problems (e.g. due to poor speech recogni- tions has shown that a combination of prosodic fea-
tion). Various researchers have shown that human tures and facial expressions improves affect recog-
users change their speaking style when the system nition (Zeng et al., 2004). The primary aim of our
misrecognizes their speech as compared to when the work in this paper is to find an automatic system to
system correctly recognized their speech (Hirschberg detect communication errors in a conversational sys-
et al., 2001; Litman et al., 2001; Oviatt and VanGent, tem. Using both visual and audio features, we com-
365
pare the performance of different classifiers in the uni-

modal stream and different audio-visual fusion strate-
gies for identification, taking into account asynchrony
between acoustic and visual user reactions, using au-
dio and video data of user interactions with a dialogue
system in a natural setting.
2 RELATED WORK
There has been limited literature on the use of low-
level audio cues and visual features in automatically
detecting dialogue-based system errors in an authen- Figure 1: Illustration of communication errors. In a., the
tic environment. A perceptual study conducted by subject is making a query of a restaurant the first time. In
Barkhuysen et al. (Barkhuysen et al., 2004) showed b., the subject is listening to the response of the system. And
that audio and visual cues are useful in detecting com- in c., the subject repeats his query. The facial expression of
munication errors. The study also showed that using the user in b. and c., as well as the tone of the user’s voice
visual cues were very effective for detecting system in c. are the cues our algorithm exploits for the detection of
errors when the user is listening in silence to the re- communication errors.
ply from the dialog manager. In this study though,
subjects were specifically instructed to face a camera ply as a separate stream.
embedded in a cellphone while speaking to it. Knowl- In this paper we propose to detect communication
edge of this camera could bias the subject’s behav- errors multimodally by using visual and audio fea-
ior. As shown by Sebe et al. (Sebe et al., 2004), this tures. We focus on an environment where the user
knowledge bias was significant for learning facial ex- is interacting with a conversational information query
pressions. In this work, subjects were viewing movie system similar to the ones present in car navigation
clips in a kiosk, without any knowledge of a cam- systems. We define a communication error as the
era capturing their facial expressions. However, no instance where the system misrecognizes the driver
prosody or audio cues of the subjects were collected. speech, and makes an erroneous reply. This is illus-
Recent work done in emotion or affect recog- trated in Figure 1. We detect the presence of this error
nition has explored the combined use of prosody when the user is speaking or when the user is listening
and facial features (Zeng et al., 2004; Chen et al., to the system.
1998). Zeng et al. (Zeng et al., 2004) used a vot- The rest of the paper is described as follows. First,
ing method to combine the facial feature and prosody we describe the features we extract from audio and
classifiers to improve affect recognition. Although visual streams to measure confusion. Then we give
this work addressed the difficult task of classifying a description of our classification models and late fu-
eleven emotional states, it suffers from the use of a sion strategies, followed by our experiment setup and
database where subjects generated emotions upon re- procedure. In the last section, we show comparative
quest, which may not be the genuine expressions in results of the different classifiers.
an authentic environment.
In the domain of detecting communication errors,
also known as system errors, audio cues have been
explored widely. Oviatt (Oviatt and VanGent, 1998) 3 MULTIMODAL INPUT
showed that there is a pattern of hyper-articulation
when there are system errors, which leads to worse 3.1 Visual Component
recognition results. Litman et al. (Litman et al., 2001)
and Hirschberg et al. (Hirschberg et al., 2001) auto- In this section we describe our algorithm for estimat-
matically extracted prosodic features of a speaker’s ing head pose and facial motion features from monoc-
utterances and showed that these features have been ular image sequences. In our framework, head mo-
useful in error detection, although the extent to which tion is decomposed into two distinct components. The
prosody is beneficial differs across studies. This im- first component consists of the 3D rigid motion of the
plies that the accuracy of error detection can be im- head. The second component consists of the local
proved by the addition of other features, e.g. visual motion generated by the non-rigid parts of the face
cues, either as a combination with audio cues or sim- (e.g. mouth, lips, eyes).
366
Figure 2: Audio-visual platform installed in a car simulator.

The face of the driver is tracked using a camera installed
near the rear view mirror: the white cube surrounding the
driver’s face corresponds to Π, the location and orientation
of the pose estimate.
The head motion estimation algorithm consists of

first estimating the rigid motion component of the
head by using a robust algorithm. This rigid motion
then is used to compensate the global motion of the
head and to estimate the local motion of the non-rigid Figure 3: Our head motion estimation algorithm consists
parts. in estimating the rigid motion component (left image) and
compensated optical flow on the face surface (right image).
3.1.1 Head Motion and Pose Estimation
The algorithm for head motion and pose estimation is

an implementation of the drift-free tracking technique our framework, the vision-based features are defined
described in (Morency et al., 2003). In contrast to the as:
original work, which required stereo inputs, our im- V f = (ū(m1 ), . . . , ū(mN )) (2)
plementation estimates head pose in monocular image where ū(mk ) is the head motion-compensated optical
sequences. In addition motion estimation is based on flow for a point mk of the face.
the robust estimator RANSAC (Fischler and Bolles,
1981) which allows large noise in the data (e.g. tem- 3.2 Audio Component
porary occlusions of the face). The face tracking al-
gorithm is initialized in a standard fashion using a We use three kinds of prosody features: the intensity
frontal-view face detector (Viola and Jones, 2004). E, the pitch, F0 , and the first formant frequency, F1 .
Our algorithm provides the head pose in a 3D co- The prosody feature vector A f is then defined as:
ordinate system and is represented by a 6-vector Π:
A f = (E, F0 , F1 ) (3)
Π = (tx ,ty ,tz , φα , φβ , φγ ) (1)
These features are computed at every 10 msecs us-
where (tx ,ty ,tz ) is the location and (φα , φβ , φγ ) the ori- ing the speech analysis software, PRAAT (Boersma,
entation (pan, tilt and swing angles) of the head. 2001). The intensity E is computed as: E =
log(∑Ni=1 (x[i] − x̄)2 ) where N is the window length
3.1.2 Facial Motion Features and x[i] is the ith sample in that window and x̄ is the lo-
cal average signal value. In our computation (and for
Let δ be the rigid motion between the last two frames the rest of this section) we used a window length of 40
t − 1 and t. The facial motion features are defined as samples. The pitch F0 is estimated as the reciprocal
the head motion-compensated optical flow, i.e. the op- of the fundamental period as described in (Boersma,
tical flow between the images It−1 and It from which 1993). In our experiments, we set the search range of
the motion δ has been ‘subtracted’. The facial motion the pitch to be 75 - 1000 Hz. As for the computation
features correspond to the local non-rigid motion gen- of the first formant frequency, F1 , a segment of N sam-
erated by the muscles of the face only (e.g. lips, jaw, ples is extracted for every time step of 1 msec. This
eyes), independent from the global head motion. In segment is multiplied by a Gaussian-like window and
367
the LPC coefficients are computed. This first formant Hidden Conditional Random Fields (HCRF)
is then extracted using these coefficients by the Burg - The HCRF is a model that has recently been
algorithm described in (Childers, 1978). introduced for the recognition of observation se-
In previous work (Sebe et al., 2004) syllable rate quences (Quattoni et al., 2004). Here we describe the
was used as a prosody feature. However, in our work, HCRF model briefly:
our audio data consists of spoken as well as non- A HCRF models the conditional probability of a
spoken words, e.g. exclamations, gasps or humming, class label given an observation sequence by:
which we want to model for automatic problem de-
tection. and our speech recognizer had a lot of dif- ∑s eΨ(y,s,x;θ)
P(y | x, θ) = ∑ P(y, s | x, θ) =
ficulty computing an accurate syllable rate. Of the ∑y′ ∈Y ,s∈Sm eΨ(y ,s,x;θ)
′
s
219 utterances processed by the speech recognizer, (4)
97 utterances have an incorrect number of hypothe- where s = {s1 , s2 , ..., sm }, each si ∈ S captures certain
sized vowel phones. On average, these incorrectly underlying structure of each class and S is the set of
recognized utterances have 2.73 syllables more than hidden states in the model. If we assume that s is ob-
the hypothesized ones. served and that there is a single class label y then the
conditional probability of s given x becomes a regular
CRF. The potential function Ψ(y, s, x; θ) ∈ ℜ, param-
4 MULTIMODAL DETECTION eterized by θ, measures the compatibility between a
label, the observation sequence, and the configuration
OF SYSTEM ERRORS of the hidden states.
In our paper, the local observations are the visual
We explore different techniques to detect communi- features, V f , or the audio features, A f . We trained a
cation errors from sequences of audio-visual features single two-class HCRF. Test sequences were run with
estimated in Section 3.2. First, we describe unimodal this model and the communication state class with the
classification models followed by the multimodal fu- highest probability was selected as the recognized er-
sion strategies we tested. ror state.
For the HMM model, the number of Gaussian
4.1 Unimodal Classification Methods mixtures and states was set by minimizing the error
on training features. For the HCRF model, the num-
We want to map an observation sequence x to class ber of hidden states was set in a similar fashion.
labels y ∈ Y , where x is a vector of t consecutive ob-
servations, x = {x1 , x2 , . . . xt }. In our case, the local
observation xt can be an audio feature A f , or a visual
4.2 Multimodal Fusion Strategies
feature, V f .
To detect communication errors, learning the se- We have a choice between early or late fusion when
quential dynamics of these observations is important. combining the audio and visual modalities. In early
Hidden Markov Models (HMMs) (Rabiner, 1989) are fusion, we can model the audio and visual features
well known generative probabilistic sequence models in a single joint feature space, and use the joint fea-
that capture sequence dynamics; Hidden Conditional ture for training a single classifier. In late fusion, we
Random Fields (HCRFs) (Quattoni et al., 2004; Wang can train a classifier on each modality separately and
et al., 2006) are discriminative analogs that have been merge the outputs of the classifiers. As illustrated in
recently introduced for gesture recognition. We com- Figure 1, our communication error detection has two
pare both techniques in our experiments below; exper- different modes: in b. we use visual features only for
iments with classifiers taking a single observation as error detection and in c. we use both audio and visual
input previously demonstrated poor results, and were features. The single mode in b. requires us to train
not included in our experiments. a classifier using a single input stream. In addition,
Hidden Markov Models (HMM) - We trained a training classifiers based on individual streams is a
HMM model for each communication state. During simpler process. As such, we choose late fusion tech-
evaluation, test sequences were passed through each niques, i.e. fusing the outputs of two classifiers. We
of these models and the model with the highest like- use two common late-fusion strategies as described
lihood was selected as the recognized communication in (Kittler et al., 1998).
state. This is a generative, sequential model with hid- Let the feature input to the j-th classifier, j =
den states. More details of this model are described 1, ..., R be x j , and the winning label be h. A uniform
in (Rabiner, 1989). prior across all classes is assumed.
368
R
PRODUCT rule: h = arg max ∏ P(wk |x j ).
1
k j=1
0.9
With the product rule, we multiply the probabilities 0.8
of the visual feature classifier and the audio feature 0.7
True Positives
classifier, and select the winning class based on the 0.6
0.5
highest scoring multiplication.
R 0.4
SUM rule: h = arg max ∑ P(wk |x j ). 0.3
k j=1 0.2
HMM
HCRF
With the sum rule, we add the probabilities of the vi- 0.1
0 0.2 0.4 0.6 0.8 1
sual feature classifier and the audio feature classifier, False Positives
and select the winning class based on the highest scor- 1
ing sum. 0.9
0.8
0.7
True Positives
0.6
5 EXPERIMENTS AND RESULTS 0.5
0.4
5.1 Data Collection 0.3
0.2
HMM
0.1
HCRF
To evaluate the performance of the different classifiers 0
0 0.2 0.4 0.6 0.8 1
and fusion strategies, we collected an audio-visual False Positives
database where the facial expressions and the audio

cues would correspond to the actual conversational Figure 4: (top) ROC curves showing the performance of the
state of the subject. There were several design issues different classifiers of visual features. (bottom) ROC curves
showing the performance of the different classifiers using
we had to consider to minimize bias of our data col-
prosody features. From this figure, HMMS and HCRFs do
lection experiment. These issues were approached in not perform well on the prosody features, while HCRFs per-
a similar fashion to the database collected for natu- form much better than HMMs on visual features.
ral facial expressions (Sebe et al., 2004). In particu-
lar, the subjects could not know that they were being
tested for their communication state 1 . Such knowl- were recorded throughout the whole experiment un-
edge could influence their communication state and der brightly lit and low noise conditions. The video
invalidate the results. was recorded at 15 Hz and the audio at 44kHz. A total
We set up a conversational kiosk with a hidden of six subjects performed the study. All six subjects
camera and microphone array. This kiosk contained a were male and were between 20-30 years of age.
web-based navigation query speech interface consist- From the database described above, all the se-
ing of a display showing a Google map with restau- quences were manually labeled and segmented for
rant icons. Subjects did not know that they were in- training our classifiers described in Section 4.1. A to-
volved in an experiment about communication error tal of 227 error sequences and 84 error-free ones were
detection: they were told their task was to test the collected. For each human subject, 90% of his/her
navigation system and report what they liked or dis- data were picked at random and used for training,
liked about it at the end of the experiment. They were while the remaining ones were used for testing.
given a list of restaurants to query for information.
The subjects had to make the queries in sequential
order, and repeat the query in any way they wished
5.2 Testing and Evaluation
(e.g. repeating the same question, or using a different
5.2.1 Visual Features Classification
phrase) when the system did not respond correctly.
They could only proceed to the next query when the
system displayed the correct restaurant information Facial motion features, V f , described in Section 3.1
on the display. The purpose of this sequential query are used as observations for training and testing. Fig-
was to create a need to solve the communication er- ure 4 shows the results of the classifiers described in
ror when it occurs. The audio and video of the user Section 4.1 using visual features. From this figure,
HCRF performs better than HMMs for visual feature
1 At the end of the experiment, we procured agreement
classification.
for the use of the audio-visual footage from the subjects for
our experiments.
369
5.2.2 Prosody Features Classification 1

Fusion using the Product rule
0.9
0.8
Using prosody features, A f , from Section 3.2 as ob- 0.7
servations, we trained two classifiers described in
False Positives
0.6
Section 4.1. Figure 4 shows the ROC curves for the 0.5
different classifiers. From this figure, both HCRFs 0.4
and HMMs perform poorly for prosody feature clas- 0.3
sification. This is due to the use of only three acoustic 0.2

HMM PRODUCT
characteristics as our prosody features and shows that 0.1 HCRF PRODUCT
0
such features are not very indicative of communica- 0 0.2 0.4 0.6
True Positives
0.8 1
tion problems. Fusion using the SUM rule

1
0.9
5.2.3 Audio-Visual Classification 0.8
0.7
We compared the performance of HMMs and HCRFs
False Positives
0.6
in the late fusion experiments. Figure 5 shows the 0.5
0.4
ROC curve of the combining the various classifiers
0.3
using the SUM and PRODUCT rule. The classi- 0.2
fiers show a significant improvement using the late fu- 0.1
HMM SUM
HCRF SUM
sion strategies, despite a poor performance when only 0

0 0.2 0.4 0.6 0.8 1
prosody features are used. Interestingly, this concurs True Positives
with findings in the area of audio-visual speech per-

ception (Massaro, 1987; Summerfield, 1987), indi- Figure 5: (top) ROC curve showing the performance of dif-
ferent classifiers using PRODUCT rule for fusion.(bottom)
cating that humans fuse information from different
ROC curve showing the performance of different classifiers
modalities at a relatively late stage. In addition, fu- using SUM rule for fusion. Both curves show that despite
sion of HCRF classifiers performed better than fusion poor performance from the audio stream, late fusion im-
of HMM classifiers. Using the SUM rule to combine proved the performance significantly. In addition, HCRFs
the HCRF classifiers produced the best result. perform better than HMMs after fusion.
a HCRF with audio-visual input and a fusion strategy

6 CONCLUSIONS using the SUM rule.
In this paper, we presented experiments evaluating

different classification and fusion methods for detect-
ing communication errors in a conversational system. REFERENCES
Authentic audio-visual data of human-dialogue inter-
actions with the conversational system was collected Barkhuysen, P., Krahmer, E., and Swerts, M. (2004). Au-
diovisual perception of communication problems. In
and labeled according to the presence of communica- Speech Prosody.
tion errors, and used to train and test the automatic
Boersma, P. (1993). Accurate short-term analysis of the fun-
system. damental frequency and the harmonis-to-noise ratio of
Features extracted from the audio included differ- a sampled sound. In IFA.
ent prosody characteristics, such as pitch and inten- Boersma, P. (2001). Praat, a system for doing phonetics by
sity and the first formant frequency. From the visual computer. Glot International, 5(9/10):341–345.
channel, the user’s global head motion and local face
Chen, L., Huang, T. S., Miyasato, T., and Nakatsu,
motion were extracted. Different strategies for clas- R. (1998). Multimodal human emotion/expression
sification from these cues were evaluated, as well as recognition. In International Conference on Face and
strategies for fusion of the two modalities. Despite the Gesture Recognition.
problems associated with asynchrony of audio and vi- Childers, D. G. (1978). Modern Spectrum Analysis. IEEE
sual features, we used simple late fusion strategies of Press.
our HCRF and HMM classifiers and showed that they
Fischler, M. A. and Bolles, R. C. (1981). Random sample
have improved error detection. To summarize, we consensus: a paradigm for model fitting with appli-
find that communication errors in a dialogue-based cations to image analysis and automated cartography.
system can be detected with a better accuracy using Communications of the ACM, 24(6):381–395.
370
Hirschberg, J., Litman, D., and Swerts, M. (2001). Identify-

ing user corrections automatically in spoken dialogue
systems. In 2nd Meeting of the North American Chap-
ter of the Association for Computational Linguistics
on Language Technologies.
Kittler, J., Hatef, M., Duin, R., and Matas, J. (1998). On
combining classifiers. IEEE Transactions on Pattern
Analysis and Machine Intelligence, 20(3):226–239.
Litman, D., Hirschberg, J., and Swerts, M. (2001). Predict-
ing user reactions to system error. In ACL.
Massaro, D. (1987). Speech Perception By Ear and Eye.
Lawrence Erlbaum Associates, Hillsdale, NJ, USA.
Morency, L., Rahimi, A., and Darrell, T. (2003). Adaptive
view-based appearance models. In IEEE Conference
803–810.
Oviatt, S. L. and VanGent, R. (1998). Error resolution
during multimodal human-computer interaction. In
SpeechCommunication.
Quattoni, A., Collins, M., and Darrell, T. (2004). Condi-
tional random fields for object recognition. In Neural
Information Processing Systems.
Rabiner, L. R. (1989). A tutorial on hidden markov models
and selected applications in speech recognition. Pro-
ceedings of the IEEE, 77(2):257–286.
Sebe, N., Lew, M., Cohen, I., Sun, Y., Gevers, T., and
Huang, T. S. (2004). Authentic facial expression anal-
ysis. In International Conference on Automatic Face
and Gesture Recognition.
Summerfield, Q. (1987). Some preliminaries to a compre-
hensive account of audio-visual speech perception. In
Dodd, B. and Campbell, R., editors, Hearing by Eye,
pages 3–51. Lawrence Erlbaum Associates, Hillsdale,
NJ, USA.
Viola, P. and Jones, M. J. (2004). Robust real-time face
detection. International Journal of Computer Vision,
57(2):137–154.
Wang, S., Quattoni., A., Morency, L.-P., Demirdjian, D.,
and Darrell, T. (2006). Hidden conditional random
fields for gesture recognition. In IEEE Conference on
Computer Vision and Pattern Recognition.
Zeng, Z., Tu, J., Liu, M., Zhang, T., Rizzolo, N., Zhang, Z.,
Huang, T. S., Roth, D., and Levinson, S. (2004). Bi-
modal hci-related affect recognition. In International
Conference on Multimodal Interfaces.
371
Danil Prokhorov
Toyota Technical Center
Toyota Motor Engineering and Manufacturing North America (TEMA)
2350 Green Rd., TRD, Ann Arbor, MI 48105, U.S.A.
[email protected]
Keywords: HEV powertrain, neurocontroller, extended Kalman filter.
Abstract: We describe an application of a computational intelligence method for superior control of the Toyota Prius
hybrid electric vehicle. We are interested in improvement of fuel efficiency while reducing emissions. We
describe our approach which is based on recurrent neural networks. The proposed approach is quite general
and applicable to other complex real-world systems.
1 INTRODUCTION (Harmon et al., 2005).

The references cited above indicate a significant
Hybrid powertrains have been gaining popularity due potential for improving HEV performance through
to their potential to improve significantly fuel econ- more efficient power management based on applica-
omy and reduce undesirable emissions. Control tion of computational intelligence (CI) techniques. To
strategies of the hybrid electric vehicle (HEV) are the best of our knowledge, there has been no work on
more complex than those of the internal combustion improving HEV control by CI methods for the Toy-
engine-only vehicle because they have to deal with ota Prius. Though the Prius efficiency is quite high
multiple power sources in sophisticated configura- already, there is a significant potential for further im-
tions. The main function of any control strategy is provement, as will hopefully become apparent from
power management. It typically implements a high- this paper.
level control algorithm which determines the appro- Unlike traditional hybrid powertrain schemes, se-
priate power split between the electric motor and the ries or parallel, the Prius hybrid implements what is
engine to minimize fuel consumption and emissions, called the power split scheme. This scheme is quite
while staying within specified constraints on drivabil- innovative and has not been studied extensively yet.
ity, reliability, battery charge sustenance, etc. The Prius powertrain uses a planetary gear mecha-
Computational intelligence techniques have pre- nism to connect an internal combustion engine, an
viously been applied to HEV power management by electric motor and a generator. A highly efficient en-
various authors. A rule-based control was employed gine can simultaneously charge the battery through
in (Baumann et al., 2000). Fuel economy improve- the generator and propel the vehicle (Figure 1). It is
ment with a fuzzy controller was demonstrated in important to be able to set the engine operating point
(Schouten et al., 2002), relative to other strategies to the highest efficiency possible and at sufficiently
which maximized only the engine efficiency. An in- low emission levels of undesirable exhaust gases such
telligent controller combining neural networks and as hydrocarbons, nitrogen oxides and carbon monox-
fuzzy logic which could adapt to different drivers and ide. The motor is physically attached to the ring gear.
drive cycles (profiles of the required vehicle speed It can move the vehicle through the fixed gear ratio
over time) was studied in (Baumann et al., 1998). and either assist the engine or propel the vehicle on its
Recently a neurocontroller was employed in a hybrid own for low speeds. The motor can also return some
electric propulsion system of a small unmanned aerial energy to the battery by working as another generator
vehicle which resulted in significant energy savings in the regenerative braking mode.
372
account (Prokhorov et al., 2001), (Puskorius et al.,

1996).
3 EXPERIMENTS
We first train a NN model to enable off-line train-
ing the neurocontroller as discussed in Section 2. To
do supervised training of the NN model in Figure
2, we gather the input-output pairs from 20 diverse
drive cycles generated in the Prius simulator. We
trained a 25-node structured RNN for 3000 epochs
using the multi-stream EKF (Prokhorov et al., 2001)
and attained the training RMSE of 5·10−4 (the largest
generalization RMSE was within 20% of the training
Figure 1: The Prius car and the main components of the
Toyota hybrid system. RMSE).
The closed-loop control system for training the
NN controller is shown in Figure 2. The converter
determines the required values of the speed ωdr and
As in our previous work (Prokhorov et al., 2001),
the torque Trd at the ring gear of the planetary mecha-
we employ recurrent neural networks (RNN) as con-
nism to achieve the desired vehicle speed specified in
trollers and train them for robustness to parametric
the drive cycle. This is done on the basis of the Prius
and signal uncertainties (known bounded variations of
model of motion. The constraint verifier assures sat-
physical parameters, reference trajectories, measure-
isfaction of various constraints which must hold for
ment noise, etc.). We intend to deploy the trained neu-
the engine, the motor and the generator speeds and
rocontroller with fixed weights.
torques in the planetary gear mechanism, i.e., ωe and
This paper is structured as follows. In the next
Te , ωm and Tm , ωg and Tg , respectively.
section we describe the main elements of our off-line
Our first control goal is to minimize the average
training. The approach permits us to create an RNN
fuel consumed by the engine. However, fuel mini-
controller which is ready for deployment with fixed
mization only is not feasible. The Prius nickel-metal
weights. We then describe our experiments in Sec-
hydride battery is the most delicate nonlinear compo-
tion 3, followed by our conclusions and directions for
nent of the system with long-term dynamics due to
future work.
discharging, charging and temperature variations. It
is important to avoid rapid and deep discharges of the
battery which can drastically reduce its life, requir-
2 OFF-LINE TRAINING ing costly repairs or even battery replacement. Thus,
the second goal of the HEV control is to maintain the
We adopt the approach of indirect or model based battery State Of Charge (SOC) throughout the drive
control development for off-line training. The Prius cycle in the safe zone. The SOC can vary between
simulator is a highly complex, distributed software 0.0 (fully discharged) and 1.0 (fully charged), but the
which makes training a neurocontroller directly in safe zone is typically above 0.4.
the simulator difficult. We implemented a hybrid ap- We combine the two control goals to ob-
proach in which the most essential elements of the tain cost(t) = λ1 s f 2 (t) + λ2 (t)(SOCd (t) − SOC(t))2 ,
simulator are approximated sufficiently accurately by where s f (t) stands for specific fuel or fuel rate con-
a neural network model. The NN model is used to sumed by the engine at time t, λ1 = 1, and λ2 (t) ∈
train a neurocontroller by effectively replacing the [10, 50] due to about one order of magnitude differ-
simulator. The trained neurocontroller performance ence between values of s f and those of SOC. The
is then verified in the simulator. desired SOCd (t) is constant in our experiments for
The use of differentiable NN for both model and simplicity. We encourage our controller to pay ap-
controller makes possible application of the industri- proximately the same level of attention to both s f and
ally proven training method which employs the back- SOC, although the optimal balance between λ1 and
propagation through time (BPTT) and the extended λ2 is yet to be determined. We also penalize reduc-
Kalman filter (EKF) for training NN. We refer the tions of the SOC below SOCd five times heavier than
reader to other references for its more comprehensive its increases to discourage the controller from staying
373
in the low-SOC region for long. Thus, λ2 (t) = 10 if lator because the NN model and the verifier only ap-
SOC(t) ≥ SOCd , and λ2 (t) = 50 if SOC(t) < SOCd . proximate the simulator’s behavior. Our results below
Ultimately, we would also like to minimize emis- pertain to the simulator, rather than its NN approxi-
sions of the harmful gases. In this study we attempt to mation.
reduce emissions indirectly through reducing the fuel The basic idea of the current Prius HEV control
consumption because they are often correlated. logic is discussed in (Hermance, 1999). When the
Our RNN controller has 5-5R-2 architecture, i.e., power demand is low and the battery SOC is suffi-
five inputs, five recurrent nodes in the fully recur- ciently high, the motor powers the vehicle. As the
rent hidden layer, and two bipolar sigmoids as output power demand and vehicle speed increase, or the SOC
nodes. The RNN receives as inputs the required out- reduces below a threshold, the engine is started (the
put drive speed ωdr and torque Trd , the current engine generator may help the motor start the engine). The
fuel rate s f , the current SOC and the desired SOC engine power is split between propelling the vehi-
SOCd (see Figure 2; the desired fuel rate is implicit, cle and charging the battery through the generator.
and it is set to zero). The RNN produces two con- As the power demand continues to grow, the engine
trol signals in the range of ±1. The first output is the might not be able to stay within its efficiency limits.
engine torque τe , and the second output is the engine In those cases the motor can provide power assist by
speed we which become Te and ωe , respectively, after driving the wheels to keep the engine efficiency rea-
passing through the constraint verifier. sonably high, as long as the battery can supply the re-
quired power. During decelerations the motor is com-
manded to operate as a generator to recharge the bat-
tery, thereby implementing regenerative braking.
It is hard to make this rule-based strategy opti-
Te(t)
SOC(t) mal for such a complex powertrain. Significant aver-
SOC(t-1)
we(t)
te(t) aging over drive cycles with quite different behavior
Constraint
Tg(t)
verifier
SOCd(t) NN NN
compromising the best achievable performance is un-
controller wg(t) model
sf (t-1)
we(t)
Tm(t)
avoidable. We believe that a strategy based on a data-
sf (t)
wm(t)
driven learning system should be able to beat the rule-
wrd (t ) based strategy because of its ability to discern differ-
d
Tr (t )
ences in driving patterns and take advantage of them
60
50 for improved performance.

Vehicle 40
Drive cycle
Converter
desired
30
20
We compare our RNN controller trained for ro-
speed
10
0
0 100 200 300 400 500
bustness with the rule-based control strategy of the
Prius on 20 drive cycles including both standard
cycles (required by government agencies) and non-
Figure 2: Block diagram of the closed-loop system for train-
standard cycles (e.g., random driving patterns). Our
ing the NN controller. The converter determines the re-
quired values of speed ωdr and torque Trd at the ring gear
RNN controller is better by 15% on average than the
of the planetary mechanism to achieve the desired vehicle rule-based controller in terms of fuel efficiency, and it
speed profile. The constraint verifier makes sure not only appears to be slightly better than the rule-based con-
that the torques and speeds are within their specified physi- troller in terms of its emissions on long drive cycles.
cal limits but also that they are consistent with constraints of It also reduces variance of the SOC over the drive cy-
the planetary gear mechanism. The trained NN model takes cle by at least 20%.
care of the remaining complicated dynamics of the plant. Figure 3 shows an example of our results. It is
The feedback loop is closed via SOC and the fuel rate s f ,
a fragment of a long drive cycle (the total length is
but the required ωdr and Trd are guaranteed to be achieved
through the appropriate design of the constraint verifier. 12, 700 seconds). Our advantage appears to be in the
more efficient usage of the engine. The engine effi-
ciency is 32% vs. 29% for the rule-based controller.
Our RNN controller is trained off-line using the We also achieved a big improvement in the genera-
multi-stream EKF algorithm described in Section 2. tor efficiency: 77% vs. 32%, with other component
When training of our NN controller from Figure 2 is efficiencies remaining basically unchanged.
finished, we can deploy it inside the high-fidelity sim-
ulator which approximates well behavior of the real
Prius and all its powertrain components. As expected,
we observed some differences between the neurocon-
troller performance in the closed loop with the NN
model and its performance in the high-fidelity simu-
374
works and fuzzy logic. In SAE Technical Paper

vehicle speed
50 981061, SAE Int. Cong. and Exposition. SAE Press.

Baumann, B. M., Washington, G. N., Glenn, B. C., and Riz-
zoni, G. (2000). Mechatronic design and control of
hybrid electric vehicles. IEEE/ASME Transactions on
0 Mechatronics, 5(1):58–72.
200 400 600 800 1000 1200 1400 1600 1800 2000
Harmon, F., Frank, A., and Joshi, S. (2005). The control of a
0.01 parallel hybrid-electric propulsion system for a small
emissions
unmanned arial vehicle using a cmac neural network.

0.005 Neural Networks, 18(5/6):772–780.
Hermance, D. (1999). Toyota hybrid system. In SAE
TOPTEC Conference Proc., Albany, NY. SAE Press.
0
200 400 600 800 1000 1200 1400 1600 1800 2000 Prokhorov, D. V., Puskorius, G. V., and Feldkamp, L. A.
0.8 (2001). Dynamical neural networks for control. In A
Field Guide to Dynamical Recurrent Networks, Edited
by J. Kolen and S. Kremer. IEEE Press.
soc
0.6 Puskorius, G. V., Feldkamp, L. A., and L. I. Davis, J.

(1996). Dynamic neural network methods applied
to on-vehicle idle speed control. Proceedings of the
200 400 600 800 1000 1200 1400 1600 1800 2000 IEEE, 84(10):1407–1420.
200 Schouten, N., Salman, M., and Kheir, N. (2002). Fuzzy
torques
logic control for parallel hybrid vehicles. IEEE Trans-

100 actions on Control Systems Technology, 10(3):460–
0 468.
−100
200 400 600 800 1000 1200 1400 1600 1800 2000
400
speeds
200
0
−200
−400
200 400 600 800 1000 1200 1400 1600 1800 2000
Figure 3: A 2000-second fragment of a long city drive cycle
illustrating the typical performance of our RNN controller.
The initial segment (from 0 to ∼ 300 seconds) has signifi-
cant unavoidable emissions due to the engine cold start. The
speed ωdr and the torque Trd are the blue lines, ωe and Te are
the green lines, the ωm and Tm are the red lines, and ωg and
Tg are the cyan lines. Note that ωm = ωdr due to the system
design constraint.
4 CONCLUSION
We illustrate our approach to training neurcontrollers
on the Toyota Prius HEV through a high-fidelity sim-
ulation, which is done for the first time by methods of
computational intelligence and with results improved
over those of the existing controller. Our approach is
applicable to many real-world control problems.
REFERENCES
Baumann, B., Rizzoni, G., and Washington, G. N. (1998).
Intelligent control of hybrid vehicles using neural net-
375
AUTHOR INDEX
Abe, K................................................ 323 Filipe, J.................................................92

Aeyels, D. ............................................ 76 Fonte, A................................................52
Aguirre, E........................................... 181 Fraisse, P. .............................................52
Ahmadabadi, M. ................................ 240 Gamallo, C. ........................................149
Alici, G............................................... 165 García, D. ...........................................220
Amat, J. ................................................ 59 García, N. ...........................................133
Angulo, C........................................... 174 García-Silvente, M. ............................181
Auer, E. .............................................. 109 Gautier, M. .........................................341
Azorín, J............................................. 133 Ghaderi, F...........................................240
Badreddin, E. ............................... 44, 327 Giralt, X................................................59
Baffet, G................................................. 5 Gomolitsky, R. .....................................84
Barro, S. ............................................. 292 Gracia, L.............................................133
Bassit, L. .............................................. 52 Haraguchi, K. .....................................200
Berns, K. .................................... 141, 157 Haverinen, J........................................280
Bertrand, D......................................... 311 Hernansanz, A. .....................................59
Bez, H. ............................................... 303 Holada, M...........................................307
Bhaskar, H. ........................................ 303 Horčička, J..........................................307
Bielecka, M. ......................................... 29 Hubený, J............................................234
Bielecki, A. .......................................... 29 Iglesias, R...........................................292
Bîzdoacă, N........................................ 319 Ishii, K................................................323
Boguslavsky, A. ................................. 284 Janeiro, J.............................................220
Boudaba, M........................................ 117 Janot, A. .............................................341
Bounab, B. ......................................... 253 Jipp, M................................................327
Brezovan, M....................................... 337 Kálmán, V. .........................................271
Burdescu, D. ...................................... 337 Kemppainen, A. .................................280
Buttarazzi, B. ..................................... 333 Khatounian, F.....................................341
Calvo, O. ............................................ 220 Kim, D................................................248
Cárdenas, C. ....................................... 220 Kjellström, H......................................365
Casals, A. ........................................... 117 Kopetschke, I......................................307
Chablat, D. ........................................... 84 Kwon, S................................................36
Chapeau-Blondeau, F......................... 195 Lechner, D..............................................5
Charara, A. ............................................. 5 Lee, S..................................................216
Charron, G. .......................................... 52 Li, X. ..................................................125
Chen, G. ............................................. 207 Li, Z....................................................165
Choi, Y............................................... 248 Lino, P. .................................................11
Clady, X. ............................................ 359 Luo, N. ...............................................259
Clément, A. ........................................ 195 Luther, W. ..........................................109
Cook, C. ............................................. 165 M’Sirdi, N. .........................................351
Correa, J. ............................................ 292 Maione, B.............................................11
Dallagi, H........................................... 296 Mannino, M........................................311
Darrell, T............................................ 365 Marshall, I. .........................................275
Degeratu, S......................................... 319 Matela, L. ...........................................307
Demirdjian, D. ................................... 365 Meng, Y..............................................101
Domenech, J....................................... 149 Miao, J................................................207
Duin, S. .............................................. 165 Miura, J. .............................................200
377
I
AUTHOR INDEX (CONT.)
Moya, E.............................................. 220 Sapaty, P...............................................92

Muñoz-Salinas, R............................... 181 Sazonov, V. ........................................284
Muracciole, V. ................................... 311 Schäfer, H...........................................141
Naamane, A. ...................................... 351 Schneider, K.......................................157
Negri, P. ............................................. 359 Schuele, T...........................................157
Nejim, S. ............................................ 296 Schwarz, M. .........................................44
Nikitin, S............................................ 191 Shi, G..................................................207
Nikolos, I. ............................................ 67 Shimada, N.........................................200
Novales, C............................................ 52 Shirai, Y. ............................................200
Nübel, M. ............................................. 44 Sidobre, D. .........................................253
Olguín-Díaz, E. .................................... 19 Skomorowski, M. .................................29
Ouattara, S. ........................................ 195 Sokolov, S. .........................................284
Pacheco, L.......................................... 259 Stanescu, L. ........................................337
Pana, C. .............................................. 244 Štilec, J. ..............................................307
Papa, G............................................... 212 Stoian, V.............................................244
Parc, P. ............................................... 267 Stoica, C. ............................................337
Pardo, D. ............................................ 174 Sugisaka, M..........................................92
Park, C. ................................................ 36 Takács, T. ...........................................271
Parra-Vega, V. ..................................... 19 Takao, Y.............................................323
Pashkevich, A. ..................................... 84 Terziyan, V.........................................191
Paúl, R................................................ 181 Tikanmäki, A......................................226
Pelc, M. .............................................. 307 Tlili, A. ...............................................296
Pérez, C.............................................. 133 Torkar, D. ...........................................212
Petrişor, A. ......................................... 319 Tsourveloudis, N. .................................67
Petrisor, R. ......................................... 319 Ulman, V. ...........................................234
Pirkl, P. .............................................. 307 Vandanjon, P. .....................................341
Pizziconi, D........................................ 333 Vareille, J. ..........................................267
Plainchault, P. .................................... 311 Viet, C. ...............................................275
Pott, P................................................... 44 Vieyres, P. ............................................52
Prevost, L. .......................................... 359 Wagner, A. ...........................................44
Proetzsch, M. ............................. 141, 157 Wang, S. .............................................365
Prokhorov, D...................................... 372 Wenger, P.............................................84
Pyötsiä, J. ........................................... 191 Zaatri, A. ............................................253
Quintía, P. .................................. 149, 292 Zell, A. ...............................................125
Rabhi, A. ............................................ 351 Zheng, Y.............................................101
Regueiro, C. ............................... 149, 292 Zhong, S. ............................................267
Reinoso, O. ........................................ 133 Zhu, P. ................................................207
Rim, S. ............................................... 216
Rodriguez, A........................................ 59
Rodríguez, M. .................................... 292
Rogge, J. .............................................. 76
Röning, J. ................................... 226, 280
Roşca, A............................................. 319
Roşca, D............................................. 319
Sabater, J............................................ 133
378

Robotics and Automation Vol 1

Uploaded by

Copyright:

Available Formats

Robotics and Automation Vol 1

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Robotics and Automation Vol 1

Uploaded by

Copyright:

Available Formats

ICINCO 2007

May 9 – 12, 2007

SPECIAL SESSION CHAIRS .............................................................................................................IV

ORGANIZING AND STEERING COMMITTEES .................................................................................... V

PROGRAM COMMITTEE ............................................................................................................... VII

AUXILIARY REVIEWERS ................................................................................................................XI

SELECTED PAPERS BOOK ........................................................................................................... XIII

CONTENTS ................................................................................................................................ XVII

SPECIAL SESSION CHAIRS

Jean Jacques Loiseau

Paulo Brito, INSTICC, Portugal

Webdesigner and Graphics Production

Secretariat and Webmaster

Gustavo Arroyo-Figueroa, Instituto de Ryszard S. Choras, University of Technology &

Krzysztof Kozlowski, Poznan University of Aníbal Matos, Faculdade de Engenharia da

Igor Paromtchik, RIKEN, Japan Jurek Sasiadek, Carleton University, Canada

A. Fernando Ribeiro, Universidade do Minho, Amanda Sharkey, University of Sheffield, U.K.

Robert Richardson, University of Manchester, U.K. Burkhard Stadlmann, University of Applied

CReSTIC, URCA, France

Juan Andrade Cetto

Institut de Robòtica i Informàtica Industrial, CSIC-UPC, Spain

Université d’Angers, France

Polytechnic Institute of Setúbal / INSTICC, Portugal

REAL TIME DIAGNOSTICS, PROGNOSTICS, & PROCESS MODELING IS-5

SYNCHRONIZATION OF MULTI-AGENT SYSTEMS IS-7

TOWARD HUMAN-MACHINE COOPERATION IS-9

ROBOTICS AND AUTOMATION

ESTIMATION PROCESS FOR TIRE-ROAD FORCES AND VEHICLE SIDESLIP ANGLE

INTEGRATED DESIGN OF A MECHATRONIC SYSTEM - THE PRESSURE CONTROL IN

ON THE FORCE/POSTURE CONTROL OF A CONSTRAINED SUBMARINE ROBOT

FUZZY-SYNTACTIC APPROACH TO PATTERN RECOGNITION AND SCENE ANALYSIS

VISUAL ALIGNMENT ROBOT SYSTEM: KINEMATICS, PATTERN RECOGNITION, AND

DISTURBANCE FEED FORWARD CONTROL OF A HANDHELD PARALLEL ROBOT

THE TELE-ECHOGRAPHY MEDICAL ROBOT OTELO2 - TELEOPERATED WITH A MULTI

RPQ: ROBOTIC PROXIMITY QUERIES - DEVELOPMENT AND APPLICATIONS

EVOLUTIONARY PATH PLANNING FOR UNMANNED AERIAL VEHICLES COOPERATION

A NOVEL STRATEGY FOR EXPLORATION WITH MULTIPLE ROBOTS

CALIBRATION OF QUASI-ISOTROPIC PARALLEL KINEMATIC MACHINES: ORTHOGLIDE

BAYES-BASED OBJECT TRACKING BOOSTED BY PARTICLE SWARM OPTIMIZATION

SMARTMOBILE – AN ENVIRONMENT FOR GUARANTEED MULTIBODY MODELING AND

GRASP CONFIGURATION MATCHING - USING VISUAL AND TACTILE SENSOR

MOTION CONTROL OF AN OMNIDIRECTIONAL MOBILE ROBOT

IMPROVEMENT OF THE VISUAL SERVOING TASK WITH A NEW TRAJECTORY

OBSTACLE DETECTION IN MOBILE OUTDOOR ROBOTS - A SHORT-TERM MEMORY

METHODOLOGY FOR LEARNING VISUAL REACTIVE BEHAVIOURS IN ROBOTICS

FORMAL VERIFICATION OF SAFETY BEHAVIOURS OF THE OUTDOOR ROBOT RAVON

A MODIFIED IMPULSE CONTROLLER FOR IMPROVED ACCURACY OF ROBOTS

COLLABORATIVE CONTROL IN A HUMANOID DYNAMIC TASK

A FUZZY SYSTEM FOR INTEREST VISUAL DETECTION BASED ON SUPPORT VECTOR

DATA INTEGRATION SOLUTION FOR PAPER INDUSTRY - A SEMANTIC STORING

FAST COMPUTATION OF ENTROPIES AND MUTUAL INFORMATION FOR MULTISPECTRAL

PROBABILISTIC MAP BUILDING CONSIDERING SENSOR VISIBILITY

A NEWMARK FMD SUB-CYCING ALGORITHM

MULTI AGENT-BASED ON-LINE DIAGNOSTIC SCHEME OF SUBSTATION IED

SCADA WEB - REMOTE SUPERVISION AND MAINTENANCE OF INDUSTRIAL PROCESSES

PROPERTY SERVICE ARCHITECTURE FOR DISTRIBUTED ROBOTIC AND SENSOR SYSTEMS

ON GENERATING GROUND-TRUTH TIME-LAPSE IMAGE SEQUENCES AND FLOW FIELDS

HELPING INSTEAD OF REPLACING - TOWARDS A SHARED TASK ALLOCATION

FUZZY LOGIC ALGORITHM FOR MOBILE ROBOT CONTROL

ON THE BALANCING CONTROL OF HUMANOID ROBOT