(Lecture Notes in Control and Information Sciences) Krzysztof R. Kozlowski - Robot Motion and Control 2007-Springer (2007)

Lecture Notes
in Control and Information Sciences 360

Editors: M. Thoma · M. Morari
Krzysztof Kozłowski (Ed.)
Robot Motion and Control 2007
123
Series Advisory Board
F. Allgöwer · P. Fleming · P. Kokotivic · A.B. Kurzhanski ·
H. Kwakernaak · A. Rantzer · J.N. Tsitsiklis
Editor
Professor Dr.-Ing. habil. Krzysztof Kozłowski
Poznan University of Technology
Institute of Control and Systems Engineering
ul. Piotrowo 3a
60-965 Poznań
Poland
Krzysztof.Kozlowski@put.poznan.pl
British Library Cataloguing in Publication Data

A catalogue record for this book is available from the British Library
Library of Congress Control Number: 2007928368
Lecture Notes in Control and Information Sciences ISSN 0170-8643

ISBN-13: 978-1-84628-973-6 e-ISBN-13: 978-1-84628-974-3
Printed on acid-free paper
© Springer-Verlag London Limited 2007
MATLAB® and Simulink® are registered trademarks of The MathWorks, Inc., 3 Apple Hill Drive,
Natick, MA 01760-2098, USA. http://www.mathworks.com
Apart from any fair dealing for the purposes of research or private study, or criticism or review, as
permitted under the Copyright, Designs and Patents Act 1988, this publication may only be reproduced,
stored or transmitted, in any form or by any means, with the prior permission in writing of the publishers,
or in the case of reprographic reproduction in accordance with the terms of licences issued by the Copyright
Licensing Agency. Enquiries concerning reproduction outside those terms should be sent to the publishers.
The use of registered names, trademarks, etc. in this publication does not imply, even in the absence of a
specific statement, that such names are exempt from the relevant laws and regulations and therefore free for
general use.
The publisher makes no representation, express or implied, with regard to the accuracy of the information
contained in this book and cannot accept any legal responsibility or liability for any errors or omissions that
may be made.
9 8 7 6 5 4 3 2 1
Springer Science+Business Media

springer.com
Preface
The main motivation in publishing this collection of papers is to present

the most recent results concerning robot motion and control to the ro-
botics community. Forty one original works have been selected out of
60 papers submitted to the Sixth International Workshop on Robot
Motion and Control (RoMoCo’07) Bukowy Dworek, Poland, June 11
to 13, 2007. This Workshop is the sixth in a series of RoMoCo Work-
shops held so far (the previous ones were held in 1999, 2001, 2002,
2004 and 2005). It is an internationally recognized event, technically
co-sponsored by the IEEE Robotics and Automation Society, IEEE
Control Systems Society, IFAC and the Polish Section of the IEEE Ro-
botics and Automation Society Chapter. The Workshop is organized
by the Chair of Control and Systems Engineering of the Poznań Uni-
versity of Technology in Poland. The selected papers went through a
rigorous review procedure and of them got two-three reviews. Based on
the reviewers’ comments most of the papers were corrected and finally
accepted for publication in the Lecture Notes in Control and Informa-
tion Sciences series.
The interest in robot motion and control has remarkably augmented
over recent years. Novel solutions of complex mechanical systems such
as industrial robots, mobile robot, flying robots and their applications
are the evidence of a significant progress in the area of robotics. It
should also be noted that among the objectives of running the Work-
shop was to build a bridge between previous the Eastern European
countries and the Western countries. It is one of the reasons why Ro-
MoCo Workshop takes place in Poland. There is a lot of appreciation of
the robotics field in Poland now and many researchers visiting Poland
have noticed this fact recently.
A careful review procedure resulted in the selection of high qual-
ity papers written by internationally recognized scientists as well as
young talented researchers (some of them Ph.D. students) from differ-
ent countries. Our goal was to encourage young scientists to contribute
VI Preface
to this book showing that many new research groups are being set up
throughout the world. This book should strengthen the relationship
between the new and old members of the European Community.
The members of the International Program Committee have worked
in the area of robotics and automation for many years. They have
experience in various fields of robotics and basically have worked on
control theory with applications to robotics for many years. They took
active part in the reviewing procedure during last months when this
book was being built up.
This book consists of nine parts. The first part deals with control
of nonholonomic systems. The second part is devoted to vision based
control robotic systems. In the third part new control algorithms for
robot manipulators are presented. The fourth part is devoted to new
trends in kinematics and localization methods. Next part deals with
trajectory planning issues for nonholonomic systems. Applications of
rehabilitation robots is the subject of the sixth part. Part seven is
dedicated to humanoid robots. Next part is devoted to application of
robotic systems. Finally, the last part deals with multiagent systems.
The book is addressed to Ph.D. students of robotics and automation,
informatics, mechatronics, and production engineering systems. It will
also be of interest to scientists and researchers working in the above
fields.
I would like to take this opportunity to thank all the reviewers
involved in the reviewing process. I am very grateful to Mr K. Ro-
manowski for his suggestions concerning improvement of English. I am
also grateful to Dr W. Wróblewski for his help and patience and type-
setting of this book.
Mr O. Jackson and Mr A. Doyle, our editors at Springer, are grate-
fully acknowledged for their encouragement in pursuing this project.
Poznań, Poland Krzysztof Kozlowski

April, 2007
Contents
Part I Control of Nonholonomic Systems
1 Modelling and Trajectory Generation of Lighter-

Than-Air Aerial Robots – Invited Paper
Yasmina Bestaoui, Salim Hima . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2 Airship Modelling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.2.1 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.2.2 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.3 Trajectory Generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.3.1 Trim Trajectories . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.3.2 Trim Trajectory Characterization . . . . . . . . . . . . . . . 10
1.3.3 Trim Trajectories Calculation . . . . . . . . . . . . . . . . . . . 12
1.4 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
1.4.1 Straight Forward Trim Flight . . . . . . . . . . . . . . . . . . . 15
1.4.2 Circular Trim Trajectories with Constant Altitude . 15
1.4.3 Helicoidal Trim Trajectories . . . . . . . . . . . . . . . . . . . . 17
1.4.4 Under-Actuation at Low Velocity . . . . . . . . . . . . . . . . 18
1.4.5 Results for the Normalized Time . . . . . . . . . . . . . . . . 23
1.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
2 Control of 3 DOF Quadrotor Model
Tae Soo Kim, Karl Stol, Vojislav Kecman . . . . . . . . . . . . . . . . . . . . . 29
2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
2.2 Modelling of Quadrotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
2.3 Experimental Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
2.4 Control Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
2.4.1 Optimal Control (LQR) . . . . . . . . . . . . . . . . . . . . . . . . 32
VIII Contents
2.4.2 LQR with Gain Scheduling . . . . . . . . . . . . . . . . . . . . . 33

2.4.3 Feedback Linearization . . . . . . . . . . . . . . . . . . . . . . . . . 34
2.4.4 Sliding Mode Control . . . . . . . . . . . . . . . . . . . . . . . . . . 35
2.5 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
2.6 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
3 Internal Model Control-Based Adaptive Attitude
Tracking
Ahmed Al-Garni, Ayman Kassem, Muhammad Shafiq, Rihan
Ahmed . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
3.2 Spacecraft Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
3.3 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
3.4 Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
3.4.1 Plant Stabilization . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
3.4.2 PID Controller Design for the Stabilized Plant . . . . 43
3.5 Adaptive Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
3.5.1 Internal Model Control – System Operation . . . . . . 44
3.5.2 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
3.6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
4 Tracking control of Automated Guided Vehicles
Lotfi Beji, Azgal Abichou . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.2 Modelling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
4.3 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
4.4 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
5 VFO Control for Mobile Vehicles in the Presence of
Skid Phenomenon
Maciej Michalek . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
5.2 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.2.1 Skid Phenomenon . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
5.3 VFO Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
5.3.1 VFO Strategy – Brief Recall . . . . . . . . . . . . . . . . . . . . 60
5.3.2 VFO Control with Skid Compensation . . . . . . . . . . . 61
5.3.3 Skid Computation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
Contents IX
5.5 Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
Part II Vision-Based Control
6 Vision-Based Dynamic Velocity Field Generation for

Mobile Robots
W. Medina-Meléndez, L. Fermı́n, J. Cappelletto, C. Murrugarra,
G. Fernández-López, J.C. Grieco . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
6.2 Problem Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
6.3 Dynamic Velocity Field Generation . . . . . . . . . . . . . . . . . . . . . 70
6.3.1 Vision System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
6.3.2 Initial Velocity Field Generation . . . . . . . . . . . . . . . . 72
6.3.3 Dynamic Velocity Field Modification . . . . . . . . . . . . . 74
6.4 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
6.5 Conclusions and Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . 78
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
7 Zoom Control to Compensate Camera Translation
within a Robot Egomotion Estimation Approach
Guillem Alenyà, Carme Torras . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
7.2 Mapping Contour Deformations to Camera Motions . . . . . . 82
7.3 Generating Zoom Demands . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
7.4 Control and Egomotion Algorithm . . . . . . . . . . . . . . . . . . . . . . 85
7.5 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
7.6 Conclusions and Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . 87
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
8 Two-Finger Grasping for Vision Assisted Object
Manipulation
Umar Khan, Thomas Nierobisch, Frank Hoffmann . . . . . . . . . . . . . 89
8.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89
8.2 Visual Servoing with a Dynamic Set of SIFT Features . . . . . 90
8.3 Grasp Point Selection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
8.4 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95
8.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98
X Contents
9 Trajectory Planning with Control Horizon Based on

Narrow Local Occupancy Grid Perception
Lluis Pacheco, Ningsu Luo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
9.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
9.2 Local Perception Horizon and Trajectory Planning . . . . . . . . 100
9.3 A Practical Approach to WMR with Monocular Image Data101
9.3.1 Image Perception and Physical Constraints . . . . . . . 102
9.3.2 Dynamic Models and Trajectory Control . . . . . . . . . 103
9.4 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
Part III New Control Algorithms for Robot Manipulators
10 Control for a Three-Joint Underactuated Planar

Manipulator – Interconnection and Damping
Assignment Passivity-Based Control Approach
Masahide Ito, Naohiro Toda . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109
10.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109
10.2 IDA-PBC for Underactuated Mechanical Systems . . . . . . . . 110
10.3 Control of a 2Ra -Ru Planar Manipulator by IDA-PBC . . . . . 112
10.3.1 PH Representation of Dynamics . . . . . . . . . . . . . . . . . 112
10.3.2 Applicability of IDA-PBC and Derivation of
Control Law . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113
10.4 Numerical Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115
10.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 117
11 A New Control Algorithm for Manipulators with
Joint Flexibility
Piotr Sauer, Krzysztof Kozlowski . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
11.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
11.2 A new Adaptive Control Algorithm . . . . . . . . . . . . . . . . . . . . . 120
11.2.1 Mathematical Description of the System . . . . . . . . . 120
11.2.2 The New Control Algorithm . . . . . . . . . . . . . . . . . . . . 121
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133
12 An Inverse Dynamics-Based Discrete-Time Sliding
Mode Controller for Robot Manipulators
Andrea Calanca, Luca M. Capisani, Antonella Ferrara, Lorenza
Magnani . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137
Contents XI
12.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137

12.2 The Considered Dynamical Model . . . . . . . . . . . . . . . . . . . . . . 138
12.3 The Inverse Dynamics Method . . . . . . . . . . . . . . . . . . . . . . . . . 139
12.4 A Discrete-Time Sliding Mode Control Approach . . . . . . . . . 139
12.5 An Alternative Discrete-Time Sliding Mode Control
Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141
12.6 Experimental Verification . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143
12.6.1 The Considered Industrial Robot . . . . . . . . . . . . . . . . 143
12.6.2 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . 144
12.7 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145
13 Velocity Tracking Controller for Rigid Manipulators
Przemyslaw Herman, Krzysztof Kozlowski . . . . . . . . . . . . . . . . . . . . . 147
13.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147
13.2 First-order Equations of Motion Containing GVC . . . . . . . . 148
13.3 Velocity Tracking Controller Using GVC . . . . . . . . . . . . . . . . 149
13.4 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155
14 Fixed Point Transformations-Based Approach in
Adaptive Control of Smooth Systems
József K. Tar, Imre J. Rudas,, Krzysztof R. Kozlowski . . . . . . . . . . 157
14.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 157
14.2 Simple Iterative Approaches . . . . . . . . . . . . . . . . . . . . . . . . . . . 159
14.3 Novel Iterative Approaches for SISO Systems . . . . . . . . . . . . 161
14.4 The Mathematical Model of the Cart – Pendulum System
and Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162
14.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164
14.6 Acknowledgment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 165
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 165
15 Driving Redundant Robots by a Dedicated
Clutch-Based Actuator
Anani Ananiev, Thorsten Michelfelder, Ivan Kalaykov . . . . . . . . . . 167
15.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167
15.2 New Method of Actuating Hyper-Redundant Robots . . . . . . 168
15.3 Modeling Magnetic Clutch-Based Actuators . . . . . . . . . . . . . 170
15.4 Controller Design, Simulation and Experiments . . . . . . . . . . 173
15.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 175
XII Contents
16 An Energy-Based Approach Towards Modeling of

Mixed Reality Mechatronic Systems
Yong-Ho Yoo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177
16.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177
16.2 Mixed Reality Bond Graphs . . . . . . . . . . . . . . . . . . . . . . . . . . . 178
16.3 Distributed Mixed Reality Haptic Ball Manipulator . . . . . . . 179
16.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184
Part IV New Trends in Kinematics and Localization Methods
17 Navigation of Autonomous Mobile Robots – Invited

Paper
J.Z. Sasiadek, Y. Lu, V. Polotski . . . . . . . . . . . . . . . . . . . . . . . . . . . . 187
17.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 187
17.1.1 Mapping and Localization . . . . . . . . . . . . . . . . . . . . . . 187
17.1.2 Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 189
17.1.3 Collision Avoidance . . . . . . . . . . . . . . . . . . . . . . . . . . . . 189
17.1.4 Trajectory Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . 189
17.2 Mobile Robot Navigation Through Gates . . . . . . . . . . . . . . . . 190
17.2.1 Problem Description . . . . . . . . . . . . . . . . . . . . . . . . . . . 191
17.2.2 Gate Identification Procedure and Signature
Concept . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192
17.2.3 Experimental Procedure . . . . . . . . . . . . . . . . . . . . . . . . 197
17.2.4 Control System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202
17.2.5 Results and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . 203
17.3 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 206
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 207
18 Kinematic Motion Patterns of Mobile Manipulators
Katarzyna Zadarnowska, Krzysztof Tchoń . . . . . . . . . . . . . . . . . . . . . 209
18.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 209
18.2 Main Idea . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 210
18.3 Kinematic Motion Patterns . . . . . . . . . . . . . . . . . . . . . . . . . . . . 211
18.4 Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 214
18.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 216
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 216
19 Generalized Kinematic Control of Redundant
Manipulators
Miroslaw Galicki . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 219
19.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 219
Contents XIII
19.2 Kinematic Control of Redundant Manipulator . . . . . . . . . . . . 220

19.3 Tackling the Problem of Manipulator Singularity . . . . . . . . . 223
19.4 Computer Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 224
19.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 225
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 226
20 Parametric Representation of the Environment of a
Mobile Robot for Measurement Update in a Particle
Filter
Tahir Yaqub, Jayantha Katupitiya . . . . . . . . . . . . . . . . . . . . . . . . . . . . 227
20.1 Introduction and Related Work . . . . . . . . . . . . . . . . . . . . . . . . 227
20.2 Preliminaries: Particle Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . 229
20.3 Feature Selection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 230
20.3.1 Definition of Features and Other Parameters . . . . . . 230
20.3.2 Data Collection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 231
20.3.3 Statistical Analysis and Bootstrap Feature Selection231
20.4 Multinomial Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233
20.4.1 Extracting the Multinomial Parameters . . . . . . . . . . 233
20.4.2 Measurement Update . . . . . . . . . . . . . . . . . . . . . . . . . . 233
20.5 Experiments and Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 234
20.5.1 Setup and Scenarios for Data Collection . . . . . . . . . . 234
20.5.2 Model Extraction and Results Updates . . . . . . . . . . . 234
20.6 Conclusion and Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . 235
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 236
21 Simulation of a Mobile Robot with an LRF in a 2D
Environment and Map Building
Luka Teslić, Gregor Klančar, Igor Škrjanc . . . . . . . . . . . . . . . . . . . . 239
21.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 239
21.2 Simulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 240
21.2.1 Robot Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 241
21.2.2 Environment Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 241
21.2.3 Laser Range-Finder Model . . . . . . . . . . . . . . . . . . . . . 241
21.3 Mapping Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 243
21.3.1 Integrating the Global Map with the Local Map . . . 243
21.4 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 245
21.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 246
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 246
XIV Contents
Part V Trajectory Planning Issues for Nonholonomic

Systems
22 Lie Algebraic Approach to Nonholonomic Motion

Planning in Cluttered Environment
Pawel Ludwików, Ignacy Dulȩba . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 249
22.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 249
22.2 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 251
22.3 The Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 252
22.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 257
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 257
23 Computationally Efficient Path Planning for Wheeled
Mobile Robots in Obstacle Dense Environments
Husnu Türker Şahin, Erkan Zergeroğlu . . . . . . . . . . . . . . . . . . . . . . . 259
23.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 259
23.2 Kinematic Model and Problem Formulation . . . . . . . . . . . . . . 260
23.3 The Proposed Path Planner . . . . . . . . . . . . . . . . . . . . . . . . . . . 261
23.3.1 Nonholonomic Steering Towards a Desired Target . 261
23.3.2 Obstacle Detection and Avoidance . . . . . . . . . . . . . . . 262
23.3.3 Extension for Large U-Blocks and Complicated
Tunnels . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 264
23.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 267
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 267
24 Piecewise-Constant Controls for Newtonian
Nonholonomic Motion Planning
Ignacy Dulȩba . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 269
24.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 269
24.2 The Newton Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 270
24.3 Simultion Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 274
24.4 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 277
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 277
25 Path Following for Nonholonomic Mobile
Manipulators
Alicja Mazur . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 279
25.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 279
25.2 Mathematical Model of Nonholonomic Mobile
Manipulator of Type (nh, h) . . . . . . . . . . . . . . . . . . . . . . . . . . . 280
Contents XV
25.2.1 Nonholonomic Constraints . . . . . . . . . . . . . . . . . . . . . . 280

25.2.2 Dynamics of Mobile Manipulator with
Nonholonomic Platform . . . . . . . . . . . . . . . . . . . . . . . . 280
25.3 Control Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . 282
25.4 Path Following for the Platform . . . . . . . . . . . . . . . . . . . . . . . . 283
25.4.1 Kinematic Controller – Pomet Algorithm . . . . . . . . . 284
25.5 Path Following for the Manipulator . . . . . . . . . . . . . . . . . . . . . 286
25.6 Dynamic Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 287
25.7 Simulation Study . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 289
25.8 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 291
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 292
Part VI Rehabilitation Robotics
26 On Simulator Design of the Spherical Therapeutic

Robot Koala
Krzysztof Arent, Marek Wnuk . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295
26.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295
26.2 Koala: Therapeutic Ball-robot . . . . . . . . . . . . . . . . . . . . . . . . 296
26.3 Virtual Koala and Sensory Signals Modelling . . . . . . . . . . . 298
26.4 Implementation Issues . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 300
26.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 301
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 302
27 Development of Rehabilitation Training Support
System using 3D Force Display Robot
Yoshifumi Morita, Akinori Hirose, Takashi Uno, Masaki Uchida,
Hiroyuki Ukai, Nobuyuki Matsui . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 303
27.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 303
27.2 Rehabilitation Training Support System . . . . . . . . . . . . . . . . . 304
27.3 3D Force Display Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 305
27.4 Control Algorithms of Rehabilitation Training . . . . . . . . . . . 306
27.4.1 Control Algorithm of Resistance Training . . . . . . . . 306
27.4.2 Control Algorithm Simulating Sanding Training . . . 307
27.4.3 Teaching/training Function Algorithm . . . . . . . . . . . 308
27.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 310
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 310
28 Applying CORBA Technology for the Teleoperation
of Wheeeler
Michal Pytasz, Grzegorz Granosik . . . . . . . . . . . . . . . . . . . . . . . . . . . . 311
28.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 311
XVI Contents
28.2 Presentation of Wheeeler . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 311

28.2.1 The Main Concept . . . . . . . . . . . . . . . . . . . . . . . . . . . . 312
28.2.2 Distributed Controllers . . . . . . . . . . . . . . . . . . . . . . . . . 313
28.3 Client-Server Communication . . . . . . . . . . . . . . . . . . . . . . . . . . 314
28.3.1 Short Review . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 314
28.3.2 CORBA Implementation . . . . . . . . . . . . . . . . . . . . . . . 315
28.4 Simulation Time and Real Time Considerations . . . . . . . . . . 317
28.5 Further Developments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 318
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 318
Part VII Humanoid Robots
29 Educational Walking Robots

Teresa Zielińska, Andrzej Chmielniak, Lukasz Jańczyk . . . . . . . . . . 321
29.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 321
29.2 Educational Walking Robots – Mechanical Structures . . . . . 322
29.2.1 Hexapod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 322
29.2.2 Quadruped . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 323
29.3 New Control System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 325
29.4 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 326
29.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 327
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 328
30 Humanoid Binaural Sound Tracking Using Kalman
Filtering and HRTFs
Fakheredine Keyrouz, Klaus Diepold, Shady Keyrouz . . . . . . . . . . . . 329
30.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 329
30.2 Previous Localization Technique . . . . . . . . . . . . . . . . . . . . . . . . 331
30.3 Enhanced Localization Algorithm . . . . . . . . . . . . . . . . . . . . . . 332
30.4 Kalman Filtering and ROI Extraction . . . . . . . . . . . . . . . . . . . 334
30.5 Simulation and Experimental Results . . . . . . . . . . . . . . . . . . . 335
30.5.1 Stationary Sound Sources . . . . . . . . . . . . . . . . . . . . . . 335
30.5.2 Moving Sound Sources . . . . . . . . . . . . . . . . . . . . . . . . . 338
30.6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 339
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 340
31 Mechatronics of the Humanoid Robot ROMAN
Krzysztof Mianowski, Norbert Schmitz, Karsten Berns . . . . . . . . . . 341
31.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 341
31.2 The Humanoid Robot ROMAN . . . . . . . . . . . . . . . . . . . . . . . . 342
31.3 Design Concept and Construction . . . . . . . . . . . . . . . . . . . . . . 398
31.3.1 Upper Body . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 398
Contents XVII
31.3.2 Artificial Eyes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 345

31.3.3 Neck . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 346
31.4 Robot Control Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . 346
31.5 Conclusion and Outlook . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 348
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 348
Part VIII Applications of Robotic Systems
32 Assistive Feeding Device for Physically Handicapped

Using Feedback Control
Rahul Pandhi, Sumit Khurana . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 351
32.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 351
32.2 Upper Arm Orthotic – Mobile Mount . . . . . . . . . . . . . . . . . . . 352
32.2.1 Slave Arm Unit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353
32.2.2 Master/Interface Unit . . . . . . . . . . . . . . . . . . . . . . . . . . 353
32.2.3 Transmission System . . . . . . . . . . . . . . . . . . . . . . . . . . 354
32.2.4 Control System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 354
32.2.5 Advantages . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 355
32.3 Upper Arm Orthotic – Stationary Mount . . . . . . . . . . . . . . . 355
32.4 Power-Assist in Human Worn Assistive Devices . . . . . . . . . . 356
32.5 Virtual Prototyping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 357
32.6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 358
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 359
33 Design and Control of a Heating-Pipe Duct
Inspection Mobile Robot
Piotr Dutkiewicz, Michal Kowalski, Bartlomiej Krysiak,
Jaroslaw Majchrzak, Mateusz Michalski, Waldemar Wróblewski . . 361
33.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 361
33.2 Chassis Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 362
33.2.1 Supporting Frame and Suspension . . . . . . . . . . . . . . . 363
33.2.2 Driving System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 364
33.2.3 Steering system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 364
33.3 Control System Concept and Implementation . . . . . . . . . . . . 364
33.4 On-board Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 367
33.5 Energy Distribution Module . . . . . . . . . . . . . . . . . . . . . . . . . . . 369
33.6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 370
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 370
XVIII Contents
34 Measurement and Navigation System of

the Inspection Robot RK-13
Piotr Dutkiewicz, Marcin Kielczewski, Dariusz Pazderski,
Waldemar Wróblewski . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 371
34.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 371
34.2 Measurement and Control System Modules . . . . . . . . . . . . . . 372
34.2.1 Communication System – Hardware Layer . . . . . . . . 372
34.2.2 Measurement System . . . . . . . . . . . . . . . . . . . . . . . . . . 373
34.2.3 Vision System for Inspection and Navigation . . . . . . 376
34.3 Localization of the Duct Inspection Robot with Use of
the Vision System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 377
34.4 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 380
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 380
35 Architecture of Multi-Segmented Inspection Robot
KAIRO-II
C. Birkenhofer, K. Regenstein, J.-M. Zöllner, R. Dillmann . . . . . . 381
35.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 381
35.2 System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 382
35.2.1 Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 383
35.2.2 Control Architecture: UCoM(Universal Controller
Module) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 384
35.2.3 Hybrid Impedance Control . . . . . . . . . . . . . . . . . . . . . 385
35.2.4 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 386
35.3 Integration and Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 387
35.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 388
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 388
36 The Project of an Autonomous Robot Capable to
Cooperate in a Group
Tomasz Buratowski, Tadeusz Uhl, Grzegorz Chmaj . . . . . . . . . . . . . 391
36.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 391
36.2 The 2-wheeled Mobile Robot Description . . . . . . . . . . . . . . . . 392
36.2.1 The Basic Assumptions Related to Model
Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 392
36.2.2 The Basic Assumptions Related to Path Planning . 392
36.3 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 8
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 8
Contents XIX
37 End-Effector Sensors’ Role in Service Robots

Cezary Zieliński, Tomasz Winiarski, Krzysztof Mianowski,
Andrzej Rydzewski, Wojciech Szynkiewicz . . . . . . . . . . . . . . . . . . . . . 401
37.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 401
37.2 Robot Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 403
37.3 Position-Force Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 405
37.4 The Gripper and Its Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . 407
37.5 Effector Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 408
37.6 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 410
37.7 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 411
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 412
Part IX Multiagent Systems
38 Detecting Intruders in Complex Environments with

Limited Range Mobile Sensors
Andreas Kolling, Stefano Carpin . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 417
38.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 417
38.2 Related Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 418
38.3 The Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 419
38.3.1 Weighted Graph Clearing . . . . . . . . . . . . . . . . . . . . . . 420
38.3.2 Weighted Tree Clearing . . . . . . . . . . . . . . . . . . . . . . . . 420
38.4 Investigation of Performance . . . . . . . . . . . . . . . . . . . . . . . . . . . 423
38.5 Discussion and Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 423
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 424
39 High-level Motion Control for Workspace Sharing
Mobile Robots
Elżbieta Roszkowska . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 427
39.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 427
39.2 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 427
39.3 Deterministic Finite State Automata . . . . . . . . . . . . . . . . . . . . 429
39.4 Discretization of Robot Motion Processes . . . . . . . . . . . . . . . . 429
39.5 DFSA Model of Distributed High-Level Control . . . . . . . . . . 431
39.6 Coordination of Multiple Robot Motion . . . . . . . . . . . . . . . . . 433
39.7 Deadlock Avoidance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 434
39.8 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 435
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 435
XX Contents
40 Urban Traffic Control and Path Planning for

Vehicles in Game Theoretic Framework
István Harmati . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 437
40.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 437
40.2 The Traffic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 438
40.3 Urban Traffic Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 439
40.4 Path Planning Algorithms for Vehicles . . . . . . . . . . . . . . . . . . 441
40.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 443
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 443
41 A Low-Cost High Precision Time Measurement
Infrastructure for Embedded Mobile Systems
Kemal Köker, Kai-Steffen Hielscher, Reinhard German . . . . . . . . . 445
41.1 Introduction and Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . 445
41.2 Architecture of the Application . . . . . . . . . . . . . . . . . . . . . . . . 446
41.2.1 Robocup F-180 Small Size League . . . . . . . . . . . . . . . 446
41.3 Time Synchronisation with GPS . . . . . . . . . . . . . . . . . . . . . . . . 447
41.3.1 Existing Infrastructure . . . . . . . . . . . . . . . . . . . . . . . . . 447
41.3.2 PPS-API . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 448
41.3.3 Wireless Dissemination of Time Signal . . . . . . . . . . . 448
41.4 Measurements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 449
41.4.1 Results of First Test Run . . . . . . . . . . . . . . . . . . . . . . . 449
41.4.2 Techniques for Improvement . . . . . . . . . . . . . . . . . . . . 450
41.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 451
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 451
1
Modelling and Trajectory Generation of
Lighter-Than-Air Aerial Robots – Invited
Paper
Yasmina Bestaoui and Salim Hima
Laboratoire IBISC, Université d’Evry Val d’Essonne, 91025 Evry, France

Yasmina.Bestaoui@ibisc.fr, Salim.Hima@ibisc.fr
1.1 Introduction
Lighter-than-air vehicles suit a wide range of applications, ranging from
advertising, aerial photography, and survey work tasks. They are safe,
cost-effective, durable, environmentally benign and simple to operate.
Since their renaissance in early 1990’s, airships have been increasingly
considered for varied tasks such as transportation, surveillance, freight
carrier, advertising, monitoring, research, and military roles. What
makes a vehicle lighter than air is the fact that it uses a lifting gas
(i.e. helium or hot air) in order to be lighter than the surrounding air.
The principle of Archimedes applies in the air as well as under water.
The difference between airships and balloons is that: balloons simply
follow the direction of the winds; in contrast, airships are powered and
have some means of controlling their direction. Non rigid airships or
pressure airships are the most common form nowadays. They are ba-
sically large gas balloons. Their shape is maintained by their internal
overpressure. The only solid parts are the gondola, the set of propeller
(a pair of propeller mounted at the gondola and a tail rotor with hori-
zontal axis of rotation) and the tail fins. The envelope holds the helium
that makes the airship lighter than air. In addition to the lift provided
by helium, airships derive aerodynamic lift from the shape of the enve-
lope as it moves through the air. The most common form of a dirigible
is an ellipsoid. It is a highly aerodynamic profile with good resistance
to aerostatics pressure.
The objective of the first part of this paper is to present a model
of an autonomous airship: kinematics and dynamics, taking into ac-
count the wind effect. The second part of this article is concerned with
methods of computing a trajectory in space that describes the desired
K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 3–28, 2007.
springerlink.com
c Springer-Verlag London Limited 2007
4 Y. Bestaoui and S. Hima
motion. The contribution of this paper is the characterization of tra-

jectories, considering the under-actuation.
This paper consists of 5 sections. Section 1.2 presents the kinematics
followed by the dynamics. Section 1.3 introduces trim trajectories then
explicits the relationship between trajectory generation algorithms and
under-actuation. Simulation results are discussed in Section 1.4 and
finally some concluding remarks are given in Section 1.5.
1.2 Airship Modelling
For kinematics, Euler angles are presented. For dynamics, a mathe-

matical description of a dirigible flight must contain the necessary in-
formation about aerodynamic, structural, and other internal dynamic
effects (engine, actuation) that influence the response of the airship to
the controls and external atmospheric disturbances. The airship is a
member of the family of under-actuated systems because it has fewer
inputs than degrees of freedom. In some studies such as [8, 16, 20],
motion is referenced to a system of orthogonal body axes fixed in the
airship, with the origin at the center of volume assumed to coincide
with the gross center of lift. The model used was written originally for
a buoyant underwater vehicle [8]. It was modified later to take into ac-
count the specifics of the airship [16, 20]. In [3], the origin of the body
fixed frame is the center of gravity.
1.2.1 Kinematics
In aeronautics, the need to define appropriate coordinate systems arises

from two considerations [25]. First, there may be some particular coor-
dinate system in which the position and velocity of the aircraft “make
sense”. For navigation we are concerned with position and velocity with
respect to the reference frame Rf considered to be inertial, whereas for
aircraft performance aerodynamic frame Ra is used extensively in flight
mechanics; both at the conceptual level with flight equations of motion
and at the practical level through the aircraft’s air data probe and other
sensors. The aircraft’s true air speed va is referenced to the aeronautic
frame.
The position of all points belonging to the aircraft body with re-
spect to Rf can be completely defined by knowing the orientation of
a fixed frame Rm to the aircraft body and the position of its origin
with respect to Rf . The choice of the body fixed frame origin can be
made at the center of gravity, center of volume or also at the nose of
1 Modelling and Trajectory Generation of Aerial Robots 5
the envelope [16, 8]. These three frames are classically considered in
the derivation of the kinematics and dynamics equations of motion of
the aerial vehicles. There are many ways to describe finite rotations
between frames. Direction cosines, quaternions, Euler angles, can serve
as examples. Some of these groups of variables are very close to each
other in their nature.
The usual minimal representation of orientation is given by a set
of three Euler angles which, assembled with the three position coordi-
nates, allow the description of a rigid body. A 3 × 3 direction cosine
matrix (of Euler parameters) is used to describe the orientation of the
body (achieved by 3 successive rotations) with respect to some fixed
frame reference. This parametrisation is a three-parameter set corre-
sponding to well known quantities like the yaw ψ, pitch θ, and roll φ,
which are directly measured by the attitude sensors such as inclinome-
ter and magnetic compass. Adopting this formulation, the rotation ma-
trix R can be given by:
 
cψ cθ −sψ cφ + cψ sθ sφ sψ sφ + cψ sθ cφ
R = sψ cθ cψ cφ + sψ sθ sφ −cψ sφ + sψ sθ cφ  , (1.1)
−sθ cθ sφ cθ cφ
where the following notation is used: cx = cos(x) and sx = sin(x). A

singularity of this transformation exists for θ = ± π2 .
During practical operations of the airship, the pitch angle of θ = ± π2
is not likely to be encountered. The rotation matrix R is an orthogonal
matrix, RT R = I3×3 , where I3×3 is a 3 × 3 identity matrix. The set of
such matrices constitutes the special orthogonal matrix group SO(3)
defined as:

SO(3) = R ∈ R3×3 , RT R = I3×3 and det (R) = 1 . (1.2)
The configuration of the airship is completely defined by associating

the orientation matrix and the Rm frame origin position vector, η1 =
(x, y, z)T , with respect to Rf using homogenous matrix:

R η1
A= . (1.3)
03×3 1
The set of all such matrices is called the special Euclidean group of
rigid-body transformations in three dimensions, denoted SE(3), defined
by:

R η1

SE(3) = A A = 3
, R ∈ SO(3), η1 ∈ R . (1.4)
03×3 1
SE(3) is a Lie group. Given a curve C(t) : [−a, a] → SE(3), an element

S(t) of the Lie algebra se(3) can be associated to the tangent vector
Ċ(t) at an arbitrary configuration A(t) by:

Sk(ν2 ) RT η˙1
S(t) = A−1 (t)Ȧ(t) = , (1.5)
0 0
where Sk(ν2 ) = RT (t)Ṙ(t) is a 3 × 3 skew symmetric operator on a

vector defined by:
 
0 −r q
Sk((p, q, r)T ) =  r 0 −p (1.6)
−q p 0
such that ∀x ∈ R3 : Sk(y)x = y × x.

A curve on SE(3) physically represents a motion of the rigid body.
If (ω(t), ν(t)) is the pair corresponding to S(t), then ν2 = (p, q, r)T
physically corresponds to the angular velocity of the rigid body, while
ν1 = (u, v, w)T is the linear velocity of the origin Om of the frame Rm
with respect to the inertial frame, both expressed in the body fixed
frame. The kinematic relationship between the different velocities is
given by:
η̇1 R(η2 ) 0 ν1
= , (1.7)
η̇2 0 J(η2 ) ν2
where R is the rotation matrix defined by Equation (1.1) and J(η2 ) is
defined by:
 
1 sin(φ) tan(θ) cos(φ) tan(θ)
J(η2 ) = 0 cos(φ) − sin(φ)  . (1.8)
0 sin(φ)/ cos(θ) cos(φ)/ cos(θ)
In presence of wind gust with velocity νw , the aerodynamic forces
and moment depend on the airship relative linear velocity ν1a with
respect to wind velocity, ν1a = ν1 − νw , and hence we define νA =
(ν1a , ν2 )T .
1.2.2 Dynamics
A mathematical description of airship’s flight dynamics needs to em-

body the important aerodynamic, structural and other internal dy-
namics effects that combine to influence the response of the airship to
control inputs and external atmospheric disturbances [2, 5, 13, 15, 16].
In this section, analytic expressions for the forces and moments on the
dirigible are derived. The forces and moments are referred to a sys-
tem of body-fixed axes, centered at the airship center of gravity. There
are in general two approaches in deriving equations of motion. One is
to apply Newton’s law and Euler’s law, which can give some physi-
cal insight through the derivation. The other one is more complicated;
it provides the linkage between the classical framework and the La-
grangian or Hamiltonian framework. In this paper, we apply Newton’s
laws of motion to relate the applied forces and moments to the re-
sulting translational and rotational accelerations. We will make in the
sequel some simplifying assumptions: the Earth fixed reference frame
is inertial, the gravitational field is constant, the airship is supposed to
be a rigid body [1], meaning that it is well inflated, the density of air
is supposed to be uniform, and the mass of the airship is considered
to be unchanged. Taking into account these suppositions, the analytic
expressions of forces and moments acting on the airship expressed in
body-fixed frame Rm , can be given by:
M ν̇ = C(ν)ν + τs + τa + τp ,
(1.9)
η̇ = J(η2 )ν,
where:

mI3×3 −mSk(CG)
• M = is the airship inertia matrix, m is
mSk(CG) IC
 
Ixx 0 Ixz
the airship total mass, and IC =  0 Iyy 0  is the moment of
Izx 0 Izz
inertia matrix;

03×3 Sk(ν1 + ν2 × CG)
• C(ν) = is the Coriolis-
Sk(ν1 + ν2 × CG) Sk(IC ν2 )
centrifugal matrix associated to the airship body mass; it depends
on the airship local velocities;

FG + FB
• τs = is the static tensor due to the weight
CG × FG + CB × FB
and buoyancy forces; this tensor can be simplified to the effect of the
weight force or to the buoyancy force if the origin of the local frame
has been chosen at the gravity center of mass or at the buoyancy
point, respectively;
• τp is the propulsion system tensor;
• τa is the aerodynamic tensor due to the atmosphere-airship interac-
tion and depends on the relative velocity.
When the airship moves, the air close to its body is moved. Contrary
to the other aerial vehicles, the mass of the displaced air is close to that
of the airship and consequently cannot be neglected. The displaced air

mass is known as “added mass” or “virtual mass”. The added mass ma-
trix A is, in general, an extra-diagonal matrix. The inertial effects of this
added mass constitute the first component of the aerodynamic tensor.
Another part of the aerodynamic forces is coming from the translation-
rotation and rotation-rotation coupling motions and can be assimilated
to Coriolis-centrifuge effects associated to the added mass and can also
be represented as a damping effect, i.e. D(ν2 )νA . Moreover, the pure
translation motion action tensor, τ1 = (01×3 , (ν1 × Aν1 )T )T , is usually
ignored in the other aerial applications. Due to the importance of the
added mass, in the case of the airship, this tensor must be included.
In addition, a pure translation-depending aerodynamic tensor is con-
sidered. These phenomena come from the forces and moment resulting
from the distribution of the pressure around the airship body and also
the friction forces due to the viscosity of the air [21, 23, 26]. This tensor
can expressed as:  
Cx
 Cy 
 
1 2  Cz 
τat = ρνa Sref  
 Cl  , (1.10)
2  
Cm 
Cn
where Cx are the aerodynamic coefficients depending on the geomet-
rical shape of the airship and the positions of the control surfaces,
rudders, and elevators.
These coefficients are calculated in two ways. The first one is a
purely experimental procedure which consists of collecting data in a
wind tunnel; the second one is analytical based on some geometric
quantities procedure [15]. Hence, the aerodynamical tensor, defined by
the contribution of all aerodynamic phenomena, can be synthetised as:
τa = −Aν̇A + D(ν2 )νA + τ1 + τat . (1.11)
Actuators provide the means for maneuvering the airship along its
course. An airship is propelled by thrust. Propellers are designed to
exert thrust to drive the airship forward. The most popular propulsion
system layout for pressurized non-rigid airships is twin ducted pro-
pellers mounted on either side of the envelope bottom. Another one
exists in the tail for torque correction and attitude control. The AS200
airship is an under-actuated system with two types of control: forces
generated by thrusters and angular input µm controlling the direction
of the thrusters
 
Fm sin µm
 Ft 
 
Fm cos(µm )

τp =  .
0 
 
 Pmz sin µ 
PTx FT
The complete mechanical-aerodynamical airship model can be given

by:
Ma ν̇ = −Aν̇w + C(ν)ν + τs + D(ν2 )νA + τ1 + τat + τp ,
(1.12)
η̇ = J(η2 )ν,
where Ma = M + A is the total mass.

In aerostatics hovering (floating), the airship stability is mainly af-
fected by its center of lift in relation to the center of gravity. The air-
ship’s center of gravity can be adjusted to obtain either stable, neutral,
or unstable conditions. Putting all weight on the top would create a
highly unstable airship with a tendency to roll over in a stable position.
In aerodynamics flight, stability can be affected by fins and the gen-
eral layout of the envelope. Control inertia can be affected by weight
distribution, dynamic (static) stability, and control power (leverage)
available.
1.3 Trajectory Generation
Trajectory generation is one of the important subjects of research in

robotics. It constitutes the intelligent part in designing non assisted
or autonomous robotic entities. When the robot is under-actuated, i.e.
there are less control inputs than degrees of freedom, the set of fea-
sible trajectories will be restricted and, consequently, the problem of
trajectory generation becomes more complicated. In the literature, a
large number of techniques have been proposed to deal with mobile
robots governed by kinematic equations and subject to non-holonomic
constraints. Most of these techniques exploit the inherent structure of
those systems and lead to very restricted algorithms. In the last decade,
in hope to resolve the problem of trajectory generation for kinodynamic
systems, i.e. systems with non negligible dynamics, non-deterministic
techniques have been created, we can refer to Probabilistic Road Map
(PRM) and Rapidly exploring Random Tree (RRT) [22, 19]. These
techniques can lead to non-smooth trajectories causing an oscillatory
behavior of the non-controlled variables such as a roll dynamics in the

case of the airship.
In general, trajectory planning for under-actuated nonlinear systems
is a very difficult task and demands substantial computing resources.
In order to reduce this complexity, it is possible to construct a nominal
trajectory by sequencing a finite number of primitives from a finite set
of basic primitives selected a priori [14]. In this scope, the problem
of trajectory planning is hierarchised into two levels. The lower level
consists of the computation of the set of basic primitives, to be used
by the higher level to construct the nominal trajectory by a convenient
sequencing strategy. In the sequel of this section we focus on the lower
level, and in particular we are interested in the trim trajectory charac-
terization for the airships. Care must be taken in the selection of the
basic primitives to preserve the structural proprieties of the dynamics
of the airship, especially controllability. Sufficient conditions are given
in [13, 7] to guarantee minimal set of trim trajectories to ensure a
controllability in the symmetric space.
1.3.1 Trim Trajectories
Trim trajectories take a particular place in aeronautic applications

[4, 11, 12, 13, 15, 18]. They correspond to forces and moments equi-
librium in body-fixed frame under a fixed control configuration. More
precisely, they correspond to the equilibrium points of dynamic equa-
tion (1.12). Hence, there are two good reasons, apart from their his-
torical importance, for algebraically deriving the small-perturbation
equations. First, the aerodynamic parameters needed for the linear
equations can be estimated relatively quickly. Second, the algebraic
small-perturbation equations provide a great deal of insight into the
relative importance of the various aerodynamic derivatives under dif-
ferent flight conditions and their effect on the stability of the aircraft
motion. We devote the remainder of this section to give a mathematical
characterization of the trim trajectories.
1.3.2 Trim Trajectory Characterization
As mentioned before, trim trajectories are characterized by the sta-

tionarity of the body-fixed velocity components. This condition can be
mathematically formalized by:
ν̇(t) ≡ 0 for ∀t ∈ [t0 , t1 ]. (1.13)

Starting with this condition, and focusing on the angular velocity kine-
matics transformation we have:
ν2 = J2 (η2 )−1 η˙2 . (1.14)
By deriving the above equation with respect to time and using condition
(1.13), we can find:
ṗ =φ̈ − ψ̈ sin(θ) − ψ̇ cos(θ)θ̇ = 0,
q̇ =θ̈ cos(φ) − θ̇ sin(φ)φ̇ + ψ̈ sin(φ) cos(θ)
− ψ̇ cos(φ) cos(θ)ψ̇ + ψ̇ sin(φ) sin(θ)θ̇ = 0, (1.15)
ṙ = − θ̈ sin(φ) − θ̇ cos(φ)φ̇ + ψ̈ cos(φ) cos(θ)
− ψ̇sin(φ) cos(θ)φ̇ − ψ̇ cos(φ) sin(θ)θ̇ = 0.
In addition, the controls are assumed to be constant. Based on the
dynamics equation (1.12), all forces and moments acting on the airship
depending on the velocity vector are constant except the vector of the
aerostatic forces and moments g(η2 ), which depends on the attitude
variables, the roll φ, and the pitch θ angles. It follows that, in order to
guarantee the stationarity of this vector, the roll θ and pitch φ angles
must be constant. Hence, Equation (1.15) can be simplified to:
p = − ψ̇ sin θ,
q =ψ̇ sin(φ) cos(θ), (1.16)
r =ψ̇ cos(φ) cos(θ).
The components of the body-fixed angular velocity vector depend on
the roll angle φ, pitch angle θ, and the yawing rate ψ̇.
In addition, by manipulating the linear velocity kinematics trans-
formation and assuming that there is no tail rotor, it is possible to
characterize the geometry of the trim trajectories as follows:
 
a
η̇1 = J1 (η2 )ν1 = Rz (ψ)  b  , (1.17)
c
 
a
where b  = Ry (θ)Rx (φ)ν1 and

c
   
a cos(ψ̇t) − b sin(ψ̇t) V cos(γ) cos(ψ̇t + ψ0 )
η̇1 =  a sin(ψ̇t) − b cos(ψ̇t)  =  V cos(γ) sin(ψ̇t + ψ0 )  . (1.18)
c −V sin(γ)
√
a2 +b2
γ = cos−1 Ve represents the flight path angle. Ve = kv1 k is the
navigation speed, and ψ0 is the initial airship heading orientation.
By integrating the above equation, the geometric characterization
of trim trajectories can be described by:
 V   
cos(γ) sin(ψ̇t + ψ0 ) x0
 ψ̇ 
η1 =  − V cos(γ) cos(ψ̇t + ψ0 )  +  y0  . (1.19)
ψ̇
−V sin(γ)t z0
[x0 , y0 , z0 ]T is the initial position of the airship. It is possible to para-

metrize the trim trajectory by the vector Te = [φe , θe , ue , ve , we , ψ̇e ],
where the subscript e denotes the equilibrium values.
1.3.3 Trim Trajectories Calculation

The above kinematic analysis of the trim trajectories shows that their
geometry depends on the body-fixed linear velocity vector ν1e , the roll
angle φe , pitch angle θe , and the rate of yaw angle ψ̇e . The choice
of these quantities should satisfy the dynamic equations, the controls
saturation, and envelope protection constraints.
Control Saturation Constraints

Airship controls belong to a specific range due to limitations of the
power, angle of vectorization, and deflection angles of control surfaces.
Using Eq. (1.12) it is straightforward to express the limitation con-
straints imposed on the force Fme and the angle of vectorization µme
in terms of trim vector components Te and angles of deflection of control
surfaces as follows:
   
Fme cos(µme ) Fx (νe , η2e , δe )
 0   Fy (νe , η2e , δe ) 
   
 Fme sin(µme )   Fz (νe , η2e , δe ) 

τp =   
0 =Mx (νe , η2e , δe ) ,
   
Fme cos(µme )Pmz+Fme sin(µme )Pmx  My (νe , η2e , δe )
0 Mz (νe , η2e , δe )
p (1.20)
Fmmin 6 Fme = Fx2 (νe , η2e , δe ) + Fz2 (νe , η2e , δe ) 6 Fmmax , (1.21)
µmin 6 µme = arctan2(Fz (νe , η2e , δe ), Fx (νe , η2e , δe )) 6 µmax . (1.22)
Limitations of angles of deflection of control surfaces are given by:
δemin 6 δee 6 δemax , (1.23)
δrmin 6 δre 6 δrmax . (1.24)
Under-Actuation Constraints
The control of the airship is performed by 4 actuators (i.e. throt-

tle, vectored angle, rudders, and elevators). Hence, in this case, the
under-actuation degree is 2. The action of rudders and elevators on the
dynamic equation appears in a nonlinear manner (1.20), making the
derivation of the under-actuated constraints very difficult. To bypass
this problem, we choose to consider the angles of the control surfaces
(rudders and elevators) in the optimization variables as will be shown
in the sequel of this paper. In this case, the under-actuated constraints
related to the propulsion systems can be derived as follows:
τp = g1 Fme cos(µme ) + g2 Fme sin(µme ), (1.25)
where g1 = (1, 0, 0, 0, Pmz , 0)T and g2 = (0, 0, 1, 0, Pmx , 0)T are the con-
trol vector fields. Let G = (g1 , g2 ) ∈ R6×2 define the subspace spanned
by the control vector fields and G T ∈ R4×6 be the orthogonal subspace
to the actuated space defined by:
G ⊥ G = 04×2 , (1.26)
 
−Pmz 0 −Pmx 010
 0 1 0 0 0 0
G⊥ = 
 0
. (1.27)
0 0 1 0 0
0 0 0 001
The under-actuated constraints related to the propulsion system are
given by:
 
My (νe , η2e , δe )−Pmz Fx (νe , η2e , δe )−Pmx Fz (νe , η2e , δe )
 Fy (νe , η2e , δe ) 
G ⊥ τp = 

 = 0.

Mx (νe , η2e , δe )
Mz (νe , η2e , δe )
(1.28)
Other Constraints
The last type of constraints considered in the calculation of trim trajec-

tory concerns the geometry of the path and the speed of the airship Ve .
The geometric parameters are the flight path angle γe and the turning
radius Re . These quantities are set to a desired values by the users to
satisfy some properties such as controllability [13, 7]. It is possible to
consider also some constraints on the attack αe and sideslip βe angles
guaranteeing the flight envelope protection. These constraints are given

by:
Ve
Re = cos(γe ), (1.29)
|ψ̇e |
√ !
a2 + b2
γe = cos−1 , (1.30)
Ve
p
Ve = u2e + ve2 + we2 , (1.31)
!
we
αmin 6 αe = sin−1 p 6 αmax , (1.32)
ue + we2
2

−1 ve
βmin 6 βe = sin 6 βmax . (1.33)
kν1e k
Taking into account the constraints previously exposed, the calcu-
lation of the trim conditions is determined by solving a set of nonlinear
equalities and inequalities for the vector Te and control surface vec-
tor δ = [δe , δr ]T that makes the body-fixed accelerations vector null,
i.e. ν̇ ≡ 0. A convenient way to do this is to formulate an optimiza-
tion problem to choose the better solution that minimizes the energy
consumed during the execution of the trim trajectory. This choice is
straightforward because the velocity and controls are constant, and
hence the energy is proportional to the arrival time. The expression of
the energy is given by: Z tf
2
E= Fm dt. (1.34)
t0
The controls are constrained to take fixed values. It follows that:

2
E = Fm (tf − t0 ) (1.35)
without loss of generality and under the constancy of the velocity norm,
we can restrict the previous expression to the energy consumed in unit
of time, i.e. tf − t0 = 1 as:
2
E = Fm . (1.36)
1.4 Simulation Results
In this section, we present some simulation results obtained by the

proposed algorithm. The controls are constrained to 0 6 Fm 6 20N ,
−45◦ 6 δe 6 45◦ , −45◦ 6 δr 6 45◦ and the navigation velocity is set to

Ve = 6ms−1 . In the simulation presented below, initially the airship is
at rest. When the controls associated to required trim conditions are
applied, the state of the airship is shown to converge to the true values.
The optimization problem is solved using the Matlab(r) Optimization
Toolbox.
1.4.1 Straight Forward Trim Flight
Here we present the simulation results corresponding to straight for-

ward trim flight situation. In this case the radius of gyration is set
to Re = ∞. Figure 1.1 depicts the 3D trim trajectories corresponding
to flight path angles γe = −10◦ , 0, 10◦ . In Figure 1.2 we sketch the
evolution of the components of trim vector Te in the case of γe = 0.
100
50
0
z
−50
−100
10 600
5
400
0
200
−5
y −10 0 x
Fig. 1.1. 3D trim trajectory for straight forward trim flight
1.4.2 Circular Trim Trajectories with Constant Altitude
We present in this section the simulation results corresponding to the

circular trim trajectories with constant altitude, i.e. γe = 0. In Fig. 1.3,
we depict the 3D circular trim trajectories with a radius of gyration
Body−fixes linear velocity components in (m/s)

5
ue
1
ve
0
we
−1
0 10 20 30 40 50 60 70 80 90 100
t (s)
Fig. 1.2. Body-fixed linear velocity components evolution
R=40m. Due to the symmetry of the dynamics of the airship with respect
to the yaw angle ψ, it is possible to find the opposite circular trim
trajectories by inverting the rudder angle δr . Figure 1.4 illustrates the
evolution of the trim vector components Te corresponding to circular
trim trajectory.
−100
−50
0 −100
50
100
−50
y
x 0
50
100
−20
−10
z
10
20
Fig. 1.3. 3D circular trim trajectories with constant altitude

Body−fixed linear velocity components (m/s)

5
ue
4
2
ve
1
0
we
−1
0 10 20 30 40 50 60 70 80 90 100
t (s)
1.4.3 Helicoidal Trim Trajectories
In this section we present the more general case. It corresponds to the

helical trim trajectories shown in Fig. 1.5 corresponding to Re = 40m,
◦ ◦
γe = −10 (blue) and γe = 10 (red). Figure 1.6 illustrates the evolution
of the trim vector Te components in the case of helical trim trajectory
◦
with γe = 10 .
100
50
0 100
−50
−100
50
−50
−100
−50
50
100
Fig. 1.5. 3D helical trim trajectories

Body−fixed linear velocity components evolution in (m/s)

6
u
5
2
w v
1
−1
0 10 20 30 40 50 60 70 80 90 100
t (s)
1.4.4 Under-Actuation at Low Velocity
In this paragraph, nominal trajectories are characterized. The model

is supposed perfect and any perturbation such as wind or sensor dis-
turbance is neglected. At low velocity, the surface control is ineffective
so we can neglect aerodynamical forces. The three equality constraints
deriving from the under-actuation are sought. Considering the dynam-
ics of the airship and its propulsion, the following dynamics equations
can be written:
 
Fm sin µm
f = M ν̇1 + ν2 × M ν1 + b =  Ft ,
Fm cos(µm )
  (1.37)
0
τ = I ν̇2 + ν2 × Iν2 + ν1 × M ν1 + β =  Pmz sin µ .
PTx FT
The roll moment being zero (see (1.37)), τ1 = 0 gives:

Ixx ℵ + (M11 − M33 )uv + (Ixx − Izz ) −θ̇Sφ + ψ̇Sφ Cθ
(1.38)
−θ̇Sφ + ψ̇Cφ Cθ + Dp φ̇φ̇Sθ + zb BCθ Sφ = 0.
z f1 = τ2 gives
In addition, Pm
Iyy ℵ+(M11 −M33 )uw+(I11 −I33 ) φ̇− ψ̇Sθ −θ̇Sφ + ψ̇Cφ Cθ

+ Dq θ̇Cφ + ψ̇Sφ Cθ − zb BSθ + zb BSθ + Pmz M11 u̇
(1.39)
+ Pmz M22 w θ̇Cφ + ψ̇Sφ Cθ − Pmz M22 v −θ̇Sφ + ψ̇Cφ Cθ
+ Pmz Du u + Pmz (mg − B)Sθ = 0,
where ℵ = θ̈Cφ − θ̇φ̇Sφ + ψ̈Sψ Cθ + ψ̇ φ̇Cφ Cθ − ψ̇ θ̇Sφ Sθ .

The third constraint can be derived from the constraint PTx f2 = τ3
and can expressed as:

ℵ ′ − −
Izz + (M22 M11 )uv + (Iyy Ixx ) φ̇ψ̇Sθ θ̇Cφ + ψ̇Sφ Cθ

+ Nr −θ̇Sφ + ψ̇Cφ Cθ + PTx M22 v̇ − PTx M22 w φ̇ − ψ̇Sθ

+PTx M11 u −θ̇Sφ + ψ̇Cφ Cθ +PTx Dv v− PTx (mg−B)Sφ Cθ = 0,
(1.40)
where ℵ′ = −θ̈Sφ − θ̇φ̇Cφ + ψ̈Cφ Cθ − ψ φ̇Sφ Cθ − ψ̇ θ̇Cφ Sθ .

The following approach is considered. The variations of the roll and
pitch angles as well as the longitudinal velocity are imposed and the
influence of the under-actuation on the variations of the yaw angle, the
lateral and vertical velocities are studied.
The first equality constraint (see (1.38)) is equivalent to the resolu-
tion of an ordinary differential equation of the from:
a(t)ψ̈ + b(t)ψ̇ 2 + c(t)ψ̇ + d(t) = 0, (1.41)
where
a(t) = Ixx Sθ ,
b(t) = (Izz − Iyy )(Cφ Sφ Cθ2 ),
(1.42)
c(t) = −Sθ Dp + 2(Izz − I22 )Cθ Cφ2 − (Ixx + Izz − Iyy )θ̇Cθ ,
d(t) = −zb BCθ Sθ − Ixx φ̈ − Dp φ̇ + (Izz − Iyy )θ̇ 2 Sθ Cθ .
If Ξ(t) = ψ̇(t) then the non-autonomous generalized logistic equation

must be solved:
a(t)Ξ̇(t) + b(t)Ξ(t)2 + c(t)Ξ(t) + d(t) = 0. (1.43)
The third equality constraint can be written as:
w(t) = α0 + α1 u + α3 uv + α4 v̇, (1.44)
where
α′0 (Iyy − I11 ) − PTx (mg − B)Cθ Sφ + Dr (ψ̇Cθ Cφ − θ̇Sφ )
α0 =
−PTx (ψ̇Sθ − φ̇)
(1.45)
Izz (ψ̈Cθ Cφ − θ̈Sφ − ψ̇Sθ Cθ Sφ − θ̇φ̇Cφ )
+ ,
−PTx M22 (ψ̇Sθ − φ̇)
(ψ̇Cθ Cφ − θ̇Sφ )M11 Dv

a1 = − , a2 = − ,
(ψ̇Sθ − φ̇)M22 (ψ̇Sθ − φ̇)M22
(1.46)
M11 − M22 1
a3 = , a4 = −
PTx M22 (ψ̇Sθ − φ̇) (ψ̇Sθ − φ̇)M22
and

α′0 = −ψ̇ 2 Sθ Cθ Sφ − ψ̇ θ̇Sθ Cφ + ψ̇ φ̇Cθ Sφ + θ̇φ̇Cφ . (1.47)
The second equality constraint gives

β0 + β1 u + β2 u2 β3 uv + β4 u2 + β5 uv̇ + β6 v + β7 v̇β8 u̇ = 0, (1.48)
where
= β0′ + β3′ α0 ,
β0 β5 = β ′ α4 ,
= β1′ + α1 β3′ + α0 β5′
β1 β6 = β2′ + α2 β3′ ,
= β5′ α1 ,
β2 β7 = α4 β3′ ,
= β5′ α2 + α3 β3′ ,
β3 β8 = β4′ ,
= β5′ α3 ,
β4

β0′ = Iyy β0′′+(Ixx−Izz )β0′′′+Dq ψ̇Cθ Sφ + θ̇Cφ −Bzb Sθ +Pmz (mg−B)Sθ ,
β0′′ = ψ̈Cθ Sφ + θ̈Cθ + ψ̇ φ̇Cθ Cφ − ψ̇ θ̇Sθ Sφ − θ̇ φ̇Sφ ,
β0′′′ = −θ̇φ̇Sφ + ψ̇ φ̇ + ψ̇ θ̇Sθ Sφ − ψ̇ 2 Cθ Sθ ,
β1′ = Pmz Xu ,

β2′ = −Pmz M22 ψ̇Cθ Cφ − θ̇Sφ ,

β3′ = Pmz M22 ψ̇Cθ Sφ + θ̇Cφ ,
β4′ = Pmz M11 ,
β5′ = (M11 − M22 ).
The differential equation
aψ̈ + bψ̇ 2 + cψ̇ + d = 0, (1.49)
where
a = −Ixx Sθ , b = (Izz − Iyy )Sφ Cθ Cφ Cθ ,

c = −Dp Sθ , d = −zb BCθ Sφ ,
admits an analytical general solution.

By using the method of separation of variables and integration by
partial fractions, in the constant coefficient case, logistic equation can
be solved and the behavior of all solutions is analyzed [6, 9, 10, 17, 24]:
For φ = 0; ψ(t) = ψ0 e−Lp t/Ix x (1.50)

s
Bzb
For θ = 0; ψ(t) = t + ψ0 (1.51)
Cφ(Izz − Iyy )
For the particular case, where ψ̇ is constant, classical trim trajectories

are encountered
• ψ̇ is constant:  
φ = cst = φ0
 θ = cst = θ0  . (1.52)
ψ = ψ0 t
The first equality constraint becomes a second-order polynomial
equation:
bψ̇ 2 + cψ̇ + d = 0. (1.53)
The third equality constraint gives
w(t) = α0 + α1 u + α2 v + α3 uv + α4 v̇, (1.54)
where
−ψ̇ 2 Sθ Cθ Sφ (Iyy − Ixx ) + Dr ψ̇Cθ Cφ
α0 =
PTx ψ̇Sθ M22
(1.55)
−PTx (mg − B)Cθ Sφ + Izz ψ̈Cθ Cφ
+ ,
PTx ψ̇Sθ M22
Cθ Cφ M11 Dv
α1 = − , α2 = , (1.56)
Sθ M22 M22 ψ̇Sθ
M11 − M22 1
α3 = , α4 = , (1.57)
PTx M22 ψ̇Sθ ψ̇Sθ
while the second equality constraint gives
β0 + β1 u + β2 u2
v=− (1.58)
β6 + β3 u + β4 u2
for u̇ = v̇ = 0. Otherwise,
β0 + β1 u+ β2 u2 + β3 uv + β4 u2 v + β5 uv̇ + β6 v + β7 v̇ + β8 u̇ = 0. (1.59)
When u̇ = v̇ = ẇ = 0, we retrieve the classical kinematics equations
of the trim equations.
• ψ̇ is not constant: in this paragraph, the roll and pitch angles are
assumed to have linear variations:
θ = θ̇0 t + θ0 , φ = φ̇0 t + φ0 .
When the coefficients of the non-autonomous logistic equation are
no longer constant, no explicit solutions can be found in general
and the equilibrium point may become unstable. For a study to
be complete, the existence of stable periodic or stable bounded so-
lutions is an essential part of qualitative theory of this differential
equation, in order to determine non trivial solutions and study their
behavior. Nkashama proved that the logistic equation with positive
non-autonomous bounded coefficients has exactly one bounded so-
lution that is positive and does not tend to zero.
Solving the first equality constraint, the roll moment being null,
for all t, implies Lp φ̇0 = 0 → φ̇0 = 0. Rearranging the first equality
constraint with this requirement gives θ̇Cφ0 Sφ0 = 0. Three cases are
possible: θ̇0 = 0, φ0 = 0, or φ0 = π/2.
The first case: trim trajectories have already been studied in
paragraph 1.3.1. If the roll angle is null, the following differential
equations must be solved

Cθ
ψ̈ + ψ̇ a + bθ̇0 = 0, (1.60)
Sθ
where a = Lp /Ixx , b = −(Izz − Iyy − Izz )/Ixx , and the following
derivative ψ̇ is obtained
− aθ
θ̇0 Sθ0 θ0b Sθ−b e θ̇0
ψ̇(t) = − . (1.61)
− cosh aθ θ˙
0
+ sinh aθ0
θ˙
0 0
The third case φ0 = π/2 gives the following differential equations:

Cθ Cθ
ψ̈ + ψ̇ a1 + a2 θ̇0 + a3 = 0, (1.62)
Sθ Sθ
where a1 = Lp /Ixx , a2 = (Ixx − Iyy + Izz )/Ixx , a3 = Bzb /Ixx .

The third equality constraint gives
w = δ1 + δ2 uv + δ3 u + δ4 v + δ5 u̇, (1.63)
where
Iyy + Izz − Ixx Nr Cθ Izz ψ̈Cθ
δ1 = θ̇0 − − ,
PTx M22 PTx Sθ M22 PTx ψ̇Sθ M22
M22 − M11 Cθ M22

δ2 = − , δ3 = − ,
PTx ψ̇Sθ M22 Sθ M22
Yv 1
δ4 = − , δ5 = − .
ψ̇Sθ M22 ψ̇Sθ
Once the yaw angle is calculated, the linear and angular velocities
are determined as well as the 3D path.
1.4.5 Results for the Normalized Time
In this paragraph, four cases are presented for a normalized simulation

time = 1.
Case 1 2 3 4
φ0 π/12 π/12 π/12 π/12
φ̇0 0 0 0 0
θ0 π/6 π/6 π/6 π/6
θ̇0 0 0.1 0 0.1
u 1 1 1 1
u̇ 0 0 0 0
Figure 7 8 9 10
For each case, four subplots are presented: the first one presents the
trajectory in space, the second one the variation of the yaw angle ψ,
the third – linear velocities v and w and finally the fourth – angular
velocities p, q, r.
Figure 1.7 presents results for case 1. The 3D trajectory represents
a part of a helix with constant torsion and curvature. The yaw angle
has a linear variation while the angular and linear velocities are con-
stant. After a transient phenomenon, in case 2 (see: Fig. 1.8), the yaw
angle has a linear variation and the path tends to a classical helix with
constant curvature and torsion. In case 3, Fig. 1.9 shows that the an-
gular and linear velocities have a slight non linear variation. After a
0
0.4
−0.05
0.2
psi
−0.1
0 −0.15
0
2
−1 1 −0.2
−2 0 0 0.5 1
1.5 0.2
1
0.1
VW
0.5
PQR
0
0
−0.1
−0.5
−1 −0.2
0 0.5 1 0 0.5 1
Fig. 1.7. Case 1
0
0 −0.05
−0.1
−0.1
psi
−0.15
−0.2
0.2 −0.2
2
0 1 −0.25
−0.2 0 0 0.5 1
1 0.2
0.5 0.1
VW
0 0
PQR
−0.5 −0.1
−1 −0.2
−1.5 −0.3
0 0.5 1 0 0.5 1
Fig. 1.8. Case 2

0
1 −0.1
−0.2
0
psi
−0.3
−1
1 −0.4
2
0 1 −0.5
−1 0 0 0.5 1
4 0.5
2 0
PQR
VW
0 −0.5
−2 −1
0 0.5 1 0 0.5 1
Fig. 1.9. Case 3
0
0 −0.1
−0.2
−0.2
psi
−0.3
−0.4
1 −0.4
2
0 1 −0.5
−1 0 0 0.5 1
2 0.5
1
PQR
VW
0 0
−1
−2 −0.5
0 0.5 1 0 0.5 1
Fig. 1.10. Case 4
certain transition time, they tend to have permanent values. For case
4, shown in Fig. 1.10, when the derivative of the longitudinal velocity
is non-zero, the nonlinear phenomenon is amplified.
1.5 Conclusions
In the first part of this paper, we have discussed dynamic modeling

of small autonomous blimps. Blimps are highly interesting study ob-
jects due to their stability properties. Here, motion is referenced to a
system of orthogonal body axes fixed in the airship, with the origin
assumed to coincide with the center of gravity. The equations of mo-
tion are derived from the Newton-Euler approach. The second part of
this paper addresses the problem of characterizing continuous paths in
3D, taking into account under-actuation. Three differential algebraic
equations must be solved as there era six degrees of freedom and three
inputs. The constraint on the yaw angle is in the form of a generalized
logistic equation, while the other constraints are differential algebraic
equations in v and w, when the variations of the longitudinal velocity
u, and the pitch and roll angles are imposed. The role of the trajectory
generator is to generate a feasible time trajectory for the aerial robot.
Once the path has been calculated in the Earth fixed frame, motion
must be investigated and reference trajectories determined taking into
account actuators constraints. This is the subject of our actual research.
This methodology can be applied to other types of UAV, taking into
account their characteristics: for fixed wing aircraft or helicopter, the
added mass and inertia are neglected.
References
1. N. Azouz, Y. Bestaoui, O. Lemaı̂tre. Dynamic analysis of airships with

small deformations, In: Proc. 3rd IEEE Workshop on Robot Motion and
Control, Bukowy Dworek, 2002, pp. 209-215.
2. Y. Bestaoui, T. Hamel. Dynamic modeling of small autonomous blimps,
In: Proc. Int. Conference on Methods and Models in Automation and
Robotics, Miedzyzdroje, Poland, 2000, pp. 579-584.
3. Y. Bestaoui, S. Hima. Some insights in path planning of small au-
tonomous blimps, Archives of Control Sciences, vol. 11, 2001, pp. 139-166.
4. Y. Bestaoui, S. Hima, C. Sentouh. Motion planning of a fully actuated
unmanned air vehicle, In: Proc. AIAA Conference on Navigation, Guid-
ance, and Control, Austin, TX, 2003.
5. Y. Bestaoui, L. Beji, A. Abichou. Modelling and control of small au-
tonomous airships, In: Modelling and control of mini flying machines, P.
Castillo, R. Lozano & A. Dzul (Eds.), Springer, 2005.
6. Y. Bestaoui. Nominal Trajectories of an autonomous under-actuated air-

ship, International Journal of Control, Automation and Systems, 2006,
pp. 395-404.
7. E. Frazzoli. Robust hybrid control for autonomous vehicle motion plan-
ning, PhD Thesis, MIT, Cambridge, MA, 2001.
8. T. Fossen. Guidance and control of ocean vehicle, Wiley, 1996.
9. G.G. Galanis, P.K. Palamides. Global positive solutions of a generalized
logistic equation with bounded and unbounded coefficients, Electronic
Journal of Differential Equations, no. 119, 2003, pp. 1-13.
10. R. Grimshaw. Non-linear ordinary differential equations, CRC Press,
1993.
11. S. Hima. Y. Bestaoui. Motion generation on trim trajectories for an au-
tonomous underactuated airship, In: Proc. 4th International Airship Con-
ference, Cambridge, England, 2002.
12. S. Hima, Y. Bestaoui. Time optimal paths for lateral navigation of an
autonomous underactuated airship, In: Proc. AIAA Conference on Nav-
igation, Guidance and Control, Austin, Texas, 2003.
13. S. Hima. Planification de trajectories d’un dirigeable autonome, PhD the-
sis, Univ. of Evry, France, 2005.
14. S. Hima, Y. Bestaoui. Trim trajectories characterization for an unmanned
autonomous airship, In: IEEE/RSJ, Int. Conf. on Intelligent Robots and
Systems IROS’06, Taiwan, 2006.
15. E. Hygounenc, P. Souères, S. Lacroix. Dynamic and aerodynamic mod-
eling of a small size airship, Proc. of the 5th workshop on Electronics,
Control, Modeling, Measurements and Signals, Toulouse, France, 2001.
16. E. Hygounenc. Modélisation et commande d’un dirigeable pour le vol au-
tonome, PhD thesis, LAAS, Univ. of Toulouse, France, 2003.
17. D. Jiang, J. Wei, B. Zhang. Positive periodic solutions of functional differ-
ential equations and population models, Electronic Journal of Differential
Equations, no. 71, 2002, pp. 1-13.
18. I. Kaminer, A. Pascoal, E. Hallberg, C. Silvestre. Trajectory tracking for
autonomous vehicles: an integrated approach to guidance and control,
AIAA J. of Guidance, Control & Dynamics, 1998.
19. L. Kavraki, J.C. Latombe. Randomized Preprocessing of Configuration
Space for Fast Path Planning, Proc. IEEE Int. Conf. on Robotic and
Automation, vol. 3, 1994, pp. 2138-2145.
20. G.A. Khoury, J.D. Gillet eds. Airship technology, Cambrigde University
Press, 1999.
21. H. Lamb. On the motion of solids through a liquid. Hydrodynamics,
Dover, New York (1st edition 1876), 6th edition 1945, pp. 120-201.
22. S.M. Lavalle. Rapidly exploring random trees: A new tool for path plan-
ning. TR 98-11, Computer Science Dept, Iowa State University, 1998.
23. M. Munk. The aerodynamic forces on Airship Hulls, NACA report

No. 184, 1924.
24. M.N. Nkashama. Dynamics of logistic equations with non autonomous
bounded coefficients, Electronic Journal of Differential Equations, no. 02,
2000, pp. 1-8.
25. B.L. Stevens. F. Lewis. Aircraft control and simulation, 2nd edition, Wi-
ley, 2003.
26. J.S. Turner. Buoyancy effects in fluids, Cambridge University Press, 1973.
2
Control of 3 DOF Quadrotor Model
Tae Soo Kim, Karl Stol, and Vojislav Kecman
Department of Mechanical Engineering, The University of Auckland,

Auckland, New Zealand
tkim020@ec.auckland.ac.nz, {k.stol, v.kecman}@auckland.ac.nz
2.1 Introduction
Unmanned aerial vehicles (UAVs) comprise various types of aircrafts

such as conventional fixed-wing aircraft, helicopters, blimps, and air-
ships. Among these, helicopters are classified as planar vertical take
off and landing (PVTOL) aircraft by which it means that unlike a
fixed-wing aircraft, it can take off and land in a limited space, hover
in the air, and move sideways and backwards. This superior maneuver-
ability allows performing important roles in many areas, which conven-
tional aircraft could not achieve. The type of their useful work includes:
dangerous applications such as in a war, victim rescue and volcano
monitoring, where other types of vehicle are inaccessible, commercial
application such as film making, and agricultural applications, farm
monitoring and spreading chemicals [1]. The demand for UAVs keeps
increasing. Unfortunately these exclusive maneuverability advantages
give a big complexity and instability in its dynamics, hence making it
hard to control. The development of a UAV is challenging, and it is an
emerging area in nonlinear control study among researchers.
A few researchers focussed on applying various control techniques
to a quadrotor. Lozano et al. [2] approached global stabilization of a
PVTOL vehicle using Lyapunov analysis, designing a simple nonlin-
ear controller by analysing boundedness and convergence of each state.
Bouabdallah et al. [3, 4] built a micro VTOL autonomous robot, ’OS4’.
OS4 was a three DOF model controlling only rotations. The Lyapunov
theorem was used for controller design. Later they explored the appli-
cation of two different techniques, PID and linear quadratic (LQ), to
OS4. OS4 at near-hover condition was tested, neglecting gyroscopic ef-
fects from the rigid body and propeller rotation, i.e. removing all cross
springerlink.com
30 T.S. Kim, K. Stol, and V. Kecman
couplings. Yang et al. [5] studied motion control of a quadrotor using

time-optimal control (TOM). With TOM, the problem was to find in-
puts that would move the system from its initial configuration to a
desired final configuration in minimum time.
Design and choice of sensors are an important aspect in UAV design.
Pounds et al. [8] worked with blade designs to optimize the thrust gen-
eration and dynamic stability. Airfoil design and material properties of
blades were studied and the flapping behaviour of a blade were analysed
by adapting an existing mathematical model. Altug et al. [6, 7] studied
motion control of a quadrotor through visual feedback using a camera.
At the University of Auckland, a number of undergraduate projects
have been undertaken to study control of helicopter models. In 2003,
R. Murphy and T. Hay [9] designed and controlled a rig that replicated
pitching motion of a helicopter using a PID controller. In 2004 W. Zhao
and T. Kim [11] worked on single DOF altitude control of a helicopter.
Also in the same year, A. Stojanovic [10] constructed a 2-DOF heli-
copter model controlling pitch and yaw using a PLC (programmable
logic controller). In 2006, D. Campbell and L. D’Souza [12] attempted
to create a free-flying semi-autonomous quadrotor helicopter. With a
micro controller programmed with a PD controller, the quadrotor was
able to hover in the air for a few seconds, yet it was not stable enough
for a free flight. Limitations of classical control on a complex dynamic
plant were observed. In this research we aimed to develop control al-
gorithms to stabilize an unstable quadrotor plant and implement this
on an experimental rig. Four different control techniques are simulated
and their performances are evaluated.
2.2 Modelling of Quadrotor
The Quadrotor in Fig. 2.1 can be modeled as a 6-DOF rigid body,

with three translations, movement along X, Y, and Z coordinates, and
three rotations, pitch, roll, and yaw, which are rotations about X, Y,
and Z axes, respectively. The quadrotor is an underactuated system;
there are fewer individual inputs than the DOF to be controlled. Some
states are controlled through other states. The movement along the Y
axis is controlled by the pitch angle φ. Increasing the force F4 relative
to F2, while the sum of all four thrust balances the weight mg results in
rotation in φ, induces the movement of the body along the Y axis, the
axis in body fixed frame B in Fig. 2.1. In a similar way, the movement
along the X axis is controlled by the roll angle θ. Movement along
the vertical Z axis occurs by increasing the thrusts from all the four
2 Control of 3 DOF Quadrotor Model 31
rotors so that the collective thrust exceeds the weight of the rig. While
doing this, the thrust from each rotor must balance the thrust from the
opposite rotor. Rotors 1 & 3 rotate clockwise and rotors 2 & 4 rotate
counter-clockwise to counter-balance the gyroscopic moments causing
the rigid body rotation about the Z axis.
Fig. 2.1. Quadrotor configuration
Prior to simulation, a mathematical model for the quadrotor is de-

rived. For the derivation, the notation from [3] is followed. The rota-
tional dynamics of the quadrotor is expressed as:

.. . . Iy − Iz Jp . l 2 2
φ = θψ − θ(Ω2 + Ω4 − Ω1 − Ω3 ) + b(Ω4 − Ω2 ), (2.1)
Ix Ix Ix

.. . . Iz − Ix Ip . l
θ = φψ +φ(Ω2 + Ω4 − Ω1 − Ω3 ) + b(Ω32 − Ω12 ), (2.2)
Iy Iy Iy

.. . . Ix − Iy l
ψ = θφ + d(Ω22 + Ω42 − Ω12 − Ω32 ), (2.3)
Iz Iz
where Ix , y, z are the rotational inertias of the body, Jp is the rotational
inertia of the propeller, Ω is the angular speed of the rotor, l is the
rotor position from the centre, b and d are thrust and drag coefficients,
respectively.
The assumptions for this model are:
• the body is rigid and symmetrical;
• the centre of mass and the body fixed frame origin coincide;
• the propellers are rigid, i.e. no blade flapping occurs.
As the equations show there is coupling present between the rotational
speeds of the body.
2.3 Experimental Setup
An experimental rig, as shown in Fig. 2.2, replicating the attitude of

the quadrotor is designed to apply control algorithm. The design spec-
ification for the rig is:
• A 3-DOF model must fully replicate the rotational motion of the
quadrotor. The translational DOFs are removed.
• The centre of mass of the rig must coincide with the centres of the
three rotations. This is to ensure the resulting motions are pure
rotations.
• The operating range for pitch and roll are ±40◦ and ±180◦ for the
yaw.
Three optical incremental encoders, HP HEDS5700, are chosen for
measuring individual rotations. A dSpace control board DS1104 is used
for data acquisition and produces PWM control signals for the motors
along with Simulink and Control Desk. The DS1104 supports two in-
cremental encoder inputs and four PWM pulse generation. The extra
encoder is interfaced through a digital bit I/O port. The motors, gears,
propellers, and carbon fibre arms come from a commercial quadrotor
design, the Dranganflyer V [13].
Fig. 2.2. Quadrotor experimental rig
2.4 Control Design
2.4.1 Optimal Control (LQR)
Linearization of the nonlinear quadrotor plant is the first step for de-
riving a linear control law. The plant is linearized about an equilibrium
point at which the three orientation angles φ, θ, ψ and its velocities

φ̇, θ̇, ψ̇ are zero and the angular speeds of the four rotors are equal. The
linearization results in:
    
∆φ̇ 000100 ∆φ
 ∆θ̇  
  0 0 0 0 1 0 
  ∆θ 

  
 ∆ψ̇   0 0 0 0 0 1   ∆ψ 
 

 =  ∆φ̇  +
 ∆φ̈   0 0 0 0 0 0
  
 
 ∆θ̈   0 0 0 0 0 0   ∆θ̇ 
∆ψ̈ 000000 ∆ψ̇
  (2.4)
0 0 0 0
 0 0 0 0  
  ∆Ω1
 0 0 0 0 
   ∆Ω2 
.

 0 −2b Ilx Ωss 0 2b Ix Ωss 
l
  ∆Ω3 
 −2b l Ω 0 2b Ily Ωss 0 
 Iy ss  ∆Ω4
−2d Ilz Ωss 2d Ilz Ωss −2d Ilz Ωss 2d Ilz Ωss
The subscript ss denotes ’steady state’.
2.4.2 LQR with Gain Scheduling
Small sphere approximating approach for linearization is implemented

with gain scheduling. Linearization is performed continuously to re-
linearize about every operating point along the state trajectory [14].
The resulting matrix form is the same as above having the correspond-
ing matrices as given below:
 
000 1 0 0
0 0 0 0 1 0 
 
0 0 0 0 0 1 
 
 − − −
A=
0 0 0 0 I1 ψ̇ −I4 Ω I1 θ̇ 
 , and B = (2.5)
 − − −
 
 0 0 0 I2 ψ̇ +I5 Ω 0 I2 φ̇ 
 − − 
000 I3 θ̇ I3 φ̇ 0
 
0 0 0 0
 0 0 0  0
 
 0 0 0  0
 − − − − 
 − − 
 I4 θ̇ −(I4 θ̇ +2bI6 Ω2 ) I4 θ̇ −I4 θ̇ +2bI6 Ω4  ,
 
 − − − − 
 −(I φ̇ +2bI Ω− ) I5 φ̇
−
−I5 φ̇ +2bI7 Ω3 I5 φ̇ 
 5 7 1 
− − − −
−2dI8 Ω1 2dI8 Ω2 2dI8 Ω3 −2dI8 Ω4
(2.6)
where I1 = (Iy −Iz )/Ix , I2 = (Iz −Ix )/Iy , I3 = (Ix −Iy )/Iz , I4 = Jp /Ix ,
I5 = Jp /Iy , I6 = l/Ix , I7 = l/Iy , I8 = l/Iz . The bar above a symbol
denotes its current value. This is more accurate linearization of the
plant, but recalculating the state matrix, input matrix, and control
gain K for every step is computationally expensive. The control law
becomes u = −K(t)x, where K(t) is the time-varying control gain
matrix.
2.4.3 Feedback Linearization
Using feedback linearization, nonlinear terms are cancelled out by the

control input. The derived control inputs are:
. .
Ix Iy − Iz Jp .
U1 = (− θ ψ + θ(Ω2 + Ω4 − Ω1 − Ω3 ) − k1 x), (2.7)
l Ix Ix

Iy . . Iz − Ix Jp .
U2 = (− φ ψ − φ(Ω2 + Ω4 − Ω1 − Ω3 ) − k2 x), (2.8)
l Iy Iy

Iz . . Ix − Iy
U3 = (− θ φ − k3 x), (2.9)
l Iz
where
U1 = F4 − F2 , U2 = F3 − F1 , U3 = db (F2 + F4 − F1 − F3 ). (2.10)
F1 to F4 are the thrust forces from the four rotors as shown in Fig. 2.1.
Equation (2.10) can be resolved into the individual forces using pseudo
inversion.
2.4.4 Sliding Mode Control

The following sliding mode controller is developed to drive all the states
to zero. The terms in the first bracket of Eqs. (2.11) through (2.13) can-
cel out the nonlinearities, while the following term with sign switches
control input to keep the system in a stable manifold [15].

Ix Iy − Iz Jp
U1 = − θ̇ψ̇ − θ̇(Ω2 + Ω4 − Ω1 − Ω3 ) + α1 φ̇
l Ix Ix
− K1 sign(φ̇ + α1 φ), (2.11)

Iy Iz − Ix Jp
U2 = − φ̇ψ̇ + φ̇(Ω2 + Ω4 − Ω1 − Ω3 ) + α2 θ̇
l Iy Iy
− K2 sign(θ̇ + α2 θ), (2.12)

Iz Ix − Iy
U3 = − θ̇φ̇ + α3 ψ̇ − K3 sign(ψ̇ + α3 ψ). (2.13)
l Iz
Two control parameters α and K in sliding mode control are tuned by
simulating a range of parameter combinations for the minimum ITSE
(Integral of Time multiplied by Square of Error).
2.5 Simulations
Simulations were run in Simulink for all the four previously derived
controllers. The controllers are set to regulate all states. The initial
conditions are: φ = 0.5, θ = 0.3 rad, ψ = −0.2 rad. The control para-
meters for each controller are tuned to return the least possible ITSE
index in state regulation. For numerical comparisons the performance
index ITSE is used with total control effort. Each controller is tuned to
return the least possible ITSE index. Figure 2.3 shows the simulation
result for the four types of controllers in state regulation in pitch ori-
entation. Figure 2.4 shows the responses with uncertainty introduced
to plant parameters.
Tables 2.1 and 2.2 summarize the performance of the four differ-
ent control techniques. With accurate plant parameters, sliding mode
controller returns the best result. It is evident that a better response
is obtained with larger control effort but relatively small percentage
changes in total control effort indicate that the effort required to im-
prove orientation control is much smaller than that for balancing the
gravitational force. On the other hand, with model uncertainty intro-
duced, the performance of sliding mode controller deteriorates and the
performance of LQR with gain scheduling becomes outstanding.
Fig. 2.3. Simulation result for four controllers in state regulation
Fig. 2.4. Simulation result for controllers with model uncertainty

Table 2.1. ITSE values and total control efforts in state regulation
Controller ITSE index % difference Σ|u(t)| % difference

LQR 0.3018 0 479.3 0
Gain Schedule 0.2491 -17.5 482.7 +0.7
Feedback Lin. 0.3291 +9.1 472.9 -1.3
Sliding Mode 0.2186 -27.6 495.7 +3.4
Table 2.2. ITSE values and total control efforts with model uncertainty
Controller ITSE index % difference Σ|u(t)| % difference

LQR 0.3180 0 479.7 0
Gain Schedule 0.2320 -27.0 481.8 +0.4
Feedback Lin. 0.3488 +9.7 473.0 -1.4
Sliding Mode 0.3078 -3.2 496.3 +3.5
2.6 Conclusions
In this paper, four different control techniques: LQR, LQR with gain
scheduling, feedback linearization, and sliding mode control were de-
rived and simulated on a 3-DOF quadrotor model. Compared under
ITSE criteria, sliding mode control produced the best result with accu-
rate plant model, while LQR with gain scheduling showed robustness
against the model uncertainty. It is concluded that nonlinear control
techniques have greater dependency on accurate model estimation. The
quadrotor model is currently undergoing a gain tuning stage. The simu-
lation results will be tested on the experimental rig for real-time result
and further comparison is to be done. Our experimental rig can be
further improved in the future by reducing the weight and using other
sensor combination for a complete 6-DOF free-flying quadrotor.
References
1. Sugiura R, Fukagawa T, Noguchi, N (2003) Field Information Sys-
tem Using an Agricultural Helicopter towards Precision Farming. Proc
IEEE/ASME Int. Conf. on Advanced Intelligent Mechatronics 1073–1078
2. Lozano R, Castillo P, Dzul A (2004) Global stabilization of the PVTOL:
real - time application to a mini-aircraft. Int. Journal of Control 77- 8:
735–740
3. Bouabdallah S, Noth A, Siegwart R (2004) PID vs LQ Control Tech-
niques Applied to an Indoor Micro Quadrotor. Proc. IEEE Int. Conf. on
Intelligent Robots and Systems
3. Bouabdallah S, Noth A, Siegwart R (2004) PID vs LQ Control Tech-

niques Applied to an Indoor Micro Quadrotor. Proc. IEEE Int. Conf. on
Intelligent Robots and Systems
4. Bouabdallah S, Murrieri P, Siegwart R (2004) Design and Control of an
Indoor Micro Quadrotor. Proc. Int. Conf. on Robotics and Automation.
5. Yang C, Lai L, Wu C (2006) Time-Optimal Control of a Hovering Quad-
Rotor Helicopter. Journal of Intelligent and Robotic Systems 45: 115–135
6. Altug E, Ostrowski J, Mahony R (2002) Control of a Quadrotor Heli-
copter Using Visual Feedback. Proc. Int. Conf. on Robotics & Automa-
tion.
7. Altug E, Ostrowski J, Taylor C (2003) Quadrotor Control Using Dual
Camera Visual Feedback. Proc. Int. Conf. on Robotics & Automation.
8. Pounds P, Mahony R, Gresham J (2004) Towards Dynamically-
Favourable Quad-Rotor Aerial Robots. Proc. Australian Conf. Robotics
and Automation.
9. Murphy R, Hay T (2003) Helicopter Pitch Control. Final year project
report, The University of Auckland.
10. Stojanovic A (2004) Two Degrees of Freedom Helicopter Controlled by a
Programmable Logic Controller. ME Thesis, The University of Auckland.
11. Kim T, Zhao V (2004) Helicopter Height Control. Final year project
report, The University of Auckland.
12. Campbell D, D’Souza L (2006) Design and Control of a Miniature Four
Rotor Helicopter. Final year project report, The University of Auckland.
13. Draganfly Inovations Inc. supplier. url: http://www.rctoys.com/
14. Zheng J (2006) Comparison of Two Linearization Methods for Optimal
attitude manoeuvre of a non-linear flexible spacecraft. IMA Journal of
Mathematical Control and Information 127–136
15. Khalil HK (2002) Nonlinear Systems. third edition, Prentice Hall
3
Internal Model Control-Based Adaptive
Attitude Tracking∗
Ahmed Al-Garni1 , Ayman Kassem1 , Muhammad Shafiq2 ,

and Rihan Ahmed1
1
Aerospace Engineering Department,K.F.U.P.M., Dhahran,31261, SaudiArabia
{algarni,akassem,rairfan}@kfupm.edu.sa
2
Systems Engineering Department,K.F.U.P.M., Dhahran, 31261,Saudi Arabia
mshafiq@kfupm.edu.sa
3.1 Introduction
The attitude of a spacecraft is its orientation in space. The orientation

is with respect to a particular reference like the Earth and Sun [1].
The spacecraft is considered to be a rigid body whose attitude can be
described by two sets of equations, namely, the kinematics equation,
which relates the time derivatives of the orientation angles to the an-
gular velocity vector and the dynamics equation, which describes the
time evolution of the angular velocity vector [2, 3]. Various parame-
terizations of the attitude exist to represent the orientation angles. A
comprehensive survey of attitude representations is given in [4]. The
attitude control problem was first presented in the literature in [5]. A
general procedure for the design and analysis of a three-axis, large-angle
attitude control system was developed based on properties common to
all attitude control systems. In [6], a general framework is prepared for
the analysis of attitude tracking control of a rigid body using the non-
singular unit quaternion representation. An adaptive tracking control
scheme wherein the unknown spacecraft inertia matrix is compensated
using linear parameterization is discussed in [7]. Reference [8] proposes
an adaptive attitude tracking controller that identifies the inertia ma-
trix via periodic command signals. Reference [9] discusses the adaptive
attitude tracking control using synthesized velocity from attitude mea-
surements by incorporating a velocity filter formulation.
∗
The authors acknowledge King Fahd University of Petroleum and Minerals,
Dhahran 31261 Saudi Arabia, for supporting this research.
springerlink.com
40 A. Al-Garni et al.
In the present paper we develop a design methodology for adaptive

attitude tracking based on internal model control [20]. The plant is first
stabilized using state feedback based on Lyapunov theory [10, 11]. The
step response of the system is used to obtain parameters of the PID
controller. Simple classical methods such as Ziegler-Nichols method and
Cohen-Coon Method are used for tuning the controller parameters.
Then an IMC based adaptive control law is developed for the output
of the plant to follow a desired input. The IMC structure is composed
of the explicit model of the plant and a stable feed-forward controller.
The control law is robust in nature. It does not require the knowledge
of spacecraft inertia and angular velocity. The control scheme does not
incorporate linear parameterization for the compensation of the inertia
matrix. So the control law is simpler as compared to those in [6, 7, 8, 9].
The rest of the paper is organized as follows. The spacecraft model is
discussed in 3.2. Section 3.3 is dedicated to the problem statement.
Controller design methodology is discussed in Section 3.4. IMC based
adaptive tracking is discussed in Section 3.5. The paper concludes in
Section 3.6 with some remarks.
3.2 Spacecraft Model
The mathematical model of a rigid spacecraft is given by the following

dynamics and kinematics equations discussed in [3].
.
h ω= −ω × hω + τ = −S(ω)hω + τ, (3.1)
.
q = R(q)ω, (3.2)
where (3.1) represents the rigid body dynamics and (3.2) represents the
kinematics equation. In (3.1) ω is the angular velocity of the spacecraft
in a body-fixed frame, h is the spacecraft’s constant inertia matrix,
τ is the control torque input, and S is the skew symmetric matrix
representing the cross product operation:
 
0 −ω3 ω2
S(ω) =  ω3 0 −ω1  . (3.3)
−ω2 ω1 0
In (3.2), q(t) represents the modified Rodrigues parameters describing

the spacecraft attitude with respect to an inertial frame, given by

φ(t)
q(t) = k̂ tan , φ ∈ [0, 2π] rad, (3.4)
4
3 Internal Model Control-Based Adaptive Attitude Tracking 41
ˆ
where k(t) and φ(t) are the Euler eigenaxis and eigenangle, respectively.
The Jacobian matrix R for the modified Rodrigues parameters is given
by
1 1 − qT q T
R(q) = I3×3 + S(q) + qq . (3.5)
2 2
Equations (3.1) and (3.2) are combined to form a second-order non-
linear dynamics equation given by
H ∗ (q)q̈ + C ∗ (q, q̇)q̇ = P T τ, (3.6)
where the matrices P (q), H ∗ (q), C ∗ (q, q̇) are defined as P (q) = R−1 (q),
H ∗ = P T hP , and C ∗ = −H ∗ ṘP − P T S(hP q̇)P . For ease of notation
we use H ∗ (q) = H ∗ , P (q) = P , and C ∗ (q) = C ∗ .
3.3 Problem Statement
The objective of this paper is to design an adaptive attitude controller

such that the spacecraft follows a desired trajectory, while the para-
meters of the model are unknown. Let q̃(t) represent the error, i.e.
q̃(t) = qd (t) − q(t). The tracking objective is accomplished if
lim q̃(t) = 0. (3.7)

t→∞
3.4 Controller Design
3.4.1 Plant Stabilization
We first design a state feedback controller for the stabilization of the

plant. The stability of the closed-loop is established using the Lyapunov
function. The closed loop structure is shown in Fig. 3.1. In this case
the control signal can be given by
τ = RT (sp q̃ − sd q̇) , (3.8)
where sp is the proportional gain of the state feedback controller and

sd is the derivative gain of the state feedback controller.
Now, we study the stability of the closed loop by considering the
following positive definite function [10]:
1
V (q, q̇) = [q̇ T H ∗ q̇ + q̃ T sp q̃] > 0. (3.9)
2
Fig. 3.1. Plant stabilization using state feedback control
The positive definite function is composed of the mechanical energy

given by 21[q˙T H ∗ q̇] and the proportional gain of the state feedback
controller, sp . The time derivative of the positive definite function in
(3.9) can be written as
1
V̇ (q, q̇) = [q̇ T H ∗ q̈ + q̇ T H˙ ∗ q̇ + q̈ T H ∗ q̇] + q̃˙T sp q̃, (3.10)
2
1
V̇ (q, q̇) = q̇ T H ∗ q̈ + q̇ T H˙ ∗ q̇ + q̃˙T sp q̃. (3.11)
2
Using (3.6) and (3.14) we obtain

1 ˙∗
V̇ (q, q̇) = q̇ T H − C ∗ q̇ + q̇ T P T τ + q̃˙T sp q̃. (3.12)
2

As in [7] the property of skew symmetry implies that 21 H˙ ∗ − C ∗ = 0.
Then substituting for the control signal from (3.11) V̇ (q, q̇) becomes
V̇ (q, q̇) = q̇ T P T [RT (sp q̃ − sd q̇)] + q̃˙T sp q̃, (3.13)

T
V̇ (q, q̇) = −q̇ sd q̇. (3.14)
˙ In practice q̇(t) is not available for the feedback
P T RT = I, −q̇ = q̃.
[9]. The velocity estimate is derived from the attitude measurement q,
using a filter αp/(p + α), which approximates the derivative operator,
p, when α (positive gain) is very large. Hence we can say that
o αp
q̂ (t) = q. (3.15)
p+α
In this case we choose τ as
o
τ = RT [sp q̃(t) − sd q̂ (t)]. (3.16)
Plant stabilization is established using the control signal given in

(3.16). Now, it is easy to show that
o o
V1 (q, q̇) = −q̇T sd q̂ (t), (3.17)
o
where V1 (q, q̇) is the time derivative of V (q, q̇), when τ is given by
(3.16). V (q, q̇) in (3.9) is a Lyapunov function as it is positive definite
and its time derivative in (3.14) is negative semi-definite. Since V̇ (q, q̇)
o
in (3.14) is negative semi-definite then V1 (q, q̇) in (3.17) is also negative
semi-definite for large α. Therefore, the closed loop will remain stable.
3.4.2 PID Controller Design for the Stabilized Plant

A PID controller is designed for the stabilized plant as shown in Fig.
3.2. The gains of the PID controller are calculated using two classical
techniques, namely the Ziegler-Nichols step response method and the
Cohen-Coon method discussed in [12, 13, 14]. These classical techniques
allow us to calculate the PID controller parameters by using the step
response of the stabilized plant. Let τP ID be the output of the PID
controller:
Z
1 t ˙
τP ID = kp q̃(t) + q̃(λ)dλ + Td q̃(t) , (3.18)
Ti 0
where kp is the proportional gain, Ti is the integral time and Td is the
derivative time of the PID controller. Table 3.1 shows the calculated
parameters of the PID controller.
Fig. 3.2. PID controller design for the stabilized plant
The control law for the stabilized plant using a PID controller is
given as
o
τf = RT τP ID + sp q̃ − sd q̂ (t)) . (3.19)
Thanks to the presence of the integral part in the PID, the plant output
tracks the low frequency reference input such as constants and low
frequency sinusoids. The PID parameters kp , Ti , and Td are updated
from the IMC based plant model estimate.
Table 3.1. PID parameters obtained by the Ziegler-Nichols step response and
Cohen-Coon methods
Technique kp Ti [sec] Td [sec]

Ziegler-Nichols method 12.25 3 0.75
Cohen-Coon method 14.07 3.58 0.5436
3.5 Adaptive Controller Design
3.5.1 Internal Model Control – System Operation
The main features of the IMC are simple structure, fine disturbance
rejection capabilities, and robustness [15, 16]. Some of the important
properties of the IMC are given in [17]. The IMC structure is shown
in Fig. 3.3, where r(t) is the reference signal, fC (.) represents the con-
troller, fP (.) and fM (.) represent the stabilized plant and its model,
respectively. The controller output uc (t) is fed to both the plant and
its model. The plant output YP (t) is compared with the output of the
ˆ represents the tracking error,
model YM (t) and the resulting signal d(t)
which is given by
ˆ = YP (t) − YM (t).
d(t) (3.20)
ˆ is a measure of the difference in behavior between the plant and
d(t)
its model and can therefore be used to improve control effort. This is
ˆ from the reference signal r(t); the resulting
given by subtracting d(t)
control signal is given by

ˆ
uc (t) = fC r(t) − d(t) . (3.21)
Fig. 3.3. IMC based adaptive control

The model of the plant is computed using a multi-channel adaptive

filter employing the normalized least mean square (NLMS) adaptation
algorithm discussed briefly in [18, 19]. The NLMS algorithm can be
summarized in the following equations:
YM = Wk uc ,
dˆ = YP − YM , (3.22)
ˆ
Wk+1 = Wk + µuc d,
where µ is the step size parameter and controls the convergence char-
acteristics of the NLMS algorithm. Wk is the weight vector between
the delayed inputs and the outputs of the adaptive filter. The weights
of the NLMS adaptive filer are adjusted to minimize the plant-model
mismatch. If the model is the exact representation of the plant, i.e.
fM (.) = fP (.), and the controller is the inverse of the model, i.e.
fC (.) = [fM (.)]−1 , then q −L + q −L ∆(q) can be regarded as the delay
Fig. 3.4. IMC based attitude tracking qd1 (t), q1 (t), qd2 (t), q2 (t), qd3 (t), q3 (t)
Fig. 3.5. Attitude tracking error e1 (t), e2 (t), e3 (t)

along the path from the input u(t) to the plant output YP (t), where
q −1 is the backward shift operator and ∆(q) represents the plant un-
certainty. We can say from Fig. 3.3 that

YP (t) = q −L + q −L ∆(q) u(t), (3.23)
YM (t) = q −L u(t), (3.24)

ˆ
u(t) = r(t) − d(t). (3.25)
Substituting (3.23) and (3.24) in (3.20) we get

ˆ = q −L ∆(q)u(t).
d(t) (3.26)
Using (3.23), (3.25), and (3.26) and on further simplification the overall
closed loop function in Fig. 3.3 is obtained as follows:
YP (t) = r(t − 1)[1 + ∆(q)] − r(t − 2)[∆(q) + ∆2 (q)]. (3.27)
If ∆(q) ≪ 1 in (3.27), then YP (t) ≈ r(t − 1). This shows that the
approximate tracking objective is accomplished.
3.5.2 Simulation Results
To demonstrate the application of the proposed scheme, we illustrate

the results obtained in this section. The mathematical model of the
rigid spacecraft given by (3.1) and (3.2) is simulated with (3.19) as the
control input and the inertia matrix is
 
20 1.2 0.9
h =  1.2 17 1.4  kgm2 , (3.28)
0.9 1.4 15
α = 100, sp = 10, sd = 8, µ = 0.01, and sampling time is 0.01 sec. The

gains of the PID controller are selected according to the Cohen-Coon
method as given in Table 3.1. The desired input (attitude trajectory)
is selected as qd (t) = k̂d tan (φd (t)/4) with
√ T
k̂d = 0.5 cos(0.2t), sin(0.2t), 3
and φ d(t) = π rad. The IMC based adaptive tracking is shown if

Fig.3.4 and the attitude tracking error is shown in Fig. 3.5. It is evident
from the plots that the IMC controller is able to track the attitude
parameters properly. Initially, while the system is improving the model,

the error is high, however, it drops rapidly soon after and the tracking
is stable.
3.6 Conclusion
In this paper an IMC based design methodology for tracking of the

spacecraft attitude is proposed. The plant is first stabilized using state
feedback control based on Lyapunov theory. Adaptive attitude tracking
is established by incorporating an IMC structure along with the feed-
forward controller using the normalized least mean square adaptation
algorithm. It has been shown that the output of the plant tracks the
desired trajectory with a delay. Computer simulation results show the
effectiveness of the proposed method.
References
1. Wertz J.R., Spacecraft attitude determination and control, D. Reidel Pub.,
USA, 1978.
2. Sidi M.J., Spacecraft dynamics and control, Cambridge Univ. Press, Cam-
bridge, 1997.
3. Hughes P.C., Spacecraft attitude dynamics, Wiley, USA, 1986.
4. Shuster M.D., A survey of attitude representations. J. of Astronautical
Sciences, vol. 41, no. 4, pp. 439-517, 1993.
5. Meyer G., Design and global analysis of spacecraft attitude control. Nasa
Technical Report, R-361, 1971.
6. Wen J.T., Kreutz-Delgado K., The attitude control problem. IEEE
Trans. on Automatic Control, vol. 36, no. 10, pp. 1148-1162, 1991.
7. Slotine J.J.E., Benedetto M.D., Hamiltonian adaptive control of space-
craft. IEEE Trans. on Automatic Control, vol. 35, no. 7, pp. 848-852,
1990.
8. Ahmed J., Coppola V.T., Bernstein D.S., Adaptive asymptotic tracking
of spacecraft attitude motion with inertia matrix identification. J. of
Guidance, Control, and Dynamics, vol. 21, no. 5, pp. 684-691, 1998.
9. Wong H., de Queiroz M.S., Kapila V., Adaptive tracking conrol using
synthesized velocity from attitude measurements. Automatica, vol. 37,
no. 6, pp. 947-953, 2001.
10. Slotine J.J.E., Li W., Applied nonlinear control. Prentice Hall, USA, 1991.
11. Åström K.J., Wittenmark B., Adaptive control. Addison-Wesley, USA,
1995.
12. Tan Kok Kiong, Wang Quing-Guo, Hang Chang Chieh, T.J. Hägglund,
Advances in PID control. Springer, London, 1999.
13. Ziegler J.G., Nichols N.B., Optimum settings for automatic controllers,
Trans. of ASME, vol. 64, pp. 759-768, 1942.
14. Cohen G.H., Coon G.A., Theoritical consideration of retarded control,
Trans. of ASME, vol. 75, pp. 827-834, 1953.
15. Datta A., Ochoa J., Adaptive internal model control: Design and stability
analysis, Automatica, vol. 32, no. 2, pp. 261-266, 1996.
16. Shafiq M., Riyaz S.H., Internal model control structure using adaptive
inverse control strategy, In: Proc. 4th Int. Conf. on Control and Automa-
tion (ICCA), 2003, p. 59.
17. Morari M., Zafiriou E., Robust process control, Prentice Hall, 1989.
18. Haykin S., Adaptive Filter Theory, Pearson Education, USA 2002.
19. Nagumo, Noda, A learning method for system identification, IEEE Trans.
on Automatic Control, vol. 12, no. 3, pp. 282-287, 1967.
20. Datta A., Adaptive Internal Model Control, Springer, 1998.
4
Tracking Control of Automated
Guided Vehicles ∗
Lotfi Beji1 and Azgal Abichou2

1
IBISC Laboratory, CNRS-FRE 2873, Université d’Evry Val d’Essonne.
40, rue du Pelvoux, 91020 Evry Cedex, France
beji@iup.univ-evry.fr
2
LIM Laboratory, Ecole Polytechnique de Tunisie,
BP 743, 2078 La Marsa, Tunisia
azgal.abichou@ept.rnu.tn
4.1 Introduction
The study of automated guided vehicle behavior when moving on a
plane occupies practically a central place in the automobile theory.
While an automated vehicle travels at a relatively low speed, controlling
it with only a kinematics model may work. However, as automated
vehicles are designed to travel at higher speeds, dynamics modelling
becomes important. For the studies concerning the automated vehicle
modeling and control we can refer to [5], [6], [8], and [14]; they deal only
with some simplified low-order linear models. These models are too
simple for studying the integrated longitudinal and lateral dynamics.
Works such as [9] and [13] highlight the contribution of the internal
variables such as the rotation angles and velocities of the wheels into
the dynamics model. The exponential stabilization of some equilibria
of automated vehicles was subject of our work in [1].
We consider in this paper a rigid vehicle moving at high non-
constant speed on a planar road. As the speed of this vehicle is not
small, the wheels must move with suitable sideslip angles to generate
cornering forces. The interaction between longitudinal and transversal
forces due to the tires as well as the nonlinear nature of these forces
are taken into account. The presence of the suspension is neglected.
Consequently, the roll and pitch angles are not considered here.
The tracking controller solution for both the kinematic and dynamic
model of Automated Guided Vehicles (AGVs) is given. The vehicle
∗
L. Beji would like to thank the Tunisian Minister of High Education Scientific
Research and Technology for the invitation within the framework of the 2006-
SERST program.
springerlink.com c Springer-Verlag London Limited 2007
50 L. Beji and A. Abichou
tracking problem takes into account the side-slip effects and the yaw
dynamics. Hence, two inputs are considered: the braking torque and
the rear axle force. However one ensures with the tracking controller
the behavior of the vehicle coordinates, the rate, the longitudinal veloc-
ity, and the front-wheel angular velocities. Exponential and asymptotic
stabilities result from a reference road to be followed and free of side-
slip. This work is an extension of the stabilizing problem solved in [2].
The proof of the main results is based on backstepping techniques and
Lyapunov theories. The achieved tracking behavior is illustrated by a
clothoid-like predefined path.
The contents of the paper is as follows: Section 4.2 deals with the
system modeling and parametrization. In Section 4.3, we detail the
tracking objectives and the control. Simulation tests and conclusions
are presented in the last section of the paper.
4.2 Modelling
For the study of AGV interactions a model able to simulate the behavior
of the vehicle has to be built. The model is based on the balance of the
forces acting on the vehicle in the longitudinal and lateral directions
(the details of forces are given in Fig.4.1), the torques and kinematic
conditions. The guidance system [8] operates the steering wheel causing
some wheels to work with a sideslip and to generate lateral forces. These
forces cause a change of attitude of the vehicle and then a sideslip of
all wheels.
Using the conventional notations where V = (u v w)T denotes the
linear velocity and Ω = (p q r)T the angular velocity, the following
nonlinear sixth-order vehicle model is considered (The important dy-
namical variables are standard and shown in Fig.4.1):
ẋ =v cos(ψ − β), ψ̇ = r,
ẏ =v sin(ψ − β), δ̇ = ω,
v̇ =θ1 v(β̇ − ψ̇) cot δ + θ2 Fad + θ3 Fas cot δ cos δ + θ4 Ras cot δ + θ5 τr ,
1
ṙ = {Fas lv cos δ − Ras lh }, (4.1)
I
ω̇ =θ6 ω + θ7 τb , v = kVk,
where cot(δ) = tan−1 (δ) and k.k is the Euclidean norm and w = p =
q = 0 since this is a planar motion. The aerodynamic side force on the
front wheel Fas and the aerodynamic side force on the rear wheel Ras
are given by [8].
4 Tracking Control of Automated Guided Vehicles 51
The known constant dynamic parameters are regrouped in the fol-

lowing (7 × 1) vector [3]:
   1

θ1 rad m + rad (Ir + If )
 θ2   1 
   2 rad CAρ 
 θ3    r m 
   ad 
Γ 
θ= θ
  4
= r ad α
vm
, (4.2)
 θ5  
vm
Γhm 
   r ad αhm 
 θ6   1

n Idw

θ7 1
n fω
Γvm ψ̇ Γhm ψ̇
Fas = (β − lv + δ); Ras = (β + lh ), (4.3)
αvm v αhm v
while the front aerodynamic drag force is
1
Fad = CAρv 2 . (4.4)
2
The rear aerodynamic drag force is neglected with respect to the front
one.
Fig. 4.1. Automatic Guided Vehicle (AGV) parameterization
Here Γh,v /αh,v represents the characteristic curves of the tires, with
Γvm = max(Γv ), αvm = max(αv ), etc. The characteristic lines include
the limitations and the descending behavior for high values in the ar-
gument (more details are in [8]).
Remark 1. In Eq. (4.1) the input in torque τb can be defined such that
ω → ωd then δ → δd . Moreover, δd can be specified in order to have in
closed loop ṙ = Wr , in which Wr is considered a new control input.
Following the analysis given in Remark 1, from system (4.1, Eq.6) we

propose to take (Wr is considered bounded)

IWr + Ras lh
δd = arccos . (4.5)
Fas lv
Then the yaw motion dynamics is transformed to ṙ = Wr .

As soon as for the dynamics of v, one proposes the following lin-
earizing feedback τr for the rear torque:
1
τr = (Wv − θ1 v(β̇ − ψ̇) cot δd − θ2 Fad
θ5
− θ3 Fas cot δd cos δd − θ4 Ras cot δd ), (4.6)
which permits to write v̇ = Wv .

Inputs (Wr , Wv ) are specified later. The vehicle tracking problem is
based on the following new writing:
ẋ =v cos(ψ − β),
ẏ =v sin(ψ − β), (4.7)
ψ̇ =r, v̇ = Wv , ṙ = Wr .
Let us define the reference path that the vehicle should follow:
ẋr =v r cos(ψ r ),
ẏ r =v r sin(ψ r ), (4.8)
ψ̇ r =r r , v̇ r = Wvr , ṙ r = Wrr .
As we can note in system (4.8), one assumes that reference trajec-

tories are ideal paths. This implies that the paths are considered free
from slip behavior. As a result, β r = β̇ r = β̈ r = 0.
The following change of variables will be introduced:
z1 = cos(ψ − β)x + sin(ψ − β)y,

z2 = − sin(ψ − β)x + cos(ψ − β)y, (4.9)
z3 =ψ − β.
4 Tracking Control of Automated Guided Vehicles 53
Hence, with respect to time, the derivative of (4.9) gives

ż1 =(ψ̇ − β̇)z2 + v,
ż2 = − (ψ̇ − β̇)z1 ,
ż3 =r − β̇, (4.10)
v̇ =Wv , ṙ = Wr .
The reference trajectory in terms of the new variables is given by
z1r = cos(ψ r )xr + sin(ψ r )y r ,

z2r = − sin(ψ r )xr + cos(ψ r )y r , (4.11)
z3r =ψ r .
Let us introduce errors which are defined as ez1 = z1 − z1r , ez2 =

z2 − z2r , and ez3 = z3 − z3r . The associated dynamics is given by
ėz1 =ez2 ψ̇ r + z2 (ψ̇ − ψ̇ r ) + (v − v r ) − β̇z2 ,

ėz2 = − ez1 ψ̇ r − z1 (ψ̇ − ψ̇ r ) + β̇z1 ,
ėz3 =r − r r − β̇, (4.12)
ėv =Wv − Wvr , ėr = Wr − Wrr .
Moreover one takes er = r − r r and ev = v − v r . After some manipula-

tion, we get
ėz1 =ez2 ψ̇ r + z2 er + ev − β̇z2 ,
ėz2 = − ez1 ψ̇ r − z1 er + β̇z1 ,
ėz3 =r − r r − β̇, (4.13)
ėv =Wv − Wvr , ėr = Wr − Wrr .
The last two equations permit to define Wv and Wr such that ev →

edv and er → edr . Consider this issue, our interest is focused on the
remaining part of 4.13. However, one proposes (kev , ker > 0)
Wv = − kev ev + Wvr + edv ,

Wr = − ker er + Wrr + edr . (4.14)
Under the fact that ev → edv and er → edr , the system of errors becomes
ėz1 =ez2 r r + z2 edr + edv − β̇z2 ,

ėz2 = − ez1 r r − z1 edr + β̇z1 , (4.15)
ėz3 =edr − β̇.
Let us introduce Λ = edr − β̇. Then

ėz1 =ez2 r r + edv + (ez2 + z2r )Λ,
ėz2 = − ez1 r r + (ez1 + z1r )Λ, (4.16)
ėz3 =Λ.
It is clear that the last relation permits to propose Λ. Consequently,
with Λ = −ez3 we ensure exponential stability to zero of lim ez3 = 0.
t→∞
The solution is given by ez3 (t) = λe−t (λ ∈ ℜ). Moreover, one concludes
that ψ → ψ r + β, where β could be close to zero.
As lim ez3 = 0, we get
t→∞
ėz1 =η(t)ez2 + edv − ez3 z2r ,

| {z }
0
r
ėz2 = − η(t)ez1 − ez3 z1 , (4.17)
| {z }
0
where η(t) = r r − ez3 . It is a time-varying linear system in errors

with η(t) = r r − λe−t . One verifies that this system is controllable for
η(t) 6= 0, which means that r r cannot have an λe−t behavior. Under
this assumption we conclude with the following lemma.
Lemma 1. The vector of errors (ez1 , ez2 ) is asymptotically stable with
edν = −kez1 ez1 (kez1 > 0).
Proof. Taking the Lyapunov function V (ez1 , ez2 ) = 12 (e2z1 + e2z2 ), the
proof of the lemma is straightforward (V̇ (.) 6 0). Further, from
LaSalle’s principle (ez1 , ez2 ) approaches the largest invariant set in
{(ez1 , ez2 ) | V̇ (.) = 0} = {(ez1 = 0, ez2 = 0)}. This ends the proof.
The vehicle characteristics are [9]: m = 1000 kg, I = 1210 kgm2 , lv =

0.87 m and lh = 1.29 m. The performance of the tracking controller is
subject to a clothoid-shape path. The following estimated parameters
are considered in simulation [3] (mathematical expressions are in (4.2)
with international system units): θ1 = 1200, θ2 = 349.06, θ3 = 200,
θ4 = −15.15, θ5 = 398.56, θ6 = 30, and θ7 = 0.001. The vehicle initial
configurations are x(0) = y(0) = 0.05 m, ψ(0) = 3◦ , β(0) = 0.6◦ ,
v(0) = 0.01 m/s, ψ̇(0) = 0. The controller parameters are kez 3 = 45,
kez 1 = 1, kev = 6, ker = 0.5.
4 Tracking control of Automated Guided Vehicles 55
Real and reference(red) shapes: clothoid

2
1.5
1
y,yr(m)
0.5
−0.5
−0.5 0 0.5 1 1.5 2
x,xr(m)
Fig. 4.2. Clothoid-shape path following
4.4 Conclusions
The kinematic-dynamic model of automated guided vehicles was pre-
sented. The tracking control problem takes the nonlinear behavior of
the system including side-slip effects. The braking angle combined with
the rear torque realize a first feedback leading to a linear behavior
of yaw motion and of longitudinal-lateral displacements. Simulation
processes result from a clothoid path which is free from slip behav-
ior. The obtained results achieve almost exact path tracking under few
adjusted controller parameters.
References
”
1. A. Abichou, L. Beji and R. Slim, Exponential stabilization of some equi-
libria of automated vehicles,” in Int. Conf. on Advanced Robotics, Coim-
bra, Portugal, juillet 2003. ”
2. L. Beji, A. Abichou and R. Slim, Longitudinal and steering stabilization
of an underactuated autonomous vehicle,” in Int. IFAC SYmposium on
RObot COntrol, Waroclaw, Poland, 2003. ”
3. L. Beji and Y. Bestaoui, An adaptive control method of automated vehi-
cles with integrated longitudinal and lateral dynamics in road following,”
in Proc. RoMoCo, Robot, Motion & Control, Poland, 2001, pp. 201-206.
”
4. Y. Bestaoui, An optimal velocity generation of a rear wheel drive tricycle
along a specified path,” in Proc. ACC, American Control Conference,
2000, pp. 2907-2911.
”
5. C.Y. Chan, Open-loop trajectory design for longitudinal vehicle ma-
neuvers: case studies with design constraints,” in Proc. ACC, American
Control Conference, 1995, pp. 4091-4095.
”
6. S.B. Choi, The design of a control coupled observer for the longitudinal
control of autonomous vehicles,” J. of Dynamic Systems Measurement
and Control, 1998, vol.120, pp.288-289.
”
7. O. Dahl and L. Nieslon, Torque-limited path following by on-line tra-
jectory time scaling”, IEEE Trans. on Robotics and Automation, 1990,
Vol.6, No.5, pp.554-561. ”
8. E. Freund and R. Mayer, Non linear path control in automated vehicle
guidance,” IEEE Trans. on Robotics and Automation, 1997, vol.13, No.1,
pp.49-60.
9. G. Genta, Motor vehicle dynamics: modelling and simulation, World Sci-
entific, 1997. ”
10. H. Hu, M. Brady and P. Probert, Trajectory planning and optimal track-
ing for an industrial mobile robot,” in Proc. SPIE, Boston, Ma, vol.2058,
1993, pp. 152-163. ”
11. Y. Kanayama and B.I. Hartmann, Smooth local path planning for au-
tonomous vehicles”, J. of Autonomous Robot Vehicles, I. J. Cox, G. T.
Wilfong ed., Springer-Verlag, 1990, pp.62-67.
12. M. Kristic et al., Nonlinear and adaptive control design, John Wiley and
Sons, inc. New York, 1995. ”
13. R. Outbib and A. Rachid, Control of vehicle speed: a non linear ap-
proach,” in Proc. CDC, Conference on Decision and Control, Australia,
2000. ”
14. T. Pilutti et al., Vehicle Steering intervention through differential brak-
ing,” Trans. of ASME, 1998, Vol.120, pp.314-321. ”
15. V. Munoz, A. Ollero, M. Prado and A. Simon, Mobile robot trajectory
planning with dynamic and kinematic constraints,” in Proc. ICAR, In-
ternational Conference in Robotics and Automation, 1994, pp.2802-2807.
”
16. D.B. Reister and F. G. Pin, Time-optimal trajectories for mobile ro-
bots with two independently driven wheels,” Int. J. of Robotics Research,
vol.13, No.1, 1994, pp. 38-54.”
17. N. Sadeghi, B. Driessen, Minimum time trajectory learning,” in Proc.
ACC, American Control Conference, 1995, pp. 1350-1354.
”
18. R.S. Sharp et al., A mathematical model for driver steering control,
with design, tuning and performance results,” Vehicle System Dynamics,
2000, Vol.33, No.5, pp.289-326.
”
19. P.A. Shavrin, On a chaotic behaviour of the car,” in Proc. ECC, Eu-
ropeen Control Conference, 2001, pp. 2187-2192.
5
VFO Control for Mobile Vehicles
in the Presence of Skid Phenomenon
Maciej Michalek
Chair of Control and Systems Engineering, Poznań University of Technology,

Piotrowo 3a, 60-965 Poznań, Poland
maciej.michalek@put.poznan.pl
5.1 Introduction
Skid is a common phenomenon in real vehicles. Sometimes this effect

is negligibly small and in a consequence can be omitted in a vehi-
cle model preserving practically acceptable control performance. In a
case of nonholonomic mobile robots it is equivalent to the rolling with-
out slipping assumption. However, in many real-life situations the skid
phenomenon influences vehicle motion in a such degree, that exclu-
sion of it in a system model leads to significant control performance
deterioration. The term skid used in this paper should be understood
as a phenomenon of motion velocity disturbance, which for nonholo-
nomic vehicles is connected with violating kinematic constraints. Wind
blowing during aircraft flight, currents and waves during ship cruising,
loosing adhesion between road surface and wheels during car ride or
moving on sloping areas are examples of situations, where a skid phe-
nomenon usually appears. To enhance tracking precision in these cases,
control laws dedicated for mobile vehicles should be robust to skid-like
disturbances. It is especially important when skidding is not vanishing
and persistently disturbs vehicle motion.
In the literature the problem of motion-with-skid control has been
considered in some papers. One can recall [1] and [6] for the case of
car-like vehicles (the second one treats about an agricultural tractor),
also [8] where the control task was designed for a skid-steering mobile
robot, and [4] dedicated to practical issues of ship steering.
In [2] a new VFO control methodology1 dedicated to tracking and
stabilization of specific subclass of nonholonomic driftless systems has
been proposed and verified. The VFO control strategy can be treated as
1
VFO is an abbreviation from words: Vector Field(s) Orientation.
58 M. Michalek
a generalization of control in polar coordinates (the need of such a gen-

eralization has been already pointed to in [5]). The results have revealed
good performance obtained for 3-dimensional systems with VFO con-
trollers guaranteeing, among others, good control quality (fast asymp-
totic error convergence, natural and non-oscillatory transient behavior),
intuitive control input interpretation, and very simple controller para-
metric synthesis. The aim of this paper is a description of the VFO
tracking control strategy extension for mobile vehicles in relation to
the original concept presented in [2]. The extension concerns a case of
skid phenomenon influence compensation during vehicle motion. The
VFO controller proposed in [2] is modified to preserve asymptotic con-
vergence of a vehicle position error despite of skid effect existence. A
similar idea connected with a drift compensation and applied to a
motion planning task can be found in [3].
5.2 Problem Formulation

Let us consider the class of restricted-mobility mobile vehicles moving
on a plane, which in absence of skid can be modeled by the equation
   
θ̇ 1 0
ẋ = 0 cos θ u1 ⇒ q̇ = G(q)u, (5.1)
u2
ẏ 0 sin θ
where q ∈ R3 is a vehicle state and u ∈ R2 is a control vector defined

in a space of velocities2 . Examples of vehicles from the considered class
with geometrical interpretation of state variables and control inputs
are depicted in Fig. 5.1.
Fig. 5.1. Three examples of mobile vehicles mov- Fig. 5.2. Mobile vehi-
ing on a plane: two-wheeled robot (A), aircraft (B), cle as a rigid body on
ship (C) a plane
2
Sometimes in real vehicles one should assume u2 ∈ R+ like in the aircraft case.
5 VFO Control in the Presence of Skid Phenomenon 59
5.2.1 Skid Phenomenon
Since we consider a motion control problem on a kinematic level, we

will not be interested in analyzing reasons of the skid phenomenon. On
this level it is rather justified to take into account only direct results
of skidding. Regardless of the reasons of the skid effect, the results are
always the same – an additional velocity vector appears in (5.1):
     
θ̇ 1 0 0
ẋ = 0 cos θ u1 + vsxg  , (5.2)
u2
ẏ 0 sin θ vsyg
∗ = [v T 2
where vsg sxg vsyg ] ∈ R is a skid velocity vector described in
a global frame (Fig. 5.2). Expressing the vsg∗ term in a local frame
attached to the vehicle body one can rewrite (5.2) as follows3 :

       
θ̇ 1 0 0
ẋ = 0 u1 + cos θ (u2 + vsx ) + − sin θ  vsy , (5.3)
ẏ 0 sin θ cos θ
where [vsx vsy ]T = vs∗ ∈ R2 is the skid velocity vector described in

a local frame (one can treat Eq.(5.3) as a two-input control system with
a drift term g0 = [0 − vsy sin θ vsy cos θ]T ). From Eq.(5.3) it follows
that the skid effect can be considered as a violation of a nonholonomic
constraint connected with system (5.1) – due to the nonzero lateral
velocity component vsy – and also as a longitudinal velocity disturbance
– due to the nonzero vsx term. Depending on the reasons causing the
skid, terms vsx and vsy can be treated as constant or varying in time
during vehicle movement4 . For example, directional wind or current
disturbing a ship cruise (see [4]) or a long-time motion along a slope
give constant skid influence in a global frame, so in a local frame skid
velocity coordinates can be varying. The opposite situation can be seen
for instance during moving along a curve on a slippery ground, where
one can notice constant lateral deviation of the vehicle velocity [6]. It
is worth mentioning that partially also dynamic effects not included
in kinematics (5.1) but appearing in experiments can be treated as a
virtual skid included in the vsx element. Now, let us formulate a control
problem, which is intended to be solved in the sequel.
3
Eq.(5.3) is equivalent to the rigid body motion q̇ = R(θ)η, where R(θ) ∈ SO(3)
and η = [u1 (u2 + vsx ) vsy ]T ∈ R3 .
4
In real conditions also random fluctuations of skid components are characteristic.
60 M. Michalek
Problem 1. For a given admissible and persistently exciting reference

trajectory qt (τ ) = [θt (τ ) xt (τ ) yt (τ )]T ∈ R3 and assuming a nonzero
∗ ∈ L ∗
skid velocity vsg ∞ with v̇sg ∈ L∞ , find a bounded control law
u(qt , q, ·) for the kinematics (5.2), which guarantees asymptotic con-
vergence of the position error e∗ = [ex ey ]T ∈ R2 to zero and bound-
edness of the orientation error eθ in the sense that limτ →∞ e∗ (τ ) = 0,
eθ (τ ) ∈ L∞ , where:
∆ ∆ ∆
ex = xt − x, ey = yt − y, eθ = θt − θ. (5.4)
The admissibility of a reference trajectory means that qt (τ ) satisfies
Eq.(5.1) for some reference inputs u1t (τ ), u2t (τ ) ∈ L∞ . Persistent exci-
tation implies that ∀τ ≥0 u2t (τ ) 6= 0 (see [2]).
5.3 VFO Controller
First of all we recall the basic idea of the VFO control strategy dedi-
cated to kinematics (5.1), hence for a case without skid disturbances.
After that, by analogy to previous considerations, an extension regard-
ing a nonzero skid velocity will be presented.
5.3.1 VFO Strategy – Brief Recall
It has been shown (for example in [2]) that the VFO control strategy
results from simple geometrical interpretations and can be applied to
a subclass of nonholonomic driftless systems with two inputs5 . System
(5.1) belongs to this class. One can decompose (5.1) into two subsys-
tems: one-dimensional orienting subsystem represented by the equa-
tion θ̇ = u1 and two-dimensional pushing subsystem represented by the
equation q̇ ∗ = g ∗2 u2 , where q ∗ = [x y]T and g ∗2 = [cos θ sin θ]T . Since
the direction (and orientation) of vector g ∗2 (θ) (and in a consequence
– of velocity q̇ ∗ (θ)) depends on the θ variable, it has been proposed to
call θ the orienting variable, and the u1 input – the orienting control.
Follo- wing this interpretation the u2 input has been called the push-
ing control, since it pushes the sub-state q∗ along the current direction
of g ∗2 . The proposed control law for a tracking task has been derived
from the desired relation between the g 2 vector field and introduced
additional convergence vector field h = [h1 h∗T ]T . This vector field has
been designed so as to indicate in any state q an instantaneous con-
vergence direction to a reference trajectory qt . In this way the VFO
5
The VFO control can be treated as a generalization of control in polar coordinates.
control strategy can be decomposed into the orienting subprocess, re-

sponsible for putting the direction of g ∗2 onto the h∗ vector (with the
u1 input), and the pushing subprocess responsible for pushing the state
q ∗ (with the u2 input) to the reference trajectory qt∗ . Since the orient-
ing variable θ plays an auxiliary role during the orienting subprocess,
only a proper construction of the h vector will guarantee asymptotic
convergence also of the θ variable to the reference signal θt near the
reference trajectory qt∗ (for details the reader is referred to [2]).
5.3.2 VFO Control with Skid Compensation
Considering the skid phenomenon, we have to use model (5.2) or (5.3)

instead of kinematics (5.1). By analogy to the comments of Section 5.3.1
we introduce the convergence vector field h = [h1 h∗T ]T ∈ R3 , where
T ∆ T
h∗ (e∗ , q̇t∗ ) = h2 h3 = kp e∗ + q̇t∗ , q̇t∗ = ẋt ẏt , (5.5)
and kp > 0 is a design parameter (definition of the h1 component will

be introduced later). Since for every error e∗ , vector h∗ (e∗ , q̇t∗ ) defines
a desired direction (and orientation) of motion for system (5.2), the
VFO strategy demands meeting the following convergence relation:
τ →∞
q̇ ∗ (τ ) −→ fk (τ ) h∗ (e∗ (τ ), q̇t∗ (τ )), (5.6)
where fk (τ ) is a some scalar and nonzero smooth function. Relation

(5.6) describes the desirable situation, where the instantaneous direc-
tions of vectors q̇∗ and h∗ are matched (subsystem q̇∗ = g∗2 u2 + vsg ∗
∗
evolves along the convergence direction defined by h ). From now on
we take fk (τ ) = fk ≡ 1 to stress that not only directions but also ori-
entations and norms of q̇∗ and h∗ should be matched. Substituting the
particular terms from (5.2) and (5.5) into (5.6) gives

u2 (τ ) cos θ(τ ) + vsxg (τ ) − h2 (τ ) = 0
lim ,
τ →∞ u2 (τ ) sin θ(τ ) + vsyg (τ ) − h3 (τ ) = 0
which can be rewritten in a more compact form as
lim [Atan2 (H3 sgn(u2 ), H2 sgn(u2 )) − θ(τ )] = 0 (5.7)

τ →∞
with
∆ ∆
H2 = h2 − vsxg , H3 = h3 − vsyg . (5.8)
The limit (5.7) describes the so-called orienting condition, which gua-
rantees matching of orientations for vectors q̇∗ and h∗ . Hereafter the
62 M. Michalek
VFO control design follows by analogy to the original concept mentio-

ned in Section 5.3.1. Recalling (5.7), we introduce an auxiliary variable
∆
θa = Atan2c (H3 sgn(u2t ), H2 sgn(u2t )) ∈ R, (5.9)
where Atan2c (·, ·) : R × R → R is a continuous version of the function

Atan2 (·, ·) : R × R → (−π, π], and the term sgn(u2 ) was replaced
with sgn(u2t ) to guarantee proper transient behavior of a closed-loop
system6 . To meet the relation (5.7) it suffices to make the auxiliary
error ea = θa − θ tend to zero. Hence, let us define the h1 component
of vector h as follows:
∆
h1 = k1 ea + θ̇a , (5.10)
where k1 > 0 is a second design coefficient, θ̇a = (Ḣ3 H2 −H3 Ḣ2 )/k H ∗ k2
results from time-differentiation of (5.9), and H ∗ = [H2 H3 ]T . Accord-
ing to the VFO strategy one proposes the following VFO control law:
u1 = h1 ⇒ u1 = k1 ea + θ̇a , (5.11)
u2 = g ∗T
2 H
∗
⇒ u2 = H2 cos θ + H3 sin θ. (5.12)
Equation (5.12) comes from an idea of careful pushing, which allows

for the fastest pushing only when the orienting condition (5.7) – and
equivalently relation (5.6) – is satisfied.
Proposition 1. Assuming that ∀τ ≥0 H ∗ (τ ) 6= 0 and the reference tra-
jectory qt∗ is sufficiently smooth: q̇t∗ , q̈t∗ ∈ L∞ , the VFO control law
(5.11,5.12) applied to system (5.2) solves Problem 1.
Proof. Let us first consider behavior of the auxiliary error ea = θa − θ.
Substituting (5.11) into (5.2) yields the equation ėa + k1 ea = 0, which shows
exponential convergence of the θ variable to the auxiliary one θa . As a second
stage we consider evolution of the e∗ = [ex ey ]T error. From (5.4), (5.5), and
(5.8) one can write
H ∗ = kp e∗ + q̇t∗ − vsg
∗
, ė∗ = −kp e∗ + r, r = H ∗ − g ∗2 u2 . (5.13)
It can be shown that the following expressions are true:
k rk = k H ∗ k γ(θ) and lim γ(θ) = 0, (5.14)

θ→θa
p
where γ(θ) = 1 − cos2 α(θ) ∈ [0, 1] and α(θ) = ∠(g ∗2 (θ), H ∗ ). Introducing a
positive definite function V = 12 (e∗T e∗ ) and using (5.13,5.14) one can assess
its time derivative as follows:
6
The term sgn(u2t ) remains constant during a tracking task for the considered
persistently exciting reference trajectories (see the comment after Problem 1).
V̇ = e∗T ė∗ = e∗T (−kp e∗ + r) = −kp k e∗ k2 + e∗T r ≤

≤ −kp k e∗ k2 + k e∗ k k rk = −kp k e∗ k2 + k e∗ k k H ∗ k γ =

= −kp k e∗ k + k e∗ k kp e∗ + q̇t∗ − vsg
∗
2
γ≤
≤ −kp (1 − γ) k e∗ k + γ k e∗ k κ = −W (e∗ , q̇t∗ , vsg
∗
2
, γ),

where κ = k q̇t∗ k + vsg
∗
. Function W (τ ) is positive definite for
γ(τ )κ(τ )
k e∗ (τ )k > Γ (τ ), where Γ (τ ) = . (5.15)
kp (1 − γ(τ ))
Function Γ is finite for γ < 1. The case γ = 1 in (5.15) is possible, but is

temporary and non-attractive. It can occur only during a transient stage (for
some τ = τ < ∞). Moreover, W (τ ) = − k e∗ (τ )k κ(τ ) < ∞ ⇒ V̇ (τ ) < ∞ (fi-
nite time escape for k e∗ k when γ = 1 is not possible). Since γ(τ ) can never get
stuck in γ = 1 and ∃τγ <∞ : ∀τ >τγ γ(τ ) < 1 (as a direct consequence of (5.14)
and exponential convergence of ea ) and since κ ∈ L∞ (from assumption),
one can conclude that (5.15) is determined for finite Γ (τ ) almost always and
∀ τ > τγ . As a consequence k e∗ (τ )k ∈ L∞ and also V, W, k H ∗ k , k rk ∈ L∞ .
Now, from (5.13) k ė∗ k ∈ L∞ and hence V̇ ∈ L∞ . Since it can be shown
that W (τ ) is uniformly continuous and integrible for τ ∈ [0, ∞), the Bar-
balat’s lemma implies limτ →∞ W (τ ) = 0 ⇒ limτ →∞ [k e∗ (τ )k − Γ (τ )] = 0.
Recalling the limit from (5.14) and since limτ →∞ ea (τ ) = 0, one concludes:
limτ →∞ γ(τ ) = 0 ⇒ limτ →∞ Γ (τ ) = 0 and finally limτ →∞ k e∗ (τ )k = 0.
The boundedness of θa and θt together with the exponential convergence of
ea implies θ ∈ L∞ , and eθ ∈ L∞ .
Remark 1. Control function (5.11) has a discontinuous nature. It results from
definition (5.9), which is not determined for H ∗ = 0 (the reason of the assum-
ption in Proposition 1). The equality H ∗ = 0 relates to two cases (compare
(5.13) and (5.5)): A) for e∗ 6= 0 ∧ vsg
∗
= h∗ – only during a transient stage or
∗ ∗ ∗
B) for e = 0 ∧ vsg = q̇t – when a system evolves exactly along a reference
trajectory qt∗ . Since for H ∗ = 0 also u2 = 0 (see (5.12)), the only term
which drives a subsystem q̇ ∗ = g ∗2 u2 + vsg
∗ ∗
is the skid velocity vsg . Hence,
∗
both cases describe a situation when the skid velocity vsg drives a system
toward7 qt∗ (case A) or along qt∗ (case B). Moreover, for case A it can be
seen from (5.13) that for H ∗ = 0 holds: r = 0 ⇒ ė∗ + kp e∗ = 0. As a
consequence, the boundedness and convergence of e∗ are still preserved. Both
cases are connected with a u1 control discontinuity set, but they seem to be
non-attractive, non-persistent, and unlikely in practice. However, to obtain a
well-defined control u1 , one proposes to introduce additional definitions for θa
and θ̇a in assumed sufficiently small ǫ-vicinity of H ∗ = 0. One proposes to
take
for k H ∗ k ≤ ǫ,
∆ ∆
θa = θa (τ− ) and θ̇a = 0 (5.16)
7
Note that h∗ defines an instantaneous convergence direction.
64 M. Michalek

where 0 < ǫ < inf τ q̇t∗ (τ ) − vsg
∗
(τ ) and τ− denotes the time instant of
reaching the ǫ-vicinity. These additional definitions together with (5.9) allow
the control function (5.11) to remain unchanged.
Remark 2 . Since the H2 , H3 and Ḣ2 , Ḣ3 terms are used in the control law
(the later two in the θ̇a term), one has to be able to compute or measure
skid components vsxg , vsyg and their time derivatives. It is a serious practi-
cal challenge to obtain these quantities. Estimating time derivatives of skid
components seems rather impractical in a noisy environment. Hence, assuming
relatively slow variation of skid components in comparison to other derivatives
in the θ̇a term, it is justified in practice to take these quantities as equal to
zero. The problem of skid computation will be considered in the next section.
5.3.3 Skid Computation
We assume that the state variables θ, x, y of a vehicle can be measured

with a sampling interval Tp using for example some exteroceptive sen-
sor (e.g. a vision system). With this assumption the simplest way to
obtain skid components vsxg , vsyg is to estimate them from Eq.(5.2). In
practice, the estimates computed in such a way result from equations:
v̂sxg (n) = ẋ(n) − u2 (n − 1) cos [θ(n − 1) + εθ (n − 1)] + εx (n), (5.17)

v̂syg (n) = ẏ(n) − u2 (n − 1) sin [θ(n − 1) + εθ (n − 1)] + εy (n), (5.18)
where ẋ(n) = [x(n) − x(n − 1)]/Tp , ẏ(n) = [y(n) − y(n − 1)]/Tp ,

u2 (n) is a pushing input sample from (5.12) and εθ , εx , εy denote ran-
dom measurement noise always present in practical implementations. If
noise causes high estimate variances, one can filter right-hand sides of
(5.17,5.18) and use filtered components v̂sxg F , v̂ F (filtered-estimator).
syg
The skid estimates can now be used in definitions (5.8) instead of true
signals (vsxg , vsyg ), which are not known in practice.
The verification of the effectiveness of the proposed controller has

been carried out for the kinematics of a differentially driven two-
wheeled mobile vehicle. To make simulation results more realistic, two
main practical issues have been taken into account: 1) a limitation
on the maximum vehicle wheel velocity ωwmax has been imposed8 with
ωwmax = 81[rad/s], 2) the skid filtered-estimator9 (5.17,5.18) computed
8
The control vector scaling procedure described in [7] has been used.
9
A first order low-pass filter with a time constant T = 0.1 has been applied.
2 0.8
e
[m/s]
eφ [rad], ex, ey [m] θ
e 0.6
x
1
syg
ey 0.4
[m/s], v
0.2
0
v
sxg
sxg
0
vsyg
v
−1 −0.2
0 5 τ [s] 10 15 0 5 τ [s] 10 15
3 100
u
1 ωR
u1 [rad/s], u2 [m/s]
u2 80 ω
2
ω , ω [rad/s]
L
60
1
L
40
R
0
20
−1 0
0 5 τ [s] 10 15 0 5 τ [s] 10 15
4 4
3 3
2 2
y [m]
y [m]
1 1
0 0
−1 −1
−4 −3 −2 −1 0 1 −4 −3 −2 −1 0 1
x [m] x [m]
Fig. 5.3. Tracking error, skid estimate, and control signal time-plots – top
four plots; vehicle (- -) and reference (-) paths on a plane – bottom two plots
(the last one on the right-hand side for the case without skid compensation)
with a sample time Tp = 0.01[s] has been used with measurement zero-
mean Gaussian errors εx , εy , and εθ with variances σ 2 = 0.01 included
in Eqs.(5.17,5.18). For simulation tests a circle-like trajectory computed
for u1t = 0.5[rad/s], u2t = 1.2[m/s], and qt (0) = [π/4 0 0]T has been
chosen as a reference trajectory. Moreover, the following values have
been chosen: kp = 5, k1 = 10, q(0) = [0 − 2 1]T . During simulation the
constant skid with components vsxg = vsyg = 0.5[m/s] has appeared
in the time instant t1 = 5[s]. The results obtained10 are illustrated in
Fig. 5.3. It can be seen that position tracking errors tend toward zero
quite fast and remain there also after the skid appearance (starting
from t1 = 5[s]). It is worth to note the compensative action of control
66 M. Michalek
signals and bounded but compensative behavior of the vehicle orienta-

tion (crabwise motion). The bottom-left plot shows the vehicle path on
a plane, which can be compared with the bottom-right plot obtained
for a case without skid compensation (vsxg = vsyg ≡ 0 in (5.8)).
5.5 Concluding Remarks
The VFO position tracking control with skid effect compensation for
kinematic mobile vehicles has been presented. The proposed control
strategy results from an extension of the original concept described in
[2]. The extension can be treated as a first attempt to use the VFO con-
trol method for dynamical systems with a drift. The simulation results
included in the paper illustrate the effectiveness of the proposed VFO
control scheme and also reveal its relative robustness to measurement
noises and control input limitations.
References
1. J. Ackermann. Robust control prevents car skidding. 1996 Bode Prize
Lecture. IEEE Control Systems, pages 23–31, 1997.
2. M. Michalek. Vector Field Orientation control method for subclass of non-
holonomic systems (in Polish). PhD thesis, Poznań University of Technol-
ogy, Chair of Control and Systems Engineering, Poznań, 2006.
3. I. Harmati, B. Kiss, and E. Szádeczky-Kardoss. On drift neutralization
of stratified systems. In Robot Motion and Control. Recent Developments,
volume 335 of LNCIS, pages 85–96. Springer, 2006.
4. T. Holzhüter and R. Schultze. Operating experience with a high-precision
track controller for commercial ships. Control Engineering Practice,
4(3):343–350, 1996.
5. I. Kolmanovsky and N. H. McClamroch. Developments in nonholonomic
control problems. IEEE Control Systems Magazine, 15(6):20–36, 1995.
6. R. Lenain, B. Thuilot, C. Cariou, and P. Martinet. High accuracy path
tracking for vehicles in presence od sliding: Application to farm vehicle
automatic guidance for agricultural tasks. Autonomuos Robots, (21):79–
97, 2006.
7. K. Kozlowski, J. Majchrzak, M. Michalek, and D. Pazderski. Posture sta-
bilization of a unicycle mobile robot – two control approaches. In Robot
Motion and Control. Recent Developments, volume 335 of LNCIS, pages
25–54. Springer, 2006.
8. K. Kozlowski and D. Pazderski. Practical stabilization of a skid-steering
mobile robot - a kinematic-based approach. In Proc. of the IEEE 3rd
International Conference on Mechatronics, pages 519–524, Budapest, 2006.
10
Simulations have been performed with Matlab/Simulink software.
6
Vision-Based Dynamic Velocity Field
Generation for Mobile Robots∗
W. Medina-Meléndez, L. Fermı́n, J. Cappelletto, C. Murrugarra,

G. Fernández-López, and J.C. Grieco
Universidad Simón Bolı́var, Valle de Sartenejas, Apartado Postal 89000,

Caracas 1080A, Venezuela
{wmedina,lfermin,cappelletto,cmquiroz,gfernandez,jcgrieco}@usb.ve
6.1 Introduction
A control strategy much studied in the last years is the velocity field
control (VFC). In 1995, velocity fields are defined as a set of velocity
vectors located at each possible position of the robot platform inside
its workspace coding a specified task [1]. The employment of a control
scheme whose reference is a velocity field has allowed to encourage dif-
ferent control problems rather than the conventional timed trajectory
tracking, such as contour following, position control, etc.
Most of research works in the area have attempted to optimize the
design of control schemes and to improve their stability [2, 3]. Other
works have targeted other areas such as adaptive control [4, 5]; and
others have combined vision to perform visual control via velocity fields
[6, 7].
Almost all the cases previously mentioned focus on the design of
control schemes taking as reference a theoretical and time invariant
velocity field; few works have considered the problem of using time
varying velocity fields [8, 9]. The objective of this work is to offer an
algorithm for the dynamic generation of velocity fields for mobile robots
so it can surround possible obstacles over its trajectory. The algorithm
proposed uses snapshots taken by a vision system developed for this
purpose that is able to observe all the workspace of the mobile robot.
This paper is organized in the following way. Section 6.2 shows the
description of the problem. Section 6.3 offers all the details of the al-
∗
The authors gratefully acknowledge the financial support of the Research and
Development and Graduate Studies Deanships of Simon Bolivar University to
the development of this project.
70 W. Melina-Meléndez et al.
gorithm proposed. Results and its analysis are shown in Section 6.4.
Conclusions are exposed in Section 6.5.
6.2 Problem Overview

The problem of the dynamic velocity field generation can be divided
into three subproblems:
1. To determine the robot position and orientation at any moment as
well as the position, orientation, and size of the possible obstacles.
2. To create an “initial” velocity field coding a specific task. This field
is the one that will be modified if there is some obstacle inside the
robot workspace.
3. To create “evader” local velocity fields to surround, as close as pos-
sible, each detected object, and combine them with the original
field in order to not alter it significantly and obtain an obstacle-free
trajectory. The influence area of this field has to be limited as a
function of the distance to the object being analyzed.
A vision system was developed for robot and obstacle identification
and information extraction from the visual scene. For the “initial” and
“evader” velocity field generation, a noncomplex novel algorithm is
proposed.
6.3 Dynamic Velocity Field Generation
6.3.1 Vision System
The camera used provides 30 frames per second of 640×480 pixels. It

is located 2.5 m from the floor and covers an area of 4 m2 after crop-
ping images to 275×275 pixels, i.e. the region of interest. The platform
employed for development was LabView v7.1, where a Dynamic Link
Library (DLL) was created in order to improve the processing speed
of the application. The DLL is called from Matlab where obstacle-free
velocity fields will be generated.
In a previous work [10] a vision system was proposed capable of
identifying the robot through a pattern matching technique based on
a correlation function, whereas obstacles were modeled as well-known
geometric figures: squares, circles, and rectangles, and through a clas-
sification technique it was possible either to identify them or to ob-
tain their positions and orientations at any moment. The vision system
6 Vision-Based Velocity Field Generation for Mobile Robots 71
proposed here retains the pattern matching approach to the robot lo-
cation2 , but to the obstacles it employs particle analysis. Instead of
detecting known geometric shapes, particles are identified and all the
necessary parameters to generate the “evader” velocity fields are ob-
tained from them.
Mobile Robot Detection

The pattern matching algorithm [10] consists in the localization of re-
gions that match a known reference pattern on a grayscale image. The
template contains information related to edge and region pixels, re-
moving redundant information in a regular image. In the case where
a pattern appearing rotated in the image is expected, it is possible to
store specially chosen pixels, whose values reflect the pattern rotation.
The correlation process involved is based on the calculation of the
squared Euclidean distance:
X
d2f,t (u, v) = [f (x, y) − t(x − u, y − v)]2 , (6.1)
x,y
where f is the image, and the sum is over (x, y), in the window contain-
ing the sub-image t located at u, v. Expanding d2 , and making some
considerations [10], the remaining term of the cross correlation
X
C(u, v) = f (x, y)t(x − u, y − v) (6.2)
x,y
is the similarity or matching measure between the image and the tem-
plate.
Particle Analysis
A particle is a group of contiguous nonzero pixels in an image. Particles
can be characterized by measurements related to their attributes, such
as particle location, area, and shape [11].
The process to detect a particle consists in passing the image
through a threshold filter in order to obtain a binary image. Then,
detecting nonzero pixels, and studying their neighborhoods consider-
ing connectivity ‘8’, to prevent appearing of small holes, it is possible
to obtain a particular particle of an arbitrary shape.
For each particle detected, the following parameters are extracted:
2
refer to [10] for implementation details.
Fig. 6.1. Parameter extraction process from particle analysis
• F : Maximum Feret Diameter;

• Fx1 : X coordinate of the start point of the Maximum Feret Diam-
eter;
• Fy1 : Y coordinate of the start point of the Maximum Feret Diam-
eter;
• Fx2 : X coordinate of the end point of the Maximum Feret Diameter;
• Fy2 : Y coordinate of the end point of the Maximum Feret Diameter;
• angle of Max. Feret Diameter β = atan ((Fy2 −Fy1 )/(Fx2 −Fx1 ));
• Convex Hull pixel coordinates (array of coordinates, i.e., (X, Y )
pairs).
The Feret Diameter is the line segment connecting the two points of the
particle that are the farthest apart and the Convex Hull is the smallest
convex polygon containing all points in the particle.
With the convex hull pixel coordinates it is possible to obtain the
point at each side of the Feret Diameter that is farthermost from it
forming a perpendicular line with it. The sum of both distances gives
the small side of the minimum rectangle that surrounds the particle.
Its large side corresponds to the Feret Diameter. The center of the
rectangle is obtained shifting the Feret Diameter over a line.
Knowing this minimum rectangle, the parameters to obtain circum-
scribed ellipse equation are easily obtained through algebraic and geo-
metric manipulation. Figure 6.1 illustrates the process just described.
6.3.2 Initial Velocity Field Generation
This system allows user defined trajectory to be followed by the robot.

It can be a handmade trace or a set of straight lines. The algorithm
developed was tested for 41×41 velocity fields and can be described as
follows.
The vision system takes a snapshot of the robot’s workspace. This

image is cropped to hold only the ROI which is subsampled to a 41×41
image (this resolution of the velocity field offers 1 vector each 5 cm,
which is less than a half of the robot dimensions). Over it, the user
traces the desired trajectory.
When the user finishes defining the desired trajectory, the coordi-
nates of the pixels to be part of the trajectory are extracted by a thresh-
olding process and stored into an N -size 1D array of (X, Y ) coordinate
pairs. N is the number of points or pixels of the initial trajectory.
The next step consists in performing a sorting process, establishing
as sorting parameter the Euclidean distance from one point to another,
organizing them from closer to more distant. If the trajectory is open,
for knowing where it begins and where ends, the neighborhood of each
element is studied.
Then an approximation vector field is defined. For that, consider a
2D array of 41×41 elements containing the coordinates (X, Y ) of all
pixels of a 41×41 image, i.e. element (i, j) has a value of (i, j). For
each element of the 2D array, the closest element of the trajectory is
searched based on the Euclidean distance from point (i, j) of the 2D
array to each element of the 1D array containing the coordinates of the
trajectory. Each approximation vector is defined as the normalized one
whose direction is obtained from the subtraction of the closest point of
the trajectory and the point of the 2D array being analyzed.
A tangent vector field is also defined. Each vector is obtained from
the subtraction of the element p + 1 with element p − 1, where p is
the index of the closest point of the trajectory to the point (i, j) being
studied. If the closest point is the point where the trajectory begins,
the subtraction is performed between elements p + 2 and p (p = 0),
whereas if it is the ending one, the points subtracted are the p and
p−2 (p = N −1). With this assumption, the tangent vectors will always
have congruent senses. The tangent vectors are normalized, too.
The “initial” velocity field is obtained performing a weighted sum,
expressed in (6.3), between the approximation and tangent vector fields.
The selection of weights depends directly on the distance between point
(i, j) and the closest one of the trajectory. As a weight function a sig-
moidal function was chosen. If point (i, j) is close to the trajectory, the
tangent vector will prevail over the approximation one and vice versa.
Vij = Vaij · f1 (dij ) + Vtij · f2 (dij ), (6.3)
where Vij is the vector of the desired velocity field, Vaij and Vpij are
the approximation and tangent vectors at ij, respectively. dij is the
Euclidean distance from point ij to the desired trajectory, whereas

f1 (dij ) and f2 (dij ) are defined by:
2
f1 (dij ) = − 1,
1 + e−α·dij (6.4)
f2 (dij ) = 1 − f1 (dij ).
Parameter α was chosen to be 0.4 because this value allows an im-

portant attenuation of the tangent vectors when dij > 6 (3 times the
dimensions of the robot). Figure 6.2 shows the effect of the weighting
functions expressed in (6.4).
Fig. 6.2. Effect of weighting functions f1 and f2 . Note that for small distances
the tangent vector is greater than the approximation one, and vice versa.
6.3.3 Dynamic Velocity Field Modification

The “evader” velocity field generation module takes information pro-
vided by the vision system, parameterizes the corresponding ellipse for
each obstacle and creates a velocity field that surrounds the object.
The proposed algorithm contemplates dividing the ellipse into four
regions: one for entry, one for exit, and two for transitions.
In the transition regions the velocity field is chosen to be parallel to
the trajectory given by the ellipse contour, i.e. tangent to the ellipse.
The general tangent line equation at any point (X0 , Y0 ) is given by
(X − P )(X0 − P ) (Y − Q)(Y0 − Q)
+ = 1, (6.5)
A2 B2
where (P, Q) are the coordinates of the location of the ellipse, and A
and B represent a half of the major and minor axes, respectively. From
(6.5), the unit tangent vector at any point (X0 , Y0 ) is deduced to be:
Vt (X0 , Y0 ) = (Vtx , Vty ),

A2 · Y0
Vtx = p ,
A4 · Y02 + B 4 · X02 (6.6)
−B 2 · X0
Vty = p .
A4 · Y02 + B 4 · X02
In the entry region the field is defined in the direction and sense
toward the ellipse contour, being turned aside smoothly as the point is
closer to the transition region, converging to the tangent vector. This
is achieved through a weighted sum of the approximation and tangent
vectors to the ellipse, where the weights depend on the distance from
a given point to the edge between the entry and transition regions.
The entry and exit regions have always the same size. The same is
also true for the transition regions. The size (angle) for the entry and
exit regions is chosen so that each one has a quarter of the area of the
ellipse, and is defined by
s !
1 + (A/B)2 · tan2 (α − β)
ϕ = 2 · atan . (6.7)
(A/B)2 + tan2 (α − β)
The orientation of the regions is given by the angle of the original field
at the point where the object is located. The regions must be rotated
for achieving an entry region aligned with the original velocity field.
For the exit region the same approach as used for the entry region is
employed. However, in this case, the field is defined leaving the ellipse.
The approximation vectors at any point (X0 , Y0 ) are given by:
Va (X0 , Y0 ) = (Vax , Vay ),
B 2 · X0
Vax = p ,
A4 · Y02 + B 4 · X02 (6.8)
A2 · Y0
Vay = p .
A4 · Y02 + B 4 · X02
The division proposed by using the defined regions wants to achieve an
“evader” velocity field similar to the sketch shown in Fig. 6.3.
6.4 Results
For testing the “initial” velocity field generator two handmade traces
were introduced. Figure 6.4 shows the obtained velocity fields.
Fig. 6.3. Sketch of the “evader” field. Following the direction and sense of
the initial field at the point where the obstacle is located, the different regions
are defined. Note the deviations of field in the entry and exit regions.
(a) (b)
Fig. 6.4. Initial velocity field. Note the handmade desired trajectory marked
gray.
The case shown in Fig. 6.4(a) corresponds to an open trajectory

having a ‘Z’ shape, and the field generated converges successfully to
it; inclusive, the end point of the trajectory results to be a sink, as
desired. Figure 6.4(b) corresponds to the well-known circular trajectory
[1, 3], here hand-traced. It is observed that the velocity field converges
as expected. It is important to remark that while the distance to the
desired trajectory is higher the approximation vectors prevail over the
tangent ones, and when it is lower, the tangent ones prevail.
The “evader” algorithm responds to an arbitrary object as shown
in Fig. 6.5.
(a) (b)
Fig. 6.5. “Evader” velocity field for an arbitrary object. The four predefined
regions are shown and the behavior of the algorithm can be observed.
Figure 6.5(a) shows an object located in a place where the original

field has an orientation of α = 140◦ and the circumscribed ellipse has
β = 75◦ . Figure 6.5(b) presents the evader field for an object whose
circumscribed ellipse has β = 80◦ and the original field, at the point
where the object is placed, has α = −30◦ . In both figures the exponen-
tial fading imposed is shown. This effect assures that the evader field
only affects the neighborhood of the object.
Next, a test of the system with the three modules combined is pre-
sented. Figure 6.6 resumes the results obtained. Inserting four arbitrary
(a) (b)
Fig. 6.6. Modified velocity field for four obstacles detected
objects at arbitrary positions for the velocity fields shown in Fig. 6.5,
the final ones obtained offer obstacle-free trajectories, however, at the
edges between the influence “evader” field and the initial one, they are
not as smooth as desired.
The trajectories shown in Fig. 6.6 were obtained through simulation,
applying Runge-Kutta iterations. Note that they start at four different
points, and each trajectory converges to the path defined by the velocity
field generated. It is important to remark that these trajectories are
smooth, including the area where the field is not.
Practical validations were performed using a differential drive wheel-
ed robot, controlling its orientation at any moment through a PI con-
troller. Details about these tests are beyond the scope of this article
and will be presented in the future.
6.5 Conclusions and Future Work
A dynamic velocity field generation algorithm was proposed and tested.

The vision system developed allows to extract a reliable position and
orientation of the mobile robot, as well as the required parameters to
surround the obstacles detected through an evasion approach based on
ellipses. The obtained velocity fields are smooth in most of the cases.
When they are not completely smooth, experimental tests suggest that
this effect can be neglected. The proposed algorithm offers an easy way
to obtain dynamic references for velocity control schemes, a helpful
issue for the researchers in this area.
Future work will tend to improve the efficiency of the proposed al-
gorithm for achieving real-time operation. Nowadays, implementations
with FPGA modules are being developed treating three aspects: speed,
capabilities, and real-time tasks.
References
1. Li PY and Horowitz R (1995) Passive Velocity field Control of Mechanical
Manipulators. In: Proc. IEEE Int. Conf. on Rob. and Aut. ICRA’01,
Nagoya, Japan 2764–2770.
2. Cervantes I, Kelly R, Alvarez J and Moreno J (2002) A Robust Velocity
Field Control IEEE Trans. on Cont. Syst. Tech. 10:6:888–894.
3. Moreno J and Kelly R (2003) Hierarchical Velocity Field Control for Ro-
bot Manipulators. In: Proc. IEEE Int. Conf. on Rob. and Aut. ICRA’03,
Taipei, Taiwan 10:4374–4379.
4. Li PY (1999) Adaptive Passive Velocity Field Control, In: Proc. American

Control Conf., San Diego, U.S.A. 774–779.
5. Dixon WE, Galluzo T, Hu G and Crane C (2005) Adaptive Velocity
Field Control of a Wheeled Mobile Robot. In: Proc. 5th Int. Workshop
on Robot Motion and Control, RoMoCo’05, Poznan, Poland 145–150.
6. Kelly R, Moreno J and Campa R (2004) Visual Servoing of Planar Robots
via Velocity Fields. In: Proc. IEEE Int. Conf. on Dec. and Cont. Bahamas
4028–4033.
7. Kelly R, Bugarı́n E and Campa R (2004) Application of Velocity Field
Control to Visual Navigation of Mobile Robots. In: Proc. 5th IFAC Symp.
on Intell. Autonomous Vehicles, Lisbon, Portugal [On CD].
8. Yamakita M and Suh JH (2000) Adaptive Generation of Desired Velocity
Field for Cooperative Mobile Robots with Decentralized PVFC. In: Proc.
IEEE/RSJ Int. Conf. on Intell. Rob. and Syst. IROS’00, Takamatsu,
Japan 1841–1846.
9. Yamakita M and Suh JH (2001) Adaptive generation of Desired Velocity
field for Leader-Follower Type Mobile Robots with Decentralized PVFC.
In: Proc. IEEE Int. Conf. on Robot. and Autom. ICRA’01, Seoul, Korea
3495–3501.
10. Bolaños J, Medina-M. W, Fermı́n L, Cappelletto J, Fernández-L. G and
Grieco JC (2006) Object Recognition for Obstacle Avoidance in Mobile
Robots. In: Proc. Art. Intell. and Soft Comp., ICAISC 2006, Lecture
Notes in Computer Science, Zakopane, Poland, 722–731.
11. National Instruments (2005) IMAQ Vision Concepts Manual 10-1–10-19,
12-1–12-8, 16-1–16-21.
7
Zoom Control to Compensate Camera
Translation Within a Robot Egomotion
Estimation Approach∗
Guillem Alenyà and Carme Torras
Institut de Robòtica i Informàtica Industrial (CSIC-UPC),

Llorens i Artigas 4-6, 08028 Barcelona, Spain
{galenya,torras}@iri.upc.edu
7.1 Introduction
Zoom control has not received the attention one would expect in view
of how it enriches the competences of a vision system. The possibility
of changing the size of object projections not only permits analysing
objects at a higher resolution, but it also may improve tracking and,
therefore, subsequent 3D motion estimation and reconstruction results.
Of further interest to us, zoom control enables much larger camera
motions, while fixating on the same target, than it would be possible
with fixed focal length cameras.
Automating zoom control is thus a very promising option for vision
systems in general, and robotic applications in particular. One such
application, container transfer within a warehouse, where the trajectory
of a mobile robot needs to be traced with low precision demands but
without any presetting of the environment, has motivated our work [1].
We devised a method to estimate robot egomotion from the image flow
captured by an on-board camera. Following the works of Blake [3] and
Martı́nez and Torras [9, 10], instead of using point correspondences, we
codify the contour deformation of a selected target in the image with
an affine shape vector.
The two main limitations of the method are that all along the robot
trajectory the target must be kept visible and it should be viewed un-
der weak-perspective conditions (i.e., the depth variation of the target
should be small compared to its distance to the camera). Note that
∗
This work is partially funded by the EU PACO-PLUS project FP6-2004-IST-
4-27657. The authors thank Gabriel Pi for their contribution in preparing the
experiments.
springerlink.com
82 G. Alenyà and C. Torras
for a robot vehicle such as that of the warehouse this reduces the set
of possible motions almost to just looming and receding. The former
limitation can be overcome by mounting the camera on a pan-and-tilt
device, while the latter calls for automating zoom control to compen-
sate translation along the optic axis, as addressed in this work.
There are a few papers presenting different strategies for zoom con-
trol. Fayman et al. [4] consider a planar target and robot translations
only along the optic axis. In order to keep a constant-sized image pro-
jection of the target, they propose a technique, named “zoom tracking”,
aimed at preserving the ratio f /Z. A thick-lens camera model and full
calibration is assumed. Tordoff and Murray [12] address also the prob-
lem of fixating the target size in the image, but considering general
robot motion, and perspective and affine camera models. With the
perspective model, only the case of pure rotating cameras is tackled, as
the algorithm needs continuous auto-calibration. This algorithm relies
also on preserving the ratio f /Z. The authors report some problems
for planar targets, far ones, and in situations where perspective effects
are not present or discrete, as common in broadcast or surveillance.
We have been investigating the potential of the affine shape repre-
sentation of the deformation induced by camera motion on an active
contour in the image plane [1]. From this representation, egomotion can
be recovered, even in the presence of zooming, as will be presented in
Section 7.2. In Sections 7.3 and 7.4 our method to recover affine scale
and generate zoom demands is introduced. Experimental results are
presented in Section 7.5 and finally some conclusions are collected in
Section 7.6.
7.2 Mapping Contour Deformations to Camera Motions
The motion of a robot carrying a camera induces changes in the image

due to changes in the viewpoint. Under weak-perspective conditions,
every 3D motion of the camera results in an affine deformation within
the image of the target in the scene. The affinity relating two views is
usually computed from a set of point matches [7, 11]. Unfortunately,
point matching can be computationally very costly, it being still one of
the key bottlenecks in computer vision. Instead, in this work we explore
the possibility of using an active contour [3] fitted to a target object.
The contour, coded as a B-spline [5], deforms between views leading to
changes in the location of the control points.
It has been formerly demonstrated [3, 9, 10] that the difference in
terms of control points Q′ − Q that quantifies the deformation of the
7 Zoom Control to Compensate Camera Translation 83
contour can be written as a linear combination of six vectors. Using

matrix notation,
Q′ − Q = WS, (7.1)
where x y
1 0 Q 0 0 Q
W= , , , , , (7.2)
0 1 0 Qy Qx 0
and S is a vector with the six coefficients of the linear combination.
This so-called shape vector
S = [tx , ty , M1,1 − 1, M2,2 − 1, M2,1 , M1,2 ]T (7.3)
encodes the affinity between two views d′ (s) and d(s) of the planar
contour:
d′ (s) = Md(s) + t, (7.4)
where M = [Mi,j ] and t = [tx ty ]T are, respectively, the matrix and
vector defining the affinity in the plane.
The contour is tracked along the image sequence with a Kalman
filter [3] and, for each frame, the shape vector and its associated covari-
ance matrix are updated. Considering a camera that possibly changes
the focal length, the affinity coded by the shape vector relates to the
3D camera motion in the following way [10]:

fi Z0 R11 R21
M= , (7.5)
f0 Z0 + Tz R21 R22

fi Tx u0 − ui
t= + , (7.6)
Z0 + Tz Ty v0 − vi
where Rij are the elements of the 3D rotation matrix R, Ti are the
elements of the 3D translation vector T, Z0 is the distance from the
viewed object to the camera in the initial position, f0 is the focal length
at the initial frame, fi is the current focal length, (u0 , v0 ) is the principal
point position at the initial frame and (u1 , v1 ) is its current position.
Using (7.5) and (7.6) the deformation of the contour parameterized as
a planar affinity permits deriving the camera motion in 3D space. In
particular, the scaled translation in direction Z is calculated as [10]
Tz fi 1
= √ − 1, (7.7)
Z0 f0 λ1
where λ1 is the largest eigenvalue of the matrix MMT . Note that, to

simplify the derivation, the reference system has been assumed to be
centered on the object.
7.3 Generating Zoom Demands
Image-based 2D methods rely solely on image measurements. The effect

of zooming by a factor fi /f0 is to translate the image point u along
a line going from the principal point u0 to the point x′ = ff0i u + (1 −
fi
f0 )u0 . At practical effects, this can be explained as multiplying the
calibration matrix corresponding to the first frame by the factor fi /f0 .
Assuming a unit aspect ratio, the scale s of the affinity that relates
two views can be recovered from the affine fundamental matrix FA [6].
Traditionally, it has been estimated from image point correspondences,
as the singular vector corresponding to the smallest singular value of
a matrix constructed with the normalized point correspondences. At
least 4 non-coplanar point correspondences are needed.
Instead, with the affinity representation we have introduced, we can
estimate the current scale of the affine deformation in relation to the
initial contour as a function of the largest singular value λ1 in the SVD
decomposition of MMT . We propose to use
1
e= √ −1 (7.8)
λ1
as the error function in the zoom control algorithm. It is not directly

the affine scale but it is linearly dependent on the related homothecy.
Observe that, in the estimation of robot egomotion2 , this error value is
already computed, and so no overcost is added to our process. But we
have now the possibility of taking advantage of a zooming camera. Note
also that e corresponds to the scaled translation (7.7) with the ratio
value between focal lengths equal to 1. This is effectively a 2D mea-
sure. Changing the focal length values with this error function neither
calibration nor estimation of the initial distance Z0 is needed . Further-
more, as noticed by Tordoff and Murray [12], the idea of recovering a
property of the overall projection instead of a property of the individ-
ual points is an advantadge in noisy conditions. This is just what we
attain with the weak-perspective camera model and the introduction
of the affine shape space.
2
Compared with [12], just 2D motion is recovered (coded as an affine shape defor-
mation), not structure or 3D reprojection, and no foreground/background extrac-
tion is performed, as it comes with the specification of the active contour within
the tracking algorithm.
7.4 Control and Egomotion Algorithm
A zoom control algorithm has been designed to drive the zoom lenses
with the proposed error function. In our tests, the velocity-controlled
algorithm did not provide any advantage, as the Visca protocol im-
plemented in the camera only provides a few possible velocities, and
this introduces instabilities in the control algorithm. As the precision
requirements in terms of translation compensation are not very strict
in our system, a simple proportional position controller proved to be
enough. We tuned the controller with a very simple process at the be-
ginning of the sequence. After the active contour is initialized, a pulse is
induced in the zoom position controller obtaining the relation between
the zoom change and the error computed by the error function. Note
that no camera calibration and no scene information are needed.
As we are using an uncalibrated camera, the focal length is unknown
and so is the ratio of focal lengths fi /f0 . However, we can use the ratio
of zoom positions, as demanded to the zoom mechanism of the camera.
We assume that a linear function relates the focal length and the zoom
demand. This is a good approximation when zoom positions are not in
the maximum focal length zone [8]. As a consequence, we will not use
extreme zoom positions.
When zooming is incorporated to the egomotion algorithm, changes
in the sequence of images are obviously produced. As zooming is con-
tinuously correcting camera translations, depths in the acquired images
are always very similar (this was just our objective!). Compared with a
fixed focal length set-up, the difference between the initial contour used
as a template and the current contour after the deformations induced
by camera motion, i.e., the shape vector, is smaller. Ideally, with a very
precise and very quick zoom control, the parameters of the affinity cod-
ifying the depth translations should be nearly zero. From the zooming
factor fi /f0 introduced, the Tz translation can be recovered.
7.5 Experimental Results
The experiment is performed with a Pioneer AT robot (Fig. 7.1(a)).

It has been equipped with a EVI-D31 pan, tilt, and zoom camera. For
this experiment, the pan and tilt are kept fixed at a constant value and
only zoom is controlled. A linear trajectory is performed with the robot
approaching the target. The target used is a common cylindrical trash
can (Fig. 7.1(c)). Lines normal to the contour (Fig. 7.1(d)) are search
lines along which the tracking algorithm searches peak gradients, with
(a) (c) (e)
(b) (d) (f)
Fig. 7.1. Images illustrating the performed experiment. (a) Robot at the ini-
tial position and (b) robot at the final position, after performing a translation.
(c) Detailed image of the target, a common trash can. (d) The initial image
of the trash can with an active contour fitted to it. (e) Image acquired at the
final position after the zoom has been changed with the proposed zoom control
algorithm. (f ) Image acquired at the final position without zoom changes.
which the shape vector is calculated. While the robot is moving, for
each acquired image the tracking algorithm estimates the affine shape
deformation of the current contour with respect to the initial one, and
computes the egomotion. At frame rate, in our current implementa-
tion at 20 fps, the system is capable of generating a zoom demand to
cancel in the image the effects of robot translation. Figure 7.1(e) shows
that the zoom control has effectively cancelled the approaching motion.
Figure 7.1(f) shows the resulting image if the zoom control is deacti-
vated. As can be seen, the target projection is much bigger, and after
a small approaching translation the target would project out of the
image plane.
Figure 7.2(a) shows the computed error function. Observe that it
is centered at 0 and the errors keep always small. In Fig. 7.2(b) the
600 600 500
400 0
500
200
Tz tracker
−500
zoom
Tz
0 400
−1000
−200
300
−400 −1500
−600 200 −2000

0 500 1000 1500 0 500 1000 1500 0 500 1000 1500
frame frame frame
(a) (b) (c)
Fig. 7.2. Results of the experiment entailing zoom position control. (a) Error
function in the center, and variance values up and down. (b) Zoom position as
demanded by the algorithm. (c) Reconstructed Tz translation in the center,
and variance values up and down.
zoom positions resulting from the zoom control algorithm are plotted.
As a trajectory approaching the target is performed, the camera zooms
out, leading to lower zoom values. The recovered scaled translation is
plotted in Fig. 7.2(c). Here the initial distance was set to 3500 mm.
The recovered translation is scaled, as typical in monocular vision. If
we would like to obtain metric information, the focal length of the cam-
era should be known. As we are using a zooming camera, the relation
between the zoom position and the corresponding focal length should
be computed.
7.6 Conclusions and Future Work
Based on the deformation of an active contour fitted to a target, we

have shown how to generate zoom control demands that compensate for
robot translation along the optic axis, thus keeping the virtual distance
from the camera to the target approximately constant. This widens the
range of applicability of an algorithm we previously proposed for robot
egomotion estimation, in that it permits longer robot translations along
the optic axis while preserving weak-perspective conditions.
The proposed zoom control uses a measure of the scale of the affinity
and, in additional experiments not included due to length limitations,
is shown to be robust to rotations of the robot. No overcost is added
to the image processing algorithm, as the proposed error measure is
already computed as a partial result within the egomotion extraction
algorithm.
We are currently working on incorporating pan-and-tilt control to

the egomotion recovery algorithm. Preliminary results are promising
when using also the shape vector estimation as error function within
the pan-and-tilt control procedure, as done here for the zoom. This will
further lengthen the robot trajectories that our egomotion estimation
algorithm could handle.
References
1. G. Alenyà, J. Escoda, A.B.Martı́nez, and C. Torras. Using laser and vision

to locate a robot in an industrial environment: A practical experience. In
Proc. IEEE Int. Conf. Robot. Automat., pages 3539–3544, Apr. 2005.
2. G. Alenyà, E. Martı́nez, and C. Torras. Fusing visual and inertial sensing
to recover robot egomotion. Journal of Robotic Systems, 21:23–32, 2004.
3. A. Blake and M. Isard. Active contours. Springer, 1998.
4. J.A. Fayman, O. Sudarsky, E. Rivlin, and M. Rudzsky. Zoom tracking
and its applications. Machine Vision and Applications, 13(1):25 – 37,
2001.
5. J. Foley, A. van Dam, S. Feiner, and F. Hughes. Computer Graphics.
Principles and Practice. Addison-Wesley Publishing Company, 1996.
6. R. Hartley and A. Zisserman. Multiple View Geometry in Computer Vi-
sion. Cambridge University Press, 2 edition, 2004.
7. J. Koenderink and A. J. van Doorn. Affine structure from motion. J. Opt.
Soc. Am. A, 8(2):377–385, 1991.
8. M. Li and J.-M. Lavest. Some aspects of zoom-lens camera calibration.
IEEE Trans. Pattern Anal. Machine Intell., 18(11):1105–1110, 1996.
9. E. Martnez and C. Torras. Qualitative vision for the guidance of legged
robots in unstructured environments. Pattern Recognition, 34:1585–1599,
2001.
10. E. Martı́nez and C. Torras. Contour-based 3d motion recovery while
zooming. Robotics and Autonomous Systems, 44:219–227, 2003.
11. L.S. Shapiro, A. Zisserman, and M. Brady. 3D motion recovery via affine
epipolar geometry. Int. J. Comput. Vision, 16(2):147–182, 1995.
12. B. Tordoff and D. Murray. Reactive control of zoom while fixating us-
ing perspective and affine cameras. IEEE Trans. Pattern Anal. Machine
Intell., 26(1):98–112, January 2004.
8
Two-Finger Grasping for Vision Assisted
Object Manipulation∗
Umar Khan, Thomas Nierobisch, and Frank Hoffmann
Chair of Control and Systems Engineering, Universität Dortmund, Germany

{umar.khan,thomas.nierobisch,frank.hoffmann}@uni-dortmund.de
8.1 Introduction
The problem of object manipulation with no prior knowledge of object
pose and geometry constitutes a key problem in the domain of service
robotics. The grasping methodologies reported so far use either force
closure or friction between the fingers and the object surfaces. Force
closure leads to stable grasping such that the total sum of forces act-
ing upon the object becomes zero [1]. Grasping methods based upon
friction are better suited for grippers with limited degrees of freedom,
for which force closure is infeasible due to gripper geometry. The fun-
damental objective is to maximize the contact area and thereby the
friction force between the gripper and the object by selecting suitable
contact points. In order to determine optimal contact points, [2] and
[3] proposed vision based grasp point selection from the extracted con-
tours of the object. Subsequently, the force applied to the object is
determined based upon the selected grasp point and object properties.
The force required for stable grasping is difficult to predict due to the
presence of friction in gripper actuators as well as the nonlinear behav-
ior of gripper and object materials. Conventional control strategies are
not suitable for such conditions. Therefore [4] investigates a fuzzy logic
approach for force control. Reference [5] addresses this issue from a dif-
ferent perspective as slippage is detected by means of dynamic tactile
sensing.
This paper presents a vision assisted approach to grasping for un-
known object pose. The end effector is first visually servoed to a previ-
ously demonstrated pre-grasping pose. Afterwards suitable grasp points
∗
Supported by German Research Foundation (DFG) in collaboration with “Com-
putational Intelligence” (SFB 531).
90 U. Khan, T. Nierobisch, and F. Hoffmann
Ȗ
TCP force sensors
YTCP
ZBASE XTCP
YBASE
ZTCP
XBASE ZOBJ
YOBJ proximity sensors
XOBJ
Fig. 8.1. Left) Manipulator Katana 6M with TCP and object coordinate
system Right) Standard two-finger gripper with IR proximity and force sensors
are selected using the proximity sensors and the grasp closure is finally
executed by regulating the force applied upon the object. The experi-
mental evaluation of the two-finger grasping for vision assisted object
manipulation is based upon the Katana 6M manipulator and a two
finger gripper depicted in Fig. 8.1. The camera is mounted to the end
effector above the gripper in an eye in hand configuration. The grip-
per is equipped with eight infrared proximity sensors and four force
sensors.Afterwards the object is scanned with infrared sensors in order
to select an optimal grasping configuration that achieves a four-point
contact between the object and the gripper. After attaining the final
grasp pose the gripper finger is regulated by using a fuzzy controller
in order to grasp the object without slippage or damage. The effective-
ness and robustness of the object manipulation scheme is confirmed
in an experimental evaluation on a real robot arm with a eye-in-hand
configuration and a standard two-finger gripper.
The paper is organized as follows: Section 8.2 introduces the visual
servoing scheme with a dynamic set of SIFT features. The grasp point
selection as well as the force control are explained in Section 8.3. The
experimental results of object manipulation are presented in Section
8.4 and the paper concludes with a summary in Section 8.5.
8.2 Visual Servoing with a Dynamic Set of SIFT

Features
Visual features based upon scale invariant feature transformations

(SIFT) introduced by [6] offer particular advantages for the purpose
8 Two-Finger Grasping for Vision Assisted Object Manipulation 91
of image based visual servoing. Most importantly this transformation

is independent of changes in scale, orientation, illumination, and affine
transformations. In addition, SIFT features are uniquely identifiable
across multiple views of an object, which allows the development of
robust model free image based visual servoing schemes [7]. Circumnav-
igating the known limitations of classical image based visual control
our approach draws inspiration from the moment-based paradigm [8].
For our application the visual servoing task of attaining a pre-grasping
pose is described by 4 DOF. Accordingly, the visual servoing controller
employs four moments of the SIFT features, one for each degree of
freedom. A single SIFT feature captures four attributes, namely its
pixel coordinates ui and vi , canonical orientation φi , and scale σi . The
scale varies inversely proportional with the distance between the cam-
era and the object, whereas the orientation φi is equal to the rotation
around the camera axis. Therefore the visual moments fγ and fz are
defined by the average orientation and the average scale, respectively.
The translation along x- and y-axis is captured by moments fx and fy ,
respectively. These moments are given by the following relationships:
n
P n
P n
P n
P
ui vi σi φi
i=1 i=1 i=1 i=1
fx = , fy = , fz = , fγ = . (8.1)
n n n n
SIFT features are matched between the current and the reference view.
The moments of the SIFT features are computed over the subset of
features matched between both views and compared to each other.
The manipulator motion is regulated such that the difference between
the desired and current moments is gradually reduced. The control law
for the gripper motion is given by
ẋ = −Kx ∆fx ẏ = −Ky ∆fy ż = −Kz ∆fz ωz = −Kγ ∆fγ , (8.2)
where ∆fx , ∆fy , ∆fz , ∆fγ are the moment errors and Kx , Ky , Kz ,
Kγ are the corresponding gain factors. Notice that in principle a sin-
gle SIFT feature is sufficient to achieve convergence to the goal pose.
Nonetheless, using aggregated moments computed over an entire subset
of matched SIFT features increases the accuracy and the robustness of
the control scheme. The method for visual servoing with sets of SIFT
features in 6 DOF is detailed in [7]. Once the desired pose with respect
to the object is attained, the end effector is moved forward by a fixed
amount equal to the distance between the camera and the link to which
it is attached in order to position the gripper above the object.
8.3 Grasp Point Selection

For stable grasping a large contact area enhances the friction force.
Therefore the geometry of the gripper is crucial for grasp point selec-
tion. Given the dimensions and geometry of the gripper it is not feasible
to grasp objects of complex shape, in particular with non-parallel op-
posite surfaces. Establishing a three- or four-point contact with this
gripper would require an object model for grasp planning. Even with
grasp planning it would be difficult to realize a stable grasp for most
types of objects. Therefore no grasp planning is implemented and the
grasping scheme assumes and is restricted to objects that have a rec-
tangular cross-section. In the following L1 , L2 , and L3 denote the IR
sensor proximity readings as depicted in the left-hand part of Fig. 8.2.
θ corresponds to the opening of the gripper and δ denotes the slope be-
tween the finger front plane R2 and the finger rear plane R1 . ∆ is the
vertical distance between the gripper axis of rotation and the location
of the center proximity sensor, whereas w indicates the object width.
The right-hand part of Fig. 8.2 illustrates an example of a four-point
grasp. The gripper fingers are brought into contact with the object at
the selected grasp points by regulating the vertical displacement of the
gripper v and its opening angle θ. Owing to the nonlinear characteristics
of the proximity sensors a fuzzy logic scheme is utilized to compute the
appropriate opening and vertical displacement based upon the percep-
tions L1 , L2 , and L3 . Adaptive neuro fuzzy inference systems (ANFIS)
[9] automatically learn the rule base from a set of training examples.
In ANFIS the rule base and its parameters are regarded as a neural
network, which is then trained by means of gradient descent. Each item
ș
į
L3 , v
R1
w
R2
L2 L1
Fig. 8.2. Left) Infrared sensors readings and geometric model of the gripper
Right) Successful four-point contact during experiments
of the training set consists of a randomly generated input and the cor-
responding correct model output. The training data set is generated
from a model of the gripper and various object geometries and the
noise afflicted sensor characteristics. The remaining part of this section
explains the four steps of the grasping process, namely gripper object
alignment, equalization, mid-point selection and four-point selection.
Before initiating the object scan with the IR sensors an open-loop mo-
tion is performed in which the gripper is moved forward and lowered
to bring the object within range of the proximity sensors. Notice that
this open-loop motion is independent of the object and does not require
knowledge of the relative transformation between the visual reference
and the final grasping pose. The demonstrated reference pose does not
need to be accurate nor is it necessary to calibrate between reference
and grasping pose during demonstration. The human simply places the
object roughly centered underneath the camera.
Alignment: The proximity sensors measure the intensity of infrared
radiation reflected from an object’s surface, which increases with prox-
imity. The gripper fingers are first aligned with the object by regulat-
ing the gripper orientation γ along the z-axis such that the cumulative
proximity sensor measurement L1 +L2 becomes minimum, which corre-
sponds to a parallel orientation between the finger tips and the object.
Once the gripper fingers are aligned, the distance between them and
the sides of the object is maximized. The gripper alignment operates
in an iterative, exploratory fashion in which the gripper rotates with
a constant step size in one direction until the cumulative sensor read-
ings increase again. The direction of rotation is then reversed, the step
size is reduced by half and the process is repeated until the cumulative
measurement becomes constant.
Equalization: In the second equalization step the lateral displacement
of the gripper is adjusted such that both finger tips are equidis-
tant to the object. An ANFIS model predicts the corrective lateral
motion of the gripper to compensate the observed misalignment. Its
inputs are the current proximity measurement difference L1 − L2 and
the gripper opening θ. Again, the training data for the ANFIS model is
obtained from the simulation model with one thousand randomly gen-
erated input configurations and the corresponding controller outputs.
Each input configuration is described by a random gripper opening
θ ∈ [30◦ , 50◦ ] and the simulated sensor measurement L1 − L2 for a
randomly chosen lateral displacement of the gripper.
Mid-Point Selection: For selecting the object longitudinal cen-

ter point the gripper is moved parallel to the object surface until the
object edge is detected by a discontinuity in the top sensor proximity

measurement. As the gripper passes the object edge the sensor reading
decreases abruptly. The gripper then moves in the opposite direction
until the opposite edge is detected in the same manner. The gripper is
finally relocated to the object center point half way between the two
end points. This approach works if the object height remains constant
or exhibits only minor continuous variation. For objects with significant
variations in height this approach will fail.
Four-Point Selection : In the final phase, the gripper vertical sep-
aration vd and opening angle θd are computed to establish the optimal
four-point contact as the gripper closes. The gripper object separation
and opening angle are predicted by two separate ANFIS models. Each
model employs three inputs, namely the current gripper opening θ, the
current vertical separation to the object v, and the proximity finger
sensor readings. Again, the training data for the ANFIS models are
generated from a thousand simulated configurations with noisy read-
ings for objects of different width. Each training sample is generated
from a randomly chosen object width w ∈ [15 mm, 35 mm], a random
gripper opening θ ∈ [30◦ , 50◦ ], and a random vertical object gripper dis-
placement v ∈ [40 mm, 60 mm]. The corresponding correct controller
outputs are generated according to Eqs. (8.3) and (8.4), which describe
the geometric constraints to establish four-point contact for the nom-
inal gripper geometry for a specific object width. The gripper object
geometry and inputs to the ANFIS model are illustrated in Fig. 8.2.
θd = sin−1 ((R2 sin θ − 0.5(L1 + L2 ) cos θ)/R1 ) (8.3)

R2 sin θ − 0.5(L1 + L2 ) cos θ
vd = − ∆ (8.4)
tan(θd + δ)
After completion of the object scanning and grasp alignment prior
to grasp closure the attained end-effector pose is compared with the
visual reference pose. The relative transformation between both poses
is calculated by means of inverse kinematics and stored together with a
visual object representation for future reference. In subsequent manip-
ulations of the same object the grasp alignment is then performed in an
open-loop fashion based on the stored transformation. This procedure
is feasible as the SIFT features allow a unique visual recognition and
the accuracy of the visual servoing in conjunction with the open-loop
motion is sufficient to attain the grasp pose.
The gripper closure under force control is initiated once the finger
tips are in contact with the object. The objective is to regulate the
applied force during grasping such that it is neither too strong to deform
or crush the object nor too weak to prevent slippage of the object.
force sensor meas. (right)

force sensor meas. (left)
200 200
150 150
Cardboard
Wood
100 Can 100
48 50 52 54 48 50 52 54
gripper opening [°] gripper opening [°]
Fig. 8.3. Variation of force sensor measurements with respect to θ
The left- and right-hand diagrams of Fig. 8.3 illustrate the force
sensor measurements for the sensors embedded in the left and right
fingers, respectively. The measurements are recorded for three different
objects. This variation in the sensor characteristics is caused by the
different compressibility and stiffness of the object as well as the contact
area between the object and the finger. The contact forces are absorbed
by the finger padding and the object’s compressibility. Due to this
uncertain and non-linear behavior a PD-fuzzy controller is designed
and tuned on the real system to regulate the force applied to the target
object. The error between the desired and the currently applied force
and its derivative are fed into the fuzzy system. The output is the
velocity θ̇ with which the fingers are closed. The setpoint for the force
control fd is
fd = 0.5 mg/µ, (8.5)
where µ denotes the friction coefficient and m the mass of the object.
The motion stops once the nominal contact force fd on either side is
exceeded.
8.4 Experimental Results
The experimental evaluation of the proposed method in realistic sce-

narios is based upon eight objects of different color and texture shown
in Fig. 8.4. A wide variety of textured objects including transparent and
high reflectivity objects are included in the test set to obtain a realis-
tic evaluation of the robustness of two-finger grasping. The two-finger
grasping succeeded without any failure in at least five independent
Fig. 8.4. Sample objects used to test the grasping approach
trials for objects 1, 2, 4, 5, 8, whereas failures occurred for the objects

3, 6, 7. Grasp point selection on object 3 fails due to the largely different
reflectivity of black and yellow surfaces on opposite sides. This differ-
ence in reflectivity results in an asymmetric gripper alignment despite
equal IR sensor readings. Grasping also fails for objects 6 and 7 be-
cause of their highly reflective and transparent surfaces. This leads to
erroneous measurements from the IR proximity sensors, thus causing
the grasp point selection step to fail. The two-finger grasping of the
integrated system for visual servoing approach and IR and force sensor
based grasping is evaluated and analyzed in more detail for object 8.
Figures 8.5(a,b) show the evolution of task and image space error dur-
ing the visual servoing pre-grasping phase. Both errors converge quickly
to zero with limited overshoot. The residual error for the translational
motion is less than 1 mm and less than 1◦ for the rotation. Notice that
this accuracy is sufficient for object manipulation with known object
geometry. Scanning the object with the gripper sensors for grasp point
selection is primarily necessary to retrieve the object width and length.
Figure 8.5(c) illustrates the evolution of the IR sensor reading and the
gripper motion during the equalization step. The proximity sensor mea-
surements are plotted against the left vertical scale, whereas the lateral
velocity of the gripper is plotted against the right scale. As the gripper
fingers center along the object, the proximity sensor measurements be-
come equal and the lateral motion stops. The performance of the grasp
closing phase is shown in Fig. 8.5(d). The actual force approaches the
nominal force of 3 N with a slight offset and the gripper closing stops
after about 0.5 s. In a separate experiment object 8 was successfully
picked up, moved, and released at a new position and orientation in
twenty consecutive trials without human intervention. Each trial re-
quires approximately seven seconds for servoing to the demonstrated
reference pose and an additional fifteen seconds for object scanning and
60 110 10
task space errors [mm−deg]
gripper lateral vel. [mm/sec]

∆x left sensor meas. 8
IR sensor measurements
105
∆y right sensor meas.
40 100 6
∆z
95 lateral vel. 4
∆γ
20 90 2
85 0
0 80 −2
75 −4
a c
−20 70 −6
0 2 4 6 0 1 2 3
time [s] time [s]
0.5 3 0
gripper closing vel. [deg/sec]

image space errors
2.25 −5
applied force [N]

0
∆f 1.5
x −10
∆f
y
−0.5
∆ fz 0.75 force[N] −15
∆f closing vel.
γ d
b
−1 0 −20
0 2 4 6 0 0.5 1
time [s] time [s]
Fig. 8.5. a) Task space error. b) Image space error. c) Sensor readings and
control during equalization. d) Force applied upon object and closing velocity.
grasping. Currently the major limiting factor is the low bandwidth be-
tween the host PC which runs the visual servoing and the manipulator
micro-controllers for axis control.
8.5 Conclusion
This paper presents a two-finger grasping scheme for vision assisted

object manipulation for unknown object pose and geometry. The main
contribution of this approach is the ease with which the desired gripper
pose is demonstrated. The user only holds the object roughly centered
underneath the camera, and this single demonstration alone suffices
for the subsequent servoing and grasping task. The only knowledge
needed is the object weight. In comparison with conventional vision
based grasping approaches, SIFT features offer the advantage of uni-
versal applicability to objects of daily use and eliminate the need for
depth estimation. In order to achieve a more flexible object manipu-
lation, the current scheme will be augmented with a large view visual
servoing controller [10]. Additional intermediate reference views are in-
troduced in order to navigate the manipulator and camera across the
entire view hemisphere of an object.
References
1. Han HY, Arimoto S, Tahara K, Yamaguchi M, Nguyen PTA (2001) Ro-

botic Pinching by Means of a Pair of Soft Fingers with Sensory Feedback.
In:IEEE Intl. Conf. on Robotics and Automation 2:188-195.
2. Sanz PJ, Requena A, Inesta JM, Pobil AP (2005) Grasping the Not-
So-Obvious Vision Based Object Handling for Industrial Applications.
In:IEEE Robotics and Automation Magazine 12:44-52.
3. Recatala G, Sanz PJ, Cervera E, Pobil AP (2004) Filter based control of a
gripper-to-object positioning movement. In:IEEE Intl. Conf. on Systems,
Man and Cybernetics 6:5423-5428.
4. Birglen L, Gosselin CM (2005) Fuzzy Enhanced Control of an Underac-
tuated Finger Using Tactile and Position Sensors. In:IEEE Intl. Conf. on
Robotics and Automation Page(s)2320-2325.
5. Tremblay MR, Cutkosky MR (1993) Estimating friction using incipient
slip sensing during a manipulation task. In: IEEE Intl. Conf. on Robotics
and Automation 1:429-434.
6. Lowe DG (2004) Distinctive image features from scale-invariant key-
points. In:Intl. Journal of Computer Vision 60:91-110.
7. Hoffmann F, Nierobisch T, Seyffarth, T Rudolph G (2006) Visual ser-
voing with moments of SIFT- features. In:IEEE Intl. Conf. on Systems,
Man, and Cybernetics 4262-4267.
8. Tahri O, Chaumette F (2005) Point-based and region-based image mo-
ments for visual servoing of planar objects. In:IEEE Trans. on Robotics
and Automation 21:1116-1127.
9. Jang JSR (1993) ANFIS: Adaptive-network-based fuzzy inference sys-
tems. In IEEE Trans. on Systems, Man, and Cybernetics 23 :665-685.
10. Nierobisch T, Krettek J, Khan U, Hoffmann F (2007) Optimal Large
View Visual Servoing with Sets of SIFT Features. IEEE Intl. Conf. on
Robotics and Automation
9
Trajectory Planning with Control Horizon
Based on Narrow Local Occupancy Grid
Perception∗
Lluis Pacheco and Ningsu Luo
Institute of Informatics and Applications, University of Girona, Spain

{lluispa,ningsu}@eia.udg.es
9.1 Introduction
The occupancy grid framework provides a robust and unified approach

to a variety of problems in spatial robot perception and navigation [1].
The occupancy field can be depicted by a probability density function
that relates the sensor measures with the real cell state. The Bayesian
estimation formulation is proposed by researchers; in this sense not
only the use of the last sensor observation measure but also the con-
sideration of the last occupancy estimate are proposed for occupancy.
The use of the 2D grids for static indoor mapping is proposed in [2].
Other works propose multidimensional grids for multi target tracking
by using obstacle state space with Bayesian filtering techniques [3]. The
integration of perception and planning is an interesting topic; hence
planning cycles are reported in [4], in which it is considered a time
horizon where partial trajectories are planned until the robot state is
close to the goal. This work proposes a methodology that obtains a
local cell and trajectory, within a narrow grid provided by the on ro-
bot system perception, which approaches the robot to the final desired
objective. The local data grid is presented as a horizon for making the
trajectory planning. A practical approach for differential driven WMR
(wheeled mobile robots) is shown by using monocular information and
model based control strategies.
∗
This work has been partially funded by the Commission of Science and Technology
of Spain (CICYT) through the coordinated research projects DPI-2005-08668-C03
and CTM2004-04205 and by the government of Catalonia through SGR00296.
100 L. Pacheco and N. Luo
9.2 Local Perception Horizon and Trajectory Planning
This section presents the use of a local narrow dense occupancy grid as
an available horizon of perception that allows final goal approach and
trajectory planning. It is assumed that a local occupancy grid data
is provided by the on robot perception system, the occupancy prob-
ability is divided in only two ranges: free and occupied. Hence, local
trajectories along the local grid can approach the robot to the final
goal while obstacle collisions are considered. The analysis is focused on
some particular indoor environment with flat floor surface; however it
can be applied in outdoor environments too. The problem is formu-
lated as finding the optimal cell that approaches the WMR to the de-
sired coordinates (Xd , Yd ) by finding the closer local desired coordinates
(Xld , Yld ). In this sense, the perception is considered as a local receding
horizon where trajectory is planned. Hence, a local map with the free
obstacle coordinates is provided. The local desired cell is obtained by
minimizing a cost function J, consisting in the distance between the
desired coordinates and the free local cell coordinates (X, Y ). Due to
the narrow field of perception, there are two minimizing possibilities:
Euclidian and orientation distances, as shown in Fig. 9.1(a). It can be
summarized as follows:
1
if atan(Yd /Xd ) = θd 6 θlg J = min[(X − Xd )2 +(Y − Yd )2 ] 2
(9.1)
if atan(Yd /Xd ) = θd > θlg J = min[(θd −atan(Y /X))2 ]1/2
The θlg value depicts the maximal angle that can be attained within
the local grid. The trajectory planning takes into account the following
constraints:
 
 kU (Xld , Yld )k 6 G1 
k[Xld , Yld ] − [Xo , Yo ]k > G2 ; α ∈ [0, 1)
 
k[X(k), Y (k)]−[Xd , Yd ]k 6 αk[X(k−1), Y (k−1)]−[Xd , Yd ]k
(9.2)
The parameter k is used for referencing the instant of time. The lim-
itation of the input signal, U (Xld , Yld ), is taken into account in the first
constraint as a function of the local desired points and the WMR dy-
namics. The second constraint is related to the obstacle points (Xo , Yo ),
and should include a heuristic wide-path, WP, related with the WMR
dynamics [5]. The wide-path is a heuristic concept, and should be big
enough to avoid the robot collision. Figure 9.1(b) shows these concepts,
in which the coordinates of configuration space Xcs can be obtained for
each obstacle row by using:
9 Trajectory Planning with Control Horizon 101
kXo − Xcs k = W P/ sin(π/2 − atan(Ycs /Xcs )), (9.3)

where the boundary obstacle coordinate is represented by Xo and kXo −
Xcs k denotes the necessary Euclidean distance in order to obtain the
configuration space. The last constraint, represented in (9.2), is just a
convergence criterion. The algorithm explores the grid considering just
the free positions. Using (9.1), the minimal distance corresponds to the
optimal local coordinates that should be reached. Thus, the proposed
algorithms consist in:
• To obtain the local desired coordinate Yld taking into account the
maximum geometric size of the closer obstacle.
• To obtain Xld based on Yld considering the free configuration space.
• To apply (9.1) to the whole local occupancy grid when no obstacle
exists.
(a) (b)
Fig. 9.1. (a) Two different desired points Ald and Bld computed by using
Euclidean and angular distances. (b) WP concept considered as a necessary
distance for planning safety trajectories with the local occupancy grid.
For coordinates Xld outside of grid, the angle minimization is used in

(9.1).
9.3 A Practical Approach to WMR with Monocular

Image Data
Biologically inspired machine vision systems use some eye features such
as stereopsis, optical flow, or accommodation as meaningful clues for
environment understanding [6]. This section presents the use of local
visual data and odometer information for the WMR control. It is as-
sumed that a feasible local occupancy grid based on the visual informa-
tion is provided by some available computer vision methods previously
introduced. The trajectory planning can be done by using the concepts
introduced in the previous section. Finally other considerations con-
cerning the narrow field of view, robot dimension and dynamics as well
as trajectory tracking are given.
9.3.1 Image Perception and Physical Constraints

The local visual occupancy grid provided by the camera is used for
planning a feasible trajectory. The available scene coordinates appear
as an image, in which each pixel coordinate corresponds to a 3D scene
coordinate by using the camera setup and the pose knowledge, and as-
suming projective perspective. In the case above, flat floor is assumed;
hence free cells belong to a 2D space. Figure 9.2(a) shows the cam-
era configuration studied in this work, and the angles of the vertical
and horizontal field of view and the tilt camera pose, respectively. The
vertical coordinate of the camera is represented by H. By using trigono-
metric relationships, the scene coordinates can be computed [7]. Figure
9.2(b) shows a local map provided by the camera configuration shown
in Fig. 9.2(a). The scene grids with low resolution can be used in or-
der to speed up the computing process. Hence, the processing methods
as space resolution can be used to compress images [8]. The results of
coordinate maps can be improved by using the calibration techniques
that allow removing for instance the radial distortion [9].
The narrow field of view and the fixed camera configuration make
it necessary that the robot orients towards the desired coordinates and
moves in advancing sense. A reactive zone should be considered accord-
ingly to the robot dynamics, in order to plan safety trajectories with
obstacle avoidance and safety stop distances. The dynamic reactive
zone is related to the processing time for each frame and should be big-
ger than the visual dead zone and safety stop distance. Moreover, the
wide-path of the robot should be also considered; Figure 9.3(a) shows
that not all the possible orientations allow a trajectory that includes the
wide-path of the robot. Figure 9.3(b) shows the available coordinates
considering these constraining aspects and static obstacles. Thus, the
local desired coordinates are obtained by using (9.1) within the avail-
able grid shown in Fig. 9.3(b). Reactive criteria are related with the
kind of obstacles, static or mobile, the robot dynamics and the percep-
tion speed. Thus, if static obstacles are assumed, the robot trajectory
should be large enough to allow a safety stop distance, which can be
(a) (b)
Fig. 9.2. (a) Fixed camera configuration and pose (angles α = 37◦ , β = 48◦ ;
H = 109 cm). (b) Example of local visual perception using 96×72 or 9×7
grids.
considered as a function of the commanded speeds. When mobile ob-

stacles are considered the allowed local map coordinates are reduced,
thus mobile obstacle intrusions should be considered. The robot follows
the desired goals except for the case when the obstacle contour tracking
happens, and then the local objectives are just contour following points.
This occurs when local minimum is produced. The odometer system is
used to know the WMR positions and control the trajectory tracking
relative to the local desired coordinates provided by monocular system.
9.3.2 Dynamic Models and Trajectory Control

The use of a dynamic window approach, with limited acceleration and
velocity derived from robot motion dynamics, allows reactive obstacle
avoidance [10]. In one of the interesting merging ideas [11], the use of
model based optimization schemes and convergence oriented potential
field methods is proposed. The knowledge of experimental WMR dy-
namic models allows efficient control, by considering important aspects
such as safety stop distance, while the trajectory stability is achieved
by using CLF (control Lyapunov functions).
The model for differential driven WMR can be obtained through
the approach of a set of linear transfer functions that include the non-
linearities of the whole system. The parametric identification process
is based on black box models [12]. The approach of multiple transfer
functions consists in making the experiments with three different (slow,
medium, and fast) speeds. The parameter estimation is done by using
(a) (b)
Fig. 9.3. (a) Unsafe trajectories, planned within the local grid, arise when
the heuristic wide-path concept is not considered. (b) Available local grid
coordinates (in green). The blue coordinates consider the necessary free of
obstacle dead zone. The necessary wide-path is shown in red color.
a PRBS (Pseudo Random Binary Signal) as excitation input signal,

and ARX (auto-regressive with external input) second-order structure
to identify the parameters of the system. The nonholonomic system
dealt with in this work is considered initially as a MIMO (multiple
input multiple output) system, due to the dynamic influence between
two DC motors. However, the coupling effects can be neglected if they
are less than 20%. The obtained results reveal different wheel gains
as well as the existence of dominant poles. Hence, the models can be
reduced to first-order SISO systems. The speed control is performed
by using PID controllers. A two-dimensional array with three differ-
ent controllers for each wheel is obtained. Hence, each controller has
an interval of validity where the transfer function is considered linear
[13]. In robot systems subject to nonholonomic constrains, it is usu-
ally difficult to achieve stabilized tracking of trajectory points using
linear feedback laws. It is demonstrated by Lyapunov stability theory
that the asymptotic stability exists in the control system with respect
to the desired trajectory [14]. The trajectory tracking using different
linear models can be used as a potential function of the Euclidean dis-
tance between the robot and the trajectory. Such functions are CLF,
and consequently asymptotic stability with respect to the desired tra-
jectory can be achieved. The use of piecewise analytic feedback laws
in discontinuous stabilization approach is reported in [15]. The desired
path to be followed can be composed by a sequence of straight lines and
circle segments [16, 17]. Hence, dealing with the discontinuous control
introduced, the implemented control laws use also heuristic concepts

attaining the experimental knowledge of the system. The linearity of
the model is preserved by sending command speeds that belong to its
interval of validity. Therefore, when the error of path distance of the
robot is greater than a heuristic threshold the control law can min-
imize the distance to the path; hence an approaching circle segment
to the straight trajectory is commanded. Otherwise the angular error
correction control laws should be used and straight line is tracked. The
knowledge of different models can provide information about the dy-
namics of the robot, and consequently about the reactive parameters
as well as the safety stop distances. Table 9.1 depicts these concepts
considering the three robot models, mobile obstacles until 0.5 m/s, and
a perception speed of 4 frames each second. Hence, the allowed tra-
jectory distance sets the speed that can be reached. The experimented
trajectory deviation using the control methodology presented is less
than 5 cm.
Table 9.1. Comparison of results for different robot models
Model Safety stop Obstacle reactive Robot Min. allowable

distances distances displacement distances
Low speed 0.085 m 0.125 m 0.038 m 0.248 m
(0.15 m/s)
Medium Speed 0.178 m 0.125 m 0.075 m 0.378 m
(0.30 m/s)
High speed 0.31 m 0.125 m 0.113 m 0.548 m
(0.45 m/s)
9.4 Conclusions
The present research allows a methodology based on the local occu-

pancy grid knowledge. Using this information a simple set of algorithms
is presented that allows trajectory planning and obstacle avoidance as
well as final goal approach. The connection with practical situations is
taken into account by considering a perception system mainly based
on a monocular system. The use of an odometer system, based on
encoders, is also considered in order to obtain both velocities and po-
sitions. The system dynamics knowledge is an important clue when
safety and control aspects are considered. Nowadays, the research is fo-
cused on analyzing the performance of the presented methodology by
using both real and virtual visual data. Further improvements can be
achieved by use of feasible maps or landmarks that provide local objec-

tive coordinates that guide the WMR to reach the final goal, while the
incremental localization methods can set the dead reckoning to zero
each time when a new localization is achieved.
References
1. A. Elfes (1989) Using occupancy grids for mobile robot perception and
navigation. IEEE Computer, 22:46-57
2. S. Thrun (2002) Robotic mapping: a survey. In: Exploring Artificial In-
telligence in the New Millenium, Morgan Kaufmann, San Mateo
3. C. Coue, C. Pradalier, C. Laugier, T. Fraichard, P. Bessiere (2006)
Bayesian Occupancy Filtering for Multitarget Tracking: An Automotive
Application. The International Journal of Robotic Research
4. R. Benenson, S. Petti, T. Fraichard, M. Parent (2006), Integrating Percep-
tion and Planning for Autonomous Navigation of Urban Vehicles. IEEE
Int. Conf. on Robots and Syst.
5. R.J. Schilling (1990) Fundamental of Robotics. Prentice-Hall Interna-
tional
6. B.K.P. Horn (1998) Robot Vision, McGraw-Hill Book Company, MIT
Press Edition
7. J. Campbell, R. Sukthankar, I. Noubakhsh (2004) Techniques for Evalu-
ating Optical Flow in Extreme terrain. Proc. IROS 2004.
8. R.C. Gonzalez, R.E. Woods (2002) Digital Image Processing. Prentice
Hall Int. Ed., Second Edition
9. E. Elsayed Hemayed (2003) A Survey of Camera Self-Calibration. IEEE
Conf. on Adv. Video and Signal Based Surveillance
10. D. Fox, W. Burgard, S. Thun (1997) The dynamic window approach to
collision avoidance. IEEE Robot. Autom. Mag., 4:23-33
11. P. Ogren, N. Leonard (2005) A convergent dynamic window approach to
obstacle avoidance. IEEE Trans. Robotics, 21:188-195
12. P. Van Overschee, B. Moor (1996), Subspace Identification for Linear Sys-
tems: Theory, Implementation and Applications, Ed. Kluwer Academic.
13. LL. Pacheco, N. Luo, R. Arbuse (2006) Experimental Modeling and Con-
trol Strategies on an Open Mobile Robot Platform. IEEE-TTTC AQTR06
14. R.W. Brockett (1983), Asymptotic stability and feedback stabilization,
In: Differential Geometric Control Theory, Birkhauser, Boston.
15. O.J. Sordalen, C. Canudas de Wit (1993) Exponential Control Law for a
Mobile Robot: Extension to Path Following. IEEE Trans. On Rob. and
Aut., 9:837-842
16. J.A. Reeds, L.A. Shepp (1990), Optimal paths for a car that goes forwars
and backwards. Pacific Journal of Mathematics, 145:367-379.
17. A. Astolfi (1996), Discontinuous Control of Nonholonomic Systems. Sys-
tems and Control Letters, 27:37-45.
10
Control for a Three-Joint Underactuated
Planar Manipulator – Interconnection and
Damping Assignment Passivity-Based Control
Approach
Masahide Ito and Naohiro Toda
Graduate School of Information Science and Technology,

Aichi Prefectural University, Nagakute-cho, Aichi-gun, Aichi 480-1198, Japan
masa.ito@toda.ist.aichi-pu.ac.jp, toda@ist.aichi-pu.ac.jp
10.1 Introduction
Passivity-based control of port-Hamiltonian (PH) systems has been in-

vestigated as one of powerful control techniques for nonlinear physical
systems [1]. Recently, it has become known that some underactuated
mechanical systems, i.e. systems with less degrees of freedom than the
number of generalized coordinates, can be controlled in this frame-
work [2, 3]. This control method is practicable since it is expected that
passivity will provide the robust stable closed loop [4, 5]. However, ad-
dressing the subject of underactuated manipulator, there is only an
application to Acrobot [6].
Underactuated manipulators are generally second-order nonholo-
nomic systems [7]. Most of them have difficulty on feedback control as
stated in the Brockett’s theorem [8]. Because underactuated manipula-
tors in the presence of gravity satisfy Brockett’s necessary condition [8]
for feedback stabilization, it is not clear whether there exist any smooth
time-invariant feedback laws which stabilize equilibria. Hence it may be
possible to create a control law for underactuated planar manipulators
other than Acrobot.
In this paper we use Interconnection and Damping Assignment Pas-
sivity Based Control (IDA-PBC) to construct a control system for the
2Ra -Ru planar manipulator whose first and second revolute joints are
actuated and third revolute joint is not. For a PH representation of
the system obtained by selecting appropriate coordinates and applying
a global input transformation, we clarify the applicable conditions of
springerlink.com
110 M. Ito and N. Toda
IDA-PBC and derive a concrete control law. These results are summa-
rized into Proposition 2. Numerical experiments verify the effectiveness
of the derived control law.
Notation. We denote the i-th basis vector in ℜn , the n-th order identity
matrix, and the m×n zero matrix as ei , In , and Om×n . The gradient of
a scalar functions H with respect to a vector x is represented by ∇x H.
10.2 IDA-PBC for Underactuated Mechanical Systems
The PH representation of underactuated mechanical systems without

physical damping is as follows:

q̇ On×n In ∇q H ⊤ On×m
= + u, (10.1)
ṗ −In On×n ∇pH ⊤ G(q)
△
with total energy H(q, p) = (1/2)p⊤ M −1 (q)p+V (q), where q ∈ ℜn , p =
M (q)q̇ ∈ ℜn are the generalized coordinates and moments, respectively,
M is the positive definite inertia matrix, V is the potential energy,
u ∈ ℜm represents the control inputs, and G ∈ ℜn×m is the input matrix
function with rank G = m < n. IDA-PBC for the underactuated PH
systems [9] is a stabilization method for the equilibria [qe , pe ] = [q ∗ , 0],
i.e. any position with zero velocity, by assigning a desired total energy
function
Hd (q, p) = (1/2) p⊤ Md−1 (q)p + Vd (q), (10.2)
which is a Lyapunov function to the closed-loop Hamiltonian system

q̇ On×n M −1 Md ∇q Hd⊤
= , (10.3)
ṗ −Md M −1 J2 − GKv G⊤ ∇pHd⊤
where Kv is a positive definite matrix, Md (q) ∈ ℜn×n is positive defi-

nite in the neighborhood of q ∗ and Vd satisfies q ∗ = arg min Vd (q) such
that the set of partial differential equations

G⊥ ∇q (p⊤ M −1 p)⊤ −Md M −1 ∇q (p⊤ Md−1 p)⊤ +2J2 Md−1 p = 0
(10.4)
⊥
⊤ −1 ⊤

G ∇q V − Md M ∇q Vd = 0 (10.5)
holds for some J2 (q, p) = −J⊤ 2 (q, p) ∈ ℜ

n×n . G⊥ ∈ ℜ(n−m)×n is a full
rank left annihilator of G, i.e. G⊥ G = 0 and rank G⊥ = n−m.

Acosta et al. [3] applied IDA-PBC to mechanical systems with un-
deractuation degree one, i.e. m = n − 1. Consequently they obtained
10 Control for a Three-Joint Underactuated Planar Manipulator 111
the following Assumptions and Proposition 1, parameterizing the skew-

symmetric matrix J2 in the form
 
0 p̃⊤ α1 p̃⊤ α2 ··· p̃⊤ αn−1
 −p̃⊤ α1 0 p̃⊤ αn ··· p̃⊤ α2n−3 
Xno  .. 
 ··· ··· 
J2 = p̃⊤ αi Wi =  −p̃⊤ α2 −p̃⊤ αn . ,
 .
. .
. · · ⊤

i=1  . . ·· ·· p̃ αn0 
−p̃⊤ αn−1 −p̃⊤ α2n−3 · · · −p̃⊤ αno 0
where the vector functions αi (q) ∈ ℜn are free parameters, p̃ = Md−1 p,

△
△
and matrices Wi ∈ ℜn×n , i = 1, . . . , no , no = n(n − 1)/2, are defined as
follows:
W1 = W12 , . . . , Wn−1 = W1n , Wn = W23 , . . . , Wno = W(n−1)n ,

Wkℓ = Fkℓ − (Fkℓ )⊤ ,
△
Fkℓ = {fijkℓ },

1, if j > i, i = k and j = ℓ
fijkℓ = i, j = 1, . . . , no , k, ℓ = 1, . . . , no .
0, otherwise,
Assumptions. Conditions needed for Proposition 1 are as follows:

A1. The underactuation degree of the system is one, i.e. m = n − 1.
A2. There exists G⊥ such that G⊥ ∇q (p⊤ M −1 p)⊤ = 0.
A3. The input matrix G is a function of a single element of q, qr , r ∈
{1, . . ., n}.
A4. The vector function γ and the function s defined as follows are
functions of qr only, with qr as in A3.
γ = [γ1 , · · · , γn ]⊤ = M −1 Md (G⊥ )⊤ ∈ ℜn ,
△
(10.6)
△ ⊥ ⊤
s = G ∇q V (10.7)
A5. γr (qr∗ ) 6= 0, where qr∗ is a single element of q ∗ with r as in A3.

ds
A6. γr (qr∗ ) dq (qr∗ ) > 0 with qr∗ in A5.
⊤ −1 r
A7. G M er 6= 0 with qr∗ in A5.
qr =q ∗
r
Proposition 1. (Acosta et al. [3]) Consider the system (10.1) that sat-
isfied A1–A3. Assume that there exist matrices Ψ and Md0 such that
A4–A6 hold with all desired (locally) positive definite inertia matrices
Z qr
Md (qr ) = G(µ)Ψ(µ)G⊤ (µ)dµ + Md0 . (10.8)
qr∗
Under these conditions, with

Y
: center of mass
of each link
ℓ3
: center of percussion
of 3rd link
Acceleration
of gravity
K
g0 d3
φ3 θ
ay
y
ax
2nd revolute joint φ2 3rd revolute joint
(unactuated)
(actuated)
1st revolute joint
(actuated)
d2
φ1 ℓ2
O x X
d1
ℓ1
Fig. 10.1. A 2Ra -Ru planar manipulator
Z qr
s(µ)
Vd (q) = dµ + Φ(z(q)) (10.9)
0 γr (µ)
for all differentiable functions Φ, the IDA-PBC
u = (G⊤ G)−1 G⊤ (∇q H ⊤ −Md M −1 ∇q Hd⊤ +J2 Md−1 p)−Kv G⊤ Md−1 p
(10.10)
ensures that the closed-loop dynamics is a Hamiltonian system (10.3)
with the total energy function (10.2), where the function z(q) is defined
as Z qr
△ γ(µ)
z(q) = q − dµ. (10.11)
0 γr (µ)
Moreover, [q∗ , 0] is a locally stable equilibrium with the Lyapunov func-
tion Hd (q, p) provided that the root qr = qr∗ of s(qr ) is isolated, the
function z(q) satisfies z(q∗ ) = arg min Φ(z) and this minimum is iso-
lated. This equilibrium will be asymptotically stable if A7 holds. ⊔
⊓
10.3 Control of a 2Ra-Ru Planar Manipulator by

IDA-PBC
10.3.1 PH Representation of Dynamics
We give a PH representation of a 2Ra -Ru planar manipulator, see

Fig. 10.1. Let ℓi , mi , Ii , and di denote the length, the mass, the moment
of inertia of the i-th link, and the distance of the center of mass from
the i-th joint for i = 1, 2, 3, respectively. The acceleration imposed by
gravity on the manipulator is represented by g0 = g cos ψ, where g is
the gravitational constant and ψ ∈ [0, π/2] is the angle of the motion
plane tilted with regard to the vertical plane.
△
Let q = [q1 , q2 , q3 ]⊤ = [x, y, θ]⊤ be generalized coordinates. By de-
riving Lagrange’s equation of motion and applying a global input trans-
formation [10] to it to partially linearize it with respect to (x, y), we
obtain the following state equation

d q q̇ O3×2 ax
= g0 + , (10.12)
dt q̇ − K cos q e
3 3 G(q 3 ) ay
△
where K = (m3 d23 + I3 )/(m3 d3 ) and
" #
△
1 0
G(q3 ) = 0 1 .
(1/K) sin q3 −(1/K) cos q3
△ △
Let p = [p1 , p2 , p3 ]⊤ = q̇ and u = [ax , ay ]⊤ ; the dynamics is ob-
tained in the form (10.1) with n = 3, m = 2, M = I3 , V (q3 ) =
(g0 /K) sin q3 . The equilibria of the system are in the case of g0 = 0
[ qe⊤ , p⊤ ∗ ∗ ∗
e ] = [ q1 , q2 , q3 , 0, 0, 0 ], ∀(q1∗ , q2∗ ) ∈ D, q3∗ ∈ [−π, π],
and in the case of 0 < g0 6 g
[ qe⊤ , p⊤ ∗ ∗
e ] = [ q1 , q2 , ±π/2, 0, 0, 0 ], ∀(q1∗ , q2∗ ) ∈ D,

where D = (q1 , q2 ) ∈ ℜ2 |ℓ1 − ℓ2 | 6 kq12 k2 6 ℓ1 + ℓ2 ,
q12 := [q1 , q2 ]⊤ . It is shown in [11, 12] that these equilibria are small
time locally controllable [13]. In the first case, however, there is no
smooth time-invariant feedback law that can stabilize the equilibria
because the system does not satisfy Brockett’s necessary condition [8];
in the second case, though the system does satisfy the condition, the
existence of a feedback law is not clear.
10.3.2 Applicability of IDA-PBC and Derivation of Control

Law
We check A1–A7 for the PH system of the 2Ra -Ru planar manipulator.
Let G⊥ be
G⊥ (q3 ) = sin q3 − cos q3 −K (10.13)
and γ3 = γ30 with an arbitrary positive constant γ30 , all Assumptions
except A6 are satisfied. The value of the condition in A6 is zero in the
case of g0 = 0, while in the case of 0 < g0 6 g

ds  = 0, q3∗ = 0, ±π
∗ ∗ 0 ∗
γ3 (q3 ) (q ) = γ3 g0 sin q3 < 0, −π < q3∗ < 0
dq3 3 
> 0, 0 < q3∗ < π
for q3∗ ∈ [−π, π]. Hence, all Assumptions are satisfied only if we select
[ qe⊤ , p⊤ ∗ ∗
e ] = [ q1 , q2 , π/2, 0, 0, 0 ], ∀(q1∗ , q2∗ ) ∈ D
as the stabilizing equilibria for 0 < g0 6 g. These equilibria correspond

on the configurations in which the third link handstands with zero
velocity while the third joint is at any position.
Under this condition we obtain the following Proposition. We omit
the proof due to space limitation.
Proposition 2. For 0 < g0 6 g, a set of energy functions of the

form (10.2) assignable via IDA-PBC to the PH system (10.1) of the
2Ra -Ru planar manipulator is characterized by the globally positive def-
inite and bounded inertia matrix
 
−k1 K cos2 q3 + k3 −k1 K sin q3 cos q3 k1 sin q3
Md (q3 ) =  −k1 K sin q3 cos q3 k1 K cos2 q3 + k3 −k1 cos q3  (10.14)
k1 sin q3 −k1 cos q3 k2
△
with k1 = γ30 + k2 K an arbitrary positive constant and k2 , k3 satisfying
k1 /(2K) < k2 < k1 /K and k3 > 3k1 K and the potential energy function
g0 1
Vd (q) = − sin q3 + {z(q) − z(q∗ )}⊤ S⊤ ∗
3 PS3 {z(q) − z(q )},
k1 − k2 K 2
(10.15)
which satisfies q∗ = arg min Vd (q) for all positive definite matrix P ∈
ℜ2×2 , where S3 ∈ ℜ2×3 is obtained by removing the third row from the
3rd-order identity matrix and z(q) in (10.15) is
 
q1 − 3(k1k−k
1K
2 K)
(2 cos 3 q + 3 cos q − 5) + k (cos q − 1)
3 3 3 3
 
z(q) =  q2 − 3(k2k 1K
1 −k2 K)
sin3 q3 + k3 sin q3 .
0
Moreover, the IDA-PBC law

1 dMd −1
u = (G G) G ∇q3 V e3 + p⊤ Md−1
⊤ −1 ⊤
Md pMd e3
2 dq3

− Md ∇q Vd + J2 Md p − Kv G⊤ Md−1 p
⊤ −1
ensures local asymptotic stability of the desired equilibrium

[q1∗ , q2∗ , π/2, 0, 0, 0] for all q1∗ , q2∗ . ⊓
⊔
3.5 0.3
0.25
3 0.2
Velocities: p
Positions: q
2.5 0.15
0.1
2 0.05
1.5 0
-0.05
1 -0.1
0 10 20 30 40 50 0 10 20 30 40 50
Time: t (s) Time: t (s)
Fig. 10.2. Simulation results for the case of transfer between stabilizable
equilibria: (left) generalized coordinates; (right) generalized velocities; x and
ẋ (solid line), y and ẏ (broken line), θ and θ̇ (chain line)
× 10-16
Constraint force: τ3 (Nm)

200 1.2
Control torques: τ (Nm)
150 0.8
0.4
100
0
50
-0.4
0 -0.8
-50 -1.2
0 10 20 30 40 50 0 10 20 30 40 50
Fig. 10.3. Joint torques for the case Fig. 10.4. Constraint force for the
of transfer between stabilizable equi- case of transfer between stabilizable
libria; τ1 (solid line), τ2 (broken line) equilibria
10.4 Numerical Experiments
We execute simulations to verify the performance of the derived control

law. We treat two control problems: transfer between two stabilizable
equilibria and the swing-up maneuver.
Both simulations are made with αi = 0, i = 1, 2, 3, i.e. J2 = 0 and
the following physical parameters: m1 = 4, m2 = 2, m3 = 1 (kg), ℓ1 =
3, ℓ2 = 2, ℓ3 = 1.2 (m). Assuming that the mass distribution of the
link is uniform, di and Ii , i = 1, 2, 3, are evaluated according to di =
ℓi /2, Ii = mi ℓ2i /12, i = 1, 2, 3, respectively. Parameters γ30 , k1 , k2 , k3 ,
P and Kv are fixed to γ30 = 0.68, k1 = 2.0, k2 = 1.65, k3 = 6.5,

0.15 0 9.5 1
P= and Kv = .
0 0.05 1 10
Transfer between Stabilizable Equilibria

The initial state is q(0) = [ 2(m), 1.5(m), π/2(rad) ]⊤ , p(0) = 0. The goal
state is q∗ = [ 3(m), 1.5(m), π/2(rad) ]⊤ , p∗ = 0. The result is shown in
Fig. 10.2–10.4. It can be seen that the state [q, p] is stabilized asymp-
totically to the desired equilibrium. The inverse dynamics yields the
5 4
4 2
3
Velocities: p
Positions: q
2 0
1 -2
0 -4
-1
-2 -6
-3 -8
0 10 20 30 40 50 0 10 20 30 40 50
Fig. 10.5. Simulation results for the case of the swing-up maneuver:
(left) generalized coordinates; (right) generalized velocities; x and ẋ (solid
line), y and ẏ (broken line), θ and θ̇ (chain line)
× 10-15
Constraint force: τ3 (Nm)

200 1.2
Control torques: τ (Nm)
100 0.8
0.4
0
0
-100
-0.4
-200 -0.8
-300 -1.2
0 10 20 30 40 50 0 10 20 30 40 50
Fig. 10.6. Joint torques for the case Fig. 10.7. Constraint force for the
of the swing-up maneuver; τ1 (solid case of the swing-up maneuver
line), τ2 (broken line)
control torques at the first and second joints and the constraint force
at the third joint. We can see that the second-order nonholonomic con-
straint is sufficiently satisfied in control by observing the constraint
force.
Swing-up Maneuver
The initial state is q(0) = [ 2(m), 1.5(m), −π/2(rad) ]⊤ , p(0) = 0. The
goal state is q ∗ = [ 2(m), 1.5(m), π/2(rad) ]⊤ , p∗ = 0. The result is
shown in Fig. 10.5–10.7. The asymptotic stabilization is also achieved
and the constraint is sufficiently satisfied.
10.5 Conclusions
In this paper we have applied IDA-PBC to a 2Ra -Ru planar manip-

ulator, which is a second-order nonholonomic system, and derived an
effective control law for this system in the presence of gravity. We have
shown that stabilizable equilibria are configurations that the third link
handstands with zero velocity and the third joint, which is unactuated,
is at any position. Two control problems were simulated to demonstrate
the effectiveness of the derived control law.
The domain of attraction of the derived control law and the de-
sign of the controller for the system with physical damping have to be
considered in future work.
References
1. van der Schaft AJ (2000) L2 -gain and Passivity Techniques in Nonlinear

Control. Springer-Verlag, London
2. Fujimoto K et al.(2001) Stabilization of Hamiltonian systems with
nonholonomic constraints based on time-varyring generalized canonical
transformations. System & Control Letters 44(4):309–319
3. Acosta JA, et al.(2005) Interconnection and damping assignment
passivity-based control of mechanical systems with underactuation de-
gree one. IEEE Trans Automatic Control 50(12):1936–1955
4. Osuka K et al.(1999) On robustness of passivity of manipulators. Proc.
the 38th IEEE Conf. on Decision and Control:3406–3409, Phoenix AZ
5. Fujimoto K, et al.(1999) Stabilization of a class of Hamiltonian systems
with nonholonomic constraints and its experimental evaluation. Proc. the
38th IEEE Conf. on Decision and Control:3478–3483, Phoenix AZ
6. Mahindrakar AD, et al.(2006) Further constructive results on intercon-
nection and damping assignment control of mechanical systems: the ac-
robot example. Int J Robust and Nonlinear Control 16(14):671–685
7. Oriolo G et al.(1991) Free-joint manipulators: motion control under
second-order nonholonomic constraints. Proc IEEE/RSJ Int Workshop
on Intelligent Robots and Systems ’91:1248–1253, Osaka Japan
8. Brockett RW (1983) Asymptotic stability and feedback stabilization.
In: Brockett RW, et al.(eds) Differential Geometric Control Theory.
Birkhauser, Boston
9. Ortega R, et al.(2002) Stabilization of underactuated mechanical sys-
tems via interconnection and damping assignment. IEEE Trans Auto-
matic Control 47(8):1218–1233
10. De Luca A et al.(2002) Trajectory planning and control for planar robots
with passive last joint. Int J Robotics Research 21(5–6):575–590
11. Iannitti S (2002) Motion planning and control of a class of underactuated
robots. PhD dissertation, Università ”La Sapienza” di Roma, Italy
12. Arai H, et al.(1998) Nonholonomic control of a three-DOF planar under-
actuated manipulator. IEEE Trans Robotics and Automation 14(5):681–
695
13. Sussmann HJ (1987) A general theorem on local controllability. SIAM J
Control and Optimization 25(1):158–194
11
A New Control Algorithm for Manipulators
with Joint Flexibility
Piotr Sauer and Krzysztof Kozlowski
Chair of Control and Systems Engineering, Poznań University of

Technology, ul. Piotrowo 3a, 60-965 Poznań, Poland
{piotr.sauer,krzysztof.kozlowski}@put.poznan.pl
11.1 Introduction
The new industrial robot is equipped with gear-boxes such as har-

monic drives, which introduce elastic deformations at the joints. These
deformations are regarded a source of problems, especially when ac-
curate trajectory tracking of high sensitivity to end–effector forces is
mandatory. It is shown that the joint elasticity should be taken into
account in modeling a robotic manipulator and designing a control al-
gorithm. If we assume that the elasticity may be modeled as a linear
spring, we obtain a dynamic model which is twice of the order of the
model of the same robot with rigid joints. In this case the design of
a control algorithm is a difficult task. If dynamic parameters are not
known exactly, adaptive control laws must be designed to guarantee
stabilization or tracking. Control of a system with uncertain elements
has been discussed in the robotics literature [1], [4], [6], [8]. In this
paper, we consider an adaptive tracking controller for a manipulator
with only revolute joint. This algorithm is an extension of the controller
proposed by Loria and Ortega [9]. We have assumed that the model
in [9] has in addition dynamic friction components on both link and
motor sides. We have incorporated a harmonic drive into our system.
The construction of this controller is based on Lyapunov theory [1],
[2], [5], [12]. We assume that link and motor positions are available
for measurements. Under these assumptions we propose a semiglobal
adaptive tracking control algorithm. The paper is organized as follows.
The mathematical description of the robot model and the control algo-
rithm are described in Section 11.2. The concluding remarks are given
in Section 11.3.
springerlink.com
120 P. Sauer and K. Kozlowski
11.2 A New Adaptive Control Algorithm
11.2.1 Mathematical Description of the System
We consider the general model of manipulator with N degrees of free-

dom, which can be described as follows:
Ml (ql )q̈l + Cl (ql , q̇l )q̇l + Gl (ql ) + Fl (ql , q̇l ) = K(µ−1 qm − ql ),

(11.1)
Jm q̈m + Fm (qm , q̇m ) + µ−1 K(µ−1 qm − ql ) = u.
(11.2)
Ml (ql ) is an N × N link inertia matrix which is symmetric and pos-

itive definite for all ql . Cl (ql , q̇l ) is an N × N matrix containing the
centripetal and Coriolis terms. Gl (ql ) is an N × 1 vector containing
the gravity terms. Friction forces are represented by vectors Fl (ql , q̇l )
for links and Fm (qm , q̇m ) for actuators. Vector ql is an N × 1 vector
representing the link displacements. Similarly qm is an N × 1 vector
representing the actuator displacement (multiplied by µ−1 result in
displacements on link side). Jm is an N × N positive definite constant
diagonal actuator inertia matrix. K is an N × N diagonal matrix of
the torsional stiffness. u is an N × 1 vector of input torque control.
µ is a constant diagonal N × N matrix containing the gear ratios for
each harmonic drive (elements of matrix µ are > 1). Equations (11.1)
and (11.2) are similar to those presented in [11], [9], and [14] with
the extensions described above. It should be noted that the models
described above neglect the electrical dynamics associated with each
actuator and there is no uncertainly in the system on both link and
motor sides. In addition the system (11.1) ÷ (11.2) assumes dynamic
and static friction phenomena. We assume that the full state is available
for measurements. We define tracking error as:
q̃l = qld − ql , q̃m = qmd − qm , (11.3)
lim q̃l = lim (qld − ql ) , (11.4)

t→∞ t→∞
where qld and qmd denote an N × 1 vector of desired link and motor
trajectories, respectively. It is also assumed that all qid ∈ C 4 , kqid k,
kq̇id k, kq̈id k 6 Bd , i = l, m, where || · || denotes the Euclidean norm.
We want to find a stable control law which assures (11.4).
11 A New Control Algorithm for Manipulators with Joint Flexibility 121
11.2.2 The New Control Algorithm
For manipulator described by Eqs. (11.1) and (11.2) the adaptive con-
trol algorithm is proposed in the following form:
b −1 K(
bm q̈md + F̂m + µ
u=J b µb −1 qmd − qld ) + Kpm q̃m + Kdm ṽm .
(11.5)
We define signal qmd as

b −1 µ
qmd = K b M c l (ql )q̈ld + C
b l (ql , q̇ld )q̇ld + ĝl (ql ) + F̂l (q̇ld ) + Kp q̃l +
Kd ṽl ) + µ
b qld , (11.6)
and
ṽ˙ l = −Al ṽl + Bl q̃˙ l , ṽ˙ m = −Am ṽm + Bm q̃˙ m , (11.7)
where
Al = diag{ali }, Bl = diag{bli },
Am = diag{ami }, Bm = diag{bmi }.
First we carry out a stability proof for a similar control algorithm for
a manipulator with rigid links. In case of negligible flexibility (K → ∞
[11, 7, 10, 13]) the model given by (11.1) and (11.2) reduces to
Dl q̈l + Cl q̇l + gl + F = τl , (11.8)
where Dl = Ml + µ2 Jm , F = Fl + µFm , and τl is an N × 1 vector

of joint torques. It is assumed that vector F (size N × 1) contains
friction terms associated with link bearings and actuators. In this case,
we propose the following adaptive control algorithm:
b l (ql )q̈ld + C
τl = D b l (ql , q̇ld )q̇ld + ĝl (ql ) + F̂ (q̇ld ) + Kpl q̃l + Kdl ṽl .
(11.9)
The control law, which is presented by (11.9), takes advantage of po-

sition measurement only. A velocity error is obtained by the filtered
position error given by the first equation in (11.7). In Eq. (11.9) new
symbols are used:
• Kpl and Kdl are positive – definite matrices, which include parame-
ters of a P D controller.
• bli are constants which satisfy the following inequality 0 < β < 1
and λM (Bl ) > βλM (Dl )/λm (Dl ) is a constant. λm (Dl ) and λM (Dl )
denote the smallest and the biggest eigenvalues of inertia matrix Dl ,
respectively. Finally, λM (Bl ) is the biggest eigenvalue of matrix Bl .
b l, C
• D b l , ĝl and F̂ are the estimates of Dl , Cl , gl , and F , respectively.
Taking into account a skew symmetric property of the matrix Ḋl (ql ) −
2Cl (ql , q̇l ) and (11.8), (11.9) the error equation of the system consid-
ered here can be written in the following form:
Dl (ql )q̃¨l + (Cl + Cd )q̃˙ l − F (q̇l ) + F (q̇ld ) + Y l (ql , q̇ld , q̈ld )ãl + Kpl q̃l +
+ Kdl ṽl = 0, (11.10)
in which Cd = Cl (ql , q̇ld ) and
b l − al ,
ãl = a (11.11)
where al is an m–dimensional vector containing the unknown dynami-

cal manipulator and actuator parameters and a b l denotes the parameter
estimation vector (it is assumed that ãl is bounded).
Further, since the matrices Dl , Cl and vectors gl , F are linear in
terms of the manipulator and actuator parameters one can write

Yl (ql , q̇ld , q̈ld )ãl = D b l (ql )−Dl (ql ) q̈ld + C b l (ql , q̇ld )−Cl (ql , q̇ld ) ·

· q̇ld + ĝl (ql ) − gl (ql ) + F̂ (q̇ld ) − F (q̇ld ) . (11.12)
Now we formulate the main Theorem 1 of this paper. Its proof is based
on theorem A1 given in the Appendix. First we prove the main result
for a rigid manipulator and next we formulate extension of this result
to a manipulator with joint flexibility.
Theorem 1:
For the closed-loop system described by (11.10) and adaptation law
given by (11.13) with control law expressed by (11.9) one can define
a Lyapunov function candidate which satisfies conditions described in
Theorem A1 (see Appendix) with initial conditions q̃l (0), q̃˙ l (0), ṽl (0),
and ãl (0), which guarantees that the state of the system represented
by q̃l (t), q̃˙ l (t), ṽl (t), and ãl (t) is uniformly bounded and converges
exponentially to closed balls Br1 , Br2 defined as follows:
q
˙ 2 ˙ 2 2
Br1 = q̃l (t), q̃l (t), ṽl (t) : kq̃l (t)k + kq̃l (t)k + kṽl (t)k 6 r1
and
n p o
Br2 = ãl (t) : kãl (t)k2 6 r2 ,
in which the adaptation law has the following form.
ã˙ l = −ηâl + εΓl−1 Y Tl (ṽl + q̃l ), (11.13)
where η > 0.
The Lyapunov function is defined by (11.14).
1 1 1 ˙
V (q̃l , q̃˙ l , ṽl , t) = q̃˙ lT Dl q̃˙ l + q̃lT Kpl q̃l + ṽl Kdl B−1 T
l ṽl + εq̃l Dl q̃l +
2 2 2
1
+ εṽlT Dl q̃˙ l + ãTl Γl ãl , (11.14)
2
where Γ is a symmetric positive–definite matrix and ε is a constant
value coefficient (ε > 0). Note that the Lyapunov function candidate
defined by (11.14) differs from that defined by Loria and Ortega [9].
Obviously V (q̃l , q̃˙ l , ṽl , t) is trivially a positive definite matrix.
Next we calculate the ball radiuses according to the assumptions pre-
sented in Theorem A1 and the Lyapunov function candidate satisfies
the following inequalities:
1 1
z T Q1 z + λm (Γl )kãl k2 6 V (q̃l , q̃˙ l , ṽl , t) 6 z T Q2 z + λM (Γl )kãl k2 ,
2 2
(11.15)
T T T T
˙
where z = q̃ q̃ ṽ , and matrices Q1 and Q2 are as follows:
 
1 ε
λm (Kpl ) λm (Dl ) 0
2 2 
 
 ε 1 ε 
Q1 =  λm (Dl ) λm (Dl ) λm (Dl ) , (11.16)
2 2 2 
 ε 1 
0 λm (Dl ) λm (Kdl )λ−1 M (Bl )
2 2
 
1 ε
λ (K ) λ (D ) 0
 2 M pl 2 M l 
 
ε 1 ε 
Q2 =  λM (Dl ) λM (Dl ) λM (Dl ) . (11.17)
2 2 2 
 ε 1 
0 λM (Dl ) λM (Kdl )λ−1 m (Bl )
2 2
These matrices are positive definite, namely ε has to fulfil the following
conditions:
s
λm (Kpl )
ε< ,
λm (Dl )
s
λm (Kpl )λm (Kdl )λ−1
M (Bl )
ε< ,
λm (Dl ) λm (Kpl ) + λm (Kdl )λ−1
M (Bl )
s
λM (Kpl )
ε< ,
λM (Dl )
s
λM (Kpl )λM (Kdl )λ−1
m (Bl )
ε< . (11.18)
λM (Dl ) λm (Kpl ) + λm (Kdl )λ−1
m (Bl )
Taking into account the above conditions the Lyapunov function (11.14)
satisfies the following inequality:
1
λm (Q1 )kzk2 + λm (Γl )kal k2 6 V (q̃l , q̃˙ l , ṽl , t) 6
2
1
6 λM (Q2 )kz||2 + λM (Γl )kal k2 .
2
(11.19)
Taking into account the skew symmetric property of the matrix Ḋl (ql )−
2Cl (ql , q̇l ) and Eqs. (11.7) and (11.9) ÷ (11.13), the time derivative of
the Lyapunov function can be rewritten in the following form:
V̇ = q̃˙ lT F (q̇l ) − q̃˙ lT F (q̇ld ) + εq̃lT F (q̇l ) − εq̃lT F (q̇ld ) + εṽlT F (q̇l )+
− εṽlT F (q̇ld ) + ãTl Γl ã˙ l − εq̃lT Y l ãl − εṽlT Yl ãl − q̃˙ lT Yl ãl +

− ṽlT Kdl B−1 ˙T ˙ ˙T ˙ T T ˙
l Al ṽl − q̃l Cd q̃l + εq̃l Dl q̃l + εq̃l Cd − Cd q̃l +

+ εq̃lT CTl − CTd q̃˙ l − εq̃lT Kpl q̃l − εq̃lT Kdl ṽl − εṽlT ATl Dl q̃˙ l +

+ εq̃˙ l BTl Dl q̃˙ l + εṽlT CTl − CTd q̃˙ l + εṽlT CTd − Cd q̃˙ l +
− εṽlT Kpl q̃l − εṽlT Kdl ṽl , (11.20)
where Cl = C(ql , q̇l ) and Cd = Cl (ql , q̇ld ).

We assume a friction model as:
F (q̇l ) = Fc sgn(q̇l ) + Fv q̇l ,

F (q̇ld ) = Fc sgn(q̇ld ) + Fv q̇ld , (11.21)
where Fc and Fv denote the diagonal matrices consisting of Coulomb

and viscous friction coefficients associated with the links, respectively.
First we establish the following bounds for the terms associated with
the friction coefficients which appear in Eq. (11.20) taking into account
Eq. (11.21):
q̃˙ lT F (q̇l ) − q̃˙ lT F (q̇ld ) 6 2λM (Fc )kq̃˙ l k − λm (Fv )kq̃˙ l k2 ,

εq̃lT F (q̇l ) − εq̃lT F (q̇ld ) 6 2ελM (Fc )kq̃l k + ελM (Fv )kq̃l kkq̃˙ l k,
εṽlT F (q̇l ) − εq̃l F (q̇ld ) 6 2ελM (Fc )kṽl k + ελM (Fv )kṽl kkq̃˙ l k, (11.22)
where λm (·) and λM (·) denote the smallest and the maximum eigen-
value of the matrix (·), respectively.
Now we can establish the following bounds for the other terms in
Eq.(11.20), which do not include elements with vector ã:
−ṽlT Kdl B−1 −1 2

l Al ṽl 6 −λm (Kdl )λM (Bl )λm (Al )kṽl k , (11.23)
−q̃˙ lT Cd q̃˙ l 6 kC Bd kq̃˙ l k2 ,
εq̃˙ lT Dl q̃˙ l 6 ελM (Dl )kq̃˙ l k2 ,

−εq̃lT CTd − Cd q̃˙ l 6 2εkC Bd kq̃l kkq̃˙ l k,

−εq̃T CT − CT q̃˙ l 6 εkC kq̃l kkq̃˙ l k2 ,
l l d
T
−εq̃l Kpl q̃l 6 −ελm (Kpl )kq̃l k2 ,
−εq̃lT Kdl ṽl 6 ελM (Kdl )kṽl kkq̃l k,
−εṽlT ATl Dl q̃˙ l 6 ελM (Al )λM (Dl )kṽl kkq̃˙ l k,
εq̃˙ lT BTl Dl q̃˙ l 6 ελM (Bl )λM (Dl )kq̃˙ l k2 ,

−εṽlT CTd − Cd q̃˙ l 6 2εkC Bd kṽl kkq̃˙ l k,
−εṽlT Kpl q̃l 6 ελM (Kpl )kṽl kkq̃l k,

−εṽlT Kdl ṽl 6 −ελm (Kdl )kṽl k2 ,

−εṽlT CTl − CTd q̃˙ l 6 εkC kq̃˙ l k2 kṽl k,
where C(q, q̇) 6 kC kq̇k and kC > 0.

Taking into account the inequalities of Eqs. (11.22) and (11.23) and
the adaptive law (11.13) we can write Eq. (11.20) in the form:
T
ε kq̃l k T kq̃l k ε kṽl k kṽl k
V̇ (t) 6 − Q3 − Q +
2 kṽl k kṽl k 2 kq̃˙ l k 4
kq̃˙ l k
T
ε kq̃l k kq̃l k
− Q5 ˙ − λ1 + ελ2 kq̃˙ l k2 − λ3 kṽl k2 +
2 kq̃˙ l k kq̃l k
+ εkC (kṽl k + kq̃l k) kq̃˙ l ||2 − ãTl Γl ηâl +

− q̃˙ lT Yl ãl + 2ελM (Fc ) (kq̃l k + kṽl k) + 2λM (Fc )kq̃˙ l k+
ε ε
− (1 − ρ)λm (Fv )kq̃˙ l k2 − λm (Kpl )kq̃l k2 − λm (Kdl )kṽl k2 , (11.24)
2 2
where
 
1
 λm (Kpl ) −λM (Kpl ) − λM (Kdl ) 
Q3 =  2 ,
1
−λM (Kpl ) − λM (Kdl ) λm (Kdl )λ−1
M (Bl )λm (Al )
2ε
"
1
λm (Kdl )λ−1
M (Bl )λm (Al )
Q4 = 2ε ...
−2kC Bd − λM (Al )λM (Dl ) − λM (Fv )

−2kC Bd − λM (Al λM (Dl ) − λM (Fv )
... 1 ,
λM (Dl )λM (Bl )
β
 1

2 λm (Kpl ) −2kC Bd − λM (Fv )
Q5 =  1 2αkC Bd  ,
−2kC Bd − λM (Fv ), λM (Bl )λM (Dl ) −
β ε
β+2
λ1 = − λM (Bl )λM (Dl )ε − γkC Bd + ρλm (Fv ),
β
1
λ2 = λm (Bl )λM (Dl ) − λM (Dl ),
β
1
λ3 = λm (Kdl )λ−1 M (Bl )λm (Al ) + ελm (Kdl ),
2
α + γ = 1, α, γ, β > 0, and 0 < ρ < 1.
The condition for the positive definiteness of matrix Q3 results in
an upper bound for ε (recall that ε > 0):
λm (Kpl )λm (Kdl )λm (Al )
ε< . (11.25)
4λM (Bl ) (λM (Kpl ) + λM (Kdl ))2
For matrix Q4 the positive definiteness condition can be written in the
following form:
λm (Kdl )λm (Bl )λm (Al )λM (Dl )
ε< . (11.26)
2β (2kC Bd + λM (Al )λM (Dl ) + λM (Fv ))2
The condition for λm (Kpl ) can be calculated from the condition for the
positive definiteness of matrix Q5
2βε (2kC Bd + λM (Fv ))2

λm (Kpl ) > , (11.27)
ελM (Bl )λM (Dl ) − 2αβkC Bd
assuming that the following condition is true:
2αβkC Bd
ε> . (11.28)
λM (Bl )λM (Dl )
Notice that the positiveness of constant λ1 imposes a lower bound on

ε:
β ρλm (Fv ) − γkC Bd
ε< . (11.29)
β + 2 λM (Bl )λM (Dl )
From the expression λm (Fv ) − γkC Bd > 0 we obtain the following
condition:
λm (Fv )
γ< . (11.30)
kC Bd
λ2 is positive under the following condition:
λM (Bl ) > β. (11.31)
λ3 is always positive. Defining vector z as in Theorem 1 and matrix Q,

which is composed of matrices Q3 , Q4 , and Q5 , permits the following
bound on V̇ (t) to be obtained:
V̇ (t) 6 −z T Qz + εkC (kṽl k + kq̃l k) kq̃˙ l k2 − ηãTl Γl âl − q̃˙ lT Y l ãl +

ε ε
− λm (Kpl )kq̃l k2 − λm (Kdl )kṽl k2 + 2ελM (Fc ) (kq̃l k + kṽl k) +
2 2
2λM (Fc )kq̃˙ l k − (1 − ρ)λm (Fv )kq̃˙ l k2 . (11.32)
To this end we include a bound on regression function Yl . Yl is a rec-

tangular matrix (Y l ∈ Rm×N ). In order to find a norm of this function,
the Frobenius matrix norm is suggested. In this paper we assume a ma-
nipulator with revolute joint only to assure a finite value of the Frobe-
nius norm. The regression matrix Y l (ql , q̇ld , q̈ld ) includes positions,
desired positions, and desired accelerations. Because of that all elements
of this matrix are limited, the regression matrix Yl is bounded:
−q̃˙ lT Y l ãl 6 kY l kF kq̃˙ l kkãl k = C1 kq̃˙ l kkãl k. (11.33)

Taking into account the estimation error of parameters and inequality

(11.33), we can write the expression −ãTl Γl ηâl −q̃˙ lT Yl ãl in the following
form:
C12 1
−ηãTl Γl âl − q̃˙ lT Y l ãl 6 kq̃˙ l k2 − ηλm (Γl )kãl k2 + ε1 . (11.34)
ηλm (Γl ) 4
Assuming that the norm of the vector al is bounded by a positive

constant ā (kal k 6 ā) one can get:
1 ηλ2M (Γl )
ε1 = kāk2 > 0.
2 λm (Γl )
The time derivative of the Lyapunov function can be written as

1
V̇ (t) 6 −z T Q′ z − ηλm (Γl )kãl k2 + ε1 + εkC kq̃l kkq̃˙ l k2 +
4
ε
+ εkC kṽl kkq̃˙ l k2 − λm (Kpl )kq̃l k2 + (11.35)
2
ε
− λm (Kdl )kṽl ||2 + 2ελM (Fc ) (kq̃l k + kṽl k) ,
2
where
 ε
λm (Kpl )
 ε 2
′ 
Q = − λM (Fv ) − εkC Bd ···
 ε 2
− (λM (Kdl ) + λM (Kpl ))
2
ε
− λM (Fv ) − εkC Bd
2
ε C12
· · · λM (Bl )λM (Dl ) − αkC Bd − ···
β ηλm (Γl )
ε ε
λM (Fv ) − εkC Bd − λM (Al )λM (Dl )
2 2
ε 
− (λM (Kdl ) + λM (Kpl ))
2 
· · · − ε λM (Fv ) − εkC Bd − ε λM (Al )λM (Dl ) 
. (11.36)
2 2
λm (Kdl )λ−1
M (Bl )λm (Al )
Matrices Q and Q′ are built based on matices Q3 , Q4 , and Q5 assuming

that we omit expressions with kq̃˙ l k2 and kṽl k2 because they are nega-
tive taking into account conditions (11.29)-(11.31). The positive defi-
niteness of matrix Q is guaranteed by conditions (11.25)-(11.28). The

positive definiteness of matrix Q′ is guaranteed by conditions (11.25)
and (11.26).
Next, we do some additional calculations:
ε ε
2ελM (Fc ) (kq̃l k + kṽl k) − λm (Kpl )kq̃l k2 − λm (Kdl )kṽl k2 +
2 2
+ 2λM (Fc )kq̃˙ l k − (1 − ρ)λm (Fv )kq̃˙ l k2 6 λ2M (Fc )· (11.37)

2ε(1 − ρ)λm (Fv ) (λm (Kpl ) + λm (Kdl )) + λm (Kpl )λm (Kdl )
· .
(1 − ρ)λm (Kpl )λm (Kdl )λm (Fv )
The time derivative of the Lyapunov function can be written as

1
V̇ (t) 6 −z T Q′ z + εkC (kṽl k + kq̃l k) kq̃˙ l k2 − ηλm (Γl )kãl k2 +ε1 +ε2 ,
4
(11.38)
The parameter ε2 is written in the following form:
λm (Kpl ) + λm (Kdl )
ε2 = 2ελ2M (Fc ) . (11.39)
λm (Kpl )λm (Kdl )
The second element of (11.38) can be bounded as follows:
εkC (kṽl k + kq̃l k) kq̃˙ l k2 6 εkC kq̃˙ l kkzk2 . (11.40)
We show that the Lyapunov function satisfies the following inequality:
˙ 2.
V (t) > ξ1 kq̃k (11.41)
To this end, we calculate the bound on the Lyapunov function:
1 1 1
V (t) > λm (Dl )kq̃˙ l k2 + λm (Kpl )kq̃l k2 + λm (Kdl )λM (Bl )kṽl k2 +
2 2 2
1
− ελm (Dl )kq̃l kkq̃˙ l k − ελM (Dl )kṽl kkq̃˙ l k + λm (Γl )kãl k2 ,
2
(11.42)
where we can write the following conditions for (11.42):

1
• λm (Kpl )kq̃l k2 − ελm (Dl )kq̃l kkq̃˙ l k =
2

1 ˙ λm (Dl ) 2 1 ε2 λ2m (Dl ) ˙ 2
= λm (Kpl ) kq̃l k − kq̃l kε − kq̃l k ,
2 λm (Kpl ) 2 λm (Kpl )
(11.43)
1 1
• λm (Kdl )λM (Bl )kṽl k2 − ελm (Dl )kṽl kkq̃˙ l || = λm (Kdl )λM (Bl )·
2 2
2
ελm (Dl ) ˙ ε2 λ2m (Dl )
· kṽl k − kq̃l k − kq̃˙ l ||2 .
λm (Kdl )λM (Bl ) 2λm (Kdl )λM (Bl )
(11.44)
Finally, the Lyapunov function can be written in the following form:

1 ε2 λ2m (Dl ) ε2 λ2m (Dl )
V (t) > λm (Dl ) − − kq̃˙ l k. (11.45)
2 2λm (Kpl ) 2λm (Kdl )λM (Bl )
If the condition for ε, derived from Eq. (11.45),
λm (Kpl )λm (Kdl )λM (Bl )

ε2 < , (11.46)
λm (Dl ) (λm (Kpl ) + λm (Kdl )λM (Bl ))
is carried out, then we can define parameter ξ1 as

s
λm (Dl ) ε2 λ2m (Dl ) ε2 λ2m (Dl )
ξ1 = − − . (11.47)
2 2λm (Kpl ) 2λm (Kdl )λM (Bl )
The parameter ξ1 is always positive. Taking this into consideration

based on (11.41), we can write
√
˙ V
kq̃k 6 √ . (11.48)
ξ1
Finally, taking into account inequalities (11.34), (11.37), (11.40), (11.48)

and the form of matrix Q′ , the time derivative of Lyapunov function
can be written as
√ !
′ V 1
V̇ (t) 6 − λm (Q ) − εkC √ kzk2 − ηλm (Γl )kãl k2 + ε3 . (11.49)
ξ1 4
The time derivative of the Lyapunov function has the same form as in
Theorem A1 (see (11.60), (11.61) in the Appendix), if we define the
following variables:
1
λ1 = λm (Q1 ), λ2 = λm (Γl ),
2
1
λ3 = λM (Q2 ), λ4 = λM (Γl ),
2
√
′ V 1
λ5 = λm (Q ) − εkC √ , λ6 = ηλm (Γl ), (11.50)
ξ1 4
and
ηλ2M (Γl ) 2
ε3 = ε1 + ε2 = ā + λ2M (Fc )·
2λm (Γl )
2ε(1 − ρ)λm (Fv ) (λm (Kpl ) + λm (Kdl )) + λm (Kpl )λm (Kdl )
· . (11.51)
(1 − ρ)λm (Kpl )λm (Kdl )λm (Fv )
The coefficient associated with ||z||2 in Eq. (11.49) requires more careful
examination. Now choose two scalar constants VM√and Vm so that
VM > Vm > V (0), and define cM = λmin (Q′ ) − εkC √VξM ; then choose
ξ 1
1 large enough so that cM > 0 (this is always possible). Next selecting
proper
˙ values of ε3 and η are can ensure that Vm 6 V1 6 VM then
V < 0. This condition together with VM > Vm √> V (0) implies that
V (t)
V (t) 6 VM ∀t, so that c(t) = λmin (Q′ ) − εkC √ξ > cm > 0 ∀t.
1
One can make parameter ε3 small when parameters ε, η are chosen
sufficiently small. We introduce the following assumption:
λm (Kpl ) = k, λm (Kdl ) = σk, (11.52)
where σ > 0 is constant.

On the basis on the above conditions, we obtain a form of V (t) and
V (t) consistent with Theorem A1. Thus for the system considered here
one can write:
( )
λM (Q2 ) 2λM (Γl )
σ1 = max p , (11.53)
λm (Q) − εkC V /ξ ηλm (Γl )
and the radiuses of the balls are:

r s
ε3 σ1 2ε3 σ1
r1 = , r2 = . (11.54)
λm (Q1 ) λm (Γl )
Based on Theorem A1 and definitions (11.53) and (11.54) we can affirm

that the coordinates z and ã for all time are bounded and exponentially
convergent to the closed balls of radiuses (11.54). This completes the
proof. The radiuses of the balls can be made small when parameters ε
and η are chosen sufficiently small. The radiuses of the balls depend on
the gain of the PD controller k and the eigenvalue of matrices Fc , Q,
andΓl . When we increase the gain of the PD controller k or parameter
Γlm , the radiuses of the balls will decrease [3].
Now we write an adaptive version of the control algorithms for the
system given by (11.1),(11.2) with joint elasticity. First we write the
closed–loop system equations for the manipulator described by (11.1),
(11.2) in the form
Mq̃¨ + (C1 + Cd )q̃˙ + Y 1 (ql , q̇ld , q̈ld , qmd , q̇md , q̈md )ã + Kp q̃ + Kd ṽ = 0,
(11.55)
and
q̃˙ = −Aṽ + B q̃,
˙ (11.56)
where
T T , T T ,
q̃ = q̃lT q̃m ṽ = ṽlT ṽm

Ml 0 Kdl 0
M= , Kd = ,
0 Jm 0 Kdm

B1 0 C(ql , q̇l ) 0
B= , C1 = ,
0 B2 0 0

C(ql , q̇ld ) 0 Kpl + K −K
Cd = , Kp = ,
0 0 −K Kpm + K

Y l (ql , q̇ld , q̈ld ) 0
Y1 = ,
0 Y m (qmd , q̇md , q̈md )
where matrices Y l and Ym are appropriate matrices associated with
links and motors, respectively. Note that (11.55) and (11.56) are similar
to (11.10) and (11.9). The conditions for matrices M, C, and Cd are
in force for matrices M, C, and Cd . The matrix Kp is positive–definite

if Kpl and Kpm are positive–definite. Equation (11.55) includes two
control laws:
• the first control law applies to the links of the manipulator, similar
to (11.10),
• the second control law applies to the actuators of the manipulator.
Now, consider the following Lyapunov function candidate:
1 ˙T 1 1 ˙
V (t) = q̃ Mq̃˙ T + q̃ T Kp q̃ + ṽ T Kd B −1 ṽ + εq̃ T D q̃+
2 2 2
1
+ εṽ T D q̃˙ + ãT Γ ã. (11.57)
2
This function is similar to the Lyapunov function for the rigid manip-
ulator (11.14). The Lyapunov function (11.57) can be calculated simi-
larly as the Lyapunov function for rigid manipulators, but we must use
the additional adaptation law for the motor parameters in the following
form:
ã˙ m = −ηâm + εΓm

−1 T
Y m (ṽm + q̃m ), (11.58)
where Γm is a positive–definite matrix. The adaptive control law is of

the form of (11.5) and the desired signal qmd is defined by (11.6) and
(11.7). The convergence is as in Theorem 1.
In this study we presented a new adaptive tracking control. This con-

trol is globally exponentially stable. The domain of attraction can be
arbitrarily enlarged by increasing the gain of the controller.
References
1. E. Barany, R. Colbaugh, Global Stabilization of Uncertain Mechanical Sys-

tems with Bounded Controls, Proceedings of the American Control Con-
ference, pp. 2212–2216, Philadelphia, June 1998.
2. R. Colbaugh, K. Glass, E. Barany, Adaptive Output Stabilization of Ma-
nipulators, Proceedings of the 33rd Conference on Decision and Control,
pp. 1296–1302, Lake Buena Vista, December 1994.
3. R. Colbaugh, K. Glass, E. Barany, Adaptive Regulation of Manipulators
Using Only Position Measurements, International Journal of Robotics Re-
search, 1987, Vol. 16, no. 5, pp. 703–713.
4. M. Corless, Tracking Controllers for Uncertain Systems: Application to

a Manutec R3 Robot, Journal of Dynamic Systems, Measurement, and
Control, Vol. 111,pp. 609–618, 1989.
5. M.Corless, Guarenteed Rates of Exponential Convergence for Uncertain
Systems, Journal of Optimization Theory and Applications, Vol. 64, No. 3,
pp. 481–494, 1990
6. R. Kelly, V. Santibanez, A. Loria, Control of Robot Manipulators in Joint
Space, Springer Verlag, 2005.
7. K.Kozowski, P.Sauer, The Stability of the Adaptive Tracking Controller
of Rigid and Flexible Joint Robots, Proceedings of the First Workshop on
Robot Motion and Control, pp.85–95, Kiekrz, 1999.
8. G.Leitmann, On the Efficacy of Nonlinear Control in Uncertain Linear
Systems, Journal of Dynamic Systems, Measurement, and Control, Vol.
102, pp. 95–102, 1981.
9. A. Loria and R. Ortega, On Tracking Control of Rigid and Flexible Joint
Robots, Applied Mathematics and Computer Science, Vol. 5, No. 2, pp.329–
341, 1995.
10. S. Nicosia, P. Tomei, A tracking controller for flexible joint robots using
only link position feedback, IEEE Trans. on Automatic Control, 1994, pp.
885–890.
11. P. Sauer, Adaptive control algorithm for manipulator with elastic joints,
Phd thesis, Pozna University of Technology, Pozna, 1999, (in Polish).
12. J.J. Slotine and W. Li, Applied Nonlinear Control, Prentice Hall, Engle-
wood Cliffs, NJ, 1991.
13. M. Spong and M. Vidyasagar, Robot Dynamics and Control, John Wiley
and Sons, 1989.
14. L. Tian and A.A. Goldenberg, Robust Adaptive Control of Flexible Joint
Robots with Joint Torque Feedback, Proceedings of the IEEE International
Conference on Robotics and Automation, pp. 1229–1234, 1995.
Appendix
Theorem A1:
Consider the following system:
x˙1 = f1 (x1 , x2 , t),

x˙2 = f2 (x1 , x2 , t). (11.59)
Suppose that V (x1 , x2 , t) is a Lyapunov function candidate for model

(11.59) satisfying the following inequalities:
λ1 kx1 k2 + λ2 kx2 k2 6 V 6 λ3 kx1 k2 + λ4 kx2 k2 , (11.60)

V̇ 6 −λ5 kx1 k2 − λ6 kx2 k2 + ε, (11.61)
where ε, λi are positive constants, p

σi = max(λ3 /λ5 , λ4 /λ6 ), and ri = σi ε/λi for i = 1, 2.
Assuming initial conditions x1 (0), x2 (0), their states x1 (t), x2 (t) for an
arbitrary time t are uniformly bounded and convergent exponentially
to the closed balls denoted Br1 , Br2 , where Bri = {xi : kxi k 6 ri }.
The proof of this theorem follows from the global exponential stability
theorem given by Corless [5].
12
An Inverse Dynamics-Based Discrete-Time
Sliding Mode Controller for Robot
Manipulators
Andrea Calanca1 , Luca M. Capisani2 , Antonella Ferrara2 ,

and Lorenza Magnani2
1
IT Engin., Provenia Group, Mantova, Italy
andrea.calanca@gmail.com
2
Department of Computer Engineering and Systems Science,
University of Pavia, Italy
{luca.capisani,antonella.ferrara,lorenza.magnani}@unipv.it
12.1 Introduction
In the past years an extensive literature has been devoted to the sub-
ject of motion control of rigid robot manipulators. Many approaches
have been proposed, such as feedback linearization [1], model predic-
tive control [2], as well as sliding mode or adaptive control [3], [4], [5].
The basic idea of feedback linearization, known in the robotic context
as inverse dynamics control [6], [7], is to exactly compensate all the
coupling nonlinearities in the dynamical model of the manipulator in a
first stage so that a second stage compensator may be designed based
on a linear and decoupled plant. Although global feedback linearization
is possible in theory, in practice it is difficult to achieve, mainly because
the coordinate transformation is a function of the system parameters
and, hence, sensitive to uncertainties which arise from joint and link
flexibility, frictions, sensor noise, and unknown loads. This is the rea-
son why the inverse dynamics approach is often coupled with robust
control methodologies [1].
Among these, a possibility is offered by sliding mode control [8],
[9] which is robust versus a significant class of parameter uncertainties
and insensitive to load disturbances. Yet, it is a common opinion that
the major drawback of sliding mode control applied to robots is the
so-called chattering phenomenon due to the high frequency switching
of the control signal, which may disrupt or damage actuators, dete-
riorating the controlled system. Such a phenomenon is increased due
to the fact that the sliding mode controller is designed by means of a
springerlink.com
138 A. Calanca et al.
continuous-time model for the system to be controlled, but, in practice,

input, output, and reference signals are sampled, leading to a discrete-
time behavior of the plant. In this way, the control signal is constant
during the sampling interval, resulting in finite-time oscillations of the
controlled variable [10].
In order to circumvent the chattering problem, many approaches
have been followed. In the case of first-order sliding mode control laws,
chattering can be circumvented by approximating, with a continuous
function, the sign function, but, in this way, only pseudo-sliding modes
are generated. By adopting second-order sliding mode control the chat-
tering effect can be made less critical by confining discontinuities to
the derivative of the control law [11], [12]. Note that a combination of
second-order sliding mode control with the inverse dynamics method
has been investigated in [13], while the combination of a first-order
pseudo-sliding mode control with compensated inverse dynamics and
an adaptive component has been presented in [4].
An alternative way to solve the problem of chattering is that of
designing the sliding mode control law directly with reference to the
discrete-time system obtained after the sampling procedure [14], [15]. In
this paper, a new version of the discrete-time sliding mode control strat-
egy described in [14], combined with the inverse dynamics approach,
is proposed to perform motion control of robot manipulators. While in
[14] the problem of the presence of uncertainties acting on the plant
model is solved by introducing an adaptive term in the control law, in
the present case a disturbance estimator is added to the discrete-time
sliding mode control law, thus simplifying the approach in presence of
input uncertainties.
The proposed control approach has been experimentally verified on
the Comau Smart3-S2 industrial robot shown in Fig. 12.1. To this end,
a suitable model of the robot has been formulated, and a practical
MIMO parameters identification procedure, recently devised [16], has
been applied. Experimental tests demonstrate the efficiency and the
appreciable tracking performances of the presented inverse dynamics-
based discrete-time sliding mode control strategy.
12.2 The Considered Dynamical Model
The dynamical model of an n-joint robot manipulator can be written

in the joint space
u = B(q)q̈ + C(q, q̇)q̇ + g(q) + Fd sign(q̇) + Fv q̇, (12.1)

12 Discrete-Time Sliding Mode C ontroller for Robot Manipulators 139
where q ∈ ℜn is the vector of joints displacements, u ∈ ℜn is the vector

of motor control torques, B(q) ∈ ℜn×n is the inertia matrix, C(q, q̇)q̇ ∈
ℜn represents the centripetal and Coriolis torques, Fv ∈ ℜn×n and
Fd ∈ ℜn×n are the viscous and static friction matrices, and g(q) ∈ ℜn
is the vector of gravitational torques.
The aim of the control strategy is to make the robot track a desired
trajectory qd , where qd represents the vector of the desired joint dis-
placements and we assume that qd , q̇d , and q̈d exist and are bounded.
In the next sections, the following symbols will be used: mi is the mass,
Ji is the inertia, θi is the angular position, li represents the link length
and bi the center of mass position with respect to joint i.
12.3 The Inverse Dynamics Method
The basic idea of inverse dynamics control [6], [7] is to transform the
nonlinear system (12.1) into a linear and decoupled system by using a
nonlinear coordinate transformation that exactly compensates all the
nonlinearities. More specifically, by choosing the following nonlinear
feedback control law
u = B(q)y + n(q, q̇) (12.2)
with n(q, q̇) = C(q, q̇)q̇ + Fv q̇ + Fd sign(q̇) + g(q), system (12.1) simply
becomes q̈ = y. Note that, even if n(q, q̇) in (12.2) has been accurately
identified, it can be quite different from the real one because of uncer-
tainties and unmodelled dynamics, friction, elasticity, and joint plays.
Now assume that the term n̂(q, q̇) considers the identified centripetal,
Coriolis, gravity, and friction torques terms, while the inertia matrix
B(q) is known.
So letting u = B(q)y + n̂(q, q̇), the compensated system becomes
q̈ = y + B −1 ñ(q, q̇) = y − f, (12.3)
where f = −B −1 ñ and ñ = n̂ − n. In normal operational conditions

the uncertainty term f is constant or slowly varying. In this paper, for
the sake of simplicity, it is supposed to be constant.
12.4 A Discrete-Time Sliding Mode Control Approach
The uncertain decoupled-linearized system (12.3), in case of an n-joint

manipulator, is composed of n independent uncertain subsystems, one
for each joint, with q̈ T = [q̈1 , . . . , q̈n ]. Then a sliding mode controller
can be derived for each of them.
Letting ei = qdi − qi be the tracking error for each joint i subsystem,
and choosing the following state variables

x1i e
xi = = i , (12.4)
x2i ėi
the continuous-time dynamics of the tracking error for each joint can
be viewed as the dynamics of a linear system
ẋi = Gxi + H(−q̈d + yi − fi ), (12.5)
where yi is the i-th component of y ∈ ℜn in (12.2) and fi can be

regarded as an unknown disturbance on the i-th joint, and it is assumed
to be bounded.
In order to circumvent the chattering phenomenon designing a
discrete-time sliding mode controller, Eq. (12.5) needs to be rewrit-
ten in discretized form as (for the sake of simplicity, the subscript i will
be omitted from now on, yet, what follows refers to joint i)
xk+1 = G∗ xk + H ∗ (−q̈dk + yk − fk ), (12.6)
where, chosen a sampling time interval δ,

Z δ
G∗ = eGδ , H ∗ = eG(δ−τ ) Hdτ (12.7)
0
and f (t) is assumed to be constant within the sampling interval.

The concept of discrete-time sliding mode was defined by [17] as
follows. Given the discrete time system zk+1 = F (zk ), zk ∈ ℜp , a
discrete time sliding mode takes place in the subset Σ on the mani-
fold s(zk ) = 0, s ∈ ℜm (m < p) if there exist an open p-dimensional
neighborhood U of this subset such that F (zk ) ∈ Σ (which means
s(F (zk )) = 0) for any zk ∈ U .
In our case, selecting the sliding variable as sk = T xk , T = [α 1],
it follows from the above definition that the sliding mode condition
sk+1 = T F (xk ) = 0 requires to the control signal to steer the sliding
variable to zero in a sampling interval.
For instance, with reference to system (12.6), discrete-time sliding
mode condition is achieved if the matrix (T H ∗ ) has an inverse, and the
control yk is chosen as the solution to the equation
sk+1 = T F (xk ) = T (G∗ xk + H ∗ (−q̈dk + yk − fk )) = 0, (12.8)

12 Discrete-Time Sliding Mode Controller for Robot Manipulators 141
that is
yk = −(T H ∗ )−1 (T G∗ xk − T H ∗ fk − T H ∗ q̈dk ) (12.9)
From (12.9) it is apparent that yk tends to infinity if δ → 0 (see [14]).
In order to make the control task easier, and to avoid the mentioned
problem, the control law is chosen so as to steer sk to zero in a finite
number of steps instead of after a single step. To this end it is possible
to define an upper bound | yk |< y0 for the control signal.
According to [14], it can be proved that, by choosing as yk to apply
in (12.6), the following control law

yk , | yk |6 y0
yk = y yk , | y |> y (12.10)
0 |yk | k 0
the necessary and sufficient condition for the discrete time sliding mode
generation | sk+1 |<| sk | is satisfied, hence | sk | decreases monoton-
ically and, after a finite number of steps, yk becomes bounded and
| yk |6 y0 so that, in one step, sk = 0, and a discrete time sliding
mode will occur. Similarly to the case of continuous time systems, the
equation sk = T xk = 0 enables system order reduction, and the desired
dynamics of the sliding mode can be designed by an appropriate choice
of matrix T in the sliding manifold equation [14].
12.5 An Alternative Discrete-Time Sliding Mode

Control Approach
It must be noted that if system (12.6) is affected by uncertainties (i.e.,

fk is not perfectly known), the control (12.10), designed on the basis
of a nominal value of fk , does not guarantee, in general, the condition
sk = 0, and the further motion is confined only to a neighborhood of
the sliding manifold. In order to obtain the control objective, making
the state trajectories closer to the ideal discrete-time sliding motion,
an adaptive approach has been proposed in [14].
In the present section, as an alternative, a simpler control law is
proposed. The basic idea is that of introducing a disturbance estima-
tor, when the | yk |6 y0 condition is satisfied. More specifically, with
reference to (12.4) and (12.8), choose
yk = q̈dk + discrP Ik (·), (12.11)

where
( y
K |ykeq
keq |
, | ykeq |> y0
discrP Ik (·) = 1+2KI ˆ (12.12)
ykeq + KP sk + KI fk , | ykeq |6 y0
being
ykeq = −(T H ∗ )−1 (T G∗ )xk (12.13)
the discrete time equivalent control law (12.9), and KI ∈ ℜ and KP ∈ ℜ
represent two suitable constants such that the tracking error dynamics
is asymptotically stable.
I ˆ
The term 1+2K
KI fk represents a suitable estimation of fk . This esti-
mation is given by
1 + 2KI ˆ
fˆk+1 = −KI [fˆk + (T H ∗ )−1 sk − KP sk−1 + (1 − )fk−1 ], (12.14)
KI
provided that | ykeq |6 y0 . In this case, deriving from (12.8)
1 + 2KI ˆ
sk = T H ∗ (KP sk−1 + fk−1 − fk−1 ) (12.15)
KI
and substituting in (12.14), it becomes
fˆk+1 = −KI (fˆk + fˆk−1 − fk−1 ). (12.16)
Since fk is supposed to be a constant disturbance, we have fk = f ,

therefore, introducing the z delay operator
F̂ (z)(1 + KI z −1 + KI z −2 ) = KI f, (12.17)
which, in this case, by choosing a suitable KI , is asymptotically stable

and for k → ∞, fk+1 = fk = fk−1 , then
fˆ(1 + 2KI ) = KI f. (12.18)
The disturbance estimation allows to compensate the unknown distur-

bance fk in (12.8).
In the sequel, the experimental results of the application of this
law to the Comau Smart3-S2 robot will be reported as a practical
demonstration of its efficacy.
Fig. 12.1. Smart3-S2 robot and three-link planar manipulator
12.6 Experimental Verification
12.6.1 The Considered Industrial Robot
The control approach described in this paper has been tested on the
Smart3-S2 industrial anthropomorphic rigid robot manipulator by Co-
mau located at the Department of Electrical Engineering of the Uni-
versity of Pavia, shown in Fig. 12.1. It consists of six links and six
rotational joints driven by brushless electric motors. For our purposes,
joints 1, 4, and 6 are not used. In this way the model formulation is
simpler. As a result, it is possible to consider the robot as a three
link-three joint (numbered {1, 2, 3} in the sequel) planar manipulator,
as schematically represented in Fig. 12.1. This choice allows us to keep
the model formulation and the identification simpler. Yet, the proposed
method can be easily extended to a n-joint robot.
Identification
The identification procedure for rigid robot manipulators presented in
[16] has been applied to identify the parameters of the model of the
Smart3-S2 industrial robot shown in Table 12.1 [16]. In Table 12.2, the
identified parameters for the robot are reported.
Table 12.1. Minimal parametrization of the manipulator model

Parameter Meaning
γ1−2 m3 b32 + J3 , J3 + m3 (l22 + b32 ) + J2 + m2 b22
γ3 J3 + m3 (l12 + l22 + b32 ) + J2 + m2 (l12 + b22 ) + J1 m1 b12
γ4−6 m1 b 1 + m2 l 1 + m3 l 1 , m2 b 2 + m3 l 2 , m3 b 3
γ7−12 Fs1 , Fv1 , Fs2 , Fv2 , Fs3 , Fv3
Table 12.2. Average value and variance for estimated parameters

θML γ1 γ2 γ3 γ4 γ5 γ6 γ7 γ8 γ9 γ10 γ11 γ12
E
0.297 10.07 87.91 57.03 9.21 0.316 190.5 66.3 21.0 14.71 4.66 8.29
[θML ]
V ar
0.003 0.04 0.2 0.06 0.02 0.003 0.6 0.3 0.3 0.1 0.03 0.02
[θML ]
V ar%
1.2 0.4 0.2 0.1 0.2 0.9 0.3 0.5 1.2 1.0 0.7 0.3
[θML ]
12.6.2 Experimental Results
The proposed discrete time sliding mode control strategy presented

in this paper has been successfully tested on the Comau Smart3-S2
robot. The chosen values of the parameters in (12.12) for the Smart3-
S2 manipulator are KP = 0.5, KI = −0.4, and K = (7, 9, 50).
Fig. 12.2 shows the static performance and the good tracking prop-
erties of the proposed strategy, in case of step and sinusoidal angular
reference trajectories for the three joints.
Fig. 12.3 shows that in presence of uncertainties the performance
of the discrete time sliding mode control law (12.10), without a proper
disturbance estimator, can be poor in terms of tracking and convergence
time, thus motivating the presence of the disturbance estimator in the
controller. Note also that with our proposal the problem of chattering
is circumvented.
40 q 20
1
20 q
q and qd on joint 1, 2, 3 [rad]
0 d1 0 q
1
q and q on joint 1, 2, 3 [rad]
−20 −20 q
d1
0 1 2 3 4
40 0 1 2
q
2
q 20
20 d2
0 q
2
0 −20 q
d2
0 1 2 3 4
d
60 q 0 0.5 1 1.5
3
40
40 q 20
d3 q
20 0 3
−20 qd3
0
−40
0 1 2 3 4 0 0.5 1
t [sec] t [sec]
Fig. 12.2. Comparison between the three joint positions and respectively
spline and sinusoidal reference trajectories using the discr-PI sliding mode
control law
3
−38
−38.5 q1 2 q1
−39
q and q on joint 1, 2, 3 [rad] qd1
−39.5 qd1
1
joint position [rad]

0 1 2 3 4 2 4 6 8
2 1
q
2
q 0 qd2
1 2
q −1
0 d2
d
0 1 2 3 4 1 2 3 4 5
2 2
1.5
q3 q3
1 1
0.5 qd3 qd3
0
0 1 2 3 4 2 4 6 8
t [sec] time [s]
Fig. 12.3. Tracking performance using the proposed algorithm (on the left)
versus that obtained using the discrete-time sliding mode control law without
disturbance estimation (on the right)
12.7 Conclusions
A discrete-time sliding mode control strategy for rigid robot manipula-

tors is presented in this paper in combination with the inverse dynam-
ics method. The proposed control scheme provides robustness versus
matched uncertainties and disturbances and allows to mitigate chat-
tering problems. Experimental tests performed on an industrial robot
manipulator demonstrate good tracking performance of the proposed
control strategy.
References
1. Abdallah, C., Dawson, D., Dorato, P., Jamshidi, M.: Survey of robust
control for rigids robots. IEEE Contr. Syst. Mag. 11(2) (1991) 24–30
2. Juang, J.N., Eure, K.W.: Predictive feedback and feedforward control for
systems with unknown disturbance. NASA/Tm-1998-208744 (1998)
3. Perk, J.S., Han, G.S., Ahn, H.S., Kim, D.: Adaptive approaches on the
sliding mode control of robot manipulators. Trans. on Contr. Aut. and
Sys. Eng. 3(2) (2001) 15–20
4. Shyu, K.K., Chu, P.H., Shang, L.J.: Control of rigid robot manipulators
via combiantion of adaptive sliding mode control and compensated inverse
dynamics approach. In: IEE Proc. on Contr. Th. App. Volume 143. (1996)
283–288
5. Colbaugh, R.D., Bassi, E., Benzi, F., Trabatti, M.: Enhancing the tra-
jectory tracking performance capabilities of position-controlled manipu-
lators. In: IEEE Ind. App. Conf. Volume 2. (2000) 1170–1177
6. Asada, H., Slotine, J.E.: Robot Analysis and Control. Wiley, NJ (1986)
7. Spong, M.W., Lewis, F., Abdallah, C.: Robot Control: Dynamics, Motion
Planning, and Analysis. IEEE Press, NJ (1993)
8. Edwards, C., Spurgeon, S.K.: Sliding Mode Control: Theory and Appli-
cations. Taylor & Francis, U.K. (1998)
9. Utkin, V.: Sliding modes in control and optimization. Moscow, Springer
(1992)
10. Milosavljevic, C.: General conditions for the existence of a quasisliding
mode on the swiching hyperplane in discrete variable structure systems.
Autom. Rem. Cont. 46(1) (1985) 307–314
11. Bartolini, G., Ferrara, A., Usai, E.: Chattering avoidance by second order
sliding mode control. IEEE Trans. Automat. Contr. 43(2) (1998) 241–246
12. Bartolini, G., Ferrara, A., Usai, E., Utkin, V.I.: On multi-input chattering-
free second-order sliding mode control. IEEE Trans. Automat. Contr.
45(9) (2000) 1711–1717
13. Ferrara, A., Magnani, L.: Motion control of rigid robot manipulators via
first and second order sliding modes. J. of Int. and Rob. Syst., Special
Issue on Model-Based Reason. in Eng. and Rob. Syst. 48 (2007) 23–36
14. Bartolini, G., Ferrara, A., Utkin, V.I.: Adaptive sliding mode control in
discrete-time systems. Automatica 31(5) (1995) 763–769
15. Sarpturk, S.Z., Istefanopulos, Y., Kaynak, O.: On the stability of discrete-
time sliding mode control system. IEEE Trans. Automat. Contr. 32(10)
(1987) 930–932
16. Capisani, L., Ferrara, A., Magnani, L.: Mimo identification with optimal
experiment design for rigid robot manipulators. In: IEEE/ASME Int.
Conf. on Adv. Int. Mech., submitted. (2007)
17. Utkin, V.I., Drakunow, S.V.: On discrete-time sliding modes control.
In: Preprints IFAC Conference on Nonlinear Control, Capri, Italy. (1989)
484–489
13
Velocity Tracking Controller for Rigid
Manipulators
Przemyslaw Herman and Krzysztof Kozlowski
Chair of Control and Systems Engineering, Poznań University of

Technology, ul. Piotrowo 3a, 60-965 Poznań, Poland
{przemyslaw.herman,krzysztof.kozlowski}@put.poznan.pl
13.1 Introduction
The tracking control problem of rigid manipulators is well-known in the

robotics literature (e.g. [6, 9]). The inverse dynamics controller which
solves this problem, leads to a decoupled system. There exist, however,
some other developments based on searching a canonical transforma-
tion. The existence of such transformation was discussed by Spong [8].
The necessary and sufficient conditions which ensure that we obtain the
canonical variables are very restrictive and rarely satisfied in practice
[3, 8]. For that reason Jain and Rodriguez [3] proposed diagonaliza-
tion in velocity space based on a spatial operator algebra approach. A
different solution using a congruency transformation was presented by
Loduha and Ravani [5]. A PD control algorithm based on the formula-
tion given in [5] was tested in [1].
The aim of this paper is to propose a controller which tracks veloc-
ity trajectory expressed in terms of the generalized velocity components
(GVC) vector. It is shown that the controller guarantees exponential
convergence of both the GVC error and its integral vector (some curvi-
linear coordinates). The novelty of the approach relies on the fact that
we use a modified inverse dynamics controller for equations of motion
expressed in terms of the GVC vector (the joint velocities are replaced
by the GVC). It is also shown that if the desired GVC are integrable
then instead of the joint positions quasi-coordinates are used. As a con-
sequence, the obtained tracking controller is analogous to the classical
control algorithm which ensures the joint velocities error as well as the
joint position error convergence. The idea of the desired GVC trajec-
tory is also explained (this quantity was not defined in [5]). It appears
that the GVC controller can be helpful for evaluation manipulator dy-
springerlink.com
148 P. Herman and K. Kozlowski
namics because it contains quantities arising from a decomposition of

the system mass matrix. Therefore, it gives some information which
is necessary at the design stage of manipulators in which simulation
investigation plays an important role. The control scheme was tested
on a spatial manipulator.
13.2 First-Order Equations of Motion Containing GVC

Recall that the classical equations of motion based on Lagrange’s for-
mulation can be given in the following form (e.g.[6]):
M (q)q̈ + C(q, q̇) + G(q) = τ, (13.1)
where q, q̇, q̈ ∈ RN are vectors of generalized positions, velocities, and
accelerations, N is the number of degrees of freedom, M (q) ∈ RN ×N
is a symmetric system mass matrix, C(q, q̇) = (Ṁ q̇ − 12 q̇ T Mq q̇) ∈ RN
is a vector of Coriolis and centrifugal forces in standard equations of
motion, where the expression q̇ T Mq q̇ is the column vector col(q̇ T Mqk q̇)
(Mqk = ∂M ∂qk denotes the partial derivative of the mass matrix M (q)
with respect to the joint coordinate qk ) and Ṁ is the time derivative
of the matrix M (q) (derivation of the term C(q, q̇) is contained e.g. in
[4]), G(q) ∈ RN is a vector of gravitational forces in classical equations
of motion. The second-order equation (13.1) is coupled because of the
mass matrix M (q). In order to reduce equations of motion to the first-
-order equation we use the decomposition method of the mass matrix
proposed in [3].
A complete set of equations of motion for the system in terms of
the GVC is expressed as:
N (q)u̇ + Cu (q, u) + Gu (q) = π, (13.2)
q̇ = Υ (q)u, (13.3)
where N (q) ∈ RN ×N is a diagonal system mass matrix in terms of
GVC obtained using the method presented in reference [5], u, u̇ ∈ RN
are vectors of generalized velocity components and its time derivative,
Cu (q, u) ∈ RN is a vector of Coriolis and centrifugal forces in terms
of GVC, Gu (q) ∈ RN is a vector of gravitational forces in terms of
GVC, π ∈ RN is a vector of quasi-forces in terms of GVC and (.)T is
the transpose operation. It is also true that u = Υ −1 (q)q̇ because the
matrix Υ (q) is invertible, which results from the positive definiteness of
the matrix M (q). The matrices and vectors are given as follows (Υ̇ (q, q̇)
means the time derivative of Υ (q)):
13 Velocity Tracking Controller for Rigid Manipulators 149
N (q) = Υ T (q)M (q)Υ (q) (13.4)

Cu (q, u) = Υ T (q)[M (q)Υ̇ (q, q̇)u + C(q, q̇)] (13.5)
Gu (q) = Υ T (q)G(q) (13.6)
π = Υ T (q)τ. (13.7)
Eq.(13.2) can be derived from Eq.(13.1) as follows. At the beginning we
calculate the time derivative of q̇ i.e. q̈ = Υ̇ T u+Υ T u̇. Then we insert this
quantity into Eq.(13.1). After some manipulations we obtain Eq.(13.2)
with the terms described by Eqs.(13.4)-(13.7).
13.3 Velocity Tracking Controller Using GVC

The aim of this section is to propose a controller which is analogous to
the classical inverse dynamics controller (e.g. [6]):
τ = M (q)y + C(q, q̇) + G(q) (13.8)
where y = q̈d + cD q̃˙ + cP q̃ (q̈d denotes a desired joint acceleration
vector). The quantities q̃ = qd − q, q̃˙ = q̇d − q̇ are the joint position
errors, and the joint velocity errors between the desired and the actual
posture, respectively. The matrices cD , cP contain diagonal, positive-
-definite gain coefficients. The system (13.1) with the controller (13.8)
is linear and decoupled with respect to the new input y.
The trajectory tracking problem in terms of GVC relies on that we
track the desired quasi-velocity vector ∀t ud (t) 6= 0 instead of track-
ing the desired joint velocity vector (the integrals of the GVC, if
they exist, differ from the joint positions). The vector is defined as
ud (t) = Υ −1 (qd (t))q̇d (t). Each k-th element of this vector contains all
joint velocities closer to the manipulator base and depends on mechan-
ical couplings between the manipulator links. Thus, the mechanical
couplings arising from the decomposition of the manipulator mass ma-
trix are reflected in the vector ud (t). Note that even if the desired GVC
are analytically integrable then their integrals do not lead to obtaining
the joint positions (the integrals are only some curvilinear coordinates).
As a result, the controller ensures only velocity tracking.
Theorem 1. Equations in the form of
π = N (u̇d + cD ũ + cP zu ) + Cu (q, u) + Gu (q), (13.9)
żu = ũ, (13.10)
where cD and cP are positive diagonal matrices, ũ = ud − u represents
the velocity error vector in terms of GVC, u̇d is time derivative of the
desired quasi-velocity trajectory, describe the velocity tracking in joint

space of a manipulator using the generalized velocity components for
the system (13.2) with (13.3), i.e. that the control objective
lim ũ(t) = 0 (13.11)
t→∞
is guaranteed.
Proof. It is assumed that the desired quantity ud (t) is continuously
differentiable, bounded by kud kN , i.e. kud (t)k 6 kud kN ∀t > 0, and
that the GVC vector ud (t) is integrable. The integral of each com-
ponent ud (k) is some curvilinear generalized coordinate. Inserting the
controller Eqs.(13.9)-(13.10) into the dynamics Eq.(13.2) one can ob-
tain the second-order differential equation (the matrix N is a positive
definite matrix):
ũ˙ + cD ũ + cP zu = 0, (13.12)
˙
where ũ(t) = u̇d (t) − u̇(t) and ũ(t) = ud (t) − u(t). Based on the vec-
tor u(t) all components of zu (t) are known because this vector results
from integration of u(t) and similarly to the vector u(t) it contains the
manipulator parameter set.
Using Eqs.(13.10) and (13.12) we obtain the closed-loop system in
the following form:
d zu ũ
= . (13.13)
dt ũ −cP zu − cD ũ
The Lyapunov function candidate is proposed as follows:
1 T 1
LE (zu , ũ) = ũ ũ + zuT cP zu . (13.14)
2 2
The time derivative of LE is:
L̇E (zu , ũ) = ũT ũ˙ + żuT cP zu = ũT (−cD ũ − cP zu + cP zu )
= −ũT cD ũ 6 0, (13.15)
because cD is a positive definite matrix. One can find an upper bound
of that time derivative in the following form:
L̇E (zu , ũ) 6 −λmin (cD ) kũk2 , (13.16)

where λmin (cD ) is the smallest value of the matrix cD . The Lya-
punov function LE (zu , ũ) is radially unbounded (it means, denoting
x = [zuT , ũT ]T , that [7] LE (x) → ∞ as ||x|| → ∞ or as x tends to infinity
in any direction) and its time derivative L̇E (zu , ũ) is negative semidef-
inite. These two facts imply that the state variables are bounded, i.e.
zu , ũ ∈ LN
∞ . Assuming the hypothesis that the desired signal kũd (t)k is
bounded for all t > 0, one can observe that the time derivative of the
state variables are bounded signals, i.e. żu , ũ˙ ∈ LN
∞ . Integrating (13.16)
leads to
Z ∞ Z ∞
2
λmin (cD )ũ(σ) dσ 6 − L̇E (zu (σ), ũ(σ))dσ
0 0
= LE (zu (0), ũ(0)) − LE (zu (∞), ũ(∞)). 6 LE (zu (0), ũ(0)) <∞(13.17)
Therefore, ũ ∈ LN2 (ũ is a square integrable function). Because a

bounded and square integrable function with bounded time derivative
must tend to zero then one can conclude that the control objective
expressed by Eq.(13.11) is fulfilled.
Exponential convergence. Based on the results presented in [9] we can
introduce the following cross term (using the vector [zuT , ũT ]T ):
1
LC (zu , ũ) = δ(zuT ũ + zuT cD zu ), (13.18)
2
where δ is a small constant so that L is positive definite. The new
Lyapunov function L = LE + LC is defined as:
1 1
L(zu , ũ) = ũT ũ + zuT (cP + δcD )zu + δzuT ũ. (13.19)
2 2
Its time derivative is:
L̇(zu , ũ) = ũT ũ˙ + zuT (cP + δcD )żu + δzuT ũ˙ + δżuT ũ. (13.20)
Calculating from Eq.(13.12) ũ˙ and recalling Eq.(13.10) one can obtain:
L̇(zu , ũ) = ũT (−cP zu − cD ũ) + zuT (cP + δcD )żu

+δzuT (−cP zu − cD ũ) + δũT ũ = −ũT cD ũ − δzuT cP zu
+δũT ũ 6 −(λmin (cD ) − δ)kũk2 − δzuT cP zu , (13.21)
where λmin (cD ) is defined as previously. Now the function L(zu , ũ) can
be written as follows:

1 zu T cP δcD zu
L(zu , ũ) = (13.22)
2 ũ 2δI I ũ
and the expression defined by Eq.(13.21) (I denotes the identity matrix)

T
zu δcP 0 zu
L̇(zu , ũ) 6 − . (13.23)
ũ 0 (λmin (cD ) − δ)I ũ
Hence, there exists a ρ such L̇ < −ρL. If we assume λmin (cD ) > δ then
this result implies that
L(t) 6 L(0)e−ρ t . (13.24)
Therefore, also the exponential convergence is proved.
Remark 1. From Eq. (13.12) it follows that the controller (13.9) guar-
antees that the quasi-position error and the quasi-velocity error tend to
zero. Each quantity uk (k-th component of the vector u) is separately
regulated. For a rigid manipulator the relationship (13.3) is invertible
(because the matrix Υ (q) is a positive definite matrix). Thus we can
write that ud − u = Υ −1 (qd )q̇d − Υ −1 (q)q̇. From Eq.(13.11) it results
only that Υ −1 (q)q̇ → Υ −1 (qd )q̇d , as soon as the time tends to infinity.
Thus, in general, we cannot guarantee the joint velocity and the joint
position error convergence at the same time. In many cases the desired
joint velocity at the end of the motion is equal to zero. In such case we
can expect that Υ −1 (q) → Υ −1 (qd ), which means that the desired joint
position error may be achieved. If we consider non-integrable GVC, we
can propose only the simplified velocity controller (without the term
cP zu ). The proposed control algorithm is associated with an O(N 3 )
forward dynamics algorithm. However, as follows from [2] for serial
manipulators, the forward dynamics algorithm requires a comparable
number of arithmetical operations to many classical algorithms.
Remark 2. The advantages of the GVC tracking controller are: 1) indi-
vidual control of links by quasi-velocity regulation; 2) observation me-
chanical couplings between joint velocities and links (in time domain);
3) an insight into the manipulator dynamics by calculating the inertia
contained in Nk (k-th diagonal element of the matrix N (q)) and the
kinetic energy related to each link Kk (because the total kinetic energy
P 1 PN
is expressed as K = 12 uT N u = 12 N 2
k=1 Nk uk = 2 k=1 Kk ).
Special case: integrable GVC. Consider a 3 d.o.f. spatial manipulator
(with inverse numbering of links according to [3] because of a nice phys-
ical interpretation) presented in Fig. 13.1(a) in order to find transfor-
mation between the zu vector and the vector of generalized coordinates
q. The quasi-velocities (GVC) are
ud1 = θ̇d1 + (1 + ko cos θd1 )θ̇d2 , ud2 = θ̇d2 , ud3 = ṡd3 , (13.25)
where ko = m1 p1 l2 /J1 (m1 is the mass of the first link, J1 is the first
link inertia, p1 means the distance between the axis of rotation and
the mass center of the first link, l2 is length of the second link). In
order to determine integrals of the udk we should assume the following
conditions: θ̇d2 = sin θd1 · θ̇d1 , θd2 = − cos θd1 (this condition is true
for, e.g., θd1 = β(at − sin at) and θd2 = − cos(β(at − sin at)), where
a, β denote constant values). Calculating analytically the integrals of
ud1 , ud2 , ud3 (constants are omitted here) one gets the following new
curvilinear desired quasi-coordinates:
1
zu1d = θd1 + θd2 + ko sin2 θd1 , zu2d = θd2 , zu3d = sd3 . (13.26)
2
In this section simulation results concerning the proposed tracking con-

troller for a 3 d.o.f. spatial manipulator (which consists of two rota-
tional joints and one translational joint) are given. The manipulator
depicted in Fig. 13.1(a) is characterized by the following set of pa-
rameters: link masses: m1 = 3kg, m2 = 4kg, m3 = 15kg; link iner-
tias : J1 = 0.3kgm2 , J2 = 0.5kgm2 ; distance between the axis of
rotation to the mass center: p1 = 0.2m, p2 = 0.2m; length of links:
l1 = 0.4m, l2 = 0.4m.
The desired joint trajectories were assumed as θd1 = β1 (at − sin at),
θd2 = −cosθd1 , θd3 = β2 (at − sin at) (a = 4, β1 = 1/4, β2 = 1/5) and
joint velocities θ̇d1 = β1 a(1 − cos at), θ̇d2 = sin θd1 · θ̇d1 , θ̇d3 = β2 a(1 −
cos at) and the duration time tf = π/2 s. Simulations were performed
for the GVC controller (13.9) using Matlab with Simulink and gain
coefficients cD (1, 2, 3) = 70, 70, 70 and cP (1, 2, 3) = 600, 600, 600.
In Fig. 13.1(b) the desired joint velocities dθd (θ̇d ) as well as the
desired quasi-velocities (ud ) for all joints are shown (couplings are ob-
servable between the first link and other links because ud1 differs from
dθd1 (θ̇d1 )). In Fig. 13.1(c) the desired joint positions θd (θd ) and quasi-
-coordinates zud for all joints are given. Figure 13.1(d) shows quasi-
-velocity error convergence eu (ũ) for all joints and for the controller
(13.9). We see that all errors are very close to zero after about 2 [s].
The dynamical couplings are reflected in eu1. It is observable that the
maximal values of the errors are about 3 times smaller than if the term
cP zu is neglected (it results from the presence of quasi-velocity inte-
grals in the controller). This fact is confirmed in Fig. 13.1(f) by quasi-
-coordinates errors convergence. Comparing Figs. 13.1(e) and 13.1(g)
(joint errors and their integrals convergence) we observe that using the
controller (13.9) the error convergence in the manipulator joint space
is guaranteed as well. The joint torques and the joint force τ 3 are given
in Fig. 13.1(h).
dθd1, dθd2, ud1, ud2 [rad/s], dsd3, ud3 [m/s]

4
ud1
dθ
3 d1
dθd2= ud2
2
dsd3= ud3
1
0
0 1 t [s] 2 3
(a) (b)
θ ,θ ,z ,z [rad] , s , z [m] eu1, eu2 [rad/s] , eu3 [m/s] ev1, ev2 [rad/s] , ev3 [m/s]
d1 d2 ud1 ud2 d3 ud3
3
0.04
0.06
θ eu2 eu1 ev2
z d1 0.04
2 ud1 0.02
0.02
0 0
1
sd3= zud3 −0.02
−0.04 eu3 −0.02
0 ev3
−0.06
θd2= zud2 −0.04 ev1
−0.08
−1
0 1 t [s] 2 3 0 1 t [s] 2 3 0 1 t [s] 2 3
(c) (d) (e)

z ,z [rad] , z [m] −3
x 10 e1, e2 [rad] , e3 [m] τ1, τ2 [Nm] , τ3 [N]
u1 u2 u3 5 300
0.005
z
0 u3 250 τ3
0
−0.005 e3 200
−0.01 −5 150
−0.015 z 100
u2
τ2
z −10 τ1
−0.02 u1 50
e2
e1
−0.025 0
−15
0 1 t [s] 2 3 0 1 t [s] 2 3 0 1 t [s] 2 3
(f) (g) (h)
Fig. 13.1. Example: a) kinematic scheme of a spatial manipulator; b) desired

joint velocities dθd (θ̇d ), quasi-velocities ud for all joints; c) desired joint posi-
tions θd (θd ), and integrals of quasi-velocities ud for all joints; d) quasi-velocity
˙
errors eu (ũ); e) joint velocity errors ev (θ̃); f) quasi-coordinate errors zu ; g)
joint position errors e (q̃); h) joint torques τ for all joints.

In this work a velocity tracking controller in terms of the GVC, which
is exponentially convergent, was presented. Based on the controller an
insight into manipulator dynamics is possible because the vector u con-
tains both the kinematic and the dynamic parameters of the system.
Thus the proposed velocity controller can be helpful at the design stage
of manipulators (we can detect mechanical couplings between links and
reduce them). It was observed that the quasi-velocity and the quasi-
-position error convergence implies that also the joint velocity errors
and the joint position errors are convergent.
References
1. Herman P, Kozlowski K (2001) Set Point Control for Serial Manipulators
Using Generalized Velocity Components Method. In: Proc. of the 10-th
ICAR’01, August 23-25, Budapest, Hungary, pp.181-186
2. Herman P, Kozlowski K (2006) A Survey of Equations of Motion in Terms
of Inertial Quasi-Velocities for Serial Manipulators. Archive of Applied
Mechanics 76:579-614
3. Jain A, Rodriguez G (1995) Diagonalized Lagrangian Robot Dynamics.
IEEE Transactions on Robotics and Automation 11:571–584
4. Koditschek D (1985) Robot Kinematics and Coordinate Transformations.
In: Proc.of the 24th IEEE Conference on Decision and Control, pp.1-4
5. Loduha TA, Ravani B (1995) On First-Order Decoupling of Equations of
Motion for Constrained Dynamical Systems. Transactions of the ASME
Journal of Applied Mechanics 62:216–222
6. Sciavicco L, Siciliano B (1996) Modeling and Control of Robot Manipu-
lators. The McGraw-Hill Companies Inc., New York
7. Slotine J-J, Li W (1991) Applied Nonlinear Control. Prentice Hall, New
Jersey
8. Spong M (1992) Remarks on Robot Dynamics: Canonical Transforma-
tions and Riemannian Geometry In: Proc. of IEEE Conference on Ro-
botics and Automation, Nice, France. pp.554-556
9. Wen JT, Bayard DS (1988) New class of control laws for robotic ma-
nipulators, Part 1, Non-adaptive case. International Journal of Control
47:1361–1385
14
Fixed Point Transformations-Based Approach
in Adaptive Control of Smooth Systems
József K. Tar1 , Imre J. Rudas1 , and Krzysztof R. Kozlowski2

1
Budapest Tech Polytechnical Institution, H-1034 Budapest,
Bécsi út 96/b, Hungary
tar.jozsef@nik.bmf.hu, rudas@bmf.hu
2
Poznań University of Technology, ul. Piotrowo 3a, 60-965 Poznań, Poland
krzysztof.kozlowski@put.poznan.pl
14.1 Introduction
The mathematical foundations of the modern Soft Computing (SC)

techniques go back to Kolmogorov’s approximation theorem stating
that each multi-variable continuous function on a compact domain can
be approximated with arbitrary accuracy by the composition of single-
variable continuous functions [1]. Since the late eighties several authors
have proved that different types of neural networks possess the universal
approximation property (e.g. [2]). Similar results have been published
since the early nineties in fuzzy theory claiming that different fuzzy
reasoning methods are related to universal approximators (e.g. in [3]).
Due to the fact that Kolmogorov’s theorem aims at the approxima-
tion of the very wide class of continuous functions, the functions to be
constructed are often very complicated and highly non-smooth, there-
fore their construction is difficult. As is well known, continuity allows
very extreme behavior even in the case of single-variable functions. The
first example of a function that is everywhere continuous but nowhere
differentiable was given by Weierstraß in 1872 [4]. At that time mathe-
maticians believed that such functions are only rare extreme examples,
but nowadays it has become clear that the great majority of the con-
tinuous functions have extreme properties. The seemingly antagonistic
contradiction between the complicated nature of the universal approx-
imators and their successful practical applications makes one arrive at
the conclusion that if we restrict our models to the far better behaving
“everywhere differentiable” functions, these problems ab ovo can be
evaded or at least reduced.
springerlink.com
158 J.K. Tar, I.J. Rudas, and K.R. Kozlowski
Really, in the vast majority of the nonlinear control literature ana-

lytical models of various physical systems are considered that are avail-
able in the form of sets of differential equations. The stability proofs
are mainly provided by the use of Lyapunov’s original method of 1892
[5] that does not require analytical solution of the equations of motion.
Instead of that the uniformly continuous nature and non-positive time-
derivative of a positive definite Lyapunov-function V constructed of the
tracking errors and the modeling errors of the system’s parameters is
assumed in the t ∈ [0, ∞) domain, from which the convergence of V̇ to
zero can be concluded according to Barbalat’s lemma [6]. From that it
normally follows that the tracking errors have to remain bounded, or in
certain special cases, have to converge to 0. For instance, the Lyapunov
technique was intensively used in the PhD Thesis by Getz under the
supervision by Marsden in 1995 [7], where the concept of the dynamic
inversion of nonlinear maps was introduced and applied for tracking
control of various physical systems, e.g. robots. The method was also
applied to provide the polar decomposition and inversion of matrices
by Marsden and Getz [8]. The same statement holds e.g. for Slotine’s
and Li’s original control, for the adaptive inverse dynamic control or
the adaptive variant of Slotine’s and Li’s method [6].
These classical approaches have the common feature that they re-
quire either an exactly known dynamic model of the system to be
controlled or at least some analytically formulated model details that
strictly hold for them. On the basis of this qualitative information in
principle learning the complete dynamic model becomes possible on the
assumption that no unknown external perturbation forces act on the
system.
The relatively high computational complexity of the above ap-
proaches as well as the phenomenological restriction that only mod-
eling errors have to be compensated make simpler problem tackling
also attractive. It would be convenient to get rid of the application of
Lyapunov’s theory for guaranteeing convergence, e.g. by applying a far
simpler iterative calculation instead. In a similar manner, compensating
the complex effects of external disturbances and modeling errors simul-
taneously without investing energy for identifying and distinguishing
between them would be advantageous as well. It can also be expected
that by giving up the requirement of learning a complete system model
and allowing the use of partial, situation-dependent models the com-
putational burdens can be reduced. This leads to the introduction of
simple iterative approaches, a novel example of which is the subject of
the present paper.
14 Fixed Point Transformations-Based Approach in Control 159
In this paper simple novel Fixed Point Transformations are intro-

duced and applied in adaptive control of a cart-pendulum system under
drastic external perturbation serving as the simplest paradigm of in-
completely actuated classical mechanical systems in which certain axles
cannot directly be driven by their dedicated drives. Instead of that in
these systems the nonlinear dynamic coupling between the axles can
be utilized for driving certain axles by the drive of another axles. The
satisfactory conditions of convergence are expounded and illustrated
via simulation results.
14.2 Simple Iterative Approaches
The first efforts in this direction are summarized in [9], in which the
sizes of the necessary uniform structures used for developing partial,
temporal, and situation-dependent models that need continuous main-
taining are definitely determined by the degree of freedom of the system
to be controlled.
These considerations do not even assume the availability of an an-
alytic system model of satisfactory accuracy. They are based on the
simple assumption that the system to be controlled can be charac-
terized by its measurable responses given to well known excitations
exerted by the controller. These excitations can be computed on the
basis of a very rough initial model. By comparing the expected and
the experienced responses in each control cycle, properly modified ex-
citations can be applied in the next cycle without trying to identify
the appropriate mapping over a wider domain. According to this idea
each control task can be formulated by using the concepts of the ex-
erted excitation Q of the controlled system to which it is expected to
respond by some prescribed or desired response rd . For instance, in the
case of classical mechanical systems the excitation physically may be
some force or torque, while the response can be linear or angular ac-
celeration. The appropriate excitation can be computed by the use of
some rough, very approximate inverse dynamic model Q = ϕ(rd ). Since
normally this model is neither complete nor exact, the actual response
determined by the system’s dynamics, ψ, results in a realized response
rr that differs from the desired one: rr ≡ ψ(ϕ(rd )) ≡ f (rd ) 6= rd . It
is worth noting that the functions ϕ() and ψ() may contain various
hidden parameters that partly correspond to the dynamic model of the
system, and partly pertain to unknown external dynamic forces acting
on it. Due to phenomenological reasons the controller can manipulate
or deform the input value from rd to rd∗ to achieve the required rd = rr

situation.
It is worth noting that in this approach the desired response can
freely be defined on the basis of purely kinematic considerations quite
independently of the dynamic behavior of the system. The task of real-
izing this desired response remains exclusively that of the adaptive part
of the controller. This feature of this novel approach considerably differs
from the characteristics of the methods based on Lyapunov functions,
in which the dynamics of the error relaxation is an inseparable part of
the control. Furthermore, since this approach is based on the application
of simple iterations, it cannot be formulated in single closed analytical
formula as some control statement that normally occurs with the ap-
plication of Lyapunov’s method in which V̇ can simply be prescribed.
Instead of that an infinite series of cascade expressions h(h(h(...))) ap-
pears, for which appropriate convergence has to be guaranteed.
In the case of Single Input Single Output (SISO) systems for con-
stant r d for learning the rn+1 = u(rn |r d ) ≡ r d rn /f (rn ) iteration was
chosen at the 1st papers. The case f (r⋆ ) = r d evidently yields r⋆ , there-
fore the above iteration corresponds to finding the fixed point of the
function r⋆ = u(r⋆ |r d ). As is well known, if |∂u/∂r| < 1 in the vicin-
ity of r⋆ the proposed iteration converges to the fixed point. For slowly
varying r d the concept of complete stability can be applied as in the case
of the cellular neural networks that dynamically map a static picture to
another static picture. If the picture to be mapped varies slowly in com-
parison with the speed of the network’s dynamics, time-varying pictures
are mapped to each other by the network [10]. For a SISO system evi-
d d r⋆ ′
dently ∂u/∂r = fr(r) − fr(r)r2 f ′ (r), which yields ∂u
∂r r=r⋆ = 1 − r d f (r⋆ ),
which results in convergence for sgn(r d ) = sgn(r) 6= 0 and for small
positive f ′ (r) in the vicinity of the fixed point. By the use of various
group theoretical and algebraic aids partly detailed in [9] this method
was also extended to MIMO systems.
In [11] this idea was further developed for negative definite SISO
systems by the use of a double-parametric transformation defined as
d
wζ,D (x) := f (x)+D+ζ(f (x)−x )
x
f (x)+D (14.1)
dwζ,D (x) ′
wζ,D (x⋆ ) = x⋆ , dx = 1 + ζfxd(x+D
⋆ )x⋆
,
x=x⋆
which is also convergent if x⋆ ≈ xd + D, if the parameter ζ > 0 is small,

′ ′
f (x⋆ ) < 0, and if |f (x⋆ )| is small, too.
In the present paper the same classical mechanical paradigm is used
to illustrate the operation of newer fixed-point transformations, the
construction and use of which seems to be simpler than that of (14.1).

Besides expounding the main ideas simulation results are presented as
well.
14.3 Novel Iterative Approaches for SISO Systems
For the purpose of obtaining the necessary deformation consider the fol-
lowing functions inspired by simple schematic geometric pictures with
the parameters ∆+ > xd , ∆− < xd , and x > D− :
f (x)−∆−
g(x|xd , D− , ∆− ) := xd −∆−
(x − D− ) + D−
d
x −∆+ (14.2)
h(x|xd , D− , ∆+ ) := f (x)−∆+ (x − D− ) + D− .
It is evident that if f (x⋆ ) = xd then g(x⋆ |xd , D− , ∆− ) = x⋆ . In a similar

manner, h(x⋆ |xd , D− , ∆+ ) = x⋆ . Thus the original control problem, i.e.
finding a properly deformed input x⋆ , is transformed to finding the
solution of fixed point problems that can be solved by simple iterations.
The question is whether these iterations obtained for the functions g
and h are convergent in the vicinity of the appropriate fixed points. To
guarantee that, it is sufficient to show that g and h are contractive in
the vicinity of their fixed points:
g′ (x⋆ ) = f ′ (x⋆ ) xx⋆d −D

−∆−
−
+ f (x⋆ )−∆−
xd −∆−
= f ′ (x⋆ ) xxd⋆ −D
−∆−
−
+ 1,
xd −∆+ (xd −∆+ )(x⋆ −D− ) ′
h′ (x⋆ ) = f (x⋆ )−∆+ − [f (x⋆ )−∆+ ]2 f (x⋆ ) = (14.3)
= 1 − f ′ (x⋆ ) xx⋆d −D
−∆
−
.
+
Evidently, if ∆+ > xd , ∆− < xd , x > D− , and f (•) is “flat enough” i.e.

|f ′ | < 1, both functions can be contractive for f ′ (x⋆ ) < 0. The width
of the appropriate basin of attraction within which the contractivity
holds can be influenced by the parameters D− , ∆− , ∆+ , and mainly
by the “flatness” of the model function ϕ(•). The applicability of this
idea is highlighted via simulations for a cart plus pendulum system in
which the rotational angle of the pendulum is indirectly controlled via
internal dynamic coupling by directly driving the translation of the
cart.
14.4 The Mathematical Model of the Cart – Pendulum

System and Simulation Results
Whenever each axle of a classical mechanical system is directly driven,

the positive definite symmetric inertia matrix guarantees a mathemat-
ically positive definite or “increasing” system in the sense that the
relationship between the generalized coordinates’ accelerations and the
generalized forces is established by this matrix. However, whenever
the accelerations of certain axles are manipulated through the dynamic
coupling between the various degrees of freedom instead of using di-
rectly acting drives, this “increasing” nature of the relationships be-
tween the excitation and the system response can disappear in certain
domains. The simplest example of such cases is the cart-pendulum sys-
tem in which the pendulum’s rotational position is controlled via the
drive causing horizontal acceleration of the cart. The Euler-Lagrange
Equations of motion of the system are
(M + m)ẍ + mL cos ϕϕ̈ = −bẋ − mL sin ϕϕ̇2 + Q1 ,
(14.4)
mL cos ϕẍ + (I + mL2 )ϕ̈ = −f ϕ̇ + mgL sin ϕ + Q2 ,
where Q1 [N] describes the control force accelerating the cart, Q2 ≡ 0
(the pendulum’s axle is not directly driven), M = 1.096 kg and m =
0.109 kg denote the mass of the cart and the pendulum, respectively,
Phase space of fi [rad/s vs rad] Phase space of fi [rad/s vs rad]

0.63 1.94
−0.01 0.80
−0.65 −0.34
−1.29 −1.48
−1.92 −2.62
−0.383 −0.088 0.208 0.504 0.799 0.384 0.488 0.592 0.696 0.799
Tracking error for fi [rad] vs time [s] Tracking error for fi [rad] vs time [s]
−0.249 0.000
−0.397 −0.075
−0.545 −0.150
−0.693 −0.225
−0.841 −0.299
0.00 1.25 2.50 3.75 5.00 0.00 1.25 2.50 3.75 5.00
Fig. 14.1. Comparison of the phase trajectories (upper row) and tracking er-
rors (lower row) of variable ϕ for slow, damped nominal motion (non-adaptive
and adaptive cases on the left and on the right, respectively) when only the
modeling errors have to be compensated
Phase space of fi [10^0 rad/s vs 10^0 rad] Phase space of fi [10^−1 rad/s vs 10^−1 rad]
75.1 63.0
38.6 27.2
2.0 −8.7
−34.6 −44.5
−71.2 −80.4
−14.0 −5.6 2.8 11.2 19.6 1.75 3.31 4.87 6.44 8.00
fi vs. time [10^0 rad vs s] fi vs. time [10^−1 rad vs s]

19.6 8.00
11.2 6.44
2.8 4.87
−5.6 3.31
−14.0 1.75
0.00 3.75 7.50 11.25 15.00 0.00 Nominal
3.75 7.50 11.25 15.00
Tracking error for fi [10^−1 rad] vs time [s] Adaptive factor s [10^−1dimless] vs time [s]
2.91 29.5
1.26 17.0
−0.39 4.5
−2.04 −8.0
−3.69 −20.5
0.00 3.75 7.50 11.25 15.00 0.00 3.75 7.50 11.25 15.00
Fig. 14.2. Results for undamped nominal motion under strong external per-
turbation: the phase space of ϕ (upper graphs) and the trajectory tracking
(central graphs) for the non-adaptive case (left-hand column), and the adap-
tive case (right-hand column) for D− = −250, ∆− = −300, ∆+ = 300 rad/s2
for a desired trajectory of fast oscillation (ω = 12 rad/s) while Q1 is also
perturbed by sinusoidal force of 120 N amplitude and ωp = 24 rad/s circular
frequency; in the lower graphs Qthe tracking error (on the left) and the cumu-
lative adaptive factor s(tn ) := nj=0 srel (tj ) constructed of the instantaneous
d
factors srel (tn ) = ÿÿ(t(tnn)−∆
)−∆+
+
for h, and srel (tn ) = ÿÿ(tn )−∆−
d (t )−∆
n −
for g (on the
right) for the adaptive cases are described; for the sake of achieving more
concentrated illustrations for f > 0 g, and for f < 0 h were applied in these
calculations
L = 0.25 m is the length and ϕ [rad] denotes the rotational angle of the
pendulum with respect to the upper vertical direction (clockwise), x [m]
denotes the horizontal translation of the cart+pendulum system in the
right direction, b = 0.1 [Ns/m] and f = 0.00218 [kgm2 /s] are viscous
friction coefficients, and I = 0.0034 [kgm2 ] denotes the momentum of
the arm of the pendulum. In the possession of the exact model (14.4)
could be used as follows: from purely kinematical considerations the
desired ϕ̈d could be determined and substituted into the lower equation
to yield the necessary ẍd . Then both values could be substituted into
the upper equation to obtain the appropriate Q1 force to accelerate the
cart:
M +m
Q1 = bẋ + mL sin ϕϕ̇2 + mL cos ϕ (−f ϕ̇ + mgL sin ϕ) +
2 2 L2 cos2 ϕ (14.5)
+ −(M +m)(I+mL )+m
mL cos ϕ ϕ̈d .
It is easy to see from (14.5) that near ϕ = 0 Q1 is a decreasing
function of ϕ̈ since the coefficient of ϕ̈d takes the following form:
−M (I+mL2 )−mI−m2 L2 sin2 ϕ
mL cos ϕ < 0, that is ∂Q 1
∂ ϕ̈d
< 0, therefore an instan-
d
taneous increase in ϕ̈ results in a decrease in Q1 and vice versa. In the
lack of the exact model instead of the result of the above considerations
a rough initial model Q1 = −0.05ϕ̈ + 15 can be used that possibly is
the simplest paradigm or “parabola” of (14.5) with a constant negative
“inertia coefficient” and an additive term. By its use the resulted angu-
lar acceleration can be observed, and in general in the kth control cycle
the deformed xd∗ d∗ d d∗ d∗ d
k := g(xk−1 |xk , D− , ∆− ) or xk := h(xk−1 |xk , D− , ∆+ )
can be introduced. In the simulations tracking properties defined by
ẍd (t) = ẍN (t) + P [xN (t) − x(t)] + D[ẋN (t) − ẋ(t)] were prescribed
with strong proportional P = 500 s−2 and weak derivative feedback
D = 0.1341641 s−1 .
Figures 14.1 and 14.2 illustrate the applicability of the proposed
control for the case of a very rough initial model and for combined
effects of modeling errors and drastic external perturbations unknown
by the controller. Following short, hectic initial learning the nominal
trajectories are nicely traced in both the space of the ϕ coordinate and
in its phase space as well.
14.5 Conclusion
In this paper a very brief overview and criticism of the universal approx-
imators based SC technologies and that of the classical methods using
Lyapunov functions was given. As alternative approach, instead of de-
veloping complete and permanent system models, incomplete, situation-
dependent, and temporal models can be used that require continuous
amendment or maintenance via machine learning. For this purpose spe-
cial Fixed Point Transformations were proposed for SISO systems. The
possibility of making this approach convergent for negative definite
systems was mathematically proved and demonstrated via illustrative
simulations. It can also be expected that by the use of the previously
applied group theoretical aids these new transformations can be ex-

tended to MIMO systems as it was done in [9].
Acknowledgment
This research was supported by the National Office for Research and
Technology (NKTH) and the Agency for Research Fund Management
and Research Exploitation (KPI) in Hungary using the resources of
the Research and Technology Innovation Fund within the project No.
RET-10/2006. The authors gratefully acknowledge the support ob-
tained from the Hungarian National Research Fund (OTKA) within
the project No. K063405.
References
1. Kolmogorov AN (1957) Doklady Akademii Nauk USSR (in Russian),
114:953-956
2. De Figueiredo JP (1980) IEEE Tr. Autom. Control, 25(6): 1227–1231
3. Wang LX (1992) Fuzzy systems are universal approximators. Proc. of
the IEEE Int. Conf. on Fuzzy Systems, San Diego, USA, California, 8-12
Mar 1992, pp. 1163-1169
4. Weierstraß K (1872) Über continuirliche Functionen eines reellen Ar-
guments, die für keinen Werth des letzeren einen bestimmten Differen-
tialquotienten besitzen. A paper presented to the ’Königliche Akademie
der Wissenschaften’ on 18 of July 1872.
5. Lyapunow AM (1892) A general task about the stability of motion, PhD
Thesis (in Russian)
6. Slotine Jean-Jacques E, Li W (1991) Applied Nonlinear Control. Prentice
Hall International, Inc., Englewood Cliffs, New Jersey.
7. Getz NH (1995) Dynamic Inversion of Nonlinear Maps with Applications
to Nonlinear Control and Robotics. PhD Thesis, University of California
at Berkeley.
8. Getz, NH, Marsden JE (1997) Linear Algebra and its Applications.
258:311-343.
9. Tar JK, Rudas IJ, Szeghegyi Á, and Kozlowski K (2006) Novel Adap-
tive Control of Partially Modeled Dynamic Systems. In ”Lecture Notes
in Control and Information Sciences”, Springer Berlin/Heidelberg, Ro-
bot Motion and Control: Recent Development, Part II - Control and
Mechanical Systems, (Ed. Krzysztof Kozlowski) 335/2006:99–111
10. Roska T (2001) Development of Kilo Real-time Frame Rate TeraOPS
Computational Capacity Topographic Microprocessors. Plenary Lecture
at 10th International Conference on Advanced Robotics (ICAR 2001),
Budapest, Hungary, August 22-25, 2001.
11. Tar JK (2005) Extension of the Modified Renormalization Transforma-

tion for the Adaptive Control of Negative Definite SISO Systems. In
the Proc. of the 2nd Romanian-Hungarian Joint Symposium on Applied
Computational Intelligence (SACI 2005), May 12-14, 2005, Timişoara,
Romania, 447–457
15
Driving Redundant Robots by a Dedicated
Clutch-Based Actuator∗
Anani Ananiev1 , Thorsten Michelfelder2 , and Ivan Kalaykov1

1
Center for Applied Autonomous Sensor Systems (AASS), Department of
Technology, Örebro University, 70182 Örebro, Sweden
{anani.ananiev,ivan.kalaykov}@tech.oru.se
2
Atlas Copco AB, Örebro, Sweden
thorsten@michelfelder.net
15.1 Introduction
The redundancy in the body construction of humans and animals makes

them very adaptable for a wide variety of natural environments. By
switching/activating/deactivating they can accommodate the neces-
sary locomotion for performing almost any task in their lifes. There-
fore redundant and hyper-redundant robots are at the focus of research
world-wide. The existing hyper-redundant robotic platforms are built
in a plenty of mechanical constructions and purposes of use, but have
a limited number of useful features that, unfortunately, limit their ap-
plicability in some important areas.
The common approach of actuating the robot modules in the major-
ity of available constructions is to embody a motor inside each module
with a respective gearbox. The advantage of this solution is the ease
of controlling the module motion independently on the other modules.
However, the plurality of all motors increases the mass and demands
higher power supply. All reviewed techniques of implementing hyper-
redundant robots have the disadvantages of: (i) using a large number
of motors, requiring multi-axis control systems, (ii) larger weight of the
construction, (iii) higher complexity of the control system due to the
need of synchronizing subsets of motors.
Therefore, we formulated a task to design a hyper-redundant robot
in a novel way in order to achieve an improved technique for simplified
driving of robot’s multiple-module mechanisms. Completely different
∗
The authors are grateful to KK-foundation, Sweden, for its support of the research
programs at the Center of Applied Autonomous Sensor Systems (AASS), Örebro
University, Sweden.
springerlink.com
168 A. Ananiev, T. Michelfelder, and I. Kalaykov
approach is when the rotation of only one irreversible DC motor is

transferred by a flexible shaft to all articulated links and a pair of
magnetic clutches enables two-direction articulation of each robot link.
They provide a certain angle between two adjacent links, and make it
possible to approach the desired angle with a certain speed. The devel-
oped concept requires a specific approach to control the angular speed
of each actuator such that the desired speed is stabilized regardless of
the speed of the only one available driving motor. Due to its robust-
ness, the sliding mode controller (SMC) is selected to be the control
algorithm.
In Section 15.2 we describe a method of actuating hyper-redundant
robots based on a single irreversible motor, the basic kinematics, and
some possible mechanical constructions. In Section 15.3 we present the
mathematical model of the dedicated actuator implemented by electro-
magnetic clutches, then in Section 15.4 a robust sliding mode controller
for stabilizing the angular speed of the secondary shaft of the actuator
is designed and tested.
15.2 New Method of Actuating Hyper-Redundant

Robots
The fundamental concept of our method [1] is to use only one irre-
versible motor for driving all segments of the hyper-redundant robot.
The produced mechanical energy is distributed selectively to a desired
destination by means of a set of electromagnetic clutches and transmis-
sion wheels. When an electromagnetic clutch is powered-on, the flexible
shaft rotation is translated to the shaft of the destination mechanism.
After a desired angle of articulation of the so driven segment is achieved,
the electromagnetic clutch is switched off. An electromagnetic break is
actuated to keep the segment at this desired angle of articulation. To
allow both positive and negative angles of articulation of the segment,
a second complementary electromagnetic clutch is attached appropri-
ately to another complementary transmission wheel.
This novel method for driving hyper-redundant robots by a single
irreversible motor requires a relatively simple control system, the first
part of which has the main task to set and stabilize the angular speed
of the motor. The second part has to perform simple on-off logic control
and can be implemented on a variety of commercial embedded micro
controllers.
We decided to make the most simple and biologically inspired struc-
ture of a snake robot as the first prototype in order to test the feasi-
15 Driving Redundant Robots by a Clutch-Based Actuator 169
bility of the concept, and identify and analyze possible technical prob-
lems. Figure 15.1 presents the construction and Fig. 15.2 the kinematic
scheme of the robot.
Fig. 15.1. First AASS hyper- Fig.15.2. Kinematics with worm gear
redundant robot prototype based mechanisms
According to Fig. 15.2, the robot comprises a plurality of segments

connected to each other, tagged 10, 20, 30, etc., actuated by a flexible
shaft 1. A pair of two adjacent modules articulates around a common
inter-link shaft 8 such that all modules 10, 20, 30, etc., form a chain.
The driving motor 100 is fastened to the body of the proximal seg-
ment 10 of the chain. The motor output shaft is connected to the prox-
imal side of the flexible shaft 1, which transfers the produced torque to
segments 10, 20, 30, etc. It is possible also to transfer the rotation to an
application dependent end-effector, articulated to the distal segment of
the robot.
A primary driving wheel 2 is fixed to the flexible shaft 1. A pair
of primary transferring wheels 3a and 3b is coupled to both sides of
wheel 2 such that wheel 3b rotates in the opposite direction to wheel
3a. Wheels 3a and 3b rotate permanently as motor 100 and shaft 1 do.
A pair of electromagnetic clutches 5a and 5b switches the rotations
further to the mechanism. The bodies of the clutches 5a and 5b are
fastened to the segment through fixture 7. The shaft of clutch 5a is
fixed to wheel 3a and the shaft of clutch 5b is fixed to wheel 3b. When
one of clutches 5a and 5b is activated, the rotation propagates further.
If none of 5a and 5b is activated, no rotation is transferred and the
segment does not articulate.
A pair of secondary wheels 4a and 4b receives the rotation from
wheels 3a and 3b, respectively. Each of wheels 4a and 4b is fixed to
the moving part of the respective clutch 5a or 5b. When clutch 5a is
activated, the secondary wheel 4a receives the rotation from wheel 3a.
When clutch 5b is activated, the secondary wheel 4b gets the rotation
from wheel 3b.
A secondary driving wheel 4c is coupled to both wheels 4a and 4b
such that it receives the rotation from one of 4a or 4b. This wheel 4c
is fixed to the inter-link shaft 8 that connects the pair of two adjacent
segments of the robot, for example 10 and 20, or 20 and 30. As shaft 8
is fastened to the body of the next segment 20 and articulates at the
body’s edge of segment 10, when the secondary driving wheel rotates,
the torque is transferred to swivel segment 20 at a desired relative angle
to segment 10. The same happens for each successive pair like segments
20 and 30, etc. Encoder 9 measures this relative angle. An electromag-
netic break 6 keeps this angle fixed for performing the desired pose of
the robot. For higher torques, the triple of wheels 3a, 2, and 3b form a
worm-type gear. At the same time the triple of wheels 4a, 4b, and 4c
form a normal teeth-wheel gear with an appropriate ratio.
Other structures based on the same principle of operation can
be composed such as a hyper-redundant SCARA robot implementing
rather complicated motions, compared to ordinary SCARA robots, or
a single-arm robot manipulator by fixing the proximal segment of the
snake to two additional degrees of freedom for obtaining a complete
three-dimensional working space.
An essential part of our concept is to stabilize the angular speed
of the secondary shaft of each module. Due to switching on-off of dif-
ferent amount of robot modules and having different payload on each
module, the load on the primary shaft is varying. Therefore a robust
stabilization of the angular speed of the secondary shaft of each mod-
ule is needed. In the next section we first model the electromagnetic
clutch based actuator, then design the system and test its robustness
properties.
15.3 Modeling Magnetic Clutch-Based Actuators
The primary axis of the magnetic clutch (Fig. 15.3), rotating freely with
a speed θ1 , is permanently connected to the anchor plate. The solenoid
is housed by an enclosure, which is freely rotating with a speed θ2
and is used as reagent to the anchor plate. The force F applied to the
anchor plate depends on the current i through the solenoid. When F is
closer to the maximum force Fmax, the two axes are coupled, therefore
θ2 = q.θ1 and ω2 = ω1 , where q 6 1 represents the slippage. When
F is too small F → 0, θ2 = const and ω2 = 0. The dynamics of the

secondary (driven) side is described by
J2 ω̇2 + a2 ω2 = τload , (15.1)
where J2 is the mass inertia and a2 is the damping parameter. The load
τload has to be compensated by a driving torque τf ric produced by the
friction between the anchor plate and the enclosure, i.e. τload = τf ric .
Fig. 15.3. Schematics of the electro - Fig. 15.4. General hysteresis loop
magnetic clutch (Bs : sat.flux, Hs : sat.field; Hc : co-
ercitive force [5])
The normal force F produces a friction force FM = F µ, where µ
is a friction coefficient. FM is orthogonal to F, static friction is not
considered. From [3] we obtain τf ric = 12 F µ(r1 + r2 ), where r1 and r2
are the inner and outer radius of the anchor plate. The normal force F
created by the coil magnetic field follows the electromagnetic model of
the coil
d(L.i) di dL dx
VL = =L +i , (15.2)
dt dt dx dt
where x is the distance (gap) between the anchor plate and the en-
closure. This gap is too small, so dL/dx ≈ 0 and Eq. (15.2) becomes
di
VL = iR + L dt . The inductance of the solenoid is L = µ0 AN 2 /l, where
N is the number of its windings, l is the solenoid length, µ0 is the
permeability of vacuum and A is the contact area between the anchor
plate and the coil. The magnetic field strength H is H = N i/l [8] and
the magnetic flux density B is obtained from the magnetic hysteresis
model [7]. The model of the hysteresis includes the magnetization and
its derivatives:

dB Bh− (H) − B dBh+ (H)
= µ0 + − µ0 , for dH > 0,
dH Bh− (H) − Bh+ (H) dH

dB B − Bh+ (H) dBh− (H)
= µ0 + − µ0 , for dH < 0,
dH Bh− (H) − Bh+ (H) dH
where Bh− (H) and Bh+ (H) are retrieved from the hysteresis loop of the
material, as shown in Fig. 15.4. By combining all the above equations
the differential equation (15.1) finally becomes
J2 ω̇2 + a2 ω2 = kµB 2 (t), (15.3)

A(r1 + r2 )
where k = .
4µ0
As Eq. (15.3) is non-linear with respect to the input voltage, we
simplify it by substituting the nonlinear B/H dependence by a linear
one, assuming σ = B(t)/H(t) is constant. Doing this we can simply
introduce the input variable u (the voltage to the solenoid) in an explicit
form. This simplification was verified by simulation of PID control of
the original hysteresis model and the linearized one. Choosing the state
variables
x1 = ω2 , x2 = ω̇2 , x3 = i,
ẋ1 = x2 , ẋ2 = ω̈2 , ẋ3 = i̇
and the voltage e applied to the coil as input u(t), we obtain
dx1 a2 σ2
= ω̇2 = − x1 + K x23 (t),
dt J2 J2
dx2 a2 σ 2 d(x23 (t))
= ω̈2 = − x2 + K , (15.4)
dt J2 J2 dt
dx3 R 1
= − x3 (t) + u(t),
dt L L
2
Aµ(r1 + r2 ) N d(x23 )
where K = and = 2x3 dxdt .
3
4µ0 l dt
Hence the nonlinear state-space model for simulation purposes is
   a2 2
    
ẋ1 − J2 0 K · σJ2 · x3 x1 0
 ẋ2  =  
 0 − aJ22 K · σJ2 · ẋ3  ·  x2  +  0  u(t).
2
(15.5)
1
ẋ3 0 0 −LR x 3 L
eq eq T
At steady (equilibrium) state the state variables are xeq = x1 0 x3 ,
where the zeroed second component corresponds to zero acceleration
xeq eq
2 = ω̇2 = 0. From this condition we can obtain a relation expressing
the x3 coordinate as a function of the other coordinates, namely
r
eq a2 eq
x3 = x , (15.6)
Kσ 2 1
which is used to simplify the clutch model. Given the reference values
x1d , x2d , and x3d , we define the error state space as
z1 = x1 − x1d ż1 = z2
and
z2 = x2 ż2 = ẋ2 = f (x) + g(x)u(t),
2 2
where f (x) = − L2 K σJ2 x23 − Ja22 x2 and g(x) = L2 K σJ2 x3 . The func-
tions f (x) and g(x) are used further to implement the sliding-mode
controller.
15.4 Controller Design, Simulation and Experiments
The model of the magnetic clutch driving mechanism was obtained

by assuming a constant load, but in reality the load changes due to
the complex kinematics of the articulated robot, i.e. J2 and τload vary
within a certain range. To obtain a robust performance we use a sliding-
mode controller (SMC), which is known with its good robustness to
parametric uncertainties [6], [4]. For our model a first-order sliding line
S with a slope λ is needed, namely
S = z2 + λ z1 = 0 or S = x2 − λ (x1 − x1d ) = 0.
Following [2] and [9] the discontinuous static feedback controller is

1
u= [−f (x) − λ x2 − W sign(S)] , (15.7)
g(x)
where W is a positive scalar. The proof of the sliding condition S · Ṡ < 0

as t → ∞ can be reviewed in [6].
The used laboratory physical system (one module of the robot of
Fig. 15.1) has following parameters: friction area A = 5.0265e − 5 mm2 ,
inertia load J2 = 0.009 kgm2 , number of windings N =544, coil resis-
tance R = 37 Ω, damping parameter a2 = 0.08, magnetic path length
l = 0.1 m, friction coefficient µ = 0.4, inner and outer radii of friction
area r1 = 0.01 m, r2 = 0.025 m, slope of the linearized hysteresis curve
γ = 0.001. For a reasonable result, the following values have been set
to the SMC for evaluation: W = 50, λ = 50 and σ = 0.0085. Some
experimental results can be seen in Fig. 15.5 and Fig. 15.6. They show
that the change of input speed ω1 does not affect the controllability for
the case ω1 > ω2 . That is mainly based on the fact that the friction
coefficient is independent on the speed difference of both friction sur-
faces, i.e. both shafts. Therefore, the change of speed of the primary
surface affects the speed of the secondary surface in an unnoticeable
amount.
Figure 15.5 demonstrates a phenomenon related to a change of
the load because of varying friction conditions. The control signal
(top curve) has low frequency oscillations with a frequency of about
0.5 sec−1 , which is equal to the desired output angular speed. They are
due to the mechanical tolerances between both rubbing surfaces of the
magnetic clutch and also because both surfaces are not perfectly par-
allel, therefore the produced torque for one rotation is not uniform. At
the same time the angular speed of the secondary shaft is quite stable
(bottom curve).
Fig. 15.5. Clutch control at desired Fig. 15.6. Step response

speed
15.5 Conclusions
The proposed novel method of driving a hyper-redundant robot by a

single irreversible motor accommodates a lot of advantages, compared
to many of the existing robot constructions. This can be considered as
a new paradigm in the design and building lightweight flexible hyper-
redundant robots for various types of applications. We believe that
many other constructions of hyper-redundant robots can be derived
upon the same methodology with minor modifications and amend-
ments.
The advantages of this method can be summarized as follows:
• As the plurality of motors is avoided, it provides a lightweight struc-
ture with a higher payload.
• The method can be used in a variety of constructions aimed at

modularity and simplicity.
• The construction can be expanded and reconfigured according to
the task to be accomplished.
• The variety of components is very low, therefore it is easy to assem-
ble and disassemble, making the robot relatively simple and eco-
nomical to manufacture and maintain.
• The control system is simplified for independent control of all mod-
ules applying on-off control actions to the electromagnetic clutches
and breaks and a simple single-axis motor regulator.
• Due to the irreversible rotation the backlashes are avoided.
The simulation and physical experiments of a dedicated magnetic
clutch based actuator show that the system is controllable with certain
restrictions. For example, acceleration is limited until the desired speed
is reached and the load change is bounded during operation. Simulation
has shown stable controllability of the system using different configu-
rations. Parameters have been chosen to be within the physical range
and achieve good performance. The physical system showed itself not
as robust as the simulation based on unmodelled nonlinearities due
to the still incomplete non-linear model reflecting imprecision of the
mechanical components and the entire assembly of the actuator.
The purpose of this paper is limited to only disclose the main con-
cept of the method and is far from being comprehensive in deriving,
studying, analyzing and proving certain theoretical, and physical prop-
erties, such as dynamic behavior, energy balance, etc. All these aspects
are subject of further study.
References
1. Ananiev A., Kalaykov I. (2004) Single-motor driven construction of
hyper-redundant robot, Proc. of Int. Conf. on Mechatronics and Robotics
’04, Aachen, Germany, 549–553.
2. Al-Muthairi N.F., Zribi M. (2003) Sliding mode control of a magnetic
levitation system, Mathematical Problems in Engineering, 2:93–108.
3. Böge, A. (1995) Formeln und Tabellen zur Technischen Mechanik, Vieweg
Verlag.
4. Edwards, C., Spurgeon S.K. (1998) Sliding Mode Control Theory and
Applications, Taylor and Francis.
5. Pohl J., Sethson M., Krus P., Palmberg J.-O. (2001) Modelling and sim-
ulation of a fast 2/2 switching valve. Fifth Int. Conf. on Fluid Power
Transmissions and Control, Hangzhou, China, pap. 2-22.
6. Slotine, J.-J., Li W. (1991) Applied Nonlinear Control, Prentice-Hall.

7. Tellinen, J. (1998) A simple scalar model for magnetic hysteresis, IEEE
Trans. on Magnetics, 34:2200–2206.
8. Tippler, P. (1994) Physik, Spektrum Akademischer Verlag.
9. Young, D.K., Utkin V., Özgüner Ü. (1999) A control engineer’s guide
to sliding mode control, IEEE Trans. on Control Systems Technology,
7:328–342.
16
An Energy-Based Approach Towards Modeling
of Mixed Reality Mechatronic Systems
Yong-Ho Yoo
Laboratory for Art, Work, Technology (artecLab) University of Bremen,

Enrique-Schmidt-Str. 7, 28359 Bremen, Germany
yoo@uni-bremen.de
16.1 Introduction
An environment where virtual worlds coexist with real worlds, mixed

reality, has been extensively studied in the last decade. Milgram’s defin-
ition of the Reality-Virtuality Continuum [8] focuses on display technol-
ogy such as head-mounted displays. However, people still feel the gap
between mixed reality environments supported only by display technol-
ogy and those closely integrating physical reality [10]. We do not restrict
mixed reality to a mixing of visual images or sounds, but include other
phenomena (e.g., electric, mechanical, hydraulic, aerodynamic, ther-
modynamic). Mechatronic technology can be used to enhance physical
usability in mixed reality. Bond graphs, originated by Paynter [2] and
further developed by Karnopp [3], are domain independent graphical
descriptions of the dynamic behavior of physical systems. Amerongen
[6] emphasized that the energy-based approaches toward the design of
mechatronic systems allow the construction of reusable and easily ex-
tendible models. Bruns [4] introduced a universal interface for mixed
reality using bond graphs, called hyper-bond. An improved hyper-bond
was presented by Yoo and Bruns [5]. A real energy dynamics can con-
nect with a virtual energy dynamics via an improved hyper-bond in-
terface providing relevant functionalities such as preservation of power
balance, causality reasoning of a connection between real and virtual
subsystems, stability, and quality. This paper presents a new methodol-
ogy of mixed reality design, called mixed reality bond graph, in which the
modeling methods of real energy dynamics, virtual energy dynamics,
and hyper-bond interfaces can be combined seamlessly. A distributed
mixed reality haptic ball manipulator, as an example using the mixed
reality bond graphs, is demonstrated.
springerlink.com
178 Y.-H. Yoo
16.2 Mixed Reality Bond Graphs

The mixed reality bond graph methodology adds the two following im-
provements to the traditional bond graph theory so that mixed reality
dynamics can be completely described:
Fig. 16.1. (a) concept of mixed reality bond graphs; (b) and (c) improved
hyper-bond interfaces
• a modified bond graph notation for discrete-time dynamics: In order

to distinguish between real and virtual dynamics in bond graphs,
two distinct bond graph notations are used. Traditional bond graphs
are used for real subsystems, and bond graphs with the new notation
are used for virtual subsystems (Fig. 16.1a). The new bond graph
notation is marked with the sign ‘*’ – e.g., virtual subsystems are
denoted R*, I*, C*, 1*, 0*, Se*, and so on. The interface between
real/virtual subsystems is achieved by a hyper-bond interface of
continuous/discrete-time dynamics.
• a new hyper-bond interface which is defined by the causality from
continuous-time dynamics to discrete-time dynamics and vice versa:
A hyper-bond interface, HB, has two causal combinations such as
flow-negative-feedback causality (Fig. 16.1b) and effort-negative-
feedback causality (Fig. 16.1c). This consists of an effort and flow
16 An Energy-Based Approach Towards Modeling 179
samplers, zero-order hold (ZOH), connection-impedance Z, effort or

flow generator MS (MSe or MSf ), and gain K – PI, PD, or PID con-
trollers may be used to improve transient response and steady-state
error. Connection-impedance Z consists of a collection of elements,
such as R-I-C, TF, and GY, to physically connect an energy subsys-
tem with the generator MS. It should be selected on the condition
of connecting to real subsystems.
16.3 Distributed Mixed Reality Haptic Ball Manipulator

A distributed mixed reality haptic ball manipulator is demonstrated
as an example using the mixed reality bond graph. Its construction is
shown in the left of Fig. 16.2. Rotational input from the user is changed
into translational motion of a virtual ball, while power is transferred
from a user’s handle to the virtual ball m; the user’s handle is provided
with haptic feedback from a virtual spring and the ball. If the torque
imposed on the user’s handle exceeds the force of the virtual spring, the
virtual ball m will be moved. If the virtual ball collides with a virtual
object M, collision energy will be added to the behavior of the haptic
ball manipulator. The pressure sensor and the edge of the pinion are en-
ergy coupling points. The mixed reality bond graph descriptions of the
Fig. 16.2. Structure of the haptic ball manipulator (left) and sketch of hard-
ware parts of the hyper-bond interface (right)
ball manipulator are shown in Fig. 16.3. Real/virtual subsystems are

described by two different types of bond graph notations, and they are
connected via a hyper-bond interface. Bond graph notations marked
with the sign “*’ again mean discrete-time dynamics of virtual subsys-
tems, and the sampling rate is determined by the hyper-bond interface.
MSe1 is the force imposed on the user’s handle; I3 is the inertia of the
user’s handle. 1/C2 and R3 are the stiffness and the friction of the
180 Y.-H. Yoo
user’s handle. TF1 is a power transformer between the user’s handle

and the pressure sensor on the connecting wheel. I2 is the inertia of the
virtual pinion, R2 is the friction between the rack/pinion, R1 is the
friction of the virtual spring, I1 is the inertia of the virtual ball/rack,
1/C1 is the stiffness of the virtual spring, and MSe2 is the collision
force when the virtual ball collides with the virtual object. The space-
Fig. 16.3. Mixed reality bond graph descriptions of the haptic ball manipu-
lator
state equations of the virtual subsystems can be derived from the bond
graphs. With the sampling rate decided in the hyper-bond interface,
the discrete transfer functions of the virtual subsystems are described
as follows:

EI1 (s) FvirtualA (s)
G1 (z) = Ztransf , G2 (z) = Ztransf ,
ErealA (s) ErealA (s)

FI1 (s) EI1 (s)−Ecollision
ErealA (s) Ecollision (s)

FvirtualA (s) FI1 (s)
Ecollision (s) Ecollision (s)
where FI1 and EI1 are the velocity and force of the virtual ball, ErealA is
the real force in the energy coupling point (port A), and FvirtualA is the
virtual velocity in the energy coupling point, Ecollision is the collision
force, G1 ,G2 , and G3 are the transfer functions when Ecollision is zero
and G4 ,G5 , and G6 are the transfer functions when EportA is zero.
A hyper-bond interface using the flow-negative-feedback causality
(Fig. 16.1b) is used. The hardware structures of the hyper-bond inter-
face is selected such as in the right of Fig. 16.2. Figure 16.4 shows the
components and connections of the hyper-bond interface and real sub-
systems. The generator MS is a current supplier (MSf1); components
of the connection-impedance Z between the current supplier and the
connecting wheel are the DC motor (GYz), friction and inertia of shafts
and gears (Rz and Iz). The position values from the position encoder
are changed into velocity values. It is necessary to derive the forward
Fig. 16.4. Components and connections of the hyper-bond interface and real
subsystems
transfer function GV toR (s) between fvirtualA and frealA in order to take
into account stability, transient response, and steady-state error of the
hyper-bond interface and real subsystems. The state equations of the
hyper-bond interface and real subsystems are
   R3 1 R3
   1 
f2 − I3 − I3 m·I f2 I3 0

d    1 1
3
     M Se1
e5 =  C2 0 − m·C2  e5 + 0 0 ,
dt f R3 m R3 Rz 1 M Sf1
10 Iz Iz − m·Iz − Iz
f 10 0 Iz
  (16.1)
f2
frealA = 0 0 1  e5  . (16.2)
f10
Therefore, the discrete forward transfer function G V toR (z) between
f virtualA and f realA is
1 − e−sT FrealA (s)
GV toR (z) = Z − transf { · }, (16.3)
s FvirtualA (s)
where M Sf1 (s) = K · FvirtualA (s). When this mixed reality system has
values of parameters as follows: R1 = 5 N-m-s, I1 = 1 N-s2/m, C1 =
0.02 m/N, C2 = 0.0001 m/N, R2 = 5000 N-m-s, R3 = 10 N-m-s, I2
= 0.05 N-s2/m, I3 = 1 N-s2/m, and TF1 = 0.8, Rz is 10 N-m-s, Iz
is 1 N-s2/m, r of GYz is 1, and the sampling rate of the hyper-bond
interface is 0.001 seconds –the common servo rate for haptic interfaces
[1] – the discrete forward transfer function GV toR (z) is
182 Y.-H. Yoo
0.001z 2 − 0.002z + 0.001

GV toR (z) = K · . (16.4)
z 3 − 2.9839z 2 + 2.9871z − 1.0035
The root locus of the negative feedback system that has the discrete
forward transfer function GV toR (z) is sketched as in the left of Fig.
16.5. Thus, the system is stable for K between 25 and 2000. The value
Fig. 16.5. The left shows the root locus of the unity feedback system that
has the discrete forward transfer function GV toR (z), and the right shows
behaviors of handle (I3) position and mass (I1) position from the simulation
of Fig. 16.6 in 20-sim
of K which ensures stability and a damping ratio of 0.7, is 1009. There,

however, is a critical difference because of the steady-state error and
its return caused by the positive feedback via the virtual subsystem.
This problem can be improved by the PI controller. The PI compen-
sator with Kp = 1009 and Ki = 100.9. Thus, the discrete-time PI
compensator is
0.00091z − 0.0009910
GP I (z) = . (16.5)
z − 0.9999
The mixed reality bond graph modeling (Fig. 16.3) is implemented in
20-sim as shown in Fig. 16.6. DiscreteDifferential1, DiscreteIntegral1,
and q-sensor (position sensor) are components to use position input in-
stead of velocity input (f-sensor). The simulation is shown in the right
of Fig. 16.5. The layout of the distributed haptic ball manipulator with
ODE (Open Dynamic Engine) is shown in Fig. 16.7. ODE is a free, in-
dustrial quality library for simulating articulated rigid body dynamics
[7]. The local engine operated by the algorithms, as shown in Fig. 16.6,
runs only to control the haptic ball manipulator locally, while the re-
mote engine operated by ODE runs to calculate the behaviors of the
virtual ball of the haptic ball manipulator and the other virtual objects
in the virtual environment. The local and remote engines are connected
Fig. 16.6. Implementation of the mixed reality bond graph model of Fig.
16.3 in 20-sim
Fig. 16.7. The distributed mixed reality haptic ball manipulator with ODE
via the Internet. To implement the hyper-bond interface of Fig. 16.6,

we used a pressure sensor and a position encoder to sense the user’s
force and a DC motor for force feedback to the user’s handle. To pro-
vide a common haptic servo rate (1000 Hz), a sensoray 626 PCI board
– with four 14-bit D/A outputs, 20 kHz update rate and sixteen 16-bit
differential A/D inputs, 15 kHz rate – is used as an A/D and D/A con-
verter. The DC motor driver, model 412CE (Copley Controls Corp.),
184 Y.-H. Yoo
is used to provide a DC motor. A pressure sensor and a Wheatstone

bridge are used to measure the user’s force.
16.4 Conclusion
The traditional bond graphs for virtual subsystems are incomplete rep-
resentations because they do not contain discrete-time dynamics which
can run directly on the digital computer. In this paper we have shown
a new methodology to clearly describe together the continuous-time
dynamics for real subsystems and the discrete-time dynamics for vir-
tual subsystems, and to couple both in a mixed reality energy model.
As an example, the distributed mixed reality haptic ball manipulator
is demonstrated. Because of the unified concept of bond graphs it can
be concluded that this methodology can be used to model also mixed
reality systems with other physical phenomena.
References
1. Salisbury K, Conti F, Barbagli F (2004) Haptic Rendering: Introductory
Concepts, IEEE Computer Graphics and Applications, 24-32
2. Paynter H M (1961) Analysis and Design of Engineering Systems. MIT
Press, Cambridge, MA
3. Karnopp D C, Margolis D L, Rosenberg R C (1975) System dynamics, a
unified approach. John Wiley, New York
4. Bruns F W (2001) Hyper-Bonds (Enabling Mixed Reality). artec-paper,
Bremen University, 82
5. Yoo Y H, Bruns F W (2006) Energy Interface for Mixed Reality. Proc.
12th IFAC Symposium on Information Control Problems in Manufactur-
ing, Saint Etienne
6. Amoerongen J V, Breedveld P (2004) Modelling of Physical Systmes for
the Design and Control of Mechatronic Systems.IFAC, Professional Brief,
Oxford, UK
7. Smith R (2004) Open Dynamics Engine v0.5 User Guide. ode.org,
http://ode.org
8. Milgram P, Kishino F (1994) A taxonomy of mixed reality visual displays.
IEICE Trans. Information Systems, E77-D:12
9. Bruns F W, Erbe H -H (2005) Mixed Reality with Hyper-Bonds. Control
Engineering Practice, Elsevier
10. Ishii H, Ullmer B (1997) Tangible bits: towards seamless interfaces be-
tween people, bits and atoms. Proc. CHI ’97, 234-241
11. Yoo Y H (2007) Mixed Reality Design Using Unified Energy Interfaces.
Dissertation, FB3, University of Bremen, Germany
17
Navigation of Autonomous Mobile Robots –
Invited Paper
J.Z. Sasiadek1 , Y. Lu1 , and V. Polotski2

1
Carleton University, Department of Mechanical and Aerospace
Engineering, 1125 Colonel By Drive, Ottawa, ON, K1S 5B6, Canada
jsas@connect.carleton.ca
2
Frontline Robotics, 1920 Research Road, U62, Ottawa Int. Airport,
Ottawa, ON, K1V 9B4, Canada
vpolotski@fl-r.ca
17.1 Introduction
Navigation of autonomous vehicles and robots can be divided into two

categories: indoor navigation and outdoor navigation. In general the
outdoor navigation is more difficult and complex task because often
the environment does not have characteristic points that can be iden-
tified and with respect to which robots could relate their position. If
environment is unstructured (e.g. off-road environment), problems re-
lated to autonomous navigation are very difficult. Problems related
to navigation in the unstructured environment could be divided into
mapping, localization, collision avoidance and trajectory tracking.
17.1.1 Mapping and Localization
In an ideal case an autonomous robot has all information regarding the

surrounding environment (local) as well as global environment. Infor-
mation usually is presented in form of map. If map is static that means
that the characteristic features of the map are not changing in time and
robot can design its trajectory before it moves from original location to
its final destination. Very often the map is dynamic, which represents
moving objects (e.g. other vehicles, peoples, changing configuration of
permanent objects etc.). In that case an autonomous robot has to cre-
ate a map as it moves along and navigates in any given environment.
This is a local map of the surrounding world, objects, and features in
neighborhood of moving robot. The map is acquired using variety of
sensors. Some sensors would give information about robot’s position
springerlink.com
188 J.Z. Sasiadek, Y. Lu, and V. Polotski
with respect to the nearest structure or obstacle (e.g., laser range find-
ers, sonar, radars, vision systems) while the other sensors would give
measurements about distance traveled from the original location (e.g.
encoders, INS). This measurement method is known as odometry. Once
information about the world around the robot is acquired, the robot
can use it for its own localization.
In general two main methods of mapping could be distinguished:
first, metric, and, second, topological. Metric maps acquire geometri-
cal features and proprieties of the environment, while the topological
methods are focused on connectivity points. The metric methods are
dominated by probabilistic methodologies. At present, there is a broad
consensus that probabilistic approaches give the best results in map-
ping and its application in autonomous vehicle navigation. There are
several problems with probabilistic methods in mapping. First problem
is that algorithms based on those methods require large computational
effort caused by high dimensionality.
The second problem is related to data association caused by un-
certainty if the mapped region can be identified and match with real
world. The third problem is connected to measurement noise. In many
statistical and probabilistic methods it is assumed that the measure-
ment noise is so called white noise with Gaussian distribution and zero
mean value. Unfortunately, this assumption is untenable for robotics
vehicle for which measurement noise is correlated and depends on robot
position, attitude and design. In short, measurement noise is colored
noise.
Over many years large number of probabilistic mapping methods
have been developed. In this paper only some of them will be addressed.
The earliest approach is occupancy grid mapping method with its
derivatives. The second are methods based on Kalman filtering and in-
clude, between others, simultaneous localization and mapping (SLAM)
method. Nowadays, more and more, SLAM name is used in conjunc-
tion with methods that are not based on Kalman filtering. The Kalman
filtering methods are essentially restricted to linear or linearized sys-
tems with measurement white noise assumption. Within the second
group there are advanced methods based on adaptive Kalman filtering.
The idea behind it related to extension of Kalman filter applicability
to measurement with non-white noise. The gain of the Kalman filter
is adapted to prevent divergence caused by colored noise. The third
approach includes particle filters. Particle filters can be applied for
non-linear systems with colored noise.
17 Navigation of Autonomous Mobile Robots 189
17.1.2 Sensor Fusion
Sensor fusion is a measurement integration procedure. As the robot

navigates, many different measurements and information are obtained
from multiple sources (sensors). All information should be integrated
using one of the sensor fusion methods. These algorithms can be classi-
fied into three into three different groups. First, fusion based on prob-
abilistic models, second, fusion based on least-squares techniques and
third, intelligent fusion. The probabilistic model methods are Bayesian
reasoning, evidence theory, robust statistics, recursive operators. The
least-squares techniques are Kalman filtering, optimal theory, regular-
ization and uncertainty ellipsoids. The intelligent fusion methods are
fuzzy logic, neural networks and genetic algorithms.
17.1.3 Collision Avoidance
An autonomous robot navigating in crowded (cluttered) environment

has to avoid variety of obstacles. Those obstacles may be static or
dynamic. In case of static obstacle an appropriate algorithm has to
be developed. Very often the method used is a simple proportional,
or constant angle navigation. In case of dynamic obstacles a special
collision avoidance algorithm has to be developed. Collision avoidance
with dynamic obstacles is a much more complex procedure. In this case
mapping and active collision avoidance and related trajectory planning
is an indispensable part of robot control system.
17.1.4 Trajectory Tracking
There are three systems required for the autonomous vehicle to follow
the designed path. Those systems are navigation, guidance, and control
system. In navigation problem, two basic position-estimation methods
usually applied: absolute and relative positioning. For positioning, two
types of sensors are available, internal and external sensors. Internal
sensor will provide physical data and information that can be measured
on the vehicle. The examples of these sensors are encoders, gyroscopes,
accelerometers and compasses. External sensors measure relationships
between the robot and its environment, which can be composed of
natural or artificial objects. The examples of external sensors are sonar,
radars and laser range finders.
Measurements of internal sensors are quite accurate for short period.
However, for long-term estimation, the measurements usually produce
a typical drift. External sensors do not produce the drift, however, be-
cause of their characteristics, the measurements are not always avail-
able. This problem may be solved by using multi sensors in navigation.
Internal sensors can be used to estimate the position of the vehicle
during a particular period. External sensors are then implemented to
correct the errors that come from internal sensors. Both of those types
of sensors have bound errors, and therefore a simple reset of internal
errors is not sufficient. A better way is to fuse those two measurements
in such a way so it will produce the best desire estimation.
The sensor fusion methods can be divided into three different
groups. First, fusion based on probabilistic models, second, fusion based
on least-squares techniques and third, intelligent fusion. The proba-
bilistic model methods are Bayesian reasoning, evidence theory, robust
statistics, recursive operators. The least-squares techniques are Kalman
filtering, optimal theory, regularization and uncertainty ellipsoids. The
intelligent fusion methods are based on fuzzy logic, neural networks
and genetic algorithms.
This paper presents an example of autonomous robot navigation in
specific conditions and environment. In this particular case, a security
type robot was used to navigate through gates. This presentation was
done on the basis of [1, 2] papers.
17.2 Mobile Robot Navigation Through Gates
A robot shown in Fig. 17.1 was described and used in [1]. The robot
was deployed in security applications. This type of robots can carry
out patrol tasks in the critical environments such as airport, nuclear
power plant stations, gas plants and other industrial establishments.
The problem of gate recognition and crossing shown below is a part
of this intelligent security mobile robot system. During patrol task of
the mobile robot, GPS provides the most global level navigation and
often is used for navigation at local level. However, there are some
disadvantages of using a GPS sensor. They could be:
1. Periodic signal blockage due to obstruction.
2. Special situations, such as the navigation of through the gate, in
which an error in GPS location signal may be too large and have
serious consequences.
Those problems are particularly important if differential GPS(DGPS)

cannot be used, either because it is not available in the area or it is
Fig. 17.1. Mobile robot platform for security applications
not desired due to logistic requirements. A few meters precision of ba-

sic localization is not sufficient for guiding through the gate or narrow
passage between two buildings, where typical width is of 4-6 meters
and required precision is of 0.3 − 05 m. Considering the above reasons,
combining GPS with other navigation technologies would be very effec-
tive. This method was using in several applications. One of the possible
applications of sensor fusion was shown in [3, 4]. This original method
was based on adaptive Kalman filtering with fuzzy logic adaptation
procedure. This procedure ensured stability and convergence of filter
gain.
17.2.1 Problem Description
The gate crossing problem addressed here consists of detecting the en-
trance using a proximity sensor and then reactively navigating through
this entrance. There are several methodologies that can be used to
solve this problem [5]. The gate crossing problem differs from range
based wall-following, since it requires the transition from an open field
GPS or fusion-based navigation to range-based navigation. This tran-
sition itself requires a feature extraction and recognition phase usually
not necessary in the environments as underground mines, where range-
based wall-following is sufficient [6].
The extraction of robust geometrical features from the data pro-
vided by the LMS sensor is not an easy task [7, 8, 9]. The proposed
solution for guidance through a gate consists of several steps: environ-
ment representation, LMS data filtering, gate recognition, localization

and motion control.
The robot platform, ARGO conquest 6×6, Fig. 17.1, is manufac-
tured by Ontario Drive & Gear Ltd. (ODG). It was retrofitted, for
computer control and equipped with NovAtel GPS, MicroStrain iner-
tial sensor, built-in wheel odometry, and LMS-221 laser scanner (man-
ufactured by SICK).
The problem described in this paper consists of guiding a vehicle
through the area delimited by two objects of known shape called posts
located at the known distance from each other (entrance width). The
geographical location of the whole structure (called gate) is supposed
to be only roughly known. This means that the longitude/latitude of
the entrance center point can be transformed to its position on the
area map with few meters accuracy, but the geometry of the gate can
be known with few centimeters accuracy.
The typical task to be performed consists of navigating along the
path that leads to the gate area and passing through the gate. Typical
entrance width is 6 m, typical size of the post is about 1 m. Experi-
mental vehicle used for a proof of concept stage is about 2 m wide and
3.5 m long.
Processing of data provided by a laser scanner is a crucial element
for gate navigation. LMS operates according to the time-of-flight prin-
ciple. The working principle is simple: a laser pulse emitted for a short
time is reflected by a target object. A count starts while the pulse is
transmitted, and stops when the reflected signal is received. The emit-
ted pulse is diverted by a rotating mirror in the scanner (LMS 2xx User
Manual, SICK AG, 2003). The angular resolution of the scanner was
set to 0.5◦ . The maximum range is 80 meters, although we have limited
it to 30 m in order to get reliable and meaningful data. Therefore, a full
scan of 180◦ provides 361 range values (indexed according to a scanning
angle). The data is transferred to the controlling computer by RS422
interface (500 Kbaud) at which data transferring of a full scan is within
13.3 ms. According to the manufacturer’s specifications, the scanner’s
error is up to ±1 cm within the maximum range 80 meters.
17.2.2 Gate Identification Procedure and Signature Concept
Two wooden boxes (cubes of 1.20 m) have been used to construct the
gate. The distance between the boxes – the gate entrance – is set to
6.00 m. The vertices are indexed for reference as shown in Fig. 17.2 (the
edge is referenced by delimiting vertices).
Depending of the position of the sensor, the perception of the gate

obtained using LMS scanner is different. To address the perception of
the gate geometry by the sensor, the neighborhood of the gate is divided
into proximity zones. Fig. 17.3 shows the ten partitioned zones of the
gate.
In each zone some posts’ edges are observable while the others are
hidden. Fig. 17.4 illustrates visible and hidden edges for the scanner
located in zone V: the edges 1-2, 2-3, 5-6, and 6-7 are observable and
the edges 3-4, 4-1, 7-8, and 8-5 are hidden.
Fig. 17.2. Gate dimension and vertex points (dimensions in meters)
The obtained scan (observable image of the gate) is shown in the

inserted rectangle. As can be easily seen, each scan contains two por-
tions (corresponding to two posts); each portion contains an isolated
segment or two adjacent segments (a corner). Fig. 17.5 is another scan-
ning illustration from zone IX.
Scanning from zones (I-IV, X in Fig. 17.3) gives poor representations
of the gate as illustrated by icons in the Table 17.1. For Zone I and II
there is only one visible edge, in zone III and IV the reflection degrades
since the part of the surface is illuminated at a very small angle. Our
control algorithm will try to avoid guiding through those zones I-IV,
using zones V-IX instead. Zone X also gives a poor representation of
the gate, but this zone is inevitably visited by the vehicle after it has
almost crossed the gate area. Recognition is not an issue by that time
– the gate is almost crossed but the special procedure is needed to
Fig. 17.3. Gate approach zones for navigation
Fig. 17.4. Visibility of edges from LMS and the visible gate segments form
zone V
estimate the relative pose of the vehicle in this zone. Gate visibility
from all ten zones has been presented in Table 17.1.
In order to compute the vehicle’s location relative to the gate, the
zones have first to be distinguished from which the scan is obtained. The
concept of the “gate signature” has been proposed. Signature vector is
computed considering the length of each observed segment, the distance
between two continuous portions of the scan (gap), and the distance
between the scan start point and the scan end point (size).
Fig. 17.5. Visibility of edges from LMS and the visible gate segments form
zone IX
Table 17.1. Gate visibility from the respective zones
Two examples of signature computation is given in Fig. 17.6: in

the upper rectangle, there are four segments, ab, bc, ef , and f g; ce
is a gap between two posts and ag is the total width of the scan. A
vector becomes [ab, bc, −ce, ef, f g, ag]. The negative sign of the third
component emphasizes the fact that it corresponds to the gap and not
to the edge. Similarly, for the low rectangle, the signature would be
[bc, cd, −de, ef, f g, ag].
Table 17.2 shows the possible canonical signature vectors for the
scans obtained from the zones V, VII, and IX. One can see that even the
Fig. 17.6. Signature illustrations of a gate segment
dimension of the signature varies from zone to zone. Canonical signa-

tures are computed for every zone. By comparing this on-line signature
against the set of canonical zone signatures we may roughly determine
the robot location with respect to the gate (corresponding zone) and
then proceed to the gate recognition and finally to the refinement of
the localization result and guidance.
Table 17.2. Signature of the gate scans

17.2.3 Experimental Procedure
An algorithm of gate recognition is described in this section. The al-

gorithm contains several modules: the GPS/INS/LMS data collection
module, the map building module, the map filtering module, the map
fitting module, the gate signature calculation module and the unex-
pected error check module. The flowchart of the algorithm is shown in
Fig. 17.7.
Data is acquired from GPS/INS/LMS firstly with GPS/INS/LMS
data collection module. The map-building module computes the map
of the current environment using row data from LMS. The preliminary
map includes all the visible objects within the range of the laser scanner.
Some of these objects are false positives; they do not correspond to
real objects but to the noisy LMS measurements (physically attributed
to multi-path reflections, dust etc.). An example of the preliminary
map from the raw data is given in Fig. 17.8. The encircled object is a
gate. Generally speaking, the preliminary map cannot be used directly
because of too many non-gate objects. All the noisy points and non-
gate objects from the preliminary map would be removed by the map
filtering module. It evaluates all the detected objects by their dimension
and relative position keeping only objects corresponding to the gate
posts. The approximations for post size and entrance width are used
for this computation (50% difference in size and width is tolerated).
The map-filtering algorithm is described in details as:
Step 1: Set the effective range to ∼ 30 m (software limit) while the
maximum range of LMS sensor is set to 80 meters (hardware limit).
Thus, all points with ranges higher than 30 meters are deleted;
Step 2: Delete all the objects consisting of only one isolated point
(usually false positives corresponding to noise);
Step 3: Determine the dimension of each object and delete those
objects that, based on their dimensions obviously do not correspond to
the gate (50%) tolerance);
Step 4: Calculate the distance between two consecutive (scanning
from left to right) objects and delete those which, based on their relative
distance are clearly not the gate posts (50% tolerance).
Figure 17.9 shows the filtered map corresponding to a preliminary
map from the Fig. 17.8. After the above steps, if the filtered map still
contains more than two objects, the tolerance conditions for the dimen-
sion and distance are tightened and the filtering algorithm is re-applied.
Fig. 17.7. Flowchart of gate navigation module
The points obtained from the scan are discrete, but represent con-
tinuous edge lines. Each edge is delimited by vertex points and can be
identified by those points. The vertex points are located on the border
of the point cloud (have neighbors only on the right or only on the
left hand side) and thus correspond to the corner of the object. The
estimated gate image is obtained by connecting the vertex point one
by one.
The detail of the segment fitting algorithm may be described in the
following steps:
Initial step: Determine which points belong to the gate. In the
module, we did not consider the effect from environmental disturbances
and assume that the only object in the filtered map is the gate. In an
example shown in Fig. 17.10, the gate includes all the points marked
as shown in the lower rectangle of Fig. 17.6 from point (b) to point (g).
Fig. 17.8. Map of the raw scanning data
Fig. 17.9. Filtered and enlarged gate object
Step 1 : Scanning from left to right, find the last point of the left
hand post, point d, and the first point of the right hand post, point e.
This is done by comparing the distance between each two consecutive
points and the canonical sizes of the post and of the entrance;
Step 2 : To find the “corners” (points c and f in Fig. 17.6 notation),
if they exist, we search for points which have the largest distance to
the line connecting the first and the last points of each post ((b),(d)
and ((e),(g)). For the case illustrated in Fig. 17.10, two corners for the
two posts exist.
When all the vertexes/corners are identified, the current signature

for the scanned gate is constructed as explained in section 17.2.2. For
example in Fig. 17.5, the current signature is a vector of dimension 6
(full): [bc, cd, −de, ef, f g, ag]. Comparing this current signature with
canonical signatures we assume that the gate is recognized if a canon-
ical signature matches the current signature. For matching decision
the squared norm of the difference between the current and canonical
signature was used.
Let the canonical signature be:

A0 = a01 a02 −a03 a04 a05 a06
and the current signature be:

A = a1 a2 −a3 a4 a5 a6 .
The criterion norm of A − A0 is:
e = (a1 − a01 )2 + (a2 − a02 )2 + (a3 − a03 )2 + (a4 − a04 )2 +

1/2
+(a5 − a05 )2 + (a6 − a06 )2 .
For matching the value e is compared against the threshold, emax . If

e < emax two vectors are matched (otherwise they are not). The positive
matching result (e < emax ) means the gate is recognized. The next map
fitting step consists of establishing correspondence between observed
points and the gate segments (edges of the posts).
Computing relative position of the vehicle with respect to the gate
is a crucial step for being capable of designing an appropriate controller
for guiding the vehicle through the gate. The localization module out-
puts the data to the gate crossing control module. Here the final step
of processing localization data is described. Let us define a coordinate
frame {OXY } attached to a vehicle with the original point O - being
a point of the platform where LMS is located, OY – being a longi-
tudinal axe of the vehicle aligned with 90◦ LMS beam. Figure 17.10
illustrates the layout. The gate recognition procedure is based on the
extraction of points {1, 2, 3, 4, 5, 6, 7, 8}. Using this information the
middle point C of the entrance and the direction of the straight line
containing {2, 3, 6, 7} (from the left post to right post) can be es-
timated. More precisely, we are estimating the direction of the beam
pointing from the left to the right post – let us call it entrance direction,
PEN T ER . An angle (ψ) of this beam with OX axe is the first parameter
to be output to the control low being designed. Let us defined a target
point T somewhere shifted from C along the direction orthogonal to

PEN T ER , called the gate direction PGAT E .
Second output parameter is an angle,α, between OT and OY . As
can be easily seen for the vehicle perfectly aligned with the gate both
angles are zero. Figure 17.12 shows extraction of the middle point and
entrance direction from the real LMS image.
Fig. 17.10. An image of the gate
Fig. 17.11. The layout of localization and control method

4
Gate left post
Middle point for navigation
0
Y (meter)
-2
Gate right post
-4
-6
-8 AR G O
14 16 18 20 22 24 26 28
X (meter)
Fig. 17.12. Final gate image and the middle guide point
17.2.4 Control System
In order to guide the vehicle through the gate one may apply either
planning/correction approach or nonlinear control approach. We have
chosen the latter, exploiting the similarity of the problem of guiding
a vehicle towards and through the gate with the parking problem ad-
dressed in [7]. In both cases we have to drive the vehicle to a desired
pose (position/orientation). For the “gate through” problem the desired
position is a middle point of the entrance segment and orientation –
orthogonal to this segment. In contrast with original settings of [7], in
our case the target point is not known in advance but has to be con-
tinuously estimated on-line. This constitutes a remarkable difference
between gating and parking problems. Another difference is that we do
not constrain the vehicle speed by zero at the target point, but rather
set this speed to the desired value (usually lower than in the field). In
order to address these differences, we (i) estimate the entrance middle
point C, (ii) we define a goal point T , (it could be any point located
in the middle of the gate and little away from the rear side of the gate
post,) as a point moving from the entrance middle point along the gate
direction (see definition above). This procedure allows us to keep the
vehicle offset from the goal point (singular point, [7]), thus ensuring a
stable motion across the gate.
An intermediate control output is defined as follows:
u = K1 α + K2 ψ (17.1)
where K1 and K2 are chosen using a pole placement technique. For

our field experiments K1 = and K2 = −0.4. This choice corresponds to
both poles set to −1 on the Z-plane.
In order to compute the final control output, a particular steering
design of the ARGO vehicle (skid-steering) is taken into account. In
order to avoid unnecessary reactions on the small offsets that can even
lead to unstable behavior a dead zone −umin , umin for u is introduced
and final control output is computed as follows:
U = N (sign(u − umin ) + sign(u + umin )) /2 (17.2)
where N stands for a value to be applied for initiating a vehicle turn.
17.2.5 Results and Discussion
Using simulation, all the developed gate-recognition and guiding-through

algorithms are tested first with synthetic data. In addition, the filtering
and signature-based recognition modules have been verified with LMS
data collected along the manual driving through the gate. Finally, the
algorithm has been fully integrated on-board and tested in the field.
Fig. 17.13 shows the whole trajectory of ARGO from GPS data during
the experiment. The area in the dashed circle is gate recognition and
crossing area.
The map filtering module continuously monitors the environment
based on the available LMS data. If the gate is actually farther than a
soft-upper-limit range the filtered map is empty. When the gate posts
become observable they completely appear in the filtered map after
relatively short transition period corresponding to the partial visibility
of the posts, noisy data etc. Our experiments show that this period is
usually about 1 second and never exceeds 3 seconds. As soon as the gate
becomes consistently visible, the recognition module computes the gate
signature, searches for a closest canonical signature, and estimates the
vehicle pose relative to the gate. The localization module computes the
gate middle point C and gate direction PGAT E , and sends them to the
motion control module, which calculates the control outputs U and send
it to the hardware (low level controller). Figure 17.15 illustrated motion
through the gate. Small circles correspond to the vehicle trajectory,
stars to few consecutive vehicle locations with gate images acquired
from those locations.
Fig. 17.13. GPS trajectory of experiment
’
Fig. 17.14. Illustration trajectory of gate through’
The uncertainty (of about 2m) in absolute (GPS/INS/Odometry-

based) position of the gate could be seen. In spite of this uncertainty
the LMS-based position of the vehicle relative to the gate is precise
enough (0.1 m, 3 deg) and a vehicle successfully navigates through the
gate using our algorithm described above. Stable gate recognition is
crucial for subsequent steps of the algorithm. Figure 17.15 shows the
recording of the recognition results while the vehicle is autonomously
entering the gate. The offset (2-norm of the difference) is between the
current signature and a canonical signature (taken from the data base
N orms of the difference between the two s ignatures

1.8
1.6
1.4
1.2
0.8
0.6
0.4
0 10 20 30 40 50 60 70 80 90 100
Gate scanning sequence
Fig. 17.15. Recognition error
according to the zone where the vehicle is located). It may be seen that
the error is reduced significantly in accordance with the distance to
the approaching gate. The soft-upper- limit range has been set based
on theoretical and experimental analysis. If the posts are located too
far apart although below the hard-range limit, the recognition results
deteriorate. This can be roughly explained as follows: observing a small
object (a singular point in the limiting case) we do not get enough
information for positioning – lateral and orientation offsets are coupled.
In practice the computation remains possible but measurement errors
are amplified and results become unusable.
The developed concept of gate signature provides an effective method
to estimate the relative position between the vehicle and the gate. The
motion control algorithm based on the nonlinear transformation to po-
lar coordinates proposed in [10] coupled with on-line estimation of the
vehicle pose and enhanced by the moving target point for avoiding sin-
gularities ensures the stable gate crossing with acceptable lateral errors
of about 0.3 m. In this work the more general fusion of range measure-
ments with GPS/INS/Odometry data have not been addressed. Using
methods proposed in [11, 12] the absolute positioning of the vehicle
along the “gate crossing” portion of the path can be improved. Such a
“tightly coupled” absolute and relative localization is expected to im-
prove the reliability of the navigation system providing better global
position estimates along the whole path and smoothing the phase of ap-
proaching the gate. However, there are interesting methodologies that
would allow navigation and the gate crossing with yet higher accuracy.
Using sensor fusion method described in [3, 4] navigation with high
positioning accuracy is possible.
The numerous field experiments performed this year with retrofitted
ARGO platform have proven that the signature concept, recognition
algorithm, and gating controller work very well and provide an effective
mean for carrying out the gate navigation tasks. Figure 17.16 illustrates
the vehicle passing through the gate in an experimental field.
Fig. 17.16. A “gate through” procedure experimental verification
17.3 Conclusions
Navigation of autonomous ground vehicles through the gate and/or

around similar characteristic structures or the environment’s features
were proved to be difficult. The performed experiments, demonstrated
validity and usefulness of presented concepts for a single gate case.
More experiments are needed to verify the conditions for the multi-gate
case. The possible improvements of the recognition module would let
the navigation algorithm recognize more than one gate and then make
its decision according the requirement from the upper level control.
The requirement may consist of an entry into a particular gate or the

sequence of gates in any given sequence.
Some limitations related to the hardware (laser scanner) have been
encountered. Conditions for object recognition are difficult in some par-
ticular situations. Creating an environment map using multi-sensor in-
formation should be considered. In particular, multi-laser systems or a
laser scanner linked with radar sensors, sonar, or video cameras ought
to be considered. The system improvement with the multi-sensor and
sensor fusion procedure would make the recognizing procedure more
effective and the overall system more robust.
References
1. Sasiadek J.Z., Lu Y., Polotski V. “Navigation of Autonomous Mobile Ro-
bot with Gate Recognition and Crossing”, Proc. of the 8-th IFAC Sympo-
sium on Robot Control (SYROCO’2006), Bologna, Italy, 6-8 September
2006
2. Polotski V., Sasiadek J.Z., Lu Y., “Gate Recognition and Crossing for
Autonomous Security Robot”, Proc. of the ISR Robotik Conference, Mu-
nich, Germany, 15-18 May 2006.
3. Sasiadek J.Z., Wang Q., “Low cost automation using INS/GPS data fu-
sion for accurate positioning”, Robotica (2003) vol. 21, pp. 255-260, 2003.
4. Sasiadek J.Z., “Sensor Fusion”, Annual Reviews in Control, IFAC Jour-
nal, no. 26 (2002), pp. 203-228.
5. Thrun S., Burgard W., Fox D., Probabilistic Robotics, MIT Press, 2005.
6. Bakambu J.N. et al., “Heading-Aided Odometry and Range data Integra-
tion for Positioning of Autonomous Mining Vehicles”, Proc. of the IEEE
Int. Conf. on Control Applications, Anchorage AK, Sept. 2000.
7. Ye C., Borenstein J., “Characterization of a 2-D Laser Scanner for Mobile
Robot Obstacle Negotiation”, Proc. of the 2002 IEEE ICRA, Washington
DC, USA, 10-17 May 2002, pp. 2512-2518.
8. Vale A., Lucas J.M., Ribeiro M.I., “Feature Extraction and Selection for
Mobile Robot Navigation in Unstructured Environments”, Proc. of the
5th IFAC Symposium on Intelligent Autonomous Vehicles, Lisbon, July
2004.
9. An D., Wang H., “VPH: a new laser radar based obstacle avoidance
method for intelligent mobile robots”, Intelligent Control and Automa-
tion, 2004, Fifth World Congress, vol. 5, Hangzhou, China, June 2004
pp. 4681-4685.
10. Astolfi K., “Exponential Stabilization of a Wheeled Mobile Robot via Dis-
continuous Control”, J. of Dynamical Systems Measurements and Con-
trol, 1999, v. 121, pp. 121-125,.
11. Goel P., Roumeliotis P.I., Sukhatme G.S., “Robot Localization using Rel-
ative and Absolute Position Estimates” in Proc. IEEE Int. Conf. on Ro-
bots and Systems, 1999, Kyongju, Korea.
12. Pfister S.T., et al., “Weighted Range Sensor Matching Algorithms for
Mobile Robot Displacement Estimation”. In Proc. 2002 IEEE Int. Conf.
on Robotics and Automation, 2002, Washington DC
18
Kinematic Motion Patterns of Mobile
Manipulators∗
Katarzyna Zadarnowska and Krzysztof Tchoń
Institute of Computer Engineering, Control, and Robotics, Wroclaw

University of Technology, ul. Janiszewskiego 11/17, 50-372 Wroclaw, Poland
{kz, tchon}@ict.pwr.wroc.pl
18.1 Introduction
We address the problem of performance evaluation of mobile manipula-

tors. It seems that this area has not been explored systematically in the
literature; a list of representative references includes [6, 2, 7, 8, 1, 3].
In dealing with performance measures for mobile manipulators we
shall assume the endogenous configuration space approach. First ideas
on this subject were formulated in [4] and then extended toward kine-
matic [5], and dynamic [9] performance measures. In comparison to
traditional approaches the novelty offered by the endogenous configu-
ration space approach lies in connecting the performance of a mobile
manipulator with controllability of its control theoretic representation.
The contribution of this paper consists in providing a uniform frame-
work for the study of the kinematic and the dynamic dexterity of mo-
bile manipulators, endowing the endogenous configuration space with
a Riemannian metric, and incorporating a coordinate-free description
of the taskspace. On this background we shall define the concept of the
motion pattern of the mobile manipulator. The motion pattern will be
regarded as a pair comprising the platform and the end-effector trajec-
tories that correspond to the configuration of the maximum dexterity.
Tracking a motion pattern will result in the most efficient utilization
of the control actions by the mobile manipulator. In this paper our
attention will be focused on kinematic motion patterns. The dynamic
dexterities and motion patterns have been discussed in [9].
The composition of this paper is as follows. Section 18.2 gives an ex-
position of the main idea of our approach. In Section 18.3 we introduce
∗
This research has been supported by the Foundation for Polish Science.
springerlink.com
210 K. Zadarnowska and K. Tchoń
basic technical concepts, culminating in a definition of the motion pat-

tern of the mobile manipulator. An illustration of the motion patterns
is given in Section 18.4. Section 18.5 concludes the paper.
18.2 Main Idea
The endogenous configuration space approach relies upon a fundamen-

tal analogy between the kinematics of a stationary manipulator and
the input–output map of a control system. To be more specific, sup-
pose that an affine control system with outputs
ż = f (z) + g(z)u, y = h(z) (18.1)
represents the equations of motion of a mobile manipulator. We let

z ∈ Rn , u ∈ Rm , y ∈ Rr , and assume that the control function u(·) ∈ U .
For a fixed initial state z0 , the control function u(·) determines the state
trajectory z(t) = ϕz0 ,t (u(·)). Given a time horizon T , the end point map
y(T ) = Hz0 ,T (u(·)) = h(ϕz0 ,T (u(·)))
computes the output at T of system (18.1). A variation △u(·) of the

control results in the following variation of the system output:
d
△y(T ) = DHz0 ,T (u(·))△u(·) = |α=0 h(ϕz0 ,T (u(·) + α△u(·))).
dα
(18.2)
The derivative DHz0 ,T (u(·)) can be computed by means of the varia-
tional system
ζ̇ = A(t)ζ + B(t)△u(t), η = C(t)ζ, (18.3)

∂
associated with (18.1), where A(t) = ∂z (f (z(t)) + g(z(t))u(t)), B(t) =
∂
g(z(t)), C(t) = ∂z h(z(t)). Denoting by Φ(t, s) the transition matrix,
∂Φ(t,s)
∂t = A(t)Φ(t, s), Φ(s, s) = In , we obtain
Z T
DHz0,T (u(·))△u(·) = C(T ) Φ(T, t)B(t)△u(t)dt.
0
The image by DHz0 ,T (u(·)) of the unit sphere in U is an ellipsoid
Ez0 ,T (u(·)) = {η ∈ Rr | η T G−1

z0 ,T (u(·))η = 1}
18 Kinematic Motion Patterns of Mobile Manipulators 211
RT
in Rr , where Gz0 ,T (u(·)) = C(T ) 0 Φ(T, t)B(t)B T (t)ΦT (T, t)dtC T (T )
denotes the Gram matrix of the variational system (18.3). By the anal-
ogy between the Gram matrix and the manipulability matrix, the de-
terminant of the Gram matrix can be used as a performance measure
of the control system at a given control u(·). Depending on the physi-
cal meaning of state, input, and output variables, this control system
may represent either the kinematics (input=velocity) or the dynamics
(input=force) of the mobile manipulator. Within the endogenous con-
figuration space approach the control applied in the system (18.1) is
referred to as the endogenous configuration of the mobile manipulator,
whereas the performance measure based on the maximization of the
determinant of the Gram matrix is called the kinematic or dynamic
dexterity.
18.3 Kinematic Motion Patterns

The mobile manipulator will be meant as a robotic system composed
of a nonholonomic mobile platform and a stationary on-board manip-
ulator. Its kinematics is represented as a driftless control system with
outputs
Xm
q̇ = G(q)u = gi (q)ui , y = k(q, x), (18.4)
i=1
where q ∈ Rn denotes generalized
platform
coordinates, x ∈ Rp is a vec-
Rd
tor of joint positions, and y = ∈ SE(3) describes the location of
0 1
the end-effector. The group SE(3) endowed with a Riemannian metric
hY1 , Y2 i(y) = Y1T Q(y)Y2 , where Y1 , Y2 ∈ R6 denote a pair of twists and
Q(y) = QT (y) > 0 is a symmetric, positive definite matrix, will be
referred to as the taskspace of the mobile manipulator. The admissible
control functions u(·) ∈ L2m [0, T ] of the platform are assumed Lebesgue
square integrable over the time interval [0, T ]. The joint positions are re-
garded as controls that remain constant over [0, T ]. Pairs (u(·), x), con-
stitute the endogenous configuration space X = L2m [0, T ] × Rp . Given
a pair of matrix functions R(u(·), x, t), R(u(·), x, t) = RT (u(·), x, t) > 0,
and W (u(·), x) = W T (u(·), x) > 0, we endow the endogenous configu-
ration space with a Riemannian metric
h(△u1 (·), △x1 )(△u2 (·), △x2 )i(u(·), x) =

Z T
△uT1 (t)R(u(·), x, t)△u2 (t)dt + △xT1 W (u(·), x)△x2 .
0
The introduction of both Riemannian metrics will potentially make the

performance measures physically consistent and gauge-invariant.
Now, given an initial state q0 ∈ Rn of the platform and a control
u(·) ∈ L2m [0, T ] we compute the state trajectory q(t) = ϕq0 ,t (u(·)) and
the end point map

R(ϕq0 ,T (u(·)), x) d(ϕq0 ,T (u(·)), x)
y(T ) = Kq0 ,T (u(·), x) = (18.5)
0 1
of control system (18.4) that will be refereed to as the kinematics of the

mobile manipulator. The kinematics describes the end-effector location
at time T on condition that the platform initialized at q0 is driven by
u(·), and that the manipulator’s joints assume position x.
In accordance with the developments presented in Section 18.2 we
introduce the variational system
ξ̇ = A(t)ξ + B(t)△u(·) (18.6)

∂(G(q(t))u(t))
associated with the dynamics of (18.4), where A(t) = ∂q ,
B(t) = G(q(t)). The mobile manipulator Jacobian

6 vT
Jq0 ,T (u(·), x) : X → R , Jq0 ,T (u(·), x)(△u(·), △x) =
ωT
transforms the variation of the configuration into the linear and the
angular (space) velocity of the end-effector at T , i.e.
d
Jq0 ,T (u(·), x)(△u(·), △x) = |α=0 Kq0 ,T (u(·)+α△u(·), x+α△x) =
dα
Z T
Jqq0 (q(T ), x) Φ(T, t)B(t)△u(t)dt + Jqx0 (q(T ), x)△x, (18.7)
0
where
" #
∂d
∂q (q(T ), x)
Jqq0 (q(T ), x) = ∂R ∂R
] ∂q1
(q(T ), x)RT (q(T ), x)[, . . . , ] ∂qn
(q(T ), x)RT (q(T ), x)[
and
" #
∂d
∂x (q(T ), x)
Jqx0 (q(T ), x) = ∂R ∂R .
] ∂x 1
(q(T ), x)RT (q(T ), x)[, . . . , ] ∂x p
(q(T ), x)RT (q(T ), x)[
Above, the symbol ] · [ denotes the usual isomorphism of the Lie al-
gebra se(3) and R6 , and the matrix Φ(t, s) is the transition matrix of
(18.6). On the basis of the Jacobian (18.7) we compute the Jacobian

pseudoinverse
(Jq#0 ,T (u(·), x)η)(t) =

−1
R (u(·), x, t)B T (t)ΦT (T, t)JqqT
0 (q(T ), x)
Dq−1 (u(·), x)η.
W −1 (u(·), x)JqxT
0
(q(T ), x) 0 ,T
The kinematic dexterity matrix of the mobile manipulator appearing

above,
Dq0 ,T (u(·), x) = Jqq0 (q(T ), x)Gq0 ,T (u(·), x)JqqT

0
(q(T ), x)+
Jqx0 (q(T ), x)W −1 (u(·), x)JqxT
0
(q(T ), x), (18.8)
is the sum of the mobility matrix of the platform and the manipulability
matrix of the on-board manipulator. The former one contains the Gram
matrix
Z T
Gq0 ,T (u(·), x) = Φ(T, t)B(t)R−1 ((u(·), x, t))B T (t)ΦT (T, t)dt.
0
Using the formulas (18.7) and (18.8) we obtain the dexterity ellipsoid
Eq0 ,T (u(·), x) = {ζ ∈ R6 | ζ T Dq−1

0 ,T
(u(·), x)ζ = 1}, (18.9)
whose volume is a kinematic dexterity measure of the configuration

(u(·), x) q
dq0 ,T (u(·), x) = det Dq0 ,T (u(·), x). (18.10)
The dexterity (18.10) characterizes the quality of transmission of in-
finitesimal motions from the endogenous configuration space to the
taskspace at a given configuration. In the robotic terminology, non-
zero dexterity means regularity of the configuration. From the control
theory point of view, non-zero dexterity implies local output controlla-
bility of (18.4).
The endogeneous configuration (u∗ (·), x∗ ) corresponding to the max-
imal dexterity defines a pair of the platform and the end-effector tra-
jectories
kmp(t) = (q ∗ (t) = ϕq0 ,t (u∗ (·)), y ∗ (t) = k(q ∗ (t), x∗ )), (18.11)
defined for t ∈ [0, T ]. The pair (18.11) will be referred to as the kine-
matic motion pattern of the mobile manipulator. Tracking the motion
pattern provides the highest sensitivity of the end-effector response at
T to the variations of platform controls and joint positions of the on-

board manipulator. By definition, the motion pattern depends on the
initial configuration of the platform and on the control time horizon.
In practical computations it is also dependent on the assumed repre-
sentation of control functions driving the mobile platform. Generally,
the motion patterns are not unique and admit a kind of symmetry that
reflects certain geometrical symmetries of the mobile manipulator.
18.4 Examples
As an example, let us consider the unicycle-type mobile platform car-
rying on board a double pendulum, shown schematically in Fig. 18.1.
The kinematics of this mobile manipulator is described by a control
system
q̇1 = u1 cos q3 , q̇2 = u1 sin q3 , q̇3 = u2 , (18.12)
with the output function
 
q1 + (l1 cos x1 + l2 cos(x1 + x2 )) cos q3 + d cos q3
y = k(q, x) =  q2 + (l1 cos x1 + l2 cos(x1 + x2 )) sin q3 + d sin q3 
l0 + l1 sin x1 + l2 sin(x1 + x2 )
defining the Cartesian end-effector position with respect to an inertial

frame. In the system (18.12) we shall apply control functions in the
form
k
X
ui (t) = ci2j−1 sin jt + ci2j cos jt, i = 1, 2, (18.13)
j=0
x2
l2
l1
x1
l0
d
z (q ,q )
1 2
q3
x
Fig. 18.1. Double pendulum on a unicycle

with either k = 1 (first order harmonics) or k = 2 (second order har-

monics). The problem to be solved consists in finding platform controls
in the form (18.13) and joint positions x1 , x2 providing the maximum
dexterity (18.10) for fixed values of geometric parameters l1 = 2, l2 = 1,
d = 1, the initial posture q0 = (0, 0, 0) of the platform, the time horizon
Table 18.1. Configurations of maximum dexterity

Constraints Optimal configurations (c, x)
{c10 , c11 , c12 , c20 , c21 , c22 , x1 , x2 } =
i = 1, 2,
{3.00, −3.00, −3.00, ±0.22, ±1, 32, ±0.38, 0.00, 0.00}
k=1
dq0 ,T (c, x) = 993.55
{c10 , c11 , c12 , c13 , c14 , c20 , c21 , c22 , c23 , c24 , x1 , x3 } =
i = 1, 2, {3.00, −3.00, −3.00, −3.00, 3.00, ±0.31, ±1.40, ±0.17,
k=2 ∓0.23, ∓0.69, 0.00, 0.00}
dq0 ,T (c, x) = 1620.80
-6
y2
y2 -8 10
-10
3 5
y3 2 -15 -10 -5
y1
40
1
y3 4
2 20
0 -5 0
-2
-4
-40 0
y2
-15 -20
-14 -10 0 -20
y1
y1 20
40 -40
Fig. 18.2. Most dextrous pose, kmp(t), and dexterity ellipsoid for k = 1
2
y2 0 y2
10
-2
3 5
y3 2
-20 -15 -10 -5 5 y1 40
1
-5 y3 4
2
0
20
0 -2
-4
-22 -40 0
y2
-21 -10 -20
y1 -20 0 -20
y1 20
40 -40
Fig. 18.3. Most dextrous pose, kmp(t), and dexterity ellipsoid for k = 2
T = 2π, the identity matrices R(u(·), x, t), W (u(·), x), and Q(y), and
the configuration constraints |cij |≤ 3, x1 , x2 ∈ [0, 2π]. The optimal con-
figurations (c, x) have been collected in Table 18.1. The corresponding
kinematic motion patterns are shown in Figs. 18.2 and 18.3.
18.5 Conclusion
We have presented a control theoretic framework for the derivation of

performance measures for mobile manipulators focused on the analogy
between the manipulator kinematics and the end point map of and
between the manipulability matrix and the Gram matrix of a control
system. Special attention has been paid to the kinematic dexterity mea-
sure whose maximization yields kinematic motion patterns for the mo-
bile manipulator. Further research will be directed toward analysis and
classification of motion patterns, and the recognition of their usability
as motion primitives in motion planning of mobile manipulators.
References
1. B. Bayle, J.-Y. Fourquet, M. Renaud (2003) Manipulability of wheeled

mobile manipulators: application to motion generation. Int. J. Robot.
Res. 22(7–8):565–581.
2. G. Foulon, I.Y. Fourquet, M. Renaud (1997) On coordinated taskes for
nonholonomic mobile manipulators. Proc. 5th IFAC Symp. Robot Con-
trol, Nantes, France 491–498.
3. J.F. Gardner, S.A. Velinsky (2000) Kinematics of mobile manipulators
and implications for design. J. Robot. Syst. 17(6):309–320.
4. K. Tchoń, R. Muszyński (2000) Instantaneous kinematics and dexterity
of mobile manipulators. Proc. 2000 IEEE Int. Conf. Robot. Automat.,
San Francisco, CA 2493–2498.
5. K. Tchoń, K. Zadarnowska (2003) Kinematic dexterity of mobile manip-
ulators: an endogenous configuration space approach. Robotica 21:521–
530.
6. Y. Yamamoto, X. Yun (1994) Coordinating locomotion and manipulation
of a mobile manipulator. IEEE Trans. Automat. Contr. 39(6):1326–1332.
7. Y. Yamamoto, X. Yun (1999) Unified analysis of mobility and manipu-
lability of mobile manipulators. Proc. 1999 IEEE Int. Conf. Robot. Au-
tomat., Detroit, MI 1200–1206.
8. Y. Yamamoto, S. Fukuda (2002) Trajectory planning of multiple mobile

manipulators with collision avoidance capability. Proc. 2002 IEEE Int.
Conf. Robot. Automat., Washington, DC 3565–3570.
9. K. Zadarnowska, K. Tchoń (2006) A control theory framework for per-
formance evaluation of mobile manipulators. Submitted for publication.
19
Generalized Kinematic Control of Redundant
Manipulators∗
Miroslaw Galicki
Institute of Medical Statistics, Computer Science and Documentation,

Friedrich Schiller University Jena,
Bachstrasse 18, D–07740 Jena, Germany
and
Department of Mechanical Engineering, University of Zielona Góra,
Podgórna 50, 65–246 Zielona Góra, Poland
Miroslav.Galicki@mti.uni-jena.de
19.1 Introduction
Recently, an interest has increased in applying redundant manipulators

in useful practical tasks which are specified in terms of a time parame-
terized geometric path (a trajectory) to be tracked by the end-effector.
Redundant degrees of freedom make it possible to achieve some useful
objectives such as collision avoidance in the task space with obstacles,
joint limit avoidance and/or avoiding singular configurations when the
manipulator moves. An effective approach to the motion control prob-
lem for redundant robotic manipulators is the so-called kinematic
control. It is based on an inverse kinematic transformation which deter-
mines a reference joint (manipulator) trajectory corresponding to the
end-effector trajectory given in the task space. One may distinguish be-
tween several approaches in this context. Among them, we mainly con-
centrate on two major approaches. The first approach is the extended
or augmented task space formulation of the inverse kinematics prob-
lem presented in works [1]-[4]. It is based on extending the dimension of
the task space by incorporating as many additional constraints as the
degree of the redundancy. These additional constraints are obtained
based on e.g. various types of optimization criteria. Consequently, the
resulting system becomes non-redundant. Unfortunately, this approach
usually introduces additional algorithmic singularities related to rank
of the so-called extended Jacobian matrix, and hence can cause the
∗
This work was supported by the DFG Ga 652/1–1.
springerlink.com
220 M. Galicki
joint velocity to become unbounded even though the manipulator is not

in a singular configuration. Moreover, the dimensionality of the inverse
kinematics problem increases. The second approach, discussed in works
[5]-[8], is the generalized pseudo-inverse based control formulation that
uses the pseudo-inverse of the manipulator Jacobian. However, applica-
tion of pseudo-inverse techniques is computationally time consuming.
In order to tackle the singular configurations potentially met along the
end-effector trajectory, the use of a damped least-squares inverse of
the Jacobian matrix has been proposed in works [9]-[10], in lieu of the
pseudo-inverse. Nevertheless, this technique suffers from errors due to
long-term numerical integration drift.
This study considers the problem of kinematic control of a redun-
dant manipulator in real time so that it tracks the prescribed end effec-
tor trajectory and simultaneously fulfils an additional objective, that is
singularity avoidance. The Lyapunov stability theory is used to derive
the trajectory generator. This approach in contrast to others does not
require any pseudo-inverse nor the augmented task space. Instead, the
Jacobian matrix is suitably partitioned resulting in a reduction of the
inverse kinematics problem dimensionality. A new method based on
a concept of a singular neighbourhood for both singularity avoidance
and passing through the trajectory generator singularities is proposed.
On account of the fact that the trajectory generator proposed here
is implemented at the joint velocity level, a joint position controller
is assumed to be available which closely tracks any reference trajec-
tory provided by our motion generator. The remainder of the paper
is organized as follows. Section 19.2 formulates the inverse kinematics
problem and provides us with the solution based on the Lyapunov sta-
bility theory. The singularity avoidance technique is given in Section
19.3. Section 19.4 presents a computer example of solving the inverse
kinematics problem for a redundant manipulator consisting of three
revolute kinematic pairs, operating in two-dimensional task space. Fi-
nally, some concluding remarks are made in Section 19.5.
19.2 Kinematic Control of Redundant Manipulator

The aim is to follow by the end-effector a prescribed trajectory (given
in the task space) described by the following equations
p(q) − φ(t) = 0, (19.1)
where Rn ∋ q = (q1 , . . . , qn )T is the vector of generalized co-ordinates
of the manipulator (its configuration); n stands for the number of kine-
19 Generalized Kinematic Control of Redundant Manipulators 221
matic pairs of the V-th class; p : Rn −→ Rm denotes an m-dimensional

non-linear (with respect to vector q) mapping constructed from the
kinematic equations of the manipulator; p(q) = (p1 (q), . . . , pm (q))T ;
φ(t) = (φ1 (t), . . . , φm (t))T stands for a given trajectory to be tracked;
t ∈ [0, ∞). On account of the redundant manipulator considered herein,
the relation n > m holds. For purposes of on-line kinematic control,
task (19.1) is transformed into a differential form as follows:
Jq̇ = −λe + φ̇(t), (19.2)
where J = ∂p(q)/∂q is the (m × n) Jacobian matrix; λ denotes a scalar

positive coefficient; e = (e1 , . . . , em )T = p(q)−φ(t) stands for the error
of the end-effector trajectory tracking. In this section, we assume that J
is a full rank matrix during the trajectory tracking. A method to enforce
fulfilment of this assumption will be proposed in the next section. Since
J is (by assumption) a full rank matrix, that is, rank(J) = m, then there
exists a m × m nonsingular matrix JR constructed from m columns of
J. Let JF denote the m × (n − m) matrix obtained by excluding JR
from J. Hence, matrix J may be decomposed into two submatrices,
that is J = [JR JF ]. Using JR and JF , the robotic task (19.2) may be
rewritten as follows:
JR q̇R + JF q̇F = −λe + φ̇(t), (19.3)
where q̇R is the m-dimensional joint velocity vector corresponding to

the reduced matrix JR ; q̇F stands for the n − m-dimensional velocity
T
vector obtained by excluding q̇R from vector q̇; q̇ = q̇R q̇F . Thus,
vector q̇R may be determined from Eq. (19.3) and then put at the
velocity vector q̇. As a result, the following equation is obtained after
simple calculations:
R −1 F
(JR )−1 (J ) J
q̇ = −λe + φ̇ − q̇F , (19.4)
0(n−m)×m −In−m
where 0(n−m)×m is the (n − m) × m null matrix; In−m denotes the

(n − m) × (n − m) identity matrix. Vector q̇F may, in fact, be chosen
arbitrarily. Nevertheless, the conditions imposed on q̇F should take
into account the existence and uniqueness of the solution to differential
equations (19.4). Solution (19.4) does not require a pseudoinverse of
the Jacobian. Instead, the inverse of the square matrix JR is computed.
Applying the Lyapunov stability theory, we derive the following result.
222 M. Galicki
Lemma 1. If matrix JR is non-singular along the end-effector trajec-

tory φ, then solution q = q(t) to the differential equations (19.4) is
asymptotically (exponentially) stable.
Proof. Consider a Lyapunov function candidate
1
V = he, ei.
2
The time derivative of V is given by
V̇ = he, Jq̇i − he, φ̇i.
Substituting q̇ from the above dependence for the right-hand side of
Eq. (19.4), we obtain after simple calculations
R −1 F
(JR )−1 (J ) J
R
V̇ = e, [J J ] F −λe + φ̇ − q̇F −
0(n−m)×m −In−m
he, φ̇i = −λhe, ei.
This implies using the Lyapunov theorem [11] that e exponentially
tends to zero.
One way of choosing vector q̇F is based on the minimization of some
criterion W (q), where W (·) stands for a non-negative function repre-
senting e.g. collision avoidance conditions or a manipulability measure.
Minimizing this function along q(t) may be achieved by minimizing
its time derivative. Hence, we can formulate the following optimization
task. Minimize
∂W
Ẇ (q) = , q̇ −→ min . (19.5)
∂q q̇F
Taking into account the right-hand side of (19.4), the solution to (19.5)
(a minimum of the scalar product is achieved for linearly dependent
vectors) may be expressed in the form given below
R −1 F T
F (J ) J ∂W
q̇ = c , (19.6)
−In−m ∂q
where c is a positive coefficient. Combining (19.4) and (19.6), we obtain
the following generalized kinematic controller minimizing additionally
criterion W (·):
R −1 F R −1 F T
(JR )−1 (J ) J (J ) J ∂W
q̇ = −λe + φ̇ − c .
0(n−m)×m −In−m −In−m ∂q
(19.7)
As is seen, there are significant differences between controller (19.7) and

an extended Jacobian algorithm requiring n − m additional constraints
which together with kinematic task (19.1) result in a non-redundant
system. Consequently, such algorithm has the order of computational
complexity (assuming constant m and non-trivial constraints) of O(n3 ).
As is not difficult to see, the order of computational complexity of con-
troller (7) equals O(n). Let us note that our algorithm requires the
inverse of sub-Jacobian JR . Two procedures have been proposed to
tackle this problem. In order to escape from the singular neighborhood,
a suitable criterion W (q) (a singularity measure) has been proposed in
Section 19.4. On the other hand, if the avoidance of singular config-
uration during the trajectory tracking is not possible, we propose a
procedure in the next section which generates joint trajectory passing
through singular manifold.
19.3 Tackling the Problem of Manipulator Singularity
In this section, we shall discuss the case in which the determinant

of matrix JR tends to approach zero (matrix JR becomes singular).
For clarity of further consideration, the trajectory generator (19.7) is
rewritten in a compact form as follows:
q̇ = f (q, t), (19.8)
where f is the right-hand side of (19.7). Following the ideas proposed

in [12], a small neighbourhood (a singular neighbourhood) around the
singular manifold is defined, based on a user-specified small number.

To be precise, let U = {q : S(q) < ǫ}, where S(q) = det JR ; det(·)
stands for the determinant of (·); ǫ is a user-defined sufficiently small
number, define a singular neighbourhood around the singular mani-
fold S(q) = 0. Moreover, the hypersurface S(q) = 0 is assumed to
be non-degenerated, i.e. ∂S/∂q 6= 0. If a penetration into U is not
detected (S(q) ≥ ǫ), then the trajectory generated by Eq. (19.8) is ac-
complished and the manipulator performs a singularity-free motion. As
soon as configuration q enters the singular neighbourhood (S(q) < ǫ),
a perturbed velocity is generated whose magnitude is proportional to
the extent of penetration. The following simple form of velocity per-
turbation is proposed to avoid singular configurations (or pass through
the singular manifold) potentially met on the manipulator trajectory
generated by Eq. (19.8):
224 M. Galicki
q̇ = f v1 + ∂S
∂q v2 ,
β (19.9)
v˙i = −ci vi + ui , i = 1 : 2,
where vi stands for auxiliary scalar functions whose role is to continu-
ously perturb the manipulator velocity; ci are positive constant gains;
ui stands for controls which are chosen in such a way as to escape from
U; β ∈ (0, 1). Using the same derivation technique (omitted herein) as
that given in our recent work [12], we may find controls ui , i = 1 : 2,
which (locally) avoid singular configurations.
19.4 Computer Example

The aim of this section is to illustrate the performance of the proposed
kinematic controllers, given by (19.4) and (19.7). For this purpose, a
planar redundant manipulator is considered comprising three-revolute
kinematic pairs of the V-th class (n = 3), and operating in the two-
dimensional task space (m = 2). The link lengths are equal to l1 = 0.4,
l2 = 0.36, l3 = 0.3. The task of the robot is to transfer the end-effector
along the trajectory (a circle), expressed by the following equations:
φ1 (t) = 0.2 cos(5t), φ2 (t) = −0.6 + 0.2 sin(5t). (19.10)
The initial configuration q0 equals q0 = (−π/2, π/2, −π/2)T . Matrix
JR has been constructed from the first two columns of Jacobian J.
Hence, q̇F = q̇3 . The gain coefficient λ has taken the value λ = 15. In
order to evaluate the performance of the inverse kinematics algorithms
proposed in Section 19.3, two computer simulations have been carried
out. In the first simulation, the kinematic controller (19.4) with q̇3 = 0 is
used to track trajectory (19.10). For such a constraint, our manipulator
becomes non-redundant.
The results of the computer simulation in this case are presented in
Fig. 19.1, which indicates that exponentially stable end-effector tra-
jectory tracking is achieved. In the second simulation, the general-
ized kinematic controller (19.7) has been applied to track trajectory
(19.10) with the optimization criterion (singularity measure) equal to
W = c · (det(JR ) − ǫ)2 for det(JR ) < ǫ and W = 0 otherwise. The
constant coefficients c and ǫ take the following values: c = 10, ǫ = 0.7.
The results of the simulation are depicted in Fig. 19.2, which shows
faster and smoother convergence of the tracking errors as compared to
those obtained in the first example. A better performance of the second
controller is a consequence of maximizing the manipulability measure,
which makes robot task (19.10) simpler to accomplish.
0.2
e
1
e2
0.15
0.1
e1, e2 [m]
0.05
−0.05
−0.1
0 1 2 3 4 5
t [s]
Fig. 19.1. Tracking errors e – kinematic controller (19.4)

0.2
e
1
e2
0.15
0.1
e1, e2 [m]
0.05
−0.05
−0.1
0 1 2 3 4 5
t [s]
Fig. 19.2. Tracking errors e – kinematic controller (19.7)
19.5 Conclusions
This study has presented computationally simple kinematic controllers

for the trajectory tracking by the end-effector. The control generation
scheme has been derived using the Lyapunov stability theory. An ad-
vantage of the proposed control laws (19.4) and (19.7) is that they do
not require any pseudo-inverse technique nor the augmented task space.
Instead, the Jacobian matrix has been suitably decomposed resulting
in a reduction of the inverse kinematics problem dimensionality. The
control strategies (19.4), (19.7) are shown also to be exponentially sta-
ble (by fulfilment of practically reasonable assumptions). The proposed
kinematic controllers have been applied to a planar redundant manip-
ulator of three revolute kinematic pairs. Numerical simulations have
shown that the results obtained are in accordance with the theoreti-
cal analysis. The novelty of the strategy proposed lies in its simplicity
in design, program code, and real-time implementation. The approach
presented here may also be directly applicable at the joint acceleration
level.
226 M. Galicki
References
1. H. Seraji and R. Colbaugh (1990) Improved configuration control for

redundant robots, J. Robot. Syst., vol. 6, no. 6: 897-928
2. Galicki, M. (1991) A closed solution to the inverse kinematics of redun-
dant manipulators, Mech. Mach. Theory, vol. 26, no. 2: 221–226
3. Nenchev,D. (1992) Restricted Jacobian matrices of redundant manipula-
tors in constrained motion task, Int. J. Rob. Res., 11: 584–597
4. Perdereau, V., C. Passi, and M. Drouin (2002) Real-time control of re-
dundant robotic manipulators for mobile obstacle avoidance, 41: 41–59
5. Siciliano B. (1990) A closed-loop inverse kinematic scheme for on-line
joint-based robot control, Robotica, vol. 8: 231–243
6. Antonelli G., Chiaverini S. and G. Fusco (2000) Kinematic control of re-
dundant manipulators with on-line end-effector path tracking capability
under velocity and acceleration constraints, In: Prep. 6th IFAC Sympo-
sium on Robot Control (SYROCO’00)
7. Antonelli G., Chiaverini S. and G. Fusco (2000) An algorithm for on-line
inverse kinematics with path tracking capability under velocity and accel-
eration constraints, In: Proc. IEEE Conference on Decision and Control
8. Antonelli G., Chiaverini S. and G. Fusco, (2001) Real-time end-effector
path following for robot manipulators subject to velocity, acceleration,
and jerk joint limits. In: Proc. 2001 IEEE/ASME International Confer-
ence on Advanced Intelligent Mechatronics
9. Nakamura, Y. and H. Hanafusa (1986) Inverse kinematic solutions with
singularity robustness for robot manipulator control, J. Dyn. Syst. Mea-
surements and Control, 108: 163–171
10. Wampler, C. W. and L. J. Leifer (1988) Applications of damped least-
squares methods to resolved-rate and resolved-acceleration control of ma-
nipulators, J. Dyn. Syst. Measurements and Control, 110: 31–38
11. Krstic,M., I. Kanellakopoulos, P. Kokotovic (1995) Nonlinear and adap-
tive control design, J. Wiley and Sons, New York
12. Galicki, M. (2003) Inverse kinematics solution to mobile manipulators,
Int. J. Rob. Res., vol. 22, no. 12: 1041–1064
20
Parametric Representation of the Environment
of a Mobile Robot for Measurement Update in
a Particle Filter∗
Tahir Yaqub and Jayantha Katupitiya
ARC Centre of Excellence for Autonomous Systems, School of Mechanical

and Manufacturing Engineering, University of New South Wales, NSW 2052,
Sydney, Australia
{t.yaqub,j.katupitiya}@unsw.edu.au
20.1 Introduction and Related Work
A mobile robot must know its position and heading, all the time during
navigation. This is called localization. Recently particle filters [1] have
become extremely popular for position estimation. These are simple
to program, can process raw sensor data and can handle any proba-
bility distributions. A good tutorial on particle filters is [2]. Particle
filters update the pose of the robot by using a motion model and a
measurement model alternatively and recursively. The motion model
predicts a few possible positions of the robot (also called particles)
based on onboard sensors when a control action is taken and assigns
weight to each of these poses. The measurement model describes the
relationship between sensor measurements and the physical world and
is used to update the weights of particles. This measurement model is
usually represented as a conditional probability or likelihood. The two
important issues in using a distribution for measurement update are
making use of maximum information available and the computational
efficiency. The particle filters require a large number of particles in or-
der to accurately estimate the state. This negates their advantage in
real-time applications. Further discussion on computational complex-
ity can be found in [3]. The likelihood updates are the major cause
of computational inefficiency. The techniques which address this prob-
lem can be divided into online and off-line techniques. Verma [4] uses
∗
This work has been supported in part by the ARC Centre of Excellence pro-
gramme, funded by the Australian Research Council (ARC) and the New South
Wales State Government.
springerlink.com
228 T. Yaqub and J. Katupitiya
fewer particles to improve the computational efficiency while maintain-

ing the accuracy but does not provide data on computational burden.
Similarly Fox [5] and Kwok [6] propose a variable size of a sample
set to improve computational efficiency. Wu [7] uses an incremental
strategy to reduce the computational cost. They pre-compute the en-
vironment feature data to make Markov Localization more efficient in
real time. Some other methods are also described in [8]. Among these
are sensor subsampling, delayed motion update, selective updating [5],
[6], and model pre-caching [9] and [10]. In sensor subsampling, speed
is achieved by evaluation of the measurement model for a subset of
all ranges. The finer resolution is desirable in discrete representations
but this demands more computational and memory resources. A tech-
nique called ray casting is also used in beam models of range finders
as described in [8] but this is also very inefficient. An alternative is to
shift some of the computational load to off-line. These techniques are
sometimes called pre-caching techniques. In this paper we describe a
fundamentally different approach for capturing the underlying model
of the environment. We use a feature extraction algorithm [12] to rep-
resent the raw laser data in the form of a set of features. These features
are used for model development in as seen by the sensor fashion and no
coordinate transformations are used. We investigate the possibility of
representing the probability distribution for the measurement model in
a standard multinomial form. During the extraction of this multinomial
approximation, we assign cell probabilities to all the possible poses for
a given resolution. This model extraction is carried out before the start
of a navigation. This eliminates the sampling from posterior and ray
casting and is an efficient alternative to beam models used for range
finders. However, the complexity of the environment does not allow any
arbitrary feature definition to be useful enough for multinomial formu-
lation. We describe the use of a bootstrap technique for the selection of
appropriate features, which can provide a multinomial distribution in a
static indoor environment. The measurement model presented here can
update the particles even if they are uniformly distributed (which is a
worst-case scenario for localization) and hence can be used for global
localization. The added advantage is that the particles will not deplete
due to the standard parametric form. The initial work was presented
in [13] using simulation and we showed six times faster update rates
over ray casting. Now we have tested the model building method using
the real laser data and validated the results with true device in both
simulated and real environments.
20 Parametric Representation of the Environment of a Mobile Robot 229
20.2 Preliminaries: Particle Filter

In a particle filter, the initial belief for the state of a robot is taken
as uniform. Then at each time step, we update this belief from the
previous time step using current observation. This belief is represented
by a set of samples called particles,
p(zt |xt , map)p(xt |ut , xt−1 , map)
p(xt |zt , map) = ,
Nc
where Nc is the normalizing constant given by:
Xm
Nc = p(zt |xt , map)[k] p(xt |ut , xt−1 , map)[k] .
k=1
Here [k] is not a power but a superscript used for poses m obtained from
motion model (also see section 20.4.2 and [14]) and p(zt |xt , map) is the
measurement model while p(xt |ut , xt−1 , map) is the motion model. In
short, whenever a control action ut is taken, the state of the mobile
robot changes according to a state transition or motion model. Particle
filters use sampling motion models, in which we get a set of poses with
associated probabilities. A motion model gives the predictions based on
the internal sensors such as wheel encoders and more precisely it also
uses a map to eliminate the implausible predictions such as a robot
passing through a wall. Assume that the motion model gives us m
[1] [m]
possible poses (particles) denoted by xt to xt with their associated
[1] [m]
weights wt to wt , where t denotes time:
[1] [m] [1] [m]
p(xt |ut , xt−1 , map) → xt , . . . , xt : wt , . . . , wt .
Measurement model updates weights using the observation and the
map. The likelihood of measurement zt given the robot is at xt is
p(zt |xt , map).
Now the question is how to get p(zt |xt , map). This has been described
in our previous work [13]. Just to revise it briefly, we get the multino-
mial approximation of the feature observations in the environment by
obtaining both of the parameters required for a multinomial distribu-
tion. These parameters are the total features N in an observation and
the cell probabilities of individual features for n locations, assuming
that n covers the whole environment for a particular resolution. With
these two, we can update the weights of particles. But another prob-
lem is that the complexity of the environment does not allow us to use
any arbitrary feature definitions for a multinomial formulation. This
problem is investigated first in the next section.
20.3 Feature Selection
Extracting features from raw laser data is a common first-step trade-

off to save resources at the cost of loosing some of the information.
Although the most basic features are straight lines and corners, since
our model extraction process is carried out off-line, we can start with
as many features as we like without computational considerations. Fur-
ther if we say that the feature which we observe at a particular location
comes from an underlying multinomial distribution with parameters N
and p, then this N should remain the same, no matter where the robot
is, because in the multinomial distribution, N is constant. However, in
a real situation, the sum of all the realization of any T features does
not remain constant. But if we can get a near normal distribution for
any τ features having aPreasonably small variance (where τ < T ), then
we can say that N ≈ τi=1 zi , where zi is the mean of the ith feature
within a reasonable variance in the environment. Then for a measure-
ment update we would only normalize the newly arrived measurement
to this N value and would be able to update the measurement.
20.3.1 Definition of Features and Other Parameters

Let us say initially we define the following 6 features to be detected in
each laser scan (i.e., T =6):
• Number of walls n (lines of length greater than a selected threshold)
• Number of corners c
• Average distance between corners located at the end of the same line in any unit
(represented as a number) µc
• Average length of all the lines detected in a scan in any unit (represented as a
number) µl
• Slope of the largest line (since the largest line would most probably be a wall
and therefore its slope could be an important feature) s
• Length of the largest line in any unit (represented as number) L
These definitions are quite arbitrary and we try to find τ features feasi-
ble for multinomial formulation, where τ < T . It should be noted that
the features would be treated as numbers, and the units are included in
the definition of features. If later we find that a feature cannot be used,
then we have two options, either to tweak the definition or drop this
feature. To check the suitability, we define the following parameters for
all T features and determine their values:
• Mean before bootstrap M1
• Mean after bootstrap M2
• Mean to standard deviation of feature vector η1
• Ratio of mean to its standard deviation after bootstrap η2
• Suitability check Υ (This determines whether a feature can be used for multino-
mial formulation or not. Obviously Υ can have any of the two values: Y for yes
or N for no.)
• Mean rounded to the nearest integer for multinomial formulation M (applicable
only if Υ is Y, otherwise N/A.)
20.3.2 Data Collection
Data collection was carried out by placing the laser scanner at a number
of locations. The x, y coordinates of these locations were measured
accurately. At each location the scanner was rotated in steps of 7.5o .
These angular measurements were accurately carried out. The set up is
shown in Fig. 20.4. Then we make a table, where each column represents
the realization of one feature at all poses of the robot. It is important
to note that the real data collection at all the poses is not feasible; for
this we can use a two-step combined real and simulated approach. In
the first step, we can collect the data at some known locations covering
the whole environment, which can take 2 to 3 hours. In the second step,
we can store the data in a file and use an algorithm to build a map from
this localized data. This map is then used in a simulation software like
Stage and a model can be extracted. This approach would be useful in
many cases and we have tested it but here the real and simulated results
are reported separately. Features having less correlation are more useful
but this is not a requirement for our formulation.
20.3.3 Statistical Analysis and Bootstrap Feature Selection
We analyze the data for simple statistics, such as mean and standard
deviation. Histograms of all the six feature vectors are also plotted.
The following table shows the parameters which we measure for each
of the six features defined above.
Table 20.1. Statistical data table

P aram n c µc µl s L
M1 3.53 1.67 10.80 1528 466.1 2682.7
η1 1.63 0.96 0.66 1.65 0.041 2.07
M2 3.53 1.67 10.78 1527.6 451.9 2682.4
η2 51.49 29.63 20.33 52.11 1.28 64.40
Υ Y Y Y Y N Y
M 4 2 11 15 N/A 27
Fig. 20.1. First row from left to right: Histogram of the features n, c, µc , µl , s and
L extracted in an obstacle free SIMULATED environment without any statistical
treatment. Second row shows the histograms of the bootstrap means of these feature
vectors in the same order. Values are not shown for simplicity.
By looking at Fig. 20.1 and Fig. 20.2 we notice that the bootstrap
means of 5 out of 6 feature vectors are nearly normally distributed
while for vector s, it is not the case. The bootstrap was first intro-
duced by Efron [16] and then used by many researchers and is a newly
developed technique for making certain types of statistical inferences.
If the bootstrap distribution is roughly normal, then standard normal
and percentile intervals will nearly agree. We can use the percentiles of
the bootstrap to define the confidence limits. We therefore discard s as
not suitable for multinomial formulation. There are many parameters
Fig. 20.2. First row from left to right: Histogram of the features n, c, µc , µl , s and
L extracted in a REAL laboratory environment without any statistical treatment.
Second row shows the histograms of the bootstrap means of these feature vectors in
the same order.
which we can tweak and as a result can get different resulting number
of features. For example if we change the length of a straight line, be-
yond which we shall include it in a wall, the number of walls detected
at any particular location will change. Similarly, threshold on lengths,
threshold on counters, using different units for thresholds of different
features, different voting schemes, removing highly correlated features
and even changes in configuration of the laser scanner can be used for
tweaking if desired.
20.4 Multinomial Formulation
20.4.1 Extracting the Multinomial Parameters
We need two parameters; one is the total number of features N and the
other is the cell probabilities. Let us say that our resolution provides
n possible poses and at each pose we observe τ features; then our cell
probability matrix would be n × τ and the value in each cell would be
pij (1 6 i 6 n and 1 6 j 6 τ ).
• Place the robot in all n poses which depends on the resolution used and find
numbers of each of the τ features. Let us say that Fij (1 6 i 6 n and 1 6 j 6 τ )
denotes the number of features of jth type detected at ith pose.
• Find the sum of all the observed features at each pose and denote it with Ni ,
where 1 6 i 6 n.
• Find the ratio Ri = N/Ni for each of the n poses.
• Multiply Fij (1 6 i 6 n and 1 6 j 6 τ ) by Ri and get the normalized number of
features at each pose denoted by Fîj (1 6 i 6 n and 1 6 j 6 τ ).
• Get pij = Fîj /Ni (1 6 i 6 n and 1 6 j 6 τ ).
20.4.2 Measurement Update
Once we have the estimates of cell probabilities pij for all poses in an en-
vironment (for a selected resolution), the measurement update process
becomes just a table lookup problem, which is much faster than the
sampling from posterior. Therefore we achieve the computational effi-
ciency in twofold manner, first by extracting only τ number of features
from raw laser scans and then by making a table of cell probabilities
and changing the sampling f rom posterior and online ray casting op-
erations into a table lookup operation. Each probability vector would
be a row corresponding to each of the m poses provided by the motion
model. We use a superscript k for these poses. Then by using probabil-
ities pj [k] = pkj (∀ k = 1, 2, .., m and ∀ j = 1, 2, .., τ ) and N values, we
can update the weights at any stage of the particle filter. If our current
′
observation vector is Z = (z1 , z2 , ....., zτ ) , then the updated weights
Wk to these m particles are assigned as follows:
τ [k] zj
Y pj
Wk = N ! k = 1, 2...m (20.1)
zj !
j=1
20.5 Experiments and Results
20.5.1 Setup and Scenarios for Data Collection
The main advantage of simulation is that it provides the truth de-

vice and we can get actual robot positions. However while doing the
same experiment in our laboratory, we got the truth device by using a
setup on the floor as described in above section. The resolution used in
(x, y, θ) was (20 cm, 20 cm, 7.5◦ ) in both cases. This must be equal or
higher than the resolution used for the motion model. The experiment
wascarried out in three different scenarios:
• in an obstacle-free simulated environment, Fig. 20.3 (a) and (b);
• in a simulated environment having obstacles, Fig. 20.3 (c) and (d);
• in a real laboratory environment with many objects of various sizes by using the
setup shown in Fig. 20.4.
(a) (b) (c) (d)

Fig. 20.3. (a) Simulated Pioneer Robot equipped with wheel encoders and a laser
scanner follows a single motion command in an obstacle-free environment. (b) The
poses from the motion model are shown in green and the actual position which
is unknown is shown in red. (c) Environment having obstacles. (d) Motion model
predictions for case (c).
20.5.2 Model Extraction and Results Updates
Table 20.2 shows the motion model predictions obtained in three sce-
narios shown in Fig. 20.3 (a) (simulated obstacle-free environment de-
noted here by SOF), Fig. 20.3 (c) (simulation with obstacles denoted
by SWO) and Fig. 20.4 (laboratory environment denoted by L).
In the worst-case scenario, if the robot starts the motion from a
shutdown state or the batteries have been replaced, there is no encoder
information and the motion model will assign equal weights to all pos-
sible poses. Since we are using only 5 for demonstration purpose, they
will have weights of 15 and we omitted the weights column in the left
tabular of Table 20.2.
(a) (b)
Fig. 20.4. (a) Setup for obtaining data for modelling. (b) Autonomous wheelchair
with a laser scanner. The wheelchair was not used during modelling due to the
difficulty in true position measurements.
Table 20.2. The table on left shows predictions from the motion model and the
true pose. The table on right shows cell probabilities found during model building
using real laser data (case L) and the weights obtained by putting values in 20.1.
Particles Cell Probabilities Weights
SOF SWO L
p1 p2 p3 p4 p5
[1]
xt (-1.4,-0.3,0.035) (-0.38,2.1,4.36) (1,0.8,1.152 ) [1]
[2] xt 0.21 0.2 0.08 0.08 0.43 0.0072
xt (-1.6,0.1,0.436) (-0.35,1.4,4.01) (0.8,0.8,0.576 ) [2]
[3]
xt 0.25 0.17 0.1 0.07 0.41 0.0135
xt (-2.5,-0.2,0.262) (0.23,1.5,4.18) (0.6,0.8,1.745) [3]
[4]
xt 0.28 0.23 0.1 0.09 0.3 0.9583
xt (-2.3,0.1,0.175) (-0.1,1.6,4.53) (1,1,0.35) [4]
[5]
xt 0.18 0.11 0.21 0.11 0.38 0.00018
xt (-1.8,0.2,0.401) (0.2,2.2,4.45) (0.6,0.6,0.698) [5]
xt 0.22 0.15 0.9 0.05 0.49 0.0029
True
Pose (-1.7,-0.1,0.35) (-0.1,1.8,4.57) (0.8,0.8,0.785) Curr.
Observ. 4 4 2 2 9 -
The right tabular of Table 20.2 shows the observation (bottom line)
and the updated weights. It is obvious that due to the standard for-
mulation, we can assign weights at any time to any number of poses
and we do not need any resampling because we have the pre-cached
cell probabilities. We hope that this method may also be able to ad-
dress the degeneracy issue or the problem of deterioration of particles,
where after a few iterations, the weight starts to concentrate on single
particle and the filter becomes vulnerable to breakage. However this
degeneracy issue is yet to be studied in detail.
20.6 Conclusion and Future Work

In this paper we have described in detail a fundamentally different
and novel approach for extracting the model of the environment of
a mobile robot. It provides the advantage of using one of the stan-
dard multinomial probability distributions for the likelihood function
in the measurement update stage of a particle filter. This eliminates

sampling from posterior and ray casting and is an alternative to beam
models used for range finders. In beam models, the conditional density
p(zt |xt , map) is a mixture of many different densities, each of which cor-
responds to a particular type of error. The proposed method makes the
measurement model simple and efficient. Most of the modelling could
be done off-line. Now we are trying to implement it on a wheelchair.
References
1. N.J. Gordon, D.J. Salmond, A.F.M. Smith. Novel approach to
nonlinear/non-gaussian bayesian state estimation. Radar and Signal
Processing, IEE Proceedings F, 140(2):107-113, 1993. 0956-375X.
2. S. Arulampalam, S. Maskell, N. Gordon, T. Clapp. A tutorial on particle
filters for on-line non-linear/non-gaussian bayesian tracking. IEEE Trans.
on Signal Processing, 50(2):174-188, Feb 2002.
3. F. Daum, J. Huang. Mysterious computational complexity of particle
filters. Proc. of SPIE Int. Soc. for Optical Eng., 4728:418-426, 2002.
4. V. Verma, G. Gordon, R. Simmons, S. Thrun. Partcle filters for rover
fault diagnosis. Robotics and Automation Magazine, June 2004.
5. D. Fox. Adapting the sample size in particle filters through kld-sampling.
Int. J. of Robotics Research, 22(12):985-1003, 2003.
6. C. Kwok, D. Fox, M. Meila. Adaptive real-time particle filters for robot
localization. In: Proc. IEEE Int. Conf. on Robotics and Automation,
vol. 2, pp. 2836-2841, Taipei, Taiwan, 2003.
7. Q. Wu, D.A. Bell, Z. Chen, S. Yan, Xi Huang, H. Wu. Rough computa-
tional methods on reducing costs of computation in Markov localization
for mobile robots. Proc. of the World Congress on Intelligent Control and
Automation (WCICA), 2:1226-1230, 2002.
8. S. Thrun, W. Burgard, D. Fox. Probabilistic Robotics. MIT Press, 2005.
9. Y. Zhang, M. Schervish, H. Choset. Probabilistic hierarchical spatial
model for mine locations and its application in robotic landmine search.
IEEE Int. Conf. on Intelligent Robots and Systems, 1:681-689, 2002.
10. R. Talluri, J.K. Aggarwal. Mobile robot self-location using model-image
feature correspondence. IEEE Trans. on Robotics and Automation,
12(1):63-77, 1996.
11. S. Thrun, D. Fox, W. Burgard, F. Dellaert. Robust Monte Carlo local-
ization for mobile robots. Artificial Intelligence, 128(1-2):99–141, 2001.
12. T. Yaqub, J. Katupitiya. Modelling the environment of a mobile robot
using feature-based principal component analysis. In: IEEE Int. Conf.
CIS-RAM, Bangkok, Thailand, June 2006.
13. T. Yaqub, R. Eaton, J. Katupitiya. Development of a parametric model
for the environment of a mobile robot. In IEEE/RSJ Int. Conf. on Intel-
ligent Robots and Systems, Beijing, China, 2006.
14. T. Yaqub, M.J. Tordon, J. Katupitiya. A procedure to make the proba-

bilistic odometry motion model of an autonomous wheelchair. In IEEE
Int. Conf. on Mechatronics and Automation, Luayang Henan,, 2006.
15. B.P. Gerkey, R.T. Vaughan, A. Howard. The player/stage project: Tools
for multi-robot and distributed sensor systems. In: Proc. of the Int.
Conf. on Advanced Robotics (ICAR 2003), pp. 317-323, Coimbra, Por-
tugal, 2003.
16. B. Efron, R. Tibshirani. An Introduction to the Bootstrap. Chapman &
Hall/CRC, 1994.
21
Simulation of a Mobile Robot with an LRF in
a 2D Environment and Map Building
Luka Teslić, Gregor Klančar, and Igor Škrjanc
Faculty of Electrical Engineering, University of Ljubljana, Tržaška 25,

1000 Ljubljana, Slovenia
{luka.teslic,gregor.klancar,igor.skrjanc}@fe.uni-lj.si
21.1 Introduction
This article deals with the modelling and simulation of a mobile robot
with a laser range finder in a 2D environment and map building. The
simulator is built in the Matlab Simulink environment, thereby taking
advantage of the powerful Matlab toolboxes for developing mapping,
localization, SLAM, and navigation algorithms. A map-building algo-
rithm is developed and tested in a simulation. The line segments, ex-
tracted from the LRF’s output in each scan, are made up of polylines,
which are merged with the existing global map to form a new global
map. The global map of the environment is represented by unions of
line segments, where each union represents an object in the environ-
ment. Map building, localization and navigation are important issues
in mobile robotics. To develop and test algorithms for executing tasks
of this kind, it is useful to have a simulator of a mobile robot equipped
with sensors in a static environment. Since a Laser Range Finder (LRF)
is often used as the basic interaction between the robot and the envi-
ronment, the represented mobile robot model also includes a model of
the LRF. The problem of robotic mapping and localization has been
widely studied. A robot has to know its own pose (localization problem)
in order to build a map, and it also needs to know the environment map
(mapping problem) to localize itself to its current pose. The problems of
mapping and localization can be handled separately if the robot’s pose
is given to the robot by a human or from using GPS and INU sensors
(outdoor environments) when map building. The map of the environ-
ment can then be used to solve the localization problem. To avoid the
known robot’s pose assumption, a SLAM (Simultaneous Localization
and Mapping) algorithm must be built, where the problems of localiza-
springerlink.com
240 L. Teslić, G. Klančar, and I. Škrjanc
tion and mapping are merged. The robot can localize itself by odometric
measurements and by comparing the local map, obtained from the cur-
rent view of the robot, with an already built global environment map.
In [1] a comprehensive survey of the SLAM problem is addressed. In the
literature different approaches to map building have been proposed. A
topological map [3] is composed of the nodes representing topological
locations and the edges between the nodes. These nodes contain infor-
mation about the way to reach a connected topological location. In [3]
the metric and topological paradigm are integrated into a hybrid sys-
tem for map building. A global topological map connects local metric
maps, allowing a compact environment model, which does not require
global metric consistency and provides both precision and robustness.
The metric approach builds a map with occupancy grids or with sim-
ple geometrical features (e.g., line segments). Occupancy grids require
a huge amount of computer memory and are therefore not appropriate
when modelling a large environment [4]. In this paper we choose line
segments for the environment model as they require a smaller amount
of computer memory. In [5] a comparison of line-extraction algorithms
using a 2D laser rangefinder is reported. In [6] the environment is repre-
sented by polygonal curves (polylines), possibly containing rich shape
information for matching environment scans. However, some environ-
ment objects cannot be represented by one polyline (consecutive line
segments). Therefore, each environment object is represented by the
union of inconsecutive line segments in this paper.
21.2 Simulator
The main reason to develop a new simulator instead of using one of

the many already available is to study navigation, localization, and
mapping algorithms in the Matlab Simulink environment. Matlab and
its tool-boxes (e.g., Fuzzy Logic, Control, etc.) represent a very powerful
tool for developing all kinds of algorithms. The simulator includes the
models of a mobile robot (Fig.21.1(a)), a laser range finder, and the
environment. The purpose of the modelling is to create a simulation
model where different algorithms for mapping can be tested. We assume
a two-dimensional environment and that the robot knows its own pose
p(t) = [xrob (t), yrob (t), ϕrob (t)] (21.1)
at time t, in a global frame of reference (Fig.21.1(a)). The denotation

for time, t, will be subsequently abandoned for simplicity.
21 Simulation of a Mobile Robot with an LRF in 2D 241
21.2.1 Robot Model
The kinematic model of the robot is given by the following equations:
ẋrob = v cos ϕrob , ẏrob = v sin ϕrob , ϕ̇rob = ω, (21.2)
where the input v denotes the tangential speed, input ω denotes the
angular speed, (xrob , yrob ) denotes the position of the robot in global
coordinates (xG , yG ), and ϕrob denotes the orientation of the robot ac-
cording to the global coordinate axis, xG . The continuous model (Eq.
(21.2)) is implemented in Matlab Simulink with a simulation scheme us-
ing the ode45 integration method. For simulation purposes it is enough
to control the robot with the inputs v and ω. The pose (Eq. (21.2))
is the input in the S-function Animation for simulating the environ-
ment model and the LRF model and the input in the S-function for
the map-building algorithm.
21.2.2 Environment Model
The two-dimensional model of the environment can be built with line

segments. The line segment is defined with two points on the line and
the normal line equation
xG cos αj + yG sin αj − pj = 0, (21.3)
where the parameter pj denotes the distance of the line from the origin,
parameter αj ∈ (π−, π] denotes the direction of the line normal passing
through the origin, and xG , yG are the global coordinates of the points
lying on the line.
21.2.3 Laser Range-Finder Model
The laser range finder in each time step gives the set of distances ds =
[ds0◦ , . . . , ds180◦ ] to obstacles (e.g., a wall) at angles θ s = [0◦ , . . . , 180◦ ].
We simulate a reflection point by calculating the intersection points
(xip (i, j), yip (i, j)) between the i-th laser beam line (Fig. 21.1(b)) and
all (j = 1, . . . , N ) the lines describing the environment line segments
with determinants and calculate distances and angles
q
d(i, j) = (xip (i, j) − xrob )2 + (yip (i, j) − yrob )2 ,
(21.4)
θ(i, j) = atan2(yip (i, j) − yrob , xip (i, j) − xrob ) − (ϕrob − 90◦ ).
If there is no intersection between the lines, this is labelled with
yG yG a
laser beam Environment
line at v line y (xip(i,j) ,yip(i,j))
angle q(i) q(i) yR R
jrob d
Environment
(xrob ,yrob) (xrob ,yrob) xR line segment
w xR
Laser beam
line
zG xG zG xG
Fig. 21.1. (a) Coordinates of the robot according to the global coordinates.
(b) Intersection point between the laser beam line and the environment line .
d(i, j) = D; D > dmax , (21.5)

where dmax denotes the maximum range of the LRF (e.g., 80 m). In the
case of concurring lines the nearest point of the environment line seg-
ment to the robot (xrob , yrob ) is chosen as the reflecting point. Because
the intersections between the lines are calculated, the intersections be-
hind the robot and the intersections in front of the robot, which do
not lie on the environment line segments, must be eliminated (labeled
with Eq. (21.5)). Furthermore, we choose a point with the minimum
distance from the robot d(i) = min(d(i, 1), . . . , d(i, N )) as the reflection
point. So, if there is no intersection point between the i-th laser beam
and all the environment lines, the distances d(i, :) take the value D and
d(i) = D. If there are more environment lines in front of the robot, the
nearest intersection point is chosen as the reflecting point of the laser
beam. We define the vectors (Fig. 21.1(b)) a = (cos ϕrob , sin ϕrob , 0)
and d = (xip (i, j) − xrob , yip (i, j) − yrob , 0). If the dot product a · d < 0,
the intersection point lies behind the robot and it is eliminated. If θ(i)
equals 0◦ (180◦ ) and there is an environment line on the left (right)
side of the robot, an intersection point between the lines, which does
not appear with the real LRF, is eliminated. This happens when
c = a × d = (0, 0, c3 ); c3 > 0 (c3 < 0), (21.6)
where the operator × denotes the cross product. We assume the LRF
noise model using
d(i)
dnoise (i) = d(i) + N (0, σ), (21.7)
dmax
where N (0, σ) denotes the normal distributed noise with zero mean
and σ variance. If the distance d(i) is large, the influence of the noise is
proportionally larger. In this way we can test the robustness of mapping
algorithms without knowing the real noise distribution.
21.3 Mapping Algorithm

Assuming that the robot knows its own pose (Eq. 21.1)) the calculated
reflection point (xG (i), yG (i)) according to the global coordinates is
xG (i) = xrob + d(i) cos θG (i), yG (i) = yrob + d(i) sin θG (i),
(21.8)
θG (i) = (ϕrob − 90◦ ) + θs (i), i = 1, . . . , n,
where θG (i) denotes the laser-beam angle according to the global co-
ordinate frame (Fig.21.1(a)). All consecutive points (21.8) by which a
reflection has occurred (d(i) 6 D) are clustered, other points (d(i) > D)
are ignored. If there is only one point in a cluster, it is also ignored.
Each cluster is then split into more clusters if the distance between
two following points is greater than the threshold. Finally, every clus-
ter is reduced in the set of consecutive line segments or polylines using
the split-and-merge algorithm (it is fast and has good accuracy) [5] and
least-squares line fitting. The line segments are defined with edge points
and line parameters pk (the distance of the line from the origin) and
αk ∈ (π−, π] (direction of the line normal passing through the origin).
21.3.1 Integrating the Global Map with the Local Map

Each local map S (Fig.21.2) represents a set of polylines (S1, S2), and
each polyline is composed of consecutive line segments described with
line parameters and edge points. The global map G (Fig. 21.2, right)
is composed of the unions (G1, G2, and G3) of line segments, which
represent objects in the environment. The local map S is united with
the previously built global map Gold to get a new global map GN EW =
S ∪ GOLD .
GNEW= S U GOLD= (S1,S2) U (G1,G2) G=(G1,G2,G3) G= (G1,G2,G3)

= (G1,G2,S1,S2)
G2 G2 G2
G1 S2 G1
S1 G1NEW=
G1OLD U S2 G3
G3=S1
Fig. 21.2. Integrating the global map G =(G1, G2) with the local map
S =(S1, S2)
When a robot makes its second local map S, GOLD is equal to the lo-
cal map SF IRST , obtained in the first environment scan. When uniting
S and GOLD each line segment of the set (GOLD , S) is compared to each
line segment in one loop. We define the conditions for merging the line
segments L1 and L2 (Fig. 21.3), which correspond to the same environ-
ment line segment for the thresholds T and R, where l(Li) denotes the
length of line segment Li. If at least two of conditions ci (i = 1, 2, 3, 4)
are satisfied and if all conditions ci (i = 5, 6, 7, 8) are satisfied, two line
segments are merged. If the conditions c1 and c2 or the conditions c3
and c4 are satisfied, two line segments are also merged. When merg-
ing two line segments, new line-segment parameters are computed by
uniting the edge points of both segments, as indicated in [7]. If the
merged line segments belong to different unions, the unions are merged
(Fig.21.2, G1N EW = G1OLD ∪ S2). The loop of comparison is then re-
peated. If the conditions for merging any two segments are not fulfilled
in the next loop, the loop is stopped.
c1 : a + b < l(L1) + T, c2 : c + d < l(L1) + T,

L1 c
d a c3 : a + c < l(L2) + T, c4 : b + d < l(L2) + T,
r2 r3 r1
r4 b R
c5 : r1 < R, c6 : r2 < R,
L2
c7 : r3 < R, c8 : r4 < R,
Fig. 21.3. The conditions for merging the line segments L1 and L2
In [2] the SLAM algorithm for a line-based environment representa-

tion is described. The global environment map is composed of the set of
the global environment lines (e.g., 1000) and the local environment map
is also composed of a set (e.g., 10) of local environment lines. For local-
ization purposes the line segments of the global map that correspond to
the same environment line segments (e.g., a wall) as the line segments of
the local map, must be found. The line parameters of the local (robot’s
coordinates) map (αLi , pLi ) are recomputed to global coordinates ac-
cording to the approximately known robot pose [2] (the prediction step
of the Extended Kalman Filter). In general, parameters (αGk , pGk ) of
each global line must be compared to recomputed parameters (α′Li , p′Li )
of each local line to find the corresponding pair of lines. In large envi-
ronments this can take a huge number of comparisons (e.g., 1000 × 10).
If the sum of the squared differences (αGk − α′Li )2 + (pGk − p′Li )2 is
below a threshold, the lines can be chosen as a corresponding pair. In
our mapping approach the map is composed of unions of lines, where
each union corresponds to an environment object. It is very likely that
objects seen by the robot in the previous environment scan are also
seen in the current environment scan. There could be many line seg-
ments that are seen in the current environment scan that correspond to
objects (unions Gi) seen in the previous environment scan. The search
strategy can be faster if the line pairs for the comparisons are first
found among all the global lines that correspond to the environment
objects seen in the previous environment scan and all the local lines
(recomputed parameters) of the current scan.
21.4 Results
Fig. 21.4(a) shows a simulated LRF, a mobile robot, and a 2D envi-

ronment. Figures 21.4(b), (c), and (d) show experiments at different
values of LRF noise variance σ (0.07 m, 0.42 m and 0.63 m) and a fixed
value dmax = 14 m (Eq. (21.7)), where the global environment map G
is built. The environment scans are taken at poses p1, p2, and p3. The
experiments show that our mapping algorithm is robust, even with a
lot of noise from the LRF. Our mapping approach builds a global map
with many sets (unions) of line segments, which correspond to the en-
vironment objects. Compared to occupancy grids [4] the environment
description with the line segments requires a much smaller amount of
computer memory. In [6] the environment is represented by polygonal
Fig. 21.4. (a) Reflecting points of the LRF in a 2D environment. (b), (c),
and (d) Experiment of building map G = (G1, G2, G3) at dmax = 14 m and
(b) σ = 0.07 m, (c) σ = 0.42 m and (d) σ = 0.63 m.
curves (polylines), possibly containing rich shape information, which

can be important when matching consecutive environment scans for
localization purposes. The object represented by the union of the lines
G3 (6 lines) in Fig. 21.4(b) could not be represented by one, but four
polylines (consecutive line segments) seen from the left, right, lower,
and upper sides of this object (14 lines). Environment representation
with polylines could require more computer memory than our repre-
sentation with unions of line segments.
21.5 Conclusion
In this paper we represent a simulator of a mobile robot with an LRF in

a 2D static environment and propose a map-building algorithm, which
is tested on the simulator. An environment map describes each envi-
ronment object with a union of line segments. In this way the search
strategy to find pairs of line segments for localization purposes could be
faster than with an environment map, which is composed of only one
set of line segments. The mapping algorithm is fast enough for real-time
and will be integrated into the SLAM algorithm in the future.
References
1. Thrun S (2002) Robotic Mapping: A Survey. In: Lakemeyer G, Nebel
B, (eds) Exploring Artificial Intelligence in the New Millenium. Morgan
Kaufmann
2. Garulli A, Giannitrapani A, Rossi A, Vicino A (2002) Mobile robot SLAM
for line-based environment representation. 44th IEEE Conf. on Decision
and Control and European Control Conf.
3. Tomatis N, Nourbakhsh I, Siegwart R (2003) Hybrid simultaneous local-
ization and map building: a natural integration of topological and metric.
Robotics and Autonomous Systems 44:3–14
4. Veeck M, Veeck W (2004) Learning polyline maps from range scan data
acquired with mobile robots. IEEE/RSJ Int. Conf. on Intelligent Robots
and Systems
5. Nguyen V, Martinelli A, Tomatis N, Siegwart R, (2005) A Comparison of
Line Extraction Algorithms using 2D Laser Rangefinder for Indoor Mo-
bile Robotics. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems.
6. Latecki LJ, Lakaemper R, Sun X, Wolter D (2004) Building Polygonal
Maps from Laser Range Data. ECAI Int. Cognitive Robotics Workshop,
Valencia, Spain, August.
7. Mazl R, Preucil L (2000) Building a 2D Environment Map from Laser
Range-Finder Data. Proc. of the IEEE Intelligent Vehicles Symposium.
22
Lie Algebraic Approach to Nonholonomic
Motion Planning in Cluttered Environment
Pawel Ludwików and Ignacy Dulȩba

University of Technology, Janiszewski St. 11/17, 50-372 Wroclaw, Poland
{pawel.ludwikow, ignacy.duleba}@pwr.wroc.pl
22.1 Introduction
Nonholonomic systems of robotics, resulting from constraints expressed

in the Pfaff form, can be described as driftless systems
m
X
q̇q = G(qq )u
u= gi (qq )ui , dim u = m < dim q = n, (22.1)
i=1
where some components of velocities q̇q define controls u . A motion

planning task in a cluttered environment is to find the controls steering
system (22.1) from the given initial configuration q0 to the final state
qf while avoiding collisions with obstacles. The controls can be used
in open-loop control. However, in most practical cases they serve as a
source of the desired trajectory, which is to be followed by low-level
control algorithms (usually taking into account also the dynamics of
the system).
Many nonholonomic motion planning algorithms have been devel-
oped so far. Works by Dubbins, Reeds and Shepp on the optimal motion
for a simple car were extended to an obstacle cluttered environment
by Laumond [6]. More numerous classes of systems for which algo-
rithms of motion planning have been developed cover chain-systems,
flat systems, or nilpotent (or nilpotentizable via a feedback transfor-
mation). A general strategy to control nonholonomic systems known as
the continuation method was introduced by Sussmann [12]. Following
his line, Divelbiss and Wen developed an anti-gradient (Newtonian in
type) technique to control nonholonomic systems in the configuration
space polluted with obstacles [2]. An extensive overview of nonholo-
nomic motion planning methods can be found in [1, 8].
springerlink.com
250 P. Ludwików and I. Dulȩba
The method proposed in this paper couples the advantages of an

elastic band method of planning a collision free holonomic path and
the local Lie algebraic method of nonholonomic motion planning used
to generate an approximation of the path with controls. In its first
step, any admissible (holonomic) path is generated. Then, with the
method of an elastic band technique, introduced in the field of robot-
ics by Quinlan and Khatib [11], the path is iteratively reshaped to
minimize its Euclidean length and preserve a safe distance from obsta-
cles. Optionally, an extra nonholonomic measure can be added to the
optimization function to evaluate the generated paths [5]. When the
optimal collision-free path is generated, it is split into small sub-paths
to set goal points for consecutive nonholonomic motion planning tasks
performed with the local Lie algebraic method. Laumond stated [7]
that any collision-free holonomic path having an open neighborhood
of the free configuration space can be generated with a nonholonomic
motion planner if only the system (22.1) is small-time controllable.
The local planning method is based on the the generalized Campbell-
Baker-Hausdorff-Dynkin formula (gCBHD), which makes it possible
to express a local motion as a function of vector fields (locally - direc-
tions) multiplied by control-dependent coefficients. Usually controls are
expressed in, appropriately truncated, a control basis. Consequently,
controls are parametrized and searching for controls is substituted with
searching for their coefficients. Usually harmonic functions are used for
this purpose. With the aid of the gCBHD formula, it is not difficult to
calculate the mapping between the space of parameters and the space
of directions of motion (some formulas to facilitate the computations
are described in the paper [4]). Because, locally, the required direction
of motion is known, the task of local motion planning is reformulated as
the inverse task for the mapping. At this stage the well-known Newton
algorithm is applied [10].
Contrary to the majority of motion planning strategies, the pre-
sented method does not need to satisfy any other properties besides
the small time local controllability. As most of the computations can
be performed off-line, its complexity is rather low. As the basic (Lie-
algebraic) method of motion planning is local, the planning is not very
sensitive to small variations of the goal state or obstacles. It can be
also adapted to follow a moving target.
In robotic literature some Lie algebraic techniques were developed.
Liu and Sussmann’s technique [9] advises to apply highly oscillatory
controls to steer nonholonomic systems. This technique cannot be eas-
ily updated to cover inadmissible (due to obstacles) islands in the con-
22 Nonholonomic Motion Planning in a Cluttered Environment 251
figuration space and needs many harmonics to generate the desired

trajectory. The presented method is free from those disadvantages.
22.2 Preliminaries
It is assumed that the reader is familiar with some basic Lie-algebraic

terms (Lie bracket, vector field, degree of a vector field, Ph. Hall ba-
sis, P H, Lie algebra LA, etc.) [4]. To avoid complicated formulas,
all the discussed concepts will be presented for two-input control sys-
tems (22.1) (n = 2, g1 = X, g2 = Y ). The generalization is straightfor-
ward.
The key theorem exploited to steer systems (22.1) is the Lie algebra
rank condition LARC defining the small time local controllability
∀qq rank(P H(gg ))(qq ) = n = rank(P H(gg ))(qq ),
g = {g1 , . . . , gm }.
(22.2)
The gCBHD formula describes (locally) a solution to a non-autonomous
system of differential equations with a given initial condition. The
systems also cover the case of Eq. (22.1). The gCBHD formula is a trans-
port operator that, for small time horizon T of a local (one-step) plan-
ning, translates a current state q0 to the position q0 + z(u u (·), T )(qq0 ),
where controls u (·) are applied on the interval [0, T ]. The state shift
operator z(·) takes the form of a Lie series composed of Ph. Hall basis
elements multiplied by control-dependent coefficients [4]
z(u(·), T ) = α1 X + α2 Y + α3 [X, Y ] + . . . . (22.3)
Coefficients α = (α1 , α2 , α3 , . . .) depend on the controls as follows:
ZT ZT
α1 = u(s)ds, α2 = v(s)ds,
0 0
(22.4)
ZT Zs2
1
α3 = (u(s1 )v(s2 ) − v(s1 )u(s2 ))ds1 ds2 .
2
s2 =0 s1 =0
When the gCBHD is applied at state q 0 , all vector fields in Eq. (22.3)
must be evaluated at that point. Usually, controls are coded with their
coefficients. To this aim any basis (harmonic, polynomial, piecewise-
constant) can be selected. The vector of parameters of all controls
will be denoted p . Substituting the controls into Eq. (22.4), a func-
tion α = k (pp) can be established. As a rule, when the direction of
motion is known at a current state, then from Eq. (22.3) coefficients

α can be determined. In the general case the number of parameters,
equal to dim p, is larger than the dimensionality of the configuration
space (n), the weighted generalized Moore-Penrose matrix inversion,
[10] is required to find p . We use the weighted inversion rather than
the usually applied classical inversion because it can be proved [4] that
the coefficient corresponding to a vector field A depends on (small) T
as follows
α ∼ T degree(A)/2 . (22.5)
It means that low degree vector fields are more effective in the state
translation than those of higher degrees. Consequently, low degree vec-
tor fields should be preferred to generate motion. It can be done with
the use of weighting coefficients in the matrix inversion [5].
22.3 The Algorithm

The Lie algebraic algorithm of nonholonomic motion planning for the
cluttered environment goes as follows (the symbol ← is reserved for
substitutions while k · k denotes the Euclidean norm):
Step 1: Read in the initial state q0 , the goal state qf , and accuracy δ of
reaching the goal.
Step 2: Plan a collision-free holonomic path q (s), s ∈ [0, sf in ]:
2a : Select any collision-free path (e.g. with the use of a potential field
method, Voronoi diagram technique or the visibility graph). The
path becomes the current path.
2b: Discretize the current path q i , i = 0, 1, . . ..
2c : Modify each point of the path
q next
i = q i + ∆qi
where ∆qi results from the minimization of the criterion func-
tion:
q i−1 − q i q i+1 − q i
F = ξ1 Fel + ξ2 Frep , Fel ≃ + (22.6)
kqq i−1 − q i k kqq i+1 − q i k
with positive weighting coefficients ξ1 , ξ2 . Fel tries to stretch the
path to shorten its length. Frep is the resulting repelling force
(the vector sum of the repelling forces of obstacles acting on the
point q i ). It is responsible for safety of the path. The repelling
force should attain small values for obstacles placed far from the
points and increase to infinity when the point is in close vicinity
of any obstacle.
2d: The path composed of points q next i , i = 0, . . . becomes the new

current path.
2e : If the new path is very close to the old one, stop the computa-
tions and output the current path, otherwise go to Step 2b.
Step 3: Set the initial point for the next local planning q ini ← q 0 .
Step 4: From the optimal holonomic path, select the goal point q f for
the next one-step motion planning.
Step 5: Plan a one-step trajectory:
5a : Evaluate the appropriate number of vector fields (from the Ph.
Hall basis) at q ini required to satisfy the LARC, (22.2). In this
way a finite set of vectors V = {vi , i = 1, . . . , I} (vector fields
evaluated at q ini ) is obtained. The number of vectors, I, is equal
to the number of Ph. Hall basis elements up to the degree of the
highest degree used to check the LARC.
5b: Select a rich enough space of controls, parametrized with p . For
each vector field represented in the set vi ∈ V , compute (using
the gCBHD formula) its coefficient αi using Eq. (22.4):
αi (T ) ≡ αi = ki (pp), i = 1, . . . , I. (22.7)
Additionally, compute Eq. (22.4) for τ < T . Some components
of p or their functions entering αi (τ ) do not enter αi (T ). These
components will be called free parameters. The parameters in-
fluence the shape of a local trajectory not influencing its end-
point attained at T .
5c : Gather α -s to form the mapping
α = (α1 , . . . , αI )T = k (pp). (22.8)
5d: Express the vector q f − q ini as a linear combination of the basis
vectors:
XI
f ini
q −q = γivi , vi ∈ V . (22.9)
i=1
In this way a vector of the desired displacement αd (qqini ) = αd =
(γ1 , . . . , γI )T is determined.
5e : With the use of the Newton algorithm applied to mapping (22.8)
find the vector of parameters p⋆ that generate the desired value
of αd , (α αd = k (pp⋆ )).
5f : For parameters p⋆ compute controls u (pp⋆ , ·) and check whether
integral curve ϕ(qqini , u (pp⋆ , ·), t) initialized at q ini and corre-
sponding to the controls is obstacle-free for t ∈ [0, T ] and close
enough (for t = T ) to the current goal q f . If both conditions are
satisfied goto Step 6.
5g : If a prescribed number of attempts to change parameters was

exceeded report failure and goto Step 6. Otherwise, to avoid
obstacles and reshape the trajectory, change free parameters of
p and goto Step 5.
Step 6: If Step 5 ended with failure, move the goal state q f closer to q ini
and go back to Step 5. Otherwise substitute q ini ← ϕ(qqini , u (pp⋆⋆ , ·), T ).
Step 7: Check the stop condition:
if kqqini − qf k < δ then output the resulting trajectory (the concate-
nation of the trajectories obtained by the consecutive local planning
tasks) and finish the algorithm. Otherwise go to Step 4.
Some details of the algorithms must be explained. To converge, the
algorithm requires the LARC (22.2) to be satisfied. Note that (Step
5a) not only those vector fields that satisfy the LARC should be taken
into account but also all those with their degrees as large as the high-
est degree of the vector field involved in checking the LARC; this is
a straightforward consequence of Eq. (22.5). The extra vector fields
impact (22.3) to the same extent as some vector fields used to check
the LARC. The convergence of the algorithm is guaranteed if only the
LARC condition is satisfied and in each iteration the current path has
got an open neighborhood free from obstacles. Arguments supporting
the claim follow: if a current segment of one-step planning is short
enough then, for a small time controllable system, the gCBHD is able
to plan (not necessarily collision-free) nonholonomic path reaching the
goal point of this iteration (low-degree vector fields surpass the impact
of higher-degree vector fields in Eq. (22.3)). If the nonholonomic path
is also obstacle-free then the local planning task is done. Otherwise
the free parameters can be used to reshape the path while preserving
the goal approaching. If for any set of free parameters the collision-free
path cannot be found (Step 5g), the goal point for the current iteration
is selected closer to the initial point (Step 6). It is instructive to analyze
when Step 5g ends with no success. As the Lie algebraic method is local,
then the truncated formula (22.3) (with higher-order terms neglected)
is valid locally, i.e. when q f is close to q ini . From the computational
point of view, it is desirable to put as small mid-points q f as possi-
ble. Therefore the next q f is selected relatively far from its q ini and is
moved closer to it only when Step 5g fails. This iterative process must
be completed successfully, as for systems satisfying LARC there exist
small distance goals that the whole path reaching each goal does not
leave the open neighborhood of the initial (direct) path to the goal and
the truncation error in formula (22.3) tends to zero. Step 5b requires
a rich enough space of controls. Obviously the number of parameters
must be at least equal to n. When the number is increased, the com-

plexity of the planning increases together with the ability of the path
to be reshaped. In practical situations it is advised to use the smallest
number of parameters and to add more parameters to the vector p only
if necessary (when Step 5g fails). All the computations (22.7) for varied
p can be done off-line.
While planning an initial path (Step 2) an extra nonholonomic cri-
terion can also be taken into account, cf. Eq (22.6). Evaluation of a
path with respect to a nonholonomic measure is described in [5].
Several tests were performed to illustrate the topics of the paper. As

a model, the unicycle robot was selected. The unicycle is described by
the kinematic equations
     
ẋ cos θ 0
q̇q = ẏ  =  sin θ  u1 + 0 u2 = X u1 + Y u2 . (22.10)
θ̇ 0 1
In Eq. (22.10) x, y denote the coordinates of the unicycle on the plane

and the angle θ describes its orientation. The control u1 is the lin-
ear velocity of the vehicle while the control u2 is used to change its
orientation. Simple calculations show that [X, Y ] = (sin θ, − cos θ, 0)T
and det(X, Y, [X, Y ]) = 1 at any configuration, so the LARC (22.2) is
satisfied.
For all the tests the time horizon T was set to 1 for each local plan-
ning and equations of motion (22.10), for a given vector u parameters
of controls, were integrated with the Runge-Kutta method of the 4th
order. The controls were expressed as a truncated harmonic series
r r
1 2
u(t) = a0 + a1 sin(ωt + ϕ),
T T
r r (22.11)
1 2
v(t) = b0 + b1 sin(ωt + ψ),
T T
where ω = 2π/T and the vector of parameters p = (a0 , a1 , ϕ, b0 , b1 , ψ)T .
Substituting (22.11) into (22.4) and integrating the expressions over the
interval [0, T ] we get coefficients α as a function of the parameters of
the controls
√ √
α1 = T a0 , α2 = T b0 ,
T √ (22.12)
α3 = 2(a1 b0 cos ϕ − b1 a0 cos ψ) + a1 b1 sin(ϕ − ψ) .
2π
Equation (22.12) is redundant (dim p = 6 > 3 = dim q). To maxi-
mize coefficient α3 for a0 = b0 = 0, it was assumed that ψ = ϕ + π/2.
Thus the vector of parameters is reduced to p = (a0 , a1 , ϕ, b0 , b1 )T .
Note that for fixed values of components α1 , α2 (coefficients a0 , b0 are
fixed there), a constant value of α3 can be obtained with many values
of a1 , b1 , ϕ. Consequently, there are free parameters required by the
algorithm to reshape local trajectories. At first the impact of the free
parameters on the resulting path was evaluated in free space. The task
was to move the robot from q 0 = (0, −0.1, 0◦ ) to qf = (0, 0, 0◦ ). The
task requires motion along vector field [X, Y ] only, therefore parame-
ters a0 = b0 = 0, and the free parameter is ϕ. In Fig. 22.1 paths for
varied ϕ are presented. When an obstacle is placed in the configuration
space, the same task can be performed with different sets of control
parameters. A resulting path can be the safest possible or rather dan-
gerous. The motion planning task was to find the controls steering the
system (22.10) from the given initial state q0 = (−10, −2, 0◦ )T to the
final state qf = (0, 0, 0◦ )T . The final point was assumed to be reached
when the distance from the end-trajectory point to the final state was
smaller than δ = 0.01. The resulting path and controls producing the
path are presented in Fig. 22.2. The generated controls are piecewise-
continuous. For some application such controls are inacceptable. As was
shown in [3] they can be made continuous by expanding them into the
Fourier series and taking an appropriate number of harmonics. The ap-
0.02
000
045
090
135
0 180
225
270
315
-0.02
-0.04
-0.06
-0.08
-0.1
-0.12
-0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4
Fig. 22.1. Paths for varied phase shift ϕ[◦ ]

2
a) obstacles
path b) u
v
-1 0
-1
-2
-2
-10 -5 0 0 1 2 3 4 5 6 7 8 9
Fig. 22.2. The path (a) and controls (b) solving the motion planning task
proximated controls can generate a trajectory as close to the trajectory

obtained with piece-wise continuous controls as required.
22.5 Conclusions
In this paper the nonholonomic motion algorithm was presented to

plan a collision free trajectory. At first, it plans a holonomic, obstacle
free path. Then the path, after partitioning, is generated with con-
trols by the local Lie algebraic method. The proposed algorithm solves
the motion planning task when simultaneously the LARC is satisfied
and the paths generated by the algorithm are surrounded by an open
neighborhood of free configuration space.
References
1. Canny J.F., Li Z. (Eds.) (1993) Nonholonomic Motion Planning, Kluwer
Acad. Publ., New York
2. Divelbiss A.W., Wen J.T. (1994) Nonholonomic path planning with
inequality constraints, IEEE Conf. on Robotics and Automat., San
Diego, 52–57.
3. Dulȩba I. (2006) Making controls generated by the Lie-algoebraic methods
continuous, Nat. Conf. on Robotics, 53–60 (in Polish)
4. Dulȩba I., Khefifi W. (2006) Pre-control form of the generalized Campbell-
Baker-Hausdorff-Dynkin formula for affine nonholonomic systems, Systems
and Control Letters, 55(2), 146–157
5. Dulȩba I., Ludwików P. (2006) Local Variation Method to Determine
Cheap Path for Nonholonomic Systems, CISM-IFToMM Symp., Robot
Design, Dynamics, and Control, Courses and Lectures No. 487, 139-146,
Springer
6. Laumond J.P. (1994) A Motion Planner for Nonholonomic Mobile Robots,

IEEE Trans. on Rob. and Autom., 10(5), 577–593
7. Laumond J.P. (1998) Robot Motion Planning and Control, Lect. Notes in
Control and Information Sc., No. 229, Springer Verlag
8. LaValle S. (2006) Planning Algorithms, Cambridge Univ. Press
9. Liu W. (1997) An approximation algorithm for nonholonomic systems,
SIAM J. Control Optim., 35(4), 1328–1365
10. Nakamura Y. (1991) Advanced Robotics: Redundancy and Optimization,
Addison Wesley, New York
11. Quinlan S., Khatib O. (1993) Elastic Bands: Connecting Path and Con-
trol, IEEE Conf. Robotics and Automat., vol. 2, Atlanta, 802–807
12. Sussmann H . (1993) A continuation method for nonholonomic path-
finding problems, IEEE Conf. Dec. Contr., San Antonio, 2718–2723
23
Computationally Efficient Path Planning for
Wheeled Mobile Robots in Obstacle Dense
Environments
Husnu Türker Şahin and Erkan Zergeroğlu
Department of Computer Engineering, Gebze Institute of Technology,

PK. 141, 41400 Gebze/Kocaeli, Turkey
{htsahin,ezerger}@bilmuh.gyte.edu.tr
23.1 Introduction
Smooth path generation for nonholonomic wheeled mobile robots
(WMRs) has been researched significantly in the recent years. The non-
holonomic constraints of WMRs impose difficulties for effective path
planning; on top of this, the presence of static and/or dynamic obsta-
cles in operation environments complicates the problem further. Al-
ternative solutions have been proposed for WMR trajectory planning
ranging from graph search methods [1, 2] to the application of potential
function based techniques [4, 3, 5, 6]. Many of these methods assume
full observability of the operational space [1, 3, 5] and some cannot pro-
vide dynamical path planning [3, 5], which is impractical for general
applications of WMRs. Recently more advanced methods have been
presented that offer better solutions to the path planning problem for
obstacle cluttered environments [7, 8]. However, these methods utilize
triangulation based mappings of the nearby environment by expensive
sensory devices. This increases the computational cost of the planning
algorithms, and necessitates more expensive electronics like laser scan-
ners. A qualitative revision of the relation between the sensing abilities
of wheeled mobile robots and the topology of their environment can be
found in [9, 10].
In this paper we present a computationally efficient approach to the
nonholonomic and smooth path planning problem for WMRs based on
limited sensing information. The proposed path planner utilizes a two
axle reference robot, the back-wheel of which forms a readily nonholo-
nomic reference trajectory for unicycle robots to follow as the front axle
is steered in the direction of the device target. A simple yet effective
springerlink.com
260 H.T. Şahin and E. Zergeroğlu
obstacle avoidance algorithm is used in parallel with this direct steer

mechanism to achieve trajectory generation under minimum sensing
conditions, ie. in the presence of only a small number of ON-OFF sen-
sors providing a small sensing zone. When an obstacle is sensed, the
planner assumes it is a circular body, and readjusts the drag force to
the front axle to keep the path distant from the center of the obsta-
cle estimate. The planner has the potential to avoid u-shaped concave
blocks of arbitrary size as well as tunnels by varying two parameters of
its algorithm. Super-ellipses with minimum number of parameters can
be utilized as virtual obstacles to fill the concave sections of u-shaped
obstacles, thereby eliminating the need for complicated mapping al-
gorithms. With these properties the proposed method is suitable for
implementation on small robots with entry level sensors, and has the
potential for extension to multi-robot trajectory planning.
The rest of the paper is organized in the following manner: in Sec-
tion 23.2 the employed WMR kinematic model and the problem for-
mulation are summarized, while the path planner details are given in
Section 23.3. Simulation results are presented in Section 23.4, followed
by conclusions in Section 23.5.
23.2 Kinematic Model and Problem Formulation
The unicycle WMR kinematics utilized in this paper is modelled by

the following relation [12]:
   
x˙c cos θ 0
 y˙c  =  sin θ 0  vl . (23.1)
θ̇
θ̇ 0 1
T
Here qc = xc , yc , θ ∈ R3×1 is the pose vector of the WMR, with
[ xc (t), yc (t) ]T ∈ R2×1 and θ(t) ∈ R1 denoting the robot center of mass
(COM) and orientation, respectively. The v(t) = [ vl , θ̇ ]T term is the
linear and angular velocity vector. The cartesian WMR velocities are
obtained from (23.1) to be:
ẋc = vl cos θ, ẏc = vl sin θ. (23.2)
The planned paths must satisfy the following nonholonomicity con-

straint for accurate tracking by WMRs,
ẋc cos θ − ẏc sin θ = 0. (23.3)

23 Computationally Efficient Path Planning for Mobile Robots 261
The main objective of our path planner is to generate a nonholo-

nomic collision-free path in an unstructured obstacle dense environ-
ment with limited sensor data. The utilized reference robot is a 2-axle
device (Figure 23.1(a)), having the current front axle COM position
P = [ xe , ye ]T ∈ R2 . If the robot is steered by a force Fs from point
P to a desired front-end location Pd , its back wheel should follow gen-
erating a readily nonholonomic trajectory between the current WMR
position Cr = [ xr , yr ]T and its target Cd = [ xd , yd ]T . Avoidance of
obstacles that may be encountered in the robot path is achieved by
changing the steer force Fs in the direction away from these blocks.
23.3 The Proposed Path Planner

23.3.1 Nonholonomic Steering Towards a Desired Target
The general equations modeling the kinematics of the bicycle reference
robot, depicted in Fig. 23.1(a), are derived from the extension of the
unicycle kinematics of (23.1), (23.2) via suitable steer force-velocity
relations as:
ẋr = vl cos(θr ), ẏr = vl sin(θr ), θ̇r = vl sin(ϕ) / L,
vl = F cos(ϕ).
(23.4)
The modified terms in (23.1) are the linear velocity which is the pro-
jection of the steer force F on the robot direction via steer angle ϕ;
and the angular velocity θ̇r related to L, the distance between the front
axle and back axle positions P and Cr in Fig. 23.1(a).
The steering force Fs is applied from the front axle center P towards
desired WMR COM position Cd as depicted in Fig. 23.1(a). This force
is not declined until point Pd is reached. Hence as the front axle reaches
Pd , the back wheel center generates a smooth reference trajectory from
the initial position Cr0 to the desired target Cd suitable for a unicycle
WMR to track. To keep the steer force as simple as possible, while
satisfying these properties, the following formula is selected:
e
Fs = K p + B ė. (23.5)
kek2 + ε
Here K, B ∈ R2×2 are diagonal positive definite scaling matrices, and
kek denotes the Euclidean norm of the error term e, defined as the dif-
ference between the desired front axle COM Pd and its current position
P as follows:
e = Pd − P = [ xed − xe , yed − ye ]T . (23.6)
y y
L Pd
C1
R1
Fs Cd Cr
P
ĳ C2
Fo2
P R2
L
ș
r
Cr
Cr0 Fo1 Fo
x x
(a) Front steer mechanism model (b) Obstacle avoidance model
Fig. 23.1. The path planner reference robot models
In Eq. (23.5), a steering force normalization is applied by the division

of the position error by its norm. The error norm is inversely propor-
tional to the distance between the terminal point and current front end
location P , thereby counterbalancing any decline in the forcing func-
tion arising from reductions in the system error. Also the ε ∈ R+ term
in (23.5) is to prevent possible instability after the desired target Pd is
reached.
23.3.2 Obstacle Detection and Avoidance
During most robotic missions, WMRs have access to information from

a limited region of the task space via their sensors. As unicycle WMRs
are of circular structure, the sensing zone is assumed to be of ellipsoidal
shape in our work. When an obstacle is detected by one of the sensors,
the path planner switches to the obstacle avoidance phase. Our obsta-
cle avoidance algorithm operates effectively with commonly accessible
hardware such as ON/OFF sensors. The properties of such a setup are
depicted in Fig. 23.1(b), where the red dots show the sensors. Depend-
ing on which sensors send the signals, the WMR assumes a “circular
blocking object” is encountered about that location. The planner then
calculates estimates for the centers of the obstacles Ci (t); and hence for
their radii Ri (t) ∈ R+ as a measure of the size of the encountered block.
In these calculations the number of neighboring ON sensors and the du-
ration of their signals are utilized. Two such obstacle estimates are de-
picted by the shaded circles in Fig. 23.1(b). After the obstacle estimates
are computed, a force component Foi = [Foxi , Foyi ]T , i = 1, . . . N ,
pointing from the obstacle center Ci (t) to the reference robot front
axle center P (t) is formed for each excited neighboring sensor group.
Then the vector sum of these components forms the overall obstacle
avoidance force Fo as follows:
N
X N
X
Fo (t) = Foi (t) = wi Ri (t) [P (t) − Ci (t))], i = 1, . . . , N,
i=1 i=1
(23.7)
where Ri , Ci , and P are the terms defined above, and wi ∈ denote R+
additional weights selected higher for front sensors to emphasize avoid-
ance in the robot forward path. The ratio of each force component Fo
in (23.7), increases according to the impact time of the related obsta-
cle. However, the overall avoidance force is kept constant by preserving
the magnitude inherited from the steer towards the target mode force
in (23.5) to ensure generation of bumpless reference velocities impera-
tive for the nonholonomic controllers. If the WMR is encircled by some
fixed or mobile obstacles, such that 5 or more of the device sensors are
excited, the planner ceases the steer forces until some of the blocks are
removed to avoid the high risk of collision.
Remark 1. The obstacle avoidance forces Foi should not be ceased im-
mediately after the encountered obstacles are out of the sensing zone.
Otherwise, the path planner may start to switch instantaneously between
the forward steer and obstacle avoidance modes, resulting in undesirable
chattering in the overall steer force F (t). For this reason the cross-over
from obstacle avoidance to the steering toward the target mode is car-
ried out in a 2-stage period. In the first stage, a small virtual margin is
added to the estimated radius of the obstacles. Thus the planner contin-
ues to execute the block avoidance mode for an additional time period
△t after the actual obstacle is out of the sensing zone. The second
stage is implemented via a first order spline-like transition. The overall
obstacle steer force function F = [Fx , Fy ]T in this period is as follows:
(t − ts ) (ts + δt − t)
F = Fs + Fo , (23.8)
δt δt
where Fs and Fo are the front steering and normalized obstacle avoid-
ance forces in (23.5) and (23.7). This cross-over phase is confined to
t ∈ [ts , ts + δt] interval, where ts is the time instant the obstacle is out
of sensing zone, and δt is the duration of the transition. This two stage
period eliminates chattering in F in addition to local smoothness loss
arising from steer angle ϕ > 900 .
23.3.3 Extension for Large U-Blocks and Complicated

Tunnels
The proposed obstacle avoidance algorithm achieves good performance

for virtually all convex and many moderately concave obstacles. How-
ever, some extremely concave blocks such as larger u-shaped barriers
may cause the WMRs to be entrapped. This is a common problem for
most path planners containing a target weight in their algorithms [7].
The advantage of our method is that local obstacle topology can be
emphasized over target bias by increasing the values of the parame-
ters δt and △t of the spline period. Moreover, this modification also
improves planner performance in complex tunnels. Thus if the algo-
rithm detects such obstacles (by monitoring the distance function to
the WMR destination, or continuous excitations from more than one
sensor, respectively), the values of δt and △t are increased to higher
values. After the obstacle avoidance mode is over, these parameters are
reset to their default lower entries for increasing manoeuvrability and
enhancing detection of short-cuts.
Remark 2. To ensure the WMR does not re-enter traps of u-blocks, we

recommend the utilization of 4th degree super-ellipses as virtual obsta-
cles to fill the concave sections. These ellipses are planar, square like
curves with minimum number of parameters in the form:
(x − x0 )4 (y − y0 )4
+ = 1.
a4 b4
On encountering u-blocks, the δt and △t parameters can be incremented
until the generated path leaves the super-ellipse zone. Then the virtual
obstacle is activated so that the parameters are reset to their initial
values without any risk for the WMR to re-enter the concave trap any
more.
We have simulated the proposed path planning method in Matlab(r)

Simulink(r) environment using C mex S-functions. Simulations for
path planning in various operational spaces with obstacles of different
topologies have been performed. ON-OFF sensors located at the ro-
bot sensing zone perimeter are utilized in our simulations. We set the
number of these sensors to 6 in parallel with the sensing equipment of
small WMR configurations. Likewise the major and minor axis lengths
(a) Top down view of the environ- (b) Linear and angular velocities
ment
(c) Generated path (d) Pose tracking errors
Fig. 23.2. The first simulation environment and generated path related plots
of the resulting sensing region and the parameters of the steering force
function in (23.5) are set to:
a = 22.5 cm, b = 20 cm,

K = diag(0.165, 0.165), B = diag(0.01, 0.01). (23.9)
The spline period parameters are assigned the values △t = 0.6, δt = 2 s,

except where otherwise stated for improved performance. The unified
nonholonomic controller in [11] is utilized in our simulations.
The first simulation is on navigation of a WMR in an environment
with moderate obstacles as depicted in Fig. 23.2(a) with the initial and
desired WMR positions marked by green and red rectangles, respec-
tively. Figure 23.2(b) depicts the WMR linear and angular velocities,
which are functions with no chattering owing to the spline transition in
(a) Low and fixed △t = 0.6, δt = 2 s (b) Virtual obstacle △t, δt incre-
mented
Fig. 23.3. Simulations on u-blocks
(a) Short spline times of △t = 0.8, (b) Longer transition △t = 1, δt =

δt = 4 s 7.5 s
Fig. 23.4. Route planning in a demanding tunnel
Remark 1. Also the envelope of the velocity does not decline in parallel
with the steer force selection in (23.5). Despite numerous obstacles the
robot reaches its desired position as depicted in Fig. 23.2(c). Finally
from Fig. 23.2(d) we observe the tracking errors to be negligible except
for small fluctuations during obstacle manoeuvres.
Next are the simulations verifying the positive influence of readjust-
ing △t and δt parameters systematically for u-blocks and tunnels. The
behavior of the path planner versus concave blocks is shown in Figs.
23.3(a) and 23.3(b). In the simulation of Fig. 23.3(a), the WMR en-
counters a u-barrier, where it is trapped. This is caused by the small,
fixed default values of spline parameters △t = 0.6 and δt = 2 s. The
simulation is repeated in Fig. 23.3(b). However, this time after the trap
is sensed, △t and δt is incremented until the WMR leaves the virtual
obstacle x4 + y 4 = 1 covering the u-gap in parallel with Remark 2.

After the WMR is outside the virtual obstacle, these parameters are
reset to their default values. Thus we observe the WMR reach its final
location with no further delay.
The final simulations are on path planning in a complex tunnel as
depicted in Fig. 23.4. The objective is to guide the WMR on the left of
the tunnel to the clearance on the right. For the first simulation on Fig.
23.4(a), the △t and δt parameters are set to smaller values of 0.6 and
2 s respectively, implying a quicker exit from the spline transmission
period. The resulting target bias causes the robot to change its course
many times in the tunnel and so that it cannot reach its target. In
the next simulation in Figure 23.4(b) the parameters are set to higher
values of 1 and 7.5 s. Thus the guidance of the planner is improved,
and the WMR reaches its destination with no direction reversals. As
a result we can conclude that our planner can also perform effectively
for large concave blocks and complex tunnels.
23.5 Conclusions
We have presented a simple yet effective algorithm for collision-free,
smooth path generation for unicycle type WMRs. The proposed path
planner integrates a virtual front steering axle to the actual kinematic
wheel enforcing a nonholonomic path for the (actual) rear axis, when
the front axle is steered towards a desired location. To tackle the obsta-
cle avoidance problem, obstacles detected by the sensors are assumed
as of circular geometry, and repulsion forces from their estimated cen-
ters are applied to the virtual front axle for an alternative collision-free
route. Extension for planning in the cases of obstacles of problematic
topology such as u-shaped barriers and tunnels without the necessity
for computationally expensive mappings is also proposed. Simulation
results confirm the computational efficiency and performance of our
planner as an alternative to the existing complex high performance
planners.
References
1. P. Jacobs and J. Canny, Planning Smooth Paths for Mobile Robots, In:
Proc. IEEE Int. Conf. on Robotics and Automation, 1, 2-7, May 1989.
2. J-P. Laumond, P. E. Jacobs, M. Taix and R. M. Murray, A Motion Plan-
ner for Nonholonomic Mobile Robots, IEEE Trans. on Robotics and Au-
tomation, 10(5), October 1994.
3. R. M. Murray and S. S. Sastry, Nonholonomic Motion Planning: Steer-

ing Sinusoids”, IEEE Trans. on Automatic Control, 38(5), 700-716, May
1993.
4. E. Rimon and D. E. Koditschek, Exact Robot Navigation using Artificial
Potential Function”, IEEE Trans. on Robotics and Automation, 8(5),
501-518, 1992.
5. J. Chen, W. E. Dixon, D. M. Dawson and T. Galluzzo, Navigation and
control of a Wheeled Mobile Robot, In: Proc. IEEE/ASME Int. Conf. on
Advanced Intelligent Mechatronics, 1145-1150, 2005.
6. F. Lamiraux and D. Bonnafous, Reactive Trajectory Deformation for
Nonholonomic Systems: Applications to Mobile Robots, In: Proc. IEEE
Int. Conf. on Robotics and Automation, 3099-3104, May 2002.
7. J. Minguez and L. Montano, Nearness diagram (ND) navigation: colli-
sion avoidance in roublesome scenarios, IEEE Trans. on Robotics and
Automation, 20(1), 45-59, Feb. 2004.
8. P. Krishnamurthy and F. Khorrami, GODZILLA: A Low-resource Algo-
rithm for Path Planning in Unknown Environments, In: Proc. American
Control Conference, 110-115, 2005.
9. J.M. O’Kane and S.M. LaValle, On Comparing the Power of Mobile Ro-
bots, Robotics: Science and Systems, August 2006.
10. B. Tovar, A. Yershova, J.M. O’Kane and S.M. LaValle, Information
Spaces for Mobile Robots, In: Proc. Int. Workshop on Robot Motion and
Control RoMoCo’05, 11-20, June 2005.
11. W. E. Dixon, D. M. Dawson, E. Zergeroglu, and F. Zhang, Robust Track-
ing and Regulation Control for Mobile Robots, Int. Journal of Robust and
Nonlinear Control, 10, 199-216, 2000.
12. R. M’Closkey and R. Murray, Exponential Stabilization of Driftless Non-
linear Control Systems Using Homogeneous Feedback, IEEE Trans. on
Automatic Control, 42(5), 614-628, May 1997.
24
Piecewise-Constant Controls for Newtonian
Nonholonomic Motion Planning
Ignacy Dulȩba

University of Technology, Janiszewski St. 11/17, 50-372 Wroclaw, Poland
ignacy.duleba@pwr.wroc.pl
24.1 Introduction
Driftless, nonholonomic systems are frequently encountered in robotics

and they are described by the equations
m
X
q̇ = G(q)u = gi (q)ui , dim u = m < dim q = n, (24.1)
i=1
where controls u are composed of some components of velocities q̇.

Kinematics of the driftless nonholonomic system (24.1) is a mapping
[9],
kq0 ,T : L2m [0, T ] −→ Rn , kq0 ,T (u(·)) = ϕT,q0 (u(·)), (24.2)
where q0 is an initial state and T is a time horizon. The flow ϕt,q0 (u(·))
defined by Eq. (24.2) describes evolution of the state over the time
period [0, t]. Also, ϕt,q0 (u(·)) is interpreted as the end-trajectory state
reached at the time t. A motion planning task is to find controls steer-
ing the system (24.1) from the given initial configuration q0 to the final
state qf . With kinematics defined by Eq. (24.2), the motion planning
of nonholonomic systems (24.1) can be considered as the inverse kine-
matics task.
Among many methods of nonholonomic motion planning [1, 4, 6]
there are only a few general enough to plan trajectories for a wide
range of nonholonomic systems. One of these methods is the Newton
algorithm for solving the inverse kinematics problem. In the field of
mobile robotics the Newton method has been introduced by Wen and
coworkers, [2, 7]. In optimization terms, the algorithm implements the
steepest-descent iterative strategy of reaching the goal state. In order to
springerlink.com
270 I. Dulȩba
effectively apply the algorithm any appropriately truncated basis in the

control space should be selected. The controls become parametrized and
searching for controls is replaced with searching for their coefficients.
Usually harmonic functions are used for this purpose [9]. In this paper
the application of piecewise constant controls for the task is evaluated.
24.2 The Newton Algorithm
Once kinematics (24.2) are defined, the task of reaching the goal state
qf can be formulated as the inverse kinematics task, i.e. kq0 ,T (u(·)) =
qf , where u(·) is sought. The Newton algorithm is applied to nonholo-
nomic systems in a similar way as it was used for holonomic manipula-
tors [8]. The algorithm modifies controls directing ϕT,q0 (u(·)) towards
qf at the time T . Its (i + 1)st iteration is described by the equation
ui+1 (·) = ui (·) − ξi Dkq#0 ,T (ui (·))(kq0 ,T (ui (·)) − qf ), (24.3)
where Dkq0 ,T is the Jacobian matrix of the system (24.1) and the #
mark introduces the generalized Moore-Penrose matrix inversion. If
kq0 ,T (ui (·)) − qf = 0, then the final state qf is reached and the tra-
jectory planning task is solved. Otherwise, controls ui (·) are modified
according to the formula (24.3). The parameter ξi is a small positive
number influencing the pace of convergence of the algorithm (24.3).
A straightforward application of formula (24.3) requires the approxi-
mation of controls ui (·) in the interval [0, T ]. To this aim a functional
basis is selected and controls are described as a series. Usually har-
monic functions are used for this purpose. They are continuous and
bounded. However, they have got also some disadvantages. It is easy
to upper bounded their amplitudes over the control interval [0, T ] but
exact maximal values of the controls (especially for many harmonics)
is troublesome. Even a slight change of any coefficient of a harmonic
control impacts the shape of the whole trajectory. Also constraints on
controls are difficult to transform into constraints on parameters of har-
monic functions. A practical situation of necessity for such constraints
happens when the turning radius of a mobile robot is restricted. Those
disadvantages are not present with piecewise constant controls.
Later on we assume that the time interval [0, T ] has been uniformly
divided into N sub-intervals of the length ∆T = T /N . Then the k-th
control is expressed as
uk (si ) = uki , k = 1, . . . , m, si ∈ [(i − 1)∆T, i∆T ]. (24.4)

24 Piecewise-Constant Controls for Newtonian Motion Planning 271
Let the matrices A(t), B(t) result from linearization of the system (24.1)
along the trajectory corresponding to controls u(·) and be defined by
the expression
∂(G(q(t))u(t))
A(t) = , B(t) = G(q(t)), (24.5)
∂q
while the fundamental matrix Φ(t, s) is the solution of the equation
d
Φ(t, s) = A(t)Φ(t, s), Φ(s, s) = I n (24.6)
dt
(II n is the identity matrix) and is computed using linearization of the
system (24.1) along the trajectory corresponding to the current set of
controls
Φ(T, s) = lim (II n + A(τr )∆s) · · · · · (II n + A(τ1 )∆s), (24.7)

∆s→0
where ∆s = (T − s)/r, τk = (k + 1/2)∆s and r is the number of

sub-intervals the interval [s, T ] is divided into. The Jacobian matrix
of kinematics of the driftless nonholonomic system (24.1) is calculated
according to the formula
 T 
Z
Dkq0 ,T (u(·))δu(·) =  Φ(T, s)B(s)ds δu = Jq0 ,T δu, (24.8)
0
where δu denotes a small change of u. The k-th column of the full

Jacobian matrix, having m · N columns, is defined as follows
Jk = Φ(T, si )Bj (si )∆T, i = ⌊k/m⌋, j = k − i · m, (24.9)
where Bj (si ) denotes the j-th column of the matrix B evaluated at the
si and ⌊·⌋ stands for the floor function. Using Eq. (24.3), the vector of
values of controls u is changed in the i-th iteration according to the
formula
ui+1 = ui − ξi Jq#0 ,T (ui (·))(kq0 ,T (ui (·)) − qf ), (24.10)
where J # = J T (JJ T )−1 is the Moore-Penrose inverse. Controls mod-

ified according to Eq. (24.10) allow us to reduce the distance between
the point kq0 ,T (ui+1 (·)) and the goal state qf to zero if only there are
no singularities (i.e. J # is well defined everywhere in the state space)
and the discretization has been selected appropriately. The Newton
algorithm for nonholonomic systems heavily depends on linearization
272 I. Dulȩba
of the system (24.1) along the current trajectory. The main difference
between holonomic and nonholonomic applications of the Newton
algorithm is that computing kinematics and the Jacobian matrix of non-
holonomic system is much more involved because numerical integra-
tion of Eq. (24.1) and the linearization along the current trajectory,
cf. Eqns. (24.8-24.7), have to be performed. Therefore, the problem
of main concern with the Newton algorithm applied to nonholonomic
motion planning is how to reduce the computational load required to
complete the planning task. One modification of the basic Newton al-
gorithm (24.10) is to vary the value of coefficient ξ. The optimal value
ξ ⋆ in the i-th iteration should be determined in one dimensional opti-
mization process:
kkq0 ,T (ui+1 (ξ ⋆ , ·)) − qf k = min kkq0 ,T (ui+1 (ξ, ·)) − qf k (24.11)

ξ
The value places the state kq0 ,T (ui+1 (·)) as close to the goal as possi-
ble. Implementation of (24.11) requires many integrations of the
sys tem (24. 1) corresponding to varied ui+1 (ξ, ·) but does not need com-
puting A(t), B(t), Φ(T, ·). The basic Newton algorithm may modify any
piece of controls in any iteration. Let us restrict changes allowed and
define the index sets: F = {1, . . . , m·N } collecting all admissible indices
and its subsets Fr , r = n, . . . , m · N composed of r element combina-
tions of F . The Jacobian matrix J{f } denotes the sub-matrix composed
of columns of J corresponding to the index set f ∈ Fr . Now, the family
of manipulability measures can be defined:
T
manr = max det(J{f } J{f } ), r = n, . . . , m · N. (24.12)
f ∈Fr
Theorem 1
manr ≤ manv , if r < v. (24.13)
Proof. To prove the theorem it is enough to show the statement for the
case of neighbor indices r and r + 1. Let the set f ⋆ correspond to the
T
manr , i.e. manr = det(J{f ⋆ } J{f ⋆ any single
⋆ } ). By adding to the set f
⋆
column g taken from the set F − f we get f being the representative
of the family Fr+1 . Obviously, for the choice we have
T
det(J{f } J{f } ) ≤ manr+1 . (24.14)
Without loosing generality,

the added column can be the last one in the
Jacobian matrix J{f } = J{f ⋆ } g . Using the block matrix multiplication
we get
T T T
det(J{f } J{f } ) = det(J{f ⋆ } J{f ⋆ } + g g ). (24.15)
Both added matrices are non-negative definite, therefore [5]

T T T T
det(J{f ⋆ } J{f ⋆ } + g g ) ≥ det(J{f ⋆ } J{f ⋆ } ) + det(g g ). (24.16)
T T for a real constant
In (24.16) equality holds when J{f ⋆ } J{f ⋆} = c · g g
c. Collecting results (24.14), (24.15), (24.16) we get the thesis: manr ≤

manr+1 .
Remark 1: Very likely manr < manr+1 as in the general case the
condition (24.16)
is a strong inequality and also not necessarily the
columns J{f ⋆ } g form the set defining manr+1 .
Remark 2: Computation
of mank is very demanding as it requires
computing m·N k determinants. (The heaviest computational load is
carried for k = m · N/2.) Therefore the number of potential columns
should be reduced even at the expense of getting the sub-optimal rather
the optimal value of the manipulability.
Remark 3: As the manipulability increases for large values of r,
numeric problems may appear while computing Moore-Penrose in-
verse (24.10).
Remark 4: All remarks valid for the piecewise constant controls are
also valid for harmonic controls with varied harmonic representation of
controls.
Remark 5: Piecewise constant controls are spanned by step control
functions applied to each interval. They can be also considered as be-
ing represented in the Haar basis over the whole interval [0, T ], [3].
In this case each particular control is spanned by some elements of
the Haar basis and the elements are not necessarily consecutive com-
ponents. Moreover, the representation can vary from one iteration to
another. Consequently, it is one more advantage of using piecewise con-
stant controls over harmonic ones (that fix one representation for all
iterations).
In order to avoid too many decision variables and still preserving
reliable linearization it is advised to split the interval [0, T ] into a large
enough number of segments and couple controls in consecutive seg-
ments (they share the same value and change it simultaneously). For
the motion planning algorithm, it means that some columns of the full
Jacobian matrix J areP added to form a column of the resulting Jacobian
matrix J new : Jinew = j∈Ii Jj , ∪i Ii = {1, . . . , N · m}.
Now let us reduce admissible changes of controls to only one seg-
ment and only one control. Obviously, one can not guarantee a mo-
274 I. Dulȩba
x
α
R qf
Fig. 24.1. The motion along the column x of the Jacobian matrix
tion along a straight line to the goal still having a chance to re-
duce the distance to the goal as shown in Fig. 24.1. Let xi = kJi k,
i = 1, . . . , N · m, denote the Euclidean length of the column Ji of the
Jacobian matrix, R = kqf − q(T )k the distance to the goal in the
current iteration and αi the angle between vectors Ji and qf − q(T )
(cos αi = hJi , qf − q(T )i/(R xi )). Without loosing generality, it can be
assumed that αi ≤ π/2 (the direction of Ji can always be inverted by
changing the sign of controls). Although one does not know how far a
straight line motion along each column of the Jacobian matrix can be
performed, it is reasonable to assume that the motion can be as long
as required. The optimal column of the Jacobian matrix to move along
provides the optimal ratio between the cost of motion (the numerator)
and the effective length to the goal decrease (the denominator) in the
expression
R cos αi /xi cos αi cos α⋆
min = min = ⋆ .
i=1,...,N ·m R − R sin αi i=1,...,N ·m xi (1 − sin αi ) xi (1 − sin α⋆ )
(24.17)
The optimal column i⋆ determines both the single segment and the only
one control u⋆ to modify: ui+1 (·) = ui (·) + ξ[0 . . . 0 u⋆ 0 . . . 0]. The con-
trol modification can be coupled with the optimization process (24.11)
to get the new, minimal control effort, algorithm of motion planning.
It has got some advantages over its predecessors: a) no computation of
the Moore-Penrose inverse is to be performed, b) if the modification of
controls is applied for large values of s then only a very small part of
the whole trajectory (on the interval [s, T ]) has to be evaluated to get
Φ(T, s), c) integration of the curve can start at s (the trajectory for
t ∈ [0, s] is not modified). Consequently, the computational complexity
of the minimal control effort algorithm is much smaller than for the
basic Newton algorithms (24.10).
24.3 Simultion Results

Several tests were performed to illustrate the topics of the paper. As
a model, the unicycle robot was selected. The unicycle is described by
the kinematic equations
     
ẋ cos θ 0
ẏ  =  sin θ  u1 + 0 u2 = X u1 + Y u2 . (24.18)
θ̇ 0 1
In Eq. (24.18) x, y denote the coordinates of the unicycle on the plane,

while the angle θ describes its orientation. The control u1 is the linear
velocity of the vehicle while the control u2 is used to change its orien-
tation. For all the tests the time horizon T was set to 1 and equations
of motion (24.18), for a given vector u of parameters of controls, were
integrated with the Runge-Kutta method of the 4th order. The motion
planning task was to find controls steering the system (24.18) from
a given initial state to the final state qf = (0, 0, 0◦ ). The final point
was assumed to be reached when the distance from the end-trajectory
point to the final state was smaller than 0.01. The aim of the first
simulation was to evaluate manipulability indices defined. The motion
planning task was to reach the destination point qf from the initial state
q0 = (30, 15, 45◦ ). Initial controls were selected as harmonic functions
(easy to set via parameters)
u1 (t) = 2 + sin(ωt) − 3 · cos(ωt), u2 (t) = 3 − cos(ωt), ω = 2π/T
and then discretized, cf. Fig. 24.2a. The task was solved with the num-
ber of segments equal to 10. The final solution was obtained in 10
iterations. The final controls are drawn in Fig. 24.2b while the current
best trajectory for each iteration is presented in Fig. 24.2c (the end
points of the consecutive trajectories approach the final state). As can
be seen, the minimization of the distance to the goal does not neces-
sarily force the end point of the current trajectory to move along the
straight line from the final point of the previous iteration to the goal. In
each iteration the manipulability indices were examined. Figure 24.2d
illustrates their values. When the number of admissible columns to de-
rive manipulability index increases monotonously, the index increases
as well; at the very beginning linearly, then it gets stabilized. In this
particular simulation, for fixed k, the manipulability indices increase
monotonously (with a small exception in iterations 8 and 9) and the
lowest placed characteristics corresponds to the initial trajectory while
that placed at the top corresponds to the final trajectory. It is not a
rule, as the manipulability for fixed k is strongly correlated with the
length of the trajectory (the amplitude of controls) and the area in
the state space the trajectory visits. However, for small values of ini-
tial controls when the trajectory stretches out to get its final shape,
the regularity may manifests itself easily. The values of the indices can
276 I. Dulȩba
vary substantially. Therefore, it is required to implement some sort of

normalization (linear scaling) of the derived Jacobian matrix to avoid
computational problems with very large values of items in the processed
matrices. It is worth observing that the linear scaling does not impact
the computation process of the Newton algorithm as the post-scaling
is also performed in one step optimization process, cf. Eq. (24.11).
a) 6
b) 50
40
5
30
4
20
3
10
u
u
2 0
-10
1
-20
0
-30
-1
-40
-2 -50
0 0.2 0.4 0.6 0.8 1 0 0.2 0.4 0.6 0.8 1
t t
c) 18
d) 450
16 400
14 350
12
300
10
250
man
y
8
200
6
150
4
2 100
0 50
-2 0
-5 0 5 10 15 20 25 30 35 2 4 6 8 10 12 14 16 18 20
x k
Fig. 24.2. Initial (a) and final (b) controls (u1 solid line, u2 dashed line),
c) The current best trajectory in consecutive iterations, d) Manipulability
indices as the function of iterations and the number of columns k
The aim of the next simulation was to check the impact of the qual-
ity of linearization on the solvability of the motion planning task. The
initial state (180, 75, 45◦ ) was selected far away from the goal one. For
the first experiment the number of segments was set to N = 10 while
for the second one this value was multiplied by the factor of 4 (in 4
consecutive segments controls were coupled, i.e. they shared the same
value and changed it simultaneously). In both cases, the initial con-
trols were as in the previous example. The first experiment completed
after 17 iterations with a failure, while the second one was completed
with success after 99 iterations. In Fig. 24.3a,b selected trajectories are
presented.
a) 80
b) 80
70 70
60 60
50 50
y
y
40 40
30 30
20 20
10 10
0 0
20 40 60 80 100 120 140 160 180 200 -20 0 20 40 60 80 100 120 140 160 180 200
x x
Fig. 24.3. a) Iterations No. 0, 3, 6, 9, 12, 15, 17 of total 17. b) Iterations No.
0, 20, 40, 60, 80, 99 of total 99.
24.4 Conclusions
In this paper the Newton algorithm of motion planning for driftless
nonholonomic systems was examined with piecewise constant controls
used as decision parameters. Some manipulability measures were de-
fined and ordered. The new, minimal control effort algorithm of motion
planning was put forward. It seems to be competitive with respect to
complexity of planning to the basic Newton algorithm.
References
1. Canny J.F., Li Z. (Eds.) (1993) Nonholonomic Motion Planning, Kluwer
Acad. Publ., New York
2. Divelbiss A.W., Wen J.T. (1994) Nonholonomic path planning with
inequality constraints, IEEE Conf. on Robotics and Automat., San
Diego, 52–57.
3. Duleba I., Ludwików P. (2006) Bases for local nonholonomic motion plan-
ning, Lect. Notes in Control and Information Sc., No. 355, Springer, 73–84.
4. Fernendes C., Gurvits L., Li Z.X. (1991) A variational approach to optimal
monholonomic motion planning, IEEE Conf. Robotics & Autom., 680–685
5. Horn R.A., Johnson C.R. (1986) Matrix analysis, Cambridge Univ. Press
6. LaValle S. (2006) Planning Algorithms, Cambridge Univ. Press
7. Lizarralde F., Wen J.T. (1995) Feedback stabilization of nonholonomic
systems based on path space iteration, Int. Conf. on Methods and Models
in Automation and Robotics, Miȩdzyzdroje, 485–490
8. Nakamura Y. (1991) Advanced Robotics: Redundancy and Optimization,
Addison Wesley, New York
9. Tchoń K., Muszyński R. (2000) Instantaneous kinematics and dexterity
of mobile manipulators, IEEE Conf. on Robotics and Automation, vol. 2,
2493-2498
25
Path Following for Nonholonomic Mobile
Manipulators
Alicja Mazur

University of Technology, ul.Janiszewskiego 11/17, 50-372 Wroclaw, Poland
alicja@ict.pwr.wroc.pl
25.1 Introduction
In the paper a new control algorithm preserving a path following for

special class of mobile manipulators, namely for nonholonomic mobile
manipulators, is presented. The mobile manipulator is a robotic system
consisting of a mobile platform equipped with non–deformable wheels
and a manipulator mounted on this platform. Such a composed sys-
tem is able to realize manipulation tasks in a much larger workspace
than a manipulator with a fixed base. Taking into account the type
of constraints of both subsystems, there are 4 possible configurations
of mobile manipulators: type (h, h) – a holonomic manipulator on a
holonomic platform, type (h, nh) – a holonomic platform with a non-
holonomic manipulator, type (nh, h) – a nonholonomic platform with a
holonomic manipulator, and finally type (nh, nh) – both the platform
and the manipulator nonholonomic. In our considerations we focus on
the nonholonomic mobile manipulator of the (nh, h) type.
This paper is dedicated to the problem of following along the de-
sired path for nonholonomic mobile manipulators. Few authors have
brought their attention to the control problem of such objects [6], [9],
[10]. We want to solve the problem of the path following decomposed
into two separated tasks defined for any subsystem: the end-effector of
the manipulator has to track a geometric path and the nonholonomic
platform has to follow a desired curve lying on the horizontal plane.
The paper is organized as follows. Section 25.2 illustrates a theo-
retical design of the mathematical model of the considered objects. In
Section 25.3 the control problem is formulated. Sections 25.4 and 25.5
are dedicated to the formulation of the path following for the plat-
form and the manipulator. In Section 25.6 a new control algorithm
springerlink.com
280 A. Mazur
is designed and its proper action is proved. Section 25.7 contains the
simulation results. Section 25.8 presents conclusions.
25.2 Mathematical Model of Nonholonomic Mobile

Manipulator of Type (nh, h)
25.2.1 Nonholonomic Constraints
A mobile manipulator consists of two subsystems: a mobile platform

and a rigid manipulator. Motion of any mobile platform is determined
by nonholonomic constraints and can be described using generalized
coordinates qm ∈ Rn and velocities q̇m ∈ Rn . Such constraints come
from the assumption that the velocity in the contact point between
each wheel and the surface is equal to zero (motion of the platform is
pure rolling without slippage of the wheels).
This assumption implies the existence of l (l < n) independent
nonholonomic constraints in the so-called Pfaff’s form
A(qm )q̇m = 0, (25.1)
where A(qm ) is a full rank matrix of (l × n) size. Due to (25.1), since

the platform velocities q̇m are always in the null space of A, it is always
possible to find a vector of special auxiliary velocities η ∈ Rm , m =
n − l, such that
q̇m = G(qm )η, (25.2)
where G(qm ) is an n × m full rank matrix satisfying A(qm )G(qm ) = 0.
As the generalized coordinates of the platform we take a vector of
variables T
qm = x y θ β1 β2 , (25.3)
where (x, y) denote Cartesian coordinates of the mass center of the
mobile platform relative to basic frame X0 Y0 , θ is an orientation of the
platform and βi is an angle of rotation about a vertical axis passing
through the center of the ith independently driven steering wheel. For
the wheeled mobile platforms with restricted mobility an index i =
0, 1, 2 depends on the class of the platform [1], [2].
25.2.2 Dynamics of Mobile Manipulator with Nonholonomic

Platform
Let a vector of generalized coordinates of the mobile manipulator be

denoted as q T = (qmT , q T ), where q
r m is a vector of mobile platform
25 Path Following for Nonholonomic Mobile Manipulators 281
coordinates (25.3) and qr ∈ Rp denotes a vector of joint coordinates of

the rigid manipulator
qr = (θ1 , θ2 , . . . , θp )T . (25.4)
Because of the nonholonomy of the constraints, to obtain the dynamic
model of the mobile manipulator the d’Alembert principle has to be
used:
Q(q)q̈ + C(q, q̇)q̇ + D(q) = A1 (qm )λ + Bτ, (25.5)
where:
Q(q) – inertia matrix of the mobile manipulator,
C(q, q̇) – matrix coming from Coriolis and centrifugal forces,
D(q) – vector of gravity,
A1 (qm ) – matrix of nonholonomic constraints,
λ ∈ Rl – vector of Lagrange multipliers,
B – input matrix,
τ – vector of controls (input signals for actuators).
This model of dynamics can be expressed in more detail as
T
Q11 Q12 q̈m C11 C12 q̇m 0 A B1 0 τm
+ + = λ+ ,
Q21 Q22 q̈r C21 C22 q̇r D2 0 0 I τr
where A is the Pfaff’s matrix defined by (25.1), D2 is a vector of gravity
for the manipulator only, B1 describes which variables of the mobile
platform are directly driven by the actuators, τ is a vector of input
forces or torques. The model of dynamics (25.5) will be called the model
in generalized coordinates.
Now we want to express the model of dynamics using auxiliary veloc-
ities (25.2) instead of generalized coordinates for the mobile platform.
We compute
q̈m = G(qm )η̇ + Ġ(qm )η,
and eliminate in model of dynamics the vector of Lagrange multipliers
using the condition GT AT = 0. Substituting q̇m and q̈m we get
T " T #
G Q11 G GT Q12 η̇ G C11 G + Q11 Ġ GT C12 η
+ +
Q21 G Q22 q̈r C21 G C22 q̇ r
T
0 G B1 0 τm
+ = (25.6)
D2 0 I τr
or the same equations in a more compact form

∗ η̇ ∗ η
Q +C + D∗ = B ∗ τ. (25.7)
q̈r q̇r
282 A. Mazur
The model (25.7) of the mobile manipulator’s dynamics expressed in

auxiliary variables, which is a point of departure for designing a new
dynamic control algorithm, has a special property.
Property 1. [3] For a mobile manipulator with a wheeled nonholo-
nomic mobile platform a skew-symmetry between inertia matrix Q∗ and
the matrix coming from Coriolis and centrifugal forces C ∗ obtained as
the Christoffel symbols does not hold anymore. To regain the skew-
symmetry, a special nontrivial correction matrix CK must be added
d ∗
Q = (C ∗ + CK ) + (C ∗ + CK )T . (25.8)
dt
Each matrix satisfying (8) can be used as CK . Such a matrix should
be calculated before starting of regulation process.
25.3 Control Problem Statement
In the paper, our goal is to find control law guaranteeing the path
following for mobile manipulator of type (nh, h). We solve the path
following problem expressed in the internal coordinates. It means that
a desired task for the mobile manipulator can be decomposed into two
independent parts: the end-effector has to follow a desired geometric
path p(s) described in Cartesian coordinates which defines a task of
this subsystem and the task of the platform is to follow a desired curve
P lying on the horizontal plane.
Determine control law τ and path parametrization s = s(t) for
the manipulating arm such that a mobile manipulator with fully
known dynamics follows the desired paths defined separately for
both subsystems and tracking errors converge asymptotically to
zero in the presence of the nonholonomic constraints.
To design the path tracking controller for the mobile manipulator,
it is necessary to observe that the complete mathematical model of
the nonholonomic system expressed in auxiliary variables is a cascade
consisting of two groups of equations: kinematics and dynamics. For
this reason the structure of the controller is divided into two parts
working simultaneously:
• kinematic controller ηr – represents a vector of embedded control
inputs, which ensure realization of the task for the kinematics (non-
holonomic constraints) if the dynamics were not present. Such a con-
troller generates ’velocity profiles’, which can be executed in prac-
tice to realize the path following for nonholonomic wheeled mobile

platform.
• dynamic controller – as a consequence of cascaded structure of the
system model, the system’s velocities cannot be commanded di-
rectly, as it is assumed in the design of kinematic control signals,
and instead they must be realized as the output of the dynamics
driven by τ .
25.4 Path Following for the Platform
In this section, equations describing the motion of a point in the task-

space relative to a given curve P are derived. The mobile platform and
the path to be followed are presented in Fig. 25.1. The admissible path
P is characterized by a curvature c(s) which is an inversion of the radius
of the circle tangenting the path at a point characterized by s. Consider
a moving point M and the associated Frenet frame defined on the curve
P by the normal and tangent unit vectors xn and dr ds . The point M is the
mass center of a mobile platform and M’ is the orthogonal projection
of M on the path P . The coordinates of the point M in the Frenet
frame are (0, l, 0) and in the basic inertial frame (x, y, 0), where l is
the distance between M and M’. A curvilinear abscissa of M’ along the
curve P is equal to s. The position of M relative to the given path P
can be expressed as follows [6]
Y0
θ P
dr
ds
M
y dr
θr
1
l ds
xn dr 2
ds
r2
M’
s
x r1 X0
Fig. 25.1. Illustration of the path following problem

284 A. Mazur

ẋ (cos θr sin θr ) ẋ
l̇ = (− sin θr cos θr ) , ṡ = . (25.9)
ẏ 1 − c(s)l ẏ
The posture of a wheeled mobile platform is defined not only by the

position of its mass center but additionally by the orientation. There-
fore orientation tracking error can be defined by θ̃ = θ − θr . The set of
Frenet variables
ξ T = (l, θ̃, s) (25.10)
constitutes the path following coordinates for any wheeled mobile plat-
form with restricted mobility. It is worth of mention that the Frenet
parametrization ξ is valid only locally in the neighborhood of the path.
The point M’ exists and is uniquely defined if the following conditions
are satisfied [8], [4]:
• The radius of any circle tangenting P at two or more points and
the interior of which does not contain any point of the curve is
lowerbounded by some positive real number denoted as rmin (this
assumption implies in particular that the curvature c(s) is not bigger
than 1/rmin ).
• Under this assumption, if the distance between the path P and the
point M is smaller than rmin , there is a unique point on P denoted
by M’.
25.4.1 Kinematic Controller – Pomet Algorithm
In [6] it has been shown that for any class of nonholonomic mobile
platforms the path following problem, i.e. convergence of the variables
(l, θ̃) (distance tracking error and orientation tracking error) and pos-
sibly βi , i = 1, 2 to 0 can be obtained using Pomet kinematic control
algorithm. We have omitted the differential equation for ṡ, because it
does not matter at which point s of the desired path the mobile plat-
form enters the desired curve P [4].
Theorem 1. [7] Let us consider a control system of the form
m
X
ẋ = G(x)η = gk (x)ηk , (25.11)
k=1
where x ∈ Rn , η ∈ Rm , m = n − l is a number of unconstrained veloci-

ties and gk are smooth vector fields. We assume that the above system
satisfies the following conditions:
1. for any x 6= 0
rank {adjg1 gk | k = 1, . . . , m, j ≥ 0}(x) = n, (25.12)
2. there exists a system of coordinates such that the vector field

g1 (x) = ∂/∂x1 in the neighborhood of 0,
3. it is possible to choose a Lyapunov function V (t, x) of the form
1 1 1
V (t, x) = (x1 + h(t, x2 , . . . , xn ))2 + x22 + . . . x2n , (25.13)
2 2 2
where the most general choice of h(t, x) is
n
!
X
h(t, x) = h(t, x2 , ..., xn ) = x2i cos t,
i=2
4. vector fields gk are such that the following conditions can be satis-
fied:

Lgk V (t, x) = dV (t, x)gk (x) = 0 k = 2, . . . , m 
∂ j h(t, x) ⇒ x = 0. (25.14)
= 0 j ≥ 1 
∂tj
Then 0 is a globally uniformly asymptotically stable equilibrium point of
the closed-loop system obtained by applying the following feedback law

 ∂h(t, x)

 η1 = − − Lg1 V (t, x),

 ∂t
η2 = −Lg2 V (t, x), (25.15)

 ..

 .

ηm = −Lgm V (t, x),
to the system (25.11) with function V (t, x) given by (25.13). Moreover,

the function V is non-increasing along solutions of the closed-loop sys-
tem
V̇ (t, x(t)) ≤ 0. (25.16)
Theorem 1 defines a method of designing a control algorithm for the

kinematics of any wheeled mobile platform. This controller guarantees
asymptotic convergence of coordinates (l, θ̃) to 0, which is equivalent
to solving the path following problem for any mobile platform; see [6]
for details.
286 A. Mazur
25.5 Path Following for the Manipulator
The desired path of the manipulator can be specified by the geometric

curve Π(s) for the end-effector. Such a path given in Rρ local workspace
is defined relative to the manipulator local base’s frame Xb Yb , where
ρ ≤ p. The goal of the control process is to follow along the path Π(s)
by the end-effector with the tracking error
ep = k(qr ) − Π(s) −→ 0. (25.17)
The k(qr ) are kinematics of the manipulator. They describe generally

the position of the end-effector and sometimes its orientation, if some
relationship between the orientation and s parameter holds (i.e. orien-
tation depends on the parameter s). We will assume that the mapping
∂k
∂qr = J, which is in fact the Jacobi matrix for the manipulator, has
always a full rank ρ (manipulator avoids singular configurations). Simi-
larly to the Frenet parametrization, s is a current parameter describing
the path, i.e. its curvilinear length.
Due to [5], where the idea of the path following was formulated, we
assume that at the initial moment, for which s(0) = 0, the manipula-
tors’s end-effector is located on the path, i.e.
k(qr (0)) = Π(0),
and that at the initial and final time moments the velocities of the path
are 0
ṡ(0) = ṡ(T ) = 0, T −→ ∞.
Moreover, we expect that the length of the path is limited to the value
smax . It implies that the path following errors ep should be defined as
eTp = (k1 − π1 , . . . , kρ − πρ ), es = s − smax ,
and es is a curvilinear distance from the end of the path.

To obtain the path following for the manipulator, two separated
equations have to be defined. The first of them is possible path para-
metrization s = s(t) and the second is a control law which is a part
of the dynamic controller designed for the whole mobile manipulator.
The path parametrization s = s(t) can be obtained from the scalar
differential equation

1 ∂γ dp
s̈ = −Kd ṡ − Km es es + γ + 2K2 eTp F, F = , (25.18)
2 ∂s ds
where Kd , Km , K2 > 0 are some regulation parameters and γ is as-

sumed to be a strictly positive function of s which is required not to
satisfy the differential equation [5]
1 ∂γ
es + γ = 0.
2 ∂s
The role of this function is to influence the behavior of the end-effector
near the endpoint of the path, but the simplest choice is γ(s) = 1.
25.6 Dynamic Controller
We consider the model of a mobile manipulator (25.7) expressed in

auxiliary velocities. We assume that we know the velocities ηr (t) com-
puted due to Pomet algorithm (25.15), which solve the path following
problem for the mobile platform. Then we propose a dynamic control
algorithm providing asymptotic convergence for full kinematic and dy-
namic coordinates of the mobile manipulator as follows

∗ −1 ∗ η̇r ∗ ηr
τ = (B ) Q +C
−J˙T ep − J T J q̇r + J T F ṡ −J T ep
+D∗ − CK Ev − KEv } , (25.19)
where F is defined by (25.18) and

eη η − ηr
Ev = = ,
q̇r + J T ep q̇r + J T ep

K11 0
K= , K11 = diag{K1 }, K12 = diag{K2 }, K1 , K2 > 0.
0 K12
The closed-loop system (25.7) and (25.19) is described by the error
equation
Q∗ Ėv = −KEv − (C ∗ + CK )Ev . (25.20)
In order to prove the convergence of trajectories of both subsystems
of mobile manipulator to the desired paths, we choose the following
Lyapunov function
1 1 1
W (t, ξ, Ev , es , ṡ) = V (t, ξ) + EvT Q∗ Ev + ṡ2 + γKm e2s + K2 eTp ep ,
2 2 2
(25.21)
where V (t, ξ) is the Lyapunov function (25.13) for the kinematic equa-
tions describing the nonholonomic constraints (25.11) for the path fol-
lowing problem. Now we calculate the time derivative of W
288 A. Mazur
∂V ∂V 1
Ẇ = + ξ̇ + EvT Q∗ Ėv + EvT Q̇∗ Ev + ṡs̈
∂t ∂ξ 2

1 ∂γ 2
+ e + γes Km ṡ + 2K2 eTp ėp .
2 ∂s s
Before we begin to evaluate Ẇ along trajectories of the closed-loop

system (25.11), (25.15), (25.18), and (25.20), it is necessary to mention
the influence of additional errors coming from dynamic control level and
disturbing solutions to the kinematic equations (25.15). We will treat
ηr as kinematic control signals in the ideal case (i.e. without dynamics),
and then, as a kinematic control for real case (with dynamics), instead
of ηi defined by (25.15) we should take modified controls as follows

 ∂h(t, ξ)

 η1 = − − Lg1 V (t, ξ) + eη1 ,

 ∂t
η2 = −Lg2 V (t, ξ) + eη2 , (25.22)

 ..

 .

ηm = −Lgm V (t, ξ) + eηm .
Now we evaluate the time derivative of W along trajectories of the

closed-loop system (25.11)-(25.22), (25.18), and (25.20) as follows
Ẇ = − (Lg1 V )2 + eη1 Lg1 V + . . . − (Lgm V )2 + eηm Lgm V − eTη K11 eη

2
3 1
− K2 q̇rT q̇r − Kd ṡ2 − eTp JJ T ep ≤ − (Lg1 V )2 − Lg1 V − eη1
4 2
2
3 1
− (K1 − 1)e2η1 − . . . − (Lgm V )2 − Lgm V − eηm
4 2
− (K1 − 1)e2ηm − K2 q̇rT q̇r − Kd ṡ2 − K2 eTp JJ T ep ≤ 0.
It is easy to observe that the Lyapunov function W is decreasing

along any trajectory of the closed-loop system if control parameters
are greater than properly chosen numbers, i.e. K1 > 1.
Matrix JJ T is a matrix of manipulability of the rigid manipulator
and it is always positive definite as a consequence of the assumption
about avoiding singularities by the manipulator. Now we use LaSalle’s
invariance principle to define a set
N = {(t, ξ, Ev , es , ṡ) | Ẇ (t, ξ, Ev , es , ṡ) = 0}.
Let H be the largest invariant set contained in N . Then setting argu-

ments similar to the proof of convergence of the Pomet algorithm, we
can conclude that every bounded solution (t, ξ, Ev , es , ṡ) converges to

the set H
H = {(t, ξ, Ev , es , ṡ) | eη = 0, ṡ = 0, q̇r = 0, ep = 0, LgiV = 0, i = 1, . . . m}.
Now it is necessary to prove that a condition Lgi V = 0, i = 1, . . . , m

guarantees the convergence of ξ to 0 and that es converges to 0. How-
ever, the convergence of ξ to 0 is a straightforward implication of the
assumption (25.14) of Theorem 1.
Now we want to prove the convergence of es to 0. Note that the
condition ṡ(T ) → 0 for T → ∞ implies s̈(T ) → 0 for T → ∞. From
the equation of the path parametrization (25.18) we can conclude that
the following equation is fulfilled
1 ∂γ 2
e (∞) + γes (∞) = 0.
2 ∂s s
It means that es (∞) = 0 or 12 ∂γ
∂s es (∞) + γ = 0. However from the
properties of the γ(s) function we know that the second possibility
does not hold. Thus es (∞) = 0. This completes the proof.
25.7 Simulation Study
The simulations were run with the Matlab package and the Simulink
toolbox1 . As an object of simulations we took a 3-pendulum on the
unicycle depicted in Fig. 25.2. The goal of the simulations was to in-
vestigate a behavior of the mobile manipulator with the controllers
(25.22), (25.18), and (25.19) proposed in the paper. The desired path
for the 3-pendulum (straight line) was selected as
π1 (s) = 1 − s [m], π2 (s) = 0.5 + 0.24s [m],

2s
γ(s) = 0.003(1 + e ), smax = 1.2 [m]
and the desired path for the nonholonomic platform was equal to the
straight line
√ √
2 2
x(s) = s [m], y(s) = [m].
2 2
1
Matlab package and the Simulink toolbox were available thanks to Wroclaw Cen-
ter of Networking and Supercomputing.
290 A. Mazur
Y0 θ2 θ3
l2 l3
l1
θ1
X2
Yb Xb θ
Y2 Yp Xp
a
y
Y1
L
X1
X0
x
Fig. 25.2. Object of simulation: a 3-pendulum on a platform of (2,0) class
The initial position of the manipulator joints was equal to
(θ1 (0), θ2 (0), θ3 (0)) = (0, −π/2, π/2)
and the initial position of the platform

(x(0), y(0), θ(0)) = (5, 0, 0).
The parameters of the dynamical controller were equal to Kd = 10,

Km = 1, and K1 = 10. Tracking of the desired path by the 3-
pendulum’s end-effector is depicted in Figs 25.3-25.4. Tracking of the
desired path for the mobile platform is presented in Fig. 25.5.
0.8 0.8
0.7 0.7
0.6 0.6
0.5 0.5
0.4 0.4
0.3 0.3
0.2 0.2
0.1 0.1
a) 0
−0.2 0 0.2 0.4 0.6 0.8 1 1.2 b) 0
−0.2 0 0.2 0.4 0.6 0.8 1 1.2
Fig. 25.3. Posture of the 3-pendulum during tracking a straight line path
(dashed line): a) K2 = 1, b) K2 = 10
0.02 0.015
0.018
0.016 0.01
0.014
0.012 0.005
[m]
[m]
0.01
p1
p2
e
e
0.008 0
0.006
0.004 −0.005
0.002
0 −0.01
a) 0 2 4 6 8 10
TIME [s]
12 14 16 18 20
b) 0 5 10
TIME [s]
15 20 25
Fig. 25.4. Tracking errors ep = k(qr ) − π(s) for the 3-pendulum for parame-
ters K2 = 1 (solid line) and K2 = 10 (dashed line): a) path tracking error ep1 ,
b) path tracking error ep2
6 1.5
5
1
0.5
[m]
3
Y
−0.5
1
0 −1
0 1 2 3 4 5 6 0 20 40 60 80 100 120 140 160 180 200
a) X [m] b) TIME [s]
Fig. 25.5. Path tracking for the nonholonomic mobile platform, K2 = 10:
a) XY plot, b) distance tracking error l (smooth line) and orientation tracking
error θ̃ (oscillating line)
25.8 Conclusions
This paper presented a solution to the path following problem for non-
holonomic mobile manipulator of (nh, h) type. The control goal for the
mobile platform was to follow along the desired flat curve without stop-
ping the motion and the manipulator should move in such a way that
its end-effector follows along the geometric path described in Cartesian
coordinates and stops at the end of the desired path. From plots de-
picted in Figs 25.3-25.5 we can see that the new controller introduced
in this paper works properly. The Pomet algorithm, which is asymp-
totically convergent, preserves only driving to the path, not a motion
along the curve. Moreover, Fig. 25.3 shows that the manipulator re-
292 A. Mazur
ally does not achieve singular configurations which was the necessary
assumption by the controller designing.
The dynamic controller (25.19) can be applied only to mobile ma-
nipulators with fully known dynamics but it is a starting point for
designing the adaptive version of the presented controller. For holo-
nomic manipulators Galicki [5] has shown that it is possible to control
a robotic arm along the prescribed path without any knowledge about
the dynamics. Future work will implement this idea for nonholonomic
mobile manipulator with unknown dynamics.
References
1. Campion G, Bastin G, d’Andrea-Novel B (1996) Structural properties
and classification of dynamical models of wheeled mobile robots. IEEE
Trans Robot Autom 12(5):47–61
2. d’Andréa-Novel B, Bastin G, Campion G (1991) Modelling and control
of nonholonomic wheeled mobile robots. In: Proc. IEEE Int Conf on
Robotics and Automation, pp. 1130–1135, Sacramento
3. Dulȩba I (2000) Modeling and control of mobile manipulators. In: Proc
5th IFAC Symp SYROCO’00, pp. 687–692, Vienna
4. Fradkov A, Miroshnik I, Nikiforov V (1999) Nonlinear and Adaptive Con-
trol of Complex Systems. Kluwer Academic Publishers, Dordrecht
5. Galicki M (2006) Adaptive control of kinematically redundant manip-
ulator. Lecture Notes Control Inf Scie, 335:129–139, London, Springer
Verlag
6. Mazur A (2004) Hybrid adaptive control laws solving a path follow-
ing problem for non-holonomic mobile manipulators. Int J Control
77(15):1297–1306
7. Pomet J B (1992) Explicit design of time-varying stabilizing control
laws for a class of controllable systems without drift. Syst Control Lett,
18:147–158
8. Samson C (1995) Control of chained systems - application to path fol-
lowing and time-varying point-stabilization of mobile robots. IEEE Trans
Aut Control 40(1):147–158
9. Tchoń K, Jakubiak J (2004) Acceleration-Driven Kinematics of Mo-
bile Manipulators: an Endogenous Configuration Space Approach. In:
Lenarčič J, Galletti C (eds) On Advances in Robot Kinematics. Kluwer
Academic Publishers, The Netherlands
10. Tchoń K, Jakubiak J, Zadarnowska K (2004) Doubly nonholonomic mo-
bile manipulators. In: Proc IEEE Int Conf on Robotics and Automation,
pp. 4590–4595, New Orleans
26
On Simulator Design of the Spherical
Therapeutic Robot Koala
Krzysztof Arent and Marek Wnuk
The Institute of Computer Engineering, Control and Robotics, Wroclaw

{krzysztof.arent,marek.wnuk}@pwr.wroc.pl
26.1 Introduction
At present child development and pediatric rehabilitation is an emerg-

ing and challenging research field in the context of potential application
and development of socially interactive robots [1]. The research inspired
by psychological and pedagogical theories, as well as by recent achieve-
ments in robotics and assistive technology, has resulted in a variety
of dedicated robots, experimental techniques, and evaluation methods.
The robots range from mobile platforms of simple shape to humanoidal
dolls [1], [2] and are equipped with a rich set of sensors and actuators.
Their behaviours are purely reactive and the corresponding software
architecture is based on the behaviour based paradigm.
It has been realised in [2] that a therapeutic robot should feature a
certain degree of autonomy, which would make it possible to adapt the
robot’s behaviour to the needs and interests of a patient-child that are
extracted from the sensors signals somehow. The results of preliminary
experiments related to this issue have been reported in [4, 5].
Inspired by [2], [3], the authors of [6] propose a new interactive robot,
which is further developed in [7]. The robot, from the child perspective,
is a usual ball capable to react to the child motion, force, and voice with
light and sound. The basic specification includes: high durability so that
a child can have unconstrained access to the robot, low cost, and full
maintenance by a therapist without any assistance. These lines delimit
significantly the set of eventual sensors and actuators, the controller
unit, the behaviour programming method, and the therapist interface.
Systematic and comprehensive analysis carried out in [6, 7] (and in
the referred papers) leads to the conclusion that the robot behaviour
springerlink.com
296 K. Arent and M. Wnuk
should be programmed by means of fuzzy logic rules. The programming

process should be supported by a suitable robot simulator.
The basic specification of the therapist – robot interface bas been
done in [6]. The main task of the simulator is to enable the therapist
to effectively verify the robot behaviour defined by him/her. Thus the
robot simulator should offer a rich set of stimuli corresponding to the
child actions and robot states, which are recorded by sensors. Moreover,
it should offer tools for visualising actions of the robot actuators.
To the best knowledge of the authors, mathematical modelling of
the surrounding world from the sensors’ perspective has not been the
subject of an intensive research yet. The significance of this issue goes
beyond the therapist - robot interface, in particular in the context of
the problem of human needs and mental states recognition from sensor
measurements.
The goal of this paper is to design and analyse a basic scheme of the
Koala robot simulator with special regard to the stimuli. Notice that
it is a good opportunity to observe various aspects of the problem of
mathematical world modelling from the perspective of a socially inter-
active social robot sensors. Koala, due to its design, is a good object
of the first step analysis. To this end in Section 26.2 we discuss some
aspects of the Koala controller scheme and characterise applied sen-
sors and actuators. In Section 26.3 virtual Koala scheme is developed
and its components are discussed. In particular, sensor models are pro-
posed. In Section 26.4 prototype implementation aspects of the virtual
robot scheme in the environment of Matlab/Simulink are discussed.
Finally, in Section 26.5, some conclusions are provided.
26.2 Koala: Therapeutic Ball-Robot
The basic Koala controller scheme [7] is presented in Fig. 26.1. As soon
as an interaction is established, a child and Koala form a closed-loop
interconnection. The child actions are recorded by the robot sensors.
The linear accelerations ax , ay , az of the robot are measured by a
3D accelerometer MMA7260 [8]. It is a low-cost capacitive microma-
chined accelerometer which features temperature compensation, linear
characteristics of output vs. acceleration, and g-Select which allowing
to select among 4 sensitivities. (± 1.5 – ± 6 g). The rotational speeds
ωx , ωy , ωz are recorded by means of three gyroscopes ENC03JA [9].
Each operates on the principle of a resonator gyro, with micromachined
Coriolis force sensing.
26 On Simulator Design of the Spherical Therapeutic Robot Koala 297
CHILD MC33794
MMA7260
KOALA
ENC03JA
SENSORS CONTROLLER ACTUATORS MPXV7025
PREPROCESSOR FUZZY RULES POSTPROCESSOR RGB LED
Fig. 26.1. The Koala controller scheme and hardware prototype
The thrust force on the robot surface is derived using a silicon

pressure sensor MPXV7025 [10], which provides signal proportional
to the pressure p inside a pneumatic robot coat. Eight electrodes uni-
formly distributed directly under the robot surface are connected to
an MC33794 chip [11] intended for use in detecting objects using elec-
tric field. On-chip generated sine wave (120 kHz) is passed through an
internal resistor to the electrodes. Objects brought into or out of the
electric field change the leakage current and the resulting voltage at the
IC terminal, which in turn reduces the voltage at the sensor output. As
a result, this sensor generates signals proportional to the touch surface
si , i = 1, . . . , 8 (provided the touching element is conductive).
The actuators consist of six uniformly spread RGB LEDs driven by
signals ri , gi , bi , i = 1, . . . , 6. The sound is generated by two speakers
driven by a square wave (50% dutycycle) with programmable period T
and amplitude V . The actuator parameters, excluding T , are generated
as PWM (pulse width modulation) signals.
The controller is based on a MC9S12C32 microcontroller, the core
of which enables efficient implementation of fuzzy logic models.
Essentially the linguistic variables of the fuzzy model of Koala do
not coincide with the input and output variables described above. For
clarity those variables will be called external variables. The external
input variables are converted
q in the preprocessor to
q the following inter-
nal variables: acc = | ax + ay + az − g|, rot = | ωx2 + ωy2 + ωz2 |. The
2 2 2
controller works periodically with period 10ms, hence acc, rot, and
press are discrete time quantities. With acc there are two more signals
associated, iacc, and dacc, which are defined as follows: iacc(n) =
(1 − ǫ)iacc(n − 1) + ǫacc(n), dacc(n) = acc(n) − iacc(n), where
n = 0, 1, . . .. Analogously irot and drot has been defined. Pressure and
touch are characterised by press, ipress, dpress, touch, itouch, and
dtouch, where press(n) := press(n) − mini6n press(i). Notice that if

ǫ . 1 then d-variable represents scaled derivative of this variable.
The output internal variables are: hue, sat, val (which correspond
to HSV parametrisation of the colour palette), ihue, isat, ival, ton,
iton, vol, and ivol. The postprocessor provides HSV to RGB trans-
formation, moreover, the i-parameters are integrated.
26.3 Virtual Koala and Sensory Signals Modelling
A general scheme of the software system for simulation of Koala can-

not be based directly on the scheme of Fig. 26.1. To the best knowledge
of the authors, there are no available psychological models which would
satisfactorily describe child’s reactions on the stimuli originated by the
robot. An open-loop scheme has to be taken into account as it is de-
picted in Fig. 26.2. It consists of three main subsystems: stimuli, virtual
robot, and animation.
stimuli virtual robot
fuzzy
sensors
child robot preprocessor model postprocessor

actions model animation
of robot
external internal internal external

input input output output
signals signals signals signals
Fig. 26.2. The virtual Koala scheme
Essentially, it is hard to specify the proper set of stimuli without ac-

quiring some knowledge about the play style of a particular child with
a robot. We can conjecture that we will face with rolling (pushing, kick-
ing), tossing, throwing, dropping, embracing, stroking, squeezing, and
pressing. The robot behaviours are consequences of child’s behaviours,
which in turn should be awarded, suppressed, or caused.
The signals recorded by the sensors are a result or consequence of a
child’s actions. The touch sensors can be active as a result of holding
the robot by a child or as a consequence of pushing the robot on the
floor by a child. For that reasons the child actions and the robot model
blocks have been distinguished in the stimuli subsystem.
Koala is modelled by a ball sphere with a centrally placed cube

with uniformly distributed mass. The cube represents the box with
hardware and batteries. The sphere is stiff; elasticity comes from the
ground (walls). The pressure is proportional to the total centripetal
force constituent acting on the surface. It is assumed that the coor-
dinate systems of the accelerometer and gyroscope coincide with the
principal axes of the cube and form the coordinate system of the robot,
XR YR ZR . The base coordinate systems will be denoted XO YO ZO .
Transformation of the coordinate system O into R can be repre-
sented by a transformation matrix [12] A = Trans(X , x)Trans(Y, y)
RT
Trans(Z, z + R)Rot(Z, φ)Rot(Y, θ)Rot(Z, ψ) = , where R de-
0 1
notes the radius of the sphere, x, y, z are coordinates of the centre of
Koala, and φ, θ, ψ are Euler angles representing orientation of robot.
It follows from considerations in Section 26.2 that gyroscopes of
Koala return rotational velocity constituents ωx , ωy , ωz in the R co-
ordinate system. Therefore, denoting by [ω] a skew-symmetric matrix
associated with ωx , ωy , ωz , we can express the signal recorded by the
gyroscopes as
[ω] = RT (t)Ṙ(t). (26.1)
A brief discussion of the applied accelerometer shows that it returns
acceleration of the mass centre of Koala biased by gravity in the R
coordinate system. Thus, if a := col(ax , ay , az ), we have
T
a = RT (t)T̈g (t), T̈g = T̈ + 0 0 −g . (26.2)
The formulae (26.1), (26.2) express measured quantities it terms of
parameters which are more natural in the context of modelling direct
or indirect child actions related to Koala motion.
For example, if the pushed ball is rolling, we can easily derive those
variables by integrating the equation of motion
q̇ = G(q)η,
GT (q)Q(q)G(q)η̇ + GT (q)(C(q, G(q)η)G(q) + Q(q)Ġ(q))η (26.3)
T T
+ G (q)D(q) = G (q)B(q)u,
where q := col(x, y, z, φ, θ, ψ), Q is an inertia matrix of the system, C
represents Coriolis and centrifugal forces, D is a gravity vector, and G
is a matrix, whose columns span the null space of the matrix, which
specify nonholonomic phase constraints in the Pfaffian form [13]. The
vector u represents non-potential generalised forces acting on the sys-
tem, in particular forces caused by a child. This parameter seems to
be appropriate to express such child actions like kicking, periodical

pushing, etc. It should be emphasised that the variables q and u have
influence on recordings of the remaining sensors, which is indicated
below.
The eight touch sensors of Koala can be characterised by the polar
coordinates θiR , φR i (i = 1, . . . , 8) in the R coordinate system, where
R −sign(i−4.5)π [2((i−1) mod 4)+1]π
θi = 3 and φR
i = 4 . Denote by si the value
of the signal generated by the i − th touching sensor and by ci the
Euclidean coordinates of the i-th sensor in the R coordinate system.
The value of si is proportional to the contact surface of the sensor.
Essentially, the touching sensors are dedicated to grasp detection. But
they record various stimuli in many other situations as well. In the
example with the rolling ball the signals recorded by the touch sensors
can be expressed as follows: si = ǫ provided |c − ci | 6 Rs , otherwise
T
si = 0, where c = RT (t) 0 0 −R . Recall that the entries of c are the
Euclidean coordinates of the contact point with the ground in the R
coordinate system. The parameter ǫ expresses certain softness of the
ground while Rs denotes the radius of the touch sensor electrode.
In the considered example the variable p representing the pressure
sensor will return the constant value for t > 0.
Modelling sensory signals associated with other child actions and
robot reactions proceeds analogously. In fact, the modelling strategy
depicted above is open to any child actions.
The remaining subsystems of the virtual Koala scheme do not
require closer attention. The issues of simulations of fuzzy logic systems
as well as of visualisation of mechanical systems have been subject of
intensive research. Many useful patterns (and standards) are available.
The preprocessor and postprocessor maps are defined in Section 26.2
and can be easily implemented. However, it has to be emphasised that
the virtual robot offers us certain flexibility. For example, we can easily
implement different maps, addressed to different linguistic variables, to
investigate and verify alternative concepts of the robot. We can also
easily switch to continuous time working mode, which is often more
suitable in the context of various theoretical considerations.
26.4 Implementation Issues
The preliminary implementation of the scheme discussed in Section 26.3

has been done in the form of Matlab Blockset/Toolbox and documented
in [14]. This software environment was acknowledged to be appropriate
at the prototyping stage. Essentially, there was implemented a reduced

form of the Koala scheme, restricted to the part between the inter-
nal input/output signals. There were proposed five kinds of stimuli
(dropping, holding, kicking, rolling, and squeezing) and five robot be-
haviours, one associated to each stimuli. The important criterion in
the process of behaviour design, beside reaching a subjective goal (e.g.
awarding), was to achieve sensitivity on selected stimuli. The simu-
lation system was based on pure Matlab, Simulink, and Fuzzy Logic
Toolbox. The latter was used to implement the fuzzy logic model of
Koala, in particular to define the robot behaviours.
It appears that the full virtual Koala scheme of Fig. 26.2 can be
effectively implemented in Matlab/Simulink environment. The basic
idea is presented in Fig. 26.3. A large class of stimuli indicated in
Fig. 26.3. The example of the virtual Koala implementation scheme in

Matlab/Simulink
Section 26.3 can be quite easily implemented with the help of SimMe-
chanics, without direct referring to formula. Likewise in [14] the core of
the virtual robot is based on the Fuzzy Logic Toolbox. In the context
of robot animation two toolboxes are appropriate: Virtual Reality and
Signal Processing.
It is worth to mention that the fis structure, used in Matlab to
represent a fuzzy logic models, is almost consistent with the IEC 1131
standard. This standard is used in the Koala project and suitable
converters exist. Therefore all the robot behaviours, defined and verified
using Koala simulator, can be easily implemented on the real robot.
26.5 Conclusions
A complete, general scheme of the Koala robot simulator has been

proposed. The scheme fully meets the key expectations: huge freedom
in defining interactive robot behaviours and a good base for a rich class
of robot stimuli. Further simulator development should be primarily
directed toward enriching the class of stimuli. At present this issue is
under way.
References
1. Michaud, F., Salter, T., Duquette, A., Laplante, J.F.: Perspectives on
mobile robots used as tools for pediatric rehabilitation. Assistive Tech-
nologies (2005)
2. Dautenhahn, K., Werry, I.: Towards interactive robots in autism therapy.
background, motivation and challenges. Pragmatics & Cognition 12(1)
(2004) 1 – 35
3. Michaud, F., Caron, S.: Roball – an autonomous toy rolling robot. In:
Proceedings of the Workshop on Interactive Robot Entertainment. (2000)
4. Salter, T., Boekhorst, R.T., Dautenhahn, K.: Detecting and analysing
children’s play styles with autonomous mobile robots: A case study com-
paring observational data with sensor readings. The 8th Conference on
Intelligent Autonomous Systems (IAS-8) (2004)
5. Salter, T., Michaud, F., Dautenhahn, K., Letourneau, D., Caron, S.:
Recognizing interaction from a robot’s perspective. American Associa-
tion for Artificial Intelligence (2001)
6. Arent, K., Kabala, M., Wnuk, M.: Towards the therapeutic spherical ro-
bot: design, programming and control. In: 8th IFAC Symposium SyRoCo.
(2006)
7. Wnuk, M.: Koala – an interactive spherical robot assisting in therapy of
autistic children. In: Advances in Robotics. Systems and Cooperation of
Robots. WKL (2006) (in Polish).
8. Freescale Semiconductor, Inc: (±1.5g - 6g Three Axis Low-g Microma-
chined Accelerometer) MMA7260Q Technical Data Rev. 1.
9. ENC-03J. Piezoelectric Gyroscopes GyroStar. http://www.murata-europe .
com. (2004)
10. MPX4200A Integrated Silicon Pressure Sensor. ( http://e-www.mot orola.
com) catalogue card.
11. Freescale Semiconductor, Inc: Electric Field Imaging Device MC33794
Technical Data Rev. 8.0. (2005)
12. Murray, R.M., Li, Z., Sastry, S.S.: A Mathematical Introduction to Ro-
botic Manipulation. CRC Press, Inc. (1994)
13. Matysek, W., Muszyński, R.: Mathematical Model of Nonholonomic
Spherical Mobile Robot RoBall. In: Cybernetics of Robotic Systems.
WKL (2004)
14. Van den Broek, T.: Design of a simulation toolbox for a therapeutic robot.
Technical report. (2007)
27
Development of Rehabilitation Training
Support System Using 3D Force Display Robot
Yoshifumi Morita, Akinori Hirose, Takashi Uno, Masaki Uchida,

Hiroyuki Ukai, and Nobuyuki Matsui
Department of Computer Science & Engineering, Faculty of Engineering,

Nagoya Institute of Technology, Gokiso, Showa, Nagoya, Aichi,
466-8555 Japan
morita@nitech.ac.jp
In this paper we propose a new rehabilitation training support system

for upper limbs. The proposed system enables therapists to quanti-
tatively evaluate the therapeutic effect of upper limb motor function
during training, to easily change the load of resistance of training and
to easily develop a new training program suitable for the subjects. For
this purpose we develop a new 3D force display robot and its control al-
gorithms of training programs. The 3D force display robot has a parallel
link mechanism with three motors. The control algorithm simulating
sanding training is developed for the 3D force display robot. Moreover
the teaching/training function algorithm is developed. It enables the
therapists to easily make training trajectory suitable for the subject’s
condition. The effectiveness of the developed control algorithms is ver-
ified by experiments.
27.1 Introduction
It is forecasted that by the year 2014, one-fourth of the people in Japan

will be over the age of 65. An increase of the elderly population hav-
ing physical function problems due to diseases and advanced age has
been one of the most serious problems in Japan. On the other hand,
because of a decrease in the youth and labor population, it has been
pointed out that a shortage of rehabilitation therapists is a serious
social problem. From such data, it is desired that the technology of
robotics, mechatronics, information, and motion control is introduced
to the rehabilitation exercises carried out by human therapists from
person-to-person in order to reduce the therapists’ work and to increase
springerlink.com
304 Y. Morita et al.
therapeutic effects. Recently, many rehabilitation support systems have

been developed. Most of them are used to perform motor function exer-
cises of lower limbs so that a subject is able to walk by himself/herself
[1, 2]. However, there are few reports concerning the development of re-
habilitation systems for the occupational therapy of upper limb motor
function [3, 4, 5].
From this background, we have developed a two-dimensional re-
habilitation support system for various training of upper limb motor
function on the basis of robotic technology [6]. This system supports
the occupational therapy for recovering physical functions. Our final
aims in the future are to quantitatively evaluate the therapeutic ef-
fect of upper limb motor function during resistance training from the
standpoint of the establishment of EBM (Evidenced Based Medicine)
and to develop a new rehabilitation training support system with a
quantitative evaluation function of therapeutic effect.
In this paper we propose a new rehabilitation training support sys-
tem for upper limbs. For this purpose we develop a new 3D force display
robot and its control algorithms of rehabilitation training programs. As
one of the training programs the control algorithm simulating sanding
training is developed. Moreover the teaching/training function algo-
rithm is developed. It enables the therapists to easily make training
trajectory suitable for the subject’s condition. The effectiveness of the
developed control algorithms is verified by experiments.
27.2 Rehabilitation Training Support System
We propose a rehabilitation support system for the exercise of upper

limb motor function. This system supports the occupational therapy
for recovering physical function and is used for reaching exercises and
resistance training of the upper limbs, especially the shoulder and elbow
joints for ADL (Activities of Daily Living). This system is effective for
the advanced people and the patients with motor dysfunction of the
upper limb to keep motion ability, to increase residual ability, and to
recover from low-grade dysfunction.
The proposed system is able to provide various exercises as would
be provided by a therapist and a useful measurement function during
exercise. The system is composed of a force display robot, a rehabili-
tation grip for the subject to grasp, a display, a biometry system, and
a control unit. The subject grasps the rehabilitation grip, and moves
it accordingly to exercise motion shown on the display. From the view-
point of aiding the therapist’s work, the development provides not only
27 Development of Rehabilitation Training Support System 305
link 6 (0.40m) FGI

link 4 (0.30m)
Grip 20

Force/Torque link 2 (0.45m)
link 5 (0.15m)
Sensor θ
θ
θ link 3 (0.45m)
link 1 (0.30m)
(0.55m)
Motor
Motor Motor
Fig. 27.1. Rehabilitation training Fig. 27.2. 3D force display robot

using 3D force display robot
a new exercise machine but also the realization of following two sup-
porting functions; (1) motion assistance function and (2) supporting
function of planning and modification of exercise programs [7].
27.3 3D Force Display Robot
The 3D force display robot(3D robot) is developed for the rehabilita-

tion training support system as shown in Figs. 27.1 and 27.2. Subjects
perform rehabilitation training by moving the grip of the 3D robot
in 3D space. The subjects receive the reaction force through the grip,
which is determined by the control algorithm. The system enables us to
easily change the load of resistance (the mass-spring-damper constants
and the angle of inclination) for the subject’s condition and to imitate
resistance training by changing the control algorithms.
The robot is driven by three DD motors (rated power: 209 W) and
belt reduction drives. Links 5 and 6 are fixed. The force (maximum:
50 N) can be generated with respect to each direction of the grip.
The training space (500×500×500 mm) is available in 3D space. A
6-axis force/torque sensor (NITTA CORP. IFS-20E12A 15-I 25-EX)
is attached between the grip and the end effector to measure the
force/torque exerted by the subjects. The parallel link mechanism is
adopted because the structural stability of the robot becomes higher,
by which the robot links become lighter in weight, and the high affinity
between the robot and the human is realized. Two types of grips are
prepared. One is spherical in shape for single hand training. The other
is a bar type for double hand training.
Z
Zn Xn
ajec tory
ed tr n-th target position
desir
n-th θ θ
On y z
Grip
Yn n-th initial position
O X
Fig. 27.3. Coordinate system for resistance training
27.4 Control Algorithms of Rehabilitation Training

The control algorithms of various rehabilitation training are developed
for the 3D force display robot from the following two standpoints; (i)
simulation of conventional rehabilitation training tools and (ii) devel-
opment of a tool for the therapists to easily make a training program
suitable for the subject’s conditions.
27.4.1 Control Algorithm of Resistance Training

In the occupational therapy many kinds of resistance training are
performed, such as sanding training. The subject moves the grip or
the handle by himself/herself feely along a horizontal/vertical/inclined
straight line, on a horizontal/vertical/inclined plane or in 3D space.
These training programs can be simulated by implementing control al-
gorithms to the 3D force display robot. For this purpose we introduce
impedance control.
Let θ1 and θ2 denote the angles of link 1 and 2, respectively. Let θ3
denote the rotational angle of the base. Figure 27.3 shows the coordi-
nate frame used in simulating resistance training. Let Σ0 (O − XY Z)
denote an inertial Cartesian coordinate frame, where the origin O is
the center position of the base. Let r G = [xG , yG , zG ] denote the grip
position with respect to Σ0 . Let Σn (On −Xn Yn Zn ) denote a coordinate
frame where the Xn axis is the straight line between the n-th initial
position and the n-th target position, the Yn axis is perpendicular to
the Xn axis, and the Zn axis is perpendicular to the Xn and Yn axes.
Let θy and θz denote the rotational angles with respect to the Y and Z
axes. Let nr G = [nxG , nyG , nzG ] denote the grip position with respect to
Σn . f = [fx , fy , fz ] denotes the force exerted through the grip whose
components are represented in Σ0 , and nf = [nfx , nfy , nfz ] is the Xn ,
Yn , and Zn axis components of the force.
In order to realize the resistance training motion of the grip, we

apply the position-based impedance control. The reference grip position
r ref is given by
−−→
r ref = OOn +RT (θn )(M s2 +Ds+K)−1 (R(θn )f + nf w + nf s ), (27.1)
where R(θn )(= Rot(y, θy ) · Rot(z, θz )) is the rotation matrix from

Σ0 to Σn , M = diag[Mx My Mz ] is the virtual mass matrix, D =
diag[Dx Dy Dz ] is the virtual damping matrix, K = diag[Kx Ky Kz ] is
the virtual rigidity matrix, and nf w denotes Coulomb’s friction and/or
gravity force. nf s is the reaction force from the virtual spring-damper
wall and the limitation of the grip speed and the speed reference input.
From the viewpoints of safety and efficient exercises, the motion range
of the grip, the motion speed range, and the input voltage range have
to be restricted for each exercise program. However, if the grip auto-
matically stops at the end of the motion range, the drastic stop has
some adverse effect on the upper limb. For this reason, we prepare the
virtual walls at the end of the motion range to realize soft restriction.
In order to realize the position-based impedance control using the
speed-feedback type servo amplifiers, the speed reference inputs v ref
are represented by
Z
v ref = ṙ ref + KP (r ref − r G ) + KI (r ref − r G )dt, (27.2)
where KP is a proportional gain and KI is an integral gain.
27.4.2 Control Algorithm Simulating Sanding Training
Sanding training is a type of resistance training for upper limbs widely

performed in occupational therapy. This training is usually performed
using a sanding training tool as shown in Fig. 27.4. This training is
performed for not only ROM (range of motion) training of shoulder
joints, but also muscular reinforcement of biceps and triceps of upper
limbs, improvement of the cooperating function between flexor muscles
and extensor muscles, endurance training, and so on. By changing the
angle of the tilting rotary board and the weight in the box, the load of
resistance can be changed.
The sanding training tool is simulated using the 3D force display
robot. For this purpose we use the following parameters in Eq. (27.1);
M (= Mx = My = Mz ) = 1.2 [kg] and 6.2 [kg], Dx = Dy = Dz =
0 [kg/s], Kx = 0 [N/m], Ky = Kz = 1000 [N/m]. The force nf w can be
expressed as
Bar Type Grip
Force/Torque
Sensor Tape Shape
Sensor
Fig. 27.4. Traditional sanding train- Fig. 27.5. Sanding training using 3D
ing force display robot
Position[m] 30 Position[m] 30
0.6 0.6
Velocity[m/sec] Velocity[m/sec]
0.4 20 0.4 20
0.2 10 0.2 10
Force[N]
Force[N]
0 0 0 0
- 0.2 - 0.2
- 10 - 10
- 0.4 - 0.4
- 20 - 20
- 0.6 - 0.6
5 10 15 - 30 5 10 15 - 30
5 10 15 5 10 15
time[sec] time[sec] time[sec] time[sec]
(a) Traditional sanding training tool (b) Simulated sanding training system
Fig. 27.6. Comparison in responses of position/velocity and force
   
−M g sin θ −Fc sgn(n ẋG )
n
fw =  −M g cos θ  +  0 ,
0 0
where Fc (= 3.0) is the Coulomb friction coefficient, g is the gravita-
tional acceleration, and θ(=30 [◦ ]) is the slope of the sanding board. By
changing the damping coefficient Dx , the subject receives the proper
resistance force. By changing M , Dx , Fc , and θ, the load of resistance
can be changed. And we use the large rigidity coefficients Ky and Kz
of the Yn and Zn axes to virtually realize the sanding board.
We compare two kinds of data obtained using the sanding tool and
the simulated training system. These are almost the same as shown in
Fig. 27.6. For this reason it may be said that the traditional sanding
training can be imitated by using the 3D force display robot.
27.4.3 Teaching/ Training Function Algorithm

The conventional training tools are widely used for all patients. How-
ever, the training programs are different for the subjects, because the
subject’s conditions are different, such as his/her height, length of up-
per arm and forearm, degree of motor dysfunction and so on. So the
Therapist 1
0.9
Subject
(Patient) 0.8
Z[m]
0.7
0.6
0.5
0.2
0 1
0.8
- 0.2
Y[m] 0.6 X[m]
Fig. 27.7. Teaching scene of training Fig. 27.8. Teaching (◦) and actual
trajectory using 3D force display robot (–) trajectories
therapist has to make a training program suitable for the subject’s con-
dition. For this reason the control algorithm with a teaching function
of training trajectories is developed. It enables the therapists to easily
make training trajectories suitable for the subject’s condition.
The teaching/training procedure is as follows:
Step 1: The subject grasps the grip of the 3D robot. If the subject
cannot grasp it by himself/herself, the subject’s hand is fixed to the
grip.
Step 2: The therapist makes a training trajectory by moving both
the subject’s hand and the grip along the desired training trajectory as
shown in Fig. 27.7. The grip position data of the training trajectory are
recorded in the 3D robot. An example of the recorded training trajec-
tory is shown in Fig. 27.8. The green circles show the recorded training
trajectory data. The grip motion can be restricted on the training tra-
jectory.
Step 3: The subject moves the grip by himself/herself along the train-
ing trajectory. The actual trajectory (blue line) is shown in Fig. 27.8.
The actual trajectory is the result performed by the healthy student.
The restriction of the grip enables the subject to easily perform the
individual training program with good therapeutic effect.
Step 4:: The therapist easily changes the load of resistance of the train-
ing program by changing the parameters on the display.
By using this teaching/training function the therapists can be ex-
pected to develop a new training program.
27.5 Conclusions
We proposed a new rehabilitation training support system for upper

limbs. A new 3D force display robot having parallel link mechanism
was developed for rehabilitation training. The sanding training was
simulated in the 3D robot. Moreover, the teaching/training function
algorithm was developed for the therapists to easily make training tra-
jectory suitable for the subject’s condition. The effectiveness of the
control algorithm of the sanding training and the teaching/training
function algorithm is verified by experiments.
A future work is to perform clinical demonstrations using the pro-
posed system in cooperation with medical facilities to establish an eval-
uation method based on the measured data during the exercise.
References
1. T. Sakaki, “TEM: therapeutic exercise machine for recovering walking
functions of stroke patients”, Industrial Robot: An Int. J., 26-6, pp.446-
450, 1999.
2. Y. Nemoto, S. Egawa and M. Fujie, “Power Assist Control Developed for
Walking Support”, J. of Robotics and Mechatronics, 11-6, pp.473-476,
1999.
3. H.I. Krebs, N. Hogan, M.L. Aisen, and B.T. Volpe, “Robot-Aided Neurore-
habilitation”, IEEE Trans. on Rehabilitation Engineering, 6-1, pp.75-87,
1998.
4. B.T. Volpe, et al., “A novel Approach to Stroke Rehabilitation”, Neurol-
ogy, vol. 54, pp.1938-1944, 2000.
5. M. Sakaguchi, J. Furusho, and E. Genda, “Basic Study on Development of
Rehabilitation Training System Using ER Actuators”, J. of the Robotics
Society of Japan, 19-5, pp.612-619, 2001 (in Japanese).
6. H. Maeda, Y. Morita, E. Yamamoto, H. Kakami, H. Ukai, and N. Matsui,
“Development of Rehabilitation Support System for Reaching Exercise of
Upper Limb”, Proc. of 2003 IEEE Int. Symp. on Computational Intelli-
gence in Robotics and Automation (CIRA2003), pp.134–139, 2003
7. Y. Morita, T. Yamamoto, T. Suzuki, A. Hirose, H. Ukai, and N. Mat-
sui, “Movement Analysis of Upper Limb during Resistance Training Us-
ing General Purpose Robot Arm PA10”, Proc. of the 3rd Int. Conf. on
Mechatronics and Information Technology (ICMIT’05), JMHWI6(1)–(6),
2005
28
Applying CORBA Technology for the
Teleoperation of Wheeeler∗
Michal Pytasz and Grzegorz Granosik
Technical University of Lódź, Institute of Automatic Control,

ul. Stefanowskiego 18/22, 90-924 Lódź, Poland
mpytasz@wpk.p.lodz.pl, granosik@p.lodz.pl
28.1 Introduction
In this paper, we present development of Wheeeler – the hyper mobile

robot. Hyper mobile robots belong to the group of highly articulated
robots, sometimes called “snake-like” or serpentine robots. Wheeeler
has 7 segments driven by wheels and interconnected by 2 degrees-
of-freedom joints (Fig. 28.1). This machine is expected to operate in
rough terrain, traverse stairs and trenches, avoid obstacles, or climb
over them, and also pass through tight spaces. Our project is in the
simulation stage and currently we focus on the communication issues.
Although, modeling and tests are performed in simulator (Webots 5
PRO) now, the same control software will work with real robot soon.
In this paper, we shortly present the actual version of model; intro-
duce the sensory suite and local controllers’ configuration. In the main
paragraph we present the implementation of CORBA technology in
client-server communication.
28.2 Presentation of Wheeeler
In many inspection tasks, search and rescue or simply research projects

we are looking for robotic platform with extraordinary mobility. Desired
motion capabilities for such a robot are:
• ability to traverse rugged terrain, such as concrete floors cluttered
with debris;
• ability to climb up and over tall vertical steps;
∗
This research is financed from grant No. 3 T11A 024 30.
springerlink.com
312 M. Pytasz and G. Granosik
Fig. 28.1. Wheeeler avoiding an obstacle
• ability to fit through small openings;

• ability to travel inside of horizontal, vertical, or diagonal pipes such
as water pipes or air shafts;
• ability to climb up and down stairs;
• ability to pass across wide gaps.
An extended literature review on hyper mobile robots can be found
in [1]. That paper provides also detailed information on design and
development of whole family of serpentine robots – Omnis. Hyper mo-
bile robots usually contains multiple segments and redundant joints;
additionally they are equipped with propulsion means (e.g. wheels,
tracks or legs). It is obvious that redundancy implies complexity in
both mechanical construction and control. Most of the hyper mobile
robots presented to date lack the autonomy or intuitive teleoperation.
Although, every robot has some control system but in most cases they
employ multi dof joysticks or sophisticated user interfaces. Those with
the most advanced controllers have very limited operation area [7, 8].
28.2.1 The Main Concept

The idea of hyper mobile robot – Wheeeler – was introduced a few
months ago. We started development of Wheeeler with this goal in
mind – to design computer aided teleoperation for highly redundant,
multi dof mobile robots. We are going to find intuitive human-robot
interface to support such complex tasks as climbing stairs or crawling
in the maize of rubles. Project is at the modeling and simulation stage
– determining sensory suite and communication strategies.
Our project is not focused on the specific (especially optimal) me-
chanical solutions for driving and actuation mechanisms. Wheeled
28 Applying CORBA Technology for the Teleoperation of Wheeeler 313
propulsion was the very first applied in articulated mobile robots [2],
it is also very effective in tight spaces [7, 8], and easy to implement.
For joint actuation – integrated servos seems to be straight forward
solution, like in [3].
Using capabilities of Webots software we have built model of Wheeel-
er. Applying masses, inertias, friction coefficients and damping made
model and simulation very realistic (see: Fig. 28.1). Webots relies on
ODE (Open Dynamics Engine) to perform accurate physics simulation
and uses OpenGL graphics.
Since the first version, some changes have been applied in the model
of our hyper mobile robot, mostly as an effect of interaction between
you-can-do-anything-in-simulation and construction possibilities. The
sensory suite of our Wheeeler robot is now enriched with distance sen-
sors, accelerometers, gyroscopes as well as position and force sensors in
suspension of each segment. We added 11 sensors per segment which
gives total of 77 new values being sent to the controller. We still re-
member of 21 control variables and two cameras already existing. This
makes large amount of data to be handled, transmitted and processed.
We decided to reorganize transmission mechanisms to make communi-
cation easier to work with and more reliable, as presented in details in
Section 28.3.
28.2.2 Distributed Controllers

We also made some investigations and found the components for local
controllers. We already knew that the best fit for the serpentine
robots is distributed control system with some serial communication –
possibly CAN bus. But when the number of sensors, we need for tele-
operation of Wheeeler, rose to the mentioned value, we had to look for
new solutions for data acquisition. It usually seems to be easy to use
any microcontroller (µC) and connect analog signals directly. Problems
appear when the number of analog signals exceeds 8 – the usual number
of ADC inputs of µC. Fortunately, the idea of distributed measurement
and conditioning came down to the single chips and various solutions
of sensors with preprocessing and serial communications are now
available. We can name the few we actually are using in our design: 3 -
axis accelerometers LIS3LV02DL (STMicroelectronics), single axis
gyroscope ADIS16100 (Analog Devices), quadrature counters LS7366R
(LSI/CSI) all with SPI interface. Moreover, gyro ADIS16100 offers ad-
ditional analog inputs for user purpose. This allowed us to connect all
necessary sensors to local microcontroller AT90CAN128 (Atmel). We
have chosen this processor for the fast AVR structure and relatively
high processing power, as for 8-bit controllers. Additionally, in-system-

programming from Atmel offers reprogramming of each processor of
the system directly through CAN bus. This will simplify development
procedure of the robot’s lowest level software. Based on our preliminary
tests with local controllers we are quite sure that presented low level
architecture will work correctly in our robot.
28.3 Client-Server Communication
How to establish a stable communication between a robot and its oper-

ator? In the case of the described mobile robot, an IP network was used.
At the beginning, the transport layer was based on the raw UDP data-
grams. This solution has numerous advantages: transmission is fast, not
being blocked when a package loss occurs, it is easy to be debugged on
network level through watching transmitted datagrams. Unfortunately,
in case where multiple types of data are transmitted – their receipt and
post processing makes a program more complex and harder to debug.
28.3.1 Short Review
There are many libraries offering high level communication mecha-

nisms, ready to be used. Let us consider a few of them. One of the oldest
available is classical ONC RPC – Open Network Computing Remote
Procedure Call. It was created by Sun Microsystems as a transport
layer for its Network File System (NFS). From the functional point of
view, a similar mechanism is a DCE (Distributed Computing Environ-
ment) RPC, created by Apollo Computer, later acquired by HP. These
two mechanisms are not interoperable. Although RPC mechanisms are
very portable, they are not prepared for a straight forward applica-
tion in object oriented programming. For this reason, atop DCE RPC
(adopted and renamed to MSRPC), Microsoft had built DCOM mech-
anism – Distributed Component Object Model. Development of this
proprietary technology began with system clipboard and OLE (Object
Linking and Embedding ) and evolved to Network OLE and finally to
DCOM. One of the foundations of COM is programming language in-
dependence, which is indeed a desired feature. However, application of
DCOM would make the robot software platform dependent. An oppo-
site problem would appear if another object oriented communication
model, based on built in Java Remote Method Invocation, was used.
Java is platform independent, unfortunately RMI has a language de-
pendency, at least when it is applied in a straight forward way. These
disadvantages do not seem to occur in CORBA – Common Object Re-

quest Architecture. This technology is portable, language, hardware
and software platform independent. From the programmer’s point of
view it provides only a communication interface definition and remote
object reference search abilities. Client and server sides are always in-
teroperable if only they follow common IDL (Interface Definition Lan-
guage).
28.3.2 CORBA Implementation
In our robot, communication can be divided into two sections: control

data (see: Fig. 28.2) and sensor data (see: Fig. 28.3). Let us consider a
simple WheeelerControl communication interface defined in an example
shown in Fig. 28.4.
Fig. 28.2. Control information tab
Fig. 28.3. Sensor information tab
Interfaces definitions are included in modules, which after trans-

lation to Java form packages. An interface describes a servant class,
which in this case would be implemented on robot side. It provides a
client with remote methods to be called2 . All methods starting with
prefix set are supposed to send commands. All methods starting with
2
All interface definitions in this paper have been limited to Wheeeler specific meth-
ods.
Fig. 28.4. WheeelerControl network interface
prefix get provide a client with ability to read the state of the robot.
The definition above has only one advantage: it is very simple to be
implemented, since all communication is initiated by one side. Unfortu-
nately, this way of solving the problem would not be efficient in a real
application due to the need of initiation of all transfers only on one
side, forcing to read all data possible. It could lead to excessive data
transfer over the network due to very frequent periodic querying, or
outdated information in case of lower querying frequency. This analy-
sis led us to a conclusion that each side should be able to initiate a
transmission whenever there is some new data, control signal or sensor
reading, to be sent. As a result, the Wheeeler IDL module evolved to
its next version, presented in Fig. 28.5.
Fig. 28.5. WheeelerControl network interface – extended version
The WheeelerControl interface could be described in a similar way

as in the previous version. Sensor reading methods could be used for
debugging purposes, as well as a way of verification, in case of not
obtaining data from the robot for a long period. There is also a new
part in this interface. Methods starting with prefix send are supposed
to provide the robot with a references to new, remotely available objects
on the client side. Representation of this new interface, implemented
on a “client” side can be seen on Fig. 28.5. The WheeelerSensors class
is supposed to receive data from sensors. This kind of implementation
allows us to transfer any kind of data required, in any direction and
time whenever it is necessary, however active servants on both sides of
communication makes the boundary of client and server less sharp.
28.4 Simulation Time and Real Time Considerations
The purpose of our paper is to present analysis and implementation of

communication layer for teleoperation software of hyper mobile robot.
The control software is now tested in simulation environment, however,
building a real robot and using the software in real time is a very near
future. We are aware about differences between simulation time and
real time in execution of control algorithm. Anything can be done in
the simulation, therefore no real time operation problems can be ob-
served in this phase. In the simulation, a constant time step takes as
much real time as needed, still meeting hard real-time requirements in
the simulation environment. Moreover, simulation software is not being
run on a real-time operating system. This leads to a conclusion, that
from the operator’s point of view, no stochastic behaviour of a simu-
lated robot – in terms of time-slicing in real time – can be observed. In
the real robot, an embedded operating system with real-time scheduler
will be applied. For the controller design, real time enhancements to
Java and CORBA will be used [4], to lower the latency leading to more
reliable operation the robot. In the current stage of the project, ap-
plication of real-time programming enhancements should not be taken
into consideration, since both, client and server are not run on a real-
time operating system. In this case, the use of programming extensions
of this kind would show no effect, or could even lead to software mal-
functions. Another concern is the network based teleoperation. In the
example discussed here, client and server are planned to be in the same
sub network, however, the primary transport layer is planned to be
wireless. For this reason, we cannot relay on constant delays in the
communication any more, and therefore, the extensive autonomy has
to be implemented in the on-board controller. We will address these
concerns as soon as the system is implemented on real-time hardware
platform.
28.5 Further Developments
In the nearest future we will implement models of all expected sensors

in the simulation software. This will allow us to test new teleoperation
strategies for Wheeeler. Also, a very important feature to be imple-
mented in the near future is video streaming, through CORBA [5].
It is supposed not only to provide a visual feedback, but also supply
additional information about the environment through further video
processing.
References
1. Granosik G, Borenstein J and Hansen MG (2007) Serpentine Robots for
Industrial Inspection and Surveillance, in: Industrial Robotics. Program-
ming, Simulation and Applications, Low Kin Huat (Ed.), pp. 633-662,
plV Pro Literatur Verlag, Germany.
2. Hirose S, Morishima A (1990) Design and Control of a Mobile Robot
With an Articulated Body, The Int. Journal of Robotics Research, vol.
9, no. 2, pp. 99-113.
3. Kamegawa T, Yamasaki T, Igarashi H, Matsuno F (2004) Snake-like Res-
cue Robot KOHGA, Proc. IEEE Int. Conf. on Robotics and Automation,
New Orleans, LA, pp. 5081-5086.
4. Krishna A, Schmidt DC, Klefstad R (2004) Enhancing Real-Time
CORBA via Real-Time Java, Proc. IEEE Int. Conf. on Distributed Com-
puting Systems (ICDCS), Tokyo, Japan.
5. Mungee S, Surendran N, Schmidt DC (1999) The Design and Perfor-
mance of a CORBA Audio/Video Streaming Service HICSS-32. Proc. of
the 32nd Annual Hawaii Int. Conference on System Sciences.
6. Pytasz M, Granosik G (2006) Modelling of Wheeeler – hyper mobile
robot. (in Polish) In: Advances in Robotics. Control, Perception, and
Communication. K. Tchoń (Ed.), pp. 313-322, WKiL, Warsaw.
7. Schempf H, Mutschler E, Goltsberg V, Skoptsov G, Gavaert Vradis G
(2003) Explorer: Untethered Real-time Gas Main Assessment Robot Sys-
tem. Proc. of Int. Workshop on Advances in Service Robotics, ASER’03,
Bardolino, Italy.
8. Streich H, Adria O (2004) Software approach for the autonomous inspec-
tion robot Makro. Proc. IEEE Int. Conf. on Robotics and Automation,
New Orleans, LA, USA, pp. 3411-3416.
29
Educational Walking Robots∗
Teresa Zielińska, Andrzej Chmielniak, and Lukasz Jańczyk
Warsaw University of Technology, Institute of Aeronautics and Applied

Mechanics (WUT–IAAM), ul. Nowowiejska 24, 00-665 Warsaw, Poland
{teresaz,achmiel}@meil.pw.edu.pl, tulukaszj@wp.pl
29.1 Introduction
Walking machines are special examples of mobile robots performing the

discrete locomotion. Here the motion path is not continuous but con-
sists of separated footprints. The majority of walking machines take
biological locomotion systems as the templates, and include two, four,
or six legs with animal like posture. Up till now many multi-legged
robots have been built; some of them are very advanced, with a great
number of sensors and complex motion abilities, and some are very
simple considering their kinematics as well as the control systems. The
overview of design approaches with the descriptions of existing walking
machines with their parameters can be found in several references and is
available on the Internet – e.g. [3, 4, 12, 13, 14] – therefore we shall not
include it in this paper. Many works are devoted to control systems and
motion synthesis methods, e.g. [1, 2, 5, 7, 8, 11, 14]. Walking machines
are mainly actuated by electric motors. In simple prototypes servomo-
tors with built-in feedback are used. It is not possible to influence the
revolution speed in servomotors. The motion range of servomotors is
usually limited to 90◦ or 180◦ ; this must be considered in leg design
and in motion synthesis. Despite the above disadvantages there exists a
group of servo-controlled legged robots with complex motion abilities.
The examples are small humanoidal robots or robotic “animals” avail-
able in the market. Servomotors are light, small, easy to mount and
control. In this paper we introduce two simple, educational walking
robots. The robots with their control systems were designed and built
by students under professional supervision. The purpose of this work
∗
This work was supported by IAAM statutory funds.
springerlink.com
322 T. Zielińska, A. Chmielniak, and L. Jańczyk
was to practice the robot design methods using the knowledge gained
during the study, and to elaborate the cheap devices for the laboratory.
29.2 Educational Walking Robots – Mechanical

Structures
29.2.1 Hexapod
A six-legged robot called MENTOR [9] is shown in Fig. 29.2. Each leg
has a simple structure with 2 dof. In the 6 legs there are used 12 ser-
vomotors (Fig. 29.1). In the first solution the machine was controlled
by a stationary PC and control signals were sent via a printer con-
nector LPT1. Currently the onboard PC104 serves high-level control
(i.e. gait generation) sending the control signals. To save the space for
the sensors and to increase the payload the robot is externally supplied.
The window-type control program was developed, with only tripod gait
implemented. By using on-screen menus the operator initiates forward,
backward, left- and right-side walking, all by tripod gait – see Fig. 29.2.
The operator types in the number of walking steps (the field in the mid-
dle of the screen) and chooses the walking direction. During walking the
leg-ends follow the reference trajectories. The maximum forward and
backward leg-end shift as well as the trajectories were evaluated con-
sidering the stability conditions and are used in the discussed control
system. We are not discussing the details of leg-end trajectory synthesis
as we used here well-know classic methods.
Fig. 29.1. MENTOR; structure of the leg

29 Educational Walking Robots 323
Fig. 29.2. MENTOR walking by Fig. 29.3. View of the screen for
tripod gait MENTOR control
The leg kinematics structure of MENTOR was used in several other

walking machines [14], therefore we shall not discuss it in detail. Our
contribution in this point includes small improvements in the motor
attachment to the construction and optimization of the trunk sizes for
better postural stability and convenient placement of electronic devices.
The trunk size optimization was done targeting the increase of support
polygon area and increase of the internal space for placing the con-
trol computer and interface electronics. It resulted in the trunk being
slightly wider (by 10%) and thinner (by 5%) compared to other similar
designs. This offers a better condition for preserving static stability be-
cause with widening of the trunk the static stability margin is greater
(especially for side walking) and with thinner trunk the mass center is
located nearer to the ground, which again increases the stability (espe-
cially when walking on inclinations). The analysis of the static stability
margin considering geometric dimension of the machine is very simple
(it is planar geometry) and the approach is well-known, therefore we
are not including it in this text.
29.2.2 Quadruped
Another simple prototype is the quadruped BAJTEK (Fig. 29.4-29.7)

[10]. Each 2dof leg is actuated by servomotors. The machine is exter-
nally supplied. It is similar to MENTOR. The legs have pantograph-like
structure. The detailed leg structure and link proportions were designed
by us referring to the general idea of a pantograph applied many years
ago to a large, hydraulically powered walking machine [12]. The real-
time control system elaborated by us [10] allows the experimental gait
synthesis. The user can choose the activation period for each servo
Fig. 29.4. BAJTEK Fig. 29.5. View of the menu of the soft-
ware used for experimental motion synthesis
(which determines its rotation angle) and can observe if it results in

successful walking. This option allows the synthesis of not only gait
sequences but the leg-end trajectories. The user defines, data-by-data,
the angular positions of the servo-motors. Those positions will result in
proper leg-end trajectories and in proper leg-transfer sequences. Using
this scheme of motion definition the students can prove experimentally
the correctness of the proposed shapes of the leg-end trajectories, as
well as of the leg-transfer sequences, observing the resultant motion’s
stability. Moreover, following the definition of static stability (which has
to result in preserving the posture with every time instant), they can
test at-hoc if every typed-in intermediate position is statically stable.
Therefore the user is allowed to perform in the device every position at
the moment or can save it, and realize as a whole sequence. Another
option is to perform previously elaborated control sequences reading
them from the computer memory (Fig. 29.5). Each leg consists of two
y O2
O2 C C
Dim. Val. (mm)
x O1A 40
O1 O1
O2C 26
A A O3D 26
O3
O3 O1B 77
D B D B AC 57
DE 75
BE 50
BF 115
E E
F F
Fig. 29.6. Leg simplified assembly drawing and kinematical sketch

links (Fig. 29.6) making thigh (O1 B) and shin (BF ). The servos are
fixed to the trunk. The servo shafts (O2 and O3 ) are attached to the
two arms (O2 C, O3 D) fixed to the sliders CA and DE. The sliders are
attached to the main leg’s thigh and shin making planar revolute pairs.
All parts were produced from using bend aluminium sheet. The foot
is just a properly shaped end of the shin. BAJTEK has four kinemati-
cally identical legs, connected to the main body (Fig. 29.7). The body
is a rigid element, made of aluminum. The total mass of the machine
is close to 1 kg.
5 21
14 5
50
175
0
17
Fig. 29.7. Quadrupled BAJTEK
29.3 New Control System
For the BAJTEK robot a control system different than the discussed
above was elaborated [6]. This system is not dedicated to the experi-
mental gait synthesis but makes a basis for advanced motion control.
The control system uses the onboard micro-controller Atmel AtMega32.
The software is written in the assembly language. The gait generator
takes data from the gait pattern memory (EPROM). It is also possible
to connect an external computer (e.g. with a path planner) to evaluate
the gait sequences and adequate reference signals for the servomotors.
The gait generator sends to the joint controllers information about joint
positions and time intervals to achieve them (this contains information
about the motor speed). The joint controller doing the speed profiling
calculates the set of intermediate positions which are transmitted to the
servo-motors. In the future it is intended to connect some sensors to
the micro-controller for the reactive control. The rotation angle of the
servo shaft is proportional to the length of the input pulse (0.5-2.5 ms).
The period of the base pulse is 20 ms. Pulse generation makes use of the
micro-controller’s timer. The control system can control up to 16 ser-

vos simultaneously. From the upper level of control system the built-in
feedback in servomotor is invisible. Typically the motor shaft rotates
with maximum speed (in technical specification it is equal to 5.5 rad/s),
however the real speed depends on the load torque and supplied volt-
age. When synchronizing the leg displacement the motor speed control
is necessary, therefore we elaborated a special method of speed control.
With this scheme the control system calculates intermediate positions
assuming that each of them will be realized in a constant period equal
to 20 ms. By appropriate modification of the intermediate position re-
alized in that period the motor speed control is achieved. During the
gait at least three legs keep the machine trunk at a constant height.
Only one leg a time is transferred. There are short periods when the
body is supported by all legs. During four-leg supporting phases the
trunk moves forward following the straight line trajectory – this results
in the machine displacement. Besides saving the gait reference data in
internal memory it is also possible to send the gait pattern using the
RS-232 interface and an external computer.
29.4 Experiments
The reference pattern for revolute speed is created by changes of in-

termediate positions which are sent to servomotors every 20 ms. In our
experiments we observed and recorded the realized motion with and
without speed control. The real angular velocity was measured by mon-
itoring the voltage drop on the potentiometer connected to the motor
shaft. The measured value was transferred by the analog-digital con-
verter of the micro-controller. The angular velocity was calculated as a
derivative of the angular positions. Figure 29.8 illustrates such recorded
velocity without speed control. It was noticed that the speed varies with
high frequency. Theoretically it would produce joint oscillations, but in
the mechanical structure they were invisible due to mechanical damp-
ing. It is possible to observe and measure those oscillations only when
motion is performed without any load. Charts of changes of angular
position (for motion range equal to 60◦ ) are given in Fig. 29.9. We can
observe (Fig. 29.9) the step-like shape. This is well visible for motion
time longer than 0.19 s. The steps are caused by the short movements
separated by stops. The high-frequency components visible in the chart
are caused by the disturbances in measurement system and by the fac-
tors related to numerical processing. The leg end trajectory is shown in
Fig. 29.10. It is expressed in the coordinate frame attached to the trunk
10
10 Given time of moving: 0.3 s
Servo speed (rad/s)

Given time of moving: 0.19 s
Servo speed (rad/s)
5
5
0 0
0.1 0.2 0.3 0.1 0.2 0.3 0.4
-5 -5
Time (s) Time (s)
Fig. 29.8. Charts of servo motor angular velocity for motion range 60◦ (no
speed control)
60
0.19s 60
50
50
0.3s 1s 2s
Servo position (°)
40 40
y (mm)
30 30
20
20
10
10
0
0 20 40 60 80 100
0 x (mm)
0.5 1 1.5 2
Time (s)
Fig. 29.9. Chart of servo positions for Fig. 29.10. Leg end trajectory
different motion time (motion range –
60◦ ) – speed control
at the point of the hip joint (Fig 29.6). During the transfer phase the
leg-end is quickly raised up, then moved forward, and finally supported
– placed on the level of the other leg-ends. The duration of the leg
transfer takes 12.5% of the gait period. Therefore the walking machine
realizes gait called quadruped crawl (duty factor β = 0.875 [12, 13]).
When one leg is in the transfer phase, the other legs do not move. This
motion scheme results from the requirement of postural stability (the
masses in motion cause the inertial forces).
29.5 Conclusions
The work on the educational robots is still in progress; the inclusion

of simple but effective sensors and development of motion abilities are
planned. The hexapod MENTOR with 12 degrees of freedom is an ed-
ucational research platform for the development of multi-tasking real-
time control system. For BAJTEK the postural stability conditions are
more critical. For statically stable gait only one leg a time can be trans-
ferred. With the transfer of two legs (on the diagonal) the body must
be dynamically balanced considering the mass distribution and iner-
tial forces. BAJTEK makes an experimental platform for dynamical
motion synthesis. Considering dynamical stability students can syn-
thesize the two-legged gait. Manual control of each degree of freedom
in the first control system of quadruped BAJTEK allows to test the leg
movements before its final implementation. Our students synthesize the
motion programs for the new gaits and leg trajectories. By giving the
joints’ angular position for assumed leg-end trajectories they prove the
correctness of the inverse kinematics problem solutions they elaborate
(for a pantograph mechanism of the quadruped BAJTEK). The robots
are used in periodic robotic exhibitions organized in Warsaw.
References
1. Bai S, Low KH, Zielińska T (1999) Quadruped free gait generation for
straight-line and circular trajectories. Adv. Rob. vol.13, no.5:513–538
2. Bai S, Low KH (2001) Terrain evaluation and its application to path
planning for walking machines. Adv. Rob. vol.15, no.7:729–748
3. Berns K (2007) Walking Machines. (http://www.walking-machines.org)
4. Birch MC et al. (2002) Cricket-based robots. Introducing an autonomous
hybrid microrobot propelled ly Legs and supported by legs and wheels.
IEEE Rob. and Autom. Mag., vol.9, no.4:20–30
5. Gonzalez de Santos P et al. (2002) SIL04: A true walking robot for the
comparative study of walking machines techniques. IEEE Rob. and Au-
tom. Mag., vol.10, no.4:23–32
6. Jańczyk L (2007) Microprocesor based control system for quadruped.
B.Eng. diploma thesis, WUT, Warsaw
7. Lee H et al. (2006) Quadruped robot obstacle negotiation via reinforce-
ment learning. In: Proc. of the 2006 IEEE ICRA
8. Lewis MA (2002) Gait Adaptation in a Quadruped Robot. Autonomous
Robots, vol. 12, no. 3:301–312
9. Metrak D (2004) Project of hexapod. B.Eng. dipl. thesis, WUT, Warsaw
10. Pawlak T (2004) Project of mechanical and control system for quadruped.
B.Eng. dipl. thesis, WUT, Warsaw
11. Sgorbissa A, Arkin RC (2003) Local navigation stategies for a team of
robots. Robotica 21:461–473
12. Shin-Min S, Waldron KJ (1989) Machines that Walk. The MIT Press
13. Todd DJ (1985) Walking Machines: An introduction to legged robots.
Kogan Page
14. Zielińska T, Heng KH (2003) Mechanical design of multifunctional
quadruped. Mechanism and Machine Theory. vol. 38, no. 5:463–478
30
Humanoid Binaural Sound Tracking Using
Kalman Filtering and HRTFs∗
Fakheredine Keyrouz1 , Klaus Diepold1 , and Shady Keyrouz2

1
Technische Universität München, 80290 Münich, Germany
{keyrouz,kldi}@tum.de
2
Notre Dame University, Zouk Mosbeh, Lebanon
sbkeyrouz@ndu.edu.lb
30.1 Introduction
Audio and visual perceptions are two crucial elements in the design
of mobile humanoids. In unknown and uncontrolled environments, hu-
manoids are supposed to navigate safely and to explore their surround-
ings autonomously. While robotic visual perception, e.g. stereo vision,
has significantly evolved during the last few years, robotic audio per-
ception, especially binaural audition, is still in its early stages. However,
non-binaural sound source localization methods based on multiple mi-
crophone arrays, like multiple signal classification and estimation of
Time-Delay Of Arrivals (TDOAs) between all microphone pairs, have
been very actively explored [1], [2].
From the viewpoint of humanoid robotics, multiple microphone ar-
rays are undesired since processing times are dramatically increased.
Instead, minimum-power consumption and real-time performance are
two very required criteria in the design of human-like binaural hearing
systems. These systems are normally inspired by the solutions devel-
oped in evolution to solve general tasks in acoustic communication
across diverse groups of species such as mammals and insects. These
species adapt their acoustic biological organs to real-time communica-
tion while maintaining an optimum level of energy consumption.
Hence, interacting robots sending and receiving acoustical informa-
tion are intended to optimize the energy consumption of the sound
signal generation processh and to tailor the signal characteristics to
the transmission channel. Equally important, these robots are to be
∗
This work is fully supported by the German Research Foundation (DFG) within
the collaborative research center SFB453 “High-Fidelity Telepresence and Tele-
action”.
springerlink.com
330 F. Keyrouz, K. Diepold, and S. Keyrouz
equipped with elaborate mechanisms for segregating the signals from

separate sources and for analyzing signal characteristics.
The human hearing organ has, throughout evolution, developed
elaborate mechanisms for segregating and analyzing sound signals from
separate sources in such a way that, under daily-life adverse conditions,
robustness and low sensitivity with respect to varying external and in-
ternal working conditions are intuitively and automatically ensured.
The human hearing organ is usually regarded as a signal conditioner
and preprocessor which stimulates the central nervous system [3]; it
provides astonishing signal processing facilities. It comprises mechanic,
acoustic, hydro-acoustic, and electric components, which, in total, re-
alize a sensitive receiver and high-resolution spectral analyzer. The hu-
man head and pinnae together form a complex direction-dependent
filter. The filtering action is often characterized by measuring the spec-
trum of the sound source and the spectrum of the sound reaching the
eardrum. The ratio of these two is called the Head Related Transfer
Function (HRTF) or equivalently the Head Related Impulse Response
(HRIR). The HRTF shows a complex pattern of peaks and dips which
varies systematically with the direction of the sound source relative to
the head and which is unique for each direction. Recently, a method to
robustly extract the frequencies of the pinnae spectral notches from the
measured HRIR, distinguishing them from other confounding features
has been properly devised [4]. The HRTF includes all linear properties
of the sound transmission and all proposed descriptors of localization
cues, most importantly: 1) interaural time difference (ITD), 2) interau-
ral intensity difference (IID), 3) monaural cues, 4) head rotation and
5) interaural coherence (IC).
In a similar fashion to the decoding process which the auditory
system undertakes when transforming the two one-dimensional signals
at the eardrums back into a three-dimensional space representation,
it has been suggested that robotics can benefit from the intelligence
encapsulated within the HRTFs to localize sound in free space [5], [6].
Motivated by the important role of the human pinnae to focus and
amplify sound, and since the HRTFs can also be interpreted as the
directivity characteristics of the two pinnae [3], a humanoid should
perform sound localization in three dimensional space using only two
microphones, two synthetic pinnae and a HRTF database.
The previously proposed system in [5] utilizes the effects of pinnae
and torso on the original sound signal in order to localize the sound
source in a simple matched filtering process. In this paper, the process-
ing time of the previously proposed algorithm is dramatically decreased
30 Humanoid Binaural Sound Tracking Using HRTFs 331
Robot Head
Microphones
1 1 1
H Li SL SR H Ri1
HRTFLi HRTFRi
1
SL H L1 H R11 SR
SL H L 12 H 1
R2 SR
…xcor …
…
…
1
SL H Li H Ri1 SR
…
…
SL H L 1710 H R 1710 SR
Maximum Filtering
Cross-correlation (Convolution)
Fig. 30.1. Flowchart of the sound localization algorithm as proposed in [5]
by reducing the number of convolution operations significantly, and by

properly integrating a Kalman filter. Parallel to the matched filtering
process, the filter estimates the location of a moving sound signal and
locates a region of interest (ROI) within the HRTF database, thus al-
lowing a quick search for the correct HRTF pair and, consequently, a
very fast tracking of the moving sound trajectory.
30.2 Previous Localization Technique
The main idea in the 3D sound detection algorithm suggested in [5] was
to first minimize the HRTFs and remove all redundancy. The resulting
truncated HRTFs are then used for localizing sound sources in the same
way the full HRTFs would be used. The algorithm relies on a straight-
forward matched filtering concept. We assume that we have received
the left and right signals of a sound source from a certain direction.
The received signal to each ear is, therefore, the original signal filtered
through the HRTF that corresponds to the given ear and direction.
Match filtering the received signals with the correct HRTF should
give back the original mono signal of the sound source. Although the
system has no information about what the sound source is, the result
of filtering the left received signal by the correct inverse left HRTFL
should be identical to the right received signal filtered by the correct
inverse right HRTFR .
In order to determine the direction from which the sound is arriv-
ing, the two signals must be filtered by the inverse of all of the HRTFs.
The inverse HRTFs that result in a pair of signals that closely resemble
each other should correspond to the direction of the sound source. This
is determined using a simple correlation function. The direction of the
sound source is assumed to be the HRTF pair with the highest correla-
tion. This method is illustrated in Fig. 30.1. Due to the computational
complexity of filtering in time domain, the algorithm is applied in the
frequency domain. The signals and reduced HRTFs are all converted
using the Fast Fourier Transform (FFT). This changes all the filtering
operations to simple array multiplications and divisions.
We assume that the reduced HRTFs, i.e. using Diffused-Field Equal-
ization (DFE) [7] and Balanced Model Truncation (BMT) [8], have al-
ready been calculated and saved. Once the audio samples are received
by the left and right inputs, they are transformed using FFT. Then
the transformed left and right signals are divided (or multiplied by a
pre-calculated inverse) by each of the HRTFs. Finally, the correlation
of each pair from the left and right is calculated. There are 1420 array
multiplications, 1420 inverse Fourier transforms, and 710 correlation
operations. After the correlations are found, the direction that corre-
sponds to the maximum correlation value is taken to be the direction
from which the sound is arriving.
30.3 Enhanced Localization Algorithm
In the previous algorithm the main goal was to pass the received signal
through all possible inverse filters. The set of filters from the correct di-
rection would result in canceling the effects of the HRTF and extracting
the original signal from both sides. However, a more direct approach
can be taken to localize a sound source. Instead of attempting to re-
trieve it, discarding the original signal from the received inputs so that
only the HRTFs are left may be possible; this is illustrated in Fig.
30.2. We shall call such an approach the Source Cancelation Algorithm
(SCA).
Basically, the received signals at the microphones inside the ear
canals could be reasonably modeled as the original sound source signal
Robot Head
Microphones
Region of Interest
(ROI)
SL H Li HRTFLi
SR
H Ri HRTFRi
H L1
H R1
… xcor …
SL H Li
SR H Ri
H L 710
H R 710
Maximum
Cross-correlation
Fig. 30.2. Flow chart of the Source Cancelation Algorithm (SCA)
convolved by the HRTF. Looking at the signals in the frequency do-
main, we see that if we divide the left and right transformed signals, we
are left with the left and right HRTFs divided by each other. The sound
source is canceled out. Likewise, the SCA depends only on the correla-
tion factor between the incoming and saved HRTFs ratios. Hence SCA
outperforms the previously proposed method as it is independent of
the characteristics of the impinging sound sources on the artificial ears
and torso, which ensures more stability and more tolerability to noise
and reverberations.
With two Fourier transforms and one array division operation, the
original signal is removed and the HRTFs are isolated. The resulting
ratio can then be compared to the ratios of HRTFs which are stored in
the system. These ratios are assumed to be pre-calculated off-line and
saved in the system database.
In a hardware-based application, using the SCA would greatly re-
duce hardware complexity as well as speed up processing. Compared to
the original algorithm, this new approach eliminates 1420 array multi-
plications and 1420 inverse Fourier transforms, and replaces them with
one single array multiplication.
30.4 Kalman Filtering and ROI Extraction
Our sound localization technique has access to a generic KEMAR

HRTF database [9] collecting 710 HRTFs each of order 512. These full-
length HRTFs can be modeled as FIR filters. These filters, denoted
by HF512
IR , are reduced to 128-order using the DFE model-truncation
method. The obtained DFE-reduced dataset shall be denoted as HF128 IR .
To make the processing time even faster, this DFE-reduced HRTF data-
base is further truncated using BMT. The BMT-reduced HRTFs can
be modeled as IIR filters, and are denoted here by HIIR m , where for
every value of m we have a truncated HRTF dataset of different order.
Although by applying appropriate reduction techniques, the length
of the impulse responses can be reduced to a hundred or even fewer
samples, thus reducing the overall localization time, the HRTF data-
base could be very dense, and thus the convolution with all possible
HRTFs in the database becomes computationally exhaustive. To solve
this problem, especially for moving sound sources, a Kalman filter is
tailored to predict a ROI the sound source might be heading to, accord-
ing to some movement models. Therefore a quick search for the correct
HRTF pair within a small ROI is now ensured, and, consequently, a
very fast tracking of the moving sound trajectory. The workflow of the
SCA attached to a Kalman filter is depicted in Fig. 30.3.
The Kalman filter applies to a linear dynamical system, the state
space model of which consists of two equations:
xk+1 = Axk + Buk + w, (30.1)

yk+1 = Cxk+1 + v. (30.2)
Equations (1) and (2) are called the process and the measurement equa-
tions, respectively. The variable x is the state of the discrete-time sys-
tem and y is the system’s output such as positions or angles depending
on the movement model. The variable u models the sound source veloc-
ity. The random variable w and v represent the white Gaussian process
and measurement noise, respectively. The matrix A relates the state
at the current time step k to the state at the future step k + 1. The
matrix B relates the optional control input u to the state x. The matrix
C relates the state to the measurement yk+1 .
The linear model we have adopted corresponds to a sound source
moving with constant velocity. This velocity is incorporated within the
state vector, x(k) = (x, y, z, ẋ, ẏ, ż)T , by taking the derivative with
dy
respect to time, ẋ = dx dz
dt , ẏ = dt , and ż = dt . According to the move-
ment equations, based on Newton, the sampling time T for each coor-
dinate Λ ∈ {x, y, z} is calculated for the transition from Tk to Tk+1 :

Λk+1 = Λk + T Λ̇k and Λ̇k+1 = Λ̇k .
The Cartesian coordinates, provided by the Kalman filter using the
above-mentioned model, are transformed to spherical coordinates to
stay compatible with the output of the SCA in Fig. 30.3.
x
Movement y
Kalman Filter
Model
z
Estimated x
Estimated y
Estimated z
Coordinate Transformation
Predicted azimuth Predicted elevation
Creation of ROI
in HRTF dataset
ROI
Sound signal Azimuth
SCA Elevation
Fig. 30.3. Flowchart of the Source Cancelation Algorithm (SCA) using a

ROI
30.5 Simulation and Experimental Results
In the first part of the simulation and experimental results, we compare

the new SCA algorithm with the one proposed in [5] by testing its
localization performance for stationary sound sources. In the second
part, we cover the case where the sound source starts moving, and we
analyze the effect of the ROI size on the tracking algorithm processing
time.
30.5.1 Stationary Sound Sources
The Matlab simulation test, done using a 2 GHz processor with 1 GB

RAM, consisted of having a broadband sound signal filtered by appro-
priate 512-sample KEMAR HRTFs at a certain azimuth and elevation.
Thus the test signal was virtually synthesized using the original full-
length HRTF set. For the test signal synthesis, a total of 100 random
HRTFs were used corresponding to 100 different random source loca-

tions in the 3D space. In order to ensure rapid localization of multiple
sources, small parts of the filtered left and right signals are consid-
ered (350msec). These signal parts are then transformed using FFT
and divided. The division result is correlated with the available 710
reduced HRTF ratios. Basically, the correlation should yield a maxi-
mum value when the saved HRTF ratio corresponds to the location
from which the simulated sound source is originating. The reduction
techniques, namely DFE and BMT, were used to create two different
reduced models of the original HRTFs. The performance of each of
these models is illustrated in Fig. 30.4. The circled solid line and the
star sign show the SCA percentage of correct localization versus the
length of the HRTFs in samples. For comparison, the squared dashed
line and the plus sign refer to the performance of the previous method.
The solid line and the circle denote the experimental results and shall
be discussed shortly.
Using the diffused-field equalized 128-sample HRTF set, HF128 IR , the
SCA simulated percentage of correct localization is around 97%. This

means that out of 100 locations, 97 where detected by our SCA al-
gorithm compared to 96% for the previous method. Using the BMT-
reduced set, HFmIR , the SCA localization percentage falls between 59%
and 93% compared to between 53% and 92% for the previous method,
with the HRTF being within 10 to 45 samples, i.e. 10 6 m 6 45. It should
be noted that, while using 35 BMT-reduced HRTFs, all the falsely lo-
calized angles fall within the close neighborhood of the simulated sound
source locations.
A plot reporting how far, on average, are the falsely localized angles
from their target location, can be seen in Fig. 30.5. Intuitively, with
more HRIR samples, the average distance to the target sound source
location decreases. Note that the minimum distance to the target posi-
tion is 5◦ , and this is due to the fact that the minimum angle between
the HRTFs of the database we are using is 5◦ . Obviously, if we use a
densely sampled database, e.g. with a HRTF every 1◦ , the average dis-
tance to the target sound locations is expected to notably decrease. In
our household experimental setup, 100 binaural recordings from differ-
ent directions were obtained using a dummy head and torso with two
artificial ears in a reverberant room. The microphones were placed at
a distance of 26mm away from the ear’s opening. The recorded sound
signals, also containing external and electronic noise, were used as in-
puts to our SCA localization algorithm. The localization accuracy is
illustrated by the solid line and the circle in Fig. 30.4.
100
Percentage of Correct Localization (%)

90
80
BMT H IIR
m
SCA
70
Simulation FIR
* DFE H128
Previous
60 BMT H IIR
m
Method
Simulation + FIR
DFE H128
50
SCA BMT H IIR

m
40
Experimental FIR
DFE H128
30
0 20 40 60 80 100 120 140
HRIR filter Coefficients (Samples)
Fig. 30.4. Percentage of correct localization using SCA compared with the
method in [5]
70
65 Previous Method
SCA
Average Distance in [°] from the Target Angles
60
55
50
45
40
35
30
25
20
15
10
5
0
0 20 40 60 80 100 120 140
HRIR Filter Coefficients (Samples)
Fig. 30.5. Falsely localized sound sources: average distance to their target
positions, for every HRIR length
It can be seen that the general performance of the SCA algorithm,

operating with laboratory measurements, dropped down compared to
the simulation results. This is principally due to the noise and rever-
berations contained within the measured sound signals, which were not
included in the simulation. The deviation in performance is also justi-
fied on the basis of the differences between the dummy manikin model
used in the experiment and the KEMAR model used to obtain the
HRTF database.
30.5.2 Moving Sound Sources
In our second experimental setup, several binaural recordings of a

broadband sound source were taken. In every recording, the source is
moving at 10deg/sec, in the zero-elevation plain, and is following a cir-
cular trajectory 2 meters away from the humanoid head located in the
center. The recorded sound signals, also containing external and elec-
tronic noise, were used as inputs to our sound localization algorithm.
The sound localization algorithm depicted in Fig. 30.1 initializes by
0.12
0.1
Localization Time in [s]
0.08
0.06
0.04
0.02
0
20 40 60 80 100 120 140 160 180
ROI Length in [°]
Fig. 30.6. SCA processing time for different ROI intervals
searching the 710 HRTFs, looking for the starting position of the sound
source. Once the initial position is pinpointed, an initial ROI is localized
around this position. Next, the source starts moving, and a new ROI is
identified and automatically updated using a Kalman filter. This way,
the sound localization operation, particularly the correlation process,
proceeds only in this ROI instead of searching the whole HRTF dataset.
Thus a considerable and dispensable computation time is avoided. The

size of the generated ROI depends on the sound source’s velocity and
acceleration and on the model’s sampling time. Obviously, the faster
the source moves, and the greater the sampling time is, the bigger
the ROI area becomes. To evaluate the performance of the new sound
tracking setup, we set the SCA algorithm to work with BMT-reduced
HRTFs of order 45. We measured the total time taken by the tracking
algorithm to localize the source at one instant in time, for several ROI
lengths.
Without a ROI, the SCA runs through all 710 HRTFs and requires
an average processing time of 0.741 secs to detect the location of a sound
at a certain location. This localization time dropped down to 0.052 sec
on average by using a ROI 50◦ long in azimuth. Further processing-
time results, corresponding to diverse ROI lengths, are depicted in Fig.
30.6.
Using a simple Kalman filter for predicting appropriate ROIs, we
attained a processing-time reduction of more than 90% as a result of less
convolution and correlation operations. These computation reductions
ensure a very quick tracking behavior.
30.6 Conclusion
Addressing the robotic binaural sound source localization problem, we

have proposed an efficient sound source localization method using a
generic HRTF dataset and two microphones inserted inside the artificial
ears of a humanoid head mounted on a torso.
Compared to the previously proposed binaural sound localization
method [5], the enhanced SCA algorithm is able to achieve significant
reduction in the processing requirements while still slightly increasing
the accuracy of the sound localization. Furthermore, with the help of a
simple Kalman filter, a ROI was introduced to account for fast moving
sound sources. Simulation and experimental results showed real-time
tracking performance. The efficiency of the new algorithm suggests a
cost-effective implementation for robot platforms and allows fast local-
ization of moving sound sources. The algorithm is to be extended to
allow the robot to locate and follow moving sound sources in noisy and
reverberant environments and to account for concurrent sources. Based
on the ideas presented in this paper, many venues for future work are
to be considered. We will mainly address the robotic monaural sound
localization, and the range estimation problem.
References
1. Brandstein M (1999) Time-delay estimation of reverberated speech ex-

ploiting harmonic structure. J Acoust Soc Am 105:2914–2919
2. Jahromi O, Aarabi P (2005) Theory and Design of Multirate Sensor Ar-
rays. IEEE Trans on Signal Processing 53:1739–1753
3. Blauert (1997) An Introduction to Binaural Technology. Binaural and
Spatial Hearing. R. Gilkey, T. Anderson, Eds., USA-Hilldale NJ, 593–
609
4. Raykara VC, Duraiswamib R (2005) Extracting the frequencies of the
pinna spectral notches in measured head related impulse responses. J
Acoust Soc Am 118:364–374
5. Keyrouz F, Naous Y, Diepold K (2006) A new method for binaural 3D
localization based on HRTFs. Proc IEEE Int Conf on Acoustics, Speech,
and Signal Processing (ICASSP) 341–344
6. Keyrouz F, Diepold K, Dewilde P (2006) Robust 3D Robotic Sound Lo-
calization Using State Space HRTF Inversion. Proc IEEE Int Conf on
Robotics and Biomimetics (ROBIO) 245–250
7. Moeller H (1992) Fundamentals of Binaural Technology. Appl Acoust
36:171–218
8. Beliczynski B, Kale I, Cain GD (1997) Low-order modeling of head-
related transfer functions using balanced model truncation. Proc IEEE
Trans Signal Processing 3:532–542
9. Gardner WG, Martin KD (1997) HRTF Measurements of a KEMAR. J
Acoust Soc Am 97:3907–3908
31
Mechatronics of the Humanoid Robot ROMAN
Krzysztof Mianowski1 , Norbert Schmitz2 , and Karsten Berns2

1
Institute of Aeronautics and Applied Mechanics, Faculty of Power and
Aeronautical Engineering, Warsaw University of Technology, Nowowiejska
str. 24, 00-665 Warsaw, Poland
kmianowski@meil.pw.edu.pl
2
Department of Computer Science, Robotics Group, University of
Kaiserslautern, Gottlieb-Daimler-Strasse, 67663 Kaiserslautern, Germany
{nschmitz,berns}@informatik.uni-kl.de
31.1 Introduction
Recent developments in the area of service robotics show an increasing

interest in personal robots. Those personal robots can help to handle
daily work and to entertain people. Both tasks require a robot that
is able to communicate with people in a natural way. For these tasks
non-verbal motions like gestures, mimic, and body pose are more and
more important. Several anthropomorphic robots like [1], [6], [8], and
[7] have been build in recent years. Although several goals could be
reached humanoid robots are not able to communicate with humans in
a natural way.
Our concept of non-verbal interaction is mainly based on FACS [4],
which consequently describes the motions of the skin, eyes, and neck.
The results of FACS are extended with information concerning body
pose and the influence on man-machine communication [5]. Based on
these research results we present the mechanical design and construc-
tion of upper body, eyes, and neck for the humanoid robot ROMAN.
The mechanical design is an extension of the previously build emo-
tional humanoid head with 3DOF neck construction ([3], [2]). Several
experiments with the previously build head revealed the importance of
additional expressive motions to realize a more natural man-machine
communication.
springerlink.com
342 K. Mianowski, N. Schmitz, and K. Berns
31.2 The Humanoid Robot ROMAN
The mechanical design of the humanoid robot is adapted to Ekmans

[4] and Givens [5] work. The possibility to express emotions is therefore
mainly based on the availability of the so called action units which have
to be combined to express a specific emotional state. Table 31.1 lists
those action units which are very important and can be realized with
the humanoid robot ROMAN. These motions include the movements
of the artificial skin, the neck, and the eyes. The movements of the
upper body are not listed as action units so we decided to realize them
similar to the natural human upper body motions. Figure 31.1(a) shows
an image of the engineering construction of the humanoid robot while
the silicon skin can be seen in Fig. 31.1(b).
Table 31.1. List of all realized action units corresponding to Ekmans num-
bering system including the approximated maximum ranges of motion
Skin Motions Head Motions Eye Motions
1 Inner Brow Raise 1 cm 51 Turn Left 60◦ 61 Eyes Left 30◦
2 Outer Brow Raise 1 cm 52 Turn Right 60◦ 62 Eyes Right 30◦
9 Nose Wrinkle 1 cm 53 Head Up 20◦ 63 Eyes Up 40◦
12 Lip Corner Puller 1 cm 54 Head Down 20◦ 64 Eyes Down 40◦
15 Lip Corner Depressor 1 cm 55 Tilt Left 30◦ 65 Walleye 30◦
20 Lip Stretch 1 cm 56 Tilt Right 30◦ 66 Crosseye 30◦
24 Lip Presser 1 cm 57 Forward 2 cm
26 Yaw Drop 10◦ 58 Back 2 cm
Fig. 31.1. (a) Engineering drawing of the upper body, the neck, and the
head construction included in Zygote’s 3D realistic human model (b) A pre-
vious version of the humanoid robot “ROMAN” (ROMAN = RObot huMan
interAction machiNe) with human-like silicon skin
31 Mechatronics of the Humanoid Robot ROMAN 343
31.3 Design Concept and Construction
Based on the insights of the previous chapter we present the mechanical

design of upper body, the artificial eyes, the neck, and the interior frame
of the robot.
31.3.1 Upper Body
The movements of the human spine will be approximated with three

degrees of freedom in relation to the base in an open kinematic chain.
The kinematic scheme of the lumbar spine is similar to the one used in
the design of the neck, while ranges of motion should be appropriate to
the functions of the spine. The ranges of motion include rotation over
the vertical axis (±60◦ ), inclination forward/backward in relation to
the horizontal axis (−30◦ and +40◦ ) and inclination left/right in the
frontal plane (±30◦ ). Figure 31.2(a) shows the engineering drawing of
the upper body.
It was assumed that two main boards will be located on the chest
in the front and in the back side. For the synthesis and recognition of
speech a four-channel sound card and a loudspeaker in the front of the
body will be applied. For the protection of the equipment the chest has
to be adequately stable and safe, so the basic design of the chest has
the form of a stiff box with artificial ribs for protection. The chest as a
mechanical part should be adequately resistant to transfer gravitational
and external moments/forces acting to the head and to the upper limbs
(arms, hands), and should be relatively lightweight. It was decided to
use the typical shell-shaped design for the box with planar walls made
as a bent of tick plates and welded duraluminium.
The mechanism (see Fig. 31.2(a)) consists of the base, the rotational
fork 1 driven by an electric motor with gear, a special cross yoke bearing
by a ball bearing in the fork 1 for inclination forward-backward and
the fork 2 for inclination left-right side bearing by a ball bearing in the
cross yoke. Driving forces (torques) are generated by electric motors
with gears chosen in such a way that typical compact solution of the
motor with planetary gear is mounted to the fork (1, 2) and each one
propels by small toothed wheel the big one mounted to the cross yoke.
The total mass of the upper body including the arms and the head is
estimated as up to 50 kg. To partially compensate the external torque
over horizontal axes produced by gravitational forces, in the proposed
solution there have been applied special additional bridges attached
outside the cross yoke on each inclination axis with elastic elements for
compensating changes of potential energy, i.e. external springs which

can be seen in Fig. 31.2(a).
Motor Dimensioning
Mext (α) = m · g · R · sin(α) (31.1)

Fspring1 (α) = 2 · k1 · (l1 (α) − l1min ) (31.2)
Mspring1 = −Fspring1 · d1 (α) (31.3)
Mspring = Mspring1 + Mspring2 (31.4)
Mresult = Mext + Mspring (31.5)
The dimensioning to the motors is mainly dependent on the necessary

torque. Figure 31.3(a) shows a sketch of the forward-backward bend-
ing joint which will be used to calculate the necessary motor torque.
The static external torque Mext (α) (see Eq. 31.1) is dependent on the
mass of the humanoid robot. We assume an overall mass m = 50 kg
(including the upper body construction with about 15 kg) at a distance
R = 0.4 m with a gravitational acceleration g = 9.81 m/s2 . The force
created by the backside springs is dependent on the spring constants
k1 = 1637.9 and l1min = 0.113 m as well as the current length of the
spring l1 (α) (see Eq. 31.2). The length l1 (α) can easily be calculated
with the help of geometry. The force Fspring1 (α) is multiplied with two
since two identical springs are mounted on each side. Equation (31.3)
Fig. 31.2. (a) Engineering drawing of the upper body of the humanoid robot
with integrated main boards, loudspeaker and the 3DOF hip with supporting
springs (b) This image shows the assembled head and neck including the
fourth neck joint (c) Mounted eye construction from the backside and (d) the
frontal view with artificial skeleton
R
r1
α
h1
Fig. 31.3. (a) This sketch of the forward-backward bending motion is used
to calculate the external and resulting torque in the rotational center (b) The
upper diagram shows the external Mexternal and the resulting torque Mresult
and the lower diagram shows the torque generated by the backside (Mspring1 )
and front side springs (Mspring2 )
shows the generated torque, which is only dependent on the gener-

ated force and the orthogonal distance d1 (α) to the rotational center.
Since the construction is not symmetrical a second torque generated
by the springs on the front side of the construction has to be calcu-
lated in a similar way. The spring-generated torque Mspring (see Eq.
31.4) can than be used to calculate the resulting torque Mresult as
shown in Eq. (31.5). The maximum external torque of 126.11 kgm2 /s2
could be reduced with the help of the springs to 79.44 kgm2 /s2 . Fig-
ure 31.3(b) shows the previous and the resulting torque as well as the
spring-generated torques dependent on the angle α. From the analysis
it is shown that the driving system (the motor with planetary gear and
one step wheel gear) should generate about 60 − 65% of the maximum
value of external gravitational torque.
31.3.2 Artificial Eyes

The construction of the eyes must be compact and lightweight since
space is limited and we must be able to move the eyeballs indepen-
dently up/down and left/right. The upper eyelid has to be movable to
assist the expression of emotions. Additionally a small camera has to
be integrated in the eyeball which is connected to exterior electronic
parts with a flexible cable.
The eyeball shown in Fig. 31.2(c) slides in a spherical attachment
and can be moved with 2 pins in the rear part of the eye. Each pin is
directly connected to a gearwheel and a stepper motor to allow high

positioning accuracy. The third gearwheel is used to move the upper
eyelid. In comparison to the human eyeball, which has a diameter of
23 mm to 29 mm, the eyeball of the robot has a diameter of 46 mm.
We increased the size to include the lightweight camera (12 g) and get
an iris with a diameter of 16 mm which is necessary for the camera.
The complete construction of a single eye has a weight of 150 g includ-
ing the motors and gears. The eyeball alone has a weight of 38 g and
can be moved with a maximum torque of 126 mNm (vertical axis) and
136 mNm (horizontal axis). The eyes are able to move ±40◦ in horizon-
tal and ±30◦ in vertical direction and can be moved from left to right
or top to bottom in about 0.5 s. The eyelid can be moved 70◦ up- and
10◦ downwards from the initial horizontal position, while a blink of an
eye can be realized in 0.4 s.
31.3.3 Neck
The formerly 3DOF design of the neck must be extended to a 4DOF

design to realize motions like nodding, which is important for non-
verbal communication. The axis of the fourth joint is located next to the
center of the head to realize a rotation along the heads pitch-axis. The
connection between the neck and the fourth joint is build of aluminum,
which has increased stiffness in contrast to POM (Polyoxymethylene).
The design of this connection is shifted to the rear of the head to form
some free space which is necessary for head rotation. The rotation is
assisted by two springs with a maximum force of 27 N each since the
center of gravity is placed in the front part of the head. The motor
is able to generate a torque of 1230 mNm to move the head with all
actuators and sensors. Figure 31.2(b) shows the construction of the
fourth neck joint including the gearwheel and the motor. Additional
information concerning the neck can be found in [2].
31.4 Robot Control Architecture
The actuator system of the robot consists of 21 different motors includ-

ing seven dc-, six stepper- and eleven servo-motors.They are connected
to the digital signal processor unit (circuit board) which consists of
a DSP (Freescale 56F803) cooperating with a logic device (CPLD –
Altera EPM70256). Additionally there are current supply elements,
CAN-bus elements, and amplifiers for the motors. This digital signal
processing unit is able to preprocess different types of encoders and
control up to two dc motors, three stepper motors (via a TMC428

stepper motor controller) and six servo motors. Several DSP units are
connected via CAN-bus to an embedded PC which is placed inside the
robot case.
Fig. 31.4. Controller structure for one joint
The controller for the dc-motor has been implemented as presented

in Figure 31.4. The controller itself consists of a cascaded closed loop
velocity controller and a closed loop position controller. The velocity
controller is an advanced PI-Controller, where the proportional value
for desired and actual inputs are separated. This type is called ReDus
unit and has the advantage that only via the proportional element fric-
tion of the motors can be compensated and the desired input can be
reached without having a difference to the actual input. The integral el-
ement is only responsible for compensating external forces to the joint.
This concept makes the controller quicker and more stable. The su-
perposed position controller is combined with an anticipation element.
The known velocity of the position calculation is a direct input to the
speed controller. Differences in the position must be compensated by
the position controller. No difference between the position actual and
desired input must be build up to get a higher desired speed. The drag
distance between the desired and actual position value is shorter. This
type of controller supports different mounted positions of the velocity
and position sensor. Certainly this works only if the desired values from
the topper modules are correct and are available continuously.
The robot control on the personal computer is implemented with the
help of the Modular Controller Architecture (MCA). MCA is a modu-
lar, network transparent, and real-time capable C/C++ framework for
controlling robots. MCA is conceptually based on modules, groups, and
edges between them, which allow implementation of hierarchical struc-
tures. The tools mcagui and mcabrowser allow the interaction with the
system allowing the user to control and observe the robot.
31.5 Conclusion and Outlook

Based on psychological research results concerning interaction be-
tween humans we designed an emotional expressive humanoid robot.
Equipped with 24 degrees of freedom and a complex sensor system
the mechanical design allows complex interactions with humans in a
natural way.
It is also obvious that the integration of human-like arms will have
a great influence on the communication task, so these will be added
in near future. Based on this mechanical design it is necessary to ex-
tend the current behavior-based controlling architecture to realize first
natural dialogs.
References
1. T. Asfour, K. Berns, and R. Dillmann. The humanoid robot armar: Design
and control. In International Conference on Humanoid Robots (Humanoids
2000), 2000.
2. K. Berns, C. Hillenbrand, and K. Mianowski. The mechatonic design of
a human-like robot head. In 16-th CISM-IFToMM Symposium on Robot
Design, Dynamics, and Control (ROMANSY), 2006.
3. K. Berns and J. Hirth. Control of facial expressions of the humanoid robot
head roman. In IEEE/RSJ International Conference on Intelligent Robots
and Systems (IROS), Beijing, China, October 9-15 2006.
4. P. Ekman, W.V. Friesen, and J.C. Hager. Facial Action Coding System
(FACS) - Manual, 2002.
5. D. B. Givens. The Nonverbal Dictionary of Gestures, Signs and Body
Language Cues. Center for Nonverbal Studies Press, 2005.
6. S. Hashimoto. Humanoid robots in waseda univeristy -hadaly-2 and
wabian-. In IARP First International Workshop on Humanoid and Hu-
man Friendly Robotics, 1998.
7. I.-W. Park and J.-H. Oh. Mechanical design of humanoid robot platfrom
khr-3(kaist humanoid robot-3: Hubo). In IEEE-RAS International Con-
ference on Humanoid Robots (Humanoids), 2005.
8. M. Zecca, S. Roccella, M.C. Carrozza, G. Cappiello, and et. al. On the de-
velopment of the emotion expression humanoid robot we-4rii with rch-1. In
Proceeding of the 2004 IEEE-RAS International Conference on Humanoid
Robots (Humanoids), pages 235–252, Los Angeles, California, USA, No-
vember 10-12 2004.
32
Assistive Feeding Device for Physically
Handicapped Using Feedback Control
Rahul Pandhi1 and Sumit Khurana2

1
Department of Electronics and Communication Engineering, Delhi College
of Engineering, Delhi University, India
rahul.pandhi@gmail.com
2
Department of Manufacturing Processes and Automation Engineering,
Netaji Subhas Institute of Technology, Delhi University, India
sumitkhurana86@gmail.com
32.1 Introduction
People suffering from neuromuscular diseases have trouble lifting their

arms against gravity although a large number of them maintain sen-
sitivity and residual strengths in their hands. Therefore a device is
desired that enables them to assist their feeding movements. There are
commercially available feeders that are useful for people who have con-
trolled movements of the head and neck and can take food off of a
feeding utensil that is brought close to the mouth. Most feeders consist
of an articulated, electrically powered arm with a spoon at its end, a
plate on a rotating turntable and an auxiliary arm that may be used
to push food on to the spoon. The user controls, through the use of
switches, the movement of the different components. Although such
feeding aids can be used effectively, there are several reasons why their
use is not as widespread as one would expect. The control switches may
initiate a movement of a certain component, for example, a rotation of
the plate.
The user may find it frustrating not to stop the motion. Visual
feedback is required for successful feeding during most of the operation.
It is acceptable if vision is required to locate the morsel of food and
to target it. But requiring the user to focus on the morsel through
the entire operation of scooping it up and bringing to the mouth can
be very exhausting for most users. Some devices require an electrical
outlet and this may be a nuisance factor. Finally, they are expensive
and are difficult to adapt to individual needs.
springerlink.com
352 R. Pandhi and S. Khurana
In order to overcome the above constraints an assistive feeder device

which customizes itself according to the needs of the patient and is also
cheap at the same time is required.
For the same we suggest two intelligent assistive feeding devices [1].
The two proposed devices use a micro-controller for the controlling op-
eration and provide precise arm movement but differ in their modes of
installation. While one is fixed on a stationary platform, the other one
is mounted on a movable platform such as a wheel chair. These devices
level themselves automatically to compensate for uneven floors, a func-
tion that can be overruled to assist forward/backward motion of the
arm. Thus, the balancer can compensate for the weight of the arm and
be adjusted to generate force to a limited (safe) extent. These devices
automatically measure the size of the patients arm using intelligent
sensors and adjust themselves to provide free and natural movement of
the arm.
32.2 Upper Arm Orthotic – Mobile Mount

The device shown in Fig. 32.1 is a wheelchair mounted, counter
balanced, electro-mechanical arm. The arm’s end point is controlled
and/or powered by a functional body part of the user, via Bowden
cables and/or an electric motor. The system consists of four main com-
ponents:
’
• the ’slave’ arm unit,
• the master’ or interface unit,
• the transmission system,
• the control system.
Fig. 32.1. Slave arm unit CAD model

32 Assistive Feeding Device for Physically Handicapped 353
32.2.1 Slave Arm Unit
The user engages to the master unit, in this case through biting, and
moves his head to control the slave arm. A transmission and control
system connect the master to the slave so that the slave follows the
master in a natural and intuitive manner. Two of the degrees of free-
dom (d.o.f.) between the units are connected through pulleys and a
Bowden cable; the third coupled d.o.f. is through an electric motor and
controller.
The slave arm is shown in Fig. 32.1. All θ values represent angular
motion about a corresponding axis; all ρ values represent linear motion
along an axis. This unit is mounted to a wheelchair such that joint θsp
is at shoulder height and the unit is to the right side of the user. The
slave arm was mechanically designed with the same d.o.f. as a spherical
coordinate system: θsp (pitch), θsy (yaw), and ρs (radial). A spherical
coordinate system was chosen because it allows any position in space to
be obtained with variables (inputs) that kinematically match a person’s
input (in this case, head) and arm movements.
Pulleys are located at joints θsp and θsy which are connected, via
a cable, to the pulleys located on the master. A motor is present at
joint θsp ; this along with the two meshing gears fixed to the ends of
the two main links result in radial motion, ρs . The slave arm is counter
balanced so that the user does not feel its weight when static. Roller
bearings are present in all joints. The two main links are constructed
of fiberglass hollow tubing, while most other components are machined
from aluminum.
32.2.2 Master/Interface Unit
One of the interface or master units is detailed in Fig. 32.2. The unit is
fixed to the user’s wheelchair such that the θmy joint is positioned above
and approximately at the center of the user’s head. This head/mouth
interface unit has four d.o.f.; the three (θmp , θmy , θmρ ) that map to the
slave unit and an additional passive joint (θmr ) at the mouthpiece so the
master unit does not constrain the natural head movement of the user.
The mouthpiece is constructed of Polyform, a thermosetting plastic
allowing the user’s dentition to be molded at low temperature. Roller
bearings are present at the yaw (θmp ) and pitch (θmy ) joints. Pulleys
are present at the θmy and θmp joints, while a rotary potentiometer
is placed at θmρ . This potentiometer measures the translation of the
mouthpiece, ρm , which ultimately controls ρs .
Fig. 32.2. Head/Mouth master interface
32.2.3 Transmission System

Although not shown for the purpose of clarity, Bowden cables run be-
tween the master and slave units.
Two sets of Bowden cables connect the yaw and pitch pulleys of the
master and slave system; that is, θmp is connected to θsp and θmy is
connected to θsy . This set-up causes proportional (in this case, equal)
angles of rotation between the two units’ pitch and roll joints. The
Bowden cables are required since relative movement occurs between the
pulleys of the master and slave units. The Bowden cable is comprised
of a flexible outer housing, a flexible steel wire, and a polyethylene liner
that lies in-between the wire and outer housing to reduce friction.
The radial position of the slave, ρs , is achieved through closed loop
position control. An electric motor rotates joint θsρ such that the er-
ror between the input (master), θmρ , and the output (slave), θsρ , is
minimized.
External power (a motor) was used at this d.o.f. to allow the person’s
small translational head input motion to control the larger radial slave
motion while still maintaining adequate force capability at the distal
end of the slave.
32.2.4 Control System

The control system of the assistive device makes use of a micro-
controller to control the movements of the slave arm unit. A rotating
potentiometer is placed on the joint θsρ , which calibrates the move-
ment of the joint θmρ of the master unit. This potentiometer output
is then fed into an analog-to-digital converter (ADC) which gives a
digital output of the analog movement, which thereafter is fed to the
microcontroller. This microcontroller is used to drive the slave unit

through a stepper motor. Keeping in mind the constraints of a micro-
controller such as its memory size and its processing speed, the micro-
controller used for the experiments was Phillips 89C51RD2BN. This
micro-controller has 64kB of flash memory and 1kB of RAM as com-
pared to Atmel 89C51 which has 4kB of flash memory and 128 byte
of RAM. The micro-controller provides the necessary control signals
to the stepper motor in the slave unit so as to provide a precise arm
movement.
32.2.5 Advantages
The advantages of this system are its ability to provide extended phys-
iological proprioception (EPP) and force reflection and its simplicity
relative to rehabilitation robots. It is more versatile and allows a per-
son with no or very little arm function to interact with his surroundings.
The target population that would benefit from such a device is very
large because the basic ideas can be adapted to any special purpose
task (feeding is an example) and to other input sites (only head control
is discussed here).
32.3 Upper Arm Orthotic – Stationary Mount
In this intelligent device ankle, knee, and thigh movements are coupled
via a set of cables and pulleys to a four degree-of-freedom articulated
arm. By moving his or her leg in a controlled manner, the user can
feed effectively. Because it is physically and intimately coupled to the
user and acts as an extension of the person, this device has the flavor
of a prosthetic limb (except for the number of degrees of freedom) and
therefore the user is always in intimate contact with the limb. This
offers the user a form of proprioceptive feedback.
Because of this intimate physical contact, the user will always know
the position and orientation of the spoon and the articulated arm and
will only use vision to locate the target morsel. This device is simple,
reliable, and inexpensive and does not require actuators.
The prototype in Fig. 32.3 is a device that uses head and neck
movements to control movement of a spoon. The linkage has three
degrees of freedom and in particular is capable of three distinct output
motions.
It can be used to scoop up the food from the plate with any approach
angle and bring the food to the user’s mouth as he/she pitches his/her
Fig. 32.3. Intelligent upper arm orthotic (CAD model)
head forward. The mechanism has three degrees of freedom driven by

cables. The nominal yaw movement of the head causes the linkage to
rotate about a vertical axis and translate in a horizontal plane so that
the spoon is always in the line of sight of the user. The nominal pitch
movement of the head drives a planar open chain (whose joints are cou-
pled linearly) so that the spoon performs a planar motion that involves
scooping up the food and bringing it to the mouth. The nominal roll
movement causes the spoon to pitch about a transverse axis. The main
concern is the prototype in Fig. 32.3 has to be worn by a user and looks
like a mechanical aid. While this may not be a concern in a dining hall
or at home, it may not be socially acceptable.
32.4 Power-Assist in Human Worn Assistive Devices
The ideal power-assist mechanism acts as an amplifier while allowing

the human to remain in direct contact with the manipulator, thus en-
abling extended physiological proprioception. The basic underlying idea
is to use force sensors to infer the force applied by the human and the
force required for the manipulative task and supply the difference using
an electromechanical actuator. A variation of this idea can be used to
suppress tremor or spasticity. Since the force applied by the human is
sensed, it can be appropriately filtered before being used as an input
signal.
One of the main concerns that needs to be taken care is that the
human user must interact physically with an actively controlled system
and the control system must be designed so that the human-machine
system remains stable under all conditions. One way of approaching this
is by requiring that the electromechanical aid remain passive under all

conditions.
32.5 Virtual Prototyping
Virtual prototyping is the process of design, analysis, simulation, and

testing of a product within the computer, and using the results to re-
fine the concept and redesign the product before making a physical
prototype. Over the last decade, high speed computer graphics work-
stations have proven to be very effective in allowing visualization of
three-dimensional complex systems. With advances in robotics tech-
nology, the potential for developing haptic interfaces that allow the
user to feel forces exerted by the virtual environment (in addition to
seeing the environment) has been successfully demonstrated. As com-
puters become faster and as more sophisticated actuators and sensors
are developed, computer interfaces will enable the user to feel, touch,
and see the virtual product in a virtual environment.
For customized design and prototyping, it is essential to integrate
virtual prototyping with data acquisition. With the measurement of
the user, the task, and the environment, we can create accurate dy-
namic models (specific to the user, the task, and the environment) and
investigate the virtual creation and installation of a customized virtual
product on a virtual human user as an integral part of the engineering
process.
Fig. 32.4. The central virtual user interface for visualization and optimization
of the design
To evaluate candidate designs, it is useful to create a simulation of

the user and the mechanical system as shown in Fig. 32.4. The mech-
anism that links the human head to the feeding device is not shown in
the figure. The designer can experiment with different kinematic cou-
pling mechanisms and see how the movements of the user are translated
into the movement of the end effector or the spoon. Three-dimensional
graphics provides visual information about the design, while a realtime
dynamics simulation package elicits information about the forces and
the velocities that are required of the human head and neck to effec-
tively accomplish feeding. By linking to an appropriate physiological
database one can verify the feasibility of the required head and neck
motions and also investigate possible sources of discomfort or trauma
with the virtual prototype before clinical tests are performed.
Being able to develop a virtual prototype of the product also allows
the consumer to use and evaluate the virtual product in an appropri-
ate virtual environment before the designer commits to the expense
of creating the physical prototype. As shown in Fig. 4 consumer feed-
back (and evaluation by experts such as therapists) during the virtual
prototyping phase and the redesign of the product in response to this
feedback at a very early stage can ensure the success of the product
and possibly avoid building multiple physical prototypes and incurring
the resulting expenses.
32.6 Conclusion
We have proposed the technology underlying assistive devices for people

with manipulative disabilities focusing on orthotic device and robotic
arms. We have pointed out the important role that robotics can play
in assistive devices. We proposed devices that bear strong resemblance
to the multiple degree-of-freedom robot arms and to the body-powered
orthotic device. We discussed how an upper arm orthotic may be an
optimal compromise that allows for extended physiological propriocep-
tion as well as strength enhancement. We have also suggested the use of
two special types of assistive devices; both have their own advantages
and the choice depends upon the patients as to which one they wish to
adopt.
Acknowledgement. We are indebted to Mr. Pradeep Khanna for his invalu-

able support and guidance, and for his constant motivation, which lead to
successful completion of this experiment. We would like to appreciate the
suggestions given to us by Professor Suneet Tuli, Associate Dean, Indian In-

stitute of Technology, New Delhi. We would like to thank the Electronics and
Communication Department of Delhi College of Engineering for providing the
necessary instruments for carrying out the experiment.
References
1. H.F.M. Van der Loos. VA/Stanford rehabilitation robotics research and
development program: Lessons learned in the application of robotics tech-
nology to the field of rehabilitation. IEEE Transactions on Rehabilitation
Engineering, 3(1): 46-55, 1995.
2. G.E. Birch, M. Fengler, R.G. Gosine, K. Schroeder, M. Schroeder and
D.L. Johnson. An assessment methodology and its application to a ro-
botic vocational assistive device. Technology and Disability, 5(2):151-166,
1996.
3. S.J. Sheredos, B. Taylor, C.B. Cobb and E.E. Dann. Preliminary evalu-
ation of the helping hand electro-mechanical arm. Technology and Dis-
ability, 5(2):229-232, 1996.
4. G. Verburg, H. Kwee, A. Wisaksana, A. Cheetham and J. van Woerden.
Manus: The evolution of an assistive technology. Technology and Disabil-
ity, 5(2):217-228, 1996.
5. M. Topping. Handy I, a robotic aid to independence for severely disabled
people. Technology and Disability, 5:233-234, 1996.
6. C. Upton. The RAID workstation. Rehabilitation Robotics Newsletter,
A.I. duPont Institute, 6(1),1994.
7. H. Hoyer. The OMNI wheelchair. Service Robot: An International Jour-
nal, 1(1):26-29, MCB University Press Limited, Bradford, England, 1995.
8. M. West and H. Asada. A method for designing ball wheels for omni-
directional vehicles. 1995 ASME Design Engineering Technical Confer-
ences, DAC-29, pp. 1931-1938, Seattle, WA 1995.
9. F.G. Pin and S.M. Killough. A new family of omni-directional and holo-
nomic wheeled platforms for mobile robots. IEEE Transactions on Ro-
botics and Automation, 10(4):480-489, 1994.
33
Design and Control of a Heating-Pipe Duct
Inspection Mobile Robot∗
Piotr Dutkiewicz, Michal Kowalski, Bartlomiej Krysiak,

Jaroslaw Majchrzak, Mateusz Michalski, and Waldemar Wróblewski

ul. Piotrowo 3a, 60-965 Poznań, Poland
name.surname@put.poznan.pl
33.1 Introduction
During operation of the heating net, there may appear leakages and fail-
ures in places which are hard to access. Repair of underground heating-
pipe ducts requires using heavy equipment as well as temporarily re-
moving neighboring infrastructure elements which are obstacles. This
implies high costs of penetrating the failure place, so it is advisable to
minimize the risk of erroneous choice of the intervention place. Local-
ization of the failure place is difficult due to its inaccessibility or lack
of information or proper tools. So, very expensive is the wrong choice
of the exploration place, when failure localization is uncertain. There
exist acoustic monitoring devices for rough detection of leakage, from
the ground level over the damage, sensitive to the noise of outgoing
steam or warm water. Nevertheless, essential is the possibility of esti-
mation of the leakage with one’s eyes. Therefore, construction of a tool
which could reach places inaccessible even for a well equipped operator
is very important. This situation is due to atmospheric conditions and
dimension of places which should be reached.
Descriptions of several in-pipe inspection robots can be found in
literature (e.g. [1, 2, 3]). However, the designed platform is intended
for inspection of ducts containing heating pipes, so the inspection of
the pipes will be carried out from the outside. There are documented
several solutions of duct inspection systems, e.g. [4, 5].
The main task of the designed mobile robot is support in inspection
tasks and estimation of the failure level. This implies two basic features
of the designed system:
∗
This work was supported by the Ministry of Science and Higher Education re-
search grant no. 3 T11A 026 27.
springerlink.com
362 P. Dutkiewicz et al.
• appropriate mobility in the area which is hard to access by a man

due to terrain and atmospheric conditions,
• transmitting of data about position of the robot and state of the
explored area.
When the robot reaches the place of a heating net failure, it should
determine its geometric position on the net map and enable observa-
tion of the range of the failure and the effects caused by it. In such
a situation, typical atmospheric conditions are: increased temperature,
high humidity, and low visibility (due to lack of illumination, steam,
etc.). Moreover, operation of the vehicle without its direct traction con-
trol from the outside is difficult as unpredictable obstacles or uneven
ground may appear. The ground of heating-pipe ducts is usually un-
even; there can be found accidentally such elements as thermal shields,
pipe coverings, crushed bricks, or construction elements of the heating-
pipe duct. It has been assumed, that there are no water pools in the
duct, but steam of low pressure may appear.
33.2 Chassis Structure
The construction of the suspension and steering system of the robot

chassis (see: Fig. 33.1) was originally inspired by the model of the off-
road car produced by Tamiya [6]. Such models are used in races of
the RC-class cars. The off-road car is controlled by specially adapted
communication devices – two radio channels operated by an integrated
transmitter and two receivers with effectors which mechanically con-
trol the driving system. They control the velocity and the direction
of the movement. This off-road car has been chosen as a reference
model, but the new chassis has been lengthened compared to the orig-
inal and the suspension system has been strengthened. It has been
assumed that the platform with the equipment must fit in a cuboid of
size 0.5 m×0.3 m×0.3 m, its mass should be smaller than 10 kg, and its
maximum speed should be 2 m/s.
The main element of the chassis is the supporting frame which en-
ables using the model in off-road races. The frame assures proper overall
dimensions and clearance to overcome unevenness of the ground. The
driving and suspension systems of the model were designed for fast
displacements in the terrain of diversified surface and significant un-
evenness. The supporting frame was redesigned to strengthen it and to
provide with fastenings for control and measurement equipment.
33 Design and Control of a Heating-Pipe Duct Inspection Robot 363
Fig. 33.1. Chassis and equipment of the RK-13 robot
33.2.1 Supporting Frame and Suspension

The new design of the supporting frame took into account two assump-
tions:
• the robot should carry heavy batteries and control equipment re-
taining low own weight and high stiffness of the frame;
• preserving a proper clearance of the running gear – it has been
assumed that the frame cannot reduce it.
The frame is built of machined aluminium plates, forming sides of the
carrying cage, as well as of screw links and cross-bars. Mounted to-
gether, they form a stiff cage for placing equipment inside it. Housings
of driving units are placed in the front and back parts of the frame.
Each driving unit consists of a driving motor integrated with a reducing
transmission gear.
Suitable properties of the running gear result from its design. The
suspension is based on four parallelogramic suspension arms of long
stroke. Each arm is rotationally fastened to the driving unit housing
and supported on it with a gas-elastic damper. Each pair of suspen-
sion arms, working coaxially, is equipped with a bendable stabilizing
lever preventing from long-lasting twist around the arm rotation axle.
Thanks to this lever, which determines the lower rotation axle of the
suspension arm, the suspension system of the vehicle became robust
both to bumps in the direction perpendicular to the wheel rotation
axle and to momentary displacements limited by the elasticity of the
beams. The maximum stroke of the suspension mechanism, resulting
from the deflection range of the damper, measured at the contact point
of the wheel to the ground, is ca. 65 mm.
33.2.2 Driving System
The vehicle is driven by two high-velocity Graupner 540 DC motors

supplied with 6 V voltage. There are two motors, each for one axle. The
motors are mechanically coupled with reduction transmission gears of
ratio 2.8 : 1, limiting influence of the vehicle dynamics. This is the first
level of rotational velocity reduction, which also enlarges the driving
torque exerted by the motor. Then, this torque is transmitted to par-
ticular wheels via the second level reduction gear of ratio 16.27 : 1 and
a differential gear. So, the driving unit consists of the motor, reduction
gears of the first and second level, the reduction gear, and the housing.
Both driving units of particular axles are identical. The driving torque
is transmitted from each output of the differential gear to the wheel
via a stiff shaft and two Cardan joints. They assure movability of this
part of the driving system together with the suspension arm.
The hubs of all the wheels are mounted to suspension arms. The
wheels are of diameter 127 mm and are equipped with pressure-less
tyres with the tread formed as rubber spikes, to improve the off-road
driving properties. The wheels have been chosen from scratch to enable
the platform crossing obstacles of height of ca. 50 mm.
33.2.3 Steering System
The hubs of the front wheel are movable relative to the suspension arms,
forming so-called steering knuckles. Rotation of the steering knuckle is
possible around the axis perpendicular to the ground, but is limited by
the range of the turn angle. Displacements of the steering knuckles are
parallel thanks to steering rods connected to them via ball-and-socket
joints. At the other side, the steering rods are joined to the servo per-
forming the turn. Thanks to this, the turn of the vehicle may be accom-
plished in a classical way – it is Ackerman turn system. The vehicle may
be qualified as a class (δm , δs ) = (2, 2) nonholonomic system, where δm
is the degree of mobility and δs is the degree of steerability of the robot
[7].
33.3 Control System Concept and Implementation
The inspection robot is non-autonomous and is controlled by an op-

erator. Movement and observation tasks executed by the robot are re-
motely operated. Essentially, the real-time control is performed by the
on-board controller, while reference values are sent from the host via
an on-board computer to it. The operator fully monitors and controls

the behavior of the robot using a dedicated application consisting of:
• a server application for control and monitoring of the robot and
its sensors (this application is implemented on the robot on-board
computer),
• a client application working on a remote host computer.
The server application works under control of the Windows XP op-
erating system. The modular computer PC104 works as a PC server
on the robot board, while the client application works on a PC work-
station under control of the Windows operating system. Both PCs are
equipped with radio net cards and these applications cooperate via a
wireless WiFi link. So, this may be treated as a master-slave system
having in mind its control structure, but taking into account the data
interchange implementation, it may be treated as a client-server sys-
tem. Moreover, the robot is equipped with a board controller of motor
drives and of turning the robot. Using it, control from the on-board
server level, or directly via the radio link, is possible.
It has been assumed that the on-board software will work as a server
while the operator console will be a client. Such solution assures that
the monitoring and reporting software may be created as independent
modules communicating with the server via TCP/IP connections. So, it
is possible to develop extra software without the necessity of integrating
it with already existing software.
The operator console, working as client software, implements the
following functions:
• preview of image sequences from the board cameras,
• listening to the sound from the board microphones,
• monitoring parameter values supplied by the sensors,
• robot control using a keyboard or a joystick,
• documenting of the travelled road with photos taken at particular
distances,
• capturing of robot surrounding photos, requested by the operator,
• automatic detection of connections between concrete slabs on the
heating-pipe duct walls.
The console software works under the Windows system with the
.NET 2.0 framework.
The main element of the robot control system is its board computer.
An embedded PC solution has been used here – an Advantech PCM-
9387 single board computer. It is equipped with an Intel ULV Celeron
M1.0 GHz processor and a set of interfaces enabling communication
with subsystems of particular elements of the on-board equipment. The

diagram of connections of the on-board computer with particular sub-
systems is presented in Fig. 33.2.
Microphone 1 Pyrometer Hygrometer Microphone 2

Manometer
RS232 RS232
Mic In Line In
On-board PC
Wi-Fi USB Camera 2
Controller
Analog radio Illumination
Power manag.
Drive control
Camera 1
Robot
Fig. 33.2. Connections of the on-board PC with robot subsystems
Currently, the robot is controlled in an open loop, i.e. all movement

commands are sent to the platform from the operator, who may re-
act to changes in images delivered from cameras to the console (the
open-loop system is closed in this way). It is planned to extend the au-
tonomy of the robot by automatic control in several tasks, e.g. obstacle
avoidance. As the robot is controlled manually, no real-time operat-
ing system is necessary for the platform software – the solution based
on the Windows XP Professional system has been implemented. The
delay in communication with the operating system has no significant
influence on the accomplishment of teleoperation tasks.
The on-board computer software (PC server) consists of several
modules served by separate program threads:
• communication module – manages connections of the computer with
the operator console, interprets commands and distributes tasks be-
tween other modules;
• on-board sensors module – reads cyclically the current values mea-
sured by the manometer, the hygrometer, the pyrometer, and the
termometer;
• motor control and energy distribution module – sends reference val-
ues to the microprocessor system of control of wheels velocity and of
turn angle; also sends commands to the energy distribution system
to turn on/off particular systems and illumination diodes as well as

to switch the board computer to the idle state;
• board camera control module – sends commands of position change
to the camera connected to the board computer via a USB port;
• board microphone module – processes sound from a microphone and
passes it to the communication module in digital form; it decides
which of the two microphones delivers the signal for analysis;
• log module – updates the history of events (non-typical data from
sensors, communication errors, erroneous images, etc.) which is
saved on a flash disk.
Power
supply { Servomechanism
Power management module 2

IC
Controller
Microprocessor PWM
USB C8051F120
PWM
On-board
PC server
PWM Power Power

bridge 1 bridge 2
Encoder 1
Radio Radio Encoder 2
module 1 module 2
Motor 1 Motor 2
Fig. 33.3. Block diagram of the robot board controller
33.4 On-Board Controller
The on-board controller module is located between the PC server and

the on-board peripherial devices (e.g. motors and turn servomech-
anism) (see Fig. 33.3). This module has been built around a C051F120
microprocessor; its localization in the robot is shown in Fig. 33.4. This
specific microprocessor has been chosen because of its advantages, i.e.
built-in devices (ADC, DAC converters, PWM I/O ports), an on-chip
JTAG debugger, and a high-speed core (100 MIPS). Those features
are crucial to obtain desirable control. The controller module also co-
operates directly with the energy distribution module and the radio
communication module for manual setting of velocity and turn angle.
Fig. 33.4. Housing of the RK-13 inspection robot; localization of the control
module is indicated
The communication of this module with the PC server is via the

USB port, and with the energy distribution module – via the I2 C bus.
The communication between these devices is implemented with a pro-
tocol common for all systems communicating with the PC server.
Movement of the robot is accomplished by setting the velocities of
the drives of both axles and by setting the turn angle of the front wheels.
Setting the velocities of the drives is done via control of two DC motors.
Each motor is controlled by a separate bridge, which ensures better
current efficiency. Both motor control systems are connected serially to
assure identical input currents, so torques exerted by both motors are
balanced. Due to limited on-board space and the requirement of high
efficiency of power bridges, it has been decided to use Zoom Carson
12 V, 18 A bridges.
The control signals of both bridges are identical, so there is no di-
versification of velocities between the front and rear wheels. The turn
angle of the front wheels is set by the servomechanism (see Fig. 33.3).
The control signals for both power bridges and the servomechanism are
of PWM nature, which decreases sensitivity of control to disturbances.
The PWM signals are generated by the microprocessor on the ground
of control signals which can be set twofold: from the on-board computer
(PC server) or manually by the operator (from the radio transmitter).
When the system works normally, the PC server generates the move-
ment tasks for the robot. They are sent to the control module as the
reference values of drive velocities and turn angle of the front wheels.
The high performance of the PC server allows implementation of var-
ious complex control algorithms compensating such effects appearing

in real environment as immobilizing wheels or skid. The feedback from
the rotational velocity of motors, implemented using hallotronic en-
coders, may detect such wrong situations and modify the drive control
in real time. The controller module works also as a supervising system
for the PC server. When the off-line state of the PC server is detected,
the emergency control mode is started. In such mode, a manual radio
controller may be used allowing remote radio control of the platform,
without using the PC server. This controller operates in several radio
channels; two of them are dedicated to control of the drive velocity and
the turn angle of the front wheels.
DC/DC Fuse Independent

Battery 5V, 5A power sections
12V DC/DC Fixed current source
12V, 2A
Motor 1
} Outside
lighting
LED
current 4 channels 4 channels

AC AC
Motor 2
current A/DC 2 D/AC
I C / Parallel
10-bit, 4-ch 8-bit, 4-ch
DC DC
2
IC
Controller
Fig. 33.5. Energy management system
33.5 Energy Distribution Module
The particular systems of the robot (PC server, drives, illumination)

require high energy consumption, so an advanced system of energy
distribution and management was required. The energy management
module, shown in Fig. 33.5, implements the following functions:
• 5 V and 12 V stabilized supplies – the DC/DC converters assuring
galvanic isolation are used;
• measurement of analog signals – a 10-bit, 4-channel A/D converter is
used; the digital values of the battery voltage, the discharge currents
and the currents of both motors are sent to the controller via the
I2 C bus;
• illumination – four LED diodes are used as external illumination of
the robot. They are supplied from the current sources (of maximum
current Imax = 300 mA) adjusted from a D/A 8-bit, 4-channel con-
verter cooperating with the I2 C bus; this enables precise changes of
the illumination intensity;
• independent supply circuit – four relays with fuses are selected indi-
vidually for each circuit; each one turns on the supply for a separate
device (i.e. PC server, cameras, transmitters, measurement devices)
through an ’I2 C/Parallel’ circuit, which converts signals from the
I2 C bus to parallel.
33.6 Conclusion
The paper presents the design and practical implementation of control
for the specialized mobile robot. The suspension and steering system
has been designed to accomplish movements in the area of heating-pipe
duct housings, where human access is difficult or even impossible due to
duct dimensions or atmospheric conditions. The robot is intended for
teleoperating inspection tasks in areas difficult to access. The working
range of the wireless link between the robot and the host computer is
up to 500 m. So far, the inspection robot has been tested in simulated
conditions, on the ground level.
References
1. Moraleda J, Ollero A, Orte M (1999) A robotic system for internal in-
spection of water pipelines. IEEE Robotics & Automation Magazine, vol.
6, no. 3, 1999, pp. 30-41
2. Roh SG, Choi HR (2002) Strategy for navigation inside pipelines with
differential-drive inpipe robot. In: Proc. of the IEEE Conf. on Robotics
and Automation, vol. 3, Washington, pp. 2575-2580
3. Hirose S, Ohno H, Mitsui T, Suyama K (1999) Design of in-pipe inspection
vehicles for φ25, φ50, φ150 pipes. Proc. of the IEEE Conf. on Robotics
and Automation, vol. 3, Detroit, MI, pp. 2309-2314
4. Lee CW, Paoluccio Jr J (2004) A probe system for cave exploration and
pipe/duct system inspection. Int. Journal of Technology Transfer and
Commercialisation, vol. 3, no.2, pp. 216-225.
5. Koh K, Choi HJ, Kim JS, Ko KW, Cho HS (2001) Sensor-based naviga-
tion of air duct inspection mobile robots. Proc. SPIE. Optomechatronic
Systems, HS Cho, GK Knopf (Eds.), vol. 4190, pp. 202-211.
6. Technical documentation of the vehicle R/C Twin Detonator. (2003)
Tamiya Inc. 3-7, Japan
7. Canudas de Wit C, Siciliano B, Bastin G (Eds.) (1996) Theory of robot
control. Springer, Berlin Heidelberg New York
34
Measurement and Navigation System of
the Inspection Robot RK-13∗
Piotr Dutkiewicz, Marcin Kielczewski, Dariusz Pazderski,

and Waldemar Wróblewski

ul. Piotrowo 3a, 60-965 Poznań, Poland
name.surname@put.poznan.pl
34.1 Introduction
Specialized robots designed for performing functions which are known

in advance have been the subject of intensive development recently.
These functions depend on the domain, where the mentioned special-
ized constructions are used. Applications come mainly from areas which
are dangerous for humans or where high manipulation precision is re-
quired. Many research papers are devoted to applications of robots in
medicine. In such cases a robot manipulator can take place of a sur-
geon’s hand or assists in laparoscopic operations, e.g. following move-
ment of surgical tools in the abdominal cavity and delivering images to
the operator [1]. Inspection robots belong to another group of special-
ized robots [2, 3]. They perform examination of application-dependent
infrastructure through measurement of specified parameters. Therefore,
such robots are equipped with suitable measurement and navigation
systems. Besides that, an inspection robot has to be equipped with a
locomotion system.
In this paper, measurement and navigation systems of the inspec-
tion robot RK-13 are described. Its main purpose is examination of
ducts of a heating net. Monitoring of the environment in the heating-
pipe ducts involves measurements of such physical parameters as tem-
perature, pressure, humidity, and noise level as well as transmission
of image sequences from the duct. A typical failure of the pipeline is
leakage, which causes sudden change of aforementioned parameters –
analysis of them enables localization of the failure location. Addition-
∗
This work was supported by the Ministry of Science and Higher Education, re-
search grant no. 3 T11A 026 27.
springerlink.com
ally, image sequences from cameras, located in archives, may be useful

for determining places where failures may happen.
According to the modular description of complex control systems
[4], the acquisition and communication systems are indispensable ele-
ments of the monitoring system. The measured environment parame-
ters are saved on a hard disk placed on board of the robot. Commands
of displacements in the heating-pipe duct from the host operator are
sent through the communication system. Localization of the robot dur-
ing inspection or its self-orientation in unknown environment are tasks
of great importance. For navigation of the inspection robot RK-13, a
method of orientation in terrain using the vision system has been de-
veloped.
The paper is organized as follows. In Section 34.2 measurement,
vision, and acquisition modules are described. Next, in Section 34.3, the
localization method of the robot in the heating-pipe duct is described.
Section 34.4 summarizes the results.
34.2 Measurement and Control System Modules
34.2.1 Communication System – Hardware Layer
Communication and data interchange between the inspection robot and

the host computer employs a wireless net working in the 2.4 GHz fre-
quency band. The block diagram of the communication system is pre-
sented in Fig. 34.1. Control commands and configuration parameters
for particular modules are sent from the control console of the operator
computer to the robot. The on-board computer sends to the console the
measurement data from the sensors and the images from the camera
connected directly to it via a USB port. A more detailed description of
the architecture of applications using the wireless net connection can
be found in [3].
The on-board computer uses a MiniPCI Intel PRO/Wireless 2200BG
wireless network card (radio module). The WiFi Sinus 2415A wireless
amplifier of the maximum output power of 1.6 W (32 dBm) is connected
to the antenna output of the network card as the robot will work in a
heating-pipe duct, where disturbances of high amplitude may appear
and the radio signal is significantly damped. Additionally, the input sig-
nal is amplified by +18 dB. An omnidirectional antenna of 3 dBi gain
is connected to the antenna output of the amplifier. In case of adverse
conditions, similar amplifying module may be used at the opposite side,
34 Measurement and Navigation System of the Inspection Robot 373
Fig. 34.1. Communication system of the RK-13 robot
i.e. at the operator’s computer. The WiFi device connected to this com-
puter works in AP (Access Point) mode. To assure the largest possible
separation from the independent video signal of the transmitter work-
ing in channel 1 of the 2.4 GHz frequency band, channel 12 of the same
band has been chosen for communication.
34.2.2 Measurement System
Monitoring the environment and analysis of its parameters are the basic
tasks of the designed inspection robot. Therefore, besides recording and
analysis of video images, also measurement of physical parameters of
the environment is a key function of the robot. At the place of a possible
failure, i.e. leakage from a heating pipeline, local changes of physical
parameters of the air (increase of temperature, pressure, and humidity)
are expected. Thus, leakage of overheated steam and appearance of fog
are accompanied by acoustic effects which may be additional source of
information for improving failure localization efficiency.
In the measurement system of the robot, a so-called external sen-
soric subsystem has been assigned to such tasks. Another system is
responsible for measurement of the robot state (i.e. electric charge re-
ceived from the battery, velocity of driven wheels, etc.) and delivers
data for control and diagnosis systems.
In the paper, the external sensoric subsystem for exploring the en-
vironment is described. It is shown in Fig. 34.2 and consists of:
• a pyrometer with a measurement transducer,
• a system for pressure, humidity, and temperature measurements,
• a set of microphones with preamplifiers.
This equipment is managed by the on-board computer of the robot with
the use of two transmission channels based on the RS232C interface.
Fig. 34.2. Block diagram of the measurement system for environment mon-
itoring
For touchless measurement of temperature, an industrial miniature

pyrometer Raytek MI with the MID head [5] is used. The head has
optical resolution of 2 : 1 and is mounted statically in front of the ro-
bot housing (see Fig. 34.3). The range of measured temperatures is
from −40◦ C to a few hundred degrees centigrade and is appropriate to
application requirements. The choice of the head of a relatively large
measurement angle results from the goal of minimizing the directional
selectivity of measurement. It was due to simplifying the mechanical
construction and abandonment of active orientation of the pyrometer
at a chosen aim. This assumption results from the fact that the absolute
value of the temperature measured by the pyrometer is not important
for failure detection, but detection of the temperature increase. Acqui-
sition of the measured data is managed by the on-board PC communi-
cating with the device using a text communication protocol specified
by the pyrometer manufacturer.
The next element of the measurement system for monitoring am-
bient pressure, temperature, and humidity was built as a controller
around an Atmel ATMega 162 8-bit microcontroller chip. Communica-
tion between the microcontroller and the on-board PC uses a binary
protocol implementing master-slave data interchange.
For pressure measurements, a semi-conductive, thermally compen-
sated analog sensor of absolute pressure – the Freescale MPX4115 [6]
– is used. Its range is optimized for air-pressure measurement. This
assures suitable accuracy of measurement and enables detecting rela-
Fig. 34.3. Pyrometer and microphones location
tively small changes of pressure in the heating-pipe duct. Conversion

of the analog voltage signal from the sensor to its digital represen-
tation is performed by the sigma-delta converter synchronized by the
microcontroller. This assures high resolution of the result (at the 16-bit
processing level) with high robustness to disturbances.
An integrated digital sensor Sensirion STH75 [7] is used to measure
humidity and pressure. It is a programmable unit and uses for data in-
terchange with the supervising microcontroller a serial bus similar, but
not compatible, to the standard I2 C bus. It allows temperature mea-
surements in the range of −40 ÷ 120◦ C with a resolution of 0.01◦ C and
relative humidity measurements in the range of 0 ÷ 100% with a res-
olution 0.01%. This sensor is supervised by the microcontroller, which
sends cyclically to the sensor commands initiating measurements. The
software of the microcontroller carries out scaling of the raw data sup-
plied by the sensor, low-pass filtration, and makes the results available
to the host computer.
The inspection robot is equipped with four capacitive electret mi-
crophones placed at front and rear sides of the housing. They are used
for acquisition in the acoustic band for leakage detection of the over-
heated steam. To improve the sensitivity and the signal-to-noise ratio,
additional analog preamplifiers of gain of ca. 30 dB are integrated with
all the microphones. The output signals of the preamplifiers are sepa-
rated by a multiplexer driven by the microcontroller and transmitted
to the stereo audio input of the host computer, which processes and
records them. Notice that using several microphones assures additional
spatial information and enables estimation of the direction where the
leakage source is located.
34.2.3 Vision System for Inspection and Navigation

The vision system of the inspection robot is equipped with two inde-
pendent vision channels with two cameras located at the front and rear
sides of the platform (see Fig. 34.4). They allow the operator remote
navigation and preview of the penetrated surrounding in both direc-
tions of journey. One of the cameras, connected directly to the on-board
computer via the USB port, is of 2 degrees of freedom, extending the
range of the monitored surrounding without moving the robot. Such
solution enables observation in the angular ranges of 200◦ horizontally
and 105◦ vertically. The image sequences from this camera, captured
by the on-board computer, are transmitted to the operator console via
the wireless network. Additionally, in order to build archives of the ro-
bot route, the image sequences may be stored at a hard disk of the
on-board or operator computer.
Fig. 34.4. Location of the vision system elements on the platform
The other independent vision channel contains a camera with analog

composite output, connected to a dedicated video signal transmitter
working in the frequency range 2.4 GHz, channel 1. The transmitter
power is ca. 1 W. On the operator computer side, the output of the
cooperating video receiver is connected to the composite input of the
TV card. The video signal, captured from this card to the operator
console, gives the preview from the other camera.
Such redundant solution of two independent vision channels enables
inspections and robot control in case of on-board computer failures or
a communication breakdown. It can be accomplished together with
remote control of the robot drives via the RC vehicle controller module
without using the on-board computer.
Due to the darkness in heating-pipe ducts, the robot has been
equipped with an illumination system consisting of white light-emitting
diodes (LEDs). There are four LEDs, two in the front and two in the
rear of the platform (see Fig. 34.4). Each diode is of 1 W power and
30 lm maximum brightness. The beaconing angle of each diode is 140◦ .
This illumination assures visibility range of ca. 2 m, depending on the
surrounding conditions. To spare the limited resources of energy of the
robot battery, each LED can be turned on individually and beaconing
intensity can be changed.
The robot vision system is mainly intended for inspection and re-
mote navigation performed by the operator. Additionally, this system
may be used for automatic localization of the platform in the heating-
pipe duct. This duct is built of concrete profiles and slabs. Given the
heating net map, the plan of the duct, and the width of concrete slabs,
it is possible to determine the position of the robot in the duct by
counting the number of passed slab connections. The precision of this
method is limited by the width of one slab. The connections of the
slabs are visible on an image as vertical or horizontal limits between
relatively homogeneous areas. Since different elements of various shapes
can be found at the bottom of the duct, it has been decided that only
vertical slab connections are to be detected and counted. The other
reason is the fact that the robot can move in the duct only on the left
or right side, where vertical connections are visible better.
34.3 Localization of the Duct Inspection Robot with Use

of the Vision System
Localization of the robot in the heating-pipe duct may be accomplished

by detecting successive slabs on the walls of the duct, which the robot
passes by. To do this, an algorithm of detection of vertical connections
between the slabs has been proposed. It looks for connections on the
image from the board camera and allows their tracking and counting
during robot movement. The algorithm consists of three basic steps:
• filtration in the frequency domain,
• edge detection and determining straight line sections using the
Hough [8] transform,
• tracking and counting the slabs passed by using the detected straight
lines.
High precision of detected connections is not crucial in the algorithm, as
localization is estimated with accuracy conditioned by the slab width.
In the first step, the image is captured from the camera in the
YUV format. For further processing, only the grayscale component is
used. Next, the processed image is converted from the space domain to
the frequency domain using a two-dimensional fast Fourier transform
(FFT). In the obtained spectrum, only coefficients related to steady-
state components and higher-frequency coefficients responsible for ver-
tical changes in the image are left unchanged. The higher-frequency
coefficients responsible for horizontal changes in the image are set to
zero. It is done with a logical mask of the size of the spectrum coefficient
matrix (image matrix), where elements equal to logical 1 form a rec-
tangular window; the elements outside this window are equal to logical
0. Selecting the proper size and orientation of such window leads to the
desirable directional filtration effect. This filtration eliminates distur-
bances leaving only distinct vertical or almost vertical edges. Then the
trimmed image spectrum is transformed back to the space domain us-
ing a two-dimensional inverse fast Fourier transform (IFFT). An exam-
ple image before the filtration (left) and after the directional filtration
(right) is presented in Fig. 34.5.
Fig. 34.5. Image before filtration (left) and after directional filtration (right)
In the next step, edges are detected in the space domain using the
Canny detector [9] on the image obtained as the result of the filtration.
During this, when selecting the proper values of the filter thresholds,
only the points belonging to sufficiently strong edges remain on the
binary image (see Fig. 34.6, left). As the filtration is carried in the
frequency domain, artifacts may appear near the margins of the image
after transformation to the space domain. They cause detection of non-
existing edges. To avoid this, edge points close to the image margins
are omitted in further analysis.
To determine straight line sections, which may represent the slab
connections, the Hough transform [8] is calculated for the binary image.
After determining the maximum values of the coefficient matrix, used
as the direction coefficients of straight lines on the image, the sections

containing the detected edge points are selected. Next, the sections of
significantly different orientation from the vertical slab connections are
removed from the list of sections. The remaining ones are grouped if
they are located very close to each other. It is due to the fact that the
concrete slabs in the duct are ca. 0.5 m (or more) wide. The resultant
line is determined using the least squares method for each group of
sections. The section which is assumed as the place of connection of two
slabs belongs to this line. The vertical edges detected for an example
image are shown in Fig. 34.6.
Fig. 34.6. Detected edges (left) and vertical connection lines on the processed
image
In the last step, a simple method of tracking the connections be-

tween the slabs has been implemented. The position of the section in
the current image frame is analyzed relative to the previous one in a
selected fragment (window) of the image. The analysis is conducted in
a window to take into account only a well visible fragment of the wall,
close to the robot. The section closest to the vertical margin of the
image is taken into account. If the section moves to the vertical margin
and disappears, then the counter of passed slabs should be incremented.
When the section moves to the middle of the image and disappears from
the selected window, the counter should be decremented. It has been
assumed, that a section may be counted as a next connection if is de-
tected on images two times in sequence. The robot cannot cover the
distance of one slab width in a time shorter than the sum of the storing
time and the processing time of three frames of the image. In case of
the inspection robot this assumption is satisfied as the slab width is
relatively large compared to the acceptable velocity of the robot in the
duct.
34.4 Conclusions
Measurement and navigation systems of the inspection robot RK-13

have been presented in the paper. The proposed navigation algorithm,
making use of information coming from the vision system, requires
experimental tests carried out in a real heating-pipe duct. Further re-
search will focus on this topic. The results of the experiments in real
ducts will help to improve the outlined algorithm. The preliminary re-
sults described in Section 34.3 are positive and encourage to continue
the conducted research.
References
1. Dutkiewicz P, Kielczewski M, Kowalski M (2004) Visual tracking of sur-
gical tools for laparoscopic surgery. In: Proc. of the 4th Int. Workshop
on Robot Motion and Control, pp. 23-28
2. Dutkiewicz P, Kozlowski K, Wróblewski W (2004) Inspection robot Safari
– construction and control. Bulletin of the Polish Academy of Sciences.
Technical Sciences, vol. 52, no. 2, pp. 129-139
3. Dutkiewicz P, Kowalski M, Krysiak B, Majchrzak J, Michalski M,
Wróblewski W (2007) Design and control of a heating-pipe duct in-
spection mobile robot. In: Kozlowski K (ed) Robot motion and control.
Springer, Berlin Heidelberg New York, pp. 361-370
4. Dutkiewicz P (2006) Complex control systems: example applications. In:
Kozlowski K (ed) Robot motion and control. Springer, Berlin Heidelberg
New York, pp. 335-350
5. html page http://www.raytek-europe.com
6. html page http://www.freescale.com
7. html page http://www.sensirion.com
8. Duda RO, Hart PE (1972) Use of the Hough transformation to detect
lines and curves in pictures. Comm. ACM, vol. 15, no. 1, pp. 1115
9. Canny J (1986) A computational approach to edge detection. IEEE
Trans. on Pattern Analysis and Machine Intelligence, vol. 8, no. 6
35
Architecture of Multi-segmented Inspection
Robot KAIRO-II
C. Birkenhofer, K. Regenstein, J.-M. Zöllner, and R. Dillmann
Research Center for Information Technologies (FZI),

Haid-und-Neu-Str. 10-14, 76131 Karlsruhe, Germany
{birkenhofer,regenstein,zoellner,dillmann}@fzi.de
35.1 Introduction
Hyper-redundant robot architectures are suitable for usage in mobile

inspection robots. An on-site inspection scenario can be characterized
by the need to bring any user-defined sensor module into the area and to
position this module precisely at the measuring point. Such inspection
manoeuvres are complex and require extensive control techniques. In
the past few years much effort has been spent into development of multi-
segmented mobile robots. Such systems offer great driving capabilities
due to their kinematics.
Several major robot systems position themselves in this field. Souryu-
III has been developed at the Tokyo Institute of Techonolgy. Its driving
capabilities are enormous; up to now lack of load capacity prevents its
application in a general user-defined inspection scenario [11]. Omni-
Tread (OT-4 and OT-8) is a robust multi-segmented robot system
[7, 8] developed at the the University of Michigan. It posesses good
driving capabilities that are typical for multi-segmented systems, but
some joints are controlled passively; thus its driving capabilities are lim-
ited. MAKRO and MAKROplus operate within non-man-sized sewer
[3, 5]. Both systems resemble the Kairo-II architecture the most, as
they have similar mechanical and electronic characteristics. The ex-
panse, load capacity, and kinematic abilities are perfect for inspection
operations in unstructured environments. However, as each single seg-
ment of these systems is driven sequentially in a open-loop master-slave
mode, hyper-redundancy of the system cannot be utilized for complex
control tasks.
Kairo-II (Fig. 35.1) closes this gap in using enhanced sensor and
control systems. As a result, the systems redundancy can be used com-
springerlink.com
382 C. Birkenhofer et al.
Fig. 35.1. MAKROplus - this hyper-redundant robot is similar in construc-

tion to Kairo-II. Modular drive segments and joint segments are connected
with one another. The multi-segmented architecture enables the robot to drive
with a large degree of freedom.
pletely to fulfil complex user-defined tasks. Such tasks are completed

in parallel to normal driving scenarios [6]. This work contributes to ad-
vanced control systems for inspection robots in implementing a closed-
loop control scheme based on hybrid impedance control.
This paper is organized as follows: in Section 35.2 crucial compo-
nents of the system are presented. These are mechanical components
(35.2.1), computer architecture (35.2.2), control structure (35.2.3), and
sensor systems (35.2.4). Integration and validation of the system follows
in Section 35.3. Finally, the paper concludes in Section 35.4.
35.2 System
The inspection robot Kairo-II is a mobile hyper-redundant robot. Ac-
cording to serial kinematics both head- and tail-segment of Kairo-II can
be interpreted as end-effectors of an open kinematic chain, depending
on the driving direction, with the head and tail segments as the robot
base, respectively.
35 Architecture of Multi-segmented Inspection Robot KAIRO-II 383
Because the system is redundant, we can enhance its driving capa-

bilities. We can use the redundancy of Kairo-II to position inspection
segments within the kinematic chain while driving. In this case the
head and tail segments represent the base of the robot. This process
can be defined as an additional user-defined task. The kinematic chain
of the system gets closed and gives rise to a major constraint for any
control strategy.
Such movements offer great opportunity for inspection tasks, but
call for appropriate mechanics and electronics, for complex control
strategies, and for internal sensor systems that provide adequate sensor
values for those control strategies and adequate modelling. All neces-
sary components are described in the following sections.
35.2.1 Robot
Both mechanics and electronics of the system have been developed

taking aspects of modularity into account (see: Fig. 35.2). As each
single inspection scenario might vary, the robot can be assembled in
a way that meets the user-defined demands. Depending on the total
amount of sensor modules or battery modules the robot’s total length
can vary.
The joint segments and drive segments are connected alternately.
The joint segments host three motors each, providing kinematic func-
tionality with 3 degrees of freedom. In contrast, the drive segments
host two motors, but carry essential components (i.e. PC, batteries,
sensory head) and user-defined inspection modules. Depending on the
inspection scenario the drive units host wheels or chain drives.
Fig. 35.2. A typical robot configuration consists of 6 drive segments and

5 joint segments. Drive segments host central processor units, batteries, and
modules for application dependent sensor systems. Universal Controller Mod-
ules (UCoM-KairoDrive and UCoM-KairoJoint) are located in every drive
and joint segment.
Table 35.1. UCoM specification (UCoM-KairoDrive and UCoM-KairoJoint)
Component Characteristics
DSP: 80 MHz DSP56F803 (Freescale)
FPGA: EPF10k30A (Altera)
Memory: 4k Bootflash, RAM, Flash, external RAM
Configuration device: EPC2-ROM
Peripherals: CAN, SCI, JTAG, SPI
Motor control: 6 PWM-Channels
Power: Three 24 V motors up to 5 A each
Current sensing: Differential current sensing for each motor channel
AD converter: 2 * 4 Channels 12 bit AD-Converter
Encoder: 6 quadraturecoded signals or up to 24 General pur-
pose IOs
Programming: Programmable via JTAG or via CAN-Bus
35.2.2 Control Architecture: UCoM(Universal Controller

Module)
The control system of Kairo-II meets demands of an inspection sce-

nario. It is based on a hierarchically organized system with the three
following levels [1]: the task planning level, the task coordination level,
and the task execution level.
Therefore an appropriate computer architecture was chosen [2]
which is based on PC/104 systems in the task planning and task co-
ordination levels. In order to minimize cabling complexity and clock
cycle delays UCoM boards ([12] – UCoM-KairoDrive and UCoM-
KairoJoint) were used for the task execution level (specification: see:
Table 35.1). Diverse sensor systems can be easily integrated into the
system, due to its variety of peripheral and programmable components.
The components used within the UCoM signal processing group
are digital signal processors (Motorola DSP56F803) and programmable
logic arrays (Altera EPF10K30A). Crucial calculation tasks can be
moved within both components to get fast and effective calculations
in the DSP (i.e. motor control) as well as flexible and adaptable calcu-
lations in the FPGA (i.e. sensors). To guarantee fast and robust com-
munication, a high-speed communication bus between the DSP and the
FPGA was implemented.
The UCoM boards have been integrated in the Kairo-II control
in two versions. UCoM-KairoDrive controls two motors of each drive
segment. This board is integrated in the drive segment’s base. UCoM-
KairoJoint controls three motors of each joint segment (see: Fig. 35.3).
Fig. 35.3. UCoM-KairoJoint board
35.2.3 Hybrid Impedance Control

As Kairo-II interacts with its environment, contact forces between the
robot and the environment arise. Those result due to collisions, robot
poses, or ground contact. Therefore, we want to integrate a control
loop that is able to solve such collisions and – furthermore – is capable
of using the robot’s redundancy for advanced driving manoeuvres in
optimizing a quality function.
Position controlled methods fail, as simultaneous control of position
and force is necessary. Numerous basic strategies exist ([13], [9], and
[10]). In the field of redundant systems, those strategies have to be
enhanced. Therefore we chose Hybrid Impedance Control, based on
inverse Jacobian [14] for this work.
In using this control loop we are able to address all major tasks
and constraints for the usage of a multi-segmented robot in complex
scenarios. The major parts of the control loop are defined in Cartesian
space.
The control loops workflow to the robot can be described as follows:
T rajectoryGeneration → Hybrid Impedance Control →
→ Redundancy Resolution → Inner Control Loop (Robot)
Trajectory Generation: The desired trajectory (position and force) is
generated in Cartesian space. The generated vector includes values
for the robot’s driving direction as well as values for the movement
of the inspection module within the kinematic chain.
Hybrid Impedance Control: In a pre-process all incoming data of the
robot (position and contact force) have to be decoupled using the
operational space formulation [10]. We therefore estimate the ro-
bot’s pose, calculate expected dynamic conditions and compare
them with the real sensor values.
Now we are able to integrate two control schemes into the robot.
First, the impedance control loop handles contact scenarios detected
by the sensor systems (see: Section 35.2.4). In tuning the desired
Cartesian mass and damping parameters the robot’s dynamics can
be set. Second, the position/force control maps the robot to the
desired trajectory. As a result we get Cartesian accelerations that
we feed into the redundancy resolution module.
Redundancy Resolution: As Kairo-II is a redundant robot, various cri-
teria have to be used to match incoming desired accelerations in
Cartesian space and (real) joint positions to outgoing desired joint
accelerations in a perfect way.
A quality function maximizes its value taking two criteria into ac-
count. Firstly, any movement will be executed with minimal overall
joint movement. Secondly, joint positions are preferred that enable
the robot to react fast on any desired movement. This is realized by
positioning certain joints in a position that is located in the center
of the workspace.
The generated output will be filtered in an inner control loop (a

PD controller) and transferred to the robot’s joints. Force feedback
information to close the control loop will be generated in the sensor
system that is presented in the next section.
35.2.4 Sensors
When installing a sensor system for force feedback, major effort has to
be put into the sensor’s positioning within the robot. The arising forces
should result in maximum change of sensor values. Their dimension
should be adequate for the control loop.
Therefore three sensor applications have been identified that fulfil
these requirements. They provide information on the arising forces and
moments along the coordinate system of the dynamic model or can
be at least transformed easily into those systems (see: Fig. 35.2). The
proposed systems measure forces and moments along the following axes:
Moments (x-axis): A sensor system based on strain gauge technology

has been integrated into the rotational gears of the joint segments.
Internal moments along this axis cause components of those gears
to elongate maximally. By integrating an H-bridge sensor array sig-
nal’s noise can be minimized. A simple transformation calculates
moments along the x-axis out of the detected mechanical deforma-
tions.
Forces and moments (y-, z-axis): The so called flange plate from each
robot module connects the drive and joint segments. This compo-
nent has been identified by fem-simulation [4] to be the part of the
robot that elongates most when forces act. The integrated sensor
system is based on strain gauge technology, too.
Forces (x-axis): Motor current of all drive segments is directly pro-
portional to forces that act in the robot’s x-direction. The signal’s
quality meets demands to extract forces along the driving direc-
tion out of the drive segment motor currents. In using least-square
methods, we smooth the signal out.
35.3 Integration and Results
Work on integration is actually in process. The components have been

tested. The details are listed below:
UCoM: The controller module has been integrated and tested not only
in the predecessor robot MAKROplus but in several other applica-
tions (i.e. humanoid robot Armar). Several extension modules have
been developed to integrate strain-gauge sensor values into the con-
trol loop.
Impedance Control: The impedance control loop has been identified
and tested as adequate control loop. The crucial blocks have been
identified and implemented in a basic configuration. Aspects like
redundancy resolution and parameter tuning will follow.
Dynamic Modelling: A dynamic model of Kairo-II has been developed
and implemented in the software work flow. A simplified dynamic
model reduces the number of parameters enormously [6]. This sim-
plified model will be used in this project as the accuracy meets
demands.
Sensors: Three sensor systems have been identified. They have been
implemented and tested as prototype application. See [4, 6] for an
analysis of those systems.
35.4 Conclusion
In this paper the key components have been presented that enable the
robot Kairo-II to operate safely in inspection scenarios. Its mechanical
structure allows the robot not only to navigate to an area of interest,
but also to move simultaneously the sensor modules along a predefined
trajectory. In using hybrid impedance control schemes, the mechanical
redundancy of Kairo-II can be used actively to fulfil general user-
defined inspection goals. Future work will be done in the field of the
robot’s redundancy resolution to solve singularity problems.
References
1. T. Asfour, D. N. Ly, K. Regenstein, and R. Dillmann. Coordinated task
execution for humanoid robots. In The 9th Int. Symposium of ISER, June
2004.
2. T. Asfour, K. Regenstein, P. Azad, J. Schroeder, A. Bierbaum,
N. Vahrenkamp, and R. Dillmann. Armar-iii: An integrated humanoid
platform for sensory-motor control. In Humanoids06, 2006.
3. K. Berns and P. Prchal. Forschungsvorhaben MAKRO – Aufgaben, De-
sign und Realisierung einer mehrsegmentigen, autonomen Kanalroboter-
plattform. In ATV-Bundestagung, 2000 (in German).
4. C. Birkenhofer, M. Hoffmeister, J.-M. Zöllner, and R. Dillmann. Com-
pliant motion of a multi-segmented inspection robot. In Int. Conf. on
Intelligent Robots and Systems, Edmonton, August 2005.
5. C. Birkenhofer, K.-U. Scholl, J.-M. Zöllner, and R. Dillmann. A new mod-
ular concept for a multi-joint, autonomous inspection robot. In Intelligent
Autonomous Systems Conf. IAS-8, Amsterdam, 2004.
6. C. Birkenhofer, S. Studer, J.-M. Zöllner, and R. Dillmann. Hybrid im-
pedance control for multi-segmented inspection robot kairo-ii. In Int.
Conf. on Informatics in Control, Automation and Robotics (ICINCO
2006), Setubal (Portugal), 2006.
7. J. Borenstein, G. Granosik, and M. Hansen. The omnitread serpentine
robot - design and field performance. In Proc. of the SPIE Defense and
Security Conference, Unmanned Ground Vehicle Technology VII, 2005.
8. G. Granosik and J. Borenstein. Minimizing air consumption of pneumatic
actuators in mobile robots. In Int. Conf. on Robotics and Automation,
2004.
9. N. Hogan. Impedance control: An approach to manipulation, parts i - iii.
ASME J. Dynam. Syst., Meas., Contr., 107:1–24, 1985.
10. O. Khatib. A unified approach to motion and force control of robot ma-
nipulators: The operational space formulation. IEEE Journal on Robotics
and Automation, 3(1):43–53, February 1987.
11. A. Masayuki and T. Takayama andS. Hirose. Development of ”souryu-

iii”: connected crawler vehicle for inspection inside narrow and winding
spaces. In Intelligent Robots and Systems, volume 1, pages 52– 57, Sendai
(Japan), 2004.
12. K. Regenstein and R. Dillmann. Design of an open hardware architec-
ture for the humanoid robot armar. In Humanoids 2003, Int. Conf. on
Humanoid Robots, Karlsruhe, October 2003.
13. K.J. Salisbury. Acitve stiffness control of manipulators in cartesian coor-
dinates. IEEE Int. Conf. Robotics and Automation, pages 95–100, 1980.
14. M. Shah and R. V. Patel. Inverse jacobian based hybrid impedance control
of redundant manipulators. In Mechatronics and Automation, 2005 IEEE
International Conference, volume 1, pages 55 – 60, 2005.
36
The Project of an Autonomous Robot Capable
to Cooperate in a Group∗
Tomasz Buratowski, Tadeusz Uhl, and Grzegorz Chmaj
AGH University of Science and Technology, Department of Robotics and

Mechatronics, Kraków, Poland
{tburatow,tuhl,chmaj}@agh.edu.pl
36.1 Introduction
One of the basic problems connected with wheeled mobile robots is the choice
of the appropriate navigational method. This refers to individual robots and
’
also, in particular, to groups of mobile robots. Navigation’ can be defined
as an issue connected with systems control, defining their position and ori-
entation as well as choosing appropriate trajectory. The consisting elements
of mobile robots navigation are: self-localization, path planning, and creating
a map of the explored area. Each of the above enumerated elements is inde-
pendent, however, the realization of the target navigation task of the mobile
robot or the group of mobile robots depends on the correct construction of
the particular elements.
In this paper we present a wheeled mobile robot project capable of cooper-
ating in a group. We decided to build three robots capable of communicating
and sending data in the process of navigation. Those robots can be used in
indoor localization. In those mechatronic systems we have several problems
to solve. The first problem is connected with communication between the ro-
bots; finding robots’ position by themselves is also an essential problem. In
our project we assumed that the group of robots has to be able to cooperate
in different tasks.
Navigation is a very complex problem especially for a group of robots. This
problem is connected with self-localization, path planning, and map creation.
This paper presents a wavefront path planning algorithm for mobile robots.
In the process of mechatronic design, three identical mobile robots have been
manufactured.
∗
This work is supported by the Polish State Committee for Scientific Research
under contract 4 T12C 046 29.
springerlink.com
392 T. Buratowski, T. Uhl, and G. Chmaj
36.2 The 2-Wheeled Mobile Robot Description
36.2.1 The Basic Assumptions Related to Model Kinematics

If we look in to the reference connected with 2-wheeled mobile robots, we can
say that for indoor solutions this type of mechatronic systems is treated as
nonholonomic [1, 2, 3]. This nonholonomic constraint is presented below and
is connected with point A in Fig. 36.1:
ẏA = ẋA tan(Θ). (36.1)
We assume that there is no skid between the wheels and the ground [2]. The
basic elements of the model are: the wheel unit, the drives of the wheels, the
self-adjusting supporting wheel, and the frame of the unit with all electronics.
When describing motion phenomena for a system such as a mobile 2-wheeled
robot, it is beneficial to attach the coordinate system to the particular robot’s
elements. This mechatronic system has 2 DOF (degree of freedom). It means
that our position should be represented by coordinates x, y and orientation is
represented by angle Θ. In Fig. 36.1 the mode of calculation focused on the
system of kinematics description [1, 2, 3] is presented. The system coordinates
are attached to the robot basic element. The system coordinate frame x0 y0 z0
is motionless and based. If our kinematics model is defined in this way, it is
possible to describe the position and the orientation of any point on our robot.
Fig. 36.1. The mode of calculation connected with system kinematics de-
scription
36.2.2 The Basic Assumptions Related to Path Planning

The basic matter in the issue of the wheeled mobile robot (and not only)
trajectory planning is describing basic definitions connected with an object
moving in space. The basic law connected with trajectory planning may be
36 Project of Autonomous Robot Capable to Cooperate in Group 393
defined as follows [4], where F S – robot free space, W – robot’s environment

and Ci denotes the obstacles:
F S = W − (∪Ci ) , (36.2)
and trajectory c ∈ C 0 is defined as c : [0, 1] → F S, where c(0) → pstart and

c(1) → ptarget .
If we take into consideration kinematics constraints for our particular mo-
bile robot presented above, there are two possible schemes of motion on our
defined surface [2, 4]. The first possible motion scheme is connected with
metric L1 which is described as follows:
(x, y) : |x| + |y| = const. (36.3)
The second possible scheme of motion is described by metric L2 defined as
(x, y) : x2 + y 2 = const. (36.4)
A graphical presentation of those metrics is shown in Fig. 36.2.
Fig. 36.2. The graphical presentation of metrics connected with robot motion
scheme
In this project the L2 metric is assumed. This mode of motion is the most
efficient and will be used in the process of path planning. According to scheme
of motion assumption in Fig. 36.3 robot movement has been presented.
Fig. 36.3. The possible movement for a particular robot
According to a basic definition connected with robots, the basic definition

of those mechatronic systems describes them as autonomous devices owning
3 units presented in Fig. 36.4. The first unit is connected with power supply.
In this unit we have an accumulator, converters, and a charger. The robot’s
controller still controls voltage level and sends the robot to the docking station.
The Motion Unit is connected with mechanical construction and it is mobile
platform equipped with wheels, two driven and one self-adjusted, and drives
Maxon A-max 32 with planetary gear GP 32 A and optic encoders HEDS5540.
Fig. 36.4. The block diagram of a 2-wheeled mobile robot
The Control Unit is the most complex. In this unit there are infrared,
sonar, and optic sensors controlled by a sensors subunit. All those sensors are
connected with the robot environment. For sensor work the robot controller is
based on ATMega128. Additionally, the robot is equipped with an embedded
computer PCM3370, which is responsible for data acquisition and managing
control, because this mechatronic device should cooperate in group with each
robot’s wireless communication device based on the Bluetooth technology.
In this project a hierarchical control system for a particular robot has
been assumed. The basic description connected with this kind of control is
presented in Fig. 36.5.
Fig. 36.5. The hierarchical control system for a mobile robot
The managing control is the highest control level. It includes special al-
gorithms and artificial intelligence elements based on swarm behavior. Those
algorithms allow to compute sensor information and recognize situation in the
robot environment. This means that the received information is interpreted
and appropriate action is taken.
Another control level is called strategic and is responsible for dividing
global tasks into a series of basic tasks. At this level path planning is cal-
culated in Cartesian coordinates. In this project we decided to create simple
robot language based on basic assumptions connected with path planning.
For example if our robot should go straight 1 meter, the command connected
with robot motion is !NN100/r. On the tactical level of the hierarchical control
system robot position is mapping from Cartesian to joint coordinates. At this
(a) (b)
Fig. 36.6. The laboratory environment and the wavefront algorithm for this
area
(a) (b)
(c) (d)
Fig. 36.7. The test rig in laboratory in environment
level the control system is using sensor information (encoders) to estimate ap-
propriate position. The main task of the control system on executive control
is to drive control in order to execute tasks planned on strategic and tacti-
cal levels. In this project for managing, strategic control, and robot language
the embedded computer is responsible. Specially designed robot controller is
responsible for tactical and executive control. In our project each robot from
the group of three is an agent. An agent [5, 6, 7] is an animate entity that
is capable of doing something on purpose. In the presented group of robots
each agent is equipped with Oracle XE database installed in the embedded
computer. One of the basic needs in robotics is to have algorithms that can be
able to convert high level specifications of tasks from humans into low-level
descriptions of how to move. In our project a wavefront algorithm has been
applied. The wavefront algorithm involves a breadth first search of the graph
beginning at the destination point until it reaches the start point. In Fig. 6(a),
an exemplary robot environment is presented and will be used in a laboratory
test rig. Figure 6(b) presents a solution, the shortest path between the start
point and the stop point.
This is just one of the many paths that can be used to reach the goal, but
it is the shortest. This means the planner should check all solutions and find
minimum time consuming trajectory.
(a) (b)
(c) (d)
Fig. 36.8. The test rig in laboratory connected with the group of robots
After test rig made in a specially prepared laboratory we see that the
result is acceptable. This means that the one robot moved to the target point
and the error was not greater than 1 cm. The exemplary, significant trajectory
points are presented in Fig. 36.7.
After the test rig based on one robot we began working with a group of
robots. The examplary test rig with the group of 3 robots is presented in Fig.
36.8. In this test the robots are responsible for environment division and its
exploration in the shortest time. Additional robots communicate and send
data connected with the environment.
36.3 Conclusions
The project is still developing. To sum up, the robots gather information
by means of sensors and pass them to the database. In the database this
information is classified by means of SQL procedures. Then the data is passed
onto the particular robots. So the collaboration rule consists in the collective
view of the environment by which the robots are surrounded. Currently work
is carried out to find the best algorithm which will divide the area of search
in order to find a target.
References
1. T. Buratowski, T. Uhl. The project and construction of group of wheeled
mobile robots. Measurements Automation Control, Warsaw, no. 5, 2006
(in Polish).
2. T. Buratowski, T. Uhl. The fuzzy logic application in rapid prototyping
of mechatronic systems. In: Proc. of the 4th International Workshop on
Robot Motion and Control ROMOCO’04, Poznań, Poland, 2004.
˙
3. T. Buratowski, T. Uhl, W. Zylski. The comparison of parallel and serial-
parallel structures of mobile robot Pioneer 2DX state emulator. In: Proc,
of the 7th Symposium on Robot Control SYROCO’03, Wroclaw, Poland,
2003.
4. J.J. Leonard, H.F. Durrant-Whyte. Directed sonar sensing for mobile
robot navigation. Kluwer Academic Pub, 1992.
5. M. Konishi, T. Nishi, K. Nakano, K. Sotobayashi. Evolutionary routing
method for multi mobile robots in transportation. In: Proc. of the IEEE
International Symposium on Intelligent Control, 2002.
6. H.A. Abbass, H. Xuan, R.I. McKay. A new method to compose com-
puter programs using colonies of ants. In: Proc. of the IEEE Congress on
Evolutionary Computation, 2002.
7. N. Baskar, R. Saravanan, P. Asokan, G. Prabhaharan. Ants colony al-

gorithm approach for multi-objective optimisation of surface grinding
operations. Advanced Manufacturing Technology, 2004.
8. S.C. Shapiro. Encyclopedia of Artificial Intelligence. Wiley, 1992.
9. S.M. LaValle. Planning algorithms Cambridge University Press, 2006.
37
End-Effector Sensors’ Role in Service Robots
Cezary Zieliński1 , Tomasz Winiarski1 , Krzysztof Mianowski2 ,

Andrzej Rydzewski1 , and Wojciech Szynkiewicz1
1
Institute of Control and Computation Engineering, Warsaw University of
Technology, ul. Nowowiejska 15/19, 00-665 Warsaw, Poland
{C.Zielinski,T.Winiarski,A.Rydzewski,W.Szynkiewicz}@ia.pw.edu.pl
2
Institute of Aeronautics and Applied Mechanics, Warsaw University of
Technology, ul. Nowowiejska 24, 00-665 Warsaw, Poland
kmianowski@meil.pw.edu.pl
37.1 Introduction
Service robots, unlike their industrial counterparts, operate in unstructured
environments and in close cooperation with human beings [3]. Thus, service
robots must rely heavily on sensing and reasoning, while industrial robots rely
mainly on precision of movement in a well-known and structured environment.
As the service robot will function in a human environment, it must be well
adjusted to such surroundings, hence, it must be two-handed, capable of self-
propulsion, possess a sense of sight, touch, and hearing. It should be capable
of reacting to unexpected situations quickly, but also should be able to reason
logically and formulate plans of actions leading to the execution of the task
at hand. The main research problem is control of this type of equipment
and integration of its numerous and diverse subsystems. The hard real-time
aspect of such systems aggravates the problem. One of the problems, that is
of paramount importance, is the design of the end-effector of a service robot,
especially the exteroceptors it should be equipped with, and how they affect
the motions of the arm – this is at the focus of the presented paper. One of
the most interesting problems is the fusion of visual and force/tactile data for
dexterous manipulation tasks [7, 11]. Visual information plays an important
role at different levels of complexity: from image segmentation to object pose
estimation [5]. In object manipulation, vision can be used in each phase of the
detect-approach-grasp loop [10]. The use of sensorial information is the most
effective way of reducing uncertainty and improving robustness.
It is clear that one universal gripper for all tasks is not feasible – even
a human hand, which is very dextrous, cannot handle too large or awkward
objects. Our research on control of service robots required a moderately com-
plicated benchmark task. We chose the Rubik’s cube puzzle. It requires [9]:
springerlink.com
402 C. Zieliński et al.
• two handed manipulation to efficiently turn the faces of the cube,

• visual capabilities, including locating the cube, visual servo-control to get
hold of it, and identification of its initial state,
• force control to avoid jamming of the cube while rotating its faces,
• fusion of deliberative and behavioural control to work out the plan of
motions solving the puzzle and to adapt quickly to sudden changes in the
environment (e.g., jamming),
• the ability to recognize spoken commands and to synthesize replies and
queries, to initiate the task at the right moment and to get human help
when the situation surmounts the capabilities of the system (e.g., the in-
tentions of the human are not clear).
However the solution to the above enumerated problems depends on the choice
of equipment – especially the end-effector and its sensing capabilities. Anthro-
pologists still ponder the question how human mental abilities evolved, but
they are certain that erect posture, that freed our hands, facilitated dexter-
ous manipulation of objects (tools), thus increasing the control requirements
imposed on our brain [2]. In robotics the interrelationship between the design
of the end-effector and the behavioural and reasoning capabilities of the con-
trol system has not been studied in depth as yet. Herein we try to initiate
that discussion by proposing a design of the end-effector of a service robot
taking into account both its capacity to manipulate objects and the ability
to sense the environment. Moreover, we look into the problem of integrating
its actuation and sensing capabilities into the overall system control structure
in such a way that the feedback loop from sensing to activating behaviours,
which subsequently influence the environment, is easy to implement. For that
purpose we have used the MRROC++ robot programming framework. Section 2
briefly describes MRROC++. Section 3 presents position-force control relying on
a force/torque sensor mounted in the wrist. Section 4 introduces the gripper
and its sensors. It should be noted that at the design stage many possible prob-
lems had been foreseen, which subsequently did not occur, however redundant
features have been included (e.g., corner shaped jaws, LED–phototransistor
pairs). Section 5 presents the gripper controller. Section 6 presents measure-
ments of the torque developed while rotating the cube face. Section 7 summa-
rizes with conclusions regarding the usefulness of the incorporated sensors and
design features. It should be noted that further experimentation on diverse
service tasks is necessary for final conclusions.
The experiments have been conducted on a Rubik’s cube puzzle. To the
best of our knowledge, only a few attempts to solve the Rubik’s cube puzzle
using robots have been made, e.g., [4, 6, 8]. It should be noted that these
robots are specialized devices that have been designed by robotics students
[6] or enthusiasts [8]. The most advanced design RuBot II is a two-handed,
pneumatically controlled robot with vision and speech synthesis system [8].
Also, the Kawasaki Heavy Industry Group has presented their dual-arm robot
for solving Rubik’s cube [4].
37 End-Effector Sensors’ Role in Service Robots 403
37.2 Robot Controller
The controller of the two-handed system equipped with special end-effectors,

each composed of an electric gripper and diverse sensors, was implemented by
using the MRROC++ robot programming framework [15, 14, 16]. MRROC++ based
controllers have a hierarchical structure composed of processes (Fig. 37.1)
(some of them consisting of threads) supervised by the QNX Neutrino real-
time operating system. The underlying software is written in C++.
The user’s task is coordinated by a single process called the Master Process
MP. It is assumed that in general there can be any number of effectors in the
system (e.g. manipulators), but in our case we have just two. From the point
of view of the executed task MP is the coordinator of all effectors present in
the system. It is responsible for trajectory generation in multi-effector sys-
tems, where the effectors are coordinated continuously – as is the case in the
presented system. In the case of sporadic coordination it just synchronizes the
effectors from time to time. In the case of fully independent actions of the ef-
fectors, the MP remains dormant most of the time. The general specification of
the task for all of the effectors is concentrated in the variable elements of MP.
In our case the MP is responsible both for producing the plan of motions of the
faces of the cube and subsequently the trajectory generation for both manipu-
lators. This trajectory can be treated as a crude reference trajectory for both
arms. At a later stage this trajectory is modified by taking into account the
force readings. Each effector has two processes controlling it: Effector Control
Process ECP and Effector Driver Process EDP. The first one is responsible
for the execution of the user’s task dedicated to this effector (in our case the
task is specified by the MP– it is defined by the reference trajectory that is
to be executed by the manipulator), and the other for direct control of this
effector. The EDP is responsible for direct and inverse kinematics computa-
tions as well as for both position and force servocontrol. The EDP has been
divided into several threads: EDP MASTER (communication with the ECP
including interpretation of its commands), EDP TRANS (designation of force
and position control directions), EDP SERVO (motor position servocontrol)
and EDP FORCE (force measurements). EDP SERVO is also responsible for
controlling the motion of the jaws of the gripper, as their motor is treated as
an extra actuator of the effector. It should be noted that when the jaws are
being closed on a rigidly held object, a certain force might cumulate in the
kinematic chain, if the location of the object is not exactly known. When an
object is being handed over to the robot by a human the exact knowledge of
its location is out of question, thus the measurement of this force is required.
It is subsequently used by a force control algorithm to modify the motion of
the arm in such a way that the tension in the arm is reduced. Finally, the Vir-
tual Sensor Processes VSPs are responsible for aggregating the data obtained
from the exteroceptors.
SEMAPHORE
PULSE
MP MESSAGE
UI_
UI INTERRUPT
COMM
SHARED
MEMORY
UI_SRP
UI_
MASTER (receives messages
VSPk
from other processes)
ECPj
VSP_ VSP_
MASTER CREAD
EDP_ EDP_ EDP_

EDPj MASTER TRANS VSP_I
EDP_ EDP_ EDP_

READER SERVO FORCE
Force/torque
Manipulator motors Sensors
transducer
Fig. 37.1. The structure of a MRROC++ based robot control system (subscript j
represents the jth end effector and thus the jth ECP/EDP pair, j = 1, . . . , ne ;
k represents the kth virtual sensor and thus the kth VSP, k = 0, . . . , nv )
MRROC++ based systems are structured into a hierarchy of processes com-

municating with each other by message passing. To avoid possible deadlock,
for each pair of processes one always executes the Send instruction (initiates
the rendezvous) and the other executes the Receive and Reply instructions.
None of the processes communicates with a third process while in between
the Receive and Reply instructions. Moreover, in principle the resources are
not shared by the processes. Communication is always handled by dedicated
threads of the processes. Exception handling is used to deal with detected
errors. All errors are handled in the uppermost layer of a process – in its shell,
which results in uniformity of the system structure and simplifies coding of
the portions of the software that are dedicated to the user’s task. The pro-
grammer focuses on the task at hand, and only provides code detecting errors
(throwing exceptions), but is relieved from reacting to those errors – that is
done by the shell, primarily on the basis of the type of error (e.g., non-fatal,
fatal, system).
37.3 Position-Force Control
The primary purpose of the end-effector is to interact with the environment.

Contact with objects always produces reaction forces acting on the gripper.
Those forces are an invaluable source of information that can be utilised in
the control of motion. Sometimes exerting a certain force is the requirement,
in other instances avoiding exerting forces is a necessity (i.e., compliant be-
haviour). To fully utilise this potential the end-effector should be equipped
with a six-axis force/torque sensor – usually mounted in the wrist between
the last link of the manipulator and the gripper. This solution has also been
followed in our design.
Force/torque sensors exhibit duality in how the information obtained from
them is utilised. First and foremost, they behave as proprioceptors, enhancing
low level control of the limb. However, they also deliver information about the
state of the surroundings, thus they can be also classified as exteroceptors.
This duality influences the structure of the control system. We have decided
that their proprioceptive character dominates, thus they have been associ-
ated with the EDP. The other reason is our assumption that proprioceptive
reactions must be much quicker than exteroceptive ones. However, we do not
want to loose their ability to act as exteroceptors, so the information from the
EDP can be transferred to a VSP (Fig. 37.1), where it can be aggregated into
a virtual sensor reading and subsequently dispatched to the ECP or the MP
for utilisation in motion generation and thus in control. Currently the EDP
uses the force/torque measurements in the implementation of the Task Frame
Formalism (TFF) [1]. Figure 37.2 presents the structure of the position-force
controller, which is composed of two parts: the macrostep execution part and
the step execution part. The ECP sends the position-force command (C) to
the EDP MASTER thread of the EDP. This command contains the definition
of the macrostep that must be executed. The definition specifies: the posi-
tion/orientation increment that is to be attained in the position controlled
directions, the force/torque to be exerted in the force controlled directions,
the selection vector specifying which directions are force controlled and which
are position controlled, as well as the local reference frame with respect to
which the arguments are expressed, and the number of servo sampling peri-
ods (motion steps) defining the duration of the macrostep. The local frame L
is represented by the GL T matrix relating it to the global reference frame G. In
the following the left superscript denotes the reference frame of the associated
vector/matrix quantity. The right subscript in parenthesis denotes the control
step number, numbered from the first step of the macrostep execution.
The position/orientation argument defines ∆ L rp , the end-effector pose
increment for the current macrostep. This increment is subdivided by the
StGen block into pose increments for each step within the macrostep ∆ L rp(k) :
∆ L rp
∆ L rp(k) = . (37.1)
n
where n is the number of steps in the macrostep and k is the current step.
EDP_
version 1 EDP_TRANS (k )
MASTER G
Pc DKin
Manipulator
G
P( k ) d (k ) -
IKin
R
generation
Macrostep
version 2 G +
Reply P( k !1)
Dl PC
L
rp L
StGen rp ( k ) (k )
C Ci M( k ) motor
Interpret SL L
Fe ( k ) control
Fd L
rF ( k ) + L
r( k )
F/T Sensor
F/T CL RT EDP_SERVO
+ - +
L
Fc ( k ) EDP_FORCE F/T conv F( k )
Macrostep Step
Fig. 37.2. Position-force controller, where: Interpret – the ECP command

interpretation, Reply – sends the status to the ECP, DKin – direct kinematic
problem, S – pose and force/torque coordinates selection,StGen – step
generator of pose component, F/T CL – force/torque control law, IKin – in-
verse kinematic problem, F/T trans – force/torque transducer, F/T conv –
force /torque reading conversion , Dl – 1 step delay ,PC – new pose
computa tion, RT – Rodrigues formula transformation
In each step the force/torque error vector L Fe(k) is the difference between
the desired force/torque vector L Fd (defined by the force/torque argument)
and the measured force/torque vector L Fc(k) :
L
Fe(k) = L Fd − L Fc(k) . (37.2)
The position/orientation vector ∆ L rF (k) is obtained from the force/torque

control law:
∆ L rF (k) = K L Fe(k) , (37.3)
where K is the viscosity. On the basis of the compound increment:
∆ L r(k) = ∆ L rp(k) + ∆ L rF (k) (37.4)
both the corresponding rotation matrix (produced by using the Rodrigues

formula in matrix form) and the corresponding translation vector form a ho-
mogeneous transformation matrix M(k) .
The end-effector pose setpoint for the current step of the macrostep exe-
cution is described by the frame G P(k) :
G
P(k) = G P(k−1) M(k) . (37.5)
The new motor position vector θd(k) is the solution of the inverse kinematic
problem for G P(k) . The vector θd(k) is a part of the command sent to the motor
controllers implemented in the EDP SERVO thread. The motor controllers

force the motors to move in the direction of the setpoint by delivering the
vector τ(k) consisting of the values of PWM (Pulse Width Modulation) duty
cycles. The EDP FORCE thread transforms the raw force/torque measurement
vector F(k) into the vector L Fc(k) .
37.4 The Gripper and Its Sensors
Unconstrained two-handed manipulation of any object requires that the closed

kinematic chain retains 6 dof. As the two IRp-6 robots, that mimic the two
hands, have 5 dof each, an extra dof had to be appended to each of the arms.
Thus, an extra joint and a gripper was designed (Fig. 37.3). Between the grip-
per and the extra joint a force/torque sensor was inserted, thus all components
of the generalized forces are measured in the wrist. Those measurements are
used in the phases of motion where position-force control is necessary, i.e.,
in the last stage of acquiring a rigid object held by a person or constrained
by some other objects, or when the two robots jointly transfer a rigid object.
The advantage of simultaneous design of the gripper and the drive of the last
link of the manipulator was that the latter could be used as a counterbal-
ance to the weight of the gripper, improving the dynamic properties of the
manipulator.
The gripper jaws are exchangeable. The ones used for grasping the cube
are corner shaped, so the cube is not grasped by its faces, but by its corners,
i.e., diagonally. This prevents the cube from slipping from between the fingers
when it is being manipulated. As the gripper was to be used in diverse service
tasks many sensors have been foreseen. A calibration procedure was carried
out on both manipulators, identifying the parameters of the kinematic chains
as well as the mutual location of the robot bases. Nevertheless, the accuracy
of the result of calibration was not certain at the moment of designing the
gripper, so each finger of one of the robots has a LED mounted in its tip, while
the fingers of the other robot contain phototransistors. Those were meant for
helping with the alignment of the fingers when turning a face of the cube.
Although present in the jaws this arrangement did not have to be used, be-
cause the motions were precise enough. The jaws also have strain gauges for
monitoring the clenching force. Although they can be used for detecting when
the cube is securely grasped, currently it suffices to use the information about
the current flowing in the motor. Crossing a certain threshold current signals
that the force is sufficient. However, the most useful sensor is the miniature
in-hand camera. It is used as a sensor in the stage of final approach to the
cube by one of the visual servos. It is also used for identifying precisely the
state of the cube, i.e., the locations of all of the colored tiles on the faces of
the cube. This is done during a series of regrasp operations.
20
2
Strain
gauges
Video camera
Force/torque with back-lighting
sensor Rubik’s
cube
Gripper jaws
F/T sensor alignment sensors
shield
Fig. 37.3. The robot and its end-effector
37.5 Effector Controller
The hardware of the controller (Fig. 37.4) of each robot consists of a PC type
host computer and as many joint controllers as there are degrees of freedom of
the robot. Each joint controller uses either a position encoder or a resolver as
a proprioceptor measuring the joint (motor shaft) position. It also produces a
PWM signal for the power amplifier driving the DC motor. Moreover, it mon-
itors the limit and synchronization switches as well as the current flowing in
the motor. The motor driving the motion of the gripper jaws is driven exactly
in the same way as the motors actuating the manipulator joints. All gripper
sensors, except the motor encoders, are treated as exteroceptors, thus they
are the source of data for the VSPs. Each jaw contains four resistance strain
gauges and either a LED or a phototransistor. Those sensors are served by
a single-chip T89C51AT2 microcomputer mounted in the gripper (Fig. 37.5).
It contains an 8-channel 10-bit A/D converter. A resistance voltage divider
is used to measure the force. The microcomputer also controls the in-hand
camera back-lighting – 16 levels of light intensity can be produced by white
light LEDs located around the lens of the camera. The host computer and the
microcomputer are connected by an RS-232 serial interface. When an object
; ; ;

;

;
;;;; ;;;; ;;;

; ; ;
; ; ;
;; ; ;

; ; ;
;;; ;;; ;
Fig. 37.4. Robot control hardware

;;
;
;%&
$

;;
;
!"
;#
;

;

;;
;
;%&

; $

;

; !"

;#

;;;;;;;
;

;

Fig. 37.5. The gripper control hardware
is being grasped, the current in the actuating motor increases. A constant

over-current is dangerous for the gripper motor. Hence, there are some safety
mechanisms introduced (both hardware and software) to monitor this cur-
rent. The software mechanism consists of a finite state automaton having two
states: ordinary work state and blocked state. The gripper is position con-
trolled. When the jaws of the gripper are not clenched, the automaton is in
the ordinary work state. If the measured current crosses a certain threshold,
the state is changed to the blocked state and as a result the effective mo-
tor current value is cut down to zero. This is possible, because the gripper
mechanism is self-locking. After a certain time the automaton returns to the
ordinary work state, to check whether the object is not slipping from between
the jaws. The automaton is implemented within the EDP SERVO thread –
namely the part dealing with servo-control of the gripper motor.
37.6 Experiments
In the experiments the forces and torques measured by the force/torque sensor
mounted in the wrist of the robot initially holding the cube were monitored.
Figure 37.6 presents the following phases of task realization.
3
[Nm] D
2.5
1.5 E
G
F
torque
0.5
A
0
H
-0.5
C
-1 B
-1.5
4.64 4.65 4.66 4.67 4.68 4.69 4.7 4.71
5
step x 10
Fig. 37.6. Torque around axis of rotation of the face of the cube (step =
2ms)
Initially the robots operate separately (until point A in the plots). Once
the grasping operation starts the torque mounts. The cube holding robot is
immobile and position controlled while the other robot is compliant in the
plane of the cube face that is to be rotated (all other directions of translation
and rotation are position controlled). Once the cube is grasped securely the
torque stabilizes (point B). The rapid change in the torque appears when
the rotation of the cube face is initiated (point C). Now the grasping robot
becomes immobile and position controlled while the robot initially holding
the cube is position controlled in two rotational directions, all three trans-
lational directions are compliant, while the direction of the face rotation is
torque controlled. This torque is set to 5 Nm, however the highest measured
value is 3 Nm. Initially the rotated face was jammed – this can be seen from
the plot: initially the torque mounts (until point D), then it suddenly drops
and becomes relatively constant (here mainly viscous friction is the cause) –
until point G. At point E the friction mounted quite quickly, however the mo-
tion was sustained and then gradually the friction dropped (until point F ).
Then again the friction increased. This proves that the production process
of the cube is far from perfect (perhaps this could be used in cube quality
control). Between points G and H the gripper is being opened (both arms are
immobile). The closed kinematic chain is being disjoined.
37.7 Conclusions
The integration of the gripper equipped with diverse sensors into a MRROC++
based controller proved fairly straight-forward once it was decided which sen-
sors will be treated as proprioceptors and which as exteroceptors. The mea-
surements obtained from the former (e.g., motor encoders) are utilised within
the EDP, thus reducing the length of the feedback loop from sensing to action.
This reduction increases the servo-sampling rate, in consequence increasing
the gain of the controller and reducing the problems with its stability. The
remaining gripper sensors are treated as exteroceptors, thus their measure-
ments are aggregated in the VSPs. This produces a longer loop, composed of
the VSP, ECP (or even MP combined with ECP) and EDP. The information
aggregated by the VSP is used by the MP or the ECP to generate the de-
sired motion. In this case the reactions are slower – usually this loop takes
several sampling periods (a macrostep) to execute. However, it works in con-
junction with the much faster loop using the proprioceptors. In fact, this can
be treated as a supplementary distinguishing factor in the case where there is
doubt whether a particular sensor should be treated as a proprioceptor or an
exteroceptor. Usually complex sensors or conglomerates of simpler sensors, re-
quiring elaborate computations leading to data aggregation, should be treated
as exteroceptors, and thus a VSP has to be allotted to them. Proprioceptors
must deliver data in a much simpler form. Moreover, their readings must be
utilised as quickly as possible.
In animals (e.g. insects or vertebrates) proprioceptors deliver information
to the gait pattern generators which are located in the spinal cord. As long
as the sequence of the stimuli from the proprioceptors located in the limbs
does not vary considerably the gait pattern does not change [13, 12]. Only
if exteroceptors detect conditions requiring the change of this pattern the
generators are retuned by the brain. In this case we have a very good biological
example of the here postulated division of sensor roles and the length and
complexity of the feedback loops associated with both kinds of receptors.
Those observations are important from the point of view of the control
system structure. We have followed those hints when associating the sensors
mounted in the end-effector with the MRROC++ processes constituting a fairly
complex controller of a dual-robot system capable of solving a Rubik’s cube
puzzle handed over to it by a human [9].
References
1. H. Bruyninckx and J. De Schutter. Specification of force-controlled actions

in the task frame formalism: A synthesis. IEEE Trans. on Robotics and
Automation, 12(4):581–589, August 1996.
2. U. Castiello. The neuroscience of grasping. Nature Reviews:Neuroscience,
6(9):726–736, 2005.
3. T. Fong, I. Nourbakhsh, and K. Dautenhahn. A survey of socially inter-
active robots. Robotics and Autonomous Systems, 42(3–4):143–166, 2003.
4. Kawasaki. Intelligent dual-arm robot system for solving Rubik’s Cube.
http://www.khi.co.jp/kawasakiworld/english/video index e.html, 2005.
5. D. Kragic, M. Björkman, H. Christensen, and J.-O. Eklundh. Vision
for robotic object manipulation in domestic settings. Robotics and Au-
tonomous Systems, 52(1):85–100, 2005.
6. D. Li, J. Lovell, and M. Zajac. Rubiks Cube Solver. http://www.eecs.
umich.edu/courses/eecs373/Labs/Web/W05/Rubic Cube Web/top.html,
2005.
7. J. Pomares and F. Torres. Movement-flow-based visual servoing and force
control fusion for manipulation tasks in unstructured environments. IEEE
Trans. on Systems, Man, and Cybernetics Part C, 35(1):4–15, 2005.
8. P. Redmond. Rubot II. http://mechatrons.com/, 2006.
9. W. Szynkiewicz, C. Zieliński, W. Czajewski, and T. Winiarski. Control
Architecture for Sensor-Based Two-Handed Manipulation. In T. Zielińska
and C. Zieliński, editors, CISM Courses and Lectures - 16th CISM–
IFToMM Symposium on Robot Design, Dynamics and Control, Ro-
ManSy’06, number 487, pages 237–244, Wien, New York, June 20–24 2006.
Springer.
10. G. Taylor and L. Kleeman. Visual Perception and Robotic Manipulation.
Springer, Berlin Heidlberg New York, 2006.
11. D. Xiao, B.K. Ghosh, N. Xi, and T.J. Tarn. Sensor-based hybrid posi-
tion/force control of a robot manipulator in an uncalibrated environment.
IEEE Transactions on Control Systems Technology, 8(4):635–645, 2000.
12. T. Zielińska. Motion synthesis. In F. Pfeiffer and T. Zielińska, edi-
tors, Walking: Biological and Technological Aspects, pages 155–191. CISM
Courses and Lectures No.467. Springer, Wien, 2004.
13. T. Zielińska. Biological inspirations in robotics: Motion planning. In 4th
Asian Conf. on Industrial Automation and Robotics, ACIAR’2005, 11-13
May, Bangkok, Thailand. 2005.
14. C. Zieliński. The MRROC++ System. In First Workshop on Robot Mo-
tion and Control, RoMoCo’99, pages 147–152, June 28–29 1999.
15. C. Zieliński, A. Rydzewski, and W. Szynkiewicz. Multi-robot system con-
trollers. In 5th Int. Symp. on Methods and Models in Automation and
Robotics MMAR’98, Miȩdzyzdroje, Poland, volume 3, pages 795–800. Au-
gust 25–29 1998.
16. C. Zieliński, W. Szynkiewicz, and T. Winiarski. Applications of MR-

ROC++ robot programming framework. In Proceedings of the 5th In-
ternational Workshop on Robot Motion and Control, RoMoCo’05, Dy-
maczewo, Poland, pages 251–257. June, 23–25 2005.
38
Detecting Intruders in Complex Environments
with Limited Range Mobile Sensors
Andreas Kolling and Stefano Carpin
University of California, Merced, School of Engineering, Merced, CA, USA

scarpin@ucmerced.edu
Summary. This paper examines the problem of detecting intruders in large

multiply-connected environments with multiple robots and limited range sen-
sors. We divide the problem into two sub-problems: 1) partitioning of the
environment and 2) a new problem called weighted graph clearing. We reduce
the latter to weighted tree clearing and present a solution for finding paths
for the robot team that ensure the detection of all intruders with a proven
upper bound on the number of robots needed. This is followed by a practical
performance analysis.
38.1 Introduction
Multi-target surveillance by a team of mobile robots as one of the more ac-
tive and novel research areas in robotics has received particular attention in
recent years. Its core problems are at an interesting cross-section between
path-planning, map building, machine learning, cooperative, behavioral, and
distributed robotics, sensor placement, and image analysis. We are currently
investigating the problem of detecting intruders in large and complicated en-
vironments with multiple robots equipped with limited range sensors. The
problem is closely related to pursuit-evasion and cooperative multi-robot ob-
servation of multiple moving targets, short CMOMMT, which we are going
to discuss shortly in Section 38.2. The main contribution of this paper is to
present a starting point for an approach for a team of robots with limited
range sensors. We divided the initial problem into two sub-problems: 1) to
partition the environment into a graph representation and 2) to clear the
graph from all possible intruders with as few robots as possible.
For the remainder of this paper we are going to focus on the latter problem.
Interestingly, the division has the effect that we can present an algorithm
to compute a clearing strategy on the graph without considering the robots’
capabilities directly. Hence with some further work the algorithm is extendable
to teams of heterogeneous robots. The clearing strategy consists of paths for
springerlink.com
418 A. Kolling and S. Carpin
the robots on the vertices and edges of the graph. When executed it ensures
that any arbitrarily fast moving target present in the environment will be
detected. To find a clearing strategy we need to solve a graph theoretical
problem which we dub weighted graph clearing, introduced in Section 38.3.1.
In Section 38.3 we provide a solution for the problem with a proven upper
bound followed by some experiments in Section 38.4. Finally, our results and
an outlook on the first sub-problem are discussed in Section 38.5.
38.2 Related Work

The two research areas most closely related to our work are CMOMMT and
pursuit-evasion. CMOMMT is related due to its focus on multi-robot teams
in large environments with sophisticated cooperation. The term was coined
by Parker who produced the first formal definition of the problem and an
algorithm in [2]. It focuses on multi-target surveillance in large environments
with mobile robots with limited range sensors with the goal to optimize re-
source usage when resources are not sufficient to surveil all targets. Recent
improvements were presented in [1] by introducing a new distributed behav-
ioral approach, called Behavioral-CMOMMT. The improvements are due to
an advanced cooperation schemes between robots which boost the overall per-
formance.
Pursuit-evasion is usually tackled from the perspective of the pursuer, i.e.
the goal is to try to make impossible for the evader to escape. Most current
solutions for this pursuit-evasion problem, however, require unlimited range
sensors. In [3] an algorithm for detecting all targets in a subclass of all simply
connected environments is presented. The restrictions on the subclasses are
relatively few and the performance of the algorithm is proven. It guarantees
to find a path to clear the environment of all targets using only a single
pursuer with a gap sensor with the full field of view and unlimited range. The
drawbacks are frequently revisited areas and hence long clearing paths. In [4]
and [5] a previous algorithm for a single pursuer is presented in more detail
and in [4] interesting bounds on the number of robots required for certain free
spaces are established. For simply-connected free spaces a logarithmic bound
is obtained, while for a multiply-connected environment a bound in the square
root of the number of holes is given. Furthermore, finding the number of robots
needed to clear an environment was shown to be NP-hard. Gerkey et al. in
[6] extended the previous algorithm to a varying field of view.
Pursuit-evasion solutions draw most of their information from a particular
method of partitioning to find a clearing strategy. This will be useful for
extensions of this paper when the first sub-problem is discussed and related
to the weighted-graph clearing problem. For now we shall present a solution
that is independent of any partitioning but yet a powerful method in the
divide and conquer spirit.
38 Detecting Intruders with Limited Range Mobile Sensors 419
38.3 The Algorithm
To prepare the grounds for the algorithm we will define our notions of envi-
ronment, regions, adjacency, and contamination. An environment, denoted by
S, is a bounded multiply connected closed subset of R2 . A region is a simply
connected subset of S. Let r1 and r2 be two regions and let r̄1 and r̄2 denote
their closures. If r̄1 ∩ r̄2 6= ∅, then we say that r1 is adjacent to r2 and vice
versa. Let S g = {r1 , . . . , rl } be a partition of S with l regions. Let Ari be the
set of all adjacent regions to ri . Define a binary function for each region ri
on its adjacent regions and across time: bri : Ari × R → {0, 1}. Let rj ∈ Ari .
If bri (rj , t) = 1, we shall say that the adjacent region rj is blocked w.r.t. ri
at time t. Define another binary function c : S g × R → {0, 1}. If c(r, t) = 1,
then we shall call region r contaminated at time t, otherwise clear. Let Acri
be the set of all contaminated adjacent regions to ri . Let c have the following
property:
∀ǫ > 0 : c(r, tc + ǫ) = 0 ⇐⇒ c(r, tc ) = 0 ∧ b |Acr (t + ǫ) = 1. (38.1)

i
Now, if c(r, tc ) = 0 and ∃ǫ > 0 s.t. ∀t ∈ (tc , tc + ǫ], c(r, t) = 1, then we say
that r is re-contaminated after time tc .
These definitions give us a very practical notation for making statements
about the state of regions. In particular, it follows that a region becomes re-
contaminated when it has a contaminated adjacent region that is not blocked.
We are now going to define two procedures, called behaviors, which set the
functions c and b, i.e. enable us to clear and block.
Definition 1 (Sweep and Block). A behavior is a set of instructions as-

signed to one or more robots. The two behaviors are defined as follows:
1. Sweep: a routine that detects all invaders in a region assuming no invaders
enter or leave the region. After the execution of a sweep on a region r at
time t we set c(r, t) = 0, i.e. not contaminated1 .
2. Block: a routine that ensures that no invaders enter from or exit to adja-
cent regions. For a region r a block is executed on an adjacent region ra in
Ar . A block on ra is defined as covering the boundary of r intersected with
ra with sensors, i.e. δr̄ ∩ r̄a ⊂ sensor coverage, where sensor coverage
is the area covered by all available sensors of all robots.
Furthermore, let us denote a sweep as safe if and only if during the execution
of the sweep we have b |Acr = 1. It follows that after a safe sweep a region
i
remains clear until a block to a contaminated region is released.
All the previous can be understood as the glue that connects the two sub-
problems, partitioning and finding the clearing strategy. Partitioning returns
the graph representation of the environment into which one can encode the
1
Therefore we assume that when a robot detects and intruder, the intruder is
neutralized and cannot operate anymore.
number of robots needed for the behaviors on the edges, the blocks, and the
vertices, the sweeps, by adding a weight to them. The values of the weigths
are specific to the robots’ capabilities and methods used for sweeping and
blocking. Depending on the partitioning, very simple methods can be used.
We assume a homogeneous robot team from here on, which makes the inter-
pretation of these weights easier and the notation for the following simpler to
follow. We now define the main problem that we are going to investigate, i.e.
weighted graph clearing. The task is to find a sequence of blocks and sweeps
on the edges and vertices of an entirely contaminated graph such that the
graph becomes cleared, i.e. not contaminated, with the least number of ro-
bots. This sequence of blocks and sweeps will be our clearing strategy to detect
all intruders.
38.3.1 Weighted Graph Clearing
We are given a graph with weighted edges and vertices, denoting costs for
blocks and sweeps. The goal is to find a sequence of behaviors to clear the
graph using the least number of robots. As the first step we attempt to reduce
the complexity of the problem by reducing the graph to a tree by removing
occurring cycles. Cycles can be eliminated by executing a permanent block
on one edge for each cycle. The choice of such edge will influence the tree
structure and hence our final strategy. This is a topic that requires further
investigation, as discussed in Section 38.5. Recall the partition S g in form of
a planar graph. Let the weights on the edges and vertices be denoted by w(ei )
on edges and w(ri ) on vertices and all be non-zero. The weight of a vertex
is the number of robots needed to complete a sweep in the associated region,
while the weight of an edge is the number of robots required to block it. To
transform S g into a tree we compute the minimum-spanning-tree (MST) with
the inverse of the w-weight of the edges. Let B be the set of all removed edges
g
to get the MST. The
P costs of blocking all edges in B and transforming S into
a tree is b(B) = ei ∈B w(ei ) and as such the minimum possible. From now
on this cost is constant and will henceforth be ignored.
38.3.2 Weighted Tree Clearing
Recall that a safe sweep requires all adjacent regions to be blocked. It is

clear that we have to block contaminated and cleared regions alike, since the
latter may get re-contaminated. Hence for a safe sweep for a region ri we
need s(ri ) = sumej ∈Edges(ri ) w(ej ) + w(ri ) robots. After a safe sweep all
blocks on edges to cleared regions can be released. To remove the blocks to
contaminated regions we have to clear these first. This suggests a recursive
clearing of the regions. To capture this we define an additional weight on each
edge, p(ej ), referred to as p-weight. For clarity: the p-weight for edges is the
number of robots needed to clear the region accessible via this edge and all
its sub-regions into the respective direction. Once we have the p-weight we
still have to choose at each vertex which edges to visit first. Let us define the
order by p(ej ) − w(ej ) with the highest value first.
The following algorithm computes the p-weight and implicitly selects a
vertex as the root, which is the starting point for the clearing. For each region
r, let Edges = {en1 , . . . , enm(r) } denote the set of edges.
1. For all edges ej that are edges to a leaf ri set p(ej ) = w(ej ) + w(ri ).
2. For all vertices ri that have its p-weight assigned for all but one edge,
denoted by en1 , do the following:
a) Sort all m − 1 assigned edges w.r.t to p(ej ) − w(ej ) with the highest
value first. Let the order be en2 , . . . , P
enm .
b) Determine h := max2≤k≤m (p(eni ) + 1<l<k w(enl )).
c) For the unassigned edge en1 of ri set p(en1 ) = max(s(ri ), h).
3. Repeat from step 2 until all edges have p assigned.
The root selected is one of the vertices connected with the last assigned
edge and will be in the center of the longest path from a leaf to another leaf.
Uniqueness is resolved trivially by a random selection of valid roots as done
by the algorithm. One could use a procedure that computes the p-weight in
both directions, so one does not have to choose a root and is independent of
directions until the final deployment of the robots. This would merely compli-
cate the notation, but since the theoretical qualities are identical we chose to
present a simpler version of the algorithm. To clarify the notation: edges are
noted w.r.t to vertex r and en1 is always the edge from vertex r to the root
while the order of the remaining edges w.r.t. p(ej ) − w(ej ) is en2 , . . . , enm . We
can now state the main theorem, namely the bound for the maximum number
of robots needed to clear a tree.
Theorem 1. Let hmax := max∀r (s(r)) > 2. Let d be the length of the longest
path in the tree S g and d∗ = ⌈d/2⌉. The maximum p-weight of an edge is
bounded by
max (p(e)) ≤ hmax + (d∗ − 1) · (hmax − 3). (38.2)

e∈Edges(S g )
Proof: The proof proceeds by identifying the worst case for a non-leaf vertex
and then starting at the worst case for leaves, construct the worst case tree.
Now, consider any vertex r that is not a leaf. Write m for m(r) and E ′
for Edges(r) \ {en1 , enm }. We assign p(en1 ) using {en2 , . . . , enm } with p(eni )
assigned for i > 1. W.l.o.g. assume that m ≥ 3, otherwise
P we have p(en1 ) =
max(p(en2 ), s(r)). Since s(r) ≤ hmax and s(r) = ej ∈Edges(r) w(ej ) + w(r)
we get
X
w(ei ) ≤ hmax − 2.
ei ∈Edges(r)\{en1 }
Consider the worst case for the last edge

P enm to be cleared from r. All other
edges have to be blocked with cost ei ∈E ′ w(ei ) while we clear sub-regions
beyond enm with cost p(enm ). Due to the ordering of edges we know that
p(enm ) − w(enm ) is smaller than for other edges. The worst case costs oc-
cur
P when the p weight of enm is as big as possible while at the same time
ei ∈E ′ w(ei ) is large. Trivially we have
X
w(ei ) ≤ hmax − 3.
ei ∈E ′
For p(enm ) we know
p(enm ) ≤ p(eni ) − w(eni ) + w(enm )
due to the ordering. Hence it follows that the worst case occurs when
p(enm ) = max(p(eni )), w(eni ) = 1, m = hmax − 2.
So we get p(eni ) = p(enj ), ∀j, i > 1. A quick proof by contradiction shows that
the last edge in the worst case is the worst edge. Now the bound for p(en1 ) if
p(enm ) > 2 becomes
p(en1 ) ≤ p(enm ) + hmax − 3.
(Note: if p(enm ) = 2 then we may get p(enm ) + hmax − 3 = hmax − 1 and

violate that possibly p(en1 ) = s(r) = hmax .)
We can now use this to derive a bound for the entire tree. We start with
the leaves at worst case hmax for a safe sweep, so all p-weights at the first stage
are hmax . Now we assume the worst case for all other vertices. This gives us
max {p(e)} ≤ hmax + (d∗ − 1) · (hmax − 3).

e∈Edges(S g )
Λ
The number of robots needed can easily be computed by adding a “virtual”
edge to the root vertex and computing its p-weight. Figure 38.1 illustrates a
worst case example with d = 6, hmax = 6 and the resulting size of the robot
team of 16. The actual exploration proceeds by choosing the subtree with the
highest p(ei ) − w(ei ) first, then once it is explored blocking it and continuing
with the new one in order until the last subtree is cleared. The example from
Fig. 38.1 is generic and establishes the bound as tight for any d and hmax .
It, however, only occurs in exactly such an example in the sense that we
need all vertices and leaves to have exactly the structure for the worst case as
indicated in the proof and figure. When seeking the optimal root vertex for the
deployment one can use the same principal method and calculate p-weights
in both directions, append a virtual edge to each vertex and then choose the
vertex with the smallest p-weight for this virtual edge.
Fig. 38.1. This is the worst case Fig. 38.2. A comparison of the aver-
example for d = 6, hmax = 6, p∗ = age upper bound across 1000 weighted
6 + 2 · 3 = 12. Blocking weight w is trees and actual maximum p-weight
indicated left and p-weight right of for varying number of vertices
: on the edge. Each vertex has its
weight in its cente.
38.4 Investigation of Performance

To show the practicability of the algorithm we ran the recursive sweep on
randomly generated weighted trees. Trees with 20, 40, 60, 80, and 100 vertices,
random edges, a random weight for a vertex between 1 and 12, and a random
weight for edges between 1 and 6 were generated. For each number of vertices
a forest of 1000 trees was created. The average values for the maximum p-
weight, d∗ , and hmax are presented in Table 1. Figure 38.2 compares the upper
bound computed from d∗ and hmax with the average maximum value of p.
38.5 Discussion and Conclusion

We presented a first approach for a complex problem and started at dissecting
it into the two sub-problems that can be paraphrased into the two major
challenges of understanding the environment and then coordinating the team
effort. We presented a solution to coordinate the team effort with a central
instance by solving the weighted graph clearing problem. The benefit is that
we can now partition an environment into many simple regions for which
one can easily find a sweeping routine for limited range sensors. Particularly
suitable are environments such as buildings for which we would suggest a
partitioning into regions that are rooms and hallways. The sweeping of a room
can then be done by aligning the robots, and then the sensors, on a line. On
our path towards a fully working system there are two sets of questions arising.
Table 38.1. Results of the experiments. The values are averaged across 1000
random trees. The max(p(e)) is the largest p-weight occuring in the tree,
without any virtual edges attached, while hmax is the largest s-weight of the
vertices. Note that if the root has the largest s-weight it may be that hmax >
max(p(e)), which is often the case with smaller trees since the root on average
has the most edges.
n Vertices max(p(e)) d* hmax

20 24.865 5.325 25.176
40 28.300 7.784 27.949
60 30.299 9.638 29.607
80 31.781 11.077 31.039
100 32.885 12.306 31.909
The first set are those regarding the theoretical part, i.e. solving the graph
clearing problem without using the heuristic of the constant cycle blocks. First
of all, choosing the cycle blocks defines the tree structure and hence influences
the resulting strategy. We might get a better strategy if we chose the block
w.r.t. to this criteria. Secondly, once we have cleared two regions connected
by an edge that has a cycle block it becomes redundant. We could free the
block and gain resources and we would want to free these additional resources
exactly when we need them, namely when we enter regions with high weights.
The second set of questions are those regarding the partitioning. One needs
to determine good methods for partitioning depending on the environment
and capabilities of the robots. Also we need to conceive criteria for partitions
that lead to good strategies on the graph. A first hint of the theorem is that
deep trees should be avoided. Understanding the theoretical properties of the
weighted graph clearing problem first, might already give us good insight on
how to start creating good partitions. Then we can work on devising good
partition strategies in coordination with sweeping and blocking methods for
particular sensors and close the loop to a working system.
References
1. A. Kolling, S. Carpin, “Multirobot Cooperation for Surveillance of Mul-
tiple Moving Targets - A New Behavioral Approach,” Proceedings of the
2006 IEEE International Conference on Robotics and Automation, pp.
1311–1316
2. L. E. Parker, “Distributed algorithms for multi-robot observation of mul-
tiple moving targets,” Autonomous robots, 12(3):231–255, 2002.
3. S. Sachs, S. Rajko, S. M. LaValle. “Visibility-Based Pursuit-Evasion in an
Unknown Planar Environment,” The International Journal of Robotics
Research, 23(1):3-26, 2004
4. L. J. Guibas, J. Latombe, S. M. LaValle, D. Lin, R. Motwani, “Visibility-

Based Pursuit-Evasion in a Polygonal Environment,” International Jour-
nal of Computational Geometry and Applications, 9(5):471–494, 1999
5. S. M. LaValle, D. Lin, L. J. Guibas, J. Latombe, R. Motwani, “Finding
an Unpredictable Target in a Workspace with Obstacles,” Proceedings of
the 1997 IEEE International Conference on Robotics and Automation,
pp. 737–724
6. B. P. Gerkey, S. Thrun, G. Gordon. “Visibility-based pursuit-evasion with
limited field of view,” The International Journal on Robotics Research,
25(4):299-316, 2006.
39
High-Level Motion Control for Workspace
Sharing Mobile Robots
Elżbieta Roszkowska

elzbieta.roszkowska@pwr.wroc.pl
39.1 Introduction
Coordination of the motions of multiple mobile robots as they perform their
task in a shared workspace is an important capability and a problem widely
studied in robotics. The theoretical studies in this area have mostly concen-
trated on motion planning with respect to collision avoidance and performance
optimization [1]. The realization of motion plans is, however, an open-loop,
time-based control, that is highly sensitive to system randomness. In a system
of autonomous, asynchronously operating robots, the eventual applicability of
such plans is rather questionable.
On the other hand, most research on the real-time control for multiple mo-
bile robot systems (MMRS) has been directed towards software development,
based on ad-hoc, rather than rigorous models, developed directly in program-
ming languages and providing no formal guarantee of their correctness. A
few works have proposed a more prospective approach to MMRS supervisory
control, employing Petri nets as a modeling formalism, e.g. [2, 3]. However,
also these papers focus on specific applications rather than deal with a gen-
eral methodology or essential robot coordination problems such as deadlock
avoidance.
As opposed to the above, in our previous papers [4, 5] we developed a
formal, DFSA (Deterministic Finite State Automaton) based systems ensuring
collision- and deadlock-free supervisory control for MMRS. In this work we
address a similar problem, but develop new solutions, as our goal is both
the logic of the workspace sharing and their implementation in robots as a
high-level velocity control.
39.2 Problem Statement

We consider a set of independent, autonomous robots that operate in a com-
mon workspace. The robots can communicate among themselves, e.g. through
springerlink.com
428 E. Roszkowska
the distributed blackboard mechanisms [6] or through peer to peer messaging

and/or message broadcasting [7]. Each robot has a path planner, which plans
its own path from the initial to the final location. At this stage no positional
constraints introduced by any other robot are taken into account. In order to
avoid collisions, the robots refine their velocity profiles1 , that is, a robot can
slow down and possibly stop its further motion until the situation changes
and it can safely resume further travel. Similar to the natural initial location
of the robots, where each robot has its own private part of the common space,
the destination positions of any two robots are presumed to be not in con-
flict. We do not make any assumptions on the robots’ dynamics except the
constraint on the stop-distance sd, i.e. the distance that is always sufficient
and acceptable for a robot to come to a full stop. In this paper we consider
it to be a robot specific constant, but the proposed approach can be directly
extended to a state dependent variable sd. Finally, it is assumed that the
robots know where they are, i.e., can determine their current coordinates in
the workspace. For such a system, the general problem that we are trying to
solve is as follows.
Find a control mechanism to dynamically modify the initially assumed ro-
bots’ motion control so that 1) in a finite time interval, all the robots will have
attained their destination locations, and 2) in each moment of this interval,
the subspaces occupied by any given pair of the robots are disjoint.
In the above statement, by the “initially assumed motion control”, we un-
derstand the control that a robot would apply if it did not share the workspace
with other robots. The requirement of its “dynamic modification” means that
changes in the robots velocity profile should be determined on-line, based
on the current information about the system state. In other words, we seek
a higher level closed-loop control enforcing admissible concurrent motion of
the robots through temporary changes in their velocity profiles. In order to
satisfy hard real-time response constraints, we need to employ a model that
reduces the complexity of the problem through an appropriate abstraction.
Consequently, we will represent the considered system of robots operating in a
common workspace as a Resource Sharing System (RSS), formally described
as a parallel composition of deterministic finite state automata (DFSA). The
component automata are dynamic models of the robots and the resource they
share - the workspace. Such an approach implies the following decomposition
of the above stated coordination problem:
1. discretization of the workspace and the robot motion processes;
2. development of the robot-DFSA and the resource-DFSA, whose dynam-
ics satisfy, respectively, the stop-distance constraint and the mutually-
exclusive use of particular resource units (which guarantees no collisions,
1
In this paper we only consider the coordination of robot motions by velocity
modifications. However, the proposed methodology can be extended to capture
also path plans’ refinement.
39 High-Level Motion Control for Workspace Sharing Mobile Robots 429
as the spaces occupied by distinct robots at any time moment will never
overlap);
3. restriction of the dynamics of the composite-DFSA so that the final state
can always be reached from any state reachable from the initial one (which,
in particular, implies deadlock avoidance).
39.3 Deterministic Finite State Automata

In this section, we shortly recall the main concepts of the DFSA [8] that will
be used in the synthesis of the high-level robot control.
Definition 1. A deterministic finite state automaton (DFSA) is a 5-tuple2

G = (S, E, Γ, δ, s0 ), where:
1. S is the set of states.
2. E is the set of events. The occurrence of an event causes a state transition
in G.
3. Γ : S → 2E is the feasible event function. Event e ∈ E is feasible (i.e.,
can occur) in state s ∈ S iff e ∈ Γ (s).
4. δ : S × E → S is the transition function. δ is a partial function defined
for pairs (s, e) such that e ∈ Γ (s). s′ = f (s, e) is the new state resulting
from the occurrence of event e in state s.
5. s0 ∈ S is the initial state.
If two or more automata require synchronization on particular common

events then they constitute a system defined as follows.
Definition 2. The parallel composition of automata G1 and G2 is the au-

tomaton G1 k G2 = (S1 × S2 , E1 ∪ E2 , Γ1k2 , δ, (s01 , s02 )) such that:
1. Γ1k2 = (Γ1 (s1 ) ∩ Γ2 (s2 )) ∪ (Γ1 (s1 ) \ E2 ) ∪ (Γ2 (s2 ) \ E1 )


 (δ1 (s1 , e), δ2 (s2 , e)) if e ∈ Γ1 (s1 ) ∩ Γ2 (s2 )

(δ1 (s1 , e), s2 ) if e ∈ Γ1 (s1 ) \ E2
2. δ((s1 , s2 ), e) =

 (s , δ
1 2 2(s , e)) if e ∈ Γ2 (s2 ) \ E1

undefined otherwise
39.4 Discretization of Robot Motion Processes

We start from a continuous representation of the Multiple Mobile Robot Sys-
tem (MMRS). The system consists of a set of mobile robots A ∈ A that share
a finite planar workspace W S ⊂ R2 with the XY coordinate system. The
2
We omit in this definition the set of marked states Sm originally included in [8],
as in this analysis we are not interested in marking any particular event sequence.
430 E. Roszkowska
robots are represented by circles with radius a and their safe stop-distance
is equal to sd. Moreover, each robot A is characterized by its specific path
π = π(A), viewed as a curve in W S that is given by a pair of functions
π(l) = (x(l), y(l)) ∈ R2 , l ∈ [0, l̄] ⊂ R. It is assumed that both at the start
and at the end of their paths the robots occupy mutually disjoint regions in
the workspace.
In the discrete abstraction of MMRS, we first scale the XY system so that
its new unit is equal to the robot radius a, and then use a grid of horizontal
and vertical lines that establish a set W of unit square cells that intersect
W S. Next, in each robot’s path π, we distinguish a sequence of border points
p, defined as follows.
Definition 3. For a path π, the sequence of its border points is a finite se-
quence p = [pj = (x(lj) , y(lj )) : j = 0, .., n] such that p0 = π(0), pn = π(l̄),
and for each j = 0, . . . , n − 2, lj+1 is the minimal value l > lj such that:
i) x(lj+1 ) is an integer or y(lj+1 ) is an integer, and ii) |x(lj+1 ) − x(lj )| ≥ 1
or |y(lj+1 ) − y(lj )| ≥ 1. A path sector πj is a sub-path of π(l) such that
l ∈ [lj−1 , lj ].
Remark 1. Notice that condition (i) requires that each point pj (except, pos-
sibly, the start-point and the end-point of π) lies on a grid line, condition (ii)
implies that the length of each path sector (except, possibly, πn ) is not less
than 1 (i.e., the scaled radius of robot A), and these results, together with the
requirement of minimization of l, establish that no path sector πj intersects
more than two grid cells.
An example of the border-points selection for two robots, A and A′ , is

given in Fig. 39.1. We note that path π = π(A) starts and ends at the same
point p0 = p43 , and all the points on the horizontal line are traversed twice.
Based on the path-partition concept, we discretize the robots’ travel processes.
Definition 4. For a path π, the stage set of π is the set of triples Z = {zj =
(j, pj , Crj )} such that ∀j = 1, . . . , n, j is the stage number, pj is the j-th
border point on π, and Crj ⊂ W is the set of all cells intersected by sector πj ,
called the center of zj . The union of Crj and the set of all cells adjacent to
k=min(j+q,n)
Crj is the region Cj of zj . The safety zone of zj is SZj = Cj+1 =
Sk=min(j+q,n)
k=j+1 Ck , i.e., the union of the regions of the q stages succeeding zj or
all the stages succeeding zj if n < j + q, where q ≥ ⌈sd⌉ is the safety distance3 .
The left part of Fig. 39.2 shows the centers of the stages associated with the
sectors of path π ′ of robot A′ , given in Fig. 39.1. The middle part of Fig. 39.2
depicts the stages’ regions, and the right part illustrates the concept of the
safety zone for the safety distance q = 3. Notice that, according to Remark 1,
3
⌈x⌉ and ⌊x⌋, x ∈ R, denote the ceiling and the floor functions, i.e., the smallest
(the greatest, resp.) integer equal to or greater (less, resp.) than x.
any center occupies either one or two cells. Hence, the region of each stage zj ,
Cj , consists of either 9 or 12 cells, while the safety zone of zj , SZj , extends
this area by at least the union of the regions Cj+1 , Cj+2 , . . . , Cj+⌈sd⌉ .
10
9 p1,16
8 p1,0 = p1,43 p1,8 = p1,35
7 p1,21
p1,28
6
5
p2,4
4
3
2
p2,0 p2,11
1
0
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24
Fig. 39.1. Illustration of the path partition given in Def. 3
7 7 7
6 6 6
5 5 5
4 4 4
3 3 3
2 2 2
1 1 1
0 0 0
2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17
Fig. 39.2. Example illustration of the notions given in Def. 4: stage centers
(left), stage regions (middle), and z1 with its safety zone (right) for q = 3
39.5 DFSA Model of Distributed High-Level Control

The concept of collision avoidance is based on the mutually exclusive use of
cells by robots. A robot can freely move within the area it has acquired, as
no other robot can access it until the cells are returned to the pool. In the
discrete model of MMRS, the actual location of the center point of robot A is
known with the accuracy to the area covered by the center Crj of its current
stage zj . Since the radius a of the circle representing the robots is a unit, at
stage zj , A can intersect any, but no other cells than those, which belong to
region Cj . However, this area is only sufficient to accommodate a motionless
robot. A moving robot needs at least its safety zone in order to be able to stop
within the currently possessed regions in the case that it can not acquire more
area for continuing the travel. Consequently, a robot can move from region zj
to region zj+1 , j = 1, . . . , n − 1, if it has acquired the safety zone SZj . Yet,
having entered zj+1 , it has to start decelerating unless it has also managed to
acquire the region succeeding SZj , Cj+d+1 . Having left region zj and entered
432 E. Roszkowska
region zj+1 , the robot gives back to the pool all the cells that are no longer
needed at the current stage.
The above characterized operation can be enforced by the following au-
tomaton, implemented in the robot controller at its top control-level.
Definition 5. The automaton model of robot A is a DFSA RobA=(S, E, Γ, δ, s0 )

such that:
1. the state set S is a set of triples s = (j, µ, B), where:
• j ∈ 1, . . . , n is the stage number
• µ ∈ {↑, →, ↓, ∼} is the operation-mode symbol denoting, respectively,
acceleration, stable motion (with the velocity v(π(l)) determined by
the initial plan for robot A), deceleration (with retardation sufficient
to come to rest within the safe distance), and no-motion
• B ⊂ W is the subset of cells allocated to the robot
2. the event set E is the union of the following disjoint sets of events:
i+q
• Ef = {fi = (i, Ci+1 − C i ) : i = mq + 1, m = 0, 1, . . . , ⌊n/q⌋}, where
4
(fi ; 1) is the identifier of event fi and fi represents acquisition of the
space (fi ; 2), which complements the robot’s current possession to the
safety zone SZi
i−1
• Ea = {ai = (i, Ci − Ci−1−q ) : i = q + 2, . . . , n}, where ai represents
acquisition of the missing region (ai ; 2) of stage zi ,
i+1+q
• Ed = {di = (i, Ci − Ci+1 ) : i = 1, . . . , n − 1}, where di represents
transition from stage zi to stage zi+1 (robot A passes point pi on its
path π) and release of the no-longer-needed space (di ; 2)
• Em = {max, stop}, representing transitions to the stable state, and to
the rest state, respectively
3. Γ (s) qualifies event e ∈ E as feasible in state s = (j, µ, B) iff
• e = fj and ZSj 6⊂ B
• e = aj+q+1 and Cj+q+1 6⊂ B
• e = dj and µ 6=∼
• e = max and µ =↑
• e = stop and Cj+1 6⊂ B
′
4. the next state s = δ((j, µ, B), e) is given as follows:
• if e = fj then s′ = (j, µ, B ∪ (fj ; 2))
• if e = aj+q+1 then s′ = (j, µ, B ∪ (aj ; 2))
• if e = dj and Cj+q+1 ⊂ B then s′ = (j + 1, µ, B − (dj ; 2))
• if e = dj and Cj+q+1 6⊂ B then s′ = (j + 1, ↓, B − (dj ; 2))
• if e = max then s′ = (j, →, B)
• if e = stop then s′ = (j, ∼, B)
5. the initial state s0 = (1, ∼, C1 ).
4
For a vector x, by (x; i) we denote the i-th component of x.
As an example, Fig. 39.3 depicts the transition graph5 of the DFSA RobA
for a robot A with stage set Z consisting of 5 stages, and the safety distance
q = 2. Notice that the trajectory from the initial state, (1, ∼, C1 ), to the final
state, (5, ∼, C5 ), that includes the top row of the states corresponds to the
initial robot’s plan for traveling along path π, as no deceleration is enforced.
The trajectory that includes the very bottom row of the states includes state
(3, ∼, C3 ), when the robot is forced to a temporary rest, and then resumes its
travel and eventually attains the final state.
a4 d1 a5 5 d2
max 1 C13 1 C41 2 C24 2 C2 3 C35
d1 max max max max
d2 d3
3 4 d1 4 5 d2 5 5
1 C1 1 C1 2 C2 a5 2 C2 3 C3 4 C4
a4 d3
a4 a5 d4
d1 2 C2
3
3 C3 d2 3 C34 5 C5
f1 d2 f4
stop
stop
f4
1 ~ C1 3 ~ C3 5 ~ C5
Fig. 39.3. Transition graph of RobA for a robot A with |Z| = 5 and q = 2
39.6 Coordination of Multiple Robot Motion

The actual trajectory executed by robot A is determined dynamically, based
on the current state of the workspace that is viewed as a set of discrete re-
sources w ∈ W . The logics of the resource sharing are given by the following
DFSA.
S S
Definition 6. Let E ′ = A∈A (EfA ∪EaA ) and E ′′ = A∈A EdA be the unions of
the respective events of of robots A in set A, and let W be the resource set of the
workspace W S. The automaton model of W S is a DFSA Res = (S, E, Γ, δ, s0 )
such that:
1. S = 2W , where state s ∈ S represents the set of the currently available
resources w ∈ W (i.e., not allocated to the robots)
2. E = E ′ ∪ E ′′
3. e ∈ Γ (s) ⇔ e ∈ E ′ ∧ (e; 2) ⊆ s or e ∈ E ′′
s \ (e; 2) if e ∈ E ′
4. s′ = δ(s, e) =
s ∪ (e; 2) if e ∈ E ′′
5. s0 = W .
5
The transition graph of a DFSA is a directed graph, whose vertices represent
the states of an automaton, arcs depict state changes, and arc-labels denote the
events that cause these transitions.
434 E. Roszkowska
Since the event sets of the robot and the resource DFSA are not disjoint,
they operate as a system of loosely coupled objects, formally defined as follows.
Definition 7. The DFSA model of a system of robots sharing a common
workspace is the parallel composition of the workspace and the robot automaton
models, M M RS = Res k (kA∈A RobA ).
By Def. 2, the above defined control model requires that in order to occur,
a resource acquisition or release event should be feasible in both the workspace
and the respective robot model, and its occurrence causes a synchronous state
transition in the two automata. Consequently, implementation of the resource
automaton must ensure that only one robot can read and modify the state
of Res at a time. This can be done in either a centralized manner (e.g., with
the blackboard mechanism) or in a distributed way, where each robot has a
copy of the Res, but their updates are controlled with a mutual-exclusion
mechanism (such as, e.g., the token ring) and communicated to all the robots.
Note that the developed system enjoys the first of the required properties.
Property 1. The control defined by M M RS guarantees no collisions.
Proof. Notice that the DFSA Res ensures that no cell can be allocated to
more than one robot at a time, and the DFSA Rob enforces a robot to slow
down and eventually stop if the region(s) required for its further movement
have been allocated to other robots.
39.7 Deadlock Avoidance

To satisfy the second of the properties posed in Section 39.2, demanding that
all the robots complete their travels in a finite time, it is necessary to ensure
that if a robot is enforced to a stop due to the lack of some resources, in
a finite time these resources will be returned to the pool and the robot will
be able to acquire them and resume its trip. A phenomenon that can make
it impossible is the deadlock. A deadlock occurs if there is a closed chain of
robots, each of which is waiting for some resources that are currently possessed
by its predecessor in the chain and cannot return the possessed resource until
it receives those that it is waiting for. In the transition graph of M M RS,
such a situation appears as a state s from which there is no path to the final
state. Avoidance of such states requires a DAP (deadlock avoidance policy)
∆(s) ⊆ Γ (s), s ∈ S, that considers fewer transitions as admissible than Γ (s),
and thus ’trims’ the transition graph so that deadlocks are not reachable. Due
to that the size of the transition graph is an exponential function of the size of
the corresponding automaton, the problem of minimally-restrictive deadlock
avoidance is typically NP-hard. Hence, practically useful are sub-optimal, less
permissible, but also less complex policies.
We note that the developed automaton MMRS is a particular case of the
CON-RAS, a class of the RAS (Resource Allocation Systems) abstraction
[9], for which there exist suboptimal polynomial DAPs, e.g., [10, 11]. These
policies can be directly implemented in M M RS, thus yielding a system that
satisfies the qualitative control requirements stated at the end of Section 39.2.
39.8 Conclusion
This paper contributes with a distributed system for coordinating the motion
of robots sharing a common workspace. The concept assumes an additional,
supervisory level of robot motion control, that enforces temporary velocity
reductions in order to avoid collisions and deadlocks among the robots. The
event-driven mechanism underlying the developed DFSA model ensures a ro-
bust coordination of mutually asynchronous robot controllers, and the mathe-
matical character of the employed abstraction formally guarantees the control
correctness.
The general character of the proposed high-level control model allows its
employment in MMRS-s that widely differ in the construction and application
of the component robots, a spectrum that can include, e.g., a fleet of mobile
robots used in industry or a sea terminal for transport purposes, a group of
autonomous vacum-cleaners or lawn mowers, as well as a system of social
robots serving as guides in a museum.
Further research will deal with a number of problems emerging from the
proposed approach, including its extension over on-line paths’ modification,
grid size in the context of its influence on the communication load and its
constraints, and the efficiency control through robot-task dependent prioriti-
zation of their access to conflict workspace regions.
References
1. LaValle, S.: Planning Algorithms. Cambridge University Press (2006)
2. Noreils, F.: Integrating multirobot coordination in a mobile-robot control
system. In: IEEE Int. Work. Intelli. Robots and Systems. (1990) 43–49
3. Liu, Y.H., Kuroda, S., Naniwa, T., Noborio, H., Arimoto, S.: A practi-
cal algorithm for planning collision-free coordinated motion of multiple
mobile robots. In: IEEE Int. Conf. Robot. Automat. (1989) 1427–1432
4. Roszkowska, E.: Supervisory control for multiple mobile robots in 2D
space. In: IEEE Int. Workshop Robot Motion Control. (2002) 187–192
5. Roszkowska, E.: Provably correct closed-loop control for multiple mobile
robot systems. In: IEEE Int. Conf. Robot. Automat. (2005) 2810–2815
6. Harmon, S., Aviles, W., Gage, D.: A technique for coordinating au-
tonomous robots. In: Int. Conf. Robot. Automat., (1986) 2029 – 2034
7. Wang, Z., Zhou, M., Ansari, N.: Ad-hoc robot wireless communication.
In: Int. Conf. SMC. Volume 4. (2003) 4045 – 4050
436 E. Roszkowska
8. Cassandras, C., Lafortune, S.: Introduction to Discrete Event Systems.

Kluwer Academic Publishers (1999)
9. Reveliotis, S.A.: Real-Time Management of Resource Allocation Systems.
Kluwer Academic Publishers (2004).
10. Ezpeleta, J., Tricas, F., Garcia-Valles, F., Colom, J.: A banker’s solution
for deadlock avoidance in FMS with flexible routing and multi-resource
states. IEEE Trans. Robot. Automat. 18(4) (2002) 621–625
11. Wojcik, R., Banaszak, Z., Roszkowska, E.: Automation of self-recovery
resource allocation procedures synthesis in FMS. In: CIM in Process and
manufacturing Industries. Pergamon Press, Oxford (1993) 127–132
40
Urban Traffic Control and Path Planning for
Vehicles in Game Theoretic Framework
István Harmati
Department of Control Engineering and Information Technology

Budapest University of Technology and Economics
H-1117, Magyar Tudósok krt. 2/B422, Budapest, Hungary
harmati@iit.bme.hu
40.1 Introduction
Heavy congestion is a major issue in the urban traffic control problem due to
the increasing number of vehicles. The main tools to control traffic are the
traffic lights at the junctions. Based on the measurement of important traffic
properties, the traffic control has to realize an optimal green time distribution
for the traffic lights that passes as many vehicles through the network as
possible.
The literature of developed traffic models and control is increasing. Cur-
rent trends mainly involve a cell-transformation model [1], [2], heuristic-based
or soft computing methods [1], [3] but knowledge based methods [4] and sto-
chastic system modeling [5] were also successfully tested in applications. The
algorithm in [6] organizes the traffic flow into an arterial structure which is
especially useful to establish a green corridor in the traffic network [7]. The
store-and-forward model described in [8] relies on a state-space representation,
which is often preferred in control engineering. The control strategy discussed
in [8] applies optimal LQ control.
The contribution of this paper is twofold. On the one hand, we convert
the urban traffic control problem into game theoretic framework and pro-
vide a suboptimal solution on the base of Nash strategy. On the other hand,
path planning algorithms are devised for vehicles that rely on the decision of
junctions. A simple example of traffic control illustrates the efficiency of the
proposed solutions.
The paper is organized as follows. Section 40.2 describes a traffic model
in a state space form. Section 40.3 proposes a game theoretic approach to
control urban traffic network. Section 40.4 describes methods that realize path
planning algorithms for vehicles. The simulation results are illustrated on
a regular traffic network (inspired by the road structure of common North
American cities) with size 5 × 5. Finally, Section 40.5 summarizes the main
results.
springerlink.com
438 I. Harmati
40.2 The Traffic Model

The traffic model in game theoretic framework relies on [9] and realizes an
extension of the Store-and-Forward model found in [8].
In the model, the urban network is realized by a graph having edges and
nodes. The edges represent road-links, the nodes represent junctions. Con-
sider a junction j. Let Ij be the set containing the incoming road links of j.
Similarly, let Oj be the set containing the outgoing road links of j. The model
is based on the following assumptions:
(ASF1): The vehicles pass every road link in a constant time. If the inflow is
higher than the outflow at the end of the road link, the vehicles are stored
(at the end of the road link). For each outgoing link, a separated lane is des-
ignated from the incoming links of the junctions.
(ASF2): Junctions assure at least a minimum green time from any of their
incoming road link to each of their outgoing road link. The minimum green
time of the jth junction from the wth incoming road link to the ith outgoing
j
road link is denoted by gw,i,min .
(ASF3): The cycle time Cj and the total lost time Lj of junction j are given.
Note that Cj and Lj are constants in the framework but they can be easily
extended to control variables as discussed in Section 40.3.
(ASF4): The relative offset of cycles between any two junctions is fixed (and
consistent to others).
(ASF5): The saturation flows Sz , z ∈ Ij are known for every junction.
(ASF6): The turning rates tz,w , z ∈ Ij , w ∈ Oj are known or estimated for
every junction.
(ASF7): The junctions are arranged in a matrix structure. Every junction has
4 incoming road links and 4 outgoing road links. (This often occurs in many
North American cities.)
(ASF8): Road links are able to accept new vehicles from their source links
without congestion. Additionally, it is assumed that green lights from an in-
coming road link of a junction to any other outgoing road link are active at
the same time. In order to avoid the hazard of collision, active green lights
for any other direction are not allowed. Based on the assumptions above, one
writes that
X j j j
gw,i + Lj = C gw,i ≥ gw,i,min ∀j, ∀i ∈ {1, . . . , 4}, (40.1)
w∈Ij
j
where gw,i is the effective green time of junction j from incoming road link
w to outgoing link i. Note that i, w ∈ {1, . . . , 4} and the exact value of i is
indifferent in (40.1). Considering a road-link z between junction M and junc-
tion N (z ∈ IN , z ∈ OM ) as shown in Fig. 40.1 (left), the discrete dynamics
is given by
40 Urban Traffic Control and Path Planning for Vehicles 439
xz (k + 1) = xz (k) + T [qz (k) − sz (k) + dz (k) − uz (k)] ,
where xz is the number of vehicles within link z, qz is the inflow, uz is the

outflow in the time period [kT, (k + 1)T ], k = 1, 2, . . . with control time in-
terval T . The additional terms dz and sz (k) = tz,0 qz (k) denote the demand
and the exit flow, respectively. Exploiting the assumptions given above, the
discrete dynamics for link z ∈ OM , z ∈ IN in state space form [9] is
xz (k + 1) = xz (k) (40.2)
" P P #
X M
Sw i∈OM gw,i N
Sw i∈ON gw,i
+ T (1 − tz,0 ) tw,z − .
C C
w∈IM
40.3 Urban Traffic Control

In order to reduce the combinatorial complexity, our method organizes junc-
tions into groups and operates only with a few decisions. A group includes at
most 4 neighboring junctions as depicted in Fig. 40.1 (right). Decisions are
also restricted to 4 different choices in the proposed technique, each choice of a
junction prefers exactly one incoming road link (by increasing its green time)
at the other incoming road links’ expense. The total green time of the junc-
tions (and the cycle time) is maintained at constant value with this strategy
during the whole traffic control. The grouping requires new notation. Let J2
be the set of junctions where an element (i, j) ∈ J2 identifies the jth junction
qz uz
M N
sz dz
Fig. 40.1. The schematics of a road link(left) and the groups of junctions
organized into subgames (right)
440 I. Harmati
i
in the ith group.
Let J2 denote the set of junctions in the ith group where
i i
J2 includes J2 junctions. Let O(i, j) (respectively I(i, j)) be the set of the
outgoing (respectively incoming) road links of junction (i, j).
Algorithm 1 (Urban traffic control with Nash strategy)
Step 1) Initialization. Let ∆g be the quantum of the change in green time.
Step 2) Measure the characteristic of the actual traffic at the kth control time
interval: Sz , tw,z , dz (k), tz,0 .
Step 3) Compute the potential decisions of each junction. Each decision of
junction j from group i prefers only one incoming road link p by increasing
i,j
its green time with δp,w = 3∆g, ∀w ∈ O(i, j), while the green times from
any other h 6= p incoming road link of the same junction are decreasing by
i,j
δh,w = −∆g, ∀w ∈ O(i, j). During this operation the total green time in
a junction does not change and neither does the cycle time. The potential
i,j
green time ĝp,w (k, τ(i,j) ) from incoming road link p to outgoing road link
i,j
w at junction j from the ith group after decision τ(i,j) is ĝp,w (k, τ(i,j) ) =
i,j i,j
gp,w (k) + δp,w (k, τ(i,j) ).
Step 4) Compute the cost for every decision of every junction (i, h) ∈ J2 :
X
X (i,h) k, τ(i,1) , . . . , τ(i,|J i |) = xz (k, τ(i,1) , . . . , τ(i,|J i |) )
2 2
z∈I(i,h)
|J2i | X
X
+λ xz (k, τ(i,1) , . . . , τ(i,|J i |) ),
2
j=1, z∈I(i,j)
j6=h
where 0 ≤ λ ≤ 1 is a weighting factor and xz (k) is computed by (40.2). Note

that λ = 1 leads to a Pareto optimal solution, while λ = 0 carries out a
local optimization if there is no reliable communication channel between the
junctions.
Step 5) Build the normal form of the game. In order to achieve the normal
form one should evaluate the vector-vector function

X i k, τ(i,1) , . . . , τ(i,|J i |) = (40.3)
2

X (i,1) (k, τ(i,1) , . . . , τ(i,|J i |) ), . . . , X (i,|J2 |) (k, τ(i,1) , . . . , τ(i,|J i |) )
i
2 2
for every combination of decisions τ(i,j) for every group i.

∗ ∗
Step 6) Find the Nash strategy (τ(i,1) , . . . , τ(i, ) of the game that satisfies
|J2i |)

∗ ∗
X (i,h) k, τ(i,1) , . . . , τ(i, | 2| ≤
J i ) (40.4)

∗ ∗ ∗
X (i,h) k, τ(i,1) , . . . , τ(i,h−i) , τ(i,h) , τ(i,h+1) , . . . τ(i,|J i |) ∀h ∈ J2i .
2
Step 7) Modify the green times due to the Nash equilibrium point. It means
i,j i,j i,j
that gp,w (k) = ĝp,w (k), where ĝp,w (k) is selected by the optimal strategies
∗
τ(i,j) .
Step 8) Repeat the procedure from Step 2) for the next control time interval.
The simulation results on a 5 × 5 sized traffic network, with T = 60 sec,
C = 300 sec, tz,0 = 0.01, dz = 0.01, Sz = 1, xz (0) = 30, ∆g = 3 sec,
j
gw,z,min = 5 sec, λ = 0.5 are shown in Fig. 40.2. It is seen that the game
theoretic solution provides around 25 percent improvement relative to the
constant green time strategy. This result is similar to the performance of LQ
control. While LQ control requires usually less computation time, junctions
in the game theoretic algorithm are able to make decisions autonomously in
an environment with poor communication service.
Fig. 40.2. The total cost with constant green times (left) and Nash strategy
(right)
Note that it is not necessary to choose the cycle time constant in the
algorithm. Game theoretic urban traffic control is able to change the cycle
time to reach better performance. However, a new control variable increases
considerably the computation time. Hence, the controlled cycle time provides
usually benefits (by making a green corridor) only if the traffic network is
equipped with a special vehicle flow distribution or the path planning of a
distinguished vehicle (e.g. ambulance) should be supported. This problem
does not belong to the scope of this paper.
40.4 Path Planning Algorithms for Vehicles
Based on the decision of junctions, one can find xz (k) vehicles on road link z
at the kth discrete time. A challenging problem arises when a vehicle has to be
steered from an initial road link to a desired road link in the traffic network. In
this section we adopt the game theoretic motion planning concept from [10]
442 I. Harmati
to urban traffic environment. We assume that the path planning is carried

out for some vehicles in the traffic network called steered vehicles. Let O(z)
denote the set of road links w where∈w O(i, j) and∈ z I(i, j).
Algorithm 2 (Deterministic path planning for vehicles)
Step 1) Initialization. Perform Step 1 of Algorithm 1. In addition, the initial
and desired positions (zi and zd , respectively) of steered vehicles are given
and let K denote a game theoretic horizon.
Step 2) Set up the game of junctions according to the Step 2 - Step 4 of
Algorithm 1.
Step 3) Determine the road link v the steered vehicle turns on from road link
z at discrete time k in the network. The cost-to-go function V is performed
recursively for k ≤ j ≤ k + K:
v = arg min Vk (w)

w∈O(z)
If k ≤ j ≤ k + K :
 
 
Vk+j (w) = min  Vk+j+1 (p) + max xk (p, τ(i,r) , τ(m,n) )
p∈O(w)  τ(i,r) ,τ(m,n) ) 
p∈O(i,r)
p∈I(m,n)
If j = k + K then Vk+j (w) = D(w, zd ),
where D(w, zd ) is the distance between w and zd in road links.

Step 4) Obtain the decision of junctions according to Step 7 in Algorithm 1.
Step 5) Repeat the procedure from Step 2) for the next control time interval.
If there is an outgoing link w of a junction with a slightly better (i.e. less) cost
than others, then using Algorithm 2, the vehicles from all the incoming road
links tend to choose the optimal outgoing road link w making a congestion
in the next time interval. This problem motivates the use of a probabilistic
version of the path planning algorithm. If a vehicle is on an incoming road
link of junction (i, j), then a modified version of the path planning algorithm
selects the outgoing road link from the set O(i, j) in a probabilistic way. The
smaller the cost of the outgoing link the higher its fitness in the path selection
method. If a vehicle stays on an incoming road link of a junction then the
selected outgoing link the vehicle turns on is obtained by fitness based roulette-
wheel selection algorithm. The simulation results are illustrated in Fig. 40.3.
Both Algorithm 2 and its probabilistic extension find the desired outgoing
road link zd . While Algorithm 2 provides the shortest path, its probabilistic
version drives temporarily the vehicles to a nonoptimal road link. Probabilistic
path planning, however, is more robust and is able to avoid congestion if many
steered vehicles carry out the same algorithm simultaneously.
6 6
5 5
4 4
3 3
2 2
1 1
0 0
0 1 2 3 4 5 6 0 1 2 3 4 5 6
Fig. 40.3. The vehicle path from zi = (1, 5, south) to zd = (4, 1, north) with
deterministic (left) and probabilistic (right) algorithm
40.5 Conclusions
A game theoretic framework using Nash strategies for urban traffic control and
path planning algorithms for vehicles have been proposed in the paper. The
simulation results underlie the intuition that a game theoretic strategy is able
to outperform constant green times strategies. The solution is computationally
expensive and can be realized only if some simplifications are carried out. The
most important simplifications appear in bundling junctions into groups and
decreasing the number of decisions. The game theoretic framework is able to
incorporate path planning algorithms for vehicles. Two algorithms have been
discussed. A deterministic path planning provides the optimal path between
two road links in the traffic network while a probabilistic approach is able to
achieve better performance when the path planning method is carried out for
many vehicles simultaneously.
Acknowledgments
The research was supported by the Hungarian Science Research Fund under
grant OTKA 68686, the National Office for Research and Technology under
grant NKTH RET 04/2004 and by the János Bolyai Research Scholarship of
the Hungarian Academy of Sciences.
References
1. B. Friedrich and E. Almasri. Online offset optimisation in urban net-
works based on cell transmission model. In Proceedings of the 5th Euro-
pean Congress on Intelligent Transport Systems and Services, Hannover,
Germany, 2005.
444 I. Harmati
2. B. Friedrich. Traffic monitoring and control in metropolitan areas. In

Proceedings of the 2nd International Symposium ”Networks for Mobility”,
Stuttgart, Germany, 2004.
3. M. Kaczmarek. Fuzzy group model of traffic flow in street network. Trans-
portation Research Part C, Elsevier Ltd., 13:93–105, 2005.
4. F. Logi and S. G. Ritchie. Development and evaluation of a knowledge-
based system for traffic congestion management and control. Transporta-
tion Research Part C, Elsevier Ltd., 9:433–459, 2001.
5. Y. Chou J. Sheu and M. Weng. Stochastic system modeling and optimal
control of incident-induced traffic congestion. Mathematical and Com-
puter Modeling, Elsevier Ltd., 38:533–549, 2003.
6. N. H. Gartner and C. Stamatiadis. Arterial based control of traffic flow
in urban grid networks. Mathematical and Computer Modeling, Elsevier
Ltd., 35:657–671, 2002.
7. K. Aboudolas M. Papageorgiou E. Ben-Shabat E. Seider C. Diakaki,
V. Dinopoulou and A. Leibov. Extensions and new applications of the
traffic control strategy tuc. In TRB 2003 Annual Meeting, 2003.
8. V. Dinopoulou, C. Diakiki, and M. Papageorgiou. Applications of urban
traffic control strategy tuc. European Journal of Operational Research,
175(3):1652–1665, 2005.
9. I. Harmati. Game theoretic control algorithms for urban traffic network.
WSEAS Transactions on systems and control, 1(2):141–148, 2006.
10. S. M. LaValle. A game theoretic framework for robot motion planning.
PhD thesis, University of Illinois at Urbana-Champaign, 1995.
41
A Low-Cost High Precision Time Measurement
Infrastructure for Embedded Mobile Systems
Kemal Köker, Kai-Steffen Hielscher, and Reinhard German
Department of Computer Science, Computer Networks and Communication

Systems, University of Erlangen-Nuremberg, Germany
{koeker,ksjh,german}@informatik.uni-erlangen.de
41.1 Introduction and Overview

Simulation is frequently used to test the performance of embedded systems in
different operating scenarios and is usually based on a system model that fo-
cuses on the relevant parts, e.g. the response time. The accuracy of the system
model is very important to gain minimum divergences to the real world and
therefore it has to be calibrated and validated multiple times. The quality of
the calibration and validation process depends on the accuracy of the moni-
tored data of a real system. As an example, hardware monitoring requires a
specialized device with a built-in clock, which generates timestamps for sig-
nals observed at some system interfaces. This solution may provide precise
timestamps but can only be applied to systems located in one place like in
[1]. The software method generates required time stamps in software, but the
accuracy is dependent on the systems‘ load [2]. Monitoring distributed mobile
systems is a challenge to keep the accuracy and requires synchronisation of all
system clocks or a global master clock for the reference to collect and compare
time-related data. The possibilities for synchronisation differ by the medium
for the transmission and the protocols for it, whereby the focus is primarily
on the accuracy for the reception of the time signal according to a master
reference clock like in [6]. Wireless dissemination of time signals in TCP/IP
networks (e.g. IEEE 802.11a/b/g) makes it hard to keep certain accuracy be-
cause of the back-off times in case of data collision. A further drawback for the
medium access in wireless networks is the CSMA/CA mode which disallows
to gain a high accuracy for a long time period. Higher accuracy gained with
other methods like in [8] requires additional expensive hardware components.
We analysed the feasibility of transmitting reliable high precision time sig-
nals for indoor measurements and time synchronisation of mobile embedded
systems like soccer robots. This work deals especially with the extension of
the timing infrastructure based on the Global Positioning System (GPS) for
indoor mobile applications. The extension should however keep the benefit
springerlink.com
446 K. Köker, K.-S. Hielscher, and R. German
of the high accuracy of the GPS system for time synchronisation and should
also provide a reliable solution in a cost-efficient manner. The work is pri-
marily based on the software monitoring infrastructure for distributed Web
servers in [5] for precise measurements of one-way delays of TCP packets, to
parameterize, validate, and verify simulation and state-space based models of
the system. Our solution uses this infrastructure in combination with an FM-
receiver module and the PPS-API for the time synchronisation of all object
systems’ clocks with a precision in the range of one microsecond. In addition,
example measurements for the timing signal generated with our infrastruc-
ture are presented to confirm the usability for distributed mobile embedded
systems.
41.2 Architecture of the Application

We selected a modified Robocup [3] scenario for the labour setting with mobile
soccer playing robots similar to the F-180 Robocup. All mobile robots in the
test bed are equipped with embedded systems using Linux and provided with
commonly used FM receivers. Furthermore, the PPS-API for the operating
system is used which is specified in RFC 2783 [7]. The PPS-API kernel patch
for the Linux system not only provides a mechanism for detecting external
PPS pulses but also improves the timing resolution from one microsecond to
one nanosecond using the timestamp counter (TSC) which is available in most
modern processor architectures.
41.2.1 Robocup F-180 Small Size League
In a common Robocup small size league system, an off-field host computer

performs remote control of each player according to the results of the strategy
software. The architecture of a soccer robot in the F-180 league usually con-
sists of the mechanical and the electronic parts. The mechanical part covers
the body and the actors. The electronic part consists of a micro controller
for the actors, a dedicated FM-module to receive action commands triggered
from the host, and a power supply with batteries. Our modification of the
system splits the robots into an upper layer with an embedded system, and
the lower layer covering the mechanical and electronic parts for the actors
and sensors. We provided each player with a PC/104 [4] as an embedded sys-
tem for the upper part and installed embedded Linux with kernel 2.6.9 as the
operating system. The embedded system of the upper layer has capabilities
to communicate with the host over WLAN (IEEE 802.11b) and furthermore
each robot is equipped with an FM-receiver module. Figure 41.1 displays our
architecture for the soccer robots using the PC/104 embedded system.
41 Low-Cost Time Measurement Infrastructure for Mobile Systems 447
Fig. 41.1. F-180 soccer robot with an embedded system
41.3 Time Synchronisation with GPS
The precision of the GPS makes it an impressive technique for any imag-
inable application that requires the determination of positions (positioning)
and direction of motion (navigation) anywhere on the Earth. The technique
is based on measurements of code-phase arrival time from the satellites with
high accurate reference clocks on board, to gain positions in three dimensions
and GPS time. This facility is often used as potential time synchronisation
to provide a globally accurate clock to each receiver. Because of reasons of
cost and convenience, it is not possible to equip every computer system with
a GPS-receiver to keep a highly accurate system clock. But instead, it is pos-
sible to equip some number of computers and let them act as primary time
servers.
41.3.1 Existing Infrastructure
The infrastructure we used in our application is based on the infrastructure

for the Web cluster project in combination of NTP with a master GPS re-
ceiver and the PPS-API. The main part of the infrastructure is the possibility
to generate high resolution timestamps derived from the TSC. The usual so-
lution for synchronizing the clocks of a number of systems is by using NTP
over a network connection. The accuracy of this method is limited to a few
milliseconds on LANs and up to a few tens of milliseconds on WANs and is
relative to the Universal Time Coordinated (UTC). Another possibility would
be to equip each of the desired system with one “cost effective” GPS receiver
module (e.g., the price for a common GPS timing module including the an-
tenna is listed as more than 100 US$) [9]. The disadvantage of the idea of
providing the embedded systems with GPS receiver modules is the need of
so many modules as soccer robots are deployed. A further drawback is the
inherent weakness of GPS: it requires line-of-sight to orbiting satellites, and
hence this method is inappropriate for indoor applications (e.g. wireless time
dissemination).
41.3.2 PPS-API
The PPS-API (pulse per second) provides a facility to timestamp external

events delivered to a system with high resolution. It is intended for connecting
external time sources like GPS receivers to a stratum-1 NTP server. The GPS
receiver cards used in the Web cluster project have a PPS signal output at
TTL level that marks the beginning of a second with an uncertainty below
500 ns with respect to UTC. In Linux, the recognition of the PPS pulse is
done by instructing the hardware to generate an interrupt in case of a signal
transition on the DCD pin of the serial port. The interrupt handling routine in
the serial port driver is modified by the patch to timestamp every invocation.
We patched the Linux kernel with the PPS-API patch to use this facility. This
patch modifies the serial port driver for detecting and time stamping signals
delivered to the DCD pin of the serial port. In addition to the PPS recognition,
the kernel patch extends the timekeeping resolution of the Linux kernel to one
nanosecond by utilizing the TSC, whereby the TSC is incremented with every
tick of the internal clock. The timestamps of the PPS pulses can be used in
two ways to discipline the kernel clock: either by using the hardpps() kernel
consumer or by using a user level NTP daemon. Both of them make use of
the kernel model for precision timekeeping as specified in RFC 1589 [10] and
estimate the frequency error of the local oscillator by averaging the measured
time interval between successive PPS pulses. Since the PPS pulse just marks
the beginning of an arbitrary second, but does not contain information on
the absolute time (second, minute, hour, day, moth, year), all clocks of the
receivers must be set to have offsets less than 500 ms. Usually this can be
achieved using a standard NTP server on a wireless network and using the
ntpdate command before starting a synchronisation with PPS or by using
the NTP daemon with a configuration file that contains two time sources,
the PPS clock driver and the NTP server.
41.3.3 Wireless Dissemination of Time Signal
Obtaining signals based on a GPS receiver infrastructure makes it possible

to use highly accurate pulses for time stamping in wired networks, but this
infrastructure is inappropriate for wireless systems. The most possibilities for
wireless signal circulation lead to non-deterministic response time. For exam-
ple, the IEEE 802.11b WLAN uses CSMA/CA for medium access to avoid
data collision. Each time before the client starts to send, the clearance of the
medium has to be checked, which leads to non-deterministic time consumption
for collision-free data reception. This drawback leads to the idea of transmit-
ting the timing signal pulse of the mentioned GPS infrastructure regardless of
the medium clearance without affecting other clients and keeping the mecha-
nism as easy as possible. For this purpose, we connected the TTL output of
the GPS timing architecture to the input of a widely spread FM-transmitter
module to broadcast the GPS second pulses on the 433,92 MHz FM band. To
Fig. 41.2. Wireless signal dissemination based on GPS
receive the signal, we connected an appropriate FM-receiver module on the

same FM band to the serial port of the robot’s PC/104 system. The cost of
both modules e.g. from [11] is listed below 20US$. Figure 41.2 illustrates our
infrastructure for the wireless signal dissemination.
41.4 Measurements
We started a test run transmitting the PPS-signal from the GPS based in-
frastructure to the robot’s PC/104 system and simultaneously tracked the
signal reception with an analogue oscilloscope. The signal course of the first
test run resulted in a lot of interference, which would lead the PPS-API fa-
cility to assume a wrong offset of the system clock. We started a long term
test run to assure that the interference is not temporary and we recorded the
internal system clock, the second pulse recognized by the PPS-API, and the
jitter.
41.4.1 Results of First Test Run

The left-hand side of Fig. 41.3 shows the plot of the results of a continuous
measurement for three days. The plotted dots are representing the difference
of two back-to-back seconds. The y-axis of the figure is scaled linearly and is
indicating the duration of the systems second. We can match this with the
deviation of the second pulse registered by the PPS-API from the true (exact)
second. The x-axis is also scaled linearly and is counting the triggered pulses.
Considering the results, we detect a lot of high ratio jitter that disallows us-
ing the modified GPS timing architecture in this mode and therefore it is not
useful to discipline the kernel clock in a highly accurate way. Primary sources
of the interferences can be found in different electronic devices with spurious
radiations (i.e. power supply of running computers), and also atmospheric dis-
turbances. Another reason for the deviation of the exact time second is the
technical design of the used FM-receiver module with a “fast-acting carrier
detect and power-up” implementation. This implementation may allow effec-
tive duty cycle power saving and a −107 dBm sensitivity but takes about 1ms
for enabling the device and is a disadvantage for our application. According
to the time measuring infrastructure of the Web cluster, the GPS cards de-
liver a PPS signal output that marks the beginning of a second with a very
high accuracy (below 500 ns) with respect to UTC. Therefore the maximum
deviation of a time second between the systems clock – using the FM module
– and the GPS subsystem should amount to not more than 1.5 ms considering
the reception delay of the module. A further disadvantage is the sensitivity of
the module to captured undefined foreign signals which just leads to enabling
the FM module and then again shutting it down because of missing signals.
41.4.2 Techniques for Improvement

To use the accurate GPS timing infrastructure and handle the disadvantages
of the FM modules, we switched the GPS card to generate a 100 Hz PPS signal
output. In this operating mode, the output delivers 98 TTL high level signals
with a length less than 10 ms and a longer 99th high level signal to mark
the beginning of a second with the next pulse. This operating mode of the
GPS card also leads the FM receiver module to maintain the enabled mode
for signal reception and therefore no additional enable time is necessary. But
to use the PPS-API facility, the signal input to the system has to be exactly
one pulse per second. To utilize the 100 Hz PPS signal for the PPS-API, we
built a time measuring circuit with a micro controller on the receiver side. The
function of the circuit is to monitor the width of a signal pulse delivered by the
GPS card output. The circuit enables an incoming signal to be forwarded with
the next pulse if and only if a longer high level signal is received. This method
assures to forward the correct signal pulse for the beginning of a second and
also decreases interference. We ensured our considerations by again tracking
with the analogue oscilloscope and monitored fewer disturbances. Ongoing we
started a new test run for 3 days and again recorded the internal system clock,
the second pulse recognized by the PPS-API and the jitter for both back-to-
back seconds. The right-hand part of Fig. 41.3 shows the measurements for the
test run, whereby the number of pulses for the x-axis is abbreviated to compare
the results of the first test run. We can identify a smoother scatter plot with
more precise boundaries for the duration of a second. These boundaries should
be considered for the system design and monitoring method.
Fig. 41.3. Scatter plot for different driving modes of the GPS cards
41.5 Conclusion
We analysed the feasibility and performance of an indoor facility for a high
precision time synchronisation for mobile devices. The facility is intended to
build a simulation model for embedded mobile systems. For this purpose we
extended an existing highly accurate GPS based infrastructure and introduced
commonly used FM modules to transmit and receive a pulse-per-second signal
to the embedded systems of mobile soccer robots. The simple usage of the FM
modules to transmit and receive signals led to higher interference ratio, also
caused from the entire design and enable delay of the FM receiver. To handle
the interferences and improve the accuracy of the GPS time synchronizing
infrastructure, we switched the GPS output to transmit a 100 Hz signal and
introduced a circuit to generate a pulse at the end of the 100th signal on the
receiver side. Using this method significantly improved the results and allowed
transmitting highly accurate pulses for a second gained from the GPS in an
appropriate way for performing measurements on mobile embedded systems.
References
1. K. Köker, R. Membarth, R. German. Performance analyses of embedded

real-time operating systems using high-precision counters, Proc. of 3rd
Int. Conf. on Autonomous Robots and Agents, New Zealand, pp 485-490,
2006.
2. A.K. Ojha. Techniques in least-intrusive computer system performance
monitoring, Proc. of SoutheastCon 2001, Clemson, SC, USA, pp 150-154,
2001.
3. Robocup Small Size League, http://www.robocup.org/02.html, visited
14/01/2007
4. PC/104 Embedded Consortium, PC/104-Specification,
http://www.pc104.org, visited 12/06/2004
5. K.S. Hielscher, R. German. A Low-Cost Infrastructure for High Precision
High Volume Performance Measurements of Web Clusters, Proc. of 13th
Conf. on Computer Performance Evaluations, Modelling Techniques and
Tools, Urbana, IL, USA, 2003.
6. D. Mills. Internet time synchronization: the Network Time Protocol,
IEEE Trans. Communications, 39(10):1482-1493, October 1991.
7. J. Mogul, D. Mills, J. Brittenson, J. Stone, and U. Windl. Pulse-per-
second API for Unix-like operating systems, Version 1. Request for Com-
ments RFC-2783, Internet Engineering Task Force, March 2000
8. H. Dai, R. Han, TSync: a lightweight bidirectional time synchronization
service for wireless sensor networks, ACM SigMobile Mobile Computing
and Communications Review, vol. 8, no. 1, pp. 125-139, 2004.
9. Synergy Systems, GPS Timing Modules, http://www.synergy-gps.

com/content/view/20/34, visited 15/12/2006
10. D. Mills. A kernel model for precision timekeeping. Request for Comments
RFC-1589, Internet Engineering Task Force, March 1994
11. Radiometrix radio modules, http://www.radiometrix.com, visited
12/12/2005
Lecture Notes in Control and Information Sciences
Edited by M. Thoma, M. Morari
Further volumes of this series can be found on our homepage:
springer.com
Vol. 360: Kozłowski K. (Ed.) Vol. 349: Rogers, E.T.A.; Galkowski, K.;
Robot Motion and Control 2007 Owens, D.H.
452 p. 2007 [978-1-84628-973-6] Control Systems Theory
and Applications for Linear
Vol. 359: Christophersen F.J.
Repetitive Processes 482 p. 2007 [978-3-540-
Optimal Control of Constrained Piecewise Affine
42663-9]
Systems
xxx p. 2007 [978-3-540-72700-2] Vol. 347: Assawinchaichote, W.; Nguang, K.S.;
Shi P.
Vol. 358: Findeisen R.; Allgöwer F.; Biegler L.T. Fuzzy Control and Filter Design
(Eds.): for Uncertain Fuzzy Systems
Assessment and Future Directions of Nonlinear 188 p. 2006 [978-3-540-37011-6]
Model Predictive Control Vol. 346: Tarbouriech, S.; Garcia, G.; Glattfelder,
XXX p. 2007 [978-3-540-72698-2] A.H. (Eds.)
Vol. 357: Queinnec I.; Tarbouriech S.; Garcia G.; Advanced Strategies in Control Systems
Niculescu S.-I. (Eds.): with Input and Output Constraints
Biology and Control Theory: Current Challenges 480 p. 2006 [978-3-540-37009-3]
589 p. 2007 [978-3-540-71987-8] Vol. 345: Huang, D.-S.; Li, K.; Irwin, G.W. (Eds.)
Vol. 356: Karatkevich A. Intelligent Computing in Signal Processing
Dynamic Analysis of Petri Net-Based Discrete and Pattern Recognition
Systems 1179 p. 2006 [978-3-540-37257-8]
166 p. 2007 [978-3-540-71464-4] Vol. 344: Huang, D.-S.; Li, K.; Irwin, G.W. (Eds.)
Intelligent Control and Automation
Vol. 355: Zhang H.; Xie L. 1121 p. 2006 [978-3-540-37255-4]
Control and Estimation of Systems with
Input/Output Delays Vol. 341: Commault, C.; Marchand, N. (Eds.)
213 p. 2007 [978-3-540-71118-6] Positive Systems
448 p. 2006 [978-3-540-34771-2]
Vol. 354: Witczak M. Vol. 340: Diehl, M.; Mombaur, K. (Eds.)
Modelling and Estimation Strategies for Fault Fast Motions in Biomechanics and Robotics
Diagnosis of Non-Linear Systems 500 p. 2006 [978-3-540-36118-3]
215 p. 2007 [978-3-540-71114-8]
Vol. 339: Alamir, M.
Vol. 353: Bonivento C.; Isidori A.; Marconi L.; Stabilization of Nonlinear Systems Using
Rossi C. (Eds.) Receding-horizon Control Schemes
Advances in Control Theory and Applications 325 p. 2006 [978-1-84628-470-0]
305 p. 2007 [978-3-540-70700-4] Vol. 338: Tokarzewski, J.
Vol. 352: Chiasson, J.; Loiseau, J.J. (Eds.) Finite Zeros in Discrete Time Control Systems
Applications of Time Delay Systems 325 p. 2006 [978-3-540-33464-4]
358 p. 2007 [978-3-540-49555-0] Vol. 337: Blom, H.; Lygeros, J. (Eds.)
Stochastic Hybrid Systems
Vol. 351: Lin, C.; Wang, Q.-G.; Lee, T.H., He, Y.
395 p. 2006 [978-3-540-33466-8]
LMI Approach to Analysis and Control of
Takagi-Sugeno Fuzzy Systems with Time Delay Vol. 336: Pettersen, K.Y.; Gravdahl, J.T.;
204 p. 2007 [978-3-540-49552-9] Nijmeijer, H. (Eds.)
Group Coordination and Cooperative Control
Vol. 350: Bandyopadhyay, B.; Manjunath, T.C.; 310 p. 2006 [978-3-540-33468-2]
Umapathy, M.
Modeling, Control and Implementation of Smart Vol. 335: Kozłowski, K. (Ed.)
Structures 250 p. 2007 [978-3-540-48393-9] Robot Motion and Control
424 p. 2006 [978-1-84628-404-5]
Vol. 334: Edwards, C.; Fossas Colet, E.; Vol. 320: Steffen, T.
Fridman, L. (Eds.) Control Reconfiguration of Dynamical Systems
Advances in Variable Structure and Sliding Mode 290 p. 2005 [978-3-540-25730-1]
Control Vol. 319: Hofbaur, M.W.
504 p. 2006 [978-3-540-32800-1] Hybrid Estimation of Complex Systems
Vol. 333: Banavar, R.N.; Sankaranarayanan, V. 148 p. 2005 [978-3-540-25727-1]
Switched Finite Time Control of a Class of Vol. 318: Gershon, E.; Shaked, U.; Yaesh, I.
Underactuated Systems H∞ Control and Estimation of State-multiplicative
99 p. 2006 [978-3-540-32799-8] Linear Systems
Vol. 332: Xu, S.; Lam, J. 256 p. 2005 [978-1-85233-997-5]
Robust Control and Filtering of Singular Systems
Vol. 317: Ma, C.; Wonham, M.
234 p. 2006 [978-3-540-32797-4]
Nonblocking Supervisory Control of State Tree
Vol. 331: Antsaklis, P.J.; Tabuada, P. (Eds.) Structures
Networked Embedded Sensing and Control 208 p. 2005 [978-3-540-25069-2]
367 p. 2006 [978-3-540-32794-3]
Vol. 316: Patel, R.V.; Shadpey, F.
Vol. 330: Koumoutsakos, P.; Mezic, I. (Eds.) Control of Redundant Robot Manipulators
Control of Fluid Flow 224 p. 2005 [978-3-540-25071-5]
200 p. 2006 [978-3-540-25140-8]
Vol. 315: Herbordt, W.
Vol. 329: Francis, B.A.; Smith, M.C.; Willems, Sound Capture for Human/Machine Interfaces:
J.C. (Eds.) Practical Aspects of Microphone Array Signal
Control of Uncertain Systems: Modelling, Processing
Approximation, and Design 286 p. 2005 [978-3-540-23954-3]
429 p. 2006 [978-3-540-31754-8]
Vol. 314: Gil’, M.I.
Vol. 328: Loría, A.; Lamnabhi-Lagarrigue, F.;
Explicit Stability Conditions for Continuous Sys-
Panteley, E. (Eds.)
Advanced Topics in Control Systems Theory tems
193 p. 2005 [978-3-540-23984-0]
305 p. 2006 [978-1-84628-313-0]
Vol. 327: Fournier, J.-D.; Grimm, J.; Leblond, J.; Vol. 313: Li, Z.; Soh, Y.; Wen, C.
Partington, J.R. (Eds.) Switched and Impulsive Systems
Harmonic Analysis and Rational Approximation 277 p. 2005 [978-3-540-23952-9]
301 p. 2006 [978-3-540-30922-2] Vol. 312: Henrion, D.; Garulli, A. (Eds.)
Vol. 326: Wang, H.-S.; Yung, C.-F.; Chang, F.-R. Positive Polynomials in Control
H∞ Control for Nonlinear Descriptor Systems 313 p. 2005 [978-3-540-23948-2]
164 p. 2006 [978-1-84628-289-8] Vol. 311: Lamnabhi-Lagarrigue, F.; Loría, A.;
Vol. 325: Amato, F. Panteley, E. (Eds.)
Robust Control of Linear Systems Subject to Advanced Topics in Control Systems Theory
Uncertain 294 p. 2005 [978-1-85233-923-4]
Time-Varying Parameters Vol. 310: Janczak, A.
180 p. 2006 [978-3-540-23950-5] Identification of Nonlinear Systems Using Neural
Vol. 324: Christofides, P.; El-Farra, N. Networks and Polynomial Models
Control of Nonlinear and Hybrid Process Systems 197 p. 2005 [978-3-540-23185-1]
446 p. 2005 [978-3-540-28456-7] Vol. 309: Kumar, V.; Leonard, N.; Morse, A.S.
Vol. 323: Bandyopadhyay, B.; Janardhanan, S. (Eds.)
Discrete-time Sliding Mode Control Cooperative Control
147 p. 2005 [978-3-540-28140-5] 301 p. 2005 [978-3-540-22861-5]
Vol. 322: Meurer, T.; Graichen, K.; Gilles, E.D. Vol. 308: Tarbouriech, S.; Abdallah, C.T.; Chias-
(Eds.) son, J. (Eds.)
Control and Observer Design for Nonlinear Finite Advances in Communication Control Networks
and Infinite Dimensional Systems 358 p. 2005 [978-3-540-22819-6]
422 p. 2005 [978-3-540-27938-9]
Vol. 307: Kwon, S.J.; Chung, W.K.
Vol. 321: Dayawansa, W.P.; Lindquist, A.; Perturbation Compensator based Robust Tracking
Zhou, Y. (Eds.) Control and State Estimation of Mechanical Sys-
New Directions and Applications in Control tems
Theory 158 p. 2004 [978-3-540-22077-0]
400 p. 2005 [978-3-540-23953-6]

(Lecture Notes in Control and Information Sciences) Krzysztof R. Kozlowski - Robot Motion and Control 2007-Springer (2007)

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

(Lecture Notes in Control and Information Sciences) Krzysztof R. Kozlowski - Robot Motion and Control 2007-Springer (2007)

Uploaded by

Copyright:

Available Formats

Lecture Notes

in Control and Information Sciences 360

Robot Motion and Control 2007

British Library Cataloguing in Publication Data

Library of Congress Control Number: 2007928368

Lecture Notes in Control and Information Sciences ISSN 0170-8643

Printed on acid-free paper

© Springer-Verlag London Limited 2007

Springer Science+Business Media

The main motivation in publishing this collection of papers is to present

Poznań, Poland Krzysztof Kozlowski

Part I Control of Nonholonomic Systems

1 Modelling and Trajectory Generation of Lighter-

2.4.2 LQR with Gain Scheduling . . . . . . . . . . . . . . . . . . . . . 33

5.5 Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

Part II Vision-Based Control

6 Vision-Based Dynamic Velocity Field Generation for

9 Trajectory Planning with Control Horizon Based on

Part III New Control Algorithms for Robot Manipulators

10 Control for a Three-Joint Underactuated Planar

12.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137

16 An Energy-Based Approach Towards Modeling of

Part IV New Trends in Kinematics and Localization Methods

17 Navigation of Autonomous Mobile Robots – Invited

19.2 Kinematic Control of Redundant Manipulator . . . . . . . . . . . . 220

Part V Trajectory Planning Issues for Nonholonomic

22 Lie Algebraic Approach to Nonholonomic Motion

25.2.1 Nonholonomic Constraints . . . . . . . . . . . . . . . . . . . . . . 280

Part VI Rehabilitation Robotics

26 On Simulator Design of the Spherical Therapeutic

28.2 Presentation of Wheeeler . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 311

Part VII Humanoid Robots

29 Educational Walking Robots

31.3.2 Artiﬁcial Eyes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 345

Part VIII Applications of Robotic Systems

32 Assistive Feeding Device for Physically Handicapped

34 Measurement and Navigation System of

37 End-Effector Sensors’ Role in Service Robots

Part IX Multiagent Systems

38 Detecting Intruders in Complex Environments with

40 Urban Traffic Control and Path Planning for

Yasmina Bestaoui and Salim Hima

Laboratoire IBISC, Université d’Evry Val d’Essonne, 91025 Evry, France

motion. The contribution of this paper is the characterization of tra-

1.2 Airship Modelling

For kinematics, Euler angles are presented. For dynamics, a mathe-

In aeronautics, the need to deﬁne appropriate coordinate systems arises

where the following notation is used: cx = cos(x) and sx = sin(x). A

The conﬁguration of the airship is completely deﬁned by associating

SE(3) is a Lie group. Given a curve C(t) : [−a, a] → SE(3), an element

where Sk(ν2 ) = RT (t)Ṙ(t) is a 3 × 3 skew symmetric operator on a

such that ∀x ∈ R3 : Sk(y)x = y × x.

A mathematical description of airship’s ﬂight dynamics needs to em-

of the airship and consequently cannot be neglected. The displaced air

τa = −Aν̇A + D(ν2 )νA + τ1 + τat . (1.11)

The complete mechanical-aerodynamical airship model can be given

where Ma = M + A is the total mass.

1.3 Trajectory Generation

Trajectory generation is one of the important subjects of research in

behavior of the non-controlled variables such as a roll dynamics in the

1.3.1 Trim Trajectories

Trim trajectories take a particular place in aeronautic applications

1.3.2 Trim Trajectory Characterization

As mentioned before, trim trajectories are characterized by the sta-