Robot Motion and Control 2007 (Lecture Notes in Control and Information Sciences)

Lecture Notes in Control and Information Sciences Editors: M. Thoma · M. Morari 360 Krzysztof Kozłowski (Ed.) Robot...

Author: Krzysztof R. Kozlowski

167 downloads 1130 Views 13MB Size Report

This content was uploaded by our users and we assume good faith they have the permission to share this book. If you own the copyright to this book and it is wrongfully on our website, we offer a simple DMCA procedure to remove your content from our site. Start by pressing the button below!

Report copyright / DMCA form

DOWNLOAD PDF

Lecture Notes in Control and Information Sciences Editors: M. Thoma · M. Morari

360

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 [email protected]

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 robotics 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 Workshops 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 Robotics and Automation Society Chapter. The Workshop is organized by the Chair of Control and Systems Engineering of the Pozna´ n University 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 ﬁnally accepted for publication in the Lecture Notes in Control and Information 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, ﬂying robots and their applications are the evidence of a signiﬁcant progress in the area of robotics. It should also be noted that among the objectives of running the Workshop was to build a bridge between previous the Eastern European countries and the Western countries. It is one of the reasons why RoMoCo Workshop takes place in Poland. There is a lot of appreciation of the robotics ﬁeld in Poland now and many researchers visiting Poland have noticed this fact recently. A careful review procedure resulted in the selection of high quality papers written by internationally recognized scientists as well as young talented researchers (some of them Ph.D. students) from diﬀerent 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 ﬁelds 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 ﬁrst 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 ﬁelds. I would like to take this opportunity to thank all the reviewers involved in the reviewing process. I am very grateful to Mr K. Romanowski for his suggestions concerning improvement of English. I am also grateful to Dr W. Wr´ oblewski for his help and patience and typesetting of this book. Mr O. Jackson and Mr A. Doyle, our editors at Springer, are gratefully acknowledged for their encouragement in pursuing this project. Pozna´ n, Poland April, 2007

Krzysztof Kozlowski

Contents

Part I Control of Nonholonomic Systems

1 Modelling and Trajectory Generation of LighterThan-Air Aerial Robots – Invited Paper Yasmina Bestaoui, Salim Hima . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2 Airship Modelling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.1 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.2 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3 Trajectory Generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.1 Trim Trajectories . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.2 Trim Trajectory Characterization . . . . . . . . . . . . . . . 1.3.3 Trim Trajectories Calculation . . . . . . . . . . . . . . . . . . . 1.4 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.4.1 Straight Forward Trim Flight . . . . . . . . . . . . . . . . . . . 1.4.2 Circular Trim Trajectories with Constant Altitude . 1.4.3 Helicoidal Trim Trajectories . . . . . . . . . . . . . . . . . . . . 1.4.4 Under-Actuation at Low Velocity . . . . . . . . . . . . . . . . 1.4.5 Results for the Normalized Time . . . . . . . . . . . . . . . . 1.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3 3 4 4 6 9 10 10 12 14 15 15 17 18 23 26 26

2 Control of 3 DOF Quadrotor Model Tae Soo Kim, Karl Stol, Vojislav Kecman . . . . . . . . . . . . . . . . . . . . . 2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2 Modelling of Quadrotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.3 Experimental Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.4 Control Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.4.1 Optimal Control (LQR) . . . . . . . . . . . . . . . . . . . . . . . .

29 29 30 32 32 32

VIII

Contents

2.4.2 LQR with Gain Scheduling . . . . . . . . . . . . . . . . . . . . . 2.4.3 Feedback Linearization . . . . . . . . . . . . . . . . . . . . . . . . . 2.4.4 Sliding Mode Control . . . . . . . . . . . . . . . . . . . . . . . . . . 2.5 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.6 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

33 34 35 35 37 37

3 Internal Model Control-Based Adaptive Attitude Tracking Ahmed Al-Garni, Ayman Kassem, Muhammad Shaﬁq, Rihan Ahmed . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2 Spacecraft Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.3 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.4 Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.4.1 Plant Stabilization . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.4.2 PID Controller Design for the Stabilized Plant . . . . 3.5 Adaptive Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.5.1 Internal Model Control – System Operation . . . . . . 3.5.2 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

39 39 40 41 41 41 43 44 44 46 47 47

4 Tracking control of Automated Guided Vehicles Lotﬁ Beji, Azgal Abichou . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Modelling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.4 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

49 49 50 54 55 55

5 VFO Control for Mobile Vehicles in the Presence of Skid Phenomenon Maciej Michalek . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2.1 Skid Phenomenon . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3 VFO Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3.1 VFO Strategy – Brief Recall . . . . . . . . . . . . . . . . . . . . 5.3.2 VFO Control with Skid Compensation . . . . . . . . . . . 5.3.3 Skid Computation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

57 57 58 59 60 60 61 64 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-Mel´endez, L. Ferm´ın, J. Cappelletto, C. Murrugarra, G. Fern´ andez-L´ opez, J.C. Grieco . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2 Problem Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.3 Dynamic Velocity Field Generation . . . . . . . . . . . . . . . . . . . . . 6.3.1 Vision System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.3.2 Initial Velocity Field Generation . . . . . . . . . . . . . . . . 6.3.3 Dynamic Velocity Field Modiﬁcation . . . . . . . . . . . . . 6.4 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.5 Conclusions and Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

69 69 70 70 70 72 74 75 78 78

7 Zoom Control to Compensate Camera Translation within a Robot Egomotion Estimation Approach Guillem Aleny` a, Carme Torras . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.2 Mapping Contour Deformations to Camera Motions . . . . . . 7.3 Generating Zoom Demands . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.4 Control and Egomotion Algorithm . . . . . . . . . . . . . . . . . . . . . . 7.5 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.6 Conclusions and Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

81 81 82 84 85 85 87 88

8 Two-Finger Grasping for Vision Assisted Object Manipulation Umar Khan, Thomas Nierobisch, Frank Hoﬀmann . . . . . . . . . . . . . 8.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.2 Visual Servoing with a Dynamic Set of SIFT Features . . . . . 8.3 Grasp Point Selection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.4 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

89 89 90 92 95 97 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 11.3 Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133 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 12.2 12.3 12.4 12.5

Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137 The Considered Dynamical Model . . . . . . . . . . . . . . . . . . . . . . 138 The Inverse Dynamics Method . . . . . . . . . . . . . . . . . . . . . . . . . 139 A Discrete-Time Sliding Mode Control Approach . . . . . . . . . 139 An Alternative Discrete-Time Sliding Mode Control Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141 12.6 Experimental Veriﬁcation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 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 13.5 Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155 14 Fixed Point Transformations-Based Approach in Adaptive Control of Smooth Systems J´ ozsef 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 Identiﬁcation 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 Tcho´ n . . . . . . . . . . . . . . . . . . . . . 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 Deﬁnition 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 Tesli´c, Gregor Klanˇcar, Igor Skrjanc . . . . . . . . . . . . . . . . . . . . 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 Ludwik´ ow, Ignacy Dul¸eba . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 249 22.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 249 22.2 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 251 22.3 The Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 252 22.4 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 255 22.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 257 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 257 23 Computationally Efficient Path Planning for Wheeled Mobile Robots in Obstacle Dense Environments Husnu T¨ urker S ¸ ahin, Erkan Zergero˘glu . . . . . . . . . . . . . . . . . . . . . . . 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.4 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 264 23.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 267 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 267 24 Piecewise-Constant Controls for Newtonian Nonholonomic Motion Planning Ignacy Dul¸eba . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 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 25.2.2

Nonholonomic Constraints . . . . . . . . . . . . . . . . . . . . . . 280 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 Zieli´ nska, Andrzej Chmielniak, Lukasz Ja´ nczyk . . . . . . . . . . 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 Artiﬁcial 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 Wr´ oblewski . . 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 Wr´ oblewski . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 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. Z¨ ollner, 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 Zieli´ nski, 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 Eﬀector 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 El˙zbieta 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 Istv´ an Harmati . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 437 40.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 437 40.2 The Traﬃc Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 438 40.3 Urban Traﬃc 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 K¨ oker, Kai-Steﬀen 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, Universit´e d’Evry Val d’Essonne, 91025 Evry, France [email protected], [email protected]

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-eﬀective, 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 diﬀerence 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 basically 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 horizontal axis of rotation) and the tail ﬁns. 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 envelope as it moves through the air. The most common form of a dirigible is an ellipsoid. It is a highly aerodynamic proﬁle with good resistance to aerostatics pressure. The objective of the ﬁrst part of this paper is to present a model of an autonomous airship: kinematics and dynamics, taking into account the wind eﬀect. 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. c Springer-Verlag London Limited 2007 springerlink.com

4

Y. Bestaoui and S. Hima

motion. The contribution of this paper is the characterization of trajectories, 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 ﬁnally some concluding remarks are given in Section 1.5.

1.2 Airship Modelling For kinematics, Euler angles are presented. For dynamics, a mathematical description of a dirigible ﬂight must contain the necessary information about aerodynamic, structural, and other internal dynamic eﬀects (engine, actuation) that inﬂuence 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 ﬁxed 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 modiﬁed later to take into account the speciﬁcs of the airship [16, 20]. In [3], the origin of the body ﬁxed frame is the center of gravity. 1.2.1 Kinematics In aeronautics, the need to deﬁne appropriate coordinate systems arises from two considerations [25]. First, there may be some particular coordinate 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 ﬂight mechanics; both at the conceptual level with ﬂight 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 respect to Rf can be completely deﬁned by knowing the orientation of a ﬁxed frame Rm to the aircraft body and the position of its origin with respect to Rf . The choice of the body ﬁxed 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 ﬁnite 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 coordinates, 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 ﬁxed frame reference. This parametrisation is a three-parameter set corresponding to well known quantities like the yaw ψ, pitch θ, and roll φ, which are directly measured by the attitude sensors such as inclinometer and magnetic compass. Adopting this formulation, the rotation matrix 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) deﬁned as: SO(3) = R ∈ R3×3 , RT R = I3×3 and det (R) = 1 . (1.2)

The conﬁguration of the airship is completely deﬁned 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), deﬁned by: R η1 3 SE(3) = A A = , R ∈ SO(3), η1 ∈ R . (1.4) 03×3 1

6

Y. Bestaoui and S. Hima

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 conﬁguration 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 deﬁned 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 ﬁxed frame. The kinematic relationship between the diﬀerent velocities is given by: η˙ 1 R(η2 ) 0 ν1 = , (1.7) η˙ 2 0 J(η2 ) ν2 where R is the rotation matrix deﬁned by Equation (1.1) and J(η2 ) is deﬁned by:   1 sin(φ) tan(θ) cos(φ) tan(θ) cos(φ) − sin(φ)  . J(η2 ) = 0 (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 deﬁne νA = (ν1a , ν2 )T . 1.2.2 Dynamics A mathematical description of airship’s ﬂight dynamics needs to embody the important aerodynamic, structural and other internal dynamics eﬀects that combine to inﬂuence 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

1 Modelling and Trajectory Generation of Aerial Robots

7

dirigible are derived. The forces and moments are referred to a system of body-ﬁxed 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 physical insight through the derivation. The other one is more complicated; it provides the linkage between the classical framework and the Lagrangian or Hamiltonian framework. In this paper, we apply Newton’s laws of motion to relate the applied forces and moments to the resulting translational and rotational accelerations. We will make in the sequel some simplifying assumptions: the Earth ﬁxed reference frame is inertial, the gravitational ﬁeld is constant, the airship is supposed to be a rigid body [1], meaning that it is well inﬂated, 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-ﬁxed frame Rm , can be given by: M ν˙ = C(ν)ν + τs + τa + τp , η˙ = J(η2 )ν, where: • M =

•

•

• •

mI3×3 −mSk(CG) mSk(CG) IC

(1.9)

is the airship inertia matrix, m is   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 CoriolisSk(ν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 simpliﬁed to the eﬀect 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 interaction 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

8

Y. Bestaoui and S. Hima

of the airship and consequently cannot be neglected. The displaced air mass is known as “added mass” or “virtual mass”. The added mass matrix A is, in general, an extra-diagonal matrix. The inertial eﬀects of this added mass constitute the ﬁrst component of the aerodynamic tensor. Another part of the aerodynamic forces is coming from the translationrotation and rotation-rotation coupling motions and can be assimilated to Coriolis-centrifuge eﬀects associated to the added mass and can also be represented as a damping eﬀect, 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 considered. 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     Cz  1 2  (1.10) τat = ρνa Sref   Cl  , 2   Cm  Cn

where Cx are the aerodynamic coeﬃcients depending on the geometrical shape of the airship and the positions of the control surfaces, rudders, and elevators. These coeﬃcients are calculated in two ways. The ﬁrst 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, deﬁned 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 propellers 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

1 Modelling and Trajectory Generation of Aerial Robots

9



 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 , η˙ = J(η2 )ν,

(1.12)

where Ma = M + A is the total mass. In aerostatics hovering (ﬂoating), the airship stability is mainly affected by its center of lift in relation to the center of gravity. The airship’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 ﬂight, stability can be aﬀected by ﬁns and the general layout of the envelope. Control inertia can be aﬀected 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 feasible 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

10

Y. Bestaoui and S. Hima

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 diﬃcult task and demands substantial computing resources. In order to reduce this complexity, it is possible to construct a nominal trajectory by sequencing a ﬁnite number of primitives from a ﬁnite 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 characterization 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. Suﬃcient 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 equilibrium in body-ﬁxed frame under a ﬁxed control conﬁguration. More precisely, they correspond to the equilibrium points of dynamic equation (1.12). Hence, there are two good reasons, apart from their historical 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 different ﬂight conditions and their eﬀect 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 stationarity of the body-ﬁxed velocity components. This condition can be mathematically formalized by: ν(t) ˙ ≡ 0 for

∀t ∈ [t0 , t1 ].

(1.13)

1 Modelling and Trajectory Generation of Aerial Robots

11

Starting with this condition, and focusing on the angular velocity kinematics 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 ﬁnd: p˙ =φ¨ − ψ¨ sin(θ) − ψ˙ cos(θ)θ˙ = 0, q˙ =θ¨ cos(φ) − θ˙ sin(φ)φ˙ + ψ¨ sin(φ) cos(θ)

− ψ˙ cos(φ) cos(θ)ψ˙ + ψ˙ sin(φ) sin(θ)θ˙ = 0, r˙ = − θ¨ sin(φ) − θ˙ cos(φ)φ˙ + ψ¨ cos(φ) cos(θ) ˙ − ψsin(φ) cos(θ)φ˙ − ψ˙ cos(φ) sin(θ)θ˙ = 0.

(1.15)

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 simpliﬁed to: p = − ψ˙ sin θ, q =ψ˙ sin(φ) cos(θ),

(1.16)

r =ψ˙ cos(φ) cos(θ). The components of the body-ﬁxed angular velocity vector depend on ˙ the roll angle φ, pitch angle θ, and the yawing rate ψ. In addition, by manipulating the linear velocity kinematics transformation 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     ˙ − b sin(ψt) ˙ ˙ + ψ0 ) a cos(ψt) V cos(γ) cos(ψt ˙ − b cos(ψt) ˙  =  V cos(γ) sin(ψt ˙ + ψ0 )  . (1.18) η˙ 1 =  a sin(ψt) c −V sin(γ)

12

Y. Bestaoui and S. Hima

√ a2 +b2 represents the ﬂight path angle. Ve = kv1 k is the γ = cos−1 Ve 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 ˙ + ψ0 ) cos(γ) sin( ψt x0 ˙  ψ  ˙ + ψ0 )  y0  . + (1.19) η1 =  − V˙ cos(γ) cos(ψt  ψ z0 −V sin(γ)t

[x0 , y0 , z0 ]T is the initial position of the airship. It is possible to parametrize 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-ﬁxed 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 speciﬁc range due to limitations of the power, angle of vectorization, and deﬂection angles of control surfaces. Using Eq. (1.12) it is straightforward to express the limitation constraints imposed on the force Fme and the angle of vectorization µme in terms of trim vector components Te and angles of deﬂection of control surfaces as follows:     Fme cos(µme ) Fx (νe , η2e , δe )    Fy (νe , η2e , δe )  0        Fz (νe , η2e , δe )  Fme sin(µme )     τp = =Mx (νe , η2e , δe ) , 0     Fme cos(µme )Pmz+Fme sin(µme )Pmx  My (νe , η2e , δe ) 0 Mz (νe , η2e , δe ) (1.20) p 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 deﬂection of control surfaces are given by:

δemin 6 δee 6 δemax , δrmin 6 δre 6 δrmax .

(1.23) (1.24)

1 Modelling and Trajectory Generation of Aerial Robots

13

Under-Actuation Constraints The control of the airship is performed by 4 actuators (i.e. throttle, 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 diﬃcult. 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 control vector ﬁelds. Let G = (g1 , g2 ) ∈ R6×2 deﬁne the subspace spanned by the control vector ﬁelds and G T ∈ R4×6 be the orthogonal subspace to the actuated space deﬁned by: G ⊥ G = 04×2 ,  −Pmz  0 G⊥ =   0 0

0 −Pmx 1 0 0 0 0 0

(1.26)  010 0 0 0 . 1 0 0 001

(1.27)

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 )  = 0. G ⊥ τp =    Mx (νe , η2e , δe ) Mz (νe , η2e , δe ) (1.28) Other Constraints The last type of constraints considered in the calculation of trim trajectory concerns the geometry of the path and the speed of the airship Ve . The geometric parameters are the ﬂight 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

14

Y. Bestaoui and S. Hima

guaranteeing the ﬂight envelope protection. These constraints are given by: Ve cos(γe ), (1.29) Re = |ψ˙ e | ! √ 2 + b2 a , (1.30) γe = cos−1 Ve p (1.31) Ve = u2e + ve2 + we2 , ! we 6 αmax , (1.32) αmin 6 αe = sin−1 p 2 ue + we2 ve −1 6 βmax . (1.33) βmin 6 βe = sin kν1e k

Taking into account the constraints previously exposed, the calculation of the trim conditions is determined by solving a set of nonlinear equalities and inequalities for the vector Te and control surface vector δ = [δe , δr ]T that makes the body-ﬁxed accelerations vector null, i.e. ν˙ ≡ 0. A convenient way to do this is to formulate an optimization 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

E=

t0

2 Fm dt.

(1.34)

The controls are constrained to take ﬁxed 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 ,

1 Modelling and Trajectory Generation of Aerial Robots

15

−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 forward trim ﬂight situation. In this case the radius of gyration is set to Re = ∞. Figure 1.1 depicts the 3D trim trajectories corresponding to ﬂight 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

z

50

0

−50

−100 10

600

5

400

0 200

−5 y

−10

0

x

Fig. 1.1. 3D trim trajectory for straight forward trim ﬂight

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

16

Y. Bestaoui and S. Hima

Body−fixes linear velocity components in (m/s)

7

6

5

ue

4

3

2

1

ve 0

we −1

10

0

20

30

40

50 t (s)

70

60

100

90

80

Fig. 1.2. Body-ﬁxed 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 ﬁnd 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

z

−10

0

10

20

Fig. 1.3. 3D circular trim trajectories with constant altitude

1 Modelling and Trajectory Generation of Aerial Robots

17

7

Body−fixed linear velocity components (m/s)

6

5

ue

4

3

2

ve 1

0

we −1

0

10

20

30

40

60

50 t (s)

70

100

90

80

Fig. 1.4. Body-ﬁxed linear velocity components evolution

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

50

100 100 50 0 −50 −100

−50

0

50

100

Fig. 1.5. 3D helical trim trajectories

18

Y. Bestaoui and S. Hima

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

7

6

u 5

4

3

2

w

v

1

0

−1

0

10

20

30

40

50 t (s)

60

70

80

90

100

Fig. 1.6. Body-ﬁxed linear velocity components evolution

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 disturbance is neglected. At low velocity, the surface control is ineﬀective so we can neglect aerodynamical forces. The three equality constraints deriving from the under-actuation are sought. Considering the dynamics of the airship and its propulsion, the following dynamics equations can be written:   Fm sin µm , Ft f = M ν˙ 1 + ν2 × M ν1 + b =  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: ˙ φ + ψS ˙ φ Cθ Ixx ℵ + (M11 − M33 )uv + (Ixx − Izz ) −θS ˙ ˙ ˙ ˙ −θSφ + ψCφ Cθ + Dp φφSθ + zb BCθ Sφ = 0.

(1.38)

1 Modelling and Trajectory Generation of Aerial Robots

19

In addition, Pm z f1 = τ2 gives ˙ φ + ψC ˙ φ Cθ ˙ ψS ˙ θ −θS Iyy ℵ+(M11 −M33 )uw+(I11 −I33 ) φ− ˙ φ Cθ − zb BSθ + zb BSθ + Pm M11 u˙ ˙ φ + ψS + Dq θC z (1.39) ˙ ˙ ˙ ˙ + Pmz M22 w θCφ + ψSφ Cθ − Pmz M22 v −θSφ + ψCφ Cθ + Pmz Du u + Pmz (mg − B)Sθ = 0,

˙ φ + ψS ¨ ψ Cθ + ψ˙ φC ˙ φ Cθ − ψ˙ θS ˙ φ Sθ . ¨ φ − θ˙φS where ℵ = θC 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θ ˙ φ + ψC ˙ φ Cθ + PT M22 v˙ − PT M22 w φ˙ − ψS ˙ θ + Nr −θS x x ˙ φ + ψC ˙ φ Cθ +PT Dv v− PT (mg−B)Sφ Cθ = 0, +PTx M11 u −θS x x

(1.40)

¨ φ − θ˙φC ˙ φ + ψC ¨ φ Cθ − ψ φS ˙ φ Cθ − ψ˙ θC ˙ φ Sθ . where ℵ′ = −θS The following approach is considered. The variations of the roll and pitch angles as well as the longitudinal velocity are imposed and the inﬂuence of the under-actuation on the variations of the yaw angle, the lateral and vertical velocities are studied. The ﬁrst equality constraint (see (1.38)) is equivalent to the resolution of an ordinary diﬀerential 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 ),

˙ θ, 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θ .

(1.42)

˙ 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)

20

Y. Bestaoui and S. Hima

where α0 =

˙ θ Cφ − θS ˙ φ) α′0 (Iyy − I11 ) − PTx (mg − B)Cθ Sφ + Dr (ψC ˙ θ − φ) ˙ −PT (ψS x

¨ θ Cφ − θS ¨ φ − ψS ˙ θ Cθ Sφ − θ˙φC ˙ φ) Izz (ψC + , ˙ θ − φ) ˙ −PT M22 (ψS

(1.45)

x

a1 = −

and

˙ θ Cφ − θS ˙ φ )M11 (ψC , ˙ θ − φ)M ˙ 22 (ψS

M11 − M22 a3 = , ˙ θ − φ) ˙ PTx M22 (ψS

Dv , ˙ θ − φ)M ˙ 22 (ψS 1 a4 = − ˙ ˙ 22 (ψSθ − φ)M

a2 = −

(1.46)

˙ θ Cφ + ψ˙ φC ˙ θ Sφ + θ˙φC ˙ φ . α′0 = −ψ˙ 2 Sθ Cθ Sφ − ψ˙ θS

(1.47)

β0 + β1 u + β2 u2 β3 uv + β4 u2 + β5 uv˙ + β6 v + β7 vβ ˙ 8 u˙ = 0,

(1.48)

The second equality constraint gives

where = β0′ + β3′ α0 , β5 = β ′ α4 , = β1′ + α1 β3′ + α0 β5′ β6 = β2′ + α2 β3′ , = β5′ α1 , β7 = α4 β3′ , = β5′ α2 + α3 β3′ , β8 = β4′ , = β5′ α3 , ˙ θ Sφ + θC ˙ φ −Bzb Sθ +Pm (mg−B)Sθ , β0′ = Iyy β0′′+(Ixx−Izz )β0′′′+Dq ψC z β0 β1 β2 β3 β4

¨ θ Sφ + θC ¨ θ + ψ˙ φC ˙ θ Cφ − ψ˙ θS ˙ θ Sφ − θ˙φS ˙ φ, β0′′ = ψC ˙ φ + ψ˙ φ˙ + ψ˙ θS ˙ θ Sφ − ψ˙ 2 Cθ Sθ , β ′′′ = −θ˙φS 0

β1′ = Pmz Xu ,

˙ θ Cφ − θS ˙ φ , β2′ = −Pmz M22 ψC ˙ θ Sφ + θC ˙ φ , β3′ = Pmz M22 ψC β4′ = Pmz M11 , β5′ = (M11 − M22 ).

The diﬀerential equation aψ¨ + bψ˙ 2 + cψ˙ + d = 0,

(1.49)

1 Modelling and Trajectory Generation of Aerial Robots

21

where a = −Ixx Sθ , c = −Dp Sθ ,

b = (Izz − Iyy )Sφ Cθ Cφ Cθ , 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 coeﬃcient 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 s Bzb + ψ0 For θ = 0; ψ(t) = t Cφ(Izz − Iyy )

(1.50)

(1.51)

For the particular case, where ψ˙ is constant, classical trim trajectories are encountered • ψ˙ is constant:   φ = cst = φ0  θ = cst = θ0  . (1.52) ψ = ψ0 t

The ﬁrst 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 α0 =

˙ θ Cφ −ψ˙ 2 Sθ Cθ Sφ (Iyy − Ixx ) + Dr ψC ˙ θ M22 PT ψS x

¨ θ Cφ −PTx (mg − B)Cθ Sφ + Izz ψC + , ˙ θ M22 PT ψS

(1.55)

x

Cθ Cφ M11 α1 = − , Sθ M22 M11 − M22 α3 = , ˙ θ PTx M22 ψS

Dv , ˙ θ M22 ψS 1 α4 = , ˙ ψSθ α2 =

while the second equality constraint gives

(1.56) (1.57)

22

Y. Bestaoui and S. Hima

v=−

β0 + β1 u + β2 u2 β6 + β3 u + β4 u2

(1.58)

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 coeﬃcients 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 solutions is an essential part of qualitative theory of this diﬀerential equation, in order to determine non trivial solutions and study their behavior. Nkashama proved that the logistic equation with positive non-autonomous bounded coeﬃcients has exactly one bounded solution that is positive and does not tend to zero. Solving the ﬁrst equality constraint, the roll moment being null, for all t, implies Lp φ˙ 0 = 0 → φ˙ 0 = 0. Rearranging the ﬁrst equality ˙ φ Sφ = 0. Three cases are constraint with this requirement gives θC 0 0 possible: θ˙0 = 0, φ0 = 0, or φ0 = π/2. The ﬁrst case: trim trajectories have already been studied in paragraph 1.3.1. If the roll angle is null, the following diﬀerential equations must be solved Cθ ¨ ˙ ˙ = 0, (1.60) ψ + ψ a + bθ0 Sθ

where a = Lp /Ixx , b = −(Izz − Iyy − Izz )/Ixx , and the following derivative ψ˙ is obtained aθ

˙ ψ(t) =−

− θ˙0 Sθ0 θ0b Sθ−b e θ˙0 . aθ0 0 + sinh − cosh aθ θ˙ θ˙ 0

(1.61)

0

The third case φ0 = π/2 gives the following diﬀerential equations: Cθ C θ = 0, (1.62) + a3 ψ¨ + ψ˙ a1 + a2 θ˙0 Sθ Sθ

1 Modelling and Trajectory Generation of Aerial Robots

23

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

¨ θ Izz ψC Nr Cθ Iyy + Izz − Ixx , − − δ1 = θ˙0 ˙ θ M22 PTx Sθ M22 PTx ψS PTx M22

M22 − M11 , ˙ θ M22 PTx ψS Yv , δ4 = − ˙ ψSθ M22

δ2 = −

Cθ M22 , Sθ M22 1 . δ5 = − ˙ ψSθ

δ3 = −

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.

4 3 2 Case 1 φ0 π/12 π/12 π/12 π/12 0 0 0 0 φ˙ 0 π/6 π/6 π/6 π/6 θ0 0.1 0 0.1 0 θ˙0 1 1 1 1 u 0 0 0 0 u˙ 10 9 8 Figure 7

For each case, four subplots are presented: the ﬁrst one presents the trajectory in space, the second one the variation of the yaw angle ψ, the third – linear velocities v and w and ﬁnally 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 constant. 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 angular and linear velocities have a slight non linear variation. After a

Y. Bestaoui and S. Hima

0 0.4

−0.05 psi

0.2 0 0

−0.1 −0.15

2

−1

1

−0.2

−2 0

0.5

1

0.5

1

0.5

1

0.5

1

0.1 PQR

VW

1 0.5 0

0 −0.1

−0.5 −1

0

0.2

1.5

−0.2

1

0.5

0

0

Fig. 1.7. Case 1

0 0

−0.05 −0.1

psi

−0.1

−0.15 −0.2 0.2

−0.2

2

0

1

−0.25

−0.2 0

0

1

0.2

0.5

0.1

0

0

PQR

VW

24

−0.5

−0.2

−1 −1.5

−0.1

0

0.5

1

−0.3

0

Fig. 1.8. Case 2

1 Modelling and Trajectory Generation of Aerial Robots

25

0 1

−0.1

psi

0

−0.2 −0.3

−1 1

−0.4

2

0

1

−0.5

−1 0

0

0.5

2

0

VW

PQR

4

1

0.5

1

0.5

1

0.5

1

−0.5

0

−2

0.5

−1

1

0.5

0

0

Fig. 1.9. Case 3

0 0

−0.1

psi

−0.2

−0.2 −0.3

−0.4 1

−0.4

2

0

1

−0.5

−1 0

0

0.5

2

PQR

VW

1 0

0

−1 −2

0

0.5

1

−0.5

0

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 ampliﬁed.

26

Y. Bestaoui and S. Hima

1.5 Conclusions In the ﬁrst part of this paper, we have discussed dynamic modeling of small autonomous blimps. Blimps are highly interesting study objects due to their stability properties. Here, motion is referenced to a system of orthogonal body axes ﬁxed in the airship, with the origin assumed to coincide with the center of gravity. The equations of motion 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 diﬀerential 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 diﬀerential 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 ﬁxed 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 ﬁxed 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 autonomous 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, Guidance, and Control, Austin, TX, 2003. 5. Y. Bestaoui, L. Beji, A. Abichou. Modelling and control of small autonomous airships, In: Modelling and control of mini ﬂying machines, P. Castillo, R. Lozano & A. Dzul (Eds.), Springer, 2005.

1 Modelling and Trajectory Generation of Aerial Robots

27

6. Y. Bestaoui. Nominal Trajectories of an autonomous under-actuated airship, International Journal of Control, Automation and Systems, 2006, pp. 395-404. 7. E. Frazzoli. Robust hybrid control for autonomous vehicle motion planning, 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 coeﬃcients, Electronic Journal of Diﬀerential Equations, no. 119, 2003, pp. 1-13. 10. R. Grimshaw. Non-linear ordinary diﬀerential equations, CRC Press, 1993. 11. S. Hima. Y. Bestaoui. Motion generation on trim trajectories for an autonomous underactuated airship, In: Proc. 4th International Airship Conference, Cambridge, England, 2002. 12. S. Hima, Y. Bestaoui. Time optimal paths for lateral navigation of an autonomous underactuated airship, In: Proc. AIAA Conference on Navigation, Guidance and Control, Austin, Texas, 2003. 13. S. Hima. Planiﬁcation de trajectories d’un dirigeable autonome, PhD thesis, 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. Sou`eres, S. Lacroix. Dynamic and aerodynamic modeling of a small size airship, Proc. of the 5th workshop on Electronics, Control, Modeling, Measurements and Signals, Toulouse, France, 2001. 16. E. Hygounenc. Mod´elisation et commande d’un dirigeable pour le vol autonome, PhD thesis, LAAS, Univ. of Toulouse, France, 2003. 17. D. Jiang, J. Wei, B. Zhang. Positive periodic solutions of functional diﬀerential equations and population models, Electronic Journal of Diﬀerential 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 Conﬁguration 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 planning. TR 98-11, Computer Science Dept, Iowa State University, 1998.

28

Y. Bestaoui and S. Hima

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 coeﬃcients, Electronic Journal of Diﬀerential Equations, no. 02, 2000, pp. 1-8. 25. B.L. Stevens. F. Lewis. Aircraft control and simulation, 2nd edition, Wiley, 2003. 26. J.S. Turner. Buoyancy eﬀects in ﬂuids, 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 [email protected], {k.stol, v.kecman}@auckland.ac.nz

2.1 Introduction Unmanned aerial vehicles (UAVs) comprise various types of aircrafts such as conventional ﬁxed-wing aircraft, helicopters, blimps, and airships. Among these, helicopters are classiﬁed as planar vertical take oﬀ and landing (PVTOL) aircraft by which it means that unlike a ﬁxed-wing aircraft, it can take oﬀ and land in a limited space, hover in the air, and move sideways and backwards. This superior maneuverability allows performing important roles in many areas, which conventional 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 ﬁlm 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 nonlinear 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 application of two diﬀerent techniques, PID and linear quadratic (LQ), to OS4. OS4 at near-hover condition was tested, neglecting gyroscopic effects from the rigid body and propeller rotation, i.e. removing all cross K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 29–38, 2007. c Springer-Verlag London Limited 2007 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 ﬁnd inputs that would move the system from its initial conﬁguration to a desired ﬁnal conﬁguration 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 generation and dynamic stability. Airfoil design and material properties of blades were studied and the ﬂapping 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 helicopter 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-ﬂying 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 ﬂight. Limitations of classical control on a complex dynamic plant were observed. In this research we aimed to develop control algorithms to stabilize an unstable quadrotor plant and implement this on an experimental rig. Four diﬀerent 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 ﬁxed 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 conﬁguration

Prior to simulation, a mathematical model for the quadrotor is derived. For the derivation, the notation from [3] is followed. The rotational dynamics of the quadrotor is expressed as: .. . . Jp . Iy − Iz l 2 2 − φ = θψ θ(Ω2 + Ω4 − Ω1 − Ω3 ) + b(Ω4 − Ω2 ), (2.1) Ix Ix Ix ..

. .

θ = φψ

Iz − Ix Iy

Ip . l φ(Ω2 + Ω4 − Ω1 − Ω3 ) + b(Ω32 − Ω12 ), (2.2) 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 coeﬃcients, respectively. The assumptions for this model are: • the body is rigid and symmetrical; • the centre of mass and the body ﬁxed frame origin coincide; • the propellers are rigid, i.e. no blade ﬂapping occurs.

As the equations show there is coupling present between the rotational speeds of the body.

32

T.S. Kim, K. Stol, and V. Kecman

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 speciﬁcation 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 incremental 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 ﬁbre arms come from a commercial quadrotor design, the Dranganﬂyer 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 ﬁrst step for deriving a linear control law. The plant is linearized about an equilibrium

2 Control of 3 DOF Quadrotor Model

33

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 ∆ψ˙ ∆ψ¨ 



0 0 0 0

0 0 0

     −2b Ilx Ωss   −2b l Ω 0  Iy ss −2d Ilz Ωss 2d Ilz Ωss

 0 0   0 0  ∆Ω1  0 0   ∆Ω2  . l 0 2b Ix Ωss    ∆Ω3   0 2b Ily Ωss  ∆Ω4 −2d Ilz Ωss 2d Ilz Ωss

(2.4)

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 relinearize about every operating point along the state trajectory [14]. The resulting matrix form is the same as above having the corresponding matrices as given below: 

000 1 0 0 0 0 0 1  0 0 0 0 0  −  − A= 0 I1 ψ˙ −I4 Ω 0 0 0  − −   0 0 0 I2 ψ˙ +I5 Ω 0  − − ˙ I3 φ˙ 000 I3 θ

0 0 1



    − I1 θ˙   , and B = −  I2 φ˙   0

(2.5)

T.S. Kim, K. Stol, and V. Kecman

34



0 0 0

0 0 0

0 0 0



0 0 0

        − − − − − −   , ˙ ˙ ˙ ˙  I4 θ I4 θ −(I4 θ +2bI6 Ω2 ) −I4 θ +2bI6 Ω4    − − − −   −  −(I φ˙ +2bI Ω− )  I5 φ˙ I5 φ˙ −I5 φ˙ +2bI7 Ω3 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: . . Jp . Iy − Iz Ix + U1 = (− θ ψ θ(Ω2 + Ω4 − Ω1 − Ω3 ) − k1 x), (2.7) Ix Ix l . . Iy U2 = (− φ ψ l

Iz − Ix Iy

−

Jp . φ(Ω2 + Ω4 − Ω1 − Ω3 ) − k2 x), (2.8) Iy

. . Iz U3 = (− θ φ l

Ix − Iy Iz

− k3 x),

(2.9)

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 Control of 3 DOF Quadrotor Model

35

2.4.4 Sliding Mode Control The following sliding mode controller is developed to drive all the states to zero. The terms in the ﬁrst bracket of Eqs. (2.11) through (2.13) cancel out the nonlinearities, while the following term with sign switches control input to keep the system in a stable manifold [15]. Jp ˙ Ix ˙ ˙ Iy − Iz ˙ − θ(Ω2 + Ω4 − Ω1 − Ω3 ) + α1 φ θψ U1 = − Ix Ix l ˙ − K1 sign(φ + α1 φ), (2.11) Iy ˙ ˙ Iz − Ix Jp ˙ + φ(Ω2 + Ω4 − Ω1 − Ω3 ) + α2 θ˙ φψ U2 = − Iy Iy l ˙ − K2 sign(θ + α2 θ), (2.12) Iz ˙ ˙ Ix − Iy + α3 ψ˙ − K3 sign(ψ˙ + α3 ψ). (2.13) θφ U3 = − Iz l

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 parameters 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 eﬀort. 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 orientation. Figure 2.4 shows the responses with uncertainty introduced to plant parameters. Tables 2.1 and 2.2 summarize the performance of the four diﬀerent 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 eﬀort but relatively small percentage changes in total control eﬀort indicate that the eﬀort required to improve orientation control is much smaller than that for balancing the gravitational force. On the other hand, with model uncertainty introduced, the performance of sliding mode controller deteriorates and the performance of LQR with gain scheduling becomes outstanding.

36

T.S. Kim, K. Stol, and V. Kecman

Fig. 2.3. Simulation result for four controllers in state regulation

Fig. 2.4. Simulation result for controllers with model uncertainty

2 Control of 3 DOF Quadrotor Model

37

Table 2.1. ITSE values and total control eﬀorts in state regulation Controller ITSE index % diﬀerence LQR 0.3018 0 Gain Schedule 0.2491 -17.5 Feedback Lin. 0.3291 +9.1 Sliding Mode 0.2186 -27.6

Σ|u(t)| % diﬀerence 479.3 0 482.7 +0.7 -1.3 472.9 495.7 +3.4

Table 2.2. ITSE values and total control eﬀorts with model uncertainty Controller ITSE index % diﬀerence LQR 0.3180 0 Gain Schedule 0.2320 -27.0 Feedback Lin. 0.3488 +9.7 Sliding Mode 0.3078 -3.2

Σ|u(t)| % diﬀerence 479.7 0 481.8 +0.4 473.0 -1.4 496.3 +3.5

2.6 Conclusions In this paper, four diﬀerent control techniques: LQR, LQR with gain scheduling, feedback linearization, and sliding mode control were derived and simulated on a 3-DOF quadrotor model. Compared under ITSE criteria, sliding mode control produced the best result with accurate 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 simulation 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-ﬂying quadrotor.

References 1. Sugiura R, Fukagawa T, Noguchi, N (2003) Field Information System 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 Techniques Applied to an Indoor Micro Quadrotor. Proc. IEEE Int. Conf. on Intelligent Robots and Systems

38

T.S. Kim, K. Stol, and V. Kecman

3. Bouabdallah S, Noth A, Siegwart R (2004) PID vs LQ Control Techniques 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 QuadRotor Helicopter. Journal of Intelligent and Robotic Systems 45: 115–135 6. Altug E, Ostrowski J, Mahony R (2002) Control of a Quadrotor Helicopter Using Visual Feedback. Proc. Int. Conf. on Robotics & Automation. 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 DynamicallyFavourable 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. Draganﬂy 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 ﬂexible 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 Shaﬁq2 , 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 [email protected]

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 angular velocity vector and the dynamics equation, which describes the time evolution of the angular velocity vector [2, 3]. Various parameterizations of the attitude exist to represent the orientation angles. A comprehensive survey of attitude representations is given in [4]. The attitude control problem was ﬁrst 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 nonsingular 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 identiﬁes the inertia matrix via periodic command signals. Reference [9] discusses the adaptive attitude tracking control using synthesized velocity from attitude measurements by incorporating a velocity ﬁlter formulation. ∗

The authors acknowledge King Fahd University of Petroleum and Minerals, Dhahran 31261 Saudi Arabia, for supporting this research.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 39–48, 2007. c Springer-Verlag London Limited 2007 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 ﬁrst 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-ﬁxed 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 modiﬁed Rodrigues parameters describing the spacecraft attitude with respect to an inertial frame, given by φ(t) ˆ , φ ∈ [0, 2π] rad, (3.4) q(t) = k tan 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 modiﬁed Rodrigues parameters is given by 1 − qT q 1 T I3×3 + S(q) + qq . (3.5) R(q) = 2 2

Equations (3.1) and (3.2) are combined to form a second-order nonlinear 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 deﬁned as P (q) = R−1 (q), ∗ T ∗ ∗ T ˙ H = P hP , and C = −H RP − P 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 parameters 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.

t→∞

(3.7)

3.4 Controller Design 3.4.1 Plant Stabilization We ﬁrst 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 deﬁnite function [10]: 1 V (q, q) ˙ = [q˙T H ∗ q˙ + q˜T sp q˜] > 0. 2

(3.9)

42

A. Al-Garni et al.

Fig. 3.1. Plant stabilization using state feedback control

The positive deﬁnite 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 deﬁnite 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˜, 2 1 V˙ (q, q) ˙ = q˙T H ∗ q¨ + q˙T H˙ ∗ q˙ + q˜˙T sp q˜. 2

(3.10) (3.11)

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˜, V˙ (q, q) ˙ = −q˙T sd q. ˙

(3.13) (3.14)

P T RT = I, −q˙ = q˜˙. In practice q(t) ˙ is not available for the feedback [9]. The velocity estimate is derived from the attitude measurement q, using a ﬁlter αp/(p + α), which approximates the derivative operator, p, when α (positive gain) is very large. Hence we can say that o

qˆ (t) =

αp q. p+α

(3.15)

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

3 Internal Model Control-Based Adaptive Attitude Tracking o

o

o V1

V1 (q, q) ˙ = −q˙T sd qˆ (t),

43

(3.17)

where (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 deﬁnite and its time derivative in (3.14) is negative semi-deﬁnite. Since V˙ (q, q) ˙ o

in (3.14) is negative semi-deﬁnite then V1 (q, q) ˙ in (3.17) is also negative semi-deﬁnite 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 ˙ q˜(λ)dλ + Td q˜(t) , (3.18) τP ID = kp q˜(t) + 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.

44

A. Al-Garni et al.

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, ﬁne 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 controller, 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 diﬀerence in behavior between the plant and d(t) its model and can therefore be used to improve control eﬀort. 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

3 Internal Model Control-Based Adaptive Attitude Tracking

45

The model of the plant is computed using a multi-channel adaptive ﬁlter employing the normalized least mean square (NLMS) adaptation algorithm discussed brieﬂy 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 characteristics of the NLMS algorithm. Wk is the weight vector between the delayed inputs and the outputs of the adaptive ﬁlter. The weights of the NLMS adaptive ﬁler 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)

46

A. Al-Garni et al.

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 uncertainty. 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 simpliﬁcation 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

3 Internal Model Control-Based Adaptive Attitude Tracking

47

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 ﬁrst stabilized using state feedback control based on Lyapunov theory. Adaptive attitude tracking is established by incorporating an IMC structure along with the feedforward 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 eﬀectiveness 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, Cambridge, 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 spacecraft. 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 identiﬁcation. 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. ˚ Astr¨om K.J., Wittenmark B., Adaptive control. Addison-Wesley, USA, 1995. 12. Tan Kok Kiong, Wang Quing-Guo, Hang Chang Chieh, T.J. H¨ agglund, Advances in PID control. Springer, London, 1999.

48

A. Al-Garni et al.

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. Shaﬁq M., Riyaz S.H., Internal model control structure using adaptive inverse control strategy, In: Proc. 4th Int. Conf. on Control and Automation (ICCA), 2003, p. 59. 17. Morari M., Zaﬁriou 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 identiﬁcation, 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 ∗ Lotﬁ Beji1 and Azgal Abichou2 1

2

IBISC Laboratory, CNRS-FRE 2873, Universit´e d’Evry Val d’Essonne. 40, rue du Pelvoux, 91020 Evry Cedex, France [email protected] LIM Laboratory, Ecole Polytechnique de Tunisie, BP 743, 2078 La Marsa, Tunisia [email protected]

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 simpliﬁed 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 nonconstant 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 Scientiﬁc Research and Technology for the invitation within the framework of the 2006SERST program. K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 49–56, 2007. c Springer-Verlag London Limited 2007 springerlink.com

50

L. Beji and A. Abichou

tracking problem takes into account the side-slip eﬀects 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 velocity, and the front-wheel angular velocities. Exponential and asymptotic stabilities result from a reference road to be followed and free of sideslip. 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 predeﬁned 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 dynamical variables are standard and shown in Fig.4.1): x˙ =v cos(ψ − β),

ψ˙ = r, δ˙ = ω,

y˙ =v sin(ψ − β), ˙ cot δ + θ2 Fad + θ3 Fas cot δ cos δ + θ4 Ras cot δ + θ5 τr , v˙ =θ1 v(β˙ − ψ)

1 r˙ = {Fas lv cos δ − Ras lh }, I ω˙ =θ6 ω + θ7 τb , v = kVk,

(4.1)

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 following (7 × 1) vector [3]:     1 (Ir + If ) rad m + rad θ1  1  θ2    2 rad CAρ       θ3   r m ad      Γ vm = r θ (4.2) θ= , ad 4 α    vm  Γhm  θ5    r ad αhm      θ6   1  n Idw 1 θ7 n fω

Fas =

ψ˙ Γvm (β − lv + δ); v αvm

Ras =

ψ˙ Γhm (β + lh ), v αhm

(4.3)

while the front aerodynamic drag force is 1 Fad = CAρv 2 . 2

(4.4)

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 argument (more details are in [8]).

52

L. Beji and A. Abichou

Remark 1. In Eq. (4.1) the input in torque τb can be deﬁned such that ω → ωd then δ → δd . Moreover, δd can be speciﬁed 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) δd = arccos

IWr + Ras lh Fas lv

.

(4.5)

Then the yaw motion dynamics is transformed to r˙ = Wr . As soon as for the dynamics of v, one proposes the following linearizing feedback τr for the rear torque: τr =

1 ˙ cot δd − θ2 Fad (Wv − θ1 v(β˙ − ψ) θ5 − θ3 Fas cot δd cos δd − θ4 Ras cot δd ),

(4.6)

which permits to write v˙ = Wv . Inputs (Wr , Wv ) are speciﬁed later. The vehicle tracking problem is based on the following new writing: x˙ =v cos(ψ − β), y˙ =v sin(ψ − β), ψ˙ =r, v˙ = Wv ,

(4.7) r˙ = Wr .

Let us deﬁne the reference path that the vehicle should follow: x˙ r =v r cos(ψ r ), y˙ r =v r sin(ψ r ), ψ˙ r =r r , v˙ r = Wvr ,

(4.8) r˙ r = Wrr .

As we can note in system (4.8), one assumes that reference trajectories 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, z3 =ψ − β.

(4.9)

4 Tracking Control of Automated Guided Vehicles

53

Hence, with respect to time, the derivative of (4.9) gives ˙ 2 + v, z˙1 =(ψ˙ − β)z ˙ 1, z˙2 = − (ψ˙ − β)z ˙ z˙3 =r − β, v˙ =Wv ,

(4.10)

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 , z3r =ψ r .

(4.11)

Let us introduce errors which are deﬁned as ez1 = z1 − z1r , ez2 = z2 − z2r , and ez3 = z3 − z3r . The associated dynamics is given by ˙ 2, e˙z1 =ez2 ψ˙ r + z2 (ψ˙ − ψ˙ r ) + (v − v r ) − βz ˙ 1, e˙z = − ez ψ˙ r − z1 (ψ˙ − ψ˙ r ) + βz 2

1

˙ e˙ z3 =r − r r − β, r e˙ v =Wv − Wv ,

e˙ r = Wr −

(4.12)

Wrr .

Moreover one takes er = r − r r and ev = v − v r . After some manipulation, we get ˙ 2, e˙ z1 =ez2 ψ˙ r + z2 er + ev − βz ˙ 1, e˙ z = − ez ψ˙ r − z1 er + βz 2

1

˙ e˙ z3 =r − r r − β, r e˙ v =Wv − Wv ,

e˙ r = Wr −

(4.13) Wrr .

The last two equations permit to deﬁne 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 ˙ 2, e˙z1 =ez2 r r + z2 edr + edv − βz ˙ 1, e˙z = − ez r r − z1 ed + βz 2

1

˙ e˙z3 =edr − β.

r

(4.15)

54

L. Beji and A. Abichou

˙ Then Let us introduce Λ = edr − β. e˙ z1 =ez2 r r + edv + (ez2 + z2r )Λ, e˙ z2 = − ez1 r r + (ez1 + z1r )Λ, e˙ z3 =Λ.

(4.16)

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 }

e˙ z2 = −

0 r η(t)ez1 − ez3 z1 ,

| {z }

(4.17)

0

where η(t) = r r − ez3 . It is a time-varying linear system in errors with η(t) = r r − λe−t . One veriﬁes 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.

4.3 Simulation Results 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 conﬁgurations 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

y,yr(m)

1

0.5

0

−0.5 −0.5

1

0.5

0

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 presented. The tracking control problem takes the nonlinear behavior of the system including side-slip eﬀects. The braking angle combined with the rear torque realize a ﬁrst 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 behavior. 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 equilibria of automated vehicles,” in Int. Conf. on Advanced Robotics, Coimbra, 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 vehicles 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. ”

”

”

L. Beji and A. Abichou

”

56

5. C.Y. Chan, Open-loop trajectory design for longitudinal vehicle maneuvers: 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 trajectory 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 Scientiﬁc, 1997. 10. H. Hu, M. Brady and P. Probert, Trajectory planning and optimal tracking 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 autonomous 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 approach,” in Proc. CDC, Conference on Decision and Control, Australia, 2000. 14. T. Pilutti et al., Vehicle Steering intervention through diﬀerential braking,” 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, International Conference in Robotics and Automation, 1994, pp.2802-2807. 16. D.B. Reister and F. G. Pin, Time-optimal trajectories for mobile robots 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, Europeen 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, Pozna´ n University of Technology, Piotrowo 3a, 60-965 Pozna´ n, Poland [email protected]

5.1 Introduction Skid is a common phenomenon in real vehicles. Sometimes this eﬀect is negligibly small and in a consequence can be omitted in a vehicle model preserving practically acceptable control performance. In a case of nonholonomic mobile robots it is equivalent to the rolling without slipping assumption. However, in many real-life situations the skid phenomenon inﬂuences vehicle motion in a such degree, that exclusion of it in a system model leads to signiﬁcant control performance deterioration. The term skid used in this paper should be understood as a phenomenon of motion velocity disturbance, which for nonholonomic vehicles is connected with violating kinematic constraints. Wind blowing during aircraft ﬂight, 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 phenomenon 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 speciﬁc subclass of nonholonomic driftless systems has been proposed and veriﬁed. The VFO control strategy can be treated as 1

VFO is an abbreviation from words: Vector Field(s) Orientation.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 57–66, 2007. c Springer-Verlag London Limited 2007 springerlink.com

58

M. Michalek

a generalization of control in polar coordinates (the need of such a generalization has been already pointed to in [5]). The results have revealed good performance obtained for 3-dimensional systems with VFO controllers guaranteeing, among others, good control quality (fast asymptotic error convergence, natural and non-oscillatory transient behavior), intuitive control input interpretation, and very simple controller parametric 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 inﬂuence compensation during vehicle motion. The VFO controller proposed in [2] is modiﬁed to preserve asymptotic convergence of a vehicle position error despite of skid eﬀect 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 0 sin θ y˙

where q ∈ R3 is a vehicle state and u ∈ R2 is a control vector deﬁned 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 moving on a plane: two-wheeled robot (A), aircraft (B), ship (C)

2

Fig. 5.2. Mobile vehicle as a rigid body on a plane

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 justiﬁed to take into account only direct results of skidding. Regardless of the reasons of the skid eﬀect, 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 0 sin θ vsyg y˙

∗ = [v T 2 where vsg sxg vsyg ] ∈ R is a skid velocity vector described in ∗ term in a local frame a global frame (Fig. 5.2). Expressing the vsg 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) 0 sin θ cos θ y˙

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 eﬀect 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 inﬂuence in a global frame, so in a local frame skid velocity coordinates can be varying. The opposite situation can be seen for instance during mo- ving 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 eﬀects 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

4

Eq.(5.3) is equivalent to the rigid body motion q˙ = R(θ)η, where R(θ) ∈ SO(3) and η = [u1 (u2 + vsx ) vsy ]T ∈ R3 . In real conditions also random ﬂuctuations of skid components are characteristic.

M. Michalek

60

Problem 1. For a given admissible and persistently exciting reference trajectory qt (τ ) = [θt (τ ) xt (τ ) yt (τ )]T ∈ R3 and assuming a nonzero ∗ ∈ L ∗ ∈ L , ﬁnd a bounded control law ˙ sg skid velocity vsg ∞ with v ∞ u(qt , q, ·) for the kinematics (5.2), which guarantees asymptotic convergence of the position error e∗ = [ex ey ]T ∈ R2 to zero and boundedness 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 (τ ) satisﬁes Eq.(5.1) for some reference inputs u1t (τ ), u2t (τ ) ∈ L∞ . Persistent excitation 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 dedicated to kinematics (5.1), hence for a case without skid disturbances. After that, by analogy to previous considerations, an extension regarding 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 subsystems: one-dimensional orienting subsystem represented by the equation θ˙ = 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 pushing 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 ﬁeld and introduced additional convergence vector ﬁeld h = [h1 h∗T ]T . This vector ﬁeld has been designed so as to indicate in any state q an instantaneous convergence 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.

5 VFO Control in the Presence of Skid Phenomenon

61

control strategy can be decomposed into the orienting subprocess, responsible 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 orienting 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 ﬁeld h = [h1 h∗T ]T ∈ R3 , where T ∆ h∗ (e∗ , q˙t∗ ) = h2 h3 = kp e∗ + q˙t∗ ,

T q˙t∗ = x˙ t y˙ t ,

(5.5)

and kp > 0 is a design parameter (deﬁnition of the h1 component will be introduced later). Since for every error e∗ , vector h∗ (e∗ , q˙t∗ ) deﬁnes 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 deﬁned by h ). From now on we take fk (τ ) = fk ≡ 1 to stress that not only directions but also orientations 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

τ →∞

with

∆

H2 = h2 − vsxg ,

∆

H3 = h3 − vsyg .

(5.7)

(5.8)

The limit (5.7) describes the so-called orienting condition, which guarantees matching of orientations for vectors q˙ ∗ and h∗ . Hereafter the

62

M. Michalek

VFO control design follows by analogy to the original concept mentioned 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 suﬃces to make the auxiliary error ea = θa − θ tend to zero. Hence, let us deﬁne the h1 component of vector h as follows: ∆ h1 = k1 ea + θ˙a , (5.10) where k1 > 0 is a second design coeﬃcient, θ˙a = (H˙ 3 H2 −H3 H˙ 2 )/k H ∗ k2 results from time-diﬀerentiation of (5.9), and H ∗ = [H2 H3 ]T . According to the VFO strategy one proposes the following VFO control law: u1 = h1 ∗ u2 = g ∗T 2 H

⇒ ⇒

u1 = k1 ea + θ˙a , u2 = H2 cos θ + H3 sin θ.

(5.11) (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 equiva- lently relation (5.6) – is satisﬁed. Proposition 1. Assuming that ∀τ ≥0 H ∗ (τ ) 6= 0 and the reference trajectory qt∗ is suﬃciently 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 ﬁrst 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,

θ→θa

(5.14)

p where γ(θ) = 1 − cos2 α(θ) ∈ [0, 1] and α(θ) = ∠(g ∗2 (θ), H ∗ ). Introducing a positive deﬁnite 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 persi- stently exciting reference trajectories (see the comment after Problem 1).

5 VFO Control in the Presence of Skid Phenomenon

63

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 γ =

2 ∗ = −kp k e∗ k + k e∗ k kp e∗ + q˙t∗ − vsg γ≤

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

∗ where κ = k q˙t∗ k + vsg . Function W (τ ) is positive deﬁnite for 2

k e∗ (τ )k > Γ (τ ),

where Γ (τ ) =

γ(τ )κ(τ ) . kp (1 − γ(τ ))

(5.15)

Function Γ is ﬁnite 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˙ (τ ) < ∞ (ﬁ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 ﬁnite Γ (τ ) 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 Barbalat’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 ﬁnally 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 deﬁnition (5.9), which is not determined for H ∗ = 0 (the reason of the assumption 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-deﬁned control u1 , one proposes to introduce additional deﬁnitions for θa and θ˙a in assumed suﬃciently small ǫ-vicinity of H ∗ = 0. One proposes to take ∆ ∆ θa = θa (τ− ) and θ˙a = 0 for k H ∗ k ≤ ǫ, (5.16) 7

Note that h∗ deﬁnes an instantaneous convergence direction.

64

M. Michalek

∗ where 0 < ǫ < inf τ q˙t∗ (τ ) − vsg (τ ) and τ− denotes the time instant of reaching the ǫ-vicinity. These additional deﬁnitions 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 practical 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 justiﬁed 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 sensor (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 random measurement noise always present in practical implementations. If noise causes high estimate variances, one can ﬁlter right-hand sides of F ,v F (ﬁltered-estimator). (5.17,5.18) and use ﬁltered components vˆsxg ˆsyg The skid estimates can now be used in deﬁnitions (5.8) instead of true signals (vsxg , vsyg ), which are not known in practice.

5.4 Simulation Results The veriﬁcation of the eﬀectiveness of the proposed controller has been carried out for the kinematics of a diﬀerentially driven twowheeled 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 ﬁltered-estimator9 (5.17,5.18) computed 8 9

The control vector scaling procedure described in [7] has been used. A ﬁrst order low-pass ﬁlter with a time constant T = 0.1 has been applied.

5 VFO Control in the Presence of Skid Phenomenon 0.8

syg

0.6 0.4

[m/s], v

1

[m/s]

e θ e x ey

0.2

sxg

0

v sxg vsyg

0

v

−1 0

5

τ [s]

10

15

3

5

τ [s]

10

15

100

u 1 u2

2

−0.2 0

ω , ω [rad/s]

eφ [rad], ex, ey [m]

2

L

60

L

1

ωR ω

80

40

R

u1 [rad/s], u2 [m/s]

65

0 5

τ [s]

10

0 0

15

4

4

3

3

2

2

y [m]

y [m]

−1 0

20

1

1

0

0

−1

−1 −4

−3

−2

−1 x [m]

0

1

τ [s]

5

−4

−3

−2

−1 x [m]

10

15

0

1

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 zeromean 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 orientation (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 eﬀect 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 ﬁrst attempt to use the VFO control method for dynamical systems with a drift. The simulation results included in the paper illustrate the eﬀectiveness 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 nonholonomic systems (in Polish). PhD thesis, Pozna´ n University of Technology, Chair of Control and Systems Engineering, Pozna´ n, 2006. 3. I. Harmati, B. Kiss, and E. Sz´adeczky-Kardoss. On drift neutralization of stratiﬁed systems. In Robot Motion and Control. Recent Developments, volume 335 of LNCIS, pages 85–96. Springer, 2006. 4. T. Holzh¨ uter 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 stabilization 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-Mel´endez, L. Ferm´ın, J. Cappelletto, C. Murrugarra, G. Fern´ andez-L´ opez, and J.C. Grieco Universidad Sim´on 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 ﬁeld control (VFC). In 1995, velocity ﬁelds are deﬁned as a set of velocity vectors located at each possible position of the robot platform inside its workspace coding a speciﬁed task [1]. The employment of a control scheme whose reference is a velocity ﬁeld has allowed to encourage different 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 ﬁelds [6, 7]. Almost all the cases previously mentioned focus on the design of control schemes taking as reference a theoretical and time invariant velocity ﬁeld; few works have considered the problem of using time varying velocity ﬁelds [8, 9]. The objective of this work is to oﬀer an algorithm for the dynamic generation of velocity ﬁelds 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 oﬀers all the details of the al∗

The authors gratefully acknowledge the ﬁnancial support of the Research and Development and Graduate Studies Deanships of Simon Bolivar University to the development of this project.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 69–80, 2007. c Springer-Verlag London Limited 2007 springerlink.com

70

W. Melina-Mel´endez 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 ﬁeld 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 ﬁeld coding a speciﬁc task. This ﬁeld is the one that will be modiﬁed if there is some obstacle inside the robot workspace. 3. To create “evader” local velocity ﬁelds to surround, as close as possible, each detected object, and combine them with the original ﬁeld in order to not alter it signiﬁcantly and obtain an obstacle-free trajectory. The inﬂuence area of this ﬁeld has to be limited as a function of the distance to the object being analyzed. A vision system was developed for robot and obstacle identiﬁcation and information extraction from the visual scene. For the “initial” and “evader” velocity ﬁeld 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 ﬂoor and covers an area of 4 m2 after cropping 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 ﬁelds 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 ﬁgures: squares, circles, and rectangles, and through a classiﬁcation technique it was possible either to identify them or to obtain 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 location2 , but to the obstacles it employs particle analysis. Instead of detecting known geometric shapes, particles are identiﬁed and all the necessary parameters to generate the “evader” velocity ﬁelds are obtained from them. Mobile Robot Detection The pattern matching algorithm [10] consists in the localization of regions that match a known reference pattern on a grayscale image. The template contains information related to edge and region pixels, removing 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 reﬂect the pattern rotation. The correlation process involved is based on the calculation of the squared Euclidean distance: X [f (x, y) − t(x − u, y − v)]2 , (6.1) d2f,t (u, v) = x,y

where f is the image, and the sum is over (x, y), in the window containing the sub-image t located at u, v. Expanding d2 , and making some considerations [10], the remaining term of the cross correlation X f (x, y)t(x − u, y − v) (6.2) C(u, v) = x,y

is the similarity or matching measure between the image and the template. 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 ﬁlter in order to obtain a binary image. Then, detecting nonzero pixels, and studying their neighborhoods considering 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.

72

W. Melina-Mel´endez et al.

Fig. 6.1. Parameter extraction process from particle analysis

• F : Maximum Feret Diameter; • Fx1 : X coordinate of the start point of the Maximum Feret Diameter; • Fy1 : Y coordinate of the start point of the Maximum Feret Diameter; • 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 circumscribed ellipse equation are easily obtained through algebraic and geometric manipulation. Figure 6.1 illustrates the process just described. 6.3.2 Initial Velocity Field Generation This system allows user deﬁned 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 ﬁelds and can be described as follows.

6 Vision-Based Velocity Field Generation for Mobile Robots

73

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 ﬁeld oﬀers 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 ﬁnishes deﬁning the desired trajectory, the coordinates of the pixels to be part of the trajectory are extracted by a thresholding 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 ﬁeld is deﬁned. 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 deﬁned 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 ﬁeld is also deﬁned. 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 ﬁeld is obtained performing a weighted sum, expressed in (6.3), between the approximation and tangent vector ﬁelds. 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 sigmoidal 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 ﬁeld, Vaij and Vpij are the approximation and tangent vectors at ij, respectively. dij is the

74

W. Melina-Mel´endez et al.

Euclidean distance from point ij to the desired trajectory, whereas f1 (dij ) and f2 (dij ) are deﬁned by: f1 (dij ) =

2 − 1, 1 + e−α·dij

(6.4)

f2 (dij ) = 1 − f1 (dij ). Parameter α was chosen to be 0.4 because this value allows an important attenuation of the tangent vectors when dij > 6 (3 times the dimensions of the robot). Figure 6.2 shows the eﬀect of the weighting functions expressed in (6.4).

Fig. 6.2. Eﬀect 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 ﬁeld generation module takes information provided by the vision system, parameterizes the corresponding ellipse for each obstacle and creates a velocity ﬁeld 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 ﬁeld 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) + B2 A2 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:

6 Vision-Based Velocity Field Generation for Mobile Robots

75

Vt (X0 , Y0 ) = (Vtx , Vty ), Vtx = p

Vty = p

A2 · Y0

,

−B 2 · X0

.

A4 · Y02 + B 4 · X02

A4 · Y02 + B 4 · X02

(6.6)

In the entry region the ﬁeld is deﬁned 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 deﬁned by s ! 1 + (A/B)2 · tan2 (α − β) . (6.7) ϕ = 2 · atan (A/B)2 + tan2 (α − β)

The orientation of the regions is given by the angle of the original ﬁeld at the point where the object is located. The regions must be rotated for achieving an entry region aligned with the original velocity ﬁeld. For the exit region the same approach as used for the entry region is employed. However, in this case, the ﬁeld is deﬁned leaving the ellipse. The approximation vectors at any point (X0 , Y0 ) are given by: Va (X0 , Y0 ) = (Vax , Vay ), Vax = p

Vay = p

B 2 · X0

,

A2 · Y0

.

A4 · Y02 + B 4 · X02

A4 · Y02 + B 4 · X02

(6.8)

The division proposed by using the deﬁned regions wants to achieve an “evader” velocity ﬁeld similar to the sketch shown in Fig. 6.3.

6.4 Results For testing the “initial” velocity ﬁeld generator two handmade traces were introduced. Figure 6.4 shows the obtained velocity ﬁelds.

76

W. Melina-Mel´endez et al.

Fig. 6.3. Sketch of the “evader” ﬁeld. Following the direction and sense of the initial ﬁeld at the point where the obstacle is located, the diﬀerent regions are deﬁned. Note the deviations of ﬁeld in the entry and exit regions.

(a)

(b)

Fig. 6.4. Initial velocity ﬁeld. 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 ﬁeld 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 ﬁeld 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.

6 Vision-Based Velocity Field Generation for Mobile Robots

(a)

77

(b)

Fig. 6.5. “Evader” velocity ﬁeld for an arbitrary object. The four predeﬁned 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 ﬁeld has an orientation of α = 140◦ and the circumscribed ellipse has β = 75◦ . Figure 6.5(b) presents the evader ﬁeld for an object whose circumscribed ellipse has β = 80◦ and the original ﬁeld, at the point where the object is placed, has α = −30◦ . In both ﬁgures the exponential fading imposed is shown. This eﬀect assures that the evader ﬁeld only aﬀects the neighborhood of the object. Next, a test of the system with the three modules combined is presented. Figure 6.6 resumes the results obtained. Inserting four arbitrary

(a)

(b)

Fig. 6.6. Modiﬁed velocity ﬁeld for four obstacles detected

78

W. Melina-Mel´endez et al.

objects at arbitrary positions for the velocity ﬁelds shown in Fig. 6.5, the ﬁnal ones obtained oﬀer obstacle-free trajectories, however, at the edges between the inﬂuence “evader” ﬁeld 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 diﬀerent points, and each trajectory converges to the path deﬁned by the velocity ﬁeld generated. It is important to remark that these trajectories are smooth, including the area where the ﬁeld is not. Practical validations were performed using a diﬀerential drive wheeled robot, controlling its orientation at any moment through a PI controller. 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 ﬁeld 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 ﬁelds are smooth in most of the cases. When they are not completely smooth, experimental tests suggest that this eﬀect can be neglected. The proposed algorithm oﬀers 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 eﬃciency of the proposed algorithm 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 ﬁeld 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 Robot Manipulators. In: Proc. IEEE Int. Conf. on Rob. and Aut. ICRA’03, Taipei, Taiwan 10:4374–4379.

6 Vision-Based Velocity Field Generation for Mobile Robots

79

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 ﬁeld 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. Bola˜ nos J, Medina-M. W, Ferm´ın L, Cappelletto J, Fern´ andez-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 Aleny`a and Carme Torras Institut de Rob`otica i Inform`atica 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 ﬁxating on the same target, than it would be possible with ﬁxed 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 ﬂow 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 aﬃne 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 under 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-IST4-27657. The authors thank Gabriel Pi for their contribution in preparing the experiments.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 81–88, 2007. c Springer-Verlag London Limited 2007 springerlink.com

82

G. Aleny`a 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 compensate translation along the optic axis, as addressed in this work. There are a few papers presenting diﬀerent strategies for zoom control. Fayman et al. [4] consider a planar target and robot translations only along the optic axis. In order to keep a constant-sized image projection 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. Tordoﬀ and Murray [12] address also the problem of ﬁxating the target size in the image, but considering general robot motion, and perspective and aﬃne 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 eﬀects are not present or discrete, as common in broadcast or surveillance. We have been investigating the potential of the aﬃne shape representation 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 aﬃne scale and generate zoom demands is introduced. Experimental results are presented in Section 7.5 and ﬁnally 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 aﬃne deformation within the image of the target in the scene. The aﬃnity 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] ﬁtted 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 diﬀerence in terms of control points Q′ − Q that quantiﬁes 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 W=

x y 1 0 Q 0 0 Q , , , , , 0 1 0 Qy Qx 0

(7.2)

and S is a vector with the six coeﬃcients 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 aﬃnity 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 deﬁning the aﬃnity in the plane. The contour is tracked along the image sequence with a Kalman ﬁlter [3] and, for each frame, the shape vector and its associated covariance matrix are updated. Considering a camera that possibly changes the focal length, the aﬃnity coded by the shape vector relates to the 3D camera motion in the following way [10]: fi Z0 R11 R21 , (7.5) M= f0 Z0 + Tz R21 R22 fi Tx u0 − ui t= + , (7.6) v0 − vi Z0 + Tz Ty

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 aﬃnity permits deriving the camera motion in 3D space. In particular, the scaled translation in direction Z is calculated as [10] Tz fi 1 √ − 1, = Z0 f0 λ1

(7.7)

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.

G. Aleny`a and C. Torras

84

7.3 Generating Zoom Demands Image-based 2D methods rely solely on image measurements. The eﬀect 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 eﬀects, this can be explained as multiplying the calibration matrix corresponding to the ﬁrst frame by the factor fi /f0 . Assuming a unit aspect ratio, the scale s of the aﬃnity that relates two views can be recovered from the aﬃne 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 aﬃnity representation we have introduced, we can estimate the current scale of the aﬃne 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 λ1

(7.8)

as the error function in the zoom control algorithm. It is not directly the aﬃne 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 eﬀectively a 2D measure. Changing the focal length values with this error function neither calibration nor estimation of the initial distance Z0 is needed . Furthermore, as noticed by Tordoﬀ and Murray [12], the idea of recovering a property of the overall projection instead of a property of the individual points is an advantadge in noisy conditions. This is just what we attain with the weak-perspective camera model and the introduction of the aﬃne shape space.

2

Compared with [12], just 2D motion is recovered (coded as an aﬃne shape deformation), not structure or 3D reprojection, and no foreground/background extraction is performed, as it comes with the speciﬁcation of the active contour within the tracking algorithm.

7 Zoom Control to Compensate Camera Translation

85

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 implemented 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 beginning 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 continuously correcting camera translations, depths in the acquired images are always very similar (this was just our objective!). Compared with a ﬁxed focal length set-up, the diﬀerence 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 aﬃnity codifying 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 ﬁxed 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

86

G. Aleny`a and C. Torras

(a)

(c)

(e)

(b)

(d)

(f)

Fig. 7.1. Images illustrating the performed experiment. (a) Robot at the initial position and (b) robot at the ﬁnal 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 ﬁtted to it. (e) Image acquired at the ﬁnal position after the zoom has been changed with the proposed zoom control algorithm. (f ) Image acquired at the ﬁnal position without zoom changes.

which the shape vector is calculated. While the robot is moving, for each acquired image the tracking algorithm estimates the aﬃne shape deformation of the current contour with respect to the initial one, and computes the egomotion. At frame rate, in our current implementation at 20 fps, the system is capable of generating a zoom demand to cancel in the image the eﬀects of robot translation. Figure 7.1(e) shows that the zoom control has eﬀectively cancelled the approaching motion. Figure 7.1(f) shows the resulting image if the zoom control is deactivated. 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

7 Zoom Control to Compensate Camera Translation 600

0 −200

−500

Tz

zoom

Tz tracker

0

500

200

400

−1000 300

−400 −600 0

500

600

400

500

1000 frame

(a)

1500

87

200 0

−1500 500

1000 frame

1500

(b)

−2000 0

500

1000 frame

1500

(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 camera 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 ﬁtted 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 aﬃnity 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.

88

G. Aleny`a and C. Torras

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. Aleny`a, 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. Aleny`a, 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 Vision. Cambridge University Press, 2 edition, 2004. 7. J. Koenderink and A. J. van Doorn. Aﬃne 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 aﬃne epipolar geometry. Int. J. Comput. Vision, 16(2):147–182, 1995. 12. B. Tordoﬀ and D. Murray. Reactive control of zoom while ﬁxating using perspective and aﬃne 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 Hoﬀmann Chair of Control and Systems Engineering, Universit¨ at 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 ﬁngers and the object surfaces. Force closure leads to stable grasping such that the total sum of forces acting 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 fundamental 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 contours 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 diﬃcult to predict due to the presence of friction in gripper actuators as well as the nonlinear behavior 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 different perspective as slippage is detected by means of dynamic tactile sensing. This paper presents a vision assisted approach to grasping for unknown object pose. The end eﬀector is ﬁrst visually servoed to a previously demonstrated pre-grasping pose. Afterwards suitable grasp points ∗

Supported by German Research Foundation (DFG) in collaboration with “Computational Intelligence” (SFB 531).

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 89–98, 2007. c Springer-Verlag London Limited 2007 springerlink.com

90

U. Khan, T. Nierobisch, and F. Hoﬀmann

Ȗ

TCP

force sensors

YTCP XTCP

ZBASE YBASE XBASE

ZTCP

ZOBJ YOBJ

proximity sensors

XOBJ

Fig. 8.1. Left) Manipulator Katana 6M with TCP and object coordinate system Right) Standard two-ﬁnger gripper with IR proximity and force sensors

are selected using the proximity sensors and the grasp closure is ﬁnally executed by regulating the force applied upon the object. The experimental evaluation of the two-ﬁnger grasping for vision assisted object manipulation is based upon the Katana 6M manipulator and a two ﬁnger gripper depicted in Fig. 8.1. The camera is mounted to the end eﬀector above the gripper in an eye in hand conﬁguration. The gripper 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 conﬁguration that achieves a four-point contact between the object and the gripper. After attaining the ﬁnal grasp pose the gripper ﬁnger is regulated by using a fuzzy controller in order to grasp the object without slippage or damage. The eﬀectiveness and robustness of the object manipulation scheme is conﬁrmed in an experimental evaluation on a real robot arm with a eye-in-hand conﬁguration and a standard two-ﬁnger 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] oﬀer 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 aﬃne transformations. In addition, SIFT features are uniquely identiﬁable across multiple views of an object, which allows the development of robust model free image based visual servoing schemes [7]. Circumnavigating 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 camera and the object, whereas the orientation φi is equal to the rotation around the camera axis. Therefore the visual moments fγ and fz are deﬁned 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:

fx =

n P

ui

i=1

n

,

fy =

n P

i=1

n

vi ,

fz =

n P

σi

i=1

n

,

fγ =

n P

φi

i=1

n

. (8.1)

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 diﬀerence 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 single SIFT feature is suﬃcient 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 eﬀector is moved forward by a ﬁxed 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.

92

U. Khan, T. Nierobisch, and F. Hoﬀmann

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 selection. Given the dimensions and geometry of the gripper it is not feasible to grasp objects of complex shape, in particular with non-parallel opposite 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 diﬃcult 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 rectangular 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 between the ﬁnger front plane R2 and the ﬁnger 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 ﬁngers 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 perceptions 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

8 Two-Finger Grasping for Vision Assisted Object Manipulation

93

of the training set consists of a randomly generated input and the corresponding correct model output. The training data set is generated from a model of the gripper and various object geometries and the noise aﬄicted 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 motion 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 ﬁnal 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 reﬂected from an object’s surface, which increases with proximity. The gripper ﬁngers are ﬁrst aligned with the object by regulating the gripper orientation γ along the z-axis such that the cumulative proximity sensor measurement L1 +L2 becomes minimum, which corresponds to a parallel orientation between the ﬁnger tips and the object. Once the gripper ﬁngers 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 readings 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 ﬁnger tips are equidistant 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 diﬀerence L1 − L2 and the gripper opening θ. Again, the training data for the ANFIS model is obtained from the simulation model with one thousand randomly generated input conﬁgurations and the corresponding controller outputs. Each input conﬁguration 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 center point the gripper is moved parallel to the object surface until the

94

U. Khan, T. Nierobisch, and F. Hoﬀmann

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 ﬁnally 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 signiﬁcant variations in height this approach will fail. Four-Point Selection : In the ﬁnal phase, the gripper vertical separation 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 ﬁnger sensor readings. Again, the training data for the ANFIS models are generated from a thousand simulated conﬁgurations with noisy readings for objects of diﬀerent 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 displacement 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 nominal gripper geometry for a speciﬁc 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 ) vd =

R2 sin θ − 0.5(L1 + L2 ) cos θ − ∆ tan(θd + δ)

(8.3)

(8.4)

After completion of the object scanning and grasp alignment prior to grasp closure the attained end-eﬀector 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 manipulations 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 suﬃcient to attain the grasp pose. The gripper closure under force control is initiated once the ﬁnger tips are in contact with the object. The objective is to regulate the

8 Two-Finger Grasping for Vision Assisted Object Manipulation

95

force sensor meas. (right)

force sensor meas. (left)

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.

200

150 Cardboard Wood Can

100 48

52 50 gripper opening [°]

54

200

150

100 48

52 50 gripper opening [°]

54

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 ﬁngers, respectively. The measurements are recorded for three diﬀerent objects. This variation in the sensor characteristics is caused by the diﬀerent compressibility and stiﬀness of the object as well as the contact area between the object and the ﬁnger. The contact forces are absorbed by the ﬁnger 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 ﬁngers are closed. The setpoint for the force control fd is fd = 0.5 mg/µ, (8.5) where µ denotes the friction coeﬃcient 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 scenarios is based upon eight objects of diﬀerent color and texture shown in Fig. 8.4. A wide variety of textured objects including transparent and high reﬂectivity objects are included in the test set to obtain a realistic evaluation of the robustness of two-ﬁnger grasping. The two-ﬁnger grasping succeeded without any failure in at least ﬁve independent

96

U. Khan, T. Nierobisch, and F. Hoﬀmann

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 diﬀerent reﬂectivity of black and yellow surfaces on opposite sides. This diﬀerence in reﬂectivity results in an asymmetric gripper alignment despite equal IR sensor readings. Grasping also fails for objects 6 and 7 because of their highly reﬂective and transparent surfaces. This leads to erroneous measurements from the IR proximity sensors, thus causing the grasp point selection step to fail. The two-ﬁnger 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 during 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 suﬃcient 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 measurements are plotted against the left vertical scale, whereas the lateral velocity of the gripper is plotted against the right scale. As the gripper ﬁngers center along the object, the proximity sensor measurements become 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 oﬀset 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 requires approximately seven seconds for servoing to the demonstrated reference pose and an additional ﬁfteen seconds for object scanning and

∆x ∆y ∆z ∆γ

20 0 −20

a

2

0

4 time [s]

2

85

0

80

−2

75

−4

c

0

−6 3

2

1 time [s]

0 ∆f

x

∆f

y

−0.5

∆ fz

−5

1.5

−10

0.75

γ

0 2

4 time [s]

6

0

2.25

∆f

b

0

applied force [N]

image space errors

95 90

3

0.5

−1

100

70

6

10 left sensor meas. 8 right sensor meas. 6 lateral vel. 4

force[N] closing vel.

d

0

−15

0.5 time [s]

1

gripper closing vel. [deg/sec]

40

110 105

97 gripper lateral vel. [mm/sec]

60

IR sensor measurements

task space errors [mm−deg]

8 Two-Finger Grasping for Vision Assisted Object Manipulation

−20

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 between the host PC which runs the visual servoing and the manipulator micro-controllers for axis control.

8.5 Conclusion This paper presents a two-ﬁnger 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 suﬃces 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 oﬀer the advantage of universal applicability to objects of daily use and eliminate the need for depth estimation. In order to achieve a more ﬂexible object manipulation, the current scheme will be augmented with a large view visual servoing controller [10]. Additional intermediate reference views are introduced in order to navigate the manipulator and camera across the entire view hemisphere of an object.

98

U. Khan, T. Nierobisch, and F. Hoﬀmann

References 1. Han HY, Arimoto S, Tahara K, Yamaguchi M, Nguyen PTA (2001) Robotic 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 NotSo-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 Underactuated 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 keypoints. In:Intl. Journal of Computer Vision 60:91-110. 7. Hoﬀmann F, Nierobisch T, Seyﬀarth, T Rudolph G (2006) Visual servoing 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 moments 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 systems. In IEEE Trans. on Systems, Man, and Cybernetics 23 :665-685. 10. Nierobisch T, Krettek J, Khan U, Hoﬀmann 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 uniﬁed approach to a variety of problems in spatial robot perception and navigation [1]. The occupancy ﬁeld 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 consideration 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 ﬁltering 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 robot system perception, which approaches the robot to the ﬁnal desired objective. The local data grid is presented as a horizon for making the trajectory planning. A practical approach for diﬀerential 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.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 99–106, 2007. c Springer-Verlag London Limited 2007 springerlink.com

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 ﬁnal goal approach and trajectory planning. It is assumed that a local occupancy grid data is provided by the on robot perception system, the occupancy probability is divided in only two ranges: free and occupied. Hence, local trajectories along the local grid can approach the robot to the ﬁnal goal while obstacle collisions are considered. The analysis is focused on some particular indoor environment with ﬂat ﬂoor surface; however it can be applied in outdoor environments too. The problem is formulated as ﬁnding the optimal cell that approaches the WMR to the desired coordinates (Xd , Yd ) by ﬁnding 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 ﬁeld 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

if atan(Yd /Xd ) = θd > θlg J = min[(θd −atan(Y /X))2 ]1/2

(9.1)

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 limitation of the input signal, U (Xld , Yld ), is taken into account in the ﬁrst constraint as a function of the local desired points and the WMR dynamics. 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 conﬁguration space Xcs can be obtained for each obstacle row by using:

9 Trajectory Planning with Control Horizon

kXo − Xcs k = W P/ sin(π/2 − atan(Ycs /Xcs )),

101

(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 conﬁguration 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 conﬁguration space. • To apply (9.1) to the whole local occupancy grid when no obstacle exists.

(a)

(b)

Fig. 9.1. (a) Two diﬀerent 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 ﬂow, or accommodation as meaningful clues for environment understanding [6]. This section presents the use of local

102

L. Pacheco and N. Luo

visual data and odometer information for the WMR control. It is assumed that a feasible local occupancy grid based on the visual information 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 concerning the narrow ﬁeld 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 assuming projective perspective. In the case above, ﬂat ﬂoor is assumed; hence free cells belong to a 2D space. Figure 9.2(a) shows the camera conﬁguration studied in this work, and the angles of the vertical and horizontal ﬁeld of view and the tilt camera pose, respectively. The vertical coordinate of the camera is represented by H. By using trigonometric relationships, the scene coordinates can be computed [7]. Figure 9.2(b) shows a local map provided by the camera conﬁguration shown in Fig. 9.2(a). The scene grids with low resolution can be used in order 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 ﬁeld of view and the ﬁxed camera conﬁguration make it necessary that the robot orients towards the desired coordinates and moves in advancing sense. A reactive zone should be considered accordingly 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 bigger 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 available grid shown in Fig. 9.3(b). Reactive criteria are related with the kind of obstacles, static or mobile, the robot dynamics and the perception speed. Thus, if static obstacles are assumed, the robot trajectory should be large enough to allow a safety stop distance, which can be

9 Trajectory Planning with Control Horizon

(a)

103

(b)

Fig. 9.2. (a) Fixed camera conﬁguration 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 obstacles 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 ﬁeld methods is proposed. The knowledge of experimental WMR dynamic models allows eﬃcient 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 diﬀerential driven WMR can be obtained through the approach of a set of linear transfer functions that include the nonlinearities of the whole system. The parametric identiﬁcation process is based on black box models [12]. The approach of multiple transfer functions consists in making the experiments with three diﬀerent (slow, medium, and fast) speeds. The parameter estimation is done by using

104

L. Pacheco and N. Luo

(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 inﬂuence between two DC motors. However, the coupling eﬀects can be neglected if they are less than 20%. The obtained results reveal diﬀerent wheel gains as well as the existence of dominant poles. Hence, the models can be reduced to ﬁrst-order SISO systems. The speed control is performed by using PID controllers. A two-dimensional array with three diﬀerent 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 usually diﬃcult 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 diﬀerent linear models can be used as a potential function of the Euclidean distance between the robot and the trajectory. Such functions are CLF, and consequently asymptotic stability with respect to the desired trajectory 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

9 Trajectory Planning with Control Horizon

105

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 minimize 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 diﬀerent models can provide information about the dynamics 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 trajectory 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 diﬀerent robot models

Model

Low speed (0.15 m/s) Medium Speed (0.30 m/s) High speed (0.45 m/s)

Min. allowable Robot Safety stop Obstacle reactive distances displacement distances distances 0.248 m 0.038 m 0.125 m 0.085 m

0.178 m

0.125 m

0.075 m

0.378 m

0.31 m

0.125 m

0.113 m

0.548 m

9.4 Conclusions The present research allows a methodology based on the local occupancy grid knowledge. Using this information a simple set of algorithms is presented that allows trajectory planning and obstacle avoidance as well as ﬁnal 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 positions. The system dynamics knowledge is an important clue when safety and control aspects are considered. Nowadays, the research is focused on analyzing the performance of the presented methodology by using both real and virtual visual data. Further improvements can be

106

L. Pacheco and N. Luo

achieved by use of feasible maps or landmarks that provide local objective coordinates that guide the WMR to reach the ﬁnal 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 Artiﬁcial Intelligence 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 Perception 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 International 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 Evaluating 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 Identiﬁcation for Linear Systems: Theory, Implementation and Applications, Ed. Kluwer Academic. 13. LL. Pacheco, N. Luo, R. Arbuse (2006) Experimental Modeling and Control Strategies on an Open Mobile Robot Platform. IEEE-TTTC AQTR06 14. R.W. Brockett (1983), Asymptotic stability and feedback stabilization, In: Diﬀerential 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. Paciﬁc Journal of Mathematics, 145:367-379. 17. A. Astolﬁ (1996), Discontinuous Control of Nonholonomic Systems. Systems 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 [email protected], [email protected]

10.1 Introduction Passivity-based control of port-Hamiltonian (PH) systems has been investigated 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 framework [2, 3]. This control method is practicable since it is expected that passivity will provide the robust stable closed loop [4, 5]. However, addressing the subject of underactuated manipulator, there is only an application to Acrobot [6]. Underactuated manipulators are generally second-order nonholonomic systems [7]. Most of them have diﬃculty on feedback control as stated in the Brockett’s theorem [8]. Because underactuated manipulators 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 Passivity Based Control (IDA-PBC) to construct a control system for the 2Ra -Ru planar manipulator whose ﬁrst 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 K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 109–118, 2007. c Springer-Verlag London Limited 2007 springerlink.com

110

M. Ito and N. Toda

IDA-PBC and derive a concrete control law. These results are summarized into Proposition 2. Numerical experiments verify the eﬀectiveness 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 G(q) ∇pH ⊤ △

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 deﬁnite 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 On×n M −1 Md q˙ ∇q Hd⊤ = , (10.3) p˙ −Md M −1 J2 − GKv G⊤ ∇pHd⊤ where Kv is a positive deﬁnite matrix, Md (q) ∈ ℜn×n is positive deﬁnite in the neighborhood of q ∗ and Vd satisﬁes q ∗ = arg min Vd (q) such that the set of partial diﬀerential 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) n×n . G⊥ ∈ ℜ(n−m)×n is a full holds for some J2 (q, p) = −J⊤ 2 (q, p) ∈ ℜ 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 underactuation 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 skewsymmetric matrix J2 in the form   0 p˜⊤ α1 p˜⊤ α2 ··· p˜⊤ αn−1  −p˜⊤ α1 0 p˜⊤ αn ··· p˜⊤ α2n−3  no   X ..   ··· ··· 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 deﬁned as follows: △

W1 = W12 , . . . , Wn−1 = W1n , Wn = W23 , . . . , Wno = W(n−1)n , Wkℓ = Fkℓ − (Fkℓ )⊤ , △

fijkℓ =

Fkℓ = {fijkℓ },

1, if j > i, i = k and j = ℓ 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 deﬁned as follows are functions of qr only, with qr as in A3. γ = [γ1 , · · · , γn ]⊤ = M −1 Md (G⊥ )⊤ ∈ ℜn , △

△

⊥

s = G ∇q V

A5. A6. A7.

⊤

(10.6) (10.7)

γr (qr∗ ) 6= 0, where qr∗ is a single element of q ∗ with r as in A3. ds (qr∗ ) > 0 with qr∗ in A5. γr (qr∗ ) dq r ⊤ −1 G M er 6= 0 with qr∗ in A5. qr =q ∗ r

Proposition 1. (Acosta et al. [3]) Consider the system (10.1) that satisﬁed A1–A3. Assume that there exist matrices Ψ and Md0 such that A4–A6 hold with all desired (locally) positive deﬁnite inertia matrices Z qr Md (qr ) = G(µ)Ψ(µ)G⊤ (µ)dµ + Md0 . (10.8) qr∗

Under these conditions, with

112

M. Ito and N. Toda Y of mass : center of each link

ℓ3

of percussion : center of 3rd link

Acceleration of gravity

K

g0

d3

φ3

θ

ay

y

ax 3rd revolute joint

φ2

2nd revolute joint

(unactuated)

(actuated)

1st revolute joint (actuated)

d2

φ1

O

ℓ2

x

d1

X

ℓ1

Fig. 10.1. A 2Ra -Ru planar manipulator

Vd (q) =

Z

qr 0

s(µ) dµ + Φ(z(q)) γr (µ)

(10.9)

for all diﬀerentiable 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 deﬁned as Z qr γ(µ) △ dµ. (10.11) z(q) = q − γr (µ) 0

Moreover, [q∗ , 0] is a locally stable equilibrium with the Lyapunov function Hd (q, p) provided that the root qr = qr∗ of s(qr ) is isolated, the function z(q) satisﬁes z(q∗ ) = arg min Φ(z) and this minimum is isolated. 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.

10 Control for a Three-Joint Underactuated Planar Manipulator

113

△

Let q = [q1 , q2 , q3 ]⊤ = [x, y, θ]⊤ be generalized coordinates. By deriving Lagrange’s equation of motion and applying a global input transformation [10] to it to partially linearize it with respect to (x, y), we obtain the following state equation d q q˙ O3×2 ax + , (10.12) = g0 ˙ G(q ) ay q cos q e − dt 3 3 3 K △

where K = (m3 d23 + I3 )/(m3 d3 ) and " # 1 0 △ 0 1 G(q3 ) = . (1/K) sin q3 −(1/K) cos q3 △

△

Let p = [p1 , p2 , p3 ]⊤ = q˙ and u = [ax , ay ]⊤ ; the dynamics is obtained 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⊤ ∀(q1∗ , q2∗ ) ∈ D, e ] = [ q1 , q2 , ±π/2, 0, 0, 0 ], 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 ﬁrst 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 satisﬁed. The value of the condition in A6 is zero in the case of g0 = 0, while in the case of 0 < g0 6 g   = 0, q3∗ = 0, ±π ds ∗ 0 ∗ ∗ (q ) = γ3 g0 sin q3 < 0, −π < q3∗ < 0 γ3 (q3 )  dq3 3 > 0, 0 < q3∗ < π

114

M. Ito and N. Toda

for q3∗ ∈ [−π, π]. Hence, all Assumptions are satisﬁed 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 conﬁgurations 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 definite 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

1 g0 ∗ sin q3 + {z(q) − z(q∗ )}⊤ S⊤ 3 PS3 {z(q) − z(q )}, k1 − k2 K 2 (10.15) which satisﬁes q∗ = arg min Vd (q) for all positive deﬁnite 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   3 q + 3 cos q − 5) + k (cos q − 1) 1K q1 − 3(k1k−k (2 cos 3 3 3 3 2 K)   1K z(q) =  q2 − 3(k2k sin3 q3 + k3 sin q3 . 1 −k2 K) 0 Vd (q) = −

Moreover, the IDA-PBC law 1 dMd −1 ⊤ −1 ⊤ u = (G G) G ∇q3 V e3 + p⊤ Md−1 Md pMd e3 2 dq3 ⊤ −1 − Md ∇q Vd + J2 Md p − Kv G⊤ Md−1 p ensures local asymptotic stability of the desired equilibrium [q1∗ , q2∗ , π/2, 0, 0, 0] for all q1∗ , q2∗ .

⊓ ⊔

10 Control for a Three-Joint Underactuated Planar Manipulator

3

Velocities: p

Positions: q

3.5

2.5 2 1.5 1

0

10

30 20 Time: t (s)

40

115

0.3 0.25 0.2 0.15 0.1 0.05 0 -0.05 -0.1

0

50

10

30 20 Time: t (s)

40

50

Constraint force: τ3 (Nm)

Control torques: τ (Nm)

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) 200 150 100 50 0 -50

0

10

30 20 Time: t (s)

40

50

Fig. 10.3. Joint torques for the case of transfer between stabilizable equilibria; τ1 (solid line), τ2 (broken line)

1.2

× 10-16

0.8

0.4

0

-0.4

-0.8

-1.2

0

10

30 20 Time: t (s)

40

50

Fig. 10.4. Constraint force for the case of transfer between stabilizable 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 ﬁxed to γ30 = 0.68, k1 = 2.0, k2 = 1.65, k3 = 6.5, 0.15 0 9.5 1 and Kv = . P= 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 asymptotically to the desired equilibrium. The inverse dynamics yields the

M. Ito and N. Toda 4

5 4 3 2 1 0 -1 -2 -3

2 Velocities: p

Positions: q

116

0 -2 -4 -6 -8

0

10

30 20 Time: t (s)

40

0

50

10

20

30

40

50

Time: t (s)

Constraint force: τ3 (Nm)

Control torques: τ (Nm)

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) 200

100

0

-100

-200

-300 0

10

20

30

40

50

Time: t (s)

Fig. 10.6. Joint torques for the case of the swing-up maneuver; τ1 (solid line), τ2 (broken line)

1.2

× 10-15

0.8 0.4 0 -0.4 -0.8 -1.2 0

10

20

30

40

50

Time: t (s)

Fig. 10.7. Constraint force for the case of the swing-up maneuver

control torques at the ﬁrst and second joints and the constraint force at the third joint. We can see that the second-order nonholonomic constraint is suﬃciently satisﬁed 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 suﬃciently satisﬁed.

10.5 Conclusions In this paper we have applied IDA-PBC to a 2Ra -Ru planar manipulator, which is a second-order nonholonomic system, and derived an eﬀective control law for this system in the presence of gravity. We have shown that stabilizable equilibria are conﬁgurations 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 eﬀectiveness of the derived control law.

10 Control for a Three-Joint Underactuated Planar Manipulator

117

The domain of attraction of the derived control law and the design 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 degree 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 interconnection and damping assignment control of mechanical systems: the acrobot 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) Diﬀerential Geometric Control Theory. Birkhauser, Boston 9. Ortega R, et al.(2002) Stabilization of underactuated mechanical systems via interconnection and damping assignment. IEEE Trans Automatic 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, Universit`a ”La Sapienza” di Roma, Italy 12. Arai H, et al.(1998) Nonholonomic control of a three-DOF planar underactuated 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, Pozna´ n University of Technology, ul. Piotrowo 3a, 60-965 Pozna´ n, Poland {piotr.sauer,krzysztof.kozlowski}@put.poznan.pl

11.1 Introduction The new industrial robot is equipped with gear-boxes such as harmonic drives, which introduce elastic deformations at the joints. These deformations are regarded a source of problems, especially when accurate trajectory tracking of high sensitivity to end–eﬀector forces is mandatory. It is shown that the joint elasticity should be taken into account in modeling a robotic manipulator and designing a control algorithm. 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 diﬃcult 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 algorithm are described in Section 11.2. The concluding remarks are given in Section 11.3. K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 119–136, 2007. c Springer-Verlag London Limited 2007 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 freedom, 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 positive deﬁnite 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 deﬁnite constant diagonal actuator inertia matrix. K is an N × N diagonal matrix of the torsional stiﬀness. 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 deﬁne tracking error as: q˜l = qld − ql ,

q˜m = qmd − qm ,

lim q˜l = lim (qld − ql ) ,

t→∞

t→∞

(11.3) (11.4)

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 ﬁnd 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 control algorithm is proposed in the following form: bm q¨md + Fˆm + µ b µ b −1 K( b −1 qmd − qld ) + Kpm q˜m + Kdm v˜m . u=J (11.5)

We deﬁne signal qmd as b −1 µ c l (ql )q¨ld + C b l (ql , q˙ld )q˙ld + gˆl (ql ) + Fˆl (q˙ld ) + Kp q˜l + b M qmd = K and

b qld , Kd v˜l ) + µ

(11.6)

˜˙ l = −Al v˜l + Bl q˜˙ l , v

˜˙ m = −Am v˜m + Bm q˜˙ m , v

(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 ﬂexibility (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 b l (ql , q˙ld )q˙ld + gˆl (ql ) + Fˆ (q˙ld ) + Kpl q˜l + Kdl v˜l . τl = D (11.9)

The control law, which is presented by (11.9), takes advantage of position measurement only. A velocity error is obtained by the ﬁltered position error given by the ﬁrst equation in (11.7). In Eq. (11.9) new symbols are used: • Kpl and Kdl are positive – deﬁnite matrices, which include parameters of a P D controller.

122

P. Sauer and K. Kozlowski

• 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 b l , gˆl and Fˆ are the estimates of Dl , Cl , gl , and F , respectively. • D 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 considered 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 )˜ al + Kpl q˜l + + Kdl v˜l = 0, (11.10) in which Cd = Cl (ql , q˙ld ) and ˜l = a b l − al , a

(11.11)

where al is an m–dimensional vector containing the unknown dynamib l denotes the parameter cal manipulator and actuator parameters and a ˜ l is bounded). estimation vector (it is assumed that a Further, since the matrices Dl , Cl and vectors gl , F are linear in terms of the manipulator and actuator parameters one can write b l (ql )−Dl (ql ) q¨ld + C b l (ql , q˙ld )−Cl (ql , q˙ld ) · Yl (ql , q˙ld , q¨ld )˜ al = D · 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 ﬂexibility. 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 deﬁne a Lyapunov function candidate which satisﬁes conditions described in ˜˙ l (0), v˜l (0), Theorem A1 (see Appendix) with initial conditions q˜l (0), q ˜ l (0), which guarantees that the state of the system represented and a ˜ l (t) is uniformly bounded and converges by q˜l (t), q˜˙ l (t), v˜l (t), and a exponentially to closed balls Br1 , Br2 deﬁned as follows: q 2 2 2 ˙ ˙ vl (t)k 6 r1 Br1 = q˜l (t), q˜l (t), v˜l (t) : kq˜l (t)k + kq˜l (t)k + k˜

11 A New Control Algorithm for Manipulators with Joint Flexibility

and

123

o n p ˜ l (t) : k˜ al (t)k2 6 r2 , Br2 = a

in which the adaptation law has the following form. ˜˙ l = −ηˆ a al + εΓl−1 Y Tl (˜ vl + q˜l ),

(11.13)

where η > 0. The Lyapunov function is deﬁned by (11.14).

1 1 1 ˜l + εq˜lT Dl q˜˙ l + V (q˜l , q˜˙ l , v˜l , t) = q˜˙ lT Dl q˜˙ l + q˜lT Kpl q˜l + v˜l Kdl B−1 l v 2 2 2 1 T ˜ l, ˜ Γl a (11.14) + ε˜ vlT Dl q˜˙ l + a 2 l

where Γ is a symmetric positive–deﬁnite matrix and ε is a constant value coeﬃcient (ε > 0). Note that the Lyapunov function candidate deﬁned by (11.14) diﬀers from that deﬁned by Loria and Ortega [9]. Obviously V (q˜l , q˜˙ l , v˜l , t) is trivially a positive deﬁnite matrix. Next we calculate the ball radiuses according to the assumptions presented in Theorem A1 and the Lyapunov function candidate satisﬁes the following inequalities:

1 1 al k2 , al k2 6 V (q˜l , q˜˙ l , v˜l , t) 6 z T Q2 z + λM (Γl )k˜ z T Q1 z + λm (Γl )k˜ 2 2 (11.15) T T T T ˙ where z = q˜ q , and matrices Q1 and Q2 are as follows: ˜ v˜   ε 1 0 λm (Kpl ) λm (Dl )  2 2   ε 1 ε   (11.16) Q1 =  λm (Dl ) λm (Dl ) , λm (Dl )  2 2 2   1 ε λm (Dl ) λm (Kdl )λ−1 0 M (Bl ) 2 2   ε 1 0 λ (K ) λ (D )   2 M pl 2 M l   ε 1  ε (11.17) Q2 =  λM (Dl ) λM (Dl ) . λM (Dl )  2 2 2   1 ε λM (Dl ) λM (Kdl )λ−1 0 m (Bl ) 2 2 These matrices are positive deﬁnite, namely ε has to fulﬁl the following conditions:

124

P. Sauer and K. Kozlowski

ε<

ε<

ε<

ε<

s

s

s

s

λm (Kpl ) , λm (Dl )

λm (Kpl )λm (Kdl )λ−1 M (Bl ) , λm (Dl ) λm (Kpl ) + λm (Kdl )λ−1 M (Bl )

λM (Kpl ) , λM (Dl )

λM (Kpl )λM (Kdl )λ−1 m (Bl ) . λM (Dl ) λm (Kpl ) + λm (Kdl )λ−1 m (Bl )

(11.18)

Taking into account the above conditions the Lyapunov function (11.14) satisﬁes the following inequality: 1 λm (Q1 )kzk2 + λm (Γl )kal k2 6 V (q˜l , q˜˙ l , v˜l , t) 6 2 6 λM (Q2 )kz||2 +

1 λM (Γl )kal k2 . 2 (11.19)

˙ l (ql )− Taking into account the skew symmetric property of the matrix D 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: vlT F (q˙l )+ V˙ = q˜˙ lT F (q˙l ) − q˜˙ lT F (q˙ld ) + εq˜lT F (q˙l ) − εq˜lT F (q˙ld ) + ε˜ ˜˙ l − εq˜lT Y l a ˜ Tl Γl a ˜ l − ε˜ ˜ l − q˜˙ lT Yl a ˜ l+ vlT Yl a − ε˜ vlT F (q˙ld ) + a ˜˙ lT Cd q˜˙ l + εq˜˙ lT Dl q˜˙ l + εq˜lT CTd − Cd q ˜l − q ˜˙ l + − v˜lT Kdl B−1 l Al v vlT ATl Dl q˜˙ l + + εq˜lT CTl − CTd q˜˙ l − εq˜lT Kpl q˜l − εq˜lT Kdl v˜l − ε˜ ˜˙ l + ε˜ vlT CTl − CTd q˜˙ l + ε˜ vlT CTd − Cd q˜˙ l + + εq˜˙ l BTl Dl q vlT Kdl v˜l , − ε˜ vlT Kpl q˜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 coeﬃcients associated with the links, respectively.

11 A New Control Algorithm for Manipulators with Joint Flexibility

125

First we establish the following bounds for the terms associated with the friction coeﬃcients 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, vl k + ελM (Fv )k˜ vl kkq˜˙ l k, (11.22) ε˜ vlT F (q˙l ) − εq˜l F (q˙ld ) 6 2ελM (Fc )k˜ where λm (·) and λM (·) denote the smallest and the maximum eigenvalue 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 ˜l 6 −λm (Kdl )λ−1 −˜ vlT Kdl B−1 vl k2 , M (Bl )λm (Al )k˜ l Al v −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

d T −εq˜l Kpl q˜l −εq˜lT Kdl v˜l −ε˜ vlT ATl Dl q˜˙ l εq˜˙ lT BTl Dl q˜˙ l −ε˜ vlT CTd − Cd q˜˙ l

(11.23)

l

6 −ελm (Kpl )kq˜l k2 ,

6 ελM (Kdl )k˜ vl kkq˜l k,

6 ελM (Al )λM (Dl )k˜ vl kkq˜˙ l k,

˜˙ l k2 , 6 ελM (Bl )λM (Dl )kq 6 2εkC Bd k˜ vl kkq˜˙ l k,

−ε˜ vlT Kpl q˜l 6 ελM (Kpl )k˜ vl kkq˜l k,

−ε˜ vlT

vl k2 , −ε˜ vlT Kdl v˜l 6 −ελm (Kdl )k˜ ˜˙ l 6 εkC kq˜˙ l k2 k˜ CTl − CTd q vl k,

˙ and kC > 0. ˙ 6 kC kqk where C(q, q) 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 ε k˜ ε kq˜l k T vl k k˜ vl k kq˜l k Q + − Q3 V˙ (t) 6 − 4 vl k k˜ vl k kq˜˙ l k 2 kq˜˙ l k 2 k˜ T ε kq˜l k kq˜l k − λ1 + ελ2 kq˜˙ l k2 − λ3 k˜ Q5 ˙ vl k2 + − kq˜l k 2 kq˜˙ l k

P. Sauer and K. Kozlowski

126

˜ Tl Γl ηˆ + εkC (k˜ vl k + kq˜l k) kq˜˙ l ||2 − a al + T ˜ l + 2ελM (Fc ) (kq˜l k + k˜ − q˜˙ l Yl a vl k) + 2λM (Fc )kq˜˙ l k+ ε ε vl k2 , (11.24) − (1 − ρ)λm (Fv )kq˜˙ l k2 − λm (Kpl )kq˜l k2 − λm (Kdl )k˜ 2 2

where

 1 λm (Kpl ) −λM (Kpl ) − λM (Kdl )   2 Q3 =  , 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 ) β

 −2kC Bd − λM (Fv ) 2αkC Bd  , 1 Q5 =  −2kC Bd − λM (Fv ), λM (Bl )λM (Dl ) − ε β 

1 2 λm (Kpl )

β+2 λ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 deﬁniteness of matrix Q3 results in an upper bound for ε (recall that ε > 0): λ1 = −

ε<

λm (Kpl )λm (Kdl )λm (Al )

4λM (Bl ) (λM (Kpl ) + λM (Kdl ))2

.

(11.25)

For matrix Q4 the positive deﬁniteness condition can be written in the following form: ε<

λm (Kdl )λm (Bl )λm (Al )λM (Dl ) . 2β (2kC Bd + λM (Al )λM (Dl ) + λM (Fv ))2

(11.26)

11 A New Control Algorithm for Manipulators with Joint Flexibility

127

The condition for λm (Kpl ) can be calculated from the condition for the positive deﬁniteness of matrix Q5 λm (Kpl ) >

2βε (2kC Bd + λM (Fv ))2 , ελM (Bl )λM (Dl ) − 2αβkC Bd

(11.27)

assuming that the following condition is true: ε>

2αβkC Bd . λM (Bl )λM (Dl )

(11.28)

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. Deﬁning 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: ˜˙ lT Y l a ˆl − q ˜ l+ vl k + kq˜l k) kq˜˙ l k2 − η˜ aTl Γl a V˙ (t) 6 −z T Qz + εkC (k˜ ε ε vl k2 + 2ελM (Fc ) (kq˜l k + k˜ vl k) + − λm (Kpl )kq˜l k2 − λm (Kdl )k˜ 2 2 (11.32) 2λM (Fc )kq˜˙ l k − (1 − ρ)λm (Fv )kq˜˙ l k2 .

To this end we include a bound on regression function Yl . Yl is a rectangular matrix (Y l ∈ Rm×N ). In order to ﬁnd a norm of this function, the Frobenius matrix norm is suggested. In this paper we assume a manipulator with revolute joint only to assure a ﬁnite value of the Frobenius 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: ˜ l 6 kY l kF kq˜˙ l kk˜ −q˜˙ lT Y l a al k = C1 kq˜˙ l kk˜ al k.

(11.33)

P. Sauer and K. Kozlowski

128

Taking into account the estimation error of parameters and inequality ˜ l in the following (11.33), we can write the expression −˜ aTl Γl ηˆ al −q˜˙ lT Yl a form: ˆ l − q˜˙ lT Y l a ˜l 6 −η˜ aTl Γl a

1 C12 al k2 + ε1 . (11.34) kq˜˙ l k2 − ηλm (Γl )k˜ 4 ηλm (Γl )

Assuming that the norm of the vector al is bounded by a positive constant a ¯ (kal k 6 a ¯) one can get: ε1 =

1 ηλ2M (Γl ) k¯ ak2 > 0. 2 λm (Γl )

The time derivative of the Lyapunov function can be written as 1 al k2 + ε1 + εkC kq˜l kkq˜˙ l k2 + V˙ (t) 6 −z T Q′ z − ηλm (Γl )k˜ 4 ε (11.35) vl kkq˜˙ l k2 − λm (Kpl )kq˜l k2 + + εkC k˜ 2 ε vl ||2 + 2ελM (Fc ) (kq˜l k + k˜ vl k) , − λm (Kdl )k˜ 2

where



ε λm (Kpl ) 2 ε − λ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 )  . 2 2 λm (Kdl )λ−1 M (Bl )λm (Al )

  ′ Q = 

(11.36)

Matrices Q and Q′ are built based on matices Q3 , Q4 , and Q5 assuming that we omit expressions with kq˜˙ l k2 and k˜ vl k2 because they are negative taking into account conditions (11.29)-(11.31). The positive deﬁ-

11 A New Control Algorithm for Manipulators with Joint Flexibility

129

niteness of matrix Q is guaranteed by conditions (11.25)-(11.28). The positive deﬁniteness of matrix Q′ is guaranteed by conditions (11.25) and (11.26). Next, we do some additional calculations: ε ε vl k2 + vl k) − λm (Kpl )kq˜l k2 − λm (Kdl )k˜ 2ελM (Fc ) (kq˜l k + k˜ 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 al k2 +ε1 +ε2 , vl k + kq˜l k) kq˜˙ l k2 − ηλm (Γl )k˜ V˙ (t) 6 −z T Q′ z + εkC (k˜ 4 (11.38)

The parameter ε2 is written in the following form: ε2 = 2ελ2M (Fc )

λm (Kpl ) + λm (Kdl ) . λm (Kpl )λm (Kdl )

(11.39)

The second element of (11.38) can be bounded as follows: vl k + kq˜l k) kq˜˙ l k2 6 εkC kq˜˙ l kkzk2 . εkC (k˜

(11.40)

We show that the Lyapunov function satisﬁes the following inequality: V (t) > ξ1 kq˜˙ k2 .

(11.41)

To this end, we calculate the bound on the Lyapunov function:

1 1 1 vl k2 + V (t) > λm (Dl )kq˜˙ l k2 + λm (Kpl )kq˜l k2 + λm (Kdl )λM (Bl )k˜ 2 2 2

1 al k2 , vl kkq˜˙ l k + λm (Γl )k˜ − ελm (Dl )kq˜l kkq˜˙ l k − ελM (Dl )k˜ 2

where we can write the following conditions for (11.42):

(11.42)

130

P. Sauer and K. Kozlowski

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 ) kql k − kql kε − kq˜l k , 2 λm (Kpl ) 2 λm (Kpl ) (11.43)

•

•

1 1 λm (Kdl )λM (Bl )k˜ vl k2 − ελm (Dl )k˜ vl kkq˜˙ l || = λm (Kdl )λM (Bl )· 2 2

2 ε2 λ2m (Dl ) ελm (Dl ) ˙ kq˜˙ l ||2 . kq˜l k − · k˜ vl k − 2λm (Kdl )λM (Bl ) λm (Kdl )λM (Bl ) (11.44)

Finally, the Lyapunov function can be written in the following form: V (t) >

ε2 λ2m (Dl ) ε2 λ2m (Dl ) 1 − λm (Dl ) − 2λm (Kpl ) 2λm (Kdl )λM (Bl ) 2

kq˜˙ l k.

(11.45)

If the condition for ε, derived from Eq. (11.45), ε2 <

λm (Kpl )λm (Kdl )λM (Bl ) , λm (Dl ) (λm (Kpl ) + λm (Kdl )λM (Bl ))

(11.46)

is carried out, then we can deﬁne parameter ξ1 as ξ1 =

s

ε2 λ2m (Dl ) λm (Dl ) ε2 λ2m (Dl ) . − − 2λm (Kpl ) 2λm (Kdl )λM (Bl ) 2

(11.47)

The parameter ξ1 is always positive. Taking this into consideration based on (11.41), we can write √ V ˙ kq˜k 6 √ . ξ1

(11.48)

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 √ ! 1 V ′ al k2 + ε3 . (11.49) kzk2 − ηλm (Γl )k˜ V˙ (t) 6 − λm (Q ) − εkC √ 4 ξ1

11 A New Control Algorithm for Manipulators with Joint Flexibility

131

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 deﬁne the following variables: λ1 = λm (Q1 ), λ3 = λM (Q2 ), √ V λ5 = λm (Q ) − εkC √ , ξ1 ′

1 λ2 = λm (Γl ), 2 1 λ4 = λM (Γl ), 2 1 λ6 = ηλm (Γl ), 4

(11.50)

and ε3 = ε1 + ε2 =

·

ηλ2M (Γl ) 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 coeﬃcient 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 deﬁne 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 suﬃciently 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: ) ( 2λM (Γl ) λM (Q2 ) p (11.53) , σ1 = max λm (Q) − εkC V /ξ ηλm (Γl )

132

P. Sauer and K. Kozlowski

and the radiuses of the balls are: r1 =

r

ε3 σ1 , λm (Q1 )

r2 =

s

2ε3 σ1 . λm (Γl )

(11.54)

Based on Theorem A1 and deﬁnitions (11.53) and (11.54) we can aﬃrm ˜ for all time are bounded and exponentially that the coordinates z and a 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 suﬃciently 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˜˙ = −A˜ v + Bq

(11.56)

where T T , T T , ˜m q˜ = q˜lT q˜m v˜ = v˜lT v Ml 0 Kdl 0 M= , Kd = , 0 Jm 0 Kdm C(ql , q˙l ) 0 B1 0 , , C1 = B= 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

11 A New Control Algorithm for Manipulators with Joint Flexibility

133

in force for matrices M, C, and Cd . The matrix Kp is positive–deﬁnite if Kpl and Kpm are positive–deﬁnite. Equation (11.55) includes two control laws: • the ﬁrst 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 1 1 ˙T ˜˙ + ˜ Mq˜˙ T + q˜T Kp q˜ + v˜T Kd B −1 v˜ + εq˜T D q q 2 2 2 1 T ˜. ˜ Γa ˜˙ + a (11.57) + ε˜ vT Dq 2

V (t) =

This function is similar to the Lyapunov function for the rigid manipulator (11.14). The Lyapunov function (11.57) can be calculated similarly as the Lyapunov function for rigid manipulators, but we must use the additional adaptation law for the motor parameters in the following form: −1 T ˜˙ m = −ηˆ a am + εΓm Y m (˜ vm + q˜m ),

(11.58)

where Γm is a positive–deﬁnite matrix. The adaptive control law is of the form of (11.5) and the desired signal qmd is deﬁned by (11.6) and (11.7). The convergence is as in Theorem 1.

11.3 Concluding Remarks In this study we presented a new adaptive tracking control. This control 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 Systems with Bounded Controls, Proceedings of the American Control Conference, pp. 2212–2216, Philadelphia, June 1998. 2. R. Colbaugh, K. Glass, E. Barany, Adaptive Output Stabilization of Manipulators, 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 Research, 1987, Vol. 16, no. 5, pp. 703–713.

134

P. Sauer and K. Kozlowski

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 Eﬃcacy 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 ﬂexible 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, Englewood Cliﬀs, 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)

11 A New Control Algorithm for Manipulators with Joint Flexibility

V˙ 6 −λ5 kx1 k2 − λ6 kx2 k2 + ε,

135

(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 2

IT Engin., Provenia Group, Mantova, Italy [email protected] 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 subject of motion control of rigid robot manipulators. Many approaches have been proposed, such as feedback linearization [1], model predictive 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 ﬁrst 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 diﬃcult 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 ﬂexibility, frictions, sensor noise, and unknown loads. This is the reason why the inverse dynamics approach is often coupled with robust control methodologies [1]. Among these, a possibility is oﬀered by sliding mode control [8], [9] which is robust versus a signiﬁcant 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, deteriorating the controlled system. Such a phenomenon is increased due to the fact that the sliding mode controller is designed by means of a K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 137–146, 2007. c Springer-Verlag London Limited 2007 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 discretetime behavior of the plant. In this way, the control signal is constant during the sampling interval, resulting in ﬁnite-time oscillations of the controlled variable [10]. In order to circumvent the chattering problem, many approaches have been followed. In the case of ﬁrst-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 chattering eﬀect can be made less critical by conﬁning 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 ﬁrst-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 strategy 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 veriﬁed 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 identiﬁcation procedure, recently devised [16], has been applied. Experimental tests demonstrate the eﬃciency and the appreciable tracking performances of the presented inverse dynamicsbased 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 n×n ℜ represents the centripetal and Coriolis torques, Fv ∈ ℜ 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 displacements 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 speciﬁcally, 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 identiﬁed, it can be quite diﬀerent from the real one because of uncertainties and unmodelled dynamics, friction, elasticity, and joint plays. Now assume that the term n ˆ (q, q) ˙ considers the identiﬁed 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

140

A. Calanca et al.

for each joint, with q¨T = [¨ q1 , . . . , 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(−¨ qd + 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 rewritten 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 ∗ (−¨ qdk + yk − fk ), where, chosen a sampling time interval δ, Z δ G∗ = eGδ , H ∗ = eG(δ−τ ) Hdτ

(12.6)

(12.7)

0

and f (t) is assumed to be constant within the sampling interval. The concept of discrete-time sliding mode was deﬁned 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 manifold 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 deﬁnition 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 ∗ (−¨ qdk + yk − fk )) = 0,

(12.8)

12 Discrete-Time Sliding Mode Controller for Robot Manipulators

that is

yk = −(T H ∗ )−1 (T G∗ xk − T H ∗ fk − T H ∗ q¨dk )

141

(12.9)

From (12.9) it is apparent that yk tends to inﬁnity 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 ﬁnite number of steps instead of after a single step. To this end it is possible to deﬁne 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 (12.10) yk = y yk , | y |> y 0 |yk |

k

0

the necessary and suﬃcient condition for the discrete time sliding mode generation | sk+1 |<| sk | is satisﬁed, hence | sk | decreases monotonically and, after a ﬁnite 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 aﬀected 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 conﬁned 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 estimator, when the | yk |6 y0 condition is satisﬁed. More speciﬁcally, with reference to (12.4) and (12.8), choose yk = q¨dk + discrP Ik (·),

(12.11)

142

A. Calanca et al.

where discrP Ik (·) =

(

y

, K |ykeq keq |

ykeq + KP sk +

1+2KI ˆ KI fk ,

| ykeq |> y0

| ykeq |6 y0

(12.12)

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 estimation is given by

1 + 2KI ˆ )fk−1 ], (12.14) fˆk+1 = −KI [fˆk + (T H ∗ )−1 sk − KP sk−1 + (1 − KI

provided that | ykeq |6 y0 . In this case, deriving from (12.8) sk = T H ∗ (KP sk−1 +

1 + 2KI ˆ fk−1 − fk−1 ) KI

(12.15)

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 disturbance 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 eﬃcacy.

12 Discrete-Time Sliding Mode Controller for Robot Manipulators 143

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 Comau located at the Department of Electrical Engineering of the University 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 identiﬁcation simpler. Yet, the proposed method can be easily extended to a n-joint robot. Identiﬁcation The identiﬁcation 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 identiﬁed parameters for the robot are reported. Table 12.1. Minimal parametrization of the manipulator model

Meaning Parameter m3 b32 + J3 , J3 + m3 (l22 + b32 ) + J2 + m2 b22 γ1−2 J3 + m3 (l12 + l22 + b32 ) + J2 + m2 (l12 + b22 ) + J1 m1 b12 γ3 m1 b 1 + m2 l 1 + m3 l 1 , m2 b 2 + m3 l 2 , m3 b 3 γ4−6 Fs1 , Fv1 , Fs2 , Fv2 , Fs3 , Fv3 γ7−12

144

A. Calanca et al.

Table 12.2. Average value and variance for estimated parameters

γ1 θML E 0.297 [θML ] V ar 0.003 [θML ] V ar% 1.2 [θML ]

γ2

γ3

γ4

γ5

γ7

γ6

γ10

γ9

γ8

γ11 γ12

10.07 87.91 57.03 9.21 0.316 190.5 66.3 21.0 14.71 4.66 8.29

0.04

0.2

0.06 0.02 0.003 0.6

0.3 0.3

0.1 0.03 0.02

0.4

0.2

0.1

0.3

0.5 1.2

1.0

0.2

0.9

0.7 0.3

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 Smart3S2 manipulator are KP = 0.5, KI = −0.4, and K = (7, 9, 50). Fig. 12.2 shows the static performance and the good tracking properties 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. q

d1

0

1

2

4

3 q

2

q

20

d2

0 0

1

2

4

3

60

q

40

q

3

d3

20

0

0

1

2

t [sec]

3

4

0

q

−20

q

1 d1

2

1

0 20

0

q

−20

q

2 d2

d

40

20

1

q

q and q on joint 1, 2, 3 [rad]

q and qd on joint 1, 2, 3 [rad]

40 20 0 −20

40 20 0 −20 −40

0

0.5

1

1.5

q

3

qd3

0

0.5

1

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

12 Discrete-Time Sliding Mode Controller for Robot Manipulators 3

−38 −38.5 −39 −39.5

q1

2

qd1

1

0

2

4

3

2 q

1

2

q

d2

0 0

1

2

4

3

2 1.5 1 0.5

joint position [rad]

d

q and q on joint 1, 2, 3 [rad]

145

q1 qd1

1

8

6

4

2

1

q

2

0

qd2

−1 4

3

2

1

5

2

0

1

3 2 t [sec]

4

q3

1

qd3

0

q3 qd3

2

4

6

8

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 manipulators is presented in this paper in combination with the inverse dynamics method. The proposed control scheme provides robustness versus matched uncertainties and disturbances and allows to mitigate chattering 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

146

A. Calanca et al.

5. Colbaugh, R.D., Bassi, E., Benzi, F., Trabatti, M.: Enhancing the trajectory tracking performance capabilities of position-controlled manipulators. 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 Applications. 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 chatteringfree 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 ﬁrst 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 discretetime sliding mode control system. IEEE Trans. Automat. Contr. 32(10) (1987) 930–932 16. Capisani, L., Ferrara, A., Magnani, L.: Mimo identiﬁcation 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, Pozna´ n University of Technology, ul. Piotrowo 3a, 60-965 Pozna´ n, 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 transformation. The existence of such transformation was discussed by Spong [8]. The necessary and suﬃcient conditions which ensure that we obtain the canonical variables are very restrictive and rarely satisﬁed in practice [3, 8]. For that reason Jain and Rodriguez [3] proposed diagonalization in velocity space based on a spatial operator algebra approach. A diﬀerent solution using a congruency transformation was presented by Loduha and Ravani [5]. A PD control algorithm based on the formulation given in [5] was tested in [1]. The aim of this paper is to propose a controller which tracks velocity 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 curvilinear coordinates). The novelty of the approach relies on the fact that we use a modiﬁed 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 consequence, 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 trajectory is also explained (this quantity was not deﬁned in [5]). It appears that the GVC controller can be helpful for evaluation manipulator dyK. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 147–156, 2007. c Springer-Verlag London Limited 2007 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 formulation 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 ˙ ∈ RN is a symmetric system mass matrix, C(q, q) ˙ = (M˙ q˙ − 12 q˙T Mq q) 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) ˙ denotes the partial derivative of the mass matrix M (q) (Mqk = ∂M ∂qk 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 ﬁrst-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) = π, q˙ = Υ (q)u,

(13.2) (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 deﬁniteness 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

N (q) = Υ T (q)M (q)Υ (q) ˙ + C(q, q)] ˙ Cu (q, u) = Υ T (q)[M (q)Υ˙ (q, q)u T Gu (q) = Υ (q)G(q) π = Υ T (q)τ.

149

(13.4) (13.5) (13.6) (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)

qd denotes a desired joint acceleration where y = q¨d + cD q˜˙ + cP q˜ (¨ 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-deﬁnite gain coeﬃcients. 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 tracking the desired joint velocity vector (the integrals of the GVC, if they exist, diﬀer from the joint positions). The vector is deﬁned 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 mechanical couplings between the manipulator links. Thus, the mechanical couplings arising from the decomposition of the manipulator mass matrix are reﬂected 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), z˙u = u ˜,

(13.9) (13.10)

˜ = ud − u represents where cD and cP are positive diagonal matrices, u the velocity error vector in terms of GVC, u˙ d is time derivative of the

150

P. Herman and K. Kozlowski

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

t→∞

(13.11)

is guaranteed. Proof. It is assumed that the desired quantity ud (t) is continuously diﬀerentiable, 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 component ud (k) is some curvilinear generalized coordinate. Inserting the controller Eqs.(13.9)-(13.10) into the dynamics Eq.(13.2) one can obtain the second-order diﬀerential equation (the matrix N is a positive deﬁnite 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 vector 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) ˜ −cP zu − cD u ˜ dt u

The Lyapunov function candidate is proposed as follows: LE (zu , u˜) =

1 1 T u ˜ u ˜ + zuT cP zu . 2 2

(13.14)

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 ) T = −˜ u cD u ˜ 6 0, (13.15) because cD is a positive deﬁnite matrix. One can ﬁnd an upper bound of that time derivative in the following form: L˙ E (zu , u ˜) 6 −λmin (cD ) k˜ uk2 ,

(13.16)

where λmin (cD ) is the smallest value of the matrix cD . The Lyapunov function LE (zu , u ˜) is radially unbounded (it means, denoting T T T x = [zu , u ˜ ] , that [7] LE (x) → ∞ as ||x|| → ∞ or as x tends to inﬁnity in any direction) and its time derivative L˙ E (zu , u ˜) is negative semidefinite. These two facts imply that the state variables are bounded, i.e.

13 Velocity Tracking Controller for Rigid Manipulators

151

zu , u˜ ∈ LN ud (t)k is ∞ . Assuming the hypothesis that the desired signal k˜ 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 ˜ ∈ LN u is a square integrable function). Because a 2 (˜ 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 fulﬁlled. Exponential convergence. Based on the results presented in [9] we can ˜T ]T ): introduce the following cross term (using the vector [zuT , u 1 ˜) = δ(zuT u ˜ + zuT cD zu ), LC (zu , u 2

(13.18)

where δ is a small constant so that L is positive deﬁnite. The new Lyapunov function L = LE + LC is deﬁned as:

1 1 T ˜. ˜ + zuT (cP + δcD )zu + δzuT u ˜ u L(zu , u ˜) = u 2 2

(13.19)

Its time derivative is: ˙ u, u L(z ˜) = 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: ˙ u , u˜) = u ˜T (−cP zu − cD u ˜) + zuT (cP + δcD )z˙u L(z +δzuT (−cP zu − cD u ˜) + δ˜ uT u ˜ = −˜ uT cD u ˜ − δzuT cP zu +δ˜ uT u ˜ 6 −(λmin (cD ) − δ)k˜ uk2 − δzuT cP zu , (13.21) where λmin (cD ) is deﬁned as previously. Now the function L(zu , u˜) can be written as follows: 1 zu T cP δcD zu (13.22) ˜) = L(zu , u ˜ 2δI I u ˜ 2 u

and the expression deﬁned by Eq.(13.21) (I denotes the identity matrix) T zu zu δcP 0 ˙ ˜) 6 − . (13.23) L(zu , u u ˜ u ˜ 0 (λmin (cD ) − δ)I

152

P. Herman and K. Kozlowski

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) guarantees 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 deﬁnite matrix). Thus we can ˙ From Eq.(13.11) it results write that ud − u = Υ −1 (qd )q˙d − Υ −1 (q)q. −1 −1 only that Υ (q)q˙ → Υ (qd )q˙d , as soon as the time tends to inﬁnity. 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 simpliﬁed 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) individual control of links by quasi-velocity regulation; 2) observation mechanical 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 2 is expressed as K = 12 uT N u = 12 N k=1 Kk ). k=1 Nk uk = 2

Special case: integrable GVC. Consider a 3 d.o.f. spatial manipulator (with inverse numbering of links according to [3] because of a nice physical interpretation) presented in Fig. 13.1(a) in order to ﬁnd transformation 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 ﬁrst link, J1 is the ﬁrst link inertia, p1 means the distance between the axis of rotation and the mass center of the ﬁrst 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

13 Velocity Tracking Controller for Rigid Manipulators

153

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

13.4 Simulation Results In this section simulation results concerning the proposed tracking controller for a 3 d.o.f. spatial manipulator (which consists of two rotational joints and one translational joint) are given. The manipulator depicted in Fig. 13.1(a) is characterized by the following set of parameters: link masses: m1 = 3kg, m2 = 4kg, m3 = 15kg; link inertias : 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 coeﬃcients 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 observable between the ﬁrst link and other links because ud1 diﬀers 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 reﬂected 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 integrals in the controller). This fact is conﬁrmed 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).

154

P. Herman and K. Kozlowski

dθd1, dθd2, ud1, ud2 [rad/s], dsd3, ud3 [m/s]

4 ud1

3

dθ

d1

dθd2= ud2

2 dsd3= ud3

1

0 0

d1

2

d2

t [s]

(a)

θ ,θ ,z

3

1

,z

ud1

ud2

[rad] , s , z d3

ud3

3

2

(b)

[m]

ev1, ev2 [rad/s] , ev3 [m/s]

eu1, eu2 [rad/s] , eu3 [m/s]

0.04 z

0.06

θ

d1

ud1

eu2

0.04

eu1

ev2

0.02

0.02 0

1

0

−0.02

sd3= zud3

−0.04

0

−1 0

1

ev3

3

1

0

(c) z ,z

0.005

u1

u2

t [s]

u3

−3

[m]

5

x 10

e3

−0.025 0

100

1

τ3

150

−5

z

t [s]

−10

u1

2

−15 0

τ2

τ1

50

e2 3

3

200

u2

−0.02

2

τ1, τ2 [Nm] , τ3 [N]

250

−0.01

t [s]

(e) 300

u3

0

z

1

0

e1, e2 [rad] , e3 [m]

−0.005

−0.015

3

2

(d)

[rad] , z

z

0

ev1

−0.04

−0.08

2

t [s]

−0.02

eu3

−0.06

θd2= zud2

e1 1

(f)

t [s]

(g)

0 2

3

0

1

t [s]

2

3

(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 positions θd (θd ), and integrals of quasi-velocities ud for all joints; d) quasi-velocity ˜˙ f) quasi-coordinate errors zu ; g) errors eu (˜ u); e) joint velocity errors ev (θ); joint position errors e (˜ q ); h) joint torques τ for all joints.

13.5 Concluding Remarks 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 contains 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.

13 Velocity Tracking Controller for Rigid Manipulators

155

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 Manipulators. 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 Transformations and Riemannian Geometry In: Proc. of IEEE Conference on Robotics and Automation, Nice, France. pp.554-556 9. Wen JT, Bayard DS (1988) New class of control laws for robotic manipulators, Part 1, Non-adaptive case. International Journal of Control 47:1361–1385

14 Fixed Point Transformations-Based Approach in Adaptive Control of Smooth Systems J´ ozsef K. Tar1 , Imre J. Rudas1 , and Krzysztof R. Kozlowski2 1

2

Budapest Tech Polytechnical Institution, H-1034 Budapest, B´ecsi u ´ t 96/b, Hungary [email protected], [email protected] Pozna´ n University of Technology, ul. Piotrowo 3a, 60-965 Pozna´ n, Poland [email protected]

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 singlevariable continuous functions [1]. Since the late eighties several authors have proved that diﬀerent 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 diﬀerent fuzzy reasoning methods are related to universal approximators (e.g. in [3]). Due to the fact that Kolmogorov’s theorem aims at the approximation of the very wide class of continuous functions, the functions to be constructed are often very complicated and highly non-smooth, therefore their construction is diﬃcult. As is well known, continuity allows very extreme behavior even in the case of single-variable functions. The ﬁrst example of a function that is everywhere continuous but nowhere diﬀerentiable was given by Weierstraß in 1872 [4]. At that time mathematicians believed that such functions are only rare extreme examples, but nowadays it has become clear that the great majority of the continuous functions have extreme properties. The seemingly antagonistic contradiction between the complicated nature of the universal approximators and their successful practical applications makes one arrive at the conclusion that if we restrict our models to the far better behaving “everywhere diﬀerentiable” functions, these problems ab ovo can be evaded or at least reduced. K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 157–166, 2007. c Springer-Verlag London Limited 2007 springerlink.com

158

J.K. Tar, I.J. Rudas, and K.R. Kozlowski

Really, in the vast majority of the nonlinear control literature analytical models of various physical systems are considered that are available in the form of sets of diﬀerential 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 timederivative of a positive deﬁnite 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 require 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 approaches as well as the phenomenological restriction that only modeling 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 eﬀects of external disturbances and modeling errors simultaneously 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 computational 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 introduced and applied in adaptive control of a cart-pendulum system under drastic external perturbation serving as the simplest paradigm of incompletely 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 ﬁrst eﬀorts 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 maintaining are deﬁnitely determined by the degree of freedom of the system to be controlled. These considerations do not even assume the availability of an analytic system model of satisfactory accuracy. They are based on the simple assumption that the system to be controlled can be characterized 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 modiﬁed excitations 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 exerted 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 acceleration. 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 diﬀers 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

160

J.K. Tar, I.J. Rudas, and K.R. Kozlowski

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 deﬁned on the basis of purely kinematic considerations quite independently of the dynamic behavior of the system. The task of realizing this desired response remains exclusively that of the adaptive part of the controller. This feature of this novel approach considerably diﬀers 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 application of Lyapunov’s method in which V˙ can simply be prescribed. Instead of that an inﬁnite series of cascade expressions h(h(h(...))) appears, for which appropriate convergence has to be guaranteed. In the case of Single Input Single Output (SISO) systems for constant 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⋆ , therefore the above iteration corresponds to ﬁnding the ﬁxed point of the function r⋆ = u(r⋆ |r d ). As is well known, if |∂u/∂r| < 1 in the vicinity of r⋆ the proposed iteration converges to the ﬁxed 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 comparison 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 ﬁxed 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 deﬁnite SISO systems by the use of a double-parametric transformation deﬁned as d

(x)−x ) wζ,D (x) := f (x)+D+ζ(f x f (x)+D ′ dwζ,D (x) ⋆ )x⋆ wζ,D (x⋆ ) = x⋆ , = 1 + ζfxd(x+D , dx

(14.1)

x=x⋆

xd + D,

which is also convergent if x⋆ ≈ 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 ﬁxed-point transformations, the

14 Fixed Point Transformations-Based Approach in Control

161

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 following functions inspired by simple schematic geometric pictures with the parameters ∆+ > xd , ∆− < xd , and x > D− : g(x|xd , D− , ∆− ) :=

h(x|xd , D− , ∆+ ) :=

f (x)−∆− (x xd −∆−

− D− ) + D−

x −∆+ f (x)−∆+ (x

− D− ) + D− .

d

(14.2)

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. ﬁnding a properly deformed input x⋆ , is transformed to ﬁnding the solution of ﬁxed 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 ﬁxed points. To guarantee that, it is suﬃcient to show that g and h are contractive in the vicinity of their ﬁxed points: − + g′ (x⋆ ) = f ′ (x⋆ ) xx⋆d −D −∆−

h′ (x⋆ ) =

xd −∆+ f (x⋆ )−∆+

f (x⋆ )−∆− xd −∆−

−

− + 1, = f ′ (x⋆ ) xxd⋆ −D −∆−

(xd −∆+ )(x⋆ −D− ) ′ f (x⋆ ) [f (x⋆ )−∆+ ]2

=

(14.3)

− . = 1 − f ′ (x⋆ ) xx⋆d −D −∆

+

Evidently, if ∆+ > xd , ∆− < xd , x > D− , and f (•) is “ﬂat 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 inﬂuenced by the parameters D− , ∆− , ∆+ , and mainly by the “ﬂatness” 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.

162

J.K. Tar, I.J. Rudas, and K.R. Kozlowski

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 deﬁnite symmetric inertia matrix guarantees a mathematically positive deﬁnite 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 directly acting drives, this “increasing” nature of the relationships between the excitation and the system response can disappear in certain domains. The simplest example of such cases is the cart-pendulum system 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 , mL cos ϕ¨ x + (I + mL2 )ϕ¨ = −f ϕ˙ + mgL sin ϕ + Q2 ,

(14.4)

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 −0.383

−0.088

0.208

0.504

0.799

−2.62 0.384

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.00

1.25

2.50

3.75

0.488

0.592

0.696

0.799

Tracking error for fi [rad] vs time [s]

5.00

−0.299 0.00

1.25

2.50

3.75

5.00

Fig. 14.1. Comparison of the phase trajectories (upper row) and tracking errors (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

14 Fixed Point Transformations-Based Approach in Control 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 −14.0

−5.6

2.8

11.2

19.6

−80.4 1.75

fi vs. time [10^0 rad vs s] 8.00

11.2

6.44

2.8

4.87

−5.6

3.31

3.75

7.50

11.25

15.00

1.75 0.00 Nominal 3.75

2.91

29.5

1.26

17.0

−0.39

4.5

−2.04

−8.0

3.75

7.50

11.25

4.87

6.44

8.00

15.00

7.50

11.25

15.00

Adaptive factor s [10^−1dimless] vs time [s]

Tracking error for fi [10^−1 rad] vs time [s]

−3.69 0.00

3.31

fi vs. time [10^−1 rad vs s]

19.6

−14.0 0.00

163

−20.5 0.00

3.75

7.50

11.25

15.00

Fig. 14.2. Results for undamped nominal motion under strong external perturbation: the phase space of ϕ (upper graphs) and the trajectory tracking (central graphs) for the non-adaptive case (left-hand column), and the adaptive 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 Q the tracking error (on the left) and the cumulative adaptive factor s(tn ) := nj=0 srel (tj ) constructed of the instantaneous d

)−∆+ − for h, and srel (tn ) = y¨y¨d(t(tnn)−∆ factors srel (tn ) = y¨y¨(t(tnn)−∆ )−∆− 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 coeﬃcients, 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

164

J.K. Tar, I.J. Rudas, and K.R. Kozlowski

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: Q1 = bx˙ + mL sin ϕϕ˙ 2 +

M +m mL cos ϕ 2

(−f ϕ˙ + mgL sin ϕ) +

2 L2

)+m + −(M +m)(I+mL mL cos ϕ

cos2 ϕ

ϕ¨d .

(14.5)

It is easy to see from (14.5) that near ϕ = 0 Q1 is a decreasing function of ϕ¨ since the coeﬃcient of ϕ¨d takes the following form: −M (I+mL2 )−mI−m2 L2 sin2 ϕ 1 < 0, therefore an instan< 0, that is ∂Q mL cos ϕ ∂ϕ ¨d 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 coeﬃcient” and an additive term. By its use the resulted angular acceleration can be observed, and in general in the kth control cycle d∗ d d∗ d∗ d the deformed xd∗ k := g(xk−1 |xk , D− , ∆− ) or xk := h(xk−1 |xk , D− , ∆+ ) can be introduced. In the simulations tracking properties deﬁned by x ¨d (t) = x ¨N (t) + P [xN (t) − x(t)] + D[x˙ N (t) − x(t)] ˙ were prescribed −2 with strong proportional P = 500 s 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 eﬀects 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 approximators based SC technologies and that of the classical methods using Lyapunov functions was given. As alternative approach, instead of developing complete and permanent system models, incomplete, situationdependent, and temporal models can be used that require continuous amendment or maintenance via machine learning. For this purpose special Fixed Point Transformations were proposed for SISO systems. The possibility of making this approach convergent for negative deﬁnite systems was mathematically proved and demonstrated via illustrative simulations. It can also be expected that by the use of the previously

14 Fixed Point Transformations-Based Approach in Control

165

applied group theoretical aids these new transformations can be extended to MIMO systems as it was done in [9].

Acknowledgment This research was supported by the National Oﬃce 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 obtained 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) Uber continuirliche Functionen eines reellen Arguments, die f¨ ur keinen Werth des letzeren einen bestimmten Diﬀerentialquotienten besitzen. A paper presented to the ’K¨ onigliche 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 Cliﬀs, 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. ´ and Kozlowski K (2006) Novel Adap9. Tar JK, Rudas IJ, Szeghegyi A, tive Control of Partially Modeled Dynamic Systems. In ”Lecture Notes in Control and Information Sciences”, Springer Berlin/Heidelberg, Robot 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.

166

J.K. Tar, I.J. Rudas, and K.R. Kozlowski

11. Tar JK (2005) Extension of the Modiﬁed Renormalization Transformation for the Adaptive Control of Negative Deﬁnite SISO Systems. In the Proc. of the 2nd Romanian-Hungarian Joint Symposium on Applied Computational Intelligence (SACI 2005), May 12-14, 2005, Timi¸soara, Romania, 447–457

15 Driving Redundant Robots by a Dedicated Clutch-Based Actuator∗ Anani Ananiev1 , Thorsten Michelfelder2 , and Ivan Kalaykov1 1

2

Center for Applied Autonomous Sensor Systems (AASS), Department of ¨ ¨ Technology, Orebro University, 70182 Orebro, Sweden {anani.ananiev,ivan.kalaykov}@tech.oru.se ¨ Atlas Copco AB, Orebro, Sweden [email protected]

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 necessary locomotion for performing almost any task in their lifes. Therefore 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 applicability in some important areas. The common approach of actuating the robot modules in the majority 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 hyperredundant 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 simpliﬁed driving of robot’s multiple-module mechanisms. Completely diﬀerent ∗

The authors are grateful to KK-foundation, Sweden, for its support of the research ¨ programs at the Center of Applied Autonomous Sensor Systems (AASS), Orebro University, Sweden.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 167–176, 2007. c Springer-Verlag London Limited 2007 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 ﬂexible 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 developed concept requires a speciﬁc 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 robustness, 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 electromagnetic 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 irreversible 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 transmission wheels. When an electromagnetic clutch is powered-on, the ﬂexible 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 oﬀ. 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 appropriately 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 ﬁrst 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-oﬀ logic control and can be implemented on a variety of commercial embedded micro controllers. We decided to make the most simple and biologically inspired structure of a snake robot as the ﬁrst 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 problems. Figure 15.1 presents the construction and Fig. 15.2 the kinematic scheme of the robot.

Fig. 15.1. First AASS hyperredundant robot prototype

Fig.15.2. Kinematics with worm gear 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 ﬂexible 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 segment 10 of the chain. The motor output shaft is connected to the proximal side of the ﬂexible 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-eﬀector, articulated to the distal segment of the robot. A primary driving wheel 2 is ﬁxed to the ﬂexible 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 ﬁxture 7. The shaft of clutch 5a is ﬁxed to wheel 3a and the shaft of clutch 5b is ﬁxed 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 ﬁxed to the moving part of the respective clutch 5a or 5b. When clutch 5a is

170

A. Ananiev, T. Michelfelder, and I. Kalaykov

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 ﬁxed 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 electromagnetic break 6 keeps this angle ﬁxed 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 ﬁxing 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-oﬀ of different amount of robot modules and having diﬀerent 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 module is needed. In the next section we ﬁrst 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

15 Driving Redundant Robots by a Clutch-Based Actuator

171

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 magnetic clutch

Fig. 15.4. General hysteresis loop (Bs : sat.ﬂux, Hs : sat.ﬁeld; Hc : coercitive force [5])

The normal force F produces a friction force FM = F µ, where µ is a friction coeﬃcient. 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 ﬁeld follows the electromagnetic model of the coil dL dx di d(L.i) , (15.2) =L +i VL = dx dt dt dt where x is the distance (gap) between the anchor plate and the enclosure. This gap is too small, so dL/dx ≈ 0 and Eq. (15.2) becomes di . The inductance of the solenoid is L = µ0 AN 2 /l, where VL = iR + L dt 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 ﬁeld strength H is H = N i/l [8] and the magnetic ﬂux density B is obtained from the magnetic hysteresis model [7]. The model of the hysteresis includes the magnetization and its derivatives: dBh+ (H) Bh− (H) − B dB − µ0 , for dH > 0, = µ0 + dH Bh− (H) − Bh+ (H) dH

172

A. Ananiev, T. Michelfelder, and I. Kalaykov

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

for dH < 0,

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 diﬀerential equation (15.1) ﬁnally becomes J2 ω˙ 2 + a2 ω2 = kµB 2 (t), where k =

(15.3)

A(r1 + r2 ) . 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 simpliﬁcation was veriﬁed by simulation of PID control of the original hysteresis model and the linearized one. Choosing the state variables x1 = ω2 ,

x2 = ω˙ 2 ,

x˙ 1 = x2 ,

x˙ 2 = ω ¨2,

x3 = i, x˙ 3 = i˙

and the voltage e applied to the coil as input u(t), we obtain

σ2 a2 dx1 = ω˙ 2 = − x1 + K x23 (t), J2 J2 dt σ 2 d(x23 (t)) a2 dx2 , (15.4) =ω ¨ 2 = − x2 + K dt J2 J2 dt 1 R dx3 = − x3 (t) + u(t), L L dt 2 d(x23 ) Aµ(r1 + r2 ) N 3 = 2x3 dx and where K = dt . dt l 4µ0 Hence the nonlinear state-space model for simulation purposes is         a2 2 − J2 0 K · σJ2 · x3 0 x1 x˙ 1  2  x˙ 2  =  (15.5)  0 − aJ22 K · σJ2 · x˙ 3  ·  x2  +  0  u(t). 1 R x x˙ 3 3 0 0 −L 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

15 Driving Redundant Robots by a Clutch-Based Actuator

173

xeq ˙ 2eq = 0. From this condition we can obtain a relation expressing 2 =ω the x3 coordinate as a function of the other coordinates, namely r a2 eq eq x , (15.6) x3 = Kσ 2 1

which is used to simplify the clutch model. Given the reference values x1d , x2d , and x3d , we deﬁne the error state space as z1 = x1 − x1d z2 = x2

and

z˙1 = z2 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 functions 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 slidingmode controller (SMC), which is known with its good robustness to parametric uncertainties [6], [4]. For our model a ﬁrst-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 u=

1 [−f (x) − λ x2 − W sign(S)] , g(x)

(15.7)

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 resistance R = 37 Ω, damping parameter a2 = 0.08, magnetic path length l = 0.1 m, friction coeﬃcient µ = 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

174

A. Ananiev, T. Michelfelder, and I. Kalaykov

that the change of input speed ω1 does not aﬀect the controllability for the case ω1 > ω2 . That is mainly based on the fact that the friction coeﬃcient is independent on the speed diﬀerence of both friction surfaces, i.e. both shafts. Therefore, the change of speed of the primary surface aﬀects 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 parallel, 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 speed

Fig. 15.6. Step response

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 ﬂexible hyperredundant 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 modiﬁcations and amendments. The advantages of this method can be summarized as follows: • As the plurality of motors is avoided, it provides a lightweight structure with a higher payload.

15 Driving Redundant Robots by a Clutch-Based Actuator

175

• The method can be used in a variety of constructions aimed at modularity and simplicity. • The construction can be expanded and reconﬁgured according to the task to be accomplished. • The variety of components is very low, therefore it is easy to assemble and disassemble, making the robot relatively simple and economical to manufacture and maintain. • The control system is simpliﬁed for independent control of all modules applying on-oﬀ 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 diﬀerent conﬁgurations. 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 reﬂecting imprecision of the mechanical components and the entire assembly of the actuator. The purpose of this paper is limited to only disclose the main concept of the method and is far from being comprehensive in deriving, studying, analyzing and proving certain theoretical, and physical properties, 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. B¨oge, 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 simulation of a fast 2/2 switching valve. Fifth Int. Conf. on Fluid Power Transmissions and Control, Hangzhou, China, pap. 2-22.

176

A. Ananiev, T. Michelfelder, and I. Kalaykov

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. ¨ uner U. ¨ (1999) A control engineer’s guide 9. Young, D.K., Utkin V., Ozg¨ 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 [email protected]

16.1 Introduction An environment where virtual worlds coexist with real worlds, mixed reality, has been extensively studied in the last decade. Milgram’s deﬁnition of the Reality-Virtuality Continuum [8] focuses on display technology such as head-mounted displays. However, people still feel the gap between mixed reality environments supported only by display technology 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, thermodynamic). 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 extendible 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 connect with a virtual energy dynamics via an improved hyper-bond interface 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 methodology 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. K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 177–184, 2007. c Springer-Verlag London Limited 2007 springerlink.com

178

Y.-H. Yoo

16.2 Mixed Reality Bond Graphs The mixed reality bond graph methodology adds the two following improvements 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 modiﬁed 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 deﬁned 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 ﬂow-negative-feedback causality (Fig. 16.1b) and eﬀort-negativefeedback causality (Fig. 16.1c). This consists of an eﬀort and ﬂow

16 An Energy-Based Approach Towards Modeling

179

samplers, zero-order hold (ZOH), connection-impedance Z, eﬀort or ﬂow generator MS (MSe or MSf ), and gain K – PI, PD, or PID controllers 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 subsystem 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 energy coupling points. The mixed reality bond graph descriptions of the

Fig. 16.2. Structure of the haptic ball manipulator (left) and sketch of hardware parts of the hyper-bond interface (right)

ball manipulator are shown in Fig. 16.3. Real/virtual subsystems are described by two diﬀerent 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 subsystems, 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 stiﬀness 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 stiﬀness 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 manipulator

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: FvirtualA (s) EI1 (s) , G2 (z) = Ztransf , G1 (z) = Ztransf ErealA (s) ErealA (s) FI1 (s) EI1 (s)−Ecollision G3 (z) = Ztransf , G4 (z) = Ztransf , ErealA (s) Ecollision (s) FvirtualA (s) FI1 (s) G5 (z) = Ztransf , G6 (z) = Ztransf , 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 ﬂow-negative-feedback causality (Fig. 16.1b) is used. The hardware structures of the hyper-bond interface 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 subsystems. 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

16 An Energy-Based Approach Towards Modeling

181

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   1  R3 − I3 − I3 f2 f2 m·I I3 0 3 d  M Se1   1 1      − m·C2  e5 + 0 0 e5 =  C2 0 , M Sf1 dt f 1 R3 R3 Rz m f 0 10 10 Iz Iz Iz − m·Iz − 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 GV toR (z) = Z − transf {

1 − e−sT FrealA (s) · }, s FvirtualA (s)

(16.3)

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

GV toR (z) = K ·

0.001z 2 − 0.002z + 0.001 . z 3 − 2.9839z 2 + 2.9871z − 1.0035

(16.4)

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 diﬀerence 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 compensator with Kp = 1009 and Ki = 100.9. Thus, the discrete-time PI compensator is GP I (z) =

0.00091z − 0.0009910 . z − 0.9999

(16.5)

The mixed reality bond graph modeling (Fig. 16.3) is implemented in 20-sim as shown in Fig. 16.6. DiscreteDiﬀerential1, DiscreteIntegral1, and q-sensor (position sensor) are components to use position input instead 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, industrial 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 remote 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

16 An Energy-Based Approach Towards Modeling

183

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 provide 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 diﬀerential A/D inputs, 15 kHz rate – is used as an A/D and D/A converter. 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 representations 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 virtual 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 uniﬁed 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 uniﬁed 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 Manufacturing, 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 between people, bits and atoms. Proc. CHI ’97, 234-241 11. Yoo Y H (2007) Mixed Reality Design Using Uniﬁed 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

2

Carleton University, Department of Mechanical and Aerospace Engineering, 1125 Colonel By Drive, Ottawa, ON, K1S 5B6, Canada [email protected] Frontline Robotics, 1920 Research Road, U62, Ottawa Int. Airport, Ottawa, ON, K1V 9B4, Canada [email protected]

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 diﬃcult and complex task because often the environment does not have characteristic points that can be identiﬁed and with respect to which robots could relate their position. If environment is unstructured (e.g. oﬀ-road environment), problems related to autonomous navigation are very diﬃcult. 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. Information 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 ﬁnal destination. Very often the map is dynamic, which represents moving objects (e.g. other vehicles, peoples, changing conﬁguration of permanent objects etc.). In that case an autonomous robot has to create 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 K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 187–208, 2007. c Springer-Verlag London Limited 2007 springerlink.com

188

J.Z. Sasiadek, Y. Lu, and V. Polotski

with respect to the nearest structure or obstacle (e.g., laser range ﬁnders, 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: ﬁrst, metric, and, second, topological. Metric maps acquire geometrical 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 mapping 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 eﬀort caused by high dimensionality. The second problem is related to data association caused by uncertainty if the mapped region can be identiﬁed and match with real world. The third problem is connected to measurement noise. In many statistical and probabilistic methods it is assumed that the measurement 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 ﬁltering and include, between others, simultaneous localization and mapping (SLAM) method. Nowadays, more and more, SLAM name is used in conjunction with methods that are not based on Kalman ﬁltering. The Kalman ﬁltering methods are essentially restricted to linear or linearized systems with measurement white noise assumption. Within the second group there are advanced methods based on adaptive Kalman ﬁltering. The idea behind it related to extension of Kalman ﬁlter applicability to measurement with non-white noise. The gain of the Kalman ﬁlter is adapted to prevent divergence caused by colored noise. The third approach includes particle ﬁlters. Particle ﬁlters 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 diﬀerent 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ﬁed into three into three diﬀerent groups. First, fusion based on probabilistic 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 ﬁltering, optimal theory, regularization 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 artiﬁcial objects. The examples of external sensors are sonar, radars and laser range ﬁnders. Measurements of internal sensors are quite accurate for short period. However, for long-term estimation, the measurements usually produce

190

J.Z. Sasiadek, Y. Lu, and V. Polotski

a typical drift. External sensors do not produce the drift, however, because of their characteristics, the measurements are not always available. 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 suﬃcient. 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 diﬀerent groups. First, fusion based on probabilistic 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 ﬁltering, 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 speciﬁc 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 diﬀerential GPS(DGPS) cannot be used, either because it is not available in the area or it is

17 Navigation of Autonomous Mobile Robots

191

Fig. 17.1. Mobile robot platform for security applications

not desired due to logistic requirements. A few meters precision of basic localization is not suﬃcient 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 eﬀective. 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 ﬁltering with fuzzy logic adaptation procedure. This procedure ensured stability and convergence of ﬁlter gain. 17.2.1 Problem Description The gate crossing problem addressed here consists of detecting the entrance 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 diﬀers from range based wall-following, since it requires the transition from an open ﬁeld GPS or fusion-based navigation to range-based navigation. This transition itself requires a feature extraction and recognition phase usually not necessary in the environments as underground mines, where rangebased wall-following is suﬃcient [6]. The extraction of robust geometrical features from the data provided 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-

192

J.Z. Sasiadek, Y. Lu, and V. Polotski

ment representation, LMS data ﬁltering, gate recognition, localization and motion control. The robot platform, ARGO conquest 6×6, Fig. 17.1, is manufactured by Ontario Drive & Gear Ltd. (ODG). It was retroﬁtted, for computer control and equipped with NovAtel GPS, MicroStrain inertial sensor, built-in wheel odometry, and LMS-221 laser scanner (manufactured 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. Experimental 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-ﬂight principle. The working principle is simple: a laser pulse emitted for a short time is reﬂected by a target object. A count starts while the pulse is transmitted, and stops when the reﬂected signal is received. The emitted 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 speciﬁcations, 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).

17 Navigation of Autonomous Mobile Robots

193

Depending of the position of the sensor, the perception of the gate obtained using LMS scanner is diﬀerent. 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 portions (corresponding to two posts); each portion contains an isolated segment or two adjacent segments (a corner). Fig. 17.5 is another scanning 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 reﬂection 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

194

J.Z. Sasiadek, Y. Lu, and V. Polotski

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 ﬁrst 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).

17 Navigation of Autonomous Mobile Robots

195

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

196

J.Z. Sasiadek, Y. Lu, and V. Polotski

Fig. 17.6. Signature illustrations of a gate segment

dimension of the signature varies from zone to zone. Canonical signatures 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 ﬁnally to the reﬁnement of the localization result and guidance. Table 17.2. Signature of the gate scans

17 Navigation of Autonomous Mobile Robots

197

17.2.3 Experimental Procedure An algorithm of gate recognition is described in this section. The algorithm contains several modules: the GPS/INS/LMS data collection module, the map building module, the map ﬁltering module, the map ﬁtting module, the gate signature calculation module and the unexpected error check module. The ﬂowchart of the algorithm is shown in Fig. 17.7. Data is acquired from GPS/INS/LMS ﬁrstly 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 reﬂections, 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 nongate objects from the preliminary map would be removed by the map ﬁltering 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% diﬀerence in size and width is tolerated). The map-ﬁltering algorithm is described in details as: Step 1: Set the eﬀective 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 ﬁltered map corresponding to a preliminary map from the Fig. 17.8. After the above steps, if the ﬁltered map still contains more than two objects, the tolerance conditions for the dimension and distance are tightened and the ﬁltering algorithm is re-applied.

198

J.Z. Sasiadek, Y. Lu, and V. Polotski

Fig. 17.7. Flowchart of gate navigation module

The points obtained from the scan are discrete, but represent continuous edge lines. Each edge is delimited by vertex points and can be identiﬁed 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 ﬁtting 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 eﬀect from environmental disturbances and assume that the only object in the ﬁltered 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).

17 Navigation of Autonomous Mobile Robots

199

Fig. 17.8. Map of the raw scanning data

Fig. 17.9. Filtered and enlarged gate object

Step 1 : Scanning from left to right, ﬁnd the last point of the left hand post, point d, and the ﬁrst 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 ﬁnd 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 ﬁrst 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.

200

J.Z. Sasiadek, Y. Lu, and V. Polotski

When all the vertexes/corners are identiﬁed, 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 canonical signature matches the current signature. For matching decision the squared norm of the diﬀerence 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 ﬁtting 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 outputs the data to the gate crossing control module. Here the ﬁnal step of processing localization data is described. Let us deﬁne 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 longitudinal 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 estimated. 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 ﬁrst parameter to be output to the control low being designed. Let us deﬁned a target

17 Navigation of Autonomous Mobile Robots

201

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

202

J.Z. Sasiadek, Y. Lu, and V. Polotski

4

Gate left post 2

Middle point for navigation

Y (meter)

0

-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 addressed 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 continuously estimated on-line. This constitutes a remarkable diﬀerence between gating and parking problems. Another diﬀerence 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 ﬁeld). In order to address these diﬀerences, we (i) estimate the entrance middle point C, (ii) we deﬁne 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 deﬁnition above). This procedure allows us to keep the vehicle oﬀset from the goal point (singular point, [7]), thus ensuring a stable motion across the gate.

17 Navigation of Autonomous Mobile Robots

203

An intermediate control output is deﬁned as follows: u = K1 α + K2 ψ

(17.1)

where K1 and K2 are chosen using a pole placement technique. For our ﬁeld experiments K1 = and K2 = −0.4. This choice corresponds to both poles set to −1 on the Z-plane. In order to compute the ﬁnal 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 oﬀsets that can even lead to unstable behavior a dead zone −umin , umin for u is introduced and ﬁnal 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 ﬁrst with synthetic data. In addition, the ﬁltering and signature-based recognition modules have been veriﬁed with LMS data collected along the manual driving through the gate. Finally, the algorithm has been fully integrated on-board and tested in the ﬁeld. 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 ﬁltering module continuously monitors the environment based on the available LMS data. If the gate is actually farther than a soft-upper-limit range the ﬁltered map is empty. When the gate posts become observable they completely appear in the ﬁltered 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.

204

J.Z. Sasiadek, Y. Lu, and V. Polotski

Fig. 17.13. GPS trajectory of experiment

’

Fig. 17.14. Illustration trajectory of gate through’

The uncertainty (of about 2m) in absolute (GPS/INS/Odometrybased) 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 oﬀset (2-norm of the diﬀerence) is between the current signature and a canonical signature (taken from the data base

17 Navigation of Autonomous Mobile Robots

205

N orms of the difference between the two s ignatures

2

1.8

1.6

1.4

1.2

1

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 signiﬁcantly 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 oﬀsets are coupled. In practice the computation remains possible but measurement errors are ampliﬁed and results become unusable. The developed concept of gate signature provides an eﬀective method to estimate the relative position between the vehicle and the gate. The motion control algorithm based on the nonlinear transformation to polar coordinates proposed in [10] coupled with on-line estimation of the vehicle pose and enhanced by the moving target point for avoiding singularities ensures the stable gate crossing with acceptable lateral errors of about 0.3 m. In this work the more general fusion of range measurements 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 improve the reliability of the navigation system providing better global

206

J.Z. Sasiadek, Y. Lu, and V. Polotski

position estimates along the whole path and smoothing the phase of approaching 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 ﬁeld experiments performed this year with retroﬁtted ARGO platform have proven that the signature concept, recognition algorithm, and gating controller work very well and provide an eﬀective mean for carrying out the gate navigation tasks. Figure 17.16 illustrates the vehicle passing through the gate in an experimental ﬁeld.

Fig. 17.16. A “gate through” procedure experimental veriﬁcation

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 diﬃcult. 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.

17 Navigation of Autonomous Mobile Robots

207

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 diﬃcult in some particular situations. Creating an environment map using multi-sensor information 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 eﬀective and the overall system more robust.

References 1. Sasiadek J.Z., Lu Y., Polotski V. “Navigation of Autonomous Mobile Robot with Gate Recognition and Crossing”, Proc. of the 8-th IFAC Symposium 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, Munich, Germany, 15-18 May 2006. 3. Sasiadek J.Z., Wang Q., “Low cost automation using INS/GPS data fusion for accurate positioning”, Robotica (2003) vol. 21, pp. 255-260, 2003. 4. Sasiadek J.Z., “Sensor Fusion”, Annual Reviews in Control, IFAC Journal, 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 Integration 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 Automation, 2004, Fifth World Congress, vol. 5, Hangzhou, China, June 2004 pp. 4681-4685. 10. Astolﬁ K., “Exponential Stabilization of a Wheeled Mobile Robot via Discontinuous Control”, J. of Dynamical Systems Measurements and Control, 1999, v. 121, pp. 121-125,.

208

J.Z. Sasiadek, Y. Lu, and V. Polotski

11. Goel P., Roumeliotis P.I., Sukhatme G.S., “Robot Localization using Relative and Absolute Position Estimates” in Proc. IEEE Int. Conf. on Robots and Systems, 1999, Kyongju, Korea. 12. Pﬁster 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 Tcho´ n 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 manipulators. 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 conﬁguration space approach. First ideas on this subject were formulated in [4] and then extended toward kinematic [5], and dynamic [9] performance measures. In comparison to traditional approaches the novelty oﬀered by the endogenous conﬁguration 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 framework for the study of the kinematic and the dynamic dexterity of mobile manipulators, endowing the endogenous conﬁguration space with a Riemannian metric, and incorporating a coordinate-free description of the taskspace. On this background we shall deﬁne 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-eﬀector trajectories that correspond to the conﬁguration of the maximum dexterity. Tracking a motion pattern will result in the most eﬃcient 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 exposition of the main idea of our approach. In Section 18.3 we introduce ∗

This research has been supported by the Foundation for Polish Science.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 209–218, 2007. c Springer-Verlag London Limited 2007 springerlink.com

210

K. Zadarnowska and K. Tcho´ n

basic technical concepts, culminating in a deﬁnition of the motion pattern 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 conﬁguration space approach relies upon a fundamental analogy between the kinematics of a stationary manipulator and the input–output map of a control system. To be more speciﬁc, suppose that an aﬃne 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 ﬁxed 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 |α=0 h(ϕz0 ,T (u(·) + α△u(·))). dα (18.2) The derivative DHz0 ,T (u(·)) can be computed by means of the variational system △y(T ) = DHz0 ,T (u(·))△u(·) =

ζ˙ = A(t)ζ + B(t)△u(t), η = C(t)ζ,

(18.3)

∂ (f (z(t)) + g(z(t))u(t)), B(t) = associated with (18.1), where A(t) = ∂z ∂ g(z(t)), C(t) = ∂z h(z(t)). Denoting by Φ(t, s) the transition matrix, ∂Φ(t,s) = A(t)Φ(t, s), Φ(s, s) = In , we obtain ∂t

DHz0,T (u(·))△u(·) = C(T )

Z

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 analogy between the Gram matrix and the manipulability matrix, the determinant of the Gram matrix can be used as a performance measure of the control system at a given control u(·). Depending on the physical 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ﬁguration space approach the control applied in the system (18.1) is referred to as the endogenous conﬁguration 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 manipulator. Its kinematics is represented as a driftless control system with outputs m X q˙ = G(q)u = gi (q)ui , y = k(q, x), (18.4) i=1

denotes generalized coordinates, x ∈ Rp is a vec platform Rd tor of joint positions, and y = ∈ SE(3) describes the location of 0 1 the end-eﬀector. 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 deﬁnite 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 regarded as controls that remain constant over [0, T ]. Pairs (u(·), x), constitute the endogenous conﬁguration 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 conﬁguration space with a Riemannian metric where q ∈

Rn

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

K. Zadarnowska and K. Tcho´ n

212

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-eﬀector 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(·) associated with the dynamics of (18.4), where A(t) = B(t) = G(q(t)). The mobile manipulator Jacobian 6

(18.6) ∂(G(q(t))u(t)) ∂q

Jq0 ,T (u(·), x) : X → R , Jq0 ,T (u(·), x)(△u(·), △x) =

vT ωT

,

transforms the variation of the conﬁguration into the linear and the angular (space) velocity of the end-eﬀector at T , i.e. d |α=0 Kq0 ,T (u(·)+α△u(·), x+α△x) = Jq0 ,T (u(·), x)(△u(·), △x) = dα Z T Jqq0 (q(T ), x) Φ(T, t)B(t)△u(t)dt + Jqx0 (q(T ), x)△x, (18.7) 0

where Jqq0 (q(T ), x) =

"

∂d ∂q (q(T ), x) ∂R ∂R (q(T ), x)RT (q(T ), x)[ (q(T ), x)RT (q(T ), x)[, . . . , ] ∂q ] ∂q n 1

#

and Jqx0 (q(T ), x) =

"

∂d ∂x (q(T ), x) ∂R ∂R (q(T ), x)RT (q(T ), x)[ (q(T ), x)RT (q(T ), x)[, . . . , ] ∂x ] ∂x p 1

#

.

Above, the symbol ] · [ denotes the usual isomorphism of the Lie algebra se(3) and R6 , and the matrix Φ(t, s) is the transition matrix of

18 Kinematic Motion Patterns of Mobile Manipulators

213

(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)η. 0 ,T W −1 (u(·), x)JqxT (q(T ), x) 0 The kinematic dexterity matrix of the mobile manipulator appearing above, Dq0 ,T (u(·), x) = Jqq0 (q(T ), x)Gq0 ,T (u(·), x)JqqT (q(T ), x)+ 0

Jqx0 (q(T ), x)W −1 (u(·), x)JqxT (q(T ), x), (18.8) 0

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 (u(·), x)ζ = 1}, 0 ,T

(18.9)

whose volume is a kinematic dexterity measure of the conﬁguration (u(·), x) q (18.10) dq0 ,T (u(·), x) = det Dq0 ,T (u(·), x).

The dexterity (18.10) characterizes the quality of transmission of inﬁnitesimal motions from the endogenous conﬁguration space to the taskspace at a given conﬁguration. In the robotic terminology, nonzero dexterity means regularity of the conﬁguration. From the control theory point of view, non-zero dexterity implies local output controllability of (18.4). The endogeneous conﬁguration (u∗ (·), x∗ ) corresponding to the maximal dexterity deﬁnes a pair of the platform and the end-eﬀector trajectories kmp(t) = (q ∗ (t) = ϕq0 ,t (u∗ (·)), y ∗ (t) = k(q ∗ (t), x∗ )),

(18.11)

deﬁned for t ∈ [0, T ]. The pair (18.11) will be referred to as the kinematic motion pattern of the mobile manipulator. Tracking the motion pattern provides the highest sensitivity of the end-eﬀector response at

214

K. Zadarnowska and K. Tcho´ n

T to the variations of platform controls and joint positions of the onboard manipulator. By deﬁnition, the motion pattern depends on the initial conﬁguration of the platform and on the control time horizon. In practical computations it is also dependent on the assumed representation of control functions driving the mobile platform. Generally, the motion patterns are not unique and admit a kind of symmetry that reﬂects certain geometrical symmetries of the mobile manipulator.

18.4 Examples As an example, let us consider the unicycle-type mobile platform carrying 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 )

deﬁning the Cartesian end-eﬀector position with respect to an inertial frame. In the system (18.12) we shall apply control functions in the form ui (t) =

k X

ci2j−1 sin jt + ci2j cos jt,

i = 1, 2,

j=0

x2 l2 l1 x1

l0

d (q ,q )

z

1

2

y q3

x

Fig. 18.1. Double pendulum on a unicycle

(18.13)

18 Kinematic Motion Patterns of Mobile Manipulators

215

with either k = 1 (ﬁrst order harmonics) or k = 2 (second order harmonics). The problem to be solved consists in ﬁnding platform controls in the form (18.13) and joint positions x1 , x2 providing the maximum dexterity (18.10) for ﬁxed 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. Conﬁgurations of maximum dexterity

Constraints

i = 1, 2, k=1

i = 1, 2, k=2

Optimal conﬁgurations (c, x) {c10 , c11 , c12 , c20 , c21 , c22 , x1 , x2 } = {3.00, −3.00, −3.00, ±0.22, ±1, 32, ±0.38, 0.00, 0.00} dq0 ,T (c, x) = 993.55 {c10 , c11 , c12 , c13 , c14 , c20 , c21 , c22 , c23 , c24 , x1 , x3 } = {3.00, −3.00, −3.00, −3.00, 3.00, ±0.31, ±1.40, ±0.17, ∓0.23, ∓0.69, 0.00, 0.00} dq0 ,T (c, x) = 1620.80

-6

y2 y2 -8

10

-10

5

3

y3 2 1

-15 -10

y1

-5

40

y3

-5

0 -15 -14 y1

4 2 0 -2 -4 -40

20 0

-20

-10

y2

-20

0

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

y3 2 1 0 -22 -21 y1 -20

5 5 y1

-20 -15 -10 -5 -5 -10

40 y3

4 2 0 -2 -4 -40

20

0 -20 y1

y2

-20

0 20 40 -40

Fig. 18.3. Most dextrous pose, kmp(t), and dexterity ellipsoid for k = 2

216

K. Zadarnowska and K. Tcho´ n

T = 2π, the identity matrices R(u(·), x, t), W (u(·), x), and Q(y), and the conﬁguration constraints |cij |≤ 3, x1 , x2 ∈ [0, 2π]. The optimal conﬁgurations (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 measure whose maximization yields kinematic motion patterns for the mobile manipulator. Further research will be directed toward analysis and classiﬁcation 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 Control, 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. Tcho´ n, R. Muszy´ nski (2000) Instantaneous kinematics and dexterity of mobile manipulators. Proc. 2000 IEEE Int. Conf. Robot. Automat., San Francisco, CA 2493–2498. 5. K. Tcho´ n, K. Zadarnowska (2003) Kinematic dexterity of mobile manipulators: an endogenous conﬁguration 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) Uniﬁed analysis of mobility and manipulability of mobile manipulators. Proc. 1999 IEEE Int. Conf. Robot. Automat., Detroit, MI 1200–1206.

18 Kinematic Motion Patterns of Mobile Manipulators

217

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. Tcho´ n (2006) A control theory framework for performance 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 G´ ora, Podg´orna 50, 65–246 Zielona G´ora, Poland [email protected]

19.1 Introduction Recently, an interest has increased in applying redundant manipulators in useful practical tasks which are speciﬁed in terms of a time parameterized geometric path (a trajectory) to be tracked by the end-eﬀector. 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 conﬁgurations when the manipulator moves. An eﬀective approach to the motion control problem for redundant robotic manipulators is the so-called kinematic control. It is based on an inverse kinematic transformation which determines a reference joint (manipulator) trajectory corresponding to the end-eﬀector trajectory given in the task space. One may distinguish between several approaches in this context. Among them, we mainly concentrate on two major approaches. The ﬁrst approach is the extended or augmented task space formulation of the inverse kinematics problem 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.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 219–226, 2007. c Springer-Verlag London Limited 2007 springerlink.com

220

M. Galicki

joint velocity to become unbounded even though the manipulator is not in a singular conﬁguration. 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, application of pseudo-inverse techniques is computationally time consuming. In order to tackle the singular conﬁgurations potentially met along the end-eﬀector 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 suﬀers from errors due to long-term numerical integration drift. This study considers the problem of kinematic control of a redundant manipulator in real time so that it tracks the prescribed end eﬀector trajectory and simultaneously fulﬁls 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 trajectory 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 stability 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. Finally, some concluding remarks are made in Section 19.5.

19.2 Kinematic Control of Redundant Manipulator The aim is to follow by the end-eﬀector 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 conﬁguration); 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 diﬀerential form as follows: ˙ Jq˙ = −λe + φ(t),

(19.2)

where J = ∂p(q)/∂q is the (m × n) Jacobian matrix; λ denotes a scalar positive coeﬃcient; e = (e1 , . . . , em )T = p(q)−φ(t) stands for the error of the end-eﬀector trajectory tracking. In this section, we assume that J is a full rank matrix during the trajectory tracking. A method to enforce fulﬁlment 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 ˙ q˙ = q˙ R q˙ F . Thus, vector obtained by excluding q˙ R from vector q; vector q˙ R may be determined from Eq. (19.3) and then put at the ˙ As a result, the following equation is obtained after velocity vector q. 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 diﬀerential 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-eﬀector trajectory φ, then solution q = q(t) to the diﬀerential 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 ˙ ˙ − he, φi. V˙ = he, Jqi 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 F ˙ ˙ V = e, [J J ] −λe + φ − q˙ F − 0(n−m)×m −In−m ˙ = −λhe, ei. he, φi

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 representing 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 ∂W (J ) J F q˙ = c , (19.6) −In−m ∂q where c is a positive coeﬃcient. Combining (19.4) and (19.6), we obtain the following generalized kinematic controller minimizing additionally criterion W (·): R −1 F R −1 F T ∂W (JR )−1 (J ) J (J ) J ˙ q˙ = −λe + φ − c . 0(n−m)×m −In−m −In−m ∂q (19.7)

19 Generalized Kinematic Control of Redundant Manipulators

223

As is seen, there are signiﬁcant diﬀerences 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 diﬃcult to see, the order of computational complexity of controller (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 conﬁguration 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 deﬁned, based on a user-speciﬁed small number. To be precise, let U = {q : S(q) < ǫ}, where S(q) = det JR ; det(·) stands for the determinant of (·); ǫ is a user-deﬁned suﬃciently small number, deﬁne a singular neighbourhood around the singular manifold 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 accomplished and the manipulator performs a singularity-free motion. As soon as conﬁguration 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 perturbation is proposed to avoid singular conﬁgurations (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 , β v˙i = −ci vi + ui ,

i = 1 : 2,

(19.9)

where vi stands for auxiliary scalar functions whose role is to continuously 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 ﬁnd controls ui , i = 1 : 2, which (locally) avoid singular conﬁgurations.

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 twodimensional 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-eﬀector 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 conﬁguration q0 equals q0 = (−π/2, π/2, −π/2)T . Matrix JR has been constructed from the ﬁrst two columns of Jacobian J. Hence, q˙ F = q˙3 . The gain coeﬃcient λ 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 ﬁrst 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-eﬀector trajectory tracking is achieved. In the second simulation, the generalized 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 coeﬃcients 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 ﬁrst 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.

19 Generalized Kinematic Control of Redundant Manipulators 0.2

225

e 1 e2

0.15

e1, e2 [m]

0.1

0.05

0

−0.05

−0.1 0

1

3

2

5

4

t [s]

Fig. 19.1. Tracking errors e – kinematic controller (19.4) 0.2

e 1 e2

0.15

e1, e2 [m]

0.1

0.05

0

−0.05

−0.1 0

1

3

2

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-eﬀector. The control generation scheme has been derived using the Lyapunov stability theory. An advantage 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 stable (by fulﬁlment of practically reasonable assumptions). The proposed kinematic controllers have been applied to a planar redundant manipulator of three revolute kinematic pairs. Numerical simulations have shown that the results obtained are in accordance with the theoretical 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 conﬁguration 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 redundant manipulators, Mech. Mach. Theory, vol. 26, no. 2: 221–226 3. Nenchev,D. (1992) Restricted Jacobian matrices of redundant manipulators in constrained motion task, Int. J. Rob. Res., 11: 584–597 4. Perdereau, V., C. Passi, and M. Drouin (2002) Real-time control of redundant 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 redundant manipulators with on-line end-eﬀector path tracking capability under velocity and acceleration constraints, In: Prep. 6th IFAC Symposium 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 acceleration constraints, In: Proc. IEEE Conference on Decision and Control 8. Antonelli G., Chiaverini S. and G. Fusco, (2001) Real-time end-eﬀector path following for robot manipulators subject to velocity, acceleration, and jerk joint limits. In: Proc. 2001 IEEE/ASME International Conference on Advanced Intelligent Mechatronics 9. Nakamura, Y. and H. Hanafusa (1986) Inverse kinematic solutions with singularity robustness for robot manipulator control, J. Dyn. Syst. Measurements and Control, 108: 163–171 10. Wampler, C. W. and L. J. Leifer (1988) Applications of damped leastsquares methods to resolved-rate and resolved-acceleration control of manipulators, J. Dyn. Syst. Measurements and Control, 110: 31–38 11. Krstic,M., I. Kanellakopoulos, P. Kokotovic (1995) Nonlinear and adaptive 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 ﬁlters [1] have become extremely popular for position estimation. These are simple to program, can process raw sensor data and can handle any probability distributions. A good tutorial on particle ﬁlters is [2]. Particle ﬁlters 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 eﬃciency. The particle ﬁlters require a large number of particles in order to accurately estimate the state. This negates their advantage in real-time applications. Further discussion on computational complexity can be found in [3]. The likelihood updates are the major cause of computational ineﬃciency. The techniques which address this problem can be divided into online and oﬀ-line techniques. Verma [4] uses ∗

This work has been supported in part by the ARC Centre of Excellence programme, funded by the Australian Research Council (ARC) and the New South Wales State Government.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 227–238, 2007. c Springer-Verlag London Limited 2007 springerlink.com

228

T. Yaqub and J. Katupitiya

fewer particles to improve the computational eﬃciency while maintaining 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 eﬃciency. Wu [7] uses an incremental strategy to reduce the computational cost. They pre-compute the environment feature data to make Markov Localization more eﬃcient 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 ﬁner resolution is desirable in discrete representations but this demands more computational and memory resources. A technique called ray casting is also used in beam models of range ﬁnders as described in [8] but this is also very ineﬃcient. An alternative is to shift some of the computational load to oﬀ-line. These techniques are sometimes called pre-caching techniques. In this paper we describe a fundamentally diﬀerent approach for capturing the underlying model of the environment. We use a feature extraction algorithm [12] to represent 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 eﬃcient alternative to beam models used for range ﬁnders. However, the complexity of the environment does not allow any arbitrary feature deﬁnition to be useful enough for multinomial formulation. 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 ﬁlter, 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) , Nc where Nc is the normalizing constant given by: m X Nc = p(zt |xt , map)[k] p(xt |ut , xt−1 , map)[k] . p(xt |zt , map) =

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 ﬁlters 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]

p(xt |ut , xt−1 , map) → xt , . . . , xt

[1]

[m]

: 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 brieﬂy, we get the multinomial approximation of the feature observations in the environment by obtaining both of the parameters required for a multinomial distribution. 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 problem is that the complexity of the environment does not allow us to use any arbitrary feature deﬁnitions for a multinomial formulation. This problem is investigated ﬁrst in the next section.

230

T. Yaqub and J. Katupitiya

20.3 Feature Selection Extracting features from raw laser data is a common ﬁrst-step tradeoﬀ 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 oﬀ-line, we can start with as many features as we like without computational considerations. Further 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 measurement 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 deﬁne 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 deﬁnitions are quite arbitrary and we try to ﬁnd τ features feasible for multinomial formulation, where τ < T . It should be noted that the features would be treated as numbers, and the units are included in the deﬁnition of features. If later we ﬁnd that a feature cannot be used, then we have two options, either to tweak the deﬁnition or drop this feature. To check the suitability, we deﬁne the following parameters for all T features and determine their values: • • • •

Mean Mean Mean Ratio

before bootstrap M1 after bootstrap M2 to standard deviation of feature vector η1 of mean to its standard deviation after bootstrap η2

20 Parametric Representation of the Environment of a Mobile Robot •

•

231

Suitability check Υ (This determines whether a feature can be used for multinomial 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 ﬁrst 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 ﬁle 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 deﬁned above. Table 20.1. Statistical data table L s µl µc c P aram n 3.53 1.67 10.80 1528 466.1 2682.7 M1 1.63 0.96 0.66 1.65 0.041 2.07 η1 3.53 1.67 10.78 1527.6 451.9 2682.4 M2 51.49 29.63 20.33 52.11 1.28 64.40 η2 Y N Y Y Y Y Υ 15 N/A 27 11 2 4 M

232

T. Yaqub and J. Katupitiya

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 ﬁrst introduced 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 deﬁne the conﬁdence 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 diﬀerent resulting number of features. For example if we change the length of a straight line, beyond 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 diﬀerent units for thresholds of diﬀerent features, diﬀerent voting schemes, removing highly correlated features and even changes in conﬁguration of the laser scanner can be used for tweaking if desired.

20 Parametric Representation of the Environment of a Mobile Robot

233

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 ﬁnd 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ˆij (1 6 i 6 n and 1 6 j 6 τ ). Get pij = Fˆij /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 environment (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 eﬃciency in twofold manner, ﬁrst 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 operations 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 probabilities 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 ﬁlter. If our current ′ observation vector is Z = (z1 , z2 , ....., zτ ) , then the updated weights Wk to these m particles are assigned as follows: Wk = N !

[k] τ Y pj

j=1

zj

zj !

k = 1, 2...m

(20.1)

234

T. Yaqub and J. Katupitiya

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 device 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 ﬂoor 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 diﬀerent 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 scenarios shown in Fig. 20.3 (a) (simulated obstacle-free environment denoted 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 possible 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.

20 Parametric Representation of the Environment of a Mobile Robot

(a)

235

(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 diﬃculty 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. SOF

[1]

xt [2] xt [3] xt [4] xt [5] xt True Pose

SWO

L

Cell Probabilities

Particles

p1 (-1.4,-0.3,0.035) (-0.38,2.1,4.36)

(1,0.8,1.152 )

(-1.6,0.1,0.436) (-0.35,1.4,4.01) (0.8,0.8,0.576 )

(-2.5,-0.2,0.262) (0.23,1.5,4.18)

(0.6,0.8,1.745)

(-2.3,0.1,0.175)

(-0.1,1.6,4.53)

(1,1,0.35)

(-1.8,0.2,0.401)

(0.2,2.2,4.45)

(0.6,0.6,0.698)

(-1.7,-0.1,0.35)

(-0.1,1.8,4.57)

(0.8,0.8,0.785)

[1]

xt [2] xt [3] xt [4] xt [5] xt Curr. Observ.

p2

0.21 0.2

p3

p4

Weights

p5

0.08 0.08 0.43 0.0072

0.25 0.17 0.1

0.07 0.41 0.0135

0.28 0.23 0.1

0.09 0.3

0.9583

0.18 0.11 0.21 0.11 0.38 0.00018

0.22 0.15 0.9

4

4

2

0.05 0.49 0.0029

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 formulation, 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 address 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 ﬁlter 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 diﬀerent and novel approach for extracting the model of the environment of a mobile robot. It provides the advantage of using one of the standard multinomial probability distributions for the likelihood function

236

T. Yaqub and J. Katupitiya

in the measurement update stage of a particle ﬁlter. This eliminates sampling from posterior and ray casting and is an alternative to beam models used for range ﬁnders. In beam models, the conditional density p(zt |xt , map) is a mixture of many diﬀerent densities, each of which corresponds to a particular type of error. The proposed method makes the measurement model simple and eﬃcient. Most of the modelling could be done oﬀ-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 ﬁlters 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 ﬁlters. Proc. of SPIE Int. Soc. for Optical Eng., 4728:418-426, 2002. 4. V. Verma, G. Gordon, R. Simmons, S. Thrun. Partcle ﬁlters for rover fault diagnosis. Robotics and Automation Magazine, June 2004. 5. D. Fox. Adapting the sample size in particle ﬁlters through kld-sampling. Int. J. of Robotics Research, 22(12):985-1003, 2003. 6. C. Kwok, D. Fox, M. Meila. Adaptive real-time particle ﬁlters 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 computational 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 localization for mobile robots. Artiﬁcial 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 Intelligent Robots and Systems, Beijing, China, 2006.

20 Parametric Representation of the Environment of a Mobile Robot

237

14. T. Yaqub, M.J. Tordon, J. Katupitiya. A procedure to make the probabilistic 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, Portugal, 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 Tesli´c, Gregor Klanˇcar, and Igor Skrjanc Faculty of Electrical Engineering, University of Ljubljana, Trˇzaˇska 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 ﬁnder 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 algorithm is developed and tested in a simulation. The line segments, extracted 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 environment. 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 environment, 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 environment 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 localizaK. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 239–246, 2007. c Springer-Verlag London Limited 2007 springerlink.com

240

ˇ L. Tesli´c, G. Klanˇcar, and I. Skrjanc

tion and mapping are merged. The robot can localize itself by odometric measurements and by comparing the local map, obtained from the current 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 diﬀerent 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 information about the way to reach a connected topological location. In [3] the metric and topological paradigm are integrated into a hybrid system 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 simple 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 rangeﬁnder is reported. In [6] the environment is represented by polygonal curves (polylines), possibly containing rich shape information for matching environment scans. However, some environment 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 ﬁnder, and the environment. The purpose of the modelling is to create a simulation model where diﬀerent 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 according to the global coordinate axis, xG . The continuous model (Eq. (21.2)) is implemented in Matlab Simulink with a simulation scheme using 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 environment 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 deﬁned 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 ﬁnder 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 reﬂection 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

242

ˇ L. Tesli´c, G. Klanˇcar, and I. Skrjanc yG

yG laser beam line at angle q(i)

q(i)

a Environment line y

v yR jrob

d

(xrob ,yrob)

w

(xip(i,j) ,yip(i,j))

R

(xrob ,yrob)

xR

Environment line segment

xR

Laser beam line zG

xG

xG

zG

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 segment to the robot (xrob , yrob ) is chosen as the reﬂecting point. Because the intersections between the lines are calculated, the intersections behind 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 reﬂection 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 reﬂecting point of the laser beam. We deﬁne 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 dnoise (i) = d(i) +

d(i) N (0, σ), dmax

(21.7)

where N (0, σ) denotes the normal distributed noise with zero mean and σ variance. If the distance d(i) is large, the inﬂuence of the noise is proportionally larger. In this way we can test the robustness of mapping algorithms without knowing the real noise distribution.

21 Simulation of a Mobile Robot with an LRF in 2D

243

21.3 Mapping Algorithm Assuming that the robot knows its own pose (Eq. 21.1)) the calculated reﬂection point (xG (i), yG (i)) according to the global coordinates is xG (i) = xrob + d(i) cos θG (i), θG (i) = (ϕrob − 90◦ ) + θs (i),

yG (i) = yrob + d(i) sin θG (i), i = 1, . . . , n,

(21.8)

where θG (i) denotes the laser-beam angle according to the global coordinate frame (Fig.21.1(a)). All consecutive points (21.8) by which a reﬂection 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 cluster 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 ﬁtting. The line segments are deﬁned 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 .

G2 G1

G= (G1,G2,G3)

G=(G1,G2,G3)

GNEW= S U GOLD= (S1,S2) U (G1,G2) = (G1,G2,S1,S2) G2

G2

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 local map SF IRST , obtained in the ﬁrst environment scan. When uniting

ˇ L. Tesli´c, G. Klanˇcar, and I. Skrjanc

244

S and GOLD each line segment of the set (GOLD , S) is compared to each line segment in one loop. We deﬁne the conditions for merging the line segments L1 and L2 (Fig. 21.3), which correspond to the same environment 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 satisﬁed and if all conditions ci (i = 5, 6, 7, 8) are satisﬁed, two line segments are merged. If the conditions c1 and c2 or the conditions c3 and c4 are satisﬁed, two line segments are also merged. When merging 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 diﬀerent unions, the unions are merged (Fig.21.2, G1N EW = G1OLD ∪ S2). The loop of comparison is then repeated. If the conditions for merging any two segments are not fulﬁlled in the next loop, the loop is stopped. L1

d r4

c

a r3

r2 L2

b

r1 R

c1 : a + b < l(L1) + T, c3 : a + c < l(L2) + T, c5 : r1 < R, c7 : r3 < R,

c2 : c + d < l(L1) + T, c4 : b + d < l(L2) + T, c6 : r2 < 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 representation 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 localization 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 according 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 ﬁnd the corresponding pair of lines. In large environments this can take a huge number of comparisons (e.g., 1000 × 10). If the sum of the squared diﬀerences (α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 segments that are seen in the current environment scan that correspond to

21 Simulation of a Mobile Robot with an LRF in 2D

245

objects (unions Gi) seen in the previous environment scan. The search strategy can be faster if the line pairs for the comparisons are ﬁrst 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 environment. Figures 21.4(b), (c), and (d) show experiments at diﬀerent values of LRF noise variance σ (0.07 m, 0.42 m and 0.63 m) and a ﬁxed 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 environment 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) Reﬂecting 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.

246

ˇ L. Tesli´c, G. Klanˇcar, and I. Skrjanc

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 representation 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 environment object with a union of line segments. In this way the search strategy to ﬁnd 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 Artiﬁcial 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 localization 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 Rangeﬁnder for Indoor Mobile 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 Ludwik´ow and Ignacy Dul¸eba Institute of Computer Engineering, Control, and Robotics, Wroclaw 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 Pfaﬀ form, can be described as driftless systems u= q˙ = G(qq )u

m X

gi (qq )ui , dim u = m < dim q = n,

(22.1)

i=1

where some components of velocities q˙ deﬁne controls u . A motion planning task in a cluttered environment is to ﬁnd the controls steering system (22.1) from the given initial conﬁguration q0 to the ﬁnal 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 developed 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 algorithms of motion planning have been developed cover chain-systems, ﬂat systems, or nilpotent (or nilpotentizable via a feedback transformation). 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 conﬁguration space polluted with obstacles [2]. An extensive overview of nonholonomic motion planning methods can be found in [1, 8]. K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 249–258, 2007. c Springer-Verlag London Limited 2007 springerlink.com

250

P. Ludwik´ ow and I. Dul¸eba

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 ﬁrst step, any admissible (holonomic) path is generated. Then, with the method of an elastic band technique, introduced in the ﬁeld of robotics by Quinlan and Khatib [11], the path is iteratively reshaped to minimize its Euclidean length and preserve a safe distance from obstacles. 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 conﬁguration 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 CampbellBaker-Hausdorﬀ-Dynkin formula (gCBHD), which makes it possible to express a local motion as a function of vector ﬁelds (locally - directions) multiplied by control-dependent coeﬃcients. Usually controls are expressed in, appropriately truncated, a control basis. Consequently, controls are parametrized and searching for controls is substituted with searching for their coeﬃcients. Usually harmonic functions are used for this purpose. With the aid of the gCBHD formula, it is not diﬃcult 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 presented method does not need to satisfy any other properties besides the small time local controllability. As most of the computations can be performed oﬀ-line, its complexity is rather low. As the basic (Liealgebraic) 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 easily updated to cover inadmissible (due to obstacles) islands in the con-

22 Nonholonomic Motion Planning in a Cluttered Environment

251

ﬁguration 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 ﬁeld, degree of a vector ﬁeld, Ph. Hall basis, P H, Lie algebra LA, etc.) [4]. To avoid complicated formulas, all the discussed concepts will be presented for two-input control systems (22.1) (n = 2, g1 = X, g2 = Y ). The generalization is straightforward. The key theorem exploited to steer systems (22.1) is the Lie algebra rank condition LARC deﬁning 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 diﬀerential equations with a given initial condition. The systems also cover the case of Eq. (22.1). The gCBHD formula is a transport operator that, for small time horizon T of a local (one-step) planu (·), T )(qq0 ), ning, translates a current state q0 to the position q0 + z(u 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 coeﬃcients [4] z(u(·), T ) = α1 X + α2 Y + α3 [X, Y ] + . . . .

(22.3)

Coeﬃcients α = (α1 , α2 , α3 , . . .) depend on the controls as follows: α1 =

ZT

u(s)ds,

1 2

ZT Zs2

0

α3 =

α2 =

ZT

v(s)ds,

0

s2 =0 s1 =0

(22.4)

(u(s1 )v(s2 ) − v(s1 )u(s2 ))ds1 ds2 .

When the gCBHD is applied at state q 0 , all vector ﬁelds in Eq. (22.3) must be evaluated at that point. Usually, controls are coded with their coeﬃcients. To this aim any basis (harmonic, polynomial, piecewiseconstant) can be selected. The vector of parameters of all controls will be denoted p . Substituting the controls into Eq. (22.4), a function α = k (pp) can be established. As a rule, when the direction of

252

P. Ludwik´ ow and I. Dul¸eba

motion is known at a current state, then from Eq. (22.3) coeﬃcients α can be determined. In the general case the number of parameters, equal to dim p, is larger than the dimensionality of the conﬁguration space (n), the weighted generalized Moore-Penrose matrix inversion, [10] is required to ﬁnd p . We use the weighted inversion rather than the usually applied classical inversion because it can be proved [4] that the coeﬃcient corresponding to a vector ﬁeld A depends on (small) T as follows α ∼ T degree(A)/2 . (22.5) It means that low degree vector ﬁelds are more eﬀective in the state translation than those of higher degrees. Consequently, low degree vector ﬁelds should be preferred to generate motion. It can be done with the use of weighting coeﬃcients 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 ﬁeld 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 = q i + ∆qi i where ∆qi results from the minimization of the criterion function: q i+1 − q i q i−1 − q i + (22.6) F = ξ1 Fel + ξ2 Frep , Fel ≃ kqq i−1 − q i k kqq i+1 − q i k

with positive weighting coeﬃcients ξ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 inﬁnity when the point is in close vicinity of any obstacle.

22 Nonholonomic Motion Planning in a Cluttered Environment

253

2d: The path composed of points q next , i = 0, . . . becomes the new i current path. 2e : If the new path is very close to the old one, stop the computations 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 ﬁelds (from the Ph. Hall basis) at q ini required to satisfy the LARC, (22.2). In this way a ﬁnite set of vectors V = {vi , i = 1, . . . , I} (vector ﬁelds 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 ﬁeld represented in the set vi ∈ V , compute (using the gCBHD formula) its coeﬃcient α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ﬂuence the shape of a local trajectory not inﬂuencing its endpoint 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: I X 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) ﬁnd the vector of parameters p⋆ that generate the desired value αd = k (pp⋆ )). of αd , (α 5f : For parameters p⋆ compute controls u (pp⋆ , ·) and check whether integral curve ϕ(qqini , u (pp⋆ , ·), t) initialized at q ini and corresponding 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 satisﬁed goto Step 6.

254

P. Ludwik´ ow and I. Dul¸eba

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 concatenation of the trajectories obtained by the consecutive local planning tasks) and ﬁnish 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 satisﬁed. Note that (Step 5a) not only those vector ﬁelds that satisfy the LARC should be taken into account but also all those with their degrees as large as the highest degree of the vector ﬁeld involved in checking the LARC; this is a straightforward consequence of Eq. (22.5). The extra vector ﬁelds impact (22.3) to the same extent as some vector ﬁelds used to check the LARC. The convergence of the algorithm is guaranteed if only the LARC condition is satisﬁed 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 ﬁelds surpass the impact of higher-degree vector ﬁelds 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 possible. 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

22 Nonholonomic Motion Planning in a Cluttered Environment

255

must be at least equal to n. When the number is increased, the complexity 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 oﬀ-line. While planning an initial path (Step 2) an extra nonholonomic criterion can also be taken into account, cf. Eq (22.6). Evaluation of a path with respect to a nonholonomic measure is described in [5].

22.4 Simulation 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 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 linear 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 conﬁguration, so the LARC (22.2) is satisﬁed. For all the tests the time horizon T was set to 1 for each local planning 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 + a1 sin(ωt + ϕ), u(t) = a0 T T (22.11) r r 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 coeﬃcients α as a function of the parameters of the controls

256

P. Ludwik´ ow and I. Dul¸eba

√ T a0 , α2 = T b0 , T √ α3 = 2(a1 b0 cos ϕ − b1 a0 cos ψ) + a1 b1 sin(ϕ − ψ) . 2π

α1 =

√

(22.12)

Equation (22.12) is redundant (dim p = 6 > 3 = dim q). To maximize coeﬃcient α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 ﬁxed values of components α1 , α2 (coeﬃcients a0 , b0 are ﬁxed 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 ﬁrst 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 ﬁeld [X, Y ] only, therefore parameters a0 = b0 = 0, and the free parameter is ϕ. In Fig. 22.1 paths for varied ϕ are presented. When an obstacle is placed in the conﬁguration space, the same task can be performed with diﬀerent sets of control parameters. A resulting path can be the safest possible or rather dangerous. The motion planning task was to ﬁnd the controls steering the system (22.10) from the given initial state q0 = (−10, −2, 0◦ )T to the ﬁnal state qf = (0, 0, 0◦ )T . The ﬁnal point was assumed to be reached when the distance from the end-trajectory point to the ﬁnal state was smaller than δ = 0.01. The resulting path and controls producing the path are presented in Fig. 22.2. The generated controls are piecewisecontinuous. 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 180 225 270 315

0

-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 ϕ[◦ ]

22 Nonholonomic Motion Planning in a Cluttered Environment

257

2

a)

b)

obstacles path

u v

0 1

-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 ﬁrst, it plans a holonomic, obstacle free path. Then the path, after partitioning, is generated with controls by the local Lie algebraic method. The proposed algorithm solves the motion planning task when simultaneously the LARC is satisﬁed and the paths generated by the algorithm are surrounded by an open neighborhood of free conﬁguration 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. Dul¸eba I. (2006) Making controls generated by the Lie-algoebraic methods continuous, Nat. Conf. on Robotics, 53–60 (in Polish) 4. Dul¸eba I., Kheﬁﬁ W. (2006) Pre-control form of the generalized CampbellBaker-Hausdorﬀ-Dynkin formula for aﬃne nonholonomic systems, Systems and Control Letters, 55(2), 146–157 5. Dul¸eba I., Ludwik´ ow 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

258

P. Ludwik´ ow and I. Dul¸eba

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 Control, IEEE Conf. Robotics and Automat., vol. 2, Atlanta, 802–807 12. Sussmann H . (1993) A continuation method for nonholonomic pathﬁnding problems, IEEE Conf. Dec. Contr., San Antonio, 2718–2723

23 Computationally Efficient Path Planning for Wheeled Mobile Robots in Obstacle Dense Environments Husnu T¨ urker S ¸ ahin and Erkan Zergero˘glu 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 signiﬁcantly in the recent years. The nonholonomic constraints of WMRs impose diﬃculties for eﬀective path planning; on top of this, the presence of static and/or dynamic obstacles in operation environments complicates the problem further. Alternative 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 provide dynamical path planning [3, 5], which is impractical for general applications of WMRs. Recently more advanced methods have been presented that oﬀer 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 scanners. 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 eﬃcient 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 nonholonomic reference trajectory for unicycle robots to follow as the front axle is steered in the direction of the device target. A simple yet eﬀective K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 259–268, 2007. c Springer-Verlag London Limited 2007 springerlink.com

260

H.T. S ¸ ahin and E. Zergero˘ glu

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 sensors 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 obstacle 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 ﬁll the concave sections of u-shaped obstacles, thereby eliminating the need for complicated mapping algorithms. 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 Section 23.2 the employed WMR kinematic model and the problem formulation 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 constraint for accurate tracking by WMRs, x˙ c cos θ − y˙ c sin θ = 0.

(23.3)

23 Computationally Eﬃcient Path Planning for Mobile Robots

261

The main objective of our path planner is to generate a nonholonomic collision-free path in an unstructured obstacle dense environment 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 generating 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 modiﬁed terms in (23.1) are the linear velocity which is the projection 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 + B e. ˙ (23.5) Fs = K p kek2 + ε

Here K, B ∈ R2×2 are diagonal positive deﬁnite scaling matrices, and kek denotes the Euclidean norm of the error term e, deﬁned as the difference between the desired front axle COM Pd and its current position P as follows: e = Pd − P = [ xed − xe , yed − ye ]T .

(23.6)

262

H.T. S ¸ ahin and E. Zergero˘ glu

y

L

Pd

y C1

Fs

Cd

P

ĳ L

R1

Cr

Fo2

C2

P ș

R2

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 proportional to the distance between the terminal point and current front end location P , thereby counterbalancing any decline in the forcing function 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 obstacle avoidance algorithm operates eﬀectively 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. Depending 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 duration of their signals are utilized. Two such obstacle estimates are depicted 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 ,

23 Computationally Eﬃcient Path Planning for Mobile Robots

263

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: Fo (t) =

N X i=1

Foi (t) =

N X i=1

wi Ri (t) [P (t) − Ci (t))], i = 1, . . . , N,

(23.7) where Ri , Ci , and P are the terms deﬁned above, and wi ∈ denote additional weights selected higher for front sensors to emphasize avoidance in the robot forward path. The ratio of each force component Fo in (23.7), increases according to the impact time of the related obstacle. 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 imperative for the nonholonomic controllers. If the WMR is encircled by some ﬁxed 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. R+

Remark 1. The obstacle avoidance forces Foi should not be ceased immediately 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 carried out in a 2-stage period. In the ﬁrst stage, a small virtual margin is added to the estimated radius of the obstacles. Thus the planner continues 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 ﬁrst order spline-like transition. The overall obstacle steer force function F = [Fx , Fy ]T in this period is as follows: F = Fs

(ts + δt − t) (t − ts ) + Fo , δt δt

(23.8)

where Fs and Fo are the front steering and normalized obstacle avoidance forces in (23.5) and (23.7). This cross-over phase is conﬁned 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 .

264

H.T. S ¸ ahin and E. Zergero˘ glu

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. However, 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 parameters δt and △t of the spline period. Moreover, this modiﬁcation also improves planner performance in complex tunnels. Thus if the algorithm 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 obstacles to ﬁll 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.

23.4

Simulation Results

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 diﬀerent topologies have been performed. ON-OFF sensors located at the robot 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 conﬁgurations. Likewise the major and minor axis lengths

23 Computationally Eﬃcient Path Planning for Mobile Robots

(a) Top down view of the environment

(c) Generated path

265

(b) Linear and angular velocities

(d) Pose tracking errors

Fig. 23.2. The ﬁrst 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, K = diag(0.165, 0.165),

b = 20 cm, 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 uniﬁed nonholonomic controller in [11] is utilized in our simulations. The ﬁrst 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, respectively. Figure 23.2(b) depicts the WMR linear and angular velocities, which are functions with no chattering owing to the spline transition in

266

H.T. S ¸ ahin and E. Zergero˘ glu

(a) Low and ﬁxed △t = 0.6, δt = 2 s

(b) Virtual obstacle △t, δt incremented

Fig. 23.3. Simulations on u-blocks

(a) Short spline times of △t = 0.8, δt = 4 s

(b) Longer transition △t = 1, δt = 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 ﬂuctuations during obstacle manoeuvres. Next are the simulations verifying the positive inﬂuence of readjusting △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 encounters a u-barrier, where it is trapped. This is caused by the small, ﬁxed 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

23 Computationally Eﬃcient Path Planning for Mobile Robots

267

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 ﬁnal location with no further delay. The ﬁnal 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 ﬁrst 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 eﬀectively for large concave blocks and complex tunnels.

23.5 Conclusions We have presented a simple yet eﬀective 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 obstacle avoidance problem, obstacles detected by the sensors are assumed as of circular geometry, and repulsion forces from their estimated centers 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 conﬁrm the computational eﬃciency 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 Planner for Nonholonomic Mobile Robots, IEEE Trans. on Robotics and Automation, 10(5), October 1994.

268

H.T. S ¸ ahin and E. Zergero˘ glu

3. R. M. Murray and S. S. Sastry, Nonholonomic Motion Planning: Steering Sinusoids”, IEEE Trans. on Automatic Control, 38(5), 700-716, May 1993. 4. E. Rimon and D. E. Koditschek, Exact Robot Navigation using Artiﬁcial 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: collision 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 Algorithm 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 Robots, 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 Tracking 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 Nonlinear 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 Dul¸eba Institute of Computer Engineering, Control, and Robotics, Wroclaw University of Technology, Janiszewski St. 11/17, 50-372 Wroclaw, Poland [email protected]

24.1 Introduction Driftless, nonholonomic systems are frequently encountered in robotics and they are described by the equations q˙ = G(q)u =

m X

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 ﬂow ϕt,q0 (u(·)) deﬁned 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 ﬁnd controls steering the system (24.1) from the given initial conﬁguration q0 to the ﬁnal state qf . With kinematics deﬁned by Eq. (24.2), the motion planning of nonholonomic systems (24.1) can be considered as the inverse kinematics 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 ﬁeld 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 K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 269–278, 2007. c Springer-Verlag London Limited 2007 springerlink.com

270

I. Dul¸eba

eﬀectively 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 coeﬃcients. 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 deﬁned, 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 nonholonomic systems in a similar way as it was used for holonomic manipulators [8]. The algorithm modiﬁes 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 ﬁnal state qf is reached and the trajectory planning task is solved. Otherwise, controls ui (·) are modiﬁed according to the formula (24.3). The parameter ξi is a small positive number inﬂuencing the pace of convergence of the algorithm (24.3). A straightforward application of formula (24.3) requires the approximation of controls ui (·) in the interval [0, T ]. To this aim a functional basis is selected and controls are described as a series. Usually harmonic 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 coeﬃcient of a harmonic control impacts the shape of the whole trajectory. Also constraints on controls are diﬃcult to transform into constraints on parameters of harmonic 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 deﬁned by the expression A(t) =

∂(G(q(t))u(t)) , ∂q

B(t) = G(q(t)),

(24.5)

while the fundamental matrix Φ(t, s) is the solution of the equation d Φ(t, s) = A(t)Φ(t, s), dt

Φ(s, s) = I n

(24.6)

(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), ∆s→0

(24.7)

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 deﬁned 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 ﬂoor 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 modiﬁed 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 deﬁned everywhere in the state space) and the discretization has been selected appropriately. The Newton algorithm for nonholonomic systems heavily depends on linearization

272

I. Dul¸eba

of the system (24.1) along the current trajectory. The main diﬀerence between holonomic and nonholonomic applications of the Newton algorithm is that computing kinematics and the Jacobian matrix of nonholonomic system is much more involved because numerical integration 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 modiﬁcation of the basic Newton algorithm (24.10) is to vary the value of coeﬃcient ξ. The optimal value ξ ⋆ in the i-th iteration should be determined in one dimensional optimization 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 possible. Implementation of (24.11) requires many integrations of the sys tem (24. 1) corresponding to varied ui+1 (ξ, ·) but does not need computing A(t), B(t), Φ(T, ·). The basic Newton algorithm may modify any piece of controls in any iteration. Let us restrict changes allowed and deﬁne the index sets: F = {1, . . . , m·N } collecting all admissible indices and its subsets Fr , r = n, . . . , m · N composed of r element combinations 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 deﬁned: T manr = max det(J{f } J{f } ), f ∈Fr

r = n, . . . , m · N.

(24.12)

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 ⋆ any single manr , i.e. manr = det(J{f ⋆ } J{f ⋆ } ). 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 (24.15) } ) = det(J{f ⋆ } J{f ⋆ } + g g ).

24 Piecewise-Constant Controls for Newtonian Motion Planning

273

Both added matrices are non-negative deﬁnite, 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 deﬁning manr+1 .

Remark 2: Computation of mank is very demanding as it requires computing m·N determinants. (The heaviest computational load is k 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 inverse (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 being 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 components. Moreover, the representation can vary from one iteration to another. Consequently, it is one more advantage of using piecewise constant controls over harmonic ones (that ﬁx 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 segments (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 segment and only one control. Obviously, one can not guarantee a mo-

274

I. Dul¸eba

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 reduce 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 eﬀective length to the goal decrease (the denominator) in the expression cos αi cos α⋆ R cos αi /xi = min = ⋆ . min i=1,...,N ·m xi (1 − sin αi ) i=1,...,N ·m R − R 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 control modiﬁcation can be coupled with the optimization process (24.11) to get the new, minimal control eﬀort, 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 modiﬁcation 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 modiﬁed). Consequently, the computational complexity of the minimal control eﬀort 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

24 Piecewise-Constant Controls for Newtonian Motion Planning

      x˙ cos θ 0 y˙  =  sin θ  u1 + 0 u2 = X u1 + Y u2 . 0 1 θ˙

275

(24.18)

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 orientation. 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 ﬁnd controls steering the system (24.18) from a given initial state to the ﬁnal state qf = (0, 0, 0◦ ). The ﬁnal point was assumed to be reached when the distance from the end-trajectory point to the ﬁnal state was smaller than 0.01. The aim of the ﬁrst simulation was to evaluate manipulability indices deﬁned. 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 number of segments equal to 10. The ﬁnal solution was obtained in 10 iterations. The ﬁnal 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 ﬁnal state). As can be seen, the minimization of the distance to the goal does not necessarily force the end point of the current trajectory to move along the straight line from the ﬁnal 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 derive manipulability index increases monotonously, the index increases as well; at the very beginning linearly, then it gets stabilized. In this particular simulation, for ﬁxed 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 ﬁnal trajectory. It is not a rule, as the manipulability for ﬁxed 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 initial controls when the trajectory stretches out to get its ﬁnal shape, the regularity may manifests itself easily. The values of the indices can

276

I. Dul¸eba

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)

b) 50

6

40

5

30 4 20 10 u

u

3 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

t

c)

0.8

1

t

18

d) 450

16

400

14

350

12

300 man

y

10 8 6

250 200

4

150

2

100

0 -2 -5

50

0

5

10

15 x

20

25

30

35

0 2

4

6

8

10

12

14

16

18

20

k

Fig. 24.2. Initial (a) and ﬁnal (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 quality 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 ﬁrst 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 controls were as in the previous example. The ﬁrst 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.

24 Piecewise-Constant Controls for Newtonian Motion Planning b)

80

80

70

70

60

60

50

50 y

y

a)

40

40

30

30

20

20

10

10

0 20

40

60

80

100

120

x

140

160

180

200

277

0 -20

0

20

40

60

80 100 120 140 160 180 200 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ﬁned and ordered. The new, minimal control eﬀort 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., Ludwik´ ow P. (2006) Bases for local nonholonomic motion planning, 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, Mi¸edzyzdroje, 485–490 8. Nakamura Y. (1991) Advanced Robotics: Redundancy and Optimization, Addison Wesley, New York 9. Tcho´ n K., Muszy´ nski 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 Institute of Computer Engineering, Control, and Robotics, Wroclaw University of Technology, ul.Janiszewskiego 11/17, 50-372 Wroclaw, Poland [email protected]

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 system is able to realize manipulation tasks in a much larger workspace than a manipulator with a ﬁxed base. Taking into account the type of constraints of both subsystems, there are 4 possible conﬁgurations of mobile manipulators: type (h, h) – a holonomic manipulator on a holonomic platform, type (h, nh) – a holonomic platform with a nonholonomic manipulator, type (nh, h) – a nonholonomic platform with a holonomic manipulator, and ﬁnally 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 desired 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 deﬁned for any subsystem: the end-eﬀector 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 theoretical 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 platform and the manipulator. In Section 25.6 a new control algorithm K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 279–292, 2007. c Springer-Verlag London Limited 2007 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 Pfaﬀ’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 ﬁnd 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 T , q T ), where q denoted as q T = (qm m is a vector of mobile platform r

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 B1 0 τm A + + = λ+ , Q21 Q22 q¨r C21 C22 q˙r D2 0 I τr 0 where A is the Pfaﬀ’s matrix deﬁned 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 velocities (25.2) instead of generalized coordinates for the mobile platform. We compute ˙ m )η, q¨m = G(qm )η˙ + G(q 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 + + q¨r q ˙r Q21 G Q22 C21 G C22 T 0 τm G B1 0 + = (25.6) D2 τr 0 I or the same equations in a more compact form η˙ η ∗ ∗ Q +C + D∗ = B ∗ τ. q¨r q˙r

(25.7)

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 nonholonomic mobile platform a skew-symmetry between inertia matrix Q∗ and the matrix coming from Coriolis and centrifugal forces C ∗ obtained as the Christoﬀel symbols does not hold anymore. To regain the skewsymmetry, a special nontrivial correction matrix CK must be added d ∗ Q = (C ∗ + CK ) + (C ∗ + CK )T . dt

(25.8)

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 ﬁnd 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-eﬀector has to follow a desired geometric path p(s) described in Cartesian coordinates which deﬁnes 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 deﬁned 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 (nonholonomic constraints) if the dynamics were not present. Such a controller generates ’velocity proﬁles’, which can be executed in prac-

25 Path Following for Nonholonomic Mobile Manipulators

283

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 directly, 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 taskspace 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 deﬁned 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 v

θ

y

P

dr ds

M

dr ds

θr

1

l xn

dr ds

2

r2

M’ s

x

r1

X0

Fig. 25.1. Illustration of the path following problem

284

A. Mazur

l˙ = (− sin θr

x˙ cos θr ) , y˙

(cos θr sin θr ) s˙ = 1 − c(s)l

x˙ . y˙

(25.9)

The posture of a wheeled mobile platform is deﬁned not only by the position of its mass center but additionally by the orientation. Therefore orientation tracking error can be deﬁned by θ˜ = θ − θr . The set of Frenet variables ˜ s) ξ T = (l, θ, (25.10)

constitutes the path following coordinates for any wheeled mobile platform 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 deﬁned if the following conditions are satisﬁed [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 ˜ (distance tracking error and orientation tracking error) and pos(l, θ) sibly βi , i = 1, 2 to 0 can be obtained using Pomet kinematic control algorithm. We have omitted the diﬀerential equation for s, ˙ because it does not matter at which point s of the desired path the mobile platform enters the desired curve P [4]. Theorem 1. [7] Let us consider a control system of the form x˙ = G(x)η =

m X

gk (x)ηk ,

(25.11)

k=1

where x ∈ Rn , η ∈ Rm , m = n − l is a number of unconstrained velocities and gk are smooth vector ﬁelds. We assume that the above system satisﬁes the following conditions:

25 Path Following for Nonholonomic Mobile Manipulators

285

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 ﬁeld g1 (x) = ∂/∂x1 in the neighborhood of 0, 3. it is possible to choose a Lyapunov function V (t, x) of the form V (t, x) =

1 1 1 (x1 + h(t, x2 , . . . , xn ))2 + x22 + . . . x2n , 2 2 2

(25.13)

where the most general choice of h(t, x) is h(t, x) = h(t, x2 , ..., xn ) =

n X i=2

!

x2i cos t,

4. vector ﬁelds gk are such that the following conditions can be satisﬁed:  Lgk V (t, x) = dV (t, x)gk (x) = 0 k = 2, . . . , m  ⇒ x = 0. (25.14) ∂ j h(t, x)  = 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)   − Lg1 V (t, x), η1 = −   ∂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 system V˙ (t, x(t)) ≤ 0. (25.16) Theorem 1 deﬁnes a method of designing a control algorithm for the kinematics of any wheeled mobile platform. This controller guarantees ˜ to 0, which is equivalent asymptotic convergence of coordinates (l, θ) 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 speciﬁed by the geometric curve Π(s) for the end-eﬀector. Such a path given in Rρ local workspace is deﬁned 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-eﬀector 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-eﬀector and sometimes its orientation, if some relationship between the orientation and s parameter holds (i.e. orientation 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 conﬁgurations). Similarly 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 manipulators’s end-eﬀector is located on the path, i.e.

k(qr (0)) = Π(0), and that at the initial and ﬁnal 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 deﬁned 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 deﬁned. The ﬁrst of them is possible path parametrization 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 diﬀerential equation 1 ∂γ dp s¨ = −Kd s˙ − Km es es + γ + 2K2 eTp F, F = , (25.18) 2 ∂s ds

25 Path Following for Nonholonomic Mobile Manipulators

287

where Kd , Km , K2 > 0 are some regulation parameters and γ is assumed to be a strictly positive function of s which is required not to satisfy the diﬀerential equation [5] 1 ∂γ es + γ = 0. 2 ∂s

The role of this function is to inﬂuence the behavior of the end-eﬀector 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) computed 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 dynamic coordinates of the mobile manipulator as follows η˙r ηr ∗ −1 ∗ ∗ τ = (B ) Q +C −J T ep −J˙T ep − J T J q˙r + J T F s˙ +D∗ − CK Ev − KEv } ,

where F is deﬁned 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 }, 0 K12

(25.19)

K1 , K2 > 0.

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 equations describing the nonholonomic constraints (25.11) for the path following problem. Now we calculate the time derivative of W

288

A. Mazur

˙ = ∂V + ∂V ξ˙ + EvT Q∗ E˙ v + 1 EvT Q˙ ∗ Ev + s¨ ˙s W ∂t ∂ξ 2 1 ∂γ 2 + e + γes Km s˙ + 2K2 eTp e˙ p . 2 ∂s s

˙ along trajectories of the closed-loop Before we begin to evaluate W system (25.11), (25.15), (25.18), and (25.20), it is necessary to mention the inﬂuence 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 deﬁned by (25.15) we should take modiﬁed 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 ˙ = − (Lg V )2 + eη Lg V + . . . − (Lgm V )2 + eηm Lgm V − eT K11 eη W 1 1 1 η 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 deﬁnite as a consequence of the assumption about avoiding singularities by the manipulator. Now we use LaSalle’s invariance principle to deﬁne a set ˙ (t, ξ, Ev , es , s) N = {(t, ξ, Ev , es , s) ˙ |W ˙ = 0}. Let H be the largest invariant set contained in N . Then setting arguments similar to the proof of convergence of the Pomet algorithm, we

25 Path Following for Nonholonomic Mobile Manipulators

289

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. However, 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 fulﬁlled 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 investigate 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 [m]. s [m], y(s) = x(s) = 2 2

1

Matlab package and the Simulink toolbox were available thanks to Wroclaw Center of Networking and Supercomputing.

290

A. Mazur θ2

Y0

l2 l1

X2

Yb

l3

θ3

θ1

Xb

θ

Yp Xp

Y2

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 3pendulum’s end-eﬀector is depicted in Figs 25.3-25.4. Tracking of the desired path for the mobile platform is presented in Fig. 25.5.

a)

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

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

25 Path Following for Nonholonomic Mobile Manipulators 0.02

291

0.015

0.018

0.016

0.01

0.014

0.012 [m]

[m]

0.005

e

e

p2

p1

0.01

0.008

0

0.006

0.004

−0.005

0.002

0

a)

0

2

4

6

8

10 TIME

12

14

16

18

−0.01

b)

20

[s]

0

5

10

15 TIME

20

25

[s]

Fig. 25.4. Tracking errors ep = k(qr ) − π(s) for the 3-pendulum for parameters K2 = 1 (solid line) and K2 = 10 (dashed line): a) path tracking error ep1 , b) path tracking error ep2 6

1.5

5 1

4

[m]

0.5

Y

3

0

2

−0.5

1

a)

0

0

1

2

3 X

4 [m]

5

6

b)

−1

0

20

40

60

80

100 TIME [s]

120

140

160

180

200

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 nonholonomic mobile manipulator of (nh, h) type. The control goal for the mobile platform was to follow along the desired ﬂat curve without stopping the motion and the manipulator should move in such a way that its end-eﬀector follows along the geometric path described in Cartesian coordinates and stops at the end of the desired path. From plots depicted in Figs 25.3-25.5 we can see that the new controller introduced in this paper works properly. The Pomet algorithm, which is asymptotically 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 conﬁgurations which was the necessary assumption by the controller designing. The dynamic controller (25.19) can be applied only to mobile manipulators with fully known dynamics but it is a starting point for designing the adaptive version of the presented controller. For holonomic 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 classiﬁcation of dynamical models of wheeled mobile robots. IEEE Trans Robot Autom 12(5):47–61 2. d’Andr´ea-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. Dul¸eba 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 Control of Complex Systems. Kluwer Academic Publishers, Dordrecht 5. Galicki M (2006) Adaptive control of kinematically redundant manipulator. Lecture Notes Control Inf Scie, 335:129–139, London, Springer Verlag 6. Mazur A (2004) Hybrid adaptive control laws solving a path following 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 following and time-varying point-stabilization of mobile robots. IEEE Trans Aut Control 40(1):147–158 9. Tcho´ n K, Jakubiak J (2004) Acceleration-Driven Kinematics of Mobile Manipulators: an Endogenous Conﬁguration Space Approach. In: Lenarˇciˇc J, Galletti C (eds) On Advances in Robot Kinematics. Kluwer Academic Publishers, The Netherlands 10. Tcho´ n K, Jakubiak J, Zadarnowska K (2004) Doubly nonholonomic mobile 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 University of Technology, ul. Janiszewskiego 11/17, 50-372 Wroclaw, Poland {krzysztof.arent,marek.wnuk}@pwr.wroc.pl

26.1 Introduction At present child development and pediatric rehabilitation is an emerging and challenging research ﬁeld 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 achievements 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 speciﬁcation 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 signiﬁcantly 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

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 295–302, 2007. c Springer-Verlag London Limited 2007 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 speciﬁcation of the therapist – robot interface bas been done in [6]. The main task of the simulator is to enable the therapist to eﬀectively verify the robot behaviour deﬁned by him/her. Thus the robot simulator should oﬀer a rich set of stimuli corresponding to the child actions and robot states, which are recorded by sensors. Moreover, it should oﬀer 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 signiﬁcance 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 interactive social robot sensors. Koala, due to its design, is a good object of the ﬁrst step analysis. To this end in Section 26.2 we discuss some aspects of the Koala controller scheme and characterise applied sensors and actuators. In Section 26.3 virtual Koala scheme is developed and its components are discussed. In particular, sensor models are proposed. 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 micromachined 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

MC33794

CHILD

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 uniformly distributed directly under the robot surface are connected to an MC33794 chip [11] intended for use in detecting objects using electric ﬁeld. On-chip generated sine wave (120 kHz) is passed through an internal resistor to the electrodes. Objects brought into or out of the electric ﬁeld 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 eﬃcient 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 in the preprocessor to q the following interq 2 2 2 nal variables: acc = | ax + ay + az − g|, rot = | ωx2 + ωy2 + ωz2 |. The 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 deﬁned 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 deﬁned. Pressure and touch are characterised by press, ipress, dpress, touch, itouch, and

298

K. Arent and M. Wnuk

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 transformation, 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 cannot 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 depicted in Fig. 26.2. It consists of three main subsystems: stimuli, virtual robot, and animation. virtual robot

child

actions

robot model

sensors

stimuli

fuzzy model of robot

preprocessor

external input signals

internal input signals

postprocessor

internal output signals

animation

external output signals

Fig. 26.2. The virtual Koala scheme

Essentially, it is hard to specify the proper set of stimuli without acquiring some knowledge about the play style of a particular child with a robot. We can conjecture that we will face with rolling (pushing, kicking), 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 ﬂoor by a child. For that reasons the child actions and the robot model blocks have been distinguished in the stimuli subsystem.

26 On Simulator Design of the Spherical Therapeutic Robot Koala

299

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 stiﬀ; 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 coordinate 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 represented 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 de0 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 coordinate 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))η T

(26.3)

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 Pfaﬃan form [13]. The vector u represents non-potential generalised forces acting on the system, in particular forces caused by a child. This parameter seems to

300

K. Arent and M. Wnuk

be appropriate to express such child actions like kicking, periodical pushing, etc. It should be emphasised that the variables q and u have inﬂuence 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 [2((i−1) mod 4)+1]π −sign(i−4.5)π R and φR . Denote by si the value θi = i = 3 4 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 deﬁned in Section 26.2 and can be easily implemented. However, it has to be emphasised that the virtual robot oﬀers us certain ﬂexibility. For example, we can easily implement diﬀerent maps, addressed to diﬀerent 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

26 On Simulator Design of the Spherical Therapeutic Robot Koala

301

at the prototyping stage. Essentially, there was implemented a reduced form of the Koala scheme, restricted to the part between the internal input/output signals. There were proposed ﬁve kinds of stimuli (dropping, holding, kicking, rolling, and squeezing) and ﬁve robot behaviours, 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 simulation 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 deﬁne the robot behaviours. It appears that the full virtual Koala scheme of Fig. 26.2 can be eﬀectively 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 SimMechanics, 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 ﬁs 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, deﬁned and veriﬁed 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

302

K. Arent and M. Wnuk

in deﬁning 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 Technologies (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 comparing 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 Association for Artiﬁcial Intelligence (2001) 6. Arent, K., Kabala, M., Wnuk, M.: Towards the therapeutic spherical robot: 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 Micromachined 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 Robotic Manipulation. CRC Press, Inc. (1994) 13. Matysek, W., Muszy´ nski, 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 [email protected]

In this paper we propose a new rehabilitation training support system for upper limbs. The proposed system enables therapists to quantitatively evaluate the therapeutic eﬀect 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 algorithms 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 eﬀectiveness of the developed control algorithms is veriﬁed 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 having 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 K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 303–310, 2007. c Springer-Verlag London Limited 2007 springerlink.com

304

Y. Morita et al.

therapeutic eﬀects. Recently, many rehabilitation support systems have been developed. Most of them are used to perform motor function exercises 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 rehabilitation systems for the occupational therapy of upper limb motor function [3, 4, 5]. From this background, we have developed a two-dimensional rehabilitation 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 ﬁnal aims in the future are to quantitatively evaluate the therapeutic effect 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 eﬀect. In this paper we propose a new rehabilitation training support system 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 algorithm is developed. It enables the therapists to easily make training trajectory suitable for the subject’s condition. The eﬀectiveness of the developed control algorithms is veriﬁed 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 eﬀective 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 rehabilitation 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 viewpoint of aiding the therapist’s work, the development provides not only

27 Development of Rehabilitation Training Support System

305

link 6 (0.40m) link 4 (0.30m)

Grip

link 2 (0.45m) θ θ

Force/Torque Sensor

θ

FGI

20 link 5 (0.15m)

link 3 (0.45m)

Motor Motor

Motor

Fig. 27.1. Rehabilitation training using 3D force display robot

(0.55m)

link 1 (0.30m)

Fig. 27.2. 3D force display robot

a new exercise machine but also the realization of following two supporting functions; (1) motion assistance function and (2) supporting function of planning and modiﬁcation of exercise programs [7].

27.3 3D Force Display Robot The 3D force display robot(3D robot) is developed for the rehabilitation 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 ﬁxed. 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 eﬀector 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 aﬃnity 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.

306

Y. Morita et al. Z Zn

Xn

ajec ed tr

On

tory

desir n-th θ θ y z

n-th target position

Grip O

Yn

n-th initial position

X

Y

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) development 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 algorithms 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 coordinate 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.

27 Development of Rehabilitation Training Support System

307

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 eﬃcient 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 automatically stops at the end of the motion range, the drastic stop has some adverse eﬀect 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 ampliﬁers, 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 ﬂexor 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

308

Y. Morita et al.

Bar Type Grip

Force/Torque Sensor

Tape Shape Sensor

Fig. 27.4. Traditional sanding train- Fig. 27.5. Sanding training using 3D force display robot ing 0.6

30

Position[m] Velocity[m/sec]

0.6

30

Position[m] Velocity[m/sec]

0.4

20

0.2

10

0.2

10

0

0

0 - 0.2

Force[N]

20

Force[N]

0.4

- 0.2

- 10

- 0.4

- 0.4 - 20

- 20

- 0.6 5

0 - 10

- 0.6 10

15

time[sec]

- 30 5

10

time[sec]

15

5

10

time[sec]

15

- 30 5

10

15

time[sec]

(a) Traditional sanding training tool (b) Simulated sanding training system Fig. 27.6. Comparison in responses of position/velocity and force

n

fw



   −M g sin θ −Fc sgn(n x˙ G ) , 0 =  −M g cos θ  +  0 0

where Fc (= 3.0) is the Coulomb friction coeﬃcient, g is the gravitational acceleration, and θ(=30 [◦ ]) is the slope of the sanding board. By changing the damping coeﬃcient 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 coeﬃcients 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. However, the training programs are diﬀerent for the subjects, because the subject’s conditions are diﬀerent, such as his/her height, length of upper arm and forearm, degree of motor dysfunction and so on. So the

27 Development of Rehabilitation Training Support System

Therapist

1 0.9 0.8

Z[m]

Subject (Patient)

309

0.7 0.6 0.5 0.2 1

0 0.8

Y[m]

Fig. 27.7. Teaching scene of training trajectory using 3D force display robot

- 0.2

0.6

X[m]

Fig. 27.8. Teaching (◦) and actual (–) trajectories

therapist has to make a training program suitable for the subject’s condition. 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 ﬁxed 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 trajectory is shown in Fig. 27.8. The green circles show the recorded training trajectory data. The grip motion can be restricted on the training trajectory. Step 3: The subject moves the grip by himself/herself along the training 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 eﬀect. Step 4:: The therapist easily changes the load of resistance of the training program by changing the parameters on the display. By using this teaching/training function the therapists can be expected to develop a new training program.

310

Y. Morita et al.

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 trajectory suitable for the subject’s condition. The eﬀectiveness of the control algorithm of the sanding training and the teaching/training function algorithm is veriﬁed by experiments. A future work is to perform clinical demonstrations using the proposed system in cooperation with medical facilities to establish an evaluation 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.446450, 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 Neurorehabilitation”, IEEE Trans. on Rehabilitation Engineering, 6-1, pp.75-87, 1998. 4. B.T. Volpe, et al., “A novel Approach to Stroke Rehabilitation”, Neurology, 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 Intelligence in Robotics and Automation (CIRA2003), pp.134–139, 2003 7. Y. Morita, T. Yamamoto, T. Suzuki, A. Hirose, H. Ukai, and N. Matsui, “Movement Analysis of Upper Limb during Resistance Training Using 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 L´ od´z, Institute of Automatic Control, ul. Stefanowskiego 18/22, 90-924 L´ od´z, Poland [email protected], [email protected]

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 degreesof-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; introduce the sensory suite and local controllers’ conﬁguration. 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 ﬂoors cluttered with debris; • ability to climb up and over tall vertical steps; ∗

This research is ﬁnanced from grant No. 3 T11A 024 30.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 311–318, 2007. c Springer-Verlag London Limited 2007 springerlink.com

312

M. Pytasz and G. Granosik

Fig. 28.1. Wheeeler avoiding an obstacle

• ability to ﬁt 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 mobile 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 ﬁnd 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 speciﬁc (especially optimal) mechanical solutions for driving and actuation mechanisms. Wheeled

28 Applying CORBA Technology for the Teleoperation of Wheeeler

313

propulsion was the very ﬁrst applied in articulated mobile robots [2], it is also very eﬀective 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 Wheeeler. Applying masses, inertias, friction coeﬃcients 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 ﬁrst version, some changes have been applied in the model of our hyper mobile robot, mostly as an eﬀect of interaction between you-can-do-anything-in-simulation and construction possibilities. The sensory suite of our Wheeeler robot is now enriched with distance sensors, 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 remember 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 communication 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 ﬁt for the serpentine robots is distributed control system with some serial communication – possibly CAN bus. But when the number of sensors, we need for teleoperation 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 oﬀers additional 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

314

M. Pytasz and G. Granosik

high processing power, as for 8-bit controllers. Additionally, in-systemprogramming from Atmel oﬀers 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 operator? 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 datagrams. 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 oﬀering high level communication mechanisms, 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 Environment) 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 application in object oriented programming. For this reason, atop DCE RPC (adopted and renamed to MSRPC), Microsoft had built DCOM mechanism – 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 ﬁnally to DCOM. One of the foundations of COM is programming language independence, which is indeed a desired feature. However, application of DCOM would make the robot software platform dependent. An opposite 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 dependency, at least when it is applied in a straight forward way. These

28 Applying CORBA Technology for the Teleoperation of Wheeeler

315

disadvantages do not seem to occur in CORBA – Common Object Request Architecture. This technology is portable, language, hardware and software platform independent. From the programmer’s point of view it provides only a communication interface deﬁnition and remote object reference search abilities. Client and server sides are always interoperable if only they follow common IDL (Interface Deﬁnition Language). 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 deﬁned in an example shown in Fig. 28.4.

Fig. 28.2. Control information tab

Fig. 28.3. Sensor information tab

Interfaces deﬁnitions are included in modules, which after translation 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 preﬁx set are supposed to send commands. All methods starting with 2

All interface deﬁnitions in this paper have been limited to Wheeeler speciﬁc methods.

316

M. Pytasz and G. Granosik

Fig. 28.4. WheeelerControl network interface

preﬁx get provide a client with ability to read the state of the robot. The deﬁnition above has only one advantage: it is very simple to be implemented, since all communication is initiated by one side. Unfortunately, this way of solving the problem would not be eﬃcient 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 analysis 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 veriﬁcation, in case of not obtaining data from the robot for a long period. There is also a new

28 Applying CORBA Technology for the Teleoperation of Wheeeler

317

part in this interface. Methods starting with preﬁx 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 diﬀerences 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 observed 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 simulated 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, application of real-time programming enhancements should not be taken into consideration, since both, client and server are not run on a realtime operating system. In this case, the use of programming extensions of this kind would show no eﬀect, or could even lead to software malfunctions. 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.

318

M. Pytasz and G. Granosik

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 implemented 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. Programming, 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 Rescue 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 Computing Systems (ICDCS), Tokyo, Japan. 5. Mungee S, Surendran N, Schmidt DC (1999) The Design and Performance 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. Tcho´ n (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 System. Proc. of Int. Workshop on Advances in Service Robotics, ASER’03, Bardolino, Italy. 8. Streich H, Adria O (2004) Software approach for the autonomous inspection robot Makro. Proc. IEEE Int. Conf. on Robotics and Automation, New Orleans, LA, USA, pp. 3411-3416.

29 Educational Walking Robots∗ Teresa Zieli´ nska, Andrzej Chmielniak, and Lukasz Ja´ nczyk 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, [email protected]

29.1 Introduction Walking machines are special examples of mobile robots performing the discrete locomotion. Here the motion path is not continuous but consists 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 servomotors with built-in feedback are used. It is not possible to inﬂuence 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” available 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.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 321–328, 2007. c Springer-Verlag London Limited 2007 springerlink.com

322

T. Zieli´ nska, A. Chmielniak, and L. Ja´ nczyk

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 servomotors (Fig. 29.1). In the ﬁrst solution the machine was controlled by a stationary PC and control signals were sent via a printer connector 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 ﬁeld in the middle 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 considering 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

Fig. 29.2. MENTOR walking by tripod gait

323

Fig. 29.3. View of the screen for 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 control computer and interface electronics. It resulted in the trunk being slightly wider (by 10%) and thinner (by 5%) compared to other similar designs. This oﬀers a better condition for preserving static stability because 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 (especially 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 externally 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 realtime control system elaborated by us [10] allows the experimental gait synthesis. The user can choose the activation period for each servo

324

T. Zieli´ nska, A. Chmielniak, and L. Ja´ nczyk

Fig. 29.5. View of the menu of the software used for experimental motion synthesis

Fig. 29.4. BAJTEK

(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 deﬁnes, 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 deﬁnition 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 deﬁnition 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

O1

O1 A

O3

O3 D

O2

C

B

D

C

x

A B

E

E

F

F

Dim. O1A O2C O3D O1B AC DE BE BF

Val. (mm) 40 26 26 77 57 75 50 115

Fig. 29.6. Leg simpliﬁed assembly drawing and kinematical sketch

29 Educational Walking Robots

325

links (Fig. 29.6) making thigh (O1 B) and shin (BF ). The servos are ﬁxed to the trunk. The servo shafts (O2 and O3 ) are attached to the two arms (O2 C, O3 D) ﬁxed 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 kinematically 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

5

175

50

14

17

0

Fig. 29.7. Quadrupled BAJTEK

29.3 New Control System For the BAJTEK robot a control system diﬀerent than the discussed above was elaborated [6]. This system is not dedicated to the experimental 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 proﬁling 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

326

T. Zieli´ nska, A. Chmielniak, and L. Ja´ nczyk

micro-controller’s timer. The control system can control up to 16 servos 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 speciﬁcation it is equal to 5.5 rad/s), however the real speed depends on the load torque and supplied voltage. 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 modiﬁcation of the intermediate position realized 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 intermediate 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 monitoring the voltage drop on the potentiometer connected to the motor shaft. The measured value was transferred by the analog-digital converter 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 damping. 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 factors 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

29 Educational Walking Robots Servo speed (rad/s)

10

10

Servo speed (rad/s)

327

Given time of moving: 0.19 s

5 0

0.1

0.2

0.3

Given time of moving: 0.3 s

5 0

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

0.3s

1s

50

2s

40

y (mm)

Servo position (°)

50

30

40 30 20

20

10

10

0 0

0 0.5

1

1.5

2

20

40

60

80

100

x (mm)

Time (s)

Fig. 29.9. Chart of servo positions for diﬀerent motion time (motion range – 60◦ ) – speed control

Fig. 29.10. Leg end trajectory

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 ﬁnally 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 eﬀective sensors and development of motion abilities are planned. The hexapod MENTOR with 12 degrees of freedom is an educational research platform for the development of multi-tasking realtime control system. For BAJTEK the postural stability conditions are

328

T. Zieli´ nska, A. Chmielniak, and L. Ja´ nczyk

more critical. For statically stable gait only one leg a time can be transferred. With the transfer of two legs (on the diagonal) the body must be dynamically balanced considering the mass distribution and inertial forces. BAJTEK makes an experimental platform for dynamical motion synthesis. Considering dynamical stability students can synthesize the two-legged gait. Manual control of each degree of freedom in the ﬁrst control system of quadruped BAJTEK allows to test the leg movements before its ﬁnal 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, Zieli´ nska 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 Autom. Mag., vol.10, no.4:23–32 6. Ja´ nczyk 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 reinforcement 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. Zieli´ nska 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 2

Technische Universit¨ at M¨ unchen, 80290 M¨ unich, Germany {keyrouz,kldi}@tum.de Notre Dame University, Zouk Mosbeh, Lebanon [email protected]

30.1

Introduction

Audio and visual perceptions are two crucial elements in the design of mobile humanoids. In unknown and uncontrolled environments, humanoids are supposed to navigate safely and to explore their surroundings autonomously. While robotic visual perception, e.g. stereo vision, has signiﬁcantly evolved during the last few years, robotic audio perception, especially binaural audition, is still in its early stages. However, non-binaural sound source localization methods based on multiple microphone arrays, like multiple signal classiﬁcation 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 arrays 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 developed 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 communication while maintaining an optimum level of energy consumption. Hence, interacting robots sending and receiving acoustical information 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 Teleaction”.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 329–340, 2007. c Springer-Verlag London Limited 2007 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 internal 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, realize a sensitive receiver and high-resolution spectral analyzer. The human head and pinnae together form a complex direction-dependent ﬁlter. The ﬁltering action is often characterized by measuring the spectrum 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 diﬀerence (ITD), 2) interaural intensity diﬀerence (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 beneﬁt 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 eﬀects of pinnae and torso on the original sound signal in order to localize the sound source in a simple matched ﬁltering process. In this paper, the processing time of the previously proposed algorithm is dramatically decreased

30 Humanoid Binaural Sound Tracking Using HRTFs

331

Robot Head Microphones

1 HRTFLi

1 H Li

SL H L 1710

Maximum Cross-correlation

H R11

SR

1 R2

SR

H

H Ri1 H R 1710

…

…

1 H Li

1 HRTFRi

H Ri1

…

SL

SR

…xcor …

…

1 SL H L1 SL H L 12

SL

SR SR

Filtering (Convolution)

Fig. 30.1. Flowchart of the sound localization algorithm as proposed in [5]

by reducing the number of convolution operations signiﬁcantly, and by properly integrating a Kalman ﬁlter. Parallel to the matched ﬁltering process, the ﬁlter estimates the location of a moving sound signal and locates a region of interest (ROI) within the HRTF database, thus allowing 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 ﬁrst 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 straightforward matched ﬁltering 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 ﬁltered through the HRTF that corresponds to the given ear and direction. Match ﬁltering 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

332

F. Keyrouz, K. Diepold, and S. Keyrouz

of ﬁltering the left received signal by the correct inverse left HRTFL should be identical to the right received signal ﬁltered by the correct inverse right HRTFR . In order to determine the direction from which the sound is arriving, the two signals must be ﬁltered 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 correlation. This method is illustrated in Fig. 30.1. Due to the computational complexity of ﬁltering 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 ﬁltering operations to simple array multiplications and divisions. We assume that the reduced HRTFs, i.e. using Diﬀused-Field Equalization (DFE) [7] and Balanced Model Truncation (BMT) [8], have already 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 corresponds 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 ﬁlters. The set of ﬁlters from the correct direction would result in canceling the eﬀects 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 retrieve 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

30 Humanoid Binaural Sound Tracking Using HRTFs

333

Robot Head Microphones Region of Interest (ROI) SL

H Li H Ri

… xcor …

SL SR

SR

HRTFLi HRTFRi H L1 H R1

H Li 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 domain, 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 correlation 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 artiﬁcial 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 oﬀ-line and saved in the system database. In a hardware-based application, using the SCA would greatly reduce hardware complexity as well as speed up processing. Compared to the original algorithm, this new approach eliminates 1420 array multiplications and 1420 inverse Fourier transforms, and replaces them with one single array multiplication.

334

F. Keyrouz, K. Diepold, and S. Keyrouz

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 fulllength HRTFs can be modeled as FIR ﬁlters. These ﬁlters, denoted IR , are reduced to 128-order using the DFE model-truncation by HF512 IR . method. The obtained DFE-reduced dataset shall be denoted as HF128 To make the processing time even faster, this DFE-reduced HRTF database is further truncated using BMT. The BMT-reduced HRTFs can be modeled as IIR ﬁlters, and are denoted here by HIIR m , where for every value of m we have a truncated HRTF dataset of diﬀerent 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 database 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 ﬁlter is tailored to predict a ROI the sound source might be heading to, according 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 workﬂow of the SCA attached to a Kalman ﬁlter is depicted in Fig. 30.3. The Kalman ﬁlter applies to a linear dynamical system, the state space model of which consists of two equations: xk+1 = Axk + Buk + w, yk+1 = Cxk+1 + v.

(30.1) (30.2)

Equations (1) and (2) are called the process and the measurement equations, respectively. The variable x is the state of the discrete-time system and y is the system’s output such as positions or angles depending on the movement model. The variable u models the sound source velocity. 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 dz respect to time, x˙ = dx dt , y˙ = dt , and z˙ = dt . According to the movement equations, based on Newton, the sampling time T for each coor-

30 Humanoid Binaural Sound Tracking Using HRTFs

335

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 ﬁlter using the above-mentioned model, are transformed to spherical coordinates to stay compatible with the output of the SCA in Fig. 30.3.

Movement Model

x y z

Kalman Filter

Estimated x Estimated y Estimated z

Coordinate Transformation Predicted elevation Predicted azimuth Creation of ROI in HRTF dataset ROI

Sound signal

SCA

Azimuth Elevation

Fig. 30.3. Flowchart of the Source Cancelation Algorithm (SCA) using a ROI

30.5

Simulation and Experimental Results

In the ﬁrst 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 eﬀect 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 ﬁltered by appropriate 512-sample KEMAR HRTFs at a certain azimuth and elevation. Thus the test signal was virtually synthesized using the original fulllength HRTF set. For the test signal synthesis, a total of 100 random

336

F. Keyrouz, K. Diepold, and S. Keyrouz

HRTFs were used corresponding to 100 diﬀerent random source locations in the 3D space. In order to ensure rapid localization of multiple sources, small parts of the ﬁltered left and right signals are considered (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 maximum 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 diﬀerent 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. IR , the Using the diﬀused-ﬁeld equalized 128-sample HRTF set, HF128 SCA simulated percentage of correct localization is around 97%. This means that out of 100 locations, 97 where detected by our SCA algorithm compared to 96% for the previous method. Using the BMTreduced 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 localized 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 position 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 distance to the target sound locations is expected to notably decrease. In our household experimental setup, 100 binaural recordings from diﬀerent directions were obtained using a dummy head and torso with two artiﬁcial 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 inputs to our SCA localization algorithm. The localization accuracy is illustrated by the solid line and the circle in Fig. 30.4.

30 Humanoid Binaural Sound Tracking Using HRTFs

337

Percentage of Correct Localization (%)

100 90 80

SCA Simulation

70

Previous Method Simulation

60 50

FIR DFE H128

*

BMT H IIR m FIR DFE H128

+

BMT H IIR m

SCA Experimental

40 30

BMT H IIR m

0

20

FIR DFE H128

40 60 80 100 HRIR filter Coefficients (Samples)

120

140

Fig. 30.4. Percentage of correct localization using SCA compared with the method in [5] 70 Previous Method SCA

Average Distance in [°] from the Target Angles

65 60 55 50 45 40 35 30 25 20 15 10 5 0

0

20

40 60 80 100 HRIR Filter Coefficients (Samples)

120

140

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-

338

F. Keyrouz, K. Diepold, and S. Keyrouz

berations contained within the measured sound signals, which were not included in the simulation. The deviation in performance is also justiﬁed on the basis of the diﬀerences 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 circular trajectory 2 meters away from the humanoid head located in the center. The recorded sound signals, also containing external and electronic noise, were used as inputs to our sound localization algorithm. The sound localization algorithm depicted in Fig. 30.1 initializes by 0.12

Localization Time in [s]

0.1

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 diﬀerent 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 identiﬁed and automatically updated using a Kalman ﬁlter. This way, the sound localization operation, particularly the correlation process, proceeds only in this ROI instead of searching the whole HRTF dataset.

30 Humanoid Binaural Sound Tracking Using HRTFs

339

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 processingtime results, corresponding to diverse ROI lengths, are depicted in Fig. 30.6. Using a simple Kalman ﬁlter 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 eﬃcient sound source localization method using a generic HRTF dataset and two microphones inserted inside the artiﬁcial 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 signiﬁcant reduction in the processing requirements while still slightly increasing the accuracy of the sound localization. Furthermore, with the help of a simple Kalman ﬁlter, a ROI was introduced to account for fast moving sound sources. Simulation and experimental results showed real-time tracking performance. The eﬃciency of the new algorithm suggests a cost-eﬀective implementation for robot platforms and allows fast localization 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.

340

F. Keyrouz, K. Diepold, and S. Keyrouz

References 1. Brandstein M (1999) Time-delay estimation of reverberated speech exploiting harmonic structure. J Acoust Soc Am 105:2914–2919 2. Jahromi O, Aarabi P (2005) Theory and Design of Multirate Sensor Arrays. 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 Localization 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 headrelated 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

2

Institute of Aeronautics and Applied Mechanics, Faculty of Power and Aeronautical Engineering, Warsaw University of Technology, Nowowiejska str. 24, 00-665 Warsaw, Poland [email protected] 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 inﬂuence on man-machine communication [5]. Based on these research results we present the mechanical design and construction of upper body, eyes, and neck for the humanoid robot ROMAN. The mechanical design is an extension of the previously build emotional 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. K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 341–348, 2007. c Springer-Verlag London Limited 2007 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 speciﬁc 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 artiﬁcial 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 numbering system including the approximated maximum ranges of motion Skin Motions 1 Inner Brow Raise 1 cm 2 Outer Brow Raise 1 cm 9 Nose Wrinkle 1 cm 12 Lip Corner Puller 1 cm 15 Lip Corner Depressor 1 cm 20 Lip Stretch 1 cm 24 Lip Presser 1 cm 26 Yaw Drop 10◦

Head Motions 51 Turn Left 60◦ 52 Turn Right 60◦ 53 Head Up 20◦ 54 Head Down 20◦ 55 Tilt Left 30◦ 56 Tilt Right 30◦ 57 Forward 2 cm 58 Back 2 cm

Eye Motions 61 Eyes Left 30◦ 62 Eyes Right 30◦ 63 Eyes Up 40◦ 64 Eyes Down 40◦ 65 Walleye 30◦ 66 Crosseye 30◦

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 previous 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 artiﬁcial 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 stiﬀ box with artiﬁcial 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

344

K. Mianowski, N. Schmitz, and K. Berns

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(α) Fspring1 (α) = 2 · k1 · (l1 (α) − l1min ) Mspring1 = −Fspring1 · d1 (α) Mspring = Mspring1 + Mspring2 Mresult = Mext + Mspring

(31.1) (31.2) (31.3) (31.4) (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 bending 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 artiﬁcial skeleton

31 Mechatronics of the Humanoid Robot ROMAN

345

R

m

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 generated 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 calculated 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 . Figure 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 independently 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 ﬂexible 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

346

K. Mianowski, N. Schmitz, and K. Berns

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 including 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 horizontal 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 nonverbal 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 stiﬀness 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 diﬀerent motors including 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 ampliﬁers for the motors. This digital signal processing unit is able to preprocess diﬀerent types of encoders and

31 Mechatronics of the Humanoid Robot ROMAN

347

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 friction of the motors can be compensated and the desired input can be reached without having a diﬀerence to the actual input. The integral element is only responsible for compensating external forces to the joint. This concept makes the controller quicker and more stable. The superposed position controller is combined with an anticipation element. The known velocity of the position calculation is a direct input to the speed controller. Diﬀerences in the position must be compensated by the position controller. No diﬀerence 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 diﬀerent 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 modular, 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-

348

K. Mianowski, N. Schmitz, and K. Berns

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 between 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 inﬂuence on the communication task, so these will be added in near future. Based on this mechanical design it is necessary to extend the current behavior-based controlling architecture to realize ﬁrst 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 Human 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 Conference on Humanoid Robots (Humanoids), 2005. 8. M. Zecca, S. Roccella, M.C. Carrozza, G. Cappiello, and et. al. On the development 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, November 10-12 2004.

32 Assistive Feeding Device for Physically Handicapped Using Feedback Control Rahul Pandhi1 and Sumit Khurana2 1

2

Department of Electronics and Communication Engineering, Delhi College of Engineering, Delhi University, India [email protected] Department of Manufacturing Processes and Automation Engineering, Netaji Subhas Institute of Technology, Delhi University, India [email protected]

32.1 Introduction People suﬀering from neuromuscular diseases have trouble lifting their arms against gravity although a large number of them maintain sensitivity 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 controlled movements of the head and neck and can take food oﬀ 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 diﬀerent components. Although such feeding aids can be used eﬀectively, 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 ﬁnd 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 diﬃcult to adapt to individual needs. K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 351–360, 2007. c Springer-Verlag London Limited 2007 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 operation and provide precise arm movement but diﬀer in their modes of installation. While one is ﬁxed 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 ﬂoors, a function 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 components: the the the the

’ ’

• • • •

slave’ arm unit, master’ or interface unit, transmission system, 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 freedom (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 ﬁxed 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 ﬁberglass 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 ﬁxed 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 .

354

R. Pandhi and S. Khurana

Fig. 32.2. Head/Mouth master interface

32.2.3 Transmission System Although not shown for the purpose of clarity, Bowden cables run between 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 ﬂexible outer housing, a ﬂexible 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 error 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 microcontroller to control the movements of the slave arm unit. A rotating potentiometer is placed on the joint θsρ , which calibrates the movement 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

32 Assistive Feeding Device for Physically Handicapped

355

microcontroller. This microcontroller is used to drive the slave unit through a stepper motor. Keeping in mind the constraints of a microcontroller such as its memory size and its processing speed, the microcontroller used for the experiments was Phillips 89C51RD2BN. This micro-controller has 64kB of ﬂash memory and 1kB of RAM as compared to Atmel 89C51 which has 4kB of ﬂash 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 physiological proprioception (EPP) and force reﬂection and its simplicity relative to rehabilitation robots. It is more versatile and allows a person with no or very little arm function to interact with his surroundings. The target population that would beneﬁt 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 eﬀectively. Because it is physically and intimately coupled to the user and acts as an extension of the person, this device has the ﬂavor 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 oﬀers 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

356

R. Pandhi and S. Khurana

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 coupled 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 ampliﬁer while allowing the human to remain in direct contact with the manipulator, thus enabling 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 diﬀerence 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 ﬁltered 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

32 Assistive Feeding Device for Physically Handicapped

357

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ﬁne the concept and redesign the product before making a physical prototype. Over the last decade, high speed computer graphics workstations have proven to be very eﬀective in allowing visualization of three-dimensional complex systems. With advances in robotics technology, 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 computers 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 dynamic models (speciﬁc 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

358

R. Pandhi and S. Khurana

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 mechanism that links the human head to the feeding device is not shown in the ﬁgure. The designer can experiment with diﬀerent kinematic coupling mechanisms and see how the movements of the user are translated into the movement of the end eﬀector 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 eﬀectively 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 appropriate virtual environment before the designer commits to the expense of creating the physical prototype. As shown in Fig. 4 consumer feedback (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 proprioception 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 invaluable support and guidance, and for his constant motivation, which lead to successful completion of this experiment. We would like to appreciate the

32 Assistive Feeding Device for Physically Handicapped

359

suggestions given to us by Professor Suneet Tuli, Associate Dean, Indian Institute 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 technology to the ﬁeld 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 robotic 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 evaluation of the helping hand electro-mechanical arm. Technology and Disability, 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 Disability, 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 Journal, 1(1):26-29, MCB University Press Limited, Bradford, England, 1995. 8. M. West and H. Asada. A method for designing ball wheels for omnidirectional vehicles. 1995 ASME Design Engineering Technical Conferences, DAC-29, pp. 1931-1938, Seattle, WA 1995. 9. F.G. Pin and S.M. Killough. A new family of omni-directional and holonomic wheeled platforms for mobile robots. IEEE Transactions on Robotics 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 Wr´ oblewski Chair of Control and Systems Engineering, Pozna´ n University of Technology, ul. Piotrowo 3a, 60-965 Pozna´ n, Poland [email protected]

33.1 Introduction During operation of the heating net, there may appear leakages and failures in places which are hard to access. Repair of underground heatingpipe ducts requires using heavy equipment as well as temporarily removing 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. Localization of the failure place is diﬃcult 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 estimation 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 research grant no. 3 T11A 026 27.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 361–370, 2007. c Springer-Verlag London Limited 2007 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 observation of the range of the failure and the eﬀects 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 control from the outside is diﬃcult as unpredictable obstacles or uneven ground may appear. The ground of heating-pipe ducts is usually uneven; there can be found accidentally such elements as thermal shields, pipe coverings, crushed bricks, or construction elements of the heatingpipe 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 oﬀroad car produced by Tamiya [6]. Such models are used in races of the RC-class cars. The oﬀ-road car is controlled by specially adapted communication devices – two radio channels operated by an integrated transmitter and two receivers with eﬀectors which mechanically control the driving system. They control the velocity and the direction of the movement. This oﬀ-road car has been chosen as a reference model, but the new chassis has been lengthened compared to the original and the suspension system has been strengthened. It has been assumed that the platform with the equipment must ﬁt 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 enables using the model in oﬀ-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 diversiﬁed surface and signiﬁcant unevenness. 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 assumptions: • the robot should carry heavy batteries and control equipment retaining low own weight and high stiﬀness 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 together, they form a stiﬀ 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 suspension 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 deﬂection range of the damper, measured at the contact point of the wheel to the ground, is ca. 65 mm.

364

P. Dutkiewicz et al.

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 inﬂuence of the vehicle dynamics. This is the ﬁrst level of rotational velocity reduction, which also enlarges the driving torque exerted by the motor. Then, this torque is transmitted to particular wheels via the second level reduction gear of ratio 16.27 : 1 and a diﬀerential gear. So, the driving unit consists of the motor, reduction gears of the ﬁrst 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 diﬀerential gear to the wheel via a stiﬀ 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 oﬀ-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 performing the turn. Thanks to this, the turn of the vehicle may be accomplished in a classical way – it is Ackerman turn system. The vehicle may be qualiﬁed 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 operator. Movement and observation tasks executed by the robot are remotely operated. Essentially, the real-time control is performed by the on-board controller, while reference values are sent from the host via

33 Design and Control of a Heating-Pipe Duct Inspection Robot

365

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 operating system. The modular computer PC104 works as a PC server on the robot board, while the client application works on a PC workstation 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 system. 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 PCM9387 single board computer. It is equipped with an Intel ULV Celeron M1.0 GHz processor and a set of interfaces enabling communication

366

P. Dutkiewicz et al.

with subsystems of particular elements of the on-board equipment. The diagram of connections of the on-board computer with particular subsystems is presented in Fig. 33.2.

Pyrometer

Microphone 1

RS232

Hygrometer Manometer

Microphone 2

RS232

Line In

Mic In

On-board PC Wi-Fi

USB

Controller Analog radio

Camera 2

Illumination

Power manag.

Camera 1

Drive control

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 react 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 autonomy of the robot by automatic control in several tasks, e.g. obstacle avoidance. As the robot is controlled manually, no real-time operating 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 signiﬁcant inﬂuence 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 between other modules; • on-board sensors module – reads cyclically the current values measured by the manometer, the hygrometer, the pyrometer, and the termometer; • motor control and energy distribution module – sends reference values to the microprocessor system of control of wheels velocity and of turn angle; also sends commands to the energy distribution system

33 Design and Control of a Heating-Pipe Duct Inspection Robot

367

to turn on/oﬀ 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 ﬂash disk.

Power supply

{

Servomechanism

Power management module

2

IC

Controller Microprocessor C8051F120

USB

PWM PWM

On-board PC server PWM

Power bridge 1

Power bridge 2

Encoder 1

Radio Radio module 1 module 2

Encoder 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 servomechanism) (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 speciﬁc 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 cooperates directly with the energy distribution module and the radio communication module for manual setting of velocity and turn angle.

368

P. Dutkiewicz et al.

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 protocol 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 eﬃciency. 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 eﬃciency 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 diversiﬁcation 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 movement 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-

33 Design and Control of a Heating-Pipe Duct Inspection Robot

369

ious complex control algorithms compensating such eﬀects appearing in real environment as immobilizing wheels or skid. The feedback from the rotational velocity of motors, implemented using hallotronic encoders, 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 oﬀ-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.

Battery 12V

DC/DC 5V, 5A

Fuse

Independent power sections

DC/DC 12V, 2A

Motor 1 current

Fixed current source

} 4 channels

4 channels AC

AC

Motor 2 current

A/DC 10-bit, 4-ch

Outside lighting LED

2

I C / Parallel

D/AC 8-bit, 4-ch DC

DC 2

Controller

IC

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

370

P. Dutkiewicz et al.

current Imax = 300 mA) adjusted from a D/A 8-bit, 4-channel converter cooperating with the I2 C bus; this enables precise changes of the illumination intensity; • independent supply circuit – four relays with fuses are selected individually 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 diﬃcult or even impossible due to duct dimensions or atmospheric conditions. The robot is intended for teleoperating inspection tasks in areas diﬃcult 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 inspection 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 diﬀerential-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 navigation 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, oblewski and Waldemar Wr´ Chair of Control and Systems Engineering, Pozna´ n University of Technology, ul. Piotrowo 3a, 60-965 Pozna´ n, Poland [email protected]

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 specialized constructions are used. Applications come mainly from areas which are dangerous for humans or where high manipulation precision is required. Many research papers are devoted to applications of robots in medicine. In such cases a robot manipulator can take place of a surgeon’s hand or assists in laparoscopic operations, e.g. following movement of surgical tools in the abdominal cavity and delivering images to the operator [1]. Inspection robots belong to another group of specialized robots [2, 3]. They perform examination of application-dependent infrastructure through measurement of speciﬁed 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 inspection robot RK-13 are described. Its main purpose is examination of ducts of a heating net. Monitoring of the environment in the heatingpipe ducts involves measurements of such physical parameters as temperature, 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, research grant no. 3 T11A 026 27.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 371–380, 2007. c Springer-Verlag London Limited 2007 springerlink.com

372

P. Dutkiewicz et al.

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 elements of the monitoring system. The measured environment parameters 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 during 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 developed. 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 frequency band. The block diagram of the communication system is presented in Fig. 34.1. Control commands and conﬁguration 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 ampliﬁer 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 signiﬁcantly damped. Additionally, the input signal is ampliﬁed by +18 dB. An omnidirectional antenna of 3 dBi gain is connected to the antenna output of the ampliﬁer. 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 computer works in AP (Access Point) mode. To assure the largest possible separation from the independent video signal of the transmitter working 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 eﬀects which may be additional source of information for improving failure localization eﬃciency. In the measurement system of the robot, a so-called external sensoric subsystem has been assigned to such tasks. Another system is responsible for measurement of the robot state (i.e. electric charge received 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 environment 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 preampliﬁers. This equipment is managed by the on-board computer of the robot with the use of two transmission channels based on the RS232C interface.

374

P. Dutkiewicz et al.

Fig. 34.2. Block diagram of the measurement system for environment monitoring

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 robot 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. Acquisition of the measured data is managed by the on-board PC communicating with the device using a text communication protocol speciﬁed by the pyrometer manufacturer. The next element of the measurement system for monitoring ambient pressure, temperature, and humidity was built as a controller around an Atmel ATMega 162 8-bit microcontroller chip. Communication between the microcontroller and the on-board PC uses a binary protocol implementing master-slave data interchange. For pressure measurements, a semi-conductive, thermally compensated 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-

34 Measurement and Navigation System of the Inspection Robot

375

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 representation 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 interchange with the supervising microcontroller a serial bus similar, but not compatible, to the standard I2 C bus. It allows temperature measurements 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 resolution 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 supplied by the sensor, low-pass ﬁltration, and makes the results available to the host computer. The inspection robot is equipped with four capacitive electret microphones placed at front and rear sides of the housing. They are used for acquisition in the acoustic band for leakage detection of the overheated steam. To improve the sensitivity and the signal-to-noise ratio, additional analog preampliﬁers of gain of ca. 30 dB are integrated with all the microphones. The output signals of the preampliﬁers are separated 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.

376

P. Dutkiewicz et al.

34.2.3 Vision System for Inspection and Navigation The vision system of the inspection robot is equipped with two independent 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 directions 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 robot 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

34 Measurement and Navigation System of the Inspection Robot

377

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 remote navigation performed by the operator. Additionally, this system may be used for automatic localization of the platform in the heatingpipe duct. This duct is built of concrete proﬁles 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 diﬀerent 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: • ﬁltration 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 ﬁrst step, the image is captured from the camera in the YUV format. For further processing, only the grayscale component is

378

P. Dutkiewicz et al.

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 coeﬃcients related to steadystate components and higher-frequency coeﬃcients responsible for vertical changes in the image are left unchanged. The higher-frequency coeﬃcients responsible for horizontal changes in the image are set to zero. It is done with a logical mask of the size of the spectrum coeﬃcient matrix (image matrix), where elements equal to logical 1 form a rectangular 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 ﬁltration eﬀect. This ﬁltration eliminates disturbances leaving only distinct vertical or almost vertical edges. Then the trimmed image spectrum is transformed back to the space domain using a two-dimensional inverse fast Fourier transform (IFFT). An example image before the ﬁltration (left) and after the directional ﬁltration (right) is presented in Fig. 34.5.

Fig. 34.5. Image before ﬁltration (left) and after directional ﬁltration (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 ﬁltration. During this, when selecting the proper values of the ﬁlter thresholds, only the points belonging to suﬃciently strong edges remain on the binary image (see Fig. 34.6, left). As the ﬁltration 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 nonexisting 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 coeﬃcient matrix, used

34 Measurement and Navigation System of the Inspection Robot

379

as the direction coeﬃcients of straight lines on the image, the sections containing the detected edge points are selected. Next, the sections of signiﬁcantly diﬀerent 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 between 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 detected 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 satisﬁed as the slab width is relatively large compared to the acceptable velocity of the robot in the duct.

380

P. Dutkiewicz et al.

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 research will focus on this topic. The results of the experiments in real ducts will help to improve the outlined algorithm. The preliminary results 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 surgical tools for laparoscopic surgery. In: Proc. of the 4th Int. Workshop on Robot Motion and Control, pp. 23-28 2. Dutkiewicz P, Kozlowski K, Wr´oblewski 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, Wr´oblewski W (2007) Design and control of a heating-pipe duct inspection 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. Z¨ ollner, 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-deﬁned 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 eﬀort has been spent into development of multisegmented mobile robots. Such systems oﬀer great driving capabilities due to their kinematics. Several major robot systems position themselves in this ﬁeld. SouryuIII 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-deﬁned inspection scenario [11]. OmniTread (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 limited. 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 expanse, load capacity, and kinematic abilities are perfect for inspection operations in unstructured environments. However, as each single segment 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 comK. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 381–390, 2007. c Springer-Verlag London Limited 2007 springerlink.com

382

C. Birkenhofer et al.

Fig. 35.1. MAKROplus - this hyper-redundant robot is similar in construction 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 fulﬁl complex user-deﬁned tasks. Such tasks are completed in parallel to normal driving scenarios [6]. This work contributes to advanced control systems for inspection robots in implementing a closedloop control scheme based on hybrid impedance control. This paper is organized as follows: in Section 35.2 crucial components 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. According to serial kinematics both head- and tail-segment of Kairo-II can be interpreted as end-eﬀectors 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 capabilities. 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 deﬁned as an additional user-deﬁned task. The kinematic chain of the system gets closed and gives rise to a major constraint for any control strategy. Such movements oﬀer 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 necessary 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-deﬁned 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 functionality 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-deﬁned inspection modules. Depending on the inspection scenario the drive units host wheels or chain drives.

Fig. 35.2. A typical robot conﬁguration 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 Modules (UCoM-KairoDrive and UCoM-KairoJoint) are located in every drive and joint segment.

384

C. Birkenhofer et al.

Table 35.1. UCoM speciﬁcation (UCoM-KairoDrive and UCoM-KairoJoint)

Component DSP: FPGA: Memory: Conﬁguration device: Peripherals: Motor control: Power: Current sensing: AD converter: Encoder:

Programming:

Characteristics 80 MHz DSP56F803 (Freescale) EPF10k30A (Altera) 4k Bootﬂash, RAM, Flash, external RAM EPC2-ROM CAN, SCI, JTAG, SPI 6 PWM-Channels Three 24 V motors up to 5 A each Diﬀerential current sensing for each motor channel 2 * 4 Channels 12 bit AD-Converter 6 quadraturecoded signals or up to 24 General purpose IOs 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 scenario. 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 coordination levels. In order to minimize cabling complexity and clock cycle delays UCoM boards ([12] – UCoM-KairoDrive and UCoMKairoJoint) were used for the task execution level (speciﬁcation: 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 eﬀective calculations in the DSP (i.e. motor control) as well as ﬂexible and adaptable calculations in the FPGA (i.e. sensors). To guarantee fast and robust communication, 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. UCoMKairoJoint controls three motors of each joint segment (see: Fig. 35.3).

35 Architecture of Multi-segmented Inspection Robot KAIRO-II

385

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 ﬁeld 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 deﬁned in Cartesian space. The control loops workﬂow 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

386

C. Birkenhofer et al.

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 robot’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 criteria 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 account. 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 ﬁltered 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 eﬀort 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 identiﬁed that fulﬁl 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:

35 Architecture of Multi-segmented Inspection Robot KAIRO-II

387

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 signal’s noise can be minimized. A simple transformation calculates moments along the x-axis out of the detected mechanical deformations. Forces and moments (y-, z-axis): The so called ﬂange plate from each robot module connects the drive and joint segments. This component has been identiﬁed 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 proportional to forces that act in the robot’s x-direction. The signal’s quality meets demands to extract forces along the driving direction 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 applications (i.e. humanoid robot Armar). Several extension modules have been developed to integrate strain-gauge sensor values into the control loop. Impedance Control: The impedance control loop has been identiﬁed and tested as adequate control loop. The crucial blocks have been identiﬁed and implemented in a basic conﬁguration. 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 ﬂow. A simpliﬁed dynamic model reduces the number of parameters enormously [6]. This simpliﬁed model will be used in this project as the accuracy meets demands. Sensors: Three sensor systems have been identiﬁed. They have been implemented and tested as prototype application. See [4, 6] for an analysis of those systems.

388

C. Birkenhofer et al.

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 predeﬁned trajectory. In using hybrid impedance control schemes, the mechanical redundancy of Kairo-II can be used actively to fulﬁl general userdeﬁned inspection goals. Future work will be done in the ﬁeld 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, Design und Realisierung einer mehrsegmentigen, autonomen Kanalroboterplattform. In ATV-Bundestagung, 2000 (in German). 4. C. Birkenhofer, M. Hoﬀmeister, J.-M. Z¨ollner, and R. Dillmann. Compliant 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. Z¨ollner, and R. Dillmann. A new modular concept for a multi-joint, autonomous inspection robot. In Intelligent Autonomous Systems Conf. IAS-8, Amsterdam, 2004. 6. C. Birkenhofer, S. Studer, J.-M. Z¨ollner, and R. Dillmann. Hybrid impedance 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 ﬁeld 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 uniﬁed approach to motion and force control of robot manipulators: The operational space formulation. IEEE Journal on Robotics and Automation, 3(1):43–53, February 1987.

35 Architecture of Multi-segmented Inspection Robot KAIRO-II

389

11. A. Masayuki and T. Takayama andS. Hirose. Development of ”souryuiii”: 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 architecture for the humanoid robot armar. In Humanoids 2003, Int. Conf. on Humanoid Robots, Karlsruhe, October 2003. 13. K.J. Salisbury. Acitve stiﬀness control of manipulators in cartesian coordinates. 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, Krak´ow, 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 deﬁned as an issue connected with systems control, deﬁning their position and orientation 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 independent, 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 cooperating 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 ﬁrst problem is connected with communication between the robots; ﬁnding 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 diﬀerent 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 Scientiﬁc Research under contract 4 T12C 046 29.

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 391–400, 2007. c Springer-Verlag London Limited 2007 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 beneﬁcial 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 deﬁned 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 description

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 deﬁnitions 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

deﬁned 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 deﬁned as c : [0, 1] → F S, where c(0) → pstart and c(1) → ptarget . If we take into consideration kinematics constraints for our particular mobile robot presented above, there are two possible schemes of motion on our deﬁned surface [2, 4]. The ﬁrst 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 deﬁned 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 eﬃcient 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 deﬁnition connected with robots, the basic deﬁnition of those mechatronic systems describes them as autonomous devices owning 3 units presented in Fig. 36.4. The ﬁrst unit is connected with power supply. In this unit we have an accumulator, converters, and a charger. The robot’s

394

T. Buratowski, T. Uhl, and G. Chmaj

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

36 Project of Autonomous Robot Capable to Cooperate in Group

395

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 algorithms and artiﬁcial 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 calculated 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

396

T. Buratowski, T. Uhl, and G. Chmaj

(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 appropriate 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-

36 Project of Autonomous Robot Capable to Cooperate in Group

397

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 speciﬁcations 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 ﬁrst 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 ﬁnd minimum time consuming trajectory.

(a)

(b)

(c)

(d)

Fig. 36.8. The test rig in laboratory connected with the group of robots

398

T. Buratowski, T. Uhl, and G. Chmaj

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, signiﬁcant 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 classiﬁed 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 ﬁnd the best algorithm which will divide the area of search in order to ﬁnd 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, Pozna´ n, Poland, 2004. ˙ 3. T. Buratowski, T. Uhl, W. Zylski. The comparison of parallel and serialparallel 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 computer programs using colonies of ants. In: Proc. of the IEEE Congress on Evolutionary Computation, 2002.

36 Project of Autonomous Robot Capable to Cooperate in Group

399

7. N. Baskar, R. Saravanan, P. Asokan, G. Prabhaharan. Ants colony algorithm approach for multi-objective optimisation of surface grinding operations. Advanced Manufacturing Technology, 2004. 8. S.C. Shapiro. Encyclopedia of Artiﬁcial 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

2

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 Institute of Aeronautics and Applied Mechanics, Warsaw University of Technology, ul. Nowowiejska 24, 00-665 Warsaw, Poland [email protected]

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 selfpropulsion, 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-eﬀector of a service robot, especially the exteroceptors it should be equipped with, and how they aﬀect 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 diﬀerent 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 eﬀective 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 complicated benchmark task. We chose the Rubik’s cube puzzle. It requires [9]: K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 401–414, 2007. c Springer-Verlag London Limited 2007 springerlink.com

402

C. Zieli´ nski et al.

• two handed manipulation to eﬃciently turn the faces of the cube,

• • • •

visual capabilities, including locating the cube, visual servo-control to get hold of it, and identiﬁcation 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 intentions of the human are not clear).

However the solution to the above enumerated problems depends on the choice of equipment – especially the end-eﬀector and its sensing capabilities. Anthropologists still ponder the question how human mental abilities evolved, but they are certain that erect posture, that freed our hands, facilitated dexterous manipulation of objects (tools), thus increasing the control requirements imposed on our brain [2]. In robotics the interrelationship between the design of the end-eﬀector and the behavioural and reasoning capabilities of the control system has not been studied in depth as yet. Herein we try to initiate that discussion by proposing a design of the end-eﬀector 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 inﬂuence the environment, is easy to implement. For that purpose we have used the MRROC++ robot programming framework. Section 2 brieﬂy 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 problems 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 measurements of the torque developed while rotating the cube face. Section 7 summarizes 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 ﬁnal 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-Eﬀector Sensors’ Role in Service Robots

37.2

403

Robot Controller

The controller of the two-handed system equipped with special end-eﬀectors, 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 realtime 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 eﬀectors 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 eﬀectors present in the system. It is responsible for trajectory generation in multi-eﬀector systems, where the eﬀectors are coordinated continuously – as is the case in the presented system. In the case of sporadic coordination it just synchronizes the eﬀectors from time to time. In the case of fully independent actions of the effectors, the MP remains dormant most of the time. The general speciﬁcation of the task for all of the eﬀectors 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 manipulators. This trajectory can be treated as a crude reference trajectory for both arms. At a later stage this trajectory is modiﬁed by taking into account the force readings. Each eﬀector has two processes controlling it: Eﬀector Control Process ECP and Eﬀector Driver Process EDP. The ﬁrst one is responsible for the execution of the user’s task dedicated to this eﬀector (in our case the task is speciﬁed by the MP– it is deﬁned by the reference trajectory that is to be executed by the manipulator), and the other for direct control of this eﬀector. The EDP is responsible for direct and inverse kinematics computations 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 eﬀector. 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 Virtual Sensor Processes VSPs are responsible for aggregating the data obtained from the exteroceptors.

404

C. Zieli´ nski et al. SEMAPHORE PULSE

MP UI

MESSAGE

UI_ COMM

INTERRUPT SHARED MEMORY

UI_SRP UI_ MASTER

VSPk

(receives messages from other processes)

EDPj

ECPj

VSP_ MASTER

EDP_ MASTER

EDP_ TRANS

EDP_ VSP_I

EDP_ READER

EDP_ SERVO

EDP_ FORCE

Manipulator motors

Force/torque transducer

VSP_ CREAD

Sensors

Fig. 37.1. The structure of a MRROC++ based robot control system (subscript j represents the jth end eﬀector 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 communicating 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 simpliﬁes coding of the portions of the software that are dedicated to the user’s task. The programmer 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 End-Eﬀector Sensors’ Role in Service Robots

37.3

405

Position-Force Control

The primary purpose of the end-eﬀector 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 behaviour). To fully utilise this potential the end-eﬀector 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 classiﬁed as exteroceptors. This duality inﬂuences the structure of the control system. We have decided that their proprioceptive character dominates, thus they have been associated 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 deﬁnition of the macrostep that must be executed. The deﬁnition speciﬁes: the position/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 periods (motion steps) deﬁning the duration of the macrostep. The local frame L is represented by the G L 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 ﬁrst step of the macrostep execution. The position/orientation argument deﬁnes ∆ L rp , the end-eﬀector 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 . (37.1) n where n is the number of steps in the macrostep and k is the current step. ∆ L rp(k) =

C. Zieli´ nski et al.

R

G

Pc

Interpret

G

version 2

Reply

G

Dl

L

C

DKin

Ci

SL

L

rp

Fd

StGen L

Fe ( k )

+ -

L

F/T CL

Macrostep

P( k !1)

P( k )

M( k ) L

Fc ( k )

d (k )

-

PC

rp ( k ) rF ( k ) +

IKin

(k )

+

r( k )

+ L

EDP_TRANS

version 1

RT

EDP_FORCE F/T conv

motor control

(k )

EDP_SERVO F( k )

Manipulator

Macrostep generation

EDP_ MASTER

F/T Sensor

406

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 – inverse 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 diﬀerence between the desired force/torque vector L Fd (deﬁned 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 homogeneous transformation matrix M(k) . The end-eﬀector pose setpoint for the current step of the macrostep execution 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

37 End-Eﬀector Sensors’ Role in Service Robots

407

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 gripper 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 counterbalance 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 ﬁngers 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 ﬁnger of one of the robots has a LED mounted in its tip, while the ﬁngers of the other robot contain phototransistors. Those were meant for helping with the alignment of the ﬁngers when turning a face of the cube. Although present in the jaws this arrangement did not have to be used, because 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 suﬃces to use the information about the current ﬂowing in the motor. Crossing a certain threshold current signals that the force is suﬃcient. However, the most useful sensor is the miniature in-hand camera. It is used as a sensor in the stage of ﬁnal 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.

408

C. Zieli´ nski et al.

20 2

Force/torque sensor

Video camera with back-lighting

Strain gauges Rubik’s cube

F/T sensor shield

Gripper jaws alignment sensors

Fig. 37.3. The robot and its end-eﬀector

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 ampliﬁer driving the DC motor. Moreover, it monitors the limit and synchronization switches as well as the current ﬂowing 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

;

; ; ;

37 End-Eﬀector Sensors’ Role in Service Robots

409

;

;;;;

;;;;

;;;

; ;

; ;

; ; ; ;

;; ;

;;;

;;;

; ;

;

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 current. The software mechanism consists of a ﬁnite state automaton having two states: ordinary work state and blocked state. The gripper is position controlled. 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 eﬀective motor 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

410

C. Zieli´ nski et al.

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] 2.5

D

2 E

1.5

G

torque

1 F

0.5 0

A H

-0.5

C B

-1 -1.5

4.64

4.65

4.66

4.67

4.68 step

4.69

4.7

4.71 5 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 translational 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 motion was sustained and then gradually the friction dropped (until point F ). Then again the friction increased. This proves that the production process

37 End-Eﬀector Sensors’ Role in Service Robots

411

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 sensors will be treated as proprioceptors and which as exteroceptors. The measurements 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 measurements 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 desired motion. In this case the reactions are slower – usually this loop takes several sampling periods (a macrostep) to execute. However, it works in conjunction 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, requiring 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-eﬀector 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].

412

C. Zieli´ nski et al.

References 1. H. Bruyninckx and J. De Schutter. Speciﬁcation 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 interactive 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. Bj¨ orkman, H. Christensen, and J.-O. Eklundh. Vision for robotic object manipulation in domestic settings. Robotics and Autonomous 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-ﬂow-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. Zieli´ nski, W. Czajewski, and T. Winiarski. Control Architecture for Sensor-Based Two-Handed Manipulation. In T. Zieli´ nska and C. Zieli´ nski, editors, CISM Courses and Lectures - 16th CISM– IFToMM Symposium on Robot Design, Dynamics and Control, RoManSy’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 position/force control of a robot manipulator in an uncalibrated environment. IEEE Transactions on Control Systems Technology, 8(4):635–645, 2000. 12. T. Zieli´ nska. Motion synthesis. In F. Pfeiﬀer and T. Zieli´ nska, editors, Walking: Biological and Technological Aspects, pages 155–191. CISM Courses and Lectures No.467. Springer, Wien, 2004. 13. T. Zieli´ nska. 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. Zieli´ nski. The MRROC++ System. In First Workshop on Robot Motion and Control, RoMoCo’99, pages 147–152, June 28–29 1999. 15. C. Zieli´ nski, A. Rydzewski, and W. Szynkiewicz. Multi-robot system controllers. In 5th Int. Symp. on Methods and Models in Automation and Robotics MMAR’98, Mi¸edzyzdroje, Poland, volume 3, pages 795–800. August 25–29 1998.

37 End-Eﬀector Sensors’ Role in Service Robots

413

16. C. Zieli´ nski, W. Szynkiewicz, and T. Winiarski. Applications of MRROC++ robot programming framework. In Proceedings of the 5th International Workshop on Robot Motion and Control, RoMoCo’05, Dymaczewo, 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 [email protected] Summary. This paper examines the problem of detecting intruders in large multiply-connected environments with multiple robots and limited range sensors. 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 ﬁnding 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 active 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 environments with multiple robots equipped with limited range sensors. The problem is closely related to pursuit-evasion and cooperative multi-robot observation 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 eﬀect 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 K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 417–426, 2007. c Springer-Verlag London Limited 2007 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 ﬁnd 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 ﬁrst 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 ﬁrst formal deﬁnition 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 resource usage when resources are not suﬃcient to surveil all targets. Recent improvements were presented in [1] by introducing a new distributed behavioral approach, called Behavioral-CMOMMT. The improvements are due to an advanced cooperation schemes between robots which boost the overall performance. 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 ﬁnd a path to clear the environment of all targets using only a single pursuer with a gap sensor with the full ﬁeld 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, ﬁnding 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 ﬁeld of view. Pursuit-evasion solutions draw most of their information from a particular method of partitioning to ﬁnd a clearing strategy. This will be useful for extensions of this paper when the ﬁrst 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

38.3

419

The Algorithm

To prepare the grounds for the algorithm we will deﬁne our notions of environment, 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 . Deﬁne 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. Deﬁne 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. i

(38.1)

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 deﬁnitions give us a very practical notation for making statements about the state of regions. In particular, it follows that a region becomes recontaminated when it has a contaminated adjacent region that is not blocked. We are now going to deﬁne 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 assigned to one or more robots. The two behaviors are deﬁned 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 adjacent regions. For a region r a block is executed on an adjacent region ra in Ar . A block on ra is deﬁned 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 subproblems, partitioning and ﬁnding 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.

420

A. Kolling and S. Carpin

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 speciﬁc 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 interpretation of these weights easier and the notation for the following simpler to follow. We now deﬁne the main problem that we are going to investigate, i.e. weighted graph clearing. The task is to ﬁnd 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 robots. 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 ﬁnd a sequence of behaviors to clear the graph using the least number of robots. As the ﬁrst 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 inﬂuence the tree structure and hence our ﬁnal 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 ﬁrst. This suggests a recursive clearing of the regions. To capture this we deﬁne 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

38 Detecting Intruders with Limited Range Mobile Sensors

421

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 ﬁrst. Let us deﬁne the order by p(ej ) − w(ej ) with the highest value ﬁrst. 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 ﬁrst. Let the order be en2 , . . . , P enm . b) Determine h := max2≤k≤m (p(eni ) + 1 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

e∈Edges(S g )

(p(e)) ≤ hmax + (d∗ − 1) · (hmax − 3).

(38.2)

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 we have p(en1 ) = P 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 }

422

A. Kolling and S. Carpin

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 occur 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

ei ∈E ′

w(ei ) ≤ hmax − 3.

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 ﬁrst stage are hmax . Now we assume the worst case for all other vertices. This gives us max

e∈Edges(S g )

{p(e)} ≤ hmax + (d∗ − 1) · (hmax − 3).

Λ 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 ) ﬁrst, 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 ﬁgure. 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.

38 Detecting Intruders with Limited Range Mobile Sensors

Fig. 38.1. This is the worst case example for d = 6, hmax = 6, p∗ = 6 + 2 · 3 = 12. Blocking weight w is indicated left and p-weight right of : on the edge. Each vertex has its weight in its cente.

38.4

423

Fig. 38.2. A comparison of the average upper bound across 1000 weighted trees and actual maximum p-weight for varying number of vertices

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 pweight, 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 ﬁrst 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 eﬀort. We presented a solution to coordinate the team eﬀort with a central instance by solving the weighted graph clearing problem. The beneﬁt is that we can now partition an environment into many simple regions for which one can easily ﬁnd 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.

424

A. Kolling and S. Carpin

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. hmax n Vertices max(p(e)) d* 24.865 5.325 25.176 20 28.300 7.784 27.949 40 30.299 9.638 29.607 60 31.781 11.077 31.039 80 32.885 12.306 31.909 100

The ﬁrst 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 deﬁnes the tree structure and hence inﬂuences 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 ﬁrst hint of the theorem is that deep trees should be avoided. Understanding the theoretical properties of the weighted graph clearing problem ﬁrst, 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 Multiple 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 multiple 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

38 Detecting Intruders with Limited Range Mobile Sensors

425

4. L. J. Guibas, J. Latombe, S. M. LaValle, D. Lin, R. Motwani, “VisibilityBased Pursuit-Evasion in a Polygonal Environment,” International Journal 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 ﬁeld of view,” The International Journal on Robotics Research, 25(4):299-316, 2006.

39 High-Level Motion Control for Workspace Sharing Mobile Robots El˙zbieta Roszkowska Institute of Computer Engineering, Control, and Robotics, Wroclaw University of Technology, ul. Janiszewskiego 11/17, 50-372 Wroclaw, Poland [email protected]

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 concentrated 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 mobile robot systems (MMRS) has been directed towards software development, based on ad-hoc, rather than rigorous models, developed directly in programming 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 speciﬁc applications rather than deal with a general 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 common workspace. The robots can communicate among themselves, e.g. through K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 427–436, 2007. c Springer-Verlag London Limited 2007 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 ﬁnal location. At this stage no positional constraints introduced by any other robot are taken into account. In order to avoid collisions, the robots reﬁne their velocity proﬁles1 , 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ﬂict. 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 suﬃcient and acceptable for a robot to come to a full stop. In this paper we consider it to be a robot speciﬁc 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 robots’ motion control so that 1) in a ﬁnite 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 understand the control that a robot would apply if it did not share the workspace with other robots. The requirement of its “dynamic modiﬁcation” means that changes in the robots velocity proﬁle 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 proﬁles. 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 ﬁnite 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 dynamics satisfy, respectively, the stop-distance constraint and the mutuallyexclusive use of particular resource units (which guarantees no collisions,

1

In this paper we only consider the coordination of robot motions by velocity modiﬁcations. However, the proposed methodology can be extended to capture also path plans’ reﬁnement.

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 ﬁnal 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 ﬁnite 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 iﬀ e ∈ Γ (s). 4. δ : S × E → S is the transition function. δ is a partial function deﬁned 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 deﬁned as follows. Definition 2. The parallel composition of automata G1 and G2 is the automaton 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 , δ (s , e)) if e ∈ Γ2 (s2 ) \ E1  1 2 2   undeﬁned otherwise

39.4 Discretization of Robot Motion Processes We start from a continuous representation of the Multiple Mobile Robot System (MMRS). The system consists of a set of mobile robots A ∈ A that share a ﬁnite planar workspace W S ⊂ R2 with the XY coordinate system. The 2

We omit in this deﬁnition 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 speciﬁc 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 ﬁrst 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, deﬁned as follows. Definition 3. For a path π, the sequence of its border points is a ﬁnite sequence 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, possibly, 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) Ck , i.e., the union of the regions of the q stages succeeding zj or k=j+1 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 ﬂoor functions, i.e., the smallest (the greatest, resp.) integer equal to or greater (less, resp.) than x.

39 High-Level Motion Control for Workspace Sharing Mobile Robots

431

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

p1,16

9 8 7

p1,0 = p1,43

p1,8 = p1,35

5

p1,21

p1,28

6

p2,4

4 3 2

p2,0

1

p2,11

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 0

1

1

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

0

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 suﬃcient 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 automaton, 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 suﬃcient 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 − C i ) : i = mq + 1, m = 0, 1, . . . , ⌊n/q⌋}, where • Ef = {fi = (i, Ci+1 4 (fi ; 1) is the identiﬁer 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) qualiﬁes event e ∈ E as feasible in state s = (j, µ, B) iﬀ • 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.

39 High-Level Motion Control for Workspace Sharing Mobile Robots

433

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 ﬁnal 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 ﬁnal state.

max

d1

3

1 C1

f1 1 ~ C1

a4 d1

1 C13

a4 max

4

d1

3

a4

1 C1

2 C2

1 C41

d2

d1 max

d2

4

2 C2 3 C3

2 C24

5

2 C2

max 5

a5

2 C2

d2

3 C34

stop

a5 d2

max

5

3 C3

a5

3 ~ C3

d2

f4 f4

d3

3 C35 d3 5 4 C4 d4 5 C5 stop

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 resources 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 deﬁned 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 deﬁned 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 ﬁrst of the required properties. Property 1. The control deﬁned 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 ﬁnite time, it is necessary to ensure that if a robot is enforced to a stop due to the lack of some resources, in a ﬁnite 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 ﬁnal 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

39 High-Level Motion Control for Workspace Sharing Mobile Robots

435

[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 satisﬁes 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 robust coordination of mutually asynchronous robot controllers, and the mathematical 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 diﬀer in the construction and application of the component robots, a spectrum that can include, e.g., a ﬂeet 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’ modiﬁcation, grid size in the context of its inﬂuence on the communication load and its constraints, and the eﬃciency control through robot-task dependent prioritization of their access to conﬂict 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 practical 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 autonomous 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 ﬂexible 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 Istv´an Harmati Department of Control Engineering and Information Technology Budapest University of Technology and Economics H-1117, Magyar Tud´osok krt. 2/B422, Budapest, Hungary [email protected]

40.1 Introduction Heavy congestion is a major issue in the urban traﬃc control problem due to the increasing number of vehicles. The main tools to control traﬃc are the traﬃc lights at the junctions. Based on the measurement of important traﬃc properties, the traﬃc control has to realize an optimal green time distribution for the traﬃc lights that passes as many vehicles through the network as possible. The literature of developed traﬃc models and control is increasing. Current trends mainly involve a cell-transformation model [1], [2], heuristic-based or soft computing methods [1], [3] but knowledge based methods [4] and stochastic system modeling [5] were also successfully tested in applications. The algorithm in [6] organizes the traﬃc ﬂow into an arterial structure which is especially useful to establish a green corridor in the traﬃc 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 traﬃc control problem into game theoretic framework and provide 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 traﬃc control illustrates the eﬃciency of the proposed solutions. The paper is organized as follows. Section 40.2 describes a traﬃc model in a state space form. Section 40.3 proposes a game theoretic approach to control urban traﬃc network. Section 40.4 describes methods that realize path planning algorithms for vehicles. The simulation results are illustrated on a regular traﬃc network (inspired by the road structure of common North American cities) with size 5 × 5. Finally, Section 40.5 summarizes the main results. K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 437–444, 2007. c Springer-Verlag London Limited 2007 springerlink.com

438

I. Harmati

40.2 The Traffic Model The traﬃc 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. Consider 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 inﬂow is higher than the outﬂow 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 designated 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 oﬀset of cycles between any two junctions is ﬁxed (and consistent to others). (ASF5): The saturation ﬂows 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 incoming 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 eﬀective 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 indiﬀerent in (40.1). Considering a road-link z between junction M and junction N (z ∈ IN , z ∈ OM ) as shown in Fig. 40.1 (left), the discrete dynamics is given by

40 Urban Traﬃc 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 inﬂow, uz is the outﬂow in the time period [kT, (k + 1)T ], k = 1, 2, . . . with control time interval T . The additional terms dz and sz (k) = tz,0 qz (k) denote the demand and the exit ﬂow, 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) # " P P N M X Sw i∈ON gw,i Sw i∈OM gw,i . − tw,z + T (1 − tz,0 ) C C

(40.2)

w∈IM

40.3 Urban Traffic Control In order to reduce the combinatorial complexity, our method organizes junctions 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 diﬀerent 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 junctions (and the cycle time) is maintained at constant value with this strategy during the whole traﬃc control. The grouping requires new notation. Let J2 be the set of junctions where an element (i, j) ∈ J2 identiﬁes the jth junction

M

qz sz

uz

N

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 traﬃc 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 traﬃc 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 = 3∆g, ∀w ∈ O(i, j), while the green times from its green time with δp,w 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 xz (k, τ(i,1) , . . . , τ(i,|J i |) ) X (i,h) k, τ(i,1) , . . . , τ(i,|J i |) = 2 2 z∈I(i,h)

+λ

|J2i | X X j=1, j6=h

z∈I(i,j)

xz (k, τ(i,1) , . . . , τ(i,|J i |) ), 2

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 i X (i,1) (k, τ(i,1) , . . . , τ(i,|J i |) ), . . . , X (i,|J2 |) (k, τ(i,1) , . . . , τ(i,|J i |) ) 2

2

for every combination of decisions τ(i,j) for every group i. ∗ ∗ , . . . , τ(i, ) of the game that satisﬁes Step 6) Find the Nash strategy (τ(i,1) |J2i |) ∗ ∗ X (i,h) k, τ(i,1) (40.4) , . . . , τ(i, i ) J | 2| ≤ ∗ ∗ ∗ 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) .

40 Urban Traﬃc Control and Path Planning for Vehicles

441

Step 8) Repeat the procedure from Step 2) for the next control time interval. The simulation results on a 5 × 5 sized traﬃc 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 traﬃc 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 beneﬁts (by making a green corridor) only if the traﬃc network is equipped with a special vehicle ﬂow 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 ﬁnd 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 traﬃc network. In this section we adopt the game theoretic motion planning concept from [10]

442

I. Harmati

to urban traﬃc environment. We assume that the path planning is carried out for some vehicles in the traﬃc 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+1 (p) + Vk+j (w) = min  p∈O(w) 

max

τ(i,r) ,τ(m,n) ) p∈O(i,r) p∈I(m,n)

If j = k + K then Vk+j (w) = D(w, zd ),



 xk (p, τ(i,r) , τ(m,n) ) 

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 modiﬁed 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 ﬁtness 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 ﬁtness based roulettewheel selection algorithm. The simulation results are illustrated in Fig. 40.3. Both Algorithm 2 and its probabilistic extension ﬁnd 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.

40 Urban Traﬃc Control and Path Planning for Vehicles 6

6

5

5

4

4

3

3

2

2

1

1

0

0

1

2

3

4

5

6

0

0

1

2

3

4

443

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 traﬃc 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 simpliﬁcations are carried out. The most important simpliﬁcations 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 traﬃc 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 Oﬃce for Research and Technology under grant NKTH RET 04/2004 and by the J´anos Bolyai Research Scholarship of the Hungarian Academy of Sciences.

References 1. B. Friedrich and E. Almasri. Online oﬀset optimisation in urban networks based on cell transmission model. In Proceedings of the 5th European Congress on Intelligent Transport Systems and Services, Hannover, Germany, 2005.

444

I. Harmati

2. B. Friedrich. Traﬃc 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 traﬃc ﬂow in street network. Transportation Research Part C, Elsevier Ltd., 13:93–105, 2005. 4. F. Logi and S. G. Ritchie. Development and evaluation of a knowledgebased system for traﬃc congestion management and control. Transportation 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 traﬃc congestion. Mathematical and Computer Modeling, Elsevier Ltd., 38:533–549, 2003. 6. N. H. Gartner and C. Stamatiadis. Arterial based control of traﬃc ﬂow 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 traﬃc control strategy tuc. In TRB 2003 Annual Meeting, 2003. 8. V. Dinopoulou, C. Diakiki, and M. Papageorgiou. Applications of urban traﬃc control strategy tuc. European Journal of Operational Research, 175(3):1652–1665, 2005. 9. I. Harmati. Game theoretic control algorithms for urban traﬃc 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 K¨ oker, Kai-Steﬀen 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 diﬀerent operating scenarios and is usually based on a system model that focuses 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 monitored data of a real system. As an example, hardware monitoring requires a specialized device with a built-in clock, which generates timestamps for signals 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 diﬀer 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 because of the back-oﬀ 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 signals 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 beneﬁt

K. Kozlowski (Ed.): Robot Motion and Control 2007, LNCIS 360, pp. 445–452, 2007. c Springer-Verlag London Limited 2007 springerlink.com

446

K. K¨ oker, 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-eﬃcient manner. The work is primarily 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 FMreceiver 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 infrastructure are presented to conﬁrm the usability for distributed mobile embedded systems.

41.2

Architecture of the Application

We selected a modiﬁed 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 speciﬁed 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 oﬀ-ﬁeld 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 consists 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 modiﬁcation 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 system 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 imaginable 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 possible 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 receiver and the PPS-API. The main part of the infrastructure is the possibility to generate high resolution timestamps derived from the TSC. The usual solution 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 eﬀective” GPS receiver module (e.g., the price for a common GPS timing module including the antenna 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).

448

41.3.2

K. K¨ oker, K.-S. Hielscher, and R. German

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 modiﬁed by the patch to timestamp every invocation. We patched the Linux kernel with the PPS-API patch to use this facility. This patch modiﬁes 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 speciﬁed 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 oﬀsets 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 conﬁguration ﬁle 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 example, 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 transmitting the timing signal pulse of the mentioned GPS infrastructure regardless of the medium clearance without aﬀecting other clients and keeping the mechanism 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

41 Low-Cost Time Measurement Infrastructure for Mobile Systems

449

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 infrastructure to the robot’s PC/104 system and simultaneously tracked the signal reception with an analogue oscilloscope. The signal course of the ﬁrst test run resulted in a lot of interference, which would lead the PPS-API facility to assume a wrong oﬀset 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 diﬀerence of two back-to-back seconds. The y-axis of the ﬁgure 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 using the modiﬁed 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 diﬀerent electronic devices with spurious radiations (i.e. power supply of running computers), and also atmospheric disturbances. 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 eﬀective 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 deliver a PPS signal output that marks the beginning of a second with a very

K. K¨ oker, K.-S. Hielscher, and R. German

450

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 undeﬁned 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-toback 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 ﬁrst 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 diﬀerent driving modes of the GPS cards

41 Low-Cost Time Measurement Infrastructure for Mobile Systems

41.5

451

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 signiﬁcantly 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. K¨oker, 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-Speciﬁcation, 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-persecond API for Unix-like operating systems, Version 1. Request for Comments 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.

452

K. K¨ oker, K.-S. Hielscher, and R. German

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.) Robot Motion and Control 2007 452 p. 2007 [978-1-84628-973-6] Vol. 359: Christophersen F.J. Optimal Control of Constrained Piecewise Affine Systems xxx p. 2007 [978-3-540-72700-2] Vol. 358: Findeisen R.; Allgöwer F.; Biegler L.T. (Eds.): Assessment and Future Directions of Nonlinear Model Predictive Control XXX p. 2007 [978-3-540-72698-2] Vol. 357: Queinnec I.; Tarbouriech S.; Garcia G.; Niculescu S.-I. (Eds.): Biology and Control Theory: Current Challenges 589 p. 2007 [978-3-540-71987-8] Vol. 356: Karatkevich A. Dynamic Analysis of Petri Net-Based Discrete Systems 166 p. 2007 [978-3-540-71464-4] Vol. 355: Zhang H.; Xie L. Control and Estimation of Systems with Input/Output Delays 213 p. 2007 [978-3-540-71118-6] Vol. 354: Witczak M. Modelling and Estimation Strategies for Fault Diagnosis of Non-Linear Systems 215 p. 2007 [978-3-540-71114-8] Vol. 353: Bonivento C.; Isidori A.; Marconi L.; Rossi C. (Eds.) Advances in Control Theory and Applications 305 p. 2007 [978-3-540-70700-4] Vol. 352: Chiasson, J.; Loiseau, J.J. (Eds.) Applications of Time Delay Systems 358 p. 2007 [978-3-540-49555-0] Vol. 351: Lin, C.; Wang, Q.-G.; Lee, T.H., He, Y. LMI Approach to Analysis and Control of Takagi-Sugeno Fuzzy Systems with Time Delay 204 p. 2007 [978-3-540-49552-9] Vol. 350: Bandyopadhyay, B.; Manjunath, T.C.; Umapathy, M. Modeling, Control and Implementation of Smart Structures 250 p. 2007 [978-3-540-48393-9]

Vol. 349: Rogers, E.T.A.; Galkowski, K.; Owens, D.H. Control Systems Theory and Applications for Linear Repetitive Processes 482 p. 2007 [978-3-54042663-9] Vol. 347: Assawinchaichote, W.; Nguang, K.S.; Shi P. Fuzzy Control and Filter Design for Uncertain Fuzzy Systems 188 p. 2006 [978-3-540-37011-6] Vol. 346: Tarbouriech, S.; Garcia, G.; Glattfelder, A.H. (Eds.) Advanced Strategies in Control Systems with Input and Output Constraints 480 p. 2006 [978-3-540-37009-3] Vol. 345: Huang, D.-S.; Li, K.; Irwin, G.W. (Eds.) Intelligent Computing in Signal Processing and Pattern Recognition 1179 p. 2006 [978-3-540-37257-8] Vol. 344: Huang, D.-S.; Li, K.; Irwin, G.W. (Eds.) Intelligent Control and Automation 1121 p. 2006 [978-3-540-37255-4] Vol. 341: Commault, C.; Marchand, N. (Eds.) Positive Systems 448 p. 2006 [978-3-540-34771-2] Vol. 340: Diehl, M.; Mombaur, K. (Eds.) Fast Motions in Biomechanics and Robotics 500 p. 2006 [978-3-540-36118-3] Vol. 339: Alamir, M. Stabilization of Nonlinear Systems Using Receding-horizon Control Schemes 325 p. 2006 [978-1-84628-470-0] Vol. 338: Tokarzewski, J. Finite Zeros in Discrete Time Control Systems 325 p. 2006 [978-3-540-33464-4] Vol. 337: Blom, H.; Lygeros, J. (Eds.) Stochastic Hybrid Systems 395 p. 2006 [978-3-540-33466-8] Vol. 336: Pettersen, K.Y.; Gravdahl, J.T.; Nijmeijer, H. (Eds.) Group Coordination and Cooperative Control 310 p. 2006 [978-3-540-33468-2] Vol. 335: Kozłowski, K. (Ed.) Robot Motion and Control 424 p. 2006 [978-1-84628-404-5]

Vol. 334: Edwards, C.; Fossas Colet, E.; Fridman, L. (Eds.) Advances in Variable Structure and Sliding Mode Control 504 p. 2006 [978-3-540-32800-1] Vol. 333: Banavar, R.N.; Sankaranarayanan, V. Switched Finite Time Control of a Class of Underactuated Systems 99 p. 2006 [978-3-540-32799-8] Vol. 332: Xu, S.; Lam, J. Robust Control and Filtering of Singular Systems 234 p. 2006 [978-3-540-32797-4] Vol. 331: Antsaklis, P.J.; Tabuada, P. (Eds.) Networked Embedded Sensing and Control 367 p. 2006 [978-3-540-32794-3] Vol. 330: Koumoutsakos, P.; Mezic, I. (Eds.) Control of Fluid Flow 200 p. 2006 [978-3-540-25140-8] Vol. 329: Francis, B.A.; Smith, M.C.; Willems, J.C. (Eds.) Control of Uncertain Systems: Modelling, Approximation, and Design 429 p. 2006 [978-3-540-31754-8] Vol. 328: Loría, A.; Lamnabhi-Lagarrigue, F.; Panteley, E. (Eds.) Advanced Topics in Control Systems Theory 305 p. 2006 [978-1-84628-313-0] Vol. 327: Fournier, J.-D.; Grimm, J.; Leblond, J.; Partington, J.R. (Eds.) Harmonic Analysis and Rational Approximation 301 p. 2006 [978-3-540-30922-2] Vol. 326: Wang, H.-S.; Yung, C.-F.; Chang, F.-R. H∞ Control for Nonlinear Descriptor Systems 164 p. 2006 [978-1-84628-289-8] Vol. 325: Amato, F. Robust Control of Linear Systems Subject to Uncertain Time-Varying Parameters 180 p. 2006 [978-3-540-23950-5] Vol. 324: Christofides, P.; El-Farra, N. Control of Nonlinear and Hybrid Process Systems 446 p. 2005 [978-3-540-28456-7] Vol. 323: Bandyopadhyay, B.; Janardhanan, S. Discrete-time Sliding Mode Control 147 p. 2005 [978-3-540-28140-5] Vol. 322: Meurer, T.; Graichen, K.; Gilles, E.D. (Eds.) Control and Observer Design for Nonlinear Finite and Infinite Dimensional Systems 422 p. 2005 [978-3-540-27938-9] Vol. 321: Dayawansa, W.P.; Lindquist, A.; Zhou, Y. (Eds.) New Directions and Applications in Control Theory 400 p. 2005 [978-3-540-23953-6]

Vol. 320: Steffen, T. Control Reconfiguration of Dynamical Systems 290 p. 2005 [978-3-540-25730-1] Vol. 319: Hofbaur, M.W. Hybrid Estimation of Complex Systems 148 p. 2005 [978-3-540-25727-1] Vol. 318: Gershon, E.; Shaked, U.; Yaesh, I. H∞ Control and Estimation of State-multiplicative Linear Systems 256 p. 2005 [978-1-85233-997-5] Vol. 317: Ma, C.; Wonham, M. Nonblocking Supervisory Control of State Tree Structures 208 p. 2005 [978-3-540-25069-2] Vol. 316: Patel, R.V.; Shadpey, F. Control of Redundant Robot Manipulators 224 p. 2005 [978-3-540-25071-5] Vol. 315: Herbordt, W. Sound Capture for Human/Machine Interfaces: Practical Aspects of Microphone Array Signal Processing 286 p. 2005 [978-3-540-23954-3] Vol. 314: Gil’, M.I. Explicit Stability Conditions for Continuous Systems 193 p. 2005 [978-3-540-23984-0] Vol. 313: Li, Z.; Soh, Y.; Wen, C. Switched and Impulsive Systems 277 p. 2005 [978-3-540-23952-9] Vol. 312: Henrion, D.; Garulli, A. (Eds.) Positive Polynomials in Control 313 p. 2005 [978-3-540-23948-2] Vol. 311: Lamnabhi-Lagarrigue, F.; Loría, A.; Panteley, E. (Eds.) Advanced Topics in Control Systems Theory 294 p. 2005 [978-1-85233-923-4] Vol. 310: Janczak, A. Identification of Nonlinear Systems Using Neural Networks and Polynomial Models 197 p. 2005 [978-3-540-23185-1] Vol. 309: Kumar, V.; Leonard, N.; Morse, A.S. (Eds.) Cooperative Control 301 p. 2005 [978-3-540-22861-5] Vol. 308: Tarbouriech, S.; Abdallah, C.T.; Chiasson, J. (Eds.) Advances in Communication Control Networks 358 p. 2005 [978-3-540-22819-6] Vol. 307: Kwon, S.J.; Chung, W.K. Perturbation Compensator based Robust Tracking Control and State Estimation of Mechanical Systems 158 p. 2004 [978-3-540-22077-0]

Robot Motion and Control 2007 (Lecture Notes in Control and Information Sciences)

Robot Motion and Control 2009 (Lecture Notes in Control and Information Sciences)

Singular Control Systems (Lecture Notes in Control and Information Sciences)

Networked Control Systems (Lecture Notes in Control and Information Sciences)

Ensuring Control Accuracy (Lecture Notes in Control and Information Sciences)